Continuous-Time Gaussian Belief Trees for Motion Planning

技術要約：連続時間ガウス・ビリーフ木（Continuous-Time Gaussian Belief Trees）によるモーションプランニング

問題定式化
本論文は、プロセスおよび観測の不確実性が存在する連続時間において、ロボットシステムが直面するサンプリングベースのモーションプランニングの課題に対処するものである。現実世界のロボットのダイナミクスは連続的に進化するが、センサは通常、離散的な間隔でしか観測値を提供しない。既存のビリーフ空間プランナは、離散時間の近似に依存することが多く、特定のタイムステップ（ノード）でのみ安全性を評価する。このアプローチは決定的な脆弱性を生む。すなわち、ノード間の離散的な更新の間、ノザによる影響でロボットの状態分布（ビリーフ）は連続的に拡大するため、離散的なチェックポイントが安全に見える場合でも、その間の区間で障害物に衝突する可能性がある。著者らは、全連続時間軌道にわたって確率的安全制約（チャンス制約）を満たし、衝突または制御違反の確率が指定された閾値 $P_{safe}$ を下回ることを保証するモーションプランを求める問題を定式化している。

手法
提案されているフレームワークである Continuous-Time Gaussian Belief Trees (CT-GBT) は、連続時間の確率的ダイナミクスを扱うために、既存のサンプリングベースプランナ（具体的にはRRTおよびSST）を拡張したものである。その手法は、以下の3つのコアコンポーネントで構成される。

1. ハイブリッド・ビリーフ伝播モデル:
   著者らは、システムのハイブリッドな性質を捉えるオフラインのビリーフ伝播モデルを導出している。観測エポックの間、ビリーフ（ガウス分布としてモデル化される）は、平均および全共分散（推定誤差共分散と推定共分散の和）を支配する連続時間の常微分方程式（ODE）に従って進化する。離散的な観測時刻において、ビリーフはカルマンフィルタの更新を通じて離散的なジャンプを行う。これにより、プランナは将来の観測に依存することなく、不確実性の進化を予測することが可能となる。

2. 連続時間確率的安全チェッカー:
   「サンプル間」の安全性のギャップに対処するため、本論文ではビリーフ・バリア関数に基づく衝突チェッカーを導入している。従来の、離散ノードでのみ安全性を検証する方法とは異なり、このチェッカーは軌道のセグメント全体を検証する。

   • 状態制約: 各障害物（凸多面体としてモデル化）に対して分離半空間を構築する。ビリーフ・バリア関数を定義することで、状態が障害物内に存在する確率がリスク予算を下回るようにする。
   • 制御制約: 制御入力は不確実な状態推定値に依存するため、確率変数として扱われる。本フレームワークは、状態推定の共分散を用いて、制御の許容性に関するチャンス制約を導出する。
   • 検証: ビリーフ・バリア関数の微分条件（$\frac{\partial h}{\partial b}\dot{b} \geq -h$）を用いた、連続時間における確率的安全性の十分条件を確立している。この条件は、各軌道セグメント内の高密度なグリッド上で検証され、観測更新間の安全性を保証する。

3. リスク配分:
   本フレームワークは、ブールの不等式（Boole's inequality）を用いて、総リスク予算 $P_{safe}$ を複数の状態制約および制御制約に配分し、違反の結合確率が制限内に収まることを保証する。

主な貢献
本論文の主な貢献は以下の4点である。

• ハイブリッド・ビリーフ伝播: 推定および推定共分散のための連続時間ODEの導出、および離散カルマンフィルタ更新の結合により、正確なオフライン・ビリーフ予測を可能にした。
• セグメントレベルの安全性保証: ビリーフ・バリア関数に基づき、軌道セグメント全体にわたって安全性を保証する連続時間衝突チェッカーを提供し、離散時間手法が見逃してしまうサンプル間の違反を検出する。
• 確率的制御制約: 従来のガウス・ビリーフ木（GBT）の手法には欠けていた、不確実な制御入力をビリーフ空間プランニングの枠組みに統合した。
• 理論的保証: アルゴリズムの正当性（健全性）および保守性に関する確率的完全性を証明し、有効な軌道が存在する場合、反復回数を増やすことでアルゴリズムがその発見確率が1に近づくことを保証した。

結果
本手法は、2つのベンチマークシステム（2D単一積分器および4D二重積分器）を用い、狭い通路を含む6つの環境において評価された。結果を、RRTおよびSSTプランナを用いた離散時間（DT）型のGBTバリアントと比較した。

• 成功率: モンテカルロ・シミュレーション（5,000回の実行）において、CT-GBTバリアントは、最も制約の厳しい狭い通路を含むすべての環境で0.94以上の成功確率を達成した。対照的に、DTバリアントは性能が急激に低下し、複雑な環境では成功率が0.05〜0.06まで落ち込んだ。
• 制約充足: CT-GBTはチャンス制約に対して高い充足確率（0.91〜1.00）を維持したが、DTプランナは、サンプル間の安全な挙動を検出できないため、頻繁に制約に違反し、最も制約の厳しいケースでは0.00まで低下した。
• 効率 vs 精度: DTプランナは計算負荷の低さからプランニング時間が短くなる場合もあったが、より高いコストの軌道を生成し、信頼性が著しく低かった。CTアプローチは、離散タイムステップを単に小さくするだけでは、連続時間定式化による安全性と性能のメリットを回復するには不十分であることを示した。

意義
本論文は、現実世界の連続時間ダイナミクスと離散時間プランニングモデルの間の溝を埋める、サンプリングベースのビリーフ・プランニングの原則的なフレームワークを提供すると主張している。連続時間の不確実性伝播を明示的に考慮し、軌道セグメント全体にわたって安全性を強制することで、提案手法はチャンス制約の堅牢な執行を実現する。著者らは、これが特に、離散時間版がサンプル間の安全な挙動を見逃して失敗するような、安全性が極めて重要なシナリオ（例：狭い通路）において極めて重要であることを強調している。本手法は、不確実性下での安全かつ効率的なモーションプランニングの基礎を築くものであり、信号時相論理（STL）のようなより豊かなタスク仕様への将来的な拡張の可能性も示唆している。