SLAM 位姿优化实战（二）：从激光雷达到 ROS 2 二维地图

本文以 ROS 2、TurtleBot3、Gazebo 和 slam_toolbox 为例，跑通一条完整的二维激光 SLAM 流程：启动仿真机器人、检查激光雷达与 TF、在线构建地图、观察回环优化，并保存占据栅格地图和位姿图。

写在前面

上一篇文章从旋转矩阵、四元数、李群李代数一直讲到了高斯—牛顿与 LM，试图回答一个问题：

机器人怎样表示自己的位姿，又怎样根据观测误差，一点一点修正这个位姿？

理论链条可以写得很完整，但真正打开一个 SLAM 工程时，我们通常不会亲手计算每一个雅可比，也不会自己实现整个非线性优化器。更多时候，我们面对的是激光雷达话题、TF 坐标变换、参数文件、RViz，以及一张随着机器人运动不断生长的地图。

所以这一篇不再继续堆公式，而是把上一篇的理论放进一个可以运行的 ROS 2 系统中。我们将使用 slam_toolbox 完成二维激光 SLAM，并重点观察三件事：

1. 激光雷达数据怎样变成二维地图；
2. 里程计误差为什么会让地图逐渐变形；
3. 回环被发现后，位姿图优化为什么会让整张地图“跳一下”。

需要先说明：slam_toolbox 是二维激光 SLAM 工具，不是视觉 SLAM 系统。它使用二维激光扫描和里程计构建位姿图，再通过扫描匹配、回环检测和图优化修正机器人轨迹。虽然传感器从相机变成了激光雷达，背后的“观测—残差—优化—更新位姿”主线并没有改变。

---

1. 最终要完成什么

本文的目标不是搭建一套完整导航系统，而是先把建图闭环跑通：

Gazebo 中的 TurtleBot3
        │
        ├── /scan：二维激光扫描
        ├── /odom：轮式里程计
        └── TF：odom → base_footprint → base_scan（TurtleBot3）
                         │
                         ▼
                  slam_toolbox
                         │
        ┌────────────────┼────────────────┐
        ▼                ▼                ▼
      /map          map → odom        位姿图与回环
        │
        ▼
  保存 map.yaml + map.pgm

完成后，我们应该能够：

• 在 RViz 中看到激光扫描与机器人模型正确重合；
• 遥控机器人移动，并看到 /map 持续更新；
• 让机器人回到起点附近，观察回环后的地图修正；
• 保存可以交给 Nav2 或 AMCL 使用的二维栅格地图；
• 保存 slam_toolbox 的序列化位姿图，以便以后继续建图。

---

2. 实验环境

本文采用下面的组合说明流程：

• ROS 2 Lyrical
• 源码编译到 /opt/nav2 的 Nav2 与 slam_toolbox
• TurtleBot3 Burger
• Gazebo Sim
• slam_toolbox
• RViz2

如果使用 ROS 2 Jazzy、Humble 或其他发行版，核心流程相同，但环境脚本、软件包名称、Gazebo 版本和部分启动文件可能不同。下面的命令以本文的 Lyrical + /opt/nav2 源码环境为准；读者必须根据自己的 ROS 2 发行版和工作空间位置修改 source 命令，不能原样照搬。

每次打开新终端后，先加载 ROS 2 环境：

source /opt/ros/lyrical/setup.bash

本文中的 slam_toolbox 随 Nav2 源码编译在 /opt/nav2，因此还要加载这个覆盖工作空间：

source /opt/nav2/setup.bash

可以用下面的命令确认当前终端实际找到的是哪一个 slam_toolbox：

ros2 pkg prefix slam_toolbox

在本文环境中，结果应指向 /opt/nav2。如果输出的是另一个工作空间，说明当前终端的覆盖顺序与预期不一致；如果提示找不到软件包，则说明 /opt/nav2/setup.bash 没有成功加载。

为了减少重复，可以把环境命令写入 ~/.bashrc，但初学阶段建议先手动执行，这样更容易判断当前终端加载了哪些工作空间。

2.1 安装核心软件包

本文环境中的 Nav2 与 slam_toolbox 已经从源码编译到 /opt/nav2。如果使用提供二进制软件包的官方 ROS 2 发行版，可以根据自己的发行版安装对应软件包，例如：

sudo apt update
sudo apt install ros-$ROS_DISTRO-slam-toolbox \
                 ros-$ROS_DISTRO-navigation2 \
                 ros-$ROS_DISTRO-nav2-bringup \
                 ros-$ROS_DISTRO-turtlebot3-teleop

并不是每个发行版都会提供上述所有二进制包。如果使用 Lyrical 或自定义环境，应以实际的源码工作空间为准，不要为了照搬命令而额外混入 Jazzy 软件包。

TurtleBot3 与 Gazebo Sim 的完整安装步骤较长，建议直接参考文末的 ROBOTIS 官方文档。本文假设下面的仿真启动命令已经能够正常运行。

---

3. 先理解 slam_toolbox 需要什么

运行 SLAM 之前，不要急着输入启动命令。先看清 slam_toolbox 的输入和输出，后面排错会容易很多。

3.1 输入一：激光扫描 /scan

二维激光雷达发布 sensor_msgs/msg/LaserScan。一帧数据可以理解为机器人在一圈角度范围内，对不同方向测得的一组距离。

检查话题是否存在：

ros2 topic list | grep scan

查看一帧数据：

ros2 topic echo /scan --once

查看发布频率：

ros2 topic hz /scan

如果 /scan 不存在，slam_toolbox 就没有环境观测，当然无法建图。

3.2 输入二：里程计与 TF

里程计根据轮子转动估计机器人移动了多远、转了多少角度。它短时间内通常比较平滑，但轮胎打滑、地面不平和模型误差都会让误差逐渐累积。

slam_toolbox 不只是读取一个 /odom 数值，还需要通过 TF 查到相应坐标关系。最重要的坐标链通常是：

map → odom → base_footprint → base_scan（TurtleBot3 示例）

其中：

• map：全局地图坐标系，由 SLAM 系统维护；
• odom：局部连续的里程计坐标系；
• base_footprint：机器人在地面上的基准坐标系；
• base_scan：TurtleBot3 使用的激光雷达坐标系。

base_scan 不是所有机器人的统一命名。其他机器人可能使用 base_laser、laser_frame 或 lidar_link。应查看 /scan 消息头中的 frame_id，再根据实际 URDF/SDF 和 TF 树替换下面命令中的坐标系名称：

ros2 topic echo /scan --once

检查 odom 到机器人底盘的变换：

ros2 run tf2_ros tf2_echo odom base_footprint

检查底盘到激光雷达的变换：

ros2 run tf2_ros tf2_echo base_footprint base_scan

如果终端持续报告找不到变换，优先解决 TF，不要先去调优化器参数。

3.3 slam_toolbox 输出什么

运行后最常用的输出包括：

• /map：nav_msgs/msg/OccupancyGrid 类型的二维占据栅格地图；
• map → odom：用于修正里程计累计漂移的坐标变换；
• 位姿与位姿图相关信息；
• 保存地图、序列化位姿图等服务。

这里有一个很重要的分工：

里程计负责提供短时间内连续、平滑的运动估计；SLAM 负责从全局角度修正它的累计误差。

因此，slam_toolbox 通常发布的是 map → odom，而不是替代底层里程计直接发布 odom → base_footprint。

---

4. 启动 TurtleBot3 仿真

打开第一个终端，设置机器人型号：

export TURTLEBOT3_MODEL=burger

启动 TurtleBot3 World：

ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

Gazebo 启动后，先确认世界中出现了 TurtleBot3，并等待传感器和控制器初始化完成。

在第二个终端检查关键话题：

ros2 topic list

至少应该能够找到 /scan，并能够获得机器人相关的 TF。如果缺少这些输入，先检查 Gazebo 插件、TurtleBot3 环境变量和工作空间是否正确加载。

---

5. 启动 slam_toolbox

打开新的终端，加载环境后运行异步在线建图：

source /opt/ros/lyrical/setup.bash
source /opt/nav2/setup.bash

ros2 launch slam_toolbox online_async_launch.py use_sim_time:=true

这里有两个关键词。

5.1 为什么是 online

online 表示系统一边接收实时激光数据，一边完成扫描匹配、位姿图更新和地图发布。机器人移动时，我们能够在 RViz 中直接看到地图生长。

5.2 为什么先用 async

异步模式不会要求每一帧激光扫描都必须进入建图流程。更准确地说，当队列中已经有一帧尚未处理的扫描、此时又到达更新扫描时，待处理项会被更新为较新的数据，而不是随机丢弃扫描。这样即使计算速度暂时跟不上输入速度，系统仍能优先处理较新的观测，适合实时运行和入门实验。

同步模式会保留扫描队列并尽量处理每一帧数据，更适合希望完整处理数据、允许系统暂时落后，或者进行离线重放的场景。第一次跑通流程时，先使用官方教程中的异步模式即可。

5.3 确认节点已经激活

检查节点：

ros2 node list | grep slam

检查生命周期状态：

ros2 lifecycle get /slam_toolbox

正常情况下应处于 active。如果停留在 unconfigured 或 inactive，需要检查启动日志、参数文件和生命周期管理设置。

---

6. 在 RViz 中观察建图

打开一个新终端：

rviz2

把左上角 Global Options 中的 Fixed Frame 设置为：

map

然后依次添加以下显示项：

RViz 显示项	建议话题或设置	用途
Map	/map	查看占据栅格地图
LaserScan	/scan	查看当前激光扫描
TF	默认	检查坐标系连接关系
RobotModel	默认	查看机器人模型与激光是否重合

如果 LaserScan 显示正常，但 Map 一片空白，可以先检查：

ros2 topic hz /map
ros2 topic echo /map --once

注意：默认地图更新周期可能比激光扫描慢，因此不要因为地图没有每帧刷新就立即判断程序故障。

---

7. 遥控机器人完成一次建图

打开新的终端：

export TURTLEBOT3_MODEL=burger
ros2 run turtlebot3_teleop teleop_keyboard

根据终端提示使用键盘控制机器人。

在继续建图前，先确认速度命令确实能够驱动底盘：

ros2 topic info /cmd_vel -v
ros2 topic echo /cmd_vel --once

如果按下遥控按键时 /cmd_vel 中已经有速度消息，但 Gazebo 里的底盘仍然不动，问题通常在底盘控制链，而不是 SLAM。应检查 gz_ros2_control、差速驱动控制器或 Gazebo diff-drive 插件是否加载，控制器订阅的话题和消息类型是否与 teleop 输出一致。不要因为机器人不动就先去修改 slam_toolbox 参数。

不同 ROS 2 发行版或机器人可能使用 geometry_msgs/msg/Twist、TwistStamped，也可能把输入话题命名为 /cmd_vel_unstamped。应以 ros2 topic info 的实际结果为准。

建图时，驾驶方式会直接影响结果。建议遵循下面几个原则：

1. 先慢后快。 第一次运行时降低线速度和角速度，确认地图更新正常后再提速。
2. 避免原地高速旋转。 旋转过快会让相邻扫描差异变大，增加扫描匹配难度。
3. 不要长时间贴着墙走。 激光雷达需要看到具有区分度的几何结构。
4. 尽量走出闭环。 绕环境一圈后回到起点附近，给系统发现回环的机会。
5. 回到旧区域后稍作停留。 让系统有时间处理扫描、搜索匹配并完成优化。

一个适合初次实验的路线是：

起点 → 沿外圈前进 → 经过若干拐角 → 回到起点附近 → 再走一小段重叠路线

在运动过程中，可以观察到地图边界逐渐扩展。由于里程计存在累计误差，机器人长距离运动后，新扫描与旧地图可能出现轻微错位。当机器人回到已经访问过的区域并成功建立回环约束时，整张地图可能突然发生小幅平移、旋转或拉直。

这不是 RViz 的显示故障，而是位姿图优化正在修正历史轨迹。

---

8. 从“地图跳了一下”理解位姿图优化

上一篇文章讨论的是一个位姿如何根据观测误差进行更新。在 slam_toolbox 中，问题变成了同时调整一串二维位姿。

可以把系统内部结构简单理解为：

节点：机器人在不同时刻的二维位姿，即 x、y 和朝向

边：两个位姿之间的相对约束
    ├── 里程计约束
    ├── 相邻扫描匹配约束
    └── 回环约束

优化：调整所有节点，让整个位姿图中的约束尽可能一致

假设机器人绕房间走了一圈。只依赖里程计时，估计轨迹的终点可能无法与起点重合。此时，系统如果识别出当前位置与起点是同一个区域，就会在两处位姿之间加入一条回环约束。

问题来了：起点和终点应该重合，但中间累积的里程计结果却说它们没有重合。系统不能只移动最后一个节点，因为那会让最后一段轨迹突然断裂。更合理的做法，是把误差分配到整条轨迹上，联合调整多个历史位姿。

位姿更新后，每帧激光扫描在地图中的位置也随之改变，于是重新生成的栅格地图会变得更一致。这就是我们在 RViz 中看到地图整体修正的原因。

slam_toolbox 默认使用 Ceres 求解器，并采用 LM 信赖域策略。也就是说，上一篇文章里的非线性优化并没有消失，而是被封装在工具内部。使用者主要负责保证传感器、TF、初值和参数合理，求解器负责完成迭代优化。

---

9. 默认配置中最值得先理解的参数

slam_toolbox 的参数很多。第一次使用时，不要从头到尾逐项修改。先理解下面这些参数，已经足够解决大部分入门问题。

9.1 坐标系和话题

odom_frame: odom
map_frame: map
base_frame: base_footprint
scan_topic: /scan

这四项必须与机器人实际发布的 TF 和话题一致。很多“完全不出图”的问题，都不是算法失效，而是这里写错了。

9.2 地图分辨率

resolution: 0.05

0.05 表示一个栅格边长为 5 厘米。分辨率越高，地图细节越多，但内存和计算开销也会增加。对常见室内移动机器人，先使用默认值通常比盲目调小更稳妥。

9.3 何时接收新的扫描节点

minimum_travel_distance: 0.5
minimum_travel_heading: 0.5

这两个参数限制机器人至少移动一定距离或转过一定角度后，才处理新的扫描。设置过大，地图更新显得迟钝；设置过小，则会产生更多节点和计算负担。

9.4 是否启用回环

do_loop_closing: true
loop_search_maximum_distance: 3.0

前者控制是否进行回环检测，后者限制候选回环搜索的距离范围。回环一直不发生时，不应立即把搜索范围调得很大，因为过宽的搜索也可能增加错误匹配风险。先检查机器人是否真的回到了几何特征足够明显的旧区域。

9.5 求解器

solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT
ceres_loss_function: None

这些配置对应上一篇文章中的非线性最小二乘求解。初学阶段建议保持默认值。只有在已经确认传感器、TF、扫描匹配和回环约束合理之后，才值得进一步研究求解器和鲁棒核。

如果真实机器人经常打滑或出现异常约束，可以继续研究 HuberLoss；但鲁棒核不是修复错误 TF、错误时间戳或严重外参误差的万能药。

9.6 使用自己的参数文件

先复制官方参数文件：

mkdir -p ~/slam_toolbox_config

cp $(ros2 pkg prefix slam_toolbox)/share/slam_toolbox/config/mapper_params_online_async.yaml \
   ~/slam_toolbox_config/mapper_params_online_async.yaml

修改后通过启动参数加载：

ros2 launch slam_toolbox online_async_launch.py \
  use_sim_time:=true \
  slam_params_file:=$HOME/slam_toolbox_config/mapper_params_online_async.yaml

不要直接修改 /opt/ros/... 下安装的软件包文件，否则系统升级后修改可能丢失，也不方便记录不同实验配置。

---

10. 保存地图：栅格地图和位姿图不是一回事

建图完成后，至少有两种保存方式。它们名称相似，但保留的信息完全不同。

10.1 保存占据栅格地图

先创建保存目录：

mkdir -p ~/maps

保存当前 /map：

ros2 run nav2_map_server map_saver_cli -f ~/maps/slam_toolbox_map

通常会得到两个文件：

slam_toolbox_map.pgm
slam_toolbox_map.yaml

其中：

• .pgm 保存地图图像；
• .yaml 保存地图分辨率、原点、占据阈值和图像路径等元数据。

这类地图适合后续交给 Nav2 地图服务器或 AMCL 使用，但它只保存最终栅格结果，不包含完整的扫描数据和位姿图。

10.2 序列化 slam_toolbox 位姿图

如果希望以后加载已有会话并继续建图，还需要保存位姿图：

ros2 service call /slam_toolbox/serialize_map \
  slam_toolbox/srv/SerializePoseGraph \
  "{filename: '/home/你的用户名/maps/slam_session'}"

这里建议使用绝对路径，并把“你的用户名”替换为实际用户名。

序列化文件保存的是 slam_toolbox 后续继续建图、定位或离线处理所需的信息。它不是一张普通图片，也不能用图片查看器直接打开。

可以这样记忆：

保存内容	主要用途
.pgm + .yaml	展示地图、Nav2 地图服务器、传统定位
序列化位姿图	继续建图、slam_toolbox 定位、位姿图处理

---

11. 高频问题排查

11.1 RViz 提示 No transform from ... to map

可能原因：

• slam_toolbox 没有正常激活；
• /scan 的坐标系无法连接到机器人底盘；
• odom_frame、base_frame 或 scan_topic 配置不一致；
• 仿真使用了 ROS 时间，但某个节点仍在使用系统时间。

检查：

ros2 lifecycle get /slam_toolbox
ros2 topic echo /scan --once
ros2 run tf2_ros tf2_echo odom base_footprint
ros2 run tf2_ros tf2_echo base_footprint base_scan  # 按实际激光雷达 frame 修改

仿真环境中的相关节点应统一设置 use_sim_time:=true。

11.2 有激光点，但地图不更新

可能原因：

• Fixed Frame 不是 map；
• /map 没有发布；
• 机器人移动距离或转角没有达到处理阈值；
• 时间戳或 TF 查询失败；
• slam_toolbox 没有进入 active 状态。

检查：

ros2 topic hz /map
ros2 topic hz /scan
ros2 lifecycle get /slam_toolbox

11.3 地图出现重影或墙体变成两条

可能原因：

• 机器人移动或旋转过快；
• 里程计误差较大；
• 激光雷达外参错误；
• 时间同步存在问题；
• 环境几何结构重复，扫描匹配出现歧义。

先降低速度并检查 TF，不要一开始就大量修改扫描匹配参数。

11.4 回到起点却没有回环

可能原因：

• 返回位置与历史位置距离仍然较远；
• 重叠区域太短；
• 环境特征不足或高度重复；
• 回环匹配阈值过严；
• do_loop_closing 没有开启。

可以让机器人在起点附近多走一段重叠路线，而不是刚刚靠近起点就停止。

11.5 地图保存失败或内容为空

保存前确认 /map 已有数据：

ros2 topic echo /map --once

同时确认目标目录存在并具有写权限。使用 map_saver_cli 时，-f 后面给的是文件名前缀，不要手动添加 .pgm。

---

12. 从仿真迁移到真实机器人

把仿真替换为真实机器人后，slam_toolbox 的主流程并不会改变，仍然需要：

/scan + odom/TF → 扫描匹配 → 位姿图 → 回环 → 优化 → /map

变化主要发生在数据质量上：

• 真实轮子会打滑；
• 激光雷达存在噪声和最小、最大测距限制；
• 传感器安装外参可能不准确；
• 不同设备的时间戳可能不同步；
• 玻璃、镜面和动态行人会产生异常激光回波。

因此，从仿真迁移到真机时，推荐按下面的顺序排查：

1. 先单独验证 /scan 是否合理；
2. 再验证完整 TF 树；
3. 检查机器人静止时里程计是否漂移；
4. 低速跑一小块区域；
5. 最后再测试大范围建图和回环。

这个顺序非常重要。输入数据不可靠时，继续调后端优化参数通常只是在掩盖真正的问题。

---

13. 总结

本文使用 TurtleBot3、Gazebo 和 slam_toolbox 跑通了一条二维激光 SLAM 的标准工作流：

启动机器人与传感器
        ↓
检查 /scan、里程计和 TF
        ↓
启动 slam_toolbox
        ↓
在 RViz 中观察在线建图
        ↓
让机器人走出闭环
        ↓
通过位姿图优化修正累计误差
        ↓
保存栅格地图与序列化位姿图

现在再回头看上一篇文章中的概念，它们已经可以在真实系统中找到位置：

理论概念	slam_toolbox 中的对应对象
位姿	位姿图中的机器人节点
观测与残差	扫描匹配和相对位姿约束的不一致
信息与权重	不同约束的可信程度
LM	Ceres 中用于求解非线性问题的信赖域策略
位姿更新	优化后重新调整的机器人轨迹
地图修正	用更新后的位姿重新放置激光扫描

所以，地图不是激光点简单叠加出来的一张图片。它背后是一条不断被估计、约束和修正的机器人轨迹。slam_toolbox 把扫描匹配、回环检测、位姿图管理和非线性优化封装成了 ROS 2 中可以直接使用的工程工具，而我们要做的，是理解数据怎样进入系统、结果怎样产生，以及出错时应该从哪里检查。

下一篇可以继续深入 slam_toolbox 的地图序列化、继续建图与定位模式，回答另一个工程问题：机器人关机以后，怎样重新加载之前的地图和位姿图，并在已有环境中继续工作？

---

参考资料

1. SLAM Toolbox 官方仓库
2. Nav2 官方教程：Navigating While Mapping
3. TurtleBot3 官方文档：Gazebo Simulation
4. TurtleBot3 官方文档：SLAM Simulation
5. SLAM Toolbox 默认异步建图参数

版本提示：ROS 2 与相关软件包会持续更新。本文的环境加载方式是 ROS 2 Lyrical + /opt/nav2 源码覆盖层；其他读者必须按实际发行版、安装位置、TF frame 和速度控制话题修改命令。发布文章时，建议在开头注明自己实际验证过的 Ubuntu、ROS 2、Gazebo 和 slam_toolbox 版本。