彻底搞懂“旋转矩阵/欧拉角/四元数”，让你体会三维旋转之美
目录
旋转矩阵
坐标变换的作用
实现坐标变换所需的数据
位姿变换
坐标变换中旋转的实质
坐标变换中平移的实质
如何计算坐标系B各坐标轴在坐标系A上的投影？（多坐标变换）
如何实现坐标变换？
欧拉角
欧拉角的作用
欧拉角与旋转矩阵
欧拉角的弊端
四元数
三维旋转
三维复数
四元数的定义
四元数的性质
四元数乘法
纯四元数
四元数的共轭
四元数与三维旋转
向量转四元数
三维旋转转四元数
旋转矩阵与四元数
四元数与欧拉角的转化
向量的旋转一共有三种表示方法：旋转矩阵、欧拉角和四元数，接下来我们介绍一下每种旋转方法的原理以及相互转换方式。
旋转矩阵
坐标变换的作用
在一个机器人系统中，每个测量元件测量同一物体得出的信息是不一样的，原因就在于“每个测量元件所测量的数据是基于不同坐标系所测量的”，例如：
在这辆车中有激光雷达M和激光雷达W，这两个雷达测量的数据截然不同，但是这辆汽车相对于测量物体的位置是唯一的，这就说明“由不同位置雷达测量的数据代表的物理含义（即都表示汽车与被测物体的相对位置）是相同的”。那既然被测物体在不同坐标系中的坐标不同但物理含义相同，这就涉及到不同坐标系中坐标的相互转化。下面这个视频将使你对坐标变换有一个初步的认识：
无所不能的矩阵 - 三维图形变换
实现坐标变换所需的数据
我们常用出发与坐标系原点终止于坐标系中坐标点的向量来表示坐标系中坐标点相对于坐标原点的位置（距离+方位）。坐标系的相互转化必须以地球坐标系为媒介才可以实现，即坐标系的相互转化必须已知“任意坐标系中各个坐标轴在world坐标系中的坐标”：
我们会疑惑：world坐标系是什么？
在空间中会有n+1个坐标系，其中只有一个坐标系起到标定作用，也就是说“其他n个坐标系全都是基于该坐标系找到自己在空间中的位置的”。只有大家都知道了自己在空间中的具体位置，坐标转换才可以顺利进行下去。
位姿变换
基于O1的世界坐标系与基于O2的坐标系如下所示：
在描述机器人运动时，我们常常提及“位姿”，其实位姿是一个合成词，我们可以将其拆解为“位置+姿态”。位置就是指“机器人某个运动关节/测量传感器在世界坐标系中的具体位置，姿态就是”基于该点的坐标系相较于世界坐标系所进行的旋转“，如下所示：
坐标变换中旋转的实质
坐标变换的实质就是“投影”。首先，我们解读一下向量是如何转化为坐标的：
理解向量坐标的由来对于理解坐标变换的实质至关重要！接下来我们考虑一下单位向量在坐标系中的投影：
单位向量在坐标系中的投影正好为向量P与各个坐标轴夹角的余弦值。
我们将坐标系A作为参考坐标系（world坐标系），基于坐标系A表示坐标系B的各个坐标轴并且将各个向量单位化，由此我们得到一个旋转矩阵，旋转矩阵各个元素的含义如下：
我们先前提到过，向量坐标的计算无非就是投影，那么向量坐标从坐标系B转换至坐标系A无非就是两次投影而已：
坐标转换的实际意义无非就是将向量P在坐标系A中各个轴的投影分别累加起来形成一个新的坐标。那问题来了，累加如何操作呢？这就涉及旋转矩阵以及矩阵乘法运算了。
其实，这个矩阵的乘法与卷积有着异曲同工之妙。旋转矩阵的性质：
坐标变换中平移的实质
向量可以在坐标系中任意移动，只要不改变向量的方向和大小，向量的属性不会发生变化。但是我们研究的是坐标系B中一个坐标点在坐标系A中的映射，因此需要考虑坐标系B的原点O2相较于坐标系A原点O1平移的距离。
每个基于坐标系B的向量先进性旋转变换，再与向量O1O2求和即可得到向量P再坐标系A中的实际映射。
如何计算坐标系B各坐标轴在坐标系A上的投影？（多坐标变换）
首先，我们要知道世界坐标系下坐标系A/坐标系B的各个坐标轴在世界坐标系（参考坐标系）的坐标：
我们使用旋转矩阵的性质可以得到坐标系B变换至坐标系A的旋转矩阵：
坐标变换流程如下：
如何实现坐标变换？
我们将P点在坐标系B中的坐标转化为P点在坐标系A中对应的坐标：
坐标变换=旋转+平移，因此坐标变换表达式如下所示：
这样不利于矩阵运算，我们可以改写为如下形式：
其中O1O2是从O1指向O2的向量。
欧拉角
欧拉角的作用
欧拉角遵循的是右手系规则，即大拇指指向坐标轴正方向，四指旋转的方向即为转动的正方向，欧拉角包含三个自由量：yaw(偏航角)、pitch(俯仰角)、roll(翻滚角)。
我们将三次旋转分开讨论，我们以绕Z轴旋转为例来进行说明：
绕Z轴旋转的三维立体图如上所示，为了方便，我们查看一下二维旋转图：
欧拉角与旋转矩阵
欧拉角可以转化为旋转矩阵，并且利用旋转矩阵的知识进行旋转操作。向量在二维平面上旋转可以让我们联想到向量在复平面中的旋转：
设向量P的坐标为(x,y)，则XOY平面变换公式如下所示：
由于“绕谁谁不变”的原则，因此Z轴坐标不会发生改变，最终Z轴旋转变换公式如下所示：
同理，我们可以按照同样的方式得出到X/Y轴旋转的公式：
旋转矩阵的旋转顺序分为外旋（x->y->z）和内旋（z->y->x），我们一般使用外旋的顺序：
欧拉角的弊端
欧拉角有三个分别为yaw(偏航角)、pitch(俯仰角)、roll(翻滚角)，代表着绕着Z/Y/X轴旋转的角度，相当于有三个独立变量（自由度）控制一架飞机进行如下旋转操作：
但是当任何一个坐标轴旋转角度为90度时，就会有两个轴的旋转动作起到对总体旋转结果相同的效果，这就被称为“死锁“，动态图如下所示：
上面的动画展示了绕X轴旋转90度之后，Y轴与Z轴正方向同向，这就导致绕Y轴旋转θ度与绕Z轴的正方向旋转θ度有同样的效果，即丢失了一个自由度。“丢失了一个自由度“也就意味着真正起到旋转作用的只有Y轴和X轴（或者Z轴和X轴），即飞机无法绕着原Z轴正方向进行旋转操作。
”死锁“现象在二维平面图中如下所示：
无论绕X轴正方向/反方向旋转90度，都会导致Y/Z轴正方向罗在一条直线上！为了解决“死锁“的问题，我们要使用四个自由量，这就引出了”四元数“的概念。
四元数
数学的美妙不在于形象的表达变换的逻辑，而在于抽象简单的给出表达式。四元数就是如此，四元数的三维表达晦涩难懂，但是四元数的表达式可以优雅的表达三维中的旋转操作，不但避免了欧拉角的死锁问题而且也避免了旋转矩阵的复杂计算。
四元数的可视化
上述视频链接如下所示：四元数的可视化_哔哩哔哩_bilibili
https://www.bilibili.com/video/BV1SW411y7W1?from=search&seid=2286694305504614618&spm_id_from=333.337.0.0上述四元数的物理解释较难理解，而我喜欢把它理解为一种旋转算法。数学之美就在于其揭示了许多新的计算方法，幸运的是，四元数就是一种用于物体旋转的计算方法。
三维旋转
该部分为后续探究三维旋转与四元数的关系打下基础。
向量V围绕着向量U进行三维空间中的旋转操作得到向量V‘，其中向量U为单位向量。现在，我们为了简化向量的旋转操作，分解向量V：
向量V在向量U平行方向上的分量不参与旋转，参与旋转的是向量V垂直于向量U的分量：
同样，旋转后的向量V’也由垂直于向量U和平行于向量U两部分分量矢量叠加而成：
最难求出的是向量V‘垂直于向量U的部分，我们以二维平面图为例进行求解：
找出向量Vw（与原垂直分量垂直）就可以对向量进行分解，要求Vw需了解三维图中Vw：
我们可以清楚的观察到，Vw与单位向量U以及向量V原垂直分量垂直，故我们可以得出：
我们结合向量投影的知识可以得到旋转后向量V的表达式：
三维复数
四元数包含了四个实参数以及三个虚部（一个实部三个虚部），从四维角度来看如下图所示：
Re实部与i/j/k三个虚部都垂直，并且在坐标系U内部三个虚部两两垂直形成了一个三维坐标系，并且三维复数的运算规则如下所示（aij=ai*aj）：
| 1 |
i |
j |
k |
|
| 1 |
1 |
i |
j |
k |
| i |
i |
-1 |
k |
-j |
| j |
j |
-k |
-1 |
i |
| k |
k |
j |
-i |
-1 |
上面这个看似简单的表格就决定了四元数的一切性质，你或许看起来很困惑：这个运算法则如何得到，又有何物理意义？回答是：这个乘法是由数学家规定的，其物理意义代表了“按照特定的方向旋转90度的操作”，如果你问我为何这个乘法被定义为三维空间中坐标系旋转90度的操作，那如下YouTube上的评论会一定程度上取消你的疑虑：
答案就是：我们可以借助这种乘法法则找到三维旋转中的某种规律性的东西。
四元数的定义
与复数类似，因为四元数其实就是对于基 {1, , , } 的线性组合，四元数也可以写成向量的形式：
除此之外，我们经常将四元数的实部与虚部分开，并用一个三维的向量来表示虚部，将它表示为标量和向量的有序对形式：
我们这里可能还感觉到迷茫，没关系，我们一步一步来！
四元数的性质
研究四元数的性质是为了纯四元数和三维空间中的旋转操作做铺垫。
四元数乘法
其实，我们也可以将其写成矩阵形式，为后面四元数转旋转矩阵做铺垫：
注意：左乘与右乘不同，这一点与矩阵的性质类似，但是四元数乘法与矩阵乘法完全是两个概念，我们以魔方旋转为例：
因为物体的6个侧面一般并不相同（相当于有6个独立不相同的面），按照"左转->下转"和"下转->左转"的顺序得到的Y轴正方向的平面并不一样，所以三维旋转不可逆。
现在我们从物理含义的角度理解一下左乘和右乘的具体含义：
1. 左乘旋转四元数（左为操作动作，右为被操作对象）
2. 右乘旋转四元数（右为操作动作，左为被操作对象）
3. 四元数旋转公式的含义
这里，我们结合左乘与右乘的相关知识就可以清楚的了解乘法法则中的变换关系：
| 乘法法则 |
1 |
i |
j |
k |
| 1 |
1 |
i |
j |
k |
| i |
i |
-1 |
k |
-j |
| j |
j |
-k |
-1 |
i |
| k |
k |
j |
-i |
-1 |
以j*i=-k，i*i=-1，k*i=j来进行说明（右乘i）：
纯四元数
在我们正式进入四元数的讨论之前，我们还需要更多关于四元数的定义。如果一个四元数能写成这样的形式：
那我们则称为一个纯四元数，即仅有虚部的四元数。因为纯四元数仅由虚部的3D向量决定，我们可以将任意的3D向量转换为纯四元数。纯四元数有一个很重要的特性：如果有两个纯四元数 = [0, v], = [0, u]，那么由四元数乘法可以得到：
四元数的共轭
我们发现四元数的性质与矩阵相似。当四元数模值为1，则该四元数被称为单位四元数。
四元数与三维旋转
向量转四元数
三维空间中向量的旋转全部可以写成纯四元数的形式：
一定要对这些符号留有印象，对后面的四元数转化为旋转矩阵的理解很有帮助。
三维旋转转四元数
向量V绕向量U进行三维空间中的旋转最重要的是向量V沿向量U垂直方向上的分量的旋转：
注意：
那么接下来我们可以将三维旋转与纯四元数进行转化：
其中，四元数q有以下性质：
其中，最容易误解的还是复数虚部的平方，其与向量U 的模值无关：
由此可得，该四元数为单位四元数，但是不是纯四元数。这里还需要两个定理：
① 定理一：
我们从物理意义出发也可以理解：向量V平行于向量U的分量绕向量U旋转相当于没旋转。
② 定理二：
证明如下所示：
现在最精彩的来了，我们看上面的式子复杂且不美观，那现在我们使用一些技巧来旋转式子变得美观些：
我们最终得到四元数与三维旋转的对应关系：
旋转矩阵与四元数
我们前面介绍过左乘和右乘在矩阵转化时的区别，因此我们可以按照乘法法则将四元数转化为如下所示的三维旋转矩阵：
上述结论可以结合前面提到过的“左乘/右乘”的知识对上述公式进行巧记：
四元数与欧拉角的转化
一个展示欧拉角与四元数动态变换关系的网站：
Quaternions - Visualisationhttps://quaternions.online/ 其中，重点注意atan2(…)与atan(…)函数的区别：
atan()函数的值域为[-PI/2,PI/2]，但是atan2()函数的值域为[-PI,PI]。atan2()函数弥补了atan()函数识别不出第一象限/第三象限、第四象限/第二象限的缺陷。atan2()函数是求解方位角的最佳选择。
#include
#include
#define PI (std::acos(-1))
int main(int argy, char* argv[])
{
std::cout << "---------第一象限---------" << std::endl;
double y1 = 1, x1 = 1;
std::cout << "std::atan(y1/x1)=" << std::atan(y1 / x1) / PI * 180 << std::endl;
std::cout << "std::atan2(y1,x1)=" << std::atan2(y1, x1) / PI * 180 << std::endl;
std::cout << "---------第二象限---------" << std::endl;
double y4 = 1, x4 = -1;
std::cout << "std::atan(y4/x4)=" << std::atan(y4 / x4) / PI * 180 << std::endl;
std::cout << "std::atan2(y4,x4)=" << std::atan2(y4, x4) / PI * 180 << std::endl;
std::cout << "---------第三象限---------" << std::endl;
double y3 = -1, x3 = -1;
std::cout << "std::atan(y3/x3)=" << std::atan(y3 / x3) / PI * 180 << std::endl;
std::cout << "std::atan2(y3,x3)=" << std::atan2(y3, x3) / PI * 180 << std::endl;
std::cout << "---------第四象限---------" << std::endl;
double y2 = -1, x2 = 1;
std::cout << "std::atan(y2/x2)=" << std::atan(y2 / x2) / PI * 180 << std::endl;
std::cout << "std::atan2(y2,x2)=" << std::atan2(y2, x2) / PI * 180 << std::endl;
return 0;
} 运行结果如下所示：
