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 existing point-distribution constraint does not fully consider the geometric properties of voxels, so that the pose estimation precision is reduced. The method comprises the following steps: acquiring surrounding environment three-dimensional point cloud data; calculating a first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system; extracting a mean value and a covariance matrix of source points from the three-dimensional point cloud data; projecting the extracted mean value and covariance matrix of each source point into a voxel map, and searching a corresponding local neighborhood voxel set; calculating a feature value corresponding to the local neighborhood voxel set and a corresponding feature vector, and constructing a point-geometric distribution distance constraint to construct a pose optimization function so as to realize estimation of a second pose; and projecting the point cloud of the current frame into the voxel map according to the pose estimation result of the current frame to complete updating of the voxel 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] As robots are widely used in industries and homes, precise positioning has become crucial for their reliable execution. Whether for automated transportation or providing various services in the home, accurate information about their location and surroundings is crucial. As the core sensor for high-precision positioning, LiDAR is becoming a mainstream choice due to its robustness to lighting changes, wide field of view, and precise ranging capabilities.

[0003] To achieve pose estimation, lidar odometry determines the robot's position and pose in space by aligning the three-dimensional spatial data acquired by the sensor with known reference data. Two typical types of lidar odometry methods are feature-based and distribution-based. Feature-based methods extract features from the raw point cloud for pose estimation, which may result in information loss. In contrast, distribution-based methods do not rely on feature point extraction. Instead, they model the point cloud as a probability distribution and utilize all point cloud information for pose estimation, offering superior accuracy. Representative methods include the Normal Distribution Transform (NDT). Researchers used the C++ programming language and various efficient libraries to achieve this. For example, they called the computeMeanAndCovarianceMatrix function in the PCL library to solve for the mean and covariance matrices of the point cloud data; used the SelfAdjointEigenSolver class in the Eigen library to perform eigenvalue decomposition on the matrix; and utilized Eigen's ldlt solver to solve the incremental equations corresponding to the pose optimization function.

[0004] As a representative distribution-based method, NDT discretizes the target point cloud into a voxel map, approximating the point cloud within each voxel as a three-dimensional normal distribution. By projecting the original point cloud onto the voxel map, data associations are formed between the points in the original point cloud and the corresponding projected voxels. Based on this, point-to-voxel distance constraints are constructed to achieve pose estimation. However, this point-to-voxel distance constraint does not fully consider the geometric properties of the voxels, reducing the accuracy of pose estimation. Therefore, finding a solution to this problem is currently a challenge for those skilled in the art. Summary of the Invention

[0005] In order to solve the above-mentioned problem in the prior art, that is, to solve the problem that the existing point-voxel distance constraint does not fully consider the geometric properties of the voxel, thereby reducing the accuracy of pose estimation, the present invention proposes a robot lidar odometry method, which includes the following steps:

[0006] Step S10: The robot senses the environment through the lidar sensor and obtains three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the lidar sensor;

[0007] Step S20, calculating the first position of the current frame coordinate system of the laser radar sensor in the world coordinate system;

[0008] Step S30, extracting the mean and covariance matrix of the source points from the three-dimensional point cloud data; the source points are points in the three-dimensional point cloud data;

[0009] Step S40: Based on the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, the mean and covariance matrix of each extracted source point are projected into the voxel map to find the corresponding local neighborhood voxel set; the eigenvalues ​​and corresponding eigenvectors corresponding to the local neighborhood voxel set are calculated, and a point-geometric distribution distance constraint is constructed to construct a pose optimization function to achieve an estimation of the second pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, and the estimated second pose is used as the pose estimation result of the current frame; the point-geometric distribution distance constraint includes a point-voxel geometric line distribution distance constraint and a point-voxel geometric surface distribution distance constraint;

[0010] Step S50: Projecting the point cloud of the current frame into the voxel map according to the pose estimation result of the current frame, thereby completing the voxel map update.

[0011] In some preferred embodiments, the first pose of the current frame coordinate system of the lidar sensor in the world coordinate system is calculated as follows:

[0012] when or hour, , is the identity matrix with 3 rows and 3 columns, is a zero vector with 3 rows and 1 column, The current frame coordinate system of the lidar sensor The first pose in the world coordinate system, is the index of the frame number;

[0013] when hour, ,in, and are the rotation matrix and translation vector of the first pose, and They are The rotation matrix and translation vector of the pose estimation result of the frame, and They are The rotation matrix and translation vector of the pose estimation result of the frame; world coordinate system The first frame coordinate system of the lidar sensor coincide.

[0014] In some preferred embodiments, the mean and covariance matrix of the source points are extracted from the three-dimensional point cloud data by:

[0015] Use the KdTree function of the PCL library to extract the three-dimensional point cloud data Search source points Neighborhood point set ;

[0016] Use the computeMeanAndCovarianceMatrix function in the PCL library to calculate the neighborhood point set Calculation source point The mean and source point The covariance matrix of .

[0017] In some preferred embodiments, based on the first pose of the current frame coordinate system of the lidar sensor in the world coordinate system, the mean and covariance matrix of each extracted source point are projected into the voxel map to find the corresponding local neighborhood voxel set, and the method is as follows:

[0018] The first posture As stated In the world coordinate system The second posture The initial value of , ; and are the rotation matrix and translation vector of the second pose respectively;

[0019] Use the initial value of the second pose to set the mean of the source point and the covariance matrix Project it into the voxel map and get the projection result as well as ,in, is the transpose operator;

[0020] Use the KdTree function of the PCL library to Perform a KD-Tree search in the voxel map and select the nearest neighbors Voxels, forming a local neighborhood voxel set , is the number of neighboring voxels.

[0021] In some preferred embodiments, the eigenvalues ​​and corresponding eigenvectors corresponding to the local neighborhood voxel set are calculated to construct a point-geometric distribution distance constraint, and the method is:

[0022] Compute local neighborhood voxel sets The mean and the covariance matrix ;

[0023] Use the SelfAdjointEigenSolver class in the Eigen library to Perform eigenvalue decomposition to obtain eigenvalues 、 、 , and the corresponding eigenvectors 、 、 ,in ;

[0024] When satisfied and When, among them, Represents the local linear structure judgment threshold, indicating The corresponding local neighborhood is a linear structure, called a voxel geometry line. At this time, the point-voxel geometry line distribution distance constraint is calculated:

[0025] ;

[0026] in, , is the identity matrix with 3 rows and 3 columns, is the transpose operator, Operators for finding the determinant of a matrix;

[0027] When satisfied and When, among them, Represents the local plane structure judgment threshold, indicating The corresponding local neighborhood is a planar structure, called a voxel geometry surface. At this time, the point-voxel geometry surface distribution distance constraint is calculated:

[0028] ;

[0029] in, .

[0030] 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 method is as follows:

[0031] Based on all point-voxel geometry line distribution distance constraints and all point-voxel geometry surface distribution distance constraints, the optimization frame pose The objective function is:

[0032] ;

[0033] in, It is Point-voxel geometry line distribution distance constraints, It is Point-voxel geometric surface distribution distance constraints, Refers to the absolute value operation, is the number of point-voxel geometry line distribution distance constraints formed, is the number of point-voxel geometric surface distribution distance constraints formed;

[0034] By minimizing the objective function , get the pose estimation result of the current frame ; and They are the pose estimation results of the current frame respectively The rotation matrix and translation vector.

[0035] In some preferred embodiments, based on the pose estimation result of the current frame, the point cloud of the current frame is projected into the voxel map to complete the voxel map update, and the method is as follows:

[0036] use Point Cloud All points in Project to the voxel map to get the projection point , and the projected point Add it to the voxel occupied by the projection point to complete the update of the voxel map.

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

[0038] The point cloud data acquisition module is configured so that the robot perceives the environment through the lidar sensor and obtains the three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the lidar sensor;

[0039] A first pose estimation module is configured to calculate the first pose of the current frame coordinate system of the lidar sensor in the world coordinate system;

[0040] a source point distribution extraction module configured to extract the mean and covariance matrix of source points from the three-dimensional point cloud data; the source points are points in the three-dimensional point cloud data;

[0041] The second pose estimation module is configured to project the mean and covariance matrix of each extracted source point into the voxel map according to the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, and find the corresponding local neighborhood voxel set; calculate the eigenvalues ​​and corresponding eigenvectors corresponding to the local neighborhood voxel set, construct a point-geometric distribution distance constraint, and thereby 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 point-geometric distribution distance constraint includes a point-voxel geometric line distribution distance constraint and a point-voxel geometric surface distribution distance constraint;

[0042] The voxel map updating module is configured to project the point cloud of the current frame into the voxel map according to the pose estimation result of the current frame, thereby completing the voxel map updating.

[0043] 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.

[0044] 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.

[0045] Beneficial effects of the present invention:

[0046] The robot laser radar odometry method of the present invention constructs point-geometric distribution distance constraints by fully considering the geometric properties of voxels, thereby improving the accuracy of pose estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] 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.

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

[0049] 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

[0050] 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.

[0051] 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.

[0052] 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.

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

[0054] Step S10: The robot senses the environment through the lidar sensor and obtains three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the lidar sensor;

[0055] Step S20, calculating the first position of the current frame coordinate system of the laser radar sensor in the world coordinate system;

[0056] Step S30, extracting the mean and covariance matrix of the source points from the three-dimensional point cloud data; the source points are points in the three-dimensional point cloud data;

[0057] Step S40: Based on the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, the mean and covariance matrix of each extracted source point are projected into the voxel map to find the corresponding local neighborhood voxel set; the eigenvalues ​​and corresponding eigenvectors corresponding to the local neighborhood voxel set are calculated, and a point-geometric distribution distance constraint is constructed to construct a pose optimization function to achieve an estimation of the second pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, and the estimated second pose is used as the pose estimation result of the current frame; the point-geometric distribution distance constraint includes a point-voxel geometric line distribution distance constraint and a point-voxel geometric surface distribution distance constraint;

[0058] Step S50: Projecting the point cloud of the current frame into the voxel map according to the pose estimation result of the current frame, thereby completing the voxel map update.

[0059] 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.

[0060] Step S10: The robot senses the environment through the lidar sensor and obtains three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the lidar sensor;

[0061] 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 (preferably Velodyne HDL64) carried by the robot. 3D point cloud data ,in is the index of the frame number, 3D point cloud data The Points, Also known as The source point.

[0062] Step S20, calculating the first position of the current frame coordinate system of the laser radar sensor in the world coordinate system;

[0063] In this embodiment, the current frame coordinate system of the laser radar sensor is calculated The first pose in the world coordinate system ,in and are the rotation matrix and translation vector of the first pose respectively; when or hour, , is the identity matrix with 3 rows and 3 columns, is a zero vector with 3 rows and 1 column; when hour, ,in and They are The rotation matrix and translation vector of the pose estimation result of the frame, and They are The rotation matrix and translation vector of the pose estimation result of the frame; world coordinate system The first frame coordinate system of the lidar sensor coincide;

[0064] Step S30, extracting the mean and covariance matrix of the source points from the three-dimensional point cloud data; the source points are points in the three-dimensional point cloud data;

[0065] In this embodiment, the KdTree function of the PCL library is used to generate the three-dimensional point cloud data. Search source points Neighborhood point set ; Then use the computeMeanAndCovarianceMatrix function in the PCL library according to the neighborhood point set Calculation source point The mean and source point The covariance matrix of ;

[0066] Step S40: Based on the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, the mean and covariance matrix of each extracted source point are projected into the voxel map to find the corresponding local neighborhood voxel set; the eigenvalues ​​and corresponding eigenvectors corresponding to the local neighborhood voxel set are calculated, and a point-geometric distribution distance constraint is constructed to construct a pose optimization function to achieve an estimation of the second pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, and the estimated second pose is used as the pose estimation result of the current frame; the point-geometric distribution distance constraint includes a point-voxel geometric line distribution distance constraint and a point-voxel geometric surface distribution distance constraint;

[0067] In this embodiment, the first position of the laser radar sensor current frame coordinate system in the world coordinate system is , extract the source point of the current frame point cloud The mean and the covariance matrix Project it into the voxel map and find the corresponding local neighborhood voxel set; calculate the eigenvalues ​​and corresponding eigenvectors corresponding to the local neighborhood voxel set, construct the point-geometric distribution distance constraint, and thus construct the pose optimization function to achieve the second pose of the current frame coordinate system of the lidar sensor in the world coordinate system. Estimates, and are respectively the rotation matrix and translation vector of the second pose; the estimated second pose is used as the pose estimation result of the current frame; the point-geometric distribution distance constraint includes a point-voxel geometric line distribution distance constraint and a point-voxel geometric surface distribution distance constraint;

[0068] Step S401: As The initial value of , ;

[0069] Step S402, using The initial value of the source point is the mean and the covariance matrix Project it into the voxel map and get the projection result as well as ,in, is the transpose operator; then the KdTree function of the PCL library is used to Perform a KD-Tree search in the voxel map and select the nearest neighbors Voxels, forming a local neighborhood voxel set ,in, is the number of neighboring voxels, Preferably 7;

[0070] Step S403, using Perform statistical analysis; specifically, calculate the local neighborhood voxel set The mean and the covariance matrix ,in, , , yes Middle Voxel The number of points in , , , is a voxel The points; use the SelfAdjointEigenSolver class in the Eigen library to Perform eigenvalue decomposition to obtain eigenvalues 、 、 , and the corresponding eigenvectors 、 、 ,in When satisfied and When, among them, Represents the local linear structure determination threshold. In the present invention, The preferred value is 0.75, indicating The corresponding local neighborhood is a linear structure, called a voxel geometry line. At this time, the point-voxel geometry line distribution distance constraint is calculated ,in, , is the identity matrix with 3 rows and 3 columns, is the transpose operator, is an operator for finding the determinant of a matrix; when and When represents the local plane structure judgment threshold, which is preferably 0.7 in the present invention, indicating that The corresponding local neighborhood is a planar structure, called a voxel geometry surface. At this time, the point-voxel geometry surface distribution distance constraint is calculated ,in, ;

[0071] Step S404: Based on all point-voxel geometric line distribution distance constraints and all point-voxel geometric surface distribution distance constraints, construct the optimized frame pose The objective function ,in, It is Point-voxel geometry line distribution distance constraints, It is Point-voxel geometric surface distribution distance constraints, Refers to the absolute value operation, is the number of point-voxel geometry line distribution distance constraints formed, Is the number of point-voxel geometric surface distribution distance constraints formed; by minimizing the objective function , get the pose estimation result of the current frame :

[0072] (1)

[0073] in, is a mathematical operator used to find the function Get the minimum value ; Use Eigen's ldlt solver to solve the formula get ,in and They are the pose estimation results of the current frame respectively The rotation matrix and translation vector of ;

[0074] Step S50, projecting the point cloud of the current frame into the voxel map according to the pose estimation result of the current frame, completing the voxel map update;

[0075] In this embodiment, using Point Cloud All points in Project to the voxel map to get the projection point , where the voxel resolution is preferably 0.5 meters, and the projected points Add it to the voxel occupied by the projection point to complete the update of the voxel map.

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

[0077] The point cloud data acquisition module 100 is configured so that the robot perceives the environment through the laser radar sensor and obtains the three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the laser radar sensor;

[0078] A first pose estimation module 200 is configured to calculate the first pose of the current frame coordinate system of the lidar sensor in the world coordinate system;

[0079] A source point distribution extraction module 300 is configured to extract the mean and covariance matrix of source points from the three-dimensional point cloud data; the source points are points in the three-dimensional point cloud data;

[0080] The second pose estimation module 400 is configured to project the extracted mean and covariance matrix of each source point into the voxel map according to the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, and find the corresponding local neighborhood voxel set; calculate the eigenvalues ​​and corresponding eigenvectors corresponding to the local neighborhood voxel set, construct a point-geometric distribution distance constraint, and thereby 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 point-geometric distribution distance constraint includes a point-voxel geometric line distribution distance constraint and a point-voxel geometric surface distribution distance constraint;

[0081] The voxel map updating module 500 is configured to project the point cloud of the current frame into the voxel map according to the pose estimation result of the current frame, thereby completing the voxel map update.

[0082] 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.

[0083] 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.

[0084] 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.

[0085] 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.

[0086] 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.

[0087] 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.

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

[0089] 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 lidar sensor and obtains three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the lidar sensor; Step S20, calculating the first position of the current frame coordinate system of the laser radar sensor in the world coordinate system; Step S30, extracting the mean and covariance matrix of the source points from the three-dimensional point cloud data; the source points are points in the three-dimensional point cloud data; Step S40: Based on the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, the mean and covariance matrix of each extracted source point are projected into the voxel map to find the corresponding local neighborhood voxel set; the eigenvalues ​​and corresponding eigenvectors corresponding to the local neighborhood voxel set are calculated, and a point-geometric distribution distance constraint is constructed to construct a pose optimization function to achieve an estimation of the second pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, and the estimated second pose is used as the pose estimation result of the current frame; the point-geometric distribution distance constraint includes a point-voxel geometric line distribution distance constraint and a point-voxel geometric surface distribution distance constraint; Step S50: Projecting the point cloud of the current frame into the voxel map according to the pose estimation result of the current frame, thereby completing the voxel map update.

2. A robot laser radar odometer method according to claim 1, characterized in that: The first pose of the current frame coordinate system of the lidar sensor in the world coordinate system is calculated as follows: when or hour, , is the identity matrix with 3 rows and 3 columns, is a zero vector with 3 rows and 1 column, The current frame coordinate system of the lidar sensor The first pose in the world coordinate system, is the index of the frame number; when hour, ,in, and are the rotation matrix and translation vector of the first pose, and They are The rotation matrix and translation vector of the pose estimation result of the frame, and They are The rotation matrix and translation vector of the pose estimation result of the frame; world coordinate system The first frame coordinate system of the lidar sensor coincide.

3. A robot laser radar odometer method according to claim 2, characterized in that: The method for extracting the mean and covariance matrix of the source points from the three-dimensional point cloud data is as follows: Use the KdTree function of the PCL library to extract the three-dimensional point cloud data Search source points Neighborhood point set ; Use the computeMeanAndCovarianceMatrix function in the PCL library to calculate the neighborhood point set Calculation source point The mean and source point The covariance matrix of .

4. A robot laser radar odometer method according to claim 3, characterized in that: According to the first pose of the current frame coordinate system of the lidar sensor in the world coordinate system, the mean and covariance matrix of each source point extracted are projected into the voxel map to find the corresponding local neighborhood voxel set. The method is as follows: The first posture As stated In the world coordinate system The second posture The initial value of , ; and are the rotation matrix and translation vector of the second pose respectively; Use the initial value of the second pose to set the mean of the source point and the covariance matrix Project it into the voxel map and get the projection result as well as ,in, is the transpose operator; Use the KdTree function of the PCL library to Perform a KD-Tree search in the voxel map and select the nearest neighbors Voxels, forming a local neighborhood voxel set , is the number of neighboring voxels.

5. A robot laser radar odometer method according to claim 4, characterized in that: Calculate the eigenvalues ​​and corresponding eigenvectors corresponding to the local neighborhood voxel set and construct a point-geometric distribution distance constraint. The method is: Compute local neighborhood voxel sets The mean and the covariance matrix ; Use the SelfAdjointEigenSolver class in the Eigen library to Perform eigenvalue decomposition to obtain eigenvalues 、 、 , and the corresponding eigenvectors 、 、 ,in ; When satisfied and When, among them, Represents the local linear structure judgment threshold, indicating The corresponding local neighborhood is a linear structure, called a voxel geometry line. At this time, the point-voxel geometry line distribution distance constraint is calculated: ； in, , is the identity matrix with 3 rows and 3 columns, is the transpose operator, Operators for finding the determinant of a matrix; When satisfied and When, among them, Represents the local plane structure judgment threshold, indicating The corresponding local neighborhood is a planar structure, called a voxel geometry surface. At this time, the point-voxel geometry surface distribution distance constraint is calculated: ； in, .

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. The method is as follows: Based on all point-voxel geometry line distribution distance constraints and all point-voxel geometry surface distribution distance constraints, the optimization frame pose The objective function is: ； in, It is Point-voxel geometry line distribution distance constraints, It is Point-voxel geometric surface distribution distance constraints, Refers to the absolute value operation, is the number of point-voxel geometry line distribution distance constraints formed, is the number of point-voxel geometric surface distribution distance constraints formed; By minimizing the objective function , get the pose estimation result of the current frame ; and They are the pose estimation results of the current frame respectively The rotation matrix and translation vector.

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 point cloud of the current frame is projected into the voxel map to complete the voxel map update, and the method is as follows: use Point Cloud All points in Project to the voxel map to get the projection point , and the projected point Add it to the voxel occupied by the projection point to complete the update of the voxel map.

8. A robot laser radar odometer system, characterized in that: include: The point cloud data acquisition module is configured so that the robot perceives the environment through the lidar sensor and obtains the three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the lidar sensor; A first pose estimation module is configured to calculate the first pose of the current frame coordinate system of the lidar sensor in the world coordinate system; a source point distribution extraction module configured to extract the mean and covariance matrix of source points from the three-dimensional point cloud data; the source points are points in the three-dimensional point cloud data; The second pose estimation module is configured to project the mean and covariance matrix of each extracted source point into the voxel map according to the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, and find the corresponding local neighborhood voxel set; calculate the eigenvalues ​​and corresponding eigenvectors corresponding to the local neighborhood voxel set, construct a point-geometric distribution distance constraint, and thereby 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 point-geometric distribution distance constraint includes a point-voxel geometric line distribution distance constraint and a point-voxel geometric surface distribution distance constraint; The voxel map updating module is configured to project the point cloud of the current frame into the voxel map according to the pose estimation result of the current frame, thereby completing the voxel map updating.

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