An Improved RRT-Based Motion Planning Algorithm for Autonomous Vehicle in Urban Environment
Yu Zhuoping, Wei Ye, Xiong Lu, Li Yishan
余卓平 卫烨 熊璐 李奕姗
（同济大学，上海 201804）
【摘要】为解决城市结构化道路环境下的无人驾驶汽车运动规划问题，针对换道和转向两种工况设计了快速偏向性RRT（FB-RRT）算法。首先以几何分析所得的安全的曲线控制点初始化RRT，随后以驾驶行为为引导并结合决策给出的目标点设计偏向性采样策略，基于车辆运动学特性设计度量基准，设计快速邻近点搜索策略，并结合环境地图保证扩展节点的安全性。最后，对RRT进行修剪和重构。通过仿真和实车测试验证了该算法的有效性和实用性。
主题词：无人驾驶汽车 运动规划 偏向性采样 邻近点搜索U461 A 10.19620/j.cnki.1000-3703.20180524中图分类号： 文献标识码： DOI:
An Improved RRT-Based Motion Planning Algorithm for Autonomous Vehicle in Urban Environment
Yu Zhuoping, Wei Ye, Xiong Lu, Li Yishan Tongji University, Shanghai 201804） （ Abstract To solve the problems associated with motion planning of autonomous vehicle in urban environment, a new【 】motion planning algorithm named Fast-Biasing RRT (FB-RRT) algorithm was designed for lane changing and turning. First, the safe control points of the curve generated from geometric analysis were used to initialize RRT. Then a biasing sampling strategy was designed based on driving behaviors as guidance combined with decision- making. Besides, a fast NN (the Nearest Node) search strategy was designed using the distance measurement baseline combined with vehicle’s kinematic property. Combined with the environment map, the security of the extended node was guaranteed. At last, RRT was trimmed and reconstructed. Simulation experiments and road test verify validity and practicability of this algorithm.
Key words: Autonomous vehicle, Motion planning, Biasing sampling, Nearest node search
1 前言
运动规划是无人驾驶汽车的关键技术，是在决策给出目标点的位姿信息后，利用车载传感器获得自车状态以及周围环境信息，在满足运动系统微分约束的基础上，及时生成一条指导汽车在接下来短时间内运动的安全路径。运动规划最初开始于机器人领域，车辆可看成是一种特殊的轮式机器人，因此车辆的运动规划算法大多由机器人领域的规划算法发展而来。较有代表性的算法有两大类：基于网格搜索的算法和基
A*[ AD*于随机采样的算法。基于网格搜索的算法如 1]、算法[2]，能保证找到可行解，后者还能在动态障碍物环境下应用，但生成安全、可行的路径需要较精确的环境地图以及成熟的代价确定准则。基于随机采样的算法
Rapidly- exploring Random Tree，以快速随机扩展树[3（]
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[ RC- RRT（Resolution Complete诺伊大学的 7]提出
RRT）算法降低障碍物周边节点获得扩展的概率，从而加速碰撞检测。尽管各种改进措施能有效提高求解质量，但仍难保证获得最优解，于是近年来出现了许多求
RRT*[ LBT-RRT渐进最优解的算法，如 8]、 算法[9]等。另外，也有部分研究人员仅采用几何曲线进行路径
B
生成，如螺旋曲线[10]、样条曲线[11]、贝塞尔曲线[12]等，但通常需要在生成路径时与障碍物进行繁琐复杂的几何分析，以保证路径无碰撞，因此效率较低。本文针对城市结构化道路环境下的无人车运动规
RRT（Fast- Biasing划问题，提出了一种快速偏向性
RRT，FB-RRT）算法。该算法首先基于几何分析结果初
RRT，
始化 随后针对城市工况下最常见的换道和转向行
KD为设计了不同的偏向性采样方法，并结合 树加速邻
RRT B近点搜索。搜索成功后对 进行裁剪重构并利用样条曲线平滑路径。最后，通过仿真以及实车试验验证了该算法的有效性和实用性。
2 车辆模型建立
无人车的运动规划问题是要求找出从起始点到终止区域的一条可行路径上的控制输入，也就是说需要先规划运动的轨迹，再根据轨迹输出和系统状态变量的各阶导数求出系统状态变量和控制输入。阿克曼转向模
1型为常见的具有此种性质的车辆模型，如图 所示。 3该模型将车辆视为平面刚体，具有 个自由度，选
1
取后轴中心点R作为参考点。图 中， b为车辆的轴距，
( X,Y)， ϕ为前轮转角，车辆在大地坐标系下的坐标为 横摆角为θ。车辆状态方程为：
Ẋ = · cosθ ì v
ï Ẏ = · sinθ 1） í v （ ï θ̇ = î vκ式中，为车速；为转弯曲率。v κ车辆模型受动力学约束，因此需满足：
式中， ϕmax 为最大前轮转角； vmax 为最大车速； κmax 为最大转弯曲率。
3 快速偏向性RRT算法3.1 基本RRT算法
RRT S. M. LaValle
算法由 提出[3]，是一种基于随机采样的增量式运动规划算法，属于典型的树状结构搜索算法，通过在状态空间中不断搜索采样生成一棵树，整棵树包括树上所有节点组成的节点集N和所有“树枝” 2
组成的边界集E，扩展过程如图 所示。
整棵树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)
样条曲线是样条曲线的一种特殊表示形式，由 个
={ ≤
+
控制点 P} 及参数节点向量 t t t 确定
=0 =0 +1
( k- 1) B
的 次 样条曲线为：
Σ n = ( t), ∈[ 1, ] 3） P( t) PN t t t （
i i, k k - n +1
i =0
k( t) ( k- 1) B deBoox-Cox
式中， Ni, 为Tn, 上 次 样条基函数，由k递推关系[14]确定：
1， ∈( i, ì ) t tt
1( t) = +1 N i
i, 0，
其它 4） í （ - =- t tN t t + = 0,1,…, ( t) t) +1 t)，i N i i n - 1( - tN + 1, - 1( i, k t i, k t i k î + -1 + +1 i k i k i
B
样条曲线具有连续性、凸包性和局部性等优点，曲线控制点个数和阶数没有必然联系，不需要复杂的数值计算。另外，通过限定控制点连线间的夹角可将曲线
3
的曲率维持在一定界限内，如图 所示。 控制点A1、O、B1决定的线段夹角的最小值αmin与曲率最大值以及控制点连线的最短线段长度 [11]：
Lmin满足
sinαmin 1- cosαmin
- 1.5
Lmin = 16·
（
κmax 8 5）
>αmin，
只要α 即可满足曲率要求。为使最终路径较好地贴合控制线段，将控制线段的中点 A2、B2也纳入
RRT
的初始节点集。
3.2.2
换道工况给定初始点和目标点的位姿后，换道工况可以简单
3 4
抽象为车辆坐标系中的 条控制线段，如图 所示。 3
假设 条线段长度分别为l1、l2和l3，总长度为L，经
4
图 所示几何分析得线段长度关系为： ì l1 = x1 ï y3 · cosθ3 -( x3 - x1) sinθ3
l2 = sin( + θ3) 6） í α （ ï y3 - l2 · sinα
l3 = sinθ3 î ( x1,y1) ( x2,y2) ( x3,y3)
式中， 、 、 分别为车辆坐标系下线段交点X1、X2以及终点X3的坐标。借助优化工具，以总路径长度L最短为优化目标即 3 7
可得到 段线段的长度，通过计算即可得 个控制点的坐标。
3.2.3
转向工况为使曲线更接近于转向工况时的圆弧曲线，设定控
5
制线段长度及控制线段间的夹角都相同，如图 所示。 假设从初始点位姿变换到目标点位姿共需n条长为l的控制线段，相邻控制线段间夹角均为α。根据边界条件可得约束方程：
ì θ +( n - 1)(π- α)= θg ï i
Σ n - 1( ))
x + l · cos( θ + i( π- α) = xg 7） í i i （
i =0 ï Σ n - 1( ))
y + l · sin( θ + i( π- α) = yg i i
î =0 i ( xg,yg,θg) ( xi,yi,θi)式中， 为终点状态； 为各控制点的状态。
7） 3
根据式（ 即求解得到 个未知量n、、l α，控制点的坐标即可求出。3.3 采样策略随机采样直接关系到树的扩展质量和搜索效率，本节结合决策给出的驾驶行为设计偏向性采样策略，引导
RRT
的扩展。
3.3.1
目标偏向性采样采样过程中概率分布函数最为重要。本文采用常见的高斯分布，便于根据已知条件进行偏向性采样。高斯分布遵循的采样策略为： = x0 + · cosθ
s r x = y0 + · sinθ 8）
s r （ y
( x0,y0) ( sx,sy) ( r,θ)
式中， 、 分别为参考采样点和采样点坐标；为高斯分布决定的采样相对值：
= r σ rrand + r0 r 9）
= （ θ σ θrand + θ0 θ
( r0,θ0) ( x0,y0)
式中， 为相对于 的补偿参数，即高斯分布的( σr,σθ) ( rrand,θrand)
均值； 为对应的高斯分布标准差； 为随机变量。因此可以通过高斯采样将采样点限制在一个环形6
区域内，如图 所示。针对决策给出的驾驶行为和目标点，结合高斯采样
( r0,θ0) ( σr,σθ)，的性质，可以设置合适的 和 引导搜索向着目标点的方向进行。
3.3.2
强制扩展策略所谓强制扩展，就是按照一定比例将目标点和树中距离目标点最近的节点利用一定的方法连接，随后结合碰撞检测判断路径的安全性，并将路径上有效部分插入到现有ε的树中，从而提高搜索效率。因此本文利用 贪婪算法，按
3.2
照一定比例采用 节所述几何分析法进行强制扩展。
偏向性采样虽然加快了搜索进程，但同时也限制了
RRT
算法的探索能力，因此，本文按照一定比例采用普通采样策略进行采样，保证了算法在障碍物较密集的环
7境中的搜索能力。整体采样策略如图 所示，其中， r1、r2
为随机数。
RRT d
除了采样策略， 的扩展步长 也直接影响了搜索速度。本文的扩展步长根据驾驶行为选择：在换道时，根据初始点和目标点的距离来选择不同的扩展步长，如果距离较大，选用较大步长加快搜索，反之选用较小步长提高成功率；在转向时，路径的曲率变化较大，因此采用较小的扩展步长以保证最终搜索结果的质量，防止采样步长过大导致路径的曲率不满足要求。
3.4 最邻近点搜索策略
利用采样策略得到安全的采样点后，需要在已有的树中寻找“距离”采样点最近的节点，然后将采样点与其最邻近点连接，确定安全后将采样点插入树中。因此，最邻近点搜索关系到树的生长方向，从而关系到最终路径的质量。本文考虑到研究对象为无人驾驶汽车，结合
8
欧氏距离与车辆的转向能力生成度量函数，如图 所示。n1到 nrand的欧式距离比n2到 nrand的欧氏距离小，但n2到 nrand朝向角的变化更小，得到的路径将更平缓，因此，本文将采样点到邻近点之间的度量函数定义为： = w1D + w2H 10）
dist （式中， DH、 分别为欧式距离和角度归一化之后的值；
w1=w2= 0.5
为对应的权重，可以根据实际应用进行设定。
8
图 度量基准示意距离和角度量纲不同，因此将这两个变量进行归一化处理， DH、分别为：
若将采样点与树中节点一一进行度量距离的比较，
KD
代价非常大，因此在计算度量距离前，先利用 树快速搜索欧式距离最近的几个邻近点作为最邻近点的备
10）
选点，然后从备选点中选择式（ 最小的点作为最邻近点，有效提高最邻近点的搜索效率。
3.5 碰撞检测
通过环境地图与车辆构型做卷积校验的碰撞检测方法不受限于环境的复杂度，普适性较强，因此本文采用此方法进行碰撞检测，将车辆构型为若干半径为 的
9
圆形，如图 所示。另外，为了保证安全，将根据实际车辆的尺寸加上安全阈值re（ rr 本文设为0.1 m）
。判断每个
1
圆形在环境地图中所占据的栅格是否安全，有 个圆形不安全则判定此检测点不安全。
在几何分析部分需要对控制线段进行碰撞检测，为了在保证安全的情况下尽可能少地检测，将相邻检测点间的距离设置为车长，一旦线段上有检测点与障碍物发生碰撞，就将此线段及后续线段从初始化边界集中除
去。在采样部分需要对采样点进行碰撞检测，检测结果安全的采样点才被加入树中。
3.6 应对搜索未成功的情况
RRT
算法具有概率完备性，因此一定存在搜索不成功的情况。采样策略具有目标偏向性，因此即使搜索没有成功，也可将已搜到的路径输出。在之后的规划周期中，不断接收实时地图检测路径安全性的同时，从上一次输出的路径终点出发搜索到目标点的路径。如果依然没有搜索成功，则利用最邻近点搜索策略，搜索已有树中距离目标点最近的树节点作为前一段路径的终点，逆向搜索得到这一段路径的控制点。
3.7 RRT的修剪
RRT
由于 算法搜索具有随机性，因此若直接根据
RRT
回溯得到的 生成路径，路径必会非常曲折，很可能无法满足车辆的最大曲率约束，路径长度也会出现不必要的增加。可通过将树中安全的节点删去的方式对其
10 n1→n2 n7→n8
进行剪裁。如图 所示，若从 到 中间都是安全的，则可以将节点从n2到n7都省略，直接将n1与n8连接，同理，可将n8与n10连接。当修剪后的树在生成曲
5）
线时不满足车辆的最大曲率约束时，可根据式（ 插入控制点。
4 仿真与试验验证
本文针对避障和转向工况分别进行仿真和实车试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
满足曲率约束的路径概率高于其余 种算法。 /
避障换道工况下，为了更快搜索到终点，将采样区11a
域固定在一个扇形区域内，如图 所示。该区域的中心线是假设以起始点和目标点为端点的线段，此处设扩d= 2 m， 11b
展步长 图 为采样结果。 12
图 所示为多障碍物的换道仿真结果。车辆前方
12 m 15 m
、距目标点 处各有一静止障碍物，车辆换道时
12a
须先躲避障碍物并回到目标车道。由图 可见，路径
12b
能够保证安全约束；由图 可见路径曲率连续，并保证在所限制的曲率范围之内。 4.1.2
转向工况转向工况下，曲线的曲率和车辆航向角变化相对避
/ 3
障换道工况更剧烈。表 所示为算法对比结果，可以看
FB- RRT
出， 算法在成功率、算法效率和路径长度上都
3
要优于另外 种算法。 转向工况下，根据循环采样的次数，可将采样过程
3 13a
分为 个阶段，如图 所示：初始阶段车辆需要进入路口，因此采样点集中在细长的A区域，获得充分采样后，车辆从路口进入目标车道，采样点集中在扇形区域B内，该区域的采样参考点nmid是初始航向所确定直线和目标航向所确定直线的交点，区域中心线是nmid和 ngoal的连线。这样的两段采样理论上能够连接初始点和终止
RRT
点。但如果环境中障碍物较多， 需向其他方向扩展以避开障碍物，因此一定比例地将范围较广的区域C作为采样区域，该扇形的半径为ninit到 ngoal的距离，其中一个扇形边界平行于ngoal的航向，另一个边界和第1
阶段的 1m
扇形边界重合。转向工况下设置步长为 以避免曲率
13b
超出边界约束。从图 的采样效果可见，采样点均出
2
现在预设的采样区域中，在第 段采样区域末尾，采样点几乎可以拟合为一条曲线，可见强制扩展策略奏效。 14 (0,0)
图 所示为多障碍转向工况仿真环境。起点
2
处左方 个车道为逆向车道，路径起点处距离路口车道
3.5 m， 2
停止线 同理，终点处下方 个车道也是逆向车道，
2 m，
设置终点处距离路口车道停止线 路口处人行横道
5m
宽为 。在起点处对面的反向直行车道上有一辆障碍车停在左上方，其尾部与目标车道的一条车道线齐平。车辆在转向时需要先避开障碍车，然后进入目标车道。
14a
由仿真图 可见，路径可以满足安全约束。车辆在转向条件不理想的情况下有可能将转向盘转至极限14b
位置，因此图 中曲线的曲率虽然达到了车辆的最大曲率，但依然符合实际情况。
4.2 实车试验验证
15
试验平台为一辆改装智能车，如图 所示。通过2
控制 个前轮电机的力矩实现线控驱动，控制转向系统的驱动电机力矩实现线控转向，以及控制一套电子液压制动系统实现线控制动。 本文所有程序都在工控机中完成，不同模块间借助
Lightweight Communications
轻量级通信与数据封送库（
and Marshalling，LCM）
进行通信。环境感知模块根据传
350 ×
感器探测到的信号，实时建立一个车辆坐标系下 格
350 20 cm
格的栅格环境地图，栅格边长为 。得到的环境
LCM
地图被封装在 通讯包中传递给运动规划模块。运动
GPS
规划模块通过 获得车辆位置进行规划，并将规划结
LCM
果也封装在 通讯包中传递给轨迹跟踪模块。
4.1 16
基于 节的仿真环境，构造了图 所示的实车试验环境，避障工况下在道路两侧分别设置路障以构造多障碍环境，转向工况为车辆从双车道路口左转弯进入匝
17 17a
道。图 所示为试验结果，图 所示的避障工况中右侧切换点为换道起点，可见路径是安全的，并且期望路
17b
径与实际路径几乎完全重合，效果较理想，图 中车辆跟踪期望路径效果不如避障工况，这是因为本文是基于阿克曼转向模型进行规划，所考虑的动力学特性是最简单的稳态特性，但实际上车辆的动力学特性非常复 杂，因此在大曲率情况下跟踪期望路径存在偏移，但车辆依然能满足功能需求，安全转向。
5 结束语
运动规划在无人驾驶研究问题中衔接着感知和控制，是使无人驾驶成为完整系统的关键技术。本文提出
FB- RRT
的 规划算法不仅利用了城市环境下结构化道
RRT
路的特点来加速算法，同时保留了 算法处理复杂环境的能力，也满足了无人驾驶汽车的曲率连续有界约束。仿真和实车试验同时表明，该算法能有效解决无人驾驶汽车在城市化道路下的复杂运动规划问题。另外，本文暂未涉及对轨迹性能的评价，后续研究将对轨迹的避障性能、稳定性、舒适性等方面进行评价研究。本文提出的算法目前致力于解决低速工况下的运动规划问题，下一步将尝试解决高速运动规划问题，进一步提高算法的鲁棒性和普适性，更好地满足实际工程需要。