自己位置推定（３）

こんにちは、ソフト班Shinpukuです。

前回の続きですね。

検出した白線をパーティクルフィルタに代入する前にオドメトリ情報を代入します。各車輪の速度（v0,v1,v2）はエンコーダから取得できますので、それを使って、


速度、進行方向、角速度を逆算することができます。

また、「ロボットの位置が(X,Y)であるとき、n段m番の白線は○○メートルで××ラジアンと検出される」というデータマップをあらかじめ作っておきます。

あとは検出した白線情報とマップ情報を比較して、差が小さければ高い尤度（もっともらしさ）を与え、パーティクルの重み付き平均をとることで自己位置を求めます。

尤度がある一定の値以上であればパーティクルフィルタ、そうでなければオドメトリというように切り替えて使うことで、ロバストな自己位置推定を行うことが出来ます。

ちなみにHibikino-Musashiプログラムでは、方位センサからの情報を付加して敵・味方コートの判別ができるような作りになっています。