歐拉角，軸角對和四元數及其互相轉化
這似乎永遠是做姿態解算入門的人繞不過去的坎兒...
歐拉角和四元數(還有方向餘弦矩陣DCM, 軸角對，羅格里德各參數等)都是描述剛體姿態的方法，其中歐拉角最直觀, 四元數計算機運算最方便，方向餘弦矩陣(DCM) 最早也最傳統。除了歐拉角，四元數和DCM，還有其他的表述方法，比如 CRP/ MRP，軸角對等等，但是小白入門時候用的並不是很多。這篇文章就介紹四元數和歐拉角。
現代一般低成本嵌入式姿態解算中，姿態表示都採取四元數，最終轉換成歐拉角只為讓人類看起來直觀。
歐拉角（Euler Angle）
歐拉角使用三個角度值表示剛體運動的姿態：
Yaw角-航向角- -psi：
Pitch角-俯仰角- -theta
Roll角-橫滾角- - phi
總結：定義機頭方向為X軸，翅膀方向為Y，然後用右手定則確定Z方向：
- 繞 Z 軸方向旋轉: 航向角yaw
範圍: -180° - 180°
- 繞 Y 軸方向旋轉: 俯仰角pitch
範圍: -90°-90°
- 繞 X 軸方向旋轉:橫滾角
oll範圍: -180°-180°
歐拉角很直觀，三個圖，不怎麼用文字就可以說明瞭。但歐拉角兩個致命缺點(特點)：
- 歐拉角有順序這麼一說: 先轉哪個軸，再轉哪個軸，最後轉哪個軸。 不同的轉法最後的歐拉角的值也不一樣(這也是為什麼網上的四元數轉歐拉角公式不統一的原因)，最流氓的就是給一個四元數轉歐拉角公式，而不告訴你歐拉角的旋轉順序是啥。額。所以，記住：說到歐拉角，不僅僅是三個值，一定還要跟上說你的旋轉順序是啥。
- 著名的歐拉死鎖問題：
任何只用三個數來表示剛體姿態的表述方法都會存在死鎖問題. 這是數學問題，無法避免。可以通過四元數或者更多數字的表示方法(如DCM)來解決。
軸角對(Aixs-Angle)
繞坐標軸的多次旋轉可以等效為繞某一轉軸旋轉一定的角度。這是除了歐拉角外，最直觀的一種表述方法
例: 繞旋轉軸為 ,旋轉90°可以表示為如下形式(實際上就是繞著Z軸轉90度)
四元數(Quaternion)
知道了軸角對，那麼四元數的概念就非常好引入： 其中
是軸角對中的角，而
就是軸角對中的軸向量，前面的cos叫做標量部分叫做
，後面的 sin叫做矢量部分，叫做
. 有的表示方法把標量放到最前面，有的放到最後面，要注意區分. 所以四元數一般寫作
或者
(wxyz 的情況居多)
四元數和歐拉角的互相轉化
雖然歐拉角有多種旋轉順序(一共有12種！嚇人不嚇人？)。但通常我們使用321(ZYX)順序的歐拉角，這也是matlab的默認順序。
可以用matlab直接體驗下四元數轉歐拉角函數：
clear; clc;
q = [-0.377964,0.755929,0.377964,0.37796];
[yaw, pitch, roll] = quat2angle(q, ZYX)
yaw =
0.5880
pitch =
-1.0297
roll =
-2.5536
你也可以嘗試用不同的旋轉順序，比如這裡再來一個213(YXZ)順序：
q = [-0.377964,0.755929,0.377964,0.37796];
[pitch, roll, yaw] = quat2angle(q, YXZ)
pitch =
2.5536
roll =
-1.0297
yaw =
2.5536
啊，好爽，matlab大法好，不過要注意哦，213順序時候，matlab輸出的可是pitch,roll,yaw ，而不是原來的yaw,pitch,roll ,這點不要忘記。
關於四元數轉歐拉角，可以用下面的網站和matlab互相印證學習：（注意四元數是 w,x,y,z 還是x,y,z,w 表示，還有，記住： X軸對應roll , Y軸對應pitch ，Z軸對應yaw）
https://www.andre-gaschler.com/rotationconverter/四元數轉歐拉角(C 語言)
這是C語言版本的四元數轉歐拉角函數，可以和上面matlab和 網站的轉換結果對應參考，其中
/* dump eul angles */
void dump_eul(float32_t *eul, const char *seq)
{
printf("EA seq:%s ", seq);
if(!strcmp(seq, "321"))
{
printf("(YPR):");
}
if(!strcmp(seq, "123"))
{
printf("(RPY):");
}
if(!strcmp(seq, "231"))
{
printf("(PYR):");
}
if(!strcmp(seq, "213"))
{
printf("(PRY):");
}
if(!strcmp(seq, "312"))
{
printf("(YRP):");
}
printf("%8.4f %8.4f %8.4f
", eul[0], eul[1], eul[2]);
}
/*
convert quat to eul angles:
321: eul[0-2] = YPR ENU:aerospace sequence, where rotation around z-axis (Yaw) takes place first, which is then followed by Pitch (around y-axis), and Roll (around x-axis).
123: eul[0-2] = RPY
231: eul[0-2] = PYR
213: eul[0-2] = PRY
312: eul[0-2] = YRP
*/
void quat2eul(float32_t *q, float32_t *eul, const char *seq)
{
float q0, q1, q2, q3;
q0 = q[0];
q1 = q[1];
q2 = q[2];
q3 = q[3];
if(!strcmp(seq, "321"))
{
eul[0] = atan2(2*(q1*q2+q0*q3),q0*q0+q1*q1-q2*q2-q3*q3);
eul[1] = asin(-2*(q1*q3-q0*q2));
eul[2] = atan2(2*(q2*q3+q0*q1),q0*q0-q1*q1-q2*q2+q3*q3);
}
if(!strcmp(seq, "123"))
{
eul[0] = atan2(-2*(q2*q3-q0*q1),q0*q0-q1*q1-q2*q2+q3*q3);
eul[1] = asin(2*(q1*q3 + q0*q2));
eul[2] = atan2(-2*(q1*q2-q0*q3),q0*q0+q1*q1-q2*q2-q3*q3);
}
if(!strcmp(seq, "132"))
{
eul[0] = atan2(2*(q2*q3+q0*q1),q0*q0-q1*q1+q2*q2-q3*q3);
eul[1] = asin(-2*(q1*q2-q0*q3));
eul[2]= atan2(2*(q1*q3 + q0*q2),q0*q0+q1*q1-q2*q2-q3*q3);
}
if(!strcmp(seq, "231"))
{
eul[0] = atan2(-2*(q1*q3-q0*q2), q0*q0+q1*q1-q2*q2-q3*q3);
eul[1] = asin(2*(q1*q2+q0*q3));
eul[2]= atan2(-2*(q2*q3-q0*q1),q0*q0-q1*q1+q2*q2-q3*q3);
}
if(!strcmp(seq, "213"))
{
eul[0] = atan2(2*(q1*q3 + q0*q2),q0*q0-q1*q1-q2*q2+q3*q3);
eul[1] = asin(-2*(q2*q3-q0*q1));
eul[2]= atan2(2*(q1*q2+q0*q3),q0*q0-q1*q1+q2*q2-q3*q3);
}
if(!strcmp(seq, "312"))
{
eul[0] = atan2(-2*(q1*q2-q0*q3),q0*q0-q1*q1+q2*q2-q3*q3);
eul[1] = asin(2*(q2*q3+q0*q1));
eul[2]= atan2(-2*(q1*q3-q0*q2),q0*q0-q1*q1-q2*q2+q3*q3);
}
}
/* a test function */
void quat2eul_test(void)
{
float eul[3];
float q[4] = {-0.377964, 0.755929, 0.377964, 0.37796};
dump_vec("quat:
", q, 4);
quat2eul(q, eul, "321");
dump_eul(eul, "321");
quat2eul(q, eul, "123");
dump_eul(eul, "123");
quat2eul(q, eul, "231");
dump_eul(eul, "231");
quat2eul(q, eul, "213");
dump_eul(eul, "213");
quat2eul(q, eul, "312");
dump_eul(eul, "312");
}
參考
https://www.astro.rug.nl/software/kapteyn-beta/_downloads/attitude.pdf
https://www.cnblogs.com/21207-iHome/p/6894128.html
推薦閱讀：
