| →目次 | |||
|
| |||
|
四元数（クォータニオン：q0+q1i+q2j+q3k）の形式で移動体の姿勢を求める手法を記述するメモです。 座標系をX軸(roll、ϕ)Y軸(pitch、θ)Z軸(yaw、ψ)としてメモを記述してまします。 「参照座標系」は地面から見る座標系で、「ボディー座標系」は移動体上のセンサーから取得したデータの座標系です。 最初は、参照座標系がボディー座標系と重なるとし、移動体の姿勢が参照座標系で表現されます。 １．オイラー角からクォータニオン（四元数）へ変換 X軸をΦ角度で回転させると、下記のようなクォータニオン（四元数）を得ます。 
XYZ軸のオイラー角をϕ、θ、ψとし、X軸Y軸Z軸の順で回転すると、三次元のクォータニオン（四元数）は下記のように求めることができます。 
C++プログラムは下記ようです。 void EulerAnglesToQuaternion(double roll, double pitch, double yaw,
double& q0, double& q1, double& q2, double& q3)
{
double cosRoll = cos(roll / 2.0);
double sinRoll = sin(roll / 2.0);
double cosPitch = cos(pitch / 2.0);
double sinPitch = sin(pitch / 2.0);
double cosYaw = cos(yaw / 2.0);
double sinYaw = sin(yaw / 2.0);
q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
}
２．回転クォータニオンの乗算から回転行列の求め方 回転クォータニオンＱでベクトルＰを回転させるとして、回転行列Ｃを下記のように求めます。 Ｑ x Ｐ x Ｑ*= =(q0+q1i+q2j+q3k)(p1i+p2j+p3k)(q0-q1i-q2j-q3k) =(q0p1i+q0p2j+q0p3k + q1p1ii+q1p2ij+q1p3ik + q2p1ji+q2p2jj+q2p3jk + q3p1ki+q3p2kj+q3p3kk)(q0-q1i-q2j-q3k) =[-(q1p1+q2p2+q3p3) + (q0p1+q2p3-q3p2)i + (q0p2-q1p3+q3p1)j + (q0p3+q1p2-q2p1)k](q0-q1i-q2j-q3k) =[-(q1p1+q2p2+q3p3) + (q0p1+q2p3-q3p2)i + (q0p2-q1p3+q3p1)j + (q0p3+q1p2-q2p1)k]q0 -[-(q1p1+q2p2+q3p3) + (q0p1+q2p3-q3p2)i + (q0p2-q1p3+q3p1)j + (q0p3+q1p2-q2p1)k]q1i -[-(q1p1+q2p2+q3p3) + (q0p1+q2p3-q3p2)i + (q0p2-q1p3+q3p1)j + (q0p3+q1p2-q2p1)k]q2j -[-(q1p1+q2p2+q3p3) + (q0p1+q2p3-q3p2)i + (q0p2-q1p3+q3p1)j + (q0p3+q1p2-q2p1)k]q3k =-(q0q1p1+q0q2p2+q0q3p3) + (q0q0p1+q0q2p3-q0q3p2)i + (q0q0p2-q0q1p3+q0q3p1)j + (q0q0p3+q0q1p2-q0q2p1)k +(q1q1p1+q1q2p2+q1q3p3)i + (q0q1p1+q1q2p3-q1q3p2) + (q0q1p2-q1q1p3+q1q3p1)k - (q0q1p3+q1q1p2-q1q2p1)j +(q1q2p1+q2q2p2+q2q3p3)j - (q0q2p1+q2q2p3-q2q3p2)k + (q0q2p2-q1q2p3+q2q3p1) + (q0q2p3+q1q2p2-q2q2p1)i +(q1q3p1+q2q3p2+q3q3p3)k + (q0q3p1+q2q3p3-q3q3p2)j - (q0q3p2-q1q3p3+q3q3p1)i + (q0q3p3+q1q3p2-q2q3p1) =(+q0q0p1+q0q2p3-q0q3p2 +q1q1p1+q1q2p2+q1q3p3 +q0q2p3+q1q2p2-q2q2p1 -q0q3p2+q1q3p3-q3q3p1)i +(+q0q0p2-q0q1p3+q0q3p1 -q0q1p3-q1q1p2+q1q2p1 +q1q2p1+q2q2p2+q2q3p3 +q0q3p1+q2q3p3-q3q3p2)j +(+q0q0p3+q0q1p2-q0q2p1 +q0q1p2-q1q1p3+q1q3p1 -q0q2p1-q2q2p3+q2q3p2 +q1q3p1+q2q3p2+q3q3p3)k =(+q0q0p1+q1q1p1-q2q2p1-q3q3p1 +q1q2p2+q1q2p2-q0q3p2-q0q3p2 +q0q2p3+q0q2p3+q1q3p3+q1q3p3)i +(+q0q3p1+q0q3p1+q1q2p1+q1q2p1 +q0q0p2-q1q1p2+q2q2p2-q3q3p2 -q0q1p3-q0q1p3+q2q3p3+q2q3p3)j +(+q1q3p1+q1q3p1-q0q2p1-q0q2p1 +q0q1p2+q0q1p2+q2q3p2+q2q3p2 +q0q0p3-q1q1p3-q2q2p3+q3q3p3)k =[(q0q0+q1q1-q2q2-q3q3)p1 + (q1q2+q1q2-q0q3-q0q3)p2 + (q0q2+q0q2+q1q3+q1q3)p3]i +[(q0q3+q0q3+q1q2+q1q2)p1 + (q0q0-q1q1+q2q2-q3q3)p2 + (q2q3+q2q3-q0q1-q0q1)p3]j +[(q1q3+q1q3-q0q2-q0q2)p1 + (q0q1+q0q1+q2q3+q2q3)p2 + (q0q0-q1q1-q2q2+q3q3)p3]k =[(q02+q12-q22-q32)p1 + 2(q1q2-q0q3)p2 + 2(q0q2+q1q3)p3]i +[2(q0q3+q1q2)p1 + (q02-q12+q22-q32)p2 + 2(q2q3-q0q1)p3]j +[2(q1q3-q0q2)p1 + 2(q0q1+q2q3)p2 + (q02-q12-q22+q32)p3]k 上記の結果から下図に示す回転行列Ｃを作れました。 
３．クォータニオン（四元数）からオイラー角へ変換 「オイラー角と回転行列間の変換」で出来た回転行列を回転クォータニオンからできた回転行列と比べると、クォータニオン（四元数）からオイラー角への変換が下記のように求められます。 
C++プログラムは下記ようです。 void QuaternionToEulerAngles(double q0, double q1, double q2, double q3,
double& roll, double& pitch, double& yaw)
{
double q0q0 = q0 * q0;
double q0q1 = q0 * q1;
double q0q2 = q0 * q2;
double q0q3 = q0 * q3;
double q1q1 = q1 * q1;
double q1q2 = q1 * q2;
double q1q3 = q1 * q3;
double q2q2 = q2 * q2;
double q2q3 = q2 * q3;
double q3q3 = q3 * q3;
roll = atan2(2.0 * (q2q3 + q0q1), q0q0 - q1q1 - q2q2 + q3q3);
pitch = asin(2.0 * (q0q2 - q1q3));
yaw = atan2(2.0 * (q1q2 + q0q3), q0q0 + q1q1 - q2q2 - q3q3);
}
| |||
|
４．ボディー座標系と参照座標系間の回転行列作成 ボディー座標系⇒参照座標系の回転行列は下図のようになります。 
{xn,yn,zn}は参照座標系のベクトルで、{xb,yb,zb}はボディー座標系のベクトルです。C++プログラムは下記ようです。センサーから取得した加速度を参照座標系へ変換の例です。 （ソースの中で、q0*q0 + q1*q1 + q2*q2 + q3*q3 = 1を利用しています） double q0, double q1, double q2, double q3;
double ax, double ay, double az; // ボディ座標
double ax_ref, double ay_ref, double az_ref; //参照系座標
void BodyAccelToRefAcce()
{
double q0q0 = q0 * q0;
double q0q1 = q0 * q1;
double q0q2 = q0 * q2;
double q0q3 = q0 * q3;
double q1q1 = q1 * q1;
double q1q2 = q1 * q2;
double q1q3 = q1 * q3;
double q2q2 = q2 * q2;
double q2q3 = q2 * q3;
double q3q3 = q3 * q3;
ax_ref = 2 * ax*(0.5 - q2q2 - q3q3) + 2 * ay*(q1q2 - q0q3) + 2 * az*(q1q3 + q0q2);
ay_ref = 2 * ax*(q1q2 + q0q3) + 2 * ay*(0.5 - q1q1 - q3q3) + 2 * az*(q2q3 - q0q1);
az_ref = 2 * ax*(q1q3 - q0q2) + 2 * ay*(q2q3 + q0q1) + 2 * az*(0.5 - q1q1 - q2q2);
}
この回転行列の転置行列を使用して、参照座標系のベクトルをボディー座標系へ変換できます。回転行列の転置行列は下図になります。 
プログラムは省略です。 ５．オイラー角変化（微分）によりクォータニオンの変化 下図は、クォータニオンの微分方程式、オイラー角の変化（微分）によりクォータニオンの変化の式を示します。 
前周期のクォータニオンと当周期のクォータニオンの関係はqi+1=qi + (1/2)Tωqiになります。 struct QUATERNION {
double q0; double q1; double q2; double q3;
};
角速度が参照座標系（地面座標系）の場合：
QUATERNION QuaternionUpdate(QUATERNION& quat, double wx, double wy, double wz, double t)
{
double gx = wx * t / 2.0;
double gy = wy * t / 2.0;
double gz = wz * t / 2.0;
QUATERNION quat1 = quat;
quat1.q0 += ( 0 - quat1.q1 * gx - quat1.q2 * gy - quat1.q3 * gz);
quat1.q1 += ( quat1.q0 * gx +0 - quat1.q2 * gz - quat1.q3 * gy);
quat1.q2 += ( quat1.q0 * gy + quat1.q1 * gz +0 - quat1.q3 * gx);
quat1.q3 += ( quat1.q0 * gz - quat1.q1 * gy + quat1.q2 * gx +0 );
return quat1;
}
角速度が移動体座標系（ボディー座標系）の場合：
QUATERNION QuaternionUpdate(QUATERNION& quat, double wx, double wy, double wz, double t)
{
double gx = wx * t / 2.0;
double gy = wy * t / 2.0;
double gz = wz * t / 2.0;
QUATERNION quat1 = quat;
quat1.q0 += ( 0 - quat1.q1 * gx - quat1.q2 * gy - quat1.q3 * gz);
quat1.q1 += ( quat1.q0 * gx +0 + quat1.q2 * gz - quat1.q3 * gy);
quat1.q2 += ( quat1.q0 * gy - quat1.q1 * gz +0 + quat1.q3 * gx);
quat1.q3 += ( quat1.q0 * gz + quat1.q1 * gy - quat1.q2 * gx +0 );
return quat1;
}
|