王 庆, 徐海明, 吕 品, 蒋 锐, 苗东东

(1.合肥工业大学 工业与装备技术研究院,安徽 合肥 230009; 2.中国科学院 合肥智能机械研究所,安徽 合肥 230031; 3.中国科学院 合肥技术创新工程院,安徽 合肥 230088)

随着无人机技术的不断发展成熟,特别是体积小且廉价的微机电系统(micro-electro-mechanical system,MEMS)惯性导航系统的成功研制,无人机正逐步从军工领域向民用领域过渡。其中多旋翼无人机仅通过改变螺旋桨转速就能实现上下、前后、左右、偏航、横滚、俯仰运动,同时结构简单、安全可靠,现已成为当下最主流的飞行器,被广泛应用于航拍、电力巡检、警用安防、农业植保、地图测绘等领域。

航迹规划是无人机实现自主飞行的关键技术之一,国内外学者已经对其做了大量的研究工作,也取得了很多成果。航迹规划实际上就是在满足一定的约束条件下,找到一条从初始点到目标点最优无碰撞的航行路线。常见的航迹规划方法包括A*算法[1-3]、人工势场法[4-5]、最优控制法、可视图法、Voronoi图法[6]等传统算法以及遗传算法[7]、蚁群算法、粒子群算法[8]等智能算法。

传统的航迹规划算法以A*算法和人工势场法最为典型。由于借助了启发式函数的引导,A*算法通常能拥有更好的性能,但是该算法的运算难度会随着维数的增加呈指数型的增长,因此一般只适用于简单的二维空间的路径规划;人工势场法在复杂环境中容易发生震荡且易陷入局部最优解;其他传统算法针对三维的路径规划都有一定的局限性。随着人工智能算法的发展,蚁群算法、粒子群算法、遗传算法、神经网络等方法因具有很强的鲁棒性、正反馈机制和自组织特点,更适用于三维路径规划计算。文献[9]将最短距离信息发送给系统进行控制,并通过改进状态转移概率的计算来优化节点选择方法;文献[10]提出了一种信息素调节的方法,将航路上残留信息素限制在一定范围内,从而避免因信息素过于集中导致的蚁群算法早熟;文献[11]将三维规划分解成二维的平面规划和高度规划,采用自适应调整系数和几何优化的方法来提升蚂蚁的个体交互能力和相互引导性,但其规划空间仍然是准三维环境;文献[12]将当前的最优与最差路径引入信息素的奖罚机制来更新全局信息素,但只是将航迹距离作为影响因子,最终规划的路径折线偏多;文献[13]采用当前最优航迹信息素和历史最优航迹更新相融合的方法,引入信息素回退清零机制并进行离线计算,虽然提高了搜索效率,但是迭代次数没有显著降低。

本文提出的三维航迹规划算法充分考虑了起始点到目标点的直线因素,优化初始信息素浓度作为蚁群搜索的先验知识;提出了基于时间与空间的自适应挥发系数,加快了收敛速度并保证了搜索范围;在分析多旋翼无人机的飞行约束条件后,将前、后节点的转角值和节点对于“理想路径”的偏移值加入启发函数计算,在缩短航迹长度的同时,尽可能减少各段航迹转角和。最后通过仿真实验验证了本文算法的有效性。

1 航迹规划问题建模

1.1 航迹的优化问题描述

本文采用的航迹规划策略是从起始点到目标点顺序生成若干节点,在节点处航迹可能存在转角,若转角过大,则允许无人机悬停并调整航向至下一节点,再启动飞行,因此多旋翼无人机可以很好地跟随任意轨迹,而忽略转弯角和爬升角的限制。

与固定翼相比,多旋翼最大的缺点是续航时间短,并且多旋翼通常在低空的复杂环境下低速飞行,飞行距离有限,航迹较为复杂。因为轨迹上的折点会造成运动的不连续,导致无人机频繁地降速至悬停再启动加速,所以相同长度的2段轨迹折点较多的花费的时间会更长,能耗更高。对于多旋翼无人机的航迹规划,转弯角和爬升角只作为航线优化参数,不同于固定翼那样有数值大小的限制。

1.2 三维空间环境建模

本文建立的无人机飞行的三维空间模型如下:

z1(x,y)=sin(y+a)+bsinx+ecosy+

(1)

(2)

z(x,y)=max(z1(x,y),z2(x,y))

(3)

其中:x、y分别为数字地图模型中点在水平面投影的坐标;z为地图坐标点(x,y)点的高度;a、b、c、d、e、f、g均为常数;n为环境中山峰的个数;(xi,yi)为第i个山峰的峰顶坐标;hi为峰顶的高度;xis,yis分别为第i个山峰沿x轴和y轴方向上的衰减量,用来控制山体的坡度。

采用(1)式建立基准地形模型来模拟较平坦的地面,采用(2)式指数函数模拟无人机飞行中遇到的较高山体,再将(1)式、(2)式的数据融合得到(3)式,即形成了最终无人机飞行的较为复杂的三维空间模型。利用(1)～(3)式在Matlab中建立三维仿真地图,用于无人机规划航迹的模拟实验。

1.3 面向蚁群算法的空间离散化

参考文献[14]，将复杂的连续空间离散为若干个空间点。假设飞行的起始点坐标为S(x0,y0,z0),目标点坐标为T(xd,yd,zd),可以得知起始点到目标点在x轴方向、y轴方向和z轴方向的距离分别为|xd-x0|、|yd-y0|、|zd-z0|;取这三者最大值作为运动的主方向(本文假设为x轴正方向),即蚂蚁移动的主方向,沿此方向将规划空间进行n等分,得到n+1个平面Mi:x=xi(i=0,1,2,…,n),再分别沿y轴方向和z轴方向将平面Mi(i=0,1,2,…,n)进行m等分和l等分;最终得到三维离散化的规划空间图形。该空间是由(n+1)×(m+1)×(l+1)个空间点组成的集合,这些空间点可以表示为p(i,j,k) (i=0,1,…,n;j=0,1,…,m;k=0,1,…,l),从而得到任意空间点的坐标为(ixgrid,jygrid,kzgrid),其中,xgrid、ygrid、zgrid为各个方向等分后的间距。

1.4 蚂蚁移动规则

为了简化空间规划的复杂程度,规定蚂蚁沿着主方向进行的移动必须顺序经过每个平面Mi(i=1,2,…,n),并且限制了另2个方向的移动量(ymax≤p=1,zmax≤q=1),从而简化了蚁群的搜索空间,减少了计算机进行数据处理的运算量和储存量,进而提高蚁群算法的搜索效率。

无人机在实际环境中飞行时,起始点到目标点在高度方向上的距离比经纬度方向上的距离短。现假设在仿真中将x轴方向作为运动的主方向,起始点与目标点在x轴方向上的距离与在y轴方向上的距离相差不大,且都大于z轴方向上的位移,则蚂蚁在z轴方向移动的过程中会有更多的灵活性;相反,在y轴方向的移动则需要更多地考虑始终向目标点靠近,否则蚂蚁在移动到最后阶段,在y方向上距离目标点有较大差距,最终导致蚂蚁会在每次搜索的最后一步,即由平面Mn-1到平面Mn移动时会横跨多个栅格,虽然这2个节点都属于无障碍点,但是这2点的连线可能会穿过障碍点,导致航迹规划失败。文献[15]将2个副运动方向上每步可移动的最大移动量定为p=q=2,该方法虽然增加了蚂蚁移动的灵活性,在复杂环境中有更好的适应能力,但是可能会造成航迹转弯过大、运动不连续等问题。

为了解决这些问题,本文提出将起始点到目标点连线在水平面上的投影作为主方向,因此要对坐标系进行旋转,为了保证旋转后起始点仍位于新坐标系下第1个平面M0′内,选择绕起始点S旋转坐标系O-XYZ至x轴与起始点和目标点连线平行,旋转后的x轴即为运动的主方向,旋转得到新的坐标系记作O-X′Y′Z′。规划空间中离散点在新的坐标系下的坐标值为:

(4)

其中:(x,y,z)为任意空间点在原坐标系下的坐标;(x′,y′,z′)为该点在新坐标系下的坐标;(x0,y0)为起始点在水平面的坐标;θ为顺时针旋转的角度。

最后,将O-X′Y′Z′下规划的航迹点通过逆运算还原成O-XYZ下的坐标,并形成最终规划航迹。

2 基于改进蚁群算法的航迹规划

2.1 蚁群算法的理论基础

蚁群在移动并选择下一节点时会遵循一定的概率,此概率可以表示为:

(5)

其中:τ为节点的信息素浓度;η为启发信息。

将N只蚂蚁置于起始点,通过(5)式分别计算每只蚂蚁下一个可行节点的概率,通过轮赌法按概率随机选择下一节点,直至蚂蚁最终到达目标点。当N只蚂蚁全都到达目标点以后,根据每只蚂蚁运动的路线长度更新节点的信息素浓度值,重新计算转移概率值P,重复上述步骤Nmax次,最终得到最优路径bestpath。

2.2 初始信息素的高斯分布

因为传统蚁群算法在处理旅行商问题(traveling salesman problem,TSP)时每个城市都必须经过,所以任意一条边的初始信息素取值都相同;而三维环境中空间点的数量要远远大于蚂蚁路过的节点数,如果信息素在空间上分布均匀,那么蚂蚁在初始阶段搜索的范围太广且无目标指向,会导致迷失方向的蚂蚁数量增加,影响航迹规划效果。文献[12]采用的初始信息素会使在直线上的节点浓度过大,而离直线ST距离远的节点信息素浓度又偏小,影响了蚂蚁的搜索范围。

因此本文考虑了空间节点“理想最优路径”,即初始点到目标点连线ST的距离因素,设计空间节点的初始信息素τ表达式如下:

τ=τ0exp(-l2)

(6)

其中:τ0为一常数,代表直线ST上节点的初始信息素,同时也是初始信息素最大值;l为空间上任一节点P到直线ST的距离,即

初始信息素浓度呈高斯分布的设计增强了蚁群在靠近直线ST附近节点的搜索力度,有利于减少蚂蚁搜索的盲目性,大幅度提高了第1代蚂蚁搜索的质量,能够集中蚂蚁沿直线搜索快速收敛到最短路径;同时也保证了蚁群的搜索范围,不至于过早地陷入局部最优。

2.3 信息素的更新策略

蚁群在觅食过程中需要信息素的引导,而信息素的更新策略很大程度上影响着路径规划的收敛速度。信息素更新包括局部信息素更新和全局信息素更新,其中局部信息素更新是指蚂蚁每经过一个节点即对其进行更新,目的是降低其他蚂蚁重复经过该节点的概率。局部信息素的表达式为:

τijk=(1-α)τijk

(7)

当所有蚂蚁完成一轮路径搜索后,结合每只蚂蚁所走的路径长度,更新全局信息素,表达式如下:

(8)

其中:ρ为全局信息素挥发系数;Fm为第m只蚂蚁走的路径综合评价值;Q为常数。

三维空间规划问题规模较大,由于挥发系数ρ的作用使得未被搜索的节点上的信息素减小过快,降低了全局的搜索能力;相反,挥发系数ρ过小,会使蚂蚁已搜索的节点信息素过大而易陷入局部最优。文献[15]提出了二维空间下的基于时间和空间的自适应挥发系数,基于时间的挥发系数ρ(t)是为了避免算法陷入局部最优,而基于空间的挥发系数ρ(l)将搜索区域划分为重要区域、核心区域和非核心区域。本文将该方法向三维空间进行扩展,使其既能进一步加快算法的收敛速度,又能提高蚁群算法的全局搜索能力。其表达式如下:

ρ(t)=1.05ρ(t)

(9)

(10)

其中:F(x,y,z)=0为空间直线ST的方程;Mmid为蚂蚁主运动方向的中间分割面;δ为常数,用于平衡ρ(t)和ρ(l)的大小。

将基于时间和基于空间上的信息素挥发系数相融合,同时为其设定了上、下限,得到的挥发系数表达式如下:

(11)

2.4 启发函数的设计

蚂蚁选择节点概率的另一个影响因素为启发值,启发函数反应当前节点到下个可行节点的能见度和期望程度,直接决定了蚂蚁寻找航迹的快慢和准确度。在TSP问题中启发值一般取为1/dij,dij表示从i点到j点的距离。但是三维空间中的航迹规划区别于TSP问题,航线不需要经过每个节点,航迹规划的线路更加趋向于寻找最佳方向,方向优则线路短,并且栅格中可通行的节点间的距离比较固定,用1/dij表示启发值区分度不大。文献[16]将节点到目标点的距离作为启发值,但是忽略了相邻节点的距离,实验证明此方法规划的路径折线较多,且容易陷入局部最优。

基于上述原因,本文设计了基于方向最优的启发函数η,综合考虑了上述2种距离,并引入了方向启发信息Φijk,减少了蚂蚁转弯的概率,具体表达式如下:

ηijk=DijkΦijkHijk

(12)

(13)

dijk=

(14)

Φijk=exp(cosθijk-1)

(15)

(16)

其中:下标a表示当前节点A;下标b表示下一个节点B;r为常数;dijk为B点到直线AT的垂直距离,用于检测蚂蚁对理想路线的偏移量;θijk为航迹在该点的转角;Φijk为角度启发信息,引入该项可以减少蚂蚁的阶梯式搜索;Hijk为保证蚂蚁仅在可行节点上移动。

2.5 三维航迹综合评定函数

为了简化计算,这里不区分转弯角和爬升角,结合1.4节的内容,通过对航迹在XY和XZ面上投影的各节点转角进行编码,计算出节点间的三维真实转角。

当xgrid=ygrid=zgrid时,当前节点在XY(XZ)面上的Y轴(Z轴)坐标值为c0,前一节点坐标值为c1,下一节点坐标值为c2,则有:

(1) 若{c1,c0,c2}={C,C,C}∪{C-1,C,C+1}∪ {C-1,C,C+1},则航迹上当前节点在该投影面上的转角记为0(0°)。其中，C为任一正整数。

(2) 若{c1,c0,c2}={C,C,C+1}∪{C-1,C,C}∪ {C+1,C,C}∪{C,C,C-1},则航迹上当前节点在该投影面上的转角记为1(45°)。

(3) 若{c1,c0,c2}={C,C+1,C}∪{C+1,C,C+1},则航迹上当前节点在该投影面上的转角记为2(90°)。

三维轨迹各线段在空间的真实转角见表1所列。

表1 三维航迹空间转角编码计算

本文将转角和航程作为衡量航迹优劣的参数,并提出了航迹的综合评价函数如下:

(17)

其中,Li、θi分别为第i段的长度和夹角。航路评价表达式(17)兼顾了飞行长度和转折量,能够选出相对最优航迹。

2.6 三维航迹的平滑处理

上述改进算法规划的航迹是由若干条线段组成的,线段之间相交且不连续。为了进一步优化航线,应将规划航迹作平滑处理。

B样条曲线因为凸包性、易于局部修改、更逼近原路径等优点,在工程中被广泛运用。此外,B样条曲线在折线连接处二阶连续,速度与加速度都是连续的,可以减少多旋翼无人机的悬停,提高飞行效率。

本文采用三阶B样条对航迹进行曲线拟合[17-18],曲线方程表达式为:

(18)

其中,t∈[0,1]。

事实证明,三次B样条曲线能呈现出较好的平滑效果。

3 仿真实验与结果分析

为了验证所提出的改进蚁群算法的航迹规划性能,本文进行了仿真实验。

实验参数如下:蚁群个数为30;起始点、目标点分别为(0,10,50)、(50,40,20);ρ=α=0.2,ρmin=0.1,ρmax=0.3;τ0=1;Q=50;迭代次数Nmax=200次。环境地图建模所使用的参数见表2所列。

表2 数字地图参数

蚁群算法航迹规划的三维空间轨迹如图1所示。

从图1可以看出:传统的蚁群算法规划的三维航迹比较“蜿蜒崎岖”,在无障碍处多次出现U或V字型折线,增加了航程和转角,影响了无人机飞行的速度,并且目标点T并不是倒数第2个节点的可视节点,最后一步纵向移动了6个单位,从而忽略了中间栅格的障碍物;本文提出的改进蚁群算法航迹点分布更加合理,在最后一步没有出现穿过多个栅格现象，并且对初始信息素分布、信息素更新、启发函数设计的改进,使得规划的航迹趋于平缓,来回反复的折线较少。

图1 传统蚁群算法和改进蚁群算法的规划路径

传统蚁群算法和改进蚁群算法的每代蚂蚁综合评价值的最优解收敛曲线如图2所示。

图2 最优综合评价值变化趋势

2种蚁群算法在航迹长度、收敛速度、最优航迹的转角代数和等方面仿真的具体结果见表3所列。

表3 2种蚁群算法性能比较

为了验证本文的改进蚁群算法能适用于不同环境,在文献[19]建立的仿真环境中进行了另外3组补充实验。实验中将蚂蚁的数量改为20个,迭代50次,其他参数不变。

文献[19]中传统蚁群算法与本文改进蚁群算法规划的三维航迹仿真效果如图3所示。2种算法在图3环境下进行的3组仿真实验数据结果见表4所列。

图3 文献[19]算法与本文改进蚁群算法规划航迹的对比

表4 2种蚁群算法的性能比较

由表4可知,改进蚁群算法规划的平均航迹长度缩短了14%,平均迭代次数由34次降到了5.3次,整个航程转角代数和降低了46.2%。这充分说明了本文改进蚁群算法的性能均优于传统的蚁群算法。

将规划出的三维航迹经过三次B样条曲线平滑,得到的结果如图4所示。从图4可以看出,优化后的航迹过渡更加平缓,更易满足无人机飞行中的动力学要求。

图4 航迹平滑处理效果图

4 结 论

(1) 本文航迹规划根据起始点到目标点进行坐标系旋转计算,避免了最后一步纵向横跨多个单元的现象。

(2) 对初始信息素沿“最优路径”在空间高斯分布,克服蚂蚁搜索的盲目性,极大优化了初代蚁群的搜索质量。

(3) 基于空间和时间的信息素挥发系数既加大了重点区域的搜索力度,又保证了搜索范围,加快了算法的收敛速度,避免蚁群陷入局部最优。

(4) 将方向和角度信息融入启发值,使得搜索的线路航程更短,总转角更小。

(5) 对蚁群算法规划的最优航迹作三阶B样条曲线平滑,有利于多旋翼无人机快速跟随航迹。