3、Costmap调试：全局/局部代价地图可视化、膨胀层参数调优、障碍物层传感器融合问题排查
代价地图（Costmap）是导航系统的“眼睛”。它告诉机器人哪里能走、哪里不能走。说白了，全局代价地图负责宏观路径规划，局部代价地图负责微观避障。这两张图要是出了问题，机器人要么撞墙，要么原地打转。
我个人习惯，调试导航的第一步就是打开代价地图可视化。别急着调参数，先看看地图长什么样。很多时候，问题一眼就能看出来。
3.1 代价地图可视化：先看再调
ROS2 里，代价地图默认会发布可视化话题。你只需要在 RViz2 里添加对应显示类型就行。
- 全局代价地图：话题名通常是
/global_costmap/costmap，显示类型选Map。 - 局部代价地图：话题名是
/local_costmap/costmap，同样用Map显示。 - 膨胀层：话题名
/global_costmap/inflated_costmap或/local_costmap/inflated_costmap，显示类型也是Map。 - 障碍物层：话题名
/global_costmap/obstacle_layer或/local_costmap/obstacle_layer，显示类型Map。
嗯，这里要注意：如果你看不到任何地图，先检查话题是否存在。用 ros2 topic list | grep costmap 看一眼。如果话题都没有，那就是代价地图节点没启动，或者配置错了。
3.2 膨胀层参数调优：别让机器人变成“胆小鬼”
膨胀层的作用，是把障碍物“扩大”一圈。这样机器人规划路径时，会主动避开障碍物边缘，而不是贴着走。但膨胀半径设得太大，机器人会变成“胆小鬼”——明明能过去的地方，它偏说过不去。
膨胀层的核心参数就几个，我列个表给你看：
| 参数名 | 默认值 | 作用 | 我的建议 |
|---|---|---|---|
inflation_radius |
0.55 m | 膨胀半径，障碍物向外扩展的距离 | 根据机器人尺寸调。小车 0.3-0.5m，大车 0.6-1.0m |
cost_scaling_factor |
10.0 | 代价衰减系数，值越大衰减越快 | 默认 10 通常够用。如果路径太贴墙，可以降到 5-8 |
inflate_unknown |
false | 是否对未知区域也做膨胀 | 建议设为 false，否则地图边缘会变成禁区 |
你想想看，inflation_radius 设成 0.55 米，意味着机器人中心距离障碍物至少要保持 0.55 米。如果你的机器人半径只有 0.2 米，那它离障碍物还有 0.35 米的余量，其实挺安全的。但如果你在狭窄走廊里，这个余量可能就太大了，机器人会卡在中间不敢动。
先调
inflation_radius，再调 cost_scaling_factor。半径决定“能不能过”，衰减系数决定“贴着走还是绕着走”。
我曾经在一个仓库项目里，机器人死活过不了一个 0.8 米宽的门口。明明车宽才 0.5 米，就是过不去。后来一看，inflation_radius 设了 0.6 米，加上车体半径 0.25 米，总共需要 0.85 米的空间。门口才 0.8 米，当然过不去。把膨胀半径降到 0.3 米，问题立刻解决。
3.3 障碍物层传感器融合：多传感器打架怎么办？
大多数机器人不止一个传感器。激光雷达、深度相机、声纳……它们各自上报障碍物信息。障碍物层的任务，就是把这些数据融合到一张代价地图里。
但融合过程经常出问题。我遇到过最典型的情况是：激光雷达扫到了桌腿，深度相机也扫到了同一根桌腿，结果代价地图上出现了“双重障碍物”——明明只有一个障碍物，地图上却显示两个，而且位置还有偏差。
为什么会这样？说白了，就是传感器之间的坐标变换（TF）没对齐。两个传感器各自发布障碍物，但它们的坐标系关系不对，导致同一个物体被映射到了地图上的不同位置。
排查步骤我总结了一下：
- 检查 TF 树：用
ros2 run tf2_tools view_frames生成 TF 树 PDF，确认所有传感器坐标系都正确连接到base_link。 - 查看传感器话题时间戳：用
ros2 topic echo /scan --once和ros2 topic echo /depth/points --once，对比时间戳是否同步。时间差超过 100ms 就可能出问题。 - 可视化传感器数据：在 RViz2 里同时显示
LaserScan和PointCloud2，看它们是否重合。如果不重合，那就是 TF 或标定问题。 - 检查障碍物层配置：打开障碍物层的 YAML 配置文件，确认
observation_sources列表里包含了所有传感器，并且每个传感器的topic、sensor_frame、data_type都正确。
voxel_filter，降低点云密度。在配置里加一行 obstacle_layer.voxel_filter.enabled: true 就行。
3.4 传感器融合配置示例
下面是一个典型的障碍物层配置，同时使用激光雷达和深度相机：
obstacle_layer:
enabled: true
observation_sources: laser_scan_sensor depth_sensor
laser_scan_sensor:
topic: /scan
sensor_frame: laser_frame
data_type: LaserScan
clearing: true
marking: true
inf_is_valid: false
depth_sensor:
topic: /depth/points
sensor_frame: camera_depth_frame
data_type: PointCloud2
clearing: true
marking: true
min_obstacle_height: 0.0
max_obstacle_height: 2.0
voxel_filter: true
voxel_size: 0.05注意看 clearing 和 marking 这两个参数。marking 表示这个传感器负责“标记”障碍物，clearing 表示它负责“清除”障碍物。如果一个传感器只标记不清除，那障碍物被移走后，地图上还会残留痕迹。我建议所有传感器都同时开启 clearing 和 marking，除非你有特殊需求。
3.5 知识体系总览
下面这张图总结了代价地图调试的核心逻辑：
3.6 常见问题速查表
最后，我整理了一份常见问题速查表。调试时遇到问题，先对照这张表看看：
| 现象 | 可能原因 | 排查方法 | 解决方案 |
|---|---|---|---|
| 代价地图全黑 | 没有收到传感器数据 | ros2 topic echo /scan 看是否有数据 |
检查传感器驱动是否启动 |
| 障碍物位置偏移 | TF 树错误 | view_frames 查看 TF 树 |
修正 TF 发布源 |
| 机器人不敢走窄道 | 膨胀半径太大 | 测量通道宽度，对比膨胀半径+车体半径 | 减小 inflation_radius |
| 路径贴着障碍物走 | 膨胀半径太小或衰减系数太大 | 可视化膨胀层，看代价分布 | 增大 inflation_radius 或减小 cost_scaling_factor |
| 障碍物移走后地图还有残留 | 传感器未开启 clearing |
检查障碍物层配置 | 在配置中添加 clearing: true |
| 代价地图更新卡顿 | 传感器数据量太大 | 查看 CPU 占用率 | 启用 voxel_filter 或降低传感器频率 |
代价地图调试，说白了就是“看图说话”。你看到的图不对，那路径规划肯定不对。别急着改代码，先打开 RViz2，把每一层地图都看一遍。很多时候，问题就藏在那些你忽略的细节里。