Laser inertial odometer method based on plane merging strategy and computer device
Through a method based on plane merge strategy, the point cloud registration process of laser inertial odometers is optimized using voxel grid and hash function, and the problems of high computational complexity and noise sensitivity in the existing technology are solved, achieving high efficiency and stability laser inertia odometer.
Patent Information
- Application Number
- CN202411143492.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-20
- Publication Date
- 2024-11-26
- Estimated Expiration
- 2044-08-20
AI Technical Summary
The existing laser inertia odometer technology has high computational complexity, slow convergence speed, and is sensitive to noise and outliers in high-density point clouds. The memory consumption of NDT in large point cloud data sets is large, affecting system stability and accuracy. sex.
Using a plane merge strategy method, the point cloud registration process is optimized and the computational complexity is reduced by voxel grid downsampling, hash function mapping, residual calculation, iteratively extended Kalman filtering, weighted least squares method and DBSCAN clustering. and improve the accuracy of system state estimation.
It significantly improves the computing efficiency and robustness of the laser inertia odometer, reduces memory usage, and enhances the stability and accuracy of the system in a noisy environment.
Smart Images

Figure CN119022955A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of laser inertial odometers, and in particular to a laser inertial odometer method and a computer device based on a plane merging strategy. Background Art
[0002] LiDAR (Light Detection and Ranging) is a technology that determines the distance of an object by emitting lasers and measuring the reflection time. It is able to generate high-resolution 3D point cloud data for accurately measuring and mapping the environment. In recent years, with the development of 3D LiDAR technology, especially the emergence of low-cost and high-density solid-state LiDAR, laser inertial odometers have been widely used in fields such as autonomous driving, drones, and 3D reconstruction. For many real robots or vehicles, LIO (Lidar Inertial Odometry) is not the only algorithm running in the system, and all active modules must share computing resources. Therefore, while ensuring the accuracy of the system, if you can have a faster and lighter LIO, the entire system will be more robust.
[0003] Fast point cloud registration and 3D reconstruction in LIO are key modules in autonomous driving. In recent years, many studies have focused on optimizing data structures to find efficient and fast laser registration methods. Laser registration methods can be roughly divided into two categories: ICP (Iterative Closest Point) and NDT (Normal Distributions Transform). ICP and its variants build maps online by registering new scan increments to the point cloud based on KD-Tree, and then rely on k-nearest neighbor (KNN) search in the point cloud for data association. The error is generated by the associated data and reduced by state estimation, and the above steps are repeated until the error drops below a threshold. Normal distribution transform and its variants are classic voxel mapping methods that divide the positioning space into voxels. By fitting the historical point cloud, each voxel contains a 3D Gaussian distribution. By calculating the probability of the point in the voxel, the state estimation is formulated as the maximum likelihood estimation.
[0004] Although the existing laser inertial odometry technology has performed well in many applications, it still has some shortcomings and defects. Although the ICP method is widely used in point cloud registration, it has some significant shortcomings. First, the computational complexity of ICP is high and it requires repeated iterations to minimize the error. Each iteration involves nearest neighbor search and error calculation, which leads to high computational cost, especially in high-density point clouds. Second, ICP converges slowly, especially when the initial registration error is large. It relies on the initial guess. If the initial position estimate is inaccurate, it may take more iterations to converge, and may even fall into a local optimal solution. ICP is very sensitive to noise and outliers in the point cloud, which can seriously affect the accuracy and stability of the registration. In addition, the computational complexity of the normal distribution transformation is also high, especially in high-density point clouds, where the voxel partitioning and Gaussian distribution fitting are computationally intensive, which affects the registration speed. In addition, NDT consumes a lot of memory when processing large point cloud datasets, which is a problem for embedded systems with limited resources. Finally, although NDT performs registration through a probabilistic method, the accuracy of the probability estimation will decrease in a noisy environment, thus affecting the overall registration effect. Summary of the invention
[0005] The purpose of the present invention is to overcome the shortcomings of the prior art and provide a laser inertial odometer method and a computer device based on a plane merging strategy, which greatly improves the efficiency of the registration calculation while taking into account the uncertainty of the laser point and posture.
[0006] The present invention adopts the following technical solutions to achieve the above-mentioned purpose. In a first aspect, the present invention provides a laser inertial odometer method based on a plane merging strategy, comprising:
[0007] S1, preprocessing of point cloud data;
[0008] The voxel grid method is used to downsample the point cloud data. By defining the size of each voxel, all points in the voxel are replaced by the centroid or average value of the voxel to reduce the amount of point cloud data.
[0009] After obtaining the prior pose obtained by the IMU through forward propagation, the pose information is used to perform backpropagation processing on the downsampled point cloud data to eliminate the point cloud distortion caused by the movement of the device. The pose refers to the position and attitude.
[0010] S2, mapping the preprocessed point cloud data to the voxel map through a hash function to complete the plane parameter estimation;
[0011] S3, performing residual calculation based on the direct relationship between the points mapped into the voxel map and the known plane parameters;
[0012] S4, based on the residual calculation result, the system state is updated by iteratively extending the error state Kalman filter through the back-end optimization algorithm;
[0013] S5, after the state is updated, by maintaining a slope detection window, when it is detected that the slope change exceeds a preset threshold, the state is adjusted and the correction reference value is updated;
[0014] S6, after the slope detection is adjusted, determine whether the system state has converged, if not, return to step S3, if yes, output the state estimation of the laser inertial odometer, and enter step S7;
[0015] S7, using the converged system state to transform the point cloud data in the laser coordinate system into the world coordinate system, and mapping the transformed point cloud data into the voxel map again through the spatial hash function;
[0016] S8, updating the plane parameters by weighted least squares method, setting different confidence weights according to the uncertainty of each point cloud data;
[0017] S9, traverse the converged plane set, and cluster the plane nodes and their neighboring nodes using the DBSCAN clustering algorithm;
[0018] S10, perform plane parameter fusion on the clustered nodes, assign the common parent plane parameters of the clustering nodes to the clustering nodes, skip the clustered nodes, and for the clustered nodes in the same cluster, perform parameter fusion with the common parent plane of the clustering nodes in this round, and recommend the clustering node with the most child nodes as the new parent node.
[0019] Furthermore, the state estimation is based on an iterative extended Kalman filter, which first obtains the prior pose through a discrete kinematic model. and covariance The discrete kinematic model is as follows:
[0020] Where i-1 is the IMU measurement index, the f function, the state value x, the kinematic input value u and the noise w are defined as:
[0021]
[0022]
[0023]
[0024]
[0025] Where M stands for manifold, which is used to represent the state space. represents 15-dimensional real numbers, SO(3) represents the Lie group, that is, the rotation matrix, Represents the rotation matrix transpose from the inertial coordinate system to the world coordinate system, describing the system attitude, represents the displacement from the inertial coordinate system to the world coordinate system, represents the speed from the inertial coordinate system to the world coordinate system, Indicates the zero bias of the gyroscope and accelerometer. G g T represents the gravitational acceleration in the world coordinate system, They represent angular velocity and acceleration respectively. Represent the Gaussian noise of the gyroscope and accelerometer, respectively. denote the random walk noise of the gyroscope and accelerometer respectively;
[0026] The prior pose Map the point cloud to the voxel map, calculate the residual, and update the state value to MAP estimate. The specific process includes:
[0027] The effective point plane matching observation equation for the i-th epoch is:
[0028]
[0029]
[0030] Among them, x i is the system state, n i represents the theoretical observation noise of the observation model, v i is the observation noise of the observed quantity, is the distance from the k-th iteration midpoint of the i-th laser measurement to the registration plane, and Ω is the parent plane normal vector of the registration plane [a, b, 1] T , the parent plane normal vector of the registration plane is first directly obtained through the spatial hash function to obtain the registration plane index, and its time complexity is O(1), and then the parent node plane parameters are accessed through the registration plane. W is the world coordinate system, L is the lidar system, and p i is a map point, t represents the system displacement;
[0031] Observation noise v i Obey i ~(0,R i ), which is mainly caused by the uncertainty of laser point and plane parameters, as shown in the following formula:
[0032]
[0033]
[0034]
[0035]
[0036] In the formula, is the plane parameter covariance, is the map point covariance, represents the uncertainty of plane and map points, are the Jacobian matrices of the observation equation with respect to map points and plane parameters, respectively. Represents the first-order derivative of the observation equation with respect to the map point and plane parameter, and finally through Calculate the uncertainty of the observation error;
[0037] Finally, the state prior is combined with all valid measurements to obtain the MAP estimate:
[0038]
[0039] Furthermore, the process of plane parameter estimation specifically includes:
[0040] Whenever a frame of laser point cloud is received, the laser point cloud is first downsampled by voxel filtering. W p i (i=1,2,3…n), and then PCA is used to determine the points in the voxel W p i Can a plane be formed? If so, determine the direction of the plane normal vector n p =[a,b,c], i.e. the axis with the minimum projection variance, and finally normalize the plane equation along the coordinate axis direction, and use WLS to estimate the 3DOF plane parameters n = [a,b,d] and ∑ n ;
[0041] If the plane equation is normalized along the z-axis, the point cloud within the voxel W p i (i=1,2,3…n) satisfies: ax+by+z+d=0;
[0042] Then the least squares optimization function is constructed as:
[0043]
[0044] At the same time, according to W p i Covariance matrix Set the weight matrix W to:
[0045] W=diag(k1,k2,…,k n )
[0046] k i =1-exp(-1 / w i )
[0047] Among them, w i is the covariance matrix The three-axis RMS of the weight mapping function y = 1.0-exp(-1 / x) will w i Mapping to k i , the function is in w i When k is less than the first setting value i Approaching 1, at w i When k is greater than the second setting value i Approaching 0;
[0048] K i =1-exp(-1 / w i ) is transformed into: An = e;
[0049]
[0050]
[0051] Finally, we get the closed-form solution of n and its covariance matrix ∑ n :
[0052]
[0053]
[0054] Among them A * is the adjoint matrix of A, |A| is the determinant of A, A * , |A| and e are all in incremental form.
[0055] In a second aspect, the present invention provides a computer device, including a memory, wherein the memory stores program instructions, and when the program instructions are run, the laser inertial odometer method based on the plane merging strategy described above is executed.
[0056] The beneficial effects of the present invention are:
[0057] The present invention directly maps the laser point cloud to the voxel through HashMap to complete the plane parameter estimation. When the laser point is mapped to the voxel through HashMap, the point-surface residual will be directly generated. The time complexity of mapping through the spatial hash function is O(1). This method does not require KNN (K-Nearest Neighbors) search, and greatly improves the registration calculation efficiency while considering the uncertainty of laser points and postures.
[0058] The present invention takes into account the map uncertainty caused by the laser radar measurement noise, explicitly parameterizes and estimates the planes in the voxels. On this basis, the weighted least squares method is used for plane fitting, which improves the robustness of the system in degraded scenarios. Secondly, the existing mainstream laser inertial systems do not deal with the relationship between different voxels, resulting in a high degree of duplication of the same plane. The present invention effectively distinguishes coplanar voxels, and develops a new plane merging strategy based on the union-find set fusion DBSCAN clustering algorithm to estimate the large plane formed by the small plane in the voxel. The uncertainty of the entire map will be significantly reduced, and the accuracy of the system state estimation will be improved. Finally, for structured scenes, a slope detection module is designed to continuously optimize the posture and velocity quantities, suppressing the divergence of the Z-axis state quantity caused by the lack of observations.
[0059] The present invention improves the plane fitting and covariance estimation method in the system from 6DOF to 3DOF. This improvement is from the perspective of engineering implementation, further improving the efficiency of covariance estimation and reducing memory usage. Secondly, the merging module not only improves the plane fitting accuracy and reduces the uncertainty of the overall map, but also reduces the memory usage of the map. BRIEF DESCRIPTION OF THE DRAWINGS
[0060] Figure 1 It is a flow chart of a laser inertial odometer method based on a plane merging strategy provided by an embodiment of the present invention;
[0061] Figure 2 It is a schematic diagram of plane merging provided by an embodiment of the present invention. DETAILED DESCRIPTION
[0062] To make the purpose, technical solution and advantages of the embodiments of the present invention more clear, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present invention.
[0063] The present invention provides a laser inertial odometer method based on a plane merging strategy. The overall process is as follows: Figure 1 As shown in the figure, after completing point accumulation, the laser radar outputs point cloud data, and then reduces the amount of calculation and noise by reducing the amount of point cloud data, thereby improving the real-time and robustness of the system.
[0064] After obtaining the prior pose of the IMU through forward propagation, the voxel downsampled point cloud data is back-propagated to eliminate its motion distortion. After the point cloud data is pre-processed, it will be mapped to the voxel map through a hash function, and the residual will be directly generated according to the point and plane parameters. Compared with other laser inertial odometers, the laser inertial odometer proposed in the present invention does not require KNN search during the residual calculation process, and has certain advantages in computational efficiency.
[0065] After the residual calculation is completed, the state is updated. The PF-LIO system maintains a slope detection window similar to the sliding window optimization in the slope detection module. When the slope counter is greater than the threshold, the state will be adjusted and the correction reference value will be updated. The PF-LIO system refers to a combination of particle filter (PF) and linear inertial odometry (LIO).
[0066] IESKF converges when the state change is extremely small. Otherwise, the system state will continue to iterate until convergence. After completing the state iteration, the preprocessed point cloud data, that is, the point cloud in the laser coordinate system, is transformed into the world coordinate system using the converged state, and then the transformed points are mapped to the voxel map through the spatial hash function. Further, the newly arrived points update the plane parameters through WLS, and use the uncertainty of the points to set different confidence levels for each point. The weight mapping function quickly approaches 1 for high-precision points, and gradually approaches 0 for low-precision points. Compared with LS, which ignores the uncertainty of points when fitting plane parameters, this function can dynamically adjust the weights, so that high-confidence data points occupy a more important position in the regression while also taking into account high-error points.
[0067] After the plane parameters are updated, the converged voxel nodes will merge the planes, which will traverse the converged plane set. Each converged plane node and its neighboring nodes will perform DBSCAN clustering on the plane parameters. The clustered nodes will fuse the plane parameters and assign the plane parameters as the common parent plane of the clustering nodes. The nodes that have been clustered before this round of plane merging will be skipped to prevent invalid and repeated plane merging. For the nodes that have been clustered before this round of plane merging in the same cluster, the plane parameters will be fused with the common parent plane of the clustering nodes in this round, and the clustering node with the most child nodes will be recommended as the new parent node. The two-layer structure of the union-find set will be pruned to maintain it. At the same time, the estimated value of the laser inertial odometer state is output.
[0068] Specifically, the laser inertial odometer method based on the plane merging strategy of the present invention includes the following steps:
[0069] 1. Preprocessing of point cloud data
[0070] After the LiDAR completes point accumulation and outputs raw point cloud data, the point cloud data preprocessing step is first performed to optimize computing efficiency and system performance. The specific steps are as follows:
[0071] Voxel downsampling: The voxel grid method is used to downsample the original point cloud. By defining the size of each voxel, all points within the voxel are replaced by the centroid or average of the voxel, thereby significantly reducing the amount of point cloud data, reducing the amount of calculation for subsequent processing, and reducing the impact of noise.
[0072] Motion distortion elimination: After obtaining the prior pose obtained by the IMU through forward propagation, the pose information is used to perform backpropagation processing on the downsampled point cloud data to eliminate the point cloud distortion caused by device motion and ensure the accuracy and consistency of the point cloud data.
[0073] 2. Hash mapping and residual calculation of point cloud data
[0074] Hash mapping: Map the preprocessed point cloud data to the voxel map through a hash function to achieve fast query and update of the spatial index.
[0075] Residual generation: Generate residuals based on the direct relationship between the points mapped to the voxel map and the known plane parameters. This method avoids the traditional KNN search and directly uses the results of the hash map for calculation, which improves the calculation efficiency.
[0076] 3. Status update and slope detection
[0077] State update: Based on the residual calculation results, the system state is updated, including position, attitude, etc., through the back-end optimization algorithm iterative extended error state Kalman filter IESKF.
[0078] Slope detection and adjustment: The PF-LIO system has a built-in slope detection module. By maintaining a slope detection window, when it detects that the slope change exceeds the preset threshold, it adjusts the status and updates the correction reference value to adapt to the terrain changes.
[0079] 4. Determine whether the system converges
[0080] Determine whether the system state has converged. If not, return to the residual generation step. If so, output the final state estimate of the laser inertial odometer, including key information such as position, attitude, and speed, and enter step 5 at the same time.
[0081] 5. Global coordinate transformation and plane parameter update of point cloud data
[0082] Coordinate transformation: Use the converged system state to transform the pre-processed point cloud data (in the laser coordinate system) into the world coordinate system to unify the coordinate system.
[0083] Spatial hash mapping: The transformed points are mapped to the voxel map again through the spatial hash function to facilitate subsequent processing.
[0084] Plane parameter update: Plane parameters are updated by weighted least squares method, and different confidence weights are set according to the uncertainty of each point. The weight mapping function ensures that high-precision points have higher weights, while the weights of low-precision points gradually decrease, thereby optimizing the fitting accuracy of plane parameters.
[0085] 6. Plane merging and parameter fusion
[0086] Plane merging: Traverse the converged plane set and use the DBSCAN clustering algorithm to cluster the plane nodes and their neighboring nodes to reduce redundant planes and optimize the map structure.
[0087] Parameter fusion: perform plane parameter fusion on the clustered nodes, assign the common parent plane parameters of the clustered nodes to the clustered nodes, and skip the clustered nodes to avoid repeated merging. For the clustered nodes in the same cluster, perform parameter fusion with the common parent plane of the clustered nodes in this round, and recommend the clustering node with the most child nodes as the new parent node, maintaining the two-layer structure of the union-find set to improve query efficiency.
[0088] 7. Iteration
[0089] Iteration convergence: When the state change is extremely small, IESKF is considered to have converged and completed an iteration process. If the convergence condition is not met, the iteration continues until convergence to ensure the accuracy and stability of the system state.
[0090] Specifically, the state estimation of the present invention is based on an iterative extended Kalman filter, which first obtains the prior pose through a discrete kinematic model. and covariance The discrete model is as follows:
[0091]
[0092] Where i-1 is the IMU measurement index, the f function, the state value x, the kinematic input value u and the noise w are defined as:
[0093]
[0094] In laser inertial odometry, manifold It can be used as a way to reduce the dimension of data, using an 18-dimensional composite manifold To describe the system state x, the rotation matrix SO(3) (Lie group) is converted into a Lie algebra to reduce the dimension of the system posture, that is, the 9-dimensional rotation matrix is reduced to a 3-dimensional Lie algebra. In the local homeomorphism of n-dimensional Euclidean space, a bijective mapping can be established.
[0095]
[0096]
[0097]
[0098] It can be seen that the state quantities other than posture are simply algebraic addition and subtraction. Corresponding to the posture, R represents the Lie group, r represents the Lie algebra, Exp is the exponential mapping, which can map the Lie algebra to the Lie group, and Log is the logarithmic mapping, which can map the Lie group to the Lie algebra. Finally, corresponding to the system, under the composite manifold The definition is as follows:
[0099]
[0100] R represents Lie group, r represents Lie algebra, and a and b represent other state quantities (displacement, velocity, etc.).
[0101]
[0102]
[0103]
[0104] The present invention mainly uses three coordinate systems in the laser inertial odometer.
[0105] World coordinate system (G): The inertial coordinate system at the time when the system is initialized.
[0106] Inertial coordinate system (I): IMU body coordinate system.
[0107] LiDAR coordinate system (L): LiDAR body coordinate system.
[0108] Therefore, in the system state x, Represents the rotation matrix transpose from I system to G system, describing the system posture, represents the displacement from the I system to the G system, Indicates the speed from I to G. Indicates the zero bias of the gyroscope and accelerometer. G g T represents the gravitational acceleration in the world coordinate system, They represent angular velocity and acceleration respectively. Represent the Gaussian noise of the gyroscope and accelerometer, respectively. They represent the random walk noise of the gyroscope and accelerometer respectively.
[0109] The prior pose Map the point cloud to the voxel map, calculate the residual, and update the state value to become the maximum a posteriori (MAP) estimate. Specifically, the effective point plane matching observation equation for the i-th epoch is
[0110]
[0111] Where x i is the system state, ni represents the theoretical observation noise of the observation model, v i is the observation noise of the observed quantity;
[0112]
[0113] Among them, W is the world coordinate system, L is the laser radar system, and p i is a map point, t represents the system displacement, is the distance from the k-th iteration midpoint of the i-th laser measurement to the registration plane, and Ω is the parent plane normal vector of the registration plane [a, b, 1] T , the parent plane normal vector of the registration plane can first directly obtain the registration plane index through the spatial hash function, and its time complexity is only O(1), and then access its parent node plane parameters through the registration plane. Compared with the mainstream framework that requires a large number of KNN search operations in residual calculation, it has a great advantage in computational efficiency. Observation noise v i Obey i ~(0,R i ), which is mainly caused by the uncertainty of laser point and plane parameters, as shown in the following equation:
[0114]
[0115]
[0116]
[0117]
[0118]
[0119] in is the plane parameter covariance, is the map point covariance, Represents the uncertainty of plane and map points. are the Jacobian matrices of the observation equation with respect to map points and plane parameters, respectively. Represents the first-order derivative of the observation equation with respect to the map point and plane parameter, and finally through Calculate the uncertainty of the observation error.
[0120] Finally, combining the state prior with all valid measurements gives the MAP estimate:
[0121]
[0122] Specifically, the plane parameter estimation process is as follows: whenever a frame of laser point cloud is received, the laser point cloud is first downsampled by voxel filtering, and then the uncertainty of the laser point in the world coordinate system is calculated by the formula. W p i (i=1,2,3…n), and then PCA is used to determine the points in the voxel W p i Is it possible to form a plane and roughly determine the direction of the plane normal vector n p =[a,b,c], which is the axis with the smallest projection variance. Finally, the plane equation is normalized along the direction of the closest coordinate axis (the subsequent theoretical derivation assumes that it is normalized along the z-axis direction), and the 3DOF plane parameters n = [a,b,d] and ∑ n , a and b are the direction vectors after dimensionality reduction, and d is the intercept.
[0123] Normalize the plane equation along the z-axis, assuming that the point cloud inside the voxel W p i (i=1,2,3…n) satisfies ax+by+z+d=0;
[0124] Considering W p i (i=1,2,3…n), the least squares optimization function can be constructed as:
[0125]
[0126] At the same time, the calculation by formula has W p i Covariance matrix The weight matrix W can be set as:
[0127] W=diag(k1,k2,…,k n )
[0128] k i =1-exp(-1 / w i )
[0129] Among them, w i is the covariance matrix The weight mapping function y = 1.0-exp(-1 / x) converts w i Mapping to k i , the function is in w i When k is smaller i rapidly approaches 1, and at w i When k is larger iGradually approaches 0. Compared with LS, which ignores the uncertainty of points in plane parameter fitting, this function can dynamically adjust the weights, so that high-confidence data points occupy a more important position in regression while also taking into account high-error points. This makes the system of the present invention more robust and less likely to diverge in challenging scenarios.
[0130] Therefore, the formula can be transformed into: An = e;
[0131]
[0132]
[0133] Finally, we can get the closed-form solution of n and its covariance matrix ∑ n :
[0134]
[0135]
[0136] Among them A * is the adjoint matrix of A, and |A| is the determinant of A:
[0137]
[0138]
[0139] Finally, A * , |A| and e are all in incremental form. Compared with the ICP method that repeatedly traverses the point cloud, PF-LIO only needs to accumulate elements when adding new elements. The incremental expression can significantly reduce memory usage and is easier to maintain and expand when implementing the algorithm.
[0140] Specifically, the plane merging algorithm of the present invention is as follows.
[0141] Plane fusion algorithm based on Union-Find 1:
[0142]
[0143]
[0144] In lines 1 to 6, if Figure 2 (a) shows the convergence plane node P in union-find. n , the cluster identifier CulsterId will be initialized. In lines 7-39, the convergence plane node P n VoxelNeighbor( P n, voxelmap) will return the convergence plane node Pn voxel map neighbor node object, S n is the set of nodes to be clustered. In lines 9-39, the clustering nodes P i Perform DBSCAN clustering. In lines 10-12, if cluster node P i If it has already been processed, skip it. In line 13, the cluster node normal vector P i· normvec searches for the node set S to be clustered through KNN. n Find the set of neighbor nodes N that is smaller than the domain radius eps i , note that this set contains P i In lines 14-18, if N i If the number of nodes is less than the threshold value minPts, the cluster node P i Otherwise, in lines 19-21, the cluster node P is determined. i As the core point and assign the cluster identifier CulsterId. In line 22, the cluster node normal vector P i· normvec neighbor node set N i Remove P i , assign it to the cluster expansion set C. In lines 23-37, cluster expansion is completed. In lines 24-28, if the neighbor node has been marked as noise, since the core clustering node P i Density is reachable, re-identify it as a cluster point and assign a cluster identifier CulsterId, and finally the neighbor node Remove the noise NOISE. In lines 29-31, if the neighbor node If it has already been processed, skip it. In lines 32-33, the neighbor nodes Identify it as a cluster point and assign it a cluster identifier CulsterId. In line 34, similarly generate neighbor nodes Neighbor node set In lines 35-37, recursively query the set of normal vector neighbor nodes in the cluster expansion set C If the number is greater than the threshold point minPts, then Merge into the cluster expansion set C to complete cluster expansion. In line 40, the parameters of the parent plane are estimated, and the plane fusion is completed through the plane merging function PlaneFusion(Culster, CulsterId). Finally, after the plane merging is completed, in lines 42-47, all operating points are marked as clustered.
[0145] Plane fusion function, algorithm 2:
[0146]
[0147]
[0148] In lines 1-3, the plane merging parameters are initialized. In lines 4-32, the plane parameters of the cluster Culster are fused. In lines 5-10, the covariance trace and covsum of the cluster Culster with cluster identifier CId are calculated, which will be used for subsequent plane parameter fusion. In lines 11-24, the plane parameters of the cluster Culster with cluster identifier CId that has not been clustered are fused according to the plane parameter fusion formula. And it is assigned to the plane node P CId , this fusion strategy achieves the trace minimization of the plane uncertainty covariance matrix through simple weighted averaging. Figure 2 As shown in (b), after the plane parameter fusion is completed, the parent node of the clustering node Culster that has not been clustered is set to P CId , thus completing the preliminary merging of clusters. In lines 25-31, as Figure 2 As shown in (c), the cluster node P under the cluster Culster that has been clustered and identified as CId is n The parent node P n· f and P CId Merge, and select the node with the most children as the new parent node, and complete the plane parameter fusion according to the plane parameter fusion formula. Figure 2 As shown in (d), the new parent node after the merger needs to be pruned to maintain the two-layer structure of the union-find set and ensure the flat retrieval efficiency of the system.
[0149] Plane parameter fusion formula:
[0150]
[0151]
[0152] The present invention can be used with ultra-low-cost MEMSIMU laser-assisted inertial devices to achieve high-precision and high-efficiency autonomous positioning and map construction by optimizing key steps such as residual calculation, state update, plane parameter update and merging, and slope detection. Tested in a 100m*80m laser odometer challenge environment, the end-to-end horizontal error is only 0.6m and the elevation error is 0.1m.
[0153] The above is only a preferred embodiment of the present invention. It should be understood that the present invention is not limited to the form disclosed herein, and should not be regarded as excluding other embodiments, but can be used in various other combinations, modifications and environments, and can be modified within the scope of the concept described herein through the above teachings or the technology or knowledge of the relevant field. The changes and modifications made by those skilled in the art shall not deviate from the spirit and scope of the present invention, and shall be within the scope of protection of the claims attached to the present invention.
Claims
1. A laser inertial odometer method based on a plane merging strategy, characterized in that: include: S1, preprocessing of point cloud data; The voxel grid method is used to downsample the point cloud data. By defining the size of each voxel, all points in the voxel are replaced by the centroid or average value of the voxel to reduce the amount of point cloud data. After obtaining the prior pose obtained by the IMU through forward propagation, the pose information is used to perform backpropagation processing on the downsampled point cloud data to eliminate the point cloud distortion caused by the movement of the device. The pose refers to the position and attitude. S2, mapping the preprocessed point cloud data to the voxel map through a hash function to complete the plane parameter estimation; S3, performing residual calculation based on the direct relationship between the points mapped into the voxel map and the known plane parameters; S4, based on the residual calculation result, the system state is updated by iteratively extending the error state Kalman filter through the back-end optimization algorithm; S5, after the state is updated, by maintaining a slope detection window, when it is detected that the slope change exceeds a preset threshold, the state is adjusted and the correction reference value is updated; S6, after the slope detection is adjusted, determine whether the system state has converged, if not, return to step S3, if yes, output the state estimation of the laser inertial odometer, and enter step S7; S7, using the converged system state to transform the point cloud data in the laser coordinate system into the world coordinate system, and mapping the transformed point cloud data into the voxel map again through the spatial hash function; S8, updating the plane parameters by weighted least squares method, setting different confidence weights according to the uncertainty of each point cloud data; S9, traverse the converged plane set, and cluster the plane nodes and their neighboring nodes using the DBSCAN clustering algorithm; S10, perform plane parameter fusion on the clustered nodes, assign the common parent plane parameters of the clustering nodes to the clustering nodes, skip the clustered nodes, and for the clustered nodes in the same cluster, perform parameter fusion with the common parent plane of the clustering nodes in this round, and recommend the clustering node with the most child nodes as the new parent node.
2. The laser inertial odometer method based on the plane merging strategy according to claim 1, characterized in that: The state estimation is based on an iterative extended Kalman filter. First, the prior pose is obtained through a discrete kinematic model. and covariance The discrete kinematic model is as follows: Where i-1 is the IMU measurement index, the f function, the state value x, the kinematic input value u and the noise w are defined as: Where M stands for manifold, which is used to represent the state space. represents 15-dimensional real numbers, SO(3) represents the Lie group, that is, the rotation matrix, Represents the rotation matrix transpose from the inertial coordinate system to the world coordinate system, describing the system attitude, represents the displacement from the inertial coordinate system to the world coordinate system, represents the speed from the inertial coordinate system to the world coordinate system, Indicates the zero bias of the gyroscope and accelerometer, GgT indicates the gravitational acceleration in the world coordinate system, They represent angular velocity and acceleration respectively. Represent the Gaussian noise of the gyroscope and accelerometer, respectively. denote the random walk noise of the gyroscope and accelerometer respectively; The prior pose Map the point cloud to the voxel map, calculate the residual, and update the state value to MAP estimate. The specific process includes: The effective point plane matching observation equation for the i-th epoch is: Among them, x i is the system state, n i represents the theoretical observation noise of the observation model, v i is the observation noise of the observed quantity is the distance from the k-th iteration midpoint of the i-th laser measurement to the registration plane, and Ω is the parent plane normal vector of the registration plane [a, b, 1] T , the parent plane normal vector of the registration plane is first directly obtained through the spatial hash function to obtain the registration plane index, and its time complexity is O(1), and then the parent node plane parameters are accessed through the registration plane. W is the world coordinate system, L is the lidar system, and p i is a map point, t represents the system displacement; Observation noise v i Obey i ~(0, R i ), which is mainly caused by the uncertainty of laser point and plane parameters, as shown in the following formula: In the formula, is the plane parameter covariance, is the map point covariance, represents the uncertainty of plane and map points, are the Jacobian matrices of the observation equation with respect to map points and plane parameters, respectively. Represents the first-order derivative of the observation equation with respect to the map point and plane parameter, and finally through Calculate the uncertainty of the observation error; Finally, the state prior is combined with all valid measurements to obtain the MAP estimate:
3. The laser inertial odometer method based on plane merging strategy according to claim 1, characterized in that: The process of plane parameter estimation specifically includes: Whenever a frame of laser point cloud is received, the laser point cloud is first downsampled by voxel filtering. W p i (i_1, 2, 3...n), and then PCA is used to determine the points in the voxel W p i Can a plane be formed? If so, determine the direction of the plane normal vector n p =[a, b, c], i.e. the axis with the minimum projection variance. Finally, the plane equation is normalized along the coordinate axis direction, and WLS is used to estimate the 3DOF plane parameters n = [a, b, d] and ∑ n , a and b are the direction vectors after dimensionality reduction, and d is the intercept; If the plane equation is normalized along the z-axis, the point cloud within the voxel W p i (i_1, 2, 3...n) satisfies: ax+by+z+d=0; Then the least squares optimization function is constructed as: At the same time, according to W p i Covariance matrix Set the weight matrix W to: In=diag(k1,k2,...,k n ) k i =1-exp(-1 / w i ) Among them, w i is the covariance matrix The three-axis RMS of the weight mapping function y = 1.0-exp(-1 / x) will w i Mapping to k i , the function is in w i When k is less than the first setting value i Approaching 1, at w i When k is greater than the second setting value i Approaching 0; K i =1-exp(-1 / w i ) is transformed into: An = e; Finally, we get the closed-form solution of n and its covariance matrix ∑n: Where A* is the adjoint matrix of A, |A| is the determinant of A, and A*, |A| and e are all in incremental form.
4. A computer device comprising a memory storing program instructions, characterized in that: When the program instructions are executed, the laser inertial odometer method based on the plane merging strategy as described in any one of claims 1 to 3 is executed.
Citation Information
Patent Citations
Laser SLAM method based on road surface point cloud plane
CN117031440A
Positioning mapping method for effectively combining environment plane information
CN117470218A
Laser inertial odometer method based on local geometric information fusion
CN117705145A
Detection method, device and equipment integrating SLAM, gradient detection and three-dimensional reconstruction
CN118444280A
Autonomous travel system, vehicle equipped with the same, and autonomous travel method
JP2019056984A
Cited By
Mobile tunnel detection robot and tunnel detection method
CN119984255A
Method and device for determining internal reference error of laser radar and electronic equipment
CN120686243A
Probability plane voxel map recursive construction and updating method for laser speedometer
CN121259213A
Probabilistic plane voxel map recursive construction and update method for laser odometry
CN121259213B
Handwriting depth measuring method based on laser scanning
CN121564062A
