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

📄 arXiv: 2511.20292v1 📥 PDF

作者: Dong Wang, Daniel Casado Herraez, Stefan May, Andreas Nüchter

分类: cs.RO

发布日期: 2025-11-25

备注: 8 pages, 5 figures

🔗 代码/项目: GITHUB

💡 一句话要点

提出Dynamic-ICP，解决动态场景下基于ICP的里程计配准问题

🎯 匹配领域: 支柱三：空间感知 (Perception & SLAM)

关键词: 动态场景 ICP配准 Doppler速度 里程计 三维重建

📋 核心要点

  1. 传统ICP在动态场景中性能下降，因为其假设场景是静态的，且在重复或低纹理几何结构中表现不佳。
  2. Dynamic-ICP利用Doppler速度信息，通过速度滤波、动态对象聚类和运动预测来处理动态点。
  3. 实验表明，Dynamic-ICP在动态场景中显著提升了旋转稳定性和平移精度，且易于集成和实时运行。

📝 摘要（中文）

本文提出了一种名为Dynamic-ICP的 Doppler 感知配准框架，旨在解决高动态环境中基于 ICP 的里程计配准问题。该方法首先通过鲁棒回归从每个点的 Doppler 速度估计自运动，并构建速度滤波器。然后，对动态对象进行聚类，并从经过自运动补偿的径向测量中重建对象级的平移速度。接着，使用恒速模型预测动态点。最后，使用紧凑的目标函数对齐扫描，该目标函数将点到面的几何残差与平移不变、仅旋转的 Doppler 残差相结合。该方法不需要外部传感器或传感器-车辆校准，并且直接在 FMCW LiDAR 的距离和 Doppler 速度上运行。在 HeRCULES、HeLiPR 和 AevaScenes 三个数据集上，针对高动态场景评估了 Dynamic-ICP。结果表明，Dynamic-ICP 在旋转稳定性和平移精度方面始终优于最先进的方法。该方法易于集成到现有流程中，可以实时运行，并为动态环境中的鲁棒配准提供了一个轻量级的解决方案。代码已开源。

🔬 方法详解

问题定义：论文旨在解决高动态环境下，传统ICP算法在里程计配准中表现不佳的问题。传统ICP算法假设场景是静态的，这在高动态环境中是不成立的，导致配准精度下降，甚至失效。此外，在重复或低纹理的几何结构中，ICP算法也容易陷入局部最优。

核心思路：论文的核心思路是利用FMCW LiDAR提供的Doppler速度信息，区分静态和动态点，并对动态点进行建模和补偿。通过融合几何残差和Doppler残差，构建更鲁棒的配准目标函数。这种方法能够有效抑制动态对象对配准的影响，提高在动态环境下的配准精度。

技术框架：Dynamic-ICP的整体框架包括以下几个主要模块： 1. 自运动估计：通过鲁棒回归从每个点的Doppler速度估计自运动。 2. 速度滤波：构建速度滤波器，过滤掉噪声和异常值。 3. 动态对象聚类和速度重建：对动态对象进行聚类，并从经过自运动补偿的径向测量中重建对象级的平移速度。 4. 动态点预测：使用恒速模型预测动态点的位置。 5. 扫描对齐：使用紧凑的目标函数对齐扫描，该目标函数结合了点到面的几何残差和Doppler残差。

关键创新：Dynamic-ICP的关键创新在于： 1. Doppler感知：首次将Doppler速度信息融入ICP框架，用于动态场景下的配准。 2. 动态对象建模：通过聚类和速度重建，对动态对象进行建模，并进行运动补偿。 3. 融合几何和Doppler残差：构建了同时考虑几何信息和Doppler信息的配准目标函数，提高了配准的鲁棒性。

关键设计： 1. 鲁棒回归：使用鲁棒回归方法估计自运动，以减少噪声和异常值的影响。 2. 恒速模型：使用恒速模型预测动态点的位置，简化了动态对象的运动模型。 3. 紧凑的目标函数：目标函数的设计需要平衡几何残差和Doppler残差的权重，以达到最佳的配准效果。具体的权重参数需要根据实际场景进行调整。

📊 实验亮点

Dynamic-ICP在HeRCULES、HeLiPR和AevaScenes三个数据集上进行了评估，结果表明，Dynamic-ICP在旋转稳定性和平移精度方面始终优于最先进的方法。具体而言，在动态场景下，Dynamic-ICP能够显著降低旋转误差和平移误差，提高里程计的精度和鲁棒性。实验结果验证了Dynamic-ICP在动态环境下的有效性。

🎯 应用场景

Dynamic-ICP在自动驾驶、机器人导航、三维重建等领域具有广泛的应用前景。尤其是在城市交通、物流配送等动态环境中，能够提供更准确、更鲁棒的定位和地图构建能力。该方法还可以应用于无人机、移动机器人等平台，提高其在复杂环境中的自主导航能力。未来，Dynamic-ICP有望成为动态场景下三维感知的重要组成部分。

📄 摘要（原文）

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.