序：本篇系列文章参照高翔老师《视觉SLAM十四讲从理论到实践》，讲解三维空间刚体运动，为读者打下坚实的数学基础。博文将原第三讲分为五部分来讲解，其中四元数部分较多较复杂，又分为四部分。如果读者急于实践，可直接阅读第五部分的机器人运动轨迹，此部分详细讲解了安装准备工作。此系列总体目录如下：

1. 旋转矩阵和变换矩阵；
2. 旋转向量表示旋转；
3. 欧拉角表示旋转；
4. 四元数包括以下部分：
   4-1. 四元数表示变换；
   4-2. 四元数线性插值方法：LinEuler/LinMat/Lerp/Nlerp/Slerp；
   4-3. 四元数多点插值方法：Squad；
   4-4. 四元数多点连续解析解插值方法：Spicv；
   4-5. 四元数多点离散数值解插值方法：Sping。
5. 实践：SLAM中显示机器人运动轨迹及相机位姿。

1. 欧拉角

1.1 定义

无论是旋转矩阵还是旋转向量，它们虽然能描述旋转，但对人类来说非常不直观，而欧拉角则提供了一种非常直观的方式来描述旋转----它使用了3个分离的转角，把一个旋转分解成3次绕不同轴的旋转。

欧拉角：欧拉角是在空间中，描述从一个用于表示某个固定的参考系的、已知的方向，经过一系列基本旋转得到新的、代表另一个参考系的方向的方式。因为只有旋转，所以原点位置并没有发生变化。

在讨论欧拉角的具体形式之前，需要明确几个概念：

1. 左手坐标系与右手坐标系：既然是在坐标系中进行变换，首先就要了解坐标系的类别。同样的数据在左手坐标系和右手坐标系会有不同的呈现结果。本文在右手坐标系中讨论。
2. 旋转的具体形式：物体旋转有多种，比如绕某个定点进行旋转，绕坐标轴进行旋转，绕任意轴进行旋转。不同的旋转需要用不同的数学方式来表达。注意，本文讨论的是三维中绕坐标轴的旋转。
3. 欧拉角顺规：欧拉角定义了一组物体的旋转次序，称为顺规，可以理解为欧拉角旋转顺序的规定。$(\alpha ,\beta ,\gamma )$在不同的旋转顺序下会有不同的结果，先绕X轴旋转$\alpha$，还是先绕y轴旋转$\beta$，最后的结果是不一样的。本文根据不同旋转方式会采用不同的顺规。
4. 静态动态欧拉角：从参考坐标系上区分，将欧拉角分为静态和动态，其中静态欧拉角以绝对坐标系为参考，不随转动改变，一般用小写的x-y-z来表示静态坐标系；动态欧拉角以刚体自身的物体坐标系为参考，一般用大写的X-Y-Z表示。

1.2 RPY角与Z-Y-X欧拉角

描述坐标系$B$相对于参考坐标系$A$的姿态有两种方式：RPY角与Z-Y-X欧拉角。
RPY角：RPY角是绕固定（参考）坐标系$A$旋转：假设开始两个坐标系重合，先将$B$绕$A$的X轴旋转$\alpha$，然后绕$A$的Y轴旋转$\beta$，最后绕$A$的Z轴旋转$\gamma$，就能旋转到当前姿态。可以称其为X-Y-Z fixed angles或RPY角(Roll, Pitch, Yaw)。由于是绕固定坐标系旋转，则旋转矩阵为：$$R_{XYZ}(\alpha ,\beta ,\gamma )=R_Z(\gamma )R_Y(\beta )R_X(\alpha ).$$其乘法顺序为：从右到左，即XYZ。展开式为式（3.1）。
Z-Y-X欧拉角：另一种姿态描述方式是绕自身坐标轴$B$旋转：假设开始两个坐标系重合，先将$B$绕自身的Z轴旋转$\gamma$，然后绕Y轴旋转$\beta$，最后绕X轴旋转$\alpha$，就能旋转到当前姿态。称其为Z-Y-X欧拉角，由于是绕自身坐标轴进行旋转，则旋转矩阵为：$$R_{X^{{}^{\prime }}{Y}^{{}^{\prime }}{Z}^{{}^{\prime }}}(\alpha ,\beta ,\gamma )=R_{Z^{{}^{\prime }}}(\gamma )R_{Y^{{}^{\prime }}}(\beta )R_{X^{{}^{\prime }}}(\alpha )$$其乘法顺序为：从左到右，即$Z^{{}^{\prime }}{Y}^{{}^{\prime }}{X}^{{}^{\prime }}$。其展开式为(3.1)。
由于展开式相等，所以绕定轴X-Y-Z旋转（RPY角） 等价于 绕动轴Z-Y-X旋转（Euler角）。
此处高翔博士对RPY角和欧拉角的解释与其它版本不同，经反复确认后，决定采用熊有伦等编著的《机器人学》，也是网上流传最多的版本，相信，读者明白这一个知识点后，后边关于欧拉角及万向锁理解的误区会迎刃而解。
此处为方便理解，根据航空中的使用的RPY角定义再对此进行阐释。欧拉角定义方式上的不确定性带来了很多实际当中的困难，所幸在特定领域内，欧拉角通常有统一的定义方式。当中常用的一种是航空领域的，用“偏航-仰俯-滚转”（yaw-pitch-row）3个角度来描述的旋转，即RPY角，它是绕定轴x-y-z轴（从右至左）的旋转。RPY角相当于Z-Y-X欧拉角（从左至右）。那么，RPY角相当于把任意旋转分解成以下3个轴的转角，如图：
图1.1 Oxyz坐标轴旋转

1. 绕物体的Z轴旋转$\gamma$，得到偏航角yaw。
2. 绕旋转之后的Y轴旋转$\beta$，得到仰俯角pitch。
3. 绕旋转之后的X轴旋转$\alpha$，得到滚转角roll。

此时，可以使用$[r,p,y{]}^T$(即欧拉角)这样一个三维的向量描述任意旋转。rpy角的旋转顺序（从右到左）是XYZ。此外，为规避万向锁现象，常用的顺序是ZXY，但是无法完全消除万向锁，下面会解释原因。下面讲解欧拉角与旋转矩阵转换的推导，然后引出万向锁问题。

2. 欧拉角到旋转矩阵

一个旋转矩阵具有三个自由度，可以与欧拉角进行转换。下边推导欧拉角到旋转矩阵的转换。欧拉角旋转每次只绕一个坐标轴旋转，因此推导可分解为三次二维情况下的坐标变换，再延伸到三维。二维下称之为基本旋转矩阵，由基本旋转组成三维下的组合旋转矩阵。先讨论二维坐标下的情景。如下图所示：
图2.1 二维坐标旋转

点$P(x,y)$在原坐标系中与X轴夹角为$\alpha$，绕Z轴旋转角$\theta$后的坐标为$P^{\prime }(x^{\prime },y^{\prime })$，现在来推导$(x^{\prime },y^{\prime })$关于$(x,y)$的表示法。
推导方法有两种，第一种是利用三角恒等式的两角和差公式，第二种是利用三角形的性质推导。本文采用第一种，对第二种感兴趣的童鞋可祥阅《旋转矩阵（Rotation Matrix）的推导及其应用》。
由图可知，$OP=O{P}^{\prime }=\sqrt{x^2+y^2}=d$，由正余弦定理可得到两组方程：$$\{\begin{array}{c}\sin \alpha =\frac{y}{d}\\ \cos \alpha =\frac{x}{d}\end{array}\text{\hspace{1em}(2.1)}$$和$$\{\begin{array}{c}\sin (\alpha +\theta )=\frac{y^{\prime }}{d}\\ \cos (\alpha +\theta )=\frac{x^{\prime }}{d}\end{array}\text{\hspace{1em}(2.2)}$$
由两角和差公式可知：$$\{\begin{array}{c}\sin (\alpha +\theta )=\sin \alpha \cos \theta +\cos \alpha \sin \theta \\ \cos (\alpha +\theta )=\cos \alpha \cos \theta -\sin \alpha \sin \theta \end{array}\text{\hspace{1em}(2.3)}$$将式（2.1）和（2.3）代入（2.2），消掉$d$得到：$$\{\begin{array}{c}{x}^{\prime }=x\cos \theta -y\sin \theta \\ y^{\prime }=y\cos \theta +x\sin \theta \end{array}\text{\hspace{1em}(2.4)}$$因此有如下推导，设$P^{\prime }$的齐次坐标为$\widetilde{P^{\prime }}$：$$\widetilde{P^{\prime }}=[x^{\prime },y^{\prime },1{]}^T=\left[\begin{array}{ccc}cos\theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=R_Z(\theta )\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]\text{\hspace{1em}(2.5)}$$因此，绕Z轴的旋转矩阵为：$$R_Z(\theta )=\left[\begin{array}{ccc}cos\theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(2.6)}$$同理可推得绕Y轴的旋转矩阵为：$$R_Y(\theta )=\left[\begin{array}{ccc}cos\theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]\text{\hspace{1em}(2.7)}$$绕X轴的旋转矩阵为：$$R_X(\theta )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right]\text{\hspace{1em}(2.8)}$$组合旋转矩阵等于基本旋转矩阵的连乘，连乘顺序依基本旋转的先后次序由右向左排列，因此顺规为ZYX的组合旋转矩阵为：$$R_{ZYX}=R_X(\alpha )R_Y(\beta )R_Z(\gamma )\text{\hspace{1em}(2.9)}$$

3. 旋转矩阵到欧拉角

已知旋转矩阵，如何求欧拉角呢？只需要对应到旋转矩阵，求反弦值即可。将式（2.9）两侧展开得：$$\left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right]=\left[\begin{array}{ccc}\cos \beta \cos \gamma & -\cos \beta \sin \gamma & \sin \beta \\ \sin \alpha \sin \beta \cos \gamma +\cos \alpha \sin \gamma & -\sin \alpha \sin \beta \sin \gamma +\cos \alpha \cos \gamma & -\sin \alpha \cos \beta \\ -\cos \alpha \sin \beta \cos \gamma +\sin \alpha \sin \gamma & \cos \alpha \sin \beta \sin \gamma +\sin \alpha \cos \gamma & \cos \alpha \cos \beta \end{array}\right]\text{\hspace{1em}(3.1)}$$
因此推得：$$\{\begin{array}{c}\alpha =\arctan (-\frac{R_{23}}{R_{33}})\\ \beta =\arcsin R_{13}\\ \gamma =\arctan (-\frac{R_{12}}{R_{11}})\end{array}\text{\hspace{1em}(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 }$或两侧轴重合时，同样会产生万向锁现象。
这个变换用公式来理解的话，则是这样（你可以自己来代入验证一下），将$(\alpha ,\frac{\pi }{2},\gamma )$代入式（3.1）得：$$\begin{array}{rl}{R}_{ZYX}(\alpha ,\frac{\pi }{2},\gamma )& =R_X(\gamma )R_Y(\frac{\pi }{2})R_Z(\alpha )\\ & =\left[\begin{array}{ccc}0& 0& 1\\ \sin \alpha \cos \gamma +\cos \alpha \sin \gamma & -\sin \alpha \sin \gamma +\cos \alpha \cos \gamma & 0\\ -\cos \alpha \cos \gamma +\sin \alpha \sin \gamma & \cos \alpha \sin \gamma +\sin \alpha \cos \gamma & 0\end{array}\right]\\ & =\left[\begin{array}{ccc}0& 0& 1\\ \sin (\alpha +\gamma )& \cos (\alpha +\gamma )& 0\\ -\cos (\alpha +\gamma )& \sin (\alpha +\gamma )& 0\end{array}\right]\\ & =R_Y(\frac{\pi }{2})R_Z(\alpha +\gamma )\end{array}\text{\hspace{1em}(4.1)}$$
利用一些三角恒等式，将原本由三个旋转矩阵所组成的变换化简成了两个变换矩阵。即便我们分别对$Z-Y-X$三轴进行了旋转，实际上这个矩阵仅仅旋转了$Z-Y$两轴，它并没有对初始的X 轴进行变换。$R_Z(\gamma )$ 与$R_X(\alpha )$这两个变换被合并为单独的一个$R_Z(\gamma +\alpha )$变换（因为化简完之后变换顺序不一样了，严格来说并不是合并，只不过是能够使用一步来完成）。
注意，我们在这里化简的并不是单独的一个变换，而是一系列变换。因为它对任意 $\alpha$ 和$\gamma$角都是成立的。也就是说，一旦 轴上的变换角将这两个旋转轴对齐，我们就没有任何办法对最初的$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();
    //旋转向量使用AngleAxisd，它底层不直接是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

本文基于《视觉SLAM十四讲：从理论到实践》和《Quaternions, Interpolation and Animation》编写，但相对于原文会适当精简，同时为便于全面理解，会收集其他网络好文，根据作者理解，加入一些注解和扩展知识点，如果您觉得还不错，请一键四连（点赞关注收藏评论），让更多的人看到。

参考文献：

1. 《视觉SLAM十四讲：从理论到实践》，高翔、张涛等著，中国工信出版社
2. 四元数与欧拉角（RPY角）的相互转换
3. 图形变换之旋转变换公式推导
4. Bonus: Gimbal Lock By Krasjet