上篇聊了Nav2的整体架构，提到代价地图是导航系统的"眼睛"。规划器再聪明，如果代价地图给的世界是错的，算出来的路径也不可能靠谱。

代价地图（Costmap）这个名字听起来挺抽象。说白了就是一张二维网格图，每个格子有一个数值，表示这个位置"能不能走"以及"走了有多大风险"。0表示畅通无阻，255表示绝对走不了（有障碍物），中间的值表示"能走但要小心"。

机器人看世界不像我们看照片那么丰富。在代价地图里，世界被简化成了一堆数字格子。但别小看这个简化——导航需要的信息基本都浓缩在这张图里了。

一、代价地图的分层结构

Nav2的代价地图不是一张简单的静态图，而是多个图层叠加的结果。

static_layer（静态层）——来自SLAM建好的地图。墙壁、柱子这些固定障碍物在这一层标记出来。这一层基本不变，机器人启动时加载一次就行。

obstacle_layer（障碍物层）——来自实时传感器数据。激光雷达、深度相机、超声波传感器检测到的动态障碍物都更新在这一层。人走过来了、椅子被挪动了，这一层会实时反映。

inflation_layer（膨胀层）——在障碍物周围生成一圈"缓冲区"。离障碍物越近，代价值越高。这一层不添加新数据，只是把已有障碍物的影响范围扩大。

最终代价 = static_layer + obstacle_layer + inflation_layer

每个图层独立计算，最后叠加成一张完整的代价地图。这种分层设计的好处是职责清晰——静态地图归静态层管，实时避障归障碍物层管，安全距离归膨胀层管。

二、膨胀层：为什么机器人不走"贴墙"路线

膨胀层是代价地图里最容易被忽视但极其重要的一层。

没有膨胀层会怎样？规划器算出来的路径可能贴着墙根走，离障碍物只有几厘米。理论上这条路是"可行的"，但实际执行时传感器噪声、定位误差、控制偏差一叠加，机器人就撞上去了。

膨胀层的做法很简单：以障碍物为中心，向外扩展一定距离，代价值随距离递减。

cost(d) = (254 - 1) * exp(-1.0 * weight * (d - inscribed_radius)) + 1

d是当前格子到障碍物的距离，inscribed_radius是机器人的内接圆半径（机器人最"瘦"的方向），weight是衰减权重。

离障碍物越远，代价值越低。到了一定距离（通常设为机器人半径的2-3倍），代价值降到接近0。

import numpy as np
import matplotlib.pyplot as plt

# 膨胀代价计算
d = np.linspace(0, 1.5, 100)  # 距离0到1.5米
inscribed_r = 0.25  # 内接圆半径
weight = 5.0

cost = 253 * np.exp(-weight * (d - inscribed_r)) + 1
cost[d < inscribed_r] = 254  # 障碍物内部设为致命代价

plt.plot(d, cost)
plt.xlabel('Distance (m)')
plt.ylabel('Cost')
plt.title('Inflation Layer Cost Profile')
plt.show()

这段代码画出来的曲线是一个从254快速衰减到1的指数曲线。机器人规划路径时，会倾向于走代价值低的区域——也就是离障碍物足够远的地方。

三、滚动代价地图：大环境小窗口

实际场景中地图可能很大（几百平方米），但机器人一次只需要关心周围一小块区域。如果每次都更新整张大地图，计算量太大了。

Nav2用的是滚动窗口（rolling window）机制：代价地图只维护以机器人为中心的一块矩形区域，机器人移动时窗口跟着移动，新进入窗口的区域更新数据，离开窗口的区域丢弃。

global_costmap:
  ros__parameters:
    use_max_speed_override: false
    plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
    width: 5
    height: 5
    resolution: 0.05
    origin_x: -2.5
    origin_y: -2.5
    rolling_window: true
    track_unknown_space: true

global_costmap通常设成滚动窗口，跟着机器人走。local_costmap也一定是滚动的。但如果机器人要在全局层面做路径规划，static_map模式下global_costmap可以覆盖整张地图，不滚动。

分辨率（resolution）是个关键参数。0.05表示每个格子代表5cm。分辨率越高越精细，但计算量也越大。一般室内导航0.05就够了，室外大场景可以用0.1。

四、voxel_layer：从2D到3D的升级

标准的obstacle_layer只处理2D数据——激光雷达的扫描线或者点云投影到地面上的2D占据栅格。但实际场景中，障碍物不一定在地面上。

比如一张悬空的桌子，激光雷达扫到桌腿，但桌面在桌腿上方。2D的obstacle_layer只标记桌腿的位置，桌面那块区域在代价地图上是"空的"。机器人从桌面下方穿过去，如果它有一定高度，就会撞上桌面。

voxel_layer解决了这个问题。它在2D栅格的基础上增加了高度维度，维护一个3D的体素栅格。传感器数据按高度分层标记，最终投影到2D代价地图上。

local_costmap:
  ros__parameters:
    plugins: ["voxel_layer", "inflation_layer"]
    voxel_layer:
      plugin: "nav2_costmap_2d::VoxelLayer"
      observation_sources: pointcloud
      pointcloud:
        topic: /camera/depth/points
        data_type: PointCloud2
        max_obstacle_height: 2.0
        min_obstacle_height: 0.1

voxel_layer的代价是计算量更大，内存占用也更多。所以一般只在局部代价地图里用voxel_layer，全局代价地图还是用2D的obstacle_layer。

五、代价地图的坐标变换

代价地图不是悬浮在空中的，它需要锚定在一个坐标系上。

Nav2的代价地图通过TF（坐标变换）来确定自己的位置。你需要指定global_frame（全局坐标系，通常是map）和robot_base_frame（机器人本体坐标系，通常是base_link）。

每次更新代价地图时，系统会查询TF得到机器人在全局坐标系中的位姿，然后以这个位姿为中心更新滚动窗口。传感器数据也要通过TF变换到代价地图的坐标系下才能正确叠加。

这里有个常见的坑：如果TF树有问题（比如某个坐标变换发布延迟或者丢失），代价地图就会"飘"——障碍物标记在错误的位置，规划出来的路径自然也不对。

六、面试高频追问

Q：代价地图的致命代价（lethal cost）是多少？ A：255。值为255的格子表示绝对不可通行。膨胀层的最大值通常设为254，表示"非常接近障碍物"。

Q：inflation_radius和inscribed_radius有什么区别？ A：inscribed_radius是机器人内接圆半径，代表机器人最"瘦"的方向。inflation_radius是膨胀的最大距离，通常设为inscribed_radius的2-3倍。两者之间的区域代价值从254衰减到0。

Q：为什么局部代价地图和全局代价地图要分开？ A：全局代价地图负责大范围的静态障碍和长距离路径规划，通常覆盖整张地图。局部代价地图负责实时避障，只关心机器人周围几米的范围，更新频率更高。分开后各自的分辨率和更新策略可以独立优化。

Q：传感器数据怎么融合到代价地图里？ A：通过obstacle_layer的observation_sources参数配置。每个传感器源指定topic、数据格式（PointCloud2/LaserScan）、最大最小距离、标记和清除的射线范围。多个传感器源的数据会叠加到同一个obstacle_layer。

Q：代价地图上的"鬼影"是什么？ A：动态障碍物移走后，代价地图上可能还残留着障碍物标记。原因是传感器的"清除射线"没有完全覆盖到那个位置。解决办法是调整clearing参数，或者用ClearCostmap恢复行为强制清除。

代价地图看似简单，但参数调优是个体力活。膨胀权重、分辨率、传感器参数、滚动窗口大小，每个参数都会影响导航效果。

上一篇：第266篇 ROS2 Navigation2框架概览

下一篇我们专门来聊这些参数的调优方法。

---

导航系列第二篇，把代价地图的底层原理讲清楚了。分层结构、膨胀机制、滚动窗口、坐标变换，这四个概念是理解代价地图的核心。下一篇聊代价地图的参数配置和调优实战。

如果这篇文章对你有帮助，欢迎点赞支持一下，你的鼓励是我持续更新的动力！