GPUを利用した勾配誘導パーティクルフィルタによる 点群６自由度モンテカルロSLAM 中尾拓道*1*2 小出健司*2 高野瀬碧輝*2 大石修士*2 横塚将志*2 伊達央*1 *1筑波大学 *2産総研 

1 森林内でのループクローズ実験 10万パーティクルリアルタイム処理 表示点群：代表値パーティクル 橙点群：現在スキャン点群 

2 エレベータ（誘拐）実験 

3 研究背景 [Koide+, 2024]  グラフベースSLAM 既存の３次元SLAMはほとんど全てこの枠組み 状態の確率分布を正規分布に近似する 点群がリッチな環境なら効率的に高精度な地図生成が可能 [Xu+, 2022] [Koide+, 2024] 

4 研究背景  正規分布近似の問題点 正規分布近似に依存している以上曖昧性の強い状況への対処は困難 曖昧性の強い状況例 • 森林内などの不確実なループ候補が複数存在する状況 • 長期的な点群の縮退 • 誘拐状態 曖昧性の強い状況にどう対処するか？ [Thrun+, 2005] 

5  Rao-Blackwellized Particle Filter (RBPF) SLAM [Griseti+, 2007] ２次元SLAMでは一般的なサンプリングベースSLAM手法 軌跡のサンプルをばらまいて，その中から最適なサンプルを選び出す手法 ノンパラメトリックな分布表現で曖昧性の強い状況にも対処が可能 実時間処理（オンラインSLAM）が可能  ３次元RBPF SLAMの課題 計算量（次元の呪い） 状態の自由度がひとつ増えるだけで必要なパーティクル数が指数関数的に増加 ３自由度から６自由度に メモリ容量 パーティクル数分の地図容量が必要 ３次元点群地図容量（つくばチャレンジでの地図が約500MB） 研究背景 どのように地図表現を軽量にするのか？ どのように計算を高速にするのか？ 

6 提案手法  キーアイディア 1. キーフレームベース地図（メモリ容量問題への提案） パーティクル毎に点群を保持しないため，必要なメモリ容量を大幅に削減 2. 勾配誘導パーティクルフィルタ（計算量問題への提案） 尤度関数の勾配を用いてパーティクルの状態（軌跡）を修正し，サンプリング効率を向上 3. GPUによる並列計算に適したアルゴリズム（計算量問題への提案） 

7 提案手法  1. キーフレームベース地図（メモリ容量問題への提案） パーティクル毎に点群を保持しないため，必要なメモリ容量を大幅に削減 2. 3. 

8 キーフレームベース地図 アイディア：地図はセンサの軌跡と点群があれば表現可能 キーフレームの姿勢のみを各パーティクルで保持し，点群は全パーティクルで共有 

9 提案手法  1. 2. 勾配誘導パーティクルフィルタ（計算量問題への提案） 尤度関数の勾配を用いてパーティクルの状態（軌跡）を修正し，サンプリング効率を向上 3. 

10 問題設定とアルゴリズム概要 現在姿勢 キーフレーム姿勢 パーティクル集合： パーティクル状態： 𝑁個のパーティクル集合により状態の事後確率分布を推定する センサ姿勢系列 地図 出力 LiDAR点群 IMU加速度 IMU角速度 入力 提案システム 予測ステップ 現在姿勢をサンプル 修正ステップ 現在姿勢＆キーフレーム姿勢を修正 代表値抽出 

11 予測ステップ ルーズカップリングLiDAR-IMUオドメトリ[Koide+, 2024] より前回フレームからの相対移動量予測を得る 相対移動量予測から各パーティクルの現在姿勢を更新 現在のオドメトリ軌跡 相対移動量： 共分散行列 前回フレームとのGICPのヘッセ行列から求められる ノイズ項： 現在姿勢： 

12 current frame keyframe 𝑲 keyframe 𝒊 + 𝟐 keyframe 𝐾 − 1 keyframe 𝑖 + 3 keyframe 𝑖 keyframe 𝒊 + 𝟏 ⋯ ⋯ 修正ステップ  尤度計算（現在点群と地図点群とのマッチング） Q．どのように地図とのマッチングを計算するのか？ A．現在位置近傍のキーフレームから局所地図を生成 • 尤度関数 現在フレームと各近傍キーフレーム間でスキャンマッチングコストを対数尤度として計算 対数尤度： 各近傍キーフレームとの対数尤度： スキャンマッチングコスト 

13 修正ステップ  尤度計算（現在点群と地図点群とのマッチング） スキャンマッチングコスト：分布間距離で精度の良いGICP[Segal+, 2009] コストを採用 対応点 現在スキャン点 各近傍キーフレームとの対数尤度： 点間マハラノビス距離 残差（点間距離） 共分散行列 

14 修正ステップ  パーティクル更新則 • リサンプリング 重みの低いパーティクルを高いパーティクルに置き換える サンプリング効率が低い 事後確率の低いパーティクルを保持できない（サンプル枯渇問題[Arulampalam+, 2002] ） • SVGD（変分推論）[Liu+, 2016] （関連研究：Stein Particle Filter[Maken+, 2022] ） 尤度の勾配情報（引力）とパーティクル近接関係情報（斥力）用いて 尤度関数にフィットするようにパーティクルを輸送 サンプリング効率が高い 事後確率の低いパーティクルを保持できる SVGD (64 particles) Resampling (1024 particles) 

15 current frame keyframe 𝑲 keyframe 𝒊 + 𝟐 keyframe 𝐾 − 1 keyframe 𝑖 + 3 keyframe 𝑖 keyframe 𝒊 + 𝟏(o) ⋯ ⋯ 修正ステップ  勾配誘導パーティクル更新則（関連研究：Stein Particle Filter[Maken+, 2022] ，MegaParticles[Koide+, 2024] ） 尤度の勾配情報を用いてパーティクルを尤度の高い状態へ誘導 現在姿勢の更新ベクトル： ガウスニュートン法による尤度関数の二次勾配情報 キーフレーム姿勢の更新ベクトル： k番目のキーフレームまでの経路長 現在フレームまでの経路長 ※大量のパーティクルを並列処理するため，厳密な大域軌跡最適化は難しい • 更新則（斥力の項がないためループクローズ時に限り更新） 従来手法：正しいループ地点にパーティクルが必要 提案手法：正しいループ地点にパーティクルがいなくても正しい軌跡を推定可能 

16 その他の処理  代表値抽出 事後確率（尤度の累積）が最大のパーティクルを代表パーティクルとして抽出 尤度 事前確率  不要なパーティクル除去 数値的に事後確率が０になったパーティクルについてのみリサンプリングする  キーフレームリスト更新 最新キーフレームと現在フレームの点群オーバーラップ率[Koide+, 2024] で更新判定 判定式： 最新キーフレームから現在姿勢へのオドメトリ軌跡 ボクセルマップ 各パーティクルの現在フレームをキーフレームリストに追加 （e.g. 0.7） 

17 提案手法  1. 2. 3. GPUによる並列計算に適したアルゴリズム（計算量問題への提案） 

18 GPUを活かした実装  ハードウェアの処理特性 • CPU：複雑な処理（条件分岐あり）に適している • GPU：単純な処理（条件分岐なし）を大量に繰り返す処理に適している  実装詳細 • GPU ：NVIDIA GeForce RTX 4090 • パーティクル数：10万 • 処理速度 ：約50 ms 観測周期が100 msであれば，十分にリアルタイム処理可能 パーティクル毎に行われる処理はGPUで並列計算し高速化 

19 森林内でのループクローズを模した実験  使用機器 • センサ ： Livox MID-360 • GPU ： NVIDIA GeForce RTX 4090  実験設定 • LiDAR座標系でX座標が負の点を除去（左半分の点群のみを利用） • 雑木林に面した道路でループクローズ 複数のループ候補が存在する環境での提案手法の多峰性表現を確認する  比較手法 最新のグラフベースSLAM手法と比較 • SLICT[Nguyen+, 2023] • DLIOM[Wang+, 2022] • GLIM[Koide+, 2024] 屋外環境 

20 森林内でのループクローズを模した実験 尤度の高い箇所にパーティクルが集合 複数の仮説グループを表現  定量評価 • 提案手法が一番高い結果に • 他のグラフベースSLAM手法は誤ったループ制約を結び精度が悪化 確率分布の多峰性を柔軟に表現 

21 エレベータ（誘拐）実験  実験設定 • エレベータによる移動を含む（1次元の誘拐状態） • エレベータ内を検知しエレベータ動作モデルに切り替える 非常に似通った環境 (c) Convergence at each floor Representative is 2F (d) Particles are distributed (a) Particles move to each floor (b) 1F 2F 3F 柔軟な多峰性表現能力を示し， 従来では困難であった誘拐状態ですら対処が可能に 

22 今後の課題  更なるサンプリング効率の向上 • GPU並列計算に適したより高精度な軌跡の修正手法 • パーティクル更新則に斥力にあたる項の追加  代表値抽出の安定性の向上 6自由度誘拐状態にも対処可能に 

23 まとめ  以下の提案により点群６自由度モンテカルロSLAM (RBPF SLAM) を実時間で実現 • キーフレームベース地図により地図にかかるメモリを大幅に削減 • 勾配誘導パーティクル更新則によりサンプリング効率を向上 • GPUによる並列計算に適したアルゴリズムにより計算速度を向上  グラフベースSLAMに対する以下の優位性を示した • 曖昧性の強い森林内を模した環境において既存手法よりも高い精度で推定 • エレベータでの移動（1自由度の誘拐状態）を経ても正しい階でループクローズに成功 誘拐状態にも対処できる能力を示した