# An Improved RRT-Based Motion Planning Algorithm for Autonomous Vehicle in Urban Environment

## Yu Zhuoping, Wei Ye, Xiong Lu, Li Yishan

Automobile Technology - - CONTENTS -

（同济大学，上海 201804）

【摘要】为解决城市结构化道路环境下的无人驾驶汽车运动规划问题，针对换道和转向两种工况设计了快速偏向性RRT（FB-RRT）算法。首先以几何分析所得的安全的曲线控制点初始化RRT，随后以驾驶行为为引导并结合决策给出的目标点设计偏向性采样策略，基于车辆运动学特性设计度量基准，设计快速邻近点搜索策略，并结合环境地图保证扩展节点的安全性。最后，对RRT进行修剪和重构。通过仿真和实车测试验证了该算法的有效性和实用性。

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 前言

Rapidly- exploring Random Tree，以快速随机扩展树[3（]

RRT）

RRT RRT

RRT性，但每次搜索的结果随机性较大；其次，标准 算法均匀随机采样过程中大量的碰撞检测严重影响效率；另外，车辆构型空间中的度量距离没有闭合形式表达式，计算困难。国内外的学者针对这些缺陷提出了一

Kuffner些解决办法。美国斯坦福大学的 和爱荷华州立

LaValle RRT-Connect

RRT棵分别起于初始状态和目标状态的树，以加快 搜

LaValle Goal- biasing RRT

Shkolnik Walter标状态扩展。英国剑桥大学的 和 等人[6]

RG-RRT（Reachability Guided RRT）

RRT

Cheng[ RC- RRT（Resolution Complete诺伊大学的 7]提出

RRT）算法降低障碍物周边节点获得扩展的概率，从而加速碰撞检测。尽管各种改进措施能有效提高求解质量，但仍难保证获得最优解，于是近年来出现了许多求

RRT*[ LBT-RRT渐进最优解的算法，如 8]、 算法[9]等。另外，也有部分研究人员仅采用几何曲线进行路径

B

RRT（Fast- Biasing划问题，提出了一种快速偏向性

RRT，FB-RRT）算法。该算法首先基于几何分析结果初

RRT，

KD为设计了不同的偏向性采样方法，并结合 树加速邻

RRT B近点搜索。搜索成功后对 进行裁剪重构并利用样条曲线平滑路径。最后，通过仿真以及实车试验验证了该算法的有效性和实用性。

2 车辆模型建立

1型为常见的具有此种性质的车辆模型，如图 所示。 3该模型将车辆视为平面刚体，具有 个自由度，选

1

( X,Y)， ϕ为前轮转角，车辆在大地坐标系下的坐标为 横摆角为θ。车辆状态方程为：

Ẋ = · cosθ ì v

ï Ẏ = · sinθ 1） í v （ ï θ̇ = î vκ式中，为车速；为转弯曲率。v κ车辆模型受动力学约束，因此需满足：

3 快速偏向性RRT算法3.1 基本RRT算法

RRT S. M. LaValle

3.2 RRT的初始化

RRT

RRT，

RRT

3.2.1 B

B B

( n+ 1)

={ ≤

+

=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

1， ∈( i, ì ) t tt

1( t) = +1 N i

i, 0，

B

3

Lmin满足

sinαmin 1- cosαmin

- 1.5

Lmin = 16·

κmax 8 5）

>αmin，

RRT

3.2.2

3 4

4

l2 = sin( + θ3) 6） í α （ ï y3 - l2 · sinα

l3 = sinθ3 î ( x1,y1) ( x2,y2) ( x3,y3)

3.2.3

5

ì θ +( 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

RRT

3.3.1

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)

( r0,θ0) ( σr,σθ)，的性质，可以设置合适的 和 引导搜索向着目标点的方向进行。

3.3.2

3.2

RRT

7境中的搜索能力。整体采样策略如图 所示，其中， r1、r2

RRT d

3.4 最邻近点搜索策略

8

dist （式中， DH、 分别为欧式距离和角度归一化之后的值；

w1=w2= 0.5

8

KD

10）

3.5 碰撞检测

9

。判断每个

1

3.6 应对搜索未成功的情况

RRT

3.7 RRT的修剪

RRT

RRT

10 n1→n2 n7→n8

5）

4 仿真与试验验证

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

12 m 15 m

、距目标点 处各有一静止障碍物，车辆换道时

12a

12b

/ 3

FB- RRT

3

3 13a

RRT

13b

2

2

3.5 m， 2

2 m，

5m

14a

4.2 实车试验验证

15

Lightweight Communications

and Marshalling，LCM）

350 ×

350 20 cm

LCM

GPS

LCM

4.1 16

17 17a

17b

5 结束语

FB- RRT

RRT