基于一致性的多无人机编队算法研究

高鹏程，刘小雄，黄剑雄，梁 晨

(西北工业大学 自动化学院，西安 710072)

0 引言

从无人机的出现到无人机技术逐渐趋于完善，已经走过了几十年的历程。随着技术的不断完善，对无人机提出的要求也在不断提高，任务的复杂程度也在不断上升，单个无人机的性能逐渐无法满足当前任务的需求。比如由于无人机的动力限制，它无法携带大量的侦察设备或者多种传感器，这可能会导致无人机功能有较大的局限性[1-2]；单个无人机执行任务的效率较低，一旦遭遇极端天气或者复杂地形等意外情况，可能会导致无法完成任务；另外如果需要对某一地区或物体进行测绘拍照，由于条件的限制，可能需要多次才能完成全部任务，这就会大大降低使用的效率，提高了运营成本。

针对以上种种情况，学者们研究通过多无人机编队飞行(Formation Flight)来解决上述问题[3]。通过多无人机编队飞行，可以大大提高无人机完成任务的效率，拓宽无人机的应用领域，从而实现安全、高效地执行任务。目前多无人机协同编队在军事和民用领域都有广泛的应用。

目前主流的编队控制算法包括长-僚机，虚拟结构法，人工势场法[4]，一致性算法[5]以及基于行为的蚁群蜂群等智能算法[6-7]。其中一致性理论由于其实用性强且成本低廉，易于进行工程实现，于是受到学者的广泛关注。1987年，Reynolds等人首次提出Boid模型[8]，设计了三条规则来模拟鸟群运动；1995年，Vicsek在Boid模型的基础上提出了“Vicsek”模型[9]；2004年，Olfati-Saber和Murry给出了连续时间内多智能体一致性问题的理论框架[5]，为未来的一致性编队奠定了方向；2007年，Li研究了有限时间多智能体的一致性编队情况，并应用于编队控制和避障的实验中[10]；2013年，Jakovetic对通讯时延和故障[11]做了深入研究。2019年，侯岳奇建立覆盖分布地图描述任务环境，提出了一种以覆盖率为搜索奖励的的集群协同区域搜索算法[12]。2020年，张佳龙针对“长机-僚机”编队模型提出了自适应队形保持的方法，减弱风场干扰的影响，实现编队稳定飞行[13]。

上述文献对无人机编队飞行控制方法进行了深入研究，基于上述研究基础，针对一致性算法能够实现状态以及行为一致的特点，改进了通信拓扑结构和由于加入了欧氏距离导致的系统不稳定状况，在此基础上设计了目标点跟踪的一致性算法，通过分段控制无人机加减速模态，从而保证无人机编队飞行的控制效果。

1 基于一致性的无人机编队建模

基于图论的设计理论，参考了基本一致性算法原理[14]，对一致性变量的边界问题进行平滑设计以及限幅处理，并在控制协议中加入了对目标点控制，通过分段规划获得最佳的控制效果，设计了一种基于一致性协议的分布式编队控制算法，实现无人机一致性的稳定控制。

1.1 图论基本原理

在理想情况下，无人机通讯链路就类似于一张网络，这张网络可以有方向，也可以无方向。而在数学中，就有专门以图或者网络为研究对象的学科，就是图论，它主要用来描述图中多个体的拓扑关系。

一个完整的图包含许多的概念和元素，而通常来说，一个由顶点集V(G)和边集E(G)组成的二元组就可以称之为一个图，一般将其命名为G。其中顶点集V=V(G)，包括图G中所有的顶点，|V|是图G的阶数。而边集E(G)中的无序数对表示图G中连接两个顶点的边。此外，边的权重W也是一个非常重要的图论的概念。在图论中边的长度一般不代表他的实际长度，而是由他的权重W来决定。边的权重代表这条边在实际中的长度或者实际含义，一般为正数，但也可以存在负数或者零。

一条边eij={(vi,vj)∈V}不仅可以有权重，也可以有方向。有方向的图被称为“有向图”，而与之相对的便是“无向图”。在“有向图”中，边eij表示无人机j可以获得无人机i的信息，反之却不行。而在“无向图”中，表示无人机i和j都可以获得对方的信息。此外，在“有向图”中还存在父子节点的概念，形如eij的边就表示，节点i是节点j的父节点，节点j是节点i的子节点。有向路径也就是由一系列形式为(v1,v2)，(v2,v3)，(vi,vj)的边所组成的，其中，vi,j∈V。在有向图中，圆是一个起点和终点是同一节点的有向路径。规定如果在有向图中，从某一个节点出发到其他的任意一个节点都存在至少一条有向路径，那么称该有向图为强连通图。邻接矩阵是表示图中各节点之间关系的矩阵，根据图论知识，给出有向图的邻接矩阵表达式：

A=[aij]∈Rn×n,aii=0,aij>0,(i≠j)

(1)

拉普拉斯矩阵表达式：

(2)

根据矩阵的定义能够知道，矩阵L具有如下性质：

(3)

定义1n表示所有元素都为1的n×1维列向量，0n表示所有元素都为0的1维列向量。那么根据拉普拉斯矩阵的定义有如下等式成立：L1n=0n。例如给定图形拓扑关系，如图1所示。

图1 图形拓扑关系

根据上述定义，图1的邻接矩阵和拉普拉斯矩阵分别表示为：

(4)

1.2 一致性编队控制协议设计

基于Reynolds等人提出的Boid模型，设计二阶一致性控制协议如下：

(5)

(6)

在一致性协议中的rij是一个单位矢量，沿着qi到qj连线，

(7)

定义的新的范数σ-范数如下，

(8)

其中:k和b两个参数的作用是调节位置控制量的变化快慢和响应速率，k∈(0,1)。该范数是向量先求范数之后进行了限幅的处理，无论无人机之间距离多远，该参数大小都不会出现突变，能够更好地调整位置控制量的大小和响应的稳定性。‖z‖σ用来构建多智能体编队的平滑结构和运动轨迹。当z=0时，‖z‖σ=0。在不同的位置，会选择不同的参数。

该范数由于参数选择的不同，可以产生不同的影响方式和大小，从而使编队控制的运动方式更加平滑高效。

为了平滑编队轨迹，定义如下的分段函数:

(9)

图2 fb(z)分段曲线

该函数被定义在[0,1]的范围内，并且在边界处渐进为零，在约束条件的作用下，一致性的协同变量可以在单位圆范围内调整，不会出现极端状况。因此将这个分段函数用于构建一致性协议的邻接矩阵。

当h=1时，该分段函数变形为一个单纯的脉冲函数，在[0,1]的区间内，函数值均为1，其余为0。利用该分段函数，定义邻接矩阵中的每个元素为:

Aij(q)=fb(‖qj-qi‖/R),j≠i

(10)

该邻接矩阵的作用范围只在范围为R的球形领域内，当二者的欧式距离超出该范围则邻接矩阵取零，而如果在范围内则不取零，矩阵的元素在[0,1]的范围内。对于任意两个被控对象，如果i=j则Aij(q)=0。最后所有的元素组成的矩阵就是该智能体系统的空间邻接矩阵，矩阵主对角线元素为0。

飞机的运动是依靠发动机的推力以及飞行时产生的空气动力完成的，通过作用于机翼上的力和力矩来改变飞行的姿态，所以可以将其近似看成是一种具有二阶微分的运动模态。综合以上，单架无人机的运动状态可以表示为：

(11)

之后将公式(5)改写成如下形式:

(12)

所以二阶动力学方程的矩阵形式为：

(13)

式中,

(14)

求Φ(t)的特征根为：

det(λ2In+(λ+ζ)Ln)

(15)

(16)

其中:ηi是拉普拉斯矩阵Ln(t)的特征值。

当Φ(t)有且仅有两个零特征根，并且其他非零特征根均具有负实部时，二阶编队系统达到一致性状态[15]。

1.3 基于目标的一致性编队算法设计

无人机编队能够执行多种任务，实现多种控制指令，而最基础的编队任务即为指向目标的无人机编队。

单纯的一致性编队只能保证多无人机能够在一致性指令的控制下，在一定时间内完成无人机的一致性编队，即位置和速度达到一致。而在添加了任务目标之后，重新设计了新的一致性控制协议：

(17)

(18)

qg表示目标点的位置，k和b是之前定义的控制参数。

(19)

(20)

dq表示无人机剩余的路程占总路程的百分比，当无人机还未到达该位置，他会先达到一致之后按照预设的速度前进；到达该位置后它会有一个减速阶段。

综合以上，加入了目标点的单架无人机的运动状态可以表示为：

(21)

1.4 一致性编队队形设计

为了能够令无人机编队保持固定的队形，需要设计一种利用几何方法表示队形的方式。本节利用了队形矩阵表示无人机的几何位置，以所有无人机的平均位置作为编队的中心位置，设计了队形矩阵。

无人机的中心位置为：

(22)

队形的结构模型是一个3行N列队形矩阵H:

(23)

N表示无人机的个数，第一行为无人机队形的序号，第二行和第三行分别表示无人机与编队中心位置X方向和Y方向之间的距离，以图3为例。

图3 菱形编队队形

该队形的队形矩阵为：

(24)

2 领航-跟随控制逻辑

领航跟随控制的基本方法是在编队中设定一个或几个领航者，其余成员作为跟随者，在编队航行的过程中领航者的作用是控制整个编队的行进路线，作为跟随者的成员与领航者之间保持一定的角度和距离便可以控制编队队形。其中领航者既可以是真实的个体成员，也可以是虚拟领航者。

图4 领航跟随示意图

根据领航-跟随的控制思想可知，在领航员的位置和航向角已知的情况下，只要设定跟随者相对于领航者的期望距离和期望相对角度，那么跟随者的位置期望值就唯一地被确定下来。无人机之间的距离以及相对角度可通过传感器数据计算获得，所以只要将两机间的实际距离和相对角度限定在期望值所允许的误差范围内,即可达到队形控制的目的。由于目前模型为质点模型，没有姿态信息，所以只要考虑二者之间的距离即可。

‖qleader-qfollower‖∈[a,b]

(25)

之后设计简单的L-F逻辑:

(26)

结合上文提出的一致性控制算法，设计了新的领航跟随控制算法

首先定义Leader和Follower的实时距离lerr=ql-qi，

(27)

d是L-F结构中设计的安全距离，保证L和F能够安全实现编队。

之后设计L-F的减速阶段，令跟随者能够和领航员保持一定的安全距离且速度保持一致:

(28)

B是一个交互矩阵，通过B来调整跟随者的控制顺序和信息是否交互。

上述公式就是领航-跟随的速度控制项，当L与F之间的距离大于5时，会以预设速度一直前进同时靠近领航员；当二者距离小于5时，会进入减速阶段，保证领航跟随结构的安全性。

综上，基于领航跟随算法的一致性编队控制算法如下：

(29)

领航跟随编队的具体步骤如下。

步骤1：设计领航员的控制方法和行进路径，根据信息交互矩阵B，选择合适的跟随者与领航员进行信息交互，将该跟随者命名为跟随者α。

步骤2：跟随者α基于领航跟随控制算法开始跟踪领航员，同时跟随者之间信息交互，获得彼此的位置和速度。多个跟随者从不同位置出发，以跟随者α为基准开始进行一致性编队。

步骤3：根据队形矩阵，分配给跟随者不同的位置，跟随者在跟踪领航员的同时逐渐实现编队一致性，速度和位置达到一致。

步骤4：当跟随者α与领航员的距离到达安全距离时，跟随者进入减速阶段，保证领航员与跟随者不会产生机间碰撞，消去跟随者由于速度过大导致的超调。

步骤5：当领航员与跟随者的距离达到预设距离时，编队系统达到动态稳定的状态，领航员与跟随者之间的距离保持一致，速度基本相同。

在编队任务的过程中，当领航员与跟随者距离大于安全距离时，始终基于公式(28)的二式进行计算，跟随者能够基于设定的安全速度，尽快的缩短与领航员之间的距离。之后将步骤1、2、3进行多次迭代。

当领航员与跟随着距离小于预设的安全距离时，会基于式(28)的一式进行编队控制，之后将步骤1～4重复迭代，最终实现无人机的编队。

3 仿真验证与分析

为了验证一致性控制协议的控制效果，进行一致性编队仿真验证，之后在此基础上加入领航跟随的思想，设计无人机从不同位置以不同速度出发，在运动过程中形成编队。

在仿真环境中，搭建编队协同控制模型，设计具体参数如下：k=0.5,b=1,c=15，d=2，k1=1.44，k2=0.32。

首先测试了基础的一致性编队算法，先采用了ode函数进行状态分析，认为设计初始状态，之后通过ode45函数求出对应时间的状态值。在函数里，将目标当前状态和时间区间(或步长)作为函数的输入值，将状态的微分作为函数的输出。之后根据目标的状态和邻接矩阵Aij(q)做矩阵运算。

实验中设置目标点为[10,10],无人机数量为4个，仿真时间为10 s，仿真步长为0.01，仿真曲线如图5所示。

图5 目标随时间的轨迹变化

图6 XY方向速度随时间的变化

图中“○”是旋翼无人机的初始位置，“☆”无人机的最终位置，在2 s时各无人机已经达到位置和速度一致，保持着1.5 m/s的速度前进，之后在5 s时开始陆续出现减速阶段。出现这种情况是因为不同的无人机与目标点的距离不同导致。最后成功到达目标点附近且速度基本为0。

之后以菱形队形作为编队队形进行仿真验证，目标点为[22.5,25]，仿真时间和步长不变。

如图7可知，无人机从不同的随机位置出发，在经过一定时间后，完成的菱形的编队队形。之后一直以该队形前进，仿真效果与预期基本保持一致。

图7 目标随时间的轨迹变化

最后设计了“一带四”的领航跟随任务，一个领航员，4个跟随者。设置领航员初始坐标为[5.7,9.2](随机生成)，用类正弦信号作为领航员的加速度输入，设置仿真时间为20 s，步长仍为0.01，仿真曲线如下所示。

从图8中可以看出，跟随者能够较好的跟踪领航员的轨迹，实现领航跟随的队形保持，在10 s后领航员和跟随者的距离基本保持在一定范围内。而在图10中，实线表示X方向的速度差，虚线表示Y方向的速度差，领航员与跟随者的速度差从2 s开始就保持在很小的范围内，且跟随者速度保持一致；在第12 s左右领航员与跟随者速度基本保持一致。

图8 目标随时间的轨迹变化

图9 领航者与跟随者位置误差

图10 领航员与跟随者速度差随时间的变化

4 结束语

基于一致性算法并对该算法进行了改进和平滑处理，从而设计编队控制律和领航跟随队形保持控制律。

首先对一致性算法进行改进，加入了邻域概念和区间分段函数，利用sigmod函数设计新的分段函数，使数据可以平滑过渡且在边界处渐进为0。之后将分段函数利用到无人机的信息控制交互范围内，设计了无人机信息交互邻域，从而定义了新的邻接矩阵Aij(q),当无人机之间的距离超出交互距离之后，无人机的通信交互逐渐过渡为0。在设计一致性编队的速度匹配项和位置控制项的参数时，定义新的范数形式σ-范数，将各项控制量的参数大小控制在一定范围内，保证编队的平滑和鲁棒性。最后基于领航跟随的思路设计了领航跟随队形保持算法，编队效果达到了预期目标，实现了一带四的领航跟随队形，且速度和位置能够保持高度的一致。由于尚未考虑编队的工程实现问题，之后将重点考虑编队无人机的工程实现可行性，开发工程样机进行物理仿真。