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

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

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

HiLetgo 3pcs GY-521 MPU6050 3軸加速度センサージャイロスコープモジュール

新品価格
¥1,130から
(2022/1/19 10:27時点)

スポンサーリンク

概要

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

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

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

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

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

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

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

動画解説

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

 3軸回転の場合は、行列式や四元数などを使ってベクトルを合わせて算出する必要があるのですか、これまた数学的理解がなかなか難解です(回転行列はそれほど難しくはないですがややこしいです。)。公式があるので使用するだけならなんとでもなりますが、今回は無理やりですが、近似的に3軸回転を考慮する算出方法で行いたいと思います。かなり無理やりな方法ですので参考程度で、、。本来であれば回転行列や四元数を使用してベクトル合わせで角度を算出する方法をお勧めします。

軸ベクトルのズレ

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

▼3DCadから角度を測定▼

 CADで作図しながら少しずつ実角度を測定してます。(3軸回転などは3DCAD使って検算すると非常にわかり易いです。よく使ってます・・。)測定している角度は、x軸廻りに30度回転後(y軸が水平面と30度の状態)でz軸回転を少しずつ加えた時の「水平面」と「x軸」との角度です。

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

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

▼正弦波と合わせると▼

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

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

 このような感じで正弦波に近い形で移り変わります。90度回転後にx軸、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) ;

 ここら辺も少し(かなり)無理やりでしょうか?・・。

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

少し横道

 少し横道ですが、上記のように邪道な感じで角度算出するのではなくきちんと回転行列などから行う場合は、例えば▼

\begin{align} R= \begin{bmatrix} 1 &0 &0\\ 0 &\cos\theta_x &\sin\theta_x\\ 0 &-\sin\theta_x &\cos\theta_x \end{bmatrix} \begin{bmatrix} \cos\theta_y &0 &-\sin\theta_y\\ 0 &1 &0\\ \sin\theta_y &0 &\cos\theta_y \end{bmatrix} \begin{bmatrix} \cos\theta_z &\sin\theta_z &0\\ -\sin\theta_z &\cos\theta_z &0\\ 0 &0 &1 \end{bmatrix} \\\\ \end{align}

▲このような行列式を基にベクトル回転をしたり、オイラー角を計算したりします。ここら辺はまたそのうちに機会があれば解説したいと思います。

 ただ、こういったジャイロからの角度算出(回転計算)には少し欠点があります。

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

ジャイロドリフトの確認

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

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

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

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

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

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

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フィルターはさすがです。特に問題なく角度算出している感じです。

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

▼最近(2022/4)撮り直しました▼

 少しは見やすいかと思います。この動画では、相補フィルターの角度算出はきちんと回転行列使ってます。カルマンフィルターはライブラリーの中身が単純な積算で角度計算しているため少しおかしな挙動してます。カルマンフィルターが悪いわけではありません。ジャイロ値単純積算では右のような挙動になってしまいます。

▼今回紹介した算出モデルを応用した、モデル、回路(ソースコード)、を頒布してます▼

▼今回の姿勢算出を応用した工作記事はこちら▼

スポンサーリンク
スポンサーリンク
スポンサーリンク

コメント

  1. あんぱん より:

    はじめまして。
    ジャイロセンサの勉強を行っているものなのですが、
    角度の検出には角速度のみでは誤差が多いことをこちらのブログを拝見して初めて知り、目から鱗です。
    1つお願いがあるのですが、madwickフィルターを用いた角度検出のソースコードを教えて下さることは可能でしょうか。

    急なお願いで申し訳ありませんが、宜しくお願い致します。

    • imo より:

      あんぱんさん、はじめまして。コメント有難うございます。
      ソースコードを教えることはなかなか難しく困ってしまいます。また私もmadgwickフィルターの中身はほとんどわかっていないためライブラリを使用しているだけとなります。もしArduinoをお使いでしたらライブラリがあるため確認頂いて、そのうえで具体的に質問頂けないでしょうか

  2. 匿名 より:

    記事を拝見させていただき、非常に参考になりました。ありがとうございます。1つ質問があるのですが、
    加速度センサーからはyaw回転の角度算出が困難であるとのことですが、確認動画の結果においてはyaw回転についても相補フィルターを通しているように思います。
    yaw回転の相補フィルターの式はどのようなものでしょうか?教えて頂けると幸いです。

    • imo より:

      匿名さん、こんにちは。コメント有難うございます。
       yaw回転の補正は残念ながら行っておりません。本ページにある動画の相補フィルターではジャイロ値を積算した値を使用してます。但し、センサー座標系と絶対座標系(ベクトルのずれ)は考慮して角度計算はしてます。動画内ではyaw回転のドリフトがたまたま目に見える程発生していないだけだと思います。

  3. 参考URL より:

    […] く丁寧に書かれている。 https://garchiving.com/calculate-angle-of-3axis-rotation-in-6axis-sensor/ https://garchiving.com/gyro-drift-correction/ […]

  4. Kokii より:

    いつもかなりお世話になっております。
    ありがとうございます。
    ひとつ気になったのでコメントさせていただきます。

    角度算出するときに、pi()/180をかけられていますが、
    そうすると単位は[rad]になると思いました。
    しかし表の縦軸は[degree]表記です。
    やはりdegree換算で正しいのでしょうか。
    恥ずかしいコメントで申し訳ないです。
    返信いただけたら幸いです。

    • imo より:

      Kokiiさん、サイト拝見下さり有難うございます。
      ご質問の件ですが、ご認識いただいている通りです。
      この手の三角関数計算は[rad]単位が一般的のため、計算式は[rad]換算してます。
      グラフは[deg]のが馴染みやすいため[deg]で表記しているだけです。
      かえって紛らわしかったですかね・・。

  5. tt より:

    はじめまして
    興味深く記事を拝見してます
    自分は今台車にこの6軸センサを付けていまして、傾斜角度に合わせてモーターの回転速度を変えようとしているのですが、台車が動いている時センサーの値が安定しないのですが、これは角度の計算の仕方がよくないからですか?madgwickフィルターを掛けているのですが、何か使い方を間違えているのですかね?
    よろしければご返事お願いします

    • imo より:

      ttさん、サイト拝見くださり有難う御座います。
      原因はいろいろ考えられるかと・・、
      加速度センサは動きの変化に弱いので加速度の重みを減らすとか
      もしくは電気的ノイズを回路が拾ってしまってるとか
      ひとつずつ確認していくことが近道かとおもいます。
      安定しない=ノイズ的なものではなく、値がずれているようなものであれば計算の仕方かも。

  6. MPU-6050で何かやってみたい より:

    とても分かりやすいMPU-6050で色々とやろうとしているものです.
    中々に興味深い記事で勉強になりました.

    端的に質問させて頂きます.
    「ジャイロドリフト」という単語で私が混乱しているのかもしれませんが,

    ・ジャイロの値を使って”姿勢角を出す際に”ドリフトが発生する.
    ・ジャイロの値を使って姿勢角を計算する際の積算の過程において”誤差の積算もしてしまうために”ドリフトが発生する.

    という理解で良いのでしょうか?
    単にジャイロの各軸で得られる値を個別利用する際には,積算する訳ではないのでドリフトを気にする必要がないと思って良いのでしょうか?
    「ジャイロドリフト」と聞くと,ジャイロそのものに問題があるように思えてしまいます.

    • imo より:

      サイト拝見下さり有難うございます。独学なのでほんとのところはよくわかってませんが、私もジャイロそのものに問題があるのではなく、角度算出に積分を行っているため、その計算方法(積算)により発生する誤差と認識してます。徐々に真値からずれていくものと認識してますでジャイロ値をそのまま角速度(rad/s)として利用する場合や、短期的な角度算出であればドリフト誤差は発生せず、単にずれは測定誤差(精度)と考えていいと思います。

      • MPU-6050で何かやってみたい より:

        ご回答頂きありがとうございます.

        ジャイロ値をそのまま角速度(rad/s)として利用する分には,問題無さそうですし,ジャイロ値を使って色々とやってみたいと思います.

        imoさんの記事「Arduinoから MPU6050の値を取得してみる」
        で,新たに質問させて頂いたのでご回答いただけると幸いです.

        Gyro設定におけるFS_SEL,加速度設定におけるAFS_SEL,LPF設定の43[Hz]はどのようにして調整するのかを教えて頂きたいです.
        (「アドレス」や「レジスタ」などがよく分からないので.)

        • imo より:

          新たな質問が送信されていないようです。

          • MPU-6050で何かやってみたい より:

            以前の質問は自身で解決しました.
            それとは別に質問があります.
            ローパスフィルタのレジスタマップの読み方についてです.

            「Arduinoから MPU6050の値を取得してみる」の記事で,ローパスフィルタの設定をしておられましたよね?

            ここで質問です.
            表示していたレジスタマップによると,DLPF_CFG1〜6ではジャイロセンサのFs(kHz)が1になる.というように書いてありますが,これは,「ローパスフィルタを使用するとジャイロセンサのサンプリング周波数が1kHzになると思って良いんですか?」

          • imo より:

            いつもサイト拝見下さり有難うございます。
            私もその認識でいいかと考えてます。

          • MPU-6050で何かやってみたい より:

            もう一つ質問させてください.

            「Arduinoから MPU6050の値を取得してみる」の記事の後半の”ソース全体”で,ローパスフィルタの設定アドレスを[0x05]にしてありますが,この場合も角速度のサンプリング周波数は1kHzになるんでしょうか?
            レジスタマップのbandwidth,delayが何を意味しているのか分かりません.
            ご存じでしたらご教授願います.

          • imo より:

            いつもサイト拝見下さり有難う御座います。
            1KHzになると思います。(詳細はデータシート確認下さい)
            また、帯域幅、立ち上がり時間のことかと思います。
            できれば、今後は内容に合わせてその投稿ページにてコメントを頂けますでしょうか。