ROS2 SLAM与Nav2全链路实战：从传感器标定到Gazebo仿真

1. 从一台扫地机说起：为什么我要跑通这条全链路

去年帮朋友调一台自研扫地机器人，硬件堆得挺猛：单线机械式LiDAR、六轴IMU、轮式编码器、两个驱动轮加一个万向轮，主控是树莓派4B跑Ubuntu 22.04加ROS2 Humble。结果第一次上电跑建图，rviz2里那点云墙全是重影，走两米就飘，导航一开直接往墙上怼。折腾了三个周末才把SLAM和Nav2这条链路彻底跑顺，中间踩的坑比想象中多得多。

这篇就把整条链路拆开讲：从传感器数据进ROS2、LiDAR和IMU的标定、SLAM建图、地图保存与加载，到Nav2的代价地图配置、路径规划、恢复行为，最后在Gazebo里做仿真验证。关键词里提到的SLAM、Nav2、ROS2、LiDAR、Gazebo都会覆盖到，但我不打算写成一份API手册——那种东西官方文档更全。我想讲的是为什么这么配、参数背后的物理意义、以及实测中哪些地方最容易翻车。

适合谁看？如果你已经装好ROS2、能跑通turtlebot3的demo，但一到自己搭车或者换传感器就懵，那这篇就是给你写的。如果你连ROS2都还没装，建议先去把Humble装好、跑通小乌龟，再回来读，不然很多概念会悬空。

整条链路我按数据流顺序组织：传感器层（LiDAR+IMU+里程计）→ 状态估计与建图（SLAM）→ 地图持久化 → 定位与导航（Nav2）→ 仿真验证（Gazebo）。每一层都有它自己的坑，而且往往是上一层的误差传到下一层才爆发，所以排查时一定要顺着数据流往回找。

2. 传感器层：LiDAR、IMU与轮式里程计的数据对齐

2.1 三种传感器各自的脾气

先搞清楚每个传感器在SLAM里扮演什么角色，这决定了后面标定和融合的策略。

轮式里程计是最便宜的位姿来源，靠编码器数轮子转了多少圈，结合轮距算出位移和转角。它的优点是高频、连续、短时间平滑；缺点是累积误差，尤其是打滑、地面不平、轮胎磨损之后，误差涨得飞快。差速底盘还有个固有问题：原地旋转时两个轮子反向转，编码器读数在低速下噪声占比很高。

IMU提供角速度和线加速度。角速度对SLAM特别有用，因为它能补上轮式里程计在旋转上的短板。但IMU有零偏，静止时输出不是零而是一个缓慢漂移的小值，积分之后角度会越飘越远。六轴IMU没有磁力计，偏航角（yaw）没有绝对参考，只能靠其他传感器拉回来。

LiDAR是建图的主力，直接给出环境的三维（或二维）几何结构。单线LiDAR扫一个平面，适合室内平地；多线LiDAR能拿到三维点云，但数据量大、对算力要求高。LiDAR的问题是扫描频率通常只有5到10Hz，比里程计低一个数量级，而且运动畸变——机器人一边走一边扫，一帧点云里不同点的采集时刻不同，快速转弯时点云会明显扭曲。

提示：如果你的机器人原地旋转时建图飘得厉害，八成是轮式里程计的角速度不可信，而IMU又没融合进去。先别急着调SLAM参数，回去看传感器融合。

2.2 TF树：SLAM的骨架，错了全盘皆输

ROS2里所有传感器数据都要通过TF树关联到统一的坐标系。这条链路的TF关系是：

map → odom → base_link → laser / imu_link

• map → odom：由SLAM或定位模块发布，表示全局地图坐标系到里程计坐标系的修正。
• odom → base_link：由轮式里程计或融合后的状态估计发布，表示机器人相对起始点的位姿。
• base_link → laser、base_link → imu_link：静态变换，由URDF或static_transform_publisher发布，表示传感器安装位置。

我见过最常见的错误是：base_link → laser的静态变换写错了方向或者平移量。比如LiDAR装在车头前方10cm、高15cm，那平移应该是(0.10, 0, 0.15)，旋转取决于LiDAR的朝向。如果这个变换错了，建出来的地图和实际环境会有一个固定的偏移或旋转，导航时机器人会系统性地撞向一侧。

用ros2 run tf2_tools view_frames生成TF树PDF，肉眼检查每个变换是否合理。再用ros2 run tf2_ros tf2_echo map base_link实时看位姿输出，机器人静止时这个值应该基本不动，如果一直缓慢漂移，说明map → odom的修正没生效或者里程计在飘。

2.3 LiDAR与IMU的标定：别跳过这一步

很多人拿到传感器直接插上就跑，结果SLAM效果差就怪算法。实际上LiDAR和IMU之间的外参（相对位置和朝向）如果不准，融合就是错的。

标定的目标是求出base_link → imu_link和base_link → laser这两个变换的精确值。工程上常用两种做法：

手工测量法：拿卷尺量传感器相对机器人几何中心的平移，用水平仪和角度尺估旋转。精度大概到厘米和几度，对室内低速扫地机够用。这是最省事的方法，我大部分项目都先用这个跑通，再考虑精细化。

算法标定法：用lidar_align这类工具，让机器人走一段有丰富几何特征的路，通过优化点云匹配和IMU积分的一致性来反解外参。精度更高，但需要采集质量好的数据，而且对初值敏感。

IMU本身还需要做零偏标定。静止放置几分钟，采集几千帧数据，求角速度和加速度的均值作为零偏，写进配置或者用imu_filter_madgwick这类节点在线估计。零偏不除，yaw角积分几分钟就能飘几十度。

注意：LiDAR和IMU的时间戳必须同步。如果两者时间戳差了几十毫秒，快速运动时融合会引入明显误差。检查方法是用ros2 topic hz看频率，用ros2 topic delay看延迟，必要时用message_filters做时间对齐。

2.4 运动畸变校正：快速转弯时的隐形杀手

单线LiDAR一帧扫描需要100ms左右（10Hz），这100ms里机器人如果转了30度，点云就会明显扭曲。校正的思路是用IMU或里程计在这100ms内的运动，把每个点反投影到帧起始时刻的坐标系。

很多SLAM算法（比如slam_toolbox）内部已经做了简单的运动畸变补偿，但补偿质量取决于你喂给它的里程计和IMU数据质量。如果发现快速转弯时地图出现扇形拖影，先确认IMU数据有没有正确融合，再考虑换用支持更精细去畸变的算法。

3. SLAM建图：slam_toolbox的配置逻辑与实测调参

3.1 为什么选slam_toolbox而不是cartographer

ROS2生态里主流的2D SLAM方案有slam_toolbox、cartographer、hector_slam。我选slam_toolbox的理由很实际：

• 原生支持ROS2，维护活跃，Humble下开箱即用。
• 支持在线和离线建图，还能做 lifelong mapping（长期建图），适合扫地机这种需要反复建图的场景。
• 参数直观，调参门槛比cartographer低。cartographer的lua配置对新手不太友好。
• 对里程计依赖适中，有里程计就用，没有也能靠LiDAR scan matching跑（虽然效果差些）。

cartographer精度可能略高，但配置复杂、编译重，对树莓派这种算力紧张的平台不友好。hector_slam不需要里程计，但对LiDAR频率要求高，单线低速LiDAR效果一般。综合下来slam_toolbox是性价比最高的选择。

3.2 关键参数逐个拆解

slam_toolbox的配置项很多，但真正影响建图效果的就那么几个。我按重要性排序讲。

resolution：地图分辨率，默认0.05米。这个值决定了地图的精细度和内存占用。0.05意味着每个栅格5cm，对扫地机够用。调到0.02会更精细但内存翻几倍，树莓派可能吃不消。别低于0.02，没意义。

max_laser_range：LiDAR最大有效距离。单线LiDAR标称12米，但实际室内反射和噪声下，超过6到8米的点就不可靠了。我一般设成实际有效距离的80%，比如标称12米就设9米。设太大，远处噪声点会污染地图；设太小，大房间建不全。

minimum_travel_distance和minimum_travel_heading：机器人移动多少距离或转多少角度才插入新的一帧。默认0.5米和0.5弧度。调大能减少计算量但地图更新慢，调小地图精细但算力吃紧。扫地机慢速移动，我一般设0.3米和0.3弧度。

map_update_interval：地图发布间隔，默认5秒。这个不影响建图质量，只影响rviz2里看到的刷新频率。调小看着流畅但占带宽，调大省资源。调试时设1秒方便观察，部署时设5秒。

loop_closure相关参数：回环检测是消除累积误差的关键。do_loop_closing设为true，loop_search_maximum_distance控制搜索范围。如果建图时发现走回原点后地图对不上，八成是回环没检测到，可以适当增大搜索距离或者降低匹配阈值。

correlation相关参数：scan matching的核心参数。correlation_search_space_dimension控制搜索窗口大小，correlation_search_space_resolution控制搜索精度。这两个参数直接决定scan matching能不能找到正确的匹配。窗口太小，快速运动时匹配失败；窗口太大，计算量爆炸还容易误匹配。默认值对低速扫地机基本够用，如果发现建图漂移严重再调。

3.3 建图实操：从启动到保存

启动slam_toolbox的典型launch：

ros2 launch slam_toolbox online_async_launch.py \ slam_params_file:=/path/to/mapper_params.yaml \ use_sim_time:=false

online_async是异步模式，建图和发布地图在不同线程，适合算力有限的平台。online_sync是同步模式，精度略高但会阻塞，树莓派上容易卡。

建图过程中用rviz2观察：

• 添加Map显示，看栅格地图。
• 添加LaserScan显示，看实时点云和地图是否对齐。
• 添加TF显示，确认坐标系正常。

建图完成后保存地图：

ros2 run nav2_map_server map_saver_cli -f /path/to/my_map

这会生成my_map.pgm（图像）和my_map.yaml（元数据）两个文件。yaml里记录了分辨率、原点、阈值等信息，导航时要用。

提示：保存地图前，让机器人回到起点附近，确认回环已经闭合、地图没有明显重影。如果地图有重影，先解决SLAM问题再保存，否则导航时定位会一直抖。

3.4 建图质量的判断标准

怎么知道建得好不好？我一般看三点：

墙壁是否直：室内墙壁应该是直线，如果建出来是波浪形或者有重影，说明scan matching或里程计有问题。

回环是否闭合：走一圈回到起点，起点附近的墙壁应该和出发时重合。如果错开一大截，回环检测失败。

角落是否清晰：直角角落应该清晰锐利，如果糊成一团，说明分辨率不够或者点云噪声大。

这三点都过了，地图基本可用。如果只有局部有问题，可能是那个区域特征太少（比如长走廊），scan matching找不到足够的几何约束，这种地方建图时慢一点走，或者手动补几圈。

4. 从地图到导航：Nav2的代价地图与行为树配置

4.1 Nav2的架构：不是单个节点，是一套系统

很多人以为Nav2就是一个导航节点，其实它是一整套节点加行为树的组合。核心组件包括：

• map_server：加载静态地图，发布/map话题。
• amcl：蒙特卡洛定位，在已知地图上估计机器人位姿。
• planner_server：全局路径规划，用A*或Dijkstra算从当前位置到目标的最短路径。
• controller_server：局部路径规划和控制，用DWA或TEB跟踪全局路径并避障。
• behavior_server：恢复行为，比如卡住时旋转、后退、清空代价地图。
• bt_navigator：行为树导航器，编排整个导航流程。

这些节点通过行为树（Behavior Tree）串联。行为树的好处是逻辑清晰、可定制，坏处是配置复杂，新手容易懵。我的建议是先跑通默认配置，再根据需求改。

4.2 代价地图：Nav2的"眼睛"

代价地图（Costmap）是Nav2的核心数据结构，把环境表示成栅格，每个栅格有一个代价值，表示通过该位置的代价。代价值从0（自由）到255（致命障碍），中间还有膨胀层产生的渐变值。

Nav2有两层代价地图：

全局代价地图：基于静态地图，覆盖整个已知环境，用于全局规划。它更新频率低，主要处理静态障碍。

局部代价地图：基于实时传感器数据，覆盖机器人周围一小块区域（比如6米×6米），用于局部避障。它更新频率高，能处理动态障碍。

代价地图的插件层是关键：

• static_layer：加载静态地图。
• obstacle_layer：用LiDAR实时数据标记障碍。
• inflation_layer：在障碍周围生成膨胀区，让机器人保持安全距离。
• voxel_layer：三维体素层，处理三维点云（如果用三维LiDAR）。

inflation_radius和cost_scaling_factor是最常调的两个参数。inflation_radius是膨胀半径，应该略大于机器人内切圆半径。比如机器人宽30cm，内切圆半径15cm，膨胀半径设20到25cm比较合适。设太小机器人会贴着墙走，容易刮蹭；设太大窄通道过不去。

cost_scaling_factor控制代价值随距离衰减的速度。值越大衰减越快，机器人越倾向于远离障碍；值越小衰减越慢，机器人更愿意靠近障碍走。默认值3.0对大多数场景够用。

4.3 全局与局部规划器的选型

全局规划器常用NavfnPlanner（Dijkstra）或SmacPlanner（混合A*）。Navfn简单快，适合大多数室内场景。SmacPlanner支持更复杂的运动学约束，适合阿克曼底盘或者需要平滑路径的场景。扫地机是差速底盘，Navfn够用。

局部规划器常用DWBLocalPlanner或TEBLocalPlanner。DWB是DWA的ROS2实现，计算快、参数多、调参麻烦但灵活。TEB基于时间弹性带，路径更平滑但计算量大，树莓派上可能跑不动。我一般先用DWB，调不好再考虑TEB。

DWB的关键参数：

• max_vel_x、max_vel_theta：最大线速度和角速度。扫地机一般0.2到0.3m/s，角速度1.0到1.5rad/s。
• acc_lim_x、acc_lim_theta：加速度限制。设太大机器人会猛冲猛停，设太小反应迟钝。
• xy_goal_tolerance、yaw_goal_tolerance：到达目标的容差。扫地机不需要太精确，0.1米和0.2弧度够用。

4.4 行为树：导航失败时的救命稻草

行为树定义了导航的完整流程：先规划全局路径，然后局部跟踪，如果卡住就触发恢复行为，恢复失败就重新规划，实在不行就放弃。

默认的行为树navigate_to_pose_w_replanning_and_recovery.xml包含这些恢复行为：

• ClearCostmap：清空代价地图，清除可能误标的障碍。
• Spin：原地旋转，尝试重新定位或脱困。
• BackUp：后退一段距离。
• Wait：等待一段时间。

这些恢复行为按顺序尝试，一个失败就试下一个。实测中Spin和BackUp最有用，ClearCostmap偶尔能解决传感器误标的问题。

如果默认行为树不够用，可以自定义。比如扫地机需要"沿墙走"模式，可以加一个FollowWall行为。行为树用XML描述，改起来不算难，但要理解每个节点的语义。

注意：恢复行为的参数要调好。Spin的旋转角度太小脱不了困，太大浪费时间。我一般设90度，转一圈试试。BackUp的距离设0.3到0.5米，太短没用，太长可能撞到后面的东西。

4.5 AMCL定位：地图和现实的桥梁

AMCL（Adaptive Monte Carlo Localization）用粒子滤波在地图上估计机器人位姿。它订阅LiDAR数据和里程计，输出map → odom变换。

关键参数：

• min_particles、max_particles：粒子数范围。粒子越多定位越准但计算越慢。树莓派上我一般设500到2000。
• laser_model_type：激光模型，likelihood_field适合大多数场景。
• update_min_d、update_min_a：移动多少距离或角度才更新定位。设太小计算频繁，设太大定位滞后。
• initial_pose：初始位姿。如果机器人每次从固定位置出发，可以设初始位姿，省去手动初始化的麻烦。

AMCL最常见的坑是初始定位失败。如果机器人启动时位置和地图对不上，AMCL会发散，表现为rviz2里粒子云散得到处都是。解决办法是在rviz2里用2D Pose Estimate手动给一个粗略初始位姿，或者让机器人原地旋转一圈让AMCL自己收敛。

5. Gazebo仿真：在虚拟环境里验证全链路

5.1 为什么仿真不能省

真机调试成本高：撞坏了要修，电池要充，场地要占。Gazebo仿真能在电脑上快速验证算法逻辑，改参数不用重新烧录，跑一百次也不累。而且仿真里能拿到真值（ground truth）位姿，方便对比SLAM和定位的误差。

但仿真和真机有差距：传感器噪声模型是简化的，物理接触是近似的，轮子打滑和地面摩擦跟真实不一样。所以仿真跑通不代表真机一定行，但仿真跑不通真机肯定不行。我的流程是：仿真验证逻辑 → 真机调参数 → 回仿真复现问题。

5.2 搭建扫地机仿真模型

Gazebo里建机器人模型用URDF或SDF。URDF更常用，因为ROS2生态支持好。一个差速扫地机的URDF包含：

• base_link：车体，一个扁圆柱或长方体。
• left_wheel_link、right_wheel_link：驱动轮，用continuous关节连接。
• caster_link：万向轮，用fixed或continuous关节。
• laser_link：LiDAR安装位置。
• imu_link：IMU安装位置。

每个link要配inertial（质量、惯量矩阵）、visual（外观）、collision（碰撞体）。惯量矩阵别乱填，填错了机器人会抖或者翻。简单形状的惯量有公式，圆柱体绕中心轴的惯量是0.5*m*r^2。

Gazebo插件负责仿真传感器和执行器：

<gazebo> <plugin name="diff_drive" filename="libgazebo_ros_diff_drive.so"> <left_joint>left_wheel_joint</left_joint> <right_joint>right_wheel_joint</right_joint> <wheel_separation>0.3</wheel_separation> <wheel_diameter>0.06</wheel_diameter> <max_wheel_torque>20</max_wheel_torque> <max_wheel_acceleration>1.0</max_wheel_acceleration> <publish_odom>true</publish_odom> <publish_odom_tf>true</publish_odom_tf> <publish_wheel_tf>true</publish_wheel_tf> <odometry_frame>odom</odometry_frame> <robot_base_frame>base_link</robot_base_frame> </plugin> </gazebo>

LiDAR插件用libgazebo_ros_ray_sensor.so，配置扫描角度、分辨率、范围、噪声。IMU插件用libgazebo_ros_imu_sensor.so，配置噪声和更新率。

5.3 仿真环境搭建的坑

世界文件：Gazebo自带一些世界，但室内场景往往要自己搭。可以用Building Editor画墙，或者导入现成的.world文件。墙的厚度、高度、材质都会影响LiDAR反射，太薄的墙可能扫不到。

物理参数：step_size（仿真步长）默认0.001秒，调大能加速仿真但精度下降。real_time_update_rate控制仿真速度和真实时间的比例，设0表示全速跑。调试时设1.0（实时）方便观察，批量测试时设0加速。

传感器噪声：仿真LiDAR默认没噪声，建图完美。但这不真实，真机LiDAR有噪声、有盲区、有反射问题。建议在插件里加高斯噪声，让仿真更接近真实。噪声参数参考传感器手册，单线LiDAR的距离噪声标准差一般1到3cm。

TF发布：Gazebo插件会发布odom → base_link，但base_link → laser和base_link → imu_link需要URDF里的关节配合robot_state_publisher发布。如果TF树断了，SLAM和Nav2都跑不起来。用view_frames检查。

5.4 仿真里跑通全链路的流程

1. 启动Gazebo世界和机器人模型：

ros2 launch my_robot_description gazebo.launch.py

2. 启动slam_toolbox建图：

ros2 launch slam_toolbox online_async_launch.py use_sim_time:=true

3. 用teleop_twist_keyboard控制机器人走一圈：

ros2 run teleop_twist_keyboard teleop_twist_keyboard

4. 保存地图，启动Nav2：

ros2 launch nav2_bringup bringup_launch.py \ map:=/path/to/my_map.yaml \ use_sim_time:=true

5. 在rviz2里用2D Pose Estimate给初始位姿，用2D Goal Pose发目标点，看机器人能不能自主导航过去。

仿真里跑通后，把同样的配置搬到真机，改掉use_sim_time和传感器话题名，基本就能用。真机上再微调参数，主要是速度限制和膨胀半径。

6. 踩坑实录：那些让我熬夜的报错与修复

6.1 TF树断裂：最常见的"导航不动"元凶

现象：Nav2启动后机器人不动，rviz2里报Timed out waiting for transform from base_link to map。

排查链路：

1. ros2 run tf2_tools view_frames看TF树，发现map → odom缺失。
2. 检查AMCL有没有启动，ros2 node list看amcl在不在。
3. AMCL在但没发布TF，看它的日志，发现Failed to transform laser scan。
4. 再查base_link → laser，发现URDF里关节名写错，robot_state_publisher没发布这个变换。

修复：改URDF关节名，重启。教训是每次改URDF后都要用view_frames确认TF树完整，别等导航报错才查。

6.2 建图重影：里程计和LiDAR打架

现象：建图时墙壁有双层重影，走直线也飘。

排查链路：

1. 静止时看odom → base_link，发现缓慢漂移，说明轮式里程计有零偏。
2. 检查编码器分辨率参数，发现wheel_separation设错了，实际轮距0.3米写成了0.25米。
3. 轮距错了导致角速度计算错误，旋转时误差累积。

修复：重新测量轮距，改参数。教训是轮距、轮径这些机械参数一定要实测，别抄别人的配置。

6.3 Nav2规划失败：代价地图膨胀太大

现象：机器人能定位但规划不出路径，报Failed to create a plan。

排查链路：

1. rviz2里看全局代价地图，发现膨胀层把窄通道完全堵死了。
2. 查inflation_radius，设了0.5米，而通道宽度只有0.6米。
3. 膨胀半径大于通道半宽，通道被完全覆盖，规划器认为不可通行。

修复：把inflation_radius降到0.25米，通道能过了。教训是膨胀半径要根据实际通道宽度调，不是越大越安全。

6.4 Gazebo仿真跑得比蜗牛慢

现象：Gazebo里机器人动一下卡三秒，实时率只有0.1。

排查链路：

1. 看CPU占用，Gazebo吃满一个核。
2. 检查世界文件，发现导入了一个超精细的模型，几万个三角面。
3. LiDAR插件设了1000线扫描，每帧计算量巨大。

修复：简化模型，LiDAR降到360线，实时率回到0.8。教训是仿真里模型精度够用就行，别追求视觉逼真。

6.5 AMCL定位跳变：粒子太少

现象：导航过程中机器人位姿突然跳变，路径跟着乱。

排查链路：

1. 看rviz2里粒子云，发现粒子数很少且集中，一旦遇到相似环境就发散。
2. 查min_particles，设了100，太少。
3. 环境里有长走廊，几何特征重复，粒子滤波容易混淆。

修复：min_particles提到500，max_particles提到2000，定位稳定了。教训是特征重复的环境要增加粒子数，或者加其他传感器辅助。

7. 真机部署前的最后检查清单

仿真跑通、参数调好之后，别急着上真机。我一般按这个清单过一遍：

传感器层：

• LiDAR和IMU的时间戳是否同步，延迟是否可接受。
• IMU零偏是否标定，静止时角速度输出是否接近零。
• TF树是否完整，每个变换的平移和旋转是否合理。
• 轮式里程计的轮距、轮径是否实测准确。

SLAM层：

• 建图时墙壁是否直，回环是否闭合，角落是否清晰。
• 地图保存后重新加载，定位是否能收敛。
• 长时间建图内存是否稳定，有没有泄漏。

Nav2层：

• 代价地图膨胀半径是否适配最窄通道。
• 全局和局部规划器参数是否匹配机器人运动能力。
• 恢复行为是否有效，卡住时能不能自己脱困。
• 速度限制是否安全，急停距离是否够。

仿真与真机差异：

• 仿真里的噪声模型是否接近真机。
• 真机的地面摩擦、轮子打滑是否在仿真里体现。
• 真机的算力是否够跑所有节点，有没有节点掉帧。

这份清单看着啰嗦，但每一条我都踩过对应的坑。真机调试时间宝贵，能提前在仿真和配置里排除的问题，就别留到现场。

最后分享一个我常用的调试技巧：用ros2 bag录下所有传感器话题和TF，出问题时回放。真机上问题往往一闪而过，录包之后可以反复回放、逐帧分析，比在现场盯着看高效得多。录包命令：

ros2 bag record /scan /imu/data /odom /tf /tf_static /cmd_vel

回放时用ros2 bag play --clock，配合use_sim_time:=true，就能在电脑上完整复现真机场景。这个习惯帮我定位过好几个偶发bug，强烈推荐。