One-Step Model Predictive Path Integral for Manipulator Motion Planning Using Configuration Space Distance Fields

技術要約：構成空間距離場を用いたマニピュレータ運動計画のためのワンステップ・モデル予測パスインテグラル

問題提起
ロボットマニピュレータの運動計画は、最適化ベースの手法とサンプリングベースの手法の二項対立に直面している。古典的な最適化手法（CHOMPやTrajOptなど）は、衝突回避制約を課すために作業空間における符号付き距離関数（SDF）に依存している。しかし、これらの手法は局所解に陥りやすく、またSDFの勾配が障害物の近くで消失する場合がある。さらに、関節空間におけるSDF値の分布は非一様であることが多い。対照的に、Model Predictive Path Integral (MPPI) コントロールのような勾配を用いない手法は、長いホライゾンの軌跡ロールアウトを用いることで、暗黙的に障害物を探索し、消失勾配の問題を回避する。効果的ではあるものの、標準的なMPPIは、数千回の軌跡サンプル、繰り返される衝突判定、および異種物理単位（距離、速度、トルクの混合など）を持つコスト関数の設計の難しさにより、計算コストが高い。

手法
著者らは、構成空間での直接的なナビゲーションを可能にするため、構成空間距離場（CDF）とModel Predictive Path Integral (MPPI) コントロールを統合したフレームワークを提案している。

1. CDF表現: 作業空間のSDFとは異なり、CDF ($f_c$) は構成空間で直接距離をモデル化する。これらは、ほとんど至る所でエイクネル方程式 ($\|\nabla_q f_c\| = 1$) を満たし、障害物から脱出するための最適な方向を指し示す、信頼性の高い非消失な勾配情報を提供する。
2. 角度ベースのコスト関数: 単位を統一し、異種単位の混在を排除するために、著者らは角度に基づくコスト関数を導入している。コスト $c(\theta_1, \theta_2)$ は以下のように定義される：
   • $\theta_1$: 運動ベクトルとCDF勾配（障害物の法線）との間の角度。これは障害物に向かう動きにペナルティを課す。
   • $\theta_2$: 運動ベクトルと目標ベクトルとの間の角度。これは目標から遠ざかる動きにペナルティを課す。
   • 条件付きロジック: 運動が障害物から遠ざかっている場合、または距離が閾値 ($d_{act}$) を超えている場合、あるいは距離が目標への距離よりも大きい場合、障害物コスト ($\theta_1$) はゼロに設定される。これにより、ロボットが不必要に障害物の周囲を旋回することを防ぐ。
3. ワンステップ・ホライゾンMPPI: CDF勾配が提供する局所的な幾何学的安全性キューを活用することで、本手法はMPPIの予測ホライゾンを単一ステップ ($H=1$) に削減できる。将来のコストを蓄積するために長いホライゾンの軌跡をシミュレーションする代わりに、このアルゴリズムは単一ステップの制御入力をサンプリングし、即時の角度ベースのコストを評価し、サンプリング分布ポリシーを更新する。
4. 関節リミットの処理: 実行前に、要素ごとのクリッピング戦略を用いて、制御入力を関節リミットによって定義される実行可能集合に投影する。

主な貢献

• CDFとMPPIの統合: 本論文は、CDFの勾配の信頼性とMPPIのサンプリングの堅牢性を組み合わせ、長いホライゾンのロールアウトを不要にした。
• 統一されたコスト関数: すべてのコストを角度として表現することで、重みのチューニングを簡素化し、異種物理単位を比較する問題を解消した。
• ワンステップ・ホライゾン: CDF勾配の使用により、予測ホライゾンを $H \gg 1$ から $H=1$ へと劇的に削減でき、衝突回避を維持しながら計算負荷を大幅に軽減した。
• 局所解の回避: 反復的な最適化ソルバー（高次元空間で局所解に陥りやすい）に頼らず、MPPIの分布更新に依存することで、混雑した環境における堅牢性を向上させた。

実験結果
本手法は、シミュレーション上で2自由度の2リンクロボットおよび7自由度のFranka Emikaマニピュレータに対して評価された。

• 2自由度ロボット: 提案手法は99.6%の成功率を達成し、元のMPPI（71.2%）およびQPベースの最適化手法（64.1%）を大幅に上回った。経路長はベースラインよりわずかに長くなったが、本手法はQPソルバーが捕らえられた局所最適を回避することに成功した。
• 7自由度Frankaロボット:
  • シナリオ1 (60個の障害物): 776 Hzの平均制御周波数とともに、100%の成功率を達成。
  • シナリオ2 (123個の障害物): 718 Hzの平均制御周波数とともに、86%の成功率を達成。
  • 比較: QPベースラインは、約13〜14%の成功率しか得られず、周波数は約220 Hzであった。標準的なMPPI実装（cuRobo）との比較では、提案手法が約700 Hz以上で動作したのに対し、標準MPPIは約28〜61 Hzであった。これは、簡略化されたワンステップ・ホライゾンと効率的なCDFの使用によるものである。

意義と主張
本論文は、提案されたCDF-MPPIフレームワークが、混雑した環境における高次元運動計画のための実用的なソリューションを提供すると主張している。CDF勾配を利用して直接的な局所安全性キューを提供することで、本手法は以下を実現している：

1. 高い効率性: 750 Hzを超える制御周波数を実現し、最適化ベースのソルバーや標準的なMPPIを大幅に上回る。
2. 高い信頼性: 2Dにおいてほぼ完璧な成功率を、複雑な7自由度のシナリオにおいて一貫して高い成功率を達成する。
3. 簡潔性: チューニングを簡素化し、長期間のシミュレーションに伴う計算負担を取り除く統一されたコスト関数。

著者らは、失敗の主な原因は、ロボットが障害物に極めて近い状態で開始した場合（分布の適応に十分な時間が残されていない場合）、または障害物付近でのCDF推定誤差によるものであると述べている。今後の課題として、障害物付近でのCDFの忠実度の向上、および球状領域への表現の拡張を挙げている。