💻 computer science

DynoJEPP: Joint Estimation, Prediction and Planning in Dynamic Environments

DynoJEPP は、予測と計画が状態推定を汚染するのを防ぐために有向因子を導入することで動的環境における安全な航行を保証する新しいファクターグラフフレームワークであり、その協調拡張は他のエージェントの挙動を取り入れることで軌道計画をさらに強化する。

原著者： Mikolaj Kliniewski, Jesse Morris, Yiduo Wang, Ian R. Manchester, Viorela Ila

公開日 2026-05-14

📖 1 分で読めます☕ さくっと読める

CC BY 4.0

原著者： Mikolaj Kliniewski, Jesse Morris, Yiduo Wang, Ian R. Manchester, Viorela Ila

原論文は CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/) でライセンスされています。 ✨ これは以下の論文のAI生成解説です。著者が執筆または承認したものではありません。技術的な正確性については原論文を参照してください。免責事項の全文を読む

繁忙な倉庫を移動しようとするロボットを想像してください。ロボットは同時に3つの主要な任務を遂行する必要があります：

推定（Estimation）：「今、私はどこにいて、あの動く箱はどこにあるのか？」
予測（Prediction）：「数秒後、あの箱はどこにいることになるのか？」
計画（Planning）：「それらを避けながら目的地に到達するには、どのように移動すべきか？」

本論文は、これら3つの任務をすべて一括して処理する新しいシステム「DynoJEPP」を紹介しています。しかし、著者らは通常、これら3つの任務が互いにどのように情報をやり取りするかという点に重大な問題があることを発見し、それを解決する巧妙な手法を開発しました。

問題点：「頼りすぎ」な友人

従来のシステムでは、ロボットの「計画」脳と「予測」脳が「推定」脳と自由に会話していました。問題点は、「計画」脳が非常に楽観的であることです。経路を見つけたいあまり、時には「推定」脳を、ロボットが実際よりも安全な場所にいると思い込ませたり、動く障害物が実際よりも早く停止すると信じさせたりすることがあります。

著者らはこれを悪循環のフィードバックに例えています。「私、きれいに見える？」と友人に尋ねて、「はい、完璧だよ」と言われれば、たとえ歯にホウレンソウが挟まっていても、それを信じてしまうかもしれません。ロボットの場合、この「歯に挟まったホウレンソウ」は危険な誤算です。ロボットは安全だと考えていますが、実際には衝突しようとしています。

実験において、ロボットが計画と予測の部分を推定部分に影響させた場合、ほぼすべてのテストで衝突しました。

解決策：一方通行の道

これを解決するため、著者らはDirected Factor（指向性因子）を構築しました。これは、ロボットの脳内における一方通行の道、あるいは厳格な管理者のようなものです。

従来の方法： 情報は行き来していました。計画が推定を変え、推定が計画を変えることができました。
新しい方法（DynoJEPP）： 情報は一方通行のみです：推定 → 予測 → 計画。

「推定」部分（真実を語る者）はセンサーを見て、「私たちが今どこにいて、箱がどこにあるのか、これが正確な位置だ」と言います。この真実を「予測」部分に送ります。予測部分は、「わかった、その真実に基づけば、箱はここへ移動するだろう」と言います。それを「計画」部分に送ります。計画部分は、「了解、それらを避けるためにこの方向へ操縦する」と言います。

重要なのは、「計画」部分は「推定」部分に返答できないことです。「実は私たちはこっちにいると思うから、箱もあっちにあるはずだ」とは言えません。「推定」部分は、ロボットが前進したいという願望によって汚染されることなく、純粋なまま保たれます。

「協調的」なアップグレード

論文では、**Cooperative DynoJEPP（協調的 DynoJEPP）**と呼ばれる特別なバージョンも紹介されています。これは、動く障害物が単なる無意識の箱ではなく、自ロボット（エゴロボット）も見える他のロボットである状況を想定しています。

このバージョンでは、ロボットは「お前が動いているのを見ており、お前も私を見ていることを知っている。私が減速すれば、お前は私を追い越すために加速するかもしれない」と言います。このシステムにより、ロボットの計画が、他の物体の経路予測を優しく誘導することが可能になります。これは、片方が相手の動きを盲目的に推測するのではなく、二人のパートナーが互いのステップに合わせて調整するダンスのようなものです。これにより、ロボットは神経質に待機するのではなく、より大胆に、そして速く移動できるようになります。

結果

著者らはシミュレーションされた倉庫でこれをテストしました：

一方通行の道なし（従来の方法）： ロボットは自身の楽観的な計画に混乱させられ、ほぼ毎回衝突しました。
一方通行の道あり（DynoJEPP）： ロボットは衝突することなく、静止した障害物も動く障害物も無事に回避して移動しました。
協調的アップグレードあり： ロボットは他の動く物体と「交渉」することで、さらに効率的に移動しました。

結論

本論文は、ロボットが動的な世界で安全に行動するためには、「現実確認（推定）」と「夢（計画）」を分離しておく必要があると主張しています。Directed Factorを用いて、計画部分が現実確認を混乱させるのを防ぎ、ロボットを現実に根ざしたままにしながらも、安全な経路を計画できるようにします。情報の流れ方というこの単純な変化が、衝突するロボットを成功するロボットへと変えたのです。

技術的サマリー：DynoJEPP – 動的環境における同時推定、予測、および計画

1. 問題提起

現実世界の環境におけるロボットの自律性は、複雑で予測不可能な運動を行う動的物体の存在によって根本的に課題に直面しています。近年の進展により、静的な環境においてロボットの状態推定と経路計画をファクターグラフフレームワーク内で統合することに成功していますが、これらのアプローチを動的なシーンに拡張することは依然として限られています。

既存の解決策は、通常、以下の 2 つの制限のいずれかに直面します：

完全な知識の仮定： 将来の物体の軌道が事前に既知であると仮定しますが、これは動的な設定では非現実的です。
反応的な戦略： 軌道予測の先見性を欠く、純粋に反応的な回避に依存しています。

同時最適化フレームワークにおいて、見過ごされがちな重要な課題は、双方向の情報フローです。従来の結合形式では、計画および予測モジュールからの情報が状態推定モジュールにフィードバックされます。このフィードバックループは、将来の計画に由来する「楽観的」なバイアスによって状態推定を汚染し、推定精度の低下、望ましくない動作、そして最終的には安全でない計画や衝突を引き起こします。

2. 手法：DynoJEPP フレームワーク

著者は、ロボットおよび物体の状態推定、軌道予測、および局所計画を同時に最適化する、統合されたファクターグラフベースのフレームワークであるDynoJEPPを提案します。中核的な革新は、情報フローを制御するための新規の方向性ファクターの導入にあります。

A. 方向性ファクター

計画および予測が推定を劣化させるのを防ぐため、DynoJEPP は一方向の情報フローを強制する方向性ファクターを導入します：

推定 $\to$ 予測/計画： 推定結果（現在のロボット姿勢および物体の運動）は、予測および計画を通知するために前方へ伝播します。
予測/計画 $\nrightarrow$ 推定： 将来の状態および計画の制約は、現在または過去の状態の推定に影響を与えることが防がれます。

技術的には、これはヤコビアン操作によって達成されます。正規方程式（ $J^\top J \delta = -J^\top r$ ）において、ソース変数（推定状態）に関するヤコビアンの項をゼロにすることで、フレームワークは計画/予測コストの勾配が推定変数を更新しないことを保証します。これにより、計画が現在の推定を利用するために必要な依存関係を維持しつつ、推定サブグラフを計画サブグラフから構造的に分離します。

B. 結合形式の構成要素

このフレームワークは、3 つのモジュールを単一のファクターグラフに統合します：

状態推定： ハイブリッド動的 SLAM に基づき、ロボット姿勢（ $W X_k$ ）および動的物体の SE(3) 剛体運動（ $W eH_k$ ）を推定します。点測定、オドメトリ、および運動平滑化ファクターを利用して、物体の重心を追跡します。
物体運動予測： 推定された SE(3) 運動を利用して、地平線 $N$ における将来の物体軌道を予測します。一定速度モデル（平滑化ファクターによって強制される）を採用しますが、静的な障害物の近傍では、静的シーン認識ファクター（ESDF ベース）を使用してこの仮定を緩和します。
局所計画： 将来のロボット姿勢、速度、および加速度を最適化するために、ファクターグラフベースのモデル予測制御（MPC）を採用します。これには以下が含まれます：
- 静的シーン認識： 静的な障害物を回避するためにユークリッド符号付き距離場（ESDF）を使用します。
- 動的シーン認識： 予測された軌道に基づいて、移動物体との近接性を罰する方向性動的障害物ファクターを使用します。

C. 協調拡張（C-DynoJEPP）

本論文は、自ロボットが協調行動を計画に組み込むことを可能にする*協調 DynoJEPP（C-DynoJEPP）*を導入します。この拡張では、動的障害物ファクターが計画*から予測*へ向けて方向付けられます。これにより、予測と計画の間に方向性サイクルが作成され、ロボットが物体に反応し、かつ物体がロボットの意図した経路に反応すると予想される相互影響をモデル化します。これにより、対話的なシナリオにおいて、より保守的ではなく、より断定的なナビゲーションが可能になります。

3. 主要な貢献

DynoJEPP フレームワーク： 動的物体の運動を推定し、その進化を予測し、これらの予測を単一の最適化問題内でロボット制御動作に組み込む、ファクターグラフベースの最初のフレームワーク。
新規の方向性ファクター： ファクターグラフにおける影響の方向を指定するメカニズムであり、推定、予測、および計画間の制御された情報フローを可能にします。これにより、将来の計画制約による状態推定の「汚染」を防ぎます。
協調最適化： 動的物体の協調レベルをパラメータ化する拡張（C-DynoJEPP）であり、状態推定の独立性を損なうことなく、ロボットの計画と物体の予測間の相互影響を可能にします。

4. 実験結果

著者は、Gazebo におけるシミュレーション倉庫環境で、差動駆動ロボットと動的障害物を用いてフレームワークを評価しました。3 つの構成を比較しました：無方向（双方向フロー）、方向性（DynoJEPP）、および分離型（推定の後、結合された予測/計画）。

安全性と成功率： 動的障害物を含む 10 回のナビゲーション実験において、無方向構成は、楽観的な計画による推定の汚染により、過半数のケースで失敗しました（10 回の動的試行のうち 8 回で衝突）。対照的に、方向性構成はすべての試行で100% の成功率を達成しました。
推定精度： 方向性手法は、分離型ベースラインと同等の推定精度を維持し、無方向アプローチを大幅に上回りました。無方向手法は、ロボットおよび物体の軌道推定の両方で著しい劣化を示しました。
協調ナビゲーション： 反応的な動的障害物を持つシナリオにおいて、C-DynoJEPPは、標準的な DynoJEPP よりも断定的にナビゲートする能力を示しました。協調重みを利用して予測に影響を与えることで、目標到達までのステップ数を短縮しました（最速の DynoJEPP 実行の 92 ステップに対し、82 ステップ）。

5. 意義と主張

本論文は、動的環境における安全な運用には、情報フローの方向性を制御することが不可欠であると主張しています。方向性ファクターがなければ、計画と推定の結合は、計画の目的が状態推定を歪めるフィードバックループにつながり、安全でない動作を引き起こします。

著者は、推定自体は計画との結合最適化から本質的に利益を得るわけではありませんが、予測および計画は、特に相互作用行動をモデル化する際に、推定との結合最適化から利益を得ると強調しています。提案されたフレームワークは、状態推定の完全性を犠牲にすることなく、安全な同時最適化を可能にする堅牢な解決策を提供します。

本研究は、リアルタイム性能を達成するために、効率的で増分的かつマルチスレッド版の DynoJEPP の実装に将来の研究を集中させることで結論付けており、Nav2 などの業界標準とのベンチマーク比較を行うことを示唆しています。