Degeneracy-Resilient Teach and Repeat for Geometrically Challenging Environments Using FMCW Lidar

TL;DR

This paper introduces a novel FMCW lidar-based teach and repeat navigation system that remains reliable in geometrically degenerate environments by using Doppler velocity measurements and degeneracy-aware localization techniques.

Contribution

The work presents a degeneracy-resilient FMCW lidar T&R system that integrates Doppler velocity data and curvature-aware localization to improve robustness in challenging environments.

Findings

Successfully navigated in environments with sparse structures.

Outperformed ICP-based systems in flat, degenerate terrains.

Demonstrated reliable autonomous navigation in real-world tests.

Abstract

Teach and Repeat (T&R) topometric navigation enables robots to autonomously repeat previously traversed paths without relying on GPS, making it well suited for operations in GPS-denied environments such as underground mines and lunar navigation. State-of-the-art T&R systems typically rely on iterative closest point (ICP)-based estimation; however, in geometrically degenerate environments with sparsely structured terrain, ICP often becomes ill-conditioned, resulting in degraded localization and unreliable navigation performance. To address this challenge, we present a degeneracy-resilient Frequency-Modulated Continuous-Wave (FMCW) lidar T&R navigation system consisting of Doppler velocity-based odometry and degeneracy-aware scan-to-map localization. Leveraging FMCW lidar, which provides per-point radial velocity measurements via the Doppler effect, we extend a geometry-independent, correspondence-free motion estimation to include principled pose uncertainty estimation that remains stable in degenerate environments. We further propose a degeneracy-aware localization method that incorporates per-point curvature for improved data association, and unifies translational and rotational scales to enable consistent degeneracy detection. Closed-loop field experiments across three environments with varying structural richness demonstrate that the proposed system reliably completes autonomous navigation, including in a challenging flat airport test field where a conventional ICP-based system fails.