技術的概要：ターミナルコストと学習された不確実性を備えた信念状態 - コスト空間におけるキネダイナミック計画

問題定義

実世界の環境で動作するロボットシステムは、モデリング誤差、知覚ノイズ、複雑な接触相互作用に起因する実行の不確実性を考慮しつつ、キネダイナミック制約を満たす動的に実行可能な運動を生成しなければならない。既存のサンプリングベースのキネダイナミックプランナ（AO-RRT、SST など）は、確率的完全性と漸近的最適性を提供しているが、通常は軌道に沿って蓄積される「実行コスト」（経路長やエネルギーなど）のみを最適化し、目標到達を単なる実行可能性のチェックとして扱っている。

このアプローチには、2 つの決定的な限界が存在する：

1. ターミナル状態の質：多くのタスクは、取った経路だけでなく、最終状態の質に依存する。例えば、平面プッシングにおいて、より短い経路は高いターミナル不確実性を引き起こし、目標に到達する確率を低下させる可能性がある。複数の目標があるシナリオ（駐車など）では、プランナは、離脱が困難または望ましくない幾何学的に近い目標を選択し、特定のターミナル領域に対する好みを無視する傾向がある。
2. 不確実性の伝播：状態を確率分布とする「信念空間」での計画は、堅牢性のために不可欠である。しかし、既存の方法は、厳密な確率閾値や軌道全体の代理モデルに依存しており、明示的にターミナル信念状態を最適化するわけではない。さらに、複雑なシステム（接触の多いプッシングなど）は、解析的な不確実性モデルを持たないことが多く、信念空間プランナと密接に統合されることのないデータ駆動型アプローチが必要とされる。

手法

著者は、漸近的最適ランダム探索木（AO-RRT）に明示的なターミナルコスト目的関数を追加したKiTe（ターミナルコストを備えたキネダイナミック計画）を提案する。この手法は、主に 3 つの層で動作する。

1. 状態空間におけるターミナルコストを備えた KiTe

核心となるアルゴリズムは、拡張状態 - コスト空間 $\mathcal{Y} = \mathcal{X} \times \mathbb{R}^+$ において、最適キネダイナミック計画問題を再定式化する。

• 拡張ダイナミクス：状態を $x$ から $y = (x, c)$ に拡張し、ここで $c$ は実行コスト $\ell(x, u)$ を蓄積する。ダイナミクスにはコスト率を含めるように拡張され、$\dot{y} = (f(x, u), \ell(x, u))$ となる。
• ターミナルコストの統合：以前の定式化では探索中にターミナルコストが無視されていたのに対し、KiTe は目標領域に到達した時点でのみ、総コスト $C_T = \int \ell dt + \phi(x(T))$ を評価する。
• 探索戦略：アルゴリズムはコスト上限 $c_{best}$ を維持する。解が見つかった場合、上限は $c_{new} + \phi(x_{new})$ に更新される。その後、サンプリング戦略はこの上限で定義される準レベル集合に焦点を当て、総コスト（実行 + ターミナル）を改善できない分岐を剪定する。
• 理論的保証：著者は、システムダイナミクス、実行コスト、ターミナルコストがリプシッツ連続性の条件を満たす限り、ターミナルコストを備えた AO-RRT が漸近的最適性を保持することを厳密に証明する。

2. 信念空間への拡張

この定式化は、状態上の確率分布の空間 $\mathcal{B}$ を用いた信念状態 - コスト空間 $\mathcal{Y}_b = \mathcal{B} \times \mathbb{R}^+$ に拡張される。

• メトリック：ガウス信念の平均と共分散の両方の変化を自然に捉えるため、信念空間のメトリックとして2-ワッサーシュタイン距離を採用する。
• 目標到達の下限：重要な理論的貢献として、半径 $r$ の球として定義された目標領域 $G$ に到達する確率は、ターミナル信念 $b(T)$ と目標におけるディラック測度 $\nu_g$ との間のワッサーシュタイン距離によって以下のように下限付けられることを証明した：
  $$\text{Pr}(x(T) \in G) \geq 1 - \frac{W_2^2(b(T), \nu_g)}{r^2}$$
  これにより、プランナはワッサーシュタイン距離をターミナルコスト $\psi(b(T))$ として使用でき、厳密で手動調整が必要な確率閾値を必要とせずに、目標到達成功の下限を直接最適化できる。
• リプシッツ連続性：著者は、確率的ダイナミクスに関する穏やかな仮定（特にプロセス不確実性とノイズ注入のラプシッツ連続性に関する仮定）の下では、信念ダイナミクスとコスト関数がワッサーシュタインメトリックに関してリプシッツ連続であることを証明した。これにより、KiTe の漸近的最適性が信念空間にも拡張されることが保証される。

3. 信念ダイナミクスの学習

解析的不確実性モデルが利用できないシステム（接触の多いプッシングなど）に対応するため、KiTe は学習された信念ダイナミクスモデルを統合する。

• モデルアーキテクチャ：ニューラルネットワーク（MLP）を訓練し、局所的な確率的遷移を予測させる。状態 $x_i$ と制御 $u_i$ が与えられると、ネットワークは平均遷移 $\Delta x_i$ と分散ベクトル $\sigma_i^2$ でパラメータ化されたプロセス共分散を出力する。
• 訓練目的：モデルは、相互作用データを用いた負の対数尤度（NLL）損失で訓練される。これにより、ネットワークは平均ダイナミクスとアレイトリック不確実性（プロセスノイズ）の両方をデータから直接学習できる。
• 伝播：学習された平均と共分散は、プランナ内で信念状態（平均と共分散）を前方に伝播するために使用され、ユークリッド空間およびリー群（例：SE(2)）の状態空間の両方をサポートする。

主要な貢献

1. アルゴリズムの拡張と証明：明示的なターミナルコストを備えた AO-RRT の実装と、状態空間および信念空間の両方で漸近的最適性が保持されることを示す厳密な証明。
2. 成功確率との理論的リンク：ターミナル信念と目標との間のワッサーシュタイン距離を最小化することが、目標到達成功の確率の下限を誘発することの証明。これにより、信頼性のための原理的なターミナル目的関数が提供される。
3. データ駆動型の信念伝播：NLL 損失を用いて相互作用データから直接信念ダイナミクスモデル（平均と不確実性）を学習し、解析モデルが存在しないシステム向けに KiTe フレームワークに統合する方法の実証。
4. 実証的検証：シミュレーションおよび実世界における 3 つの多様なタスク（フラッピーバード、駐車、平面プッシング）における包括的な評価。KiTe を最先端のベースライン（AO-RRT、SST、ガウス信念木）と比較する。

実験結果

本論文は、KiTe を 3 つのタスクで検証する：

• フラッピーバード（決定論的）：KiTe がターミナルコストを最適化することで目標の好み（例、隙間の中央に鳥を位置させる）を符号化できることを実証。一方、ベースラインは目標領域に到達すると改善を停止する。KiTe は、実行コストのわずかな増加のみで、目標の中心に近い軌道を実現した。
• 駐車（不確実性を伴う SE(2)）：複数の実行可能な駐車スペースがあるタスクにおいて、KiTe は「好ましい」目標領域（前方のスペース）を正常に優先し、経路長のみを最適化する、またはチャンス制約を使用するベースラインと比較して目標到達成功率を向上させた。ターミナルコストを備えた信念空間計画は、最も高い実行成功率をもたらした。
• 平面プッシング（接触が豊富、実世界）：ゴミトラックやクラッカーボックスなどの物体に対して学習されたダイナミクスを使用し、KiTe はモデル不確実性を伴うアクティブプランニングを含むベースラインを実行成功率において凌駕した。プランナは、より低いターミナル不確実性と高い成功確率をもたらす、より長い幾何学的経路を選択し、ターミナル信念の質を最適化することの価値を実証した。

意義と主張

本論文は、KiTe がキネダイナミック計画における根本的なギャップ、すなわち軌道最適化とターミナル状態の質の分離を解決すると主張する。ターミナル状態を明示的に最適化することで、この手法は以下の点を実現する：

• 信頼性の向上：目標へのワッサーシュタイン距離を最小化することで、不確実性下での目標到達確率を直接向上させる。
• 好みの符号化：プランナが、単なる幾何学的近接性ではなく、望ましさ（離脱の容易さ、安定性など）に基づいて複数の実行可能な目標を区別することを可能にする。
• 学習と計画の架け橋：学習された不確実性モデルをサンプリングのバイアスとしてだけでなく、信念伝播と最適化ループに密接に統合するフレームワークを提供する。

著者は、この手法が設計パラメータ（ターミナル重み）を導入し、学習されたモデルに依存するものの、不確実な環境において動的に実行可能であるだけでなく、信頼性が高く、好みを考慮した運動を生成するための原理的かつ理論的に裏付けられたアプローチを提供すると結論づけている。