无人机刚体运动学方程

平动运动学

由无人机平动动力学方程得到无人机的速度后，经过积分即可得到位置信息。
在NED坐标系下
$${\mathbf{X}}_N=\int {\mathbf{v}}_N^E=\int [uvw{]}^T$$
在WGS-84坐标系下
$$\begin{gathered} \mu =\int \frac{u}{R_N+h},(-\pi /2\le \mu \le \pi /2) \\ \lambda =\int \frac{v}{\cos \mu (R_E+h)},(-\pi \le \lambda \le \pi ) \\ h=-\int w \end{gathered}$$
经纬度的积分计算要特别考虑在边界附近的翻转。

转动运动学

欧拉角和方向余弦矩阵

欧拉角是用来唯一地确定定点转动刚体相对位置关系的三个角度，可以用来描述两个坐标系之间的转动关系。从参考坐标系到目标坐标系的定点转动可以通过参考坐标系绕其坐标轴作三次定轴旋转得到，每次旋转的角度统称为欧拉角。欧拉角的数值与旋转的顺序有关，不同的旋转顺序可能得到不同的欧拉角。航空中通常使用z-y-x的旋转顺序，这三次转动欧拉角分别为偏航角($\psi$)、俯仰角($\theta$)和滚转角($\varphi$)。
欧拉角具有直观的物理意义，在通常的表达中一般采用欧拉角。欧拉角在特定位置会出现奇异，即一个确定的转动无法用唯一确定的一组欧拉角来表示。
同一个矢量在不同坐标系中的投影分量之间的转换关系，可以通过矩阵运算来表示
$${\mathbf{r}}_B={\mathbf{M}}_{BN}{\mathbf{r}}_N$$
${\mathbf{M}}_{BN}$即由坐标系N到坐标系B的方向余弦矩阵(DCM，Direction Consine Matrix)。方向余弦矩阵是一个三阶正交方阵。方向余弦矩阵中的列元素为参考坐标系各坐标轴单位向量在目标坐标系中的投影分量。

根据欧拉角的定义，由NED坐标系到机体坐标系的第一次旋转是绕NED坐标系z轴转动$\psi$角，相应方向余弦矩阵为
M z = [ cos ⁡ ψ sin ⁡ ψ 0 − sin ⁡ ψ cos ⁡ ψ 0 0 0 1 ] \bm{M}_z= \begin{bmatrix} \cos\psi & \sin\psi & 0\\ -\sin\psi & \cos\psi & 0\\ 0 & 0 & 1 \end{bmatrix} Mz​= ​cosψ−sinψ0​sinψcosψ0​001​ ​
第二次绕新坐标系y轴旋转$\theta$角，相应方向余弦矩阵为
M y = [ cos ⁡ θ 0 − sin ⁡ θ 0 1 0 sin ⁡ θ 0 cos ⁡ θ ] \bm{M}_y= \begin{bmatrix} \cos\theta & 0 & -\sin\theta\\ 0 & 1 & 0\\ \sin\theta & 0 & \cos\theta \end{bmatrix} My​= ​cosθ0sinθ​010​−sinθ0cosθ​ ​
第三次绕新坐标系x轴旋转$\varphi$角，相应方向余弦矩阵为
M x = [ 1 0 0 0 cos ⁡ ϕ sin ⁡ ϕ 0 − sin ⁡ ϕ cos ⁡ ϕ ] \bm{M}_x= \begin{bmatrix} 1 & 0 & 0\\ 0 & \cos\phi & \sin\phi\\ 0 & -\sin\phi & \cos\phi \end{bmatrix} Mx​= ​100​0cosϕ−sinϕ​0sinϕcosϕ​ ​
因此，由NED坐标系到机体坐标系的方向余弦矩阵为
M B N = M x M y M z = [ cos ⁡ θ cos ⁡ ψ cos ⁡ θ sin ⁡ ψ − sin ⁡ θ sin ⁡ ϕ sin ⁡ θ cos ⁡ ψ − cos ⁡ ϕ sin ⁡ ψ sin ⁡ ϕ sin ⁡ θ sin ⁡ ψ + cos ⁡ ϕ cos ⁡ ψ sin ⁡ ϕ cos ⁡ θ cos ⁡ ϕ sin ⁡ θ cos ⁡ ψ + sin ⁡ ϕ sin ⁡ ψ cos ⁡ ϕ sin ⁡ θ sin ⁡ ψ − sin ⁡ ϕ cos ⁡ ψ cos ⁡ ϕ cos ⁡ θ ] \bm{M}_{BN}=\bm{M}_x\bm{M}_y\bm{M}_z= \begin{bmatrix} \cos\theta\cos\psi & \cos\theta\sin\psi & -\sin\theta\\ \sin\phi\sin\theta\cos\psi-\cos\phi\sin\psi & \sin\phi\sin\theta\sin\psi+\cos\phi\cos\psi & \sin\phi\cos\theta\\ \cos\phi\sin\theta\cos\psi+\sin\phi\sin\psi & \cos\phi\sin\theta\sin\psi-\sin\phi\cos\psi & \cos\phi\cos\theta \end{bmatrix} MBN​=Mx​My​Mz​= ​cosθcosψsinϕsinθcosψ−cosϕsinψcosϕsinθcosψ+sinϕsinψ​cosθsinψsinϕsinθsinψ+cosϕcosψcosϕsinθsinψ−sinϕcosψ​−sinθsinϕcosθcosϕcosθ​ ​
由方向余弦矩阵求欧拉角的方法为
$$\{\begin{array}{c}\varphi ={\tan }^{-1}\frac{m_{23}}{m_{33}}\\ \theta =-{\sin }^{-1}{m}_{13}\\ \psi ={\tan }^{-1}\frac{m_{12}}{m_{11}}\end{array}$$
可见，当$\theta \to \pm 90^{\circ }$的时候，$\cos \theta \to 0$，$\varphi$和$\psi$将无法确定。

四元数

利用四元数表示姿态不会产生奇异问题，且运算量较小，通常在计算中采用四元数进行运算。
四元数定义为
$$\mathbf{Q}=q_0+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}={\left[\begin{array}{cccc}{q}_0& q_1& q_2& q_3\end{array}\right]}^T$$
两个四元数的乘积仍为四元数，四元数乘法满足结合律，但不满足交换律。两个四元数$\mathbf{P}$和$\mathbf{Q}$的乘积为
P ∘ Q = [ 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 ] [ q 0 q 1 q 2 q 3 ] = [ 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 ] [ p 0 p 1 p 2 p 3 ] \bm{P}\circ\bm{Q}= \begin{bmatrix} 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{bmatrix} \begin{bmatrix} q_0\\ q_1\\ q_2\\ q_3 \end{bmatrix}= \begin{bmatrix} 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{bmatrix} \begin{bmatrix} p_0\\ p_1\\ p_2\\ p_3 \end{bmatrix} P∘Q= ​p0​p1​p2​p3​​−p1​p0​p3​−p2​​−p2​−p3​p0​p1​​−p3​p2​−p1​p0​​ ​ ​q0​q1​q2​q3​​ ​= ​q0​q1​q2​q3​​−q1​q0​−q3​q2​​−q2​q3​q0​−q1​​−q3​−q2​q1​q0​​ ​ ​p0​p1​p2​p3​​ ​
$\mathbf{Q}$的共轭四元数可表示为
$${\mathbf{Q}}^{-1}=q_0-q_1\mathbf{i}-q_2\mathbf{j}-q_3\mathbf{k}={\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\end{array}\right]}^T$$
$\mathbf{Q}$的模为
$$\Vert \mathbf{Q}\Vert =\sqrt{\mathbf{Q}\circ {\mathbf{Q}}^{-1}}=\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}$$
若$\mathbf{Q}$的模为1，则称$\mathbf{Q}$为单位四元数。
空间中矢量的旋转可由绕转轴$\mathbf{\xi }$转动有限转角$\rho$的一次旋转完成，该过程可用单位四元数表示
$$\mathbf{Q}=\cos \frac{\rho }{2}+\sin \frac{\rho }{2}\mathbf{\xi }$$
对于四元数矢量$\mathbf{r}={\left[0r_x{r}_y{r}_z\right]}^T$，在空间中绕转轴$\mathbf{\xi }$转动有限转角$\rho$的转动可以表示为
$${\mathbf{r}}^{\prime }=\mathbf{Q}\circ \mathbf{r}\circ {\mathbf{Q}}^{-1}$$
而坐标系的旋转的过程也可由四元数运算体现，与矢量的旋转相反。若${\mathbf{Q}}_{BN}$表示由NED坐标系到机体坐标系的旋转四元数，则相应的同一矢量的在两个坐标系中投影坐标的变换过程为
$${\mathbf{r}}_b={\mathbf{Q}}_{BN}^{-1}\circ {\mathbf{r}}_n\circ {\mathbf{Q}}_{BN}$$
姿态四元数之间的递推关系为
$${\mathbf{Q}}_{BI}={\mathbf{Q}}_{NI}\circ {\mathbf{Q}}_{BN}$$
！千万注意不要混淆矢量旋转和坐标变换，否则后续的推导将出错！
姿态四元数与欧拉角、方向余弦矩阵的转换关系为
$$\{\begin{array}{c}{q}_0=\cos \frac{\varphi }{2}\cos \frac{\theta }{2}\cos \frac{\psi }{2}+\sin \frac{\varphi }{2}\sin \frac{\theta }{2}\sin \frac{\psi }{2}\\ q_1=\sin \frac{\varphi }{2}\cos \frac{\theta }{2}\cos \frac{\psi }{2}-\cos \frac{\varphi }{2}\sin \frac{\theta }{2}\sin \frac{\psi }{2}\\ q_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}\\ q_3=\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}$$
M B N = [ q 0 2 + q 1 2 − q 2 2 − q 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 ) q 0 2 − q 1 2 + q 2 2 − q 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 ) q 0 2 − q 1 2 − q 2 2 + q 3 2 ] \bm{M}_{BN}= \begin{bmatrix} q_0^2+q_1^2-q_2^2-q_3^2 & 2\left(q_1q_2+q_0q_3\right) & 2\left(q_1q_3-q_0q_2\right)\\ 2\left(q_1q_2-q_0q_3\right) & q_0^2-q_1^2+q_2^2-q_3^2 & 2\left(q_2q_3+q_0q_1\right)\\ 2\left(q_1q_3+q_0q_2\right) & 2\left(q_2q_3-q_0q_1\right) & q_0^2-q_1^2-q_2^2+q_3^2 \end{bmatrix} MBN​= ​q02​+q12​−q22​−q32​2(q1​q2​−q0​q3​)2(q1​q3​+q0​q2​)​2(q1​q2​+q0​q3​)q02​−q12​+q22​−q32​2(q2​q3​−q0​q1​)​2(q1​q3​−q0​q2​)2(q2​q3​+q0​q1​)q02​−q12​−q22​+q32​​ ​

姿态微分方程

欧拉角的变化率$\dot{\varphi }$、$\dot{\theta }$、$\dot{\psi }$，机体坐标系相对于导航坐标系的转动角速度${\mathbf{\omega }}_B^{NB}=[pqr{]}^T$，两者并不等价。两者之间的关系可由欧拉运动方程表示
[ p q r ] = [ 1 0 − sin ⁡ θ 0 cos ⁡ ϕ sin ⁡ ϕ cos ⁡ θ 0 − sin ⁡ ϕ cos ⁡ ϕ cos ⁡ θ ] [ ϕ ˙ θ ˙ ψ ˙ ] \begin{bmatrix} p\\ q\\ r \end{bmatrix}= \begin{bmatrix} 1 & 0 & -\sin\theta\\ 0 & \cos\phi & \sin\phi\cos\theta\\ 0 & -\sin\phi & \cos\phi\cos\theta \end{bmatrix} \begin{bmatrix} \dot{\phi}\\ \dot{\theta}\\ \dot{\psi} \end{bmatrix} ​pqr​ ​= ​100​0cosϕ−sinϕ​−sinθsinϕcosθcosϕcosθ​ ​ ​ϕ˙​θ˙ψ˙​​ ​
根据欧拉动力学方程求出机体运动角速度，求解上述微分方程，可以得到相应欧拉角。但由于方程具有大量三角函数运算，一般不直接求解该微分方程，而是采用四元数的方法求解。

简单推导
[ p q r ] = [ ϕ ˙ 0 0 ] + M x [ 0 θ ˙ 0 ] + M x M y [ 0 0 ψ ˙ ] \begin{bmatrix} p\\ q\\ r \end{bmatrix}= \begin{bmatrix} \dot{\phi}\\ 0\\ 0 \end{bmatrix}+\bm{M}_x \begin{bmatrix} 0\\ \dot{\theta}\\ 0 \end{bmatrix}+\bm{M}_x\bm{M}_y \begin{bmatrix} 0\\ 0\\ \dot{\psi} \end{bmatrix} ​pqr​ ​= ​ϕ˙​00​ ​+Mx​ ​0θ˙0​ ​+Mx​My​ ​00ψ˙​​ ​
整理后可得到欧拉运动学方程。
取逆过程即可得到欧拉角的微分方程
[ ϕ ˙ θ ˙ ψ ˙ ] = [ 1 sin ⁡ ϕ tan ⁡ θ cos ⁡ ϕ tan ⁡ θ 0 cos ⁡ ϕ − sin ⁡ ϕ 0 sin ⁡ ϕ sec ⁡ θ cos ⁡ ϕ sec ⁡ θ ] [ p q r ] \begin{bmatrix} \dot{\phi}\\ \dot{\theta}\\ \dot{\psi} \end{bmatrix}= \begin{bmatrix} 1 & \sin\phi\tan\theta & \cos\phi\tan\theta\\ 0 & \cos\phi & -\sin\phi\\ 0 & \sin\phi\sec\theta & \cos\phi\sec\theta \end{bmatrix} \begin{bmatrix} p\\ q\\ r \end{bmatrix} ​ϕ˙​θ˙ψ˙​​ ​= ​100​sinϕtanθcosϕsinϕsecθ​cosϕtanθ−sinϕcosϕsecθ​ ​ ​pqr​ ​

四元数表示的姿态微分方程为
$${\dot{\mathbf{Q}}}_{NB}=\frac{1}{2}{\mathbf{Q}}_{NB}\circ {\mathbf{\omega }}_B^{NB}$$
展开可得
[ q 0 ˙ q 1 ˙ q 2 ˙ q 3 ˙ ] = 1 2 [ 0 − p − q − r p 0 r − q q − r 0 p r q p 0 ] [ q 0 q 1 q 2 q 3 ] \begin{bmatrix} \dot{q_0}\\ \dot{q_1}\\ \dot{q_2}\\ \dot{q_3} \end{bmatrix}=\frac{1}{2} \begin{bmatrix} 0 & -p & -q & -r\\ p & 0 & r & -q\\ q & -r & 0 & p\\ r & q & p & 0 \end{bmatrix} \begin{bmatrix} q_0\\ q_1\\ q_2\\ q_3 \end{bmatrix} ​q0​˙​q1​˙​q2​˙​q3​˙​​ ​=21​ ​0pqr​−p0−rq​−qr0p​−r−qp0​ ​ ​q0​q1​q2​q3​​ ​
根据动力学方程求出机体运动角速度，求解上述微分方程，可以得到${\mathbf{Q}}_{NB}$，再通过姿态四元数与欧拉角和方向余弦矩阵关系可转换为欧拉角和方向余弦矩阵。

简单推导过程
姿态四元数随时间的变化率定义为
$${\dot{\mathbf{Q}}}_{NB}=\underset{\delta t\to 0}{\lim }\frac{{\mathbf{Q}}_{NB}\left(t+\delta t\right)-{\mathbf{Q}}_{NB}\left(t\right)}{\delta t}=\underset{\delta t\to 0}{\lim }\frac{{\mathbf{Q}}_{NB}\left(t\right)\circ {\mathbf{Q}}_{NB}\left(\delta t\right)-{\mathbf{Q}}_{NB}\left(t\right)}{\delta t}={\mathbf{Q}}_{NB}\circ \underset{\delta t\to 0}{\lim }\frac{{\mathbf{Q}}_{NB}\left(\delta t\right)-\mathbf{Q}\left(1\right)}{\delta t}$$
其中，$\mathbf{Q}(1)=[1000{]}^T$，${\mathbf{Q}}_{NB}\left(\delta t\right)$为$t$时刻到$t+\delta t$时刻转动的姿态四元数，当$\delta t\to 0$时，转动的角度为小角度，可作小角度近似${\mathbf{Q}}_{NB}\left(\delta t\right)=[1\varphi /2\theta /2\psi /2{]}^T$，同时，利用机体坐标系相对于NED坐标系的转动角速度近似代替小角度欧拉角的微分，可得
$${\dot{\mathbf{Q}}}_{NB}={\mathbf{Q}}_{NB}\circ \underset{\delta t\to 0}{\lim }\frac{{\mathbf{Q}}_{NB}\left(\delta t\right)-\mathbf{Q}\left(1\right)}{\delta t}={\mathbf{Q}}_{NB}\circ \underset{\delta t\to 0}{\lim }\frac{{\left[1\frac{\varphi }{2}\frac{\theta }{2}\frac{\psi }{2}\right]}^T-{\left[1000\right]}^T}{\delta t}=\frac{1}{2}{\mathbf{Q}}_{NB}\circ {\left[0pqr\right]}^T$$

在不考虑地球自转和地球曲率的情况下（通常情况），可以认为${\mathbf{\omega }}_B^{NB}={\mathbf{\omega }}_B^{IB}$，直接采用陀螺仪测量数据进行计算即可。一般的陀螺仪精度不足以测量出地球转动的角速度，因此在导航计算中一般不考虑地球自转等影响。

四元数的归一化

姿态四元数为单位四元数，在微分方程求解过程中有可能因为数值误差使四元数模式偏离1，因此需要对四元数进行归一化处理。一般情况下，归一化可简单进行
$$\mathbf{Q}=\frac{\mathbf{Q}}{\Vert \mathbf{Q}\Vert }$$
引入反馈控制的思想，可令$\lambda =k(1-{\Vert \mathbf{Q}\Vert }^2)$，作为反馈补偿项加入到四元数微分方程的求解中，使姿态四元数在求解微分方程的过程中自动逐渐收敛到1。
[ q 0 ˙ q 1 ˙ q 2 ˙ q 3 ˙ ] = 1 2 [ λ − p − q − r p λ r − q q − r λ p r q p λ ] [ q 0 q 1 q 2 q 3 ] \begin{bmatrix} \dot{q_0}\\ \dot{q_1}\\ \dot{q_2}\\ \dot{q_3} \end{bmatrix}=\frac{1}{2} \begin{bmatrix} \lambda & -p & -q & -r\\ p & \lambda & r & -q\\ q & -r & \lambda & p\\ r & q & p & \lambda \end{bmatrix} \begin{bmatrix} q_0\\ q_1\\ q_2\\ q_3 \end{bmatrix} ​q0​˙​q1​˙​q2​˙​q3​˙​​ ​=21​ ​λpqr​−pλ−rq​−qrλp​−r−qpλ​ ​ ​q0​q1​q2​q3​​ ​