A robot lidar odometry method, system, and device

Abstract

This invention belongs to the field of robotics technology, specifically relating to a method, system, and device for robot lidar odometry. It aims to address the problem that existing point-distribution constraints do not fully consider the geometric properties of voxels, thus reducing pose estimation accuracy. The method includes: acquiring 3D point cloud data of the surrounding environment; calculating the first pose of the lidar sensor's current frame coordinate system in the world coordinate system; extracting the mean and covariance matrices of the source points from the 3D point cloud data; projecting the extracted mean and covariance matrices of each source point onto a voxel map to find the corresponding local neighborhood voxel set; calculating the eigenvalues ​​and corresponding eigenvectors of the local neighborhood voxel set to construct a point-geometric distribution distance constraint, thereby constructing a pose optimization function to achieve the estimation of the second pose; and projecting the current frame point cloud onto the voxel map based on the pose estimation result of the current frame to complete the voxel map update. This invention improves the accuracy of pose estimation.

Description

Technical Field

[0001] This invention relates to the field of robotics, and specifically to a method, system, and device for robot lidar odometry. Background Technology

[0002] With the widespread application of robots in industry, homes, and other fields, precise positioning has become crucial for their reliable task execution. Whether it's automated material handling or providing various services in the home, accurately knowing one's own location and information about the surrounding environment is essential. LiDAR, as a core sensor for achieving high-precision positioning, is gradually becoming the mainstream choice due to its high robustness to changes in lighting, wide field of view, and accurate ranging capabilities.

[0003] To achieve pose estimation, lidar odometry determines the robot's position and orientation in space by aligning and integrating the 3D spatial data acquired by the sensor with known reference data. Feature-based lidar odometry methods and distribution-based lidar odometry methods are two typical types. 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; they model the point cloud as a probability distribution and utilize all point cloud information for pose estimation, offering an accuracy advantage. Representative methods include NDT (Normal Distribution Transform). Researchers used the C++ programming language, along with various efficient libraries, to meet the requirements. For example, they called the computeMeanAndCovarianceMatrix function in the PCL library to solve for the mean and covariance matrices of the point cloud data; they used the SelfAdjointEigenSolver class in the Eigen library to perform eigenvalue decomposition on the matrix; and they used 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, points in the original point cloud form data associations with their corresponding projected voxels. Based on this, point-voxel distance constraints are constructed to achieve pose estimation. However, this point-voxel distance constraint does not fully consider the geometric properties of voxels, reducing the accuracy of pose estimation. Therefore, proposing a solution to address the above problems is a current challenge for those skilled in the art. Summary of the Invention

[0005] To address the aforementioned problems in the prior art, specifically the issue that existing point-voxel distance constraints do not adequately consider the geometric properties of voxels, thus reducing pose estimation accuracy, this invention proposes a robot lidar odometry method, which includes the following steps:

[0006] Step S10: 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.

[0007] Step S20: Calculate the first pose of the current frame coordinate system of the lidar sensor in the world coordinate system;

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

[0009] Step S40: Based on the first pose of the current frame coordinate system of the lidar sensor in the world coordinate system, project the extracted mean and covariance matrix of each source point onto the voxel map to 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 construct the 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 point-geometric distribution distance constraint includes the point-voxel geometric line distribution distance constraint and the point-voxel geometric surface distribution distance constraint;

[0010] Step S50: Based on the pose estimation result of the current frame, project the point cloud of the current frame onto the voxel map to complete the voxel map update.

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

[0012] when or hour, , It is a 3x3 identity matrix. It is a 3x1 zero vector. The current frame coordinate system of the lidar sensor The first pose in the world coordinate system The index for the frame number;

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

[0014] In some preferred embodiments, the mean and covariance matrix of the source points are extracted from the 3D point cloud data, and the method is as follows:

[0015] Using the KdTree function of the PCL library on 3D point cloud data Search source neighborhood point set ;

[0016] Using the computeMeanAndCovarianceMatrix function in the PCL library based on the neighborhood point set Calculation source point mean and source point covariance matrix .

[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 extracted mean and covariance matrix of each source point are projected onto the voxel map to find the corresponding local neighborhood voxel set. The method is as follows:

[0018] The first pose As mentioned In the world coordinate system The second pose below The initial value, i.e. , ; and These are the rotation matrix and translation vector of the second pose, respectively;

[0019] The average of the source points is obtained using the initial value of the second pose. Covariance Matrix Projecting onto a voxel map yields the projection result. as well as ,in, It is the transpose operator;

[0020] Using the KdTree function of the PCL library to Perform a KD-Tree search on the voxel map and select the nearest neighbors. Individual elements constitute a local neighborhood set of voxels. , 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 the point-geometric distribution distance constraint, and the method is as follows:

[0022] Calculate the local neighborhood voxel set mean Covariance Matrix ;

[0023] Using the SelfAdjointEigenSolver class from the Eigen library Perform eigenvalue decomposition to obtain eigenvalues , , and the corresponding feature vectors , , ,in ;

[0024] When satisfied and At that time, among them, This indicates the threshold for determining local linear structures, signifying... The corresponding local neighborhood has a linear structure, called a voxel geometry line. In this case, the point-to-voxel geometry line distribution distance constraint is calculated as follows:

[0025] ;

[0026] in, , It is a 3x3 identity matrix. The transpose operator, Operators for calculating the determinant of a matrix;

[0027] When satisfied and At that time, among them, This indicates the threshold for determining local planar structures, signifying... The corresponding local neighborhood is planar, referred to as a voxel geometry surface. In this case, the distance constraint between the computation point and the voxel geometry surface distribution is:

[0028] ;

[0029] in, .

[0030] In some preferred embodiments, a pose optimization function is constructed to estimate the second pose of the lidar sensor in the world coordinate system within the current frame coordinate system. The method is as follows:

[0031] Based on all point-voxel geometric line distribution distance constraints and all point-voxel geometric surface distribution distance constraints, construct the optimization... frame pose Objective function:

[0032] ;

[0033] in, It is the first Point-voxel geometric line distribution distance constraint It is the first Point-voxel geometric surface distribution distance constraint This refers to the absolute value operation. It is the number of point-voxel geometric line distribution distance constraints formed. It is the number of point-voxel geometric surface distribution distance constraints formed;

[0034] By minimizing the objective function To obtain the pose estimation result for the current frame. ; and These are the pose estimation results for the current frame. 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 onto the voxel map to complete the voxel map update. The method is as follows:

[0036] use Point cloud All points in Projecting onto a voxel map yields projection points. and project the point Adding to the voxel occupied by the projection point completes the voxel map update.

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

[0038] The point cloud data acquisition module is configured to enable the robot to perceive the environment through the lidar sensor and acquire the three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the lidar sensor.

[0039] The 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] The source point distribution extraction module is configured to extract the mean and covariance matrix of the source points from the three-dimensional point cloud data; the source points are the points in the three-dimensional point cloud data.

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

[0042] The voxel map update module is configured to project the point cloud of the current frame onto the voxel map based on the pose estimation result of the current frame, thereby completing the voxel map update.

[0043] In a third aspect, the present invention provides a storage device storing a plurality of programs adapted to be loaded and executed by a processor to implement the above-described robot lidar odometry method.

[0044] In a fourth aspect, the present invention provides a processing apparatus, 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-described robot lidar odometry method.

[0045] The beneficial effects of this invention are:

[0046] The robot lidar odometry method of the present invention improves the accuracy of pose estimation by fully considering the geometric properties of voxels and constructing a point-geometric distribution distance constraint. Attached Figure Description

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

[0048] Figure 1 This is a flowchart illustrating a robot lidar odometry method according to an embodiment of the present invention;

[0049] Figure 2 This is a schematic diagram of the framework of a robot lidar odometer system according to an embodiment of the present invention. Detailed Implementation

[0050] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0051] The present application will now be described in further detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are for illustrative purposes only and are not intended to limit the invention. Furthermore, it should be noted that, for ease of description, only the parts relevant to the invention are shown in the accompanying drawings.

[0052] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other.

[0053] The present invention provides a robot lidar odometry method, such as... Figure 1 As shown, the method includes the following steps:

[0054] Step S10: 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.

[0055] Step S20: Calculate the first pose of the current frame coordinate system of the lidar sensor in the world coordinate system;

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

[0057] Step S40: Based on the first pose of the current frame coordinate system of the lidar sensor in the world coordinate system, project the extracted mean and covariance matrix of each source point onto the voxel map to 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 construct the 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 point-geometric distribution distance constraint includes the point-voxel geometric line distribution distance constraint and the point-voxel geometric surface distribution distance constraint;

[0058] Step S50: Based on the pose estimation result of the current frame, project the point cloud of the current frame onto the voxel map to complete the voxel map update.

[0059] To more clearly illustrate the robot lidar odometry method of the present invention, the steps of one embodiment of the method of the present invention will be described in detail below.

[0060] Step S10: 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.

[0061] In this embodiment, the robot obtains the surrounding environment in the current frame coordinate system of the lidar sensor using an onboard lidar sensor (preferably a Velodyne HDL64). 3D point cloud data ,in The index of the frame number. 3D point cloud data The first in One point, Also known as The source point.

[0062] Step S20: Calculate the first pose of the current frame coordinate system of the lidar sensor in the world coordinate system;

[0063] In this embodiment, the coordinate system of the current frame of the lidar sensor is calculated. The first position in the world coordinate system ,in and These are the rotation matrix and translation vector of the first pose, respectively; when or hour, , It is a 3x3 identity matrix. It is a 3x1 zero vector; when hour, ,in and They are the first The rotation matrix and translation vector of the pose estimation result of the frame. and They are the first The rotation matrix and translation vector of the pose estimation result of the frame; world coordinate system Coordinate system of the first frame of the lidar sensor coincide;

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

[0065] In this embodiment, the KdTree function of the PCL library is used in 3D point cloud data. Search source neighborhood point set Then, the computeMeanAndCovarianceMatrix function from the PCL library is used based on the neighborhood point set. Calculation source point mean and source point covariance matrix ;

[0066] Step S40: Based on the first pose of the current frame coordinate system of the lidar sensor in the world coordinate system, project the extracted mean and covariance matrix of each source point onto the voxel map to 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 construct the 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 point-geometric distribution distance constraint includes the point-voxel geometric line distribution distance constraint and the point-voxel geometric surface distribution distance constraint;

[0067] In this embodiment, based on the first pose of the current frame coordinate system of the lidar sensor in the world coordinate system... The source points extracted from the point cloud of the current frame. mean Covariance Matrix Projecting the data onto a voxel map, the corresponding local neighborhood voxel set is found; the eigenvalues ​​and corresponding eigenvectors of the local neighborhood voxel set are calculated, and a point-geometric distribution distance constraint is constructed. Based on this constraint, a pose optimization function is built to achieve the second pose of the lidar sensor's current frame coordinate system in the world coordinate system. The estimate, and These are the rotation matrix and translation vector of the second pose, respectively; the estimated second pose is used as the pose estimation result of the current frame; the point-geometric distribution distance constraint includes the point-voxel geometric line distribution distance constraint and the point-voxel geometric surface distribution distance constraint;

[0068] Step S401, will As The initial value, i.e. , ;

[0069] Step S402, using The initial value will be the mean of the source point. Covariance Matrix Projecting onto a voxel map yields the projection result. as well as ,in, This is the transpose operator; subsequently, the KdTree function from the PCL library is used to... Perform a KD-Tree search on the voxel map and select the nearest neighbors. Individual elements constitute a local neighborhood set of voxels. ,in, The number of nearest-neighbor voxels. The preferred value is 7;

[0070] Step S403, using Perform statistical analysis; specifically, calculate the local neighborhood voxel set. mean Covariance Matrix ,in, , , yes The Middle Individual factors The number of points in , , It is a voxel The first in One point; using the SelfAdjointEigenSolver class from the Eigen library to... Perform eigenvalue decomposition to obtain eigenvalues , , and the corresponding feature vectors , , ,in When satisfied and At that time, among them, The threshold for determining local linear structures is represented in this invention. The preferred value is 0.75, indicating The corresponding local neighborhood has a linear structure, called a voxel geometry line. In this case, the distance constraint between the computation point and the voxel geometry line distribution is calculated. ,in, , It is a 3x3 identity matrix. The transpose operator, The operator for calculating the determinant of a matrix; when the following conditions are met. and At that time, among them The threshold for determining local planar structures is preferably 0.7 in this invention. The corresponding local neighborhood has a planar structure, called a voxel geometry surface. In this case, the distance constraint between the computation point and the voxel geometry surface 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 first... frame pose objective function ,in, It is the first Point-voxel geometric line distribution distance constraint It is the first Point-voxel geometric surface distribution distance constraint This refers to the absolute value operation. It is the number of point-voxel geometric line distribution distance constraints formed. It is the number of point-voxel geometric surface distribution distance constraints formed; by minimizing the objective function To obtain the pose estimation result for the current frame. :

[0072] (1)

[0073] in, It is a mathematical operator used to find functions that make Get the minimum value Solve the formula using Eigen's ldlt solver. get ,in and These are the pose estimation results for the current frame. The rotation matrix and translation vector;

[0074] Step S50: Based on the pose estimation result of the current frame, project the point cloud of the current frame onto the voxel map to complete the voxel map update.

[0075] In this embodiment, using Point cloud All points in Projecting onto a voxel map yields projection points. The voxel resolution is preferably 0.5 meters, and the projection point is... Adding to the voxel occupied by the projection point completes the voxel map update.

[0076] A robot lidar odometry system according to a second embodiment of the present invention, such as Figure 2 As shown, the system includes:

[0077] The point cloud data acquisition module 100 is configured to allow the robot to perceive the environment through the lidar sensor and acquire the three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the lidar sensor.

[0078] The 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] The 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, based on the first pose of the current frame coordinate system of the lidar sensor in the world coordinate system, project the extracted mean and covariance matrix of each source point onto the voxel map to find the corresponding local neighborhood voxel set; calculate the eigenvalues ​​and corresponding eigenvectors corresponding to the local neighborhood voxel set, construct point-geometric distribution distance constraints, thereby constructing 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 point-geometric distribution distance constraints include point-voxel geometric line distribution distance constraints and point-voxel geometric surface distribution distance constraints;

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

[0082] Those skilled in the art will understand that, for the sake of convenience and brevity, the specific working process and related descriptions of the system described above can be found in the corresponding processes in the foregoing method embodiments, and will not be repeated here.

[0083] It should be noted that the robot lidar odometry system provided in the above embodiments is only an example of the division of the above functional modules. In practical 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 merged 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 distinguishing the various modules or steps and are not considered as an improper limitation of the present invention.

[0084] A storage device according to a third embodiment of the present invention stores a plurality of programs, the programs being adapted to be loaded by a processor and implement the above-described robot lidar odometry method.

[0085] A processing apparatus according to a fourth embodiment of the present invention includes 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-described robot lidar odometry method.

[0086] Those skilled in the art will understand that, for the sake of convenience and brevity, the specific working process and related descriptions of the storage device and processing device described above can be referred to the corresponding process in the foregoing method examples, and will not be repeated here.

[0087] Those skilled in the art will recognize that the modules and method steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of both. The programs corresponding to the software modules and method steps can be placed in random access memory (RAM), main 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. To clearly illustrate the interchangeability of electronic hardware and software, the components and steps of the various examples have been generally described in terms of functionality in the foregoing description. Whether these functions are implemented in electronic hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of the invention.

[0088] The terms “first,” “second,” “third,” etc., are used to distinguish similar objects, not to describe or indicate a specific order or sequence.

[0089] The technical solution of the present invention has been described above with reference to 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 can make equivalent changes or substitutions to the relevant technical features, and the technical solutions after these changes or substitutions will all fall within the scope of protection of the present invention.

Claims

1. A robotic laser radar odometry method, characterized by, The method comprises the following steps: Step S10, the robot perceives the environment through the laser radar sensor, and acquires three-dimensional point cloud data of the surrounding environment in a current frame coordinate system of the laser radar sensor; Step S20, a first pose of the current frame coordinate system of the laser radar sensor in a world coordinate system is calculated; Step S30, a mean value and a covariance matrix of a source point are extracted from the three-dimensional point cloud data; the source point is a point in the three-dimensional point cloud data; Step S40, the mean value and the covariance matrix of each source point are projected into a voxel map according to the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, and a corresponding local neighborhood voxel set is searched; eigenvalues corresponding to the local neighborhood voxel set and corresponding eigenvectors are calculated, a point-geometric distribution distance constraint is constructed, a pose optimization function is constructed, a second pose of the current frame coordinate system of the laser radar sensor in the world coordinate system is estimated, and the estimated second pose is taken as a pose estimation result of the current frame; Step S50, according to the pose estimation result of the current frame, the current frame point cloud is projected into the voxel map, and the voxel map is updated; The eigenvalues corresponding to the local neighborhood voxel set and the corresponding eigenvectors are calculated, and the point-geometric distribution distance constraint is constructed, and the method is as follows: Computing the mean of the local neighborhood set of voxels and the covariance matrix ;​ Eigen library is used to perform eigenvalue decomposition to obtain eigenvalues , , , , , , where ; When the following conditions are satisfied and wherein, denotes a local linear structure decision threshold, indicating the corresponding local neighborhood is a linear structure, referred to as a voxel geometric line, and the point-voxel geometric line distribution distance constraint is calculated at this time: ； wherein , is a 3 by 3 identity matrix, is a transpose operator, is an operator for calculating a determinant of a matrix, , are, respectively, a mean and a covariance matrix of the source point projected into the voxel map using the initial value of the second pose, resulting in a projection. When the following conditions are satisfied and wherein, represents a local planar structure determination threshold, indicating the corresponding local neighborhood is a planar structure, referred to as a voxel geometric face, at which point the point-voxel geometric face distribution distance constraint is calculated: ； wherein ; The pose optimization function is constructed, and the second pose of the current frame coordinate system of the laser radar sensor in the world coordinate system is estimated, and the method is as follows: Based on all the point-voxel geometric line distribution distance constraints and all the point-voxel geometric face distribution distance constraints, an optimization first Frame pose Objective function: ； wherein, is the point-voxel geometric line distribution distance constraint, is the point-voxel geometric line distribution distance constraint, is the point-voxel geometric face distribution distance constraint, is the point-voxel geometric face distribution distance constraint, refers to the absolute value operation, is the number of point-voxel geometric line distribution distance constraints formed, is the number of point-voxel geometric face distribution distance constraints formed; by minimizing an objective function , the pose estimate of the current frame is obtained ; and are the rotation matrix and the translation vector of the pose estimate of the current frame , respectively.

2. The robotic laser radar odometry method of claim 1, wherein, The first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system is calculated, and the method is as follows: When or , , is a 3x3 identity matrix, is a 3x1 zero vector, is 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 These are the rotation matrix and translation vector of the first pose, respectively. and They are the first The rotation matrix and translation vector of the pose estimation result of the frame. and They are the first The rotation matrix and translation vector of the pose estimation result of the frame; world coordinate system Coordinate system of the first frame of the lidar sensor coincide.

3. The robotic laser radar odometry method of claim 2, wherein, The mean value and the covariance matrix of the source point are extracted from the three-dimensional point cloud data, and the method is as follows: The KdTree function of the PCL library is used to search the neighborhood point set of the source point in the three-dimensional point cloud data ;​​ The mean and covariance matrix of the source point are computed using the computeMeanAndCovarianceMatrix function from the PCL library based on the neighborhood point set . .​​ 4. The robotic laser radar odometry method of claim 3, wherein, According to the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, the mean value and the covariance matrix of each source point are projected into the voxel map, and the corresponding local neighborhood voxel set is searched, and the method is as follows: the first pose as the initial value of the second pose in a world coordinate system under the first pose , i.e. , ; and are the rotation matrix and the translation vector of the second pose, respectively using the initial value of the second pose to project the mean and covariance matrix of the source points into the voxel map to obtain a projection result and wherein, is a transpose operator; The KdTree function of the PCL library is used to find the nearest neighbor of the point cloud The KD-Tree search is performed in the voxel map, and the nearest neighbor of the voxel is selected to form a local neighborhood voxel set , The number of nearest neighbor voxels.​ 5. The robotic laser radar odometry method of claim 1, wherein, According to the pose estimation result of the current frame, the current frame point cloud is projected into the voxel map, and the voxel map is updated, and the method is as follows: Utilizing projecting the point cloud all points in the point cloud to a voxel map to obtain projected points and adding the projected points to voxels occupied by the projected points, that is, completing updating of the voxel map.

6. A robotic laser radar odometry system, characterized by, It comprises: A point cloud data acquisition module is configured to acquire three-dimensional point cloud data of a surrounding environment in a current frame coordinate system of a laser radar sensor through a laser radar sensor of a robot; A first pose estimation module is configured to calculate a first pose of the current frame coordinate system of the laser radar sensor in a world coordinate system; A source point distribution extraction module is configured to extract a mean value and a covariance matrix of a source point from the three-dimensional point cloud data; the source point is a point in the three-dimensional point cloud data; The second pose estimation module is configured to project the mean and covariance matrix of each source point extracted into a voxel map according to the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, find a corresponding local neighborhood voxel set, calculate the eigenvalue and corresponding eigenvector corresponding to the local neighborhood voxel set, construct a point-geometric distribution distance constraint, and construct a pose optimization function to realize the estimation of the second pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, and take the estimated second pose as the pose estimation result of the current frame. The voxel map updating module is configured to project the current frame point cloud into the voxel map according to the pose estimation result of the current frame to complete the voxel map updating. The method for calculating the eigenvalue and corresponding eigenvector corresponding to the local neighborhood voxel set and constructing the point-geometric distribution distance constraint is as follows: Computing the mean of the local neighborhood set of voxels and the covariance matrix ;​ Eigen library is used to perform eigenvalue decomposition to obtain eigenvalues , , , and corresponding eigenvectors , , , where ; When the following conditions are satisfied and wherein, denotes a local linearity structure decision threshold, indicating that the corresponding local neighborhood is a linearity structure, called voxel geometric line, and the point-voxel geometric line distribution distance constraint is calculated at this time: ； wherein , is a 3 by 3 identity matrix, is a transpose operator, is an operator for calculating a determinant of a matrix, , are, respectively, a mean and a covariance matrix of the source point projected into the voxel map using the initial value of the second pose, resulting in a projection. When the following conditions are satisfied and wherein, represents a local planar structure decision threshold, indicating the corresponding local neighborhood is a planar structure, referred to as a voxel geometric face, at which point the point-voxel geometric face distribution distance constraint is calculated: ； wherein ; The method for constructing the pose optimization function to realize the estimation of the second pose of the current frame coordinate system of the laser radar sensor in the world coordinate system is as follows: Based on all the point-voxel geometric line distribution distance constraints and all the point-voxel geometric face distribution distance constraints, an optimization first frame pose objective function: ； wherein, is the kth point-voxel geometric line distribution distance constraint, is the kth point-voxel geometric face distribution distance constraint, refers to the absolute value operation, is the number of point-voxel geometric line distribution distance constraints formed, is the number of point-voxel geometric face distribution distance constraints formed;​​ by minimizing an objective function , the pose estimate of the current frame is obtained ; and are the rotation matrix and the translation vector of the pose estimate of the current frame , respectively.

7. A storage device in which a plurality of programs are stored, characterized by The program is suitable for being loaded and executed by a processor to realize the robot laser radar odometry method in any one of claims 1-5.

8. A processing device comprising a processor, a storage device; the processor is adapted to execute each program; the storage device is adapted to store a plurality of programs; characterized in that, The program is suitable for being loaded and executed by a processor to realize the robot laser radar odometry method in any one of claims 1-5.

Images