ICPの試験動画

今回は、本当は、前回実装した電源回路（これ）の試験と、マイコン(SH7125)側のファームウェアの改良をやろうと思っていたが、平日にちょっとだけFlash ROMにプログラムを転送しようと思ったらすぐに転送できなくなってしまった。どうもCPUボードを壊してしまったようだ。新しく実装した電源回路が悪いのか、それ以外なのか不明。しかたがないので秋月でもう一度買った。4800円。ぐた。

仕方がないので今日は、北陽のLRF（レーザーレンジファインダ）Top-URG(UTM-30LX)の試験。
去年、ICPパターンマッチング(これ）を実装したものの精度が出ずに実用に至らなかった。結局のところ、LRFは10Hzでスキャンするのだが、ロボットの移動量からして前フレームと現フレームの差が小さく、また、LRFも誤差10mmほどあるため、併進成分は誤差に埋もれてしまうことと、回転性分も役に立つものの誤差を含んでいるため、それを積算すると累積誤差が溜まるのが問題。
そこで今日は、前フレームとの差ではなく、キーフレームを数十フレームごとに用意してそことの相対にすることでキーフレーム内は誤差が蓄積しないのでないかと思い、実装してみた。つまり下記のような感じ。

キーフレームアルゴリズム（今、命名）
上図の説明。真の位置が毎フレーム１づつ増加とする。LRFの前フレームと現フレームとのマッチングにより、相対位置が求まり、それが誤差を含むため1.1であったとする。すると自己位置推定は、累積誤差のため10フレーム後には1の誤差となる（図の上側）。
キーフレームアルゴリズムは、毎フレーム、前フレームと現フレームのマッチングをするのではなく、最後のキーフレームと現フレームのマッチングより、対キーフレームの相対位置を求める。同じキーフレームの間は誤差が蓄積しない。ただキーフレーム間隔が長すぎると景色が変わりすぎてしまっていて、マッチングしなくなるため、適当な調整が必要と思われる（ロータリーエンコーダなどで進んだ距離とか回転した量をみてキーフレームを決めるなど）。
で、実装してみたところ、失敗でした。
マッチング精度が悪化して、トータルでも悪化でした。
バグなのか、マッチング精度を上げれば良い結果になるのかまだ不明です。

キーフレームアルゴリズムは失敗だったので動画に取っていませんが、ICPマッチングなしのLRFの可視化と、ICPマッチングあり（毎フレームアルゴリズム）の動画を取りました。

試験方法は、ロボットのロータリーエンコーダをOFFにして、LRFだけをログに取り、ICPマッチング計算をして、自己姿勢推定という感じです。去年から改善されていないため、併進成分はとれていません。
回転成分は推定できていますが、ロータリエンコーダの方がまだ精度がよい感じです。

環境地図作成まではまだ遠い。