融合改进A*和动态窗口法的AGV动态路径规划

吴飞龙，郭世永

(青岛理工大学机械与汽车工程学院，青岛 266520)

路径规划是AGV(automated guided vehicle)能够完成自动导航、任务调度、泊车等功能的核心技术之一。路径规划是指，按照一定的评价指标准(如距离、时间、代价等)，能使移动机器人避开障碍物安全高效地完成起点运动到目标点的任务[1-3]。

路径规划问题一直是智能机器人领域的一个研究热点[4]。目前，应用于路径规划的算法很多，包括基于采样的Voronoi图方法、RRT(rapidly exploring random tree)、遗传算法等[5-6]，基于节点的Dijkstra算法、A*算法、D*算法等[7-8]，基于模型的人工势场法、动态窗口法(dynamic window approach,DWA)算法等[9-11]，还包括其他一些经典算法，如蚁群算法、神经网络等。

AGV的路径规划一般要解决以下3个问题[4]：①规划出一条能使AGV从起点运动到终点的路径；②规划的路径要能避开环境中的所有障碍物；③以规划的路径最短为前提的条件下尽量是路径平稳圆滑为优化目标。

目前来说，A*算法是AGV自主路径规划领域常用的算法之一，文献[12]提出的一种改进后的A*算法虽然有效地规划出一条使移动机器人从起点运动到终点的相对圆滑的最短路径，能避免机器人横穿障碍物的危险，但规划的路径存在使AGV斜穿障碍物顶点的可能；文献[13]提出的改进的动态窗口算法虽然规划的路径最短，也能有效地避开障碍物，但不满足全局最优的路径规划。

提出一种全局采用结合跳点搜索算法[14]后改进的A*算法，局部采用改进的DWA(dynamic window approach)算法，由8个搜索方向变为5个，使得移动机器人寻路效率提高，设定了安全阈值(即路径与障碍物顶点的安全距离)和曲线圆滑函数，在保证全局最优路径的前提下，进行实时动态规划路径，规划后的路径比较圆滑，运动相对平稳。

1 AGV的运动学模型

简化后的AGV模型如图1所示，假设大地坐标系下的原点坐标为[x(t)，y(t)]，AGV在t0、tn时刻的位姿分别为X(t0)=[x(t0)，y(t0)，θ(t0)]，X(tn)=[x(tn)，y(tn)，θ(tn)],则AGV的运动学模型可以表示为

图1 AGV运动学模型Fig.1 AGV’s kinematics model

(1)

式(1)中：θ表示航向角；t表示时间。

1.1 运动约束

移动机器人运动学上面的约束分为速度约束和转角约束。

1.2 速度约束

设移动机器人初始状态下的速度为Vs,则：

Vs={(v,w)|v∈[vmin,vmax],w∈[wmin,

wmax]}

(2)

式(2)中，v、w分别表示速度和角速度，vmin、vmax和wmin、wmax分别速度和角速度的最大值和最小值。

当给机器人施加一定的加速度后，其速度变为Vd,则

Vd={(v,w)|v∈[vm-at，vm+at],

w∈[wm-α·t，wm+αt]}

(3)

式(3)中：vm、wm分别表示移动机器人当前的速度和角速度，a，α分别表示表示加速度和角加速度。

1.3 转角约束

AGV转角满足如下约束：

θr∈[θmin,θmax]

(4)

(5)

式中：θr表示当前时刻的航向角；θmin、θmax为航向角的最大值和最小值；Wθ表示当前时刻的角速度；L表示轴距。

2 基于改进后的A*算法全局路径规划

2.1 传统的A*算法

传统的A*算法的参考标准主要依据评价函数f(n)=G+H，其中，f(n)为从起始位置到目标位置的代价估计，G为从起始位置到当前位置的实际代价，H为从当前位置到目标位置的最佳路径的估计代价，如图2所示，浅绿色栅格点29为起点，红色栅格点33为中点，浅蓝色栅格点22、31、40为障碍点。首先，进入开始搜索模式，将起点29添加到open列表中，设置点29的G(即从起始点移动到当前节点的代价值)设置为0；接下来对点29进行四个方向的搜索，从而拓展为4个子节点，即点20、38、28和30,再将4个子节点添加到open列表中；然后将起点29从open列表中删除，添加到close列表中，分别计算4个子节点的f(f为从起始位置到目标位置的代价估计值)。以20为例，从父节点29移动到20需消耗1，即G为1，点20与终点33行数和列数分别相差1和4，所以H为5，这里设定一个基数10，最终点20的f为60，用此方法分别去计算其余三个节点的f，然后比较得到点30的f最小，为40，所以将节点30从open列表中删除，添加到close列表中，然后将点30更新为父节点，并继续四个方向的搜索。如此往复下去，不断更新，从而能找到最短的路径。当遇到障碍物时，将障碍物对应的结点删除，向其他几个方向进行拓展搜索。

图2 传统的A*算法的参考标准Fig.2 Reference standard of traditional A* algorithm

如图3所示，创建引导路径集合path,判断起始点S与目标点G之间是否为连通路径，若为连通路径，则直接将目标点作为下一个移动栅格点，形成路线1，由于起始点与目标点之间存在障碍物，所以此线路不通；接下来进入第一次全局搜索模式，将搜索到的离目标点最近的栅格点作为第2个移动栅格点，然后在进行第二次搜索，确定第3个到目标点最近的移动栅格点，如此不断地更新，将所有搜索到的最优移动栅格点保存到引导路径集合path中，形成path={S,N1,N2,…,Nn,G},如图3所示，确定的该路径为路线2={S,N1,N2,N3,G}。路线2存在的过多的折线路径，这样会使得移动机器人运动时很不平稳，而且从点N2移动到N3时经过障碍物的顶点，存在与障碍物碰撞的可能性，这种情况需要避免，因此，传统路径规划下确定的路线2存在不合理性，路线3成为改进的目标。

图3 引导路径Fig.3 Guided path

2.2 改进的A*算法

改进后的A*算法的流程如图4所示。

图4 改进A*算法流程Fig.4 Improved A* algorithm flow

2.2.1 评价函数的设计

A*算法是在静态环境中搜索最短路径行之有效的算法之一，其传统的评价函数为

f(n)=g(n)+h(n)

(6)

式(6)中：g(n)为当前点到起始点的路径代价函数；h(n)为当前节点到目标点的路径代价的启发函数。根据AGV的位置信息，设置了代价函数和启发函数的权重函数:

(7)

(8)

式中：n_x、n_y为当前点的横纵坐标；s_x、s_y为起始点的横纵坐标；g_x、g_y为目标点的横纵坐标。

改进后的评价函数：

(9)

式(9)中：r为当前点到目标点的距离；R为起始点到目标点的距离。

2.2.2 删除多余的转折点，保留关键的转折点

如果从起始点S经过两次更新移动到当前栅格点2(即节点)，若当前栅格点与第一次移动栅格点1及起始点之间在同一直线上，则第一次移动栅格点为多余节点，删除第一次移动的栅格点，直接从起点S移动到节点2，更新路径，依次搜索从起点S到终点G的所有节点，按照上述方式进行删除，使得从起点到转折点到终点的路径更加平滑。

2.2.3 安全阈值的设定

安全域值(k)即为移动路径与静态障碍物顶点的安全距离,如图5所示，设定k=0.8,假设当前节点为N1，连接N1、N2,则线段N1N2到静态障碍物顶点的距离必须大于等于安全域值(k)，如果段N1N2到静态障碍物顶点的距离小于安全域值k，则删除该节点，选择下一节点，直到满足上一节点与当前节点连接的线段到静态障碍物顶点的距离大于等于安全域值为止；若上一节点N1与下一节点N3的连接线段N1N3到静态障碍物顶点的距离也大于等于安全域值k，则当前节点为多余节点，删除当前节点，更新路径，从节点N3开始，不断重复上述操作，直到从起点到终点之间的所有转折点满足上述条件为止。

图5 安全域值Fig.5 Security threshold value

2.2.4 节点的优化

传统的A*算法，是在当前节点即父节点的周围进行8个方向的搜索，在不存在障碍物的前提条件下，移动机器人可以向周围8个方向的子节点移动，如图6所示。

图6 子节点图Fig.6 Child nodes graph

第一步将传统的8个搜索节点方向改为5个，主要判断依据是起始点与目标点的连线与正北方向的夹角，规则如表1所示。

表1 方向搜索规则Table 1 Direction search rules

第二步根据父节点与8个字节点的关系，设定子节点2和字节点6为Ⅰ类，子节点8和子节点4为Ⅱ类。如果障碍物为Ⅰ类，则删除子节点2和子节点6；如果障碍物为Ⅱ类，则删除子节点4和子节点8；如果障碍物不是Ⅰ类和Ⅱ类，则不做处理。

全局路径优化后，移动机器人不再可能斜穿障碍物顶点，且搜索效率更高，运行相对平稳。

3 融合改进A*算法和DWA算法后的局部路径规划

局部路径采用DWA算法，该算法通过在线实时规划路径，检测窗口滚动前进，具有良好的避障能力[15]。先用改进后的A*算法进行全局路径规划，在保证全局路径最优的情况下，进行局部在线实时规划路径。规划的动态窗口如图7所示。

图7 动态窗口Fig.7 Dynamic window

考虑到全局规划好的最优路径为折线路径，不满足现实环境中AGV的运行路况，移动机器人在这种折线路况下运动时，速度和加速度相对不稳定，基于这一点，实时在线规划后的曲线为很平滑的圆弧曲线如图8所示，其推导公式为

图8 局部路径规划下的AGV运动轨迹Fig.8 AGV’s trajectory under local path planning

Xk=(xk,yk,θk)

(10)

Xk+1=(xk+1,yk+1,θk+1)

(11)

Xk+1=f(Xk,uk+1)=

|Δθk+1|>0

(12)

Δθ=(ΔdL-ΔdR)/a

(13)

ΔD=(ΔdL+ΔdR)/2

(14)

(15)

(16)

式中：Xk为AGV当前时刻的位姿，xk、yk、θk分别为AGV当前的横纵坐标以及航向角；Δθ为航向角的增加量，ΔdL、ΔdR分别为改进前AGV直线行驶时的位移增加量，改进后AGV圆弧行驶时的位移增加量；Δxk、Δyk分别为AGV横纵坐标的增加量；rk为AGV圆弧行驶时的圆弧半径。

4 算法仿真验证

为了验证改进后的A*算法和融合DWA算法后的有效性，以建立的栅格地图来作为模拟环境，如图9所示。

从图9可以看出，改进后的A*算法所规划的全局路径转折点更少，路径更加平滑，更不会出现路径斜穿障碍物顶点的情况。由表2可以看出，改进后的A*算法搜索节点数明显减少，搜索效率得到大大提升，效率提升接近50%，最短路径长度也在一定程度上得到减少。

表2 传统A*算法和改进A*算法的搜索时间与搜索节点数量对比Table 2 Comparison of search time and number of search nodes of traditional A* algorithm and improved A* algorithm

图10分别为在传统DWA算法和改进DWA算法下的在线实时规划后的路径，灰色物体表示临时动态障碍物，经过实时规划后的路径能有效地避开动态障碍物，更符合现实生活中复杂的环境条件。图11和图12分别为传统DWA算法及融合改进DWA算法后的实时路径规划下移动机器人的运行速度和姿态变化情况，可以看出后者规划的路径更加平滑，后者规划下的移动机器人运行得更加平稳。

图11 移动机器人的移动速度Fig.11 Moving speed of mobile robot

图12 移动机器人的实时姿态Fig.12 Real-time posture of the mobile robot

空白区域为AGV可自由移动的安全区域；黑色区域为不同分布的障碍物；△表示AGV的运动起始点；○表示AGV需到达的目标位置图9 全局路径仿真结果Fig.9 Simulation results of global path

图10 动态窗口下的局部路径规划Fig.10 Local path planning under dynamic windows

5 实验验证

将改进后的融合算法应用到基于树莓派4B的智能小车上，该小车的整体结构如图13所示，它有1个树莓派4B主板，1个摄像头，1个电源装置，L298P电机驱动系统，4个驱动电机，2个避障传感器等组成。

图13 智能小车整体结构Fig.13 Overall structure of Intelligent Car

以智能小车来代替AGV作为研究对象，运用 Python语言实现系统功能的软件程序设计。该系统的控制框架如图14所示。

图14 系统的控制框架Fig.14 Control framework

采用融合改进的DWA算法对小车进行实时动态规划路径，实时规划的路径如图15所示，小车能有效地避开所有障碍物，平稳地从起始点运行到目标点。

图15 实时规划的路径Fig.15 Real-time path planning

为了进一步验证算法的有效性和可操作性，设置了不同的起始点和目标点，进行了多次实验，分别采用传统的A*算法和融合的新算法来进行比较，实验一共分为6组，每组分为5次去做，每组所设定的起始点和目标点相近，主要是为了减少操作带来的随机误差，每组取一个平均值，得到的结果如图16所示。

图16 传统A*算法和改进A*算法的数据比较Fig.16 Data comparison between traditional algorithm and improved A* algorithm

为了增加环境的复杂程度，又设置不同范围环境障碍物分布率β，设定β的值分别为10%、20%、30%、40%、50%，得到不同环境障碍物分布情况下的最短路径长度和转折角度的均值，如图17、图18所示。由表3可知，改进后的路径长度减少了1.6%，平均转折角度减少了接近2%，由此可见，改进算法后的智能小车具有良好的避障能力。

图17 平均路径长度Fig.17 Average path length

表3 传统A*算法和改进A*算法的各项平均值Table 3 The mean values of the traditional A* algorithm and improved A* algorithm in this paper

图18 平均转折角度Fig.18 Average turning angle

6 结论

(1)针对传统A*算法的不足进行改进，提出了融合的改进A*算法和DWA算法，一是设计了新的评价函数，根据AGV的位置信息，设置了代价函数和启发函数的权重函数；二是设置了安全域值，对路径节点进行了优化，删除了转折节点中多余的节点，保留关键的节点；三是通过动态路径规划对路径曲线进行圆滑处理，最后通过在实验环境中设置不同分布的障碍物，让AGV小车从不同起点运动到终点，小车都能完成任务，且具有良好的避障能力，进一步验证了算法的可行性。

(2)本文算法具有以下优点：①由传统的8个搜索方向变为5个，提高了搜索效率；②不会出现斜穿障碍物的危险，避障能力强，安全性提高；③路径曲线相对圆滑，最短路径长度和平均转折角度大大减少。