无人机编队路径自动规划方法及装置与流程

本发明涉及无人机技术领域，更具体的，本发明的实施方式涉及一种无人机编队路径自动规划方法及装置。

背景技术：

本部分旨在为权利要求书中陈述的本发明的实施方式提供背景或上下文。此处的描述不因为包括在本部分中就承认是现有技术。

目前，无人机编队路径规划大多采用离线的集中式规划法。离线式规划法是在飞行任务实施前，预先计算好飞行任务中每架无人机的飞行航线，由于运算时间充分，可以得到较好的飞行航线。但发明人在实施本发明过程中发现，该集中式规划法至少存在如下缺陷：

1、实时性不强、更新效率低，由于现场环境是实时变化的，离线规划的航迹意味着随着时间推移可能变得非最优甚至无法执行，例如当遇到飞行环境发生变化时，不能及时做出响应，调整航线。

2、集中式规划法，是利用全局信息为每架无人机独立给出规划结果，在无人机数量众多时，运算压力呈指数上涨，不利于系统规模化。

技术实现要素：

本发明实施例的主要目的在于提出一种无人机编队的路径自动规划方法及装置，以解决无人机编队路径规划中存在的实时性不强、更新效率低且运算压力大的问题。

为了达到上述目的，本发明实施例提供一种无人机编队路径自动规划方法，包括：

无人机编队中的每一架无人机加载整个编队的机群路径规划数据和机群阵型描述数据，所述整个编队的机群路径规划数据为：无人机编队作为一个整体，为该整体组成的阵型中的预定点规划的行进轨迹数据；所述机群阵型描述数据至少包含所述编队中无人机数量以及每一架无人机相对所述预定点的位置坐标数据；所述每一架无人机根据所述整个编队的机群路径规划数据和机群阵型描述数据，采用基于斥力-引力模型的群智能算法进行实时的自身的路径规划。

为了达到上述目的，本发明实施例还提供一种无人机编队路径自动规划装置，包括：数据加载模块，用于加载整个编队的机群路径规划数据和机群阵型描述数据，所述整个编队的机群路径规划数据为：无人机编队作为一个整体，为该整体组成的阵型中的预定点规划的行进轨迹数据；所述机群阵型描述数据至少包含所述编队中无人机数量以及每一架无人机相对所述预定点的位置坐标数据；实时规划模块，用于根据所述整个编队的机群路径规划数据和机群阵型描述数据，采用基于斥力-引力模型的群智能算法进行实时的自身的路径规划。

本发明实施例的无人机编队路径自动规划方法及装置，每一架无人机加载的整个编队的机群路径规划数据和机群阵型描述数据是离线规划的整个无人机编队的规划数据，每架无人机基于该离线规划数据进行在线的自身路线的规划，提出的是一种离线规划和在线规划相结合的双层级规划法。此方法具有先粗糙后精细的计算特点，第一层继承了全局规划的优势，第二层解决了传统离线规划法无法对变化场景适应的问题。考虑到无人机群先天具备分布式计算的特点，对在线规划问题进行分布式设计，充分利用了无人机自身运算能力，节省了中心计算资源，为大规模组队创造了可能。并且，实现阵型保持、阵型变换、阵型避障等问题在统一算法框架下的求解，避免了传统方法纷繁的模式设计，更具通用性，且算法复杂度不随编队成员数量的增多而上升。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例的无人机编队路径自动规划方法的处理流程图；

图2为本发明实施例的无人机采用基于斥力-引力模型的群智能算法进行实时的自身的路径规划的方法流程图；

图3为本发明实施例的计算无人机所受引力的方法流程图；

图4为本发明实施例的计算无人机所受斥力的方法流程图；

图5示意了本发明实施例的当无人机的计算半径内没有障碍物时的受力情况；

图6示意了本发明实施例的当无人机的计算半径内出现障碍物时的受力情况；

图7为本发明另一实施例的无人机编队路径自动规划方法的处理流程图；

图8为本发明实施例的无人机执行的路线在线规划方法的完整处理流程图；

图9为本发明实施例的无人机编队路径自动规划装置的结构示意图；

图10为本发明实施例的实时规划模块12的结构示意图；

图11为本发明实施例的受力确定模块121的结构示意图；

图12为本发明实施例的引力确定单元1211的结构示意图；

图13为本发明实施例的斥力确定单元1212的结构示意图；

图14为本发明另一实施例的实时规划模块12的结构示意图；

图15为本发明一具体实施例的无障碍情况的编队阵型示意图；

图16为图15所示实施例的右侧出现障碍物及第一轮力合成运算示意图；

图17为图15所示实施例的右侧出现障碍物及第二轮力合成运算示意图；

图18为图15所示实施例的右侧出现障碍物及第三轮力合成运算示意图；

图19为图15所示实施例的右侧出现障碍物及第四轮力合成运算示意图；

图20为图15所示实施例的右侧出现障碍物及第五轮力合成运算示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本领域技术技术人员知道，本发明的实施方式可以实现为一种系统、装置、设备、方法或计算机程序产品。因此，本公开可以具体实现为以下形式，即：完全的硬件、完全的软件(包括固件、驻留软件、微代码等)，或者硬件和软件结合的形式。

下面参考本发明的若干代表性实施方式，详细阐释本发明的原理和精神。

本发明把整个路径规划问题分成两个子问题进行求解：离线规划问题和在线规划问题。总体来说，是自顶向下法与自底向上法相结合，离线与在线相结合，先粗后精的规划方法。其中，离线规划负责完成飞行前的整体编队的飞行轨迹规划，其特征为：离线的、粗糙的、整队的；在线规划可由每架无人机的MPU计算得到，负责完成无人机自身的实时位置的规划和调整，其特征为：实时的、精细的、单机的。

针对整体路径规划，本申请实施例可以采用启发式算法(如A*算法)，首先为机群规划出粗糙的离线路径。此路径是机群行动的参考蓝本，并非最终实施路径，实际路径在动态实时规划时，进行修正。离线路径的意义在于，整体把握路径规划，充分利用全局信息，降低动态路径规划的寻优难度。

针对动态路径规划，本申请实施例设计了分布式控制系统替代了原有的集中控制系统，解决了离线规划航迹的缺点。分布式规划法，由于可以把规划运算压力分配到每个无人机的每个时间片上，可以有效避免实时性不佳的问题；并且规划算法运算复杂度随无人机个数增加是以线性增加甚至不增加，如此就不对系统连接数量构成限制。路径规划运算分摊到每台无人机的MPU上，使运算压力不随无人机数量增加。在考虑到无人机阵型保持和避障问题上，引入基于引力-斥力的群智能算法，把无人机视为同时受到目的地位置引力和障碍物斥力作用的电荷，利用矢量合成确定单个无人机的路径。整个无人机群每步一轮迭代，动态中实现了各机间的规划协调，避免冲突，从而实现整体协调。

图1为本发明实施例的无人机编队路径自动规划方法的处理流程图。如图1所示，包括：

步骤S101，无人机编队中的每一架无人机加载整个编队的机群路径规划数据和机群阵型描述数据，所述整个编队的机群路径规划数据为：无人机编队作为一个整体，为该整体组成的阵型中的预定点规划的行进轨迹数据；所述机群阵型描述数据至少包含所述编队中无人机数量以及每一架无人机相对所述预定点的位置坐标数据；

步骤S102，所述每一架无人机根据所述整个编队的机群路径规划数据和机群阵型描述数据，采用基于斥力-引力模型的群智能算法进行实时的自身的路径规划。

通过图1的实施例，可以看出，本发明将规划运算的压力分配到每个无人机的每个时间片上，有效避免了实时性不佳的问题。整个编队的机群路径规划数据和机群阵型描述数据可以通过离线生成，例如可由中控服务器完成，该离线规划只负责计算编队整体的路径规划，并不涉及编队内每架无人机的路径规划问题；当编队规划路径文件计算完毕后，通过建立好的通信链路，加载到编队中的每架无人机上；当每架无人机具备起飞条件时，执行在线规划部分，并根据在线规划结果，执行飞行任务。

在本实施例中，所述整个编队的机群路径规划数据为：无人机编队作为一个整体，为该整体组成的阵型中的预定点规划的行进轨迹数据；所述机群阵型描述数据至少包含所述编队中无人机数量以及每一架无人机相对所述预定点的位置坐标数据。

需要明确的是，本发明实施例中所指出的“阵型中的预定点”可以为无人机编队组成的阵型的阵心位置，阵心即为编队阵型中的所有无人机的几何中心。当然，本申请实施例并不局限于此。

(1)采用A*算法搜索代价最小的路径，生成整个编队的机群路径规划数据。其中，所述机群路径规划数据包括所述无人机编队的总飞行时间、帧率以及编队阵型中的预定点的行进轨迹数据。在本发明实施例中，采用启发式A*算法获取机群路径规划数据，其基本思想是，搜索代价最小的路径。A*(A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法。

机群路径规划数据是时间-空间描述数据，描述的最小时间单位为帧，每一帧由一个空间点(X，Y，Z)或者(经度，纬度，高度)和姿态(yaw，pitch，roll)表示。通过指定时间间隔，确定了整个无人机编队的路径轨迹以及执行的速率。

如下所示，机群路径规划数据的起始是总时间、帧率，第二行往后是实际轨迹数据(该轨迹数据为编队阵型中的预定点的行进轨迹数据)，每帧含有6个变量，用逗号隔开，帧间用分号隔开：

total_time,FPS

x₁,y₁,z₁,yaw₁,pitch₁,roll₁；x₂,y₂,z₂,yaw₂,pitch₂,roll₂；…,x_n,y_n,z_n,yaw_n,pitch_n,roll_n；

(2)所述机群阵型描述数据至少包含所述编队中无人机数量以及每一架无人机相对所述预定点的位置坐标数据。

具体的，机群阵型描述数据是以阵心(预定点)为笛卡尔坐标系中心，生成机群坐标系，然后在该机群坐标系下描述每架无人机的自身坐标与阵心偏移量。如下所示，第一行描述无人机编队中的无人机的数量，第二行至第N+1行描述每架无人机在机群坐标系下的位置和姿态：

Num

ID₁,x₁,y₁,z₁,yaw₁,pitch₁,roll₁

…

ID_n,x_n,y_n,z_n,yaw_n,pitch_n,roll_n

例如，对于具有6架无人机的楔形阵型图来讲，可以设定其机群阵型描述数据为：

6

1，0，1，0，0，0，0

2，-1，0，0，0，0，0

3，1，0，0，0，0，0

4，-2，-1，0，0，0，0

5，0，-1，0，0，0，0

6，2，-1，0，0，0，0

对于具有6架无人机的六角环阵图来讲，可以设定其机群阵型描述数据为：

6

1，2，0，0，90，0，0

2，1.41，0.5，0，60，0，0

3，1.41，-0.5，0，-60，0，0

4，-2，0，0，-90，0，0

5，-1.41，-0.5，0，-120，0，0

6，-1.41，0.5，0，120，0，0

具体实施时，在步骤S102中，所述每一架无人机根据所述整个编队的机群路径规划数据和机群阵型描述数据，采用基于斥力-引力模型的群智能算法进行实时的自身的路径规划，如图2所示，包括：

步骤S1021，确定当前帧所述无人机受到的引力Fa的大小及方向及受到的斥力Fr_i的大小及方向；

步骤S1022，将所述引力Fa及斥力Fr_i进行矢量合成，得到合力的大小及方向；

步骤S1023，将所述合力的方向确定为所述无人机当前的目标行进方向，及

步骤S1024，根据所述合力的大小确定无人机的位移量s。

在本实施例的步骤S102中，每架无人机进行实时的在线规划，其任务是在离线规划的基础上，因地制宜地调整本机的局部位态，可使得最终路径实现了路径避障和阵型保持二者的平衡。

无人机进行自身路径规划的方法可以看作是一种群智能算法，它是在只需要局部信息和少量规则就可实现的算法。步骤S101中加载的整个编队的机群路径规划数据和机群阵型描述数据在编队中的无人机之间共享，且每架无人机只负责自身行为，不对其他单元进行控制。

在步骤S1021中，利用基于斥力-引力模型的群智能算法计算每架无人机受到的引力Fa和斥力Fr_i的大小和方向。该算法模型为每架无人机同时面临障碍物(包括友机、敌机、山峰等视为斥力)和目的地(视为引力)的共同作用，斥力和引力的合力方向和大小决定无人机的下一步飞行策略，公式如下：

v∝∑Fr_i+Fa；

其中，Fr_i为所述无人机编队中的无人机受到的斥力，Fa为该无人机受到的引力，v为该无人机的目标速度，包含大小和方向，“∝”为正比符号，表示v的大小正比于Fr_i和Fa的合力的大小。

其实现方法具体如下：

(1)计算无人机所受引力Fa

由前文描述可知，所述整个编队的机群路径规划数据中包括所述预定点在预设飞行时间内每一帧的行进轨迹数据，包括如下变量：x,y,z,yaw,pitch,roll，每一帧对应一具体时间t，所述变量x,y,z为预定点的位置坐标数据，则在所述机群路径规划数据中预定点在时间t的位置坐标数据可表示为FC_offline(X_fc,Y_fc,Z_fc,t)，确定当前帧所述无人机受到的引力Fa的大小及方向，如图3所示，具体步骤包括：

步骤S10211，获取当前帧无人机编队中所有无人机的线上位置坐标数据X_t(x,y,z)；

步骤S10212，根据当前帧的所有无人机的线上位置坐标数据，确定当前帧所有无人机组成的阵型中所述预定点的线上位置坐标数据FC_online(X_fc,Y_fc,Z_fc,t)；

步骤S10213，根据所述机群路径规划数据确定当前帧所述预定点的速度V_FC；

步骤S10214，根据当前帧所述预定点的速度V_FC，利用如下公式确定所述预定点在下一帧的线上预期位置坐标数据FCo_nline(X_fc,Y_fc,Z_fc,t+1)；

步骤S10215，根据所述预定点在下一帧的线上预期位置坐标数据和机群阵型描述数据中无人机相对所述预定点的位置坐标数据，确定下一帧无人机的线上预期位置坐标数据X_t+1(x,y,z)；

步骤S10216，根据所述线上预期位置坐标数据X_t+1(x,y,z)和当前位置坐标数据X_t(x,y,z)确定无人机受到的引力Fa的大小，Fa＝k₁*||X_t+1-X_t||，k₁为比例系数。在本申请实施例中，k₁的取值范围为-0.1～10.0，一般情形下默认为1.0，数值越大，表示无人机的调整速度越快。

步骤S10217，将无人机当前位置指向预期位置的方向确定为无人机受到的引力Fa的方向。

所述线上位置坐标数据即为无人机在执行任务过程中的实际位置坐标数据。在一种具体的实施方式中，所述无人机的位置坐标数据可通过光学动作捕捉系统定位获得，该光学动作捕捉系统包括多个光学动作捕捉设备以及处理器。该多个光学动作捕捉设备布置于无人机的指定运动区域，可以用于从不同方向获取无人机的图像，以使定位系统的处理器基于所述图像对所述无人机进行定位。其中，所述无人机在指定运动区域内正常运动的情况下，可被至少两个光学动作捕捉设备获取到图像。在无人机的表面均设置有至少三个标记点，设置于无人机表面的标记点按不同排列方式排列，无人机的标记点均具有唯一的排列方式。该无人机的表面的至少三个标记点为处理器可对其成像进行识别的标记点。处理器接收多个光学动作捕捉设备获取的包括无人机的标记点的至少两幅图像，并对至少两幅图像中的标记点进行处理，获得标记点的坐标数据。

由以上具体实施步骤可以看出，根据下一帧无人机的预期位置与当前帧的无人机位置计算引力，当前帧的无人机位置可以直接得到，而下一帧的无人机的预期位置可以通过下一帧的预定点位置与机群阵型描述数据中无人机相对所述预定点的位置坐标数据得到。

由于每次预定点位置坐标数据的计算源于无人机编队中所有无人机的当前位置坐标数据，所以无人机位置的改变就会影响到预定点的改变，进而影响了整个编队其他单元轨迹的改变，而这种改变对于离线规划路径来看是微小的和局部的，当无人机受到障碍物的影响而产生了横向的速度，而这些横向的位移，并不会影响编队最后执行到目的地。

在步骤S10214中，当编队中有无人机遇到外部障碍时，处理过程中使用增量式计算预定点位置，增量部分是根据步骤S101中加载的机群路径规划数据确定当前帧的预定点的速度V_FC；而无人机没有遇到障碍物时，预定点位置直接使用机群路径规划数据的坐标赋值。依据得到的下一帧的预定点坐标和机群阵型描述数据中无人机相对所述预定点的位置坐标数据，可求解每架无人机期望存在的位置。

(2)计算无人机所受斥力Fr_i

本发明实施例中，步骤S1021中还确定当前帧所述无人机受到的斥力Fr_i的大小及方向，如图4所示，具体步骤包括：

步骤S10218，获取当前帧距离所述无人机预设半径R范围内的所有障碍物；

步骤S10219，确定所述每一障碍物对所述无人机的斥力的大小及方向，其中，所述每一障碍物对所述无人机的斥力的大小与所述障碍物到所述无人机的距离成反比，方向为障碍物指向所述无人机的方向；

步骤S10210，将所有障碍物对所述无人机的斥力进行矢量合成，得到合成后的斥力合力Fr_i的大小及方向，作为所述无人机当前帧受到的斥力的大小和方向。

由于每个无人机只需考虑周边附近区域的影响因素，所以通过设定一个计算半径R，来决定参与运算斥力合力的因素。例如：对于距离目标过远的斥力点，可以忽略不计。如此设计有效控制了运算量，并且忽略了对于本机低相关性的环境变量。进一步，计算障碍物距离本机的距离d_i，然后比较d_i与计算半径R的关系，对于小于R的障碍物，才进行斥力的计算。

参看图5和图6，分别示意了当计算半径内没有障碍物和出现障碍物时的受力情况。图5中，编号为#1的无人机的计算半径R内没有障碍物出现，无人机的下一步的位置为虚线位置。当计算半径R内出现障碍物(方块)时，参看图6，Fr方向是障碍物与无人机连线方向，Fr大小是距离的反比。Fa大小是当前无人机位置与下一帧无人机位置的连线方向，大小是距离的正比。F是Fr与Fa的矢量合成方向，表示了实际无人机下一帧应去的位置。

在步骤S1024中，所述根据所述合力的大小确定无人机的位移量s，包括：所述无人机的位移量s与所述合力F成正比。

如图7所示，在步骤S1024根据所述合力的大小确定无人机的位移量s后，还可以包括步骤S1025，根据所述无人机的位移量s确定无人机的速度v，Δt为相邻两帧的时间差。

图8为本发明实施例的无人机执行的路线在线规划方法的完整处理流程图。本实施例以所述预定点为阵心为例进行说明。

步骤S801，根据当前帧的所有无人机的位置，计算当前帧的阵心位置FC_online(X_fc,Y_fc,Z_fc,t)。具体实施时，阵心位置通常为编队中的所有无人机的几何中心，即：

步骤S802，根据加载的机群路径规划数据，计算阵心速度V_FC：

V_FC＝FC_offline(X_fc,Y_fc,Z_fc,t+1)-FC_offline(X_fc,Y_fc,Z_fc,t)，FC_offline(X_fc,Y_fc,Z_fc,t)为所述机群路径规划数据中的t时刻的阵心的位置坐标数据，FC_offline(X_fc,Y_fc,Z_fc,t+1)为所述机群路径规划数据中的t+1时刻的阵心的位置坐标数据。

步骤S803，计算下一帧的预期的阵心位置坐标也就是说，当编队中有无人机遇到外部障碍时，处理过程中使用增量式计算预期的阵心位置，增量部分是步骤S402中根据机群路径规划数据计算而得到的阵型速度；而无人机没有遇到障碍物时，阵心位置直接使用机群路径规划数据的坐标赋值。

步骤S804，根据编队的阵心在下一帧的预期的阵心位置坐标FC_online(t+1)和机群阵型描述数据中无人机相对阵心的位置坐标数据，确定下一帧无人机的预期位置坐标数据X_t+1(x,y,z)。即：X_t+1(x,y,z)＝FC_online(t+1)+X_offset(x,y,z)；其中，X_t+1(x,y,z)为无人机在t+1时刻的期望坐标，FC_online(t+1)为t+1时刻的预期的阵心坐标，X_offset为机群阵型描述数据中该无人机相对阵心的位置坐标数据。

步骤S805，根据所述预期坐标X_t+1(x,y,z)和当前位置X_t(x,y,z)计算引力Fa，Fa＝k₁*||X_t+1(x,y,z)-X_t(x,y,z)||，k₁为比例系数。

步骤S806，计算无人机周边的目标距离矩阵D，其元素即当前无人机位置与其他周边物体(无人机或障碍物)的距离d_i＝||X_t-X_i||，其中X_t为无人机的位置坐标，X_i为周边物体的位置坐标。

步骤S807，通过目标距离矩阵D计算斥力矩阵Fr，其元素Fr_i通过d_i计算，k₂是比例系数。

步骤S808，根据计算半径R与目标距离矩阵D，统计在R范围有效的Fr_i，生成有效的斥力合力∑Fr_i；

步骤S809，对有效的斥力合力∑Fr_i和引力Fa进行矢量合成运算生成合力F，F＝∑Fr_i+Fa；

步骤S810，根据合力F，使无人机移动一定位移向量s，位移方向同F方向，位移大小与F成正比：s＝k₃*F，k₃为比例系数；

步骤S811，根据位移相量s计算出无人机的速度向量Δt是前后两帧的时间差；

步骤S812，根据位移s及速度v，发送以所述速度v向所述F方向位移s的控制命令；

步骤S813，重复步骤S801-S812，直到完成离线规划的整个编队的路径轨迹。

其中，步骤S801-805为计算引力的方法步骤，步骤S806-808为计算斥力的方法步骤，但是，本发明并不限制步骤S801-805和步骤S806-808的执行顺序，也可以先计算斥力，再计算引力。

从图8所示的处理方法中可以得出，在线规划方法实际上还采用一个循环迭代的算法，通过不断迭代执行，实现整体寻优效果，但是对于单个无人机，每步执行只关心上一帧和当前帧的位移。

应当注意，尽管在附图中以特定顺序描述了本发明方法的操作，但是，这并非要求或者暗示必须按照该特定顺序来执行这些操作，或是必须执行全部所示的操作才能实现期望的结果。附加地或备选地，可以省略某些步骤，将多个步骤合并为一个步骤执行，和/或将一个步骤分解为多个步骤执行。

在介绍了本发明示例性实施方式的方法之后，接下来，参考图5对本发明示例性实施方式的无人机编队路径自动规划装置进行介绍。该装置的实施可以参见上述方法的实施，重复之处不再赘述。以下所使用的术语“模块”和“单元”，可以是实现预定功能的软件和/或硬件。尽管以下实施例所描述的模块较佳地以软件来实现，但是硬件，或者软件和硬件的组合的实现也是可能并被构想的。

图9为本发明实施例的无人机编队路径自动规划装置的结构示意图，包括：

数据加载模块11，用于加载整个编队的机群路径规划数据和机群阵型描述数据，所述整个编队的机群路径规划数据为：无人机编队作为一个整体，为该整体组成的阵型中的预定点规划的行进轨迹数据；所述机群阵型描述数据至少包含所述编队中无人机数量以及每一架无人机相对所述预定点的位置坐标数据；实时规划模块12，用于根据所述整个编队的机群路径规划数据和机群阵型描述数据，采用基于斥力-引力模型的群智能算法进行实时的自身的路径规划。

在本实施例中，如图10所示，所述实时规划模块12采用基于斥力-引力模型的群智能算法进行实时的自身的路径规划，具体包括：

受力确定模块121，用于确定当前帧所述无人机受到的引力Fa的大小及方向及受到的斥力Fr_i的大小及方向；

受力合成模块122，用于将所述引力Fa及斥力Fr_i进行矢量合成，得到合力的大小及方向；

方向确定模块123，用于将所述合力的方向确定为所述无人机当前的目标行进方向，及

位移量确定模块124，用于根据所述合力的大小确定无人机的位移量s。

在本实施例中，如图11所示，所述受力确定模块121包括引力确定单元1211和斥力确定单元1212，其中，引力确定单元1211用于当前帧所述无人机受到的引力Fa的大小及方向，斥力确定单元1212用于确定当前帧所述无人机受到的引力Fr_i的大小及方向。

如图12所示，所述引力确定单元1211具体包括：

无人机坐标获取单元12111，用于获取当前帧无人机编队中所有无人机的线上位置坐标数据X_t(x,y,z)；

预定点坐标确定单元12112，用于根据当前帧的所有无人机的线上位置坐标数据，确定当前帧所有无人机组成的阵型中所述预定点的线上位置坐标数据FCo_nline(X_fc,Y_fc,Z_fc,t)；

预定点速度确定单元12113，用于根据所述机群路径规划数据确定当前帧所述预定点的速度V_FC；

预定点预期坐标确定单元12114，用于根据当前帧所述预定点的速度V_FC，利用如下公式确定所述预定点在下一帧的线上预期位置坐标数据FCo_nline(X_fc,Y_fc,Z_fc,t+1)；

其中FCo_ffline(X_fc,Y_fc,Z_fc,t+1)为机群路径规划数据中包括的预定点在t+1时刻的位置坐标数据。

无人机预期坐标确定单元12115，用于根据所述预定点在下一帧的线上预期位置坐标数据和机群阵型描述数据中无人机相对所述预定点的位置坐标数据，确定下一帧无人机的线上预期位置坐标数据X_t+1(x,y,z)；

引力大小确定单元12116，用于根据所述线上预期位置坐标数据X_t+1(x,y,z)和当前位置坐标数据X_t(x,y,z)确定无人机受到的引力Fa的大小，Fa＝k₁*||X_t+1(x,y,z)-X_t(x,y,z)||，k₁为比例系数；

引力方向确定单元12117，用于将无人机当前位置指向预期位置的方向确定为无人机受到的引力Fa的方向。

在本实施例中，如图13所示，所述斥力确定单元1212具体包括：

障碍物确定单元12121，用于获取当前帧距离所述无人机预设半径R范围内的所有障碍物；

斥力确定单元12122，用于确定所述每一障碍物对所述无人机的斥力的大小及方向，其中，所述每一障碍物对所述无人机的斥力的大小与所述障碍物到所述无人机的距离成反比，方向为障碍物指向所述无人机的方向；

斥力合成单元12123，用于将所有障碍物对所述无人机的斥力进行矢量合成，得到合成后的斥力合力Fr_i的大小及方向，作为所述无人机当前帧受到的斥力的大小和方向。

在本实施例中，所述位移量确定模块124根据所述合力的大小确定无人机的位移量s具体包括：所述无人机的位移量s与所述合力成正比。

在本实施例中，如图14所示，所述实时规划模块12还包括：

速度确定单元125，用于根据所述无人机的位移量s确定无人机的速度v，Δt为相邻两帧的时间差。

在本实施例中，所述预定点为无人机编队组成的阵型的阵心位置。

此外，尽管在上文详细描述中提及了无人机编队路径自动规划装置的若干单元模块，但是这种划分仅仅并非强制性的。实际上，根据本发明的实施方式，上文描述的两个或更多单元的特征和功能可以在一个单元中具体化。同样，上文描述的一个单元的特征和功能也可以进一步划分为由多个单元来具体化。

以下举例一个三架无人机的楔形阵型下，面对障碍物时候如何利用本发明的基于斥力-引力模型进行实时路径调整的。

首先，根据全局离线规划，得知整个无人机编队由南向北飞行，飞行速度为一个速度单位，在无障碍物时，编队保持既定阵型，如图15所示。每架无人机距离超过计算半径，所以飞行单位只收到Fa的作用，此时Fa即为合力F。

当障碍物进入第一架无人机的计算变径后，如图16所示，预期位置产生的引力Fa和障碍物(方形物体)对1#产生斥力Fr共同作用于1#，其合力F使1#飞向左侧。其他两架仍然保持前行。

通过当前位置计算整个阵心，即三个飞行单元组成的大的三角形的质心(图中十字位置)，在质心位置基础上，向北移动一个单位速度产生的位移，如图17所示的虚线十字坐标，即为下一帧期望阵心位置，而三个虚线三角形，标出了期望阵型下，每架无人机的期望位置。由于上一帧的影响，1#向左飞出一段距离，2#向前飞出一段距离，导致二者在此帧时，距离过近，所以1#在受到位置牵引Fa的同时受到2#的北偏东方向的推力Fr，合力F使其超越了期望位置，而2#因为受到1#的反作用力，并未达到期望位置，而是向西北方向飞去。3#受到期望位置和障碍物斥力，产生了一个向西的合成力。

由于1#和2#的排斥，此次阵型较为分散，计算质心后，阵型往北再次步进同样的距离，如图18所示。1#再次受到障碍物的作用，但这次方向与期望位置方向相近，合力F超过期望位置。2#不受周围斥力影响，只受引力。3#因为障碍物的减速导致了期望位置的拉远，致使这次的Fa比上一轮要增大，而Fa方向因为阵心上轮的调整，也获得了一定的向西移动。使得3#这轮的合力方向向西北为主，而且幅度很大。

在计算阵心和设定固定偏移后，如图19所示，可以看出1#和2#只受Fa影响，而3#因为一直受合成力减缓了前进的位移，使得Fa在增大，而3#更加贴近障碍物也加剧了Fr的产生，所以这次F合力比上一阵更加明显。

在经过上述4帧时间的调整后，如图20所示，可以看出第5帧的时间，已经相互不再影响，整个编队已经远离了障碍物且朝向预期位置移动，实现了避障和保持阵型。

本发明实施例的无人机编队路径自动规划方法及装置，提出的是一种离线规划和在线规划相结合的双层级规划法。此方法具有先粗糙后精细的计算特点，第一层继承了全局规划的优势，第二层解决了传统离线规划法无法对变化场景适应的问题。考虑到无人机群先天具备分布式计算的特点，对在线规划问题进行分布式设计，充分利用了无人机自身运算能力，节省了中心计算资源，为大规模组队创造了可能。并且，实现阵型保持、阵型变换、阵型避障等问题在统一算法框架下的求解，避免了传统方法纷繁的模式设计，更具通用性，且算法复杂度不随编队成员数量的增多而上升。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

本发明中应用了具体实施例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。