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

旋转矩阵和变换矩阵；

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

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

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

描述坐标系相对于参考坐标系的姿态有两种方式：RPY角与Z-Y-X欧拉角。

RPY角：RPY角是绕固定（参考）坐标系旋转：假设开始两个坐标系重合，先将绕的X轴旋转，然后绕的Y轴旋转，最后绕的Z轴旋转，就能旋转到当前姿态。可以称其为X-Y-Z fixed angles或RPY角(Roll, Pitch, Yaw)。由于是绕固定坐标系旋转，则旋转矩阵为：其乘法顺序为：从右到左，即XYZ。展开式为式（3.1）。

Z-Y-X欧拉角：另一种姿态描述方式是绕自身坐标轴旋转：假设开始两个坐标系重合，先将绕自身的Z轴旋转，然后绕Y轴旋转，最后绕X轴旋转，就能旋转到当前姿态。称其为Z-Y-X欧拉角，由于是绕自身坐标轴进行旋转，则旋转矩阵为：其乘法顺序为：从左到右，即。其展开式为(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坐标轴旋转

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

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

图2.1 二维坐标旋转

点在原坐标系中与X轴夹角为，绕Z轴旋转角后的坐标为，现在来推导关于的表示法。

推导方法有两种，第一种是利用三角恒等式的两角和差公式，第二种是利用三角形的性质推导。本文采用第一种，对第二种感兴趣的童鞋可祥阅《旋转矩阵（Rotation Matrix）的推导及其应用》。

由图可知，，由正余弦定理可得到两组方程：和

由两角和差公式可知：将式（2.1）和（2.3）代入（2.2），消掉得到：因此有如下推导，设的齐次坐标为：因此，绕Z轴的旋转矩阵为：同理可推得绕Y轴的旋转矩阵为：绕X轴的旋转矩阵为：组合旋转矩阵等于基本旋转矩阵的连乘，连乘顺序依基本旋转的先后次序由右向左排列，因此顺规为ZYX的组合旋转矩阵为：

已知旋转矩阵，如何求欧拉角呢？只需要对应到旋转矩阵，求反弦值即可。将式（2.9）两侧展开得： 因此推得：由上面推导，引出万向锁问题。

万向锁问题：欧拉角的一个重大缺点就是会碰到著名的万向锁问题（Gimbal Lock）,比如对于rpy，当仰俯角为时，第一次旋转与第三次旋转使用同一个轴，使得系统丢失了一个自由度（由3次旋转变成了两次旋转），这被称为奇异性问题，或万向锁，其它顺规形式的欧拉角也同样存在奇异性问题。

理论上可以证明，只要想用3个实数来表达三维旋转，都不可避免的碰到奇异性问题。由于这种原理，欧拉角不适用于插值和迭代，往往只用于人机交互中，因此我们很少在SLAM程序、滤波以及优化中使用欧拉角表达旋转。

万向锁理论性描述讲完了，相信大部分人都没理解，网文解释的也比较模糊。以外国小哥的视频：欧拉旋转，配合截图，试讲解如下：

以欧拉角的顺规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轴旋转角度为时，X轴和Z轴重合共面，丢失了一个自由度。三次旋转变换仅仅覆盖了两个外部轴的旋转，一个自由度就这样丢失了，这也就导致了 Gimbal Lock 的现象。Gimbal Lock 问题的核心还是在于我们采用了固定的旋转顺序。对于其它5种组合的顺规，当中间的轴正好旋转或两侧轴重合时，同样会产生万向锁现象。

这个变换用公式来理解的话，则是这样（你可以自己来代入验证一下），将代入式（3.1）得：

利用一些三角恒等式，将原本由三个旋转矩阵所组成的变换化简成了两个变换矩阵。即便我们分别对三轴进行了旋转，实际上这个矩阵仅仅旋转了两轴，它并没有对初始的X 轴进行变换。 与这两个变换被合并为单独的一个变换（因为化简完之后变换顺序不一样了，严格来说并不是合并，只不过是能够使用一步来完成）。

注意，我们在这里化简的并不是单独的一个变换，而是一系列变换。因为它对任意 和角都是成立的。也就是说，一旦 𝑦 轴上的变换角将这两个旋转轴对齐，我们就没有任何办法对最初的轴进行旋转了。无论轴与轴的旋转角是多少，变换都会丧失一个自由度。

笔者收集到两种解决办法，第一种会尽大可能规避，但无法避免；第二种能彻底解决，但会耗费一定内存。如有其它方法，请留言告知。

至此，推导部分结束，让我们练练手吧。

现在，我们实际演练前面讲到的各种旋转表达方式。将在Eigen中使用旋转向量、欧拉角和旋转矩阵，演示它们之间的变换方式。 代码可在博客资源中下载，截取部分如下（已加注释）：