Degradation-Aware Cooperative Multi-Modal GNSS-Denied Localization Leveraging LiDAR-Based Robot Detections

TL;DR

This work tackles robust long-term localization in GNSS-denied environments by distributing sensors across a heterogeneous robot team and fusing asynchronous, multi-modal measurements. It introduces a factor-graph based, loosely-coupled cooperative localization framework that fuses LiDAR-Inertial Odometry (LIO) and Visual-Inertial Odometry (VIO) outputs with direct 3D inter-robot detections via an interpolation-based factor, and adapts to degradations using Hessian-based LiDAR degeneracy detection and a Wasserstein-distance-based weighting for VIO uncertainty. Key contributions include the interpolation-based detection factor, adaptive odometry weighting, theoretical observability analysis under degradations, and extensive real-world validation with UGV-UAV and UAV-only teams showing significant localization improvements under sensory degradations. The method enables robust, scalable cooperative localization in heterogeneous robot teams without requiring shared sensing modalities or GNSS, enhancing operation in dynamic, feature-poor, or visually-degraded environments.

Abstract

Accurate long-term localization using onboard sensors is crucial for robots operating in Global Navigation Satellite System (GNSS)-denied environments. While complementary sensors mitigate individual degradations, carrying all the available sensor types on a single robot significantly increases the size, weight, and power demands. Distributing sensors across multiple robots enhances the deployability but introduces challenges in fusing asynchronous, multi-modal data from independently moving platforms. We propose a novel adaptive multi-modal multi-robot cooperative localization approach using a factor-graph formulation to fuse asynchronous Visual-Inertial Odometry (VIO), LiDAR-Inertial Odometry (LIO), and 3D inter-robot detections from distinct robots in a loosely-coupled fashion. The approach adapts to changing conditions, leveraging reliable data to assist robots affected by sensory degradations. A novel interpolation-based factor enables fusion of the unsynchronized measurements. LIO degradations are evaluated based on the approximate scan-matching Hessian. A novel approach of weighting odometry data proportionally to the Wasserstein distance between the consecutive VIO outputs is proposed. A theoretical analysis is provided, investigating the cooperative localization problem under various conditions, mainly in the presence of sensory degradations. The proposed method has been extensively evaluated on real-world data gathered with heterogeneous teams of an Unmanned Ground Vehicle (UGV) and Unmanned Aerial Vehicles (UAVs), showing that the approach provides significant improvements in localization accuracy in the presence of various sensory degradations.

Figures (11)

• Figure 1: (a) A lidar-equipped UGV with a single camera-carrying UAV and (b) one lidar-equipped UAV cooperating with two camera-equipped UAV.
• Figure 2: Robot $X$ is localized in local frame $L$ using a LIO algorithm. Robot $Y$ is localized in a local frame $V$ using a VIO algorithm. Robot $X$ detects the relative 3D position of robot $Y$. Robot $Z$ represents another VIO-utilizing robot detected by robot $X$. $W$ denotes the world reference frame of the cooperative localization algorithm. All the reference frames are gravity-aligned. Blue dashed lines represent the estimated variables ${\mathbf{T}}^{W}_{X}, {\mathbf{T}}^{W}_{Y}$. Orange solid lines represent poses ${\mathbf{T}}^{L}_{X},{\mathbf{T}}^{V}_{Y}$ provided by the individual odometry algorithms. The red dotted line represents the relative detection ${\bm{d}}^{X}_{Y}$ of robot $Y$ by robot $X$.
• Figure 3: Factor graph representation of the cooperative localization problem for two robots. Circles represent the estimated variables. Squares represent the factors, i.e., the measurements and prior information. Relative poses of robot $X$ are obtained from a LIO algorithm, while relative poses of robot $Y$ are provided by a VIO algorithm.
• Figure 4: Simplified factor graph used in the observability analysis of the cooperative localization problem. The graph represents two poses of robot $X$ and two poses of robot $Y$. Detections are synchronized with the odometry measurements, removing the need for interpolation. In the visualized situation, the factor graph is fully constrained.
• Figure 5: $xy$-plots of odometry data (LIO, VIO), the proposed cooperative localization method (COOP), and ground truth (GT) from the experimental evaluation. All estimated trajectories were aligned to the ground-truth data for evaluation.