6軸センサーから3軸回転の傾き角度算出とドリフト補正方法

　6軸センサー（MPU6050）から、3軸回転を考慮した傾き角度（姿勢角度）の算出方法、補正方法（ジャイロドリフト補正）はいろいろあると思いますが一例を解説したいと思います。（ここら辺りの内容は散発して記事を投稿していたのですが、一度纏めたいと思います）

　使用する6軸センサーはMPU-6050、マイコンはArduinoUNOです。

HiLetgo 3pcs GY-521 MPU6050 3軸加速度センサージャイロスコープモジュール6自由度6軸加速度センサージャイロセンサーモジュール16ビットAD変換器Arduino用データ出力IIC

概要

　MPU-6050ではジャイロ（3軸角速度）、加速度センサー（3軸加速度）の値を取得できます。ジャイロのみでも加速度センサーのみでも角度算出はできるのですが、センサー毎に特徴がありその特徴を補う形で補正を行い、より正確な姿勢角を導きます。

　センサー値の補正には便利なフィルター、カルマンフィルターやMadgwickフィルターなどがあります。言語ライブラリも存在し使用するだけなら比較的簡単なのですが中身が私とっては非常に難解です。ブラックボックス的なものはあまり使用したくないということもあり今回は使用しないで姿勢角を算出・補正したいと思います。

　ではまずジャイロ値（角速度）からの角度算出方法とのその特徴です。

ジャイロ（角速度）からの3軸角度算出方法と特徴

MPU-6050の使い方
角速度から角度の算出方法
はそれぞれ別の投稿記事で解説してますのでそちらを参考にして下さい。

　ジャイロからの角度算出ですが、センサーから取得した角速度を時間で積分すれば角度となります。1軸回転のみを検出するのであれば単純な積算でも算出できるのですが、3軸回転となると少しややこしくなります。

　センサー軸自体が傾いてしまうため、そのベクトル（センサー軸の傾き）を考慮する必要があります。

動画解説

　（わかり難いでしょうか）上図のような回転があった場合、最終的には姿勢角は水平になるはずなのですが（z軸回転のみ残る）、単純な積算ではx軸、y軸ともに角速度からの積算値が残ってしまうため、おかしな算出値になってしまいます。

　３軸回転の場合は、行列式や四元数などを使ってベクトルを合わせて算出する必要があるのですか、これまた数学的理解が私にとっては非常に難解です（回転行列はそれほど難しくはないですが頭が痛くなります・・。）。公式があるので使用するだけならなんとでもなりますが、今回は無理やりですが、近似的に3軸回転を考慮する算出方法で行いたいと思います。

軸ベクトルのズレ

　まず、x軸回転後にz軸回転を加えた場合、x軸の傾きがどのように変わっていくのかを3DCadで作図しながらその角度を測定してみました。

▼3DCadから角度を測定▼

　このような感じでCADで作図しながら少しずつ実角度を測定してます。（3軸回転などは3DCAD使って検算すると非常にわかり易いです。よく使ってます・・。）測定している角度は、x軸廻りに30度回転後（y軸が水平面と30度の状態）でz軸回転を少しずつ加えた時の「水平面」と「x軸」との角度です。（3DCadはフリーのFusion360使用してます。AutoDeskのギャラリーに作品をアップしてます。宜しければご覧ください）

▼z軸回転角度とx軸の傾きの移り変わりをグラフにすると▼

　こんな感じです。正弦波っぽい移り変わりをします。

▼正弦波と合わせると▼

　微妙に違いますが概ね合います。（ここら辺が少し無理やりです）

▼z軸回転とy軸傾きも▼

　このような感じで正弦波に近い形で移り変わります。90度回転後にｘ軸、y軸の傾きは入れ替わります。

無理やりベクトル合わせ

　ということで単純に積算したx軸、y軸の傾き角度に、「z軸回転角度分の正弦波」を加味させていけば3軸回転（ベクトルの移り変わり）を考慮した傾き角度が算出できるはずです。

　▼言語風に描くと、▼

roll  += gx * dt;   
pitch += gy * dt;   
yaw   += gz * dt; 

　まずジャイロ値（角速度）から3軸の回転角度を単純に積算（時間積分）します。

　▼その後、z軸回転（yaw回転）分の移り変わりを加味します▼

roll  += pitch * sin(gz * dt * pi()/180) ;   
pitch -= roll * sin(gz * dt* pi()/180) ; 

　ここら辺も少し無理やりでしょうか？・・。

　このような感じでジャイロから傾き角度（姿勢角）を算出します。当然、回転行列や四元数使った方が正確かと思いますが・・。

　またジャイロからの角度算出には少し欠点があります。

　角度算出に積分を使用しているため、センサー誤差やノイズなどをどんどん積算してしまい角度がずれていってしまいます（ジャイロドリフト）。

ジャイロドリフトの確認

　このグラフではジャイロドリフトを確認してます。センサーを水平に置いたまま、ジャイロ値を積算し角度を算出しているのですが、時間が経つにつれて角度が発生します。誤差やノイズなども積算してしまうため発生する現象です。（発生具合は個体差や環境によります。）

　このずれ（ジャイロドリフト）を補正する必要があるのですが、補正を行うために加速度センサーの値を使用します。

加速度センサーからの角度算出方法と特徴

加速度センサーからの角度算出方法

はこちらで詳細説明してます。

　まず加速度センサーからの取得値からの傾き角度ですが

acc[PITCH] = atan2(ax, sqrt(ay * ay + az * az)) * pi()/180; 
acc[ROLL]  = atan2(ay, az) * pi()/180;

　いろいろと算出方法があると思います。計算の一例です・・・。

　加速度センサーからはz軸廻り（yaw回転）の角度は算出は困難です。また加速度センサーは感度が良すぎるというのか、センサーの加減速を拾ってしまうため値が結構暴れます。またセンサー自体に加減速がある場合には計算される角度がおかしくなってしまいます。

　こういったことを考慮に入れて使用する必要があります。

各センサーからの角度算出まとめ

　ジャイロからでも加速度センサーからでも傾き角度（姿勢角度）は算出できるのですが、それぞれの特徴があり、補正を行ってより正確な傾き角度を算出します。

◆ジャイロの場合

• 3軸回転がある場合は単純な積算ではなくベクトルを考慮する必要がある。
• 誤差が蓄積して角度が少しずつずれていく。（ジャイロドリフト）

◆加速度センサーの場合

• z軸廻り（yaw回転）の角度算出が困難（できない？）
• センサーが加減速しているときには正確な角度算出は困難
• ドリフトは無い

それぞれの特徴を映像で確認

　左側が加速度センサーのみから角度計算したものです。センサー自体に加減速が加わるとおかしな角度検出となってます。

　右側がジャイロのみから角度算出したものです。この映像では今回紹介したz軸廻り（yaw回転）の補正は加えてません。ですのでz軸に回転が加わると角度（ベクトル）がずれてしまいます。またさらにドリフト補正も行っていないため、どんどん角度がおかしくなります。ただセンサー自体の加減速の影響は受けていません。

ドリフト補正方法

　ここで2つのセンサーから、より正確な角度を算出するための補正を行います。基本的にはジャイロで角度算出し、加速度センサーでジャイロドリフトを補正する方法です。

roll = 0.995 * (roll + gx * dt) + 0.005 * rollAccel

　相補フィルター（簡易？）です。加重平均をとっているような（いいとこ取り？）みたいな感じ、加速度センサー側の値を小さくすればするほど微細な動きの影響は無視できますが、補正が遅くなる感じでしょうか。ジャイロ、加速度のどっちに重みを持たせるかってとこでしょうか。

ドリフト補正の確認動画

　いろいろな方法で角度算出したものを比較してみました。

▼確認動画▼

　左から順に、加速度センサーのみ。ジャイロのみ。相補フィルター（本記事で紹介した方法）。madgwickフィルターとなってます。（動画がコマ落ちしてますがすみません）

　加速度センサーはz軸（yaw回転）回転は可視化してません。動画途中でセンサーを揺すってますが、加減速が発生すると角度がおかしくなってます。

　ジャイロのみからの算出（ここではz軸回転の補正を入れてます）は割といい感じで姿勢角を算出できてますが、動画最後にはドリフトが発生していて、水平に戻っていません（少しわかり難いですが）。

　簡易相補フィルター（本記事で紹介した方法）ではz軸回転（yaw回転）時に動きが少し微妙で、角度も少し遅れて補正する感じでしょうか（係数小さすぎ？）。目的によってはこの方法でも十分使えそう？です。

　madgwickフィルターはさすがです。特に問題なく角度算出している感じです。中身は全くわかませんがライブラリ使うだけなら簡単です。

　長文となってしまいました。ここまで読んで頂きなんですが特にこだわりがなければ、きちんと四元数や回転行列使ってベクトル算出するのが無難かと・・・。

Arduinoでドローン（クアッドコプター）の自作