Safety-aware Model Predictive Path Integral Control with Signal Temporal Logic

技術要約：Signal Temporal Logicを用いた安全性認識型モデル予測パスインテグラル制御

問題提起
ロボティクスにおける安全性認識型の運動計画は、ミッションが時間的に極めて重要であり、かつ複雑でリアルタイムな時空間仕様に支配されている場合に、大きな課題に直面します。Signal Temporal Logic (STL) は、そのような要件（例：「最終的にターゲットに到達しつつ、常に障害物を回避する」）を表現するための厳密な形式言語を提供します。しかし、既存の計画手法は計算効率の面で課題があります。混合整数計画法（MIP）を用いる従来のアプローチは厳密な保証を提供しますが、オンライン実行にはコストがかかりすぎる場合が多いです。一方、Model Predictive Path Integral (MPPI) のようなサンプリングベースの手法は、計算効率が高く並列化が可能ですが、一般にSTL制約を満たすための形式的な保証に欠けており、多くの場合、安全性充足に直接マッピングされないコスト関数の近似に依存しています。

手法：safety-aware-stl-mppi
本論文では、Control Barrier Functions (CBF) を用いてSTL仕様をMPPIコントローラに直接統合する、リカレットホライゾン（後退ホライゾン）計画フレームワークである safety-aware-stl-mppi を提案しています。この手法は、以下の3つの段階で進行します。

1. STLから時変CBFへのエンコーディング:
   著者らは、先行研究[5]による連続時間系向けのSTLに対する連続時間CBFエンコーディングを、離散時間系へと適応させました。彼らは、条件 $h_\phi(x, t) \geq 0$ がSTL式 $\phi$ の充足を意味するような、時変関数 $h_\phi(x, t)$ を構築します。

   • 再帰的構成: エンコーディングは、STLの再帰的な意味論に従います。原子述語は滑らかな関数にマッピングされます。時間演算子（Globally $G$、Finally $F$、Until $U$）および論理結合は、候補となるCBFを滑らかな最小演算子の下近似（$\tilde{\min}$）を用いて組み合わせることで処理されます。
   • 削除メカニズム: 保守性を低減するために、本フレームワークは削除戦略を採用しています。これは、特定のタイムステップにおいて、すでに充足されたか、あるいは期限が切れた時間制約に対応するCBF項を、最適化の引数から取り除くものです。

2. 安全性拡張コストを用いたMPPI:
   コアとなるプランナーは、制御入力の摂動をガウス分布からサンプリングして、候補となる軌跡（ロールアウト）を生成するMPPIを利用します。

   • コスト関数: 標準的なMPPIのコストは、CBFから導出された安全性項によって拡張されます。ロールアウト $k$ に対するコストは、$J^{(t)}_{\phi, k} = J^{(t)}_k + \eta \sum C_{CBF}$ と定義され、$C_{CBF}$ はCBF条件の違反（すなわち、軌跡が安全集合の前方不変性を維持できない場合）に対してペナルティを課します。
   • 重み付け: サンプルは、それらの拡張されたコストに基づいて重み付けされ、名目上の重み付きロールアウト $v^+$ を計算します。

3. CBF投影（安全性フィルタ）:
   重み付きロールアウト $v^+$ は、必ずしもCBF制約を満たすとは限らないため、投影ステップが適用されます。アルゴリズムは、名目上の入力 $v^+_\tau$ に最も近く、かつ離散時間CBF条件を満たす制御入力 $v^*_\tau$ を見つけるための制約付き最適化問題を解きます。
   $$h_\phi(f(x_\tau, v^*_\tau), \tau+1) - h_\phi(x_\tau, \tau) \geq -\alpha h_\phi(x_\tau, \tau)$$

   • 近似: 離散時間においては制約が非凸であるため、著者らはシステムダイナミクスとCBF関数の1次テイラー展開を提案しています。これにより、制約をアフィン不等式へと変換し、投影を二次計画問題（QP）として効率的に解くことを可能にします。
   • 閉形式解: 制御入力が非有界（$U = \mathbb{R}^m$）の場合、このQPは閉形式の解を持ち、これにより計算効率がさらに向上します。

主な貢献

• フレームワークの統合: ユーザー定義のコスト関数を最小化しつつ、STL仕様をCBFに基づいた制約として組み込んだ、サンプリングベースのプランナーである safety-aware-stl-mppi の提案。
• 離散時間への適応: 連続時間文献における時変CBFエンコーディングを離散時間システムへと適応させ、これらの制約を強制するための軽量な投影メカニズムを組み合わせたこと。
• 実証的検証: 数値的なケーススタディおよびNVIDIA Isaac Labを用いたクアッドコプター・シミュレーションを通じた、本アルゴリズムの有効性の実証。

結果
著者らは、多様な環境とコスト設定を持つ4つの人工的なMars Rover（火星探査車）の計画ケーススタディを用いて、本手法をいくつかのMPPIベースラインと比較評価しました。

• 安全性と効率性: 提案手法は、一貫して高い安全性レベル（STL仕様の充足）を達成し、かつ計算効率を維持しました。
• ベースラインとの比較: STLの充足を明示的な制約投影なしにコスト関数の一部として扱う典型的な他のMPPIベースラインは、同等の安全性保証を達成できませんでした。
• Hardware-in-the-Loop: クアッドコプターの計画実験により、本アプローチの実用的な適用可能性が示されました。

意義と主張
本論文は、safety-aware-stl-mppi が、サンプリングベースのプランナーの計算効率と、形式手法による厳密な安全性保証の間の溝を埋めるものであると主張しています。このアプローチは、充足のための形式的な十分性を与える正確なSTL-CBF条件に基づいています。しかし、著者らは、リアルタイムの実装においてはこの条件を近似していることを明示的に述べています。したがって、本手法はSTL制約を満たすことを目的としていますが、形式的な保証（定理2）は、正確な制約を強制するコントローラに対して厳密に適用されるものであり、提案されたアルゴリズム1は、リアルタイム実行のために1次近似に依存しています。著者らは、本手法が一般的な非凸コスト関数を許容しながら、結果として得られるクローズドループの軌跡が、ユーザー定義のSTLミッション要件を満たすように努めるものであることを強調しています。さらなる研究を促進するため、アルゴリズムのコードは公開されています。