基于Voronoi图的多无人机编队队形可靠变换方法与流程

本发明属于多无人机航迹规划及动态避障领域，特别是涉及基于voronoi图的多无人机编队队形可靠变换方法。

背景技术：

随着无人机的广泛应用，相比于单架无人机，无人机的编队飞行在完成多样化任务和能够充分收集环境信息、提高任务执行的安全性及可靠性，以及增强系统鲁棒性等方面表现出明显的优势，已成为广大学者的研究热潮。无人机编队协同飞行的过程中，会面临一些未知的因素，比如编队任务发生变化、环境突发改变，面对这种情况，无人机的编队就需要实时调整其队形，调整的过程就需要涉及到实时的航迹规划问题。多架无人机为适应任务要求就需要进行某种队形排列和任务分配，其中编队飞行的队形产生、保持和变化以及飞行任务的规划和组织都是需要考虑的因素。

无人机编队飞行在展现出众多优势的同时，也有不可忽略的局限性。相比单机控制，多机编队飞行队形控制的要求更高，在单机通信、姿态以及控制等方面的基本要求都符合的条件下，还需重点考虑多机之间的协调问题，如任务协同、队形变换以及多机与地面控制中心的通信等。另外，因扰动而可能引起的编队内部碰撞也是限制无人机编队安全飞行的一个主要因素。在多无人机协同飞行的过程中，当编队因外界因素要进行队形调整时，就需要对航迹重新规划。多无人机航迹规划不同于单机，它不仅需要满足多类约束条件，而且规划系统根据实时的状态信息与各无人机的具体任务，为每一架无人机规划出从当前状态到局部目标点的可行航迹，使其在保证安全可靠的基础上完成协同飞行，在指定的性能指标下达到最优或较优。多无人机规划出的航迹对个体无人机来说，可能不是最优的，但将多机视为一个整体系统时，该轨迹就是整体的最优轨迹。另外，多机航迹规划的过程受许多因素的影响，各因素之间并不相互独立，而是存在相互作用的关系，这就增加了多机航迹规划的难度和复杂性。其中，多无人机的航迹规划对实时性的要求较高，由于多机轨迹规划过程相比单机轨迹的规划需要考虑的约束更多，变量更多，因此易受环境等其他因素的影响，从而导致航迹跟踪精度下降，甚至会出现碰撞的危险。因此多机轨迹规划应具备在线实时解算的能力，以应对重构过程中可能遇到的危险。特别是当编队飞行的无人机数量较多时，航迹规划的效率是制约多机轨迹规划的一个重要因素。

针对无人机轨迹规划问题，国内外学者提出了许多具体的解决方案，主要包括：势场法、基于图论的算法、智能优化算法和最优控制法等。其中航迹搜索算法，是无人机能否安全迅速地实现轨迹变换的重要因素。常用的航迹搜索算法有：a*搜索算法、进化算法、遗传算法和粒子群算法等。

timothyw.mclain提出了一种时间协同多无人机航迹规划方法。它以voronoi图为理论，通过离散化和光滑处理，使无人机在遇到障碍物时自动寻找安全航迹，从而使编队完成队形变换。

西工大肖自兵等利用a*定长航迹搜索算法，以代价函数确定最优航路节点，构成固定长度的理想航迹，通过约束提高航迹的可跟踪性，从而可以实现协同飞行的目标，确保编队完成队形变换。

sriramvenkataramanan设计了一种非线性控制器来实现无人机紧密队形的重构。它是利用虚拟长机的控制模式，考虑风场以及涡流的影响，提出了一种可以生成安全重构航迹的算法，解决了紧密队形重构的问题。

r.rajasrss提出了一种无人机队形变换的控制方法。它是通过选择合适的队形向量，以实现队形之间的变换。选取时间变化的队形向量有助于实现圆形观测队形的变换，在保证良好的通信链路下，该控制器可以实现对静止目标或者低速运动目标的观测。

技术实现要素：

本发明的目的在于提供一种基于voronoi图的多无人机编队队形可靠变换方法,以便提高无人机航迹规划效率。

本发明提供的技术方案是：基于voronoi图的多无人机编队队形可靠变换方法，其特征是：包括以下步骤：

步骤1)确定初始队形中每架无人机抵达目标队形中的位置，以整体路径最短为目标，确定对应关系；

步骤2)以初始队形中每架无人机的位置为离散点，用voronoi图对无人机的路径规划空间进行建模，构造voronoi图结构；

步骤3)在voronoi图中，根据步骤1)每架无人机的确定的目标，判断起始点和目标点的位置；

步骤4)在每架无人机完成第一次飞行之后，以当前飞机所在位置，重新构建voronoi图，重复步骤3)，规划所有无人机第二次的路径，依次迭代，直至每架无人机安全到达目标位置，则完成编队的变换。

所述的步骤1)整体路径最短为目标的函数是：

式中符号表达见说明书附表。

所述步骤2)中构造voronoi图结构是由一系列的按照一定规则生成的网格组成，是一种基本的几何结构，voronoi图中的网格将地图空间按照点集划分为很多区域，每个区域到区域内点的距离最小，voronoi图的边界则离周围点集距离最远，给出ω内的一组初始点(i＝1,2,3…n)，定义vi为：

vi＝{y∈ω|||y-xi||＜||y-xj||,j＝1,2,3…n,j≠i}(2)

vi为ω内的voronoi图结构，利用voronoi图对每架飞机的周围环境进行建模，即将无人机的飞行环境用voronoi图进行表示，以便使用算法来进行路径搜寻。

所述步骤3)具体包括：

a.若目标点在起始点所在的voronoi图中，则不考虑其他因素直接飞行，在第一次飞行到达目标点后等候其它无人机完成至目标点的飞行；

b.若目标点不在起始点所在的voronoi图中，则以每架无人机所在voronoi图的顶点为节点数据，用a*算法，在voronoi图中实现每架无人机的全局路径规划；用a*算法，在voronoi图中实现每架无人机的全局路径规划包括：

b1.判断规划的全局路径，如若没有轨迹交叉，则在节点处设置局部目标点，采用人工势场法，从起始点到最近节点的内部轨迹为每架无人机的第一次飞行；

b2.若轨迹交叉，则在交叉点设置障碍物点，且在无人机位置距离交叉点路程的1/2处重新设置节点，采用a*算法再次规划全局路径，在新节点处和轨迹与当前voronoi图边界的交叉处设置局部目标点，用人工势场法引导无人机运动，则无人机到第二个局部目标点的voronoi图内部轨迹为每架无人机的第一次飞行。

所述的a*算法在voronoi图中进行全局路径规划，a*算法加入了启发函数h(n)，从而提高了搜索效率，a*算法采用的代价函数为

f(n)＝g(n)+h(n)(3)

式中，f(n)为节点n的预测总代价。g(n)为从初始位置到节点n已经花费的代价，h(n)为节点到目标位置的预测代价。

所述步骤4)具体包括：重新构建voronoi图，依次迭代，直至每架无人机到达目标位置，则完成无人机队形的变换，否则，采用分步模式，在每架无人机到达当前局部目标点时重构建voronoi图，直到每架无人机到达目标点。

所述的人工势场法通过在运动目标点设置对全局的引力场，障碍物设置一定范围的斥力场，引力与斥力的合力引导物体进行运动。

本发明的有益效果在于：

1.在本发明中，采用voronoi图对无人机环境进行建模。在a*算法规划路径的过程中，以voronoi图的多边形顶点为节点，开启全局路径的搜索。

2.本发明中，由于算法在动态环境中运行，为提高算法的实际可用性，在算法增加判断条件，合理设置障碍物点和局部目标点。

3.本发明中提出了一种基于a*算法和人工势场法的路径规划算法，根据设置的约束条件，在全局路径上设置局部目标将复杂的环境划分为简单的环境，然后用人工势场法进行局部路径规划，以解决全局路径规划和规避动态障碍物的问题。

4.在本发明中，采用分步模式，在每架无人机到达当前局部目标点时重新构建voronoi图，直到每架无人机安全到达目标点，保证了无人机的每次飞行都在所在的voronoi图内飞行以避免机间碰撞。

附图说明

图1三角形队形下的voronoi图结构；

图2方形队形下的voronoi图结构；

图3十架无人机起始voronoi图及对应关系；

图4十架无人机第一次飞行规划；

图5十架无人机第一次飞行轨迹(实线)；

图6未到达目标位置的无人机第二次飞行前的voronoi图结构；

图7未到达目标位置的无人机的第二次飞行轨迹(实线)；

图8十架无人机第三次飞行前的voronoi图结构；

图9未到达目标位置的无人机的第三次飞行轨迹(实线)；

图10未到达目标位置的无人机的第四飞行前的voronoi图结构；

图11未到达目标位置的无人机的第四次飞行轨迹(完成变换)。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面结合附图对本发明做进一步的描述。为了验证算法的鲁棒性，我们取10架无人机在140*140的空间随机给定它们位置，结合算法对三角形队形变换到方形队形。本发明包含以下步骤：

步骤1)确定初始队形中每架无人机抵达目标队形中的位置，以整体路径最短为目标，确定对应关系。图3为从三角形变换到方形下的10架无人机的对应关系，其中s为起始位置，uav为目标位置。

多无人机队形变换具有变量多、约束条件复杂等特点，同时对动态性要求较高，这就使航迹规划必须具有实时解算的能力。为应对这一要求，可以从模型降维和提高搜索效率等两方面入手。

如图3所示，以二维平面内同构的十架无人机为研究对象，以编队按最短路径完成队形重构为目标。假设无人机速度变化的范围不大，且对协同飞行的要求较高，因此队形重构的目标主要依靠航迹的实时规划来实现。重构的队形中，不考虑重构过程中无人机与外部障碍物的碰撞，仅研究构型内部的防撞处理。

初始状态队形变换到目标状态队形时，首先须知当前队形下无人机即将去往目标队形的哪一个目标位置。初始状态下，各无人机在惯性坐标系中的位置pi(a,b)均可以实时获取。在某一时刻，系统接收到指定命令，要求编队变换队形，以执行新的任务。初始状态每架无人机的坐标pi(a,b)已知，目标状态队形变换后的每架无人机的位置也已知qi(x,y)。以所有无人机飞过的总航迹最短为目标来规划每架无人机到目标队形的位置。无人机规划出的航迹对个体无人机来说，可能不是最短的，但将多机视为一个整体系统时，该轨迹就是整体的最短路径。

假设此时无人机的初始队形为三角形，如图1所示，且每架飞机的位置已知，设定目标队形为方形且位置已知，如图2所示，取整体最短路径s为目标函数：

以整体编队队形到目标队形的最短路径为目的，求出形变换中每架无人机之间的对应关系，即找到当前每架无人机到目标队形的目标位置。计算整体最短路径，寻求每架飞机到达目标点的位置，开始对其进行规划。

步骤2)用voronoi图对无人机的路径规划空间进行建模，以初始队形中每架无人机的位置为离散点，构造voronoi图结构。图1是三角形队形下的voronoi图结构，图2是方形队形下的voronoi图结构。

voronoi图是由一系列的按照一定规则生成的网格组成，是一种基本的几何结构。voronoi图中的网格将地图空间按照点集划分为很多区域，每个区域到区域内点的距离最小，voronoi图的边界则离周围点集距离最远。给出ω内的一组初始点(i＝1,2,3…n)，定义vi为：

vi＝{y∈ω|||y-xi||＜||y-xj||,j＝1,2,3…n,j≠i}

(2)

vi为ω内的voronoi图结构，利用voronoi图对每架飞机的周围环境进行建模，即将无人机的飞行环境用voronoi图进行表示，以便使用算法来进行路径搜寻。

建立voronoi图有多种方法，最常用也是效率最快的是delaunay三角形剖分算法，采用bowyer-watson算法将离散点按照规则连接成标准的三角形网格，构造delaunay三角网格。

步骤3)在voronoi图中，根据步骤1)每架无人机的确定的目标，判断起始点和目标点的位置，规划飞行轨迹。

a.若目标点在起始点所在的voronoi图中，则不考虑其他因素直接飞行，在第一次飞行到达目标点后等候其它无人机完成至目标点的飞行。如图1所示中的无人机1、2、3、4、6和10号分别抵达各自的目标位置后等待其它无人机到达目标点以完成队形变换。

b.若目标点不在起始点所在的voronoi图中，则以每架无人机所在voronoi图的顶点为节点数据，用a*算法，在voronoi图中实现每架无人机的全局路径规划。如图1所示中5、7、8、9号无人机的轨迹。

b1.判断规划的全局路径，如若没有轨迹交叉，则在节点处设置局部目标点，采用人工势场法，从起始点到最近节点的内部轨迹为每架无人机的第一次飞行。如图4中的5、7号无人机(实线)轨迹所示。

b2.若轨迹交叉，则在交叉点设置障碍物点，且在无人机位置距离交叉点路程的1/2处重新设置节点，采用a*算法再次规划全局路径，在新节点处和轨迹与当前voronoi图边界的交叉处设置局部目标点，用人工势场法引导无人机运动，则无人机到第二个局部目标点(1/2处新节点下a*全局路径与voronoi图交叉的边界点)的voronoi图内部轨迹为每架无人机的第一次飞行。如图5中8、9号无人机(实线)轨迹所示，o为设置的障碍物点，q为所设置的节点，q、m为设置的局部目标点。

a*算法在voronoi图中进行全局路径规划，a*算法加入了启发函数h(n)，从而提高了搜索效率。a*算法采用的代价函数为

f(n)＝g(n)+h(n)(3)

式中，f(n)为节点n的预测总代价。g(n)为从初始位置到节点n已经花费的代价，h(n)为节点到目标位置的预测代价。

经典的a*搜索算法经过改造就可以实现在voronoi图中进行最短路径搜索，将voronoi图的各自无人机所在的多边形的顶点代替栅格节点，利用a*搜索算法的结构。在出现轨迹有交叉时，在无人机与顶点轨迹的1/2处设置节点，采用a*重新规划全局路径。

人工势场法经常运用于局部路径规划，算法通过在运动目标点设置对全局的引力场，障碍物设置一定范围的斥力场，引力与斥力的合力引导物体进行运动。人工势场法由于其良好的避障性能，在局部路径规划中应用十分广泛。

力场是一个矢量场，空间中每一个位置的力场都会有大小和方向并且值都是确定的。可以将无人机看作一个质点，在某一时刻力场对这个质点的作用力f都是唯一确定的。作用力f是由势场函数通过微分的方法得到，势场一般由两种性质的场决定，一种是来自目标位置的引力场，它对物体产生引力，在引力的作用下使物体接近目标位置，引力的大小与目标与物体的距离成正比，目标与物体距离越远，引力越大；第二种是来自障碍物的斥力场，斥力场产生斥力，使物体远离障碍物，斥力场只在障碍物周围一段距离内存在，超出这一个范围后障碍物的斥力场变为零，这是斥力场与引力场的不同所在。物体在引力和斥力的合力引导下运动，逐步到达目标位置。传统的引力场的表达式为如式(4)所示：

式中，uatt(q)为引力场数值，pg(q)为到目标位置的距离，k为引力增益常数。引力增益常数k可以用来调节引力的大小，从引力场的表达式可以看到，引力场的数值与离目标的距离pg(q)的二次方成正比，这将导致离目标越远的位置梯度越大。

传统斥力场的函数表达式为如式(5)所示：

式中urep(q)为斥力场数值，p(q)为与障碍物的距离，η为斥力增益常数，p0为障碍物的影响范围。

运动方向由力场函数取负梯度产生，如式(6)(7)所示：

fatt(q)＝-kpg(q)(6)

式中fatt(q)为引力，frep(q)为斥力。

总的场就是引力场和斥力场的叠加，对合场求梯度就得到物体应该运动的方向，如式(8)(9)所示。

u(q)＝uatt(q)+urep(q)(8)

无人机在复杂的空间做队形变换时，周围环境是未知的。在路径规划的算法中，不仅要确保搜索到最优路径，而且要保证无人机具备良好的避障性能，现有的算法中很少有能够同时到达这两个要求的。

在规避动态障碍物的方法中，人工势场法由于具备良好的避障性能被广泛的应用，但是用人工势场法规划路径只用到了局部的环境信息，会导致局部极小值问题。我们采用全局路径规划的方法对全局路径进行搜索，在全局路径上设置局局部目标，然后用人工势场法进行局部路径规划。

采用a*算法为每架无人机做全局路径规划，为解决基于栅格的a*算法规划的路径紧靠障碍物、解算时间长等问题，使用voronoi图构造全局路径规划，使a*算法在voronoi图下搜索到最优的路径，增加判断条件，分析轨迹走势，合理设置障碍物点以及在全局路径上设置局部目标点，由人工势场法以局部目标点为终点引导无人机运动，这样就解决了全局路径搜索和规避动态障碍物的问题。

步骤4)在每架无人机完成第一次飞行之后，以当前飞机所在位置，重新构建voronoi图，重复步骤3)，规划所有无人机第二次的路径，依次迭代，直至每架无人机安全到达目标位置，则完成编队的变换。

在每架无人机完成第一次飞行之后，以当前无人机所在位置，重新构建voronoi图，如图6所示，是每架无人机在完成第一次飞行之后，在第二次飞行前每架无人机当前位置生成的voronoi图。规划所有无人机第二次的路径，依次迭代，直至每架无人机安全到达目标位置，则完成编队的变换。判定此时8、9号的目标位置在所在的voronoi图内，则直接飞行至目标点完成第二次飞行，判定5、7号的目标点不在voronoi图中，则以所在voronoi图多边形顶点为节点，采用a*规划全局路径，根据规划的全局路径，判断没有轨迹交叉，则在节点处设置局部目标点，采用人工势场法引导无人机运动。图7为未到达目标位置的5、7、8、9号无人机的第二次飞行轨迹(实线)。图8为完成第二次飞行之后当前无人机的voronoi图，判定此时5、7号的目标点不在voronoi图中，则以所在voronoi图多边形顶点为节点，采用a*规划全局路径，根据规划的全局路径，判断没有轨迹交叉，则在节点处设置局部目标点，采用人工势场法引导无人机运动。图9为未达到目标位置的5、7号无人机的第三次飞行轨迹(实线)。图10为第三次飞行之后，当前无人机所在位置的voronoi图结构。判定此时5、7号的目标位置在当前的voronoi图内，则直接到达目标到达目标点，完成第四次飞行。图11为未到达目标位置的5、7号无人机的第四次飞行路径，至此全部无人机都到达自己的目标位置，则完成整个队形由三角形到方形的变换。

本发明说明书和权利要求书中的公式或函数的符号含义如下：

以上所述仅为本发明的验证实施例，并不用以限制本发明，凡在本发明技术思想下所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。