Tight coupling laser radar-inertial odometer method based on iterative error state Kalman filtering

Abstract

The invention provides a tight coupling laser radar-inertial odometer method based on iterative error state Kalman filtering. The method belongs to the field of laser Lidar positioning. Comprising the following steps of: firstly, initializing a Lidar point cloud by preprocessing the Lidar point cloud and carrying out motion compensation on IMU (Inertial Measurement Unit) data; secondly, storing and incrementally updating plane parameters or statistical information through a hash table to construct a global map; feature fitting is carried out through plane features and statistical information in voxels, and point cloud registration is carried out by adaptively utilizing the information according to environment features; then state estimation is carried out through IESKF (Iterative Error State Kalman Filter), and the positioning precision is improved by combining the constraint of point-to-plane and point-to-plane distribution; and finally, updating plane parameters and statistical information in the voxel map through the new Lidar scanning data increment. According to the tight coupling laser radar-inertial odometer method based on iterative error state Kalman filtering, the positioning precision and robustness of a laser radar SLAM algorithm based on the tight coupling laser radar-inertial odometer method are greatly improved compared with other advanced algorithms.

Description

Technical Field

[0001] The present invention belongs to the technical field of laser Lidar positioning, and particularly relates to a tightly-coupled lidar-inertial odometry method based on iterative error state Kalman filtering. Background Art

[0002] Currently, for point cloud registration, the mainstream algorithms are all based on the structured features in the environment. They extract feature points from the geometric features of the target point cloud and the source point cloud for registration, or fit geometric features such as planes in the global map. As a form of environmental representation, structured features such as lines and planes have been widely used in urban scenes and achieved good results. However, in a cluttered scene, it is difficult to obtain sufficient and stable structured features for point cloud alignment. The lack of these structured constraints will lead to poor positioning accuracy or even failure. In a scene with cluttered features, point-to-point distribution registration can make more effective use of point cloud data and avoid feature fitting failure or point cloud discarding caused by the lack of structured information. However, in the traditional NDT algorithm, all cells with sufficient number of points are equally weighted to construct the objective function. This does not correspond to the variability of the distribution between cells in the real situation and affects the accuracy of point cloud alignment. We propose a new tightly-coupled lidar-inertial odometry method based on iterative error state Kalman filtering, which realizes the adaptive utilization of structured features and statistical information in the point cloud. Summary of the Invention

[0003] Aiming at the problem of low accuracy of the existing SLAM (Simultaneous Localization and Mapping) system in a cluttered scene, the present invention proposes a tightly-coupled lidar-inertial odometry method based on iterative error state Kalman filtering.

[0004] In order to achieve the above object, the technical solution adopted by the present invention is as follows:

[0005] This solution provides a tightly-coupled lidar-inertial odometry method based on iterative error state Kalman filtering, which is characterized by including the following steps:

[0006] S1. Initialize the Lidar point cloud by performing motion compensation through preprocessing the Lidar point cloud and IMU data;

[0007] S2. Construct a global map by storing and incrementally updating plane parameters or statistical information through a hash table;

[0008] S3. Perform feature fitting through the plane features and statistical information in the voxel, and adaptively utilize this information for point cloud registration according to the environmental features;

[0009] S4, state estimation is performed through iterative error state Kalman filtering (IESKF), while combining point-to-plane and point-to-distribution constraints to improve positioning accuracy;

[0010] S5. Incrementally update the plane parameters and statistical information in the voxel map using the new Lidar scan data.

[0011] Furthermore, the Lidar point cloud initialization steps in step S1 are as follows:

[0012] When the system receives raw LiDAR point cloud data from the sensor, it determines whether it contains IMU data. If so, it uses the inertial measurement data provided by the IMU for motion compensation to eliminate distortion caused by movement during the LiDAR point cloud scanning process. Otherwise, it uses a motion compensation method based on linear interpolation to eliminate point cloud motion distortion.

[0013] Furthermore, step S2 includes the following steps:

[0014] In the laser SLAM (simultaneous localization and mapping) positioning system, we use hash tables to build a global map. In the global world coordinate system, we divide the space into small voxels at a certain resolution, where the key of the hash table is the identifier of each voxel, and the value of the hash table represents the plane parameters and statistical information of the voxel, and also serves as a lookup node for plane merging. After the first LIDAR scan arrives, the points in it are scattered into voxels, and an index is added to the voxel containing the point in the hash table. If the voxel contains enough points, PCA (principal component analysis) is used to determine whether the points in the voxel can fit into a plane.

[0015] Furthermore, step S3 includes the following steps:

[0016] Unlike other methods that only utilize geometric features of the environment, we not only fit planar features within voxels, but also fit a normal distribution based on the 3D point cloud. The subsequent residual construction process can adaptively utilize these two types of environment representations: if a plane exists in the voxel, the 3D planar representation is used to leverage the planar features in the voxel for point-to-plane alignment. If no plane exists in the voxel, a normal distribution is fitted using the voxel's statistical information to directly construct a point-to-distribution alignment.

[0017] S301. We use 3DOF plane representation and calculate the plane parameters n = [a, b, d] by linear least squares method. T ]. Assume we have a set of coplanar lidar w p i, (i = 1, ···, N), without loss of generality, assume the main axis is the z-axis. By normalizing along the main axis, all points on the plane satisfy the formula: ax + by + z + d = 0. Therefore, we can derive the least squares optimization function:

[0018]

[0019] Obtain the closed-form solution of the plane parameter n.

[0020] Assume a voxel contains a set of LiDAR points w p i , (i = 1, ···, N), each point has its own uncertainty ∑ w p i . For each voxel, we approximate it with a multivariate normal distribution N(μ, Σ). The statistical information parameters are calculated as follows:

[0021]

[0022] S302. We perform matching according to the following strategy: If the point can establish a point-plane match with the corresponding voxel, construct a point-plane constraint. If there is no voxel containing a plane for matching, construct a point-to-distribution constraint based on the normal distribution estimation of the voxel.

[0023] Here we focus on the construction of the point distribution constraint. Assume the point w p i predicted using the prior position and orientation in the world coordinate system does not find the corresponding plane after querying, and the voxel satisfies the multivariate normal distribution N(μ, Σ). We construct the point-to-distribution constraint as follows:

[0024] d i = ( W p i - μ) T ∑ -1 ( W p i - μ)

[0025] And substitute the uncertainty of the LiDAR point w p i and the voxel average value μ into the above formula:

[0026]

[0027] Generally, the first term should be as small as possible and close to zero. Therefore, d i can be estimated as a normal distribution That is to say, if a point is located in the corresponding voxel, its corresponding d i should satisfy the above distribution

[0028] Furthermore, step S4 includes the following steps:

[0029] S401. The state estimation adopts the iterative error state Kalman filtering method. However, due to the simultaneous presence of point-to-plane and point-to-distribution constraints, our method will have two different observation functions. Assume there is a prior of state estimation and covariance We will update it through the constraints of point-to-plane distance and point-to-distribution, and obtain a maximum a posteriori estimation problem. The observation function obtained from the i-th successful point-to-distribution matching is:

[0030]

[0031] where we use subscript 1 to represent the relevant quantities of point-to-distribution matching and subscript 2 to represent the relevant quantities of point-to-plane matching. is the residual representation of point-to-point distribution obtained according to formula (). v 1i is the LiDAR point L p 1i The observation noise of, and the voxel mean μ follows a (0, R 1i ) distribution. The corresponding derivation is as follows:

[0032]

[0033] S402. Similarly, we can obtain the observation equation and observation noise of the j-th valid point-to-plane matching, which we distinguish with subscript 2. Finally, by combining the state prior and two types of valid observations (N and M respectively), we can obtain the MAP estimation:

[0034]

[0035] S403. Since there are two observation models, the measurement matrix H and measurement noise covariance R are calculated as follows in the subsequent Kalman gain calculation:

[0036]

[0037] R = diag(R 11 ,..., R 1N , R 21 ,...P 2M )

[0038] Furthermore, step S5 includes the following steps:

[0039] S501. When new lidar points from a scan arrive, the state estimation component is first used to calculate the positions of these points in the global world coordinate system and register these points into the global map. If the index voxels have not been constructed, they are built based on the newly added points and the corresponding parameters are calculated. Otherwise, we use an incremental update method to register these points with the constructed voxels. For statistical information, assume that the mean and variance in the voxel are already available, denoted as μ H and Σ H , the mean and variance of the newly registered points are denoted as μ A and ∑ A , then the statistical information μ and Σ of the updated voxel are calculated as follows:

[0040]

[0041] S502. After the global map is updated, we discard all the points in each voxel and only retain its plane parameters, statistical information and its uncertainty. Then, we use a plane merging method based on union-find to merge the coplanar surfaces in the updated global map.

[0042] Compared with the prior art, the beneficial effects of the present invention are:

[0043] The present invention proposes a tightly coupled lidar-inertial odometry method based on iterative error state Kalman filtering. In the world coordinate system, the space is divided into small voxels according to a certain resolution; each frame scanned by the Lidar sensor is scattered into the voxels; a plane feature and a normal distribution are fitted within the voxel, and the uncertainty introduced by the LiDAR measurement noise is considered; and the point cloud registration is adaptively performed using the plane feature and the normal distribution to construct the constraints of point-to-plane and point-to-distribution; then the iterative error state Kalman filter (IESKF) is used for state estimation, combined with the constraints of point-to-plane and point-to-distribution, to improve the positioning accuracy. While maintaining good robustness, the present invention also effectively addresses the problem of increased computational burden existing in most methods, significantly reducing the computational burden in the feature association stage, improving the response speed of the system, and better meeting the real-time requirements of the SLAM system. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 is the flowchart of the method of the present invention.

[0045] Figure 2 is the estimated trajectory and ground truth map of the method of the present invention in the public botanical garden dataset.

[0046] Figure 3 is a comparison diagram of the trajectories and xyz-axis offsets of the method of the present invention and other algorithms on the LoopB route. DETAILED DESCRIPTION OF THE INVENTION

[0047] The specific embodiments of the present invention will be described below to facilitate those skilled in the art of this technology to understand the present invention. However, it should be clear that the present invention is not limited to the scope of the specific embodiments. For those of ordinary skill in the art of this technology, as long as various changes are within the spirit and scope of the present invention defined and determined by the appended claims, these changes are obvious, and all inventions created using the concept of the present invention are within the scope of protection.

[0048] Embodiment

[0049] The present invention proposes a tightly coupled lidar-inertial odometry method based on iterative error state Kalman filter. First, the lidar point cloud is initialized by preprocessing the lidar point cloud and IMU data for motion compensation; secondly, the global map is constructed by storing and incrementally updating the plane parameters or statistical information through a hash table; then, the plane features and statistical information in the voxel are used for feature fitting, and these information are adaptively used for point cloud registration according to the environmental features; then, the state estimation is carried out through the iterative error state Kalman filter (IESKF), and at the same time, the constraints of point-to-plane and point-to-distribution are combined to improve the positioning accuracy; finally, the plane parameters and statistical information in the voxel map are incrementally updated through the new lidar scan data. As Figure 1 shown, the present invention proposes a tightly coupled lidar-inertial odometry method based on iterative error state Kalman filter, and its implementation method is as follows:

[0050] S1. Initialize the lidar point cloud by preprocessing the lidar point cloud and IMU data for motion compensation:

[0051] When the system starts to receive the original lidar point cloud data through the sensor, it is judged whether IMU data is included. If IMU data is included in the system, the inertial measurement data provided by the IMU is used for motion compensation to eliminate the distortion of the lidar point cloud caused by motion during the scanning process. Otherwise, the motion compensation method based on linear interpolation is used to eliminate the point cloud motion distortion.

[0052] S2. Construct the global map by storing and incrementally updating the plane parameters or statistical information through a hash table, and its implementation method is as follows:

[0053] In the laser SLAM (Simultaneous Localization and Mapping) positioning system, we use a hash table to construct the global map. In the global world coordinate system, we divide the space into small voxels according to a certain resolution. The key of the hash table is the identifier of each voxel, and the value of the hash table represents the plane parameters and statistical information of the voxel, and also serves as a lookup node for plane merging. After the first LIDAR scan arrives, the points in it are scattered into the voxels, and an index is added to the voxels containing the points in the hash table. If there are enough points in the voxel, PCA (Principal Component Analysis) is used to determine whether the points in the voxel can be fitted to a plane.

[0054] S3. Feature fitting is performed through the plane features and statistical information in the voxel, and these information are adaptively used for point cloud registration according to the environmental features. The implementation method is as follows:

[0055] Different from other methods that only use environmental geometric features, we not only fit plane features within the voxel, but also fit the normal distribution based on the three-dimensional point cloud. The subsequent residual construction process can adaptively use these two types of environmental representations: if there is a plane in the voxel, the 3DOF plane representation is used to utilize the plane features in the voxel for point-plane alignment. If there is no plane in the voxel, the statistical information of the voxel is used to fit the normal distribution, and the alignment of points to the distribution is directly constructed.

[0056] S301. We use the 3DOF plane representation and calculate the plane parameter n = [a, b, d] through the linear least squares method T . Suppose we have a set of coplanar lidar W p i , (i = 1, ···, N). Without loss of generality, assume the main axis is the z-axis. By normalizing along the main axis, all points on the plane satisfy the formula: ax + by + z + d = 0. Therefore, we can derive the least squares optimization function:

[0057]

[0058] Obtain the closed-form solution of the plane parameter n.

[0059] Suppose a voxel contains a set of lidar points w p i , (i = 1, ···, N), and each point has its own uncertainty Σ w p i . For each voxel, we approximate it with a multivariate normal distribution that follows N(μ, Σ). The statistical information parameters are calculated as follows:

[0060]

[0061] S302. We perform matching according to the following strategy: If the point can establish a point-plane match with the corresponding voxel, a point-plane constraint is constructed. If there is no voxel containing a plane for matching, a point-to-distribution constraint is constructed based on the normal distribution estimation of the voxel.

[0062] Here we focus on the construction of the point distribution constraint. Assume a point w p i predicted using prior position and attitude in the world coordinate system. After querying, no corresponding plane is found, and the voxel satisfies the multivariate normal distribution N(μ, Σ). We construct the point-to-distribution constraint as follows:

[0063] d i = ( W p i - μ) T Σ -1 ( W p i - μ)

[0064] And substitute the uncertainty of the LiDAR point w p i and the voxel mean μ into the above formula:

[0065]

[0066] Generally, the first term should be as small as possible and close to zero. Therefore, d i can be estimated as a normal distribution That is to say, if a point is located in the corresponding voxel, its corresponding d i should satisfy the above distribution

[0067] S4. State estimation is performed through Iterated Error State Kalman Filter (IESKF), and at the same time, the point-to-plane and point-to-distribution constraints are combined. The implementation method is as follows:

[0068] S401. The state estimation uses the Iterated Error State Kalman Filter method. However, due to the simultaneous existence of the point-to-plane and point-to-distribution constraints, our method will have two different observation functions. Assume there is a prior and covariance for state estimation. We will update them through the constraints of the point-to-plane distance and point-to-distribution, and obtain a maximum a posteriori estimation problem. The observation function obtained from the i-th successful point-to-distribution match is:

[0069]

[0070] where we use subscript 1 to represent the quantities related to the point-to-distribution match and subscript 2 to represent the quantities related to the point-to-plane match. is the residual representation of the point-to-point distribution obtained according to formula (). v 1i is the LiDAR point L p 1i is the observation noise of, and the voxel mean μ follows a (0, R 1i ) distribution. The corresponding derivation is as follows:

[0071]

[0072] S402. Similarly, we can obtain the observation equation and observation noise for the j-th valid point to plane matching, which we distinguish with subscript 2. Finally, by combining the state prior and two types of valid observations (N and M respectively), we can obtain the MAP estimate:

[0073]

[0074] S403. Since there are two observation models, the measurement matrix H and the measurement noise covariance R are calculated as follows in the subsequent Kalman gain calculation:

[0075]

[0076] R = diag(R 11 ,..., R 1N , R 21 ,...R 2M )

[0077] S5. The plane parameters and statistical information in the voxel map are incrementally updated through the new Lidar scan data, and the implementation method is as follows:

[0078] S501. When the Lidar points from the new scan arrive, the state estimation component is first used to calculate the positions of these points in the global world coordinate system and register these points into the global map. If the indexed voxels have not been constructed, they are constructed based on the newly added points and the corresponding parameters are calculated. Otherwise, we use the incremental update method to register these points with the constructed voxels. For the statistical information, assuming that the mean and variance in the voxel are already available, denoted as μ H and Σ H , and the mean and variance of the newly registered points are denoted as μ A and ∑ A , then the updated voxel statistical information μ and Σ are calculated as follows:

[0079]

[0080] S502. After the global map update is completed, we discard all the points in each voxel and only retain its plane parameters, statistical information and its uncertainty. Then, we use the plane merging method based on union-find to merge the coplanar in the updated global map.

[0081] The present invention was tested on the unstructured natural environment public dataset BotanicGarden, and a dataset was collected by ourselves in the outdoor environment of the campus and compared with other latest methods.

[0082] We tested on the unstructured natural environment public dataset BotanicGarden, collected a dataset by ourselves in the outdoor environment of the campus, and compared with other latest methods. All experiments were carried out on a desktop computer equipped with a 13th generation Intel(R) Core(TM) i7-13700F processor and 16GB of RAM. The comparison results can be seen in Tables 1 and 2. And the Figure 2 shows the comparison results of the estimated trajectory and the ground truth trajectory accuracy of the method of the present invention under the public plant dataset.

[0083] Table 1 Accuracy ATE of different methods on the public plant dataset

[0084]

[0085] Experimental data show that in a cluttered environment such as a forest, the structural features of the environment are not obvious, which affects the feature extraction and matching process of LOAM and results in poor accuracy. FAST-LIO2 adopts a deterministic plane representation, and at the same time, for real-time considerations, only a small number of points are used to fit the plane, making the plane estimation inaccurate. Environmental factors such as tree clusters have a high degree of variability, which also leads to difficulties in feature extraction. And VoxelMap++ fails to localize on most routes and often has large offsets when turning at large angles. However, in this scenario, on the one hand, our method can mainly extract plane features from the ground through the method of probabilistic plane representation, and on the other hand, it can construct additional constraints through the statistical information of non-planar voxels in the surrounding unstructured area, and perform state estimation based on the constraints obtained from these two aspects, making full use of environmental information and achieving better accuracy.

[0086] To further evaluate the performance of our algorithm in different environments, we collected additional routes on the campus using our own data collection platform and conducted experimental tests. Table 2 gives the detailed results under four routes. And Figure 3 shows the trajectory comparison and xyz-axis offsets of the method of the present invention and other algorithms on Loop B under the self-collected dataset.

[0087] Table 2 End-to-end error (m) of the self-collected sequence

[0088]

[0089] It can be observed that the end-to-end error of our method is better than that of VoxelMap and VoxelMap++ on all paths. Notably, under loops B and C, compared with FAST-LIO2, our method also has a significant improvement in accuracy. However, for the other two routes, loop A and loop D, our performance is not as strong as FAST-LIO2, which may be due to the presence of a large number of straight tree trunks in the forest environment and FAST-LIO2 effectively selects these tree trunks as features to improve the positioning accuracy. On the other hand, our method can adaptively fit planes or estimated statistical information according to the environment, extract and search for possible plane mergers, provide point-to-plane constraints for structured areas such as the ground and building walls, and provide point-to-distribution constraints based on voxel statistical information for cluttered environments (such as grasslands and trees). This also shows that for this route with large environmental changes and coexistence of structured and unstructured scenes, our method still maintains good accuracy and has high applicability and robustness.

Claims

1. A tightly coupled lidar-inertial odometry method based on iterative error state Kalman filter, characterized in that Including the following steps: S1. Initialize the Lidar point cloud by performing motion compensation on the preprocessed Lidar point cloud and IMU data; S2. Construct a global map by storing and incrementally updating plane parameters or statistical information in a hash table; S3. Perform feature fitting based on plane features and statistical information in voxels, and adaptively utilize this information for point cloud registration according to environmental features; S4. Perform state estimation through Iterative Error State Kalman Filter (IESKF), and at the same time combine the constraints of point-to-plane and point-to-distribution to improve the positioning accuracy; S5. Incrementally update the plane parameters and statistical information in the voxel map with new Lidar scan data.

2. The tightly coupled lidar-inertial odometry method based on iterative error state Kalman filtering according to claim 1, wherein The steps for initializing the Lidar point cloud in step S1 are as follows: When the system starts to receive the original Lidar point cloud data through the sensor, it is judged whether IMU data is included. If IMU data is included in the system, the inertial measurement data provided by the IMU is used for motion compensation to eliminate the distortion of the LiDAR point cloud caused by motion during the scanning process. Conversely, a motion compensation method based on linear interpolation is used to eliminate the point cloud motion distortion.

3. The tightly-coupled lidar-inertial odometry method based on iterative error state Kalman filtering according to claim 1, characterized in that, The steps of step S2 are as follows: In the laser SLAM (Simultaneous Localization and Mapping) positioning system, we construct a global map with a hash table. In the global world coordinate system, we divide the space into small voxels according to a certain resolution. The key of the hash table is the identifier of each voxel, and the value of the hash table represents the plane parameters and statistical information of the voxel, and also serves as a search node for plane merging. After the first LIDAR scan arrives, the points in it are scattered into voxels, and an index is added to the voxels containing the points in the hash table. If there are enough points in the voxel, PCA (Principal Component Analysis) is used to determine whether the points in the voxel can be fitted to a plane.

4. The tightly coupled lidar-inertial odometry method based on iterative error state Kalman filtering according to claim 1, characterized in that, The steps of step S3 are as follows: Different from other methods that only utilize environmental geometric features, we not only fit plane features within voxels, but also fit a normal distribution based on the three-dimensional point cloud. The subsequent residual construction process can adaptively utilize these two types of environmental representations: if there is a plane in the voxel, a 3DOF plane representation is used to utilize the plane features in the voxel for point-plane alignment. If there is no plane in the voxel, the statistical information of the voxel is used to fit a normal distribution to directly construct the alignment of point to distribution. S301. We use a 3DOF plane representation and calculate the plane parameters \(n = [a, b, d]\) by linear least squares method. T . Suppose we have a set of coplanar lidar W p i , (\(i = 1,\cdots,N\)). Without loss of generality, assume the main axis is the z-axis. By normalizing along the main axis, all points on the plane satisfy the formula: \(ax + by+z + d = 0\). Therefore, we can derive the least squares optimization function: Obtain the closed-form solution of the plane parameter n. Suppose a voxel contains a set of lidar points W p i , (i = 1, ···, N), each point has its own uncertainty Σ w p i . For each voxel, we approximate it with a multivariate normal distribution that follows N(μ, Σ). The statistical information parameters are calculated as follows: S302. We perform matching according to the following strategy: if the point can establish a point-plane match with the corresponding voxel, a point-plane constraint is constructed. If there is no voxel containing a plane for matching, a point-to-distribution constraint is constructed based on the normal distribution estimation of the voxel. Here we focus on the construction of point distribution constraints. Assume that points predicted using prior position and orientation in the world coordinate system w p i After querying, no corresponding plane is found, and the voxel satisfies the multivariate normal distribution N(μ, Σ). We construct the constraint of the point to the distribution as follows: d i =( W p i -μ) T Σ -1 ( W p i -μ) and substitute the uncertainty of the LiDAR point w p i and the voxel average value μ into the above formula: In general, the first term should be as small as possible and close to zero. Thus, d i can be estimated as a normal distribution That is to say, if a point is located in the corresponding voxel, its corresponding d i should satisfy the above distribution 5. The tightly coupled lidar-inertial odometry method based on iterative error state Kalman filter according to claim 1, characterized in that The steps of step S4 are as follows: S401. The state estimation adopts the iterative error state Kalman filtering method. However, due to the simultaneous existence of point-to-plane and point-to-distribution constraints, our method will have two different observation functions. Assume there is a prior of state estimation and covariance We will update it through the constraints of point-to-plane distance and point-to-distribution, and obtain a maximum a posteriori estimation problem. The observation function obtained from the successful point-to-distribution matching at the i-th time is: Among them, we use subscript 1 to represent the quantities related to point-to-distribution matching, and subscript 2 to represent the quantities related to point-to-plane matching. is the residual representation of the point-to-point distribution obtained according to Equation (). v 1i is the LiDAR point L p 1i observation noise, and the voxel mean μ follows a (0, R 1i ) distribution. The corresponding derivation is as follows: S402. Similarly, we can obtain the observation equation and observation noise of the jth valid point-plane match, which are distinguished by subscript 2. Finally, combining the state prior and two types of valid observations (N and M respectively), we can obtain the MAP estimation: S403. Since there are two observation models, the measurement matrix H and the measurement noise covariance R are calculated as follows in the subsequent Kalman gain calculation: R = diag(R 11 ,..., R 1N , R 21 ,...R 2M ).

6. The tightly coupled lidar-inertial odometry method based on iterative error state Kalman filter according to claim 1, characterized in that The steps of step S5 are as follows: S501. When new lidar points from a scan arrive, the state estimation component is first used to calculate the positions of these points in the global world coordinate system and register these points in the global map. If the indexed voxels have not been constructed, they are built based on the newly added points and the corresponding parameters are calculated. Otherwise, we use an incremental update method to register these points with the constructed voxels. For statistical information, assuming that the mean and variance in the voxel are already available, denoted as μ H and Σ H , the mean and variance of the newly registered points are denoted as μ A and ∑ A , then the statistical information μ and Σ of the updated voxel are calculated as follows: S502. After the global map is updated, we discard all points in each voxel and only retain its plane parameters, statistical information, and its uncertainty. Then, we use a plane merging method based on union-find to merge coplanar surfaces in the updated global map.

Images