質問: 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 #以上を繰り返してクォータニオンによって姿勢制御を求めていく #角度での表記が必要であればオイラー角へq2をプログラムでオイラー角にする。 ``` 以上で座標系における角速度が求められる状態での表記が可能であるということがわかってきました。 この段階で間違えていたらすみませんが、アドバイスください。 この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追記いたしました。 クォータニオン便利ノートより抜粋させていただいております。 ![![イメージ説明](https://teratail.storage.googleapis.com/uploads/contributed_images/43d7d4d2e4e41c0188e407e51b46d322.png)](4b4879968eb5a2490aa553e1130434c1.png) 以上をみて考えたのですが、 クォータニオンの回転を行なったr=(x,y,z)をクォータニオンBで回転させたとする。 r'=Brが成立するのであれば、 Bの逆行列をB_1とすると両辺に左からB_1をかけて B_1*r'=rが成り立つ。 ここでいうB_1が"加速度におけるセンサー座標系から基準状態に戻すクォータニオンである",との理解はあっていますでしょうか。
回答: 持っているもの センサーの座標系での加速度 基準状態の座標系から現在のセンサー座標系への回転クォータニオン 欲しいもの 基準状態の座標系での加速度 やること 現在のセンサー座標系から基準状態に戻すクォータニオンを求めて センサー座標系での加速度に作用させる