CN115047910A - 一种基于雁形阵的无人机编队巡航控制方法 - Google Patents

Abstract

本发明公开了一种基于雁形阵的无人机编队巡航控制方法。首先对无人机系统编队巡航问题进行数学建模。随后，设置领航者和跟随者，基于一致性的编队控制算法进行大雁形编队控制过程。接着，利用人工势场法构建虚拟势场，使得无人机根据势场值的变化决定飞行的方向和速度，进行避障。最后，重复以上步骤，使得无人机可以到达多个目标点，实现编队巡航目的。

Description

一种基于雁形阵的无人机编队巡航控制方法

技术领域

本发明涉及一种基于雁形阵的多无人机编队巡航控制技术，将无人机系统建立为一个单向通信拓扑模型，并用人工势场法实现路径规划，属于多智能体编队控制领域。

背景技术

近几年，无人机得到广泛应用，在情报侦察、目标搜索和跟踪以及目标攻击等各种任务中占据关键位置。单架无人机虽然具有机动性强、维护简单等优点。但依然存在侦查视野有限，完成任务的上限较低等缺点。随着任务和环境更加复杂，单个无人机无法轻松地完成指令任务，由此，无人机群诞生，且在未来市场占据重要位置。无人机集群既能形成协调有序的集体运动模式，又能快速、一致地应对外界刺激，具有自组织性强、协调性高、稳定性强等优点，且对环境具有较强的适应能力。

无人机编队控制问题是当前无人机集群领域的一个研究热点，在21世纪，编队飞行的研究随着现代控制方法的更新而逐步趋于与先进的控制方法结合。通过编队控制可以充分利用有限的单机资源,使无人机共同执行复杂任务。

大雁是一种常见的大型候鸟，每年都会进行大规模、长时间的迁徙。在迁徙过程中，雁群会形成“一”型或“V”型队形在空中飞行。而雁形阵则是通过模仿雁群行为的一种阵形。雁形阵是一种横向展开，左右两翼向前或者向后梯次排列的战斗队形，具有很好的稳定性和高效性。

在无人机编队控制领域中常见的方法有领航者-跟随者法，人工势场法，虚拟结构法，图论法等等。将领航者-跟随者法，人工势场法以及图论法三者结合可以弥补各自在某些方面的不足，并将其运用到编队飞行中能有效地提升编队控制，避障以及巡航的效果，具有一定的应用前景与意义。

发明内容

技术问题：

多无人机的编队控制主要研究多无人机在复杂环境下的定位、队形生成、队形保持、编队巡航和避障等问题。本发明专利实现了对由领航者和跟随者形成的多无人机系统进行编队巡航及避障，主要用基于一致性的编队控制方法结合人工势场法解决了多无人机系统的队形保持和避障问题，并且实现了雁形阵的编队巡航。

技术方案：

一种基于雁形阵的无人机编队巡航控制方法，其特征在于，包括以下步骤：

步骤1：建立多无人机系统的数学模型；

步骤2：设定无人机系统的航向角度及协同编队形成雁形阵达到稳定时各无人机之间的相对位置关系；

步骤3：基于一致性的编队控制算法进行雁形阵编队控制过程；

步骤4：利用人工势场法构建虚拟势场，使得无人机根据势场值的变化决定航向角度和速度，进行避障；

步骤5：重复步骤2-4，使得无人机可以到达多个目标点，实现巡航目的。

进一步地，步骤1所述建立无人机编队系统的数学模型具体包括：

步骤1.1建立多无人机系统编队通信结构和绝对坐标系

一个多无人机系统中含有n架无人机，用有向图G＝{V，E}表示多无人机系统编队通信结构，该结构为单向通信，即信息只能由父节点传递至子节点，选定系统中的一个无人机为leader，剩下的无人机为follower，设A＝{a_iji，j∈N}为该系统连通图的邻接矩阵，其中a_ij＝1表示第i个无人机可以与第j个无人机交换信息，否则a_ij＝0，由此构建该多无人机系统的编队通信结构和绝对坐标系；

步骤1.2定义速度更新和位置更新模型

leader的速度计算：

(3)引力影响：V_x1＝K_Nd₁ cosθ，V_y1＝K_Nd₁ sinθ

(4)斥力影响：当障碍物进入无人机探测范围

V_x＝V_x1+βR_x

V_y＝V_y1+βR_y

其中，V_x为领航者在x方向由引力影响的速度分量，V_x1为领航者在x方向合速度，V_y为领航者在y方向合速度，V_y1为领航者在y方向由引力影响的速度分量，d₁为领航者与下一目标点的欧式距离，θ为目标点和领航者之间的连线与x轴夹角，R_x为领航者在x方向由斥力影响的速度分量，R_y为领航者在y方向由斥力影响的速度分量，x_l为领航者的x坐标，

为领航者与第j个障碍物之间x坐标之差，y_l为领航者的y坐标，

为领航者与第s个障碍物之间y坐标之差，N为障碍点的个数，K_N、β为常量，d_s为领航者与第s个障碍物之间的欧氏距离，d_M为无人机的探测距离；

follower的速度计算：

首先要根据一阶编队的协同原理计算leader与follower的误差信息并施加给follower的速度，使其向协同的位置运动，然后计算检测范围内障碍物对机器人的影响，两者的速度加权在一起即为follower的速度；

位置更新：

x'＝x+d_t*V′_x

y′＝y+d_t*V′_y

其中，x'为更新后x坐标，x为更新前x坐标，d_t为控制周期，V′_x为更新后x方向上的速度,y′为更新后y坐标，y为更新前y坐标，V_y′为更新后x方向上的速度；

步骤1.3定义运动约束

定义无人机运动约束常量：目标点位置信息goal、速度上限V_max、控制周期d_t、障碍物的位置信息ob_temp、机器人的探测距离d_M以及通信连接的最大距离d_max；

根据无人机编队巡航系统的动力学特性，运动约束定义为：

由

V_y同理，由

其中，V_x2为定义x方向速度约束的中间变量，V_x0为更新前x方向的速度，ΔV_x为理论计算y方向上的下一速度与当前速度的差值，ω_x为角速度，V_xmax为给定的x方向最大速度值，V_y2为定义y方向速度约束的中间变量，V_y0为更新前y方向的速度，ΔV_y为理论计算x方向上的下一速度与当前速度的差值，ω_y为角速度，V_ymax为给定的y方向最大速度值；

当leader与目标点欧氏距离小于0.005即可向下一目标点行进。

进一步地，步骤2的具体包括方法包括如下步骤：

步骤2.1确定无人机系统的航向角度

设雁形阵中轴线与水平方向的夹角为航向角度α，则

其中，(x_m，y_m)表示在所建立的绝对坐标系下第m个目标点的位置，(x_l，y_l)表示leader的位置；

步骤2.2设定无人机系统协同编队形成雁形阵达到稳定时各无人机之间的相对位置关系

根据leader与目标点的相对位置将无人机系统分成两类模型，并设定无人机系统协同编队形成雁形阵达到稳定时follower与leader的相对位置关系；

(1)当目标点在leader左侧时，设定无人机系统协同编队形成雁形阵达到稳定时follower与leader的相对距离为：

其中，x_lf为第f个follower与leader的水平距离差，y_lf为第f个follower与leader的垂直距离差，

表示雁形阵两翼与中轴线之间的夹角，k为常数，d表示follower与leader在形成雁形阵时的相对距离，当follower在中轴线左侧时取加号，在中轴线右侧时取减号；

(2)当目标点在leader右侧时，设定无人机系统协同编队形成雁形阵达到稳定时follower与leader的相对距离为：

当follower在中轴线左侧时取加号，在中轴线右侧时取减号。

进一步地，步骤3的具体包括方法包括如下步骤：

步骤3.1设计编队系统的一阶连续系统模型

编队系统的一阶连续系统模型为：

其中，x_i∈Rⁿ表示无人机i的状态量，u_i∈Rⁿ表示无人机i的输入量，这里的n表示状态量的维度。

在理想状态下，控制输入量为：

其中，a_ij为编队邻接矩阵的元素，N_i为无人机i的邻居集合；

再另全局状态量x＝[x₁，…，x_n]^T∈Rⁿ得到全局动态关系为：

其中，D＝diagd_i和L＝D-A分别为编队拓扑通信中的对角入度矩阵和拉普拉斯矩阵，A为……，从上式知，拉普拉斯矩阵及特征值在无人机编队的稳定性和稳定速度上扮演着重要角色，其可决定编队的闭环动态特性；

步骤3.2构建生成树，使得leader为整个通信拓扑结构中唯一的根节点

固定拓扑下，一阶多无人机系统的一致性问题具有稳定收敛性解的重要条件是系统的网络拓扑存在一颗生成树，且要使多无人机系统形成雁形阵，就需要每个无人机的航向角度和速度大小最终收敛为同一预定值，而只要leader为整个通信拓扑结构中唯一的根节点，那么总能实现所有的follower都向其收敛，

当这样的最小生成树形成时，上式中-L的特征值均位于复平面左半平面，保证无人机系统能够达到一致性，则最终状态值为：

其中，C为最终状态值，p_i为无人机i在所建坐标系下的位置，n为无人机的个数；

步骤3.3构建leader和follower的控制算法

采用leader-follower中的分布式控制结构，通过控制leader和follower之间的相对距离、速度和航向角度，使其达到一定的稳定状态，从而实现大雁形编队；

(4)在离散时间下，编队系统的一阶连续系统模型可化为：

x_i(k+1)＝x_i(k)+u_i(k)

其中，x_i(k+1)为k+1时刻第i个无人机的状态量，x_i(k)为k时刻第i个无人机的状态量，u_i(k)为时刻第i个无人机的输入量，

(5)理想状态下，follower的控制算法为：

其中，ε＞0，，a_ij为编队邻接矩阵的元素，r_ij(k)为k时刻无人机i和无人机j之间的相对距离，即follower的速度受到与其他无人机的影响；

(6)leader的控制算法为：

其中，a和b为常数，d(k)为k时刻leader与目标点的距离，即leader的速度受到follower和目标点的影响，u_l(k)为k时刻leader的输入量，r_lj(k)为k时刻leader与第j个follower的相对距离，a_lj为leader在编队邻接矩阵中所对应的元素。

进一步地，步骤4的具体方法包括如下步骤：

步骤4.1设计单个无人机与目标点的人工势场：

(1)设计引力场函数

假设无人机的坐标为x，目标点的坐标为x_d，则可以得到两者的引力场函数：

其中，U_d(x)为引力场，||·||表示范数，用于度量某个向量空间中的某个向量的长度或大小；

由公式

可得引力为

其中ω为增益系数且ω＞0，F_d为无人机受到的引力，引力的方向为无人机指向目标点；

(2)设计斥力场函数

斥力函数也受无人机与障碍物的距离影响。当无人机与障碍物距离越近则斥力势能越大，越远则斥力势能越小，而若无人机与障碍物的距离达到一定的程度，斥力势能则会为零；所以设计障碍物斥力场函数需要合理设计斥力场的大小，不能太大否则会影响无人机的行进路线，也不能太小否则会导致无人机与障碍物存在距离过近而相撞的风险；

由此斥力场函数设计为：

其中ε为斥力增益系数，c为一个正的常数，表示无人机与障碍物的最大作用距离。D(x)表示无人机与周围离它最近的障碍物之间的欧氏距离，U_r(x)为斥力场；

由此可得斥力为：

其中，F_r为无人机受到的斥力，

为求导；

(3)设计总势场函数

最终的全局总势场函数U(x)为引力场函数与斥力场函数的矢量相加，无人机受到的合力F为引力与斥力的矢量相加

U(x)＝U_d(x)+U_r(x)

无人机的运动由此合力决定；

步骤4.2解决传统人工势场法目标点不可达的问题

在实际运动环境中，当障碍物处于无人机的运动路线上而导致无人机所受目标点的引力和障碍物的斥力相等且方向相反时，无人机受到的合力为零，无人机则会在原地停止运动或者原地徘徊从而陷入局部陷阱无法到达最终目标点；

修改斥力场函数：

修改后的斥力场函数可以使无人机在最终目标点处斥力势能值最小，从而解决局部最小值问题，

则修改后的斥力函数为：

步骤4.3考虑无人机内部的避碰

当考虑无人机编队时，在编队内部各无人机可通过分布式通信拓扑实现局部的信息交互，避免碰撞并保持队形，由此还需设计编队内部各无人机之间的势场，即编队中单个无人机受到来自目标点的引力，外部障碍物的斥力和内部相邻无人机的编队力，故其受到的力为三者的合力；

当某一无人机检测到障碍物的同时，编队中与其相邻的无人机也在该无人机的斥力范围内，这就需要该无人机同时进行障碍物的避障和与其他无人机的避碰，使得无人机同时进行避障和避碰的思路是：

某时刻一无人机同时受到来自障碍物的斥力和相邻无人机的斥力，当该无人机与其相邻无人机距离较近以至于其受到的来自相邻无人机的斥力大于来自障碍物的斥力时，无人机所受的合力将会使其优先避免与相邻无人机发生碰撞，而当该无人机与相邻无人机的距离拉开之后，虽然仍然受到来自相邻无人机的斥力，但该力却比来自障碍物的斥力要小，所以在合力的作用下无人机将会选择远离障碍物飞行，由此即可实现同时避障与避碰。

有益效果：

1.本发明专利提出了一种基于雁形阵的无人机编队巡航控制方法及系统。在无人机编队系统的数学模型下，利用一致性的编队控制算法实现大雁形编队控制过程，使得领导者和跟随者形成一个固定的队形并前进。一个用于多智能体系统仿真的虚拟平台，提供了一个完整的仿真框架；plot等matlab自带函数用于生成无人机运动图画，显示多智能体的动态运行情况。

2.本发明专利提出了一种基于雁形阵的无人机编队巡航控制方法及系统，其特征在于：利用拓扑结构创建N个作为智能体的无人机，将其分为领导者和跟随者，并设置其初始信息(包括x,y坐标以及航向角度)和相对位置关系。运行时会在每次计算速度后更新当前无人机位置，并且每次更新后运行已经配置好的画图函数，从而获得多智能体的运动动画。在无人机停止巡航后，画图显示出无人机的运动轨迹。建立数学模型编写相应算法，用于领导者到达目标点的过程，从而完成雁形阵的刚性编队巡航功能。

3.本发明专利采取雁形阵进行巡航控制，该阵型是一种横向展开，左右两翼向前或者向后梯次排列的战斗队形，具有很好的稳定性和高效性。该阵型可以保护两翼和后方的安全，防止敌人迂回，达到相互协作，相互侦测的作用，可以提高胜率。由于分布空间较大，从而提高巡航效率。

4.本发明专利运用一致性的编队控制算法，利用单向通信的有向图建立系统的绝对坐标系，对无人机的刚性编队问题进行创新研究，提高了编队的效率，实现了自主编队。

5.本发明专利通过建立数学模型以达到改变航向，编队巡航的目的。该方法可以在保持雁形阵的前提下，移动距离更短，到达目标点更快，从而提高移动效率。

6.本发明专利利用人工势场协同避障算法，构建虚拟势场并利用分布式通信拓扑的方法实现局部的信息交互，使得无人机根据势场值的变化决定飞行的方向和速度，进行避障的同时避免了内部碰撞。

7.本发明专利所建立的编队模型不仅可以用于无人机的编队巡航，也可用于其他场景，比如无人车、水下机器人等。

附图说明

图1为本发明专利总体框架图；

图2为本发明专利的整体程序流程图；

图3为多无人机系统(UAVs)的绝对坐标系；

图4为由五个无人机构成的多无人机系统的编队通信结构

图5为模拟仿真雁形阵编队效果图

图6为多无人机系统(UAVs)进行遇到障碍物进行避障和避碰的流程图

图7为模拟仿真雁形阵编队巡航无人机运动轨迹图

具体实施方式

下面对本发明方法和系统进行详细的阐述。

本发明专利是一种基于雁形阵的无人机编队巡航控制方法，其特征在于，包括以下步骤：

步骤1：建立多无人机系统的数学模型。包括建立该系统的编队通信结构和绝对坐标系，建立速度更新和位置更新模型以及进行无人机运动约束。

(1)建立多无人机系统编队通信结构和绝对坐标系

一个多无人机系统(UAVs)中含有n架无人机，用有向图G＝{V，E}表示多无人机系统编队通信结构，该结构为单向通信，即信息只能由父节点传递至子节点。选定系统中的一个无人机为leader，剩下的无人机为follower。设A＝{a_iji，j∈N}为该系统连通图的邻接矩阵，其中a_ij＝1表示第i个无人机可以与第j个无人机交换信息，否则a_ij＝0。由此构建该多无人机系统的编队通信结构和绝对坐标系。

该系统的绝对坐标系如图3所示。

(2)建立速度更新和位置更新模型

leader的速度计算：

①引力影响：V_x1＝K_Nd₁ cosθ，V_y1＝K_Nd₁ sinθ

②斥力影响：当障碍物进入无人机探测范围

V_x＝V_x1+βR_xi

V_y＝V_y1+βR_yi

follower的速度计算：

首先要根据一阶编队的协同原理计算leader与follower的误差信息并施加给follower的速度，使其向协同的位置运动，然后与leader类似，需要计算检测范围内障碍物对机器人的影响，两者的速度加权在一起即为follower的速度。

位置更新：

x＝x+d_t*V_x

(3)定义运动约束

定义一些无人机运动约束常量：目标点位置信息goal、速度上限V_max、控制周期d_t、障碍物的位置信息ob_temp、机器人的探测距离d_M以及通信连接的最大距离d_max。

根据无人机编队巡航系统的动力学特性，运动约束定义为：

由

V_y同理，由

步骤2：设定无人机系统的航向角度及协同编队形成雁形阵达到稳定时各无人机之间的相对位置关系。

(1)确定无人机系统的航向角度

设雁形阵中轴线与水平方向的夹角为航向角度α，则

(2)设定无人机系统协同编队形成雁形阵达到稳定时各无人机之间的相对位置关系

根据leader与目标点的相对位置将无人机系统分成两类模型，并设定无人机系统协同编队形成雁形阵达到稳定时follower与leader的相对位置关系。

①当目标点在leader左侧时，设定无人机系统协同编队形成雁形阵达到稳定时follower与leader的相对距离为：

当follower在中轴线左侧时取加号，在中轴线右侧时取减号。

②当目标点在leader右侧时，设定无人机系统协同编队形成雁形阵达到稳定时follower与leader的相对距离为：

当follower在中轴线左侧时取加号，在中轴线右侧时取减号。

以五个无人机为例，选定第五个无人机为leader，所预想的最终形成的雁形阵如图4。通过matlab的plot函数进行画图的效果如图5，其中以无人机的位置为圆心，适当的距离为半径画圆，圆面积部分为无人机的探测范围，并标注无人机的航行方向。

步骤3：基于一致性的编队控制算法进行雁形阵编队控制过程；

(1)设计编队系统的一阶连续系统模型

编队系统的一阶连续系统模型为：

在理想状态下，控制输入量为：

全局动态关系为：

其中，拉普拉斯矩阵及特征值在无人机编队的稳定性和稳定速度上扮演着重要角色，其可决定编队的闭环动态特性。

(2)构建生成树，使得leader为整个通信拓扑结构中唯一的根节点

构建以leader为整个通信拓扑结构中唯一的根节点的生成树，实现所有的follower都向其收敛，使得每个无人机的航向角度和速度大小最终收敛为同一预定值，从而使得多无人机系统(UAVs)形成稳定雁形阵。

最终状态值为：

(3)构建leader和follower的控制算法

采用leader-follower中的分布式控制结构，通过控制leader和follower之间的相对距离、速度和航向角度，使其达到一定的稳定状态，从而实现大雁形编队。

在离散时间下，编队系统的一阶连续系统模型可化为：

x_i(k+1)＝x_i(k)+u_i(k)

理想状态下，follower的控制算法为：

follower的速度受到与其他无人机的影响。

leader的控制算法为：

leader的速度受到follower和目标点的影响。

步骤4：利用人工势场法构建虚拟势场，使得无人机根据势场值的变化决定航向角度和速度，进行避障；

(1)构建单个无人机与目标点的人工势场：

引力函数受无人机与目标点的距离的影响。无人机与目标点的距离越远则引力势能越大，越近则引力势能越小，当无人机到达目标点时引力势能则为零。

目标点对无人机的引力场函数为：

由公式

可得引力为

其中为ω增益系数且ω>0，引力的方向为无人机指向目标点。

斥力函数也受无人机与障碍物的距离影响。当无人机与障碍物距离越近则斥力势能越大，越远则斥力势能越小，而若无人机与障碍物的距离达到一定的程度，斥力势能则会为零。

构建斥力场函数为：

由此可得斥力为：

最终的全局总势场函数为引力场函数与斥力场函数的矢量相加，无人机受到的合力为引力与斥力的矢量相加。

U(x)＝U_d(x)+U_r(x)

无人机的运动由此合力决定。

(2)解决传统人工势场法目标点不可达的问题

由于人工势场法的思想是无人机受目标点的引力和障碍物的斥力的驱动而进行运动及避障。而在实际运动环境中，当障碍物处于无人机的运动路线上而导致无人机所受目标点的引力和障碍物的斥力相等且方向相反时，无人机受到的合力为零。无人机则会在原地停止运动或者原地徘徊从而陷入局部陷阱无法到达最终目标点。

修改斥力场函数：

修改后的斥力场函数可以使无人机在最终目标点处斥力势能值最小，从而解决局部最小值问题。

则修改后的斥力函数为：

(3)考虑无人机内部的避碰

当考虑无人机编队时，在编队内部各无人机可通过分布式通信拓扑实现局部的信息交互，避免碰撞并保持队形。在编队中单个无人机受到来自目标点的引力，外部障碍物的斥力和内部相邻无人机的编队力，其受到的力为三者的合力。

当某一无人机检测到障碍物的同时，编队中与其相邻的无人机也在该无人机的斥力范围内，这就需要该无人机同时进行障碍物的避障和与其他无人机的避碰。

某时刻一无人机同时受到来自障碍物的斥力和相邻无人机的斥力，当该无人机与其相邻无人机距离较近以至于其受到的来自相邻无人机的斥力大于来自障碍物的斥力时，无人机所受的合力将会使其优先避免与相邻无人机发生碰撞。而当该无人机与相邻无人机的距离拉开之后，虽然仍然受到来自相邻无人机的斥力，但该力却比来自障碍物的斥力要小，所以在合力的作用下无人机将会选择远离障碍物飞行。由此即可实现同时避障与避碰。

多无人机(UAVs)系统遇到障碍物时进行避障和避碰的流程图如图6所示。

步骤5：重复步骤2-4，使得无人机可以到达多个目标点，实现巡航目的。

当leader与目标点的欧式距离小于0.005时，认为无人机系统到达该目标点，重复步骤2-4，使得无人机系统朝着设定的下一个目标点行进，直到到达最后一个目标点，从而实现巡航目的。具体效果如图7。

本发明专利提出了一种基于雁形阵的无人机编队巡航控制方法，使用matlab的plot函数创建几个无人机，选定其中一个作为领航者，剩下的作为跟随者，并设置其初始位置和颜色，以及设置障碍点和目标点的位置和颜色。随后以无人机的位置为圆心，适当的距离为半径画圆，并标注无人机的航行方向，在每次更新的时候都重新画图实现动态显示。通过所构建的一致性算法编队控制算法以及人工势场，使得多无人机系统形成稳定雁形阵，并在巡航途中在进行避障和避碰。

Claims (5)

1.一种基于雁形阵的无人机编队巡航控制方法，其特征在于，包括以下步骤：

步骤1：建立多无人机系统的数学模型；

步骤2：设定无人机系统的航向角度及协同编队形成雁形阵达到稳定时各无人机之间的相对位置关系；

步骤3：基于一致性的编队控制算法进行雁形阵编队控制过程；

步骤4：利用人工势场法构建虚拟势场，使得无人机根据势场值的变化决定航向角度和速度，进行避障；

步骤5：重复步骤2-4，使得无人机可以到达多个目标点，实现巡航目的。

2.根据权利要求1所述的基于雁形阵的无人机编队巡航控制方法，其特征在于，步骤1所述建立无人机编队系统的数学模型具体包括：

步骤1.1建立多无人机系统编队通信结构和绝对坐标系

一个多无人机系统中含有n架无人机，用有向图G＝{V，E}表示多无人机系统编队通信结构，该结构为单向通信，即信息只能由父节点传递至子节点，选定系统中的一个无人机为leader，剩下的无人机为follower，设A＝{a_iji，j∈N}为该系统连通图的邻接矩阵，其中a_ij＝1表示第i个无人机可以与第j个无人机交换信息，否则a_ij＝0，由此构建该多无人机系统的编队通信结构和绝对坐标系；

步骤1.2定义速度更新和位置更新模型

leader的速度计算：

(1)引力影响：V_x1＝K_Nd₁cosθ，V_y1＝K_Nd₁sinθ

(2)斥力影响：当障碍物进入无人机探测范围

V_x＝V_x1+βR_x

V_y＝V_y1+βR_y

其中，V_x为领航者在x方向由引力影响的速度分量，V_x1为领航者在x方向合速度，V_y为领航者在y方向合速度，V_y1为领航者在y方向由引力影响的速度分量，d₁为领航者与下一目标点的欧式距离，θ为目标点和领航者之间的连线与x轴夹角，R_x为领航者在x方向由斥力影响的速度分量，R_y为领航者在y方向由斥力影响的速度分量，x_l为领航者的x坐标，

为领航者与第j个障碍物之间x坐标之差，y_l为领航者的y坐标，

为领航者与第s个障碍物之间y坐标之差，N为障碍点的个数，K_N、β为常量，d_s为领航者与第s个障碍物之间的欧氏距离，d_M为无人机的探测距离；

follower的速度计算：

首先要根据一阶编队的协同原理计算leader与follower的误差信息并施加给follower的速度，使其向协同的位置运动，然后计算检测范围内障碍物对机器人的影响，两者的速度加权在一起即为follower的速度；

位置更新：

x′＝x+d_t*V′_x

y′＝y+d_t*V′_y

其中，x′为更新后x坐标，x为更新前x坐标，d_t为控制周期，V′_x为更新后x方向上的速度，y′为更新后y坐标，y为更新前y坐标，V_y′为更新后x方向上的速度；

步骤1.3定义运动约束

定义无人机运动约束常量：目标点位置信息goal、速度上限V_max、控制周期d_t、障碍物的位置信息ob_temp、机器人的探测距离d_M以及通信连接的最大距离d_max；

根据无人机编队巡航系统的动力学特性，运动约束定义为：

由

V_y同理，由

其中，V_x2为定义x方向速度约束的中间变量，V_x0为更新前x方向的速度，ΔV_x为理论计算y方向上的下一速度与当前速度的差值，ω_x为角速度，V_xmax为给定的x方向最大速度值，V_y2为定义y方向速度约束的中间变量，V_y0为更新前y方向的速度，ΔV_y为理论计算x方向上的下一速度与当前速度的差值，ω_y为角速度，V_ymax为给定的y方向最大速度值；

当leader与目标点欧氏距离小于0.005即可向下一目标点行进。

3.根据权利要求1所述的一种基于雁形阵的无人机编队巡航控制方法，其特征在于，步骤2的具体包括方法包括如下步骤：

步骤2.1确定无人机系统的航向角度

设雁形阵中轴线与水平方向的夹角为航向角度α，则

其中，(x_m，y_m)表示在所建立的绝对坐标系下第m个目标点的位置，(x_l，y_l)表示leader的位置；

步骤2.2设定无人机系统协同编队形成雁形阵达到稳定时各无人机之间的相对位置关系

根据leader与目标点的相对位置将无人机系统分成两类模型，并设定无人机系统协同编队形成雁形阵达到稳定时follower与leader的相对位置关系；

(1)当目标点在leader左侧时，设定无人机系统协同编队形成雁形阵达到稳定时follower与leader的相对距离为：

其中，x_lf为第f个follower与leader的水平距离差，y_lf为第f个follower与leader的垂直距离差，

表示雁形阵两翼与中轴线之间的夹角，k为常数，d表示follower与leader在形成雁形阵时的相对距离，当follower在中轴线左侧时取加号，在中轴线右侧时取减号；

(2)当目标点在leader右侧时，设定无人机系统协同编队形成雁形阵达到稳定时follower与leader的相对距离为：

当follower在中轴线左侧时取加号，在中轴线右侧时取减号。

4.根据权利要求1所述的基于雁形阵的无人机编队巡航控制方法，其特征在于，步骤3的具体包括方法包括如下步骤：

步骤3.1设计编队系统的一阶连续系统模型

编队系统的一阶连续系统模型为：

其中，x_i∈Rⁿ表示无人机i的状态量，u_i∈Rⁿ表示无人机i的输入量，这里的n表示状态量的维度。

在理想状态下，控制输入量为：

其中，a_ij为编队邻接矩阵的元素，N_i为无人机i的邻居集合；

再另全局状态量x＝[x₁，...，x_n]^T∈Rⁿ得到全局动态关系为：

其中，D＝diagd_i和L＝D-A分别为编队拓扑通信中的对角入度矩阵和拉普拉斯矩阵，A为……，从上式知，拉普拉斯矩阵及特征值在无人机编队的稳定性和稳定速度上扮演着重要角色，其可决定编队的闭环动态特性；

步骤3.2构建生成树，使得leader为整个通信拓扑结构中唯一的根节点

固定拓扑下，一阶多无人机系统的一致性问题具有稳定收敛性解的重要条件是系统的网络拓扑存在一颗生成树，且要使多无人机系统形成雁形阵，就需要每个无人机的航向角度和速度大小最终收敛为同一预定值，而只要leader为整个通信拓扑结构中唯一的根节点，那么总能实现所有的follower都向其收敛，

当这样的最小生成树形成时，上式中-L的特征值均位于复平面左半平面，保证无人机系统能够达到一致性，则最终状态值为：

其中，C为最终状态值，p_i为无人机i在所建坐标系下的位置，n为无人机的个数；

步骤3.3构建leader和follower的控制算法

采用leader-follower中的分布式控制结构，通过控制leader和follower之间的相对距离、速度和航向角度，使其达到一定的稳定状态，从而实现大雁形编队；

(1)在离散时间下，编队系统的一阶连续系统模型可化为：

x_i(k+1)＝x_i(k)+u_i(k)

其中，x_i(k+1)为k+1时刻第i个无人机的状态量，x_i(k)为k时刻第i个无人机的状态量，u_i(k)为时刻第i个无人机的输入量，

(2)理想状态下，follower的控制算法为：

其中，ε＞0，，a_ij为编队邻接矩阵的元素，r_ij(k)为k时刻无人机i和无人机j之间的相对距离，即follower的速度受到与其他无人机的影响；

(3)leader的控制算法为：

其中，a和b为常数，d(k)为k时刻leader与目标点的距离，即leader的速度受到follower和目标点的影响，u_l(k)为k时刻leader的输入量，r_lj(k)为k时刻leader与第j个follower的相对距离，a_lj为leader在编队邻接矩阵中所对应的元素。

5.根据权利要求1所述的基于雁形阵的无人机编队巡航控制方法，其特征在于，步骤4的具体方法包括如下步骤：

步骤4.1设计单个无人机与目标点的人工势场：

(1)设计引力场函数

假设无人机的坐标为x，目标点的坐标为x_d，则可以得到两者的引力场函数：

其中，U_d(x)为引力场，||·||表示范数，用于度量某个向量空间中的某个向量的长度或大小；

由公式

可得引力为

其中ω为增益系数且ω＞0，F_d为无人机受到的引力，引力的方向为无人机指向目标点；

(2)设计斥力场函数

斥力函数也受无人机与障碍物的距离影响。当无人机与障碍物距离越近则斥力势能越大，越远则斥力势能越小，而若无人机与障碍物的距离达到一定的程度，斥力势能则会为零；所以设计障碍物斥力场函数需要合理设计斥力场的大小，不能太大否则会影响无人机的行进路线，也不能太小否则会导致无人机与障碍物存在距离过近而相撞的风险；

由此斥力场函数设计为：

其中ε为斥力增益系数，c为一个正的常数，表示无人机与障碍物的最大作用距离。D(x)表示无人机与周围离它最近的障碍物之间的欧氏距离，U_r(x)为斥力场；

由此可得斥力为：

其中，F_r为无人机受到的斥力，

为求导；

(3)设计总势场函数

最终的全局总势场函数U(x)为引力场函数与斥力场函数的矢量相加，无人机受到的合力F为引力与斥力的矢量相加

U(x)＝U_d(x)+U_r(x)

无人机的运动由此合力决定；

步骤4.2解决传统人工势场法目标点不可达的问题

在实际运动环境中，当障碍物处于无人机的运动路线上而导致无人机所受目标点的引力和障碍物的斥力相等且方向相反时，无人机受到的合力为零，无人机则会在原地停止运动或者原地徘徊从而陷入局部陷阱无法到达最终目标点；

修改斥力场函数：

修改后的斥力场函数可以使无人机在最终目标点处斥力势能值最小，从而解决局部最小值问题，

则修改后的斥力函数为：

步骤4.3考虑无人机内部的避碰

当考虑无人机编队时，在编队内部各无人机可通过分布式通信拓扑实现局部的信息交互，避免碰撞并保持队形，由此还需设计编队内部各无人机之间的势场，即编队中单个无人机受到来自目标点的引力，外部障碍物的斥力和内部相邻无人机的编队力，故其受到的力为三者的合力；

当某一无人机检测到障碍物的同时，编队中与其相邻的无人机也在该无人机的斥力范围内，这就需要该无人机同时进行障碍物的避障和与其他无人机的避碰，使得无人机同时进行避障和避碰的思路是：

某时刻一无人机同时受到来自障碍物的斥力和相邻无人机的斥力，当该无人机与其相邻无人机距离较近以至于其受到的来自相邻无人机的斥力大于来自障碍物的斥力时，无人机所受的合力将会使其优先避免与相邻无人机发生碰撞，而当该无人机与相邻无人机的距离拉开之后，虽然仍然受到来自相邻无人机的斥力，但该力却比来自障碍物的斥力要小，所以在合力的作用下无人机将会选择远离障碍物飞行，由此即可实现同时避障与避碰。

Images