改进A*和动态窗口法的移动机器人路径规划

陈 娇，徐 菱，陈 佳，刘 卿

(西南交通大学 交通运输与物流学院，四川 成都 611756)

0 引言

目前，无人车、移动机器人等移动智能体广泛应用于工业制造、物流、医疗等多个领域，路径规划问题是亟待解决的首要问题和难点。路径规划指用算法为移动机器人规划一条从起点到终点的无障碍、付出代价最小的最优或次优路径，路径规划的核心问题是路径规划算法。按照适用场景的不同，可将路径规划算法分为静态全局路径规划算法和动态局部路径规划算法。全局路径规划算法主要用于解决已知的静态环境中的移动机器人路径规划问题，普遍为传统算法，如人工势场法[1]、A*[2]、快速扩展随机树[3]等；局部路径规划算法主要解决在动态的全局未知或部分已知环境中的移动机器人路径规划问题，如动态窗口算法[4]、遗传算法[5]、蚁群算法[6]、神经网络算法[7]、粒子群算法[8]等。

A*算法虽然计算速度快、路径短，是目前普遍采用的全局路径规划方法，但是其规划的路径存在拐点过多、节点冗余、与障碍物距离近等问题。CAO等[9]提出Any-Angle A*算法，该算法基于可视图，在起点到目标点的连线附近进行搜索，减少了搜索节点，缩短了计算时间，然而其进行防撞处理时未考虑移动机器人的自身体积；ZHANG等[10]提出一种改进的A*算法对移动机器人的路径进行优化，该方法基于A*算法规划得到初始路径，然后遍历初始路径上的所有节点，删除不必要的节点和连接，并采用关键点选择策略进行二次规划，确定自动导引小车(Automated Guided Vehicle, AGV)在拐点处的旋转方向和旋转角度，该方法能够以更短的路径和更短的转弯时间提供更有效的路径规划;WANG等[11]在openlist中添加一个阈值N来减少搜索网格点的数量，并结合Floyd算法去除冗余节点，从而降低断点的锐度，最终通过几何优化过程得到一条新的路径，该方法提高了路径的平滑度，减少了路径长度，更适用于机器人导航。

应用动态窗口法(Dynamic Window Approach, DWA)进行路径规划虽然使移动机器人具有很好的避障能力，而且路径较为平滑，但是很容易陷入局部最优解，不能沿着全局最优路径到达指定目标。LI等[12]提出一种改进的动态窗口方法，该方法考虑移动机器人自身尺寸与障碍物之间可行空间的关系，使算法可以求解局部极小问题，提高了路径的平滑度；EDUARDO等[13]在传统动态窗口算法的基础上提出移动障碍物动态窗口法(Dynamic Window for Dynamic Obstacles, DW4DO)和移动障碍物树动态窗口法(Dynamic Window for Dynamic Obstacles Tree, DW4DOT)来处理动态障碍物，提高了算法的安全性和稳定性；MISSURA等[14]在动态窗口方法中加入一个动态碰撞模型来预测未来与环境的碰撞，同时考虑到其他物体的运动，不仅使算法保持较高的计算效率，还减少了动态环境下的碰撞次数。

由于移动机器人路径规划的任务和场景越来越复杂，对路径规划算法的要求不断提高，通常需要将全局路径规划与局部路径规划结合。程传奇等[15]提出一种融合改进A*算法和DWA的全局动态路径规划方法，针对传统A*算法设计了一种关键点选取策略，构造了考虑全局最优路径的评价函数，再结合DWA进行实时路径规划，使路径更加平滑；ZHU等[16]提出基于评价函数的A*算法和DWA的融合算法，在保证移动机器人局部避障能力的同时保持路径全局最优；王洪斌等[17]提出一种改进A*算法与DWA相结合的混合算法，使移动机器人能依次到达多个目标点，还提出将改进动态窗口算法与全局路径规划信息相结合的在线路径规划法，使移动机器人能够在动态复杂环境中局部避障并追击动态目标点，提升了算法的运行效率，有效缩短了路径长度并降低了转折总度数。Hong等[18]提出一种结合模糊逻辑的改进动态窗口算法，该算法通过分析目标和障碍物的信息，使用模糊逻辑实时确定合适的权重来实现，该方法使机器人的移动更加安全、平稳;白建龙等[19]针对增强的蚁群算法易陷入局部最优解的问题，设计了具有负反馈机制的改进的蚁群算法来解决机器人路径规划问题；李鑫等[20]针对自动导引小车在仓储物流搬运系统中的路径冲突问题，提出一种基于时空冲突约束的A*算法。

上述改进算法对传统A*算法和DWA从不同侧面进行了改进，但是未能完全解决所规划路径折点多、路径冗余、路径平滑度低及容易陷入局部最小等问题。因此，本文对传统A*算法和DWA进行改进，提出一种融合算法，利用A*算法进行全局规划后，采用关键折点提取策略剔除全局路径的冗余拐点和冗余节点，再结合改进DWA进行局部路径规划，最终为移动机器人规划出路径长度最短、平滑度高的安全路径。

1 算法基础

1.1 传统A*算法

A*算法是一种有效求解最短路径的搜索方法，也是典型的启发式算法，该方法从起点开始对周围指定的方向进行搜索，并计算当前节点周围每个节点到起点的累积实际代价值和到终点的估计代价值，选择总代价值最小的节点作为下一拓展节点，拓展到终点时停止搜索，完成路径规划。A*算法的启发式函数为

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

(1)

式中：f(p)为当前节点p的总代价值；g(p)为当前状态点p到起点s的实际代价值；h(p)为当前状态p到目标点d的估计代价值。常见的计算代价的方法有曼哈顿距离、欧氏距离和切比雪夫距离，本文算法重点比较所规划路径的长度，因此采用欧氏距离度量节点之间的代价，欧氏距离计算公式为

(2)

式中：(xs,ys)为起点s的坐标；(xd,yd)为目标点的坐标。

1.2 动态窗口法

DWA符合移动机器人的运动特征、灵活性强，因此成为动态环境下局部避障的主要算法。DWA的基本思想是结合移动机器人的运动特性，在路径规划过程中实时预测时间t内移动机器人的速度空间(vt,wt)和状态空间(xt,yt,yawt,vt,wt)，模拟移动机器人在预测时间内的运动轨迹，再根据评价函数确定最优运动轨迹，直至到达目标位置完成路径规划。假设移动机器人在时间段(t2-t1)内运动，则其位姿变化信息为：

xt2=xt1+vΔtcos(θΔt)；

yt2=yt1+vΔtsin(θΔt)；

θt2=θt1+wΔt。

(3)

式中：xt2和yt2分别为t2处移动机器人的x，y坐标位置，θt2为t2处移动机器人的航向角；v为移动机器人的线速度；w为移动机器人的角速度；Δt为t1～t2的时间间隔。

对预测时间t内的移动机器人速度空间采样后，采用评价函数评价预测轨迹。DWA的评价函数通常根据实际需要确定，一般情况下主要由目标距离代价、速度代价、障碍物代价、方位角代价等代价函数组成，本文设定DWA的评价函数如式(4)所示，规定在路径规划过程中，总代价越小越优。

C(v,w)=

αD(v,w)+βS(v,w)+λO(v,w)。

(4)

式中：C(v,w)为速度采样空间的总代价；D(v,w)为速度采样空间到目标点位置的距离代价；S(v,w)为速度采样的速度代价；O(v,w)为速度采样空间到障碍物的距离代价；α，β，λ分别为目标距离代价、速度代价、障碍物距离代价的单位代价增益。

各代价函数D(v,w),S(v,w),O(v,w)的具体函数如下：

S(v,w)=vmax-vt；

i=1,2,3,…,n。

(5)

式中：(xt,yt)为预测节点的(x,y)坐标位置；(xd,yd)为目标点的(x,y)坐标位置；vmax为移动机器人配置的最大速度；vt为预测速度空间的速度；(xoi,yoi)为第i个障碍物的(x,y)坐标位置。

2 融合算法

传统的A*算法应用范围广、适应性强，但是其规划的路径往往折点较多，路径平滑度极低。DWA规划的路径平滑度高，能实现实时避障，但是极易陷入局部最优，寻路成功率低。本文提出一种基于改进A*算法和DWA的融合算法，充分利用两种算法的优势，实现路径长度、平滑度和安全性的三重优化。

2.1 环境模型描述

本文采用栅格法创建路径规划的环境地图(如图2)，将环境划分为若干大小均等的栅格，根据实际环境将对应栅格设置为空闲状态和占据状态，其中空闲栅格用白色表示，占据栅格用黑色表示。结合二维平面直角坐标系，栅格横轴用x坐标表示，数值从左到右依次递增，纵轴用y表示，数值从下到上依次递增，栅格的具体位置用P(xi,yj)(i,j=1,2,3,…,n)表示。

假设移动机器人路径规划的起点为S(1,1)，终点为D(14,13)，分别采用传统A*算法和DWA在图2所示的栅格地图上进行路径规划，得到的路径如图3所示。

由图3a可见，传统A*算法规划的路径存在较多冗余节点和路段，其转折角度大，路径未达到最短，且路径曲率不连续，不符合移动机器人的运行规则。由图3b可见，DWA规划的路径整体比较平滑，但是存在大量冗余路段，通过长途绕路规划的路径并非最优。

2.2 全局路径优化

针对A*算法规划的路径存在较多冗余节点和冗余路段等缺点，提出一种关键节点提取算法过滤A*算法所规划路径的冗余节点，保留路径必经的关键折点，使优化后的路径更短、折点更少，全局优化的具体步骤如下：

(1)获取A*算法所规划路径的全部节点集U{Pi,1≤i≤n}，其中P1表示规划路径的起点，Pn表示规划路径的终点。创建关键节点集V，用于存放算法优化后的关键转折点，集合初始值为V{P1,Pn}，即路径起点P1和终点Pn。

(2)从P1开始作直线，依次连接P3,P4,…,Pm，判断直线P1Pm是否经过障碍物，是则节点Pm-1为路径的必经节点，且必为关键转折点，将节点Pm-1添加至集合V；否则，判定节点P2,…,Pm-1为冗余节点，从Pm继续向后连接节点集U中的节点，直到连接到路径的终点Pn，将所有关键节点均添加至集合V。关键节点选取完成后，集合V{P1,Pm-1,…,Pm+k,Pn}中就包含了所有关键节点，设该关键节点数为m。

(3)依次连接集合V中的所有节点，完成对路径的全局优化，具体步骤如图4所示。

比较图3a和图4c可知，全局优化后的路径长度从20.90缩短到18.91，优化了9.52%；折点从17个减少到5个，优化了70.59%；路径的累积转弯角度从270°降低到140.03°，优化了48.14%。综上所述，利用关键节点提取算法对路径进行全局优化后实现了对路径长度和路段冗余的优化，然而路径仍然存在平滑度低、与障碍物距离过近等缺点。

2.3 局部路径优化

针对DWA易陷入局部最优和成功率低等缺点，以及全局优化后路径仍然存在平滑度低等问题，继续对路径进行局部优化。用2.2节全局优化得到的关键节点对路径进行分段，再用改进DWA进行局部路径优化，改善传统DWA容易陷入局部最优的问题，并提高整体路径的平滑度和安全性。

由DWA的原理可知，路径的影响因素主要是移动机器人的航向角和角速度，因为角速度通常为固定配置值，所以影响路径平滑程度和路径长度的主要因素是航向角。航向角偏差越大，所需的转弯距离越长，越容易造成路径冗余，因此本文主要对航向角进行改进和优化，具体步骤如下：

(1)从起点P1开始，依次将关键节点集V{P1,Pm-1,…,Pm+k,Pn}中除终点Pn外的所有节点作为局部路径规划的起点S1,S2,…,Sm-1；从第2个节点Pm-1开始，依次将集合V{P1,Pm-1,…,Pm+k,Pn}中的所有节点作为局部路径规划的终点D1,D2,…,Dm-1，将全局路径分为S1D1,S2D2,…,Sm-1Dm-1共m-1段路。

(2)计算路径S1D1,即原线段P1Pm-1的倾斜角度

(6)

式中：xS1,yS1,xD1,yD1分别为第1段路径起点S1和终点D1对应的x轴、y轴坐标值。

(3)将路径的倾斜角度转换为弧度值，作为移动机器人的初始航向角。倾斜角度转化弧度值的公式为

yaw=α×180°/π。

(7)

(4)初始化移动机器人的状态参数集L{l}，其中l(x,y,yaw,v,w)记录移动机器人路径规划过程中的状态参数，包括位置、航向角、线速度、角速度。

(5)按照实际环境需要，设定移动机器人的初始线速度v(单位：m/s)、初始角速度w(单位：rad/s)，结合前3步得到起点S1(单位：m)、终点D1(单位：m)、初始航向角yaw(单位：rad)，综合得到l1(x1,y1,yaw1,v1,w1)。

(6)采用DWA进行局部路径规划，并将机器人的所有状态参数li更新到到状态参数集L中，记录路径的所有节点和机器人的位姿信息，每段路径完成后状态参数集变成L{l1,l2,…,li}，li(xi,yi,yawi,vi,wi)表示移动机器人的最新状态参数。

(7)按照式(2)计算下一段路径的倾斜角度α，同时将机器人上一状态参数l中的yaw转换为角度值β，弧度值转化为角度值的公式为

β=yaw×π/180°。

(8)

(8)重复步骤(7)，直至移动机器人到达第m-1路段的终点Dm-1，得到记录移动机器人的所有状态参数集L{l1,l2,…,li,li+1,…,ln}，完成局部路径优化。

路径局部优化步骤如图5所示。

比较传统DWA和局部优化后的路径图以及两条路径的相关数据可知，路径长度从26.81缩短到20.25，优化了24.47%，改善了传统算法易陷入局部最优、路段冗余等问题，而且基本实现路径最短。综上所述，改进A*算法和改进DWA的融合算法，一方面成功改善了传统A*算法所规划的路径平滑度和安全性低、不符合移动机器人运动特征等问题，另一方面成功改善了动态窗口算法规划路径时易陷入局部最优、路径过长等缺点，所规划的路径平滑度、安全性、长度3方面均得到较大程度的改善与优化。

3 仿真实验与分析

为了验证本文所提融合算法的有效性，采用传统A*算法、DWA和本文融合算法进行了多组仿真对比实验。

3.1 对比仿真实验

在文献[15]中规模为20×20的栅格地图上，采用3种算法对移动机器人的路径规划进行仿真，为保持良好的对比效果，在实验过程中，移动机器人的最大线速度、最大角速度、最大线加速度、最大角加速度、线速度分辨率、角速度分辨率、时间分辨率、预测周期等参数均与文献[15]所述的仿真参数相同，分别为1 m/s，20°/s，0.2 m/s2，50°/s2，0.01 m/s，1°/s，0.1 s，3 s，路径规划的起点与终点也相同，分别为S(2.5,1.5)，D(11.5,19.5)。实验环境为运行内存8 GB的64位WIN 10操作系统，实验平台为Python 3.7。

采用传统A*算法进行路径规划得到的路径如图6a所示，此时路径的长度为22.899 5，共有21个节点，转弯次数为4，累积转弯角度为180°。对路径进行全局优化得到的路径如图6b所示，此时路径的长度为20.208 135，共有3个节点，转弯次数为3，累积转弯角度为63.43°。采用传统DWA进行路径规划，算法迭代2 000次后得到的路径如图6c所示，此时算法陷入局部最优，寻路失败。本文改进融合算法规划的路径如图6d所示，此时路径长度为21.579 0。

由图6a可见，传统A*算法规划的全局路径存在较多转折点和冗余路段，路径曲率不连续，且路径上有些节点与障碍物距离过近，安全性较低。由图6d可见，融合算法规划的路径转折明显减少，路径曲率连续，平滑度高，且路径各节点都与障碍物保持适当的安全距离。对比传统A*算法和融合算法的仿真数据可得，路径长度优化了5.77%，路径平滑度优化了64.76%，路径安全性优化了100%，融合算法同时实现了路径长度、路径平滑度和路径安全性的三重优化。

3.2 实际环境仿真实验

为验证本文融合算法在实际应用中的有效性，在机器人操作系统(Robot Operating System, ROS)中建立16×16×3的实际仿真环境，如图7所示。采用即时定位与地图构建(Simultaneous Localization and Mapping, SLAM)扫描仿真环境建立相应的地图，如图8所示。实验环境为运行内存为4 GB的64位Ubuntu 16.04操作系统，实验平台为ROS(Kinetic)，实验引入的移动机器人为Turtlebot3-Burger。路径规划实验为从起点位置(1,1)到目标点(14,2)处获取物品。实验中Burger的参数设置与文献[15]相同。

采用ROS系统默认的导航算法和本文所提融合算法在环境中进行仿真实验，得到Burger的路径如图9和图10所示。

综上可知，采用ROS默认的导航算法规划的路径比较平滑且曲率连续，符合移动机器人的运动规则，然而该路径有较多转折路段，且转折角度较大，导致存在多段冗余路径。采用本文所提融合算法成功规划出了无障碍路径，而且在保证路径平滑程度的基础上减少了路径转折和冗余路段，实现了对路径平滑度和长度的双重优化。

4 结束语

针对传统A*算法所规划路径平滑度和安全性低、DWA所规划路径易陷入局部最优等问题，本文提出一种综合采用改进A*算法和DWA的融合算法，其结合两种算法优势，避免了路径规划中的缺陷。最后通过传统A*算法和DWA的仿真对比实验，证明本文改进融合算法实现了对路径长度、平滑性和安全性的优化；同时结合ROS的实际环境仿真验证了本文改进融合算法能高效完成实际环境的路径规划任务，优化路径长度和平滑度。未来将在多种应用场景中研究多移动机器人的路径规划与协调算法，推动移动机器人的算法研究和实践应用。