一种基于图论的集群无人机编队飞行轨迹优化方法与流程

1.本发明涉及无人机导航技术领域，具体涉及一种基于图论的集群无人机编队飞行轨迹优化方法。


背景技术：

2.近年来，随着自主导航技术的日益成熟和嵌入式设备的高速发展，无人机在民用和军用领域都得到了大量的部署和应用。但是单架无人机在执行任务时仍然存在单机荷载小、系统容错率低等不足，而集群无人机通过增加了系统冗余度，能够更加鲁棒地完成既定任务。同时对于一些需要多个体协作的场景，如快速搜索救援、智能物流运输、协同建图等，集群无人机能够提供更系统的解决方案。作为集群无人机研究的一个重要方向，多无人机编队飞行问题是提高集群系统协同性和场景适应性的关键研究领域。
3.在无人机集群在进行复杂编队导航任务的时候，需要至少以下四点得到保证：a.复杂场景中的无人机飞行安全；b.飞行过程中对期望编队队形的保持；c.集群中各无人机之间的安全飞行间隔；d.无人机自身机体的动力学限制。对于复杂场景中的飞行安全，需要无人机对周围环境地图进行在线感知和构建以获得障碍物信息，常用的地图类型包括点云地图、欧式距离地图或概率栅格地图。对于实现集群中无人机之间的飞行避碰以及期望队形保持，需要集群系统能够通过广播网络进行无人机之间的通讯，主要通讯内容为各无人机当前位置信息和规划的轨迹。为了保证所规划轨迹能够被无人机正常执行，机体本身所具有的动力学约束也应被满足，通常包括最大电机推力、最大飞行倾角和最大滚转速率等物理限制。对于acd三点要求，目前在学界已经有了系统性的解决方案。文献
1.中，浙江大学的zhou等人提出了一个无人机集群自主导航系统，该系统采用时间－空间轨迹优化方法，能够用无人机机载电脑在毫秒级的时间内计算生成出安全无碰撞且满足动力学限制的高质量轨迹，本发明便是基于此背景技术。
4.对于上述的要求b，目前学界有很多的研究工作聚焦于解决无人机集群编队的协同控制和规划，但大多数方法只能够实现空旷场景中的集群无人机队形保持，在有障碍物的复杂环境中的编队飞行还是一个尚未有系统性解决方案的难题。因为在复杂场景下，每架无人机躲避障碍物的安全需求和维持期望队形的任务需求经常是矛盾的，因此如何系统地平衡这两个矛盾需求成为了解决无人机编队避障飞行的核心问题。目前大部分的编队飞行方案都依赖于设计基于集群一致性的局部反馈控制律。该方法对于空旷场景中的期望队形有较好的收敛性，但因为局部反馈控制律无法考虑未来一段时间窗口中的系统状态，缺乏对多障碍物场景进行提前应对处理的能力，使得该方法难以适应复杂环境中的编队飞行要求。相比而言，基于集群轨迹优化的编队飞行方案则更适合解决复杂场景中常遇到的避障问题。目前大部分的轨迹优化方案通过对各无人机施加相对位置的硬约束来实现几何队形的保持，但这些位置约束和避碰约束难以同时得到满足，从而导致优化问题不可解。一部分基于优化的工作通过被动地放弃队形的位置约束来保证无人机的飞行安全，这样会极大地破坏无人机集群的协同性。综上，目前学界亟需一种能够同时解决复杂环境下编队避障
飞行的无人机集群轨迹规划方案。
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，andf.gao，“decentralizedspatial－temporaltrajectoryplanningformulticopterswarms，”arxivpreprintarxiv：，2021
[0010]
[2]z.han，l.wang，andz.lin，“localformationcontrolstrategieswithundeterminedanddeterminedformationscalesforco－leadervehiclenetworks，”in52ndieeeconferenceondecisionandcontrol.ieee，2013，pp.7339
–
7344.
[0011]
[3]s.zhao，“affifineformationmaneuvercontrolofmultiagentsystems，”ieeetransactionsonautomaticcontrol，vol.63，no.12，pp.4140
–
4155，2018.
[0012]
[4]j.alonso－mora，e.montijano，m.schwager，andd.rus，“distributedmulti－robotformationcontrolamongobstacles：ageometricandoptimizationapproachwithconsensus，”in2016ieeeinternationalconferenceonrobotics
andautomation(icra).ieee，2016，pp.5356
–
5363.
[0013]
[5]d.zhou，z.wang，andm.schwager，“agilecoordinationandassistivecollisionavoidanceforquadrotorswarmsusingvirtualstructures，”ieeetransactionsonrobotics，vol.34，no.4，pp.916
–
923，2018.
[0014]
[6]r.vanparysandg.pipeleers，“distributedmodelpredictiveformationcontrolwithinter－vehiclecollisionavoidance，”in201711thasiancontrolconference(ascc).ieee，2017，pp.2399
–
2404.
[0015]
[7]z.wang，x.zhou，c.xu，andf.gao，“geometricallyconstrainedtrajectoryoptimizationformulticopters，”arxivpreprintarxiv：，2021.
[0016]
[8]m.turpin，n.michael，andv.kumar，“trajectorydesignandcontrolforaggressiveformationflflightwithquadrotors，”autonomousrobots，vol.33，no.1，pp.143
–
156，2012.


技术实现要素：

[0017]
针对现有技术的不足，本发明旨在提供一种基于图论的集群无人机编队飞行轨迹优化方法。
[0018]
为了实现上述目的，本发明采用如下技术方案：
[0019]
一种基于图论的集群无人机编队飞行轨迹优化方法，主要包括以下步骤：
[0020]
s1、对于由n个无人机组成的集群，对集群进行建模并计算队形相似性指标及其梯度，具体过程为：
[0021]
s1.1、对于由n个无人机组成的集群，根据其当前的编队队形，将其相互拓扑关系建模成一个全连接无向图g＝(v，e)，其中，v＝{1，2，...，n}代表顶点的集合，代表边的集合；在图g中，顶点i表示第i个无人机的位置坐标pi，当且仅当第i个无人机和第j个无人机之间能通过网络通讯获取到相互的位置信息时，连接顶点i和顶点j的边e
ij
才存在；
[0022]
s1.2、对步骤s1.1所构建的无向图中连接顶点i和顶点j的边e
ij
赋予非负权重w
ij
，得到加权无向图；
[0023]
s1.3、对步骤s1.2所得到的加权无向图进行表征矩阵的计算，包括邻接矩阵a、度矩阵d和拉普拉斯矩阵l；拉普拉斯矩阵l的计算公式为：
[0024]
l＝d-a
[0025]
通过度矩阵d对拉普拉斯矩阵l进行归一化处理得到对称规范拉普拉斯阵s，公式为
[0026]
s＝d-1/2
ld-1/2
＝i-d-1/2
ad-1/2
；
[0027]
i为单位矩阵；
[0028]
s1.4、对任务指定的期望编队队形也进行步骤s1.1－s1.3的处理，求得期望队形的邻接矩阵a
des
、拉普拉斯矩阵l
des
以及对称规范拉普拉斯矩阵s
des
；
[0029]
s1.5、计算当前编队队形与期望编队队形之间的相似性差异；编队队形的相似性指标定义如下：
[0030][0031]
对于第i个无人机，构建其邻接权重向量，将与该第i个无人机节点邻接的边权重
值写为向量，得到
[0032]
wi＝[w
i1
，...，w
ij
，...，w
in
]
t
[0033]
所述相似性指标jf的对第i个无人机位置pi的梯度的计算公式为：
[0034][0035]
其中，该相似性指标jf对图g中边的权重的梯度由下式计算得出：
[0036][0037][0038][0039]
之后再根据边权重函数计算出权重w
ij
对顶点位置pi的梯度，便可以利用链式法则得到最终队形相似性指标jf对于第i个无人机位置pi的梯度；
[0040]
s2、在计算得到了当前编队队形与期望编队队形的相似性指标及其梯度之后，代入优化框架求解得出最终的高质量无人机轨迹，具体过程为：
[0041]
s2.1、采用最小控制输入的多项式轨迹类minco来构造时间－空间参数化的无人机平坦输出轨迹并作为优化对象；此轨迹类依靠参数 c和t来表征被优化的无人机轨迹，其中向量c代表分段多项式的轨迹系数，向量t表征了分段轨迹的时间分布；在minco轨迹表示的基础上，将编队飞行的轨迹生成构造成了一个无约束优化问题：
[0042][0043]
其中带下标的j表示某一惩罚项，下标{e，t，o，f，r，d}分别代表能量成本项、轨迹总时间项、安全避障约束项、编队队形相似性项、集群间相互避碰项以及动力学可行性项，λ表示权重；
[0044]
s2.2、生成当前环境中的esdf，获取邻近的障碍物信息及对应数值梯度，构建安全避障约束项jo；jf即为步骤s1获得的相似性指标；利用无人机之间根据通信获得的相互之间位置信息构建集群间相互避碰项jr；根据飞行任务参数与无人机机体参数构建出剩下的惩罚项je，j
t
和jd；
[0045]
s2.3、使用基于梯度信息的优化算法对步骤s2.1中的优化问题进行迭代优化，最后为当前无人机生成出一条安全无碰撞的可维护队形的高质量轨迹。
[0046]
进一步地，步骤s1.2中，对于连接顶点i和顶点j的边e
ij
，其权重w
ij
为一个两无人机间欧式距离的非负函数，即：
[0047]wij
＝||p
i-pj||2[0048]
pi为第i个无人机的位置坐标，pj为第j个无人机的位置坐标。
[0049]
本发明的有益效果在于：与目前其他方法相比，本发明方法中基于图论的可微分的编队队形相似性指标，以及同时考虑编队和避障的轨迹优化框架，不仅能够凭借有效的整体几何拓扑表征来实现更稳定的队形保持效果，同时该相似性指标还具有平移旋转以及尺寸放缩的不变性，使无人机在复杂场景中编队飞行时能够通过旋转或放缩整体队形来更
灵活地应对障碍物，大幅提高了基于优化的方法在此类问题中的成功率，有重大的工程应用意义。
附图说明
[0050]
图1为本发明实施例1中无人机集群编队建模示意图；
[0051]
图2为本发明实施例1中编队队形的相似性指标及其梯度场示例图；
[0052]
图3为本发明实施例2中各方法在同一场景中编队飞行效果对比图。
具体实施方式
[0053]
以下将结合附图对本发明作进一步的描述，需要说明的是，本实施例以本技术方案为前提，给出了详细的实施方式和具体的操作过程，但本发明的保护范围并不限于本实施例。
[0054]
实施例1
[0055]
本实施例提供一种基于图论的集群无人机编队飞行轨迹优化方法，用于复杂环境下集群无人机编队飞行的轨迹生成。所述方法主要包括以下步骤：
[0056]
首先对集群无人机的起始位置以及终点位置进行初始化并设置期望的集群几何飞行队形，并用图论知识表征期望队形的拓扑关系；
[0057]
飞行过程中集群内各无人机通过广播网络确定其他无人机的当前位置与局部轨迹，进而确定当前集群所形成的队形及其拓扑图表征；通过机载传感器生成当前环境的欧氏距离场(esdf：euclideansigneddistancefield)，该距离场记录了空间中任意一点到与其最接近的障碍物的距离值，同时也可以根据被索引点邻域内障碍物距离值的变化来得出可用于数值优化的一阶导数值；
[0058]
无人机的机载设备运行优化算法，同时考虑该无人机轨迹的能量成本、轨迹时间、编队队形的相似性、轨迹的动力学可行性以及安全避障约束，将轨迹生成构建为一个优化问题，最终迭代优化出一条安全无碰撞的可维持队形的高质量轨迹供无人机执行。
[0059]
本实施例采用文献[7]中所定义的最小控制输入的多项式轨迹类(minco：minimumcontroleffortpolynomialtrajectoryclass)来构造时间－空间参数化的无人机平坦输出轨迹并作为优化对象。此轨迹类依靠参数c和t来表征被优化的无人机轨迹，其中向量c代表分段多项式的轨迹系数，向量t表征了分段轨迹的时间分布。在minco轨迹表示的基础上，本实施例方法将编队飞行的轨迹生成构造成了一个无约束优化问题
[0060][0061]
其中c，t为minco参数，即优化变量，带下标的j表示某一惩罚项及其权重，下标{e，t，o，f，r，d}分别代表能量成本项、轨迹总时间项、安全避障约束项、编队队形相似性项、集群间相互避碰项以及动力学可行性项，λ表示权重。由于本实施例方法主要提出基于图论的几何队形相似性指标以及其模型构造，以下展开介绍队形相似性指标jf的构造方式。
[0062]
对于由n个无人机组成的集群，本实施例方法对集群进行建模并计算队形相似性指标及其梯度，具体过程为：
[0063]
第1步：如图1所示，对于由n个无人机组成的集群，根据其当前的编队队形，本实施
例方法将其相互拓扑关系建模成一个全连接无向图g＝(v，e)，其中，v：＝{1，2，...，n}代表顶点的集合，代表边的集合。在g中，顶点i表示第i个无人机的位置坐标pi.当且仅当第i个无人机和第j个无人机之间能通过网络通讯获取到相互的位置信息时，连接顶点i和顶点j的边e
ij
才存在。由此将无人机编队的几何形状建模为一个无向图。
[0064]
第2步：对第1步所构建无向图的边赋予非负权重，得到加权无向图；对于连接顶点i和顶点j的边e
ij
，其权重w
ij
为一个两无人机间欧式距离的非负函数，本实施例中设为：
[0065]wij
＝||p
i-pj||2[0066]
第3步：对第2步所得到的加权无向图进行表征矩阵的计算，包括邻接矩阵a、度矩阵d和拉普拉斯矩阵l。拉普拉斯矩阵l的计算公式为
[0067]
l＝d-a. 通过图的度矩阵d对拉普拉斯矩阵l进行归一化处理得到对称规范拉普拉斯阵s，公式为
[0068]
s＝d-1/2
ld-1/2
＝i-d-1/2
ad-1/2
.
[0069]
i为单位矩阵；
[0070]
第4步：对任务所指定的期望队形也进行第1－3步的处理，求得期望队形的邻接矩阵a
des
、拉普拉斯矩阵l
des
以及对称规范拉普拉斯矩阵s
des
。
[0071]
第5步：计算当前编队队形与任务期望的编队队形之间的相似性差异；本实施例提出的相似性指标定义如下
[0072][0073]
此队形相似性指标即为优化问题中的相似性惩罚项jf。该指标定量评估了两个几何队形之间的差异程度，当两队形相同时，该项值为零，两几何队形差异越大，该项值越大。值得注意的是，本实施例所提出该相似性指标jf有三点非常好的性质。第一点是其具有平移旋转的不变性，即当集群队形在空间中进行整体的平移或旋转时，jf大小不会发生变化，这是因为第2步中的图权重赋值为欧式距离的函数，能够合理地提取出基于相对距离而非绝对位置的几何表征。第二点是该指标有尺寸放缩不变性，因为集群队形在尺寸大小上的等比例放缩并不影响编队的几何形状，因此本实施例通过利用度矩阵对拉普拉斯矩阵的归一化来达到放缩不变性的效果。第三点是该指标非负且可微分，即jf对于集群中各个无人机的位置p的梯度值是解析可求的。此性质保证了优化框架可以对该指标进行基于梯度信息的优化，从而生成出可维护队形的无人机飞行轨迹。对于第i个无人机，构建其邻接权重向量，将与该无人机节点邻接的边权重值写为向量，得到：
[0074]
wi＝[w
i1
，...，w
ij
，...，w
in
]
t
[0075]
则指标jf的对第i个无人机位置pi的梯度可由下式计算得出：
[0076][0077]
其中指标对图边权重的梯度由下式计算得出：
[0078][0079]
其中各部分梯度分别为：
[0080]
[0081][0082]
之后再根据边权重函数计算出权重w
ij
对顶点位置pi的梯度，便可以利用链式法则得到最终队形相似性指标jf对于第i个无人机位置pi的梯度。图2展示了四个无人机形成正方形编队时，其中一个无人机位置改变所带来的队形相似性指标的改变效果，以及相对应的梯度方向。
[0083]
在计算得到了当前队形与期望队形的相似性指标及其梯度之后，代入优化框架求解得出最终的高质量无人机轨迹，具体过程为：
[0084]
第1步：生成当前环境中的esdf，获取邻近的障碍物信息及对应数值梯度，构建安全避障约束项jo；无人机之间根据通信获得的相互之间位置信息构建编队队形相似性项jf和集群间相互避碰项jr；根据飞行任务参数与无人机机体参数构建出剩下的惩罚项je，j
t
和jd. 除jf外其他惩罚项的构建表达式可参考文献[1]。之后则构建出最终的优化问题
[0085][0086]
第2步：使用基于梯度信息的优化算法对第1步中的优化问题进行迭代优化，最后为当前无人机生成出一条安全无碰撞的可维护队形的高质量轨迹。
[0087]
与目前其他方法相比，本实施例方法中基于图论的可微分的编队队形相似性指标，以及同时考虑编队和避障的轨迹优化框架，不仅能够凭借有效的整体几何拓扑表征来实现更稳定的队形保持效果，同时该相似性指标还具有平移旋转以及尺寸放缩的不变性，使无人机在复杂场景中编队飞行时能够通过旋转或放缩整体队形来更灵活地应对障碍物，大幅提高了基于优化的方法在此类问题中的成功率，有重大的工程应用意义。
[0088]
实施例2
[0089]
本实施例将实施例1方法与两种目前业界最优秀的集群编队飞行方法进行对比实验，以展现实施例1方法的高效性和鲁棒性。两种对比的方法分别是文献
5.中的zhou方法和文献
[8]
中的turpin方法。本实施例在三种不同障碍物密度的场景中对各方法进行了编队飞行实验，并将各方法的飞行实验结果在队形绝对位置误差e
dist
、队形相似性指标e
sim
以及飞行任务的成功率三个维度上进行了对比，实验数据如表1所示。
[0090]
表1
[0091]
[0092][0093]
从表1可以看出，实施例1所提出的方法在各个方面都是优于其他方法的。成功率方面，即使在有密集障碍物的场景中，实施例1的方法依然能够达到95％的高成功率。在队形保持的效果方面，在三种密度场景下，实施例1的方法无论是队形绝对位置误差e
dist
还是队形相似性指标e
sim
都要小于另外两种方法，说明实施例1的方法可以使集群队形保持的更加稳定。
[0094]
图3显示了三种方法在集群飞行中同一场景下队形保持效果的对比，(a)为zhou方法，(b)为turpin方法，(c)为实施例1方法。可以直观地看出由实施例1方法所生成的飞行轨迹有着最好的队形保持效果，也取得了最小的队形绝对位置误差e
dist
。
[0095]
对于本领域的技术人员来说，可以根据以上的技术方案和构思，给出各种相应的改变和变形，而所有的这些改变和变形，都应该包括在本发明权利要求的保护范围之内。