CN113985922B - 一种多目标约束下的无人机分级式路径规划方法 - Google Patents

Abstract

本发明属于无人机技术领域，具体涉及一种多目标约束下的无人机分级式路径规划方法，该方法包括以下步骤：首先，根据无人机起止点进行坐标系转换；其次，针对不同场景建立多目标约束函数，采用天牛须搜索算法进行初次路径规划得到一级路径；第三，以一级路径为中心线，向中心线周围空间扩展，形成无人机飞行走廊；第四，在飞行走廊内重新设计多目标约束函数，采用天牛须搜索算法、粒子群优化算法进行二次联合路径规划得到二级路径；第五，将二级路径点转换为原始坐标系下的坐标集合，平滑处理后供无人机跟随运行。本发明优点在于分级式的规划体系提高规划效率；分别设计多目标约束函数使规划结果真实可靠；提出了一种联合寻优算法，路径性能更优。

Description

一种多目标约束下的无人机分级式路径规划方法

技术领域

本发明属于无人机技术领域，具体涉及一种多目标约束下的无人机分级式路径规划方法。

背景技术

无人机技术突飞猛进，在军民各领域取得了广泛应用，如侦察、救灾、航拍、植保等；然而如何提高无人机运行过程中的自主性，成为限制其发展的关键因素，尤其是无人机路径规划方法——如何能够使无人机高效避开空域障碍物，同时使得无人机飞行路径代价最低、保证无人机做到跟随运行，成为一个重点问题。

所谓无人机路径规划，就是在指定空域内，从某起始点到某终点生成一条适合无人机自身特性的运行路径，对生成的运行路径一般有如下要求：首先，准确避开环境内各种已知的障碍物，包括地形障碍物等，为了不与障碍物发生碰撞，通常可将障碍物视为危险区域；其次，运行路径代价最小，比如运行路径长度最短等，运行路径短，则能节省能耗，进而延长无人机运行寿命，同时减少暴露在危险环境中的时间；第三，运行路径适合无人机自身特性，在无人机性能范围内能够跟随运行，比如转弯角度、爬升角度、下降角度等均在无人机自身性能范围以内。

总结现有的无人机路径规划方法，大致可分为三类：第一类是基于距离的规划方法，该方法通常将无人机运行路径长度作为唯一目标，保证生成的运行路径距离最短，代表算法有Dijkstra算法、A*算法、ARA*算法、D*算法等，此类算法通过反复运行能够遍历所有可能的邻近节点，找到满足运行路径总长度最短的最优节点，并将这些节点作为无人机最终的路径点保存，由于此类算法通常只考虑了距离问题，往往忽略了无人机自身性能因素的作用效果，使得生成的路径不适合无人机运行，实用性较差，而且算法遍历的节点越多，那么算法执行时间越长，导致搜索效率较低；第二类是基于图论的规划方法，该方法通过典型的图形理论构造无人机运行路径，将无人机运行路径分解为若干个路径段，通过在规划空间内设定评价准则，根据不同路径段的优劣，选取最优值而使得最终运行路径最优，代表算法有Voronoi图算法、PRM算法、可视图算法、概率图算法等，由于此类算法是在理想的空间内寻找最优路径段，对于飞行环境中潜在的障碍物或随机危险不做考虑，不适用于复杂的低空环境，另外，由于受限于搜索维度，不能将无人机运行高度、转弯角度等实际参数纳入模型考虑范围，此类方法规划的无人机运行路径脱离实际，无人机无法跟随运行；第三类是启发式智能规划方法，代表算法有天牛须搜索算法(BAS)、粒子群优化算法(PSO)、遗传算法(GA)、蚁群算法(ACO)、人工势场法(APF)等，此类算法模型简单且一般不需要复杂的参数设定，能够在一定范围内找到无人机运行路径最优解，相比另外两类传统算法，不仅搜索效率高，而且所得到的最优解精度高，非常适用于无人机路径规划问题，然而，此类算法也存在一些亟需解决的问题，比如复杂情况下易陷入局部最优，迭代前期寻优效率低，一旦超出最优解范围，迭代后期对最优解的求解精度会降低，因此，需要对此类算法做针对性的改进。

发明内容

针对现有技术的缺陷及存在的不足，本发明的目的是提供一种多目标约束下的无人机分级式路径规划方法，该方法可同时满足无人机二维或三维条件下的高精度路径规划要求，有效克服现有技术中不适于无人机跟随运行导致的实用性较差、无人机运行路径最优解搜索效率低甚至无法定位最优解、复杂场景下易陷入局部最优等问题。

为了实现以上发明目的，本发明采取的技术方案如下：

一种多目标约束下的无人机分级式路径规划方法，包括以下步骤：

S1、根据给定无人机起始点S以及目的地点D进行坐标系转换，将转换后的坐标系定义为新坐标系，将转换前的坐标系定义为原始坐标系，转换规则为：无人机起始点S作为新坐标系的原点，无人机起始点S以及目的地点D的连线作为新坐标系的X轴，/>表示由起始点S指向目的地点D的矢量，在原始坐标系所在二维平面内过起始点S做/>的垂线作为新坐标系的Y轴，过起始点S做原始坐标系所在二维平面的垂线作为新坐标系的Z轴；

S2、在S1所述新坐标系下，无人机二维路径规划问题，以无人机飞行路径最短、距离危险区域边界直线距离最远为优化目标，将无人机运行环境内的障碍物定义为危险区域，二维路径规划问题中危险区域是圆形区域，采用天牛须搜索算法进行初次路径规划，形成无人机一级路径；无人机三维路径规划问题，以无人机飞行路径最短、距离危险区域边界直线距离最远、无人机飞行高度最低为优化目标，将无人机运行环境内的障碍物定义为危险区域，三维路径规划问题中危险区域是圆柱体区域，采用天牛须搜索算法进行初次路径规划，形成无人机一级路径；

S3、在S1所述新坐标系下，以S2形成的无人机一级路径为中心线，无人机二维路径规划问题，以垂直距离d向中心线两侧平面扩展，形成不进入危险区域的无人机二维飞行走廊；无人机三维路径规划问题，以垂直距离d为半径向中心线周围空间扩展，形成不进入危险区域的无人机三维飞行走廊；

S4、在S3形成的无人机飞行走廊内，二维路径规划问题，以无人机飞行路径最短、距离危险区域边界直线距离最远、转弯角度最小为优化目标，采用天牛须搜索算法、粒子群优化算法进行二次联合路径规划，形成无人机二级路径；三维路径规划问题，以无人机飞行路径最短、距离危险区域边界直线距离最远、转弯角度最小、升降角度最小、无人机飞行高度最低为优化目标，采用天牛须搜索算法、粒子群优化算法进行二次联合路径规划，形成无人机二级路径；

S5、将S4形成的无人机二级路径作为最优路径并进行坐标系转换，转换规则为：将新坐标系下得到的无人机二级路径点坐标转换为原始坐标系下的路径点坐标，经平滑处理后，上传至无人机机载飞控模块后供无人机使用。

进一步的，步骤S2包括以下分步骤：

S21、设定所要规划无人机运行路径上的路径点数量为N_dim，其中N_dim也是天牛须搜索算法中搜索空间的维度，随机生成无人机每个路径点的初始坐标并将所有初始路径点表示为路径点集合WP＝{WP_m}，其中m＝1,2,…,N_dim，路径点集合WP即为天牛的位置坐标；分别计算以下优化目标：无人机飞行路径初始长度J₁、所有路径点距离每个危险区域边界初始最短直线距离总和的倒数J₂、无人机初始路径点飞行高度总和J₃；二维路径规划问题中J₃＝0；得到目标函数其中，J为路径代价值，并将min(J)作为最终优化目标；存储路径代价值J及路径点集合WP；

S22、开始迭代，根据公式A_L＝WP+Dir·d₀计算天牛左须在当前代数对应的位置坐标、根据公式A_R＝WP-Dir·d₀计算天牛右须在当前代数对应的位置坐标，公式A_L＝WP+Dir·d₀和A_R＝WP-Dir·d₀中，WP表示路径点集合，Dir表示天牛右须指向左须的随机单位向量，d₀表示天牛两须之间的距离；根据公式计算天牛左须对应的左须代价值、根据公式/>计算天牛右须对应的右须代价值；采用公式WP＝WP-Step·Dir·Sign(VA_L-VA_R)，更新WP后，将更新后的路径点坐标重新存储在WP中，从而更新天牛的位置坐标，其中Step表示天牛每次运动的可变步长，Dir表示天牛右须指向左须的随机单位向量，Sign()表示符号函数，d₀表示天牛两须之间的距离；

S23、根据更新后的天牛位置坐标重新计算无人机路径代价值J，与已保存的路径代价值J比对，存储最小的路径代价值J及最小的路径代价值J对应的路径点集合WP；判断是否达到指定的迭代次数，结果为否，返回步骤S22；结果为是，执行步骤S24；

S24、迭代完成，将最后存储的路径点集合WP作为无人机一级路径的路径点集合。

进一步的，步骤S4包括以下分步骤：

S41、在S3形成的无人机飞行走廊内，采用粒子群优化算法，随机生成粒子维度为N_dim、粒子数量为N_P的初始粒子群，第i个粒子的位置代表飞行走廊内可行的无人机路径点集合WP′_i，表示为路径点集合WP′_i的维度为N_dim，i＝1,2,…,N_P；对于每一个粒子，分别计算以下优化目标：无人机飞行路径初始长度J′₁、所有路径点距离每个危险区域边界初始最短直线距离总和的倒数J′₂、无人机初始路径点飞行高度总和J′₃、无人机相邻路径点间的转弯角度总和J′₄、无人机相邻路径点间的升降角度总和J′₅；二维路径规划问题中J′₃＝0、J′₅＝0；得到目标函数/>其中，J'为粒子的路径代价值，并将min(J')作为最终优化目标；存储每个粒子的路径代价值J'及路径点集合WP′_i；以上述N_P个路径代价值中的最小值为最小路径代价值J′_min、以最小路径代价值J'_min对应的路径点集合为WP'，存储最小路径代价值J'_min及路径点集合WP'；

S42、开始迭代，随机选取N_P/2个粒子并将其中任意粒子视为一个天牛个体，并对每个天牛个体采用天牛须搜索算法，根据公式A'_L＝WP′_i+Dir'·d'₀计算天牛左须在当前代数对应的位置坐标、根据公式A'_R＝WP′_i-Dir'·d'₀计算天牛右须在当前代数对应的位置坐标，公式A'_L＝WP′_i+Dir'·d'₀和A'_R＝WP′_i-Dir'·d'₀中，WP′_i表示第i个粒子的路径点集合，Dir'表示第i个粒子的天牛右须指向左须的随机单位向量，d'₀表示第i个粒子的天牛两须之间的距离；根据公式计算天牛左须对应的左须代价值、根据公式计算天牛右须对应的右须代价值；采用公式WP′_i＝WP′_i-Step'·Dir'·Sign(VA'_L-VA'_R)，更新WP′_i后，将更新后的路径点坐标重新存储在WP′_i中，从而更新每个天牛的位置坐标，即更新每个粒子的位置坐标，其中Step'表示第i个粒子的天牛每次运动的可变步长，Dir'表示第i个粒子的天牛右须指向左须的随机单位向量，Sign()表示符号函数；对于剩余N_P/2个粒子，采用粒子群优化算法，对于剩余N_P/2个粒子中的任意粒子，采用公式/>更新第i个粒子在t+1代的速度，采用公式更新第i个粒子在t+1代的位置，即更新第i个粒子在t+1代可行的无人机路径点集合，公式/>和/>中，α和β分别表示第i个粒子运动速度的惯性系数以及第i个粒子所处位置的惯性系数，c₁和c₂分别表示一个随机数，P_i ^L和P_i ^G分别表示当前代数t内第i个粒子的局部最优路径代价值与全局最优路径代价值，WP_i'^t与WP_i'^(t+1)分别表示第i个粒子在当前第t代和第t+1代的位置坐标，即第i个粒子在当前第t代和第t+1代可行的无人机路径点集合；

S43、根据更新后的全部N_P个粒子位置坐标重新计算每个粒子对应的无人机路径代价值J'，与已保存的最小路径代价值J'_min比对，以该N_P+1个路径代价值中的最小值更新最小路径代价值J'_min，一并存储更新后的最小路径代价值J'_min对应的路径点集合WP'；判断是否达到指定的迭代次数，结果为否，返回步骤S42；结果为是，执行步骤S44；

S44、迭代完成，将最后存储的路径点集合WP'作为无人机二级路径的路径点集合。

与现有技术相比，本发明具有以下有益效果：

1.通过建立分级式的路径规划体系，首先为无人机规划出一级路径，进而缩小了路径点的寻优空间，在一级路径的基础上找到二级路径并作为最优路径，不仅提高了规划效率，而且提升了无人机运行路径最优解的求解精度。

2.在一级路径、二级路径规划中，考虑了无人机飞行路径最短、距离危险区域边界直线距离最远、转弯角度最小、升降角度最小、飞行高度最低等多个优化目标，形成了多目标约束下的优化函数，符合无人机实际运行要求，使得规划结果更加真实、可靠。

3.在二级路径规划中，采用了天牛须搜索算法与粒子群优化算法结合的联合路径规划，不仅克服了天牛须搜索算法寻优方向单一、寻优步长有限的不足，而且有效解决了粒子群优化算法寻优效率低、易陷入局部最优的缺陷，规划出的路径更优。

附图说明

图1是本发明一种多目标约束下的无人机分级式路径规划方法流程图

图2是图1中步骤S1的二维条件下的坐标系转换示意图；

图3是图1中步骤S3的二维飞行走廊构建示意图；

图4为本发明实施例中规划得到的二维路径。

具体实施方式

为了使本发明的目的、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明做进一步详细说明，应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

实施例：如图1所示，在本发明实施例中，提供了一种多目标约束下的无人机分级式路径规划方法，包括如下步骤：

S1、根据给定无人机起始点S以及目的地点D进行坐标系转换，将转换后的坐标系定义为新坐标系，将转换前的坐标系定义为原始坐标系，转换规则为：无人机起始点S作为新坐标系的原点，无人机起始点S以及目的地点D的连线作为新坐标系的X轴，/>表示由起始点S指向目的地点D的矢量，在原始坐标系所在二维平面内过起始点S做/>的垂线作为新坐标系的Y轴，过起始点S做原始坐标系所在二维平面的垂线作为新坐标系的Z轴；如图2所示，为二维条件下的坐标系转换示意图。

具体的，如图2所示，起始点S以及目的地点D在新坐标系下的坐标分别为(0,0)、(|SD|,0)，|SD|表示起始点S与目的地点D间的距离；X’为新坐标系的X轴，Y’为新坐标系的Y轴。

S2、在S1所述新坐标系下，无人机二维路径规划问题，以无人机飞行路径最短、距离危险区域边界直线距离最远为优化目标，将无人机运行环境内的障碍物定义为危险区域，二维路径规划问题中危险区域是圆形区域，采用天牛须搜索算法进行初次路径规划，形成无人机一级路径；无人机三维路径规划问题，以无人机飞行路径最短、距离危险区域边界直线距离最远、无人机飞行高度最低为优化目标，将无人机运行环境内的障碍物定义为危险区域，三维路径规划问题中危险区域是圆柱体区域，采用天牛须搜索算法进行初次路径规划，形成无人机一级路径。

其中，步骤S2包括以下分步骤：

S21、设定所要规划无人机运行路径上的路径点数量为N_dim，其中N_dim也是天牛须搜索算法中搜索空间的维度，随机生成无人机每个路径点的初始坐标并将所有初始路径点表示为路径点集合WP＝{WP_m}，其中m＝1,2,…,N_dim，路径点集合WP即为天牛的位置坐标；分别计算以下优化目标：无人机飞行路径初始长度J₁、所有路径点距离每个危险区域边界初始最短直线距离总和的倒数J₂、无人机初始路径点飞行高度总和J₃；二维路径规划问题中J₃＝0；得到目标函数其中，J为路径代价值，并将min(J)作为最终优化目标；存储路径代价值J及路径点集合WP；

S22、开始迭代，根据公式A_L＝WP+Dir·d₀计算天牛左须在当前代数对应的位置坐标、根据公式A_R＝WP-Dir·d₀计算天牛右须在当前代数对应的位置坐标，公式A_L＝WP+Dir·d₀和A_R＝WP-Dir·d₀中，WP表示路径点集合，Dir表示天牛右须指向左须的随机单位向量，d₀表示天牛两须之间的距离；根据公式计算天牛左须对应的左须代价值、根据公式/>计算天牛右须对应的右须代价值；采用公式WP＝WP-Step·Dir·Sign(VA_L-VA_R)，更新WP后，将更新后的路径点坐标重新存储在WP中，从而更新天牛的位置坐标，其中Step表示天牛每次运动的可变步长，Dir表示天牛右须指向左须的随机单位向量，Sign()表示符号函数，d₀表示天牛两须之间的距离；

S23、根据更新后的天牛位置坐标重新计算无人机路径代价值J，与已保存的路径代价值J比对，存储最小的路径代价值J及最小的路径代价值J对应的路径点集合WP；判断是否达到指定的迭代次数，结果为否，返回步骤S22；结果为是，执行步骤S24；

S24、迭代完成，将最后存储的路径点集合WP作为无人机一级路径的路径点集合。

具体的，随机生成空间维度为N_dim的初始路径点集合WP＝{WP_m}，路径点集合WP即为天牛的位置坐标，WP中的元素WP_m分别代表了无人机各路径点坐标，根据公式(1)求取无人机飞行路径初始长度J₁和所有路径点距离每个危险区域边界初始最短直线距离总和的倒数J₂，根据公式(2)求取无人机初始路径点飞行高度总和J₃，公式(1)和公式(2)中||·||表示2范数，T_j表示无人机运行空域内危险区域j的圆心坐标，R_j表示无人机运行空域内危险区域j的半径，N_T表示危险区域的数量，表示路径点m的竖坐标，公式(3)给出了目标函数表达式。

执行天牛须搜索算法，根据公式(4)分别计算天牛左须和天牛右须在当前代数对应的位置坐标，采用公式(5)分别计算天牛左须和天牛右须对应的代价值；采用公式(6)更新天牛的位置坐标，公式(6)中Step表示天牛每次运动的可变步长，Dir表示天牛右须指向左须的随机单位向量，Dir可由公式(7)获得，公式(7)中eps、norm()是MATLAB软件中的内置函数，分别表示浮点相对精度、求矩阵最大奇异值，Sign()表示符号函数，d₀表示天牛两须之间的距离；d₀与Step存在公式(8)所示的关系，公式(8)中c为接近1的常数。

WP＝WP-Step·Dir·Sign(VA_L-VA_R)(6)

根据更新后的天牛位置坐标采用公式(3)重新计算无人机路径代价值J，与已保存的路径代价值J比对，存储最小的路径代价值J及最小的路径代价值J对应的路径点集合WP；判断是否达到指定的迭代次数，结果为否，则继续执行上述算法；结果为是，则将最后存储的路径点集合WP作为无人机一级路径的路径点集合。

S3、在S1所述新坐标系下，以S2形成的无人机一级路径为中心线，无人机二维路径规划问题，以垂直距离d向中心线两侧平面扩展，形成不进入危险区域的无人机二维飞行走廊；无人机三维路径规划问题，以垂直距离d为半径向中心线周围空间扩展，形成不进入危险区域的无人机三维飞行走廊；图3给出了二维条件下无人机飞行走廊示意图，图3中T1、T2、T3、T4、T5分别表示五个障碍物，被视为危险区域。

S4、在S3形成的无人机飞行走廊内，二维路径规划问题，以无人机飞行路径最短、距离危险区域边界直线距离最远、转弯角度最小为优化目标，采用天牛须搜索算法、粒子群优化算法进行二次联合路径规划，形成无人机二级路径；三维路径规划问题，以无人机飞行路径最短、距离危险区域边界直线距离最远、转弯角度最小、升降角度最小、无人机飞行高度最低为优化目标，采用天牛须搜索算法、粒子群优化算法进行二次联合路径规划，形成无人机二级路径。

其中，步骤S4包括以下分步骤：

S41、在S3形成的无人机飞行走廊内，采用粒子群优化算法，随机生成粒子维度为N_dim、粒子数量为N_P的初始粒子群，第i个粒子的位置代表飞行走廊内可行的无人机路径点集合WP′_i，表示为路径点集合WP′_i的维度为N_dim，i＝1,2,…,N_P；对于每一个粒子，分别计算以下优化目标：无人机飞行路径初始长度J′₁、所有路径点距离每个危险区域边界初始最短直线距离总和的倒数J′₂、无人机初始路径点飞行高度总和J′₃、无人机相邻路径点间的转弯角度总和J'₄、无人机相邻路径点间的升降角度总和J′₅；二维路径规划问题中J′₃＝0、J′₅＝0；得到目标函数/>其中，J'为粒子的路径代价值，并将min(J')作为最终优化目标；存储每个粒子的路径代价值J'及路径点集合WP′_i；以上述N_P个路径代价值中的最小值为最小路径代价值J'_min、以最小路径代价值J'_min对应的路径点集合为WP'，存储最小路径代价值J'_min及路径点集合WP'；

S42、开始迭代，随机选取N_P/2个粒子并将其中任意粒子视为一个天牛个体，并对每个天牛个体采用天牛须搜索算法，根据公式A'_L＝WP′_i+Dir'·d'₀计算天牛左须在当前代数对应的位置坐标、根据公式A'_R＝WP′_i-Dir'·d'₀计算天牛右须在当前代数对应的位置坐标，公式A'_L＝WP′_i+Dir'·d'₀和A'_R＝WP′_i-Dir'·d'₀中，WP′_i表示第i个粒子的路径点集合，Dir'表示第i个粒子的天牛右须指向左须的随机单位向量，d′₀表示第i个粒子的天牛两须之间的距离；根据公式计算天牛左须对应的左须代价值、根据公式计算天牛右须对应的右须代价值；采用公式WP′_i＝WP′_i-Step'·Dir'·Sign(VA'_L-VA'_R)，更新WP′_i后，将更新后的路径点坐标重新存储在WP′_i中，从而更新每个天牛的位置坐标，即更新每个粒子的位置坐标，其中Step'表示第i个粒子的天牛每次运动的可变步长，Dir'表示第i个粒子的天牛右须指向左须的随机单位向量，Sign()表示符号函数；对于剩余N_P/2个粒子，采用粒子群优化算法，对于剩余N_P/2个粒子中的任意粒子，采用公式/>更新第i个粒子在t+1代的速度，采用公式更新第i个粒子在t+1代的位置，即更新第i个粒子在t+1代可行的无人机路径点集合，公式/>和/>中，α和β分别表示第i个粒子运动速度的惯性系数以及第i个粒子所处位置的惯性系数，c₁和c₂分别表示一个随机数，P_i ^L和P_i ^G分别表示当前代数t内第i个粒子的局部最优路径代价值与全局最优路径代价值，WP_i'^t与WP_i'^(t+1)分别表示第i个粒子在当前第t代和第t+1代的位置坐标，即第i个粒子在当前第t代和第t+1代可行的无人机路径点集合；

S43、根据更新后的全部N_P个粒子位置坐标重新计算每个粒子对应的无人机路径代价值J'，与已保存的最小路径代价值J'_min比对，以该N_P+1个路径代价值中的最小值更新最小路径代价值J'_min，一并存储更新后的最小路径代价值J'_min对应的路径点集合WP'；判断是否达到指定的迭代次数，结果为否，返回步骤S42；结果为是，执行步骤S44；

S44、迭代完成，将最后存储的路径点集合WP'作为无人机二级路径的路径点集合。

具体的，在S3形成的飞行走廊内，随机生成粒子维度为N_dim、粒子数量为N_P的粒子群，第i个粒子的位置代表飞行走廊内可行的无人机路径点集合WP′_i，表示为路径点集合WP′_i维度为N_dim，i＝1,2,…,N_P，根据公式(9)求取无人机飞行路径初始长度J′₁、所有路径点距离每个危险区域边界初始最短直线距离总和的倒数J'₂、无人机相邻路径点间的转弯角度总和J'₄，根据公式(10)求取无人机相邻路径点间的升降角度总和J′₅，根据公式(11)求取无人机初始路径点飞行高度总和J′₃，公式(9)、公式(10)和公式(11)中||·||表示2范数，||·||_XOY表示XOY平面上的2范数，XOY平面为如图2所示坐标轴OX、OY所在的平面，T_j表示无人机运行空域内危险区域j的圆心坐标，R_j表示无人机运行空域内危险区域j的半径，N_T表示危险区域的数量，/> 分别表示粒子i位置集合中路径点q的横坐标、纵坐标、竖坐标，公式(12)给出了目标函数表达式，并将min(J')作为最终优化目标；存储每个粒子的路径代价值J'及路径点集合WP′_i，求取所有粒子的最小路径代价值J'_min以及最小路径代价值J'_min对应的路径点集合WP'。

随机选取N_P/2个粒子并对其执行天牛须搜索算法，根据公式(13)分别计算天牛左须和天牛右须在当前代数对应的位置坐标，采用公式(14)分别计算天牛左须和天牛右须对应的代价值；采用公式(15)更新天牛的位置坐标，公式(15)中Step'表示第i个粒子的天牛每次运动的可变步长，Dir'表示第i个粒子的天牛右须指向左须的随机单位向量，Dir'可由公式(16)获得，公式(16)中eps、norm()是MATLAB软件中的内置函数，分别表示浮点相对精度、求矩阵最大奇异值，Sign()表示符号函数，d'₀表示第i个粒子的天牛两须之间的距离；d'₀与Step'存在公式(17)所示的关系，c'为接近1的常数。

WP′_i＝WP′_i-Step'·Dir'·Sign(VA′_L-VA'_R)(15)

对于剩余N_P/2个粒子，对其执行粒子群优化算法，对于剩余N_P/2个粒子中的任意粒子，采用公式(18)更新第i个粒子在t+1代的速度、位置，公式(18)中α和β分别表示第i个粒子运动速度的惯性系数和第i个粒子所处位置的惯性系数，c₁和c₂分别表示一个随机数，P_i ^L和P_i ^G分别表示当前代数t内第i个粒子的局部最优路径代价值与全局最优路径代价值，WP_i'^t与WP_i'^(t+1)分别表示第i个粒子在当前第t代和第t+1代的位置坐标，即第i个粒子在当前第t代和第t+1代可行的无人机路径点集合。

根据更新后的全部N_P个粒子位置坐标采用公式(12)重新计算每个粒子对应的无人机路径代价值J'，与已保存的最小路径代价值J'_min比对，以该N_P+1个路径代价值中的最小值更新最小路径代价值J'_min，一并存储更新后的最小路径代价值J'_min对应的路径点集合WP'；判断是否达到指定的迭代次数，结果为否，则继续执行上述算法；结果为是，将最后存储的路径点集合WP'作为无人机二级路径的路径点集合。

S5、将S4形成的无人机二级路径作为最优路径并进行坐标系转换，转换规则为：将新坐标系下得到的无人机二级路径点坐标转换为原始坐标系下的路径点坐标，经平滑处理后，上传至无人机机载飞控模块后供无人机使用。二维条件下规划得到的最优路径如图4所示，图4中T1、T2、T3、T4、T5分别表示五个障碍物，被视为危险区域，五个危险区域的圆心坐标分别为T1(40.00,20.00)、T2(65.00,26.29)、T3(70.00,57.74)、T4(93.59,43.58)、T5(23.00,57.61)，对应的半径分别为14.00、19.98、10.00、16.46、12.79，起始点S以及目的地点D的坐标分别为S(14.00,13.40)、D(106.00,66.50)；由图4可知，规划的无人机运行路径能够有效规避危险区域，且经平滑处理后的运行路径适合无人机跟随运行。

上面结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的。本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护之内。

Claims (2)

1.一种多目标约束下的无人机分级式路径规划方法，其特征在于，包括以下步骤：

S1、根据给定无人机起始点S以及目的地点D进行坐标系转换，将转换后的坐标系定义为新坐标系，将转换前的坐标系定义为原始坐标系，转换规则为：无人机起始点S作为新坐标系的原点，无人机起始点S以及目的地点D的连线作为新坐标系的X轴，/>表示由起始点S指向目的地点D的矢量，在原始坐标系所在二维平面内过起始点S做/>的垂线作为新坐标系的Y轴，过起始点S做原始坐标系所在二维平面的垂线作为新坐标系的Z轴；

S2、在S1所述新坐标系下，无人机二维路径规划问题，以无人机飞行路径最短、距离危险区域边界直线距离最远为优化目标，将无人机运行环境内的障碍物定义为危险区域，二维路径规划问题中危险区域是圆形区域，采用天牛须搜索算法进行初次路径规划，形成无人机一级路径；无人机三维路径规划问题，以无人机飞行路径最短、距离危险区域边界直线距离最远、无人机飞行高度最低为优化目标，将无人机运行环境内的障碍物定义为危险区域，三维路径规划问题中危险区域是圆柱体区域，采用天牛须搜索算法进行初次路径规划，形成无人机一级路径；

S3、在S1所述新坐标系下，以S2形成的无人机一级路径为中心线，无人机二维路径规划问题，以垂直距离d向中心线两侧平面扩展，形成不进入危险区域的无人机二维飞行走廊；无人机三维路径规划问题，以垂直距离d为半径向中心线周围空间扩展，形成不进入危险区域的无人机三维飞行走廊；

S4、在S3形成的无人机飞行走廊内，二维路径规划问题，以无人机飞行路径最短、距离危险区域边界直线距离最远、转弯角度最小为优化目标，采用天牛须搜索算法、粒子群优化算法进行二次联合路径规划，形成无人机二级路径；三维路径规划问题，以无人机飞行路径最短、距离危险区域边界直线距离最远、转弯角度最小、升降角度最小、无人机飞行高度最低为优化目标，采用天牛须搜索算法、粒子群优化算法进行二次联合路径规划，形成无人机二级路径；

所述步骤S4包括以下分步骤：

S41、在S3形成的无人机飞行走廊内，采用粒子群优化算法，随机生成粒子维度为N_dim、粒子数量为N_P的初始粒子群，第i个粒子的位置代表飞行走廊内可行的无人机路径点集合WP′_i，表示为路径点集合WP′_i的维度为N_dim，i＝1,2,…,N_P；对于每一个粒子，分别计算以下优化目标：无人机飞行路径初始长度J′₁、所有路径点距离每个危险区域边界初始最短直线距离总和的倒数J'₂、无人机初始路径点飞行高度总和J′₃、无人机相邻路径点间的转弯角度总和J'₄、无人机相邻路径点间的升降角度总和J′₅；二维路径规划问题中J′₃＝0、J′₅＝0；得到目标函数/>其中，J'为粒子的路径代价值，并将min(J')作为最终优化目标；存储每个粒子的路径代价值J'及路径点集合WP_i'；以上述N_P个路径代价值中的最小值为最小路径代价值J′_min、以最小路径代价值J'_min对应的路径点集合为WP'，存储最小路径代价值J'_min及路径点集合WP′；

S42、开始迭代，随机选取N_P/2个粒子并将其中任意粒子视为一个天牛个体，并对每个天牛个体采用天牛须搜索算法，根据公式A'_L＝WP_i'+Dir'·d'₀计算天牛左须在当前代数对应的位置坐标、根据公式A'_R＝WP′_i-Dir'·d'₀计算天牛右须在当前代数对应的位置坐标，公式A'_L＝WP_i'+Dir'·d'₀和A'_R＝WP_i'-Dir'·d'₀中，WP_i'表示第i个粒子的路径点集合，Dir'表示第i个粒子的天牛右须指向左须的随机单位向量，d'₀表示第i个粒子的天牛两须之间的距离；根据公式计算天牛左须对应的左须代价值、根据公式计算天牛右须对应的右须代价值；采用公式WP′_i＝WP′_i-Step'·Dir'·Sign(VA'_L-VA'_R)，更新WP′_i后，将更新后的路径点坐标重新存储在WP_i'中，从而更新每个天牛的位置坐标，即更新每个粒子的位置坐标，其中Step'表示第i个粒子的天牛每次运动的可变步长，Dir'表示第i个粒子的天牛右须指向左须的随机单位向量，Sign()表示符号函数；对于剩余N_P/2个粒子，采用粒子群优化算法，对于剩余N_P/2个粒子中的任意粒子，采用公式/>更新第i个粒子在t+1代的速度，采用公式更新第i个粒子在t+1代的位置，即更新第i个粒子在t+1代可行的无人机路径点集合，公式/>和/>中，α和β分别表示第i个粒子运动速度的惯性系数以及第i个粒子所处位置的惯性系数，c₁和c₂分别表示一个随机数，P_i ^L和P_i ^G分别表示当前代数t内第i个粒子的局部最优路径代价值与全局最优路径代价值，WP_i'^t与WP_i'^(t+1)分别表示第i个粒子在当前第t代和第t+1代的位置坐标，即第i个粒子在当前第t代和第t+1代可行的无人机路径点集合；

S43、根据更新后的全部N_P个粒子位置坐标重新计算每个粒子对应的无人机路径代价值J'，与已保存的最小路径代价值J'_min比对，以该N_P+1个路径代价值中的最小值更新最小路径代价值J'_min，一并存储更新后的最小路径代价值J'_min对应的路径点集合WP'；判断是否达到指定的迭代次数，结果为否，返回步骤S42；结果为是，执行步骤S44；

S44、迭代完成，将最后存储的路径点集合WP'作为无人机二级路径的路径点集合；

S5、将S4形成的无人机二级路径作为最优路径并进行坐标系转换，转换规则为：将新坐标系下得到的无人机二级路径点坐标转换为原始坐标系下的路径点坐标，经平滑处理后，上传至无人机机载飞控模块后供无人机使用。

2.根据权利要求1所述的多目标约束下的无人机分级式路径规划方法，其特征在于，所述步骤S2包括以下分步骤：

S21、设定所要规划无人机运行路径上的路径点数量为N_dim，其中N_dim也是天牛须搜索算法中搜索空间的维度，随机生成无人机每个路径点的初始坐标并将所有初始路径点表示为路径点集合WP＝{WP_m}，其中m＝1,2,…,N_dim，路径点集合WP即为天牛的位置坐标；分别计算以下优化目标：无人机飞行路径初始长度J₁、所有路径点距离每个危险区域边界初始最短直线距离总和的倒数J₂、无人机初始路径点飞行高度总和J₃；二维路径规划问题中J₃＝0；得到目标函数其中，J为路径代价值，并将min(J)作为最终优化目标；存储路径代价值J及路径点集合WP；

S22、开始迭代，根据公式A_L＝WP+Dir·d₀计算天牛左须在当前代数对应的位置坐标、根据公式A_R＝WP-Dir·d₀计算天牛右须在当前代数对应的位置坐标，公式A_L＝WP+Dir·d₀和A_R＝WP-Dir·d₀中，WP表示路径点集合，Dir表示天牛右须指向左须的随机单位向量，d₀表示天牛两须之间的距离；根据公式计算天牛左须对应的左须代价值、根据公式计算天牛右须对应的右须代价值；采用公式WP＝WP-Step·Dir·Sign(VA_L-VA_R)，更新WP后，将更新后的路径点坐标重新存储在WP中，从而更新天牛的位置坐标，其中Step表示天牛每次运动的可变步长，Dir表示天牛右须指向左须的随机单位向量，Sign()表示符号函数，d₀表示天牛两须之间的距离；

S23、根据更新后的天牛位置坐标重新计算无人机路径代价值J，与已保存的路径代价值J比对，存储最小的路径代价值J及最小的路径代价值J对应的路径点集合WP；判断是否达到指定的迭代次数，结果为否，返回步骤S22；结果为是，执行步骤S24；

S24、迭代完成，将最后存储的路径点集合WP作为无人机一级路径的路径点集合。