基于距离抑制向量的机器人动态路径规划方法

孙嘉明,刘卫朋,*,巩祥瑞,山圣旗

(1.河北工业大学 省部共建电工装备可靠性与智能化国家重点实验室,天津 300131;2.河北工业大学 电气工程学院,天津 300131)

0 引言

路径规划是实现移动机器人智能化的重要技术,一直以来也是机器人控制领域的一个重要研究热点[1],其核心思想就是使机器人依据一个或者多个评价指标,在所应用的路径规划环境中找到一条从起点到目标点无碰撞的最优或次优路径。目前常用的路径规划指标有最短路径距离、最短运行时间、最少运动耗能等。根据传感器所接收环境信息的不同,路径规划问题也可分为全局路径规划和局部路径规划两种。其中,全局路径规划侧重于整体路径上的最优性,局部路径规划则侧重于机器人在运行过程中的安全性。目前常用的全局路径规划算法有A*[2]、D*、RRT[3]算法等,常用的局部路径规划算法有人工势场法[4]、动态窗口法、蚁群算法[5]、遗传算法等。

动态窗口法(Dynamic Window Approach,DWA)是基于机器人运动学和动力学的一种局部路径规划算法,该算法将路径规划问题转化为在速度矢量空间中的约束优化问题,在速度二维空间中采样多组速度(线速度、角速度),并在一定时间间隔内,模拟移动机器人在这些速度下的轨迹,在获取多组可行轨迹后,根据所设计的评价函数,选取最优轨迹所对应的速度(线速度、角速度),驱动机器人完成局部避障行驶,是一种常用的局部路径规划算法。

文献[6]通过引入全局路径规划结果作为参考轨迹,提出了方向评价子函数、平滑速度评价子函数和加速度评价子函数,优化了移动机器人的路径,但只在较短路径中效果显著,在较长路径下仍然存在规划路径过长的问题。文献[7]提出了一种将传统A*算法与动态窗口法相融合的方法。针对移动障碍物碰撞问题,提出速度障碍区的概念,改进了动态窗口法,实现了动态和静态的实时避障,但该算法计算效率较低,规划路径时所用时间过长。文献[8]提出一种融合改进A*与动态窗口法的随机避障方法,在相邻的节点之间采用动态窗口法进行局部规划,实现了路径平滑和随机避障,但并没有考虑运行时出现动态障碍物时的情况。

由于传统动态窗口法在局部路径规划时缺乏中间点的引导,因此在机器人运行至“L”型、“C”型等障碍物距离相近的区域时会出现评价函数失灵的问题。如在穿过障碍物数量较多区域时会出现绕行的情况,增大了总路径长度。同时,原有的评价函数中并未考虑出现动态障碍物的情况。

针对以上缺陷,本文提出距离抑制向量的概念。当机器人运行路径上出现动态障碍物时,首先根据机器人的运动方向与障碍物的运行方向预测是否可能发生碰撞,然后计算距离抑制向量,得到新的评价子函数;在全局最优性方面,将传统A*算法与改进灰狼优化算法结合:首先使用传统A*算法计算节点代价值并将其作为灰狼优化算法的初值。其次,改进灰狼优化算法的更新步,在每次迭代时,考虑距离加权进行位置向量更新;提取全局路径中的转折点作为DWA算法的中间指引点,定义中间点评价子函数,最后得到新的评价函数,实现对传统DWA算法在局部和全局规划能力上的优化。

1 传统DWA算法的基本原理及改进

1.1 传统DWA算法基本原理

传统的动态窗口法通过对机器人运动时的线速度、角速度进行采样,根据机器人本身的限制以及环境的限制,可以将机器人的速度控制在一个确定的范围内,进而模拟出移动机器人在这些速度下的轨迹。在获取多组可行轨迹后,根据评价函数G(v,ω),对每条轨迹进行打分:

其中:h(v,ω)为方位角评价函数,用来评价机器人在当前采样的速度下,达到模拟轨迹终点时的朝向与目标点之间的夹角θ,θ越小,评分越高;d(v,ω)为距离评价函数,当所选路径与障碍物无碰撞时,该项设置为一个常数;s(v,ω)为速度评价函数,用来评价当前速度大小。

为了防止评价函数不连续,3个评价函数的结果在相加之前要进行归一化处理:

在对所有评价子函数进行归一化处理后,可以得到一条平滑的路径。

通过对传统DWA算法[9]的原理分析可知,在进行局部规划时,该算法存在以下问题:

1)在进行局部规划时,由于没有全局信息做指引,仅使用机器人在局部环境下的信息做参考,使得算法所计算出的全局路径容易陷入局部最优,进而导致路径规划失败。

2)当机器人运动至障碍物数量密集区域或多个碰撞距离相近的障碍物区域时,会选择绕开密集障碍物区,从而导致总路径过长,并且对于障碍物的状态并没有进行区分,对动态障碍物的躲避能力有限。

1.2 距离抑制向量的提出与评价函数的改进

为了改善机器人使用传统DWA算法时出现的两个问题,首先根据机器人在运动路径附近可能出现的障碍物类型将其分为3类:静态已知障碍物、静态未知障碍物及动态障碍物。

为了使机器人运动至相近障碍物时,能够顺利避开局部最优点,并且不出现绕行现象,提出一种距离抑制向量的概念,下面结合图1进行说明。

图1 动态障碍物环境下的运动示意图Fig.1 Schematic diagram of motion in dynamic obstacle

在机器人运动的路径附近出现静态未知或动态障碍物时,距离抑制向量PCOn由机器人此时的位置O’、预测发生碰撞的位置PZn、障碍物的速度Vn及机器人与障碍物之间的相对位置NXn决定,并满足以下关系:

其中,PCOn的方向与障碍物速度方向相同取正,表示该障碍物即将与机器人在预测的轨迹上发生碰撞;PCOn的方向与障碍物速度方向相反取负,表示障碍物远离预测的运动轨迹,即不会与机器人发生碰撞。若静态未知障碍物出现后无速度,如障碍物2,则距离抑制向量不存在。为了将距离抑制向量引入原动态窗口法中,定义距离抑制评价子函数

其值越大代表障碍物对机器人的运动轨迹影响越大,即发生碰撞的概率越高,从而可以得到基于距离抑制向量的改进型DWA算法新的评价函数:

式(5)通过引入距离抑制评价子函数,改善了原算法对于不同障碍物类型的区分能力,从而解决了机器人在使用传统DWA算法进行局部路径规划时无法规避动态障碍物的问题。

2 传统A*算法的基本原理及改进

2.1 融合改进灰狼与A*算法的全局路径规划算法

作为一种启发式算法,A*算法是在静态环境中用于求解最优路径的最有效的直接搜索算法之一,因此在室内机器人的路径规划领域中得到了广泛应用。

在进行路径规划探索时,A*算法主要依靠评价函数得到所有可能路径权重的代价值:

其中,G(n)为机器人运动起点到节点n的实际距离,H(n)为节点n到终点的估计距离,也称作启发函数。

大量的实验证明,A*算法在小环境中搜索效率高,并且能够搜索到最优路径,但是,由于其本身计算方法的局限性,导致理论上搜索出的路径转折角度太大,不利于将其直接应用在实际工作中。

2.2 灰狼优化算法

灰狼优化算法是由Mirjalili等人[10]在2014年提出的一种新型智能群体算法,该算法受灰狼的捕食行为启发而来,依据灰狼内部的等级制度及狩猎原理寻找最优值。狼群内部按阶级高低可分为α狼、β狼、γ狼、ω狼四级。

α狼为狼群中的头狼。β狼作为整个狼群的第二阶级,服从于α狼,协助其进行决策,也可支配其他阶级的狼。γ狼与ω狼的作用与β狼类似,α狼、β狼、γ狼可以分别看为待解决问题的最优解、次优解及第3优解。

在寻优过程中,该算法首先确定当前种群中的前三个最优解,然后依据下式进行目标搜索:

初次搜索后,使得狼群内其他成员向最优解处(猎物处)移动:

此外,在每次搜索后,群体内各成员依据下式更新自己的位置向量:

在式(7)～(9)中,t代表当前进行的迭代次数,X代表当前灰狼的位置向量,D代表候选狼与最优三头狼之间的距离向量,A为位置调节系数,C为距离调节向量。

2.3 改进灰狼优化算法与A*算法的结合

为了进一步增强灰狼优化算法的全局搜索能力,使其在进行规划时对终止点的敏感度进一步提升。在算法每次迭代结束后,对成员进行位置向量更新之前,根据成员与最优狼之间的距离,对位置向量X进行加权修正。定义非线性距离权重系数

其中,di为成员与最优狼之间的距离大小。在每次迭代后,对非线性更新权重进行位置更新:

最后,对最优距离加权得到最优位置向量:

作为一种非线性优化算法,灰狼优化算法的初值选取对于算法收敛性有很大影响。因此,考虑将传统A*算法中的F(n)值作为灰狼优化算法的初值,即将路径起始点周围可行栅格的F(n)值自大到小排列,并将其值前三位所在栅格初始化为狼群的α狼、β狼、γ狼,其余栅格作为ω狼进行迭代,这样不仅保证了算法的收敛性,同时在原理上对传统A*算法的搜索方式也进行了改善,从而得到了一种新的全局路径规划算法(GMA*),其计算流程如图2所示。

图2 GMA*算法计算流程Fig.2 The calculation process of the GMA*algorithm

2.4 基于距离抑制向量的动态路径规划

针对传统动态窗口法缺乏全局信息指引的问题,将改进后的全局规划算法(GMA*)所规划出的路径转折点作为动态窗口法的中间指引点,在式(1)的基础上,引入中间点评价子函数与距离抑制评价子函数,从而得到了基于距离抑制向量的动态路径规划算法评价函数:

其中,m(v,ω)为中间点评价子函数,表示动态速度模拟轨迹上的点与全局路径中间点之间距离。最终得到了基于距离抑制向量的动态路径规划算法,其计算流程如下:

1)初始化地图参数,初始化GMA*算法初始参数。

2)建立环境地图。

3)运行GMA*算法,得到全局最优路径。

4)提取路径中间点,存储进中间点评价函数中。

5)初始化改进型DWA算法参数。

6)确定速度窗口、形成运动轨迹。

7)评价函数归一化,判断是否到达目的地。

8)如到达目的地,则输出最优路径,否则返回第6步,循环搜索直到到达终点。

图3为该算法的整体流程图。

图3 基于距离抑制向量的动态路径规划算法计算流程Fig.3 The calculation process of dynamic path planning algorithm based on distance suppression vector

3 仿真验证

为了验证改进后的新算法的性能,在MATLAB 2018a环境中建立栅格地图模型进行仿真验证。

3.1 融合A*与改进灰狼算法仿真实验

由于机器人在实际运行中所在的环境较为复杂。为了简化计算,保证仿真的实时性和计算的精确性,本文通过建立栅格地图的方式来模拟实际环境。其中,障碍物为栅格地图中的黑色方格,位置随机生成,地图复杂度以黑色方格数量占总栅格数量的百分比及地图边长决定。栅格地图大小设置为50m×50 m,障碍物占总栅格数量的百分比设置为20%,为了保证算法规划出路径的一般性,选择起点和终点尽量位于地图的对角线位置。设置起始点为[0,5],目标点为[27,20]。为了保证算法可靠收敛,最大迭代次数设置为500,仿真结果如图4所示。

图4 模拟复杂环境下融合算法(GMA*)与传统A*算法仿真结果Fig.4 Simulation results of the fusion algorithm(GMA*)and the traditional A*algorithm in a simulated complex environment

图5为传统A*算法与本文算法在7组不同地图复杂度环境下的仿真数据比较。

可以发现,在环境地图不变的条件下,融合算法(GMA*)在进行全局路径规划时,在路径长度以及转折角度的减少方面,相比于传统A*算法均有较大提升,从而证明了其理论上的优越性。

图5 改进前后两种算法的性能对比Fig.5 Performance comparison of the two algorithms before and after the improvement

3.2 改进型DWA算法仿真实验

在仿真之前首先设置算法所需的基本参数,包括机器人的机械特性参数、DWA算法的评价函数参数以及障碍物参数。为了使仿真过程中产生的误差在可接受的阈值内,机器人的机械特性参数设置为实验用机器人Turtlebot3-Burger的机械特性参数,如表1所示。

表1 机器人机械特性参数Tab.1 Robot mechanical characteristic parameters

由于机器人通过激光雷达获取外界地图,并采用基于粒子滤波的GMapping-SLAM算法建立环境地图,故最大线加速度与角加速度不宜设置过大,以免出现环境栅格地图在转角处失真的现象。

改进后的DWA算法引入距离抑制评价子函数及中间点评价子函数,为了使距离抑制向量以及中间点在局部规划和全局规划时发挥更明显的作用,考虑将以上两个子函数的权重设置为0.50。DWA算法基本参数设置如表2所示。

表2 DWA算法评价函数参数Tab.2 DWA algorithm evaluation function parameters

考虑到机器人自身尺寸与电机最大减速度,为了保证在躲避障碍物时的安全性,将障碍物尺寸参数设置为0.50 m,最小碰撞阈值设置为0.50 m,障碍物位置横坐标X、纵坐标Y设置范围不超过地图边长,即X、Y的范围均为0～50 m。

3.2.1 静态环境规划仿真

首先进行静态环境下的算法测试。在仿真环境中首先进行环境地图参数的设置,并初始化DWA算法所需参数。

图6为传统DWA算法所规划出的路径,可以发现,在进入虚线所示区域内时,因为机器人与四周障碍物的距离相差不大,出现了评价函数失灵的现象。

图6 传统DWA算法所规划出的路径Fig.6 The path planned by the traditional DWA algorithm

此外,在全局路径的选择上,当使用传统DWA算法进行路径规划时,由于算法并没有考虑全局环境信息,只对局部环境信息进行了提取,因此机器人选择以绕行的方式避开障碍物密度大的区域,导致机器人整体路径过长。

在相同障碍物栅格环境下,使用改进A*算法(GMA*)进行全局规划,并提取路径中的转折点作为中间指引点,得到的全局规划路径如图7所示。

图7 GMA*算法所规划出的路径及中间指引点Fig.7 The path planned by the GMA*algorithm and the intermediate guide point

图8为使用改进后DWA算法所规划出的路径。可以明显发现,在机器人运行至全局规划路径的中间点时,在中间点评价子函数的作用下,机器人的路径长度得到了大幅度减小,也没有出现绕行障碍物区域的现象,因此改进后的算法实现了对局部评价函数失灵问题和绕行稠密障碍物区域两个主要问题的优化。

图8 改进型DWA算法规划出的路径Fig.8 The path planned by the improved DWA algorithm

3.2.2 动态障碍物规划仿真

为了测试机器人在运行中出现动态障碍物时的算法性能,在仿真环境中分别加入不同类型的障碍物进行了仿真分析,如图9所示。

图9 出现动态障碍物时的仿真结果Fig.9 Simulation result with dynamic obstacles

在全局规划路径上加入临时出现的静态未知障碍物,并在机器人运行过程中,设置一个运动轨迹与机器人路径存在交点的动态障碍物。如图9(a)所示,当机器人运行至动态障碍物附近时,对于动态障碍物可以有效地进行躲避。

由图9(b)中可以看出,基于距离抑制向量的动态路径规划算法不仅实现了对全局路径中中间点的提取,得到了一条光滑的全局路径,而且在机器人路径上出现动态与静态位置障碍物时,也能够顺利进行规避,从而提高了运行过程中的安全性。

4 实验验证

为了进一步验证改进后的优化算法在机器人实际运行中的性能,本文选择使用Willow Garage公司出品的Turtlebot3-Burger机器人平台进行实验[11],实验场地为5 m×8 m的实验室。通过摆放纸箱及挡板模拟环境中的障碍物。在实验开始前首先通过Wi-Fi建立树莓派与上位笔记本电脑之间的通信,然后使用基本SLAM功能包GMapping-SLAM建立室内环境的栅格地图[12]。

4.1 传统A*算法与改进A*算法(GMA*)实验对比

在获得环境地图的先验信息后,通过RViz中的2D Pose Estimate按钮进行动态实时地图与静态地图的矫正,使小车雷达所扫描到的实时点云数据与栅格地图重合。首先使用传统A*算法[13]进行路径规划实验,通过点击2D Nav Goal[14]按钮设定路径终点,所规划的路线如图10(a)所示;同理设置相同的起点和终点,使用改进后的算法(GMA*)进行实验,规划出的路线如图10(b)所示。

图10 改进前后算法实验路径对比Fig.10 Comparison of algorithm test paths before and after the improvement

从图10(a)中可以看出,传统A*算法在障碍物密集的环境中进行路径规划时,规划出的路径长度较长,并且路径转折角度不平滑,在进入密集障碍物区域时,出现了绕行的现象。

图10(b)为本文提出的改进后的全局路径规划算法GMA*所规划的路径,相比原A*算法能够有效地缩短路径长度,当机器人驶入障碍物密集区域时,也能够保持全局路径最短的性能指标,路径转折角度也得到了减小。

4.2 基于距离抑制向量的动态路径规划算法实验验证

为了验证改进后的DWA算法性能,将实验场景分别设置为室内实验室及长距离走廊,动态未知障碍物由另一台上位机控制的工业机器人底盘代替,静态未知障碍物由临时增加的挡板代替。实验结果如图11所示。

图11 动态路径规划实验环境及规划路径Fig.11 Dynamic path planning experimental environment and planned path

由图11可知,Turtlebot3-Burger在使用本文提出的基于距离抑制向量的动态路径规划算法进行路径规划时,不仅能在地图全局信息的基础之上完成全局路径规划,并且在机器人运行过程中也能够躲避未知的静态与动态障碍物。

以上实验结果表明,本文提出的基于距离抑制向量的机器人动态路径规划算法能够在复杂环境中更加高效完成动态避障与路径规划。考虑到实验用机器人本体质量较轻,可能会出现轮胎打滑、转弯不稳定导致定位与环境地图不重合等问题,在每次实验后可通过对机器人舵机进行校正减小实验误差。

5 结论

针对传统动态窗口法进行局部路径规划时出现评价函数失灵及绕行障碍物的缺点,本文提出了一种基于距离抑制向量的改进型DWA路径规划算法。通过引入距离抑制向量及全局中间点,使得机器人在运行过程中能够对未知的动态障碍物完成躲避,从而提升了机器人在运行时的可靠性;针对传统A*算法在路径长度和转折角度上存在的问题,将其与改进后的灰狼优化算法相结合,利用距离权重系数改进更新步,大幅度减少了机器人在复杂环境中进行路径规划时的路径长度及运行时产生的转折角度,最后将两种改进算法结合,得到了基于距离抑制向量的动态路径规划算法。仿真及实验表明,本文算法在实际应用中表现出较好的路径规划性能。