3. SLAM Toolbox 建图实战：在线同步建图与传感器融合
说实话，SLAM Toolbox 是我在 Nav2 生态里最喜欢的一个建图工具。它不像 gmapping 那样老态龙钟，也不像 Cartographer 那样配置起来让人头疼。说白了，它就是为现代机器人量身定做的——支持在线同步建图、异步建图、纯定位，还能随时保存和继续地图。
这一章，我们就来实战一把。我会带你从零开始，用 SLAM Toolbox 跑一次在线同步建图，同时把激光雷达和里程计怎么融合这件事彻底讲透。
3.1 为什么选 SLAM Toolbox？
我在项目里用过不少建图方案。gmapping 虽然简单，但地图质量一般，而且不支持继续建图。Cartographer 精度高，可配置项多到让人崩溃——我记得第一次调参，光看那个 Lua 配置文件就花了两天。
SLAM Toolbox 的好处在于：
- 在线同步模式：边移动边建图，实时看到地图更新
- 异步模式：先录数据包，后建图，适合调试
- 地图续建：今天建一半，明天接着建，不丢数据
- 纯定位模式：地图建好后，只用它做定位，不更新地图
- 内置生命周期管理：和 Nav2 无缝集成
你想想看，一个工具能覆盖建图、定位、续建三个场景，是不是很省心？
3.2 在线同步建图的核心流程
在线同步建图，说白了就是机器人一边走，一边用激光雷达扫描周围环境，同时用里程计估计自己的位置，然后把扫描到的点云拼接到全局地图上。
我画了一张流程图，帮你理清数据流向：
嗯，这里要注意：激光雷达和里程计的数据频率往往不一样。激光一般是 10Hz，里程计可能到 50Hz 甚至更高。SLAM Toolbox 内部会做时间同步，把两个数据流对齐到同一个时间轴上。
3.3 启动 SLAM Toolbox 在线同步模式
启动方式很简单。我个人习惯用 launch 文件来组织，这样参数都在一个地方，方便调。
先看一个最简的启动配置：
<launch>
<node pkg="slam_toolbox" type="sync_slam_node" name="slam_toolbox" output="screen">
<param name="use_sim_time" value="false"/>
<param name="base_frame" value="base_footprint"/>
<param name="odom_frame" value="odom"/>
<param name="map_frame" value="map"/>
<param name="mode" value="mapping"/>
<!-- 激光雷达参数 -->
<param name="max_laser_range" value="12.0"/>
<param name="minimum_time_interval" value="0.1"/>
<!-- 地图参数 -->
<param name="resolution" value="0.05"/>
<param name="map_update_interval" value="0.5"/>
<!-- 图优化参数 -->
<param name="minimum_travel_distance" value="0.2"/>
<param name="minimum_travel_heading" value="0.3"/>
</node>
</launch>这里有几个参数，我重点说一下：
| 参数名 | 默认值 | 我的建议 |
|---|---|---|
| resolution | 0.05 | 室内用 0.05，室外用 0.1，太细了计算量大 |
| map_update_interval | 0.5 | 地图更新频率，0.5 秒一次够用，太快了 CPU 扛不住 |
| minimum_travel_distance | 0.2 | 机器人移动 0.2 米才触发一次图优化，防止原地抖动 |
| max_laser_range | 12.0 | 根据你的激光雷达实际量程来设，别超过硬件极限 |
minimum_travel_distance 和 minimum_travel_heading，减少图优化的触发频率。
3.4 激光雷达与里程计融合原理
这个问题，我当年刚接触 SLAM 时也困惑过。激光雷达和里程计，到底谁更靠谱？
答案是：都不完美，但可以互补。
- 激光雷达：精度高，但容易受环境干扰。比如在长走廊里，激光打出去没有回波，数据就丢了。还有玻璃、镜子这些，激光直接穿透或反射，测出来的距离全是错的。
- 里程计：短时间内的相对运动很准，但误差会累积。你想想看，轮子打滑一下，里程计就漂了。IMU 也有零偏，时间长了角度就歪了。
SLAM Toolbox 是怎么做的呢？它用了一个叫 图优化（Graph SLAM） 的方法。
简单说：
- 把机器人的每个位姿（位置+朝向）当作一个节点
- 把激光雷达的观测和里程计的运动当作节点之间的约束
- 用最小二乘法，调整所有节点的位置，让所有约束的误差总和最小
我举个例子。假设机器人走了三步：
- 第一步：里程计说走了 1 米，激光雷达说看到了一面墙
- 第二步：里程计说又走了 1 米，激光雷达说看到了墙角
- 第三步：里程计说再走 1 米，激光雷达说回到了墙的另一端
如果只用里程计，第三步的位置可能已经漂了 0.1 米。但激光雷达发现，第三步看到的墙和第一步看到的墙应该是同一面墙——这就是 闭环检测。图优化会把第一步和第三步之间的误差拉平，重新调整所有位姿，让地图自洽。
3.5 实战步骤：跑一次完整的建图
好了，理论说完了，我们来动手。以下是我在实际项目中的标准操作流程：
- 启动机器人底盘：确保里程计和激光雷达都在发数据。用
rostopic echo /scan和rostopic echo /odom确认一下。 - 启动 SLAM Toolbox：运行上面的 launch 文件。你会看到终端里刷出 "Registering scan..." 之类的日志。
- 打开 RViz：添加 Map 显示，话题选
/map。再添加 LaserScan 显示，话题选/scan。这样你能同时看到地图和实时激光数据。 - 开始移动机器人：用手柄或键盘控制机器人慢慢走。速度别太快，我建议线速度不超过 0.3 m/s，角速度不超过 0.3 rad/s。
- 观察地图构建：你会看到地图从无到有，逐渐清晰。如果发现地图有重影或错位，说明闭环没做好，可以原地转几圈，让激光雷达多扫一些特征。
- 保存地图：建完后，用以下命令保存：
ros2 run nav2_map_server map_saver_cli -f my_map这会生成 my_map.yaml 和 my_map.pgm 两个文件。以后导航就用这个地图。
ros2 service call /slam_toolbox/save_map slam_toolbox/srv/SaveMap 手动保存中间结果。万一机器人撞墙了，至少还有之前的地图。
3.6 常见问题与调参建议
建图过程中，你可能会遇到这些问题。我列个表，方便你对照排查：
| 现象 | 可能原因 | 解决办法 |
|---|---|---|
| 地图出现重影 | 闭环检测失败，或里程计漂移太大 | 降低移动速度，增加环境特征，调小 minimum_travel_distance |
| 地图边缘模糊 | 激光雷达数据噪声大，或分辨率太低 | 提高 resolution，检查激光雷达是否干净 |
| 建图时 CPU 100% | 图优化频率太高，或地图太大 | 调大 map_update_interval，限制建图范围 |
| 机器人位置突然跳变 | 闭环检测触发了大幅位姿调整 | 这是正常现象，但说明里程计质量差，建议先标定里程计 |
嗯，最后再啰嗦一句。建图这件事，七分靠硬件，三分靠算法。如果你的激光雷达质量不行，或者里程计标定不准，再好的 SLAM 算法也救不了。所以，动手建图之前，先把传感器标定好——这是我一贯的原则。