三、四元数以及陀螺仪角速度

---

前言

本节结来讨论四元数

---

一、从复数到四元数

众所周知，在复平面上旋转变换非常容易，那么有没有比复数系更加复杂的数系，理论上是没有，因为一旦我们对复数进行推广就必须牺牲乘法交换律。那么牺牲交换律过后我们能把复数扩张到哪种程度呢？
首先复数的一般形式为$a+bi$，那么我们先来看能不能增加一个虚数单元呢？
假设复数扩张后我们得到数的更一般的形式$a+bi+cj$，其中$a,b,c$为实数，$i,j$是两个虚数单位，并且满足已知的复数乘法规则：$ii=-1,jj=-1$
显然推广后在做乘法的时候会出现$ij$乘积：我们假设$ij=a+bi+cj$，
那么就有$-j=iij=i(a+bi+cj)=ai-b+cij=ai-b+c(a+bi+cj)=ca-b+(a+cb)i+c^{2}j$，显然这个式子要成立，我们需要实数$c$满足$c^2=-1$，很遗憾我们看到“三元数”是不能存在的。
上面得到的教训是$ij$不属于集合$a+bi+cj$，于是我们令$ij=k$，并把集合扩大成$a+bi+cj+dk$，新的形式如果做乘法一定会出现诸如$ij,jk,ki,ji,kj,ik$的形式，那么参照推导“三元数”的逻辑，可以令$jk=i,ki=j$，那么还有$ji,kj,ik$的乘积呢?
试一下：$-j=iij=ik$，因此有$ki=-ik$，我们看到为了使得逻辑自洽，我们只能牺牲乘法交换律了。
当然我们还要看看除法，实际上由于乘法不满足交换律，我们也只能像矩阵一样的给出逆的左乘和右乘了，而不能像复数那样法除数写在分数线下面了。沿着复数的足迹，老实计算一下发现：
$(a+bi+cj+dk)(a-bi-cj-dk)=a^2+b^2+c^2+d^2=|a+bi+cj+dk{|}^2$
所以我们发现四元数的逆和共轭于复数简直是一模一样。
因此我们宣布，除了乘法交换律不满足以外，复数的最小扩张就是四元数了。

二、用矢量表示四元数

由于四元数有3个虚分量，因此我们用向量形式来简化书写，并且可以使用向量的内积和叉乘来简化公式。
则两个四元数可以表示如下：
$u_1+{\mathbf{v}}_1$
$u_2+{\mathbf{v}}_2$
则$(u_1+{\mathbf{v}}_1)(u_2+{\mathbf{v}}_2)=u_1{u}_2-{\mathbf{v}}_1\cdot {\mathbf{v}}_2+u_1{\mathbf{v}}_2+u_2{\mathbf{v}}_1+{\mathbf{v}}_1\times {\mathbf{v}}_2$
其中内积和叉乘使用一般的向量计算的规则。
棣莫弗公式的推广：
复数的旋转计算中棣莫弗公式非常好用，既然我们都用向量表示了四元数，那么可以可以推广一下棣莫弗公式用来做三维圆转呢？推广的时候注意一下虚部是三维的。
$$u+\mathbf{v}=\sqrt{u^2+{\mathbf{v}}^2}(\frac{u}{\sqrt{u^2+{\mathbf{v}}^2}}+\frac{|\mathbf{v}|}{\sqrt{u^2+{\mathbf{v}}^2}}\frac{\mathbf{v}}{|\mathbf{v}|})$$
如果记：
$$\begin{array}{r}\frac{u}{\sqrt{u^2+{\mathbf{v}}^2}}=cos\theta ,\frac{|\mathbf{v}|}{\sqrt{u^2+{\mathbf{v}}^2}}=sin\theta ,\frac{\mathbf{v}}{|\mathbf{v}|}=\mathbf{n}\end{array}$$
则我们得到一个用模和$\theta$表示的四元数
$$u+\mathbf{v}=|u+\mathbf{v}|(cos\theta +\mathbf{n}sin\theta )$$

三、用四元数对一个矢量进行旋转

简单起见用一个实部为$0$的四元数$A$来表示某个空间向量，该向量绕单位矢量$\mathbf{n}$旋转$\theta$角可以用下面的式子计算：
$$\begin{array}{rl}& \left(cos\frac{\theta }{2}+\mathbf{n}sin\frac{\theta }{2}\right)\mathbf{A}\left(cos\frac{\theta }{2}-\mathbf{n}sin\frac{\theta }{2}\right)\\ =& \left(cos\frac{\theta }{2}\mathbf{A}-\mathbf{n}\cdot \mathbf{A}sin\frac{\theta }{2}+\mathbf{n}\times \mathbf{A}sin\frac{\theta }{2}\right)\left(cos\frac{\theta }{2}-\mathbf{n}sin\frac{\theta }{2}\right)\\ =& co{s}^2\frac{\theta }{2}\mathbf{A}-\mathbf{n}\cdot \mathbf{A}sin\frac{\theta }{2}cos\frac{\theta }{2}+\mathbf{n}\times \mathbf{A}sin\frac{\theta }{2}cos\frac{\theta }{2}\\ & +cos\frac{\theta }{2}sin\frac{\theta }{2}(\mathbf{A}\cdot \mathbf{n}-\mathbf{A}\times \mathbf{n})+si{n}^2\frac{\theta }{2}(\mathbf{n}\cdot \mathbf{A})\mathbf{n}+si{n}^2\frac{\theta }{2}[(\mathbf{n}\times \mathbf{A})\cdot \mathbf{n}-(\mathbf{n}\times \mathbf{A})\times \mathbf{n}]\\ =& co{s}^2\frac{\theta }{2}\mathbf{A}+2cos\frac{\theta }{2}sin\frac{\theta }{2}\mathbf{n}\times \mathbf{A}+si{n}^2\frac{\theta }{2}(\mathbf{n}\cdot \mathbf{A})\mathbf{n}+si{n}^2\frac{\theta }{2}\mathbf{n}\times (\mathbf{n}\times \mathbf{A})\\ =& {\cos }^2\frac{\theta }{2}\mathbf{A}+2cos\frac{\theta }{2}sin\frac{\theta }{2}\mathbf{n}\times \mathbf{A}+si{n}^2\frac{\theta }{2}(\mathbf{n}\cdot \mathbf{A})\mathbf{n}+si{n}^2\frac{\theta }{2}[(\mathbf{n}\cdot \mathbf{A})\mathbf{n}-\mathbf{A}]\\ =& \left(co{s}^2\frac{\theta }{2}-si{n}^2\frac{\theta }{2}\right)\mathbf{A}+2cos\frac{\theta }{2}sin\frac{\theta }{2}\mathbf{n}\times \mathbf{A}+(\mathbf{n}\cdot \mathbf{A})\mathbf{n}-\left(co{s}^2\frac{\theta }{2}-si{n}^2\frac{\theta }{2}\right)(\mathbf{n}\cdot \mathbf{A})\mathbf{n}\\ =& (\mathbf{n}\cdot \mathbf{A})\mathbf{n}+[\mathbf{A}-(\mathbf{n}\cdot \mathbf{A})\mathbf{n}]cos\theta +\mathbf{n}\times \mathbf{A}sin\theta \end{array}$$

四、旋转公式的几何意义

我们考察矢量$\mathbf{A}$，绕单位矢量$\mathbf{n}$，按右螺旋法则转动$\theta$角度的变换。
首先$\mathbf{A}$在旋转轴上分量$(\mathbf{n}\cdot \mathbf{A})\mathbf{n}$，不会随转动改变
而$\mathbf{A}$在旋转轴垂直的平面上的投影$\mathbf{A}-(\mathbf{n}\cdot \mathbf{A})\mathbf{n}$一定会随着转动而变换
我们来看我们来考察一下这个平面：
我们令$\mathbf{A}-(\mathbf{n}\cdot \mathbf{A})\mathbf{n}$方向为平面的x轴方向，则$\mathbf{n}\times [\mathbf{A}-(\mathbf{n}\cdot \mathbf{A})\mathbf{n}]=\mathbf{n}\times \mathbf{A}$相当于平面的y轴方向。
由于$\mathbf{A}-(\mathbf{n}\cdot \mathbf{A})\mathbf{n}$垂直于$\mathbf{n}$垂直，因此恰好有$|\mathbf{n}\times [\mathbf{A}-(\mathbf{n}\cdot \mathbf{A})\mathbf{n}]|=|\mathbf{A}-(\mathbf{A}\cdot \mathbf{n})\mathbf{n}|$
因此把$\mathbf{A}-(\mathbf{n}\cdot \mathbf{A})\mathbf{n}$旋转$\theta$角后，就是：$[\mathbf{A}-(\mathbf{n}\cdot \mathbf{A})\mathbf{n}]cos\theta +\mathbf{n}\times \mathbf{A}sin\theta$
因此旋转过后完整的矢量表示为：
$(\mathbf{n}\cdot \mathbf{A})\mathbf{n}+[\mathbf{A}-(\mathbf{n}\cdot \mathbf{A})\mathbf{n}]cos\theta +\mathbf{n}\times \mathbf{A}sin\theta$
可以看到第三节的公式确实代表一个绕单位矢量$\mathbf{n}$的旋转。

五、四元数表示旋转

前面已经验证了，四元数确实可以表示一个旋转，因此我们干脆把一个四元数
$$\begin{array}{r}\mathbf{q}=cos\frac{\theta }{2}+\mathbf{n}sin\frac{\theta }{2}\end{array}$$
看成一个旋转变换变换。当然具体作用到某个实际矢量时采用$\mathbf{q}\mathbf{A}{\mathbf{q}}^{-1}$的方式进行。
有一个非常经典的一般方法比较难解决，但四元数可以秒杀的问题：
对任意一个矢量，我们先让它绕${\mathbf{n}}_1$转动角度${\theta }_1$，然后让它绕${\mathbf{n}}_2$转动角度${\theta }_2$，总的转动效果可以看成绕${\mathbf{n}}_3$转动角度${\theta }_3$，这个总效果怎么求？
四元数给出的方案非常简洁：
$$cos\frac{{\theta }_3}{2}+{\mathbf{n}}_{3}sin\frac{{\theta }_3}{2}=\left(cos\frac{{\theta }_2}{2}+{\mathbf{n}}_{2}sin\frac{{\theta }_2}{2}\right)\left(cos\frac{{\theta }_1}{2}+{\mathbf{n}}_{1}sin\frac{{\theta }_1}{2}\right)$$
简记为：${\mathbf{q}}_3={\mathbf{q}}_2{\mathbf{q}}_1$
落实到一个具体的矢量$\mathbf{A}$的旋转效果就是这样的：${\mathbf{q}}_3\mathbf{A}{\mathbf{q}}_3^{-1}={\mathbf{q}}_2{\mathbf{q}}_1\mathbf{A}{\mathbf{q}}_1^{-1}{\mathbf{q}}_2^{-1}$
今后我们将专注于旋转变换的观念，不再讨论对某个具体矢量的变换。

六、四元数的微分方程

四元数$\mathbf{q}$的时间变化率：
$$\begin{array}{r}\frac{d\mathbf{q}}{dt}=-sin\frac{\theta }{2}\frac{d\theta }{2dt}+\mathbf{n}cos\frac{\theta }{2}\frac{d\theta }{2dt}+sin\frac{\theta }{2}\frac{d\mathbf{n}}{dt}\end{array}$$
可以看到右边的第三项不好处理，四元数描述的是绕单位向量$\mathbf{n}$以角速度$d\theta /dt$的转动，显然$\mathbf{n}$代表了转动角速度的方向，但是在一般情况下角速度的方向$\mathbf{n}$也是随着时间变化的，因此一般来说$d\mathbf{n}/dt$并不是$0$，也可以说要还要考虑角加速度的影响。
但是这里为了简化问题，我们忽略角加速度，并且我们也没有传感器直接测量角加速度，我们只能通过陀螺仪测量角速度。因此我们考虑$d\mathbf{n}/dt=0$，这样问题就得到一定程度的简化：
$$\begin{array}{r}\frac{d\mathbf{q}}{dt}=-sin\frac{\theta }{2}\frac{d\theta }{2dt}+\mathbf{n}cos\frac{\theta }{2}\frac{d\theta }{2dt}=\frac{1}{2}\frac{d\theta }{dt}\mathbf{n}\left(cos\frac{\theta }{2}+\mathbf{n}sin\frac{\theta }{2}\right)=\frac{1}{2}\frac{d\theta }{dt}\mathbf{n}\mathbf{q}\end{array}$$
这里需要澄清一下这里的转动是相对于地面坐标系讲过的，但是飞行器上的陀螺仪测量到的角速度$\mathbf{\omega }=({\omega }_x,{\omega }_y,{\omega }_z{)}^T$是相对于飞行器坐标的，因此我们要想办法把飞行器坐标系中的角速度转化成地面坐标系的角速度。
我们这样思考：$\mathbf{q}$代表了地面坐标系通过旋转转换到飞行器坐标系的变换，那么飞行器坐标系中的任意向量也可以通过$\mathbf{q}$变换成地面坐标系中的向量。
这其实是旋转的被动观点和主动观点的关系，一个容易记忆的例子就是考虑飞行器坐标系相对于地面坐标系绕$z$轴旋转了一个角度，此时观察随坐标系转动的一个是矢量很容易看出来。
因此我们有：
$$\begin{array}{r}\frac{d\theta }{dt}\mathbf{n}=\mathbf{q}\mathbf{\omega }{\mathbf{q}}^{-1}\end{array}$$
所以：
$$\begin{array}{r}\frac{d\mathbf{q}}{dt}=\frac{1}{2}\frac{d\theta }{dt}\mathbf{n}\mathbf{q}=\frac{1}{2}\mathbf{q}\mathbf{\omega }{\mathbf{q}}^{-1}\mathbf{q}=\frac{1}{2}\mathbf{q}\mathbf{\omega }\end{array}$$
我们希望的到矩阵表达式，为此我们先看一下任意两个四元数之间的乘法如何用矩阵表示，设有两个四元数，分别表示为：
$$\begin{array}{rlr}& \mathbf{p}=\left[\begin{array}{c}{p}_0\\ p_1\\ p_2\\ p_3\end{array}\right]& \mathbf{q}=\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]\end{array}$$
很容易验证，四元数乘法可以表示成：
$$\begin{array}{rl}& \mathbf{q}\mathbf{p}=\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& q_0& -q_3& q_2\\ q_2& q_3& q_0& -q_1\\ q_3& -q_2& q_1& q_0\end{array}\right]\left[\begin{array}{c}{p}_0\\ p_1\\ p_2\\ p_3\end{array}\right]\end{array}$$
由于：$$\begin{array}{rl}& \mathbf{\omega }=\left[\begin{array}{c}0\\ {\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]\end{array}$$

所以我们有第一种表现形式：
$$\begin{array}{r}\frac{d\mathbf{q}}{dt}=\frac{1}{2}\mathbf{q}\mathbf{\omega }=\frac{1}{2}\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& q_0& -q_3& q_2\\ q_2& q_3& q_0& -q_1\\ q_3& -q_2& q_1& q_0\end{array}\right]\left[\begin{array}{c}0\\ {\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]\end{array}$$
这样一种形式显然不是很方便，我们想把$\mathbf{\omega }$写在前面，怎么办呢？
我们回顾第二小节：
$(u_1+{\mathbf{v}}_1)(u_2+{\mathbf{v}}_2)=u_1{u}_2-{\mathbf{v}}_1\cdot {\mathbf{v}}_2+u_1{\mathbf{v}}_2+u_2{\mathbf{v}}_1+{\mathbf{v}}_1\times {\mathbf{v}}_2$
如果交换相乘的次序，不同的地方在于${\mathbf{v}}_1\times {\mathbf{v}}_2$和${\mathbf{v}}_2\times {\mathbf{v}}_1$而已，反映在矩阵上实际上就是：
$$\begin{array}{rl}& \mathbf{q}\mathbf{p}=\left[\begin{array}{cccc}{p}_0& -p_1& -p_2& -p_3\\ p_1& p_0& p_3& -p_2\\ p_2& -p_3& p_0& p_1\\ p_3& p_2& -p_1& p_0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]\end{array}$$
所以我们得到了常用形式：
$$\begin{array}{r}\frac{d\mathbf{q}}{dt}=\frac{1}{2}\mathbf{q}\mathbf{\omega }=\frac{1}{2}\left[\begin{array}{cccc}0& -{\omega }_x& -{\omega }_y& -{\omega }_z\\ {\omega }_x& 0& {\omega }_z& -{\omega }_y\\ {\omega }_y& -{\omega }_z& 0& {\omega }_x\\ {\omega }_z& {\omega }_y& -{\omega }_x& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]\end{array}$$

---

总结

本文说明了四元数的来历，讲述了四元数和旋转的关系，今后就可以使用四元数表示飞行器坐标相对于地面的转动关系，最后得到基于飞行器坐标系中的陀螺仪测得的角速度的四元数的微分方程，通过这个微分方程，我们可以用测量到的角速度来先验推演飞行器相对于地面的转动关系。