CN106970646A - 基于自适应积分反步的四旋翼飞行器控制方法 - Google Patents

Abstract

本发明公开了自适应积分反步的四旋翼飞行器控制方法，属于自动控制技术领域。本发明包括双闭环控制结构，外环为位移控制器，由高度控制器和水平位移控制器所组成，分别由z、y、x的期望值z_d、y_d、x_d与其实际反馈值做差后，经过自适应积分反步控制算法求出控制高度和水平位移的输入项即U₁，u_x和u_y，再进入内环即姿态控制器，由u_x和u_y反解算出俯仰和横滚两个姿态角的期望值φ_d、θ_d，与实际反馈值做差，而后经过积分反步控制算法，获得控制俯仰角和横滚角的输入项即U₂，U₃，控制航向角的输入项U₄也如此。

Description

基于自适应积分反步的四旋翼飞行器控制方法

技术领域

本发明涉及基于自适应积分反步的四旋翼飞行器控制方法，属于自动控制技术领域。

背景技术

过去二十年中，无人机在世界各地的军民两用领域都得以扩大应用。无人机目前用于军事各个部门，从侦查，监测，情报收集到战场损害评估等。民用领域包括遥感，运输，勘探和科学研究等。由于航空领域多样化的使命，无人机发挥越来越重要的作用。

反步法(Backstepping)是以Lyapunov控制理论为基础，要求系统方程为严反馈形式，一种由前向后递推的设计方法。其主要优点是可以与自适应技术结合使用。自适应控制一般以不同程度的不确定性为研究对象，通过量测信号对被控对象的未知参数进行在线估计，从而实时改变控制器的输入。将自适应控制与反步法相结合，应用于飞行器的飞行控制上，比传统的反步法有了明显的抗干扰性，飞行更稳定，鲁棒性更强。积分反步法是在传统的反步法基础上添加了跟踪误差的积分项，以此来弥补稳态误差，用于控制飞行器稳定飞行，但在飞行器受到外界扰动时，表现稍差。

发明内容

为了克服上述的不足，本发明提出了将自适应控制与积分反步法相结合，应用到受外界环境干扰的飞行器的轨迹跟踪上，既可以减少稳态误差，又可以提高飞行器自身的飞行抗干扰性，大大增强了飞行器的鲁棒性。

本发明采取的技术方案如下：

基于自适应积分反步的四旋翼飞行器控制方法，包括双闭环控制结构，外环为位移控制器，由高度控制器和水平位移控制器所组成，分别由z、y、x的期望值z_d、y_d、x_d与其实际反馈值做差后，经过本发明提出的自适应积分反步控制算法求出控制高度和水平位移的输入项即U₁，u_x和u_y，再进入内环即姿态控制器，由u_x和u_y反解算出俯仰和横滚两个姿态角的期望值φ_d、θ_d，与实际反馈值做差，而后经过积分反步控制算法，获得控制俯仰角和横滚角的输入项即U₂，U₃，控制航向角的输入项U₄也如此。

高度控制器的设计方法如下：

针对二阶系统：

首先，定义一个跟踪误差以及其积分项：

定义为Z方向上的扰动的估计值，为实际的扰动值与扰动估计值的误差，即

一般情况，阵风假定扰动在数值上是未知的和不可测量的，而且是时不变的时不变的，故

定义一个Lyapunov函数：

可以很明显看出V(K₅,e₅)≥0，即正定，对上式进行求导可得：

f₁是虚拟控制量，令

将式(16)带入式(15)即可得，负定，由Lyapunov稳定性可知，稳定，

其次，定义一个二阶的跟踪误差：

那么，

在这里，定义一个Lyapunov函数：

显然是正定的，同样求导可得：

为使负定，必须令

系统稳定时可得：

水平位移控制器的设计方法如下：水平位移控制器分为X方向位移控制与Y方向位移控制，由于两者算法推导相似，故这里只给出Y方向位移控制方法推导：

假设二阶虚拟系统：

同高度Z方向，可以定义并推导得出式(25-32)：

同样，定义一个Lyapunov函数：

这里的ε是一个关于e₄的函数，对上式求导可得：

令

此时，

若

那么，

假设，下式成立：

ζ是一个包含K₃，e₃，e₄三个变量的函数，那么，若

则

当选取时，

可知成立且系统稳定，若选取整合比较后便可得到

同理可以求出X方向上的控制项，即：

其中，

最后，通过反解模块就可将俯仰角和横滚角的期望值求出，即

其中，

u_x＝(cosφsinθcosψ+sinφsinψ)，

u_y＝(cosφsinθcosψ-sinφsinψ)，

姿态控制器的设计方法如下：内环的姿态控制中，我们采用基于积分型的反步控制，且由于对俯仰角、航向角和横滚角的算法推导近似，故只列写俯仰角θ的推导公式：

假设二阶虚拟系统：

同理可定义并推导得到公式(48-53)：

在这里，定义一个Lyapunov函数：

显然是正定的，同样求导可得：

为使负定，必须令

同理可知，该系统是稳定的，由上式可知，

同理可得：

其中：

最后通过式(1)反解并开方可得飞行器的四个电机的运转速度，即：

本发明有益效果：

本文提出的基于积分型的自适应反步的控制算法应用于MIMO且呈非线性强耦合的四旋翼无人机飞行控制系统中，不但能有效减小稳态误差，而且可以防止飞行器因外界较强的阵风环境干扰等不确定因素而偏移期望轨迹。通过与积分反步算法进行比较实验，充分证明系统收敛性好且稳定，稳态误差极小，控制效果较为理想，轨迹跟踪特性较强，具有较强的鲁棒性与抗阵风干扰性。后续工作即将本文的控制算法进行实物飞行试验，进一步测试算法在真实飞行过程中的优化性能。

附图说明

图1飞行器的速度。

图2飞行器的速度绝对误差。

图3飞行器的位置。

图4飞行器的位置绝对误差。

图5三维轨迹。

图6俯视轨迹。

图7飞行器的姿态角。

图8飞行器的姿态角绝对误差。

图9飞行器的控制输入。

图10飞行器电机转速。

图11阵风干扰估计的绝对误差。

具体实施方式

我们设定的期望轨迹如式(59)，初始状态为：X＝[0_12×1]。本文拟设定飞行器受到的外界阵风干扰为D＝[D_x,D_y,D_y]^T＝[20,20,20]^T，即三个方向上的干扰力均为20N的较强阵风。

从图1可以看出，uvw分别为飞行器机体坐标轴xyz三个方向上的速度，可以清晰地看出在外界环境即阵风的干扰下，采用自适应积分反步控制可以快速跟踪期望速度，而积分反步控制则反映出跟踪迟缓的问题。

图2将飞行器的三个方向速度uvw与期望速度进行做差并取绝对值，可以看出采用自适应积分反步控制算法要比只是积分反步算法的速度绝对误差值小很多，尤其是z方向上的速度w。

图3展示了飞行器的xyz三个方向上的位移，可以清楚的看出，采用自适应积分反步算法可以较快地跟踪期望位移，而积分反步法跟踪效果稍差。

图4同样对比了运用自适应积分反步算法和积分反步算法在xyz三个方向上的位移跟踪绝对误差，可以看出自适应积分反步算法可以快速地将跟踪误差降低到很小。

图5展示了飞行器跟踪轨迹的三维视图，立体地观看出飞行器采用自适应积分反步算法在轨迹跟踪任务上的优越性。

图6展示了飞行器进行轨迹跟踪任务的三维俯视图，可以看出飞行器采用自适应积分反步法进行轨迹跟踪，一开始就可以快速跟踪上期望轨迹，而不需要很长的过度时间，轨迹基本与期望轨迹吻合。

图7展示了飞行器在受到较强阵风干扰时的三个姿态角的变化示意图，从图中可以看出应用自适应积分反步算法来控制飞行器的姿态角，可以更快地跟踪期望姿态角，几乎吻合。

图8展示了飞行器在进行轨迹跟踪任务时，因受到持续阵风干扰，姿态角与期望姿态角有一定的绝对误差，但是应用自适应积分反步法控制的姿态角的绝对误差值稳定在一个极小的范围内。

图9展示了飞行器在运行自适应积分反步控制算法时进行轨迹跟踪任务的四个控制输入随时间变化的示意图。

图10展示的是飞行器在进行轨迹跟踪任务时的四个电机的转速。

图11展示了飞行器在未知强度的阵风干扰下，运用自适应积分反步控制算法对未知阵风干扰进行估计与实际阵风的绝对误差，且其数值非常之小。

Claims (4)

1.基于自适应积分反步的四旋翼飞行器控制方法，其特征在于：包括双闭环控制结构，外环为位移控制器，由高度控制器和水平位移控制器所组成，分别由z、y、x的期望值z_d、y_d、x_d与其实际反馈值做差后，经过自适应积分反步控制算法求出控制高度和水平位移的输入项即U₁，u_x和u_y，再进入内环即姿态控制器，由u_x和u_y反解算出俯仰和横滚两个姿态角的期望值φ_d、θ_d，与实际反馈值做差，而后经过积分反步控制算法，获得控制俯仰角和横滚角的输入项即U₂，U₃，控制航向角的输入项U₄也如此。

2.根据权利要求1所述的基于自适应积分反步的四旋翼飞行器控制方法，其特征在于：高度控制器的设计方法如下：

针对二阶系统：

$\left\{\begin{array}{c}{\stackrel{\·}{X}}_5=X_6\\ {\stackrel{\·}{X}}_6=\frac{\cos X_7\cos X_9{U}_1+D_z}{m}-g\end{array}\right.,---\left(10\right)$

首先，定义一个跟踪误差以及其积分项：

$e_5=X_{5d}-X_5,K_5={\∫}_0^t{e}_5\left(\mathrm{\τ}\right)d\mathrm{\τ},---\left(11\right)$

定义为Z方向上的扰动的估计值，为实际的扰动值与扰动估计值的误差，即

${\tilde{D}}_z=D_z-{\hat{D}}_z,---\left(12\right)$

一般情况，阵风假定扰动在数值上是未知的和不可测量的，而且是时不变的时不变的，故

${\stackrel{\·}{\tilde{D}}}_z=-{\stackrel{\·}{\hat{D}}}_z,---\left(13\right)$

定义一个Lyapunov函数：

$V(K_5,e_5)=\frac{1}{2}{e}_5^2+\frac{1}{2}{\mathrm{\λ}}_5{K}_5^2,({\mathrm{\λ}}_5>0),---\left(14\right)$

可以很明显看出V(K₅,e₅)≥0，即正定，对上式进行求导可得：

$V(K_5,e_5)=e_5({\mathrm{\λ}}_5{K}_5+{\stackrel{\·}{e}}_5)=e_5({\mathrm{\λ}}_5{K}_5+{\stackrel{\·}{X}}_{5d}-f_1),---\left(15\right)$

f₁是虚拟控制量，令

$f_1={\mathrm{\λ}}_5{K}_5+{\stackrel{\·}{X}}_{5d}+{\mathrm{\α}}_5{e}_5,({\mathrm{\α}}_5>0),---\left(16\right)$

将式(16)带入式(15)即可得，负定，由Lyapunov稳定性可知，稳定，

其次，定义一个二阶的跟踪误差：

$e_6=f_1-X_6={\mathrm{\λ}}_5{K}_5+{\stackrel{\·}{X}}_{5d}+{\mathrm{\α}}_5{e}_5-{\stackrel{\·}{X}}_5,---\left(17\right)$

那么，

${\stackrel{\·}{e}}_5={\stackrel{\·}{X}}_{5d}-{\stackrel{\·}{X}}_5=e_6-{\mathrm{\α}}_5{e}_5-{\mathrm{\λ}}_5{K}_5,---\left(18\right)$

${\stackrel{\·}{e}}_6={\mathrm{\λ}}_5{e}_5+{\stackrel{\·\·}{X}}_{5d}+{\mathrm{\α}}_5(e_6-{\mathrm{\α}}_5{e}_5-{\mathrm{\λ}}_5{K}_5)-\frac{\cos X_7\cos X_9{U}_1+D_z}{m}+g,---\left(19\right)$

在这里，定义一个Lyapunov函数：

$V(K_5,e_5,e_6,{\tilde{D}}_z)=\frac{1}{2}{e}_5^2+\frac{1}{2}{\mathrm{\λ}}_5{K}_5^2+\frac{1}{2}{e}_6^2+\frac{{\tilde{D}}_z^2}{2{\mathrm{m\γ}}_z},({\mathrm{\γ}}_z>0)---\left(20\right)$

显然是正定的，同样求导可得：

$\stackrel{\·}{V}(K_5,e_5,e_6,{\tilde{D}}_z)=e_5({\mathrm{\λ}}_5{K}_5+{\stackrel{\·}{e}}_5)+e_6{\stackrel{\·}{e}}_6+\frac{{\tilde{D}}_z{\stackrel{\·}{\tilde{D}}}_z}{{\mathrm{m\γ}}_z}=-{\mathrm{\α}}_5{e_5}^2+e_6({\stackrel{\·}{e}}_6+e_5)+\frac{{\tilde{D}}_z{\stackrel{\·}{\tilde{D}}}_z}{{\mathrm{m\γ}}_z},---\left(21\right)$

为使负定，必须令

$-{\mathrm{\α}}_6{e}_6=e_5+{\mathrm{\λ}}_5{e}_5+{\stackrel{\·\·}{X}}_{5d}+{\mathrm{\α}}_5(e_6-{\mathrm{\α}}_5{e}_5-{\mathrm{\λ}}_5{K}_5)-\frac{\cos X_7\cos X_9{U}_1+D_z}{m}+g+\frac{{\tilde{D}}_z{\stackrel{\·}{\tilde{D}}}_z}{{\mathrm{m\γ}}_z{e}_6},({\mathrm{\α}}_6>0),---\left(22\right)$

系统稳定时可得：

$\left\{\begin{array}{c}{U}_1=\frac{m\left(\begin{array}{c}{e}_5+{\mathrm{\λ}}_5{e}_5+{\stackrel{\·\·}{X}}_{5d}\\ +{\mathrm{\α}}_5(e_6-{\mathrm{\α}}_5{e}_5-{\mathrm{\λ}}_5{K}_5)\\ +g+{\mathrm{\α}}_6{e}_6\end{array}\right)-{\hat{D}}_z}{\cos X_7\cos X_9}\\ {\stackrel{\·}{\hat{D}}}_z=-{\mathrm{\γ}}_z{e}_6\end{array}\right..---\left(23\right)$

3.根据权利要求1所述的基于自适应积分反步的四旋翼飞行器控制方法，其特征在于：水平位移控制器的设计方法如下：水平位移控制器分为X方向位移控制与Y方向位移控制，由于两者算法推导相似，故这里只给出Y方向位移控制方法推导：

假设二阶虚拟系统：

$\left\{\begin{array}{c}{\stackrel{\·}{X}}_3=X_4\\ {\stackrel{\·}{X}}_4=\frac{u_y{U}_1+D_y}{m}\end{array}\right.,---\left(24\right)$

同高度Z方向，可以定义并推导得出式(25-32)：

$e_3=X_{3d}-X_3,K_3={\∫}_0^t{e}_3\left(\mathrm{\τ}\right)d\mathrm{\τ},---\left(25\right)$

${\tilde{D}}_y=D_y-{\hat{D}}_y,---\left(26\right)$

$V(K_3,e_3)=\frac{1}{2}{e}_3^2+\frac{1}{2}{\mathrm{\λ}}_3{K}_3^2,({\mathrm{\λ}}_3>0),---\left(27\right)$

$\stackrel{\&CenterDot;}{V}(K_3,e_3)=-{\mathrm{\&alpha;}}_3{e}_3^2<0,({\mathrm{\&alpha;}}_3>0),---\left(28\right)$

$e_4={\mathrm{\&lambda;}}_3{K}_3+{\stackrel{\&CenterDot;}{X}}_{3d}+{\mathrm{\&alpha;}}_3{e}_3-{\stackrel{\&CenterDot;}{X}}_3,---\left(29\right)$

${\stackrel{\&CenterDot;}{e}}_3={\stackrel{\&CenterDot;}{X}}_{3d}-{\stackrel{\&CenterDot;}{X}}_3=e_4-{\mathrm{\&alpha;}}_3{e}_3-{\mathrm{\&lambda;}}_3{K}_3,---\left(30\right)$

${\stackrel{\&CenterDot;}{e}}_4={\mathrm{\&lambda;}}_3{e}_3+{\stackrel{\&CenterDot;\&CenterDot;}{X}}_{3d}+{\mathrm{\&alpha;}}_3(e_4-{\mathrm{\&alpha;}}_3{e}_3-{\mathrm{\&lambda;}}_3{K}_3)-\frac{u_y{U}_1+D_y}{m},---\left(31\right)$

同样，定义一个Lyapunov函数：

$V(K_3,e_3,e_4,{\tilde{D}}_y)=\frac{1}{2}{e}_3^2+\frac{1}{2}{\mathrm{\&lambda;}}_3{K}_3^2+\frac{{\tilde{D}}_y^2}{2{\mathrm{m\&gamma;}}_y}+\mathrm{\&epsiv;},---\left(32\right)$

这里的ε是一个关于e₄的函数，对上式求导可得：

$\stackrel{\&CenterDot;}{V}(K_3,e_3,e_4,{\tilde{D}}_y)=e_3({\mathrm{\&lambda;}}_3{K}_3+{\stackrel{\&CenterDot;}{e}}_3)-\frac{{\tilde{D}}_y}{{\mathrm{m\&gamma;}}_y}{\stackrel{\&CenterDot;}{\hat{D}}}_y+\frac{\&part;V}{\&part;e_4}({\mathrm{\&lambda;}}_3{e}_3+{\stackrel{\&CenterDot;\&CenterDot;}{X}}_{3d}+{\mathrm{\&alpha;}}_3{\stackrel{\&CenterDot;}{e}}_3-\frac{u_y{U}_1+D_y}{m}),---\left(33\right)$

令

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{\hat{D}}}_y=-{\mathrm{\&gamma;}}_y\frac{\&part;V}{\&part;e_4}\\ u_y=u_{y1}+u_{y2}\\ u_{y1}=\frac{m}{U_1}\left({\stackrel{\&CenterDot;\&CenterDot;}{X}}_{3d}-\frac{{\hat{D}}_y}{m}\right)\end{array}\right.,\left({\mathrm{\&gamma;}}_y>0\right),---\left(34\right)$

此时，

$\stackrel{\&CenterDot;}{V}(K_3,e_3,e_4,{\tilde{D}}_y)=e_3({\mathrm{\&lambda;}}_3{K}_3+{\mathrm{\&lambda;}}_3\frac{\&part;V}{\&part;e_4})+\stackrel{\&CenterDot;}{e_3}(e_3+{\mathrm{\&alpha;}}_3\frac{\&part;V}{\&part;e_4})-\frac{u_{y2}{U}_1}{m}\frac{\&part;V}{\&part;e_4},---\left(35\right)$

若

$\left\{\begin{array}{c}{u}_{y2}=u_{y3}+u_{y4}\\ u_{y3}=\frac{m}{U_1}({\mathrm{\&lambda;}}_3{e}_3+{\mathrm{\&alpha;}}_3\stackrel{\&CenterDot;}{e_3})\end{array}\right.,---\left(36\right)$

那么，

$\stackrel{\&CenterDot;}{V}(K_3,e_3,e_4,{\tilde{D}}_y)={\mathrm{\&lambda;}}_3{K}_3{e}_3+{\stackrel{\&CenterDot;}{e}}_3{e}_3-\frac{u_{y4}{U}_1}{m}\frac{\&part;V}{\&part;e_4},---\left(37\right)$

假设，下式成立：

${\mathrm{\&lambda;}}_3{K}_3{e}_3+{\stackrel{\&CenterDot;}{e}}_3{e}_3-\mathrm{\&zeta;}\frac{\&part;V}{\&part;e_4}=0,---\left(38\right)$

ζ是一个包含K₃，e₃，e₄三个变量的函数，那么，若

$u_{y4}=\frac{m}{U_1}(\mathrm{\&zeta;}+\mathrm{\&beta;}\frac{\&part;V}{\&part;e_4}),(\mathrm{\&beta;}>0),---\left(39\right)$

则

$\stackrel{\&CenterDot;}{V}(K_3,e_3,e_4,{\tilde{D}}_y)=-\mathrm{\&beta;}{\left(\frac{\&part;V}{\&part;e_4}\right)}^2<0,---\left(40\right)$

当选取时，

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{\hat{D}}}_y=-{\mathrm{\&gamma;}}_y\left({\mathrm{\&eta;}}_1{K}_3+{\mathrm{\&eta;}}_2{e}_3+{\mathrm{\&eta;}}_3{e}_4\right)\\ u_y=\frac{m}{U_1}\left(\begin{array}{c}{\mathrm{\&lambda;}}_3{e}_3+{\stackrel{\&CenterDot;\&CenterDot;}{X}}_{3d}\\ +{\mathrm{\&alpha;}}_3(e_4-{\mathrm{\&alpha;}}_3{e}_3-{\mathrm{\&lambda;}}_3{K}_3)+\mathrm{\&zeta;}\\ +\mathrm{\&beta;}({\mathrm{\&eta;}}_1{K}_3+{\mathrm{\&eta;}}_2{e}_3+{\mathrm{\&eta;}}_3{e}_4)-\frac{{\hat{D}}_y}{m}\end{array}\right)\end{array}\right.,---\left(41\right)$

可知成立且系统稳定，若选取整合比较后便可得到

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{\hat{D}}}_y=-{\mathrm{\&gamma;}}_y\left({\mathrm{\&eta;}}_1{K}_3+{\mathrm{\&eta;}}_2{e}_3+{\mathrm{\&eta;}}_3{e}_4\right)\\ u_y=\frac{m}{U_1}\left(\begin{array}{c}(1+{\mathrm{\&lambda;}}_3-{\mathrm{\&alpha;}}_3^2)e_3+{\stackrel{\&CenterDot;\&CenterDot;}{X}}_{3d}\\ -{\mathrm{\&alpha;}}_3{\mathrm{\&lambda;}}_3{K}_3+({\mathrm{\&alpha;}}_3+{\mathrm{\&alpha;}}_4)e_4-\frac{{\hat{D}}_y}{m}\end{array}\right)\end{array}\right.,---\left(42\right)$

同理可以求出X方向上的控制项，即：

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{\hat{D}}}_x=-{\mathrm{\&gamma;}}_x\left({\mathrm{\&eta;}}_4{K}_1+{\mathrm{\&eta;}}_5{e}_1+{\mathrm{\&eta;}}_6{e}_2\right)\\ u_x=\frac{m}{U_1}\left(\begin{array}{c}(1+{\mathrm{\&lambda;}}_1-{\mathrm{\&alpha;}}_1^2)e_1+{\stackrel{\&CenterDot;\&CenterDot;}{X}}_{1d}\\ -{\mathrm{\&alpha;}}_1{\mathrm{\&lambda;}}_1{K}_1+({\mathrm{\&alpha;}}_1+{\mathrm{\&alpha;}}_2)e_2-\frac{{\hat{D}}_x}{m}\end{array}\right)\\ {\mathrm{\&alpha;}}_1,{\mathrm{\&alpha;}}_2,{\mathrm{\&eta;}}_4,{\mathrm{\&eta;}}_5,{\mathrm{\&eta;}}_6>0\end{array}\right.,---\left(43\right)$

其中，

$\left\{\begin{array}{c}{e}_1=X_{1d}-X_1\\ K_1={\&Integral;}_0^t{e}_1\left(\mathrm{\&tau;}\right)d\mathrm{\&tau;}\\ e_2={\stackrel{\&CenterDot;}{X}}_{1d}+{\mathrm{\&lambda;}}_1{e}_1+{\mathrm{\&alpha;}}_1{e}_1-{\stackrel{\&CenterDot;}{X}}_1\end{array}\right.,$

最后，通过反解模块就可将俯仰角和横滚角的期望值求出，即

$\left\{\begin{array}{c}{\mathrm{\&phi;}}_d=\arcsin \left(u_x\sin \mathrm{\&psi;}-u_y\cos \mathrm{\&psi;}\right)\\ {\mathrm{\&theta;}}_d=\arcsin \left(\frac{u_x-{\mathrm{sin\&phi;}}_d\sin \mathrm{\&psi;}}{{\mathrm{cos\&phi;}}_d\cos \mathrm{\&psi;}}\right)\end{array}\right.,---\left(44\right)$

其中，

u_x＝(cosφsinθcosψ+sinφsinψ)，

u_y＝(cosφsinθcosψ-sinφsinψ)。

4.根据权利要求1所述的基于积分反步的四旋翼飞行器控制方法，其特征在于：姿态控制器的设计方法如下：内环的姿态控制中，我们采用基于积分型的反步控制，且由于对俯仰角、航向角和横滚角的算法推导近似，故只列写俯仰角θ的推导公式：

假设二阶虚拟系统：

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{X}}_9=X_{10}\\ {\stackrel{\&CenterDot;}{X}}_{10}=X_8{X}_{12}{a}_3+X_8{a}_4{\mathrm{\&omega;}}_d+b_2{U}_3\end{array}\right.,---\left(45\right)$

同理可定义并推导得到公式(48-53)：

$e_9=X_{9d}-X_9,K_9={\&Integral;}_0^t{e}_9\left(\mathrm{\&tau;}\right)d\mathrm{\&tau;},---\left(46\right)$

$V(K_9,e_9)=\frac{1}{2}{e}_9^2+\frac{1}{2}{\mathrm{\&lambda;}}_9{K}_9^2,({\mathrm{\&lambda;}}_9>0),---\left(47\right)$

$\stackrel{\&CenterDot;}{V}(K_9,e_9)=-{\mathrm{\&alpha;}}_9{e}_9^2<0,({\mathrm{\&alpha;}}_9>0),---\left(48\right)$

$e_{10}={\mathrm{\&lambda;}}_9{K}_9+{\stackrel{\&CenterDot;}{X}}_{9d}+{\mathrm{\&alpha;}}_9{e}_9-{\stackrel{\&CenterDot;}{X}}_9,---\left(49\right)$

${\stackrel{\&CenterDot;}{e}}_9={\stackrel{\&CenterDot;}{X}}_{9d}-{\stackrel{\&CenterDot;}{X}}_9=e_{10}-{\mathrm{\&alpha;}}_9{e}_9-{\mathrm{\&lambda;}}_9{K}_9,---\left(50\right)$

${\stackrel{\&CenterDot;}{e}}_{10}={\mathrm{\&lambda;}}_9{e}_9+{\stackrel{\&CenterDot;\&CenterDot;}{X}}_{9d}+{\mathrm{\&alpha;}}_9(e_{10}-{\mathrm{\&alpha;}}_9{e}_9-{\mathrm{\&lambda;}}_9{K}_9)-X_8{X}_{12}{a}_3-X_8{a}_4{\mathrm{\&omega;}}_d-b_2{U}_3,---\left(51\right)$

在这里，定义一个Lyapunov函数：

$V(K_9,e_9,e_{10})=\frac{1}{2}{e}_9^2+\frac{1}{2}{e}_{10}^2+\frac{1}{2}{\mathrm{\&lambda;}}_9{K}_9^2,---\left(52\right)$

显然是正定的，同样求导可得：

$\stackrel{\&CenterDot;}{V}(K_9,e_9,e_{10})=e_9({\mathrm{\&lambda;}}_9{K}_9+{\stackrel{\&CenterDot;}{e}}_9)+e_{10}{\stackrel{\&CenterDot;}{e}}_{10}=-{\mathrm{\&alpha;}}_9{e_9}^2+e_{10}({\stackrel{\&CenterDot;}{e}}_{10}+e_9),---\left(53\right)$

为使负定，必须令

$-{\mathrm{\&alpha;}}_{10}{e}_{10}=e_9+{\mathrm{\&lambda;}}_9{e}_9+{\stackrel{\&CenterDot;\&CenterDot;}{X}}_{9d}+{\mathrm{\&alpha;}}_9(e_{10}-{\mathrm{\&alpha;}}_9{e}_9-{\mathrm{\&lambda;}}_9{K}_9)-X_8{X}_{12}{a}_3-X_8{a}_4{\mathrm{\&omega;}}_d-b_2{U}_3,---\left(54\right)$

同理可知，该系统是稳定的，由上式可知，

$U_3=\frac{e_9+{\mathrm{\&lambda;}}_9{e}_9+{\stackrel{\&CenterDot;\&CenterDot;}{X}}_{9d}+{\mathrm{\&alpha;}}_9(e_{10}-{\mathrm{\&alpha;}}_9{e}_9-{\mathrm{\&lambda;}}_9{K}_9)-X_8{X}_{12}{a}_3-X_8{a}_4{\mathrm{\&omega;}}_d+{\mathrm{\&alpha;}}_{10}{e}_{10}}{b_2},---\left(55\right)$

同理可得：

$\left\{\begin{array}{c}{U}_2=\frac{\begin{array}{c}{e}_7+{\mathrm{\&lambda;}}_7{e}_7+{\stackrel{\&CenterDot;\&CenterDot;}{X}}_{7d}\\ +{\mathrm{\&alpha;}}_7(e_8-{\mathrm{\&alpha;}}_7{e}_7-{\mathrm{\&lambda;}}_7{K}_7)\\ -X_{10}{X}_{12}{a}_1-X_{10}{a}_2{\mathrm{\&omega;}}_d+{\mathrm{\&alpha;}}_8{e}_8\end{array}}{b_2}\\ U_4=\frac{\begin{array}{c}{e}_{11}+{\mathrm{\&lambda;}}_{11}{e}_{11}+{\stackrel{\&CenterDot;\&CenterDot;}{X}}_{11d}\\ +{\mathrm{\&alpha;}}_{11}(e_{12}-{\mathrm{\&alpha;}}_{11}{e}_{11}-{\mathrm{\&lambda;}}_{11}{K}_{11})\\ -X_8{X}_{10}{a}_5+{\mathrm{\&alpha;}}_{12}{e}_{12}\end{array}}{b_3}\end{array}\right.,---\left(56\right)$

其中：

$\left\{\begin{array}{c}{e}_7=X_{7d}-X_7\\ K_7={\&Integral;}_0^t{e}_7\left(\mathrm{\&tau;}\right)d\mathrm{\&tau;}\\ e_8={\stackrel{\&CenterDot;}{X}}_{7d}+{\mathrm{\&lambda;}}_7{e}_7+{\mathrm{\&alpha;}}_7{e}_7-{\stackrel{\&CenterDot;}{X}}_7\\ e_{11}=X_{11d}-X_{11}\\ K_{11}={\&Integral;}_0^t{e}_{11}\left(\mathrm{\&tau;}\right)d\mathrm{\&tau;}\\ e_{12}={\stackrel{\&CenterDot;}{X}}_{11d}+{\mathrm{\&lambda;}}_{11}{e}_{11}+{\mathrm{\&alpha;}}_{11}{e}_{11}-{\stackrel{\&CenterDot;}{X}}_{11}\\ {\mathrm{\&alpha;}}_7,{\mathrm{\&alpha;}}_8,{\mathrm{\&alpha;}}_{11},{\mathrm{\&alpha;}}_{12},{\mathrm{\&lambda;}}_7,{\mathrm{\&lambda;}}_{11}>0\end{array}\right.,---\left(57\right)$

最后通过式(1)反解并开方可得飞行器的四个电机的运转速度，即：

$\left\{\begin{array}{c}{\mathrm{\&omega;}}_1=\sqrt{\frac{U_1}{4b}-\frac{U_3}{2b}-\frac{U_4}{4d}}\\ {\mathrm{\&omega;}}_2=\sqrt{\frac{U_1}{4b}-\frac{U_2}{2b}+\frac{U_4}{4d}}\\ {\mathrm{\&omega;}}_3=\sqrt{\frac{U_1}{4b}+\frac{U_3}{2b}-\frac{U_4}{4d}}\\ {\mathrm{\&omega;}}_4=\sqrt{\frac{U_1}{4b}+\frac{U_2}{2b}+\frac{U_4}{4d}}\end{array}\right..$