求解无人驾驶车辆路径规划问题的QPC-MT-RRT算法

2023-07-05苏莹莹谢冬冰

沈阳大学学报(自然科学版) 2023年3期

苏莹莹, 谢冬冰

(沈阳大学机械工程学院, 辽宁沈阳 110044)

近些年来,随着社会日新月异的发展,道路上汽车的数量急速增加,车辆造成的诸多社会问题也随之而来,且不容小觑,例如交通事故的频发、车辆尾气造成的污染及由于车辆过多造成的交通拥堵等[1]。道路交通安全是许多专家学者一直研究的课题,因为它与人们的生命财产安全息息相关。近年来出现的无人驾驶送货车提高了产品配送的效率,降低了企业的生产成本,真正为人民提供了便利[2]。因此针对无人驾驶车辆相关问题的研究极其重要,它作为一项新兴的科学技术,从开始提出就受到了社会的广泛关注。与需要人类驾驶的传统交通工具完全不同,无人驾驶车辆利用了各种主动安全与驾驶辅助系统,设计的理念是只通过车辆的网络地图信息和车载传感器对周围环境进行自动感知,获取从起点到目的地的路线和车辆自身周围的交通环境,包含车辆的位置信息及障碍物的位置信息。通过对这些信息的采集判断车辆的行驶路线,并在车辆行驶过程中对车辆的纵向速度和前轮转向角度进行控制,使车辆能顺利实现避障,最终安全地到达目标点。

对无人驾驶车辆路径规划算法的研究,能更好地杜绝因疲劳驾驶、酒后驾驶、不规范操作等引发的交通事故,对保障人们生命安全、解决道路拥堵及极端环境救援等多方面问题具有重大意义。未来,对无人驾驶车辆的研究将越来越成熟,也将推动汽车领域的飞速发展,推动社会朝着更智能、更安全、更快速的方向发展。

目前,对路径规划算法的研究较为成熟,主要包括A*算法[3]、遗传算法[4]、蚁群算法[5]、人工势场算法[6-7]及快速搜索随机树(RRT)算法等,并据此衍生出了许多改进算法。作为解决无人驾驶车辆路径规划问题的高效算法,对RRT算法的研究较为广泛。Suh等[8]结合RRT*与交叉熵相算法,提出一种效率更高的路径规划算法,该种方法生成了2棵随机树:第1棵树为标准随机树,用它来确定随机树中最近节点以及被扩展的树中节点;第2棵树则包括了第1棵树及扩展的长度,并通过保持2棵树的独立,可以在确保RRT*达到渐近最优性的同时,非单调地增长1棵随机树,来提高规划算法的效率;Taid等[9]提出了添加梯度启发式算法形成智能双向RRT*算法,确保在复杂路况下也可进行路径规划; Cao等[10]将目标重力(target gravity)遗传算法用于机器人手臂的路径规划;Jeong等[11]提出了Quick-RRT*算法,为树中的每个节点添加权重,在添加新节点时,根据权重和基于三角不等式的代价函数对父节点进行重新计算,使得到的初始路径具有更高的质量以及更快的收敛速度;Mashayekhi等[12]提出了一种混合RRT算法,通过对2棵树进行双向的搜索得到初始解,然后将2棵树合并成1棵树,最后进行初始解的优化,使算法的单向搜索效率得到提高;冯来春等[13]利用A*算法引导路径的生成,随后利用RRT算法进行路径的扩展,得到了GA-RRT(guiding-area RRT based on A*)算法;宋晓琳等[14]以快速随机扩展树算法为基础算法,对智能车辆展开路径规划的研究;陈秋莲等[15]研究认为RRT算法能在复杂的环境下进行搜索,应用范围广,搜索能力强,搜索时间短,同时也指出RRT算法的节点利用率比较低,得到的路径不稳定等问题;何佳等[16]根据RRT算法步长选取、搜索随机树扩展方向以及节点选择等,提出了改进的路径规划算法;袁师召等[17]研究发现,路径规划算法中无论是局部路径规划还是全局路径规划都各有优劣;宋林忆等[18]对于简单的RRT*算法进行约束,对其步长的极值进行限制,使算法搜索效率更高,路径优越性更强。

目前单一的路径规划已经无法满足无人驾驶产品的需求,多种规划算法相结合的方法已成未来的研究重点。

1 问题描述及背景知识

1.1 问题描述

在无人驾驶车辆路径规划过程中常用位姿空间对车辆进行位置集合描述,而位姿空间是指无人驾驶车辆在运动行驶过程中的所有位置点位姿的集合。在无人驾驶车辆运动学分析过程中,整个地图区域空间为O,划分位姿空间S分别为障碍物所处空间So和无障碍物即自由空间Sf。

无人驾驶车辆的行驶在位姿空间内还应满足车辆运动模型的需求。图1为车辆运动学模型,针对路径规划问题在图上对车辆运动学模型分析相关参数定义如下:P为车辆前轴轴心点;Q为车辆后轴轴心点;σ为车辆行驶过程中的横摆角;θf为车辆前轮转角;M为车辆轴距;vb为后轴中心速度;vf为前轴中心速度。

图1 车辆运动学模型Fig.1 Vehicle kinematics models

车辆前、后轴中心点P(Xf,Yf)、Q(Xb,Yb)处速度分别为:

前、后轴运动学约束分别为:

将式(2)、式(4)联立可得:

在前、后轴处利用三角函数几何关系可知:

由式(7)、式(8)解得的前、后轴横纵长度关系及式(5)、式(6)中的速度关系,通过三角函数可解得车辆横摆角为

(9)

车辆转向半径R与车辆轴距M可利用勾股定理进行转换,求得转向半径为

(10)

前轮偏角与车轮转向半径R与车轮转向半径M通过勾股定理可解得,前轮偏角为

(11)

在路径规划中以[vb,β]作为汽车的控制,通过限制速度的大小以及车辆偏转角度的大小来实现车辆在合理速度范围及合理角度转向范围内的安全行驶。因此,将式(8)～式(11)联立可解得[vb,β]为:

1.2 RRT算法

无人驾驶应用中的路径规划通常需要对多个变量进行约束,精确路径规划的复杂性与变量的数量呈指数关系,因此规划时通常要考虑维数关系。由于现有的传统算法无法处理复杂高维空间中的路径规划问题,学者们更倾向于采用在高维空间中具有良好性能的基本采样算法。

RRT算法属于一种基于随机采样的路径规划搜索算法。算法在初始时随机树只包含路径起点,并把其作为随机树的根节点,通过在未扩展的状态空间内进行随机采样不断获取新的子节点,随后连接随机树中各个节点,从而得到1棵无目的性、且不断向外扩展的随机树,此时当随机树状态满足了给定的限制条件,或者随机树搜索产生的新节点已经达到目标状态点时,就可以结束随机树的搜索过程,同时停止对新节点的不断扩展,然后通过每个子节点从目标状态点向初始根节点反向遍历该路径,就可找到从初始根节点到目标点的规划路径。

如图2所示,RRT算法以初始点作为快速扩展随机树的引导根节点,以该根节点扩展生成随机树实现快速搜索,在无障碍的空间内进行随机选取来获取随机采样点,以一定步长进行搜索,从而生成一个新节点,若在随机采样点与新节点的搜索过程中无障碍区间或不与之发生碰撞,则就把此新节点放入随机树。最后若子节点已经属于目标节点中的一个,从目标节点向初始点逆向寻路,得到一条由初始点到目标点的可行路径。但如果在搜索过程中没实现避障,与障碍物发生了碰撞,那么实验失败。为了避免RRT算法在搜索过程中由于失误而无限期地进行搜索,则需要提前设定算法的迭代上限次数,若过程中搜索次数已达上限,但仍未得到规划路径,那么此次搜索过程失败。

图2 RRT算法示意Fig.2 RRT algorithm schematic diagram

若要构建RRT算法,先要对环境信息、起终点位置、节点的生成次数以及随机点与最近树节点的距离进行输入;最后得到的主要有随机树的节点、起点与终点间的原始路径、生成的连接起终点的随机树以及平滑处理后的优化路径。因此,RRT算法作为1种单向搜索算法,在地图内从初始点Xinitial开始初始化后进行迭代,其迭代过程如下:

1) 将起始点进行初始化,并将其作为随机树的根节点加入随机树中;

2) 将随机树开始for循环,从次数1开始,遍历极限次数为N,则随机树最多遍历N次;

3) 如果随机树遍历到N次后目标节点仍没有在随机树中,那么此次搜索失败;

4) 利用RANDOM_NODE()函数寻找出一个新节点Xrand,随后检验其是否会与障碍物发生碰撞;

5) 对路径进行碰撞检测,如果路径不与障碍物碰撞则继续下一步,如果发生碰撞则继续寻找新节点;

6) 利用NEAREST_TREE(Xrand,T)函数,找到随机树中离已经找到的新节点距离最近的节点作为下一个节点;

7) 通过if语句对最近节点与新节点间的路线进行是否有碰撞行为的判断;

8) 若存在碰撞则开始下一个循环,若不存在碰撞则开始下一步;

9) 通过NEW_NODE(Xnear,Xrand,S)函数寻找新节点并添加到随机树的节点中;

10) 当随机树中的目标点包含于子节点,连接该子节点与目标节点,从目标节点开始进行逆向搜索,即可在随机树中生成一条从初始点到目标点的规划路径。

RRT算法迭代过程的伪代码如下:

{Tree(Xinitial,Xgoal,S)};

v←Xinitial;

Tree.init(Xinitial);

forn=1 toN;

Xrand←RANDOM_NODE();

Xrand←NEAREST_TREE(Xrand,Tree);

collision(Xrand,Xnear)=true

continue;

Xnew←NEW_NODE(Xnear,Xrand,S);

W←T.add(Xnew);

E←T.add(Xnew,Xnear);

if T.contains(Xgoal)=true

break;}

2 基本RRT算法的改进

2.1 MT-RRT算法

本文在基本RRT算法的基础上,通过计算寻求多个引导根节点,形成多棵随机树,提出从多个方向开始搜索的1种双向快速搜索随机树算法,即MT-RRT路径规划算法。RRT算法开始之前无法确定根节点的数量,MT-RRT路径规划算法的主要改进点是根据所在地图中的位置信息,通过求引导根节点的计算公式来获取一定数量、且不产生碰撞的引导根节点。将引导根节点与起始节点和目标节点一起作为进行扩展的根节点,分别从这3点开始展开扩展形成多棵随机树。该算法有效地引导了树的生长方向,减少了节点的数量,提高搜索速度,也提高了路径的可行性。

如图3所示,MT-RRT算法的原理是在确定范围的路径规划地图中,设定好车辆初始节点、目标节点、扩展给定的步长、障碍物的分布情况;判断车辆与障碍物是否会发生碰撞,同时判断起始点Xinitial到目标点Xgoal的距离是否小于设定的步长;如果初始点到目标点的距离小于步长,且不会与周围的障碍物发生碰撞,则将起始节点和目标节点连接起来,实现完整路径规划;反之,则要从起始节点Xinitial向着目标节点Xgoal寻求一定数量的不与障碍物产生碰撞的引导根节点Xroot。

图3 MT-RRT算法示意Fig.3 MT-RRT algorithm schematic diagram

也就是说,在给定的地图上,MT-RRT算法将初始点和目标点均作为根节点,以2个节点方向开始同时进行搜索,这样就形成2棵树。算法以一定的步长扩展,同时在扩展过程中通过公式计算下1个引导根节点,最后等到2棵树在扩展中相遇时将2棵树合并。这样就得到1棵完整随机树,那么这棵树中所有的可行节点则会形成1条从初始点到目标点的完整路径。将起始点作为根节点开始扩展随机树,与此同时,以目标点为根节点的随机树也开始进行构建。2棵随机树分别生成新的引导根节点,此时要检查新产生的节点距离另1棵随机树是不是小于给定的值。若2棵随机树节点间的距离小于给定值,即将2棵树合并为1棵随机树,从而得到1条路径。为了避免RRT算法在搜索过程中由于失误而无限期地进行搜索,则提前设定算法的迭代上限次数,若过程中搜索次数已达上限但仍未得到规划路径,那么此次搜索过程失败。迭代过程如下:

1) 建立地图,确定初始点、目标点及障碍物位置分布。

2) 用if语句对车辆是否会与周围障碍物碰撞以及初始点到目标点的距离是否小于步长进行判断。若距离小于步长或会发生碰撞,则将2节点直接连接作为寻得的路径。

3) 利用GETROOTS(Xinitial,Xgoal,MAP)函数计算引导根节点。

4) 将起始点、目标点以及求得的引导根节点分别作为随机树的根节点,开始搜索。

5) 开始循环过程,并遍历每棵随机树。

6) 通过RANDOM_NODE()函数寻求新的随机节点。

7) 通过MS-EXTEND(Tnum,Xrand,S)函数继续扩展所得随机树。

8) 遍历所有随机树,找到与新随机树距离最近的随机树,并找到2棵树中距离最近的根节点。随后生成1个新的随机点,设置1个固定值,使新产生的节点朝向最近的树,并向具有目标偏向性的方向进行扩展,对新节点进行碰撞检测,如路径与障碍物无碰撞,则将新节点加入到树中。

9) 判断是否存在碰撞以及新节点距离最近树的根节点是否小于步长,若新节点距离最近树的根节点小于步长,且路径不与障碍物碰撞,那么将2棵树合并为1棵随机树。在随机树中添加最近树中的所有生成出的节点,随后淘汰最近树,结束此循环。

10) 在while循环里,计算树的列表Tree List中树的总数是否为1,如果为1,则结束循环,并寻得1条从起始点到目标点的最优路径;如果不为1,则继续进行下一次迭代,返回步骤7。

11) 判断整个循环内最后得到的随机数树总数是不是为1,若数量为1,将树中所有节点相连,得到搜索出的路径。若数量大于1,重复循环,直到为1。

MT-RRT算法迭代过程的伪代码如下:

{Map←create Map;

Xinitial.init();Xgoal.init();

if|Xinitial-Xgoal|≤step &collistonTnum

WO(Xinitial,Xgoal)=0;

path←[Xinitial,Xgoal];

else

ROOTS←GETROOTS(Xinitial,Xgoal)

T1,init(Xinitial);TreeList.push(T1);

fori=2 toK

T1.init(ROOTS(i));TreeList.push(T1);

end

Tk+1.init(Xgoal);

repeat

for num=1 to TreeList.size();

Xrand←RANDOM_NODE();

if MS_EXTEND(T1,Xrand)=1 then;

[Tnear,Tnum]←NEAREST_TREE(TreeList,Xnew);

if(LINKEN(Ti,Xnew)=1) then

TreeList←TREE_MERGE(T,Tnear);

TreeList.remove(Tnear);

end

until TreeList,size()=1;

return failure;}

相对于传统RRT算法,MT-RRT算法调用 MS-EXTEND(Tnum,Xrand,S)进行双向搜索,搜索效率明显提高,降低了快速扩展随机树算法的随机性。由于MT-RRT算法中的2棵随机树需要分别生成自己的新节点,并且它们在自由空间内都会向对方节点方向进行快速搜索,因此MT-RRT算法在搜索过程中速度得以加快,搜索时间也明显减少,有效避免了与障碍物会发生的碰撞的可能。在进一步提高了无人驾驶车辆路径规划效率的同时,也在面对障碍物分布密集、通过较为狭窄的通道等复杂的环境时可以起到优质的实验效果。

2.2 QPC-MT-RRT算法

由于MT-RRT算法规划出的路径均是由一系列离散节点构成的连续线段,所得路径的形状角度变化较大、形态各不相同,使路径不平滑,安全性和平稳性较低。为进一步提高RRT算法的搜索效率和规划得出的路径的最优性,本文采用5次多项式曲线与MT-RRT算法的融合算法求解无人驾驶车辆多目标路径规划问题,减少原路径中不必要的拐点,减少车辆行驶中的角度变化,增加路径平滑性,增强无人驾驶车辆行车的安全性。设5次多项式的函数表达式为:

曲线的切线角r为

y′=tan(r)。

(16)

设曲线的曲率为k,则曲线曲率表达式为

(17)

已知起始点Xinitial及目标点Xgoal坐标为

(Xinitial,Yinitial);(Xgoal,Ygoal)。

(18)

起始点和目标点的方向角为

(Xinitial,tan(rinitial));(Xgoal,tan(rgoal))。

(19)

起始点曲率为

(Xinitial,kinitial(1+y2))=(Xinitial,kinitial(1+tan(rinitial)2))。

(20)

目标点曲率为

(21)

3 实验仿真与结果分析

本文改进基础RRT算法试验仿真平台是MATLAB,试验在500 mm×500 mm环境空间范围进行,黑色几何物体为障碍物。其中,起始点Xinitial坐标为(0,500),目标点Xgoal坐标为(450,0),算法每次搜索步长设置为30, 最大循环次数为10 000。为分析本文提出的算法的性能, 将RRT算法、MT-RRT算法、QPC-MT-RRT算法在地图上进行仿真分析。利用路径搜索时间、节点个数、节点利用率、所规划出的路径长度以及搜索成功率等参数对3种算法进行对比,其中取前4个参数在100次仿真实验中结果的均值。图4为RRT算法、MT-RRT算法、QPC-MT-RRT算法分别在仿真地图上的路径规划结果,最左上方较大圆形节点为起始点Xinitial,最右下方较大圆形节点为目标点Xgoal,所有较大圆形节点连接线为所得路径。由图可以看出:基础RRT算法在仿真地图中节点分布随机,搜索路径离散,所得路径长度最长;MT-RRT算法通过随机树的双向搜索,分别从起始点和目标点开始向地图中进行搜索,使所得路径随机性降低,节点能够均匀分布,节点利用率也更高。对比来看,QPC-MT-RRT算法规划出的路径减少了多余拐点,使所得路径平滑度更高,也使车辆在行驶过程中能更加符合车辆运动模型对车辆行驶的最大转向角的要求。

(a) RRT算法(b) MT-RRT算法(c) QPC-MT-RRT算法

RRT算法、MT-RRT算法、QPC-MT-RRT算法在仿真地图上路径规划结果的各项参数如表1所示。仿真实验结果表明:采用QPC-MT-RRT算法在实现路径平滑处理的同时,能保证MT-RRT算法的可行性、高效性及稳定性,还能加快收敛速度,大大减少搜索时间,缩短路径的长度,提高了路径的平滑度和优越性;采用QPC-MT-RRT算法进行平滑处理后的路径能够更好地适应无人驾驶车辆的运动模式,满足车辆运动学和转向角度的约束,使车辆能更平滑、更安全地行驶。