CN105759832A - 一种基于反演法的四旋翼飞行器滑模变结构控制方法 - Google Patents

Abstract

本发明公开了一种基于反演法的四旋翼飞行器滑模变结构控制方法，首先对四旋翼飞行器动力学模型分析并且进行简化，将系统分解为全驱动子系统和欠驱动子系统；进而以滑模变结构控制理论为基础，利用反演控制方法推导出滑模控制面，分别为两个子系统设计了控制律，并通过李雅普诺夫稳定性理论验证了系统的稳定性；最终设计出四旋翼飞行器的控制器。本发明所提出的方法将反演法和滑模变结构控制方法相结合，用于飞行器的定点悬停和轨迹跟踪控制，对其动态性和稳定性进行分析，有效地提高了四旋翼飞行器的响应速度和控制精度，增加了系统的抗干扰性。

Description

一种基于反演法的四旋翼飞行器滑模变结构控制方法

技术领域

本发明涉及一种基于反演法的四旋翼飞行器滑模变结构控制方法，属于飞行器控制技术领域。

背景技术

近年来，四旋翼飞行器逐渐成为航空领域学者们的研究热点，作为一种无人飞行器，由于其特有的优势，并且其对环境和场地的要求比较小，使得其在军用与民用领域应用越来越广泛，包括侦查、环境监测、电力巡检、航拍、灾难救援等。改变四旋翼飞行器的四个螺旋桨的转速，可以改变各种飞行姿态，实现垂直升降、定点悬停、轨迹跟踪等六自由度的运动。目前学者们已经在飞行器上取得了很多成就，然而飞行器具有非线性、强耦合、欠驱动等特点，导致其动力学模型非常复杂和控制器设计十分繁琐，因而需要更好的控制方法来保证四旋翼飞行器的飞行品质，其中，姿态控制是飞行器稳定飞行的基本要求。

对四旋翼飞行器控制的方法有很多种，包括PID控制、LQ控制、DI控制等，而PID控制最为常见，虽然具有一定的鲁棒性，但在受到外界干扰时控制效果不太理想，不能适应复杂多变的环境，同时PID控制和LQ控制忽略了模型的非线性因素，导致模型的精度较差，影响了控制的精度。滑模变结构控制是变结构控制系统的一种策略，这种控制策略与常规控制的根本区别在于控制的不连续性，即一种使系统“结构”随时间变化的开关特性。对飞行器进行单独的滑模控制器设计，需由高阶非线性约束导出姿态角，增加了系统的参数不确定性，控制效果差，甚至造成四旋翼飞行器的不稳定。

发明内容

针对上述现有技术问题，本发明要解决的技术问题是：提供一种基于反演法的四旋翼飞行器滑模变结构控制方法，使得飞行器的控制器对外界干扰具有很强的鲁棒性，性能更加稳定。

为解决上述技术问题，本发明采用如下技术方案：

一种基于反演法的四旋翼飞行器滑模变结构控制方法，其特征在于：给定四旋翼飞行器的期望轨迹，然后由反演控制法计算出需要旋转的滚转角和俯仰角，结合飞行器的三个姿态角，通过滑模控制法得到当前的控制律送入四旋翼飞行器动力学模型中，有效控制飞行器的悬停和轨迹跟踪运动。

上述技术方案中，具体包括如下步骤：

步骤S1：建立四旋翼飞行器动力学模型，确定飞行器四个电机的角速度输入与姿态和位置的关系，以及期望轨迹与姿态角之间的关系：

步骤S2：设计四旋翼飞行器控制器：根据四旋翼飞行器的动力学特性，将步骤S1建立的动力学模型分为全驱动子系统和欠驱动子系统，采用基于反演法的滑模变结构控制方法进行滑模控制器设计；

步骤S3：控制器设计结束，通过滑模控制法得到当前的控制律送入步骤S1中的四旋翼飞行器动力学模型中，产生的状态变量反馈到位置环和姿态环，以此来控制四旋翼飞行器稳定飞行，有效控制飞行器的悬停和轨迹跟踪运动。

上述技术方案中，步骤S1建立四旋翼飞行器动力学模型如式(1)所示：

式中：θ为俯仰角，γ为滚转角，为偏航角，为三个姿态角；J_x、J_y、J_z分别为四旋翼飞行器机体绕机体坐标系三个轴的转动惯量；l为四旋翼飞行器螺旋桨中心至机体坐标系原点的距离；g为重力加速度；m为四旋翼飞行器的质量；x，y，z分别为四旋翼飞行器在导航坐标系中的位置量；ω₁，ω₂，ω₃，ω₄分别为四旋翼飞行器四个电机的输入角速度；公式(1)就是对6个方向的自由度解耦以后的方程；所述6个方向的自由度包括沿x，y，z三个直角坐标轴方向的移动自由度和绕这三个直角坐标轴的转动自由度；

将四旋翼飞行器的飞行状态分为四个独立的飞行通道：上下通道、左右通道、前后通道、偏航通道；定义

$\left\{\begin{array}{c}{U}_1=b({\mathrm{\ω}}_1^2+{\mathrm{\ω}}_2^2+{\mathrm{\ω}}_3^2+{\mathrm{\ω}}_4^2)\\ U_2=b({\mathrm{\ω}}_1^2-{\mathrm{\ω}}_3^2)\\ U_3=b({\mathrm{\ω}}_2^2-{\mathrm{\ω}}_4^2)\\ U_4=d({\mathrm{\ω}}_1^2+{\mathrm{\ω}}_3^2-{\mathrm{\ω}}_2^2-{\mathrm{\ω}}_4^2)\end{array}\right.---\left(2\right)$

式中：b为旋翼的升力系数，d为旋翼的阻力系数；

U₁、U₂、U₃、U₄为由4个螺旋桨的角速度决定的系统控制律：具体为U₁为上下通道的控制律，U₂为前后通道及俯仰角的控制律，U₃为左右通道及横滚角的控制律，U₄为偏航角控制律；将式(2)代入到式(1)得到结合了四个飞行通道的四旋翼飞行器动力学模型，如式(3)所示：

将式(3)表示成状态空间形式：

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

其中，为系统的状态量，U＝[U₁U₂U₃U₄]为系统的控制律，f函数为由当前系统状态量求取下一时刻系统状态量的函数，具体表示为：

则结合式(3)和式(4)，可以得到最终的四旋翼飞行器动力学模型，如式(5)：

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_1=x_2\\ {\stackrel{\·}{x}}_2=\frac{(J_y-J_z)}{J_x}{x}_4{x}_6+\frac{l}{J_x}{U}_2\\ {\stackrel{\·}{x}}_3=x_4\\ {\stackrel{\·}{x}}_4=\frac{(J_z-J_x)}{J_y}{x}_2{x}_6+\frac{l}{J_y}{U}_3\\ {\stackrel{\·}{x}}_5=x_6\\ {\stackrel{\·}{x}}_6=\frac{(J_x-J_y)}{J_z}{x}_2{x}_4+\frac{1}{J_z}{U}_4\\ {\stackrel{\·}{x}}_7=x_8\\ {\stackrel{\·}{x}}_8=\frac{U_1}{m}\left(\cos x_1\cos x_3\right)-g\\ {\stackrel{\·}{x}}_9=x_{10}\\ {\stackrel{\·}{x}}_{10}=\frac{U_1}{m}(\cos x_1\sin x_3\cos x_5+\sin x_1\sin x_5)\\ {\stackrel{\·}{x}}_{11}=x_{12}\\ {\stackrel{\·}{x}}_{12}=\frac{U_1}{m}(\sin x_5\sin x_3\cos x_1-\cos x_5\sin x_1)\end{array}\right.---\left(5\right)$

上述技术方案中，步骤S2四旋翼飞行器控制器设计包括如下步骤：

步骤S21：将步骤S1中所建立的四旋翼飞行器动力学模型分为全驱动通道和欠驱动通道，分别对各自通道进行设计；

首先进行全驱动通道控制器设计：全驱动通道包括上下通道z和偏航角两个通道，上下通道z和偏航角两个通道的动力学方程为：

直接使用滑模变结构控制方法求取控制律，首先计算上下通道z的控制律：

上下通道z的状态变量为：

$\left\{\begin{array}{c}{x}_7=z\\ x_8=\stackrel{\·}{z}\end{array}\right.---\left(7\right)$

定义误差变量：

e₇＝x₇-x_7d

其中，x₇为当前高度；x_7d为期望高度；e₇为当前与期望高度差。

设计滑模函数为：

$s_z=c_7{e}_7+{\stackrel{\·}{e}}_7$

其中，c₇为系统参数；为高度差的导数；s_z为z轴的滑模面函数；

定义通道z的Lyapunov函数为则

${\stackrel{\·}{s}}_z=c_7{\stackrel{\·}{e}}_7+{\stackrel{\·\·}{e}}_7=c_7(x_8-{\stackrel{\·}{x}}_{7d})+({\stackrel{\·\·}{x}}_7-{\stackrel{\·\·}{x}}_{7d})$

且

$s_z{\stackrel{\·}{s}}_z=s_z\left(c_7\right(x_8-{\stackrel{\·}{x}}_{7d})+\frac{U_1}{m}(\cos x_1\cos x_3)-g-{\stackrel{\·\·}{x}}_{7d})$

为了保证设计上下通道z的控制律U₁

$U_1=(-\mathrm{\ϵ}\mathrm{sgn}(s_z)-{\mathrm{ks}}_z+c_7({\stackrel{\·}{x}}_{7d}-x_8)+g+{\stackrel{\·\·}{x}}_{7d})\frac{m}{\cos x_1\cos x_3}---\left(8\right)$

则

$s_z{\stackrel{\·}{s}}_z=-\mathrm{\ϵ}\left|s_z\right|-{\mathrm{ks}}_z^2\≤0$

从而

$\stackrel{\·}{V}\≤0$

式中，ε和k是滑模控制中趋近律的常数；sgn(s_z)为滑模面s_z的符号函数；

使用同样的方法求出偏航角通道的控制律U₄：

其中，为滑模面的符号函数；c₅为系统参数；

步骤S22：对步骤S1中所建立的四旋翼飞行器动力学模型进行欠驱动通道控制器设计：欠驱动通道包括x-γ及y-θ两个通道，其动力学方程先后为：

先通过反演算法由位置信息反演得到姿态信息，在此基础上再定义耦合通道的滑模面；

下面首先设计x-γ通道的控制律：

四旋翼飞行器在x轴上运动时，虽然俯仰角θ对x轴上的运动有影响，但这种影响可以忽略，对俯仰角θ作小角度假设，那么此时对应的x-γ通道的动力学模型为：

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_9=x_{10}\\ x_{10}=\cos x_5\sin x_3\frac{U_1}{m}\\ {\stackrel{\·}{x}}_3=x_4\\ {\stackrel{\·}{x}}_4=\frac{J_z-J_x}{J_y}{x}_2{x}_6+\frac{{\mathrm{lU}}_3}{J_y}\end{array}\right.---\left(12\right)$

定义误差变量：

e₉＝x₉-x_9d(13)

其中，x₉为当前x位置量；x_9d为期望x位置量；e₉为当前与期望x位置量差。

具体设计过程如下：

(1)定义状态变量x的Lyapunov函数为

$V_9=\frac{1}{2}{e}_9^2---\left(14\right)$

则

${\stackrel{\·}{V}}_9=e_9{\stackrel{\·}{e}}_9=e_9(x_{10}-{\stackrel{\·}{x}}_{9d})$

取其中c₉>0，是系统参数；e₁₀为误差变量，也为虚拟控制量，

$e_{10}=x_{10}+c_9{e}_9-{\stackrel{\·}{x}}_{9d}---\left(15\right)$

则

${\stackrel{\·}{V}}_9=-c_9{e}_9^2+e_9{e}_{10}---\left(16\right)$

如果e₁₀＝0，则所以必须消除耦合项，为此，需要进行下一步设计；

(2)定义状态变量的Lyapunov函数

$V_{10}=V_9+\frac{1}{2}{e}_{10}^2---\left(17\right)$

则

${\stackrel{\·}{V}}_{10}={\stackrel{\·}{V}}_9+e_{10}{\stackrel{\·}{e}}_{10}=-c_9{e}_9^2+e_9{e}_{10}+e_{10}{\stackrel{\·}{e}}_{10}$

取其中c₁₀>0，是系统参数；e₃为误差变量，也为虚拟控制量，

$e_3={\stackrel{\·}{e}}_{10}+c_{10}{e}_{10}+e_9---\left(18\right)$

则

$\begin{array}{c}{\stackrel{\·}{V}}_{10}=-c_9{e}_9^2+e_9{e}_{10}+e_{10}(-c_{10}{e}_{10}-e_9+e_3)\\ =-c_9{e}_9^2-c_{10}{e}_{10}^2+e_{10}{e}_3\end{array}---\left(19\right)$

如果e₃＝0，则所以必须消除耦合项，为此，需要进行下一步设计；

(3)定义状态变量γ的Lyapunov函数

$V_3=V_{10}+\frac{1}{2}{e}_3^2---\left(20\right)$

则

${\stackrel{\·}{V}}_3={\stackrel{\·}{V}}_{1\stackrel{\·}{0}}+e_3{\stackrel{\·}{e}}_3=-c_9{e}_9^2-c_{10}{e}_{10}^2+e_{10}{e}_3+e_3{\stackrel{\·}{e}}_3$

取其中c₃>0，是系统参数；e₄为误差变量，也为虚拟控制量，

$e_4=c_3{e}_3+e_{10}+{\stackrel{\·}{e}}_3---\left(21\right)$

则

$\begin{array}{c}{\stackrel{\·}{V}}_3=-c_9{e}_9^2+e_{10}{e}_{10}^2+e_3{e}_{10}+e_3(-c_3{e}_3-e_{10}+e_4)\\ =-c_9{e}_9^2-c_{10}{e}_{10}^2-c_3{e}_3^2+e_4{e}_3\end{array}---\left(22\right)$

此时，中有个耦合项e₄e₃，若将此项消除，便可得出此系统是稳定的；

下一步是设计此通道的滑模面；

(4)上述过程中我们得到x-γ通道的位移速度差e₃和角速度差e₄，由此设计姿态控制器：

由式(18)得

$\begin{array}{c}{e}_3=(1+c_9{c}_{10})(x_9-x_{9d})+(c_9+c_{10})x_{10}\\ -(c_9+c_{10}){\stackrel{\·}{x}}_{9d}+{\stackrel{\·}{x}}_{\mathrm{10}}-{\stackrel{\·}{x}}_{9d}\end{array}---\left(23\right)$

结合滑模变结构定义x轴上的滑模面为

s_x＝e₄(24)

(5)定义状态变量的Lyapunov函数

$V_4=V_3+\frac{1}{2}{s}_x^2---\left(25\right)$

则

${\stackrel{\·}{V}}_4=-c_9{e}_9^2-c_{10}{e}_{10}^2-c_3{e}_3^2+s_x{e}_3+s_x{\stackrel{\·}{s}}_x---\left(26\right)$

为使设计控制器为

$\begin{array}{c}{U}_3=\left(\right(-\mathrm{\ϵ}\mathrm{sgn}\left(s_x\right)-{\mathrm{ks}}_x-(c_9+c_3+c_9{c}_{10}{c}_3)({\stackrel{\·}{x}}_9-{\stackrel{\·}{x}}_{9d})\\ -(2+c_9{c}_{10}+c_9{c}_3+c_3{c}_{10})\cos x_5\sin x_3\frac{U_1}{m}\\ +(c_9+c_{10}+c_3)x_6\sin x_5\sin x_3\frac{U_1}{m}\\ -(c_9+c_{10}+c_3)x_4\cos x_5\cos x_3\frac{U_1}{m}\\ +x_6^2\cos x_5\sin x_3\frac{U_1}{m}+2x_4{x}_6\sin x_5\cos x_3\frac{U_1}{m}\\ +x_4^2\cos x_5\sin x_3\frac{U_1}{m}-(c_9+c_{10}){\stackrel{\·}{x}}_{9d}\\ +(1+c_9{c}_{10})(x_9-x_{9d})+(c_9+c_{10})x_{10}+{\stackrel{\·}{x}}_{10}\\ -{\stackrel{\·\·}{x}}_{9d}+(2+c_9{c}_{10}+c_9{c}_3+c_3{c}_{10}){\stackrel{\·\·}{x}}_{9d}\\ +(c_3+c_9+c_{10}){\stackrel{\·\·\·}{x}}_{9d}+{\stackrel{\·\·\·\·}{x}}_{9d})/\left(\cos x_5\cos x_3\frac{U_1}{m}\right)\\ -\frac{J_z-J_x}{J_y}{x}_2{x}_6)\frac{J_y}{l}\end{array}---\left(27\right)$

其中，ε>0,k>0，是系统参数；sgn(s_x)是滑模面函数s_x的符号函数；则

${\stackrel{\·}{V}}_4=-c_9{e}_9^2-c_{\mathrm{10}}{e}_{10}^2-c_3{e}_3^2-{\mathrm{ks}}_x^2-\mathrm{\ϵ}\left|s_x\right|\≤0---\left(28\right)$

由知，该系统是稳定的；

y-θ通道控制律的设计跟上述方法相同，

$\begin{array}{c}{U}_2=(-(c_{11}+c_1+c_{11}{c}_{12}{c}_1)({\stackrel{\·}{x}}_{11}-{\stackrel{\·}{x}}_{11d})\\ +(2+c_{11}{c}_{12}+c_{11}{c}_1+c_{12}{c}_1)\cos x_5\sin x_1\frac{U_1}{m}\\ -(c_{11}+c_{12}+c_1)x_6\sin x_5\sin x_1\frac{U_1}{m}-(-\mathrm{\ϵ}\mathrm{sgn}(s_y)\\ +(c_{11}+c_{12}+c_1)x_2\cos x_5\cos x_1\frac{U_1}{m}-{\mathrm{ks}}_y\\ -x_6^2\cos x_5\sin x_1\frac{U_1}{m}-2x_4{x}_6\sin x_5\cos x_1\frac{U_1}{m}\\ -x_2^2\cos x_5\sin x_1\frac{U_1}{m}-(c_{11}+c_{12}){\stackrel{\·}{x}}_{11d}\\ +(1+c_{11}{c}_{12})(x_{11}-x_{11d})+(c_{11}+c_{12})x_{12}+{\stackrel{\·}{x}}_{12}\\ -{\stackrel{\·\·}{x}}_{11d}+(2+c_{11}{c}_{12}+c_{11}{c}_1+c_{12}{c}_1){\stackrel{\·\·}{x}}_{11d}\\ +(c_1+c_{11}+c_{12}){\stackrel{\·\·\·}{x}}_{11d}+{\stackrel{\·\·\·\·}{x}}_{11d})/\left(\cos x_5\cos x_1\frac{U_1}{m}\right)\\ -\frac{J_y-J_z}{J_x}{x}_4{x}_6)\frac{J_x}{l}\end{array}---\left(29\right)$

其中，ε>0,k>0，是系统参数；sgn(s_y)是滑模面函数s_y的符号函数；

至此，四个通道的控制律分别求得；

上述所求得控制律中的符号函数sgn(s)使用饱和函数sat(s)来代替，以抑制符号函数项导致的抖振现象，

$sat\left(s\right)=\left\{\begin{array}{cc}\begin{array}{cc}1,& s>\mathrm{\&Delta;}\\ ky,& \left|s\right|\&le;\mathrm{\&Delta;}\\ -1,& s<-\mathrm{\&Delta;}\end{array}& k=\frac{1}{\mathrm{\&Delta;}}\end{array}\right.---\left(30\right)$

式中，Δ称为边界层。在边界层外，采用切换控制，在边界层之内，采用线性化反馈控制；

上述技术方案中，步骤S3具体为：控制器设计结束，将步骤S2中求得的四个控制律U₁、U₂、U₃、U₄代入步骤S1中推导的四旋翼飞行器动力学模型公式(5)，便可有效控制飞行器的悬停和轨迹跟踪运动。

本发明一种基于反演法的四旋翼飞行器滑模变结构控制方法，反演设计方法的基本思想是将复杂的非线性系统分解成不超过系统阶数的子系统，然后为每个子系统分别设计李雅普诺夫函数和中间虚拟控制量，一直后退到整个系统，直到完成整个控制律的设计，而对于四旋翼飞行器，若将反演控制方法与滑模变结构控制方法相结合，则可以扩大反演控制方法的使用范围，使得飞行器的控制器对外界干扰具有很强的鲁棒性。与现有技术相比，其优点是：

(1)本发明将系统模型分为全驱动子系统和欠驱动子系统，分别对其进行控制器设计，针对性较强；

(2)本发明方法采用反演法和滑模变结构控制方法相结合，解耦性强，提高了系统的动态性和稳定性；

(3)本发明所设计的控制器可以通过调节参数来改善系统的性能，能够精确地悬停和跟随指定轨迹。

因此，本发明的一种基于反演法的四旋翼飞行器滑模变结构控制方法可以应用于飞行器领域。

附图说明

图1是本发明四旋翼飞行器控制系统结构框图；

图2是本发明四旋翼飞行器结构模型图；

图3是本发明四旋翼飞行器位置控制曲线图；

图4是本发明四旋翼飞行器姿态控制曲线图；

图5是本发明四旋翼飞行器位置线速度控制曲线图；

图6是本发明四旋翼飞行器姿态角速度控制曲线图；

图7是本发明四旋翼飞行器悬停控制律曲线图；

图8是本发明四旋翼飞行器轨迹跟踪曲线图；

图9是本发明四旋翼飞行器轨迹跟踪误差曲线图。

具体实施方式

下面结合附图和实例对本发明作更进一步的说明。

一种基于反演法的四旋翼飞行器滑模变结构控制方法，如图1所示，控制器采用双闭环控制，分别为内部姿态环和外部位置环，给定四旋翼飞行器的期望轨迹然后由反演控制算法计算出需要旋转的滚转角γ_d和俯仰角θ_d，结合飞行器的三个姿态角，通过滑模控制算法得到当前的控制律送入四旋翼飞行器动力学模型中，产生的状态变量反馈到位置环和姿态环，以此来控制四旋翼飞行器稳定飞行。图2是四旋翼飞行器示意图。

上述技术方案中，具体包括如下步骤：

步骤S1：建立四旋翼飞行器动力学模型，确定飞行器四个电机的角速度输入与姿态和位置的关系，以及期望轨迹与姿态角之间的关系，如式(1)所示：

式中：参数J_x＝0.05887kg·m²，J_y＝0.05887kg·m²，J_z＝0.13151kg·m²，l＝0.3875m，m＝2.467kg,g＝9.81m/s²。

公式(1)就是对四旋翼飞行器6个方向的自由度(包括沿x，y，z三个直角坐标轴方向的移动自由度和绕这三个坐标轴的转动自由度)解耦以后的方程，将四旋翼飞行器的飞行状态分为四个独立的通道：上下通道，左右通道，前后通道，偏航通道，定义

$\left\{\begin{array}{c}{U}_1=b({\mathrm{\&omega;}}_1^2+{\mathrm{\&omega;}}_2^2+{\mathrm{\&omega;}}_3^2+{\mathrm{\&omega;}}_4^2)\\ U_2=b({\mathrm{\&omega;}}_1^2-{\mathrm{\&omega;}}_3^2)\\ U_3=b({\mathrm{\&omega;}}_2^2-{\mathrm{\&omega;}}_4^2)\\ U_4=d({\mathrm{\&omega;}}_1^2+{\mathrm{\&omega;}}_3^2-{\mathrm{\&omega;}}_2^2-{\mathrm{\&omega;}}_4^2)\end{array}\right.---\left(2\right)$

式中：参数b＝2.2893×10^-5N·s²，d＝1.1897×10^-6N·ms²。

U₁、U₂、U₃、U₄为由4个螺旋桨的角速度(ω₁，ω₂，ω₃，ω₄)决定的系统控制律：具体表示U₁为上下通道的控制律，U₂为前后通道及俯仰角的控制律，U₃为左右通道及横滚角的控制律，U₄为偏航角控制律。将式(2)代入到式(1)得到结合了四个飞行通道的四旋翼飞行器动力学模型，如式(3)所示：

将所建立的数学模型(3)表示成状态空间形式：

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

其中，为系统的状态量，U＝[U₁U₂U₃U₄]为系统的控制律，f函数为由当前系统状态量求取下一时刻系统状态量的函数，具体表示为

则结合式(3)和式(4)，可以得到最终的四旋翼飞行器动力学模型，如式(5)：

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_1=x_2\\ {\stackrel{\&CenterDot;}{x}}_2=\frac{(J_y-J_z)}{J_x}{x}_4{x}_6+\frac{l}{J_x}{U}_2\\ {\stackrel{\&CenterDot;}{x}}_3=x_4\\ {\stackrel{\&CenterDot;}{x}}_4=\frac{(J_z-J_x)}{J_y}{x}_2{x}_6+\frac{l}{J_y}{U}_3\\ {\stackrel{\&CenterDot;}{x}}_5=x_6\\ {\stackrel{\&CenterDot;}{x}}_6=\frac{(J_x-J_y)}{J_z}{x}_2{x}_4+\frac{1}{J_z}{U}_4\\ {\stackrel{\&CenterDot;}{x}}_7=x_8\\ {\stackrel{\&CenterDot;}{x}}_8=\frac{U_1}{m}\left(\cos x_1\cos x_3\right)-g\\ {\stackrel{\&CenterDot;}{x}}_9=x_{10}\\ {\stackrel{\&CenterDot;}{x}}_{10}=\frac{U_1}{m}(\cos x_1\sin x_3\cos x_5+\sin x_1\sin x_5)\\ {\stackrel{\&CenterDot;}{x}}_{11}=x_{12}\\ {\stackrel{\&CenterDot;}{x}}_{12}=\frac{U_1}{m}(\sin x_5\sin x_3\cos x_1-\cos x_5\sin x_1)\end{array}\right.---\left(5\right)$

步骤S2：四旋翼飞行器控制器设计。四旋翼飞行器前后方向的移动依靠俯仰时的侧倾角，这两个自由度是耦合在一起的通道，一个动力源的输入产生两个方向的自由度，是一个欠驱动的系统，四轴的不稳定性就在于此，同理，左右通道与滚转角也是耦合通道；上下通道与偏航是两个完全独立的通道，通道之间没有耦合，是一个全驱动的子系统。根据四旋翼飞行器的动力学特性和特点，将步骤S1中建立的四旋翼飞行器动力学模型(公式5)分为全驱动子系统和欠驱动子系统，采用基于反演法的滑模变结构控制方法分别对各自通道进行控制器设计。

步骤S21：全驱动通道控制器设计。全驱动通道包括上下通道z和偏航角两个通道，上下通道z和偏航角两个通道的动力学方程为：

由于全驱动通道之间不存在耦合，可以直接使用滑模变结构控制方法求取控制律，首先计算上下通道z的控制律。

z通道的状态变量为：

$\left\{\begin{array}{c}{x}_7=z\\ x_8=\stackrel{\&CenterDot;}{z}\end{array}\right.---\left(7\right)$

定义误差变量：

e₇＝x₇-x_7d

其中，x₇为当前高度；x_7d为期望高度；e₇为当前与期望高度差。

设计滑模函数为：

$s_z=c_7{e}_7+{\stackrel{\&CenterDot;}{e}}_7$

其中，c₇为系统参数；为高度差的导数；s_z为z轴的滑模面函数。

定义z通道的Lyapunov函数为则

${\stackrel{\&CenterDot;}{s}}_z=c_7{\stackrel{\&CenterDot;}{e}}_7+{\stackrel{\&CenterDot;\&CenterDot;}{e}}_7=c_7(x_8-{\stackrel{\&CenterDot;}{x}}_{7d})+({\stackrel{\&CenterDot;\&CenterDot;}{x}}_7-{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{7d})$

且

$s_z{\stackrel{\&CenterDot;}{s}}_z=s_z\left(c_7\right(x_8-{\stackrel{\&CenterDot;}{x}}_{7d})+\frac{U_1}{m}(\cos x_1\cos x_3)-g-{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{7d})$

为了保证设计滑模控制律为

$U_1=(-\mathrm{\&epsiv;}\mathrm{sgn}(s_z)-{\mathrm{ks}}_z+c_7({\stackrel{\&CenterDot;}{x}}_{7d}-x_8)+g+{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{7d})\frac{m}{\cos x_1\cos x_3}---\left(8\right)$

则

$s_z{\stackrel{\&CenterDot;}{s}}_z=-\mathrm{\&epsiv;}\left|s_z\right|-{\mathrm{ks}}_z^2\&le;0$

从而

$\stackrel{\&CenterDot;}{V}\&le;0$

式中，ε和k是滑模控制中趋近律的常数；sgn(s_z)为滑模面s_z的符号函数；

控制律U₁保证了由此过程得到的系统便是稳定的。

至此，就得到了上下通道z的控制律U₁。

而偏航角通道的控制律U₄可以使用同样的方法求出：

其中，为滑模面的符号函数；c₅为系统参数。

步骤S22：对步骤S1中所建立的四旋翼飞行器动力学模型进行欠驱动通道控制器设计：欠驱动通道包括x-γ及y-θ两个通道，其动力学方程先后为：

对于欠驱动通道控制律的设计，先通过反演算法由位置信息反演得到姿态信息，在此基础上再定义耦合通道的滑模面；

下面首先设计x-γ通道的控制律：

四旋翼飞行器在x轴上运动时，虽然俯仰角θ对x轴上的运动有影响，但这种影响可以忽略，对俯仰角θ作小角度假设，那么此时对应的x-γ通道的动力学模型为：

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_9=x_{10}\\ x_{10}=\cos x_5\sin x_3\frac{U_1}{m}\\ {\stackrel{\&CenterDot;}{x}}_3=x_4\\ {\stackrel{\&CenterDot;}{x}}_4=\frac{J_z-J_x}{J_y}{x}_2{x}_6+\frac{{\mathrm{lU}}_3}{J_y}\end{array}\right.---\left(12\right)$

定义误差变量：

e₉＝x₉-x_9d(13)

其中，x₉为当前x位置量；x_9d为期望x位置量；e₉为当前与期望x位置量差。

具体设计过程如下：

(6)定义状态变量x的Lyapunov函数为

$V_9=\frac{1}{2}{e}_9^2---\left(14\right)$

则

${\stackrel{\&CenterDot;}{V}}_9=e_9{\stackrel{\&CenterDot;}{e}}_9=e_9(x_{10}-{\stackrel{\&CenterDot;}{x}}_{9d})$

取其中c₉>0，是系统参数；e₁₀为误差变量，也为虚拟控制量，

$e_{10}=x_{10}+c_9{e}_9-{\stackrel{\&CenterDot;}{x}}_{9d}---\left(15\right)$

则

${\stackrel{\&CenterDot;}{V}}_9=-c_9{e}_9^2+e_9{e}_{10}---\left(16\right)$

如果e₁₀＝0,则所以必须消除耦合项，为此，需要进行下一步设计。

(7)定义状态变量的Lyapunov函数

$V_{10}=V_9+\frac{1}{2}{e}_{10}^2---\left(17\right)$

则

${\stackrel{\&CenterDot;}{V}}_{10}={\stackrel{\&CenterDot;}{V}}_9+e_{10}{\stackrel{\&CenterDot;}{e}}_{10}=-c_9{e}_9^2+e_9{e}_{10}+e_{10}{\stackrel{\&CenterDot;}{e}}_{10}$

取其中c₁₀>0，是系统参数；e₃为误差变量，也为虚拟控制量，

$e_3={\stackrel{\&CenterDot;}{e}}_{10}+c_{10}{e}_{10}+e_9---\left(18\right)$

则

$\begin{array}{c}{\stackrel{\&CenterDot;}{V}}_{10}=-c_9{e}_9^2+e_9{e}_{10}+e_{10}(-c_{10}{e}_{10}-e_9+e_3)\\ =-c_9{e}_9^2-c_{10}{e}_{10}^2+e_{10}{e}_3\end{array}---\left(19\right)$

如果e₃＝0,则所以必须消除耦合项，为此，需要进行下一步设计。

(8)定义Lyapunov函数

$V_3=V_{10}+\frac{1}{2}{e}_3^2---\left(20\right)$

则

${\stackrel{\&CenterDot;}{V}}_3={\stackrel{\&CenterDot;}{V}}_{1\stackrel{\&CenterDot;}{0}}+e_3{\stackrel{\&CenterDot;}{e}}_3=-c_9{e}_9^2-c_{10}{e}_{10}^2+e_{10}{e}_3+e_3{\stackrel{\&CenterDot;}{e}}_3$

取其中c₃>0，是系统参数；e₄为误差变量，也为虚拟控制量，

$e_4=c_3{e}_3+e_{10}+{\stackrel{\&CenterDot;}{e}}_3---\left(21\right)$

则

$\begin{array}{c}{\stackrel{\&CenterDot;}{V}}_3=-c_9{e}_9^2-c_{10}{e}_{10}^2+e_3{e}_{10}+e_3(-c_3{e}_3-e_{10}+e_4)\\ =-c_9{e}_9^2-c_{10}{e}_{10}^2-e_3{e}_3^2+e_4{e}_3\end{array}---\left(22\right)$

此时，中有个耦合项e₄e₃，若将此项消除，便可得出此系统是稳定的。下一步是设计此通道的滑模面。

(9)上述过程中我们得到x-γ通道的位移速度差e₃和角速度差e₄，由此设计姿态控制器。

由式(18)得

$\begin{array}{c}{e}_3=(1+c_9{c}_{10})(x_9-x_{9d})+(c_9+c_{10})x_{10}\\ -(c_9+c_{10}){\stackrel{\&CenterDot;}{x}}_{9d}+{\stackrel{\&CenterDot;}{x}}_{\mathrm{10}}-{\stackrel{\&CenterDot;}{x}}_{9d}\end{array}---\left(23\right)$

结合滑模变结构定义x轴上的滑模面为

s_x＝e₄(24)

(10)定义状态变量的Lyapunov函数

$V_4=V_3+\frac{1}{2}{s}_x^2---\left(25\right)$

则

${\stackrel{\&CenterDot;}{V}}_4=-c_9{e}_9^2-c_{10}{e}_{10}^2-c_3{e}_3^2+s_x{e}_3+s_x{\stackrel{\&CenterDot;}{s}}_x---\left(26\right)$

为使设计控制器为

$\begin{array}{c}{U}_3=\left(\right(-\mathrm{\&epsiv;}\mathrm{sgn}\left(s_x\right)-{\mathrm{ks}}_x-(c_9+c_3+c_9{c}_{10}{c}_3)({\stackrel{\&CenterDot;}{x}}_9-{\stackrel{\&CenterDot;}{x}}_{9d})\\ -(2+c_9{c}_{10}+c_9{c}_3+c_3{c}_{10})\cos x_5\sin x_3\frac{U_1}{m}\\ +(c_9+c_{10}+c_3)x_6\sin x_5\sin x_3\frac{U_1}{m}\\ -(c_9+c_{10}+c_3)x_4\cos x_5\cos x_3\frac{U_1}{m}\\ +x_6^2\cos x_5\sin x_3\frac{U_1}{m}+2x_4{x}_6\sin x_5\cos x_3\frac{U_1}{m}\\ +x_4^2\cos x_5\sin x_3\frac{U_1}{m}-(c_9+c_{10}){\stackrel{\&CenterDot;}{x}}_{9d}\\ +(1+c_9{c}_{10})(x_9-x_{9d})+(c_9+c_{10})x_{10}+{\stackrel{\&CenterDot;}{x}}_{10}\\ -{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{9d}+(2+c_9{c}_{10}+c_9{c}_3+c_3{c}_{10}){\stackrel{\&CenterDot;\&CenterDot;}{x}}_{9d}\\ +(c_3+c_9+c_{10}){\stackrel{\&CenterDot;\&CenterDot;\&CenterDot;}{x}}_{9d}+{\stackrel{\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;}{x}}_{9d})/\left(\cos x_5\cos x_3\frac{U_1}{m}\right)\\ -\frac{J_z-J_x}{J_y}{x}_2{x}_6)\frac{J_y}{l}\end{array}---\left(27\right)$

其中，ε>0,k>0，是系统参数；sgn(s_x)是滑模面函数s_x的符号函数；则

${\stackrel{\&CenterDot;}{V}}_4=-c_9{e}_9^2-c_{10}{e}_{10}^2-c_3{e}_3^2-{\mathrm{ks}}_x^2-\mathrm{\&epsiv;}\left|s_x\right|\&le;0---\left(28\right)$

由知，该系统是稳定的。

y-θ通道控制律的设计跟上述方法相同，

$\begin{array}{c}{U}_2=(-(c_{11}+c_1+c_{11}{c}_{12}{c}_1)({\stackrel{\&CenterDot;}{x}}_{11}-{\stackrel{\&CenterDot;}{x}}_{11d}))\\ +(2+c_{11}{c}_{12}+c_{11}{c}_1+c_{12}{c}_1)\cos x_5\sin x_1\frac{U_1}{m}\\ -(c_{11}+c_{12}+c_1)x_6\sin x_5\sin x_1\frac{U_1}{m}-(-\mathrm{\&epsiv;}\mathrm{sgn}(s_y))\\ +(c_{11}+c_{12}+c_1)x_2\cos x_5\cos x_1\frac{U_1}{m}-{\mathrm{ks}}_y\\ -x_6^2\cos x_5\sin x_1\frac{U_1}{m}-2x_4{x}_6\sin x_5\cos x_1\frac{U_1}{m}\\ -x_2^2\cos x_5\sin x_1\frac{U_1}{m}-(c_{11}+c_{12}){\stackrel{\&CenterDot;}{x}}_{11d}\\ +(1+c_{11}{c}_{12})(x_{11}-x_{11d})+(c_{11}+c_{12})x_{12}+{\stackrel{\&CenterDot;}{x}}_{12}\\ -{\stackrel{\&CenterDot;\&CenterDot;}{x}}_{11d}+(2+c_{11}{c}_{12}+c_{11}{c}_1+c_{12}{c}_1){\stackrel{\&CenterDot;\&CenterDot;}{x}}_{11d}\\ +(c_1+c_{11}+c_{12}){\stackrel{\&CenterDot;\&CenterDot;\&CenterDot;}{x}}_{11d}+{\stackrel{\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;}{x}}_{11d})/\left(\cos x_5\cos x_1\frac{U_1}{m}\right)\\ -\frac{J_y-J_z}{J_x}{x}_4{x}_6)\frac{J_x}{l}\end{array}---\left(29\right)$

其中，ε>0,k>0，是系统参数；sgn(s_y)是滑模面函数s_y的符号函数。

至此，四个通道的控制律分别求得。

上述所求得控制律中的符号函数sgn(s)使用饱和函数sat(s)来代替，以抑制符号函数项导致的抖振现象，

$sat\left(s\right)=\left\{\begin{array}{cc}\begin{array}{cc}1,& s>\mathrm{\&Delta;}\\ ky,& \left|s\right|\&le;\mathrm{\&Delta;}\\ -1,& s<-\mathrm{\&Delta;}\end{array}& k=\frac{1}{\mathrm{\&Delta;}}\end{array}\right.---\left(30\right)$

式中，Δ称为边界层。在边界层外，采用切换控制，在边界层之内，采用线性化反馈控制。

步骤S3：控制器设计结束，将步骤S2中求得的四个控制律U₁、U₂、U₃、U₄代入步骤S1中推导的四旋翼飞行器动力学模型(公式5)，便可有效控制飞行器的悬停和轨迹跟踪运动

针对基于反演法的四旋翼飞行器滑模变结构控制方法进行了仿真验证，首先设置飞行器悬停在一个点上，如：并假设其初始状态为：同时设置滑模控制器的相关参数如下：c₁＝c₃＝c₇＝c₉＝c₁₀＝c₁₁＝c₁₂＝1.8，c₅＝3.2，ε＝1.4，k＝6.2。

图3-图6为四旋翼飞行器的位置环和姿态环曲线图，曲线表明飞行器能够在4s之内很快达到目标位置并保持稳定状态，图7为四旋翼飞行器悬停控制律曲线图，曲线表明飞行器定点悬停稳定状态时U₂,U₃,U₄＝0，而上下通道的控制律U₁则有数值输出，这个数值需要抵消飞行器悬停时自身的重量。

再者设置飞行器x轴和y轴上分别跟踪正弦曲线运动，而z轴目标高度为2m，图8和图9分别为飞行器轨迹跟踪曲线图和轨迹跟踪误差曲线图，结果显示其能够很好地跟踪期望轨迹运动并且误差较小，最大不超过5cm，控制效果良好。

综上所述，本发明所提出的控制方法分别对子系统进行控制器设计，针对性较强，通过实时调节滑模控制器的相关参数，使其在悬停和目标轨迹跟踪上进行精确地控制，动态响应和稳态特性得到很大的提高，具有很高的工程实用价值。

本发明保护范围不仅局限于以上所述的较佳实施方式，凡在与本发明相同原理下的变更或润饰均应包含在本发明的保护范围之内。

Claims (5)

1.一种基于反演法的四旋翼飞行器滑模变结构控制方法，其特征在于：给定四旋翼飞行器的期望轨迹，然后由反演控制法计算出需要旋转的滚转角和俯仰角，结合飞行器的三个姿态角，通过滑模控制法得到当前的控制律送入四旋翼飞行器动力学模型中，有效控制飞行器的悬停和轨迹跟踪运动。

2.根据权利要求1所述的基于反演法的四旋翼飞行器滑模变结构控制方法，其特征在于：具体包括如下步骤：

步骤S1：建立四旋翼飞行器动力学模型，确定飞行器四个电机的角速度输入与姿态和位置的关系，以及期望轨迹与姿态角之间的关系：

步骤S2：设计四旋翼飞行器控制器：根据四旋翼飞行器的动力学特性，将步骤S1建立的动力学模型分为全驱动子系统和欠驱动子系统，采用基于反演法的滑模变结构控制方法进行滑模控制器设计；

步骤S3：控制器设计结束，通过滑模控制法得到当前的控制律送入步骤S1中的四旋翼飞行器动力学模型中，产生的状态变量反馈到位置环和姿态环，以此来控制四旋翼飞行器稳定飞行，有效控制飞行器的悬停和轨迹跟踪运动。

3.根据权利要求2所述的基于反演法的四旋翼飞行器滑模变结构控制方法，其特征在于：步骤S1建立四旋翼飞行器动力学模型如式(1)所示：

式中：θ为俯仰角，γ为滚转角，为偏航角，为三个姿态角；J_x、J_y、J_z分别为四旋翼飞行器机体绕机体坐标系三个轴的转动惯量；l为四旋翼飞行器螺旋桨中心至机体坐标系原点的距离；g为重力加速度；m为四旋翼飞行器的质量；x，y，z分别为四旋翼飞行器在导航坐标系中的位置量；ω₁，ω₂，ω₃，ω₄分别为四旋翼飞行器四个电机的输入角速度；公式(1)就是对6个方向的自由度解耦以后的方程；所述6个方向的自由度包括沿x，y，z三个直角坐标轴方向的移动自由度和绕这三个直角坐标轴的转动自由度；

将四旋翼飞行器的飞行状态分为四个独立的飞行通道：上下通道、左右通道、前后通道、偏航通道；定义

式中：b为旋翼的升力系数，d为旋翼的阻力系数；

U₁、U₂、U₃、U₄为由4个螺旋桨的角速度决定的系统控制律：具体为U₁为上下通道的控制律，U₂为前后通道及俯仰角的控制律，U₃为左右通道及横滚角的控制律，U₄为偏航角控制律；将式(2)代入到式(1)得到结合了四个飞行通道的四旋翼飞行器动力学模型，如式(3)所示：

将式(3)表示成状态空间形式：

其中，为系统的状态量，U＝[U₁U₂U₃U₄]为系统的控制律，f函数为由当前系统状态量求取下一时刻系统状态量的函数，具体表示为：

则结合式(3)和式(4)，可以得到最终的四旋翼飞行器动力学模型，如式(5)：

4.根据权利要求3所述的基于反演法的四旋翼飞行器滑模变结构控制方法，其特征在于：步骤S2四旋翼飞行器控制器设计包括如下步骤：

步骤S21：将步骤S1中所建立的四旋翼飞行器动力学模型分为全驱动通道和欠驱动通道，分别对各自通道进行设计；

首先进行全驱动通道控制器设计：全驱动通道包括上下通道z和偏航角两个通道，上下通道z和偏航角两个通道的动力学方程为：

直接使用滑模变结构控制方法求取控制律，首先计算上下通道z的控制律：

上下通道z的状态变量为：

定义误差变量：

e₇＝x₇-x_7d

其中，x₇为当前高度；x_7d为期望高度；e₇为当前与期望高度差。

设计滑模函数为：

其中，c₇为系统参数；为高度差的导数；s_z为z轴的滑模面函数；

定义通道z的Lyapunov函数为则

且

为了保证设计上下通道z的控制律U₁

则

从而

式中，ε和k是滑模控制中趋近律的常数；sgn(s_z)为滑模面s_z的符号函数；

使用同样的方法求出偏航角通道的控制律U₄：

其中，为滑模面的符号函数；c₅为系统参数；

步骤S22：对步骤S1中所建立的四旋翼飞行器动力学模型进行欠驱动通道控制器设计：欠驱动通道包括x-γ及y-θ两个通道，其动力学方程先后为：

先通过反演算法由位置信息反演得到姿态信息，在此基础上再定义耦合通道的滑模面；

下面首先设计x-γ通道的控制律：

四旋翼飞行器在x轴上运动时，虽然俯仰角θ对x轴上的运动有影响，但这种影响可以忽略，对俯仰角θ作小角度假设，那么此时对应的x-γ通道的动力学模型为：

定义误差变量：

e₉＝x₉-x_9d(13)

其中，x₉为当前x位置量；x_9d为期望x位置量；e₉为当前与期望x位置量差。

具体设计过程如下：

(1)定义状态变量x的Lyapunov函数为

则

取其中c₉>0，是系统参数；e₁₀为误差变量，也为虚拟控制量，

则

如果e₁₀＝0，则所以必须消除耦合项，为此，需要进行下一步设计；

(2)定义状态变量的Lyapunov函数

则

取其中c₁₀>0，是系统参数；e₃为误差变量，也为虚拟控制量，

则

如果e₃＝0，则所以必须消除耦合项，为此，需要进行下一步设计；

(3)定义状态变量γ的Lyapunov函数

则

取其中c₃>0，是系统参数；e₄为误差变量，也为虚拟控制量，

则

此时，中有个耦合项e₄e₃，若将此项消除，便可得出此系统是稳定的；

下一步是设计此通道的滑模面；

(4)上述过程中我们得到x-γ通道的位移速度差e₃和角速度差e₄，由此设计

姿态控制器：

由式(18)得

结合滑模变结构定义x轴上的滑模面为

s_x＝e₄(24)

(5)定义状态变量的Lyapunov函数

则

为使设计控制器为

其中，ε>0,k>0，是系统参数；sgn(s_x)是滑模面函数s_x的符号函数；则

由知，该系统是稳定的；

y-θ通道控制律的设计跟上述方法相同，

其中，ε>0,k>0，是系统参数；sgn(s_y)是滑模面函数s_y的符号函数；

至此，四个通道的控制律分别求得；

上述所求得控制律中的符号函数sgn(s)使用饱和函数sat(s)来代替，以抑制符号函数项导致的抖振现象，

式中，Δ称为边界层。在边界层外，采用切换控制，在边界层之内，采用线性化反馈控制。

5.根据权利要求4所述的基于反演法的四旋翼飞行器滑模变结构控制方法，其特征在于：步骤S3具体为：控制器设计结束，将步骤S2中求得的四个控制律U₁、U₂、U₃、U₄代入步骤S1中的四旋翼飞行器动力学模型公式(5)，便可有效控制飞行器的悬停和轨迹跟踪运动。