歐拉角與旋轉矩陣

---

歐拉角因爲其奇異性，雖然在優化和插值的不會使用，但是當我們對別人描述一個旋轉的過程是怎麼樣的時候，歐拉角還是很有用的，比如，做無人機姿態控制的時候使用的就是歐拉角，但是搞明白歐拉角與旋轉矩陣的轉換確實是一件頭疼的事，所以就寫下了這篇總結，希望對大家理解歐拉角有所幫助

---

需要區分每次旋轉是繞固定軸旋轉的，還是繞旋轉之後的軸旋轉的，如果不特殊指明，下面的討論都是指：繞旋轉之後的軸旋轉的。不要侷限於表示旋轉的旋轉順序是什麼樣的，使用ZYX（先繞Z再繞Y最後繞X）僅僅是因爲習慣而已。僅僅繞單個軸旋轉一個角度得到的也是旋轉矩陣！

• 基礎旋轉矩陣：繞某單一座標軸進行旋轉對應的矩陣
• 組合旋轉矩陣：多次旋轉組合對應的矩陣

---

1、歐拉角的定義

定義一個歐拉角，需要明確下面5條：

• 三個旋轉角的組合方式
• 旋轉角度的參考座標系統（旋轉是相對於固定的座標系還是相對於自身的座標系）
• 使用旋轉角度是左手系還是右手系
• 三個旋轉角的記法
• 主動旋轉還是被動旋轉

1.1 表示旋轉的歐拉角旋轉順序有12種

• Proper/classic Euler angle
  z-x-z，x-y-x，y-z-y，z-y-z, x-z-x，y-x-y
• Tait-Bryan angle（也稱作：Cardan angles; nautical angles; heading、elevation、bank; yaw、pitch、rooll）
  x-y-z，y-z-x，z-x-y，x-z-y，z-y-x，y-x-z

Proper/classic Euler angle說明這些角度並不是獨立的，例如當下面的旋轉組合：先繞x軸旋轉90度，再繞y軸旋轉90度，最後繞x軸旋轉-90度，這一些列組合得到的效果與只繞z軸旋轉-90度是一樣的。也就是說我們僅僅在2個平面上進行旋轉（其中一個平面上必須進行兩次旋轉）就可以得到任意的三維旋轉！

1.2 內在旋轉(intrinsic rotations)和外在旋轉(extrinsic rotations)
內在旋轉每次旋轉圍繞的軸是上次旋轉之後座標系的某個軸，外在旋轉每次旋轉的軸是固定座標系中的軸。內在旋轉與外在旋轉的轉換關係：互換第一次和第三次旋轉的位置則兩者結果相同。例如Z-Y-X旋轉的內部旋轉和X-Y-Z旋轉的外部旋轉的旋轉矩陣相同

1.3 使用旋轉角度是左手系還是右手系
使用右手的大拇指指向旋轉軸，其他4個手指在握拳過程中的指向便是正的角度

• 右手系是逆時針
• 左手系是順時針

1.4 主動旋轉和被動旋轉
主動旋轉是指將向量逆時針圍繞旋轉軸旋轉，被動旋轉是對座標軸進行的逆時針旋轉，相當於主動旋轉的逆操作

---

2、不同軸的歐拉角轉換成旋轉矩陣

給出逆時針旋轉的角度爲正時（與右手系旋轉方向相同的爲旋轉正方向），繞不同軸的旋轉結果：

$R_x=\left[\begin{array}{ccc}{1} & {0} & {0} \\ {0} & {\cos \theta} & {-\sin \theta} \\ {0} & {\sin \theta} & {\cos \theta}\end{array}\right], R_y=\left[\begin{array}{ccc}{\cos \phi} & {0} & {\sin \phi} \\ {0} & {1} & {0} \\ {-\sin \phi} & {0} & {\cos \phi}\end{array}\right], R_z=\left[\begin{array}{ccc}{\cos \psi} & {-\sin \psi} & {0} \\ {\sin \psi} & {\cos \psi} & {0} \\ {0} & {0} & {1}\end{array}\right]$

假設有一個座標系 $b$（其上有一個點 $p$（點和向量是空間中一樣東西，只有當選取座標系時才討論它的的座標），座標也用 $p$ 表示），它要繞X軸逆時針轉 $α$ 角度，那麼在旋轉之後的座標系下點 $p$ 的座標（記爲，$p'$）變成了多少？做一下數學轉換得到：
$R_x*p'=p \tag{1}$

轉換一下得到，
$p'=R_x^T * p \tag{2}$

當從w系依次進行ZYX順序的旋轉得到b系，那麼根據式(2)可得$R_{bw}$：
$\begin{aligned} R_{bw} &= R_x^T * R_y^T * R_z^T \\ &= \left[\begin{array}{ccc}{1} & {0} & {0} \\ {0} & {\cos \theta} & {-\sin \theta} \\ {0} & {\sin \theta} & {\cos \theta}\end{array}\right]^T * \left[\begin{array}{ccc}{\cos \phi} & {0} & {\sin \phi} \\ {0} & {1} & {0} \\ {-\sin \phi} & {0} & {\cos \phi}\end{array}\right]^T * \left[\begin{array}{ccc}{\cos \psi} & {-\sin \psi} & {0} \\ {\sin \psi} & {\cos \psi} & {0} \\ {0} & {0} & {1}\end{array}\right]^T \\ &= \left[\begin{array}{ccc}{1} & {0} & {0} \\ {0} & {\cos \theta} & {\sin \theta} \\ {0} & {-\sin \theta} & {\cos \theta}\end{array}\right] * \left[\begin{array}{ccc}{\cos \phi} & {0} & {-\sin \phi} \\ {0} & {1} & {0} \\ {\sin \phi} & {0} & {\cos \phi}\end{array}\right] * \left[\begin{array}{ccc}{\cos \psi} & {\sin \psi} & {0} \\ {-\sin \psi} & {\cos \psi} & {0} \\ {0} & {0} & {1}\end{array}\right] \end{aligned}$

從w系變換到b系所進行的ZYX順序的旋轉的含義是什麼呢？我們知道 $t_{wb}$ 是b系的座標原點在w系下的座標，那麼此處的ZYX順序的旋轉就是指：b系在w系下的歐拉角，表示w系按照此歐拉角順序旋轉即可得到b系！既然是b系在w系下的歐拉角，那麼我們求解的應該是$R_{wb}$，所以對$R_{bw}$做轉置，得到 $R_{wb}$，如下：
$\begin{aligned} R_{wb}=R_{bw}^T &=R_z * R_y * R_x\\ &=\left[\begin{array}{ccc}{\cos \psi} & {-\sin \psi} & {0} \\ {\sin \psi} & {\cos \psi} & {0} \\ {0} & {0} & {1}\end{array}\right] * \left[\begin{array}{ccc}{\cos \phi} & {0} & {\sin \phi} \\ {0} & {1} & {0} \\ {-\sin \phi} & {0} & {\cos \phi}\end{array}\right]* \left[\begin{array}{ccc}{1} & {0} & {0} \\ {0} & {\cos \theta} & {-\sin \theta} \\ {0} & {\sin \theta} & {\cos \theta}\end{array}\right] \end{aligned}$

注意，此時的左乘順序跟旋轉順序是相反的，這也是VINS代碼裏的寫法。同樣，當從旋轉矩陣$R_{wb}$轉換成ZYX順序的歐拉角時，此歐拉角也表示：b系在w系下的歐拉角，即從w系（$R_{wb}$中目的座標系）變換到b系（$R_{wb}$中源座標系）所用的歐拉角！

從旋轉矩陣轉換歐拉角時，將歐拉角轉換成旋轉矩陣使用的什麼樣的公式，那麼從旋轉矩陣轉換成歐拉角的時候也要使用同一套公式，這樣纔不容易亂套

---

3、旋轉的本質

座標系 $b_1$（其上有一個點 $p$，座標也用 $p$ 表示），先繞X軸轉 $α$ 角度，再繞Y軸轉 $β$，再繞Z軸轉 $γ$，得到 $b_2$ 座標系，計算 $R_{b_2b_1}$？將這個過程分爲下面三步進行：
1）座標系 $b_1$ 先繞X軸轉 $α$，在新座標系 $b1'$ 下 $p$ 的座標是：
$p'=R_x^T * p$

你也可以寫成：$R_x*p'=p$，但是因爲後面接下來還有別的旋轉，後面的旋轉可以看成在本次旋轉的結果的疊加，所以爲了可持續發展，當然要使用在當前這個座標系下 $p$ 點的座標 $p'$ 了
2）座標系 $b1'$ 再繞Y軸轉 $β$ ，在新座標系 $b_1''$ 下 $p$ 的座標是：
$p''=R_y^T * p'$

3）座標系 $b_1''$ 再繞Z軸轉 $γ$，在新座標系 $b_1'''$ 下 $p$ 的座標是：
$p'''=R_z^T * p''$

將上面的過程合併（組合）得，
$p'''=R_z^T * R_y^Tp' = R_z^T * R_y^T * R_x^T * p$

所以
$R_{b_2b_1} = R_z^T * R_y^T * R_x^T$

如果明白了上面的推導過程，以後所有的旋轉都能這麼推導，因爲旋轉是疊加的（在上次的結果上繼續左乘）。假設存在這樣的旋轉，先繞X軸轉 $α$ 角度，再還是繞X軸轉 $α'$，再繞Y軸轉$β$，然後還是繞X軸轉 $α''$，整個旋轉下來組合的旋轉矩陣就是：
$R=R(α'')^T * R(β)^T * R(α')^T * R(α)^T = R(α'')^T * R(β)^T * R(α'+α)^T$

之所以 $α'$ 和 $α$ 能合併是因爲他倆的旋轉軸相同！

---

4、內部旋轉（Z-Y-X）對應的旋轉矩陣

上面按照XYZ的旋轉順序，推導得到結果是：
$R_{b_2b_1} = R_z^T * R_y^T * R_x^T$

如果將 $b_2$ 換成 $b$，$b_1$ 換成 $w$，即得，$R_{bw} = R_z^T * R_y^T * R_x^T$，再次轉置得到，
$R_{wb} = R_x * R_y * R_z$

如果我們換成ZYX順序旋轉（從w繫到b系），就自然可以得到：
$R_{wb} = R_z * R_y * R_x$

這個結果就和我們之前的推導是一樣的了！注意這是 $R_{wb}$，而不是 $R_{bw}$！見6中的驗證

---

5、爲什麼內部旋轉（Z-Y-X）和外部旋轉（X-Y-Z）對應的旋轉矩陣是相同的

因爲每次都是繞固定的座標系（記爲，$w$）進行旋轉，那麼不管旋轉之後的 $b_1$ 系在哪裏，都可以認爲 $w$ 和 $b_1$ 系是剛性連接的（第一次旋轉之前，可認爲$w$ 和 $b_1$ 是重合的），當發生了旋轉之後，那麼它們之間就會相差了一個旋轉矩陣外參：
假設先對 $w$ 做了一次繞 X軸的旋轉，得到 $b_1'$，$R_{b_1'w} = R_{w'w} = R_x^T$，這就是此時的外參

如果接着再對 $w$ 做一次繞Y軸的旋轉，得到 $b_1''$，顯然 $R_{w'w}=R_y^T$，使用外參進行轉換，
$R_{b_1''w}= (R_{b_1'w} * R_{w'w} * R_{b_1'w}^T) * R_{b_1'w} = R_x^T * R_y^T * R_x * R_x^T= R_x^T * R_y^T$

如果繞 $w$ 做一次繞Z軸的旋轉，得到 $b_1'''$，即，
$R_{b_1'''w}=R_x^T * R_y^T * R_z^T，R_{wb_1'''}=R_z * R_y * R_x$

所以就可以得到，繞固定軸旋轉的XYZ旋轉順序和繞旋轉之後的軸的ZYX旋轉順序是等價的！！！

---

6、驗證內部旋轉（Z-Y-X）得到的是 $R_{wb}$，而不是 $R_{bw}$

Eigen::Matrix3d zyxToRotationMatrix(Eigen::Vector3d zyx)
{
    // 計算旋轉矩陣的X分量
    Eigen::Matrix3d R_x;
    R_x << 1,            0,             0,
           0,  cos(zyx[2]),  -sin(zyx[2]),
           0,  sin(zyx[2]),   cos(zyx[2]);

    // 計算旋轉矩陣的Y分量
    Eigen::Matrix3d R_y;
    R_y << cos(zyx[1]),  0,  sin(zyx[1]),
                     0,  1,            0,
          -sin(zyx[1]),  0,  cos(zyx[1]);

    // 計算旋轉矩陣的Z分量
    Eigen::Matrix3d R_z;
    R_z << cos(zyx[0]),  -sin(zyx[0]),  0,
           sin(zyx[0]),   cos(zyx[0]),  0,
                     0,             0,  1;

    // 依次左乘，合併
    Eigen::Matrix3d R = R_z*R_y*R_x;
    return R;
}

int main()
{
     Eigen::AngleAxisd r_z (      0, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 軸旋轉
     Eigen::AngleAxisd r_y ( M_PI/6, Eigen::Vector3d ( 0,1,0 ) ); //沿 Y 軸旋轉
     Eigen::AngleAxisd r_x ( M_PI/6, Eigen::Vector3d ( 1,0,0 ) ); //沿 X 軸旋轉
     Eigen::Quaterniond q_zyx = r_z*r_y*r_x; //ZYX旋轉順序（繞旋轉後的軸接着旋轉）                                    
     //【1-2】: 四元數-->>旋轉矩陣                                  
     Eigen::Matrix3d rotation_matrix = q_zyx.toRotationMatrix(); //如果是從w系按照ZYX旋轉得到b系，那麼此旋轉矩陣是Rwb，不是Rbw
     cout << (rotation_matrix * Eigen::Vector3d(1,0,0)).transpose() << endl;      
     cout << (rotation_matrix.transpose() * Eigen::Vector3d(1,0,0)).transpose() << endl;

     //【2-1】: 歐拉角(機體座標系旋轉)                   
     Eigen::Vector3d euler_zyx(0, M_PI/6, M_PI/6);     
     //【2-2】: 歐拉角-->>旋轉矩陣
     rotation_matrix = zyxToRotationMatrix(euler_zyx); //此轉換結果與【1-1】相同
     cout << (rotation_matrix * Eigen::Vector3d(1,0,0)).transpose() << endl; 

//......
     return 0;
}

輸出結果是：

0.866025  -1.38778e-17 -0.5
0.866025   0.25         0.433013
0.866025   0           -0.5

將上面代碼里歐拉角顯示出來，如下圖，

測試代碼地址：https://github.com/LeatherWang/slam_car

---

<完>
@leatherwang

---