SLAMはどこまで解決されたのか？

ROBOMECH2026 ワークショップ　SLAMの現在地：　到達点・課題・展望
2026年6月29日

Transcript

1. SLAMはどこまで解決されたのか？ 2026年 6月29日 千葉工業大学 未来ロボット技術研究センター 友納正裕 ROBOMECH2026 ワークショップ SLAMの現在地： 到達点・課題・展望
2. 2 自己紹介 • ～1999 NEC勤務 • 1999～2002 筑波大学 油田研究室（ロボ研） 博士課程 • 2002～2006 JSTさきがけ研究者 • 2006～2008 東洋大学准教授 • 2008～ 千葉工業大学 未来ロボット技術研究センター 主席研究員・副所長 経歴 ロボットの研究はここから • 移動ロボットのナビゲーションとそのための環境認識 昔は、物体認識や地図構築の研究は少なかった よそ者、ばか者、若者 研究分野 当時の私
3. 3 SLAMのざっくりとした歴史 SLAM： Simultaneous Localization and Mapping • 1986年～ Smith and Cheeseman が基本概念を提唱 • 1990年～ Leonard and Durrant-Whyte がカルマンフィルタで理論整備 • 2000年～ Burgard, Fox and Thrun が ベイズフィルタやグラフ最適化で発展 • 2010年～ ポーズグラフ最適化、3D-SLAM、Visual SLAM • 2020年～ IMU融合、深層学習SLAM 昔は、地図を手作業で作っていた。 研究室の模様替え → 地図作成（一大行事） Lidarの登場で SLAMブーム いい加減な地図で ナビゲーション 当時の私の研究テーマ
4. 4 ロボットの地図は進化している 1990年代 超音波センサ、ランドマーク地図 2000年代 2D-Lidar、2D点群地図 2010年代～ 3D-Lidar、3D点群地図 数点のランドマーク地図から数千万点の3D点群地図へ これも捨てがたい シンプルは善 当時はかっこよかった 今やこれが当たり前
5. 5 地図はとても重要 ナビゲーションのほぼすべてで地図を必要とする • 自己位置推定 ： 初期位置あり（逐次推定）、初期位置なし（大域推定） • 経路計画 ： 目的地設定、既知障害物 （事前地図） • 障害物回避 ： 未知障害物、移動障害物 （オンライン地図） • 走行領域認識 ： 地形、材質・地質、交通法規、マナー • 場所認識 ： 目的地判定、場所ごとの行動選択
6. 6 SLAMとは何だったか？ 地図構築のジレンマ （by Sebastian Thrun） • センサはロボットに載っていて、ロボットは移動しながらセンサデータを撮る • センサデータをつなぎ合わせて地図を作るには、ロボットの位置が必要 • しかし、地図がないとロボットの位置を決められない ロボット位置に沿って並べる 全データを原点に並べる
7. 7 SLAMとは何だったか？ 考え方はシンプル • ロボット位置とランドマーク位置の連立方程式 （Simultaneous equations） • 冗長な方程式になるので、最小二乗法で解く （最適化アプローチ） ロボット位置と地図の同時推定が必要 x1 q1 z1 x2 x3 z5 z6 q3 q2 z3 z4 q4 z2 a1 a2 𝑞1 = 𝑥1 + 𝑧1 𝑞2 = 𝑥1 + 𝑧2 𝑥2 = 𝑥1 + 𝑎1 𝑥2 = 𝑞2 − 𝑧3 𝑞3 = 𝑥2 + 𝑧4 𝑥3 = 𝑥2 + 𝑎2 𝑥3 = 𝑞3 − 𝑧5 𝑞4 = 𝑥3 + 𝑧6
8. 8 8 𝐺 = ෍ 𝑡 (𝑥𝑡 − 𝑔(𝑥𝑡−1 , 𝑎𝑡 ))𝑇Ω𝑡 −1(𝑥𝑡 − 𝑔 𝑥𝑡−1 , 𝑎𝑡 )+ ෍ 𝑡 ෍ 𝑖 (𝑧𝑡 𝑖 − ℎ(𝑥𝑡 , 𝑞𝑗𝑖))𝑇Σ𝑖,𝑡 −1(𝑧𝑡 𝑖 − ℎ(𝑥𝑡 , 𝑞𝑗𝑖)) 負の対数をとる 完全SLAM 𝑝(𝑥0:𝑡 , 𝑚 𝑧1:𝑡 , 𝑎1:𝑡 , 𝑐1:𝑡 = 𝜂𝑝(𝑥0 ) ς𝑡 𝑝 𝑥𝑡 𝑥𝑡−1 , 𝑎𝑡 ς𝑖 𝑝(𝑧𝑡 𝑖|𝑥𝑡 , 𝑚, 𝑐𝑡 𝑖) [Thrun, Burgard & Fox 2005] 最適化アプローチ • 運動予測とランドマーク計測の確率モデルを作る • 運動モデルと計測モデルからベイズ推定を構成 • 各モデルに正規分布を仮定し、最小二乗法に帰着 （ガウス-ニュートン法 ≈ 繰り返しEKF） • 全データが揃った後に一括処理： 適切な初期値が必要
9. 9 9 𝐺𝑡 = (𝑥𝑡 − 𝑔(𝑥𝑡−1 , 𝑎𝑡 ))𝑇Ω𝑡 −1(𝑥𝑡 − 𝑔 𝑥𝑡−1 , 𝑎𝑡 )+ ෍ 𝑖 (𝑧𝑡 𝑖 − ℎ(𝑥𝑡 , 𝑞𝑗𝑖))𝑇Σ𝑖,𝑡 −1(𝑧𝑡 𝑖 − ℎ(𝑥𝑡 , 𝑞𝑗𝑖)) 逐次SLAM （ ≈ オンラインSLAM） 最適化アプローチ • 各時刻のデータから逐次的に地図を作る： 完全SLAMの初期値になる • 時刻 t-1の地図にスキャンをマッチングさせて、時刻 t のロボット位置を推定する • ロボット位置に沿って、スキャンを配置して地図に加える
10. 10 10 ロボット位置と地図の分離 最適化アプローチ • ランドマーク位置は、ロボットの軌跡が決まれば互いに独立（条件独立性） • ロボットの軌跡を求めた後、ランドマーク位置を個別に求める • これにより、一度に最適化する変数の個数を大幅に減らせる 𝑝 𝑥1:𝑡 , 𝑚 𝑧1:𝑡 , 𝑎1:𝑡 ) = 𝑝 𝑥1:𝑡 𝑧1:𝑡 , 𝑎1:𝑡 ) ෑ 𝑗 𝑝(𝑞𝑗 | 𝑥1:𝑡 , 𝑧1:𝑡 ) 軌跡 ランドマーク 𝑝 𝑥𝑡 , 𝑚𝑡 𝑧1:𝑡 , 𝑎1:𝑡 ) ≈ 𝑝 𝑥𝑡 𝑧𝑡 , 𝑎𝑡 , 𝑚𝑡−1 ) ෑ 𝑗 𝑝(𝑞𝑗 | 𝑥𝑡 , 𝑧𝑡 , 𝑚𝑡−1 ) 位置 ランドマーク 完全SLAM 逐次SLAM 𝑚𝑡−1 は定数
11. 11 SLAMは解決されたか？ SLAMの課題 （本講演ではLidar SLAMに限定） 課題 内容 解法 同時推定 ロボット位置とランドマーク位置の同時推定 完全SLAM、逐次SLAM 計測誤差 センサデータのノイズ、ドリフト ベイズ推定、最小二乗法 スケーラビリティ 大規模空間の膨大なセンサデータ、処理時間 ロボット位置と地図の分離 データ対応づけ センサデータとランドマークをどう対応づけるか ？ ロバスト性 センサデータの外れ値、歪み、速い動き、環境変化 ？ 退化 位置合せのためのセンサデータが不足 ？ ループ閉じ込み 推定誤差の累積によって周回路で地図がずれる ？ キッドナップ ロボットを強制移動 （大域自己位置推定） ？
12. 12 データ対応づけ SLAM = 位置の最適化（連続）＋ 対応づけ最適化（離散） • 何も制約がないと計算量が膨大 • 地形・物体は変形しない → 対応づけはロボット位置でほぼ決まる （最近傍探索の場合） 解法： スキャンマッチング • 外れ値（誤対応）の除去： M推定 • IMUによるロボット位置の予測 センサデータとランドマークをどう対応づけるか 非常に有効 ランドマーク t-1 スキャン t ロボット位置 t-1
13. 13 LIOはなぜ精度が高いのか？ IMUによるスキャンマッチングの初期値設定 • スキャンマッチングには局所解がつきもの： 初期値が離れているほど局所解のリスクが高い • IMUにより初期値が最適値のすぐ近くに得られる： 局所解に陥る確率が減る Lidar Inertial Odometry （LIO） の高精度には何が効いているか？ （例） スキャン10,000個で地図構築 （A） 初期値＝前回位置： 局所解に陥る確率 1/100 scan 100個のスキャンで大ずれ （B） 初期値＝IMU予測値： 局所解に陥る確率 1/10000 scan 1個のスキャンで小ずれ
14. 14 ロバスト性 ロボットの速い動き • マッチング初期値が遠く離れる（特に回転） • スキャンデータが大きく歪む 思い切り振り回しても、破綻しなかった 解法 • IMUで初期値を予測： 回転に有効、並進もＯＫ • スキャンの歪みも修正
15. 15 ロバスト性 環境変化 （自己位置推定、地図更新） • 静止物体の増減、移動物体の存在 • ランドマークの増減により、データ対応づけに影響 自己位置推定： 既存地図と現在スキャンのマッチング 逐次SLAM： 現在地図と現在スキャンのマッチング 環境変化に弱い ランドマーク増減はほぼない 解法 • 外れ値の除去： M推定 • 自己位置推定と逐次SLAMの同時実行
16. 16 退化 ヘッセ行列のランク落ち 退化 （degeneracy）： ランドマーク不足で位置決めができない G 𝒙𝑘 = ෍ 𝑗=1 𝑚 ( 𝒛𝑗 + 𝐻𝑗 𝒙𝑘 ))𝑇𝑄𝑗 −1(𝒛𝑗 + 𝐻𝑗 𝒙𝑘 ) • スキャンマッチングの最適化計算において、ヘッセ行列がランク落ちして解が不定になる 解法 • データ融合： 車輪オドメトリ、脚オドメトリ、IMU、GNSS、カメラ、レーザ輝度情報 • 潜在情報の利用： 振動情報、速度指令、擬似計測（変数制約） 𝑥𝑘 = − ෍ 𝑗=1 𝑚 𝐻𝑗 𝑇𝑄𝑗 −1𝐻𝑗 −1 ෍ 𝑗=1 𝑚 𝐻𝑗 𝑇𝑄𝑗 −1𝑧𝑗 ヘッセ行列
17. 17 退化 有望な解法 データ融合も完璧ではない • FMCW-Lidar （周波数変調連続波Lidar）： ドップラー効果で相対速度を計測可能 • 地面さえあれば、自分の速度を知ることができる • ただし、退化時はオドメトリとして使うので、どこかでランドマークによる位置修正は必要 • 車輪オドメトリ・脚オドメトリ： 車輪や脚がないとダメ。人の手持ちLidarは？ • IMU： 並進推定が弱い。回転もバイアス補正が必要 • カメラ・輝度情報： テクスチャが必要 • GNSS： 衛星電波が悪い環境（高層ビル、森林、トンネル、地下、屋内）に弱い
18. 18 ループ閉じ込み 解法 ループを手がかりに地図の広域誤差を削減 • 軌跡の修正： ポーズグラフ内の相対姿勢制約の残差を最小化 （ポーズ調整） • ループ検出： 歪み小 → スキャンマッチング、 歪み大 → 大域自己位置推定 • ループ検出： 同じ場所に戻ったことを検出 • 軌跡の修正： 各点の位置誤差を最小化するように軌跡を調整 • 地図の修正： 修正軌跡に沿ってマップ点を再配置
19. 19 キッドナッピング 解法 大域自己位置推定： 初期位置なしで自己位置推定する • 特徴空間探索： 点群の特徴量、画像特徴量で場所検索 • 位置空間探索： 分枝限定法、パーティクルフィルタ • センサ融合： GNSS、カメラ • ロボット誘拐問題： ロボットのセンサを無効にして別の場所に移す • 広大なループ検出や地図結合でも生じ得る 万能策はない 問題設定が重要
20. 20 SLAMは解決されたか？ SLAMの課題 （本講演ではLidar SLAMに限定） 課題 内容 解法 同時推定 ロボット位置とランドマーク位置の同時推定 完全SLAM、逐次SLAM ノイズ センサデータのノイズ、ドリフト、歪み ベイズ推定、最小二乗法 スケーラビリティ 大規模空間の膨大なセンサデータ、処理時間 ロボット位置と地図の分離 データ対応づけ センサデータとランドマークをどう対応づけるか IMUによる予測、M推定 ロバスト性 センサデータの外れ値、速い動き、環境変化 IMUによる予測、LIO+LOC 退化 位置合せのためのセンサデータが不足 データ融合、FMCW-Lidar? ループ閉じ込み 推定誤差の累積によって周回路で地図がずれる ポーズ調整、？ キッドナップ ロボットを強制移動 （大域自己位置推定） ？
21. 21 SLAMは解決されたか？ 地図構築全般の課題はまだまだある 課題 内容 解法 移動物体 地図に広範囲なノイズを残す ？ 地図更新 環境の時間変化を地図に反映 ？ 地図結合 地図領域を拡張する、複数ロボットの地図を結合 ？ 探査と地図構築 未知環境でロボットが自律的にデータ収集して地図を作る ？ セマンティクス 場所や物体の認識、データのセグメンテーション ？ 作成管理コスト 地図の軽量化、トポロジカルマップ、経路グラフ ？
22. 22 移動物体の検出・除去 移動物体の検出・除去 • 移動物体は地図上で広範囲なノイズとして残り、経路計画において障害物となる • 移動物体の点群を地図から取り除く： 物体認識、運動検出、速度検出 レイキャスティングによる運動検出 • 原理： Lidarのレーザ光（レイ）が通過した区画（セル）には物体はなく、反射したセルに物体がある • 複数のレイからセルの占有確率を計算し、占有確率が小さいセルを移動物体とみなす [友納:ロボシン2021] Lidar p1 p3 レイ 占有 p2 非占有 未観測 [Hara: AR2020]
23. 23 処理例 3D地図： 静止物体＋移動物体 3D地図： 静止物体 3D地図： 移動物体 追跡結果（スキャン点）
24. 24 地図結合 部分地図を結合して全体地図を作る • カメラSLAMによる部分地図生成 • 大域自己位置推定 ＋ グループマッチング • ループ整合性 ＋ ポーズ調整 データセット 大学校舎ビル [Tomono： IROS2013]
25. 25 地図結合 全体地図 部分地図 実験結果 7601フレーム 1498キーフレーム
26. 26 セマンティクス ロボットの動作の高度化 地図に意味を付与： 形状、位置、属性 • 物体： 机、椅子、ドア、冷蔵庫、自動車、バイク、人間、・・・ • 場所： 玄関、部屋、廊下、道路、歩道、階段、ホール、畑、公園、・・・ • 走行可能性の判断： 車道、歩道、段差、スロープ、水たまり、芝生、砂利道 • 適切な場所の判断： 玄関から出入りする、階段で上がる、横断歩道を渡る • 物体操作の判断： 荷物を持って運搬する、ドアを開ける、自動車を警戒する ロボットの自律化に必須の機能
27. 27 27 例： 物体地図 椅子A：９ 椅子B：１ ケース：１ 茶箪笥：１ 机：９ モニタ：７ キャビネット：２ 本棚：２ 流し台：１ ホワイトボード：１ 小型ロボット：２ 対象物体：11種36個 画像からの物体地図の生成 • 地図画像から3D地図を生成 • 背景の異なる物体画像から 共起エッジ点を抽出 [Tomono： ISVC2009]
28. 28 28 地図から物体の抽出 • 地図画像から3D地図を生成 • 地図画像と背景の異なる画像から共起エッジ点を抽出 物体画像 ３D地図 投影マッチング ３D物体モデル SIFTマッチング 地図画像 共起エッジ点 [Tomono： IROS2008]
29. 29 SLAMは解決されたか？ 課題 内容 解法 移動物体 地図に広範囲なノイズを残す 物体・運動・速度検出 地図更新 環境の時間変化を地図に反映 いろいろ 地図結合 地図領域を拡張する、複数ロボットの地図を結合 いろいろ 探査と地図構築 未知環境でロボットが自律的にデータ収集して地図を作る いろいろ セマンティクス 場所や物体の認識・セグメンテーション いろいろ 作成管理コスト トポロジカルマップ いろいろ 地図構築全般の課題はまだまだある
30. 30 まとめ 未解決問題 SLAMの進化 • SLAM： 退化、大域自己位置推定（ループ検出、キッドナップ、地図結合） • 地図構築全般： 広く果てしない • 課題は世代依存： 応用や目標次第でさまざまな課題が生まれる • センサの役割は大きい： ソナー、Lidar、カメラ、IMU、・・・ • 理論・アルゴリズムの整備も重要： フィルタ、ベイズ推定、最適化、リー代数、・・・ よそ者、ばか者、若者 期待