STA23では、メインのセンシングは、
スキー板の絶対位置をBase（F9P）のRTKで基線長位置を求めます。
スキー板の姿勢をRover（F9PかF9H）のMovingBaseで相対座標を求めます。
スキーの傾き（角付け角）は、IMUを使って脛のレガースの姿勢で、pitch,rollを求めます。
※2023年5月から回転治具に磁気エンコーダー手作りしてyaw絶対位置を決めて繰り返し位置精度測定実験開始
【STA23】回転治具磁気エンコーダー位置精度その２＜補間計算手間取った＞
【STA23】磁気エンコーダ回転治具試運転その１＜RTK精度良い、BNO劣る＞
【STA23】BNO055回転データの統計処理方法＜回転角を積算する＞
【RTK23】回転治具でBNO055とRTK Heading角基礎実験その０＜角度合わせ＞
＝＞2023/6/5現在の感想：BNO055は、フィルターの特性によって条件次第でyaw精度がころころ変わります。
●RTK MovingBase法だとミリオーダーで相対位置精度がでるので、アンテナ間距離を１ｍ以上にすればHEADING角度は、±1度未満の高精度になります。その高精度のRTKのHEADING角とBNO055のYAW角を比較実験をします。実験は、3月から５月にかけてスキー滑走実験で行います。データを収集して、夏までに、総合的な精度評価を行って、MovingBaseの代替としてBNO055をスキーのHEADING用途で使えることを立証できれば、本テーマは成功ということになります。
◆３回目のスキー測定状況：データをまとめてみました。右スキーのBNOは、RTK MovingBase Heading角と波形が似てます。左は全然なので、グラフ以前の話しで、何か理由がありそうです。似ているグラフはこちらでご覧ください。
◆2回目のスキー測定状況：右スキーBNOのシールド線を片GNDに落として、正常動作できました。右左スキー靴から脛にかけて縦置きでとりつけました。右と左でBNOの姿勢が９０度回転した姿勢でとりつけました。結果は、右はRTKのHEADING角に追従してますが、左は全然違う波形になりました。何故こんなに差がでるのか、解析してみます。右側のデータならＲＴＫ MovingBaseの代替えに検討できるレベルでした。左側がなぜ悪いのか明確にします。
◆１回目のスキー測定状況：
左スキーのBNO055のYAW角は、５－１０度範囲の精度で出ます。動きによっては、１－２度精度になったり、１０度になったりTPOで精度が大きく異なりますので、使い方を工夫すれば、５度以内で使えるとおもいます。右スキーのBNOは、シールド線を片GNDに落としてなかったので、不安定でNGでした。
●BNO055採用理由
pitchとrollだけなら、6軸のIMUでも、カルマンフィルター処理すればそこそこの精度で、姿勢角が得られます。
しかし、YAW角は、磁気センサと加速度、ジャイロセンサの９軸センサフュージョン計算をしないと、求められません。特に、磁気センサの傾斜補正が精度よくでないため、RTK MovingBaseを採用してきた経緯があります。
磁気傾斜補正で失敗した記事（２０２０年１月）は下記です。
【L-RTK】ＩＭＵ磁気方位センシング傾斜補正でこけた＜スキーならＲＴＫ技術が有利＞
■RTK MovingBaseのおかげで、HEADING角精度±１度で、スキーの姿勢を得られて、正確な横滑り角を測定することができました。しかし、RTK MovingBase法だと、スキー場に行ってから、滑走測定するまでに１時間以上かかって準備しないといけませんので、システムが複雑で配線がからみあっていて、日常のスキー遊びに使うには、不適です。２０２３年からは、スキーアナライザでなく、実用的なスキートレーシングシステムを開発して、日常のスキー遊びでも楽しく使えるシステムを検討してます。
そこで、３年経過した今から、IMUの補正が全部入っているSensor FusionチップBNO055を採用して、
姿勢角のうちYAW方向がMovingBaseと比較してそこそこ使えるレベルになるかを実験することにしました。
センサFUSIONの各種フィルターのプログラムを動作させる手間を考えると、アルゴリズム内臓の
BNO０５５は御の字です。
※本記事から３週間後ようやくRTKシステムにBNO055を組み込みました。
相当苦労しました。記事はこちら
※2023年２月１６日 スキー滑走を測定しました。
BNO055は、使えます。RTKよりは、最悪５度くらい外れている場合がありますが、普段は、1－2度で合ってました。その代り、RTKより小型で実装性が抜群に良いので、実用的です。詳細は記事をご覧ください。スキーには、BNO055おすすめです。
※２０２３年４月２５日 BNO055のサンプリング方式をハードウェアタイマーにしたらハマりました。
正解は、内部タイマーでなく外付けタイマーを取り付けて、ピン割り込みでＢＮＯを測定することでした。
何しろ、変換速度が遅いのが欠点で、ライブラリーのBNOの読み取り関数を一連で読むと5mecかかってしまうのでいろいろなことをやっているプログラムだと割り込みタイマーとよそのスレッドがぶつかって、変数化けしたり、データが飛んだりして大変なことになりました。無理やり内部タイマー使っても所詮マイコンのクロック精度の12ppm程度なので、３０分も回せば、30-40msecズレてしまいますので、労多くして結果が良くないプログラムでした。
【STA23】BNO055測定 Interval Timerではまった＜Priorityと現合調整＞
外付けタイマーの威力が絶大です。
【パワーメーター2019】割り込みタイマー精度測定結果＜TCXOは計測の必需品＞
※２０２３年２月１０日追記 RTKシステムに組み込んで、精度実験始めました。
とりあえず、RTKのYAW精度に対して、±２～３度で追従してます。
EULER角出力では、姿勢の制限があるので、QuarternionからEuler角を計算することで
制限がなくなって、高精度になりました。
【STA23】レガースに組み込んで試運転してみた＜無事動作できた＞
※２０２３年３月２日 追記、測定データを流れるグラフプログラムを作って、全データを眺めてみたら、静止状態が続いた後にBNO055のデータがドリフトすることを発見しました。
波形は、RTKの波形に似ているので、単純に下駄はいたドリフトでした。これは、自動キャリブレーションが静止時に働いて、ゼロ点が変化したからだと思います。そこで、プログラムで、補正をかけることにしました。スキーの場合、NUTRALポイント通過時点で、スキー板の方向と速度ベクトルの方向が一致するので、その瞬間にBNO055の角度の補正値を計算して、次のNUTRALポイントまでその補正値で角度を補正する方法です。これでやると、RTKの角度に対してで５度以内の誤差でおさまります。しかし、BNOのYAWは、５度以内の場合もあれば１０度もずれてしまうことがありますので、ＲＴＫなしでは、使えないと感じてます。±１度のheading角精度を普通に得られるＲＴＫは偉大です。関連記事はこちら
※2023年3月４日追記 スキーコースのフォールラインの求め方の検討で、RTKの強みがでました。
※３月１１日追記 ２回目のスキー場での測定で、BNO055ではFallLineが見れないことがわかりました。
やはりYAW角の精度が１０度もずれる点で、全データがずれてしまっているのが課題です。
RTKで全データを補正できるか検討してみます。その間は、MovingBaseメインで測定していきます。
●秋月のAE-BNO055ーB0が手頃です。スキーで使うので2個買いました。
Adfruitなど海外製は、円安で非常に価格が上がっているので、国内在庫があるお店が安いです。
■一個壊してギリギリ修理した（ジャンパ小さすぎて半田付け難しい）
私の場合、推奨設定でつかわないでシリアル設定にするために、ジャンパ５をカットして、ジャンパ３に半田をのせて、シリアルで動作させようとしたのですが、いかんせん、AdafruitのライブラリーがＩ２Ｃのみだったので、ジャンパ５に半田でつないで、ジャンパ３の半田を除去しました。Ｉ２Ｃの初期動作を確認して本記事を書いて、いざ実装基板に組み込んだのですが、実装基板に半田付けする
時に半田ごてをジャンパ５に近づけてしまったらしく、ジャンパ５が不通になってました。この状態でＩ２Ｃを使うとＳＤＡが2段階の電圧波形になって、I2Cが動作しない故障になってしまいました。壊したかと思って、４M変動をチェックして、ジャンパ５が不通になっていました。UEW線載せて引っ張ったらパッド自体が剥がれてしまいました。しょうがないので、回路図をみて、残った片側のパッドをＧＮＤに結線して無事Ｉ２Ｃモードになって動作できました。
●組付け
①シリアルはやめたほうが良いらしい
届いたら、取説をみるとI2Cとシリアルが使えると書いてあるので、シリアルでやろうと思ったのですが、
シリアルだとライブラリーが無いし、安定動作しないというadafruitForumに解説があったので、Ｉ２Ｃにしました。https://forums.adafruit.com/viewtopic.php?f=25&t=186990
②I２C配線
秋月の基板は、I２C用電圧レベル変換チップFXMA2102LBXを使ってあって、プルアップもBNO055との間にR1R2でしてあるので、ユーザーがプルアップ抵抗を取り付けなくてもよくて便利です。
但し、Teensyの場合は、Teensy側にプルアップをつけないと安定動作しないというWEBコメントがあります。
私は、STA22用基板にプルアップがもともとついていたので、そのまま使いました。後で、プルアップ抵抗外したら、データサンプリングが歯抜けになってしまったので、Teensyでは、プルアップ必須です。
●ケーブルでハマった＝＞Ｉ２Ｃも４０ｃｍ長になるとこけやすいです。
母艦からセンサFUSIONボードまで1本のケーブルにまとめて接続したのですが、なんとI2Cが動作不良になりました。シリアルRXTX＋I2C２本＋３．３ｖ＋ＧＮＤの６本を６芯のロボットケーブルにまとめて接続したら、
データ転送はしているのですが、ゼロデータになってしまいました。オシロで見ると、ＳＤＡ波形が鈍ってました。そこで、シールド無しの４芯にして、Ｉ２Ｃ線だけだと正常に通信できましたが、不安定さがあって、時々データゼロになることがあるので、やはり、正しい使い方である片GNDにおとしたシールド線を使うことが必須です。シールドフリーにすると全然だめでした。
シールド付きの4芯でシールドフリーにしたら、時々不通が発生しました。シールドをGND線につないで浮かせた片シールドにすると正常に動作しました。T
TeensyのI2Cが速すぎるのではないかと思います。Adafruitライブラリーの設定からTeensyのI2Cが
他のCPUとは、違うためにシールド線まで気を使わないといけなくなります。
UARTなら混在していても問題ないのですが、I2Cは、クロックが速い分だけ、うるさいです。私は、そんなに速度いらない用途なので、シリアルで十分なのですが、Ｉ２Ｃしかライブラリが無かったから、Ｉ２Ｃにしたのですが、将来的にはＢＮＯ０５５もシリアル制御で動作させたほうが、システムが組みやすいと思います。
※注意：BNO055のAdafruitライブラリのフルパラメータの測定では、変換速度が5msecかかるので、測定時刻がその間ということで、センサとしては、時間精度が悪いです。加速度測定で、5msecも取得時間がかかっていては、使えない用途がたくさんあるので、ＢＮＯ０５５の用途は遅い現象専用です。私の場合は、人間の運動なので、遅くてもよいですが、自動車、機械などの動作だと厳しいと思います。
③Arduino IDEでAdfruit BNO０５５libraryをインストール
スケッチ＞ライブラリーのインクルード＞ライブラリーの管理 ＷＩＮＤＯＷをだして
BNO055と入力すれば、しばらくすると３個くらいライブラリーがでてきますので、
Adfruit BNO055 をインストールします。今回は、ver1.6.1を使ってます。
●基礎知識
基本は、クオータニオン出力なのですが、学習して計算するのが面倒なので、ライブラリーが
計算してくれる結果だけいただくことにしました。
クォータニオンはＷＥＢでたくさんありますが、なかなか簡単で分かりやすいのがないです。
その中でも、とっつきやすかったのは昔の文献
http://icb-lab.naist.jp/members/yoshi/ics_lecture/multi_media/3d_programming/Enter_the_3D_Programming.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://www.sports-sensing.com/brands/labss/motionmeasurement/motion_biomechanics/euler_angle_expression.html
●プログラム作り
注意）BNO055のオイラー角は、範囲の制限がpitch,rollにあって、それを外れると角度がでなくなってしまうらしいので、オイラー角は、使わないで、Quarternion出力からオイラー角を計算しないと正しいオイラー角が得られないことをこのサイト様が解説されています。非常に感謝です。
https://rokuen.work/bv/101/2017/06/
webserial_3d.inoというAdafruiteサンプルプログラムを使いました。そこから得られたQuarternion データを上記ブログ様の教えてくれた計算式を丸ごとコピペして使わせいただいて、Euler角を得る方法でプログラム組みました。void bnoread(int no)の中でやってます。GISTにあります。
https://gist.github.com/dj1711572002/a95b6944ce5ccb60faecd0a3edfcf9df
◆コピペしたQuarternionからEuler角への計算部分は下記です。各データはログ用に配列[no]にしてあります。
//Quartenion Copy from https://nobita-rx7.hatenablog.com/entry/27642606
imu::Quaternion quat = bno.getQuat();
qW[no]=(float)quat.w();
qX[no]=(float)quat.x();
qY[no]=(float)quat.y();
qZ[no]=(float)quat.z();
//time pulse millis
tst[no]=tp;
//Calc Euler from Quaternion
double w=qW[no];
double x=qX[no];
double y=qY[no];
double z=qZ[no];
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 = 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 = asin(t2);
// yaw (z-axis rotation)
double t3 = +2.0 * (w * z + x * y);
double t4 = +1.0 – 2.0 * (ysqr + z * z);
//rad->deg
yaw = atan2(t3, t4);
roll *= 57.2957795131;
pitch *= 57.2957795131;
yaw *= 57.2957795131;
if(yaw<0)
{
yaw=360+yaw;
}
yaw=360-yaw;
//——————————
eX[no]=yaw;
eY[no]=pitch;
eZ[no]=roll;
●簡単に姿勢角をみたいので、3次元の姿勢角を出力してくれるプログラム例
（上記QurternionからEULER角計算ではなくBNO055のEULER角です）
ファイル＞スケッチ例＞カスタムライブラリーのスケッチ例（一番下）の
Adafruit BNO055＞をクリックするとwebserial_3dがあります。
このプログラムのPrintを変更して、CPLTでグラフにするように改造したプログラムがこれです。
https://gist.github.com/dj1711572002/82216217b96c8abc2d62b5c6d122bf87
●キャリブレーション
通常の磁気センサだと、起動時に八の字を描いて、キャリブレーションしないといけないのですが
BOSCHのQUICK START GUIDEでもやはり八の字をやれと書いてありました。動画例
のように電源オン直後に、八の字で３回くらい振り回すとMagのkyライブレーション値が３となります。
３＝フルキャリブレーションOK状態、２＝センサーFUSION動作OK、１＝８の字モーションが必要。
となってます。
※Calibrationについて仕様書に記述がありました。やはり動かさないとダメみたいです。
常時キャリブレーションが動いていて、データを蓄積しているアルゴリズムだそうです。
https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf
その代り起動時にセンサがむいてる方角を０度（３６０度）と定義してします。
実使用じには、RTKと方向角を補正しないといけません。
丁度、RTKのＦＩＸと基線長解析時間と同じくらいで、キャリブレーションできるので、その結果を
ＲＴＫと補正してから本測定がはじめられます。
●YAW方角実験 方角は一定にして、センサの傾斜角を変化させた場合
磁気センサは、傾斜補正精度がネックになるので、そこをまず最初にみました。
戸建て古屋だと、地磁気の影響が少ないみたいで、室内こたつで実験ができました。RTKでは考えられない手軽さです。姿勢角の正確さは、なかなかだせないので、プラスチックケースにBNO055を貼り付けて、ケースの姿勢を縦と水平で９０度ずつ変化させて、データをみてみました。
方向は、手前から奥の方向が現在の方角になります。
①電源オン 水平置き 方角は、手前向き
Orientation.x=-0.06度,y=-0.9度,z=-3.1度
②鉛直に立てた 方角は手前向きで同じで、ｚ軸が９０度傾斜した場合
Orientation.x=2.87度,y=2.375度,z=-91.17度
度2.87度ｘがずれてます。ケースの精度と置く精度があるので、全てセンサの誤差ではありません。
③方角を左斜め方向へ変えて、電源オンしなおした場合、
Orientation.x=359.75度,y=-0.812度,z=-2.5度
④ ③を垂直に立てた場合
Orientation.x=1.25度,y=?度,z=-2.8度
1度ずれた。
●結果
キャリブレーションをどうやって、管理するかが課題です。レガースを八の字に振り回すことにします。
姿勢差が９０度変化してもＹＡＷ角が１－３度程度で収まっているのは優秀です。
ただし静的な実験なので、動的にはいろいろな振動成分を拾うので、どこまで精度がでるかですので
静的な精度最高精度とみれば、良くて、１－３度で、動的ならさらに悪化するのは、予想できます。
ＲＴＫの±１度には、追いつけませんが、３度以内に収まってくれれば、遊びで使う分には使えるのでは
と思います。
●以後
ＳＴＡ２２システムに組み込んで、実際のスキー場で測定してＲＴＫの進行方向データと比較してみます。
