前两期文章讲完了气动力和气动力矩的模型，本期将来讲解一下六自由度部分的动力学与运动学方程。

1、六自由度动力学方程

三维空间中的运动可以分为平动（线运动）和转动（角运动）。平动：前后、左右、上下；转动：滚转、俯仰、偏航。
线运动动力学方程可以表示如下：
$$\sum \mathbf{F}=\frac{\mathrm{d}}{\mathrm{d}t}(m\mathbf{V})$$
角运动动力学方程可以表示如下：
$$\sum \mathbf{M}=\frac{\mathrm{d}}{\mathrm{d}t}(\mathbf{L})$$
式中，$\mathbf{F}$、$\mathbf{M}$为飞行器所收到的合外力、合外力距。
在动坐标系(机体坐标系)中将地面坐标系中得到的机体速度$\mathbf{V}$及动量矩$\mathbf{L}$向机体轴分解：
$$\begin{array}{rl}\frac{\mathrm{d}\mathbf{V}}{\mathrm{d}t}& ={\mathbf{e}}_{\mathbf{V}}\frac{{\delta }_{\mathbf{V}}}{{\delta }_t}+\mathbf{\Omega }\times \mathbf{V}\\ \frac{\mathrm{d}\mathbf{L}}{\mathrm{d}t}& ={\mathbf{e}}_{\mathbf{L}}\frac{{\delta }_{\mathbf{L}}}{{\delta }_t}+\mathbf{\Omega }\times \mathbf{L}\end{array}$$
${\mathbf{e}}_{\mathbf{V}},{\mathbf{e}}_{\mathbf{L}}$为沿飞行速度矢量$\mathbf{V}$和动量矩$\mathbf{L}$的单位向量；
$\frac{{\delta }_{\mathbf{V}}}{{\delta }_t},\frac{{\delta }_{\mathbf{L}}}{{\delta }_t}$为动坐标系内的相对导数；
$\mathbf{\Omega }$为动坐标系相对静坐标系的总角速度向量。
将$\mathbf{\Omega }$,$\mathbf{V}$,$\mathbf{L}$在动坐标系(机体系)中分解:
$$\begin{array}{rl}\mathbf{V}& =\mathbf{i}u+\mathbf{j}v+\mathbf{k}w\\ \mathbf{\Omega }& =\mathbf{i}p+\mathbf{j}q+\mathbf{k}r\\ \mathbf{L}& =\mathbf{i}\left(p{I}_x-r{I}_{xz}\right)+\mathbf{j}\left(q{I}_y\right)+\mathbf{k}\left(r{I}_z-p{I}_{xz}\right)\end{array}$$
据此给出下面六自由度方程组(详细推导过程可参考《飞行控制系统》)：

• 力方程组(Force Equations)
  $$\{\begin{array}{l}\dot{u}=vr-wq-g\sin \theta +\frac{F_x}{m}\\ \dot{v}=-ur+wp+g\cos \theta \sin \varphi +\frac{F_y}{m}\\ \dot{w}=uq-vp+g\cos \theta \cos \varphi +\frac{F_z}{m}\end{array}$$
  式中，$F_x,F_y,F_z$为机体系下气动力和发动力所产生的力的合力；$g$为重力加速度$u,v,w$为机体系下三轴速度矢量；$\dot{u},\dot{v},\dot{w}$为机体系下三轴加速度矢量；$p,q,r$为机体系下三轴角速度。
• 运动学方程组(Kinematic Equations)
  $$\{\begin{array}{l}\dot{\varphi }=p+(r\text{cos }\varphi +q\text{sin }\varphi )\tan \theta \\ \dot{\theta }=q\text{cos }\varphi -r\text{sin }\varphi \\ \dot{\psi }=\frac{1}{\cos \theta }(r\cos \varphi +q\text{sin }\varphi )\end{array}$$
  式中，$\varphi ,\theta ,\psi$为姿态欧拉角；$\dot{\varphi },\dot{\theta },\dot{\psi }$为姿态角变化率。这里需要注意$\dot{\varphi },\dot{\theta },\dot{\psi }$与$p,q,r$的区别，$\dot{\varphi },\dot{\theta },\dot{\psi }$是在北东地地面坐标系下角度变化率，$p,q,r$是在机体系下的机体角速度；两者之间互相转化需要乘以对应的旋转矩阵，转换关系如下：
  $$\{\begin{array}{ll}p=\dot{\varphi }-\dot{\varphi }\sin \theta & \\ q=\dot{\theta }\cos \varphi +\dot{\psi }\cos \theta \sin \varphi & \\ r=-\dot{\theta }\sin \varphi +\dot{\psi }\cos \theta \cos \varphi & \end{array}$$
  $$\{\begin{array}{l}\dot{\varphi }=p+(r\cos \varphi +q\sin \varphi )\tan \theta \\ \dot{\theta }=q\cos \varphi -r\sin \varphi \\ \dot{\psi }=\frac{1}{\cos \theta }(r\cos \varphi +q\sin \varphi )\end{array}$$
• 力矩方程组(Moment Equations)
  $$\{\begin{array}{l}\dot{p}=(c_{1}r+c_{2}p)q+c_{3}L+c_{4}N\\ \dot{q}=c_{5}pr-c_6(p^2-r^2)+c_{7}M\\ \dot{r}=(c_{8}p-c_{2}r)q+c_{4}L+c_{9}N\end{array}$$
  式中：$L,M,N$为机体系下受到的合外力矩；$\dot{p},\dot{q},\dot{r}$为机体角速度变化率；$c_1-c_9$表达如下：
  $$\begin{array}{rl}& c_1=\frac{\left(I_y-I_z\right)I_z-I_{xz}^2}{\Sigma },c_2=\frac{\left(I_x-I_y+I_z\right)I_{xz}}{\Sigma },c_3=\frac{I_z}{\Sigma }\\ & c_4=\frac{I_{xx}}{\sum },c_5=\frac{I_z-I_x}{I_y},c_6=\frac{I_{xx}}{I_y},c_7=\frac{1}{I_y}\\ & c_8=\frac{I_x\left(I_x-I_y\right)+I_{xx}^2}{\sum },c_9=\frac{I_x}{\sum },\Sigma =I_x{I}_x-I_{xx}^2\end{array}$$
• 导航方程组(Navigation Equations)
  $$\{\begin{array}{l}{\dot{x}}_g=u\cos \theta \cos \psi +v(\sin \varphi \sin \theta \cos \psi -\cos \varphi \sin \psi )+w(\sin \varphi \sin \psi +\cos \varphi \sin \theta \cos \psi )\\ {\dot{y}}_g=u\cos \theta \sin \psi +v(\sin \varphi \sin \theta \sin \psi +\cos \varphi \cos \psi )+w(-\sin \varphi \cos \psi +\cos \varphi \sin \theta \sin \psi )\\ \dot{h}=u\sin \theta -v\sin \varphi \cos \theta -w\cos \varphi \cos \theta \end{array}$$
  式中，$\dot{x_g},\dot{y_g},\dot{h}$为位置变化率，

2、Simulink 6DOF

下面介绍一下Simulink中的六自由度动力学模块。

六自由度动力学模块以刚体质心为参考点，输入量为作用在质心的合外力与和外力矩。输出为北东地速度$v_n,v_e,v_d$；北东地位置$x_e,y_e,z_e$；姿态欧拉角$\varphi ,\theta ,\psi$；北东地坐标系到机体系的旋转矩阵$DC{M}_{be}$；机体系下速度$u,v,w$；机体角速度$p,q,r$；机体角加速度$\dot{p},\dot{q},\dot{r}$；机体系下加速度$\dot{u},\dot{v},\dot{w}$。

按住ctrl+u可以进入查看封装模块内部构造。

内部构造可以大致按照第一节中讲述的几大方程组进行划分。

• 力方程组计算出$\dot{u},\dot{v},\dot{w}$，之后通过积分获得$u,v,w$；
• 力矩方程组计算出$\dot{p},\dot{q},\dot{r}$，之后通过积分获得$p,q,r$；
• 运动学方程输入$p,q,r$后进行姿态解算，获得欧拉角和旋转矩阵；
  
• 导航部分先使用旋转矩阵将$u,v,w$转换得到$v_n,v_e,v_d$，对$v_n,v_e,v_d$积分得到$x_e,y_e,z_e$

3、小结

综上，我们完成了六自由度动力学部分的介绍。在实际工程中，只要定义好合外力与合外力矩的输入，就可以使用Simulink中现有的的6 DOF模块进行状态求解。
下一期将会带来固定翼飞行器在Simulink中的建模、配平及线性化的讲解。
技术交流查看评论区。
END

迅翼SwiftWing致力于固定翼技术共享，汇聚固定翼领域技术极客，推动固定翼技术持续创新！