一种基于逆推法的多无人机协同航迹规划方法与流程

本发明属于无人机航迹规划技术领域，具体涉及一种基于逆推法的多无人机协同航迹规划方法。

背景技术：

随着计算机技术、传感器技术、无线通信技术和无人机技术的发展，无人机逐渐具有高度自主性控制能力，与有飞行员驾驶的载人飞行器相比，无人机具有众多潜在的优点，如：自主飞行的无人机在燃油充足的情况下不需要休息，可以进行更长时间的巡航；无人机不需要提供飞行员必须的生命支持系统，这不仅减少了飞行成本，还为部署传感器和其他设备提供了更多空间。多无人机协同作战能够提高无人机作战效能，成为无人机作战应用的发展趋势，而无人机的航迹规划为其提供了实现的可能性。

于是研究高效率的多无人机协同航迹规划方法是提升无人机协同作战能力的有效途径，也逐渐成为一个研究热点。多无人机协同航迹规划旨在为编队中每架无人机设计一条符合曲率约束的可飞行路径，使得无人机群同时到达指定地点，并保证无人机之间不发生碰撞。利用计算机智能的算法代替人工地为无人机进行航迹规划，不仅节约时间和人力，而且在无人机和任务数量较多的时候计算机计算出的路径往往会比人工地规划结果更加合理。所以，如何在复杂的战场环境中对无人机进行更加合理的航迹规划也成为无人机航迹规划研究的目的和方向，具有较大的理论和实践意义。

无人机协同路径规划主要包括编队集结路径规划、编队保持路径规划、编队重构路径规划三方面。多无人机协同航迹规划考虑的主要约束有可飞性约束和安全性约束。其中可飞性约束指规划的路径能够满足各无人机的运动学约束条件，如最大曲率约束，最大挠率约束、路径曲率连续约束等。路径安全性是指无人机组内不发生碰撞，并且无人机规划的路径不与环境中的障碍物发生碰撞。其他的约束还有时间协同约束，即各无人机必须同时到达目标点完成队形重构。路径规划的目标是使得路径整体性能指标最优，包括路径长度、平滑性能和隐身性能等。

路径规划是无人系统中不可分割的一部分，目前大多数采用的路径规划技术都是从地面机器人领域借鉴而来的。常用的求解路径规划的方法有最优控制法、人工势场法、基于图形学的算法和智能优化方法等。

传统方法具有比较好的完备性，但是在环境过于复杂的时候计算量会大大增加，并且有可能找不到路径。因此需要一种极大减小计算量的多无人机协同航迹规划方法。

技术实现要素：

本发明所要解决的技术问题在于针对上述现有技术中的不足，提供一种基于逆推法的多无人机协同航迹规划方法，其方法简单，采用逆推法从航路终止位姿向起始位姿逆推计算碰撞点，对两条可飞行路径分别在碰撞点或起点位姿进行补偿，在保证多无人机安全飞行的同时，极大地减小了计算时长，减少了航迹生成时间，使用效果好。

为解决上述技术问题，本发明采用的技术方案是：一种基于逆推法的多无人机协同航迹规划方法，其特征在于，包括以下步骤：

步骤一、可飞行路径规划：分别确定n架无人机的起始位姿和终止位姿，为第i架无人机产生连接起始位姿φsi和终止位姿φfi的可飞行路径，n架无人机的可飞行路径均满足曲率约束，其中φsi表示第i架无人机的起始位姿，φfi表示第i架无人机的终止位姿，i＝1,2,...，n,n表示无人机的数量；

步骤二、调整可飞行路径参数产生可飞行等长路径：

步骤201、选取参考路径：分别计算n架无人机的可飞行路径的路径长度，选择n架无人机的可飞行路径中路径长度最长的一条作为参考路径；

步骤202、修正剩余可飞行路径：调整第i条可飞行路径的起始曲率半径ρsi和终止曲率半径ρfi，使li＝lref，其中lref表示参考路径的路径长度，li表示第i架无人机的可飞行路径的路径长度；

步骤三、任意两条可飞行等长路径碰撞检测：计算第p架无人机和第q架无人机在飞行t时刻时的分离距离dt-p,q，若dt-p,q≥rp+rq，则进入步骤五，否则进入步骤四，其中rp表示第p架无人机的安全圆半径，rq表示第q架无人机的安全圆半径，第p架无人机和第q架无人机表示n架无人机中任意两架，p和q均为不大于n的正整数；

步骤四、逆推法调整可飞行等长路径：

步骤401、逆推碰撞点：从航路终止位姿向起始位姿逆推，计算第p架无人机和第q架无人机第一次发生碰撞的碰撞点；

步骤402、碰撞点规避：同时修改第p架无人机的可飞行路径和第q架无人机的可飞行路径，使第p架无人机在碰撞点沿绕行圆周绕行一周，第p架无人机的绕行圆周的半径为rp，第p架无人机的绕行圆周与碰撞点的位姿相切；第q架无人机在起始位姿沿绕行圆周绕行一周，第q架无人机的绕行圆周的半径为rq，第q架无人机的绕行圆周与第q架无人机的起始位姿相切；

步骤403、返回步骤三；

步骤五、生成安全可飞行路径。

上述的一种基于逆推法的多无人机协同航迹规划方法，其特征在于：步骤一中采用满足曲率约束的dubins路径规划方法进行可飞行路径规划。

上述的一种基于逆推法的多无人机协同航迹规划方法，其特征在于：步骤202中采用粒子群算法进行起始曲率半径ρsi和终止曲率半径ρfi寻优。

上述的一种基于逆推法的多无人机协同航迹规划方法，其特征在于：第p架无人机的绕行圆周的半径rp大于第p架无人机的最小转弯半径，第q架无人机的绕行圆周的半径rq大于第q架无人机的最小转弯半径。

本发明与现有技术相比具有以下优点：

1、本发明采用逆推法从航路终止位姿向起始位姿逆推计算碰撞点，并以碰撞点为基础进行可飞行路径碰撞规避，在保证多无人机安全飞行的同时，极大地减小了计算时长，减少了航迹生成时间，使用效果好。

2、本发明在碰撞点规避时，对两条可飞行路径的其中一条在碰撞点进行路径补偿，对另一条在起点位姿进行路径补偿，使得两条可飞行路径在同一时刻对应的航路点全部发生改变，可最大程度的减少补偿调整后的可飞行路径发生碰撞的可能性。

综上所述，本发明方法简单，采用逆推法从航路终止位姿向起始位姿逆推计算碰撞点，对两条可飞行路径的其中一条在碰撞点进行路径补偿，对另一条在起点位姿进行路径补偿，在生成协同航路时能有效兼顾可飞性、安全性与时间协同，并缩短路径规划时间。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

图1为本发明的方法流程图。

图2为本发明调整可飞行等长路径的示意图。

具体实施方式

下面结合附图及本发明的实施例对本发明的方法作进一步详细的说明。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

需要说明的是，本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施方式例如能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

为了便于描述，在这里可以使用空间相对术语，如“在……之上”、“在……上方”、“在……上表面”、“上面的”等，用来描述如在图中所示的一个器件或特征与其他器件或特征的空间位置关系。应当理解的是，空间相对术语旨在包含除了器件在图中所描述的方位之外的在使用或操作中的不同方位。例如，如果附图中的器件被倒置，则描述为“在其他器件或构造上方”或“在其他器件或构造之上”的器件之后将被定位为“在其他器件或构造下方”或“在其他器件或构造之下”。因而，示例性术语“在……上方”可以包括“在……上方”和“在……下方”两种方位。该器件也可以其他不同方式定位(旋转90度或处于其他方位)，并且对这里所使用的空间相对描述作出相应解释。

如图1所示，本发明包括以下步骤：

步骤一、可飞行路径规划：分别确定n架无人机的起始位姿和终止位姿，为第i架无人机产生连接起始位姿φsi和终止位姿φfi的可飞行路径，n架无人机的可飞行路径均满足曲率约束，其中φsi表示第i架无人机的起始位姿，φfi表示第i架无人机的终止位姿，i＝1,2,...，n,n表示无人机的数量。

实际使用时，φsi＝(xsi,ysi,θsi)，θsi表示第i架无人机的起始航向角，φfi＝(xfi,yfi,θfi)，θfi表示第i架无人机的终止航向角。

计算机根据给定的起始位姿φsi和终止位姿φfi为n架无人机分别按照最大曲率κmax产生初始dubins路径，dubins路径采用解析几何法设计。

具体实施时，对于第i架无人机来说，当给定第i架无人机的起始位姿φsi和终止位姿φfi，采用dubins路径规划方法可以产生4种符合曲率约束的dubins路径，分别为lsl、lsr、rsr、rsl型，计算机分别计算4种符合曲率约束的dubins路径的路径长度，并选择路径长度最小的飞行路径作为第i架无人机的可飞行路径。据此分别确定n架无人机的可飞行路径。

步骤二、调整可飞行路径参数产生可飞行等长路径：

步骤201、选取参考路径：分别计算n架无人机的可飞行路径的路径长度，选择n架无人机的可飞行路径中路径长度最长的一条作为参考路径。

因此需要对其他n-1条可飞行路径做修正。

步骤202、修正剩余可飞行路径：调整第i条可飞行路径的起始曲率半径ρsi和终止曲率半径ρfi，使li＝lref，其中lref表示参考路径的路径长度，li表示第i架无人机的可飞行路径的路径长度。

实际使用时，使n条可飞行路径的路径长度相同，在假设n个无人机速度相等时，即可保证n架无人机能同时到达各自的终止位姿。

对于dubins路径来说，每架无人机的dubins路径都有两个可以优化的参数，分别是起始曲率半径ρsi和终止曲率半径ρfi，当给定第i架无人机的起始位姿φsi和终止位姿φfi，决定第i架无人机可飞行路径的路径长度的参数就只有起始曲率半径ρsi和终止曲率半径ρfi。

具体实施时，本实施例中，采用粒子群优化算法寻找符合目标函数的曲率半径。假设编队内有n架无人机，则可以设置n个种群，每个种群需要优化的参数是dubins路径的起始曲率半径ρsi和终止曲率半径半径ρfi，即搜索空间维度dim＝2。每个子群的粒子数目设置为m＝30，最大迭代次数设置为maxdt＝50。粒子的搜寻位置设置为[ρmin,ρmax]，其中ρmax根据实际环境而设定。粒子的搜寻速度取[-1.5,1.5]。首先对每个子群的粒子生成符合条件的随机初始位置和初始速度，然后计算每个粒子的适应度，进而初始化个体极值pi^best和群体极值gbest。在这里粒子的适应度函数定义为：f＝f(ρsi,ρfi)＝-li-lref，i＝1,2,...,n-1，当粒子的适应度越高时，表示路径长度和参考路径长度约相近，粒子的位置更优。随着迭代的进行，粒子会向路径等长的优化目标靠近。

粒子在每一次迭代中，通过学习个体极值pi^best和群体极值gbest来更新自己的速度和位置。速度的更新算法为：位置的更新算法为：

为了避免速度过大跳过最优解，或者速度变化过小难以到达最优解，通常为粒子的搜寻速度和搜寻位置各取一个限定的范围：[-vmin，vmax]和[xmin,xmax]。粒子每更新一次位置，就重新计算适应度，进而更新个体极值pi^best和群体极值gbest，直到满足终止条件，输出最优解，即每架无人机的起始曲率半径和终止曲率半径，至此多无人机协同航路规划为每架无人机生成了满足曲率约束的dubins路径，并保证n个无人机可飞行路径的路径长度相同，在假设n个无人机速度相等时，通过产生等长路径来解决同时到达问题。

步骤三、任意两条可飞行等长路径碰撞检测：计算机计算第p架无人机和第q架无人机在飞行t时刻时的分离距离dt-p,q，若dt-p,q≥rp+rq，则进入步骤五，否则进入步骤四，其中rp表示第p架无人机的安全圆半径，rq表示第q架无人机的安全圆半径，第p架无人机和第q架无人机表示n架无人机中任意两架，p和q均为不大于n的正整数；

实际使用时，无人机的碰撞检测可以抽象成一个不等式约束问题，即任意两架无人机的最小分离距离大于或等于两架无人机安全半径之和，即dt-p,q≥rp+rq，说明该两架无人机没有碰撞风险。需要对任意两架无人机进行碰撞检测，从而确保无人机安全飞行，对n架无人机的机群来说，需要进行次碰撞规避计算。

具体实施时，假设n架无人机均做等速匀速运动，则航路就可以离散成许多间隔相等的航路点。通过计算任意两架无人机路径上同一时刻对应的航路点就可以得到任意两架无人机之间的实时距离dt-p,q，dt-p,q的计算公式为其中(xt-p，yt-p，zt-p)表示t时刻第p架无人机的航路点坐标，(xt-q，yt-q，zt-q)表示t时刻第q架无人机的航路点坐标。在二维平面内，可写成

步骤四、逆推法调整可飞行等长路径：

步骤401、逆推碰撞点：从航路终止位姿向起始位姿逆推，计算得到第p架无人机和第q架无人机第一次发生碰撞的碰撞点。

对于编队集结的多无人机来说，多无人机分别从四面八方飞来集合成编队，多无人机的起始位姿相距较远，而终止位姿相距较近，因此多无人机在越靠近终止位姿的航路点上发生碰撞的可能性越大，因此采用逆推法计算碰撞点，并以碰撞点为基础进行可飞行路径碰撞规避，在保证多无人机安全飞行的同时，极大地减小了计算时长，减少了航迹生成时间，使用效果好。

步骤402、碰撞点规避：同时修改第p架无人机的可飞行路径和第q架无人机的可飞行路径，使第p架无人机在碰撞点沿绕行圆周绕行一周，第p架无人机的绕行圆周的半径为rp，第p架无人机的绕行圆周与碰撞点的位姿相切；使第q架无人机在起始位姿沿绕行圆周绕行一周，第q架无人机的绕行圆周的半径为rq，第q架无人机的绕行圆周与第q架无人机的起始位姿相切。

需要说明的是，第p架无人机的绕行圆周的半径rp大于第p架无人机的最小转弯半径，第q架无人机的绕行圆周的半径rq大于第q架无人机的最小转弯半径。

如图2所示，第p架无人机的可飞行路径为p，第q架无人机的可飞行路径为q，可飞行路径p和可飞行路径q的碰撞点分别为可飞行路径p上的a点与可飞行路径q上的b点。其中实线表示可飞行路径，虚线表示起始圆或终止圆。

碰撞规避方法为：第p架无人机在其航路的碰撞点a处做与碰撞点a位姿相切的圆周运动，第p架无人机的其余航路点不变；第q架无人机在其航路的碰撞点b处继续做直线运动，第q架无人机在其航路的起始位姿做与起始位姿相切的圆周运动，使得第p架无人机和第q架无人机在同一时刻对应的航路点全部发生改变，从而避免第p架无人机和第q架无人机发生碰撞，且rp＝rq，保证了第q架无人机跟随第p架无人机修改后的可飞行路径的路径长度。

对于编队集结的多无人机来说，多无人机分别从四面八方飞来集合成编队，多无人机的起始位姿相距较远，而终止位姿相距较近，因此在起点位姿对可飞行路径进行补偿，可最大程度的减少补偿调整后的可飞行路径发生碰撞的可能性。

步骤403、返回步骤三。经过修正调整后的n条可飞行路径，返回步骤三，进行碰撞检测，若n条可飞行路径中任意两条可飞行路径均不存在碰撞风险，可进入步骤五，生成安全可飞行路径。

以上所述，仅是本发明的实施例，并非对本发明作任何限制，凡是根据本发明技术实质对以上实施例所作的任何简单修改、变更以及等效结构变化，均仍属于本发明技术方案的保护范围内。