技术领域
[0001] 本
发明属于
机器人技术领域，尤其涉及一种基于多种群协作果蝇优化的多无人机协同路径规划方法。技术背景
[0002] 多无人机协同已经成为很多任务的首选方法，三维航路规划是无人机自主控制的关键组成部分。航路规划
算法的目标是在为无人机计算出最优的飞行路径，这个飞行路线能使无人机躲开特定的区域，在以较短的时间，较少的能耗到达目标点。多无人机三维航路协同规划问题属于多约束的复杂优化问题，通常具有如下特点：评价飞行路径的指标很多，构成的目标函数计算开销大；地图环境情况复杂，数据量大或描述困难；无人机飞行性能限制等；需要优化的变量多，搜索空间往往具有维数爆炸的特点；机上设备有限，计算速度慢。因此，无人机航路规划最关键的就是如何处理无人机自身物理特性带来的多种复杂约束，在较短时间内快速得出最优航路。
[0003] 求解多机三维航路规划问题的方法大体可以分为传统规划算法和智能规划算法。其中，传统规划算法有动态规划法，A星算法等；智能规划方法有神经网络方法、进化计算及
群体智能方法等。从这些规划算法的特点和航路规划问题的要求来看，智能规划方法已成为求解无人机航路规划问题的发展趋势，包括
遗传算法、
退火算法、
鸟群算法等。尽管这些航路规划技术正进一步向智能化、实用化方向发展，但仍各自存在着诸多问题。例如，A星算法基于确定的过程，利用数学优化的方法进行路径规划，所需时间长；群体智能方法的控制参数难于选择，性能不稳定、随机性强，容易过早收敛或陷入局部最优。因此，需要进一步发展更加优越的规划方法。
[0004] 果蝇
优化算法(Fruit fly optimization algorithm,FOA)一种基于果蝇觅食行为的启发式全局最优化算法。果蝇在嗅觉与视觉上，优于其他物种。果蝇的嗅觉器官能够搜集到空气中的食物的气味，甚至能嗅到很远距离的食物。当果蝇靠近食物
位置后，能够通过的视觉发现食物的具体位置与同伴的聚集位置，并且朝该方向飞去。受果蝇上述特点而启发的果蝇优化算法在对单机进行路径规划时的效果很好，但是在处理多机问题时效果较差，且存在着编码方式适用性限制多、易陷于局部最优等缺点，因此在应用果蝇优化算法解决三维航路规划问题时需要对原始算法进行改进。
发明内容
[0005] 发明目的：
[0006] 本发明基于果蝇优化算法，改进后得到一种用于解决多无人机协同路径规划的方法。
[0007] 技术方案：
[0008] 本发明利用果蝇优化算法，并对其进行改进，将果蝇分群，令果蝇种群之间互相竞争，提高了其在协同规划多机飞行路径情况下的收敛速度，降低了无人机的飞行成本。
[0009] 准备工作：确定无人机飞行任务信息，包括起点坐标(Xs,j,Ys,j,Zs,j)和终点坐标(XT,YT,ZT)，任务需要的无人机的数量Num，无人机的最大飞行速度vmax，最大转弯过载nmax，重
力加速度g和飞行最低高度hmin。任务地图信息p，边界 分别表示地图在长、宽、高上的限制。每架飞机需要规划出的航路控制点个数D；
[0010] 确定敌方地面武器信息，包括威胁类型，如雷达、导弹、高炮等，武器位置(xthreatj,ythreatj)，以及各自的威胁范围。
[0011] 设定改进果蝇算法的相关参数，包括最大
迭代次数NCmax，果蝇群体数gr，果蝇群体规模Mpop，杂交遗传系数coe1和coe2。
[0012] 步骤一：开始，令N＝1。
[0013] 步骤二：随机生成最优航路控制点
[0014]
[0015]
[0016] xbest与ybest各项都在[-1,1]范围内。令NC＝1。
[0017] 步骤三：令i＝1，j＝1，g＝1。令果蝇个体开始进行嗅觉搜索。
[0018] 步骤四：嗅觉搜索的操作如下：
[0019]
[0020]
[0022]
[0023] 其中 即为味道浓度判定值。若 则说明超出地图限制，转到步骤四。若j＜3*D，则j＝j+1，转到步骤四。否则转到步骤五。
[0024] 步骤五：执行生成飞行路径操作。操作过程如下：利用起点坐标，终点坐标和Si，可得该果蝇个体的飞行路径pathi。
[0025] 步骤六：计算pathi的航路曲线的味道浓度值，即为航路代价值，记为[0026] 步骤七：令g＝g+1,若g＜gr，令j＝1，转到步骤四，否则转到步骤八。
[0027] 步骤八：令i＝i+1,若i＜Mpop，令j＝1，g＝1，转到步骤四，否则转到步骤九。
[0028] 步骤九：执行果蝇视觉搜索操作。操作步骤如下：对cost中所有路径的味道浓度值排序后，取最小的五个，即前五个，称为最优个体群。视觉搜索操作结束。把最优个体群的航路控制点记为 和 并记录最优个体群在步骤五中已经计算出来的味道浓度值，记为oldcost1……oldcost5。
[0029] 步骤十：果蝇群体间杂交开始。令i＝1，g＝1，j＝1。
[0030] 步骤十一：五个最优个体 和 分别与种群中的果蝇随机杂交生成新种群，杂交过程如下所示：
[0031]
[0032]
[0033] 其中j的范围是[1,D]。random(1,gr)产生在[1,gr]范围内的随机整数。
[0034] 步骤十二：利用公式(3)计算味道浓度判定值 其中，用 代替 用代替 若 则说明超出地图限制，转到步骤十一，否则转到步骤十三。
[0035] 步骤十三：若j＜3*D，则j＝j+1，转到步骤十一。否则转到步骤十四。
[0036] 步骤十四：执行生成路径操作，用Snewi替换原操作中的Si，得到pathi。执行步骤六，得到
[0037] 步骤十五：令g＝g+1,若g≤gr，令j＝1，转到步骤十，否则转到步骤十五。
[0038] 步骤十六：令i＝i+1，若i≤Mpop，令j＝1，g＝1，转到步骤十，否则转到步骤十六。
[0039] 步骤十七：执行果蝇视觉搜索操作。对最优个体群进行如下操作：把他们的航路控制点记为 和 并记录他们在步骤十三中计算出的味道浓度值，记为
[0040] 步骤十八：执行种群竞争操作，对 和两组共10个个体进行自然选择与淘汰。种群竞争操作
过程如下：比较 和 共十个个体，留下味道浓度值最小的
五个个体，记为 和
[0041] 步骤十九：若NC＜NCmax，令NC＝NC+1,转到步骤二，否则计算最优个体 的味道浓度判定值，执行生成飞行路径操作，得到飞行路径pathend并输出。
[0042] 步骤二十：若N＜Num，令N＝N+1，转到步骤一，否则结束。
[0043] 上述算法中采用双编码形式，即利用两组数据共同编码果蝇的航路控制点。在规划多机路径时，一次只为一架飞机规划路线，路径的规划顺序与无人机的编号顺序相同。
[0044] 步骤一中，生成的最优航路控制点中，xbest与ybest共同记录最优航路控制点的D个控制点。
[0045] 步骤三中，random(-1,1)产生(-1,1)之间的随机实数。其中 与 共同记录第g个群体中第i个果蝇的第j个航路控制点。
[0046] 步骤四中，生成路径的具体方式如下：
[0047] 先将起点和终点用如下公式记录，
[0048]
[0049]
[0050]
[0051]
[0052]
[0053]
[0054] 再将每个控制点记录，
[0055]
[0056]
[0057]
[0058] 其中，(Xs,N,Ys,N,Zs,N)是第N架飞机的起点坐标， 是该路径上所有控制点的x轴上的坐标， 是这条路径上所有控制点的y轴上的坐标， 是这条路径上所有控制点的z轴上的坐标。k表示控制点的编号，此处k的取值范围为2到D+1。记该果蝇个体的找到的路径为pathi。
[0059] 步骤五中，味道浓度值的计算方法如下：
[0060] 计算每一条路径的味道浓度值J，味道浓度值J由下式确定：
[0061]
[0062] 其中fi，i＝1……7，包括高度代价JH，路径长度JL，威胁代价JT，地形约束CH，转弯
角约束CT，爬升下滑约束CG，碰撞
风险JP。
[0063] 其中，路径长度JL由下式确定：
[0064] 其中
[0065] 其中，高度代价JH由下式确定：
[0066]
[0067] 其中，地形约束CH由下式计算：
[0068] 且
[0069] 其中，H(p)是地表高度，path(z)是路径的高度。
[0070] 其中，威胁代价JT由下式计算：
[0071]
[0072] 其中Tde(j)是第j个地面防御武器造成的威胁概率。
[0073] 若为导弹，Tde(j)的计算式如下：
[0074]
[0075] Rmax是导弹的威胁半径最大长度，d是无人机与武器之间的
水平距离。
[0076] 若为雷达，Tde(j)的计算式如下：
[0077]
[0078] Rmax是雷达的威胁半径最大长度，d是无人机与武器之间的水平距离。
[0079] 若为高炮，Tde(j)的计算式如下：
[0080]
[0081] Rmax是高炮的威胁半径最大长度，d是无人机与武器之间的水平距离。
[0082] 转弯角约束CT由下式确定：
[0083] 且
[0084] 其中，爬升下滑约束CG由下式确定：
[0085] 且
[0086] 在上式中，
[0087]
[0088]
[0089]
[0090] 碰撞风险JP的计算与说明如下：
[0091] 在本发明中，因为规划路径的顺序按照无人机编号的顺序，所以第一架飞机不需要考虑与其他飞机碰撞的风险，之后的第N架飞机需要避免与之前的N-1架飞机碰撞的风险。在判断碰撞风险时，采用两两判断的方式，第N架飞机需要分别与第1架飞机，第2架飞机……第N-1架飞机判断碰撞风险。计算式如下所示：
[0092]
[0093] 其中JPn是第N架飞机与第n架飞机之间的碰撞风险。
[0094] 且 式中，disk是同一时刻两架无人机之间的距离，PC是惩罚因子。
[0095] 步骤八中，在视觉搜索过程中，记录最优个体群中每个果蝇的编号，之后在X与Y中按照编号寻找最优个体群中的果蝇，后将其的航路控制点记录。相同地，利用编号在cost中寻找最优个体群中果蝇的味道浓度值，后将其的味道浓度值记录。
[0096] 步骤十五中，记录最优个体群中果蝇的航路控制点与味道浓度值的步骤与步骤八中相同。
[0097] 本发明的优点及效果：
[0098] 本发明提出了一种基于果蝇优化算法的改进的三维路径协同规划方法。该方法在果蝇的每次迭代中将果蝇分群，并使果蝇种群之间互相竞争，提高了算法的收敛速度，改善了果蝇优化算法在规划多机路径中收敛过慢，陷入局部最优解等问题。
附图说明
[0100] 图1(b)图1(a)中所提到的全体果蝇嗅觉搜索部分的细节流程图；
[0101] 图2改进算法及原始算法生成的三架飞机的路径的俯视图，其中，实线是改进算法生成的飞行路径，虚线是原始算法生成的飞行路径；
[0102] 图3改进算法及原始算法生成的三架飞机的路径的三维视图，其中，实线是改进算法生成的飞行路径，虚线是原始算法生成的飞行路径；
[0103] 图4改进算法及原始算法生成的第一架飞机的航路代价值的进化曲线对比图；
[0104] 图5改进算法及原始算法生成的第二架飞机的航路代价值的进化曲线对比图；
[0105] 图6改进算法及原始算法生成的第三架飞机的航路代价值的进化曲线对比图。
具体实施方式
[0106] 下面通过一个具体例子来描述算法的具体实施过程，并验证算法性能。例子中所采用
软件为MATLAB2018a。具体流程图如附图1所示。
[0107] 确定无人机飞行任务信息，起点坐标(5,10,Zs,1)，(5,55,Zs,2)，(70,15,Zs,3)和终点坐标(85,85,ZT)，任务需要的无人机的数量Num＝3，无人机的最大飞行速度vmax＝200m/s，最大转弯过载nmax＝5g，
重力加速度g＝9.8和飞行最低高度hmin＝0.05m。任务地图信息p，边界 分别表示地图在长、宽、高上的限制。每架飞机需要规划出的航路控制点个数D＝4；
[0108] 确定敌方地面武器信息，雷达坐标(40,25),(75,60)，威胁范围15，导弹坐标(35,70),(20,41)，威胁范围8，高炮坐标(45,45)，威胁范围9。
[0109] 设定改进果蝇算法的相关参数，包括最大迭代次数NCmax＝1000，果蝇群体数gr＝5，果蝇群体规模Mpop＝40。杂交遗传系数coe1＝0.5,coe2＝0.5。
[0110] 步骤一：令N＝1。
[0111] 步骤二：随机生成最优路径群体
[0112]
[0113]
[0114] xbest与ybest各项都在[-1,1]范围内。其中k＝12,D＝4,gr＝5。
[0115] 步骤三：令i＝1，j＝1，NC＝1。令果蝇个体开始进行嗅觉搜索。
[0116] 步骤四：嗅觉搜索的操作为：
[0117]
[0118]
[0119] 计算味道浓度判定值的公式如下，
[0120]
[0121] 其中 即为味道浓度判定值。若 则说明超出地图限制，转到步骤四，否则转到步骤五。
[0122] 步骤五：利用起点坐标，终点坐标和Si，先将起点和终点用如下公式记录，[0123]
[0124]
[0125]
[0126]
[0127]
[0128]
[0129] 再将每个控制点记录，
[0130]
[0131]
[0132]
[0133] 可得该果蝇个体的路径pathi，即为第i只果蝇寻找到的路径。
[0134] 步骤六：计算pathi的航路曲线的味道浓度值J，味道浓度值J由下式确定：
[0135]
[0136] 其中fi，i＝1……7，包括高度代价JH，路径长度JL，威胁代价JT，地形约束CH，转弯角约束CT，爬升下滑约束CG，碰撞风险JP。
[0137] 其中，路径长度JL由下式确定：
[0138] 其中
[0139] 其中，高度代价JH由下式确定：
[0140]
[0141] 其中，hmin＝0.05m。
[0142] 其中，地形约束CH由下式计算：
[0143] 且 其中，H(p)是地表高度，path(z)是路径的高度。
[0144] 其中，威胁代价JT由下式计算：
[0145]
[0146] 其中Tde(j)是第j个地面防御武器造成的威胁概率。
[0147] 若为导弹，Tde(j)的计算式如下：
[0148]
[0149] Rmax是导弹的威胁半径最大长度，此处为8km，d是无人机与武器之间的水平距离。
[0150] 若为雷达，Tde(j)的计算式如下：
[0151]
[0152] Rmax是雷达的威胁半径最大长度，此处为15km，d是无人机与武器之间的水平距离。
[0153] 若为高炮，Tde(j)的计算式如下：
[0154]
[0155] Rmax是高炮的威胁半径最大长度，此处为9km，d是无人机与武器之间的水平距离。
[0156] 转弯角约束CT由下式确定：
[0157] 且
[0158] 其中，爬升下滑约束CG由下式确定：
[0159] 且
[0160] 在上式中，
[0161]
[0162]
[0163]
[0164] 碰撞风险JP由下式确定：
[0165] 且 式中，disk是无人机之间的距离，PC是惩罚因子，取20000。
[0166] 得到航路代价值，记为costi。
[0167] 步骤七：令g＝g+1,若g＜gr，令j＝1，转到步骤四，否则转到步骤八。
[0168] 步骤八：令i＝i+1,若i＜Mpop，令j＝1，g＝1，转到步骤四，否则转到步骤九。
[0169] 步骤九：执行果蝇视觉搜索操作。操作步骤如下：对cost中所有路径的味道浓度值排序后，取最小的五个，即前五个，称为最优个体群。视觉搜索操作结束。利用最优个体群中果蝇的编号，在X和Y中把最优个体群的果蝇找到，并将最优个体群的果蝇的航路控制点记为 和 并记录最优个体群在步骤五中已经计算出来的味道浓度值，记为oldcost1……oldcost5。
[0170] 步骤十：果蝇群体间杂交开始。令i＝1，g＝1，j＝1。
[0171] 步骤十一：五个最优个体 和 分别与种群中的果蝇随机杂交生成新种群，杂交过程如下所示：
[0172]
[0173]
[0174] 其中j的范围是[1,D]。random(1,gr)产生在[1,gr]范围内的随机整数。
[0175] 步骤十二：利用公式(3)计算味道浓度判定值 其中，用 代替 用代替 若 则说明超出地图限制，转到步骤十一，否则转到步骤十三。
[0176] 步骤十三：若j＜3*D，则j＝j+1，转到步骤十一。否则转到步骤十四。
[0177] 步骤十四：执行生成路径操作，用Snewi替换原操作中的Si，得到pathi。执行步骤五，得到
[0178] 步骤十五：令g＝g+1,若g≤gr，令j＝1，转到步骤十一，否则转到步骤十六。
[0179] 步骤十六：令i＝i+1，若i≤Mpop，令j＝1，g＝1，转到步骤十一，否则转到步骤十七。
[0180] 步骤十七：执行果蝇视觉搜索操作。对最优个体群进行如下操作：把他们的航路控制点记为 和 并记录他们在步骤十四中计算出的味道浓度值，记为
[0181] 步骤十八：执行种群竞争操作，对 和两组共10个个体进行自然选择与淘汰。种群竞争操作
过程如下：比较 和 共十个个体，留下味道浓度值最小的
五个个体，记为 和
[0182] 步骤十九：若NC＜NCmax，令NC＝NC+1,转到步骤二，否则计算最优个体 的味道浓度判定值，执行生成飞行路径操作，得到飞行路径pathend并输出。
[0183] 步骤二十：若N＜Num，令N＝N+1，转到步骤一，否则结束。
[0184] 附图2显示了本发明所提方法计算产生的三维飞行路径图。从结果中可以看出，飞机绕过了所有的威胁区域，并顺利到达终点且无碰撞。附图4至附图6是迭代过程中最优味道浓度值的曲线，可以看出本算法优于原始算法。
[0185] 本发明提出的方法不仅可用于多无人机协同路径规划，同时也可用于求解机器人、工业等领域存在的其他高维函数优化问题。
