机器人 / 系统课时
点到线匹配怎样改善二维扫描对齐
点到线匹配怎样改善二维扫描对齐
在走廊或室内墙面附近,两帧激光往往打中同一面墙的不同位置。逐点硬配对容易把沿墙的采样错位算成配准误差;改用点到线残差后,可以集中校正垂直墙面的偏差,同时看清哪些运动仍然无法从扫描中判断。
学习目标
- 区分点到点距离与点到线法向残差。
- 用两面垂直墙的数值例子计算平移修正。
- 判断何时需要舍弃线段匹配或引入里程计约束。
点到点为什么会惩罚沿墙错位
点到点 ICP 将变换后的当前帧点与参考帧中的离散点配对,再最小化两点间的欧氏距离。假设参考墙为 ,当前点 匹配到参考采样点 ,距离是 m。
其中 0.3 m 是沿墙切线方向的差异:两帧雷达可能只是击中了同一平面的不同采样位置。参考点稀疏、角度采样错位时,强行压低这部分误差可能引入错误的位姿修正。点到点并非总是错,只是它未显式利用“这些点属于同一条墙”的结构。
用墙面法向定义点到线残差
对参考扫描中相邻且连续的点拟合短线段,取线段上的点 和单位法向 。当前帧点 经二维刚体变换后,其有符号法向残差为:
优化目标通常是减小 ,其中权重 可以降低不可靠匹配的影响。上例的水平墙法向为 ,法向距离仅为 0.200 m;沿墙的 0.300 m 差异不进入这一项。

注意残差是到有效线段的几何距离近似,不代表可以无限延长墙线。若投影落在线段外,或相邻参考点跨越门框、遮挡造成距离突变,应拒绝该匹配或重新选用合适的局部结构。
用两面墙计算一次平移修正
下面的 Python 3 示例不依赖第三方库。先比较固定对应点的两种距离;再假定角度已正确、只需估计平移:参考水平墙为 ,参考竖直墙为 。各给出三个当前点的法向坐标,最小二乘修正就是相应坐标均值的相反数。
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²")预期输出:
点到点: 0.361 m
点到线: 0.200 m
平移修正: (-0.100, -0.200) m
平方残差和: 0.1514 -> 0.0014 m²水平墙提供 m,竖直墙提供 m,平方残差和明显降低。0.361 与 0.200 不是同一评价指标下的优劣分数,而是展示各自测量了什么;真实扫描配准还须同时求角度、更新对应关系。
放进二维扫描匹配的迭代过程
实际点到线 ICP(如 PL-ICP)仍然遵循“匹配、求解、重匹配”的迭代过程,可按下面顺序实现或排查:
- 将
/scan中的有效距离和角度换成二维点,用里程计相对运动作为 初值;运动中的扫描先做好时间对齐或去畸变。 - 从参考帧相邻点构造短墙段并计算法向。试验时可先剔除端点间距超过 0.30 m 的候选线段,避免跨越明显断裂;阈值须随距离与角分辨率调整。
- 变换当前点,寻找邻近参考线段;只保留投影在线段内、法向残差绝对值不超过 0.15 m 的对应。门边、动态物体和远距离稀疏点宜另行筛除。
- 固定这一轮对应,使用高斯—牛顿或 LM 最小化加权法向残差平方和,更新平移与旋转,再重新匹配;可先设置最多 20 次迭代。
- 检查有效对应数量、法向 RMSE、最终位姿与里程计初值的差异,再在 RViz 2 观察墙线是否仍出现双影。阈值仅为近距离室内实验起点,不是通用验收标准。
什么情况下不能只信点到线
一条长直墙无法约束沿墙平移。 只有水平墙时,点到线残差对 不敏感:即使残差降到零,机器人仍可能沿墙滑动。拐角、不同朝向的墙、里程计或 IMU 先验,才能补充缺失的信息。
此外,错误初值可能让扫描点配到另一面墙;短线段过稀、玻璃反射、人群移动或门洞两侧被误连,也会产生虚假的法向。此时应限制搜索范围、剔除异常线段并使用鲁棒损失,必要时结合角点的点到点约束。PL-ICP 并不保证在大初始偏差下总优于传统 ICP;可参见 Censi 的 PL-ICP 论文。
最终应同时看残差和约束几何:两面方向不同的可靠墙通常比一面拟合得极好的长墙更有定位价值。