LiDAR SLAMにおける高信頼なループ閉合の実装について 企画討論：信頼できる自動運転のための技術 RSJ2021 OS14 確率ロボティクスとデータ工学ロボティクス　2021年9月10日 https://sites.google.com/view/naokiakaigoo/%E4%BF%A1%E9%A0%BC%E3%81%A7%E3%81%8D%E3%82%8B%E8%87%AA%E5%8B%95%E9%81%8B%E8%BB%A2%E3%81%AE%E3%81%9F%E3%82%81%E3%81%AE%E6%8A%80%E8%A1%93?authuser=0

1. LiDAR SLAMにおける高信頼
なループ閉合の実装について
産業技術総合研究所
横塚 将志
企画討論：信頼できる自動運転のための技術
RSJ2021 OS14 確率ロボティクスとデータ工学ロボティクス
1

2. 2
https://www.youtube.com/watch?v=cDpMtXU6gQU

3. ループの検出と閉合
検出：過去に来た場所を特定
閉合：ループの誤差を最小化
検出を高信頼化すべきか？
閉合を高信頼化すべきか？
検出→学習器(ループ検出器)の改善
閉合→最適化手法の改善
3

4. 失敗のない検出器は存在しない
１００％成功する学習器は作れない
失敗があることを前提に最適化問題を解く→閉合を高信頼化
ロバスト推定：L1最小化、RANSAC、LMedS、M推定…
M推定(＝重み付き最小二乗)を選択
微分可能、実装が容易、近似的に任意のLpノルムが使える
(L2より、L1より、L0と、pが小さいほどロバスト性が高い)
難しいアルゴリズム ≠ 実用的・高性能とはかぎらない
4

5. ループ閉合問題
ループ検出器：ICP（スキャンマッチング）を利用
(ICPは初期値の大きく依存するため、失敗しやすい)
重みの設計 が 高信頼化へのカギ
𝑤 = 1：正しいループ、𝑤 = 0：間違ったループ（0 ≤ 𝑤 ≤ 1）
𝑒𝑖𝑗 = 𝑥𝑖 − 𝑥𝑗 − ∆𝑥𝑖𝑗
キーフレームi,jの
位置の差分
誤差
ループ検出器から
得られた差分
𝑥 = min
𝑥 𝑖,𝑗
𝑤𝑖𝑗 𝑒𝑖𝑗
2
重み
ループ閉合問題（ポーズグラフ最適化）
5

6. 重みの設計
時間的に連続したキーフレーム間 → 𝑤𝑖𝑗 = 1
オドメトリの計算は、小さい誤差はあるが、外れ値はない。
ICPで検出したループ → 𝑤𝑖𝑗 = 1 −
𝑒𝑖𝑗
𝑒𝑖𝑗 +𝜎
大きな誤差を含むループは、
失敗の可能性が高い → 重みを下げる。
ループ検出(ICP)は失敗してもいいので、
多くのキーフレームの組合せで実施
6

7. まとめ
• ループクロージング手法は、様々提案されているが、
実用上は、M推定（重み付き最小化）で十分。
• M推定は、最適化の教科書に必ず乗っている、
基本的なアルゴリズム。
• 問題に応じて必要最小限のアルゴリズムを考えることが重要
7