ROSとGazeboによる自律移動シミュレーション 〜SLAMアルゴリズムの比較〜

# はじめに

**■ROSとGazeboによる自律移動シミュレーション**
[1.モデル作成](https://qiita.com/Crafty_as_a_Fox/items/1f1db7f4ed042ac300b4)
2.SLAMアルゴリズムの比較　←　今ココ

前回、モデル作成が終わったので次はSLAMを構築して地図生成と自己位置推定を行えるようにしたいと思います。
SLAMを構築する場合、全てを１からコーディングしていこうとすると、規模が大きいので莫大な時間と労力が必要になります。
そこで、SLAMを構築する場合、SLAMライブラリを用いるのが一般的です。
世の中には数多くのSLAMライブラリがあり、色んな大学、研究機関でSLAMライブラリの開発・公開が行われています。

- [ROSのLidarSLAMまとめ](https://qiita.com/nnn112358/items/814c0fb0d2075eb71da0)
  - Gmapping（Particle Filterベース）
  - Hector SLAM（拡張カルマンフィルタベース）
  - Karto SLAM（Graph-based SLAMベース）
  - Google Cartographer（Graph-based SLAMベース）
  - Ethzasl icp mapping（ICP-based SLAMベース）
- [TUMのRGB-D SLAMについてまとめる](https://qiita.com/SatoshiGachiFujimoto/items/1e4cf330b92df43afafe)
  - RGB-D SLAM（visual SLAMベース）
- [ROSのLSD-SLAMを動かしてみた。](https://qiita.com/nnn112358/items/815c90703244d2d00d4c)
  - LSD-SLAM（単眼 visual SLAMベース）
- [ROSのORB_SLAM2を動かしてみた。](https://qiita.com/nnn112358/items/1087d5b0e4df5367f48b)
  - ORB-SLAM（単眼カメラ、ステレオカメラ・RGB-Dカメラ visual SLAMベース）
- [PTAMを15分でなんとなく理解する](http://render.s73.xrea.com/pipe_render/2009/03/ptam-1.html)
  - PTAM（visual SLAMベース）

ホントいっぱいあります。どれを使おうか迷います。

先駆者の中にはそれぞれの手法を比較されている方もいて非常に参考になりました。
[![Roomblock: Mapping Experimets using ROS packages](https://farm5.staticflickr.com/4637/25088659838_f5b2e59aeb.jpg)](https://www.youtube.com/watch?v=dd_BNo22qxM&t=3s&index=16&list=WL)

上記の動画では**Gmapping**、**Karto SLAM**、**Hector SLAM**、**Google Cartographer**の地図生成と自己位置推定結果を比較しています。

**※※※　注意　※※※**
上記のアルゴリズムは全てデフォルトパラメータで動かしているらしく、その状況に適したチューニングを行っていないとのことなので参考程度に！

## Gmapping
- **ベースアルゴリズム**：Particle Filter
- **作成者**：フライブルク大学, サピエンツァ大学,チューリッヒ工科大学
- **loop closure**：有り
- **DOF**：3DOF(Odom+Lidar)
- **参考論文**
 - Implementation of Odometry with EKF for Localization of Hector SLAM Method[^1]

**※参考** 
・[OpenSLAM](http://www.openslam.org/gmapping)：　オープンSLAMとして公開されています。
・[ROS Wiki - gmapping](http://wiki.ros.org/gmapping)：　ROSパーケージの説明
・[ROS navigationパッケージを使ってみよう](https://at-wat.github.io/ROS-quick-start-up/ros-navigation.html)：　環境構築方法について書かれています。体験的に使ってみるときに参考になりそう。

**＜コメント＞**
通称**FastSLAM**と呼ばれる初期型の完成されたSLAMアルゴリズム。スケーラビリティがなく、特別な用途でのみ使用されていたEKF-SLAMを、格子地図を用いて改良されたアルゴリズムです[^2]。4つのアルゴリズムの中では最も知名度が高いのではないでしょうか。ネット上でも比較的多くの情報が見つかります。
比較動画でも安定して地図が生成できていました。ただ、ループして元の位置に戻ってきたときに誤差の蓄積のためか、位置がずれてしまいました。しかし、ループ検出が成功したようで上手く補正できています。

## Karto SLAM
- **ベースアルゴリズム**：Graph-based SLAM
- **製作元**：[SRI International](https://www.kartorobotics.com/)
- **loop closure**：有り
- **DOF**：3DOF(Odom+Lidar)

**※参考** 
・[ROS Wiki - karto_slam](http://wiki.ros.org/slam_karto)：　ROSパーケージの説明

**＜コメント＞**
パーティクルフィルタベースのGmappingや拡張カルマンフィルタベースのHector SLAMはベイズフィルタに基づく手法であるのに対し、グラフベースのKarto SLAMは最適化に基づく手法です。グラフベースのSLAMは大規模な地図構築に適しているらしいのですが、比較動画では最後のループ・クローズに失敗しています。チューニング次第ではループ・クローズに成功するのかもしれません。

## Hector SLAM
- **ベースアルゴリズム**：Scan Matching and EKF(Extended Kalman Filter)
- **製作元**：ダルムシュタット工科大学
- **loop closure**：無し
- **DOF**：3DOF(Lidar) 6DOF(IMU+Lidar)
- **参考論文**
 - Implementation of Odometry with EKF for Localization of Hector SLAM Method[^3]

**※参考** 
・[ROS Wiki - hector_slam](http://wiki.ros.org/hector_slam)：　ROSパーケージの説明

**＜コメント＞**
拡張カルマンフィルタベースのアルゴリズムでRoboCup 2011向けに開発されたアルゴリズムらしい。４つのアルゴリズムの中では唯一loop closure機能が無い。比較動画でも２つめの角を曲がってから直進しているはずなのに、マップ上では直進したことになっておらず、お粗末な地図が出来上がってしまった。おそらく一番酷い結果になってしまったのではないでしょうか。但し、これもチューニング次第で改善する可能性もあります。


## Google Cartographer
- **ベースアルゴリズム**：Graph-based SLAM
- **製作元**：Google, Inc.
- **loop closure**：有り
- **DOF**：3DOF(Lidar), 6DOF(IMU+Lidar)
- **参考論文**
 - Real-Time Loop Closure in 2D LIDAR SLAM[^4]

**※参考** 
・[Cartographer ROS Integration](http://google-cartographer-ros.readthedocs.io/en/latest/#)

**＜コメント＞**
GoogleリリースのSLAMライブラリ。Karto SLAMと同様にベースとなっているのはGraph-based SLAMです。室内でのリアルタイム・マッピングを想定して作成されているらしく、リソース面で良好なパフォーマンスを得るためにパーティクルフィルタではなく、グラフベースを採用している[^4]。比較動画でも割と正確に地図が描けており、ループしたときにほぼ正確に元の位置に戻って来れました。４つのSLAMの中では一番正確に地図が描けたのではないでしょうか。

# おわりに
いかがでしたでしょうか？
一概にSLAMといっても種類は豊富で状況や環境、用途によって有利に働くアルゴリズム、不利に働くアルゴリズム等あると思います。
どのアルゴリズムを使おうか検討するのも大事ですが、あれこれ悩み続けてもしょうがないと思うので、ある程度当たりをつけて、まずは実際に自分で動かして見ようと思います。

# 参考文献

[^1]: 友納 正裕：移動ロボットの環境認識 —地図構築と自己位置推定

[^2]: [G. Grisetti, C. Stachniss, W. Burgard, "Improved Techniques for Grid Mapping With Rao-Blackwellized Particle Filters", In IEEE Transactions on Robotics, vol. 23, no. 1, pp. 34-46, 2007.](http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti07tro.pdf)

[^3]: [Kao-Shing Hwang Wei-Cheng Jiang Zuo-Syuan Wang. ‘Implementation of Odometry with EKF for Localization of Hector SLAM Method’. In: IEEE 2016.](http://www.uni-obuda.hu/users/szakala/SMC%202016%20pendrive/1118_smc2016.pdf)

[^4]: [W. Hess, D. Kohler, H. Rapp, and D. Andor, ‘Real-time loop closure in 2D LIDAR SLAM’. In: IEEE International Conference on Robotics and Automation (ICRA). Stockholm, Sweden, May 2016.](https://static.googleusercontent.com/media/research.google.com/ja//pubs/archive/45466.pdf)