GPUを利用したStein Particle Filterによる点群6自由度モンテカルロSLAM

第43回日本ロボット学会学術講演会発表スライド

Transcript

1. GPUを利用したStein Particle Filterによる 点群６自由度モンテカルロSLAM 中尾拓道*1*2 小出健司*2 高野瀬碧輝*2 大石修士*2 横塚将志*2 伊達央*1 *1筑波大学 *2産総研
2. 1 動画
3. 2 研究背景  正規分布近似（既存の３次元SLAM）の問題点 正規分布近似に依存している以上曖昧性の強い状況への対処は困難 曖昧性の強い状況例 • 森林内などの不確実なループ候補が複数存在する状況 • 長期的な点群の縮退 • 誘拐状態 [Thrun+, 2005] 複数仮説に基づいたSLAM手法により曖昧性の強い状況に対処したい
4. 3 関連研究  Rao-Blackwellized Particle Filter (RBPF) SLAM [Griseti+, 2007] ２次元SLAMでは一般的なサンプリングベースSLAM手法 軌跡のサンプルをばらまいて，その中から最適なサンプルを選び出す手法  これまでの提案手法（ICRA2025） パーティクル毎に簡易的な大域軌跡最適化 （ループクロージャ） RBPF SLAM ＋ 確率分布の多峰性を柔軟に表現 定式化の不足 大域軌跡最適化の精度が低い 代表値抽出の安定性が低い 課題 10万パーティクルをGPU並列処理して 6自由度モンテカルロSLAMを実現
5. 4 提案手法  キーアイディア 1. 最新姿勢についてStein Particle Filterを適用 2. 大域マッチングコスト最小化とGaussian Belief Propagation (GBP) による パーティクル毎の大域軌跡最適化（グラフ最適化） 3. 大域マッチングコストによる代表値抽出の安定化 4. GPUの並列演算能力を最大限活かすアルゴリズム キーアイディアを完全SLAM問題として定式化 少数のサンプルであっても事後確率分布を柔軟に表現
6. 5 システム概要と問題設定  SLAMのバックエンドシステムを実装  問題設定 𝑁個のパーティクル集合により状態（完全SLAM）の事後確率分布を推定する パーティクル集合： パーティクル状態： ※今後𝑖, 𝑗はパーティクルインデックス LiDAR点群 IMU加速度 IMU角速度 入力 LiDAR Inertial Odometry (LIO) サブマップ構築 フロントエンドシステム モンテカルロSLAM （提案システム） バックエンドシステム サブマップ
7. 6 問題設定  完全SLAMの事後分布 𝑥0 𝑖 𝑥1 𝑖 𝑥2 𝑖 𝑥4 𝑖 𝑥3 𝑖 𝑥5 𝑖 例：𝑖番目のパーティクルのファクタグラフ 観測モデル （SVGD） 動作モデル （現在姿勢へのノイズ付与） 事前モデル （パーティクル毎のグラフ最適化）  実装上の工夫 地図の代わり 一般的なグラフベースSLAM[Thrun+, 2005]との違い 観測モデル： 近傍の変数とのみファクタを結べば十分 各パーティクルは疎なファクタグラフを保持
8. 7  SVGD（変分推論）[Liu+, 2016] （関連研究：Stein Particle Filter[Maken+, 2022] ） 確率分布の勾配情報（引力）とパーティクル近接関係情報（斥力）用いて 確率分布にフィットするようにパーティクルを輸送 サンプリング効率が高い 低確率パーティクルを保持できる 高次元になると破綻する[Ramdas+, 2015] （SLAMへのそのままの適用は難しい） 関連研究 SVGD (64 particles) 従来のパーティクル更新則 Resampling (1024 particles) MegaParticles[Koide+, 2024]
9. 8  二次最適更新ベクトル 最新姿勢についてStein Particle Filterを適用  Gauss-Newton SVGD[Koide+, 2024] 更新式： 更新ベクトル： 引力 斥力 尤度分布 の二次最適更新ベクトル カーネル関数 • Localizationの場合 事前地図と現在フレームのスキャンマッチングで求まる • SLAMの場合 地図が確率分布（パーティクル）なので，各パーティクルの地図とスキャンマッチング 各パーティクルの地図毎の更新ベクトル （スキャンマッチング結果）
10. 9 GBPによる大域軌跡最適化 SVGDによって現在姿勢を適切に輸送したが，軌跡の一貫性が保てないパーティクルも存在…  iSAM2[Kaess+, 2011] 一般的なグラフ最適化手法 インクリメンタル最適化で小計算量 厳密な２次最適解が得られる（直接法） GPU並列処理のボトルネックがいっぱい存在 パーティクル毎に大域軌跡最適化（グラフ最適化）を実施したい  Gaussian Belief Propagation (GBP) [Ortiz+, 2021] 各変数・ファクタが隣接するノードからのメッセージ （正規分布）の再帰的なやり取りから最適化を行う 同じ処理の繰り返し（反復法）で近似解が得られる GPU並列処理に向いたアルゴリズム SE(3)空間でのGBPをGPUの並列演算能力を最大限活かすよう実装[Murai+, 2024]
11. 10 大域マッチングコスト最小化  ファクタ選定 • 相対姿勢ファクタ 相対姿勢を正規分布近似（固定の正規分布） 誤った推定値が精度悪化の原因に • スキャンマッチングファクタ 相対姿勢を非線形なまま表現（可変の正規分布） 計算コストが高い  実装の工夫 • 大量のファクタの線形化（スキャンマッチング）をGPU並列処理で高速化 • 線形化点からのずれが大きいファクタを優先的に線形化  代表値抽出にファクタグラフの誤差を利用 低確率の姿勢に輸送されたパーティクルも後から復帰可能に
12. 11 動画
13. 12 まとめ わずか1000個のパーティクルでも6自由度モンテカルロSLAMを実現 1. 最新姿勢についてStein Particle Filterを適用 ー 最新姿勢のサンプル多様性を表現，サンプリング効率を向上させた 2. 大域マッチングコスト最小化とGaussian Belief Propagation (GBP) による大域軌跡最適化 ー 各パーティクルで軌跡の一貫性を保ち，サンプリング効率を向上させた 3. 大域マッチングコストにより代表値抽出を安定化 今後の課題：パーティクル数をスケールさせる工夫が必要 根本的に膨大なサンプル数がないと6自由度誘拐などの極度の不確実性に対処できない 理論上，尤度の勾配計算，SVGDの更新，両方にパーティクル𝑂(𝑁2)… カーネル関数を使っているので，遠方をはじいて，𝑂(1)にすることは可能 既存手法との定量的な評価も必要