Tightly Coupled 3D Lidar Inertial Odometry and Mapping

Haoyang Ye, Yuying Chen, Ming Liu

TL;DR

This paper introduces a robust, tightly coupled 3D Lidar-Inertial Odometry (LIO) and mapping framework. By jointly optimizing raw Lidar and IMU measurements in a single framework, the system achieves high-frequency pose estimation that remains accurate even during rapid motion or in "Lidar-degraded" environments like featureless corridors.

What Problem Does This Paper Address?

Reliable ego-motion estimation (knowing where a robot is and how it's moving) is the "bread and butter" of autonomous systems. While 3D Lidar is a powerful tool for this, it has three major Achilles' heels:

  1. Motion Distortion: Because a Lidar spins to collect data, the robot's own movement "smears" the point cloud.
  2. Low Update Rates: Lidar typically operates at 10Hz, which is too slow for high-speed robot control.
  3. Degraded Environments: In places like long, smooth corridors, the Lidar sees very few unique features, causing the localization to "slip."

Previous "loosely coupled" methods treated Lidar and IMU as separate black boxes, missing out on the deeper synergies of the data. This paper moves toward a tightly coupled approach, where the raw constraints from both sensors are solved together to overcome these individual weaknesses.

Methods

The authors propose a two-tiered system:

  • LIO (Lidar-IMU Odometry): This is the high-frequency component. It uses a sliding window (fixed-lag smoother) to optimize the robot's state (position, velocity, orientation, and sensor biases).
  • Pre-integration: They use IMU pre-integration to "clump" high-frequency IMU data into constraints that match the Lidar's timing.
  • Feature Selection: Interestingly, the authors found that planar features (flat surfaces) were much more reliable for this fusion than edge features, so they prioritize plane-matching.
  • LIO-Mapping (Refinement): To prevent long-term drift, a second thread aligns the local odometry with a global map.
  • Rotation-Constrained Refinement: This is a key innovation. Since the IMU is very good at sensing the direction of gravity but drifts in "yaw," the mapping process uses the IMU's orientation as a constraint. This ensures the global map stays "upright" and doesn't tilt over time.

Datasets

The system was evaluated using:

  • Custom Indoor/Outdoor Data: Collected using a Velodyne VLP-16 Lidar and an Xsens MTi-100 IMU. Quantitative ground truth was provided by a high-precision Motion Capture system.
  • KAIST Urban Dataset: A well-known public benchmark for large-scale autonomous driving scenarios.
  • Challenging Scenarios: Hand-held tests and golf cart runs designed to induce rapid motion and feature-poor environments.

Key Insights and Ideas

  • The Power of Tight Coupling: By optimizing Lidar and IMU together, the IMU provides the "glue" during fast turns or in corridors where Lidar features are sparse, while the Lidar constantly corrects the IMU's inherent drift.
  • Planar over Edges: In 3D Lidar-IMU fusion, the authors observed that edge features often add noise without significant gain in accuracy. Focusing on planar constraints simplified the optimization and improved robustness.
  • Gravity as an Anchor: Using the IMU to constrain the "roll" and "pitch" of the global map (rotation-constrained mapping) is a highly effective way to maintain global consistency over long distances.
  • IMU-Rate Feedback: Because the IMU states are optimized, the system can output pose estimates at the IMU's frequency (e.g., 400Hz), which is a massive boon for real-time robot control.

Key Results

  • Superior Accuracy: The proposed method consistently outperformed LOAM (a state-of-the-art Lidar-only method) and loosely coupled alternatives, especially in translational error during fast motion.
  • Robustness to Speed: In "fast" test sequences, LIO-mapping achieved a translation RMSE of ~0.05m, whereas Lidar-only methods often struggled or failed to track.
  • Efficiency: The odometry thread runs in ~128ms and the mapping thread in ~108ms on a standard desktop CPU. Because these run in parallel, the system maintains real-time performance.
  • Open Source: At the time of publication, this was the first open-source implementation of tightly coupled 3D Lidar-IMU fusion available to the community.

Limitations and Future Work

  • Initialization: Like most IMU-based systems, this method requires an initialization phase where the robot moves sufficiently to "awaken" the IMU states (making biases and gravity observable).
  • Environment Assumptions: While robust, the reliance on planar features means the system might perform differently in purely organic, unstructured environments (like a dense forest) compared to man-made urban environments.
  • Computational Load: Tightly coupled optimization is more computationally expensive than filtering or loosely coupled methods, though the authors' use of a sliding window successfully manages this for real-time use.

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 lidarIMU odometry (LIO) can perform well with considerable drifts after long-term experiment, even in challenging cases where the lidar measurement 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},
}

Loading related content...

Figure