CN113900449A - 多无人机轨迹规划方法、装置、无人机及存储介质 - Google Patents
多无人机轨迹规划方法、装置、无人机及存储介质 Download PDFInfo
- Publication number
- CN113900449A CN113900449A CN202111248705.1A CN202111248705A CN113900449A CN 113900449 A CN113900449 A CN 113900449A CN 202111248705 A CN202111248705 A CN 202111248705A CN 113900449 A CN113900449 A CN 113900449A
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- aerial vehicle
- target
- state
- constraint
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 74
- 238000003860 storage Methods 0.000 title claims abstract description 13
- 238000005070 sampling Methods 0.000 claims abstract description 76
- 238000005096 rolling process Methods 0.000 claims abstract description 69
- 238000005457 optimization Methods 0.000 claims abstract description 35
- 230000007613 environmental effect Effects 0.000 claims abstract description 31
- 230000036461 convulsion Effects 0.000 claims description 31
- 206010048669 Terminal state Diseases 0.000 claims description 28
- 239000002245 particle Substances 0.000 claims description 26
- 230000001133 acceleration Effects 0.000 claims description 23
- 238000004422 calculation algorithm Methods 0.000 claims description 21
- 238000005265 energy consumption Methods 0.000 claims description 18
- 230000008569 process Effects 0.000 claims description 13
- 238000012549 training Methods 0.000 claims description 12
- 230000004888 barrier function Effects 0.000 abstract description 23
- 230000009286 beneficial effect Effects 0.000 abstract 1
- 230000006870 function Effects 0.000 description 80
- 230000003068 static effect Effects 0.000 description 15
- 238000004364 calculation method Methods 0.000 description 12
- 230000002349 favourable effect Effects 0.000 description 12
- 238000004590 computer program Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 6
- 239000011159 matrix material Substances 0.000 description 6
- 230000005540 biological transmission Effects 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 230000004044 response Effects 0.000 description 4
- 238000011161 development Methods 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 238000010276 construction Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000007689 inspection Methods 0.000 description 2
- 238000012804 iterative process Methods 0.000 description 2
- 238000012544 monitoring process Methods 0.000 description 2
- 230000001737 promoting effect Effects 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000003062 neural network model Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
- G05D1/104—Simultaneous control of position or course in three dimensions specially adapted for aircraft involving a plurality of aircrafts, e.g. formation flying
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种多无人机轨迹规划方法、装置、无人机及存储介质，其中，上述多无人机轨迹规划方法包括：建立无人机运动模型；获取目标状态约束运动基元采样模型；对于多无人机集合中任一目标无人机，构建包括飞行安全约束和目标点距离优化约束的目标函数，飞行安全约束限定目标无人机与障碍物之间的距离，障碍物包括环境障碍物和除目标无人机以外的其它无人机；求解目标函数，获取目标无人机在当前滚动预测时域内的最优末端状态，并基于上述目标状态约束运动基元采样模型和上述无人机运动模型获取上述目标无人机在当前滚动预测时域内对应的目标飞行轨迹，目标飞行轨迹包括一组状态量。与现有技术相比，本发明方案有利于提升无人机的安全性。
Description
技术领域
本发明涉及飞行控制技术领域，尤其涉及的是一种多无人机轨迹规划方法、装置、无人机及存储介质。
背景技术
随着科学技术的发展，无人机技术迅速发展，无人机的应用也越来越广泛。例如，无人机可以用于城市或野外环境中的海关缉私、搜索营救、电力巡检、目标追捕等。单无人机作业时存在视野和监测范围有限、容错性不足、鲁棒性差等缺陷，导致无人机在执行特定任务时任务效能和成功率受到一定程度的影响，因此多无人机协同作业应运而生。多无人机协同执行任务可有效地弥补单无人机的局限性，提高任务执行的可靠性和效率。
现有技术中，对于无人机的轨迹规划和控制集中在单无人机控制领域，通常仅考虑环境中静止的障碍物对无人机的影响，为无人机规划路径以规避环境中静止的障碍物。现有技术的问题在于，仅考虑环境中静止障碍物的影响，在多无人机协同执行任务时容易导致无人机之间发生碰撞等事故，影响无人机的安全性。
因此，现有技术还有待改进和发展。
发明内容
本发明的主要目的在于提供一种多无人机轨迹规划方法、装置、无人机及存储介质，旨在解决现有技术中仅考虑环境中静止的障碍物对无人机的影响，为无人机规划路径以规避环境中静止的障碍物的方案在多无人机协同执行任务时容易导致无人机之间发生碰撞等事故，影响无人机的安全性的问题。
为了实现上述目的，本发明第一方面提供一种多无人机轨迹规划方法，上述方法用于对多无人机集合中的每一架无人机分别进行轨迹规划，其中，上述方法包括：
建立无人机运动模型，其中，上述无人机运动模型用于基于输入量获取无人机的状态量；
获取目标状态约束运动基元采样模型，其中，上述状态约束运动基元采样模型用于基于给定的初始状态和末端状态获取无人机的状态约束运动基元；
对于上述多无人机集合中的任一目标无人机，构建上述目标无人机的轨迹对应的目标函数，其中，上述目标函数中包括飞行安全约束和目标点距离优化约束，上述飞行安全约束用于限定上述目标无人机与障碍物之间的距离，上述障碍物包括环境障碍物和上述多无人机集合中除上述目标无人机以外的其它无人机；
求解上述目标无人机的目标函数，获取上述目标无人机在当前滚动预测时域内的最优末端状态，并基于上述目标状态约束运动基元采样模型和上述无人机运动模型获取上述目标无人机在当前滚动预测时域内对应的目标飞行轨迹，其中，上述目标飞行轨迹包括一组状态量。
可选的，上述无人机运动模型为模型预测控制运动模型。
可选的，上述无人机运动模型的输入量为无人机的加加速度，上述状态量包括无人机的位置、速度和加速度，上述状态约束运动基元为无人机的加加速度。
可选的，上述获取目标状态约束运动基元采样模型，包括：
获取待训练的状态约束运动基元采样模型；
获取训练数据，基于上述训练数据对上述待训练的状态约束运动基元采样模型进行离线动态规划迭代训练，获取训练后的状态约束运动基元采样模型并作为目标状态约束运动基元采样模型。
可选的，上述目标函数中还包括能耗优化约束，上述能耗优化约束用于限定上述目标无人机的轨迹对应的能耗。
可选的，在上述构建上述目标无人机的轨迹对应的目标函数之前，上述方法还包括：
基于双目相机获取环境点云信息；
基于上述环境点云信息获取欧式距离场地图，其中，上述欧式距离场地图中任意栅格中存储了该栅格到最近环境障碍物的欧式距离信息。
可选的，上述飞行安全约束包括环境障碍物距离约束和其它无人机距离约束，上述环境障碍物距离约束基于上述欧式距离场地图建立，上述其它无人机距离约束基于上述其它无人机的状态信息建立，其中，上述状态信息包括各上述其它无人机的当前位置状态和预测轨迹。
可选的，上述求解上述目标无人机的目标函数，获取上述目标无人机的最优末端状态，并基于上述目标状态约束运动基元采样模型和上述无人机运动模型获取上述目标无人机对应的目标飞行轨迹，包括：
基于并行粒子群算法对上述目标函数进行迭代求解，获取上述目标无人机在当前滚动预测时域内对应的最优末端状态，其中，上述并行粒子群算法中每个粒子为上述目标无人机的一个候选末端状态；
基于上述最优末端状态、上述目标状态约束运动基元采样模型和上述无人机运动模型获取上述目标无人机在当前滚动预测时域内对应的目标飞行轨迹。
可选的，上述基于并行粒子群算法对上述目标函数进行迭代求解，获取上述目标无人机在当前滚动预测时域内对应的最优末端状态，包括：
基于并行粒子群算法对上述目标函数进行g次迭代求解，获得上述目标无人机在当前滚动预测时域内对应的最优末端状态，在迭代求解的过程中，第1次迭代时使用的各上述其它无人机的预测轨迹是基于预设的初始加加速度值获得的，第h次迭代时使用的各上述其它无人机的预测轨迹是根据各上述其它无人机第h-1次迭代后的末端状态获得的，g为预设的迭代次数，h大于1且不大于g。
可选的，上述方法还包括：
基于获取的上述目标无人机在当前滚动预测时域内对应的目标飞行轨迹，在当前滚动控制时域内对上述目标无人机进行控制，上述当前滚动控制时域小于上述当前滚动预测时域。
本发明第二方面提供一种多无人机轨迹规划装置，上述装置用于对多无人机集合中的每一架无人机分别进行轨迹规划，其中，上述装置包括：
无人机运动模型建立模块，用于建立无人机运动模型，其中，上述无人机运动模型用于基于输入量获取无人机的状态量；
目标状态约束运动基元采样模型获取模块，用于获取目标状态约束运动基元采样模型，其中，上述状态约束运动基元采样模型用于基于给定的初始状态和末端状态获取无人机的状态约束运动基元；
目标函数构建模块，用于对于上述多无人机集合中的任一目标无人机，构建上述目标无人机的轨迹对应的目标函数，其中，上述目标函数中包括飞行安全约束和目标点距离优化约束，上述飞行安全约束用于限定上述目标无人机与障碍物之间的距离，上述障碍物包括环境障碍物和上述多无人机集合中除上述目标无人机以外的其它无人机；
目标函数求解模块，用于求解上述目标无人机的目标函数，获取上述目标无人机在当前滚动预测时域内的最优末端状态，并基于上述目标状态约束运动基元采样模型和上述无人机运动模型获取上述目标无人机在当前滚动预测时域内对应的目标飞行轨迹，其中，上述目标飞行轨迹包括一组状态量。
本发明第三方面提供一种无人机，其中，多个上述无人机构成一个多无人机集合，上述多无人机集合中的各上述无人机分别基于上述任意一种多无人机轨迹规划方法进行轨迹规划。
本发明第四方面提供一种计算机可读存储介质，上述计算机可读存储介质上存储有多无人机轨迹规划程序，上述多无人机轨迹规划程序被处理器执行时实现任意一项上述多无人机轨迹规划方法的步骤。
由上可见，本发明方案中的多无人机轨迹规划用于对多无人机集合中的每一架无人机分别进行轨迹规划，方法包括：建立无人机运动模型，其中，上述无人机运动模型用于基于输入量获取无人机的状态量；获取目标状态约束运动基元采样模型，其中，上述状态约束运动基元采样模型用于基于给定的初始状态和末端状态获取无人机的状态约束运动基元；对于上述多无人机集合中的任一目标无人机，构建上述目标无人机的轨迹对应的目标函数，其中，上述目标函数中包括飞行安全约束和目标点距离优化约束，上述飞行安全约束用于限定上述目标无人机与障碍物之间的距离，上述障碍物包括环境障碍物和上述多无人机集合中除上述目标无人机以外的其它无人机；求解上述目标无人机的目标函数，获取上述目标无人机在当前滚动预测时域内的最优末端状态，并基于上述目标状态约束运动基元采样模型和上述无人机运动模型获取上述目标无人机在当前滚动预测时域内对应的目标飞行轨迹，其中，上述目标飞行轨迹包括一组状态量。本发明方案中考虑其它无人机的影响，结合环境障碍物以及多无人机集合中其它无人机为目标无人机构建飞行安全约束，并基于飞行安全约束和目标点距离优化约束为当前需要进行轨迹规划的目标无人机构建目标函数，通过求解目标函数来实现对目标无人机的轨迹规划。与现有技术相比，本发明方案结合考虑了环境中静止障碍物和其它移动的无人机的影响，有利于实现多无人机协同轨迹规划，避免无人机之间发生碰撞事故，且有利于提升无人机的安全性。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。
图1是本发明实施例提供的一种多无人机轨迹规划方法的流程示意图；
图2是本发明实施例图1中步骤S200的具体流程示意图；
图3是本发明实施例提供的一种无人机飞行安全示意图；
图4是本发明实施例提供的一种无人机到目标点的距离示意图；
图5是本发明实施例图1中步骤S400的具体流程示意图；
图6是本发明实施例提供的一种多无人机轨迹规划装置的结构示意图；
图7是本发明实施例提供的一种无人机轨迹规划系统的结构示意图；
图8是本发明实施例提供的一种图7中系统的工作原理示意图。
具体实施方式
以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本发明实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本发明。在其它情况下，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本发明的描述。
应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。
还应当理解，在本发明说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本发明。如在本发明说明书和所附权利要求书中所使用的那样，除非上下文清楚地指明其它情况，否则单数形式的“一”、“一个”及“该”意在包括复数形式。
还应当进一步理解，在本发明说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。
如在本说明书和所附权利要求书中所使用的那样，术语“如果”可以依据上下文被解释为“当…时”或“一旦”或“响应于确定”或“响应于检测到”。类似的，短语“如果确定”或“如果检测到[所描述条件或事件]”可以依据上下文被解释为意指“一旦确定”或“响应于确定”或“一旦检测到[所描述的条件或事件]”或“响应于检测到[所描述条件或事件]”。
下面结合本发明实施例的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是本发明还可以采用其它不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本发明内涵的情况下做类似推广，因此本发明不受下面公开的具体实施例的限制。
随着科学技术的发展，无人机技术迅速发展，无人机的应用也越来越广泛。例如，无人机可以用于城市或野外环境中的海关缉私、搜索营救、电力巡检、目标追捕等。单无人机作业时存在视野和监测范围有限、容错性不足、鲁棒性差等缺陷，导致无人机在执行特定任务时任务效能和成功率受到一定程度的影响，因此多无人机协同作业应运而生。多无人机协同执行任务可有效地弥补单无人机的局限性，提高任务执行的可靠性和效率。但多无人机协同执行任务时，多个决策智能体(无人机)共享同一个任务空间，因此多无人机规划任务相比于单智能体具有更高的挑战性。在多无人机情况下，轨迹规划算法必须考虑相邻的智能体和环境影响，计算无碰撞和面向各自飞行目标的飞行轨迹。此外，由于旋翼无人机运动规划需要充分考虑飞行灵活性、飞行效率和动力学约束等诸多因素，因此对规划方法具有很高的要求。同时，在实际应用中，高效的计算效率也至关重要。
现有技术中，对于无人机的轨迹规划和控制集中在单无人机控制领域，通常仅考虑环境中静止的障碍物对无人机的影响，为无人机规划路径以规避环境中静止的障碍物。现有技术的问题在于，仅考虑环境中静止障碍物的影响，在多无人机协同执行任务时容易导致无人机之间发生碰撞等事故，影响无人机的安全性。在典型的复杂动态环境中，在进行多无人机协同轨迹规划任务中，需要对不断变化的周边无人机运动状态和实时在线检测到的环境障碍物等信息做出积极的反应，在满足任务性能需求的同时保证无人机自身的飞行安全、动态约束以及能耗优化等。因此目前亟需一种能够应用于复杂动态环境实时多无人机协同高效飞行轨迹规划的方法。
为了解决现有技术的问题，本发明提供一种多无人机轨迹规划方法，用于对多无人机集合中的每一架无人机分别进行轨迹规划，在本发明实施例中，建立无人机运动模型，其中，上述无人机运动模型用于基于输入量获取无人机的状态量；获取目标状态约束运动基元采样模型，其中，上述状态约束运动基元采样模型用于基于给定的初始状态和末端状态获取无人机的状态约束运动基元；对于上述多无人机集合中的任一目标无人机，构建上述目标无人机的轨迹对应的目标函数，其中，上述目标函数中包括飞行安全约束和目标点距离优化约束，上述飞行安全约束用于限定上述目标无人机与障碍物之间的距离，上述障碍物包括环境障碍物和上述多无人机集合中除上述目标无人机以外的其它无人机；求解上述目标无人机的目标函数，获取上述目标无人机在当前滚动预测时域内的最优末端状态，并基于上述目标状态约束运动基元采样模型和上述无人机运动模型获取上述目标无人机在当前滚动预测时域内对应的目标飞行轨迹，其中，上述目标飞行轨迹包括一组状态量。本发明方案中考虑其它无人机的影响，结合环境障碍物以及多无人机集合中其它无人机为目标无人机构建飞行安全约束，并基于飞行安全约束和目标点距离优化约束为当前需要进行轨迹规划的目标无人机构建目标函数，通过求解目标函数来实现对目标无人机的轨迹规划。与现有技术相比，本发明方案结合考虑了环境中静止障碍物和其它移动的无人机的影响，有利于实现多无人机协同轨迹规划，避免无人机之间发生碰撞事故，且有利于提升无人机的安全性。
示例性方法
如图1所示，本发明实施例提供一种多无人机轨迹规划方法，上述方法用于对多无人机集合中的每一架无人机分别进行轨迹规划，具体的，上述方法包括如下步骤：
步骤S100，建立无人机运动模型，其中，上述无人机运动模型用于基于输入量获取无人机的状态量。
其中，上述无人机运动模型是用于描述无人机的运动状态的模型。具体的，无人机运行模型可以基于输入量获取无人机(与输入量对应的无人机)的状态量，状态量包括一个或一组(本实施例中为一组)用于描述无人机的飞行状态的参数。本实施例中建立的无人机运动模型的输入量选为无人机的加加速度，在其它应用场景中，输入量还可以根据实际需求选为其它参数，在此不作具体限定。
步骤S200，获取目标状态约束运动基元采样模型，其中，上述状态约束运动基元采样模型用于基于给定的初始状态和末端状态获取无人机(与初始状态和末端状态对应的无人机)的状态约束运动基元。
其中，上述初始状态是无人机当前的运动状态，末端状态是无人机在预设的时间后(例如对应的滚动预测时域之后)需要达到的状态。本实施例中，无人机的初始状态包括无人机当前的位置、速度和加速度；末端状态包括末端(预设时间之后)对应的位置、速度和加速度；例如，如果预设的滚动预测时域为20秒，则初始状态是当前时刻的状态，末端状态是20秒之后的状态。
本实施例中，通过目标状态约束运动基元采样模型对目标无人机的状态约束运动基元进行采样(即求解)时，输入目标状态约束运动基元采样模型的初始状态是实时获取的目标无人机的当前状态，末端状态是通过求解目标无人机的目标函数获得的最优末端状态。具体的，求解获得的目标无人机的状态约束运动基元与上述无人机运动模型中的输入量对应，即可以作为上述无人机运动模型的输入量从而获得目标无人机的状态量，方便进行计算和规划。同时，也可以将求无人机轨迹的问题转化为一种求末端状态的问题，方便计算，有利于提升计算和处理的实时性。
步骤S300，对于上述多无人机集合中的任一目标无人机，构建上述目标无人机的轨迹对应的目标函数，其中，上述目标函数中包括飞行安全约束和目标点距离优化约束，上述飞行安全约束用于限定上述目标无人机与障碍物之间的距离，上述障碍物包括环境障碍物和上述多无人机集合中除上述目标无人机以外的其它无人机。
其中，上述目标无人机是多无人机集合中需要进行轨迹规划的无人机。需要说明的是，本实施例中，基于上述多无人机轨迹规划方法对多无人机集合中的每一架无人机分别进行轨迹规划，具体的，对于多无人机组成的集合中的每一架无人机都使用相同的方法进行轨迹规划，因此上述目标无人机可以是多无人机集合中的任意一架无人机，本实施例中以对一架目标无人机进行的操作为例进行说明，但不作为具体限定。实际使用过程中，多无人机集合中的每一架无人机都同时且实时地根据上述多无人机轨迹规划方法进行分布式的轨迹规划(即多架无人机各自计算各自的轨迹，而不是集中式地计算)，即每一架无人机都可以作为目标无人机，有利于进一步提升计算速度，满足实时性的要求。
其中，上述目标函数是用于描述和限定目标无人机的飞行轨迹的函数。本实施例中，通过为目标函数设置约束条件，使得规划出的飞行轨迹能够满足约束。具体的，目标函数中包括飞行安全约束和目标点距离约束。飞行安全约束用于限定目标无人机与障碍物之间的距离，例如，限制距离始终大于0(或其它预设的大于0安全距离值)，以保证目标无人机不会与障碍物碰撞，且障碍物是考虑环境障碍物和其它无人机的，可以避免多个无人机之间互相碰撞。目标点距离约束用于在考虑障碍物的基础上，最小化无人机到目标点的距离值，有利于提升无人机轨迹规划的合理性和安全性。
步骤S400，求解上述目标无人机的目标函数，获取上述目标无人机在当前滚动预测时域内的最优末端状态，并基于上述目标状态约束运动基元采样模型和上述无人机运动模型获取上述目标无人机在当前滚动预测时域内对应的目标飞行轨迹，其中，上述目标飞行轨迹包括一组状态量。
具体的，本实施例中，通过求解目标函数可以获得目标无人机在当前滚动预测时域内的最优末端状态，如此，可以在根据目标函数获得当前滚动预测时域内的一组最优末端状态的情况下，结合目标状态约束运动基元采样模型和无人机运动模型，获取在当前滚动预测时域内的一组无人机的状态量，从而获得对应的目标飞行轨迹，有利于实现对无人机的控制。
由上可见，本发明实施例提供的多无人机轨迹规划方法中，建立无人机运动模型，其中，上述无人机运动模型用于基于输入量获取无人机的状态量；获取目标状态约束运动基元采样模型，其中，上述状态约束运动基元采样模型用于基于给定的初始状态和末端状态获取无人机的状态约束运动基元；对于上述多无人机集合中的任一目标无人机，构建上述目标无人机的轨迹对应的目标函数，其中，上述目标函数中包括飞行安全约束和目标点距离优化约束，上述飞行安全约束用于限定上述目标无人机与障碍物之间的距离，上述障碍物包括环境障碍物和上述多无人机集合中除上述目标无人机以外的其它无人机；求解上述目标无人机的目标函数，获取上述目标无人机在当前滚动预测时域内的最优末端状态，并基于上述目标状态约束运动基元采样模型和上述无人机运动模型获取上述目标无人机在当前滚动预测时域内对应的目标飞行轨迹，其中，上述目标飞行轨迹包括一组状态量。本发明方案中考虑其它无人机的影响，结合环境障碍物以及多无人机集合中其它无人机为目标无人机构建飞行安全约束，并基于飞行安全约束和目标点距离优化约束为当前需要进行轨迹规划的目标无人机构建目标函数，通过求解目标函数来实现对目标无人机的轨迹规划。与现有技术相比，本发明方案结合考虑了环境中静止障碍物和其它移动的无人机的影响，有利于实现多无人机协同轨迹规划，避免无人机之间发生碰撞事故，且有利于提升无人机的安全性。具体的，本实施例中，上述无人机运动模型为模型预测控制(MPC，ModelPredictive Control)运动模型。
进一步的，上述无人机运动模型的输入量为无人机的加加速度，上述状态量包括无人机(与状态量对应的无人机)的位置、速度和加速度，上述状态约束运动基元为无人机的加加速度。
具体的，本实施例中，选取无人机的加加速度作为模型预测控制(MPC)的控制输入量，在三维空间中，在每一个维度q∈x,y,z中建立如下的无人机三阶运动模型(即MPC运动模型)：
sq[n+1]＝Asq[n]+buq[n] (1)
其中，sq＝[p,v,a]，sq代表一种包括位置p、速度v和加速度a的状态量(可以以状态矩阵的形式体现位置、速度和加速度)。需要说明的是，n是一种离散变量，即n＝1，2，3，……，n*Δt代表具体的时刻，sq[n+1]代表第(n+1)*Δt时刻对应的状态量sq，其它参数同理，在此不再赘述。uq为MPC运动模型的输入，本实施例中，uq为无人机的加加速度。矩阵A和矩阵b分别如下所示：
其中，Δt为系统时间步长常数，可以根据实际需求进行设置和调整。针对上述公式(1)所示的无人机运动模型，当确定系统输入uq后，即可代入模型中得到无人机的状态量sq，如位置、速度、加速度等，还可以包括其它用于描述无人机的位置和状态量，在此不作具体限定。
具体的，本实施例中，如图2所示，上述步骤S200具体包括如下步骤：
步骤S201，获取待训练的状态约束运动基元采样模型。
步骤S202，获取训练数据，基于上述训练数据对上述待训练的状态约束运动基元采样模型进行离线动态规划迭代训练，获取训练后的状态约束运动基元采样模型并作为目标状态约束运动基元采样模型。
其中，上述待训练的状态约束运动基元采样模型是需要进行训练的状态约束运动基元采样模型，可以是一种神经网络模型，可以预先构建也可以在需要时进行构建，基于实际需求和与当前的应用场景相对应的数据对其进行适当的训练可以实现对状态约束运动基元的求解。状态约束是指速度、加速度、加加速度等状态量是有约束的(例如不能大于对应的预设值)，从而保证安全性和飞行的平稳性。本实施例中，目标状态约束运动基元采样模型采样获得的状态约束运动基元实际是指加加速度。
具体的，采用离线数据(即预先存储或获取的训练数据)，通过离线动态规划迭代训练的方法对待训练的状态约束运动基元采样模型进行训练。实现在给定初始状态和末端状态时可以快速得到状态约束运动基元，从而将对状态约束运动基元的采样转换为对末端状态的搜索。
具体通过如下方式进行动态规划：在给定初始状态和末端状态前提下，无人机的轨迹规划可通过求解标准的两点边界值问题得到。因此，当无人机在初始时刻t0的状态xq＝[pq,vq,aq]和末端时刻t0+T的期望状态xd＝[θ,0,0]时，无人机的局部运动规划问题可转化为一种优化问题：使得无人机在t0到t0+T时刻之间的运行能耗的积分与终端能耗之和最小。其中，T是预设的时间长度，本实施例中，T为一个滚动预测时域的时间长度，即进行一次预测时，会预测当前时刻(t0)到末端时刻(t0+T)的无人机的目标飞行轨迹。需要说明的是，末端时刻t0+T的期望状态xd＝[θ,0,0]是本申请中根据实际需求指定的一个具体的状态，θ是t0+T时刻无人机对应的具体末端位置，xd＝[θ,0,0]代表t0+T时刻无人机处于位置θ，且速度和加速度都为0。在其它应用场景中，速度和加速度的值还可以不为0，在此不作具体限定。
进一步的，定义相对状态量sr＝xq-xd＝[pq-θ,vq,aq]，为了使无人机达到状态xd，应该调整sr，使其变为零。动态规划的迭代过程中的值函数如下公式(4)所示：
Q(sr[n])＝r(sr[n],u[n])+Q(sr[n+1]) (4)
其中，Q(sr[n])和Q(sr[n+1])分别代表sr[n]和sr[n+1]状态时对应的值函数，r(sr[n],u[n])为瞬时能耗，可表示为如下公式(5)所示：
其中，Qw是预设的权值矩阵，λ是预设的权重值，可以根据实际需求进行设置和调整。u是控制输入量(即与无人机运动模型对应的输入量)，本实施例中具体为无人机的加加速度，在本实施例的其它内容部分中u有相同的意义，不再赘述，Qc为代表状态空间约束的惩罚函数，其作用为限制速度、加速度、加加速度等满足约束条件(例如不超过预设的阈值范围)，具体的：
Qc(sr,u)＝wvμ2(v,vmin,vmax)+waμ2(a,amin,amax)+wjμ2(j,jmin,jmax) (6)
其中，μ(k1,k2,k3)＝max(k2-k1,0)+max(k1-k3,0)，k1,k2,k3属于实数集。wv,wa,wj分别为速度、加速度以及加加速度对应的软约束权重值(可以根据实际需求进行设置)，v,a,j分别代表速度、加速度以及加加速度，vmin,vmax是速度最大值和最小值，amin,amax是加速度最大值和最小值，jmin,jmax是加加速度最大值和最小值，预先设置对应的最大值和最小值，可以实现对无人机运动状态的约束。需要说明的是，本实施例中u和j都代表加加速度，其中，u实际为输入量，j仅代表加加速度这个物理量，公式(6)中的v和a都可以由输入量u获得。通过上述操作，所生成的状态约束运动基元将直接满足设定的动力学约束条件，大大降低在线求解的问题复杂度。对于上述的值函数，值迭代操作将贝尔曼最优性转化为迭代赋值。在值迭代过程中，当对于所有可能的状态如果均满足如下公式(7)时，值迭代结束：
其中，Q*(sr[n])和Q*(sr[n+1])分别代表状态sr[n]和sr[n+1]的最优值函数，γ为预设的权重值。u[n]代表加加速度(输入量)的序列，上述公式(7)是一个迭代的过程，即有若干个u，所以使用u[n]对一系列的u进行表示。同时，u是有约束限制范围的，上述E代表所有可选的u的集合，即大于或等于其最小值，小于或等于其最大值。在一种应用场景中，上述值迭代的结果为一个关于最优策略π(sr)的查找表，其中对应的输出值为该状态下最优的动作，即加加速度值，具体的，输出的是一组加加速度值，从而由所选的一组加加速度值作为运动模型的输入量，生成一条飞行轨迹，以调整无人机从相对状态sr到零状态，达到由初始状态到末端状态的目的。在另一种应用场景中，对待训练状态约束运动基元采样模型进行训练之后，无需查表，可以直接根据给定的初始状态和末端状态获取对应无人机的加加速度值。由上可知，在已知每一架无人机的当前状态的情况下，可以将状态约束运动基元的采样转化为对末端状态的求解。
具体的，本实施例中，上述目标函数中还包括能耗优化约束，上述能耗优化约束用于限定上述目标无人机的轨迹对应的能耗，可降低能耗损失且维持无人机运动的平稳性。
本实施例中，在上述构建上述目标无人机的轨迹对应的目标函数之前，上述方法还包括：基于双目相机获取环境点云信息；基于上述环境点云信息获取欧式距离场地图，其中，上述欧式距离场地图中任意栅格中存储了该栅格到最近环境障碍物的欧式距离信息。具体的，通过记载计算机和双目相机进行环境感知，实时采集环境点云信息并将点云信息转换为欧式距离场地图，使得无人机可以快速获取到最近环境障碍物的欧式距离值。
需要说明的是，上述飞行安全约束包括环境障碍物距离约束和其它无人机距离约束，上述环境障碍物距离约束基于上述欧式距离场地图建立，上述其它无人机距离约束基于上述其它无人机的状态信息建立，其中，上述状态信息包括各上述其它无人机的当前位置状态和预测轨迹。
具体的，建立每一架无人机协同飞行轨迹生成的目标函数，本实施例中，以对一架目标无人机进行的操作为例进行说明。建立目标无人机轨迹对应的目标函数，目标函数中包括能耗优化约束、飞行安全约束、目标点距离优化约束。能耗优化为无人机运动轨迹的能耗最小化。飞行安全约束保证无人机对环境中静态障碍物的避碰以及与其它无人机之间的避碰，如图3所示，保证无人机i与静态障碍物之间的距离ci以及无人机i与无人机j与之间的距离di不小于预设的安全距离值，图3中画斜线的物体是环境障碍物。在一种应用场景中，还可以设置动态可行性约束，例如约束无人机的最大速度、加速度和加加速度。目标点距离优化为最小化无人机到目标点的考虑障碍物信息的距离值，如图4所示，在考虑障碍物的基础上，使得无人机到目标点的距离值最小化。
本实施例中，假设当前的目标无人机是多无人机集合中的第k架无人机，则在规划域(即当前滚动预测时域)内第k架无人机(即目标无人机)的目标函数J(k)如下公式(8)所示：
J(k)＝w1J1+w2J2+w3J3+w4J4 (8)
其中，w1,w2,w3,w4分别代表对应子目标函数的权值，可以根据实际需求进行预先设置和调整，在此不作具体限定。
J1代表无人机能耗优化约束，其中，u k(t)代表第k架无人机在t时刻的加加速度(jerk)，
代表加加速度的平方。t0表示当前时刻，T为模型预测控制(MPC)的规划域，也即一个滚动预测时域，其它公式中同理，不再赘述。
J2代表在规划域内无人机与其它无人机之间的欧氏距离大于0，避免碰撞。dk(t)表示第k架无人机在t时刻与其它无人机之间的最近欧式距离值，可以根据当前时刻其它无人机的当前位置状态和预测轨迹计算获得，本实施例中，各无人机之间通过数传电台传输当前位置状态和预测轨迹，因此目标无人机基于数传电台获得其它无人机的位置信息和预测轨迹，从而计算获得dk(t)。
J3代表规划域内无人机到静态障碍物的距离大于0，避免碰撞。ck(t)表示第k架无人机飞行轨迹距离最近障碍物的欧式距离，可以通过欧氏距离场地图直接获得。
J4代表第k架无人机与其目标位置之间的距离优化项，M(pk(t))表示考虑环境障碍物信息情况下第k架无人机在t时刻的位置pk(t)到其目标位置之间的距离值，可以采用Dijkstra等算法快速得到。
如此，通过J2保证无人机之间不发生碰撞，通过J3保证无人机与环境中的障碍物不发生碰撞，通过J4保证无人机朝着自己的目标位置运动。通过J1保证无人机有更好的飞行效果，维持轨迹的平滑性。
具体的，本实施例中，如图5所示，上述步骤S400包括：
步骤S401，基于并行粒子群算法对上述目标函数进行迭代求解，获取上述目标无人机在当前滚动预测时域内对应的最优末端状态，其中，上述并行粒子群算法中每个粒子为上述目标无人机的一个候选末端状态。
步骤S402，基于上述最优末端状态、上述目标状态约束运动基元采样模型和上述无人机运动模型获取上述目标无人机在当前滚动预测时域内对应的目标飞行轨迹。
具体的，采用GPU加速的并行粒子群算法对目标函数进行求解，其中算法中的每个粒子选为各架无人机的候选的末端状态，即粒子群算法的中各个粒子代表的是一个无人机状态(包括位置、速度、加速度)，采用粒子群优化方法找到最优的粒子，即最优的末端状态(位置、速度、加速度)。通过GPU加速对粒子群算法进行并行求解，得到最优的末端状态，进而通过目标状态约束运动基元采样模型中的动态规划算法得到规划域的加加速度矩阵(包括一组连续的各个时刻对应的加加速度)，并代入无人机运动模型中快速地得到满足飞行安全约束、动力学约束和平滑约束的目标飞行轨迹(包括一组连续的各个时刻对应的状态量)，并发送至无人机的飞行控制器，完成飞行轨迹跟踪。通过借助GPU并行计算能力，保证系统工作的实时性。
本实施例中，上述基于并行粒子群算法对上述目标函数进行迭代求解，获取上述目标无人机在当前滚动预测时域内对应的最优末端状态，包括：基于并行粒子群算法对上述目标函数进行g次迭代求解，获得上述目标无人机在当前滚动预测时域内对应的最优末端状态，在迭代求解的过程中，第1次迭代时使用的各上述其它无人机的预测轨迹是基于预设的初始加加速度值获得的，第h次迭代时使用的各上述其它无人机的预测轨迹是根据各上述其它无人机第h-1次迭代后的末端状态获得的，g为预设的迭代次数，h大于1且不大于g。
本实施例中，每一架目标无人机在进行迭代计算的同时，将迭代获得的预测轨迹(和/或其对应的加加速度)通过数传电台发送给其它所有无人机，使得其它无人机可以根据目标无人机的预测轨迹进行规避，同理，其它所有无人机也会将迭代获得的预测轨迹通过数传电台发送给目标无人机。进一步的，目标无人机在进行第h次迭代时，使用的是其它无人机第h-1次的迭代结果，其它无人机在进行第h次迭代时，使用的是目标无人机第h-1次的迭代结果。
具体的，并行粒子群算法对上述目标函数进行迭代求解，迭代次数g可以根据实际需求预先设置，本实施例中设g为20，通过迭代和计算获得加加速度，进而获得对应的目标飞行轨迹。在每一个滚动预测时域内，进行迭代时，第1次迭代时使用其它无人机对应的初始值，该初始值可以根据实际需求进行设置和调整，在此不作具体限定。第2次迭代时，目标无人机获取到其它无人机第1次迭代后对应的预测轨迹，集合该预测轨迹进行目标函数的求解。如此，实时考虑其它无人机的预测轨迹，实现多无人机的协同轨迹规划。
进一步的，本实施例中，上述方法还包括：基于获取的上述目标无人机在当前滚动预测时域内对应的目标飞行轨迹，在当前滚动控制时域内对上述目标无人机进行控制，上述当前滚动控制时域小于上述当前滚动预测时域。
具体的，通过窗口滚动预测和滚动控制的方式对无人机进行持续的预测和控制，将多无人机的协同飞行轨迹生成问题表述为对有限滚动时域内各个无人机的带有状态约束的运动基元的采样，并代入旋翼无人机的运动模型中得到无人机的飞行轨迹。其中，滚动的预测窗口相当于滚动预测时域，当前滚动预测时域是当前时刻对应的预测时域，当前滚动控制时域为当前时刻对应的控制时域。在一种应用场景中，滚动预测时域设置为20秒，滚动控制时域设置为5秒，如此，每一次预测未来20秒的目标轨迹，但只用其中的5秒来进行控制，在5秒之后进入下一个滚动预测时域，重新预测20秒，并控制下一个5秒的动作。实现对于各个无人机的滚动预测和控制，使得无人机的飞行轨迹更安全合理。其中，通过数传电台将各个无人机当前位置状态以及通过滚动时域规划周期内的预测轨迹共享至其它无人机。在一种应用场景中，每一个窗口(滚动预测时域)的预测都是独立的，与上一个窗口的预测值无关，可以有效减小计算量，提高预测的实时性。
如此，本发明实施例提供一种基于分布式模型预测控制(DMPC，DistributedModel Predictive Control)的复杂动态环境中多无人机协同轨迹规划方法，能够在复杂动态环境中综合考虑无人机与环境障碍物以及无人机之间的避碰、无人机动力学约束、飞行平滑性以及轨迹规划实时性等问题，实现复杂环境多无人机协同轨迹规划任务。本发明实施例，提供的一种复杂动态环境中多无人机协同的轨迹规划方法，采用并行启发式优化算法(粒子群算法)，对环境复杂度变化具有较强的鲁棒性。通过机载计算机处理双目相机的图像信息进行环境感知与建图，并转换为欧式距离场地图。采用分布式模型预测控制(DMPC)的方法和基于GPU加速的并行粒子群算法，并考虑复杂动态环境中各个无人机飞行轨迹的平滑性、飞行安全性和动态可行性，进行滚动时域内的状态约束运动基元求解，得到未来时刻各架无人机的飞行轨迹，保证多无人机协同轨迹规划的实时性、安全性和高效性。
需要说明的是，本实施例中以对无人机进行轨迹规划为例进行说明，实际使用过程中，上述方法还可以用于其它飞行器的轨迹规划，在此不作具体限定。
示例性装置
如图6所示，对应于上述多无人机轨迹规划方法，本发明实施例还提供一种多无人机轨迹规划装置，上述多无人机轨迹规划装置包括：
无人机运动模型建立模块510，用于建立无人机运动模型，其中，上述无人机运动模型用于基于输入量获取无人机的状态量。
其中，上述无人机运动模型是用于描述无人机的运动状态的模型。具体的，无人机运行模型可以基于输入量获取无人机的状态量，状态量包括一个或一组(本实施例中为一组)用于描述无人机的飞行状态的参数。本实施例中建立的无人机运动模型的输入量选为无人机的加加速度，在其它应用场景中，输入量还可以根据实际需求选为其它参数，在此不作具体限定。
目标状态约束运动基元采样模型获取模块520，用于获取目标状态约束运动基元采样模型，其中，上述状态约束运动基元采样模型用于基于给定的初始状态和末端状态获取无人机的状态约束运动基元。
其中，上述初始状态是无人机当前的运动状态，末端状态是无人机在预设的时间后(例如对应的滚动预测时域之后)需要达到的状态。本实施例中，无人机的初始状态包括无人机当前的位置、速度和加速度；末端状态包括末端(预设时间之后)对应的位置、速度和加速度；例如，如果预设的滚动预测时域为20秒，则初始状态是当前时刻的状态，末端状态是20秒之后的状态。
本实施例中，通过目标状态约束运动基元采样模型对目标无人机的状态约束运动基元进行采样(即求解)时，输入目标状态约束运动基元采样模型的初始状态是实时获取的目标无人机的当前状态，末端状态是通过求解目标无人机的目标函数获得的最优末端状态。具体的，求解获得的目标无人机的状态约束运动基元与上述无人机运动模型中的输入量对应，即可以作为上述无人机运动模型的输入量从而获得目标无人机的状态量，方便进行计算和规划。同时，也可以将求无人机轨迹的问题转化为一种求末端状态的问题，方便计算，有利于提升计算和处理的实时性。
目标函数构建模块530，用于对于上述多无人机集合中的任一目标无人机，构建上述目标无人机的轨迹对应的目标函数，其中，上述目标函数中包括飞行安全约束和目标点距离优化约束，上述飞行安全约束用于限定上述目标无人机与障碍物之间的距离，上述障碍物包括环境障碍物和上述多无人机集合中除上述目标无人机以外的其它无人机。
其中，上述目标无人机是多无人机集合中需要进行轨迹规划的无人机。需要说明的是，本实施例中，基于上述多无人机轨迹规划方法对多无人机集合中的每一架无人机分别进行轨迹规划，具体的，对于多无人机组成的集合中的每一架无人机都使用相同的方法进行轨迹规划，因此上述目标无人机可以是多无人机集合中的任意一架无人机，本实施例中以对一架目标无人机进行的操作为例进行说明，但不作为具体限定。实际使用过程中，多无人机集合中的每一架无人机都同时且实时地根据上述多无人机轨迹规划方法进行分布式的轨迹规划(即多架无人机各自计算各自的轨迹，而不是集中式地计算)，即每一架无人机都可以作为目标无人机，有利于进一步提升计算速度，满足实时性的要求。
其中，上述目标函数是用于描述和限定目标无人机的飞行轨迹的函数。本实施例中，通过为目标函数设置约束条件，使得规划出的飞行轨迹能够满足约束。具体的，目标函数中包括飞行安全约束和目标点距离约束。飞行安全约束用于限定目标无人机与障碍物之间的距离，例如，限制距离始终大于0(或其它预设的大于0安全距离值)，以保证目标无人机不会与障碍物碰撞，且障碍物是考虑环境障碍物和其它无人机的，可以避免多个无人机之间互相碰撞。目标点距离约束用于在考虑障碍物的基础上，最小化无人机到目标点的距离值，有利于提升无人机轨迹规划的合理性和安全性。
目标函数求解模块540，用于求解上述目标无人机的目标函数，获取上述目标无人机在当前滚动预测时域内的最优末端状态，并基于上述目标状态约束运动基元采样模型和上述无人机运动模型获取上述目标无人机在当前滚动预测时域内对应的目标飞行轨迹，其中，上述目标飞行轨迹包括一组状态量。
具体的，本实施例中，通过求解目标函数可以获得目标无人机在当前滚动预测时域内的最优末端状态，如此，可以在根据目标函数获得当前滚动预测时域内的一组最优末端状态的情况下，结合目标状态约束运动基元采样模型和无人机运动模型，获取在当前滚动预测时域内的一组无人机的状态量，从而获得对应的目标飞行轨迹，有利于实现对无人机的控制。
由上可见，本发明实施例提供的多无人机轨迹规划装置通过无人机运动模型建立模块510建立无人机运动模型，其中，上述无人机运动模型用于基于输入量获取无人机的状态量；通过目标状态约束运动基元采样模型获取模块520获取目标状态约束运动基元采样模型，其中，上述状态约束运动基元采样模型用于基于给定的初始状态和末端状态获取无人机的状态约束运动基元；对于上述多无人机集合中的任一目标无人机，通过目标函数构建模块530构建上述目标无人机的轨迹对应的目标函数，其中，上述目标函数中包括飞行安全约束和目标点距离优化约束，上述飞行安全约束用于限定上述目标无人机与障碍物之间的距离，上述障碍物包括环境障碍物和上述多无人机集合中除上述目标无人机以外的其它无人机；通过目标函数求解模块540求解上述目标无人机的目标函数，获取上述目标无人机在当前滚动预测时域内的最优末端状态，并基于上述目标状态约束运动基元采样模型和上述无人机运动模型获取上述目标无人机在当前滚动预测时域内对应的目标飞行轨迹，其中，上述目标飞行轨迹包括一组状态量。本发明方案中考虑其它无人机的影响，结合环境障碍物以及多无人机集合中其它无人机为目标无人机构建飞行安全约束，并基于飞行安全约束和目标点距离优化约束为当前需要进行轨迹规划的目标无人机构建目标函数，通过求解目标函数来实现对目标无人机的轨迹规划。与现有技术相比，本发明方案结合考虑了环境中静止障碍物和其它移动的无人机的影响，有利于实现多无人机协同轨迹规划，避免无人机之间发生碰撞事故，且有利于提升无人机的安全性。
具体的，本实施例中，上述无人机轨迹规划装置及其各个模块对应的功能及具体的处理流程可以参照多无人机轨迹规划方法中的具体描述，在此不再赘述。
本发明实施例中，还提供一种无人机，多个上述无人机构成一个多无人机集合，上述多无人机集合中的各上述无人机分别上述任意一种多无人机轨迹规划方法进行轨迹规划。
进一步的，本实施例中，还采用不少于2架无人机、机载双目相机、机载计算机以及数传电台组成的无人机轨迹规划系统进行协同轨迹规划，图7是本发明实施例提供的一种无人机轨迹规划系统的结构示意图，如图7所示，上述系统中包括无人机本体、数传电台、双目相机、机载计算机和飞行控制器。图8是本发明实施例提供的一种图7中系统的工作原理示意图，如图8所示，通过双目相机采集环境信息，将采集的图像信息传输到机载计算机，机载计算机将图像信息进行处理获得欧式距离场地图；数传电台则将其它无人机的运动状态与预测轨迹也传输到机载计算机；机载计算机基于并行PSO(粒子群优化算法)对应的模型进行预测控制求解，最终获得目标飞行轨迹，将目标飞行轨迹发送给飞行控制器，并通过飞行控制器实现对各无人机的飞行轨迹控制。
本发明实施例还提供一种计算机可读存储介质，上述计算机可读存储介质上存储有多无人机轨迹规划程序，上述多无人机轨迹规划程序被处理器执行时实现本发明实施例提供的任一种多无人机轨迹规划方法的步骤。
应理解，上述实施例中各步骤的序号大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将上述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本发明的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各实例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟是以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同的方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
在本发明所提供的实施例中，应该理解到，所揭露的装置/终端设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/终端设备实施例仅仅是示意性的，例如，上述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以由另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。
上述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，上述计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，上述计算机程序包括计算机程序代码，上述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。上述计算机可读介质可以包括：能够携带上述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，RandomAccess Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，上述计算机可读存储介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减。
以上所述实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解；其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不是相应技术方案的本质脱离本发明各实施例技术方案的精神和范围，均应包含在本发明的保护范围之内。
Claims (13)
1.一种多无人机轨迹规划方法，其特征在于，所述方法用于对多无人机集合中的每一架无人机分别进行轨迹规划，所述方法包括：
建立无人机运动模型，其中，所述无人机运动模型用于基于输入量获取无人机的状态量；
获取目标状态约束运动基元采样模型，其中，所述状态约束运动基元采样模型用于基于给定的初始状态和末端状态获取无人机的状态约束运动基元；
对于所述多无人机集合中的任一目标无人机，构建所述目标无人机的轨迹对应的目标函数，其中，所述目标函数中包括飞行安全约束和目标点距离优化约束，所述飞行安全约束用于限定所述目标无人机与障碍物之间的距离，所述障碍物包括环境障碍物和所述多无人机集合中除所述目标无人机以外的其它无人机；
求解所述目标无人机的目标函数，获取所述目标无人机在当前滚动预测时域内的最优末端状态，并基于所述目标状态约束运动基元采样模型和所述无人机运动模型获取所述目标无人机在当前滚动预测时域内对应的目标飞行轨迹，其中，所述目标飞行轨迹包括一组状态量。
2.根据权利要求1所述的多无人机轨迹规划方法，其特征在于，所述无人机运动模型为模型预测控制运动模型。
3.根据权利要求1所述的多无人机轨迹规划方法，其特征在于，所述无人机运动模型的输入量为无人机的加加速度，所述状态量包括无人机的位置、速度和加速度，所述状态约束运动基元为无人机的加加速度。
4.根据权利要求1所述的多无人机轨迹规划方法，其特征在于，所述获取目标状态约束运动基元采样模型，包括：
获取待训练的状态约束运动基元采样模型；
获取训练数据，基于所述训练数据对所述待训练的状态约束运动基元采样模型进行离线动态规划迭代训练，获取训练后的状态约束运动基元采样模型并作为目标状态约束运动基元采样模型。
5.根据权利要求1所述的多无人机轨迹规划方法，其特征在于，所述目标函数中还包括能耗优化约束，所述能耗优化约束用于限定所述目标无人机的轨迹对应的能耗。
6.根据权利要求1所述的多无人机轨迹规划方法，其特征在于，在所述构建所述目标无人机的轨迹对应的目标函数之前，所述方法还包括：
基于双目相机获取环境点云信息；
基于所述环境点云信息获取欧式距离场地图，其中，所述欧式距离场地图中任意栅格中存储了该栅格到最近环境障碍物的欧式距离信息。
7.根据权利要求6所述的多无人机轨迹规划方法，其特征在于，所述飞行安全约束包括环境障碍物距离约束和其它无人机距离约束，所述环境障碍物距离约束基于所述欧式距离场地图建立，所述其它无人机距离约束基于所述其它无人机的状态信息建立，其中，所述状态信息包括各所述其它无人机的当前位置状态和预测轨迹。
8.根据权利要求7所述的多无人机轨迹规划方法，其特征在于，所述求解所述目标无人机的目标函数，获取所述目标无人机的最优末端状态，并基于所述目标状态约束运动基元采样模型和所述无人机运动模型获取所述目标无人机对应的目标飞行轨迹，包括：
基于并行粒子群算法对所述目标函数进行迭代求解，获取所述目标无人机在当前滚动预测时域内对应的最优末端状态，其中，所述并行粒子群算法中每个粒子为所述目标无人机的一个候选末端状态；
基于所述最优末端状态、所述目标状态约束运动基元采样模型和所述无人机运动模型获取所述目标无人机在当前滚动预测时域内对应的目标飞行轨迹。
9.根据权利要求8所述的多无人机轨迹规划方法，其特征在于，所述基于并行粒子群算法对所述目标函数进行迭代求解，获取所述目标无人机在当前滚动预测时域内对应的最优末端状态，包括：
基于并行粒子群算法对所述目标函数进行g次迭代求解，获得所述目标无人机在当前滚动预测时域内对应的最优末端状态，在迭代求解的过程中，第1次迭代时使用的各所述其它无人机的预测轨迹是基于预设的初始加加速度值获得的，第h次迭代时使用的各所述其它无人机的预测轨迹是根据各所述其它无人机第h-1次迭代后的末端状态获得的，g为预设的迭代次数，h大于1且不大于g。
10.根据权利要求1所述的多无人机轨迹规划方法，其特征在于，所述方法还包括：
基于获取的所述目标无人机在当前滚动预测时域内对应的目标飞行轨迹，在当前滚动控制时域内对所述目标无人机进行控制，所述当前滚动控制时域小于所述当前滚动预测时域。
11.一种多无人机轨迹规划装置，其特征在于，所述装置用于对多无人机集合中的每一架无人机分别进行轨迹规划，所述装置包括：
无人机运动模型建立模块，用于建立无人机运动模型，其中，所述无人机运动模型用于基于输入量获取无人机的状态量；
目标状态约束运动基元采样模型获取模块，用于获取目标状态约束运动基元采样模型，其中，所述状态约束运动基元采样模型用于基于给定的初始状态和末端状态获取无人机的状态约束运动基元；
目标函数构建模块，用于对于所述多无人机集合中的任一目标无人机，构建所述目标无人机的轨迹对应的目标函数，其中，所述目标函数中包括飞行安全约束和目标点距离优化约束，所述飞行安全约束用于限定所述目标无人机与障碍物之间的距离，所述障碍物包括环境障碍物和所述多无人机集合中除所述目标无人机以外的其它无人机；
目标函数求解模块，用于求解所述目标无人机的目标函数，获取所述目标无人机在当前滚动预测时域内的最优末端状态，并基于所述目标状态约束运动基元采样模型和所述无人机运动模型获取所述目标无人机在当前滚动预测时域内对应的目标飞行轨迹，其中，所述目标飞行轨迹包括一组状态量。
12.一种无人机，其特征在于，多个所述无人机构成一个多无人机集合，所述多无人机集合中的各所述无人机分别基于如权利要求1-10任意一项所述的多无人机轨迹规划方法进行轨迹规划。
13.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有多无人机轨迹规划程序，所述多无人机轨迹规划程序被处理器执行时实现如权利要求1-10任意一项所述的多无人机轨迹规划方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111248705.1A CN113900449B (zh) | 2021-10-26 | 2021-10-26 | 多无人机轨迹规划方法、装置、无人机及存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111248705.1A CN113900449B (zh) | 2021-10-26 | 2021-10-26 | 多无人机轨迹规划方法、装置、无人机及存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113900449A true CN113900449A (zh) | 2022-01-07 |
| CN113900449B CN113900449B (zh) | 2023-12-22 |
Family
ID=79026426
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111248705.1A Active CN113900449B (zh) | 2021-10-26 | 2021-10-26 | 多无人机轨迹规划方法、装置、无人机及存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113900449B (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116774735A (zh) * | 2023-08-24 | 2023-09-19 | 北京理工大学 | 一种基于边缘计算的无人机集群轨迹规划方法及系统 |
| CN117826845A (zh) * | 2024-03-04 | 2024-04-05 | 易创智芯(西安)科技有限公司 | 一种航空运行安全主动避障及规划方法 |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105841702A (zh) * | 2016-03-10 | 2016-08-10 | 赛度科技（北京）有限责任公司 | 一种基于粒子群优化算法的多无人机航路规划方法 |
| CN108388270A (zh) * | 2018-03-21 | 2018-08-10 | 天津大学 | 面向安全域的集群无人机轨迹姿态协同控制方法 |
| CN110632941A (zh) * | 2019-09-25 | 2019-12-31 | 北京理工大学 | 一种复杂环境下无人机目标跟踪的轨迹生成方法 |
| CN111562797A (zh) * | 2020-07-06 | 2020-08-21 | 北京理工大学 | 确保收敛的无人机飞行时间最优实时轨迹优化方法 |
| CN111833381A (zh) * | 2020-06-24 | 2020-10-27 | 鹏城实验室 | 一种无人机目标跟踪轨迹生成方法、无人机及存储介质 |
| CN112733251A (zh) * | 2020-12-24 | 2021-04-30 | 中国人民解放军空军工程大学 | 一种多无人飞行器协同航迹规划方法 |
| CN112817330A (zh) * | 2021-01-05 | 2021-05-18 | 北京联合大学 | 一种多无人机四维航迹协同规划方法及系统 |
| CN112923925A (zh) * | 2021-01-07 | 2021-06-08 | 天津大学 | 双模式盘旋追踪地面目标的多无人机协同航迹规划方法 |
| CN113064450A (zh) * | 2021-04-01 | 2021-07-02 | 河海大学 | 一种基于退火算法的量子粒子群求解无人机路径规划方法 |
| CN113359853A (zh) * | 2021-07-09 | 2021-09-07 | 中国人民解放军国防科技大学 | 一种无人机编队协同目标监视的路径规划方法及系统 |
-
2021
- 2021-10-26 CN CN202111248705.1A patent/CN113900449B/zh active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105841702A (zh) * | 2016-03-10 | 2016-08-10 | 赛度科技（北京）有限责任公司 | 一种基于粒子群优化算法的多无人机航路规划方法 |
| CN108388270A (zh) * | 2018-03-21 | 2018-08-10 | 天津大学 | 面向安全域的集群无人机轨迹姿态协同控制方法 |
| CN110632941A (zh) * | 2019-09-25 | 2019-12-31 | 北京理工大学 | 一种复杂环境下无人机目标跟踪的轨迹生成方法 |
| CN111833381A (zh) * | 2020-06-24 | 2020-10-27 | 鹏城实验室 | 一种无人机目标跟踪轨迹生成方法、无人机及存储介质 |
| CN111562797A (zh) * | 2020-07-06 | 2020-08-21 | 北京理工大学 | 确保收敛的无人机飞行时间最优实时轨迹优化方法 |
| CN112733251A (zh) * | 2020-12-24 | 2021-04-30 | 中国人民解放军空军工程大学 | 一种多无人飞行器协同航迹规划方法 |
| CN112817330A (zh) * | 2021-01-05 | 2021-05-18 | 北京联合大学 | 一种多无人机四维航迹协同规划方法及系统 |
| CN112923925A (zh) * | 2021-01-07 | 2021-06-08 | 天津大学 | 双模式盘旋追踪地面目标的多无人机协同航迹规划方法 |
| CN113064450A (zh) * | 2021-04-01 | 2021-07-02 | 河海大学 | 一种基于退火算法的量子粒子群求解无人机路径规划方法 |
| CN113359853A (zh) * | 2021-07-09 | 2021-09-07 | 中国人民解放军国防科技大学 | 一种无人机编队协同目标监视的路径规划方法及系统 |
Non-Patent Citations (2)
| Title |
|---|
| CHUYI XIONG等: "Multi-UAV 3D Path Planning in Simultaneous Attack", 《2020 IEEE 16TH INTERNATIONAL CONFERENCE ON CONTROL & AUTOMATION (ICCA)》, pages 500 - 505 * |
| 徐广通等: "基于滚动规划框架的多无人机协同轨迹快速生成方法", 《无人系统技术》, vol. 4, no. 2, pages 33 - 39 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116774735A (zh) * | 2023-08-24 | 2023-09-19 | 北京理工大学 | 一种基于边缘计算的无人机集群轨迹规划方法及系统 |
| CN116774735B (zh) * | 2023-08-24 | 2023-11-10 | 北京理工大学 | 一种基于边缘计算的无人机集群轨迹规划方法及系统 |
| CN117826845A (zh) * | 2024-03-04 | 2024-04-05 | 易创智芯(西安)科技有限公司 | 一种航空运行安全主动避障及规划方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113900449B (zh) | 2023-12-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Kamel et al. | Robust collision avoidance for multiple micro aerial vehicles using nonlinear model predictive control | |
| CN108549407B (zh) | 一种多无人机协同编队避障的控制算法 | |
| Liang et al. | Three-dimensional path following of an underactuated AUV based on fuzzy backstepping sliding mode control | |
| CN113900449B (zh) | 多无人机轨迹规划方法、装置、无人机及存储介质 | |
| Li et al. | A UAV path planning method based on deep reinforcement learning | |
| Frew et al. | Obstacle avoidance with sensor uncertainty for small unmanned aircraft | |
| Yasin et al. | Navigation of autonomous swarm of drones using translational coordinates | |
| Xi et al. | GTO-MPC-based target chasing using a quadrotor in cluttered environments | |
| Ke et al. | Cooperative path planning for air–sea heterogeneous unmanned vehicles using search-and-tracking mission | |
| Zhao et al. | Four-dimensional trajectory generation for UAVs based on multi-agent Q learning | |
| Vijayakumari et al. | Receding-horizon trajectory planning for multiple uavs using particle swarm optimization | |
| Wang et al. | Decentralized MPC-based trajectory generation for multiple quadrotors in cluttered environments | |
| Hafez et al. | Formation reconfiguration of cooperative UAVs via Learning Based Model Predictive Control in an obstacle-loaded environment | |
| Fang et al. | Piecewise-potential-field-based path planning method for fixed-wing UAV formation | |
| Yuan et al. | Distributed game strategy for unmanned aerial vehicle formation with external disturbances and obstacles | |
| Huang et al. | Vision-based distributed multi-UAV collision avoidance via deep reinforcement learning for navigation | |
| Bodi et al. | Reinforcement learning based UAV formation control in GPS-denied environment | |
| CN114138022A (zh) | 一种基于精英鸽群智能的无人机集群分布式编队控制方法 | |
| Selje et al. | Gps-denied three dimensional leader-follower formation control using deep reinforcement learning | |
| Jiang | Binocular stereo vision intelligent control framework for the landing position of quadrotor UAV based on fuzzy control | |
| CN115903882A (zh) | 一种视场角约束下集群无人机协同目标跟踪控制方法 | |
| CN114943168A (zh) | 一种水上浮桥组合方法及系统 | |
| Ning et al. | Lattice flocking of multi-quadrotor system: an algorithm based on artificial potential field | |
| Xu et al. | Recent progress on multiple-unmanned aerial vehicle collision avoidance algorithms | |
| Xiong et al. | Multi-Robot Formation with Obstacle Avoidance: An Improved Velocity Obstacle-Based Approach |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |