机器人 / 系统课时

回环检测如何识别曾经到过的位置

回环检测如何识别曾经到过的位置

机器人绕行一圈后,里程计的累计误差可能让起点和终点在地图上错开。本篇通过历史扫描候选检索与几何一致性验证,判断机器人是否真正重访某处,并决定何时向位姿图添加回环约束。

学习目标

  • 区分回环候选生成与扫描匹配确认的职责。
  • 能设置空间搜索、时间排除和几何质量检查条件。
  • 能在 ROS 2 中核查回环相关参数并观察地图变化。

位置接近不代表回环成立

二维激光 SLAM 把带位姿的历史扫描保存为关键帧。回到旧地点时,即使激光形状相似,累计里程计误差也可能让当前估计位置偏移;反过来,两条平行走廊位置很近,却不是同一个地方。

假设当前关键帧编号为 100,估计位置是 (5.0, 5.0) m;历史帧 5 位于 (5.8, 5.5) m,帧 26 位于 (6.0, 5.8) m。它们距当前位置分别约 0.94 m 和 1.28 m,都会进入 3 m 搜索范围,但只有真正对齐的扫描才能形成回环。

候选只是值得检查的历史帧。通过几何验证后,系统才得到当前帧与历史帧之间的相对位姿测量,将其作为非相邻约束加入位姿图;全局轨迹和地图的修正由后续图优化完成。

检索历史候选

一个适合二维室内激光地图的检索策略如下。这里的数值用于展示筛选逻辑,并非所有 SLAM 系统的通用默认值。

  1. 保存历史关键帧的编号、估计位姿和对应扫描;新扫描到来后先排除最近 20 个关键帧,避免把连续观测误认为回环。
  2. 以当前位置为中心,在历史位姿中查找半径 3.0 m 内的帧。可用空间网格或 KD-tree 加快查询。
  3. 若候选过多,优先处理空间距离较小、扫描结构较相似的前 5 个,再分别进行扫描匹配,不以距离排名直接认定回环。

只按位姿半径检索可能漏掉漂移超过 3 m 的真实重访;支持地点描述子的系统可以另用扫描形状特征召回候选,再交给几何验证。不要把描述子相似当成几何匹配成功。ROS 2 的 slam_toolbox 有自己的空间搜索与扫描相关性匹配参数,不能把上述前 5 个候选的示例策略当成它的配置选项。

历史扫描候选检索与几何验证的分支关系

用几何一致性确认回环

对每个候选,扫描匹配求解当前扫描到历史扫描的二维刚体变换 T=(Δx,Δy,Δθ)T=(\Delta x,\Delta y,\Delta\theta),再检查是否有足够多的静态结构在同一变换下对齐。可以把对应点的均方根残差作为直观指标:

RMSE=1N∑k=1N∥Tpk−qk∥2\mathrm{RMSE}=\sqrt{\frac{1}{N}\sum_{k=1}^{N}\lVert Tp_k-q_k\rVert^2}

以下 Python 3 示例同时演示候选搜索和残差筛选。为便于独立运行,它假设四个特征点已正确一一对应、两帧朝向相同,因此只估计平移;真实激光扫描需要搜索点或线对应关系、估计旋转,并排除动态障碍与退化匹配。

python
from math import hypot, sqrt

current_id, current_xy = 100, (5.0, 5.0)
current_points = [(0.2, 0.1), (2.2, 0.1), (2.2, 1.1), (0.2, 2.1)]
history = [
    (5, (5.8, 5.5), [(0., 0.), (2., 0.), (2., 1.), (0., 2.)]),
    (26, (6.0, 5.8), [(0., 0.), (2., 0.), (2., 1.), (0., 3.)]),
    (98, (5.1, 5.1), [(0., 0.), (2., 0.), (2., 1.), (0., 2.)]),
]

def rmse_translated(src, dst):
    # 已知点对应时,质心之差给出最小二乘平移
    dx = sum(q[0] - p[0] for p, q in zip(src, dst)) / len(src)
    dy = sum(q[1] - p[1] for p, q in zip(src, dst)) / len(src)
    return sqrt(sum((p[0]+dx-q[0])**2 + (p[1]+dy-q[1])**2
                    for p, q in zip(src, dst)) / len(src))

for frame_id, position, points in history:
    # 先排除时间近邻,再按位置生成候选
    if current_id - frame_id < 20 or hypot(current_xy[0]-position[0], current_xy[1]-position[1]) > 3.0:
        continue
    error = rmse_translated(current_points, points)
    state = "通过" if error <= 0.25 else "拒绝"
    print(f"历史帧 {frame_id}: RMSE={error:.3f} m, {state}")

预期输出:

text
历史帧 5: RMSE=0.000 m, 通过
历史帧 26: RMSE=0.433 m, 拒绝

0.25 m 是本算例的教学阈值,并非 slam_toolbox 的匹配阈值。实际还要检查内点数量、覆盖区域、匹配得分、相对位姿是否合理,以及匹配结果是否与其他图约束冲突。长直走廊或重复货架可能在局部给出看似不错的残差,此时应拒绝证据不足的回环。

在 ROS 2 中检查回环行为

以 ROS 2 Humble/Jazzy 常见的 slam_toolbox 二维建图配置为例,do_loop_closing 控制是否尝试回环,loop_search_maximum_distance 限制搜索距离,loop_match_minimum_response_coarse 与 loop_match_minimum_response_fine 则约束粗、精匹配响应。响应阈值不是以米为单位的残差。

确保 slam_toolbox 正在运行且节点名为 /slam_toolbox,在另一个已加载 ROS 2 环境的终端执行:

bash
ros2 param get /slam_toolbox do_loop_closing
ros2 param get /slam_toolbox loop_search_maximum_distance
ros2 param get /slam_toolbox loop_match_minimum_response_fine

预期输出(当启动配置分别设为 true、3.0、0.45 时):

text
Boolean value is: True
Double value is: 3.0
Double value is: 0.45

这些是 slam_toolbox 官方示例配置中使用过的参数值;实际输出取决于已加载配置。若提示节点不存在,先用 ros2 node list 检查节点名和运行状态,不要直接调大回环阈值。

在 RViz 2 设置 Fixed Frame 为 map,添加 Map 显示并订阅 /map,沿有明显墙角特征的闭合路线返回起点,比较重访前后的墙面重影和轨迹接合情况。若始终无法闭合,先排查扫描质量、TF 和累计漂移是否导致候选漏检;若地图突然扭曲,优先怀疑重复结构造成的误匹配,而不是继续放宽验证条件。

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

目录与阅读设置

阅读设置

17

记录一个想法

回环检测如何识别曾经到过的位置

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

登录哪都通