机器人 / 系统课时
扫描匹配如何求出两帧相对运动
扫描匹配如何求出两帧相对运动
机器人移动后,同一面墙在相邻两帧激光扫描中会落在不同位置。把两帧扫描转换成二维点集,就能寻找一个旋转加平移,使对应的墙面点尽量重合。本篇用 ICP 建立这个优化过程,并通过可运行的 NumPy 示例求出相对位姿。
学习目标
- 明确前后两帧扫描的坐标方向,建立二维刚体对齐目标。
- 理解 ICP 的最近邻关联、离群点剔除和迭代求解。
- 运行示例,验证平移、转角和匹配残差,并识别失效场景。
统一点集与坐标方向
将上一帧扫描记为参考点集 ,当前帧记为待对齐点集 。目标是估计变换 :把当前激光坐标系中的点映射到上一帧激光坐标系,而不是直接求地图中的绝对位置。
ROS 2 的 sensor_msgs/msg/LaserScan 给出 ranges、angle_min、angle_increment 等字段。第 束角度为 angle_min + i × angle_increment;有效距离 投影为 、。例如距离为 2 m、角度为 90° 的点为 m。应过滤非有限值和不在 [range_min, range_max] 内的距离;实际处理还需使用采样时刻对应的 TF,避免运动导致的扫描扭曲。
二维刚体变换满足 ,其中 为逆时针旋转角, 为当前激光原点在上一帧坐标中的位置。若雷达相对 base_link 有偏移,必须通过外参转换,才能将扫描间的变换解释为底盘运动。
最小二乘的几何目标
给当前帧点 在参考帧中选择最近的候选点 ,记通过距离门限检查的对应点索引集合为 。点到点扫描匹配最小化平方距离:
这个目标包含三个未知量:、(单位 m)和 (内部计算用 rad)。不能简单比较相同激光束的距离,因为机器人平移或转向后,同一束角度通常照到不同的物体。
最近邻配对依赖当前的位姿估计,因此一次求解无法保证找到正确对应。扫描匹配通常先用轮式里程计提供接近真实值的初始变换,再反复更新点对和位姿。
ICP 怎样逐轮靠近
这里选用二维点到点 ICP(Iterative Closest Point)。一次迭代执行以下操作:
- 用初始 把 变换到参考坐标;示例初值为旋转 6°、平移 m。
- 为每个变换后的当前点寻找 中最近的点;示例只保留距离小于 0.5 m 的点对,减少错误关联。
- 对保留的原始当前点和参考点减去各自质心,利用二维 SVD 解最小平方误差的旋转,再以两组质心求平移;限制 ,避免镜像反射。
- 更新 并重新配对;变化小于阈值时停止,否则最多执行 15 轮。

每轮若对 个当前点与 个参考点直接穷举最近邻,配对成本为 ;实际工程可用 KD-tree 降低典型查询开销。ICP 通常收敛到局部最优,并不保证初值差很大时仍能正确对齐。
用 NumPy 验证相对变换
使用 Python 3.10+,先安装 NumPy(命令 python -m pip install numpy),把下面代码保存为 scan_icp.py,执行 python scan_icp.py。代码以已知位姿生成一组无噪声模拟扫描;真值只用于制造数据,求解函数只接收两组点与有误差的初值。
import numpy as np
def rot(angle):
c, s = np.cos(angle), np.sin(angle)
return np.array([[c, -s], [s, c]])
def icp(reference, current, r0, t0, gate=0.5, max_iter=15):
r, t = r0.copy(), t0.copy()
for _ in range(max_iter):
# 将当前点转换到参考坐标后建立最近邻对应
warped = current @ r.T + t
d2 = ((warped[:, None, :] - reference[None, :, :]) ** 2).sum(axis=2)
ids = d2.argmin(axis=1)
keep = d2[np.arange(len(current)), ids] < gate ** 2
if keep.sum() < 3:
raise ValueError("有效点对不足")
a, b = current[keep], reference[ids[keep]]
ca, cb = a.mean(axis=0), b.mean(axis=0)
# SVD 求刚体旋转,镜像解需要纠正
u, _, vt = np.linalg.svd((a - ca).T @ (b - cb))
new_r = vt.T @ u.T
if np.linalg.det(new_r) < 0:
vt[-1, :] *= -1
new_r = vt.T @ u.T
new_t = cb - new_r @ ca
done = (np.linalg.norm(new_t - t) < 1e-10
and np.linalg.norm(new_r - r) < 1e-10)
r, t = new_r, new_t
if done:
break
return r, t, int(keep.sum())
def main():
reference = np.array([
[0.0, 0.0], [1.2, 0.1], [2.8, 0.3], [0.3, 1.5],
[1.7, 1.8], [3.0, 1.3], [0.8, 2.9], [2.6, 2.7]
])
# 真值只用于合成数据,不传给 ICP
truth_r, truth_t = rot(np.deg2rad(8.0)), np.array([0.25, -0.10])
current = (reference - truth_t) @ truth_r
r, t, pairs = icp(reference, current, rot(np.deg2rad(6.0)),
np.array([0.18, -0.06]))
residual = current @ r.T + t - reference
rmse = np.sqrt(np.mean(np.sum(residual ** 2, axis=1)))
yaw = np.rad2deg(np.arctan2(r[1, 0], r[0, 0]))
print(f"tx={t[0]:.3f} m, ty={t[1]:.3f} m, yaw={yaw:.2f} deg")
print(f"RMSE={rmse:.6f} m, pairs={pairs}")
if __name__ == "__main__":
main()预期输出:
tx=0.250 m, ty=-0.100 m, yaw=8.00 deg
RMSE=0.000000 m, pairs=8此处求得的相对变换是当前帧到上一帧,不是反方向。RMSE 为零只因为示例点无噪声、无遮挡且对应关系明确;真实雷达数据一般不会得到零残差。
从残差判断结果是否可信
不能只看优化器报告“收敛”。应同时记录有效对应点数、重叠比例、匹配前后 RMSE,以及结果相对里程计初值的偏移。示例 8 个点全部匹配,且从初值 6° 修正到 8°,因此可用合成真值独立验收。
下列问题需分别处理:
- 长直走廊:沿墙方向约束弱,即使点到线残差很低也可能发生滑动;需要转角、端点等额外结构或运动先验。
- 动态物体或重叠不足:门限过宽会接受错误点对,门限过窄则对应不足;可过滤运动目标,按预期帧间运动和噪声设置匹配门限。
- 初值太差或转弯太快:最近邻可能配错并陷入局部最优;缩小帧间位移、加入里程计初值,必要时采用更大范围的初始搜索。
将得到的 按正确方向组合,才能累计局部轨迹;累计匹配仍会漂移,不能代替 SLAM 后端的回环与全局优化。