基于改进RRT算法的无人车路径规划

李伟东, 李 乐

(大连理工大学 汽车工程学院,辽宁 大连 116024)

0 引言

随着自动驾驶技术的快速发展，路径规划作为无人车领域的核心技术之一，也逐渐成为研究热点。路径规划可以分为全局路径规划和局部路径规划。全局路径规划要求地图信息已知并且不考虑动态障碍物，其能够规划出一条从起点到终点的可行路径。局部路径规划算法是指无人车在沿着全局路径行驶时，如果全局路径上出现障碍物导致车辆无法正常通过，需要规划出用于局部避障的路径[1]。与高精度地图相结合，全局路径规划算法可以在整个位姿空间内找到一条从初始位姿到目标位姿的无碰撞路径，同时该路径还满足环境约束、搜索时间约束和车辆运动学约束等约束条件。全局路径规划算法可以大致分为以下几类：(1)基于仿生学的路径规划算法，如遗传算法[2]、蚁群算法等[3]；(2)基于搜索的路径规划算法[3-4]；(3)基于采样的路径规划算法[5]。应用最广泛的基于采样的算法便是由Lavalle提出的RRT[6]算法。 它最显着的优点是不需要对空间进行预处理并且具备概率完备性。但是，基本的RRT算法在规划路径时存在几个缺点[7]：(1)采用随机采样算法，搜索过程具有盲目性和随机性；(2)路径曲折，不能应用于无人车全局路径规划领域；(3)收敛速度慢，搜索效率低。

很多学者都致力于改进RRT算法的缺陷。LaValle和Kuffner提出了RRT-Connect[7]算法，该算法会从起始节点和目标节点并行生成随机树来提高搜索速度。Karaman 和Frazzoli提出的RRT*[9]算法引入了路径代价信息和重布线操作，虽然得到的路径质量最优，但算法的收敛时间也随之增加。此外还有很多RRT系列算法在不断地涌现，例如Dynamic-RRT[10]算法、Informed RRT*[11]算法和批处理信息树[12]( BIT*, batch informed trees)算法等。与此同时RRT系列算法也开始与其他算法结合。冯来春[13]提出将A*算法和RRT算法结合的方法，利用A*算法在格栅图中生成的最短路径来引导RRT算法的扩展。Huang Shunyu[14]将人工势场法引入到RRT算法中，通过建立障碍物斥力场来限制障碍物附近的搜索区域，并在随机树的扩展中添加引力分量加快算法的收敛速度，但是可能会出现局部最小值问题。Ivojevic等人[15]针对类汽车机器人的路劲规划问题，将RRT算法的节点间的直线连接替换为Dubins曲线，虽然能规划出满足汽车运动学约束的路径，但是路径过于冗余且转弯太多。朱冰[16]提出基于安全场的改进RRT*智能汽车路径规划算法，通过建立安全场模型引导车辆避障行驶，但是路径的搜索具有较大盲目性。

综合已有研究成果，针对上述提到的RRT算法的缺点，本文提出一种应用于无人车全局路径规划领域的改进RRT算法。本文算法首先使用人工势场法和目标动态概率采样策略来减少算法在搜索过程中的盲目性以及在连接终点位姿时使用Reeds-Shepp曲线，减少目标点处额外的位姿调整，从而减少算法消耗的时间；然后算法考虑了车辆的非完整性约束，并对规划的路径进行转角约束检测和碰撞检测，保证汽车跟踪行驶的安全性；最后对路径进行后处理优化，最终得到的全局路径更适合无人车跟踪行驶。

1 汽车运动学模型

标准RRT算法仅可以寻找一条能够连接起点和终点路径，但是规划的路径曲折且无目的性，不能满足无人车的运动学约束，从而车辆无法跟随规划的全局路径行驶。因此本文算法在进行路径规划时考虑车辆的运动学约束，车辆运动学模型如图1。

图1 车辆运动学模型

以车辆的后轴中心为车辆的参考点，b为车的轴距，φ为前轮转角，θ为航向角，r为转弯半径，k为路径曲率。假设车辆的速度为v，那么根据图1就能得到车辆的运动学方程，如式(1)：

(1)

车辆受到的运动学约束需要满足下式：

(2)

式中,vmax为最大车速，φmax为最大等效前轮转向角，kmax为最大路径曲率，Rmin为最小转弯半径。

2 改进的RRT算法

2.1 地图及障碍物栅格化

为了能够精确表示地图信息、障碍物信息以及定位信息，需要将无人车所处环境的高精度地图作为算法的基础。全局路径规划可以使用栅格地图，二维栅格地图使用占用栅格的形式存储环境中的静态障碍物信息，为全局路径规划提供信息[17]。本文使用已知的二维栅格地图作为地图信息输入，在全局路径规划算法开始时初始化地图信息。

2.2 基本RRT算法

当得到场景的栅格地图后，便可以根据目标位姿和起始位姿构造搜索树了。RRT算法示意图如图2所示，以起始点作为随机树的根节点，通过随机采样，将随机树的扩展引导到可行区域直到搜索到目标点，从而生成一条从起始点到目标点的全局规划路径。

图2 RRT算法示意图

基本步骤如下：

1)以状态空间的起点Xinit作为根节点构造随机树；

2)在搜索空间内产生随机采样点Xrand，用于引导随机树的扩展；

3)遍历整个随机树上已产生的节点，选出距离随机点Xrand最近的树节点Xnear；

4)从Xnear节点沿着Xrand节点的方向以扩展步长生成新的节点Xnew作为新的树节点。如果扩展过程中遇到障碍物则取消本次扩展。重复上述迭代过程直到目标节点变为叶子节点或者超过规定的迭代次数时搜索结束。从目标点回溯到起点便可得到规划的路径。

2.3 目标动态概率采样

为了减少RRT算法搜索过程的盲目性，解决盲目搜索的问题，本文算法在随机采样时引入了目标动态概率采样。动态概率采样策略首先会设置一个目标采样阈值pbias(0

(3)

当随机树扩展过程中遇到的障碍物较少时，直接使用目标点作为采样点往往会加快搜索速度。但是当环境中障碍物较多时，在目标点方向上进行简单的概率扩展，可能会陷入局部困境问题。因此为了应对多种不同的场景，目标点采样概率阈值pbias应该被设置为动态变化的值，Niter为当前迭代次数，Ncurr为当前所生成的有效节点个数，目标点采样概率阈值pbias公式如式(4)：

(4)

式中,k代表目标概率采样阈值的最大值。Ncurr和Niter的比值代表栅格地图中障碍物的稠密程度。比值越大代表当前有效节点占比越多，障碍物越少，随机树向目标节点扩展的可能性越大。相反，意味着无效节点数目较多，障碍物越稠密，随机树向随机点扩展的可能性更大。

2.4 基于人工势场的节点扩展优化

RRT算法由于其搜索的随机性强，会在空间进行搜索时产生许多无用节点，收敛速度低，搜索时间过长。同时，在人工势场法路径规划中，无人车总是朝向虚拟势场的负梯度方向运动，在复杂和特殊的环境可能会出现势能最小处在某个受到的引力和斥力相互抵消的非目标点位置而不是目标点位置的情况，出现局部最小值的情况，导致目标节点不可达[18]。

针对上述问题，本文将人工势场法的目标引力作用和障碍物斥力作用融入到RRT算法的扩展节点过程中，根据先验格栅地图构造地图的人工势场，并使随机树的扩展能够在势场的引导下朝着目标点进行扩展，加快算法的收敛速度。同时依靠目标动态概率采样和RRT算法的扩展随机性，可以有效避免传统的人工势场法在进行路径规划时会陷入局部最小值的问题。人工势场的引力场Ua(p)、斥力场Ur(p)以及合力势场Utotal的函数如下所示：

(5)

(6)

Utotal(p)=Ua(p)+Ur(p)

(7)

其中:ka是目标引力场增益系数，kr是斥力场增益系数。ρ0是一个距离阈值，当节点到最近的障碍物距离大于该阈值则不产生斥力。ρ(p,pgoal)和ρ(p,pobs)分别代表当前节点到目标点和最近障碍物的距离。如图3所示，图(a)的环境从起点到终点产生的总势场图如图(b)所示。

图3 总势场示意图

因此算法扩展过程中，受到的目标引力函数与障碍物斥力函数分别如式(8)所示：

Fa(p)=kaρ(p,pgoal)

(8)

(9)

Ftotal(p)=Fa(p)+Fr(p)

(10)

在人工势场中，目标产生的引力和障碍物产生的斥力的合力Ftotal的方向就是总势场的负梯度方向。在新节点Xnew扩展过程中融合人工势场的作用，使节点扩展的方向不仅沿着Xrand，还会考虑势场合力Ftotal的引导作用。扩展节点由两个向量叠加而成，扩展节点的计算公式如式(11)所示：

Xnew=Xnear+d(wrnrand+(1-wr)nf)

(11)

式中,d为扩展步长，wr为随机点的扩展偏向权重，nrand为Xrand方向的单位向量，nf为合力Ftotal方向的单位向量。

新节点扩展过程示意图如图4所示。

图4 节点扩展示意图

2.5 路径约束条件

由前文分析可知，由于汽车的动力学约束，生成路径中的转角应该小于最大前轮转角φmax，因此在生成新节点和生成路径时需要考虑角度约束。转角的计算如图5所示，路径中3个连续节点X1，X2，X3及其夹角角度φ，计算公式如式(12)所示：

(12)

在扩展过程中会根据公式判断扩展的新节点Xnew和Xnear的连线与Xnear及其父节点Xparent的连线之间的夹角是否大于前轮最大转角，如果大于约束，则放弃本次扩展。

图5 转角约束示意图

除了路径转角约束以外，为了保证所规划出的路径对于无人车来说是安全无碰撞的，还需要对路径进行碰撞检测。本文采用方向包围盒(OBB, oriented bounding box)的碰撞检测方法，一个对象的OOB指的是包含该对象且相对于坐标轴方向任意的最小长方体[19]。如果两个凸多边形不发生碰撞，则一定存在一条分离轴，使得两物体在该分离轴上的投影不相交。在格栅地图中，OBB的检测原理如图6所示，矩形A和B分别代表车辆和障碍物的OOB。Ax、Ay为以矩形A中心点OA为原点坐标的局部坐标系下的单位向量，Bx、By为以矩形B中心点为OB原点坐标的局部坐标系下的单位向量。α为全局坐标系下车辆的航向角，φ为两矩形中心连线与格栅地图的全局坐标系x轴正方向的夹角。

图6 OBB碰撞检测示意图

在实际应用中只需要判断两矩形分别在Ax、Ay、Bx、By4个方向的投影是否有重叠即可。如果矩形A和矩形B没有碰撞的话，应该满足下式4个条件：

(13)

式中，L为矩形中心点的距离LA、LB分别代表矩形A、B的长度；WA、WB分别代表矩形A、B的宽度。

2.6 目标点贪心连接

Reeds-Shepp[20]曲线是由多段半径为最小转弯半径的圆弧曲线或直线拼接而成，能够在一个位姿到另一个位姿之间生成满足运动学约束的前进或倒车轨迹。在原RRT算法中，当随机树新扩展的节点到目标节点的距离小于扩展步长时，就宣告搜索结束找到路径，但如果有终点位姿要求，RRT算法就需要反复搜索以保证满足终点的位姿要求。本文采用Reeds-Shepp曲线来代替原RRT算法的目标点连接方式，当新扩展节点进入到目标点的可直接连接范围后，每次搜索都尝试使用Reeds-Shepp曲线对目标点连接，从而避免反复搜索以满足终点位姿要求。通常目标点的连接范围可以设置为10倍的扩展步长，这样以来就能减少大量搜索时间和扩展的节点，避免多余的位姿调整。

2.7 路径后处理优化

假设使用该算法规划得到的初始全局路径点的集合为P{P1,P2,P3,…,Pn}，如图7所示，假设节点P1与节点P4之间能够通过直线连接并且连线又同时满足转角约束和无碰撞约束，则P1和P4之间的P2和P3节点均为冗余节点，可以将其从路径节点集合中删除。在得到初始路径后进行路径剪枝，从起点开始，遍历整个节点集合，找到并去除所有冗余节点，能够减少路径长度并使车辆驾驶的平顺性提高。

图7 路径剪枝示意图

但是此时得到路径仍由若干线段连接而成，不满足车辆对平顺性和安全性的要求，则需对路径进行平滑处理。由于三阶贝塞尔曲线能够保证始末位姿要求和曲率连续要求，则对3个节点形成的两段直线采用两段三阶贝塞尔曲线进行平滑处理。n阶贝塞尔曲线表达式如式(14)所示：

(14)

式中,Pi代表贝塞尔曲线的控制点，u代表贝塞尔曲线的参数且0≤u≤1。Bi,n为n次Bernstein基函数，且满足：

(15)

则原来由3个随机树节点确定的两段直线路径经过两段三阶贝塞尔曲线平滑后的结果示意图如图8所示。

图8 贝塞尔曲线应用示意图

图中P1、P2、P3分别为路径相连的两直线的节点。A1、A2、A3、P为第一段三阶贝塞尔曲线的控制点。B1、B2、B3、P为第二段三阶贝塞尔曲线的控制点。其中控制点P为A3和B3连线的中点。

本文的改进RRT算法的流程图如图9所示。

图9 改进RRT算法流程图

3 仿真实验对比分析

本文使用Matlab对本文所提出的改进RRT算法进行仿真实验，测试主机的处理器为Intel(R) Core(TM) i7-9700，主频为3.0 GHz，内存大小为16 GB。

为了验证本文的改进RRT算法具有更好的性能，将RRT算法、RRT*算法和改进RRT算法在简单障碍物环境、复杂障碍物环境和单一通道环境3种二维仿真栅格地图环境下进行全局路径规划对比试验，3种仿真地图大小均为500×500个像素的栅格地图表示，起点均为(50,50)，终点均为(450,450)。在3种环境下每种算法均进行100次重复实验，并统计算法各个指标的平均值。3种环境下各算法的规划结果如图10～12所示，其中(a)图为基本RRT算法规划效果，(b)图为基本RRT*算法规划效果，(c)图为改进RRT算法得到的路径优化前的效果，(d)图为改进RRT算法对初始路径优化后的效果。

图10为简单障碍物环境的算法规划对比图，地图中障碍物较大，环境较为简单。图(a)为基本RRT算法规划的一条可行路径，但由于RRT算法的盲目搜索特性，生长树会随机分布在无障碍区域，算法效率较低并且得到路径质量很差。图(b)为RRT*算法规划的一条可行路径，由于RRT*算法具有渐进最优的特性，所以RRT*能够得到较优的路径长度，但是需要大量反复迭代，节点利用率和搜索效率较差，并且不满足路径安全约束。图(c)和图(d)是本文改进RRT算法在进行路径优化前后的规划路径，由图可见路径剪枝优化效果显著。

图10 简单环境下算法效果对比

从表1的实验数据中可以得出，由于地图环境比较简单，目标概率采样和人工势场的引导作用可以被充分利用，减少了大量无用节点的扩展，明显加快了搜索速度，相比于RRT算法和RRT*算法，扩展节点数目分别减少了43.50%和83.37%，搜索时间分别减少了62.12%和77.46%。并且在经过节点剪枝和路径优化后，改进RRT算法的路径长度较优，其路径长度相比于RRT算法的路径长度缩短13.86%，但是由于考虑了运动学约束和碰撞约束，相比于路径长度最优的RRT*的算法增加了3.39%。

表1 简单障碍物环境实验结果对比

图11为复杂环境地图的规划效果，环境中存在大量大小不一的障碍物，从起点到终点可以有多条路径可以选择。图(a)中RRT算法的规划效果图，可以看出随机树的扩展杂乱无章，路径曲折。图(b)中RRT*算法能够利用自身的重布线特性在复杂的环境中规划出一条较优路线，但是扩展树所需节点较多，搜索效率较差。图(c)和图(d)中的规划效果明显优于RRT算法和RRT*算法，并总能找到较优路径，并且在需要多次转折的场景下，路径优化的效果仍显著。

图11 复杂环境下算法效果对比

表2的实验结果得知，本文改进的RRT*算法的节点扩展数目相比于RRT算法和RRT*算法分别减少了67.36%和85.07%，规划时间分别减少了63.14%和77.46%。规划得到的路径长度相比RRT算法缩短了23.12%，并且和路径长度最优的RRT*算法差距不大。因此本文的改进RRT算法该环境下能够保证路径长度达到相对较小的同时提高了算法的规划效率，路径也变得更加平滑。

表2 复杂障碍物环境实验结果对比

图12为单一通道障碍物环境，特点是从起点到终点只有一条狭窄的通道可以经过。3种算法的规划效果对比来看，RRT和RRT*在寻找通道入口前需要经过大量的随机扩展，节点利用率和搜索效率非常低。而本文改进的RRT算法能够根据人工势场的引导作用，快速找到通道的入口，搜索时间相比于RRT算法和RRT*算法大幅降低，并且能够依靠障碍物斥力作用和碰撞检测机制，能够在狭窄通道内较快得到一条安全无碰撞的可行的全局路径。

图12 单一通环境下算法效果对比

从表3的实验数据中可以得出，改进RRT算法的平均扩展节点数为 41.25个，比 RRT减少 65.97%，比RRT*减少87.21%；平均规划时长为1.05 s，比RRT缩短58.33%，比RRT*缩短76.87%；平均路径长度为616.83 m，比RRT减少14.80%，比RRT*少幅度增加了2.32%。

表3 单一通道环境实验结果对比

根据上述的仿真图和实验结果可以明显的看出，本文的改进RRT算法无论是在规划时长、扩展节点数还是路径长度上都相比标准RRT算法具有明显优势。虽然本文的RRT算法得到的路径略长于RRT*算法得到的路径长度，但是花费时间和无用节点的数量明显减少。除了规划时间、节点数量和路径成本等指标以外，RRT和RRT*算法得到的路径曲折并且不满足碰撞约束，汽车的行驶安全和平顺性不能得到保障。并且由图10到图12的(c)图和(d)图可以发现，本文提出的路径后剪枝和优化方法效果显著。因此，本文所提出的改进RRT算法在3种复杂的障碍物环境下，具有规划效率更高，路径长度较优，路径光滑且安全的特点。

4 结束语

本文针对RRT算法在无人车全局路径规划领域存在的问题，提出一种综合改进算法。首先引入目标动态概率采样和人工势场引导随机树扩展的策略，提高了算法的搜索效率，减少冗余节点的产生，内存占用更少；然后定义了路径的转角约束和碰撞约束，使路径能够满足汽车的运动学约束并使无人车安全无碰撞行驶；其次当随机树扩展到目标点可连接范围后，每次迭代都尝试使用Reeds-Shepp曲线连接目标点，减少多余的位姿调整，进一步提高算法效率；最后在路径初步规划完成后，提出一种路径修剪的方法，对路径进行简化，并使用两段三阶贝塞尔曲线对路径转角进行平滑连接，去除冗余节点的同时还能使路径更加光滑。通过与RRT算法和RRT*算法仿真实验对比，结果表明本文提出的改进RRT算法在多种环境下的规划得到的路径在搜索效率、路径质量上都更具优越性。该算法对无人车的全局路径规划具有一定的参考意义。