四元数与旋转矩阵到运动学方程

本文将从二维旋转入手，讲解三维空间中的向量是如何利用四元数进行旋转的，最后介绍基于四元数的空间运动学方程的由来。

四元数与旋转矩阵到运动学方程

二维旋转

​
要想理解四元数，需要先从二维空间入手，给定一个向量 $z=x+iy=r(\cos \delta +i\sin \delta )=r{e}^{\delta i},$ 将$z$乘以$e^{i\varphi }=(\cos \varphi +i\sin \varphi )$ 得到 $z^{{}^{\prime }}=r[\cos (\varphi +\delta )+i\sin (\varphi +\delta )]$。如下图所示，所以，复数(一个虚轴)可以用来描述平面的转动

那么，按照这个思路，刚体在三维空间中的运动就需要3个虚轴来描述，因为虽然，三维空间虽然只多了一维，但是确多了两个转轴，所以需要三个虚轴来描述刚体在三维空间中的转动。因此，三个虚部加一个实部就构成了四元数。

四元数

四元数描述的是两个坐标轴之间的旋转关系
四元数定义:
$$\begin{array}{rl}q=& q_0+q_{1}i+q_{2}j+q_{3}k\\ =& (\cos \frac{\varphi }{2}+e_x\sin \frac{\varphi }{2}+e_y\sin \frac{\varphi }{2}+e_z\sin \frac{\varphi }{2})\cdot \Vert q\Vert \end{array}$$
其中: $\left|q\right|=\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}=\sqrt{\Vert q\Vert }$; $[e_x,e_y,e_z]$为转轴; $i,j,k$满足：$ii=jj=kk=-1$; $ij=-ji=k;jk=-kj=i;ki=-ik=j$。
当$\Vert q\Vert =1$时
$$\begin{array}{rl}q=& q_0+q_{1}i+q_{2}j+q_{3}k\\ =& (\cos \frac{\varphi }{2}+e_x\sin \frac{\varphi }{2}+e_y\sin \frac{\varphi }{2}+e_z\sin \frac{\varphi }{2})\\ =& \cos \frac{\varphi }{2}+n\cdot \sin \frac{\varphi }{2}\end{array}$$
所以根据二维旋转类比推理，如果一个纯四元数向量$p=[0,p_1,p_2,p_3]$希望绕$[e_x,e_y,e_z]$旋转$\frac{\varphi }{2}$，得
$$\begin{array}{rl}Q=& q\cdot p\\ =& \left(\cos \frac{\varphi }{2}+n\cdot \sin \frac{\varphi }{2}\right)\cdot p\end{array}$$
将上式写为矩阵与向量相乘的形式
$$\begin{array}{rl}Q=& \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}$$
那么，两个四元数相乘转变成矩阵$L$右乘四元数$q$。调换一下顺序，我们还可以将该行为写成矩阵$R$左乘四元数$q$，具体形式如下：
$$\begin{array}{rl}q\cdot p=& L\left(q\right)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}{r}{p}_0\\ p_1\\ p_2\\ p_3\end{array}\right]\\ q\cdot p=& R\left(p\right)q=\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}{r}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]\end{array}$$
上式虽然将三维空间中的向量$[p_1,p_2,p_3]$旋转了$\frac{\varphi }{2}$，但同时也将其代入了四维空间，因此需要在旋转$\frac{\varphi }{2}$将其拉回三维空间，因此得出四元数旋转方程：
$$\begin{array}{c}{R}_1=q\cdot p\cdot q^{-1}=L\left(q\right)p{q}^{-1}=R\left(q^{-1}\right)L\left(q\right)p\end{array}$$
写为矩阵形式：
$$\begin{array}{rl}{R}_1=q\cdot p\cdot q^{-1}=R\left(q\right)L\left(q\right)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}{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\\ p_1\\ p_2\\ p_3\end{array}\right]\\ & =\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& q_0^2+q_1^2-q_2^2-q_3^2& 2\left(q_1{q}_2-q_0{q}_3\right)& 2\left(q_1{q}_3+q_0{q}_2\right)\\ 0& 2\left(q_1{q}_2+q_0{q}_3\right)& q_0^2-q_1^2+q_2^2-q_3^2& 2\left(q_2{q}_3-q_0{q}_1\right)\\ 0& 2\left(q_1{q}_3-q_0{q}_2\right)& 2\left(q_2{q}_3+q_0{q}_1\right)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]\left[\begin{array}{c}0\\ p_1\\ p_2\\ p_3\end{array}\right]\end{array}$$
因此，我们得到了四元数旋转矩阵。

以上部分内容借鉴于文章四元数应用——转矩阵、Slerp插值与万向节

基于四元数的运动学方程

首先定义坐标系：$n$系为导航坐标系(东北天)，$b$系为载体坐标系，则$n$系到$b$的旋转四元数可以表示为：
$$\begin{array}{r}Q=\cos \frac{\varphi }{2}+{\mu }_n\cdot \sin \frac{\varphi }{2}\end{array}$$
其中，${\mu }_n$为转轴，$\varphi$为旋转角，对两边求导可得：
$$\begin{array}{c}\frac{dQ}{dt}=-\frac{\dot{\varphi }}{2}\sin \frac{\varphi }{2}+{\mu }^n\frac{\dot{\varphi }}{2}\cos \frac{\varphi }{2}+\sin \frac{\varphi }{2}\frac{d{\mu }^n}{dt}\end{array}$$
根据哥氏定理：
$$\begin{array}{c}\frac{d{\mu }^n}{dt}=C_b^n\frac{d{u}^b}{dt}+{\omega }_{nb}^n\times {\mu }^n\end{array}$$
其中${\omega }_{nb}^n$、${\omega }_{nb}^b$代表载体$b$相对于$n$的角速度在$n$、$b$中的表示。
根据文章四元数微分方程的推导和解算实现，得到
$$\begin{array}{c}\frac{d{u}^b}{dt}=0\end{array}$$
和
$$\begin{array}{c}{\omega }_{nb}^n=\dot{\varphi }{\mu }_n\end{array}$$
上式中的意思是：$n$系到$b$的角速度在$n$系上的投影。
所以
$$\begin{array}{c}\frac{d{\mu }_n}{dt}=\dot{\theta }{\mu }_n\times {\mu }_n=0\end{array}$$
因此，根据四元数乘法法则：${\omega }_{nb}^n\otimes {\omega }_{nb}^n=-1$
$$\begin{array}{rl}\frac{dQ}{dt}& =-\frac{\dot{\theta }}{2}\sin \frac{\theta }{2}+{\mu }^n\frac{\dot{\theta }}{2}\cos \frac{\theta }{2}\\ & ={\mu }^n\frac{\dot{\theta }}{2}\left(\cos \frac{\theta }{2}+{\mu }^n\sin \frac{\theta }{2}\right)\\ & =\frac{1}{2}{\omega }_{nb}^{n}Q\end{array}$$
将(1)式的${\omega }_{nb}^n=Q\cdot {\omega }_{nb}^b\cdot Q^{-1}$代入得
$$\begin{array}{c}\frac{dQ}{dt}=\frac{1}{2}Q{\omega }_{nb}^b\end{array}$$
由此我们得到了基于四元数的运动学方程
$$\begin{array}{c}\left[\begin{array}{c}{\dot{q}}_0\\ {\dot{q}}_1\\ {\dot{q}}_2\\ {\dot{q}}_3\end{array}\right]=\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}$$