四旋翼飞行器抗干扰轨迹跟踪控制方法研究

孙克文，刘国宁，杨允权，杨 洁

（郑州大学机械与动力工程学院，河南 郑州 450001）

1 引言

四旋翼飞行器相对于传统飞行器有着可垂直起降、机动性和可操纵性强等特点，当前应用范围广泛，如战场侦察、电力巡检、农林植保、灾情救援等［1-2］。同时在物流投递、空中数据链中继等方面的应用也是目前的研究热点［3］。但由于四旋翼的机身小、质量轻，外界突发的环境变化如阵风、风紊流等对其稳定控制方面存在不利影响，尤其在轨迹跟踪控制方面［4］。

现阶段在四旋翼轨迹跟踪研究方面已有一定的研究成果。其中，文献［5］运用反步法实现飞行器的轨迹跟踪，但反步法需要四旋翼系统模型具有确定的严反馈形式，对模型的准确性要求很高，若系统建模误差过大会严重影响控制器的跟踪精度。文献［6-7］分别使用PID算法及自适应PID算法对轨迹跟踪问题进行研究，实现了飞行器对目标轨迹的稳定跟踪，但在引入外部干扰因素的情况下，此线性控制方法很难保证跟踪精度。文献［8-9］结合传统线性控制和智能控制方法进行研究，但控制方式的智能化往往对硬件处理器要求较高。文献［10］采用反步滑模的控制方法对飞行器的姿态及高度进行控制，实现稳定的姿态和定高控制，但由于在姿态控制器设计中引入了符号函数，会导致飞行器出现高频抖震，严重影响飞行稳定性。

由于现有的在无干扰环境下四旋翼的轨迹跟踪研究已趋于完善，但在现实飞行中，四旋翼飞行器的飞行环境往往是复杂的，故针对复杂环境中四旋翼的轨迹跟踪问题进行研究，充分考虑到外界风场环境对四旋翼飞行稳定性的影响，采用经典的内外双闭环控制框架，在此框架内设计了全新的位置滑模控制器和姿态双闭环滑模控制器，在仿真实验中与传统滑模控制器进行各项性能的对比后得出结论：这里提出的四旋翼轨迹跟踪控制方法具有更优的跟踪性能和抗干扰性。

2 风场分析与建模

现实中风场的组成成分是多样的，其中包括：风切变成分、阵风成分、风紊流成分等。为克服这些风场特性对四旋翼飞行的不利影响，本节针对这些风场特性进行了合理的工程化建模，为模拟较真实的外界风场提供了技术手段。

2.1 风切变模型

由于四旋翼飞行器主要飞行在中低空范围，因此需要研究的是靠近地面的边界层风场环境。

其中，低空风场的风切变特性是影响飞行器飞行稳定的主要因素，Prandt对数模型是当前运用广泛的风切变工程化模型，此模型适用300m以下的高度范围，模型为［11］：

式中：Vp—风切变平均风速值；h—离地高度；h'—参考高度，通常取6.096m；z0—底面粗糙度，一般取0.05；u20为h'=6.096m处的风速。

2.2 阵风模型

根据以往四旋翼飞行器的试飞实验，发现阵风干扰也是影响飞行器姿态稳定的不利因素。故而这里将阵风模型引入风场建模中，以此建立起更加真实的风场环境。阵风模型主要描述风速周期性变化的特性，风速在某时间点持续增强一段时间后减弱至消失，此为一个阵风周期。其工程化模型表示为［12］：

式中：Vgmax—阵风峰值；t1—阵风开始时间；Tg—阵风周期。

2.3 紊流模型

现实环境中的风场有多种组成成分，既包含有风切变和阵风这些低频类型，也存在有高频类型的风紊流成分。当今应用最为广泛的风紊流工程化模型是Dryden模型，此模型能够真实反映大气中风紊流的随机特性。将真实风场的频谱函数进行分解后可得到生成紊流风速Vd的成型滤波器，其中滤波器的输入为高斯白噪声。拆分后的风场频谱函数为［13］：

式中：σu、σv、σw、Lu、Lv、Lw—纵、横和竖轴上的紊流强度及紊流尺度；Vv、Vu、Vw—飞行器速度矢量在三轴上的速度分量。由于飞行器一般在低空飞行，根据MIL-HDBK-1797规范，在（3～300）m高度范围内，风场紊流强度和尺度满足如下关系：

为获取更加真实的风场环境，将上述三种风场模型进行叠加，即：

式中：Va、Vd—模拟的风场风速和Dryden紊流风速。

实际飞行中，风场作用在飞行器上的风力和风力矩可当做是风扰动具体表现形式，公式为：

式中：D=[Dx Dy Dz]T—风力；d=[dx dy dz]T—1力矩；Ki=[K1K2K3]—平动空气阻力系数；Kj=[K4K5K6]—滚转空气阻力系数，可通过风洞试验获取；ρ—空气密度；S=[Sx Sy Sz]T—机身迎风面积；l—飞行器机臂长度；ζ=[u v w]T—对流速度，飞行器对地速度与风速作差可得。

3 风场下四旋翼系统建模

3.1 坐标系定义与转换

建立合理的坐标系是准确描述四旋翼飞行器空间位置及飞行姿态的前提，本节建立了与飞行器固连的机体坐标系B(oxyz)和与大地相固定的地面坐标系E(OXYZ)，通过两坐标系的旋转变换关系可描述飞行器的空间姿态。在地面坐标系尺度下，飞行器的空间位置由P=[x，y，z]表示，飞行姿态由欧拉角Θ=[ϕ，θ，ψ]表示。其中ϕ，θ，ψ分别代表飞行器的横滚角、俯仰角以及偏航角。飞行器的旋翼旋转产生4个垂直于机体水平面且方向朝上的升力Ti以及4个作用在机体的反扭矩控制着飞行器的俯仰、滚转和偏航运动，如图1所示。机体和地面坐标系间转换关系为：

图1 四旋翼飞行器运动控制简图Fig.1 The Motion Control Diagram of Quadrotor

s(·)和c(·)分别为sin(·)和cos(·)的缩写。

为了简便，在四旋翼飞行器建模过程中做出以下假设：

（1）将飞行过程中四旋翼机体和螺旋桨考虑为刚性体。

（2）飞行器质量及转动惯量恒定不变。

（3）飞行器机体上质量分布均匀，各机臂长度均相等。

（4）飞行器只受重力和螺旋桨拉力，不考虑近地效应以及旋翼旋转所造成的抖震。

3.2 旋翼空气动力学模型

旋翼升力与转速的关系［13］如下：

反扭矩表示为：

式中：cT—升力系数；cM—转矩系数；ωi(i=1，2，3，4)—旋翼转速。

3.3 风场下四旋翼飞行器动力学模型

在文献［14］中给出了不考虑扰动的飞行器动力学模型。而这里将影响飞行器运动特性的风扰及空气阻力因素进行数学描述，得到风场下矢量形式的四旋翼飞行器动力学模型：

式中：ve—飞行器速度矢量；F—旋翼总升力；ωb—飞行器旋转角速度矢量；τ—旋翼作用在机体的控制力矩；J—飞行器转动惯量矩阵。

3.4 四旋翼飞行器运动学模型

四旋翼运动学模型的输入为速度矢量和机体轴角速度矢量，输出的是飞行器的位置和姿态。模型表示如下：

式中：W—姿态角变化率与机体旋转角速度转换矩阵为：

考虑到四旋翼处于飞行平衡点时，滚转角和俯仰角度变化较小(ϕ→0，θ→0)，根据泰勒公式，飞行器在平衡点附近处有sinϕ→0，tanθ=0，cosϕ→1，所以W可近似为单位矩阵，得到近似关系：≈ωb。

将式（9）～式（11）和式（13）联立整合并转换为标量形式的四旋翼飞行器系统模型：

式中：Ix、Iy、Iz—飞行器在三轴向的转动惯量。

式中：U1-3—飞行器在x、y、z方向上运动的虚拟控制量；U4—飞行器绕z轴转动的虚拟控制量。

4 控制策略

由四旋翼飞行器系统模型式（15）可得，此系统模型是具有四个控制量U1-4、六个被控输出量(x，y，z，ϕ，θ，ψ)的欠驱动且非线性的系统。基于此特点，采用内外双闭环的控制架构［15］，控制策略，如图2所示。内外环分别控制飞行器姿态和位置。同时在此框架下，在内外环中分别进行位置控制器和姿态控制器的设计，以此实现四旋翼飞行器对目标轨迹[xd(t)，yd(t)，zd(t)，ψd(t)] 的稳定跟踪，同时保持横滚角ϕ(t)与俯仰角θ(t)的随动镇定以提高飞行稳定性。

图2 四旋翼飞行器控制结构Fig.2 Control Structure of Quadrotor

首先将飞行器系统模型中的姿态角耦合项设计为虚拟控制量，即：

四旋翼飞行器位置系统模型转化为：

其次针对式（18），按照对目标轨迹的跟踪要求，设计出性能优越的位置控制器，求得四旋翼系统的虚拟控制量ux、uy和uz。依据式（17）及期望偏航角ψd(t)，可反解出期望横滚角ϕd(t)、期望俯仰角θd(t)及高度控制输入量U1，求解关系如下：

最后在内环中进行姿态控制器的设计，期望的横滚角ϕd(t)和俯仰角θd(t)作为姿态控制器的输入，输出为控制量U2、U3；同理以给定的期望偏航角ψd(t)为输入，输出为控制量U4。根据电机控制分配模型式（16）反解出各电机转速并输入到飞行器系统模型中，获得飞行器的实时位置(x，y，z)与姿态(ϕ，θ，ψ)，最终以负反馈形式传输给两控制器以完成闭环控制。

5 控制器设计

5.1 位置滑模控制器设计

在风场环境下，针对四旋翼飞行器对目标轨迹精确跟踪的问题。本节基于传统滑模控制器的设计思想，提出一种指数趋近律，此趋近律引入了滑模切换鲁棒项，以此来保证飞行器受扰动下的飞行稳定性。

定义高度误差为：

定义一般形式的滑模面［12］为：

其中，cz＞0为控制器可调参数。经仿真验证，当sz=0时，高度误差ez以指数速率趋近于0，可实现飞行器向期望高度位置的快速逼近。

将sz对时间求导，把式（20）带入可得到滑模面sz的状态方程：

其中kz＞0，将式（23）和趋近律式（24）联立，得出改进的高度通道滑模控制律为：

其中，ηz、kz＞0，均为控制器可调参数。

稳定性证明：

根据Lyapunov稳定性理论判断系统状态sz的稳定性，首先选取正定的Lyapunov函数：

对其求导得：

当参数ηz满足关系式ηz≥max时得：

即 |sz|≤，则证得sz指数收敛至平衡点0，当t→∞时，sz=0。从而ez→0，z→zd，即实现高度方向的位置跟踪。

由于四旋翼系统（15）的水平通道与高度通道的模型形式一致，同样得出改进的水平通道滑模控制律ux和uy为：

其中，sx=cxex+，sy=cyey+，cx、cy＞0，ηx、ηy＞0，kx、ky＞0，均为可调参数。

同时利用上述Lyapunov稳定理论也能证明：当t→∞时，sx=0，sy=0。

5.2 姿态双闭环积分滑模控制器设计

要实现四旋翼飞行器对目标轨迹的精确跟踪，首先必须保障其能够快速稳定的控制机身姿态。由于四旋翼飞行器易受风、气流等扰动因素影响，加大了姿态控制器的设计难度。滑模控制器对外部扰动不敏感，响应快且算法实现简单，满足姿态控制器的设计需求。另外，这里设计了双闭环形式的姿态控制架构，内外环分别控制姿态角和角速度。姿态控制结构，如图3所示。

图3 姿态双闭环控制结构Fig.3 Posture Double Closed-Loop Control Structure

下面以横滚角ϕ为例进行双闭环滑模控制律的设计：

式中：p、q、r—横滚角速度、俯仰角速度和偏航角速度—横滚角加速度，a1=(Iy-Iz)/Ix，C1=L/Ix。

设定控制目标：对于给定的横滚角指令ϕd(t)，设计控制量U2，使得横滚角ϕ(t) →ϕd(t)。

横滚角跟踪误差量定义为：

式中：ϕd—横滚角指令；ϕ—横滚角。

选取动态滑模面为：

其中，kϕ＞0为可调参数，经验证，当sϕ=0时，误差eϕ在快速趋近于0，满足滑模面设计要求。

式（33）对时间求导后，将式（32）带入其中可得到关于sϕ的动态方程：

构造一阶正定Lyapunov函数：

求导可得：

考虑到滑模控制受强扰动时，角速度内环的稳定性会受影响且不能快速恢复稳定，将角加速度误差积分项加入到角速度滑模面中以补偿外部扰动的不利影响，减小了角速度跟踪的稳态误差，其中，横滚角速度的滑模面设计如下：

横滚角速度p的滑模指数趋近律的设计同式（24）相同：

其中，η1、α1＞0。

结合式（40）、式（41）得到横滚角ϕ的滑模控制律：

稳定性证明：

选取正定的Lyapunov函数：

可知sp以指数速度趋近于平衡点sp=0，从而证得滑模面sp和指数趋近律设计的正确性。

俯仰角θ和偏航角ψ的滑模控制律同理可得：

其中，a2=(Iz-Ix)/Iy，a3=(Ix-Iy)/Iz，C2=L/Iy，C3=1/Iz，η2、η3＞0，α2、α3＞0，kq、kr＞0，以上均为姿态控制器可调参数。

5.3 边界层技术抑制扰动

上文设计出的控制律ux、uy、uz、Ui(i=1，2，3，4)存在符号函数sgn(·)，引起系统高频抖动，因此利用边界层技术，将符号函数sgn(·)用饱和函数sat(si/δ)代替，可减小抖震，饱和函数如下：

式中：δ—边界层厚度，又称消震因数，一般取较小正整数。

6 仿真结果与分析

这里以DJI公司的F450机架为基础搭建四旋翼实验平台，并参照飞行器实际物理参数，在Matlab/Simulink中将图2所示的飞行器整体控制框架进行搭建以验证所设计控制器的各项性能，四旋翼实验平台实拍图，如图4所示。飞行器具体参数［15］及控制器参数，如表1、表2所示。

表1 四旋翼飞行器物理参数Tab.1 Physical Parameters of Four-Rotor Aircraft

表2 控制器参数Tab.2 Controller Parameters

图4 实验测试的四旋翼飞行器Fig.4 The Experimentally Tested Quad-Rotor Aircraft

6.1 四旋翼姿态控制约束

结合飞行器实际的飞行状态，为避免飞行器产生失稳甚至是坠机事故，需要对仿真系统中的飞行器姿态角进行范围限定，约束范围，如表3所示。

表3 姿态角约束范围Tab.3 Range of Attitude Angle Constraints

6.2 风场数值模拟

在Matlab/Simulink环境中，将白噪声通过传递函数为G(s)［12］的滤波器生成模拟的风紊流信号Va将四旋翼飞行器飞行高度设置为10m，平动速度设置为2m/s，高度6.096m处风场平均风速为10m/s，飞行器飞行高度10m处的阵风峰值Vgmax设为7m/s，阵风周期Tg为5s，仿真时间为20s，得到的风场数值模拟图如下，如图5所示。

图5 风场数值模拟Fig.5 Numerical Simulation of Wind Field

由图5中可以看出，仿真得到三轴向模拟风速值的范围均在（0～15）m/s之间。同时由表4可知，模拟风场的最强风力等级为7级，风力等级较强。将此风场干扰引入飞行器控制系统中，以便充分验证所设计控制器的鲁棒性和稳定性。

表4 风力等级表Tab.4 Wind Power Rating Table

6.3 风场扰动下的轨迹跟踪仿真

在搭建的飞行器控制系统框架中引入上述风场模型。引用经典的单环滑模控制（SMC）作为对比实验以验证所提出控制器的优越性能。设定期望的四旋翼螺旋上升轨迹为：[xd，yd，zd，ψd]=[5 cos(t)，5 sin(t)，2 +t/10，π/3 ]，设定飞行器初始位置和姿态分别为P=[x，y，z]，Θ=[0，0，0 ]。

轨迹跟踪的效果，如图6、图7所示。仿真结果表明无论是提出的方法还是传统滑模控制方法（SMC），均能实现空间轨迹的跟踪。但由图7可以看出：这里的方法相比于SMC，跟踪目标轨迹时间更短，具有更快的收敛速度，且不存在超调，这对于快速机动下飞行器的轨迹跟踪是比较有利的。

图6 风场下四旋翼的轨迹跟踪Fig.6 Trajectory Tracking of Aircraft in the Wind Field

图7 俯视轨迹Fig.7 Top View Track

从图8可以看出，在对飞行器施加外部强扰动的情况下，采用传统滑模控制的四旋翼跟随轨迹存在波动，且向目标轨迹逼近的时间较长，误差收敛速度慢，可见此方法无法抑制强扰动，抗干扰性能差；而这里的方法控制下的四旋翼在三轴向上均能快速且平滑地跟踪目标轨迹，响应速度更快，具有更小超调量，跟踪性能优越。

图8 轨迹跟踪误差对比Fig.8 Trajectory Tracking Error Comparison

图9、图10分别给出了轨迹跟踪过程中飞行器的横滚角及俯仰角的随动效果。从图9可以看出：由于加入扰动因素，传统滑模方法控制下，生成的飞行器期望横滚角轨迹在随动初始阶段会出现较明显的不规则抖动，这会对姿态环控制器的跟踪性能提出较高要求，不利于后续横滚角的稳定跟踪；而在这里的方法控制下，生成的期望横滚角轨迹较为平滑，从细节图可见，有短暂小幅抖动，幅值在60°以内，属于合理范围。通过图10不难看出，飞行器俯仰角的随动效果与横滚角相似，这里不再赘述。

图9 横滚角随动镇定效果对比Fig.9 Comparison of Follow-Up Stabilization Effects of Roll Angle

图10 俯仰角随动镇定效果对比Fig.10 Comparison of Follow-Up Stabilization Effects of Pitch Angle

6.4 姿态控制抗干扰仿真

在simulink环境中搭建的四旋翼飞行器姿态控制系统框架，如图3所示。将飞行器初始姿态设定为悬停状态，即Θ=[0，0，0 ]。期望姿态设为：Θ=[30，60，45]，同时在第5s引入x轴方向风扰模型，10s引入y轴方向风扰模型，15s引入z轴方向风扰模型，风扰维持3s，设仿真时间为20s。

飞行器各姿态角的目标跟随效果，如图11所示。这里的姿态双闭环控制器明显优于传统的滑模控制器，在前者控制下，各姿态角在2s左右均能到达期望值，横滚角和俯仰角的超调明显小于后者。从细节图中看出，姿态角上升时间和调节时间也小于传统滑模控制器，四旋翼的暂态性能得到明显优化。在引入干扰后，两控制器控制下的飞行器姿态角均出现不同程度抖动。双闭环滑模控制下姿态角跟随有小幅偏离，但可在0.5s内快速收敛回期望值；反观传统滑模控制器，其偏离期望值的幅度大，收敛速度也较慢。

图11 姿态角跟随效果对比Fig.11 Comparison of Posture Angle Following Effect

从图12可以看出：所设计姿态控制器控制下，飞行器的控制量曲线连续且平滑，在突遇外部扰动时，控制量的变化也较为平稳，这对飞行器上直流电机的控制性能来说是非常友好的；而传统滑模控制下飞行器的控制量出现了严重震荡，这在实际控制中这是难以实现的。

图12 抑制抖震效果对比Fig.12 Comparison of Anti-Shake Effects

7 结论

针对复杂风场环境下四旋翼飞行器的轨迹跟踪问题，首先考虑到空气阻力及外部扰动对系统的影响，将这些影响因素进行数学描述并引入到四旋翼系统方程中；其次采用经典的四旋翼内外环的飞控框架，基于滑模控制理论分别在姿态内环和位置外环中进行位置滑模控制器和姿态双闭环滑模控制器的设计，并利用Lyapunov稳定性判据证明了此框架下四旋翼飞行器系统的飞行稳定性。最终进行与传统滑模控制器的控制性能对比实验后得出结论：所提出控制方法可实现风场强扰动下四旋翼飞行器稳定的轨迹跟踪，相比较传统方法具有更优的跟踪性能和鲁棒性。