基于改进A∗算法与优化DWA 的差速移动机器人路径规划

黄勇强刘砚菊宋建辉刘晓阳

(沈阳理工大学 自动化与电气工程学院，沈阳 110159)

目前，移动机器人在航空、物流、农业以及医疗等诸多领域得到广泛的应用，其中差速移动机器人对野外、室内等不同环境的适应能力较强，是一种集环境感知、动态决策和执行等多功能于一体的智能化机器人[1]，具有机械结构简单、驱动和控制方便、机动灵活、工作效率高等优点[2]。

路径规划是移动机器人研究的核心内容之一。 为使机器人在不同环境中成功躲避障碍，需根据对环境的各种信息掌握程度，选择相应的算法规划出合适的路径[3－4]。 适用于差速移动机器人的路径规划方法可分为全局路径规划和局部路径规划两大类:全局路径规划属于静态规划(又称离线规划)算法，包括A∗算法[5]、Dijkstra 算法[6]、蚁群算法[7]、粒子群算法[8]、遗传算法[9]等；局部路径规划属于动态规划(又称在线规划)算法，包括动态窗口法(DWA)[10]、D∗算法[11]和人工势场法[12]等。

文献[13]在双轮差速移动机器人路径规划中先后利用Floyd 算法和圆弧平滑方法减少A∗算法拐点，并使拐点处路径更圆滑，减少了路径长度以及转折次数。 文献[14]针对双轮差速移动机器人模型，提出一种基于Hermite 插值法的轮式机器人路径规划方法，提高了轮式机器人路径规划的平稳性以及精确性。 文献[15]提出平滑A∗算法，并用于室内移动机器人的路径规划，提高了路径的平滑度，但仅适用于全局路径规划。 文献[16]改进了DWA 算法，使移动机器人能够动态地避开障碍物，但不能保证轨迹为全局最优。 虽然路径规划算法种类繁多，但每种算法都存在一定的局限性[17]，由此融合算法开始出现并得到发展。

文献[18]通过优化传统A∗算法的搜索点选取策略及评价函数，去除路径中的多余节点，且将改进后的A∗算法融合DWA 算法，在全局路径规划的基础上对局部路径规划进行修正。 文献[19]针对传统A∗算法拐点多、无法避开障碍物等缺陷，通过扩展领域和去除冗余节点优化全局路径，并利用改进DWA 算法的速度评价功能，在提高机器人规划路径平滑度的同时有效避开障碍物。文献[20]将A∗算法与跳点算法结合，拓展跳跃子节点，同时利用Floyd 算法平滑路径，再融合DWA 算法进行机器人全局动态路径规划，在多种栅格地图中验证了算法的路径规划能力。

根据差速移动机器人运动规律及运动原理，本文提出一种基于改进A∗算法和优化DWA 的融合算法。 将矢量角余弦值加入传统A∗算法的启发函数中，减少对无用拓展节点的搜寻，提高搜索效率；加入路径优化算法，去除全局路径的冗余节点，提高机器人规划路径平滑度，缩短规划时间；对DWA 算法的距离评价因子进行改进，区分已知的全局障碍物和新添未知障碍物；融合优化后的两种算法，将优化后全局路径的关键节点作为DWA 算法的局部规划目标点，引导局部规划路径更贴近全局最优路径，提高机器人动态避障能力，同时满足路径全局最优性。

1 改进A∗算法

1.1 改进启发函数

传统A∗算法是移动机器人常用的一种全局路径规划方法[21]，通过连续计算路径的评价函数值，启发式地搜索节点来构建最优路径。 评价函数为

式中:n表示路径中的当前节点；F(n)是从起点到目标节点的评价函数；G(n)是状态空间中从起点到当前节点n的实际代价成本；H(n)是当前节点n到目标节点的估计代价成本。 本文代价成本采用曼哈顿距离表示，其计算式为

式中d(p1，p2)表示点p1(x1，y1)到点p2(x2，y2)的曼哈顿距离。

在传统A∗算法中节点的选择仅取决于最小F(n)值，缺乏其他约束条件，故许多拓展节点并非最优路径节点，导致空间和时间成本增加。 为减少对无用节点的排查，引入矢量角余弦值改进启发函数，角度范围限制在0 ~90°。 余弦函数在0~90°之间单调递减，保证了启发函数的单调性约束，使拓展节点的选择更具方向性。 具体步骤如下。

1)构建一个从初始点到结束节点的向量a，和一个从当前节点的拓展节点到结束节点的向量b，如图1 所示。 计算两个向量之间角度θ的余弦，如式(3)所示。

图1 矢量图Fig.1 Vector diagram

2)过滤掉余弦值小于0.5 的拓展节点，使向量a和向量b之间的角度小于60°，确保搜索路径更接近目的地。

3)使用η作为权重，其值为地图单位长度乘以分辨率。 分辨率指1 m 单位长度包含的栅格数量，本文η＝1。 构造函数H1(n)作为启发函数，表达式为

由式(1)和式(4)可得到新的评价函数为

引入矢量角余弦值改进启发函数可有效将A∗算法的搜索节点约束在一定范围内，减少无用节点的数量，提高A∗算法的搜索效率。

1.2 路径节点优化

传统A∗算法规划的路径存在冗余节点多的问题，造成移动机器人转弯次数过多。 本文从起始点开始删除冗余节点，平滑处理路径。 具体步骤如下。

1)删除冗余节点。 针对全局路径，从第二个节点开始，判断当前节点和前一个节点的运动方向是否一致，若一致，则删掉前一个节点，否则保留前一节点。 遍历所有路径节点，将所有冗余共线节点删除，过程示意如图2 所示，删除后的路径为X1→X5→X6→X7→X8。

图2 删除冗余节点示意图Fig.2 Schematic diagram of removing redundant nodes

2)删除多余拐点。 在剩余的拐点中取新的节点Xi，连接X1和Xi。 设定安全距离D＝0.75 m，障碍物栅格与路径的距离为d1。 如果两点之间没有障碍物或d1>D，继续连接X1和Xi＋1，直到d1＝D，选择当前节点作为新拐点；若d1

3)提取剩余节点，输出平滑处理后的路径。

2 优化DWA 算法评价函数

2.1 差速移动机器人运动模型分析

DWA 算法在动态窗口范围内对机器人的线速度v以及角速度ω进行采样，首先需要建立移动机器人的运动学模型[22]。 本文以双轮差速移动机器人为例，该模型具有控制简单、位姿计算简单的特点，图3 为双轮差速移动机器人底盘运动学模型。 图中l为两轮间距，r为转弯半径，v1和v2分别为左右两轮的线速度，d为左右两轮在单位时间Δt内的位移差，旋转角度θ1＝θ2＝θ3。

图3 双轮差速移动机器人底盘运动学模型Fig.3 Kinematic model of the chassis of a two-wheeled differential mobile robot

机器人线速度v为两轮线速度的均值，即

在单位时间Δt内θ2变化较小，故

机器人角速度ω即为Δt内θ3的变化，结合式(7)可得

机器人在做弧线运动时，转弯半径r为线速度v和角速度ω的比值，结合式(6)和式(8)可得

双轮差速移动机器人可控制左右两轮的线速度完成转弯及非匀速运动，假设机器人在t－1 时刻的位姿坐标为(xt－1，yt－1，θt－1)，结合t时刻的v和ω计算式，可得到机器人在t时刻的位姿坐标为

由此得到机器人的位姿变化过程。

2.2 移动机器人速度采样

DWA 算法的原理是对机器人的速度矢量空间进行采样，并根据机器人运动模型进行前向模拟，以确定与采样对应的轨迹，然后依据评价函数为每一条轨迹打分，得分最高的轨迹就是最佳估计轨迹。 速度矢量作为控制信号发送给移动机器人，速度采样区间的确定是DWA 算法的核心，其受三个因素的限制，分别为本身最大(最小)速度Vm、电机加(减)速度Vd和安全制动速度Va。 约束表达式如下。

式中:νmax、νmin分别为机器人最大、最小线速度；ωmax、ωmin分别为机器人最大、最小角速度。

式中:νc、ωc分别为机器人当前时刻的线速度和角速度；νa1、ωa1分别为机器人最大线减速度、最大角减速度；νa2、ωa2分别为机器人最大线加速度、最大角加速度。

式中dist(ν，ω)表示最大安全制动距离。

机器人的最终速度V是以上三个约束速度的交集，即

2.3 优化评价函数

当采样样本中有若干组可行轨迹时，采用评价函数对每条可行轨迹打分，以确定最佳路径。评价函数G(v，ω)为

式中:heading(ν，ω)表示方位角评价函数，在当前速度下，机器人当前姿态方向与目标位置的方位角偏差为δ，则heading(ν，ω) ＝180°－ ｜δ｜，机器人的方向越接近终点，δ越小，方位偏差越小，认为其轨迹越好；dist(ν，ω)表示距离评价函数，用来评价模拟轨迹与障碍物的最近距离；vel(ν，ω)表示速度评价函数，用来评价机器人在模拟轨迹最后一段的速度，速度越大，得分越高；α、β、γ为三项加权系数；σ为归一化平滑系数。

传统DWA 算法采用前进模拟轨迹中障碍物到移动机器人之间的最短距离作为距离评价指标，如权重过大会使轨迹与规划路径的偏差增大，权重过小则会导致两者碰撞。 本文设定dist1(ν，ω)和dist2(ν，ω)作为新距离评价因子，以降低已知障碍物和未知障碍物之间的相互影响。dist1(ν，ω)用于评估前进模拟轨迹终端到已知障碍物的最小距离，作用是控制已知全局障碍物对局部路径规划的干扰；dist2(ν，ω)则用于评价前进模拟轨迹终端到未知障碍物的最小距离，作用是控制避障的灵敏度。 新的评价函数表达式为

式中λ为新增项的加权系数。

3 算法融合

改进的A∗算法在躲避全局未知障碍物方面仍显性能不足，改进后的DWA 算法在没有全局路径指引的情况下易陷入局部最优，甚至在障碍物情况复杂的环境中会规划失败，两者融合则可弥补各自的缺陷。 利用改进A∗算法的关键节点进行规划，将其应用为DWA 算法局部规划中的目标点，以引导局部路径规划，两者结合保证了动态规划的全局最优性。 本文融合算法流程如图4所示。

图4 本文融合算法流程图Fig.4 Flowchart of fusion algorithm in this paper

4 仿真实验和算法验证

采用Matlab 进行仿真实验，验证改进A∗算法以及本文融合算法的有效性。

4.1 改进A∗算法的仿真结果

本文分别在Matlab 中设置环境1 和环境2 两个不同的栅格地图，栅格地图的单位长度为1 m，黑色栅格为障碍物，不能通行，白色栅格为空旷区域，可以自由通行。S标记为起始点位置，T标记为目标点位置。 图5 为A∗算法改进前后在环境1中规划路径的仿真结果；图6 为A∗算法改进前后在环境2 中规划路径的仿真结果。

图6 环境2 中规划路径的仿真结果Fig.6 Simulation results of the planned path in environment 2

表1 分别从拐点个数、路径长度以及运行时间三个方面进行A∗算法改进前后对比，表中时间为机器人离线规划的时间。

表1 A∗算法改进前后路径规划对比Table 1 Comparisons of path planning before and after improvement of A∗algorithm

通过图5 和图6 中A∗算法改进前后的规划路径对比，并结合表1 数据可知，改进A∗算法可以适应简单环境和复杂环境，且改进A∗算法搜索更具方向性，转折减少，路径平滑度得到很大改善，减少了静态全局最优路径长度和离线规划时间。

4.2 本文融合算法的仿真结果

在融合算法的仿真实验中，全局已知地图添加了未知的静态障碍物和动态障碍物，DWA 算法评价函数中四个参数分别取值为α＝0.3、β＝0.5、λ＝0.5、γ＝0.2。 移动机器人的运动学参数设置如表2 所示。

表2 机器人运动学参数设置Table 2 Robot kinematic parameters settings

图7 为融合算法路径规划过程仿真图。图7(a)中移动机器人正在避开新增的静态障碍物；图7(b)中移动机器人正在避开未知动态障碍物；图7(c)中移动机器人即将到达第一个局部目标点；图7(d)中移动机器人到达目标点。

图7 融合算法路径规划过程仿真图Fig.7 Simulation of the path planning process of the fusion algorithm

仿真图中左上角“Δ”标记表示机器人的起始点位置，右下角“○”标记表示目标点位置，融合算法路径上不断移动的小圆圈表示双轮差速机器人模型，其前面的一簇线代表前进模拟轨迹，改进A∗算法路径上的“∗”代表改进A∗算法的关键点，也是局部规划目标点，新增的静态障碍物在图中用较大灰色方块表示，环境中随机出现的动态障碍物在图中用较小灰色方块表示。

由图7 可以看出，改进A∗算法先规划出一条静态全局最优路径，融合算法的路径尽可能地靠近静态全局最优路径，在增加全局未知障碍物的情况下，融合算法可以同时成功避开新增的未知动态和静态障碍物。

4.3 不同融合算法对比

在Matlab 中分别构建与文献[18]中地图1、文献[19]中地图2 和文献[20]中地图3 环境相同的地图，分别设置起点和终点一致、路径障碍物情况一致、线速度和角速度一致，对比不同环境下融合算法规划的路径长度、运行时间、路径平滑度以及路径全局一致性。

图8 ~10是本文融合算法分别在地图1 ~3中的规划路径。 机器人最大线速度和最大角速度在地图1 中分别为1 m/s 和0.35 rad/s，在地图2中分别为2 m/s 和0.7 rad/s，在地图3 中分别为2 m/s和0.35 rad/s。

图8 地图1 中规划路径Fig.8 A planning path in Map 1

表3 为本文融合算法与文献中融合算法仿真结果对比，表中时间为机器人抵达目标点时间。

表3 融合算法对比Table 3 Comparisons of fusion algorithms

由图8 和表3 可知，本文融合算法规划的路径始终贴近静态全局最优路径，路径全局最优性更好，且路径长度及运行时间更短。 由图9 和表3可知，本文融合算法路径全局最优性较好，在检测到没有新增障碍物时，保证平滑性和安全距离的条件下始终尽可能靠近静态全局最优路径，路径长度及运行时间更短。 由图10 和表3 可知，在障碍物密集时，本文融合算法转折更少，且保证路径具有全局最优性的前提下，与文献[20]选择从密集障碍物中间穿过的路径不同，在经过第一个和最后一个未知障碍物时，本文融合算法选择了安全性更高的路径。

图9 地图2 中规划路径Fig.9 A planning path in Map 2

图10 地图3 中规划路径Fig.10 A planning path in Map 3

5 结论

提出了一种改进A∗算法和优化DWA 的融合算法。 将矢量角余弦值引入A∗算法的启发函数中，使拓展节点的搜索更具选择性，并对路径进行节点优化处理，减少了多余的拐点和共线节点。仿真实验结果表明，改进A∗算法规划路径的平滑度得到很大提升，且路径长度平均减少了4.97%，计算时间平均减少了30.5%。 将改进A∗算法的路径关键节点提取出来，并作为局部路径规划的目标点，实现了基于全局最优的算法融合。 仿真实验结果显示，该融合算法的路径最大程度地接近于静态全局最优路径，且对新增的动态和静态障碍物实现较好的避障效果，验证了融合算法的有效性。 对比其他常用融合算法，本文融合算法在路径平滑度、安全性及路径全局最优性方面表现更出色，规划路径长度平均减少了3.7%，规划时间平均减少了5.1%。