面向目标对峙跟踪的四旋翼协同编队控制方法

邵星灵，刘 俊，李东光

（1. 中北大学仪器与电子学院，太原030051；2. 中北大学仪器科学与动态测试教育部重点实验室，太原030051；3. 中北大学机电工程学院，太原030051）

1 引 言

四旋翼无人机利用其搭载的传感器对目标进行持续观测，可以获取大量目标信息，进而实现目标识别与精确定位，因而四旋翼无人机对峙跟踪地面目标在军事和民用领域具有很大的潜在应用价值［1］。然而，单架四旋翼在执行目标跟踪任务时由于其传感器探测范围有限、自身运动/动力学约束往往容易造成目标丢失、目标状态估计性能不佳等问题，难以确保目标跟踪任务的鲁棒性。使用多架四旋翼从不同角度以编队形式对目标进行环绕观测，通过邻居间的信息交互，实现目标观测性能协同超越，对目标可能逃逸的范围进行超前覆盖，可显著提高对目标运动状态的估计精度并降低目标丢失的概率，提高四旋翼执行跟踪任务的成功率［2-6］。

四旋翼对峙跟踪目标算法是根据四旋翼位置和目标位置的偏差，控制四旋翼飞行，使得四旋翼能够持续在目标附近进行跟踪［7］。Kim S 等［6］利用Lyapunov 导航向量场提出了一种利用多架无人飞行器对移动目标群进行协调对峙跟踪的方法，并且考虑了对峙跟踪距离改变的情况。季荣涛［8］将Lyapunov 导航向量场和人工势场法结合，在考虑避障要求的情况下，完成了目标对峙跟踪。Agate［9］利用切向量导航向量场和Lyapunov 导航向量场提出了一种动态路径规划算法，用于四旋翼跟踪地面目标。罗健［10］针对目标速度的影响，提出修正Lyapunov 导航向量场，完成了对地面移动目标的对峙跟踪。Lim S 等［11］则将李雅普诺夫向量场制导法应用于多无人机协同跟踪目标问题中。Ratnoo A［12］提出曲率约束的Lyapunov 导航向量场对峙目标跟踪方法，将参考点制导修正为四旋翼横向制导规律，利用非线性微分方程对四旋翼与目标之间的相对距离收敛过程进行建模。针对目标对峙跟踪问题，目前大部分都是在无人机质点模型的基础上建立的，对于动力学约束较少考虑，这也就导致大多数对峙跟踪算法只能应用于理想的质点模型因而实用性欠佳。此外，在对峙跟踪目标过程中由于无人机处于机动飞行状态，当多架无人机协同跟踪同一目标时，快速盘旋的机动飞行动作使得无人机难以确保机间避撞的安全性。

四旋翼协同编队飞行方式可以在对峙跟踪过程中使四旋翼之间保持稳定的相对距离从而避免相撞，同时具备扩展任务能力、扩大任务执行范围、增强高危环境作业能力、提升环境适应性等诸多优势。针对多四旋翼协同编队问题，Ghamry K等［13］研究了运用滑模控制算法实现对四旋翼位置和姿态的精准控制的多四旋翼协同编队问题，运用领航者-跟随者的编队控制算法得到了多四旋翼快速稳定编队的控制效果。Haibo D 等［14］研究了有限时间多四旋翼协同编队控制策略，确保了四旋翼在较短时间内收敛到期望队形并跟踪指令轨迹。由于四旋翼的空气动力学参数难以用测量仪器精确获得，给模型带来了较大参数不确定性。此外，四旋翼空间飞行过程中不可避免遭受外部未知风扰的影响，给四旋翼控制器设计带来了困难［15-18］。因此，需要在考虑编队几何构型约束和参数不确定性以及外部干扰的情况下，构造适用于四旋翼典型动力学特征并具有强鲁棒性的多四旋翼协同编队控制方法。针对存在外部干扰的多四旋翼编队飞行控制问题，Wesam J 等［19］提出了基于次优H∞的四旋翼编队抗干扰控制方法，具有良好的稳定性和鲁棒性。目前对于四旋翼编队抗干扰控制方法的研究屈指可数，因此迫切需要设计先进的抗干扰控制策略以适应四旋翼编队飞行的实际需求。

本文针对多四旋翼编队飞行过程中对地面目标对峙跟踪、几何队形生成、稳固保持和协同抗干扰问题，研究了一种可应对外部环境干扰和气动参数不确定性的多四旋翼主从式协同对峙跟踪目标方法。首先，利用Lyapunov导航向量场使领航者四旋翼对峙跟踪地面目标；其次，在四旋翼轨迹回路中引入二阶一致性理论，结合代数图论和通信拓扑，以领航者位置作为动态编队的几何中心，将多四旋翼期望几何队形生成和保持问题转化为跟随者位置一致性跟踪与领航者的相对位置偏差问题，从而构造多四旋翼分布式位置保持控制器；最后，对于姿态回路的参数不确定性和外部干扰，借鉴自抗扰控制思想，将参数不确定性和外部干扰视为集总扰动，采用扩张状态观测器对其进行在线观测与补偿，实现对给定姿态的高精度跟踪控制。所提方法可以在四旋翼编队持续对峙跟踪目标的同时极大改善四旋翼编队系统的抗干扰能力，提升干扰环境下多四旋翼编队几何构型的稳固性和飞行的安全性。

2 问题描述与数学模型

根据多四旋翼无人机协同编队对地面目标对峙跟踪问题需求，设计如图1所示控制器结构框图。针对多无人机对地目标对峙跟踪问题构建了基于李雅普诺夫向量场，综合航向和相位控制，使得无人机以固定对峙半径飞行到以目标为中心的上空的圆环区域内，确保了无人机与目标跟踪的连续性；以多智能体二阶一致性算法为理论基础结合四旋翼无人机动力学模型建立了基于虚拟领导者航迹的主从式有限时间协调编队控制模型；考虑到四旋翼无人机具有大的不确定性和时变且有外部扰动，引入了线性自抗扰控制器，所设计的控制器具有强鲁棒性、抗干扰性，对未知的外界扰动和内部不确定性有良好的控制效果，实现了可适应不同目标运动轨迹的高抗扰、强稳定多无人机协同编队对峙跟踪。

图1 控制器结构框图Fig.1 Structure diagram of the control scheme

定义四旋翼编队数量集合Γ =(1，2，…，n)，i ∈Γ，给出n 架四旋翼组成编队中的第i 架四旋翼运动学/动力学模型［20-21］：

其中，mi为第i 架四旋翼的质量，t 为时间，Gi=[0，0，mig]T，g为重力加速度，Ji= diag(Ji，1，Ji，2，Ji，3)∈R3×3表示一个正定的对角惯性矩阵，Ji，1、Ji，2、Ji，3分别是第i架四旋翼在惯性坐标系下沿x，y，z 轴的转动惯量。Θi=[φi，θi，ψi]T分别表示第i 个四旋翼在惯性坐标系下的位置矢量和在机体坐标系下的姿态角。Π1，i= diag(kx，i，ky，i，kz，i)，Π2，i= diag(kφ，i，kθ，i，kψ，i)是第i 个四旋翼的空气阻尼矩阵，kr，i∈R，r = x，y，z，φ，θ，ψ 是第i 架四旋翼的空气阻力系数。控制输入ui∈R 为第i 架四旋翼的拉力，τi=[τx，iτy，iτz，i]T为绕机体x，y，z 轴的三个控制力矩。

g1，i=[c(ψi) s(θi) c(φi)+ s(ψi) s(φi)，s(ψi) s(θi)c(φi)- c(ψi)s(φi)，c(θi)c(φi)]T表示与姿态相关的位置回路输入矩阵，s(·)与c(·)分别表示正弦函数和余弦函数。g2，i= diag(li，li，ci)∈R3×3，其中li是螺旋桨到四旋翼质心的几何距离，ci是力矩系数。dΘ，i(t) =[dφ，i，dθ，i，dψ，i]T表示姿态回路中的有界外部干扰。

接下来，为便于后续位置控制器和姿态控制器的构造，引入如下符号定义：

其中，Fi=[Fi，x，Fi，y，Fi，z]T∈R3×1表示虚拟控制输入量。d4，i为姿态回路的集总扰动，包含外部扰动和参数不确定性的综合影响。δ1，i、δ2，i是第i架四旋翼的参数化不确定性矩阵分别是Π2、g2，i的标称值。

借助上述中间变量，可将四旋翼运动/动力学模型式（1）改写为如下严格反馈形式：

3 基于Lyapunov导航向量场的四旋翼对地面目标跟踪方法

假设四旋翼的实时位置为P0=[x0，y0]T，目标位 置 为Pt=[xt，yt]T，设Lyapunov 函 数 为V =(r2-其 中是四旋翼和目标之间的几何距离，R0是对峙跟踪半径，xr、yr为四旋翼与地面目标沿x 轴、沿y 轴的相对位置分量。由李雅普诺夫导航向量场可得到无人机期望速度：

其中，gx、gy分别为地面目标静止时四旋翼在惯性坐标系下沿x 轴、y 轴的期望速度分量，k = v0r(r2+为四旋翼的巡航飞行速度。α 为期望速度的修正因子，它满足：

当无人机的速度在有效范围内，α必有正数解。

在Lyapunov 导航向量场的作用下，四旋翼无人机将航行到以目标上空为中心的极限环上，在对目标持续跟踪的同时环绕目标飞行。

4 基于二阶一致性的四旋翼无人机主从式编队控制方法

4.1 多四旋翼通信拓扑和编队样式设计

第i 架和第j 架从机相对于领航者的位置矢量为Δi=[Δi，x，Δi，y，Δi，z]T和Δj=[Δj，x，Δj，y，Δj，z]T，则第i架和第j架四旋翼从机间的相对位置偏差可由如下等式Δij= Δi- Δj=[Δi，x，Δi，y，Δi，z]T-[Δj，x，Δj，y，Δj，z]T=[Δij，x，Δij，y，Δij，z]T描述。

以三架四旋翼的几何中心为虚拟主机，以三架四旋翼为顶点的几何图形为边长m 的正三角形。为达到正三角形的要求，设计如下的相对坐标分别为从机1、2、3相对于主机0的坐标。

4.2 多四旋翼通信拓扑和编队样式设计

领航者轨迹运动信息可由Lyapunov 导航向量场产生：

其中，xd、yd、zd分别为领航者在惯性坐标系下沿x，y，z 轴的位置分量，分别为领航者在惯性坐标系下沿x，y，z轴的速度分量。

由于领航者定高飞行，故领航者在z 轴的速度分量为0，并且领航者在z轴的位置分量为5m。

由式（1）可得第i架四旋翼的位置动力学模型：

根据上述第i 架四旋翼的位置动力学模型，不难选择控制量，接下来设计位置保持控制器：

其中，Fi，x、Fi，y、Fi，z均为虚拟控制输入。

那么，位置动力学模型可如下表示：

根据多四旋翼主从式编队通讯拓扑和期望领航者轨迹信息，结合多智能体二阶一致性原理，构造如下虚拟控制输入(Fi，x，Fi，y，Fi，z)T：

其中，k1、k2表示待设计的控制器参数，Ni⊆Γ 且i ∉Ni，xi为第i架四旋翼在惯性坐标系下沿x轴的位置分量，xj为第j架四旋翼沿x轴的位置分量，vi，x为第i架四旋翼在惯性坐标系下沿x轴的速度分量，vj，x为第j架四旋翼惯性坐标系下沿x轴的速度分量。为领航者在惯性坐标系下沿x 轴，y 轴，z 轴的加速度分量。

在获得上述虚拟控制输入(Fi，x，Fi，y，Fi，z)T的基础上，结合式（2）进行逆动力学解算，可得到如下的期望姿态角指令

其中，ui为四旋翼的期望拉力，分别为机体坐标系下的期望滚转角、期望俯仰角以及期望偏航角。

5 基于线性自抗扰控制的多四旋翼姿态跟踪控制器构造

自抗扰控制不依赖于模型精度，适用于针对存在参数不确定性和外部干扰的四旋翼模型构造姿态跟踪控制器。线性自抗扰控制器由PD 和线性扩张状态观测器（LESO）两部分构成，其设计过程简洁，且便于参数整定。

由式（1）可知四旋翼姿态动力学模型：

设一个二阶被控系统表达式为：

其中，f为系统整体扰动，是系统模型参数，u是系统输入，x为系统输出。

线性扩张状态观测器（LESO）表达式为

其中，zi是对输出xi的估计，z2是z1的导数，z3是对系统总扰动f 的估计，β1、β2、β3是观测器的增益，β1=3ω0，β2=，β3=，ω0为观测器带宽。

线性状态误差反馈（LSEF）为：

当z3对系统总扰动的估计误差忽略不计，系统就可以简化为双积分串联型，即：

从而线性状态误差反馈（LSEF）控制律的表达式为：

其中，xd为期望，kp和kd是控制器增益，kp=，kd=2ξωc，ωc是闭环系统的自然频率，阻尼系数ξ通常取1，且ω0与ωc成比例。

对四旋翼动力学方程进一步整理可得

在本文中，设控制量τi为：

其中，kp=为正的控制器比例增益，kd= 2wc为正的控制器微分增益，wc为姿态回路的控制器带宽，为姿态回路集总干扰的估计值，可由如下仅依赖于控制输入力矩和姿态角量测输出的模型辅助扩张状态观测器给出：

其中，z1，i是对x3，i的估计，z2，i是对x4，i的估计，z3，i是对的估计，wo为姿态回路观测器带宽。

6 仿真实验

为验证上述四旋翼无人机自抗扰控制算法的有效性，进行仿真实验。四旋翼无人机的质量m=2kg，重力加速度g=9.8m/s2，电机到质心的距离l=0.4m，力矩系数c=0.05。阻尼矩阵为：

惯性矩阵为：

考虑一个以三架跟随者四旋翼无人机和一个虚拟领航者组成的网络系统，无人机间的信息交流可描述为图2 所示的无向拓扑图，各边的权值为a12= a21= 1，a23= a32= 1，b1= 1。气动阻尼系数Ki，j= 0.01，i =1，2，3，j = 1，2，3，4，5，6。编队控制器增益为α1= 0.8，α2= 0.9，k1= 1.7，k2= 1.7。

相对位置偏差为：

图2 无人机三角形编队通信拓扑Fig.2 Communication topology of UAV triangle formation

各四旋翼无人机的初始条件为：

各四旋翼无人机由不同的初始条件以三角形编队的形式对起始点位于(0，0)的机动目标展开对峙半径为2m 的环绕跟踪，目标运动方程为：vxt=0.5，vyt= 0。

图3为三维坐标系下多四旋翼无人机协同编队跟踪地面机动目标的响应曲线，三架无人机从不同的初始位置出发，经过一段时间后，在形成稳定三角形编队队形的同时对一运动目标展开对峙半径为2m的环绕跟踪。图4为x-y平面下多四旋翼无人机协同编队跟踪地面目标的相应曲线，从图中可以看到无人机在跟踪目标的过程中始终保持着稳定的三角形队形。

图5 为各四旋翼无人机x、y、z 方向的位置响应曲线，图6为各四旋翼无人机x、y、z方向的速度响应曲线，图7 为各四旋翼无人机横滚角、俯仰角、偏航角的姿态响应曲线，可以看到，无人机在有限时间内形成了三角形队形并完成对期望轨迹跟踪，保持了良好的一致性。

图3 三维坐标系下多四旋翼无人机跟踪地面目标图Fig.3 Tracking performance of the ground targets under 3D coordinate system

图4 x-y平面下多四旋翼无人机跟踪地面目标Fig.4 Tracking performance of the ground targets under x-y coordinate system

图5 四旋翼无人机位置响应曲线Fig.5 Position response curve of quadrotors

图6 四旋翼无人机速度响应曲线Fig.6 Velocity response curve of quadrotors

图7 四旋翼无人机姿态响应曲线Fig.7 Attitude response curve of quadrotors

7 结 论

本文针对多源干扰影响下多无人机主从式对地目标对峙跟踪问题，构建了基于Lyapunov 导航向量场的多无人机对地目标对峙跟踪策略，使得多无人机以固定对峙半径收敛于目标上空的极限环，确保了无人机对目标跟踪的连续性；以自抗扰控制和多智能体二阶一致性算法为理论基础，建立了基于虚拟领导者航迹的主从式有限时间协调编队控制模型，可在外部干扰以及参数不确定性的影响下，提升多四旋翼编队队形的稳固性，提高多四旋翼编队系统的抗干扰能力。