0 引言
路径规划算法是移动平台进行自主导航的关键算法[1],可以使运动对象在一定的约束条件下,从起点高效、安全地移动到终点。目前常用的路径规划方法主要有A*算法、Dijkstra算法、蚁群算法和遗传算法等[2-3]。其中,由于A*算法采用启发函数指导路径搜索的特性[4],在有限图形搜索问题中,可以快速找到一条最佳路线,使其逐渐成为使用率最高和研究最热门的路径规划算法之一[5]。
但是,A*算法在路径规划问题中也面临着一些问题[6],如A*算法的启发函数的优良对算法最终生成路径的质量有着较大的影响,导致在较为复杂的场景中,极易陷入局部最优解[7]、导致效率低下和增加数据存储空间。此外,由于栅格地图的特性,路径不仅会出现许多多余的拐角,最终呈现的路线也不够平滑。针对这些问题,国内外学者提出了一些解决方案。
国内外学者主要从算法本身改进和算法融合2种方案对A*算法进行改进和优化。A*算法改进主要有2种改进方式:对算法的评价函数进行优化和对算法的搜索方式进行改进,而通过融合方式改进的算法主要有局部规划算法如动态窗口算法(dynamic window approach,DWA)和智能搜索算法如遗传算法、蚁群算法等。
学者FAN从算法本身进行优化[8],提出根据障碍物比例动态调整的自适应启发函数,明显缩短搜索时间;吴鹏等[9]采用双向搜索的方式,从起点和终点同时向对方开始搜索,实验证明,双向A*算法减少了搜索时间,且可以得到最优路径,然而此种方法会产生非常明显的储存空间占用问题[10-11];唐彬等[12]将路径搜索过程进行分段,每一个阶段采用不同的评价函数指导算法的路径规划,经仿真证明,可以减少搜索时间和缩短路径长度,但是极易在某一个阶段陷入局部最优解,导致无法得到最优路径。
算法融合方面,许多学者将A*算法与局部路径规划算法DWA进行融合,使算法[13-14]可以根据实际应用场景中出现的未知障碍物进行动态避障;在文献[15]中,学者通过与DWA算法融合来避开道路中凹凸不平的路段以缩短路径长度,提升路径平滑度。此外,有学者还通过优化蚁群算法的信息素模型使之与A*算法结合来增加对目标点的感知[16],提高路径的全局收敛速度、提升路径平滑度。
受到上述学者的启发,本文中针对A*算法在路径规划过程中产生的问题对A*算法本身进行改进,通过融合特殊动态权重、子节点选择优化和路径平滑度优化3种优化方案来提升A*算法的路径规划效率、避免了生成危险路径以及优化多余转角问题、提升了路径的安全性且大幅减少数据存储空间占用。
1 原始A*算法
A*算法是一种常用的启发式搜索算法,用于有限图形搜索问题中快速找到一条最佳路径。
在路径规划过程中,A*算法首先从起始点开始,选择其周围评价函数最小值的节点作为下一个中心节点,依此方法继续向周围节点搜索,直至达到目标点或无可行性解。
设A*算法的评价函数为f(n),则其表达式如下:
f(n)=g(n)+h(n)
(1)
式(1)中,g(n)表示在栅格地图中,起始点到当前节点n之间距离代价总和的代价函数,而h(n)表示当前节点n到目标节点之间距离的估计代价的启发函数。
评价函数f(n)的精确度对A*算法路径规划的性能和效率有着决定性作用。由式(1)可知,评价函数f(n)由代价函数g(n)和启发函数h(n)共同决定。在子节点选择时,算法会优先考虑待选子节点中启发函数值较小者。因此,启发函数h(n)在评价函数中的比重直接影响最终路径规划的效果。
设子节点到目标节点的实际代价为C(n),不同的启发函数值比重对算法最终效果如表1所示。
表1 启发函数的不同比重对算法结果的影响
Table 1 Influence of different heuristic function weights
权重路径规划效果h(n)=0时间最长,路径最短h(n)
随着h(n)比重的增加,路径的终点指向性越来越明显,路线更倾向于选择启发值最小的节点,此时,探索范围越来越小、所用时间也越来越短,然而,路径也更容易陷入局部最优解,导致算法最终规划出的路径距离更长、转折角度更大。
因此,根据场景中障碍物的分布情况对启发函数h(n)进行实时调整可以使算法所规划出的路径达到最优。
2 算法优化
2.1 启发函数优化
通过引入特殊动态权重系数ω(n)来动态控制启发函数在评价函数中的比重,使最终产生的路径最优。评价函数f(n)表达式如式(2)所示:
f(n)=g(n)+ω(n)*h(n)
(2)
ω(n)的具体表达式如下:
(3)
式(3)中: D表示当前点到目标点的代价距离,N表示起始点到目标点的代价距离,D/N则表示当前点于局部地图中的位置。α表示起始点与目标点所组成的矩形环境和机器人可运动的局部环境大小的比值。
若起始点坐标为(xs,ys),目标点坐标为(xg,yg),局部环境大小为E,则α可以表示为如式(4)所示:
(4)
设α=1/2,则D/N和ω(n)随时间t的变化如图1所示。随着算法不断地向目标点扩展,D/N和ω(n)也随之不断变小,同时启发函数h(n)的比重从开始探索时的最大值逐渐减小,使算法可以在开始快速探索出一条路线,并在逐渐接近目标点的同时,增加探索节点的数量,增加路径的优良性。
图1 D/N与ω(n)随时间t变化
Fig.1 Variation of D/N &ω(n) over time
2.2 子节点选择优化
A*算法在基于栅格地图搜索中,每一次子节点的搜索都是基于当前中心节点周围的待选节点来进行,将其中评价函数值最小者选择作为子节点。若该点是障碍物,则不考虑此节点。因而,在路径规划时,时常会出现路线接触障碍物的情况,具体有2种形式:路线接触障碍物的顶点和路线穿过2个障碍物的接触点。路线接触障碍物示意图如图2所示。
图2 路线接触障碍物示意图
Fig.2 Situations of route meet with obstacle
针对此类问题,将每一个中心栅格节点周围的8个待选栅格节点分为3类:a、b、c类,其中a类栅格为中心栅格上、下方向的栅格,即a1、a2;b类栅格为中心栅格左、右方向的栅格,即b1、b2;而c类栅格为中心栅格左上、右上、左下和右下方向的栅格,即c1、c2、c3和c4。如图3所示。
图3 待选节点分类示意图
Fig.3 Types of optional nodes
由图3可知,障碍物处于不同方位的栅格均有可能出现路径接触障碍物栅格的情况。因此,对于不同的情况的应对方案如下:
1) 当障碍物单独出现在a1或a2栅格中时,则不考虑将c1和c2或c3和c4栅格作为子节点的情况;
2) 当障碍物同时出现在a1和a2栅格中时,则不考虑全部c类栅格作为子节点的情况;
3) 当障碍物单独出现在b1或b2栅格中时,则不考虑将c1和c3或c2和c4栅格作为子节点的情况;
4) 当障碍物同时出现在b1和b2栅格中时,则不考虑全部c类栅格作为子节点的情况。
子节点选择规则执行过程如图4所示。在每次子节点的选择中加入此规则,可在实际路径规划过程中有效避免高风险路径的产生和进一步减少对危险子节点评价函数值的计算。
图4 子节点选择判断流程
Fig.4 Subnode selection optimization flow
2.3 路径平滑度优化
由于栅格地图的点位标记特性,A*算法最终生成的路径通常是由多个中心点的连线构成,常常会在障碍物周围产生许多多余的转角,甚至会陷入局部最优解。不仅如此,在实际应用场景中,由于运动对象的尺寸大小各异,存在一定概率与障碍物产生摩擦或碰撞。
为了解决多余转角问题,本文中提出双向路径平滑度规则判断,即从起始点和目标点两端向对方开始判断:计算两个不相邻节点之间的距离是否小于规划的路径长度,若小于,则将两点连线替换原有路径。
此外,在双向优化路径到最简的基础上,加入安全距离判断,通过比较障碍物到路径之间的垂直距离与设置的安全距离之间的关系,来判断机器人路径规划的安全性。具体判断规则如图5所示。
图5 安全距离示意图
Fig.5 Safety distance
图5中,假设以点C为圆心的表示障碍物的外接圆,r为圆C的半径,线段AB表示经过双向优化的路径,d为障碍物到路径的垂直距离,l为圆C到路径AB的纵轴距离,α分别表示CE与CD之间的夹角、路径AB与横轴之间的偏转角度。
障碍物到路径的垂直距离d可表示为
d=lcosα
(5)
安全距离设置为D(D>r),将由式(5)计算得出的垂直距离d与D的大小进行比较,判断该路径是否安全:
1) 若d ≥D,则该路径为安全路径,可以选择;
2) 若d<D,则该路径为危险路径,不可选择。
经过双向路径平滑度优化和安全距离规则判断优化之后的路径以图6为例。在该栅格地图中,A*算法生成的路径表示为蓝色线段,即S-n1-n2-n3-n4-n5-n6-n7-n8-n10-G,优化后的路径表示为绿色线段,即S-n11-G,黑色区域为障碍物,红色圆点表示起始点,绿色圆点表示目标点。因此,由图6可以看出,由传统A*算法生成的算法在障碍物周围产生许多多余的转角,大幅增加路径长度。
图6 路径平滑度优化示意图
Fig.6 Route smoothness optimization
路径平滑度优化规则判断如下:
步骤1 删除同一条直线路径中的中间点,只保留路径的起点、转折点和终点。以图6为例,优化后的路径为:S-n5-n6-n7-G。
步骤2 从起始点S开始正向判断,在保留的拐点之间每隔一定距离k取一个节点,并由式(5)计算障碍物到两点之间的连线的垂直距离是否在所设置安全距离之外。若符合,则保留该路径;若不符合,则不保留。以图6为例,优化后的路径为:S-n9-G。
步骤3 从目标点G开始反向判断,重复步骤2的取点方式,确保所选择的路径为安全路径。以图6为例,优化后的路径为:S-n11-G。
路径平滑度规则执行过程如图7所示。经过双向路径平滑度优化之后,使得路径的节点摆脱栅格地图中心点的限制,最终可以大幅减少路径的转折次数,增加路径的平滑度。
图7 路径平滑度优化判断流程
Fig.7 Route smoothness optimization flow
3 算法仿真
本文中将改进后的A*算法与传统A*算法在Matlab 2023a平台中进行对比实验,其中分为有效性实验和适应性实验。栅格地图中,黑色区域为障碍物,白色区域为可行动区域,设置单个栅格长度为1 m,“S”为路径规划起始点,“G”为路径规划目标点,蓝色线段为改进前后的A*算法规划的路径。设置改进算法中取参考节点的距离k为1、安全距离D为0.8 m。
3.1 有效性实验
为了测试改进后的A*算法是否在效率和路径平滑度方面相对于传统算法有所改进,对改进前后算法进行了有效性实验。该实验建立在一个30*30的栅格地图中,改进前后A*算法的对比如图8所示,实验结果数据如表2所示。
图8 A*算法改进前后有效性实验
Fig.8 Experimental comparison of the effectiveness of A* algorithm before and after improvement
表2 A*算法改进前后有效性对比实验
Table 2 Comparison of the effectiveness of A* algorithm before and after improvement
算法路径长度/m转折次数/次探索空间/节点数传统A∗算法43.3613264改进A∗算法43.306120
由图8可以看出,与传统A*算法相比,改进后的A*算法转折次数明显减少,路径更加平滑,且与障碍物保持在安全距离之外,可以有效避免运动对象与障碍物发生碰撞。
从数据方面看,虽然改进后A*算法生成的路径长度与传统A*算法差距不大,但是转折次数减少了54%,如此可以在不增加额外路线的前提下,大幅提升应用场景中路径使用者对于路径的满意程度。此外,改进后的A*算法在路径规划中探索的节点数显著减少,可以在保证路径精确度的同时,减少数据存储问题。
3.2 适应性实验
为了测试改进后的A*算法在不同场景中的表现差异,本文中进行了3组实验。为了对比不同组别之间的路径规划效果,这3组实验均建立在同一50*50的栅格地图中。其中,前2组实验为局部地图实验,即起始点到目标点所占区域约为全局地图的1/2;第3组实验则是全局路径规划对比实验。
前2组局部地图A*算法路径规划对比图如图9所示,图中蓝色实线表示改进后A*算法所规划的路径,而红色虚线则表示传统A*算法所规划出的路径;第3组对比图如图10所示,图10中灰色方格表示算法在路径规划过程中所探索过的栅格。
图9 第1、2组对比实验
Fig.9 Comparison of the 1st &2nd experiment
图10 第3组对比实验
Fig.10 Comparison of the 3rd experiment
图9中的两组对实验分别位于全局地图的左侧和右侧,通过对比改进前后A*算法路径规划结果,改进后的A*算法规划的路径避开了障碍物聚集区域,拐角更少、路径更平滑。因此,在实际应用场景中,可以在不增加路径长度的前提下,增加运动对象的安全性。
由图10的算法对比结果可以看出,改进后的A*算法在路径规划过程中遍历的栅格大幅减少,它不仅可以缩短算法运行时间、提高搜索效率,还能大幅减少计算存储空间。
上述进行的3组对比实验的结果数据如表3所示。改进后的A*算法最终生成的路径长度比传统算法平均减少5.3%,转折次数平均降低67.7%,转折角度也平均降低71.3%。由上述数据可以看出,改进后A*算法所规划出的路径不仅保持了传统A*算法高效的特点,还大幅减少了多余转角,使得最终生成的路径更平滑,安全系数也更高。
表3 三组A*算法改进前后结果对比
Table 3 Comparison of results before and after the improvement of three groups of A* algorithms
实验算法路径长度/m转折次数/次转折角度/(°)第1组传统A∗24.63379改进A∗22.58128第2组传统A∗40.046135改进A∗37.86153第3组传统A∗92.3613900改进A∗90.014107
4 结论
本文中提出一种基于改进A*算法的路径规划方法,成果如下:
1) 提出动态权重方案对传统A*算法的启发函数进行改进,进一步缩短A*算法的规划时间和生成路径长度;
2) 通过子节点优化策略,避免传统A*算法路径规划过程中出现的路线与障碍物碰撞问题,使路径更加安全;
3) 通过路径平滑度规则判断,大幅减少路径转折点、缩短路径长度、提升路径平滑度,同时,融合安全距离规则,使得路径在更平滑的基础上,与障碍物保持一定距离,适用多种运动对象。
当前,改进A*算法所面临的难题在于不能高效地适用不同的场景类型。本文中提出的改进方法,将特殊动态权重优化、子节点选择优化和路径平滑度优化3种优化方案同时融合进A*算法中,使得改进后的算法能更好地适应不同复杂度的应用场景。不仅如此,对于不同的应用对象而言,该改进算法还可以进一步保证运动对象的安全性。因此,本文中提出的改进方案体现出较好的工程应用价值,也给后续的A*算法改进提供了参考。
[1] 韩李涛,周丽娟,龚城,等.顾及步行习惯的室内导航网络及其生成算法[J].测绘学报,2022,51(5):729-738.HAN Litao,ZHOU Lijuan,GONG Cheng,et al.Anindoor navigation network considering walking habits and its generation algorithm[J].Acta Geodaeticaet Cartographica Sinica,2022,51(5):729-738.
[2] 林韩熙,向丹,欧阳剑,等.移动机器人路径规划算法的研究综述[J].计算机工程与应用,2021,57(18):38-48.LIN Hanxi,XIANG Dan,OUYANG Jian.Review of path planning algorithms for mobile robots[J].Computer Engineering and Applications,2021,57(18):38-48.
[3] PATLE B K,BABU L G,PANDEY A,et al.A review:On path planning strategies for navigation of mobile robot[J].Defence Technology,2019,15(4):582-606.
[4] HUANG F S,TANG S F,TONG Z Y.Overview of path planning methods for autonomous mobile robots[J].Software Guide,2008,17(10):1-5.
[5] TANG Z,MA H.An overview of path planning algorithms[J].IOP Conference Series:Earth and Environmental Science,2021,804(2):1-10.
[6] ZHANG L,MIN H,WEI H,et al.Global path planning for mobile robot based on A* algorithm and genetic algorithm[C]//2012 IEEE International Conference on Robotics and Biomimetics,Guangzhou,2012.
[7] CUI ZHI.Research on path planning of mobile robot based on A* algorithm[J].International Journal of Engineering Research and Technology,2020,8(11):653-656.
[8] FAN L,CAO H,GANWENXING W,et al.Dynamic adaptive security path planning based on A* algorithm[J].2021 International Symposiumon Intelligent Robotics and Systems:Conference Series,2022,2234(1):1-6.
[9] 吴鹏,桑成军,陆忠华,等.基于改进A*算法的移动机器人路径规划研究[J].计算机工程与应用,2019,55(21):227-233.WU Peng,SANG Chengjun,LU Zhonghua,et al.Research on mobile robot path planning based on improved A* algorithm[J].Computer Engineering and Applications,2019,55(21):227-233.
[10] XIANGRONG T,YUKUN Z,XINXIN J.Improved Astar algorithm for robot path planning in static environment[J].Journal of Physics:Conference Series,2021,1792(1):1-8.
[11] JING X,YANG X.S Application and improvement ofheuristic function in A* algorithm[C]//2018 37th Chinese Control Conference.Wuhan,2018:2191-2194.
[12] 唐彬,张德,彭田琳,等.基于改进A*算法的移动机器人路径规划研究[J].机械管理开发,2023,38(1):74-77.TANG Bing,ZHANG De,PENG Tianlin,et al.Research on path planning of mobile robot based on improved A* algorithm[J].Mechanical Management and Development,2023,38(1):74-77.
[13] 刘彪.移动机器人路径规划算法的研究与仿真[D].呼和浩特:内蒙古大学,2022.LIU Biao.Research and simulation of mobile robotpath planning algorithm[D].Hohhot:Inner Mongolia University,2022.
[14] 刘建娟,薛礼啟,张会娟,等.融合改进A*与DWA算法的机器人动态路径规划[J].计算机工程与应用,2021,57(15):73-81.LIU Jianjuan,XUE Liqi,ZHANG Huijuan,et al.Robot dynamic path planning based on improved A* and DWA algorithm[J].Computer Engineering and Applications,2021,57(15):73-81.
[15] JI X,FENG S,HAN Q,et al.Improvement and fusion of A* algorithm and dynamic window approach considering complex environmental information[J].Arabian Journal for Science and Engineering,2021,46(8):7445-7459.
[16] 绳红强,黄海英,崔毅刚.基于A*蚁群融合算法的避障路径规划研究[J].机电工程技术,2022,51(7):45-49.SHENG Hongqiang,HUANG Haiying,CUI Yigang.A research on obstacle avoidance path planning based on ant colony algorithm with A* heuristic method[J].Mechanical &Electrical Engineering Technology,2022,51(7):45-49.