Monte Carlo Localization ~ パーティクルフィルタを用いた自己位置推定（実験編）

確率ロボティクス８章の内容です．

目的

「こんなこと，何に使うねん？」って話ですが，自律移動のロボットを作るときに，走行する環境の地図をあらかじめ持っている場合が結構あります．（つくばチャレンジ，学習後のお掃除ロボット等）この「あらかじめ持っている地図」を有効に活用して自律移動するためには，「地図の中で，実際に自分がいる位置はどこか？」をある程度正確に求めなくては行けません．この時，タイヤの回転数を積分して自己位置を推定する方法（デッドレコニング）だと誤差が蓄積してしまって走行距離が長くなると全然使い物にならなくなるので，通常は Lidar，カメラ，ソナーなどのセンサーの結果も同時に使用します．ここで，よく似たような話が出てきます．

自己位置を求めるために，
１．デッドレコニング
２．Lidar の入力
の二つがあるけど，どうやって統合する？という， EKF Localization の時にも話したようなよく似たような話が出てきます．今回のアルゴリズム（Monte Carlo Localization）も，デッドレコニングと Lidar のデータの両方を用いて地図の中で自分がどの位置にいるかを推定します．地図に対して自分がどこにいるかわかることで，次に進むべき方向とか位置がはっきりします．

実験結果

問題設定のイメージですが，前回の EKF Localization のエントリのように特徴点ベースの地図だと GMapping とか ROS AMCL と結びつかないので，OGMを使ってみました．自己位置推定するフィールドは仮想的につくったものですが，下記になります．上図が自己位置推定対象となる環境で，下図が実際にロボットが通った真値のパスです．

Monte Carlo Localization の仮想環境

で，この実験の前提としては，上図の地図を予め知っているという前提で，このコースを走行するロボットが自己位置推定をします．仮想的に 2D Lidar を想定して実験をしてみました．実験の手順としては，

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

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

どうも自分の Ray Casting の実装が下手くそなのか，Python だからある程度は仕方ないのか，結構実行に時間がかかります．上記のビデオ，ゴールにたどり着くまでに40分くらい時間がかかってます（笑）．この動画では，１０個のパーティクルを使ってますが，割とうまいこと自己位置推定ができています．下図は，推定した自己位置からスキャン結果を用いて作った OGM になります．

理論とか式典解とか．．．．

EKF Localization の時と同じく，，，また時間ができたらアップします．ただ，Monte Carlo Localization は Fast Slam や GMapping とも関連が非常に強いので，SLAM を勉強する上では必須の内容だと思います．Fast Slam の実験結果を次のエントリで書こうと思っていますが，Monte Carlo Localization ができてしまった後は，意外と簡単に作れました．

ソースコード

いつもの Github においてます．
github.com

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

cd tests
python3 monte_carlo_localization_example.py

とやれば動きます！が時間がかかります（笑）

参考文献

いつものことながら，確率ロボティクスです．

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