CN109521794A - 一种多无人机航路规划及动态避障方法 - Google Patents

Abstract

本发明公布了一种Voronoi蚁群算法与人工势场法结合的多无人机航路规划及动态避障方法，所述方法首先利用激光雷达及摄像头等模块获取环境中静态障碍物位置；其次通过Voronoi图分割静态环境，并根据改进蚁群算法离线规划出各无人机的最优路径；最后在各无人机沿离线规划的航路飞行时，实时通过超声波传感器监测动态障碍物，并以其为威胁源建立无人机所受的引力及斥力模型，再根据引力及斥力求出无人机所受的合力，飞至静态航路，无人机沿离线规划航路继续飞行并依据此法循环直至各无人机全部飞至目标航点。该方法主要为多无人机编队规划出避开所有静态障碍物的最优航路且迭代时间较短，同时能够实时监测动态障碍物并及时规划局部航路防止碰撞。

Description

一种多无人机航路规划及动态避障方法

技术领域

本发明属于多无人机航路规划及动态避障技术领域，特别是一种Voronoi蚁群算法与人工势场法结合的多无人机航路规划及动态避障方法。

技术背景

无人机路径规划问题是编队控制的一个重点和难点，同时也是任务规划的一个研究的重点。无人机路径规划问题需要考虑许多制约因素，包括静态环境因素，雷达及静态障碍物因素，飞行的航路长度(航路燃油代价)以及飞行总时间等。在考虑到外界各类环境因素以及无人机本身各项飞行指标和性能等问题后，规划出一条从起始点到目标点的最优或较优路径。在静态环境及静态威胁源已知的情况下，根据静态航路规划所研究的内容，已经可以通过多机协同航路规划层为各无人机规划出各自的从起始点到目标点的最优或次优路径。然而在实际编队飞行中，当无人机在离线规划好的路径上飞行时，有时会出现未知的移动障碍物如移动雷达或其他无人机等，如果各无人机依然沿着规划好的路径飞行，则会发生事故，因此无人机编队在离线规划好路径并按路径飞行时，还应沿应实时监测动态障碍物信息并动态调整整体航路。

近年来，国内外的学者对于路径规划问题做了许多研究，提出的方法主要有Voronoi图法、蚁群算法、粒子群算法、A*算法、模拟退火算法等。本文提出了一种基于Voronoi图和改进蚁群算法的路径规划研究，首先根据已知的雷达或静态障碍物生成加权Voronoi图，然后运用改进蚁群算法进行最优路径规划，找到一条最优的飞行路径。针对前面研究的离线静态路径规划方法，本文又提出了一种双层滚动规划法用于解决动态路径规划的问题，将动态障碍物根据运动速度及无人机飞行步长时间膨胀为静态障碍物并建立模型，之后则基于人工势场法局部路径规划，设计了对于实时动态未知的障碍物的避障策略。人工势场法(APF) 是一种虚拟力法，其基本思想是，通过构造一种抽象的势力场，无人机在追踪目标过程中受到该势力场的影响，目标对无人机产生“引力”，障碍物对无人机产生“斥力”，引力与斥力叠加求出合力控制无人机运动。此方法较为直观且计算量小，是一种动态路径规划方法，在目标追踪中得到广泛应用，但该算法也存在易陷入局部最优、路径振荡等问题。

发明内容

本发明的目的在于提供一种Voronoi蚁群算法与人工势场法结合的多无人机航路规划及动态避障方法，该方法可以为无人机编队多架无人机根据静态障碍物离线规划出最优路径，并在无人机沿离线路径飞行时实时避开动态障碍物。

为实现上述目的，本发明采用以下技术方案：

一种Voronoi蚁群算法与人工势场法结合的多无人机航路规划及动态避障方法，包括：

第一步，设置环境区域大小，无人机起点位置，无人机飞行步长、每架无人机最大飞行速度V_max，而多旋翼无人机可以悬停因此无最小速度限制，威胁代价及航路燃油代价的系数k₁＝k₂。设置增益系数，包括蚁群算法蚂蚁数量共m，最大迭代次数N_cmax，信息素权重因子α，启发函数权重因子β，信息素的挥发系数ρ，信息素强度Q，目标引力增益K_A，障碍物斥力增益K_R。

第二步，利用机载摄像头和激光雷达检测障碍物位置、障碍物大小和目标位置。

第三步，建立加权Voronoi图并通过改进后的蚁群算法为每架无人机规划出离线航路。

第四步，各无人机沿静态规划航路飞行并通过超声波传感器实时监测动态障碍物范围，若未监测到动态障碍物，各无人机则继续沿离线规划路径飞行直至每架无人机均到达目标航点，一旦发现动态障碍物则跳转至第五步。

第五步，根据监测到的动态障碍物坐标位置(x_o，y_o)、飞行速度V_o以及无人机飞行一步长时间T建立其膨胀后的动态障碍物模型。

第六步，计算监测到动态障碍物的无人机此时受到目标引力F_att，此时的目标航点为离线规划航路上未受动态膨胀后的障碍物影响的下一航点，计算受到的障碍物斥力F_rep，并求出该无人机此时所受的合力F_total。

第七步，由无人机所受合力F_total可以得出该无人机局部规划航路的下一航点的飞行方向，并飞行一步长，判断是否到达离线规划航路上的航点，若到达，跳转至第八步，否则继续计算无人机此时所受的合力F_total并继续沿其方向飞行，重复此步骤直至飞行至离线规划航路。

第八步，无人机继续沿离线规划航路飞行，若再次监测到动态障碍物则跳转至第五步，直至所有无人机飞行至目标航点。

本发明具有以下优点：

1.本发明首先通过机载摄像头及激光雷达获取静态障碍物信息，建立威胁源模型，并建立Voronoi图并采用改进后的蚁群算法离线规划出最优路径，与传统格栅法蚁群算法相比规划出的路径总长度较短，且迭代时间更短。并且本发明改进了蚁群算法的启发函数，加入下一航点到目标航点的距离作为代价，因此可以更快的规划出距离较短的路径。

2.在离线静态规划路径后，本发明还将实时监测动态障碍物并通过人工势场法进行局部路径规划，因此在静态障碍物避障的基础上，本发明可以针对突然出现的不定向障碍物进行重规划路径避障。

3.本发明给出了每架无人机对于动态障碍物的斥力势场和斥力的合理定义，新的斥力使得无人机飞行做机动动作时，不至于频繁地大转角以避障，而是缓慢地避障，避免出现过大激动；其次，由于斥力函数具有连续性，避免了因斥力跳跃变化导致的无人机急促转弯现象，有效解决了动态障碍物避障问题。

附图表说明

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

图2为障碍物膨胀示意图。

图3为人工势场法示意图。

图4为采用传统格栅法蚁群算法的单机离线规划路径图。

图5为采用Voronoi改进蚁群算法的单机离线规划路径图。

图6为采用Voronoi改进蚁群算法的多机离线规划路径图。

图7为采用人工势场法局部路径规划的多机动态避障规划路径图。

具体实施方式

结合所附图表，对本发明的技术方案作具体说明。

本发明的一种Voronoi蚁群算法与人工势场法结合的多无人机航路规划及动态避障方法，具体包括以下步骤：

步骤1 设置环境区域大小，无人机起点位置，无人机飞行步长、每架无人机最大飞行速度V_max，而多旋翼无人机可以悬停因此无最小速度限制，威胁代价及航路燃油代价的系数k₁＝k₂。设置增益系数，包括蚁群算法蚂蚁数量共m，最大迭代次数N_cmax，信息素权重因子α，启发函数权重因子β，信息素的挥发系数ρ，信息素强度Q，目标引力增益K_A，障碍物斥力增益K_R。

步骤2 利用机载摄像头和激光雷达检测障碍物位置、障碍物大小和目标位置。

步骤3 建立加权Voronoi图并通过改进后的蚁群算法为每架无人机规划出离线航路，具体计算公式：

(1)Voronoi图各边的威胁代价计算：

式中，J_threat，i代表了第i条路径上的威胁代价，L_i代表了该条路径的总长度， 分别为该条路径上处到障碍物的距离。

J_fuel，i＝L_i (2)

式中，J_fuel，i代表了无人机飞行燃油代价，只与飞行长度有关，因此用路径长度表示。

(2)改进蚁群算法状态转移概率函数计算：

式中，表示的是蚁群算法中，无人机k从节点i转移到另一可以选择的节点k 的概率，α代表了信息素的重要程度，与信息素浓度在整体航路规划过程中所发挥的作用成正比。β代表了启发函数重要程度，同理，与启发函数在路径规划过程中发挥作用成正比。ρ表示的是信息素挥发因子其中η_ij(t)为启发函数，表示为：

式中，J_E代表了为下一可选节点到目标终点的距离代价，式(3)中式中τ_ij(t)代表着在某一时刻t从节点i转移至节点j的信息素，其数值大小对于转移概函数影响较大，表示为：

其中，Q代表了信息素强度，用于表示蚂蚁迭代完一次后路径上所残留的信息素总量，具体的影响是整体算法的收敛速度。L_k代表的是蚂蚁所走的路径总长度。

(3)改进蚁群算法具体实现步骤：

第一步 根据环境变量建立加权Voronoi图并初始化全部参数；

第二步 将总的m只蚂蚁随机放在起始点上并且将其禁忌表清零；

第三步 根据式(3)计算状态转移概率，并且根据计算出的概率为每一只蚂蚁选择概率最大的点；

第四步 将每只蚂蚁新的节点写入路径表中，即将各蚂蚁移动到其选择的新一节点，并将其添加至个蚂蚁的禁忌表；

第五步 如果m只蚂蚁都已到达设定好的目标节点，则跳至第六步，否则跳至第三步继续循环；

第六步 若每只蚂蚁都已到达目标点则记录本次迭代的最优路径，同时更新信息素信息，并且迭代次数加一N_c＝N_c+1；

第七步 若此时迭代次数N_c＝N_cmax，则跳转至第八步，否则继续跳至第三步；

第八步 得到此次循环最优路径；

步骤4 各无人机沿静态规划航路飞行并通过超声波传感器实时监测动态障碍物范围，若未监测到动态障碍物，各无人机则继续沿离线规划路径飞行直至每架无人机均到达目标航点，一旦发现动态障碍物则跳转至步骤5。

步骤5 根据监测到的动态障碍物坐标位置(x_o，y_o)、飞行速度V_o以及无人机飞行一步长时间T建立其膨胀后的动态障碍物模型，具体为：

式中，O表示的是膨胀后动态障碍物圆心位置，即传感器反馈的x_o，y_o坐标，R 表示的是膨胀后动态障碍物的半径大小，T代表的是无人机飞行一步长时间，V_o为传感器反馈的障碍物速度大小。

步骤6 计算监测到动态障碍物的无人机此时受到目标引力F_att，此时的目标航点为离线规划航路上未受动态膨胀后的障碍物影响的下一航点，计算受到的障碍物斥力F_rep，并求出该无人机此时所受的合力F_total，具体公式为：

式中，F_rep为在斥力势场中受到的来自于威胁源的斥力，K_R表示的是障碍物的斥力场系数，X_UAV表示的是无人机当前所在位置，X₀所表示的是威胁源位置， d(X_UAV，X₀)是一个大小为无人机与威胁源所在位置的欧几里得距离|X₀-X_UAV| 且方向为威胁源到无人机位置的矢量，d_m为威胁源斥力势场的最远影响范围，即若无人机离威胁源距离大于d_m则不受斥力影响只受引力影响，无人机在威胁源产生的斥力势场中所受斥力F_rep的方向为由威胁源指向无人机。F_rep随着无人机与威胁源之间距离的减小而增大，并且如果过于靠近威胁源中心会变为无限大，因此要设置最小距离d_min，当无人机与威胁源之间的距离d→d_min时，其产生的斥力F_rep→∞。

F_att(X_UAV)＝K_A×d(X_UAV，X_t) (9)

式中，F_att表示的是无人机所受目标航点的引力，K_A表示的是引力场系数， d(X_UAV，X_t)是一个大小为无人机到目标点的欧几里得距离|X_t-X_UAV|，方向为无人机指向目标点的矢量。

F_total(X_UAV)＝F_att(X_UAV)+F_rep(X_UAV) (10)

式中，F_total表示的是无人机在虚拟势场中受到的合力，无人机在虚拟势场中受到的合力F_total即为无人机在当前位置所规划出的航线及下一航点所指方向。

步骤7 由无人机所受合力F_total可以得出该无人机局部规划航路的下一航点的飞行方向，并飞行一步长，判断是否到达离线规划航路上的航点，若到达，跳转至步骤8，否则继续计算无人机此时所受的合力F_total并继续沿其方向飞行，重复此步骤直至飞行至离线规划航路。

步骤8 无人机继续沿离线规划航路飞行，若再次监测到动态障碍物则跳转至第五步，直至所有无人机飞行至目标航点。

为了验证方法的可行性和有效性，下面结合实例对本发明做进一步详细的描述。

仿真实验在Intel Core i5-3210M，主频2.5GHz处理器，内存为8GB的计算机上进行，操作系统为Windows 10，使用MATLAB2014软件实现。模拟一个多无人机飞行区域为60km×70km二维平面，共有3架多旋翼无人机进行编队飞行，每架无人机最大飞行速度V_max＝45km/h，而多旋翼无人机可以悬停因此无最小速度限制。计算航路代价时，威胁代价及航路燃油代价的系数k₁＝k₂＝0.5，蚁群算法蚂蚁数量共m＝5，最大迭代次数N_cmax＝200，信息素权重因子α＝1，启发函数权重因子β＝1，信息素的挥发系数ρ＝0.7，信息素强度Q＝1这三架无人机编号分别为UAV1、UAV2、UAV3，三架无人机的起始位置分别为(50km，47km)、 (4km，40km)和(60km，18km)，共有16个障碍物按编号1-16分布在目标追踪区域内，具体参数如表1所示。

表1仿真参数设置

仿真实验分为单机传统栅格法蚁群算法规划路径、单机Voronoi改进蚁群算法、多机静态航路规划以及多机动态人工势场法局部路径规划避障四种情况已说明本发明的有效性。

采用传统格栅法的蚁群算法离线路径规划如图1所示，无人机可以避开所有静态障碍物到达目标航点，采用了Voronoi改进蚁群算法的单机离线规划如图2 所示，两者相比较采用了Voronoi改进蚁群算法所规划出的路径相较于传统格栅法蚁群算法规划路径航路长度较短，可以看出Voronoi改进蚁群算法相较于传统格栅法路径规划航路长度短，更为优越。

多机无人机离线规划出的航路如图3所示，障碍物按1-16编号，由图可知，三机均能避开所有静态障碍物到达目标航点，多机动态人工势场法局部路径规划避障是在多机离线规划出的路径基础上，考虑动态障碍物进行实时局部路径规划避障。图中红色航点为原先全局航路规划的航点，黑色点为障碍物及运动方向，绿色航点为局部规划出用以避障的新航点。可以看出采用了人工势场法局部路径规划之后在避开所有静态障碍物的同时还可以实时避开动态障碍物并且到达目标航点。从以上的仿真结果可以看出本发明Voronoi蚁群算法与人工势场法结合的多无人机航路规划及动态避障方法，相较于传统的蚁群算法可以为各无人机规划出路径较短的航路，同时加入了人工势场法局部路径规划，可以避开突然出现的不定向障碍物，大大提升了多无人机编队的飞行安全性。

Claims (4)

1.一种Voronoi蚁群算法与人工势场法结合的多无人机航路规划及动态避障方法，其特征在于，包括以下步骤：

第一步，利用机载摄像头和激光雷达检测目标和障碍物位置。

第二步，根据Voronoi图划分战场并通过改进后的蚁群算法静态规划出各无人机的最优航路。

第三步，无人机沿静态规划航路时通过超声波模块实时监测动态障碍物并建立其膨胀后的模型。

第三步，构建膨胀后的动态障碍物斥力势场，并计算其与无人机的斥力。

第四步，根据无人机当前所受的引力与斥力，求出所受到的合力。

第五步，求出无人机下一时刻的飞行角度，并计算出无人机下一个航点的位置。

第六步，判断无人机是否到达动态障碍物影响斥力范围之外的静态规划航路下一航点，若没有则跳转至第四步直至下一航点为静态规划航路的航点。

第七步，沿静态规划航路继续飞行，若传感器监测到动态障碍物则跳转至第三步。

第八步，判断每架无人机的位置是否为目标所在位置，如果是，则表示所有无人机均到达目标航点，停止航路规划，若未到达则沿静态航路继续飞行直至到达目标点。

2.如权利要求1所述的一种Voronoi蚁群算法与人工势场法结合的多无人机航路规划及动态避障方法，其特征在于，所述第二步中根据Voronoi图划分战场并通过改进后的蚁群算法静态规划出各无人机的最优航路，具体为：

式中，J_threat，i代表了第i条路径上的威胁代价，L_i代表了该条路径的总长度， 分别为该条路径上处到障碍物的距离。

J_fuel，i＝L_i (2)

式中，J_fuel，i代表了无人机飞行燃油代价，只与飞行长度有关，因此用路径长度表示。

式中，表示的是蚁群算法中，无人机k从节点i转移到另一可以选择的节点k的概率，α代表了信息素的重要程度，与信息素浓度在整体航路规划过程中所发挥的作用成正比。β代表了启发函数重要程度，同理，与启发函数在路径规划过程中发挥作用成正比。ρ表示的是信息素挥发因子其中η_ij(t)为启发函数，表示为：

式中，J_E代表了为下一可选节点到目标终点的距离代价，式(3)中式中τ_ij(t)代表着在某一时刻t从节点i转移至节点j的信息素，其数值大小对于转移概率函数影响较大，表示为：

其中，Q代表了信息素强度，用于表示蚂蚁迭代完一次后路径上所残留的信息素总量，具体的影响是整体算法的收敛速度。L_k代表的是蚂蚁所走的路径总长度。

3.如权利要求1所述的一种Voronoi蚁群算法与人工势场法结合的多无人机航路规划及动态避障方法，其特征在于，无人机沿静态规划航路时通过超声波模块实时监测动态障碍物并建立其膨胀后的模型，具体为：

式中，O表示的是膨胀后动态障碍物圆心位置，即传感器反馈的x_o，y_o坐标，R表示的是膨胀后动态障碍物的半径大小，T代表的是无人机飞行一步长时间，V_o为传感器反馈的障碍物速度大小。

4.如权利要求1所述的一种Voronoi蚁群算法与人工势场法结合的多无人机航路规划及动态避障方法，其特征在于，根据无人机当前所受的引力与斥力，求出所受到的合力，具体为：

式中，E_rep为在斥力势场中受到的来自于威胁源的斥力，K_R表示的是障碍物的斥力场系数，X_UAV表示的是无人机当前所在位置，X₀所表示的是威胁源位置，d(X_UAV，X₀)是一个大小为无人机与威胁源所在位置的欧几里得距离|X₀-X_UAV|且方向为威胁源到无人机位置的矢量，d_m为威胁源斥力势场的最远影响范围，即若无人机离威胁源距离大于d_m则不受斥力影响只受引力影响，无人机在威胁源产生的斥力势场中所受斥力F_rep的方向为由威胁源指向无人机。F_rep随着无人机与威胁源之间距离的减小而增大，并且如果过于靠近威胁源中心会变为无限大，因此要设置最小距离d_min，当无人机与威胁源之间的距离d→d_min时，其产生的斥力F_rep→∞。

F_att(X_UAV)＝K_A×d(X_UAV，X_t) (9)

式中，F_att表示的是无人机所受目标航点的引力，K_A表示的是引力场系数，d(X_UAV，X_t)是一个大小为无人机到目标点的欧几里得距离|X_t-X_UAV|，方向为无人机指向目标点的矢量。

F_total(X_UAV)＝F_att(X_UAV)+F_rep(X_UAV) (10)

式中，F_total表示的是无人机在虚拟势场中受到的合力，无人机在虚拟势场中受到的合力F_total即为无人机在当前位置所规划出的航线及下一航点所指方向。