真面目にamclの代替ROSパッケージを作った

Wed May 5 09:49:04 JST 2021 (modified: Wed May 5 10:47:35 JST 2021)
views: 5971, keywords:emcl,詳解確率ロボティクスこの記事は最終更新日が2年以上前のものです。

真面目にamclの代替ROSパッケージを作った

　連休中はずっとこれを作ってました。amclを置き換えるための2次元LiDAR用の2次元LiDAR用の自己位置推定パッケージです。とりあえず学内での反応がよさそうなので普及を目指します。

mclに膨張リセットを実装してemclというパッケージを作りました。パーティクルの場所をずらしても少しなら収束します。こっちは実用を目指します。

（たぶんamclより使い勝手は良いはず。）https://t.co/u6MQhjRRuN pic.twitter.com/sxPFCd0IzF
— 上田隆一 (@ryuichiueda) May 4, 2021

なぜわざわざこんなことをしているのか

　amclで済む場合はamclを使えばよいのですが、次の点で書き直してもいいんじゃないかと思いました。

amclはROSのインターフェイス部分がC++、推定ルーチンがCで書かれていて、手を入れようとするとややこしい
amclはロボットが止まっていると推定もほぼ停止
KLDサンプリングがコードの複雑さを10倍くらいにしており、しかも必須ではない
adaptive MCL（しきい値をふたつ使うパーティクルの置き直し方法）も、必須ではない

　特に2については、同じリポジトリに同居している割には、navigationメタパッケージのナビゲーション方法と相性が悪いのが気になってました。というのは、navigationパッケージを使うと、自己位置推定に何かトラブルがあったときにロボットが静止することが多く、この場合にamclも止まってしまうと何も解決しないからです。もちろん、ロボットが静止していてもセンサデータを無理やり取り込むことはできるのですが、それなら最初から全部のセンサデータを使うようにしておいたほうが自然です。

　3、4については、なんとなくデファクトスタンダードになっており、（そしてお二人の開発者ともRoboCupの4足ロボットリーグで一緒で、ちょうどその時期に開発された手法なので私も馴染みが深いのですが）、正直言うとあんまり重要ではありません・・・。たぶん私が英語であることないことをベラベラ話す人間だったら、現在の状況がちょっと変わっていたかもしれません・・・。

どういう実装にしたか

　とりあえず、パーティクルの更新は次のようにしています。

ロボットが動いていようが止まっていようが、一定周期でTFからオドメトリデータを取得。動いていたらパーティクルの位置を更新
センサデータは非同期に取り込んでおいて、オドメトリの処理（静止時の処理を含む）が終わったときに新しいセンサデータが届いていたら、そのデータを使ってパーティクルの重みを更新してリサンプリング
- 重み更新には尤度場を使用（amclはいくつか選べるけど、いまのところ自分の実装ではこの一択）
パーティクルの重みの更新のあと、更新した重みの総和がしきい値を下回ったら膨張リセット

膨張リセットについては、詳解確率ロボティクスに記述してあります。

　ちなみにしっかり排他制御はしていますが、amclのコードではちらほら見かけるミューテックスなど特別な道具は使ってません。オッサンの知恵です。RoboCupで、ほぼ生のC++98でロボット制御と画像処理を書いていた人間をナメてはいけません。生きる化石です。大事にしましょう（？）

　この実装によって、上のTweetの動画みたいに、ロボットが止まっていてもパーティクルが動くパーティクルフィルタになります。弊学科つくばチャレンジチームでも、ロボットを置いたときに初期位置が微調整されないままになっていたみたいなので、それだけでもうれしいそうです。

考えられる問題

　オッサンの経験で、実機でも動くように実装したつもりですが、おそらく実機で使うと細かい（でもロボットが止まってしまうという意味で重大な）問題が出てくると思います。あと、レーザーの距離が大きい/小さい場合の打ち切りのパラメータをまだ設定してませんので、LiDARによっては雑音が多くて困る場合があるかもしれません。それから、センサデータのタイムスタンプも見てないのですが、これはシステムがビジーになったときにちょっと良くないかもしれません。