四元数的微分形式

四元数微分形式的推导

关于四元数表示旋转的一些知识

1.四元数表示向量旋转
定义$p_v={\left[\begin{array}{ccc}{p}_x& p_y& p_z\end{array}\right]}^T$为三维空间中的一点，将其转换成纯四元数形式即：$p={\left[\begin{array}{cc}0& p_v\end{array}\right]}^T$，令$$q={\left[\begin{array}{cc}cos(\frac{\theta }{2})& vsin(\frac{\theta }{2})\end{array}\right]}^T$$为单位四元数，则有$$p^{\prime }=q\cdot p\cdot q^{-1}={\left[\begin{array}{cc}0& p_v^{\prime }\end{array}\right]}^T$$其中$p_v^{\prime }={\left[\begin{array}{ccc}{p}_x^{\prime }& p_y^{\prime }& p_z^{\prime }\end{array}\right]}^T$表示$p_v$绕旋转轴$v$旋转$\theta$角度后得到的新向量在原三维空间中的坐标表示。

2.四元数表示坐标系旋转
定义向量$v_0$在$oxyz$坐标系中的表示为$v_1={\left[\begin{array}{ccc}{v}_x& v_y& v_z\end{array}\right]}^T$，令坐标系$oxyz$绕单位旋转轴$v$旋转$\theta$角度，得到新坐标系$o^{\prime }{x}^{\prime }{y}^{\prime }{z}^{\prime }$，此时$v_0$在新坐标系中的坐标表示为$v_1^{\prime }={\left[\begin{array}{ccc}{v}_x^{\prime }& v_y^{\prime }& v_z^{\prime }\end{array}\right]}^T$。那么，向量$v_0$在两个坐标系之间的坐标转换关系为：
$${\left[\begin{array}{cc}0& v_1^{\prime }\end{array}\right]}^T=q^{-1}\cdot {\left[\begin{array}{cc}0& v_1\end{array}\right]}^T\cdot q$$
其中
$$q={\left[\begin{array}{cc}cos(\frac{\theta }{2})& vsin(\frac{\theta }{2})\end{array}\right]}^T$$
值得注意的是，上述两种四元数表示旋转的方式得到的四元数旋转关系是不同的，这是因为1.表示向量在同一个坐标系中的旋转，而2.中旋转的是坐标系。

以1.为例，先做$q_1$旋转，再做$q_2$旋转之后向量$x$在新坐标系中的表示为$x^{\prime }$，则有：
$$x_{temp}=q_1\cdot x\cdot {q_1}^{-1}$$
$$x^{\prime }=q_2\cdot x_{temp}\cdot {q_2}^{-1}$$
$$x^{\prime }=q_2{q}_1\cdot x_{temp}\cdot {q_2{q}_1}^{-1}$$

四元数$q_2{q}_1$表示了连续两次的旋转。

以2.为例，先做$q_1$旋转，再做$q_2$旋转之后向量$x$在新坐标系中的表示为$x^{\prime }$，则有：
$$x_{temp}=q_1^{-1}\cdot x\cdot q_1$$
$$x^{\prime }=q_2^{-1}\cdot x_{temp}\cdot q_2$$
$$x^{\prime }={q_1{q}_2}^{-1}\cdot x_{temp}\cdot q_1{q}_2$$

四元数$q_1{q}_2$表示了连续两次的旋转。

四元数微分形式推导

以无人机的姿态表示为例子，我们定义单位四元数$q(t)$来表示从无人机的地理系E到机体系B的旋转关系。在$t+\Delta t$时刻，旋转可表示为$q(t+\Delta t)$；即在$\Delta t$过程中，机体坐标系又经过了一个微小旋转，这个微小旋转的瞬时旋转角速度为$w$；接着对瞬时旋转轴做单位化处理$$\hat{w}=w/||w||$$在$\Delta t$时刻转过的角度为$$\Delta \theta =\Delta t||w||$$则这次的微小旋转可由如下形式的单位四元数表示：

$$\Delta q=\cos \frac{\Delta \theta }{2}+\sin \frac{\Delta \theta }{2}\hat{w}=\cos \frac{||w||}{2}\Delta t+\sin \frac{||w||}{2}\Delta t\hat{w}$$

假定地理系到机体系的旋转四元数为$q_e^b={\left[\begin{array}{cccc}{q}_0& q_1& q_2& q_3\end{array}\right]}^T$；
则根据上述2.有
$${\left[\begin{array}{cc}0& {}^{e}r\end{array}\right]}^T=(q_b^e{)}^{-1}{\left[\begin{array}{cc}0& {}^{b}r\end{array}\right]}^T{q}_b^e$$
即(注意：此处转化为与1.相同的表示形式，但两者的物理意义不同)：
$${\left[\begin{array}{cc}0& {}^{e}r\end{array}\right]}^T=(q_e^b){\left[\begin{array}{cc}0& {}^{b}r\end{array}\right]}^T(q_e^b{)}^{-1}$$

那么根据上面的推导，连续两次的旋转可以表示为：$q(t+\Delta t)=\Delta q\cdot q$；则有：

$$\begin{gathered} q(t+\Delta t)-q(t)=(\cos \frac{||w||}{2}\Delta t+\sin \frac{||w||}{2}\Delta t\hat{w})q(t)-q(t) \\ =-2{\sin }^2\frac{||w||}{2}\Delta tq(t)+\hat{w}\sin \frac{||w||}{2}\Delta tq(t) \end{gathered}$$

略去高阶项可得：
$$q(t+\Delta t)-q(t)=\hat{w}\sin \frac{||w||}{2}\Delta tq(t)$$
$$\begin{gathered} \dot{q(t)}=\underset{\Delta t\to 0}{\lim }\frac{q(t+\Delta t)-q(t)}{\Delta t}=\underset{\Delta t\to 0}{\lim }\frac{\hat{w}\sin \frac{||w||}{2}}{\Delta t}\cdot q(t) \\ =\frac{1}{2}\hat{w}\cdot \Vert w\Vert \cdot q(t) \\ =\frac{1}{2}\cdot w\cdot q(t) \end{gathered}$$

注意：此处的$\cdot$表示四元数乘法；$w$为角速度的纯四元数表示，$w={\left[\begin{array}{cccc}0& w_x& w_y& w_z\end{array}\right]}^T$;

由于实际工程中我们都是通过固连在机体上的陀螺仪等传感器来获知机体角速度$$w^b$$它与地理坐标系下的角速度表示有如下关系$$w=q(t)w^{b}q(t{)}^{-1}$$带入上式即可得到姿态解算过程中常用的四元数的微分形式$$\dot{q}(t)=\frac{1}{2}q(t)w^b$$
可以看出，通过一次四元数乘法运算便可得到四元数的微分。
上式可以写成如下的矩阵形式：

$$\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^b\\ {\omega }_y^b\\ {\omega }_z^b\end{array}\right]$$

或者：

$$\left[\begin{array}{l}{\dot{q}}_0\\ {\dot{q}}_1\\ {\dot{q}}_2\\ {\dot{q}}_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\omega }_x^b& -{\omega }_y^b& -{\omega }_z^b\\ {\omega }_x^b& 0& {\omega }_z^b& -{\omega }_y^b\\ {\omega }_y^b& -{\omega }_z^b& 0& {\omega }_x^b\\ {\omega }_z^b& {\omega }_y^b& -{\omega }_x^b& 0\end{array}\right]\left[\begin{array}{l}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]$$

四元数与旋转矩阵的关系

我们知道，从地理坐标系到机体坐标系的旋转矩阵可以有如下形式表示：（一般均为zyx顺序旋转）

$$R_e^b=R_X\cdot R_Y\cdot R_z=\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]$$

从机体系到地理系的由四元数表示的旋转矩阵为

$$R_b^e=\left[\begin{array}{lll}1-2q_2^2-2q_3^2& 2q_1{q}_2-2q_0{q}_3& 2q_1{q}_3+2q_0{q}_2\\ 2q_1{q}_2+2q_0{q}_3& 1-2q_1^2-2q_3^2& 2q_2{q}_3-2q_0{q}_1\\ 2q_1{q}_3-2q_0{q}_2& 2q_2{q}_3+2q_0{q}_1& 1-2q_1^2-2q_2^2\end{array}\right]$$
则有$$R_b^e={R_e^b}^T$$再根据对应项相等，即可求出由四元数表示的欧拉角

$$\varphi =\arctan 2\left(2\left(q_2{q}_3+q_0{q}_1\right),\left(1-2\left(q_1^2+q_2^2\right)\right)\right)$$

$$\theta =-\arcsin \left(2\left(q_1{q}_3-q_0{q}_2\right)\right)$$

$$\psi =\arctan 2\left(2\left(q_1{q}_2+q_0{q}_3\right),\left(1-2\left(q_2^2+q_3^2\right)\right)\right)$$

同样的，由欧拉角表示的四元数如下：

$$q==\left[\begin{array}{c}\cos \frac{\varphi }{2}\cos \frac{\theta }{2}\cos \frac{\psi }{2}+\sin \frac{\varphi }{2}\sin \frac{\theta }{2}\sin \frac{\psi }{2}\\ \sin \frac{\psi }{2}\cos \frac{\psi }{2}-\cos \frac{\varphi }{2}\sin \frac{\theta }{2}\sin \frac{\psi }{2}\\ \cos \frac{\varphi }{2}\sin \frac{\theta }{2}\cos \frac{\psi }{2}+\sin \frac{\varphi }{2}\cos \frac{\theta }{2}\sin \frac{\psi }{2}\\ \cos \frac{\varphi }{2}\cos \frac{\theta }{2}\sin \frac{\psi }{2}-\sin \frac{\varphi }{2}\sin \frac{\theta }{2}\cos \frac{\psi }{2}\end{array}\right]$$

上式中$$\varphi ,\theta ,\psi$$分别表示绕机体三轴转动的欧拉角;

欧拉角表示法的弊端

欧拉角速率与机体角速度之间的关系如下式所示：

$$\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{ccc}1& \sin \varphi \tan \theta & \cos \varphi \tan \theta \\ 0& \cos \varphi & -\sin \varphi \\ 0& \sin \varphi /\cos \theta & \cos \varphi /\cos \theta \end{array}\right]\left[\begin{array}{l}\mathbf{p}\\ \mathbf{q}\\ \mathbf{r}\end{array}\right]$$

可以看到，当$$\theta =90^{。}$$这时0出现在分母上，也就是我们所说的解不存在的情况，此时无法实现有效控制。这就是常说的“万向节锁死”；这也是不用旋转矩阵来进行姿态递推的原因。