移动机器人是一种不需要人工干预,并能够自主移动的设备,在实际应用中扮演着越来越重要的角色[1]。路径规划问题一直是移动机器人领域的重点研究内容之一。路径规划是指机器人能够在无人干预的情况下,自发地规划出一条从起始点到目标点的路径,并能够躲避未知障碍物和动态障碍物[2]。路径规划分为全局路径规划和局部路径规划。目前,得到广泛应用的全局路径规划算法有A-star算法[3]、Dijkstra算法[4]、深度优先搜索算法[5]、RRT算法[6]、粒子群算法[7]等。局部路径规划有动态窗口法(dynamic window approach,DWA)[8]、人工势场法[9]、模型预测控制法[10]等。
传统的A-star算法存在拐点过多、冗余点过多、探索节点过多、算法耗时长等问题。传统的DWA算法容易陷入局部最优,无法按照全局最优路径到达目标点。随着上述问题在机器人领域局限性越来越明显,许多专家学者对其进行了研究[11-13]。孙岩霆等[14]对A-star算法引入启发函数动态加权策略,提高了A-star算法的搜索效率,并采用路径转角节点角度削弱策略,减少了转角和路径长度,并以全局路径提供子目标点引导DWA算法进行局部路径规划。汤玉春等[15]针对A-star算法耗时长、搜索节点过多、路径不平滑等问题,对A-star算法引入障碍物信息和搜索节点到起始位置的距离信息动态调节启发函数的权重,提高了A-star的搜索性能。Li等[16]对环境信息进行量化,并引入A-star算法的评价函数,实现了评价函数中启发函数的自适应变化,提高了算法的灵活性。Yin等[17]改进了A-star算法的评价函数和双向搜索策略,提高了路径搜索效率,消除了路径中不必要的路径点。Liao等[18]在A-star算法的启发函数中加入自适应权重,引入了Douglas-Pucker细化算法消除冗余点,在DWA算法中加入轨迹点估计函数,解决了传统A-star算法遍历过多、搜索时间过长、路径不平滑等问题。Liu等[19]改进了A-star算法结构,修改了传统的A-star算法的路径规划规则,减少了算法的计算量和数据冗余,在复杂环境中具有明显的优势。
为了提高A-star算法搜索最优路径的能力,本文对A-star算法的启发函数进行改进,加入了梯度计算、自适应权重分配函数和转折点代价。为了提高传统DWA的避障能力和按照全局路径寻路能力,本文对DWA的评价函数进行改进,将障碍物评价函数拆分为静态障碍物评价函数和动态障碍物评价函数,设计了轨迹距离评价函数。并从A-star算法规划的路径中提取关键点作为DWA算法的局部目标点,指导机器人以达到最终目标点。
1 A-star算法
1.1 A-star算法基础
A-star算法因其具有高效的搜索效率和路径规划速度,被广泛应用于路径规划。A-star算法是基于图搜索的算法,从起始点开始,A-star算法都会寻找与当前节点相邻的周围节点,并计算其评估函数值,在周围节点中选择评估函数值最小的节点作为下一次搜索节点,直到搜索到目标点。其评估函数为
F(n)=G(n)+H(n)。
式中:F(n)为总评估代价;G(n)为起点到当前节点的实际代价;H(n)为启发函数,即当前节点到目标点的估计代价。G(n)和H(n)通常使用欧几里得距离表示,其计算公式为
G(n)= ,
H(n)= 。
式中:(xc,yc)为当前点坐标;(xs,ys)为起始点坐标;(xg,yg)为目标点坐标。
1.2 改进A-star算法
1.2.1 启发函数改进
传统的A-star算法在复杂环境下搜索效率较低。针对以上问题,对A-star算法的启发函数进行改进。为启发函数H(n)引入子节点的梯度信息,提供更多的地形和环境信息,加强算法处理复杂环境的能力。设当前节点周围节点坐标为(xi,yi),i∈{1,2,3,4,5,6,7,8},计算子节点与当前节点的方向向量,再以方向向量乘以子节点与当前节点的估计代价差值作为梯度Gd,则其计算公式为
Gd=min[((xi-xc,yi-yc))×(H(xi,yi)-H(xc,yc))]。
式中:H(xi,yi)表示计算周围子节点的估计代价值;H(xc,yc)表示计算当前点的估计代价值。改进后的启发函数H'(n)为
H'(n)=Gd 。
1.2.2 启发函数动态加权
设对启发函数增加的动态权重为η,权重η的取值会对A-star的搜索效率和搜索性能产生直接的影响。如果权重η=0,从而使A-star算法退化成Dijkstra算法,搜索效率会显著下降。如果权重η的值很大,导致A-star算法变为BFS算法,容易忽视其他潜在的路径选择,导致找到的路径可能并非最优解。当权重η接近1的时候,算法会相对均衡地考虑到启发函数提供的启发信息和实际代价,这种情况下,算法更容易找到较优路径。而根据地图环境选取合适的动态权重,可以对地图的环境进行动态适应,提高A-star算法的搜索效率。环境中障碍物较多时,启发函数的影响更加显著,可能导致算法忽略一些可能最优的路径选择,因此在障碍物较多时应该适当降低权重值η。环境中障碍物较少的时候,启发函数的影响较小,会导致算法搜索较多无用的节点,所以在这种情况下应适当地提高权重η值。综上,考虑到距离目标点位置的动态变化和障碍物信息,提出一种动态加权权重η为
η= +Cln OB+1。
式中:Dc为当前点到目标点的欧几里得距离;Ds为起始点到目标点的欧几里得距离;OB为全局已知障碍物大小总和与地图大小的比值;C为小于1的常数。
1.2.3 转弯代价函数设计
传统A-star算法在规划路径的时候,规划的路径非最优路径,存在拐点过多的情况,会造成路径长度增加、路径的复杂性增加等不利影响。为了减少上述影响,在总代价函数中加入转弯代价函数。函数的计算过程如下:遍历closeList_path(),从起始点开始,选取相邻的3个节点(x1,y1)、(x2,y2)、(x3,y3),计算其两斜率是否相等来判断是否存在拐点,计算公式为
(y2-y1)×(x3-x2)=(y3-y2)×(x2-x1)。
若公式(7)成立,则点(x2,y2)不是拐点,反之则是拐点,遍历完closeList_path(),计算得出路线总的拐点数,存入turn_counts()中,turn_counts()即为转弯代价函数,记作tc,加入总的代价函数F(n)中,改进后的代价函数F(n)为
F(n)=G(n)+ηH'(n)+tc。
2 DWA算法
2.1 DWA算法基础
DWA算法是一种常用的局部路径规划方法,通过考虑机器人当前状态下的速度和加速度限制,以及局部环境的障碍物信息,来生成安全有效的移动轨迹。假设机器人在时间段(T0-T1)内运动,且为非全向移动,则对应的位姿变化信息为
式中: 和 分别为T1处机器人的x、y坐标位置;θ(ΔT)为ΔT时间内机器人的角度变化; 为T1时刻机器人的航向角;v为机器人的线速度;ω为机器人的角速度;ΔT=T0-T1。
在速度空间(v,ω)中进行采样,根据运动学模型推测对应的轨迹,再通过总评价函数对轨迹进行打分,选取最优的机器人轨迹,总评价函数为
G(v,ω)=σ[α×hd(v,ω)+β×dt(v,ω)+γ×vl(v,ω)]。
式中:hd(v,ω)是方位角评价函数,即机器人在当前设定速度下,轨迹末端朝向与目标点之间的角度差距;dt(v,ω)是机器人预测轨迹末端与地图上障碍物最近的距离;vl(v,ω)是当前机器人的线速度;σ、α、β、γ为系统调节参数。
2.2 改进DWA算法
针对DWA算法易陷入局部最优的问题,引入A-star算法规划的全局路径来指导DWA算法进行局部路径规划。改进了DWA评价函数,将传统障碍物评价函数拆分成静态障碍物评价函数sdist(v,ω)和动态障碍物评价函数ddist(v,ω)。设机器人预测轨迹t时刻的坐标为(xt,yt),静态障碍物的坐标为(xst,yst),动态障碍物的坐标为(xdy,ydy)。考虑规则的障碍物,并首次膨胀处理,首次膨胀后半径为robs,考虑机器人半径rrobot,将静态障碍物再次膨胀1.15×(robs+rrobot)倍后的半径为SR,将动态障碍物再次膨胀1.2×(robs+rrobot)倍后的半径为DR,则机器人与障碍物的计算为
ds=min(),
dd=min()。
式中:ds为机器人t时刻与静态障碍物最近的欧几里得距离;dd为机器人t时刻与动态障碍物最近的欧几里得距离。
障碍物评价函数为
sdist(v,ω)=
ddist(v,ω)=
式中:sdist(v,ω)为静态障碍物评价函数;ddist(v,ω)为动态障碍物评价函数。
为了使局部路径更贴合全局路径,增加了路径偏差评价函数Dis(v,ω)。如图1所示,首先计算出每段直线的直线方程,其次以直线段两个端点的x坐标划分区域,计算DWA预测轨迹末端到对应线段的距离。
将起始点、目标点和拐点按顺序排列,设其中一个点的坐标为(xj,yj),j>2,则前一个点的坐标为(xj-1,yj-1),DWA轨迹末端点的坐标设为(xaend,yaend),则在[xj-1,xj]间的直线方程为
a×x+b×y+c=0。
式中:a=yj-yj-1;b=xj-1-xj;c=xj×yj-1-xj-1×yj。
路径偏差Dis(v,ω)为
Dis(v,ω)= 。
修改后的DWA总评价函数G(v,ω)为
G(v,ω)=σ[α×hd(v,ω)+β×sdist(v,ω)+γ×ddist(v,ω)+μ×Dis(v,ω)+ψ×vl(v,ω)]。
3 算法融合
4 实验与分析
4.1 改进A-star算法仿真对比
表1 不同A-star方法对比Tab.1 Comparison of different A-star methods |
| 算法 | A-star算法改进方式 |
|---|---|
| 文献[14] | 1.启发函数动态加权; 2.综合水面舰艇运动特性,设计了一种路径节点转角角度削弱策略。 |
| 文献[15] | 1.启发函数动态加权; 2.通过贝塞尔曲线进行平滑处理。 |
| 本文A-star | 1.对启发函数引入梯度概念; 2.启发函数动态加权; 3.设计转弯代价计算函数。 |
仿真实验平台为MATLAB 2021a,笔记本处理器为Intel(R) Core(TM) i9-13900HX 2.20 GHz,RAM为16.0 GB。
图3 15 m×15 m地图算法仿真路径对比Fig.3 Comparison of algorithms simulated paths using 15 m×15 m map |
图4 30 m×30 m地图算法仿真路径对比Fig.4 Comparison of algorithms simulated paths using 30 m×30 m map |
图5 50 m×50 m地图算法仿真路径对比Fig.5 Comparison of algorithms simulated paths using 50 m×50 m map |
表2 15 m×15 m地图算法仿真实验数据对比Tab.2 Comparison of algorithm simulation experiment data for 15 m×15 m map |
表3 30 m×30 m地图算法仿真实验数据对比Tab.3 Comparison of algorithm simulation experiment data for 30 m×30 m map |
4.2 融合算法仿真对比
地图中,灰色方块为未知障碍物,箭头为动态障碍物移动方向。简单地图环境中,传统融合算法路径长度为20.815 m,用时32.938 s,局部路径不贴合全局路径,改进融合算法路径长度为19.982 9 m,用时30.364 s,并且较传统融合算法规划的路径更贴合全局路径,路径更加平滑,在躲避障碍物时规划的路径更加合理、安全。复杂地图环境中,传统融合算法规划的路径长度为79.386 m,用时464.657 s,并且局部路径偏离全局路径,会生成大拐弯的路径,生成的路径非最优,出现错误,如图7方框中所示;改进融合算法规划的路径长度为69.517 m,用时353.189 s,局部路径贴合全局路径,在躲避静态障碍物、动态障碍物和未知障碍物时性能更加卓越,相比于传统融合算法规划的路径更加符合机器人运动规律。
4.3 实景验证
5 结语
1)本文将子节点的梯度信息引入A-star算法的启发函数,根据环境信息设计了自适应动态函数,并在总代价函数中增加转弯代价函数,提升了算法在复杂环境中规划最优路径的性能。
2)本文对DWA算法增加了路径偏差评价函数,使得DWA算法规划的路径更加贴合全局路径,将传统DWA算法中的障碍物评价函数拆分成静态障碍物评价函数和动态障碍物评价函数,增强了DWA算法在复杂环境中的障碍物避障能力。
3)经仿真对比与实体机器人的实验,可以验证本文提出的融合路径规划算法能够在复杂环境中规划出更适合机器人移动的路径,实现更优的动态路径规划,并能够有效地躲避静态障碍物、动态障碍物和未知障碍物。
4)本文改进的A-star算法在时效上具有一定的局限性,运行速率有待优化提高,后续研究要将改进算法融入多智能编队控制中,提高算法的时效性等。