四元数姿态解算算法基础

姿态的表示方法

载体姿态有多种表示方法，常见的三种：欧拉角，姿态矩阵，四元数。
欧拉角的物理意义比较直观，即航向角$\psi$，俯仰角$\theta$，横滚角$\gamma$，分别是导航系到载体系的三个旋转角度。
姿态矩阵可以由欧拉角直接计算得到，即三个角度对应的转换矩阵依相乘。

注意的是导航系到载体系是按照航向角，俯仰角，横滚角的顺序变换的所以导航系到载体系变换时，三个矩阵相乘的顺序应该是$C(\gamma )\ast (\theta )\ast C(\psi )$.

四元数表示方法较为抽象，直接从四元数“很难”看出载体姿态（这个姿态是对人的想象来说的姿态，因为四元数和欧拉角本无本质区别，只是一种表示方法），那么他的物理意义是什么呢？

四元数表示姿态的物理意义

四元数表示姿态时有多个写法，其中一种表示方法如下：
$$q=cos(\frac{\theta }{2})+u^{n}sin(\frac{\theta }{2})$$

欧拉角表示姿态变换时，表征的是分别绕着$(i,j,k)$三个坐标轴的三次旋转。根据欧拉定理，这三次旋转可以等效成绕着某轴一次旋转而成。这个某轴就是公式中的$u^n=(u_1,u_2,u_3)$,旋转角度就是公式中的$\theta$。
四元数通常写为$q=q_0+q_1+q_2+q_3$
那么用于姿态表示时,
$q_0=cos(\frac{\theta }{2})$
$q_i=u_i\ast sin(\frac{\theta }{2}),i=1,2,3$

使用四元数进行载体姿态更新方程

$$q(t+\delta t)=q(t)+\frac{dq}{dt}\times \delta t$$
四元数的更新可通过上式完成。因此只要知道初始四元数（载体的初始姿态）和四元数的导数就可以完成任意时刻四元数的解算。
例如px4中姿态更新的代码如下

// Apply correction to state
	_q += _q.derivative(corr) * dt;
1
2

四元数微分方程

$\frac{dq}{dt}$ 需要求解四元数微分方程，基本推导过程如下：

$$\frac{dq}{dt}=-\frac{\dot{\theta }}{2}sin(\frac{\theta }{2})+u^n\frac{\dot{\theta }}{2}cos(\frac{\theta }{2})+sin(\frac{\theta }{2})\frac{d{u}^n}{dt}$$
其中，$\frac{d{u}^n}{dt}=0$，因此：

$$\frac{dq}{dt}=-\frac{\dot{\theta }}{2}sin(\frac{\theta }{2})+u^n\frac{\dot{\theta }}{2}cos(\frac{\theta }{2})$$

$$=u^n\times u^n\frac{\dot{\theta }}{2}sin(\frac{\theta }{2})+u^n\frac{\dot{\theta }}{2}cos(\frac{\theta }{2})$$

$$=u^n\frac{\dot{\theta }}{2}\times (u^{n}sin(\frac{\theta }{2})+cos(\frac{\theta }{2}))$$

$$=u^n\frac{\theta }{2}\times q=\frac{{\omega }_{nb}^n\times q}{2}$$

结果中的角速度为导航系到机体系在导航系下的旋转角速度矢量，但是我们通过陀螺仪测得的是载体系下的角速度。因此还需要一部变换：

$${\omega }_{nb}^n=q\times {\omega }_{nb}^b\times q^{\ast }$$
由此可得到，
$$\frac{dq}{dt}=q\times \frac{{\omega }_{nb}^b}{2}$$

我们得到了四元数导数和载体系角速度的数学关系，展开上式得$\frac{dq}{dt}$：
[ 0 − ω x − ω y − ω z ω x 0 ω z − ω y ω y − ω z 0 ω x ω z ω y ω x 0 ] × [ q 0 q 1 q 2 q 3 ]

$$\begin{bmatrix} 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{bmatrix}$$ \times $$\begin{bmatrix} q_0 \\ q_1 \\q_2\\q_3 \end{bmatrix}$$ ⎣⎢⎢⎡​0ωx​ωy​ωz​​−ωx​0−ωz​ωy​​−ωy​ωz​0ωx​​−ωz​−ωy​ωx​0​⎦⎥⎥⎤​×⎣⎢⎢⎡​q0​q1​q2​q3​​⎦⎥⎥⎤​
也可以写成，
$$\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& 0& q_3& -q_2\\ q_2& -q_3& 0& q_1\\ q_3& q_2& q_1& q_0\end{array}\right]\times \left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]$$

四元数初始值确定

px4 中的初始四元数计算。

bool AttitudeEstimatorQ::init()
1

四元数初始值的确实可以看做是一个初始对准的过程。
而上述模型仅仅是理想状态下的数学模型，实际角速度中必然包含零偏、噪声等误差，更新过程中又不可避免的有飘漂移发生。所以仅用上式是无法给出可以使用的姿态的。
实际使用中常常用kalman滤波或者补偿滤波的方法来“校正”这些误差。因此初始值可以不必那么精确。给出一个概略初始值即可。
例如可以通过加速度z轴方向和磁罗盘的输出向量，来完成对准。两者从物理概念上实际上分别完成了调平和指北的任务。