【IMU】回転行列とオイラー角表現ハマった＜JAXAの論文が良い＞

回転行列からオイラー角を求める学習始めて２週間たってますが、ハマったきり出口が見えません。
私の場合出口は、現物のセンサの出力で、学習した計算結果が実証できた時点で出口としてますが、ＢＮＯ０５５では、計算値と実験値の一致ができませんでした。

●ＢＮＯ０５５を教材として実験値と計算値を合わせる学習方法はダメでした
BNO055の測定値GYROを積分した角度で行列計算をしらみつぶしに照合して、合致する計算方法を探すという
試行錯誤で、数打てば当たる式でやってみました。一回だけの回転行列なら回転前と回転後のオイラー角は、計算と実際は会うのですが、連続回転で姿勢を次々と変えながら、元の基準位置に対して連続変換すると、5～20度くらいずれた値になってしまってハマってます。元々BNO055の内蔵FUSIONでは、yaw角精度は、±５度最悪１５度と精度悪いのでセンサ自体の精度と計算精度がごっちゃになってしまってます。計算以前にセンサの精度が悪いので、学習に使うには、不適なセンサ教材だということかもしれません。BNO055のyaw角は±５度でれば御の字と割り切ってあきらめてしまえば、yaw角はRTKに任せて、精度の出ているroll,pitch角を活用したＩＭＵの使い方をすればよいです。この学習方法も、回転行列のオイラー計算をRTKのyaw角とBNOのpitch,roll角を組み合わせて実験値を設定すれば、計算方法の学習がすんなりできると思います。後日余裕がでたらやってみます。
●理論だけの学習では、ＷＥＢ上には、初心者の学習に適したページが無い。
更に、教科書となるべき初心者用解説ページが回転行列とオイラー角分野には、見あたりませんでした。例えば、固定角回転ZYX系を計算しながら図示してみると、Z軸、Y軸は、固定軸で回転するのですが、最後のX軸だけは、固定軸でなく、回転移動後の軸を中心に回転してしまいます。これは、回転行列の公式上そうなるのですが、固定角回転の詳細原理についてWEB上でこの矛盾点について解説している資料は見当たりませんでした。回転行列、オイラー角については、総合的汎用的に解説している資料がなく、作者の目的に合わせて記号、用語とオイラー角定義を使っている使い方が多く、更に記載ミスがあったりして、初歩の独習者が単独で、学習できる資料が回転行列とオイラー角には見当たらないので、学習が進まないという壁にぶつかりました。しかし、姿勢角の表現方法としては、回転行列は基本中の基本なので、手間をかけて学習して良かったと思います。更にオイラーの法則として、剛体の一点周りの回転姿勢は、１軸の回転で表せるという原理原則は、全ての基本ですので学習できて良かったです。

※QIITAでもオイラー角の課題について、注意書きがありました。感謝です。
オイラー角に潜む５つの罠
 https://qiita.com/take4eng/items/0f5a9ff47fd345e5fc33

●目的
この４年間スキー板と人間の姿勢の計測での角度表現でGNSS　RTK技術をメインにやってきたのですが、RTKの機能で不足している部分をIMUで補うという開発を２０２３年はやってます。最近気づいたのですが、スキー滑走の場合は、地平面基準のオイラー角とかGNSS　Heading角では、真のスキー姿勢が得られません、なぜなら、斜面に沿ってスキー板の姿勢が決まるので、斜面を基準座標として、スキー板の姿勢を表現しないと、正確な姿勢角度がでない点で、GNSSとオイラー角では精度不足ということになります。斜度が大きければ大きいほど、GNSSのHeading角とオイラー角と実際のスキー板の方向で誤差が発生するので、斜面基準の座標系を新たに導入して表現することをSTA２４から実施していきます。そのためにもIMUの座標系を自在に使えるようにならないといけませんので、姿勢角表現の学習はあと１年くらい続けていきます。BNO055も内蔵アルゴリズムの癖と精度、センサの基本的精度的に、目的に沿うかどうか危ういレベルなので、別のＩＭＵも検討していきます。

●WEB上での解説記事は、一長一短で結局最後まで正解にいきつくものはありませんでした。
回転行列の解説記事たくさんあるのですが、実際にIMUから計算をしている記事無いです。
たくさんあっても、よくよく見ると、矛盾が必ずあって、完全な記事がないことが分ってきました。
これぞ絶対間違いないというのが皆無だということです。
何故、不完全な記事になるかというと、オイラー角が１２種類もあって、それぞれの名称とか計算式が違うので全部を検証する手間をかけている人はいないということで、皆さんご自分の用途を満たせるところまで
記述しているだけです。

●ＪＡＸＡの論文が教科書として一番信頼おけると感じてます。

ＪＡＸＡの昔の論文　宇宙相手なので、物体座標と慣性空間座標という表現です。名称定義がはっきりしていて
名称で迷うことが無いのがいいです。ＷＥＢ上の他の資料だと、名称、記号に重複、誤記、間違いやすい名称などがあって、オイラー角関係混迷してしますのですが、ＪＡＸＡの論文は、名称と内容の出典の信頼性が高いと感じました。
https://jaxa.repo.nii.ac.jp/records/44219

　クオータニオンとオイラー角によるキネマティックス表現の比較について

これを眺めていて分かったのは、回転行列とオイラー角を使うのは、航空宇宙の世界ではあまりお勧めでない
クォータニオンがこれからは便利だよということです。１９９１年の論文ですので、３２年前からクォータニオンを使って宇宙開発研究をしていたということです。

私の場合、クォータニオンは数値から図形が見えないので、回転行列とオイラー角で３次元グラフを回転させて理解しようと２週間やってきたのですが、だんだん回転行列の順序の面倒くささとオイラー角のややこしさを知るにつれクォータニオンが楽だよと言われてきた意味が分かってきました。以後は、オイラー角からクォータニオンへ
シフトする活動をしていきます。
●そうは言っても、回転行列とオイラー角の実体験をしてみる。
矛盾：この2週間、３面のグラフで回転を視覚的に理解しようとしてきたのですが、回転行列に矛盾があって、それを解説しているWEB記事がありませんでした。矛盾とは、固定角回転方式ですが、ZYX系で回すと、Z軸、Y軸は固定軸から回転するのですが、
最後のＸ軸になると、回転した結果のＸ軸を中心に回転する計算になってます。固定角方式でありながら、最後は、固定角でなくオイラー回転式で回しているという矛盾がありました。オイラー角と固定角回転の順序関係は、明確に教えてくれるサイトが無い点で、独習は難しいのではないかと思います。
＝＞しょうがないので、３面図で回転動作を目視判定する方法をやめて、逆行列からオイラー角を計算する方法で、実際に回転した角度が計算ででるか確認する方法に変更しました。
やり方：IMUで３水準の姿勢を作って、回転治具で１２０度区間を回転させて、データから
　　　　回転行列を作ってオイラー角を計算して、１２０度になる計算方法を探す。
EXCELマクロでやっていると融通がきかないので、EXCELシートで、全ての計算をします。
行列計算セル群とそこからオイラー角計算をするセル群を作ってコピペして、多種類の
行列計算を試行錯誤でやっていきます。

計算方法の組み合わせ：
オイラー角が１２種類もあるので、やりやすいXYZ系オイラー角を選定しました。
①IMUの角度データ
GYROの角速度を積分した角度データを20msecサンプリングログします。同時に、BNO055の
内蔵機能でのEuler角とQuarternionとGravityのデータもログします。
大事なのは、水平状態から電源オンして、IMUを所定の姿勢までもっていく過程もデータログして
最後に１２０度回転させるまで全部データをログします。

②回転行列の種類
こちらのhttps://www.sky-engin.jp/blog/rotation-matrix/記事から抜粋ですが、

これだけでは、どうしたらいいか判らないので、XYZ系回転行列をIMUの回転している座標を１，２，３個
測定データから拾って、行列の積を２個分とか３個分作ってそれらの逆行列を計算して、その要素からオイラー角を計算することを、組み合わせで何通りか繰り返して、オイラー角Z軸　yaw角が１２０度付近になる計算方法を見つけるという原始的な方法です。

●Excel計算
　①IMUのデータから動作節目の部分をきりとって座標１，２，３に割り当てて回転行列を作成

②Rx,Ry ,Rzから、Rxyz　とRxyzー１の逆行列を計算して、更に座標１，２，３の組み合わせ積を作る

Rx*Ry*Rzを作ってから逆行列計算する方法とRx-1*Ry-1*Rz-1と各回転行列の逆行列を作ってから積算する方法など、考えられる組み合わせ全部やってみました。更に、座標１，２，３の組合わせも上記逆行列の組合わせに
いれるので、十数個の組合わせで行列計算をします。

③オイラー角の計算間違いやすいので、備忘録しておきますが、

「IMUのGyro出力データは、センサ座標でのX軸、Y軸、Z軸周りの回転角速度なので
積分して、回転角度にして、ＲｘＲｙＲｚ回転行列を作って、Ｒｘ＊Ｒｙ＊Ｒｚで積を計算して
Ｒxyz回転行列を得ます。これは、センサ座標の角度を入力して、基準座標へ変換する行列です。
オイラー角は、基準座標（地平面）からセンサの姿勢を表現する角度ですので、基準座標のオイラー角を
入力して、センサ座標に変換する行列が必要です。この行列は、Rxyzの逆行列です。
オイラー角の計算は、センサの値から求めたRxyz行列の逆行列を計算して、その要素の値とxyz系オイラー角の
式の連立方程式で、X軸まわりのオイラー角Roll Y軸まわりのオイラー角Pitch　Z軸まわりのオイラー角yawを求めます。下記公式のR13=sinβはとセンサ値から計算した逆行列のR13の値＝sinβなので、β＝Asin(R13)で
求めれます。βがわかるので、下記式のR11=cosβcos γからγ＝Acos(R11/cos β）　cosβは上計算で既知です。
αもR33＝cosα　cosβからα＝Acos（R３３/cosβ）で判ります。

参考になったＷＥＢ記事　el-ment様　http://el-ement.com/blog/2018/05/19/euler-angles/

●結果
１１８度から１２５度までばらついた計算結果となりました。
BNO０５５の内蔵EULER出力のほうがTYPICALでは１２０度±２度程度ででてきますが、
TPOで悪くなるので手計算と内蔵アルゴリズムで迷うところです。

●考察
１２０度ドンピシャになりませんでしたが、GYROのノイズ、多分積分精度、行列計算精度、三角関数精度、計算開始位置などでおちてますが、１２０度±４度程度に収まってます。これだとBNO０５５の内蔵アルゴリズムと比較して悪いかもしれないので、後日、BNO０５５と手計算変換との精度比較やってどちらを採用するか決めます。