基于干扰观测器的分布式四旋翼编队跟踪控制

窦立谦，陈 涛，毛 奇

(天津大学电气自动化与信息工程学院，天津 300072)

近年来，具有机载智能能力的无人机(UAV)已广泛应用于军事领域和民用工业领域．这些系统在机器人领域开辟新的应用，包括监视、灾害(环境、工业和城市)修复、搜索、救援等[1]．四旋翼无人机因其结构简单且具有垂直起降、定点悬停等优点，获得了广泛关注．四旋翼无人机是一个具有 6自由度和 4个控制输入的欠驱动系统[2]，具有非线性、强耦合、多变量等特点．许多控制方法，诸如 PID、反馈线性化、自适应、滑模等方法已经被运用到四旋翼飞行控制中．然而单架四旋翼无人机在完成复杂任务时，由于自身载荷和传感器性能的不足，应用日益受到限制，取而代之的是越来越多的多无人机协同工作完成任务．因而多无人机协作越来越引起国内外诸多研究人员的研究兴趣．

无人机编队控制方法主要有领航跟随法[3]、虚拟结构法[4]、基于行为法[5]和图论法[6]等．尽管在大多数情况中，假定长机的轨迹可以传递到每个智能体，但在实际中，假设只有部分个体可以知晓期望的轨迹是更有意义的．近来，线性时不变群体系统或多智能体系统一致性问题受到越来越广泛的关注．一致性意味着所有智能体就某些相关的协作变量上达成一致．文献[7]采用一致性策略设计分布式编队控制器，实现了多四旋翼系统时变队形的协同控制．文献[8]研究了小型四旋翼机群自主编队，在串级控制系统框架下采用平均一致性算法和有领导一致性算法产生各四旋翼飞行器的期望轨迹，并给出一种基于有向Hamilton 环的通信拓扑设计方案．但文中将四旋翼模型简化为一阶系统，模型较为简单，控制误差较大．文献[9]将四旋翼飞行器描述为二阶系统，利用二阶一致性算法，实现多四旋翼编队集结和编队机动等行为．但该控制器中没有充分考虑四旋翼模型的特点，也会有一定的控制偏差．文献[10]充分考虑了四旋翼的模型特点，为编队系统设计了分布式控制算法，实现编队的协作控制．以上文献都没有考虑系统参数摄动和外界环境干扰，这些因素都会对系统稳定性和控制效果产生较大影响．

本文针对存在外界环境干扰和系统参数摄动等不确定性的多四旋翼编队系统，设计了一种基于干扰观测器和终端滑模的分布式编队跟踪控制算法，而且期望的位置信息只有部分个体可以获得．利用快速终端滑模观测器实现对复合干扰的实时估计并补偿，利用积分滤波器避免对虚拟控制输入求导时产生的微分爆炸问题．最后通过稳定性分析来验证分布式控制器的有效性，并通过仿真来验证该算法的性能．

1 四旋翼飞行器数学模型

图1给出了四旋翼飞行器的基本模型，并且建立两个坐标系：机体坐标系Oxyz和地面坐标系

图1 四旋翼的结构和坐标系Fig.1 Structure of the quadrotor and coordinate system

为了简化四旋翼的建模和控制器设计过程，文中做出如下假设．

假设1四旋翼飞行器为刚体，忽略其弹性形变；飞行器质心与坐标原点重合，且结构关于平面Oxz和Oyz对称．

假设 2由于四旋翼飞行器的飞行速度较低，气动阻力对飞行器影响较小，飞行器受到的气动阻力可以忽略不计[11]．

根据 Newton-Euler公式，四旋翼飞行器模型的动力学和运动学方程[11]为

式中：P和V分别为飞行器在地面坐标系中的位置向量和速度向量；Θ为欧拉角向量T；Ω为飞行器在机体坐标系中的角速度；m为飞行器的质量；g为重力加速度；e3为在地面坐标系中的单位向量，J为飞行器的惯性矩阵，Ga为电机转动时产生的附加力矩，，Jr为电机的转动惯量为各旋翼转速；f为控制力，f=，uT为旋翼产生的升力；τ为控制力矩．

Oxyz到的转换矩阵R( Θ)为

转换矩阵Π ( Θ)为

假设 3由于四旋翼飞行器的飞行速度较慢，飞行高度较低，升力系数c和阻力系数k可以视为常值．

式中l、c、k分别为旋翼中心到飞行器质心的距离、升力系数和阻力系数．

考虑模型中存在的外界干扰和不确定性，式(1)改写为

式中为模型中的复合干扰，包括系统参数摄动不确定以及外界环境干扰．

假设 4 当t≥0时恒成立，则式(3)中Π( Θ )是非奇异的．

2 图论基础知识

文中设计了一组具有n架四旋翼的编队系统．在控制这些四旋翼运动时，假设每架四旋翼通过传感器和通信通道来获取自己的状态信息和部分相邻个体的位置信息．因此，本文通过有向图来模拟四旋翼之间的信息交换．通常将有向图G表示为(N,)ε，其中N代表非空节点集合，ε是边的集合．有向图中的边(i, j)表示个体i可以从个体 j获取信息，并把 j定义为父节点，将i定义为子节点．若对于图G中的每一对节点，都存在一条路径连接两点，则称该图为连通图．在有向图中，如果存在一个边的子集，能够连接图中所有节点，则称这个边的子集为一个生成树，同时称该图包含一个有向生成树．

邻接矩阵用代数矩阵的方式来描述节点之间的位置关系，其中

拉普拉斯矩阵定义为，其中

同时定义一个矩阵表示个体i和长机之间的连接情况，当个体i与长机连通，，否则

如图2所示，本文的目标是设计一个分布式编队控制器，使多架四旋翼飞行器在长机的带领下达到并保持期望的队形．

图2 编队控制结构Fig.2 Formation control architecture

定义为第i个四旋翼相对长机的期望偏差，i=1,⋅⋅⋅,n ，则本文的控制目标为

式中和Lψ分别代表虚拟长机的位置和偏航角．

3 分布式编队控制方法设计

图 3给出了第i架四旋翼的控制结构，每架四旋翼可以获取自身的状态信息和部分邻居个体的状态信息，并根据获取的信息计算出自身控制量，使多架四旋翼形成期望的队形．

图3 第i架四旋翼控制结构Fig.3 Controller structure for the i-th quadrotor

3.1 干扰观测器设计

本节针对实际系统中存在的外界环境干扰和系统参数摄动等不确定性的影响，设计一种基于快速终端滑模的干扰观测器(TSMDO)，对干扰进行估计和补偿，从而减少外加干扰和系统参数摄动等对控制效果的影响．

假设 5假设式(6)和式(7)中复合干扰是有界的，即存在正实常数ρVi和 ρΩi，使得和成立．

对于系统(6)，设计干扰观测器使得

式中是对系统状态的估计；dˆVi是对复合干扰 dVi的估计．

定义滑模变量为

对sVi求导得

设计基于快速终端滑模的干扰观测器为

式中：为正常数，且满足

引理1[12]针对非线性系统

式中是定义域u到n维空间满足局部 Lipschitz连续性的连续函数．假设存在连续正定 函 数 W(x):u→R ，α1＞0和使 得成立，则该非线性系统是有限时间稳定的，且到达时间为

式中W(x(0))为W(x)的初始值．

定理 1 在假设 5的条件下，针对位置子系统动力学模型(式(6))，选取合适的参数，则设计的干扰观测器(式(15))可以使干扰的估计值快速收敛到

证明 为证明可以快速收敛到dVi，将式(15)代入到式(14)中，得到

选取Lyapunov函数

对时间求导得

根据引理 1可得辅助变量sVi可以在有限时间内收敛到 0，从而保证干扰估计能够快速收敛到同理，可以设计相同的干扰观测器使得干扰估计快速收敛到dΩi．

3.2 位置控制器设计

首先，定义第i架四旋翼的位置跟踪误差

式中为长机的位置．对其求导得

选取Lyapunov函数

对时间求导后得

为保证，设计虚拟控制Vd,i为

式中；K1i为待设计的正常数．定义速度跟踪误差，则有

对Z2i求导并将式(6)代入得

式中．定义Ui为

Uequ,i和Usmc,i分别表示等效控制和转换控制．其中Uequ,i设计为

式中是对复合干扰dVi的估计．

将式(28)和式(29)代入式(27)中，得

式中

设计基于快速终端滑模的控制方法为

式中1λi、2λi、1γi、2γi为正常数，且满足

定理 2 针对位置子系统(6)，若干扰观测器设计为式(15)，且参数选取合适，则在控制率式(28)、式(29)和式(31)的作用下，速度跟踪误差Z2i可以在有限时间收敛到 0，从而使得位置跟踪误差式(21)快速收敛到0．

证明 为证明Z2i可以快速收敛到 0，将式(31)代入到式(30)中得

选取Lyapunov函数

求导得

由第3.1节可知，可以有限时间收敛到dVi，表明干扰估计误差有界．参考式(20)证明过程可得

根据引理 1可知，在参数选取合适的条件下，速度跟踪误差Z2i可以在有限时间收敛到 0，此时式(26)更新为

故以上所设计的位置控制器能够保证该四旋翼的位置跟踪误差快速收敛到0．

由可得旋翼提供的升力uT,i为

并通过简单运算可得

对式(36)～式(38)进行代数运算，可得输入到姿态回路的给定滚转角φd,i和俯仰角θd,i分别为

式中ψi为第i架四旋翼的偏航角．

3.3 姿态控制器设计

针对姿态子系统数学模型式(7)以及式(41)、式(42)和Lψ组成期望的姿态角．由于直接对该虚拟控制量求导会产生微分爆炸问题，而且其抖振也会传递到其导数中，因此本文采用积分滤波器来避免这些问题．

参考文献[13]设计积分滤波器为

式中：ε1i、ε2i、ε3i是滤波常数；μ1i、μ2i、μ3i、Δ1i、Δ2i、Δ3i是设计参数．由文献[13]可知，ξ1i收敛到Θd,i，且 ξ2i和 ξ3i分别是和的估计值．定义滤波误差为

定义姿态角跟踪误差为

对时间求导后得

选取Lyapunov函数为

求导后得

为保证3＜0，设计虚拟控制Ωd,i为

式中：ξ2i是的估计值；K3i为待设计的正常数．定义速度跟踪误差，则

仿照位置控制器设计姿态控制器为

式中：3iλ、λ4i、γ3i、γ4i为正常数，且满足；dˆΩi是对复合干扰dΩi的估计，定义估计误差，则

由位置控制器的证明过程可知，式(52)、式(53)和式(54)设计的姿态控制器能够保证姿态角跟踪误差快速收敛到0．

3.4 编队稳定性分析

定义，则式(21)改写为

定义为编队系统的拉普拉斯矩阵，则式(55)可以写成向量形式为

式中，⊗表示 Kronecker积为单位矩阵．

由第 3.2节可知Z1可以快速收敛到 0，且，可得

定义，式(57)可以写为

根据文献[14]中引理 3可知，若编队系统通信拓扑图G是连通的，则H为非奇异矩阵，表明，即，从而实现编队的稳定．

4 系统仿真分析

为验证所提控制策略的正确性和有效性，下面进行仿真分析．图4给出了编队通信拓扑，其中0代表虚拟长机，1～4分别代表4架四旋翼从机．

图4 编队通信拓扑Fig.4 Formation communication topology

虚拟长机的轨迹为

期望偏航角为

仿真所用四旋翼飞行器参数如下：

仿真实验中，控制器各参数选择如下：

四旋翼飞行器初始状态如下：

期望偏差为

四旋翼飞行过程中受到的复合干扰选取为

结果分析如下．

(1) 从图 5可以看到，终端滑模干扰观测器(TSMDO)的三维估计误差可以快速收敛到 0，表明文中提出的干扰观测器满足系统要求．

图5 第1架四旋翼干扰估计误差Fig.5 Estimated error of disturbances for quadrotor 1

(2) 从图6和图7中可以看到，虚拟长机的轨迹是半径为15,m的圆，其余4架四旋翼从机由初始状态快速形成围绕长机的期望的正方形，并且保持该队形．为了便于观察队形的形成过程，图 7给出了 x-y平面的编队轨迹．可以看到，在t=3s时，编队的队形基本形成，并且一直保持该队形直到仿真结束，表明所设计的编队控制器可以保证编队系统的稳定性．

(3) 从图 8(a)和(b)中可以看到，俯仰角和滚转角跟踪误差可以快速收敛到0，从图8(c)可以看到偏航角可以快速收敛到长机的偏航角，表明所设计的姿态控制器可以使四旋翼的姿态角快速收敛到期望值，从而使整个编队系统实现稳定．

图6 编队轨迹三维效果Fig.6 Formation trajectory in three dimensions

图7 x-y平面编队轨迹Fig.7 Formation trajectory in x-y plane

图8 姿态角跟踪效果Fig.8 Attitude tracking effect

5 结 语

本文针对多四旋翼无人机编队，提出了一种基于干扰观测器和终端滑模的分布式编队跟踪控制算法而且虚拟长机的位置信息只有部分从机可以获得．利用快速终端滑模观测器来快速估计模型中的复合干扰，并在控制律中实时补偿，增强了系统的抗干扰能力．对于姿态控制的虚拟参考输入量，采用滤波器进行积分滤波，不仅避免了微分爆炸，同时也提高了系统稳定性．最后，仿真结果验证了综合控制策略的有效性．

参考文献：

［1］Kendoul F，Lara D，Fantoni I，et al. Nonlinear control for systems with bounded inputs：Real-time embedded control applied to UAVs[C]// IEEE Conference on Decision and Control. San Diego，CA，USA，2006：5888-5893.

［2］Lee D B，Nataraj C，Burg T C，et al. Adaptive tracking control of an underactuated aerial vehicle[C]//American Control Conference(ACC). San Francisco，California，USA，2011：2326-2331.

［3］Roldão V，Cunha R，Cabecinhas D，et al. A leaderfollowing trajectory generator with application to quadrotor formation flight[J]. Robotics & Autonomous Systems，2014，62(10)：1597-1609.

［4］Wang J，Xin M. Integrated optimal formation control of multiple unmanned aerial vehicles[J]. IEEE Transactions on Control Systems Technology，2013，21(5)：1731-1744.

［5］Turpin M，Michael N，Kumar V. Decentralized formation control with variable shapes for aerial robots[C]//IEEE International Conference on Robotics and Automation. Saint Paul，Mi nnesotas，USA，2012：23-30.

［6］Ramazani S，Selmic R R，de Queiroz M. Stabilization of non-planar multi-agent layered formations with double integrator model[C]// Control Applications IEEE. Sydney，Australia，2015：1386-1391.

［7］Dong X，Yu B，Shi Z，et al. Time-varying formation control for unmanned aerial vehicles：Theories and applications[J]. IEEE Transactions on Control Systems Technology，2015，23(1)：340-348.

［8］邢关生，杜春燕，宗 群，等. 基于一致性的小型四旋翼机群自主编队分布式运动规划[J]. 控制与决策，2014，29(11)：2081-2084.Xing Guansheng，Du Chunyan，Zong Qun，et al. Consensus-based distributed motion planning for autonomous formation of miniature quadrotor groups[J]. Control and Decision，2014，29(11)：2081-2084(in Chinese).

［9］李 磊，李小民，郑忠贵，等. 基于一致性理论的四旋翼无人机分布式编队控制方法[J]. 电光与控制，2015，22(10)：19-23.Li Lei，Li Xiaomin，Zheng Zhonggui，et al. Consensus based distributed formation control of quadrotor UAVs[J]. Electronics Optics and Control， 2015，22(10)：19-23(in Chinese).

［10］Ghommam J，Luque-Vega L F，Castillo-Toledo B，et al. Three-dimensional distributed tracking control for multiple quadrotor helicopters[J]. Journal of the Franklin Institute，2016，353(10)：2344-2372.

［11］Zuo Z. Trajectory tracking control design with commandfiltered compensation for a quadrotor[J]. Control Theory and Applications，IET，2010，4(11)：2343-2355.

［12］Moulay E，Perruquetti W. Finite time stability and stabilization of a class of continuous systems[J]. Journal of Mathematical Analysis & Applications，2006，323(2)：1430-1443.

［13］Li C Y，Jing W X，Gao C S. Adaptive backsteppingbased flight control system using integral filters[J].Aerospace Science & Technology，2009，13(2/3)：105-113.

［14］Hong Y，Hu J，Gao L. Tracking control for multi-agent consensus with an active leader and variable topology[J]. Automatica，2007，42(7)：1177-1182.