An optimization method for laser radar inertial odometer based on adaptive error state fusion
By calculating the error state vectors for ground and non-ground feature points separately and performing adaptive weighted fusion, the problem of insufficient pose estimation accuracy in lidar inertial odometry is solved, achieving higher pose accuracy and better system adaptability.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- SHENZHEN AUTOMOTIVE RES INST BEIJING INST OF TECH (SHENZHEN RES INST OF NAT ENG LAB FOR ELECTRIC VEHICLES)
- Filing Date
- 2026-04-03
- Publication Date
- 2026-07-24
AI Technical Summary
Existing lidar inertial odometry methods fail to adequately consider the differentiated contributions of different types of feature points to pose estimation when processing ground and non-ground feature points, resulting in insufficient pose estimation accuracy, especially when the number of feature points is unbalanced.
An adaptive error state fusion method is adopted to calculate the error state vectors of ground and non-ground feature points respectively, and then fuse them through an adaptive weighting strategy to generate a total error state vector to update the pose.
It significantly improves pose estimation accuracy, enhances the algorithm's performance in scenarios with an uneven number of feature points, and strengthens the system's versatility and accuracy.
Smart Images
Figure CN121953983B_ABST
Abstract
Description
Technical Field
[0001] This application relates to the field of robot localization and navigation technology, and in particular to an optimization method for lidar inertial odometry based on adaptive error state fusion. Background Technology
[0002] SLAM is a key technology for enabling robots to locate and map in unknown environments. Among them, LiDAR-Inertial SLAM has significant advantages in accuracy and robustness due to its combination of high-precision point clouds and short-term motion priors from IMU. Point cloud matching is a core component, and mainstream methods include probabilistic model-based and feature-based methods. The latter often uses a point-to-surface error model, optimizes using edge points and planar points, and improves feature quality and matching stability through virtual point construction, texture fusion, deep learning, and semantic information. However, existing methods often optimize ground and non-ground feature points uniformly, ignoring the different constraints they impose on different degrees of freedom of pose—ground points mainly constrain z-axis translation, roll, and pitch angles, while non-ground points dominate x and y-axis translation and heading angles. When feature distribution is unbalanced, this uniform treatment can easily lead to insufficient optimization of some pose components, affecting overall accuracy, indicating that current methods have not fully modeled the differentiated contributions of different types of features to pose estimation. Summary of the Invention
[0003] This invention addresses the technical problems existing in the background art by proposing an optimization method for lidar inertial odometry based on adaptive error state fusion. This method minimizes the point-to-surface error for different types of feature points and fuses the corresponding error states to improve pose estimation accuracy. First, the point cloud is segmented into ground points and non-ground planar points. Then, the error state vectors of each type of feature point are calculated, and an adaptive weighted fusion strategy is used to update the pose.
[0004] To solve the technical problem, the technical solution of the present invention is as follows:
[0005] An optimization method for lidar inertial odometry based on adaptive error state fusion, the method comprising:
[0006] Step S1: Perform motion distortion compensation on the original point cloud acquired by the lidar, and perform pre-integration based on the inertial measurement unit data to obtain the initial pose estimate of the current frame relative to the previous frame.
[0007] Step S2: Perform ground segmentation on the current frame point cloud after motion compensation to obtain a set of ground feature points and a set of non-ground feature points; using the initial pose estimation, transform the set of ground feature points and the set of non-ground feature points to the global map coordinate system respectively, and search for the corresponding plane in the map for each feature point to establish a point-to-plane matching relationship.
[0008] Step S3: Based on the matching relationship, construct the point-to-plane distance error function for the ground feature point set and the non-ground feature point set respectively, and solve for the first error state vector and the second error state vector respectively; according to the geometric constraint characteristics of different translation and rotation degrees of freedom, adopt an adaptive weighting strategy to fuse the first error state vector and the second error state vector to generate a total error state vector, and use the total error state vector to update the pose of the current frame.
[0009] Furthermore, ground segmentation of the current frame point cloud includes: constructing a two-dimensional grid map of fixed size, and projecting the current frame point cloud into the corresponding grid.
[0010] Furthermore, the ground segmentation also includes: performing principal component analysis on the point cloud within each grid cell to estimate its local normal vector, and determining grid cells whose vertical component of the normal vector is greater than a preset threshold as ground candidate grid cells.
[0011] Furthermore, the ground segmentation also includes: taking the ground candidate grid closest to the origin as the starting grid, traversing its neighboring grids through breadth-first search, and determining the final set of ground feature points and non-ground feature points from the ground candidate grids according to the geometric continuity criterion.
[0012] Furthermore, the establishment of the point-to-plane matching relationship includes: for each feature point transformed to the global map coordinate system, searching for its nearest neighbor set in the global map, fitting a local plane based on the nearest neighbor set using the moving least squares method and obtaining the normal vector of the plane, thereby establishing the matching relationship between the feature point and the fitted plane.
[0013] Furthermore, the first error state vector and the second error state vector are obtained by linearizing the point-to-plane distance error functions corresponding to the ground feature point set and the non-ground feature point set, respectively.
[0014] Furthermore, in the total error state vector, the horizontal component of the translation error is taken from the second error state vector, and the vertical component of the translation error is taken from the first error state vector.
[0015] Furthermore, the yaw angle error component in the total error state vector is taken from the second error state vector.
[0016] Furthermore, the roll angle and pitch angle error components in the total error state vector are obtained by weighted summation of the corresponding components in the first error state vector and the second error state vector according to adaptive weights.
[0017] Furthermore, the method for determining the adaptive weights includes: generating multiple candidate weights within a preset interval; calculating the corresponding weighted fusion result for each candidate weight, and calculating the matching residuals of the ground feature point set and the non-ground feature point set based on the result; and selecting the optimal candidate weight as the adaptive weight based on minimizing the comprehensive index of the matching residuals of the two types of feature point sets.
[0018] An optimization method for lidar inertial odometry based on adaptive error state fusion, the system comprising:
[0019] Data preprocessing and initial pose acquisition module: performs motion distortion compensation on the raw point cloud acquired by the lidar, and performs pre-integration based on the inertial measurement unit data to obtain the initial pose estimate of the current frame relative to the previous frame.
[0020] Feature classification and map matching module: Perform ground segmentation on the current frame point cloud after motion compensation to obtain ground feature point set and non-ground feature point set; Using the initial pose estimation, transform the ground feature point set and the non-ground feature point set to the global map coordinate system respectively, and search for the corresponding plane in the map for each feature point to establish the point-to-plane matching relationship;
[0021] The adaptive fusion optimization and pose update module: Based on the matching relationship, it constructs a point-to-plane distance error function for the ground feature point set and the non-ground feature point set, respectively, and solves for the first error state vector and the second error state vector. According to the geometric constraint characteristics of different translation and rotation degrees of freedom, it adopts an adaptive weighting strategy to fuse the first error state vector and the second error state vector to generate a total error state vector, and uses the total error state vector to update the pose of the current frame.
[0022] This application has the following advantages:
[0023] (1) The decoupling optimization method for different types of feature points proposed in this application lays an important foundation for solving the problem of decreased pose estimation accuracy caused by uneven distribution of feature points.
[0024] (2) The error pose state adaptive fusion method proposed in this application significantly improves the versatility of the overall algorithm and can be applied to a variety of complex scenarios.
[0025] (3) Compared with the traditional point-to-surface ICP SLAM algorithm, the method proposed in this application can achieve higher pose accuracy even under the premise of uneven number of feature points. Attached Figure Description
[0026] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.
[0027] Figure 1 This is a schematic diagram of the correspondence between feature points in the embodiment;
[0028] Figure 2 This is a schematic diagram of the pose graph in the embodiment;
[0029] Figure 3 This is a technical roadmap for an optimization method of lidar inertial odometry based on adaptive error state fusion proposed in the embodiments. Detailed Implementation
[0030] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.
[0031] Example 1:
[0032] like Figure 3 As shown, an optimization method for lidar inertial odometry based on adaptive error state fusion includes the following steps:
[0033] S1. Data Preprocessing:
[0034] The system integrates a data preprocessing module, which consists of motion compensation and IMU pre-integration. Specifically: Motion compensation corrects distortions in the LiDAR point cloud caused by motion using IMU data; IMU pre-integration calculates the relative motion between consecutive LiDAR scans.
[0035] S2. Feature point classification and corresponding modules;
[0036] S21. Feature point classification;
[0037] S211. First, construct a fixed-size two-dimensional raster map (e.g., Figure 1 (As shown in line 4 of Algorithm 1), and input point cloud data. Projected onto the raster map (rows 5–8): for each point in the point cloud According to its , The coordinates determine the grid to which the point belongs, and the point is stored in the point set of the corresponding grid.
[0038] S212. Subsequently, normal vector estimation and type determination are performed for each raster cell (lines 9–16): using the Principal Component Analysis (PCA) method (denoted as the function). Calculate the local normal vector of the point cloud within this raster. If the normal vector is in The component in the axial direction is greater than a preset threshold. (Right now If the condition is met, the grid is marked as "ground" type and its normal vector is saved; otherwise, it is marked as "non-ground" type.
[0039] S213. Further, such as Figure 2 As shown in Algorithm 2, the system uses a function The starting point is selected as the ground grid closest to the origin, and its neighboring grids are traversed using breadth-first search (BFS). During the traversal, the ground point determination conditions defined in line 12 of Algorithm 2 are used to verify whether the neighboring grids satisfy the ground continuity and geometric consistency. If they satisfy the conditions, they are classified as ground grids; otherwise, they are classified as non-ground grids.
[0040] S214. Finally, through the above ground segmentation process, the original point cloud is... Separate into ground point sets Non-ground point set (As shown in line 17 of Algorithm 1), and output as input to the subsequent pose optimization module.
[0041] S22. Frame to map feature point correspondence;
[0042] Step S221: Divide the current frame point cloud into the current frame ground point cloud and the current frame non-ground point cloud according to the result of step S1, and obtain the global map point cloud and the current frame initial pose obtained by IMU pre-integration.
[0043] Step S222 (taking ground point cloud as an example): Using the initial pose, the ground point cloud of the current frame is... The ground point cloud in the global coordinate system is obtained by transforming the sensor coordinate system to the global map coordinate system, denoted as . .
[0044] Step S223: For Each point in Perform the following operations:
[0045] Step S2231: Apply point cloud data to the global map In the middle, point Perform a nearest neighbor search to obtain the 5 nearest neighbor points;
[0046] Step S2232: Based on the 5 neighboring points, perform plane fitting using the Moving Least Squares (MLS) method to obtain the points... The corresponding fitting plane normal vector ;
[0047] Step S2233: Select a distance point from the five neighboring points. The nearest neighbor point, which is the corresponding point of this point in the global map point cloud, is denoted as... ;
[0048] Step S2234: Based on the point and corresponding points The geometric relationship between them is used to calculate the distance from a point to a plane or related characteristic values, denoted as... And construct the corresponding tuples ,in, For distance features or error terms, For the corresponding map points, This is the normal vector of the corresponding plane.
[0049] Step S224: For the non-ground point cloud of the current frame, repeat the processing flow of steps S202 to S203 to establish a correspondence between each point in the non-ground point cloud and the global map point cloud, and obtain the corresponding tuple set of the non-ground point cloud.
[0050] Step S225: Input the corresponding tuple sets obtained from the ground point cloud and non-ground point clouds into the subsequent matching and pose optimization module to construct the point-to-plane error function and solve for the pose optimization result. The initial pose is provided by IMU pre-integration.
[0051] S3. Adaptive optimized registration;
[0052] This module includes error state solving and adaptive error state fusion.
[0053] S31. Solving for the error state:
[0054] The distance error from a point to a plane is widely used as the objective function in existing SLAM methods, and its general form can be written as:
[0055] (1)
[0056] in, This represents the i-th point in the current LiDAR frame. This is the normal vector of the corresponding plane. Let be a rotation matrix. It is a translation vector. These are the corresponding points in the global map. Ground feature points and non-ground feature points use the same optimization objective function, therefore the resulting error state vectors have the same form.
[0057] Therefore, in the subsequent error state calculation, non-ground feature points will be used as an example for detailed explanation. First, based on the correspondence established in the previous feature point classification section, a point-to-plane error function is constructed for non-ground feature points:
[0058] (2)
[0059] in, This represents the distance error from the k-th non-ground feature point to the plane. This represents the k-th non-ground feature point in the i-th frame of LiDAR (in the current frame coordinate system). Indicates and The corresponding global map point, This is the normal vector of the corresponding plane.
[0060] Based on this, the error function is linearized, and the least squares method can be used to optimize the pose error state variable δX. The optimization problem can be written as:
[0061] (3)
[0062] Where m represents the number of non-ground feature points. Let $\frac{k}{k}$ be the initial point-to-plane distance error of the k-th non-ground feature point under the current pose estimation. For the k-th error term, the error state The Jacobian matrix.
[0063] For calculation and We introduce pose error state variables and re-express the error function. Let the translation error be...
[0064] (4)
[0065] Attitude error (Euler angle increment) is
[0066] (5)
[0067] The overall error state vector is
[0068] (6)
[0069] Let the rotation increment be Then the error of non-ground feature points can be written as:
[0070] (7)
[0071] For small attitude increments, a first-order approximation is used:
[0072] (8)
[0073] in It is a 3×3 identity matrix. For vector The constructed 3×3 antisymmetric matrix. After substitution, the error is approximately:
[0074] (9)
[0075] When expanded, it can be written as:
[0076] (10)
[0077] in, For vector Construct a 3×3 antisymmetric matrix.
[0078] Therefore, the initial distance error is defined as follows:
[0079] (11)
[0080] Based on the above formula, taking the partial derivative with respect to the error state δX, we can obtain the Jacobian matrix of the distance error with respect to the error state. Let I_{3×3} be the 3×3 identity matrix, then the Jacobian of the k-th error term can be written as:
[0081] (12)
[0082] By stacking the Jacobians and residuals of all non-ground feature points row by row, a linear system can be constructed:
[0083] (13)
[0084] in, It is an m×6 Jacobian matrix (each row corresponds to an error term for a non-ground feature point). Let m be the residual vector. Taking the derivative of the least squares objective function corresponding to equation (13) and setting it to zero, we can obtain the normal equation for the error state:
[0085] (14)
[0086] in, That is, the error state vector obtained from non-ground feature points.
[0087] It should be noted that the above derivation completes the error state calculation for non-ground feature points. For ground feature points, the same derivation can be used to obtain the corresponding error state vector. For the sake of brevity:
[0088] The error state vector obtained based on non-ground feature points is denoted as... ;
[0089] The error state vector obtained based on ground feature points is denoted as... .
[0090] Subsequently, based on these two error state vectors, an adaptive weight allocation strategy can be used to fuse the error states and update the pose variables.
[0091] S32. Adaptive error fusion:
[0092] Based on the above derivation, two types of error state vectors have been obtained. The key issue now is how to use these two error state vectors to update the pose variables. To this end, this application proposes an optimization method for lidar inertial odometry based on adaptive error state fusion.
[0093] First, this application considers the fusion of the translation component in the error state vector. Since non-ground feature points cannot provide constraints on translation along the z-axis, and ground feature points cannot provide effective constraints on translation in the xy-plane, the fused translation error... The following method is used for calculation (denoted as Equation (15)):
[0094]
[0095] (15)
[0096]
[0097] in, Represents the error state vector The component in the x-direction, , Indicates in , The directional component.
[0098] Next, this application considers the fusion of the yaw components in the error state vector. Since ground feature points cannot provide constraints on rotation around the yaw direction, the yaw components in the fused error state directly adopt the results of non-ground feature points (denoted as Equation (16)):
[0099] (16)
[0100] Then, this application considers the fusion of pitch and roll components in the error state vector. Ground feature points have a constraining effect on both angles, while non-ground feature points can also provide effective constraints. Therefore, this application proposes a fusion strategy based on adaptive weight allocation to achieve joint constraints on pitch and roll. In this strategy, based on the residual information corresponding to ground and non-ground points, the fusion is performed on... and The roll and pitch components are assigned different weights to achieve weighted fusion. The calculation method of the fused roll and pitch components is as follows (denoted as Equation (17)):
[0101]
[0102] (17)
[0103] in, This represents the weighting coefficient.
[0104] After obtaining the fused error state variables, the pose is updated using the following form (denoted as Equation (18)):
[0105] (18)
[0106] in, This represents the updated pose variable; This is another representation of the state vector X, which can be converted using a standard pose mapping relationship. .
[0107] As mentioned above, weighting coefficients The determination depends on the residual information provided by ground points and non-ground points. Specifically, a series of candidate error state vectors are first generated by linear interpolation according to equations (11)–(13). In this process, the coefficients The value is taken within the interval [0,1] with a step size of 0.1. Next, for each candidate error state vector, the corresponding residual vector is calculated at both the non-ground and ground points (denoted as Equation (19)):
[0108]
[0109] (19)
[0110] Then calculate the residual norm (denoted as Equation (20)):
[0111]
[0112] (20)
[0113] in, Represents the residuals of non-ground points. This represents the residual at the ground point. Further, a comprehensive residual index is defined. (denoted as formula (21)):
[0114] (twenty one)
[0115] in, This represents the comprehensive residual index given the weight coefficient a. The optimal interpolation coefficient a* is determined by the following formula (denoted as Equation (22)):
[0116] (twenty two)
[0117] Ultimately, Substitute the optimal weight coefficients into equation (17) to complete the weighted fusion of roll and pitch components.
[0118] Although preferred embodiments of this application have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments as well as all changes and modifications falling within the scope of this application.
[0119] Obviously, those skilled in the art can make various modifications and variations to this application without departing from the spirit and scope of this application. Therefore, if such modifications and variations fall within the scope of the claims of this application and their equivalents, this application also intends to include such modifications and variations.
Claims
1. An optimization method for lidar inertial odometry based on adaptive error state fusion, characterized in that, The method includes: Step S1: Perform motion distortion compensation on the original point cloud acquired by the lidar, and perform pre-integration based on the inertial measurement unit data to obtain the initial pose estimate of the current frame relative to the previous frame. Step S2: Perform ground segmentation on the current frame point cloud after motion compensation to obtain a set of ground feature points and a set of non-ground feature points; using the initial pose estimation, transform the set of ground feature points and the set of non-ground feature points to the global map coordinate system respectively, and search for the corresponding plane in the map for each feature point to establish a point-to-plane matching relationship. Step S3: Based on the matching relationship, construct point-to-plane distance error functions for the ground feature point set and the non-ground feature point set respectively, and solve for the first error state vector and the second error state vector respectively; wherein, the first error state vector is obtained by linearly solving the point-to-plane distance error function corresponding to the ground feature point set, and the second error state vector is obtained by linearly solving the point-to-plane distance error function corresponding to the non-ground feature point set. The first error state vector and the second error state vector are fused to generate a total error state vector. The x-axis and y-axis components of the translation error in the total error state vector are taken from the second error state vector, the z-axis component of the translation error is taken from the first error state vector, the yaw angle error component is taken from the second error state vector, and the roll angle and pitch angle error components are obtained by weighted summation of the corresponding components in the first error state vector and the second error state vector according to adaptive weights. The adaptive weights are determined as follows: multiple candidate weights are generated within a preset interval; a weighted fusion result is calculated for each candidate weight, and the matching residuals of the ground feature point set and the non-ground feature point set are calculated based on the result; the optimal candidate weight is selected as the adaptive weight based on minimizing the comprehensive index of the matching residuals of the two types of feature point sets; and the pose of the current frame is updated using the total error state vector.
2. The method according to claim 1, characterized in that, Ground segmentation of the current frame point cloud includes: constructing a two-dimensional raster map of fixed size and projecting the current frame point cloud into the corresponding raster.
3. The method according to claim 2, characterized in that, The ground segmentation further includes: performing principal component analysis on the point cloud within each grid cell to estimate its local normal vector, and identifying grid cells whose vertical component of the normal vector is greater than a preset threshold as ground candidate grid cells.
4. The method according to claim 3, characterized in that, The ground segmentation further includes: taking the ground candidate grid closest to the origin as the starting grid, traversing its neighboring grids through breadth-first search, and determining the final set of ground feature points and non-ground feature points from the ground candidate grids according to the geometric continuity criterion.
5. The method according to claim 1, characterized in that, The establishment of the point-to-plane matching relationship includes: for each feature point transformed to the global map coordinate system, searching for its nearest neighbor set in the global map, fitting a local plane based on the nearest neighbor set using the moving least squares method and obtaining the normal vector of the plane, thereby establishing the matching relationship between the feature point and the fitted plane.