A robotic laser radar odometry method, system, apparatus

Abstract

The present application belongs to the field of robot technology, and particularly relates to a robot laser radar odometry method, system and device, aiming at solving the problem of inaccurate distance optimization constraint caused by using line or surface form local neighborhood representation in prior art, and affecting the pose estimation accuracy. The method comprises: obtaining three-dimensional point cloud data, and estimating a first pose of a current frame coordinate system of a laser radar sensor in a world coordinate system; according to the first pose, projecting feature points extracted from the current frame into a target feature point map to find corresponding local neighborhood point sets, obtaining local neighborhood representation, constructing a probability distance optimization constraint, and constructing a pose optimization function to realize pose estimation of the current frame coordinate system of the laser radar sensor in the world coordinate system; and according to the pose estimation result of the current frame, projecting feature points in a feature point set of the current frame into corresponding feature point maps to complete updating of the target feature point map. The present application improves the pose estimation accuracy.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robots, and particularly relates to a robot laser radar odometry method, system and device. BACKGROUND

[0002] Robots are widely used in industrial manufacturing, medical health, home service and other fields, and have important significance in improving production efficiency and improving human life. For mobile robots, obtaining high-precision positioning results is the premise of completing various tasks, and the odometry method is an important technology for robots to obtain accurate positioning information. Visual odometry and laser radar odometry are two main odometry methods. Compared with visual odometry, laser radar odometry is widely concerned due to the robustness of laser radar to light changes and the accurate ranging capability.

[0003] In order to estimate the pose of the current frame, a direct and commonly used scheme is frame-to-frame matching, that is, the pose of the current frame is obtained by registering the current frame with the last frame. This method is computationally efficient, but over time, the error between frames accumulates, affecting the accuracy of pose estimation. In order to solve this problem, researchers have proposed a frame-to-map matching strategy. Compared with frame-to-frame matching, frame-to-map matching matches the current frame with a local map containing information of multiple historical frames, effectively reduces the cumulative error with more abundant environmental information, and improves the accuracy of pose estimation. However, frame-to-map matching, although excellent in accuracy, has high computational complexity due to the need to process more point cloud data. In order to balance accuracy and efficiency, combining frame-to-frame matching and frame-to-map matching becomes the preferred solution, and representative methods include LOAM (LiDAR Odometry and Mapping), A-LOAM (Advanced LiDAR Odometry and Mapping), etc. Researchers often call third-party libraries such as PCL (Point Cloud Library), Eigen and Ceres for laser radar point cloud processing, pose optimization process, such as calling the computeMeanAndCovarianceMatrix function in the PCL library to solve the mean and covariance matrix of the point cloud data, using the KD-Tree (K-Dimensional Tree) in the PCL library for fast neighbor search of point cloud data; using the SelfAdjointEigenSolver class in the Eigen library to perform eigenvalue decomposition on the matrix; using the Solve function in the Ceres library to solve the optimization problem, etc.

[0004] As a representative laser radar odometry method, LOAM projects the feature points of the current frame into the target feature point map, finds the corresponding local neighborhood point set in the target feature point map, and then fits a line or a plane to form the association with the feature point projection, and then obtains the target function to be optimized by accumulating all point-line distance optimization constraints and point-plane distance optimization constraints, and minimizes the target function to realize the estimation of the pose. However, the local neighborhood representation in the form of line or plane is insufficient in capturing 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-plane distance optimization constraints, affecting the accuracy of the pose estimation. Therefore, how to propose a solution to the above problems is a problem that those skilled in the art need to solve at present. SUMMARY

[0005] In order to solve the above problems in the prior art, that is, to solve the problem that the existing robot laser radar odometry method uses a line or plane form of local neighborhood representation to cause inaccurate distance optimization constraints and affect the accuracy of pose estimation, the first aspect of the present application proposes a robot laser radar odometry method, which comprises the following steps:

[0006] Step S10, the robot perceives 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 a first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system is estimated;

[0007] Step S20, according to the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, the feature points extracted from the current frame are projected into the target feature point map to find the corresponding local neighborhood point set; based on the corresponding local neighborhood point set of each feature point, the corresponding local neighborhood representation is obtained, and then the probability distance optimization constraint is constructed to construct the pose optimization function, so as 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 the estimated second pose is taken as the pose estimation result of the current frame; the probability distance optimization constraint includes point-probability line distance optimization constraint and point-probability plane distance optimization constraint;

[0008] Step S30, 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, and the update of the target feature point map is completed.

[0009] In some preferred embodiments, based on the three-dimensional point cloud data, feature points are extracted, and then a first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system is estimated, and the method is:

[0010] based on the three-dimensional point cloud data The LOAM method is used to extract feature points and form a feature point set. The feature points include edge feature points and planar feature points.

[0011] Using LOAM frame-to-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 coordinate system of the lidar sensor in the previous frame pose transformation ;

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

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

[0014] The Edge feature points in and planar feature points Projected onto the world coordinate system respectively to obtain projection points and , ; and They are the aforementioned In the world coordinate system The second pose below The rotation matrix and translation vector;

[0015] Using the KdTree function of the PCL library to respectively... exist Perform a KD-Tree search on the aforementioned exist Perform a KD-Tree search to obtain the... In the Local neighborhood point set in and the In the Local neighborhood point set in ;in, , representing a target feature point map, , representing an edge feature point map, a 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 the method of:

[0017] obtaining the mean value and the covariance matrix of the local neighborhood point set corresponding to each feature point by using the computeMeanAndCovarianceMatrix function in the PCL library; Eigen decomposition of the covariance matrix is performed by using the SelfAdjointEigenSolver class in the Eigen library to obtain the eigenvalues and the corresponding eigenvectors, wherein, when the condition is satisfied, the probability line representation corresponding to the local neighborhood point set is constructed, is a set linear structure determination threshold, the parameter of is recorded as ;

[0018] obtaining the mean value and the covariance matrix of the local neighborhood point set corresponding to each feature point by using the computeMeanAndCovarianceMatrix function in the PCL library; Eigen decomposition of the covariance matrix is performed by using the SelfAdjointEigenSolver class in the Eigen library to obtain the eigenvalues and the corresponding eigenvectors, wherein, when the condition is satisfied, the probability plane representation corresponding to the local neighborhood point set is constructed, is a set plane structure determination threshold, the parameter of is recorded as ,​​​​​​​​​​​​​​​​​​​​​​​​​​​ .

[0019] In some preferred embodiments, the probability distance optimization constraints are constructed, including point-probability line distance optimization constraints and point-probability plane distance optimization constraints, in the following way:

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

[0021] ;

[0022] The constraints are added to the first constraint set ;

[0023] The point-probability plane distance optimization constraints :

[0024] ;

[0025] The constraints are added to the second constraint set ;

[0026] wherein is a 3-by-3 identity matrix, is a transpose operator.

[0027] In some preferred embodiments, the pose optimization function is constructed to achieve the estimation of the second pose of the current frame coordinate system of the lidar sensor in the world coordinate system, and the estimated second pose is taken as the pose estimation result of the current frame, in the following way:

[0028] Based on the first constraint set and the second constraint set , the objective function for optimizing the pose of the first frame is constructed as follows: :

[0029] ;

[0030] wherein is the i-th point-probability line distance optimization constraint in the first constraint set , is the number of point-probability line distance optimization constraints contained in the first constraint set , is the i-th point-probability plane distance optimization constraint in the second constraint set , is the number of point-probability plane distance optimization constraints contained in the second constraint set , is the number of point-probability plane distance optimization constraints contained in 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] Minimizing the objective function and solving by using the Solve function in the Ceres library to obtain the pose estimation result of the current frame , And Respectively, the rotation matrix and translation vector of the pose estimation result of the current frame.

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

[0033] Using the The edge feature points And the plane feature points In the Respectively projected into the world coordinate system to obtain the projection points And All projection points And Respectively added to the edge feature point map And the plane feature point map Complete the update of the target feature point map.

[0034] In the second aspect of the present application, a robot laser radar odometry system is provided, which comprises:

[0035] The first pose estimation module is configured to enable the robot to perceive the environment through the laser radar sensor, acquire 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 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 into the target feature point map according to the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system to find the corresponding local neighborhood point set; based on the corresponding local neighborhood point set of each feature point, the corresponding local neighborhood representation is obtained, and then the probability distance optimization constraint is constructed to construct the pose optimization function, 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 probability distance optimization constraint includes point-probability line distance optimization constraint and point-probability surface distance optimization constraint.

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

[0038] In a third aspect, the application provides a storage device, which stores a plurality of programs suitable for being loaded and executed by a processor to implement the robot laser radar odometry method.

[0039] In a fourth aspect, the application provides a processing device, which comprises a processor and a storage device, wherein the processor is suitable for executing each program, and the storage device is suitable for storing a plurality of programs, and the programs are suitable for being loaded and executed by the processor to implement the robot laser radar odometry method.

[0040] The application has the following beneficial effects:

[0041] The robot laser radar odometry method of the application improves the accuracy of the local neighborhood representation by representing the local neighborhood of the linear structure in the form of a probability line and representing the local neighborhood of the planar structure in the form of a probability surface, and further constructs more accurate distance optimization constraints and improves the accuracy of the pose estimation. BRIEF DESCRIPTION OF DRAWINGS

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

[0043] Figure 1 Fig. 1 is a flow diagram of a robot laser radar odometry method according to an embodiment of the application;

[0044] Figure 2 Fig. 2 is a framework diagram of a robot laser radar odometry system according to an embodiment of the application. DETAILED DESCRIPTION

[0045] In order to make the objects, technical solutions and advantages of the embodiments of the application clearer, the technical solutions in the embodiments of the application will be described clearly and completely below with reference to the drawings in the embodiments of the application. Obviously, the described embodiments are only part of the embodiments of the application, rather than all the embodiments of the application. Based on the embodiments in the application, all other embodiments obtained by those skilled in the art without creative work fall within the protection scope of the application.

[0046] The application will be described in further detail below with reference to the drawings and embodiments. It can be understood that the specific embodiments described herein are only used to explain the related application, and not to limit the application. In addition, it should be noted that only the parts related to the application are shown in the drawings for ease of description.

[0047] It should be noted that the embodiments in the present application and the features in the embodiments can be combined with each other without conflict.

[0048] A robot laser radar odometry method of the application, as shown in the figure, comprises the following steps: Figure 1

[0049] Step S10, the robot perceives the environment through the laser radar sensor, acquires three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the laser radar sensor, extracts feature points based on the three-dimensional point cloud data, and further estimates a first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system;

[0050] Step S20, according to the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, the feature points extracted from the current frame are projected into the target feature point map to find the corresponding local neighborhood point set; based on the local neighborhood point set corresponding to each feature point, the corresponding local neighborhood representation is obtained, and then a probability distance optimization constraint is constructed to construct a pose optimization function, so as to estimate a second pose of the current frame coordinate system of the laser radar sensor in the world coordinate system, and the estimated second pose is taken as the pose estimation result of the current frame; the probability distance optimization constraint comprises a point-probability line distance optimization constraint and a point-probability surface distance optimization constraint;

[0051] Step S30, 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, and the update of the target feature point map is completed.

[0052] In order to more clearly describe the robot laser radar odometry method of the application, the steps in an embodiment of the method of the application will be described in detail below.

[0053] Step S10, the robot perceives the environment through the laser radar sensor, acquires three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the laser radar sensor, extracts feature points based on the three-dimensional point cloud data, and further estimates a first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system;

[0054] In this embodiment, the robot acquires three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the laser radar sensor carried by the robot ​​wherein represents the index of the frame, the laser radar sensor is preferably Velodyne HDL64. Based on the three-dimensional point cloud data , the application preferably uses the LOAM method to extract edge feature points and plane feature points to form a feature point set wherein and represent the edge feature point set and the plane feature point set, respectively, and are the first edge feature point and the first plane feature point, respectively. Subsequently, using the frame-to-frame matching of LOAM, based on and the three-dimensional point cloud data of the previous frame, the pose transformation of the laser radar sensor coordinate system relative to the previous frame is estimated , wherein and are the rotation matrix and the translation vector of relative to . On this basis, based on , the first pose of the laser radar sensor coordinate system relative to the world coordinate system , the first pose of in the world coordinate system is obtained, wherein and are the rotation matrix and the translation vector of , respectively, and are the rotation matrix and the translation vector of . The world coordinate system is coincident with the first frame coordinate system of the laser radar sensor , when , , is a 3x3 identity matrix, is a 3x1 zero vector. The LOAM method is described in detail in the literature: J. Zhang, S. Singh. LOAM: Lidar Odometry and Mapping in Real-Time. Robotics: Science and Systems, 2014.

[0055] Step S20: Based on the first pose of the current frame coordinate system of the lidar sensor in the world coordinate system, project the feature points extracted in the current frame onto the target feature point map to find the corresponding local neighborhood point set; based on the local neighborhood point set corresponding to each feature point, obtain the corresponding local neighborhood representation, and then construct probabilistic distance optimization 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 probabilistic distance optimization constraints include point-probabilistic line distance optimization constraints and point-probabilistic surface distance optimization constraints;

[0056] In this embodiment, based on the first pose of the current frame coordinate system of the lidar sensor in the world coordinate system... Project the feature points extracted from the current frame onto the target feature point map. The projection point is obtained, and the corresponding local neighborhood point set is found around the projection point, where the local neighborhood point set is the map. The nearest neighbors of the projection point N ty N points ty Option 5; based on edge feature points For the corresponding local neighborhood point set, a probability line is constructed to achieve local neighborhood representation. For planar feature points... For the corresponding local neighborhood point set, a probability surface is constructed to represent the local neighborhood. Based on this, probabilistic distance optimization constraints are built, including point-probability line distance optimization constraints and point-probability surface distance optimization constraints. This is used to construct the pose optimization function, realizing the current frame coordinate system of the LiDAR sensor in the world coordinate system. The second pose below The estimate, and They are The rotation matrix and translation vector; this second pose is used as the pose estimation result for the current frame; specifically as follows:

[0057] Step S201, will As The initial value, i.e. , ;

[0058] Step S202, using For the feature point set of the current frame and target feature point map Perform data association, where, and These are edge feature point maps and planar feature point maps, respectively; specifically, they will... Edge feature points in and planar feature points Projecting them onto the world coordinate system yields the projection points. and Then, the KdTree function of the PCL library was used to... and In respectively and Perform a KD-Tree search to obtain... exist Local neighborhood point set in as well as exist Local neighborhood point set in ;

[0059] Step S203, for and Local neighborhood representations are performed separately. Specifically, the computeMeanAndCovarianceMatrix function from the PCL library is used to obtain... corresponding mean Covariance Matrix The SelfAdjointEigenSolver class from the Eigen library is used to... Perform eigenvalue decomposition to obtain eigenvalues , , and the corresponding feature vectors , , ,in When satisfied At that time, among them The set linear structure determination threshold is used in this invention. 0.8 is preferred, indicating The corresponding local neighborhood has a linear structure, at which point the construction... Corresponding probability line representation , The parameters are denoted as ,in, , ;

[0060] Step S204: Use the computeMeanAndCovarianceMatrix function in the PCL library to obtain... corresponding mean Covariance Matrix The SelfAdjointEigenSolver class from the Eigen library is used to... Perform eigenvalue decomposition to obtain eigenvalues , , and the corresponding feature vectors , , ,in When satisfied At that time, among them The threshold for determining planar structures is set in this invention. 0.8 is preferred, indicating The corresponding local neighborhood has a planar structure, at which point the construction... Corresponding probability surface representation , The parameters are denoted as ,in, , ;

[0061] Step S205, using Correlation probability line representation Calculate the point-probability line distance optimization constraint and will constrain Add to the first constraint set In China; utilizing Probabilistic surface representation of association Calculate the point-probability surface distance optimization constraint and will constrain Add to the second constraint set In China; among them It is a 3x3 identity matrix. For example, the transpose operator, To Perform a transpose operation;

[0062] Step S206, based on the first constraint set Second constraint set , construct optimization frame pose objective function ,in, For the first set of constraints The first in Point-probability line distance optimization constraint It is the first set of constraints The number of point-probability line distance optimization constraints included. For the second set of constraints The first in Point-probability surface distance optimization constraint It is the second set of constraints The number of point-probability surface distance optimization constraints included; This refers to the absolute value operation. is Huber kernel function. By minimizing the objective function , we get :

[0063] (1)

[0064] where, is a mathematical operator that finds the value of x that makes the function take its minimum value. The equation is solved using the Solve function in the Ceres library to get , where and are the rotation matrix and translation vector of the pose estimation result of the current frame, respectively. Step S30, according to the pose estimation result of the current frame, the feature points in the current frame feature point set are projected into the corresponding feature point map, and the update of the target feature point map is completed.

[0065] In this embodiment, the edge feature points and the plane feature points

[0066] in are projected into the world coordinate system respectively to obtain the projection points and , and all the projection points and are added to the edge feature point map and the plane feature point map respectively, and the update of the target feature point map is completed. A robot laser radar odometry system according to the second embodiment of the present application, as shown in , comprises:

[0067] A first pose estimation module 100 is configured to enable the robot to perceive the environment through the laser radar sensor, acquire 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 further estimate the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system. Figure 2

[0068]

[0069] ​​​The second pose estimation module 200 is configured to project the feature points extracted from the current frame into the target feature point map to find corresponding local neighborhood point sets according to the first pose of the current frame coordinate system of the laser radar sensor in the world coordinate system; based on the local neighborhood point sets corresponding to each feature point, a corresponding local neighborhood representation is obtained, and then a probability distance optimization constraint is constructed to construct a pose optimization function, so as 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 the estimated second pose is taken as the pose estimation result of the current frame.

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

[0071] The skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working process and related description of the above-described system can refer to the corresponding process in the foregoing method embodiments, which will not be repeated here.

[0072] It should be noted that the robot laser radar odometry system provided in the above embodiments is only exemplified by the division of the above functional modules, and in actual application, the above functions can be completed by different functional modules according to needs, that is, the modules or steps in the embodiments of the present application are further decomposed or combined, for example, the modules in the above embodiments can be combined into one module, or can be further split 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 application are only for distinguishing the modules and steps, and should not be considered as an improper limitation of the present application.

[0073] The third embodiment of the present application is a storage device, which stores a plurality of programs, and the programs are suitable for being loaded by a processor and implementing the above-mentioned robot laser radar odometry method.

[0074] The fourth embodiment of the present application is a processing device, which includes a processor and a storage device; the processor is suitable for executing each program; the storage device is suitable for storing a plurality of programs; the programs are suitable for being loaded by the processor and executed to implement the above-mentioned robot laser radar odometry method.

[0075] The skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working process and related description of the above-described storage device and processing device can refer to the corresponding process in the foregoing method embodiments, which will not be repeated here.

[0076] Those skilled in the art should clearly understand that the modules and method steps of each example described in combination with the embodiments disclosed herein can be realized by electronic hardware, computer software or a combination of both. The programs corresponding to the software modules and method steps can be placed in a random access memory (RAM), a memory, a read-only memory (ROM), an electrically programmable ROM, an electrically erasable programmable ROM, a register, a hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art. In order to clearly illustrate the interchangeability of electronic hardware and software, the components and steps of each example have been described in the above description in general terms. Whether the functions are performed by 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 present application.

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

[0078] So far, the technical solutions of the present application have been described in combination with the preferred embodiments shown in the drawings, but those skilled in the art can easily understand that the protection scope of the present application is obviously not limited to these specific embodiments. Those skilled in the art can make equivalent changes or replacements to the related technical features without departing from the principles of the present application, and the technical solutions after such changes or replacements will all fall within the protection scope of the present application.

Claims

1. A method for robot lidar odometry, characterized in that, The method includes the following steps: Step S10: The robot perceives the environment through the lidar sensor and acquires the three-dimensional point cloud data of the surrounding environment in the current frame coordinate system of the lidar sensor; based on the three-dimensional point cloud data, feature points are extracted, and then the first pose of the current frame coordinate system of the lidar sensor in the world coordinate system is estimated. Step S20: Based on the first pose of the current frame coordinate system of the lidar sensor in the world coordinate system, project the feature points extracted in the current frame onto the target feature point map to find the corresponding local neighborhood point set; based on the local neighborhood point set corresponding to each feature point, obtain the corresponding local neighborhood representation, and then construct probabilistic distance optimization constraints to construct a pose optimization function, thereby estimating 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 probabilistic distance optimization constraints include point-probabilistic line distance optimization constraints and point-probabilistic surface distance optimization constraints. The point-probability line distance optimization constraint : ； Constraints Add to the first constraint set middle; The point-probability surface distance optimization constraint : ； Constraints Add to the second constraint set middle; in, It is a 3x3 identity matrix. The transpose operator, , Represent the sets of feature points respectively Mid-edge feature points Planar feature points Projecting these points onto the world coordinate system yields the projection points. The LOAM method is used to extract feature points from 3D point cloud data to form a feature point set. , They represent , The mean, , They represent The local neighborhood point set obtained by performing KD-Tree search on the edge feature point map, The local neighborhood point set obtained by performing a KD-Tree search on a planar feature point map; covariance matrix Eigenvalues ​​are obtained through eigenvalue decomposition. , , and the corresponding feature vectors , , , When satisfied When constructing the Corresponding probability line representation , The threshold for determining linear structures is set. The parameters are denoted as , , The covariance matrix Eigenvalues ​​are obtained through eigenvalue decomposition. , , and the corresponding feature vectors , , ,in, When satisfied When constructing the Corresponding probability surface representation , The threshold for determining planar structures is set. The parameters are denoted as , , ; 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 estimated second pose is then used as the pose estimation result for the current frame. The method is as follows: Based on the first set of constraints and the second set of constraints , construct optimization Frame 2 pose objective function : ； in, For the first set of constraints The first in Point-probability line distance optimization constraint It is the first set of constraints The number of point-probability line distance optimization constraints included. For the second set of constraints The first in Point-probability surface distance optimization constraint It is the second set of constraints The number of point-probability surface distance optimization constraints included; This refers to the absolute value operation. It is the Huber kernel function; Minimize the objective function and solve it using the Solve function from the Ceres library to obtain the pose estimation result for the current frame. , and These are the rotation matrix and translation vector of the pose estimation result for the current frame, respectively; Step S30: Based on the pose estimation result of the current frame, project the feature points in the feature point set of the current frame onto the corresponding feature point map to complete the update of the target feature point map.

2. The robot lidar odometry method according to claim 1, characterized in that, Based on the aforementioned 3D point cloud data, feature points are extracted, and then the first pose of the current frame coordinate system of the lidar sensor in the world coordinate system is estimated. The method is as follows: Based on the three-dimensional point cloud data The LOAM method is used to extract feature points and form a feature point set. The feature points include edge feature points and planar feature points. Using LOAM frame-to-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 coordinate system of the lidar sensor in the previous frame pose transformation ; Based on the above Previous frame lidar sensor coordinate system Relative to the world coordinate system The first pose is obtained to obtain the... In the world coordinate system The first position , and They are respectively the The rotation matrix and translation vector.

3. The robot lidar odometry method according to claim 2, characterized in that, The method for projecting the feature points extracted in the current frame onto the target feature point map to find the corresponding local neighborhood point set is as follows: The Edge feature points in and planar feature points Projected onto the world coordinate system respectively to obtain projection points and , ; and They are respectively the In the world coordinate system The second pose below The rotation matrix and translation vector; Using the KdTree function of the PCL library to respectively... exist Perform a KD-Tree search on the... exist Perform a KD-Tree search to obtain the... In the Local neighborhood point set in and the In the Local neighborhood point set in ;in, , Represents a map of target feature points. , These represent edge feature point maps and planar feature point maps, respectively.

4. The robot lidar odometry 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. The method is as follows: The computeMeanAndCovarianceMatrix function in the PCL library is used to obtain the... corresponding mean Covariance Matrix The SelfAdjointEigenSolver class from the Eigen library is used to perform the above operations. Perform eigenvalue decomposition to obtain eigenvalues , , and the corresponding feature vectors , , ,in, When satisfied When constructing the Corresponding probability line representation , The threshold for determining linear structures is set. The parameters are denoted as , , ; The computeMeanAndCovarianceMatrix function in the PCL library is used to obtain the... corresponding mean Covariance Matrix The SelfAdjointEigenSolver class from the Eigen library is used to perform the above operations. Perform eigenvalue decomposition to obtain eigenvalues , , and the corresponding feature vectors , , ,in, When satisfied When constructing the Corresponding probability surface representation , The threshold for determining planar structures is set. The parameters are denoted as , , .

5. The robot lidar odometry method according to claim 1, characterized in that, Based on the pose estimation result of the current frame, the feature points in the feature point set of the current frame are projected onto the corresponding feature point map to complete the update of the target feature point map. The method is as follows: Using the The Edge feature points in and planar feature points Projected onto the world coordinate system respectively, the projection points are obtained. and All projection points and Added to the edge feature point map respectively and the planar feature point map In the process, the map of the target feature points is updated.

6. A robot lidar odometry system, characterized in that, The system includes: The first pose estimation module is configured to allow the robot to perceive the environment through a lidar sensor and acquire 3D point cloud data of the surrounding environment in the current frame coordinate system of the lidar sensor; based on the 3D point cloud data, feature points are extracted, and then the first pose of the current frame coordinate system of the lidar sensor in the world coordinate system is estimated. 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 feature points extracted in the current frame onto the target feature point map to find the corresponding local neighborhood point set; based on the local neighborhood point set corresponding to each feature point, obtain the corresponding local neighborhood representation, and then construct probabilistic distance optimization 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 probabilistic distance optimization constraints include point-probabilistic line distance optimization constraints and point-probabilistic surface distance optimization constraints; The point-probability line distance optimization constraint : ； Constraints Add to the first constraint set middle; The point-probability surface distance optimization constraint : ； Constraints Add to the second constraint set middle; in, It is a 3x3 identity matrix. The transpose operator, , Represent the sets of feature points respectively Mid-edge feature points Planar feature points Projecting these points onto the world coordinate system yields the projection points. The LOAM method is used to extract feature points from 3D point cloud data to form a feature point set. , They represent , The mean, , They represent The local neighborhood point set obtained by performing KD-Tree search on the edge feature point map, The local neighborhood point set obtained by performing a KD-Tree search on a planar feature point map; covariance matrix Eigenvalues ​​are obtained through eigenvalue decomposition. , , and the corresponding feature vectors , , , When satisfied When constructing the Corresponding probability line representation , The threshold for determining linear structures is set. The parameters are denoted as , , The covariance matrix Eigenvalues ​​are obtained through eigenvalue decomposition. , , and the corresponding feature vectors , , ,in, When satisfied When constructing the Corresponding probability surface representation , The threshold for determining planar structures is set. The parameters are denoted as , , ; 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 estimated second pose is then used as the pose estimation result for the current frame. The method is as follows: Based on the first set of constraints and the second set of constraints , construct optimization Frame 2 pose objective function : ； in, For the first set of constraints The first in Point-probability line distance optimization constraint It is the first set of constraints The number of point-probability line distance optimization constraints included. For the second set of constraints The first in Point-probability surface distance optimization constraint It is the second set of constraints The number of point-probability surface distance optimization constraints included; This refers to the absolute value operation. It is the Huber kernel function; Minimize the objective function and solve it using the Solve function from the Ceres library to obtain the pose estimation result for the current frame. , and These are the rotation matrix and translation vector of the pose estimation result for the current frame, respectively; The map update module is configured to project the feature points in the current frame feature point set onto the corresponding feature point map based on the pose estimation result of the current frame, thereby completing the update of the target feature point map.

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

8. A processing apparatus, comprising a processor and a storage device; the processor being adapted to execute various programs; the storage device being adapted to store multiple programs; characterized in that, The program is applicable to being loaded and executed by a processor to implement a robotic lidar odometry method as described in any one of claims 1-5.

Images