CN104932512A - 一种基于mimo非线性不确定反步法的四旋翼位姿控制方法 - Google Patents

Abstract

本发明提供一种基于MIMO非线性不确定反步法的四旋翼位姿控制方法，有助于提高四旋翼飞行器的飞行安全性和控制性能。所述方法包括：基于空气动力学和旋翼飞行器运动原理，并根据控制性能要求建立四旋翼飞行器数学模型；根据所述四旋翼飞行器数学模型和精度要求，建立四旋翼飞行器模型；根据所述四旋翼飞行器数学模型和四旋翼飞行器模型的不确定性、外界干扰、欠驱动、强耦合特性，对所述四旋翼飞行器模型进行分析解耦，确定基于内外环控制的反步鲁棒补偿控制器。本发明适用于自动化控制技术领域。

Description

一种基于MIMO非线性不确定反步法的四旋翼位姿控制方法

技术领域

本发明涉及自动化控制技术领域，特别是指一种基于MIMO非线性不确定反步法的四旋翼位姿控制方法。

背景技术

固定翼飞行器载重能力强，飞行速度快，适合长航时的大面积巡航任务，旋翼飞行器可执行垂直起降，前后侧飞，悬停等高难度任务，具有机动能力强，位姿跟踪能力出色等特点。正是由于在工作方式上具有优势互补的特点，在特定的应用环境中，旋翼飞行器可以完成许多固定翼飞行器难以完成的任务，如地面目标跟踪，电力巡线、环境勘测等，具有重大的实用价值。因此，对旋翼飞行器进行深入的研究是很有必要的。

四旋翼飞行器是除了传统直升机外，旋翼类飞行器的又一典型代表，直升机的机械结构包括主桨和尾桨，尾桨用于抵消主桨旋转产生的附加力矩，控制较复杂，四旋翼利用分布在机体周围的四个旋翼提供升力，具有旋翼挥舞面积小、转速低、安全系数高等特点。四旋翼的飞行方式灵活多变，机动能力较强，但由其运动原理决定的欠驱动、强耦合、非线性等特点，使控制器的设计工作变得较为困难。

旋翼类飞行器是非自稳系统，如果不对其加以控制，在外界或自身扰动的作用下，旋翼类飞行器是不稳定的，会出现失控(如坠机、角度震荡发散等)情况。旋翼类飞行器的控制器一般是利用反馈信息和参考信号之间的偏差作为输入，按照设计的控制律计算控制器输出，通常也是被控对象的执行机构输入，当控制律表征的输入输出关系合理时，执行机构的动作会抑制旋翼类飞行器的不稳定运动趋势。

目前，工程上广泛采用常规的PID控制方法设计的常规控制器，该常规控制器对四旋翼飞行器模型的精度无太大要求，忽略四旋翼飞行器模型中不确定项的影响，然而这种近似只在四旋翼处于近悬停状态成立，当四旋翼进行大机动飞行时，常规控制器性能急剧变差，存在潜在的安全隐患；同时，常规控制器仅为控制系统输出提供反馈通道，控制律设计过程简单，控制精度低和控制性能差，难以应对飞行过程中的强耦合、四旋翼飞行器模型的不确定性和外界干扰，如：四旋翼执行航迹跟踪任务时，常规的PID控制器，难以保证轨迹跟踪精度高、响应快速和抗干扰能力强等特性同时得到满足。

发明内容

本发明要解决的技术问题是提供一种基于MIMO非线性不确定反步法的四旋翼位姿控制方法，以解决现有技术所存在的采用常规的PID控制方法难以应对飞行过程中的强耦合、四旋翼飞行器模型的不确定性和外界干扰的问题。

为解决上述技术问题，本发明实施例提供一种基于MIMO非线性不确定反步法的四旋翼位姿控制方法，包括:

基于空气动力学和旋翼飞行器运动原理，并根据控制性能要求建立四旋翼飞行器数学模型；

根据所述四旋翼飞行器数学模型和精度要求，建立四旋翼飞行器模型；

根据所述四旋翼飞行器数学模型和四旋翼飞行器模型的不确定性、外界干扰、欠驱动、强耦合特性，确定基于内外环控制的反步鲁棒补偿控制器。

可选地，所述根据所述四旋翼飞行器数学模型和精度要求，建立四旋翼飞行器模型包括：

对四旋翼飞行器进行受力分析和运动学分析，根据所述四旋翼飞行器数学模型和精度要求，通过机理建模确定四旋翼飞行器模型的结构；

根据四旋翼飞行器模型的结构，通过系统辨识、测量方式确定四旋翼飞行器模型的参数，其中，辨识对象为实践工程中的四旋翼飞行器，辨识的内容包括：转动惯量、旋翼臂长、模型阶次；

对四旋翼飞行器模型的结构和参数进行验证；

将所述四旋翼飞行器模型用确定性和不确定性两个部分表示，并结合实践工程，减小不确定性部分所占的比重。

可选地，所述四旋翼飞行器数学模型包括：表示四旋翼飞行器在地面坐标系中位置的位置方程组和表示四旋翼飞行器在地面坐标系中姿态的姿态方程组；

所述位置方程组表示如下：

$\left[\begin{array}{c}\stackrel{\·\·}{x}\\ \stackrel{\·\·}{y}\\ \stackrel{\·\·}{z}\end{array}\right]=\left[\begin{array}{c}\cos \mathrm{\φ}*\sin \mathrm{\θ}*\cos \mathrm{\ψ}+\sin \mathrm{\φ}*\sin \mathrm{\ψ}\\ \cos \mathrm{\φ}*\sin \mathrm{\θ}*\sin \mathrm{\ψ}-\sin \mathrm{\φ}*\cos \mathrm{\ψ}\\ \cos \mathrm{\φ}*\cos \mathrm{\θ}\end{array}\right]*\frac{u_1}{m}-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]+\left[\begin{array}{c}{\mathrm{\Δ}}_1(m,c_x,\stackrel{\·}{x})\\ {\mathrm{\Δ}}_2(m,c_y,\stackrel{\·}{y})\\ {\mathrm{\Δ}}_2(m,c_z,\stackrel{\·}{z})\end{array}\right]$

其中，不确定项 ${\mathrm{\Δ}}_1(m,c_x,\stackrel{\·}{x})=-c_x*{\stackrel{\·}{x}}^2/m,$ ${\mathrm{\Δ}}_2(m,c_y,\stackrel{\·}{y})=-c_y*{\stackrel{\·}{y}}^2/m,$ φ,θ,ψ表示欧拉角，u₁表示旋翼升力，m表示四旋翼质量，g表示重力加速度，c_x,c_y,c_z表示空气阻力系数，x,y,z表示四旋翼飞行器的空间位置，分别表示x,y,z的导数；分别表示的导数；

所述姿态方程组表示如下：

$\left[\begin{array}{c}\stackrel{\·\·}{\mathrm{\φ}}\\ \stackrel{\·\·}{\mathrm{\θ}}\\ \stackrel{\·\·}{\mathrm{\ψ}}\end{array}\right]=\left[\begin{array}{c}{u}_2/J_x\\ u_3/J_y\\ u_4/J_z\end{array}\right]+\left[\begin{array}{c}\stackrel{\·}{\mathrm{\θ}}*\stackrel{\·}{\mathrm{\ψ}}*(J_y-J_z)/J_x\\ \stackrel{\·}{\mathrm{\ψ}}*\stackrel{\·}{\mathrm{\φ}}*(J_z-J_x)/J_y\\ \stackrel{\·}{\mathrm{\φ}}*\stackrel{\·}{\mathrm{\θ}}*(J_x-J_y)/J_z\end{array}\right]+\left[\begin{array}{c}{\mathrm{\Δ}}_4(c_p,\stackrel{\·}{\mathrm{\φ}},\stackrel{\·}{\mathrm{\θ}},J_r,J_x,W)\\ {\mathrm{\Δ}}_5(c_q,\stackrel{\·}{\mathrm{\φ}},\stackrel{\·}{\mathrm{\θ}},J_r,J_y,W)\\ {\mathrm{\Δ}}_6(c_r,\stackrel{\·}{\mathrm{\ψ}},J_z)\end{array}\right]$

其中，不确定项 ${\mathrm{\Δ}}_4(c_p,\stackrel{\·}{\mathrm{\φ}},\stackrel{\·}{\mathrm{\θ}},J_r,J_x,W)=J_r*\stackrel{\·}{\mathrm{\θ}}/J_x*{\mathrm{\Σ}}_{i=1}^4{(-1)}^i*w_i-c_p*\stackrel{\·}{\mathrm{\φ}}/J_x,$ ${\mathrm{\Δ}}_5(c_q,\stackrel{\·}{\mathrm{\φ}},\stackrel{\·}{\mathrm{\θ}},J_r,J_y,W)=-J_r*\stackrel{\·}{\mathrm{\φ}}/J_y*{\mathrm{\Σ}}_{=1}^4{(-1)}^i*w_i-c_q*\stackrel{\·}{\mathrm{\θ}}/J_y,{\mathrm{\Δ}}_6(c_r,\stackrel{\·}{\mathrm{\ψ}},J_z)=-c_r*\stackrel{\·}{\mathrm{\ψ}}/J_z,$ u₂,u₃,u₄表示三轴转矩，J_x，J_y，J_z表示转动惯量矩阵J的对角线元素J_x，J_y，J_z，分别表示φ,θ,ψ的导数，分别表示的导数，W表示电机转速，J_r表示电机转动惯量，c_p,c_q,c_r表示转矩阻力系数。

可选地，所述反步鲁棒补偿控制器采用内外环控制，内环姿态参考信号通过外环控制律确定；

所述反步鲁棒补偿控制器包括：反步控制器和鲁棒补偿控制器；

通过反步控制器控制四旋翼飞行器模型状态变量的理想运动轨迹；

通过鲁棒补偿控制器对四旋翼飞行器模型误差进行补偿，所述鲁棒补偿控制器的跟踪性能和频率特性由控制性能要求和系统特性确定。

可选地，所述根据所述四旋翼飞行器数学模型和四旋翼飞行器模型的不确定性、外界干扰、欠驱动、强耦合特性，对所述四旋翼飞行器模型进行分析解耦，确定基于内外环控制的反步鲁棒补偿控制器包括：

对所述四旋翼飞行器数学模型进行整合分析，确定四旋翼飞行器数学模型的状态空间表示形式；

根据四旋翼飞行器数学模型的状态空间表示形式，结合四旋翼飞行器模型的不确定性，外界干扰，控制性能要求，四旋翼飞行器的欠驱动、强耦合特性，位置、姿态不同的响应特性，并基于内外环控制思想，对四旋翼飞行器的位置、偏航控制解耦成单通道的位姿控制，再对单通道的二阶子系统进行反步鲁棒补偿控制律设计，对四旋翼飞行器模型的不确定性和外界干扰进行补偿。

可选地，所述四旋翼飞行器数学模型的状态空间表示形式如下：

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_{11}=x_{12},{\stackrel{\·}{x}}_{12}=\left(\cos \right(x_{13})*\sin \left(x_{23}\right)*\cos \left(x_{41}\right)+\sin \left(x_{13}\right)*\sin \left(x_{41}\right))*\frac{u_1}{m}+{\mathrm{\Δ}}_1\\ {\stackrel{\·}{x}}_{13}=x_{14},{\stackrel{\·}{x}}_{14}=\frac{J_y-J_1}{J_x}*x_{24}*x_{42}+\frac{u_2}{J_x}+{\mathrm{\Δ}}_2\\ {\stackrel{\·}{x}}_{21}=x_{22},{\stackrel{\·}{x}}_{22}=\left(\cos \right(x_{13})*\sin \left(x_{23}\right)*\sin \left(x_{41}\right)-\sin \left(x_{13}\right)*\cos \left(x_{41}\right))*\frac{u_1}{m}+{\mathrm{\Δ}}_2\\ {\stackrel{\·}{x}}_{23}=x_{24},{\stackrel{\·}{x}}_{24}=\frac{J_z-J_x}{J_y}*x_{42}*x_{14}+\frac{u_3}{J_y}+{\mathrm{\Δ}}_5\\ {\stackrel{\·}{x}}_{31}=x_{32},{\stackrel{\·}{x}}_{32}=\cos \left(x_{13}\right)*\cos \left(x_{23}\right)*\frac{u_1}{m}-g+{\mathrm{\Δ}}_3\\ {\stackrel{\·}{x}}_{41}=x_{42},{\stackrel{\·}{x}}_{42}=\frac{J_x-J_y}{J_z}*x_{14}*x_{24}+\frac{u_4}{J_z}+{\mathrm{\Δ}}_6\end{array}\right.$

其中，x₁₁,x₂₁,x₃₁与空间位置x,y,z一一对应，x₁₃,x₂₃,x₄₁与欧拉角φ,θ,ψ一一对应，x₁₂,x₂₂,x₃₂与速度v_x,v_y,v_z一一对应，x₁₄,x₂₄,x₄₂与欧拉角一一对应，△_i(i＝1,2,…6)为不确定项△_i(*)的简写。

可选地，所述对四旋翼飞行器的位置、偏航控制解耦成单通道的位姿控制，再对单通道的二阶子系统进行反步鲁棒补偿控制律设计，对四旋翼飞行器模型的不确定性和外界干扰进行补偿包括：

z通道的控制输入为：u₁＝m*(-α₃₂*e₃₂+f₃₂*w₃₂+g)/(cos(x₁₃)*cos(x₂₃))

其中， $e_{32}=x_{32}-{\hat{x}}_{32},{\hat{x}}_{32}=-{\mathrm{\α}}_{31}*e_{31}+{\stackrel{\·}{z}}_d,e_{31}=x_{31}-z_d,w_{32}=-(1+\frac{{\mathrm{\α}}_{32}}{s})*e_{32},$ z_d为z方向的参考信号，为z_d的导数，为虚拟输入，α₃₁和α₃₂表示反步鲁棒补偿控制器的调节参数；

偏航通道的控制律为：u₄＝J_z*(-α₄₂*e₄₂+f₄₂*w₄₂)-(J_x-J_y)*x₁₄*x₂₄

其中， $e_{42}=x_{42}-{\hat{x}}_{42},{\hat{x}}_{42}=-{\mathrm{\α}}_{41}*e_{41}+{\stackrel{\·}{\mathrm{\ψ}}}_d,e_{41}=x_{41}-{\mathrm{\ψ}}_d,w_{42}=-(1+\frac{{\mathrm{\α}}_{42}}{s})*e_{42},$ ψ_d为ψ角度的参考信号，为ψ_d的导数，为虚拟输入，α₄₁和α₄₂表示反步鲁棒补偿控制器的调节参数；

滚转通道的控制律为：u₂＝J_x*(-α₁₄*e₁₄+f₁₄*w₁₄)-(J_y-J_z)*x₂₄*x₄₂

其中， $e_{14}=x_{14}-{\hat{x}}_{14},{\hat{x}}_{14}=-{\mathrm{\α}}_{13}*e_{13}+{\stackrel{\·}{\mathrm{\φ}}}_d,e_{13}=x_{13}-{\mathrm{\φ}}_d,w_{14}=-(1+\frac{{\mathrm{\α}}_{14}}{s})*e_{14},$ ${\mathrm{\φ}}_d=arcsin(\frac{m}{u_1}*((-{\mathrm{\α}}_{12}*e_{12}+f_{12}*w_{12})*sin\left(x_{41}\right)-(-{\mathrm{\α}}_{22}*e_{22}+f_{22}*w_{22})*cos\left(x_{41}\right)\left)\right),$ φ_d为滚转角的参考信号，α₁₂、f₁₂、α₂₂和f₂₂为反步鲁棒补偿控制器的调节参数， $w_{12}=-\frac{\mathrm{\Δ}12}{s+f_{12}}=-(1+\frac{1}{s})*e_{12},w_{22}=-\frac{\mathrm{\Δ}22}{s+f_{22}}=-(1+\frac{1}{s})*e_{22},e_{12}=x_{12}-{\hat{x}}_{12},$ 为虚拟输入， 为虚拟输入，为φ_d的导数，为虚拟输入，α₁₃和α₁₄表示反步鲁棒补偿控制器的调节参数；

俯仰通道的控制律为：u₃＝J_y*(-α₂₄*e₂₄+f₂₄*w₂₄)-(J_z-J_x)*x₁₄*x₄₂

其中， $e_{24}=x_{24}-{\hat{x}}_{24},{\hat{x}}_{24}=-{\mathrm{\α}}_{23}*e_{23}+{\stackrel{\·}{\mathrm{\θ}}}_d,e_{23}=x_{23}-{\mathrm{\θ}}_d,w_{24}=-(1+\frac{{\mathrm{\α}}_{24}}{s})*e_{24},$ ${\mathrm{\θ}}_d=arcsin(\frac{m}{u_1*cos\left(x_{13}\right)}*((-{\mathrm{\α}}_{12}*e_{12}+f_{12}*w_{12})*cos\left(x_{41}\right)+(-{\mathrm{\α}}_{22}*e_{22}+f_{22}*w_{22})*sin\left(x_{41}\right)\left)\right),$ θ_d为俯仰角的参考信号，α₁₂、f₁₂、α₂₂和f₂₂为反步鲁棒补偿控制器的调节参数， $w_{12}=-\frac{{\mathrm{\Δ}}_{12}}{s+f_{12}}=-(1+\frac{1}{s})*e_{12},w_{22}=-\frac{{\mathrm{\Δ}}_{22}}{s+f_{22}}=-(1+\frac{1}{s})*e_{22},e_{12}=x_{12}-{\hat{x}}_{12},$ 为虚拟输入， 为虚拟输入，为θ_d的导数，为虚拟输入，α₂₃和α₂₄表示反步鲁棒补偿控制器的调节参数。

可选地，所述方法还包括：在实践工程中，对四旋翼飞行器机载的传感器采集的四旋翼飞行器的飞行信息进行滤波融合处理；

所述对四旋翼飞行器机载的传感器采集的四旋翼飞行器的飞行信息进行滤波融合处理包括：

通过四旋翼飞行器机载的传感器采集四旋翼飞行器的飞行信息；

通过低通滤波器对采集的四旋翼飞行器的飞行信息进行低通滤波处理，其中，所述低通滤波器是结合实践工程中的四旋翼飞行器确定的；

通过卡尔曼滤波器冗余机制对不同传感器采集的隐含同类型的经过低通滤波处理后的飞行信息进行数据融合，得到目标观测信息；

其中，数据融合包括：陀螺仪传感器、加速度计传感器，磁力计传感器、GPS传感器、超声波传感器、气压计传感器采集的并经过低通滤波后的信息，及建立在此基础上的角速度、欧拉角内环信息、速度、位置外环信息。

可选地，所述方法还包括：

在实践工程中，对确定的反步鲁棒补偿控制算法进行离散化处理，离散化程度由反步鲁棒补偿控制器的控制性能和传感器的采样频率决定。

可选地，所述根据所述四旋翼飞行器数学模型和四旋翼飞行器模型的不确定性、外界干扰、欠驱动、强耦合特性，对所述四旋翼飞行器模型进行分析解耦，确定基于内外环控制的反步鲁棒补偿控制器之后包括：

在理论研究阶段，根据确定的反步鲁棒补偿控制器，建立四旋翼飞行器仿真模型，并调节反步鲁棒补偿控制器的调节参数，直至控制性能满足控制性能要求，对基于反步鲁棒补偿控制器的四旋翼飞行器仿真模型进行初步验证，其中，所述控制性能包括：响应时间、超调、抗干扰能力；

在实践工程中，对基于反步鲁棒补偿控制器的四旋翼飞行器的实际飞行效果进行后期验证。

本发明的上述技术方案的有益效果如下：

上述方案中，通过空气动力学和旋翼飞行器运动原理，并根据控制性能要求建立四旋翼飞行器的数学模型，再根据所述四旋翼飞行器的数学模型和精度要求，确定四旋翼飞行器模型，最后，根据所述四旋翼飞行器数学模型和四旋翼飞行器模型的不确定性、外界干扰、欠驱动、强耦合特性，对所述四旋翼飞行器模型进行分析解耦，确定基于内外环控制的反步鲁棒补偿控制器。这样，对四旋翼飞行器模型进行分析解耦为确定基于内外环控制的反步鲁棒补偿控制器服务，能够避免常规近似处理带来的控制性能恶化问题，并采用基于内外环控制的反步鲁棒补偿控制器来补偿四旋翼飞行器模型的欠驱动、强耦合、不确定性和外界干扰对飞行过程造成的干扰，改善四旋翼飞行器模型的控制精度和控制性能，从而提高了四旋翼飞行器模型的飞行安全性和可操控性。

附图说明

图1为本发明实施例提供的基于MIMO非线性不确定反步法的四旋翼位姿控制方法流程图；

图2为本发明实施例提供的基于反步鲁棒补偿控制方法的控制系统结构示意图；

图3为本发明实施例提供的四旋翼飞行器模型解耦后的单通道反步鲁棒补偿控制律的二阶子系统结构示意图；

图4为本发明实施例提供的传感器采集到的冗余信息进行数据融合的示意图；

图5为图5中卡尔曼滤波器原理框图；

图6为本发明实施例提供的基于反步鲁棒补偿控制方法的simulink平台仿真框图；

图7为本发明实施例提供的验证基于反步鲁棒补偿控制方法的有效性的方法流程图；

图8为本发明实施例提供的仿真中采用阶跃输入时的位置响应交叉验证效果图；

图9为本发明实施例提供的仿真中采用内外环控制结构的姿态响应多通道交叉验证效果图；

图10为本发明实施例提供的位置跟踪飞行测试时，单通道位置跟踪飞行测试效果图；

图11为本发明实施例提供的姿态跟踪飞行测试时，单通道偏航角跟踪飞行测试时效果图；

图12为本发明实施例提供的室外飞行试验中进行轨迹跟踪的效果图。

具体实施方式

为使本发明要解决的技术问题、技术方案和优点更加清楚，下面将结合附图及具体实施例进行详细描述。

本发明针对现有的采用常规的PID控制方法难以应对飞行过程中的强耦合、四旋翼飞行器模型的不确定性和外界干扰的问题，提供一种基于MIMO非线性不确定反步法的四旋翼位姿控制方法。

实施例一

参看图1所示，本发明实施例提供的一种基于MIMO非线性不确定反步法的四旋翼位姿控制方法，包括:

S1：基于空气动力学和旋翼飞行器运动原理，并根据控制性能要求建立四旋翼飞行器数学模型；

S2：根据所述四旋翼飞行器数学模型和精度要求，建立四旋翼飞行器模型；

S3：根据所述四旋翼飞行器数学模型和四旋翼飞行器模型的不确定性、外界干扰、欠驱动、强耦合特性，对所述四旋翼飞行器模型进行分析解耦，确定基于内外环控制的反步鲁棒补偿控制器。

本发明实施例所述的基于多变量输入多变量输出(Multiple-InputMultiple-Output，MIMO)非线性不确定反步法的四旋翼位姿控制方法，通过空气动力学和旋翼飞行器运动原理，并根据控制性能要求建立四旋翼飞行器的数学模型，再根据所述四旋翼飞行器的数学模型和精度要求，确定四旋翼飞行器模型，最后，根据所述四旋翼飞行器数学模型和四旋翼飞行器模型的不确定性、外界干扰、欠驱动、强耦合特性，对所述四旋翼飞行器模型进行分析解耦，确定基于内外环控制的反步鲁棒补偿控制器。这样，对四旋翼飞行器模型进行分析解耦为确定基于内外环控制的反步鲁棒补偿控制器服务，能够避免常规近似处理带来的控制性能恶化问题，并采用基于内外环控制的反步鲁棒补偿控制器来补偿四旋翼飞行器模型的欠驱动、强耦合、不确定性和外界干扰对飞行过程造成的干扰，改善四旋翼飞行器模型的控制精度和控制性能，从而提高了四旋翼飞行器模型的飞行安全性和可操控性。

在前述基于MIMO非线性不确定反步法的四旋翼位姿控制方法的具体实施方式中，可选地，所述根据所述四旋翼飞行器数学模型和精度要求，建立四旋翼飞行器模型包括：

对四旋翼飞行器进行受力分析和运动学分析，根据所述四旋翼飞行器数学模型和精度要求，通过机理建模确定四旋翼飞行器模型的结构；

根据四旋翼飞行器模型的结构，通过系统辨识、测量方式确定四旋翼飞行器模型的参数，其中，辨识对象为实践工程中的四旋翼飞行器，辨识的内容包括：转动惯量、旋翼臂长、模型阶次；

对四旋翼飞行器模型的结构和参数进行验证；

将所述四旋翼飞行器模型用确定性和不确定性两个部分表示，并结合实践工程，减小不确定性部分所占的比重。

本发明实施例中，四旋翼飞行器模型的精度(简称模型精度)，直接影响后续的模型特性分析(包括：欠驱动、强耦合等特性)和反步鲁棒补偿控制器的设计，影响模型精度的因素主要有两个：四旋翼飞行器模型的结构和模型参数，在提高模型精度时，要兼顾模型描述的准确性和复杂度，考虑过多输入的复杂模型，对反步鲁棒补偿控制器性能提升的帮助有限。为了方便描述，将四旋翼飞行器模型的结构简称为模型结构，四旋翼飞行器模型的参数简称为模型参数。

本发明实施例中，为了更好的理解模型结构和模型参数，先对模型结构和模型参数的相关知识进行简单说明：

模型结构从本质上反映了模型的特点，是影响模型精度的主要因素，模型结构直接影响后续设计反步鲁棒补偿控制器的全局性能。模型参数是影响模型精度的又一重要因素，模型参数以确定的模型结构为前提，脱离结构讨论模型参数没有实际意义，当模型结构确定后，模型参数在模型类中对四旋翼飞行器的特有细节特征进行定位，与模型结构一起，对四旋翼飞行器的属性进行完整描述。

一般而言，可以通过机理建模和系统辨识的方式确定模型结构，机理建模基本能在全局范围内准确地描述系统特性，但需要丰富的先验知识和扎实的理论基础，系统辨识得到的模型结构简单，且辨识过程可以通过不同的辨识工具包完成，基本上能在系统常规工作模式下较客观地描述系统特性，但辨识结果通常具有一定的局限性。

一般而言，通过直接测量和辨识的方式获得模型参数，前者适用于直观的能直接进行测量的参数对象，如本发明中的旋翼中心到四旋翼飞行器质心距离(臂长L)；难以用简单直接的方式进行辨识的参数对象，通常需要根据参数特性，设计具有实际可行性的实验，借助能直接测量的观测量，反向推算出待辨识参数值的大小，如本发明中四旋翼飞行器的三轴转动惯量、电机转动惯量等，可以分别通过设计三悬线转动惯量测量，电机结构简单拆解分析等实验，得到系统未知的待辨识参数。

本发明实施例中，在建模过程中，为保证模型精度，对四旋翼飞行器进行受力分析和运动学分析，并采用机理建模的方式确定模型结构，确保模型状态在大范围内全局一致有效；通过设计实验，并借助Matlab辨识工具箱完成识别模型参数，并通过不同环境中获得的数据对建立的四旋翼飞行器模型进行反复验证。待辨识参数中，由于选用的电调、电机和旋翼特性对用户不透明，而在实践验证环节，搭建的四旋翼飞行器硬件平台不太可能出现较大的变动，所以本发明在对旋翼升力系数和阻力系数进行辨识时，并不是通过寻找旋翼转速与升力间关系的方式进行，为减小不必要的工作量，且不影响模型的完整性，本发明直接辨识执行机构控制信号与旋翼升力之间的关系，而尽量不关心每个串联执行机构间的输入输出特性。本发明通过执行机构控制信号与旋翼升力的映射实验，得到旋翼系统升力与脉冲宽度调制(Pulse Width Modulation，PWM)、电池电压等输入信号之间的关系，并在反步鲁棒补偿控制器部分通过反变换体现了这种映射关系。

本发明实施例中，还将所述四旋翼飞行器模型用确定性和不确定性两个部分表示，并结合实践工程，减小不确定性部分所占的比重，为下一步的控制律设计工作做准备，这样就避免了常规控制方法中对模型的简化，从而引起设计的反步鲁棒补偿控制器性能与实际飞行效果存在差异，通常这种差异在大机动飞行时非常明显，甚至导致飞行器失稳。本发明实施例中，基于空气动力学和飞行器运动原理，并根据控制性能要求，对四旋翼飞行器进行受力分析，并结合运动学相关知识，通过如下步骤确定四旋翼飞行器数学模型，所述四旋翼飞行器数学模型包括：表示四旋翼飞行器在地面坐标系中位置的位置方程组和表示四旋翼飞行器在地面坐标系中姿态的姿态方程组。

在惯性坐标系(地面坐标系)中，四旋翼飞行器数学模型方程式满足式(1)：

$m*\left[\begin{array}{c}\stackrel{\·\·}{x}\\ \stackrel{\·\·}{y}\\ \stackrel{\·\·}{z}\end{array}\right]=\left[\begin{array}{c}\cos \mathrm{\φ}*\sin \mathrm{\θ}*\cos \mathrm{\ψ}+\sin \mathrm{\φ}*\sin \mathrm{\ψ}\\ \cos \mathrm{\φ}*\sin \mathrm{\θ}*\sin \mathrm{\ψ}-\sin \mathrm{\φ}*\cos \mathrm{\ψ}\\ \cos \mathrm{\φ}*\cos \mathrm{\θ}\end{array}\right]*u_1-m*\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]-\left[\begin{array}{c}{c}_x*{\stackrel{\·}{x}}^2\\ c_y*{\stackrel{\·}{y}}^2\\ c_z*{\stackrel{\·}{z}}^2\end{array}\right]---\left(1\right)$

式(1)中，φ,θ,ψ表示欧拉角，u₁表示旋翼升力，m表示四旋翼质量，g表示重力加速度，c_x,c_y,c_z表示空气阻力系数，x,y,z表示四旋翼飞行器的空间位置，分别表示x,y,z的导数；分别表示的导数。

在机体坐标系(NED)中，四旋翼飞行器数学模型方程式满足式(2)：

$J*\left[\begin{array}{c}\stackrel{\·}{p}\\ \stackrel{\·}{q}\\ \stackrel{\·}{r}\end{array}\right]=\left[\begin{array}{c}{u}_2\\ u_3\\ u_4\end{array}\right]-\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\×(J*\left[\begin{array}{c}p\\ q\\ r\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ {\mathrm{\Σ}}_{i=1}^4{(-1)}^{i+1}*w_i\end{array}\right]*J_r)-\left[\begin{array}{c}{c}_p*p\\ c_q*q\\ c_r*r\end{array}\right]---\left(2\right)$

式(2)中，J表示3*3的机体转动惯量矩阵，u₂,u₃,u₄表示三轴转矩，分别表示φ,θ,ψ的导数，分别表示的导数，w_i表示第i电机的转速，J_r表示电机转动惯量，c_p,c_q,c_r表示转矩阻力系数。

直观上讲，惯性坐标系是无人飞行器的习惯坐标系，所以需要将在机体坐标系中建立的模型方程转化到特殊的惯性坐标系中，通过变换过程可知，机体角速度与欧拉角的对应关系为式(3)：

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}\\ \stackrel{\·}{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\ψ}}\end{array}\right]=\left(\begin{array}{ccc}1& sin\mathrm{\φ}*tan\mathrm{\θ}& cos\mathrm{\φ}*tan\mathrm{\θ}\\ 0& cos\mathrm{\φ}& -sin\mathrm{\φ}\\ 0& \sin \mathrm{\φ}/\cos \mathrm{\θ}& cos\mathrm{\φ}/\cos \mathrm{\θ}\end{array}\right)*\left[\begin{array}{c}p\\ q\\ r\end{array}\right]---\left(3\right)$

将式(2)、式(3)两式联合，可得式(4)：

$\left[\begin{array}{c}\stackrel{\·\·}{\mathrm{\φ}}\\ \stackrel{\·\·}{\mathrm{\θ}}\\ \stackrel{\·\·}{\mathrm{\ψ}}\end{array}\right]=\stackrel{.}{T}*T^{-1}*\left[\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}\\ \stackrel{\·}{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\ψ}}\end{array}\right]+T*J^{-1}*(\left[\begin{array}{c}{u}_2\\ u_3\\ u_4\end{array}\right]-T^{-1}*\left[\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}\\ \stackrel{\·}{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\ψ}}\end{array}\right]\×(J*T^{-1}*\left[\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}\\ \stackrel{\·}{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\ψ}}\end{array}\right]+J_r*\left[\begin{array}{c}0\\ 0\\ {\Σ}_{i=1}^4{(-1)}^{i+1}*w_i\end{array}\right])-\left(\begin{array}{ccc}{c}_p& 0& 0\\ 0& c_q& 0\\ 0& 0& c_r\end{array}\right)*T^{-1}*\left[\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}\\ \stackrel{\·}{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\ψ}}\end{array}\right])---\left(4\right)$

式(3)中，T、T^-1分别为下述矩阵：

$T=\left(\begin{array}{ccc}1& sin\mathrm{\φ}*tan\mathrm{\θ}& cos\mathrm{\φ}*tan\mathrm{\θ}\\ 0& cos\mathrm{\φ}& -\sin \mathrm{\φ}\\ 0& sin\mathrm{\φ}/\cos \mathrm{\θ}& cos\mathrm{\φ}/\cos \mathrm{\θ}\end{array}\right)$

$T^{-1}=\left(\begin{array}{ccc}1& 0& -sin\mathrm{\θ}\\ 0& cos\mathrm{\φ}& sin\mathrm{\φ}*cos\mathrm{\θ}\\ 0& -\sin \mathrm{\φ}& cos\mathrm{\φ}*\cos \mathrm{\θ}\end{array}\right)$

将欧拉角方程用机体角速度方程代替，角度模型之间的误差通过对不确定性部分的补偿输入进行弥补。

根据四旋翼飞行器硬件平台的特点可知，转动惯量矩阵J的非对角线元素的值很小，相对于转动惯量矩阵J的对角线元素J_x，J_y，J_z可以忽略不计，所以欧拉角方程可转化为式(5)：

$\left[\begin{array}{c}\stackrel{\·\·}{\mathrm{\φ}}\\ \stackrel{\·\·}{\mathrm{\θ}}\\ \stackrel{\·\·}{\mathrm{\ψ}}\end{array}\right]=\left[\begin{array}{c}{u}_2/J_x\\ u_3/J_y\\ u_4/J_z\end{array}\right]+\left[\begin{array}{c}\stackrel{\·}{\mathrm{\θ}}*\stackrel{\·}{\mathrm{\ψ}}*(J_y-J_z)/J_x\\ \stackrel{\·}{\mathrm{\ψ}}*\stackrel{\·}{\mathrm{\φ}}*(J_z-J_x)/J_y\\ \stackrel{\·}{\mathrm{\φ}}*\stackrel{\·}{\mathrm{\θ}}*(J_x-J_y)/J_z\end{array}\right]+\left[\begin{array}{c}{J}_r*\stackrel{\·}{\mathrm{\θ}}/J_x\\ -J_r*\stackrel{\·}{\mathrm{\φ}}/J_y\\ 0\end{array}\right]*{\mathrm{\Σ}}_{i=1}^4{(-1)}^i*w_i-\left[\begin{array}{c}{c}_p*\stackrel{\·}{\mathrm{\φ}}/J_x\\ c_q*\stackrel{\·}{\mathrm{\θ}}/J_y\\ c_r*\stackrel{\·}{\mathrm{\ψ}}/J_z\end{array}\right]---\left(5\right)$

根据式(1)，将位置方程重写为式(6)：

$\left[\begin{array}{c}\stackrel{\·\·}{x}\\ \stackrel{\·\·}{y}\\ \stackrel{\·\·}{z}\end{array}\right]=\left[\begin{array}{c}\cos \mathrm{\φ}*\sin \mathrm{\θ}*\cos \mathrm{\ψ}+\sin \mathrm{\φ}*\sin \mathrm{\ψ}\\ \cos \mathrm{\φ}*\sin \mathrm{\θ}*\sin \mathrm{\ψ}-\sin \mathrm{\φ}*\cos \mathrm{\ψ}\\ \cos \mathrm{\φ}*\cos \mathrm{\θ}\end{array}\right]*\frac{u_1}{m}-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]+\left[\begin{array}{c}{\mathrm{\Δ}}_1(m,c_x,\stackrel{\·}{x})\\ {\mathrm{\Δ}}_2(m,c_y,\stackrel{\·}{y})\\ {\mathrm{\Δ}}_2(m,c_z,\stackrel{\·}{z})\end{array}\right]---\left(6\right)$

式(6)中，不确定项为：

$\begin{array}{c}{\mathrm{\Δ}}_1(m,c_x,\stackrel{\·}{x})=-c_x*{\stackrel{\·}{x}}^2/m\\ {\mathrm{\Δ}}_2(m,c_y,\stackrel{\·}{y})=-c_y*{\stackrel{\·}{y}}^2/m\\ {\mathrm{\Δ}}_1(m,c_z,\stackrel{\·}{z})=-c_z*{\stackrel{\·}{z}}^2/m\end{array}$

根据式(5)，将姿态方程重写为式(7)：

$\left[\begin{array}{c}\stackrel{\·\·}{\mathrm{\φ}}\\ \stackrel{\·\·}{\mathrm{\θ}}\\ \stackrel{\·\·}{\mathrm{\ψ}}\end{array}\right]=\left[\begin{array}{c}{u}_2/J_x\\ u_3/J_y\\ u_4/J_z\end{array}\right]+\left[\begin{array}{c}\stackrel{\·}{\mathrm{\θ}}*\stackrel{\·}{\mathrm{\ψ}}*(J_y-J_z)/J_x\\ \stackrel{\·}{\mathrm{\ψ}}*\stackrel{\·}{\mathrm{\φ}}*(J_z-J_x)/J_y\\ \stackrel{\·}{\mathrm{\φ}}*\stackrel{\·}{\mathrm{\θ}}*(J_x-J_y)/J_z\end{array}\right]+\left[\begin{array}{c}{\mathrm{\Δ}}_4(c_p,\stackrel{\·}{\mathrm{\φ}},\stackrel{\·}{\mathrm{\θ}},J_r,J_x,W)\\ {\mathrm{\Δ}}_5(c_q,\stackrel{\·}{\mathrm{\φ}},\stackrel{\·}{\mathrm{\θ}},J_r,J_y,W)\\ {\mathrm{\Δ}}_6(c_r,\stackrel{\·}{\mathrm{\ψ}},J_z)\end{array}\right]---\left(7\right)$

式(7)中，W表示电机转速，不确定项定义为：

$\begin{array}{c}{\mathrm{\Δ}}_4(c_p,\stackrel{\·}{\mathrm{\φ}},\stackrel{\·}{\mathrm{\θ}},J_r,J_x,W)=J_r*\stackrel{\·}{\mathrm{\θ}}/J_x*{\Σ}_{i=1}^4{(-1)}^i*w_i-c_p*\stackrel{\·}{\mathrm{\φ}}/J_x\\ {\mathrm{\Δ}}_5(c_q,\stackrel{\·}{\mathrm{\φ}},\stackrel{\·}{\mathrm{\θ}},J_r,J_y,W)=-J_r*\stackrel{\·}{\mathrm{\φ}}/J_y*{\Σ}_{i=1}^4{(-1)}^i*w_i-c_q*\stackrel{\·}{\mathrm{\θ}}/J_x\\ {\mathrm{\Δ}}_6(c_r,\stackrel{\·}{\mathrm{\ψ}},J_z)=-c_r*\stackrel{\·}{\mathrm{\ψ}}/J_z\end{array}$

至此，根据表示四旋翼飞行器在地面坐标系中位置的位置方程组式(6)和表示四旋翼飞行器在地面坐标系中姿态的姿态方程组式(7)确定的四旋翼飞行器数学模型。

在前述基于MIMO非线性不确定反步法的四旋翼位姿控制方法的具体实施方式中，可选地，所述反步鲁棒补偿控制器采用内外环控制，内环姿态参考信号通过外环控制律确定；

所述反步鲁棒补偿控制器包括：反步控制器和鲁棒补偿控制器；

通过反步控制器控制四旋翼飞行器模型状态变量的理想运动轨迹；

通过鲁棒补偿控制器对四旋翼飞行器模型误差进行补偿，所述鲁棒补偿控制器的跟踪性能和频率特性由控制性能要求和系统特性确定。

本发明实施例中，参看图2所示，反步鲁棒补偿控制器分为外环位置控制和内环姿态控制两部分，内环姿态参考信号通过外环控制律确定，其中，6DOF指六自由度(包括：3个位置自由度和3个姿态自由度)。具体的，将四旋翼飞行器的外环控制律根据位置参考信号和当前位置信息，经过运算得到内环姿态参考信号，内环控制律结合姿态信息，输出控制信号驱动执行机构按照既定的方式动作，最终使四旋翼平台到达期望的状态，完成诸如定点悬停、轨迹跟踪等飞行任务。

本发明实施例通过结合实践工程中，对四旋翼飞行器模型进行分析，确定导致四旋翼飞行器模型输出与输入存在不确定性的因素，这些因素包括：模型参数不确定性、未建模动态、系统噪声(包括：传感器噪声)和外部干扰等。当量化的不确定性与确定性部分满足一定的关系时，不确定性造成的一般控制器的性能恶化现象可通过鲁棒补偿改善。

本发明实施例中，正是考虑到实际实践工程中存在的各种不确定性和外部噪声，结合旋翼飞行器欠驱动、强耦合等MIMO典型特点，且为减少控制器设计的保守性，防止鲁棒补偿执行机构饱和，在进行模型不确定性分离时，本发明放弃了一般的模型线性化原则，而是最大可能地保留了模型的确定性部分，为确保四旋翼飞行器的控制性能和工程实现的可能性，本发明设计了对外部干扰和不确定性具有较强鲁棒性的反步鲁棒补偿控制器，与一般的控制器相比，四旋翼飞行器模型的反步鲁棒补偿控制器具有抗干扰能力强，控制性能优良的特点。

本发明实施例中，为使反步鲁棒补偿控制器发挥良好的性能，使被控的四旋翼飞行器的模型结构尽可能符合客观实际，模型结构和模型参数偏差，在不超过一定范围时，可以通过反步鲁棒补偿控制器来减小其对控制性能的不利影响，该反步鲁棒补偿控制器具有如下特点：

反步鲁棒补偿控制器从结构上分为反步控制器和鲁棒补偿控制器两部分，反步控制器是针对四旋翼飞行器模型确定性部分设计的控制器，作用是设计旋翼飞行器状态变量的理想运动轨迹，也称为标称控制器。

鲁棒补偿控制器是控制器的重要组成部分，其作用在于补偿由于四旋翼飞行器模型的不确定部分(不确定性和外界干扰)对飞行过程造成的干扰，使得补偿后的四旋翼飞行器模型与标称模型一样，便于通过标称模型设计控制律，从而确保四旋翼飞行器模型不确定性部分的控制性能基本上与针对四旋翼飞行器模型确定性部分设计的反步控制器一致，使得四旋翼飞行器在一定程度的干扰下，仍能按照期望的性能完成既定的飞行任务，鲁棒补偿控制器的跟踪性能和频率特性，由控制性能要求和系统特性确定，其中，标称模型是四旋翼飞行器模型的确定性部分，能够通过实验或客观规律得到的。所述反步鲁棒补偿控制器具有良好的跟踪特性，响应快速、超调小、静差以指数形式趋近于零，适用于诸如四旋翼的复杂非线性不确定系统，但不局限于此。

在前述基于MIMO非线性不确定反步法的四旋翼位姿控制方法的具体实施方式中，可选地，所述根据所述四旋翼飞行器数学模型和四旋翼飞行器模型的不确定性、外界干扰、欠驱动、强耦合特性，对所述四旋翼飞行器模型进行分析解耦，确定基于内外环控制的反步鲁棒补偿控制器包括：

对所述四旋翼飞行器数学模型进行整合分析，确定四旋翼飞行器数学模型的状态空间表示形式；

根据四旋翼飞行器数学模型的状态空间表示形式，结合四旋翼飞行器模型的不确定性，外界干扰，控制性能要求，四旋翼飞行器的欠驱动、强耦合特性，位置、姿态不同的响应特性，并基于内外环控制思想，对四旋翼飞行器的位置、偏航控制解耦成单通道的位姿控制，再对单通道的二阶子系统进行反步鲁棒补偿控制律设计，对四旋翼飞行器模型的不确定性和外界干扰进行补偿。

本发明实施例中，对所述四旋翼飞行器数学模型进行整合分析，确定四旋翼飞行器数学模型的状态空间表示形式，为后续反步鲁棒补偿控制器设计工作打好基础，所述四旋翼飞行器数学模型的状态空间表示形式如下：

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_{11}=x_{12},{\stackrel{\·}{x}}_{12}=\left(\cos \right(x_{13})*\sin \left(x_{23}\right)*\cos \left(x_{41}\right)+\sin \left(x_{13}\right)*\sin \left(x_{41}\right))*\frac{u_1}{m}+{\mathrm{\Δ}}_1\\ {\stackrel{\·}{x}}_{13}=x_{14},{\stackrel{\·}{x}}_{14}=\frac{J_y-J_z}{J_x}*x_{24}*x_{42}+\frac{u_2}{J_x}+{\mathrm{\Δ}}_2\\ {\stackrel{\·}{x}}_{21}=x_{22},{\stackrel{\·}{x}}_{22}=\left(\cos \right(x_{13})*\sin \left(x_{23}\right)*\sin \left(x_{41}\right)-\sin \left(x_{13}\right)*\cos \left(x_{41}\right))*\frac{u_1}{m}+{\mathrm{\Δ}}_2\\ {\stackrel{\·}{x}}_{23}=x_{24},{\stackrel{\·}{x}}_{24}=\frac{J_z-J_x}{J_y}*x_{42}*x_{14}+\frac{u_3}{J_y}+{\mathrm{\Δ}}_5\\ {\stackrel{\·}{x}}_{31}=x_{32},{\stackrel{\·}{x}}_{32}=\cos \left(x_{13}\right)*\cos \left(x_{23}\right)*\frac{u_1}{m}-g+{\mathrm{\Δ}}_3\\ {\stackrel{\·}{x}}_{41}=x_{42},{\stackrel{\·}{x}}_{42}=\frac{J_x-J_y}{J_z}*x_{14}*x_{24}+\frac{u_4}{J_z}+{\mathrm{\Δ}}_6\end{array}\right.$

其中，x₁₁,x₂₁,x₃₁与空间位置x,y,z一一对应，x₁₃,x₂₃,x₄₁与欧拉角φ,θ,ψ一一对应，x₁₂,x₂₂,x₃₂与速度v_x,v_y,v_z一一对应，x₁₄,x₂₄,x₄₂与欧拉角一一对应，△_i(i＝1,2,…6)为不确定项△_i(*)的简写。四旋翼飞行器可在空中进行六自由度的运动，六自由度分别指位置三自由度和角度三自由度，这些可观测的自由度状态变量及其微分，构成了四旋翼飞行器模型的状态变量空间。

本发明实施例中，根据确定的四旋翼飞行器数学模型的状态空间表示形式，结合四旋翼飞行器模型的不确定性，外界干扰，控制性能要求，四旋翼飞行器的欠驱动、强耦合特性，位置、姿态不同的响应特性，并基于内外环控制思想，对四旋翼飞行器的位置、偏航控制解耦成单通道的位姿控制，再对单通道的二阶子系统进行反步鲁棒补偿控制律设计，对四旋翼飞行器模型的不确定性和外界干扰进行补偿，参看图3所示为四旋翼模型解耦后的单通道反步鲁棒补偿控制律的二阶子系统结构示意图。

本发明实施例中，反步鲁棒补偿控制器的设计步骤如下：

z位置和偏航通道均可视为关于控制输入的二阶子系统，先为z通道设计基于反步法的反步鲁棒补偿控制器：

$\begin{array}{cc}{e}_{31}=x_{31}-z_d& {\hat{x}}_{32}=-{\mathrm{\α}}_{31}*e_{31}+{\stackrel{\·}{z}}_d\\ {\stackrel{\·}{e}}_{31}=x_{32}-{\stackrel{\·}{z}}_d& x_{32}=x_{32}-{\hat{x}}_{32}\end{array}$

其中，z_d为z方向的参考信号，为z_d的导数，为虚拟输入，α₃₁表示反步鲁棒补偿控制器的调节参数，对e₃₂求导：

$\begin{array}{c}{\stackrel{\·}{e}}_{32}={\stackrel{\·}{x}}_{32}-{\stackrel{\stackrel{\·}{^}}{x}}_{32}=\cos \left(x_{13}\right)*\cos \left(x_{23}\right)*\frac{u_1}{m}-g+{\mathrm{\Δ}}_3-{\stackrel{\stackrel{\·}{^}}{x}}_{32}\\ =\cos \left(x_{13}\right)*cos\left(x_{23}\right)*\frac{u_1}{m}-g+{\mathrm{\Δ}}_{32}\end{array}$

令 $cos\left(x_{13}\right)*cos\left(x_{23}\right)*\frac{u_1}{m}-g=-{\mathrm{\α}}_{32}*e_{32}+f_{32}*w_{32},$ α₃₂和f₃₂为反步鲁棒补偿控制器的调节参数，△₃₂为不确定项，为使控制输入对不确定性的补偿效果较好，取其中，s表示拉普拉斯算子，进行频域处理的特殊运算形式。

故应设计控制输入为：

u₁＝m*(-α₃₂*e₃₂+f₃₂*w₃₂+g)/(cos(x₁₃)*cos(x₂₃))   (8)

本发明实施例中，偏航角子系统的同结构控制器设计步骤与z位置类似，可得偏航通道的反步鲁棒补偿控制器为：

$\begin{array}{cc}{e}_{41}=x_{41}-{\mathrm{\ψ}}_d& {\hat{x}}_{42}=-{\mathrm{\α}}_{41}*e_{41}+{\stackrel{\·}{\mathrm{\ψ}}}_d\\ {\stackrel{\·}{e}}_{41}=x_{41}-{\stackrel{\·}{\mathrm{\ψ}}}_d& x_{42}=x_{42}-{\hat{x}}_{42}\end{array}$

其中，ψ_d为ψ角度的参考信号，为ψ_d的导数，为虚拟输入，α₄₁表示反步鲁棒补偿控制器的调节参数，对e₄₂求导：

$\begin{array}{c}{\stackrel{\·}{e}}_{42}={\stackrel{\·}{x}}_{42}-{\stackrel{\·}{\hat{x}}}_{42}=\frac{J_x-J_y}{J_z}*x_{14}*x_{24}+\frac{u_4}{J_z}+{\mathrm{\Δ}}_6-{\stackrel{\stackrel{\·}{^}}{x}}_{42}\\ \frac{J_x-J_y}{J_z}*x_{14}*x_{24}+\frac{u_4}{J_z}+{\mathrm{\Δ}}_{62}\end{array}$

令 $\frac{J_x-J_y}{J_z}*x_{14}*x_{24}+\frac{u_4}{J_z}=-{\mathrm{\α}}_{42}*e_{42}+f_{42}*w_{42},$ α₄₂和f₄₂为反步鲁棒补偿控制器的调节参数，△₆₂为不确定项，其中，

故偏航通道的控制律为：

u₄＝J_z*(-α₄₂*e₄₂+f₄₂*w₄₂)-(J_x-J_y)*x₁₄*x₂₄    (9)

本发明实施例中，当对位置分量x和y进行控制时，由于控制输入隐含在状态变量中，需要进行转换变形，以便进行控制律设计，避免了常规补偿器的微分爆炸效应，针对位置分量x：

$\begin{array}{cc}{e}_{11}=x_{11}-x_d& {\hat{x}}_{12}=-{\mathrm{\α}}_{11}*e_{11}+{\stackrel{\·}{x}}_d\\ {\stackrel{\·}{e}}_{11}={\stackrel{\·}{x}}_{11}-{\stackrel{\·}{x}}_d& x_{12}=x_{12}-{\hat{x}}_{12}\end{array}$

其中，x_d为x方向的参考信号，为x_d的导数，为虚拟输入，α₁₁表示反步鲁棒补偿控制器的调节参数，对e₁₂求导：

$\begin{array}{c}{\stackrel{\·}{e}}_{12}={\stackrel{\·}{x}}_{12}-{\stackrel{\·}{\hat{x}}}_{12}=\left(\cos \right(x_{13})*\sin \left(x_{23}\right)*\cos \left(x_{41}\right)+\sin \left(x_{13}\right)*\sin \left(x_{41}\right))*\frac{u_1}{m}+{\mathrm{\Δ}}_1-{\stackrel{\stackrel{\·}{^}}{x}}_{12}\\ \left(\cos \right(x_{13})*\sin (x_{23})*\cos (x_{41})+\sin (x_{13})*\sin (x_{41}\left)\right)*\frac{u_1}{m}+{\mathrm{\Δ}}_{12}\end{array}$

令

$\left(cos\right(x_{13})*sin(x_{23})*cos(x_{41})+sin(x_{13})*sin(x_{41}\left)\right)*\frac{u_1}{m}=-{\mathrm{\α}}_{12}*e_{12}+f_2*w_{12}---\left(10\right)$

式(10)中，α₁₂和f₁₂为反步鲁棒补偿控制器的调节参数，△₁₂为不确定项， $w_{12}=-\frac{{\mathrm{\Δ}}_{12}}{s+f_{12}}=-(1+\frac{1}{s})*e_{12}$

由于状态变量间的耦合，还需要y方向的控制输入信息，依上可得：

$\begin{array}{cc}{e}_{21}=x_{21}-z_d& {\hat{x}}_{22}=-{\mathrm{\α}}_{21}*e_{21}+{\stackrel{\·}{y}}_d\\ {\stackrel{\·}{e}}_{21}={\stackrel{\·}{x}}_{21}-{\stackrel{\·}{y}}_d& x_{22}=x_{22}-{\hat{x}}_{22}\end{array}$

其中，y_d为y方向的参考信号，为y_d的导数，为虚拟输入，α₂₁表示反步鲁棒补偿控制器的调节参数，对e₂₂求导：

$\begin{array}{c}{\stackrel{\·}{e}}_{22}={\stackrel{\·}{x}}_{22}-{\stackrel{\stackrel{\·}{^}}{x}}_{22}=\left(\cos \right(x_{13})*\sin \left(x_{23}\right)*\sin \left(x_{41}\right)-\sin \left(x_{13}\right)*\cos \left(x_{41}\right))*\frac{u_1}{m}+{\mathrm{\Δ}}_2-{\stackrel{\stackrel{\·}{^}}{x}}_{22}\\ =\left(\cos \right(x_{13})*\sin (x_{23})*\sin (x_{41})-\sin (x_{13})*\cos (x_{41}\left)\right)*\frac{u_1}{m}+{\mathrm{\Δ}}_{22}\end{array}$

令

$\left(cos\right(x_{13})*sin(x_{23})*sin(x_{41})-sin(x_{13})*cos(x_{41}\left)\right)*\frac{u_1}{m}=-{\mathrm{\α}}_{22}*e_{22}+f_{22}*w_{22}---\left(11\right)$

式(11)中，α₂₂和f₂₂为反步鲁棒补偿控制器的调节参数，△₂₂为不确定项， $w_{22}=-\frac{{\mathrm{\Δ}}_{22}}{s+f_{22}}=-(1+\frac{1}{s})*e_{22}$

综合式(10)、式11)两式，可得滚转角的参考信号φ_d，俯仰角的参考信号θ_d：

${\mathrm{\φ}}_d=arcsin(\frac{m}{u_1}*((-{\mathrm{\α}}_{12}*e_{12}+f_{12}*w_{12})*sin\left(x_{41}\right)-(-{\mathrm{\α}}_{22}*e_{22}+f_{22}*w_{22})*cos\left(x_{41}\right)\left)\right)$

${\mathrm{\θ}}_d=arcsin(\frac{m}{u_1*cos\left(x_{13}\right)}*((-{\mathrm{\α}}_{12}*e_{12}+f_{12}*w_{12})*cos\left(x_{41}\right)+(-{\mathrm{\α}}_{22}*e_{22}+f_{22}*w_{22})*sin\left(x_{41}\right)\left)\right)$

在获得滚转和俯仰角的参考信号后，可以按照常规方式进行内环欧拉通道的闭环鲁棒补偿控制律设计，对于滚转角有：

$\begin{array}{cc}{e}_{13}=x_{13}-{\mathrm{\φ}}_d& {\hat{x}}_{14}=-{\mathrm{\α}}_{13}*e_{13}+{\stackrel{\·}{\mathrm{\φ}}}_d\\ {\stackrel{\·}{e}}_{13}={\stackrel{\·}{x}}_{13}-{\stackrel{\·}{\mathrm{\φ}}}_d& x_{14}=x_{14}-{\hat{x}}_{14}\end{array}$

其中，为φ_d的导数，为虚拟输入，α₁₃表示反步鲁棒补偿控制器的调节参数，对e₁₄求导：

$\begin{array}{c}{\stackrel{\·}{e}}_{14}={\stackrel{\·}{x}}_{14}-{\stackrel{\stackrel{\·}{^}}{x}}_{14}=\frac{J_y-J_z}{J_x}*x_{24}*x_{42}+\frac{u_2}{J_x}+{\mathrm{\Δ}}_4-{\stackrel{\stackrel{\·}{^}}{x}}_{14}\\ =\frac{J_y-J_z}{J_x}*x_{24}*x_{42}+\frac{u_2}{J_x}+{\mathrm{\Δ}}_{42}\end{array}$

令 $\frac{J_y-J_z}{J_x}*x_{24}*x_{42}+\frac{u_2}{J_x}=-{\mathrm{\α}}_{14}*e_{14}+f_4*w_{14},$ α₁₄和f₁₄为反步鲁棒补偿控制器的调节参数，△₄₂为不确定项，

得到滚转通道控制律为：

u₂＝J_x*(-α₁₄*e₁₄+f₁₄*w₁₄)-(J_y-J_z)*x₂₄*x₄₂   (12)

同理可得俯仰通道控制律为：

u₃＝J_y*(-α₂₄*e₂₄+f₂₄*w₂₄)-(J_z-J_x)*x₁₄*x₄₂   (13)

式(13)中， $\begin{array}{c}{e}_{24}=x_{24}-{\hat{x}}_{24}\\ w_{24}=-(1+\frac{{\mathrm{\α}}_{24}}{s})*e_{24}\end{array},$ $\begin{array}{c}{\hat{x}}_{24}={\mathrm{\α}}_{23}*e_{23}+{\stackrel{\·}{\mathrm{\θ}}}_d\\ e_{23}=x_{23}-{\mathrm{\θ}}_d\end{array},$ 为θ_d的导数，为虚拟输入，α₂₃、α₂₄和f₂₄表示反步鲁棒补偿控制器的调节参数。

至此，综合式(8)-(13)，反步鲁棒补偿控制器的设计工作已完成。

在前述基于MIMO非线性不确定反步法的四旋翼位姿控制方法的具体实施方式中，可选地，所述方法还包括：在实践工程中，对四旋翼飞行器机载的传感器采集的四旋翼飞行器的飞行信息进行滤波融合处理；

所述对四旋翼飞行器机载的传感器采集的四旋翼飞行器的飞行信息进行滤波融合处理包括：

通过四旋翼飞行器机载的传感器采集四旋翼飞行器的飞行信息；

通过低通滤波器对采集的四旋翼飞行器的飞行信息进行低通滤波处理，其中，所述低通滤波器是结合实践工程中的四旋翼飞行器确定的；

通过卡尔曼滤波器冗余机制对不同传感器采集的隐含同类型的经过低通滤波处理后的飞行信息进行数据融合，得到目标观测信息；

其中，数据融合包括：陀螺仪传感器、加速度计传感器，磁力计传感器、GPS传感器、超声波传感器、气压计传感器采集的并经过低通滤波后的信息，及建立在此基础上的角速度、欧拉角内环信息、速度、位置外环信息。

本发明实施例中，在实践工程中，还依据四旋翼飞行器系统的控制性能和运算、接口通信等要求，选择反步鲁棒补偿控制器的硬件实现基础，包括飞行器外围设备、飞控板和传感器。例如，所述飞行器外围设备包括：DJI F450飞行器机架，朗宇X2212无刷直流电机，FUN系列10*4.7P旋翼，好盈platinumpro 30A电调、Xsens公司的Mti-G-700传感器等。飞控板是反步鲁棒补偿控制器实现其功能的重要部件，在综合考虑反步鲁棒补偿控制器性能和成本的基础上，选择基于TMS320F28335的DSP为处理核心设计飞控板，并进行适应工作环境的优化，即采用基于TMS320F28335的DSP控制单元运行本发明设计的反步鲁棒补偿控制算法。

本发明实施例中，在实践工程中，为对设计的四旋翼飞行器的反步鲁棒补偿控制器的可行性、控制性能进行分析，需选择满足需求的传感器，结合不同传感器特点，搭建四旋翼飞行器的传感器平台，对传感器参数进行校正、以及对传感器采集的数据进行处理。

本发明实施例中，当进行纯粹的理论分析时，通常假设四旋翼飞行器传感器的采集的四旋翼飞行器的飞行信息准确无偏，但是，在实践工程中，微小型传感器由于机械结构、观测原理等因素，容易受到外界环境的干扰，造成采集信息的可信度低，从而影响整个控制系统的性能。

旋翼类飞行器依靠旋翼产生的升力抵消自身的重力，达到飞行目的，现阶段小型四旋翼飞行器基本依靠3S锂电池作为动力来源，载重能力有限，不能搭载大型复杂的高精度传感器为四旋翼导航。为解决载重能力与传感器精度之间的矛盾，结合当前四旋翼控制领域较为成熟的传感器数据融合技术，为传感器设计数据滤波与融合通道。数据融合利用多个传感器得到某观测变量的相关信息，利用冗余机制对不同来源的信息进行整合，得到可信度较高的目标观测信息。

本发明实施例中，结合四旋翼飞行器模型的特点，对传感器采集的四旋翼飞行器的飞行信息进行初步处理，减小错误信息对反步鲁棒补偿控制器和滤波器性能的影响。例如，在实践工程中搭建四旋翼飞行器时，由于旋翼不能提供与重力方向相同的力，所以四旋翼飞行器的加速度不会小于重力加速度的相反数；实践环节的测试结果表明，由于四旋翼飞行器动力系统电调、电机、电源等硬件的性能限制，四旋翼飞行器的加速度不会超过重力加速度。所以在对加速度计进行滤波时，可以先进行限幅处理，防止异常的传感器信息导致执行机构的误动作，导致四旋翼飞行器系统不稳定。

在实践工程搭建的四旋翼飞行器中，传感器的异常信息带来的影响有两个方面：

一是导致反步鲁棒补偿控制器决策失当，引起执行机构误动作，使四旋翼飞行器产生振荡甚至失稳，造成安全隐患；

二是使滤波器在进行参数估计时，对不同来源信息的噪声估计出现较大偏差，导致之后较长时间的估计信息可信度较低，特别是这段时间再次出现较强干扰时，四旋翼飞行器容易进入不稳定飞行状态，发生意外。

本发明实施例中，先通过低通滤波器对采集的四旋翼飞行器的飞行信息进行低通滤波处理，减少传感器原始数据对融合不利的成分，便于卡尔曼滤波器快速收敛，其中，所述低通滤波器是结合实践工程中的四旋翼飞行器确定的，再利用成熟的滤波理论，对不同传感器采集的隐含同类型的经过低通滤波处理后的飞行信息进行数据融合，该融合过程应根据不同传感器信息的特点，有目的地进行。例如，传感器中，我们可以通过陀螺仪和磁力计获得偏航角信息，陀螺仪是通过角速度积分得到角度信息的，获得的角度信息随着时间漂移，且偏差越来越大，这种无界的漂移对控制器来说往往是致命的，通过磁力计获得的偏航角信息具有较好的静态特性，但其对快速的角度变化不敏感，存在明显的滞后。经过上述简单分析，可以很自然地想到利用某种手段将陀螺仪和磁力计两种偏航角信息获取方式的优点结合起来，使处理结果既具有磁力计稳定准确的静态特性，同时又具备陀螺仪对角度变化敏感快速的动态响应能力。

本发明实施例中，参看图4所示，利用卡尔曼滤波器对冗余的传感器信息进行数据融合处理，得到可信度较高的目标观测信息，为实践工程中控制系统良好的可视化性能打好基础，参看图5所示为卡尔曼滤波器原理框图。

在前述基于MIMO非线性不确定反步法的四旋翼位姿控制方法的具体实施方式中，可选地，所述方法还包括：

在实践工程中，对确定的反步鲁棒补偿控制算法进行离散化处理，离散化程度由反步鲁棒补偿控制器的控制性能和传感器的采样频率决定。

本发明实施例中，为在实践工程中顺利实现对四旋翼飞行器的控制，需对本发明实设计的反步鲁棒补偿控制算法进行离散化处理，离散程度由反步鲁棒补偿控制器的控制性能和传感器的采样频率决定，例如，离散化控制频率可以为100Hz。

在前述基于MIMO非线性不确定反步法的四旋翼位姿控制方法的具体实施方式中，可选地，所述根据所述四旋翼飞行器数学模型和四旋翼飞行器模型的不确定性、外界干扰、欠驱动、强耦合特性，对所述四旋翼飞行器模型进行分析解耦，确定基于内外环控制的反步鲁棒补偿控制器之后包括：

在理论研究阶段，根据确定的反步鲁棒补偿控制器，建立四旋翼飞行器仿真模型，并调节反步鲁棒补偿控制器的调节参数，直至控制性能满足控制性能要求，对基于反步鲁棒补偿控制器的四旋翼飞行器仿真模型进行初步验证，其中，所述控制性能包括：响应时间、超调、抗干扰能力；

在实践工程中，对基于反步鲁棒补偿控制器的四旋翼飞行器的实际飞行效果进行后期验证。

本发明实施例中，利用上述设计的反步鲁棒补偿控制器，在simulink仿真平台上搭建四旋翼飞行器仿真模型，在理论研究中和实践工程中，分析四旋翼飞行器控制系统的状态并依此调节反步鲁棒补偿控制器的调节参数，使系统性能(包括响应时间、超调、抗干扰能力等)满足预期要求。系统性能的评测主要从两方面进行，在理想干净环境下(Simulink仿真平台)，反步鲁棒补偿控制器响应速度、超调量、静态误差等指标。实际飞行时，结合地面站信息、飞行环境和飞行任务，对飞行质量作出综合测评。

本发明实施例中，参看图6所示为反步鲁棒补偿控制的simulink平台仿真框图，仿真中考虑了四旋翼飞行器模型不确定性、传感器噪声、外界干扰、执行机构不对称性、动力性能变化、积分器饱和等因素，并将这些因素整合到被控四旋翼飞行器模型中，设计反步鲁棒补偿控制器并检验其性能，为实践工程做参数调整的提供初期参考。为使仿真能更好地模拟实际飞行时的情况，设置仿真采样时间与传感器的数据发送频率一致，并在电机-旋翼模型间加入随机的白噪声干扰，幅值为进行升力测试试验中的最大变动范围，使仿真结果具有较高的说服力。

本发明实施例中，为验证本发明设计的反步鲁棒补偿控制算法的有效性，需对基于反步鲁棒补偿控制方法的控制系统进行验证，不仅在理论阶段对基于反步鲁棒补偿控制方法的有效性进行初步验证，在实践工程中，也要对四旋翼飞行器的实际飞行效果进行测试验证，在实践工程中，该控制系统主要包括两部分，硬件平台和软件平台，硬件给控制算法的执行提供了平台，软件代码指明了在硬件平台中进行怎样的操作，来实现设计的控制方法，硬件平台和软件平台是相互依存，不可分割的关系。硬件平台包括：搭载软件的飞控板、四旋翼机体、传感器、接收机及飞行器外围设备，硬件部分为验证发明目的提供了实体和可能。

本发明实施例中，软件平台中，传感器和接收机与飞控板的数据通讯通过中断方式实现，这有利于提高程序的执行效率并减少出错，程序采用模块化方式编写，预留接口以便于移植。

本发明实施例中，所述软件平台按照用途划分为嵌入式控制程序和地面站两个部分，嵌入式控制程序是完整的关于传感器信息处理、上位机通信、控制策略执行解决方案，主要在软件平台中完成开发工作。简易版本的地面站具有调节控制参数，监控状态变量，控制性能评估等功能，完成地面站初期上位机平台的搭建。

本发明实施例中，为验证本发明设计的基于反步鲁棒补偿控制方法的控制系统的有效性，对两类的飞行试验进行验证：定点悬停和轨迹跟踪，参看图7所示为验证基于反步鲁棒补偿控制方法的有效性，其中，i表示通信接口标志，num表示通信接口总数量，CH_pro表示接收机信息处理标志，Sen_pro＝1表示传感器信息处理标志，UKF指卡尔曼滤波器，参看图8和图9所示，展示了采用本发明设计的反步鲁棒补偿控制算法控制四旋翼飞行器到达指定位置并定点悬停的过程，从图8中可以看出，四旋翼飞行器控制系统能在7s内从原点到达给定悬停点(7,-2,3)，响应快速无超调，反步鲁棒补偿控制器基本上消除了不同通道之间的相互干扰，第25s时，四旋翼飞行器控制系统的位置响应出现了较小的波动，结合图9可知，这主要是四旋翼飞行器控制系统快速跟踪给定偏航角导致的，位置和角度偏差都是以指数形式收敛到零的，其中，Pitch表示俯仰角θ，Yaw表示偏航角ψ，Roll表示滚转角φ。

本发明实施例中，在实践工程中，根据simulink仿真结果，对反步鲁棒补偿控制器参数进行微调，使四旋翼飞行性能满足要求，因室外干扰源较多，且产生的干扰具有随机性，而反步鲁棒补偿控制进行输入补偿需要一定的时间，再加上传感器的测量噪声无法完全消除，所以实际飞行时，位置和姿态响应会有一些较小的抖动，但整体表现良好，参看图10和图11所示，图12为四旋翼飞行器上升至给定的高度，跟踪边长为30米正方形，将飞行数据无线传输至地面站，并绘出飞行效果图的情况，参看图12所示为室外飞行试验中进行轨迹跟踪的效果图。

以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明所述原理的前提下，还可以作出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

1.一种基于MIMO非线性不确定反步法的四旋翼位姿控制方法，其特征在于，包括:

基于空气动力学和旋翼飞行器运动原理，并根据控制性能要求建立四旋翼飞行器数学模型；

根据所述四旋翼飞行器数学模型和精度要求，建立四旋翼飞行器模型；

根据所述四旋翼飞行器数学模型和四旋翼飞行器模型的不确定性、外界干扰、欠驱动、强耦合特性，对所述四旋翼飞行器模型进行分析解耦，确定基于内外环控制的反步鲁棒补偿控制器。

2.根据权利要求1所述的方法，其特征在于，所述根据所述四旋翼飞行器数学模型和精度要求，建立四旋翼飞行器模型包括：

对四旋翼飞行器进行受力分析和运动学分析，根据所述四旋翼飞行器数学模型和精度要求，通过机理建模确定四旋翼飞行器模型的结构；

根据四旋翼飞行器模型的结构，通过系统辨识、测量方式确定四旋翼飞行器模型的参数，其中，辨识对象为实践工程中的四旋翼飞行器，辨识的内容包括：转动惯量、旋翼臂长、模型阶次；

对四旋翼飞行器模型的结构和参数进行验证；

将所述四旋翼飞行器模型用确定性和不确定性两个部分表示，并结合实践工程，减小不确定性部分所占的比重。

3.根据权利要求1所述的方法，其特征在于，所述四旋翼飞行器数学模型包括：表示四旋翼飞行器在地面坐标系中位置的位置方程组和表示四旋翼飞行器在地面坐标系中姿态的姿态方程组；

所述位置方程组表示如下：

$\left[\begin{array}{c}\stackrel{\·\·}{x}\\ \stackrel{\·\·}{y}\\ \stackrel{\·\·}{z}\end{array}\right]=\left[\begin{array}{c}\cos \mathrm{\φ}*\sin \mathrm{\θ}*\cos \mathrm{\ψ}+\sin \mathrm{\φ}*\sin \mathrm{\ψ}\\ \cos \mathrm{\φ}*\sin \mathrm{\θ}*\sin \mathrm{\ψ}-\sin \mathrm{\φ}*\cos \mathrm{\ψ}\\ \cos \mathrm{\φ}*\sin \mathrm{\θ}\end{array}\right]*\frac{u_1}{m}-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]+\left[\begin{array}{c}{\mathrm{\Δ}}_1(m,c_x,\stackrel{\·}{x})\\ {\mathrm{\Δ}}_2(m,c_y,\stackrel{\·}{y})\\ {\mathrm{\Δ}}_3(m,c_z,\stackrel{\·}{z})\end{array}\right]$

其中，不确定项 ${\mathrm{\Δ}}_1(m,c_x,\stackrel{\·}{x})=-c_x*{\stackrel{\·}{x}}^2/m,$ ${\mathrm{\Δ}}_2(m,c_y,\stackrel{\·}{y})=-c_y*{\stackrel{\·}{y}}^2/m,$ φ,θ,ψ表示欧拉角，u₁表示旋翼升力，m表示四旋翼质量，g表示重力加速度，c_x,c_y,c_z表示空气阻力系数，x,y,z表示四旋翼飞行器的空间位置，分别表示x,y,z的导数；分别表示的导数；

所述姿态方程组表示如下：

$\left[\begin{array}{c}\stackrel{\·\·}{\mathrm{\φ}}\\ \stackrel{\·\·}{\mathrm{\θ}}\\ \stackrel{\·\·}{\mathrm{\ψ}}\end{array}\right]=\left[\begin{array}{c}{u}_2/J_x\\ u_3/J_y\\ u_4/J_z\end{array}\right]+\left[\begin{array}{c}\stackrel{\·}{\mathrm{\θ}}*\stackrel{\·}{\mathrm{\ψ}}*(J_y-J_z)/J_x\\ \stackrel{\·}{\mathrm{\ψ}}*\stackrel{\·}{\mathrm{\φ}}*(J_z-J_x)/J_y\\ \stackrel{\·}{\mathrm{\φ}}*\stackrel{\·}{\mathrm{\θ}}*(J_x-J_y)/J_z\end{array}\right]+\left[\begin{array}{c}{\mathrm{\Δ}}_4(c_p,\stackrel{\·}{\mathrm{\φ}},\stackrel{\·}{\mathrm{\θ}},J_r,J_x,W)\\ {\mathrm{\Δ}}_5(c_q,\stackrel{\·}{\mathrm{\φ}},\stackrel{\·}{\mathrm{\θ}},J_r,J_y,W)\\ \mathrm{\Δ}{6}_{}(c_r,\stackrel{\·}{\mathrm{\ψ}},J_z)\end{array}\right]$

其中，不确定项 ${\mathrm{\Δ}}_4(c_p,\stackrel{\·}{\mathrm{\φ}},\stackrel{\·}{\mathrm{\θ}},J_r,J_x,W)=J_r*\stackrel{\·}{\mathrm{\θ}}/J_x*{\mathrm{\Σ}}_{i=1}^4{(-1)}^i*w_i-c_p*\stackrel{\·}{\mathrm{\φ}}/J_x,$ ${\mathrm{\Δ}}_5(c_q,\stackrel{\·}{\mathrm{\φ}},\stackrel{\·}{\mathrm{\θ}},J_r,J_y,W)=-J_r*\stackrel{\·}{\mathrm{\φ}}/J_y*{\mathrm{\Σ}}_{i=1}^4{(-1)}^i*w_i-c_q*\stackrel{\·}{\mathrm{\θ}}/J_y,$ ${\mathrm{\Δ}}_6(c_r,\stackrel{\·}{\mathrm{\ψ}},J_z)=-c_r*\stackrel{\·}{\mathrm{\ψ}}/J_z,$ u₂,u₃,u₄表示三轴转矩，J_x，J_y，J_z表示转动惯量矩阵J的对角线元素J_x，J_y，J_z，分别表示φ,θ,ψ的导数，分别表示的导数，W表示电机转速，J_r表示电机转动惯量，c_p,c_q,c_r表示转矩阻力系数。

4.根据权利要求3所述的方法，其特征在于，所述反步鲁棒补偿控制器采用内外环控制，内环姿态参考信号通过外环控制律确定；

所述反步鲁棒补偿控制器包括：反步控制器和鲁棒补偿控制器；

通过反步控制器控制四旋翼飞行器模型状态变量的理想运动轨迹；

通过鲁棒补偿控制器对四旋翼飞行器模型误差进行补偿，所述鲁棒补偿控制器的跟踪性能和频率特性由控制性能要求和系统特性确定。

5.根据权利要求4所述的方法，其特征在于，所述根据所述四旋翼飞行器数学模型和四旋翼飞行器模型的不确定性、外界干扰、欠驱动、强耦合特性，对所述四旋翼飞行器模型进行分析解耦，确定基于内外环控制的反步鲁棒补偿控制器包括：

对所述四旋翼飞行器数学模型进行整合分析，确定四旋翼飞行器数学模型的状态空间表示形式；

根据四旋翼飞行器数学模型的状态空间表示形式，结合四旋翼飞行器模型的不确定性，外界干扰，控制性能要求，四旋翼飞行器的欠驱动、强耦合特性，位置、姿态不同的响应特性，并基于内外环控制思想，对四旋翼飞行器的位置、偏航控制解耦成单通道的位姿控制，再对单通道的二阶子系统进行反步鲁棒补偿控制律设计，对四旋翼飞行器模型的不确定性和外界干扰进行补偿。

6.根据权利要求5所述的方法，其特征在于，所述四旋翼飞行器数学模型的状态空间表示形式如下：

$\left\{\begin{array}{c}{\stackrel{\·\·}{x}}_{11}=x_{12},{\stackrel{\·}{x}}_{12}=\left(\cos \right(x_{13})*\sin \left(x_{23}\right)*\cos \left(x_{41}\right)+\sin \left(x_{13}\right)*\sin \left(x_{41}\right))*\frac{u_1}{m}+{\mathrm{\Δ}}_1\\ {\stackrel{\·}{x}}_{13}=x_{14},{\stackrel{\·}{x}}_{14}=\frac{J_y-J_z}{J_x}{x}_{24}*x_{42}+\frac{u_2}{J_x}+{\mathrm{\Δ}}_4\\ {\stackrel{\·}{x}}_{21}=x_{22},{\stackrel{\·}{x}}_{22}=\left(\cos \right(x_{13})*\sin \left(x_{23}\right)*\sin \left(x_{41}\right)-\sin \left(x_{13}\right)*\cos \left(x_{41}\right))*\frac{u_1}{m}+{\mathrm{\Δ}}_2\\ {\stackrel{\·}{x}}_{23}=x_{24},{\stackrel{\·}{x}}_{24}=\frac{J_z-J_x}{J_y}{x}_{42}*x_{14}+\frac{u_3}{J_y}+{\mathrm{\Δ}}_5\\ {\stackrel{\·}{x}}_{31}=x_{32},{\stackrel{\·}{x}}_{32}=\cos \left(x_{13}\right)*\cos \left(x_{23}\right)*\frac{u_1}{m}-g+{\mathrm{\Δ}}_3\\ {\stackrel{\·}{x}}_{41}=x_{42},{\stackrel{\·}{x}}_{42}=\frac{J_x-J_y}{J_z}*x_{14}*x_{24}+\frac{u_4}{J_z}+{\mathrm{\Δ}}_6\end{array}\right.$

其中，x₁₁,x₂₁,x₃₁与空间位置x,y,z一一对应，x₁₃,x₂₃,x₄₁与欧拉角φ,θ,ψ一一对应，x₁₂,x₂₂,x₃₂与速度v_x,v_y,v_z一一对应，x₁₄,x₂₄,x₄₂与欧拉角一一对应，△_i(i＝1,2,…6)为不确定项△_i(*)的简写。

7.根据权利要求6所述的方法，其特征在于，所述对四旋翼飞行器的位置、偏航控制解耦成单通道的位姿控制，再对单通道的二阶子系统进行反步鲁棒补偿控制律设计，对四旋翼飞行器模型的不确定性和外界干扰进行补偿包括：

z通道的控制输入为：u₁＝m*(-α₃₂*e₃₂+f₃₂*w₃₂+g)/(cos(x₁₃)*cos(x₂₃))

其中， $e_{32}=x_{32}-{\hat{x}}_{32},$ ${\hat{x}}_{32}=-{\mathrm{\α}}_{31}*e_{31}+{\stackrel{\·}{z}}_d,$ e₃₁＝x₃₁-z_d， $w_{32}=-(1+\frac{{\mathrm{\α}}_{32}}{s})*e_{32},$ z_d为z方向的参考信号，为z_d的导数，为虚拟输入，α₃₁和α₃₂表示反步鲁棒补偿控制器的调节参数；

偏航通道的控制律为：u₄＝J_z*(-α₄₂*e₄₂+f₄₂*w₄₂)-(J_x-J_y)*x₁₄*x₂₄

其中， $e_{42}=x_{42}-{\hat{x}}_{42},$ ${\hat{x}}_{42}=-{\mathrm{\α}}_{41}*e_{41}+{\stackrel{\·}{\mathrm{\ψ}}}_d,$ e₄₁＝x₄₁-ψ_d， $w_{42}=-(1+\frac{{\mathrm{\α}}_{42}}{s})*e_{42},$ ψ_d为ψ角度的参考信号，为ψ_d的导数，为虚拟输入，α₄₁和α₄₂表示反步鲁棒补偿控制器的调节参数；

滚转通道的控制律为：u₂＝J_x*(-α₁₄*e₁₄+f₁₄*w₁₄)-(J_y-J_z)*x₂₄*x₄₂

其中， $e_{14}=x_{14}-{\hat{x}}_{14},$ ${\hat{x}}_{14}=-{\mathrm{\α}}_{13}*e_{13}+{\stackrel{\·}{\mathrm{\φ}}}_d,$ e₁₃＝x₁₃-φ_d， $w_{14}=-(1+\frac{{\mathrm{\α}}_{14}}{s})*e_{14},$ ${\mathrm{\φ}}_d=arcsin(\frac{m}{u_1}*((-{\mathrm{\α}}_{12}*e_{12}+f_{12}*w_{12})*sin\left(x_{41}\right)-(-{\mathrm{\α}}_{22}*e_{22}+f_{22}*w_{22})*cos\left(x_{41}\right)\left)\right),$ φ_d为滚转角的参考信号，α₁₂、f₁₂、α₂₂和f₂₂为反步鲁棒补偿控制器的调节参数， $w_{12}=-\frac{{\mathrm{\Δ}}_{12}}{s+f_{12}}=-(1+\frac{1}{s})*e_{12},$ $w_{22}=-\frac{{\mathrm{\Δ}}_{22}}{s+f_{22}}=-(1+\frac{1}{s})*e_{22},$ $e_{12}=x_{12}-{\hat{x}}_{12},$ 为虚拟输入， 为虚拟输入，为φ_d的导数，为虚拟输入，α₁₃和α₁₄表示反步鲁棒补偿控制器的调节参数；

俯仰通道的控制律为：u₃＝J_y*(-α₂₄*e₂₄+f₂₄*w₂₄)-(J_z-J_x)*x₁₄*x₄₂

其中， $e_{24}=x_{24}-{\hat{x}}_{24},$ ${\hat{x}}_{24}=-{\mathrm{\α}}_{23}*e_{23}+{\stackrel{\·}{\mathrm{\θ}}}_d,$ e₂₃＝x₂₃-θ_d， $w_{24}=-(1+\frac{{\mathrm{\α}}_{24}}{s})*e_{24},$ ${\mathrm{\θ}}_d=arcsin(\frac{m}{u_1*cos\left(x_{13}\right)}*((-{\mathrm{\α}}_{12}*e_{12}+f_{12}*w_{12})*cos\left(x_{41}\right)+(-{\mathrm{\α}}_{22}*e_{22}+f_{22}*w_{22})*sin\left(x_{41}\right)\left)\right),$ θ_d为俯仰角的参考信号，α₁₂、f₁₂、α₂₂和f₂₂为反步鲁棒补偿控制器的调节参数， $w_{12}=-\frac{{\mathrm{\Δ}}_{12}}{s+f_{12}}=-(1+\frac{1}{s})*e_{12},$ $w_{22}=-\frac{{\mathrm{\Δ}}_{22}}{s+f_{22}}=-(1+\frac{1}{s})*e_{22},$ $e_{12}=x_{12}-{\hat{x}}_{12},$ 为虚拟输入， 为虚拟输入，为θ_d的导数，为虚拟输入，α₂₃和α₂₄表示反步鲁棒补偿控制器的调节参数。

8.根据权利要求1所述的方法，其特征在于，还包括：在实践工程中，对四旋翼飞行器机载的传感器采集的四旋翼飞行器的飞行信息进行滤波融合处理；

所述对四旋翼飞行器机载的传感器采集的四旋翼飞行器的飞行信息进行滤波融合处理包括：

通过四旋翼飞行器机载的传感器采集四旋翼飞行器的飞行信息；

通过低通滤波器对采集的四旋翼飞行器的飞行信息进行低通滤波处理，其中，所述低通滤波器是结合实践工程中的四旋翼飞行器确定的；

通过卡尔曼滤波器冗余机制对不同传感器采集的隐含同类型的经过低通滤波处理后的飞行信息进行数据融合，得到目标观测信息；

其中，数据融合包括：陀螺仪传感器、加速度计传感器，磁力计传感器、GPS传感器、超声波传感器、气压计传感器采集的并经过低通滤波后的信息，及建立在此基础上的角速度、欧拉角内环信息、速度、位置外环信息。

9.根据权利要求8所述的方法，其特征在于，还包括：

在实践工程中，对确定的反步鲁棒补偿控制算法进行离散化处理，离散化程度由反步鲁棒补偿控制器的控制性能和传感器的采样频率决定。

10.根据权利要求1所述的方法，其特征在于，所述根据所述四旋翼飞行器数学模型和四旋翼飞行器模型的不确定性、外界干扰、欠驱动、强耦合特性，对所述四旋翼飞行器模型进行分析解耦，确定基于内外环控制的反步鲁棒补偿控制器之后包括：

在理论研究阶段，根据确定的反步鲁棒补偿控制器，建立四旋翼飞行器仿真模型，并调节反步鲁棒补偿控制器的调节参数，直至控制性能满足控制性能要求，对基于反步鲁棒补偿控制器的四旋翼飞行器仿真模型进行初步验证，其中，所述控制性能包括：响应时间、超调、抗干扰能力；

在实践工程中，对基于反步鲁棒补偿控制器的四旋翼飞行器的实际飞行效果进行后期验证。