クオータニオンの勉強

2021年02月23日

カテゴリ： Arduino

ずっと、続けている、MPU関係ですが、ちょっと問題があって・・・。

あまり気にせずセンサをマウントしたのですが、MPUの出力（センサ）のXYZ軸が、
機体に搭載したときのXYZ軸と違うというか・・・。

で、MPUの出力に座標変換をかまして、センサのマウントの問題を処理しようとしたのですが、
MPUの直接の出力は、クオータニオンなんです。

で、ここ、とか、ここで勉強したのですが、やりたいことには繋がらず、
ここ

つまり、姿勢＝回転をあらわすクオータニオンに更に座標変換＝回転をおこなうためには、クオータニオンの掛け算を使う。
ある点＝ベクトル（長さは単一でもない）を回転させるためには、さらに共役クオータニオンを掛けるけど、これは必要ない。

さらに、落とし穴だったのが、最初に回転したとき、実は座標軸も一緒に回転しているので、もう一度回転させる（掛け算する）
ときには、一緒に回転した座標軸を元に回転させる必要があるという・・・。
分かりにくい表現ですが、ここのアニメーションを見てもらうとわかると思います。
オイラー角とかの回転処理が頭にあると、座標軸まで動いているって考えがないので、結構、試行錯誤して、意味不明で、混乱していました・・・。

よって、今回の機体のセンサマウントの方向からすると、
MPUの出力結果（クオータニオン）に、その結果回転した座標軸基準に、X軸回りに90度回す（クオータニオンを掛ける）だけでOKでした。
クオータニオンの作り方はここがわかりやすかった。

qx =int(( S*q1 +0*q2 +0*q3 +S*q0 ) * 16384.0f);

qy =int(( 0*q1 +S*q2 +S*q3 +0*q0 ) * 16384.0f);

qz =int(( 0*q1 -S*q2 +S*q3 +0*q0 ) * 16384.0f);

qw =int((-S*q1 +0*q2 +0*q3 +S*q0 ) * 16384.0f);

また、今回は、右から掛ける方を行列にする計算を使っています。
それ以外にも、配列上で、qwが[0]に入る場合や、[4]に入る場合など、作法がいろいろあって
このあたりもややこしいのがクオータニオンです。作法の違いは、ここ参考。

で、その結果を、もう一度、プロセッシングで飛行機を飛ばして確認したのですが、
ヨー軸に初期値があるとわかりにくいので、下記のとおりオイラー角でアニメーション処理して、
ヨー軸を殺して、serialEvent()のオイラー角計算部分を復活させると、
ヨー軸に無関係に、X-Y軸回りにのみ傾く飛行機が表示されました。
結果は、OK。これでまた先に進めます。

// rotateY(-ypr[0]);

rotateZ(-ypr[1]);

rotateX(-ypr[2]);

// float[] axis = quat.toAxisAngle();

// rotate(axis[0], -axis[1], axis[3], axis[2]);

お気に入りの記事を「いいね！」で応援しよう