机器人 / 系统课时
ICP 为什么依赖良好的初始位姿
ICP 为什么依赖良好的初始位姿
机器人将当前激光扫描对齐到上一帧或局部地图时,ICP 并不会自动识别每个点的真实身份,而是根据当前位姿猜测点与点的对应关系。通过一组可运行的二维实验,你将看到好坏初值如何改变匹配、收敛结果,以及怎样识别错误配准。
学习目标
- 解释初始旋转和平移为何会改变最近邻匹配。
- 使用二维点到点 ICP 验证迭代和局部最优。
- 根据残差、重叠率和运动先验判断配准是否可信。
初始位姿先决定点与谁配对
设当前扫描为源点集 ,局部地图为目标点集 ;我们要求的是源扫描到目标地图的刚体变换,即旋转 和平移 。ICP 通常先接受里程计、IMU 或其他匹配方法提供的初始估计,再逐步修正,而不是从任意位置开始搜索所有可能姿态。
考虑地图中沿 x 轴有两个不同的角点,分别位于 0 m 和 2 m。某源点实际对应 0 m 的角点:如果初始变换后该点位于 0.2 m,最近邻会选 0 m;若错误初值让它落在 1.7 m,最近邻就会选 2 m。下一轮优化会努力拟合这组错误对应,并不懂哪个角点才是真实目标。
因此,好初值的作用不是直接给出最终答案,而是使第一轮对应关系落在正确的邻域。重复走廊、平行墙或低重叠扫描中,即使位姿初值较近,也可能存在歧义。
最近邻与刚体求解如何反复进行
点到点 ICP 在一轮内固定最近邻索引 ,再最小化源点变换后与对应地图点的平方距离:
一次迭代按以下顺序执行:
- 用初始 将所有源点放到地图坐标系。
- 为每个变换后的源点找欧氏距离最近的目标点;普通最近邻允许多个源点选中同一目标点。
- 对固定的点对,分别减去质心,利用二维奇异值分解(SVD)求最优刚体旋转,再由质心计算平移;要求旋转行列式为 +1,避免镜像。
- 用新的 重新寻找最近邻,直到变化足够小或到达迭代上限(例如 30 轮)。
匹配索引会随位姿改变,因此最小化的是当前匹配下的局部误差,并不保证全局最优。对 个源点和 个目标点,直接穷举最近邻、迭代 轮的代价为 ;生产实现通常用空间索引、降采样和最大关联距离降低开销。

用相同点云复现两种收敛结果
下面使用 Python 3.10+ 与 NumPy 1.24+,仅实现二维点到点 ICP。目标地图含 7 个不对称点,合成扫描的真实“源到目标”变换是旋转 12°、平移 (1.2, -0.6) m。比较接近真值的初值和明显偏离的初值,算法与数据完全相同。
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")预期输出:
良好初值: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 二维激光前端时,可按以下顺序处理:
- 先检查坐标与时间:确认初始变换方向是扫描帧到目标地图帧,检查
LaserScan.header.stamp、激光外参和里程计时刻;运动时需考虑逐束扫描畸变。 - 控制对应搜索范围:例如点云按约 0.1 m 间距降采样后,从 0.3 m 最大关联距离、30 次迭代开始试验;这只是示例起点,需根据初始位姿误差、扫描密度及重叠范围调整。
- 同时检查重叠与残差:可先尝试“有效对应比例不少于 70%、对应 RMSE 不高于 0.15 m”的实验门槛,再使用真实日志标定;低重叠时不能只看小样本 RMSE。
- 验证运动合理性:将 ICP 修正量与轮式里程计、IMU 和环境结构对照。若残差较小但位姿突然跃迁,应拒绝该帧或转入粗到细、多初值、全局重定位,而不是直接写入地图。
ICP 适合在已有大致位姿时做局部精配准。如果初始化超出正确对应的捕获范围,应先改善里程计估计或做粗配准,再让 ICP 负责最后的小范围修正。