Dynamic Initialization for LiDAR-inertial SLAM

TL;DR

This work tackles the problem of initializing LiDAR-inertial SLAM when the platform is in motion, where traditional stationary initialization fails. It introduces D-LI-Init, a dynamic initialization framework that tightly couples LiDAR-gyroscope odometry (LGO) with IMU pre-integration via an error state iterated Kalman filter (ESIKF) to estimate the initial gyroscope bias, velocity, and gravity direction by iteratively aligning LiDAR odometry to IMU constraints. The method lever rotational distortion compensation from gyroscope data and translational distortion compensation during iterative updates, along with a degeneracy-detection mechanism, to ensure robust initialization across vehicles, handheld devices, and UAVs; it also offers open-source datasets and code. Experiments on public datasets and real-world tests demonstrate improved robustness and accuracy for dynamic initialization, with real-time performance suitable for urgent missions and restart scenarios in rescue or disaster-response contexts.

Abstract

The accuracy of the initial state, including initial velocity, gravity direction, and IMU biases, is critical for the initialization of LiDAR-inertial SLAM systems. Inaccurate initial values can reduce initialization speed or lead to failure. When the system faces urgent tasks, robust and fast initialization is required while the robot is moving, such as during the swift assessment of rescue environments after natural disasters, bomb disposal, and restarting LiDAR-inertial SLAM in rescue missions. However, existing initialization methods usually require the platform to remain stationary, which is ineffective when the robot is in motion. To address this issue, this paper introduces a robust and fast dynamic initialization method for LiDAR-inertial systems (D-LI-Init). This method iteratively aligns LiDAR-based odometry with IMU measurements to achieve system initialization. To enhance the reliability of the LiDAR odometry module, the LiDAR and gyroscope are tightly integrated within the ESIKF framework. The gyroscope compensates for rotational distortion in the point cloud. Translational distortion compensation occurs during the iterative update phase, resulting in the output of LiDAR-gyroscope odometry. The proposed method can initialize the system no matter the robot is moving or stationary. Experiments on public datasets and real-world environments demonstrate that the D-LI-Init algorithm can effectively serve various platforms, including vehicles, handheld devices, and UAVs. D-LI-Init completes dynamic initialization regardless of specific motion patterns. To benefit the research community, we have open-sourced our code and test datasets on GitHub.

Figures (6)

• Figure 1: Demonstration of real-world scenarios where the robot requires dynamic initialization, along with point cloud maps created using SOTA algorithms and ours.
• Figure 2: D-LI-Init overview.
• Figure 3: The effect of the motion distortion compensation method is demonstrated for a simple indoor environment.
• Figure 4: Trajectory generated by various algorithms for different datasets. LGO performs well, outperforming LO and approaching LIO. The subfigures depict the following: (a) - (h) trajectories of various algorithms in the NEW_quad, MCD_night_04, MCD_night_13, Gnd_MBLR_1, NEW_sloitter, NEW_math, MCD_day_02, UbanLoco_test, respectively.
• Figure 5: The error for the D-LI-Init method in estimating the gravity vector and initial gyroscope bias, where the error denotes the difference between the further refined gyroscope bias and gravity vector obtained by the LiDAR-inertial system (initialized using the D-LI-Init method) after completing dynamic initialization and the initial values estimated by the D-LI-Init method.