ここではIMUセンサを用いた角度推定方法を紹介する。
以前、クォータニオンで拡張カルマンフィルタを使って角度を推定した。
その記事は下のようなもの
なぜクォータニオンを用いたかというとジンバルロックを防ぐためである。
この記事ではクォータニオンではなく、オイラー角で計算し、実際にジンバルロックが起こるのかを確認する。
使うセンサと軸の定義
まず今回使うセンサは以前同様のICM-20948である。
ここではX軸回りを$\phi$(pitch), Y軸回りを$\theta$(yaw), Z軸回りを$\psi$(roll)と定義する。
推定方法
図１に今回の推定方法を表すブロック線図を示す。
IMUセンサで計測できる３軸の加速度センサよりpitch角とyaw角を推定する。この部分は以前にクォータニオンを用いた手法と同様である。
この角度は、IMUセンサをねじる方向以外の角度。pitch角度がx軸回り。yaw角度がｙ軸回り。
$x$軸回りの$\phi$は、
\begin{gather}
\frac{a_{y}}{a_{z}}=\frac{g\sin{\phi}\cos{\theta}}{g\cos{\phi}\cos{\theta}} =\tan{\phi} \\ \therefore \phi = \tan^{-1}(\frac{a_{y}}{a_{z}})\tag{1}\\
\end{gather}
$y$軸回りの$\theta$は、
\begin{gather}
\frac{a_{x}}{\sqrt{a_{y}^2+a_{z}^2}}=\frac{-g\sin{\theta}}{g\cos{\theta}}=\tan{\theta}\\
\therefore \theta = \tan^{-1}(\frac{-a_{x}}{\sqrt{a_{y}^2+a_{z}^2}}) \tag{2}
\end{gather}と求まる。ここで$a_{x},a_{y},a_{z}$はIMUで計測される加速度である
この$\phi$,$\theta$を用いて３軸を推定するためのモデル式は次のように表せる[1]。
\begin{gather}
\begin{bmatrix}
\dot{\phi}\\
\dot{\theta}\\
\dot{\psi}
\end{bmatrix}=
\begin{bmatrix}
1&\sin{\phi}\tan{\theta}&\cos{\phi}\tan{\theta}\\
0&\cos{\phi} &-\sin{\phi}\\
0&\frac{\sin{\phi}}{\cos{\theta}}& \frac{\cos{\phi}}{\cos{\theta}}
\end{bmatrix}
\begin{bmatrix}
w_{x}\\
w_{y}\\
w_{z}
\end{bmatrix}\tag{3}
\end{gather}
注意点は、(3)式からもわかる通り、$y$軸回りの$\theta$が90度もしくは-90度になったときに、ジンバルロックが起きるこｔ。0で割ることになっていしまう。
(3)式を見ると90度に傾く$\theta$は問題ないが、$x$軸と$z$軸回りの$\phi$と$\psi$がおかしくなることが予想できる。
Pythonコード
式(3)を用いて角度を推定した。
その時のPythonコードを以下に示す。(コードが汚い)
マイコン(今回はSeeeduino)に書いたArduinoコードは前回(9軸IMUセンサで角度推定)と同じである。
"""
2024/10/17
オイラー角で角度を推定した。
オイラー法で推定する。
"""
import csv
import numpy as np
import os
from datetime import datetime
import time
import serial, struct
from numpy import sin, cos, pi, tan, arctan, sqrt, arcsin,arctan2,arcsin
import matplotlib.pyplot as plt
import pandas as pd
# ----------
path = os.getcwd() + "/csv"
extens = ".csv"
# -----------------------------
ary_IMU = []
firstloop_initial = True
ser = serial.Serial('COM3', 9600, timeout=None) #new ICM
# ary_IMU= [proc_T,dt,Ax, Ay,Az,Gx, Gy,Gz,Mx, My,Mz,angle_x,angle_y,angle_z,"\n"]
ary_IMU_name = ['proc_T','dt','AX','AY','AZ','Gx','Gy','Gz','Mx','My','Mz',
'angle_x','angle_y','angle_z','pitch','yaw','\n']
filename = datetime.now().strftime("%m_%d__%H_%M_%S")
with open(path + "/" + filename + "_" + extens,'a',newline='') as f:
writer = csv.writer(f)
writer.writerow(ary_IMU_name)
val = struct.pack("B", 0x01) # Serial-ON
ser.write(val)
print('start===========')
start_T = time.time()
pre_time = start_T
time.sleep(0.01)
#-----------------------------------------------
print('start===========2')
try:
while True:
cur_time = time.time()
dt_py = 0.005 #[s]
dt_Python=0.005
proc_T = cur_time - start_T
dt_loop = cur_time - pre_time #[s]
pre_time = cur_time
line = ser.readline()
stripped_str = str(line, 'utf-8').strip()# byte型を文字列に変換して前後の空白改行除去
datas = [float(d) for d in stripped_str.split(',')] # 整数ではなく浮動小数点数に変換する
if len(datas) == 10:
Ax = datas[0]
Ay = datas[1]
Az = datas[2]
Gx = datas[3]
Gy = datas[4]
Gz = datas[5]
Mx = datas[6]
My = datas[7]
Mz = datas[8]
dt = datas[9]*0.001
Ax=(Ax*0.01)-0.719+0.4
Ay=(Ay*0.01)-0.6
Az=(Az*0.01)-0.1
Gx = Gx-0.348
Gy = Gy-0.138
Gz = Gz+0.405
Mx = Mx*0.001
My = My*0.001
Mz = Mz*0.001
Wx,Wy,Wz = Gx*pi/180, Gy*pi/180, Gz*pi/180 #[rad] G→Wに変更
Omg_s = np.array([ [Wx],[Wy],[Wz] ]) #[rad/s]
Acc_s = np.array( [[Ax],[Ay],[Az]] )
pitch = arctan2(Acc_s[1,0],Acc_s[2,0])#x軸回り
yaw = arctan2( -Acc_s[0,0] ,sqrt(Acc_s[1,0]**2 + Acc_s[2,0]**2) )#y軸まわり
one3 = np.ones(3)
matrix = np.array([[1,sin(pitch)*tan(yaw),cos(pitch)*tan(yaw)],
[0,cos(pitch),-sin(pitch)],
[0,(sin(pitch))/(cos(yaw)),(cos(pitch))/(cos(yaw))]])
if 1 :
#初期値 Accel-->Theta
if firstloop_initial:
firstloop_initial = False
flag_second = True
angle_x=pitch
angle_y=yaw
Angle = np.zeros(3)
if flag_second:
matrix = np.array([[1,sin(angle_x)*tan(angle_y),cos(angle_x)*tan(angle_y)],
[0,cos(angle_x),-sin(angle_x)],
[0,(sin(angle_x))/(cos(angle_y)),(cos(angle_x))/(cos(angle_y))]])
Angle = Angle+dt_Python*(matrix@Omg_s)
# ジャイロの値をオイラー法で積分したもの
angle_x=Angle[0,0]
angle_y=Angle[1,0]
angle_z=Angle[2,0]
pitch=pitch*180/pi
yaw=yaw*180/pi
print("angle_x=",angle_x*180/pi,"angle_y=",angle_y*180/pi,"angle_z=",angle_z*180/pi)
ary_IMU= [proc_T,dt,Ax, Ay,Az,Gx, Gy,Gz,Mx, My,Mz,angle_x*180/pi,angle_y*180/pi,angle_z*180/pi,pitch,yaw,"\n"]
with open(path + "/" + filename + "_" + extens,'a',newline='') as f:
writer = csv.writer(f)
writer.writerow(ary_IMU) # ary_IMUを直接渡す
else:
print('not 10 data')
except KeyboardInterrupt:
val = struct.pack("B", 0x02) # Serial-OFF
ser.write(val)
time.sleep(0.5)
print("saved")
# Read CSV file and create a scatter plot
df = pd.read_csv(path + "/" + filename + "_" + extens, delimiter=',')
time_csv=df['proc_T']
# Acc
AX_csv=df['AX']
AY_csv=df['AY']
AZ_csv=df['AZ']
# Gyro
Gx_csv=df['Gx']
Gy_csv=df['Gy']
Gz_csv=df['Gz']
# angle
angle_x_csv=df['angle_x']
angle_y_csv=df['angle_y']
angle_z_csv=df['angle_z']
# accからのpitch/yaw
pitch_csv=df['pitch']
yaw_csv=df['yaw']
size = 2
FONTSIZE=12
"""
オイラー角/地磁気は使ってない
"""
fig, axes = plt.subplots(3, 1, tight_layout=True)
fig.suptitle(filename)
axes = axes.flatten()
# acc
axes[0].scatter(time_csv,AX_csv,label='x',s=size)
axes[0].scatter(time_csv,AY_csv,label='y',s=size)
axes[0].scatter(time_csv,AZ_csv,label='z',s=size)
axes[0].set_xlabel('Time/s',fontsize=FONTSIZE)
axes[0].set_ylabel('Acc/deg',fontsize=FONTSIZE)
axes[0].legend(loc="upper left", fontsize=14) # (7)凡例表示
axes[0].grid()
# angle
axes[1].scatter(time_csv,angle_x_csv,label='x',s=size)
axes[1].scatter(time_csv,angle_y_csv,label='y',s=size)
axes[1].scatter(time_csv,angle_z_csv,label='z',s=size)
axes[1].set_xlabel('Time/s',fontsize=FONTSIZE)
axes[1].set_ylabel('angle/deg',fontsize=FONTSIZE)
axes[1].legend(loc="upper left", fontsize=14) # (7)凡例表示
axes[1].grid()
axes[2].scatter(time_csv,pitch_csv,label='x',s=size)
axes[2].scatter(time_csv,yaw_csv,label='y',s=size)
axes[2].set_xlabel('Time/s',fontsize=FONTSIZE)
axes[2].set_ylabel('angleAcc/deg',fontsize=FONTSIZE)
axes[2].legend(loc="upper left", fontsize=14) # (7)凡例表示
axes[2].grid()
plt.show()
実験
IMUセンサをX軸に90度Y軸に90度動かしたときの、加速度、加速度から推定された角度((1),(2)式)、ジャイロ入力によるモデルより導出された角度((3)式)を図2に上から示す。
この結果から、X軸を90度に回転させたときは問題なく推定ができているが、Y軸を90度にしたときに
(1)式のX軸回りがおかしくなっています。これはZ軸方向の加速度が0になり(1)式の分母が0になったためです。
また、同様に(3)式もX軸周りとZ軸回りを求めるところで０割を起こします。
これがジンバルロックだと確認できました。
参考
[1]6軸IMU慣性センサ～拡張カルマンフィルタ,株式会社翔飛
コメント