Doppler-SLAM: Doppler-Aided Radar-Inertial and LiDAR-Inertial Simultaneous Localization and Mapping

TL;DR

Doppler-SLAM addresses robust SLAM under challenging conditions by unifying 4D radar-inertial and FMCW LiDAR-inertial SLAM through Doppler velocity integration. The framework couples a four-module front-end—velocity filtering, motion compensation, state estimation via IEKF—and an online calibration-enabled graph back-end with loop closure to maintain sensor alignment. Key innovations include a Doppler-based scan-matching approach, a dynamic-point velocity filter that does not assume a static majority, and an online extrinsic calibration mechanism aided by Doppler velocity and loop closures. Extensive experiments across multiple public and proprietary datasets demonstrate superior accuracy and robustness in dynamic environments, with open-source code and a new IMADAR dataset to foster further research.

Abstract

Simultaneous localization and mapping (SLAM) is a critical capability for autonomous systems. Traditional SLAM approaches, which often rely on visual or LiDAR sensors, face significant challenges in adverse conditions such as low light or featureless environments. To overcome these limitations, we propose a novel Doppler-aided radar-inertial and LiDAR-inertial SLAM framework that leverages the complementary strengths of 4D radar, FMCW LiDAR, and inertial measurement units. Our system integrates Doppler velocity measurements and spatial data into a tightly-coupled front-end and graph optimization back-end to provide enhanced ego velocity estimation, accurate odometry, and robust mapping. We also introduce a Doppler-based scan-matching technique to improve front-end odometry in dynamic environments. In addition, our framework incorporates an innovative online extrinsic calibration mechanism, utilizing Doppler velocity and loop closure to dynamically maintain sensor alignment. Extensive evaluations on both public and proprietary datasets show that our system significantly outperforms state-of-the-art radar-SLAM and LiDAR-SLAM frameworks in terms of accuracy and robustness. To encourage further research, the code of our Doppler-SLAM and our dataset are available at: https://github.com/Wayne-DWA/Doppler-SLAM.

Figures (7)

• Figure 1: Generalizability of our proposed Doppler-SLAM on the HeRCULES dataset "Sports Complex". Left: radar map and trajectory (blue) generated with Doppler-SLAM. Right: FMCW LiDAR map and trajectory (green) generated with Doppler-SLAM.
• Figure 2: Pipeline of Doppler-SLAM consists of four main modules: velocity filter (Sec. \ref{['velocityFilter']}), motion compensation (Sec. \ref{['motionCom']}), state estimation (Sec. \ref{['stateEsti']}), and loop closure with graph optimization (Sec. \ref{['gtaphOpti']}). The graph on the right illustrates the workflow of online extrinsic calibration between the IMU and either radar or LiDAR using graph optimization. In this approach, we combine the IMU pre-integration factor, odometry factor, and ego velocity factor to construct a factor graph. Once a loop closure factor is detected, additional optimization refines the extrinsic transformation.
• Figure 3: Velocity Filter in a highly dynamic scenario with a moving tram. The top panel presents the camera view, the left panel shows the radar point cloud after processing by our proposed velocity filter, where purple indicates dynamic points and green indicates static points, and the right panel illustrates the traditional least-squares method, in which green points are inliers (static objects) detected by the method and red points are outliers (dynamic objects). The least-squares method relies on the Doppler velocities of all inliers to fit the ego velocity profile (blue curve) but struggles in highly dynamic environments because it incorrectly incorporates Doppler measurements from moving objects (tram). In contrast, our IMU-based velocity filter effectively distinguishes between dynamic and static points, yielding more accurate ego velocity estimates and robust performance in complex, real-world scenarios.
• Figure 4: Geometry and Doppler Residual
• Figure 5: Experiment setup (left: sensor platform mounted on a car, right: CAD Model of the platform)