3、代价地图：全局代价地图与局部代价地图、膨胀层与障碍物层、代价地图的更新机制

代价地图这东西，说白了就是机器人的「安全距离感知器」。我刚开始做导航时，总觉得它就是个简单的黑白图——能走的地方是白的，不能走的是黑的。后来在仓库项目里撞了三次货架才明白，代价地图的学问深着呢。

今天咱们就把代价地图拆开揉碎了讲。你想想看，机器人要安全地从一个点走到另一个点，光知道哪里有障碍物够吗？不够。它还得知道「离障碍物多远算安全」、「哪些障碍物是临时的」、「地图要不要实时更新」。这些，全是代价地图的活。

3.1 全局代价地图 vs 局部代价地图

ROS2 里维护着两张代价地图：一张全局的，一张局部的。它们分工明确，各管一摊。

全局代价地图，我习惯叫它「老地图」。它基于静态地图（比如 SLAM 建好的图）生成，更新频率很低。主要给全局路径规划用——规划出一条从起点到终点的粗路径。

局部代价地图，我叫它「实时地图」。它只关心机器人周围一小块区域（比如半径 3 米），更新频率很高（10Hz 以上）。主要给局部路径规划用——实时躲避动态障碍物。

我在项目中遇到过一个问题：全局地图更新太慢，导致机器人明明看到前面有临时堆的纸箱，全局路径还是从纸箱中间穿过去。后来我把局部地图的更新频率调高，同时让局部规划器优先参考局部代价地图，问题就解决了。

3.2 膨胀层与障碍物层

代价地图不是一张简单的二值图。它由多个「层」叠加而成。最核心的两层是：障碍物层和膨胀层。

障碍物层负责记录传感器检测到的障碍物。激光雷达扫到一堵墙，障碍物层就在那个位置标记为「不可通行」。它是最原始的数据层。

膨胀层，嗯，这才是关键。它把障碍物「往外扩一圈」。为什么？因为机器人有尺寸啊！你不能让机器人贴着墙走，得留出安全距离。

膨胀的规则很简单：离障碍物越近，代价越高。通常用「衰减函数」来计算——距离障碍物 0 米处代价最高（255），距离等于机器人半径处代价降为 0。

我建议你把膨胀半径设成机器人半径的 1.2 倍。太小了容易刮蹭，太大了机器人钻不过窄门。我在一个医院项目里吃过亏——膨胀半径设了 0.8 米，结果机器人死活进不去 0.9 米宽的电梯门。后来改成 0.5 米，配合精确的定位，顺利通过。

3.3 代价地图的更新机制

代价地图怎么更新？说白了就是「传感器数据 → 障碍物层 → 膨胀层 → 叠加 → 输出」。但这里面有几个坑。

更新流程：

1. 传感器（激光雷达/深度相机）获取点云或激光数据
2. 障碍物层将数据投影到代价地图坐标系，标记障碍物位置
3. 膨胀层根据障碍物位置，计算周围区域的代价
4. 各层叠加，生成最终的代价地图
5. 局部规划器读取代价地图，规划避障路径

更新策略有两种：

• 覆盖式更新：新数据直接覆盖旧数据。适合动态障碍物频繁的场景。
• 衰减式更新：旧数据逐渐衰减，新数据权重更高。适合传感器有噪声的场景。

我曾经在工厂里遇到一个诡异的问题：机器人老是突然急刹车，但周围明明没有障碍物。查了半天，发现是代价地图的「衰减时间」设得太短——激光雷达偶尔扫到地面反光点，被当成障碍物，但还没来得及衰减就又扫到了。后来我把衰减时间从 0.5 秒调到 2 秒，同时加了高度滤波，问题消失。

3.4 核心知识体系

下面这张图，是我自己总结的代价地图知识框架。你一看就明白各层之间的关系。

3.5 配置实战

ROS2 中代价地图的配置在 YAML 文件里。我贴一份我常用的配置，你直接拿去改参数就行。

# costmap_common_params.yaml
global_frame: map
robot_base_frame: base_footprint
update_frequency: 5.0    # 全局地图更新频率
publish_frequency: 2.0   # 发布频率

# 膨胀层配置
inflation_layer:
  enabled: true
  inflation_radius: 0.55  # 膨胀半径（米）
  cost_scaling_factor: 10.0  # 代价衰减系数

# 障碍物层配置
obstacle_layer:
  enabled: true
  observation_sources: laser_scan_sensor
  laser_scan_sensor:
    sensor_frame: laser_frame
    data_type: LaserScan
    topic: /scan
    marking: true
    clearing: true
    obstacle_range: 2.5
    raytrace_range: 3.0

嗯，代价地图的内容就这些。你记住三句话：全局管远、局部管近、膨胀管安全。剩下的就是多调参、多试错。我在项目里踩过的坑，你大概率也会遇到——到时候翻翻这节内容，应该能帮你省点时间。