A Real-Time Motion Planner with Trajectory Optimization for
Autonomous Vehicles
Wenda Xu, Junqing Wei, John M. Dolan, Huijing Zhao and Hongbin Zha
Abstract— In this paper, an efficient real-time autonomous
driving motion planner with trajectory optimization is pro-
posed. The planner first discretizes the plan space and searches
for the best trajectory based on a set of cost functions. Then
an iterative optimization is applied to both the path and speed
of the resultant trajectory. The post-optimization is of low
computational complexity and is able to converge to a higher-
quality solution within a few iterations. Compared with the
planner without optimization, this framework can reduce the
planning time by 52% and improve the trajectory quality. The
proposed motion planner is implemented and tested both in
simulation and on a real autonomous vehicle in three different
scenarios. Experiments show that the planner outputs high-
quality trajectories and performs intelligent driving behaviors.
I. INTRODUCTION
A. Background
In the last few decades, researchers have put considerable
effort into autonomous driving. Autonomous vehicles have
great potential to improve the performance and safety of the
transportation system. They can also free people from the
task of driving, which could save commuters considerable
time daily. To achieve this objective without affecting exist-
ing human drivers on the road, autonomous vehicles need
to have human-acceptable driving performance. The planner
also needs to meet strict real-time requirements to react fast
enough in emergency situations. In summary, it is important,
but difficult, to develop a practical high-performance real-
time motion planner for on-road driving.
B. Related work
Autonomous driving Systems: The NAVLAB project at
Carnegie Mellon University (CMU) has built a series of
experimental platforms which are able to run autonomously
on freeways [1]. In 2007, the DARPA Urban Challenge
provided researchers a practical scenario in which to test the
latest sensors, computer technologies and artificial intelli-
gence algorithms [2]. Basic interaction between autonomous
vehicles and human-driven vehicles was proven in low-
density, low-speed traffic. Most planners in the competition
This work was supported by NSF Grant CNS1035813 and NSFC Grants
No.90920304 and No.60975061.
Wenda Xu, Huijing and Hongbin Zha are with School of Electronics
Engineering and Computer Science, Peking University, Beijing, China
{xuwenda, zhaohj, zha}@cis.pku.edu.cn
Junqing Wei is with the Department of Electrical and Computer
Engineering, Carnegie Mellon University, Pittsburgh, PA 15213, USA
junqingw@cmu.edu
John M. Dolan is with the Robotics Institute, Carnegie Mellon University,
Pittsburgh, PA 15213, USA jmd@cs.cmu.edu
were designed aggressively to win the race, rather than being
focused on the trajectory’s quality and human acceptance.
In recent years, commercial driving safety assistance sys-
tems such as adaptive cruise control and lane assist systems
have been widely used in high-end volume-produced cars.
These systems are helpful in reducing accidents caused by
distractions, drowsiness or driver error. However, they cannot
perform complex driving behavior such as dealing with
merging vehicles, circumventing other cars, or responding
intelligently to unexpected dynamic obstacles. Also, these
systems still need constant human supervision.
Trajectory generation: Trajectory generation for au-
tonomous vehicles in road scenarios needs to consider
three constraints: kinematic, dynamic, and road shape. More
specifically, the rate of change of curvature and acceleration
should be continuous in the commanded trajectory to make
sure the car can execute it. The paths should also conform
to the road shape.
Kelly and Nagy ([3], [4]) propose an inverse path genera-
tion method, using curvature polynomials to ensure continu-
ous rate of change of curvature. Based on Kelly and Nagy’s
method, McNaughton et al. ([5], [6]) present a planner that
first samples endpoints along the road and then connects
them using curvature polynomials to make all paths conform
to the road shape. After that, a set of trajectories is generated
by specifying different acceleration profiles for each path.
Paths generated in [6] can be tracked very well by an
autonomous vehicle. But because the acceleration profile is
not continuous, it is hard for the vehicle to follow the profile
accurately and smoothly.
Werling et al. [7] deal with this problem using an alterna-
tive method. They generate the lateral and longitudinal tra-
jectory using quintic polynomials versus time, which ensures
continuous acceleration. In addition, they use a Frenet Frame
referenced to the road center line to combine lateral and
longitudinal motion. This makes the trajectory longitudinal
coincide with the road shape. However, the curvature of
every point on each trajectory needs to be computed and
verified, which is computationally expensive. Additionally,
although the curvature is continuous, the sign of the first
derivative of curvature (moving direction of the steering
wheel) changes very frequently, which leads to jerky steering
wheel movement.
Search algorithm: After trajectories are generated, search
algorithms are often applied to find the optimal result.
Searches in state lattice planners are usually based on
heuristics (e.g. A* and ARA* [8]) or sampling (e.g. RRT
[9]). For heuristic-based algorithms, a good estimate of cost