机器人 / 系统课时

点到线匹配怎样改善二维扫描对齐

点到线匹配怎样改善二维扫描对齐

在走廊或室内墙面附近,两帧激光往往打中同一面墙的不同位置。逐点硬配对容易把沿墙的采样错位算成配准误差;改用点到线残差后,可以集中校正垂直墙面的偏差,同时看清哪些运动仍然无法从扫描中判断。

学习目标

  • 区分点到点距离与点到线法向残差。
  • 用两面垂直墙的数值例子计算平移修正。
  • 判断何时需要舍弃线段匹配或引入里程计约束。

点到点为什么会惩罚沿墙错位

点到点 ICP 将变换后的当前帧点与参考帧中的离散点配对,再最小化两点间的欧氏距离。假设参考墙为 y=0y=0,当前点 P=(1.3,0.2)P=(1.3,0.2) 匹配到参考采样点 Q=(1.0,0)Q=(1.0,0),距离是 0.32+0.22≈0.361\sqrt{0.3^2+0.2^2}\approx0.361 m。

其中 0.3 m 是沿墙切线方向的差异:两帧雷达可能只是击中了同一平面的不同采样位置。参考点稀疏、角度采样错位时,强行压低这部分误差可能引入错误的位姿修正。点到点并非总是错,只是它未显式利用“这些点属于同一条墙”的结构。

用墙面法向定义点到线残差

对参考扫描中相邻且连续的点拟合短线段,取线段上的点 qi\mathbf q_i 和单位法向 ni\mathbf n_i。当前帧点 pi\mathbf p_i 经二维刚体变换后,其有符号法向残差为:

ri=niT(R(θ)pi+t−qi)r_i=\mathbf n_i^{\mathsf T}\bigl(R(\theta)\mathbf p_i+\mathbf t-\mathbf q_i\bigr)

优化目标通常是减小 ∑iwiri2\sum_i w_i r_i^2,其中权重 wiw_i 可以降低不可靠匹配的影响。上例的水平墙法向为 (0,1)(0,1),法向距离仅为 0.200 m;沿墙的 0.300 m 差异不进入这一项。

点到点与点到线扫描残差几何对比

注意残差是到有效线段的几何距离近似,不代表可以无限延长墙线。若投影落在线段外,或相邻参考点跨越门框、遮挡造成距离突变,应拒绝该匹配或重新选用合适的局部结构。

用两面墙计算一次平移修正

下面的 Python 3 示例不依赖第三方库。先比较固定对应点的两种距离;再假定角度已正确、只需估计平移:参考水平墙为 y=0y=0,参考竖直墙为 x=0x=0。各给出三个当前点的法向坐标,最小二乘修正就是相应坐标均值的相反数。

python
from math import hypot

# 固定同一组对应点,仅比较两种残差定义
p, q = (1.3, 0.2), (1.0, 0.0)
d_point = hypot(p[0] - q[0], p[1] - q[1])
d_line = abs(p[1] - q[1])  # 水平墙的单位法向为 (0, 1)
print(f"点到点: {d_point:.3f} m")
print(f"点到线: {d_line:.3f} m")

# 旋转误差视为零;两面互相垂直的墙分别提供 y、x 约束
wall_y = [0.18, 0.21, 0.21]
wall_x = [0.08, 0.10, 0.12]
tx = -sum(wall_x) / len(wall_x)
ty = -sum(wall_y) / len(wall_y)
before = sum(y * y for y in wall_y) + sum(x * x for x in wall_x)
after = sum((y + ty) ** 2 for y in wall_y) + sum((x + tx) ** 2 for x in wall_x)
print(f"平移修正: ({tx:.3f}, {ty:.3f}) m")
print(f"平方残差和: {before:.4f} -> {after:.4f} m²")

预期输出:

text
点到点: 0.361 m
点到线: 0.200 m
平移修正: (-0.100, -0.200) m
平方残差和: 0.1514 -> 0.0014 m²

水平墙提供 ty=−0.200t_y=-0.200 m,竖直墙提供 tx=−0.100t_x=-0.100 m,平方残差和明显降低。0.361 与 0.200 不是同一评价指标下的优劣分数,而是展示各自测量了什么;真实扫描配准还须同时求角度、更新对应关系。

放进二维扫描匹配的迭代过程

实际点到线 ICP(如 PL-ICP)仍然遵循“匹配、求解、重匹配”的迭代过程,可按下面顺序实现或排查:

  1. 将 /scan 中的有效距离和角度换成二维点,用里程计相对运动作为 (tx,ty,θ)(t_x,t_y,\theta) 初值;运动中的扫描先做好时间对齐或去畸变。
  2. 从参考帧相邻点构造短墙段并计算法向。试验时可先剔除端点间距超过 0.30 m 的候选线段,避免跨越明显断裂;阈值须随距离与角分辨率调整。
  3. 变换当前点,寻找邻近参考线段;只保留投影在线段内、法向残差绝对值不超过 0.15 m 的对应。门边、动态物体和远距离稀疏点宜另行筛除。
  4. 固定这一轮对应,使用高斯—牛顿或 LM 最小化加权法向残差平方和,更新平移与旋转,再重新匹配;可先设置最多 20 次迭代。
  5. 检查有效对应数量、法向 RMSE、最终位姿与里程计初值的差异,再在 RViz 2 观察墙线是否仍出现双影。阈值仅为近距离室内实验起点,不是通用验收标准。

什么情况下不能只信点到线

一条长直墙无法约束沿墙平移。 只有水平墙时,点到线残差对 txt_x 不敏感:即使残差降到零,机器人仍可能沿墙滑动。拐角、不同朝向的墙、里程计或 IMU 先验,才能补充缺失的信息。

此外,错误初值可能让扫描点配到另一面墙;短线段过稀、玻璃反射、人群移动或门洞两侧被误连,也会产生虚假的法向。此时应限制搜索范围、剔除异常线段并使用鲁棒损失,必要时结合角点的点到点约束。PL-ICP 并不保证在大初始偏差下总优于传统 ICP;可参见 Censi 的 PL-ICP 论文。

最终应同时看残差和约束几何:两面方向不同的可靠墙通常比一面拟合得极好的长墙更有定位价值。

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

目录与阅读设置

阅读设置

17

记录一个想法

点到线匹配怎样改善二维扫描对齐

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

登录哪都通