Nav2 Inflation Layer 膨胀层公式、内切半径与 RViz 颜色理解

适合场景ROS2 Nav2 局部规划 / DWB / MPPI 调参尤其是想搞清楚inflation_radius和cost_scaling_factor到底怎么影响避障行为。1. Costmap 里的 254、253、0 分别是什么Nav2 costmap 内部使用0 ~ 255的 unsigned char 数值表示栅格代价内部 cost 值名称含义0FREE_SPACE完全自由空间1 ~ 252inflated cost膨胀层产生的渐变代价253INSCRIBED_INFLATED_OBSTACLE距离障碍物小于等于机器人内切半径的区域极危险254LETHAL_OBSTACLE致命障碍物也就是障碍物所在格子255NO_INFORMATION未知区域可以简单理解为254障碍物本体所在格子不能碰 253离障碍物非常近机器人中心如果进入这个区域footprint 很可能已经撞到或擦到障碍物 1~252膨胀代价区越靠近障碍物 cost 越高 0自由空间注意253不是一个独立插件也不是传感器直接打出来的障碍物而是 Inflation Layer 根据机器人 footprint / robot_radius 算出来的高危险区。2. Inflation Layer 的核心公式Nav2 Inflation Layer 的核心计算逻辑可以简化为if(distance0){costLETHAL_OBSTACLE;// 254}elseif(distance*resolutioninscribed_radius){costINSCRIBED_INFLATED_OBSTACLE;// 253}else{cost(INSCRIBED_INFLATED_OBSTACLE-1)*exp(-cost_scaling_factor*(distance*resolution-inscribed_radius));}换成数学公式就是如果 d 0 cost 254 如果 0 d r_inscribed cost 253 如果 r_inscribed d inflation_radius cost 252 × exp(-k × (d - r_inscribed)) 如果 d inflation_radius cost 0其中符号含义d当前栅格到最近 lethal obstacle 栅格的距离单位是米distance源码里的距离单位是 costmap cell不是米resolutioncostmap 分辨率单位是 m/cell例如0.05表示每格 5cmdistance * resolution把 cell 距离转换成米r_inscribed机器人内切半径Nav2 根据footprint或robot_radius自动计算kcost_scaling_factor控制代价衰减速度inflation_radius膨胀影响的最大半径3. distance 到底是谁到谁的距离这是最容易误解的地方。源码里的distance不是障碍物到 base_footprint 的距离 障碍物到机器人 footprint 边框的距离 雷达到障碍物的距离它实际是当前 costmap 栅格中心 到 最近 lethal obstacle 栅格中心 之间的欧氏距离并且源码里这个distance一开始是以 cell 为单位的。例如 costmap 分辨率是resolution:0.05# 5cm/cell如果某个栅格离障碍物格子 10 个 celldistance 10 resolution 0.05 m/cell 实际距离 d distance × resolution 10 × 0.05 0.5m所以源码里才会写distance*resolutioninscribed_radius意思是当前栅格到障碍物格子的实际距离 机器人内切半径如果成立就把这个格子标成253。4. inscribed_radius 内切半径从哪里来inscribed_radius不是你直接在 Inflation Layer 里手动设置的。Nav2 会根据 costmap 的机器人形状自动算footprint:[[0.45, 0.35], [0.45, -0.35], [-0.45, -0.35], [-0.45, 0.35]]或者robot_radius:0.35如果你配置的是footprintNav2 会计算inscribed_radius base_footprint 原点到 footprint 边或顶点的最小距离 circumscribed_radius base_footprint 原点到 footprint 边或顶点的最大距离对一个中心在base_footprint的矩形车体footprint:[[L/2, W/2], [L/2, -W/2], [-L/2, -W/2], [-L/2, W/2]]那么inscribed_radius min(L/2, W/2) circumscribed_radius sqrt((L/2)^2 (W/2)^2)举例车长 L 0.90m 车宽 W 0.70m inscribed_radius min(0.45, 0.35) 0.35m circumscribed_radius sqrt(0.45² 0.35²) ≈ 0.57m工程含义inscribed_radius 内的区域是非常危险区域 如果机器人中心进入距离障碍物小于内切半径的位置机器人 footprint 很可能已经碰到障碍物。注意如果你的base_footprint原点不在车体几何中心或者 footprint 点写错Nav2 算出来的内切/外接半径也会跟着错。5. inflation_radius 和 cost_scaling_factor 分别控制什么这两个参数作用不同参数控制什么直观理解inflation_radius膨胀影响范围最大到多远影响范围cost_scaling_factor这个范围内 cost 衰减多快衰减速度不要误解成inflation_radius 1.0m所以 1m 内都是高代价正确理解是1m 内会产生指数衰减代价 越靠近障碍物 cost 越高越远 cost 越低 cost_scaling_factor 越大下降越快。6. 例子inflation_radius 1.0minscribed_radius 0.30m假设inflation_radius:1.0inscribed_radius:0.30不同cost_scaling_factor下距离障碍物不同位置的内部 cost 大概如下距离障碍物k1k2k3k5k100.0m2542542542542540.1m2532532532532530.3m2532532532532530.4m228206186152920.5m20616813892340.6m18613810256120.8m15292562011.0m1256230701.2m00000可以看到k 越小代价下降越慢远处也还有明显 cost车更不容易贴障碍 k 越大代价下降越快只有障碍旁边很小一圈有明显 cost。所以如果你想让车更倾向走通道中间一般不要把cost_scaling_factor设得太大。7. 如何根据目标 cost 反推 cost_scaling_factor公式cost 252 × exp(-k × (d - r_inscribed))反推k -ln(cost / 252) / (d - r_inscribed)例如inscribed_radius 0.30m 希望 d 1.00m 时 cost 约为 80那么k -ln(80 / 252) / (1.00 - 0.30) ≈ 1.64参考表希望 1.0m 处 cost 约为对应 cost_scaling_factor假设 inscribed_radius0.30m1500.741001.32801.64502.31203.628. RViz 里的颜色代表什么RViz 颜色显示的是 costmap 的可视化结果不同显示模式/主题可能略有差异但一般可以这样理解RViz 颜色大概含义内部 cost黑色 / 透明free space0蓝色低代价膨胀区较低的 1~252红色 / 橙色高代价膨胀区较高的 1~252青色inscribed inflated obstacle内切半径危险区253紫色 / 粉紫色lethal obstacle障碍物本体254灰蓝 / 灰绿unknown255调参时不要只看颜色“好不好看”重点看障碍物周围是不是有连续渐变 通道中间的 cost 是否明显低于贴边位置 膨胀区有没有把窄通道完全堵死 253/254 区域是否只在真正危险区域附近如果想看原始代价值可以看 raw costmapros2 topicecho/local_costmap/costmap_raw或者写一个小脚本订阅/local_costmap/costmap_raw取某个格子的 cost 值。9. 针对园区巡检车的调参思路如果你的车比较大且目标是安全优先可以先从下面的方向试local_costmap:local_costmap:ros__parameters:footprint:[[0.45, 0.35], [0.45, -0.35], [-0.45, -0.35], [-0.45, 0.35]]footprint_padding:0.05inflation_layer:plugin:nav2_costmap_2d::InflationLayerinflation_radius:1.0cost_scaling_factor:2.0调参顺序建议先写准footprint。确认 RViz 里 footprint 和真实车宽一致。设置一个偏保守的inflation_radius比如0.8 ~ 1.2m。把cost_scaling_factor先设在1.5 ~ 3.0。看车是否更愿意远离障碍物。如果窄路过不去减小inflation_radius或增大cost_scaling_factor。如果还贴障碍增大inflation_radius或减小cost_scaling_factor。经验理解太贴障碍inflation_radius 增大或 cost_scaling_factor 减小 太保守、窄路过不去inflation_radius 减小或 cost_scaling_factor 增大10. 一句话总结inflation_radius 决定“影响多远” cost_scaling_factor 决定“掉得多快” inscribed_radius 由 footprint / robot_radius 自动计算 distance 是 costmap 栅格到最近障碍物栅格的距离不是机器人到障碍物的实时距离 253 是内切半径危险区254 是障碍物本体。参考资料Nav2 Inflation Layer Parameters: https://docs.nav2.org/configuration/packages/costmap-plugins/inflation.htmlNav2 Inflation Layer 源码computeCost()https://api.nav2.org/nav2-rolling/html/inflation__layer_8hpp_source.htmlNav2 Footprint 源码calculateMinAndMaxDistances()https://api.nav2.org/nav2-humble/html/footprint_8cpp_source.htmlNav2 Costmap 2D 配置文档https://docs.nav2.org/configuration/packages/configuring-costmaps.html