ステレオカメラを用いた自己位置推定
前回の更新からほぼ二か月空いてしまいました．．．結局１０月は一度もブログを更新することなく終えてしまいましたが，今年のつくばチャレンジも終わりました．
結果は別のエントリで書くとして，以前のエントリで自己位置推定のことをちょこっと書いてましたが，ようやく完成して使えるレベルになったので記事に起こしておきます．
思い起こせば，２０１７年は自己位置推定との戦いでした．．
１．SWEEP（格安の2Dレーザスキャナ）を購入して，ROSのAMCLを用いてマップ作製・自己位置推定
―＞屋外使用でスキャナのレンジが３ｍくらいしかなく，撃沈．
２．ステレオの出力を疑似的にレーザースキャンに変更，ROSのAMCLを用いて自己位置推定にトライ
―＞ステレオ視差情報から求まる位置の距離に対する精度劣化が大きく，変換しても使えず．．．
３．ステレオ位置情報から求まる位置情報を路面のグリッドに投影．グリッド毎に代表高さを決めてやり，グリッド間勾配を見て通行可能／不可能のマップを作製．
自動走行時には上記のマップに対してパターンマッチングをして自己位置推定．
―＞この実装でつくばチャレンジに参加．
ということで，３の方法に落ち着きました．結局作りこみが甘く，大清水公園内でのリタイアになってしまいましたが，自己位置推定はできていたように思います．
下記，ステレオデータを使って実際に自己位置推定している様子です．ビデオを早送り編集できなかったので，ちょっと無駄に長い動画になってしまいましたが．．
左側が事前に取得しておいた地図で，右側の小さい正方形がロボットが自走しているときの周辺地図です．この周辺地図を事前に取得した地図にテンプレートマッチング
することで自己位置推定しています．
で，最終的に得られた結果が下記になります．青色の線がデッドレコニングの結果，赤色の点がテンプレートマッチングの結果，黄緑色の線がその二つを拡張カルマンフィルタで
統合した結果です．割といい感じに自己位置推定できていると思います．自画自賛ですが．．．ただ，現状はカルマンフィルタを使って統合しているだけというのもあって，
一度自分の位置を見失うと，復帰できないので，この辺りはパーティクルフィルタとかほかのアルゴリズムを実装してちょっと作りこまないといけないと思います．
つくばチャレンジの結果はまた次のポストで書きます！
