【STA23】BNO Gyro積分回転精度測定＜IMUモードでＦＵＳＩＯＮ無しで使う＞

yaw角の代替対策としてGyroを積分して角度を得る方法の精度測定をしました。
yaw角を使わないということはMagnetセンサを使わないということですので、NDOFモードで
なくて、ＩＭＵモードでMagnetセンサをキャリブレーションから除外したほうが精度がよいだろうということで、IMUモードとＮＤＯＦモードでの１回転３６０度でのGyro積分角度精度測定しました。
*************************************************************************************************
※その後２０２３年１１月にBNO055に代わるIMU BNO085を入手しては、BNO055は、お役御免となりました。
BNO055は、キャリブレーションアルゴリズムにバグがあります。頻繁にキャリブレーションをしすぎるので、Fusionが意図せず狂います。補正であるはずのキャリブレーションが過剰な補正で、データを壊してしまっています。これは、BOSCH社が製品を回収しなければならないのが筋ですがBOSH社は、BNOの開発を撤退して、CEVA社に開発を移譲しました。
CEVA社は、IMUのノウハウをもっているHillCrest研究所を傘下にもっているので、BNO08ｘシリーズで、ＢＯＳＨのファームウエアを全部入れ替えて、ＢＮＯ０５５よりはまともなファームに乗せ換えたので、安物IMUで本来あるべき精度（yaw角±５度）が確保されてます。ということで、バグをもっているBNO055を買ってFUSION無しで使うか、捨ててＢＮＯ０８ｘシリーズを買い替えるかとなります。
*********************************************************************************************************************

—————-※その後７月にはいって、IMUモードでのいろいろな測定をしてきました。—————————————-
BNO055の特徴であるFusion計算機能がIMUモードでは、yaw,pitch,rollとQuarternionの値が出鱈目になることが判明しました。
NDOFモードで磁気センサが狂うのでＩＭＵモードに変更したのですが、どちらも狂うということで回転治具のような高角速度領域(200dps以上)ではFUSIONは使えない結果となりました。現象としては、回転治具で１周360度通過点でエンコード信号いれてみてみると Gyro積分では、毎回３７０度±４度程度で一定値をたもちますが、yawとQuarternion値は、１周通過点でも360度にならずに、２０度くらいオーバーしてから３６０度になります、更に回転速度が遅くなるとどんどん遅延して、最後は１００度くらいずれてしまうというとんでもないデータを吐き出してきます。ＩＭＵモードでは、ＦＵＳＩＯＮのyawとQuarterionが使えないので、結局普通のＩＭＵと同様に、自分でセンサ座標と絶対座標の変換計算をしないといけなくなりました。１つだけ良い点は、Fusion計算のうちlinearAccとＧravityAccの値は正確にでているみたいなので、普通のＩＭＵよりは、機能が良いということです。BNOを何年も使い続ける気がなくなったので、もっと、ＭＥＭＳでセンサノイズの少ないデバイスを探す活動を続けていきます。
————————————————————————————
Gyro初めて使うので、水平と傾斜での違いも測定しました。水平で電源オンして、キャリブレーションさせた後　４５度傾斜させたまま回転させる測定と水平のままで回転させる条件２水準をN＝５とモード２種類で、２０回測定しました。１回２０回転以上なので、１条件で１２０回転分のデータがでるので、平均と３σで比較します。

■Gyroの積分プログラム
元プログラムは。AdafruitのライブラリーのExampleでread_all_data.inoを改造してます。回転治具で回すので無線です。ESP32のBlueToothでデータをＰＣへ飛ばしてます。
gistにあります。https://gist.github.com/dj1711572002/407159fd909ce474d9b89b25fb33719c

50Hz(20msec)での台形積分です。出力値がRadianなので、print時に　Degreeに変換してます。

gx_1=gx;
gy_1=gy;
gz_1=gz;
tg_1=tg;
tg=millis();
gx = event->gyro.x;
gy = event->gyro.y;
gz = event->gyro.z;
gxsum=gxsum+(gx+gx_1)/2*(tg-tg_1)/1000;
gysum=gysum+(gy+gy_1)/2*(tg-tg_1)/1000;
gzsum=gzsum+(gz+gz_1)/2*(tg-tg_1)/1000;

磁気エンコーダーのピン割り込み関数内で積分sum値をゼロリセットすることで１回転360度毎の回転角度を確定してます。

//MAG PIN intgerrupt
void magin()
{
tmag=millis();
magflag=1;
gxsum=0;
gysum=0;
gzsum=0;}

■IMUモードへの変更
①ＢＮＯ055のモード表
datasheetの２１ページにあります。Fusionモードを使わないとBNOのメリットがでませんが、Adafruitのライブラリーのデフォルトは、NDOFモードになってます。Acc Mag Gyro　全９軸をフルでFusion計算するモードですが、
Magを使うと激しい動きで精度悪化しますので、Mag無しで使うことにしました。

②IMUモードへの変更方法
Mag無しなら、IMUモードならFusionをAccとGyroでやってくれるので、精度も良くなるはずということで
ライブラリーにパッチをあてて、モード変更しました。
Aruduinoフォルダー内にあるlibrariesフォルダーにAdafruit_BNO055フォルダーがあるので、その中の
Adafruit_BNO055.hをTextEditorで開いて、285行目のモード設定行を変更します。

デフォルトでは、NDOFモード指定になっています。
bool begin(adafruit_bno055_opmode_t mode = OPERATION_MODE_NDOF);

これを下記のIMUモードへ変更します。

bool begin(adafruit_bno055_opmode_t mode =OPERATION_MODE_IMUPLUS);
//Change OPERATION_MODE_IMUPLUS 2023/6/23に変更

これで、プログラムをコンパイルするとＩＭＵモードに変更されます。

●実験結果
電源オンするときの姿勢でキャリブレーション値が変わりますので、条件としては重要です。
更に、回転姿勢を変えることで、キャリブレーションの良しあしがＦｕｓｉｏｎ精度にでてきます。回転させかたは、手でいきおいよく回して停止するまで自由回転させます。磁気エンコーダを１点だけ設置してあるので、その点を通過すると３６０度ぴったりになるはずですが、
Gyroの積分値が360度からどれだけ離れて、平均値とばらつき３σを測定します。
N=５回で、回転数は、合計１２０回転の平均と３σです。

電源オン姿勢	回転姿勢	NDOFモード	ＩＭＵモード
水平	水平	３７２度±８．４度	３６０度±８．５度
水平	傾斜４５度	３６８度±１７度	３７０度±７．９度

結果１：水平オン水平回転では、NDOFモードとIMUモードの差がでません。
結果２：水平ンオン傾斜４５度回転では、NDOFモードが±１７度もばらつきますが、
　　　　IMUモードは、水平の時と変わりありません。
ということで、GyroとAccしか使わない場合は、IMUモードでFusion計算させたほうが精度良いです。

結果３：ばらつきが±８度というのは悪いという感じですが、MEMS　IMUの３６０度積分値で
　　　　3sigma　8度なら、半回転だと±４度、１２０度回転なら±２．６度なので、RTKの補間
　　　　に使えるレベルになります。RTK側のBNOと同期させるリセットポイントは、１２０度から１８０度
　　　　で使う予定です。