本发明属于机器人技术领域,尤其涉及一种基于改进果蝇优化算法的三维航路规划方法。
背景技术:
航路规划是无人机自主控制的关键组成部分,航路规划算法的目标是在为无人机计算出最优或次最优的飞行路线,这个飞行路线能使无人机躲开复杂地形障碍并突防敌方武器威胁,在以较短的时间到达目标点的同时自我生存。无人机三维航路规划问题属于多目标、多约束的复杂优化问题,通常具有如下特点:1)评价航路性能的指标很多,构成的目标函数难以计算;2)战场环境情况复杂且动态变化;3)无人机自身性能约束条件较多;4)事先已知的信息量不全,而且可能发生变化;5)优化变量多,搜索空间往往具有维数爆炸的特点;6)机上设备有限,允许存储的信息量较少并且机载计算机计算速度有限。因此,无人机航路规划的一个关键挑战就是如何处理无人机自身物理特性带来的多种复杂约束,克服变量过多带来的维数爆炸问题,并在较短时间内快速的得出满足各种限制条件且性能指标接近最优的航路。
国内外求解无人机航路规划问题的方法大体可以分为传统规划算法和智能规划算法。其中,传统规划算法有动态规划法、以voronoi图为代表的基于图形的方法、以a星算法为代表的基于栅格的方法等;智能规划方法有神经网络方法、人工势场方法、进化计算及群体智能方法等。从这些规划算法的特点和航路规划问题的要求来看,以进化计算及群体智能为代表的人工智能技术,已成为求解无人机航路规划问题的发展趋势,包括遗传算法(geneticalgorithm,ga)、微粒群算法(particleswarmoptimization,pso)、蚁群优化算法(antcolonyoptimization,aco)、人工蜂群算法(artificialbeecolony,abc)、微分进化算法(differentialevolution,de)、万有引力算法(gravitationalsearchalgorithm,gsa)、智能水滴算法(intelligentwaterdrops,iwd)、生物地理优化(biogeography-basedoptimization,bbo)以及memetic算法等。尽管这些航路规划技术正进一步向智能化、实用化方向发展,但仍各自存在着诸多问题。例如,a星算法搜索空间需求太大,计算时间过长;神经网络算法收敛速度慢,得到的航路可能只是局部最优解而非全局最优;人工势场法的收敛速度都具有较大的不确定性,收敛速度受地形轮廓影响较大;进化计算及群体智能方法的控制参数难于选择,性能不稳定、随机性较强,容易出现过早收敛和停滞现象。因此,现有的方法均没能真正有效地从实际意义上解决无人机三维航路规划问题,需要进一步发展更加优越的规划方法
果蝇优化算法(fruitflyoptimizationalgorithm,foa)一种基于果蝇觅食行为的启发式全局最优化算法。果蝇在感官知觉,尤其是嗅觉与视觉上,优于其他物种。果蝇的嗅觉器官能够很好的搜集到空气中的各种气味,甚至能嗅到很远距离以外的食物源。当果蝇靠近食物位置后,能够使用敏锐的视觉器官发现食物的具体位置与同伴的聚集位置,并且朝该方向飞去。与其它群体智能算法相比,果蝇优化算法的步骤更加简单,在编程时只需要很少的程序语句便可描述出核心算法程序。但是原始的果蝇优化算法存在着编码方式适用性限制多、易陷于局部最优等缺点,因此在应用果蝇优化算法解决三维航路规划问题时需要对原始算法进行改进。本发明针对无人机自身特点,在对原始果蝇优化算法进行改进的基础上,设计用于求解无人机三维航路规划问题的计算方法。与现有航路规划方法相比,本发明所提出的方法更加简单高效、计算速度快。与原始果蝇优化算法相比,本发明提出的方法更加稳定,得到的航路具有更优的代价值。该方法是解决复杂战场态势下无人机三维航路规划的有效技术途径,也可应用于地面机器人路径规划、城市交通规划等技术领域。
技术实现要素:
本发明的目的是提供一种基于改进果蝇优化算法的三维航路规划方法,在考虑复杂态势与无人机性能约束的前提下,为无人机计算出最优或次优三维飞行航路。
为实现上述目的,本发明采用如下的技术方案:
一种基于改进果蝇优化算法的三维航路规划方法,首先确定无人机飞行任务信息,包括起点坐标(xs,ys,zs)t和终点坐标(xt,yt,zt)t,任务地图边界,需要规划出的航路控制点个数n;确定敌方地面武器信息,包括威胁类型(雷达、导弹、高炮),武器位置(xthreatj,ythreatj)t,以及各自的威胁范围;设定改进果蝇算法的相关参数,包括最大迭代次数gmax,嗅觉搜索次数mosp,以及变异概率pm,其包括以下步骤:
步骤一:设迭代次数nc=1,随机初始化相位角向量θaxis=[θaxis,1,λ,θaxis,d],其中每一维相位角取值都在[-π/2,π/2]内,且d=3n,并设定相应的初始味道浓度判定值smellaxis=+∞;
步骤二:按照如下公式计算果蝇的搜索半径
步骤三:执行果蝇嗅觉搜索操作,令i=1,j=1;
步骤四:在0到1区间上随机产生一个实数rj,若rj<pm,则转步骤五;若rj≥pm,则转步骤六;
步骤五:执行变异操作,即计算
步骤六:不执行变异操作,即计算
步骤七:计算此次嗅觉搜索的味道浓度判定值如下
其中,smax,j和smin,j分别为搜索空间的上下边界;
步骤八:令j=j+1,若j≤d,则转到步骤五,否则,转到步骤十;
步骤九:利用航路起点和终点,以及向量[si,j,si,n+j,si,2n+j]生成完整的航路puav,i;
步骤十:计算航路曲线的味道浓度值(航路代价值)
步骤十一:令i=i+1,若i≤mosp,则令j=1,并转到步骤四;否则,转到步骤十二;
步骤十二:执行果蝇视觉搜索操作,选出具有最小味道浓度值的个体,即该个体的索引号为
步骤十三:若
步骤十四:更新迭代次数nc=nc+1,若迭代次数nc≤gmax,则转到步骤二;若迭代次数nc>gmax,则将θaxis转换为saxis=[saxis,1,saxis,2,λ,saxis,d],进而生成无人机飞行航路,并输出最终的规划结果。
作为优选,对果蝇的位置采用的是相位角编码方式,即用相位角向量θaxis=[θaxis,1,θaxis,2,λ,θaxis,d]来表示果蝇所处的位置,每一维上的相位角均在区间[-π/2,π/2]上。用smin,
saxis=((smax-smin)sinθaxis+smax+smin,)/2(3)
作为优选,在步骤九和步骤十四中所述生成无人机航路的方式,其详细描述及计算方式如下:
设果蝇位置浓度判定值向量为[s1,s2,λ,sd],利用该向量确定出为控制点w1,w2,…,wn,其中每个控制点的三维坐标为wj=(xj,yj,zj)t=(sj,sn+j,s2n+j)t。另外,取任务起点为控制点w0,终点为控制点wn+1。
将无人机航路表示为n+2个离散点的集合puav:{p0,p1,p2,λ,pn,pn+1},其中p0和pn+1分别表示任务起点与终点,且每一个航路点pk的三维坐标为(xk,yk,zk)t。利用控制点wi,i=1,2…,n+2,计算航路点pk坐标的方式如下:
式中,bi,k(k)为航路弯曲函数,由下式确定:
且
式中,k为航路的阶次,表征了航路的平滑度;参数k以固定的步长从0变化到n-k+3。
作为优选,在步骤十中所述的航路代价值j(puav),考虑航路长度、飞行高度、敌方武器威胁、地图限制、转弯角限制、爬升/下滑限制、地形限制等,其计算过程如下:
式中,fi,i=1,2,λ,7分别为航路长度代价jl,飞行高度代价jh,威胁代价jt,任务地图溢出代价jmap,转弯代价jturn,爬升/下滑代价js,地形代价jter。
其中,
jl是整条飞行航路的长度,通过下式进行计算:
jh为飞行高度沿航路的积分,通过下式进行计算:
式中,zp为航路点p的绝对高度,hter(zp)为航路点p下方的地面高度。
jt为各个航路段受到敌方威胁程度的总和,通过下式进行计算:
式中,pj,k为第j个地面武器对航路段pkpk+1造成的威胁概率,计算方式如下:
若地面武器为雷达,则威胁概率如下
式中,
式中,αz=sinψe,βz=cosψe,αφ=sinφe,βφ=cosφe,其中ψe为无人机运动方向与其相对于雷达的位置矢量之间的夹角,φe=φ-arctan(tanθ/sinψ)且φ、θ、ψ分别为无人机的位置相对于雷达的滚转角、仰角、方位角。
若地面武器为地空导弹,则威胁概率如下
式中,rmmax为地空导弹的最大打击范围,d为无人机到导弹中心的距离。
若地面武器为高炮,则威胁概率如下
式中,rg和rgmax分别为高炮的有效打击范围和最大打击范围,d为无人机和防空高炮之间的水平距离。
jmap为任务地图溢出代价,通过下式进行计算:
式中,
jturn为航路转弯代价,通过下式进行计算:
式中,
js为爬升/下滑代价,通过下式进行计算:
式中,
jter为地形代价,通过下式进行计算:
式中,hsafe为无人机距地面的最小安全飞行高度,hter(xk,yk)为第k个航路点处的地面高度。
本发明的有益如下:
本发明提出了一种基于改进果蝇优化算法的三维航路规划方法,该方法控制参数少,更加简单且易于实现,无论使用何种编程语言,核心算法程序都仅需要少数的代码语句便可完成期望的计算。在果蝇的嗅觉搜索过程引入了变异适应操作,有利于果蝇从最优个体解中继承优秀的元素。同时,在算法中设定果蝇嗅觉搜索范围随着迭代的进行而逐渐缩小,从而提高算法的收敛能力。本发明所提出的这种基于改进果蝇优化算法的三维航路规划方法增加了航路搜索的多样性,既提高了算法的收敛速度,又克服了陷于局部最优的缺点,提高了获得最优航路解的概率。该航路规划方法使无人机能突破敌方地面武器的威胁,在满足自身与环境限制的同时能以较短时间到达任务终点。本发明能够有效地解决无人机三维航路规划问题,也可用于其它多维函数优化问题。
附图说明
图1为相位角映射函数曲线;
图2果蝇嗅觉搜索范围随迭代次数变化的曲线;
图3改进果蝇优化算法的流程图;
图4本发明所提方法得到的无人机飞行航路的三维视图;
图5本发明所提方法得到的无人机飞行航路的俯视图;
图6本发明所提方法得到的无人机飞行航路的高度随航路长度的变化曲线;
图7本发明所提方法的航路代价进化曲线与原始果蝇算法的对比;
图中标号及符号说明如下:
s——果蝇的味道浓度判定值
θ——相位角
gmax——最大迭代次数
nc——当前计算迭代
rnc——第nc次计算迭代的果蝇嗅觉搜索半径
i——第i次嗅觉搜索
j——第j维的变量
d——变量维数
rand——0到1区间上的随机数
pm——变异概率
θaxis——果蝇位置
smellaxis——果蝇位置的味道浓度值
mosp——果蝇最大嗅觉搜索次数
具体实施方式
本发明实施例提供一种基于改进果蝇优化算法的三维航路规划方法,利用通过对原始果蝇优化算法做出改进,用来求解复杂地势环境与战场威胁态势下的无人机三维航路规划,其步骤如下。
预备工作:
确定无人机飞行任务信息,包括起点坐标(xs,ys,zs)t和终点坐标(xt,yt,zt)t,任务地图边界,需要规划出的航路控制点个数n;
确定敌方地面武器信息,包括威胁类型(雷达、导弹、高炮),武器位置(xthreatj,ythreatj)t,以及各自的威胁范围。
设定改进果蝇算法的相关参数,包括最大迭代次数gmax,嗅觉搜索次数mosp,以及变异概率pm。
步骤一:设迭代次数nc=1,随机初始化相位角向量θaxis=[θaxis,1,λ,θaxis,d],其中每一维相位角取值都在[-π/2,π/2]内,且d=3n,并设定相应的初始味道浓度判定值smellaxis=+∞。
步骤二:按照如下公式计算果蝇的搜索半径
步骤三:执行果蝇嗅觉搜索操作,令i=1,j=1。
步骤四:在0到1区间上随机产生一个实数rj,若rj<pm,则转步骤五;若rj≥pm,则转步骤六。
步骤五:执行变异操作,即计算
步骤六:不执行变异操作,即计算
步骤七:计算此次嗅觉搜索的味道浓度判定值如下
其中,smax,j和smin,j分别为搜索空间的上下边界。
步骤八:令j=j+1,若j≤d,则转到步骤五,否则,转到步骤十。
步骤九:利用航路起点和终点,以及向量[si,j,si,n+j,si,2n+j]生成完整的航路puav,i。
步骤十:计算航路曲线的味道浓度值(航路代价值)
步骤十一:令i=i+1,若i≤mosp,则令j=1,并转到步骤四;否则,转到步骤十二。
步骤十二:执行果蝇视觉搜索操作,选出具有最小味道浓度值的个体,即该个体的索引号为
步骤十三:若
步骤十四:更新迭代次数nc=nc+1,若迭代次数nc≤gmax,则转到步骤二;若迭代次数nc>gmax,则将θaxis转换为saxis=[saxis,1,saxis,2,λ,saxis,d],进而生成无人机飞行航路,并输出最终的规划结果。
其中,上述算法步骤中对果蝇的位置采用的是相位角编码方式,即用相位角向量θaxis=[θaxi,s1,θaxi,s2,λ,θaxi,sd]来表示果蝇所处的位置,每一维上的相位角均在区间[-π/2,π/2]上。用smin,
saxis=((smax-smin)sinθaxis+smax+smin,)/2(22)
其中,在步骤一中所述的随机初始化,是指在算法的初始化阶段,将果蝇的相位角θaxis=[θaxi,s1,θaxi,s2,λ,θaxi,sd]在[-π/2,π/2]上随机取值,按照下式进行计算:
θaxis,j=random(-π/2,π/2),j=1,2,λ,d(23)
式中的random(·)表示在区间内按照均匀分布选取的随机实数。
其中,在步骤二中所述果蝇的搜索半径rnc,随迭代次数的增加而逐渐缩小,如附图2所示。
其中,在步骤九和步骤十四中所述生成无人机航路的方式,其详细描述及计算方式如下:
记算法得到的果蝇位置浓度判定值向量为[s1,s2,λ,sd],利用该向量确定出为控制点w1,w2,…,wn,其中每个控制点的三维坐标为wj=(xj,yj,zj)t=(sj,sn+j,s2n+j)t。另外,取任务起点为控制点w0,终点为控制点wn+1。
将无人机航路表示为n+2个离散点的集合puav:{p0,p1,p2,λ,pn,pn+1},其中p0和pn+1分别表示任务起点与终点,且每一个航路点pk的三维坐标为(xk,yk,zk)t。利用控制点wi,i=1,2…,n+2,计算航路点pk坐标的方式如下:
式中,bi,k(k)为航路弯曲函数,由下式确定:
且
式中,k为航路的阶次,表征了航路的平滑度;参数k以固定的步长从0变化到n-k+3。
其中,在步骤十中所述的航路代价值j(puav),考虑航路长度、飞行高度、敌方武器威胁、地图限制、转弯角限制、爬升/下滑限制、地形限制等,其计算过程如下:
式中,fi,i=1,2,λ,7分别为航路长度代价jl,飞行高度代价jh,威胁代价jt,任务地图溢出代价jmap,转弯代价jturn,爬升/下滑代价js,地形代价jter。
其中,
jl是整条飞行航路的长度,通过下式进行计算:
jh为飞行高度沿航路的积分,通过下式进行计算:
式中,zp为航路点p的绝对高度,hter(zp)为航路点p下方的地面高度。
jt为各个航路段受到敌方威胁程度的总和,通过下式进行计算:
式中,pj,k为第j个地面武器对航路段pkpk+1造成的威胁概率,计算方式如下:
若地面武器为雷达,则威胁概率如下
式中,
式中,αz=sinψe,βz=cosψe,αφ=sinφe,βφ=cosφe,其中ψe为无人机运动方向与其相对于雷达的位置矢量之间的夹角,φe=φ-arctan(tanθ/sinψ)且φ、θ、ψ分别为无人机的位置相对于雷达的滚转角、仰角、方位角。
若地面武器为地空导弹,则威胁概率如下
式中,rmmax为地空导弹的最大打击范围,d为无人机到导弹中心的距离。
若地面武器为高炮,则威胁概率如下
式中,rg和rgmax分别为高炮的有效打击范围和最大打击范围,d为无人机和防空高炮之间的水平距离。
jmap为任务地图溢出代价,通过下式进行计算:
式中,
jturn为航路转弯代价,通过下式进行计算:
式中,
js为爬升/下滑代价,通过下式进行计算:
式中,
jter为地形代价,通过下式进行计算:
式中,hsafe为无人机距地面的最小安全飞行高度,hter(xk,yk)为第k个航路点处的地面高度。
下面通过一个具体实例描述本发明所提基于改进果蝇优化算法的三维航路规划方法的实施方式,并验证本发明所提方法的性能。实例所用计算软件为matlab2009a,具体实施流程如附图3所示,详细实施步骤如下(步骤中的长度单位均为km)。
确定无人机飞行任务信息:起点坐标(5,10,zs)和终点坐标(85,85,zt),任务地图边界,需要规划出的航路控制点个数n=5,其中的zs和zt分别为起点和终点处的地面高度。
确定敌方地面武器信息:高炮武器位置坐标(45,45),威胁范围9;两个导弹武器位置坐标分别为(35,70)和(20,41),威胁范围均为8;两个雷达位置坐标分别为(40,25)和(75,60),威胁范围均为15.
设定改进果蝇算法的相关参数:最大迭代次数gmax=400,嗅觉搜索次数mosp=40,变异概率pm=0.9.
步骤一:设迭代次数nc=1,按照公式θaxis,j=random(-π/2,π/2),j=1,2,λ,d随机生成初始的相位角向量θaxis=[θaxis,1,λ,θaxis,d],使得向量中的每一维相位角取值都在[-π/2,π/2]内,且向量维数d=3n=15,并设定相应的初始味道浓度判定值smellaxis=+∞。
步骤二:按照如下公式计算果蝇的搜索半径
步骤三:执行果蝇嗅觉搜索操作,令i=1,j=1。
步骤四:在0到1区间上随机产生一个实数rj,若rj<pm,则转步骤五;若rj≥pm,则转步骤六。
步骤五:执行变异操作,即计算
步骤六:不执行变异操作,即计算
步骤七:计算此次嗅觉搜索的味道浓度判定值如下
其中,smax,j和smin,j分别为任务地图的上下边界。
步骤八:令j=j+1,若j≤d,则转到步骤五,否则,转到步骤十。
步骤九:令w0坐标为航路起点坐标,即w0=(x0,y0,z0)=(xs,ys,zs),
wn+1坐标为航路终点坐标,即wn+1=(xn+1,yn+1,zn+1)=(xt,yt,zt),
wj坐标为
利用点列w0,w1,w2,…,wn+1,通过下面的公式计算出航路点pk的三维坐标(xk,yk,zk)t,
式中,取k=3,参数k以固定的步长从0变化到n-k+3,且
且
knot(i)=0,若i<k
knot(i)=i-k+1,若k≤i≤n+1
knot(i)=n-k+3,若n<i-1
通过上述计算,得出完整的无人机航路点序列puav,j:{p0,p1,p2,λ,pn,pn+1}。
步骤十:计算出航路曲线puav,i的航路代价值,并将其作为向量[si,j,si,n+j,si,2n+j]的味道浓度值
通过下式进行计算出航路的长度代价jl:
通过下式计算出航路的飞行高度代价jh:
式中,zp为航路点p的绝对高度,hter(zp)为航路点p下方的地面高度。
通过下式计算各个航路段受到敌方威胁程度的总和jt:
式中,nt=5为地面武器威胁的个数,pj,1和pj,2为两个雷达武器对航路段pkpk+1造成的威胁概率,计算方式如下:
式中,k=1,2,
式中,αz=sinψe,βz=cosψe,αφ=sinφe,βφ=cosφe,其中ψe为无人机运动方向与其相对于雷达的位置矢量之间的夹角,φe=φ-arctan(tanθ/sinψ)且φ、θ、ψ分别为无人机的位置相对于雷达的滚转角、仰角、方位角。
pj,3和pj,4为两个导弹武器对航路段pkpk+1造成的威胁概率,计算方式如下:
式中,k=3,4,rkmax为地空导弹的最大打击范围,d为无人机到导弹中心的距离。
pj,5高炮武器对航路段pkpk+1造成的威胁概率,计算方式如下:
式中,k=5,rk和rkmax分别为高炮的有效打击范围和最大打击范围,d为无人机和防空高炮之间的水平距离。
通过下式计算航路的任务地图溢出代价jmap,:
式中,
通过下式计算航路转弯代价jturn:
式中,
通过下式计算航路的爬升/下滑代价js:
式中,
通过下式航路的地形代价jter:
式中,hsafe=0.03km,hter(xk,yk)为第k个航路点处的地面高度,通过任务地图的地形数据得到。
综合上面的航路代价值,计算puav,i的总的代价如下:
j(puav,i)=jl+jh+jt+jmap+jturn+js+jter
步骤十一:令i=i+1,若i≤mosp,则令j=1,并转到步骤四;否则,转到步骤十二。
步骤十二:执行果蝇视觉搜索操作,选出具有最小味道浓度值的个体,即该个体的索引号为
步骤十三:若
步骤十四:更新迭代次数nc=nc+1,若迭代次数nc≤gmax,则转到步骤二;若迭代次数nc>gmax,则将θaxis转换为saxis=[saxis,1,saxis,2,λ,saxis,d],进而生成无人机飞行航路,并输出最终的规划结果。
附图4显示了通过上述计算步骤得到的无人机航路的三维立体视图,附图5为无人机航路的俯视图,其中同心圆为敌方地面武器的威胁范围,附图6为得到的航路高度随着航路长度的变化情况。从结果图中可以看出,本发明所提算法得到的航路成功地绕开了所有威胁,为无人机规划出一条平滑有效的飞行路径。附图7为计算过程中得到的最优航路代价值随迭代次数变化的曲线,图中将本发明所提的改进果蝇算法与原始的果蝇算法进行了对比,显示出本发明所提算法的收敛速度、优化能力上均优于原始算法。
本发明所提基于改进果蝇优化算法的三维航路规划方法为无人机战场航路规划问题提供了解决方案,同时也为高维函数优化问题提供了非常有效的途径,可广泛应用于机器人、航空、航天、工业生产等涉及高维函数优化问题的领域。