4、代价地图基础：代价地图的图层结构、代价值含义与更新机制

在ROS2的导航体系中，代价地图（Costmap）是连接感知与规划的核心枢纽。它并非一张简单的二维栅格图，而是一个由多个图层（Layer）叠加而成的复合数据结构。Nav2通过插件化架构管理这些图层，每个图层负责一种特定的环境语义，最终融合为一张供全局规划器（Global Planner）和局部规划器（Local Planner）使用的统一代价图。理解代价地图的图层结构、代价值的量化含义以及其更新机制，是进行安全避障和路径优化的前提。本章将深入剖析这三个核心维度，帮助你在实际机器人部署中精准调优。

上图展示了Nav2代价地图的核心数据流。传感器数据（激光雷达、深度相机等）分别注入静态层、障碍物层和膨胀层，经过融合算法生成Master代价地图，供全局和局部规划器使用。规划器输出的路径又反过来影响控制指令，形成一个闭环的实时避障系统。下面我们逐一拆解每个环节的细节。

4.1 图层结构：三层各司其职

Nav2的代价地图采用分层架构，每一层都是一个独立的插件，通过plugin_names和plugin_types参数在costmap_common_params.yaml中声明。这种设计使得开发者可以灵活组合，甚至编写自定义图层。默认情况下，最常用的三个图层是静态层（Static Layer）、障碍物层（Obstacle Layer）和膨胀层（Inflation Layer）。

4.1.1 静态层：环境先验知识的载体

静态层通常加载预先构建的SLAM二维栅格地图（如map_server发布的nav_msgs/OccupancyGrid消息）。它提供了环境的固定结构信息，例如墙壁、柱子、固定家具等。该图层在机器人启动时加载，除非地图被重新加载，否则不会主动更新。静态层的代价值直接来源于地图的占据概率：占据概率越高，代价值越大。在配置中，map_topic指定地图话题，track_unknown_space参数决定是否将未知区域（值为-1）视为可通行区域。

4.1.2 障碍物层：实时感知的动态屏障

障碍物层是避障的核心。它订阅传感器数据（如sensor_msgs/LaserScan或sensor_msgs/PointCloud2），通过射线追踪（Ray Tracing）算法将传感器检测到的点云或扫描数据投影到代价地图的栅格中。每个栅格根据传感器测量值被标记为空闲（Free）、占据（Occupied）或未知（Unknown）。该层支持多个传感器输入，通过observation_sources参数配置多个数据源，并可为每个源设置clearing（清除）和marking（标记）功能。例如，激光雷达的扫描线经过的区域会被标记为Free，而击中点则被标记为Occupied。

4.1.3 膨胀层：安全距离的数学表达

膨胀层本身不直接接收传感器数据，而是基于前两层（尤其是障碍物层）的结果进行二次处理。它将障碍物周围的栅格按照机器人半径和用户设定的膨胀半径（inflation_radius）进行代价梯度衰减。膨胀层的核心作用是防止机器人因自身尺寸而碰撞障碍物，同时为规划器提供平滑的代价梯度，引导路径远离障碍物。膨胀代价的计算通常采用指数衰减函数：距离障碍物越近，代价值越高，直至达到致命代价（100）。

4.2 代价值的含义：从0到100的量化决策

代价地图中的每个栅格都存储一个0-100的整数值，这个值直接决定了规划器的行为。理解这些数值的物理意义是调参的基础。

• 0（Free Space）：完全空闲，机器人可以自由通行，规划器优先选择此类区域。
• 1-49（Low Cost）：低风险区域，通常位于膨胀区的边缘。规划器会尽量避免，但在必要时可以穿越。
• 50-99（High Cost）：高风险区域，靠近障碍物。规划器会将其视为“软约束”，除非无路可走，否则不会选择。
• 100（Lethal）：致命障碍，机器人不可进入。这包括障碍物占据的栅格以及膨胀层中距离障碍物小于机器人内切半径的区域。
• -1（Unknown）：未知区域，通常出现在地图边界或传感器盲区。是否可通行取决于track_unknown_space参数。若设为true，未知区域被视为致命；若为false，则视为自由。

4.3 更新机制：从传感器到代价栅格的完整链路

代价地图的更新是一个多线程、事件驱动的过程。Nav2的Costmap2DROS节点维护一个内部循环，以update_frequency（默认5Hz）和publish_frequency（默认0Hz）运行。更新流程如下：

1. 数据采集：障碍物层订阅传感器话题，将点云或扫描数据存入缓冲区。
2. 射线追踪：对每个传感器测量值，从传感器原点出发，沿射线方向遍历栅格。射线经过的栅格标记为Free，射线终点（即击中点）标记为Occupied。
3. 图层融合：Master代价地图遍历所有图层，对于每个栅格，取各图层中最大的代价值作为最终值。这种“最大值融合”策略确保了安全性——任何图层认为危险，整体即为危险。
4. 膨胀处理：膨胀层在障碍物层更新后，对障碍物周围的栅格进行代价梯度赋值。该过程使用BFS（广度优先搜索）算法，从每个致命障碍栅格向外扩散，直到达到inflation_radius。
5. 发布与使用：融合后的代价地图通过/costmap话题发布，供规划器订阅。规划器在每次路径计算时读取当前代价地图快照。

# costmap_common_params.yaml 关键配置示例
global_costmap:
  global_frame: map
  robot_base_frame: base_footprint
  update_frequency: 5.0
  publish_frequency: 2.0
  plugins:
    - {name: static_layer, type: "nav2_costmap_2d::StaticLayer"}
    - {name: obstacle_layer, type: "nav2_costmap_2d::ObstacleLayer"}
    - {name: inflation_layer, type: "nav2_costmap_2d::InflationLayer"}
  obstacle_layer:
    observation_sources: laser_scan_sensor
    laser_scan_sensor:
      topic: /scan
      data_type: LaserScan
      marking: true
      clearing: true
      raytrace_range: 5.0
  inflation_layer:
    inflation_radius: 0.55
    cost_scaling_factor: 3.0

4.4 实战案例：动态障碍物避让中的图层协同

假设一个室内服务机器人在走廊中执行送药任务，走廊宽度1.5米，机器人半径0.3米。初始时，静态层加载了走廊地图，障碍物层无数据，膨胀层将墙壁边缘向外扩展0.55米（机器人半径+0.25米安全余量）。此时规划器生成的路径位于走廊中央。

当一个人（宽度约0.4米）突然从侧面房间走出，站在走廊中时，激光雷达检测到该人，障碍物层立即将该区域标记为Occupied。膨胀层随即在0.55米半径内生成梯度代价。此时，走廊的有效通行宽度变为1.5 - 0.4 - 2*0.55 = 0米，理论上无法通行。但实际中，膨胀代价并非全为100，距离人0.55米处的代价约为50，距离0.3米处为100。规划器会尝试寻找一条绕过人的路径——如果走廊足够宽，它会偏向另一侧墙壁，但代价地图会显示另一侧墙壁的膨胀代价也较高，最终规划器可能选择减速并等待人离开，或者寻找一个代价总和最小的折中路径。

在这个案例中，三个图层各司其职：静态层提供了走廊边界约束，障碍物层实时捕捉了人的位置，膨胀层则量化了“靠近人”和“靠近墙”的风险。通过调整cost_scaling_factor（值越小，衰减越慢），可以让机器人更早地开始避让，但也会导致路径过度保守。

4.5 常见误区与调优总结

在代价地图的使用中，工程师常陷入以下误区：

• 误区一：所有图层使用相同的更新频率。静态层无需高频更新，而障碍物层需要高频响应。应根据传感器特性和场景动态程度分别设置。
• 误区二：忽略未知区域的处理。在室外或GPS-denied环境中，未知区域过多会导致规划失败。应结合传感器覆盖范围合理设置track_unknown_space。
• 误区三：膨胀半径设置过大。虽然安全，但会严重限制机器人在狭窄通道中的通行能力。建议膨胀半径 = 机器人外接圆半径 + 0.1~0.2米。
• 误区四：障碍物层只使用单一传感器。激光雷达在低矮障碍物（如台阶）面前失效，而深度相机在强光下失效。多传感器融合（如激光+声纳）可以显著提高鲁棒性。

最后，务必记住：代价地图是“感知”与“决策”之间的桥梁。一个调优良好的代价地图，应当让规划器在绝大多数情况下“感觉不到”膨胀层的存在，只在真正危险时提供平滑的避让引导。通过本章的学习，你已经掌握了图层结构、代价值语义和更新机制，下一步可以深入探索如何利用AI识别结果动态修改代价地图，实现更智能的避障策略。