城市工况下基于改进RRT的无人车运动规划算法*

余卓平 卫烨 熊璐 李奕姗

（同济大学，上海 201804）

主题词：无人驾驶汽车 运动规划 偏向性采样 邻近点搜索

1 前言

运动规划是无人驾驶汽车的关键技术，是在决策给出目标点的位姿信息后，利用车载传感器获得自车状态以及周围环境信息，在满足运动系统微分约束的基础上，及时生成一条指导汽车在接下来短时间内运动的安全路径。运动规划最初开始于机器人领域，车辆可看成是一种特殊的轮式机器人，因此车辆的运动规划算法大多由机器人领域的规划算法发展而来。较有代表性的算法有两大类：基于网格搜索的算法和基于随机采样的算法。基于网格搜索的算法如A*[1]、AD*算法[2]，能保证找到可行解，后者还能在动态障碍物环境下应用，但生成安全、可行的路径需要较精确的环境地图以及成熟的代价确定准则。基于随机采样的算法以快速随机扩展树[3]（Rapidly-exploring Random Tree，RRT）法为代表，此类方法不需要对状态空间显式建模，因此搜索速度快，并且可以在搜索过程中考虑车辆自身客观存在的约束。但此方法也有一些不足之处。以RRT算法为例：首先，虽然RRT算法具有概率完备性，但每次搜索的结果随机性较大；其次，标准RRT算法均匀随机采样过程中大量的碰撞检测严重影响效率；另外，车辆构型空间中的度量距离没有闭合形式表达式，计算困难。国内外的学者针对这些缺陷提出了一些解决办法。美国斯坦福大学的Kuffner和爱荷华州立大学的LaValle提出了RRT-Connect算法[4]，同时构建两棵分别起于初始状态和目标状态的树，以加快RRT搜索。随后LaValle又提出Goal-biasing RRT算法[5]，在随机采样序列中以一定比例插入目标状态，引导树向目标状态扩展。英国剑桥大学的Shkolnik和Walter等人[6]提出了RG-RRT（Reachability Guided RRT）算法以消除不准确的度量距离对RRT探索能力的影响。美国伊利诺伊大学的Cheng[7]提出RC-RRT（Resolution Complete RRT）算法降低障碍物周边节点获得扩展的概率，从而加速碰撞检测。尽管各种改进措施能有效提高求解质量，但仍难保证获得最优解，于是近年来出现了许多求渐进最优解的算法，如RRT*[8]、LBT-RRT算法[9]等。

另外，也有部分研究人员仅采用几何曲线进行路径生成，如螺旋曲线[10]、B样条曲线[11]、贝塞尔曲线[12]等，但通常需要在生成路径时与障碍物进行繁琐复杂的几何分析，以保证路径无碰撞，因此效率较低。

本文针对城市结构化道路环境下的无人车运动规划问题，提出了一种快速偏向性RRT（Fast-Biasing RRT，FB-RRT）算法。该算法首先基于几何分析结果初始化RRT，随后针对城市工况下最常见的换道和转向行为设计了不同的偏向性采样方法，并结合KD树加速邻近点搜索。搜索成功后对RRT进行裁剪重构并利用B样条曲线平滑路径。最后，通过仿真以及实车试验验证了该算法的有效性和实用性。

2 车辆模型建立

无人车的运动规划问题是要求找出从起始点到终止区域的一条可行路径上的控制输入，也就是说需要先规划运动的轨迹，再根据轨迹输出和系统状态变量的各阶导数求出系统状态变量和控制输入。阿克曼转向模型为常见的具有此种性质的车辆模型，如图1所示。

图1 车辆模型

该模型将车辆视为平面刚体，具有3个自由度，选取后轴中心点R作为参考点。图1中，b为车辆的轴距，ϕ为前轮转角，车辆在大地坐标系下的坐标为(X,Y)，横摆角为θ。车辆状态方程为：

式中，v为车速；κ为转弯曲率。

车辆模型受动力学约束，因此需满足：

式中，ϕmax为最大前轮转角；vmax为最大车速；κmax为最大转弯曲率。

3 快速偏向性RRT算法

3.1 基本RRT算法

RRT算法由S.M.LaValle提出[3]，是一种基于随机采样的增量式运动规划算法，属于典型的树状结构搜索算法，通过在状态空间中不断搜索采样生成一棵树，整棵树包括树上所有节点组成的节点集N和所有“树枝”组成的边界集E，扩展过程如图2所示。

图2 RRT扩展示意

整棵树T从初始点ninit开始扩展，利用采样策略对状态空间随机采样得到采样点nrand，利用邻近点搜索算法在已有树T中搜索“距离”nrand最近的节点nnear，之后根据设置的扩展机制（如确定的扩展步长d）连接nnear和nrand，并得到新节点nnew，检测nnew和连接线段enew的安全性，如果满足要求，那么将nnew和enew分别插入节点集N和边界集E中，树T得到扩展。整个扩展过程循环进行，直到新节点扩展到目标区域或达到最大循环次数为止。最后，由目标节点Ngoal反向回溯到根节点ninit，就可得到规划路径。

3.2 RRT的初始化

由于RRT搜索具有随机性，因此在搜索前先用几何分析法生成曲线控制线段和控制点，经碰撞检测后保留无碰撞的控制点与线段，用以初始化RRT，这样可有效提高RRT算法的搜索效率。城市化道路具有结构性，在此类环境下典型城市驾驶工况可分为直行、换道、转向和掉头[13]。本文以换道和转向为例，介绍控制点和控制线段的获得。

3.2.1 B样条曲线

本文选择B样条曲线作为最终路径的平滑曲线。B样条曲线是样条曲线的一种特殊表示形式，由(n+1)个控制点及参数节点向量Tn,k=(t≤tii+1的(k-1)确定次B样条曲线为：

式中，Ni,k(t)为Tn,k上(k-1)次B样条基函数，由deBoox-Cox递推关系[14]确定：

B样条曲线具有连续性、凸包性和局部性等优点，曲线控制点个数和阶数没有必然联系，不需要复杂的数值计算。另外，通过限定控制点连线间的夹角可将曲线的曲率维持在一定界限内，如图3所示。

图3 曲线控制线段模型

控制点A1、O、B1决定的线段夹角的最小值αmin与曲率最大值以及控制点连线的最短线段长度Lmin满足[11]：

只要α>αmin，即可满足曲率要求。为使最终路径较好地贴合控制线段，将控制线段的中点A2、B2也纳入RRT的初始节点集。

3.2.2 换道工况

给定初始点和目标点的位姿后，换道工况可以简单抽象为车辆坐标系中的3条控制线段，如图4所示。

图4 换道参数示意

假设3条线段长度分别为l1、l2和l3，总长度为L，经图4所示几何分析得线段长度关系为：

式中，(x1,y1)、(x2,y2)、(x3,y3)分别为车辆坐标系下线段交点X1、X2以及终点X3的坐标。

借助优化工具，以总路径长度L最短为优化目标即可得到3段线段的长度，通过计算即可得7个控制点的坐标。

3.2.3 转向工况

为使曲线更接近于转向工况时的圆弧曲线，设定控制线段长度及控制线段间的夹角都相同，如图5所示。

图5 转向参数示意

假设从初始点位姿变换到目标点位姿共需n条长为l的控制线段，相邻控制线段间夹角均为α。根据边界条件可得约束方程：

式中，(xg,yg,θg)为终点状态；(xi,yi,θi)为各控制点的状态。

根据式（7）即求解得到3个未知量n、l、α，控制点的坐标即可求出。

3.3 采样策略

随机采样直接关系到树的扩展质量和搜索效率，本节结合决策给出的驾驶行为设计偏向性采样策略，引导RRT的扩展。

3.3.1 目标偏向性采样

采样过程中概率分布函数最为重要。本文采用常见的高斯分布，便于根据已知条件进行偏向性采样。高斯分布遵循的采样策略为：

式中，(x0,y0)、(sx,sy)分别为参考采样点和采样点坐标；(r,θ)为高斯分布决定的采样相对值：

式中，(r0,θ0)为相对于(x0,y0)的补偿参数，即高斯分布的均值；(σr,σθ)为对应的高斯分布标准差；(rrand,θrand)为随机变量。

因此可以通过高斯采样将采样点限制在一个环形区域内，如图6所示。

针对决策给出的驾驶行为和目标点，结合高斯采样的性质，可以设置合适的(r0,θ0)和(σr,σθ)，引导搜索向着目标点的方向进行。

图6 高斯采样点分布规律

3.3.2 强制扩展策略

所谓强制扩展，就是按照一定比例将目标点和树中距离目标点最近的节点利用一定的方法连接，随后结合碰撞检测判断路径的安全性，并将路径上有效部分插入到现有的树中，从而提高搜索效率。因此本文利用ε贪婪算法，按照一定比例采用3.2节所述几何分析法进行强制扩展。

偏向性采样虽然加快了搜索进程，但同时也限制了RRT算法的探索能力，因此，本文按照一定比例采用普通采样策略进行采样，保证了算法在障碍物较密集的环境中的搜索能力。整体采样策略如图7所示，其中，r1、r2为随机数。

图7 采样策略架构

除了采样策略，RRT的扩展步长d也直接影响了搜索速度。本文的扩展步长根据驾驶行为选择：在换道时，根据初始点和目标点的距离来选择不同的扩展步长，如果距离较大，选用较大步长加快搜索，反之选用较小步长提高成功率；在转向时，路径的曲率变化较大，因此采用较小的扩展步长以保证最终搜索结果的质量，防止采样步长过大导致路径的曲率不满足要求。

3.4 最邻近点搜索策略

利用采样策略得到安全的采样点后，需要在已有的树中寻找“距离”采样点最近的节点，然后将采样点与其最邻近点连接，确定安全后将采样点插入树中。因此，最邻近点搜索关系到树的生长方向，从而关系到最终路径的质量。本文考虑到研究对象为无人驾驶汽车，结合欧氏距离与车辆的转向能力生成度量函数，如图8所示。n1到nrand的欧式距离比n2到nrand的欧氏距离小，但n2到nrand朝向角的变化更小，得到的路径将更平缓，因此，本文将采样点到邻近点之间的度量函数定义为：

式中，D、H分别为欧式距离和角度归一化之后的值；w1=w2=0.5为对应的权重，可以根据实际应用进行设定。

图8 度量基准示意

距离和角度量纲不同，因此将这两个变量进行归一化处理，D、H分别为：

若将采样点与树中节点一一进行度量距离的比较，代价非常大，因此在计算度量距离前，先利用KD树快速搜索欧式距离最近的几个邻近点作为最邻近点的备选点，然后从备选点中选择式（10）最小的点作为最邻近点，有效提高最邻近点的搜索效率。

3.5 碰撞检测

通过环境地图与车辆构型做卷积校验的碰撞检测方法不受限于环境的复杂度，普适性较强，因此本文采用此方法进行碰撞检测，将车辆构型为若干半径为r的圆形，如图9所示。另外，为了保证安全，将根据实际车辆的尺寸加上安全阈值rerr（本文设为0.1 m）。判断每个圆形在环境地图中所占据的栅格是否安全，有1个圆形不安全则判定此检测点不安全。

图9 车辆构型示意

圆形相关参数为：

在几何分析部分需要对控制线段进行碰撞检测，为了在保证安全的情况下尽可能少地检测，将相邻检测点间的距离设置为车长，一旦线段上有检测点与障碍物发生碰撞，就将此线段及后续线段从初始化边界集中除去。在采样部分需要对采样点进行碰撞检测，检测结果安全的采样点才被加入树中。

3.6 应对搜索未成功的情况

RRT算法具有概率完备性，因此一定存在搜索不成功的情况。采样策略具有目标偏向性，因此即使搜索没有成功，也可将已搜到的路径输出。在之后的规划周期中，不断接收实时地图检测路径安全性的同时，从上一次输出的路径终点出发搜索到目标点的路径。如果依然没有搜索成功，则利用最邻近点搜索策略，搜索已有树中距离目标点最近的树节点作为前一段路径的终点，逆向搜索得到这一段路径的控制点。

3.7 RRT的修剪

由于RRT算法搜索具有随机性，因此若直接根据回溯得到的RRT生成路径，路径必会非常曲折，很可能无法满足车辆的最大曲率约束，路径长度也会出现不必要的增加。可通过将树中安全的节点删去的方式对其进行剪裁。如图10所示，若从n1→n2到n7→n8中间都是安全的，则可以将节点从n2到n7都省略，直接将n1与n8连接，同理，可将n8与n10连接。当修剪后的树在生成曲线时不满足车辆的最大曲率约束时，可根据式（5）插入控制点。

图10 RRT的修剪示意

4 仿真与试验验证

本文针对避障和转向工况分别进行仿真和实车试验，所涉及的车辆参数如表1所示。

表1 车辆参数

仿真与实车试验中速度设定为满足最大车速以及最大侧向加速度约束的最大速度：

4.1 仿真验证

为了证明FB-RRT算法的实时性和安全性，本文将只采用随机采样的基本RRT算法、RRT*算法以及同样带有导向性采样的Goal-baising RRT算法作为对比算法，设Goal-biasing RRT算法中目标状态被扩展的比率为10%。整个算法用C++语言编写，仿真验证在处理器是2.60 GHz Intel®Core™ i7-6500U，7.9 GB内存的笔记本电脑上完成，分别仿真1 000次，考察算法的平均表现。

4.1.1 避障/换道工况

不同算法的对比结果如表2所示。由表2可知，FB-RRT算法在时间效率和成功率上都明显优于其余3种算法。由于FB-RRT算法会对最终生成的RRT进行修剪，因此在路径长度上FB-RRT算法与其余3种算法相差不多，甚至更短。RRT*由于每次新加入节点时都要搜索最佳父节点，加入节点后还要对已有的RRT进行重新连接，因此耗时较长。另外，考虑到车辆转向系统的限制，FB-RRT最终搜索到的树都需要在满足3.2节所述曲率约束的条件上生成路径，因此成功生成满足曲率约束的路径概率高于其余3种算法。

表2 避障工况算法对比结果

避障/换道工况下，为了更快搜索到终点，将采样区域固定在一个扇形区域内，如图11a所示。该区域的中心线是假设以起始点和目标点为端点的线段，此处设扩展步长d=2 m，图11b为采样结果。

图11 避障工况采样示意

图12所示为多障碍物的换道仿真结果。车辆前方12 m、距目标点15 m处各有一静止障碍物，车辆换道时须先躲避障碍物并回到目标车道。由图12a可见，路径能够保证安全约束；由图12b可见路径曲率连续，并保证在所限制的曲率范围之内。

图12 避障工况仿真

4.1.2 转向工况

转向工况下，曲线的曲率和车辆航向角变化相对避障/换道工况更剧烈。表3所示为算法对比结果，可以看出，FB-RRT算法在成功率、算法效率和路径长度上都要优于另外3种算法。

表3 转向工况算法对比结果

转向工况下，根据循环采样的次数，可将采样过程分为3个阶段，如图13a所示：初始阶段车辆需要进入路口，因此采样点集中在细长的A区域，获得充分采样后，车辆从路口进入目标车道，采样点集中在扇形区域B内，该区域的采样参考点nmid是初始航向所确定直线和目标航向所确定直线的交点，区域中心线是nmid和ngoal的连线。这样的两段采样理论上能够连接初始点和终止点。但如果环境中障碍物较多，RRT需向其他方向扩展以避开障碍物，因此一定比例地将范围较广的区域C作为采样区域，该扇形的半径为ninit到ngoal的距离，其中一个扇形边界平行于ngoal的航向，另一个边界和第1阶段的扇形边界重合。转向工况下设置步长为1 m以避免曲率超出边界约束。从图13b的采样效果可见，采样点均出现在预设的采样区域中，在第2段采样区域末尾，采样点几乎可以拟合为一条曲线，可见强制扩展策略奏效。

图13 转向工况采样示意

图14所示为多障碍转向工况仿真环境。起点(0,0)处左方2个车道为逆向车道，路径起点处距离路口车道停止线3.5 m，同理，终点处下方2个车道也是逆向车道，设置终点处距离路口车道停止线2 m，路口处人行横道宽为5 m。在起点处对面的反向直行车道上有一辆障碍车停在左上方，其尾部与目标车道的一条车道线齐平。车辆在转向时需要先避开障碍车，然后进入目标车道。

图14 转向工况仿真

由仿真图14a可见，路径可以满足安全约束。车辆在转向条件不理想的情况下有可能将转向盘转至极限位置，因此图14b中曲线的曲率虽然达到了车辆的最大曲率，但依然符合实际情况。

4.2 实车试验验证

试验平台为一辆改装智能车，如图15所示。通过控制2个前轮电机的力矩实现线控驱动，控制转向系统的驱动电机力矩实现线控转向，以及控制一套电子液压制动系统实现线控制动。

图15 无人驾驶试验平台

试验平台所配备的传感器以及控制器见表4。

表4 试验平台相关配置

本文所有程序都在工控机中完成，不同模块间借助轻量级通信与数据封送库（Lightweight Communications and Marshalling，LCM）进行通信。环境感知模块根据传感器探测到的信号，实时建立一个车辆坐标系下350格×350格的栅格环境地图，栅格边长为20 cm。得到的环境地图被封装在LCM通讯包中传递给运动规划模块。运动规划模块通过GPS获得车辆位置进行规划，并将规划结果也封装在LCM通讯包中传递给轨迹跟踪模块。

基于4.1节的仿真环境，构造了图16所示的实车试验环境，避障工况下在道路两侧分别设置路障以构造多障碍环境，转向工况为车辆从双车道路口左转弯进入匝道。图17所示为试验结果，图17a所示的避障工况中右侧切换点为换道起点，可见路径是安全的，并且期望路径与实际路径几乎完全重合，效果较理想，图17b中车辆跟踪期望路径效果不如避障工况，这是因为本文是基于阿克曼转向模型进行规划，所考虑的动力学特性是最简单的稳态特性，但实际上车辆的动力学特性非常复杂，因此在大曲率情况下跟踪期望路径存在偏移，但车辆依然能满足功能需求，安全转向。

图16 试验场景

图17 实车跟踪效果

5 结束语

运动规划在无人驾驶研究问题中衔接着感知和控制，是使无人驾驶成为完整系统的关键技术。本文提出的FB-RRT规划算法不仅利用了城市环境下结构化道路的特点来加速算法，同时保留了RRT算法处理复杂环境的能力，也满足了无人驾驶汽车的曲率连续有界约束。仿真和实车试验同时表明，该算法能有效解决无人驾驶汽车在城市化道路下的复杂运动规划问题。另外，本文暂未涉及对轨迹性能的评价，后续研究将对轨迹的避障性能、稳定性、舒适性等方面进行评价研究。本文提出的算法目前致力于解决低速工况下的运动规划问题，下一步将尝试解决高速运动规划问题，进一步提高算法的鲁棒性和普适性，更好地满足实际工程需要。