机器人 / 系统课时
如何保存和恢复可继续优化的位姿图
如何保存和恢复可继续优化的位姿图
建图暂停后,如果只留下栅格图片,下一次就无法直接恢复扫描约束并继续优化。本篇用 ROS 2 的 slam_toolbox 保存位姿图和激光数据,再在新会话中载入,检查旧地图是否保留以及新扫描能否继续扩展地图。
学习目标
- 区分用于导航的静态栅格地图与可继续建图的序列化状态。
- 调用 slam_toolbox 服务,保存并检查
.posegraph、.data文件。 - 在重启后加载位姿图,验证旧约束与新观测能够衔接。
静态地图与位姿图有什么不同
同一次二维激光建图可以产生两套文件,但用途不能互换:
| 保存方式 | 典型文件 | 保留的信息与用途 |
|---|---|---|
Nav2 map_saver_cli 导出 |
floor1.pgm、floor1.yaml |
占据栅格、分辨率与地图原点;适合地图服务器和 AMCL |
| slam_toolbox 序列化 | floor1.posegraph、floor1.data |
位姿节点、扫描数据、图约束及相关元数据;可恢复后继续匹配与优化 |
栅格图是当前优化结果的一次栅格化快照,无法凭图片还原回环边与原始激光观测。需要下次接着建图时,应保留序列化文件对;若还要给 Nav2 交付静态地图,可额外导出栅格文件。

在建图进程中保存状态
以下操作适用于安装了 slam_toolbox 与 nav2_map_server 的 ROS 2 Humble/Jazzy 常见环境。假设 slam_toolbox 正以 mode: mapping 运行,/scan 持续发布,odom 到机器人底盘及激光坐标系的 TF 正常,RViz 已能看到非空地图。先确认序列化和反序列化服务均已激活:
ros2 service type /slam_toolbox/serialize_map
ros2 service type /slam_toolbox/deserialize_map预期输出:
slam_toolbox/srv/SerializePoseGraph
slam_toolbox/srv/DeserializePoseGraph保持建图节点运行,在同一台机器的另一终端执行。filename 传不带扩展名的绝对路径,目录必须提前存在;服务在运行 slam_toolbox 的主机上写文件。
MAP_BASE="$HOME/slam_maps/floor1"
mkdir -p "$(dirname "$MAP_BASE")"
# 保存图结构与关联扫描,而不是只保存 /map 栅格
ros2 service call /slam_toolbox/serialize_map \
slam_toolbox/srv/SerializePoseGraph "{filename: '$MAP_BASE'}"预期输出中的响应部分:
response:
slam_toolbox.srv.SerializePoseGraph_Response(result=0)result=0 表示服务报告成功;还要检查两个文件均非空:
test -s "$MAP_BASE.posegraph" && test -s "$MAP_BASE.data" && echo "posegraph+data OK"预期输出:
posegraph+data OK重启后加载并继续优化
先停止旧建图节点,确保两份文件已备份。用原有配置重新启动 mapping 模式 的 slam_toolbox,但先不要设置 map_file_name 自动加载,也不要边建新图边导入旧图。使用实机系统时间时,可以在一个终端启动:
ros2 launch slam_toolbox online_async_launch.py use_sim_time:=false预期结果:启动进程保持运行,并进入 active 生命周期状态;不同 ROS 2 版本的启动日志可能不同。仿真或回放使用 /clock 时,应改为与原会话一致的 use_sim_time:=true。如果之前使用了自定义 slam_params_file,重启时也必须指定原配置文件,以保持传感器话题和 TF 坐标系一致。
让机器人回到上次开始建图的位置附近,在另一个已加载 ROS 2 环境的终端调用恢复服务。match_type: 1 表示从序列化图的第一个节点附近开始关联扫描。
MAP_BASE="$HOME/slam_maps/floor1"
ros2 service call /slam_toolbox/deserialize_map \
slam_toolbox/srv/DeserializePoseGraph \
"{filename: '$MAP_BASE', match_type: 1, initial_pose: {orientation: {w: 1.0}}}"预期输出中的响应部分:
response:
slam_toolbox.srv.DeserializePoseGraph_Response(result=0)若机器人实际位于旧图中已知的其他位置,应在新启动的空白建图节点上改用 match_type: 2,并填写 initial_pose.position.x/y 及 initial_pose.orientation 四元数。例如位置 (2.0, 1.0) 米、朝向 0° 对应 position: {x: 2.0, y: 1.0} 与 orientation: {w: 1.0}。
注意,initial_pose 的类型是 geometry_msgs/Pose,不是 {x, y, theta}。两种恢复方式二选一,不要连续调用覆盖正在处理的地图。
验证恢复是否成功
在 RViz 中把 Fixed Frame 设为 map,同时显示 /map 和 /scan。恢复后先确认原来的走廊、墙角仍在;再让机器人沿旧图可匹配区域行走约 1~2 米,进入未扫描过的小区域,检查新区域逐步加入且原有墙体没有明显重影。保存为 floor1_v2 并核对新的 .posegraph、.data 文件,才能证明第二次建图状态也可持久化。
| 症状 | 具体检查与修正 |
|---|---|
序列化 result=255 或缺少 .data |
检查服务端目录、写权限及磁盘容量,修复后重存 |
反序列化 result=3 |
检查文件基名、不带扩展名的路径和 .posegraph/.data 是否配对且未损坏 |
result=0 但扫描不接入旧地图 |
核对当前 /scan、TF、时间源与旧地图起点;位置不同则改用已知初始位姿的 match_type: 2 |
| 地图被重复叠加或错位 | 停止移动,重新启动空白 mapping 节点,只加载一次并检查初始位置 |
result=0 只说明读写服务成功,不等于已经完成重定位或增加新节点。序列化文件还可能受 slam_toolbox 版本和数据格式变更影响;长期归档时同时记录 ROS 2 发行版、slam_toolbox 版本和机器人传感器配置。