クラシックマウスを作ろう！（10）〜IMUとエンコーダ〜
初めましての方は初めまして、
そうでない方はお疲れ様です。
なんだかんだで10本目です。
概要しか攫っていないのに全然おわらない…
字数としてはそこまで多くないはずですが、同人誌とか書く方々スゴイです…
今回はDCマウスは勿論、多くのロボットで採用されるセンサ、IMUとエンコーダについてです。
どうせなのでマウスに限らない範囲で考えてみましょう。他のロボットを知る上でも基礎となる部分ですので。
もくじはコチラ。
自己位置推定
ロボットをロボットたらしめる重要な技術として自己位置推定があげられます。これはまさに文字通りで
「センサの情報からロボット自身がロボットの位置や姿勢を推定する事」
です。
人間で言えば、立っているのか座っているのか、自分がどの位置にいるのかを「常に」「推測」しています。
（視覚や聴覚は感覚器官を用いたセンサなので、人間はあくまで「正確な推測」としてしか世界を観測できない）
この自己位置推定だけで辞書ぐらいの本が出ているので、読んでみるといいかもです。
統計数学も出てくる洋書ですが日本語訳や諸々もあります。
でも自己位置推定って、突き詰めると深いだけで原理としてはそんなに難しくないです。
例えば、ルンバをイメージしてください。
壁にぶつかった時にタクトスイッチが押される機構だったとします。
スイッチが押されている時、ルンバ自身は「壁に衝突する位置にいる」と推定するわけです。
はい、自己位置推定です笑。
勿論、スイッチが人間や猫によって押されている可能性はありますがあくまで「推定」なのでヨシ。
センサの情報から自分の位置や状態を予測するって、難しそうですが、そもそもセンサってその為につけるので当たり前、って認識になってもらえればと思います。
LiDARやステレオカメラ、カルマンフィルタを使わないから自己位置推定ではない、なんて言わないでください…。
オドメトリ（デッドレコニング）
はてさて、そんな自己位置推定ですがまずは人間をベースに考えてみます。
人間が自分の位置を簡単に知るためにはどんな方法があるでしょうか？
イメージとしては過去の移動を覚えて逆算すればスタートからどこまで離れた場所に来たか分かりますね。
例で考えると
移動が右に3歩、上に2歩だったとします。
1歩が1と仮定し、スタート地点を原点とすると移動後の座標はx-y座標で（3,2）と表せます。
これを無限時間で積算していけば理論上は常に現在の位置が分かるはずです。
この様に
「移動情報を積算して位置を推定する方法」
はオドメトリ（デッドレコニング）と呼ばれます。
正式名称はデッドレコニングのはずですが巷ではオドメトリの方がメジャーな気がしてます…。
リンク :
デッドレコニング [JSME Mechanical Engineering Dictionary]
移動情報のセンシングには車輪の回転数を測るエンコーダ、又は加速度や角速度を取得するIMUが頻繁に用いられます。
大きな物かつ屋外環境ではGPS情報を利用することも多いです。
物体の移動は並進移動と回転移動に分割できるので、理論上はIMUさえあれば充分です。しかし、加速度だと速度を計算するために積分が必要、それによる誤差が懸念されるため「エンコーダとIMU」の組み合わせがDCマウスではよく用いられます。
この２つのセンサを使うと「極座標」で機体の位置を表現することができます。取得したログの解析などをする時に役立ちます。ただ座標計算だと三角関数が出てくるのでリアルタイムで処理する場合は工夫がいるかもしれません。
マイコンの性能と周波数次第ですね。（大体、大丈夫な気がするけども…）
インクリメンタルエンコーダ
さて、そんなわけでエンコーダを使う！ってなるならばエンコーダについて考えてみましょう。
エンコーダは主に2種類あります。
- アブソリュート
- インクリメンタル
違いとしては以下のリンクが正確に述べているかと思います。
ロータリエンコーダのインクリメンタル形とアブソリュート形の違いを教えてください。 - 製品に関するFAQ | オムロン制御機器
1番納得できた端的な説明だと
アブソリュートは電源が切れても角度が分かるだけ
インクリメンタルは角速度が分かるだけ
ぐらいです。
ただ、どちらのエンコーダを使ったマウスも存在しています。
このシリーズでは1717を使用するので、モータ付属のエンコーダについて考えましょう。
はい、光学式インクリメンタル型です。
原理的には次のリンクが分かりやすいです。
リンク：計装豆知識｜ロータリエンコーダ
STM32でのエンコーダ読込実装に関しては次のリンクを参考に。
STM32 ＋HALでエンコーダモードを使用してみる | Sora's Activity Record
インクリメンタルエンコーダは回転するとパルスが送られてきます。
1回転で送られるパルスの総数が分解能です。マイコンだとこのパルスの立ち上がりと立ち下がりを別に検出することで同じエンコーダでも４倍の分解能を得る音ができます。
例えば分解能が100だとすると、1回転で100パルスなので1パルスだと3.6[deg]に値します。マイコンでこれを4逓倍すると0.9[deg]ごとに角度変化を知ることができます。これを1[kHz]、つまり0.001秒毎に取得して積算すると角度が得られます。
この分解能についても、最高回転数の時にサンプリングレート内で読み落としが発生するかどうかで最低ラインを理論計算することはできます。（とりあえず細かければええやろ、で作ったと言えない）
読み落としは回路時定数の問題で発生することもあります。エンコーダパルスの波形が鈍っていたりするのでオシロスコープを当てると見えます。この場合は素子よりも回路自体が要らぬ導通をしている可能性が高いです。
そんなわけで、走行距離、極座標で言うところのrは測定できるはずですね。
IMU
では極座標のθをセンシングしたい欲求が生まれます。
角度を直接取得するのは厳しいので角速度取得から積分（足し算）して角度を求めるのがよくある方法です。
そんなわけで角速度取得センサとしてメジャーな慣性測定機器（イナーシャメジャーメントユニット）、通称IMUの登場というわけです。
俗にはジャイロセンサと呼ばれる類のもので、有名なのはMPU6500とかMPU9250、BNO055とかです。
何故、慣性センサかというと、文字通り慣性力を計測して電気信号にしてるからですね。
- 並進運動の慣性力から加速度
- 回転運動の慣性力から角速度
が分かります。
原理は運動方程式なのでそこまで複雑ではないですが、自分で実装するのは辛そうですね。
リンク
慣性センサの基礎知識 ～ジャイロセンサ、加速度センサ～ - 電子デバイス・産業用機器 - Panasonic
慣性計測なので多くの高性能IMUは角速度と加速度、それぞれ3軸ずつ取得できます。
それに地磁気センサ3軸をプラスしたのが9軸ジャイロとか呼ばれてるやつですね。
実際に使う場合にはSPI通信やI2C通信でIMU内のレジスタを書き込んだり、読み込んだりなのでデータシートと睨めっこが続きます。
参考
STM32＋HALでMPU6500実装：
STM32 + HALのSPIでMPU-6500と通信する | Sora's Activity Record
同様にMPU9250での実装：
Advent calendar revenge match in2020 (HAL+MPU9250) - judgeのブログ
しかしInvensane社のMPUシリーズは廃盤・新規設計非推奨などが続いており、ICMシリーズが強く推奨されています。このため、ブログやライブラリなどの情報が少ないチップを使うことが増えるかと思います。
（データシートとお友達になる良い機会かと思います）
チップの選定基準は様々ですが、自分はQFNパッケージであることを大事にしています。性能に関してはある程度以上までいけば大体オッケーですが、クラッシュ等でクラックが生じた際に半田ごてを当てられるICじゃないとデバッグが辛いと考えているためです。
デバッグ性やメンテナンス性は軽視されがちですが、マイクロマウス等の個人で作るロボットの場合、苦しむのは自分なので回路やメカ、ソフトも気を使った方が後がマシになります。（楽になるかはその人次第です）
理論と現実のズレ
さて、これで2次元の極座標で自己位置推定ができるはずです。じゃあソフトウェア書いてやってみよう！ってなると、実際は時間経過と共に実位置と推定位置がズレていくと思います。
（やってみて「そんなにズレてない」と思ってもマイコン内部の数値データで見るとズレてる事はよくあることです）
理論と現実のズレは無くならない上に、その要因も複合的なので低減するのも一苦労です。これは複雑に絡まった毛玉を1本ずつ解いていくようなものです。回路やソフトに主要なノイズ要因として以下の問題が考えられます。（少なくとも自分は苦しんだ問題です）
- 区分求積法による積分誤差
- ジャイロのドリフト
まず区分求積法の誤差について。これはセンサ値の速度や角速度を積分する場合に厳密な積分ではなく、あるステップサイズの長方形と近似して面積を求めているために生じる誤差になります。このステップサイズは時間なので、1kHz毎に取得しているとしたら1/1000sec ＝ 1msec毎にこの誤差が累積して、距離や角度などの誤差につながっていきます。
対策としてはステップサイズを小さくすることが有名ですがマイコンスペック的に限度があります。一般的には積分を使う以上、この誤差は逃れられない問題とされています。しかし、ごく短時間においてはこの誤差は非常に小さいため無視することが可能です。長時間の稼働には他のセンサによる補完や修正による解決策が多いかと思います。マウスでは壁センサによる壁切れ（柱検知）、袋小路で壁との距離を目標値に修正する前壁制御が有名です。
次に、ジャイロのドリフトについて。IMUで姿勢制御をしていると動かしていないのに徐々に回転していったり、前進していたりする現象（ドリフト）があります。
これはセンサ自体が移動していない時にも微小なオフセット出力を吐いてしまうがために生じる現象です。このオフセットは個体差があるため一律に定数を引けば解決する問題ではないです。
よくある方法としてはリセット直後にサンプルデータを複数取得して、オフセット値を算出してしまい、以後の取得では測定値とオフセット値の差分をセンサ取得値とする方法です。
ただし、人が触れているとその振動をオフセットと認識してしまうので人の手から離れた後に取得することをオススメします。
他にも特性を関数で近似する、ノイズを考慮したモデルで予測する方法など深くやればいくらでもあります。
ただ、1番大事な事は
「センシングは真の値を得ることはできない」
ってことです。
人間の感覚でさえ、搭載している感覚センサの範囲内でしか物事を把握できません。使用する上で「誤差とみなせる」「許容できるズレであればよい」って線引きは常に持っておくと良いです。
センシングによる観測は突き詰めると結局のところ「確率」や「尤もらしさ」に行きつきます。確率ロボティクスですね。
はい、余談も交えながらIMUとエンコーダの話でした。
ロボット工学は沼が深いですね。
それでは、また…
