CN105912011A - 一种四旋翼飞行器姿态的线性自抗扰控制方法 - Google Patents

Abstract

本发明公开了一种四旋翼飞行器姿态的线性自抗扰控制方法，属于无人飞行器自动控制领域。针对机体刚性且严格对称的四旋翼飞行器建立数学模型，利用线性自抗扰控制器结构简单、参数调节简便结合模型已知动态设计了线性扩张状态观测器(LESO)，把各通道间的动态耦合部分视为系统内部不确定干扰，将其与外界干扰作为作用于系统的未知综合扰动。该观测器只对当前系统的未知扰动进行快速估计，降低了观测器的负担，从而提高了对扰动的估计能力。利用LESO对扰动的估计值在俯仰通道的控制输入中引入相应补偿，既可实现对扰动的抑制，又可实现姿态控制。在此基础上，设计了线性状态反馈控制器对扰动进行在线补偿，实现了稳定的姿态控制。

Description

一种四旋翼飞行器姿态的线性自抗扰控制方法

技术领域

本发明涉及飞行器控制技术领域，特别是涉及一种四旋翼飞行器姿态的线性自抗扰控制方法。

背景技术

四旋翼飞行器的姿态控制研究和应用受到了广泛重视。相比固定翼飞行器，四旋翼飞行器能够完成空中定点悬停、低速飞行和室内飞行等任务，具有显著的民用和军用价值。针对其非线性、强耦合、多变量等特性，且在实际四旋翼飞行系统中会受到多种物理效应的作用以及外部环境的干扰，已开展的研究有反步法控制、滑模控制、鲁棒控制等，研究虽都取得了一定进展，但如何设计对模型依赖程度低、闭环系统鲁棒性强的控制器对于实现四旋翼飞行器的高性能控制具有重要的理论和实际意义。

韩京清教授提出的自抗扰控制(ARDC)，可以有效的解决具有复杂结构的不确定系统的控制问题。目前，已经有研究人员将该控制方法应用于四旋翼飞行器系统，达到快速机动和高稳定度的控制要求。针对ADRC参数过多，难以给定参数整定方法等问题，高志强教授提出线性自抗扰控制器(LADRC)。LADRC只有两个调节参数，结构简单，且物理意义明确，使得它更适合应用于实际系统。

本发明提出了一种四旋翼飞行器自抗扰姿态控制的方法，利用模型已知动态设计了线性扩张状态观测器(LESO)，把各通道间的动态耦合部分视为系统内部不确定干扰，将其与外界干扰作为作用于系统的未知综合扰动。该观测器只对当前系统的未知扰动进行快速估计，降低了观测器的负担，从而提高了对扰动的估计能力。在此基础上，设计线性状态反馈控制器对扰动进行在线补偿，实现了稳定的姿态控制。

发明内容

本发明要解决的技术问题是：使用一种提高四旋翼无人机的飞行品质的四旋翼飞行器姿态的线性自抗扰控制方法。

本发明为解决公知技术中存在的技术问题所采取的技术方案是：

一种四旋翼飞行器姿态的线性自抗扰控制方法，其特征在于：包括如下步骤：

步骤101：建立四旋翼系统的动力学模型：具体为：

针对机体刚性且对称的四旋翼飞行器建立数学模型，根据动量矩定理，飞行器绕质心运动的动力学方程表示为：

τ_f-τ_d-τ_g＝Jw+w×Jw

其中：J＝dig(J_x,J_y,J_z)为机体坐标系下的转动惯量矩阵，J_x、J_y、J_z依次为机体绕滚转轴、俯仰轴、偏航轴的转动惯量；w＝[w_x w_y w_z]^T∈R³为机体坐标系下绕上述机体绕滚转轴、俯仰轴、偏航轴三个轴的角速度；用Φ＝[γ θ ψ]^T表示欧拉角，γθψ依次为滚转角、俯仰角、偏航角；则在小角度姿态下，

机体转动力矩来源于：机体受到的升力力矩，空气阻力扭矩，陀螺效应下的扭矩；

τ_f为机体受到的升力力矩，表示为：

$\begin{array}{c}{\mathrm{\τ}}_f=\left[\begin{array}{c}l(F_r-F_l)\\ l(F_f-F_b)\\ M_f+M_b+M_l+M_r\end{array}\right]\\ =\left[\begin{array}{c}{\mathrm{lK}}_f(V_r-V_l)\\ {\mathrm{lK}}_f(V_f-V_b)\\ K_{t,n}(V_r+V_l)+K_{t,c}(V_f+V_b)\end{array}\right]\end{array}$

其中：K_f为执行器驱动旋翼转动产生转矩时，电机电压与旋翼产生升力之间的系数，K_t,n为顺时针力矩系数，K_t,c为逆时针力矩系数，并且K_t,n＝-K_t,c，l为俯仰轴与旋翼中心的距离，F_i、M_i(i＝f,b,l,r)分别为前旋翼，后旋翼，左旋翼，右旋翼产生的升力和力矩，V_i(i＝f,b,l,r)为四个电机电压；

τ_d为空气阻力扭矩，表示为：τ_d＝K_afw

其中：K_af为空气阻力系数，且K_af＝dig(K_afx,K_afy,K_afz)

τ_g为陀螺效应下的力矩，表达式为：

${\mathrm{\τ}}_g={\mathrm{\Σ}}_{i=1}^{4}w\×J_{r}W$

其中：J_r为旋翼转子的转动惯量，q_i(i＝f,b,l,r)是前旋翼，后旋翼，左旋翼，右旋翼四个旋翼的角速度，且W＝[0 0 q_l+q_r-q_b-q_f]^T；

四旋翼飞行器姿态系统的动态模型为：

$\left\{\begin{array}{c}\stackrel{\·\·}{\mathrm{\γ}}=\[{\mathrm{lK}}_f(V_r-V_l)+\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}(J_y-J_z)-K_{afx}\stackrel{\·}{\mathrm{\γ}})\]/J_x-\stackrel{\·}{\mathrm{\θ}}{J}_{rz}(q_l+q_r-q_b-q_f)/J_x\\ \stackrel{\·\·}{\mathrm{\θ}}=\[{\mathrm{lK}}_f(V_f-V_b)+\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\ψ}}(J_z-J_x)-K_{afy}\stackrel{\·}{\mathrm{\θ}})\]/J_y+\stackrel{\·}{\mathrm{\γ}}{J}_{rz}(q_l+q_r-q_b-q_f)/J_y\\ \stackrel{\·\·}{\mathrm{\ψ}}=\[K_{t,n}(V_r+V_l)+K_{t,c}(V_f+V_b)-K_{afz}\stackrel{\·}{\mathrm{\ψ}}\]/J_z+\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\θ}}(J_x-J_y)/J_z\end{array}\right.$

步骤102：建立四旋翼姿态的数学模型如下：

$\left\{\begin{array}{c}\stackrel{\·\·}{\mathrm{\γ}}=\[{\mathrm{lK}}_f(V_r-V_l)+\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}(J_y-J_z)-K_{afx}\stackrel{\·}{\mathrm{\γ}})\]/J_x-\stackrel{\·}{\mathrm{\θ}}{J}_{rz}(q_l+q_r-q_b-q_f)/J_x\\ \stackrel{\·\·}{\mathrm{\θ}}=\[{\mathrm{lK}}_f(V_f-V_b)+\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\ψ}}(J_z-J_x)-K_{afy}\stackrel{\·}{\mathrm{\θ}})\]/J_y+\stackrel{\·}{\mathrm{\γ}}{J}_{rz}(q_l+q_r-q_b-q_f)/J_y\\ \stackrel{\·\·}{\mathrm{\ψ}}=\[K_{t,n}(V_r+V_l)+K_{t,c}(V_f+V_b)-K_{afz}\stackrel{\·}{\mathrm{\ψ}}\]/J_z+\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\θ}}(J_x-J_y)/J_z\end{array}\right.$

当四旋翼飞行器大角度动作时，系统表现为强耦合非线性特性；引入控制量U＝[U₁ U₂ U₃]^T；此时，将整个系统分为滚转通道、俯仰通道、偏航通道，B为多变量变换矩阵：

$U=\left[\begin{array}{c}{U}_1\\ U_2\\ U_3\end{array}\right]=\left[\begin{array}{cccc}0& 0& 1& -1\\ 1& -1& 0& 0\\ -1& -1& 1& 1\end{array}\right]\left[\begin{array}{c}{V}_f\\ V_b\\ V_r\\ V_l\end{array}\right]=Bu$

同时，将三个通道之间的动态耦合部分视为系统内部不确定扰动，则

$\left\{\begin{array}{c}\stackrel{\·\·}{\mathrm{\γ}}={\mathrm{lK}}_f{U}_1/J_x-K_{afx}\stackrel{\·}{\mathrm{\γ}}/J_x+{\mathrm{\ϵ}}_1\\ \stackrel{\·\·}{\mathrm{\θ}}={\mathrm{lK}}_f{U}_2/J_y-K_{afy}\stackrel{\·}{\mathrm{\θ}}/J_y+{\mathrm{\ϵ}}_2\\ \stackrel{\·\·}{\mathrm{\ψ}}=K_{t,n}{U}_3/J_z-K_{afz}\stackrel{\·}{\mathrm{\ψ}}/J_z+{\mathrm{\ϵ}}_3\end{array}\right.$

其中ε_i(i＝1,2,3)为系统内部不确定扰动，

${\mathrm{\ϵ}}_1=\[\stackrel{\·}{\mathrm{\θ}}{J}_{rz}(q_b+q_f-q_l-q_r)+\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}(J_y-J_z)\]/J_x$

${\mathrm{\ϵ}}_2=\[\stackrel{\·}{\mathrm{\γ}}{J}_{rz}(q_l+q_r-q_b-q_f)+\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\ψ}}(J_z-J_x)\]/J_y$

${\mathrm{\ϵ}}_3=\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\θ}}(J_x-J_y)/J_z$

四旋翼飞行器在实际飞行时，将系统内部不确定扰动和外界扰动作为作用于系统的未知综合扰动，则姿态系统的模型为：

$\left\{\begin{array}{c}\stackrel{\·\·}{\mathrm{\γ}}={\mathrm{lK}}_f{U}_1/J_x-K_{afx}\stackrel{\·}{\mathrm{\γ}}/J_x+f_1\\ \stackrel{\·\·}{\mathrm{\θ}}={\mathrm{lK}}_f{U}_2/J_y-K_{afy}\stackrel{\·}{\mathrm{\θ}}/J_y+f_2\\ \stackrel{\·\·}{\mathrm{\ψ}}=K_{t,n}{U}_3/J_z-K_{afz}\stackrel{\·}{\mathrm{\ψ}}/J_z+f_3\end{array}\right.$

其中f_i＝ε_i+d_i(i＝1,2,3)为作用于系统的未知综合扰动，d_i为作用于三个通道的外界扰动；

步骤103：设计自抗扰控制器；

对系统的三个通道分别进行姿态控制；

LESO直接利用输入输出信息对系统的状态以及“综合扰动”进行实时估计；设计线性状态反馈控制器在线补偿综合扰动，实现姿态控制；

步骤1031：俯仰通道自抗扰姿态控制器的设计；

当x₃＝f₂为俯仰通道的扩张状态变量时，f₂可导，则俯仰通道的模型表达为：

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_1=x_2\\ {\stackrel{\·}{x}}_2=x_3+{\mathrm{lK}}_f{U}_2/J_y-K_{afy}{x}_2/J_y\\ {\stackrel{\·}{x}}_3=h\\ \mathrm{\θ}=x_1\end{array}\right.$

其中：U₂为俯仰通道控制输入，-K_afyx₂/J_y为系统已知动态，x₁，x₂，x₃依次为俯仰角，俯仰角速度，俯仰通道综合扰动；

基于已知俯仰角速度信息设计如下LESO对系统的状态和扰动进行观测：

$\left\{\begin{array}{c}{\stackrel{\·}{z}}_1=z_2-{\mathrm{\β}}_1(z_1-x_1)\\ {\stackrel{\·}{z}}_2=z_3-{\mathrm{\β}}_2(z_1-x_1)-{\mathrm{az}}_2+b_0{U}_2\\ {\stackrel{\·}{z}}_3=-{\mathrm{\β}}_3(z_1-x_1)\end{array}\right.$

其中：a＝K_afy/J_y，b₀＝lK_f/J_y，z_i(i＝1,2,3)分别为x_i(i＝1,2,3)的观测值，β_i(i＝1,2,3)为观测器增益，将观测器极点都配置到-w_o，且β₁＝3w_o，β₂＝3w_o ²，β₃＝w_o ³，w_o为观测器带宽；

利用LESO对扰动的估计值在俯仰通道的控制输入中引入相应补偿；设计的线性状态反馈控制器如下：

$\left\{\begin{array}{c}{U}_2=(u_2-z_3)/b_0\\ u_2=k_p({\mathrm{\θ}}_d-z_1)-k_d{z}_2\end{array}\right.$

其中：θ_d为偏航通道给定值，k_p，k_d为控制器增益，且k_p＝w_c ²，k_d＝2w_c，w_c为控制器带宽，u₂为反馈控制量；

重复步骤1031，实现滚转通道和偏航通道的自抗扰姿态控制；

步骤104：闭环稳定性分析；具体为

步骤1041：俯仰通道的闭环稳定性分析

基于频域理论，建立俯仰通道的闭环控制回路，对式和式进行拉氏变换，则

$\left\{\begin{array}{c}{z}_1\left(s\right)=\frac{3w_o{s}^2+3w_{o}s(a+w_o)+{w_o}^3}{{(s+w_o)}^3+as(s+3w_o)}\mathrm{\θ}\left(s\right)+\frac{b_{0}s}{{(s+w_o)}^3+as(s+3w_o)}{U}_2\left(s\right)\\ z_2\left(s\right)=\frac{3{w_o}^2{s}^2+{w_o}^{3}s}{{(s+w_o)}^3+as(s+3w_o)}\mathrm{\θ}\left(s\right)+\frac{(s+3w_o)b_{0}s}{{(s+w_o)}^3+as(s+3w_o)}{U}_2\left(s\right)\\ z_3\left(s\right)=\frac{({w_o}^3{s}^2+{{\mathrm{aw}}_o}^{3}s)}{{(s+w_o)}^3+as(s+3w_o)}\mathrm{\θ}\left(s\right)-\frac{b_0{w_o}^3}{{(s+w_o)}^3+as(s+3w_o)}{U}_2\left(s\right)\end{array}\right.$

$U_2\left(s\right)=\frac{1}{b_0}\left({w_c}^2\right({\mathrm{\θ}}_d\left(s\right)-z_1\left(s\right))-2w_c{z}_2(s)-z_3(s\left)\right)$

通过LESO对俯仰通道扰动项估计，则

其中：为f₂(s)与z₃(s)之间的估计误差；

将俯仰通道控制对象记为：

$\mathrm{\θ}\left(s\right)=\frac{b_0}{s(s+a)}{U}_2\left(s\right)+z_3\left(s\right)+\widetilde{\mathrm{\θ}}\left(s\right)$

得到俯仰角输出θ(s)与俯仰角给定值θ_d(s)和扰动的估计误差之间的关系式：

$\mathrm{\θ}\left(s\right)=G_{\mathrm{\θ}}\left(s\right){\mathrm{\θ}}_d\left(s\right)+G_{\widetilde{\mathrm{\θ}}}\left(s\right)\widetilde{\mathrm{\θ}}\left(s\right)$

其中

$G_{\mathrm{\θ}}\left(s\right)=\frac{{w_c}^2}{s^2+(2w_c+a)s+{w_c}^2}$

$G_{\widetilde{\mathrm{\θ}}}\left(s\right)=\frac{s^2+(2w_c+3w_o+a)s+6w_c{w}_o+3{w_o}^2+3{\mathrm{aw}}_o+{w_c}^2}{s^4+c_1{s}^3+c_2{s}^2+c_{3}s+c_4}$

c₁＝3w_o+2a+2w_c

c₂＝(2w_c+a)(3w_o+a)+3w_o ²+3aw_o+w_c ²

c₃＝(3w_o ²+3aw_o)(2w_c+a)+w_c ²(3w_o+a)

c₄＝w_c ²(3w_o ²+3aw_o)

以θ_d(s)为输入，θ(s)为输出，忽略对扰动的估计误差，则俯仰通道闭环传递函数为：

$G_{cl}\left(s\right)=\frac{{w_c}^2}{s^2+(2w_c+a)s+{w_c}^2}$

根据Routh判据，闭环系统稳定的充要条件是：

$\left\{\begin{array}{c}2w_c+a>0\\ {w_c}^2>0\end{array}\right.$

由于a>0，w_c>0，则整个俯仰通道闭环系统稳定；

重复步骤1041，对滚转通道和偏航通道的闭环稳定性进行分析。

本发明具有的优点和积极效果是：

本发明相比于传统的PID控制方法，具有更好的跟踪性能，并且对随机的干扰噪声具有很好地抗干扰性。传统非线性ADRC设计的姿态控制器参数过多，参数整定方法较为困难，而本发明所设计的各通道线性自抗扰控制器(LADRC)只有两个调节参数，结构简单，且物理意义明确，使得它更适合应用于实际系统。本发明设计的线性自抗扰姿态控制器应用于四旋翼飞行器系统，可以达到快速机动和高稳定度的控制要求，具有很好的鲁棒性能和抗扰性能。

附图说明

图1为现有技术的四旋翼的结构示意图；

图2为本发明系统姿态控制结构示意图；

图3为本发明的自抗扰控制器的结构示意图；

图4为本发明的系统半实物实验原理图；

图5为优选实施例1的俯仰角跟踪效果对比图；

图6为优选实施例1的滚转角跟踪效果对比图；

图7为优选实施例1的偏航角跟踪效果对比图；

图8为优选实施例2的俯仰角输出响应效果对比图；

图9为优选实施例2的滚转角输出响应对比图；

图10为优选实施例2的偏航角输出响应对比图。。

具体实施方式

为能进一步了解本发明的发明内容、特点及功效，兹例举以下实施例，并配合附图详细说明如下：

请参阅图1至图10，一种四旋翼飞行器姿态的线性自抗扰控制方法，包括如下步骤：

步骤1：建立四旋翼系统的动力学模型：

针对机体刚性且严格对称的四旋翼飞行器建立数学模型，根据动量矩定理，飞行器绕质心运动的动力学方程可表示为：

τ_f-τ_d-τ_g＝Jw+w×Jw

其中：J＝dig(J_x,J_y,J_z)为机体坐标系下的转动惯量矩阵，J_x、J_y、J_z分别为机体绕滚转轴、俯仰轴、偏航轴的转动惯量。为机体坐标系下绕3个轴的角速度。用Φ＝[γ θ ψ]^T表示欧拉角，分别为滚转角、俯仰角、偏航角。则在小角度姿态下，

考虑能够反应四旋翼飞行器本质特性的模型，机体转动力矩主要来源于：机体受到的升力力矩，空气阻力扭矩，陀螺效应下的扭矩。

τ_f为机体受到的升力力矩，可以表示为：

$\begin{array}{c}{\mathrm{\τ}}_f=\left[\begin{array}{c}l(F_r-F_l)\\ l(F_f-F_b)\\ M_f+M_b+M_l+M_r\end{array}\right]\\ =\left[\begin{array}{c}{\mathrm{lK}}_f(V_r-V_l)\\ {\mathrm{lK}}_f(V_f-V_b)\\ K_{t,n}(V_r+V_l)+K_{t,c}(V_f+V_b)\end{array}\right]\end{array}$

其中：K_f为执行器驱动旋翼转动产生转矩时，电机电压与旋翼产生升力之间的系数，K_t,n为顺时针力矩系数，K_t,c为逆时针力矩系数，并且K_t,n＝-K_t,c，l为俯仰轴与旋翼中心的距离，F_i、M_i(i＝f,b,l,r)分别为前，后，左，右旋翼产生的升力、力矩，V_i(i＝f,b,l,r)为四个电机电压。

τ_d为空气阻力扭矩，可以表示为：τ_d＝K_afw

其中：K_af为空气阻力系数，且K_af＝dig(K_afx,K_afy,K_afz)

τ_g为陀螺效应下的力矩，其表达式为：

${\mathrm{\τ}}_g={\mathrm{\Σ}}_{i=1}^{4}w\×J_{r}W$

其中：J_r为旋翼转子的转动惯量，q_i(i＝f,b,l,r)是前，后，左，右四个旋翼的角速度，且W＝[0 0 q_l+q_r-q_b-q_f]^T

综上所述，四旋翼飞行器姿态系统的动态模型为：

$\left\{\begin{array}{c}\stackrel{\·\·}{\mathrm{\γ}}=\[{\mathrm{lK}}_f(V_r-V_l)+\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}(J_y-J_z)-K_{afx}\stackrel{\·}{\mathrm{\γ}})\]/J_x-\stackrel{\·}{\mathrm{\θ}}{J}_{rz}(q_l+q_r-q_b-q_f)/J_x\\ \stackrel{\·\·}{\mathrm{\θ}}=\[{\mathrm{lK}}_f(V_f-V_b)+\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\ψ}}(J_z-J_x)-K_{afy}\stackrel{\·}{\mathrm{\θ}})\]/J_y+\stackrel{\·}{\mathrm{\γ}}{J}_{rz}(q_l+q_r-q_b-q_f)/J_y\\ \stackrel{\·\·}{\mathrm{\ψ}}=\[K_{t,n}(V_r+V_l)+K_{t,c}(V_f+V_b)-K_{afz}\stackrel{\·}{\mathrm{\ψ}}\]/J_z+\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\θ}}(J_x-J_y)/J_z\end{array}\right.$

步骤2：建立四旋翼姿态的数学模型如下：

$\left\{\begin{array}{c}\stackrel{\·\·}{\mathrm{\γ}}=\[{\mathrm{lK}}_f(V_r-V_l)+\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}(J_y-J_z)-K_{afx}\stackrel{\·}{\mathrm{\γ}})\]/J_x-\stackrel{\·}{\mathrm{\θ}}{J}_{rz}(q_l+q_r-q_b-q_f)/J_x\\ \stackrel{\·\·}{\mathrm{\θ}}=\[{\mathrm{lK}}_f(V_f-V_b)+\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\ψ}}(J_z-J_x)-K_{afy}\stackrel{\·}{\mathrm{\θ}})\]/J_y+\stackrel{\·}{\mathrm{\γ}}{J}_{rz}(q_l+q_r-q_b-q_f)/J_y\\ \stackrel{\·\·}{\mathrm{\ψ}}=\[K_{t,n}(V_r+V_l)+K_{t,c}(V_f+V_b)-K_{afz}\stackrel{\·}{\mathrm{\ψ}}\]/J_z+\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\θ}}(J_x-J_y)/J_z\end{array}\right.$

由此可见，当四旋翼飞行器大角度动作时，系统表现为强耦合非线性特性。引入控制量U＝[U₁ U₂ U₃]^T，其可由四个电机的电压表示。此时，将整个系统分为滚转通道、俯仰通道、偏航通道，B为多变量变换矩阵：

$U=\left[\begin{array}{c}{U}_1\\ U_2\\ U_3\end{array}\right]=\left[\begin{array}{cccc}0& 0& 1& -1\\ 1& -1& 0& 0\\ -1& -1& 1& 1\end{array}\right]\left[\begin{array}{c}{V}_f\\ V_b\\ V_r\\ V_l\end{array}\right]=Bu$

同时，将3个通道之间的动态耦合部分视为系统内部不确定扰动，则

$\left\{\begin{array}{c}\stackrel{\·\·}{\mathrm{\γ}}={\mathrm{lK}}_f{U}_1/J_x-K_{afx}\stackrel{\·}{\mathrm{\γ}}/J_x+{\mathrm{\ϵ}}_1\\ \stackrel{\·\·}{\mathrm{\θ}}={\mathrm{lK}}_f{U}_2/J_y-K_{afy}\stackrel{\·}{\mathrm{\θ}}/J_y+{\mathrm{\ϵ}}_2\\ \stackrel{\·\·}{\mathrm{\ψ}}=K_{t,n}{U}_3/J_z-K_{afz}\stackrel{\·}{\mathrm{\ψ}}/J_z+{\mathrm{\ϵ}}_3\end{array}\right.$

其中ε_i(i＝1,2,3)为系统内部不确定扰动，

${\mathrm{\ϵ}}_1=\[\stackrel{\·}{\mathrm{\θ}}{J}_{rz}(q_b+q_f-q_l-q_r)+\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}(J_y-J_z)\]/J_x$

${\mathrm{\ϵ}}_2=\[\stackrel{\·}{\mathrm{\γ}}{J}_{rz}(q_l+q_r-q_b-q_f)+\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\ψ}}(J_z-J_x)\]/J_y$

${\mathrm{\ϵ}}_3=\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\θ}}(J_x-J_y)/J_z$

四旋翼飞行器在实际飞行时，需要考虑各种外界环境扰动力矩影响。因此，将系统内部不确定扰动和外界扰动作为作用于系统的未知综合扰动，则姿态系统的模型改写为：

$\left\{\begin{array}{c}\stackrel{\·\·}{\mathrm{\γ}}={\mathrm{lK}}_f{U}_1/J_x-K_{afx}\stackrel{\·}{\mathrm{\γ}}/J_x+f_1\\ \stackrel{\·\·}{\mathrm{\θ}}={\mathrm{lK}}_f{U}_2/J_y-K_{afy}\stackrel{\·}{\mathrm{\θ}}/J_y+f_2\\ \stackrel{\·\·}{\mathrm{\ψ}}=K_{t,n}{U}_3/J_z-K_{afz}\stackrel{\·}{\mathrm{\ψ}}/J_z+f_3\end{array}\right.$

其中f_i＝ε_i+d_i(i＝1,2,3)为作用于系统的未知综合扰动，d_i为作用于3个通道的外界扰动；

步骤3：设计自抗扰控制器

四旋翼飞行器的姿态控制是整个飞行控制的关键和基础，对系统的3个通道分别进行姿态控制，其结果将提高飞行品质。

LESO可直接利用输入输出信息对系统的状态以及“综合扰动”进行精确的实时估计，降低因使用高精度陀螺仪提高的设计成本。进而设计线性状态反馈控制器在线补偿综合扰动，实现姿态控制。由于补偿的存在，系统能够获得对扰动的抑制能力，从而提高了系统的鲁棒性。

步骤3.1：俯仰通道自抗扰姿态控制器的设计

令x₃＝f₂为俯仰通道的扩张状态变量，f₂可导，则俯仰通道的模型可重新表达为：

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_1=x_2\\ {\stackrel{\·}{x}}_2=x_3+{\mathrm{lK}}_f{U}_2/J_y-K_{afy}{x}_2/J_y\\ {\stackrel{\·}{x}}_3=h\\ \mathrm{\θ}=x_1\end{array}\right.$

其中：U₂为俯仰通道控制输入，-K_afyx₂/J_y为系统已知动态，x₁，x₂，x₃分别为俯仰角，俯仰角速度，俯仰通道综合扰动。

基于已知俯仰角速度信息设计如下LESO对系统的状态和扰动进行观测：

$\left\{\begin{array}{c}{\stackrel{\·}{z}}_1=z_2-{\mathrm{\β}}_1(z_1-x_1)\\ {\stackrel{\·}{z}}_2=z_3-{\mathrm{\β}}_2(z_1-x_1)-{\mathrm{az}}_2+b_0{U}_2\\ {\stackrel{\·}{z}}_3=-{\mathrm{\β}}_3(z_1-x_1)\end{array}\right.$

其中：a＝K_afy/J_y，b₀＝lK_f/J_y，z_i(i＝1,2,3)分别为x_i(i＝1,2,3)的观测值，β_i(i＝1,2,3)为观测器增益，将观测器极点都配置到-w_o，且β₁＝3w_o，β₂＝3w_o ²，β₃＝w_o ³，w_o为观测器带宽。

在此基础上，利用LESO对扰动的估计值在俯仰通道的控制输入中引入相应补偿，既可实现对扰动的抑制，又可实现姿态控制。设计的线性状态反馈控制器如下：

$\left\{\begin{array}{c}{U}_2=(u_2-z_3)/b_0\\ u_2=k_p({\mathrm{\θ}}_d-z_1)-k_d{z}_2\end{array}\right.$

其中：θ_d为偏航通道给定值，k_p，k_d为控制器增益，且k_p＝w_c ²，k_d＝2w_c，w_c为控制器带宽，u₂为反馈控制量。

同样的方法，可以实现步骤3.2滚转通道和步骤3.3偏航通道的自抗扰姿态控制。

步骤4：闭环稳定性分析

步骤4.1：俯仰通道的闭环稳定性分析

基于频域理论，建立俯仰通道的闭环控制回路，对式和式进行拉氏变换，则

$\left\{\begin{array}{c}{z}_1\left(s\right)=\frac{3w_o{s}^2+3w_{o}s(a+w_o)+{w_o}^3}{{(s+w_o)}^3+as(s+3w_o)}\mathrm{\θ}\left(s\right)+\frac{b_{0}s}{{(s+w_o)}^3+as(s+3w_o)}{U}_2\left(s\right)\\ z_2\left(s\right)=\frac{3{w_o}^2{s}^2+{w_o}^{3}s}{{(s+w_o)}^3+as(s+3w_o)}\mathrm{\θ}\left(s\right)+\frac{(s+3w_o)b_{0}s}{{(s+w_o)}^3+as(s+3w_o)}{U}_2\left(s\right)\\ z_3\left(s\right)=\frac{({w_o}^3{s}^2+{{\mathrm{aw}}_o}^{3}s)}{{(s+w_o)}^3+as(s+3w_o)}\mathrm{\θ}\left(s\right)-\frac{b_0{w_o}^3}{{(s+w_o)}^3+as(s+3w_o)}{U}_2\left(s\right)\end{array}\right.$

$U_2\left(s\right)=\frac{1}{b_0}\left({w_c}^2\right({\mathrm{\θ}}_d\left(s\right)-z_1\left(s\right))-2w_c{z}_2(s)-z_3(s\left)\right)$

通过LESO对俯仰通道扰动项估计，则

其中：为f₂(s)与z₃(s)之间的估计误差。

可将俯仰通道控制对象记为

$\mathrm{\θ}\left(s\right)=\frac{b_0}{s(s+a)}{U}_2\left(s\right)+z_3\left(s\right)+\widetilde{\mathrm{\θ}}\left(s\right)$

得到俯仰角输出θ(s)与俯仰角给定值θ_d(s)和扰动的估计误差之间的关系式：

$\mathrm{\θ}\left(s\right)=G_{\mathrm{\θ}}\left(s\right){\mathrm{\θ}}_d\left(s\right)+G_{\widetilde{\mathrm{\θ}}}\left(s\right)\widetilde{\mathrm{\θ}}\left(s\right)$

其中

$G_{\mathrm{\θ}}\left(s\right)=\frac{{w_c}^2}{s^2+(2w_c+a)s+{w_c}^2}$

$G_{\widetilde{\mathrm{\θ}}}\left(s\right)=\frac{s^2+(2w_c+3w_o+a)s+6w_c{w}_o+3{w_o}^2+3{\mathrm{aw}}_o+{w_c}^2}{s^4+c_1{s}^3+c_2{s}^2+c_{3}s+c_4}$

c₁＝3w_o+2a+2w_c

c₂＝(2w_c+a)(3w_o+a)+3w_o ²+3aw_o+w_c ²

c₃＝(3w_o ²+3aw_o)(2w_c+a)+w_c ²(3w_o+a)

c₄＝w_c ²(3w_o ²+3aw_o)

以θ_d(s)为输入，θ(s)为输出，忽略对扰动的估计误差，则俯仰通道闭环传递函数为：

$G_{cl}\left(s\right)=\frac{{w_c}^2}{s^2+(2w_c+a)s+{w_c}^2}$

根据Routh判据，可以证明，闭环系统稳定的充要条件是：

$\left\{\begin{array}{c}2w_c+a>0\\ {w_c}^2>0\end{array}\right.$

由于a>0，w_c>0，则整个俯仰通道闭环系统稳定。类似地，可以对步骤4.2滚转通道，步骤4.3偏航通道的闭环稳定性进行分析。

将自抗扰控制技术应用到四旋翼飞行控制中，首先考虑四旋翼系统的基本结构和动力学特性。如图1所示，四旋翼飞行器的机体固定联接在一个刚性十字交叉结构上，并由四个相互独立的电机驱动旋翼转动提供飞行动力，通过改变电机转动速度可以控制旋翼的转速，最终实现飞行器的飞行姿态和位置控。由于四旋翼飞行器有四个驱动即四个输入量，但是有六个自由度的输出量，因此四旋翼飞行器是一个典型欠驱动强耦合系统。位于同一对角线上的旋翼同向旋转，而相邻旋翼反向旋转。以前旋翼作为机体的机头，后旋翼作为机体的机尾。将前后旋翼作为一组正桨逆时针旋转，左右旋翼作为一组负桨顺时针旋转，这样设计可以消除机体自身的反扭力矩。在这种旋转模式下，四旋翼飞行器可实现的主要运动状态有：垂直运动、俯仰运动、滚转运动、偏航运动等。

本发明的姿态控制结构示意图如图2所示，对系统的3个通道分别进行姿态控制，其结果将提高飞行品质。具体步骤如步骤3.1至3.3所述，整个系统闭环稳定性分析如步骤4.1至4.3所述。

本发明的自抗扰控制器的结构示意图如图3所示，LESO可直接利用输入输出信息对系统的状态以及“综合扰动”进行精确的实时估计，降低因使用高精度陀螺仪提高的设计成本。进而设计线性状态反馈控制器在线补偿综合扰动，实现姿态控制。由于补偿的存在，系统能够获得对扰动的抑制能力，从而提高了系统的鲁棒性。

不论是ESO或LESO观测的总扰动中都包含系统已知动态部分，这样将增加ESO的观测负担，提高设计成本。因此对LESO进行重构，设计改进的LARDC，其中LESO只对未知的扰动及不确定部分进行观测。

通过俯仰通道，说明改进的LADRC设计：

将俯仰通道动态耦合部分以及外界扰动d的共同作用作为俯仰通道的扩张状态变量x₃，且f₂可导，则俯仰通道的模型可重新表达为：

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_1=x_2\\ {\stackrel{\·}{x}}_2=x_3+{\mathrm{lK}}_f{U}_2/J_y-K_{afy}{x}_2/J_y\\ {\stackrel{\·}{x}}_3=h\\ \mathrm{\θ}=x_1\end{array}\right.$

其中：U₂为俯仰通道控制输入，-K_afyx₂/J_y为系统已知动态，x₁，x₂，x₃分别为俯仰角，俯仰角速度，俯仰通道综合扰动。

基于已知俯仰角速度信息设计如下LESO对系统状态和扰动进行观测：

$\left\{\begin{array}{c}{\stackrel{\·}{z}}_1=z_2-{\mathrm{\β}}_1(z_1-x_1)\\ {\stackrel{\·}{z}}_2=z_3-{\mathrm{\β}}_2(z_1-x_1)-{\mathrm{az}}_2+b_0{U}_2\\ {\stackrel{\·}{z}}_3=-{\mathrm{\β}}_3(z_1-x_1)\end{array}\right.$

其中：a＝K_afy/J_y，b₀＝lK_f/J_y，z_i(i＝1,2,3)分别为x_i(i＝1,2,3)的观测值，β_i(i＝1,2,3)为观测器增益，将观测器极点都配置到-w_o，且β₁＝3w_o，β₂＝3w_o ²，β₃＝w_o ³，w_o为观测器带宽。

在此基础上，利用重构LESO对扰动的估计值在俯仰通道的控制输入中引入相应补偿，既可实现对扰动的抑制，又可实现姿态控制，设计的线性状态反馈控制器如下：

$\left\{\begin{array}{c}{U}_2=\frac{u_2-z_3}{b_0}\\ u_2=k_p({\mathrm{\θ}}_d-z_1)-k_d{z}_2\end{array}\right.$

其中：θ_d为俯仰通道给定值，k_p，k_d为控制器增益，且k_p＝w_c ²，k_d＝2w_c，w_c为控制器带宽，u₂为反馈控制量。

同理，可以实现滚转通道和偏航通道基于改进LADRC的姿态控制。

图4为实施例1、2的系统信息传输结构。

实施例1：

对系统进行稳定跟踪控制，选取四旋翼飞行器的初始姿态角为[γ₀ θ₀ ψ₀]^T＝[0° 0° 0°]^T，将姿态角的理想值都设置为幅值为3°，频率为0.05Hz的方波信号，实验时间为40s。姿态控制器参数见表1。

表1 控制器参数

最终系统的姿态跟踪结果及线性扩张状态观测器对姿态角的观测值如图5至图7所示。定义如下几项性能指标，姿态角首次跟踪上理想值的时间，用t_s表示；机体进入稳态以后，系统姿态角实际跟踪值与理想值之间误差的均方根，用σ₁表示；线性扩张状态观测器对系统姿态角的观测值与姿态角实际跟踪值之间误差的均方根，用σ₂表示，姿态角的最大超调量用M_p表示，则3通道各项性能指标见表2.

表2 跟踪实验的性能指标

图5至图7和表2表明，俯仰角、滚转角和偏航角均能快速、准确跟踪给定姿态信号，线性扩张状态观测器能够有效的对姿态角进行观测。在姿态跟踪过程中，系统能够同时兼顾稳态性能和动态性能。

实施例2：

对系统进行抗扰性试验。选取四旋翼飞行器的初始姿态角[γ₀ θ₀ ψ₀]^T＝[-3° -3° -3°]^T，当系统运行到14s时，在稳定的俯仰通道中加入外界扰动力矩，检验自抗扰姿态控制器对扰动力矩的抑制效果，系统的输出响应如图8至图10所示。扰动消失后，系统的调整时间为T_s，此时，系统3个通道的各项性能指标见表3。

表3 扰动作用下的性能指标

由图8至图10和表3表明，俯仰通道在扰动作用时出现了较大超调，但是在干扰消失后仍可快速稳定地跟踪理想信号，且偏航通道和滚转通道几乎未受扰动影响。因此，所设计的自抗扰姿态控制器能够有效抑制和消除通道间的耦合以及外界扰动带来的影响，具有一定的鲁棒性。

本发明对四旋翼飞行器的姿态控制问题进行研究，利用四旋翼飞行器姿态系统的动态模型信息，并考虑模型的内部不确定性和外界扰动等因素，设计线性扩张状态观测器。在此基础上，通过线性状态反馈实现姿态控制和扰动的动态补偿。利用频域理论，分析了闭环系统的稳定性。实施例结果表明，本发明所设计的自抗扰姿态控制器能够有效实现四旋翼飞行器的鲁棒跟踪控制，且系统具有良好的动态性能和稳态性能。

以上对本发明的实施例进行了详细说明，但所述内容仅为本发明的较佳实施例，不能被认为用于限定本发明的实施范围。凡依本发明申请范围所作的均等变化与改进等，均应仍归属于本发明的专利涵盖范围之内。

Claims (1)

1.一种四旋翼飞行器姿态的线性自抗扰控制方法；其特征在于：包括如下步骤：

步骤101：建立四旋翼系统的动力学模型：具体为：

针对机体刚性且对称的四旋翼飞行器建立数学模型，根据动量矩定理，飞行器绕质心运动的动力学方程表示为：

τ_f-τ_d-τ_g＝Jw+w×Jw

其中：J＝dig(J_x，J_y，J_z)为机体坐标系下的转动惯量矩阵，J_x、J_y、J_z依次为机体绕滚转轴、俯仰轴、偏航轴的转动惯量；w＝[w_x w_y w_z]^T∈R³为机体坐标系下绕上述机体绕滚转轴、俯仰轴、偏航轴三个轴的角速度；用Φ＝[γ θ ψ]^T表示欧拉角，γ θ ψ依次为滚转角、俯仰角、偏航角；则在小角度姿态下，

机体转动力矩来源于：机体受到的升力力矩，空气阻力扭矩，陀螺效应下的扭矩；

τ_f为机体受到的升力力矩，表示为：

$\begin{array}{c}{\mathrm{\τ}}_f=\left[\begin{array}{c}l\left(F_r-F_l\right)\\ l\left(F_f-F_b\right)\\ M_f+M_b+M_l+M_r\end{array}\right]\\ =\left[\begin{array}{c}{\mathrm{lK}}_f\left(V_r-V_l\right)\\ {\mathrm{lK}}_f\left(V_f-V_b\right)\\ K_{t,n}\left(V_r+V_l\right)+K_{t,c}\left(V_f+V_b\right)\end{array}\right]\end{array}$

其中：K_f为执行器驱动旋翼转动产生转矩时，电机电压与旋翼产生升力之间的系数，K_t，n为顺时针力矩系数，K_t，c为逆时针力矩系数，并且K_t，n＝-K_t，c，l为俯仰轴与旋翼中心的距离，F_i、M_i(i＝f，b，l，r)分别为前旋翼，后旋翼，左旋翼，右旋翼产生的升力和力矩，V_i(i＝f，b，l，r)为四个电机电压；

τ_d为空气阻力扭矩，表示为：τ_d＝K_afw

其中：K_af为空气阻力系数，且K_af＝dig(K_afx，K_afy，K_afz)

τ_g为陀螺效应下的力矩，表达式为：

${\mathrm{\τ}}_g={\mathrm{\Σ}}_{i=1}^{4}w\×J_{r}W$

其中：J_r为旋翼转子的转动惯量，qi(i＝f，b，l，r)是前旋翼，后旋翼，左旋翼，右旋翼四个旋翼的角速度，且W＝[0 0 q_l+q_r -q_b -q_f]^T；

四旋翼飞行器姿态系统的动态模型为：

$\left\{\begin{array}{c}\stackrel{\·\·}{\mathrm{\γ}}=\[{\mathrm{lK}}_f\left(V_r-V_l\right)+\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}\left(J_y-J_z\right)-K_{afx}\stackrel{\·}{\mathrm{\γ}}\]/J_x-\stackrel{\·}{\mathrm{\θ}}{J}_{rz}\left(q_l+q_r-q_b-q_f\right)/J_x\\ \stackrel{\·\·}{\mathrm{\θ}}=\[{\mathrm{lK}}_f\left(V_f-V_b\right)+\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\ψ}}\left(J_z-J_x\right)-K_{afy}\stackrel{\·}{\mathrm{\θ}}\]/J_y+\stackrel{\·}{\mathrm{\γ}}{J}_{rz}\left(q_l+q_r-q_b-q_f\right)/J_y\\ \stackrel{\·\·}{\mathrm{\ψ}}=\[K_{t,n}\left(V_r+V_l\right)+K_{t,c}\left(V_f+V_b\right)-K_{afz}\stackrel{\·}{\mathrm{\ψ}}\]/J_z+\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\θ}}\left(J_x-J_y\right)/J_z\end{array}\right.$

步骤102：建立四旋翼姿态的数学模型如下：

$\left\{\begin{array}{c}\stackrel{\·\·}{\mathrm{\γ}}=\[{\mathrm{lK}}_f\left(V_r-V_l\right)+\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}\left(J_y-J_z\right)-K_{afx}\stackrel{\·}{\mathrm{\γ}}\]/J_x-\stackrel{\·}{\mathrm{\θ}}{J}_{rz}\left(q_l+q_r-q_b-q_f\right)/J_x\\ \stackrel{\·\·}{\mathrm{\θ}}=\[{\mathrm{lK}}_f\left(V_f-V_b\right)+\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\ψ}}\left(J_z-J_x\right)-K_{afy}\stackrel{\·}{\mathrm{\θ}}\]/J_y+\stackrel{\·}{\mathrm{\γ}}{J}_{rz}\left(q_l+q_r-q_b-q_f\right)/J_y\\ \stackrel{\·\·}{\mathrm{\ψ}}=\[K_{t,n}\left(V_r+V_l\right)+K_{t,c}\left(V_f+V_b\right)-K_{afz}\stackrel{\·}{\mathrm{\ψ}}\]/J_z+\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\θ}}\left(J_x-J_y\right)/J_z\end{array}\right.$

当四旋翼飞行器大角度动作时，系统表现为强耦合非线性特性；引入控制量U＝[U₁ U₂ U₃]^T；此时，将整个系统分为滚转通道、俯仰通道、偏航通道，B为多变量变换矩阵：

$U=\left[\begin{array}{c}{U}_1\\ U_2\\ U_3\end{array}\right]=\left[\begin{array}{cccc}0& 0& 1& -1\\ 1& -1& 0& 0\\ -1& -1& 1& 1\end{array}\right]\left[\begin{array}{c}{V}_f\\ V_b\\ V_r\\ V_l\end{array}\right]=Bu$

同时，将三个通道之间的动态耦合部分视为系统内部不确定扰动，则

$\left\{\begin{array}{c}\stackrel{\·\·}{\mathrm{\γ}}={\mathrm{lK}}_f{U}_1/J_x-K_{afx}\stackrel{\·}{\mathrm{\γ}}/J_x+{\mathrm{\ϵ}}_1\\ \stackrel{\·\·}{\mathrm{\θ}}={\mathrm{lK}}_f{U}_2/J_y-K_{afy}\stackrel{\·}{\mathrm{\θ}}/J_y+{\mathrm{\ϵ}}_2\\ \stackrel{\·\·}{\mathrm{\ψ}}=K_{t,n}{U}_3/J_z-K_{afz}\stackrel{\·}{\mathrm{\ψ}}/J_z+{\mathrm{\ϵ}}_3\end{array}\right.$

其中ε_i(i＝1，2，3)为系统内部不确定扰动，

${\mathrm{\ϵ}}_1=\[\stackrel{\·}{\mathrm{\θ}}{J}_{rz}(q_b+q_f-q_l-q_r)+\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}(J_y-J_z)\]/J_x$

${\mathrm{\ϵ}}_2=\[\stackrel{\·}{\mathrm{\γ}}{J}_{rz}(q_l+q_r-q_b-q_f)+\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\ψ}}(J_z-J_x)\]/J_y$

${\mathrm{\ϵ}}_3=\stackrel{\·}{\mathrm{\γ}}\stackrel{\·}{\mathrm{\θ}}(J_x-J_y)/J_z$

四旋翼飞行器在实际飞行时，将系统内部不确定扰动和外界扰动作为作用于系统的未知综合扰动，则姿态系统的模型为：

$\left\{\begin{array}{c}\stackrel{\·\·}{\mathrm{\γ}}={\mathrm{lK}}_f{U}_1/J_x-K_{afx}\stackrel{\·}{\mathrm{\γ}}/J_x+f_1\\ \stackrel{\·\·}{\mathrm{\θ}}={\mathrm{lK}}_f{U}_2/J_y-K_{afy}\stackrel{\·}{\mathrm{\θ}}/J_y+f_2\\ \stackrel{\·\·}{\mathrm{\ψ}}=K_{t,n}{U}_3/J_z-K_{afz}\stackrel{\·}{\mathrm{\ψ}}/J_z+f_3\end{array}\right.$

其中f_i＝ε_i+d_i(i＝1，2，3)为作用于系统的未知综合扰动，d_i为作用于三个通道的外界扰动；

步骤103：设计自抗扰控制器；

对系统的三个通道分别进行姿态控制；

LESO直接利用输入输出信息对系统的状态以及“综合扰动”进行实时估计；设计线性状态反馈控制器在线补偿综合扰动，实现姿态控制；

步骤1031：俯仰通道自抗扰姿态控制器的设计；

当x₃＝f₂为俯仰通道的扩张状态变量时，f₂可导，则俯仰通道的模型表达为：

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_1=x_2\\ {\stackrel{\·}{x}}_2=x_3+{\mathrm{lK}}_f{U}_2/J_y-K_{afy}{x}_2/J_y\\ {\stackrel{\·}{x}}_3=h\\ \mathrm{\θ}=x_1\end{array}\right.$

其中：U₂为俯仰通道控制输入，-K_afyx₂/J_y为系统已知动态，x₁，x₂，x₃依次为俯仰角，俯仰角速度，俯仰通道综合扰动；

基于已知俯仰角速度信息设计如下LESO对系统的状态和扰动进行观测：

$\left\{\begin{array}{c}{\stackrel{\·}{z}}_1=z_2-{\mathrm{\β}}_1(z_1-x_1)\\ {\stackrel{\·}{z}}_2=z_3-{\mathrm{\β}}_2(z_1-x_1)-a{z}_2+b_0{U}_2\\ {\stackrel{\·}{z}}_3=-{\mathrm{\β}}_3(z_1-x_1)\end{array}\right.$

其中：a＝K_afy/J_y，b₀＝lK_f/J_y，z_i(i＝1，2，3)分别为x_i(i＝1，2，3)的观测值，β_i(i＝1，2，3)为观测器增益，将观测器极点都配置到-w_o，且β₁＝3w_o，β₂＝3w_o ²，β₃＝w_o ³，w_o为观测器带宽；

利用LESO对扰动的估计值在俯仰通道的控制输入中引入相应补偿；设计的线性状态反馈控制器如下：

$\left\{\begin{array}{c}{U}_2=(u_2-z_3)/b_0\\ u_2=k_p({\mathrm{\θ}}_d-z_1)-k_d{z}_2\end{array}\right.$

其中：θ_d为偏航通道给定值，k_p，k_d为控制器增益，且k_p＝w_c ²，k_d＝2w_c，w_c为控制器带宽，u₂为反馈控制量；

重复步骤1031，实现滚转通道和偏航通道的自抗扰姿态控制；

步骤104：闭环稳定性分析；具体为

步骤1041：俯仰通道的闭环稳定性分析

基于频域理论，建立俯仰通道的闭环控制回路，对式和式进行拉氏变换，则

$\left\{\begin{array}{c}{z}_1\left(s\right)=\frac{3w_o{s}^2+3w_{o}s(a+w_o)+{w_o}^3}{{(s+w_o)}^3+as(s+3w_o)}\mathrm{\θ}\left(s\right)+\frac{b_{0}s}{{(s+w_o)}^3+as(s+3w_o)}{U}_2\left(s\right)\\ z_2\left(s\right)=\frac{3{w_o}^2{s}^2+{w_o}^{3}s}{{(s+w_o)}^3+as(s+3w_o)}\mathrm{\θ}\left(s\right)+\frac{(s+3w_o)b_{0}s}{{(s+w_o)}^3+as(s+3w_o)}{U}_2\left(s\right)\\ z_3\left(s\right)=\frac{({w_o}^3{s}^2+{{\mathrm{aw}}_o}^{3}s)}{{(s+w_o)}^3+as(s+3w_o)}\mathrm{\θ}\left(s\right)-\frac{b_0{w_o}^3}{{(s+w_o)}^3+as(s+3w_o)}{U}_2\left(s\right)\end{array}\right.$

$U_2\left(s\right)=\frac{1}{b_0}\left({w_c}^2\right({\mathrm{\θ}}_d\left(s\right)-z_1\left(s\right))-2w_c{z}_2(s)-z_3(s\left)\right)$

通过LESO对俯仰通道扰动项估计，则

其中：为f₂(s)与z3(s)之间的估计误差；

将俯仰通道控制对象记为：

$\mathrm{\θ}\left(s\right)=\frac{b_0}{s(s+a)}{U}_2\left(s\right)+z_3\left(s\right)+\widetilde{\mathrm{\θ}}\left(s\right)$

得到俯仰角输出θ(s)与俯仰角给定值θ_d(s)和扰动的估计误差之间的关系式：

$\mathrm{\θ}\left(s\right)=G_{\mathrm{\θ}}\left(s\right){\mathrm{\θ}}_d\left(s\right)+G_{\widetilde{\mathrm{\θ}}}\left(s\right)\widetilde{\mathrm{\θ}}\left(s\right)$

其中

$G_{\mathrm{\θ}}\left(s\right)=\frac{{w_c}^2}{s^2+(2w_c+a)s+{w_c}^2}$

$G_{\widetilde{\mathrm{\θ}}}\left(s\right)=\frac{s^2+(2w_c+3w_o+a)s+6w_c{w}_o+3{w_o}^2+3{\mathrm{aw}}_o+{w_c}^2}{s^4+c_1{s}^3+c_2{s}^2+c_{3}s+c_4}$

c₁＝3w_o+2a+2w_c

c₂＝(2w_c+a)(3w_o+a)+3w_o ²+3aw_o+w_c ²

c₃＝(3w_o ²+3aw_o)(2w_c+a)+w_c ²(3w_o+a)

c₄＝w_c ²(3w_o ²+3aw_o)

以θ_d(s)为输入，θ(s)为输出，忽略对扰动的估计误差，则俯仰通道闭环传递函数为：

$G_{cl}\left(s\right)=\frac{{w_c}^2}{s^2+(2w_c+a)s+{w_c}^2}$

根据Routh判据，闭环系统稳定的充要条件是：

$\left\{\begin{array}{c}2w_c+a>0\\ {w_c}^2>0\end{array}\right.$

由于a>0，w_c>0，则整个俯仰通道闭环系统稳定；

重复步骤1041，对滚转通道和偏航通道的闭环稳定性进行分析。