CN104536457A - 基于小型无人机导航的滑模控制方法 - Google Patents

Abstract

本发明提供一种基于小型无人机导航的滑模控制方法，包括(1)确定滑模面s(x)：滑模面代表系统的理想动态特性，选择的滑模面为非线性滑模面；(2)滑模控制器设计：使到达条件得到满足，从而使趋近运动于有限时间到达滑模面，并且在趋近的过程中快速、抖振小。本发明针对滑模控制中出现的抖振问题，重新进行了控制律设计，并对制导参数进行了优化；并分析了无人机的运动学和动力学特性，提出了一种基于滑模控制理论的非线性导航算法。实际测试结果表明，本发明给出的导航控制算法具有良好的性能指标，可跟踪任意航路点，从而可实现无人机自主飞行。

Description

基于小型无人机导航的滑模控制方法

技术领域

本发明涉及小型无人机导航的滑模控制，具体涉及一种基于小型无人机导航的滑模控制方法。

背景技术

小型无人机以其独特的外形、结构及广阔的应用前景，成为当前的研究热点。导航控制是无人飞行器研究的关键问题之一，其关键性能指标之一是在面对系统的不确定性及环境干扰情况下保证对飞行轨迹控制的精确性。因此，设计一个简单、高效、稳健的导航算法，可以降低系统的复杂度，增加系统的稳定性，提高飞行轨迹控制的精确性，实现获得更满意的控制效果，显得尤为重要。

发明内容

针对上述问题，本发明的目的是在滑模控制(Sliding Mode Control，SMC)理论的基础上提出了一种非线性的导航控制方法。

实现上述目的，本发明采用如下技术方案：一种基于小型无人机导航的滑模控制方法，包括如下步骤：

(1)飞行前，根据飞机类型，查询手册，得到飞机的最大滚转角φ_max以及飞机的最大巡航速度V_max；

(2)确定滑模面s(x)：滑模面代表系统的理想动态特性，设计一个非线性滑模面，如图4所示。飞机以最大速度V_max飞行时，最小转弯半径Y＝2R_min，参数  $\mathrm{\λ}=\frac{\mathrm{\π}}{2Y};$

(3)滑模控制器设计：设计滑模控制器，使到达条件得到满足，从而使趋近运动于有限时间到达滑模面，并且在趋近的过程中快速、抖振小；

根据选择的滑模面，对任意的侧向偏离距离y均有一个固定的期望ψ_E值，根据无人机的当前航迹方位角ψ_G，计算出当前的航迹误差角ψ_CE与期望的ψ_E角的误差ε_E；为了产生一个平滑的导航输出φ_r，根据当前的ε_E来设计控制律；

根据选择的滑模面s计算当前的ε_E，具体如下式所示：

${\mathrm{\&epsiv;}}_E=\left\{\begin{array}{cc}{\mathrm{\&psi;}}_{\mathrm{CE}}-\frac{\mathrm{\&pi;}}{2}& (y>Y)\\ {\mathrm{\&psi;}}_{\mathrm{CE}}+\mathrm{\&lambda;y}& (-Y<y<Y)\\ {\mathrm{\&psi;}}_{\mathrm{CE}}+\frac{\mathrm{\&pi;}}{2}& (y<-Y)\end{array}\right.---\left(13\right)$

式中ψ_CE＝ψ_G-ψ_T，-π≤ψ_CE≤π，当前航迹方位角ψ_G指向目标航线右侧，ψ_CE为正；

当无人机以滚转角φ转弯时，根据角速度及式(2)可得：

$\mathrm{\&omega;}=\frac{g\tan \mathrm{\&phi;}}{V}---\left(14\right)$

由ωT＝-ψ_E可得：

$\mathrm{\&phi;}=-\arctan \left(\frac{V{\mathrm{\&psi;}}_E}{\mathrm{gT}}\right)---\left(15\right)$

式中T为期望的响应时间，单位为秒，根据系统的响应速度进行设置；T越大，响应越快，可能会发生抖振现象；T越小，系统越稳定，但响应很慢；

当无人机的侧向偏离距离值|y|在期望的控制精度ε内时，导航算法的控制目标变为保持ψ_CE＝0，总的控制律如下：

$\mathrm{\&phi;}=\left\{\begin{array}{cc}-\arctan \left(\frac{V{\mathrm{\&psi;}}_E}{\mathrm{gT}}\right)& \left|y\right|>\mathrm{\&epsiv;}\\ -\arctan \left(\frac{\mathrm{\&lambda;}{V}^2\sin \left({\mathrm{\&psi;}}_{\mathrm{CE}}\right)}{g}\right)& \left|y\right|\&le;\mathrm{\&epsiv;}\end{array}\right.---\left(16\right)$

无人机机身倾斜时，升力竖直方向的分力平衡自身重力，故滚转角|φ|会有一个上限值φ_max，故计算时应限定|φ|≤φ_max；

在实际飞行中，飞机的俯仰角θ不一定为0，故最后的导航输出制导量为φ_r＝φcosθ；

(4)无人机控制；滑模控制器最终输出的制导量φ_r为图3中需要控制的滚转角φ，将其送给无人机的控制驱动系统，最终控制无人机的舵机，将飞机的实际滚转角φ控制到滑模控制器输出的φ_r角。

在飞行过程中，循环执行(3)、(4)过程，即可将无人机从一个目标航点引导控制到下一个目标航点。

相比现有技术，本发明具有如下有益效果：

本发明针对小型无人机导航控制系统成本低、计算能力有限的特点，提出了一种简单、 有效的基于滑模控制理论的非线性导航控制方法，解决了无人机两点间的平面导航问题。飞行测试结果表明，该方法合理、有效、稳定性好，可将侧向偏离误差控制在3米内，可以满足民用小型无人机的应用要求。

本发明在导航控制系统设计时，采用将导航与控制的设计问题分解成外环的导航设计与内环的控制设计的方法，首先选取一个非线性的滑模面，然后运用几何学和运动学推导出总的控制律，最后针对具体需求重新进行了控制律的设计。在自主研发的飞控系统上的飞行测试结果表明，沿滑模面运动时，对于任意的侧向偏离距离，该系统保持稳定，抖振现象小，且稳态侧向偏离误差在3米内，呈现出良好的航迹跟踪性能。

附图说明

图1本发明滑模控制流程图；

图2无人机的系统导航参数表示示意图；

图3无人机协调转弯时竖直方向的受力示意图；

图4无人机非线性滑模面示意图；

图5本发明测试飞机及自驾仪主板照片；

图6大侧向偏离距离测试的实测轨迹图；

图7侧向偏离距离与航迹误差角的实时变化图；

图8无人机进入目标航线后的侧向偏离距离变化图；

图9无人机连续航点导航测试飞行轨迹图；

图10连续航点导航测试侧向偏离距离变化图；

图11实际滚转角(roll)对参考滚转角(nav_roll)的跟随变化图；

图12自动导航过程中的高度变化图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步详细说明。

一种民用小型无人机的飞行控制器，包括飞行控制微处理器、GPS模块、舵机驱动模块、传感器、数据存储模块、电源以及地面测控模块；形成一个资源统一调度分配、各部分相互协同运作的数字控制系统；其中，

所述飞行控制微处理器是飞行控制系统的核心，担负传感器数据的采集、姿态解算、控制律解算、数据存储和飞行任务管理任务，选用STM32F103RE；采用体积小的LQFP封装芯片；

传感器包括微陀螺仪/加速度计、三轴数字罗盘和高度传感器，分别用于测量无人机角速率、加速度、磁场矢量信息和飞机高度信息；采集到的信息通过各自的通信接口被传送给微处理器进行滤波、姿态解算；所述GPS模块采用u-blox公司型号为LEA-6H的GPS接收模块。

所述舵机驱动模块采用STM32F103RE微处理器，舵机驱动模块产生6路PWM信号，分别对油门、横滚、偏航、俯仰进行控制，以及预留两路PWM信号作开伞信号或其他用途；在微处理器与舵机之间加入主要由快速光电隔离器HCPL2530组成的隔离电路。

所述数据存储模块主要由EEPROM和TF卡存储两部分组成；EEPROM主要用来存储系统初始化时所需的参数信息和航点信息，采用型号为AT24C512的EEPROM器件。

无人机滑模控制方法以机载传感器信息得到的姿态信息为基础，将导航与控制的设计问题分解成外环的导航设计与内环的控制设计的方法，利用滑模控制方法进行导航控制，首先选取一个非线性的滑模面，然后运用几何学和运动学推导出总的控制律，并针对具体应用进行控制律设计，实行无人机自主导航。

参见图1，本发明基于小型无人机导航的滑模控制方法，包括如下步骤：

(1)飞行前，根据飞机类型，查询手册，得到飞机的最大滚转角φ_max以及飞机的最大巡航速度V_max；

(2)确定滑模面s(x)：滑模面代表系统的理想动态特性，设计一个非线性滑模面，如图4所示。飞机以最大速度V_max飞行时，最小转弯半径Y＝2R_min，参数  $\mathrm{\&lambda;}=\frac{\mathrm{\&pi;}}{2Y};$

(3)滑模控制器设计：设计滑模控制器，使到达条件得到满足，从而使趋近运动于有限时间到达滑模面，并且在趋近的过程中快速、抖振小；

根据选择的滑模面，对任意的侧向偏离距离y均有一个固定的期望ψ_E值，根据无人机的当前航迹方位角ψ_G，计算出当前的航迹误差角ψ_CE与期望的ψ_E角的误差ε_E；为了产生一个平滑的导航输出φ_r，根据当前的ε_E来设计控制律；

根据选择的滑模面s计算当前的ε_E，具体如下式所示：

${\mathrm{\&epsiv;}}_E=\left\{\begin{array}{cc}{\mathrm{\&psi;}}_{\mathrm{CE}}-\frac{\mathrm{\&pi;}}{2}& (y>Y)\\ {\mathrm{\&psi;}}_{\mathrm{CE}}+\mathrm{\&lambda;y}& (-Y<y<Y)\\ {\mathrm{\&psi;}}_{\mathrm{CE}}+\frac{\mathrm{\&pi;}}{2}& (y<-Y)\end{array}\right.---\left(13\right)$

式中ψ_CE＝ψ_G-ψ_T，-π≤ψ_CE≤π，当前航迹方位角ψ_G指向目标航线右侧，ψ_CE为正；

当无人机以滚转角φ转弯时，根据角速度及式(2)可得：

$\mathrm{\&omega;}=\frac{g\tan \mathrm{\&phi;}}{V}---\left(14\right)$

由ωT＝-ψ_E可得：

$\mathrm{\&phi;}=-\arctan \left(\frac{V{\mathrm{\&psi;}}_E}{\mathrm{gT}}\right)---\left(15\right)$

式中T为期望的响应时间，单位为秒，根据系统的响应速度进行设置；T越大，响应越快，可能会发生抖振现象；T越小，系统越稳定，但响应很慢；

当无人机的侧向偏离距离值|y|在期望的控制精度ε内时，导航算法的控制目标变为保持ψ_CE＝0，总的控制律如下：

$\mathrm{\&phi;}=\left\{\begin{array}{cc}-\arctan \left(\frac{V{\mathrm{\&psi;}}_E}{\mathrm{gT}}\right)& \left|y\right|>\mathrm{\&epsiv;}\\ -\arctan \left(\frac{\mathrm{\&lambda;}{V}^2\sin \left({\mathrm{\&psi;}}_{\mathrm{CE}}\right)}{g}\right)& \left|y\right|\&le;\mathrm{\&epsiv;}\end{array}\right.---\left(16\right)$

无人机机身倾斜时，升力竖直方向的分力平衡自身重力，故滚转角|φ|会有一个上限值φ_max，故计算时应限定|φ|≤φ_max；

在实际飞行中，飞机的俯仰角θ不一定为0，故最后的导航输出制导量为φ_r＝φcosθ；

(4)无人机控制；滑模控制器最终输出的制导量φ_r为图3中需要控制的滚转角φ，将其送给无人机的控制驱动系统，最终控制无人机的舵机，将飞机的实际滚转角φ控制到滑模控制器输出的φ_r角。

在飞行过程中，循环执行(3)、(4)过程，即可将无人机从一个目标航点引导控制到下一个目标航点。

一、相关工作原理：

无人机需搭载各种载荷实现自主飞行。所谓自主飞行，是指无人机在其飞行过程中没有人为直接的控制决策，由飞机本身的控制系统和导航系统稳定与控制飞机并引导飞机沿一定航线从一处飞到另一处。为了使飞机能在三维空间按预定航线飞行，可以把导航系统分为平面和高度导航两部分，针对小型无人机低空及距离飞行范围小的特点，可以采用航线空间投影的方法实现平面和高度导航的分离。这样平面导航就可以以投影航线作为理论航线来进行平面导航的计算。本发明的导航控制方法是针对解决平面导航问题提出的。相 对其它无人机而言，固定翼无人机具有飞行效率高、飞行速度快、飞行距离远、有效载荷大、成本与使用费用低等优点，在无人机应用中获得的应用比例最大。本发明所述无人机特指固定翼无人机。

平面导航所需的最基本导航参数是载体的即时位置、速度和航向。对于无人机，导航算法利用给定飞行计划和系统导航参数可以计算出飞机导航系统的制导参数。本发明使用的无人机的系统导航参数如图2所示，WP1和WP2为航线上的两个航点，航迹方位角ψ_G为地速V与正北方向的夹角。目标航线角ψ_T为WP1-WP2航线与正北方向的夹角，范围为0～360°。y为侧向偏离距离，飞机在航线右侧为正，左侧为负。航迹误差角ψ_E＝ψ_G-ψ_T，范围为-π～π，地速指向目标航线右侧为正。导航算法的任务是保持侧向偏离距离y尽可能小，并且保证当y≈0时ψ_E≈0。当y不为零时，无人机均采用的倾斜转弯方式(bank-to-turn,BTT)来进行侧向偏离的修正与控制。当侧向偏离距离较大时，为了将无人机尽快地引导到目标航行上，最理想的办法是引导无人机朝目标航线垂直飞行，即 或当侧向偏离距离减少时，ψ_E应当相应减小。

由于采用倾斜转弯来控制侧向偏离距离的方案，导航算法输出的制导参数为飞机的滚转角φ，如图3中所示。当飞机以滚转角φ进行协调转弯飞行时，飞机在水平和垂直方向的受力分析如图3所示，可得到如下平衡方程：

$\left.\begin{array}{c}L\sin \mathrm{\&phi;}=m\frac{V^2}{R}\\ L\cos \mathrm{\&phi;}=\mathrm{mg}\end{array}\right\}---\left(1\right)$

式(1)中L为升力，m为质量，g为重力加速度，V为地速，R为飞机做圆周运动的半径。求解式(1)可得：

$\tan \mathrm{\&phi;}=\frac{V^2}{\mathrm{gR}}---\left(2\right)$

飞机进行协调转弯时有ω为飞机做圆周运动的角速度，为ψ_G的变化率式(2)可表示为：

$\tan \mathrm{\&phi;}=\frac{V{\stackrel{\&CenterDot;}{\mathrm{\&psi;}}}_G}{g}---\left(3\right)$

由ψ_E＝ψ_G-ψ_T有在实际任务飞行中，目标航线角ψ_T变化较慢，故其变化率可以忽略，即式(3)可变为：

$\tan \mathrm{\&phi;}=\frac{V{\stackrel{\&CenterDot;}{\mathrm{\&psi;}}}_E}{g}---\left(4\right)$

由于系统的控制级循环比导航级循环快5-10倍，可以认为导航输出的参考滚转角φ_r≈φ，可将φ_r和φ认为是同一变量，因此式(4)可近似为：

$\tan {\mathrm{\&phi;}}_r=\frac{V{\stackrel{\&CenterDot;}{\mathrm{\&psi;}}}_E}{g}---\left(5\right)$

φ_r为导航输出的制导量。由式(5)可以看出，地速V的测量对导航算法十分关键，通常由高精度GPS模块测量，现在GPS的更新率可达5Hz，可以满足要求。要得到目标制导量，还需获得ψ_E的变化率。此外，在侧向偏离距离变化过程中，为了保证系统的稳定性，需要一个平滑的制导量φ_r。本发明提出的导航算法针对的是两点之间的平面导航，目标是对任意的侧向偏离距离y产生一个平滑的制导量φ_r，同时控制侧向偏离距离在精度范围内。

二、基于非线性滑模面的导航控制方法(算法)：

2.1滑模控制理论 

滑模控制(Sliding Mode Control,SMC)是一种特殊的非线性控制方法，其优点是能够克服系统的不确定性，对干扰和未建模动态具有很强的鲁棒性。该方法尤其对非线性系统的控制具有良好的控制效果，现已广泛应用于机器人控制领域。由于无人机系统是一个典型的非线性系统，本发明提出了一个基于非线性滑模面的导航算法，并进行了控制律设计，以得到满意的控制效果。

滑模控制系统设计分为两步：

(1)确定滑模面s(x)：滑模面代表了系统的理想动态特性，确定的滑模面所确定的滑动模态渐近稳定且有良好的品质。通常选择的滑模面为线性滑模面；

(2)滑模控制器设计：设计滑模控制器，使到达条件得到满足，从而使趋近运动于有限时间到达滑模面，并且在趋近的过程中快速、抖振小。

滑模控制量由等效控制量和开关控制量构成。等效控制量用于保证开关控制量用于保证系统在任意状态都能到达滑模面，但同时也会造成系统沿滑模面运动时产生抖振。

2.2基于非线性滑模面的导航算法

在无人机导航控制中，需解决以下关键问题：1)当无人机侧向偏离距离较大时，需要一个较大的ψ_E角，快速切近航线；2)当侧向偏离距离减小到一定程度时，为了平滑切 近航线，ψ_E角要进行适当调整。

为解决上述问题，本发明提出采用如图4所示的滑模面s，可通过式(6)表示：

$s=\left\{\begin{array}{cc}{\mathrm{\&psi;}}_E-\frac{\mathrm{\&pi;}}{2}& (y>Y)\\ {\mathrm{\&psi;}}_E+\mathrm{\&lambda;y}& (-Y<y<Y)\\ {\mathrm{\&psi;}}_E+\frac{\mathrm{\&pi;}}{2}& (y<-Y)\end{array}\right.---\left(6\right)$

式中Y为一正值，单位为米，其取值与飞机的参数有关。导航算法的目标是保持s＝0。由式(2)有转弯半径最小值为在确定参数时，V值用飞机的最大巡航速度，实际测试Y值可取1.5～3倍R_min，参数

当|y|＞Y时，导航的目标是保证或只要控制系统稳定，系统总能在有限时间内快速到达滑模面。将图2中的地速V分解到平行于目标航线与垂直目标航线的方向上，并对侧向偏离距离y求导，得到下式：

$\stackrel{\&CenterDot;}{y}=V\sin {\mathrm{\&psi;}}_E---\left(7\right)$

当|y|≤Y时，导航目标是保证ψ_E＝-λy，将其代入式(7)有：

$\stackrel{\&CenterDot;}{y}=-V\sin \left(\mathrm{\&lambda;y}\right)---\left(8\right)$

特别地，当y很小时，即或更小时，式(8)可近似为：

$\stackrel{\&CenterDot;}{y}=-\mathrm{V\&lambda;y}---\left(9\right)$

该微分方程的解为：

y＝y_ie^-Vλt                  (10) 

式中y_i为式(9)成立时的y值。从式(10)中可看出，y随时间成指数减小，参数λ越大，y减小得越快，并且当t→+∞时，y→0，导航算法稳定。

等效控制量用于保证当|y|≤Y时，可得将其联立式(7)和式(5)可得等效控制量：

${\mathrm{\&phi;}}_{\mathrm{eq}}=-\arctan \left(\frac{\mathrm{\&lambda;}{V}^2\sin \left({\mathrm{\&psi;}}_E\right)}{g}\right)---\left(11\right)$

加上一个开关控制量，可得总的控制量：

$\mathrm{\&phi;}=-\arctan (\frac{\mathrm{\&lambda;}{V}^2\sin \left({\mathrm{\&psi;}}_E\right)}{g}+\mathrm{ksgn}\left(s\right))---\left(12\right)$

式中sgn(·)为符号函数，k为一正值常量。若采用此控制律，由于开关量的引入，控制量可能会有正负突变，会造成抖振现象。因此，需要重新进行控制律的设计问题。

2.3控制律设计 

根据选择的滑模面，对任意的侧向偏离距离y均有一个固定的期望ψ_E值，根据无人机的当前航迹方位角ψ_G，可以计算出当前的航迹误差角ψ_CE与期望的ψ_E角的误差ε_E，为了产生一个平滑的导航输出φ_r，可以根据当前的ε_E来设计控制律。根据选择的滑模面s计算当前的ε_E，具体如下式所示：

${\mathrm{\&epsiv;}}_E=\left\{\begin{array}{cc}{\mathrm{\&psi;}}_{\mathrm{CE}}-\frac{\mathrm{\&pi;}}{2}& (y>Y)\\ {\mathrm{\&psi;}}_{\mathrm{CE}}+\mathrm{\&lambda;y}& (-Y<y<Y)\\ {\mathrm{\&psi;}}_{\mathrm{CE}}+\frac{\mathrm{\&pi;}}{2}& (y<-Y)\end{array}\right.---\left(13\right)$

式中ψ_CE＝ψ_G-ψ_T，-π≤ψ_CE≤π，当前航迹方位角ψ_G指向目标航线右侧，ψ_CE为正。

当无人机以滚转角φ转弯时，根据角速度及式(2)可得：

$\mathrm{\&omega;}=\frac{g\tan \mathrm{\&phi;}}{V}---\left(14\right)$

由ωT＝-ψ_E可得：

$\mathrm{\&phi;}=-\arctan \left(\frac{V{\mathrm{\&psi;}}_E}{\mathrm{gT}}\right)---\left(15\right)$

式中T为期望的响应时间，单位为秒，可根据系统的响应速度进行设置。T越大，响应越快，可能会发生抖振现象，T越小，系统越稳定，但响应很慢。

当无人机的侧向偏离距离值值|y|在期望的控制精度ε内时，导航算法的控制目标变为保持ψ_CE＝0，此时可用式(11)的控制律，总的控制律如下：

$\mathrm{\&phi;}=\left\{\begin{array}{cc}-\arctan \left(\frac{V{\mathrm{\&psi;}}_E}{\mathrm{gT}}\right)& \left|y\right|>\mathrm{\&epsiv;}\\ -\arctan \left(\frac{\mathrm{\&lambda;}{V}^2\sin \left({\mathrm{\&psi;}}_{\mathrm{CE}}\right)}{g}\right)& \left|y\right|\&le;\mathrm{\&epsiv;}\end{array}\right.---\left(16\right)$

无人机机身倾斜时，升力竖直方向的分力平衡自身重力，故滚转角|φ|会有一个上限值φ_max，故计算时应限定|φ|≤φ_max。在实际飞行中，飞机的俯仰角θ不一定为0，故最后的导航输出 制导量为φ_r＝φcosθ。

滑模控制器最终输出的制导量φ_r为图3中需要控制的滚转角φ，将其送给无人机的控制驱动系统，最终控制无人机的舵机，将飞机的实际滚转角φ控制到滑模控制器输出的φ_r角。

在飞行过程中，控制系统保证φ＝φ_r即可将无人机从一个目标航点引导控制到下一个目标航点。

三、实验结果及分析

为了验证本发明提出的导航算法的有效性，在重庆大学图像信息处理实验室研制的飞控系统上进行了实际飞行测试。用于飞行测试的飞机如图5(a)所示。图5(b)为测试用飞控系统主板。测试飞机为Bixler 2型，该飞机的巡航速度在10-20m/s。飞控系统搭载的传感器主要有3轴陀螺仪与及加速度计MPU6000，磁强计HMC5883L，Ublox GPS以及高度计MS5611，处理器采用小体积低功耗的Cortex-M3处理器。

针对测试飞机的具体性能，飞机的滚转角上限值φ_max＝45°，滑模面选择时的Y参数设为40m，控制律参数响应时间T＝1，侧向偏离距离控制精度ε＝3米。为了测试导航算法对大侧向偏离距离及小侧向偏离距离的稳定性，进行了高度为100米的大侧向偏离距离的两点航点间的导航测试以及小侧向偏离距离的连续航点间的导航测试。

图6～图8为大侧向偏离情况下的两点间导航测试结果。从图6的轨迹图中可以看出，无人机先进行转弯将导航的目标航向调整到与目标航线航向垂直的方向，并保持沿该方向飞行，当侧向偏离小于设定值时，再进行转弯，最终切近目标航线，运动轨迹符合本发明设计的导航逻辑。从图7中可以看到，5～10s过程中，侧向偏离距离一直大于的本发明设定值，无人机的航迹误差角一直保持在90°左右。在10s之后，侧向偏离距离小于设定值，航迹误差角也相应减小，最终将侧向偏离距离控制在要求的精度内，如图8所示，说明了本发明导航控制方法与控制律的有效性和稳定性。

为了进一步验证本发明的有效性，进行了20个连续航点的导航测试，测试结果如图9～图12所示。从图9中可以看出，本发明控制方法对连续航点间的导航也是有效稳定的，呈现出良好的航迹跟踪性能。从图10中可以看出，连续航点测试侧向偏离距离误差比两点间导航测试结果大，造成的主要原因是航点间距离比较近，航点切换较快，系统没有足够的调整时间。图11为测试过程中飞机实际滚转角(roll)与导航算法输出的参考滚转角φ_r(nav_roll)之间的跟随变化图。从该图中可以看出本发明控制方法输出的制导量φ_r变化平滑，实际滚转角对其跟随效果好，验证了算法推导过程中φ_r≈φ近似的正确性。从图12的 高度变化曲线中可以看出，系统稳定后，高度误差控制在2米内，验证了将三维导航分解为平面导航与高度导航的适用性和精确性。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管申请人参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的宗旨和范围的，均应涵盖在本发明的权利要求范围当中。

Claims (1)

1.基于小型无人机导航的滑模控制方法，包括如下步骤：

(1)飞行前，根据飞机类型，查询手册，得到飞机的最大滚转角φ_max以及飞机的最大巡航速度V_max；

(2)确定滑模面s(x)：滑模面代表系统的理想动态特性，设计一个非线性滑模面；飞机以最大速度V_max飞行时，最小转弯半径Y＝2R_min，参数

(3)滑模控制器设计：设计滑模控制器，使到达条件得到满足，从而使趋近运动于有限时间到达滑模面，并且在趋近的过程中快速、抖振小；

根据选择的滑模面，对任意的侧向偏离距离y均有一个固定的期望ψ_E值，根据无人机的当前航迹方位角ψ_G，计算出当前的航迹误差角ψ_CE与期望的ψ_E角的误差ε_E；为了产生一个平滑的导航输出φ_r，根据当前的ε_E来设计控制律；

根据选择的滑模面s计算当前的ε_E，具体如下式所示：

${\mathrm{\&epsiv;}}_E=\left\{\begin{array}{cc}{\mathrm{\&psi;}}_{\mathrm{CE}}-\frac{\mathrm{\&pi;}}{2}& (y>Y)\\ {\mathrm{\&psi;}}_{\mathrm{CE}}+\mathrm{\&lambda;y}& (-Y<y<Y)\\ {\mathrm{\&psi;}}_{\mathrm{CE}}+\frac{\mathrm{\&pi;}}{2}& (y<-Y)\end{array}\right.---\left(13\right)$

式中ψ_CE＝ψ_G-ψ_T，-π≤ψ_CE≤π，当前航迹方位角ψ_G指向目标航线右侧，ψ_CE为正；

当无人机以滚转角φ转弯时，根据角速度及式(2)可得：

$\mathrm{\&omega;}=\frac{g\tan \mathrm{\&phi;}}{V}---\left(14\right)$

由ωT＝-ψ_E可得：

$\mathrm{\&phi;}=-\arctan \left(\frac{V{\mathrm{\&psi;}}_E}{\mathrm{gT}}\right)---\left(15\right)$

式中T为期望的响应时间，单位为秒，根据系统的响应速度进行设置；T越大，响应越快，可能会发生抖振现象；T越小，系统越稳定，但响应很慢；

当无人机的侧向偏离距离值|y|在期望的控制精度ε内时，导航算法的控制目标变为保持ψ_CE＝0，总的控制律如下：

$\mathrm{\&phi;}=\left\{\begin{array}{cc}-\arctan \left(\frac{V{\mathrm{\&psi;}}_E}{\mathrm{gT}}\right)& \left|y\right|>\mathrm{\&epsiv;}\\ -\arctan \left(\frac{\mathrm{\&lambda;}{V}^2\sin \left({\mathrm{\&psi;}}_{\mathrm{CE}}\right)}{g}\right)& \left|y\right|\&le;\mathrm{\&epsiv;}\end{array}\right.---\left(16\right)$

无人机机身倾斜时，升力竖直方向的分力平衡自身重力，故滚转角|φ|会有一个上限值φ_max，故计算时应限定|φ|≤φ_max；

在实际飞行中，飞机的俯仰角θ不一定为0，故最后的导航输出制导量为φ_r＝φcosθ；

(4)无人机控制；滑模控制器最终输出的制导量φ_r为需要控制的滚转角φ，将其送给无人机的控制驱动系统，最终控制无人机的舵机，将飞机的实际滚转角φ控制到滑模控制器输出的φ_r角。

在飞行过程中，循环执行(3)、(4)过程，即可将无人机从一个目标航点引导控制到下一个目标航点。