融合A-Star与DWA双优化算法的自动引导车路径规划

董翼宁,曹景胜,李刚

(辽宁工业大学汽车与交通工程学院,锦州 121001)

随着智能技术的蓬勃发展和不断进步,需要自动引导车的场景越来越多,许多智能制造车间,封闭园区的货物配送等都采用自动引导车,路径规划技术对自动引导车非常关键,它决定了自动引导车的工作效率。

在路径规划技术中,主要基于以下三大方案,一类是基于图搜索的路径规划方案,如A-Star算法[1-2]、Dijkstra算法[3]、人工势场法[4]等。姜辰凯等[5]针对自动引导车路径规划问题提出了一种改进Dijkstra算法,该算法可以避免规划过程的碰撞冲突。孔慧芳等[6]提出一种改进A-star算法,通过改进启发函数,引入转弯正代价参数,从而提高规划效率和路径平滑性。第二种是基于采样的路径规划方案。如RRT[7-8]算法,贺伊琳等[9]提出了一种优化RRT算法的无人驾驶路径规划,在传统RRT算法的基础上,限制采样可行区域,基于目标偏向策略,以一定的概率将目标点作为随机点进行随机树扩张,该方案改善了路径规划的效率。江洪等[10]针对RRT路径规划算法效率低且采样随机的问题提出了优化算法,提出了一种偏向目标点的新节点扩展方法。该方法将目标点引力、障碍物斥力以及随机点引力三力合一,并且添加了与障碍物距离相关的自适应函数。第三种方案是基于智能仿生的路径规划方案。如将蚁群算法[11]、遗传算法[12]应用到路径规划中。郭蓬等[13]将蚁群算法应用到无人车路径规划中,通过蚁群算法设置始末位置、构造解空间、更新信息素、增加迭代次数、判断路径长度,最后输出最短路线。针对传统蚁群算法在路径规划中搜索速度慢,路径规划容易陷入局部最优的问题,张仪夫等[14]提出一种双向蚁群算法。设计相遇机制,引入奖惩因子,加快求解最优路径的速度。

以上路径规划方案在静态环境中均有很好的效果。针对自动引导车路径规划中存在效率低,动态避障效果差的情况,现提出一种融合A-Star与DWA双优化路径规划算法。首先基于传统的A-Star算法设计自适应启发函数,进行关键点的选取,删除冗余点,之后融合优化动态窗口算法,进行实时地动态避障。

1 A-Star算法

1.1 传统A-Star算法

A-Star算法是一种传统的路径规划算法,路径规划领域得到了广泛的应用,该算法是由广度优先搜索算法(BFS)与Dijkstra算法进行融合,通过设置启发函数做静态全局路径规划。A-Star算法是静态全局路径规划中最有效的算法,它能找到最短路径。

A-Star算法本质上是广度优先搜索算法(BFS)的优化,它是一种搜索算法,首先将目标区域地图进行栅格化,在等效栅格化的地图中设置起始点和目标点,遍历起点周围八领域栅格空间内的点,寻找最优点,然后继续遍历该点周围八领域栅格周围内的最优点,不断更新最优点,直到绕过障碍找到目标点。最优点的获取原则是基于路径优劣评价函数,即

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

(1)

式(1)中:G(n)为从起点到达状态点n的代价估计;H(n)为启发函数,是状态点n到目标点的最佳路径代价估计。

启发函数H(n)是整个A-Star算法中最重要的部分,启发函数H(n)的设计直接影响路径规划的效率以及能否找到最优规划路径。传统A-Star算法中启发函数H(n)一般选用曼哈顿距离[式(2)]和欧几里得距离[式(3)]。

H(n)=|nx-tx|+|ny-ty|

(2)

(3)

式中:nx为当前节点的横坐标;ny为当前节点的纵坐标;tx为目标节点的横坐标;ty为目标节点的纵坐标。

曼哈顿距离允许向上、下、左、右4个方向上进行移动,欧几里得距离允许在上、下、左、右和斜向8个方向上进行移动。

1.2 优化A-Star算法

启发函数H(n)影响整个自动引导车路径规划的整体效率以及能否找到最优路径,启发函数H(n)相较实际代价越小就越容易找到一条优路径,但是牺牲了路径规划的效率。如果启发函数H(n)相较实际代价大,就会牺牲最优路径的选取,但是很大程度上缩短了自动引导车路径规划所需的时间。因此启发函数H(n)的设计至关重要,启发函数的设计要从全局出发,全局中障碍物比率也对路径规划有着重要的影响,本文根据当前位置距目标点的距离以及障碍物比率提出一种自适应的启发函数,通过对启发函数H(n)的优化来提升自动引导车的路径寻优和规划效率。由于车辆运动时在转弯和避障的方向不确定,因此要减小方向约束,曼哈顿距离和欧几里得距离均可以满足自动引导车路径规划在方向上的要求,自适应启发函数的设计选用欧几里得距离公式进行设计。自适应启发函数的设计为

(4)

Hadp(n)=(1+D)(1-lnK)H(n)

(5)

式中:sx为起始点的横坐标;sy起始点的纵坐标;D为当前节点到目标点的距离与起始点到目标点距离的比值;K为障碍物占比。

A-Star算法中实现了静态全局路径规划,但规划路径时存在冗余点过多,而且路径不够平滑的情况,冗余点对路径规划意义不大,而且增加了路径规划时的计算复杂度,本文研究针对这一问题提出了关键点选取策略,

在A-Star算法中冗余点很多是节点共线的情况,现提出一种去除冗余点的算法,其中,设路径点A坐标为(x1,y1),从A点起的第2个路径点的坐标为(x2,y2),从A点起的第3个路径点坐标为(x3,y3)。

(6)

式(6)中:C为常数。

如果满足式(6),则从路径点A起的第2个点为冗余点。随着A-Star算法不断进行节点迭代更新,也不断会生成新的路径点,依次循环遍历到目标点,去除所有满足式(6)的路径点。

利用插点法继续进行关键点选取,平滑路径。插点法的原理如图1所示。

Obstacle为障碍物

假设1为子路径点,4为目标路径点,从点1～点4为最短规划路径,2、3为规划路径点,从点1到点2再到点4的距离和小于点1到点3再到点4的距离和,因此选择路径点1、2、4。依次在搜索范围内遍历路径点进行选取。

对传统A-Star算法进行启发函数的优化和关键点选取后,缩短了路径的距离,平滑了路径,减少了路径规划的时间,实现了最优全局路径规划,优化的A-Star算法可以在一定程度上提升自动引导车路径规划的效率。

2 优化DWA算法

自动引导车在复杂的路面上行驶会遇到各种难以预料的障碍,静态路径规划不能满足自动引导车对动态障碍物的躲避,而动态窗口算法可以解决无法对动态障碍物实时避障的问题。DWA算法的基本原理是速度采样,建立由线速度和角速度(v,ω)构成的速度采样空间,对其进行实时采样,得到自动引导车的运动轨迹,得到的轨迹有许多条,按照评价函数对这些轨迹进行择优筛选,得到最优轨迹,根据最优轨迹的速度给到自动引导车控制器使车到达目标地点。

2.1 自动引导车运动模型

图2为离散化的自动引导车运动学模型,得到车辆位姿(x,y,θ)通过线速度和角速度的变化来反映引导车的运动,通过采样速度空间得到的一组(vt,ωt)代表一条所预测的轨迹。

图2 自动引导车运动模型

当采样时间间隔Δt比较小时,车辆的轨迹可认为是由一段短的线段连接而成。车辆的微分约束可以写成离散化的形式,即

(7)

2.2 速度空间采样

在速度空间中采样得到多组速度对,根据自动引导车的自身要求和其他环境的要求,对采样的条件进行限制。采样空间被限制在一个动态窗口中,根据自动引导车的自身要求,需要对其最大行驶速度、加减速的能力等进行约束。

(1)最大速度约束为

(8)

(2)车辆加减速能力约束为

(9)

(3)距离约束为

(10)

得到的动态窗口,分别对v、ω等时间间隔t采样,得到vi、ωi,将其组合得到多对(v,ω)i。dist(v,ω)表示轨迹上与障碍物最近的距离。

2.3 评价函数

评价函数是DWA算法中关键的一个步骤,传统的DWA算法的评价函数考虑了方位角、当前距障碍物的距离、当前速度这3个指标。

G(v,ω)=σ[αheading(v,ω)+βdist(v,ω)+γvelocity(v,ω)]

(11)

式(11)中:heading(v,ω)为方位角评价函数,用来评价自动引导车在当前设定的采样速度下,达到模拟轨迹末端时的朝向和目标之间的角度差距,其值越大,就说明此时模拟轨迹越朝向目标点,是全局搜索能力的代表;dist(v,ω)为自动引导车在当前轨迹上与障碍物之间的最近距离,是一项安全阈值;Velocity(v,ω)用来评价当前轨迹的速度大小,Velocity(v,ω)的值越大表明当前引导车线速度大,角速度小,沿着轨迹直线快速行驶。3个评价指标对最终路径规划和避障能力有着重要的影响,而全局障碍物比率K也对规划有着一定影响,引入障碍物比率关联3个评价指标,重新设计评价函数。优化后的评价函数为

G(v,ω)=σ[(1+k)αheading(v,ω)+

0.76(1-k)βdist(v,ω)+

(2-k)γvelocity(v,ω)]

(12)

为了避免某项标准在评价函数中太占优势,减小评价标准对评价函数最终结果的影响,对评价函数中的权重项进行平滑处理,进行归一化操作,如式(13)所示,进行平滑操作后是得自动引导车避开障碍朝着目标地点以一定的速度行驶。

(13)

式(13)中:normal_head(i)、normal_dist(i)、normal_velocity(i)分别为归一化后的方位角、距离以及速度评价函数。

3 融合优化A-Star与优化DWA算法

传统的A-Star路径规划算法应用范围广,适应性好,能够找到全局最优路径,但搜索效率不可控,优化A-Star算法对小车进行了高效静态的全局路径规划,找到了静态状态下的最优全局路径,但自动引导车在实际工作环境中不只需要对静态障碍物进行避障,道路环境中还有其他如行人等动态障碍物,DWA算法通过对自动引导车速度空间进行采样,约束车辆的运动状态,达到局部路径规划最终实现动态避障的效果。优化A-Star算法与DWA算法融合充分发挥了两个算法的优势。优化A-Star算法与优化DWA算法的融合算法流程如图3所示。

图3 融合算法流程图

4 仿真分析

为了验证本文优化的A-Star算法静态全局路径规划和优化DWA算法的性能以及优化A-Star算法和优化DWA算法的融合算法在动态避障局部路径规划的有效性,本文研究在windows10系统,i7-8550U CPU @ 1.80 GHz的环境下利用MATLAB2021a对算法进行仿真分析。

4.1 优化A-Star算法对比分析

文献[15]对A-Star算法进行了优化,提出了一种新型的启发函数,能够找到最佳路径,并在搜索效率上针对传统算法有一定提升,为了验证优化后的A-Star算法的鲁棒性和泛化性,选取3个不同环境进行传统算法中的Dijkstra算法、文献[15]算法和优化后的A-Star算法对比。如图4～图6所示。

Δ为路径规划的起始点;Ο为目标点

Δ为路径规划的起始点;Ο为目标点

图6 环境3

从3种不同的环境图上分析Dijkstra算法、文献[15]算法和优化A-Star算法上的性能上看,由图4～图6可以看出,优化后的A-Star算法规划的路径更加平滑。图中灰色阴影区域代表算法在路径规划过程中的搜索区域,可以明显看到在环境1、2、3中优化后的A-Star算法相较Dijkstra算法和文献[15]算法极大地减少了路径规划过程中的搜索范围,减少了计算路径规划系统的计算量,提高了工作效率。仿真结果如表1所示,从表1中可以看出,优化后的A-Star算法在环境1、2、3中路径规划距离相较Dijkstra和文献[15]算法有所减小,而且规划时间也大幅减小。关键点选取算法的使用减少了传统算法在路径规划过程中转折点过多的问题,减少了路径规划过程中路径点的数量。优化后的A-Star算法极大地提高了路径规划效率。

表1 不同算法性能对比

从仿真结果上来看,优化后的A-Star算法无论在路径规划最短距离、规划时间、搜索节点数、路径规划点上都比传统Dijkstra、文献[15]算法上有提升,优化后的A-Star算法达到了路径全局最优规划的目的,而且提高了效率。

4.2 优化DWA算法对比分析

对优化后的DWA算法与传统DWA算法进行仿真,对比分析两种算法的性能。优化评价函数,引入障碍物比率,将评价函数中heading(v,ω)、dist(v,ω)和velocity(v,ω)3个权重项关联。算法仿真如图7所示。算法性能对比如表2所示。

表2 优化DWA算法对比

Δ为规划起点;Ο为目标点

从该组实验结果上来看在同样的规划路径下,优化算法在动态规划时间上比传统算法减少了5.343%。提升了路径规划效率。

4.3 融合算法仿真分析

DWA算法的融入是为了保证自动引导车在运行过程中对动态障碍物的躲避,DWA算法实现了局部的动态避障规划。文献[15]优化了A-Star算法的启发函数并融入传统DWA算法实现了全局最优规划和实时避障的效果。为了验证本文提出融合算法的鲁棒性和泛化性,在3种不同环境与文献[15进行对比仿真,如图8～图10所示。表3为两种融合算法的性能对比分析,分别采用移动时间和移动距离对两种算法进行比较。从表3可以看到,本文的融合算法相较其他算法在效率上有很大提升,在3种环境下平均提升了19.24%,移动距离在简单环境下提升相对较小但是在复杂环境上有一定提升,提升了11.07%。

表3 融合算法性能对比

图8 环境1

图9 环境2

图10 环境3

5 结论

针对自动引导车路径规划问题提出了一种静态全局最优规划加局部动态避障规划的路径规划方案,满足自动引导车在工作过程中对路径规划算法所要求的全局最优和动态实时避障的要求。静态全局最优规划优化传统A-Star算法,优化后的算法不仅能得到最优路径而且规划出的路径与传统算法相比,较平滑,减少了转折点的数量,减少算法计算量,提高了规划效率。针对实时避障的要求,融合优化DWA算法。融合算法既达到了全局最优路径规划又做到了对动态障碍物的实时躲避。提高了路径规划效率和安全性。