机器人 / 系统课时

ICP 为什么依赖良好的初始位姿

ICP 为什么依赖良好的初始位姿

机器人将当前激光扫描对齐到上一帧或局部地图时,ICP 并不会自动识别每个点的真实身份,而是根据当前位姿猜测点与点的对应关系。通过一组可运行的二维实验,你将看到好坏初值如何改变匹配、收敛结果,以及怎样识别错误配准。

学习目标

  • 解释初始旋转和平移为何会改变最近邻匹配。
  • 使用二维点到点 ICP 验证迭代和局部最优。
  • 根据残差、重叠率和运动先验判断配准是否可信。

初始位姿先决定点与谁配对

设当前扫描为源点集 PP,局部地图为目标点集 MM;我们要求的是源扫描到目标地图的刚体变换,即旋转 RR 和平移 tt。ICP 通常先接受里程计、IMU 或其他匹配方法提供的初始估计,再逐步修正,而不是从任意位置开始搜索所有可能姿态。

考虑地图中沿 x 轴有两个不同的角点,分别位于 0 m 和 2 m。某源点实际对应 0 m 的角点:如果初始变换后该点位于 0.2 m,最近邻会选 0 m;若错误初值让它落在 1.7 m,最近邻就会选 2 m。下一轮优化会努力拟合这组错误对应,并不懂哪个角点才是真实目标。

因此,好初值的作用不是直接给出最终答案,而是使第一轮对应关系落在正确的邻域。重复走廊、平行墙或低重叠扫描中,即使位姿初值较近,也可能存在歧义。

最近邻与刚体求解如何反复进行

点到点 ICP 在一轮内固定最近邻索引 c(i)c(i),再最小化源点变换后与对应地图点的平方距离:

E(R,t)=∑i∥Rpi+t−mc(i)∥2E(R,t)=\sum_i\|Rp_i+t-m_{c(i)}\|^2

一次迭代按以下顺序执行:

  1. 用初始 R,tR,t 将所有源点放到地图坐标系。
  2. 为每个变换后的源点找欧氏距离最近的目标点;普通最近邻允许多个源点选中同一目标点。
  3. 对固定的点对,分别减去质心,利用二维奇异值分解(SVD)求最优刚体旋转,再由质心计算平移;要求旋转行列式为 +1,避免镜像。
  4. 用新的 R,tR,t 重新寻找最近邻,直到变化足够小或到达迭代上限(例如 30 轮)。

匹配索引会随位姿改变,因此最小化的是当前匹配下的局部误差,并不保证全局最优。对 NN 个源点和 MM 个目标点,直接穷举最近邻、迭代 KK 轮的代价为 O(KNM)O(KNM);生产实现通常用空间索引、降采样和最大关联距离降低开销。

良好与较差初始位姿导致不同最近邻关联及收敛结果

用相同点云复现两种收敛结果

下面使用 Python 3.10+ 与 NumPy 1.24+,仅实现二维点到点 ICP。目标地图含 7 个不对称点,合成扫描的真实“源到目标”变换是旋转 12°、平移 (1.2, -0.6) m。比较接近真值的初值和明显偏离的初值,算法与数据完全相同。

python
import numpy as np

def rotation(deg):
    a = np.deg2rad(deg)
    return np.array([[np.cos(a), -np.sin(a)],
                     [np.sin(a),  np.cos(a)]])

def fit_rigid(source, matched):
    ps, pm = source.mean(axis=0), matched.mean(axis=0)
    u, _, vt = np.linalg.svd((source - ps).T @ (matched - pm))
    r = vt.T @ u.T
    if np.linalg.det(r) < 0:  # 排除镜像变换
        vt[-1, :] *= -1
        r = vt.T @ u.T
    return r, pm - r @ ps

def icp(source, target, angle0, shift0, max_iter=30):
    r, t = rotation(angle0), np.array(shift0, dtype=float)
    for step in range(1, max_iter + 1):
        moved = source @ r.T + t
        # 每轮用当前位姿重新做最近邻关联
        d2 = ((moved[:, None, :] - target[None, :, :]) ** 2).sum(axis=2)
        nearest = np.argmin(d2, axis=1)
        nr, nt = fit_rigid(source, target[nearest])
        change = np.linalg.norm(nr - r) + np.linalg.norm(nt - t)
        r, t = nr, nt
        if change < 1e-10:
            break
    aligned = source @ r.T + t
    d2 = ((aligned[:, None, :] - target[None, :, :]) ** 2).sum(axis=2)
    rmse = np.sqrt(np.min(d2, axis=1).mean())
    angle = np.rad2deg(np.arctan2(r[1, 0], r[0, 0]))
    return step, angle, t, rmse

target = np.array([[0., 0.], [2., 0.], [4., 0.], [0., 2.],
                   [0., 5.], [3., 3.], [6., 2.]])
true_r, true_t = rotation(12), np.array([1.2, -0.6])
source = (target - true_t) @ true_r  # 构造已知真值的源扫描

for label, angle, shift in [
    ("良好初值", 11, (1.0, -0.5)),
    ("较差初值", -20, (0.0, 0.0)),
]:
    n, a, t, e = icp(source, target, angle, shift)
    print(f"{label}:{n}轮,角度={a:.2f}°,"
          f"平移=({t[0]:.2f},{t[1]:.2f})m,RMSE={e:.3f}m")

预期输出:

text
良好初值:2轮,角度=12.00°,平移=(1.20,-0.60)m,RMSE=0.000m
较差初值:2轮,角度=-13.04°,平移=(-0.55,0.02)m,RMSE=0.981m

这里的 RMSE 是最终变换后到最近目标点的均方根距离,不是与已知真实点对计算的误差。合成数据无噪声,因此良好初值会得到近似零误差;较差初值虽然也很快停止,却收敛到错误姿态。“迭代次数少”显然不等于“位姿正确”。

在激光 SLAM 中防止误配

真实扫描常有遮挡、移动物体和长直墙。长走廊沿墙方向的信息不足,错误滑动仍可能产生很低的距离误差;重复立柱可能导致整体错位一个柱距。仅比较 ICP 的 RMSE,容易把这种几何歧义当成成功。

实际接入 ROS 2 二维激光前端时,可按以下顺序处理:

  1. 先检查坐标与时间:确认初始变换方向是扫描帧到目标地图帧,检查 LaserScan.header.stamp、激光外参和里程计时刻;运动时需考虑逐束扫描畸变。
  2. 控制对应搜索范围:例如点云按约 0.1 m 间距降采样后,从 0.3 m 最大关联距离、30 次迭代开始试验;这只是示例起点,需根据初始位姿误差、扫描密度及重叠范围调整。
  3. 同时检查重叠与残差:可先尝试“有效对应比例不少于 70%、对应 RMSE 不高于 0.15 m”的实验门槛,再使用真实日志标定;低重叠时不能只看小样本 RMSE。
  4. 验证运动合理性:将 ICP 修正量与轮式里程计、IMU 和环境结构对照。若残差较小但位姿突然跃迁,应拒绝该帧或转入粗到细、多初值、全局重定位,而不是直接写入地图。

ICP 适合在已有大致位姿时做局部精配准。如果初始化超出正确对应的捕获范围,应先改善里程计估计或做粗配准,再让 ICP 负责最后的小范围修正。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

ICP 为什么依赖良好的初始位姿

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通