改进A*和DWA融合的移动机器人导航算法

袁千贺,魏国亮,田 昕,沈斯杰

1(上海理工大学 光电信息与计算机工程学院,上海 200093)2(上海理工大学 理学院,上海 200093)

1 引 言

对于移动机器人来说,导航能力是衡量机器人性能的重要指标.机器人从当前位置行驶到目标位置的导航过程可以分为两个部分:1)在全局环境中规划出最优或者接近最优的行驶路径;2)在遵循全局路径轮廓的基础上,规划局部环境路径,避开障碍物[1].可以说,全局路径最优与实时避障是移动机器人完成导航工作的重中之重.因此,路径规划技术是机器人实现自主导航功能的核心技术[2],可以在存在障碍物的环境中,找到一条从起始点到目标点的最优路径,并能够避开所有障碍物[3,4].此外,路径规划可以分为全局规划和局部规划两种形式[5].广泛使用的全局路径规划算法有Dijkstra算法[6]、A*算法[7]和RRT*算法[8]等;广泛使用的局部路径规划算法有人工势场法[9]、DWA算法[10]和蚁群算法[11]等.

对于全局路径规划算法,A*算法的效果较好.但A*算法规划的路径通过节点连接,导致曲率不连续,存在路径冗长的缺点.赵晓等[12]提出了一种基于跳点搜索算法的改进A*算法,提高了路径的搜索速度,减少了计算量.Zhang等[13]提出了一种改进A*算法,遍历路径的所有节点,然后删除不必要的节点,减短了机器人的行驶路径和转弯时间.然而,这些改进A*算法仅考虑了全局路径的优化,机器人却不能避开未知障碍物.对于局部路径规划算法,DWA算法具有很好的局部避障能力.王永雄等[14]提出了参数自适应的DWA算法,获得了机器人运动的最佳速度,并提高了安全性.Mai等[15]提出了一种能够提前感知密集物体分布情况的改进DWA算法,可以使机器人稳定地避开密集区域.然而,改进DWA算法在实现机器人避障的同时,却无法做到全局路径最优.以上改进算法均只考虑了移动机器人导航过程中的全局路径最优或者局部避障,没有同时满足两者.因此,可采用全局路径规划与局部路径规划相结合的方法[16-18],进一步满足移动机器人完成导航工作的所需条件.

综上所述,为实现移动机器人既能以全局最优路径行驶,又能够实时避开障碍物,提出了一种基于改进A*算法与DWA算法相融合的导航算法.该算法具有以下创新点:1)引入环境信息优化传统A*算法的代价函数,实现其自适应调整,提高算法效率;2)利用一种关键点选取策略,解决存在冗余的共线节点和转折点的问题,保留必要的路径节点,得到只具有关键点的全局路径;3)构造结合关键点信息的评价函数,然后应用DWA算法,使局部路径规划遵循全局路径轮廓,从而使路径更加平滑,并实现实时避障.

2 改进A*算法

2.1 传统A*算法

A*算法结合Dijkstra算法和最佳优先搜索算法,是一种通过与节点信息相关的代价函数,可以有效求解全局路径的搜索算法[19].A*算法从起始点开始向周围搜索,计算各个节点的代价函数值,得到代价值最小的节点,并作为新的起始搜索点继续搜索下一个节点,直至到达目标点为止,从而生成全局路径.代价函数F(n)为:

F(n)=G(n)+H(n)

(1)

式中:n为当前节点;F(n)为综合代价值;G(n)为当前节点到起始点的实际代价值;H(n)是启发函数,为当前节点到目标点的估计代价值.

2.2 代价函数优化

由上述可得,在已知全局环境的地图信息下,传统A*算法需搜索计算一系列节点的代价函数值,遍历节点过多,导致运算效率低.其中,启发函数H(n)主导算法的搜索性能,影响着算法的运行速率.因此,可以优化H(n)在不同情况下的权重大小,从而提高算法的搜索效率和灵活度.

当前节点离目标点较远时,估计代价值远小于实际值,此时搜索空间大,搜索节点较多,应适当增大H(n)的权重,提高搜索效率;当前节点逐渐靠近目标点时,估计代价值也随之增大,接近实际值,为防止估计值较大而陷入局部最优,应适当减小H(n)的权重.因此,为了自适应调整启发函数的权重,引入随着移动机器人的当前位置变化而变化的环境障碍率K.假设机器人的起始点与目标点组成的矩形栅格数为M;当前节点到目标点搜索范围内的栅格障碍数为N.则环境障碍率表达式为:

(2)

将环境障碍率K引入代价函数F(n),可自适应改变启发函数H(n)的权重.此时,代价函数F(n)优化为:

F(n)=G(n)+eKH(n)

(3)

随着机器人从起始点移动至目标点的过程,环境障碍率K逐渐减小,从而使H(n)的权重逐渐减小.满足了当前节点远离目标点时,权重较大;当前节点靠近目标点时,权重较小.通过实现H(n)的自适应调整,提高机器人在不同位置下的搜索效率.

2.3 关键点选取策略

传统A*算法搜索的路径节点位于栅格地图的每个栅格中心位置,然后依次连接构成路径,存在一些冗余的共线节点和转折点.若以此路径导航行驶,机器人在每个不必要的转折点处都需要改变航向角,这些多余动作会直接影响到机器人的正常运行.在实际的导航过程中,机器人较流畅的移动可以减少误差的产生,更利于精准地完成工作.因此,针对这一问题,利用一种关键点选取策略优化传统A*算法规划的全局路径,只保留必要的路径节点,提高路径的高效性.关键点选取策略的具体实施步骤如下:

1)若夹角大小为零,说明P1、P2、P3这3个路径节点在同一条直线上,则P2为冗余的共线节点,需剔除.

2)若夹角大小不为零,说明P1、P2、P3这3个路径节点不在同一条直线上,则求解线段方程L(P1P3),并判断该线段的预设安全距离范围内是否存在障碍物.若不存在障碍物,则P2为冗余转折点,需剔除;若存在障碍物,则P2为必要转折点,需保留.

由于冗余点的剔除,会造成原有的路径节点集发生变化.因此,在重复上述步骤的同时,需不断更新剔除冗余点之后的路径节点集,并依次计算检验所有的路径节点.最终,通过关键点选取策略剔除冗余点,保留必要的路径节点,优化得到一条只包含起始点,关键点和目标点的全局导航路径.

3 DWA算法

在载入完整环境信息的全局地图下,移动机器人可以很好地进行导航工作.然而,在实际情况下,存在一些异变因素,可能会有未知障碍物出现在机器人原有的导航路径上,若不采取相应的措施,机器人很容易碰撞到障碍物.因此,为实现机器人的实时避障,采用具有局部避障能力的DWA算法规划局部路径,避开障碍物,保证自身安全性.

DWA算法在速度可行范围内采样多组移动机器人的速度对(v,ω),根据运动模型计算模拟机器人在一定时间间隔内的移动轨迹[20].然后,通过评价函数选取出最优模拟轨迹,并以该轨迹对应的线速度和角速度作为机器人当前的最佳移动速度.下面对机器人运动模型,速度采样和评价函数进行描述.

3.1 机器人运动模型

在对机器人移动轨迹进行模拟时,首先需要分析机器人的运动模型.对于常见的两轮差速模型,设机器人t时刻的线速度和角速度分别为vt,ωt;t时刻的位姿为(xt,yt,θt)T.则与之时长相差Δt的t+1时刻的位姿(xt+1,yt+1,θt+1)T可表示为:

(4)

3.2 速度采样

速度采样可以采样到无穷多组速度对(v,ω),但由于机器人本身,电机性能以及制动距离的影响,速度采样会被约束在一定的范围内.

1)机器人本身约束的最小、最大速度集合为:

Vm={(v,ω)|v∈[vmin,vmax],ω∈[ωmin,ωmax]}

(5)

式中:vmin,vmax为线速度的最小值和最大值;ωmin,ωmax为角速度的最小值和最大值.

2)机器人在模拟移动的一定时间间隔dt内,由电机加减速约束的最小、最大速度集合为:

(6)

3)机器人即将撞到障碍物时应立即停止移动,由最大减速度约束的速度集合为:

(7)

式中:Dist(v,ω)为轨迹与障碍物的最近距离.

由上述3种约束限制了机器人的移动速度范围.设机器人的速度集合为Vr,则:

Vr=Vm∩Vd∩Vs

(8)

在速度集合Vr中,对于每一组不同的速度对(v,ω),根据式(4)的机器人运动模型可得出移动一定时间后的位姿,从而模拟出一定时间间隔内不同方向的移动轨迹.

3.3 评价函数优化

根据机器人的移动速度范围和运动模型,模拟出多条移动轨迹后,通过评价函数综合选取出最优模拟轨迹.评价函数为:

E(v,ω)=αHead(v,ω)+βDist(v,ω)+γVel(v,ω)

(9)

式中:Head(v,ω)为轨迹终点与最终目标点的方位角偏差;Dist(v,ω)为轨迹与障碍物的最近距离;Vel(v,ω)评价当前速度;α、β、γ为加权系数.

对上述评价函数分析可知,只有一个最终目标点指引DWA算法,若环境中障碍物较多,便难以寻找到达目标点的路径.考虑到可设置一定的中间目标点作为DWA算法的指引,实现分段进行局部路径规划,从而避免到达不了目标点.因此,构造一种结合关键点信息的评价函数,以此应用DWA算法,将关键点作为中间目标点,确保局部路径规划按照全局路径轮廓的指引进行.评价函数优化为:

E(v,ω)=αPHead(v,ω)+βDist(v,ω+γVel(v,ω)

(10)

式中:PHead(v,ω)为轨迹终点与当前中间目标点的方位角偏差.中间目标点是选取的各个全局路径关键点.随着机器人向最终目标点的移动,关键点按照选取的顺序依次作为DWA算法的中间目标点,机器人会根据实际情况接近或者到达.

4 融合导航算法

改进A*算法得到了只包含起始点,关键点和目标点的导航路径,但无法避开环境中出现的未知障碍物.DWA算法具有良好的局部避障能力,但只有一个最终目标点作为指引,容易陷入局部最优.因此,本文将两种算法相融合,以提取的改进A*算法规划的全局路径关键点作为DWA算法的中间目标点,优化后的评价函数使得局部路径规划遵循已规划的全局路径轮廓.通过融合导航算法,以实现移动机器人导航过程中的全局路径最优,同时兼具实时避障功能.算法的具体流程如图1所示.

图1 融合导航算法流程图Fig.1 Flow chart of fusion navigation algorithm

首先,采用改进A*算法规划出一条全局路径,利用其优化的代价函数以及关键点选取策略对路径进行优化,并得到路径必要的关键节点.然后,以提取的路径关键点作为DWA算法的中间目标点,指引局部路径的规划.DWA算法对移动机器人的速度进行采样,并模拟出各速度对的移动轨迹,利用结合关键点信息的评价函数选取出最优的模拟移动轨迹,并以最优轨迹对应的速度控制机器人向目标点的移动,从而实现改进A*算法和DWA算法的融合.最后,在融合导航算法下,移动机器人遵循全局路径轮廓依次接近或到达中间目标点,直至到达最终目标点.

5 仿真实验

为验证所提出算法的有效性,搭建栅格地图进行仿真实验.实验环境为运行内存16GB的64位WIN10操作系统,平台为MATLAB 2017a.仿真实验地图设计为3种15m×15m,20m×20m和25m×25m的栅格地图.其中,黑色部分表示障碍物区域;白色部分表示可移动区域;S表示起始点,位于左上角;T表示目标点,位于右下角.DWA算法评价函数各加权系数α=0.05、β=0.2、γ=0.2.机器人运动参数如表1所示.

表1 机器人运动参数Table 1 Robot motion parameters

5.1 改进A*算法仿真对比实验

相对于传统A*算法,改进A*算法优化了代价函数,并利用一种关键点选取策略.两种算法规划路径的仿真实验结果对比如图2所示,性能指标对比如表2所示.

图2和表2的结果表明,相比于传统A*算法,改进A*算法通过代价函数的优化和关键点选取策略的使用,能明显改善全局路径.改进后的算法不仅剔除了路径上冗余的共线节点和转折点,而且缩短了路径长度和规划时间,提高了路径的高效性和搜索效率.改进A*算法有效地得到了只包含起始点,关键点和目标点的全局导航路径.

图2 传统A*算法和改进A*算法路径对比图Fig.2 Comparison of traditional A* algorithm and improved A* algorithm path

表2 改进前后算法性能指标对比Table 2 Comparison of algorithm performance indicators before and after improvement

5.2 融合导航算法仿真实验

为了验证融合导航算法的路径规划与实时避障能力,选用20m×20m的栅格地图进行实验.在机器人必经的改进A*算法规划的全局路径上,设置3个未知障碍物,用褐色栅格表示.仿真实验结果如图3所示.

图3(a)中虚线为改进A*算法规划的全局路径,图中*号为提取的其中一个关键点,作为移动机器人此时刻的中间目标点.随着机器人向最终目标点的前进,下一个关键点会成为新的中间目标点.图3(b)为在融合导航算法下,机器人从起始点到达目标点的路径,可以看出有效地避开了设置的未知障碍物,且路径与图2中改进A*算法规划的路径相比,明显更加平滑.实验结果表明,通过构造结合关键点信息的评价函数,各个关键点依次作为DWA算法的中间目标点,从而在全局路径的基础上规划局部路径.在仿真环境下,融合导航算法实现了移动机器人导航的全局路径最优与实时避障功能.

图3 融合导航算法路径Fig.3 Fusion navigation algorithm path

6 真实环境实验

为进一步验证所提出算法在实际应用中的可行性,选用Turtlebot3移动机器人进行真实环境实验.实验环境为运行内存4GB的64位Ununtu16.04操作系统,平台为ROS Kinetic.通过局域网完成网络连接配置,实现对Turtlebot3移动机器人的远程操作,控制调试机器人的导航.真实实验环境位于实验室楼层的走廊过道,如图4所示.首先,在实验环境中不存在未知障碍物,如图4(a)所示.然后,通过Gmapping算法构建地图,打开Rviz可视化界面,并启动键盘节点控制机器人移动,构建环境地图,待进行导航时载入到移动机器人中.

图4 真实实验环境Fig.4 Real experimental environment

此时,在真实环境中不放置未知障碍物.启动导航算法并选定已构建好的地图,打开Rviz可视化界面,选定导航的起始点和目标点.融合导航算法规划的导航路径结果如图5所示.

图5中,散点为移动机器人的激光雷达采集的实时观测数据;黑色实线为机器人从起始点到目标点的路径;箭头表示设定的目标点.实验结果表明,在无未知障碍物的情况下,所提出的融合导航算法可以有效的为移动机器人规划出较为平滑的全局导航路径.

图5 移动机器人导航路径Fig.5 Mobile robot navigation path

为验证融合导航算法的实时避障能力,在移动机器人已知的实验环境中添加两个未知障碍物,并将障碍物放置在图5所示的导航路径上.此时,真实实验环境如图4(b)所示.启动融合导航算法,所得导航路径结果如图6所示.

图6 放置未知障碍物的移动机器人导航路径Fig.6 Mobile robot navigation path with unknown obstacles placed

在放置两个未知障碍物的情况下,当移动机器人未观测到未知障碍物时,融合导航算法规划的路径与图5一致.图6的实验结果表明,随着机器人向目标点的移动,当机器人观测到未知障碍物1时,通过DWA算法对局部路径进行调整,规避障碍物1后回到全局路径上,继续移动;当机器人再次观测到未知障碍物2时,通过同样的方式进行规避,最终到达目标点.

由上述实验结果表明,在真实环境下,融合导航算法可以有效地规划出全局路径.此外,当机器人在路径上观测到未知障碍物时,能够在全局路径的基础上规划局部路径绕开障碍物,保证自身的安全性.在真实环境下,融合导航算法同样实现了移动机器人导航的全局路径最优与实时避障功能.

7 结束语

本文提出一种基于改进A*算法与DWA算法相融合的移动机器人导航算法,以满足机器人在导航过程中的全局路径最优与实时避障的需求.引入环境信息优化传统A*算法的代价函数,使得代价函数随着机器人的位置自适应调整,并利用关键点选取策略剔除冗余点,保留必要的路径节点.实验结果表明,改进A*算法有效地提高了算法的搜索效率和路径的高效性,得到了只具有关键点的全局路径.进而构造结合关键点信息的评价函数,以关键点作为DWA算法的中间目标点,在全局路径的指引下完成局部路径规划,提高路径平滑性,且能够避开环境中出现的未知障碍物.通过仿真实验和真实环境实验的联合论证,所提出的融合导航算法实现了移动机器人导航的全局路径最优以及实时避障功能,具有一定的有效性和可行性.

由于移动机器人的导航随着环境的愈加复杂,会有更多的要求有待完善.因此,对于大规模的复杂环境,未来还需做出进一步的研究.