在复杂地形下三维UAV航迹规划的改进A*算法*

倪昌浩,邹 海

(安徽大学 计算机科学与技术学院,安徽 合肥 230601)

0 引 言

航迹规划是无人机执行任务时的重要步骤之一,其目的是寻找出一条安全的无碰撞路径。无人机的工作环境较为复杂,如何快速规划出一条合理且风险性较低的路径,已成为航迹规划中的研究热点。

针对航迹规划问题,目前最主要的算法有蚁群算法[1]、粒子群优化算法[2]、人工势场法[3]、RRT算法[4]和A*算法等,其中A*算法是经典启发式搜索算法之一,且易于实现,但是其算法具有一定局限性且计算量较大,国内外学者也提出很多改进策略。文献[5]在进行航迹规划时,在对节点扩展进行了角度约束,解决了航迹平滑问题。文献[6]在三维平面中,结合约束条件剪除搜索空间,有效提高了搜索效率。文献[7]对更加复杂的环境进行建模,基于传统A*算法设计策略防止无人机陷入死区,使无人机在复杂环境下也能规划出较好的路径。本文为了更好解决三维航迹规划问题,考虑到无人机的物理运动约束,结合飞行环境提出一种改进的A*算法,有效避免航迹贴合障碍物情况的发生,同时引入B样条曲线使路径更加平滑。

1 问题描述

飞经区域往往会有复杂地形阻碍无人机行至目的地,为确保无人机安全到达终点,飞行前需对路径进行规化,根据地形环境考虑对飞行有影响的威胁因素,在满足约束条件的同时规划出一条从起始点S到目标点G的航迹r。假设在三维空间中将无人机视为均速运动的质点,航迹节点Pi(xi,yi,zi)表示位置和高度,规划空间模型为

{(x,y,z)|-a≤x≤a,-b≤y≤b,-c≤z≤c}

(1)

无人机航迹可表示为r=[S,P1,P2,…,Pi,…,PN,G]。

2 改进的A*算法

2.1 传统A*算法

A*算法是经典启发式搜索算法之一,在搜索过程中通过不断评估路径节点i的代价f(i)以选择航迹节点,以构造最优路径,评价函数f(i)的表达式[8,9]

f(i)=g(i)+h(i)

(2)

式中i为当前路径节点,g(i)为从起始点S(x0,y0,z0)到点Pi(xi,yi,zi)的航路实际代价,h(i)为启发函数,即当前节点到目标点G(xN+1,yN+1,zN+1)的代价估计,本文使用三维空间中节点之间的欧氏距离进行评估,如式(3)所示[10]

(3)

A*算法在进行搜索时,需创建两个表,分别为OPEN表和CLOSED表,OPEN表用于存储可扩展的节点,CLOSED表用于存储已扩展的节点和所处位置有障碍物的节点,航迹规划过程中,两个表数据不断更新,直至终目标点G包含在CLOSED表中。

2.2 搜索约束

传统A*算法在搜索路径时,并未考虑实际约束条件,且计算量较大。根据无人机的机动性和实际飞行状况,需对偏航角φi和俯仰角θi进行约束,如图1所示,设最大偏航角为φmax,最大俯仰角为θmax,每一航段向量为ai=(xi-xi-1,yi-yi-1,zi-zi-1),航迹中所有节点应满足

图1 搜索角度

(4)

通过飞行角度约束,可以大大减小航迹的搜索范围,提高算法效率。

2.3 危险估计

使用A*算法进行航迹规划时,经常会出现规划后的路径与障碍物贴合甚至是相交的情况,若按航迹行进,无人机与障碍物宜出现相撞的情况。为提高航迹的可行性和安全性,本文引入危险因子t(i),即根据上述约束确定i节点的搜索角度,设危险探测半径为p,A*算法的相邻节点搜索步长为L,本文定义p=2L,如图2所示,节点i的危险探测空间是以O为球心p为半径的三维球体的一部分区域,图中∠BOH=2∠BOE=2θmax,∠DOF=2∠DOE=2φmax,在此空间中均匀选取q个探测点,若所取点位置存在障碍物,则提升i节点处的危险度,否则无需操作,若q个探测点中有n个点处于障碍物范围内,t(i)的表达式为

图2 危险探测

(5)

式中k(j)为检测点到i节点的距离,可表示为

(6)

由此可把评价函数进行改进,改进后的表达式为

f(i)=g(i)+h(i)+t(i)

(7)

2.4 平滑路径

A*算法生成的航迹节点依次连接,可得到一条简单航迹,但无人机在飞行过程中并不能够第一时间在拐点处对航行角度进行修改,为了满足实际飞行要求,需对航迹r进行再处理,本文采用三次B样条曲线对路径进行平滑操作。

n次B样条曲线段可表示为

(8)

式中Pi为航迹节点,Bi,n(t)为基函数,其表达式为

(9)

把每条线段依次连接,可得到的一条平滑曲线。当n=3时,可得三次B样条曲线,此时基函数可表示为

(10)

节点i至节点i+3的拟合曲线可表示[11]为

(11)

对路径节点依次使用上式,可得一条平滑的航迹。

3 仿真分析

为了验证改进算法的性能,利用MATLAB软件进行路径规划仿真实验,本文搜索区域为65 km×65 km×30 km,地图3、图4中x，y，z轴的单位长度为1 km,无人机的起始点S坐标为(2,3,3)km,终点G坐标为(63,60,25)km,探测点个数q取30,步长L取3 km,最大俯仰角θmax和最大偏航角φmax分别设定为π/6,π/3。

图3为在约束条件下A*算法的规划航迹,结合俯视图3(a)和图3(b)可以看出路径在节点处没有过度,且在较为复杂的山地条件下,无人机无法沿规划航迹安全飞行,在经过障碍物一、二的位置时,航迹与障碍物区域相交,经过障碍物三时航迹紧贴障碍物边缘。本文改进A*算法规划的三维路径如图4所示,可以看出航迹整体较为平滑,符合实际需求,能够较好躲避障碍物,图3和图4的航迹长度分别为91.02,94.28 km,改进A*算法的路径长度相较于加入约束的传统A*算法路径加长了3.58 %,但是航迹的可行性和安全性有了明显的提升,且易于实现。

图3 传统A*算法的航迹规划结果

图4 改进A*算法的航迹规划结果

本文另取三个不同的终点,得到的结果如表1所示。

表1 不同目标点得到的结果

从表1中可以看出,本文改进算法依然可以很好地找到目标位置,而传统A*算法规划航迹出现无法通过障碍物的情况,且当终点为(40,20,30)km时,规划路径会陷入死区,未能成功得出结果。改进A*算法规划航迹的长度未有明显的提升,且当目标点为(30,60,20)km时,相对于传统A*算法航迹长度有一定的缩短,再次证明了本文改进算法的鲁棒性。

4 结 论

在三维环境中,本文考虑障碍物的同时对无人机行进角度进行约束,缩小航迹节点搜索范围,并且引入危险因子,改进了A*算法的评价函数,增加整体航迹的安全性和可行性,通过B样条曲线法使得航迹更为平滑。仿真结果表明：本文提出的算法在保持较短航迹长度的同时,可使无人机避开所有障碍物,到达终点,满足实际的航迹规划需求,有效解决了三维航迹规划的安全问题。