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

Pocket

arduino sheild 3Dモデル

 

 ジャイロのドリフト補正を改良してみました。6軸センサー(ジャイロ、加速度センサー)から姿勢角を算出する際、ジャイロドリフトを加速度センサーから算出した姿勢角で補正してなるべく正確な姿勢角を算出すると思います。

 

 ただセンサー自体に加減速がある場合、加速度センサーからの姿勢角算出がおかしくなってしまいます。

 

▼確認動画▼

 センサー値をProcessingを使用して可視化してます。左が加速度センサーのみから算出した角度、右がジャイロから算出したものです。

 

 センサー自体を揺らすと加速度センサーの方はおかしな角度となってしまいます。センサーの加速度を拾ってしまうためです。(加速度センサーから角度算出

 

 ジャイロの方は3軸ベクトルを合わせていない(z軸回転)のとドリフトが発生しているため最後にはかなりおかしな角度となってます。(6軸センサーから角度算出

 

 今回はこの加速度の影響をなるべく軽減できないかを相補フィルターを改良して試してみました。

 

 まず普通?の相補フィルターですが

補正後角度 = 0.9 * (ジャイロセンサーからの角度) + 0.1* (加速度センサーからの角度)

  係数は適当です。それぞれの算出角度に重み付け平均しているイメージです。

 

<スポンサーリンク>

ここでセンサー自体に加減速が働いた場合、係数によっては、加速度センサーに影響が出てしまい、補正角度にも影響が出てしまいます。

 

 そこで重み付けしている係数をセンサーの加速度に合わせて可変できるように式を少しいじります。

補正後角度 = (1-k) * (ジャイロセンサーからの角度) +  k * (加速度センサーからの角度)

このような感じにして、 kの式を

こんな感じにします。αはセンサーにかかる加速度、cは適当な係数です。その他の定数も適当です。(正規分布の公式をいじってます)

 

▼グラフ化すると▼

 このような感じです。センサーの加速が1Gに近いときにドリフト補正を効かせて、加速が掛かっているときには重みを減らしている感じです。式の定数いじれば山なりの具合は変更できます。

 

 こんな感じで補正したものの効果を確認してみました。

 

▼確認動画▼

 左が改良したフィルター、右がカルマンフィルターです。

 

【捕捉】(誤解を生むといけないため)
 カルマンフィルターはネット上に転がっていたライブラリを使用。中身がいまいちわからないため、係数はすべてデフォルトのままです。ですので、係数いじれば恐らくもっと加速の影響を消せると思います。

 また普通?の相補フィルターでも重み付けの係数いじれば、この動画くらいの加減速であれば十分加速の影響を取り除けることは確認してます。ですので正確には本記事で紹介した方法がほんとに加速の影響を消せているかの確認はできていません・・・。

Pocket

<スポンサーリンク>


この投稿へのコメント

  1. 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軸回転があるのであれば軸自体が回転してしまうため、オイラー角や四元数による角度算出は必要かと思います。コメント欄での説明は厳しいのでよろしければ次回にでも簡単な説明の投稿をしたいとおもいます。

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

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

  3. m said on 2017-11-05 at 00:22

    研究で現在非常に困っているところでとても参考になっております!
    丁寧な記事ありがとうございます。
    3軸回転を考慮したときの説明を是非ともお願いしたいです。

    宜しくお願い致します。

  4. tm said on 2018-02-09 at 16:10

    以前このドリフト補正の記事を見かけた時、興味深く拝見しておりました。
    また見に来てみたのですが、削除されてしまっていたのですね・・・
    もう公開されることはないのでしょうか?

    • imo said on 2018-02-09 at 19:28

      tmさん、コメント有難う御座います。
      また記事を閲覧にきて頂いたのに削除しており申し訳ありません。

      その後いろいろ検証を続けておりまして・・、ケースバイケースかと思いますが本記事の内容が悪影響する場合もあり、また相補フィルターも結構優秀で、、、、本記事内容程のことをしなくてもいい場合もあり・・。

      誤解を生むといけないため削除しました。私の理解が不十分であり、ご迷惑をおかけ致します。
      今のところ再公開の予定はありません。もし知りたい内容があるようであれば「問い合わせ」フォームからご連絡頂ければ可能な限り回答したいと思います。

      • tm said on 2018-02-09 at 20:42

        返信ありがとうございます。
        独自に両センサの重み付けの処理を行っていて、センサに加速が働く中でもかなり正確に角度の検出ができていたように記憶しております。
        削除に関してはお考えあってのことでしょうから、どうかお気になさらず。強いて気になることと言えば、あのような重み付けを行う発想にどのようにして至ったのかが少し気になるくらいでしょうか。

        今後の投稿も楽しみにしております。

        • imo said on 2018-02-10 at 00:03

          重み付けの可変についてですが、
          最初、加速があまり無い(例えば0.95G<a<1.05G)ときのみドリフト補正して、、加速中はジャイロのみで角度検出してといった具合のアルゴリズムで考えてまして(補正のON、OFFのみです。)、、、。ジャイロのみでもそれなりに角度検出しますし・・ずっと加速し続ける装置もそうそうないですしね。
          そののちに、1/abs(1-a)で重み付けを加速に反比例させたりしてました。(この場合は1Gで式が発散するので1Gの時は除外)この辺が発想のもとです。ただ本記事にあった加速程度(手で揺する程度)であれば普通の相補フィルターでも加速の影響がほとんどでませんでした。
          ここの投稿の一番最後の動画途中で同様にセンサー揺すって加速させてますが、相補、madgewickフィルターともにほとんど加速の影響を受けてません。このときに気付いたのですが重み付けの可変が効いている(た)のか怪しくなったのです。そもそも相補フィルターで十分なのにもしかして余計なことしてる?
          ここら辺がいったん非公開にした理由です。
          長文すみません。今後ともサイトを宜しくお願いします。
          追記:文面変更と捕捉を追記して再公開しました。

コメントを残す

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

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

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

トラックバック URL