CN102945002A - 基于非线性数学模型的通用型无人机仿真方法及系统 - Google Patents

Abstract

本发明公开了一种基于非线性数学模型的通用型无人机非线性模型构建方法，同时涉及一种用于实现该构建方法的系统。所述的方法主要包括模型解算、双坐标系支持和CMEX封装三个方法模块，所述的系统包括仿真系统输入模块、进行无人机运动状态初始化的初始化模块、状态量导数解算模块、状态量更新模块、辅助物理量计算模块和仿真系统输出模块。本发明的方法及系统把更多的工作交给计算机，只需用户输入外力和外力矩，即可得到完备的无人机的物理量，使用简单、方便。

Description

基于非线性数学模型的通用型无人机仿真方法及系统

技术领域

本发明涉及一种无人机数学模型的构建方法，尤其涉及一种通用的无人机非线性模型构建方法，同时涉及一种用于实现该构建方法的系统。

背景技术

在无人机飞行控制律设计和验证的过程中，六自由度非线性数学模型的建立是前提和基础。一般无人机的数学模型建立的过程中，首先选择适当的坐标系；其次根据无人机当前迎角和侧滑角从吹风数据中获得气动导数，计算气动力和力矩，结合发动机模型和重力场模型，求得无人机合外力与合外力矩；然后在此基础上根据六自由度动力学和运动学方程，求取无人机状态量导数；接着通过数值积分解算，更新无人机的状态；最终利用新的状态计算所需要的其他物理量。这种建模方法存在三个方面的不足：

第一，当对不同型号无人机构建其非线性数学模型时，由于气动外形的差异，导致气动力和力矩的求取不同，它们的建模需都要重复上述过程，过程复杂，建模效率低，周期较长；

第二，针对苏联坐标系和欧美坐标系这两种不同的坐标系定义方式，原有的建模方法需要分别建模，无法同时支持两种坐标系；

第三，通常无人机控制律在MATLAB环境完成设计，而半物理仿真程序则是用C语言开发，二者都依赖于无人机非线性数学模型，需要用M语言和C语言分别编程实现无人机非线性模型，加大了建模的工作量。

发明内容

技术问题

本发明要解决的技术问题是提供一种不受无人机型号限制、进行通用数学模型构建的方法，该方法可用于不同型号的无人机在不同坐标系和不同开发环境下的建模，同时亦提供一种用于实现该方法的通用型无人机非线性数学模型构建系统。

技术方案

为了解决上述的技术问题，本发明的通用型无人机非线性数学模型构建方法包括下列步骤：

步骤一：调用仿真系统通用模型软件提供的用户接口函数，初始化无人机的运动状态；

步骤二：设置无人机的质量和转动惯量，并向仿真系统输入无人机所受不含重力的外力和外力矩；

步骤三：根据面对称刚体六自由度运动模型计算无人机状态量导数，以平面大地为惯性系，在苏联坐标系下进行解算：

线动力学方程：

$\left\{\begin{array}{c}{\stackrel{.}{V}}_x=V_z{\mathrm{\ω}}_y-V_y{\mathrm{\ω}}_z+F_x/m+G_x\\ {\stackrel{.}{V}}_y=V_x{\mathrm{\ω}}_z{-V}_z{\mathrm{\ω}}_x+F_y/m{G}_y\\ {\stackrel{.}{V}}_x=V_y{\mathrm{\ω}}_x-V_x{\mathrm{\ω}}_y+F_z/m+G_z\end{array}\right.---\left(1\right)$

角动力学方程：

$\left\{\begin{array}{c}{\stackrel{.}{\mathrm{\ω}}}_x=\frac{I_{\mathrm{yy}}{M}_x+I_{\mathrm{xy}}{M}_y-I_{\mathrm{xy}}(I_{\mathrm{xx}}-I_{\mathrm{yy}}+I_{\mathrm{zz}}){\mathrm{\ω}}_x{\mathrm{\ω}}_z+(I_{\mathrm{yy}}{I}_{\mathrm{yy}}+I_{\mathrm{xy}}{I}_{\mathrm{xy}}-I_{\mathrm{yy}}{I}_{\mathrm{zz}}){\mathrm{\ω}}_y{\mathrm{\ω}}_z}{I_{\mathrm{xx}}{I}_{\mathrm{yy}}-I_{\mathrm{xy}}{I}_{\mathrm{xy}}}\\ {\stackrel{.}{\mathrm{\ω}}}_y=\frac{I_{\mathrm{xx}}{M}_y+I_{\mathrm{xy}}{M}_x+I_{\mathrm{xy}}(I_{\mathrm{xx}}-I_{\mathrm{yy}}+I_{\mathrm{zz}}){\mathrm{\ω}}_y{\mathrm{\ω}}_z-(I_{\mathrm{xx}}{I}_{\mathrm{xx}}+I_{\mathrm{xy}}{I}_{\mathrm{xy}}-I_{\mathrm{xx}}{I}_{\mathrm{zz}}){\mathrm{\ω}}_x{\mathrm{\ω}}_z}{I_{\mathrm{xx}}{I}_{\mathrm{yy}}-I_{\mathrm{xy}}{I}_{\mathrm{xy}}}\\ {\stackrel{.}{\mathrm{\ω}}}_z=\frac{M_z-(I_{\mathrm{yy}}-I_{\mathrm{xx}}){\mathrm{\ω}}_x{\mathrm{\ω}}_y+I_{\mathrm{xy}}({\mathrm{\ω}}_x{\mathrm{\ω}}_x-{\mathrm{\ω}}_y{\mathrm{\ω}}_y)}{I_{\mathrm{zz}}}\end{array}\right.---\left(2\right)$

线运动学方程：

$\left\{\begin{array}{c}{\stackrel{.}{x}}_g=V_x\cos \mathrm{\ψ}\cos \mathrm{\υ}+V_y(\sin \mathrm{\ψ}\sin \mathrm{\γ}-\cos \mathrm{\ψ}\sin \mathrm{\υ}\cos \mathrm{\γ})+V_z(\sin \mathrm{\ψ}\cos \mathrm{\γ}+\cos \mathrm{\ψ}\sin \mathrm{\υ}\sin \mathrm{\γ})\\ {\stackrel{.}{h}}_g=V_x\sin \mathrm{\υ}+V_y\cos \mathrm{\υ}\cos \mathrm{\γ}-V_z\cos \mathrm{\υ}\sin \mathrm{\γ}\\ {\stackrel{.}{z}}_g=-V_x\sin \mathrm{\ψ}\cos \mathrm{\υ}+V_y(\cos \mathrm{\ψ}\sin \mathrm{\γ}+\sin \mathrm{\ψ}\sin \mathrm{\υ}\sin )+V_z(\cos \mathrm{\ψ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\υ}\sin \mathrm{\γ})\end{array}\right.---\left(3\right)$

角运动学方程：

$\left\{\begin{array}{c}\stackrel{.}{\mathrm{\υ}}={\mathrm{\ω}}_y\sin \mathrm{\γ}+{\mathrm{\ω}}_z\cos \mathrm{\γ}\\ \stackrel{.}{\mathrm{\γ}}={\mathrm{\ω}}_x-\tan \mathrm{\υ}({\mathrm{\ω}}_y\cos \mathrm{\γ}-{\mathrm{\ω}}_z\sin \mathrm{\γ})\\ \stackrel{.}{\mathrm{\ψ}}=({\mathrm{\ω}}_y\cos \mathrm{\γ}-{\mathrm{\ω}}_z\sin \mathrm{\γ})/\cos \mathrm{\υ}\end{array}\right.---\left(4\right)$

其中，

F_x、F_y、F_z为外力(不含重力)沿机体轴的分量；

M_x、M_y、M_z为外力矩沿机体轴的分量；

G_x、G_y、G_z为重力加速度沿机体轴的分量；

m为无人机质量；

I_xx、I_yy、I_zz、I_xy、分别是无人机沿机体轴向的转动惯量和惯性积；

V_x、V_y、V_z为地速沿机体轴的分量；

x_g、h_g、z_g为分别是无人机北向、天向和东向的位移；

ω_x、ω_y、ω_z为绕机体轴转动的角速率沿机体轴的分量；

υ、γ、ψ为俯仰角、滚转角、偏航角；

步骤四：使用Adams数值积分算法更新无人机状态量，

$y_{n+4}=y_{n+3}+\frac{h}{24}(55{\stackrel{.}{y}}_{n+3}-59{\stackrel{.}{y}}_{n+2}+37{\stackrel{.}{y}}_{n+1}-9{\stackrel{.}{y}}_n)---\left(5\right)$

截断误差为

其中：

h表示仿真步长；

分别表示第n+3、n+2、n+1、n时刻的导数；

y_n+4、y_n+3分别表示n+4、n+3时刻的值；

步骤五：模型中内置风模型，根据用户设置的风速，合成空速，：

$\left\{\begin{array}{c}{V}_{e2\mathrm{wind}}=V_e-V_{\mathrm{wind}\_e}\\ V_{n2\mathrm{wind}}=V_n-V_{\mathrm{wind}\_n}\\ V_{u2\mathrm{wind}}=V_u-V_{\mathrm{wind}\_u}\end{array}---\left(6\right)\right.$

其中：

V_e、V_n、V_u分别是无人机地速在东向、北向、天向的分量；

V_{wind_e}、V_{wind_n}、V_{wind_u}分别是气流的东向速度、北向速度、天向速度；

V_e2wind、V_n2wind、V_u2wind分别是空速在东向、北向、天向的分量；

本步骤中，用户可以随时设定气流速度V_{wind_e}、V_{wind_n}、V_{wind_u},从而实现风的动态加入；

步骤六：根据步骤四得到的无人机状态量和步骤五得到的空速，在标准大气模型、重力场模型的基础上，计算空速、气流角、动压等辅助量，支持模型加风；

1962年美国标准大气：

ρ查1962年标准大气表                     (7)

V_sonic查1962年标准大气表                 (8)

$\mathrm{\σ}=\frac{\mathrm{\ρ}}{{\mathrm{\ρ}}_{\mathrm{sea}\_\mathrm{level}}}---\left(9\right)$

H_e＝3.2808×H                            (10)

$T_s=0.55556\&times;\left\{\begin{array}{cc}518.7\&times;(1.0-6.875e-6\&times;H);& 0<H_e<36089\\ 518.7\&times;0.751985;& 36089<H_e<240000\end{array}---\left(11\right)\right.$

$P_s=47.88018\&times;\left\{\begin{array}{cc}2113.8\&times;{(1.0-6.875e\&times;H)}^{5.256};& 0<H<36089\\ 2113.8\&times;0.2234\&times;e^{-4.806e-5\&times;(H-36089)};& 36089<H<240000\end{array}---\left(12\right)\right.$

其中：

H、H_e分别是海拔高度的公制和英制表示；

ρ表示当前高度处的大气密度；

V_sonic表示当前高度处的音速；

σ表示当前高度和海平面的大气密度之比；

T_s表示当前高度处大气的静温；

P_s表示当前高度处的大气的静压。

重力场模型：

$\left\{\begin{array}{c}{K}_R=0.00108263*{\left(\frac{H+R_e}{R_e}\right)}^2\\ K_g=\sqrt{2.25({5\sin }^4\left(D\right)-{2\sin }^2\left(D\right)+1)K_R^2+3({-3\sin }^2\left(D\right)+1)K_R+1}\\ g=K_g\frac{\mathrm{GM}}{{(H+R_e)}^2}\end{array}\right.---\left(13\right)$

其中：

H表示海拔高度；

R_e表示地球半径；

D表示纬度；

G表示引力常数；

M表示地球质量；

本步骤中的其他辅助量(指示空速、真空速、动压、压差、气流角、气流角变化率和无人机相对于跑道的坐标)可根据其定义和物理机理进行计算；

步骤七：调用通用模型软件用户接口函数获得完备的描述无人机运动的物理量；

步骤八：重复步骤二至步骤七，实现无人机运动的非线性仿真。

更进一步地，本发明的仿真方法可实现一个模型同时支持两种坐标系下的仿真。本发明技术方案中提到的苏联坐标系和欧美坐标系实质上是一个事实的两种不同的描述，因此可以把欧美坐标系的输入转化为苏联坐标系的输入，在苏联坐标系进行模型解算后，把苏联坐标系的输出再转化为欧美坐标系的输出，实现一个模型对两种坐标系的支持。

更进一步地，本发明的仿真方法可同时支持C语言环境和MATLAB环境下的仿真。本发明在VC++6.0下用ANSI C编程实现并封装成静态库，提供用户接口函数；在此基础上进行CMEX封装，生成可在MATLAB环境运行的函数，可用于C开发环境和MATLAB开发环境下无人机的建模，支持多平台运行的要求。

上述步骤中所涉及的接口函数可分为八类：①基本操作函数(设置无人机所受外力和外力矩、设置无人机惯性量、触发模型解算)；②设置刚体基本状态量函数；③获取刚体基本状态量函数；④获取刚体基本状态量导数函数；⑤相对气流功能函数；⑥坐标系之间的相互转化函数；⑦跑到相关功能函数；⑧内部核心算法函数(实现刚体六自由度模型、大气模型、重力场模型、状态量更新和辅助物理量的计算)。接口函数的合理设计和分类使得软件使用更易于使用。

本发明提供的用实现权利要求1所述方法的系统，包括：

仿真系统输入模块；

进行无人机运动状态初始化的初始化模块；

状态量导数解算模块；

状态量更新模块；

辅助物理量计算模块；

仿真系统输出模块；

其中，无人机状态参数通过仿真系统输入模块输入初始化模块，初始化模块对无人机初始状态信息进行处理并将处理结果输入初始化状态量更新模块；无人机所受不含重力的外力和外力矩通过仿真系统输入模块输入状态量导数解算模块，状态量导数解算模块解算得到状态量导数，并输出至状态量更新模块；状态量更新模块更新运动状态，将新的值输出至辅助物理量计算模块；状态量导数解算模块、状态量更新模块、辅助物理量计算模块的输出结果通过仿真系统输出模块提供给用户使用。

有益效果

本发明的方法在软件中编写了复杂的模型解算程序，把更多的工作交给计算机，只需用户输入外力和外力矩，即可得到完备的无人机的物理量，使用简单、方便。由于通用建模方法的对象为面对称刚体，适用于所有无人机非线性数学模型在不同坐标系和不同开发环境下的构建，该方法使得设计开发人员只要专注于气动力和力矩的解算，系统搭建简单易行，简化了无人机的建模过程，缩短建模周期，提高了效率。

附图说明

图1是非线性模型解算流程图；

图2是风的动态加入原理图；

图3是接口函数分类示意图；

图4是双坐标系支持原理图；

图5是CMEX封装示意图；

图6仿真系统原理图；

图7仿真系统输入模块原理图；

图8仿真系统输出模块原理图。

具体实施方式

实施例一：

如图1-图5所示，本实施例涉及一种基于非线性数学模型的通用型无人机仿真方法，主要包括模型解算、双坐标系支持和CMEX封装三个方法模块，具体步骤如下：

步骤一：调用通用模型软件提供的用户接口函数，初始化无人机的运动状态；

本实施例设计了两种初始化的方式：初始化地速在机体坐标系的分量和初始化地速在地理坐标系的分量。允许用户选择其中的一种进行初始化，通过模型中提供的机体坐标系和地理坐标系之间的转化矩阵，软件程序自动完成另一种速度的初始化。

步骤二：设置无人机的质量和转动惯量，输入无人机所受外力(不含重力)和外力矩；

步骤三：根据面对称刚体六自由度运动模型计算无人机状态量导数，认为平面大地是惯性系，在苏联坐标系下解算：

线动力学方程：

$\left\{\begin{array}{c}{\stackrel{.}{V}}_x=V_z{\mathrm{\&omega;}}_y-V_y{\mathrm{\&omega;}}_z+F_x/m+G_x\\ {\stackrel{.}{V}}_y=V_x{\mathrm{\&omega;}}_z{-V}_z{\mathrm{\&omega;}}_x+F_y/m{G}_y\\ {\stackrel{.}{V}}_x=V_y{\mathrm{\&omega;}}_x-V_x{\mathrm{\&omega;}}_y+F_z/m+G_z\end{array}\right.---\left(1\right)$

角动力学方程：

$\left\{\begin{array}{c}{\stackrel{.}{\mathrm{\&omega;}}}_x=\frac{I_{\mathrm{yy}}{M}_x+I_{\mathrm{xy}}{M}_y-I_{\mathrm{xy}}(I_{\mathrm{xx}}-I_{\mathrm{yy}}+I_{\mathrm{zz}}){\mathrm{\&omega;}}_x{\mathrm{\&omega;}}_z+(I_{\mathrm{yy}}{I}_{\mathrm{yy}}+I_{\mathrm{xy}}{I}_{\mathrm{xy}}-I_{\mathrm{yy}}{I}_{\mathrm{zz}}){\mathrm{\&omega;}}_y{\mathrm{\&omega;}}_z}{I_{\mathrm{xx}}{I}_{\mathrm{yy}}-I_{\mathrm{xy}}{I}_{\mathrm{xy}}}\\ {\stackrel{.}{\mathrm{\&omega;}}}_y=\frac{I_{\mathrm{xx}}{M}_y+I_{\mathrm{xy}}{M}_x+I_{\mathrm{xy}}(I_{\mathrm{xx}}-I_{\mathrm{yy}}+I_{\mathrm{zz}}){\mathrm{\&omega;}}_y{\mathrm{\&omega;}}_z-(I_{\mathrm{xx}}{I}_{\mathrm{xx}}+I_{\mathrm{xy}}{I}_{\mathrm{xy}}-I_{\mathrm{xx}}{I}_{\mathrm{zz}}){\mathrm{\&omega;}}_x{\mathrm{\&omega;}}_z}{I_{\mathrm{xx}}{I}_{\mathrm{yy}}-I_{\mathrm{xy}}{I}_{\mathrm{xy}}}\\ {\stackrel{.}{\mathrm{\&omega;}}}_z=\frac{M_z-(I_{\mathrm{yy}}-I_{\mathrm{xx}}){\mathrm{\&omega;}}_x{\mathrm{\&omega;}}_y+I_{\mathrm{xy}}({\mathrm{\&omega;}}_x{\mathrm{\&omega;}}_x-{\mathrm{\&omega;}}_y{\mathrm{\&omega;}}_y)}{I_{\mathrm{zz}}}\end{array}\right.---\left(2\right)$

线运动学方程：

$\left\{\begin{array}{c}{\stackrel{.}{x}}_g=V_x\cos \mathrm{\&psi;}\cos \mathrm{\&upsi;}+V_y(\sin \mathrm{\&psi;}\sin \mathrm{\&gamma;}-\cos \mathrm{\&psi;}\sin \mathrm{\&upsi;}\cos \mathrm{\&gamma;})+V_z(\sin \mathrm{\&psi;}\cos \mathrm{\&gamma;}+\cos \mathrm{\&psi;}\sin \mathrm{\&upsi;}\sin \mathrm{\&gamma;})\\ {\stackrel{.}{h}}_g=V_x\sin \mathrm{\&upsi;}+V_y\cos \mathrm{\&upsi;}\cos \mathrm{\&gamma;}-V_z\cos \mathrm{\&upsi;}\sin \mathrm{\&gamma;}\\ {\stackrel{.}{z}}_g=-V_x\sin \mathrm{\&psi;}\cos \mathrm{\&upsi;}+V_y(\cos \mathrm{\&psi;}\sin \mathrm{\&gamma;}+\sin \mathrm{\&psi;}\sin \mathrm{\&upsi;}\sin )+V_z(\cos \mathrm{\&psi;}\cos \mathrm{\&gamma;}-\sin \mathrm{\&psi;}\sin \mathrm{\&upsi;}\sin \mathrm{\&gamma;})\end{array}\right.---\left(3\right)$

角运动学方程：

$\left\{\begin{array}{c}\stackrel{.}{\mathrm{\&upsi;}}={\mathrm{\&omega;}}_y\sin \mathrm{\&gamma;}+{\mathrm{\&omega;}}_z\cos \mathrm{\&gamma;}\\ \stackrel{.}{\mathrm{\&gamma;}}={\mathrm{\&omega;}}_x-\tan \mathrm{\&upsi;}({\mathrm{\&omega;}}_y\cos \mathrm{\&gamma;}-{\mathrm{\&omega;}}_z\sin \mathrm{\&gamma;})\\ \stackrel{.}{\mathrm{\&psi;}}=({\mathrm{\&omega;}}_y\cos \mathrm{\&gamma;}-{\mathrm{\&omega;}}_z\sin \mathrm{\&gamma;})/\cos \mathrm{\&upsi;}\end{array}\right.---\left(4\right)$

其中：

F_x、F_y、F_z为外力(不含重力)沿机体轴的分量；

M_x、M_y、M_z为外力矩沿机体轴的分量；

G_x、G_y、G_z为重力加速度沿机体轴的分量；

m为无人机质量；

I_xx、I_yy、I_zz、I_xy、分别是无人机沿机体轴向的转动惯量和惯性积；

V_x、V_y、V_z为地速沿机体轴的分量；

x_g、h_g、z_g为分别是无人机北向、天向和东向的位移；

ω_x、ω_y、ω_z为绕机体轴转动的角速率沿机体轴的分量；

υ、γ、ψ分别是俯仰角、滚转角、偏航角。

步骤四：采用Adams显示数值积分算法更新无人机状态量：

$y_{n+4}=y_{n+3}+\frac{h}{24}(55{\stackrel{.}{y}}_{n+3}-59{\stackrel{.}{y}}_{n+2}+37{\stackrel{.}{y}}_{n+1}-9{\stackrel{.}{y}}_n)---\left(5\right)$

截断误差为

其中：

h表示仿真步长；

分别表示第n+3、n+2、n+1、n时刻的导数；

y_n+4、y_n+3分别表示n+4、n+3时刻的值。

步骤五：内置风模型，允许用户设置风速；

参照图2，根据用户设置的风速，合成空速，：

$\left\{\begin{array}{c}{V}_{e2\mathrm{wind}}=V_e-V_{\mathrm{wind}\_e}\\ V_{n2\mathrm{wind}}=V_n-V_{\mathrm{wind}\_n}\\ V_{u2\mathrm{wind}}=V_u-V_{\mathrm{wind}\_u}\end{array}---\left(6\right)\right.$

其中：

V_e、V_n、V_u分别是无人机地速在东向、北向、天向的分量；

V_{wind_e}、V_{wind_n}、V_{wind_u}分别是气流的东向速度、北向速度、天向速度；

V_e2wind、V_n2wind、V_u2wind分别是空速在东向、北向、天向的分量。

用户可以随时设定气流速度V_{wind_e}、V_{wind_n}、V_{wind_u}，从而实现风的动态加入。

步骤六：已知无人机的状态量和风速，在标准大气模型、重力场模型的基础上，计算空速、气流角、动压……等辅助量，支持模型加风；

1962年美国标准大气：

ρ查1962年标准大气表                        (7)

V_sonic查1962年标准大气表                    (8)

$\mathrm{\&sigma;}=\frac{\mathrm{\&rho;}}{{\mathrm{\&rho;}}_{\mathrm{sea}\_\mathrm{level}}}---\left(9\right)$

H_e＝3.2808×H                 (10)

$T_s=0.55556\&times;\left\{\begin{array}{cc}518.7\&times;(1.0-6.875e-6\&times;H);& 0<H_e<36089\\ 518.7\&times;0.751985;& 36089<H_e<240000\end{array}---\left(11\right)\right.$

$P_s=47.88018\&times;\left[\begin{array}{cc}2113.8\&times;{(1.0-6.875e\&times;H)}^{5.256};& 0<H<36089\\ 2113.8\&times;0.2234\&times;e^{-4.806e-5\&times;(H-36089)};& 36089<H<240000\end{array}---\left(12\right)\right]$

其中：

H、H_e分别是海拔高度的公制和英制表示；

ρ表示当前高度处的大气密度；

V_sonic表示当前高度处的音速；

σ表示当前高度和海平面的大气密度之比；

T_s表示当前高度处大气的静温；

P_s表示当前高度处的大气的静压。

重力场模型：

$\left\{\begin{array}{c}{K}_R=0.00108263*{\left(\frac{H+R_e}{R_e}\right)}^2\\ K_g=\sqrt{2.25({5\sin }^4\left(D\right)-{2\sin }^2\left(D\right)+1)K_R^2+3({-3\sin }^2\left(D\right)+1)K_R+1}\\ g=K_g\frac{\mathrm{GM}}{{(H+R_e)}^2}\end{array}\right.---\left(13\right)$

其中：

H表示海拔高度；

R_e表示地球半径；

D表示纬度；

G表示引力常数；

M表示地球质量。

其他辅助量(指示空速、真空速、动压、压差、气流角、气流角变化率和无人机相对于跑道的坐标)可根据其定义和物理机理进行计算。

步骤七：调用用户接口函数获得完备的描述无人机运动的物理量，供设计人员使用；

步骤八：重复步骤二至步骤七，实现无人机运动的非线性仿真。

如图3所示，上述步骤中所涉及的接口函数可分为八类：①基本操作函数(设置无人机所受外力和外力矩、设置无人机惯性量、触发模型解算)；②设置刚体基本状态量函数；③获取刚体基本状态量函数；④获取刚体基本状态量导数函数；⑤相对气流功能函数；⑥坐标系之间的相互转化函数；⑦跑到相关功能函数；⑧内部核心算法函数(实现刚体六自由度模型、大气模型、重力场模型、状态量更新和辅助物理量的计算)。

把欧美坐标系的输入转化为苏联坐标系的输入如图4所示，在苏联坐标系进行模型解算后，把苏联坐标系的输出再转化为欧美坐标系的输出，实现一个模型对两种坐标系的支持。

欧美坐标系到苏联坐标系转换矩阵

$T_{E2S}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 0& -1\\ 0& 1& 0\end{array}\right]---\left(14\right)$

苏联坐标系到欧美坐标系转换矩阵

$T_{S2E}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 0& 1\\ 0& -1& 0\end{array}\right]---\left(15\right)$

如图2、图4、图7、图8所示，在苏联坐标系下定义的物理量直接输入仿真系统，在欧美坐标系下定义的物理量通过“欧美坐标系-苏联坐标系”11转换后，再输入仿真系统，满足仿真系统的解算要求。如图3所示，仿真系统的物理量直接输出，满足用户在苏联坐标系下的仿真要求；仿真系统的物理量通过“苏联坐标系-欧美坐标系”61转换后再输出，满足用户在欧美坐标系下的仿真要求。

在VC++6.0下用ANSI C编程实现并封装成静态库，提供用户接口函数；在此基础上，按如下步骤进行CMEX封装：

首先，用ANSI C编写CMEX封装所需要中间文件(FileName.c)，它依赖于通用模型静态库(LibName.lib)，只含有一个固定的接口函数：

void mexFunction(int nlhs，mxArray*plhs[]，int nrhs，const mxArray*prhs[])；

其中：

nlhs是返回值的个数；

plhs是返回值的首地址；

nrhs是形参的个数；

prhs是形参的首地址。

其次，在MATLAB命令窗口按要求输入命令：

mex FileName.c LibName.lib

生成可在MATLAB环境运行的函数。

如图5所示，通用模型静态库(.lib)可用于半物理仿真和等效仿真等仿真平台，通用模型MATLAB程序(.mexw32)可用于S-function和M语言非线性仿真等开发平台，支持多平台运行的要求。

实施例二：

本实施例涉及一种用于实现如实施例一所述的仿真方法的仿真系统，如图6-图8所示，其包括：

仿真系统输入模块1；

进行无人机运动状态初始化的初始化模块2；

状态量导数解算模块3；

状态量更新模块4；

辅助物理量计算模块5；

仿真系统输出模块6；

其中，无人机状态参数通过仿真系统输入模块1输入初始化模块2，初始化模块2对无人机初始状态信息进行处理并将处理结果输入初始化状态量更新模块4；无人机所受不含重力的外力和外力矩通过仿真系统输入模块1输入状态量导数解算模块3，状态量导数解算模块3解算得到状态量导数，并输出至状态量更新模块4；状态量更新模块4更新运动状态，将新的值输出至辅助物理量计算模块5；状态量导数解算模块3、状态量更新模块4、辅助物理量计算模块5的输出结果通过仿真系统输出模块6提供给用户使用。

Claims (5)

1.一种基于非线性数学模型的通用型无人机仿真方法，其特征在于，包括下列步骤：

步骤一：调用仿真系统通用模型软件提供的用户接口函数，初始化无人机的运动状态；

步骤二：设置无人机的质量和转动惯量，并向仿真系统输入无人机所受不含重力的外力和外力矩；

步骤三：根据面对称刚体六自由度运动模型计算无人机状态量导数，以平面大地为惯性系，在苏联坐标系下进行解算：

线动力学方程：

$\left\{\begin{array}{c}{\stackrel{.}{V}}_x=V_z{\mathrm{\&omega;}}_y-V_y{\mathrm{\&omega;}}_z+F_x/m+G_x\\ {\stackrel{.}{V}}_y=V_x{\mathrm{\&omega;}}_z{-V}_z{\mathrm{\&omega;}}_x+F_y/m{G}_y\\ {\stackrel{.}{V}}_x=V_y{\mathrm{\&omega;}}_x-V_x{\mathrm{\&omega;}}_y+F_z/m+G_z\end{array}\right.---\left(1\right)$

角动力学方程：

$\left\{\begin{array}{c}{\stackrel{.}{\mathrm{\&omega;}}}_x=\frac{I_{\mathrm{yy}}{M}_x+I_{\mathrm{xy}}{M}_y-I_{\mathrm{xy}}(I_{\mathrm{xx}}-I_{\mathrm{yy}}+I_{\mathrm{zz}}){\mathrm{\&omega;}}_x{\mathrm{\&omega;}}_z+(I_{\mathrm{yy}}{I}_{\mathrm{yy}}+I_{\mathrm{xy}}{I}_{\mathrm{xy}}-I_{\mathrm{yy}}{I}_{\mathrm{zz}}){\mathrm{\&omega;}}_y{\mathrm{\&omega;}}_z}{I_{\mathrm{xx}}{I}_{\mathrm{yy}}-I_{\mathrm{xy}}{I}_{\mathrm{xy}}}\\ {\stackrel{.}{\mathrm{\&omega;}}}_y=\frac{I_{\mathrm{xx}}{M}_y+I_{\mathrm{xy}}{M}_x+I_{\mathrm{xy}}(I_{\mathrm{xx}}-I_{\mathrm{yy}}+I_{\mathrm{zz}}){\mathrm{\&omega;}}_y{\mathrm{\&omega;}}_z-(I_{\mathrm{xx}}{I}_{\mathrm{xx}}+I_{\mathrm{xy}}{I}_{\mathrm{xy}}-I_{\mathrm{xx}}{I}_{\mathrm{zz}}){\mathrm{\&omega;}}_x{\mathrm{\&omega;}}_z}{I_{\mathrm{xx}}{I}_{\mathrm{yy}}-I_{\mathrm{xy}}{I}_{\mathrm{xy}}}\\ {\stackrel{.}{\mathrm{\&omega;}}}_z=\frac{M_z-(I_{\mathrm{yy}}-I_{\mathrm{xx}}){\mathrm{\&omega;}}_x{\mathrm{\&omega;}}_y+I_{\mathrm{xy}}({\mathrm{\&omega;}}_x{\mathrm{\&omega;}}_x-{\mathrm{\&omega;}}_y{\mathrm{\&omega;}}_y)}{I_{\mathrm{zz}}}\end{array}\right.---\left(2\right)$

线运动学方程：

$\left\{\begin{array}{c}{\stackrel{.}{x}}_g=V_x\cos \mathrm{\&psi;}\cos \mathrm{\&upsi;}+V_y(\sin \mathrm{\&psi;}\sin \mathrm{\&gamma;}-\cos \mathrm{\&psi;}\sin \mathrm{\&upsi;}\cos \mathrm{\&gamma;})+V_z(\sin \mathrm{\&psi;}\cos \mathrm{\&gamma;}+\cos \mathrm{\&psi;}\sin \mathrm{\&upsi;}\sin \mathrm{\&gamma;})\\ {\stackrel{.}{h}}_g=V_x\sin \mathrm{\&upsi;}+V_y\cos \mathrm{\&upsi;}\cos \mathrm{\&gamma;}-V_z\cos \mathrm{\&upsi;}\sin \mathrm{\&gamma;}\\ {\stackrel{.}{z}}_g=-V_x\sin \mathrm{\&psi;}\cos \mathrm{\&upsi;}+V_y(\cos \mathrm{\&psi;}\sin \mathrm{\&gamma;}+\sin \mathrm{\&psi;}\sin \mathrm{\&upsi;}\sin )+V_z(\cos \mathrm{\&psi;}\cos \mathrm{\&gamma;}-\sin \mathrm{\&psi;}\sin \mathrm{\&upsi;}\sin \mathrm{\&gamma;})\end{array}\right.---\left(3\right)$

角运动学方程：

$\left\{\begin{array}{c}\stackrel{.}{\mathrm{\&upsi;}}={\mathrm{\&omega;}}_y\sin \mathrm{\&gamma;}+{\mathrm{\&omega;}}_z\cos \mathrm{\&gamma;}\\ \stackrel{.}{\mathrm{\&gamma;}}={\mathrm{\&omega;}}_x-\tan \mathrm{\&upsi;}({\mathrm{\&omega;}}_y\cos \mathrm{\&gamma;}-{\mathrm{\&omega;}}_z\sin \mathrm{\&gamma;})\\ \stackrel{.}{\mathrm{\&psi;}}=({\mathrm{\&omega;}}_y\cos \mathrm{\&gamma;}-{\mathrm{\&omega;}}_z\sin \mathrm{\&gamma;})/\cos \mathrm{\&upsi;}\end{array}\right.---\left(4\right)$

其中，

F_x、F_y、F_z为外力沿机体轴的分量，其中的外力不含无人机重力；

M_x、M_y、M_z为外力矩沿机体轴的分量；

G_x、G_y、G_z为重力加速度沿机体轴的分量；

m为无人机质量；

I_xx、I_yy、I_zz、I_xy、分别是无人机沿机体轴向的转动惯量和惯性积；

V_x、V_y、V_z为地速沿机体轴的分量；

x_g、h_g、z_g为分别是无人机北向、天向和东向的位移；

ω_x、ω_y、ω_z为绕机体轴转动的角速率沿机体轴的分量；

υ、γ、ψ为俯仰角、滚转角、偏航角；

步骤四：使用Adams数值积分算法更新无人机状态量，

$y_{n+4}=y_{n+3}+\frac{h}{24}(55{\stackrel{.}{y}}_{n+3}-59{\stackrel{.}{y}}_{n+2}+37{\stackrel{.}{y}}_{n+1}-9{\stackrel{.}{y}}_n)---\left(5\right)$

截断误差为

其中：

h表示仿真步长；

分别表示第n+3、n+2、n+1、n时刻的导数；

y_n+4、y_n+3分别表示n+4、n+3时刻的值；

步骤五：在仿真系统中内置风模型，根据用户设置的风速，合成空速：

$\left\{\begin{array}{c}{V}_{e2\mathrm{wind}}=V_e-V_{\mathrm{wind}\_e}\\ V_{n2\mathrm{wind}}=V_n-V_{\mathrm{wind}\_n}\\ V_{u2\mathrm{wind}}=V_u-V_{\mathrm{wind}\_u}\end{array}---\left(6\right)\right.$

其中：

V_e、V_n、V_u分别是无人机地速在东向、北向、天向的分量；

V_{wind_e}、V_{wind_n}、V_{wind_u}分别是气流的东向速度、北向速度、天向速度；

V_e2wind、V_n2wind、V_u2wind分别是空速在东向、北向、天向的分量；

步骤六：根据步骤四得到的无人机状态量和步骤五得到的空速，在标准大气模型、重力场模型的基础上，计算空速、气流角、动压等辅助量；

1962年美国标准大气：

ρ查1962年标准大气表                (7)

V_sonic查1962年标准大气表            (8)

$\mathrm{\&sigma;}=\frac{\mathrm{\&rho;}}{{\mathrm{\&rho;}}_{\mathrm{sea}\_\mathrm{level}}}---\left(9\right)$

H_e＝3.2808×H                       (10)

$T_s=0.55556\&times;\left\{\begin{array}{cc}518.7\&times;(1.0-6.875e-6\&times;H);& 0<H_e<36089\\ 518.7\&times;0.751985;& 36089<H_e<240000\end{array}---\left(11\right)\right.$

$P_s=47.88018\&times;\left[\begin{array}{cc}2113.8\&times;{(1.0-6.875e\&times;H)}^{5.256};& 0<H<36089\\ 2113.8\&times;0.2234\&times;e^{-4.806e-5\&times;(H-36089)};& 36089<H<240000\end{array}---\left(12\right)\right]$

其中：

H、H_e分别是海拔高度的公制和英制表示；

ρ表示当前高度处的大气密度；

V_sonic表示当前高度处的音速；

σ表示当前高度和海平面的大气密度之比；

T_s表示当前高度处大气的静温；

P_s表示当前高度处的大气的静压；

重力场模型：

$\left\{\begin{array}{c}{K}_R=0.00108263*{\left(\frac{H+R_e}{R_e}\right)}^2\\ K_g=\sqrt{2.25({5\sin }^4\left(D\right)-{2\sin }^2\left(D\right)+1)K_R^2+3({-3\sin }^2\left(D\right)+1)K_R+1}\\ g=K_g\frac{\mathrm{GM}}{{(H+R_e)}^2}\end{array}\right.---\left(13\right)$

其中：

H表示海拔高度；

R_e表示地球半径；

D表示纬度；

G表示引力常数；

M表示地球质量；

步骤七：调用通用模型软件用户接口函数获得完备的描述无人机运动的物理量；

步骤八：重复步骤二至步骤七，实现无人机运动的非线性仿真。

2.如权利要求1所述的基于非线性数学模型的通用型无人机仿真方法，其特征在于，可支持所有面对称无人机运动的仿真。

3.如权利要求1所述的基于非线性数学模型的通用型无人机仿真方法，其特征在于，可同时支持两种坐标系下的仿真。

4.如权利要求1所述的基于非线性数学模型的通用型无人机仿真方法，其特征在于，可同时支持C语言环境和MATLAB环境下的仿真。

5.一种用于实现权利要求1所述方法的系统，其特征在于，包括：

仿真系统输入模块(1)；

进行无人机运动状态初始化的初始化模块(2)；

状态量导数解算模块(3)；

状态量更新模块(4)；

辅助物理量计算模块(5)；

仿真系统输出模块(6)；

其中，无人机状态参数通过仿真系统输入模块(1)输入初始化模块(2)，初始化模块(2)对无人机初始状态信息进行处理并将处理结果输入初始化状态量更新模块(4)；无人机所受不含重力的外力和外力矩通过仿真系统输入模块(1)输入状态量导数解算模块(3)，状态量导数解算模块(3)解算得到状态量导数，并输出至状态量更新模块(4)；状态量更新模块(4)更新运动状态，将新的值输出至辅助物理量计算模块(5)；状态量导数解算模块(3)、状态量更新模块(4)、辅助物理量计算模块(5)的输出结果通过仿真系统输出模块(6)提供给用户使用。

Images