A laser inertial odometer method based on plane merging strategy and computer device

Abstract

This invention relates to the field of laser inertial odometry, specifically to a laser inertial odometry method and computer device based on a plane merging strategy, which greatly improves registration calculation efficiency. The technical solution includes: mapping preprocessed point cloud data to a voxel map using a hash function to complete plane parameter estimation; calculating residuals based on the direct relationship between points mapped to the voxel map and known plane parameters; updating the system state based on the residual calculation results; performing slope detection and state adjustment after the state update; determining whether the system state has converged; if so, outputting the laser inertial odometry state estimate, and simultaneously using the converged system state to transform the point cloud data in the laser coordinate system to the world coordinate system to update the point cloud data; updating and fusing plane parameters using a weighted least squares method. This invention is applicable to laser radar inertial navigation.

Description

Technical Field

[0001] This invention relates to the field of laser inertial odometry, and more specifically to a laser inertial odometry method and computer device based on a planar merging strategy. Background Technology

[0002] LiDAR (Light Detection and Ranging) is a technology that determines the distance to objects by emitting lasers and measuring the reflection time. It can generate high-resolution 3D point cloud data for accurate environmental measurement and mapping. In recent years, with the development of 3D LiDAR technology, especially the emergence of low-cost, high-density solid-state LiDAR, laser inertial odometry has been widely used in autonomous driving, drones, and 3D reconstruction. For many real-world robots or vehicles, LIO (Lidar Inertial Odometry) is not the only algorithm running in the system; all active modules must share computing resources. Therefore, while ensuring system accuracy, a faster and lighter LIO would make the entire system more robust.

[0003] Fast point cloud registration and 3D reconstruction in LiDAR (LiDAR Registration) are key modules in autonomous driving. In recent years, much research has focused on optimizing data structures to find efficient and fast LiDAR registration methods. LiDAR registration methods can be broadly classified into two categories: ICP (Iterative Closest Point) and NDT (Normal Distributions Transform). ICP and its variants construct maps online by registering new scan increments to the point cloud based on a KD-Tree, and then rely on k-nearest neighbor (KNN) search in the point cloud for data association. Errors are generated from the associated data, and state estimation is used to reduce these errors. This process is repeated until the error falls below a threshold. Normal distribution transformation and its variants are classic voxel mapping methods that divide the localization space into voxels. By fitting historical point clouds, each voxel contains a 3D Gaussian distribution. State estimation is expressed as maximum likelihood estimation by calculating the probability of points within the voxels.

[0004] While existing laser inertial odometry (ICP) technology performs well in many applications, it still has some shortcomings and limitations. Although the ICP method is widely used in point cloud registration, it suffers from several significant drawbacks. First, ICP has high computational complexity, requiring repeated iterations to minimize errors. Each iteration involves nearest neighbor search and error calculation, leading to high computational costs, especially in high-density point clouds. Second, ICP converges slowly, particularly when the initial registration error is large. It relies on initial guesses, and if the initial position estimate is inaccurate, more iterations may be needed for convergence, or it may even get stuck in local optima. ICP is highly sensitive to noise and outliers in point clouds, which severely affect the accuracy and stability of registration. Furthermore, the computational complexity of the normal distribution transformation is also high, especially in high-density point clouds, where voxel partitioning and Gaussian distribution fitting are computationally intensive, impacting registration speed. Additionally, NDT consumes significant memory when processing large point cloud datasets, posing a problem for resource-constrained embedded systems. Finally, although NDT uses a probabilistic method for registration, the accuracy of probability estimation decreases in noisy environments, thus affecting the overall registration effect. Summary of the Invention

[0005] The purpose of this invention is to overcome the shortcomings of the prior art and provide a laser inertial odometry method and computer device based on a planar merging strategy, which greatly improves the registration calculation efficiency while taking into account the uncertainty of laser point and pose.

[0006] The present invention achieves the above-mentioned objectives by adopting the following technical solution: Firstly, the present invention provides a laser inertial odometry method based on a planar 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 within the voxel are replaced with the centroid or average value of the voxel, thereby reducing the amount of point cloud data.

[0009] After obtaining the prior pose of the IMU through forward propagation, the pose information is used to perform backward propagation processing on the downsampled point cloud data to eliminate point cloud distortion caused by device motion. Pose refers to position and orientation.

[0010] S2. Map the preprocessed point cloud data to a voxel map using a hash function to complete the planar parameter estimation;

[0011] S3. Perform residual calculation based on the direct relationship between the points mapped to the voxel map and the known plane parameters;

[0012] S4. Based on the residual calculation results, the error state Kalman filter is iteratively extended through the back-end optimization algorithm to update the system state;

[0013] S5. After the status is updated, a slope detection window is maintained. When the slope change is detected to exceed the preset threshold, the status is adjusted and the correction benchmark value is updated.

[0014] S6. After the slope detection adjustment, determine whether the system state has converged. If not, return to step S3. If yes, output the state estimate of the laser inertial odometry and proceed to step S7.

[0015] S7. Use the converged system state to transform the point cloud data in the laser coordinate system to the world coordinate system, and then map the transformed point cloud data to the voxel map again through the spatial hash function;

[0016] S8. Update the plane parameters using the weighted least squares method, setting different confidence weights based on the uncertainty of each point cloud data.

[0017] S9. Traverse the converged set of planes and use the DBSCAN clustering algorithm to cluster the plane nodes and their neighboring nodes.

[0018] S10. Perform planar parameter fusion on the clustered nodes, assign the common parent plane parameters of the clustered nodes to the clustered nodes, and skip the already clustered nodes. For the already clustered nodes in the same cluster, perform parameter fusion with the common parent plane of the current clustered nodes, and elect the clustered 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. Covariance The discrete kinematic model is as follows:

[0020] Where i-1 is the IMU measurement index, and the function f, state value x, kinematic input value u, and noise w are defined as follows:

[0021]

[0022]

[0023]

[0024]

[0025] In the formula, M represents the manifold, used to represent the state space. SO(3) represents a 15-dimensional real number, and SO(3) represents a Lie group, i.e., a rotation matrix. The transpose of the rotation matrix from the inertial coordinate system to the world coordinate system describes the system's attitude. This represents the displacement from the inertial coordinate system to the world coordinate system. This represents the velocity from the inertial coordinate system to the world coordinate system. This indicates that the gyroscope and accelerometer have zero bias. G g T Represents gravitational acceleration in the world coordinate system. These represent angular velocity and acceleration, respectively. These represent the Gaussian noise of the gyroscope and accelerometer, respectively. These represent the random walk noise of the gyroscope and accelerometer, respectively.

[0026] The prior pose The point cloud is mapped onto a voxel map, the residuals are calculated, and the updated state values ​​are used as MAP estimates. The specific process includes:

[0027] The effective point-plane matching observation equation for the i-th epoch is:

[0028]

[0029]

[0030] Where, x i For the system state, n i The observation noise of the representative observation model, v i It is the observation noise of the observed quantity. Let Ω be the distance from the midpoint of the k-th iteration of the i-th laser measurement to the registration plane, and Ω be the normal vector of the parent plane of the registration plane [a,b,1]. T The normal vector of the parent plane of the registration plane is first obtained directly through a spatial hash function to obtain the index of the registration plane, which has a time complexity of O(1). Then, the parameters of its parent node plane are accessed through the registration plane. W is the world coordinate system, L is the lidar system, and p i Let t be a map point, and t represent the system displacement.

[0031] Observation noise v i Obey v i ~(0,R i This is mainly caused by the uncertainty of the laser point and plane parameters, as shown in the following formula:

[0032]

[0033]

[0034]

[0035]

[0036] In the formula, where It is the covariance of the plane parameters. It is the map point covariance. This indicates the uncertainty of plane and map points. These are the Jacobian matrices of the observation equations with respect to map points and plane parameters, respectively. This represents the first derivative of the observation equation with respect to map points and plane parameters, and finally... Calculate the uncertainty of the observation error;

[0037] Finally, by combining the state prior with all valid measurements, the MAP estimate is obtained:

[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 using voxel filtering, and the point cloud within the voxel is... 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 n of the plane's normal vector. p = [a,b,c], which is the axis with the minimum projection variance. Finally, the equation of the plane is normalized along the coordinate axis directions, and the 3DOF plane parameters n = [a,b,d] and ∑ are estimated using WLS. n ;

[0041] If we normalize the plane equation along the z-axis, the voxel in-point cloud W p i (i = 1, 2, 3…n) satisfy: ax + by + z + d = 0;

[0042] The least squares optimization function is then constructed as follows:

[0043]

[0044] At the same time, according to W p i covariance matrix Set the weight matrix W as follows:

[0045] W = diag(k1, k2, ..., k n )

[0046] k i =1-exp(-1 / w i )

[0047] Among them, w i Covariance matrix The three-axis RMS, the weight mapping function y = 1.0 - exp(-1 / x) will transfer w i Mapped to k i The function in w i When k is less than the first set value i Approaching 1, at w i When k is greater than the second set value i Approaching 0;

[0048] k i =1-exp(-1 / w i The equivalent transformation is: An = e;

[0049]

[0050]

[0051] Finally, we obtain the closed-form solution for n and its covariance matrix ∑ n :

[0052]

[0053]

[0054] Where A * Let A be the adjoint matrix, |A| be the determinant of A, and A * 、|A| and e are all in incremental form.

[0055] In a second aspect, the present invention provides a computer device including a memory storing program instructions, wherein when the program instructions are executed, the laser inertial odometry method based on the planar merging strategy described above is performed.

[0056] The beneficial effects of this invention are as follows:

[0057] This invention directly maps the laser point cloud to voxels using a HashMap to complete planar parameter estimation. After the laser points are mapped to voxels via the HashMap, point-plane residuals are directly generated. The time complexity of mapping using a spatial hash function is O(1). This method does not require KNN (K-Nearest Neighbors) search, which greatly improves the registration calculation efficiency while taking into account the uncertainty of laser points and poses.

[0058] This invention addresses map uncertainties caused by lidar measurement noise by explicitly parameterizing and estimating planes within voxels. Weighted least squares fitting is then used for plane fitting, improving the system's robustness in degraded scenarios. Secondly, existing mainstream lidar inertial systems fail to handle relationships between different voxels, leading to height repetition on the same plane. This invention effectively distinguishes coplanar voxels and develops a novel plane merging strategy based on the union-find set fusion DBSCAN clustering algorithm. This strategy estimates the large planes formed by small planes within voxels, significantly reducing overall map uncertainty and improving system state estimation accuracy. Finally, for structured scenarios, a slope detection module is designed to continuously optimize pose and velocity quantities, suppressing Z-axis state divergence caused by insufficient observations.

[0059] This invention improves the plane fitting and covariance estimation methods in the system from 6DOF to 3DOF. This improvement, from an engineering implementation perspective, further enhances the efficiency of covariance estimation and reduces memory usage. Secondly, the merging module not only improves plane fitting accuracy and reduces overall map uncertainty, but also reduces map memory consumption. Attached Figure Description

[0060] Figure 1 This is a flowchart of a laser inertial odometry method based on a planar merging strategy provided by an embodiment of the present invention;

[0061] Figure 2 This is a schematic diagram of planar merging provided in an embodiment of the present invention. Detailed Implementation

[0062] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings.

[0063] This invention provides a laser inertial odometry method based on a planar merging strategy, the overall process of which is as follows: Figure 1 As shown, after the lidar completes point accumulation, it outputs point cloud data. Then, by reducing the amount of point cloud data, the computational load and noise are reduced, thereby improving the real-time performance and robustness of the system.

[0064] After obtaining the prior pose from the IMU through forward propagation, the voxel downsampled point cloud data is backpropagated to eliminate motion distortion. After preprocessing, the point cloud data is mapped to a voxel map using a hash function, and residuals are directly generated based on point and plane parameters. Compared to other laser inertial odometry systems, the laser inertial odometry proposed in this invention does not require KNN lookup during residual calculation, thus offering an advantage in computational efficiency.

[0065] After residual calculation is completed, the state is updated. The PF-LIO system maintains a slope detection window similar to a sliding window optimization in the slope detection module. When the slope counter exceeds a threshold, state adjustment is performed and the baseline value is updated and corrected. The PF-LIO system refers to a combination based on particle filtering (PF) and linear inertial measurement units (LIO).

[0066] When the state change is extremely small, IESKF converges. Otherwise, the system state continues to iterate until convergence. After completing the state iteration, the preprocessed point cloud data (i.e., the point cloud in the laser coordinate system) is transformed to the world coordinate system using the converged state. Then, the transformed points are mapped to the voxel map using a spatial hash function. Further, newly arrived points are updated using WLS to update the plane parameters. Different confidence levels are set for each point based on the uncertainty of the points. The weight mapping function rapidly approaches 1 for high-precision points and gradually approaches 0 for low-precision points. Compared to LS, which ignores the uncertainty of points when fitting plane parameters, this function can dynamically adjust the weights, giving high-confidence data points a more important role in the regression while also considering high-error points.

[0067] After the plane parameters are updated, the converged voxel nodes will undergo plane merging. This involves traversing the set of converged planes, and each converged plane node and its neighbors will perform DBSCAN clustering on its plane parameters. The clustered nodes will then have their plane parameters fused, and these parameters will be assigned to the common parent plane of the clustering nodes. Nodes already clustered before this round of plane merging will be skipped to prevent invalid and redundant plane merging. For nodes within the same cluster that were already clustered before this round of plane merging, their plane parameters will be fused with the common parent plane of the current clustering nodes. The clustering node with the most child nodes will be elected as the new parent node, and the two-layer structure of the union-find set will be pruned. Simultaneously, the laser inertial odometry state estimate will be output.

[0068] Specifically, the laser inertial odometry method based on a planar 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, a preprocessing step is performed to optimize computational efficiency and system performance. The specific steps are as follows:

[0071] Voxel downsampling: The original point cloud is downsampled using a voxel grid method. By defining the size of each voxel, all points within the voxel are replaced with the centroid or average value of the voxel, thereby significantly reducing the amount of point cloud data, reducing the computational load of subsequent processing, and reducing the impact of noise.

[0072] Motion distortion elimination: After obtaining the prior pose of the IMU through forward propagation, the downsampled point cloud data is processed by backward propagation using this pose information to eliminate 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: The preprocessed point cloud data is mapped to the voxel map through a hash function, enabling fast querying and updating of the spatial index.

[0075] Residual generation: Residuals are generated based on the direct relationship between points mapped to the voxel map and known plane parameters. This method avoids traditional KNN lookup, directly utilizing the results of hash mapping for calculation, thus improving computational efficiency.

[0076] 3. Status Update and Slope Detection

[0077] State update: Based on the residual calculation results, the system state, including position and attitude, is updated by iteratively expanding the error state Kalman filter (IESKF) through a back-end optimization algorithm.

[0078] Slope detection and adjustment: The PF-LIO system has a built-in slope detection module. By maintaining a slope detection window, when a slope change is detected to exceed a preset threshold, the system adjusts the status and updates the correction benchmark value to adapt to terrain changes.

[0079] 4. Determine if the system has converged.

[0080] Determine if the system state has converged. If not, return to the residual generation step. If yes, output the final state estimate of the laser inertial odometry, including key information such as position, attitude, and velocity, and proceed to step 5.

[0081] 5. Global coordinate transformation and planar parameter update of point cloud data

[0082] Coordinate transformation: The preprocessed point cloud data (in the laser coordinate system) is transformed to the world coordinate system using the converged system state to unify the coordinate system.

[0083] Spatial hash mapping: The transformed points are mapped back to the voxel map using a spatial hash function for easier subsequent processing.

[0084] Planar parameter update: The plane parameters are updated using weighted least squares, with different confidence weights assigned based on 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 the plane parameters.

[0085] 6. Plane merging and parameter fusion

[0086] Plane merging: Traverse the converged set of planes 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: Planar parameter fusion is performed on the clustered nodes. The common parent plane parameters of the clustered nodes are assigned to the clustered nodes, while skipping already clustered nodes to avoid duplicate merging. For already clustered nodes in the same cluster, parameter fusion is performed with the common parent plane of the current clustered nodes, and the clustered node with the most child nodes is elected as the new parent node, maintaining the two-layer structure of the union-find set to improve query efficiency.

[0088] 7. Iteration

[0089] Iterative convergence: When the state change is extremely small, IESKF is considered convergent, and one iteration is completed. If the convergence condition is not met, iteration continues until convergence, ensuring the accuracy and stability of the system state.

[0090] Specifically, the state estimation of this invention is based on an iterative extended Kalman filter, which first obtains the prior pose through a discrete kinematic model. Covariance The discrete model is as follows:

[0091]

[0092] Where i-1 is the IMU measurement index, and the function f, state value x, kinematic input value u, and noise w are defined as follows:

[0093]

[0094] In laser inertial odometry, manifold It can be used as a method for data dimensionality reduction, employing an 18-dimensional composite manifold. To describe the system state x, we reduce the dimensionality of the system attitude by transforming the rotation matrix SO(3) (a Lie group) into a Lie algebra; that is, the 9-dimensional rotation matrix is ​​reduced to a 3-dimensional Lie algebra. This is done for manifolds. Local homeomorphisms exist in n-dimensional Euclidean space, allowing the establishment of bijective mappings.

[0095]

[0096]

[0097]

[0098] As can be seen, state variables other than pose are simply algebraic additions and subtractions. In the pose context, R represents a Lie group, r represents a Lie algebra, Exp is the exponential mapping (which maps a Lie algebra to a Lie group), and Log is the logarithmic mapping (which maps a Lie group to a Lie algebra). Ultimately, this corresponds to the state variables under the composite manifold in the system. The definition is as follows:

[0099]

[0100] R represents the Lie group, r represents the Lie algebra, and a and b represent other state variables (displacement, velocity, etc.).

[0101]

[0102]

[0103]

[0104] This invention mainly uses three coordinate systems in laser inertial odometry.

[0105] World coordinate system (G): The inertial coordinate system at the moment the system initialization is completed.

[0106] Inertial coordinate system (I): IMU body coordinate system.

[0107] LiDAR coordinate system (L): LiDAR volume coordinate system.

[0108] Therefore, in system state x, This represents the transpose of the rotation matrix from the I-frame to the G-frame, describing the system attitude. This represents the displacement from system I to system G. Indicates the velocity from the I-series to the G-series. This indicates that the gyroscope and accelerometer have zero bias. G g T Represents gravitational acceleration in the world coordinate system. These represent angular velocity and acceleration, respectively. These represent the Gaussian noise of the gyroscope and accelerometer, respectively. These represent the random walk noise of the gyroscope and accelerometer, respectively.

[0109] The prior pose Mapping the point cloud onto a voxel map, calculating the residuals, and updating the state values ​​yields the maximum a posteriori (MAP) estimate. Specifically, the effective point-plane matching observation equation for the i-th epoch is:

[0110]

[0111] In the formula x i For the system state, ni The observation noise of the representative observation model, v i It is the observation noise of the observed quantity;

[0112]

[0113] Where W is the world coordinate system, L is the lidar system, and p i Let t be a map point, and t represent the system displacement. Let Ω be the distance from the midpoint of the k-th iteration of the i-th laser measurement to the registration plane, and Ω be the normal vector of the parent plane of the registration plane [a,b,1]. T The normal vector of the parent plane of the registration plane can be directly obtained through a spatial hash function to obtain the index of the registration plane, with a time complexity of only O(1). Then, the parameters of its parent node plane can be accessed through the registration plane. Compared with the mainstream frameworks that require a large number of KNN lookup operations in residual calculation, this has a significant advantage in computational efficiency. Observation noise v i Obey v i ~(0,R i This is mainly caused by the uncertainty of the laser point and plane parameters, as shown in the following formula:

[0114]

[0115]

[0116]

[0117]

[0118]

[0119] in It is the covariance of the plane parameters. It is the map point covariance. This represents the uncertainty of planes and map points. These are the Jacobian matrices of the observation equations with respect to map points and plane parameters, respectively. This represents the first derivative of the observation equation with respect to map points and plane parameters, and finally... The uncertainty of the observation error is calculated.

[0120] Finally, by combining the state prior with all valid measurements, the MAP estimate can be obtained:

[0121]

[0122] Specifically, the planar parameter estimation process is as follows: Whenever a frame of laser point cloud is received, the laser point cloud is first downsampled using voxel filtering, and then the uncertainty of the laser points in the world coordinate system is calculated using the formula. (Voxel in-cloud point cloud) 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, and can the direction of the plane's normal vector n be roughly determined? p = [a,b,c], which is the axis with the minimum projection variance. Finally, the plane equation is normalized along the direction of the closest coordinate axis (subsequent theoretical derivations assume normalization along the z-axis), and the 3DOF plane parameters n = [a,b,d] and ∑ are estimated using WLS. n , where a and b are the dimensionality-reduced direction vectors, and d is the intercept.

[0123] Normalize the plane equations along the z-axis, assuming an in-voxel point cloud. W p i (i = 1, 2, 3…n) satisfy ax + by + z + d = 0;

[0124] Considering W p i For (i = 1, 2, 3…n), the least squares optimization function can be constructed as follows:

[0125]

[0126] At the same time, calculations can be performed using formulas. W p i covariance matrix The weight matrix W can be set as follows:

[0127] W = diag(k1, k2, ..., k n )

[0128] k i =1-exp(-1 / w i )

[0129] Among them, w i Covariance matrix The three-axis RMS. The weight mapping function y = 1.0 - exp(-1 / x) will... i Mapped to k i The function in w i When k is small i It rapidly approaches 1, while w i When k is large iThe values ​​gradually approach 0. Compared to LS, which ignores the uncertainty of points when fitting planar parameters, this function can dynamically adjust the weights, giving high-confidence data points a more significant role in the regression while also taking into account high-error points. This makes the system of this invention more robust and less prone to divergence in challenging scenarios.

[0130] Therefore, the equation can be transformed into: An = e;

[0131]

[0132]

[0133] Finally, we can obtain the closed-form solution of n and its covariance matrix ∑ n :

[0134]

[0135]

[0136] Where A * Let |A| be the adjoint matrix of A, and |A| be the determinant of A.

[0137]

[0138]

[0139] Finally, A * |A| and e are both in incremental form. Compared to the repeated ICP method that repeatedly traverses the point cloud, PF-LIO only needs to increment the element when adding a new element. The incremental form of the expression can significantly reduce memory usage and is easier to maintain and extend when implementing the algorithm.

[0140] Specifically, the plane merging algorithm of this invention is as follows.

[0141] Planar fusion algorithm based on Union-Find 1:

[0142]

[0143]

[0144] In lines 1-6, such as Figure 2 As shown in (a), 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 The VoxelNeighbor and its voxel map neighbor nodes are merged in a planar manner. P n, voxelmap) will return the convergence plane node Pn The voxel map neighbor node object, S n This represents the set of nodes to be clustered. Lines 9-39 specify the nodes P to be clustered. i Perform DBSCAN clustering. In lines 10-12, if cluster node P... i Skip if already processed. In line 13, the cluster node normal vector P... i· Normvec uses KNN search to find the clustering set S of nodes to be clustered. n Find the set N of neighbor nodes smaller than the neighborhood radius eps. i Note that this set contains P. i In lines 14-18, if N i If the number of cluster nodes is less than the threshold number minPts, then cluster node P will be... i If identified as noise, then in lines 19-21, the cluster node P will be determined. i Assign the core point and a cluster identifier CulsterId. In line 22, assign the cluster node normal vector P. i· The set of neighbor nodes of normvec N i Remove P from i This is assigned to the cluster expansion set C. Lines 23-37 complete the cluster expansion. Lines 24-28 check if the neighboring nodes... It has been marked as noise due to the core cluster node P i Density is reachable, so it is re-identified as a cluster point and assigned a cluster identifier CulsterId. Finally, neighboring nodes are... Remove noise (NOISE). In lines 29-31, if neighboring nodes... Skip if already processed. In lines 32-33, the neighboring nodes... Identify cluster points and assign a cluster identifier (CulsterId). Line 34 similarly generates neighbor nodes. Neighbor node set Lines 35-37 recursively query the set of normal neighbor nodes in the cluster expansion set C. If the number of points is greater than the threshold number of points minPts, then... The cluster expansion is completed by incorporating the expanded cluster set C into the new cluster. Line 40 performs parameter estimation on the parent plane and then merges the planes using the plane merging function PlaneFusion(Culster, CulsterId). Finally, after merging, lines 42-47 mark all operation points as clustered.

[0145] Planar blending function, Algorithm 2:

[0146]

[0147]

[0148] Lines 1-3 initialize the plane merging parameters. Lines 4-32 perform plane parameter fusion on the Culster clusters. Lines 5-10 calculate the covariance trace and covsum of the Culster clusters with cluster identifier CId, which will be used for subsequent plane parameter fusion. Lines 11-24 perform plane parameter fusion on the Culster clusters with un-clustered cluster identifier CId according to the plane parameter fusion formula and assign the results to the plane node P. CId This fusion strategy minimizes the trace of the covariance matrix of planar uncertainty through a simple weighted average. For example... Figure 2 As shown in (b), after completing the planar parameter fusion, let P be the parent node of the un-clustered cluster node Culster. CId This completes the initial merging of clusters. In lines 25-31, as shown... Figure 2 As shown in (c), the cluster nodes P under Culster that have already been clustered and identified as CId are... n Parent node P n· f and P CId Merge the nodes, selecting the one with the most child nodes as the new parent node, and complete the planar parameter fusion according to the planar parameter fusion formula. Finally, as... Figure 2 As shown in (d), the new parent node after merging needs to be pruned to maintain the two-layer structure of the union-find set and ensure the planar retrieval efficiency of the system.

[0149] Planar parameter fusion formula:

[0150]

[0151]

[0152] This invention can be used with ultra-low-cost MEMSIMU laser-assisted inertial devices to achieve high-precision and high-efficiency autonomous positioning and map building by optimizing key steps such as residual calculation, state update, plane parameter update and merging, and slope detection. In a challenging 100m*80m laser odometry environment, the end-to-end horizontal error was only 0.6m, and the elevation error was 0.1m.

[0153] The above description is merely a preferred embodiment of the present invention. It should be understood that the present invention is not limited to the forms disclosed herein and should not be construed as excluding other embodiments. It can be used in various other combinations, modifications, and environments, and can be altered within the scope of the concept described herein through the above teachings or related technologies or knowledge. Modifications and variations made by those skilled in the art that do not depart from the spirit and scope of the present invention should be within the protection scope of the appended claims.

Claims

1. A laser inertial odometry method based on a planar 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 within the voxel are replaced with the centroid or average value of the voxel, thereby reducing the amount of point cloud data. After obtaining the prior pose of the IMU through forward propagation, the pose information is used to perform backward propagation processing on the downsampled point cloud data to eliminate point cloud distortion caused by device motion. Pose refers to position and orientation. S2. Map the preprocessed point cloud data to a voxel map using a hash function to complete the planar parameter estimation; S3. Perform residual calculation based on the direct relationship between the points mapped to the voxel map and the known plane parameters; S4. Based on the residual calculation results, the error state Kalman filter is iteratively extended through the back-end optimization algorithm to update the system state; S5. After the status is updated, a slope detection window is maintained. When the slope change is detected to exceed the preset threshold, the status is adjusted and the correction benchmark value is updated. S6. After the slope detection adjustment, determine whether the system state has converged. If not, return to step S3. If yes, output the state estimate of the laser inertial odometry and proceed to step S7. S7. Use the converged system state to transform the point cloud data in the laser coordinate system to the world coordinate system, and then map the transformed point cloud data to the voxel map again through the spatial hash function; S8. Update the plane parameters using the weighted least squares method, setting different confidence weights based on the uncertainty of each point cloud data. S9. Traverse the converged set of planes and use the DBSCAN clustering algorithm to cluster the plane nodes and their neighboring nodes. S10. Perform planar parameter fusion on the clustered nodes, assign the common parent plane parameters of the clustered nodes to the clustered nodes, and skip the already clustered nodes. For the already clustered nodes in the same cluster, perform parameter fusion with the common parent plane of the current clustered nodes, and elect the clustered node with the most child nodes as the new parent node.

2. The laser inertial odometry method based on a planar merging strategy according to claim 1, characterized in that, The state estimation is based on an iterative extended Kalman filter, which first obtains the prior pose through a discrete kinematic model. Covariance The discrete kinematic model is as follows: Where i-1 is the IMU measurement index, and the function f, state value x, kinematic input value u, and noise w are defined as follows: In the formula, M represents the manifold, used to represent the state space. SO(3) represents a 15-dimensional real number, and SO(3) represents a Lie group, i.e., a rotation matrix. The transpose of the rotation matrix from the inertial coordinate system to the world coordinate system describes the system's attitude. This represents the displacement from the inertial coordinate system to the world coordinate system. This represents the velocity from the inertial coordinate system to the world coordinate system. This indicates zero bias for the gyroscope and accelerometer, while GgT represents gravitational acceleration in the world coordinate system. These represent angular velocity and acceleration, respectively. These represent the Gaussian noise of the gyroscope and accelerometer, respectively. These represent the random walk noise of the gyroscope and accelerometer, respectively. The prior pose The point cloud is mapped onto a voxel map, the residuals are calculated, and the updated state values ​​are used as MAP estimates. The specific process includes: The effective point-plane matching observation equation for the i-th epoch is: Where, x i For the system state, n i The observation noise of the representative observation model, v i The observation noise of the observation quantity Let Ω be the distance from the midpoint of the k-th iteration of the i-th laser measurement to the registration plane, and Ω be the normal vector of the parent plane of the registration plane [a, b, 1]. T The normal vector of the parent plane of the registration plane is first obtained directly through a spatial hash function to obtain the index of the registration plane, which has a time complexity of O(1). Then, the parameters of its parent node plane are accessed through the registration plane. W is the world coordinate system, L is the lidar system, and p i Let t be a map point, and t represent the system displacement. Observation noise v i Obey v i ~(0, R) i This is mainly caused by the uncertainty of the laser point and plane parameters, as shown in the following formula: In the formula, where It is the covariance of the plane parameters. It is the map point covariance. This indicates the uncertainty of plane and map points. These are the Jacobian matrices of the observation equations with respect to map points and plane parameters, respectively. This represents the first derivative of the observation equation with respect to map points and plane parameters, and finally... Calculate the uncertainty of the observation error; Finally, by combining the state prior with all valid measurements, the MAP estimate is obtained:

3. The laser inertial odometry method based on a planar 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 using voxel filtering, and the point cloud within the voxel is... 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 n of the plane's normal vector. p = [a, b, c], which is the axis with the minimum projection variance. Finally, the equation of the plane is normalized along the coordinate axis directions, and the 3DOF plane parameters n = [a, b, d] and ∑ are estimated using WLS. n a and b are the reduced-dimensional direction vectors, and d is the intercept; If we normalize the plane equation along the z-axis, the voxel in-point cloud W p i (i_1, 2, 3...n) satisfy: ax + by + z + d = 0; The least squares optimization function is then constructed as follows: At the same time, according to W p i covariance matrix Set the weight matrix W as follows: W=diag(k1,k2,...,k n ) k i =1-exp(-1 / w i ) Among them, w i Covariance matrix The three-axis RMS, the weight mapping function y = 1.0 - exp(-1 / x) will transfer w i Mapped to k i The function in w i When k is less than the first set value i Approaching 1, at w i When k is greater than the second set value i Approaching 0; k i =1-exp(-1 / w i The equivalent transformation is: An = e; Finally, we obtain the closed-form solution for 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 odometry method based on the planar merging strategy as described in any one of claims 1-3 is performed.

Images