机器人 / 系统课时
关键帧和子地图应该在何时更新
关键帧和子地图应该在何时更新
机器人以 10 Hz 接收激光时,把每帧扫描都加入位姿图会增加匹配与优化开销,长期不更新又会失去新的几何约束。本篇用位移、转角和匹配质量制定关键帧插入规则,并说明重叠子地图的更新时机及 ROS 2 调参方法。
学习目标
- 根据相对上一关键帧的位移、转角和匹配质量决定是否插入新帧。
- 解释活动子地图的重叠更新与冻结条件。
- 用示例数据和
slam_toolbox参数检查插帧策略。
用位移和转角筛掉冗余帧
关键帧是保留为前端约束或位姿图节点的代表性扫描,并不意味着激光雷达只在插帧时工作。对于差速机器人,可先试用“相对上一关键帧平移达到 0.20 m,或最小夹角变化达到 10°”的或条件;比较对象不是上一条原始扫描,否则连续的小幅运动可能永远达不到阈值。
设机器人以 0.30 m/s 直行、雷达频率 10 Hz,每帧约前进 0.03 m,约第 7 帧才累计超过 0.20 m。原地旋转时,平移接近零,但转过 10° 仍应插帧。计算航向差要处理跨越 ±180° 的回绕,例如 179° 与 -179° 实际只差 2°。
距离阈值过小会在静止抖动时产生密集节点;阈值过大则可能错过门框、转角等关键几何特征。0.20 m 与 10° 是演示起点,应结合运动速度、雷达覆盖范围和走廊特征调整。
匹配不可靠时不要强制入图
运动量达标只是候选条件,还需检查扫描与局部地图的匹配。下文脚本自定义 0~1 的演示质量值,要求至少 0.60;这不是 ROS 2 或某个 SLAM 包通用的匹配评分阈值。真实系统应使用对应匹配器的得分定义、残差、内点率及协方差判断,不能直接移植 0.60。
例如候选帧平移 0.27 m、质量值仅 0.35:应暂缓把它当作可信约束写入地图,检查 TF 时间对齐、激光有效点比例及运动预测,必要时扩大搜索或重新定位。若用错误位姿直接更新栅格,墙面会被涂成重影;若长时间拒绝所有帧,则应进入匹配丢失的恢复流程,而不是无期限停更。
重叠子地图怎样完成交接
局部子地图保存一段行程内的障碍物与空闲空间观测,让当前扫描与邻近、结构较完整的地图匹配,而不是每次搜索全局地图。采用两张活动子地图重叠更新时,新图与旧图共享一段扫描,能够避免在交接边界骤然失去几何参考。
以 Cartographer 2D 的 submaps.num_range_data = 30 为例:一张新子地图先接收约 30 次有效扫描插入,随后启动下一张;旧图继续接收数据,累计到 60 次左右后冻结,不再修改内部栅格。新旧地图因而约有 30 次插入的重叠。这里的计数是经过前端运动过滤后实际插入的距离数据,不是原始雷达消息总数。

若机器人快速穿过结构重复的走廊,子地图过长会积累局部漂移;若每几帧就冻结,局部地图又缺乏足够特征。先以 30 作为该 Cartographer 参数的试验值,观察同一墙面是否变厚、匹配是否频繁失败,再调整有效扫描插入数量和运动过滤阈值。
用可运行脚本检查插帧规则
下面用 Python 3 标准库模拟七帧位姿。数据中的质量值是人工构造的,程序只验证“运动达标且匹配可信才插帧”的决策,不执行真实激光匹配或栅格更新。保存为 keyframes.py 后运行 python3 keyframes.py。
from math import hypot
def angle_gap(a, b):
# 将角度差折回 [-180, 180],避免跨界误判
return abs((a - b + 180) % 360 - 180)
# 时间、x、y、航向角(度)、示例匹配质量
scans = [
(0.0, 0.00, 0.0, 0, 0.91),
(0.1, 0.07, 0.0, 2, 0.94),
(0.2, 0.23, 0.0, 3, 0.88),
(0.3, 0.24, 0.0, 17, 0.82),
(0.4, 0.51, 0.0, 17, 0.35),
(0.5, 0.51, 0.0, 17, 0.92),
(0.6, 0.52, 0.0, 17, 0.90),
]
last_keyframe = None
for t, x, y, yaw, quality in scans:
if quality < 0.60:
print(f"{t:.1f}s: 匹配差,暂停插入并请求恢复")
continue
moved = (last_keyframe is None or
hypot(x - last_keyframe[0], y - last_keyframe[1]) >= 0.20 or
angle_gap(yaw, last_keyframe[2]) >= 10)
if moved:
last_keyframe = (x, y, yaw) # 仅在通过门控后更新比较基准
print(f"{t:.1f}s: 插入关键帧")
else:
print(f"{t:.1f}s: 跳过冗余帧")预期输出:
0.0s: 插入关键帧
0.1s: 跳过冗余帧
0.2s: 插入关键帧
0.3s: 插入关键帧
0.4s: 匹配差,暂停插入并请求恢复
0.5s: 插入关键帧
0.6s: 跳过冗余帧0.3 秒这一帧只比上一关键帧平移 0.01 m,但转过 14°,因此保留;0.4 秒即使平移充分,质量不足仍被拒绝,且不覆盖上一关键帧基准。
在 ROS 2 中对应到真实参数
slam_toolbox 与 Cartographer 的地图组织方式不同:它采用扫描链、位姿图和扫描缓冲,scan_buffer_size 并不是子地图大小。在已装好 slam_toolbox、/scan 与 odom 到机器人基座的 TF 可用的 ROS 2 环境中,可以先复制官方 mapper_params_online_async.yaml,再合并下列参数。
片段示例:仅展示需要调整的键;必须保留原文件中的帧名、雷达话题及其他建图设置,不能直接用此片段替换整份配置。
slam_toolbox:
ros__parameters:
minimum_travel_distance: 0.20
minimum_travel_heading: 0.1745
check_min_dist_and_heading_precisely: true
scan_buffer_size: 10
scan_buffer_maximum_scan_distance: 10.0其中 minimum_travel_heading 的单位是弧度,0.1745 约为 10°;check_min_dist_and_heading_precisely: true 用于精确检查距离或角度门槛。请把下方 <PATH> 替换为修改后的 YAML 绝对路径,在真实机器人运行时使用 use_sim_time:=false,仿真或 rosbag2 回放时按时钟来源改为 true。
ros2 launch slam_toolbox online_async_launch.py slam_params_file:=<PATH> use_sim_time:=false预期结果:节点进入活动状态并订阅 /scan;收到有效扫描及 TF 后持续建图。具体启动日志和地图更新时间取决于驱动、TF 与雷达数据。
在另一终端检查配置是否被实际加载:
ros2 param get /slam_toolbox minimum_travel_distance
ros2 param get /slam_toolbox minimum_travel_heading预期输出(加载了上述配置时):
Double value is: 0.2
Double value is: 0.1745再对同一段约 10 m、含转角的 rosbag2 数据比较调整前后轨迹:记录保留扫描数量、处理延迟、墙面重影及回环后地图形状。若帧数减少但门框出现错位,说明阈值已过大;若静止时仍产生大量节点,应优先检查里程计噪声与 TF,而非一味调大角度阈值。map_update_interval 只影响栅格地图发布周期,不能把它当成关键帧或子地图的冻结条件。
实际参数可对照 slam_toolbox 官方在线异步配置。