Robot laser radar odometer method, system and device

Abstract

The invention belongs to the technical field of robots, particularly relates to a robot laser radar odometer method, system and device, and aims to solve the problem that the pose estimation precision is affected due to inaccurate distance optimization constraint caused by local neighborhood representation in a line or surface form in the prior art. The method comprises the following steps: acquiring three-dimensional point cloud data, and estimating a first pose of a laser radar sensor in a world coordinate system in a current frame coordinate system; according to the first pose, projecting the feature points extracted from the current frame to a target feature point map to find a corresponding local neighborhood point set, obtaining local neighborhood representation, and constructing probability distance optimization constraints, thereby constructing a pose optimization function, and realizing pose estimation of the current frame coordinate system of the laser radar sensor in a world coordinate system; and projecting the feature points in the feature point set of the current frame to a corresponding feature point map according to the pose estimation result of the current frame to complete updating of a target feature point map. According to the invention, the precision of pose estimation is improved.

Description

Technical Field

[0001] The present invention relates to the field of robotics technology, and in particular to a robot laser radar odometer method, system, and device. Background Art

[0002] Robots are widely used in a variety of fields, including industrial manufacturing, healthcare, and home services, and are crucial for improving productivity and enhancing human well-being. For mobile robots, obtaining high-precision positioning results is a prerequisite for completing various tasks. Odometry is a crucial technology for robots to obtain precise positioning information. Visual odometry and lidar odometry are two mainstream odometry methods. Compared to visual odometry, lidar odometry has attracted widespread attention due to its robustness to illumination variations and precise ranging capabilities.

[0003] To estimate the pose of the current frame, a straightforward and commonly used approach is frame-by-frame matching, which involves registering the current frame with the previous frame. While this method is computationally efficient, the inter-frame error accumulates over time, affecting the accuracy of pose estimation. To address this issue, researchers have proposed frame-image matching. Compared to frame-by-frame matching, frame-image matching matches the current frame with a local map containing information from multiple historical frames. Leveraging richer environmental information, it effectively reduces the accumulated error and improves pose estimation accuracy. However, while frame-image matching offers excellent accuracy, it also incurs high computational complexity due to the need to process more point cloud data. To balance accuracy and efficiency, combining frame-by-frame matching with frame-image matching has become a preferred approach. Representative methods include LOAM (LiDAR Odometry and Mapping) and A-LOAM (Advanced LiDAR Odometry and Mapping). Researchers often use third-party libraries such as PCL (Point Cloud Library), Eigen, and Ceres for lidar point cloud processing and pose optimization. For example, they call the computeMeanAndCovarianceMatrix function in the PCL library to solve the mean and covariance matrices of point cloud data, use the KD-Tree (K-Dimensional Tree) in the PCL library to perform fast nearest neighbor search for point cloud data; use the SelfAdjointEigenSolver class in the Eigen library to perform eigenvalue decomposition on the matrix; and use the Solve function in the Ceres library to solve optimization problems.

[0004] As a representative lidar odometry method, LOAM projects the feature points of the current frame into the target feature point map, and then fits a line or surface to form an association with the feature point projection by finding the corresponding local neighborhood point set in the target feature point map. Then, by accumulating all point-line distance optimization constraints and point-surface distance optimization constraints, the objective function to be optimized is obtained, and the pose estimation is achieved by minimizing the objective function. However, this local neighborhood representation in the form of lines or surfaces is insufficient to capture the details of the environment, because the representation ignores the roughness of the local neighborhood, resulting in inaccurate point-line distance optimization constraints and point-surface distance optimization constraints, which affects the accuracy of pose estimation. Therefore, how to propose a solution to the above problem is a problem that those skilled in the art currently need to solve. Summary of the Invention

[0005] In order to solve the above-mentioned problems in the prior art, that is, to solve the problem that the existing robot lidar odometry method uses a local neighborhood representation in the form of a line or surface, resulting in inaccurate distance optimization constraints and affecting the accuracy of pose estimation, the first aspect of the present invention proposes a robot lidar odometry method, which includes the following steps:

[0006] Step S10: The robot senses the environment through the laser radar sensor and obtains three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the laser radar sensor; based on the three-dimensional point cloud data, feature points are extracted, and then the first position of the laser radar sensor current frame coordinate system in the world coordinate system is estimated;

[0007] Step S20: projecting the feature points extracted from the current frame onto the target feature point map to find a corresponding local neighborhood point set based on the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system; obtaining a corresponding local neighborhood representation based on the local neighborhood point set corresponding to each feature point, and then constructing a probabilistic distance optimization constraint to construct a pose optimization function, thereby estimating the second pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, and using the estimated second pose as the pose estimation result of the current frame; the probabilistic distance optimization constraint includes a point-probability line distance optimization constraint and a point-probability surface distance optimization constraint;

[0008] Step S30 , projecting the feature points in the feature point set of the current frame into the corresponding feature point map according to the pose estimation result of the current frame, thereby completing the update of the target feature point map.

[0009] In some preferred embodiments, based on the three-dimensional point cloud data, feature points are extracted to estimate the first position of the current frame coordinate system of the lidar sensor in the world coordinate system, and the method is:

[0010] Based on the three-dimensional point cloud data , use the LOAM method to extract feature points and form a feature point set ; The feature points include edge feature points and plane feature points;

[0011] Using LOAM frame matching, according to the and the previous frame of 3D point cloud data , estimate the current frame coordinate system of the lidar sensor Relative to the previous frame's lidar sensor coordinate system Pose transformation ;

[0012] Based on the , the previous frame lidar sensor coordinate system Relative to the world coordinate system The first pose of In the world coordinate system The first position , and They are respectively The rotation matrix and translation vector.

[0013] In some preferred embodiments, the feature points extracted from the current frame are projected onto the target feature point map to find the corresponding local neighborhood point set, and the method is:

[0014] The Edge feature points in and plane feature points Project them into the world coordinate system to get the projection points and , ; and They are respectively In the world coordinate system The second posture The rotation matrix and translation vector of ;

[0015] Use the KdTree function of the PCL library to exist Perform KD-Tree search on the exist Perform KD-Tree search in and get the In the The local neighborhood point set in and the In the The local neighborhood point set in ;in, , Represents the target feature point map, 、 They represent edge feature point map and plane feature point map respectively.

[0016] In some preferred embodiments, based on the local neighborhood point set corresponding to each feature point, a corresponding local neighborhood representation is obtained by:

[0017] Use the computeMeanAndCovarianceMatrix function in the PCL library to obtain the The corresponding mean and the covariance matrix ; Use the SelfAdjointEigenSolver class in the Eigen library to solve the Perform eigenvalue decomposition to obtain eigenvalues 、 、 , and the corresponding eigenvectors 、 、 ,in, When satisfied When constructing the The corresponding probability line representation , is the set linear structure judgment threshold, The parameters are recorded as , , ;

[0018] Use the computeMeanAndCovarianceMatrix function in the PCL library to obtain the The corresponding mean and the covariance matrix ; Use the SelfAdjointEigenSolver class in the Eigen library to solve the Perform eigenvalue decomposition to obtain eigenvalues 、 、 , and the corresponding eigenvectors 、 、 ,in, When satisfied When constructing the Corresponding probability surface representation , is the set plane structure judgment threshold, The parameters are recorded as , , .

[0019] In some preferred embodiments, a probabilistic distance optimization constraint is constructed, including a point-probability line distance optimization constraint and a point-probability surface distance optimization constraint, and the method is as follows:

[0020] The point-probability line distance optimization constraint :

[0021] ;

[0022] Constrain Add to the first constraint set middle;

[0023] The point-probability surface distance optimization constraint :

[0024] ;

[0025] Constrain Add to the second constraint set middle;

[0026] in, is the identity matrix with 3 rows and 3 columns, is the transpose operator.

[0027] In some preferred embodiments, a pose optimization function is constructed to estimate the second pose of the current frame coordinate system of the lidar sensor in the world coordinate system, and the estimated second pose is used as the pose estimation result of the current frame, and the method is as follows:

[0028] Based on the first constraint set and the second constraint set , build optimization frame pose The objective function :

[0029] ;

[0030] in, The first constraint set The point-probability line distance optimization constraints, is the first constraint set The number of point-probability line distance optimization constraints included, The second constraint set The Point-probability surface distance optimization constraints, is the second constraint set The number of point-probability surface distance optimization constraints included; Refers to the absolute value operation, is the Huber kernel function;

[0031] Minimize the objective function and use the Solve function in the Ceres library to solve it and get the pose estimation result of the current frame , and The rotation matrix and translation vector of the pose estimation result of the current frame are respectively.

[0032] In some preferred embodiments, based on the pose estimation result of the current frame, the feature points in the feature point set of the current frame are projected into the corresponding feature point map to complete the update of the target feature point map, and the method is as follows:

[0033] Using the The Edge feature points in and plane feature points Project them into the world coordinate system respectively to get the projection points and , all the projected points and Add to the edge feature point map respectively and the plane feature point map In the step 3, the update of the target feature point map is completed.

[0034] In a second aspect, the present invention provides a robot laser radar odometry system, the system comprising:

[0035] The first pose estimation module is configured to sense the environment of the robot through the laser radar sensor, obtain three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the laser radar sensor, extract feature points based on the three-dimensional point cloud data, and then estimate the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system;

[0036] The second pose estimation module is configured to project the feature points extracted from the current frame onto the target feature point map to find the corresponding local neighborhood point set based on the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system; obtain the corresponding local neighborhood representation based on the local neighborhood point set corresponding to each feature point, and then construct a probabilistic distance optimization constraint to construct a pose optimization function to achieve the estimation of the second pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, and use the estimated second pose as the pose estimation result of the current frame; the probabilistic distance optimization constraint includes a point-probability line distance optimization constraint and a point-probability surface distance optimization constraint;

[0037] The map updating module is configured to project the feature points in the feature point set of the current frame into the corresponding feature point map according to the pose estimation result of the current frame, thereby completing the update of the target feature point map.

[0038] In a third aspect of the present invention, a storage device is proposed, in which a plurality of programs are stored. The programs are suitable for being loaded and executed by a processor to implement the above-mentioned robot lidar odometry method.

[0039] In a fourth aspect of the present invention, a processing device is proposed, comprising a processor and a storage device; the processor is adapted to execute various programs; the storage device is adapted to store multiple programs; the programs are adapted to be loaded and executed by the processor to implement the above-mentioned robot lidar odometer method.

[0040] Beneficial effects of the present invention:

[0041] The robot lidar odometry method of the present invention improves the accuracy of local neighborhood representation by representing the local neighborhood of linear structures in the form of probability lines and the local neighborhood of planar structures in the form of probability surfaces, thereby constructing more accurate distance optimization constraints and improving the accuracy of pose estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Other features, objects and advantages of the present application will become more apparent from the detailed description of non-limiting embodiments made with reference to the following drawings.

[0043] Figure 1 It is a flowchart of a robot laser radar odometer method according to an embodiment of the present invention;

[0044] Figure 2 It is a schematic diagram of the framework of a robot laser radar odometry system according to an embodiment of the present invention. DETAILED DESCRIPTION

[0045] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts shall fall within the scope of protection of the present invention.

[0046] The present application will be further described in detail below with reference to the accompanying drawings and examples. It should be understood that the specific embodiments described herein are intended only to illustrate the relevant invention and are not intended to limit the invention. It should also be noted that, for ease of description, only portions relevant to the relevant invention are shown in the accompanying drawings.

[0047] It should be noted that, unless there is any conflict, the embodiments and features in the embodiments of this application can be combined with each other.

[0048] A robot laser radar odometer method of the present invention, such as Figure 1 As shown, the method includes the following steps:

[0049] Step S10: The robot senses the environment through the laser radar sensor and obtains three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the laser radar sensor; based on the three-dimensional point cloud data, feature points are extracted, and then the first position of the laser radar sensor current frame coordinate system in the world coordinate system is estimated;

[0050] Step S20: projecting the feature points extracted from the current frame onto the target feature point map to find a corresponding local neighborhood point set based on the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system; obtaining a corresponding local neighborhood representation based on the local neighborhood point set corresponding to each feature point, and then constructing a probabilistic distance optimization constraint to construct a pose optimization function, thereby estimating the second pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, and using the estimated second pose as the pose estimation result of the current frame; the probabilistic distance optimization constraint includes a point-probability line distance optimization constraint and a point-probability surface distance optimization constraint;

[0051] Step S30 , projecting the feature points in the feature point set of the current frame into the corresponding feature point map according to the pose estimation result of the current frame, thereby completing the update of the target feature point map.

[0052] In order to more clearly illustrate a robot laser radar odometer method of the present invention, each step in an embodiment of the method of the present invention is described in detail below.

[0053] Step S10: The robot senses the environment through the laser radar sensor and obtains three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the laser radar sensor; based on the three-dimensional point cloud data, feature points are extracted, and then the first position of the laser radar sensor current frame coordinate system in the world coordinate system is estimated;

[0054] In this embodiment, the robot obtains the surrounding environment in the current frame coordinate system of the laser radar sensor through the laser radar sensor it carries. 3D point cloud data ,in Indicates the index of the frame. The preferred LiDAR sensor is Velodyne HDL64. Based on 3D point cloud data The present invention preferably uses the LOAM method to extract edge feature points and plane feature points to form a feature point set ,in and Represent the edge feature point set and plane feature point set respectively, and They are edge feature points and Planar feature points. Then, using LOAM frame matching, according to and the previous frame of 3D point cloud data , estimated Relative to the previous frame's lidar sensor coordinate system Pose transformation ,in, and They are Relative to The rotation matrix and translation vector of . On this basis, based on , the previous frame lidar sensor coordinate system Relative to the world coordinate system The first posture ,get In the world coordinate system The first position ,in, and They are The rotation matrix and translation vector, and They are The rotation matrix and translation vector of the world coordinate system. The first frame coordinate system of the lidar sensor overlap, when hour, , is a 3-row 3-column identity matrix, is a zero vector with 3 rows and 1 column. For details about the LOAM method, see the paper: J. Zhang, S. Singh. LOAM: Lidar Odometry and Mapping in Real-Time. Robotics: Science and Systems, 2014.

[0055] Step S20: projecting the feature points extracted from the current frame onto the target feature point map to find a corresponding local neighborhood point set based on the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system; obtaining a corresponding local neighborhood representation based on the local neighborhood point set corresponding to each feature point, and then constructing a probabilistic distance optimization constraint to construct a pose optimization function, thereby estimating the second pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, and using the estimated second pose as the pose estimation result of the current frame; the probabilistic distance optimization constraint includes a point-probability line distance optimization constraint and a point-probability surface distance optimization constraint;

[0056] In this embodiment, the first position of the laser radar sensor current frame coordinate system in the world coordinate system is , project the feature points extracted from the current frame to the target feature point map Get the projection point and find the corresponding local neighborhood point set around the projection point, where the local neighborhood point set is the map N neighbors around the mid-projection point ty points, N ty Optimize 5; based on edge feature points Corresponding local neighborhood point set, construct probability line to realize local neighborhood representation, for plane feature points The corresponding local neighborhood point set is constructed to construct a probability surface to realize the local neighborhood representation. On this basis, the probability distance optimization constraint is constructed, including the point-probability line distance optimization constraint and the point-probability surface distance optimization constraint, so as to construct the pose optimization function and realize the current frame coordinate system of the lidar sensor in the world coordinate system. The second posture Estimates, and They are The rotation matrix and translation vector of ; the second pose is used as the pose estimation result of the current frame; specifically as follows:

[0057] Step S201: As The initial value of , ;

[0058] Step S202, using The feature point set of the current frame and target feature point map Perform data association, where and They are edge feature point map and plane feature point map respectively; specifically, Edge feature points in and plane feature points Project them into the world coordinate system to get the projection points and Then use the KdTree function of the PCL library to and respectively and Perform KD-Tree search in and get exist The local neighborhood point set in as well as exist The local neighborhood point set in ;

[0059] Step S203: and Specifically, the computeMeanAndCovarianceMatrix function in the PCL library is used to obtain The corresponding mean and the covariance matrix , using the SelfAdjointEigenSolver class in the Eigen library Perform eigenvalue decomposition to obtain eigenvalues 、 、 , and the corresponding eigenvectors 、 、 ,in When satisfied When is the linear structure determination threshold set, in the present invention The preferred value is 0.8, indicating The corresponding local neighborhood is a linear structure, and the construction The corresponding probability line representation , The parameters are recorded as ,in, , ;

[0060] Step S204, use the computeMeanAndCovarianceMatrix function in the PCL library to obtain The corresponding mean and the covariance matrix , using the SelfAdjointEigenSolver class in the Eigen library Perform eigenvalue decomposition to obtain eigenvalues 、 、 , and the corresponding eigenvectors 、 、 ,in When satisfied When is the set plane structure determination threshold, in the present invention The preferred value is 0.8, indicating The corresponding local neighborhood is a planar structure, and the construction Corresponding probability surface representation , The parameters are recorded as ,in, , ;

[0061] Step S205, using Probability line representation of association , calculate the point-probability line distance optimization constraint , and constrain Add to the first constraint set in; utilize Probabilistic representation of association , calculate the point-probability surface distance optimization constraint , and constrain Add to the second constraint set among is the identity matrix with 3 rows and 3 columns, is the transpose operator, for example For Perform transpose operation;

[0062] Step S206: Based on the first constraint set and the second constraint set , build optimization frame pose The objective function ,in, The first constraint set The point-probability line distance optimization constraints, is the first constraint set The number of point-probability line distance optimization constraints included, The second constraint set The Point-probability surface distance optimization constraints, is the second constraint set The number of point-probability surface distance optimization constraints included; Refers to the absolute value operation, is the Huber kernel function. By minimizing the objective function , to obtain :

[0063] (1)

[0064] in, is a mathematical operator used to find the function Get the minimum value . Use the Solve function in the Ceres library to solve the formula get ,in and are the pose estimation results of the current frame respectively. Rotation matrix and translation vector;

[0065] Step S30, projecting the feature points in the feature point set of the current frame into the corresponding feature point map according to the pose estimation result of the current frame, thereby completing the update of the target feature point map;

[0066] In this embodiment, using Will Edge feature points in and plane feature points Project them into the world coordinate system respectively to get the projection points and , all the projected points and Add to edge feature point map respectively and plane feature point maps In the process, the target feature point map is updated.

[0067] A robot laser radar odometer system according to a second embodiment of the present invention is as follows: Figure 2 As shown, including:

[0068] The first pose estimation module 100 is configured to sense the environment of the robot through the laser radar sensor, obtain three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the laser radar sensor, extract feature points based on the three-dimensional point cloud data, and then estimate the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system;

[0069] The second pose estimation module 200 is configured to project the feature points extracted from the current frame onto the target feature point map to find the corresponding local neighborhood point set based on the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system; obtain the corresponding local neighborhood representation based on the local neighborhood point set corresponding to each feature point, and then construct a probabilistic distance optimization constraint to construct a pose optimization function to achieve the estimation of the second pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, and use the estimated second pose as the pose estimation result of the current frame; the probabilistic distance optimization constraint includes a point-probability line distance optimization constraint and a point-probability surface distance optimization constraint;

[0070] The map updating module 300 is configured to project the feature points in the feature point set of the current frame into the corresponding feature point map according to the pose estimation result of the current frame, thereby completing the update of the target feature point map.

[0071] Those skilled in the art will clearly understand that, for the convenience and brevity of description, the specific working process and related instructions of the system described above can refer to the corresponding process in the aforementioned method embodiment and will not be repeated here.

[0072] It should be noted that the above embodiment provides a robot lidar odometry system, which is illustrated only by the division of the above functional modules. In actual applications, the above functions can be assigned to different functional modules as needed, that is, the modules or steps in the embodiments of the present invention can be further decomposed or combined. For example, the modules in the above embodiments can be combined into one module, or further divided into multiple sub-modules to complete all or part of the functions described above. The names of the modules and steps involved in the embodiments of the present invention are only for the purpose of distinguishing the modules or steps and are not to be considered as improper limitations of the present invention.

[0073] A storage device according to a third embodiment of the present invention stores a plurality of programs, wherein the programs are suitable for being loaded by a processor and implementing the above-mentioned robot laser radar odometer method.

[0074] A processing device of the fourth embodiment of the present invention includes a processor and a storage device; the processor is suitable for executing various programs; the storage device is suitable for storing multiple programs; the programs are suitable for being loaded and executed by the processor to implement the above-mentioned robot lidar odometry method.

[0075] Those skilled in the art will clearly understand that, for the convenience and brevity of description, the specific working processes and related instructions of the storage device and processing device described above can refer to the corresponding processes in the aforementioned method examples and will not be repeated here.

[0076] Those skilled in the art should be able to appreciate that the modules and method steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of the two, and the programs corresponding to the software modules and method steps can be placed in random access memory (RAM), internal memory, read-only memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disks, removable disks, CD-ROMs, or any other form of storage medium known in the art. In order to clearly illustrate the interchangeability of electronic hardware and software, the above description has generally described the components and steps of each example according to their functions. Whether these functions are performed in electronic hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art may use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0077] The terms "first", "second", "third", etc. are used to distinguish similar objects rather than to describe or indicate a particular order or sequence.

[0078] Thus far, the technical solutions of the present invention have been described in conjunction with the preferred embodiments shown in the accompanying drawings. However, it will be readily understood by those skilled in the art that the scope of protection of the present invention is obviously not limited to these specific embodiments. Without departing from the principles of the present invention, those skilled in the art may make equivalent changes or substitutions to the relevant technical features, and the technical solutions after such changes or substitutions will fall within the scope of protection of the present invention.

Claims

1. A robot laser radar odometry method, characterized in that: The method comprises the following steps: Step S10: The robot senses the environment through the laser radar sensor and obtains three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the laser radar sensor; based on the three-dimensional point cloud data, feature points are extracted, and then the first position of the laser radar sensor current frame coordinate system in the world coordinate system is estimated; Step S20: projecting the feature points extracted from the current frame onto the target feature point map to find a corresponding local neighborhood point set based on the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system; obtaining a corresponding local neighborhood representation based on the local neighborhood point set corresponding to each feature point, and then constructing a probabilistic distance optimization constraint to construct a pose optimization function, thereby estimating the second pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, and using the estimated second pose as the pose estimation result of the current frame; the probabilistic distance optimization constraint includes a point-probability line distance optimization constraint and a point-probability surface distance optimization constraint; Step S30 , projecting the feature points in the feature point set of the current frame into the corresponding feature point map according to the pose estimation result of the current frame, thereby completing the update of the target feature point map.

2. A robot laser radar odometer method according to claim 1, characterized in that: Based on the three-dimensional point cloud data, feature points are extracted to estimate the first position of the current frame coordinate system of the lidar sensor in the world coordinate system. The method is as follows: Based on the three-dimensional point cloud data , use the LOAM method to extract feature points and form a feature point set ; The feature points include edge feature points and plane feature points; Using LOAM frame matching, according to the and the previous frame of 3D point cloud data , estimate the current frame coordinate system of the lidar sensor Relative to the previous frame's lidar sensor coordinate system Pose transformation ; Based on the , the previous frame lidar sensor coordinate system Relative to the world coordinate system The first pose of In the world coordinate system The first position , and They are respectively The rotation matrix and translation vector.

3. A robot laser radar odometer method according to claim 2, characterized in that: Project the feature points extracted from the current frame onto the target feature point map to find the corresponding local neighborhood point set. The method is: The Edge feature points in and plane feature points Project them into the world coordinate system to get the projection points and , ; and They are respectively In the world coordinate system The second posture The rotation matrix and translation vector of ; Use the KdTree function of the PCL library to exist Perform KD-Tree search on the exist Perform KD-Tree search in and get the In the The local neighborhood point set in and the In the The local neighborhood point set in ;in, , Represents the target feature point map, 、 They represent edge feature point map and plane feature point map respectively.

4. A robot laser radar odometer method according to claim 3, characterized in that: Based on the local neighborhood point set corresponding to each feature point, the corresponding local neighborhood representation is obtained as follows: Use the computeMeanAndCovarianceMatrix function in the PCL library to obtain the The corresponding mean and the covariance matrix ; Use the SelfAdjointEigenSolver class in the Eigen library to solve the Perform eigenvalue decomposition to obtain eigenvalues 、 、 , and the corresponding eigenvectors 、 、 ,in, When satisfied When constructing the The corresponding probability line representation , is the set linear structure judgment threshold, The parameters are recorded as , , ; Use the computeMeanAndCovarianceMatrix function in the PCL library to obtain the The corresponding mean and the covariance matrix ; Use the SelfAdjointEigenSolver class in the Eigen library to solve the Perform eigenvalue decomposition to obtain eigenvalues 、 、 , and the corresponding eigenvectors 、 、 ,in, When satisfied When constructing the Corresponding probability surface representation , is the set plane structure judgment threshold, The parameters are recorded as , , .

5. A robot laser radar odometer method according to claim 4, characterized in that: Construct probabilistic distance optimization constraints, including point-probabilistic line distance optimization constraints and point-probabilistic surface distance optimization constraints, and the method is as follows: The point-probability line distance optimization constraint : ； Constrain Add to the first constraint set middle; The point-probability surface distance optimization constraint : ； Constrain Add to the second constraint set middle; in, is the identity matrix with 3 rows and 3 columns, is the transpose operator.

6. A robot laser radar odometer method according to claim 5, characterized in that: Construct a pose optimization function to estimate the second pose of the current frame coordinate system of the lidar sensor in the world coordinate system, and use the estimated second pose as the pose estimation result of the current frame. The method is as follows: Based on the first constraint set and the second constraint set , build optimization frame pose The objective function : ； in, The first constraint set The point-probability line distance optimization constraints, is the first constraint set The number of point-probability line distance optimization constraints included, The second constraint set The Point-probability surface distance optimization constraints, is the second constraint set The number of point-probability surface distance optimization constraints included; Refers to the absolute value operation, is the Huber kernel function; Minimize the objective function and use the Solve function in the Ceres library to solve it and get the pose estimation result of the current frame , and The rotation matrix and translation vector of the pose estimation result of the current frame are respectively.

7. A robot laser radar odometer method according to claim 6, characterized in that: According to the pose estimation result of the current frame, the feature points in the feature point set of the current frame are projected into the corresponding feature point map to complete the update of the target feature point map, and the method is as follows: Using the The Edge feature points in and plane feature points Project them into the world coordinate system respectively to get the projection points and , all the projected points and Add to the edge feature point map respectively and the plane feature point map In the step 3, the update of the target feature point map is completed.

8. A robot laser radar odometer system, characterized in that: The system includes: The first pose estimation module is configured to sense the environment of the robot through the laser radar sensor, obtain three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the laser radar sensor, extract feature points based on the three-dimensional point cloud data, and then estimate the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system; The second pose estimation module is configured to project the feature points extracted from the current frame onto the target feature point map to find the corresponding local neighborhood point set based on the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system; obtain the corresponding local neighborhood representation based on the local neighborhood point set corresponding to each feature point, and then construct a probabilistic distance optimization constraint to construct a pose optimization function to achieve the estimation of the second pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, and use the estimated second pose as the pose estimation result of the current frame; the probabilistic distance optimization constraint includes a point-probability line distance optimization constraint and a point-probability surface distance optimization constraint; The map updating module is configured to project the feature points in the feature point set of the current frame into the corresponding feature point map according to the pose estimation result of the current frame, thereby completing the update of the target feature point map.

9. A storage device storing a plurality of programs, characterized in that: The program is suitable for being loaded and executed by a processor to implement a robot lidar odometry method as described in any one of claims 1-7.

10. A processing device comprising a processor and a storage device; the processor is adapted to execute various programs; the storage device is adapted to store a plurality of programs; and the processing device comprises: The program is suitable for being loaded and executed by a processor to implement a robot lidar odometry method as described in any one of claims 1-7.

Images