上篇聊了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)) 1d是当前格子到障碍物的距离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: trueglobal_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.1voxel_layer的代价是计算量更大内存占用也更多。所以一般只在局部代价地图里用voxel_layer全局代价地图还是用2D的obstacle_layer。五、代价地图的坐标变换代价地图不是悬浮在空中的它需要锚定在一个坐标系上。Nav2的代价地图通过TF坐标变换来确定自己的位置。你需要指定global_frame全局坐标系通常是map和robot_base_frame机器人本体坐标系通常是base_link。每次更新代价地图时系统会查询TF得到机器人在全局坐标系中的位姿然后以这个位姿为中心更新滚动窗口。传感器数据也要通过TF变换到代价地图的坐标系下才能正确叠加。这里有个常见的坑如果TF树有问题比如某个坐标变换发布延迟或者丢失代价地图就会飘——障碍物标记在错误的位置规划出来的路径自然也不对。六、面试高频追问Q代价地图的致命代价lethal cost是多少A255。值为255的格子表示绝对不可通行。膨胀层的最大值通常设为254表示非常接近障碍物。Qinflation_radius和inscribed_radius有什么区别Ainscribed_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框架概览下一篇我们专门来聊这些参数的调优方法。导航系列第二篇把代价地图的底层原理讲清楚了。分层结构、膨胀机制、滚动窗口、坐标变换这四个概念是理解代价地图的核心。下一篇聊代价地图的参数配置和调优实战。如果这篇文章对你有帮助欢迎点赞支持一下你的鼓励是我持续更新的动力