Nav2 代价地图深挖：膨胀、清除与可通行宽度的定量关系

1. 「规划器不行」往往是膨胀算错了

1.1 m 宽的门口，DWB 参数拧了两周仍过不去——最后发现 inflation_radius=0.8 把有效通道砍到 0.9 m，而 footprint 外接宽度 0.55 m。膨胀不是「滑两个条」，是距离变换 + 指数衰减把几何安全距离编码进代价值场。控制器只读 [0, 254] 的 cost，不看见原始激光——调 MPPI/DWB 增益前，先确认代价场是否忠实、及时、与车体几何一致。

2. 代价地图在闭环里的位置

text

传感器 → 障碍层/体素层 → 膨胀层 → 规划/控制读取代价
         ↑                    ↑
    voxel_layer          inflation_layer
    (3D→2D)              (distance transform)

占据栅格经 distance transform 得到每个 cell 到最近障碍的距离 $d$ （米）。膨胀层映射：

$d < r_{inscribed}$ → LETHAL (254)
$r_{inscribed} \leq d < r_{inflation}$ → 指数衰减代价
$d \geq r_{inflation}$ → FREE (0)

Nav2 典型公式：

cost (d) = (INSCRIBED - 1) \cdot exp (- λ \cdot (d - r_{inscribed}))

$λ$ 即 cost_scaling_factor：越大，靠近障碍时代价上升越陡，规划器越早绕开。inscribed_radius 应等于 footprint 内接圆半径（最大可内切圆），不是外接圆——设错会让 LETHAL 区与真实碰撞轮廓脱节。

3. 窄门通过的不等式

设物理门宽 $W$ 、footprint 外接宽度 $w$ 、规划器拒绝代价 $> C_{thresh}$ 的 cell。有效离障距离 $d_{clear} (C_{thresh})$ 由 $r_{inflation}$ 与 $λ$ 决定。通过条件：

W_{eff} = W - 2 \cdot d_{clear} (C_{thresh}) \geq w + margin

实验方法：固定门宽与 footprint，网格搜索 $(r_{inflation}, λ)$ ，统计通过率——比玄学调参 honest。我遇到过：cost_scaling=10 时 $d_{clear} (128) \approx 0.35$ m；换成 3 同一门刚好能过。圆形 footprint 实现简单但转角保守；多边形更贴 CAD 轮廓，对角点敏感——用 CAD + 2–5 cm 安全 margin，不要目测。

4. 局部窗口与刹车距离

局部 costmap 边长 $L$ ，最大速度 $v$ ，可用减速度 $a$ ，传感延迟 + 地图更新 + 控制周期 + 执行器响应合计 $t_{delay}$ ：

d_{stop} = \frac{v ^{2}}{2 a} + v \cdot t_{delay}

要求 $L/2 > d_{\text{stop}} + \text{safety_margin}$ 。例： $v = 1.0$ m/s， $a = 0.5$ m/s²， $t_{delay} = 0.3$ s → $d_{stop} = 1.3$ m，局部 map 至少 3 m × 3 m。否则高速下障碍刚进入窗口就来不及刹停——表现像「控制器抖」，其实是 map 来不及。

5. 清除层与动态障碍

障碍层写入 occupied，清除层用当前 scan 射线 trace free space。幽灵障碍：人走过位置长期 occupied → raytrace_range 不够或 observation_persistence 太长。闪烁：噪声点让代价场抖动 → 路径抖。诊断：录 bag 看 /local_costmap/costmap 时间序列，不是只看最终路径。obstacle_layer 的 max_obstacle_height 与 voxel_layer 配合过滤地面/天花板反射。

全局与局部不必参数一致：全局稍保守保证长距离通道；局部跟传感器负责实时避障。但两者对「门是否存在」的判定不应矛盾——否则全局规划穿门、局部永远卡死。

6. 与 SLAM 地图的关系

SLAM 输出的 static map 分辨率影响 obstacle layer 精度。0.2 m 分辨率的墙在 costmap 上阶梯化，膨胀后窄门通过率不可预测。SLAM 地图质量是 costmap 的上游——定位跳变还会让障碍层与 static layer 错位，表现像「膨胀突然变大」。

7. 调参实验（半天可做完）

选 3 个门宽：勉强 / 舒适 / 宽裕
固定最大速度档位
网格搜索：inflation_radius × cost_scaling_factor
指标：通过率、最小离墙距离、人工干预次数、完成时间
固化「慢速巡检」和「快速转运」两套配置

8. 参数声明与 yaml 漂移

膨胀「改了没效果」有时是 yaml 键名与节点声明不一致：inflation_radius 写在根层，节点读 costmap.inflation_radius，未声明访问静默落默认。启动后用 ros2 param get 核对有效值，比盯 RViz 诚实。Composable 容器里 overlay 顺序更绕——以加载后的 param dump 为准。

lethal_cost_threshold 与 inscribed_cost 设错，会让 $d_{clear}$ 计算与 RViz 颜色不一致：规划器以为能过，RViz 看起来是红的。Half-day 调参实验应固化 yaml hash 与 footprint 版本，换场地只改门宽实测值，不重搜全网格。

9. 验收

窄门通过率 vs 膨胀参数（曲线，不是单点）
最小离障距离分布
清除延迟：动态障碍离开后多久变 free
局部 map 更新 latency
$d_{stop}$ vs $L /2$ 比值（安全裕度）
全局/局部对同一窄门判定一致
启动 param dump 与 yaml 声明键一致

膨胀层把「离障碍多远才安全」变成可计算的代价场。算错不等式，再精致的 DWB 也只是在堵死的通道里挣扎。