ドローンの自作｜Arduinoで姿勢角の算出

ジャイロから姿勢角を算出する場合、各軸のから測定された角速度を積算していけば軸廻りの角度が算出できます。（角速度から角度の算出方法）

　ですが、単純に各軸の積算を行うだけではドローンの姿勢角としてはおかしなことになってしまいます。

▼各軸を積算しただけの角度算出▼

　ジャイロから各軸をそれぞれ積算しただけの角度（3軸）と加速度センサーから算出した角度（加速度センサーではｚ軸廻りの回転算出は困難です）を可視化してます。（見難い動画ですみません）

　ジャイロでは少しドリフト発生、加速度センサーでは振動に敏感な反応をしてしまってますがここではスルーして下さい。

　動画ではまず、roll軸（x軸）で30°程回転してます。その後、yaw軸（z軸）に90°程回転を加えてます。

　回転後だけを見れば、roll、pitchの角度が入れ替わるのが本来かと思いますが（飛行機でいうとノーズアップしていたのが両翼の傾きに変わる）、ジャイロからの角度算出ではroll、pitchの角度に変化は無いです。（加速度センサーではちゃんと算出してます。）

　z軸回転ため、x,y軸の角速度は発生しないためです。

　ですのでジャイロから姿勢角を算出するためには、センサー軸自体が回転しているため、ベクトルを合わせる必要があります。便利な関数、オイラー角、回転行列やクォータニオン等を利用する必要があります。

　ただ、これらは公式を使用するだけならなんとかなりそうなのですが、数学的な理解が私にとっては非常に難解です。

　なるべく単純でわかり易いアルゴリズムで構築したいと考えているのですが、中身がブラックボックスだと、その他の問題発生時に対処が困りそうです・・・。

▼クォータニオンを使ってみました▼

　ジャイロからでもベクトルの向きをきちんと算出しているようです。