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

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

 

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

 

▼確認動画▼

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

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

 

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

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

 

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

 

<スポンサーリンク>

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

 

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

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

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

 

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

097_03
このような式でフィルターの係数を可変します。c:適当な係数、a:各軸の加速度ベクトル和。2乗は4乗でも問題無いと思います。

 

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

 

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

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

 

 

<スポンサーリンク>

この投稿へのコメント

コメントはありません。

コメントを残す

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

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

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

トラックバック URL