复杂未知环境下分布式集群无人机编队飞行轨迹生成方法与流程

1.本发明涉及无人机协同控制技术领域，具体涉及一种复杂未知环境下分布式集群无人机编队飞行轨迹生成方法。


背景技术：

2.近年来，无人机的自主导航技术日益成熟，但是单架无人机在执行任务时仍然存在单机载荷小、续航时间短、系统容错率低等不足，而集群无人机则通过多机冗余系统加强了执行任务的能力，有效地弥补了上述缺陷，因此发展集群无人机技术迫在眉睫。
3.而在集群无人机技术中有一类技术叫集群无人机编队飞行，即根据任务需求，集群无人机保持一定的编队队形飞行，这样的做法可以显著提升集群无人机执行特定任务的协作能力，在军事和民生领域都有非常多的应用，例如保持圆形编队队形可以提高集群对同一信号源的感知能力，保持一字型编队队形可以提高集群的覆盖范围等。
4.集群无人机在复杂未知环境下执行编队导航任务时，需要至少保证以下三点：a.保持无人机集群编队队形；b.在复杂未知环境下保证飞行安全；c.具备在飞行过程中切换编队队形的能力。集群无人机通过相互之间的通讯了解自己与集群的实时状态，并根据实时状态调整自身的飞行轨迹以保持无人机集群编队队形，考虑到通讯延时和机载运算能力不足，飞行轨迹的计算需要采用由粗糙到细致的指导思想，通过多层轨迹规划框架，在不同框架中侧重考虑不同的飞行需求，对于延续整个任务的飞行需求，例如编队飞行，可以在每层的规划框架中都予以优先级，而对于瞬时性很强的飞行需求，例如障碍物避障，可以在最后一层实际执行的飞行轨迹规划中予以优先级。受文献
1.启发，上述思想的集群无人机编队飞行轨迹生成框架，具备灵活处理复杂未知环境下编队飞行的能力，同时由于多层轨迹规划框架的设计，集群无人机可以在大幅减少运算资源消耗的前提下，完成高质量、高精度、高灵活性的编队飞行任务。
5.目前在集群编队飞行领域有许多方法被提出，例如虚拟结构法，导航函数法，反应行为法和基于一致性理论的局部反馈控制法等等。但是以上这些方法大部分只能够在没有外部障碍物的空旷场景中实现无人机集群编队飞行。
6.在复杂环境中，编队飞行任务的难度大幅提高，主要因为各无人机在保持期望队形的同时还要满足集群飞行的安全性。有一类方法是通过设计反馈控制律来同时实现无人机的避障和队形维护。文献
2.中，han等人提出了一种以复拉普拉斯矩阵作为反馈增益的编队飞行控制器，在飞行过程中队形的尺寸大小由领航者(长机)决定，领航者通过控制整体队形的大小来使集群完成特定飞行任务，例如通过一条窄缝。在文献
3.中，zhao提出了一个基于领航者－跟随者框架的控制律，该控制律可以使得无人机队形可以根据环境的变化进行仿射变换。大部分的反馈控制方法都采用了领航者－跟随者的框架，但这种模式使得编队队形的参数只由领航者控制，一旦领航者出现故障，那么整个集群的飞行将会失败。
7.相比于上述的领航者－跟随者框架，去中心化的策略可以更好地应对集群中部分无人机出现故障的情况。文献
4.中，alonso－mora等人把集群无人机的轨迹规划分为两个
阶段，第一个阶段是根据当前的地图信息对最优的队形参数以及相应的无人机－目标点分配关系进行计算，第二个阶段是根据第一阶段的计算结果，各无人机独自进行局部的轨迹规划来到达所分配的目标点。这种方法可以实现无人机集群对障碍物的躲避，但因为第二阶段中未考虑编队的要求所以队形保持的实时性很差。文献
5.中，zhou等人采用虚拟结构的方法，并结合人工势场法来生成无碰撞的编队飞行轨迹，但由于叠加的人工势场中容易出现局部最优点，使用该方法的无人机集群容易在飞行过程中产生轨迹震荡乃至死锁，同时也无法保证整体轨迹的最优性。文献
6.中，pary等人提出了使用分布式模型预测控制的方法来实现集群编队飞行，该方法主要通过对无人机施加相对位置约束来保持几何队形。在飞行过程中，一旦环境中出现障碍物使得无人机相对位置的要求无法被满足，该框架则会放弃编队协同的要求来满足飞行的安全性。这种被动的机制虽然可以保证避障，但极大地降低了集群系统的协同性。
8.参考文献：
9.[1]x.zhou，z.wang，x.wen，j.zhu，c.xu，and f.gao，“decentralized spatial－temporal trajectory planning for multicopter swarms，”arxiv preprint arxiv：，2021
[0010]
[2]z.han，l.wang，and z.lin，“local formation control strategies with undetermined and determined formation scales for co－leader vehicle networks，”in 52nd ieee conference on decision and control.ieee，2013，pp.7339
–
7344.
[0011]
[3]s.zhao，“affifine formation maneuver control of multiagent systems，”ieee transactions on automatic control，vol.63，no.12，pp.4140
–
4155，2018.
[0012]
[4]j.alonso－mora，e.montijano，m.schwager，and d.rus，“distributed multi－robot formation control among obstacles：a geometric and optimization approach with consensus，”in 2016 ieee international conference on robotics and automation(icra).ieee，2016，pp.5356
–
5363.
[0013]
[5]d.zhou，z.wang，and m.schwager，“agile coordination and assistive collision avoidance for quadrotor swarms using virtual structures，”ieee transactions on robotics，vol.34，no.4，pp.916
–
923，2018.
[0014]
[6]r.van parys and g.pipeleers，“distributed model predictive formation control with inter－vehicle collision avoidance，”in 2017 11th asian control conference(ascc).ieee，2017，pp.2399
–
2404.


技术实现要素：

[0015]
针对现有技术的不足，本发明旨在提供一种复杂未知环境下分布式集群无人机编队飞行轨迹生成方法。
[0016]
为了实现上述目的，本发明采用如下技术方案：
[0017]
复杂未知环境下分布式集群无人机编队飞行轨迹生成方法，包括如下五个步骤：
[0018]
(1)用户需求初始化：在自动导航任务开始前，用户需要根据任务类型定义集群无
人机编队的参数，包括无人机数量、期望编队队形、集群几何中心的目标终点和编队任务初始分配；用户完成定义后，集群无人机会按照用户的定义分布式启动，执行后续任务；
[0019]
(2)全局轨迹：首先在不考虑障碍物的情况下，生成一条连接无人机当前位置和目标位置的全局轨迹，作为理想情况下集群无人机编队飞行的指导轨迹；在全局轨迹的基础上，选择从当前位置飞行设定时间后到达的局部终点作为后续轨迹规划的末状态；得到集群无人机的局部终点集合后，判断在该局部终点集合情况下，编队任务分配是否最优，如果是最优，则以当前的局部终点集合对应的任务分配开始后续步骤，如果不是最优，则进行编队任务重分配，以计算后的局部终点集合对应的最优任务分配开始后续步骤；
[0020]
(3)前端参考轨迹：首先是以无人机当前位置和局部终点作为首状态和末状态，进行前端多拓扑路径搜索，经过采样和剪枝后得到了复杂环境中的拓扑路径，之后基于编队队形最优的指标，选出多拓扑路径中编队队形最优的拓扑，并在最优拓扑的基础上生成高阶连续的前端参考轨迹；
[0021]
(4)对步骤(3)生成的前端参考轨迹进行后端优化轨迹：为了生成高质量的局部轨迹，构造一个多目标的轨迹优化问题，同时考虑无人机避障、编队飞行相似性因素，通过离散轨迹的形式降低优化变量数量，以保证优化求解实时性，得到后端优化轨迹后还会再次检查是否安全，如果安全则执行该轨迹，如果不安全则弱化编队飞行相似性因素的考虑，再次优化直到得到安全的轨迹为止，然后执行该轨迹；
[0022]
(5)重规划轨迹检查：由于无人机的传感器有效视野有限，飞行轨迹可能会与刚进入传感器视野的障碍物发生碰撞，因此以1毫秒/次的检查频率判断轨迹是否与环境障碍物发生碰撞，如果存在危险，则开启返回至步骤(2)进行轨迹重规划，同时基于滚动优化的策略，重规划时会根据全局轨迹分配更新的局部终点。
[0023]
进一步地，步骤(1)的具体过程为：
[0024]
在用户需求初始化步骤，需要定义集群无人机数量n，代表该集群有n架无人机，每架无人机的编号为i＝1，2，
…
，n；采用图结构描述无人机的编队队形，定义一个无向全连接图g＝(v，e)，其中v：＝{1，2，...，n}代表顶点的集合，代表边的集合；在图g中，顶点i表示第i个无人机的位置坐标pi，边e
i,j
代表第i个无人机和第j个无人机之间的相互关系；还需要定义集群几何中心的终点坐标pg，而每架无人机的终点坐标则由其在编队队形中的位置进行解算，p
i,g
＝pg δp
i.o
，其中δp
i.o
代表第i架无人机在理想编队队形中相对于集群几何中心的位移；最后还需要定义编队任务初始分配，对于n架无人机的集群，编队任务集合为a＝{a1,
…
,an}，ai代表第i架无人机的编队分配，即第i架无人机对应于编队队形中的相对位置。
[0025]
进一步地，步骤(2)的具体过程为：
[0026]
首先在不考虑障碍物的情况下，规划一条连接无人机当前状态和全局终点的轨迹；所述轨迹用分段多项式表示，一条有m个分段的三维轨迹定义为：
[0027][0028]
其中t为当前时刻，tk为第k段轨迹的轨迹时间；
[0029]
其中第k段分段轨迹由一条高阶多项式表示为：
[0030]
[0031]
其中c为分段多项式的系数，t为分段多项式的时间分布，tk为第k段多项式的总时间，β(t)为时间基元；规划全局轨迹时，为了给后续轨迹优化提供更合适的方向指引和时间分布，全局轨迹兼顾考虑无人机飞行轨迹的能量消耗和飞行总时间，求解下列优化问题：
[0032][0033]
s.t.p(0)＝p0[0034]
p(t)＝pg[0035]
其中，p0为无人机的当前位置坐标，m是控制输入所对应的阶次；
[0036]
获得全局轨迹后，根据实际应用场景和传感器的感知范围，设置不同的局部规划区域，并求解全局轨迹与局部规划区域的交点，即为局部终点；
[0037]
在全局轨迹上查找出集群中各无人机的局部终点位置储存为向量h，将期望编队队形中各无人机的位置储存为向量s；在生成局部轨迹前需要根据h和s将局部终点调整为符合期望编队队形的局部目标点，并根据调整后局部目标点对各无人机进行任务分配，因此需要分为两部分计算：编队队形对齐和编队任务分配；
[0038]
在根据局部终点重新生成符合期望编队队形的局部目标点时，仅考虑期望编队队形的平移和放缩，而不考虑编队队形的旋转；
[0039]
将期望编队队形经过平移和尺寸放缩变换后的坐标，即对齐后的坐标，记为q，则有：
[0040]
qj＝αsj d
[0041]
其中sj表示期望编队队形中的第j个位置的坐标点，j＝1,2,
…
,n，α为尺寸放缩的比例系数，d为平移向量；在给定对齐之后，将对齐后的坐标到各局部终点坐标的欧式距离记为成本项：
[0042][0043]
根据此成本项构建分配问题的成本函数，以及对应的整数规划问题为：
[0044][0045][0046][0047]
x
ij
＝{0，1}，i，j＝1，...，n
[0048]
其中x
ij
为分配变量，当x
ij
＝1时代表第i个无人机被分配给队形中的第j个坐标；具体过程为：
[0049]
首先根据局部终点位置h以及期望队形s，重新构建整数规划问题：
[0050][0051][0052]
然后利用分配算法，对上述整数规划问题进行求解，得到最优分配x＊以及相应的最小成本函数值k＊；
[0053]
其后，按下式计算参数s和ss，以及ds；
[0054][0055]
计算最优对齐的参数α＊和d＊：
[0056][0057][0058]
此时已求出对齐参数α＊、d＊和最优分配x＊，则最优对齐坐标为：
[0059]q*
＝α
*
s d
*
[0060]
得到最优对齐坐标后，即可按照x＊分配给各无人机作为局部目标点；此时的局部目标点由全局轨迹上查询出的各无人机局部终点位置优化得出，使其能够满足期望编队队形的几何分布。
[0061]
进一步地，步骤(3)的具体过程为：
[0062]
在前端参考轨迹生成步骤，首先是基于无人机当前位置和局部目标点进行前端多拓扑路径搜索，经过采样和剪枝后得到了复杂环境中的拓扑路径。
[0063]
更进一步地，步骤(3)中，拓扑路径生成具体采用的是概率路线图方法，步骤如下：
[0064]
(1)初始化采样树，根据采样起点和采样终点设置采样空间，并将采样起点和采样终点添加进采样树中；
[0065]
(2)判断采样点总数是否小于设定的阈值，如果小于则结束，否则继续；在采样空间中随机采样获得一个采样点，判断其是否在障碍物中，如果在，则重复第(2)步，如果不在，则进入第(3)步；
[0066]
(3)判断采样点是否能与采样树中的节点无碰撞连接，无碰撞连接是指采样点与节点的连线没有穿过障碍物，如果不能，则将该采样点添加进采样树中，并重复第(2)步，如果能，则进入第(4)步；
[0067]
(4)对于可以与采样树中的节点无碰撞连接的采样点，需要判断其是否能够替换当前采样树中的节点，判断标准为采样点与其邻居节点构成的路径长度是否小于原节点与相同邻居节点构成的路径长度，如果小于，则用采样点替换当前采样树中的节点，并重复第(2)步。
[0068]
更进一步地，步骤(3)中，生成复杂环境中的拓扑路径的具体步骤如下：
[0069]
1)首先对采样树进行广度优先搜索，寻找出连接搜索起点和搜索终点的所有路径，之后对这些路径进行拓扑判断，将归属于同一拓扑的路径归为一类，并保留其中长度最短的一条路径；
[0070]
2)对保留的路径进行剪枝，由于原始路径是在有限次采样后得到的，不能保证获
得该拓扑下的最短路径，剪枝过程中，由搜索起点出发向原始路径发出射线，直到射线碰上障碍物后，将上一射线作为剪枝后路径的一部分，由此循环直到射线终点抵达搜索终点，即可完成路径剪枝；
[0071]
3)最后得到若干条剪枝后的拓扑路径，并对这些路径两两进行是否为同拓扑路径的判断，如果两条路径互为同拓扑路径，则保留其中最短路径；判断是否为同拓扑路径的定义为：给定两条路径ξ1(r)和ξ2(r)，其中r代表路径的参数化，且r∈[0,1]，若两条路径满足ξ1(0)＝ξ2(0)，ξ1(1)＝ξ2(1)，即有相同起点和终点，且对于连线没有碰上障碍物，则两条路径为同拓扑路径，反之则为不同拓扑路径；
[0072]
4)以飞行距离为依据，从步骤3)得到的几条拓扑路径中筛选出最短路径作为参考轨迹。
[0073]
进一步地，步骤(4)的具体过程为：
[0074]
后端轨迹优化在前端参考轨迹的基础上，同时考虑无人机安全避障、编队队形飞行的因素，构成无约束优化问题，优化求解得到更高质量的轨迹；构造的后端轨迹的无约束优化问题形式如下：
[0075][0076]
其中分段多项式系数c和时间分布t为优化变量，带下标的j表示某一目标函数或者约束惩罚项，λ表示对应的权重系数，下标{e，a，o，f，b，h}分别代表能量成本项、轨迹总时间项、安全避障约束项、编队队形相似性项、集群间相互避碰项以及动力学可行性项；之后，基于梯度信息对上述优化问题进行迭代优化，最后为当前无人机生成出一条安全无碰撞的可维护队形的高质量轨迹。
[0077]
本发明的有益效果在于：本发明提出了一个针对集群无人机在复杂未知环境下编队飞行的通用轨迹规划框架，该框架采用多层轨迹规划结构，通过用户需求初始化、全局轨迹规划、前端参考轨迹规划、后端轨迹优化、重规划状态检查等多层架构分布式地生成高质量的集群无人机编队飞行轨迹。与目前其他方法相比，本发明方法基于由粗糙到细致的轨迹规划思想，通过多层轨迹规划生成局部飞行轨迹，在保证局部轨迹高质量的同时，还保证了实时性，可以应对复杂未知环境。除此之外，多层轨迹规划的结构具有高度灵活性，在不同的层级分配了编队飞行的优先级，方便用户定义编队任务，对工程实际应用具有指意义。
附图说明
[0078]
图1为本发明实施例1中方法原理示意图；
[0079]
图2为本发明实施例1中集群无人机编队队形全连接示意图；
[0080]
图3为本发明实施例1中全局轨迹和局部轨迹关系示意图；
[0081]
图4为本发明实施例1中最优集群局部终点编队对齐和编队分配示意图；
[0082]
图5为本发明实施例1中拓扑路径搜索示意图；
[0083]
图6为本发明实施例1中拓扑路径剪枝效果示意图；
[0084]
图7为本发明实施例2中大规模集群心形编队飞行仿真实验结果示意图。
具体实施方式
[0085]
以下将结合附图对本发明作进一步的描述，需要说明的是，本实施例以本技术方案为前提，给出了详细的实施方式和具体的操作过程，但本发明的保护范围并不限于本实施例。
[0086]
实施例1
[0087]
本实施例提供一种复杂未知环境下分布式集群无人机编队飞行轨迹生成方法，该方法基于由粗糙到细致的轨迹规划思想，通过多层轨迹规划最终生成高质量的局部飞行轨迹，用于复杂环境下集群无人机编队飞行轨迹的生成。
[0088]
如图1所示，所述方法主要包括如下五个步骤：
[0089]
(1)用户需求初始化：在自动导航任务开始前，用户需要根据任务类型定义集群无人机编队的参数，在整个任务过程中这是唯一一次用户交互，在用户完成无人机数量、期望编队队形、集群几何中心的目标终点和编队任务初始分配的定义后，集群无人机会按照用户的定义分布式启动，执行后续任务；
[0090]
(2)全局轨迹：首先进行全局轨迹规划，即在不考虑障碍物的情况下，快速生成一条连接无人机当前位置和目标位置的全局轨迹，作为理想情况下集群编队飞行的指导轨迹；在全局轨迹的基础上，选择从当前位置飞行设定时间后到达的局部终点作为后续轨迹规划的末状态，由此得到集群无人机的局部终点集合后，判断在该局部终点集合情况下，编队任务分配是否最优，如果是最优，则以当前的局部终点集合对应的任务分配开始后续步骤，如果不是最优，则进行编队任务重分配，以计算后的局部终点集合对应的最优任务分配开始后续步骤；
[0091]
(3)前端参考轨迹：首先是以无人机当前位置和局部终点作为首状态和末状态，进行前端多拓扑路径搜索，经过采样和剪枝后得到了复杂环境中的拓扑路径，之后基于编队队形最优的指标，选出多拓扑路径中编队队形最优的拓扑，并在最优拓扑的基础上生成高阶连续的前端参考轨迹；
[0092]
(4)对步骤(3)生成的前端参考轨迹进行后端优化轨迹：为了生成高质量的局部轨迹，构造一个多目标的轨迹优化问题，同时考虑无人机避障、编队飞行相似性因素，通过离散轨迹的形式降低优化变量数量，以保证优化求解实时性，得到后端优化轨迹后还会再次检查是否安全，如果安全则执行该轨迹，如果不安全则弱化编队飞行相似性因素的考虑，再次优化直到得到安全的轨迹为止，然后执行该轨迹；
[0093]
(5)重规划轨迹检查：由于无人机的传感器有效视野有限，飞行轨迹可能会与刚进入传感器视野的障碍物发生碰撞，因此以1毫秒/次的检查频率判断轨迹是否与环境障碍物发生碰撞，如果存在危险，则开启返回至步骤(2)进行轨迹重规划，同时基于滚动优化的策略，重规划时会根据全局轨迹分配更新的局部终点。
[0094]
具体地，步骤(1)的具体过程为：
[0095]
在用户需求初始化步骤，需要定义集群无人机数量n，代表该集群有n架无人机，每架无人机的编号为i＝1，2，
…
，n；采用图结构描述无人机的编队队形，定义一个无向全连接图g＝(v，e)，其中v＝{1，2，...，n}代表顶点的集合，代表边的集合；在图g中，顶点i表示第i个无人机的位置坐标pi，边e
i,j
代表第i个无人机和第j个无人机之间的相互关系；还需要定义集群几何中心的终点坐标pg，而每架无人机的终点坐标则由其在编队队形中的
位置进行解算，即pi,g＝pg δp
i.o
，其中δp
i.o
代表第i架无人机在期望编队队形中相对于集群几何中心的位移；最后还需要定义编队任务初始分配，对于n架无人机的集群，编队任务集合为a＝{a1,
…
,an}，ai代表第i架无人机的编队分配，即第i架无人机在编队队形中的相对位置。
[0096]
图2为n＝5时的无人机编队示例图。
[0097]
在本实施例中，步骤(2)的具体过程为：
[0098]
首先在不考虑障碍物的情况下，规划一条连接无人机当前状态和全局终点的轨迹。所述轨迹用分段多项式表示，一条有m个分段的三维轨迹定义为：
[0099][0100]
其中t为当前时刻，tk为第k段轨迹的轨迹时间；
[0101]
其中第k段分段轨迹由一条高阶多项式表示为：
[0102][0103]
其中c为分段多项式的系数，t为分段多项式的时间分布，tk为第k段多项式的总时间，β(t)为时间基元；规划全局轨迹时，为了给后续轨迹优化提供更合适的方向指引和时间分布，全局轨迹兼顾考虑无人机飞行轨迹的能量消耗和飞行总时间，求解下列优化问题：
[0104][0105]
s.t.p(0)＝p0[0106]
p(t)＝pg[0107]
其中，p0为无人机的当前位置坐标，m是控制输入所对应的阶次。
[0108]
获得全局轨迹后，根据实际应用场景和传感器的感知范围，设置不同的局部规划区域，并求解全局轨迹与局部规划区域的交点，即为局部终点。
[0109]
图3所示为全局轨迹和局部轨迹关系示意图。本实施例方法定义局部终点为全局轨迹与局部感知范围的交点，但在后端优化轨迹时，还需要对局部终点进行调整得到局部目标点，后者为局部轨迹的实际终点。
[0110]
在全局轨迹上查找出集群中各无人机的局部终点位置储存为向量h，将期望编队队形中各无人机的位置储存为向量s。在生成局部轨迹前需要根据h和s将局部终点调整为符合期望编队队形的局部目标点，并根据调整后局部目标点对各无人机进行任务分配，因此需要分为两部分计算：编队队形对齐(alignment)和编队任务分配(assignment)。
[0111]
本实施例方法在根据局部终点重新生成符合期望编队队形的局部目标点时，仅考虑期望编队队形的平移和放缩，而不考虑编队队形的旋转。在这种情况下，对齐问题和分配问题可以同时被优化，其中对齐问题的最优解可以被解析得出，而不需要迭代对齐－分配的过程来逼近最优。
[0112]
将期望编队队形经过平移和尺寸放缩变换后的坐标，即对齐后的坐标，记为q，则有：
[0113]
qj＝αsj d
[0114]
其中sj表示期望编队队形中的第j个位置的坐标点，j＝1,2,
…
,n，α为尺寸放缩的
比例系数，d为平移向量。在给定对齐之后，本实施例中将对齐后的坐标到各局部终点坐标的欧式距离记为成本项：
[0115][0116]
根据此成本项构建分配问题的成本函数，以及对应的整数规划问题为：
[0117][0118][0119][0120]
x
ij
＝{0，1}，i，j＝1，...，n
[0121]
其中x
ij
为分配变量，当x
ij
＝1时代表第i个无人机被分配给队形中的第j个坐标。具体过程为：
[0122]
首先根据局部终点位置h以及期望队形s，重新构建整数规划问题：
[0123][0124][0125]
然后利用分配算法，例如匈牙利算法，对上述整数规划问题进行求解，得到最优分配x＊以及相应的最小成本函数值k＊。
[0126]
其后，按下式计算参数s和ss，以及ds。
[0127][0128]
计算最优对齐的参数α＊和d＊：
[0129][0130][0131]
此时已求出对齐参数α＊、d＊和最优分配x＊，则最优对齐坐标为：
[0132]q*
＝α
*
s d
*
[0133]
得到最优对齐坐标后，即可按照x＊分配给各无人机作为局部目标点。此时的局部目标点由全局轨迹上查询出的各无人机局部终点位置优化得出，使其能够满足期望编队队形的几何分布。图4所示为最优集群局部终点编队对齐和编队分配示意图。
[0134]
进一步地，在本实施例中，步骤(3)的具体过程为：
[0135]
在前端参考轨迹生成步骤，首先是基于无人机当前位置和局部目标点进行前端多拓扑路径搜索，经过采样和剪枝后得到了复杂环境中的拓扑路径。
[0136]
图5为拓扑路径搜索示意图。拓扑路径生成具体采用的是概率路线图方法，步骤如
下：
[0137]
(1)初始化采样树，根据采样起点和采样终点设置采样空间，并将采样起点和采样终点添加进采样树中；
[0138]
(2)判断采样点总数是否小于设定的阈值，如果是则结束，否则继续。在采样空间中随机采样获得一个采样点，判断其是否在障碍物中，如果在，则重复第(2)步，如果不在，则进入第(3)步；
[0139]
(3)判断采样点是否能与采样树中的节点无碰撞连接，无碰撞连接是指采样点与节点的连线没有穿过障碍物，如果不能，则将该采样点添加进采样树中，并重复第(2)步，如果能，则进入第(4)步；
[0140]
(4)对于可以与采样树中的节点无碰撞连接的采样点，需要判断其是否能够替换当前采样树中的节点，判断标准为采样点与其邻居节点构成的路径长度是否小于原节点与相同邻居节点构成的路径长度，如果小于，则用采样点替换当前采样树中的节点，并重复第(2)步。
[0141]
通过上述概率路线图的方法构造连接起点和终点的环境拓扑路径。生成复杂环境中的拓扑路径的具体步骤如下：
[0142]
1)首先对采样树进行广度优先搜索，寻找出连接搜索起点和搜索终点的所有路径，之后对这些路径进行拓扑判断，将归属于同一拓扑的路径归为一类，并保留其中长度最短的一条路径；
[0143]
2)对保留的路径进行剪枝，如图6所示，由于原始路径是在有限次采样后得到的，不能保证获得该拓扑下的最短路径，剪枝过程中，由搜索起点出发向原始路径发出射线，直到射线碰上障碍物后，将上一射线作为剪枝后路径的一部分，由此循环直到射线终点抵达搜索终点，即可完成路径剪枝；
[0144]
3)最后得到若干条剪枝后的拓扑路径，并对这些路径两两进行是否为同拓扑路径的判断，如果两条路径互为同拓扑路径，则保留其中最短路径；判断两条路径是否同拓扑的定义为：给定两条路径ξ1(r)和ξ2(r)，其中r代表路径的参数化，且r∈[0,1]，若两条路径满足ξ1(0)＝ξ2(0)，ξ1(1)＝ξ2(1)，即有相同起点和终点，且对于连线没有碰上障碍物，则两条路径为同拓扑路径，反之则为不同拓扑路径。
[0145]
4)以飞行距离为依据，从步骤3)得到的几条拓扑路径中筛选出最短路径作为参考轨迹。
[0146]
后端轨迹优化主要在前端参考轨迹的基础上，同时考虑无人机安全避障、编队队形飞行等因素，构成无约束优化问题，优化求解得到更高质量的轨迹。本实施例方法构造的后端轨迹的无约束优化问题形式如下：
[0147][0148]
其中分段多项式系数c和时间分布t为优化变量，带下标的j表示某一目标函数或者约束惩罚项，λ表示对应的权重系数，下标{e，a，o，f，b，h}分别代表能量成本项、轨迹总时间项、安全避障约束项、编队队形相似性项、集群间相互避碰项以及动力学可行性项。之后，基于梯度信息对上述优化问题进行迭代优化，最后为当前无人机生成出一条安全无碰撞的可维护队形的高质量轨迹。
[0149]
与目前其他方法相比，本实施例方法基于由粗糙到细致的轨迹规划思想，通过多层轨迹规划生成局部飞行轨迹，在保证局部轨迹高质量的同时，还保证了实时性，可以应对复杂未知环境。除此之外，多层轨迹规划的结构具有高度灵活性，在不同的层级分配了编队飞行的优先级，方便用户定义编队任务，对工程实际应用具有指意义。
[0150]
实施例2
[0151]
为了验证实施例1所述方法在应对大规模集群不规则编队飞行任务时的表现，仿真环境实验部分设计了一个由十架无人机组成的心形编队，同时在80m
×
20m的空间内放置了300个柱形障碍物和80个环形障碍物，如图7所示，无人机集群从左向右以心形进行编队飞行，期间穿越了非常稠密的障碍物区域，以20m的间距在飞行过程中进行截图，可以看到在整个飞行过程中不规则的心形始终保持得很好，从无人机集群的飞行轨迹上可以看出，每架无人机在飞行途中都遇到了许多障碍物，但在实施例1方法的控制下，无人机在躲避障碍物的同时于飞行过程中保持了心形编队，这充分展现了实施例1方法的通用性和鲁邦性，出色地完成了这样的大规模集群不规则编队飞行仿真实验。
[0152]
对于本领域的技术人员来说，可以根据以上的技术方案和构思，给出各种相应的改变和变形，而所有的这些改变和变形，都应该包括在本发明权利要求的保护范围之内。