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

丰雪艳,李振璧,2*

(1. 安徽理工大学 电气与信息工程学院,安徽 淮南 232000;2. 亳州学院 电子与信息工程系,安徽 亳州 236000)

0 引言

连接起点位置和终点位置的序列点或曲线称之为路径,构成路径的策略称之为路径规划[1].根据障碍物是否完全已知,路径规划又可分为全局路径规划和局部路径规划.

全局路径规划算法有基于图搜索的A*算法、D*算法,也有智能仿生算法如粒子群算法、人工鱼群算法等[2].其中具有启发式思想的A*算法被认为是进行静态全局路径规划最有效的方法,但它规划出的路径存在拐角过大、扩展节点较多等缺点.申瑞等[3]提出的改进A*算法采用精简搜索策略、安全保护策略、路径平滑策略可以快速搜索出一条安全平滑的路径,提高了搜索效率和安全性.王迪等[4]将A*算法的邻域节点进行分级,根据节点的优先级确定要扩展的方向,然后利用Floyd算法剔除所规划路径中的多余节点,提高规划路径的平滑度.汪四新等[5]采用双向A*算法进行路径规划,并根据人工势场法的思想设置虚拟目标点和距离衰减系数,使得双向搜索能在中间区域相遇,在规划路径的基础上提高了算法的收敛速度.

局部路径规划算法有DWA算法[5]和人工势场法[6](Artificial Potential Field,APF).DWA算法可用于未知环境下的动态避障,可以较好地躲避障碍物,其优点是计算复杂度低,同时考虑了移动机器人的动力学特性,可以规划出一条安全无碰撞的路径[7-8].Mai等[9]提出的改进DWA算法可以提前感知障碍物的分布情况,使机器人在行驶时安全地避开障碍物密集的区域.姚进鑫等[10]将曼哈顿和欧氏距离进行结合作为优化A*算法的评价函数,在获得全局路径后以DWA算法为核心规划出一条平滑度较高的全局最优路径,有效解决了优化A*算法规划路径转折曲率不连续的问题.

综上所述,本文提出一种融合改进A*算法和DWA算法的机器人路径规划算法.该算法首先在传统A*算法启发函数前引入动态的权重系数,实现自适应调整以提高算法的搜索效率,然后采用关键点选取策略剔除路径上的冗余节点,规划出一条只含有关键点的全局路径,最后将改进A*算法所规划的路径上的关键点作为DWA算法的中间目标点,使局部路径遵循全局路径,并实现动态避障.

1 改进的A*算法

1.1 传统A*算法

A*算法是全局路径规划中常用的一种图搜索算法,其结合了Dijkstra算法和广度优先算法(Breadth-First-Search,BFS)的优点,提升了算法的搜索效率[12].代价函数为:

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

(1)

式中:n为当前节点;F(n)为总的代价值;G(n)为起点到当前点所花费的实际代价;H(n)是启发函数,为当前点到终点的预估代价值.传统A*算法常用的启发函数有曼哈顿距离、切比雪夫距离和欧几里德距离3种[13],本文选取欧几里得距离作为启发函数.

1.2 优化评价函数

本文对A*算法进行优化,结合标准正态分布引入随机器人位置而变化的动态权重系数w(x),当前点离终点较远时w(x)较大,此时启发函数在代价函数中所占的比重增加,提高了算法的搜索速度;随着移动机器人逐渐向终点靠近,w(x)也随之减小,此时启发函数在代价函数中所占的比重减少,扩大了算法的搜索范围,使其能够搜索出最短路径.具体计算公式为:

F(n)=G(n)+w(x)H(n).

(2)

(3)

(4)

式中:r为起点到当前点的距离;R为当前点到终点的距离.

1.3 关键点选取策略

传统A*算法规划的路径存在一些共线的冗余点和多余的转折点,若机器人以传统A*算法所规划的路径行驶,则需要在一些不必要的转折点改变其航向角,这些多余的转折会影响机器人的正常行驶.在实际工作中,机器人运动路径的平滑程度会影响机器人工作的完成度,路径越平滑,工作时产生误差的几率越小,工作的完成度就越好.因此,本文引入了关键点选取策略,即在节点(m-1,m,m+1)中,连接不相邻的两点(m-1,m+1),如果连线经过障碍物区域,则m为关键点,否则为冗余点.通过关键点选取策略剔除冗余点,得到一条包含关键点,起点和终点的优化路径.关键点选取策略示如图1所示,优化前的路径为s-1-2-3-4-5-g;优化后的路径为s-4-g,其中4为选取的关键点.

图1 关键点选取策略

2 DWA算法

DWA算法是具有避障能力的局部路径规划算法,其考虑了机器人动力学约束问题[14].DWA算法在机器人运动过程中对其线速度v与角速度w进行采样,以此来模拟机器人在一定时间间隔Δt内可能出现的运动轨迹,并利用评价函数对该运动轨迹进行评价,选取出评价最高的轨迹,将该轨迹对应的线速度v和角速度w作为机器人下一周期运动的速度.

2.1 机器人运动学模型

在对机器人的运动轨迹进行模拟前,需要根据机器人的线速度v和角速度w建立机器人的运动学模型,对于两轮差速模型,机器人t时刻的线速度为vt、角速度为wt、航向角为θt,设机器人在较短时间间隔Δt内做匀速运动,则其运动轨迹近似为一条直线,运动学模型表示为[15]:

(5)

2.2 速度空间

(1)受机器人硬件性能以及环境等因素的影响,机器人速度约束表示为:

(6)

式中,vmin、vmax分别表示线速度的最大值和最小值;wmin、wmax分别表示角速度的最小值和最大值.

(2)受机器人电机性能的影响,机器人需要在电机力矩可以承受的范围内进行加减速,机器人电机加减速约束表示为:

(7)

式中,vc、wc分别表示当前的线速度和角速度;va、wa分别表示机器人的最大线加速度和角加速度;vb、wb分别表示机器人的最大线减速度和角减速度.

(3)为了避免机器人与动态障碍物发生碰撞,需要保证机器人与障碍物之间有一定的安全距离,因此需要对机器人的速度进行约束,速度约束表示为:

(8)

式中,dist(v,w)为轨迹与障碍物的距离.

机器人的速度范围vr满足以上3种约束,即:vr∈vm∩vd∩vs.

2.3 评价函数

根据机器人的运动学模型和速度范围可以模拟出多条采样轨迹,需要采用评价函数对多条采样轨迹进行评价,选取一条评价最高的轨迹作为机器人下一周期的运动轨迹,评价函数为:

(9)

式中:σ为平滑系数;α、β、γ为各子函数的加权系数;head(v,w)为当前速度下轨迹末端点与目标点的方位角偏差;dist(v,w)为轨迹与障碍物的最近距离;vel(v,w)为当前速度大小的评价函数.

3 融合算法路径规划

改进后的A*算法虽然能寻找到一条最优路径,但无法躲避环境中出现的动态障碍物.DWA算法虽然可以躲避动态障碍物,但缺少全局指引,在障碍物较多的复杂环境中易陷入局部最优或无法规划出一条到达目标点的路径.因此本文对两种算法进行融合,将改进A*算法所规划路径的关键点作为DWA算法的当前目标点,通过融合算法规划的路径,可以在保证全局最优的基础上实现动态避障.融合算法流程如图2所示.

图2 融合算法流程

4 仿真实验及分析

为了验证融合算法的可行性,在Matlab2022a中进行仿真实验.仿真实验地图为20 m×20 m、40 m×40 m两种,黑色区域表示已知障碍物,灰色区域表示未知障碍物,白色区域表示可移动区域,S表示起始点,T表示目标点.

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

改进的A*算法在传统A*算法的基础上对启发函数进行了优化,并利用关键点选取策略剔除冗余点.在20 m×20 m、40 m×40 m的栅格地图上,分别使用传统A*算法和本文A*算法进行对比实验,在20 m×20 m栅格地图中,起始点为S(1.5,1.5),目标点为T(19.5,17.5);在40 m×40 m栅格地图中,起始点为S(1.5,1.5),目标点为T(39.5,37.5).两种算法的性能指标如表1所列,仿真结果如图3和图4所示.

表1 改进前后算法性能指标对比

(a)传统的A*算法 (b)改进的A*算法图3 20 m×20 m栅格地图下的对比实验

(a)传统A*算法 (b)改进A*算法图4 40 m×40 m栅格地图下的对比实验

根据表1可知,在20 m×20 m的栅格地图中,改进的A*算法所规划的路径长度比传统A*算法所规划的路径长度减少了4.89%,其规划路径所用时间比传统A*算法减少了18.18%,其规划路径的转折次数比传统A*算法减少了57.14%,路径的转折角度也比传统A*算法减少了63.31%;在40 m×40 m的栅格地图中,改进A*算法所规划的路径长度比传统A*算法所规划的路径长度减少了4.33%,其规划路径所用时间比传统A*算法减少了31.48%,其规划路径的转折次数比传统A*算法减少了53.33%,路径的转折角度也比传统A*算法减少了58.06%.结合图3、图4可知,改进的A*算法不仅剔除了共线的冗余点和多余的转折点,其规划的路径长度和所用时间相比传统A*算法也有所减少,提高了搜索效率和路径的平滑度.

4.2 融合算法对比实验

为验证融合算法的路径规划及动态避障能力,在20 m×20 m的栅格地图中进行实验,在改进的A*算法所规划的全局路径上添加随机障碍物,其在图中用灰色方格表示,融合算法的仿真实验结果如图5所示.

(a)躲避未知障碍物 (b)到达目标点图5 融合算法规划路径

虚线代表改进的A*算法所规划的路径,实线代表融合算法所规划的路径,*代表融合算法的当前目标点.随着机器人的运动,下一个关键点将会成为新的当前目标点.

实验结果表明,提取改进A*算法所规划路径的关键点,将其依次作为DWA算法进行局部路径规划的当前目标点,可在全局路径的基础上规划出一条能够躲避未知障碍物的局部路径,实现了机器人导航的全局路径最优与动态避障功能.

5 结论

针对传统的A*算法存在搜索效率低、规划路径平滑度差以及不能躲避未知障碍物等问题,本文提出一种融合改进的A*算法和DWA算法的机器人路径规划算法,与传统A*算法相比,该算法所规划的路径更加平滑,且能躲避环境中的未知障碍物;与DWA算法相比,该算法所规划的路径更优,且不易陷入局部最优.实验结果表明,融合算法实现了机器人导航路径全局最优以及动态避障功能,具有一定的可行性.