基于局部障碍率预获取和双向父节点变更的A*算法优化

张志远,陈海进,章一鸣

(南通大学信息科学技术学院,江苏 南通 226019)

1 引言

随着人工智能产业的高速发展,扫地机器人和送餐机器人等逐渐在人们的生活中普及,为现代化生活注入了“信息”基因。作为移动机器人[1]研究领域的核心,路径规划问题[2-4]的研究自20世纪70年代至今从未停止,研究人员积累了大量的研究成果。主流的传统路径规划方法有可视图法[5,6]、拓扑法、栅格法、Dijkstra算法和A*算法[7,8]等,其中可视图法、拓扑法和栅格法通常应用于机器人感知环境信息的地图建模[9],而Dijkstra算法和A*算法通常应用于最优路径的求解[10-13]。

近年来,相关学者为优化机器人运动轨迹,提高机器人的运行效率,根据传统A*算法的缺点进行了各自相应的改进。文献[14]采用欧氏距离和曼哈顿距离的线性组合作为评价函数,使得评价值更接近实际值,一定程度上提高了搜索效率,但并未明确给出评价函数各部分的权值的分配方案。文献[15]根据机器人已走路径在总路径中的占比动态分配评价函数各个部分的权值,已达到优化评价函数的目的,一定程度上优化了轨迹平滑度。但是,该算法不考虑地图局部障碍率的高低,可能导致在局部障碍率较低的区域搜索较大的空间,降低搜索效率;也可能导致在局部障碍率较高的区域搜索的空间较小,从而错失最优节点,陷入局部最优。文献[16]加权处理评价函数,并通过人工搜索标记,缩小了搜索区域,牺牲搜索精度换取了搜索效率。文献[17]提出一种动态启发式算法,避免算法陷入局部最优,在保证搜索效率的前提下优化了生成路径。文献[18]在复杂地形寻路的过程中,在评价函数中加入安全成本,在保证运行效率的同时,提高了机器人寻路的安全性。然而路径规划问题应不仅与搜索算法相关,更应该与环境地图密切相关。

本文提出了基于局部障碍率预获取和双向父节点变更的改进A*算法,能有效地识别局部环境信息,依据环境复杂程度动态调整搜索空间,提高搜索效率,优化路径长度。首先,基于漂移矩阵预获取地图各个部分的局部障碍率信息,记录各个矩阵内障碍率的值。其次,为矩阵内障碍率的值建立映射函数,通过映射函数动态分配评价函数的权值,依据局部环境复杂程度灵活调整搜索空间。最后,依据改进的父节点变更方式,减少生成路径的冗余点和拐点,进一步优化路径。实验结果表明,本文改进后的算法有效缩短了路径长度,提升了搜索效率,具有较强的适应性。

2 传统A*算法

2.1 栅格法地图建模

传统A*算法用栅格法进行地图建模,用二进制编码0和1来表示环境地图,0表示无障碍栅格,1表示障碍栅格。设机器人的工作区域为a′×b′的矩形区域,栅格单位长度为c,所得栅格地图为m×n的矩阵,m和n数值如式(1)所示:

(1)

其中floor(·)为向下取整函数。

2.2 A*算法路径规划

A*算法每移动一格都要对周围8邻域栅格内的评价函数进行计算,其计算公式如式(2)所示:

(2)

其中,(x,y)为当前栅格s的坐标,(xS,yS)为起点S的坐标,(xD,yD)为终点D的坐标,g(s)为当前栅格s到起点的欧氏距离,h(s)为当前栅格s到终点的欧氏距离。依次将8邻域内的可选栅格(无障碍栅格、非已走过栅格)放入open列表,并从open列表中选出评价函数f(·)值最小的栅格作为下一个节点栅格,并将当前节点栅格放入close列表。传统A*算法从起点出发搜索至终点结束,生成路径。

3 分配权重的A*算法及其相关性质证明

分配权重的A*算法对h(s)和g(s)赋予不同的权重a和b,以达到增大或缩小搜索区域的目的,其评价函数如式(3)所示:

fweight(s)=a·h(s)+b·g(s),a+b=1

(3)

对于分配权重的A*算法,有如下结论:

结论1当a<0.5

证明如图1所示,当a<0.5g(B),且点B满足以下关系:

Figure 1 Proof of the relationship between function weight and search area size

BS+BD=AS+AD=

g(B)+h(B)=g(A)+h(A)

(4)

由式(3)可得:

(5)

式(5)中两式相减后,令b=a+a1,经变换得:

fweight(A)-fweight(B)=

a·(h(A)+g(A)-(h(B)+g(B)))+

a1·(g(A)-g(B))

(6)

由式(4)可得:

fweight(A)-fweight(B)=a1·(g(A)-g(B))

(7)

因为g(A)-g(B)>0,故在点B处,fweight(A)-fweight(B)>0。

过点B作起终点连线SD的垂线,当点B在垂线上向远离SD方向移动时,h(B)和g(B)值也随之增大,记增大后的值为h′(B)和g′(B),则:

(8)

其中,Δh(B)和Δg(B)为点B远离SD带来的增量。

由式(8)可将式(6)变换为:

fweight(A)-fweight(B)=-a·(Δh(B)+Δg(B))-

a1·Δg(B)+a1·(g(A)-g(B))

(9)

式(9)中第1项和第2项为负值增量,第3项为正值定量,可以看出点B移动产生的负值增量使得式(9)的值逐渐向0逼近,直至式(9)的值为0,点B停止运动,记此时点B运动至点P,fweight(A)=fweight(B)。

对于传统A*算法,由式(2)和式(4)可得,在点B处就已经满足f(A)=f(B)的关系,所以三角形SBD区域内的f(·)值均小于f(A)。故在点A被选取之前,三角形SBD区域内的点依次会被回溯选取为当前节点。三角形SBD即为此时新增的搜索区域。而对于分配权重的A*算法(a<0.5

□

结论2当a>0.5>b时,分配权重的A*算法搜索区域小于传统A*算法的搜索区域。

证明式(5)中两式相减后令a=b+b1,经变换可得:

fweight(A)-fweight(B)=

b·(h(A)+g(A)-(h(B)+

g(B)))+b1·(h(A)-h(B))

(10)

由式(4)得:

fweight(A)-fweight(B)=b1·(h(A)-h(B))

(11)

因为h(A)-h(B)<0,故在点B处,fweight(A)-fweight(B)<0。

将垂线上的点B向靠近SD方向移动,h(B)和g(B)的值随之减小,记减小后的值为:

(12)

随点B向靠近SD方向移动,由式(12)可将式(10)改写为:

fweight(A)-fweight(B)=

b·(Δh(B)+Δg(B))+

b1·Δg(B)+b1·(h(A)-h(B))

(13)

式(13)中第1和第2项为正值增量,第3项为负值定量,可以看出点B向靠近SD方向移动产生的正值增量使式(13)的值逐渐向0逼近,直至其值为0,点B停止运动,记此时点B运动至点C,fweight(A)=fweight(B)。显然,a>0.5>b时,三角形SCD即为新增的搜索区域,其面积小于三角形SBD,结论2得证。

□

结论3分配权重的A*算法搜索区域大小与a的值成反比,与b的值成正比。

证明当aΔg(B),所以总的负值增量减小,故点B需要运动到比点P更远的位置才能使式(9)的值为0。在此种情况下,算法搜索区域增大,搜索区域大小与b值成正比,与a值成反比。

当a>b>0.5,a=b+b1时,增大a的值,b1的值增大,b的值减小,使得式(13)中,第1项正值增量系b数减小,第2项b1正值增量系数增大,由于Δh(B)+Δg(B)>Δg(B),总的正值增量减小,故点B需要运动至比点C更靠近SD的位置才能使式(13)的值为0。此时算法搜索区域缩小,搜索区域大小仍与b值成正比,与a值成反比,结论3得证。

□

4 基于局部障碍率预获取的改进A*算法

本文提出基于局部障碍率预获取的改进A*算法,利用漂移矩阵在对地图的局部障碍率信息进行预获取后,通过第3节分配权重评价函数已证明的相关结论,将量化后的局部障碍率信息融入评价函数中,进一步对权重的分配方式进行改进,使得在障碍物复杂程度不同的各个区域内,算法可依据环境信息自适应调整搜索空间,在生成路径最优化的同时,也提升了搜索的灵活性,确保算法高效运行。

4.1 基于漂移矩阵获取地图局部障碍率

基于栅格法对机器人工作环境进行地图建模。设所获栅格地图为m×n的矩形。构建漂移矩阵A1。在同一地图范围内,漂移矩阵的边长l取值越小,漂移矩阵数量越多,所获取的局部障碍率信息越丰富。显然,在同一位置用不同大小的漂移矩阵获取局部障碍率,其边长l越小,局部障碍率的精度越高,越能准确反映当前位置附近的环境信息。本文中l取3个单位栅格长度,为可取范围内的最小取值,所得漂移矩阵为当前位置邻域内3×3大小的栅格矩阵。

用式(3)作为评价函数的A*算法,给予h(s)高权重,快速规划出一条自起点至终点的路径,漂移矩阵在A*算法寻路过程中跟随移动并记录当前矩阵内部障碍率,每当寻路所在当前栅格超出漂移矩阵范围时,则重新构建漂移矩阵,记当前栅格坐标为(x,y),记第i个漂移矩阵Ai的障碍率值如式(14)所示:

(14)

其中,oi是矩阵中值为1的栅格(障碍栅格)的数目。记最后一个漂移矩阵的局部障碍率为pk,从p1到pk中获得地图局部障碍率p的取值在[pmin,pmax]。基于漂移矩阵获取地图局部障碍率具体流程如图2所示。

Figure 2 Flow chart of drift matrix algorithm

图3所示为基于漂移矩阵获取地图局部障碍率的实例,设栅格法建模所获地图为30×30矩阵,起点S坐标为(1,1),终点D坐标为(30,30),漂移矩阵为3×3大小的栅格矩阵。按图3所示流程,由于起点(1,1)的横纵坐标值均小于2,故算法首先以a1(2,2)为中心构建包含起点的漂移矩阵A1,并记录当前矩阵内的障碍率值p1。当A*算法寻路至a2点时,当前位置坐标(4,4)超出漂移矩阵A1所在范围,且其横纵坐标值均大于2,故以当前位置(4,4)为中心构建新的漂移矩阵A2,同样记录当前矩阵内的障碍率值p2,以此类推,当算法寻路至终点D时,统计漂移矩阵A1至An内障碍率的极大值pmax和极小值pmin,获取局部障碍率p值的取值区间[pmin,pmax],算法结束。

Figure 3 Schematic diagram of map local obstacle rate pre-acquisition

4.2 评价函数权重比例的改进

由第3节已证明的相关结论可知,分配权重的评价函数通过改变各项的权重比例可以扩大或缩小搜索空间。当h(s)的权重增大时,算法的表现为搜索区域缩小,运行效率提升,更偏向于选择距终点代价更小的路径,会陷入局部最优;当h(s)的权重减小时,搜索区域逐渐增大,运行效率降低,但往往能够优化生成路径的长度。为了平衡启发式信息强度和优化生成的路径,本文将4.1节基于漂移矩阵预获取的障碍率信息融入评价函数中,设计如式(15)所示的权重关系函数:

(15)

其中,c和d为权重调节因子,p为局部障碍率。

将式(15)代入式(3)可得:

(16)

当p值变化时,g(s)的权重与h(s)的权重始终成反比,且二者之和为1。当d/(c+p)取值为0.5时,式(16)改写为:

fweight(s)=0.5·g(s)+0.5·h(s)

(17)

式(17)中2项权重均为0.5,搜索空间和生成路径与传统A*算法一致。c、d为常数,与预获取的局部障碍率信息pmin、pmax满足以下关系:

(18)

通过式(18)确定常数c、d的值,同时对应p的取值在[pmin,pmax]内,权重d/(c+p)和(c-d+p)/(c+p)的取值始终在[λ1,λ2],λ1和λ2为搜索区域调节因子,当λ1和λ2分别处于0.45和0.55的邻域内时,算法既不会因为g(s)的权重过低(<0.45)而产生较多的冗余点和拐点,也不会因为g(s)的权重过高(>0.55)而产生过多无用的搜索空间。评价函数的权重始终维持在一个合理的范围之内,在不同的漂移矩阵内依据其内部的局部障碍信息,自适应地调整权重,按需产生不同的有效的搜索空间,在对路径优化的同时,也保证了算法高效、灵活地运行。

5 父节点变更方式优化

A*算法作为一种启发式搜索算法,在寻路过程中会产生多条子路径,而最先找到终点的路径为最终生成的规划路径,该条件下,各个子路径之间栅格父节点的变更对路径优化起着至关重要的作用。本文在传统A*算法父节点变更的方式上进行改进,以有效减少生成路径的冗余点,进一步优化路径。

如图4所示,栅格V及其它空白栅格为未选栅格,Xn+1～Xn+7按下标顺序先后被选为当前栅格,设置子父节点的链接后放入close列表(图中黑色箭头的起点为父节点,终点为指向子节点)。设由起点start→栅格V为路径route1,由起点start→栅格Xn为路径route2,且route1的长度远小于route2的。显然,路径route1→Xn+4→Xn+2→Xn+5要优于实际路径route2→Xn+1→Xn+2→Xn+5。这是由于分配给h(s)较高的权值,算法遵循终点最优原则,会优先选取靠近终点的栅格作为当前栅格,搜索区域也相对较小,因此错失更优路径上的栅格V,因而错失全局最优路径,陷入局部最优。

Figure 4 Pointing diagram of child and parent nodes in traditional A* algorithm

针对上述问题,本文对传统A*算法的父节点变更方式进行改进,在检测由起点至当前栅格到子路径栅格是否存在更优路径的同时,也检测子路径栅格所属路径到当前栅格是否存在更优路径,由原本的单向访问优化为双向访问,在保证搜索空间有限的前提下不遗漏更优路径,确保路径的全局最优性。

步骤1从待选栅格列表选取栅格作为当前栅格,并记当前栅格current的8邻域内的栅格依次为currenti(i=1,…,8),判断当前栅格是否为终点,若是,算法运行结束,若不是,转到步骤2。

步骤2判断i是否等于8,若等于返回步骤1,若不等于,i=i+1,转到步骤3。

步骤3判断currenti是否为障碍,若为障碍,返回步骤2;若不为障碍,转到步骤4。

步骤4判断currenti是否在open列表中,若在open列表中,转到步骤5;若不在,则将currenti父节点设为当前栅格current,i=i+1,转到步骤2。

步骤5判断经由所属路径到达currenti是否更优,若更优则将currenti的父节点变更为当前栅格current,转到步骤2。

步骤6判断经由currenti所属路径到达当前栅格current是否更优,若更优,则将当前栅格的父节点变更为currenti,同时将currenti作为下一个待选栅格放入待选栅格列表,转到步骤2。

图5a所示为改进前的示意图,当选中Xn+1为当前栅格时,经其右上角栅格V所属路径route1到达当前栅格Xn+1,显然比Xn+1所属路径route2代价更小。改进后,经由route1→Xn+4→Xn+2→Xn+5的路径要优于原先由route2→Xn+1→Xn+2→Xn+5的路径。可以看出,在h(s)权值较高,搜索区域较小的情况下,即使所属更优路径的栅格V没有被选为当前栅格,按改进后的父节点变更方式,也可以通过其相邻栅格检测出V所在的更优路径,并以V为当前栅格进一步对该条路径进行延续性扩展,由局部路径优化延续为全局路径优化,有效减少生成路径的冗余点。

Figure 5 Comparison diagram of parent node changes before and after improvement

6 仿真与结果分析

6.1 基于局部障碍率预获取的改进A*算法仿真

为验证基于局部障碍率预获取的改进A*算法的适应性和有效性,本节选取3个不同尺寸和障碍物密度的栅格地图,将本文算法和传统A*算法及文献[15]引入路径权值的改进A*算法进行对比,3组实验的对比仿真结果如图6所示。

Figure 6 Comparison of simulation results among three algorithms

本次对比实验是理想仿真,建立了30×30的移动机器人工作环境地图,假定环境地图的栅格边长为1 m,对角线长度为1.42 m,为测算实际情况下机器人按实验路径寻路所花费的时间,假设机器人的移动速度v=0.5 m/s,原地旋转45°所需时间为1 s。3种算法的评价指标为:栅格搜索个数(搜索区域大小)、路径长度、转角次数和机器人实际寻路时间。本文具体实验数据统计结果如表1所示。

Table 1 Analysis of three algorithms simulation results

从表1的对比仿真数据可以看出,传统A*算法不能很好地识别地图信息,陷入了局部最优,增加了不必要的冗余节点,转弯次数多,消耗了机器人的移动时间。对于文献[15]中引入路径权值的改进A*算法,相较于传统A*算法扩大了搜索空间,缩短了路径长度,减少了转角次数。但是,依靠路径权值对搜索空间的调整,没有达到依据具体环境按需分配搜索空间的目的。比较表1中数据可以看出,本文算法在局部复杂地区,扩大了搜索空间,确保路径最优,在局部障碍率低的区域,缩小了搜索空间,提升了搜索效率,不但在搜索空间个数这一指标上明显下降,较文献[15]中引入路径权值的A*算法,在3种不同地图环境中的路径长度分别由50.34 m缩短为47.82 m,47.08 m缩短为44.66 m,46.4 m缩短为45.24 m,且转弯次数这一指标上也有明显提升,有效缩短了机器人在实际环境工作中的寻路时间。

6.2 优化父节点变更方式算法仿真

为验证改进的父节点变更方式可进一步优化生成路径,本节在6.1节实验的基础上将改进的父节点变更方式融入算法,其仿真结果对比如图7所示。

Figure 7 Comparison before and after optimizing parent node

图7中,S为起点,D为终点,黑色栅格为障碍,白色栅格为无障碍区域,黑色实线为寻路路径。图7a即为优化父节点前图6c的算法仿真结果,图7b为优化父节点后的算法仿真结果。可以看出,在地图左上角的局部区域内,障碍率较低,依据式(16),本文算法分配给h(s)较高的权值,以达到缩小搜索区域,提高运行效率的目的,但也因此错过更优路径,陷入局部最优。在这种情况下,改进的父节点变更方式很好地弥补了这一弊端,将路径总长度由47.82 m缩短为46.98 m,在保证运行效率的同时,筛选出了当前区域内的最优路径并延续扩展至终点,由局部最优上升为全局最优,有效地减少了生成路径的冗余点。

7 结束语

本文就机器人如何更高效地识别和利用已知地图进行路径规划的问题,提出了一种基于漂移矩阵地图预获取的改进A*算法。通过漂移矩阵预获取地图各个部分的局部障碍信息,并融入评价函数,使得算法能在不同复杂程度区域按需调整搜索空间,并用改进的父节点变更方式进一步优化生成路径。仿真结果表明,本文算法较传统算法更具适应性和有效性,在相同大小的搜索区域内,本文算法可以获得更短的路径长度,更少的转角次数,有效减少生成路径的冗余点;在获得同等路径长度的条件下,可以有效缩小搜索空间,缩短运行时间,提高机器人的运行效率。