SLAMと地図作成
ロボットが地図を作りながら、その地図の中で自分の位置を求める仕組み。フロントエンドとバックエンド、LiDAR SLAMとビジュアルSLAM、占有格子地図などの地図の種類、ループクロージャ、既存の地図での自己位置推定を扱います。
概要
同時自己位置推定と地図作成（SLAM）は、互いに依存する2つの問題を解きます。地図を作るには、各計測を取得したときにロボットがどこにいたかを知る必要があります。ロボットの位置を知るには地図が必要です。SLAMは外部の測位手段を使わず、ロボット自身のセンサから両方を同時に推定します。
実際に稼働している移動ロボットの多くは、SLAMを一度だけ使います。導入時に地図を作成し、日々の運用ではその地図に対して自己位置推定を行います。未知の場所を探索するロボット、たとえばフィールドロボット、ドローン、一部の脚式ロボットは、SLAMを常時実行します。このハブページは、SLAMシステムを選定・調整するエンジニアや、地図に壁が二重に写ったり廊下が曲がったりする原因を突き止めようとしているエンジニアに向けたものです。
重要な考え方
フロントエンドとバックエンド
Cadenaらによる2016年のサーベイ論文は、現代のSLAMを2つの部分に分けて説明しています。フロントエンドは生データを制約に変換します。LiDARのスキャンを直前のスキャンと照合したり、画像の特徴点をフレーム間で追跡したりして相対運動を求め、どの観測が同じランドマークや場所を指しているかを判断します。バックエンドはそれらの制約を受け取り、制約に最もよく合う軌跡と地図を求めます。通常は、非線形最小二乗法で最適化するポーズグラフとして解きます。これは/topics/state-estimation/で扱うファクターグラフの仕組みそのものです。Grisettiらのチュートリアルは、これをSLAMに当てはめて順を追って解説しています。
この区別が実用上重要なのは、2つの部分の失敗の仕方が異なるからです。フロントエンドはデータが原因で失敗します。特徴の少ない廊下、ガラス、モーションブラー、動く人などです。バックエンドは誤った制約が原因で失敗します。データ対応付けを1つ誤るだけで、地図全体が歪むことがあります。
LiDAR SLAMとビジュアルSLAM
LiDAR SLAMは点群を直接照合します。平らな床の上での2次元LiDAR SLAMは成熟した技術です。Nav2のbringupローンチファイルが地図作成に使うslam_toolboxは、スキャンマッチングからポーズグラフを構築します。3次元では、LOAMがエッジ点と平面点を抽出してスキャンを位置合わせします。KISS-ICPは、単純なpoint-to-point ICPでも丁寧に実装すればオドメトリには十分だと主張しています。LiDARでは実スケールが得られ、暗所でも動作します。一方で、特徴の少ない長い廊下や、どのスキャンも似て見える開けた野外は苦手です。
ビジュアルSLAMは、カメラのフレーム間で特徴点や画素の輝度を追跡します。安価で、場所認識に使える豊富なデータが得られます。ただし、単眼カメラでは実スケールを復元できず、照明の変化や速い動きで追跡が破綻します。慣性計測装置（IMU）を加える（ビジュアル慣性オドメトリ）と、スケールと重力方向が定まり、短時間のデータ欠落も補えます。ORB-SLAM3は単眼・ステレオ・RGB-Dカメラに対応し、IMUの有無を問いません。また複数の地図を保持し、同じ場所を認識するとそれらを統合します。
占有格子地図とその他の地図
占有格子地図はElfes（1989）が提案したもので、空間をセルに分割し、各セルが占有されている確率を保持します。距離計測が1回得られるたびに、ビームが当たったセルの確率を上げ、ビームが通過したセルの確率を下げます。実装では対数オッズを保持するため、更新は加算になります。経路計画器が入力として使うのは格子地図で、3次元版ではボクセルを使います。点群地図は自己位置推定のために生の幾何形状を保持します。疎な特徴地図はランドマークだけを保持するため、コンパクトで再自己位置推定に向いています。ただし、それ単体では経路計画には使えません。
ループクロージャと既知の地図での自己位置推定
オドメトリの誤差（ドリフト）は蓄積します。そのため、建物を一周して出発点に戻ったロボットの推定位置は、出発時の位置と一致しません。ループクロージャは再訪した場所を認識し、2つの姿勢の間に制約を追加します。バックエンドはその補正をループ全体に分配します。正しいループクロージャは地図をまっすぐに直します。一方、見た目の似た2本の廊下を誤って結び付けると、地図が折れ曲がることがあります。そのため各システムは、ループクロージャを受け入れる前に幾何的な整合性を確認し、最適化にはロバストなコスト関数を使います。
地図ができてしまえば、自己位置推定はSLAMより簡単です。Nav2のAMCLは占有格子地図に対して粒子フィルタを実行し、slam_toolboxにも独自の自己位置推定モードがあります。どちらもREP 105に従い、map座標系でのロボットの姿勢を計算して、mapからodomへの補正を配信します。これにより、odom座標系は局所的な制御のために連続したまま保たれます。
どこから始めるか
/learn/beginner/07-differential-drive/ではオドメトリを求め、それがどのようにドリフトするかを示します。/learn/beginner/08-grid-path-planning/では、SLAMが生成するような格子地図の上で経路計画を行います。/learn/beginner/06-sensors-and-noise/では、この問題を難しくしているノイズを説明します。
ツールとプラットフォーム
| ツール | 用途 | ライセンス | プラットフォーム |
|---|---|---|---|
| slam_toolbox | ROS 2での2次元LiDAR SLAM、長期的な地図作成（lifelong mapping）と自己位置推定 | LGPL-2.1 | /platforms/ros/ |
| RTAB-Map | RGB-D・ステレオ・LiDAR向けのグラフSLAM。外観ベースのループクロージャを備える | BSD-3-Clause | /platforms/ros/ |
| ORB-SLAM3 | 特徴点ベースのビジュアルSLAMとビジュアル慣性SLAM。著者らは別途商用ライセンスを提供している | GPL-3.0 | /platforms/opensource/ |
| KISS-ICP | パラメータの少ないLiDARオドメトリ | MIT | /platforms/opensource/ |
| Cartographer | 2次元・3次元SLAM。READMEには、現在は積極的にメンテナンスされていないと記載されている | Apache-2.0 | /platforms/opensource/ |
| Nav2 AMCL | 既知の2次元地図における粒子フィルタによる自己位置推定 | LGPL-2.1-or-later | /platforms/ros/ |
よくある間違い
- オドメトリが不正確なまま地図を作り、SLAMのバックエンドが直してくれると期待すること。先に車輪半径とトレッド幅を較正してください。
- LiDARからベースへの座標変換が1度ずれたまま放置すること。すべてのスキャンが回転し、壁が厚く写ります。
- 人の多い時間帯に地図を作ること。人やパレットが恒久的な壁として地図に残ります。
- すべてのループクロージャを受け入れること。誤った照合が1つあるだけで、1時間かけて作った地図が台無しになることがあります。
- 固定した地図での自己位置推定のほうが挙動を予測しやすいのに、本番運用でSLAMをフルに動かすこと。
さらに深く
- C. Cadena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza, J. Neira, I. Reid and J. J. Leonard, Past, present, and future of simultaneous localization and mapping: toward the robust-perception age, IEEE Transactions on Robotics 32(6), 2016.
- G. Grisetti, R. Kümmerle, C. Stachniss and W. Burgard, A tutorial on graph-based SLAM, IEEE Intelligent Transportation Systems Magazine 2(4), 2010.
- J. Zhang and S. Singh, LOAM: lidar odometry and mapping in real-time, Robotics: Science and Systems X, 2014.
- C. Campos, R. Elvira, J. J. Gómez Rodríguez, J. M. M. Montiel and J. D. Tardós, ORB-SLAM3: an accurate open-source library for visual, visual-inertial and multimap SLAM, IEEE Transactions on Robotics 37(6), 2021.
- W. Hess, D. Kohler, H. Rapp and D. Andor, Real-time loop closure in 2D LIDAR SLAM, ICRA 2016. Cartographerのドキュメントが、2次元の手法の出典として引用している論文です。
- S. Macenski and I. Jambrecic, SLAM Toolbox: SLAM for the dynamic world, Journal of Open Source Software 6(61), 2021.
- Sebastian Thrun, Wolfram Burgard and Dieter Fox, Probabilistic Robotics, MIT Press, 2005.