无人机刚体动力学方程

无人机平动动力学

推导

无人机相对地球（地心地固坐标系）表面的速度与惯性速度的关系可表示为
$${\mathbf{v}}^I={\mathbf{v}}^E+{\mathbf{\omega }}^{IE}\times \mathbf{r}\text{\hspace{1em}(1.1)}$$
${\mathbf{v}}^E$相对于机体系求导
$${\left(\frac{\mathrm{d}}{\mathrm{d}t}\right)}^B{\mathbf{v}}^E={\left(\frac{\mathrm{d}}{\mathrm{d}t}\right)}^I{\mathbf{v}}^E-{\mathbf{\omega }}^{IB}\times {\mathbf{v}}^E\text{\hspace{1em}(1.2)}$$
根据矢量在旋转坐标系下的求导规则，对式(1.1)两边在惯性坐标系下求导
$${\mathbf{a}}^I=\left[{\left(\frac{\mathrm{d}}{\mathrm{d}t}\right)}^I{\mathbf{v}}^E+{\mathbf{\omega }}^{IE}\times {\mathbf{v}}^E\right]+\left[{\mathbf{\alpha }}^{IE}\times \mathbf{r}+{\mathbf{\omega }}^{IE}\times \left({\mathbf{\omega }}^{IE}\times \mathbf{r}\right)\right]\text{\hspace{1em}(1.3)}$$
(1.3)式中，${\mathbf{a}}^I$为惯性加速度，有
$${\mathbf{a}}^I=\mathbf{f}+\mathbf{\mu }\text{\hspace{1em}(1.4)}$$
(1.4)式中，$\mathbf{f}$为无人机比力，$\mathbf{\mu }$为万有引力加速度。
地球万有引力加速度与地球重力加速度的关系为
$$\mathbf{g}=\mathbf{\mu }-\left[{\mathbf{\alpha }}^{IE}\times \mathbf{r}+{\mathbf{\omega }}^{IE}\times \left({\mathbf{\omega }}^{IE}\times \mathbf{r}\right)\right]\text{\hspace{1em}(1.5)}$$
即重力加速度是万有引力加速度去除了地球自转引起的加速度项的影响。
${\mathbf{\alpha }}^{IE}$为地球自转加速度，可认为地球为匀速转动，因此此项为0。将(1.3)(1.4)带入(1.2)中整理可得
$${\left(\frac{\mathrm{d}}{\mathrm{d}t}\right)}^B{\mathbf{v}}^E=\mathbf{f}+\mathbf{\mu }-{\mathbf{\omega }}^{IE}\times {\mathbf{v}}^E-\left[{\mathbf{\alpha }}^{IE}\times \mathbf{r}+{\mathbf{\omega }}^{IE}\times \left({\mathbf{\omega }}^{IE}\times \mathbf{r}\right)\right]-{\mathbf{\omega }}^{IB}\times {\mathbf{v}}^E\text{\hspace{1em}(1.6)}$$
将式(1.5)带入式(1.6)可得
$${\left(\frac{\mathrm{d}}{\mathrm{d}t}\right)}^B{\mathbf{v}}^E=\mathbf{f}+\mathbf{g}-{\mathbf{\omega }}^{IE}\times {\mathbf{v}}^E-{\mathbf{\omega }}^{IB}\times {\mathbf{v}}^E\text{\hspace{1em}(1.7)}$$
式(1.7)在机体系下的投影可表示为
$${\mathbf{a}}_B^{EB}={\mathbf{f}}_B+{\mathbf{g}}_B-{\mathbf{\omega }}_B^{IE}\times {\mathbf{v}}_B^E-{\mathbf{\omega }}_B^{IB}\times {\mathbf{v}}_B^E\text{\hspace{1em}(1.8)}$$

应用

求解式(1.8)微分方程，可以得到无人机的速度在机体系下的表示。
${\mathbf{f}}_B+{\mathbf{g}}_B$为无人机动力、气动力、重力等合外力产生的加速度，合起来可以表示为
$${\mathbf{f}}_B+{\mathbf{g}}_B=\frac{{\mathbf{F}}_B}{m}+{\mathbf{M}}_{BN}\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]\text{\hspace{1em}(1.9)}$$
${\mathbf{\omega }}_B^{IE}$为地球转动角速度在机体系下的投影，可以表示为
$${\mathbf{\omega }}_B^{IE}={\mathbf{M}}_{BN}{\mathbf{M}}_{NE}\left[\begin{array}{c}0\\ 0\\ {\omega }^{IE}\end{array}\right]={\mathbf{M}}_{BN}\left[\begin{array}{ccc}-\sin \mu \cos \lambda & -\sin \mu \sin \lambda & -\cos \mu \\ -\sin \lambda & \cos \lambda & 0\\ -\cos \mu \cos \lambda & -\cos \mu \sin \lambda & -\sin \mu \end{array}\right]\left[\begin{array}{c}0\\ 0\\ {\omega }^{IE}\end{array}\right]\text{\hspace{1em}(1.10)}$$
${\mathbf{\omega }}_B^{IB}$为无人机转动角速度在机体系下的投影，即陀螺仪的测量值。
带入式(1.8)可得
$${\dot{\mathbf{v}}}_B^E=\frac{{\mathbf{F}}_B}{m}+{\mathbf{M}}_{BN}\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]-{\mathbf{M}}_{BN}\left[\begin{array}{ccc}-\sin \mu \cos \lambda & -\sin \mu \sin \lambda & -\cos \mu \\ -\sin \lambda & \cos \lambda & 0\\ -\cos \mu \cos \lambda & -\cos \mu \sin \lambda & -\sin \mu \end{array}\right]\left[\begin{array}{c}0\\ 0\\ {\omega }^{IE}\end{array}\right]\times {\mathbf{v}}_B^E-{\mathbf{\omega }}_B^{IB}\times {\mathbf{v}}_B^E\text{\hspace{1em}(1.11)}$$
根据当前姿态转换矩阵、经纬度、陀螺仪测量角速度、机体系合外力、当前重力加速度可以求解微分方程(1.11)，得到无人机速度在机体系下的分量。经过姿态转换矩阵，可得到无人机在NED坐标系下的速度。

无人机转动动力学

无人机刚体绕质心转动的过程可以由欧拉动力学方程描述
$${\dot{\mathbf{\omega }}}^{IB}={\mathbf{I}}^{-1}\left[\mathbf{M}-{\mathbf{\omega }}^{IB}\times \left(\mathbf{I}{\mathbf{\omega }}^{IB}\right)\right]\text{\hspace{1em}(2.1)}$$
其中，${\mathbf{\omega }}^{IB}$为机体系相对惯性系转动的角速度，可由陀螺仪测量出。$\mathbf{I}$为原点在刚体质心的惯量矩阵，可表示为
$$\mathbf{I}=\left[\begin{array}{ccc}{I}_{xx}& -I_{xy}& -I_{xz}\\ -I_{yx}& I_{yy}& -I_{yz}\\ -I_{zx}& -I_{zy}& I_{zz}\end{array}\right]\text{\hspace{1em}(2.2)}$$
对于结构上关于x-z、y-z平面对称的多旋翼，惯量矩阵可以简化为对角阵，即 I = diag { I x x   I y y   I z z } \bm{I}=\textup{diag}\{I_{xx}\:I_{yy}\:I_{zz}\} I=diag{Ixx​Iyy​Izz​}。
(2.1)式在机体系下的投影表达式为
$${\dot{\mathbf{\omega }}}_B^{IB}={\mathbf{I}}^{-1}\left[{\mathbf{M}}_B-{\mathbf{\omega }}_B^{IB}\times \left(\mathbf{I}{\mathbf{\omega }}_B^{IB}\right)\right]\text{\hspace{1em}(2.3)}$$
根据当前的合外力矩、无人机惯量矩阵求解微分方程(2.3)，可以得到无人机相对惯性系转动角速度在机体系下的分量。
得出${\mathbf{\omega }}^{IB}$后，可容易得出机体系相对于NED坐标系的转动角速度：
$${\mathbf{\omega }}_B^{NB}={\mathbf{\omega }}_B^{IB}-{\mathbf{\omega }}_B^{IE}-{\mathbf{\omega }}_B^{EN}\text{\hspace{1em}(2.4)}$$
其中，${\mathbf{\omega }}_B^{EN}$可以表示为
$${\mathbf{\omega }}_B^{EN}={\mathbf{M}}_{BN}\left[\begin{array}{c}-\frac{u}{R_N+h}\\ \frac{v}{R_E+h}\\ -\frac{v\tan \mu }{R_E+h}\end{array}\right]$$