机器人 / 系统课时

扫描匹配如何求出两帧相对运动

扫描匹配如何求出两帧相对运动

机器人移动后,同一面墙在相邻两帧激光扫描中会落在不同位置。把两帧扫描转换成二维点集,就能寻找一个旋转加平移,使对应的墙面点尽量重合。本篇用 ICP 建立这个优化过程,并通过可运行的 NumPy 示例求出相对位姿。

学习目标

  • 明确前后两帧扫描的坐标方向,建立二维刚体对齐目标。
  • 理解 ICP 的最近邻关联、离群点剔除和迭代求解。
  • 运行示例,验证平移、转角和匹配残差,并识别失效场景。

统一点集与坐标方向

将上一帧扫描记为参考点集 PP,当前帧记为待对齐点集 QQ。目标是估计变换 Tk−1,kT_{k-1,k}:把当前激光坐标系中的点映射到上一帧激光坐标系,而不是直接求地图中的绝对位置。

ROS 2 的 sensor_msgs/msg/LaserScan 给出 ranges、angle_min、angle_increment 等字段。第 ii 束角度为 angle_min + i × angle_increment;有效距离 rir_i 投影为 xi=ricos⁡αix_i=r_i\cos\alpha_i、yi=risin⁡αiy_i=r_i\sin\alpha_i。例如距离为 2 m、角度为 90° 的点为 (0,2)(0,2) m。应过滤非有限值和不在 [range_min, range_max] 内的距离;实际处理还需使用采样时刻对应的 TF,避免运动导致的扫描扭曲。

二维刚体变换满足 p≈R(θ)q+tp\approx R(\theta)q+t,其中 θ\theta 为逆时针旋转角,t=(tx,ty)t=(t_x,t_y) 为当前激光原点在上一帧坐标中的位置。若雷达相对 base_link 有偏移,必须通过外参转换,才能将扫描间的变换解释为底盘运动。

最小二乘的几何目标

给当前帧点 qiq_i 在参考帧中选择最近的候选点 pc(i)p_{c(i)},记通过距离门限检查的对应点索引集合为 II。点到点扫描匹配最小化平方距离:

E(θ,t)=1∣I∣∑i∈I∥R(θ)qi+t−pc(i)∥2E(\theta,t)=\frac{1}{|I|}\sum_{i\in I}\|R(\theta)q_i+t-p_{c(i)}\|^2

这个目标包含三个未知量:txt_x、tyt_y(单位 m)和 θ\theta(内部计算用 rad)。不能简单比较相同激光束的距离,因为机器人平移或转向后,同一束角度通常照到不同的物体。

最近邻配对依赖当前的位姿估计,因此一次求解无法保证找到正确对应。扫描匹配通常先用轮式里程计提供接近真实值的初始变换,再反复更新点对和位姿。

ICP 怎样逐轮靠近

这里选用二维点到点 ICP(Iterative Closest Point)。一次迭代执行以下操作:

  1. 用初始 R,tR,t 把 QQ 变换到参考坐标;示例初值为旋转 6°、平移 (0.18,−0.06)(0.18,-0.06) m。
  2. 为每个变换后的当前点寻找 PP 中最近的点;示例只保留距离小于 0.5 m 的点对,减少错误关联。
  3. 对保留的原始当前点和参考点减去各自质心,利用二维 SVD 解最小平方误差的旋转,再以两组质心求平移;限制 det⁡(R)=1\det(R)=1,避免镜像反射。
  4. 更新 R,tR,t 并重新配对;变化小于阈值时停止,否则最多执行 15 轮。

相邻两帧激光扫描由错位到重合的 ICP 配准示意

每轮若对 NN 个当前点与 MM 个参考点直接穷举最近邻,配对成本为 O(NM)O(NM);实际工程可用 KD-tree 降低典型查询开销。ICP 通常收敛到局部最优,并不保证初值差很大时仍能正确对齐。

用 NumPy 验证相对变换

使用 Python 3.10+,先安装 NumPy(命令 python -m pip install numpy),把下面代码保存为 scan_icp.py,执行 python scan_icp.py。代码以已知位姿生成一组无噪声模拟扫描;真值只用于制造数据,求解函数只接收两组点与有误差的初值。

python
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()

预期输出:

text
tx=0.250 m, ty=-0.100 m, yaw=8.00 deg
RMSE=0.000000 m, pairs=8

此处求得的相对变换是当前帧到上一帧,不是反方向。RMSE 为零只因为示例点无噪声、无遮挡且对应关系明确;真实雷达数据一般不会得到零残差。

从残差判断结果是否可信

不能只看优化器报告“收敛”。应同时记录有效对应点数、重叠比例、匹配前后 RMSE,以及结果相对里程计初值的偏移。示例 8 个点全部匹配,且从初值 6° 修正到 8°,因此可用合成真值独立验收。

下列问题需分别处理:

  • 长直走廊:沿墙方向约束弱,即使点到线残差很低也可能发生滑动;需要转角、端点等额外结构或运动先验。
  • 动态物体或重叠不足:门限过宽会接受错误点对,门限过窄则对应不足;可过滤运动目标,按预期帧间运动和噪声设置匹配门限。
  • 初值太差或转弯太快:最近邻可能配错并陷入局部最优;缩小帧间位移、加入里程计初值,必要时采用更大范围的初始搜索。

将得到的 Tk−1,kT_{k-1,k} 按正确方向组合,才能累计局部轨迹;累计匹配仍会漂移,不能代替 SLAM 后端的回环与全局优化。

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

目录与阅读设置

阅读设置

17

记录一个想法

扫描匹配如何求出两帧相对运动

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

登录哪都通