ジャイロのドリフト補正を改良

 ジャイロのドリフト補正方法を改良してみました。センサー自体に加速度が発生している場合は、単純なドリフト補正ではうまく角度を検出できません。今回は簡易相補フィルターを改造して対応してみました。

 

 まず、センサー自体に加速度が発生している場合どうなるのか。加速度センサー及びジャイロからそれぞれで角度算出した場合の特徴を見てみました。

 

▼確認動画▼

 左側が加速度センサーから角度算出したものです。センサー自体に加速が発生すると、検出角度に影響します(ゆらゆら揺れてしまいます)。角度計算に加速度値を使用しているため、各軸から検出される加速度の合成ベクトル和が≒1G(重力加速度のみ)でないと、うまく角度が算出できません。(とりこぼしているだけでしょうか。制御周期や感度の設定であまり検出されない場合もありました)

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

 

 右側がジャイロから角度算出したものです。センサー自体が加速しても角度検出はできています。がしばらくすると大きくドリフトが発生しています。

ジャイロから角度算出方法
ジャイロドリフト補正

 

 ジャイロのドリフト補正には、カルマンフィルターや相補フィルターを使用していたのですが、両フィルターとも加速度センサーからの値でドリフト補正を行うため、先ほどのように、センサー自体に加速が生じた場合に検出角度がおかしなことになってしまいます。

 

 今回は簡易相補フィルターを改良して、センサー自体が加速している場合でも角度検出が正確にできないかを行ってみました。

<スポンサーリンク>

 

 まず、簡易相補フィルターですが、

  angle = 0.95 * (angle + gx * dt) + 0.05 * angleAccel

ジャイロからの角度積算値と加速度センサーからの角度算出値を重み付けするような計算となります。ここで加速度センサーの重み成分をセンサー自体の加速に合わせて可変できように式を少し弄ります。

 

 重み付けしている係数を加速に合わせて可変できるようにします。


このような式でフィルターの係数を可変します。(式が間違っていたので修正しました2017/6/13)

 c:適当な係数、a:各軸の加速度ベクトル和。2乗は4乗でも問題無いと思います。

 

097_02
 このような感じで、加速がかかる場合は、加速度センサーの重みを無くして、加速がない場合(1Gに近い場合)にドリフトを補正する感じです。

 

▼実際に試した動画です▼

 右がカルマンフィルター、左が改造したフィルターです。センサー自体を揺らして加速させてみても改造したフィルターの方ではおかしな角度検出はしていません。

 

2017/9/08:追記
 ただ、カルマンフィルターはライブラリを使用。内容が難解でまったくわからないため、係数はデフォルトのまま使用してます。今回実施した改造のフィルターは、加減速が発生していない場合にドリフト補正を実施するような物ですので、常に加減速が掛かる装置や振動(ノイズ)等がある場合にはあまりドリフト補正が働かないかい可能性もあるかも・・・。カルマンフィルターは一般的にノイズに強いとも聞いてます。

 

<スポンサーリンク>


この投稿へのコメント

  1. mochy said on 2017-06-13 at 11:59

    大変参考にさせていただいております。
    感謝です。
    説明と結果が当方の知識不足から補完できないことも、多々有りまして。。
    ソースコードとか公開してもらえませんか。。
    ご検討下さい。

    ところで、
    文中式
    k=0.05*(1/(c-c*a)^2)
    に対して
    グラフではa=1 のときk=0.05になっていますが、
    a=1のときkは無限大に発散しますよね。

    • imo said on 2017-06-13 at 13:05

      mochyさん、コメント有難う御座います。文中式、申し訳ありません。確かに発散します。間違っていたので修正しておきました。k=0.05*(1/(1+(c-c*a)^2))が正です。

      • mochy said on 2017-06-13 at 14:58

        ありがとうございました。
        ソースも是非。。。

  2. imo said on 2017-06-13 at 19:55

    この記事の動画に使用したソースがそのまま残ってましたのでアップしておきます。
    Arduinoソース
    Processingソース
    暫くの間アップしておきますので参考にして下さい。
    使用しているのはArduino/Genuino101です。6軸センサーは内蔵を使用。z軸(yaw軸)のドリフト補正はしていないので注意して下さい。ソースではカルマンフィルターと改造した相補フィルターの両方で角度算出してます。Processingも参考にアップしておきます。検討していたソースのため少し見難いですがすみません。頑張って下さい。

    • mochy said on 2017-06-14 at 11:49

      ありがとうございます。
      大変お手間をお掛けしました。
      感謝致します。

  3. Hiro-H said on 2017-09-06 at 16:31

    傾斜センサをいろいろ検討していてここにたどり着きました。

    現在MPU6050を用いていろいろ実験しておりますが、
    このページの補正関数入り相補フィルターがなかなかうまく動作しません。
    傾斜のみをセンシングしたいのですが、どうしても装置自体の水平方向の加速度を
    拾ってしまいます。

    そこで、教えて欲しいのですが、このページの動画にある相補フィルターの定数を
    参考までに教えていただけますでしょうか。

    ちなみに、当方の定数は、
    angle_x = 0.995 * (angle_x + Gyro_x * dt) + 0.005 * k * Accel_x
    dt = 0.02(20ms)
    k = (1 / (1 + (c – c * a) ^ 4))
    a = Accel_x ^ 2 + Accel_y ^ 2 + Accel_y ^ 2
    c = 32
    radianで水平に近い傾斜のみなので、加速度Gをそのままradian読みしています。

    よろしくお願いいたします。

    • imo said on 2017-09-06 at 20:27

      Hiro-Hさん コメント有難う御座います。
      相補フィルターの係数ですが、ジャイロ及び加速度からの算出角度の両方に効くようにする必要があります。ですので
      angle_x = 0.995 * (angle_x + Gyro_x * dt) + 0.005 * k * Accel_x は

      angle_x = (1-k) * (angle_x + Gyro_x * dt) + k * Accel_x
      k = 0.005*(1 / (1 + (c – c * a) ^ 4))
      とこのようにした方が良いかと思います。
      あと全体の加速度算出はルートで括る必要があります。
      a = sqrt(Accel_x ^ 2 + Accel_y ^ 2 + Accel_y ^ 2)
      その他の定数は、
      dtはすみません。ポーリング制御で実測値代入していたため不明です。たいした制御していないためたぶん3ms程度だと思います。
      cは「1000」です。ここら辺は装置の加振具合やフィルターを効かせたい範囲で変わってくると思います。(完全に加速の影響を取り除くのは厳しいと思います)

  4. ma said on 2017-09-08 at 15:21

    コメント失礼します。
    加速度補正を行っているのですが、こちらのサイト大変参考になりとてもありがたいです…。
    簡易相補フィルタの式や今回の重みづけしている係数の式の参考文献等ありましたらぜひアップしていただけないでしょうか。
    よろしくお願い致します。

    • imo said on 2017-09-08 at 15:53

      maさん 参考にして頂き有難う御座います。この手の内容は紹介されているサイトは少ないですよね。情報集めるのに私も苦労しております。
      簡易相補フィルターついてはここで情報を入手しました。ページ中ほど以降で参考文献へリンクしてます。
      https://sites.google.com/site/nekolabwiki/electronic/posturecontroll
      (サイト管理者様に感謝します)

      重み付けしている係数の可変に関しては、アイデアを頂き数式化しただけですのでこれといった情報は他ではわからないです・・。

  5. ma said on 2017-09-08 at 16:31

    相補フィルタやカルマンフィルタは内容が難しい上、サイトも少ないので苦心していたところこちらのサイトを見つけることが出来たのでとても感謝しております…。
    URL載せていただきありがとうございます!
    そちらのサイトの論文も読んで参考にさせていただきます。
    重み係数はご自身で数式化されたものだったんですね…!わかりました、ありがとうございます。
    質問ばかりで申し訳ないのですが、簡易式のgxは角速度計測時の生データでしょうか?
    生データの角速度積分値は物理的意味を持たないと指摘され、gxをdθ(オイラー角やロールピッチヨー角の微分値)に変換するような式を求めなければいけないのかと悩んでいるところなのです。。。

    • imo said on 2017-09-08 at 16:55

       記事中の動画に関してはgx、gyは単純積分値を使ってます。おっしゃる通り姿勢角を検出する装置、またはその数値の利用方法によっては、各軸の生データの単純積算では全く意味をなさない場合があります。(動画内ではyaw軸に廻さないようにしてます)

       3軸回転があるのであれば軸自体が回転してしまうため、オイラー角や四元数による角度算出は必要かと思います。コメント欄での説明は厳しいのでよろしければ次回にでも簡単な説明の投稿をしたいとおもいます。(記事最後に少し補足内容追加しました)

  6. ma said on 2017-09-08 at 17:12

    やはり三軸回転の場合はオイラー角やクォータニオンを計算しないといけないんですね…。
    次回のご投稿お待ちしております!
    丁寧にご教示くださりありがとうございました!

コメントを残す

メールアドレスが公開されることはありません。

この投稿へのトラックバック

トラックバックはありません。

トラックバック URL