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

刘建娟，薛礼啟，张会娟，刘忠璞

1.河南工业大学 电气工程学院，郑州 450000

2.河南工业大学 机电设备及测控技术研究所，郑州 450000

目前，路径规划可分为局部路径规划和全局路径规划两大类，用于局部路径规划的算法包括DWA算法[1-3]、粒子群算法[4-6]、蚁群算法[7-9]、遗传算法[10-11]等；用于全局路径规划的算法包括D*算法[12-13]、A*算法[14-15]、快速扩展随机树[16-17]等。

A*算法广泛应用于移动机器人路径规划，但是传统A*算法在移动机器人的应用中存在拐点多、路径不平滑等不足，不利于机器人的运行。针对这些不足，许多学者对传统A*算法进行不同层面的改进，也取得了不错的效果。文献[18]利用JPS 算法对A*算法进行改进，优化搜索策略，同时利用贝塞尔曲线对路径进行平滑优化，由实验结果可知此算法明显提升了路径规划效率，在大尺度环境中有较好的适应性，但是A*算法是在全局已知的前提下进行搜索和扩展的，因此无法实现随机障碍物的动态避障。文献[19]提出一种单边矩形扩展A*算法，利用受迫扩展规则，简化终止条件，提高搜索效率和路径规划质量，但是算法搜索地图的不规则性和破碎化程度不断加重，算法更适合在大尺度环境中的路径规划。DWA算法常用于移动机器人的局部路径规划，但是在大尺度复杂环境中由于缺少中间引导，无法得到最优路径，甚至无法达到目标点。文献[20]通过移动机器人的尺寸与障碍物之间的自由空间关系对DWA算法进行改进，由于对障碍物与机器人之间的距离进行限制，在复杂环境中出现的狭窄区域会造成机器人反复启停，甚至停止运行。文献[21]通过立体视觉进行避障，严格定义机器人的搜索空间得到机器人最优速度，虽然可以实现动态路径规划，但是由于缺乏中间指引，规划路径很难达到最优，而立体视觉又增加了时间和空间复杂性，对硬件设备性能提出更高的要求，经济性会有所降低。目前来说，单一改进A*算法路径规划能得到从起点到终点的最优路径，但是无法及时躲避局部障碍物；单一改进DWA算法虽然能有效躲避局部障碍物，但是不满足全局最优路径。因此，结合两者的优势，对两种算法进行改进并实现两种算法融合是当前研究的热点。文献[22]将搜索邻域由3×3 扩展为7×7，优化了搜索角度，减少了路径的转折点，使得路径更加平滑，然后将改进后的A*算法与DWA算法融合，虽然保证了全局最优，但是由于没有对已知障碍物和未知动态障碍物进行区分，导致在动态路径规划时两者相互影响，造成动态避障灵敏度低。文献[23]通过判断起点和终点的位置关系，将传统的8搜索方向改为5搜索方向，设计新的启发函数，实现无斜穿路径，增加了路径平滑度，实现算法融合。但是融合算法存在一定的不足，首先，由于没有改进DWA 算法动态窗口评价函数，使得动态避障灵敏度不高，其次，将传统8 搜索方向改为5 搜索方向时，没有考虑到剩余5搜索方向都存在障碍物的情况并加以判断，搜索可能会陷入死区，无法实现路径规划。文献[24]通过关键点提取策略对传统A*算法进行改进，并构建全局最优的动态窗口评价函数，将改进的算法进行融合，实现实时最优路径规划。算法虽然对动态窗口评价函数进行改进，增加机器人末端到终点的距离函数，其目的是不断缩短与终点的距离，但是仍然没有区分障碍物类型，既难以获得全局最优路径，又无法保证动态避障的灵敏度。

针对这些问题，将传统A*算法的启发函数中引入环境信息，实现对A*算法启发函数的自适应调整；设计路径平滑优化算法，删除冗余节点、减少转折、提高路径平滑度；提取改进A*算法规划路径的关键点作为DWA算法的中间目标点做全局引导，在全局最优的基础上实现改进A*与DWA算法融合。

1 传统A*与DWA算法

首先对现有文献中传统A*算法及DWA 算法的基本原理进行介绍。分析算法存在的不足及形成原因，为第2章中提出算法改进和算法融合提供理论依据。

1.1 传统A*算法基本原理

A*算法广泛应用于二维地图的路径规划，是一种典型的启发式搜索型路径规划算法，利用评价函数来指导节点的搜索与扩展，因此评价函数影响搜索空间的大小和算法的速度。传统A*算法的评价函数如公式（1）所示：

在评价函数公式（1）中，n表示当前节点，F(n)表示从起点经由节点n到达目标点的评价函数；G(n)表示从起点到达节点n的实际代价；H(n)为估计值，表示从节点n到目标点估计代价，又叫启发函数。由启发函数H(n)的选取，可以控制节点的搜索与扩展，进而可以控制算法的速度和精度。传统A*算法常用的启发函数有曼哈顿距离、切比雪夫距离、欧几里德距离三种，三种距离的示意图如图1所示。因为启发函数H(n)与实际距离的关系会直接影响算法的速度和精度，为了提高算法的精度和灵活性，需要对启发函数进行改进。

图1 三种启发函数距离示意图Fig.1 Distance diagram of three heuristic functions

1.2 DWA算法基本原理

1.2.1 运动学模型

本文选用Turtlebot2移动机器人平台作为实验平台进行实验验证。由于DWA算法要将位置控制转化为速度控制，因此需要对Turtlebot2 的运动模型进行分析，Turtlebot2的两轮差速运动的运动学模型如图2所示。

图2 Turtlebot2差速运动学模型Fig.2 Differential kinematics model of Turnlebot2

假设v(t)和ω(t)分别表示Turtlebot2 在世界坐标系下t时刻的线速度和角速度，在采样周期Δt内，位移较小，近似做匀速直线运动，则此运动学模型的数学表达式可表示为公式（2）：

1.2.2 速度采样

DWA算法将动态避障问题描述为速度空间中三个带约束的优化问题，要根据实际情况对速度采样范围进行约束。

（1）移动机器人速度约束

机器人的采样速度应控制在移动机器人最佳角速度和线速度区间内，则约束为公式（3）：

式中，vmax、vmin指机器人的最大、最小线速度，ωmax、ωmin指机器人最大、最小角速度。

（2）移动机器人电机加减速约束

机器人的采样速度应考虑电机性能，速度应保持在当前速度在最大减速度和最大加速度条件下，速度可变范围内，则约束为公式（4）：

式中，vc、ωc指当前时刻线速度和角速度，admax、aimax指机器人最大减速度和最大加速度，αdmax、αimax指机器人的最大角减速度和最大角加速度。

（3）移动机器人安全距离约束

机器人在进行局部路径规划时，为了保证其安全，在约束条件（2）最大减速度条件下，使得移动机器人到达障碍物之前线速度和角速度都降为零，则约束为公式（5）：

式中，dist(v,ω) 指模拟轨迹末端距离障碍物的最近距离，admax、αdmax指机器人最大减速度和最大加速度。

最终的动态窗口的速度为以上三个约束条件的交集，动态窗口速度Vw满足公式（6）：

1.2.3 评价函数

DWA 算法的评价函数指标有偏向角、线速度及模拟轨迹末端与距障碍物的最近距离。由于这三个指标的度量单位不同，为避免某一个量的数值过大而占比过大，需要对每个指标进行归一化处理，归一化处理后得到的轨迹评价函数如公式（7）：

式中，α、β、γ是每一项的加权系数。Head(v,ω)为模拟轨迹终点方向和目标点之间方向角偏差；Vel(v,ω)用来评价移动机器人在当前估计中运行的速度大小；Dist(v,ω) 是指模拟轨迹终点与障碍物的最近距离。

对传统DWA算法的原理介绍可知，传统DWA算法存在两点不足：第一，只有一个目标点，缺少中间指引，在大尺度环境中无法得到最优路径；第二，未将已知障碍物和未知障碍物进行区分，导致动态避障灵敏度低。

2 算法改进与算法融合

针对上文提到的传统算法存在的不足，提出自己算法的改进和融合思路。首先，量化环境信息，将环境信息引入传统A*算法的评价函数，实现了评价函数中启发函数的自适应改变，提高了算法的灵活性，其次设计路径平滑算法对路径进行平滑优化，使得路径更加适合机器人的运动。最后改进DWA 算法的评价函数，减少已知障碍物和未知障碍物的影响，并将改进后的两种算法进行融合。

2.1 改进A*算法

2.1.1 量化环境信息

传统A*算法的评价函数由实际代价函数G(n)和启发函数H(n)组成。其中，启发函数H(n)主导传统A*算法的搜索性能。当起点与终点没有障碍物时，选用欧几里德距离公式估计的路径与实际路径相同，此时算法速度快、准确率高，但是实际应用中往往存在障碍物，导致搜索空间变大、效率降低、产生较多冗余节点。因此，当障碍物较少时，可以适当增大启发函数H(n)权重，减小搜索范围、提高搜索效率。当障碍物较多时，可以适当减小启发函数H(n)权重，提高搜索范围避免陷入局部最优。障碍物的存在使得产生多条可选路径，最短路径也会大于起点和终点之间的欧几里德距离。为了估计最短路径，引入环境障碍物比率K抽象反应环境复杂度。假设移动机器人起点为(xs,ys)终点为(xt,yt)，起点和终点组成的矩形栅格障碍物栅格数为N，那么障碍物的比率K可表示为公式（8）：

2.1.2 环境信息引入评价函数

环境障碍物比率K由障碍物栅格数与起点终点共同决定，K的大小随起点终点的改变而改变。将环境障碍物比率K引入评价函数F(n)，改变启发函数H(n)的权重，实现评价函数F(n)的自适应调整。启发函数H(n)选取欧几里德距离如公式（9）所示，改进后的评价函数F(n)如公式（10）所示。

由公式（10）可知，当环境中障碍物较少时，启发函数H(n)的系数(1-lnK)大，算法缩小了搜索空间，提高了搜索效率。当环境中障碍物多时，启发函数H(n)的系数(1-lnK)小，算法扩大了搜索空间，降低搜索速度，避免陷入局部最优。

2.1.3 路径平滑优化

传统A*算法路径规划是由连续栅格中心点连接组成，存在许多冗余节点，路径转折次数多，路径不平滑。针对这些问题，基于Floyd 算法思想设计路径平滑优化算法。

路径平滑优化原理如图3 所示，以此为例，传统A*算法规划的路径规划由栅格中心点的连线组成，优化前的路径为（S，1，2，…，13，T），存在许多冗余节点。在考虑安全距离D的基础上，对路径进行平滑优化，使得路径的选择不再局限于过栅格的中心位置。优化后的路径平滑度增加，路径长度和转折点减少，路径平滑优化步骤如下：

图3 路径平滑优化原理图Fig.3 Optimization of path smoothing

步骤1遍历所有节点，删除每一段路径中间的冗余节点，保留起始点和拐点。删除中间节点后剩下S，2，6，8，9，10，11，13，T五个节点。

步骤2遍历起始点和拐点，从起点开始将每一个节点都与后面的节点相连作为备选路径，判断每条路径与障碍物栅格的距离di与安全距离D的关系。若di≤D，则删除路径，若di>D，保留路径，删除路径之间的拐点。删除不必要拐点后剩余S，6，8，T三个节点。

步骤3提取剩余节点，输出优化路径，算法结束。

2.2 DWA算法评价函数优化

为了提高融合算法在复杂环境中的避障性能，需要对传统DWA 算法的评价函数进行优化。在传统DWA算法路径规划时，评价函数中Dist(v,ω)的权重，决定轨迹与障碍物的距离。若增大Dist(v,ω)系数，则生成路径长度增大，达不到全局最优；若减小Dist(v,ω)系数，则遇到未知障碍物时，不能及时避障。为了减少全局已知障碍物与全局未知障碍物之间的相互干扰，将评价函数中Dist(v,ω)区分为模拟轨迹终点与已知障碍物的最近距离Dist_s(v,w)和模拟轨迹终点与未知障碍物的最近距离Dist_d(v,w)。则优化后的DWA 算法评价函数如公式（11）：公式（11）中，α、β、γ、φ是每一项的加权系数。Heading(v,w)为模拟轨迹终点和子目标点之间方向角偏差，随着路径的前进，按照提取的关键点顺序不断更新子目标点；Vel(v,ω) 用来评价移动机器人在当前估计中运行的速度大小；Dist_s(v,w)表示模拟轨迹终点与已知障碍物的最近距离，控制已知障碍物对局部路径规划的干扰；Dist_d(v,w)表示模拟轨迹终点与未知障碍物的最近距离，用来控制避障的灵敏度。

2.3 融合算法

改进A*算法能够获取全局路径规划中的最优解，在静态工作环境中能很好地完成全局路径规划能力，但是对环境中出现的未知障碍物无法及时避障进行局部路径规划。DWA 算法路径规划缺少全局指引，只有目的地一个方向指引，在障碍物较多的环境中，容易陷入局部最优，导致全局路径变大，甚至路径规划失败。针对上述两种算法的问题，首先提取改进A*算法规划路径上的关键点，然后将提取的关键点作为DWA 算法的中间目标点，引导局部路径规划，融合算法又将已知障碍物和未知障碍物进行分类，减少已知障碍物对路径的影响，融合算法流程图如图4所示。

图4 融合算法流程图Fig.4 Flow chart of fusion algorithm

3 仿真验证

3.1 改进A*算法仿真实验

仿真实验在MATLAB 2016b 环境下进行，为验证算法的有效性和适应性，将对比实验的地图设计为迷宫式栅格地图。在栅格地图环境中，每个栅格设置为长为1 m的正方形，黑色栅格表示障碍物区域，白色栅格表示空白可移动区域，△表示机器人初始位置，○表示目标点。改进A*算法中障碍物比重参数K随起点和终点的位置变化而变化，安全距离D设置为0.8 m。传统A*算法与改进A*算法的仿真结果如图5所示，性能比较如表1所示。

图5 传统A*算法与改进A*算法仿真结果对比图Fig.5 Comparison of simulation results of traditional A* algorithm and improved A* algorithm

表1 传统A*算法与改进A*算法性能指标对比Table 1 Performance comparison between traditional A*algorithm and improved A* algorithm

由图5和表1可知，通过改进算法的评价函数，引入环境信息，在环境信息的引导下搜索更有方向性，使得路径规划时间减少0.8 s，转弯次数减少48.0%，转折角度减少了42.1%。通过借鉴Floyd算法思想对路径进行平滑优化，使得路径长度减少8.0%。由此可见改进算法不仅提高了算法的效率，而且降低了路径长度，提高了平滑度。

3.2 融合算法仿真实验

融合算法仿真实验环境地图在改进A*算法仿真实验的基础上做两点改变：（1）在改进A*算法规划路径上随机加上四个未知静态障碍物；（2）在仿真环境中随机加入一个未知动态障碍物。融合算法的评价函数的各项系数α=0.3、β=0.4、γ=0.2、φ=0.2，融合算法仿真实验机器人运动学参数如表2所示。

表2 机器人运动学参数Table 2 Kinematic parameters of robot

融合算法仿真实验结果如图6所示，图6（a）中粉色虚线为改进A*算法在静态环境中的全局路径规划，路径上的*为提取的关键点，作为融合算法的中间目标点。图6（b）中红色方块用来模拟全局路径上随机加入的未知静态障碍物，黄色方块用来模拟环境中随机出现的动态障碍物，且正在绕过第一个静态障碍物。图6（c）表示已绕过第二个静态障碍物，而后与动态障碍物相遇开始局部动态路径规划。图6（d）表示机器人在保证全局最优的基础上完成了起点到终点的动态路径规划。在路径规划的过程中轨迹末端绿色短曲线为模拟轨迹，黑色虚线为动态障碍物的移动轨迹，蓝色实线为融合算法规划路径。

图6 融合算法路径规划仿真结果Fig.6 Simulation results of path planning based on fusion algorithm

由图6融合算法仿真结果可知，提取改进A*算法路径上的关键点，作为DWA算法的中间目标点，使得规划路径靠近改进A*算法静态最优路径；融合算法又将已知障碍物和未知障碍物进行分类，减少已知障碍物对路径的影响，使得规划路径及时有效地避开未知动静态障碍物。

4 实验验证

实验选取Turtlebot2移动机器人平台进行物理平台实验验证。为满足实验需要，除Turtlebot2 自身所带的传感器外，还配备了思岚A2激光雷达和Kinect v1深度相机来感知周围环境。为了方便调试和机器人的自主导航，在Turtlebot2上配备一台小型工控机作为主机，个人PC作为从机，主机和从机都安装64位ubuntu16.04操作系统和机器人操作系统ROS（Kinetic）并在局域网内完成主机与从机的网络配置，通过IP 实现个人PC 对Turtlebot2 的远程控制。为了验证算法的可行性，用Turtlebot2 移动机器人，在同一环境中对三种算法进行对比验证。Turtlebot2 移动机器人的实验参数设置如下：最大线速度0.5 m/s，最大角速度0.25 rad/s，线速度采样个数为10，角速度采样个数为20，采样周期0.2 s。实验场地如图7所示。

图7 实验场地地图Fig.7 Experimental site map

通过从机打开从机终端，启动底盘，启动Gmapping构图算法。打开从机终端，启动键盘节点，打开Rviz可视化工具，通过键盘控制节点控制机器人运动，构建环境地图如图8所示。

图8 实验场地建图Fig.8 Construction of experimental site

4.1 传统A*算法实验验证

在ROS 系统下通过主机远程控制软件登录，实现对Turtlebot2 的远程控制。启动底盘控制节点，启动定位并选定地图，打开Rviz可视化界面，启动传统A*算法路径规划，选定起点和终点。运行结果如图9所示。

图9 传统A*算法路径规划Fig.9 Traditional A* algorithm for path planning

传统A*算法实验结果由图9，图中红色箭头摆动幅度反应拐弯的角度。由实验结果可以看出，传统A*算可以实现在静态环境中的全局路径规划，但是转弯的折点多，路径不够平滑，不利于机器人的行驶。

4.2 改进A*算法实验验证

保持起点和终点位置不变，启动改进A*算法路径规划，实验结果如图10所示。

图10 改进A*算法路径规划Fig.10 Improved A* algorithm for path planning

改进A*算法实验结果由图10，图中红色箭头摆动幅度反应拐弯的角度。由实验结果可知，相比于传统A*算法，改进A*算法的路径规划算法运动更加平滑，但是改进A*算法是全局静态路径规划，无法及时躲避环境中出现的障碍物。

4.3 融合算法实验验证

实验场景和设备与改进A*算法实验保持一致，在实验环境中站立三个人静止不动，作为静态未知障碍物，在小车运行过程中走动拍摄者作为动态未知障碍物。保持起点和终点位置不变，启动融合算法路径规划，实验结果如图11所示。

图11 融合算法路径规划Fig.11 Path planning based on fusion algorithm

改进A*算法实验结果由图11，图中红色箭头摆动幅度反应拐弯的角度。由实验结果可知，融合算法在全局最优的基础上，又能及时躲避环境中出现的动静态障碍物，提高了移动机器人在复杂环境中的适应能力。

5 结束语

传统A*算法路径规划存在效率低、路径规划不平滑等缺点，不能满足机器人的实际要求。这篇文章将环境信息引入传统A*算法的评价函数，优化搜索策略，使得路径的搜索随着环境复杂度的变化而自适应调整，提高了算法的效率和灵活性；基于Floyd 算法思想设计路径平滑算法，删除冗余节点和不必要的拐点，使得路径更加平滑，实验结果表明，改进A*算法在大尺度复杂环境中不仅能生成最优路径，效率也有所提高。提取改进A*算法的关键点作为DWA算法的中间目标点，在全局最优的基础上实现了算法融合。实验结果表明，融合算法规划路径围绕全局最优路径轻微浮动，且成功对比全局路径上出现的障碍物及环境中随机出现的动态障碍物。通过仿真验证和实验验证，结果表明了融合算法在机器人路径规划中的可行性。