Kimera-Multi

Tian 2022 · 論文

一行要約 — Kimera-Multiは、閉ループの外れ値に対してロバストであり、ピアツーピア通信のみによって完全に分散され、かつ大域的に一貫したメトリック・セマンティックな3Dメッシュをリアルタイムで構築できる、初のマルチロボットシステムである。

問題

これまでの協調SLAMシステムは、中央サーバーに依存するか、セマンティックな内容を持たない純粋に幾何的なマップを生成するかのいずれかであり、いずれも知覚的エイリアシングにさらされていた――視覚的に似た場所は、共同推定を損なう可能性のある誤ったロボット間・ロボット内閉ループを生成する。既存のロバスト手法は初期化に過度に依存するか、（PCMの最大クリークのような）再現率の低いヒューリスティック探索を用いるかのいずれかであった。Kimera-Multiは、リンクが利用可能なときに近隣とのみ通信するロボットチームが、偽の閉ループを識別・棄却しながら、大域的に一貫した「セマンティックな」3Dメッシュをリアルタイムで構築できるかを問う。

手法とアーキテクチャ

各ロボットのフロントエンド。 各ロボットはKimeraを実行する。視覚慣性オドメトリにはKimera-VIOを用い、面がセマンティックラベルを持つローカル3Dメッシュを生成する。ロボットが通信範囲内に入ると、分散型の場所認識がバグオブワーズベクトルを交換し、マッチが幾何学的検証を引き起こす。これによりキーポイントと特徴記述子が伝送され、推定のロボット間閉ループが計算される。

段階1 — ロバストな初期化。 ロボット$\alpha$（フレーム$A$）のポーズ$i$とロボット$\beta$（フレーム$B$）のポーズ$j$の間の閉ループは、候補となるフレーム整合を生成する。

$$\widehat{X}^{A}_{B_{ij}} \triangleq \widehat{X}^{A}_{\alpha_i}\, \widetilde{X}^{\alpha_i}_{\beta_j}\, \big(\widehat{X}^{B}_{\beta_j}\big)^{-1},$$

ここで$\widehat{X}$はオドメトリのポーズ推定値、$\widetilde{X}^{\alpha_i}_{\beta_j}$は計測された閉ループである。インライアの整合は相互に一致するため、相対フレーム変換はロバストなポーズ平均化によって求められる。$\widehat{X}^{A}_{B} \in \arg\min_{X \in \mathrm{SE}(3)} \sum_{(i,j) \in L_{\alpha,\beta}} \rho(r_{ij}(X))$ であり、$\rho$は切断最小二乗（TLS）コストで、GNC（GTSAM）を用いてローカルに解かれる。ロボットレベルの依存グラフのスパニングツリーが、1つのロボットのフレームをチーム全体に伝播させる。

段階2 — 分散グラジュエーテッド非凸性（D-GNC）。 すべての軌跡は、オドメトリ（二次コスト）と閉ループ（TLSコスト）に対するロバストPGOによって洗練され、残差はコーダル距離で計測される。GNCはBlack–Rangarajan双対性を用いて、ロバスト推定を次のように書き換える。

$$\min_{x\in\mathcal{X},\, w_i\in[0,1]}\; \sum_i \big[\, w_i\, r_i^2(x) + \Phi_{\rho_\mu}(w_i) \,\big],$$

ここで$w_i$は各計測値に対する信頼度重み、$\Phi_{\rho_\mu}$は外れ値プロセスへのペナルティ、制御パラメータ$\mu$は代理コストを凸から真のTLSコストへとアニーリングする。D-GNCは、完全に分散された2つのステップを交互に実行する。(i) 変数更新 — 階数制限緩和（デフォルトで階数5、更新ごとに15反復）上でリーマン多様体ブロック座標降下（RBCD）ソルバーによって解かれる重み付きPGO。各ロボットは自身の軌跡のみを更新し、近隣とは「公開ポーズ」のみを交換する。(ii) 重み更新 — TLSの閉形式解で、各閉ループごとに独立に計算される。

$$w_i \leftarrow \begin{cases} 0, & \widehat{r}_i^{\,2} \in \big[\tfrac{\mu+1}{\mu}\bar{c}^2,\, +\infty\big], \\ \frac{\bar{c}}{\widehat{r}_i}\sqrt{\mu(\mu+1)} - \mu, & \widehat{r}_i^{\,2} \in \big[\tfrac{\mu}{\mu+1}\bar{c}^2,\, \tfrac{\mu+1}{\mu}\bar{c}^2\big], \\ 1, & \widehat{r}_i^{\,2} \in \big[0,\, \tfrac{\mu}{\mu+1}\bar{c}^2\big], \end{cases}$$

$\widehat{r}_i$は現在の残差、$\bar{c}$はTLSしきい値である――外れ値の重みは$\mu$のアニーリングにともなって0に向かう。最後に、各ロボットはメッシュ変形によって自身のローカルセマンティックメッシュを補正し、再構成が最適化された軌跡と整合するようにする。

実験結果

• シミュレーション + EuRoC（表I、ATE単位はm）: フォトリアリスティックなMedfieldシーン（総軌跡2396 m）では、D-GNCが3.92 mに達するのに対し、単純最小二乗は64.2 m、PCMは12.5 m、そして「集中型」GNCは3.88 mである。EuRoC Machine Hall（5つのシーケンスを5台のロボットとして扱う、466 m）では0.41 mに対し、PCMは1.76 m、集中型は0.52 mである。PCMに続いてGNCを行う方式は、PCMの再現率の低さにより、D-GNC単体より一貫して劣る。
• 通信（表II）: Medfieldでは、完全な分散パイプラインが65.9 MB（場所認識22.6 MB＋幾何学的検証41.5 MB＋DPGO 1.8 MB）を使用するのに対し、生画像を集中管理する場合は2113 MB（キーポイントのみなら141 MB）となる。
• 実際の屋外データセット（Jackal UGV、RealSense D435i）: Medfield（600/860/728 mのロボット）では、エンドツーエンド誤差が18.74/14.84/24.55 m（Kimera-VIO）から0.01/0.13/0.09 mに低下し、これは集中型ソルバーと同一である。15650ポーズのグラフでの100回のRBCD反復は53秒かかる。より難しいMIT Stataデータセット（ロボット間閉ループが少ない）では、誤差は集中型の0.01/21.56/1.17 mに対し0.03/33.13/1.26 mであり、完全な変数更新（2000回のRBCD反復、14分）を必要とする。D-GNCは大量の外れ値混入を棄却し、例えばMedfieldではロボット1とロボット2の間の707個の推定閉ループのうち340個を受理する。

SLAMにおける意義

Kimera-Multiは、ロボットチームに拡張されたMIT SPARK Kimeraエコシステムの旗艦であり、現代の分散SLAMシステムが提供すべき標準を打ち立てた。ロバスト性（DOOR-SLAMのPCMが開いた道を踏襲するGNCによる外れ値除去）、集中型と同等の精度を持つ分散化、そして下流の計画に利用可能なセマンティックに意味のある高密度マップである。そのメトリック・セマンティックメッシュの出力は、シーングラフ系の研究（Kimera、Hydra、Hydra-Multi）にも供給されている。今日、セマンティクスを備えたマルチロボットマッピングが必要なら、これが標準的な参照システムである。

関連ノート

• DOOR-SLAM — ロバストな分散閉ループ除去の前身
• Swarm-SLAM — 競合する分散型C-SLAMフレームワーク
• Kimera-VIO — 単一ロボットの視覚慣性フロントエンド
• Kimera / 3D Dynamic Scene Graph — 単一ロボットのメトリック・セマンティックな基盤
• Hydra-Multi — この系譜の上に構築されたマルチロボットシーングラフ
• Robust pose-graph optimization — GNCの単一ロボット版