三維空間剛體運動3：歐拉角表示旋轉

本篇繼續參照高翔老師《視覺SLAM十四講從理論到實踐》，講解三維空間剛體運動。博文將原第三講分爲四部分來講解：1、旋轉矩陣和變換矩陣；2、旋轉向量表示旋轉；3、歐拉角表示旋轉；4、四元數表示變換。本文相對於原文會適當精簡，同時爲便於理解，會加入一些註解和補充知識點，本篇爲第三部分：歐拉角表示旋轉，另外三部分請參照博主的其他博文。

1. 歐拉角定義

無論是旋轉矩陣還是旋轉向量，它們雖然能描述旋轉，但對人類來說非常不直觀，而歐拉角則提供了一種非常直觀的方式來描述旋轉----它使用了3個分離的轉角，把一個旋轉分解成3次繞不同軸的旋轉。

歐拉角：歐拉角是在空間中，描述從一個用於表示某個固定的參考系的、已知的方向，經過一系列基本旋轉得到、新的代表另一個參考系的方向的方式。因爲只有旋轉，所以原點位置並沒有發生變化。

在討論歐拉角的具體形式之前，需要明確幾個概念：

1. 左手座標系與右手座標系：既然是在座標系中進行變換，首先就要了解座標系的類別。同樣的數據在左手座標系和右手座標系會有不同的呈現結果。本文在右手座標系中討論。
2. 旋轉的具體形式：物體旋轉有多種，比如繞某個定點進行旋轉，繞座標軸進行旋轉，繞任意軸進行旋轉。不同的旋轉需要用不同的數學方式來表達。注意，本文討論的是三維中繞座標軸的旋轉。
3. 歐拉角順規：歐拉角定義了一組物體的旋轉次序，稱爲順規，可以理解爲歐拉角旋轉順序的規定。(α, β, γ)在不同的旋轉順序下會有不同的結果，先繞X軸旋轉α，還是先繞y軸旋轉β，最後的結果是不一樣的。
4. 靜態動態歐拉角：從參考座標系上區分，將歐拉角分爲靜態和動態，其中靜態歐拉角以絕對座標系爲參考，一般用小寫的x-y-z來表示靜態座標系；動態歐拉角以剛體自身的物體座標系爲參考，一般用大寫的X-Y-Z表示。

rpy：歐拉角定義方式上的不確定性帶來了很多實際當中的困難，所幸在特定領域內，歐拉角通常有統一的定義方式。當中常用的一種是航空中的，用“偏航-仰俯-滾轉”（yaw-pitch-row）3個角度來描述的旋轉，它等價於繞ZYX軸的旋轉。那麼，ZYX轉角相當於把任意旋轉分解成以下3個軸的轉角，如圖：
圖1.1 Oxyz座標軸旋轉

1. 繞物體的Z軸旋轉$\psi$，得到偏航角yaw。
2. 繞旋轉之後的Y軸旋轉$\theta$，得到仰俯角pitch。
3. 繞旋轉之後的X軸旋轉$\phi$，得到滾轉角roll。

此時，可以使用$[r,p,y]^{T}$這樣一個三維的向量描述任意旋轉。rpy角的旋轉順序是ZYX。此外，爲規避萬向鎖現象，常用的順序是ZXY，但無法消除。下面講解歐拉角的推導，然後引出萬向鎖問題。

2. 歐拉角到旋轉矩陣

一個旋轉矩陣具有三個自由度，可以與歐拉角進行轉換。下邊推導歐拉角到旋轉矩陣的轉換。歐拉角旋轉每次只繞一個座標軸旋轉，因此推導可分解爲三次二維情況下的座標變換，再延伸到三維。二維下稱之爲基本旋轉矩陣，由基本旋轉組成三維下的組合旋轉矩陣。先討論二維座標下的情景。如下圖所示：
圖2.1 二維座標旋轉

點$P(x,y)$在原座標系中與X軸夾角爲$\alpha$，繞Z軸旋轉角$\theta$後的座標爲$P'(x',y')$，現在來推導$(x',y')$關於$(x,y)$的表示法。
推導方法有兩種，第一種是利用三角恆等式的兩角和差公式，第二種是利用三角形的性質推導。本文采用第一種，對第二種感興趣的童鞋可祥閱《旋轉矩陣（Rotation Matrix）的推導及其應用》。
由圖可知，$OP=OP'=\sqrt{x^{2} + y^{2}}=d$，由正餘弦定理可得到兩組方程：$\left\{\begin{matrix} \sin\alpha = \frac{y}{d}\\ \cos\alpha = \frac{x}{d} \end{matrix}\right. \tag{2.1}$和$\left\{\begin{matrix} \sin(\alpha + \theta )= \frac{y'}{d}\\ \cos(\alpha + \theta )= \frac{x'}{d} \end{matrix}\right. \tag{2.2}$
由兩角和差公式可知：$\left\{\begin{matrix} \sin(\alpha + \theta )= \sin\alpha\cos\theta+\cos\alpha\sin\theta\\ \cos(\alpha + \theta )= \cos\alpha\cos\theta-\sin\alpha\sin\theta \end{matrix}\right. \tag{2.3}$將式（2.1）和（2.3）代入（2.2），消掉$d$得到：$\left\{\begin{matrix} x' = x\cos\theta-y\sin\theta\\ y' = y\cos\theta+x\sin\theta \end{matrix}\right.\tag{2.4}$因此有如下推導，設$P'$的齊次座標爲$\tilde{P'}$：$\tilde{P'}=[x',y',1]^{T}=\begin{bmatrix} cos\theta & -\sin\theta & 0\\ \sin\theta & \cos\theta & 0\\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} x\\ y\\ 1 \end{bmatrix}=R_Z(\theta )\begin{bmatrix} x\\ y\\ 1 \end{bmatrix}\tag{2.5}$因此，繞Z軸的旋轉矩陣爲：$R_Z(\theta )=\begin{bmatrix} cos\theta & -\sin\theta & 0\\ \sin\theta & \cos\theta & 0\\ 0 & 0 & 1 \end{bmatrix}\tag{2.6}$同理可推得繞Y軸的旋轉矩陣爲：$R_Y(\theta )=\begin{bmatrix} cos\theta & 0 & \sin\theta\\ 0 & 1 & 0\\ -\sin\theta & 0 & \cos\theta \end{bmatrix}\tag{2.7}$繞X軸的旋轉矩陣爲：$R_X(\theta )=\begin{bmatrix} 1 & 0 & 0\\ 0 & \cos\theta & -\sin\theta\\ 0 & \sin\theta & \cos\theta \end{bmatrix}\tag{2.8}$組合旋轉矩陣等於基本旋轉矩陣的連乘，連乘順序依基本旋轉的先後次序由右向左排列，因此順規爲ZYX的組和旋轉矩陣爲：$R_{ZYX}=R_X(\phi )R_Y(\theta )R_Z(\psi )\tag{2.9}$

3. 旋轉矩陣到歐拉角

已知旋轉矩陣，如何求歐拉角呢？只需要對應到旋轉矩陣，求反弦值即可。將式（2.9）兩側展開得：$\begin{bmatrix} R_{11} & R_{12} & R_{13}\\ R_{21} & R_{22} & R_{23}\\ R_{31} & R_{32} & R_{33} \end{bmatrix}= \begin{bmatrix} \cos \theta \cos \psi & -\cos \theta \sin \psi & \sin \theta \\ \sin \phi \sin \theta \cos \psi +\cos \phi \sin \psi & -\sin \phi \sin \theta \sin \psi +\cos \phi \cos \psi & -\sin \phi \cos \theta \\ -\cos \phi \sin \theta \cos \psi +\sin \phi \sin \psi & \cos \phi \sin \theta \sin \psi +\sin \phi \cos \psi & \cos \phi \cos \theta \end{bmatrix}\tag{3.1}$
因此推得：$\left\{\begin{matrix} \phi =\arctan (-\frac{R_{23}}{R_{33}})\\ \theta =\arcsin R_{13}\\ \psi =\arctan (-\frac{R_{12}}{R_{11}}) \end{matrix}\right.\tag{3.2}$由上面推導，引出萬向鎖問題。

4. 萬向鎖

4.1 定義

萬向鎖問題：歐拉角的一個重大缺點就是會碰到著名的萬向鎖問題（Gimbal Lock）,對於rpy，當仰俯角爲$\pm 90^{\circ }$時，第一次旋轉與第三次旋轉使用同一個軸，使得系統丟失了一個自由度（由3次旋轉變成了兩次旋轉）。這被稱爲奇異性問題，其它順規形式的歐拉角也同樣存在奇異性問題。

理論上可以證明，只要想用3個實數來表達三維旋轉，都不可避免的碰到奇異性問題。由於這種原理，歐拉角不適用於插值和迭代，往往只用於人機交互中，因此我們很少在SLAM程序、濾波以及優化中使用歐拉角表達旋轉。

4.2 順規ZYX的萬向鎖

萬向鎖理論性描述講完了，相信大部分人都沒理解，網文解釋的也比較模糊。以外國小哥的視頻：歐拉旋轉，配合截圖，試講解如下：

以歐拉角的順規ZYX爲例，如下圖所示：圖中藍色爲Z軸，綠色爲Y軸，紅色爲X軸，它們有一定的層級關係，Z爲Y的父級，Y爲X的父級。當繞Z軸旋轉時，它的子級Y軸和X軸也會跟着移動；當繞Y軸旋轉時，X軸會隨之移動，Z軸不發生改變；當繞X軸旋轉時，Z軸和Y軸都不會發生改變。層級關係是理解萬向節鎖問題的關鍵。初始狀態如圖所示：
圖4.1 ZYX歐拉旋轉-1

箭頭繞Z軸旋轉，此時Y軸和X軸也跟着變化，如圖所示：
圖4.2 ZYX歐拉旋轉-2

箭頭繼續繞Y軸旋轉，此時Z軸固定不變，X軸隨之變化，如圖所示：
圖4.3 ZYX歐拉旋轉-3
當Y軸旋轉角度爲$\pm 90^{\circ }$時，X軸和Z軸重合共面，丟失了一個自由度。三次旋轉變換僅僅覆蓋了兩個外部軸的旋轉，一個自由度就這樣丟失了，這也就導致了 Gimbal Lock 的現象。Gimbal Lock 問題的核心還是在於我們採用了固定的旋轉順序。對於其它5中組合的順規，當中間的軸正好旋轉$\pm 90^{\circ }$或兩側軸重合時，同樣會產生萬向鎖現象。
這個變換用公式來理解的話，則是這樣（你可以自己來代入驗證一下），將$(\phi ,\frac{\pi }{2} ,\psi)$代入式（3.1）得：$\begin{aligned} R_{ZYX}(\phi ,\frac{\pi }{2} ,\psi ) &= R_{X}(\psi )R_{Y}(\frac{\pi }{2})R_{Z}(\phi )\\ &= \begin{bmatrix} 0 & 0 & 1\\ \sin \phi \cos \psi +\cos \phi \sin \psi & -\sin \phi \sin \psi +\cos\phi \cos \psi & 0\\ -\cos \phi \cos \psi +\sin \phi \sin \psi & \cos \phi \sin \psi +\sin \phi \cos \psi & 0 \end{bmatrix}\\&= \begin{bmatrix} 0 & 0 & 1\\ \sin (\phi + \psi ) & \cos (\phi + \psi ) & 0\\ -\cos (\phi + \psi ) & \sin (\phi + \psi ) & 0 \end{bmatrix}\\&=R_{Y}(\frac{\pi }{2})R_{Z}(\phi + \psi ) \end{aligned}\tag{4.1}$
利用一些三角恆等式，將原本由三個旋轉矩陣所組成的變換化簡成了兩個變換矩陣。即便我們分別對$Z-Y-X$三軸進行了旋轉，實際上這個矩陣僅僅旋轉了$Z-Y$兩軸，它並沒有對初始的X 軸進行變換。$R_{Z}(\psi)$ 與$R_{X}(\phi)$這兩個變換被合併爲單獨的一個$R_{Z}(\psi + \phi)$變換（因爲化簡完之後變換順序不一樣了，嚴格來說並不是合併，只不過是能夠使用一步來完成）。
注意，我們在這裏化簡的並不是單獨的一個變換，而是一系列變換。因爲它對任意 $\phi$ 和$\psi$角都是成立的。也就是說，一旦 𝑦 軸上的變換角將這兩個旋轉軸對齊，我們就沒有任何辦法對最初的$X$軸進行旋轉了。無論$X$軸與$Z$軸的旋轉角是多少，變換都會喪失一個自由度。

4.3 解決方法

筆者收集到兩種解決辦法，第一種會盡大可能規避，但無法避免；第二種能徹底解決，但會耗費一定內存。如有其它方法，請留言告知。

1. 在三維空間中，無法完全避免，竅門是找到物體不太可能指向的方向，可以儘量規避這種現象。如對於相機模型，想象人持相機，建立右手座標系，正前方爲Y軸正向，左手側爲X軸正向，頭頂爲Z軸正向。則最常用的方向爲正向握持，即Y軸正向，將相機樹立對應X軸，此種操作相對較少，而朝天或朝地下拍攝，對應Z軸，即相機向上或倒放的操作最少，因此確立順規爲ZXY的歐拉旋轉，可以最大可能規避萬向鎖問題，但無法完全避免。
2. 另外一種徹底解決的辦法是，將歐拉角轉換爲四元數，對四元數進行slerp即球面線性插值，再將這一系列四元數轉換爲對應的歐拉角，而後作用於角色。缺點是耗費一定的內存，但角色可以任意旋轉，靈活度高。四元數部分詳見下一篇博客。

至此，推導部分結束，讓我們練練手吧。

5. 實踐：Eigen幾何模塊

現在，我們實際演練前面講到的各種旋轉表達方式。將在Eigen中使用旋轉向量、歐拉角和旋轉矩陣，演示它們之間的變換方式。
代碼可在博客資源中下載，截取部分如下（已加註釋）：

#include<iostream>
#include<cmath>
using namespace std;

#include<eigen3/Eigen/Core>
#include<eigen3/Eigen/Geometry>

using namespace Eigen;

//本程序演示了Eigen幾何模塊的使用方法
int main(int argc, char **argv){
    //Eigen/Geometry模塊提供了各種旋轉和平移的表示，3D旋轉矩陣直接使用Matrix3d或Matrix3f
    Matrix3d rotation_matrix = Matrix3d::Identity();
    //旋轉向量使用AngleAxis，它底層不直接是Matrix，但運算可以當做矩陣，因爲重載了運算符
    AngleAxisd rotation_vector(M_PI/4, Vector3d(0, 0, 1));
    //設置輸出精度
    cout.precision(3);
    cout<<"rotation matrix =\n"<<rotation_matrix<<endl;
    cout<<"rotation vector =\n"<<rotation_vector.matrix()<<endl;
    
    //旋轉向量轉換的矩陣可以直接賦值給旋轉矩陣
    rotation_matrix = rotation_vector.toRotationMatrix();
    cout<<"rotation vector to Matrix =\n"<<rotation_matrix<<endl;
    
    //用旋轉向量進行座標變換
    Vector3d v(1, 0, 0);
    Vector3d v_rotated = rotation_vector * v;
    cout<<"(1,0,0) after rotation (by angle axis) = "<<v_rotated.transpose()<<endl;
    //用旋轉矩陣進行座標變換
    v_rotated = rotation_matrix * v;
    cout<<"(1,0,0) after rotation (by matrix) = "<<v_rotated.transpose()<<endl;

    //歐拉角：可以將旋轉矩陣直接轉換成歐拉角
    Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0);
    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;
    //cout<<"euler angles matrix= "<<euler_angles.matrix()<<endl;
    
    //歐式變換矩陣使用Eigen::Isometry3d
    Isometry3d T = Isometry3d::Identity();
    cout<<"Isometry3d T = \n"<<T.matrix()<<endl; 
    T.rotate(rotation_vector);
    cout<<"T rotate rotation_vector = \n"<<T.matrix()<<endl; 
    T.pretranslate(Vector3d(1,3,4));
    cout<<"Transform matrix = \n"<<T.matrix()<<endl; 
    
    //用變換矩陣進行座標變換
    Vector3d v_transformed = T * v;
    cout<<"v = "<<v.transpose()<<endl;
    cout<<"v transofrmed = "<<v_transformed.transpose()<<endl;
}

運行結果如下：

rotation matrix =
1 0 0
0 1 0
0 0 1
rotation vector =
 0.707 -0.707      0
 0.707  0.707      0
     0      0      1
rotation vector to Matrix =
 0.707 -0.707      0
 0.707  0.707      0
     0      0      1
(1,0,0) after rotation (by angle axis) = 0.707 0.707     0
(1,0,0) after rotation (by matrix) = 0.707 0.707     0
yaw pitch roll = 0.785    -0     0
Isometry3d T = 
1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
T rotate rotation_vector = 
 0.707 -0.707      0      0
 0.707  0.707      0      0
     0      0      1      0
     0      0      0      1
Transform matrix = 
 0.707 -0.707      0      1
 0.707  0.707      0      3
     0      0      1      4
     0      0      0      1
v = 1 0 0
v transofrmed = 1.71 3.71    4

參考：

1. 《視覺SLAM十四講：從理論到實踐》，高翔、張濤等著，中國工信出版社
2. 圖形變換之旋轉變換公式推導
3. Bonus: Gimbal Lock By Krasjet