A Terrain-Adaptive epsilon-Constraint MPC for Uneven Terrain Kinodynamic Planning

技术摘要：面向非结构化地形的地形自适应 $\epsilon$-约束模型预测控制（MPC）用于运动动力学规划

问题陈述
在非结构化、崎岖环境（如建筑工地、灾害区域、行星探测）中，类车机器人的自主导航面临双重挑战：既要生成无碰撞路径，又要满足复杂的运动动力学约束。传统的基于网格的规划器（如利用快速行进法 FMM 的规划器）擅长生成感知地形的几何路径，但往往无法纳入动态可行性（如速度限制和车辆稳定性）。相反，运动动力学规划器在平衡相互竞争的目标时经常遇到困难——具体而言是路径效率与姿态稳定性（防止翻车）之间的权衡——且容易陷入局部最优或依赖过度简化的二维假设。现有的数据驱动方法（如深度强化学习）往往缺乏在不同起止配置下的泛化能力，且未能整合连续的全局引导结构。此外，标准的多目标优化方法通常依赖加权标量化，这可能导致路径长度主导解的选择，从而忽视崎岖地形上关键的稳定性约束。

方法论
作者提出了一种滚动时域控制框架，该框架将半参数化车辆 - 地形模型与地形自适应 $\epsilon$-约束模型预测控制（MPC）相结合。

1. 车辆 - 地形建模：

   • 半参数化方法： 系统将类车车辆的解析运动学模型与数据驱动的残差校正相结合。标称模型基于线速度和角速度预测状态演化。
   • 稀疏高斯过程（SGP）： 为了考虑未建模的地形效应（如打滑、复杂的接触动力学），利用 SGP 基于局部地形描述符进行训练，以预测标称模型与实际系统行为之间的残差误差。
   • 地形描述符： 模型利用丰富的描述符集合，包括面积加权法向量、表面粗糙度（$\sigma_z$）、高斯曲率和平均曲率，以及总邻域面积。
   • 高度函数： 一种基于投影的方法通过将查询点映射到三角网格上的最近面来确定地形高度 $h(x,y)$，从而确保精确的朝向计算。

2. 优化框架：

   • 多目标公式化： 规划器优化两个相互竞争的目标：
     • 路径效率（$f_1$）： 最小化与全局 FMM 向量场的方向偏差，并由目标接近度缩放因子加权。
     • 姿态稳定性（$f_2$）： 最小化相对于局部地形法向的累积朝向偏差（横滚和俯仰），对超过安全阈值（$\alpha_{safe}$）的偏差进行惩罚，并强制执行硬限制（$\alpha_{max}$）以防止翻车。
   • 地形自适应 $\epsilon$-约束： 该方法不采用加权标量化，而是将问题转化为约束单目标优化。稳定性约束（$f_2 \leq \epsilon_{stab}$）根据局部地形粗糙度和坡度动态调整。在具有挑战性的地形中，稳定性预算收紧；在良性地形中，预算放宽，从而允许更激进的探索。
   • 采样策略： 候选控制序列通过混合方法生成：
     • 热启动： 对先前成功的控制序列进行移位并添加噪声，以确保时间连续性。
     • FMM 偏置采样： 将全局 FMM 向量场建议的控制与随机探索相结合，以跳出局部极小值。

主要贡献
本文声称有四项主要贡献：

1. 一种用于类车车辆在非结构化三角网格上运动的半参数化模型，结合了解析动力学与基于 SGP 的残差学习。
2. 一种源自连续 FMM 向量场的、用于网格导航的代价函数。
3. 一种在滚动时域框架内利用地形自适应 $\epsilon$-约束优化的求解方法，用于生成平衡效率与稳定性的非支配轨迹。
4. 一种专为实时部署设计的 ROS 兼容规划器（Move Base Flex）。

实验结果
所提出的方法（$\epsilon$-MPC）在仿真（Gazebo）和真实世界实验中进行了评估，使用了 AgileX Scout Mini 机器人在两个不同的 3D 网格地形上运行。该方法与模型预测路径积分（MPPI）和遗传算法运动动力学（GAKD）基线进行了比较。

• 性能指标： 所提出的方法在仿真中实现了 94% 的导航成功率，优于 MPPI（87%）和 GAKD（89%）。
• 稳定性： 与 MPPI 相比，它将最大朝向偏差降低了 24%，并将多目标权衡指标（$T$）提高了 23%。
• 消融研究：
  • FMM 偏置： $\alpha_{FMM}=0.7$ 的 FMM 偏置强度产生了最佳结果，与纯随机采样相比，成功率提高了 11–27%。
  • 自适应性： 地形自适应约束方法消除了固定 $\epsilon$ 约束下观察到的所有翻车事故，并将最大朝向偏差降低了 32%。
  • 真实世界部署： 在物理实验中，该方法保持了 80% 的成功率（MPPI 为 67%），成功穿越了因横滚角过大而导致基线方法失败的陡峭路段。规划时间保持在实时约束范围内（每次迭代约 35–48 毫秒）。

意义与主张
本文断言，其主要意义在于提供了一个原则性框架，明确解决了非结构化地形上运动动力学规划的多目标性质。通过根据局部地形描述符动态调整稳定性约束，该方法在不依赖复杂且计算昂贵的进化搜索（如 NSGA-II，研究发现其对于实时应用而言过慢）的情况下，实现了路径最优性与物理安全性之间的卓越平衡。作者得出结论，虽然加权标量化方法可以优化单个目标，但地形自适应 $\epsilon$-约束方法在确保非结构化户外环境中可行、安全的导航方面更为有效，提供了一种从仿真扩展到真实世界部署的实用解决方案。