融合改进A*算法和优化动态窗口法的路径规划

邹 文,韩丙辰,李鹏飞,田剑锋

(1.太原师范学院 计算机系,山西 晋中 030619;2.太原师范学院 物理系,山西 晋中 030619)

0 引言

移动机器人因其自主导航、灵活移动等特点,被广泛用于物流运输、日常服务、工业制造等多个领域。路径规划是移动机器人诸多技术中不可或缺的重要组成部分,目的是在有障碍物的环境中从起点到目标点规划出一条无碰撞且较优的路径。按照机器人移动过程中对环境感知的不同,可分为全局规划和局部规划两大类。全局路径规划主要应用于已知的静态环境,常用的算法有A*算法[1]、D*算法[2]、人工势场法[3]等;局部路径规划主要解决机器人移动过程中遇到的未知障碍物,常用的算法有动态窗口法[4]、粒子群算法[5]、遗传算法[6]、蚁群算法[7]等。

A*算法是一种经典的启发式搜索算法,具有计算过程简单、规划路径短等优点。但是传统的A*算法存在计算量大、耗时长,以及路径平滑度低、拐点多等缺点。对此,一些研究对A*算法的启发式搜索过程进行优化。例如:MIN等[8]通过设置冗余安全空间来避免轮廓碰撞,并在启发式函数设计中加入了路径曲率的代价,提高了路径的平滑性,得到一条更加满足车辆运动约束的路径。槐创锋等[9]将传统A*算法3×3的搜索领域扩展为7×7,优化搜索角度,减少了折点提高了平滑度。或者将A*算法已经规划出的路径进行优化处理,如文献[10]～文献[13]不同程度地结合了Floyd算法思想优化路径,删除了冗余的节点,减少转折。这些研究都提高了路径的平滑度,且保证了路径长度最优,但都只是针对规划的路径效果,而没有考虑到A*算法启发式搜索时的计算效率,均未减小全局路径规划的计算量和消耗时间。王洪斌等[14]提出多目标点和自适应圆弧优化算法得到多目标路径并对其进行平滑处理,有效提高了搜索效率和路径的平滑度,但是未说明多个目标点如何确定。王云峰等[15]在A*算法的启发函数中加入“方向函数”来减少仓储自动引导车(Automated Guided Vehicle,AGV)的转弯次数和冲突节点数量,有效地降低了总完成时间,但该方法仅适用于仓储大规模AGV环境下。

动态窗口法(Dynamic Window Approach,DWA)可以使机器人在移动过程中及时避让未知的障碍物,提高路径平滑度,但存在运动状态单一、容易陷入局部最优陷阱等缺点。为此,一些研究者将A*算法和改进动态窗口法相融合,封硕等[16]在动态窗口法的评价函数中扩展动态障碍物的速度信息,并结合优化A*算法,获得全局路径的同时提高了局部实时避障能力。文献[17]将A*算法规划的路径作为DWA算法的全局路径指引,既保证了全局路径最优,又避免机器人陷入局部最优陷阱。或者结合环境信息来优化DWA算法,MAI等[18]在动态窗口法路径评价函数中引入与密度变化相关的评价因子,提前避免进入密集区域,改进后的动态窗口法具有相对稳定的速度和加速度。这些研究都避免机器人陷入局部最优,增强局部规划能力,但都未改变DWA算法仅有单一运动状态的缺点,机器人遇到障碍物仅会避让,而不会选择其他运动状态,可能会使机器人的移动路径过长、转折角度过大。

上述文献不同程度地对A*算法和动态窗口法进行了优化,但都未解决A*算法搜索节点计算量大、耗时长以及动态窗口法运动状态单一、灵活性差的问题。对此,本文通过建立拓扑层地图来减少A*算法搜索时的节点数量和耗费时间,并在栅格地图中对路径进行优化得到关键折点,再融合增加运动状态的优化DWA算法完成机器人的局部动态规划,最终为机器人规划出长度较优,又能对环境情况作出灵活反应的路径。

1 改进A*算法

1.1 A*算法基础

A*算法是一种求解最短路径很有效的启发式搜索方法之一,其融合了Dijkstra算法和Best First(路径优先)算法的优点。它的核心代价函数为:

f(n)=g(n)+h(n)。

(1)

式中:f(n)表示当前节点栅格n的总移动代价值;g(n)表示当前节点栅格n到初始点的移动代价值;h(n)表示当前节点栅格n到目标点的移动代价值。每次选取f(n)值最小的栅格作为下一次拓展的父节点,h(n)是代价函数最重要的部分,本文算法注重减少计算量,故选取欧式距离作为计算h(n)移动价值的代价函数,该函数表示为:

(2)

式中:(xn,yn)表示当前节点栅格所在的坐标;(xg,yg)表示目标点所在栅格的坐标。

1.2 双层优化A*算法

1.2.1 拓扑地图

当在较大的场景下且需要精度高的环境模型时,栅格数量会呈现指数型增长,从而导致计算时间的增加。本文首先在栅格地图之上建立一层拓扑地图,并在拓扑层地图中使用传统A*算法得到初步全局路径。在栅格地图上,以固定间隔为单位距离取出栅格点作为拓扑点,建立一个拓扑层地图。间隔距离可以根据不同情况进行设置。本文取间隔距离为10,建立的拓扑层地图如图1所示。

图1 建立拓扑层地图

1.2.2 最优中继点选取

在双层地图中,栅格层中的初始点和目标点往往不在拓扑点上,因此在路径规划前需要先找到离初始点和目标点最优的拓扑点分别作为拓扑层路径规划的起点和终点。被选中的拓扑点分别命名为初始中继点和目标中继点。

首先将拓扑点命名,如图2所示,将初始点周围的4个拓扑点顺时针依次命名为P01,P02,P03,P04,目标点周围的4个拓扑点顺时针依次命名为P11,P12,P13,P14。然后用目标点坐标减去初始点坐标,根据坐标差值的正负分别选出初始中间点和目标中继点,判别公式如下:

(3)

图2 拓扑点命名

式中:(Δx,Δy)表示坐标差值;(xg,yg)表示目标点坐标;(xo,yo)表示初始点坐标。差值分类及相应选择如表1所示。

表1 中继拓扑点选择

1.2.3 全局优化路径

在建立好大栅格层地图以及确定最优中继点后,本文先在大栅格层地图中使用传统A*算法进行路径规划,得到初步全局路径,之后再结合Floyd算法思想在小栅格地图中进一步对全局路径优化。在减少计算量、缩短计算时间的同时又得到长度更短、转折点更少的全局路径。具体步骤归纳如下:

步骤1在大栅格层地图中,以初始中继点为起点,目标中继点为终点,用传统A*算法进行路径规划得到路径1,并记录路径1经过的拓扑点的坐标,将其存入列表1中。使用拓扑层得到初步路径会极大减少A*算法在启发式搜索时需要计算的节点数量,较原本栅格层节点数量减少的比例计算公式如下:

(4)

式中:m表示栅格总长度;d表示拓扑点间隔的栅格数量。如图1所示在栅格层中取间隔为10,栅格总数量由图1a中的50×50减少到图1b中拓扑点量为5×5,节点数量减少比例为99%。

步骤2将列表1中的拓扑点依次转换到小栅格地图中的坐标,并在列表第一行和最后一行分别加上初始点坐标和目标点坐标,存入关键点列表2中。

表2 改进算法性能对比

步骤3判断列表2中相邻两个点的连线上有无障碍物,若有则在相关两个点之间用传统A*算法进行路径规划,并得到该段路径的关键折点。将关键折点存入关键折点列表Kpoint中,并同时将关键折点加入列表2中相应两个点之间,得到新的列表3。若无则继续下一步,此步骤得到路径2。

表3 改进DWA算法性能对比

步骤4将关键折点列表3中的第一个点取出,作为起点与列表3中比该点更靠近终点的拓扑点依次连接,并判断连线上有无障碍物,直到连线上有障碍物为止。若与第n个点连线有障碍物,则将第n-1个点提取出来存入列表4中。

表4 融合改进算法性能对比

步骤5将第n-1个点作为新的起点,执行步骤4,直到连接到目标点为止。将列表4中所有点连接起来,得到最终全局路径3。

改进算法全局规划的步骤路径如图3所示。

图3 改进A*算法规划路径步骤

2 优化动态窗口算法

2.1 传统动态窗口法

动态窗口法是常用的机器人局部规划算法,其既能有效地避开路径中出现的未知障碍物,又能提升路径的平滑性。该算法主要是在速度空间(线速度、角速度)中采样多组速度,并模拟这些速度组在下一段时间间隔内的运动轨迹,将与障碍物碰撞的轨迹剔除,最后通过评价函数选取出最优的模拟轨迹所对应的速度组来驱动机器人完成相应的运动。

2.1.1 机器人运动学模型

动态窗口法针对机器人在窗口区域内的速度空间进行采样,并根据机器人运动学模型,模拟出下一时间间隔Δt内的运动轨迹,在一个最小单位时间内,假定机器人做匀速直线运动。速度空间的线速度v和角速度w反馈出的便是机器人的运动状态。其运动模型可表示为:

xt=xt-1+vx×Δtcosθt-vy×Δtsinθt;

yt=yt-1+vx×Δtsinθt-vy×Δtcosθt;

θt=θt-1+wt×Δt。

(5)

2.1.2 机器人速度采样

在速度空间(v,w)的二维平面中存在无穷组速度,根据机器人自身性能和环境限制,对采样速度的范围进行约束:

vm={v∈[vmin,vmax],w∈[wmin,wmax]}。

(6)

因加速度受电机性能约束,在一个时间间隔Δt内,机器人实际能到达的速度为:

(7)

式中:(vc,wc)表示当前速度组,(va,wa)表示机器人最大加速度组,(vb,wb)表示机器人最大减速度组。

2.1.3 评价函数

采样的各速度组的模拟轨迹中,除去与障碍物发生碰撞的轨迹外,还存在很多组可行轨迹,因此通过以下评价函数筛选出最优的轨迹:

G(v,w)=σ(α×heading(v,w)+

β×dist(v,w)+γ×velocity(v,w))。

(8)

式中:heading(v,w)为方位角评价子函数,表示对应速度组模拟轨迹末端时的朝向与目标位置航向角之间的角度偏差;dist(v,w)为障碍物评价子函数,表示对应速度组模拟轨迹与障碍物最近的距离;velocity(v,w)为速度评价子函数,表示模拟轨迹的速度大小;σ为平滑系数,α,β,γ为加权系数。

2.2 优化运动状态的动态窗口法

传统的动态窗口法仅在已知的全局静态障碍物的情况下,有良好的避障性能,但当路径中出现较多的移动障碍物时,传统动态窗口法存在灵活性差、避障路径较长和转折角度过大等缺点。针对上述问题,本文通过增加行为状态来优化动态窗口法。

2.2.1 增加刹车等待运动状态

针对传统动态窗口法运动过程中仅有单一运动状态,即简单地朝目标点移动以及持续地避开障碍物,只要未到达目标点,机器人会持续不断地运动。本文提出增加刹车等待和保持跟随两种运动状态。

当机器人遇到动态的L型陷阱时,传统动态窗口法会规划出与移动障碍物运动方向相近的朝向或转折角度过大的路径,此时移动距离过长,路径不是最优或较优。故本文提出增加刹车等待的运动状态,当同时满足以下情况时:移动障碍物处于机器人前进方向;与机器人距离小于d1;障碍物移动速度大于v1;障碍物移动方向与机器人规划当前朝向夹角大于π/4,如图4所示。此时,机器人将等待移动障碍物消失在规划的路径前方,再向前运动。d1与v1参数可根据机器人自身不同情况进行设置。

图4 刹车等待示意图

2.2.2 增加保持跟随运动状态

当移动障碍物处于机器人前方且运动方向与机器人相同或相近时,传统动态窗口法仅会向左或向右绕开障碍物,但实际运动过程中,往往机器人跟随障碍物移动才是最优的路径。故本文提出增加保持跟随运动状态。当同时满足以下情况时:移动障碍物处于机器人前方的全局路径之上;与机器人距离小于d2;障碍物移动速度大于机器人最大速度约束的2/3;障碍物移动方向与机器人规划当前朝向夹角小于π/12,如图5所示。此时,机器人将保持原本航线不变,并将最大速度约束设置为障碍物移动速度。d2参数可根据机器人自身不同情况进行设置。

图5 保持跟随示意图

3 融合算法

经过本文改进后的A*算法能快速地规划出一条路径短,折点少的全局路径,但不能及时地避让环境中出现的移动障碍物,且存在路径转折角度较大、与障碍物距离过近等不足;经本文优化后的动态窗口法不能做到全局最优路径,且容易陷入局部最优陷阱中。

针对两种优化算法中存在的不足,本文将改进后的A*算法和优化后的动态窗口法进一步融合。将改进A*算法规划出的全局路径关键点提取出来,作为优化动态窗口法各个阶段的临时目标点。融合算法在确保全局路径最优的同时又保证了局部规划时的良好避障和移动效果。融合算法流程如图6所示。

图6 融合算法流程图

4 仿真实验及分析

为了验证本文改进A*算法和优化动态窗口法的改进性能,以及融合算法的有效性,在MATLAB 2016a环境下进行仿真对比试验,以及进行实物实验,并对各实验结果进行分析。

4.1 改进A*算法仿真实验及分析

改进A*算法仿真环境为二维栅格地图,其中黑色栅格表示障碍物,白色栅格表示可通过路径点。为验证改进A*算法对于规划路径长度、耗费时间以及折点数方面性能的提升,分别在50×50、75×75以及100×100大小的栅格地图中对A*算法改进前后进行仿真对比试验,结果分别如图7～图9所示。上述仿真实验结果统计如表2所示。

图7 50×50栅格地图场景下仿真结果

图8 75×75栅格地图场景下仿真结果

图9 100×100栅格地图场景下仿真结果

结合图7～图10以及表2可知,在相同的静态环境中,相比于传统A*算法,改进A*算法因为首先进行拓扑层地图的路径规划,所以极大地减少了启发式搜索时需要计算的栅格数量,使耗费时间大幅减少;并删除了路径中冗余的折点,使路径不但长度减少而且更加平滑。联合图11和表2可知,当地图大小由50×50增加到100×100时,各项性能指标优化效果更加明显,尤其是耗费时间减少比例由38.6%增加至71.8%。由此可见,地图越大、栅格数量越多的场景越适合用改进A*算法。

图10 算法改进前后各性能对比

图11 改进A*算法不同大小地图中性能优化趋势

4.2 优化动态窗口法仿真实验及分析

针对不同的移动障碍物场景,分别对改进动态窗口法和传统动态窗口法进行仿真实验。仿真环境为15×15二维地图,障碍物及影响范围用圆形表示,设置多个静态障碍物和两个移动障碍物。

针对动态L型陷阱仿真对比结果如图12和图13所示。针对同向动态障碍物场景仿真结果如图14和图15所示。

图13 优化动态窗口法路径

图14 传统动态窗口法路径

图15 优化动态窗口法路径

由图12、图13和表3可看出,传统动态窗口法遇到动态的L型障碍时,规划出的路径弧度过大导致最终路径长度较大,而优化动态窗口法能合理地刹车等待,规划出的路径长度由27.82减少到18.30,花费时间由43.93 s减少到35.16 s。由图14和图15可看出,遇到移动障碍挡在航线前方时,传统动态窗口法只会单一地绕过障碍物,最终的路径可能偏离原本的航线,导致路径长度过长;而动态窗口法能根据航线前方的障碍物速度选择跟随障碍物,规划出的路径长度由28.57减少到15.37,花费时间由43.52 s减少到34.76 s,改进动态窗口法规划出的路径更短、更合理。

4.3 融合算法仿真实验及分析

融合算法的仿真实验设置两个仿真环境,第一个大小为50×50的栅格地图,第二个大小为75×60的栅格地图。障碍物为黑色栅格和移动的红色圆圈,黑色小圆圈表示机器人当前位置,蓝色实线表示路径。为验证融合算法的合理性,在第一个仿真环境中的静态环境中用改进A*算法、优化动态窗口法以及改进后的融合算法进行路径规划仿真实验;在动态环境中用改进A*算法分别与传统动态窗口法和本文优化后的动态窗口法融合作对比仿真实验。其中静态环境仿真结果如图16所示,动态环境仿真结果如图17～图20和表4所示。

图16 静态环境

图17 传统融合算法用于动态环境

由图16可知,改进A*算法能快速有效地规划出从起点到终点的最优路径,但是改进A*算法无法实时规划路径,不能有效躲避移动障碍物;优化动态窗口法没有全局目标点的指引,可能会陷入局部最优,寻路失败;融合改进A*算法和动态窗口法的融合算法不但能有效地规划出路径,而且路径平滑度更高,曲率连续,且能和障碍物保持合适的安全距离。

第二个仿真环境中,除在区域1和区域4设置移动速度较快的动态L型障碍和同向移动障碍外,还在区域2和区域3设置移动速度缓慢的动态L型障碍和同向移动障碍。

由图17～图20可知,传统融合算法和改进融合算法都能有效地避开动态障碍物到达终点,但是传统融合算法遇到不同的动态环境时,都只有单一的避让状态,而改进融合算法能根据障碍物情况选择不同的运动状态。对比图17、图18及表4可知,在遇到动态L型障碍和同向移动障碍时,改进算法会分别进入刹车等待和保持跟随状态,路径长度由79.65减少到66.45,遇到动态L型障碍路径曲率由20.17减少到3.25,遇到同向移动障碍时曲率由12.74减少到7.91。对比图19和图20及表4可知,改进融合算法能根据环境中移动速度缓慢的障碍物选择避让状态,而对移动速度快的障碍物情况灵活地选择刹车等待、保持跟随状态,整体路径长度由103.72减少到97.67,曲率变化更小。

图18 改进融合算法用于动态环境

图19 传统融合算法用于动态环境

图20 改进融合算法用于动态环境

联合图17～图20及表4可得,改进融合算法能根据不同的环境情况灵活地选择不同的运动状态,既减少了整体路径长度,又减少了经过障碍物时的转折角度,增强了机器人路径规划时的灵活性。

4.4 实验验证及分析

为验证本文算法在实际应用中的有效性,除仿真实验外,还增加实际实验。实验选用搭载机器人操作系统ROS(Kinetic)的移动机器人,实验场地与融合算法的第二个仿真环境类似,环境中以行人构建动态L型和同向移动障碍,实验场地和用Gmapping算法构建的代价地图如图21所示,实验结果如图22和图23所示,图中红色箭头反映转折角度。

图21 实验场地及代价地图

图22 传统融合算法

图23 改进融合算法

由图22和图23可知,两种融合算法都能避让障碍物,使机器人安全到达终点。对比图22和图23中的区域a和区域b可证明,当机器人在遇到动态L型障碍和同向移动障碍时,相比于传统融合算法,选用改进融合算法规划出的路径不仅长度更短,转折角度也更平缓,规划出的路径更合理、更有利于机器人移动。

5 结束语

为提升传统A*算法在栅格数量较多时的路径规划效率以及增强传统动态窗口法在不同场景下的灵活性。本文通过建立拓扑层地图并删除多余折点来改进A*算法,通过增加运动状态来优化动态窗口法,并融合两种改进算法。融合算法不仅能快速地规划出一条折点数少路径较优的全局路径,还能在机器人移动过程中有效地对各种场景下的移动障碍物作出不同的状态变化。通过对比试验验证了改进后的融合算法的有效性和合理性。但所有实验都是在已经获得障碍物运动信息的假定前提条件下,未来将研究移动障碍物的准确感知、目标跟踪和实时预测,探索在不同复杂场景下的移动机器人路径规划算法。