1. 从一台扫地机说起：为什么我要跑通这条全链路
去年年底我接手了一个小项目，需求说起来很简单：让一台差速轮式机器人（底盘结构跟主流扫地机几乎一样）在未知的室内环境里自己跑起来，先建图，再基于建好的图做自主导航，能自己规划路径、绕开障碍、到达指定点位。听起来像是把SLAM和Nav2两个包跑起来就完事了，但真正动手之后我才发现，从点云到地图、从地图到导航这条链路上，坑远比想象中多。
这篇文章就是我把整条链路完整跑通之后的复盘。核心关键词是SLAM、Nav2、ROS2、LiDAR、Gazebo，我会从仿真环境搭建一直讲到实机部署，把每个环节的关键参数、踩过的坑、以及为什么这么选的理由都摊开来说。适合谁看？如果你正在学ROS2，想找一个完整的项目把SLAM和导航串起来；或者你已经跑过TurtleBot3的官方demo，但换成自己的机器人就各种报错；再或者你对激光雷达建图和Nav2导航的底层逻辑感兴趣，想搞清楚每个参数到底在干什么——那这篇内容应该能帮到你。
整条链路我拆成五块：仿真环境与机器人模型、LiDAR与IMU的标定与数据接入、SLAM建图、地图后处理与八叉树转换、Nav2导航配置与调优。每一块我都会给出可直接复现的步骤和参数，同时解释背后的原理。文章里涉及的所有代码和配置都基于ROS2 Humble + Gazebo Classic 11，这是目前最稳定、资料最全的组合，新手直接抄作业就行。
2. 仿真环境搭建：Gazebo里的扫地机模型怎么建
2.1 为什么先用Gazebo而不是直接上实机
很多人一上来就想接实机跑SLAM，我的建议是先别急。实机调试的成本太高了——传感器标定不准、时间同步有问题、底盘控制有延迟，任何一个环节出问题都会让SLAM建图直接崩掉，而你根本分不清是算法的问题还是硬件的问题。Gazebo的价值在于它提供了一个可控的、可重复的测试环境：激光雷达的噪声模型、IMU的漂移、轮子的打滑，这些都可以在仿真里复现，而且你可以随时暂停、回放、改参数。
更重要的是，Gazebo里的传感器数据是"干净"的，没有实机上那些莫名其妙的干扰。先用仿真把SLAM和Nav2的配置调通，确认算法逻辑没问题，再迁移到实机，这样排查问题的范围会小很多。我自己的流程就是：Gazebo跑通 → 录bag → 用bag离线调参 → 实机验证。
2.2 机器人URDF模型的关键部件
扫地机这类差速底盘，URDF模型里必须包含这几个核心部件：base_footprint（地面投影坐标系）、base_link（底盘中心）、左右驱动轮left_wheel_link和right_wheel_link、万向轮caster_link，以及激光雷达laser_link和IMUimu_link。这里有几个容易出错的点：
第一，base_footprint到base_link的z轴偏移要设对。如果你的底盘中心离地5cm，那这个偏移就是0.05，设错了会导致激光雷达扫描平面高度不对，建出来的图会有重影。
第二，轮子的joint类型要用continuous而不是revolute，因为驱动轮是无限旋转的。同时要在<gazebo>标签里给轮子加<mu1>和<mu2>摩擦系数，默认值0.5在仿真里经常打滑，我一般设成1.0。
第三，激光雷达的<samples>和<min_angle>/<max_angle>要匹配真实雷达。我用的是360度单线雷达，所以<samples>设360，角度范围-π到π，<range>设0.12到8.0米。如果你用的是思岚A1这类雷达，最大距离12米，那就改成12.0。
<gazebo reference="laser_link"> <sensor type="ray" name="laser_sensor"> <pose>0 0 0 0 0 0</pose> <visualize>true</visualize> <update_rate>10</update_rate> <ray> <scan> <horizontal> <samples>360</samples> <resolution>1</resolution> <min_angle>-3.14159</min_angle> <max_angle>3.14159</max_angle> </horizontal> </scan> <range> <min>0.12</min> <max>8.0</max> </range> <noise> <type>gaussian</type> <mean>0.0</mean> <stddev>0.01</stddev> </noise> </ray> </sensor> </gazebo>注意：
<update_rate>设10Hz就够了，设太高会让Gazebo的CPU占用飙升，而且实际雷达也就10-15Hz，仿真里没必要追求更高。
2.3 差速控制插件配置
Gazebo里控制差速底盘要用libgazebo_ros_diff_drive.so插件。关键参数包括left_joint、right_joint、wheel_separation（轮距）、wheel_diameter（轮径）。轮距和轮径这两个值必须和URDF里的几何尺寸完全一致，否则里程计会算错，SLAM建图会直接歪掉。
我见过有人轮距填了0.3但实际模型是0.35，结果机器人转90度，里程计显示转了105度，建出来的图整个是扭曲的。这种问题在仿真里还好看出来，实机上你根本不知道是哪里错了。
<plugin name="diff_drive" filename="libgazebo_ros_diff_drive.so"> <ros> <namespace>/</namespace> </ros> <update_rate>50</update_rate> <left_joint>left_wheel_joint</left_joint> <right_joint>right_wheel_joint</right_joint> <wheel_separation>0.35</wheel_separation> <wheel_diameter>0.065</wheel_diameter> <max_wheel_torque>20</max_wheel_torque> <max_wheel_acceleration>1.0</max_wheel_acceleration> <command_topic>cmd_vel</command_topic> <odometry_topic>odom</odometry_topic> <odometry_frame>odom</odometry_frame> <robot_base_frame>base_footprint</robot_base_frame> <publish_odom>true</publish_odom> <publish_odom_tf>true</publish_odom_tf> <publish_wheel_tf>true</publish_wheel_tf> </plugin>2.4 仿真世界文件的选择与修改
Gazebo自带的empty_world太干净了，建图没什么挑战。我建议用turtlebot3_world或者自己搭一个带家具的室内场景。如果要自己搭，在.world文件里加几个立方体当墙壁、圆柱体当桌腿就行。关键是要有足够多的特征，纯白墙的走廊对激光SLAM来说是噩梦，因为扫描到的都是一样的距离，匹配算法会失效。
我一般会在场景里放一些不规则形状的障碍物，比如斜放的箱子、不同直径的柱子，这样激光扫描到的轮廓才有区分度。另外地面要设摩擦系数，太滑的话轮子会打滑，里程计就不准了。
3. LiDAR与IMU：标定与数据接入的实操细节
3.1 为什么单线雷达也需要IMU
很多人觉得2D激光SLAM不需要IMU，只用轮式里程计就够了。在理想情况下确实可以，但实际中轮式里程计有两个致命问题：一是打滑，二是角度累积误差。机器人转几个弯之后，里程计的角度偏差可能就有十几度，这时候激光匹配就会失败。
IMU的作用是提供角速度和线加速度，在SLAM里主要用来做两个事：一是给激光匹配提供一个初始的角度预测，减少匹配搜索范围；二是和轮式里程计做融合，得到更准的odom。我用的是MPU6050这类六轴IMU，在Gazebo里可以直接用libgazebo_ros_imu_sensor.so插件模拟。
3.2 LiDAR和IMU的外参标定
外参标定就是确定LiDAR和IMU之间的相对位置和姿态。在仿真里，如果你在URDF里把两个link的joint位置写对了，外参就是准的。但实机上，雷达和IMU的安装位置总有偏差，这个偏差如果不标定，SLAM建图会有系统性的偏移。
标定方法我说一个最实用的：手动测量+激光匹配验证。先拿尺子量出LiDAR到IMU的x、y、z偏移，填到URDF或TF里。然后跑SLAM，看建出来的图有没有明显的重影或弯曲。如果有，微调外参里的yaw角，直到地图变直。这个过程可能需要反复几次，但比用什么标定算法都快。
实操心得：外参里的roll和pitch一般不用管，因为雷达和IMU通常都是水平安装的。重点调yaw，也就是两个传感器之间的旋转角度偏差。
3.3 时间同步：一个容易被忽略的坑
ROS2里多个传感器数据融合，时间同步是必须处理的。如果LiDAR和IMU的时间戳差了几十毫秒，融合出来的位姿就会抖。在仿真里，Gazebo的时间是统一的，一般没问题。但实机上，每个传感器的驱动都有自己的时间戳，需要用message_filters做时间同步。
我在实机上遇到过一个问题：雷达的时间戳用的是系统时间，IMU用的是自己的硬件时间，两者差了200ms，结果SLAM建图时机器人一转弯地图就糊。后来统一用/clock话题做时间源，问题才解决。如果你用ROS2的ros2 bag record录数据，记得加--use-sim-time参数，保证回放时时间一致。
3.4 点云数据格式与预处理
2D雷达输出的是sensor_msgs/LaserScan，3D雷达输出的是sensor_msgs/PointCloud2。如果你用的是3D雷达但只想做2D SLAM，需要把点云压扁成LaserScan。方法很简单：取点云中z坐标在某个范围内（比如-0.1到0.1米）的点，投影到xy平面，然后按角度分bin，每个bin取最近距离。
这个预处理步骤在pointcloud_to_laserscan这个包里已经实现了，配置好min_height、max_height、angle_min、angle_max就行。但要注意，3D雷达的点云密度比2D雷达高很多，直接转成LaserScan可能会有很多噪点，建议先做一次体素滤波降采样。
4. SLAM建图：从第一帧点云到一张完整地图
4.1 SLAM算法选型：Cartographer还是SLAM Toolbox
ROS2里主流的2D SLAM方案有两个：Cartographer和SLAM Toolbox。我两个都跑过，说下我的选择逻辑。
Cartographer的优势是建图质量高，回环检测强，适合大场景。但它的配置复杂，参数多，而且对IMU的依赖比较重。SLAM Toolbox的优势是配置简单，和Nav2的集成度高，支持在线建图和离线建图两种模式，而且它的pose graph优化在中小场景下效果很好。
我的选择是SLAM Toolbox，原因有三个：一是它原生支持ROS2，不需要像Cartographer那样折腾编译；二是它的async模式可以在建图的同时做定位，方便后续导航；三是它的参数少，调起来快。对于扫地机这种室内中小场景，SLAM Toolbox完全够用。
4.2 SLAM Toolbox的核心参数配置
SLAM Toolbox的配置文件里，这几个参数最关键：
mode：建图时用mapping，导航时用localization。resolution：地图分辨率，默认0.05米。如果你的机器人很小，可以设0.03；如果场景很大，设0.1可以减少内存占用。max_laser_range：雷达最大距离，设成和实际雷达一致，设大了会引入远处噪点。minimum_travel_distance：机器人移动多少距离后更新一次地图，默认0.5米。设太小会导致地图更新太频繁，CPU占用高；设太大则地图更新不及时。minimum_travel_heading：机器人转动多少弧度后更新地图，默认0.5。这个参数对转弯多的场景很重要，设小了地图会更精细。
slam_toolbox: ros__parameters: solver_plugin: solver_plugins::CeresSolver ceres_linear_solver: SPARSE_NORMAL_CHOLESKY ceres_preconditioner: SCHUR_JACOBI ceres_trust_strategy: LEVENBERG_MARQUARDT ceres_dogleg_type: TRADITIONAL_DOGLEG ceres_loss_function: None odom_frame: odom map_frame: map base_frame: base_footprint scan_topic: /scan mode: mapping resolution: 0.05 max_laser_range: 8.0 minimum_travel_distance: 0.3 minimum_travel_heading: 0.3 map_update_interval: 2.0 transform_publish_period: 0.02 transform_timeout: 0.2 tf_buffer_duration: 30.0 stack_size_to_use: 40000000 enable_interactive_mode: true4.3 建图实操：从启动到保存地图
建图的完整流程是这样的：
- 启动Gazebo仿真环境：
ros2 launch my_robot gazebo.launch.py - 启动SLAM Toolbox：
ros2 launch slam_toolbox online_async_launch.py params_file:=mapper_params_online_async.yaml - 启动RViz2，添加Map、LaserScan、TF显示
- 用键盘或手柄控制机器人移动：
ros2 run teleop_twist_keyboard teleop_twist_keyboard - 让机器人走遍整个场景，注意要走闭环，也就是回到起点，这样回环检测才能生效
- 建图完成后保存地图：
ros2 run nav2_map_server map_saver_cli -f my_map
注意：建图时速度不要太快，建议线速度0.2m/s，角速度0.5rad/s。速度太快会导致激光匹配跟不上，地图会歪。转弯时要慢，因为转弯时里程计误差最大。
4.4 建图质量评估与常见问题
建图完成后，怎么判断地图质量好不好？我的标准是：墙壁是不是直的，直角是不是90度，有没有重影。如果墙壁有弯曲，说明里程计或外参有问题；如果有重影，说明激光匹配没做好，可能是回环检测没生效。
常见问题排查表：
| 问题现象 | 可能原因 | 解决方法 |
|---|---|---|
| 地图墙壁弯曲 | 里程计标定不准 | 重新标定轮距和轮径 |
| 地图有重影 | 回环检测未生效 | 确保走闭环，检查回环参数 |
| 地图缺失区域 | 雷达被遮挡 | 检查雷达安装位置 |
| 地图整体旋转 | IMU方向错误 | 检查IMU的yaw角符号 |
| 建图过程中地图跳变 | 激光匹配失败 | 降低移动速度，增加迭代次数 |
5. 地图后处理与八叉树转换：让导航更高效
5.1 为什么需要八叉树地图
SLAM建出来的是一张2D栅格地图，每个格子只有"占用"和"空闲"两种状态。这种地图对2D导航够用，但如果你想做3D导航，或者想区分"障碍物"和"可穿越区域"，就需要八叉树地图。八叉树把3D空间递归分成八个子空间，每个节点存储占用概率，这样既能表示3D结构，又能压缩存储。
在ROS2里，八叉树地图用octomap_server这个包来生成。输入是3D点云，输出是octomap_msgs/Octomap。对于扫地机来说，八叉树地图的好处是：可以区分地面和障碍物，导航时不会把地面当成障碍。
5.2 从2D地图到八叉树的转换流程
如果你只有2D雷达，没有3D点云，也可以生成八叉树地图，方法是把2D地图的每个占用格子拉伸成3D柱体。但更常见的做法是直接用3D雷达的点云生成八叉树。
流程是这样的：
- 启动3D雷达驱动，发布
/points话题 - 启动
octomap_server，订阅/points，发布/octomap_full和/octomap_binary - 在RViz2里添加Octomap显示，查看3D地图
- 保存八叉树地图：
ros2 run octomap_server octomap_saver_node -f my_octomap.bt
octomap_server: ros__parameters: frame_id: map resolution: 0.05 sensor_model: max_range: 8.0 hit: 0.7 miss: 0.4 min_range: 0.12 occupancy_min_z: -0.1 occupancy_max_z: 2.0 filter_ground: true ground_filter_distance: 0.04 ground_filter_angle: 0.15 ground_filter_plane_distance: 0.07实操心得：
filter_ground这个参数很关键，设成true可以过滤掉地面点，避免地面被当成障碍物。ground_filter_distance设0.04米，意思是高度差小于4cm的点被认为是地面。
5.3 八叉树地图在Nav2中的使用
Nav2默认用的是2D代价地图，不直接支持八叉树。但你可以把八叉树地图投影成2D代价地图，方法是取八叉树中z坐标在机器人高度范围内的占用格子，投影到xy平面。这个工作在nav2_costmap_2d里可以通过voxel_layer插件实现，它订阅3D点云，实时生成3D代价地图。
如果你用的是预建的八叉树地图，可以用octomap_server的projected_map话题，它会把八叉树投影成2D栅格地图，然后直接给Nav2用。这样既保留了3D信息，又兼容了2D导航。
6. Nav2导航配置：从代价地图到路径规划
6.1 Nav2的整体架构与插件选型
Nav2的架构比ROS1的move_base复杂得多，它把导航拆成了多个独立的服务器：planner_server负责全局路径规划，controller_server负责局部路径跟踪，behavior_server负责恢复行为，bt_navigator负责行为树调度。每个服务器都可以换不同的插件。
我的插件选型是这样的：
- 全局规划器：
NavfnPlanner，基于Dijkstra算法，稳定可靠，适合室内场景。 - 局部控制器：
RegulatedPurePursuitController，纯跟踪算法，参数少，调起来快。 - 代价地图层：
static_layer（静态地图）、obstacle_layer（实时障碍物）、inflation_layer（膨胀层）。 - 恢复行为：
Spin、BackUp、Wait。
6.2 代价地图的关键参数
代价地图是Nav2里最需要调的部分。inflation_radius（膨胀半径）决定了机器人离障碍物多远就开始避让。这个值应该设成机器人半径加上安全余量。我的机器人半径0.17米，安全余量0.1米，所以膨胀半径设0.27米。
cost_scaling_factor（代价缩放因子）决定了代价随距离衰减的速度。设大了，机器人会贴着障碍物走；设小了，机器人会离障碍物很远。我一般设3.0，实测下来比较平衡。
local_costmap: local_costmap: ros__parameters: update_frequency: 5.0 publish_frequency: 2.0 global_frame: odom robot_base_frame: base_footprint rolling_window: true width: 3 height: 3 resolution: 0.05 plugins: ["obstacle_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: true observation_sources: scan scan: topic: /scan max_obstacle_height: 2.0 clearing: true marking: true data_type: "LaserScan" inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 inflation_radius: 0.276.3 路径规划与跟踪的调参经验
全局路径规划器NavfnPlanner的参数不多，主要是tolerance（目标点容差）和use_astar（是否用A*算法）。我一般设tolerance为0.5米，use_astar为false，因为Dijkstra在室内场景下更稳定。
局部控制器RegulatedPurePursuitController的参数需要仔细调：
desired_linear_vel：期望线速度，我设0.26m/s，和扫地机实际速度匹配。lookahead_dist：前视距离，设0.6米。设小了机器人会震荡，设大了会切内弯。min_approach_linear_velocity：接近目标时的最小速度，设0.05m/s，避免急停。max_allowed_time_to_collision_up_to_carrot：最大碰撞时间，设1.0秒。
实操心得：调参时先用仿真跑，录下bag，然后离线反复回放调。实机上调参风险太大，撞一次可能就坏了。
6.4 行为树与恢复行为配置
Nav2的行为树决定了导航的整体流程。默认的行为树是：规划路径 → 跟踪路径 → 如果失败，执行恢复行为 → 重新规划。恢复行为包括Spin（原地旋转）、BackUp（后退）、Wait（等待）。
我一般会把Spin的角度设成90度，BackUp的距离设成0.3米，Wait的时间设成5秒。这些值可以根据你的场景调整。如果机器人经常在狭窄空间卡住，可以把BackUp距离设大一点。
7. 常见问题与排查技巧实录
7.1 TF树问题：最常见的报错来源
TF树问题是ROS2导航里最常见的报错。典型症状是RViz里机器人模型不显示，或者Nav2报"Timed out waiting for transform"。排查方法是：ros2 run tf2_tools view_frames生成TF树图，看有没有断链。
常见原因有三个：一是base_footprint到base_link的TF没发布；二是odom到base_footprint的TF没发布；三是时间戳不一致。前两个检查URDF和里程计插件，第三个检查use_sim_time参数。
7.2 导航时机器人原地转圈或抖动
这个问题通常是局部控制器参数没调好。先检查lookahead_dist是不是太小，太小会导致机器人频繁修正方向。然后检查min_approach_linear_velocity是不是太大，太大会导致机器人到目标点附近时来回震荡。最后检查代价地图的inflation_radius是不是太大，太大会导致机器人觉得到处都是障碍。
7.3 建图时地图漂移严重
地图漂移的根本原因是里程计不准。先检查轮距和轮径参数，这两个值必须和实际完全一致。然后检查IMU的yaw角方向，如果IMU的yaw和里程计的yaw方向相反，融合后会互相抵消，导致角度估计完全错误。最后检查激光雷达的安装位置，如果雷达不在机器人中心，转弯时会有额外的位移误差。
7.4 仿真里跑得好，实机上就崩
这是最典型的问题。仿真和实机的差异主要来自三个方面：传感器噪声、时间延迟、底盘响应。仿真里的雷达没有噪声，实机上有；仿真里的控制指令立即生效，实机上有延迟；仿真里的轮子不打滑，实机上会打滑。
解决方法是在仿真里加噪声模型，给控制指令加延迟，然后重新调参。实机上先低速跑，确认没问题再提速。我一般实机调试时线速度不超过0.15m/s，等稳定了再慢慢加。
7.5 常见问题速查表
| 问题 | 排查方向 | 快速解决 |
|---|---|---|
| RViz不显示地图 | 检查/map话题 | ros2 topic echo /map --once |
| 导航无路径 | 检查全局代价地图 | 看目标点是否在障碍物内 |
| 机器人不动 | 检查/cmd_vel | ros2 topic echo /cmd_vel |
| 定位丢失 | 检查AMCL粒子 | 重新初始化位姿 |
| 地图与雷达不重合 | 检查TF时间戳 | 统一use_sim_time |
| 建图卡顿 | 检查CPU占用 | 降低update_rate |
8. 从仿真到实机：迁移时要注意的几个细节
仿真跑通之后，迁移到实机是最后一步，也是最容易出问题的一步。我的经验是：先录bag，再离线调参，最后实机验证。具体做法是，在实机上手动控制机器人走一圈，用ros2 bag record录下/scan、/odom、/imu、/tf这几个话题，然后在电脑上回放bag，用SLAM Toolbox离线建图。这样你可以反复调参，不用担心实机撞坏。
实机部署时，有几个参数必须改：max_laser_range改成实际雷达的最大距离，robot_radius改成实际机器人半径，inflation_radius相应调整。另外，实机的odom话题频率可能和仿真不一样，需要在Nav2配置里调整expected_odom_rate。
最后说一个我踩过的坑：实机上电机的死区。仿真里给0.01m/s的速度轮子就会转，实机上给0.05m/s可能都不动。这个死区如果不处理，Nav2发很小的速度指令时机器人不动，但里程计认为它在动，结果就是定位漂移。解决方法是在底盘驱动里加一个死区补偿，或者把Nav2的最小速度设大一点。
这条链路我从头到尾跑了大概两个月，中间推翻重来了好几次。现在回头看，最难的不是某个算法，而是各个模块之间的衔接——TF、时间同步、坐标系转换，这些"胶水"部分才是真正花时间的地方。希望这篇复盘能帮你少走一些弯路。
