六縁試策名義でスイッチサイエンスさんに委託している
「気圧温湿度複合9軸センサ基板」ですが、好調に売れているようです。
（複雑な機能を持つ製品なのに問い合わせが本当に少ないので、すごい不安な日々ですが・・・）
搭載されているBOSCH社のBNO055は、メーカーのパンフにも掲載されているように
「面倒な計算なしにオイラー角を直接出力できる！」ことが利点です。
しかし、実はこのオイラー角直接出力機能にはいくつかの弱点が存在します。
- 出力される値が16LSB(標準)なので、誤差が生じやすい
- オイラー角の視点がYaw軸が0 – 359[deg]になっている
- Pitch軸またはRoll軸が±45[deg]の範囲を超えると、Yaw軸の値が不安定になる
これらの何が問題なのか、そしてそれぞれの解決法は？？
というのを追記に書いてます。
BNO055の問題点1:オイラー角出力機能で出力される値が16LSBである
オイラー角出力機能では、符号付16bitのshort型の値で提供され、それにLSB値の逆数を乗算することで浮動小数の角度として扱うようになっています。([rad]の場合は900LSB)
この16LSBというのは
「short型の値でいう 1 が、実際の値の 16分の1」であることを示します。
つまり、出力された値の精度は0.0625度刻み(1 / 16 = 0.0625[deg])です。
出力値が 0x0B1C(2844) だとすると、この角度は 177.75 [deg]になります。
我々の暮らしの中で使う分には申し分ない精度ですが、高速度の機械制御の世界では
40LSB(1 / 40 = 0.025[deg])くらいは欲しい時があります。(ロケットとか衛星とか)
そういう場合は、オイラー角直接出力で出した数値はちょっと誤差が大きいかもしれません。
BNO055の問題点2:オイラー角の視点がYaw軸が0 – 359[deg]になっている
あまり大きな問題ではないかもしれませんが、オイラー角出力で
Roll軸は±90[deg]、Pitch軸は±180[deg]なのに
なぜかYaw軸だけ、0 – 359[deg]の正の浮動小数の出力になっています。
自分の立ち位置を 0 として + と – で考える航法が一般的(?)なので
本来であれば±180[deg]としてほしいところなんです。
自分でわざわざ値から -180.0 を加えないといけないのがなんとも情けない。
浮動小数の加減算1行で済む話だからどうということはないだろうけど・・・。
BNO055の問題点3:Pitch軸またはRoll軸が±45[deg]の範囲を超えると、Yaw軸の値が不安定になる
ここが一番大きな弱点だと思います。
例えば、姿勢が
Yaw:0.0[deg], Roll:15.0[deg], Pitch:15.0[deg]
のとき、Yaw軸の値は信頼できます。
しかし、
Yaw:0.0[deg], Roll:45.5[deg], Pitch:15.0[deg]
のとき、Yaw軸の値は信頼できません。
なぜなら、Yaw軸の値が90[deg]ずれている可能性があるからです。
この問題はBNO055を搭載したボードの使用者からいくつも問い合わせが挙がっているもので、BOSCH社によれば、「オイラー角出力機能は、ロール軸とピッチ軸が±45度の範囲を超えるとヤバイので、e-compassとか(船の方位とかを見る機械)でしか使わないでね！」(意訳)とのこと。
実際に手元の基板で試すとRoll軸またはPitch軸が±45[deg]の範囲を超えると
Yaw軸の値が急激に加減します。しかもぴったりと90[deg]。
おそらく内部の積分の際に計算速度を向上させるために
符号情報を一部省略していることが原因だと思います。
え？じゃあ9軸センサーとして使えないじゃん？お金返せ！！
と思う方も多いかと思いますが、余裕で対処可能ですのでご安心ください。
それぞれの問題の対処法
- 四元数出力機能を使用して角度を求める
- 180.0を減算する(まんま)
- 四元数出力機能を使用して角度を求める
問題点2の対処法は前述のとおり、180.0を減算するしかありません。というか0 – 359[deg]で対処できるように計算式を変えるよりそっちのほうが計算コスト低いし・・・。
問題点1と3は、BNO055本来の機能である
「四元数(Quaternion)出力機能」を使う
ことであっさりと解決できます。
(オイラー角直接出力機能はこれの副産物でオマケ程度です)
精度については算出元の四元数精度が2 ^ 14LSBなので、かなり精度の高い角度の算出が可能で、
Roll軸が45[deg]を超えようがPitch軸が45[deg]を超えようが、Yaw軸の信頼性は絶対です！
方法について書くと
BNO055から出力された四元数を生の値に変換して
オイラー角に変換する数式にブチこむだけです。
四元数を生の値に変換するには、LSB値を除算すればいいので
倍精度浮動小数の枠を用意して、採ってきたデータ ÷ 16384 を入れればいいだけです。
四元数は４つがそろって初めて意味を成すものなので。
double w = (double)((short)w_data / 16384); double x = (double)((short)x_data / 16384); double y = (double)((short)y_data / 16384); double z = (double)((short)z_data / 16384);
こんな感じで四元数の生の値を生成します。
そして、これらを使ってオイラー角に変換します。
方法は Wikipedia にもわかりやすく書かれているとおりにします。
double ysqr = y * y; // roll (x-axis rotation) double t0 = +2.0 * (w * x + y * z); double t1 = +1.0 - 2.0 * (x * x + ysqr); roll = std::atan2(t0, t1); // pitch (y-axis rotation) double t2 = +2.0 * (w * y - z * x); t2 = t2 > 1.0 ? 1.0 : t2; t2 = t2 < -1.0 ? -1.0 : t2; pitch = std::asin(t2); // yaw (z-axis rotation) double t3 = +2.0 * (w * z + x * y); double t4 = +1.0 - 2.0 * (ysqr + z * z); yaw = std::atan2(t3, t4);
こんな感じであっさりとradian(弧度法)の角度が手に入ります。
このあとに、rad(弧度法)からdeg(度数法)に変換しないといけませんので
roll *= 57.2957795131; pitch *= 57.2957795131; yaw *= 57.2957795131;
として、これでやっと度数法の私たちに馴染みのある角度を
申し分ない精度で手に入れることができます。57.2957795131は 180.0 / PI です。
Wikipediaの記事中にも書かれているとおり
atan2やasinの代わりにarctanやarcsinを使うこともできますが
出力される範囲が±90[deg]になってしまいます。
可能な限りatan2などの関数を使って精度の高い計算にする必要があります。
私が作成したライブラリでも上記を使いやすくまとめたものを5月30日付で実装済みですので
よかったら使ってみてください。(getEulerFromQという関数)
それ以前に使用されていた場合は、BNO055ライブラリのヘッダとソースを書き換えればそのまま使用可能です。(六縁試策の製品ページにまとめています)
なんだよ！BNO055で簡単に角度が手に入ると思ったのに！
結局自分で計算する必要があるじゃねえか！詐欺だろ！金返せ！
って思った人たち。
9軸(加速度、角速度、地磁気)から四元数を求めるのが
どれだけ大変で、非常に計算コストがかかるかをよく勉強してみてください。
これでもびっくりするくらい簡単なんだよ！
私は以前に こんな記事 を書いていて、そのときに
「9軸の情報から四元数を求めるだけでこれだけ大変なのか」ということを
痛いくらいわかっているので・・・。
BNO055のように内部で計算済みのものをフィルタ処理して出力してくれるというのは
非常に画期的で、便利で、計算コストを意識せずにプログラムを組めるということを理解すべきかと思います。