Fast SLAM with OGM ~ パーティクルフィルタを用いた SLAM （実験編）

確率ロボティクス１３章の内容です．ついにここまできました(*ﾉωﾉ)

目的

前回の Monte Carlo Localization (下記リンクのエントリ) は，環境の地図をすでに持っている前提で，自分が地図のどこにいるか？を求める問題でした．で，ここで一つ疑問が出てきます．
daily-tech.hatenablog.com

「環境の地図をすでに持っているのはええけど，じゃあ，環境の地図はどうやって作るの？」

で，この問題を解くために使う方法論が SLAM になります．「環境の地図を作る」という行為がなんで難しいかというと，

１．正確な地図を作るためには，自分の位置が正確にわかっていないといけない．
２．自分の位置を正確に知るためには，地図がないといけない．

ということで，卵が先か，鶏が先か問題にぶち当たってしまいます．で，いろんな SLAM アルゴリズムがありますが，ベイズの理論をベースとしている SLAM アルゴリズムは結局下記の前提をもとに問題を解決しています．

１．オドメトリは距離の短い二点間の移動に対しては比較的正確だけど，距離が長くなってくると誤差が積みあがって使い物にならない．
２．計測（Lidar）は精度がとても高い．

上記の前提を使って，下記のフローで問題を解決します．

１．地点 x_t-1 -> x_t への移動（比較的短距離）を知るために，オドメトリを使う．
２．オドメトリから求めた x_t の位置は精度がそれほど高くないので，現時点で最新（作りかけ）の MAP と計測を使って x_t を微調整して精度を上げる．
３．微調整した x_t をもとに，MAP を更新し，１に戻る．

オフライン SLAM の場合なんかは，一通りデータを取得した後に最適化計算を回したりするので，またちょっと流れが変わってくると思うのですが，これは２月にやってみます．

実験結果

問題設定のイメージですが，前回 Monte Carlo Localization で使った仮想環境と同じものを使います．上図がSLAMの対象となる環境で，下図が実際にロボットが通った真値のパスです．

Fast SLAM の仮想実験環境

で，この実験の前提としては，事前の地図を持っておらず，ロボットはノイズの乗っているオドメトリ情報と 2D Lidar の計測結果を使ってこの環境の地図を作ります．実験の手順としては，

１．真値のパスに沿ってロボットを走行させます．が，ロボット自身が取得できるオドメトリデータには白色ノイズを載せてます．（＝実際には真値のパスを通っているが，ロボットはその正確な情報を取得できない状況．）
２．0.25mロボットが進む毎にロボットに搭載されている 2D Lidar からのデータを取得できます．（＝2D Lidar の計測値は実際のパスからの計測値．だけど，若干誤差が乗っている．）
３．2D Lidar から取得したデータと，自分の推定位置と作りかけのMAPから得られると想定される 2D Lidar の値を比較します．
４．比較したスコア順にパーティクルに重みをつけ，重み比例する形でパーティクルをリサンプリング．

で，実験結果が下記のようになります．
youtu.be

ちょっと角度のずれが発生してしまって，その影響で地図が一部乱れてしまってます．パラメータをちょっと変えて実行するとうまくいくかもしれなかったんですが，実行にすごく時間がかかるので，あきらめました．．．

理論とか式展開とか．．．．

ほかのエントリと同じく，，，また時間ができたらアップします．ROS でデフォルトの SLAM アルゴリズムとして使われている GMapping はこのFAST Slam に少し改良を加えたものになりますが，本質的なところは同じなので，ここまでくれば GMapping もすんなり理解できると思います．

ソースコード

github.com

上記のリポジトリを落として，

cd tests
python3 fast_slam_1_ogm_example.py

とやれば動きます！こちらも完了するまでには結構時間がかかります．

参考文献

毎度の確率ロボティクスです．

確率ロボティクス (プレミアムブックス版)