CN104133379A - 四旋翼飞行器仿真方法 - Google Patents

Abstract

四旋翼飞行器仿真方法，涉及一种无人飞行器实时仿真方法。解决现有半实物仿真***规模庞大，连接复杂、仿真存在虚拟的状态变量导致的准确度低、难以应用基于模型的控制器设计方法和数值仿真***机理建模得到的模型准确度不高的问题。本发明把实际的飞行器作为被控对象接入仿真回路，为MATLAB/Simulink中机理建模得到的数值仿真模型提供了一个参考，使得飞行器机理建模准确性可以快速得到验证；使用了参数辨识的方法确保了数值仿真中对象模型的准确性，从而数值仿真中验证可行的控制器最终能够有效地应用到实际的物理对象上。本发明具体用于四旋翼飞行器仿真领域。

Description

四旋翼飞行器仿真方法

技术领域

本发明涉及一种无人飞行器实时仿真方法。

背景技术

四旋翼飞行器在动力学上具有欠驱动、强耦合和非线性的特点，这些都增加了飞行控制器设计的难度。

在控制器的设计阶段，通常有两种方式可以去验证控制器性能：第一种是实物飞行，这种验证方法可信度高，但具有效率低、风险大、成本高的缺点；第二种方法是数值仿真，这种方法效率高、成本低，但通常会由于数值模型中参数估计不准确、未建模动态和未考虑扰动等因素导致即便是在数值仿真中验证可行的控制器也难以应用到实际的飞行器上。

半实物仿真，又称为硬件在回路中的仿真(Hardware in the Loop Simulation)，是指在仿真实验***的仿真回路中接入部分实物的实时仿真。实时性是进行半实物仿真的必要前提。半实物仿真同其它类型的仿真方法相比具有经济地实现更高真实度的可能性，同时具有快速搭建和部署控制算法的优势。从***的观点来看，半实物仿真允许在***中接入部分实物，意味着可以把部分实物放在***中进行考察，从而使部件能在满足***整体性能指标的环境中得到检验，因此半实物仿真是提高***设计的可靠性和研制质量的必要手段。

实现半实物仿真的一种手段是采用dSPACE实时仿真***，它是由德国dSPACE公司开发的一套基于MATLAB/Simulink的控制***在实时环境下的开发及测试工作平台，实现了实际物理***和MATLAB/Simulink的无缝连接，但价格昂贵。

另一种支持数据可视化的基于xPC的半实物仿真***要两台计算机(宿主机、目标机)才能完成仿真，连接复杂成本较高。

一种四旋翼飞行器的半实物仿真方法将飞行器固定在一个与地面固连的万向节上，限制了其在空间平动的三个自由度，从而保证其飞行的安全性；但这种方法使用了虚拟的位置状态，仿真准确度不高，而且并不适合用于验证高度控制算法。

发明内容

本发明是为了解决现有半实物仿真***规模庞大，连接复杂、仿真存在虚拟的状态变量导致的准确度低、难以应用基于模型的控制器设计方法和数值仿真***机理建模得到的模型准确度不高的问题，本发明提供了一种四旋翼飞行器仿真方法。

四旋翼飞行器半实物仿真方法，它是基于下述装置实现的，该装置包括仿真计算机、机载微控制器、1号无线收发器、2号无线收发器和机载传感器；

机载微控制器的数据信号输入端与机载传感器的数据信号输出端连接，机载微控制器的通讯端与2号无线收发器的信号输入输出端连接，2号无线收发器通过无线通信的方式与1号无线收发器进行数据传输，1号无线收发器的信号输入输出端与仿真计算机的信号输入输出端连接，

仿真计算机内嵌入Matlab/Simulink仿真模块，所述的Matlab/Simulink仿真模块包括数据输入模块、状态估计与滤波模块、姿态与高度控制模块、飞行状态显示模块、数据输出模块、半实物仿真数据记录模块和控制指令生成模块；

该半实物仿真方法的具体过程为：

首先，通过机载微控制器采集机载传感器输出的数据信号，且机载微控制器依次通过2号无线收发器、1号无线收发器将采集到的数据信号送至仿真计算机的数据输入模块，该数据信号包括测距读数、三轴加速度读数、三轴角速度读数和三轴磁感应强度信号；

通过数据输入模块接收输入的数据信号，并将该数据送至状态估计与滤波模块，状态估计与滤波模块根据接收的数据信号利用互补滤波的方法获得两组数据，该两组数据中的第一组数据为姿态欧拉角估计值和高度估计值，第二组数据为角速度的估计值和竖直方向上速度的估计值，

且将第一组数据送至飞行状态显示模块，飞行状态显示模块用于实时显示四旋翼飞行器的飞行状态，

将第一组和第二组数据送至姿态与高度控制模块，姿态与高度控制模块根据控制指令生成模块产生的控制指令，计算获得姿态和高度控制信号，然后经过控制分配得到四旋翼飞行器的四个电机的期望转速，该四个电机的期望转速通过数据输出模块依次经过1号无线收发器、2号无线收发器后送至机载微控制器用于控制四旋翼飞行器的电子调速器及电机，

半实物仿真数据记录模块用于存储状态估计与滤波模块输出的两组数据和四旋翼飞行器的四个电机的期望转速。

所述的机载传感器包括三轴电子罗盘、六轴运动传感器和超声测距传感器，

三轴电子罗盘用于采集三轴磁感应强度信号，

六轴运动传感器用于采集三轴加速度读数和三轴角速度读数，

超声测距传感器用于采集测距读数。

所述的三轴电子罗盘的采用型号为HMC5883L的集成电路实现，六轴运动传感器的型号为MPU6050的集成电路实现，超声测距传感器采用型号为US-100的集成电路实现。

所述的机载微控制器采用TMS320F28335型DSP实现。

所述的姿态与高度控制模块的优化过程为，

在模型辨识子***中，参数辨识器接收四旋翼飞行器的四个电机的期望转速Ω₁,Ω₂,Ω₃,Ω₄、姿态欧拉角估计值高度估计值角速度的估计值和竖直方向上速度的估计值该参数辨识器根据待辨识模型结构，求得参数向量的辨识结果，进而确定了飞行器运动学与动力学模型，从而根据数值仿真子***完成对姿态与高度控制模块的优化，

所述的待辨识模型结构通过下述的状态空间方程描述：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}=p\cos \mathrm{\θ}+r\sin \mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}=q-r\cos \mathrm{\θ}\tan \mathrm{\φ}+p\sin \mathrm{\θ}\tan \mathrm{\φ}\\ \stackrel{\·}{\mathrm{\ψ}}=r\cos \mathrm{\θ}\sec \mathrm{\φ}-p\sin \mathrm{\θ}\sec \mathrm{\φ}\\ \stackrel{\·}{p}=\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}\left(\frac{I_{\mathrm{yy}}-I_{\mathrm{zz}}}{I_{\mathrm{xx}}}\right)-\stackrel{\·}{\mathrm{\θ}}\frac{J_r}{I_{\mathrm{xx}}}{\mathrm{\Ω}}_r+\frac{l}{I_{\mathrm{xx}}}{U}_2\\ \stackrel{\·}{q}=\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\ψ}}\left(\frac{I_{\mathrm{zz}}-I_{\mathrm{xx}}}{I_{\mathrm{yy}}}\right)+\stackrel{\·}{\mathrm{\φ}}\frac{J_r}{I_{\mathrm{yy}}}{\mathrm{\Ω}}_r+\frac{l}{I_{\mathrm{yy}}}{U}_3\\ \stackrel{\·}{r}=\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\φ}}\left(\frac{I_{\mathrm{xx}}-I_{\mathrm{yy}}}{I_{\mathrm{zz}}}\right)+\frac{1}{I_{\mathrm{zz}}}{U}_4\\ \stackrel{\·}{z}=w\\ \stackrel{\·}{w}=g-\left(\cos \mathrm{\φ}\cos \mathrm{\θ}\right)\frac{1}{m}{U}_1\end{array}\right.$    (公式一)，

其中， $\left\{\begin{array}{c}{U}_1=b({{\mathrm{\Ω}}_1}^2+{\mathrm{\Ω}}_2^2+{\mathrm{\Ω}}_3^2+{\mathrm{\Ω}}_4^2)\\ U_2=b(-{\mathrm{\Ω}}_2^2+{\mathrm{\Ω}}_4^2)\\ U_3=b({{\mathrm{\Ω}}_1}^2-{\mathrm{\Ω}}_3^2)\\ U_4=d(-{{\mathrm{\Ω}}_1}^2+{\mathrm{\Ω}}_2^2-{\mathrm{\Ω}}_3^2+{\mathrm{\Ω}}_4^2)\\ {\mathrm{\Ω}}_r={\mathrm{\Ω}}_2+{\mathrm{\Ω}}_4-{\mathrm{\Ω}}_1-{\mathrm{\Ω}}_3\end{array}\right.$    (公式二)，

所述的参数辨识器根据待辨识模型结构，获得参数向量的辨识结果的具体过程为：

当四个电机的期望转速Ω₁,Ω₂,Ω₃,Ω₄、作为飞行器模型的输入时，通过公式一和公式二求得部分预测状态的数值解为

ξ(k)＝[φ(k),θ(k),ψ(k),p(k),q(k),r(k),z(k),w(k)]^T，

其次，采用代价函数 $V_N\left(r\right)=\underset{k=1}{\overset{N}{\mathrm{\Σ}}}({\widehat{\mathrm{\ξ}}}^2\left(k\right)-{\mathrm{\ξ}}^2\left(k\right)),$ 求取r₀，

$r_0=\arg \underset{r}{\min }{V}_N\left(r\right),$

r₀即为参数向量的辨识结果，r为待辨识参数向量，其中，r＝[m,l,I_xx,I_yy,I_zz,J_r,b,d]^T，

待辨识参数向量r包括飞行器总质量m，相对电机轴距离l，机体转动惯量(I_xx,I_yy,I_zz)、旋翼与电机转子绕电机轴转动惯量J_r，升力系数b，扭矩系数d，N为记录的采样点数；k为采样时间点，U₁、U₂、U₃、U₄和Ω_r均为中间变量，g为重力加速度。

四旋翼飞行器的数值仿真方法，该方法的具体过程为，

首先，参数辨识器接收从半实物仿真***获得数据信息，该参数辨识器根据待辨识模型结构，获得参数向量的辨识结果，并通过数值仿真子***根据获得的参数向量的辨识结果，获得飞行器运动学与动力学模型，从而完成四旋翼飞行器的数值仿真。

所述的参数辨识器接收从半实物仿真***获得数据信息包括四旋翼飞行器的四个电机的期望转速Ω₁,Ω₂,Ω₃,Ω₄、姿态欧拉角估计值高度估计值角速度的估计值和竖直方向上速度的估计值

所述的待辨识模型结构通过下述的状态空间方程描述：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}=p\cos \mathrm{\θ}+r\sin \mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}=q-r\cos \mathrm{\θ}\tan \mathrm{\φ}+p\sin \mathrm{\θ}\tan \mathrm{\φ}\\ \stackrel{\·}{\mathrm{\ψ}}=r\cos \mathrm{\θ}\sec \mathrm{\φ}-p\sin \mathrm{\θ}\sec \mathrm{\φ}\\ \stackrel{\·}{p}=\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}\left(\frac{I_{\mathrm{yy}}-I_{\mathrm{zz}}}{I_{\mathrm{xx}}}\right)-\stackrel{\·}{\mathrm{\θ}}\frac{J_r}{I_{\mathrm{xx}}}{\mathrm{\Ω}}_r+\frac{l}{I_{\mathrm{xx}}}{U}_2\\ \stackrel{\·}{q}=\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\ψ}}\left(\frac{I_{\mathrm{zz}}-I_{\mathrm{xx}}}{I_{\mathrm{yy}}}\right)+\stackrel{\·}{\mathrm{\φ}}\frac{J_r}{I_{\mathrm{yy}}}{\mathrm{\Ω}}_r+\frac{l}{I_{\mathrm{yy}}}{U}_3\\ \stackrel{\·}{r}=\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\φ}}\left(\frac{I_{\mathrm{xx}}-I_{\mathrm{yy}}}{I_{\mathrm{zz}}}\right)+\frac{1}{I_{\mathrm{zz}}}{U}_4\\ \stackrel{\·}{z}=w\\ \stackrel{\·}{w}=g-\left(\cos \mathrm{\φ}\cos \mathrm{\θ}\right)\frac{1}{m}{U}_1\end{array}\right.$    (公式一)，

其中， $\left\{\begin{array}{c}{U}_1=b({{\mathrm{\Ω}}_1}^2+{\mathrm{\Ω}}_2^2+{\mathrm{\Ω}}_3^2+{\mathrm{\Ω}}_4^2)\\ U_2=b(-{\mathrm{\Ω}}_2^2+{\mathrm{\Ω}}_4^2)\\ U_3=b({{\mathrm{\Ω}}_1}^2-{\mathrm{\Ω}}_3^2)\\ U_4=d(-{{\mathrm{\Ω}}_1}^2+{\mathrm{\Ω}}_2^2-{\mathrm{\Ω}}_3^2+{\mathrm{\Ω}}_4^2)\\ {\mathrm{\Ω}}_r={\mathrm{\Ω}}_2+{\mathrm{\Ω}}_4-{\mathrm{\Ω}}_1-{\mathrm{\Ω}}_3\end{array}\right.$    (公式二)，

所述的参数辨识器根据待辨识模型结构，获得参数向量的辨识结果的具体过程为：

当四个电机的期望转速Ω₁,Ω₂,Ω₃,Ω₄、作为飞行器模型的输入时，通过公式一和公式二求得部分预测状态的数值解为ξ(k)＝[φ(k),θ(k),ψ(k),p(k),q(k),r(k),z(k),w(k)]^T，

其次，采用代价函数 $V_N\left(r\right)=\underset{k=1}{\overset{N}{\mathrm{\Σ}}}({\widehat{\mathrm{\ξ}}}^2\left(k\right)-{\mathrm{\ξ}}^2\left(k\right)),$ 求取r₀，

$r_0=\arg \underset{r}{\min }{V}_N\left(r\right),$

r₀即为参数向量的辨识结果，r为待辨识参数向量，其中，r＝[m,l,I_xx,I_yy,I_zz,J_r,b,d]^T，

待辨识参数向量r包括飞行器总质量m，相对电机轴距离l，机体转动惯量(I_xx,I_yy,I_zz)、旋翼与电机转子绕电机轴转动惯量J_r，升力系数b，扭矩系数d，N为记录的采样点数；k为采样时间点，U₁、U₂、U₃、U₄和Ω_r均为中间变量，g为重力加速度。

本发明建立了一种四旋翼飞行器半实物验证平台，把实际的飞行器作为被控对象接入仿真回路，为MATLAB/Simulink中建好的数值仿真模型提供一个参考，使得飞行器数值建模准确性可以快速得到验证，进而确保数值仿真中验证可行的控制器最终能够有效地应用到实际的物理对象上。同时为飞行器模型分析和控制参数整定提供一个可视化的数据显示平台，提高状态观测器和控制器的设计开发效率。

***包括组成部分与功能：

安装有MATLAB/Simulink的仿真计算机作为半实物验证***的核心部分，它运行了飞行器实体对象和仿真环境的数学模型和程序，采用了层次化、模块化的建模方法，可以以数据流的驱动方式实时运行。

仿真计算机中安装了MATLAB的Real-time Windows Target(RTWT)产品，它提供了一个实时引擎使得Simulink仿真模型的不同的模块能够在Windows***下实时地与硬件输入输出板卡连接，同时实现信号的可视化与模型运行时的参数调节功能。

外部模式则允许Simulink引擎作为客户端(宿主)，RTWT生成的代码作为服务器(目标)，客户端和服务器之间以共享内存的方式进行通信。具体来看，Simulink引擎发送消息请求目标接收数据并且上传测量数据，而目标则以执行请求作为回应。

在外部模式下，半实物仿真模型最高可以以5KHz的频率执行，Simulink模型就变成了一个图形用户界面，不需要重新编译便可以调整模型参数。运行在Simulink中的仿真模型包括：

(1)仿真模型硬件接口包括数据输入模块和数据输出模块，分别可以完成串口通信的二进制编码和解码的工作，与机载微控制器建立数据连接；

(2)数值对象模型，包括飞行器的运动学方程、动力学方程、传感器的测量模型以及模拟的噪声模型。传感器包括三轴电子罗盘、六轴运动传感器和超声测距传感器。此部分与实际的物理模型对应，用于滤波算法和控制算法的初步验证；

(3)状态估计与滤波模块，根据对地磁矢量和重力矢量的观测和三轴方位读数信号的积分，利用互补滤波的方法获得对姿态信息的估计。根据高度的测量值、姿态的估计值与加速度的测量值，同样使用互补滤波的方法获得高度变化率的估计。

(4)姿态与高度控制模块，包括四个分别以滚转角、俯仰角、偏航角和高度为被控变量的PID控制器，经过控制分配，得到四个电机的期望转速作为输出。

以上(2)(3)(4)构成控制回路可以实现数值仿真，以上(1)(3)(4)构成控制回路并配合实际的物理模型可以完成半实物仿真，

四旋翼飞行器的机载控制板搭载了数字信号处理器(DSP)TMS320F28335用于采集飞行姿态和位置信息传给仿真计算机并执行仿真计算机发出的控制指令。

DSP的I²C接口分别与六轴运动传感器MPU6050(包含三轴加速度计和三轴陀螺仪)和三轴电子罗盘HMC5883L连接。通过UART与超声测距模块US-100连接用以测量高度。通过SPI总线与运行Simulink的仿真计算机交换数据：按照收到的请求向仿真计算机发送采集到的传感器数据，根据收到的控制信号生成PWM信号并发给电子调速器从而控制电机转速。在仿真的开始阶段，应当将飞行器重心固定在一个万向节上从而保证试验的安全性。

射频无线通信部分包含了一个同DSP连接的NRF24L01无线收发器，和一个与仿真计算机连接的兼容NRF24L01的USB无线收发器，实现了仿真计算机与DSP间的无线数据交换。

本发明所提出的四旋翼飞行器半实物仿真方法，可以直观地针对各种在数值仿真中已经验证成功的飞行控制算法结合实际对象进行半实物仿真，既能真实地呈现四旋翼无人飞行器的状态和控制量变化曲线，又能在线修改滤波器与控制器参数，能够为嵌入式控制器的设计提供极具参考价值的测试结果，可极大地缩减研发周期，同时节约飞行实验成本。

本发明在仿真过程中，其被控对象采用实体四旋翼无人飞行器，而非纯数值形式的动力学模型，姿态传感器可实时读取到真实的飞行状态数据，仿真效果贴近直实情况。

本发明使用半实物验证***的扰动响应数据，将***辨识的方法应用到数值模型的修正工作上，为四旋翼飞行器的机理建模方法提供了有效的验证工具。

仿真***的两个数据终端(仿真计算机和实物平台)使用射频无线通信，近距离情况下可以保证快速稳定的数据交换，提高了仿真的灵活性。

由于所需要的控制周期在200Hz以内，MATLAB提供的可以植入到Windows中的实时内核完全可以胜任本发明所需要的数据带宽，本发明所涉及的仿真计算机在提供人机交互功能的同时完成了实际飞行器模型实时控制的任务，省去了考虑专用计算机成本和兼容性问题的烦恼。

而本发明对现有基于xPC的半实物仿真***进行改进，上述目标机内运行的仿真程序转移到宿主机中的MATLAB提供的实时内核中运行，既保证了***运行的实时性，又缩减了***硬件的规模，降低了模块连接的复杂度，提高了运行的可靠性。由于本发明采用了全自由度的半实物仿真，没有将飞行器限制在万向节上，因此仿真得到的数据具有更高的真实性。

本发明把实际的飞行器作为被控对象接入仿真回路，为MATLAB/Simulink中建好机理建模得到的数值仿真模型提供了一个参考，对于确认机理建模得到的数值仿真模型的有效性具有重要意义

对于四旋翼飞行器的控制问题，数据驱动的控制器设计方法(比如PID)不依赖于具体模型的参数，因此参数辨识的过程没有太大存在的意义。在过去的几十年中，各种新型的基于模型的控制器设计方法不断涌现。为了利用这些方法，首先必须获得准确的被控对象模型。与机理建模得到的仿真模型相比，本发明所提出的参数辨识器可以得到更为准确的对象模型，这为基于模型的控制器设计方法的应用提供了必要的条件。

附图说明

图1为本发明所述的四旋翼飞行器半实物仿真方法的原理示意图；

图2为具体实施方式六所述的四旋翼飞行器的数值仿真方法的原理示意图。

具体实施方式

具体实施方式一：参见图1说明本实施方式，本实施方式所述的四旋翼飞行器半实物仿真方法，它是基于下述装置实现的，该装置包括仿真计算机1、机载微控制器2、1号无线收发器3、2号无线收发器4和机载传感器5；

机载微控制器2的数据信号输入端与机载传感器5的数据信号输出端连接，机载微控制器2的通讯端与2号无线收发器4的信号输入输出端连接，2号无线收发器4通过无线通信的方式与1号无线收发器3进行数据传输，1号无线收发器3的信号输入输出端与仿真计算机1的信号输入输出端连接，

仿真计算机1内嵌入Matlab/Simulink仿真模块，所述的Matlab/Simulink仿真模块包括数据输入模块1-1、状态估计与滤波模块1-2、姿态与高度控制模块1-3、飞行状态显示模块1-4、数据输出模块1-5、半实物仿真数据记录模块1-6和控制指令生成模块1-7；

该半实物仿真方法的具体过程为：

首先，通过机载微控制器2采集机载传感器5输出的数据信号，且机载微控制器2依次通过2号无线收发器4、1号无线收发器3将采集到的数据信号送至仿真计算机1的数据输入模块1-1，该数据信号包括测距读数、三轴加速度读数、三轴角速度读数和三轴磁感应强度信号；

通过数据输入模块1-1接收输入的数据信号，并将该数据送至状态估计与滤波模块1-2，状态估计与滤波模块1-2根据接收的数据信号利用互补滤波的方法获得两组数据，该两组数据中的第一组数据为姿态欧拉角估计值和高度估计值，第二组数据为角速度的估计值和竖直方向上速度的估计值，

且将第一组数据送至飞行状态显示模块1-4，飞行状态显示模块1-4用于实时显示四旋翼飞行器的飞行状态，

将第一组和第二组数据送至姿态与高度控制模块1-3，姿态与高度控制模块1-3根据控制指令生成模块1-7产生的控制指令，计算获得姿态和高度控制信号，然后经过控制分配得到四旋翼飞行器的四个电机的期望转速，该四个电机的期望转速通过数据输出模块1-5依次经过1号无线收发器3、2号无线收发器4后送至机载微控制器2用于控制四旋翼飞行器的电子调速器及电机，

半实物仿真数据记录模块1-6用于存储状态估计与滤波模块1-2输出的两组数据和四旋翼飞行器的四个电机的期望转速。

具体实施方式二：本实施方式与具体实施方式一所述的四旋翼飞行器半实物仿真方法的区别在于，所述的机载传感器5包括三轴电子罗盘5-1、六轴运动传感器5-2和超声测距传感器5-3，

三轴电子罗盘5-1用于采集三轴磁感应强度信号，

六轴运动传感器5-2用于采集三轴加速度读数和三轴方位读数，

超声测距传感器5-3用于采集测距读数。

具体实施方式三：本实施方式与具体实施方式二所述的四旋翼飞行器半实物仿真方法的区别在于，所述的三轴电子罗盘5-1的采用型号为HMC5883L的集成电路实现，六轴运动传感器5-2的型号为MPU6050的集成电路实现，超声测距传感器5-3采用型号为US-100的集成电路实现。

具体实施方式四：本实施方式与具体实施方式一、二或三所述的四旋翼飞行器半实物仿真方法的区别在于，所述的机载微控制器2采用TMS320F28335型DSP实现。

具体实施方式五：本实施方式与具体实施方式一、二或三所述的四旋翼飞行器半实物仿真方法的区别在于，所述的姿态与高度控制模块1-3的优化过程为，

在模型辨识子***中，参数辨识器接收四旋翼飞行器的四个电机的期望转速Ω₁,Ω₂,Ω₃,Ω₄、姿态欧拉角估计值高度估计值角速度的估计值和竖直方向上速度的估计值该参数辨识器根据待辨识模型结构，求得参数向量的辨识结果，进而确定了飞行器运动学与动力学模型，从而根据数值仿真子***完成对姿态与高度控制模块1-3的优化，

所述的待辨识模型结构通过下述的状态空间方程描述：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}=p\cos \mathrm{\θ}+r\sin \mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}=q-r\cos \mathrm{\θ}\tan \mathrm{\φ}+p\sin \mathrm{\θ}\tan \mathrm{\φ}\\ \stackrel{\·}{\mathrm{\ψ}}=r\cos \mathrm{\θ}\sec \mathrm{\φ}-p\sin \mathrm{\θ}\sec \mathrm{\φ}\\ \stackrel{\·}{p}=\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}\left(\frac{I_{\mathrm{yy}}-I_{\mathrm{zz}}}{I_{\mathrm{xx}}}\right)-\stackrel{\·}{\mathrm{\θ}}\frac{J_r}{I_{\mathrm{xx}}}{\mathrm{\Ω}}_r+\frac{l}{I_{\mathrm{xx}}}{U}_2\\ \stackrel{\·}{q}=\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\ψ}}\left(\frac{I_{\mathrm{zz}}-I_{\mathrm{xx}}}{I_{\mathrm{yy}}}\right)+\stackrel{\·}{\mathrm{\φ}}\frac{J_r}{I_{\mathrm{yy}}}{\mathrm{\Ω}}_r+\frac{l}{I_{\mathrm{yy}}}{U}_3\\ \stackrel{\·}{r}=\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\φ}}\left(\frac{I_{\mathrm{xx}}-I_{\mathrm{yy}}}{I_{\mathrm{zz}}}\right)+\frac{1}{I_{\mathrm{zz}}}{U}_4\\ \stackrel{\·}{z}=w\\ \stackrel{\·}{w}=g-\left(\cos \mathrm{\φ}\cos \mathrm{\θ}\right)\frac{1}{m}{U}_1\end{array}\right.$    (公式一)，

其中， $\left\{\begin{array}{c}{U}_1=b({{\mathrm{\Ω}}_1}^2+{\mathrm{\Ω}}_2^2+{\mathrm{\Ω}}_3^2+{\mathrm{\Ω}}_4^2)\\ U_2=b(-{\mathrm{\Ω}}_2^2+{\mathrm{\Ω}}_4^2)\\ U_3=b({{\mathrm{\Ω}}_1}^2-{\mathrm{\Ω}}_3^2)\\ U_4=d(-{{\mathrm{\Ω}}_1}^2+{\mathrm{\Ω}}_2^2-{\mathrm{\Ω}}_3^2+{\mathrm{\Ω}}_4^2)\\ {\mathrm{\Ω}}_r={\mathrm{\Ω}}_2+{\mathrm{\Ω}}_4-{\mathrm{\Ω}}_1-{\mathrm{\Ω}}_3\end{array}\right.$    (公式二)，

所述的参数辨识器根据待辨识模型结构，获得参数向量的辨识结果的具体过程为：

当四个电机的期望转速Ω₁,Ω₂,Ω₃,Ω₄、作为飞行器模型的输入时，通过公式一和公式二求得部分预测状态的数值解为ξ(k)＝[φ(k),θ(k),ψ(k),p(k),q(k),r(k),z(k),w(k)]^T，

其次，采用代价函数 $V_N\left(r\right)=\underset{k=1}{\overset{N}{\mathrm{\Σ}}}({\widehat{\mathrm{\ξ}}}^2\left(k\right)-{\mathrm{\ξ}}^2\left(k\right)),$ 求取r₀，

$r_0=\arg \underset{r}{\min }{V}_N\left(r\right),$

r₀即为参数向量的辨识结果，r为待辨识参数向量，其中，r＝[m,l,I_xx,I_yy,I_zz,J_r,b,d]^T，

待辨识参数向量r包括飞行器总质量m，相对电机轴距离l，机体转动惯量(I_xx,I_yy,I_zz)、旋翼与电机转子绕电机轴转动惯量J_r，升力系数b，扭矩系数d，N为记录的采样点数；k为采样时间点，U₁、U₂、U₃、U₄和Ω_r均为中间变量，g为重力加速度。

具体实施方式六：参见图2说明本实施方式，本实施方式所述的四旋翼飞行器的数值仿真方法，首先，参数辨识器接收从半实物仿真***获得数据信息，该参数辨识器根据待辨识模型结构，获得参数向量的辨识结果，并通过数值仿真子***根据获得的参数向量的辨识结果，获得飞行器运动学与动力学模型，从而完成四旋翼飞行器的数值仿真。

所述的参数辨识器接收从半实物仿真***获得数据信息包括四旋翼飞行器的四个电机的期望转速Ω₁,Ω₂,Ω₃,Ω₄、姿态欧拉角估计值高度估计值角速度的估计值和竖直方向上速度的估计值

所述的待辨识模型结构通过下述的状态空间方程描述：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}=p\cos \mathrm{\θ}+r\sin \mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}=q-r\cos \mathrm{\θ}\tan \mathrm{\φ}+p\sin \mathrm{\θ}\tan \mathrm{\φ}\\ \stackrel{\·}{\mathrm{\ψ}}=r\cos \mathrm{\θ}\sec \mathrm{\φ}-p\sin \mathrm{\θ}\sec \mathrm{\φ}\\ \stackrel{\·}{p}=\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}\left(\frac{I_{\mathrm{yy}}-I_{\mathrm{zz}}}{I_{\mathrm{xx}}}\right)-\stackrel{\·}{\mathrm{\θ}}\frac{J_r}{I_{\mathrm{xx}}}{\mathrm{\Ω}}_r+\frac{l}{I_{\mathrm{xx}}}{U}_2\\ \stackrel{\·}{q}=\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\ψ}}\left(\frac{I_{\mathrm{zz}}-I_{\mathrm{xx}}}{I_{\mathrm{yy}}}\right)+\stackrel{\·}{\mathrm{\φ}}\frac{J_r}{I_{\mathrm{yy}}}{\mathrm{\Ω}}_r+\frac{l}{I_{\mathrm{yy}}}{U}_3\\ \stackrel{\·}{r}=\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\φ}}\left(\frac{I_{\mathrm{xx}}-I_{\mathrm{yy}}}{I_{\mathrm{zz}}}\right)+\frac{1}{I_{\mathrm{zz}}}{U}_4\\ \stackrel{\·}{z}=w\\ \stackrel{\·}{w}=g-\left(\cos \mathrm{\φ}\cos \mathrm{\θ}\right)\frac{1}{m}{U}_1\end{array}\right.$    (公式一)，

其中， $\left\{\begin{array}{c}{U}_1=b({{\mathrm{\Ω}}_1}^2+{\mathrm{\Ω}}_2^2+{\mathrm{\Ω}}_3^2+{\mathrm{\Ω}}_4^2)\\ U_2=b(-{\mathrm{\Ω}}_2^2+{\mathrm{\Ω}}_4^2)\\ U_3=b({{\mathrm{\Ω}}_1}^2-{\mathrm{\Ω}}_3^2)\\ U_4=d(-{{\mathrm{\Ω}}_1}^2+{\mathrm{\Ω}}_2^2-{\mathrm{\Ω}}_3^2+{\mathrm{\Ω}}_4^2)\\ {\mathrm{\Ω}}_r={\mathrm{\Ω}}_2+{\mathrm{\Ω}}_4-{\mathrm{\Ω}}_1-{\mathrm{\Ω}}_3\end{array}\right.$    (公式二)，

所述的参数辨识器根据待辨识模型结构，获得参数向量的辨识结果的具体过程为：

当四个电机的期望转速Ω₁,Ω₂,Ω₃,Ω₄、作为飞行器模型的输入时，通过公式一和公式二求得部分预测状态的数值解为ξ(k)＝[φ(k),θ(k),ψ(k),p(k),q(k),r(k),z(k),w(k)]^T，

其次，采用代价函数 $V_N\left(r\right)=\underset{k=1}{\overset{N}{\mathrm{\Σ}}}({\widehat{\mathrm{\ξ}}}^2\left(k\right)-{\mathrm{\ξ}}^2\left(k\right)),$ 求取r₀，

$r_0=\arg \underset{r}{\min }{V}_N\left(r\right),$

r₀即为参数向量的辨识结果，r为待辨识参数向量，其中，r＝[m,l,I_xx,I_yy,I_zz,J_r,b,d]^T，

(φ,θ,ψ)为姿态欧拉角，z为高度，(p,q,r)为角速度，w为竖直方向上速度，

待辨识参数向量r包括飞行器总质量m，相对电机轴距离l，机体转动惯量(I_xx,I_yy,I_zz)、旋翼与电机转子绕电机轴转动惯量J_r，升力系数b，扭矩系数d，N为记录的采样点数；k为采样时间点，U₁、U₂、U₃、U₄和Ω_r均为中间变量，g为重力加速度。

本发明中仿真环境配置及操作步骤：

在开始进行半实物仿真之前，首先需要在MATLAB 2013b中完成以下配置：

1.安装实时内核

RTWT需要一个带有windows接口的实时内核，实时内核会将最高的执行优先级非配给实时可执行文件，使它能够在设定的采样频率下不受干扰地运行。使用rtwintgt–install命令即可安装实时内核。

2.设定外部模式代码生成参数

在建立Simulink模型之后就可以输入工Simulink Coder进行代码生成与建立实时应用程序的仿真参数了。在模型配置参数对话框中选择代码生成面板，在目标选择部分单击浏览按钮，在***目标文件列表里，选择用于建立Real-time Windows Target应用程序的***目标文件rtwin.tlc，这样对话框会自动把模板生成文件设定为rtwin.tmf、生成命令设定为make_rtw。在硬件实现面板中，设备供应商选择Generic，设备类型选择32位x86兼容，勾选“测试硬件与产品硬件相同”复选框。

3.设定示波器参数

在添加示波器模块到Simulink模型里之后就可以输入示波器参数用于信号跟踪了。打开示波器参数对话框，在时间范围文本框中输入时间范围的上界。右击坐标轴选择坐标轴属性，可以设定y轴的显示范围。

4.外部模式控制面板设置

在代码菜单下选择外部模式控制面板。单击数据归档按钮，勾选使能归档，则在外部模式下Simulink示波器和到工作空间模块可以将数据存储在硬盘上。

5.在线修改参数：第一种是双击需要修改参数的模块，修改参数后单击OK即可；第二种是在命令行中重新为变量赋值，按快捷键Ctrl+D便可以将修改下载到可执行文件中去。

用Simulink替代C语言为DSP编程：

利用Simulink中Embedded Coder和DSP System Toolbox工具箱，可实现与本型号数字信号处理器相匹配的交叉编译，即把Simulink搭建程序生成数字信号处理器可直接执行的.out文件，摆脱了繁杂的C语言编程，大大提高了开发效率。

利用Simulink的Embedded Coder自动生成C2000可执行的代码，首先需要配置编译环境，即将Simulink环境与CCS环境连接，具体步骤为：

1、首先安装以下程序：

MATLAB 2013B、CCS4.1.2、编译器(Compiler)CGT 5.2.3、链接器(linker)、实时操作***BIOS 5.41.02.14、DSP实时组件XDC Tools 3.16.02.32以及烧写工具FlashTools 2.10。

2、MATLAB实时内核配置：

在命令框中输入>>rtwintgt–setup

3、连接Simulink环境与CCS环境：

在命令框中输入>>checkEnvSetup('ccsv4','f28335','setup')，并且依次选择工具所在文件夹，即可完成连接。

4、Makefile编译配置：

在命令框中输入>>xmakefilesetup，并选择ticcs_c2000_ccsv4_clone代码生成环境。

在配置好编译环境后，搭建框图并进行交叉编译，具体步骤为：

1、进入Simulink环境利用DSP System Tools和Embedded Coder工具箱，搭建模型；

2、对仿真参数进行配置，将求解器(solver)选择为离散值定步长求解器。

3、进入代码生成(Code Generation)面板，选择ert.tlc为***目标文件、C为目标语言，目标硬件则选择TI Delfino F2833x(boot from flash)，生成过程的工具链选择CCSv4(C2000)，配置为快速生成以缩短编译链接时间。

4、成功生成代码之后，在CCS环境中将.out文件加载到目标中即可。

Claims (6)

1.四旋翼飞行器半实物仿真方法，其特征在于，它是基于下述装置实现的，该装置包括仿真计算机(1)、机载微控制器(2)、1号无线收发器(3)、2号无线收发器(4)和机载传感器(5)；

机载微控制器(2)的数据信号输入端与机载传感器(5)的数据信号输出端连接，机载微控制器(2)的通讯端与2号无线收发器(4)的信号输入输出端连接，2号无线收发器(4)通过无线通信的方式与1号无线收发器(3)进行数据传输，1号无线收发器(3)的信号输入输出端与仿真计算机(1)的信号输入输出端连接，

仿真计算机(1)内嵌入Matlab/Simulink仿真模块，所述的Matlab/Simulink仿真模块包括数据输入模块(1-1)、状态估计与滤波模块(1-2)、姿态与高度控制模块(1-3)、飞行状态显示模块(1-4)、数据输出模块(1-5)、半实物仿真数据记录模块(1-6)和控制指令生成模块(1-7)；

该半实物仿真方法的具体过程为：

首先，通过机载微控制器(2)采集机载传感器(5)输出的数据信号，且机载微控制器(2)依次通过2号无线收发器(4)、1号无线收发器(3)将采集到的数据信号送至仿真计算机(1)的数据输入模块(1-1)，该数据信号包括测距读数、三轴加速度读数、三轴角速度读数和三轴磁感应强度信号；

通过数据输入模块(1-1)接收输入的数据信号，并将该数据送至状态估计与滤波模块(1-2)，状态估计与滤波模块(1-2)根据接收的数据信号利用互补滤波的方法获得两组数据，该两组数据中的第一组数据为姿态欧拉角估计值和高度估计值，第二组数据为角速度的估计值和竖直方向上速度的估计值，

且将第一组数据送至飞行状态显示模块(1-4)，飞行状态显示模块(1-4)用于实时显示四旋翼飞行器的飞行状态，

将第一组和第二组数据送至姿态与高度控制模块(1-3)，姿态与高度控制模块(1-3)根据控制指令生成模块(1-7)产生的控制指令，计算获得姿态和高度控制信号，然后经过控制分配得到四旋翼飞行器的四个电机的期望转速，该四个电机的期望转速通过数据输出模块(1-5)依次经过1号无线收发器(3)、2号无线收发器(4)后送至机载微控制器(2)用于控制四旋翼飞行器的电子调速器及电机，

半实物仿真数据记录模块(1-6)用于存储状态估计与滤波模块(1-2)输出的两组数据和四旋翼飞行器的四个电机的期望转速。

2.根据权利要求1所述的四旋翼飞行器半实物仿真方法，其特征在于，所述的机载传感器(5)包括三轴电子罗盘(5-1)、六轴运动传感器(5-2)和超声测距传感器(5-3)，

三轴电子罗盘(5-1)用于采集三轴磁感应强度信号，

六轴运动传感器(5-2)用于采集三轴加速度读数和三轴角速度读数，

超声测距传感器(5)用于采集测距读数。

3.根据权利要求2所述的四旋翼飞行器半实物仿真方法，其特征在于，所述的三轴电子罗盘(5-1)的采用型号为HMC5883L的集成电路实现，六轴运动传感器(5-2)的型号为MPU6050的集成电路实现，超声测距传感器(5)采用型号为US-100的集成电路实现。

4.根据权利要求1、2或3所述的四旋翼飞行器半实物仿真方法，其特征在于，所述的机载微控制器(2)采用TMS320F28335型DSP实现。

5.根据权利要求1、2或3所述的四旋翼飞行器半实物仿真方法，其特征在于，所述的姿态与高度控制模块(1-3)的优化过程为，

在模型辨识子***中，参数辨识器接收四旋翼飞行器的四个电机的期望转速Ω₁,Ω₂,Ω₃,Ω₄、姿态欧拉角估计值高度估计值角速度的估计值和竖直方向上速度的估计值该参数辨识器根据待辨识模型结构，求得参数向量的辨识结果，进而确定了飞行器运动学与动力学模型，从而根据数值仿真子***完成对姿态与高度控制模块(1-3)的优化，

所述的待辨识模型结构通过下述的状态空间方程描述：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}=p\cos \mathrm{\θ}+r\sin \mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}=q-r\cos \mathrm{\θ}\tan \mathrm{\φ}+p\sin \mathrm{\θ}\tan \mathrm{\φ}\\ \stackrel{\·}{\mathrm{\ψ}}=r\cos \mathrm{\θ}\sec \mathrm{\φ}-p\sin \mathrm{\θ}\sec \mathrm{\φ}\\ \stackrel{\·}{p}=\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}\left(\frac{I_{\mathrm{yy}}-I_{\mathrm{zz}}}{I_{\mathrm{xx}}}\right)-\stackrel{\·}{\mathrm{\θ}}\frac{J_r}{I_{\mathrm{xx}}}{\mathrm{\Ω}}_r+\frac{l}{I_{\mathrm{xx}}}{U}_2\\ \stackrel{\·}{q}=\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\ψ}}\left(\frac{I_{\mathrm{zz}}-I_{\mathrm{xx}}}{I_{\mathrm{yy}}}\right)+\stackrel{\·}{\mathrm{\φ}}\frac{J_r}{I_{\mathrm{yy}}}{\mathrm{\Ω}}_r+\frac{l}{I_{\mathrm{yy}}}{U}_3\\ \stackrel{\·}{r}=\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\φ}}\left(\frac{I_{\mathrm{xx}}-I_{\mathrm{yy}}}{I_{\mathrm{zz}}}\right)+\frac{1}{I_{\mathrm{zz}}}{U}_4\\ \stackrel{\·}{z}=w\\ \stackrel{\·}{w}=g-\left(\cos \mathrm{\φ}\cos \mathrm{\θ}\right)\frac{1}{m}{U}_1\end{array}\right.$    (公式一)，

其中， $\left\{\begin{array}{c}{U}_1=b({{\mathrm{\Ω}}_1}^2+{\mathrm{\Ω}}_2^2+{\mathrm{\Ω}}_3^2+{\mathrm{\Ω}}_4^2)\\ U_2=b(-{\mathrm{\Ω}}_2^2+{\mathrm{\Ω}}_4^2)\\ U_3=b({{\mathrm{\Ω}}_1}^2-{\mathrm{\Ω}}_3^2)\\ U_4=d(-{{\mathrm{\Ω}}_1}^2+{\mathrm{\Ω}}_2^2-{\mathrm{\Ω}}_3^2+{\mathrm{\Ω}}_4^2)\\ {\mathrm{\Ω}}_r={\mathrm{\Ω}}_2+{\mathrm{\Ω}}_4-{\mathrm{\Ω}}_1-{\mathrm{\Ω}}_3\end{array}\right.$    (公式二)，

所述的参数辨识器根据待辨识模型结构，获得参数向量的辨识结果的具体过程为：

当四个电机的期望转速Ω₁,Ω₂,Ω₃,Ω₄、作为飞行器模型的输入时，通过公式一和公式二求得部分预测状态的数值解为ξ(k)＝[φ(k),θ(k),ψ(k),p(k),q(k),r(k),z(k),w(k)]^T，

其次，采用代价函数 $V_N\left(r\right)=\underset{k=1}{\overset{N}{\mathrm{\Σ}}}({\widehat{\mathrm{\ξ}}}^2\left(k\right)-{\mathrm{\ξ}}^2\left(k\right)),$ 求取r₀，

$r_0=\arg \underset{r}{\min }{V}_N\left(r\right),$

r₀即为参数向量的辨识结果，r为待辨识参数向量，其中，r＝[m,l,I_xx,I_yy,I_zz,J_r,b,d]^T，

(φ,θ,ψ)为姿态欧拉角，z为高度，(p,q,r)为角速度，w为竖直方向上速度，

待辨识参数向量r包括飞行器总质量m，相对电机轴距离l，机体转动惯量(I_xx,I_yy,I_zz)、旋翼与电机转子绕电机轴转动惯量J_r，升力系数b，扭矩系数d，N为记录的采样点数；k为采样时间点，U₁、U₂、U₃、U₄和Ω_r均为中间变量，g为重力加速度。

6.四旋翼飞行器的数值仿真方法，其特征在于，该方法的具体过程为，

首先，参数辨识器接收从半实物仿真***获得数据信息，该参数辨识器根据待辨识模型结构，获得参数向量的辨识结果，并通过数值仿真子***根据获得的参数向量的辨识结果，获得飞行器运动学与动力学模型，从而完成四旋翼飞行器的数值仿真。

所述的参数辨识器接收从半实物仿真***获得数据信息包括四旋翼飞行器的四个电机的期望转速Ω₁,Ω₂,Ω₃,Ω₄、姿态欧拉角估计值高度估计值角速度的估计值和竖直方向上速度的估计值

所述的待辨识模型结构通过下述的状态空间方程描述：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}=p\cos \mathrm{\θ}+r\sin \mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}=q-r\cos \mathrm{\θ}\tan \mathrm{\φ}+p\sin \mathrm{\θ}\tan \mathrm{\φ}\\ \stackrel{\·}{\mathrm{\ψ}}=r\cos \mathrm{\θ}\sec \mathrm{\φ}-p\sin \mathrm{\θ}\sec \mathrm{\φ}\\ \stackrel{\·}{p}=\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}\left(\frac{I_{\mathrm{yy}}-I_{\mathrm{zz}}}{I_{\mathrm{xx}}}\right)-\stackrel{\·}{\mathrm{\θ}}\frac{J_r}{I_{\mathrm{xx}}}{\mathrm{\Ω}}_r+\frac{l}{I_{\mathrm{xx}}}{U}_2\\ \stackrel{\·}{q}=\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\ψ}}\left(\frac{I_{\mathrm{zz}}-I_{\mathrm{xx}}}{I_{\mathrm{yy}}}\right)+\stackrel{\·}{\mathrm{\φ}}\frac{J_r}{I_{\mathrm{yy}}}{\mathrm{\Ω}}_r+\frac{l}{I_{\mathrm{yy}}}{U}_3\\ \stackrel{\·}{r}=\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\φ}}\left(\frac{I_{\mathrm{xx}}-I_{\mathrm{yy}}}{I_{\mathrm{zz}}}\right)+\frac{1}{I_{\mathrm{zz}}}{U}_4\\ \stackrel{\·}{z}=w\\ \stackrel{\·}{w}=g-\left(\cos \mathrm{\φ}\cos \mathrm{\θ}\right)\frac{1}{m}{U}_1\end{array}\right.$    (公式一)，

其中， $\left\{\begin{array}{c}{U}_1=b({{\mathrm{\Ω}}_1}^2+{\mathrm{\Ω}}_2^2+{\mathrm{\Ω}}_3^2+{\mathrm{\Ω}}_4^2)\\ U_2=b(-{\mathrm{\Ω}}_2^2+{\mathrm{\Ω}}_4^2)\\ U_3=b({{\mathrm{\Ω}}_1}^2-{\mathrm{\Ω}}_3^2)\\ U_4=d(-{{\mathrm{\Ω}}_1}^2+{\mathrm{\Ω}}_2^2-{\mathrm{\Ω}}_3^2+{\mathrm{\Ω}}_4^2)\\ {\mathrm{\Ω}}_r={\mathrm{\Ω}}_2+{\mathrm{\Ω}}_4-{\mathrm{\Ω}}_1-{\mathrm{\Ω}}_3\end{array}\right.$    (公式二)，

所述的参数辨识器根据待辨识模型结构，获得参数向量的辨识结果的具体过程为：

当四个电机的期望转速Ω₁,Ω₂,Ω₃,Ω₄、作为飞行器模型的输入时，通过公式一和公式二求得部分预测状态的数值解为ξ(k)＝[φ(k),θ(k),ψ(k),p(k),q(k),r(k),z(k),w(k)]^T，

其次，采用代价函数 $V_N\left(r\right)=\underset{k=1}{\overset{N}{\mathrm{\Σ}}}({\widehat{\mathrm{\ξ}}}^2\left(k\right)-{\mathrm{\ξ}}^2\left(k\right)),$ 求取r₀，

$r_0=\arg \underset{r}{\min }{V}_N\left(r\right),$

r₀即为参数向量的辨识结果，r为待辨识参数向量，其中，r＝[m,l,I_xx,I_yy,I_zz,J_r,b,d]^T，

待辨识参数向量r包括飞行器总质量m，相对电机轴距离l，机体转动惯量(I_xx,I_yy,I_zz)、旋翼与电机转子绕电机轴转动惯量J_r，升力系数b，扭矩系数d，N为记录的采样点数；k为采样时间点，U₁、U₂、U₃、U₄和Ω_r均为中间变量，g为重力加速度。