【STA23】BNO055をRTKシステムへ組み込んだ＜Quarternion必須＞

※本記事の10日後にスキー装備に組み込んでスキー場で滑って測定しました。

結果は、素晴らしいデータが得られました。ＲＴＫの速度ベクトルとBNOで計測したpitch角は、スキー板の傾きとしてRTKとぴったりと合ってました。

yaw角もそこそこの精度でRTKに追随してました。RTKの補完センサとして非常に有用で、一部代替できる使い方も見えてきました。詳細記事は、こちらです。【STA23】測定データ見事に狙い通りになった＜BNO055使える＞

RTK　MovingBaseの補完用に高速GPSのM9NとFusion内蔵IMU　BNO055をシステムに組み込みました。

BNO055いじりだしてから１か月ほどかかりましたが、なんとかBNO055もRTKの補完に役立ちそうです。

●STA23システム：

１：RTK MovingBaseシステム（左右）　Ublox F９P２個　Teensy4.1

２：RTK補完用センサFUSIONボード’

（中央）　Ublox　M9N　Bosch　BNO055

３：右端BlueTooth用　M5Atom ＝＞補完の意味 速度ベクトルは、F9PとM9NでGPSドップラー速度、IMUの加速度積分と３か所のデータ比較板の姿勢は、RTK MovingBaseで±１度以内の精度とIMUのFUSIONでＹＡＷ角の２か所で比較位置精度は、F9P　RTKで±１－３ｃｍ　MovingBaseで１cm以下の精度を確保とそれぞれの得意不得意を組み合わせて、データの精度を高めることを目指してます。 ●BNO055をシステムに組み込んだ備忘録

⓪BNO055は、Ｉ２Ｃで使ったほうが良い。 　シリアルにしようと微小パッドに半田付けしたら、パッドが剥げてしまった。そうしたら、SDAの波形が壊れてしまって故障状態になったので、パッド代りにＵＥＷ線でパッドＧＮＤ短絡させて復活させた。 ①I2Cケーブルは、４芯で他の信号と同居させてはダメ ６芯でシリアル信号線を同居させたら、Ｉ２Ｃがまともに動作しませんでした。

■ＱＵＡＲＴＥＲＮＩＯＮ資料 クォータニオンはＷＥＢでたくさんありますが、なかなか簡単で分かりやすいのがないです。

その中でも、とっつきやすかったのは昔の文献 http://icb-lab.naist.jp/members/yoshi/ics_lecture/multi_media/3d_programming/Enter_the_3D_Programming.pdf

クォータニオン計算便利帳という資料もあります。
https://www.mesw.co.jp/business/report/pdf/mss_18_07.pdf

あとは、QIITAで現役のエンジニア様が解説している記事がたくさんあります。 https://qiita.com/search?q=%E3%82%AF%E3%82%A9%E3%83%BC%E3%82%BF%E3%83%8B%E3%82%AA%E3%83%B3

クォータニオン総整理という資料もあります。
https://qiita.com/drken/items/0639cf34cce14e8d58a5

姿勢角としてオイラー角について詳細に解説されているのは、スポーツセンシング社様で感謝です。

https://www.sports-sensing.com/brands/labss/motionmeasurement/motion_biomechanics/euler_angle_expression.html

＝＞知らなくても、BNo055が出力してくれるので、yaw,pitch,rollさえわかっていればなんとかなります。

②BNO055の出力は、QuarternionからEULER角を計算しないとYAW精度でない 当初、Eulker角出力(Orientation)を使っていたのですが、いろいろな姿勢で使うと、YAW角がずれてしまいます。この件は、こちらのブログ様の解説に感謝です。 https://rokuen.work/bv/101/2017/06/ どうやら、BNO055の仕様上pitch,rollが４５度以上傾けるとYAWが不安定になることを指摘されてます。その解決策として、Quarternion出力から、yaw,pitch,rollを計算する方法を解説されていて感謝です。

Quarternionからyaw,pitch,rollの計算は、サンプルプログラム末ごとコピーさせていただきました。

③RTKとIMUのタイミングの同期方法で手間取った

RTKは、リアルタイムキネティクスと言われてますが、一般的なセンサのリアルタイムとは程遠いほど リアルタイムではありません。衛星電波を受信してから、演算処理時間が20-70msec遅れてマイコンに受信データが届きます。ですので、IMUと同期させる場合は、RTKのデータは役立たないので、タイムパルス信号で割り込みをいれてIMUと同期させる必要があります。 SimpleRTK2Bは、丁寧にTIMEPULSE信号ラベルが貼ってあります。１番の白線をマイコンGPIO22にいれてます。 ■Teensyの場合のピン割り込みは超簡単です。 setup() 内で下記１行を定義しておいて //—-time pulse interrupt set——— attachInterrupt(digitalPinToInterrupt(22), myInterrupt, RISING); 関数　myInterrupt　を作っておけばいいだけです。 // —–interrupt timepulse pin22 void myInterrupt(){ tp=millis();//タイムパルス信号が立ち上がった時刻をマイコンのタイムで測定して基準とする tp0=tp; tpflag=1; } このtp時刻を基準にしてIMUのデータサンプリングタイミングを作ります。 loop()内で //============bnoread()配列に3エポック周期で収====================== //タイムパルスに同期させる //=============================================================== if(millis()>tp){ //このif文だけで、IMUのタイミングができます。 tp=tp+40;//40msec周期なので、基準時刻に40msec足して次のタイミング時刻を作っておきます。 bnoread(bn%3);//readする関数を呼び出しloop全体が120msec周期なので、３回カウントして呼び出す。 bn++; } ④SDログ用データ加工保存するデータは、Quarternion ４個とQuarternionから計算したEuler角(yaw,pitch,roll)と測定時刻です。 40msec　２５HzでM9Nの周期にあわせました。Magの周期が20Hzなので、無理してるかもしれません。 Adfaruitのライブラリーは、floatで出力されるのですが、SDログ用にはFLOATをバイナリにすると、後のデータ処理が面倒になるので、1000000倍して整数にして４バイトの符号あり整数としました。整数なら、簡単に４バイトに変換できるし、戻すときも直観的にできます。整数を４バイトに変換する関数 //+++++++++++++++i_to_char++++++++++++++++++++++++++++++++++++ // i=IntegerValueData,*d=Array pointer, n=Array start No void i_to_char(int i, uint8_t *d, int n) { d[n] = i & 0x000000ff; d[n + 1] = (i & 0x0000ff00) >> 8; d[n + 2] = (i & 0x00ff0000) >> 16; d[n + 3] = (i & 0xff000000) >> 24; // Serial.printf(“itochar:i=%d,d[0]=%x,d[1]=%x,d[2]=%x,d[3]=%x\n\r”,i,d[0],d[1],d[2],d[3]); } //４バイトを符号付整数に変換する関数 //+++++++++++++4byte Binary to Long ++++++++++++++++++++++++++++++++++++++++++++++ long B2L(uint8_t b4 , uint8_t b3 , uint8_t b2 , uint8_t b1 ) { //pc.printf(“B2L IN=%s,%x,%x,%x,%x,b4&0x80=%d\n\r”,sen,b4,b3,b2,b1,b4 &0x80); //pc.printf(“B2L IN=b4&0x80=%d\n\r”,b4 & 0x80); long su; if (b4 & 0x80) { //最上位ビットたっていればマイナス //su = -(256 – (long)b1) + (255 – (long)b2) * 256 + (255 – (long)b3) * 65536 + (255 – (long)b4) * 256 * 256 * 256; //pc.printf(“B2L-:sen=%s,%d,%d,%d,%d,%d\n\r”,sen,b4,b3,b2,b1,su); uint32_t i1 = b1; uint32_t i2 = b2 << 8; uint32_t i3 = b3 << 16; uint32_t i4 = b4 << 24; uint32_t i5 = i1 | i2 | i3 | i4; su = (long)i5; } else { su = (long)b1 + (long)b2 * 256 + (long)b3 * 65536 + (long)b4 * 256 * 256 * 256; //pc.printf(“B2L+:sen=%s,%d,%d,%d,%d,%d,%d\n\r”,sen,b4,b3,b2,b1,su); } return su; } ●姿勢差実験 https://www.bosch-sensortec.com/products/smart-sensors/bno055/ data sheetではこうなってますが、座標は覚えておく必要ありませんでした。 https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf BNO055は、電源オンされたときの姿勢で座標を作られます。写真の姿勢では、Y軸が上下方向、Z軸が前後方向、X軸が左右方向の併進ですが、 yaw出力は、Z軸の指す方向を360度（０度）と定義されます。 pitch出力は、この写真だと-7５度くらいでてきます。 roll出力は、この写真だと10度以下です。 TERATERMでモニターしたデータ　RTKの１周期に３回BNO055のデータ取得されてます。データがSDファイルにログできているか確認しました。 DIRがモニターで見られようになってますので、ファイルのサイズと日付時刻をみて確認できます。ファイルの中身をバイナリーエディタでみました。BNO0,BNO1,BNO2がヘッダとなってRTKデータと連なってます。 ●以後 RTK MovingBaseと方向を一致させるキャリブレーション機能をプログラムに追加します。来週には、スキー場で初実験をしたいところで焦ってきました。