日記

フリーページ

2021年02月23日
XML
カテゴリ: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秒


PR

キーワードサーチ

▼キーワード検索

プロフィール


nisiken2002

お気に入りブログ

まだ登録されていません

© Rakuten Group, Inc.