|
カテゴリ:Arduino
ずっと、続けている、MPU関係ですが、ちょっと問題があって・・・。
あまり気にせずセンサをマウントしたのですが、MPUの出力(センサ)のXYZ軸が、 機体に搭載したときのXYZ軸と違うというか・・・。 で、MPUの出力に座標変換をかまして、センサのマウントの問題を処理しようとしたのですが、 MPUの直接の出力は、クオータニオンなんです。 で、ここ、とか、ここで勉強したのですが、やりたいことには繋がらず、 ここを見て、やっと理解できました・・・。 つまり、姿勢=回転をあらわすクオータニオンに更に座標変換=回転をおこなうためには、クオータニオンの掛け算を使う。 ある点=ベクトル(長さは単一でもない)を回転させるためには、さらに共役クオータニオンを掛けるけど、これは必要ない。 さらに、落とし穴だったのが、最初に回転したとき、実は座標軸も一緒に回転しているので、もう一度回転させる(掛け算する) ときには、一緒に回転した座標軸を元に回転させる必要があるという・・・。 分かりにくい表現ですが、ここのアニメーションを見てもらうとわかると思います。 オイラー角とかの回転処理が頭にあると、座標軸まで動いているって考えがないので、結構、試行錯誤して、意味不明で、混乱していました・・・。 よって、今回の機体のセンサマウントの方向からすると、 MPUの出力結果(クオータニオン)に、その結果回転した座標軸基準に、X軸回りに90度回す(クオータニオンを掛ける)だけでOKでした。 クオータニオンの作り方はここがわかりやすかった。 もともとのスケッチの、printIMUData()の変更箇所はこんな感じです。 #define S 0.707106781 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); 0部分は書く必要はないのですが、理解しやすいために残してあります。 また、今回は、右から掛ける方を行列にする計算を使っています。 それ以外にも、配列上で、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]);
最終更新日
2021年02月23日 21時07分13秒
|
|