融合改进A*算法与DWA算法的机器人实时路径规划

张 振，张华良，邓永胜，白士宇

((1.东北大学 机械工程与自动化学院，辽宁 沈阳 110819；2.中国科学院沈阳自动化研究所 工业控制网络与系统研究室，辽宁 沈阳 110015；3.中国科学院机器人与智能制造创新研究院，辽宁 沈阳 110000；4.沈阳理工大学 自动化与电气工程学院，辽宁 沈阳 110168)

0 引言

路径规划是移动机器人完成导航功能的基本环节之一[1-2]，根据对环境信息掌握程度的不同分为全局路径规划和局部路径规划[3-4]。应用于全局路径规划的算法有：Dijkstra算法[5]、A*算法[6-7]和快速扩展随机树(Rapidly-exploring Random Trees，RRT)算法[8]等；应用于局部路径规划的算法有：TEB(Time Elastic Band)算法[9]、人工势场法[10]和动态窗口(Dynamic Window Approach，DWA)算法[11-12]等。

A*算法是应用较广泛的一种全局路径规划方法，已经有许多学者对传统A*算法进行改进。为提高搜索效率，文献[13]将A*算法与跳点搜索算法结合，将筛选出的跳点添加至Open集与Close集中，减少寻路拓展节点。文献[14]以指数衰减的方式对估计函数加权，并对优化后的路径进行5次多项式平滑处理得到光滑曲线。为提高路径安全性，文献[15]通过引入邻域矩阵进行障碍搜索，有效提升了路径安全性；文献[16]通过在障碍物边缘设置“缓冲区”使机器人远离障碍物。

以上研究都是在对全局环境信息已知的情况下进行寻路，无法使机器人规避未知障碍物，而DWA算法具有良好的局部避障能力，文献[17]根据障碍物稠密程度自适应调整目标函数权值，兼顾路径的平滑性与安全性；文献[18]提出了一种较优方位角范围约束,通过轨迹评价函数确定最优轨迹对应的速度。然而，仅应用DWA算法进行路径规划往往会陷入局部最优的困境，将A*算法与DWA算法融合进行混合路径规划可以获得较好的避障效果[19-20]。

综上所述，本文提出了一种改进A*算法与DWA算法相融合的实时路径规划算法。该算法的主要创新点有：优化A*算法评价函数，加入周围环境及父节点的影响，以指数加权的方式提高寻路效率；增加节点安全扩展策略保证路径安全性；使用关键点提取策略剔除冗余节点；将提取的关键点信息加入DWA算法的评价函数中，实现算法融合。

1 A*算法描述与改进

1.1 传统A*算法描述

A*算法是一种常见的路径查找和图形遍历算法，是Dijkstra算法的扩展。相比于Dijkstra算法，A*算法增加了一种启发函数的引导，从而使机器人的寻路过程具有方向性，极大地提升了算法效率[21]。用传统A*算法评价函数来计算遍历节点的优先级，评价函数核心表达式为：

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

(1)

式中,F(n)表示由起始点S至目标点G的估计代价值；G(n)表示由起始点S至当前节点C的实际代价值；H(n)表示由当前节点C至目标点G的估代价值，也称作启发式搜索函数。常用的启发函数有曼哈顿距离、切比雪夫距离和欧式距离等，其中欧式距离应用较广泛，表达式为：

(2)

A*算法使用评价函数F(n)来计算上一节点扩展至当前节点C的代价值，并从中选取F(n)最小(优先级最高)的节点作为下一个待遍历的节点。

传统A*算法的不足主要表现为：

① 重复搜索节点，使寻路效率下降；

② 启发函数H(n)代价值小于实际值，导致搜索空间变大、路径冗余；

③ 将机器人简化为质点，导致机器人与障碍物距离过近，甚至斜穿障碍物；

④ 路径拐点较多，对机器人的电机不利。

1.2 A*算法改进

1.2.1 评价函数优化

(1)在评价函数F(n)的启发项中加入其父节点对当前节点扩展的影响，有效减少了节点的往返搜索现象，将优化后的启发函数记为H(n)′，具体形式为：

(3)

式中，H(p)为当前节点C的父节点P到目标点G的距离；Px，Py分别为父节点P的横纵坐标值。

(2)引入环境障碍物率K，量化环境中的障碍物稠密程度，表达式如式(4)所示，根据环境障碍物率K对优化后的启发函数H(n)′进行指数加权，实现启发项的快速自适应调整，提高机器人在栅格地图中不同位置的寻路效率和灵活度。

(4)

式中，N表示由当前节点C到目标点G为顶点所组成的矩形区域中障碍物的数量。评价函数F(n)优化为：

F(n)=G(n)+exp[H(n)/K]*(H(p)+H(n))。

(5)

由式(5)可知，当前节点离目标点较远或环境中障碍物较少时，评价函数中的启发项权重变大，机器人寻路效率提高，遍历节点减少；同理，当前节点离目标点较近或环境中障碍物较多时，机器人降低寻路速度，保证目标点可达。

1.2.2 安全扩展策略

采用八方向的搜索方式进行节点扩展，具体如图1(a)所示，在节点扩展时进行安全检测，根据父节点周围的环境信息自适应地调整扩展方向。安全扩展策略的具体步骤为：

步骤①：以父节点与待扩展的子节点为顶点构建矩形栅格区域，如图1(b)红色矩形框所示；

(a)节点八方向扩展

步骤②：检测矩形栅格区域内除父节点与子节点外的栅格是否是障碍物区域；

步骤③：若存在障碍物区域，则将此子节点的评价函数F(n)设为无穷大，A*算法就会转而扩展其他评价函数值较小的子节点，使机器人远离障碍物；

步骤④：若不存在障碍物区域，则对此子节点进行正常扩展。

1.2.3 关键点提取策略

提出关键点提取策略减少路径节点，重新分配拐点来减少路径长度及转折角度；引入安全阈值d来评价优化后路线的安全性，安全阈值d即该段路线与周边相邻障碍物顶点之间的距离。

关键点提取策略如图2所示，图2中由节点S，1～7，G所组成的路径为优化前的路径，可以看出这条路径均经过栅格中心点，拐点较多。下面对这条路线进行平滑化处理，并重新提取关键点：

步骤①：由起始点S遍历所有节点，直到遍历至终点G结束。检测由起始点S到节点1所组成的矩形区域内是否存在栅格障碍物，不存在障碍物则舍弃此节点。依次进行起始点S至节点2及以下节点矩形区域的障碍检测。当检测至节点5时，发现矩形区域内出现障碍物，则计算矩形区域内障碍物顶点相距路线S5的最小距离d5,若d5≥d则舍弃节点5，继续进行节点6及以下节点矩形区域的障碍检测；若d5

图2 关键点提取策略

步骤②：对初步优化后的关键点进行共线检测，若存在共线点，则保留首尾节点，剔除不必要的中间点，对关键点进行二次优化，进一步减少节点数。

步骤③：提取剩余的关键点，输出二次优化后的路径，算法结束。

2 DWA算法优化

2.1 机器人航迹推算

对本文所使用四轮全向机器人建立运动学模型，具体如图3所示。

图3 全向移动机器人运动学模型

图中，vx,vy分别为四轮全向机器人的水平方向及竖直方向的速度；ω为移动机器人绕其中心Or运动的角速度；θt为机器人的航向角。设移动机器人在相邻时刻之间匀速运动，而时间间隔Δt很短，所以将移动机器人从t时刻到t+Δt时刻的运动近似为一条直线，从而可以得出机器人的航迹推算公式：

(6)

2.2 速度矢量空间采样

由式(6)可知，对于当前时刻机器人的位姿，都可以通过每一个速度矢量(vx,vy,ω)推算出采样周期内的位移位姿及其运动轨迹。因此，DWA算法只需要对机器人的速度矢量空间进行采样，然后评价产生轨迹的好坏即可确定最佳轨迹。在实际环境中，速度矢量空间采样会受到机器人本身性能及环境的限制，具体约束表现为：

(1)由于机器人硬件性能对机器人最小和最大速度都有限制，所以移动机器人速度约束表示为：

Vm={vx∈[vmin,vmax],vy∈[vmin,vmax],

w∈[wmin,wmax]}，

(7)

式中，vmax,vmin分别为最大、最小线速度限制；ωmax,ωmin为分别为最大、最小角速度限制。

(2)由于机器人电机性能的约束，机器人应保证在电机力矩可承受范围内进行速度矢量空间采样，即机器人电机加速度约束：

(8)

式中，axmin,axmax分别为移动机器人x轴方向的最大减加速度和最大加速度；aymin,aymax分别为移动机器人y轴方向的最大减加速度和最大加速度；aωmin,aωmax分别为移动机器人角速度的最大减加速度和最大加速度。

(3)当机器人遇到动态障碍物时，应该保证机器人与障碍物之间保持安全距离，即移动机器人安全距离约束：

(9)

式中，dist(vx,vy,ω)为机器人轨迹距离障碍物的最小距离。

同时满足以上3个约束即可作为采样空间内的速度Vr，具体形式如下：

Vr∈Vm∩Vd∩Va。

(10)

2.3 评价函数优化

在进行速度采样后会得到多组可行的机器人轨迹，因此需要采用评价函数找出对于机器人最可行的轨迹，传统DWA算法的评价函数为：

G(vx,vy,ω)=σ[αhead(vx,vy,ω)+βdist(vx,vy,ω)+

γvel(vx,vy,ω)]，

(11)

式中，head(vx,vy,ω)为轨迹末端点与目标点的方位角偏差；vel(vx,vy,ω)为此轨迹对应的线速度值；α，β，γ为夹角、距离及速度各子函数的加权系数；σ为对此3项子函数的归一化参数。

由式(11)可知，传统DWA算法只单纯设置一个目标点对机器人进行引导，会导致机器人在障碍物较多或路线较长时陷入局部最优。因此将使用改进A*算法优化后的关键点对机器人进行局部引导，计算各个采样速度所对应的模拟轨迹末端点与引导点的距离，将其作为引导点评价子函数guid(vx,vy,ω)加入对模拟轨迹的评价函数中；此外，由于四轮全向机器人可以全向运动，所以不需要考虑轨迹末端点与目标点的方位角偏差head(vx,vy,ω)，经过优化后的评价函数为：

G′(vx,vy,ω)=σ[αguid(vx,vy,ω)+βdist(vx,vy,ω)+

γvel(vx,vy,ω)]，

(12)

式中，guid(vx,vy,ω)为模拟轨迹末端点到引导点的距离；α,β,γ分别为3项子函数的加权系数；σ为对此3项子函数的归一化参数。

3 融合算法路径规划

由于全局和局部路径规划都有各自的缺陷，A*算法能获取全局范围内的最佳路径却无法躲避动态障碍物，DWA算法能躲避动态障碍物却容易陷入局部最优。本文将上述2种方法融合，提取改进后A*算法的全局关键点作为改进DWA算法的引导点，通过融合算法产生的路径能在保证全局最优的前提下实时躲避动态障碍物，算法流程如图4所示。

图4 融合算法流程

4 仿真分析

为了验证融合算法的有效性，在Pycharm中进行仿真实验，所用电脑配置为AMD A6-6310 APU @1.8 GHz，搭建了20×20及40×40的环境栅格地图，黑色部分代表已知障碍物，灰色部分代表未知障碍物，白色部分代表可通行区域，绿色部分代表访问节点区域，●代表位于左上角的起始点S，▲代表位于右下角的目标点G。

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

改进A*算法对评价函数进行优化，设置安全阈值d=0.5，实验所用地图为20×20栅格地图，分别使用传统A*算法、文献[14]改进A*算法、文献[15]改进A*算法及本文改进A*算法进行对比实验，算法性能指标对比如表1所示,仿真结果如图5所示。

表1 简单情况下路径规划性能指标对比

(a)传统A*

为验证本文改进A*算法的可靠性，在40×40随机栅格地图对传统A*算法与本文改进的A*算法进行仿真实验，实验结果如图6所示，算法性能指标对比如表2所示。

(a)传统A*

表2 复杂情况下路径规划性能指标对比

由上述实验结果可知，在20×20的简单栅格环境地图中，本文改进A*算法的寻路时间相比于传统A*算法降低了81%，相比于文献[15]改进A*算法下降了84%；同时，本文改进A*算法的安全性相比于传统A*算法和文献[14]改进A*算法有大幅提高，使机器人在转弯时与障碍物保持安全距离；而且本文改进A*算法在保证路径较短的同时，最大程度减小了转折角度和次数，使规划路线更加平滑。在40×40的复杂随机栅格环境地图中，由于地图环境较复杂且同时要考虑路径的安全性，导致本文改进A*算法较传统A*算法的路径长度及转折角度略有上升，但本文改进A*算法规划的路径距离障碍物的平均距离更远，也不会出现斜穿障碍物的情况，而且相比于传统A*算法的寻路时间降低了89%，从而保证机器人可以安全、快速地避开环境内的已知障碍物。

4.2 融合导航算法仿真实验

实验所用全向移动机器人运动学参数如表3所示。

表3 全向移动机器人运动学参数

为验证机器人对于未知障碍物的动态避障能力，使用20×20栅格地图作为实验环境，在改进A*算法规划的路线上添加3个突然出现的障碍物，融合算法改进后的评价函数各项系数分别为：α=0.15,β=0.4,γ=0.4，融合算法的路径仿真结果如图7所示，在图中以灰色栅格表示突然出现的障碍物，*代表由改进A*算法所得到全局关键点，蓝色虚线表示由改进A*算法得到的路径，红色实线表示由融合算法得到的路径。

由图7可知，当环境中不存在未知障碍物时，改进A*算法和融合算法均能顺着关键点的指引到达目标点，具体如图7(a)所示;此时在机器人的行进路线上分别添加1～3个未知障碍物，分别如图7(b)～(d)所示，可以看出改进A*算法所规划的路径并没有规避未知障碍物，而融合导航算法在全局路径的引导下对未知障碍物进行合理规避，并与障碍物之间始终保持安全距离。由上述仿真结果可知，融合导航算法能在保证全局最优的基础上，安全、快速地实现动态避障功能。

(a)无未知障碍物

5 实验验证

为验证融合导航算法在真实环境中的动态避障效果，使用四轮全向移动机器人进行实验，具体结构如图8所示，在机器人上搭载的主要器件有：1个Jetson Nano B01控制器、1个STM32F103运动控制器、4个驱动电机、1个激光雷达和1个深度相机。在机器人控制器和个人PC中预装基于Ubuntu18.04的机器人操作系统——Melodic。

实验条件如图8所示。以个人PC为主机、机器人控制器为从机进行局域网内主从机网络配置，在主机端开启键盘控制节点、从机端开启Cartographer算法建图节点构建如图9所示的环境地图。

(a)机器人实物

图9 实验场地栅格地图

控制机器人完成在真实环境下的导航功能。在如图8(b)所示的真实环境中分别对原始A*算法、改进A*算法以及融合算法进行导航实验。

打开Rviz可视化界面，完成相应显示配置，进行初始位姿估计并指定目标点，基于建好的环境地图对原始A*算法、改进A*算法进行导航实验，运行结果如图10和图11所示。

(a)时刻“1”

(a)时刻“1”

从实验结果可以看出，改进A*算法规划的路线较原始A*算法规划的路线转折更小、平滑性更好。

保持以上起始点和目标点不变，在改进A*算法规划的全局路径中加入2个计算机主机箱作为未知障碍物基于融合算法进行导航实验，加入未知障碍物的实验场地如图12所示，实验结果如图13所示。

图12 加入未知障碍物后的实验场地

(a)时刻“1”

从实验结果可以看出，虽然融合算法在遇到未知障碍物时转折角度会变大，但成功地避开了环境中的2个未知障碍物。

6 结束语

传统A*算法规划的路径存在效率低、不平滑、靠近环境中已知障碍物及不能躲避环境中未知障碍物等缺点。在传统A*算法的评价函数中加入环境信息及父节点信息加快寻路效率；提出安全扩展策略自适应改变扩展方向避免机器人距离障碍物过近；提出关键点提取策略剔除冗余节点使路径平滑；将改进A*算法输出的关键点信息加入到改进DWA算法的轨迹评价函数中完成算法融合；设置仿真实验与实物实验进行理论验证。最终实验结果证明，改进A*算法的多项性能指标相比于传统A*算法都有了大幅提升，且融合改进A*算法与DWA算法规划的路径能成功避开环境中的未知障碍物，使机器人安全达到目标点。