Long time anti-drift lidar-inertial odometry method and system

Abstract

This invention discloses a long-term anti-drift lidar-inertial odometry method and system. Addressing the long-term drift problem caused by IMU bias model mismatch and low-order discretization errors in existing lidar-inertial odometry systems, this invention constructs a time-varying error model based on a first-order Gaussian-Markov process, modeling the gyroscope and accelerometer bias as time-varying stochastic processes and augmenting them into state vectors; simultaneously, it constructs a discretized continuous motion model based on a second-order prediction-correction method using Lie groups, constraining the system state to an SO(3) manifold; embedding these two improvements into an iterative error state Kalman filter framework, utilizing lidar point cloud features to construct observation residuals, and achieving tight coupling fusion through iterative optimization and backpropagation. This invention effectively suppresses pose drift during long-term operation, improves self-sustaining positioning capabilities in high-dynamic and degraded scenarios, and does not affect real-time performance, making it suitable for robot autonomous navigation and positioning.

Description

Technical Field

[0001] This invention belongs to the field of robot navigation and positioning technology, specifically relating to an odometry method based on tight coupling of lidar and inertial measurement unit, which is particularly suitable for high-precision, drift-resistant pose estimation under long-term and complex working conditions. Background Technology

[0002] As autonomous systems move from the laboratory to large-scale, long-term, complex, and unstructured real-world scenarios, high-precision, robust, and drift-free autonomous positioning and state estimation have become core technological bottlenecks and essential requirements. LiDAR-Inertial Odometry (LIO) has become the mainstream positioning solution in GNSS failure scenarios due to its strong complementarity, good real-time performance, and high accuracy. Existing LIO systems can typically achieve centimeter-level accuracy in short periods, but they generally suffer from the core problems of pose drift accumulation and trajectory divergence after long-term operation. This deficiency directly leads to path planning failure and autonomous navigation interruption, severely restricting the engineering implementation and practical application of autonomous systems in long-term operations, large-scale inspections, and continuous mission scenarios. Therefore, existing technologies generally face the core bottlenecks of "rapid drift accumulation, insufficient long-term effectiveness, and weak robustness." This mainly stems from:

[0003] The fundamental flaw of inertial navigation: the integration mechanism of the inertial measurement unit (IMU) is the underlying cause of long-term drift in lidar-inertial odometry (IO). The IMU calculates attitude and position by integrating angular velocity and acceleration; however, minute measurement biases and noise amplify over time. These errors exhibit non-linear accumulation characteristics, causing position errors to diverge rapidly under pure inertial calculations. Even if short-term accuracy is controllable, long-term operation will result in significant attitude deviations, becoming the core source of drift.

[0004] Limitations of LiDAR observation: The limitations of lidar perception further amplify the drift problem caused by IMU modeling defects. In degraded scenes such as weak textures, long corridors, and open planes, lidar point cloud matching constraints fail, making it impossible to effectively correct inertial accumulation errors. Simultaneously, the lidar refresh rate is much lower than that of the IMU, making it prone to distortion and observation delays under high dynamic motion, and difficult to constrain IMU error growth throughout the entire process. Once external geometric observations fail, the system can only rely on inertial calculations with modeling defects, leading to a sharp increase in drift.

[0005] In recent years, tightly coupled LIO frameworks, represented by Fast-LIO2, have achieved high computational performance and environmental robustness through iterative error state Kalman filtering and backpropagation mechanisms, becoming the mainstream baseline solution in practical engineering applications. However, analysis of existing LIO systems based on Kalman filtering methods reveals two fundamental inherent limitations when facing long-term continuous tasks, leading to accumulated system pose drift and decreased positioning accuracy:

[0006] The IMU error model is oversimplified: the zero bias of the IMU gyroscope and accelerometer is simply modeled as a random constant driven by Gaussian white noise. However, under actual dynamic and complex conditions, the IMU zero bias exhibits significant time-varying, nonlinear, and time-dependent characteristics. This model mismatch leads to deviations between the filtered estimates and the actual physical processes, and the errors cannot be effectively compensated. Over time, these errors accumulate and transform into attitude and position drift.

[0007] Insufficient accuracy of discretization algorithm: To ensure real-time performance, a first-order Euler method is used to discretize and integrate the IMU's continuous dynamic equations. While this method has controllable errors for short-time integration, low-order discretization introduces inherent truncation errors during long-term state propagation. Especially during high-dynamic maneuvers (such as high-frequency rotation, abrupt acceleration and deceleration), the integration error amplifies exponentially over time. In such cases, the system becomes overly reliant on lidar observations for error correction. Once in a degraded scenario, external observations fail, drift intensifies dramatically, and ultimately, navigation fails.

[0008] Existing technical solutions mostly focus on improving the efficiency of laser point cloud matching or optimizing the filtering framework, but fail to improve the two root causes of drift: the accuracy of IMU dynamic modeling and the accuracy of continuous state discretization. Therefore, it is difficult to completely solve the pose drift problem under long-term operation. Summary of the Invention

[0009] To address the shortcomings of existing technologies, this invention provides a long-term drift-resistant lidar-inertial odometry method and system. While fully retaining the advantages of a tightly coupled architecture and efficient iterative Kalman filtering, this invention fundamentally suppresses long-term drift by improving the IMU time-varying error model and employing a high-precision discretization integration strategy. This significantly enhances the system's positioning stability and self-sustaining capability in long-term, high-dynamic, and degraded scenarios, while maintaining real-time performance, thus adapting to practical engineering application requirements.

[0010] According to one aspect of the present invention, a long-term drift-resistant lidar-inertial odometry method is provided, comprising: A time-varying error model for an inertial measurement unit (IMU) based on a first-order Gauss-Markov process is constructed. The time-varying error model models the gyroscope zero bias and accelerometer zero bias of the IMU as a first-order Gauss-Markov random process and augments the zero bias as part of the system state vector, so as to realize online estimation and compensation of time-varying error in the iterative error state Kalman filter framework. A second-order predictor-corrector discretized continuous motion model based on Lie groups is constructed. The discretized continuous motion model is used to solve the continuous-time dynamic model of the inertial measurement unit and the system state is constrained on the SO(3) manifold. The time-varying error model and the discretized continuous motion model are embedded into an iterative error state Kalman filter framework. The observation residual is constructed using the features of the lidar point cloud. Through iterative optimization and backpropagation, the tight coupling and fusion of the inertial measurement unit state and lidar observation are achieved, and the optimal pose estimate is output.

[0011] As a further technical solution, the embedding method of the time-varying error model in the iterative error state Kalman filter framework includes: In the continuous-time kinematic model, the differential equations for the gyroscope's zero bias and the accelerometer's zero bias are modeled as first-order Gaussian-Markov processes, respectively: , in , The correlation time constant is zero bias. , The random walk Gaussian noise is of zero bias. After discretizing the differential equation, the diagonal block corresponding to the zero bias is replaced with an expression containing the relevant time constant and sampling period in the state transition Jacobian matrix constructed using the second-order Adams-Bashforth-Moulton predictor-corrector algorithm.

[0012] As a further technical solution, the second-order Adams-Bashforth-Moulton predictor-corrector algorithm includes a predictor step and a corrector step;

[0013] In the state transition Jacobian matrix of the prediction step, the diagonal block corresponding to the zero bias is replaced with and ;

[0014] In the state transition Jacobian matrix of the correction step, the diagonal block corresponding to the zero bias is replaced with and ;in The sampling period of the inertial measurement unit. It is an identity matrix.

[0015] As a further technical solution, the second-order predictor-corrector discretized continuous motion model based on Lie groups employs Adams-Bashforth prediction and Adams-Moulton correction, and its joint recursive formula for state and covariance is as follows: Prediction step: , , Calibration step: , , in For generalized addition operators on manifolds, For the system kinematic function values ​​at step i and step i-1, Let i be the system state at step i. The sampling period of the inertial measurement unit. , , The Jacobian matrix is ​​derived based on the prediction-correction step. To predict the state, To utilize the function values ​​from the past two steps , The estimated value obtained by extrapolation Let be the covariance matrix of the inertial measurement unit process noise in step i. Let be the covariance of the i-th step, with superscripts P and C representing the prediction and correction, respectively.

[0016] As a further technical solution, the method of achieving tight coupling and fusion of inertial measurement unit state and lidar observation through iterative optimization and backpropagation includes: In each iteration, the following optimization problem is solved to update the state error. : , in , To predict the state and its covariance, For the point-to-surface iterative nearest point residual in the lidar point cloud, The Jacobian matrix of the measurement function with respect to the state error. To measure the noise covariance of a lidar system; After iterative convergence, the state and covariance of the inertial measurement unit are corrected through backpropagation.

[0017] As a further technical solution, the backpropagation includes: Based on the time-varying error model and the discretized continuous motion model, the inertial measurement unit is propagated forward to obtain the inertial measurement unit pose prediction value at all sampling times within a frame of lidar scanning cycle; using the pose prediction value, each laser point is back-projected from the coordinate system at its sampling time to the unified coordinate system at the end of the frame to compensate for lidar motion distortion.

[0018] As a further technical solution, the iterative optimization includes: In each iteration, the iterative nearest point residual and measurement Jacobian matrix of the point-to-surface in the laser point cloud are calculated based on the current state estimate. The state correction is solved and the state estimate is updated. The solution and update process is repeated until the magnitude of the state correction is less than a preset threshold.

[0019] According to one aspect of the present invention, a long-term drift-resistant lidar-inertial odometry system is provided, comprising: The time-varying error modeling module is used to construct a time-varying error model of the inertial measurement unit based on a first-order Gauss-Markov process. The time-varying error model models the gyroscope zero bias and accelerometer zero bias of the inertial measurement unit as a first-order Gauss-Markov random process and augments the zero bias as part of the system state vector, so as to realize online estimation and compensation of time-varying error in the iterative error state Kalman filter framework. A high-order discretization integration module is used to construct a second-order predictor-corrector discretized continuous motion model based on Lie groups. The discretized continuous motion model is used to discretize and solve the continuous-time dynamics model of the inertial measurement unit and constrain the system state on the SO(3) manifold. The tightly coupled iterative filtering module is used to embed the time-varying error model and the discretized continuous motion model into the iterative error state Kalman filter framework, construct the observation residual using the lidar point cloud features, and achieve tight coupling fusion of the inertial measurement unit state and lidar observation through iterative optimization and backpropagation to output the optimal pose estimate.

[0020] According to one aspect of the present invention, an electronic device is provided, comprising: An inertial measurement unit is used to acquire measurements of angular velocity and linear acceleration. LiDAR is used to collect environmental point cloud data; One or more processors; Memory, which stores computer program instructions; The instructions are executed by the processor to implement the method.

[0021] According to one aspect of the present invention, a computer-readable storage medium is provided that stores computer program instructions thereon, which, when executed by a processor, implement the method described thereon.

[0022] Compared with the prior art, the beneficial effects of the present invention are as follows: This invention employs a first-order Gaussian-Markov model to describe the IMU's zero bias, closely aligning with real-world physical laws and effectively addressing the model mismatch problem caused by the traditional Gaussian white noise assumption, thus reducing error accumulation at its source. Simultaneously, it utilizes a second-order predictor-corrector algorithm based on Lie groups, which, compared to the first-order Euler method, significantly reduces discrete truncation error, enhances long-term integration stability, and prevents exponential error amplification in high-dynamic scenarios. Through these dual improvements in model and algorithm, system drift during long-term continuous operation is effectively suppressed. In laser degradation scenarios, reliable positioning can be maintained through high-precision IMU propagation, achieving long-term self-sustainability without external observation. Furthermore, this method is fully compatible with tightly coupled and iterative filtering frameworks, introducing no significant computational overhead, and maintaining real-time performance while improving accuracy. Attached Figure Description

[0023] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the accompanying drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the accompanying drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0024] Figure 1 This is a flowchart illustrating a long-term drift-resistant lidar-inertial odometry method provided in an embodiment of the present invention.

[0025] Figure 2 This diagram illustrates trajectory estimation and error analysis on the Final_Challenge_UGV2 challenge sequence in the SbuT-MRS dataset, as provided in this embodiment of the invention. Figures (a) and (b) show the results of the proposed TDR-LIO algorithm, corresponding to the XY plane trajectory, the time-varying curves of errors in each axis, and the cumulative distribution function (CDF) of the errors, respectively. Figures (c) and (d) show the corresponding results of the baseline method Fast-LIO2, where the scale of the coordinate axes in (c) and (d) is 10. 6 m.

[0026] Figure 3 This is a schematic diagram of trajectory estimation and error analysis on the Final_Challenge_UGV3 challenge sequence of the SbuT-MRS dataset provided in an embodiment of the present invention. (a) and (b) in the figure show the results of the TDR-LIO algorithm proposed in this invention, which correspond to the XY plane trajectory, the curves of error change over time in each axis, and the cumulative distribution function (CDF) of the error, respectively. (c) and (d) are the corresponding results of the baseline method Fast-LIO2.

[0027] Figure 4 This diagram illustrates the trajectory comparison and quantitative evaluation results of a large-scale lidar-IMU dataset provided in this embodiment of the invention. (a)-(c) show the results of the TDR-LIO algorithm proposed in this invention; (d)-(f) show the results of the baseline algorithm Fast-LIO2. Specifically, (a) and (d) show the comparison between the estimated trajectory and the actual trajectory; (b) and (e) show the curves of X / Y / Z axis position errors over time; and (c) and (f) show the cumulative distribution function (CDF) of the overall position error. Detailed Implementation

[0028] This invention provides a long-term drift-resistant lidar-inertial odometry method. This method employs a tightly coupled architecture and iterative error state Kalman filtering, achieving drift resistance through improved IMU error modeling and discretized integration strategies. Specifically, it includes the following steps: S1. Constructing a time-varying error model for the IMU based on a first-order Gaussian-Markov process: Abandoning traditional constant or simple random walk assumptions, the zero bias of the IMU gyroscope and accelerometer is modeled as a first-order Gaussian-Markov stochastic process and extended as part of the system's state vector. This model accurately characterizes the physical properties of zero bias that are time-dependent, continuously smooth, and boundedly random. Based on this, the state propagation equation and observation update equation of this time-varying zero bias under the iterative error state Kalman filter framework are derived, achieving online optimal estimation and real-time compensation of the time-varying error.

[0029] S2. Constructing a second-order predictor-corrector discretized continuous motion model based on Lie groups: Abandoning the low-precision first-order Euler method, a second-order Adams-Bashforth-Moulton predictor-corrector algorithm based on Lie groups is adopted to discretize and solve the IMU continuous-time dynamics model. This strategy strictly constrains rotation, attitude, and other states to the Lie group manifold, and combined with continuous-time state representation, significantly reduces truncation errors during long-term integration, ensures the accuracy of state covariance propagation, and effectively suppresses the divergence of integration errors under high-dynamic motion.

[0030] S3. Tightly Coupled Iterative Filtering and State Update: The improved IMU time-varying error model and discretized continuous motion model are embedded into an iterative error state Kalman filter framework. The observation residuals are constructed using planar or edge features of the lidar point cloud. Through iterative optimization and backpropagation, tight coupling and fusion of the IMU state and lidar observations are achieved. Each iteration employs high-precision IMU state propagation to fully correct accumulated errors and ensure the global consistency of the system over long-term operation.

[0031] The terms “comprising” and “having”, and any variations thereof, in the specification, claims, and accompanying drawings of this invention are intended to cover a non-exclusive inclusion, such as a process, method, system, product, or apparatus that includes a series of steps or units, not necessarily limited to those explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0032] 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. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention. In addition, the technical features of the various embodiments or individual embodiments provided by the present invention can be arbitrarily combined to form new technical solutions. Such combinations are not bound by the order of steps and / or structural composition patterns, but must be based on the ability of those skilled in the art to implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection claimed by the present invention.

[0033] Figure 1 This is a simplified flowchart of the anti-drift method in the embodiments of the present invention, highlighting the innovations of the present invention in IMU time-varying error modeling and second-order discretized integration. This flowchart has been integrated into the overall system architecture flowchart. Figure 2 The system is constructed based on a tightly coupled and iterative filtering framework, which retains its core architectural advantages and makes targeted improvements, as shown in the complete functional and data flow diagram of the system.

[0034] The sensor input and front-end preprocessing unit serves as the system's data source entry point, divided into an IMU input link and a LiDAR input link, providing time-synchronized raw measurement data for subsequent state estimation. Its core function is to receive raw measurement data from the IMU gyroscope and accelerometer, perform forward propagation of the system state, provide high-frequency pose priors for laser point cloud motion distortion compensation, and simultaneously provide state predictions and covariance predictions for iterative Kalman filtering. Input parameters include: the optimal state estimate at the end of the previous laser scan frame. The corresponding covariance matrix This module also includes all raw IMU measurements within the current laser scan frame period. The core of this module is the propagation of the system's 24-dimensional state vector, defined as follows: , Where the state manifold , dimension ; , , These represent the attitude, position, and velocity of the inertial measurement unit in the global coordinate system, respectively. For IMU gyroscope zero bias, Zero bias for IMU accelerometer; This is the gravity vector in the global coordinate system (magnitude 9.81 m / s²). , These are the rotational and translational extrinsic parameters between the lidar and the inertial measurement unit.

[0035] The continuous-time kinematic model is as follows: , in, , These are Gaussian white noise from the gyroscope and accelerometer, respectively. , The correlation time constant is zero bias. , These are the random walk Gaussian noises of the gyroscope and accelerometer zero bias, respectively. This is the IMU angular velocity measurement value. These are IMU acceleration measurements. It is an antisymmetric matrix of vectors.

[0036] The differential equation for the zero-biased first-order Gauss-Markov process is: .

[0037] In step S2, the implementation process of the second-order Adams-Bashforth-Moulton predict-correction algorithm is as follows: the Adams-Bashforth algorithm is used for initial state prediction, and then the Adams-Moulton algorithm is used for state correction, forming a second-order accurate predict-correction loop. The forward propagation of the system state is based on the following discretized state transition model:

[0038] Prediction step: For and ,have , Calibration step: For ,have .

[0039] when When the Euler method is used as an approximation, that is... , in For generalized addition on a manifold, the kinematic function , For inertial measurement unit measurement input, For process noise, This is the sampling period for the inertial measurement unit.

[0040] In step S3, according to the IMU sampling period Discrete continuous kinematics model, performing forward propagation:

[0041] First, the initial values ​​and the state and covariance of step 1 are obtained using the Euler scheme: , in, and These are the last fusion (i.e., the first) The optimal state estimate and covariance obtained after scanning the lidar data (subject to multiple scans). The Jacobian matrix of the state transition function with respect to the state variables and the Jacobian matrix of the state transition function with respect to the process noise are shown below: ,

[0042] in For exponential mapping, It is SO (3) Li Qun's right Jacobi.

[0043] Secondly, starting from the third iteration, the Adams-Bashforth method is used for prediction: , in , covariance , In the above formula, .

[0044] The matrix is And matrix , , , in .

[0045] Correction was performed using the Adams-Moulton method: , in noise matrix , State matrix and The specific expression is as follows: , , in .

[0046] Iterative error state Kalman filtering minimizes the observation residuals through multiple iterations. , in To predict the covariance of the state, To measure the noise covariance of lidar, This represents the number of point clouds currently being scanned. Backpropagation is then used to correct the IMU state and covariance, ensuring global consistency during long-term system operation.

[0047] The distortion correction core transform formula, for the sampling time laser spot (In the LiDAR coordinate system), its coordinates in the global coordinate system are: .

[0048] It needs to be projected to the end of the frame. In the LiDAR coordinate system, the distortion-free points are obtained. ,satisfy: , Combining the two equations, we obtain the final formula for distortion correction: , In the formula This is the extrinsic transformation matrix of the LiDAR-IMU. The formula's function is to calculate the sampling time using the IMU's pose sequence. Until the end of the frame The sensors move relative to each other, and then the laser points are inversely transformed to eliminate the positional shift caused by the motion. Finally, all points within a frame are unified. In the LiDAR coordinate system at any given time, the point cloud is considered as sampled at the same time, thus resolving the motion distortion problem. The output is: the original laser point cloud of a single frame after distortion removal. ( The number of point clouds in a single frame is directly input into the state estimation stage.

[0049] The state estimation is based on an Iterative Extended Kalman Filter (IEKF) framework on a manifold. It iterative optimization is used to find the optimal estimate of the system state, ultimately outputting the odometry result and providing the optimized pose for the mapping module. The state estimation stage takes the forward propagation as input and outputs the state prior. Covariance Prior And the distortion-free original laser point cloud output by backpropagation. .

[0050] The system measurement model is based on the point-to-area (ICP) assumption: after correct pose transformation, the actual laser point should fall on the corresponding local plane in the map. Laser point measurement model: , In the formula These are the actual coordinates of the laser point. This refers to the ranging and beam direction noise of LiDAR, which follows a Gaussian distribution. Global point-to-surface constraint equations: , in, In the state vector The first in The transformation matrix of the IMU relative to the global coordinate system at the end of the frame. It is the unit normal vector of the local plane corresponding to that point on the map. It is the centroid point on the local plane. After the laser point is projected onto the global coordinate system, its directed distance to the local plane is 0; this distance is the measurement residual. Define the measurement function. , The measurement model is Its state estimate in the current iteration. ( Performing a first-order Taylor expansion at (the iteration number) yields: , Among them, residual term It is the residual of point-to-surface ICP. It is a measurement function For state error Jacobian matrix, It is the state error of the current iteration. This is the noise measurement item.

[0051] The iterative update stage is based on the Kalman filter framework, fusing the state prior and laser measurement residuals to solve for the maximum a posteriori estimate of the state. Through multiple iterations, the linearization error of the Taylor expansion is eliminated, improving the pose estimation accuracy. The Gaussian distribution constraint of the state prior is: .

[0052] Incorporating the state error of the current iteration, a Jacobian matrix on the manifold is introduced. Transform the prior covariance to the linearization point of the current iteration: , in It is the Jacobian matrix of the state error on the manifold, used for processing The nonlinear nature of rotation ensures the consistency of filtering across the manifold. Maximum a posteriori estimation optimization objective: , in, Covariance. The above formula represents the search for the optimal state error correction amount that minimizes the sum of the Mahalanobis distance of the state prior and the Mahalanobis distance of the measurement residuals, i.e., balancing the prior information of IMU predictions with the measurement information of LiDAR registration. The closed-form solution to this optimization problem is given by iterative Kalman filtering: .

[0053] Kalman gain The dimension of the inverse matrix is ​​only This greatly reduces the computational load, and state updates are achieved. The operator operates on the manifold, ensuring the orthogonality of the rotation matrix and eliminating the need for additional normalization operations. Furthermore, the linearization points are continuously updated during the iteration process, reducing the linearization error of the nonlinear system and improving the estimation accuracy in large motion scenarios.

[0054] In the convergence judgment and state update phase, the logic of the convergence judgment is to determine whether the change in state before and after the iteration is less than a set threshold. ,Right now If the convergence condition is not met, return to the residual calculation stage, use the new state estimate as the linearization point, recalculate the residuals and Jacobian, and execute the next iteration; if the convergence condition is met, execute the final state update. State and covariance updates after convergence: , in , For the first The final optimal state estimate and covariance matrix of the frame are used as the initial values ​​for forward propagation in the next frame. The final output of this module is the IMU pose in the optimal state estimate. , This refers to the robot's real-time 6-DOF pose; optimal state estimation. Covariance The input is fed into the forward propagation module of the next frame, forming a filtering closed loop on the time series.

[0055] Figure 2 This paper presents quantitative comparison results between the proposed algorithm TDR-LIO and the current industry-leading baseline algorithm Fast-LIO2. In long-range navigation tasks, suppressing cumulative drift and maintaining global consistency are core criteria for evaluating the performance of SLAM systems. Figure 2As shown in the XY plane trajectory in (a), TDR-LIO maintained a high degree of consistency with the real trajectory throughout the entire runtime. Despite slight local shifts in the extremely challenging sections towards the end of the sequence, the system successfully suppressed unbounded drift and maintained the topological correctness of the trajectory. In stark contrast, the baseline algorithm Fast-LIO2 exhibited severe drift on this sequence. Figure 2 As shown in (c), the estimated trajectory deviates completely from the actual motion constraints, with a drift of an astonishing 10⁶ meters. This indicates that Fast-LIO2 suffers from severe odometry degradation and failure when facing unstructured environments or feature-scarce regions in the later stages of the sequence, making it unable to complete the mapping task for the entire sequence. By observing the trend of the change of coordinate errors of each axis over time ( Figure 2 As shown in (b) and (d) in the diagram, TDR-LIO exhibits extremely strong error constraint capabilities. During its 3400-second operation, TDR-LIO strictly controlled the absolute translation error of each axis within ±2 meters for the vast majority of the time, demonstrating its high robustness and positioning accuracy during long-duration navigation exceeding 3000 seconds. Even in extremely challenging road conditions after 3000 seconds (where the Y-axis error fluctuated to a maximum of approximately 10 meters), the system still demonstrated excellent resilience and did not experience systemic failure. In contrast, Fast-LIO2 ( Figure 2 In (d) of the algorithm, the XY axis error exhibits an exponential explosion after approximately 3000 seconds, directly causing the positioning system to fail. Furthermore, the cumulative error distribution (CDF) further highlights the absolute advantage of the algorithm presented in this paper from a statistical perspective.

[0056] Figure 3 This presents the comprehensive evaluation results on another challenging sequence in the SuMa-MRS dataset, "Final_Challenge_UGV3". From... Figure 3 As can be seen from the global XY plane trajectory in (a), despite the complex motion pattern and drastic directional changes, the TDR-LIO algorithm proposed in this invention still perfectly maintains the precision of the trajectory structure. The estimated trajectory closely matches the true reference trajectory, without any obvious closed-loop misalignment or long-term divergence. In contrast, the baseline algorithm Fast-LIO2 fails again in this sequence (e.g., Figure 3 As shown in (c)). A deeper analysis of the error evolution over time reveals their distinctly different dynamic stability. The timing error curves of the TDR-LIO output ( Figure 3(b) of the above demonstrates extremely excellent error boundary convergence: during a task lasting nearly half an hour, the errors of the X and Y axes were consistently and strictly limited to an extremely narrow range of ±2 meters; the Z-axis error, which is crucial for 3D mapping, also remained extremely stable near the zero reference line, without any significant cumulative drift. In contrast, Fast-LIO2 ( Figure 3 In (d) of the sequence, the system suffered a fatal failure after about 1000 seconds, with the Y-axis error deteriorating rapidly, exceeding an alarming 2 × 10⁻⁶ at the end of the sequence. 6 On the order of meters. From a statistical perspective, the cumulative probability distribution (CDF) curve provides the most convincing overall performance assessment.

[0057] This invention conducted further comparative experiments on sequences of a large-scale lidar-IMU dataset, and the results are as follows: Figure 4 As shown. From Figure 4 The comparison of the global XY trajectories in (a) and (d) clearly shows that although neither system exhibits the catastrophic divergence common in challenging degradation scenarios, their ability to suppress trajectory drift differs significantly. The baseline algorithm, Fast-LIO2 (… Figure 4 The estimated trajectory (d) in the figure exhibits extremely severe drift as the running distance increases, especially in the complex turnaround area in the lower right corner of the figure, where the red estimated trajectory has significantly deviated from the blue true trajectory. In contrast, the TDR-LIO algorithm proposed in this paper ( Figure 4 (a) in the model exhibits excellent trajectory fidelity, with its overall trajectory highly consistent with reality. Even at the end of long-distance exploration, the trajectory deviation is controlled within a very small visually perceptible range. From the perspective of temporal error, such as... Figure 4 As shown in (e), shortly after Fast-LIO2 started, its Y-axis error rapidly climbed to nearly 50 meters and then went out of control again at the end of the sequence. The three-axis error fluctuations were drastic, reflecting that traditional point cloud registration strategies are prone to error accumulation during long-term operation. In contrast, TDR-LIO successfully suppressed the divergence trend of the error. Figure 4 (b)). Within the same operating time window, the error fluctuation range of each axis of the TDR-LIO was significantly compressed, the early high-amplitude error peaks were completely eliminated, and the Z-axis (height) error remained almost consistently stable near the zero line throughout the entire operating cycle. Figure 4As shown in (f), Fast-LIO2's median operational error (50th percentile) is as high as 24.924 meters, and its maximum deviation at 99% confidence level is close to 65 meters (64.353 meters), which is unacceptable in demanding autonomous driving or high-precision map building tasks. In stark contrast, TDR-LIO's median position error is only 12.779 meters, nearly 50% lower than Fast-LIO2; its extreme drift (99th percentile of 37.755 meters) is also reduced by almost half (e.g., Figure 4 (as shown in (c)).

[0058] Based on the same inventive concept as the foregoing method embodiments, this embodiment of the invention also provides a long-term drift-resistant lidar-inertial odometry system for performing the steps in the above method embodiments. The system includes:

[0059] The time-varying error modeling module is used to construct a time-varying error model for the inertial measurement unit (IMU) based on a first-order Gaussian-Markov process. This model models the gyroscope and accelerometer zero biases of the IMU as first-order Gaussian-Markov stochastic processes, and augments these zero biases as part of the system state vector. The module introduces a relevant time constant as a model parameter to characterize the time correlation and smooth, bounded stochastic properties of the gyroscope and accelerometer zero biases. In the discretized state transition Jacobian matrix, the module replaces the diagonal block corresponding to the zero bias with an expression containing the relevant time constant and the sampling period, thereby achieving online estimation and compensation of the time-varying zero bias within the Kalman filter framework.

[0060] A high-order discretization integration module is used to construct a second-order predictor-corrector discretized continuous motion model based on Lie groups. This model employs Adams-Bashforth prediction and Adams-Moulton correction to discretize the continuous-time dynamics model of the inertial measurement unit (IMU) and constrains the system state to the SO(3) manifold. Specifically, in the initial sampling stage of the IMU, the module starts with the Euler method; in subsequent stages, the Adams-Bashforth method is first executed for state prediction, followed by the Adams-Moulton method for state correction, and the state recursion is completed on the manifold using a generalized addition operator. This module simultaneously performs joint recursion of state and covariance, ensuring the accuracy and stability of long-term integration.

[0061] A tightly coupled iterative filtering module is used to embed the time-varying error model and the discretized continuous motion model into an iterative error state Kalman filter framework. This module utilizes planar or edge features in the lidar point cloud to construct observation residuals (e.g., point-to-surface iterative nearest point residuals), and achieves tight coupling fusion of the inertial measurement unit state and lidar observations through iterative optimization and backpropagation, outputting the optimal pose estimate.

[0062] The system state vector maintained by the system is defined on the direct product manifold of SO(3) and the real space, specifically including: the attitude, position, and velocity of the inertial measurement unit in the global coordinate system, the gyroscope bias, the accelerometer bias, the gravity vector, and the rotational and translational extrinsic parameters between the lidar and the inertial measurement unit.

[0063] Based on the same inventive concept as the foregoing method embodiments, this invention also provides an electronic device, which can be a robot, an unmanned vehicle, a handheld surveying instrument, or any mobile platform requiring real-time positioning and mapping. The electronic device includes: An inertial measurement unit is used to acquire raw measurements of triaxial angular velocity and triaxial acceleration. LiDAR is used to collect point cloud data of the surrounding environment; One or more processors; The memory stores computer program instructions; when executed by the processor, the instructions implement the long-term anti-drift lidar-inertial odometry method described in any of the above method embodiments.

[0064] During operation, the inertial measurement unit (IMU) and lidar acquire data at their respective frequencies. The processor reads program instructions from memory and executes them according to the steps described above: constructing a time-varying error model based on a first-order Gaussian-Markov process, constructing a second-order prediction-correction discretized continuous motion model based on a Lie group, embedding the two improvements into an iterative error state Kalman filter framework, constructing the observation residual using lidar point cloud features, and outputting the optimal pose estimate through iterative optimization and backpropagation. This electronic device can maintain low pose drift during long-term continuous operation and maintain reliable positioning through high-precision IMU propagation in lidar degradation scenarios (such as long corridors and open planes).

[0065] Based on the same inventive concept as the foregoing method embodiments, this invention also provides a computer-readable storage medium storing computer program instructions thereon. The computer-readable storage medium may be a read-only memory (ROM), random access memory (RAM), USB flash drive, portable hard drive, optical disc (CD-ROM), magnetic disk, or various semiconductor memories, etc., which are non-transitory storage media. When the instructions are executed by a processor, they implement the long-term drift-resistant lidar-inertial odometry method described in any of the above method embodiments.

[0066] When the processor executes the instructions, it can perform the following steps: constructing a time-varying error model of the inertial measurement unit based on a first-order Gaussian-Markov process; constructing a second-order predictor-corrector discretized continuous motion model based on a Lie group; embedding two improved iterative error state Kalman filter frameworks into Fast-LIO2; constructing observation residuals using lidar point cloud features; achieving tight coupling fusion of the inertial measurement unit state and lidar observations through iterative optimization and backpropagation; and outputting the optimal pose estimate. By executing the instructions, the processor can effectively suppress pose drift during long-term operation, improving the positioning accuracy and robustness of the system in complex dynamic environments.

[0067] In summary, this invention belongs to the field of autonomous navigation and state estimation technology, and discloses a lidar-inertial odometry method for long-term anti-drift, aiming to solve the problems of IMU model mismatch, pose drift accumulation due to insufficient discretization accuracy, and weak self-sustainability in degraded scenarios that exist in existing LIO frameworks during long-term operation. This invention retains the core advantages of the Fast-LIO2 tightly coupled architecture and iterative error state Kalman filtering, and accurately characterizes the time-dependent characteristics of IMU zero bias by constructing an IMU time-varying error model based on a first-order Gaussian-Markov process. Simultaneously, a second-order predictor-corrector Adams-Bashforth-Moulton algorithm on a Lie group is used to replace the traditional first-order Euler method, reducing the truncation error of long-term integration. Finally, the improved model and discretization method are embedded into a tightly coupled iterative filtering framework to achieve efficient fusion of IMU state and lidar observation, improving the system's anti-drift capability, adaptability to high-dynamic scenarios, and self-sustainability in degraded scenarios during long-term operation.

[0068] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features therein; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the technical solutions of the embodiments of the present invention.

Claims

1. A long-term drift-resistant lidar-inertial odometry method, characterized in that, include: A time-varying error model for an inertial measurement unit (IMU) based on a first-order Gauss-Markov process is constructed. The time-varying error model models the gyroscope zero bias and accelerometer zero bias of the IMU as a first-order Gauss-Markov random process and augments the zero bias as part of the system state vector, so as to realize online estimation and compensation of time-varying error in the iterative error state Kalman filter framework. A second-order predictor-corrector discretized continuous motion model based on Lie groups is constructed. The discretized continuous motion model is used to solve the continuous-time dynamic model of the inertial measurement unit and the system state is constrained on the SO(3) manifold. The time-varying error model and the discretized continuous motion model are embedded into an iterative error state Kalman filter framework. The observation residual is constructed using the features of the lidar point cloud. Through iterative optimization and backpropagation, the tight coupling and fusion of the inertial measurement unit state and lidar observation are achieved, and the optimal pose estimate is output.

2. The long-term drift-resistant lidar-inertial odometry method according to claim 1, characterized in that, The embedding methods of the time-varying error model in the iterative error state Kalman filter framework include: In the continuous-time kinematic model, the differential equations for the gyroscope's zero bias and the accelerometer's zero bias are modeled as first-order Gaussian-Markov processes, respectively: ， in , The correlation time constant is zero bias. , The random walk Gaussian noise is of zero bias. After discretizing the differential equation, the diagonal block corresponding to the zero bias is replaced with an expression containing the relevant time constant and sampling period in the state transition Jacobian matrix constructed using the second-order Adams-Bashforth-Moulton predictor-corrector algorithm.

3. The long-term drift-resistant lidar-inertial odometry method according to claim 2, characterized in that, The second-order Adams-Bashforth-Moulton predictor-corrector algorithm includes a predictor step and a correction step. In the state transition Jacobian matrix of the prediction step, the diagonal block corresponding to the zero bias is replaced with and ; In the state transition Jacobian matrix of the correction step, the diagonal block corresponding to the zero bias is replaced with and ;in The sampling period of the inertial measurement unit. It is an identity matrix.

4. The long-term drift-resistant lidar-inertial odometry method according to claim 1, characterized in that, The second-order predictor-corrector discretized continuous motion model based on Lie groups employs Adams-Bashforth prediction and Adams-Moulton correction, and its joint recursive formula for state and covariance is as follows: Prediction step: , ， Calibration step: , ， in For generalized addition operators on manifolds, The system kinematic function values ​​are for the i-th step and the (i-1)-th step. Let i be the system state at step i. The sampling period of the inertial measurement unit. , , The Jacobian matrix is ​​derived based on the prediction-correction step. To predict the state, To utilize the function values ​​from the past two steps , The estimated value obtained by extrapolation Let be the covariance matrix of the inertial measurement unit process noise in step i. Let be the covariance of the i-th step, with superscripts P and C representing the prediction and correction, respectively.

5. The long-term drift-resistant lidar-inertial odometry method according to claim 1, characterized in that, The method of achieving tight coupling and fusion of inertial measurement unit state and lidar observation through iterative optimization and backpropagation includes: In each iteration, the following optimization problem is solved to update the state error. : ， in , To predict the state and its covariance, For the point-to-surface iterative nearest point residual in the lidar point cloud, The Jacobian matrix of the measurement function with respect to the state error. To measure the noise covariance of a lidar system; After iterative convergence, the state and covariance of the inertial measurement unit are corrected through backpropagation.

6. The long-term drift-resistant lidar-inertial odometry method according to claim 1, characterized in that, The back propagation includes: Based on the time-varying error model and the discretized continuous motion model, the inertial measurement unit is propagated forward to obtain the inertial measurement unit pose prediction value at all sampling times within a frame of lidar scanning cycle; using the pose prediction value, each laser point is back-projected from the coordinate system at its sampling time to the unified coordinate system at the end of the frame to compensate for lidar motion distortion.

7. The long-term drift-resistant lidar-inertial odometry method according to claim 1, characterized in that, The iterative optimization includes: In each iteration, the iterative nearest point residual and measurement Jacobian matrix of the point-to-surface in the laser point cloud are calculated based on the current state estimate. The state correction is solved and the state estimate is updated. The solution and update process is repeated until the magnitude of the state correction is less than a preset threshold.

8. A long-term drift-resistant lidar-inertial odometry system, characterized in that, include: The time-varying error modeling module is used to construct a time-varying error model of the inertial measurement unit based on a first-order Gauss-Markov process. The time-varying error model models the gyroscope zero bias and accelerometer zero bias of the inertial measurement unit as a first-order Gauss-Markov random process and augments the zero bias as part of the system state vector, so as to realize online estimation and compensation of time-varying error in the iterative error state Kalman filter framework. A high-order discretization integration module is used to construct a second-order predictor-corrector discretized continuous motion model based on Lie groups. The discretized continuous motion model is used to discretize and solve the continuous-time dynamics model of the inertial measurement unit and constrain the system state on the SO(3) manifold. The tightly coupled iterative filtering module is used to embed the time-varying error model and the discretized continuous motion model into the iterative error state Kalman filter framework, construct the observation residual using the lidar point cloud features, and achieve tight coupling fusion of the inertial measurement unit state and lidar observation through iterative optimization and backpropagation to output the optimal pose estimate.

9. An electronic device, characterized in that, include: An inertial measurement unit is used to acquire measurements of angular velocity and linear acceleration. LiDAR is used to collect environmental point cloud data; One or more processors; Memory, which stores computer program instructions; When the instructions are executed by the processor, they implement the method as described in any one of claims 1 to 7.

10. A computer-readable storage medium having computer program instructions stored thereon, characterized in that, When the instructions are executed by the processor, they implement the method as described in any one of claims 1 to 7.

Images