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

技術的概要：未整地環境における運動力学的計画のための地形適応型 $\epsilon$ 制約 MPC

問題定義
建設現場、災害地域、惑星探査などの構造化されていない不均一な環境における車輪型自律移動体の自律航行は、複雑な運動力学的制約を遵守しつつ衝突を回避する経路を生成するという二重の課題に直面している。高速進行法（FMM）を利用する従来のメッシュベースの計画手法は、地形を考慮した幾何学的経路の生成においては優れているが、速度制限や車両の安定性といった動的実現可能性の考慮においてはしばしば失敗する。逆に、運動力学的計画手法は、経路効率と姿勢安定性（転倒防止）という競合する目的のバランスを取る際に、局所最適解に陥ったり、過度に単純化された 2 次元仮定に依存したりする傾向がある。既存のデータ駆動型アプローチ（深層強化学習など）は、多様な開始点・目標点構成における汎化能力が不足しており、連続的なグローバルガイダンス構造の統合に失敗することが多い。さらに、標準的な多目的最適化手法は重み付きスカラー化に依存することが多く、これにより経路長が解を支配し、粗い地形における重要な安定性制約が軽視される結果となっている。

手法
著者は、半パラメトリックな車両 - 地形モデルと地形適応型 $\epsilon$ 制約モデル予測制御（MPC）を統合した、再帰的ホライズン制御フレームワークを提案する。

1. 車両 - 地形モデル化:

   • 半パラメトリック手法: システムは、車輪型車両の解析的運動学モデルと、データ駆動型の残差補正を組み合わせる。名义モデルは、線形速度と角速度に基づいて状態の進化を予測する。
   • 疎ガウス過程（SGP）: 未モデル化の地形効果（すべり、複雑な接触力学など）を考慮するため、SGP は局所地形記述子に基づいて訓練され、名义モデルと実際のシステム動作との間の残差誤差を予測する。
   • 地形記述子: モデルは、面積重み付き法線ベクトル、表面粗さ（$\sigma_z$）、ガウス曲率と平均曲率、および全近傍面積を含む豊富な記述子セットを利用する。
   • 高さ関数: 投影ベースの手法により、三角形メッシュ上の最も近い面へクエリ点をマッピングすることで地形高さ $h(x,y)$ を決定し、正確な姿勢計算を確保する。

2. 最適化フレームワーク:

   • 多目的定式化: 計画手法は、2 つの競合する目的を最適化する。
     • 経路効率（$f_1$）: 目標近接スケーリング係数で重み付けされた、グローバル FMM ベクトル場との方向誤差を最小化する。
     • 姿勢安定性（$f_2$）: 局所地形法線に対する累積的な姿勢偏差（ロールとピッチ）を最小化し、安全閾値（$\alpha_{safe}$）を超えた偏差に対してペナルティを課し、転倒を防ぐために厳格な上限（$\alpha_{max}$）を強制する。
   • 地形適応型 $\epsilon$ 制約: 重み付きスカラー化の代わりに、本手法は問題を制約付き単一目的最適化に変換する。安定性制約（$f_2 \leq \epsilon_{stab}$）は、局所地形の粗さや傾斜に基づいて動的に調整される。過酷な地形では安定性予算が厳格化され、穏やかな地形では緩和され、より積極的な探索を可能にする。
   • サンプリング戦略: 候補制御系列は、ハイブリッドアプローチにより生成される。
     • ウォームスタート: 時間的連続性を確保するため、前の成功した制御系列をシフトさせ、ノイズを加える。
     • FMM 偏重サンプリング: 局所最小値からの脱出を図るため、グローバル FMM ベクトル場が示唆する制御とランダム探索を混合する。

主要な貢献
本論文は、以下の 4 つの主要な貢献を主張する。

1. 解析的ダイナミクスと SGP ベースの残差学習を組み合わせる、不均一な三角形メッシュ上での車輪型車両運動のための半パラメトリックモデル。
2. 連続的な FMM ベクトル場から導出された、メッシュ航行のためのコスト関数。
3. 効率と安定性のバランスを取る非支配軌道を生成するための、再帰的ホライズンフレームワーク内での地形適応型 $\epsilon$ 制約最適化を利用した解決アプローチ。
4. 実時間展開用に設計されたROS 互換プランナー（Move Base Flex）。

実験結果
提案手法（$\epsilon$-MPC）は、Gazebo によるシミュレーションおよび AgileX Scout Mini ロボットを用いた 2 つの異なる 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 ms）に留まった。

意義と主張
本論文は、その主な意義が、不均一な地形における運動力学的計画の多目的性を明示的に扱う原理的なフレームワークの提供にあると主張する。局所地形記述子に基づいて安定性制約を動的に調整することで、このアプローチは、実時間用途には遅すぎる（NSGA-II など）複雑で計算コストの高い進化的探索に依存することなく、経路最適性と物理的安全性の間の優れたバランスを達成する。著者は、重み付きスカラー化手法は個々の目的を最適化し得るが、構造化されていない屋外環境における実現可能で安全な航行を確保するには、地形適応型 $\epsilon$ 制約手法の方が効果的であると結論づけている。これは、シミュレーションから実世界展開までスケーリング可能な実用的な解決策を提供するものである。