MPU6050で現在の姿勢を推定する~プログラムの実装編~

表示形式を変えた場合

$varphi$方向に90度傾けた場合

補足になりますが、傾けすぎて$theta$方向にマイナス側で回転してしまった場合は
出力される値の符号が逆になってしまいます。設置する際はこのことを意識して設置したいと思います。(将来的にはソフトウェアで処理したいと思います。)

$\varphi$方向に90度傾けた場合($theta$方向にプラスで傾いていた場合)

今回実装したソースコード

import smbus
import math
from time import sleep
import time
import sys
import traceback
import matplotlib.pyplot as plt
import datetime
import numpy as np

np.set_printoptions(formatter={'float': '{:.8e}'.format})#値が小さい場合に上手く表示されない事象があったので、指数形式で表示するよう
#各種レジスタのアドレス
#----------------------------------------------------------------------------------
DEV_ADDR = 0x68
CONFIG = 0x1A
GYRO_CONFIG = 0x1B
ACCEL_CONFIG = 0x1C

ACCEL_XOUT = 0x3b
ACCEL_YOUT = 0x3d
ACCEL_ZOUT = 0x3f
TEMP_OUT = 0x41
GYRO_XOUT = 0x43
GYRO_YOUT = 0x45
GYRO_ZOUT = 0x47

PWR_MGMT_1 = 0x6b
PWR_MGMT_2 = 0x6c
#----------------------------------------------------------------------------------

#定数
#----------------------------------------------------------------------------------
CAL_SIZE = 5000#キャリブレーションに使うデータ数
IGNORE_DATA_SIZE = 50#キャリブレーションを行う際に最初の方のデータは信頼性が無いため、何個のデータを無視するか設定
X_BUF_SIZE = 5 #描画する際に何個の点をプロットするか
Y_BUF_SIZE = 5 #描画する際に何個の点をプロットするか
Z_BUF_SIZE = 5 #描画する際に何個の点をプロットするか
PLLOT_TIME = 0.01#何秒間隔でプロットするか
X_OFFSET = -703#オフセット値の計算結果を入れる(計算前は0を入れる.計算後は-703を入れる)
Y_OFFSET = 394#オフセット値の計算結果を入れる(計算前は0を入れる.計算後は394を入れる)
Z_OFFSET = 1867#オフセット値の計算結果を入れる(計算前は0を入れる.計算後は1867を入れる)
#----------------------------------------------------------------------------------

#変数/配列の初期化処理
#----------------------------------------------------------------------------------
data_x_list = []#データ格納用の配列作成
data_y_list = []
data_z_list = []

data_x_offset_list = []#データ格納用の配列作成(オフセット後)
data_y_offset_list = []
data_z_offset_list = []

bus = smbus.SMBus(1)

ax_average = 0#x加速度の平均値を格納するglobal変数
ay_average = 0#y加速度の平均値を格納するglobal変数
az_average = 0#z加速度の平均値を格納するglobal変数

initial_theta = 0#初期状態のθ
initial_phi = 0#初期状態のφ
initial_Rxyz = 0#初期値を元に算出した回転行列
#----------------------------------------------------------------------------------

#リアルタイム描画する際に必要な処理
#----------------------------------------------------------------------------------
fig,axes = plt.subplots(1,1)
lines, = axes.plot(0,0,"-o",lw=5)#何かプロットしないといけないとのことなので、空プロット
plt.grid()
axes.set_xlabel("X_AXIS[G]")
axes.set_ylabel("Y_AXIS[G]")

buf_x =[]
buf_y =[]
buf_z =[]
#----------------------------------------------------------------------------------

# x_offset = -703#オフセット値の計算結果を入れる(計算前は0を入れる.計算後は-703を入れる)
# y_offset = 394#オフセット値の計算結果を入れる(計算前は0を入れる.計算後は394を入れる)
# z_offset = 1867#オフセット値の計算結果を入れる(計算前は0を入れる.計算後は1867を入れる)
#----------------------------------------------------------------------------------

#デバイスの初期化処理
#----------------------------------------------------------------------------------
# bus.write_byte_data(DEV_ADDR, PWR_MGMT_1, 0x80)#write_byte_data(int addr,char cmd,char val) 1:I2C通信対象のアドレス。今回は加速度センサのみなので68固定,2:レジスタのアドレス,3:値)0x80はデバイスリセット
bus.write_byte_data(DEV_ADDR, PWR_MGMT_1, 0x00)
# bus.write_byte_data(DEV_ADDR, PWR_MGMT_2, 0x07)#FIFO/I2C_MST/SIG_CONDのリセット
bus.write_byte_data(DEV_ADDR, PWR_MGMT_2, 0x00)
#----------------------------------------------------------------------------------

#センサーの設定
#----------------------------------------------------------------------------------
# bus.write_byte_data(DEV_ADDR, CONFIG, 0x00)#CONFIG(フィルターなし)
bus.write_byte_data(DEV_ADDR, CONFIG, 0x06)#CONFIG(フィルター最大)
bus.write_byte_data(DEV_ADDR, GYRO_CONFIG, 0x00)#250°/s
bus.write_byte_data(DEV_ADDR, ACCEL_CONFIG, 0x00)#2g
#----------------------------------------------------------------------------------


def read_word(adr):#センサーの値が2バイトであらわされるため、2バイト分のデータを結合する関数
    high = bus.read_byte_data(DEV_ADDR, adr)#上位バイトのデータ
    low = bus.read_byte_data(DEV_ADDR, adr+1)#下位バイトのデータ
    val = (high << 8) + low#上位バイトのデータをビット1バイト分(8bit)左にシフトして、下位バイトのデータと足し合わせて1つのデータにしている。
    return val

def read_word_sensor(adr):
    val = read_word(adr)
    if (val >= 0x8000):  return -((65535 - val) + 1)#0x8000が最上位ビットが立っている状態なので、マイナス。もし、マイナス値だった場合は符号を反転させてる。(ただ反転させるだけではダメなので注意)
    else:  return val

def get_temp():
    temp = read_word_sensor(TEMP_OUT)
    x = temp / 340 + 36.53      # data sheet(register map)記載の計算式.
    return x

def getGyro():
    x = read_word_sensor(GYRO_XOUT)/ 131.0# data sheet(register map)記載の計算式.(250°を選択した場合)
    y = read_word_sensor(GYRO_YOUT)/ 131.0
    z = read_word_sensor(GYRO_ZOUT)/ 131.0
    return [x, y, z]


def getAccel(): 
    x = read_word_sensor(ACCEL_XOUT)/ 16384.0# data sheet(register map)記載の計算式.(2gを選択した場合)
    y = read_word_sensor(ACCEL_YOUT)/ 16384.0
    z = read_word_sensor(ACCEL_ZOUT)/ 16384.0
    return [x, y, z]

def getAccel_offset(): #オフセット分の考慮した場合のデータ
    x = (read_word_sensor(ACCEL_XOUT)+X_OFFSET)/ 16384.0# data sheet(register map)記載の計算式.(2gを選択した場合)
    y = (read_word_sensor(ACCEL_YOUT)+Y_OFFSET)/ 16384.0
    z = (read_word_sensor(ACCEL_ZOUT)+Z_OFFSET)/ 16384.0
    return [x, y, z]

def calc_offset_acc():#平面に置いた場合のオフセット計算処理
    global X_OFFSET
    global Y_OFFSET
    global Z_OFFSET
    for i in range(CAL_SIZE):
        if i>IGNORE_DATA_SIZE:#最初のデータは信憑性が無いため、無視
            X_OFFSET += -read_word_sensor(ACCEL_XOUT)#x,yは0が基準なので、0は省略している。
            Y_OFFSET += -read_word_sensor(ACCEL_YOUT)
            Z_OFFSET += 16384-read_word_sensor(ACCEL_ZOUT)#変換前の値で16384が1なので、16384を基準に計算する。
        else:#捨てデータは空読み
            read_word_sensor(ACCEL_XOUT)
            read_word_sensor(ACCEL_YOUT)
            read_word_sensor(ACCEL_ZOUT)

    X_OFFSET =int(X_OFFSET/(CAL_SIZE-IGNORE_DATA_SIZE)) #データの個数(無視したデータも考慮)で割ってオフセット分の平均を算出
    Y_OFFSET =int(Y_OFFSET/(CAL_SIZE-IGNORE_DATA_SIZE)) 
    Z_OFFSET =int(Z_OFFSET/(CAL_SIZE-IGNORE_DATA_SIZE)) 
    print("offset_calc_finish")

def calc_average():#センサーの値の平均値を求めグローバル変数に入れる。
    global ax_average
    global ay_average
    global az_average
    
    for i in range(CAL_SIZE):
        if i>IGNORE_DATA_SIZE:#最初のデータは信憑性が無いため、無視
            x,y,z = getAccel_offset()
            ax_average += x
            ay_average += y
            az_average += z 

        else:#念のため空読み
            x,y,z = getAccel()

    ax_average = ax_average/(CAL_SIZE-IGNORE_DATA_SIZE)
    ay_average = ay_average/(CAL_SIZE-IGNORE_DATA_SIZE)
    az_average = az_average/(CAL_SIZE-IGNORE_DATA_SIZE)
    print("ax_average")
    print(ax_average)
    print("ay_average")
    print(ay_average)
    print("az_average")
    print(az_average)

def calc_initial_angle():#センサーの平均値から現在の角度を求める(ラジアンで返す)
    global initial_phi
    global initial_theta
    if (ax_average == 0 ) and (ay_average == 0) and (az_average == 0):#もし平均値の計算が出来てなければ平均値を計算し、グローバル変数に値を格納する
        calc_average()  
    initial_phi = np.arctan(ay_average/az_average)
    initial_theta = np.arctan(-ax_average/(np.sqrt(ay_average**2+az_average**2)))
    print("phi")
    print( initial_phi)
    print("theta")
    print(initial_theta)
    print("phi_deg")
    print( np.rad2deg(initial_phi))
    print("theta_deg")
    print( np.rad2deg(initial_theta))
def calc_rotation(ax, ay, az):
    global initial_Rxyz
    a_initial = np.array([[ax], [ay], [az]])#回転前の加速度の列ベクトルを作成

    Cp = np.cos(initial_phi)#phiのCos値(初期値の角度分回転)
    Sp = np.sin(initial_phi)#phiのSin値(初期値の角度分回転)
    Ct = np.cos(initial_theta)#thetaのcos値(初期値の角度分回転)
    St = np.sin(initial_theta)#thetaのsin値(初期値の角度分回転)
    Cps = 1 #psi方向のCos値(psi方向は初期値が求められないため、0度として計算)
    Sps = 0 #psi方向のSin値(psi方向は初期値が求められないため、0度として計算)
    Rx = np.matrix([[1,0,0],[0,Cp,-Sp],[0,Sp,Cp]])#x方向の回転行列
    Ry = np.matrix([[Ct,0,St],[0,1,0],[-St,0,Ct]])#y方向の回転行列
    Rz = np.matrix([[Cps,-Sps,0],[Sps,Cps,0],[0,0,1]])#z方向の回転行列
    initial_Rxyz = Rz@Ry@Rx#@で行列の演算が出来るとの事。X->Y->Zの順で回転させる
    
    a_rotate = initial_Rxyz@a_initial#初期状態の角度回転させた場合の値
    
    print("initial_Rxyz")
    print(initial_Rxyz)
    print("a_rotate")
    print(a_rotate)
    #print(initial_Rxyz*10**10)
    #print(a_rotate[0]*10**10)#テスト用。これを記述して一度実行したら何故か2回目実行時も問題無く表示されるように。。。
def real_time_plot():
    global lines
    global ax
    global buf_x
    global buf_y
    global buf_z

    x,y,z = getAccel_offset()
    buf_x.append(x)
    buf_y.append(y)
    buf_z.append(z)
    #何個の点を描画するか
    if len(buf_x) > X_BUF_SIZE:
        del buf_x[0]
    if len(buf_y) > Y_BUF_SIZE:
        del buf_y[0]
    if len(buf_z) > Z_BUF_SIZE:
        del buf_z[0]
    lines.set_data(buf_x,buf_y)
    # print("bus_x,bus_y")
    # print(buf_x)
    # print(buf_y)
    axes.set_xlim(-2.1,2.1)#今回は2gのセンサーを使っている為、範囲は2g+マージンで0.1の範囲で描画
    axes.set_ylim(-2.1,2.1)#今回は2gのセンサーを使っている為、範囲は2g+マージンで0.1の範囲で描画
    draw_circle = plt.Circle(xy=(0, 0), radius=0.2,fill=False,ec = "b")
    axes.add_artist(draw_circle)
    draw_circle2 = plt.Circle(xy=(0, 0), radius=0.5,fill=False,ec = "b")
    axes.add_artist(draw_circle2)
    draw_circle3 = plt.Circle(xy=(0, 0), radius=1.0,fill=False,ec = "b")
    axes.add_artist(draw_circle3)
    draw_circle4 = plt.Circle(xy=(0, 0), radius=1.5,fill=False,ec = "b")
    plt.pause(PLLOT_TIME)
def file_output():
    dt_now = datetime.datetime.now()
    file = open(str(dt_now)+"calc_data.txt","w",encoding="utf-8")#統計データ格納用のファイル
    #統計情報をファイルに出力
    #----------------------------------------------------------------------------------
    print("x_max",file=file)
    print(max(data_x_list),file=file)
    print("x_min",file=file)
    print(min(data_x_list),file=file)
    print("x_ave",file=file)
    print(sum(data_x_list)/len(data_x_list),file=file)
    print("y_max",file=file)
    print(max(data_y_list),file=file)
    print("y_min",file=file)
    print(min(data_y_list),file=file)
    print("y_ave",file=file)
    print(sum(data_y_list)/len(data_y_list),file=file)
    print("z_max",file=file)
    print(max(data_z_list),file=file)
    print("z_min",file=file)
    print(min(data_z_list),file=file)
    print("z_ave",file=file)
    print(sum(data_z_list)/len(data_z_list),file=file)

    print("x_offset_max",file=file)
    print(max(data_x_offset_list),file=file)
    print("x_offset_min",file=file)
    print(min(data_x_offset_list),file=file)
    print("x_offset_ave",file=file)
    print(sum(data_x_offset_list)/len(data_x_offset_list),file=file)
    print("y_offset_max",file=file)
    print(max(data_y_offset_list),file=file)
    print("y_offset_min",file=file)
    print(min(data_y_offset_list),file=file)
    print("y_offset_ave",file=file)
    print(sum(data_y_offset_list)/len(data_y_offset_list),file=file)
    print("z_offset_max",file=file)
    print(max(data_z_offset_list),file=file)
    print("z_offset_min",file=file)
    print(min(data_z_offset_list),file=file)
    print("z_offset_ave",file=file)
    print(sum(data_z_offset_list)/len(data_z_offset_list),file=file)

    print("OFFSET_X",file=file)
    print(X_OFFSET,file=file)
    print("OFFSET_Y",file=file)
    print(Y_OFFSET,file=file)
    print("OFFSET_Z",file=file)
    print(Z_OFFSET,file=file)
    print("OFFSET_Z_cal",file=file)
    print(Z_OFFSET/16384,file=file)
    file.close()
    #----------------------------------------------------------------------------------

# calc_offset_acc()#オフセットの計算をする場合にこの関数をコールする。(一度計算すれば問題無し。計算が終わったらコメントアウト)
print("lecoding")
while True:
    try:
        ax, ay, az = getAccel()#加速度センサーの情報読み出し
        gx, gy, gz = getGyro()#ジャイロセンサーの情報読み出し
        ax_offset,ay_offset,az_offset = getAccel_offset()
        magnitude_a_offset = math.sqrt(ax_offset**2+ay_offset**2+az_offset**2)#加速度の大きさを算出
        temp = get_temp()#温度センサーの情報読み出し
        raw_ax = read_word(ACCEL_XOUT)
        raw_ay = read_word(ACCEL_YOUT)
        raw_az = read_word(ACCEL_ZOUT)
        if (ax_average == 0 ) and (ay_average == 0) and (az_average == 0):#初期状態の角度を計算してない場合に初期状態の角度を求める
            print("初期状態の角度を求めます")
            calc_initial_angle()
            calc_rotation(ax_average,ay_average,az_average)
        else:#センサーの値をリアルタイムで回転させていく処理
            calc_rotation(ax,ay,az)

        # real_time_plot()#リアルタイム描画の関数
        # print ('GYRO:{:4.3f},{:4.3f},{:4.3f},' .format(gx, gy, gz)) 
        # print ('ACC:{:4.3f},{:4.3f},{:4.3f},' .format(ax, ay, az))    
        # print ('ACC_RAW:{:4.3f},{:4.3f},{:4.3f},' .format(raw_ax, raw_ay, raw_az))    
        # print ('ACC_MAG:{:4.3f}'.format(magnitude_a_offset))
        # print ('TEMP:{:4.3f}'.format(temp))
        # data_x_list.append(ax)#リアルタイム描画確認する際に不要なので一旦コメントアウト
        # data_y_list.append(ay)#リアルタイム描画確認する際に不要なので一旦コメントアウト
        # data_z_list.append(az)#リアルタイム描画確認する際に不要なので一旦コメントアウト
        # data_x_offset_list.append(ax_offset)#リアルタイム描画確認する際に不要なので一旦コメントアウト
        # data_y_offset_list.append(ay_offset)#リアルタイム描画確認する際に不要なので一旦コメントアウト
        # data_z_offset_list.append(az_offset)#リアルタイム描画確認する際に不要なので一旦コメントアウト


    except:#ctrl+Cでコードの実行を停止した場合にこちらの処理が実行(将来的にはCAN途絶したらこの処理に入るように変更予定)
        bus.close()
        del data_x_list[:50]#最初の方のデータが0なので除外
        del data_y_list[:50]
        del data_z_list[:50]
        del data_x_offset_list[:50]#最初の方のデータが0なので除外
        del data_y_offset_list[:50]
        del data_z_offset_list[:50]
        
        #現状ctrl+cでこの処理に入るので本当にエラーが起きた時用にエラーメッセージを出力
        #----------------------------------------------------------------------------------        
        print("Error Occur")
        print(traceback.format_exc())
        sys.exit()#後ほど削除
        #----------------------------------------------------------------------------------
        
        # file_output()#ファイル下記だし用の処理 


        #ロギングしたデータをプロット
        #----------------------------------------------------------------------------------
        # time_list = range(len(data_x_list))
        # time_list_offset = range(len(data_x_offset_list))        
        # plt.plot(time_list,data_x_list,label="x_not_offset")
        # plt.plot(time_list,data_y_list,label="y_not_offset")
        # plt.plot(time_list_offset,data_x_offset_list,label="x_offset")
        # plt.plot(time_list_offset,data_y_offset_list,label="y_offset")
        # plt.legend()
        # # plt.ylim(-0.015,0.015)
        # plt.show()
        # plt.plot(time_list,data_z_list,label="z_not_offset")
        # plt.plot(time_list_offset,data_z_offset_list,label="z_offset")
        # plt.legend()
        # # plt.ylim(0.9,1.1)
        # plt.show()
        # sys.exit()
        #----------------------------------------------------------------------------------    

まとめ