Conference PaperPDF Available

A Practical Trajectory Planning Framework for Autonomous Ground Vehicles Driving in Urban Environments

  • The Beijing Institute of Basic Medical Sciences

Abstract and Figures

This paper presents a practical trajectory planning framework towards fully autonomous driving in urban environments. Firstly, based on the behavioral decision commands , a reference path is extracted from the digital map using the LIDAR-based localization information. The reference path is refined and interpolated via a nonlinear optimization algorithm and a parametric algorithm, respectively. Secondly, the trajectory planning task is decomposed into spatial path planning and velocity profile planning. A closed-form algorithm is employed to generate a rich set of kinematically-feasible spatial path candidates within the curvilinear coordinate framework. At the same time, the velocity planning algorithm is performed with considering safety and smoothness constraints. The trajectory candidates are evaluated by a carefully developed objective function. Subsequently, the best collision-free and dynamically-feasible trajectory is selected and executed by the trajectory tracking controller. We implemented the proposed trajectory planning strategy on our test autonomous vehicle in the realistic urban traffic scenarios. Experimental results demonstrated its capability and efficiency to handle a variety of driving situations, such as lane keeping, lane changing, vehicle following, and static and dynamic obstacles avoiding, while respecting traffic regulations.
Content may be subject to copyright.
A Practical Trajectory Planning Framework for Autonomous Ground
Vehicles Driving in Urban Environments
Xiaohui Li, Zhenping Sun, Zhen He, Qi Zhu, and Daxue Liu
Abstract This paper presents a practical trajectory plan-
ning framework towards fully autonomous driving in urban
environments. Firstly, based on the behavioral decision com-
mands, a reference path is extracted from the digital map
using the LIDAR-based localization information. The reference
path is refined and interpolated via a nonlinear optimization
algorithm and a parametric algorithm, respectively. Secondly,
the trajectory planning task is decomposed into spatial path
planning and velocity profile planning. A closed-form algorithm
is employed to generate a rich set of kinematically-feasible
spatial path candidates within the curvilinear coordinate frame-
work. At the same time, the velocity planning algorithm is
performed with considering safety and smoothness constraints.
The trajectory candidates are evaluated by a carefully develope-
d objective function. Subsequently, the best collision-free and
dynamically-feasible trajectory is selected and executed by the
trajectory tracking controller. We implemented the proposed
trajectory planning strategy on our test autonomous vehicle
in the realistic urban traffic scenarios. Experimental results
demonstrated its capability and efficiency to handle a variety of
driving situations, such as lane keeping, lane changing, vehicle
following, and static and dynamic obstacles avoiding, while
respecting traffic regulations.
Autonomous driving technologies have great potentials
to improve driving safety, comfort and efficiency. The past
three decades have witnessed the remarkable development
of autonomous driving technologies, which have attracted
considerable interest and efforts from both academia and
industry. Particularly in the last decade, with the advent of
advances in sensing, computer technologies, artificial intel-
ligence, and so forth, the autonomous driving technologies
have become an extraordinary active research field in the
robotics community. During the DARPA Grand Challenges
(DGC) and DARPA Urban Challenge (DUC), AGVs exhib-
ited their abilities of autonomously driving in both off-road
and on-road environments [1], [2], [3], [4].
When AGVs advance towards handling realistic dynamic
urban traffic scenarios, they are required to be capable of
handling a variety of complex maneuvers, such as lane
keeping, vehicle following, lane changing, merging, while
respecting traffic rules, avoiding static and dynamic ob-
stacles, and interacting with other traffic participants [5].
To enable the vehicle to autonomously drive in realistic
*This work was supported in part by the National Natural Science
Foundation of China under Grant 61473303 and 91220301.
Xiaohui Li, Zhenping Sun, Zhen He, Qi Zhu, and Daxue Liu
are with the College of Mechatronic Engineering and Automation,
National University of Defense Technology, Changsha, P. R.
China (e-mail: xiaohui,,, zhq
urban environments, various state-of-the-art technologies are
required. As one of the core technologies, motion planning
plays a critical role in coping with these challenges. Based
upon amount of previous research on this topics [3], [4],
[6], this paper focuses on addressing the trajectory planning
problem for AGVs driving in realistic urban scenarios in a
practical way rather than a theoretical way.
A large number of motion planning algorithms have been
proposed for AGVs driving in urban environments. These
algorithms can be roughly categorized into two major groups.
One focuses on computing a long-term collision-free path
using deterministic or random graph-search algorithms [7],
[8], [9], which are capable of handling complex environmen-
tal constraints and efficiently preventing reactive motions.
Most of these powerful algorithms are especially suitable
for AGVs driving in partially or completely unknown envi-
ronments. However, the resultant paths are often comprised
of concatenated short-term motion primitives, such as stop-
and-redirect motions, which cannot meet the smooth and
relatively high-speed requirements in typical urban driving
scenarios. In addition, most of these search-based algorithms
are too computationally demanding to deal with the dynamic
traffic situations.
There has been substantial research on trajectory planning
for AGVs driving in urban environments. A well-known and
efficient strategy follows a discrete sampling and optimiza-
tion scheme. In [3], a model-predictive motion planner is
proposed using the curvature polynomials to parameterize the
control input space based on the work in [10]. A similar work
can be found in [11] and [12]. In [13], nudges and swerves
laterally shifting from the base trajectory are employed to
avoid obstacles along the reference path while being aligned
with the reference path. A similar scheme is adopted in [5],
but considering both the lateral and longitudinal movements
within the Fren´
et framework. The authors in [14] considers
both the vehicle model and closed-loop feedback control
laws to generate drivable trajectory candidates aligning with
the reference path. A similar strategy was applied in [9].
Based on the Boss work in DUC [3], a powerful trajectory
planner for highway driving is proposed using spatiotemporal
lattice conforming to the road geometry [15]. Gu et al.
proposed a two-step motion planner aiming at addressing
both urban and highway driving in one framework [16]. Most
of these approaches generate a rich set of drivable trajectory
candidates firstly. Then the best trajectory which minimizes
a specified cost function is selected for execution. Instead
2015 IEEE Intelligent Vehicles Symposium (IV)
June 28 - July 1, 2015. COEX, Seoul, Korea
978-1-4673-7266-4/15/$31.00 ©2015 IEEE 1160
of applying the discrete optimization scheme, very recent
work in [6] formulates the trajectory planning problem as a
nonlinear optimization problem with inequality constraints.
Based on a reference corridor provided by the digital map,
online perception data, and high-level decision process, the
planner explicitly considers hard constraints imposed by both
static obstacle and dynamic vehicles, as well as the maximal
steering curvature. The Newton type method is applied to
solve the optimization problem in the continuous state space.
Based upon previous work mentioned above, this paper
focuses on addressing the trajectory planning problem for
AGVs driving in urban environments. The reference path is
extracted using the digital map relying on the accurate and
robust LIDAR-based localization technology. We admit that
the trajectory planner assumes more responsibility to handle
many dynamic traffic situations in an reactive manner with
explicitly considering the constraints imposed by vehicle
motion model and dynamic environments. Even though, it
is still necessary for the trajectory planner to take advantage
of maneuver decisions from the behavioral planner. In this
way, it does not only significantly reduce the search space for
trajectory planning but also avoid over-reactive or aggressive
maneuvers in most of non-imminent traffic situations.
An overview of the system framework is described here.
As shown in Fig. 1, the reference path is derived firstly. To
meet realtime requirements, we apply the nonlinear optimiza-
tion algorithm to refine the reference path locally rather than
globally. After that, the cubic B-spline algorithms is used
to interpolate the optimized path in order to obtain dense
waypoints. Considering the road geometry constraints as well
as handling both static and moving objects, we decompose
the trajectory planning problem into two subtasks: spatial
path planning and velocity planning. Instead of using the
optimization scheme to produce a sole optimal trajectory in
each replan cycle [6], the trajectory planner is capable of
generating a rich set of sub-optimal candidates, which could
efficiently overcome noises in the perception and localization
systems. Besides, it ensures that the vehicle could safely stop
in the imminent situations.
Reference path extraction
and smoothening
Spatial path
Velocity profile
Collision Test
Fig. 1: System framework overview.
To guarantee driving safety and improve driving comfort,
the collision test is performed to trim the trajectory collide
with static and moving objects. Then, a objective function
with a set of cost terms is carefully designed to select the
best trajectory candidate. To handle various realistic driving
situations in urban environments, a situation-aware policy is
introduced to adaptively tune the weights of the cost terms
in the objective function. The best trajectory is transformed
into actuator commands by the trajectory tracking controller
A. Online perception and localization for trajectory plan-
Many researchers have proposed efficient trajectory plan-
ning algorithms for AGVs driving in highways and urban
environments. Most of them assume that lanes could be
obtained as prior information. To the best of the authors’
knowledge, even in urban environments, the lanes cannot be
robustly detected in many areas. Besides, the lane detection
performance is highly dependent on weather and light con-
ditions. A possible method to mitigate these issues is using
digital maps combined with GPS-based inertial navigation
System. Nevertheless, it may still suffer from the localization
noises resulting from the block and reflection effects caused
by tall buildings or forests. All of these negative effects make
it hard to obtain robust localization information of lane-level
accuracy. To overcome these problems, many researchers
take advantage of LIDAR or vision-based localization tech-
nologies [17], [18].
Fig. 2: Local trajectory planning map based on reference
information from the digital map.
In this study, the trajectory planner uses the reference
path information based on the LIDAR-based localization ap-
proach, which is similar to that proposed in [17]. Combining
the data from GPS, IMU, and Velodyne HD-LIDAR, the
high-resolution grid-map are created offline. Then we use
the online LIDAR perception data for real-time localization.
The experiments demonstrate its ability to provide reliable
localization information for realtime autonomous driving in
the urban environments with narrow roads. Based on the
localization method, we are able to use lane information
provided by the digital map. As shown in Fig. 2, the
green and blue waypoints represent the right and left lanes
respectively. The grey-scale mother map is created offline
with high resolution (20cm ×20cm per cell) using cloud
point data of the 64-beam HD-LIDAR. The grey-scale value
represents the height value of each grid. The cells with
magenta color indicates online detected obstacles, including
dynamic and static obstacles. The rectangles represent the
detected dynamic obstacles with arrows indicating their
moving directions.
B. Reference path smoothing
Based upon the aforementioned localization approach, rich
information of the digital map can be utilized for AGVs.
For trajectory planning, the reference path could be the
centerline of the desired lane, which could be obtained
from behavioral commands. However, the centerline may
not be smoothly enough to be tracked in many situations,
e.g. when the vehicle negotiates a curvy turn. To ensure
that the desired path could be easily tracked by the vehi-
cle, the conjugate gradient method is applied to refine the
reference path. A similar method was referred in [8]. It
is able to smoothen the centerline in a continuous space.
Giving discrete waypoints of the centerline as seed vertices
i),(i=1, ..., N ), which can be slightly tuned to
minimize the following objective function:
arg min(ω1
xi+1 Δxi)2+ω2
where Δxi=xixi1,(i>=2)is the dis-
placement vector at the vertex xi=(xi,y
Δxi+1 tan1Δyi
is the vector direction change at
the vertex; ω1
2are weights. The first term minimizes the
the vector change between the adjacent vectors, while the
second term minimizes the cumulative curvature along the
After applying the aforementioned optimization algorith-
m, a much smoother path could be obtained. In practice,
to achieve realtime performance, the waypoints taken for
smoothing are often sparse and the distances between the
waypoints are often large. To solve this problem, we take the
sparse point as the control points and use the cubic B-spline
to interpolate the refined reference path. In the meanwhile,
four dimensional states (x, y, θ, κ)of each point along the
smooth path could be obtained in an analytical manner. The
tangent angle θ(i)and the curvature κ(i)of the waypoints
along the path are defined as
κ(t)= ˙x¨y¨x˙y
As shown in Fig.3, the green curve with equidistant way-
points is the optimized reference path. To simplify the nonlin-
ear optimization problem and achieve realtime performance,
environmental constraints are not explicitly considered dur-
ing the path smoothing process. Obstacle avoidance will
be taken into account by the trajectory planning algorithms
discussed later.
right bound
left bound
C.G. optimization result
Fig. 3: Conjugate gradient algorithm is applied to smooth the
centerline. Then dense waypoints are obtained using cubic
B-spline interpolating method.
C. Spatial path generation based on the curvilinear coordi-
nate framework
When AGVs drive in urban environments, in order to
safely and friendly interact with other traffic participants
like human drivers while respecting traffic regulations, it
is necessary to generate trajectories explicitly considering
both road geometry and speed constraints. To achieve this,
instead of using the Cartesian coordinate framework, the
road geometry model is described using the curvilinear
coordinate framework. More precisely, the reference path is
parameterized as a function of the arc length. In this way,
vehicles’ movements could be naturally divided into two
dimensions: one is the lateral movement, and the other is
the longitudinal movement. Relying on this framework, the
trajectory planning are inherently decoupled into spatial path
generation and velocity profile generation. The spatial path
generation problem is solved firstly.
After using the smoothing algorithm mentioned above, the
reference path is obtained and taken as the base frame of
the curvilinear system. To ensure the consistency between
consecutive replans, the generated path is required to be
aligned with the base frame. In [3], a model-predictive
motion planner is proposed, which sampled a finite set of
terminal states laterally shifting from the base frame firstly.
Then, the path generation problem was reformulated as a
two-point boundary value problem. To reduce the solution
space while guaranteeing the representativeness of output
maneuvers, curvature polynomials were employed to param-
eterize the control input space. The approach considered
constraints imposed by the sampling terminal configurations.
However, it ignored the road geometry. As a consequence,
when the reference path is curvy, it is difficult to generate
long-term paths being aligned with the road geometry.
To overcome this disadvantage, we refer to the trajec-
tory planning strategy mentioned in [5]. The well-known
sampling-based motion planning strategy is employed to
efficiently generate a rich set of path candidates, which are
capable of avoiding static obstacles as well as being aligned
with the reference path geometry. The details are described
as follows.
Based on the curvilinear coordinate framework, the sam-
pling terminal states could be represented by two parameters,
one is the preview distance sfalong the reference path
and the other is the lateral offset lf. This is different from
the sampling method in the Cartesian coordinate [3]. The
preview distance sfdetermines how fast the vehicle is
regulated to be aligned with the base frame. In practice, it
could be adaptively tuned according to the vehicle speed
without violating the maximal lateral acceleration limit. The
lateral offset l0is the perpendicular distance between the
vehicle current position and the corresponding point the
nearest point along the base frame.
Base on the differential flatness control theory referred
in [19], polynomial splines can be applied to represent the
smooth transition from the current lateral offset l0to the
sampling terminal lateral offset lf. To ensure the path could
be analytically solved and cubic polynomial is applied as
referred in [20], quintic polynomial could also be applied to
guarantee the continuity of the first-derivative of the lateral
offset. But in practice, we found that it is also more sensible
to the initial conditions.
Hence, it can be obtained
Besides, the heading difference θ(s)between the vehicle
and the base frame could be derived by the first derivative
of the later offset l(s)with respect to the arc length s.
θ(s) = arctan( dl
For simplicity, the terminal heading of the generated path
is set to be same as that of the base frame at the preview
distance sf. The vehicle’s initial heading angle may be
different from the tangent direction of the base frame at the
current position, so we can obtain:
θ(s0)=θ0(sf)=0 (7)
θ0is the initial heading difference between the vehicle and
the base frame. Since the number of unknown parameter-
s is equal to that of equations, the unknown parameters
3}could be analytically solved.
The curvature of the generated spatial path κ(s)can be
derived from the curvature κbof the base frame [21], [20],
κ(s)= 1
Q(κb+(1 b)(d2l/ds2)+κb(dl/ds)2
To guarantee the kinematical-feasibility of the generated
path, (1 b)is required to be greater than zero. In other
words, using this method is not able to generate drivable
paths when the lateral offset lsis greaterer than the radius of
the corresponding point on the base frame. The motion plan-
ning approach referred in [3] could be applied to overcome
the issue. Since the curvature state is the invariant interme-
diate variable between the curvilinear coordinate framework
and the Cartesian coordinate framework, the spatial path in
the Cartesian coordinate could be numerically solved using
the vehicle kinematic equations as follows.
κ(s)=max{κmin,min{κ(s)max }}
where κmin and κmax indicate steering angle constraints
imposed by the vehicle. When vehicles drive at high speed, it
is necessary for the local planner to generate long-term path
to avoid reactive maneuvers. To achieve this, d2l/ds2=0
and dl/ds =0, when (s>=sf). In this way, the generated
paths are set to be aligned with the reference path after they
reach the sampling terminal states.
As shown in Fig. 4(a), the generated spatial path candi-
dates are able to connect the current vehicle state with the
sampling terminal states. Figure 4(b) illustrates that when
the vehicle has the initial lateral deviation (curves with dark
green color) or a certain initial heading difference (curves
with red color) from the base frame. The path generation
approach is capable of generating spatial path candidates
which are aligned with the base frame.
generated path
reference path
terminal states
Fig. 4: Path candidates generation results. (a) l0=0and
θ0=0,(b) l0dand θ0θ
D. Velocity profile generation
Velocity planning has a significant impact on the driving
safety and comfort. Especially when vehicles drive in the
dynamic road traffic, where velocity planning is required
to be explicitly considered. In this paper, velocity profile
generation is performed considering a variety of constraints,
such as commands from the behavioral planner, maximal
allowed lateral and longitudinal accelerations. These con-
straints could considerably reduce the solution space for the
velocity planning as well as allow the velocity planner to
concentrate on the space where the optimal solution is most
likely to exist.
In this study, a hierarchical velocity profile generation
scheme is developed. The maximal speed is determined
firstly. Then, the trapezoidal profile is employed to generate
the initial velocity law assigned along the generated path.
After that, to improve smoothness of the longitudinal control,
polynomial splines are applied to smoothen the linear ve-
locity profiles and generate acceleration-continuous velocity
Firstly, in order to guarantee safety and improve driving
comfort, several constraints are taken into account to deter-
mine the maximal speed as follows.
(i) The maximal allowed speed Vlimit1. It could be derived
by considering the high-level behavioral planner reasoning
about the road conditions, traffic regulations and the safe
distance of the generated path to the obstacles.
(ii) The maximal allowed lateral acceleration AccMaxLat
limit2 |κ|≤AccMaxLat
To prevent lateral force acting on tires from entering into the
nonlinear or saturated zone, alleviate sideslip effects as well
as ensure the yaw stability, the AccM axLat is required to be
carefully determined considering vehicle dynamics.
(iii) The maximal longitudinal velocity acceleration limit
AccMax Lon and deceleration limit DecMax Lon.
limit3 v2
2AccMax Lon
limit3 v2
2DecMax Lon
+Dsafe =S
where v0is the current velocity, vfis the terminal speed,
and Srepresents the path length. The terminal speed vf
is determined by the high-level behavioral planner, which
considers the surround moving vehicles and traffic regu-
lations. For instance, when the vehicle executes a vehicle
following maneuver, the terminal speed is set to be the same
as the front vehicle. When the vehicle approaches a stop
sign/line or meets the passing pedestrians, vfis set to be
zero. The details will not be discussed in this paper. Besides,
the safety distance Dsafe, which involves the response time
delay tdelay and imminent braking distance Dimminent. The
imminent braking distance Dimminent can be represented as
a function with respect to vf, e.g. the well-known constant
time gap law can be employed.
Dsafe =tdelayv0+Dimminent(vf)
max 2
(, )va
(, )va
(, )va
(, )va
velocity profile
velocity profile
Fig. 5: Velocity profile generation results. (a) trapezoidal
velocity profile generation,(b) velocity profile smoothing
As shown in Fig. 5, the trapezoidal velocity profile is
employed to generate the initial velocity profile. For sim-
plicity, the linear velocity profile with constant acceleration
and deceleration values is used. Base on our experimental
experience gained in field tests, to avoid oscillations of
the longitudinal control, the velocity is required to be kept
at the maximal speed vmax for a certain period of time,
which is longer than tsteady. Therefore, vmax does not
necessarily reach the maximal allowed speed Vlimit, such
as the blue solid velocity curve shows in Fig. 5. Given the
initial speed v0, terminal speed vf, the path length S, the
maximal allowed speed Vlimit, and user-specified parameters
stable}, the closed-form velocity profile
can be obtained.
For the smooth tracking purpose, we smoothen the ini-
tially generated trapezoidal velocity profiles to guarantee the
continuity of the longitudinal acceleration. Inspired by the
parametric velocity profile generation method in [16], the
velocity is smoothed using cubic polynomial.
v(t)=v0+at +bt2+ct3
Correspondingly, the acceleration acc(t)and path length
s(t)can be derived through the first derivative and integration
of the velocity function respectively. Given initial velocity v0
and acceleration a0, the terminal velocity vf, acceleration af,
and path length sf. The unknown parameters {a, b, c, tf}in
(10) could be analytically solved via the following equations.
acc(0) = a=a0
As shown in Fig. 5, the acceleration-continuous S-shaped
ramp-up velocity profile (the red solid line) and ramp-down
velocity profile (the blue dash line) are solved respectively.
The collision test is performed to trim the part of tra-
jectories colliding with obstacles instead of the entire tra-
jectory. To achieve this, the local trajectory planning map
is represented as an occupancy grid map based on the
online perception information. To reduce the computational
complexity of the collision test, the rectangular shape of the
vehicle is approximated by a set of circles with the same
radius [6].
Noticed that avoidance of the moving objects which run in
the same or opposite directions of ours are handled through
the longitudinal velocity planning using the lane information
from the digital map. To avoid the pedestrians or vehicles
runs in the traffic intersection, we conservatively consider
the prediction of the their movements within a finite time
horizon (e.g 3s). The research in [22] provides a promising
approach to handle moving obstacle avoiding in dynamic
traffic scenarios.
After the collision test, the remaining trajectories are eval-
uated via an objective function comprised of four weighted
cost terms as
where J{p, s, d, c}are cost terms, ω{p, s, d, c}are the
corresponding weighted factors, and τi(i=1, ..., N)is the
evaluated trajectory candidate. The cost value is normalized
to be within [0, 1]. The cost term Jpprefer longer trajectory
in order to prevent over-reactive maneuvers.
Jp=(Smax S(τi))/Smax (12)
where S(τi)is the path length of the trajectory candidate τi
along the base frame and Smax is the path length threshold.
To avoid aggressive lateral movements, smoothness criterion
is considered by the cost term Js, which is the integration
of curvature along the trajectory candidate.
ds (13)
To ensure the vehicle track the base frame accurately, the
cost term Jdpenalizes path which deviates far from the base
ds (14)
where D(τi(s)) is the deviation distance and Dmax is the
deviation distance threshold.
The cost term Jcpenalizes the path inconsistency during
consecutive replans. The inconsistency between consecutive
replans can easily result in abrupt steering actions, control
overshoots or even control instability. To solve the problem,
we minimize the sampling terminal lateral offset variations
between consecutive replans.
where l(τi)is the sampling terminal lateral offset of the
current path, lp(τi)is the sampling terminal lateral offset
of the best path in the previous plan.
High Low
Fig. 6: Trajectory candidates evaluation.
As shown in Fig. 6, the color illustrates the cost of the
generated trajectory candidates, the lowest cost trajectory
candidate with green color is selected for execution. Cur-
rently, the weights are empirically determined to make a
compromise between the cost terms mentioned above, the
machine learning technique could be introduced to adaptively
tune these parameters.
Our test autonomous vehicle is shown in Fig.7. The low-
level steer and speed control systems have been modified to
be drive-by-wire. Field experiments have been carried out
on this platform to verify the efficiency and applicability of
the proposed trajectory planning algorithms in the realistic
urban environments.
The proposed local trajectory planning algorithms are
implemented in C++, which runs on a PC (Intel(R) Core(TM)
Riegl Laser
Velodyne Laser
Stereo Vision
Fig. 7: Our test autonomous vehicle.
i7@2.70GHz, 4.0GB memory) with Linux system. The re-
plan cycle is set to 100ms. The trajectory planning consumes
around 20ms, while the time spent on path smoothing
is around 50ms. In practice, this can meet the real-time
The maximal speed is set to 25km/h limited by the
traffic regulation of the urban area, where we carry out the
experiments. The terminal speed of the trajectory is set to
20km/h, when it does not collide with obstacles. The maxi-
mal path length is limited to 80m limited by the perception
information. The maximal lateral acceleration limit is set to
3.0 m/s2to ensure the driving comfort. The total number
of the generated trajectory candidates is 500, which could
be enhanced to cope with the complex environments. For
instance, to improve the probability of the existence of the
collision-free path candidates, sampling terminal states, such
as lateral offset and preview distance, can be sampled more
Figure 8 shows the trajectory planning results when the
AGV negotiates a smooth turn and a curvy turn, respectively.
It can be seen that vehicle is capable of generating the tra-
jectory candidates which are aligned with the road geometry
while avoid stationary obstacles.
(a) (b)
Fig. 8: Trajectory Planning results for Lane keeping. (a) a
smooth turn, (b) a curvy turn
Figure. 9 depicts the trajectory planning results when the
AGV interacts with other traffic participants. As Fig. 9 (a)
shows, the vehicle executes a lane-changing maneuver when
the current lane is occupied by a slowly moving vehicle in
front. It can be seen In Fig. 9(b) that although the current
lane is taken up by a vehicle in front, which has a relative
higher speed than the AGV. In this case, the AGV executes a
vehicle following maneuvers. Besides, the trajectory planner
is also capable of planning a trajectory which keeps a certain
safe distance from the oncoming vehicle in the adjacent lane.
(a) (b)
Fig. 9: Interact with the other traffic participants. (a) lane
changing, (b) vehicle following
This paper has proposed a practical and efficient trajec-
tory planning framework for autonomous driving in urban
environments. The presented trajectory planning strategy
employs a hierarchical strategy. A reference path is extracted
from the digital map relying on the accurate and robust
localization information using LIDAR-based localization
technology. The reference path is refined and interpolated
via the nonlinear optimization algorithm and parametric
algorithm respectively. Dividing the trajectory planning into
spatial path generation and velocity profile generation has
significantly reduced the solution space and enabled the
planner to generate a rich set of drivable trajectory candi-
dates, which are aligned with road geometry and capable
of handling a variety of dynamic traffic situations, such as
lane keeping, lane changing, vehicle following, static and
moving objects avoidance. Although the presented planning
framework focuses on solving the motion planning in on-
road environments, it can easily be extended to integrate
with the graph-search algorithm in off-road environments.
Further research will consider uncertainty issues resulting
from localization, perception and control systems to interact
with the other traffic participants more naturally in urban
[1] S. Thrun, M. Montemerlo, H. Dahlkamp, D. Stavens, A. Aron,
J. Diebel, P. Fong, J. Gale, M. Halpenny, and G. Hoffmann, “Stanley:
The robot that won the darpa grand challenge,” Journal of field
Robotics, vol. 23, no. 9, pp. 661–692, 2006.
[2] U. Ozguner, C. Stiller, and K. Redmill, “Systems for safety and
autonomous behavior in cars: The darpa grand challenge experience,
Proceedings of the IEEE, vol. 95, no. 2, pp. 397–412, 2007.
[3] C. Urmson, J. Anhalt, D. Bagnell, C. Baker, R. Bittner, M. Clark,
J. Dolan, D. Duggins, T. Galatali, and C. Geyer, “Autonomous driving
in urban environments: Boss and the urban challenge,Journal of Field
Robotics, vol. 25, no. 8, pp. 425–466, 2008.
[4] M. Montemerlo, J. Becker, S. Bhat, H. Dahlkamp, D. Dolgov, S. Et-
tinger, D. Haehnel, T. Hilden, G. Hoffmann, B. Huhnke et al., “Junior:
The stanford entry in the urban challenge,” Journal of Field Robotics,
vol. 25, no. 9, pp. 569–597, 2008.
[5] M. Werling, S. Kammel, J. Ziegler, and L. Groll, “Optimal trajectories
for time-critical street scenarios using discretized terminal manifolds,”
The International Journal of Robotics Research, vol. 31, no. 3, pp.
346–359, 2012.
[6] J. Ziegler, P. Bender, T. Dang, and C. Stiller, “Trajectory planning for
berthała local, continuous method,” in Intelligent Vehicles Symposium
Proceedings. IEEE, 2014, Conference Proceedings, pp. 450–457.
[7] M. Likhachev and D. Ferguson, “Planning long dynamically feasible
maneuvers for autonomous vehicles,The International Journal of
Robotics Research, vol. 28, no. 8, pp. 933–945, 2009.
[8] D. Dolgov, S. Thrun, M. Montemerlo, and J. Diebel, “Path planning
for autonomous vehicles in unknown semi-structured environments,
The International Journal of Robotics Research, vol. 29, no. 5, pp.
485–501, 2010.
[9] Y. Kuwata, S. Karaman, J. Teo, E. Frazzoli, J. How, and G. Fiore,
“Real-time motion planning with applications to autonomous urban
driving,Control Systems Technology, IEEE Transactions on, vol. 17,
no. 5, pp. 1105–1118, 2009.
[10] T. M. Howard and A. Kelly, “Optimal rough terrain trajectory genera-
tion for wheeled mobile robots,” The International Journal of Robotics
Research, vol. 26, no. 2, pp. 141–166, 2007.
[11] X. Li, Z. Sun, A. Kurt, and Q. Zhu, “A sampling-based local
trajectory planner for autonomous driving along a reference path,
in Intelligent Vehicles Symposium Proceedings, 2014 IEEE. IEEE,
2014, Conference Proceedings, pp. 376–381.
[12] X. Li, Z. Sun, D. Liu, Q. Zhu, and Z. Huang, “Combining local
trajectory planning and tracking control for autonomous ground vehi-
cles navigating along a reference path,” in Intelligent Transportation
Systems. IEEE, 2014, Conference Proceedings, pp. 725–731.
[13] S. Thrun, M. Montemerlo, H. Dahlkamp, D. Stavens, A. Aron,
J. Diebel, P. Fong, J. Gale, M. Halpenny, and G. Hoffmann, “Stanley:
The robot that won the darpa grand challenge,” Journal of field
Robotics, vol. 23, no. 9, pp. 661–692, 2006.
[14] U. Schwesinger, M. Rufli, P. Furgale, and R. Siegwart, “a sampling-
based partial motion planning framework for system-compliant nav-
igation along a reference path,” in Intelligent Vehicles Symposium
Proceedings. IEEE, 2013, Conference Paper, pp. 391–396.
[15] M. McNaughton, C. Urmson, J. M. Dolan, and J.-W. Lee, “Motion
planning for autonomous driving with a conformal spatiotemporal
lattice,” in Robotics and Automation (ICRA), 2011 IEEE International
Conference on. IEEE, 2011, Conference Proceedings, pp. 4889–4895.
[16] T. Gu, J. Snider, J. M. Dolan, and J.-w. Lee, “Focused trajectory
planning for autonomous on-road driving,” in Intelligent Vehicles
Symposium (IV), 2013 IEEE. IEEE, 2013, Conference Proceedings,
pp. 547–552.
[17] J. Levinson, J. Askeland, J. Becker, J. Dolson, D. Held, S. Kammel,
J. Z. Kolter, D. Langer, O. Pink, and V. Pratt, “Towards fully
autonomous driving: Systems and algorithms,” in Intelligent Vehicles
Symposium (IV), 2011 IEEE. IEEE, 2011, Conference Proceedings,
pp. 163–168.
[18] J. Ziegler, H. Lategahn, M. Schreiber, C. G. Keller, C. Knoppel,
J. Hipp, M. Haueis, and C. Stiller, “Video based localization for
bertha,” in Intelligent Vehicles Symposium Proceedings. IEEE, 2014,
Conference Proceedings, pp. 1231–1238.
[19] M. J. Van Nieuwstadt and R. M. Murray, “Real time trajectory
generation for differentially flat systems,” International Journal of
Robust and Nonlinear Control, vol. 8, pp. 995–1020, 1996.
[20] K. Chu, M. Lee, and M. Sunwoo, “Local path planning for off-
road autonomous driving with avoidance of static obstacles,IEEE
Transaction on Intelligent Transportation System, vol. 13, no. 4, pp.
1599–1616, 2012.
[21] T. D. Barfoot and C. M. Clark, “Motion planning for formations of
mobile robots,” Robotics and Autonomous Systems, vol. 46, no. 2, pp.
65–78, 2004.
[22] F. Damerow and J. Eggert, “Predictive risk maps,” in Intelligent Trans-
portation Systems (ITSC), 2014 IEEE 17th International Conference
on. IEEE, 2014, pp. 703–710.
... Geometric methods use curve fitting based on the given start and end states and avoidable obstacles as inputs and generate the path as a combination of straight lines, circular arcs, or splines [5]. Though such methods are computationally fast, the generated path's dynamical feasibility is not guaranteed; hence, it has to be evaluated afterwards [6]. ...
... To solve this article's basic problem, the status variables are to specify the vehicle's initial speed and designate an area over which the vehicle must travel most optimally. The state-space (6) consists of 11 continuous states. ...
... Comparison of performances on tracks(1,5,6,10). ...
Full-text available
Motion planning plays an essential role in designing self-driving functions for connected and autonomous vehicles. The methods need to provide a feasible trajectory for the vehicle to follow, fulfilling different requirements, such as safety, efficiency, and passenger comfort. In this area, algorithms must also meet strict real-time expectations, since, especially in an emergency, the decision time is limited, which raises a trade-off for the feasibility requirements. This article proposes a hierarchical path planning solution for evasive maneuvering, where a Twin Delayed DDPG reinforcement learning agent generates the parameters of a geometric path consisting of chlotoids and straight sections, and an underlying model predictive control loop fulfills the trajectory following tasks. The method is applied to the automotive double lane-change test, a common emergency situation, comparing its results with human drivers' performance using a dynamic simulation environment. Besides the test's standardized parameters, a broader range of topological layouts is chosen, both for the training and performance evaluation. The results show that the proposed method highly outperforms human drivers, especially in challenging situations, while meeting the computational requirements, as the pre-trained neural network and path generation algorithm can provide a solution in an instant, based on the experience gained during the training process.
... Zhou et al. [23] adopt the Pontryagin's maximum principle to find the solutions of quadratic optimal control problems and propose a vehicle trajectory planning method for autonomous on-ramp merging operation. In [24], the trajectory is optimized considering safety and smoothness constraints under a rich set of kinematically feasible spatial path candidates generated by the polynomial curve. Hu et al. [11] present a dynamically optimized path planning method based on the cubic spline fitting interpolation and fully takes the static safety, comfortability and dynamic cost into account. ...
... Therefore, both the speed limit and collision-avoidance are correlated with the speed and distance directly. As a result, the optimization method is subject to the constraints on speed and distance, as detailed in (24). By this manner, we can find the optimal lane-changing trajectory with the incorporation of efficiency, comfort and safety model by solving e s and e v . ...
Full-text available
Automatic lane-changing is a complex and critical task for autonomous vehicle control. Existing researches on autonomous vehicle technology mainly focus on avoiding obstacles; however, few studies have accounted for dynamic lane changing based on some certain assumptions, such as the lane-changing speed is constant or the terminal state is known in advance. In this study, a typical lane-changing scenario is developed with the consideration of preceding and lagging vehicles on the road. Based on the local trajectory generated by the global positioning system, a path planning model and a speed planning model are respectively established through the cubic polynomial interpolation. To guarantee the driving safety, passenger comfort and vehicle efficiency, a comprehensive trajectory optimization function is proposed according to the path planning model and speed planning model. In addition, a dynamic decoupling model is established to solve the problems of real-time application to provide viable solutions. The simulations and real vehicle validations are conducted, and the results highlight that the proposed method can generate a satisfactory lane-changing trajectory for automatic lane-changing actions. Index Terms-Autonomous vehicle, dynamic lane-changing, trajectory planning, real-time optimization, discrete global trajectory NOMENCLATURE A. ACRONYMS ADAS advanced driving assistant systems PFP potential field projection method RRT rapidly exploring random tree TP preceding vehicle on the target lane TL lagging vehicle on the target lane P preceding vehicle on the initial lane E the ego vehicle GPS global positioning system NGSIM next generation simulation SLC the static lane-changing DOLC dynamic optimized lane-changing
... Semantic segmentation has been widely studied in various fields, such as autonomous driving [2,3,9,17] and environment modeling [7,25,28]. Most existing methods are specially designed for RGB images [1,4,24,27]. ...
Full-text available
Semantic segmentation is a basic task in computer vision, which is widely used in various fields such as autonomous driving, detection, augmented reality and so on. Recent advances in deep learning have achieved commendable results in the semantic segmentation for visible (RGB) images. However, the performance of these methods will decline precipitously in dark or visually degraded environments. To overcome this problem, thermal images are introduced into semantic segmentation tasks because they can be captured in all-weather. To make full use of the two modal information, we propose a novel cross-guided fusion attention network for RGB-T semantic segmentation, which uses an attention mechanism to extract the weights of two modalities and guide each other. Then we extract the global information and add it to the decoding process. In addition, we propose a dual decoder to decode the features of different modalities. The two decoders will predict three prediction maps. We use these three prediction maps to train our network jointly. We conducted experiments on two public datasets, and the experimental results demonstrate the effectiveness of the proposed method.
... In some path/speed decoupled methods, polynomial curvature spiral is generated, and spatial trajectory is solved by using Lagragian method [9][10] [11]. Cubic polynomial is also used to generate path [12] [13]. In the two methods, piece-wise velocity profile is then generated satisfying some road and path constraints. ...
Full-text available
This paper presents an automatic merging algorithm for autonomous driving vehicles, which decouples the specific motion planning problem into a Dual-Layer Automatic Merge Planning (DL_AMP) and a Descent-Based Trajectory Optimization (DBTO). This work leads to great improvements in finding the best merge opportunity, lateral and longitudinal merge planning and control, trajectory postprocessing and driving comfort.
... Discrete optimisation approaches have been widely used to plan local trajectories and achieved success in the field of autonomous driving [18,19,[31][32][33]. The essential idea for these algorithms is describing the local trajectory in the Frenet coordinate framework, which defines the road centre line as the baseline and terms the lateral offset from the baseline as the other coordinate axis. ...
Full-text available
This study proposes an effective trajectory planning algorithm based on the quartic Bézier curve and dangerous potential field for automatic vehicles. To generate collision‐free trajectories, potential field functions are introduced to evaluate the collision risk of path candidates. However, many studies on artificial potential field approaches primarily focus on static and straight roads, and attach less importance to more complex driving scenarios, such as curving roads. In this study, a novel method based on the Frenet coordinate system is proposed to address such limitations. Moreover, to balance the driving comfortability and the driving safety of the path candidate, the path‐planning problem is converted to an optimisation problem, and sequential quadratic programming algorithm is employed to tackle this task. Another merit of this algorithm is the curvature of the generated path is continuous even at the joints of adjacent sub‐trajectories by utilising several specific properties of the Bézier curve. Furthermore, to execute the generated trajectory, a framework of velocity generation is proposed while vehicle dynamic constraints are considered. Some typical traffic scenarios, including lane‐changing, lane‐keeping, and collision avoidance have been designed to verify the performance of the proposed algorithm, and simulations demonstrate the validity of this method.
Tactical decision making and strategic motion planning for autonomous highway driving are challenging due to the complication of predicting other road users' behaviors, diversity of environments, and complexity of the traffic interactions. This paper presents a novel end-to-end continuous deep reinforcement learning approach towards autonomous cars' decision-making and motion planning. For the first time, we define both states and action spaces on the Frenet space to make the driving behavior less variant to the road curvatures than the surrounding actors' dynamics and traffic interactions. The agent receives time-series data of past trajectories of the surrounding vehicles and applies convolutional neural networks along the time channels to extract features in the backbone. The algorithm generates continuous spatiotemporal trajectories on the Frenet frame for the feedback controller to track. Extensive high-fidelity highway simulations on CARLA show the superiority of the presented approach compared with commonly used baselines and discrete reinforcement learning on various traffic scenarios. Furthermore, the proposed method's advantage is confirmed with a more comprehensive performance evaluation against 1000 randomly generated test scenarios.
Conference Paper
Full-text available
This paper addresses the problem of risk assessment in dynamic traffic environments for future behavior evaluation and planning. Risk assessment has to be driven from behavioral needs of the acting scene entity to evaluate the best possible future behavior. We present a general approach for a temporally continuous future risk estimation. Using this risk estimation, we introduce predictive risk maps based on the variation of the acting entities' possible dynamics. The predictive risk maps indicate how critical a certain behavior will be in the future and we use them for future behavior evaluation and planning. Explicitly introducing risk indicators for the collision case of moving entities we show the generality of our approach by applying it to several different traffic scene types.
Conference Paper
Full-text available
In this paper a generic framework for sampling-based partial motion planning along a reference path is presented. The sampling mechanism builds on the specification of a vehicle model and a control law, both of which are freely selectable. Via a closed-loop forward simulation, the vehicle model is regulated onto a carefully chosen set of terminal states aligned with the reference path, generating system-compliant sample trajectories in accordance with the specified system and environmental constraints. The consideration of arbitrary state and input limits make this framework appealing to nonholonomic systems. The rich trajectory set is evaluated in an online sampling-based planning framework, targeting realtime motion planning in dynamic environments. In an example application, a Volkswagen Golf is modeled via a kinodynamic single-track system that is further constrained by steering angle/rate and velocity/acceleration limits. Control is implemented via state-feedback onto piecewise C0-continuous reference paths. Experiments demonstrate the planner's applicability to online operation, its ability to cope with discontinuous reference paths as well as its capability to navigate in a realistic traffic scenario.
Conference Paper
Full-text available
On-road motion planning for autonomous vehicles is in general a challenging problem. Past efforts have proposed solutions for urban and highway environments individually. We identify the key advantages/shortcomings of prior solutions, and propose a novel two-step motion planning system that addresses both urban and highway driving in a single framework. Reference Trajectory Planning (I) makes use of dense lattice sampling and optimization techniques to generate an easy-to-tune and human-like reference trajectory accounting for road geometry, obstacles and high-level directives. By focused sampling around the reference trajectory, Tracking Trajectory Planning (II) generates, evaluates and selects parametric trajectories that further satisfy kinodynamic constraints for execution. The described method retains most of the performance advantages of an exhaustive spatiotemporal planner while significantly reducing computation.
Full-text available
This paper deals with the trajectory generation problem faced by an autonomous vehicle in moving traffic. Being given the predicted motion of the traffic flow, the proposed semi-reactive planning strategy realizes all required long-term maneuver tasks (lane-changing, merging, distance-keeping, velocity-keeping, precise stopping, etc.) while providing short-term collision avoidance. The key to comfortable, human-like as well as physically feasible trajectories is the combined optimization of the lateral and longitudinal movements in street-relative coordinates with carefully chosen cost functionals and terminal state sets (manifolds). The performance of the approach is demonstrated in simulated traffic scenarios.
Conference Paper
In this paper, we develop an integrated local trajectory planning and control scheme for the navigation of autonomous ground vehicles (AGVs) along a reference path with avoidance of static obstacles. Instead of applying traditional cross track-based feedback controllers to steer the vehicle to track the reference path as closely as possible, we decompose the path following task into two subtasks. Firstly, in order to follow the reference path with smooth motions and avoid obstacles as well, we apply an efficient model-based predictive trajectory planner, which considers geometric information of the desired path, kinematic constraints and partial-dynamic constraints to obtain a collision-free, and dynamically-feasible trajectory in each planning cycle. Then, the generated trajectory is fed to the low-level trajectory tracking controller. Relying on the steady-state steering characteristics of vehicles, we develop an internal model controller to track the desired trajectory, while rejecting the negative effects resulting from model uncertainties and external disturbances. Simulation results demonstrate capabilities of the proposed algorithm to smoothly follow a reference path while avoiding static obstacles at a high speed.
Conference Paper
In this paper, a state space sampling-based local trajectory generation framework for autonomous vehicles driving along a reference path is proposed. The presented framework employs a two-step motion planning architecture. In the first step, a Support Vector Machine based approach is developed to refine the reference path through maximizing the lateral distance to boundaries of the constructed corridor while ensuring curvature-continuity. In the second step, a set of terminal states are sampled aligned with the refined reference path. Then, to satisfy system constraints, a model predictive path generation method is utilized to generate multiple path candidates, which connect the current vehicle state with the sampling terminal states. Simultaneously the velocity profiles are assigned to guarantee safe and comfort driving motions. Finally, an optimal trajectory is selected based on a specified objective function via a discrete optimization scheme. The simulation results demonstrate the planner's capability to generate dynamically-feasible trajectories in real time and enable the vehicle to drive safely and smoothly along a rough reference path while avoiding static obstacles.
Conference Paper
In this paper, we present the strategy for trajectory planning that was used on-board the vehicle that completed the 103 km of the Bertha-Benz-Memorial-Route fully autonomously. We suggest a local, continuous method that is derived from a variational formulation. The solution trajectory is the constrained extremum of an objective function that is designed to express dynamic feasibility and comfort. Static and dynamic obstacle constraints are incorporated in the form of polygons. The constraints are carefully designed to ensure that the solution converges to a single, global optimum.
Conference Paper
In August 2013, the modified Mercedes-Benz SClass S500 Intelligent Drive (“Bertha”) completed the historic Bertha-Benz-Memorial-Route fully autonomously. The self-driving 103 km journey passed through urban and rural areas. The system used detailed geometric maps to supplement its online perception systems. A map based approach is only feasible if a precise, map relative localization is provided. The purpose of this paper is to give a survey on this corner stone of the system architecture. Two supplementary vision based localization methods have been developed. One of them is based on the detection of lane markings and similar road elements, the other exploits descriptors for point shaped features. A final filter step combines both estimates while handling out-of-sequence measurements correctly.
This article describes the robot Stanley, which won the 2005 DARPA Grand Challenge. Stanley was developed for high-speed desert driving without manual intervention. The robot's software system relied predominately on state-of-the-art artificial intelligence technologies, such as machine learning and probabilistic reasoning. This paper describes the major components of this architecture, and discusses the results of the Grand Challenge race. © 2006 Wiley Periodicals, Inc.
In this paper, a real-time path-planning algorithm that provides an optimal path for off-road autonomous driving with static obstacles avoidance is presented. The proposed planning algorithm computes a path based on a set of predefined waypoints. The predefined waypoints provide the base frame of a curvilinear coordinate system to generate path candidates for autonomous vehicle path planning. Each candidate is converted to a Cartesian coordinate system and evaluated using obstacle data. To select the optimal path, the priority of each path is determined by considering the path safety cost, path smoothness, and path consistency. The proposed path-planning algorithms were applied to the autonomous vehicle A1, which won the 2010 Autonomous Vehicle Competition organized by the Hyundai-Kia Automotive Group in Korea.