改进RRT在汽车避障局部路径规划中的应用.docx

    改进RRT在汽车避障局部路径规划中的应用    宋晓琳+周南+黄正瑜+曹昊天摘 要：根据传统快速搜索随机树算法（rapidly random-exploring trees，简称RRT）搜索速度快、所需时间短，但随机性大以及约束不足等特点，建立了直道和弯道的期望路径模型，采用高斯分布描述随机采样点，并引入启发式搜索机制，改进RRT算法.与原算法仿真对比，结果表明：改进算法所规划的路径质量显著提高，规划时间缩短一倍.同时，在Prescan软件中搭建直道和弯道仿真场景，跟随规划路径，结果表明：改进后RRT算法所得路径具有很好的跟随效果，且侧向加速度在车辆稳定性要求范围内，说明采用改进后的RRT算法进行汽车局部路径规划可行实用.Key：快速搜索随机树；汽车局部路径规划；高斯分布；路径跟随：TP242 文献标志码：AAn Improved RRT Algorithm of Local Path Planning for Vehicle Collision AvoidanceSONG Xiaolin，ZHOU Nan，HUANG Zhengyu，CAO Haotian（Stake Key Laboratory of Advanced Design and Manufacturing for Vehicle Body， Hunan University，Changsha 410082，China）Abstract：The original Rapidly-exploring Random Trees（RRT） algorithm has a rapid exploring-speed and short required time in path planning though it has large randomness and lacks of constraints. Thus， an improved RRT was proposed where the expected paths were built in both straight and curved roads. The random points were accorded with normal distribution around the expected paths. Heuristic search method that led the random points to the goal with a certain probability was also used for improvement. Compared with the original RRT algorithm， it performs quite well in both time-efficient and path quality in the simulation. Meanwhile， the effectiveness of the improved RRT algorithm was verified in Prescan. The path can be followed well and the secure lateral acceleration was satisfied. In conclusion， the improved RRT is effective in the path planning for vehicle collision avoidance.Key words： rapidly-exploring random trees； vehicle path planning； Gauss distribution； path following近年来随着汽车向智能化的不断发展，无人驾驶汽车技术引得越来越多人开始关注.路径规划作为其中一项关键技术，许多学者开展了有益的探索，并取得了一些研究成果.比如A*算法[1]、D*算法[2]等启发式搜索算法，在复杂环境下有着很好的规划速度，但是路径并不理想；人工势场法[3]是一种虚拟力算法，其优点是规划的路径平滑，但是容易陷入局部最优解；人工势场法与弹性绳模型结合[4-6]在车辆局部路径规划时有理想的路径，但由于模型比较复杂，搜索效率低；此外蚁群算法、遗传算法、神经网络算法、水滴算法等智能仿生算法[7-10]，在處理复杂动态环境信息时有较好表现，但由于需迭代计算，时效性不够好.快速搜索随机树算法（RRT）[11]由Lavalle于1998年提出，是一种基于树结构的典型算法，在移动机器人路径规划中有着较早应用.由于算法在进行路径规划时是随机采样，不需要预处理，因此有着很快的搜索速度.路径点之间可以经过运动学、动力学仿真生成可执行曲线，因此该方法可用于解决含有运动学约束的路径规划问题.但RRT算法存在一些不足：1）重现性差，对同一环境进行多次路径规划结果不同；2）寻找最优或者次优路径能力较差；3）随机采样点在整个可行域内随机寻点的搜索方式，存在很多不必要的运算，影响算法速度.随着RRT算法的应用和改进，一些学者也提出了不同的方法.偏向RRT[12]以及双向RRT[13]的提出使得算法的搜索效率得到了提高；DRRT[14]与MP-RRT[15]原算法在搜索路径的稳定性上做出了改进.但上述改进之后的结果并不适用于汽车行驶路径.针对以上不足，本文将建立道路模型，考虑路径约束，改进RRT算法，使其规划出的路径能够适用于汽车运动.1 道路环境建模在环境已知的情况下，建立道路环境模型.直道环境模型根据道路长度以及车道宽即可得到，弯道环境模型如图1所示，位于道路上的惯性坐标系的原点选取在道路中心线上，道路宽度为W，规划起始点即主车位置A，目标点C，障碍车位置为B.高速路直线之间的缓和曲线通常采用回旋线来实现平滑过渡，回旋线的特征是其曲率变化为常数.假定长度为l的回旋线线段起始曲率为C1，终止曲率为Cf，变化常数为C2，则有C（l）=C1+C2×l成立，C2=（Cf-C1）/l.回旋线常采用多项式逼近的形式表示： 式中：R0为道路中心线初始横向偏移；C0为初始的方向角.根据图1，结合边界条件R（0）=0，R′（0）=0可以将R0和C0消除掉.为了保持车道宽度一致，弯道的左右边界是通过中心线上点向着其法线方向上下平移单个车道宽度来得到.在道路坐标系下中心线上的点可由式（2）表示.中心线上一点的切向量和法向量则可以表示成：因此道路左右邊界则可以由（4）来表示2 RRT算法原理基本的RRT算法如图2所示，RRT算法以给定的起始点为随机树根节点，根据当前环境快速有效地搜索可行域空间，通过随机采样点，将搜索导向空白区域并增加随机树的叶节点直至目标点区域，从而生成从起始点到目标点的路径.算法的一般步骤如下：1）首先通过environment（）函数建立环境数据模型，初始化各项参数；2） 通过while循环来判断树节点是否达到目标点goal范围内，若没有，则开始扩展点.先生成随机采样点Prand，在树节点上通过函数Nearest（）选择距离Prand最近的树节点作为扩展节点Pnear，通过扩展函数New得到新的树节点Tnew，并将其添加到随机树上，直到循环终止.3）通过getpath（）函数来得到生成的路径path.原算法主体程序如下：如图3所示，传统的RRT算法应用到车辆路径规划中存在以下问题：1）由于随机采样点随机性大，导致搜索空间中有过多的冗余搜索，表现为搜索树布满了道路环境空间；2）搜索出来的路径曲率变化过大，甚至出现小范围内直角变化，这样的路径并不能满足汽车行驶的正常状态；3）路径在靠近障碍时才开始避障，对于运动中的汽车会造成失稳或者与障碍物发生碰撞.道路长度/m3 RRT算法的改进3.1 期望路径模型针对原RRT算法表现出来的问题，提出了一种随机采样点高斯分布的改进RRT算法.根据汽车行驶环境不同，设计了两种期望路径模型.3.1.1 直道模型驾驶经验丰富的驾驶员期望的避障路径模型如图4（a）所示.期望路径以函数Epz表示，其中各段均为直线段，start为起始点，goal为目标点.提前避障在车辆避障路径规划中是必要的，故在模型中需要添加提前避障距离S，并根据车速调整大小.设V为当前车速，tc为换道时间，通常完成换道时间tc为1～2 s，ΔS为自定义安全提前量.对于车速为V的汽车其刹车距离则提前避障距离图4（a）中fz2表示提前避障区域，区间长度为S，fz4区间长度也为S.期望路径只是粗略的路径模型，在此基础上进行平滑得出的路径并不能满足汽车安全稳定行驶的要求.因此采用RRT算法进行路径寻优搜索.为了使随机采样点分布在期望路径周围，利用高斯分布函数生成的点集中在其均值周围的特点，结合期望路径函数则可以实现这一目的.在道路坐标系下随机采样点的高斯分布概率函数为：令μ=Epz（x），则可以得到如图4（b）所示的随机采样点分布趋势图.道路长度/m（a）期望路径模型道路长度/m（b）随机采样点高斯分布σ的大小决定了随机点在Epz（x）周围的集中程度，σ越小越靠近Epz（x）.特别地，对于fz2与fz4周围的随机采样点，如图4（a）以fz2为例，通过相应水平范围的随机点高斯分布旋转处理得到，即对旋转角度：3.1.2 弯道模型将弯道分为多段，采用直线代替弯道曲线的形式来完成期望路径模型的建立.如图5（a）弯道的期望路径以函数Epw来表示.随机采样点在fw各段函数区间的分布同直道的处理相同，从而可以得到如图5（b）所示的分布趋势图.3.2 约束条件要使一条规划出来的路径有效地应用于汽车运动，即路径可跟踪，则规划路径时必须满足道路环境约束.首先，随机树节点的生成要满足道路环境约束，设Bleft，Bright分别为道路的左右边界，则树节点位置坐标要满足：考虑到汽车是具有几何形体的，设其车宽为D，则上述y方向的约束变为：假定汽车质心沿着规划的路径运动，为了保证行驶过程中的稳定性，规划出的路径的曲率变化不能过大.若在实际情况下前轮最大转角为θmax，则路径中子节点与其父节点的连线和父节点与其父节点的连线之间的夹角β必须满足β<θmax，通常不同车型的θmax值在30°～40°之间.如图2中子节点Tb的父节点为Ta，Tc的父节点为Tb，那么夹角约束表现为：其中：K1为直线TaTb的斜率；K2为TcTb的斜率.βT为夹角限制值.为了保证所扩展的点不与障碍车有交集，即树节点不与障碍车碰撞的要求，采用安全椭圆包络障碍车，并适当放大安全椭圆以保证避障要求.若新节点与其父节点的连线不与安全椭圆相交，则所扩展的新点满足避障要求.取连线上的五等分点Pi（x，y），则约束方程表现为：其中（xob，yob）为障碍车的位置，半车长a=2 m；半车宽b=1 m；s为安全椭圆放大系数，当s=2时，安全椭圆正好包络矩形的障碍车，因此从安全避障考虑，s≥2.3.3 启发式搜索原算法在扩展随机树时，由于缺乏导向机制，算法的收敛速度在一定程度上受到了影响，为了提高算法计算速度，引入启发式机制来对随机采样点以及随机树节点的选择进行处理.采样点Prand在随机生成过程中会以一定概率ρ0选择目标点，从而将随机树节点向目标点引导，通常ρ0=0.1. 其中，GaussRand（）为随机采样点生成函数.另外，在选择Pnear时不再单独以距离Prand最近作为选择标准，而是以随机树节点的择优系数Ch来决定，Pnear确定为Ch值最小所对应的树节点.其中ωi，ωj为权重系数，且ωi+ωj=1；Dpr为树节点到Prand的距离，Dg为树节点到目标点goal的距离.当ωj>ωi时，选择出的Pnear具有向目標点靠。