机器人 / 系统课时

关键帧和子地图应该在何时更新

关键帧和子地图应该在何时更新

机器人以 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。

python
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: 跳过冗余帧")

预期输出:

text
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,再合并下列参数。

片段示例:仅展示需要调整的键;必须保留原文件中的帧名、雷达话题及其他建图设置,不能直接用此片段替换整份配置。

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。

bash
ros2 launch slam_toolbox online_async_launch.py slam_params_file:=<PATH> use_sim_time:=false

预期结果:节点进入活动状态并订阅 /scan;收到有效扫描及 TF 后持续建图。具体启动日志和地图更新时间取决于驱动、TF 与雷达数据。

在另一终端检查配置是否被实际加载:

bash
ros2 param get /slam_toolbox minimum_travel_distance
ros2 param get /slam_toolbox minimum_travel_heading

预期输出(加载了上述配置时):

text
Double value is: 0.2
Double value is: 0.1745

再对同一段约 10 m、含转角的 rosbag2 数据比较调整前后轨迹:记录保留扫描数量、处理延迟、墙面重影及回环后地图形状。若帧数减少但门框出现错位,说明阈值已过大;若静止时仍产生大量节点,应优先检查里程计噪声与 TF,而非一味调大角度阈值。map_update_interval 只影响栅格地图发布周期,不能把它当成关键帧或子地图的冻结条件。

实际参数可对照 slam_toolbox 官方在线异步配置。

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

目录与阅读设置

阅读设置

17

记录一个想法

关键帧和子地图应该在何时更新

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

登录哪都通