CN106325291A - 基于滑模控制律和eso的四旋翼飞行器姿态控制方法及*** - Google Patents

Abstract

本发明涉及自动控制技术领域，具体涉及一种基于滑模控制律和ESO的四旋翼飞行器姿态控制方法及***。本发明提供了一种基于滑模控制律和ESO的四旋翼飞行器姿态控制方法，包括利用滑模控制法，实现对四旋翼三个姿态角回路的控制；利用ESO实现对***总扰的实时估计；将滑模控制模型与ESO相结合，实现对四旋翼飞行器姿态的控制。本发明与现有的姿态控制方法相比，不仅能够实现四旋翼飞行器的姿态稳定，同时对姿态角指令具有良好的跟踪性能，且相对于普通的滑模控制具有更强的抗干扰能力。

Description

基于滑模控制律和ESO的四旋翼飞行器姿态控制方法及***

技术领域

本发明涉及自动控制技术领域，具体涉及一种基于滑模控制律和ESO的四旋翼飞行器姿态控制方法及***。

背景技术

四旋翼无人飞行器在军事、工业、民用领域有着广泛的用途，在侦查监视、电子干扰、武器攻击、交通监视、森林防火、地质勘探、灾害搜救、航拍成图等领域有着广泛的应用前景。上述应用均需要高精度的自动飞行控制，其中，姿态控制是四旋翼飞行器稳定飞行最为基本的控制要求。四旋翼飞行器的姿态运动具有强耦合、多变量、非线性。不确定等特点，且易受外界干扰，因此，姿态控制是其飞行控制的关键技术和难点。

目前，对于四旋翼的姿态控制，有如下几种控制方法：

1.基于学习的控制方法：基于模糊控制、神经网络自适应算法、鲁棒神经网络控制设计四旋翼飞行器的姿态控制器。这类控制器虽然不需要四旋翼的运动模型，但是需要大量的实验和飞行数据来训练***，算法复杂且对硬件要求较高，且大部分研究停留在仿真阶段，没有得到实际应用。

2.线性控制方法：基于PID、LQ控制方法设计四旋翼姿态控制器。该类线性控制器实现较为简单，但是当四旋翼飞行器脱离标称条件或者大范围机动时，其控制器性能会明显下降。目前也有基于自抗扰理论设计的姿态控制方法，但并未在硬件中得到实现。

3.基于模型的非线性控制方法：基于反馈线性化、反步法和滑模控制设计四旋翼控制器，这类控制方法需要依靠精确的模型，在能够得到模型参数的前提下，这类方法能使四旋翼控制性能和运动范围得到增强。如滑模控制具有较好的鲁棒性，但是由于这类控制器并未对***干扰进行实时观测，当干扰较大时，其控制效果也并不理想。

因此，有必要提供一种能够实现难度小、对硬件***要求低，且能有效增强***刚干扰能力，实现对四旋翼飞行器姿态高品质控制的姿态控制方法。

发明内容

针对现有技术中的上述不足之处，本发明提供了一种的基于滑模控制律和ESO的四旋翼飞行器姿态控制，包括：

根据模型参数推导出四旋翼飞行器动力学模型，根据所述四旋翼飞行器动力学模型构造滑模面，根据所述滑模面获取滑模控制律，以实现对四旋翼飞行器三个姿态角回路的控制；

根据四旋翼的滚转角回路、俯仰角回路和偏航角回路的控制输入输出构建ESO，利用所述ESO对***总扰动进行实时估计；

采用所述滑模控制律与所述ESO相结合，以实现对所述四旋翼飞行器姿态的控制。

本发明提供的基于滑模控制律和ESO的四旋翼飞行器姿态控制方法利用ESO(EXtended State Observer，简称ESO，中文意思为：扩张状态观测器)实现对***总扰的实时估计，并将其与滑模控制结合，不仅能够实现四旋翼飞行器的姿态稳定，同时对姿态角指令具有良好的跟踪性能，且相对于普通的滑模控制具有更强的抗干扰能力。

进一步地，四旋翼飞行器动力学模型参数包括转动惯量J_x、J_y、J_z；升力系数c_T；扭矩系数c_Q；电机的时间常数T。

进一步地，获取所述滑模控制模型包括如下步骤：(1)将四旋翼视为刚体，则所述四旋翼飞行器姿态非线性动态方程为：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}=p+qsin\mathrm{\φ}tan\mathrm{\θ}+rcos\mathrm{\φ}tan\mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}=q\cos \mathrm{\φ}-rsin\mathrm{\φ}\\ \stackrel{\·}{\mathrm{\ψ}}=\sec \mathrm{\θ}(q\sin \mathrm{\φ}+rcos\mathrm{\φ})\end{array}\right.$

$\left\{\begin{array}{c}\stackrel{\·}{p}=\[qr(J_y-J_z)+{\mathrm{\τ}}_{\mathrm{\φ}}\]\·1/J_x\\ \stackrel{\·}{q}=\[pr(J_z-J_x)+{\mathrm{\τ}}_{\mathrm{\θ}}\]\·1/J_y\\ \stackrel{\·}{r}=\[pq(J_x-J_y)+{\mathrm{\τ}}_{\mathrm{\ψ}}\]\·1/J_z\end{array}\right.$

其中：φ,θ,ψ分别为滚转角、俯仰角、偏航角；

p,q,r分别为本体角速度ω在本体坐标系x,y,z轴上的分量；

τ_φ,τ_θ,τ_ψ分别为三个本体轴方向的控制力矩；

J_x,J_y,J_z分别为四旋翼沿着x,y,z轴方向的转动惯量；

(2)将所述非线性动态方程改写为状态空间的形式：

$\stackrel{\·}{X}=f(X,U)$

其中，U为输入矢量，X为状态矢量，具体表达式如下：

状态变量：

$\begin{array}{ccc}{x}_1=\mathrm{\φ}& x_3=\mathrm{\θ}& x_5=\mathrm{\ψ}\\ x_2={\stackrel{\·}{x}}_1=\stackrel{\·}{\mathrm{\φ}}& x_4={\stackrel{\·}{x}}_3=\stackrel{\·}{\mathrm{\θ}}& x_6={\stackrel{\·}{x}}_5=\stackrel{\·}{\mathrm{\ψ}}\end{array}$

输入矢量：U＝[U₁ U₂ U₃]^T＝[τ_φ τ_θ τ_ψ]^T

(3)姿态角变化率与本体角速率之间的转换矩阵，在悬停或者小角度飞行情况下，作为单位矩阵，获得所述滑模控制模

型：

$f(X,U)=\left(\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}\\ \frac{(J_y-J_z)}{J_x}\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}+\frac{1}{J_x}{U}_1\\ \stackrel{\·}{\mathrm{\θ}}\\ \frac{(J_z-J_x)}{J_y}\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\ψ}}+\frac{1}{J_y}{U}_2\\ \stackrel{\·}{\mathrm{\ψ}}\\ \frac{(J_x-J_y)}{J_z}\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\θ}}+\frac{1}{J_z}{U}_3\end{array}\right)$

其中，其中:φ,θ,ψ分别为滚转角、俯仰角、偏航角；

p,q,r分别为本体角速度ω在本体坐标系x,y,z轴上的分量；

τ_φ,τ_θ,τ_ψ分别为三个本体轴方向的控制力矩；

J_x,J_y,J_z分别为四旋翼沿着x,y,z轴方向的转动惯量。

进一步地，在根据模型参数推导出四旋翼飞行器动力学模型，根据所述四旋翼飞行器动力学模型构造滑模面的过程中，

构造的滑模面为获得的滑模控制律为：

$U_1=(J_z-J_y)x_4{x}_6-J_x({{\mathrm{\α}}_1}^2{z}_1+k_{1}sign(s_2)+k_2{s}_2-{\stackrel{\·\·}{x}}_{1d})$

$U_2=(J_x-J_z)x_2{x}_6-J_y({{\mathrm{\α}}_2}^2{z}_3+k_{3}sign(s_3)+k_4{s}_3-{\stackrel{\·\·}{x}}_{3d})$

$U_3=(J_y-J_x)x_2{x}_4-J_z({{\mathrm{\α}}_3}^2{z}_5+k_{5}sign(s_4)+k_6{s}_4-{\stackrel{\·\·}{x}}_{5d})$

其中，

$\left\{\begin{array}{c}{z}_3=x_{3d}-x_3\\ s_3=x_4-{\stackrel{\·}{x}}_{3d}-{\mathrm{\α}}_2{z}_3\\ z_5=x_{5d}-x_5\\ s_4=x_6-{\stackrel{\·}{x}}_{5d}-{\mathrm{\α}}_3{z}_5\end{array}\right.$

其中，sign代表符号函数，用近似的连续饱和函数sat(s)来替代所述符号函数；函数表达式为：

sat(s)＝s/(|s|+e)e∈[0,1]，取e＝0.5；

构造二阶滤波器：

其中，为输入值，X_c为输出值。

由此，得到在时域中的表达式：选取阻尼比ξ＝0.8，自然频率ω_n＝4.375。

进一步地，在根据四旋翼的滚转角回路、俯仰角回路和偏航角回路的控制输入输出构建ESO的过程中，

$\left\{\begin{array}{c}e=z_1-y\\ {\stackrel{\·}{z}}_1=z_2-{\mathrm{\β}}_{01}e\\ {\stackrel{\·}{z}}_2=z_3-{\mathrm{\β}}_{02}fal(e,a_1,h)+b_0{U}_1\\ {\stackrel{\·}{z}}_3=-{\mathrm{\β}}_{03}fal(e,a_2,h)\end{array}\right.$

其中，b₀为控制量系数1/J_x，选择预设的参数β₀₁,β₀₂,β₀₃和a_i(i＝1,2)，令a₁＝0.5,a₂＝0.25。

进一步地，在采用所述滑模控制律与所述ESO相结合的过程中，

$\left\{\begin{array}{c}e=z_{1\mathrm{\φ}}-x_1\\ {\stackrel{\·}{z}}_{1\mathrm{\φ}}=z_{2\mathrm{\φ}}-{\mathrm{\β}}_{01}e\\ {\stackrel{\·}{z}}_{2\mathrm{\φ}}=z_{3\mathrm{\φ}}-{\mathrm{\β}}_{02}fal(e,0.5,h)+(1/J_x)U_1\\ {\stackrel{\·}{z}}_{3\mathrm{\φ}}=-{\mathrm{\β}}_{03}fal(e,0.25,h)\\ U_1=(J_z-J_y)x_4{x}_6-J_x({{\mathrm{\α}}_1}^2{z}_1+k_{1}sat(s_2)+k_2{s}_2-{\stackrel{\·\·}{x}}_{1d})-J_x{z}_{3\mathrm{\φ}}\end{array}\right.$

同理，得到其他两个姿态回路的控制输出为：

$U_2=(J_x-J_z)x_2{x}_6-J_y({\mathrm{\α}}_2^2{z}_3+k_{3}sat(s_3)+k_4{s}_3-{\stackrel{\·\·}{x}}_{3d})-J_y{z}_{3\mathrm{\θ}}$

$U_3=(J_y-J_x)x_2{x}_4-J_z({\mathrm{\α}}_3^2{z}_5+k_{5}sat(s_4)+k_6{s}_4-{\stackrel{\·\·}{x}}_{5d})-J_z{z}_{3\mathrm{\ψ}}$

其中，z_3φ,z_3θ,z_3ψ分别为滚转角回路、俯仰角回路和偏航角回路的ESO得到的被扩张状态量。

本发明还提供了基于滑模控制律和ESO的四旋翼飞行器姿态控制***，包括：

姿态角回路控制单元，用于根据模型参数推导出四旋翼飞行器动力学模型，根据所述四旋翼飞行器动力学模型构造滑模面，根据所述滑模面获取滑模控制律，以实现对四旋翼飞行器三个姿态角回路的控制；

***总扰实时估计单元，用于根据四旋翼的滚转角回路、俯仰角回路和偏航角回路的控制输入输出构建ESO，利用所述ESO对***总扰动进行实时估计；

四旋翼飞行器姿态控制单元，用于采用所述滑模控制律与所述ESO相结合，以实现对所述四旋翼飞行器姿态的控制。

进一步地，四旋翼飞行器动力学模型参数包括转动惯量J_x、J_y、J_z；升力系数c_T；扭矩系数c_Q；电机的时间常数T。

进一步地，姿态角回路控制单元获取滑模控制模型包括如下步骤：

(1)将四旋翼视为刚体，则四旋翼飞行器姿态非线性动态方程为：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}=p+qsin\mathrm{\φ}tan\mathrm{\θ}+rcos\mathrm{\φ}tan\mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}=q\cos \mathrm{\φ}-rsin\mathrm{\φ}\\ \stackrel{\·}{\mathrm{\ψ}}=\sec \mathrm{\θ}(q\sin \mathrm{\φ}+rcos\mathrm{\φ})\end{array}\right.$

$\left\{\begin{array}{c}\stackrel{\·}{p}=\[qr(J_y-J_z)+{\mathrm{\τ}}_{\mathrm{\φ}}\]\·1/J_x\\ \stackrel{\·}{q}=\[pr(J_z-J_x)+{\mathrm{\τ}}_{\mathrm{\θ}}\]\·1/J_y\\ \stackrel{\·}{r}=\[pq(J_x-J_y)+{\mathrm{\τ}}_{\mathrm{\ψ}}\]\·1/J_z\end{array}\right.$

其中:φ,θ,ψ分别为滚转角、俯仰角、偏航角；

p,q,r分别为本体角速度ω在本体坐标系x,y,z轴上的分量；

τ_φ,τ_θ,τ_ψ分别为三个本体轴方向的控制力矩；

J_x,J_y,J_z分别为四旋翼沿着x,y,z轴方向的转动惯量；

(2)将所述非线性动态方程改写为状态空间的形式：

$\stackrel{\·}{X}=f(X,U)$

其中，U为输入矢量，X为状态矢量，具体表达式如下：

状态变量：

$\begin{array}{ccc}{x}_1=\mathrm{\φ}& x_3=\mathrm{\θ}& x_5=\mathrm{\ψ}\\ x_2={\stackrel{\·}{x}}_1=\stackrel{\·}{\mathrm{\φ}}& x_4={\stackrel{\·}{x}}_3=\stackrel{\·}{\mathrm{\θ}}& x_6={\stackrel{\·}{x}}_5=\stackrel{\·}{\mathrm{\ψ}}\end{array}$

输入矢量：U＝[U₁ U₂ U₃]^T＝[τ_φ τ_θ τ_ψ]^T

(3)姿态角变化率与本体角速率之间的转换矩阵，在悬停或者小角度飞行情况下，作为单位矩阵，获得所述滑模控制模型：

$f(X,U)=\left(\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}\\ \frac{(J_y-J_z)}{J_x}\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}+\frac{1}{J_x}{U}_1\\ \stackrel{\·}{\mathrm{\θ}}\\ \frac{(J_z-J_x)}{J_y}\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\ψ}}+\frac{1}{J_y}{U}_2\\ \stackrel{\·}{\mathrm{\ψ}}\\ \frac{(J_x-J_y)}{J_z}\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\θ}}+\frac{1}{J_z}{U}_3\end{array}\right)$

其中，φ,θ,ψ分别为滚转角、俯仰角、偏航角；

p,q,r分别为本体角速度ω在本体坐标系x,y,z轴上的分量；

τ_φ,τ_θ,τ_ψ分别为三个本体轴方向的控制力矩；

J_x,J_y,J_z分别为四旋翼沿着x,y,z轴方向的转动惯量。

进一步地，姿态角回路控制单元在根据模型参数推导出四旋翼飞行器动力学模型，根据所述四旋翼飞行器动力学模型构造滑模面，根据所述滑模面获取滑模控制律的过程中，

构造的滑模面为获得的滑模控制律为：

$U_1=(J_z-J_y)x_4{x}_6-J_x({{\mathrm{\α}}_1}^2{z}_1+k_{1}sign(s_2)+k_2{s}_2-{\stackrel{\·\·}{x}}_{1d})$

$U_2=(J_x-J_z)x_2{x}_6-J_y({{\mathrm{\α}}_2}^2{z}_3+k_{3}sign(s_3)+k_4{s}_3-{\stackrel{\·\·}{x}}_{3d})$

$U_3=(J_y-J_x)x_2{x}_4-J_z({{\mathrm{\α}}_3}^2{z}_5+k_{5}sign(s_4)+k_6{s}_4-{\stackrel{\·\·}{x}}_{5d})$

其中，

$\left\{\begin{array}{c}{z}_3=x_{3d}-x_3\\ s_3=x_4-{\stackrel{\·}{x}}_{3d}-{\mathrm{\α}}_2{z}_3\\ z_5=x_{5d}-x_5\\ s_4=x_6-{\stackrel{\·}{x}}_{5d}-{\mathrm{\α}}_3{z}_5\end{array}\right.$

其中，sign代表符号函数，用近似的连续饱和函数sat(s)来替代所述符号函数；函数表达式为：

sat(s)＝s/(|s|+e)e∈[0,1]，取e＝0.5；

构造二阶滤波器：

其中，为输入值，X_c为输出值。

由此，得到在时域中的表达式：选取阻尼比ξ＝0.8，自然频率ω_n＝4.375。

进一步地，***总扰实时估计单元在根据四旋翼的滚转角回路、俯仰角回路和偏航角回路的控制输入输出构建ESO的过程中，

$\left\{\begin{array}{c}e=z_1-y\\ {\stackrel{\·}{z}}_1=z_2-{\mathrm{\β}}_{01}e\\ {\stackrel{\·}{z}}_2=z_3-{\mathrm{\β}}_{02}fal(e,a_1,h)+b_0{U}_1\\ {\stackrel{\·}{z}}_3=-{\mathrm{\β}}_{03}fal(e,a_2,h)\end{array}\right.$

其中，b₀为控制量系数1/J_x，选择预设的参数β₀₁,β₀₂,β₀₃和a_i(i＝1,2)，令a₁＝0.5,a₂＝0.25。

进一步地，四旋翼飞行器姿态控制单元在采用所述滑模控制律与所述ESO相结合的过程中，

$\left\{\begin{array}{c}e=z_{1\mathrm{\φ}}-x_1\\ {\stackrel{\·}{z}}_{1\mathrm{\φ}}=z_{2\mathrm{\φ}}-{\mathrm{\β}}_{01}e\\ {\stackrel{\·}{z}}_{2\mathrm{\φ}}=z_{3\mathrm{\φ}}-{\mathrm{\β}}_{02}fal(e,0.5,h)+(1/J_x)U_1\\ {\stackrel{\·}{z}}_{3\mathrm{\φ}}=-{\mathrm{\β}}_{03}fal(e,0.25,h)\\ U_1=(J_z-J_y)x_4{x}_6-J_x({{\mathrm{\α}}_1}^2{z}_1+k_{1}sat(s_2)+k_2{s}_2-{\stackrel{\·\·}{x}}_{1d})-J_x{z}_{3\mathrm{\φ}}\end{array}\right.$

同理，得到其他两个姿态回路的控制输出为：

$U_2=(J_x-J_z)x_2{x}_6-J_y({{\mathrm{\α}}_2}^2{z}_3+k_{3}sat(s_3)+k_4{s}_3-{\stackrel{\·\·}{x}}_{3d})-J_y{z}_{3\mathrm{\θ}}$

$U_3=(J_y-J_x)x_2{x}_4-J_z({{\mathrm{\α}}_3}^2{z}_5+k_{5}sat(s_4)+k_6{s}_4-{\stackrel{\·\·}{x}}_{5d})-J_z{z}_{3\mathrm{\ψ}}$

其中，z_3φ,z_3θ,z_3ψ分别为滚转角回路、俯仰角回路和偏航角回路的ESO得到的被扩张状态量。

采用上述技术方案，包括以下有益技术效果：

1)利用ESO实现对***总扰的实时估计；

2)并将ESO与滑模控制律结合，不仅能够实现四旋翼飞行器的姿态稳定，同时对姿态角指令具有良好的跟踪性能，且相对于普通的滑模控制具有更强的抗干扰能力。

附图说明

图1示例性地示出了本发明基于滑模和ESO的四旋翼飞行器姿态控制流程示意图；

图2示例性地示出了本发明滑模控制模型与ESO结合的结构原理图；

图3示例性地示出了本发明滑模控制模型中二阶低通滤波器结构图；

图4示例性地示出了本发明四旋翼滚转角响应曲线对比图；

图5示例性地示出了本发明四旋翼俯仰角响应曲线对比图；

图6示例性地示出了本发明四旋翼偏航角响应曲线对比图；

图7示例性地示出了本发明姿态各回路ESO干扰估计量；

图8-1和图8-2示例性地示出了本发明两种控制方法的滚转角实际曲线对比图；

图9-1和图9-2示例性地示出了本发明两种控制方法的俯仰角实际曲线对比图；

图10-1和图10-2示例性地示出了本发明外扰下的两种控制方法姿态角实际曲线对比图；

图11示例性地示出了本发明基于滑模控制律和ESO的四旋翼飞行器姿态控制***逻辑结构示意图。

具体实施方式

下面通过具体的实施例并结合附图对本发明做进一步的详细描述。

如图1所示，本发明提供了一种的基于滑模控制律和ESO的四旋翼飞行器姿态控制，包括：

S110：根据四旋翼飞行器动力学模型参数获取滑模控制模型，根据滑模控制模型构造滑模面，根据滑模面获取滑模控制律，以实现对四旋翼飞行器三个姿态角回路的控制；

S120：根据四旋翼的滚转角、仰俯角或者偏航角建立ESO，利用ESO对***总扰进行实时估计；

S130：采用滑模控制模型与所述ESO相结合，以实现对四旋翼飞行器姿态的控制。

上述为基于滑模控制律和ESO的四旋翼飞行器姿态控制方法的具体流程，在获得滑模控制模型之前，通过实验测试的方法获取模型参数；其中，实际测量的参数包括转动惯量J_x,J_y,J_z；升力系数c_T；扭矩系数c_Q；电机的时间常数T。

在步骤S110中，获得滑模控制模型的步骤包括：

(1)将四旋翼视为刚体，忽略旋转运动产生的陀螺效应，得到的四旋翼飞行器姿态非线性动态方程为：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}=p+qsin\mathrm{\φ}tan\mathrm{\θ}+rcos\mathrm{\φ}tan\mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}=q\cos \mathrm{\φ}-rsin\mathrm{\φ}\\ \stackrel{\·}{\mathrm{\ψ}}=\sec \mathrm{\θ}(q\sin \mathrm{\φ}+rcos\mathrm{\φ})\end{array}\right.$

$\left\{\begin{array}{c}\stackrel{\·}{p}=\[qr(J_y-J_z)+{\mathrm{\τ}}_{\mathrm{\φ}}\]\·1/J_x\\ \stackrel{\·}{q}=\[pr(J_z-J_x)+{\mathrm{\τ}}_{\mathrm{\θ}}\]\·1/J_y\\ \stackrel{\·}{r}=\[pq(J_x-J_y)+{\mathrm{\τ}}_{\mathrm{\ψ}}\]\·1/J_z\end{array}\right.$

其中，φ,θ,ψ分别为滚转角、俯仰角、偏航角；p,q,r分别为本体角速度ω在本体坐标系x,y,z轴上的分量；τ_φ,τ_θ,τ_ψ分别为三个本体轴方向的控制力矩；J_x,J_y,J_z分别为四旋翼沿着x,y,z轴方向的转动惯量。

(2)将上式改写为状态空间的形式：其中U是输入矢量，X是状态矢量，其具体表达式如下：

状态变量：

$\begin{array}{ccc}{x}_1=\mathrm{\φ}& x_3=\mathrm{\θ}& x_5=\mathrm{\ψ}\\ x_2={\stackrel{\·}{x}}_1=\stackrel{\·}{\mathrm{\φ}}& x_4={\stackrel{\·}{x}}_3=\stackrel{\·}{\mathrm{\θ}}& x_6={\stackrel{\·}{x}}_5=\stackrel{\·}{\mathrm{\ψ}}\end{array}$

输入矢量：

U＝[U₁ U₂ U₃]^T＝[τ_φ τ_θ τ_ψ]^T

(3)姿态角变化率与本体角速率之间的转换矩阵，在悬停或者小角度飞行情况下，可以看作为单位矩阵，即由此得到控制模型：

$f(X,U)=\left(\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}\\ \frac{(J_y-J_z)}{J_x}\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}+\frac{1}{J_x}{U}_1\\ \stackrel{\·}{\mathrm{\θ}}\\ \frac{(J_z-J_x)}{J_y}\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\ψ}}+\frac{1}{J_y}{U}_2\\ \stackrel{\·}{\mathrm{\ψ}}\\ \frac{(J_x-J_y)}{J_z}\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\θ}}+\frac{1}{J_z}{U}_3\end{array}\right)$

其中，φ,θ,ψ分别为滚转角、俯仰角、偏航角；

p,q,r分别为本体角速度ω在本体坐标系x,y,z轴上的分量；

τ_φ,τ_θ,τ_ψ分别为三个本体轴方向的控制力矩；

J_x,J_y,J_z分别为四旋翼沿着x,y,z轴方向的转动惯量。

根据滑模控制模型构造滑模面：

构造扩展李雅普诺夫函数：令

$\begin{array}{c}{\stackrel{\·}{s}}_2=-k_{1}sign\left(s_2\right)-k_2{s}_2\\ ={\stackrel{\·}{x}}_2-{\stackrel{\·\·}{x}}_{1d}-{\mathrm{\α}}_1{\stackrel{\·}{z}}_1\\ =\frac{J_y-J_z}{J_x}{x}_4{x}_6+\frac{1}{J_x}{U}_1-{\stackrel{\·\·}{x}}_{1d}+{\mathrm{\α}}_1(s_2+{\mathrm{\α}}_1{z}_1)\end{array}$

其中，k₁,k₂>0，sign代表符号函数。

由此推出滑模控制律为：

$U_1=(J_z-J_y)x_4{x}_6-J_x({{\mathrm{\α}}_1}^2{z}_1+k_{1}sign(s_2)+k_2{s}_2-{\stackrel{\·\·}{x}}_{1d})$

$U_2=(J_x-J_z)x_2{x}_6-J_y({{\mathrm{\α}}_2}^2{z}_3+k_{3}sign(s_3)+k_4{s}_3-{\stackrel{\·\·}{x}}_{3d})$

$U_3=(J_y-J_x)x_2{x}_4-J_z({{\mathrm{\α}}_3}^2{z}_5+k_{5}sign(s_4)+k_6{s}_4-{\stackrel{\·\·}{x}}_{5d})$

其中：

$\left\{\begin{array}{c}{z}_3=x_{3d}-x_3\\ s_3=x_4-{\stackrel{\·}{x}}_{3d}-{\mathrm{\α}}_2{z}_3\\ z_5=x_{5d}-x_5\\ s_4=x_6-{\stackrel{\·}{x}}_{5d}-{\mathrm{\α}}_3{z}_5\end{array}\right.$

用连续饱和函数sat(s)代替符号函数，sat(s)＝s/(|s|+e)e∈[0,1]，取e＝0.5。

构造二阶滤波器：

为输入值，X_c为输出值。

由此，得到该式在时域中的表达式：选取阻尼比ξ＝0.8，自然频率ω_n＝4.375。

在本发明的实施例中，二阶滤波器的具体结构如图3所示。

在图1所示的步骤S120中，ESO具体结构实现如下：

以四旋翼的滚转角通道为例，说明ESO的具体结构实现。结合状态空间方程，将四旋翼的滚转角φ方程写成如下形式：

$\begin{array}{c}\left\{\begin{array}{c}{\stackrel{\·}{x}}_1=x_2\\ {\stackrel{\·}{x}}_2=f_1(\mathrm{\φ},\stackrel{\·}{\mathrm{\φ}},\mathrm{\θ},\stackrel{\·}{\mathrm{\θ}},\mathrm{\ψ},\stackrel{\·}{\mathrm{\ψ}})+{\mathrm{\ω}}_2\left(t\right)+b_0{U}_1\\ y=x_1\end{array}\right.\\ \left\{\begin{array}{c}{\stackrel{\·}{x}}_1=x_2\\ {\stackrel{\·}{x}}_2=x_3+b_0{U}_1\\ {\stackrel{\·}{x}}_3={\mathrm{\ω}}_0\\ y=x_1\end{array}\right.\end{array}$

其中，b₀为控制量系数1/J_x；ω₂(t)为外界干扰量；为各种扰动作用(包括建模、未建模动态和外扰)的总和。令总扰动量看成未知的被扩张状态变量，即x₃＝a，则式子变成如下形式的线性***：

然后对上述***建立非线性扩张状态观测器:

$\left\{\begin{array}{c}e=z_1-y\\ {\stackrel{\·}{z}}_1=z_2-{\mathrm{\β}}_{01}e\\ {\stackrel{\·}{z}}_2=z_3-{\mathrm{\β}}_{02}fal(e,a_1,h)+b_0{U}_1\\ {\stackrel{\·}{z}}_3=-{\mathrm{\β}}_{03}fal(e,a_2,h)\end{array}\right.$

其中，

$fal(e,a,\mathrm{\δ})=\left\{\begin{array}{cc}|e{|}^{a}sign\left(e\right)& \left|e\right|>\mathrm{\δ}\\ e/{\mathrm{\δ}}^{-a}& \left|e\right|\≤\mathrm{\δ}\end{array}\right.---\left(25\right)$

选择适当的参数β₀₁,β₀₂,β₀₃和a_i(i＝1,2)，通常情况下取a₁＝0.5,a₂＝0.25,则扩张状态观测器输出量z_i能很好地估计原***各状态量x_i，其中z₃→x₃＝f(·)+ω(t)，虽然f(·)和ω(t)的具体函数表达式未知，但被扩张状态z₃仍可以很好地估计出***总扰动量a。

同样，其他两个角回路，即俯仰角θ和偏航角ψ采用相同的算法处理，则可以实现各个通道的动态补偿线性化。

在步骤S130中，以滚转角φ回路为例，基于滑模控制律和ESO相结合，其公式如下：

$\left\{\begin{array}{c}e=z_{1\mathrm{\φ}}-x_1\\ {\stackrel{\·}{z}}_{1\mathrm{\φ}}=z_{2\mathrm{\φ}}-{\mathrm{\β}}_{01}e\\ {\stackrel{\·}{z}}_{2\mathrm{\φ}}=z_{3\mathrm{\φ}}-{\mathrm{\β}}_{02}fal(e,0.5,h)+(1/J_x)U_1\\ {\stackrel{\·}{z}}_{3\mathrm{\φ}}=-{\mathrm{\β}}_{03}fal(e,0.25,h)\\ U_1=(J_z-J_y)x_4{x}_6-J_x({{\mathrm{\α}}_1}^2{z}_1+k_{1}sat(s_2)+k_2{s}_2-{\stackrel{\·\·}{x}}_{1d})-J_x{z}_{3\mathrm{\φ}}\end{array}\right.$

同理，可以得到其他两个姿态回路的控制输出为：

$U_2=(J_x-J_z)x_2{x}_6-J_y({{\mathrm{\α}}_2}^2{z}_3+k_{3}sat(s_3)+k_4{s}_3-{\stackrel{\·\·}{x}}_{3d})-J_y{z}_{3\mathrm{\θ}}---\left(27\right)$

$U_3=(J_y-J_x)x_2{x}_4-J_z({{\mathrm{\α}}_3}^2{z}_5+k_{5}sat(s_4)+k_6{s}_4-{\stackrel{\·\·}{x}}_{5d})-J_z{z}_{3\mathrm{\ψ}}---\left(28\right)$

其中，z_3φ,z_3θ,z_3ψ分别为滚转角回路、俯仰角回路和偏航角回路的ESO得到的被扩张状态量。在图2所示的实施中，详细地示出了本发明中ESO和滑模控制模型的相结合的原理，其中，本发明中滑模控制模型也称为滑模控制器。

在本发明中，在下面的实施例中进行仿真验证和试飞验证。

为了验证本文所设计的控制器及其抗干扰效果，设定四旋翼飞行器的姿态初始值为(φ,θ,ψ)＝(30°,-45°,-15°)。当0s<t<6s时，φ_d＝θ_d＝ψ_d＝0°；当6s<t<20s时，φ_d＝θ_d＝ψ_d＝10°；当20s<t<30s时，φ_d＝θ_d＝ψ_d＝sin(t)(单位:度)；当9.5s<t<10.5s时在三个姿态回路上同时施加0.008N·m的外部干扰，当14.5s<t<15.5s时在三个姿态回路上同时施加-0.008N·m的外部干扰。

滑模控制模型部分的参数通过调试取α₁＝α₂＝α₃＝1,k₁＝k₃＝k₅＝2.5,k₂＝k₄＝3,k₆＝2.扩张状态观测器部分的参数参考文献[19]所说，h为采样步长，这里为0.004，参数β₀₁,β₀₂,β₀₃是由采样步长h来决定的(不管怎样的被控对象，采用步长一定，则可以使用相同的β₀₁,β₀₂,β₀₃)，这里取β₀₁＝β₀₂＝β₀₃＝80，三个姿态回路采用相同的ESO参数。

为了更好地说明本控制器的优越性，在相同初始条件、跟踪指令和外部干扰的情况下，将与单独滑模控制器的结果进行对比。仿真结果如图4至图6所示。

图4至图6为四旋翼飞行器姿态控制各回路的仿真曲线对比图。对比的是单独滑模控制和带有ESO的滑模控制。从图中可以看出，整个过程中，两种控制方法都能使四旋翼姿态角快速响应并跟踪姿态指令；但是，在9.5s<t<10.5s和14.5s<t<15.5s时段，由于***扰动的作用，带有ESO的滑模控制相对于单独滑模控制，其姿态角变化量较小，可见其算法抗干扰性强；此外，图4和图6可以看出，带有ESO的滑模控制下，滚转角和偏航角响应时间更短。

图7示出了姿态各回路ESO干扰估计量。从图7可以看出，当快速响应跟踪指令和受到外部干扰时，ESO的值不为零，而其他时间段为零，表明其能够实时估计出扰动总和。

在本发明的实施例中，试飞时，整个四旋翼硬件平台在室外进行等高模式试飞，其能稳定在一个指定高度，同时姿态控制使用了本文设计的控制器，由遥控器给出姿态指令。同样为了更好说明本控制器的优越性，将在滚转角回路和俯仰角回路与单独滑模控制器的试飞结果进行对比；此外，为了更好说明本控制器的抗干扰性，试飞中给这两个姿态角回路分别挂载108g的物体，作为施加的外部干扰力矩，并进行结果对比。实际的结果图如图8至图10所示。

其中，需要说明的是，图8和图9分别为两种控制方法下的滚转角和俯仰角实际曲线对比图。从图8-1和图9-1中可以看出单独滑模控制时，滚转角实际跟踪误差最大达到5.5度，零度附近波动在±5.7^°之间；俯仰角实际跟踪误差最大达到4.5度，零度附近波动在±5.1°之间；基于滑模和ESO控制时，滚转角实际跟踪误差最大达到4.6度，零度附近波动在±5.7^°之间；俯仰角实际跟踪误差最大达到2度，零度附近波动在±3^°之间；表明基于滑模和ESO的控制方法的跟踪性能更准确，实际波动范围更小。

在本发明的实施例中，图10-1和图10-2为外扰力矩下两种控制方法的姿态角实际曲线对比图。从图10-1中可以看出在常值滚转力矩干扰下，基于滑模和ESO控制的滚转角曲线震动明显较小，且震动范围能维持在1.2°以下；在常值俯仰力矩干扰下，基于滑模和ESO控制的俯仰角曲线偏离零度线范围较小，且其俯仰角有大于零的情况，表明该控制器有补偿外部干扰的能力，有较强的抗干扰能力。

上述为本发明提供的基于滑模控制律和ESO的四旋翼飞行器姿态控制方法，与基于滑模控制律和ESO的四旋翼飞行器姿态控制方法相对应，本发明还提供一种基于滑模控制律和ESO的四旋翼飞行器姿态控制***。

图11示出了根据本发明实施例的基于滑模控制律和ESO的四旋翼飞行器姿态控制***逻辑结构，如图11所示，本发明提供的基于滑模控制律和ESO的四旋翼飞行器姿态控制***逻辑结构1100包括：姿态角回路控制单元1110、***总扰实时估计单元1120和四旋翼飞行器姿态控制单元1130。

其中，姿态角回路控制单元1110，用于根据模型参数推导出四旋翼飞行器动力学模型，根据所述四旋翼飞行器动力学模型构造滑模面，根据所述滑模面获取滑模控制律，以实现对四旋翼飞行器三个姿态角回路的控制；

***总扰实时估计单元1120，用于根据四旋翼的滚转角回路、俯仰角回路和偏航角回路的控制输入输出构建ESO，利用所述ESO对***总扰动进行实时估计；

四旋翼飞行器姿态控制单元1130，用于采用所述滑模控制律与所述ESO相结合，以实现对所述四旋翼飞行器姿态的控制。

进一步地，四旋翼飞行器动力学模型参数包括转动惯量J_x、J_y、J_z；升力系数c_T；扭矩系数c_Q；电机的时间常数T。

进一步地，姿态角回路控制单元1110获取滑模控制模型包括如下步骤：

(1)将四旋翼视为刚体，则四旋翼飞行器姿态非线性动态方程为：

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{\mathrm{\&phi;}}=p+qsin\mathrm{\&phi;}tan\mathrm{\&theta;}+rcos\mathrm{\&phi;}tan\mathrm{\&theta;}\\ \stackrel{\&CenterDot;}{\mathrm{\&theta;}}=q\cos \mathrm{\&phi;}-rsin\mathrm{\&phi;}\\ \stackrel{\&CenterDot;}{\mathrm{\&psi;}}=\sec \mathrm{\&theta;}(q\sin \mathrm{\&phi;}+rcos\mathrm{\&phi;})\end{array}\right.$

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{p}=\&lsqb;qr(J_y-J_z)+{\mathrm{\&tau;}}_{\mathrm{\&phi;}}\&rsqb;\&CenterDot;1/J_x\\ \stackrel{\&CenterDot;}{q}=\&lsqb;pr(J_z-J_x)+{\mathrm{\&tau;}}_{\mathrm{\&theta;}}\&rsqb;\&CenterDot;1/J_y\\ \stackrel{\&CenterDot;}{r}=\&lsqb;pq(J_x-J_y)+{\mathrm{\&tau;}}_{\mathrm{\&psi;}}\&rsqb;\&CenterDot;1/J_z\end{array}\right.$

其中:φ,θ,ψ分别为滚转角、俯仰角、偏航角；

p,q,r分别为本体角速度ω在本体坐标系x,y,z轴上的分量；

τ_φ,τ_θ,τ_ψ分别为三个本体轴方向的控制力矩；

J_x,J_y,J_z分别为四旋翼沿着x,y,z轴方向的转动惯量；

(2)将所述非线性动态方程改写为状态空间的形式：

$\stackrel{\&CenterDot;}{X}=f(X,U)$

其中，U为输入矢量，X为状态矢量，具体表达式如下：

状态变量：

$\begin{array}{ccc}{x}_1=\mathrm{\&phi;}& x_3=\mathrm{\&theta;}& x_5=\mathrm{\&psi;}\\ x_2={\stackrel{\&CenterDot;}{x}}_1=\stackrel{\&CenterDot;}{\mathrm{\&phi;}}& x_4={\stackrel{\&CenterDot;}{x}}_3=\stackrel{\&CenterDot;}{\mathrm{\&theta;}}& x_6={\stackrel{\&CenterDot;}{x}}_5=\stackrel{\&CenterDot;}{\mathrm{\&psi;}}\end{array}$

输入矢量：U＝[U₁ U₂ U₃]^T＝[τ_φ τ_θ τ_ψ]^T

(3)姿态角变化率与本体角速率之间的转换矩阵，在悬停或者小角度飞行情况下，作为单位矩阵，获得所述滑模控制模型：

$f(X,U)=\left(\begin{array}{c}\stackrel{\&CenterDot;}{\mathrm{\&phi;}}\\ \frac{(J_y-J_z)}{J_x}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\stackrel{\&CenterDot;}{\mathrm{\&psi;}}+\frac{1}{J_x}{U}_1\\ \stackrel{\&CenterDot;}{\mathrm{\&theta;}}\\ \frac{(J_z-J_x)}{J_y}\stackrel{\&CenterDot;}{\mathrm{\&phi;}}\stackrel{\&CenterDot;}{\mathrm{\&psi;}}+\frac{1}{J_y}{U}_2\\ \stackrel{\&CenterDot;}{\mathrm{\&psi;}}\\ \frac{(J_x-J_y)}{J_z}\stackrel{\&CenterDot;}{\mathrm{\&phi;}}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}+\frac{1}{J_z}{U}_3\end{array}\right)$

其中，φ,θ,ψ分别为滚转角、俯仰角、偏航角；

p,q,^r分别为本体角速度ω在本体坐标系x,y,z轴上的分量；

τ_φ,τ_θ,τ_ψ分别为三个本体轴方向的控制力矩；

J_x,J_y,J_z分别为四旋翼沿着x,y,z轴方向的转动惯量。

进一步地，姿态角回路控制单元1110在根据模型参数推导出四旋翼飞行器动力学模型，根据所述四旋翼飞行器动力学模型构造滑模面，根据所述滑模面获取滑模控制律的过程中，

构造的滑模面为获得的滑模控制律为：

$U_1=(J_z-J_y)x_4{x}_6-J_x({{\mathrm{\&alpha;}}_1}^2{z}_1+k_{1}sign(s_2)+k_2{s}_2-{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{1d})$

$U_2=(J_x-J_z)x_2{x}_6-J_y({{\mathrm{\&alpha;}}_2}^2{z}_3+k_{3}sign(s_3)+k_4{s}_3-{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{3d})$

$U_3=(J_y-J_x)x_2{x}_4-J_z({{\mathrm{\&alpha;}}_3}^2{z}_5+k_{5}sign(s_4)+k_6{s}_4-{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{5d})$

其中，

$\left\{\begin{array}{c}{z}_3=x_{3d}-x_3\\ s_3=x_4-{\stackrel{\&CenterDot;}{x}}_{3d}-{\mathrm{\&alpha;}}_2{z}_3\\ z_5=x_{5d}-x_5\\ s_4=x_6-{\stackrel{\&CenterDot;}{x}}_{5d}-{\mathrm{\&alpha;}}_3{z}_5\end{array}\right.$

其中，sign代表符号函数，用近似的连续饱和函数sat(s)来替代所述符号函数；函数表达式为：

sat(s)＝s/(|s|+e)e∈[0,1]，取e＝0.5；

构造二阶滤波器：

其中，为输入值，X_c为输出值。

由此，得到在时域中的表达式：选取阻尼比ξ＝0.8，自然频率ω_n＝4.375。

进一步地，***总扰实时估计单元1120在四旋翼的滚转角回路、俯仰角回路和偏航角回路的控制输入输出构建ESO的过程中，

$\left\{\begin{array}{c}e=z_1-y\\ {\stackrel{\&CenterDot;}{z}}_1=z_2-{\mathrm{\&beta;}}_{01}e\\ {\stackrel{\&CenterDot;}{z}}_2=z_3-{\mathrm{\&beta;}}_{02}fal(e,a_1,h)+b_0{U}_1\\ {\stackrel{\&CenterDot;}{z}}_3=-{\mathrm{\&beta;}}_{03}fal(e,a_2,h)\end{array}\right.$

其中，，b₀为控制量系数1/J_x，选择预设的参数β₀₁,β₀₂,β₀₃和a_i(i＝1,2)，令a₁＝0.5,a₂＝0.25。

进一步地，四旋翼飞行器姿态控制单元1130在采用所述滑模控制律与所述ESO相结合的过程中，

$\left\{\begin{array}{c}e=z_{1\mathrm{\&phi;}}-x_1\\ {\stackrel{\&CenterDot;}{z}}_{1\mathrm{\&phi;}}=z_{2\mathrm{\&phi;}}-{\mathrm{\&beta;}}_{01}e\\ {\stackrel{\&CenterDot;}{z}}_{2\mathrm{\&phi;}}=z_{3\mathrm{\&phi;}}-{\mathrm{\&beta;}}_{02}fal(e,0.5,h)+(1/J_x)U_1\\ {\stackrel{\&CenterDot;}{z}}_{3\mathrm{\&phi;}}=-{\mathrm{\&beta;}}_{03}fal(e,0.25,h)\\ U_1=(J_z-J_y)x_4{x}_6-J_x({{\mathrm{\&alpha;}}_1}^2{z}_1+k_{1}sat(s_2)+k_2{s}_2-{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{1d})-J_x{z}_{3\mathrm{\&phi;}}\end{array}\right.$

同理，得到其他两个姿态回路的控制输出为：

$U_2=(J_x-J_z)x_2{x}_6-J_y({{\mathrm{\&alpha;}}_2}^2{z}_3+k_{3}sat(s_3)+k_4{s}_3-{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{3d})-J_y{z}_{3\mathrm{\&theta;}}$

$U_3=(J_y-J_x)x_2{x}_4-J_z({{\mathrm{\&alpha;}}_3}^2{z}_5+k_{5}sat(s_4)+k_6{s}_4-{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{5d})-J_z{z}_{3\mathrm{\&psi;}}$

其中，z_3φ,z_3θ,z_3ψ分别为滚转角回路、俯仰角回路和偏航角回路的ESO得到的被扩张状态量。

本发明利用ESO实现对***总扰的实时估计；并将ESO与滑模控制律结合，实现四旋翼飞行器的姿态稳定，同时对姿态角指令具有良好的跟踪性能，相对于普通的滑模控制具有更强的抗干扰能力。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于滑模控制律和ESO的四旋翼飞行器姿态控制方法，其特征在于，包括：

根据四旋翼飞行器动力学模型参数推导出四旋翼飞行器动力学模型，根据所述四旋翼飞行器动力学模型构造滑模面，根据所述滑模面获取滑模控制律，以实现对四旋翼飞行器三个姿态角回路的控制；

根据四旋翼的滚转角回路、俯仰角回路和偏航角回路的控制输入输出构建ESO，利用所述ESO对***总扰动进行实时估计；

采用所述滑模控制律与所述ESO相结合，以实现对所述四旋翼飞行器姿态的控制。

2.根据权利要求1所述的基于滑模控制律和ESO的四旋翼飞行器姿态控制方法，其特征在于，所述四旋翼飞行器动力学模型参数包括转动惯量J_x、J_y、J_z；升力系数c_T；扭矩系数c_Q；电机的时间常数T。

3.根据权利要求1所述的基于滑模控制律和ESO的四旋翼飞行器姿态控制方法，其特征在于，获取所述滑模控制模型包括如下步骤：

(1)将四旋翼视为刚体，则所述四旋翼飞行器姿态非线性动态方程为：

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{\mathrm{\&phi;}}=p+qsin\mathrm{\&phi;}tan\mathrm{\&theta;}+rcos\mathrm{\&phi;}tan\mathrm{\&theta;}\\ \stackrel{\&CenterDot;}{\mathrm{\&theta;}}=qcos\mathrm{\&phi;}-rsin\mathrm{\&phi;}\\ \stackrel{\&CenterDot;}{\mathrm{\&psi;}}=\sec \mathrm{\&theta;}(q\sin \mathrm{\&phi;}+rcos\mathrm{\&phi;})\end{array}\right.$

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{p}=\&lsqb;qr(J_y-J_z)+{\mathrm{\&tau;}}_{\mathrm{\&phi;}}\&rsqb;\&CenterDot;1/J_x\\ \stackrel{\&CenterDot;}{q}=\&lsqb;pr(J_z-J_x)+{\mathrm{\&tau;}}_{\mathrm{\&theta;}}\&rsqb;\&CenterDot;1/J_y\\ \stackrel{\&CenterDot;}{r}=\&lsqb;pq(J_x-J_y)+{\mathrm{\&tau;}}_{\mathrm{\&psi;}}\&rsqb;\&CenterDot;1/J_z\end{array}\right.$

其中:φ,θ,ψ分别为滚转角、俯仰角、偏航角；

p,q,r分别为本体角速度ω在本体坐标系x,y,z轴上的分量；

τ_φ,τ_θ,τ_ψ分别为三个本体轴方向的控制力矩；

J_x,J_y,J_z分别为四旋翼沿着x,y,z轴方向的转动惯量；

(2)将所述非线性动态方程改写为状态空间的形式：

$\stackrel{\&CenterDot;}{X}=f(X,U)$

其中，U为输入矢量，X为状态矢量，具体表达式如下：

状态变量：

x₁＝φ x₃＝θ x₅＝ψ

$\begin{array}{ccc}{x}_2={\stackrel{\&CenterDot;}{x}}_1=\stackrel{\&CenterDot;}{\mathrm{\&phi;}}& x_4={\stackrel{\&CenterDot;}{x}}_3=\stackrel{\&CenterDot;}{\mathrm{\&theta;}}& x_6={\stackrel{\&CenterDot;}{x}}_5=\stackrel{\&CenterDot;}{\mathrm{\&psi;}}\end{array}$

输入矢量：U＝[U₁ U₂ U₃]^T＝[τ_φ τ_θ τ_ψ]^T

(3)姿态角变化率与本体角速率之间的转换矩阵，在悬停或者小角度飞行情况下，作为单位矩阵，获得所述滑模控制模型：

$f(X,U)=\left(\begin{array}{c}\stackrel{\&CenterDot;}{\mathrm{\&phi;}}\\ \frac{(J_y-J_z)}{J_x}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\stackrel{\&CenterDot;}{\mathrm{\&psi;}}+\frac{1}{J_x}{U}_1\\ \stackrel{\&CenterDot;}{\mathrm{\&theta;}}\\ \frac{(J_z-J_x)}{J_y}\stackrel{\&CenterDot;}{\mathrm{\&phi;}}\stackrel{\&CenterDot;}{\mathrm{\&psi;}}+\frac{1}{J_y}{U}_2\\ \stackrel{\&CenterDot;}{\mathrm{\&psi;}}\\ \frac{(J_x-J_y)}{J_z}\stackrel{\&CenterDot;}{\mathrm{\&phi;}}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}+\frac{1}{J_z}{U}_3\end{array}\right)$

其中，φ,θ,ψ分别为滚转角、俯仰角、偏航角；

p,q,r分别为本体角速度ω在本体坐标系x,y,z轴上的分量；

τ_φ,τ_θ,τ_ψ分别为三个本体轴方向的控制力矩；

J_x,J_y,J_z分别为四旋翼沿着x,y,z轴方向的转动惯量。

4.根据权利要求1所述的基于滑模控制律和ESO的四旋翼飞行器姿态控制方法，其特征在于，在根据模型参数推导出四旋翼飞行器动力学模型，根据所述四旋翼飞行器动力学模型构造滑模面的过程中，

构造的滑模面为获得的滑模控制律为：

$U_1=(J_z-J_y)x_4{x}_6-J_x({{\mathrm{\&alpha;}}_1}^2{z}_1+k_{1}sign(s_2)+k_2{s}_2-{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{1d})$

$U_2=(J_x-J_z)x_2{x}_6-J_y({{\mathrm{\&alpha;}}_2}^2{z}_3+k_{3}sign(s_3)+k_4{s}_3-{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{3d})$

$U_3=(J_y-J_x)x_2{x}_4-J_z({{\mathrm{\&alpha;}}_3}^2{z}_5+k_{5}sign(s_4)+k_6{s}_4-{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{5d})$

其中，

$\left\{\begin{array}{c}{z}_3=x_{3d}-x_3\\ s_3=x_4-{\stackrel{\&CenterDot;}{x}}_{3d}-{\mathrm{\&alpha;}}_2{z}_3\\ z_5=x_{5d}-x_5\\ s_4=x_6-{\stackrel{\&CenterDot;}{x}}_{5d}-{\mathrm{\&alpha;}}_3{z}_5\end{array}\right.$

其中，sign代表符号函数，用近似的连续饱和函数sat(s)来替代所述符号函数；函数表达式为：

sat(s)＝s/(|s|+e)e∈[0,1]，取e＝0.5；

构造二阶滤波器：

其中，为输入值，X_c为输出值。

由此，得到在时域中的表达式：选取阻尼比ξ＝0.8，自然频率ω_n＝4.375。

5.根据权利要求1所述的基于滑模控制律和ESO的四旋翼飞行器姿态控制方法，其特征在于，在根据四旋翼的滚转角回路、俯仰角回路和偏航角回路的控制输入输出构建ESO的过程中，

$\left\{\begin{array}{c}e=z_1-y\\ {\stackrel{\&CenterDot;}{z}}_1=z_2-{\mathrm{\&beta;}}_{01}e\\ {\stackrel{\&CenterDot;}{z}}_2=z_3-{\mathrm{\&beta;}}_{02}fal(e,a_1,h)+b_0{U}_1\\ {\stackrel{\&CenterDot;}{z}}_3=-{\mathrm{\&beta;}}_{03}fal(e,a_2,h)\end{array}\right.$

其中，b₀为控制量系数1/J_x，选择预设的参数β₀₁,β₀₂,β₀₃和a_i(i＝1,2)，令a₁＝0.5,a₂＝0.25。

6.根据权利要求1所述的基于滑模控制律和ESO的四旋翼飞行器姿态控制方法，其特征在于，在采用所述滑模控制律与所述ESO相结合的过程中，

$\left\{\begin{array}{c}e=z_{1\mathrm{\&phi;}}-x_1\\ {\stackrel{\&CenterDot;}{z}}_{1\mathrm{\&phi;}}=z_{2\mathrm{\&phi;}}-{\mathrm{\&beta;}}_{01}e\\ {\stackrel{\&CenterDot;}{z}}_{2\mathrm{\&phi;}}=z_{3\mathrm{\&phi;}}-{\mathrm{\&beta;}}_{02}fal(e,0.5,h)+(1/J_x)U_1\\ {\stackrel{\&CenterDot;}{z}}_{3\mathrm{\&phi;}}=-{\mathrm{\&beta;}}_{03}fal(e,0.25,h)\\ U_1=(J_z-J_y)x_4{x}_6-J_x({{\mathrm{\&alpha;}}_1}^2{z}_1+k_{1}sat(s_2)+k_2{s}_2-{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{1d})-J_x{z}_{3\mathrm{\&phi;}}\end{array}\right.$

同理，得到其他两个姿态回路的控制输出为：

$U_2=(J_x-J_z)x_2{x}_6-J_y({{\mathrm{\&alpha;}}_2}^2{z}_3+k_{3}sat(s_3)+k_4{s}_3-{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{3d})-J_y{z}_{3\mathrm{\&theta;}}$

$U_3=(J_y-J_x)x_2{x}_4-J_z({{\mathrm{\&alpha;}}_3}^2{z}_5+k_{5}sat(s_4)+k_6{s}_4-{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{5d})-J_z{z}_{3\mathrm{\&psi;}}$

其中，z_3φ,z_3θ,z_3ψ分别为滚转角回路、俯仰角回路和偏航角回路的ESO得到的被扩张状态量。

7.一种基于滑模控制律和ESO的四旋翼飞行器姿态控制***，其特征在于，包括：姿态角回路控制单元，用于根据模型参数推导出四旋翼飞行器动力学模型，根据所述四旋翼飞行器动力学模型构造滑模面，根据所述滑模面获取滑模控制律，以实现对四旋翼飞行器三个姿态角回路的控制；

***总扰实时估计单元，用于根据四旋翼的滚转角回路、俯仰角回路和偏航角回路的控制输入输出构建ESO，利用所述ESO对***总扰动进行实时估计；

四旋翼飞行器姿态控制单元，用于采用所述滑模控制律与所述ESO相结合，以实现对所述四旋翼飞行器姿态的控制。

8.根据权利要求7所述的基于滑模控制律和ESO的四旋翼飞行器姿态控制***，其特征在于，所述四旋翼飞行器动力学模型参数包括转动惯量J_x、J_y、J_z；升力系数c_T；扭矩系数c_Q；电机的时间常数T。

9.根据权利要求7所述的基于滑模控制律和ESO的四旋翼飞行器姿态控制***，其特征在于，所述姿态角回路控制单元获取所述滑模控制模型包括如下步骤：

(1)将四旋翼视为刚体，则所述四旋翼飞行器姿态非线性动态方程为：

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{\mathrm{\&phi;}}=p+qsin\mathrm{\&phi;}tan\mathrm{\&theta;}+rcos\mathrm{\&phi;}tan\mathrm{\&theta;}\\ \stackrel{\&CenterDot;}{\mathrm{\&theta;}}=qcos\mathrm{\&phi;}-rsin\mathrm{\&phi;}\\ \stackrel{\&CenterDot;}{\mathrm{\&psi;}}=\sec \mathrm{\&theta;}(q\sin \mathrm{\&phi;}+rcos\mathrm{\&phi;})\end{array}\right.$

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{p}=\&lsqb;qr(J_y-J_z)+{\mathrm{\&tau;}}_{\mathrm{\&phi;}}\&rsqb;\&CenterDot;1/J_x\\ \stackrel{\&CenterDot;}{q}=\&lsqb;pr(J_z-J_x)+{\mathrm{\&tau;}}_{\mathrm{\&theta;}}\&rsqb;\&CenterDot;1/J_y\\ \stackrel{\&CenterDot;}{r}=\&lsqb;pq(J_x-J_y)+{\mathrm{\&tau;}}_{\mathrm{\&psi;}}\&rsqb;\&CenterDot;1/J_z\end{array}\right.$

其中：φ,θ,ψ分别为滚转角、俯仰角、偏航角；

p,q,r分别为本体角速度ω在本体坐标系x,y,z轴上的分量；

τ_φ,τ_θ,τ_ψ分别为三个本体轴方向的控制力矩；

J_x,J_y,J_z分别为四旋翼沿着x,y,z轴方向的转动惯量；

(2)将所述非线性动态方程改写为状态空间的形式：

$\stackrel{\&CenterDot;}{X}=f(X,U)$

其中，U为输入矢量，X为状态矢量，具体表达式如下：

状态变量：

x₁＝φ x₃＝θ x₅＝ψ

$\begin{array}{ccc}{x}_2={\stackrel{\&CenterDot;}{x}}_1=\stackrel{\&CenterDot;}{\mathrm{\&phi;}}& x_4={\stackrel{\&CenterDot;}{x}}_3=\stackrel{\&CenterDot;}{\mathrm{\&theta;}}& x_6={\stackrel{\&CenterDot;}{x}}_5=\stackrel{\&CenterDot;}{\mathrm{\&psi;}}\end{array}$

输入矢量：U＝[U₁ U₂ U₃]^T＝[τ_φ τ_θ τ_ψ]^T

(3)姿态角变化率与本体角速率之间的转换矩阵，在悬停或者小角度飞行情况下，作为单位矩阵，获得所述滑模控制模型：

$f(X,U)=\left(\begin{array}{c}\stackrel{\&CenterDot;}{\mathrm{\&phi;}}\\ \frac{(J_y-J_z)}{J_x}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\stackrel{\&CenterDot;}{\mathrm{\&psi;}}+\frac{1}{J_x}{U}_1\\ \stackrel{\&CenterDot;}{\mathrm{\&theta;}}\\ \frac{(J_z-J_x)}{J_y}\stackrel{\&CenterDot;}{\mathrm{\&phi;}}\stackrel{\&CenterDot;}{\mathrm{\&psi;}}+\frac{1}{J_y}{U}_2\\ \stackrel{\&CenterDot;}{\mathrm{\&psi;}}\\ \frac{(J_x-J_y)}{J_z}\stackrel{\&CenterDot;}{\mathrm{\&phi;}}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}+\frac{1}{J_z}{U}_3\end{array}\right)$

其中，φ,θ,ψ分别为滚转角、俯仰角、偏航角；

p,q,r分别为本体角速度ω在本体坐标系x,y,z轴上的分量；

τ_φ,τ_θ,τ_ψ分别为三个本体轴方向的控制力矩；

J_x,J_y,J_z分别为四旋翼沿着x,y,z轴方向的转动惯量。

10.根据权利要求7所述的基于滑模控制律和ESO的四旋翼飞行器姿态控制***，其特征在于，所述姿态角回路控制单元在根据模型参数推导出四旋翼飞行器动力学模型，根据所述四旋翼飞行器动力学模型构造滑模面，根据所述滑模面获取滑模控制律的过程中，

构造的滑模面为获得的滑模控制律为：

$U_1=(J_z-J_y)x_4{x}_6-J_x({{\mathrm{\&alpha;}}_1}^2{z}_1+k_{1}sign(s_2)+k_2{s}_2-{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{1d})$

$U_2=(J_x-J_z)x_2{x}_6-J_y({{\mathrm{\&alpha;}}_2}^2{z}_3+k_{3}sign(s_3)+k_4{s}_3-{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{3d})$

$U_3=(J_y-J_x)x_2{x}_4-J_z({{\mathrm{\&alpha;}}_3}^2{z}_5+k_{5}sign(s_4)+k_6{s}_4-{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{5d})$

其中，

$\left\{\begin{array}{c}{z}_3=x_{3d}-x_3\\ s_3=x_4-{\stackrel{\&CenterDot;}{x}}_{3d}-{\mathrm{\&alpha;}}_2{z}_3\\ z_5=x_{5d}-x_5\\ s_4=x_6-{\stackrel{\&CenterDot;}{x}}_{5d}-{\mathrm{\&alpha;}}_3{z}_5\end{array}\right.$

其中，sign代表符号函数，用近似的连续饱和函数sat(s)来替代所述符号函数；函数表达式为：

sat(s)＝s/(|s|+e)e∈[0,1]，取e＝0.5；

构造二阶滤波器：

其中，为输入值，X_c为输出值。

由此，得到在时域中的表达式：选取阻尼比ξ＝0.8，自然频率ω_n＝4.375。

所述***总扰实时估计单元在根据四旋翼的滚转角回路、俯仰角回路和偏航角回路的控制输入输出构建ESO的过程中，

$\left\{\begin{array}{c}e=z_1-y\\ {\stackrel{\&CenterDot;}{z}}_1=z_2-{\mathrm{\&beta;}}_{01}e\\ {\stackrel{\&CenterDot;}{z}}_2=z_3-{\mathrm{\&beta;}}_{02}fal(e,a_1,h)+b_0{U}_1\\ {\stackrel{\&CenterDot;}{z}}_3=-{\mathrm{\&beta;}}_{03}fal(e,a_2,h)\end{array}\right.$

其中，b₀为控制量系数1/J_x，选择预设的参数β₀₁,β₀₂,β₀₃和a_i(i＝1,2)，令a₁＝0.5,a₂＝0.25；

四旋翼飞行器姿态控制单元在采用所述滑模控制律与所述ESO相结合的过程中，

$\left\{\begin{array}{c}e=z_{1\mathrm{\&phi;}}-x_1\\ {\stackrel{\&CenterDot;}{z}}_{1\mathrm{\&phi;}}=z_{2\mathrm{\&phi;}}-{\mathrm{\&beta;}}_{01}e\\ {\stackrel{\&CenterDot;}{z}}_{2\mathrm{\&phi;}}=z_{3\mathrm{\&phi;}}-{\mathrm{\&beta;}}_{02}fal(e,0.5,h)+(1/J_x)U_1\\ {\stackrel{\&CenterDot;}{z}}_{3\mathrm{\&phi;}}=-{\mathrm{\&beta;}}_{03}fal(e,0.25,h)\\ U_1=(J_z-J_y)x_4{x}_6-J_x({{\mathrm{\&alpha;}}_1}^2{z}_1+k_{1}sat(s_2)+k_2{s}_2-{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{1d})-J_x{z}_{3\mathrm{\&phi;}}\end{array}\right.$

同理，得到其他两个姿态回路的控制输出为：

$U_2=(J_x-J_z)x_2{x}_6-J_y({{\mathrm{\&alpha;}}_2}^2{z}_3+k_{3}sat(s_3)+k_4{s}_3-{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{3d})-J_y{z}_{3\mathrm{\&theta;}}$

$U_3=(J_y-J_x)x_2{x}_4-J_z({{\mathrm{\&alpha;}}_3}^2{z}_5+k_{5}sat(s_4)+k_6{s}_4-{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{5d})-J_z{z}_{3\mathrm{\&psi;}},$

其中，z_3φ,z_3θ,z_3ψ分别为滚转角回路、俯仰角回路和偏航角回路的ESO得到的被扩张状态量。