FAST-LIVGO: A Degeneracy-Robust LiDAR-Inertial-Visual-GNSS Fusion Odometry

2026-06-17

AI Summary

This work addresses the challenges of severe cumulative drift and frequent failure of multi-sensor fusion odometry in geometrically degenerate or texture-less environments under long-duration, large-scale, and highly dynamic conditions. To this end, we propose a tightly coupled LiDAR-inertial-visual-GNSS odometry framework built upon an error-state iterative Kalman filter. Our approach incorporates a degradation-aware dual-mode outlier rejection strategy, a dynamic time warping–driven online spatiotemporal alignment module, and a millimeter-level GNSS relative constraint model fusing Doppler measurements with carrier-phase differences from fixed-anchor time-differenced observations. Experiments on the M3DGR benchmark and a custom high-speed fixed-wing UAV dataset demonstrate that the proposed method significantly suppresses trajectory drift and map ghosting, achieving superior accuracy and robustness compared to state-of-the-art approaches.

Abstract

Robust state estimation and mapping in long-term, large-scale, and highly dynamic environments remains a key challenge in robotics. Existing LiDAR-Inertial-Visual Odometry (LIVO) systems achieve strong local accuracy but suffer from accumulated drift over long distances and may fail in geometrically degraded or textureless scenes. Meanwhile, GNSS-aided fusion frameworks often rely on LiDAR or visual odometry for state prediction and outlier rejection, making them vulnerable when odometry degenerates. To address these limitations, we propose a tightly coupled LiDAR-Inertial-Visual-GNSS fusion framework based on an Error-State Iterated Kalman Filter. An online spatiotemporal alignment module using Dynamic Time Warping is introduced for highly dynamic conditions. To better exploit GNSS precision, we develop observation models based on Doppler shifts and fixed-anchor Time-Differenced Carrier Phase, providing millimeter-level relative constraints without augmenting historical anchor states. We further design a degeneracy-aware dual-mode outlier rejection strategy that switches between LIVO-prior-guided rejection and GNSS-aided recovery according to the LIVO degeneracy level. Experiments on the public M3DGR dataset and a custom 20~m/s fixed-wing UAV dataset demonstrate that our system reduces accumulated drift and map ghosting, outperforming state-of-the-art methods in accuracy and robustness.

Problem

• LiDAR-Inertial-Visual Odometry
• GNSS fusion
• degeneracy
• accumulated drift
• dynamic environments

Innovation

• LiDAR-Inertial-Visual-GNSS fusion
• degeneracy-aware outlier rejection
• Time-Differenced Carrier Phase
• Dynamic Time Warping
• Error-State Iterated Kalman Filter