A Dynamic Motion Planning Framework for Autonomous Driving in
Urban Environments
Yuncheng Jiang
1,2
, Xiaofeng Jin
1
, Yanfei Xiong
1
, Zhaoyong Liu
1
1. Global Technology Co, Ltd. Shanghai 201800, China
E-mail: jiangyuncheng@glb-auto.com
2. Hongkong University of Science and Technology, Hongkong
E-mail: yjiangbw@connect.ust.hk
Abstract: we present a framework for robust autonomous driving motion planning system in urban environments which includes
trajectory refinement, trajectory interpolation, avoidance of static and dynamic obstacles, and trajectory tracking. Given road
centerline, our approach smoother the original line via cubic spline. Fifth order Bezier curve is then used to generate more human-
like trajectories that guarantee at least second order continuity and curvature continuity. Dynamic trajectory planning task is
decoupled into lateral spatial and longitudinal velocity planning problems. A bunch of candidate trajectory sets are generated
and evaluated by an object function which considers kinematic feasibility, trajectory smoothness, driving comfort and collision-
checking. Meanwhile, an LQG controller is used to generate longitudinal velocity profile to ensure safety and comfort. After
that, spatial and velocity profiles are transformed into commands executed by lateral steering and longitudinal acceleration
controllers. This framework is validated within a simulation study and has been deployed on our autonomous vehicle shown in
Fig.1 that has travelled thousands of miles in urban environments.
Key Words: autonomous driving, motion planning, optimal control
1 Introduction
Autonomous vehicle has become a hot research topic in
the past few decades. Basically, an autonomous vehicle
consists of four subsystems: perception system, localization
system, decision system, and control system [1]. Based on
sensor fusion methods, perception system detects dynamic
traffic via Lidar, Radar, camera, etc. Localization system
provides accurate vehicle position by GPS navigation or
Simultaneous Localization and Mapping (SLAM). The main
functionality of decision system is to generate global routes,
make vehicle behavior decisions and generate feasible
trajectories dynamically according to the information from
perception system. The control system calculates desired
steering wheel angle and vehicle longitudinal acceleration so
that the vehicle accurately tracks the desired trajectory.
The decision system design can be further divided into
three subsystems: mission planning, path planning and path
tracking. They are hierarchically layered to achieve the goal
of motion planning. In this paper, the main focus is on
pathing planning and path tracking.
In path planning system, the original reference waypoint
is given by mission planning system, but the waypoint
cannot be directly used due to its discontinuity. In our
approach, cubic spline functions are used to generate new
continuous polynomials, based on the original road
centerline, consisting of uniformly distributed waypoints.
We also present a fifth order Bezier curve control points
generation method that generate control points based on
given waypoints. The control points are then used to
generate a smooth Bezier curve that has good continuity
property [2].
Dynamic trajectory planning is divided into lateral spatial
and longitudinal velocity planning. In lateral spatial
planning, optimal control method is applied to achieve an
optimal lateral trajectory in Frenet coordinate [3], while in
longitudinal direction, to facilitate computation efficiency,
an Adaptive Cruise Control (ACC) controller is used for
velocity control and distance control. Applying optimal
control approaches to pathing planning system is not new.
The general process of lateral spatial planning based on
optimization method is to generate feasible cost functional,
and then minimize it under some constraints. While the
criteria of choosing a cost functional is compliance with the
principle of Bellman’s optimality, the trajectory generated
with minimum cost function should be similar to driving
behavior of what human being will do. Therefore, in lateral
spatial planning, this paper first elaborates a well-defined
cost function which takes into consideration of trajectory
continuity, comfort, safety (collision-free), kinematic
feasibility and trajectory compliance between two iterations,
etc. Then, to ensure instantaneity, the cost functional
minimization is solved by KD tree, lazy collision-checking
and exhaustive search [4][5]. In longitudinal velocity
planning, ACC controller using LQR is used to generate
comfortable and safe velocity profile. Kalman filter is used
to deal with environment noise.
Another very important issue is path tracking. Basically,
there are two difference methods in vehicle control: optimal
control methods that rely on vehicle dynamics and geometric
methods that rely on vehicle kinematics. In this paper, we
use the Stanley model [6], and make some modifications to
improve its system stability and performance.
The main contribution of this work is a comprehensive
trajectory smoother, motion planning and trajectory tracking
framework for autonomous driving that includes the
following features:
⚫ Cubic spline functions are used to generate new
continuous polynomials, based on the original road
centerline, consisting of uniformly distributed
waypoints.
⚫ A fifth order Bezier curve-based trajectory smoother is
put forward which guarantees curvature continuity and
driving comfort.