从点云到栅格地图：ROS2 SLAM与Nav2导航全链路实战

1. 为什么我要把扫地机器人的整条链路拆开讲

扫地机器人这个品类，看起来是个消费电子，实际上它是一个把SLAM（同步定位与建图）、路径规划、运动控制、传感器融合全部塞进一个直径三十多厘米圆盘里的移动机器人平台。我接触过不少做ROS2的朋友，他们学完官方教程之后最大的困惑就是：TurtleBot3跑通了，但换成自己搭的机器人，雷达一装、底盘一接，建图就开始飘，导航就开始撞。问题往往不在某一个算法上，而在整条链路的衔接处。

这篇内容我打算把"从点云到地图"这件事完整走一遍：从LiDAR和IMU的原始数据怎么进系统，到SLAM节点怎么吐出栅格地图，再到Nav2怎么拿着这张地图做全局和局部规划，最后Gazebo里怎么验证。关键词覆盖SLAM、Nav2、ROS2、LiDAR、Gazebo，我会尽量把每个环节的"为什么这么选"讲清楚，而不是只丢一堆launch文件让你抄。

适合谁看？如果你已经装好了ROS2，能跑通基础的talker/listener，但一到建图和导航就卡壳，那这篇就是给你写的。如果你连ROS2都还没装，建议先把环境搞定再回来，因为下面很多操作是建立在ROS2 Humble或者Jazzy基础上的。我实测下来，Humble的生态最稳，Gazebo和Nav2的配合也最成熟，新手优先选它。

整条链路我习惯分成四段来看：感知层（LiDAR、IMU、里程计）、建图层（SLAM算法输出地图）、导航层（Nav2的规划与控制）、仿真层（Gazebo提供闭环验证环境）。这四层任何一层出问题，最终表现都是"机器人不听话"，但根因完全不同。下面我逐层拆。

2. 感知层：LiDAR、IMU和里程计到底谁说了算

2.1 三种传感器各自的职责边界

很多人一上来就纠结"用激光还是用视觉"，其实在扫地机器人这个场景里，2D LiDAR几乎是默认答案。原因很直接：扫地机工作在平面，房间结构以墙面、家具腿这类垂直特征为主，2D激光雷达扫一圈就能拿到非常干净的轮廓，计算量还小。视觉SLAM虽然信息更丰富，但对光照、纹理依赖大，地毯、白墙这种低纹理场景很容易丢。

但光有LiDAR不够。LiDAR做scan matching（扫描匹配）时，如果机器人原地旋转或者走过一段长走廊，累积误差会很明显。这时候IMU就派上用场了，它提供角速度和加速度，能在两次扫描之间给出姿态变化的预测。而轮式里程计提供的是平移量的粗略估计。三者关系可以这样理解：

• LiDAR：提供环境结构的绝对观测，精度高但频率低（一般5到10Hz）
• IMU：提供高频的姿态变化（100Hz以上），但会漂移
• 里程计：提供平移估计，受轮子打滑影响大

SLAM的核心工作，就是把这些信息用一个滤波器或者图优化框架融合起来。我见过太多人只喂LiDAR给SLAM，结果机器人一转圈地图就糊了，就是因为缺少IMU的姿态约束。

2.2 坐标系树是整条链路的骨架

在ROS2里，TF树是感知层的命脉。你必须搞清楚这几个坐标系的关系：

map到odom的变换由SLAM节点发布，odom到base_link由里程计发布，base_link到传感器由静态TF发布。这条链一旦断了或者有环，RViz2里机器人就会闪烁甚至消失。我踩过的坑是：静态TF的base_link到base_laser写反了方向，结果雷达点云整体镜像，建出来的地图左右颠倒，排查了整整一个下午。

提示：用ros2 run tf2_tools view_frames生成TF树PDF，一眼就能看出哪条链断了。这个命令比在RViz2里肉眼找快十倍。

2.3 LiDAR和IMU的标定为什么不能省

标题热词里出现了"lidar imu标定"，这不是玄学。LiDAR和IMU之间存在外参（安装位置和姿态的偏差）和时间戳偏差。如果雷达和IMU的安装角度差了几度没标定，融合时姿态就会系统性偏斜，表现为地图缓慢旋转。

标定分两步：外参标定和时间同步。外参标定可以用手眼标定思路，让机器人做特定运动，通过观测一致性反推外参。时间同步更关键，如果雷达和IMU的时间戳差了50毫秒，机器人快速转向时融合结果就会错位。ROS2里可以用message_filters做时间对齐，或者用硬件触发同步。

我的经验是：消费级扫地机上，外参标定做到1度以内、时间同步做到10毫秒以内，建图质量就有质的提升。别小看这几个数字，它直接决定了你后面Nav2导航时地图和实际环境对不对得上。

3. 建图层：SLAM算法选型与点云到栅格地图的转换

3.1 2D SLAM的两大流派：滤波 vs 图优化

SLAM算法大致分两派。滤波派以EKF-SLAM、FastSLAM为代表，实时性好，但地图规模大了之后一致性差。图优化派以Cartographer、SLAM Toolbox为代表，把位姿和路标建成图，用非线性优化求解，回环检测能力强，适合大场景。

扫地机器人场景我推荐SLAM Toolbox，它是ROS2生态里维护最活跃的2D SLAM方案，支持在线建图和离线建图两种模式。Cartographer也很强，但配置复杂，调参门槛高。SLAM Toolbox的默认配置在大多数室内环境下开箱即用，这对新手非常友好。

选它的核心理由：它基于Karto SLAM，做了大量工程优化，支持** lifelong mapping**（长期建图），也就是机器人可以分多次建图并合并，这对扫地机"第一次跑建图、之后一直用"的使用模式非常契合。

3.2 从点云到栅格地图的完整转换链路

LiDAR吐出来的是LaserScan或者PointCloud2，但Nav2需要的是OccupancyGrid（栅格地图）。中间发生了什么？

第一步，扫描匹配。SLAM节点把当前帧激光和已有地图做匹配，估计出机器人当前位姿。这一步用的是相关性扫描匹配或者ICP类算法。

第二步，位姿图优化。把历史位姿和约束建成图，检测到回环时（比如机器人绕了一圈回到起点），优化整个轨迹，消除累积误差。

第三步，栅格化。把激光点投影到二维平面，标记为占据（occupied）、空闲（free）或未知（unknown）。这里有个关键参数叫分辨率，一般设0.05米，也就是每个栅格代表5厘米。分辨率太高地图文件巨大，太低则细节丢失，扫地机过窄缝时容易误判。

第四步，地图发布。SLAM节点通过/map话题发布OccupancyGrid，同时发布map到odom的TF变换。

我实测下来，一个100平米的房子，0.05米分辨率建出来的地图大概几MB，完全可接受。如果你要做八叉树地图（热词里提到的"八叉树地图导航"），那是3D场景的需求，2D导航用不上，但如果你要处理多层建筑或者立体障碍，OctoMap就是下一步。

3.3 建图质量差，八成是这几个原因

建图糊、重影、墙壁变厚，我总结下来无非几个原因：

• 里程计标定不准：轮子直径、轮距参数错了，导致平移估计系统性偏差。这个必须先用标定工具测准。
• 雷达安装高度不当：太低会扫到地面杂物，太高会扫到桌面上方，最佳高度是离地15到20厘米，能扫到家具腿和墙脚。
• 运动速度过快：SLAM需要时间做匹配，机器人跑太快，两帧之间位移过大，匹配就失败。建图时建议限速在0.2米每秒以内。
• 环境动态物体太多：走来走去的人、宠物会让地图出现鬼影。建图时尽量清场。

注意：建图阶段不要开导航，两者同时跑会互相干扰。先建好图保存，再切到导航模式加载地图，这是标准流程。

4. 导航层：Nav2的插件化架构与规划器选择

4.1 Nav2不是单个节点，而是一套行为树驱动的系统

很多人以为Nav2就是一个"导航节点"，其实它是一整套架构。核心组件包括：

• BT Navigator：行为树导航器，负责调度整个导航流程
• Planner Server：全局路径规划，从起点到目标点算一条路径
• Controller Server：局部控制，跟踪全局路径并避障
• Recovery Server：恢复行为，卡住时执行后退、旋转等动作
• Costmap 2D：代价地图，把障碍物膨胀成机器人可通行的代价

这套架构的好处是插件化。全局规划器可以换NavFn、Smac、ThetaStar，局部控制器可以换DWB、TEB、MPPI。扫地机场景我一般用Smac Planner做全局（支持混合A*，路径更平滑），DWB或MPPI做局部（MPPI对动态障碍响应更好）。

4.2 代价地图的膨胀半径是个技术活

代价地图里有个参数叫inflation_radius（膨胀半径），它决定了障碍物周围多大范围被标记为高代价。这个参数设小了，机器人贴着墙走容易刮蹭；设大了，窄门口直接过不去。

计算逻辑是这样的：膨胀半径至少要大于机器人内切圆半径。假设扫地机直径0.35米，内切圆半径0.175米，那膨胀半径设0.2米比较稳妥。但门口宽度如果只有0.4米，膨胀后两边各占0.2米，中间就只剩0.0米，机器人判定不可通行。这时候要么减小膨胀半径，要么用cost_scaling_factor调整代价衰减曲线，让机器人"愿意"贴着障碍走。

我的做法是：先按内切圆半径设膨胀，然后在实际门口测试，如果过不去再微调。别一上来就设很大，那样机器人会显得很"怂"，到处绕路。

4.3 全局规划和局部规划的配合逻辑

全局规划器算出的路径是"理想路径"，它假设环境静态。局部控制器负责跟踪这条路径，同时用当前激光数据实时避障。两者通过局部代价地图衔接。

这里有个常见误区：全局路径穿过了一个动态障碍（比如临时放的椅子），局部控制器会尝试绕开，但如果绕开幅度太大，偏离全局路径太远，就会触发重规划。Nav2里通过controller_server的failure_tolerance参数控制这个容忍度。

我调参的经验是：局部控制器的前瞻距离（lookahead distance）要跟速度匹配。速度0.3米每秒时，前瞻0.3到0.5米比较合适。前瞻太短机器人会画龙，太长则转弯迟钝。

5. 仿真层：Gazebo里把整条链路跑通再上真机

5.1 为什么一定要先在Gazebo里验证

真机调试的成本太高了：撞一次可能就坏一个雷达，电池续航有限，调试时间碎片化。Gazebo能提供可重复、可加速、可观测的验证环境。你可以在Gazebo里把机器人速度调到真机的三倍，快速验证SLAM和Nav2的鲁棒性。

Gazebo仿真环境模型（热词里提到）的搭建，核心是URDF/Xacro描述机器人，SDF描述世界。URDF定义连杆、关节、传感器，Gazebo插件负责把传感器数据以ROS2话题形式发布出来。常用的插件有libgazebo_ros_laser.so（雷达）、libgazebo_ros_imu_sensor.so（IMU）、libgazebo_ros_diff_drive.so（差速驱动）。

5.2 仿真和真机的差异，心里要有数

Gazebo再真实，也有几个坑：

• 雷达噪声模型：默认雷达是理想无噪声的，建图会异常干净，容易让你误以为算法很稳。建议手动加高斯噪声，模拟真实雷达。
• 轮子打滑：Gazebo里轮子几乎不打滑，里程计过于完美。真机上地毯、门槛都会导致打滑，所以里程计融合策略要留余量。
• 物理引擎差异：Gazebo的摩擦系数、碰撞模型和真实世界有偏差，导航参数不能直接照搬。

我的建议是：Gazebo里把参数调到"比真机更苛刻"，比如加噪声、加延迟，这样上真机时反而会觉得"比仿真还简单"。

5.3 从仿真到真机的迁移检查清单

迁移前逐项确认：

1. TF树结构一致，坐标系命名统一
2. 传感器话题名称和消息类型一致
3. 里程计标定参数按真机重新测量
4. 代价地图尺寸覆盖真机工作区域
5. 速度限制按真机电机能力下调
6. 急停和恢复行为在真机上实测有效

这套流程走下来，真机首次跑导航的成功率会高很多。我见过太多人仿真里跑得飞起，真机一上就撞墙，基本都是迁移时漏了某一项。

6. 那些让我熬夜的坑，以及怎么绕过去

6.1 ROS2环境安装的公钥报错

热词里反复出现"由于没有公钥，无法验证下列签名"这个报错，这是ROS2安装最经典的拦路虎。根因是apt的密钥环里没有ROS仓库的公钥。解决办法是重新导入公钥，然后apt update。如果还不行，检查系统时间是否准确，时间偏差会导致签名验证失败。

Ubuntu 22.04对应ROS2 Humble，24.04对应Jazzy。别装错版本，否则Gazebo和Nav2的包依赖会对不上。我建议新手直接用Humble，社区资料最多，遇到问题一搜就有答案。

6.2 RViz2里地图不显示或机器人闪烁

这个问题九成是TF问题。排查顺序：先view_frames看树结构，再ros2 topic echo /tf看变换是否在发布，最后检查时间戳是否同步。如果机器人闪烁，通常是map到odom的变换在跳变，说明SLAM位姿估计不稳定，回去检查传感器融合配置。

6.3 导航时机器人原地打转或撞墙

原地打转一般是局部控制器参数问题，检查min_vel_theta和max_vel_theta是否合理，以及代价地图里机器人 footprint 是否设置正确。撞墙则多半是膨胀半径太小或者雷达盲区没处理。扫地机雷达一般有360度覆盖，但如果有遮挡，要在代价地图里把盲区标记为未知。

6.4 建图保存后加载，地图和实际对不上

这是地图原点问题。SLAM保存地图时，地图原点对应建图起点的位姿。如果导航时机器人初始位姿和建图起点不一致，又没做重定位，地图就会错位。解决办法是用AMCL做粒子滤波重定位，或者手动在RViz2里用2D Pose Estimate给初始位姿。

7. 把这套链路复用到其他机器人上

这套"感知-建图-导航-仿真"的链路，其实不限于扫地机。你把底盘换成差速、阿克曼或者全向，把雷达换成3D LiDAR或者深度相机，整体架构不变。热词里提到的Panda机械臂Gazebo仿真、MoveIt2和RViz2同步，那是机械臂领域的类似问题，核心同样是TF树和插件化架构。

我个人的体会是：ROS2的学习曲线陡，但一旦你把TF、话题、插件这三件事吃透，后面换任何机器人都是"换汤不换药"。真正难的不是某个算法，而是把整条链路的每个环节都调通、调稳。建图时多花一小时标定，导航时就能少撞十次墙，这个投入产出比非常划算。

最后分享一个我常用的小技巧：建图和导航切换时，写一个launch文件用参数控制模式，避免手动改配置。这样你可以在同一个终端里一键切换，调试效率能提升不少。