机器人 / 系统课时
错误回环为什么会破坏整张地图
错误回环为什么会破坏整张地图
机器人把两段相似走廊误认成同一地点时,回环检测会向位姿图插入错误约束。全局优化可能移动多个位姿来迁就这条边,导致走廊缩短、墙体重影或地图折叠。本篇用可复现的数值实验和 ROS 2 配置说明如何发现并限制这种错误。
学习目标
- 解释感知混淆怎样通过错误回环影响整张位姿图。
- 用数值实验比较普通最小二乘与鲁棒核的结果。
- 结合
slam_toolbox参数筛选候选回环并验证修复效果。
一条错误约束如何拖动整条轨迹
在二维激光 SLAM 中,关键帧是位姿节点;相邻帧的里程计、扫描匹配形成局部约束,回环边连接间隔较远的节点。重复门洞、对称走廊或相似货架可能让扫描匹配给出很高的相似度,却把两处不同地点错误地认成同一地点。
全局图优化会同时降低局部边和回环边的加权残差。若错误回环被赋予较高置信度,优化器可能把原来分布在数米范围的节点挤到一起,而不是舍弃这条边;由这些位姿投影的激光点也会随之错位。

考虑一个故意简化的一维案例:机器人从 沿直走廊每次前进 1 米,共有四条正确的里程计边,真实终点是 米。错误回环却声称终点与起点相距 0 米。固定起点后,优化目标是:
其中 是回环残差的损失函数。若它也采用平方损失,五个节点分别位于 0、0.2、0.4、0.6、0.8 米,四米走廊被压成 0.8 米。真实二维位姿图还会同时改变航向,产生弯曲、交叉墙线等更复杂的畸变。
用 Python 复现普通优化与鲁棒核
下面使用 Python 3 标准库求解上面的对称一维模型,不依赖 ROS 2。四条里程计边和回环边采用相同权重;只有错误回环使用 Huber 损失时,超过阈值的残差不再按平方速度增加惩罚。
def solve(n, delta=None):
# 每条里程计边测得1米,错误回环却声称首尾位移为0米
least_squares_step = 1.0 / (1.0 + n)
if delta is None or n * least_squares_step <= delta:
step = least_squares_step
else:
# 仅对回环边使用Huber,超阈值后其梯度被限制为delta
step = 1.0 - delta
return [round(i * step, 1) for i in range(n + 1)]
n = 4
print("仅里程计:", [float(i) for i in range(n + 1)])
print("错误回环+二乘:", solve(n))
print("错误回环+Huber:", solve(n, delta=0.1))预期输出:
仅里程计: [0.0, 1.0, 2.0, 3.0, 4.0]
错误回环+二乘: [0.0, 0.2, 0.4, 0.6, 0.8]
错误回环+Huber: [0.0, 0.9, 1.8, 2.7, 3.6]使用示例阈值 米后,Huber 将错误回环对终点的拉扯限制到 0.4 米,但并未完全排除它。这里的阈值属于未归一化的一维教学模型,不能直接当作真实图优化器的配置值;实际残差常按信息矩阵加权。
在接纳回环之前做几何核验
筛选顺序应是候选生成、局部匹配、跨帧核验、约束入图,而不是“匹配分数超过阈值就立即优化”。以下数值可作为室内重复走廊数据的初始实验条件,需按里程计漂移和场景重复程度调整。
- 限制候选范围:先只比较时间间隔超过 30 秒、当前估计位置相距 2 米以内的非相邻关键帧。范围过紧会漏掉漂移较大的真实回环,因此必须与回访路线核对。
- 检查独立几何证据:除相关匹配分数外,核查至少两处稳定结构,例如墙角和立柱;行人或单排重复门洞不能单独证明回环。
- 比较运动一致性:若匹配宣称两帧位移约为 0 米,但里程计预测相差 3 米,且该方向累计标准差仅 0.3 米,就有约 10 倍标准差的冲突,应暂缓添加约束。长时间运行时要随不确定度增大而放宽容差。
- 复查邻近扫描:让回环前后各连续几帧的相对位姿也能支持同一对齐,而不是仅靠一次偶然高分的扫描匹配。跨帧一致性属于额外验证逻辑,并非下文所有参数都能直接实现。
在 ROS 2 中收紧回环配置
slam_toolbox 的 Ceres 后端支持 HuberLoss,也暴露了回环搜索范围、匹配响应门槛等参数。以下是片段示例:将字段合并进项目实际加载的 slam_toolbox 参数 YAML,保留已有 odom_frame、map_frame、base_frame 和 scan_topic 等设置。
slam_toolbox:
ros__parameters:
solver_plugin: solver_plugins::CeresSolver
ceres_loss_function: HuberLoss
do_loop_closing: true
loop_search_maximum_distance: 2.0
loop_match_minimum_chain_size: 12
loop_match_minimum_response_coarse: 0.40
loop_match_minimum_response_fine: 0.55这是比官方 ROS 2 示例配置更保守的测试起点:搜索距离从 3.0 米收至 2.0 米,匹配响应阈值由 0.35/0.45 提高至 0.40/0.55。阈值并非概率,不能解释为“55% 正确”;实际取值应通过同一组录包对照评估。
修改后以加载该 YAML 的配置重启节点。在默认节点名为 /slam_toolbox 的情况下执行:
ros2 param get /slam_toolbox ceres_loss_function预期输出:
String value is: HuberLoss如果节点使用了命名空间,需将 /slam_toolbox 换成实际完整节点名。鲁棒核只能降低高残差约束的影响,不能判定地点身份;如果错边与其他错误边相互支持,最终残差变小,地图仍可能变形。
用同一段数据确认地图是否被修复
发生“地图突然吸合”或墙线成片错位时,不应只盯着最后的地图图片。请保存包含 /scan、/tf、/tf_static 和实际使用的里程计数据的 rosbag2,再固定同一录包和同一坐标系配置进行 A/B 对照。
- 第一轮关闭
do_loop_closing,观察相邻扫描是否基本对齐;若本来就错位,应先排查 TF、时间同步和里程计,而不是调回环阈值。 - 第二轮开启回环,分别比较原参数与收紧参数。记录首次出现全局位姿跳变的时间、候选回环两端所在位置和优化前后走廊长度。
- 在 RViz 2 中同时查看
Map、LaserScan、TF;重点检查墙体是否重影、平行走廊是否合并。对示例而言,真实 4 米路段不能被缩短成 0.8 米。 - 如果定位到错误约束,应从位姿图中剔除该边并重新优化;无法安全编辑已有图时,用修改后的策略重放原始数据重建地图。仅平移导出的栅格图无法修复错误位姿图。
正确回环通常会让重访区域对齐、累计漂移减少;错误回环却可能让多个远处路段一起改变形状。验收时既要看闭环区域,也要检查未参与匹配的长走廊和房间尺寸是否保持合理。