Doppler-enhanced FMCW LiDAR odometry based on linear continuous-time representation

Weitong Wu, Chi Chen, Bisheng Yang, Yuhang Xu, Yichen Shen, Xiufeng He2025 · DOI

Abstract

LiDAR-based simultaneous localization and mapping (SLAM) plays a crucial role in applications such as search and rescue, infrastructure inspection, and underground exploration. However, conventional LiDAR-based methods often exhibit significantly reduced accuracy in degenerate environments. To address this challenge, this paper proposes a simple yet effective linear continuous-time FMCW (Frequency-Modulated Continuous Wave) LiDAR odometry method that tightly integrates Doppler constraints and point-to-plane constraints within a sliding-window-based factor graph optimization framework. The proposed method is comprehensively validated using datasets collected from a vehicle equipped with an Aeva I FMCW LiDAR in both typically degenerate scenes and highway scenarios. Experimental results demonstrate that the proposed method achieves the lowest trajectory root mean square error (RMSE) among the three sequences out of the total eight sequences, outperforming all compared methods. Notably, on Sequence 7, which spans an approximately trajectory length of 7,300 m, our method achieves a minimum trajectory RMSE of 10.19 m.

Introduction

Standard LiDAR-based SLAM relies heavily on identifying geometric features like corners and planes to match sequential scans. However, in "feature-sparse" or "degenerate" environments—such as long, smooth tunnels or straight highways—there are often insufficient unique shapes to ensure accurate localization, leading to significant drift. Furthermore, traditional LiDAR sensors suffer from motion distortion because the sensor moves while the laser sweeps.

Frequency-Modulated Continuous Wave (FMCW) LiDAR has emerged as a powerful solution, providing instantaneous radial velocity (Doppler) for every point in a scan. This paper introduces a framework that leverages this Doppler data within a linear continuous-time trajectory model to achieve high accuracy in challenging environments where traditional geometric-only methods fail.

Methodology

The proposed framework moves away from treating LiDAR scans as discrete snapshots. Instead, it employs a linear continuous-time representation by slicing standard 100ms frames into smaller, equal segments (e.g., 40ms). Within each segment, motion is assumed to be linear, allowing the system to implicitly correct motion distortion during the optimization process.

Overview of the proposed linear continuous-time FMCW LiDAR odometry framework.

The system utilizes a sliding-window factor graph to maintain a balance between historical context and real-time efficiency. The optimization engine minimizes a cost function composed of three tightly-coupled factors:

  1. Geometric Factors: Traditional point-to-plane residuals that match points to a voxel map.
  2. Doppler Factors: A novel constraint relating the LiDAR's measured radial velocity to the estimated system velocity, acting as an absolute velocity reference.
  3. Marginalization Factors: A mathematical "memory" that retains information from older states as they exit the sliding window.

Experimental Evaluation

The method was evaluated using real-world data from a vehicle equipped with an Aeva Aeries I FMCW LiDAR. The dataset consists of 8 sequences covering urban tunnels (feature-poor) and high-speed highway scenarios.

Bird's eye view of the FMCW LiDAR point cloud data with Doppler velocity color-coding.

Key Results and Insights

  • Doppler as a Velocity Prior: In tunnel environments where walls look identical, Doppler measurements provide a critical longitudinal anchor. In Sequence 1 (a tunnel), the proposed method and other Doppler-enhanced approaches achieved errors of approximately 3m, while geometric-only methods like CT-ICP saw errors spike to 28m.
  • Efficiency of the Linear Assumption: While some state-of-the-art methods use complex Gaussian Process regression for continuous-time modeling, this research demonstrates that a simple linear assumption over short time slices is highly effective, providing high accuracy without excessive computational overhead.
  • Long-Range Stability: The method proved exceptionally stable over long distances. On Sequence 7, spanning over 7.3 kilometers of highway, it achieved a trajectory RMSE of just 10.19 meters, outperforming all compared algorithms including CT-ICP, Traj-LO, and STEAM-DICP.

Conclusion

By integrating Doppler constraints into a linear continuous-time framework, this method offers a robust solution for LiDAR odometry in degenerate environments. It successfully resolves motion distortion and provides an absolute velocity reference that prevents drift in feature-sparse scenes. Future work will focus on implementing adaptive frame lengths to automatically optimize the balance between accuracy and speed based on real-time motion dynamics.

Summary updated

Abstract

LiDAR-based simultaneous localization and mapping (SLAM) plays a crucial role in applications such as search and rescue, infrastructure inspection, and underground exploration. However, conventional LiDAR-based methods often exhibit significantly reduced accuracy in degenerate environments. To address this challenge, this paper proposes a simple yet effective linear continuous-time FMCW (Frequency-Modulated Continuous Wave) LiDAR odometry method that tightly integrates Doppler constraints and point-to-plane constraints within a sliding-window-based factor graph optimization framework. The proposed method is comprehensively validated using datasets collected from a vehicle equipped with an Aeva I FMCW LiDAR in both typically degenerate scenes and highway scenarios. Experimental results demonstrate that the proposed method achieves the lowest trajectory root mean square error (RMSE) among the three sequences out of the total eight sequences, outperforming all compared methods. Notably, on Sequence 7, which spans an approximately trajectory length of 7,300 m, our method achieves a minimum trajectory RMSE of 10.19 m.
BibTeX
@inproceedings{wu2025doppler,
  author = {Weitong Wu and Chi Chen and Bisheng Yang and Yuhang Xu and Yichen Shen and Xiufeng He},
  title = {{Doppler-enhanced FMCW LiDAR odometry based on linear continuous-time representation}},
  booktitle = {˜The},
  year = {2025},
  doi = {10.5194/isprs-archives-xlviii-1-w5-2025-147-2025},
}
Figure