Tightly Coupled 3D Lidar Inertial Odometry and Mapping

Haoyang Ye, Yuying Chen, Ming Liu

Abstract

Ego-motion estimation is a fundamental requirement for most mobile robotic applications. By sensor fusion, we can compensate the deficiencies of stand-alone sensors and provide more reliable estimations. We introduce a tightly coupled lidar-IMU fusion method in this paper. By jointly minimizing the cost derived from lidar and IMU measurements, the lidar-IMU odometry (LIO) can perform well with acceptable drift after long-term experiment, even in challenging cases where the lidar measurements can be degraded. Besides, to obtain more reliable estimations of the lidar poses, a rotation-constrained refinement algorithm (LIO-mapping) is proposed to further align the lidar poses with the global map. The experiment results demonstrate that the proposed method can estimate the poses of the sensor pair at the IMU update rate with high precision, even under fast motion conditions or with insufficient features.

Introduction

Reliable ego-motion estimation—the ability of a robot to track its position and movement—is critical for autonomous navigation. While 3D Lidar is a cornerstone sensor for this task, it faces significant challenges: motion distortion caused by the sensor's scanning mechanism, low update rates (typically 10Hz), and failure in feature-poor environments like long, uniform corridors.

Traditional "loosely coupled" methods process Lidar and Inertial Measurement Unit (IMU) data independently, often failing to exploit the full synergy between the two. This paper presents a tightly coupled approach that jointly optimizes raw Lidar and IMU measurements, providing high-frequency, robust pose estimation even in demanding scenarios.

Methodology: Tightly Coupled Fusion

The proposed framework consists of two primary components: Lidar-IMU Odometry (LIO) and LIO-Mapping.

Lidar-IMU Odometry (LIO)

LIO serves as the high-frequency state estimator. It employs a sliding window (fixed-lag smoother) to optimize the robot's state, including position, velocity, orientation, and sensor biases. To synchronize the sensors, IMU pre-integration is used to consolidate high-frequency IMU data into constraints that align with Lidar frames. A key finding of this work is the reliability of planar features over edge features in 3D Lidar-IMU fusion; consequently, the system prioritizes plane-matching to simplify optimization and enhance robustness.

LIO-Mapping

To mitigate long-term drift, the LIO-Mapping thread aligns local odometry with a global map. A significant innovation here is rotation-constrained refinement. Since IMUs are highly accurate at sensing gravity but prone to yaw drift, the mapping process uses the IMU-derived orientation as a constraint. This ensures the global map remains consistent and "upright" over time.

System architecture of the proposed tightly coupled Lidar-Inertial Odometry and mapping framework.

Key Insights

  • Synergistic Optimization: By solving Lidar and IMU constraints together, the IMU provides essential motion cues during rapid turns or in feature-sparse areas, while the Lidar continuously corrects the IMU's inherent drift.
  • Planar Feature Prioritization: Focusing on planar constraints rather than noisier edge features leads to a more stable and efficient optimization process.
  • High-Frequency Feedback: The system outputs pose estimates at the IMU's update rate (e.g., 400Hz), enabling precise real-time control for high-speed robotic platforms.

Experimental Results

The system was rigorously tested using custom indoor/outdoor datasets (validated by motion capture), the KAIST Urban Dataset, and challenging hand-held/golf cart sequences.

The results demonstrate that the proposed method consistently outperforms state-of-the-art Lidar-only methods like LOAM. In high-speed tests, LIO-mapping achieved a translation Root Mean Square Error (RMSE) of approximately 0.05m, where Lidar-only methods frequently failed.

Comparison of translational and rotational errors across different methods, demonstrating the superior accuracy of LIO-mapping.

Conclusion

This tightly coupled 3D Lidar-Inertial Odometry and mapping framework provides a robust solution for ego-motion estimation. While it requires an initial motion phase to calibrate IMU states and carries a higher computational load than loosely coupled filters, its ability to maintain accuracy during rapid motion and in degraded environments makes it a powerful tool for autonomous systems.

Summary updated

Abstract

Ego-motion estimation is a fundamental requirement for most mobile robotic applications. By sensor fusion, we can compensate the deficiencies of stand-alone sensors and provide more reliable estimations. We introduce a tightly coupled lidar-IMU fusion method in this paper. By jointly minimizing the cost derived from lidar and IMU measurements, the lidar-IMU odometry (LIO) can perform well with acceptable drift after long-term experiment, even in challenging cases where the lidar measurements can be degraded. Besides, to obtain more reliable estimations of the lidar poses, a rotation-constrained refinement algorithm (LIO-mapping) is proposed to further align the lidar poses with the global map. The experiment results demonstrate that the proposed method can estimate the poses of the sensor pair at the IMU update rate with high precision, even under fast motion conditions or with insufficient features.
BibTeX
@inproceedings{ye2019tightly,
  author = {Haoyang Ye and Yuying Chen and Ming Liu},
  title = {{Tightly Coupled 3D Lidar Inertial Odometry and Mapping}},
  booktitle = {IEEE International Conference on Robotics and Automation},
  year = {2019},
  url = {https://openreview.net/forum?id=OIcB6LjsmQ},
}
Figure