旋转矩阵与四元数、欧拉角的关系，以及如何通过四元数解算欧拉角，一篇文章讲清楚

旋转矩阵

所谓旋转，是指一个坐标系到另一个坐标系的变换。一个坐标系，对应着一种描述世界的“尺度”和“角度”，这里我们描述的对象为向量（vector）。

假定存在目标向量$\alpha$，$\alpha$客观存在，客观静止。此时我想要去描述这个向量，那么我可以选定一个参考系（坐标系），以此为基准去描述该向量。假设此时有两个参考系W（大地坐标系）和B（机体坐标系），在W下$\alpha$为$k_w=(x0,y0,z0)$，而在B下$\alpha$为$k_b=(x1,y1,z1)$，那么这两个向量其实描述的是一个东西，只是参考系不同。

由于W和B之间存在一个相对位置关系，因此不难理解kw和kb之间也存在着一个“必然”的联系，而这个“联系”就是所谓的“旋转矩阵”。存在这样一个关系：
$$k_b=C_w^b\ast k_w$$
$C_w^b$便是“W坐标系到B坐标系”的旋转变化矩阵，$C_w^b$只跟W和B的相对位置关系有关，反映了W到B的一个“变化”，可以把它理解为一个“动作”。

那么具体拆解，$C_w^b$是一个3*3的矩阵$\left|\begin{array}{ccc}a& b& c\\ d& e& f\\ g& h& i\end{array}\right|$ ，如果更具象一点理解它，该矩阵的每一列分别代表了W坐标系xyz三轴在B系的投影，比如说，原先$X_w$在W系下描述为(1,0,0)，而$X_w$（仍如刚才所说，向量客观静止不变）在B系下的描述就为(a,d,g)。

事实上，正如刚才所说，$C_w^b$只跟W和B的相对位置关系有关，因此，对于任何一个向量，关于其在W和B下的描述，都存在上述的变换关系。

而想要将向量从B系的描述转为W系的描述，自然也存在一个$C_b^w$，而$C_w^b$与$C_b^w$又“恰巧”存在这样一个关系：
$$C_b^w=(C_w^b{)}^T$$

欧拉角

欧拉角在姿态的描述非常的常见，那么它与刚才的旋转矩阵又有什么样的关系呢？还记得刚才提过，旋转矩阵是描述W到B的“动作”，那么这个“动作”的具体分解其实就是欧拉角。

W到B的动作过程中，我们肯定希望动作的过程“有序”，“规范”，而不是“稀里糊涂”就过去了，这样显然更利于我们的理解和描述。自然而然的，我们想到W先绕某个轴旋转若干度，再绕某个轴旋转若干度。。。最后与B重合。这就与我们的欧拉角很接近了。

一般我们如下图建立坐标系，机头方向为X，机翼方向为Y，上下为Z。

默认地，称绕Z轴旋转的角为偏航角（yaw，$\psi$），绕Y轴旋转的为俯仰角（pitch，$\theta$），绕X轴旋转的角为横滚角（roll，$\varphi$）。建系的同时也规定了旋转的正方向，即：
$$从原点向轴的正方向看，顺时针为该轴旋转的正方向$$

那么假定W和B现在都确定了，我现在想要用欧拉角来描述W到B的变换，那么这个“描述”与什么有关呢？假设我有以下两次描述：

第一次：
W先绕Z轴旋转yaw度，然后绕Y轴（旋转后的Y轴）旋转pitch度，最后绕X轴旋转roll度，到达B
第二次：
W先绕X轴旋转roll度，再绕Y轴旋转pitch度，最后绕Z轴旋转yaw度，到达B

W和B都是固定的，即“起点”和“终点”都是确定的，那么两次的（yaw，pitch，roll）对应相等吗？答案显然是否定的，也就是说，尽管描述的是同一个“动作”，欧拉角的大小与旋转顺序有关。

第一次的旋转顺序是 “Z-Y-X” 而第二次是 “X-Y-Z”。

而在航模有关的领域中，我们一般习惯用“Z-Y-X”的顺序，姿态模拟常用的软件“匿名四轴上位机”的协议也是默认这个顺序。因此，接下来讲的欧拉角默认都是这个顺序。

那么，欧拉角与我们的旋转矩阵又有什么关系呢？

W系经过“Z（yaw）-Y（pitch）-X（roll）”这一旋转过程到达B，过程中实际还经历两个坐标系，分别记为1系和2系
$$W\underset{yaw}{\stackrel{C_w^1}{⟶}}1\underset{pitch}{\stackrel{C_1^2}{⟶}}2\underset{roll}{\stackrel{C_2^b}{⟶}}B$$
那么假设有一个向量${\alpha }_w$（始终静止），描述于W系下，经过$C_w^1$的变换到达${\alpha }_1$（以1系为参考描述的），也就是左乘一个$C_w^1$，那么如何求$C_w^1$呢？记得在旋转矩阵那一节中笔者提到过这样一句话：
$$该矩阵的每一列分别代表了W坐标系xyz三轴在B系的投影$$
这句话是我们推导欧拉角到旋转矩阵的关键。

试想，W系三轴（xyz）在1系中的投影是什么，不难知道（画个草图即可看出），是：
$$\left|\begin{array}{ccc}\cos \psi & \sin \psi & 0\\ -\sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right|$$
这个便是$C_w^1$，同理可得$C_1^2$和$C_2^b$，于是有：
$${\alpha }_b=C_2^b{C}_1^2{C}_w^1{\alpha }_w$$
等式右侧是左结合的，因此要从右往左去理解。那么不难发现，运用结合律其实$C_w^b=C_2^b{C}_1^2{C}_w^1$，见下图：

那么至此，欧拉角便和旋转矩阵建立了联系，二者相互转换。

内旋和外旋

这里顺便提一下外旋和内旋的概念，其实很简单，第一次旋转后到达1系后，下一次旋转时所说的轴是1系的轴还是最初W系的“轴”，若是指1系的轴，则为内旋；若是从始至终都是W系的“轴”，则为外旋。文中所指皆默认内旋。

关于死锁

使用欧拉角描述存在这样一个bug，即万向节死锁（Gimbal Lock），这里需要明确一点，死锁只存在于内旋的情况中。

那么什么叫死锁？同样还是以yaw-pitch-roll的顺序，假定pitch的值（第二个旋转角）为±90度，那么会发现，等到roll旋转时，彼时的x轴跟最开始的z轴重合，也就是说，这时的roll角，与开始的yaw角，表示的是同一个“自由度”。这时的roll角转动效果完全可以“转移”到一开始的yaw角，无非就是yaw开始时多转一点或者少转一点。3个自由度变成了2个自由度，丢失了一个自由度，这便是死锁。

下图是一个普通的欧拉旋转，不是死锁，可以看着理解（该图是Z-Y-Z的顺序，注意区分）：

四元数

由于欧拉角存在“死锁”的bug，因此引入四元数这个概念，可以消除bug，除此之外，四元数还具有很多优越性。四元数有关的知识，证明较为复杂，本文不做非常深入的分析，更多是认知性的结论与理解，达到使用的目的。

---

轴角法

在讲四元数之前要先引入“轴角法”，如果说欧拉角是将变换分解为三个动作，那么轴角法便是“一步到位”。试想任何一个旋转，无非就是明确：
$$旋转轴\eta ，旋转角度\alpha$$
那么W到B的旋转，是不是可以找到这样的一组$(\eta ,\alpha )$，描述该旋转？

答案是肯定的，而且，这样的一组$(\eta ,\alpha )$是存在且唯一的。

用这样的$(\eta ,\alpha )$去描述变换就叫“轴角法”。

---

那么回到我们的四元数，顾名思义，四元数有4个信息量，$q=(q0,q1,q2,q3)$，具象上的理解：
$$q=1个实数+1个向量$$
因此四元数还可以表示成$q=(\theta ,\upsilon )$，$q0$就是这个实数，$q1,q2,q3$便是向量的三个维度。

对于一个纯向量，用四元数表示时其$q0=0$,余下三个维度正常，这样的四元数我们称为纯四元数。一般来说，要描述的对象（向量）都是一个纯四元数，描述的动作都是一个非纯四元数。

那么如何将轴角法构造成一个四元数呢？这里直接给出结论：
$$q=(\cos \frac{\alpha }{2},\eta \sin \frac{\alpha }{2})$$
且这里进一步地规定，$\eta$是一个单位向量。

那么有$q$是一个单位四元数（模长为1），那么假设有一个向量$\upsilon$，经过轴角$(\eta ,\alpha )$变换为$v^{\prime }$，那么变换关系为：
$$v^{\prime }=q\ast v\ast q^{-}1$$
注意，这里的乘法是四元数特有的乘法“Graßmann积”

---

Graßmann积

假设有四元数$$k1=(a,\alpha ),k2=(b,\beta )$$，那么他们的Graßmann积为：
$$k1\ast k2=(ab-\alpha \beta ,a\beta +b\alpha +\alpha \times \beta )$$

---

$q^{-}1$是$q$的逆，即$q\ast q^{-}1=1$，由于$q$是单位四元数，$q^{-}1=q^{\ast }(q的共轭,实数部分不变，向量部分取反)$。

由此，四元数$q$可以视作一个变换（w系到b系的变换），那么$q$与旋转矩阵又有什么联系呢？这里直接给出结论：
$$C_w^b=\left|\begin{array}{ccc}1-2q_2^2-2q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& 1-2q_1^2-2q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& 1-2q_1^2-2q_2^2\end{array}\right|$$

结合欧拉角那节的结论，

$$C_w^b=\left|\begin{array}{ccc}\cos \theta \cos \psi & \cos \theta \sin \psi & -\sin \theta \\ \sin \varphi \sin \theta \cos \psi -\cos \varphi \sin \psi & \sin \varphi \sin \theta \sin \psi +\cos \varphi \cos \psi & \sin \varphi \cos \theta \\ \cos \varphi \sin \theta \cos \psi +\sin \varphi \sin \psi & \cos \varphi \sin \theta \sin \psi -\sin \varphi \cos \psi & \cos \varphi \cos \theta \end{array}\right|$$

在W和B确定时，转换关系确定，$C_w^b$自然也是确定且唯一的，因此这两个$C_w^b$矩阵完全相等，对应的元素也相等。

也就是说，当知道四元数时，我们可以完全地解出这个姿态矩阵$C_w^b$，解出矩阵的目的往往是为了去解算欧拉角，这里很显然，我们可以用解反三角函数的方法。

由于两个矩阵的对应元素相等，对号入座，因此有：
$$\begin{gathered} \psi =atan2(2(q1q2+q0q3),1-2q{2}^2-2q{3}^2) \\ \theta =arcsin(2(q0q2-q1q3)) \\ \varphi =atan2(2(q2q3+q0q1),1-2q{1}^2-2q{2}^2) \end{gathered}$$

---

关于atan

关于函数“$atan$”，某些朋友可能比较陌生，其实跟“$arctan$”是类似的含义，就是值域会扩大一些，后者的值域只有-90~90,前者可以到-180~180。因为前者将分子分母拆开输入，保留了分子分母符号的信息。

---

那么至于如何更新四元数，请移步《GY-86传感器的校准及使用》