pythonを使用した加速度とジャイロセンサーによる回転行列について

Question

測定した加速度とジャイロのデータを回転行列で分解するプログラムを作成しております。
動作データと測定データの誤差が大きいのですが、何が課題かわからず困っております。

#取得した加速度データをそれぞれacX,acY,acZとする
取得したジャイロデータをそれぞれradX,radY,radZとする。
#加速度はG表示のため、取得時にm/s*2へ変換

#角速度から角度へ　単位はradに
kakudoX=[]
kakudoY=[]
kakudoZ=[]
for i in range(len(radX)):
    radsecX=radX[i]+radX[i+1]*0.1
    kakudoX.append(radsecX)
    radsecY=radY[i]+radY[i+1]*0.1
    kakudoY.append(radsecY)
    radsecZ=radZ[i]+radZ[i+1]*0.1
    kakudoZ.append(radsecZ)

#acgyのリストを作成
acgy_list=[]
def kaiten(rx,ry,rz):
    #回転行列を定義
    Rx=np.array([[1,0,0],[0,np.cos(rx),-np.sin(rx)],[0,np.sin(rx),np.cos(rx)]])
    Ry=np.array([[np.cos(ry),0,np.sin(ry)],[0,1,0],[-np.sin(ry),0,np.cos(ry)]])
    Rz=np.array([[np.cos(rz),-np.sin(rz),0],[np.sin(rz),np.cos(rz),0],[0,0,1]])
    #基準座標系からみた座標の方向余弦行列.オイラー角z:ヨー角>x：ピッチ角>y：ロール角
    R=Rz.dot(Ry).dot(Rx)
    return R
accelgyro=kaiten(np.radians(x),np.radians(y),np.radians(z)).dot([[acX[i]],[acY[i]],[acZ[i]]])
    acgy_list.append(accelgyro)

#ノイズ除去のためにフィルタリングを行う。
# ローパスフィルターでフィルタリング
#1に近づけるほど平滑化の度合いは大きくなっていく
lowpass_acX=[]
lowpass_acY=[]
lowpass_acZ=[]
for i in range(len(acX)):
    a=0.3
    low=a*dataX[i]+(1-a)*dataX[i]
    lowpass_acX.append(low)
    low=a*dataY[i]+(1-a)*dataY[i]
    lowpass_acY.append(low)
    low=a*dataZ[i]+(1-a)*dataZ[i]
    lowpass_acZ.append(low)
highpass_X=[]
highpass_Y=[]
highpass_Z=[]
#次にハイパスフィルターでフィルタリング
for i in range(len(acX)):
    #ローパスフィルター：現在の値＝係数*一つ前の値＋(1-係数)センサーの値
    #ハイパスフィルター：センサーの値ーローパスフィルター
    a=0.4
    lowX=a*lowpass_X+rdataX[i]*(1-a)
    highX=rdataX[i]-lowX
    lowY=a*lowpass_Y+rdataY[i]*(1-a)
    highY=rdataY[i]-lowY
    lowZ=a*lowpass_Z+rdataZ[i]*(1-a)
    highZ=rdataZ[i]-lowZ
    highpass_X.append(highX)
    highpass_Y.append(highY)
    highpass_Z.append(highZ)  


コード

以上の方法で加速度とジャイロデータを回転行列として計算しております。このaccel*gyroデータをもとに速度、距離(台形積分法なども試しました)を求めていくと誤差が拡大し困っています。
何が問題かアドバイスをいただけますと幸いです。
よろしくお願いいたします。

Comments

[ozwk]

temp_pitch=temp_pitch+motion_Pitch[num]*0.2 <-これは何をしているつもりなんですか?角速度の積分?

[yohehe]

回転行列にかけるために角速度rad/secをradianの単位に変更しようとしたのですが。もしかして必要がないのでしょうか。

Answer 1

1
理由は面倒なので説明しませんが、
3次元の回転において、角速度の値を軸ごとに個別に足し合わせて"角度"を求めるのは誤りです。

そもそも3次元の回転は角度で持たずにクォータニオンや回転行列で持ったほうが計算上楽です。
クォータニオンや回転行列を角速度で更新する方法を調べてください。

「無限小回転」「微小回転」というキーワードが役に立つかもしれません。

2
加速度を2回の積分で位置とするのはとんでもなく誤差が大きくなります。
ある条件下では速度を0と推定するというような何らかの制約をつける必要があります。
(例えば、動加速度の大きさが一定以下なら静止しているものとみなすなど)

Comments

[yohehe]

ozwk様ありがとうございます。そもそも角速度の段階が間違っていたために、徐々に誤差が大きくなっていくという現象だったのですか。ベクトルと同じように考えて速度と同じよう考えず、無限小回転、小回転について調べて一つずつ追記いたします。

[ozwk]

もとの質問から大幅に内容が変わるなら前の質問を残すか質問を別で立ててください

[yohehe]

すみません、そのほうが適切でした。消してしまってます前のプログラムデータを追記いたします。

[yohehe]

ozwk様、文章を修正し、この質問を終了して別の質問にさせていただくようにいたします。ありがとうございます。