VICP（Velocity-based ICP）：通过运动校准实现精准姿态估计

一、为什么需要VICP？

激光雷达采集一帧点云通常需要几十毫秒，在此期间如果机器人在运动，不同点的采集时刻对应不同的位置，最终会出现畸变（如“拉弯”或“拖影”）。

常规的 ICP 方法假设整帧点云是刚体，因此无法处理这种情况。VICP 则从根本上建模了这一问题 —— 假设机器人在扫描过程中做的是匀速刚体运动，通过估计其线速度和角速度，实现连续运动建模 + 去畸变 + 配准一体化。

二、VICP 的基本思想

VICP 和 ICP 一样是做帧间点云配准，但不同点在于：

VICP 将点云视为在采集过程中不断变化的位置的组合，通过优化速度向量来去除畸变并配准。

三、VICP 算法流程详解

下面以二维激光雷达为例进行解释，三维场景原理类似。

输入：

• 当前帧点云 P={xj,tj}\mathcal{P} = \{x_j, t_j\}P={xj​,tj​}，每个点带时间戳
• 参考帧点云 Q={qj}\mathcal{Q} = \{q_j\}Q={qj​}
• 初始速度估计 ${\mathbf{v}}_0,{\mathbf{\omega }}_0$

步骤：

1. 初始化

V_i = V_{i-1}  # 使用上一次的速度作为初值

2. 开始迭代

while ||V_i - V_{i-1}|| > ε:

3. 计算点云补偿变换

对每个点，根据其时间差 $\Delta t_j$ 计算在该时刻下的雷达位姿：

$$T_j=\exp (-\hat{\xi }\cdot \Delta t_j)$$

使用这个变换将点 $x_j$ 补偿到参考时刻（去畸变）：

$${\tilde{x}}_j=T_j\cdot x_j$$

4. 对去畸变点云做 ICP

T = ICP( \tilde{\mathcal{P}}, \mathcal{Q} )

这一步用于估计去畸变后的点云与参考帧之间的刚体变换。

5. ICP 结果反推出新的速度

$${\xi }_{\text{new}}=\frac{1}{\Delta t}\log (T)$$

更新速度并进入下一轮迭代。

四、一个实际示例

假设：

• 激光帧扫描时间为 100ms
• 点云采集中，机器人沿前方运动 0.5m，左转 10°
• 每个点都记录了采样时间

初始化：

v_0 = (0.5, 0) m/s
ω_0 = (0, 0, 10°/s)

然后每个点根据时间补偿出当时的位置，整帧点云就被“拉直”了。补偿后的点云可以准确和上一帧对齐，实现精确配准。

五、VICP 最终输出结果

• 平均线速度 $\mathbf{v}$
• 平均角速度 $\mathbf{\omega }$
• 去畸变后的点云 $\tilde{\mathcal{P}}$
• 帧间位姿变换 $T$

其中速度估计可用于 odometry、去畸变点云可用于建图或前端定位。

六、初始速度从哪里来？

• 有IMU：可以直接使用IMU估计速度

• 无IMU：

  • 使用上一帧的速度估计
  • 或通过原始点云做粗略 ICP，推导速度
  • 或默认小速度初值（如 0.1m/s）

七、VICP适用场景

• 移动机器人/无人车激光SLAM前端
• 高速运动激光雷达去畸变（如Velodyne、Livox）
• 与IMU联合估计（如LIO-SAM、FAST-LIO等）

八、总结

VICP 不是传统意义上的“配准算法”，而是将速度引入配准过程的运动建模方案。它通过优化线速度和角速度，实现了对点云运动畸变的精确补偿，并提升了帧间配准精度。