6軸ジャイロ・加速度センサー値をグラフにする

コロナ禍での自粛で外に出れないので、電子工作で時間を潰せないかと考えるこの頃です。

さて先日、6軸ジャイロ・加速度センサーのMPU-6050を購入しました。
MPU-6050 使用 ３軸ジャイロスコープ・３軸加速度センサー モジュール

6軸センサーとは

早速ラズパイに取り付けて遊んでみますが、その前に物理のお勉強です。

ジャイロセンサーは角速度（回転運動をする点の速度を、中心に対して単位時間に回転する角度で表したもの）を測定します。
何か難しいですが、物体の１秒当たりの回転速度と理解しました。

そして、加速度センサーは、物体の1秒当たりの速度変化を測定します。

これをロール軸（x軸）、ピッチ軸（y軸）、ヨー軸（z軸）の３つの軸でそれぞれ計測できるので、3 × 2 = 6軸センサーということらしいです。
ジャイロセンサーでデバイスの回転。
加速度センサーでデバイスにかかる重力、振動、衝撃を計測できるということらしいです。

地磁気（コンパス）センサーを加えた9軸のものもあるそうです。

Raspberry Piに接続してセンサーデータを取得する

配線する

まずは、ピンをはんだ付けします。
曲がってる方にしました。
下手ですが…。
よく見るとロール軸（x軸）とピッチ軸（y軸）の向きが書いてありますね。

Raspberry Piに配線します。

1PIN（+3.3V）をVCC
6PIN（GND）をGND
5PIN（GPIO3）をSCL（I2C）
3PIN（GPIO2）をSDA（I2C）

にそれぞれ接続します。

センサーデータを取得する

MPU-6050とのデータ通信にはI2C（Inter-Integrated Circuit）を使います。

ターミナルから「sudo raspi-config」コマンドを実行するかモニターを繋いでいるなら「Raspberry Piの設定->インターフェイス」からI2Cを有効にします。

I2Cを有効にしたら、デバイスを認識しているか確認します。 
$ sudo i2cdetect -y 1
     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f
00:          -- -- -- -- -- -- -- -- -- -- -- -- --
10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
60: -- -- -- -- -- -- -- -- 68 -- -- -- -- -- -- --
70: -- -- -- -- -- -- -- --
コマンド実行結果が上記のようになっていれば認識されています。

これで準備完了です。
センサーデータをPythonで取得してみます。 
$ git clone https://github.com/Tijndagamer/mpu6050.git
$ cd mpu6050/
$ sudo python3 setup.py install
$ vi mpu6050.py
from mpu6050 import mpu6050
from time import sleep

sensor = mpu6050(0x68)

while True:
    gyro_data = sensor.get_gyro_data()
    accel_data = sensor.get_accel_data()
    temp = sensor.get_temp()

    # 小数点以下第3位まで表示
    print("【角速度】 x:" + "%6.3f" % gyro_data['x'] + " y:" + "%6.3f" % gyro_data['y'] + " z:" + "%6.3f" % gyro_data['z'])

    # 小数点以下第3位まで表示
    print("【加速度】 x:" + "%6.3f" % accel_data['x'] + " y:" + "%6.3f" % accel_data['y'] + " z:" + "%6.3f" % accel_data['z'])

    # 小数点以下第1位まで表示
    print("【温度】" + "%4.1f" % temp + "℃")

    sleep(0.5)
$ python3 mpu6050.py
【角速度】 x:-250.137 y:-223.679 z:-98.435
【加速度】 x:-3.893 y:-0.881 z: 9.483
【温度】25.7℃
【角速度】 x:82.191 y:-9.412 z:-111.466
【加速度】 x: 0.807 y:-4.324 z: 5.073
【温度】25.8℃
【角速度】 x:250.130 y:73.191 z:91.771
【加速度】 x:-2.172 y:-1.434 z: 9.629
【温度】25.8℃
【角速度】 x:-141.870 y:-122.191 z:42.802
【加速度】 x: 1.027 y: 8.384 z: 4.077
【温度】25.8℃
【角速度】 x:79.740 y:49.321 z:-250.137
【加速度】 x: 3.455 y: 1.063 z: 5.389
【温度】25.8℃
【角速度】 x:-91.595 y:17.397 z:-28.229
【加速度】 x:-4.271 y: 6.819 z: 8.191
【温度】25.8℃
【角速度】 x:-21.573 y: 4.458 z:-3.000
【加速度】 x:-4.621 y: 6.136 z: 7.187
【温度】25.8℃
【角速度】 x:-8.542 y:-0.863 z:-0.458
【加速度】 x:-4.836 y: 6.105 z: 6.869
【温度】25.8℃
こんな感じでデータが取れました。
温度も取れますが、実際の温度より5℃以上高いのであまり精度は高くなさそうです。

取得したデータをグラフにする

データは取れましたが、これではセンサーの状態とデータの関連が分かりにくいので、Matplotlibでグラフにして可視化します。 
from mpu6050 import mpu6050
from time import sleep

sensor = mpu6050(0x68)

import numpy as np
import matplotlib.pyplot as plt

def plot_loop():
    # センサーデータ取得
    temp = "%4.1f" % sensor.get_temp()
    gyro_data = sensor.get_gyro_data()
    accel_data = sensor.get_accel_data()
    
    fig, (ax_temp, ax_gyro, ax_accel) = plt.subplots(ncols=3, figsize=(10,7))
    
    # X座標
    sec = np.arange(-np.pi, np.pi, 0.1)
    
    # 温度のY座標
    temp_list = np.zeros(63)
    temp_list[0] = temp
    line_temp, = ax_temp.plot(sec, temp_list, color="red")
    ax_temp.set_title("temperature")
    ax_temp.set_ylim(-10, 40)
    ax_temp.set_xticks([]) # X軸のメモリ非表示

    # 角速度のY座標
    # ロール軸(x)
    gyro_list_x = np.zeros(63)
    gyro_list_x[0] = "%6.3f" % gyro_data['x']
    gyro_x_lines, = ax_gyro.plot(sec, gyro_list_x, color="red", label="x")

    # ピッチ軸(y)
    gyro_list_y = np.zeros(63)
    gyro_list_y[0] = "%6.3f" % gyro_data['y']
    gyro_y_lines, = ax_gyro.plot(sec, gyro_list_y, color="blue", label="y")
    
    # ヨー軸(z)
    gyro_list_z = np.zeros(63)
    gyro_list_z[0] = "%6.3f" % gyro_data['z']
    gyro_z_lines, = ax_gyro.plot(sec, gyro_list_z, color="green", label="z")
    
    ax_gyro.legend() # ラベル描画
    ax_gyro.set_title("gyro")
    ax_gyro.set_ylim(-300, 300)
    ax_gyro.set_xticks([]) # X軸のメモリ非表示

    # 加速度のY座標
    # ロール軸(x)
    accel_list_x = np.zeros(63)
    accel_list_x[0] = "%6.3f" % accel_data['x']
    accel_x_lines, = ax_accel.plot(sec, accel_list_x, color="red", label="x")

    # ピッチ軸(y)
    accel_list_y = np.zeros(63)
    accel_list_y[0] = "%6.3f" % accel_data['y']
    accel_y_lines, = ax_accel.plot(sec, accel_list_y, color="blue", label="y")
    
    # ヨー軸(z)
    accel_list_z = np.zeros(63)
    accel_list_z[0] = "%6.3f" % accel_data['z']
    accel_z_lines, = ax_accel.plot(sec, accel_list_z, color="green", label="z")
        
    ax_accel.legend() # ラベル描画
    ax_accel.set_title("accel")
    ax_accel.set_ylim(-30, 30)
    ax_accel.set_xticks([]) # X軸のメモリ非表示

    # plotし続ける
    while True:
        # センサーデータ取得
        temp = "%4.1f" % sensor.get_temp()
        gyro_data = sensor.get_gyro_data()
        accel_data = sensor.get_accel_data()
        
        # データの更新
        sec += 0.1
        
        temp_list = np.roll(temp_list, 1)
        temp_list[0] = temp

        gyro_list_x = np.roll(gyro_list_x, 1)
        gyro_list_x[0] = "%6.3f" % gyro_data['x']
        gyro_list_y = np.roll(gyro_list_y, 1)
        gyro_list_y[0] = "%6.3f" % gyro_data['y']
        gyro_list_z = np.roll(gyro_list_z, 1)
        gyro_list_z[0] = "%6.3f" % gyro_data['z']

        accel_list_x = np.roll(accel_list_x, 1)
        accel_list_x[0] = "%6.3f" % accel_data['x']
        accel_list_y = np.roll(accel_list_y, 1)
        accel_list_y[0] = "%6.3f" % accel_data['y']
        accel_list_z = np.roll(accel_list_z, 1)
        accel_list_z[0] = "%6.3f" % accel_data['z']

        # グラフへデータの再セット
        line_temp.set_data(sec, temp_list)
        line_temp.set_data(sec, temp_list)
        
        gyro_x_lines.set_data(sec, gyro_list_x)
        gyro_y_lines.set_data(sec, gyro_list_y)
        gyro_z_lines.set_data(sec, gyro_list_z)

        accel_x_lines.set_data(sec, accel_list_x)
        accel_y_lines.set_data(sec, accel_list_y)
        accel_z_lines.set_data(sec, accel_list_z)

        # X軸の更新
        ax_temp.set_xlim((sec.min(), sec.max()))
        ax_gyro.set_xlim((sec.min(), sec.max()))
        ax_accel.set_xlim((sec.min(), sec.max()))

        print("【温度】" + temp + "℃")
        print("【角速度】 x:" + "%6.3f" % gyro_data['x'] + " y:" + "%6.3f" % gyro_data['y'] + " z:" + "%6.3f" % gyro_data['z'])
        print("【加速度】 x:" + "%6.3f" % accel_data['x'] + " y:" + "%6.3f" % accel_data['y'] + " z:" + "%6.3f" % accel_data['z'])

        plt.pause(0.1) # sleep時間（秒）

if __name__ == "__main__":
    plot_loop()

センサーの値が動的にグラフに反映されます。

ロール軸（x軸）をひねるとグラフのxの値が変化してることが見て取れて、センサーの向きとデータの変化の関連が分かりやすいですね。
【国内正規代理店品】Raspberry Pi4 ModelB 4GB ラズベリーパイ4 技適対応品

やってみたMatplotlib, Python, Raspberry Pi

Posted by danishi