一种面向非结构化环境的改进跳点搜索路径规划算法

魏博闻, 严 华

(四川大学电子信息学院, 成都 610065)

近年来,智能机器人迅速发展,人们对智能机器人的研究取得了长足的进步。智能机器人是一个集环境感知、动态决策与规划、行为控制与执行等多功能于一体的综合系统。自主导航是移动机器人的核心算法,为机器人提供基于感知的导航规划能力,是学术与工业界的热点。其具体定义可以做如下描述:根据相关性能指标(满足时间约束,满足动力学约束,满足运动学约束),在运动空间中找到一条从起始状态到目标状态且可以避开障碍物的最优路径[1-3]。

当周围环境的障碍物信息对智能体完全已知的情况下,这类规划称为全局路径规划[4]。全局路径算法主要包括如下3类:智能仿真算法，如蚁群算法、遗传算法、粒子群算法等[5]；基于图搜索的算法，如A*、Dijkstra、HybridA*、D*、D*Lite等[6]基于采样的算法,包括概率路图法以及随机搜索树的各种变种算法[7]。

A*算法被广泛使用于移动机器人的自主路径规划中[8]。但是A*算法仍然存在问题。首先,全局路径规划只是智能机器人系统中的一步,通常系统只会分配小部分算力来进行路径规划,在面对复杂的非结构化地图进行路径规划时,A*算法有计算量大、内存消耗严重等缺陷。其次,由于智能机器人自身存在动力学约束,A*算法给出的不平滑的路径可能导致机器人无法直接执行。

A*算法在扩展过程会对当前节点的每个邻居节点做检测,所以该算法需要维护非常庞大的OpenList和CloseList表,这是导致内存占用严重、计算时间长的关键原因。A*算法在运算过程中,每次从优先队列中选取代价值最小(优先级最高)的节点作为下一个待遍历的节点,A*算法使用两个集合来表示待遍历的节点与已经遍历过的节点,通常称之为openset与closeset。针对A*运算速度慢的问题,文献[9]提出了跳点搜索算法(jump point search, JPS)进行了改进,提高了运算效率。但是,由于启发函数设计过于简单,该方法在复杂不规则地图中会无法保证最终路径是全局最优路径。

文献[10]中提出了一种基于lattice采样规划方法,将障碍物和智能机器人本身转化到frenet坐标系中,在frenet坐标系中使用lattice方式进行规划,将规划路径使用三次多项式进行连接。虽然该方法能规划一条免碰撞且符合车辆动力学约束的路径,但是其笛卡儿坐标系与frenet坐标系转化计算量巨大,很难满足路径规划模块实时性要求,故该方法并未在实际中进行推广。

近年来,尽管已经提出了很多优秀的路径规划算法,但是生成一条满足实时性的路径仍是一项比较困难的工作。为了满足路径生成的实时性与安全性需解决以下问题:

(1)如何在保证全局最短路径的前提下,降低算法的计算复杂性,降低算法运算时间,进而满足路径规划算法实时性的要求。

(2)考虑到路径点稀疏、不平滑的特征,为更好地控制机器人,如何构造状态空间中的一条轨迹(不考虑障碍物)连接任意给定的两个状态,从而将稀疏的路径点变成平滑的曲线或稠密的轨迹点。

针对当前研究现状中存在的问题,提出一种融合改进跳点搜索算法和微分平坦法的全局路径规划算法,主要思路如下:

(1)基于跳点搜索的算法,设计一种更加符合智能驾驶规划的启发式函数。引入cross算子,实现启发式函数的动态调整,保证全局最短路径的情况下,扩展节点有限,用时较短。

(2)使用微分平坦法对改进跳点搜索算法生成的路径点进行优化,从而生成更适合底层控制的平滑轨迹。

1 算法设计

1.1 跳点搜索算法

跳点搜索算法整体的算法框架与A*搜索算法相同,其中跳点是指在全局搜索过程中可以直接跳跃的点。相比于A*在搜索过程中需要对路径上每个节点的估价数值进行比较,跳点搜索算法引入一套完整的扩展结点策略。JPS算法通过寻找跳点的方式,排除了大量无关的点,减少了openlist中搜索点的数量,从而提升搜索速度。整个寻找跳点的过程可以做如下解释。

1.1.1 概念描述

强迫邻居:如果节点n是x的邻居,并且节点n的邻居有阻挡(不可行走的格子),并且从parent(x)、x、n的路径长度比其他任何从parent(x)到n且不经过x的路径短,则n为x的强迫邻居,x为n的跳点,如图1(a)左图中,5号节点是x的强迫邻居，在图1(a)左图中,1号节点是x的强迫邻居。

跳点:如果点x是起点或目标点,则x是跳点；如果x有强迫邻居则x是跳点；如果parent(x)到x是水平或者垂直移动,并且x经过水平或垂直方向移动可以到达跳点,则x是跳点,例如图1(b)左图中,x是跳点；如果parent(x)到x是对角线移动,并且x经过水平或垂直方向移动可以到达跳点,则x是跳点,如图1(b)右图中,x是跳点。

1.1.2 在执行算法的过程中,JPS的跳跃规则

(1)JPS搜索跳点的过程中,如果直线方向、对角线方向都可以移动,则首先在直线方向搜索跳点,然后再在对角线方向搜索跳点。

(2)只有跳点才会加入openset,因为跳点会改变行走方向,而非跳点不会改变行走方向,最后寻找出来的路径点也都是跳点。

JPS算法使用的启发式函数与A*搜索算法相同,仍然是用传统的欧氏距离或者曼哈顿距离做距离表达，即

(1)

hManhattan=dx+dy

(2)

式中：dx=|node.x-goal.x|；dy=|node.y-goal.y|。

做全局路径规划的时候,使用栅格地图,默认智能机器人可以8个方向前进。面向这样结构化的设定,如果使用曼哈顿距离,算法无法保证最终输出最优路径；使用欧氏距离,算法会拓展过多的无用节点,只有在当前节点到目标节点的距离数值正好等于从当前节点移动到目标代价的时候,算法效果最好[11]。依据这样的思想,使用对角线距离表示两点之间的距离来改进启发式函数的性能。

1.2 启发函数的改进

启发式函数h(n)估计当前点n到终点的代价。不同的启发式函数的选择对应着不同算法的运行效率。在启发函数数值为0的时候,整个算法退化为迪杰斯特拉算法,能够寻得最优路径,但是扩展的节点特别多；当启发函数数值非常大时,算法无法保证最终的路径是最短路径,这违背了全局路径算法的目的。想要保证最终的搜索算法既能寻得最短路径,又能在保证寻得最短路径的前提下扩展的节点有限,这需要选择最合适的启发式函数。

对启发式函数做改进的主要目的是寻找最短路径。如图2所示,其中Sstart表示搜索算法的起点,Sgoal表示搜索算法的终点,使用传统的扩展节点的方法,会生成很多无用的扩展节点。

图2 基于搜索的算法在执行过程中的扩展点图示Fig.2 The extension point of a search-based algorithm

针对JPS算法采用的传统启发式函数表达,做如下改进。

(1)使用对角线距离来表达当前点与终点之间的距离，即

(3)

式(3)中：dx与dy的定义与式(2)中保持一致。diag(node)表示当前节点node的对角线距离。具体可视化如图3所示。

图3 3种距离描述方法在二维空间中的图示Fig.3 The representation of three distance description methods in two-dimensional space

(2)如果当前节点代价函数f对应着多条路径时,这些路径都会被搜索,但其实只需要其中的一条路径即可。这种情况在障碍物较少的地图中出现非常频繁。为解决该问题,在启发式函数中添加附加值,大小为初始点指向目标点向量和当前点指向目标向量的向量叉积,进而改变启发式函数h的值,使之在选择路径上更倾向从初始点到目标点的连线。具体描述为

cross=Vc_e×Vs_e

(4)

式(4)中：cross表示初始点指向目标点向量和当前点指向目标向量的向量叉积；向量下标c表示当前节点current,e表示目标点end,s表示起始点start。

(3)考虑到实际运行情况,路径搜索算法在开始搜索时,最重要的是迅速移动到任意位置；而在搜索接近结束时,最重要的是移动到目标点。所以在启发式函数中设计权重w(w≥1),当接近目标时,降低这个权值,从而降低启发函数的重要性,同时增加了路径真实代价的相对重要性。

综上3点改进,可以将h函数表示为

(5)

使用改进启发式函数的JPS算法,称之为带权重的跳点搜索(weighted jump point search, WJPS)算法。

1.3 轨迹优化

WJPS算法输出的是面向栅格地图的全局最优路径点。考虑到路径点稀疏、不平滑的特征,为更好地控制机器人,需要构造状态空间中的一条轨迹(不考虑障碍物)连接任意给定的两个状态,从而将稀疏的路径点变成平滑的曲线或稠密的轨迹点,再传输给底层驱动做控制。

采用微分平坦方法[12],将轨迹表示为n阶多项式。使用多项式来拟合最终的轨迹有如下优点:①因为多项式方程能够多阶求导,所以可以确保整条轨迹满足光滑性质；②计算多项式参数的时候比较方便；③面向多维空间,多项式方程可以分离开,分别求解每个维度多项式方程的对应参数。

首先将轨迹建模为多项式方程模型,具体为

(6)

式(6)中：p为多项式轨迹参数,可以将之设成参数向量:

p=[p0,p1,…,pn]T

(7)

进一步将上述轨迹表述写成向量形式:

p(t)=[1,t,t2,…,tn]p

(8)

接下来,轨迹需要根据具体情况判断使用的多项式的次数。在微分平坦空间中,有多项式次数与机器人的平移、旋转与推力的一一对应关系,具体如表1所示。

表1 多项式次数对应的物理意义Table 1 The physical meaning of the degree of polynomial

根据表1,在通常轨迹优化手段中,有如下两种最小化原则:

(1)minimum jerk,本质上是在最小化角加速度。通常无人机在初始化的时候,需要缓慢移动摄像头,使得视差的变化最小,所以这种方法通常在无人机上为了实现更好的视觉跟踪而被采用。

(2)minimum snap,这种方法实际是在最小化推力的导数,也就是说要使得推力的变化尽可能平缓,从而可以达到节省能耗的目的。

根据上述公式,对于任意时刻t,可以根据参数计算出轨迹的位置、速度、加速度、jerk和snap,对应的方程为

v(t)=p′(t)=[0,1,2t,3t2,…,ntn-1]p

(9)

a(t)=p″(t)=[0,0,2,6,12t2,…,n(n-1)tn-2]p

(10)

(11)

(12)

因为无法由一个多项式将整条运动轨迹表述,这部分将移动速度固定,从而根据每段运动轨迹的长度,将时间分成多段,每一段用一条多项式表示为

(13)

式(13)中：k为轨迹的段数。那么第i段轨迹的参数向量就可以表示为

pi=[pi0,pi1,…,pin]T

(14)

接下来公式表述主要以x轴为例,其他坐标方向以此类推。至此,已经完成了全部轨迹建模的工作。出于效率与轨迹光滑的综合考量,采用三次样条插值的方法,来对前文生成的路径点进行平滑处理,最终求得每一段轨迹的系数,进而使得整体轨迹满足起始点、终止点、中间点的约束以及整条轨迹的光滑连续约束。

如图4所示,x、y分别表示地图的两个垂直方向。蓝色轨迹是直接连接离散的路径点的结果，红色曲线是对绿色离散点做曲线拟合后生成的轨迹。

图4 轨迹优化后的结果Fig.4 Trajectory optimization results

2 实验验证

实验验证分为3部分。第1部分面向JPS算法改进的WJPS算法,在选取丰富复杂的地图后,横向对比A*、JPS和WJPS 3种算法,分别从运行时间、路径长度以及扩展节点数3方面做考量,定量评估3种方法的优缺点。第2部分是针对第1部分WJPS算法输出的路径点,做可视化的轨迹优化展示,并就实验结果做具体分析。所有的实验,均在python3.7环境下实现。

2.1 WJPS算法实验结果

根据文献[13]专门设计了9张地图,包括复杂巷道、复杂室内场景、复杂停车场、复杂商业区,以及迷宫场景,力求覆盖尽可能多的非结构化地图场景。然后在每张地图上分别应用3种算法进行路径规划结果,可视化结果如图5～图13所示,其中x、y分别表示地图的两个垂直方向。

图5 地图1(巷道)中3种算法的可视化对比Fig.5 Comparison of the three algorithms in map 1 (roadway)

图6 地图2(复杂巷道)中3种算法的可视化对比Fig.6 Comparison of the three algorithms in map 2 (complex roadway)

图7 地图3(迷宫)中3种算法的可视化对比Fig.7 Comparison of the three algorithms in map 3 (labyrinth)

图8 地图4(复杂迷宫)中3种算法的可视化对比Fig.8 Comparison of the three algorithms in map 4 (complex labyrinth)

图9 地图5(商场)中3种算法的可视化对比Fig.9 Comparison of the three algorithms in map 5 (shopping)

图10 地图6(复杂商场)中3种算法的可视化对比Fig.10 Comparison of the three algorithms in map 6 (complex shopping)

图11 地图7(复杂迷宫)中3种算法的可视化对比Fig.11 Comparison of the three algorithms in map 7(complex labyrinth)

图12 地图8(复杂停车场)中3种算法的可视化对比Fig.12 Comparison of the three algorithms in map 8 (complex park)

图13 地图9(复杂居民区)中3种算法的可视化对比Fig.13 Comparison of the three algorithms in map 9 (complex residential area)

9张地图中,除了地图1巷道和地图5商场外,其余都是非常复杂的环境,包括迷宫、居民区等。观察上述路径规划结果,可以做如下分析:在最终路径长度上,WJPS算法无论在复杂环境还是简单环境,都是路径最短的算法,而JPS算法的启发式函数设计简单,导致寻路效果过于激进,在极其复杂的场景，如地图4、地图8、地图9中甚至出现打弯现象,这是全局路径规划一定要避免的情况。对于全局路径规划来说,一条全局最短的路径是最重要的,因为后续的工作都要基于这个全局路径来进行。3种方法在多个地图运行10次的平均数据如表2所示。

在表2最重要的路径长度项中,可以清晰得知,WJPS算法无论在简单环境还是在复杂环境中都能保证生成路径是最短的。

表2 3种方法在多个地图运行10次的平均数据Table 2 Three method’s average data on multimaps 10 times

分析表2中的寻路时间可以得知, 3种算法在寻路时间上都是毫秒级别,其中JPS由于扩展节点少,在寻路时间上往往更快速,但在复杂环境中并未寻得最短路径；A*算法由于其算法本身扩展过多无用节点的原因,通常是最慢的一种。WJPS算法因为利用JPS跳点算法中寻找拓展点的策略,同样能够实现毫秒级别的规划,算法效率能够满足智能体对路径规划层的要求。

所以本文的结论是:如果场景足够简单,机器人系统对运算速度有特别强的需求的时候,JPS算法是一种比较好的选择；但是当场景比较复杂,WJPS算法能够寻得最短路径。考虑到后续的轨迹优化以及控制层执行轨迹的时候都要依赖全局路径规划的结果,在时间都是毫秒级别的情况下,全局路径规划最重要的指标是输出一条全局最短的轨迹点,所以认为WJPS算法更适合非结构化复杂场景。

2.2 路径拟合实验结果

面向WJPS算法生成的路径点做曲线拟合,生成一条光滑的轨迹。具体曲线拟合的效果如图14所示,每一个小图表示对应离散地图点做拟合后的结果，其中红色曲线为轨迹优化的结果,绿色节点为WJPS算法生成的路径点,蓝色线段为WJPS算法生成的折线路径。

图14 任意6张地图中对生成路径点做优化的结果Fig.14 The results of optimizing the generated path points in any 6 maps

对轨迹优化算法进行实验验证。使用的三次样条曲线对WJPS算法输出的路径点做拟合,最终将稀疏的路径点连接成为平滑的曲线或稠密的轨迹点,更适合机器人底层做具体控制。

3 结论

针对非结构化复杂场景下的路径规划问题,提出了一种改进路径规划算法WJPS算法,并对其生成的路径点做轨迹优化。经过详细的对比实验得到以下结论：

(1)与传统A*算法以及基于A*改进的JPS算法相比,WJPS算法可以在复杂的非结构化的环境中找到长度最短的路径。

(2)WJPS算法的算法执行时间是毫秒级别,完全满足工业中对路径规划层的实时性要求。

(3)使用微分平坦法对路径点做拟合,将稀疏的路径点连接成为平滑的曲线或稠密的轨迹点,保证了最终路径连续平滑。