ジャイロセンサーから測定したquaternionによる姿勢推定を用いた加速度の回転方法について

Question

quaternionを用いた姿勢制御、それを用いた加速度への回転行列定義についてpythonのプログラムを作成使用としております。

ある動作における加速度とジャイロ動作を取得したとする。
30hz毎の測定時において
加速度a=(ax,ay,az)
ジャイロセンサーの値w=(wx,wy,wz)が取得できるとする。
座標系を用いて姿勢制御について検討する。

姿勢制御については、

#quaternionを用いた姿勢推定においては
q=np.array(qx,qy,qz,w)
#quaternion微分値をdqとすると　初期位置を0,0,0として座標系における計算とする。
dq=np.array([0,wz,-wy,wx],[-wz,0,wx,wy],[wy,-wx,0,wz],[-wx,-wy,-wz,0]).dot(q)

#初期位置を四元数を用いて(0,0,0,1)とすると
q_shoki=np.array(0,0,0,1)

#初期位置からの次点でのQuaternion q(t+dt)をq2とすると、
q2=q_shoki+dt

#q2を正規化する。
l=np.sqrt(q2[0]*q2[0]+q2[1]*q2[1]+q[2]*q[2]+q2[3]*q2[3])
q2_seiki=q2/l


#以上を繰り返してクォータニオンによって姿勢制御を求めていく
#角度での表記が必要であればオイラー角へq２をプログラムでオイラー角にする。

以上で座標系における角速度が求められる状態での表記が可能であるということがわかってきました。
この段階で間違えていたらすみませんが、アドバイスください。

このquarternionを用いた姿勢推定に加速度のベクトル成分を導入していくプログラムについてわからずにおります。
今の頭の中での考えでは、

#この姿勢制御に加速度の要素を入れて位置を推定するプログラムを作成していくとすると、


#Quaternionによる姿勢制御から座標系の回転を行う行列を定義
#q2における(q0,q1,q2,q3)とすして回転行列を定義

R=np.array([q0*q0-q1*q1-q2*q2+q3*q3,2(q1*q2+q3*q0),2(q3*q1-q2*q0)],
[2(q1*q2-q3*q0),q2*q2-q3*q3-q1*q1+q0*q0,2(q2*q3+q1*q0)],
[2(q3*q1+q2*q0),2(q2*q3-q1*q0),q3*q3-q1*q1-q2*q2+q0*q0])

#q2における加速度をa2とすると
a2=R.dot(a2)
#となると思われる。すみません、この方法で加速度を導入していくことが間違っているのかわからないでおります。
#以上の考え方が正しかったとすると、n次点での加速度a_nは
a_n=a1+a2+a3+,,,,,,an
#以上で計算することが可能??なのでしょうか。
コード

以上のコードの流れを作成しているのですが、以下の点がわかりません。

1. 次点次点での姿勢における加速度の回転の導入方法の考え方がそもそもあっておりますでしょうか。
2. 取得できる加速度データは位置ベクトルと考えると、姿勢制御で得られたqを加速度を位置ベクトルとして処理すべきなのか。色々考えているのですが、わからないでおります。

すみませんが、よろしくお願いいたします。

---

7/2追記いたしました。

クォータニオン便利ノートより抜粋させていただいております。

以上をみて考えたのですが、

クォータニオンの回転を行なったr=(x,y,z)をクォータニオンBで回転させたとする。

r'=Brが成立するのであれば、
Bの逆行列をB_1とすると両辺に左からB_1をかけて
B_1*r'=rが成り立つ。

ここでいうB_1が"加速度におけるセンサー座標系から基準状態に戻すクォータニオンである",との理解はあっていますでしょうか。

Answer 1

持っているもの
センサーの座標系での加速度
基準状態の座標系から現在のセンサー座標系への回転クォータニオン

欲しいもの
基準状態の座標系での加速度

やること
現在のセンサー座標系から基準状態に戻すクォータニオンを求めて
センサー座標系での加速度に作用させる

Comments

[yohehe]

ozwk様、いつもすみません。本当にありがとうございます。
加速度へのクォータニオンについて頭のなかでの整理ができました。

センサー座標系から基準系にも戻すクォータニオンとは、
各次点におけるクォータニオンq(x,y,z,w)における逆quaternion(単純quaternionにおいては逆クォータニオンと共役クォータニオンはイコールだとすると共役でもいい)ということだと自分では考えたのですが、、あってますでしょうか。

[yohehe]

q(t+dt)=q(t)+q(dt)、wを座標系の角速度だとしてセンサー座標系へのクォータニオンを求めるとする。その場合において、
q(dt)=0.5*q*wとしているとすると、
qに戻すような 2*w^-1(wの逆行列の意味)が基準系に戻す式が基準座標系に戻すという式になる?とは間違っておりますよね。

[ozwk]

普通に共役で

[yohehe]

ありがとうございます。共役quaternionで進めてみます。

[yohehe]

今すすめているのですが、あと一つ質問させてください。

加速度に共役クォータニオンを作用をさせるとは、加速度をquaternionの４元数表記にして加速度のquaternionとジャイロから得られているquaternionの積を取るということでよかったですか?

[ozwk]

（ある意味あってるけど）違います
クォータニオン計算便利ノートに書いてあります

とりあえずX軸に90度回転させるクォータニオンでも作って適当なベクトル相手に検算すればいいです。

なんか対象が加速度だから難しく考えているのかもしれませんが
そもそものやりたいことは、
「センサー座標系でとれたこのベクトルは、基準座標系でいくつになるんだろう」ってだけです。(ですよね?)
もし回転行列で扱っていたら回転行列かけて終わる話です

[yohehe]

返信がおそくなり申し訳ありませんが、ozwk様ありがとうございます。
基準座標系におけるベクトルがいくつになるか測定したいだけです。なかなか理解が追いつかずすみません。確かに加速度のために複雑に考えているのかもしれません。

クォータニオン計算便利ノートにのクォータニオン表現による変換行列の(ii)だと思われるのでやってみます。ありがとうございます。

[yohehe]

すみません、悩み悩んで追記させていただきました。

ozwk様、本文に追記したのですが、Bの逆行列が求めるべき基準状態での加速度を求めるために使用するクォータニオンでこれを各次点での加速度にかけていくという判断で大丈夫でしょうか?
何度も質問すみません。

[ozwk]

用語はともかく大丈夫です

適当に状況を設定して検算してみてください

[yohehe]

ozwk様、ありがとうございます。プログラムを設定して検算してみます。