姿勢角を求めるには

去る11/14日、mbedワークショップ 2015 VOL1 〜mbedでIoTプロトタイピング〜というのに参加してきた。参加費6000円でmbedの一番安いやつ(mded LPC1114FN28)+WiFiモジュールやGroveというハンダ付けいらずでセンサー類を付けられる端子のついた拡張ボード(Simple IoTボード)+温度センサーがもらえるというもの。(正直mbedボードはもうすこしいいやつが欲しかったし、個人的に作りたいものの方向性が違うのでSimple IoTボードと温度センサーはいらなかった感。mbedで作りたいものはあるけど、とりあえずはインターネットにつなげてどうこうするやつではないので。)

まー何をつくりたいかは一旦おいておくとして、とりあえず必要な機能として「現在の角度(ピッチとヨー)」というものがほしい。スマートフォンの画面回転とかのあれである。
「どの方向へ回転しているか」、つまり角加速度とか角速度ではなく、現在の角度であることがポイント。
二次元座標で例えると、現在の速度ではなく現在の座標が知りたいということになる。

人間で言うと、回れ右はヨー、逆上がりはピッチ、側転がロール


  


この目的を叶えるためにはいくつか方法があるもよう。


・角速度センサーを使う(角加速度センサーかも)

角速度センサーから得た角速度の値を積分して姿勢角を得る方法。この方法だとピッチロールヨーすべて取ることができるという利点はあるが、角速度センサーには必ず誤差があって静止状態でも常に同じ方向に回転しているようにノイズが入るらしい。(いろんな方向にぐねぐね動いているようなノイズではないらしい)ノイズの大きさは温度によって変化するので単にオフセットをとって補正…という方法では対処できない。


・加速度センサーを使う

地球上では常に鉛直下向きに重力加速度がかかっているので、静止状態ではピッチの軸とロールの軸それぞれの向きに加速度がかかっている。このためそれぞれの軸方向にどれくらい加速度がかかっているかから姿勢角を求めることができる。
ただしこの方法だと重力加速度と同じ方向の軸を持つヨー角は求められないし、動いている状態だと重力加速度以外の加速度がノイズとして入ってしまう。


・角速度センサーと加速度センサーを組み合わせる

角速度センサーでは常に同じ方向へのノイズ、つまり周波数の低いノイズが入る。いっぽうで加速度センサーでは重力加速度以外のいろいろな方向に変化する加速度というノイズ、つまり周波数の高いノイズが入る。
なので角速度センサーにはハイパスフィルターを、加速度センサーにはローパスフィルターをかけてそれぞれの出力値を合わせて考えると結構精度よくピッチロールヨー全部とれるんだと。単に双方の値を適当な割合で重み付き平均をとるだけでも実用的な精度は出るみたい。(相補フィルター)
しかも、六軸センサーという角速度センサーと加速度センサーを1セットにまとめたパッケージがあるのでコスト的にもよい。そういえばPS3の初代コントローラーの名前はSIXAXIS。


・地磁気センサーと加速度センサーを組み合わせる
加速度センサーではヨー角、つまりコマのような鉛直線を軸にした回転運動による変異をとらえることができない。これを補うために地磁気センサーすなわち方位磁針を組み合わせる方法。
センサーが2つ必要になってしまうけれども、単に回転だけではなく方向を指し占めすデバイスなんかを作る場合にはこれを使うんだと思う。



とまぁ面倒なことを書いてきたけれども、Amazon価格でたったの340円なMPU6050という六軸センサーにすらハード的に姿勢角を計算してくれる回路がついているらしい。
というわけで何も考えず姿勢角は簡単にとれるのであった。。。
DMPという機能で、姿勢角以外にも補正済み角速度や加速度も容易に取得可能。

mbedではなくArduino向け記事だけど参考: InvenSense のジャイロ・加速度センサ MPU-6050 を使う

これは余談だけど、LPC1114のストレージは32KBしかないのにMPU6050用のライブラリとサンプルプログラムだけで18KBも使ってしまう。困った。