CN115903895A - 一种无人机集群的轨迹规划方法、装置和系统 - Google Patents

Abstract

本发明提供了一种无人机集群的轨迹规划方法、装置和系统。方法包括：接收无人机集群的编队队形，通过拉普拉斯矩阵表示编队队形；从拉普拉斯矩阵中获取每个无人机的起点和目标点，基于轨迹规划算法为每个无人机规划一条初始无碰撞路径，对初始无碰撞路径进行轨迹优化，获得优化无碰撞路径，轨迹优化包括平滑度优化、优化前后差异性优化和连续性优化；对每个无人机构建与其他无人机之间的避障目标函数，基于避障目标函数对优化无碰撞路径进行轨迹优化，得到每个无人机的最终无碰撞路径。本发明在考虑外部障碍、无人机之间障碍的情况下，提出基于轨迹优化的无人机集群的轨迹规划方法，根据该方法规划的轨迹与障碍物无碰撞且光滑可用。

Description

一种无人机集群的轨迹规划方法、装置和系统

技术领域

本发明主要涉及飞行器控制领域，尤其涉及一种无人机集群的轨迹规划方法、装置和系统。

背景技术

单架无人机不能满足复杂的任务需求，可以完成的任务非常有限。例如，当面对大范围的搜索任务时，单架无人机很难有效地覆盖整个监视区域；在战斗中执行打击任务时，单架无人机的命中半径和攻击能力等许多方面都有很大的局限性，因此任务的成功率会大大降低，总体来说，单架无人机执行任务时在保障自身安全、大范围搜索探测、战场精准打击、抗干扰和有效载荷等方面具有较大的局限性。

现有技术方案提出了一种基于速度障碍物(Velocity Obstacle,VO)的无人机集群的轨迹规划方法，该方法可以实现多无人机的相互避免碰撞。然而，基于速度障碍物的方法无法保证生成的轨迹的平滑性，这严重影响了无人机轨迹规划的可用性。

发明内容

本发明要解决的技术问题是提供一种无人机集群的轨迹规划方法、装置和系统，解决现有技术方案规划的无人机集群的轨迹可用性低的问题。

为解决上述技术问题，本发明提供了一种无人机集群的轨迹规划方法，包括如下步骤：接收无人机集群的编队队形，通过拉普拉斯矩阵表示所述编队队形；从所述拉普拉斯矩阵中获取每个无人机的起点和目标点，基于轨迹规划算法为每个无人机规划一条初始无碰撞路径，对所述初始无碰撞路径进行轨迹优化，获得优化无碰撞路径，所述轨迹优化包括平滑度优化、优化前后差异性优化和连续性优化；对每个无人机构建与其他无人机之间的避障目标函数，基于所述避障目标函数对所述优化无碰撞路径进行轨迹优化，得到每个无人机的最终无碰撞路径。

可选地，通过拉普拉斯矩阵表示所述编队队形包括：根据所述编队队形构建无向图，其中所述无向图的顶点表示单个无人机，所述无向图的边表示两个无人机之间的几何距离；根据所述无向图的邻接矩阵和度矩阵构建拉普拉斯矩阵。

可选地，基于轨迹规划算法为每个无人机规划一条初始无碰撞路径：基于轨迹规划算法为每个无人机规划一条不考虑障碍物的从所述起点到所述目标点的最优路径；判断所述最优路径是否穿过障碍物，如果是，则通过做射线方法获取障碍物外侧的顶点，连接所述起点、所述顶点和所述目标点构成初始无碰撞路径。

可选地，通过做射线方法获取障碍物外侧的顶点包括：获取所述最优路径穿过障碍物的位置点，在位置点连线的中点作垂直线，从所述中点开始沿所述垂直线的两端方向寻找无障碍物的网格，将所述无障碍物的网格作为所述障碍物外侧的顶点。

可选地，对所述初始无碰撞路径进行轨迹优化包括：构建所述初始无碰撞路径的第一优化问题，对所述第一优化问题进行优化，其中通过如下公式表示所述第一优化问题：

Q＝arg min J＝λ_sJ_s+λ_hJ_h+λ_cJ_c

其中，Q为所述优化问题，J_s为平滑度，J_h为优化前后差异性，J_c为连续性，λ_s,λ_h,λ_c分别为各项的权重。

可选地，所述优化前后差异性为优化轨迹与原始轨迹点之间的平方差的积分。

可选地，所述连续性为所述初始无碰撞路径的路径段之间的加速度的平方差。

可选地，基于所述避障目标函数对所述优化无碰撞路径进行轨迹优化包括：将所述避障目标函数加入所述第一优化问题构成第二优化问题，对所述第二优化问题进行优化。

可选地，对每个无人机构建与其他无人机之间的避障目标函数包括：接收其他无人机在同一时刻的第一轨迹，计算所述优化无碰撞路径与所述第一轨迹的距离差值，根据所述距离差值构建所述避障目标函数。

可选地，所述避障目标函数表示为如下形式：

其中J_f,k为所述避障目标函数，d_k,i(t)为无人机k与其他无人机i之间的距离差值。

为解决上述技术问题，本发明提供了一种无人机集群，无人机集群包括多个无人机，每个无人机包括轨迹规划单元，其中所述轨迹规划单元用于接收无人机集群的编队队形，通过拉普拉斯矩阵表示所述编队队形，从所述拉普拉斯矩阵中获取无人机的起点和目标点，基于轨迹规划算法为所述无人机规划一条初始无碰撞路径，对所述初始无碰撞路径进行轨迹优化，获得优化无碰撞路径，所述轨迹优化包括平滑度优化、优化前后差异性优化和连续性优化，对所述无人机构建与其他无人机之间的避障目标函数，基于所述避障目标函数对所述优化无碰撞路径进行轨迹优化，得到所述无人机的最终无碰撞路径。

为解决上述技术问题，本发明提供了一种无人机集群的轨迹规划系统，包括：无人机集群；广播单元，用于接收无人机的最终无碰撞路径并将所述最终无碰撞路径作为第一轨迹广播发送给其他无人机；顺序单元，用于设置无人机集群内各个无人机启动轨迹规划的顺序。

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

本发明的人机集群的轨迹规划方法首先考虑了无人机集群外部障碍物，为每个无人机规划一条初始无碰撞路径，然后对初始无碰撞路径进行轨迹优化获得优化无碰撞路径，其中轨迹优化包括平滑度优化、优化前后差异性优化和连续性优化，之后考虑了无人机之间的障碍，通过避障目标函数对优化无碰撞路径进行调整得到最终无碰撞路径，根据该方法规划的无人机集群的轨迹与障碍物无碰撞且光滑可用。

附图说明

包括附图是为提供对本申请进一步的理解，它们被收录并构成本申请的一部分，附图示出了本申请的实施例，并与本说明书一起起到解释本发明原理的作用。附图中：

图1是根据本发明一实施例的无人机集群的轨迹规划方法的流程图。

图2是根据本发明一实施例的初始无碰撞路径生成示意图。

图3是根据本发明一实施例的仿真编队飞行中规划路径图。

图4是根据本发明一实施例的无人机集群的系统框图。

图5是根据本发明一实施例的无人机集群的轨迹规划系统的框图。

具体实施方式

为了更清楚地说明本申请的实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单的介绍。显而易见地，下面描述中的附图仅仅是本申请的一些示例或实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图将本申请应用于其他类似情景。除非从语言环境中显而易见或另做说明，图中相同标号代表相同结构或操作。

如本申请和权利要求书中所示，除非上下文明确提示例外情形，“一”、“一个”、“一种”和/或“该”等词并非特指单数，也可包括复数。一般说来，术语“包括”与“包含”仅提示包括已明确标识的步骤和元素，而这些步骤和元素不构成一个排它性的罗列，方法或者设备也可能包含其他的步骤或元素。

除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本申请的范围。同时，应当明白，为了便于描述，附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为授权说明书的一部分。在这里示出和讨论的所有示例中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它示例可以具有不同的值。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。

此外，需要说明的是，使用“第一”、“第二”等词语来限定零部件，仅仅是为了便于对相应零部件进行区别，如没有另行声明，上述词语并没有特殊含义，因此不能理解为对本申请保护范围的限制。此外，尽管本申请中所使用的术语是从公知公用的术语中选择的，但是本申请说明书中所提及的一些术语可能是申请人按他或她的判断来选择的，其详细含义在本文的描述的相关部分中说明。此外，要求不仅仅通过所使用的实际术语，而是还要通过每个术语所蕴含的意义来理解本申请。

本申请中使用了流程图用来说明根据本申请的实施例的系统所执行的操作。应当理解的是，前面或下面操作不一定按照顺序来精确地执行。相反，可以按照倒序或同时处理各种步骤。同时，或将其他操作添加到这些过程中，或从这些过程移除某一步或数步操作。

正如背景技术提到的现有的基于速度障碍物的方法无法保证生成的轨迹的平滑性，这严重影响了无人机轨迹规划的可用性。本发明在考虑无人机集群外部障碍、无人机之间障碍的情况下，提出基于轨迹优化的无人机集群的轨迹规划方法，根据该方法规划的轨迹与障碍物无碰撞且光滑可用。

图1是本发明一实施例的无人机集群的轨迹规划方法的流程图。如图1所示，无人机集群的轨迹规划方法100包括如下步骤：

步骤S11：接收无人机集群的编队队形，通过拉普拉斯矩阵表示编队队形。

无人机集群的编队队形可以是几何形状，例如三角形、梯形等，也可以汉字或英文字母等，本申请对无人机集群的编队队形不作限制。通过拉普拉斯矩阵表示编队队形包括：

根据编队队形构建无向图，其中无向图的顶点表示单个无人机，无向图的边表示两个无人机之间的几何距离。以无向图G＝(v,ε)表示N个无人机的编队队形为例，其中v是顶点集，ε是边的集合。在无向图G中，顶点i表示第i个无人机并且其位置为P_i＝[x_i,y_i,z_i]∈R³，边e_ij表示顶点i和顶点j之间的连接，其含义是无人机i和j彼此之间几何距离，其中e_ij∈ε。

在本发明中，每个无人机与所有其他无人机通信，因此形成无向图G是完整的。无向图G的每一条边表示两个无人机之间的几何距离，其可以通过一个非负数作为权重来表示，例如，边e_ij的权重公式为w_ij＝||p_i-p_j||²,(i,j)∈ε，||·||代表欧几里得范数图。至此，无向图G的邻接矩阵A和度矩阵D都已经被定义，由此得出拉普拉斯矩阵为L＝D-A。在一些实施例中，计算中多使用正则化拉普拉斯矩阵，进而得到正则化的拉普拉斯矩阵

作为一种图表示矩阵，拉普拉斯矩阵包含关于图结构的信息，以此来作为编队队形的期望。

步骤S12：从拉普拉斯矩阵中获取每个无人机的起点和目标点，基于轨迹规划算法为每个无人机规划一条初始无碰撞路径。

基于轨迹规划算法为每个无人机规划一条初始无碰撞路径包括：

(1)基于轨迹规划算法为每个无人机规划一条不考虑障碍物的从起点到目标点的最优路径。轨迹规划算法可以是RRT*算法，RRT*是快速随机探索树算法(Rapidly-exploring Random Tree，RRT)的一种改进型。在不考虑障碍物的情况下，利用RRT*算法寻找一条起点到目标点的最优路径。图2是本发明一实施例的初始无碰撞路径生成示意图。如图2所示，图中灰色形状为障碍物，其覆盖的灰色网格为有障碍物网格。穿过障碍物的曲线21即为RRT*搜索出来的从起点到目标点的最优路径。

(2)判断最优路径是否穿过障碍物，如果是，则通过做射线方法获取障碍物外侧的顶点，连接起点、顶点和目标点构成初始无碰撞路径。通过做射线方法获取障碍物外侧的顶点包括：获取最优路径穿过障碍物的位置点，在位置点连线的中点作垂直线，从中点开始沿垂直线的两端方向寻找无障碍物的网格，将无障碍物的网格作为障碍物外侧的顶点。如图2中所示，曲线21即为RRT*搜索出来的最优路径，沿着曲线21从起点到目标点搜索，记录曲线21穿入与穿出各个障碍物的位置。其中穿入位置记为A，穿出位置记为B。连接A点和B点形成穿越线，在各穿越线的各个中点作垂直线L1，从各个中点开始沿各个垂直线L1的两端方向寻找无障碍物的网格。当两端找到处于无障碍的网格时追踪停止，停止处的网格作为顶点C。连接曲线21所代表的最优路径的起点、各个顶点C和最优路径的目标点构成无碰撞路径。如图2中所示，无碰撞路径包括无碰撞路径22和无碰撞路径23。计算每条无碰撞路径的从起点到目标点的代价，选择代价最小的无碰撞路径作为初始无碰撞路径。

步骤S13：对初始无碰撞路径进行轨迹优化，获得优化无碰撞路径，轨迹优化包括平滑度优化、优化前后差异性优化和连续性优化。

对初始无碰撞路径进行轨迹优化包括：考虑路径的平滑度、优化前后差异性以及连续性三部分，构建初始无碰撞路径的第一优化问题，对第一优化问题进行优化，其中通过如下公式表示第一优化问题：

Q＝arg min J＝λ_sJ_s+λ_hJ_h+λ_cJ_c

其中，Q为优化问题，J_s为平滑度，J_h为优化前后差异性，J_c为连续性，λ_s,λ_h,λ_c分别为各项的权重。

进行轨迹优化之前需要对问题进行建模，对于{x,y,z}的每个维度，考虑一个m段、n阶多项式轨迹p_m(t)＝c₀+c₁t+c₂t²+c_ntⁿ，令c_m＝[c₀；c₁；c₂；···；c_n]^T为第m段的系数，最终目标是为轨迹的每一段找到最优系数。如图2所示，初始无碰撞路径22包括路径段221、路径段222、路径段223和路径段224。轨迹优化最终目标是为路径段221、路径段222、路径段223和路径段224找到最优系数。

针对平滑度优化，通过使曲线的导数最小化即可实现轨迹光滑的目的，在一些实施例中，平滑度J_s由轨迹的导数平方的积分构成：

其中T＝t₁+t₂+···+t_m为m段轨迹的总时间，p_k(t)为优化轨迹，(j)为j阶导数，

进一步的将轨迹多项式带入其中，可得到：

其中t_i为每一段轨迹的时间，t^(j)是t的j阶导数向量，

是m段的系数向量。

针对优化前后差异性优化，在一些实施例中，优化前后差异性J_h为优化轨迹与原始轨迹点之间的平方差的积分：

其中T为m段轨迹的总时间，

为原始轨迹，p_k(t)为优化轨迹。

针对连续性优化，轨迹在每一段内加速度是连续的，但是处于段与段连接处的加速度是跳变的，这对飞行器的控制是不利的。在一些实施例中，连续性J_c为初始无碰撞路径的路径段之间的加速度的平方差：

其中

是第i段的终端加速度，

是第i+1段的开始加速度，两者都在k维中。

针对单个无人机，经过轨迹优化后，优化的轨迹与外界障碍物是无碰撞的且轨迹是光滑可用的。但是，优化的轨迹未考虑无人机之间的障碍，可能存在无人机之间相互碰撞。对此，可以建立一个广播网络，一旦一个飞行器生成一个新的优化轨迹，它就会立即广播给所有其他飞行器，然后其他飞行器接收并存储此轨迹，用于在必要时为自己生成安全轨迹。

步骤S14：对每个无人机构建与其他无人机之间的避障目标函数，基于避障目标函数对优化无碰撞路径进行轨迹优化，得到每个无人机的最终无碰撞路径。

对每个无人机构建与其他无人机之间的避障目标函数包括：接收其他无人机在同一时刻的第一轨迹，计算优化无碰撞路径与第一轨迹的距离差值，根据距离差值构建避障目标函数。

避障目标函数表示为如下形式：

其中J_f,k为避障目标函数，d_k,i(t)为无人机k与其他无人机i之间的距离差值。

基于避障目标函数对优化无碰撞路径进行轨迹优化包括：将避障目标函数加入第一优化问题构成第二优化问题，对第二优化问题进行优化，得到每个无人机的最终无碰撞路径。每个飞行器在接收到第一轨迹时立即计算距离差值，根据距离差值判断是否存在潜在碰撞，如果检测到潜在碰撞，则将避障目标函数加入第一优化问题构成第二优化问题，对第二优化问题进行优化，从而对优化的轨迹进行调整，生成最终无碰撞路径。

在一些实施例中，无人机集群的轨迹规划方法还考虑了计算复杂度随飞行器数量的增加而增加的问题。在规划之前，每个飞行器将其当前位置与接收到的周围飞行器的第一轨迹进行比较，其中任何超出规划范围的第一轨迹都将被忽略，从而降低计算复杂度。

在一些实施例中，无人机集群的轨迹规划方法还包括在开始为无人机集群的轨迹规划时，设置无人机集群内各个无人机启动轨迹规划的顺序。这样避免了启动时由于同步轨迹生成造成的混乱，因为此时无人机没有来自其他无人机的第一轨迹信息。

为了进一步说明本发明的工作原理和技术效果，下面提供了采用本发明的无人机集群编队仿真图予以说明。图3是本发明一实施例的仿真编队飞行中规划路径图。如图3所示，无人机31、无人机32、无人机33呈三角形队形向着目标点飞行，前面遇到无法正常通行的走廊O，线条S1为无人机31规划的最终无碰撞路径，线条S2为无人机32规划的最终无碰撞路径，线条S3为无人机33规划的最终无碰撞路径。可见每条线条与障碍物无碰撞且光滑可行，充分体现了本发明方法的稳定性与良好的性能。

本发明的人机集群的轨迹规划方法首先考虑了无人机集群外部障碍物，为每个无人机规划一条初始无碰撞路径，然后对初始无碰撞路径进行轨迹优化获得优化无碰撞路径，之后考虑了无人机之间的障碍，通过避障目标函数对优化无碰撞路径进行调整得到最终无碰撞路径，根据该方法规划的无人机集群的轨迹与障碍物无碰撞且光滑可用。

图4是根据本发明一实施例的无人机集群的系统框图。如图4所示，该无人机集群400包括无人机41、无人机42和无人机43。无人机41包括轨迹规划单元410，无人机42包括轨迹规划单元420，无人机43包括轨迹规划单元430。其中轨迹规划单元410用于接收无人机集群400的编队队形，通过拉普拉斯矩阵表示编队队形，从拉普拉斯矩阵中获取无人机41的起点和目标点，基于轨迹规划算法为无人机41规划一条初始无碰撞路径，对初始无碰撞路径进行轨迹优化获得优化无碰撞路径，轨迹优化包括平滑度优化、优化前后差异性优化和连续性优化，对无人机41构建与无人机42和无人机43之间的避障目标函数，基于避障目标函数对优化无碰撞路径进行轨迹优化，得到无人机41的最终无碰撞路径。轨迹规划单元420和轨迹规划单元430的说明可参考轨迹规划单元410的说明，在此不再赘述。

图5是根据本发明一实施例的无人机集群的轨迹规划系统的框图。如图5所示，无人机集群的轨迹规划系统500包括：无人机集群51、广播单元52和顺序单元53。其中无人机集群51的说明可参考前文的无人机集群400。广播单元52用于接收无人机的最终无碰撞路径并将最终无碰撞路径作为第一轨迹广播发送给其他无人机。顺序单元53用于设置无人机集群内各个无人机启动轨迹规划的顺序。在一些实施例中，顺序单元53为链式网络。在系统启动时，无人机以一个预定义的顺序生成轨迹。每个无人机生成自身最初的轨迹在收到通过更高的优先级的链式网络发来的其他无人机的第一轨迹之后。这样避免了在系统启动时由于同步轨迹生成造成的混乱，因为此时无人机没有来自其他无人机的第一轨迹信息。

上文已对基本概念做了描述，显然，对于本领域技术人员来说，上述发明披露仅仅作为示例，而并不构成对本申请的限定。虽然此处并没有明确说明，本领域技术人员可能会对本申请进行各种修改、改进和修正。该类修改、改进和修正在本申请中被建议，所以该类修改、改进、修正仍属于本申请示范实施例的精神和范围。

同时，本申请使用了特定词语来描述本申请的实施例。如“一个实施例”、“一实施例”、和/或“一些实施例”意指与本申请至少一个实施例相关的某一特征、结构或特点。因此，应强调并注意的是，本说明书中在不同位置两次或多次提及的“一实施例”或“一个实施例”或“一替代性实施例”并不一定是指同一实施例。此外，本申请的一个或多个实施例中的某些特征、结构或特点可以进行适当的组合。

本申请的一些方面可以完全由硬件执行、可以完全由软件(包括固件、常驻软件、微码等)执行、也可以由硬件和软件组合执行。以上硬件或软件均可被称为“数据块”、“模块”、“引擎”、“单元”、“组件”或“系统”。处理器可以是一个或多个专用集成电路(ASIC)、数字信号处理器(DSP)、数字信号处理器件(DAPD)、可编程逻辑器件(PLD)、现场可编程门阵列(FPGA)、处理器、控制器、微控制器、微处理器或者其组合。此外，本申请的各方面可能表现为位于一个或多个计算机可读介质中的计算机产品，该产品包括计算机可读程序编码。例如，计算机可读介质可包括，但不限于，磁性存储设备(例如，硬盘、软盘、磁带……)、光盘(例如，压缩盘CD、数字多功能盘DVD……)、智能卡以及闪存设备(例如，卡、棒、键驱动器……)。

同理，应当注意的是，为了简化本申请披露的表述，从而帮助对一个或多个发明实施例的理解，前文对本申请实施例的描述中，有时会将多种特征归并至一个实施例、附图或对其的描述中。但是，这种披露方法并不意味着本申请对象所需要的特征比权利要求中提及的特征多。实际上，实施例的特征要少于上述披露的单个实施例的全部特征。

虽然本申请已参照当前的具体实施例来描述，但是本技术领域中的普通技术人员应当认识到，以上的实施例仅是用来说明本申请，在没有脱离本申请精神的情况下还可作出各种等效的变化或替换，因此，只要在本申请的实质精神范围内对上述实施例的变化、变型都将落在本申请的权利要求书的范围内。

Claims (12)

1.一种无人机集群的轨迹规划方法，其特征在于，包括如下步骤：

接收无人机集群的编队队形，通过拉普拉斯矩阵表示所述编队队形；

从所述拉普拉斯矩阵中获取每个无人机的起点和目标点，基于轨迹规划算法为每个无人机规划一条初始无碰撞路径；

对所述初始无碰撞路径进行轨迹优化，获得优化无碰撞路径，所述轨迹优化包括平滑度优化、优化前后差异性优化和连续性优化；

对每个无人机构建与其他无人机之间的避障目标函数，基于所述避障目标函数对所述优化无碰撞路径进行轨迹优化，得到每个无人机的最终无碰撞路径。

2.如权利要求1所述的方法，其特征在于，通过拉普拉斯矩阵表示所述编队队形包括：

根据所述编队队形构建无向图，其中所述无向图的顶点表示单个无人机，所述无向图的边表示两个无人机之间的几何距离；

根据所述无向图的邻接矩阵和度矩阵构建拉普拉斯矩阵。

3.如权利要求1所述的方法，其特征在于，基于轨迹规划算法为每个无人机规划一条初始无碰撞路径：

基于轨迹规划算法为每个无人机规划一条不考虑障碍物的从所述起点到所述目标点的最优路径；

判断所述最优路径是否穿过障碍物，如果是，则通过做射线方法获取障碍物外侧的顶点，连接所述起点、所述顶点和所述目标点构成初始无碰撞路径。

4.如权利要求3所述的方法，其特征在于，通过做射线方法获取障碍物外侧的顶点包括：获取所述最优路径穿过障碍物的位置点，在位置点连线的中点作垂直线，从所述中点开始沿所述垂直线的两端方向寻找无障碍物的网格，将所述无障碍物的网格作为所述障碍物外侧的顶点。

5.如权利要求1所述的方法，其特征在于，对所述初始无碰撞路径进行轨迹优化包括：

构建所述初始无碰撞路径的第一优化问题，对所述第一优化问题进行优化，其中通过如下公式表示所述第一优化问题：

Q＝argminJ＝λ_sJ_s+λ_hJ_h+λ_cJ_c

其中，Q为所述优化问题，J_s为平滑度，J_h为优化前后差异性，J_c为连续性，λ_s,λ_h,λ_c分别为各项的权重。

6.如权利要求5所述的方法，其特征在于，所述优化前后差异性为优化轨迹与原始轨迹点之间的平方差的积分。

7.如权利要求5所述的方法，其特征在于，所述连续性为所述初始无碰撞路径的路径段之间的加速度的平方差。

8.如权利要求5所述的方法，其特征在于，基于所述避障目标函数对所述优化无碰撞路径进行轨迹优化包括：将所述避障目标函数加入所述第一优化问题构成第二优化问题，对所述第二优化问题进行优化。

9.如权利要求1所述的方法，其特征在于，对每个无人机构建与其他无人机之间的避障目标函数包括：接收其他无人机在同一时刻的第一轨迹，计算所述优化无碰撞路径与所述第一轨迹的距离差值，根据所述距离差值构建所述避障目标函数。

10.如权利要求8所述的方法，其特征在于，所述避障目标函数表示为如下形式：

其中J_f,k为所述避障目标函数，d_k,i(t)为无人机k与其他无人机i之间的距离差值。

11.一种无人机集群，包括多个无人机，其特征在于，每个无人机包括轨迹规划单元，其中所述轨迹规划单元用于接收无人机集群的编队队形，通过拉普拉斯矩阵表示所述编队队形，从所述拉普拉斯矩阵中获取无人机的起点和目标点，基于轨迹规划算法为所述无人机规划一条初始无碰撞路径，对所述初始无碰撞路径进行轨迹优化，获得优化无碰撞路径，所述轨迹优化包括平滑度优化、优化前后差异性优化和连续性优化，对所述无人机构建与其他无人机之间的避障目标函数，基于所述避障目标函数对所述优化无碰撞路径进行轨迹优化，得到所述无人机的最终无碰撞路径。

12.一种无人机集群的轨迹规划系统，其特征在于，包括：

如权利要求11所述的无人机集群；

广播单元，用于接收无人机的最终无碰撞路径并将所述最终无碰撞路径作为第一轨迹广播发送给其他无人机；

顺序单元，用于设置无人机集群内各个无人机启动轨迹规划的顺序。

Images