2019年12月24日火曜日

壁切れ検知手法の紹介

この記事はMicro Mouse Advent Calendar 2019の24日目の記事です。
昨日の記事はVrIyoeさんの「吸引ファンの設計」でした。
私も吸引勢の仲間入りしたいと考えていたのでものすごく役立つ記事でした。
ブックマークに入れて100回くらい読み直したいと思います。



世の中はクリスマスとかいう行事で盛り上がっているようですが、皆様いかがお過ごしでしょうか。

この記事はこのブログ最初の記事なのですが、ブログ自体は2017年に作ったものです。
下書きをみてみると、2017年の全日本大会の結果を報告しようとしている痕跡が見えます。
3日坊主ですら失敗したブログですが、これからなにかネタが見つかったら更新していきたいと思います。

本題ですが、私がやっている壁切れ検知の手法を紹介しようと思います。

ことの発端は私が初めて作ったハーフマウス(今はマイクロマウスですね)で、探索中に壁の切れ目で吸い込まれてしまったことです。
吸い込まれるとは、マウスが前進しているとき、壁の切れ目に差し掛かると、なくなった壁の方向へ曲がっていってしまうという現象です。
マウスは通常、横壁との距離を見ながら自身の姿勢を制御(壁補正)しています。私の場合は、マウスが迷路の中心にいるときの壁との距離をレファレンス値として持っていて、その値と現在の壁との距離の差にPゲインをかけて目標角速度とする制御となっています。
下の動画は実際に壁に吸い込まれる様子です。(わかりやすくするため、通常よりゆっくり走行させています)



クラシックマウスをやっていたときにはこのような吸い込まれるような挙動はなかったのですが、ハーフマウスでこのような挙動が出たのは、以下のような点が原因であると考えています。
  • 慣性モーメントの違い
    クラシックマウスでも同様にセンサ値の上昇がなまっており余計な壁補正がはたらいていたが、実際の角速度が発生する前にしきい値を上回り、見た目上、吸い込まれるような動きがなかった。
  • センサの半値角の違い
    クラシックマウスのセンサはSFH4550(±3°)+ST-1KL3A(±6°)と半値角の小さい組み合わせに対して、ハーフマウスではOSI5FU3A11C(15°)+LTR-4206E(±10°)という半値角の大きい組み合わせであり、壁に投光されるスポット光が大きかった。
  • センサ角度の違い
    クラシックマウスに比べ、壁切れを速く読もうとマウスから遠い位置を見るようにしたためスポット光がマウスの前後方向に伸びた。
上記の原因はすべて推測なのですが、まぁまぁ当たっていると思います。
ハーフサイズで使えるサイズの半値角が小さい素子があれば解決しそうですが、自分が探した限り見つからなかったのと、当時大会まで時間がなかったのでソフトで対処しようと考えました。マウス十則からするとダメなパターン。

以下のグラフは壁補正を切って真っ直ぐ走行させた場合の右側のセンサ値(距離に変換したあとのもの)と壁補正しきい値のグラフです。壁補正しきい値よりセンサ値が小さい場合に壁補正を行います。130~180 msあたりの位置は柱を読んでいるので多少センサ値が上下しています。センサ値は100 mmを上限としています。


理想としては黄色のラインのように、壁がなくなった瞬間にセンサ値が100 mmとなり、壁補正しきい値を上回ってほしいわけです。
しかし、現実には170 msあたりからセンサ値がしきい値を上回る220 msあたりまで壁補正がはたらき、動画のように曲がってしまうわけです。
というわけで、このようなセンサデータから壁切れを検知できれば、それ以降の壁補正をキャンセルすることで吸い込まれないようにすることができるはずです。

最初に、mice Wikiのセンサ値の差分がしきい値以上だったら、壁補正しきい値を引き下げるという方法を試してみました。
下のグラフは現在のセンサ値-前回のセンサ値で差分を取り、上のグラフに追加したものです。
なだらかにセンサ値が上昇している部分では差分値はほとんど0に近く、センサ値が急に上昇する210 ms付近でようやく壁切れ検知のしきい値を超えています。170 ms付近で壁切れ検知となるように壁切れ検知しきい値を設定しようにも、難しいことがわかります。

センサ値を微分した結果にになんらかの係数をかけて目立たせることを考えたのですが、そのままだとノイズも一緒に大きくなってしまうので、脳死で「微分 ノイズ 除去」とかのキーワードで検索した記憶があります。そこで見つけたのが平滑化微分というワードです。こちらの株式会社 常光さんのページがわかりやすいです。
常光さんのページでは前後9点のデータで平滑化微分をしていますが、現在のデータを含めた4点で平滑化微分をしています。壁センサの値をリングバッファに溜めていおいて、以下の式で傾きを出しています。

傾き = (センサ値(0) - センサ値(-3) * 4) + (センサ値(-1) - センサ値(-2) * 1)

この式おけるセンサ値(-i)はi個前のセンサ値を表す。
また、係数の4と1は適当に決めた値。

このように、壁切れ検知しきい値は同じですが、壁切れ検知の位置を手前にすることができました。これくらい手前で壁切れ検知して壁補正しきい値を引き下げることができれば、吸い込まれなくなります。

ここまで書いておいてなんですが、ブログの記事にするにあたってデータを取り直してみたら自分が想像していたよりノイズが残っていますね。しきい値や式の係数がかなり絶妙な気がします。このあたりの値はエクセルでいろいろ数字をいじりながら決めた記憶があります。
ですが、この手法で壁切れ検知を始めてから誤検知などで苦しんだことはないので、私のマウスには(なんとか)適用できている、ということなのだと思います。

というわけで、かなり微妙な結果となってしまいましたが、なにかの参考になれば幸いです。
自分はこうしてるよ!とかこうしたほうがいいよ!とかここおかしいんじゃない?とかありましたら、ぜひとも教えて下さい。ぜひとも。
(来年福井で行われるマウス合宿の発表ネタにどうでしょうか)



さて、明日のアドベントカレンダーは全日本でアニキをぶっ倒して優勝したうめさんです。
私は来年からうめさんと同期になるらしいのですが、会社の人から「彼は全日本優勝したらしいけど君は?」などと言われたらどうしようかと考えると今から心臓がドキドキです。

では、良いお年を。