前言

最近在学惯性器件，想着先把理论知识脉络打通，于是便开始学习空间坐标系旋转和四元数，正好结合刚刚结课的课程《机器人控制技术》，记录一下学习心得。

旋转矩阵和齐次变换矩阵部分主要参考自教材 《机器人学导论》 中的第2章【有需要的可以去z-library上免费下载】

1 平面坐标系旋转

推导空间坐标系旋转之前，不妨先看一下平面坐标系的旋转。
如下图所示，存在一个向量 $v$ ，绕原点逆时针旋转（一般平面上的旋转都是往角度增大的方向，即逆时针方向旋转） $\theta$ 角度，得到向量 $v^{'}$ ，求向量 $v$ 到 $v^{'}$ 的旋转矩阵。【图片来源】
在这里插入图片描述

假定向量 $v$ 的模长为 $r$ ，初始角度为 $\phi$ ，则 $v$ 的坐标为 $(rcos\phi, rsin\phi)$ ，逆时针旋转 $\theta$ 角度后，得到 $v^{'}$ ，其坐标变为 $(rcos(\theta+\phi), rsin(\theta+\phi))$ ，展开得到：
$\left[ \begin{array}{c} x_{v’}\\ y_{v’}\\ \end{array} \right] =\left[ \begin{array}{c} r\cos \phi \cos \theta -r\sin \phi \sin \theta\\ r\cos \phi \sin \theta +r\sin \phi \cos \theta\\ \end{array} \right] =\left[ \begin{matrix} \cos \theta& -\sin \theta\\ \sin \theta& \cos \theta\\ \end{matrix} \right] \left[ \begin{array}{c} r\cos \phi\\ r\sin \phi\\ \end{array} \right] =\left[ \begin{matrix} \cos \theta& -\sin \theta\\ \sin \theta& \cos \theta\\ \end{matrix} \right] \left[ \begin{array}{c} x_v\\ y_v\\ \end{array} \right]$

故，矩阵 $\left[ \begin{matrix} \cos \theta& -\sin \theta\\ \sin \theta& \cos \theta\\ \end{matrix} \right]$ 即为旋转矩阵。

记住这个表达式，尤其是正负号，后面要考。

当然，上面讨论的是向量的旋转，如果把这个向量看作坐标系的一个轴，那么这个旋转矩阵也能应用到坐标系变换当中。比如已知一个点在A坐标系下的xy坐标，求该点在B坐标系下的xy坐标，就可以左乘一个旋转矩阵，从而得到新坐标系下的xy坐标。

2 空间坐标系旋转

了解了平面坐标系旋转，再来看看空间坐标系的旋转。

2.1 旋转矩阵的推导

参考上面的方法，即在一个固定坐标系中表示出发生旋转的坐标系前后的坐标，从而求出其旋转矩阵，我们在此基础上应用投影法来推导空间坐标系的旋转矩阵。
如下图所示，有两个坐标系{A}和{B}，其中{B}相对于{A}发生了旋转，现在在{B}中有一个矢量 $^BP$ （左上标代表“基于xx坐标系”），问该矢量在{A}中的坐标是多少？即求 $^AP$ 。
在这里插入图片描述
基于上面平面坐标系旋转的例子类推，我们不难猜出， $P$ 矢量在{A}和{B}下的坐标 $^AP, ^BP$ 之间存在一个旋转矩阵的关系：
$^AP=_B^AR\,\,^{\,\, B}P$

那怎么来求这个旋转矩阵 $_B^AR$ 呢？一般采用的是投影法。
$_{B}^{A}R=\left[ ^A\hat{X}_B\,\,^A\hat{Y}_B\,\,^A\hat{Z}_B \right] =\left[ \begin{matrix} \hat{X}_B\cdot \hat{X}_A& \hat{Y}_B\cdot \hat{X}_A& \hat{Z}_B\cdot \hat{X}_A\\ \hat{X}_B\cdot \hat{Y}_A& \hat{Y}_B\cdot \hat{Y}_A& \hat{Z}_B\cdot \hat{Y}_A\\ \hat{X}_B\cdot \hat{Z}_A& \hat{Y}_B\cdot \hat{Z}_A& \hat{Z}_B\cdot \hat{Z}_A\\ \end{matrix} \right]$

其中， $_{B}^{A}R$ 描述了{B}相对于{A}的旋转（左下标相对于左上标的旋转），即在{A}中描述{B}的姿态。

从这个角度来理解上面的等式 $^AP=_B^AR\,\,^{\,\, B}P$ ：等号两边是等价的，左边是矢量在{A}中的表示，等式右边由于 $^BP$ 是在{B}中的表示，所以要左乘以一个{B}相对于{A}的旋转矩阵，将这个旋转给“抵消”。

为方便记忆，可以把左上标视为分子，左下标视为分母，然后利用分数乘法中的相消原则，来书写表达式，这个方式在多个旋转矩阵相乘时更加有用。

此外， $\left[ ^A\hat{X}_B\,\,^A\hat{Y}_B\,\,^A\hat{Z}_B \right]$ 分别表示 {B}中三个主轴的单位矢量（含有“帽号”表示单位矢量）在{A}中三个主轴的投影，都是列向量。后面展开的矩阵，每一个元素都是两个单位矢量的点乘，其中每一列{B}中的向量保持不变，每一行{A}中的向量保持不变。

需要注意的是，向量乘积的前提是基于同一个坐标系，因此这里的 $\hat{X}_B$ 都是在坐标系A中的描述，如果在坐标系B中描述就直接是 $[1, 0, 0]$ 了

2.2 旋转矩阵的性质

旋转本身就是可逆的，既然有{B}相对于{A}的旋转矩阵 $_B^AR$ ，那必然也有{A}相对于{B}的旋转矩阵 $_A^BR$ ，即
$^BP=_A^BR\,\,^{\,\, A}P$
再根据上面的表达式：
$^AP=_B^AR\,\,^{\,\, B}P$
两边同时左乘 $_A^BR$ ，得到
$_{A}^{B}R\,\,^AP=_{A}^{B}R\,\,_{B}^{A}R^{\,\, B}P=^BP$
故 $_{A}^{B}R\,\,_{B}^{A}R=I$ ，即 $_{A}^{B}R=\,_{B}^{A}R^{-1}$ 。

此外，根据上面的 $_B^AR$ 的推导，同样可以得出{A}相对于{B}发生旋转时的旋转矩阵 $_A^BR$ ：
$_{A}^{B}R=\left[ ^B\hat{X}_A\,\,^B\hat{Y}_A\,\,^B\hat{Z}_A \right] =\left[ \begin{array}{c} {^A\hat{X}_B}^T\\ {^A\hat{Y}_B}^T\\ {^A\hat{Y}_B}^T\\ \end{array} \right] ={_{B}^{A}R}^T$

联立上面表达式，可以得出： $_{B}^{A}R^{-1} ={_{B}^{A}R}^T$ ，因此，可以得出，旋转矩阵为一个正交矩阵，同时还是一个标准正交矩阵，即其行列式恒为+1（非标准正交矩阵行列式为-1）.

3 齐次变化矩阵*

以上关于空间旋转的叙述，可以发现其实都含有一个前提：坐标原点不变，但在实际的机器人控制过程中，机构的运动并不是单独的旋转，还包含平移运动，所以一般描述位姿变换，常采用4*4的齐次变换矩阵。【如果只想看旋转部分，这节可以跳过】

3.1 坐标系平移

首先来看坐标系平移。和旋转要保持坐标原点不变的前提一样，讨论坐标系平移时，也要保证姿态一样，即不发生旋转。
坐标系{B}相对于坐标系{A}发生了平移，一般只需要描述{B}原点相对于{A}的位置即可：
$^AP_{Borg} =\left[ \begin{array}{c} ^Ap_x\\ ^Ap_y\\ ^Ap_z\\ \end{array} \right]$
这里，同样采用了左上标的符号来表示“相对”， $o r g$ 下标表示origin，即原点。

3.2 齐次变换矩阵

以上讨论的都是单独的平移和旋转，因此都要满足一定的前提条件，那能不能把这两者给结合起来呢？答案肯定是可以的。
$\left[ \begin{array}{c} ^AP\\ 1\\ \end{array} \right] =\left[ \begin{matrix} _{B}^{A}R& ^AP_{Borg}\\ \text{0 0 }0& 1\\ \end{matrix} \right] \left[ \begin{array}{c} ^BP\\ 1\\ \end{array} \right]$

通过增加0和1，巧妙地将旋转矩阵和平移向量结合到一个4*4的矩阵当中，这个矩阵称为齐次变换矩阵，用 $_B^AT$ 表示，即
$\left[ \begin{array}{c} ^AP\\ 1\\ \end{array} \right] =_{B}^{A}T\left[ \begin{array}{c} ^BP\\ 1\\ \end{array} \right]$

如果要计算多次变换，只需要把多个齐次变换矩阵依次相乘即可。

3.3 平移和旋转算子

所谓算子，即能够单独进行某一种特定运算的表达式，相当于是齐次变换矩阵的一种特殊情况。

平移算子
对于两个坐标系 $P_1$ 和 $P_2$ ，满足这样的平移关系 $^AP_2=^AP_1+^AQ$ ，将其用齐次变换矩阵对应的算子表示，即 $^AP_2=D_Q\left( q \right) ^AP_1$ ，则：
$D_Q\left( q \right) =\left[ \begin{matrix} 1& 0& 0& ^AQ_x\\ 0& 1& 0& ^AQ_y\\ 0& 0& 1& ^AQ_z\\ 0& 0& 0& 1\\ \end{matrix} \right]$
绕z轴旋转算子
对于两个坐标系 $P_1$ 和 $P_2$ ，满足这样的旋转关系 $^AP_2=R_z(\theta)\,\,^AP_1$ ，如下图所示。

那么其对应的齐次变换矩阵算子 $T_z(\theta)$ 为
$T_z\left( \theta \right) =\left[ \begin{matrix} \cos \theta& -\sin \theta& 0& 0\\ \sin \theta& \cos \theta& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\\ \end{matrix} \right]$

这里可以发现，其中使用到的表达式正好是前面二维平面旋转对应的表达式。

绕x轴旋转算子
对于两个坐标系 $P_1$ 和 $P_2$ ，满足这样的旋转关系 $^AP_2=R_x(\theta)\,\,^AP_1$ ，即绕x轴逆时针旋转 $\theta$ 角度。空间想象一下，和上面各个坐标系对应一下（X指向纸面外；Y代替X；Z代替Y），可以推出齐次变换算子 $T_x(\theta)$
$T_x\left( \theta \right) =\left[ \begin{matrix} 1& 0& 0& 0\\ 0& \cos \theta& -\sin \theta& 0\\ 0& \sin \theta& \cos \theta& 0\\ 0& 0& 0& 1\\ \end{matrix} \right]$
绕y轴旋转算子
对于两个坐标系 $P_1$ 和 $P_2$ ，满足这样的旋转关系 $^AP_2=R_y(\theta)\,\,^AP_1$ ，即绕y轴逆时针旋转 $\theta$ 角度。空间想象一下，绕z轴旋转的情况对应一下（Y指向纸面外；X代替Y；Z代替X），这里有点特殊，不能直接套用上面的样式，需要计算一下，可以得到齐次变换算子 $T_y(\theta)$
$T_y\left( \theta \right) =\left[ \begin{matrix} \cos \theta& 0& \sin \theta& 0\\ 0& 1& 0& 0\\ -\sin \theta& 0& \cos \theta& 0\\ 0& 0& 0& 1\\ \end{matrix} \right]$

4 固定角和欧拉角

旋转矩阵虽然直观易懂，且对于公式的推导具有很大的便利，但是它也存在一个很大的问题，那就是描述一个角度需要9个变量，这在进行大量旋转运算时是很耗费性能的。但是可以发现旋转矩阵的9个变量并不是独立，而是存在6个约束条件（比如秩为1等），因此实际的变量个数为3，那能不能找到3个参数来描述空间中的旋转呢？
最直观的想法就是将空间旋转转换到矢量绕坐标系三个轴的旋转，但是这里存在一个难点，那就是空间中的旋转是不可交换的，即先绕x轴旋转再绕y轴旋转并不等价于先绕y轴旋转再绕x轴旋转，其本质原因在于矩阵相乘并不满足交换律。因此，在寻找这三个参数时，也需要约定旋转的先后顺序。这里介绍两种比较常用的表达方式：X-Y-Z固定角和Z-Y-X欧拉角。

4.1 X-Y-Z固定角

首先将坐标系{B}和一个已知的参考坐标系{A}重合，先将{B}绕 $\hat{X}_A$ 旋转 $\gamma$ 角，再绕 $\hat{Y}_A$ 旋转 $\beta$ 角，最后绕 $\hat{Z}_A$ 旋转 $\alpha$ 角 。
以上每一次旋转都是绕固定的坐标系A的轴，即所谓“固定”。
根据前面总结的原则，每进行一次旋转，旋转后的坐标都要左乘一个旋转矩阵，才能得到旋转前坐标系中的坐标。因此可以推导出按照X-Y-Z旋转固定角的旋转矩阵：
在这里插入图片描述
其中 $c\alpha$ 是 $cos\alpha$ 的缩写， $s\alpha$ 是 $sin\alpha$ 的缩写，等等。将矩阵相乘，得到：

在这里插入图片描述
这相当于已知绕各个固定轴的旋转角，计算出其对应的旋转矩阵。那么它的逆问题就变成了已知这样一个旋转矩阵，怎么求出其对应的绕各个固定轴的旋转角，即 $\alpha, \beta, \gamma$ 。
令

故可以根据正余弦之间的关系求出三个旋转角：
在这里插入图片描述
其中 $A t a n 2$ 函数叫双反正切函数，需要传入两个参数：正弦值和余弦值，其表达式为 $Atan2(sin\alpha, cos\alpha)$ ，它会根据正余弦的正负来确定角度，因此它的函数值范围不再是-90° ~ 90°，而是-180° ~ 180°。

4.2 Z-Y-X欧拉角

首先将坐标系{B}和一个已知的参考坐标系{A}重合，先将{B}绕 $\hat{Z}_B$ 旋转 $\alpha$ 角，再绕 $\hat{Y}_B$ 旋转 $\beta$ 角，最后绕 $\hat{X}_B$ 旋转 $\gamma$ 角 。
以上每一次都是绕运动的坐标系{B}的轴旋转而不是绕固定的坐标系{A}的轴旋转，这样的三个一组的旋转称为 欧拉角。注意这里每一次旋转的轴取决于上一次的旋转。{B}初始与{A}重合，不妨假定{B}第一次旋转后得到{B’}，第二次旋转得到{B’'}，第三次旋转得到最终姿态{B}，故{B}相对于{A}的旋转矩阵为中间坐标系对应的旋转矩阵相乘：
$_{B}^{A}R=_{B'}^{A}R\,\,_{B''}^{B'}R\,\,_{B}^{B''}R$
代入旋转算子，可以得到：
在这里插入图片描述
把相乘计算出来，可以得到：

惊奇地发现，这个结果和上面固定角旋转得到的结果的一样的！因此可以得到一个结论：三次绕固定轴旋转的最终姿态和以相反顺序绕运动坐标系的轴转动的姿态相同。因此，如果要求出其对应的欧拉角，也可以套用上面总结的公式。

4.3 欧拉角到底指什么？

在很多不太专业的博客中，关于欧拉角上来就是Yaw，Pitch，Roll，都没有搞清楚欧拉角是怎么定义的，是绕哪个轴在转动。
通过对比不同领域的文献，我发现关于欧拉角的定义似乎有所差别，比如在一些文献当中，是不区分固定角和欧拉角的，绕固定轴和绕运动轴的区别称为外旋和内旋。因此，为了和上面保持一致，这里就以Z-Y-X绕运动轴旋转的角度称为欧拉角，一般用Yaw，Pitch，Roll表示。
在讨论旋转时，首先明确一下坐标系，一般来说，会有一个世界的参考坐标系，常用右手系东北天坐标系，即“东-X，北-Y，天-Z”，如下图所示。
在这里插入图片描述
而在讨论物体姿态时，初始位置一般和参考坐标系对齐，即“前面为Y，右边为X，上面为Z”（想象一架飞机平放在你的眼前）这样直观的认识。而在讨论欧拉角转动方向时，按照“右手定则”： 坐标轴正方向为拇指所指方向，四指所指方向为转动正方向。

偏航角Yaw：绕Z轴转动，从X到Y为正方向（机头从右往左水平转）
俯仰角Pitch：绕X轴转动，从Y到Z为正方向（飞机从下往上抬头）
横滚角Roll：绕Y轴转动，从Z到X为正方向（飞机从左往右翻滚）

以上图片来自参考链接

4.4 用欧拉角或固定角的弊端

再仔细看看上面那个求固定角的表达式。
在这里插入图片描述
会发现一个问题，那就是当 $\beta=90\degree$ 时， $\alpha, \gamma$ 是解不出来的，也可以将其代入到展开的矩阵当中：
$_{B}^{A}R_{XYZ}\left( \gamma ,\beta ,\alpha \right) =\left[ \begin{matrix} 0& c\alpha s\gamma -s\alpha c\gamma& c\alpha c\gamma +s\alpha s\gamma\\ 0& s\alpha s\gamma +c\alpha c\gamma& s\alpha c\gamma -c\alpha s\gamma\\ -1& 0& 0\\ \end{matrix} \right]$
这只能求出 $\alpha+\gamma$ ，在一些特定场合，可以指定某个角度为多少，然后求出另一个角度，如指定 $\alpha=0$ 。

这个问题一般称为万向节死锁（Gimbal Lock），这也是用欧拉角（固定角）表示旋转最大的问题，由此也引出了下文的四元数。

5 四元数

对于如何理解四元数，有一种公认的比较好的方式，那就是从复数角度去理解。这里提供一个学习链接，是中文翻译版的，文中也附了英文原版链接。个人建议先详细阅读一遍。

上面的教程关于四元数旋转部分讲得不够明确，这里简单总结一下。
用四元数来描述旋转，一般是 “ $p_1$ 矢量绕 $p_2$ 矢量转动 $\theta$ 角度” 这样的形式，同时，由于 $p_1$ 和 $p_2$ 都是实际三维空间中的矢量，所以都是纯四元数，其对应的四元数分别是 $q_1=\left( \text{0, }\vec{v}_1 \right)$ ， $q_2=\left( \text{0, }\vec{v}_2 \right)$ ，
令
$q=\left( \cos \frac{\theta}{2}, \sin \frac{\theta}{2}\vec{v}_2 \right)$

注意，这里的角度是需要旋转角度的一半，同样，其旋转的正方向也要符合右手定则。

则 $p_1$ 旋转后的矢量 $p_{1}^{'}$ 等于
$p_{1}^{'}=qp_1q^*$
其中， $q^*$ 为 $q$ 的共轭四元数，其表达式如下所示。
$q^*=\left( \cos \frac{\theta}{2}, -\sin \frac{\theta}{2}\vec{v}_2 \right)$