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

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.

Figures (11)

• Figure 1: Our proposed degeneracy-resilient FMCW Lidar Teach-and-Repeat (LT&R) system operating in a flat airport test field. The top image shows the raw lidar scan, followed by uniformly downsampled and curvature-based downsampled scans (coloured by curvature, where non-planar features are highlighted in different colours). In contrast to uniform downsampling, which treats all points equally and often discards sparse geometric features, curvature-based downsampling selectively reduces flat surfaces and preserves informative geometry, improving computational efficiency and the reliability of scan-to-map data association in geometrically degenerate environments. Fake rocks, circled in red, are added to the scene to introduce minimal geometric structure.
• Figure 2: Structure of the Teach-and-Repeat (T&R) pose graph during (a) the teach pass and (b) the repeat pass. $\boldsymbol{\mathcal{F}}_k$ denotes the current robot frame while $\boldsymbol{\mathcal{F}}_v$ and $\boldsymbol{\mathcal{F}}_m$ indicate the vertex frame from the current pass (teach or repeat), and the local submap frame from the reference (teach) pass, respectively. During both teach and repeat passes, the transformation $\hat{\mathbf{T}}_{k,v}$ from the latest vertex frame $\boldsymbol{\mathcal{F}}_v$ to the robot frame $\boldsymbol{\mathcal{F}}_k$ is estimated using odometry. During the repeat pass, $\boldsymbol{\mathcal{F}}_{v^\prime}$ denotes the most recently localized vertex against the reference vertex $\boldsymbol{\mathcal{F}}_{m^\prime}$. A prior for $\mathbf{T}_{k,m}$ is obtained by compounding transformations through $\boldsymbol{\mathcal{F}}_m$, $\boldsymbol{\mathcal{F}}_{v^\prime}$, $\boldsymbol{\mathcal{F}}_v$, and is refined via scan-to-map localization.
• Figure 3: Example FMCW lidar scan with points coloured by per-point radial velocity in the lidar frame. Lighter colours indicate positive radial velocity (moving away from the sensor), while darker colours indicate negative radial velocity (moving toward the sensor).
• Figure 4: (a) Apollo 16 lunar surface imagery (NASA) and (b) Mars 2020 mission imagery (NASA/JPL-Caltech/MSSS) illustrating large-scale, sparsely structured terrain dominated by planar surfaces and gradual elevation changes. In such environments, strong geometric features cannot be assumed, making degeneracy-resilient navigation essential for reliable long-term planetary exploration.
• Figure 5: An overview of the proposed Doppler LTR system, showing the interaction between modules in the teach and repeat passes. During the teach pass, Doppler velocity and gyroscope measurements are used to estimate correspondence-free Doppler-inertial odometry, referred to throughout as Doppler odometry, while curvature information is computed and stored in local submaps. During the repeat pass, incoming lidar scans are processed using the same curvature-based preprocessing and combined with Doppler odometry to provide a motion prior. Scan-to-map localization is performed using degeneracy-aware ICP (DA-ICP) optimization against the stored submaps to refine the robot pose and enable closed-loop path following in geometrically challenging environments.