Laser radar pose estimation method based on surface element features
Through the lidar pose estimation method based on surface element features, multi-resolved surface element features are extracted using pyramid grid and octree data structure, combining the nearest neighbor search of seven-dimensional vector space and the lidar inertial tight coupling odometer to solve the problem. The problem of decreasing position estimation accuracy in underground environments is achieved, and high-precision and robust position estimation is achieved.
Patent Information
- Application Number
- CN202410924878.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-11
- Publication Date
- 2024-11-15
AI Technical Summary
In underground environments, the position estimation accuracy of autonomous mobile robots decreases, especially due to sparse features and environmental complexity, it is difficult for existing SLAM systems to achieve robust and high-precision positioning, resulting in accumulated position estimation errors and degradation of system performance.
Using the lidar pose estimation method based on surface element features, the point cloud is voxelized and stored by extracting multi-resolved surface element features using pyramid grids and octree data structures, and combined with the nearest neighbor search of seven-dimensional vector space With the lidar inertial guide tightly coupled odometer, it achieves high-precision pose estimation.
High-precision and robust pose estimation are achieved in multiple challenging scenarios, improving the positioning accuracy of mobile robots in underground environments, avoiding the vicious cycle of pose estimation errors, and improving the stability and performance of the system.
Smart Images

Figure CN118967800A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of laser radar pose estimation, and in particular to a laser radar pose estimation method based on facet features. Background Art
[0002] Faced with dangerous and complex underground environments, people hope to use robotics technology to bring about a technological innovation in the fields of underground resource exploration, digital management of mine production, and underground military operations, so as to reduce casualties in underground missions. As the application scenarios of fully autonomous mobile robots become more and more extensive, robotics technology is also facing greater challenges. Real-time localization and mapping (SLAM) is a key technology for autonomous mobile robots to perform tasks in unknown environments, but it is still very difficult to deploy an accurate and robust SLAM system in underground environments. Module designs such as the fusion of asynchronous data from multiple sensors, the design of features that can be reliably detected and matched in a wide range of scenarios, and the construction of a reasonable optimization framework are the key to determining the performance of the SLAM system. At present, most SLAM methods focus more on processing general scenarios, and in underground environments with challenges such as no GPS signals, limited sensor field of view, severe jitter of the mobile platform, and severe feature degradation, the accuracy of pose estimation decreases, and even the system fails.
[0003] In the multi-sensor fusion SLAM system, the data of the two frames of sensor data are associated to construct the cost function of the robot's posture transformation between the two frames, so as to solve the relative posture transformation of the robot from the current frame to the previous frame (or the global coordinate system). Then, the current frame data is registered to the global map using the estimated posture. However, due to the existence of wrong data association, there is a certain error in the posture estimation. The error in the posture estimation will reduce the accuracy of the map, and then reduce the accuracy of the posture estimation at the next data association. This repeated iteration creates a vicious cycle. Designing a robust and versatile lidar inertial navigation SLAM system remains a challenge, especially in the field of reliably detecting and matching features in a wide range of scenarios. It is a difficult task to perform effective data association.
[0004] The main challenges of pose estimation of mobile robots in underground environments are sparse features and complex environments. In recent years, there are many pose estimation methods based on 3D point clouds to achieve the positioning of mobile robots. For example, the Chinese patent "CN114004869A A positioning method based on 3D point cloud registration" proposes a positioning method based on 3D point clouds. When matching between frames, the nearest neighbor search is used to search for point cloud data adjacent to the current frame point cloud. The ICP matching algorithm is used to calculate the relative transformation relationship between the current robot pose and the robot pose in the point cloud map, and it is added as a constraint to the pose map to optimize the laser odometer and obtain accurate robot pose information. However, the ICP-based point cloud registration method has a large amount of calculation and cannot achieve good positioning accuracy in underground environments.
[0005] There are also many learning-based methods to achieve point cloud registration. For example, the Chinese patent "CN117689702A A point cloud registration method and device based on geometric attention mechanism" proposes a point cloud registration method and device based on geometric attention mechanism, including: real laser radar acquisition to obtain target point cloud data, virtual laser radar acquisition to obtain source point cloud data; point cloud data is downsampled by voxels and input into the local feature extraction network to obtain local feature descriptors; geometric structure information in point cloud data is extracted, and in the Transformer network based on geometric attention mechanism, the geometric structure information is embedded in the local feature descriptor to associate global context information and enhance local geometric structure information; nearest neighbor search, random sampling consistency algorithm and optimal Euclidean transformation are used to complete point cloud registration. However, learning-based methods require extra time for training, and the quality of the model depends on the training data, and the generalization of the model is also a problem. Summary of the invention
[0006] (I) Purpose of the invention
[0007] In order to solve the technical problems existing in the background technology, the present invention proposes a laser radar pose estimation method based on facet features.
[0008] (II) Technical solution
[0009] To solve the above problems, the present invention proposes a laser radar pose estimation method based on facet features, comprising:
[0010] Step S100, extracting facet features:
[0011] Capture the real environment based on the laser radar to obtain the target point cloud;
[0012] Voxelize the target point cloud using a pyramid grid;
[0013] Use the octree data structure to store voxelized point clouds of different resolutions;
[0014] Calculate the shape parameters of the point cloud in the voxel so that multi-resolution surface element features can be extracted in the point cloud of the current frame;
[0015] Step S200, data association of multi-resolution facets:
[0016] Use the nearest neighbor in the seven-dimensional vector space to search for the nearest face element feature in the current frame and the local map respectively, and if they are closest to each other, a pair of data associations is constructed;
[0017] Step S300, lidar inertial navigation tightly coupled odometer:
[0018] The extracted face features and IMU measurements are input into the state estimation module for state estimation. The point cloud is registered to the global frame through the estimated posture, and the newly registered point cloud is merged with the point cloud mapping in the map. The updated map is used for the next step of data association to register the point cloud of a new frame.
[0019] Preferably, in the voxelization process, the number of point clouds in each grid is recorded, and in a scenario where a voxel contains at least seven points, shape parameters of the point clouds in the voxel are calculated.
[0020] Preferably, each voxel is marked with the average measurement time of its internal points. If the average measurement time of the points in a voxel is later than the time of the earliest measured point in the voxel, it indicates that these points are not included in the voxel.
[0021] Preferably, when extracting face element features from each voxel, the face element feature description is to fit the distribution of points by calculating local feature parameters based on the mean and covariance of the point cloud in each voxel, and for the point cloud scanned by the three-dimensional lidar, clustering and ellipsoid fitting are performed according to the position and timestamp of the points to generate multi-scale face element features.
[0022] Preferably, the step of clustering and ellipsoid fitting the point cloud scanned by the three-dimensional laser radar according to the position and time stamp of the point to generate multi-scale surface element features includes:
[0023] Divide the space into a set of multi-scale cube cells and aggregate the lidar points with similar timestamps within each cube cell;
[0024] Fit an ellipsoid to each sufficiently large point group based on a predetermined threshold;
[0025] The center and shape of each ellipsoid are determined by the sample mean and covariance of the three-dimensional points in the point group, respectively. The length of the ellipsoid axis determines whether the local point cloud structure is planar / cylindrical.
[0026] Preferably, the facet features are stored in S ur In the data structure of surfelmap, the node i in the data structure of surfelmap based on octree corresponds to the face element attribute in a voxel, and the node depth represents the scale of the voxel. For leaf nodes, the depth attribute of the face element is 0, and the depth attribute of the parent node is 1. The face element on node i in surfelmap is defined by the point set contained in the voxel, which is recorded as The basic attribute formula of the surface element is as follows: i =|V i |,
[0027] In the above formula, N i , S i , C i The storage value of each surface element in the Surfelmap data structure. V can be quickly calculated based on the properties i The mean μ i and covariance Γ i , other properties can be obtained by the following formula:
[0028]
[0029] In the formula, μ i is the mean, Γ i is the covariance with the eigenvalues λ0, λ1, λ2, n i =λ0 is the normalized eigenvector associated with λ0; ρ i is the plane similarity of the surface;
[0030] The mean and covariance of the point cloud in the corresponding pyramid grid are recursively calculated from the high-resolution to the low-resolution grid to improve the computational efficiency.
[0031] Preferably, in step S200, by kd-t ree Perform nearest neighbor search to complete the association matching between face elements;
[0032] The residual of the distance between the centroids of two surfels is minimized along a common plane normal to obtain the sensor pose transformation between the two frames.
[0033] Preferably, the calculated facet shape feature information is incorporated into the ICP corresponding step to achieve data association, and each contains a sufficient number of voxels to form a seven-dimensional space vector:
[0034]
[0035] Where μ is the centroid of the point, v1 is the normal vector of the plane, and the corresponding grid size is obtained by obtaining the nearest neighbor voxel of each voxel in the seven-dimensional space, and the correspondence between the voxels in each plane is estimated in each iteration;
[0036] It also includes the panel construction residual, which is expressed as follows:
[0037] e y =ξ y n ij (μ i (δT(τ i ))-μ j )
[0038] Among them, the common surface normal n ij is the normal vector corresponding to the minimum eigenvalue of the covariance recalculated after the midpoint fusion of the matching face element, μ i is the centroid of panel i, μ j is the centroid of panel j, and the coefficient Depends on the sensor measurement noise σ r and eigenvalues λ1,δT(τ i ) is the estimated position of the lidar from the current frame to the previous frame.
[0039] Preferably, the matching between the face element features is constrained by the offset of the center of mass in the direction of its surface normal and the angle between its surface normals, and the face element matching result is limited by the distance in the seven-dimensional vector space and the size of the angle between the mutually matching face element normal vectors to eliminate some external points that are most closely associated with each other.
[0040] Preferably, in step S300, the laser radar inertial navigation tightly coupled odometer includes:
[0041] Construct an observation model that describes the relationship between sensor observations, the robot's true state, and measurement errors;
[0042] Calculate the covariance matrix of the error so that the weights of each least squares term are correctly applied in the optimization problem;
[0043] Calculate the residual between each observation and its predicted value;
[0044] Compute the Jacobian to ensure fast optimization.
[0045] The above technical solution of the present invention has the following beneficial technical effects:
[0046] The technical solution of the present invention can achieve global positioning and high-precision robust positioning. High-precision pose estimation is achieved through facet-based point cloud feature extraction, multi-resolution facet-based data association, and lidar inertial navigation tightly coupled odometer. Through evaluation in multiple challenging scenarios such as unstructured underground mines, structured artificial tunnels, structured buildings, and parks with unstructured vegetation, and comparison with other methods, the experimental results show that the method proposed in the present invention can achieve higher pose estimation accuracy. By using the pose estimation method proposed in the present invention, high-precision robust positioning of mobile robots can be achieved in underground scenarios. BRIEF DESCRIPTION OF THE DRAWINGS
[0047] Figure 1 is a flow chart of the method of the present invention;
[0048] Figure 2 It is the overall algorithm system structure diagram of the present invention;
[0049] Figure 3 It is a schematic diagram of facet feature extraction of the present invention;
[0050] Figure 4 It is a schematic diagram of multi-scale facet recursive calculation of the present invention;
[0051] Figure 5 This is a schematic diagram of the visualization of multi-scale bins of laser points of the present invention;
[0052] Figure 6 This is a schematic diagram of the coordinate relationship of this multi-sensor fusion system. DETAILED DESCRIPTION
[0053] In order to make the purpose, technical scheme and advantages of the present invention clearer, the present invention is further described in detail below in conjunction with specific embodiments and with reference to the accompanying drawings. It should be understood that these descriptions are only exemplary and are not intended to limit the scope of the present invention. In addition, in the following description, the description of well-known structures and technologies is omitted to avoid unnecessary confusion of the concept of the present invention.
[0054] In one embodiment, the present invention can realize high-precision pose estimation of a mobile robot in an underground environment; by extracting multi-scale surface element features from a three-dimensional point cloud, and then using the seven-dimensional vectors of the point cloud centroid, normal vector, and scale factor in the pyramid grid to represent the surface element features extracted from the grid pyramid, and then using the kd-t ree The nearest neighbor search is performed to complete the association matching between the face elements, and then the residual of the distance between the centroids of the two face elements is minimized along a common plane normal to solve the pose transformation of the sensor between the two frames. Compared with the existing method, the performance is better and it can better adapt to the underground environment. The present invention is further explained below.
[0055] refer to Figure 1-6 The laser radar pose estimation method based on facet features proposed in the present invention includes step S100, extracting facet features, capturing the real environment according to the laser radar to obtain a target point cloud, voxelizing the target point cloud using a pyramid grid, storing the voxelized point clouds of different resolutions using an octree data structure, and calculating the point cloud shape parameters in the voxels so as to extract multi-resolution facet features in the current frame point cloud.
[0056] Specifically, the number of point clouds in each grid is recorded during the voxelization process of the pyramid grid. Only three non-collinear point clouds can be distributed exactly on the same plane. A voxel contains at least 7 points before the shape parameters of the point cloud in the voxel are further calculated. Each voxel also marks the time of the voxel with the average time of its internal points. In order to avoid large point cloud distortion problems, if the measurement time of a point in a voxel is significantly later than the earliest measured point in the voxel, then these points are not included in the voxel. When extracting face element features from each voxel, the face element feature description is to fit the distribution of points by calculating local feature parameters by the mean and covariance of the point cloud in each voxel. For the point cloud scanned by the three-dimensional lidar, clustering and ellipsoid fitting are performed according to the position and timestamp of the points to generate multi-scale face element features.
[0057] In an optional embodiment, the space is divided into a group of multi-scale cubic units, and the lidar points with similar timestamps in each cubic unit are clustered together. An ellipsoid is fitted for each sufficiently large point group according to a predetermined threshold. The center and shape of each ellipsoid are determined by the sample mean and covariance of the three-dimensional points in the point group, respectively. The length of the ellipsoid axis determines whether the local point cloud structure is planar / cylindrical.
[0058] For example, when the smallest eigenvalue is much smaller than the other two eigenvalues, the point cloud resembles a plane; when the smallest and second-smallest eigenvalues are both much smaller than the largest eigenvalue, the point cloud resembles a cylinder; and when all three eigenvalues are of similar magnitude, the point cloud is more spherical in shape.
[0059] Surfel feature extraction based on mesh pyramid voxelization is implemented based on octree and stored in the Surfelmap data structure. Node i in the Surfelmap data structure based on octree corresponds to the attribute of the surfel in a voxel. The node depth indicates the scale of the voxel. For leaf nodes, the depth attribute of the surfel is 0, and the depth attribute of the parent node is 1. The surfel on node i in Surfelmap is defined by the point set contained in the voxel, which is recorded as The basic attribute formulas that constitute the surface element are as follows:
[0060] N i =|V i |,
[0061] In the above formula, N i , S i , C i The storage value of each surface element in the Surfelmap data structure. V can be quickly calculated based on the properties i The mean μ i and covariance Γ i , other properties can be obtained by the following formula:
[0062]
[0063] In the formula, μ i is the mean, Γ i is the covariance with the eigenvalues λ0, λ1, λ2, n i =λ0 is the normalized eigenvector associated with λ0; ρ i For the planar similarity of surfaces, in addition to the above properties, the depth and scale of voxel nodes are often queried.
[0064] The mean and covariance of the point cloud in the corresponding pyramid grid are recursively calculated from the high-resolution to the low-resolution grid to improve the computational efficiency. Figure 4 , assuming that the parent node i has two child nodes m and n, the facet attribute of node i can be recursively calculated by the Welford formula, as follows:
[0065] α:=1 / [N m N n (N m +N n )],β:=N n S m -N m S n
[0066] C i ←C m +C n +αββ
[0067] N i ←N m +N n ,S i ←S m +S n
[0068] For valid surfels with more than two high-resolution grids in the next-level resolution grid, the above method can be iterated. According to the above formula, if the child node n is removed from the mapping, the process can update the surfel attribute of node i as follows:
[0069] N i ←Ni -N n ,S i ←S i -S n
[0070] α:=1 / [N i N n (N i +N n )],β:=N n S i -N i S n
[0071] C i ←C i -C n -αββ'
[0072] refer to Figure 5 , showing the visualization of facet features at three different resolutions, specifically available in light green facets, yellow facets, and blue (not shown in the figures).
[0073] Step S200, data association of multi-resolution face elements, using the nearest neighbor search of the seven-dimensional vector space in the current frame and the local map respectively, constructing a pair of data associations if they are closest to each other, completing the association matching between face elements through nearest neighbor search through kd-tree, minimizing the residual of the distance between the two face element centroids along a common plane normal, so as to solve and obtain the sensor posture transformation between the two frames.
[0074] The calculated surface element shape feature information is incorporated into the ICP corresponding step to achieve data association, and each contains a sufficient number of voxels to form a seven-dimensional space vector:
[0075]
[0076] Where μ is the centroid of the point, v1 is the normal vector of the plane, and the corresponding grid size is obtained by obtaining the nearest neighbor voxel of each voxel in the seven-dimensional space, and the correspondence between the voxels in each plane is estimated in each iteration;
[0077] It also includes the panel construction residual, which is expressed as follows:
[0078] e y =ξ y n ij (μ i (δT(τ i ))-μ j )
[0079] Among them, the common surface normal n ijis the normal vector corresponding to the minimum eigenvalue of the covariance recalculated after the midpoint fusion of the matching face element, μ i is the centroid of panel i, μ j is the centroid of panel j, and the coefficient Depends on the sensor measurement noise σ r and eigenvalues λ1,δT(τ i ) is the estimated position of the lidar from the current frame to the previous frame.
[0080] Furthermore, the matching between facet features is constrained by the offset of the center of mass in the direction of its surface normal and the angle between its surface normals. Since there is already a clear definition describing the facet, the facet matching results are restricted by the distance in the seven-dimensional vector space and the size of the angle between the normal vectors of the matching facets to eliminate some external points that are closest to each other.
[0081] In step S300, the lidar inertial navigation is tightly coupled to the odometer, and the extracted face element features and IMU measurement values are input into the state estimation module for state estimation. The point cloud is registered to the global frame through the estimated posture, and the newly registered point cloud is merged with the point cloud mapping in the map. The updated map is used for the next step of data association, thereby registering a new frame of point cloud.
[0082] Specifically, the lidar inertial navigation tightly coupled odometer includes building an observation model that describes the relationship between sensor observations, the robot's true state and measurement errors, then calculating the error covariance matrix so that the weight of each least squares term can be correctly applied in the optimization problem, then calculating the residual between each observation value and its predicted value, and finally calculating the Jacobian determinant to ensure the rapid implementation of the optimization process.
[0083] refer to Figure 6 , the specific algorithm is as follows:
[0084] Use X k Indicates t k All states estimated at the moment are as follows:
[0085]
[0086] The robots are k Quaternion, position and velocity of the world coordinate system at the moment; It is the accelerometer bias and gyroscope bias measured by the IMU.
[0087] A cost function will be constructed based on the observations of LiDAR and IMU measurements at each time. Using all the observation data obtained by LiDAR and IMU, it is possible to k+m Construct the cost function for:
[0088]
[0089] LiDAR Factor:
[0090] (1) Coordinate system symbol definition: Define {B i} is the robot at time t k In the body coordinate system, {W} is the world coordinate system. The purpose of the multi-sensor fusion SLAM system is to obtain Estimates:
[0091]
[0092] in, and is at k The present invention converts the IMU coordinate system {I k} and the body coordinate system coincide, that is, at any time k, {B k}={I k {C k} represents the camera coordinate system at time k, {L k} represents the lidar coordinate system at time k. and Usually called external parameters, the coordinate relationship between camera, lidar and inertial navigation can also be easily realized through some open source camera lidar inertial navigation calibration tools.
[0093] (2) Observation model:
[0094]
[0095] Among them, E(φ k ), d k is the error in the estimation process, Δq k and Δp k is the time t k to k+1 The relative rotation and displacement of . The normalization is defined as:
[0096]
[0097] The observed values of pose and displacement can be obtained as follows:
[0098]
[0099] (3) Observation error covariance:
[0100] The noise φ in the above formula k and d kare modeled as independent zero-mean Gaussian distributions with the following covariance:
[0101]
[0102] in, …are some parameters that can be determined empirically.
[0103] (4) Residual derivation:
[0104] Based on the observations in the observation model, the lidar-based residuals can be calculated as follows:
[0105]
[0106] Among them, vec(.) returns the vector part of the quaternion, is at time t k Estimated pose change.
[0107] (5) Jacobi derivation:
[0108] First deduce From the above formula, we can deduce Substitute it into Since on an identity element we have vec(q) = -vec(q -1 ), so:
[0109]
[0110] It can be further deduced that:
[0111] Similarly, we can deduce the following at t k+1 Jacobi
[0112]
[0113] To obtain In actual calculations, the amount of trigonometric function calculations may be large, and the variable in the denominator may cause errors of division by zero. Usually, the following first-order linear approximation is used:
[0114]
[0115] in, R n →M is Local parameterization, local parameterization results in rotation matrices and quaternions For δθ k The derivative is:
[0116]
[0117] The Jacobian for the translation part is derived as follows:
[0118]
[0119] IMU pre-integration factor:
[0120] Only the residual derivation is listed here without further elaboration:
[0121] IMU residual Defined as:
[0122]
[0123] in,
[0124] The principles and advantages of the present invention are as follows: the present invention inputs the point cloud into the feature extraction module to obtain multi-scale face element features; the data association module uses the seven-dimensional vectors of the point cloud centroid, normal vector, and scale factor in the grid to represent the face element features extracted from the grid pyramid; the nearest neighbor search is performed through the kd-tree to complete the matching between face elements; the state estimation module uses the extracted face element features and IMU measurement values to perform state estimation at a frequency of 10 Hz; finally, the point cloud is registered to the global framework through the estimated posture, and the newly registered point cloud is merged with the point cloud mapping in the map; the updated map is used for the next step of data association, thereby registering a new frame of point cloud; the present invention connects these three modules so that the mobile robot can also perform robust posture estimation in underground environments.
[0125] The above shows and describes the basic principles, main features and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited by the above embodiments, and the above embodiments and descriptions are only preferred examples of the present invention, and are not intended to limit the present invention. Without departing from the spirit and scope of the present invention, the present invention may have various changes and improvements, and these changes and improvements all fall within the scope of the present invention to be protected.
Claims
1. A laser radar pose estimation method based on surface element features, characterized in that: include: Step S100, extracting facet features: Capture the real environment based on the laser radar to obtain the target point cloud; Voxelize the target point cloud using a pyramid grid; Use the octree data structure to store voxelized point clouds of different resolutions; Calculate the shape parameters of the point cloud in the voxel so that multi-resolution surface element features can be extracted in the point cloud of the current frame; Step S200, data association of multi-resolution facets: Use the nearest neighbor in the seven-dimensional vector space to search for the nearest face element feature in the current frame and the local map respectively, and if they are closest to each other, a pair of data associations is constructed; Step S300, lidar inertial navigation tightly coupled odometer: The extracted face features and IMU measurements are input into the state estimation module for state estimation. The point cloud is registered to the global frame through the estimated posture, and the newly registered point cloud is merged with the point cloud mapping in the map. The updated map is used for the next step of data association to register the point cloud of a new frame.
2. The laser radar pose estimation method based on surface element features according to claim 1, characterized in that: In the voxelization process, the number of point clouds in each grid is recorded, and when a voxel contains at least seven points, the shape parameters of the point clouds in the voxel are calculated.
3. The laser radar pose estimation method based on surface element features according to claim 2, characterized in that: Each voxel is marked with the average measurement time of its internal points. If the average measurement time of the points in a voxel is later than the time of the earliest measured point in the voxel, it indicates that these points are not included in the voxel.
4. The laser radar pose estimation method based on surface element features according to claim 2, characterized in that: When extracting face element features from each voxel, the face element feature description is to fit the distribution of points by calculating local feature parameters based on the mean and covariance of the point cloud in each voxel. For the point cloud scanned by the three-dimensional lidar, clustering and ellipsoid fitting are performed according to the position and timestamp of the points to generate multi-scale face element features.
5. The laser radar pose estimation method based on surface element features according to claim 4 is characterized in that: The point cloud scanned by the three-dimensional laser radar is clustered and ellipsoidally fitted according to the position and time stamp of the point to generate multi-scale surface element features, including: Divide the space into a set of multi-scale cube cells and aggregate the lidar points with similar timestamps within each cube cell; Fit an ellipsoid to each sufficiently large point group based on a predetermined threshold; The center and shape of each ellipsoid are determined by the sample mean and covariance of the three-dimensional points in the point group, respectively. The local point cloud structure is determined to be planar / cylindrical according to the length of the ellipsoid axis.
6. The laser radar pose estimation method based on surface element features according to claim 5, characterized in that: The facet features are stored in the Surfelmap data structure. Node i in the octree-based Surfelmap data structure corresponds to a facet attribute in a voxel. The node depth represents the scale of the voxel. For a leaf node, the facet depth attribute is 0, and the depth attribute of the parent node is 1. The facet on node i in Surfelmap is defined by the point set contained in the voxel, which is recorded as The basic attribute formulas that constitute the surface element are as follows: In the above formula, N i , S i , C i The storage value of each surface element in the Surfelmap data structure. V can be quickly calculated based on the properties i The mean μ i and covariance Γ i , other properties can be obtained by the following formula: In the formula, μ i is the mean, Γ i is the covariance with the eigenvalues λ0, λ1, λ2, n i =λ0 is the normalized eigenvector associated with λ0; ρ i is the plane similarity of the surface; The mean and covariance of the point cloud in the corresponding pyramid grid are recursively calculated from the high-resolution to the low-resolution grid to improve the computational efficiency.
7. The laser radar pose estimation method based on surface element features according to claim 1, characterized in that: In the step S200, the association matching between the facets is completed by performing the nearest neighbor search through the kd-tree; The residual of the distance between the centroids of two surfels is minimized along a common plane normal to obtain the sensor pose transformation between the two frames.
8. The laser radar pose estimation method based on surface element features according to claim 7 is characterized in that: The calculated surface element shape feature information is incorporated into the ICP corresponding step to achieve data association, and each contains a sufficient number of voxels to form a seven-dimensional space vector: Where μ is the centroid of the point, v1 is the normal vector of the plane, and the corresponding grid size is obtained by obtaining the nearest neighbor voxel of each voxel in the seven-dimensional space, and the correspondence between the voxels in each plane is estimated in each iteration; It also includes the panel construction residual, which is expressed as follows: e y =ξ y n ij (m i (δT(τ i ))-m j ) Among them, the common surface normal nij is the normal vector corresponding to the minimum eigenvalue of the covariance recalculated after the midpoint fusion of the matching facets, μ i is the centroid of panel i, μ j is the centroid of panel j, and the coefficient Depends on the sensor measurement noise σ r and eigenvalues λ1,δT(τ i ) is the estimated position of the lidar from the current frame to the previous frame.
9. The laser radar pose estimation method based on surface element features according to claim 8, characterized in that: The matching between the face element features is constrained by the offset of the center of mass in the direction of its surface normal and the angle between its surface normals. The face element matching result is limited by the distance in the seven-dimensional vector space and the size of the angle between the normal vectors of the mutually matching face elements to eliminate some external points that are closest to each other.
10. The laser radar pose estimation method based on surface element features according to claim 1, characterized in that: In step S300, the laser radar inertial navigation tightly coupled odometer includes: Construct an observation model that describes the relationship between sensor observations, the robot's true state, and measurement errors; Calculate the covariance matrix of the error so that the weights of each least squares term are correctly applied in the optimization problem; Calculate the residual between each observation and its predicted value; Compute the Jacobian to ensure fast optimization.
