LOCUS: A Multi-Sensor Lidar-Centric Solution for High-Precision Odometry and 3D Mapping in Real-Time

Matteo Palieri, Benjamin Morrell, Abhishek Thakur, Kamak Ebadi, Jeremy Nash, Arghya Chatterjee, Christoforos Kanellakis, Luca Carlone, Cataldo Guaragnella, Ali-akbar Agha-mohammadi

TL;DR

This paper introduces LOCUS, a robust and high-precision lidar-centric odometry system designed for challenging, GPS-denied environments. It achieves this through a multi-stage scan matching approach with a health-aware sensor integration module that allows for seamless fusion of additional sensors, making it resilient to sensor failures.

What Problem Does This Paper Address?

Autonomous robots are increasingly being deployed in complex and unforgiving environments, such as mines, disaster sites, and industrial facilities. In these scenarios, robots face significant challenges for accurate navigation and mapping. These include poor lighting conditions, obscurants like dust and smoke, featureless or self-similar terrains, and slippery surfaces that can lead to jerky robot movements. A fundamental requirement for robots to operate autonomously in such conditions is a reliable source of odometry – knowing precisely where the robot is and how it's moving. Traditional odometry systems often struggle in these "perceptually challenging" environments. While fusing data from multiple sensors (like lidar and IMUs) can improve robustness, these systems can still be compromised if one of the sensors fails. This paper aims to address this by developing a lidar-centric odometry system that is not only accurate and fast but also highly resilient to sensor failures.

Methods

LOCUS employs a multi-stage approach to achieve its goals:

  • Point Cloud Preprocessing: Raw lidar data is first processed to correct for motion distortion during scan acquisition. This is crucial for accurate point cloud alignment, especially with high-rate robot rotations. The system also includes a point cloud merger for robots with multiple lidars and a filter (voxel grid and random downsampling) to reduce noise and data volume while preserving structural information.
  • Scan Matching Unit: This core component performs two main tasks:
  • Scan-to-Scan Matching: It uses a Generalized Iterative Closest Point (GICP) algorithm to estimate the robot's motion between consecutive lidar scans.
  • Scan-to-Submap Matching: The scan-to-scan estimate is further refined by matching the current scan against a local submap built from previous scans, ensuring global consistency.
  • Health-Aware Sensor Integration: This is a key innovation. LOCUS uses a "loosely-coupled" approach to integrate data from other sensors (like VIO, KIO, or IMU). A health monitor assesses the reliability of these input sources. If a primary sensor fails, LOCUS can seamlessly switch to a secondary sensor or even rely solely on lidar odometry, preventing a complete loss of navigation capability. This "fail-safe" mechanism is crucial for extreme environments.
  • Environment Adaptation (Flat Ground Assumption - FGA): The system can optionally leverage prior knowledge of flat ground in the environment to further constrain the odometry estimates, reducing drift in the vertical (Z) axis, roll, and pitch. This can be activated based on context or detected by an IMU.

Datasets

The paper evaluates LOCUS using datasets collected from the DARPA Subterranean Challenge (SubT Challenge), specifically from the Urban and Tunnel circuits. These datasets are characterized by challenging perceptual conditions. The authors also mention using data from a Clearpath Husky ground rover and a Boston Dynamics Spot robot. No new benchmarks were introduced; rather, existing challenging real-world datasets were utilized.

Key Insights and Ideas

  • The power of "loosely-coupled" sensor fusion for robustness: Unlike tightly-coupled systems that can fail catastrophically if one sensor is lost, LOCUS's health-aware, loosely-coupled integration allows it to gracefully degrade or switch sensor inputs, maintaining operational capability.
  • Health monitoring is critical for real-world autonomy: In unpredictable environments, sensor failures are not exceptions but likely occurrences. A system that can adapt to these failures is essential for reliable operation.
  • Lidar remains a strong foundation, but needs augmentation: While lidar provides rich 3D data, its performance can be significantly impacted by environmental conditions. Fusing it with other sensors, even in a loosely-coupled manner, provides substantial benefits.
  • The "Flat Ground Assumption" is a useful, but not essential, optimization: While it can improve accuracy, LOCUS performs well even without this environmental prior, demonstrating its general applicability.

Key Results

  • Top-class accuracy: LOCUS achieved accuracy comparable to or better than state-of-the-art open-source lidar odometry algorithms across multiple challenging datasets. For example, in the Urban Alpha dataset, LOCUS achieved a mean Absolute Position Error (APE) of 0.62m, with FGA further reducing it to 0.26m.
  • Robustness to sensor failures: LOCUS demonstrated superior resilience. In simulated WIO/IMU failure scenarios, it could seamlessly transition to lidar-only odometry, while other systems stopped or degraded significantly. Similarly, during simulated lidar drops, its loose coupling allowed it to maintain an accurate initial transform when lidar data resumed.
  • Real-time performance: The system was demonstrated to operate in real-time on various robotic platforms, including the Husky and Spot robots, during the DARPA SubT Challenge. While it requires a relatively high CPU load, it consistently achieved real-time processing speeds.
  • Key component in a winning solution: LOCUS was a critical part of the CoSTAR team's winning solution in the Urban Circuit of the DARPA Subterranean Challenge, highlighting its practical effectiveness in a high-stakes competition.

Limitations and Future Work

  • CPU Load: While achieving real-time performance, LOCUS requires a significant CPU load compared to some other methods, which could be a limitation on resource-constrained platforms.
  • Future Work: The authors mention ongoing work to integrate more sophisticated health monitoring techniques, such as those from the Heterogeneous Robust Odometry (HeRO) system, to further enhance sensor health analysis. They also suggest that the system's adaptability to different platforms and its robustness to sensor failures make it well-suited for future autonomous exploration tasks in challenging environments.

Summary updated

Abstract

A reliable odometry source is a prerequisite to enable complex autonomy behaviour in next-generation robots operating in extreme environments. In this work, we present a high-precision lidar odometry system to achieve robust and real-time operation under challenging perceptual conditions. LOCUS (Lidar Odometry for Consistent operation in Uncertain Settings), provides an accurate multi-stage scan matching unit equipped with an health-aware sensor integration module for seamless fusion of additional sensing modalities. We evaluate the performance of the proposed system against state-of-the-art techniques in perceptually challenging environments, and demonstrate top-class localization accuracy along with substantial improvements in robustness to sensor failures. We then demonstrate real-time performance of LOCUS on various types of robotic mobility platforms involved in the autonomous exploration of the Satsop power plant in Elma, WA where the proposed system was a key element of the CoSTAR team's solution that won first place in the Urban Circuit of the DARPA Subterranean Challenge.
BibTeX
@article{palieri2021locus,
  author = {Matteo Palieri and Benjamin Morrell and Abhishek Thakur and Kamak Ebadi and Jeremy Nash and Arghya Chatterjee and Christoforos Kanellakis and Luca Carlone and Cataldo Guaragnella and Ali-akbar Agha-mohammadi},
  title = {{LOCUS: A Multi-Sensor Lidar-Centric Solution for High-Precision Odometry and 3D Mapping in Real-Time}},
  journal = {IEEE Robotics Autom. Lett.},
  year = {2021},
  url = {https://openreview.net/forum?id=H7lexgkSaZ},
}
Figure