This article has been accepted for inclusion in a future issue of this journal.

Content is final as presented, with the exception of pagination.

IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS 1

Efficient Sampling-Based Motion Planning for

On-Road Autonomous Driving
Liang Ma, Jianru Xue, Member, IEEE, Kuniaki Kawabata, Member, IEEE,
Jihua Zhu, Chao Ma, and Nanning Zheng, Fellow, IEEE

Abstract—This paper introduces an efficient motion planning For example, new transportation service in the city, personal
method for on-road driving of the autonomous vehicles, which supporting service for elderly people and persons with hand-
is based on the rapidly exploring random tree (RRT) algorithm. icaps, and logistics for delivery service are typical significant
RRT is an incremental sampling-based algorithm and is widely
used to solve the planning problem of mobile robots. However, applications.
due to the meandering path, the inaccurate terminal state, and Motion planning is an important and fundamental technology
the slow exploration, it is often inefficient in many applications of autonomous vehicles [2], which can solve the problem of
such as autonomous vehicles. To address these issues and con- computing a sequence of control values or feasible movement
sidering the realistic context of on-road autonomous driving, we states for the vehicle to maneuver among obstacles from an
propose a fast RRT algorithm that introduces a rule-template set
based on the traffic scenes and an aggressive extension strategy initial state toward a desired terminal state, taking into account
of search tree. Both improvements lead to a faster and more the vehicle’s kinematic and dynamic model [3]. In addition,
accurate RRT toward the goal state compared with the basic the traffic rules and the geometric structural information of
RRT algorithm. Meanwhile, a model-based prediction postprocess roads are also considered in the motion planning for on-road
approach is adopted, by which the generated trajectory can be autonomous driving.
further smoothed and a feasible control sequence for the vehicle
would be obtained. Furthermore, in the environments with dy- Motion planning problem has been widely studied in the field
namic obstacles, an integrated approach of the fast RRT algorithm of mobile robotics, and many motion planning approaches have
and the configuration-time space can be used to improve the been presented in the robotics literature over the past three
quality of the planned trajectory and the replanning. A large decades [4]. Some well-known planning algorithms, such as
number of experimental results illustrate that our method is fast Dijkstra [5] and A∗ [6], can find the shortest path in an equally
and efficient in solving planning queries of on-road autonomous
driving and demonstrate its superior performances over previous sized grid map representing the environment information. How-
approaches. ever, the choice of the grid resolution and tracking the path
composed of the connected grids are always difficult problems.
Index Terms—Autonomous vehicles, motion planning, on-road
driving, rapidly exploring random tree (RRT). D∗ [7] and its variants [8] also cannot address these weaknesses
of the grid-based algorithm, although they introduce the envi-
ronmental dynamic. The potential field approach [9] can plan a
I. I NTRODUCTION path using the repulsive forces from obstacles and the attractive
forces from the goal. However, the virtual forces trapping in
A S one of six intelligent transportation system major cat-
egories [1], autonomous vehicles were originally devel-
oped in the 1980s. Google driverless car has been allowed
the local minima will make a path that is not always available.
One common drawback of these algorithms is that they cannot
testing and self-driving on Michigan’s roads according to the take the complex dynamic and differential constraints of the
legislation. With the development of the autonomous driving vehicle into consideration easily. Therefore, they are not very
technology, the autonomous vehicle has become one of the key suitable for the application of autonomous vehicle, particularly
issues for supporting our daily life and economical activities. in the context of high-speed or precise motion control for on-
road driving.
Recently, sampling-based techniques for motion planning
have become quite popular and widespread. Two chief families
Manuscript received November 26, 2014; accepted December 22, 2014. of sampling-based techniques are single-query algorithms and
This work was supported in part by the National Natural Science Foundation multiquery algorithms [10]. The single-query algorithms are
of China projects under Grants 91320301 and 61273252. The Associate Editor in general regarded to be better and more suitable for motion
for this paper was L. Li.
L. Ma, J. Xue, J. Zhu, C. Ma, and N. Zheng are with The Institute of Artificial planning of the single robot, because it does not normally invest
Intelligence and Robotics, Xi’an Jiaotong University, Xi’an 710049, China substantial time to preprocess the models and cover the free
(e-mail: mlxjtu@gmail.com; jrxue@mail.xjtu.edu.cn; zhujh@mail.xjtu.edu.cn; space in the same manner that multiquery algorithms would
machao0919@stu.xjtu.edu.cn; nnzheng@mail.xjtu.edu.cn).
K. Kawabata is with The Institute of Physical and Chemical Research [11]. Rapidly exploring random tree (RRT) is a primary single-
(RIKEN), 2-1, Hirosawa, Wako, Saitama, 351-0198, Japan (e-mail: kuniakik@ query algorithm based on incremental sampling, which was first
riken.jp). proposed by Steven in a technical report [12]. Its main advan-
Color versions of one or more of the figures in this paper are available online
at http://ieeexplore.ieee.org. tage is that it is easy to take into account the complex system
Digital Object Identifier 10.1109/TITS.2015.2389215 dynamics by directly using the control set of admissible inputs
1524-9050 © 2015 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission.
See http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

2 IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS

[11], [13]. Hence, RRT was widely used for motion planning of planning method against the whole competition. An improved
mobile robots and manipulations. Nevertheless, there is still a RRT as a real-time online motion planning algorithm was used
large gap between the basic RRT and those applications. Many in the MIT’s autonomous vehicle [29], where an important
RRT variants were proposed for advancing it. RRT-Connect, extension was the use of closed-loop prediction model [30].
a bidirectional version, synchronously searches with two trees Furthermore, Hanyang University’s “A1” won autonomous ve-
from start point and goal point for improving the efficiency hicle competition in Korea. The highlight of their work is the
[14]. This RRT variant was applied to parking of intelligent design of a local path planning approach for the off-road au-
vehicles [15], but the difficulty of using this algorithm is how to tonomous driving [31]. In China, the Intelligent Vehicle Future
deal with the discontinuity existing at the place where two trees Challenge competition has been held for several times, hosted
connect. Execution extended RRT biases search toward goals by the National Natural Science Foundation of China [32].
and waypoints [16], and Urmson and Simmons introduced Some teams attempt to solve the motion planning problems
a heuristic function for biasing tree growth [17]. These bias of autonomous vehicles by the RRT algorithm. For instance,
techniques are very efficient and are used in a wide range an RRT variant is presented by Du et al. [33]. This algorithm
of applications. Rodriguez et al. [18] presented an obstacle- is only applied to the narrow passages scenario. In addition,
based RRT algorithm that can exploit in nine different methods the postprocess operation of the algorithm will possibly make
based on the obstacle geometries. Although the obstacle vectors the RRT algorithm lose its advantage of satisfying the system
cannot always be obtained accurately, selecting various growth dynamic constraints.
methods is a novelty. Subsequently, Denny et al. introduced Easily introducing system dynamics and favorable search
the machine learning method into selecting of growth methods ability make the RRT algorithm fit for solving the motion
[19]. Particle RRT (pRRT) casted the particle filter into the planning problems. Some of the aforementioned applications
RRT’s framework [20], which can select the most promising achieved good performance. However, there exist some weak-
nodes from the distributions of states instead of single states. nesses in the motion planning of autonomous vehicle when
Because pRRT is time consuming, it is used to teleoperate using the RRT algorithm.
a Mars exploration rover rather than a real-time autonomous
vehicle. In 2011, Karaman and Frazolli presented an optimal • The path generated by RRT is often jagged and meander-
RRT (RRT∗ ) that can get an asymptotic optimal path, which the ing, owing to the growth way of the tree. Although a lot
RRT algorithm lacked [21], and is applied to off-road vehicle of works were proposed to smooth the path planned by
maneuvers [22]. The asymptotic optimal solution is obtained at RRT, lessened meanders might still exist in connects of
the cost of consuming much time by a rewire process. In recent two edges. For instance, lane keeping is the most common
years, many works have been done on improving the efficiency maneuver for vehicles on road when without obstacles.
of RRT∗ [23]–[25]. However, the efficiency and trajectory Using the RRT makes it difficult to plan a trajectory
quality of RRT algorithm are still worth being studied. This is shaped like a straight line for the lane keeping.
because the RRT∗ algorithm is to constantly rewire and refine • Whether for RRT or RRT∗ , the accurate terminal configu-
on the basis of the planning results of the RRT algorithm. ration cannot always be reached. Both algorithms extend
Note that autonomous vehicles competitions promoted the by the forward kinematics, and the extension result is that
development of the autonomous driving technology, particu- the new branch grows toward the sample configuration or
larly the application of motion planning technology to autono- the goal configuration, but the new node of this branch
mous vehicles. For example, a series of autonomous vehicles does not necessarily reach those configurations. Thus, one
competitions were organized by the Defense Advanced Re- of the stop conditions of such algorithms is often that
search Projects Agency in the U.S. The winner team “Boss” the tree enters a certain goal region. This is risky, if the
from Carnegie Mellon University in the 2007 Urban Challenge vehicle cannot reach the desired goal state.
applied anytime dynamic A∗ to off-road driving [26] and the • An important requirement of motion planning for au-
model-predictive trajectory generation method to on-road driv- tonomous vehicles is real time, whereas the efficiency
ing [27]. This trajectory generation method takes into account of RRT mainly depends on whether sampling domain
the vehicles’ kinematics and dynamics, resulting in smooth and well adapts to the problem or not. To address this issue,
feasible trajectories, but it does not consider the obstacles in the method in [29] used the Gaussian distributions over
the process of trajectory generation. The obstacle avoidance is sampling domain, whose related parameters were then de-
accomplished in the manner of lane changing by selecting from termined according to the traffic scenes. However, for on-
candidate trajectories, which is probably not able to deal with road environments, can the efficiency of RRT algorithm
the cluttered environments. Furthermore, the error in linearizing be further improved?
the system may lead to the terminal state error of the generated
trajectory. The autonomous vehicle from Stanford University To overcome these shortages presented above, this paper
also applied different approaches to common road navigation focuses on an efficient sampling-based motion planning method
and free-style navigation, including a parallel trajectory planner for on-road autonomous driving, including an improved RRT
and a modified version of A∗ [28]. The parallel trajectory variant, a model-based prediction postprocess, and the inte-
planner with the cumulative cost computed can accomplish the gration with configuration-time space. A large number of ex-
passing on road, but it is unable to handle the cases of blocked perimental results illustrate that the proposed method is faster
roads or intersections. The MIT team proposed a unified motion and more efficient and its planned trajectory can accurately
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

MA et al.: EFFICIENT SAMPLING-BASED MOTION PLANNING FOR ON-ROAD AUTONOMOUS DRIVING 3

III. P ROBLEM F ORMULATION

A. Motion Planning Definition
The motion planning problem is always formulated as in the
following. The state space and the control space of system are
represented by compact sets X ⊂ Rn and U ⊂ Rm , respec-
tively. The time-invariant dynamical system, i.e.,

ẋ(t) = f (x(t), u(t)) x(0) = x0 (1)

where x(t) ∈ X, u(t) ∈ U , and x0 is the initial state. In addi-

tion, the desired goal state is represented as xgoal ∈ X. Denote
Xobs ⊂ X as the regions occupied by obstacles; an obstacle-
free region can be defined as Xf ree ⊂ X\Xobs . The motion
planning task is to compute

x(t) ∈ Xf ree ∀ t ∈ [0, tf ]

Fig. 1. Overall structure of our method. x(tf ) = xgoal

u(t) ∈ U ∀ t ∈ [0, tf ] (2)
reach the desired configuration compared with other related where x is called the trajectory, and x(t) is the reachable state
algorithms. The meanders of trajectory can also be eliminated of system; u is called the control input sequence, and u(t) is
in most cases. the feasible control variable for the system. When the control
The rest of this paper is organized as follows. Section II input is u(tf ) at time t = tf , the system can reach the goal state
presents the framework overview of our method. Section III xgoal .
introduces the problem formulation of motion planning with
respect to the autonomous vehicle. Section IV, after reviewing
B. The Vehicle Model
the basic RRT algorithm, describes the fast RRT algorithm
in detail. Section V analyzes the existing problems in the re- Simplified from a front-wheel steering Ackerman vehicle,
planning process of motion planning algorithms and introduces the bicycle model is employed, because it is suitable for our
an integrated approach of the fast RRT and the configuration- autonomous vehicle prototype KuaFu-1, which is developed for
time space. Section VI discusses the experiments and results, the Intelligent Vehicle Future Challenge competition in China,
including a large number of statistical experiments in different as shown in Fig. 2. Since the main task is to realize self-drive on
scenes and maneuvers. Section VII concludes this paper. road in urban environments, its motion planning problem is con-
sidered in the 2-D plane. The kinematic model is described by
⎡ ⎤ ⎡ ⎤ ⎡ ⎤
II. OVERVIEW OF THE F RAMEWORK ẋ cos(θ) 0
⎢ ẏ ⎥ ⎢ sin(θ) ⎥ ⎢ ⎥
The overall structure of our method is illustrated in Fig. 1. ⎢ ⎥ = ⎢ tan(θ) ⎥ v + ⎢ 0 ⎥ δ̇ (3)
⎣ θ̇ ⎦ ⎣ ⎦ ⎣0⎦
Our method is designed for the motion planning module of L
δ̇ 0 1
on-road autonomous driving, which receives a short-term goal
state and environmental information from high-level modules, where x and y denote the coordinates of the center point of
scenes analysis and understanding module, and detection mod- the rear axle, θ is the vehicle heading angle with respect to
ule, respectively. The output of our method is a smooth tra- the x-axis, and L is the vehicle wheelbase. As control input
jectory and a feasible control sequence, which are sent to the parameters to vehicle, v and δ̇ are the longitudinal velocity
vehicle’s actuator, aimed at controlling the vehicle to reach the and the angle velocity of the steering wheel, respectively.
goal state without collisions. The core of our method is an Moreover, some important boundary parameters derived from
improved RRT variant, which is named fast RRT algorithm. the prototype are used to constrain the vehicle model as
Based on previous work [29], it introduces a rule-template
set (detailed in Section IV-C) in terms of the traffic scenes L = 2.790 (m)
and an aggressive extension strategy (detailed in Section IV-D)
vmin = 0 (m/s) vmax = 12 (m/s)
of search tree [34]. A model-based prediction postprocess
(detailed in Section IV-E), a closed-loop prediction approach, δmin = −0.5236 (rad) δmax = 0.5236 (rad)
is adopted for further smoothing and computing the trajectory
amin = −5.0 (m/s2 ) amax = 0.9 (m/s2 )
and control sequence for the vehicle. To address the motion
planning problem under the environments with dynamic obsta- δ̇min = −0.2183 (rad/s) δ̇max = 0.2183 (rad/s) (4)
cles, the fast RRT algorithm and the configuration-time space
are integrated (detailed in Section V ) to improve the quality of where a is the forward acceleration, and δ represents the
the planned trajectory and the replanning. steering wheel angle.
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

4 IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS

Algorithm 1 Fast RRT algorithm for Autonomous Vehicles

fRRTmain()
1: templatei ← L OAD T EMPLATE(xgoal , T emplateSet);
2: Ttemp ← D ECOMPOSE C URVE(templatei );
3: Ttrimtemp ← T RIM T REE(Ttemp );
4: T .AddSubTree(Ttrimtemp );
5: if success ==RUSH T OWARDS G OAL(T , xgoal ) then
6: return T ;
7: end if
8: for k = 1 to K do
9: if Random(0, 1) < λss then
10: if success ==RUSH T OWARDS G OAL(T , xgoal )
then
11: return T ;
12: end if
13: else
14: xrand ← G ET R ANDOM S AMPLE(xgoal );
15: xnear ← G ET N EAREST N EIGHBOR(T , xgoal );
16: xnew ← E XTEND(T , xnear , xrand );
17: if C OLLISION F REE D ETECTION(xnear , xnew )
then
18: T .AddVertex(xnew );
19: T .AddEdge(xnear , xnew );
20: end if
21: end if
22: end for
Fig. 2. Our autonomous vehicle prototype and its geometric model.
(a) KuaFu-1: autonomous vehicle of Xi’an Jiaotong University. (b) Autonomous RUSH T OWARDS G OAL(T , xgoal )
vehicle’s bicycle model. 1: xnear ← G ET N EAREST N EIGHBOR(T , xgoal );
2: Pnear2goal ← G ENERATE PATH(xnear , xgoal );
In this paper, the vehicle model is used in four different as- 3: T2goal ← D ECOMPOSE C URVE(Pnear2goal );
pects: 1) the generation of the rule templates; 2) the extension of 4: if C OLLISION F REE D ETECTION(T2goal ) then
the search tree; 3) the trajectory generation using the aggressive 5: T .AddSubTree(T2goal );
extension strategy; 4) the closed-loop simulation in the model- 6: return success;
based prediction stage. 7: else
8: Ttrim2goal ← T RIM T REE(T2goal );
IV. A LGORITHM D ESCRIPTION 9: T .AddSubTree(Ttrim2goal );
10: return f ailure;
Here, we propose a fast RRT algorithm for on-road driving
11: end if
of the autonomous vehicles, and the algorithm flow is described
in detail. We first present a review on the operation of the basic
RRT algorithm and then the proposed fast RRT algorithm.

B. Overview of the Fast RRT Algorithm

A. Basic RRT
An overview of the proposed approach is given in
Given an initial state as a root, the basic RRT incrementally
Algorithm 1. Compared with the basic RRT, two improve-
grows a tree to explore the state space outward from the root
ments, namely, the rule templates and an aggressive extension
until the goal state is reached, by iterations. Each iteration
strategy, are added. By introducing the rule templates of the
consists of three main steps:
trajectories, more nodes and edges will derive from the rule
1) Draw a random sample, i.e., xrand , from the state space templates rather than online generation in the initial phase if
over a specified sampling distribution, i.e., prand (x); the obstacles distribute sparsely on road. Furthermore, using the
2) Find the nearest node in the tree to the sample using a aggressive extension strategy with a certain probability can not
designated distance metric, i.e., ρ(x); only accelerate the growth speed of the search tree at the end of
3) Expand the selected nearest node toward the random the period but also increase the accuracy of the terminal state of
sample, when a control input is determined for forward the planned trajectory.
simulation. The resulting state, xnew , is then obtained and A rule-template set contains several rule templates, which
added to the search tree. is generated offline according to the context of traffic scenes.
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

MA et al.: EFFICIENT SAMPLING-BASED MOTION PLANNING FOR ON-ROAD AUTONOMOUS DRIVING 5

Fig. 3. Various rule templates based on different maneuvers. (a) Go straight. (b) Turn left. (c) Turn right. (d) U-turn.

At the beginning of the proposed algorithm, the rule-template C. Generation and Trim of Trajectory Rule Templates
set is loaded, and an appropriate rule template can be selected
Here, more details about the design and using of the rule
automatically according to a short-term goal state (line 1).
templates are described.
This template is then decomposed and saved as the treelike
Compared with a mobile robot running in the maze, the
structure (line 2). If some nodes and edges of a trajectory in
autonomous vehicle traveling on road in urban environments
the rule template are not collision free with the obstacles or the
has the characteristic of simple and relative monotonous ma-
boundaries of the road, these portions will be discarded, and
the remainders in the rule template will be retained (line 3) and neuvers. Some former researchers mainly focused on the path
added to the root (line 4). Thus, at this moment, the tree proba- search for mobile robots in the maze. The environment in
bly has already possessed a part of branches and leaves before the maze is complicated, and the motion of robots is diverse
randomly searching. The aggressive extension strategy is first and hardly predicted. Likewise, the cases of some complex
used after the rule template is loaded and trimmed (lines 5–7), system planning in the high-dimensional configuration space
by calling the function RUSH T OWARDS G OAL(), in which a are similar. Therefore, the rule template cannot be used in
trajectory generator is used for planning from the current state these cases. However, there is a lot of geometric structural
to the goal state. This aggressive strategy can directly plan information in the urban environments. A vehicle is always
a trajectory toward the goal state when without obstacles, going straight and changing the lane in most of the time when
to avoid the meaningless search. If the function is executed it runs on the road. A small portion of time is for turning, and a
successfully, the algorithm ends and returns the tree. If the smaller portion of time is for U-turn. Hence, the maneuvers can
function fails, the algorithm starts the iteration step. In each be categorized in terms of the traffic scenes, by which the rule
iteration step (lines 8–22), two extension strategies are chosen templates can be generated and applied.
stochastically, namely, the aggressive extension strategy and Currently, the maneuvers are roughly divided into four types,
the ordinary extension strategy. We denote the probability of namely, go straight, turn right, turn left, and U-turn. In each
the aggressive extension strategy being selected as λss . The category, considering the driving experience and prior knowl-
aggressive extension strategy is used if a random number (0 edge, various terminal states are casted. These terminal states
to 1) is less than λss (line 9). In this case (lines 10–12), the with different longitudinal and horizontal distances to the start
function RUSH T OWARDS G OAL() is still called; then, the result state are set, in order that generated rule template can cover five
is that a trajectory reaching the goal will be found (if return lanes centered at the current lane. For each given terminal state,
success) or a partial collision-free trajectory is added to the tree a trajectory generator based on inverse kinematics can be used
and the algorithm continues (if return failure). Otherwise, an to solve this two-point boundary value problem for generating
ordinary extension strategy similar to that of the basic RRT trajectories in the rule template [3]. In addition, a path generator
algorithm is chosen (lines 13–21). During this process, we considering the vehicle constraints can also be employed to
adopt some small modifications that refer to [29], such as the rapidly generate paths [35], as these paths will be transferred to
sampling distribution and the Dubins distance metric. The stop the trajectories and control input in the model-based prediction
condition of the proposed algorithm is that the goal state is stage. Fig. 3 presents four offline-generated rule templates
reached by the aggressive extension strategy or the maximum based on different maneuvers. For example, in the “Go straight”
number of iterations K is exceeded. rule template shown in Fig. 3(a), the horizontal distance ranges
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

6 IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS

from −8.1 to 8.1 m, which is able to cover five lanes. There are
three ranges for the longitudinal distance of terminal states of
trajectories, namely, from 19 to 25 m, from 29 to 35 m, and from
39 to 45 m. Note that the design of rule-template size needs to
consider the real perception range of the autonomous vehicles.
In addition, the “U-turn” rule template shown in Fig. 3(d) is
only for turning left because this is to obey Chinese traffic
rules.
Various generated rule templates constitute a template set
that is loaded before the algorithm is executed and from which
an appropriate rule template will be selected as the basis for
the later growth of the tree. Generally, a higher level module,
i.e., the scenes analysis and understanding module, offers a
short-term planning goal according to the local environmental
information perceived by the sensors on the vehicle. The de- Fig. 4. Extension results by the aggressive extension strategy. (a) Success.
(b) No extension. (c) Partial extension with multiple edges.
sired position and heading of this goal state are to separately
match with that of each rule template. The rule template with
D. Aggressive Extension Strategy
successful match both in position and heading will be easily
picked out from the rule-template set loaded. For example, the Here, the principle and the implementation process of the ag-
position of a given goal state is [20, 20] and the heading angle gressive extension strategy are illustrated in detail. Meanwhile,
is 90◦ , then the rule template “Turn right” is selected. the difference between the aggressive extension strategy and the
The selected rule template first needs to be decomposed goal bias [13], [17] used in the traditional RRT is introduced
and saved as the treelike structure. Then, the nodes and edges specifically.
in the rule template will be checked for collisions with the It is well known that traditional RRT grows step by step,
boundaries of the road and obstacles detected. The collision- and each step length is fixed or has few changes. However,
free trajectories will be reserved, while the trajectories with under the environment with a few obstacles, “facing” the goal
possible collision are not discarded entirely. Instead, the col- without hindrance, the step-by-step way of growth, seems to
lision portion is abandoned on the basis of the nodes, whereas be slow and clumsy, leading to meandering paths. To solve
the collision-free portion is reserved. This process is shown in these problems, the aggressive extension strategy is introduced.
later experimental results. Under open environments, the aggressive extension strategy can
As the application of the rule template, the tree quite proba- make a certain branch grow as long as possible from qnear , even
bly has already possessed a part of branches and leaves rather to the goal. The implementation of the aggressive extension
than only a root before randomly searching in the beginning. strategy is by means of the function RUSH T OWARDS G OAL() in
Usually, it is unlikely to be trimmed for all branches and leaves, Algorithm 1. Its principle is to take the goal state as sampling
because the vehicle on road is hardly blocked by obstacles at state and plan a trajectory directly from the current state to the
a close distance in most cases. These remaining branches and goal state using the same trajectory generator as in Section IV-C
leaves can save the time spent on the initial search of the tree. (line 2). If it is successful, the trajectory reaching the goal will
Furthermore, some difficult situations in using the rule tem- be found (line 4–6), but if not, the collision-free part of the gen-
plate are also discussed. Usually, the automatic selection of erated trajectory will be reserved in the search tree (line 8–10).
the appropriate rule template has high accuracy by using the Even so, the search tree will increase a relatively large branch.
position and heading of the goal state. However, the selection As shown in Fig. 4, there are three results for the aggressive
is possibly ambiguous when a road is not straight such as the extension strategy. Fig. 4(a) presents a successful result of this
roads in the ramp of the interchanges or built along the rivers strategy, where the trajectory can rush directly toward the goal
or mountains. We can still use the “Go straight” rule template from qnear without any collisions. This also means that the
against the road with small curvature. If the road has great planning task is accomplished successfully. Sometimes, the tra-
curvature, both the go straight template and the corresponding jectory generated by this strategy cannot reach the goal directly
tuning template possibly match with the obtained short-term and would collide with obstacles. In this case, if the length of
goal state. We can randomly select one from these possible rule the collision-free part is smaller than that of an edge in the tree,
templates. Most of the branches and leaves are trimmed because the whole trajectory generated will be discarded, as shown in
of collisions with road boundaries. Only those branches and Fig. 4(b). On the contrary, if the length of the collision-free
leaves that are close to start state are retained, probably less than is larger, its trajectory will be saved as a part of the treelike
10 m. Hence, the retained parts have no big differences, no mat- structure. In Fig. 4(c), two nodes and two edges are added into
ter if the “Go straight” or the “Turn right/left” rule template is the tree, which needs two iterations in the traditional RRT. If
selected. In the extreme case, a root can be returned without se- under open environments, more nodes and edges are probably
lecting any rule template. The search and the aggressive exten- added into the tree in one-time calling aggressive extension
sion strategy will still work afterward. Moreover, for special or strategy, greatly increasing the growth speed of the tree.
unexpected roads, their rule templates can be customized or ob- The aggressive extension strategy seems similar to the goal
tained by scaling the existing rule templates before the driving. bias strategy used in traditional RRT. In fact, they are quite
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

MA et al.: EFFICIENT SAMPLING-BASED MOTION PLANNING FOR ON-ROAD AUTONOMOUS DRIVING 7

different. Although the goal bias strategy takes the goal as E. Model-Based Prediction Stage
sampling state as well, there is no change and improvement in
When the search tree reaches the goal state, the task of
its way of extension, which extends toward the goal by using
Algorithm 1 is completed, and a tree T is returned. Here, a type
the forward simulation in constant step length or time interval.
of forward simulation approach based on the vehicle’s model,
In other words, it can only grow one step toward the goal at
named the closed-loop prediction, is adopted [30], which can
a time. Compared with the goal bias, our extension strategy is
further smooth the portion of the rule template and the portion
more aggressive, which can plan a trajectory from current state
of tree generated online, while a new trajectory and a feasible
to goal state based on the inverse kinematic.
control sequence for the vehicle can be calculated and obtained.
This extension strategy is applied in two different places of
the proposed algorithm (line 5 and line 10). First, it is definitely
used before the search, which has the advantage of direct trajec- Algorithm 2 Closed-Loop Prediction Trajectory Generation
tory planning under the collision-free environment. Second, it
is used with a certain probability in the iterative search, which 1: Proot2goal ← E XTRACT B RANCH(T , xgoal );
can not only promote rapid growth of the search tree but also 2: X0 ← Troot
make the tree rush directly toward the goal when passing the 3: while xgoal not reached do
obstacles in the later period of search. In this case, one of the 4: ui ← P URE P URSUIT PI(Xi , Proot2goal , Mvehicle );
most important problems is how to determine probability λss of 5: Xi+1 ←F ORWARD S IMULATION(Xi ,ui , Mvehicle ,Δt);
the aggressive extension strategy being chosen. We design it as 6: i = i + 1;
an adaptive adjusting parameter. Initially, λss = 0.2; after the 7: end while
calling aggressive extension strategy, the probability λss will 8: return X , u;
be updated based on whether the search tree is extended or not,
which is shown as Algorithm 2 shows the overall flow of the closed-loop predic-
tion algorithm. In a backward manner, the function E XTRACT-
⎧
⎪ B RANCH() can extract the trajectory/path Proot2goal in the tree
⎨λss (k) + (11− λss (k))
⎪
starting from the goal state xgoal (line 1). Before the forward
−
λss (k + 1) = × γe 2npo +(1−γ) Llee if extendable
⎪
⎪
simulation, the root of the tree is taken as the initial state of
⎩λ (k)e− 2n1ne if unextendable the trajectory (line 2). As a reference input, Proot2goal is sent
ss
(5) to a stable closed-loop system, which consists of a steering
controller by the Stanley method, which is an improved Pure-
where γ ∈ [0, 1] is a constant describing a weighting for two Pursuit [36], a proportional–integral speed controller [29], and
factors that influence the change in λss . These two factors the vehicle’s model (line 4). The steering and speed controller
include the number of consecutive successful extensions npo is presented as
and the trajectory length of the extension le , when the search  
tree can be extended by the aggressive extension strategy. −1 ks de
δu = θe + tan (6)
Le = max{le , lmax } denotes the larger one between the current vact
 t
extension trajectory length le and the length of the longest
vu = kp (vcmd − vact ) + ki (vcmd − vact ) dτ (7)
trajectory in the tree lmax . Contrarily, when the search tree is 0
not extended by the strategy, the probability λss will decrease
according to the number of consecutive unsuccessful extensions where θe is the angle error between the current heading of the
nne . npo and nne are a pair of mutually exclusive parameters. vehicle and the heading of the path at the nearest point. de de-
In the extendable and unextendable cases, one is accumulated notes the distance error to the path. vact and vcmd represent the
and the other one is reset as zero. Furthermore, once a node current actual speed and commanded speed, respectively. ks is a
in the tree is chosen as the qnear and the aggressive extension gain parameter of the Stanley method. kp and ki are the propor-
strategy is executed, the result will be certain. It is unnecessary tional and integral gain parameters. When the control signalui
to reuse this strategy to the same node. Hence, after executing from the closed-loop system is taken as the input to the vehicle’s
the aggressive extension strategy, the node then will be put model, the new state in the trajectory will be generated by run
into a closed list and will not be used again. The probability ning forward simulation for the time interval Δt (line 5). Repeat
of the aggressive extension strategy being chosen is adaptive steps in lines 4 and 5 until the resulting trajectory reaches the
to the heterogeneous environment. When the environment is goal state. Finally, an easy-tracking trajectory X and a feasible
cluttered, the aggressive extension strategy will be chosen with control sequence u for the vehicle will be obtained.
a small probability, and under open and sparse environments,
V. P LANNING IN THE C ONFIGURATION -T IME S PACE
this strategy will be adopted with a high probability.
Using the aggressive extension strategy can not only speed The proposed algorithm, like many other motion planning al-
up the growth of the search tree and avoid some meaningless gorithms, only applies to the environments with static obstacles.
searches but also improve the terminal state accuracy of the However, in real world, there are a lot of dynamic obstacles,
planned trajectory. The reason is that, as a stop condition of the particularly in on-road environments. Generally, under dynamic
proposed algorithm, this strategy makes the planned trajectory and uncertain environments, replanning technology is often
accurately reach the desired terminal state, not just a region. used [37], [38], which repeatedly calls the planning algorithm
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

8 IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS

Fig. 6. Tree and dynamic obstacle in the configuration-time space.

Fig. 7. Comparison among different methods in lane keeping maneuver.

(a) Result of RRT. (b) Result of CL-RRT. (c) First phase result of our method.
(d) Second phase result of our method.

Fig. 5. Unideal replanning cases due to not considering dynamic obstacles. replanning under environments with dynamic obstacles. The
(a) Avoiding an obstacle that has left, due to taking the dynamic obstacle as configuration-time space is to incorporate the time as an ad-
a static one initially. After replanning, unnecessary lane changing is executed
twice. Light green lines represent the trajectories before replanning. Cadet blue
ditional dimension in the configuration space, denoted by CT.
lines denote the planned trajectories after replanning. (b) Dangerous example of In the configuration-time space, the vehicle and obstacles all
lane changing, due to not taking the speed of the dynamic obstacle initially. The can be presented as pairs c, t , where c is the configuration
coming collision triggers the replanning mechanism, resulting in a meandering
curve.
of the vehicle or obstacle at time t ∈ [0, ∞). According to
the current velocities of obstacles, both static and dynamic
obstacles can be extruded and transformed to static obstacles
with respect to static obstacles. There are two ways of calling. in this 3-D space. The planning task in the configuration-
One is to repeatedly call the planning algorithm with a fixed time space is formulated as finding a 3-D trajectory TCT
duration. The other one is to call the planning algorithm in cases such that TCT (0) = x0 , TCT (tf ) = xgoal , and ∀ (t ∈ [0, tf ] ::
where the environmental changes affect the original planning, TCT (t), t ∈ CTfree ), where tf denotes the arrival time of the
for example, the planned trajectory ahead has been occupied by trajectory. In the extension of the search tree, the arrival time
obstacles and the avoidance becomes meaningless due to the t of each state in new nodes and new edges can be computed
departure of the obstacles, as shown in Fig. 5. In a real-time [39], shown as
planning system, using the first way of calling is always avoided
le
because of its large computational overhead. If the second way t= (8)
is adopted, dynamic obstacles’ velocity information should be v
considered in the planning algorithm to plan a more feasible where le is the length of extended trajectory segment. v is the
trajectory. longitudinal velocity of the vehicle. Resulting time t as a new
We combine the proposed fast RRT algorithm with the dimension is added into each state of the trajectory. Likewise,
configuration-time space to improve the performance of the the arrival time t will also be added into the generated rule
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

MA et al.: EFFICIENT SAMPLING-BASED MOTION PLANNING FOR ON-ROAD AUTONOMOUS DRIVING 9

Fig. 8. Comparison among different methods in U-turn maneuver. (a) Result of RRT. (b) Result of CL-RRT (c) First phase result of our method. (d) Second
phase result of our method.

template, as shown in Fig. 6. In this figure, red volume denotes as RRT-gb, and closed-loop RRT (CL-RRT) algorithm [30]
the dynamic obstacle moving in duration [0, 5s]. Blue curves without the optimization heuristics for updating the resulting
represent a 3-D tree in configuration-time space, in which the trajectory [29], because we hope that the performance of the
z-axis denotes the arrival time. In the plane of the same arrival algorithm in a successful planning cycle can be tested. In these
time, collision between configuration of the vehicle and the tests, the stop conditions of the RRT, RRT-gb, and CL-RRT are
section of the obstacle is detected. This is similar to the collision that the maximum number of the iterations is reached or the
detection of motion planning in 2-D environments. search tree reaches a region close to the goal. The maximum
When the search tree reaches a vertical line that is perpen- number of the iteration is set as 3000. The region is a circle with
dicular to the XY plane and through the goal, the planning a radius equal to 1 m, centered at the goal. Note that the heading
in configuration-time space will be accomplished. So far, a error is not considered in the stop conditions. The stop condi-
3-D trajectory, including the arrival time, can be generated. tion of the proposed algorithm is that the tree accurately reaches
Meanwhile, a 2-D trajectory can also be obtained from its the goal state by the aggressive extension strategy. Furthermore,
projection onto the XY plane. The proposed algorithm can to address the problem of dynamic environments, some results
be simply applied to this configuration-time space, and the with respect to our algorithm planning in the configuration-time
current speeds of obstacles are used to predict their future space are also presented. All utilized approaches were imple-
states. In this way, the planned trajectory is more reasonable mented in MATLAB on a laptop computer with 2.67-GHz CPU
and feasible, which could reduce the computational overhead of and 3.8-GB RAM. In comparison illustrations (see Figs. 7–11),
replanning and make the whole trajectory smoother. Therefore, the first phase and second phase results are all presented. The
the integration of the proposed algorithm into the configuration- first phase result is generated by the fast RRT algorithm, and the
time space is the guarantee of good replanning. second phase result is produced by its postprocess approach.
The purpose of presentation in two separated phases is to
synchronously show the components of the generated trajectory
and final results. However, the results of two phases are always
VI. E XPERIMENTAL R ESULTS AND D ISCUSSION
highly overlapping. This is because the fast RRT algorithm
To verify the performance of the proposed approach, experi- takes into account the model of vehicle in trajectory generation.
ments are conducted under two different types of environments, In all experiments shown in the following, the cadet blue lines
namely, urban environments with and without obstacles. Under and the magenta points denote the edges and nodes of the search
urban environments without obstacles, various maneuvers are tree, respectively. Yellow lines denote the trajectories generated
taken into account. In urban environments with obstacles, the by searching. Magenta lines indicate the paths generated by
scenes with a single obstacle and multiple obstacles are set in the aggressive extension strategy. Cyan lines are the resulting
the test, respectively. The proposed approach is compared with trajectories by forward simulation, which could be the result of
two other related approaches, namely, standard RRT algorithm CL-RRT or the result of the proposed algorithm in the second
[13], or RRT algorithm with 5% goal bias, which is abbreviated phase.
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

10 IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS

Fig. 9. Planning results of the proposed method in other maneuvers (the first phase and second phase results). (a) Results in lane change maneuver. (b) Results
in turn left maneuver. (c) Results in turn right maneuver.

Fig. 10. Comparison among different methods in turning left maneuver at the intersection with one obstacle. (a) Result of RRT. (b) Result of CL-RRT. (c) First
phase result of our method. (d) Second phase result of our method.
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

MA et al.: EFFICIENT SAMPLING-BASED MOTION PLANNING FOR ON-ROAD AUTONOMOUS DRIVING 11

in the tree, the success rate, the trajectory length, and the run
time. The number of samples and available nodes can reflect
the efficiency and relative speed of these algorithms. Note that
the nodes in the rule template are not included in the results,
because they are generated offline. It shows that each algorithm,
particularly the CL-RRT algorithm, has good success rate under
the obstacle-free environments, except for the U-turn maneuver.
Compared with the proposed algorithm, the related algorithms
spend more runtime and need more sampling numbers, because
they find the trajectory by means of searching alone. For these
five basic maneuvers, the proposed algorithm can achieve a
smooth trajectory only by combining the rule templates and the
trajectory generated by aggressive extension strategy, without
any searching. Therefore, the runtime is very short, which is
less than 3.5 ms. Furthermore, with respect to the trajectory
length for the five maneuvers, the proposed algorithm generates
the shortest trajectory with less meander and winding, com-
pared with other related algorithms.
Fig. 11. Comparison among different methods in overtaking maneuver under
the cluttered traffic environment with multiple obstacles. (a) Result of RRT.
(b) Result of CL-RRT. (c) First phase result of our method. (d) Second phase B. Environments With Obstacles
result of our method.
Under the environments with obstacles, two types of cases
are considered, namely, with a single obstacle and with multiple
obstacles (four obstacles are set). For the comprehensive eval-
A. Environments Without Obstacles
uation, two classical traffic scenes, namely, “straight road” and
In the urban environments without obstacles, five common “intersection,” are taken into account, respectively. Similarly,
maneuvers, namely, on-road lane keeping, on-road lane change, two typical illustrations are given. One is for only one obstacle
turn left at the intersection, turn right at the intersection, and at the intersection, as shown in Fig 10. The other one is for avoi-
U-turn, are used for test. The comparison results of the pro- ding multiple obstacles on a straight road, as shown in Fig 11.
posed algorithm and the related algorithms for on-road lane Additionally, the results of the proposed algorithm under the
keeping and U-turn are shown in Figs. 7 and 8, respectively. environment with traffic cones, a kind of special obstacle, are
The planning results of the proposed algorithm for the other also presented.
three maneuvers are also given in Fig. 9. Fig. 10 shows that the vehicle turns left and bypasses a
Lane keeping on current lane without obstacles is the most single obstacle at the intersection. Fig. 10(a) shows the result
common driving behavior on road. Fig. 7(a) presents the result of the standard RRT algorithm under this environment. The
of the RRT algorithm, which obviously shows great meander uniform distribution of sampling makes the search tree need
and terminal state error. Fig. 7(b) shows the result of CL-RRT. a large number of attempts to accomplish turning left and
Although the meander is lessened, there still exists the ter- bypassing the obstacle. CL-RRT obtains a better result due
minal state error. Fig. 7(c) and (d) show the results of the to its sampling distribution. However, there still exist minor
proposed algorithm in two phases, respectively. It shows that, meanders and terminal state error, as shown in Fig. 10(b).
for lane keeping, the proposed approach can plan a trajectory The proposed algorithm can easily bypass the obstacle and
that approximates a straight line and reaches the terminal state accomplish the turn left, as shown in Fig. 10(c) and (d). By
accurately. comparing two results of CL-RRT and our algorithm, we can
U-turn is a challenging maneuver for motion planning algo- see that the trajectory planned by CL-RRT is too close to the
rithms. Standard RRT algorithms often traverse whole configu- edge of the intersection and the available lanes, whereas the tra-
ration space to accomplish the turning, as shown in Fig. 8(a). It jectory planned by our algorithm, due to the constraint of the
is difficult for selecting appropriate Gaussian sampling clouds rule template, is around the center of the intersection, which
to bias the tree to the terminal state. On the contrary, it causes is more like a real path by a human driver. A great number of
many collisions before turning, as shown in Fig. 8(b). The experiments under environments with a single obstacle show
proposed algorithm can easily achieve the U-turn using the rule that the proposed algorithm can efficiently accomplish the
template generated offline and the trajectory segment generated planning only by the rule template and the aggressive extension
by the aggressive extension strategy, as shown in Fig. 8(c). strategy without using any search.
Model-based forward simulation approach can make the trajec- Fig. 11 shows the overtaking on a two-lane road with mul-
tory at the place of U-turn smoother, as shown in Fig. 8(d). tiple obstacles, which is more complicated than the single-
The test results of different algorithms for five maneuvers obstacle environment. Fig. 11(a) and (b) are the results of the
are given in Table I. To eliminate randomness, each algorithm RRT and CL-RRT algorithms, respectively. Meander trajectory
was tested for 3000 Monte Carlo (MC) trials. Table I presents and terminal state error are still their weaknesses, although
the averages of the number of samples, the number of nodes CL-RRT is relatively smooth and avoids some meaningless
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

12 IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS

TABLE I
P ERFORMANCE C OMPARISON A MONG D IFFERENT A PPROACHES IN VARIOUS M ANEUVERS

extensions. Fig. 11(c) shows the first phase result of the the length of the trajectory planned by the proposed algorithm
proposed algorithm. It shows that, in this complex case, the is shorter than those planned by the related algorithms in most
algorithm cannot directly plan the path only by the rule template cases, which indicates that a relatively optimal solution can be
(green line) and the aggressive extension strategy (magenta achieved by our algorithm. Only under the environment with
line). Some search branches (yellow line) can supplement the one obstacle at the intersection is the average trajectory length
gap between them, which is also one of the important advan- of the proposed algorithm larger than that of CL-RRT, which
tages of the proposed approach. Fig. 11(d) shows the second has been presented and analyzed above.
phase result of the proposed algorithm, in which a smooth Furthermore, traffic cone is a special kind of obstacle because
trajectory (cyan line) is generated. It is obviously seen that it has both properties of hindrance as a general obstacle and
some portions of the path generated in the first phase are further guidance as a traffic sign. Because of road maintenance or other
smoothed. reasons, traffic cones are often used on road or at intersection
Table II presents the test results under the environment with to guide the vehicles redirection, which is also an important
obstacles in various cases. They are still the averages of 3000 testing part in the Chinese intelligent vehicle competition. The
MC trials results like in Table I. It is shown that each algorithm proposed algorithm can accomplish the planning task easily
has a good result on the straight road, no matter with a single in the traffic scenes, as shown in Fig. 12. Obviously, the rule
obstacle or multiple obstacles. Due to the large configura- template and the aggressive extension strategy play important
tion space of the intersection environment, all of the related roles in improving the planning efficiency, when the vehicle
algorithms have lower success rates, particularly with multiple comes into and departs the narrow passage formed by traffic
obstacles. On the contrary, the proposed algorithm still keeps cones.
very high success rates. Because of a large number of search The aforementioned experimental results show that the pro-
and collision detection, the related algorithms all spend long posed algorithm is a faster and more efficient RRT variant,
runtimes and need many samples. The results of our algorithm compared with other related algorithms. Rather than generating
are completely different. Under an environment with a single the trajectory only by searching, like what the conventional
obstacle, our algorithm can accomplish the planning without RRT algorithm does, the proposed algorithm generates the
searching. Under a multiple-obstacle environment, it only needs trajectory with three mechanisms, namely, the rule template,
a small number of searches to accomplish the planning. Under the rushing from the aggressive extension strategy, and the
a single-obstacle environment, the runtime of the proposed searching. The first two mechanisms greatly reduce the time
algorithm is less than 2.5 ms. Even in the multiple-obstacle consumption. Therefore, the proposed algorithm can generate
environment, the runtime is also less than 80 ms. Moreover, a trajectory rapidly. By analyzing the breakdown of planned
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

MA et al.: EFFICIENT SAMPLING-BASED MOTION PLANNING FOR ON-ROAD AUTONOMOUS DRIVING 13

TABLE II
P ERFORMANCE C OMPARISON A MONG D IFFERENT A PPROACHES IN THE E NVIRONMENTS W ITH O BSTACLES

Fig. 13. Breakdown of planned trajectory length with different numbers

of obstacles. (a) Without obstacles. (b) With an obstacle. (c) With multiple
obstacles.

C. Test in the Configuration-Time Space

We also test the integrated method of the proposed algorithm
and the configuration-time space, which is to deal with the
dynamic obstacles and to improve the quality of the replanning.
A typical experimental result is given in Fig. 14. In this exper-
iment, on a two-lane road, two vehicles are driving with low
speed. We hope that a feasible trajectory for the autonomous
vehicle is planned to overtake these two vehicles from the space
between them. In Fig. 14(a), it is shown that two obstacles are
extruded according to their velocities, which is marked as red
volume. A 3-D trajectory is calculated in this 3-D space, which
Fig. 12. Planning results of the proposed method for traffic redirection by has no collision with the volume. Fig. 14(b) shows the resulting
cones used on road and at intersection. trajectory projected on the 2-D plane. For comparison, the plan-
ning result without considering the dynamic obstacles is also
trajectory length, we find that, under environments with a few
given (black line). It shows great differences in the shapes of
or without obstacles, the proposed algorithm can accomplish
the two trajectories. The result without considering the dynamic
the trajectory generation only by the rule template and the
obstacles will inevitably be replanned, triggered by the coming
aggressive extension strategy, without any searching. Under
collision with the vehicles ahead. However, the planning result
multiple-obstacle environment, the proposed algorithm does
in the configuration-time space is more reasonable and does not
not only take the advantage of these two ways but also bridge
need to trigger the replanning for a short time, if there is no
the gap between them by searching, as shown in Fig. 13.
sudden change in the velocities of those vehicles.
In summary, the proposed algorithm does not only keep the
characteristic of searching but also becomes more efficient than
VII. C ONCLUSION
the conventional RRT. The use of the rule template can make
the generated trajectory more in accordance with the geometric This paper has presented an efficient sampling-based motion
structural information of the roads. planning method designed for on-road driving of autonomous
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

14 IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS

Fig. 14. Result of our algorithm in the configuration-time space (the black line in 2-D view indicates the planning result when without considering the dynamic
obstacles). (a) 3-D view. (b) 2-D view.

vehicles. The core of this method is a fast RRT variant, which is more efficient than the traditional RRT algorithms, it is quite
introduces a rule-template set based on the traffic scenes and possible to refine the trajectories, like what RRT∗ does.
an aggressive extension strategy of search tree, and combines
a prior closed-loop prediction approach. For improving the re-
planning in the dynamic environments, an integrated approach R EFERENCES
of the fast RRT and the configuration-time space is proposed
and used. [1] L. Figueiredo, I. Jesus, J. T. Machado, J. Ferreira, and J. M. Carvalho,
A large number of statistical experiments were conducted “Towards the development of intelligent transportation systems,” in Proc.
for comparing the performance of our approach against other 4th IEEE Int. Conf. Intell. Transp. Syst., Oakland, CA, USA, 2001,
vol. 88, pp. 1206–1211.
related approaches in various scenes. Experimental results [2] H. Cheng, Autonomous Intelligent Vehicles: Theory, Algorithms, and
demonstrate that the proposed approach can plan a trajectory Implementation. New York, NY, USA: Springer-Verlag, 2011, pp. 3–5.
as follows: 1) faster with a lower density of obstacles; 2) more [3] T. M. Howard, M. Pivtoraiko, R. A. Knepper, and A. Kelly, “Model-
predictive motion planning,” IEEE Robot. Autom. Mag., vol. 21, no. 1,
accurate toward goal state; and 3) shorter and with less mean- pp. 64–73, Mar. 2014.
ders. By the analysis, the introduction of the rule template can [4] J. C. Latombe, “Motion planning: A journey of robots, molecules,
improve the efficiency of the RRT algorithm in the early stage digital actors, and other artifacts,” Int. J. Robot. Res., vol. 18, no. 11,
pp. 1119–1128, Nov. 1999.
and can make the generated trajectory more consistent with the [5] E. W. Dijkstra, “A note on two problems in connexion with graphs,”
geometric structural information on the roads, while the use Numer. Math., vol. 1, no. 1, pp. 269–271, 1959.
of the aggressive extension strategy can raise the efficiency of [6] P. Hart, N. Nilsson, and B. Raphael, “A formal basis for the heuristic
determination of minimum cost paths,” IEEE Trans. Syst. Sci. Cybern.,
the algorithm in the final phase and guarantee the accuracy of vol. SSC-4, no. 2, pp. 100–107, Jul. 1968.
the terminal state. In some simple environments, the proposed [7] A. Stentz, “Optimal and efficient path planning for unknown and dy-
algorithm can accomplish the trajectory planning only by these namic environments,” Int. J. Robot. Autom., vol. 10, no. 3, pp. 89–100,
Aug. 1995.
two improvements without any searching. Under the cluttered [8] S. Koenig and M. Likhachev, “D∗ lite,” in Proc. 8th Nat. Conf. AAAI,
environments, the algorithm can still keep the search ability 2002, pp. 476–483.
of the traditional RRT and accomplish the planning efficiently [9] J. Barraquand, B. Langlois, and J. C. Latombe, “Numerical potential field
techniques for robot path planning,” IEEE Trans. Syst., Man Cybern.,
by the integration of three options, namely, the rule template, vol. 22, no. 2, pp. 224–241, Mar./Apr. 1992.
the rushing by the extension strategy, and the searching. The [10] L. E. Kavraki, P. Svestka, J. C. Latombe, and M. H. Overmars, “Probabilis-
rule-template technique perhaps is only suitable for the appli- tic roadmaps for path planning in high-dimensional configuration spaces,”
IEEE Trans. Robot. Autom., vol. 12, no. 4, pp. 566–580, Aug. 1996.
cation of autonomous vehicles, whereas the aggressive exten- [11] S. M. LaValle, Planning Algorithms. Cambridge, U. K.: Cambridge
sion strategy can be generalized to various application fields. Univ. Presss, 2006.
The forward simulation postprocess can make the resulting [12] S. M. LaValle, “Rapidly-Exploring random trees: A new tool for path
planning,” Comput. Sci. Dept. , Iowa State Univ., Ames, IA, USA, Tech.
trajectory smoother. The proposed approach integrated into the Rep. 98-11, 1998.
configuration-time space can plan more reasonable and reliable [13] S. M. LaValle and J. J. Kuffner, “Randomized kinodynamic planning,”
trajectory, addressing the dynamic obstacles. Int. J. Robot. Res., vol. 20, no. 5, pp. 378–400, May 2001.
[14] J. J. Kuffner and S. M. LaValle, “RRT-connect: An efficient approach
In future work, we will attempt to find an asymptotic optimal to single-query path planning,” in Proc. IEEE Int. Conf. Robot. Autom.,
path based on the fast RRT variant. As the proposed algorithm San Francisco, CA, USA, 2000, vol. 2, pp. 995–1001.
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

MA et al.: EFFICIENT SAMPLING-BASED MOTION PLANNING FOR ON-ROAD AUTONOMOUS DRIVING 15

[15] H. Long, Q. H. Do, and S. Mita, “Unified path planner for parking an Liang Ma received the B.S. degree in electronic
autonomous vehicle based on RRT,” in Proc. IEEE Int. Conf. Robot. and information engineering and the M.S. degree in
Autom., Shanghai, China, 2011, pp. 5622–5627. communication and information system from China
[16] J. Bruce and M. Veloso, “Real-time randomized path planning for robot University of Geosciences, Wuhan, China, in 2005
navigation,” in Proc. IEEE/RSJ Int. Conf. Intell. Robot. Syst., Zurich, and 2008, respectively. He is currently working to-
Switzerland, 2002, vol. 3, pp. 2383–2388. ward the Ph.D. degree with the Institute of Artificial
[17] C. Urmson and R. G. Simmons, “Approaches for heuristically biasing Intelligence and Robotics, Xi’an Jiaotong University,
RRT growth,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Las Vegas, Xi’an, China.
NV, USA, 2003, pp. 1178–1183. From 2008 to 2009, he was a Research Staff with
[18] S. Rodriguez, X. Tang, J. M. Lien, and N. M. Amato, “An obstacle-based the Xi’an Institute of Optic and Precision Mechanics,
rapidly-exploring random tree,” in Proc. IEEE Int. Conf. Robot. Autom., Chinese Academy of Sciences, Xi’an. His research
Orlando, FL, USA, 2006, pp. 895–900. interests include motion planning, autonomous navigation, and computer
[19] J. Denny, M. Morales, S. Rodriguez, and N. M. Amato, “Adapting RRT vision.
growth for heterogeneous environments,” in Proc. IEEE/RSJ Int. Conf.
Intell. Robots Syst., Tokyo, Japan, 2013, pp. 1772–1778.
[20] N. A. Melchior and R. Simmons, “Particle RRT for path planning with
uncertainty,” in Proc. IEEE Int. Conf. Robot. Autom., Rome, Italy, 2007,
pp. 1617–1624.
[21] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal
motion planning,” Int. J. Robot. Res., vol. 30, no. 7, pp. 846–894, Jianru Xue (M’06) received the B.S. degree from
Jun. 2011. Xi’an University of Technology, Xi’an, China, in
[22] J. H. Jeon, S. Karaman, and E. Frazzoli, “Anytime computation of time- 1994 and the M.S. and Ph.D. degrees from Xi’an
optimal off-road vehicle maneuvers using the RRT∗ ,” in Proc. 50th Jiaotong University, Xi’an, in 1999 and 2003,
IEEE Conf. Decision Control Eur. Control, Orlando, FL, USA, 2011, respectively.
pp. 3276–3282. From 2002 to 2003, he was with Fuji Xerox,
[23] G. Goretkin, A. Perez, R. J. Platt, and G. Konidaris, “Optimal Tokyo, Japan. In 2008 to 2009, he visited the Uni-
sampling-based planning for linear-quadratic kinodynamic systems,” versity of California, Los Angeles, CA, USA. He is
in Proc. IEEE Int. Conf. Robot. Autom., Karlsruhe, Germany, 2013, currently a Professor with the Institute of Artificial
pp. 2429–2436. Intelligence and Robotics, Xi’an Jiaotong University.
[24] S. Karaman and E. Frazzoli, “Sampling-based optimal motion planning His research field includes computer vision, visual
for non-holonomic dynamical systems,” in Proc. IEEE Int. Conf. Robot. navigation, and video coding based on analysis.
Autom., Karlsruhe, Germany, 2013, pp. 5041–5047. Prof. Xue served as a Coorganization Chair for the Asian Conference on
[25] D. Kim, J. Lee, and S. Yoon, “Cloud RRT∗ : Sampling cloud based Computer Vision 2009 and the International Conference on Virtual System and
RRT∗ ,” in Proc. IEEE Int. Conf. Robot. Autom., Hong Kong, 2014, Multimedia 2006. He also served as a Program Committee Member for the
pp. 2519–2526. International Conference on Pattern Recognition 2012, the Asian Conference
[26] M. Likhachev, D. Ferguson, G. Gordon, A. T. Stentz, and S. Thrun, “Any- on Computer Vision 2010 and 2012, and the International Conference on
time dynamic A∗ : An anytime, replanning algorithm,” in Proc. ICAPS, Multimedia and Expo 2014.
2005, pp. 1–10.
[27] D. Ferguson, T. M. Howard, and M. Likhachev, “Motion planning in
urban environments,” J. Field Robot., vol. 25, no. 11, pp. 939–960,
2008.
[28] M. Montemerlo et al., “Junior: The Stanford entry in the urban challenge,”
J. Field Robot., vol. 25, no. 9, pp. 569–597, 2008.
[29] Y. Kuwata et al., “Real-time motion planning with applications to Kuniaki Kawabata (M’93) received the B.E., M.E.,
autonomous urban driving,” IEEE Trans. Control Syst. Technol., vol. 17, and Ph.D. degrees from Hosei University, Tokyo,
no. 5, pp. 1105–1118, Sep. 2009. Japan, in 1992, 1994, and 1997, respectively, all in
[30] Y. Kuwata et al., “Motion planning in complex environments using electrical engineering.
closed-loop prediction,” in Proc. AIAA Guid., Navigat., Control Conf., From 1997 to 2000, he was a Special Post-
Honolulu, Hawaii, 2008, pp. 1–22. doctoral Researcher with the Biochemical Systems
[31] K. Chu, M. Lee, and M. Sunwoo, “Local path planning for off-road au- Laboratory, The Institute of Physical and Chemical
tonomous driving with avoidance of static obstacles,” IEEE Trans. Intell. Research (RIKEN), Wako, Japan, where he is cur-
Transp. Syst., vol. 13, no. 4, pp. 1599–1616, Dec. 2012. rently the Unit Leader of the RIKEN-XJTU Joint
[32] J. Xin, C. Wang, Z. Zhang, and N. Zheng, “China future challenge: Research Unit. He is also currently a Visiting Re-
Beyond the intelligent vehicle,” IEEE Intell. Transp. Syst. Soc. Newslett., searcher with the Research into Artifacts, Center
vol. 16, no. 2, pp. 8–10, Apr. 2014. for Engineering, The University of Tokyo, Tokyo; a Visiting Professor with
[33] M. Du et al., “An improved RRT-based motion planner for autonomous Kagawa University, Takamatsu, Japan; and a Project Associate Professor
vehicle in cluttered environments,” in Proc. IEEE Int. Conf. Robot. with Keio University, Tokyo. His research interests include mobile robotics,
Autom., Hong Kong, 2014, pp. 4674–4679. distributed autonomous systems, networked system, and understanding social
[34] L. Ma et al., “A fast RRT algorithm for motion planning of autonomous adaptability of the insect.
road vehicles,” in Proc. 17th IEEE Int. Conf. Intell. Transp. Syst., Dr. Kawabata is a member of the Robotics Society of Japan, the Society
Qingdao, China, 2014, pp. 1033–1038. of Instrument and Control Engineers, and the Japanese Society of Mechanical
[35] K. Kawabata, L. Ma, J. R. Xue, and N. N. Zheng, “A path generation Engineers.
method for automated vehicles based on Bezier curve,” in Proc. IEEE/
ASME Int. Conf. Adva. Intell. Mechatron., Wollongong, NSW, Australia,
2013, pp. 991–996.
[36] S. Thrun et al., “Stanley: The robot that won the DARPA grand
challenge,” J. Field Robot., vol. 23, no. 9, pp. 661–692, Sep. 2006.
[37] D. Ferguson, N. Kalra, and A. Stentz, “Replanning with RRTs,” Jihua Zhu received the B.E. degree in automation
in Proc. IEEE Int. Conf. Robot. Autom., Orlando, Fl, USA, 2006, from Central South University, Changsha, China, in
pp. 1243–1248. 2004 and the Ph.D. degree in control science and
[38] M. Zucker, J. Kuffner, and M. Branicky, “Multipartite RRTs for rapid engineering from Xi’an Jiaotong University, Xi’an,
replanning in dynamic environments,” in Proc. IEEE Int. Conf. Robot. China, in 2011.
Autom., Rome, Italy, 2007, pp. 1603–1609. He is currently an Associate Professor with the
[39] I. Ardiyanto and J. Miura, “3D time–space path planning algorithm in School of Software Engineering, Xi’an Jiaotong
dynamic environment utilizing arrival time field and heuristically random- University. His research interests include computer
ized tree,” in Proc. IEEE Int. Conf. Robot. Autom., Rome, Italy, 2012, vision, autonomous robots, and image processing.
pp. 187–192.
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

16 IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS

Chao Ma received the B.S. degree in automatic Nanning Zheng (SM’94–F’06) received the Bach-
control engineering from Xi’an Jiaotong University, elor’s degree and M.E. degree in information and
Xi’an, China, in 2010. He is currently working control engineering from Xi’an Jiaotong University,
toward the Ph.D. degree in the Institute of Artificial Xi’an, China, in 1975 and 1981, respectively, and
Intelligence and Robotics, Xi’an Jiaotong University. the Ph.D. degree in electrical engineering from Keio
His research interests include motion planning and University, Tokyo, Japan, in 1985.
control. He is currently a Professor and the Director of
the Institute of Artificial Intelligence and Robotics
with Xi’an Jiaotong University. His research inter-
ests include computer vision, pattern recognition,
computational intelligence, image processing, and
hardware implementation of intelligent systems.
Prof. Zheng became a member of the Chinese Academy of Engineering in
1999. Since 2000, he has been the Chinese Representative on the Governing
Board of the International Association for Pattern Recognition. He currently
serves as an Executive Editor of the Chinese Science Bulletin.