CN105911866A - 四旋翼无人飞行器的有限时间全阶滑模控制方法 - Google Patents

Abstract

一种四旋翼无人飞行器的有限时间全阶滑模控制方法，针对系统模型参数耦合非线性的四旋翼无人飞行器系统，利用全阶滑模控制方法，再结合一阶滤波器，设计四旋翼无人飞行器的有限时间全阶滑模控制方法。全阶滑模面的设计是为了保证系统的快速稳定收敛。在实际的控制系统中通过设计位置和姿态角两个全阶滑模面，分为内外控制环实现飞行器快速跟踪，并且增加滤波器来改善抖振问题。本发明提供一种四旋翼无人飞行器的有限时间全阶滑模控制方法，基于系统模型参数耦合非线性的情况，保证系统在有限时间内快速收敛至平衡点，加快系统的响应速度，改善滑模控制输入的抖振，有效地满足系统快速稳定的跟踪性能。

Description

四旋翼无人飞行器的有限时间全阶滑模控制方法

技术领域

本发明涉及一种四旋翼无人飞行器的有限时间全阶滑模控制方法，适用于低速飞行的四旋翼无人飞行器飞行控制。

背景技术

无人飞行器(Unmanned Aerial Vehicle,UAV)是通过远程遥控或基于飞行器自身传感器实现自主飞行的飞行器。其中旋翼无人飞行器具有很强的可操作性，且机动性强，能够很方便地完成起飞与降落等动作，对工作环境的要求也比较低。四旋翼无人飞行器作为旋翼式飞行器的一种,以其体积小、机动性能好、设计简单、制造成本低廉等优点,吸引了国内外大学、研究机构、公司的广泛关注。然而四旋翼无人飞行器在飞行过程中,容易受到外部环境的干扰,产生外部噪声、参数摄动等问题，并直接影响旋翼飞行器的灵敏度和稳定性。针对这些控制问题，存在很多控制方法，例如PID控制，反演控制，滑模控制等。

滑模控制在解决系统不确定性和外部扰动方面被认为是一个有效的鲁棒控制方法。滑模控制方法具有算法简单、响应速度快、对外界噪声干扰和参数摄动鲁棒性强等优点。因此，滑模控制方法被广泛应用于各个领域。传统线性滑模只是渐近稳定,而终端滑模控制的优越性在于它的有限时间收敛。然而，两种滑模控制都存在奇异性和抖振问题。为了解决这些问题，许多改进的方法相继被提出，例如切换滑模控制方法、高阶滑模控制方法、观测器控制方法等。然而上述方法中，在解决抖振的同时增大了跟踪误差，减慢了响应速度，奇异性也始终存在于降阶滑模面中。

发明内容

为了克服现有的四旋翼无人飞行器滑模控制方法中存在的响应速度慢和控制输入抖振问题，本发明提供一种四旋翼无人飞行器的有限时间全阶滑模控制方法，基于系统模型参数耦合非线性的情况，保证系统在有限时间内快速收敛至平衡点，加快系统的响应速度，改善滑模控制输入的抖振，有效地满足系统快速稳定的跟踪性能。

为了解决上述技术问题提出的技术方案如下：

一种四旋翼无人飞行器的有限时间全阶滑模控制方法，包括以下步骤：

步骤1，建立四旋翼无人飞行器的动态模型，初始化系统状态、采样时间以及控制参数，过程如下：

1.1在忽略空气阻力和陀螺效应的基础上，对四旋翼无人飞行器进行以下假设：飞行器是刚性的；飞行器的结构是完全对称的；飞行器的重心与机体坐标系原点重合；并定义机体坐标系到惯性坐标系的转移矩阵为：

$R=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\cos \mathrm{\φ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\θ}& \sin \mathrm{\φ}\cos \mathrm{\θ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]---\left(1\right)$

其中，ψ、θ、φ分别为飞行器的偏航角、俯仰角、翻滚角，表示飞行器的惯性坐标系依次绕坐标轴z_E、y_E、x_E旋转的角度；

1.2采用牛顿-欧拉法，从受力角度分析飞行器，其中牛顿公式为：

$F=m\stackrel{\·\·}{\mathrm{\ξ}}---\left(2\right)$

其中，ξ表示飞行器在惯性坐标系下的位置，代表二阶微分，m表示飞行器的质量，F表示作用在飞行器上的合外力，包括飞行器所受重力mg和四个旋翼产生的合力U_F；

式(2)展开为：

$\left[\begin{array}{c}0\\ 0\\ -mg\end{array}\right]+R\left[\begin{array}{c}0\\ 0\\ U_F\end{array}\right]=m\left[\begin{array}{c}\stackrel{\·\·}{x}\\ \stackrel{\·\·}{y}\\ \stackrel{\·\·}{z}\end{array}\right]---\left(3\right)$

其中，x、y、z分别表示飞行器在惯性坐标系下x_E、y_E、z_E轴的位置；

将式(1)代入式(3)中得到如下等式：

$\left\{\begin{array}{c}\stackrel{\·\·}{x}=\frac{U_F}{m}(cos\mathrm{\φ}sin\mathrm{\θ}cos\mathrm{\ψ}+sin\mathrm{\φ}sin\mathrm{\ψ})\\ \stackrel{\·\·}{y}=\frac{U_F}{m}(cos\mathrm{\φ}sin\mathrm{\θ}sin\mathrm{\ψ}-sin\mathrm{\φ}cos\mathrm{\ψ})\\ \stackrel{\·\·}{z}=-g+\frac{U_F}{m}\cos \mathrm{\φ}sin\mathrm{\θ}\end{array}\right.---\left(4\right)$

1.3机体坐标系下欧拉公式为：

$\mathrm{\τ}=I\stackrel{\·}{\mathrm{\ω}}+\mathrm{\ω}\×I\mathrm{\ω}---\left(5\right)$

其中，τ表示作用在飞行器上的力矩，I表示飞行器在机体坐标系下的转动惯量，ω表示飞行器姿态角速度，表示飞行器姿态角加速度，×表示叉乘；

式(5)展开为：

$\left[\begin{array}{c}{\mathrm{\τ}}_x\\ {\mathrm{\τ}}_y\\ {\mathrm{\τ}}_z\end{array}\right]=\left[\begin{array}{ccc}{I}_{xx}& 0& 0\\ 0& I_{yy}& 0\\ 0& 0& I_{zz}\end{array}\right]\left[\begin{array}{c}\stackrel{\·}{p}\\ \stackrel{\·}{q}\\ \stackrel{\·}{r}\end{array}\right]+\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\×\left[\begin{array}{ccc}{I}_{xx}& 0& 0\\ 0& I_{yy}& 0\\ 0& 0& I_{zz}\end{array}\right]\left[\begin{array}{c}p\\ q\\ r\end{array}\right]---\left(6\right)$

其中，τ_x、τ_y、τ_z分别代表机体坐标轴上的各轴力矩分量，I_xx、I_yy、I_zz分别代表机体坐标轴上的各轴转动惯量分量，p、q、r分别代表机体坐标轴上的各轴姿态角速度分量，分别代表机体坐标轴上的各轴姿态角加速度分量；

式(6)表示为：

$\left\{\begin{array}{c}\stackrel{\·}{p}=\frac{I_{yy}-I_{zz}}{I_{xx}}qr+\frac{1}{I_{xx}}{\mathrm{\τ}}_x\\ \stackrel{\·}{q}=\frac{I_{zz}-I_{xx}}{I_{yy}}pr+\frac{1}{I_{yy}}{\mathrm{\τ}}_y\\ \stackrel{\·}{r}=\frac{I_{xx}-I_{yy}}{I_{zz}}pq+\frac{1}{I_{zz}}{\mathrm{\τ}}_z\end{array}\right.---\left(7\right)$

考虑到飞行器一般处于低速飞行或者悬停状态下，姿态角变化较小，此时认为将式(7)改写为：

$\left\{\begin{array}{c}\stackrel{\·\·}{\mathrm{\φ}}=\frac{I_{yy}-I_{zz}}{I_{xx}}\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}+\frac{1}{I_{xx}}{\mathrm{\τ}}_x\\ \stackrel{\·\·}{\mathrm{\θ}}=\frac{I_{zz}-I_{xx}}{I_{yy}}\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\ψ}}+\frac{1}{I_{yy}}{\mathrm{\τ}}_y\\ \stackrel{\·\·}{\mathrm{\ψ}}=\frac{I_{xx}-I_{yy}}{I_{zz}}\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\θ}}+\frac{1}{I_{zz}}{\mathrm{\τ}}_z\end{array}\right.---\left(8\right)$

1.4由于存在测量噪声，电源变化以及外界干扰的影响，式(4)和式(8)中的系统参数并不能准确的获得，因此系统的动态模型改写为：

$\left\{\begin{array}{c}\stackrel{\·\·}{x}=U_x+d_x\\ \stackrel{\·\·}{y}=U_y+d_y\\ \stackrel{\·\·}{z}=U_z+d_z\\ \stackrel{\·\·}{\mathrm{\φ}}=a_1\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}+b_1{\mathrm{\τ}}_x+d_{\mathrm{\φ}}\\ \stackrel{\·\·}{\mathrm{\θ}}=a_2\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\ψ}}+b_2{\mathrm{\τ}}_y+d_{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\ψ}}=a_3\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\θ}}+b_3{\mathrm{\τ}}_z+d_{\mathrm{\ψ}}\end{array}\right.---\left(9\right)$

其中 d_x、d_y、d_z、d_φ、d_θ、d_ψ分别代表模型不确定和外界干扰项；

步骤2，计算系统位置跟踪误差，设计位置全阶滑模面，过程如下：

2.1定义系统位置跟踪误差为

e_q＝q_d-q (10)

其中，q＝[x；y；z]，q_d＝[x_d；y_d；z_d]表示为三阶可导期望轨迹；那么式(10)的一阶微分和二阶微分被表示为：

${\stackrel{\·}{e}}_q={\stackrel{\·}{q}}_d-\stackrel{\·}{q}---\left(11\right)$

${\stackrel{\·\·}{e}}_q={\stackrel{\·\·}{q}}_d-\stackrel{\·\·}{q}---\left(12\right)$

2.2因此，为了避免奇异问题，位置全阶滑模面将定义为：

$s_q={\stackrel{\·\·}{e}}_q+c_{q2}\mathrm{sgn}\left({\stackrel{\·}{e}}_q\right)|{\stackrel{\·}{e}}_q{|}^{{\mathrm{\α}}_{q2}}+c_{q1}\mathrm{sgn}\left(e_q\right)|e_q{|}^{{\mathrm{\α}}_{q1}}---\left(13\right)$

其中，sgn(·)表示符号函数；c_q1和c_q2是一个正的常数，它的选择是保证多项式p²+c_q2p+c_q1的全部特征根在复平面的左半部分以保证系统的稳定性；α_q1和α_q2的选取则是通过以下多项式：

$\left\{\begin{array}{c}{\mathrm{\α}}_1=\mathrm{\α},n=1\\ {\mathrm{\α}}_{i-1}=\frac{{\mathrm{\α}}_i{\mathrm{\α}}_{i+1}}{2{\mathrm{\α}}_{i+1}-{\mathrm{\α}}_i},i=2,...,n,\∀n\≥2\end{array}\right.---\left(14\right)$

其中，α_n+1＝1，α_n＝α，α∈(1-ε,1)以及ε∈(0,1)；

步骤3，基于四旋翼无人飞行器动态模型，根据位置全阶滑模面以及一阶滤波器，设计位置全阶滑模控制器，过程如下：

3.1考虑式(9)，位置全阶滑模控制器被设计为：

u＝(u_eq+u_n) (15)

$u_{eq}=-c_{q2}\mathrm{sgn}\left({\stackrel{\·}{e}}_q\right)|{\stackrel{\·}{e}}_q{|}^{{\mathrm{\α}}_{q2}}-c_{q1}\mathrm{sgn}\left(e_q\right)|e_q{|}^{{\mathrm{\α}}_{q1}}---\left(16\right)$

${\stackrel{\·}{u}}_n+T_q{u}_n=v_q---\left(17\right)$

v_q＝-(k_qd+k_qT+η_q)sgn(s_q) (18)其中，c_i和α_i是常数，i＝q1,q2，已在式(13)中被定义；T_q是正的常数；k_qd，k_qT和η_q都是常数；

3.2将式(9)带入式(13)中得到如下等式：

$s_q=u+d_q+c_{q2}\mathrm{sgn}\left({\stackrel{\·}{e}}_q\right)|{\stackrel{\·}{e}}_q{|}^{{\mathrm{\α}}_{q2}}+c_{q1}\mathrm{sgn}\left(e_q\right)|e_q{|}^{{\mathrm{\α}}_{q1}}---\left(19\right)$

其中，d_q＝[d_x；d_y；d_z]代表系统位置扰动项，并且各元素是有界的，则假定d_q≤l_qd并且其中l_qd是一个有界的常数；k_qT的选取是要求在T_q＞0时满足k_qT≥T_ql_qd；

将式(15)-式(16)代入式(19)，全阶滑模面被表示成如下等式：

s_q＝d_q+u_n (20)

将式(18)带入式(17)中得到：

$\begin{array}{c}{u}_n\left(t\right)=\left(u_n\left(t_0\right)+\left(1/T_q\right)\left(k_{qd}+k_{qT}+{\mathrm{\η}}_q\right)\mathrm{sgn}\left(s_q\right)\right)e^{t-t_0}\\ -\left(1/T_q\right)\left(k_{qd}+k_{qT}+{\mathrm{\η}}_q\right)\mathrm{sgn}\left(s_q\right)\end{array}---\left(21\right)$

在u_n(0)＝0的情况下，得到如下等式：

k_qT≥T_ql_qd≥T_q|u_n(t)|_max≥T_q|u_n(t)| (22)

3.3设计李亚普诺夫函数：

$V_q=\frac{1}{2}{s_q}^T{s}_q---\left(23\right)$

对式(20)进行求导得：

${\stackrel{\·}{s}}_q={\stackrel{\·}{d}}_q+{\stackrel{\·}{u}}_n={\stackrel{\·}{d}}_q+{\stackrel{\·}{u}}_n+T_q{u}_n-T_q{u}_n={\stackrel{\·}{d}}_q+v_q-T_q{u}_n---\left(24\right)$

将式(18)带入式(24)中得到：

${\stackrel{\·}{s}}_q={\stackrel{\·}{d}}_q-(k_{qd}+k_{qT}+{\mathrm{\η}}_q)sgn\left(s_q\right)-T_q{u}_n---\left(25\right)$

因此，

$\begin{array}{c}{s}_q{\stackrel{\·}{s}}_q={\stackrel{\·}{d}}_q{s}_q-\left(k_{qd}+k_{qT}+{\mathrm{\η}}_q\right)s_q\mathrm{sgn}\left(s_q\right)-T_q{u}_n{s}_q\\ =\left({\stackrel{\·}{d}}_q{s}_q-k_{qd}{s}_q\right)+\left(-k_{qT}-T_q{u}_n\right)\left|s_q\right|-{\mathrm{\η}}_q\left|s_q\right|\end{array}---\left(26\right)$

对式(23)进行微分，并代入式(22)得到：

${\stackrel{\·}{V}}_q=s_q{\stackrel{\·}{s}}_q\≤-{\mathrm{\η}}_q\left|s_q\right|\≤0---\left(27\right)$

因此系统位置跟踪误差能在有限时间内收敛至零，表明系统是稳定的；步骤4，给定偏航角，计算总升力，俯仰角和翻滚角期待值，过程如下：

4.1由式(9)得到：

$\left\{\begin{array}{c}{U}_F=m\sqrt{{U_x}^2+{U_y}^2+{(U_z+g)}^2}\\ {\mathrm{\φ}}_d=\arcsin \[\frac{m}{U_F}(U_x{\mathrm{sin\ψ}}_d-U_y{\mathrm{cos\ψ}}_d)\]\\ {\mathrm{\θ}}_d=\arctan \[\frac{1}{U_z+g}(U_x{\mathrm{cos\ψ}}_d+U_y{\mathrm{sin\ψ}}_d)\]\end{array}\right.---\left(28\right)$

步骤5，计算系统姿态角跟踪误差，设计姿态全阶滑模面，过程如下：

5.1定义系统姿态角跟踪误差为

e_Ω＝Ω_d-Ω (29)

其中，Ω＝[φ；θ；ψ]，Ω_d＝[φ_d；θ_d；ψ_d]表示为三阶可导期望姿态角；那么式(29)的一阶微分和二阶微分被表示为：

${\stackrel{\·}{e}}_{\mathrm{\Ω}}={\stackrel{\·}{\mathrm{\Ω}}}_d-\stackrel{\·}{\mathrm{\Ω}}---\left(30\right)$

$e_{\mathrm{\Ω}}={\stackrel{\·\·}{\mathrm{\Ω}}}_d-\stackrel{\·\·}{\mathrm{\Ω}}---\left(31\right)$

5.2因此，为了避免奇异问题，姿态全阶滑模面将定义为：

$s_{\mathrm{\Ω}}={\stackrel{\·\·}{e}}_{\mathrm{\Ω}}+c_{\mathrm{\Ω}2}\mathrm{sgn}\left({\stackrel{\·}{e}}_{\mathrm{\Ω}}\right)|{\stackrel{\·}{e}}_{\mathrm{\Ω}}{|}^{{\mathrm{\α}}_{\mathrm{\Ω}2}}+c_{\mathrm{\Ω}1}\mathrm{sgn}\left(e_{\mathrm{\Ω}}\right)|e_{\mathrm{\Ω}}{|}^{{\mathrm{\α}}_{\mathrm{\Ω}1}}---\left(32\right)$

其中，c_Ω1和c_Ω2是一个正的常数，它的选择是保证多项式p²+c_Ω2p+c_Ω1的全部特征根在复平面的左半部分以保证系统的稳定性；α_Ω1和α_Ω2的选取则是通过以下多项式：

$\left\{\begin{array}{c}{\mathrm{\α}}_1=\mathrm{\α},n=1\\ {\mathrm{\α}}_{i-1}=\frac{{\mathrm{\α}}_i{\mathrm{\α}}_{i+1}}{2{\mathrm{\α}}_{i+1}-{\mathrm{\α}}_i},i=2,...,n,\∀n\≥2\end{array}\right.---\left(33\right)$

其中，α_n+1＝1，α_n＝α，α∈(1-ε,1)以及ε∈(0,1)；

步骤6，基于四旋翼无人飞行器动态模型，根据姿态全阶滑模面以及一阶滤波器，设计姿态全阶滑模控制器，过程如下：

6.1考虑式(9)，姿态全阶滑模控制器被设计为：

τ＝b^-1(τ_eq+τ_n) (34)

${\mathrm{\τ}}_{eq}=-af-c_{\mathrm{\Ω}2}\mathrm{sgn}\left({\stackrel{\·}{e}}_{\mathrm{\Ω}}\right)|{\stackrel{\·}{e}}_{\mathrm{\Ω}}{|}^{{\mathrm{\α}}_{\mathrm{\Ω}2}}-c_{\mathrm{\Ω}1}\mathrm{sgn}\left(e_{\mathrm{\Ω}}\right)|e_{\mathrm{\Ω}}{|}^{{\mathrm{\α}}_{\mathrm{\Ω}1}}---\left(35\right)$

${\stackrel{\·}{\mathrm{\τ}}}_n+T_{\mathrm{\Ω}}{\mathrm{\τ}}_n=v_{\mathrm{\Ω}}---\left(36\right)$

v_Ω＝-(k_Ωd+k_ΩT+η_Ω)sgn(s_Ω) (37)

其中，a＝[a₁；a₂；a₃]，b＝[b₁；b₂；b₃]，c_i和α_i是常数，i＝Ω1,Ω2，已在式(32)中被定义；T_Ω是正的常数；k_Ωd，k_ΩT和η_Ω都是常数；

6.2将式(9)带入式(32)中得到如下等式：

$s_{\mathrm{\Ω}}=b\mathrm{\τ}+af+d_{\mathrm{\Ω}}+c_{\mathrm{\Ω}2}\mathrm{sgn}\left({\stackrel{\·}{e}}_{\mathrm{\Ω}}\right)|{\stackrel{\·}{e}}_{\mathrm{\Ω}}{|}^{{\mathrm{\α}}_{\mathrm{\Ω}2}}+c_{\mathrm{\Ω}1}\mathrm{sgn}\left(e_{\mathrm{\Ω}}\right)|e_{\mathrm{\Ω}}{|}^{{\mathrm{\α}}_{\mathrm{\Ω}1}}---\left(38\right)$

其中，d_Ω＝[d_φ；d_θ；d_ψ]代表系统位置扰动项，并且各元素是有界的，则假定d_Ω≤l_Ωd并且其中l_Ωd是一个有界的常数；k_ΩT的选取是要求在T_Ω＞0时满足k_ΩT≥T_Ωl_Ωd；

将式(34)-式(35)代入式(38)，全阶滑模面被表示成如下等式：

s_Ω＝d_Ω+τ_n (39)

将式(37)带入式(36)中得到：

$\begin{array}{c}{\mathrm{\τ}}_n\left(t\right)=\left({\mathrm{\τ}}_n\right(t_0)+(1/T_{\mathrm{\Ω}}\left)\right(k_{\mathrm{\Ω}d}+k_{\mathrm{\Ω}T}+{\mathrm{\η}}_{\mathrm{\Ω}}\left)sgn\right(s_{\mathrm{\Ω}}\left)\right)e^{t-t_0}\\ -\left(1/T_{\mathrm{\Ω}}\right)\left(k_{\mathrm{\Ω}d}+k_{\mathrm{\Ω}T}+{\mathrm{\η}}_{\mathrm{\Ω}}\right)\mathrm{sgn}\left(s_{\mathrm{\Ω}}\right)\end{array}---\left(40\right)$

在τ_n(0)＝0的情况下，得到如下等式：

k_ΩT≥T_Ωl_Ωd≥T_Ω|τ_n(t)|_max≥T_Ω|τ_n(t)| (41)

6.3设计李亚普诺夫函数：

$V_{\mathrm{\Ω}}=\frac{1}{2}{s_{\mathrm{\Ω}}}^T{s}_{\mathrm{\Ω}}---\left(42\right)$

对式(39)进行求导得：

${\stackrel{\·}{s}}_{\mathrm{\Ω}}={\stackrel{\·}{d}}_{\mathrm{\Ω}}+{\stackrel{\·}{\mathrm{\τ}}}_n={\stackrel{\·}{d}}_{\mathrm{\Ω}}+{\stackrel{\·}{\mathrm{\τ}}}_n+T_{\mathrm{\Ω}}{\mathrm{\τ}}_n-T_{\mathrm{\Ω}}{\mathrm{\τ}}_n={\stackrel{\·}{d}}_{\mathrm{\Ω}}+v_{\mathrm{\Ω}}-T_{\mathrm{\Ω}}{\mathrm{\τ}}_n---\left(43\right)$

将式(37)带入式(43)中得到：

${\stackrel{\·}{s}}_{\mathrm{\Ω}}={\stackrel{\·}{d}}_{\mathrm{\Ω}}-(k_{\mathrm{\Ω}d}+k_{\mathrm{\Ω}T}+{\mathrm{\η}}_{\mathrm{\Ω}})\mathrm{sgn}\left(s_{\mathrm{\Ω}}\right)-T_{\mathrm{\Ω}}{\mathrm{\τ}}_n---\left(44\right)$

因此，

$\begin{array}{c}{s}_{\mathrm{\Ω}}{\stackrel{\·}{s}}_{\mathrm{\Ω}}={\stackrel{\·}{d}}_{\mathrm{\Ω}}{s}_{\mathrm{\Ω}}-(k_{\mathrm{\Ω}d}+k_{\mathrm{\Ω}T}+{\mathrm{\η}}_{\mathrm{\Ω}})s_{\mathrm{\Ω}}\mathrm{sgn}\left(s_{\mathrm{\Ω}}\right)-T_{\mathrm{\Ω}}{\mathrm{\τ}}_n{s}_{\mathrm{\Ω}}\\ =\left({\stackrel{\·}{d}}_{\mathrm{\Ω}}{s}_{\mathrm{\Ω}}-k_{\mathrm{\Ω}d}{s}_{\mathrm{\Ω}}\right)+\left(-k_{\mathrm{\Ω}T}-T_{\mathrm{\Ω}}{\mathrm{\τ}}_n\right)\left|s_{\mathrm{\Ω}}\right|-{\mathrm{\η}}_{\mathrm{\Ω}}\left|s_{\mathrm{\Ω}}\right|\end{array}---\left(45\right)$

对式(42)进行微分，并代入式(41)得到：

${\stackrel{\·}{V}}_{\mathrm{\Ω}}=s_{\mathrm{\Ω}}{\stackrel{\·}{s}}_{\mathrm{\Ω}}\≤-{\mathrm{\η}}_{\mathrm{\Ω}}\left|s_{\mathrm{\Ω}}\right|\≤0---\left(46\right)$

因此系统姿态角跟踪误差能在有限时间内收敛至零，表明系统是稳定的。

本发明基于系统模型参数耦合非线性的情况，利用全阶滑模，设计四旋翼无人飞行器的有限时间全阶滑模控制方法，保证系统在有限时间内快速收敛至平衡点，加快系统的响应速度，改善滑模控制输入的抖振，保证系统快速稳定收敛。

本发明的技术构思为：针对系统模型参数耦合非线性的四旋翼无人飞行器系统，利用全阶滑模控制方法，再结合一阶滤波器，设计四旋翼无人飞行器的有限时间全阶滑模控制方法。全阶滑模面的设计是为了保证系统的快速稳定收敛。在实际的控制系统中通过设计位置和姿态角两个全阶滑模面，分为内外控制环实现飞行器快速跟踪，并且增加滤波器来改善抖振问题。本发明提供一种加快系统响应速度，改善控制输入抖振问题的控制方法，实现系统的快速稳定控制。

本发明的有益效果为：加快系统动态响应，改善抖振问题，实现快速稳定收敛。

附图说明

图1为本发明的飞行器位置跟踪效果示意图，其中，(a)表示x轴位置跟踪效果，(b)表示y轴位置跟踪效果，(c)表示z轴位置跟踪效果。

图2为本发明的飞行器姿态角收敛效果示意图，其中，(a)表示翻滚角收敛效果，(b)表示俯仰角收敛效果，(c)表示偏航角收敛效果。

图3为本发明的飞行器位置控制器输入示意图，其中，(a)表示位置控制器在x轴方向的输入，(b)表示位置控制器在y轴方向的输入，(c)表示位置控制器在z轴方向的输入。

图4为方法S1飞行器的姿态角控制器输入示意图，其中，(a)表示该姿态角控制器在x轴方向的输入，(b)表示该姿态角控制器在y轴方向的输入，(c)表示该姿态角控制器在z轴方向的输入。

图5为本发明的飞行器姿态角控制器输入示意图，其中，(a)表示该姿态角控制器在x轴方向的输入，(b)表示该姿态角控制器在y轴方向的输入，(c)表示该姿态角控制器在z轴方向的输入。

图6为本发明的控制流程示意图。

具体实施方式

下面结合附图对本发明做进一步说明。

参照图1-图6，一种四旋翼无人飞行器的有限时间全阶滑模控制方法，包括以下步骤：

步骤1，建立四旋翼无人飞行器的动态模型，初始化系统状态、采样时间以及控制参数，过程如下：

1.1在忽略空气阻力和陀螺效应的基础上，对四旋翼无人飞行器进行以下假设：飞行器是刚性的；飞行器的结构是完全对称的；飞行器的重心与机体坐标系原点重合；并定义机体坐标系到惯性坐标系的转移矩阵为：

$R=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\cos \mathrm{\φ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\θ}& \sin \mathrm{\φ}\cos \mathrm{\θ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]---\left(1\right)$

其中，ψ、θ、φ分别为飞行器的偏航角、俯仰角、翻滚角，表示飞行器的惯性坐标系依次绕坐标轴z_E、y_E、x_E旋转的角度；

1.2采用牛顿-欧拉法，从受力角度分析飞行器，其中牛顿公式为：

$F=m\stackrel{\·\·}{\mathrm{\ξ}}---\left(2\right)$

其中，ξ表示飞行器在惯性坐标系下的位置，代表二阶微分，m表示飞行器的质量，F表示作用在飞行器上的合外力，包括飞行器所受重力mg和四个旋翼产生的合力U_F；

式(2)展开为：

$\left[\begin{array}{c}0\\ 0\\ -mg\end{array}\right]+R\left[\begin{array}{c}0\\ 0\\ U_F\end{array}\right]=m\left[\begin{array}{c}\stackrel{\·\·}{x}\\ \stackrel{\·\·}{y}\\ \stackrel{\·\·}{z}\end{array}\right]---\left(3\right)$

其中，x、y、z分别表示飞行器在惯性坐标系下x_E、y_E、z_E轴的位置；

将式(1)代入式(3)中得到如下等式：

$\left\{\begin{array}{c}\stackrel{\·\·}{x}=\frac{U_F}{m}(cos\mathrm{\φ}sin\mathrm{\θ}cos\mathrm{\ψ}+sin\mathrm{\φ}sin\mathrm{\ψ})\\ \stackrel{\·\·}{y}=\frac{U_F}{m}(cos\mathrm{\φ}sin\mathrm{\θ}sin\mathrm{\ψ}-sin\mathrm{\φ}cos\mathrm{\ψ})\\ \stackrel{\·\·}{z}=-g+\frac{U_F}{m}\cos \mathrm{\φ}sin\mathrm{\θ}\end{array}\right.---\left(4\right)$

1.3机体坐标系下欧拉公式为：

$\mathrm{\τ}=I\stackrel{\·}{\mathrm{\ω}}+\mathrm{\ω}\×I\mathrm{\ω}---\left(5\right)$

其中，τ表示作用在飞行器上的力矩，I表示飞行器在机体坐标系下的转动惯量，ω表示飞行器姿态角速度，表示飞行器姿态角加速度，×表示叉乘；

式(5)展开为：

$\left[\begin{array}{c}{\mathrm{\τ}}_x\\ {\mathrm{\τ}}_y\\ {\mathrm{\τ}}_z\end{array}\right]=\left[\begin{array}{ccc}{I}_{xx}& 0& 0\\ 0& I_{yy}& 0\\ 0& 0& I_{zz}\end{array}\right]\left[\begin{array}{c}\stackrel{\·}{p}\\ \stackrel{\·}{q}\\ \stackrel{\·}{r}\end{array}\right]+\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\×\left[\begin{array}{ccc}{I}_{xx}& 0& 0\\ 0& I_{yy}& 0\\ 0& 0& I_{zz}\end{array}\right]\left[\begin{array}{c}p\\ q\\ r\end{array}\right]---\left(6\right)$

其中，τ_x、τ_y、τ_z分别代表机体坐标轴上的各轴力矩分量，I_xx、I_yy、I_zz分别代表机体坐标轴上的各轴转动惯量分量，p、q、r分别代表机体坐标轴上的各轴姿态角速度分量，分别代表机体坐标轴上的各轴姿态角加速度分量；

式(6)表示为：

$\left\{\begin{array}{c}\stackrel{\·}{p}=\frac{I_{yy}-I_{zz}}{I_{xx}}qr+\frac{1}{I_{xx}}{\mathrm{\τ}}_x\\ \stackrel{\·}{q}=\frac{I_{zz}-I_{xx}}{I_{yy}}pr+\frac{1}{I_{yy}}{\mathrm{\τ}}_y\\ \stackrel{\·}{r}=\frac{I_{xx}-I_{yy}}{I_{zz}}pq+\frac{1}{I_{zz}}{\mathrm{\τ}}_z\end{array}\right.---\left(7\right)$

考虑到飞行器一般处于低速飞行或者悬停状态下，姿态角变化较小，此时认为将式(7)改写为：

$\left\{\begin{array}{c}\stackrel{\·\·}{\mathrm{\φ}}=\frac{I_{yy}-I_{zz}}{I_{xx}}\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}+\frac{1}{I_{xx}}{\mathrm{\τ}}_x\\ \stackrel{\·\·}{\mathrm{\θ}}=\frac{I_{zz}-I_{xx}}{I_{yy}}\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\ψ}}+\frac{1}{I_{yy}}{\mathrm{\τ}}_y\\ \stackrel{\·\·}{\mathrm{\ψ}}=\frac{I_{xx}-I_{yy}}{I_{zz}}\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\θ}}+\frac{1}{I_{zz}}{\mathrm{\τ}}_z\end{array}\right.---\left(8\right)$

1.4由于存在测量噪声，电源变化以及外界干扰的影响，式(4)和式(8)中的系统参数并不能准确的获得，因此系统的动态模型改写为：

$\left\{\begin{array}{c}\stackrel{\·\·}{x}=U_x+d_x\\ \stackrel{\·\·}{y}=U_y+d_y\\ \stackrel{\·\·}{z}=U_z+d_z\\ \stackrel{\·\·}{\mathrm{\φ}}=a_1\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}+b_1{\mathrm{\τ}}_x+d_{\mathrm{\φ}}\\ \stackrel{\·\·}{\mathrm{\θ}}=a_2\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\ψ}}+b_2{\mathrm{\τ}}_y+d_{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\ψ}}=a_3\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\θ}}+b_3{\mathrm{\τ}}_z+d_{\mathrm{\ψ}}\end{array}\right.---\left(9\right)$

其中 d_x、d_y、d_z、d_φ、d_θ、d_ψ分别代表模型不确定和外界干扰项；

步骤2，计算系统位置跟踪误差，设计位置全阶滑模面，过程如下：

2.1定义系统位置跟踪误差为

e_q＝q_d-q (10)

其中，q＝[x；y；z]，q_d＝[x_d；y_d；z_d]表示为三阶可导期望轨迹；那么式(10)的一阶微分和二阶微分被表示为：

${\stackrel{\·}{e}}_q={\stackrel{\·}{q}}_d-\stackrel{\·}{q}---\left(11\right)$

${\stackrel{\·\·}{e}}_q={\stackrel{\·\·}{q}}_d-\stackrel{\·\·}{q}---\left(12\right)$

2.2因此，为了避免奇异问题，位置全阶滑模面将定义为：

$s_q={\stackrel{\·\·}{e}}_q+c_{q2}\mathrm{sgn}\left({\stackrel{\·}{e}}_q\right)|{\stackrel{\·}{e}}_q{|}^{{\mathrm{\α}}_{q2}}+c_{q1}\mathrm{sgn}\left(e_q\right)|e_q{|}^{{\mathrm{\α}}_{q1}}---\left(13\right)$

其中，sgn(·)表示符号函数；c_q1和c_q2是一个正的常数，它的选择是保证多项式p²+c_q2p+c_q1的全部特征根在复平面的左半部分以保证系统的稳定性；α_q1和α_q2的选取则是通过以下多项式：

$\left\{\begin{array}{c}{\mathrm{\α}}_1=\mathrm{\α},n=1\\ {\mathrm{\α}}_{i-1}=\frac{{\mathrm{\α}}_i{\mathrm{\α}}_{i+1}}{2{\mathrm{\α}}_{i+1}-{\mathrm{\α}}_i},i=2,...,n,\∀n\≥2\end{array}\right.---\left(14\right)$

其中，α_n+1＝1，α_n＝α，α∈(1-ε,1)以及ε∈(0,1)；

步骤3，基于四旋翼无人飞行器动态模型，根据位置全阶滑模面以及一阶滤波器，设计位置全阶滑模控制器，过程如下：

3.1考虑式(9)，位置全阶滑模控制器被设计为：

u＝(u_eq+u_n) (15)

$u_{eq}=-c_{q2}\mathrm{sgn}\left({\stackrel{\·}{e}}_q\right)|{\stackrel{\·}{e}}_q{|}^{{\mathrm{\α}}_{q2}}-c_{q1}\mathrm{sgn}\left(e_q\right)|e_q{|}^{{\mathrm{\α}}_{q1}}---\left(16\right)$

${\stackrel{\·}{u}}_n+T_q{u}_n=v_q---\left(17\right)$

v_q＝-(k_qd+k_qT+η_q)sgn(s_q) (18)

其中，c_i和α_i是常数，i＝q1,q2，已在式(13)中被定义；T_q是正的常数；k_qd，k_qT和η_q都是常数；

3.2将式(9)带入式(13)中得到如下等式：

$s_q=u+d_q+c_{q2}\mathrm{sgn}\left({\stackrel{\·}{e}}_q\right)|{\stackrel{\·}{e}}_q{|}^{{\mathrm{\α}}_{q2}}+c_{q1}\mathrm{sgn}\left(e_q\right)|e_q{|}^{{\mathrm{\α}}_{q1}}---\left(19\right)$

其中，d_q＝[d_x；d_y；d_z]代表系统位置扰动项，并且各元素是有界的，则假定d_q≤l_qd并且其中l_qd是一个有界的常数；k_qT的选取是要求在T_q＞0时满足k_qT≥T_ql_qd；

将式(15)-式(16)代入式(19)，全阶滑模面被表示成如下等式：

s_q＝d_q+u_n (20)

将式(18)带入式(17)中得到：

$\begin{array}{c}{u}_n\left(t\right)=\left(u_n\left(t_0\right)+\left(1/T_q\right)\left(k_{qd}+k_{qT}+{\mathrm{\η}}_q\right)\mathrm{sgn}\left(s_q\right)\right)e^{t-t_0}\\ -\left(1/T_q\right)\left(k_{qd}+k_{qT}+{\mathrm{\η}}_q\right)\mathrm{sgn}\left(s_q\right)\end{array}---\left(21\right)$

在u_n(0)＝0的情况下，得到如下等式：

k_qT≥T_ql_qd≥T_q|u_n(t)|_max≥T_q|u_n(t)| (22)

3.3设计李亚普诺夫函数：

$V_q=\frac{1}{2}{s_q}^T{s}_q---\left(23\right)$

对式(20)进行求导得：

${\stackrel{\·}{s}}_q={\stackrel{\·}{d}}_q+{\stackrel{\·}{u}}_n={\stackrel{\·}{d}}_q+{\stackrel{\·}{u}}_n+T_q{u}_n-T_q{u}_n={\stackrel{\·}{d}}_q+v_q-T_q{u}_n---\left(24\right)$

将式(18)带入式(24)中得到：

${\stackrel{\·}{s}}_q={\stackrel{\·}{d}}_q-(k_{qd}+k_{qT}+{\mathrm{\η}}_q)sgn\left(s_q\right)-T_q{u}_n---\left(25\right)$

因此，

$\begin{array}{c}{s}_q{\stackrel{\·}{s}}_q={\stackrel{\·}{d}}_q{s}_q-\left(k_{qd}+k_{qT}+{\mathrm{\η}}_q\right)s_q\mathrm{sgn}\left(s_q\right)-T_q{u}_n{s}_q\\ =\left({\stackrel{\·}{d}}_q{s}_q-k_{qd}{s}_q\right)+\left(-k_{qT}-T_q{u}_n\right)\left|s_q\right|-{\mathrm{\η}}_q\left|s_q\right|\end{array}---\left(26\right)$

对式(23)进行微分，并代入式(22)得到：

${\stackrel{\·}{V}}_q=s_q{\stackrel{\·}{s}}_q\≤-{\mathrm{\η}}_q\left|s_q\right|\≤0---\left(27\right)$

因此系统位置跟踪误差能在有限时间内收敛至零，表明系统是稳定的；

步骤4，给定偏航角，计算总升力，俯仰角和翻滚角期待值，过程如下：

4.1由式(9)得到：

$\left\{\begin{array}{c}{U}_F=m\sqrt{{U_x}^2+{U_y}^2+{(U_z+g)}^2}\\ {\mathrm{\φ}}_d=\arcsin \[\frac{m}{U_F}(U_x{\mathrm{sin\ψ}}_d-U_y{\mathrm{cos\ψ}}_d)\]\\ {\mathrm{\θ}}_d=\arctan \[\frac{1}{U_z+g}(U_x{\mathrm{cos\ψ}}_d+U_y{\mathrm{sin\ψ}}_d)\]\end{array}\right.---\left(28\right)$

步骤5，计算系统姿态角跟踪误差，设计姿态全阶滑模面，过程如下：

5.1定义系统姿态角跟踪误差为

e_Ω＝Ω_d-Ω (29)

其中，Ω＝[φ；θ；ψ]，Ω_d＝[φ_d；θ_d；ψ_d]表示为三阶可导期望姿态角；那么式(29)的一阶微分和二阶微分被表示为：

${\stackrel{\·}{e}}_{\mathrm{\Ω}}={\stackrel{\·}{\mathrm{\Ω}}}_d-\stackrel{\·}{\mathrm{\Ω}}---\left(30\right)$

$e_{\mathrm{\Ω}}={\stackrel{\·\·}{\mathrm{\Ω}}}_d-\stackrel{\·\·}{\mathrm{\Ω}}---\left(31\right)$

5.2因此，为了避免奇异问题，姿态全阶滑模面将定义为：

$s_{\mathrm{\Ω}}={\stackrel{\·\·}{e}}_{\mathrm{\Ω}}+c_{\mathrm{\Ω}2}\mathrm{sgn}\left({\stackrel{\·}{e}}_{\mathrm{\Ω}}\right)|{\stackrel{\·}{e}}_{\mathrm{\Ω}}{|}^{{\mathrm{\α}}_{\mathrm{\Ω}2}}+c_{\mathrm{\Ω}1}\mathrm{sgn}\left(e_{\mathrm{\Ω}}\right)|e_{\mathrm{\Ω}}{|}^{{\mathrm{\α}}_{\mathrm{\Ω}1}}---\left(32\right)$

其中，c_Ω1和c_Ω2是一个正的常数，它的选择是保证多项式p²+c_Ω2p+c_Ω1的全部特征根在复平面的左半部分以保证系统的稳定性；α_Ω1和α_Ω2的选取则是通过以下多项式：

$\left\{\begin{array}{c}{\mathrm{\α}}_1=\mathrm{\α},n=1\\ {\mathrm{\α}}_{i-1}=\frac{{\mathrm{\α}}_i{\mathrm{\α}}_{i+1}}{2{\mathrm{\α}}_{i+1}-{\mathrm{\α}}_i},i=2,...,n,\∀n\≥2\end{array}\right.---\left(33\right)$

其中，α_n+1＝1，α_n＝α，α∈(1-ε,1)以及ε∈(0,1)；

步骤6，基于四旋翼无人飞行器动态模型，根据姿态全阶滑模面以及一阶滤波器，设计姿态全阶滑模控制器，过程如下：

6.1考虑式(9)，姿态全阶滑模控制器被设计为：

τ＝b^-1(τ_eq+τ_n) (34)

${\mathrm{\τ}}_{eq}=-af-c_{\mathrm{\Ω}2}\mathrm{sgn}\left({\stackrel{\·}{e}}_{\mathrm{\Ω}}\right)|{\stackrel{\·}{e}}_{\mathrm{\Ω}}{|}^{{\mathrm{\α}}_{\mathrm{\Ω}2}}-c_{\mathrm{\Ω}1}\mathrm{sgn}\left(e_{\mathrm{\Ω}}\right)|e_{\mathrm{\Ω}}{|}^{{\mathrm{\α}}_{\mathrm{\Ω}1}}---\left(35\right)$

${\stackrel{\·}{\mathrm{\τ}}}_n+T_{\mathrm{\Ω}}{\mathrm{\τ}}_n=v_{\mathrm{\Ω}}---\left(36\right)$

v_Ω＝-(k_Ωd+k_ΩT+η_Ω)sgn(s_Ω) (37)

其中，a＝[a₁；a₂；a₃]，b＝[b₁；b₂；b₃]，c_i和α_i是常数，i＝Ω1,Ω2，已在式(32)中被定义；T_Ω是正的常数；k_Ωd，k_ΩT和η_Ω都是常数；

6.2将式(9)带入式(32)中得到如下等式：

$s_{\mathrm{\Ω}}=b\mathrm{\τ}+af+d_{\mathrm{\Ω}}+c_{\mathrm{\Ω}2}\mathrm{sgn}\left({\stackrel{\·}{e}}_{\mathrm{\Ω}}\right)|{\stackrel{\·}{e}}_{\mathrm{\Ω}}{|}^{{\mathrm{\α}}_{\mathrm{\Ω}2}}+c_{\mathrm{\Ω}1}\mathrm{sgn}\left(e_{\mathrm{\Ω}}\right)|e_{\mathrm{\Ω}}{|}^{{\mathrm{\α}}_{\mathrm{\Ω}1}}---\left(38\right)$

其中，d_Ω＝[d_φ；d_θ；d_ψ]代表系统位置扰动项，并且各元素是有界的，则假定d_Ω≤l_Ωd并且其中l_Ωd是一个有界的常数；k_ΩT的选取是要求在T_Ω＞0时满足k_ΩT≥T_Ωl_Ωd；

将式(34)-式(35)代入式(38)，全阶滑模面被表示成如下等式：

s_Ω＝d_Ω+τ_n (39)

将式(37)带入式(36)中得到：

$\begin{array}{c}{\mathrm{\τ}}_n\left(t\right)=\left({\mathrm{\τ}}_n\right(t_0)+(1/T_{\mathrm{\Ω}}\left)\right(k_{\mathrm{\Ω}d}+k_{\mathrm{\Ω}T}+{\mathrm{\η}}_{\mathrm{\Ω}}\left)sgn\right(s_{\mathrm{\Ω}}\left)\right)e^{t-t_0}\\ -\left(1/T_{\mathrm{\Ω}}\right)\left(k_{\mathrm{\Ω}d}+k_{\mathrm{\Ω}T}+{\mathrm{\η}}_{\mathrm{\Ω}}\right)\mathrm{sgn}\left(s_{\mathrm{\Ω}}\right)\end{array}---\left(40\right)$

在τ_n(0)＝0的情况下，得到如下等式：

k_ΩT≥T_Ωl_Ωd≥T_Ω|τ_n(t)|_max≥T_Ω|τ_n(t)| (41)

6.3设计李亚普诺夫函数：

$V_{\mathrm{\Ω}}=\frac{1}{2}{s_{\mathrm{\Ω}}}^T{s}_{\mathrm{\Ω}}---\left(42\right)$

对式(39)进行求导得：

${\stackrel{\·}{s}}_{\mathrm{\Ω}}={\stackrel{\·}{d}}_{\mathrm{\Ω}}+{\stackrel{\·}{\mathrm{\τ}}}_n={\stackrel{\·}{d}}_{\mathrm{\Ω}}+{\stackrel{\·}{\mathrm{\τ}}}_n+T_{\mathrm{\Ω}}{\mathrm{\τ}}_n-T_{\mathrm{\Ω}}{\mathrm{\τ}}_n={\stackrel{\·}{d}}_{\mathrm{\Ω}}+v_{\mathrm{\Ω}}-T_{\mathrm{\Ω}}{\mathrm{\τ}}_n---\left(43\right)$

将式(37)带入式(43)中得到：

${\stackrel{\·}{s}}_{\mathrm{\Ω}}={\stackrel{\·}{d}}_{\mathrm{\Ω}}-(k_{\mathrm{\Ω}d}+k_{\mathrm{\Ω}T}+{\mathrm{\η}}_{\mathrm{\Ω}})\mathrm{sgn}\left(s_{\mathrm{\Ω}}\right)-T_{\mathrm{\Ω}}{\mathrm{\τ}}_n---\left(44\right)$

因此，

$\begin{array}{c}{s}_{\mathrm{\Ω}}{\stackrel{\·}{s}}_{\mathrm{\Ω}}={\stackrel{\·}{d}}_{\mathrm{\Ω}}{s}_{\mathrm{\Ω}}-(k_{\mathrm{\Ω}d}+k_{\mathrm{\Ω}T}+{\mathrm{\η}}_{\mathrm{\Ω}})s_{\mathrm{\Ω}}\mathrm{sgn}\left(s_{\mathrm{\Ω}}\right)-T_{\mathrm{\Ω}}{\mathrm{\τ}}_n{s}_{\mathrm{\Ω}}\\ =\left({\stackrel{\·}{d}}_{\mathrm{\Ω}}{s}_{\mathrm{\Ω}}-k_{\mathrm{\Ω}d}{s}_{\mathrm{\Ω}}\right)+\left(-k_{\mathrm{\Ω}T}-T_{\mathrm{\Ω}}{\mathrm{\τ}}_n\right)\left|s_{\mathrm{\Ω}}\right|-{\mathrm{\η}}_{\mathrm{\Ω}}\left|s_{\mathrm{\Ω}}\right|\end{array}---\left(45\right)$

对式(42)进行微分，并代入式(41)得到：

${\stackrel{\·}{V}}_{\mathrm{\Ω}}=s_{\mathrm{\Ω}}{\stackrel{\·}{s}}_{\mathrm{\Ω}}\≤-{\mathrm{\η}}_{\mathrm{\Ω}}\left|s_{\mathrm{\Ω}}\right|\≤0---\left(46\right)$

因此系统姿态角跟踪误差能在有限时间内收敛至零，表明系统是稳定的。

为了验证所提方法的有效性，本发明给出了以下二种方法进行对比：

S1：传统线性滑模控制方法；

S2：本发明的有限时间全阶滑模控制方法。

为了更有效的进行对比，系统所有初始状态和模型参数都是一致的，即：x＝y＝z＝0，φ＝θ＝ψ＝0；m＝0.625kg，I_xx＝2.3×10^-3kg·m²，I_yy＝2.4×10^-3kg·m²，I_zz＝2.6×10^-3kg·m²，g＝10m·s^-2；滑模面参数：c_q1＝16，c_q2＝10，c_Ω1＝16，c_Ω1＝10，α_q1＝9/23，α_q2＝9/16，α_Ω1＝9/23，α_Ω1＝9/16；控制器参数：T_q＝1，T_Ω＝1，k_qd+k_qT+η_q＝1，k_Ωd+k_ΩT+η_Ω＝10；参考轨迹：x_d＝y_d＝z_d＝2，ψ_d＝0.5。

由图1可以看出，方法S2较方法S1要更早跟上期望信号，大约提前1.3秒，可见方法S2相比S1响应速度更快，能有效满足飞行器快速跟踪的要求；由图2可以看出，方法S2较方法S1，其中翻滚角和俯仰角收敛至零的时间几乎一样，但偏航角更早达到期望值，大约提前1.3秒；由图3可以看出，方法S2于方法S1，位置控制器的输入都几乎不存在抖振情况；由图4和图5可以看出，方法S1较方法S2存在严重的抖振情况，导致的原因是方法S2的实际控制律中不再存在滑模切换函数。

以上阐述的是本发明给出的一个实施例表现出的优良优化效果，显然本发明不只是限于上述实施例，在不偏离本发明基本精神及不超出本发明实质内容所涉及范围的前提下对其可作种种变形加以实施。

Claims (1)

1.一种四旋翼无人飞行器的有限时间全阶滑模控制方法，其特征在于：包括以下步骤：

步骤1，建立四旋翼无人飞行器的动态模型，初始化系统状态、采样时间以及控制参数，过程如下：

1.1在忽略空气阻力和陀螺效应的基础上，对四旋翼无人飞行器进行以下假设：飞行器是刚性的；飞行器的结构是完全对称的；飞行器的重心与机体坐标系原点重合；并定义机体坐标系到惯性坐标系的转移矩阵为：

$R=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\cos \mathrm{\φ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\θ}& \sin \mathrm{\φ}\cos \mathrm{\θ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]---\left(1\right)$

其中，ψ、θ、φ分别为飞行器的偏航角、俯仰角、翻滚角，表示飞行器的惯性坐标系依次绕坐标轴z_E、y_E、x_E旋转的角度；

1.2采用牛顿-欧拉法，从受力角度分析飞行器，其中牛顿公式为：

$F=m\stackrel{\·\·}{\mathrm{\ξ}}---\left(2\right)$

其中，ξ表示飞行器在惯性坐标系下的位置，代表二阶微分，m表示飞行器的质量，F表示作用在飞行器上的合外力，包括飞行器所受重力mg和四个旋翼产生的合力U_F；

式(2)展开为：

$\left[\begin{array}{c}0\\ 0\\ -mg\end{array}\right]+R\left[\begin{array}{c}0\\ 0\\ U_F\end{array}\right]=m\left[\begin{array}{c}\stackrel{\·\·}{x}\\ \stackrel{\·\·}{y}\\ \stackrel{\·\·}{z}\end{array}\right]---\left(3\right)$

其中，x、y、z分别表示飞行器在惯性坐标系下x_E、y_E、z_E轴的位置；

将式(1)代入式(3)中得到如下等式：

$\left\{\begin{array}{c}\stackrel{\·\·}{x}=\frac{U_F}{m}(\cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ})\\ \stackrel{\·\·}{y}=\frac{U_F}{m}(\cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ})\\ \stackrel{\·\·}{z}=-g+\frac{U_F}{m}\cos \mathrm{\φ}\sin \mathrm{\θ}\end{array}\right.---\left(4\right)$

1.3机体坐标系下欧拉公式为：

$\mathrm{\τ}=I\stackrel{\·}{\mathrm{\ω}}+\mathrm{\ω}\×I\mathrm{\ω}---\left(5\right)$

其中，τ表示作用在飞行器上的力矩，I表示飞行器在机体坐标系下的转动惯量，ω表示飞行器姿态角速度，表示飞行器姿态角加速度，×表示叉乘；

式(5)展开为：

$\left[\begin{array}{c}{\mathrm{\τ}}_x\\ {\mathrm{\τ}}_y\\ {\mathrm{\τ}}_z\end{array}\right]=\left[\begin{array}{ccc}{I}_{xx}& 0& 0\\ 0& I_{yy}& 0\\ 0& 0& I_{zz}\end{array}\right]\left[\begin{array}{c}\stackrel{\·}{p}\\ \stackrel{\·}{q}\\ \stackrel{\·}{r}\end{array}\right]+\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\×\left[\begin{array}{ccc}{I}_{xx}& 0& 0\\ 0& I_{yy}& 0\\ 0& 0& I_{zz}\end{array}\right]\left[\begin{array}{c}p\\ q\\ r\end{array}\right]---\left(6\right)$

其中，τ_x、τ_y、τ_z分别代表机体坐标轴上的各轴力矩分量，I_xx、I_yy、I_zz分别代表机体坐标轴上的各轴转动惯量分量，p、q、r分别代表机体坐标轴上的各轴姿态角速度分量，分别代表机体坐标轴上的各轴姿态角加速度分量；

式(6)表示为：

$\left\{\begin{array}{c}\stackrel{\·}{p}=\frac{I_{yy}-I_{zz}}{I_{xx}}qr+\frac{1}{I_{xx}}{\mathrm{\τ}}_x\\ \stackrel{\·}{q}=\frac{I_{zz}-I_{xx}}{I_{yy}}pr+\frac{1}{I_{yy}}{\mathrm{\τ}}_y\\ \stackrel{\·}{r}=\frac{I_{xx}-I_{yy}}{I_{zz}}pq+\frac{1}{I_{zz}}{\mathrm{\τ}}_z\end{array}\right.---\left(7\right)$

考虑到飞行器一般处于低速飞行或者悬停状态下，姿态角变化较小，此时认为将式(7)改写为：

$\left\{\begin{array}{c}\stackrel{\·\·}{\mathrm{\φ}}=\frac{I_{yy}-I_{zz}}{I_{xx}}\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}+\frac{1}{I_{xx}}{\mathrm{\τ}}_x\\ \stackrel{\·\·}{\mathrm{\θ}}=\frac{I_{zz}-I_{xx}}{I_{yy}}\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\ψ}}+\frac{1}{I_{yy}}{\mathrm{\τ}}_y\\ \stackrel{\·\·}{\mathrm{\ψ}}=\frac{I_{xx}-I_{yy}}{I_{zz}}\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\θ}}+\frac{1}{I_{zz}}{\mathrm{\τ}}_z\end{array}\right.---\left(8\right)$

1.4由于存在测量噪声，电源变化以及外界干扰的影响，式(4)和式(8)中的系统参数并不能准确的获得，因此系统的动态模型改写为：

$\left\{\begin{array}{c}\stackrel{\·\·}{x}=U_x+d_x\\ \stackrel{\·\·}{y}=U_y+d_y\\ \stackrel{\·\·}{z}=U_z+d_z\\ \stackrel{\·\·}{\mathrm{\φ}}=a_1\stackrel{\·}{\mathrm{\θ}}\stackrel{\·}{\mathrm{\ψ}}+b_1{\mathrm{\τ}}_x+d_{\mathrm{\φ}}\\ \stackrel{\·\·}{\mathrm{\θ}}=a_2\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\ψ}}+b_2{\mathrm{\τ}}_y+d_{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\ψ}}=a_3\stackrel{\·}{\mathrm{\φ}}\stackrel{\·}{\mathrm{\θ}}+b_3{\mathrm{\τ}}_z+d_{\mathrm{\ψ}}\end{array}\right.---\left(9\right)$

其中

$U_x=\frac{U_F}{m}(cos\mathrm{\φ}\sin \mathrm{\θ}cos\mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ}),U_y=\frac{U_F}{m}(\cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}cos\mathrm{\ψ}),$

$U_z=-g+\frac{U_F}{m}cos\mathrm{\φ}cos\mathrm{\θ},a_1=\frac{I_{yy}-I_{zz}}{I_{xx}},a_2=\frac{I_{zz}-I_{xx}}{I_{yy}},a_3=\frac{I_{xx}-I_{yy}}{I_{zz}},$ d_x、d_y、d_z、d_φ、d_θ、d_ψ分别代表模型不确定和外界干扰项；

步骤2，计算系统位置跟踪误差，设计位置全阶滑模面，过程如下：

2.1定义系统位置跟踪误差为

e_q＝q_d-q (10)

其中，q＝[x；y；z]，q_d＝[x_d；y_d；z_d]表示为三阶可导期望轨迹；那么式(10)的一阶微分和二阶微分被表示为：

${\stackrel{\·}{e}}_q={\stackrel{\·}{q}}_d-\stackrel{\·}{q}---\left(11\right)$

${\stackrel{\·\·}{e}}_q={\stackrel{\·\·}{q}}_d-\stackrel{\·\·}{q}---\left(12\right)$

2.2因此，为了避免奇异问题，位置全阶滑模面将定义为：

$s_q={\stackrel{\·\·}{e}}_q+c_{q2}\mathrm{sgn}\left({\stackrel{\·}{e}}_q\right)|{\stackrel{\·}{e}}_q{|}^{{\mathrm{\α}}_{q2}}+c_{q1}\mathrm{sgn}\left(e_q\right)|e_q{|}^{{\mathrm{\α}}_{q1}}---\left(13\right)$

其中，sgn(·)表示符号函数；c_q1和c_q2是一个正的常数，它的选择是保证多项式p²+c_q2p+c_q1的全部特征根在复平面的左半部分以保证系统的稳定性；α_q1和α_q2的选取则是通过以下多项式：

$\left\{\begin{array}{c}{\mathrm{\α}}_1=\mathrm{\α},n=1\\ {\mathrm{\α}}_{i-1}=\frac{{\mathrm{\α}}_i{\mathrm{\α}}_{i+1}}{2{\mathrm{\α}}_{i+1}-{\mathrm{\α}}_i},i=2,...,n,\∀n\≥2\end{array}\right.---\left(14\right)$

其中，α_n+1＝1，α_n＝α，α∈(1-ε,1)以及ε∈(0,1)；

步骤3，基于四旋翼无人飞行器动态模型，根据位置全阶滑模面以及一阶滤波器，设计位置全阶滑模控制器，过程如下：

3.1考虑式(9)，位置全阶滑模控制器被设计为：

u＝(u_eq+u_n) (15)

$u_{eq}=-c_{q2}\mathrm{sgn}\left({\stackrel{\·}{e}}_q\right)|{\stackrel{\·}{e}}_q{|}^{{\mathrm{\α}}_{q2}}-c_{q1}\mathrm{sgn}\left(e_q\right)|e_q{|}^{{\mathrm{\α}}_{q1}}---\left(16\right)$

${\stackrel{\·}{u}}_n+T_q{u}_n=v_q---\left(17\right)$

v_q＝-(k_qd+k_qT+η_q)sgn(s_q) (18)

其中，c_i和α_i是常数，i＝q1,q2，已在式(13)中被定义；T_q是正的常数；k_qd，k_qT和η_q都是常数；

3.2将式(9)带入式(13)中得到如下等式：

$s_q=u+d_q+c_{q2}\mathrm{sgn}\left({\stackrel{\·}{e}}_q\right)|{\stackrel{\·}{e}}_q{|}^{{\mathrm{\α}}_{q2}}+c_{q1}\mathrm{sgn}\left(e_q\right)|e_q{|}^{{\mathrm{\α}}_{q1}}---\left(19\right)$

其中，d_q＝[d_x；d_y；d_z]代表系统位置扰动项，并且各元素是有界的，则假定d_q≤l_qd并且其中l_qd是一个有界的常数；k_qT的选取是要求在T_q＞0时满足k_qT≥T_ql_qd；

将式(15)-式(16)代入式(19)，全阶滑模面被表示成如下等式：

s_q＝d_q+u_n (20)

将式(18)带入式(17)中得到：

$\begin{array}{c}{u}_n\left(t\right)=\left(u_n\right(t_0)+(1/T_q)(k_{qd}+k_{qT}+{\mathrm{\η}}_q)\mathrm{sgn}\left(s_q\right))e^{t-t_0}\\ -(1/T_q)(k_{qd}+k_{qT}+{\mathrm{\η}}_q)\mathrm{sgn}\left(s_q\right)\end{array}---\left(21\right)$

在u_n(0)＝0的情况下，得到如下等式：

k_qT≥T_ql_qd≥T_q|u_n(t)|_max≥T_q|u_n(t)| (22)

3.3设计李亚普诺夫函数：

$V_q=\frac{1}{2}{s_q}^T{s}_q---\left(23\right)$

对式(20)进行求导得：

${\stackrel{\·}{s}}_q={\stackrel{\·}{d}}_q+{\stackrel{\·}{u}}_n={\stackrel{\·}{d}}_q+{\stackrel{\·}{u}}_n+T_q{u}_n-T_q{u}_n={\stackrel{\·}{d}}_q+v_q-T_q{u}_n---\left(24\right)$

将式(18)带入式(24)中得到：

${\stackrel{\·}{s}}_q={\stackrel{\·}{d}}_q-(k_{qd}+k_{qT}+{\mathrm{\η}}_q)sgn\left(s_q\right)-T_q{u}_n---\left(25\right)$

因此，

$\begin{array}{c}{s}_q{\stackrel{\·}{s}}_q={\stackrel{\·}{d}}_q{s}_q-(k_{qd}+k_{qT}+{\mathrm{\η}}_q)s_{q}sgn\left(s_q\right)-T_q{u}_n{s}_q\\ =({\stackrel{\·}{d}}_q{s}_q-k_{qd}{s}_q)+(-k_{qT}-T_q{u}_n)\left|s_q\right|-{\mathrm{\η}}_q\left|s_q\right|\end{array}---\left(26\right)$

对式(23)进行微分，并代入式(22)得到：

${\stackrel{\·}{V}}_q=s_q{\stackrel{\·}{s}}_q\≤-{\mathrm{\η}}_q\left|s_q\right|\≤0---\left(27\right)$

因此系统位置跟踪误差能在有限时间内收敛至零，表明系统是稳定的；

步骤4，给定偏航角，计算总升力，俯仰角和翻滚角期待值，过程如下：

4.1由式(9)得到：

$\left\{\begin{array}{c}{U}_F=m\sqrt{{U_x}^2+{U_y}^2+{(U_z+g)}^2}\\ {\mathrm{\φ}}_d=\arcsin \[\frac{m}{U_F}(U_x{\mathrm{sin\ψ}}_d-U_y{\mathrm{cos\ψ}}_d)\]\\ {\mathrm{\θ}}_d=\arctan \[\frac{1}{U_z+g}(U_x{\mathrm{cos\ψ}}_d+U_y{\mathrm{sin\ψ}}_d)\]\end{array}\right.---\left(28\right)$

步骤5，计算系统姿态角跟踪误差，设计姿态全阶滑模面，过程如下：

5.1定义系统姿态角跟踪误差为

e_Ω＝Ω_d-Ω (29)

其中，Ω＝[φ；θ；ψ]，Ω_d＝[φ_d；θ_d；ψ_d]表示为三阶可导期望姿态角；那么式(29)的一阶微分和二阶微分被表示为：

${\stackrel{\·}{e}}_{\mathrm{\Ω}}={\stackrel{\·}{\mathrm{\Ω}}}_d-\stackrel{\·}{\mathrm{\Ω}}---\left(30\right)$

$e_{\mathrm{\Ω}}={\stackrel{\·\·}{\mathrm{\Ω}}}_d-\stackrel{\·\·}{\mathrm{\Ω}}---\left(31\right)$

5.2因此，为了避免奇异问题，姿态全阶滑模面将定义为：

$s_{\mathrm{\Ω}}={\stackrel{\·\·}{e}}_{\mathrm{\Ω}}+c_{\mathrm{\Ω}2}\mathrm{sgn}\left({\stackrel{\·}{e}}_{\mathrm{\Ω}}\right)|{\stackrel{\·}{e}}_{\mathrm{\Ω}}{|}^{{\mathrm{\α}}_{\mathrm{\Ω}2}}+c_{\mathrm{\Ω}1}\mathrm{sgn}\left(e_{\mathrm{\Ω}}\right)|e_{\mathrm{\Ω}}{|}^{{\mathrm{\α}}_{\mathrm{\Ω}1}}---\left(32\right)$

其中，c_Ω1和c_Ω2是一个正的常数，它的选择是保证多项式p²+c_Ω2p+c_Ω1的全部特征根在复平面的左半部分以保证系统的稳定性；α_Ω1和α_Ω2的选取则是通过以下多项式：

$\left\{\begin{array}{c}{\mathrm{\α}}_1=\mathrm{\α},n=1\\ {\mathrm{\α}}_{i-1}=\frac{{\mathrm{\α}}_i{\mathrm{\α}}_{i+1}}{2{\mathrm{\α}}_{i+1}-{\mathrm{\α}}_i},i=2,...,n,\∀n\≥2\end{array}\right.---\left(33\right)$

其中，α_n+1＝1，α_n＝α，α∈(1-ε,1)以及ε∈(0,1)；

步骤6，基于四旋翼无人飞行器动态模型，根据姿态全阶滑模面以及一阶滤波器，设计姿态全阶滑模控制器，过程如下：

6.1考虑式(9)，姿态全阶滑模控制器被设计为：

τ＝b^-1(τ_eq+τ_n) (34)

${\mathrm{\τ}}_{eq}=-af-c_{\mathrm{\Ω}2}\mathrm{sgn}\left({\stackrel{\·}{e}}_{\mathrm{\Ω}}\right)|{\stackrel{\·}{e}}_{\mathrm{\Ω}}{|}^{{\mathrm{\α}}_{\mathrm{\Ω}2}}-c_{\mathrm{\Ω}1}\mathrm{sgn}\left(e_{\mathrm{\Ω}}\right)|e_{\mathrm{\Ω}}{|}^{{\mathrm{\α}}_{\mathrm{\Ω}1}}---\left(35\right)$

${\stackrel{\·}{\mathrm{\τ}}}_n+T_{\mathrm{\Ω}}{\mathrm{\τ}}_n=v_{\mathrm{\Ω}}---\left(36\right)$

v_Ω＝-(k_Ωd+k_ΩT+η_Ω)sgn(s_Ω) (37)

其中，a＝[a₁；a₂；a₃]，b＝[b₁；b₂；b₃]，c_i和α_i是常数，i＝Ω1,Ω2，已在式(32)中被定义；T_Ω是正的常数；k_Ωd，k_ΩT和η_Ω都是常数；

6.2将式(9)带入式(32)中得到如下等式：

$s_{\mathrm{\Ω}}=b\mathrm{\τ}+af+d_{\mathrm{\Ω}}+c_{\mathrm{\Ω}2}\mathrm{sgn}\left({\stackrel{\·}{e}}_{\mathrm{\Ω}}\right)|{\stackrel{\·}{e}}_{\mathrm{\Ω}}{|}^{{\mathrm{\α}}_{\mathrm{\Ω}2}}+c_{\mathrm{\Ω}1}\mathrm{sgn}\left(e_{\mathrm{\Ω}}\right)|e_{\mathrm{\Ω}}{|}^{{\mathrm{\α}}_{\mathrm{\Ω}1}}---\left(38\right)$

其中，d_Ω＝[d_φ；d_θ；d_ψ]代表系统位置扰动项，并且各元素是有界的，则假定d_Ω≤l_Ωd并且其中l_Ωd是一个有界的常数；k_ΩT的选取是要求在T_Ω＞0时满足k_ΩT≥T_Ωl_Ωd；

将式(34)-式(35)代入式(38)，全阶滑模面被表示成如下等式：

s_Ω＝d_Ω+τ_n (39)

将式(37)带入式(36)中得到：

$\begin{array}{c}{\mathrm{\τ}}_n\left(t\right)=\left({\mathrm{\τ}}_n\right(t_0)+(1/T_{\mathrm{\Ω}})(k_{\mathrm{\Ω}d}+k_{\mathrm{\Ω}T}+{\mathrm{\η}}_{\mathrm{\Ω}})\mathrm{sgn}\left(s_{\mathrm{\Ω}}\right))e^{t-t_0}\\ -(1/T_{\mathrm{\Ω}})(k_{\mathrm{\Ω}d}+k_{\mathrm{\Ω}T}+{\mathrm{\η}}_{\mathrm{\Ω}})\mathrm{sgn}\left(s_{\mathrm{\Ω}}\right)\end{array}---\left(40\right)$

在τ_n(0)＝0的情况下，得到如下等式：

k_ΩT≥T_Ωl_Ωd≥T_Ω|τ_n(t)|_max≥T_Ω|τ_n(t)| (41)

6.3设计李亚普诺夫函数：

$V_{\mathrm{\Ω}}=\frac{1}{2}{s_{\mathrm{\Ω}}}^T{s}_{\mathrm{\Ω}}---\left(42\right)$

对式(39)进行求导得：

${\stackrel{\·}{s}}_{\mathrm{\Ω}}={\stackrel{\·}{d}}_{\mathrm{\Ω}}+{\stackrel{\·}{\mathrm{\τ}}}_n={\stackrel{\·}{d}}_{\mathrm{\Ω}}+{\stackrel{\·}{\mathrm{\τ}}}_n+T_{\mathrm{\Ω}}{\mathrm{\τ}}_n-T_{\mathrm{\Ω}}{\mathrm{\τ}}_n={\stackrel{\·}{d}}_{\mathrm{\Ω}}+v_{\mathrm{\Ω}}-T_{\mathrm{\Ω}}{\mathrm{\τ}}_n---\left(43\right)$

将式(37)带入式(43)中得到：

${\stackrel{\·}{s}}_{\mathrm{\Ω}}={\stackrel{\·}{d}}_{\mathrm{\Ω}}-(k_{\mathrm{\Ω}d}+k_{\mathrm{\Ω}T}+{\mathrm{\η}}_{\mathrm{\Ω}})\mathrm{sgn}\left(s_{\mathrm{\Ω}}\right)-T_{\mathrm{\Ω}}{\mathrm{\τ}}_n---\left(44\right)$

因此，

$\begin{array}{c}{s}_{\mathrm{\Ω}}{\stackrel{\·}{s}}_{\mathrm{\Ω}}={\stackrel{\·}{d}}_{\mathrm{\Ω}}{s}_{\mathrm{\Ω}}-(k_{\mathrm{\Ω}d}+k_{\mathrm{\Ω}T}+{\mathrm{\η}}_{\mathrm{\Ω}})s_{\mathrm{\Ω}}\mathrm{sgn}\left(s_{\mathrm{\Ω}}\right)-T_{\mathrm{\Ω}}{\mathrm{\τ}}_n{s}_{\mathrm{\Ω}}\\ =({\stackrel{\·}{d}}_{\mathrm{\Ω}}{s}_{\mathrm{\Ω}}-k_{\mathrm{\Ω}d}{s}_{\mathrm{\Ω}})+(-k_{\mathrm{\Ω}T}-T_{\mathrm{\Ω}}{\mathrm{\τ}}_n)\left|s_{\mathrm{\Ω}}\right|-{\mathrm{\η}}_{\mathrm{\Ω}}\left|s_{\mathrm{\Ω}}\right|\end{array}---\left(45\right)$

对式(42)进行微分，并代入式(41)得到：

${\stackrel{\·}{V}}_{\mathrm{\Ω}}=s_{\mathrm{\Ω}}{\stackrel{\·}{s}}_{\mathrm{\Ω}}\≤-{\mathrm{\η}}_{\mathrm{\Ω}}\left|s_{\mathrm{\Ω}}\right|\≤0---\left(46\right)$

因此系统姿态角跟踪误差能在有限时间内收敛至零，表明系统是稳定的。