Dynamic-ICP: Doppler-Aware Iterative Closest Point Registration for Dynamic Scenes

2025-11-25

AI Summary

In highly dynamic environments, conventional ICP-based LiDAR odometry suffers from failure of the static-world assumption and severe performance degradation in low-texture or repetitive-structure scenes. Method: This paper proposes a robust LiDAR registration framework integrating Doppler velocity measurements. It introduces a Doppler-aware, rotation-invariant residual term that jointly models point-to-plane geometric constraints and velocity observations, enabling dynamic point detection and rejection without external sensors. The framework further incorporates robust motion estimation, velocity filtering, dynamic clustering, constant-velocity prediction, and joint optimization into a unified objective function. Results: Evaluated on multiple dynamic datasets, the method achieves significant improvements in translational accuracy (average +32%) and rotational stability (RPE reduced by 41%), operates in real time, and is readily integrable into existing SLAM systems.

Technology Category

• Intelligent Robots: State Estimation
• Computer Vision: Motion & Tracking
• Planning, Routing, and Scheduling: Replanning and Plan Repair

Application Category

• Graph Algorithms and Modeling for the Web: Efficient manipulation of static and dynamic Web-related graphs
• Systems and Infrastructure for Web, Mobile and WoT: Location- and context-aware Web and WoT applications and services
• Security and Privacy: Large-scale security measurements

Abstract

Reliable odometry in highly dynamic environments remains challenging when it relies on ICP-based registration: ICP assumes near-static scenes and degrades in repetitive or low-texture geometry. We introduce Dynamic-ICP, a Doppler-aware registration framework. The method (i) estimates ego motion from per-point Doppler velocity via robust regression and builds a velocity filter, (ii) clusters dynamic objects and reconstructs object-wise translational velocities from ego-compensated radial measurements, (iii) predicts dynamic points with a constant-velocity model, and (iv) aligns scans using a compact objective that combines point-to-plane geometry residual with a translation-invariant, rotation-only Doppler residual. The approach requires no external sensors or sensor-vehicle calibration and operates directly on FMCW LiDAR range and Doppler velocities. We evaluate Dynamic-ICP on three datasets-HeRCULES, HeLiPR, AevaScenes-focusing on highly dynamic scenes. Dynamic-ICP consistently improves rotational stability and translation accuracy over the state-of-the-art methods. Our approach is also simple to integrate into existing pipelines, runs in real time, and provides a lightweight solution for robust registration in dynamic environments. To encourage further research, the code is available at: https://github.com/JMUWRobotics/Dynamic-ICP.

Problem

• Addresses ICP degradation in dynamic scenes with Doppler-aware registration
• Estimates ego motion and clusters dynamic objects using Doppler velocity
• Improves registration accuracy in repetitive or low-texture dynamic environments

Innovation

• Estimates ego motion using Doppler velocity regression
• Clusters dynamic objects and reconstructs their velocities
• Aligns scans with geometry and rotation-only Doppler residuals