基于改进RRT的自动驾驶车辆路径规划算法研究

张 涌,高 峰,赵奉奎

(南京林业大学 汽车与交通工程学院,江苏 南京 210037)

0 引 言

路径规划问题一直是自动驾驶车辆整体规划与控制系统的重要组成部分。路径规划是指在已知环境中车辆的起始位置、结束位置以及障碍物的分布情况下,规划出一条不与障碍物发生碰撞的路径,提 供给汽车控制器,以精确控制车辆沿规划路径的行驶[1]。

近年来,研究人员对路径规划算法进行了大量的探索,路径规划算法不断发展。其中基于采样搜索的路径规划算法包括概率图算法(PRM)和快速探索随机树(RRT)算法[2-3]。RRT算法因其适合于求解动态多障碍条件下的路径规划问题而得到了广泛的应用,但RRT算法存在收敛速度慢、搜索效率低、规划的路径难以最优的缺点[4],同时RRT算法在路径规划过程中未考虑车辆运动动学约束,致使生成的路径曲折不连续,可行度较低[5],为此,国内外研究人员提出不同的方法,以用于改进RRT的局限性。为了加快路径收敛,提高计算速度,J.G.KANG等[6]提出了“后三角重新布线”方法,使规划时间的牺牲最小化,并克服了快速探索随机树RRT算法的最优性限制;Z.WU等[7]提出了一种Fast-RRT算法,该算法由改进RRT和快速优化两个模块组成,前者的目标是快速稳定地找到一条初始路径,后者的目标是通过合并多条初始路径得到一条最优路径,以达到更快地找到最优路径;A.H.QURESHI等[8]提出了基于势函数的RRT*,在RRT*中加入了人工势场算法,该算法大大减少迭代次数,从而导致更有效的内存利用和加速收敛速度;Y.LI等[9]提出了一种新的算法PQ-RRT*,它结合了P-RRT*(基于RRT*的潜在函数)和Quick-RRT*的优点,保证了快速收敛到最优解,并产生更好的初始解;Y.GAN等[10]提出了一种1-0 Bg-RRT算法,使用1-0的Bg-RRT构造树的速度更快,且能够及时跳出局部最小值大大减少了计算时间和复杂度。这些方法虽然都加快了迭代速度,但未考虑车辆运动学约束问题,规划出的路径不适用于车辆的实际行驶。

考虑到车辆运动学约束的问题,朱冰等[11]提出了具备安全场引导和角度约束等策略的改进RRT*算法,满足了车辆轨迹曲率约束,同时具有较快的搜索速度和更高的成功率;罗辉等[12]提出一种二阶段RRT算法TSRRT(Two-Stage RRT)采用融合最大转向角度的3次Bezier曲线进行上边界曲率优化,使规划路径能够满足车辆运动的转向角度,让车辆在行驶过程中能够以不停车的方式进行连续平稳转向;同时为了加快算法的收敛速度。针对路径搜索效率及车辆运动学约束问题,笔者基于城市道路场景,提出了一种基于势力场优化采样区域的改进RRT算法,该方法基于势力场对采样区域进行动态优化,采取基于安全距离和270°碰撞检测的动态步长选择策略,并在此基础上结合贪心思想及曲率约束对路径进行后处理;通过仿真实验,验证了算法的有效性和适应性。

1 车辆运动学模型

运动学是从几何学的角度研究物体的运动规律,在车辆路径规划过程中应用运动学模型,可以使规划出的路径更切合实际,满足行驶过程中的运动学几何约束。

使用前轮转向的车辆运动学模型,以车道行驶方向为Y轴正方向,建立坐标系,如图1,车辆的状态可以用Xn=(x,y,θ)表示。(x,y)表示车辆在当前在坐标系中的坐标位置,θ为车辆纵轴轴线与坐标轴之间的夹角,φ为车辆前轮转角,d为轴距,v为车辆行驶速度。

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

当车辆转弯的时候,将方向盘转到极限位置,并使车辆保持比较稳定的速度,转向过程中,转向中心点到外侧车轮所行驶的轨迹之间的最小距离,被称为最小转弯半径。不同类型车辆的转弯半径各不相同,最小转弯半径Rmin以及最大曲率ρmax可以表示为:

(1)

(2)

在车辆以车速v运动过程中车辆运动学模型可以表示为:

(3)

根据运动学模型及最大小转弯半径的分析可知,所规划的路径的曲率应该满足最小转弯半径的限制以及车辆的运动学约束,所以可以根据转弯曲率和安全距离来对车辆的路径规划进行约束以达到自动驾驶车辆实际的行驶路径要求。

2 改进的RRT路径规划算法

2.1 RRT及Bg-RRT算法

RRT算法以车辆的起点Xinit作为随机树T的初始根节点,利用随机抽样的原则在地图内随机选取一个节点Xrand,根据设定步长step,沿着可取的随机节点的方向选择新的节点Xnew,并进行碰撞检测,若碰撞检测通过则将新节点加入随机树,依次重复上述过程,直到找到要搜索的目标点。其伪代码如图2。

RRT算法在地图中随机选取节点,扩大了搜索空间,然而RRT的随机采样也会导致路径规划的盲目性,不仅会造成节点的冗余,还会增加搜索时间,特别是在结构化道路的场景下,随机性会使路径搜索点均匀分布在整个区域中,解的收敛时间增长。为了提高RRT算法的效率,研究人员提出了Bg-RRT算法,将图2的RRT算法流程中RANDOM_STATE()函数改写为如图3中形式,即给定一个概率因子,使在一定概率下采样点为目标点,以提高采样的效率,加快迭代速度。

图3 Bg-RRT算法Fig. 3 Bg-RRT algorithm

2.2 改进的RRT算法流程

通过对RRT算法以及基于目标的Bg-RRT算法的分析,提出了一种在城市工况下基于势力场优化采样区域的改进RRT算法,首先基于环境的对采样区域进行动态优化,再基于势力场对采样区域调整,缩小样本空间加快了迭代速度,增加搜索精度;同时基于270°碰撞检测的动态步长选择策略,减少采样节点数,增加了路径的安全系数,最后进行后处理并生成最终路径,保证路径的平滑度及满足车辆运动学约束。

2.3 基于势力场的采样区域动态优化

2.3.1 基于环境的采样区域动态优化

车辆通过自身多种传感器获取道路信息及障碍信息并生成地图,基于现实的车道环境所生成的全局信息已知的地图场景如图5,根据实际车辆行驶情况做出如下假设:①仅考虑车辆按车道方向行驶;②仅考虑车辆在路面平面内进行运动。

考虑实际车辆运行情况对采样区域进行优化,建立如式(4)的采样区域优化函数opt,对采样区域进行优化,得到优化后的采样区域Copt如图5中阴影区域,优化分两步进行,首先根据车道边界及车辆当前位置及目标位置进行初步优化得到如式(5)的区域Copt1,考虑到全局规划因素,这里的目标位置由人为设定;再根据车辆行驶的规律及意图结合车道中心线及安全距离约束,将采样区域进一步优化,得到式(6)的区域Copt2(x),最终优化后的采样区域为式(7)中的Copt。

Copt=fopt(l1,l2,ld,Xmax,Ymax,s,x,y,xg,yg,Cobs)

(4)

(5)

(6)

(7)

式中:l1为左侧车道中心线在坐标系中的横坐标;l2为右侧车道中心线在坐标系中的横坐标;ld为左侧车道中心线在坐标系中的横坐标;x、y分别为车辆在坐标系中的当前位置坐标;xg、yg分别为车辆在坐标系中的目标位置坐标;s为车辆安全距离;Xmax和Ymax分别为车道地图横纵坐标最大值;Cobs为障碍车区域。

图5 采样区域动态优化Fig. 5 Dynamic optimization of the sampling area

2.3.2 基于势力场的采样区域调整

通过上述的基于环境的采样区域动态优化将RRT的采样空间进行了初步优化,为了实现更高精度和更快速度的路径搜索,建立了基于道路场景的势力场函数:

(8)

式中:Uatt为目标点对车辆施加的引力势场函数;Ureq为障碍车对车辆施加的斥力势场函数;Fatt为车辆受到的引力;Freq为车辆受到的斥力;U为环境对车辆的施加的总势场;F为车辆受到的合力。

考虑到传统斥力场在道路场景中的局限性及计算代价过大的问题,针对传统斥力场函数进行改进。首先根据式(7)中优化后的采样区域给斥力场加入采样边界约束;再将传统障碍物斥力场作用域限制变为自动驾驶车辆最大探索边界限制;最后将传统的斥力影响原则改为前侧、左侧、右侧、左前侧、右前侧5个方向的斥力影响,经过探索得出5个方向道的障碍物距离如图6,再将自动驾驶车辆5个方向到障碍物的距离分别用d1～d5来表示。通过这些距离建立优化后的斥力场函数Ureq(X),对所建立的斥力场函数进行负梯度求导得到斥力函数Freq(X):

(9)

(10)

图6 势力场模型Fig. 6 Force field model

同时基于传统人工势场法建立引力场函数Uatt(X)并对所建立的引力场函数进行负梯度求导得到引力函数Fatt(X),通过计算得出所求出的势力场对自动驾驶车辆的合力F(X)并利用反三角函数求出合力方向与X轴夹角α,如图7。

(11)

(12)

F(X)=Fatt(X)+Freq(X)

(13)

(14)

式中:dg为静态与目标点之间的欧式距离;Fy(X)为y方向的分力;Fx(X)为x方向的分力。

若仅根据势力场作用的合力方向作为采样方向会受到势力场的局部最优影响,因此将这个合力方向变成一个方向范围区域,以增加采样的鲁棒性,这里加入车辆转向角作为约束指标,前轮最大转向角由内轮最大转角和外轮最大转角两个数据组成,一般内轮最大转角39.6°,外轮最大转角33.5°;车的转向角度与车的实际大小,运载底盘有关系,根据式(1)[13]及市面常规车辆最小转弯半径和轴距计算得出,一般汽车的转向角在30°～40°之间,笔者为了便于计算人为选取最大转角为35°,则根据转角进一步约束得到最终采样区域C。

(15)

2.4 基于安全距离和270°碰撞检测的动态步长

根据上述的采样区域优化后,为使RRT能够更快速的规划出自动驾驶车辆的可行路径,避免节点冗余,相对理想的状态是在无障碍且满足自动驾驶车辆安全距离比例较大的自由空间里,路径搜索的步长应该选择较大值,加快路径的搜索;然而,在接近障碍物或安全距离满足比例较小的情况时,为保证车辆行驶安全,应当相对细致的规划,步长应选择较小值,确保自动驾驶车辆可以成功规避障碍。笔者的改进RRT算法的采样区域虽然基于人工势场进行了约束,但每个采样点所受合力方向是单独计算的,故无需考虑势力场对采样步长的影响,而选择较为高效的碰撞检测方法进行步长选择。由于传统的碰撞检测方法是以采样点为圆心,遍历360°圆周式范围内是否有障碍物;根据前文假设,车辆向前行驶,则无需考虑车后冗余的90°范围,即从减少计算量的角度,建立式(16)的基于安全距离的270°碰撞检测函数用以选取步长,动态步长可以较快的得到相对较优的车辆行驶路径,并有效减少路径所需的节点数。

(16)

(17)

(18)

式中:l为采样步长;λ为安全距离比例系数;ρ(λ)为基于比例系数λ的270°碰撞检测函数;lmax为最大步长限制值;δ为以车辆前进方向为一二象限下的270°方向角;Cobs为障碍车区域A=[cosδsinδ;δ∈-π/4,5π/4]。

通过270°的碰撞检测函数得到安全距离比例系数λ,用于控制步长的选择。当自动驾驶车辆靠近障碍区域行驶时,采样步长根据自动驾驶车辆当前位置与障碍的距离的变化而调整,当自动驾驶车辆与障碍的距离大于λ倍的安全距离时,步长设置为0.95λs,但不高于最大步长限制,既保证路径快速规划又保证采样的鲁棒性。

2.5 路径后处理

在采样区域优化及步长选择策略基础上,改进的RRT算法搜索出的路径已经大大优化,但由于RRT算法本身的随机采样性[14],仍会存在路径距离代价大,安全系数低且曲率不连续的问题,这对后续的轨迹跟踪造成严重影响,故需对生成的初始路径进行逆向寻优和平滑后处理。

2.5.1 基于车辆安全距离的逆向寻优

为了解决搜索到的路径存在的路径距离代价较大的问题,采用基于安全距离的逆向寻优。如图8,X1～X4分别为4个路径方向的节点,从X1→X4需经过路径a1→a2→a3,若仅根据贪心思想,由于a1+a2+a3>b且该路径处于自由区域内,则可将X1→X4的路径替换为路径b,此时的路径代价最小,但这样的方式无法保证车辆的安全行驶,因此,为考虑车辆行驶的安全问题,这里采用基于车辆安全距离的逆向寻优方法,首先根据贪心思想可知a1+a2+a3>c+a3,再通过基于车辆安全距离的270°的碰撞检测方法判断路径可行,则将X1→X4的原始路径替换为路径c→a3,在实现路径代价减小的同时满足车辆安全行驶的要求。

图8 基于安全距离的逆向寻优示意Fig. 8 Schematic diagram of reverse optimization based on safe distance

2.5.2 基于三次B样条曲线的路径生成

为了满足车辆最大转角的限制,生成的路径应该满足最大曲率的约束,B样条曲线具有局部性和连续性等特点,可以在对路径进行平滑处理时只在局部进行调整而不改变整个路径形状[15]。为达到平滑路径的目的,这里采用参数化的三次B样条曲线来规划路径,如图9中的4个控制点Pi(i=0,1,2,3)为4个节点,Di(i=1,2,3)为三段连接线段,则3次b样条曲线可以定义为:

(19)

其中Ni,3(t)为三次B样条线的基函数,根据deBoor-Cox递推可以得到:

(20)

这里将参数节点的向量区间设置为[0,1],则通过式(21)表达三次B样条生成的路径点:

(21)

式中:ωi为两条线段的夹角。

则曲线曲率为:

(22)

则要满足车辆转向要求则需满足:

(23)

通过三次B样条拟合平滑后的路径,更符合车辆的实际行驶状态。

图9 三次B样条曲线的控制段Fig. 9 Control segment plot of a cubic B-spline curve

3 仿真验证

分别建立少静态障碍物下超车、多静态障碍物下超车、多静态障碍物下右转3种工况下所对应的地图1、地图2、地图3;在此基础上建立多动态障碍物及部分静态障碍物下的超车工况所对应的地图4;这4种工况具有比较典型的路况代表性,可以有效验证算法的有效性和适应性。

在搭载Intel Corei5-8300H CPU和16G内存的电脑上利用Matlab2019(a)分别对RRT,Bg-RRT,以及改进的RRT算法进行仿真。路径搜索过程的仿真结果如图10,通过3种地图下的对比可知,相较于前2种算法,笔者提出的改进算法采样方向更为精准,有效解决采样点冗余的情况;同时在初始路径平滑度上明显得到提升,并且拥有较高的安全系数。

图10 路径搜索过程仿真对比Fig. 10 Path search process simulation comparison

为了更好的对结果进行量化对比,对3种地图下3种算法分别进行30次仿真,记录平均节点数、平均路径搜索时间、和平均路径长度如表1,可以明显看出3种地图下通过改进的RRT算法在采样节点数、路径搜索时间及路径长度方面都有着明显的优化效果,经过加权计算得出,改进RRT算法相较Bg-RRT算法在节点数、路径搜索时间、路径长度分别减少了72.16%、83.57%、6.22%。有效的减少了路径搜索所需的节点数,大大提高了采样效率,降低了生成的路径长度。

表1 路径搜索过程30次仿真平均结果Table 1 Average results of 30 simulations during path search process

3种地图下路径后处理过程如图11,其中逆向寻优路径是在初步路径的基础上基于贪心思想和车辆安全距离约束对其进行逆向寻优,以实现路径代价减小且同时保证车辆行驶安全,图11中的样条平滑路径是在逆向寻优路径的基础上,基于车辆最大转角约束使用3次B样条线进行平滑的路径,解决了逆向寻优路径仍存在个别转角过大的问题,经过后处理平滑后的路径更符合车辆实际行驶路径。

图11 后处理仿真结果Fig. 11 Aftertreatment simulation results

针对两种地图环境各进行30次仿真,结果如表2,逆向寻优对路径长度的减少是比较明显的,3次B样条线在平滑路径上有非常明显的作用,同时也相应减少了路径长度。为了评价路径平滑度这里采用符合曲率连续的路径段数来作为平滑度的评价参数,路径分段数越少则平滑度越高。

表2 后处理30次仿真平均结果Table 2 Average results of 30 simulations after treatment

综上所述,提出的改进RRT算法在以城市路口为基础设计的3种地图场景下的路径规划效率有明显的提升,相较于RRT、Bg-RRT算法,改进RRT算法搜索路径所需节点数、时间都大大减少;经过后处理,路径的平滑度得到提升,生成的路径符合了实际车辆运行的需求。

结合前3个地图场景的仿真,可以明显看出改进后的算法在路径搜索时间上的提升明显,为了进一步验证算法的适应性,在地图4场景下进行多动态障碍物及部分静态障碍物的超车寻径仿真,为较好可视化且考虑到RRT算法的全局规划性,笔者采取分帧进行展示,其中地图4是按真实道路等比例绘制,图中A、B两车分别以30 km/h和20 km/h的时速按地箭头方向进行行驶,C车处于故障静止状态。仿真结果如图12中,从T=0 s到T=5 s,平均规划时间为507.8 ms,有效验证了改进后的RRT算法可以根据障碍物位置的变化快速高效规划出有效的可行路径,证明了改进后算法在低速场景下的有效性和适应性。

图12 地图4下分帧仿真结果Fig. 12 Frame simulation results under map 4

4 结 语

针对城市工况提出了一种基于势力场优化采样区域的改进RRT算法;该方法首先基于道路环境及车辆位置对采样区域动态优化,有效缩小了采样区域减少迭代时间,再基于势力场和车辆最大转角约束对采样区域进行实时调整,实现更高精度的采样,最后采取基于安全距离和270°碰撞检测的动态步长选择策略,大大提高搜索效率,并在此基础上结合贪心思想及曲率约束对路径进行后处理,实现更少路径代价及路径平滑,通过仿真实验验证了其有效性及适应性。

后续将进行实车验证并尝试解决非结构化道路场景下的路径规划问题,为更好的满足各种环境下的自动驾驶车辆及其他车辆的路径规划的实际工程需求。