Micropython-6軸MPU6050モジュールの使用

PYBをROSロボットのマスターコントロールとして使用したい場合、ジャイロスコープと加速度計をどのように減らすことができますか？

MPU6050モジュール

MPU6050は、非常に人気のある宇宙モーションセンサーチップであり、デバイスの現在の3つの加速度成分と3つの回転角速度を取得できます。小型、強力な機能、高精度により、業界で広く使用されているだけでなく、模型飛行機愛好家向けのアーティファクトであり、さまざまな航空機に搭載されて青空を飛んでいます。
MPU6050チップには、フィルタリングアルゴリズムが組み込まれたデータ処理サブモジュールDMPが付属しています。多くのアプリケーションで、DMPによって出力されるデータは要件を十分に満たすことができます。使用時には、オフセットを差し引くだけで使用できます。精度に対する要件が高い場合は、上位のコンピューターを使用するか、カルマンフィルターを使用してデータを再度最適化できます。

購入時にジャイロスコープと加速度計だけが必要な場合は、上または下の写真のように6軸GY-521 6軸MPU6050モジュールを購入できます。特定の宝物で約4元しかかかりません。非常に高いです。安いです、そしてあなたは間違ったモデルを買わないでください、さもなければあなたがそれを使うときそれはあなたを涙なしで泣かせます。ここで直接ライブラリを使用できない場合があります。

ハードウェア接続

配線の方向とMPUデータを図に示します。

PYB	MPU6050
VCC	VCC
GND	GND
SCL（X9、IIC1）	SCL
SDL（X10、IIC1）	SDL

コード

MPU6050ライブラリファイル

mpu6050.py

import machine

class accel():
    def __init__(self, i2c, addr=0x68):
        self.iic = i2c
        self.addr = addr
        self.iic.start()
        self.iic.writeto(self.addr, bytearray([107, 0]))
        self.iic.stop()

    def get_raw_values(self):
        self.iic.start()
        a = self.iic.readfrom_mem(self.addr, 0x3B, 14)
        self.iic.stop()
        return a

    def get_ints(self):
        b = self.get_raw_values()
        c = []
        for i in b:
            c.append(i)
        return c

    def bytes_toint(self, firstbyte, secondbyte):
        if not firstbyte & 0x80:
            return firstbyte << 8 | secondbyte
        return - (((firstbyte ^ 255) << 8) | (secondbyte ^ 255) + 1)

    def get_values(self):
        raw_ints = self.get_raw_values()
        vals = {
    
    }
        vals["AcX"] = self.bytes_toint(raw_ints[0], raw_ints[1])
        vals["AcY"] = self.bytes_toint(raw_ints[2], raw_ints[3])
        vals["AcZ"] = self.bytes_toint(raw_ints[4], raw_ints[5])
        vals["Tmp"] = self.bytes_toint(raw_ints[6], raw_ints[7]) / 340.00 + 36.53
        vals["GyX"] = self.bytes_toint(raw_ints[8], raw_ints[9])
        vals["GyY"] = self.bytes_toint(raw_ints[10], raw_ints[11])
        vals["GyZ"] = self.bytes_toint(raw_ints[12], raw_ints[13])
        return vals  # returned in range of Int16
        # -32768 to 32767

    def val_test(self):  # ONLY FOR TESTING! Also, fast reading sometimes crashes IIC
        from time import sleep
        while 1:
            print(self.get_values())
            sleep(0.05)

メインプログラム

from machine import I2C,Pin
i2c = I2C(scl=Pin("X9"), sda=Pin("X10"))
accel = mpu6050.accel(i2c)
accel_dict = accel.get_values()
print(accel_dict)

演算結果：

パラメータ分析

6軸MPU6050は、すべてのパラメータを出力します。

• 加速度計のX軸コンポーネント：ACC_X
• 加速度計のY軸コンポーネント：ACC_Y
• 加速度計のZ軸コンポーネント：ACC_Z
• 現在の温度：TEMP
• X軸を中心とした回転角速度：GYR_X
• Y軸周りの回転角速度：GYR_Y
• Z軸を中心とした回転角速度：GYR_Z

MPU6050チップの座標系は次のように定義されます：チップの表面を手前に向け、表面のテキストを正しい角度に向けます。このとき、チップの中心が原点で、水平方向の右がXです。軸であり、上向きの垂直方向はYです。それ自体を指す軸はZ軸です。

3軸加速度計

加速度計の3軸コンポーネントACC_X、ACC_Y、およびACC_Zは、すべて16ビットの符号付き整数であり、それぞれ3軸でのデバイスの加速度を表します。値が負の場合、加速度は座標の負の方向に沿っています。軸であり、値が正の場合、正の方向に沿っています。

3つの加速度成分は重力加速度gの倍数の単位であり、表現できる加速度範囲、つまり倍率を均一に設定でき、2g、4g、8g、16gの4つのオプションの倍率があります。倍率の変更方法はここでは紹介していません。変更したい場合は、異なる倍率に対応するフラグビットを対応するレジスタに書き込むだけです。倍率のデフォルト設定は2gです。たとえばACC_X、倍率を使用する2g場合ACC_X、最小値は-32768、最大値は32768です。それは場合ACC_X32768で、現在の加速度がX軸の正方向に重力の2倍の加速度です。

加速度の計算式を図に示します。

3軸ジャイロスコープ

X、Y、Zの3つの座標回転軸の周りの角速度成分GYR_XでGYR_YありGYR_Z、16ビットの符号付き整数です。原点から回転軸方向に見て、正の値は時計回りの回転を意味し、負の値は反時計回りの回転を意味します。

3つの角速度成分はすべて「度/秒」で表され、表現できる角速度の範囲、つまり倍率を均一に設定でき、250度/秒、500度/秒の4つのオプションの倍率があります。 、1000度/秒、2000度/秒。たとえばGYR_X、倍率が250度/秒に設定されている場合、GYRが正の最大値32768をとる場合、現在の角速度は時計回りに250度/秒になります。500度/秒に設定されている場合、は32768に設定され、現在の角速度を意味します。時計回りに500度/秒です。明らかに、倍率が低いほど精度が高くなり、倍率が高いほど範囲が広くなります。倍率のデフォルト設定は250度/秒です。具体的な変更方法についてはここでは説明しません。

角速度の変換式は次のとおりです。

データ処理と実現

MPU6050チップによって提供されるデータは深刻なノイズと混合され、チップが静的状態を処理しているときにデータスイングが2％を超える可能性があります。ノイズに加えて、データにはオフセット現象もあります。つまり、データは静的動作点を中心にスイングしないため、最初にデータオフセットを調整してから、フィルタリングアルゴリズムによってノイズを除去する必要があります。 。

較正

キャリブレーションは比較的簡単な作業であり、スイングデータの周りの中心点を見つけるだけで済みます。例としてGRY_Xを取り上げましょう。チップが静的状態の場合、読み取り値は理論的には0であるはずですが、多くの場合オフセットがあります。たとえば、次のように10ms間隔で10個の値を読み取ります。

-158.4, -172.9, -134.2, -155.1, -131.2, -146.8, -173.1, -188.6, -142.7, -179.5

これらの10個の値の平均を取ります。つまり、この読み取り値のオフセットは-158.25です。オフセットを取得した後、各読み取り値からオフセットを差し引いて、校正された読み取り値を取得します。もちろん、このオフセットは推定値に過ぎず、より正確なオフセットは、大量のデータに対して統計を実行することによってのみ取得できます。データの量が多いほど正確になりますが、統計時間は遅くなります。通常、キャリブレーションはシステムが起動するたびに実行できるため、精度と起動時間の間でトレードオフを行う必要があります。

3つの角速度の読み取り値GYR_X、GYR_Y、およびGYR_Zはすべて統計的平均化によって取得できますが、チップが静止しているときの加速度がゼロではないため、3つの加速度成分を取得することはできません。

加速度値のオフセットは2つの側面から生じます。1つは、チップの測定精度のためにチップによって測定された加速度ベクトルが地面に対して垂直ではないこと、もう1つは、システム全体でのチップの精度（ドローンなど）は限られています。はい、システムとチップの座標系を完全に一致させることは困難です。前者は読み取りオフセットと呼ばれ、後者は角度オフセットと呼ばれます。読み取り値と角度の間には非線形の関係があるため、高精度で校正する場合は、最初に読み取り値オフセットを個別に校正してから、システムにチップを固定して角度オフセットを校正する必要があります。ただし、角度オフセットの校正には専門の機器が必要であり、一般的な用途では、2段階校正による精度の向上は大きくなく、通常は読み取り校正のみが必要です。

校正方法の読み取り：

• 読み取り値のオフセットの影響をできるだけ回避するために、最初にMPU6050をシステムにしっかりと固定し、2つの座標系を可能な限り一致させます。
  現時点では、チップのACC_X合計のACC_Y理論値は0でありACC_Z、理論値は-16384（デフォルトの倍率2g）であると考えています。
  のでACC_XとACC_Y理論値がキャリブレーションに似た角速度で、0である必要があり、2つの校正は、統計的平均利用できる読取手段を相殺します。ACC_Zもう1つのステップが必要です。つまり、オフセットをカウントするプロセスで、各読み取り値に16384を追加する必要があります。その後、統計平均キャリブレーションが実行されます。

カルマンフィルター

ノイズの多いデータの場合、カルマンフィルターの効果は間違いなく最高です。アルゴリズムの詳細を考慮したくない場合は、ArduinoのKlamanFilterライブラリを直接使用できます。私たちのモデルでは、カルマンフィルターは軸角度値、角速度値、時間増分を受け入れ、ノイズを排除する角度値を推定します。現在の角度値と前のラウンドで推定された角度値、および2つの推定ラウンド間の間隔時間に基づいて、角速度を推測してノイズを除去することもできます。

カルマンフィルターは、ホストコンピューターに実装することをお勧めします。Micropython自体は、行列演算を特に適切にサポートしていません。ホストコンピューターでは、numpyを使用して高速のカルマンフィルター演算を実装できます。

参考記事：

• Arduinoチュートリアル：MPU6050のデータ収集、分析、処理