1. 引言
路径规划是机器人导航领域的核心问题之一,其目的是通过规划算法在给定环境中高效地找到从起点到终点的安全路径[1]。随着智能系统的快速发展,对路径规划的要求日益增加。传统的路径规划算法虽然在理论上能够保证最优解,但在处理大规模环境时往往面临计算效率低下的问题[2]。因此,研究高效的路径规划算法具有重要的理论意义和应用价值。
JPS算法是一种基于栅格地图的路径规划方法,相较于A*算法,JPS算法能够跳过大量无关的中间节点,直接定位路径中的关键点,使搜索复杂度大大降低[3]。这一特性使其在大规模地图和动态环境中表现出优越的性能,成为近年来路径规划领域的研究热点之一。鉴于JPS算法的独特优势,国内外很多学者对其进行了不同的改进和应用。程擎等[4]运用几何航迹碰撞检测法改进JPS算法,提升了无人机在全局环境中的航迹规划能力;赵晓东等[5]提出一种基于蜂窝栅格地图的JPS算法,解决了规划过程中穿越墙角的不安全行为;陈芹等[6]使用双向JPS算法并与B样条曲线结合,提升了路径的平滑性。周熙栋等[7]将JPS算法与A*算法结合,提出一种基于分层栅格地图的融合算法,解决了非结构化场景的全局规划问题。
上述改进算法在不同程度上提高了JPS算法的性能,但是并没有很好地解决其需要计算冗余节点的问题,其搜索效率仍有大量提升空间。针对JPS算法的不足,本文提出三种优化策略,分别对搜索方向、搜索范围和路径长度进行优化,使算法能够高效地规划路径。
2. JPS算法
JPS算法的搜索流程和A*算法基本相同,需要维护一个待扩展节点列表Openlist和一个已扩展节点列表Closelist [8],但不同的是JPS算法仅维护跳点节点,最终得到的路径为一个跳点坐标的集合,具体流程如下:
1) 初始化并将起始节点加入Openlist。
2) 从Openlist中选择代价最低的节点x作为当前节点,并将该节点从Openlist中移除。节点x的代价为
,其中g(x)为初始节点到节点x的实际路径长度,h(x)为节点x到目标节点的预估路径长度。
3) 若当前节点x是目标节点,则搜索成功,回溯父节点得到路径。否则,检查节点x是否有强迫邻居,若有,则将强迫邻居加入Openlist,设置它们的父节点为x,并将x加入Closelist。
4) 以先水平,再垂直,后斜向的顺序进行方向跳跃性搜索。
若搜索到目标节点,则将目标节点的父节点设为x,并将x加入Closelist,搜索结束,返回路径。若搜索到跳点,则将跳点加入Openlist,将跳点的父节点设为x,并将x加入Closelist,停止搜索。若搜索到障碍物,则停止当前方向的搜索。
5) 重复步骤2)到步骤4),直到Openlist为空。
一个完整的JPS算法搜索过程如图1所示,其中黑色栅格为障碍物,蓝色栅格为跳点,红色路径即为算法得到的最优路径。
Figure 1. Schematic diagram of the JPS algorithm
图1. JPS算法示意图
3. 改进JPS算法
3.1. 优化启发式函数
传统JPS算法的启发式函数为
,g(x)为初始节点到节点x的实际路径长度,h(x)为节点x到目标节点的预估路径长度。通过对h(x)进行优化,可以使算法在搜索过程中的不同时期表现出不同的倾向性。
常见的h(x)估计函数有4种,如图2所示,这4种表示方法分别适用于不同场景。图中蓝色线条为欧几里得距离,适用于连续空间或自由移动场景;橙色线条为Octile距离,适用于八方向移动场景,即只能水平、垂直和45˚斜向移动的栅格地图;绿色线条为曼哈顿距离,适用于只能水平或垂直移动的栅格地图;紫色线条为切比雪夫距离,适用于八方向移动的栅格地图,但往往会低估代价。
Figure 2. Common heuristic functions
图2. 常见的启发式函数
由于JPS算法的搜索方向为水平、垂直和斜向的共八个方向,故选取Octile距离最能贴合实际需求。针对h(x),在前方没有障碍物的情况下,Octile距离即为实际的代价距离,在有障碍物的情况下,利用Octile距离也能很好地规避无效节点并估计代价距离。Octile距离的数学表达为:
(1)
为了使算法在搜索过程中,尤其是距离目标节点较远的初期时,能够迅速朝着目标节点的方向进行探索,可以对h(x)进行优化,让h(x)距离目标节点越远,其在f(x)中的占比越大,使路径充分考虑搜索方向,从而提升搜索效率。为优化启发式函数,提出启发式权重影响因子α,并将原启发式函数优化为:
(2)
(3)
其中,D为起始节点到目标节点的Octile距离,取O等于1/D,即起始节点到目标节点Octile距离的倒数,最终的启发式函数可以表示为式(4)。针对一次路径规划过程,该函数的O为定量,故新函数没有引入过多的计算量,并且充分考虑了搜索方向对搜索效率的影响。
(4)
3.2. 限制搜索范围
为了进一步降低JPS算法的计算量,减少无效跳点,引入动态扩展边界来限制算法的搜索区域,在路径规划过程中通过双焦点定位动态椭圆作为扩展边界,从而大大减小搜索范围。
(1) 运用动态扩展边界进行搜索的流程:
1) 将起始节点和目标节点作为椭圆的两个焦点,确定初始状态下的动态扩展边界。
2) 在限定的搜索范围内(包含边界),结合优化后的启发式函数进行路径搜索。
3) 在搜索过程中,若当前限制范围内无可行路径,则以当前节点和目标节点为焦点,重新确定动态扩展边界。
4) 经过步骤3)后,若在新的限制范围内仍无可行路径,可能此时已经足够接近目标节点并且目标节点附近存在大量障碍物,则放弃扩展边界的限制,直接在原始地图下进行搜索,直到搜索到下一跳点后以此节点和目标节点为焦点,重新确定动态扩展边界。
5) 重复步骤2)到步骤4),直到搜索到最优路径,返回最优路径。
(2) 动态扩展边界的确定:
将当前节点和目标节点设为焦点,绘制椭圆形动态扩展边界。椭圆的焦距2c即为两点间的距离,为充分利用已有的计算结果、减轻计算压力,两点间距离的计算依旧采用前文中的Octile距离,即当前节点的h(x)值。椭圆形动态扩展边界的数学表达式为:
(5)
其中,m和n分别表示椭圆中心的横坐标和纵坐标,可由两个焦点的坐标计算得出。2a为椭圆的长轴长,2b为椭圆的短轴长。提出动态扩展边界膨胀系数β,定义
。
根据椭圆的几何性质可知焦距与长短轴之间的关系为:
(6)
其中,c为焦距的一半,即两点间Octile距离的一半,可由式(1)得出,即c = h/2。那么:
(7)
进一步整理,则b为:
(8)
由此可见,扩展边界的椭圆可以完全由当前节点坐标、目标节点坐标、当前节点到目标节点的预估代价值h、和动态扩展边界膨胀系数β得到。
扩展边界膨胀系数β应该与局部地图的复杂度正相关,以当前节点和目标节点作为对角点的矩形为边界,以矩形框内占用栅格占总栅格的比例来衡量地图的复杂程度。为尽可能避免无可行解情况的发生,局部地图复杂度越高,动态扩展椭圆应设置的越大。这种动态设置扩展边界的方式,既可以防止因搜索范围过大而增加计算压力,又可以避免因搜索范围过小而出现无解的情况。图3为局部地图复杂度的确定,其中黑色栅格为障碍物,红色虚线矩形框内为确定复杂度的局部地图。左图中局部地图共有栅格36个,其中障碍物占5个,复杂度为0.139;右图中局部地图共有栅格15个,其中障碍物占3个,复杂度为0.200。为简化计算,当复杂度小于0.1时,β取0.6;当复杂度大于等于0.1小于0.2时,β取0.8,当复杂度大于等于0.2时,β取1.0。
Figure 3. Determination of local map complexity
图3. 局部地图复杂度的确定
动态扩展边界用于限制当前迭代下的地图搜索范围。图4展示了利用一个迭代周期内进行搜索时,有、无扩展边界对搜索范围以及跳点数的影响。其中,蓝色栅格代表跳点,灰色栅格代表搜索范围。右图运用椭圆形动态扩展边界,剔除了非核心节点,在边界内(包含边界)进行搜索。与左图所示的传统搜索方式相比,动态扩展边界的运用使得算法的搜索方向更明确、搜索节点(灰色栅格)和计算节点(蓝色栅格,跳点)更少,大大降低了计算量。图4中当前的局部地图复杂度为0.063,β取0.6。椭圆形动态扩展边界的焦距2c为9.070,长轴长2a为10.884,短轴长2b为6.016。运用动态扩展边界进行搜索后,搜索节点栅格数从271减少到62,减少了77.1%;计算节点栅格数从11减少到8,减少了27.3%。
(a) (b)
Figure 4. Comparison of search processes with and without dynamic boundary expansion. (a) Without dynamic boundary expansion; (b) With dynamic boundary expansion
图4. 有无动态扩展边界搜索过程对比。(a) 不使用动态扩展边界;(b) 使用动态扩展边界
3.3. 提取关键跳点
在优化启发式函数并限制扩展边界后,所规划出的路径依旧存在个别跳点是冗余跳点,这些跳点并不会影响搜索速度,但是会使路径变长、拐点变多。针对该问题,通过提取关键跳点来进一步优化路径,从而缩短路径的实际长度,减少拐点的数量。具体步骤如下:
1) 取出原路径列表中的第一个跳点p和它的后方跳点p + 1和p + 2。
2) 连接p和p + 2,判断该连线是否经过障碍物。若未经过障碍物,则p + 1为冗余跳点,删掉该跳点,令原路径中的p + 2和p + 3作为新一轮的p + 1和p + 2,重复操作直到p与p + 2的连线经过障碍物为止。
3) 令当前的p + 1为新的p。
4) 判断p + 2是否为终点。若否,返回步骤2),若是,跳出流程并返回全局路径。
是否提取关键跳点的最终路径对比如图5所示。
(a) (b)
Figure 5. Comparison of final paths with and without key jump point extraction. (a) Without key jump point extraction; (b) With key jump point extraction
图5. 是否提取关键跳点最终路径对比。(a) 不提取关键跳点;(b) 提取关键跳点
4. 仿真和实际场景验证
4.1. 仿真实验
为验证改进JPS算法的性能,在栅格地图上进行对比仿真实验,分别运用JPS算法和改进JPS算法进行路径规划。实验基于matlab R2021a平台,结果如图6所示。其中,橙色栅格为起始节点,绿色栅格为目标节点,黑色栅格为障碍物,灰色栅格为计算节点,红色线条为最优路径。
由表1可知,改进JPS算法相比传统JPS算法在各项指标上均有提升。仿真实验结果表明,启发式函数的改进和动态扩展边界的引入使JPS算法的遍历节点数和计算节点数大幅下降,计算量减少;提取关键跳点策略的运用使传统JPS算法拐点数减少、路径长度缩短。其中,路径长度缩短了2.15%,规划时间缩短了12.96%,规划效率显著提升。
Table 1. Comparison of simulation experiment results
表1. 仿真实验结果数据对比
算法 |
拐点数 |
遍历节点数 |
计算节点数 |
路径长度 |
规划时间/s |
JPS |
3 |
320 |
41 |
20.97 |
0.0162 |
改进JPS |
2 |
206 |
36 |
20.52 |
0.0141 |
(a) (b)
Figure 6. Comparison of simulation experiment results. (a) Planning result of baseline JPS; (b) Planning result of enhanced JPS
图6. 仿真实验结果对比。(a) JPS算法规划结果;(b) 改进JPS算法规划结果
4.2. 实际场景验证
为进一步验证改进JPS算法在实际应用中的可行性,在ROS机器人上进行了实验。实验基于Ubuntu20.04系统、ROS Noetic平台,将ROS导航系统GlobalPlanner中的算法替换为改进JPS算法,在建立好的地图上运行,运行结果如图7所示。
(a)
(b)
(c)
Figure 7. Field test validation results. (a) Initial state; (b) During operation; (c) Final state
图7. 实际场景验证结果。(a) 初始状态;(b) 运行过程中;(c)运行结束
经过实际测试,改进JPS算法在复杂环境中表现出良好的实时性和稳定性,能够满足实际应用的需求。
5. 总结
传统JPS算法存在搜索方向不明确、搜索范围过大和存在冗余节点等问题,为解决这些问题,本文提出三种改进措施分别对算法进行优化。对启发式函数添加权重系数,使算法在规划初期能迅速朝目标节点的方向进行搜索;引入动态扩展边界限制搜索范围,减少遍历节点数量以提升效率;通过提取关键跳点优化拐点数量和路径长度。实验结果表明,改进算法与传统算法相比,规划效率更高,可以满足实际应用需求,并且优化效果明显。
NOTES
*第一作者。
#通讯作者。