机器人 / 系统课时

匹配残差如何暴露场景退化问题

匹配残差如何暴露场景退化问题

扫描匹配得到很小的误差,不代表机器人位姿一定正确:长走廊可能沿轴滑动,重复门洞可能匹配到相邻位置,移动行人则可能拉偏估计。本篇把匹配残差、几何约束和空间分布放在一起检查,并用可运行的数值实验区分三类问题。

学习目标

  • 解释低残差为何不等于位姿可靠,并读懂几何信息矩阵的弱约束方向。
  • 识别长走廊、重复结构和动态障碍物各自的残差特征。
  • 运行简化诊断脚本,决定何时接受、降权或拒绝一次扫描更新。

联合观察残差与几何约束

在二维点到线扫描匹配中,扫描点 pi\boldsymbol p_i 经位姿变换后,与地图参考点 qi\boldsymbol q_i 的法向误差为 ri=niT(Rpi+t−qi)r_i=\boldsymbol n_i^{\mathsf T}(R\boldsymbol p_i+\boldsymbol t-\boldsymbol q_i)。它测量的是垂直于局部墙面的偏差,不能单独说明沿墙方向的位置误差。

每次优化应同时保留三组诊断量:

  • 贴合程度:有效关联的残差 RMSE(单位 m)及绝对残差分位数。记录计算前是否已用鲁棒核削弱离群点;不同计算口径的得分不能直接比较。
  • 覆盖程度:通过关联门限的点数、占有效扫描点的比例,以及内点在不同激光角区的分布。只剩一段墙面,即使 RMSE 很小也很脆弱。
  • 几何约束:由残差对小位姿增量的雅可比 JJ 构建 H=JTWJH=J^{\mathsf T}WJ。检查其最弱特征方向;若最小特征值接近零,该方向的运动难以被扫描区分。

例如将点到线绝对残差超过 0.15 m 标为大残差,可以在下面的模拟数据中定位异常点;它不是 ROS 2 或 ICP 的通用默认门限。比较特征值前还须统一平移与旋转的尺度,例如用特征长度 L=1L=1 m 将角度增量换成等效位移,并固定权重、点数归一化方式。

三类场景为何给出误导性匹配

  1. 长走廊:约束方向缺失。 两侧平行墙的法向主要指向横向,横向位移和航向角通常能被约束,但沿走廊方向移动 0.4 m 仍可能保持很小的点到线残差。信息矩阵在纵向方向出现很小的特征值;仅提高 ICP 迭代次数无法补出缺失的几何信息。
  2. 重复门洞:存在多个低误差解。 相邻门洞间距若为 2 m,将扫描错移一个周期后也可能获得较好的局部匹配。某个解附近的 HH 甚至条件良好,因此必须比较多个初始位姿,并使用里程计预测和前后帧一致性排除假匹配。
  3. 移动障碍:残差局部成簇。 行人经过会使同一角度扇区出现一组较大的正负残差,内点比例下降。只看全局平均值可能掩盖异常位置;如果移动物体占据大量扫描,甚至可能形成错误却低残差的匹配。

长走廊、重复结构和动态障碍的匹配退化对比

用局部雅可比复现实验

下面用 Python 3 和 NumPy 构造六个走廊点、八个房间墙面点。假设对应关系已知,点坐标相对机器人中心、已旋转到地图轴向;脚本只检查局部几何与给定残差,不执行完整 ICP。雅可比的第三列表示小角度转动带来的法向位移。

python
import numpy as np

def report(name, pts, normals, residuals):
    # 2D 点到线残差对 dx、dy、dtheta 的一阶敏感度
    J = np.column_stack((normals[:, 0], normals[:, 1],
                         -normals[:, 0] * pts[:, 1] + normals[:, 1] * pts[:, 0]))
    eig = np.linalg.eigvalsh(J.T @ J)
    rmse = np.sqrt(np.mean(residuals ** 2))
    # 使用未截断残差检测动态遮挡,而非只看优化后的得分
    outlier = np.mean(np.abs(residuals) > 0.15)
    print(f"{name}: RMSE={rmse:.3f} m, 特征值比={eig[0] / eig[-1]:.2f}, 大残差占比={outlier:.0%}")

corridor = np.array([(x, y) for y in (-1., 1.) for x in (-1., 0., 1.)])
corridor_n = np.tile([0., 1.], (6, 1))
room = np.array([(-2., -1.), (-2., 1.), (2., -1.), (2., 1.),
                 (-1., -2.), (1., -2.), (-1., 2.), (1., 2.)])
room_n = np.array([[1., 0.]] * 4 + [[0., 1.]] * 4)

report("长走廊", corridor, corridor_n, np.full(6, 0.02))
report("静态房间", room, room_n, np.full(8, 0.02))
report("动态遮挡", room, room_n, np.array([0.02] * 6 + [0.35, -0.35]))

预期输出:

text
长走廊: RMSE=0.020 m, 特征值比=0.00, 大残差占比=0%
静态房间: RMSE=0.020 m, 特征值比=0.50, 大残差占比=0%
动态遮挡: RMSE=0.176 m, 特征值比=0.50, 大残差占比=25%

长走廊 RMSE 与静态房间相同,但沿轴平移对应的雅可比列全为零;动态遮挡虽然保留足够的几何方向,却有 25% 的大残差。此处特征值比是固定坐标尺度、等权重样本下的演示数值,真实系统应在剔除异常对应点或设置鲁棒权重后重算,不能直接以 0.50 作为合格标准。重复门洞的全局多解也无法从单个局部特征值比中发现。

按证据定位并修复

在 ROS 2 的同一段 rosbag2 回放中,先核对 /scan 的时间戳、laser 到 base_link 的 TF、odom 连续性,避免把时间错位误判为几何退化。然后固定同一组关联距离门限,保存每帧的原始残差、内点掩码、角度扇区统计、优化前后位姿,以及最弱信息方向。

  1. 如果 RMSE 很低但最弱方向几乎无约束,查看该方向是否与走廊轴一致。保留里程计或 IMU 的运动先验,对该方向的扫描校正降权;待转角、横墙或其他可区分结构出现再加强校正。
  2. 如果 RMSE 很低但位置发生整数周期跳变,从里程计预测位姿及前后各一个门洞间距启动多个匹配候选。例如相距 2 m 的两个候选 RMSE 仅差 0.01 m 时标记为歧义,不凭单帧得分强行确认。
  3. 如果 大残差集中于少数连续角区,检查该扇区是否有行人或推车;屏蔽动态点或采用鲁棒核后,用剩余静态点重新计算残差与 HH。静态约束不足时拒绝本帧扫描更新,而不是让动态物体决定地图坐标。

可先为一台经过标定的机器人试设告警条件:RMSE 高于 0.08 m、内点比例低于 60%,或几何最弱方向接近不可观测;这些只用于触发复核,最终阈值须由静态场景回放和误报率确定。验收时对比启用诊断前后的轨迹跳变次数、静态墙面的重影程度和走廊纵向漂移,确认既没有误收错误匹配,也没有把正常转弯全部拒绝。

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

目录与阅读设置

阅读设置

17

记录一个想法

匹配残差如何暴露场景退化问题

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

登录哪都通