Hydra-Multi

Chang 2023 · 論文

一行要約 — 最初のマルチロボット空間認知システム: 各ロボットがHydraを実行してオンラインでローカルな3Dシーングラフを構築し、中央局がそれらを位置合わせ・最適化・統合して一つの大域的に整合した階層的マップにする。

問題

3Dシーングラフは表現力の高い高次マップ表現であることが示されたが、Hydraは単一ロボットの視点からしかそれを構築できず、広大な環境を1台のロボットで迅速にカバーするのは非現実的だった。一方、マルチロボットSLAMシステムは階層的なセマンティクスを持たないメトリックマップを生成していた。オンラインでマルチロボットシーングラフを構築するには、3つの結合した問題を解く必要がある: ロボットフレーム間の相対変換を推定すること（初期キャリブレーションなしで）、強いパーセプチュアルエイリアシングにもかかわらずロボット間ループクロージャを検出すること、そして異なるロボットが提供する重複したシーングラフノードを統合することである。

手法とアーキテクチャ

フロントエンド（コントロールステーション）。 各ロボットはローカルなHydraインスタンスを実行し、シーングラフ全体を定期的に送信する。シーングラフプロセッサはこれらを蓄積して、慎重なノードごとの参照フレーム管理のもとで単一の未最適化・未統合なフロントエンドグラフを構築する。ロボット間ループクロージャは、Hydraの階層的検出器 — トップダウンの記述子比較（places → objects → 視覚的見え方）に続く、RANSAC（視覚キーポイント）またはTEASER++（オブジェクト）によるボトムアップの幾何検証 — によって発見される。

バックエンド: 位置合わせ — 最適化 — 統合。

  1. 初期位置合わせ。 ロボットAAとBB間の各ロボット間ループクロージャ(αi,βj)(\alpha_{i},\beta_{j})は、AAのフレームにおけるBBのフレームのノイズを含む推定値を与える:

X^B,ijA=X^αiA X~βjαi(X^βjB)−1\widehat{\mathbf{X}}^{A}_{B,ij}=\widehat{\mathbf{X}}^{A}_{\alpha_{i}}\,\widetilde{\mathbf{X}}^{\alpha_{i}}_{\beta_{j}}\big(\widehat{\mathbf{X}}^{B}_{\beta_{j}}\big)^{-1}

ここでX^\widehat{\mathbf{X}}はオドメトリ姿勢推定値、X~βjαi\widetilde{\mathbf{X}}^{\alpha_{i}}_{\beta_{j}}はループクロージャ計測値である。これらの推定値は、打ち切り最小二乗コストρ\rhoを用いたロバスト姿勢平均化によって融合される:

X^BA∈arg⁡min⁡X∈SE(3)  ∑(i,j)∈LA,Bρ(∥X⊟X^B,ijA∥Σ)\widehat{\mathbf{X}}^{A}_{B}\in\arg\min_{\mathbf{X}\in\mathrm{SE}(3)}\;\sum_{(i,j)\in L_{A,B}}\rho\left(\big\|\mathbf{X}\boxminus\widehat{\mathbf{X}}^{A}_{B,ij}\big\|_{\Sigma}\right)

GTSAM内のGraduated Non-Convexity (GNC)で解かれる。ロボットはk=5k=5個のインライアが残った時点で初期化済みとみなされ、変換はロボットレベルの依存関係グラフの全域木に沿って連結される。 2. 統合の提案。 位置合わせ後、統合候補が提案される: 重なり合う場所ペア（距離≤0.01\leq 0.01m、類似した半径）と、同一のセマンティックラベルを持ち重なるバウンディングボックスを持つオブジェクトペア（そのメッシュ頂点に対するICPから変換を求める）。 3. ロバストなシーングラフ最適化。 エージェント姿勢グラフ、場所、統合候補オブジェクト、サブサンプリングされたメッシュ制御点がエンベデッド変形グラフを構成し、姿勢グラフ最適化として最適化される

arg⁡min⁡T1,…,Tn∈SE(3)  ∑Eij∥Ti−1Tj⊟Eij∥Ωij2\arg\min_{\mathbf{T}_{1},\ldots,\mathbf{T}_{n}\in\mathrm{SE}(3)}\;\sum_{\mathbf{E}_{ij}}\big\|\mathbf{T}_{i}^{-1}\mathbf{T}_{j}\boxminus\mathbf{E}_{ij}\big\|^{2}_{\mathbf{\Omega}_{ij}}

ここで辺Eij\mathbf{E}_{ij}はオドメトリ、ループクロージャ、メッシュ剛性、統合の各因子である。GNCは誤ったループクロージャや誤った統合を外れ値として除去する。 4. ノードの統合。 GNCインライアの統合が実行され、メッシュが再補間され、オブジェクトの重心/ボックスが再計算される。提案された統合の半数未満しか有効でない場合、すべて取り消され、統合された場所から部屋が再分割される。

異種チーム。 少なくとも1つの層と互換性のあるマップを持つロボットであれば貢献できる: オブジェクトベースSLAMロボットはオブジェクト層に貢献し、セマンティクスを持たないメッシュを持つLiDARロボットもメッシュと場所には貢献できる — この階層構造こそが異種マップを融合可能にしているものである。

実験結果

SLAMにおける意義

Hydra-Multiは、ロボット群にわたって協調的に3Dシーングラフを構築した最初のシステムであり、Kimera/Hydraの系譜を単一ロボットからフリート規模のセマンティックマッピングへと拡張した。その位置合わせ-最適化-統合バックエンドは、ループクロージャを補正するのと同じロバストな仕組み（変形グラフ上のGNC）が、ロボット間のノード統合を調停するのにも使えることを示している。また、その異種チームの結果はアーキテクチャ上の重要な指摘をもたらす: シーングラフの階層構造自体が相互運用層であり、LiDARのみのロボットと視覚ベースのロボットが1つの世界モデルを共有できるようにする — これは捜索救助、倉庫、その他あらゆるマルチロボット展開に関連する。

関連ノート