注目キーワード
  1. Photoshop
  2. Python
  3. Raspberry Pi
  4. Arduino

BMX055から取得したセンサ値を相補フィルタで補正&Pythonで実装してみた

ここでは「Raspberry Pi:ラズパイ」と「BMX055」を使って取得した角速度の値を利用した相補フィルタについて紹介します。

 

この記事を読むことで

相補フィルタとは何か

相補フィルタを実装するPythonプログラム

相補フィルタの精度検証

を知ることができます。

 

また、この記事は下記記事であるラズパイとBMX055からセンサ値を取得する方法の続きとなっているため、Pythonプログラムの説明は下記よりご覧ください。

ラズパイでBMX055の使い方と地磁気などのセンサ値を取得してみた

 

センサ値から取得したデータを相補フィルタで補正したい方のお役に立てれば幸いです。

それではさっそく始めていきましょう。

相補フィルタでセンサ値を補正する

相補フィルタの理論式

まずは相補フィルタとは「センサフィージョン」(様々なセンサの値をまとめること)の1つ

取得した加速度、角速度の値を利用して角度を補正する

フィルタと考えてもらえば良いかと思います。

 

そもそもなぜフィルタを使用するかというと、取得したセンサ値の誤差を最小限に抑えるためであり、フィルタをかけることでより精度の高いデータを扱えるためです。

 

相補フィルタの理論式は一般的に、求めた角度θn+1、ひとつ前に求めた角度θ、フィルタ定数k、加速度の値から求めた角度θaccl、各軸の角速度の値Gyro、角度算出までの時間dtとして以下の式で表すことができます。(下記URLの理論式を参照)

θ以降のRoll、Pitch、Yaw角は著者自身が相補フィルタの理論式を元に書き直した式になります。(間違いがある場合はご教授いただければ幸いです。)

また、Yaw角への相補フィルタ導入は理論式を元に式をいじくったので、相補フィルタかと言われると不明です(-ω-)/

bmx055-cpfilter-01

Philip Salmony:IMU Attitude Estimation

 

今回は、この理論式を元に相補フィルタを実装し、BMX055から取得したセンサ値の値を補正していきます。

 

相補フィルタを実装するPythonプログラム

ではさっそく、Pythonプログラムで相補フィルタを実装し、ラズパイとBMX055を使ってセンサ値を取得してみたいと思います。

 

相補フィルタで角度を算出するまでのΔt はtime.time()を使って表現します。

またデータの初期誤差をなくすために flg を使ってRoll、Pitch、Yaw角の取得を遅らせています。

BMX055からセンサ値を取得するプログラムは上記で紹介した、前回の記事をご覧ください。

 

bmx055_complementary.py

if __name__ == "__main__":

    bmx_setup()
    time.sleep(0.1)
    flg = 0
    roll_error, pitch_error, yaw_error = 0.0, 0.0, 0.0
    k = 0.6
    new_roll, new_pitch, new_yaw = 0.0, 0.0, 0.0
    now = time.time()
    dt = 0.0

    while True:
        dt = time.time() - now
        now = time.time()

        acc = acc_value()
        gyro= gyro_value()
        mag = mag_value()

        roll = math.atan2(acc[1] , math.sqrt(acc[0]**2+acc[2]**2))
        pitch = math.atan2(acc[0] , math.sqrt(acc[1]**2+acc[2]**2))
        numerator = math.cos(roll)*mag[1] - math.sin(roll)*mag[2]
        denominator = math.cos(pitch)*mag[0] + math.sin(pitch)*math.sin(roll)*mag[1] + math.sin(pitch)*math.cos(roll)*mag[2]
        yaw = -math.atan2(numerator,denominator)

        for i in range(3):
            gyro[i] *= math.pi/180

        # ComplementaryFilter : 相補フィルタ
        new_roll = k*(new_roll+gyro[0]*dt) + (1-k)*roll
        new_pitch = k*(new_pitch+gyro[1]*dt) + (1-k)*pitch
        new_yaw = k*(new_yaw+gyro[2]*dt) + (1-k)*yaw

        roll = math.degrees(new_roll)
        pitch = math.degrees(new_pitch)
        yaw = math.degrees(new_yaw)

        if(flg < 20):
            roll_error = -roll
            pitch_error = -pitch
            yaw_error = -yaw
        flg += 1

        roll += roll_error
        pitch += pitch_error
        yaw += yaw_error

        if abs(roll) > 5 or abs(pitch) > 5:
            yaw = np.nan

        print("roll:{}".format(round(roll,2)))
        print("pitch:{}".format(round(pitch,2)))
        print("yaw:{}".format(round(yaw,2)))
        print("\n")
        time.sleep(0.1)

 

ラズパイとBMX055で取得したセンサ値から、相補フィルタまでの一連の流れをまとめたプログラムは「Github」で公開したので、プログラム全文を確認したい方は下記よりご覧ください。

 

Github:BMX055_ComplementaryFilter

 

相補フィルタの精度検証

ラズパイとBMX055、Pythonプログラムの準備・実行ができたら、Roll、Pitch、Yaw角の精度を検証していきたいと思います。

グラフの色分けは

青色:各センサ値のみ

橙色:相補フィルタの実装

となっています。

 

Roll角の精度検証

まずはRoll角の精度検証結果です。

BMX055本体をRoll角方向へ

-90° → 0 → +90°→ ランダム動作

と動かしたときの変化をグラフ化しました。

bmx055-cpfilter-02

グラフから、相補フィルタの方が誤差が小さくい、ランダムに動かしたときに変化がゆっくりになっていることが分かります。

急な角度変化で値が飛び飛びにならなくて済むので、フィルタの効果があると確認できますね。

 

Pitch角の精度検証

続いてPitch角の精度検証結果です。

BMX055本体をPitch角方向へ

-90° → 0 → +90°→ ランダム動作

と動かしたときの変化をグラフ化しました。

bmx055-cpfilter-03

取得値の誤差が3°ほど発生していますが、概ねセンサ値と相補フィルタでの値に変化はありません。

一方、相補フィルタの方がランダムに動かしたときの急激な変化に強いことが分かります。

 

Yaw角の精度検証

最後にYaw角の精度検証結果です。

BMX055本体をYaw角方向へ

+90° → 0 → -90°→ ランダム動作

と動かしたときの変化をグラフ化しました。

bmx055-cpfilter-yaw

地磁気の相補フィルタは、相補フィルタの理論式を元に式を勝手に作りましたが、地磁気のみの場合と比べてYaw角の誤差が少ないことが確認できます。

特に、0~2秒、10~12秒にかけては相補フィルタの方が誤差が少ないことが顕著に現れています。

 

相補フィルタは計算コストが非常に低く、扱いやすいので、簡単な誤差補正には十分の性能を示すことができると考えられますね。

 

以上、相補フィルタの実装、精度検証でした。

これでやっと機体にセンサを搭載して姿勢制御ができますね!

お疲れ様でした。

bmx055-cpfilter-top
学びに関する情報をチェック!