Navigation2中MPPI控制器的碰撞检测与距离计算优化

概述

在机器人导航系统中，精确的碰撞检测和障碍物距离计算对于安全导航至关重要。本文深入探讨了Navigation2项目中MPPI（Model Predictive Path Integral）控制器的碰撞检测机制，特别是关于机器人足迹（footprint）与障碍物之间距离计算的优化问题。

碰撞检测机制分析

Navigation2的MPPI控制器主要通过两个评价器（critic）来处理障碍物避障：

1. ObstacleCritic：负责碰撞检测和行为引导

   • 严格碰撞检测：拒绝与障碍物碰撞的轨迹样本
   • 行为引导：对靠近障碍物的有效轨迹给予更高成本

2. CostCritic：完全基于成本地图的评估

   • 不直接使用距离信息
   • 利用成本地图的指数衰减特性调整障碍物行为

现有问题

当前实现中存在几个关键问题：

1. 距离计算限制：当使用完整足迹检查时，到障碍物的距离计算下限为内切半径（inscribed radius）。这意味着在某些情况下（如沿墙行驶），无法获得精确的实际距离。

2. 成本评估逻辑：CostCritic中存在看似冗余的代码段，检查中心点成本是否≥253（内切膨胀障碍物成本）。实际上，这个条件永远不会触发，因为：

   • 如果中心点成本≥253，足迹检查将返回碰撞
   • 碰撞情况下会提前终止循环，不会执行后续条件判断

技术深入

足迹检查的局限性

当前实现中，当机器人接近障碍物时，距离计算会被限制在内切半径范围内。例如，即使机器人足迹几乎接触到墙壁，计算得到的距离仍显示为内切半径（如0.6米）。这导致：

• 无法精确反映实际距离
• 碰撞边缘距离（collision_margin_distance）可能永远不会被触发
• 长条形机器人（如20米长、3米宽的AGV）导航时问题尤为明显

成本评估优化

分析CostCritic的实现发现：

1. 中心点成本检查（pose_cost ≥ 253）实际上是冗余代码，可以安全移除
2. 更合理的做法是：
   • 使用circumscribed_cost_作为临界值
   • 或者直接利用footprintCostAtPose的返回值进行精确评估

解决方案与改进方向

1. 精确距离计算：

   • 考虑实现TSDF（截断符号距离函数）等精确距离计算方法
   • 虽然会增加计算负担，但对于局部代价地图（尺寸较小）是可接受的

2. 代码优化：

   • 移除CostCritic中的冗余条件判断
   • 优化碰撞检测逻辑，直接使用足迹检查结果

3. 全局一致性：

   • 确保控制器行为与全局规划器一致
   • 避免控制器认为可行的路径被全局规划器标记为致命区域

实际影响

这些改进将显著提升：

• 长条形机器人的导航性能
• 沿墙行驶等场景的精确控制
• 系统对狭窄通道的处理能力

结论

Navigation2的碰撞检测机制在大多数情况下表现良好，但对于特殊形状的机器人和特定场景仍有优化空间。通过实现更精确的距离计算方法和优化现有代码逻辑，可以显著提升系统的导航性能和可靠性。这些改进不仅限于MPPI控制器，也可能为整个Navigation2系统的其他组件带来益处。