CN110502032A - 一种基于行为控制的无人机集群编队飞行方法 - Google Patents

Abstract

本发明提出了一种基于行为控制的无人机集群编队飞行方法，属于智能控制领域。所述飞行方法包括以下步骤：S1、在无人机集群中，根据无人机的运动模型，对无人机进行编号；S2、利用编队控制器对无人机集群进行编队控制；所述编队控制器包括信息感知模块、信息融合模块、行为决策模块和飞行控制模块；S3、基于行为的编队控制策略，选择无人机集群的编队飞行行为；所述无人机集群的编队飞行行为主要包括：向任务目标运动行为、队形保持行为、碰撞避免行为和离群保持行为。采用本发明可以使多无人机***在完成任务的过程中应对异常状态、避开障碍物、到达任务目标点，实现多无人机的实时编队控制，从而完成需要多无人机协同的编队飞行任务。

Description

一种基于行为控制的无人机集群编队飞行方法

技术领域

本发明设计智能控制领域，具体设计一种基于行为控制的无人机集群编队飞行方法。

技术背景

随着传感器技术、通讯技术、控制理论、人工智能等相关领域应用的不断发展，无人机的应用水平也在快速提高，具体体现为需要完成的任务变得更加复杂化，所处的环境变得更加不可预知。面对未知环境下的复杂任务，人们考虑采用多人协作的方式弥补单个无人机的能力不足问题。多无人机***能够通过协作在时间和空间上并发的完成复杂任务，对无人机的设计难度比较低，只需要设计功能特定的无人机，不需要拥有能执行所有任务的高级无人机，各个无人机在功能上互补，具有经济、可靠、响应快等优点。

中国发明专利(CN105589470A)公开了一种多无人机的分布式编队控制方法，通过构造多无人机之间的链式结构，无人机通过分布式控制算法计算无人机的期望速度，进行速度控制从而使无人机保持给定的队形。中国发明专利(CN108508911A)公开了无人机编队飞行控制方法及无人机，主要能够根据航点的四个维度信息，合理调整无人机的飞行速度，保证无人机定时定点飞行到目标航点上。

目前无人机集群编队控制面对的主要问题是：如何在无人机集群执行编队飞行任务的过程中提高队形保持的效率。为了形成编队需要多无人机之间交换状态信息(速度和位置信息)，在领航者跟随者编队控制方法中会让领航者承担较大的通信负荷和计算量，并且面临通信状态不稳定的状况会难以处理；编队控制器依赖定位信息计算控制输入，通常定位数据会由于非***原因出现不稳定的奇异点，如何处理定位数据也是编队控制中的关键问题。

发明内容

本发明的目的是为了解决现有技术中的上述缺陷，提供一种基于行为控制的无人机集群编队方法。通过对无人机获取的GPS传感器数据进行位置有效性判断以及在编队控制器中设计主要的编队飞行行为，根据不同的环境和任务需求进行编队飞行行为决策从而得到无人机最后的控制输入，以控制无人机的运动来达成编队，提高编队飞行过程中无人机集群的队形保持效率。

本发明至少通过如下技术方案之一实现。

一种基于行为控制的无人机集群编队飞行方法，包括以下步骤：

S1、在无人机集群中，根据无人机的运动模型，对无人机进行编号；

S2、利用编队控制器对无人机集群进行编队控制；

S3、基于行为的编队控制策略，选择无人机集群的编队飞行行为。

优选的，所述的无人机的运动模型满足：

其中，n为整数代表无人机数量，n≥3，i对应第i个无人机，p_i代表第i个无人机的位置，表示第i个无人机对位置的微分，代表对第i个无人机的控制输入,和分别表示对应于第i个无人机的线速度和角速度，定义无人机的位置状态量p_i＝(x_i,y_i,z_i)，其中x_i,y_i,z_i分别对应于无人机i在大地坐标系中的x、y和z轴坐标；

由n架无人机组成的无人机集群描述为{uav₁，uav₂,…,uav_i,…,uav_n}，其中uav_i表示第i架无人机的编号。对于分布式的多无人机集群，每个无人机通过自身携带的GPS传感器获取本机的位置信息，分布式多无人机采用基于行为的控制方式保证无人机集群能够应付异常情况的前提下，能够维持队形完成任务。

优选的，所述无人机集群的编队飞行行为主要包括：向任务目标运动行为、队形保持行为、碰撞避免行为和离群保持行为。

优选的，所述编队控制器包括信息感知模块、信息融合模块、行为决策模块和飞行控制模块；

信息感知模块包括传感器，用于获取无人机自身的位置信息以及飞行状态信息，包括无人机的定位信息，速度信息和航偏角信息；

信息融合模块，结合其他无人机的飞行状态信息以及环境中的障碍物和威胁区域信息生成编队控制中的准确的无人机位置信息和路径点信息；

行为决策模块，根据各行为对应的重要性因子以及相关信息计算行为权重，并通过比较各行为的权重进行行为决策，根据决策生成控制输入指令，并将控制输入指令发送到飞行控制模块；无人机编队飞行的主要行为对应于相关权值。本发明采用行为之间相互抑制的方式，在无人机编队飞行的过程中通过对比各行为权值的大小进行行为决策，以生成对应于当前所决策行为的控制输入u_i，表示对应于第i个无人机在当前时刻的控制输入。

飞行控制模块，接受到飞行控制指令后依据控制指令驱动无人机的飞行控制器。

优选的，行为决策模块进行行为决策的过程如下：

使用w₁、w₂、w₃和w₄分别表示向任务目标运动行为、队形保持行为、碰撞避免行为和离群保持行为的权重，定义对应权值的取值范围为0≤w₁,w₂,w₃,w₄≤1；

使用a₁、a₂和a₃分别对应无人机执行编队任务时各行为的重要性因子，可以通过调整它们的大小来调整编队中无人机的编队行为个性。通过调整a₁、a₂和a₃的大小来调整编队中无人机的编队行为；若向任务目标运动行为的重要性因子a₁大于a₂和a₃，则无人机集群在执行任务的过程中更倾向于优先到达任务目标点以完成任务；若队形保持行为的重要性因子a₂大于a₁和a₃，则表示无人机集群更倾向于保持完整的几何队形；若碰撞避免行为的重要性因子a₃大于a₁和a₂，则无人机在编队飞行的过程中会重点关注周围障碍信息以免发生碰撞；无人机在编队飞行的过程中会显得相当谨慎，谨防与周围的其他无人机或者环境障碍物发生碰撞。

以上各行为权值的计算如下：

(1)w₁＝a₁，其中a₁为常数，0＜a₁＜1，表示任务中向目标点运动的重要性，由控制者设定；

(2)其中a₂为常数，0＜a₁＜1，表示了任务中向目标点运动的重要性，由控制者设定，l_if表示无人机i距离队形期望位置的距离，其中f表示期望位置，p表示当前无人机所允许的队形偏差；

(3)其中a₃为的常数，0＜a₁＜1，表示了任务中向目标点运动的重要性，由控制者设定，D_safe为无人机当前时刻的安全距离，D_i为无人机i检测到障碍物的最小距离；

(4)如果无人机处于通信失效状态，则w₄＝1，其余行为所对应的权值均为0，此时将选择离群保持行为。

优选的，无人机集群中的无人机通过地面控制中心进行无人机之间的信息交相互，同时每架无人机保持与地面控制中心的信息交互，其中，无人机与地面控制中心的信息交互，使得无人机获取控制地面控制中心分派的任务，以及在无人机异常状态下地面控制中心接管无人机的飞行控制器；无人机之间的信息交互用于无人机处于编队过程中交换各自的速度和位置信息，维持编队队形和向目标点运动。

在交互层面分布式多无人机集群采用混合式交互方式，各分布式无人机可以与地面控制中心交互信息，使得控制者可以在任意时刻调整无人机的状态。

优选的，步骤S2中，对无人机集群进行编队控制的步骤包括：

S21、每架无人机通过自身携带的GPS传感器获取自己的位置，并经过位置有效性判断和位置预测获取有效位置范围，将该有效位置范围发送到信息感知模块；

S22、信息融合模块接收信息，结合其他无人机的飞行状态信息以及环境中的障碍物和威胁区域信息生成编队控制中无人机的位置信息和路径点信息，当无人机GPS传感器的位置数据超出有效位置范围时，采用粒子滤波算法对无人机的无效位置进行预测分析，得到无人机的位置数据；

S23、行为决策模块在无人机编队飞行的过程中通过对比各行为权值的大小进行行为决策，以生成对应于当前所决策行为的控制输入u_i，并将控制输入指令发送到飞行控制模块控制无人机。

优选的，步骤S21中，无人机的位置数据由自身携带的位置GPS传感器所获取，由于GPS传感器存在自身的***误差e_uav以及会出现奇异点即在某些区域内数据大范围的偏离无人机的有效位置，因此，需要引入位置有效性判断，假设在T₀时刻，无人机i的位置p_i(x_i,y_i,z_i)为无人机的有效位置，这一时间段内无人机的控制输入转化为无人机运动学为v_i，由于无人机的控制输入到无人机运动之间存在***误差e_uav，在一段时间ΔT之后无人机的位置p_i'(x'_i,y'_i,z'_i)满足以下约束：

x_i+∫(v_ix-e_uav)dt<x'_i<x_i+∫(v_ix+e_uav)dt (15)

y_i+∫(v_iy-e_uav)dt<y'_i<y_i+∫(v_iy+e_uav)dt (16)

z_i+∫(v_iz-e_uav)dt<z'_i<z_i+∫(v_iz+e_uav)dt (17)

公式(2)至公式(4)为无人机在运动一段时间之后的有效位置范围，如果GPS传感器数据偏离了有效位置范围，那么将采取粒子滤波来对位置进行预测，从而获得更稳定的位置数据。v_ix、v_iy、v_iz分别表示当前时刻无人机在全局坐标系下x,y,z方向对应的速度。

优选的，步骤S23中，无人机计算位置控制输入具体如下：

无人机i其自身相对于领航参照无人机的距离和夹角分别为l_i和所对应的期望位置到领航参考无人机的距离和夹角记为l_if和 表示期望航偏角，x_if、y_if和z_if分别表示l_if对应于大地坐标系在x、y、z轴上的分量l_if，引入角度参数β便于表示，记其中θ_i表示无人机自身的航偏角，领航参考无人机的速度记为v_L，无人机i在队形保持行为的控制输入为：

和分别表示第i个无人机的线速度控制指令和角速度控制指令，ω_i表示领航参照无人机的角速度；

形成初始队形之后，保持良好的通信状况下如果无人机没有遇到障碍物区域或者发生集群内部无人机之间的碰撞，无人机将选择向任务目标运动行为；若无人机i当前位置与目标位置G的距离为R，无人机运动正方向与任务目标之间的夹角为则当前行为产生的控制输入为：

其中α₁和α₂是给定的控制系数，d表示无人机的直径；

考虑实际飞行环境中可能存在障碍物，导致无人机集群需要切换队形通过障碍区域，这时候如果检测到无人机到障碍物的距离小于安全距离，则选择碰撞避免行为，先保障无人机的安全，无人机的安全距离表示为：

其中，a，b为控制参数，可取任意大于等于0的实数；D₀代表无人机因为气动因素需要与障碍物保持的最小距离，r_uav代表无人机的半径，V无人机的速度，代表无人机机体坐标系的Y轴与障碍物的夹角；将集群内的其他无人机以及环境中的障碍区域均视为危险区域，当无人机与危险区域的距离小于安全距离时无人机决策为碰撞避免行为，使无人机驶出危险区域；

碰撞避免行为无人机的控制输入为：

其中，V_t-1为上一时刻无人机的速度，k₁和k₂为无人机的控制系数，可取任意大于等于0的实数；D_i表示无人机与障碍物的距离，ω_max表示无人机的最大角速度；V_t-1为上一时刻无人机的速度。

在编队飞行的过程中无人机会与集群内的其他无人机保持通信，当检测到通信质量不佳而处于通信失效状态即通信延时大于500ms，那么该无人机会选择离群保持行为。

将无人机集群分为集群区域、离群保持区域、离群区域，根据集群内所有无人机的位置计算出无人机集群的中心，并找到当前队形距离集群中心最远的无人机，记该无人机到集群中心的距离为R₁，R₂＝R₁+2d，R₃＝R₂+2d，这里d表示无人机的直径，R₁,R₂,R₃分别为集群区域、离群保持区域和离群区域的半径；当无人机处于离群保持行为时，首先根据数据判断当前处于集群的哪个区域,并设此时拟运动距离为r，根据当前无人机的拟运动距离计算出控制输入为：

其中和分别表示距离r和无人机运动正方向与目标点之间的夹角为对时间的微分，v_i表示无人机运动线速度，ω_i表示无人机运动角速度；

最终的控制目标是使得r＝0，得离群保持行为中无人机的控制输入为：

其中α₃和α₄是给定的控制系数，取值范围为大于0小于等于1的实数；当处于通信失效状态下的无人机恢复正常通信状态后，由于已经偏离队形期望位置故采取队形保持行为继续保证编队的正常运行，随后无人机集群继续保持向编队飞行的任务目标运动直到最终完成编队跟踪飞行任务。

优选的，如果GPS传感器数据偏离了有效位置范围，本发明采取粒子滤波来对位置进行预测，从而获得更稳定的位置数据。基于粒子滤波的位置预测分为粒子重要性采样、重要性权值计算并归一化、粒子重采样三个步骤，具体如下：

S221、取初始时刻k＝1，从无人机位置的概率密度函数p(x_k)中抽取一组初始粒子其中表示k时刻从概率密度函数中抽取的第a个粒子，a∈[1,N]，N表示粒子的数量；

S222、对N个粒子进行重要性采样：

其中，粒子a的概率值服从为重要性概率密度函数，符号～表示服从概率密度函数，表示概率值对粒子a初始时刻到k-1时刻的集合z_1:k＝{z₁,z₂,…,z_k)是到时刻k的测量集合，z_k为时刻k是得到的概率测量值；

S223、计算粒子重要性的权值并进行权值归一化：

其中和分别为和 表示概率观测量对应该粒子的概率密度函数，表示同一粒子前后时刻的概率密度函数，为前后时刻的对应粒子的条件概率；

粒子归一化之后的权值为：

其中表示归一化之后的权值，j∈[1,N]，表示每个粒子的权值；

S224、从集合中根据重要性权值重新采样得到新的N个粒子的集合并更新粒子的权值

S225、输出经过粒子滤波的无人机位置数据：

其中表示k时刻粒子滤波之后的无人机的位置数据，表示重新采样得到的粒子。。

作为进一步优选，向任务目标运动行为的原则是根据编队整体的任务目标点，控制无人机集群向任务目标点运动，该行为的触发条件通常是无人机集群已经形成要求的几何队形。队形保持行为的原则是使得集群中的所有无人机在执行任务的过程中采取队形保持行为可以有效的形成以及维持队形，该行为的触发条件通常是在编队跟踪任务执行之初需要形成队形，以及避障任务完成之后需要恢复队形。碰撞避免行为的原则是为了保证整个无人机集群能够顺利的进行无人机自主避碰以及无人机集群避障。离群保持行为的原则是当无人机通信状态不佳的时候，无论采取怎样的控制补偿措施都难以使无人机集群保持编队队形，为了保证无人机的安全，设定离群保持行为使无人机在离群范围内等下通信状态恢复。

作为进一步优选，向任务目标运动行为若无人机i当前位置与目标位置G的距离为R，无人机运动正方向与任务目标点之间的夹角为向任务目标运动行为的原则是对无人机实施控制，使得无人机与目标位置的距离缩小至0以及运动方向的夹角靠近

作为进一步优选，队形保持行为取领航参考点的方式，进行队形期望位置的计算，一旦队形确定，那么每架无人机就可以计算出自身所对应的队形期望点。各个无人机根据相对于领航参考无人机的距离l和夹角来确定本无人机在编队中的位置和方向，因此也称为基于反馈控制的队形保持算法。当集群中以一台无人机为领航参考无人机，其他无人机作为跟随保持无人机时，当领航无人机做匀速直线运动或者圆周运动时，基于线性反馈的队形保持算法是稳定的。

作为进一步优选，考虑无人机的半径、无人机的飞行速度、无人机与障碍物的夹角等因素设置安全距离。当无人机当前航向与障碍物之间的夹角小于π/2时，无人机的速度越快则需要保持的安全距离越大；当无人机当前的航向角与障碍物之间的夹角大于π/2时，无人机的速度对安全距离没有影响，只需要考虑气动因素需要保持的最小距离以及无人机和障碍物的半径。

作为进一步优选，当无人机与集群内其他无人机之间的通信时延大于500ms时为了保持编队队形的稳定以及防止无人机集群内部发生碰撞，会进入离群保持行为。将无人机集群分为集群区域，离群保持区域，离群区域三个范围。当无人机处于离群保持行为时，首先根据最近可用数据判断当前处于集群的哪个区域,并设此时拟运动距离为r，根据当前无人机的拟运动距离计算出控制输入，该行为的原则是使得无人机保持在离群区域，以避免与集群内的其他无人机发生碰撞。

同现有技术相比较，本发明的有益效果体现在：

1、本发明提供一种基于位置预测和行为控制的分布式编队方法，对无人机GPS传感器收到的位置数据结合无人机运动学状态进行位置有效性判断，对于无效的位置数据采用上一时刻的有效位置数据进行粒子滤波预测当前时刻的位置数据替换掉无效位置，这样做法有效的减少了由于位置数据不稳定带来的编队控制震荡的情况，加强了编队控制的稳定性。

2、在分布式无人机集群的基础上加上地面控制中心形成混合式的信息交互模式。配合基于行为的编队控制方式，给出了无人机集群编队飞行的主要行为，并通过各行为之间相互抑制的方法进行编队飞行过程中无人机的行为决策最后生成用于飞行的控制输入，该编队控制方式能够使无人机在编队飞行的过程中有效的形成队形并且提高无人机集群编队飞行过程中队形保持率。

附图说明

图1为实施例中一种基于行为控制的无人机集群编队飞行方法控制模式示意图；

图2为实施例中集群无人机之间信息交互模式示意图；

图3为实施例中无人机集群队形保持行为示意图；

图4为实施例中无人机离群保持行为示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本实施例展示了一种基于行为控制的无人机集群编队飞行方法，在执行编队跟踪飞行任务的过程中，使用本发明提出的编队飞行方案对无人机进行控制，具体包括以下步骤：

S1、在无人机集群中，根据无人机的运动模型，对无人机进行编号；

由n架无人机组成的无人机集群，按顺序对无人机进行编号，无人机集群描述为{uav₁，uav₂,…,uav_i,…,uav_n}，其中uav_i表示第i架无人机的编号。

无人机满足如下运动模型：

其中，n为整数代表无人机数量，n≥3，i对应第i个无人机，p_i代表第i个无人机的位置，表示第i个无人机对位置的微分，代表对第i个无人机的控制输入,和分别表示对应于第i个无人机的线速度和角速度，定义无人机的位置状态量p_i＝(x_i,y_i,z_i)，其中x_i,y_i,z_i分别对应于无人机i在大地坐标系中的x，y，z轴坐标；θ_i表示无人机i的航偏角。

S2、利用编队控制器对无人机集群进行编队控制。

S3、基于行为的编队控制策略，选择无人机集群的编队飞行行为。所述无人机集群的编队飞行行为主要包括：向任务目标运动行为、队形保持行为、碰撞避免行为和离群保持行为。

采用各行为之间相互抑制的行为融合策略，进行控制输入的生成，无人机i的编队控制结构如图1所示。无人机的编队控制器包括信息感知模块、信息融合模块、行为决策模块和飞行控制模块。无人机通过信息感知模块获取自身的位置定位信息以及飞行状态信息；无人机通过信息融合模块结合自身信息、其他无人机的状态信息以及环境信息生成编队控制中的可用信息；行为决策模块根据编队飞行行为对应的重要性因子以及相关信息计算行为权重，并通过比较个行为的权重进行行为决策，生成控制输入指令u_i；飞行控制模块接受到飞行控制指令后依据控制指令驱动无人机的飞行控制器。

无人机各个行为的选择可以根据任务的需要进行设置。使用w₁、w₂、w₃、w₄分别表示对应于向任务目标运动行为、队形保持行为、碰撞避免行为、离群保持行为的权重。定义对应权值的取值范围为0≤w₁,w₂,w₃,w₄≤1。

以上各行为权值的计算如下：

(1)w₁＝a₁，其中a₁为大于0小于1的常数，表示了任务中向目标点运动的重要性，由控制者设定。

(2)其中a₂为大于0小于1的常数，表示了任务中向目标点运动的重要性，由控制者设定。l_if表示无人机i距离队形期望位置的距离，p表示当前无人机***所允许的队形偏差。

(3)其中a₃为大于0小于1的常数，表示了任务中向目标点运动的重要性，由控制者设定。D_safe为无人机当前时刻的安全距离，D_i为检测到障碍物的最小距离。

(4)如果无人机处于通信失效状态，则w₄＝1，其余行为所对应的权值均为0，此时离群保持行为的优先级最高。

人机各行为的重要性可以根据具体任务的要求灵活设置。a₁,a₂,a₃分别对应无人机执行编队任务时各行为的重要性因子，可以通过调整它们的大小来调整编队中无人机的编队行为个性。若向任务目标点运动行为的重要性因子a₁相对较高，则无人机集群在执行任务的过程中更倾向于优先完成任务，而对队形保持程度以及碰撞避免的关注度较弱；若队形保持的重要性因子a₂相对较高，则表示无人机集群更倾向于保持完整的几何队形，会显得运动速度较慢，但是队形保持的更好；若碰撞避免行为的重要性因子a₃相对较高，则无人机在编队飞行的过程中会显得相当谨慎，谨防与周围的其他无人机或者环境障碍物发生碰撞。

编队飞行任务执行的过程中，为了实现无人机集群编队控制，各无人机与地面控制中心之间采取一种混合式的信息交互方式，参见图2。无人机之间互相有信息交互，同时每个无人机保持与地面控制中心的信息交互。无人机与地面控制中心的信息交互，可以使得无人机最初获取控制中心分派任务，以及异常状态下控制中心接管无人机的控制器；无人机之间的信息交互用于无人机处于编队过程中交换各自的状态(速度，位置)信息，用于维持编队和向目标点运动。

在编队飞行的过程中，每架无人机通过其携带的GPS传感器获取位置数据，并经过位置有效性判断和位置预测获取有效位置范围，将该有效位置范围发送到信息感知模块。由于GPS传感器存在自身的***误差(e_uav)以及可能会出现奇异点(即在某些区域内数据大范围的偏离了无人机的有效位置)。这需要时候引入位置有效性判断，假设在T₀时刻，无人机的位置p_i(x_i,y_i,z_i)为无人机的有效位置，这一时间段内无人机的控制输入转化为无人机运动学为v_i，由于无人机的控制输入到无人机运动之间存在***误差e_uav，那么在一段时间ΔT之后无人机的位置p_i'(x'_i,y'_i,z'_i)满足以下约束：

x_i+∫(v_ix-e_uav)dt<x'_i<x_i+∫(v_ix+e_uav)dt (2)

y_i+∫(v_iy-e_uav)dt<y'_i<y_i+∫(v_iy+e_uav)dt (3)

z_i+∫(v_iz-e_uav)dt<z'_i<z_i+∫(v_iz+e_uav)dt (4)

公式(2)至公式(4)给出了无人机在运动一段时间之后的有效位置范围，如果GPS传感器数据偏离了有效位置范围，那么就采取粒子滤波的方法来对位置进行预测，从而获得更稳定的位置数据。

当无人机GPS传感器数据超出有效位置范围时采取粒子滤波方法对无人机的位置进行预测，其步骤如下：

S221、取初始时刻k＝1，从无人机位置的概率密度函数p(x_k)中抽取一组初始粒子其中表示k时刻从概率密度函数中抽取的第a个粒子，a∈[1,N]，N表示粒子的数量；

S222、对N个粒子进行重要性采样：

其中，粒子a的概率值服从为重要性概率密度函数，符号～表示服从概率密度函数，表示概率值对粒子a初始时刻到k-1时刻的集合z_1:k＝{z₁,z₂,…,z_k)是到时刻k的测量集合，z_k为时刻k是得到的概率测量值；

S223、计算粒子重要性的权值并进行权值归一化：

其中和分别为和 表示概率观测量对应该粒子的概率密度函数，表示同一粒子前后时刻的概率密度函数，为前后时刻的对应粒子的条件概率；

粒子归一化之后的权值为：

其中表示归一化之后的权值，j∈[1,N]，表示每个粒子的权值；

S224、从集合中根据重要性权值重新采样得到新的N个粒子的集合并更新粒子的权值

S225、输出经过粒子滤波的无人机位置数据：

其中表示k时刻粒子滤波之后的无人机的位置数据，表示重新采样得到的粒子。

编队跟踪飞行任务开始时，各无人机处于不同位置，当无人机收到来自地面站的任务指令后开始执行编队跟踪飞行任务，需要依次对任务目标点实行跟踪观测。本发明采取领航参考的编队形成和保持方法，由于各无人机的初始位置距离当前编队期望位置比较远，根据行为选择策略，无人机如果通信状态正常，会选择队形保持行为去形成编队队形，如图3所示。每架无人机根据自己在队形中的期望位置计算控制输入，而达成期望队形。队形保持行为的示意图如图3所示，无人机i其自身相对于领航参照无人机的距离和夹角分别为l_i和所对应的期望位置到领航参考无人机的距离和夹角记为l_if和 表示期望航偏角，x_if、y_if和z_if分别表示l_if对应于大地坐标系在x、y、z轴上的分量l_if，并记θ_i表示无人机自身的航偏角，ω_i为无人机当前的角速度，领航参考无人机的速度记为v_L，无人机i在队形保持行为的控制输入为：

和分别表示第i个无人机的线速度控制指令和角速度控制指令；

形成初始队形之后，保持良好的通信状况下如果无人机没有遇到障碍物区域或者发生集群内部无人机之间的碰撞，无人机会更倾向于向任务目标点运动行为。若无人机i当前位置与目标位置G的距离为R，无人机运动正方向与任务目标点之间的夹角为则当前行为产生的控制输入为：

上述公式中其中α₁和α₂是给定的控制系数。

考虑实际飞行环境中可能存在障碍物，导致无人机集群需要切换队形通过障碍区域，这时候如果检测到无人机到障碍物的距离小于安全距离，则选择碰撞避免行为，优先保障无人机的安全，无人机的安全距离表示为：

其中，D₀代表无人机因为气动因素需要与障碍物保持的最小距离，r_uav代表无人机的半径，代表无人机机体坐标系的Y轴与障碍物的夹角；将集群内的其他无人机以及环境中的障碍区域均视为危险区域，当无人机与危险区域的距离小于安全距离时无人机决策为碰撞避免行为，使无人机驶出危险区域；

碰撞避免行为无人机的控制输入为：

上述公式中，V_t-1为上一时刻无人机的速度，k₁和k₂为控制系数，D_i表示无人机与障碍物的距离，ω_max表示无人机的最大角速度。

在编队飞行的过程中无人机会与集群内的其他无人机保持通信，当检测到通信质量不佳而处于通信失效状态即通信延时大于500ms，那么该无人机会选择离群保持行为，离群保持行为的示意图如图4所示。

将无人机集群分为集群区域、离群保持区域、离群区域，根据集群内所有无人机的位置计算出无人机集群的中心，并找到当前队形距离集群中心最远的无人机，记该无人机到集群中心的距离为R₁，R₂＝R₁+2d，R₃＝R₂+2d，这里d表示无人机的直径，R₁,R₂,R₃分别为集群区域、离群保持区域和离群区域的半径。当无人机处于离群保持行为时，首先根据最近可用数据判断当前处于集群的哪个区域，并设此时拟运动距离为r，根据当前无人机的拟运动距离计算出控制输入为：

其中和分别表示距离r和无人机运动正方向与目标点之间的夹角为对时间的微分，v_i表示无人机运动线速度，ω_i表示无人机运动角速度。

最终的控制目标是使得r＝0,可得离群保持行为中无人机的控制输入为：

其中α₃和α₄是给定的控制系数。当处于通信失效状态下的无人机恢复正常通信状态后，由于已经偏离队形期望位置故采取队形保持行为继续保证编队的正常运行。随后无人机集群继续保持向编队飞行的任务目标点运动直到最终完成编队跟踪飞行任务。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (10)

1.一种基于行为控制的无人机集群编队飞行方法，其特征在于，包括以下步骤：

S1、在无人机集群中，根据无人机的运动模型，对无人机进行编号；

S2、利用编队控制器对无人机集群进行编队控制；

S3、基于行为的编队控制策略，选择无人机集群的编队飞行行为。

2.根据权利要求1所述的基于行为控制的无人机集群编队飞行方法，其特征在于，所述的无人机的运动模型满足：

其中，n为整数代表无人机数量，n≥3，i对应第i个无人机，p_i代表第i个无人机的位置，表示第i个无人机对位置的微分，代表对第i个无人机的控制输入,和分别表示对应于第i个无人机的线速度和角速度，定义无人机的位置状态量p_i＝(x_i,y_i,z_i)，其中x_i,y_i,z_i分别对应于无人机i在大地坐标系中的x、y和z轴坐标；

由n架无人机组成的无人机集群描述为{uav₁，uav₂,…,uav_i,…,uav_n}，其中uav_i表示第i架无人机的编号。

3.根据权利要求1所述的基于行为控制的无人机集群编队飞行方法，其特征在于，无人机集群的编队飞行行为主要包括：向任务目标运动行为、队形保持行为、碰撞避免行为和离群保持行为。

4.根据权利要求1所述的基于行为控制的无人机集群编队飞行方法，其特征在于，所述编队控制器包括信息感知模块、信息融合模块、行为决策模块和飞行控制模块；

信息感知模块包括传感器，用于获取无人机自身的位置信息以及飞行状态信息，包括无人机的定位信息，速度信息和航偏角信息；

信息融合模块，结合其他无人机的飞行状态信息以及环境中的障碍物和威胁区域信息生成编队控制中无人机的位置信息和路径点信息；

行为决策模块，根据各行为对应的重要性因子以及相关信息计算行为权重，并通过比较各行为的权重进行行为决策，根据决策生成控制输入指令，并将控制输入指令发送到飞行控制模块；

飞行控制模块，接受到飞行控制指令后依据控制指令驱动无人机的飞行控制器。

5.根据权利要求4所述的基于行为的无人机集群编队飞行方法，其特征在于，行为决策模块进行行为决策的过程如下：

使用w₁、w₂、w₃和w₄分别表示向任务目标运动行为、队形保持行为、碰撞避免行为和离群保持行为的权重，定义对应权值的取值范围为0≤w₁,w₂,w₃,w₄≤1；

使用a₁、a₂和a₃分别对应无人机执行编队任务时各行为的重要性因子，无人机各行为的重要性根据具体任务的要求设置，通过调整a₁、a₂和a₃的大小来调整编队中无人机的编队行为；若向任务目标运动行为的重要性因子a₁大于a₂和a₃，则无人机集群在执行任务的过程中更倾向于优先到达任务目标点以完成任务；若队形保持行为的重要性因子a₂大于a₁和a₃，则表示无人机集群更倾向于保持完整的几何队形；若碰撞避免行为的重要性因子a₃大于a₁和a₂，则无人机在编队飞行的过程中会重点关注周围障碍信息以免发生碰撞；

以上各行为权值的计算如下：

(1)w₁＝a₁，其中a₁为常数，0＜a₁＜1，表示任务中向目标点运动的重要性，由控制者设定；

(2)其中a₂为常数，0＜a₁＜1，表示任务中向目标点运动的重要性，由控制者设定，l_if表示无人机i距离队形期望位置的距离，其中f表示期望位置，p表示当前无人机所允许的队形偏差；

(3)其中a₃为的常数，0＜a₁＜1，表示任务中向目标点运动的重要性，由控制者设定，D_safe为无人机当前时刻的安全距离，D_i为无人机i检测到障碍物的最小距离；

(4)如果无人机处于通信失效状态，则w₄＝1，其余行为所对应的权值均为0，此时将选择离群保持行为。

6.根据权利要求1所述的基于行为控制的无人机集群编队飞行方法，其特征在于，无人机集群中的无人机通过地面控制中心进行无人机之间的信息交相互，同时每架无人机保持与地面控制中心的信息交互，其中，无人机与地面控制中心的信息交互，使得无人机获取控制地面控制中心分派的任务，以及在无人机异常状态下地面控制中心接管无人机的飞行控制器；无人机之间的信息交互用于无人机处于编队过程中交换各自的速度和位置信息，维持编队队形和向目标点运动。

7.根据权利要求1所述的基于行为控制的无人机集群编队飞行方法，其特征在于，步骤S2中，对无人机集群进行编队控制的步骤包括：

S21、每架无人机通过自身携带的GPS传感器获取自己的位置，并经过位置有效性判断和位置预测获取有效位置范围，将该有效位置范围发送到信息感知模块；

S22、信息融合模块接收信息，结合其他无人机的飞行状态信息以及环境中的障碍物和威胁区域信息生成编队控制中无人机的位置信息和路径点信息，当无人机GPS传感器的位置数据超出有效位置范围时，采用粒子滤波算法对无人机的无效位置进行预测分析，得到无人机的位置数据；

S23、行为决策模块在无人机编队飞行的过程中通过对比各行为权值的大小进行行为决策，以生成对应于当前所决策行为的控制输入u_i，并将控制输入指令发送到飞行控制模块控制无人机。

8.根据权利要求7所述的基于行为控制的无人机集群编队飞行方法，其特征在于，步骤S21中有效位置范围的获取如下：

假设在T₀时刻，无人机i的位置p_i(x_i,y_i,z_i)为无人机的有效位置，无人机的控制输入转化为无人机运动学为v_i，由于无人机的控制输入到无人机运动之间存在***误差e_uav以及会出现奇异点即在某些区域内数据大范围的偏离无人机的有效位置，在一段时间ΔT之后无人机的位置p_i'(x'_i,y'_i,z'_i)满足以下约束：

x_i+∫(v_ix-e_uav)dt<x'_i<x_i+∫(v_ix+e_uav)dt (2)

y_i+∫(v_iy-e_uav)dt<y'_i<y_i+∫(v_iy+e_uav)dt (3)

z_i+∫(v_iz-e_uav)dt<z'_i<z_i+∫(v_iz+e_uav)dt (4)

公式(2)至公式(4)为无人机在运动一段时间之后的有效位置范围，v_ix、v_iy、v_iz分别表示当前时刻无人机在全局坐标系下x,y,z方向对应的速度。

9.根据权利要求7所述的基于行为控制的无人机集群编队飞行方法，其特征在于，步骤S23中，无人机计算位置控制输入具体如下：

无人机i其自身相对于领航参照无人机的距离和夹角分别为l_i和所对应的期望位置到领航参考无人机的距离和夹角记为l_if和 表示期望航偏角，x_if、y_if和z_if分别表示l_if对应于大地坐标系在x、y、z轴上的分量l_if，引入角度参数β便于表示，记其中θ_i表示无人机自身的航偏角，领航参考无人机的速度记为v_L，无人机i在队形保持行为的控制输入为：

和分别表示无人机i的线速度和角速度，ω_i表示领航参照无人机的角速度；

形成初始队形之后，保持良好的通信状况下如果无人机没有遇到障碍物区域或者发生集群内部无人机之间的碰撞，无人机将选择向任务目标运动行为；若无人机i当前位置与目标位置G的距离为R，无人机运动正方向与任务目标之间的夹角为则当前行为产生的控制输入为：

其中α₁和α₂是给定的控制系数，取值范围为大于0小于等于1的实数，d表示无人机的直径；

考虑实际飞行环境中可能存在障碍物，导致无人机集群需要切换队形通过障碍区域，这时候如果检测到无人机到障碍物的距离小于安全距离，则选择碰撞避免行为，先保障无人机的安全，无人机的安全距离表示为：

其中，a，b为控制参数，取任意大于等于0的实数；D₀代表无人机因为气动因素需要与障碍物保持的最小距离，r_uav代表无人机的半径，V无人机的速度，代表无人机机体坐标系的Y轴与障碍物的夹角；将集群内的其他无人机以及环境中的障碍区域均视为危险区域，当无人机与危险区域的距离小于安全距离时无人机决策为碰撞避免行为，使无人机驶出危险区域；

碰撞避免行为无人机的控制输入为：

其中，V_t-1为上一时刻无人机的速度，k₁和k₂为无人机的控制系数，取任意大于等于0的实数；D_i表示无人机与障碍物的距离，ω_max表示无人机的最大角速度；

在编队飞行的过程中无人机会与集群内的其他无人机保持通信，当检测到通信质量不佳而处于通信失效状态即通信延时大于500ms，那么该无人机会选择离群保持行为；

将无人机集群分为集群区域、离群保持区域、离群区域，根据集群内所有无人机的位置计算出无人机集群的中心，并找到当前队形距离集群中心最远的无人机，记该无人机到集群中心的距离为R₁，R₂＝R₁+2d，R₃＝R₂+2d，d表示无人机的直径，R₁,R₂,R₃分别为集群区域、离群保持区域和离群区域的半径；当无人机处于离群保持行为时，首先根据数据判断当前处于集群的哪个区域,并设此时拟运动距离为r，根据当前无人机的拟运动距离计算出控制输入为：

其中和分别表示距离r和无人机运动正方向与目标点之间的夹角为对时间的微分，v_i表示无人机运动线速度，ω_i表示无人机运动角速度；

最终的控制目标是使得r＝0，得离群保持行为中无人机的控制输入为：

其中α₃和α₄是给定的控制系数，取值范围为大于0小于等于1的实数；当处于通信失效状态下的无人机恢复正常通信状态后，由于已经偏离队形期望位置故采取队形保持行为继续保证编队的正常运行，随后无人机集群继续保持向编队飞行的任务目标运动直到最终完成编队跟踪飞行任务。

10.根据权利要求7所述的基于行为控制的无人机集群编队飞行方法，其特征在于，步骤S22中，采用粒子滤波算法对无人机的无效位置进行预测分析，包括以下步骤：

S221、取初始时刻k＝1，从无人机位置的概率密度函数p(x_k)中抽取一组初始粒子其中表示k时刻从概率密度函数中抽取的第a个粒子，a∈[1,N]，N表示粒子的数量；

S222、对N个粒子进行重要性采样：

其中，粒子a的概率值服从为重要性概率密度函数，符号～表示服从概率密度函数，表示概率值对粒子a初始时刻到k-1时刻的集合z_1:k＝{z₁,z₂,…,z_k)是到时刻k的测量集合，z_k为时刻k是得到的概率测量值；

S223、计算粒子重要性的权值并进行权值归一化：

其中和分别为和 表示概率观测量对应该粒子的概率密度函数，表示同一粒子前后时刻的概率密度函数，为前后时刻的对应粒子的条件概率；

粒子归一化之后的权值为：

其中表示归一化之后的权值，j∈[1,N]，表示每个粒子的权值；

S224、从集合中根据重要性权值重新采样得到新的N个粒子的集合并更新粒子的权值

S225、输出经过粒子滤波的无人机位置数据：

其中表示k时刻粒子滤波之后的无人机的位置数据，表示重新采样得到的粒子。