欧拉角与旋转矩阵的转换关系

欧拉角因为其奇异性，虽然在优化和插值的不会使用，但是当我们对别人描述一个旋转的过程是怎么样的时候，欧拉角还是很有用的，比如，做无人机姿态控制的时候使用的就是欧拉角，但是搞明白欧拉角与旋转矩阵的转换确实是一件头疼的事，所以就写下了这篇总结，希望对大家理解欧拉角有所帮助

需要区分每次旋转是绕固定轴旋转的，还是绕旋转之后的轴旋转的，如果不特殊指明，下面的讨论都是指：绕旋转之后的轴旋转的。不要局限于表示旋转的旋转顺序是什么样的，使用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 \varphi & 0& \sin \varphi \\ 0& 1& 0\\ -\sin \varphi & 0& \cos \varphi \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轴逆时针转 $\alpha$ 角度，那么在旋转之后的坐标系下点 $p$ 的坐标（记为，$p^{\prime }$）变成了多少？做一下数学转换得到：
$$R_x\ast p^{\prime }=p\text{\hspace{1em}(1)}$$

当从w系依次进行ZYX顺序的旋转得到b系，那么根据式(2)可得$R_{bw}$：
$$\begin{array}{rl}{R}_{bw}& =R_x^T\ast R_y^T\ast R_z^T\\ & ={\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right]}^T\ast {\left[\begin{array}{ccc}\cos \varphi & 0& \sin \varphi \\ 0& 1& 0\\ -\sin \varphi & 0& \cos \varphi \end{array}\right]}^T\ast {\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]\ast \left[\begin{array}{ccc}\cos \varphi & 0& -\sin \varphi \\ 0& 1& 0\\ \sin \varphi & 0& \cos \varphi \end{array}\right]\ast \left[\begin{array}{ccc}\cos \psi & \sin \psi & 0\\ -\sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right]\end{array}$$

从w系变换到b系所进行的ZYX顺序的旋转的含义是什么呢？我们知道 $t_{wb}$ 是b系的坐标原点在w系下的坐标，那么此处的ZYX顺序的旋转就是指：b系在w系下的欧拉角，表示w系按照此欧拉角顺序旋转即可得到b系！既然是b系在w系下的欧拉角，那么我们求解的应该是$R_{wb}$，所以对$R_{bw}$做转置，得到 $R_{wb}$，如下：
$$\begin{array}{rl}{R}_{wb}=R_{bw}^T& =R_z\ast R_y\ast R_x\\ & =\left[\begin{array}{ccc}\cos \psi & -\sin \psi & 0\\ \sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right]\ast \left[\begin{array}{ccc}\cos \varphi & 0& \sin \varphi \\ 0& 1& 0\\ -\sin \varphi & 0& \cos \varphi \end{array}\right]\ast \left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right]\end{array}$$

注意，此时的左乘顺序跟旋转顺序是相反的，这也是VINS代码里的写法。同样，当从旋转矩阵$R_{wb}$转换成ZYX顺序的欧拉角时，此欧拉角也表示：b系在w系下的欧拉角，即从w系（$R_{wb}$中目的坐标系）变换到b系（$R_{wb}$中源坐标系）所用的欧拉角！

从旋转矩阵转换欧拉角时，将欧拉角转换成旋转矩阵使用的什么样的公式，那么从旋转矩阵转换成欧拉角的时候也要使用同一套公式，这样才不容易乱套

3、旋转的本质

坐标系 $b_1$（其上有一个点 $p$，坐标也用 $p$ 表示），先绕X轴转 $\alpha$ 角度，再绕Y轴转 $\beta$，再绕Z轴转 $\gamma$，得到 $b_2$ 坐标系，计算 $R_{b_2{b}_1}$？将这个过程分为下面三步进行：
1）坐标系 $b_1$ 先绕X轴转 $\alpha$，在新坐标系 $b{1}^{\prime }$ 下 $p$ 的坐标是：
$$p^{\prime }=R_x^T\ast p$$

你也可以写成：$R_x\ast p^{\prime }=p$，但是因为后面接下来还有别的旋转，后面的旋转可以看成在本次旋转的结果的叠加，所以为了可持续发展，当然要使用在当前这个坐标系下 $p$ 点的坐标 $p^{\prime }$ 了
2）坐标系 $b{1}^{\prime }$ 再绕Y轴转 $\beta$ ，在新坐标系 $b_1^{\prime \prime }$ 下 $p$ 的坐标是：
$$p^{\prime \prime }=R_y^T\ast p^{\prime }$$

3）坐标系 $b_1^{\prime \prime }$ 再绕Z轴转 $\gamma$，在新坐标系 $b_1^{\prime \prime \prime }$ 下 $p$ 的坐标是：
$$p^{\prime \prime \prime }=R_z^T\ast p^{\prime \prime }$$

将上面的过程合并（组合）得，
$$p^{\prime \prime \prime }=R_z^T\ast R_y^T{p}^{\prime }=R_z^T\ast R_y^T\ast R_x^T\ast p$$

如果明白了上面的推导过程，以后所有的旋转都能这么推导，因为旋转是叠加的（在上次的结果上继续左乘）。假设存在这样的旋转，先绕X轴转 $\alpha$ 角度，再还是绕X轴转 ${\alpha }^{\prime }$，再绕Y轴转$\beta$，然后还是绕X轴转 ${\alpha }^{\prime \prime }$，整个旋转下来组合的旋转矩阵就是：
$$R=R({\alpha }^{\prime \prime }{)}^T\ast R(\beta {)}^T\ast R({\alpha }^{\prime }{)}^T\ast R(\alpha {)}^T=R({\alpha }^{\prime \prime }{)}^T\ast R(\beta {)}^T\ast R({\alpha }^{\prime }+\alpha {)}^T$$

之所以 ${\alpha }^{\prime }$ 和 $\alpha$ 能合并是因为他俩的旋转轴相同！

4、内部旋转（Z-Y-X）对应的旋转矩阵

上面按照XYZ的旋转顺序，推导得到结果是：
$$R_{b_2{b}_1}=R_z^T\ast R_y^T\ast R_x^T$$

如果将 $b_2$ 换成 $b$，$b_1$ 换成 $w$，即得，$R_{bw}=R_z^T\ast R_y^T\ast R_x^T$，再次转置得到，
$$R_{wb}=R_x\ast R_y\ast R_z$$

如果我们换成ZYX顺序旋转（从w系到b系），就自然可以得到：
$$R_{wb}=R_z\ast R_y\ast 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^{\prime }$，$R_{b_1^{\prime }w}=R_{w^{\prime }w}=R_x^T$，这就是此时的外参

如果接着再对 $w$ 做一次绕Y轴的旋转，得到 $b_1^{\prime \prime }$，显然 $R_{w^{\prime }w}=R_y^T$，使用外参进行转换，
$$R_{b_1^{\prime \prime }w}=(R_{b_1^{\prime }w}\ast R_{w^{\prime }w}\ast R_{b_1^{\prime }w}^T)\ast R_{b_1^{\prime }w}=R_x^T\ast R_y^T\ast R_x\ast R_x^T=R_x^T\ast R_y^T$$

如果绕 $w$ 做一次绕Z轴的旋转，得到 $b_1^{\prime \prime \prime }$，即，
$$R_{b_1^{\prime \prime \prime }w}=R_x^T\ast R_y^T\ast R_z^T，R_{w{b}_1^{\prime \prime \prime }}=R_z\ast R_y\ast 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. -1.38778e-17 -0.5 0. 0.25 0. 0. 0 -0.5 

<完>

@leatherwang