GaRLILEO: Gravity-aligned Radar-Leg-Inertial Enhanced Odometry

TL;DR

GaRLILEO presents a gravity-aligned, continuous-time radar–leg–IMU odometry framework that decouples velocity estimation from the IMU using a velocity spline derived from SoC radar Doppler and leg kinematics, while estimating gravity with a novel soft \mathcal{S}^2 constraint to curb vertical drift. The method employs a B-spline trajectory representation and a factor-graph that fuses IMU, radar, leg kinematics, and gravity information in an incremental optimization loop, enabling querying at arbitrary times and robust performance in challenging terrains such as stairs and slopes. Key contributions include a velocity-aware gravity factor, a continuous-time radar–leg–IMU fusion pipeline, and a self-collected dataset with ground-truth trajectories for both indoor and outdoor sequences; results show state-of-the-art vertical accuracy and resilient odometry under perception-degraded conditions. The approach enables reliable legged navigation without reliance on LiDAR or cameras, with potential impact on robust SLAM and loco-localization in cluttered, feature-sparse environments, and is released as open-source to spur further research.

Abstract

Deployment of legged robots for navigating challenging terrains (e.g., stairs, slopes, and unstructured environments) has gained increasing preference over wheel-based platforms. In such scenarios, accurate odometry estimation is a preliminary requirement for stable locomotion, localization, and mapping. Traditional proprioceptive approaches, which rely on leg kinematics sensor modalities and inertial sensing, suffer from irrepressible vertical drift caused by frequent contact impacts, foot slippage, and vibrations, particularly affected by inaccurate roll and pitch estimation. Existing methods incorporate exteroceptive sensors such as LiDAR or cameras. Further enhancement has been introduced by leveraging gravity vector estimation to add additional observations on roll and pitch, thereby increasing the accuracy of vertical pose estimation. However, these approaches tend to degrade in feature-sparse or repetitive scenes and are prone to errors from double-integrated IMU acceleration. To address these challenges, we propose GaRLILEO, a novel gravity-aligned continuous-time radar-leg-inertial odometry framework. GaRLILEO decouples velocity from the IMU by building a continuous-time ego-velocity spline from SoC radar Doppler and leg kinematics information, enabling seamless sensor fusion which mitigates odometry distortion. In addition, GaRLILEO can reliably capture accurate gravity vectors leveraging a novel soft S2-constrained gravity factor, improving vertical pose accuracy without relying on LiDAR or cameras. Evaluated on a self-collected real-world dataset with diverse indoor-outdoor trajectories, GaRLILEO demonstrates state-of-the-art accuracy, particularly in vertical odometry estimation on stairs and slopes. We open-source both our dataset and algorithm to foster further research in legged robot odometry and SLAM. https://garlileo.github.io/GaRLILEO

Figures (16)

• Figure 1: Overall preview of GaRLILEO. The four subfigures in the upper row present the problematic situations that quadrupedal robots may encounter while performing real-world tasks, while the yellow letters specify the situations and the red words explain the substantial issues generated from them. Two boxes in the left part of the lower row summarize the major contribution and method of the GaRLIELEO, which significantly reduces odometry error, especially in the vertical direction. Two graphs in the right part of the lower row present the short experimental results, showing the accuracy of GaRLILEO in multiple sequences that include loops, sharp turns, and staircases, where most baselines fail to maintain accuracy in odometry estimation.
• Figure 2: Comparison between body-centric and contact-centric leg locomotion of a single leg during the contact state. \ref{['fig:body_centric']} On body-centric calculation, the end-effector position differs as time passes. Based on the contact information from the contact sensor positioned at every foot, every contact frame should remain static while the contact sensor is on. Therefore, using the forward kinematics, the ego-centric velocity of the robot base can be calculated as in \ref{['fig:contact_centric']}.
• Figure 3: Soft $\mathcal{S}^2$-constrained gravity factor $r_{\mathcal{S}^2}$. Prior to optimization, the orange vectors are initialized through gravity spline extrapolation. During optimization, the factor (blue) constrains vectors that would otherwise drift in the $\mathbb{R}^3$ manifold (red), steering them to lie on the $\mathcal{S}^2$ surface (green).
• Figure 4: Relationship between sensor data and splines during incremental optimization. Gray-shaded boxes indicate sensor measurements active within the sliding optimization window; colored lines denote inter-spline and measurement-spline dependencies. In GaRLILEO, velocity is decoupled from the IMU, constructing a continuous-time ego-velocity spline from radar and leg-kinematics measurements. This decoupling is particularly advantageous for legged robots, where ground contact induces noisy accelerations on IMU.
• Figure 5: Overview pipeline of GaRLILEO