Content uploaded by Yongjun Yan
Author content
All content in this area was uploaded by Yongjun Yan on Nov 28, 2022
Content may be subject to copyright.
IEEE/ASME TRANSACTIONS ON MECHATRONICS 1
A Hierarchical Motion Planning System for
Driving in Changing Environments:
Framework, Algorithms, and Verifications
Yongjun Yan, Lin Peng, Jinxiang Wang , Member, IEEE, Hui Zhang , Senior Member, IEEE, Tong Shen,
and Guodong Yin , Senior Member, IEEE
Abstract—In this article, a hierarchical real-time motion
planning system is proposed to solve complex navigation
problem in realistic dynamic traffic environments. First, a
longitudinal safety spacing model is established to de-
scribe the possible collision risk in the lane-changing (LC)
process, and a piecewise linearization method is proposed
to convert the nonlinear constraints into linear constraints.
Second, a decision-making strategy is proposed to decide
whether the trajectory should be replanned according to
the real-time prediction of the surrounding environments,
and select the optimal lane. Third, the quintic B-spline
curve method and the quadratic programming method are
integrated to obtain the optimal LC trajectory, considering
factors of safety, comfort, and efficiency. The terminal po-
sition objective function based on the artificial potential
field is designed to soften the hard security constraints and
guide the LC trajectory toward the center of safety zone.
Finally, a model predictive control method based on the
kinematics vehicle model is utilized to track the planned
trajectory. The experimental results of the miniature intel-
ligent vehicle group verify the real-time performance and
effectiveness of the proposed motion planning system.
Index Terms—Decision making, potential field, quadratic
programming (QP), trajectory planning.
I. INTRODUCTION
AUTONOMOUS driving technology is an effective way to
solve the problems of driving safety and traffic conges-
tion [1]. The occupancy rate of autonomous vehicles (AVs) on
the road will gradually increase, but it will be a long process.
Manuscript received 24 May 2022; revised 31 August 2022; accepted
24 October 2022. Recommended by Technical Editor Y. Wang and
Senior Editor X. Chen. This work was supported in part by the National
Natural Science Foundation of China (NSFC) under Grant 52072073
and Grant 52025121. (Corresponding author: Jinxiang Wang.)
Yongjun Yan, Lin Peng, Jinxiang Wang, Tong Shen, and Guodong Yin
are with the School of Mechanical Engineering, Southeast University,
Nanjing 211189, China (e-mail: yanyj@seu.edu.cn; 220200367@seu.
edu.cn; wangjx@seu.edu.cn; 230218564@seu.edu.cn; ygd@seu.
edu.cn).
Hui Zhang is with the School of Transportation Science and
Engineering, Beihang University, Beijing 100191, China (e-mail:
huizhang285@gmail.com).
Color versions of one or more figures in this article are available at
https://doi.org/10.1109/TMECH.2022.3219617.
Digital Object Identifier 10.1109/TMECH.2022.3219617
Human-driven vehicles and the AVs will coexist on roads in
the future for a long time [2]. The behavior of human drivers
is uncertain, and their driving intention is difficult to obtain,
which brings high requirements to the real-time performance
of the decision making, trajectory planning, and motion control
modules of AVs [3].
Accurately describing the collision risk between the host car
and surrounding cars is the premise of trajectory planning. A
general model was presented in [4] to calculate the minimum
longitudinal spacings that the cars should follow for no collision
to occur during a lane-changing (LC)/merging scenario. The
model was applied as safety constraints in [5], [6] to ensure that
the planned optimized trajectory was conducive to a safe LC.
But, only the minimum safe distance was taken into account in
this model. If the car ahead slams on the brakes, the host car
traveling too close to the car in front may not react in time [7].
Gipps [8] proposed a safety margin of braking, which may allow
the host car to brake slowly in advance. And the Gipps’ safe
distance rule was introduced in [9], which improved the safety
and comfort of traveling. However, the constraints of braking
are nonlinear, which greatly reduce the solution efficiency.
In recent years, many approaches have been applied to oper-
ate safety-oriented trajectories, such as artificial potential field
(APF), polynomial curves, optimal control, etc. [10]. A semire-
active planning strategy was proposed in [11], which utilized
the time-based quintic polynomial curves to represent the lateral
and longitudinal motion of the host car. In [12], the polynomial-
curve-based method was adopted to achieve a cluster of LC
trajectories. Then, the optimal trajectory was selected, based
on the stable handling envelope defined from vehicle dynamics
limits and the constraints for avoiding collision. The aforemen-
tioned researches of trajectory planning assume that the motion
states of the environmental cars remain unchanged during the
LC process, and the trajectory planning was only executed once
at the initial moment [13]. However, in the actual dynamic traffic
environments, surrounding cars may adjust their speeds at any
time, such as a sudden acceleration/deceleration behavior, as
shown in Fig. 1, which makes the originally planned trajectory no
longer safe [14]. Some scholars have studied dynamic trajectory
planning method that can adapt to environmental changes. In [9],
an LC model based on polynomial curves was established. When
the safety conditions are not met, the model can adaptively
1083-4435 © 2022 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission.
See https://www.ieee.org/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.
Authorized licensed use limited to: Southeast University. Downloaded on November 23,2022 at 08:55:30 UTC from IEEE Xplore. Restrictions apply.
2IEEE/ASME TRANSACTIONS ON MECHATRONICS
Fig. 1. LC trajectory planning.
adjust the longitudinal displacement and vehicle speed of LC to
realize collision-avoidance and roll prevention. But the impacts
of vehicles in the current lane were not considered. Liu [15] and
Li [16] took into account the behaviors of car following, and
established the real-time periodic planning models of vehicle
trajectory based on cubic polynomial. But only the first-order
continuity was considered, and there was a problem of curva-
ture mutation in trajectory replanning. At the same time, the
problem of trajectory optimization was not considered in these
articles. In [17], a trajectory planning method was presented
based on the integration of sampling and optimization methods,
which can adapt to complex traffic environments. However, this
method continuously carries out trajectory planning according to
the environmental changes, which requires high computational
efficiency. In [18], an elastic soft constraint of the safety domain
was proposed to reduce unnecessary trajectory replanning. But
the vehicle speed was assumed to be unchanged during the LC
process, which is inconsistent with the reality. In these articles,
the objective of safety was considered as hard constraints, result-
ing in the planned trajectory approaching the safety boundaries,
which is very dangerous in dynamic environments due to the
constantly changing safety constraints [19], [20].
The model predictive control (MPC) method has received
significant attention for trajectory planning, due to its capability
of systematically dealing with the problem of multiobjective
optimization with multiple constraints [10]. In [21] and [22], the
kinematic vehicle model-based MPC methods were utilized to
generate collision-free trajectories. However, in order to obtain a
smooth and continuous trajectory, the time step in the prediction
horizon needs to be set very short and the number of prediction
steps needs to be increased. Then, the computational complex-
ity of the algorithm will be greatly increased [23]. Recently,
end-to-end learning-based approaches have become popular in
strategic decisions and trajectory planning of AVs [24], [25].
In [26], a long short-term memory neural network was developed
to predict the trajectories of surrounding vehicles and generate
human-like driving trajectories. Naveed et al. [27] proposed a
robust-hierarchical reinforcement learning framework, which
could plan safe trajectories under uncertain and dynamic traffic
environments. In [28], the model-free adaptive control algorithm
was proposed to deal with lateral control problem of AVs under
the influence of uncertainties. However, model training requires
a large amount of high-quality data, which leads to many lim-
itations in practical applications [29]. Thus, the sampling and
optimization methods are combined in this article to deal with
the trajectory planning problem, which is more appropriate and
has real-time performance.
In the time-varying traffic environments, if the trajectory
is continuously replanned, it will occupy more computing re-
sources, reduce traveling comfort, and even cause instability
of vehicle control. At this time, a decision-making module of
whether the trajectory should be replanned, and which lane
should be chosen as the target lane is crucial [30]. Hang et al.
adopted the cooperative and noncooperative game methods to
deal with the decision making and trajectory planning of AVs,
with consideration of social behaviors of surrounding traffic
occupants [31]. The game approaches were formulated with
MPC, which could describe the interactions of cars in a more
realistic way [32]. However, the time cost is a big problem.
A slot-based approach was proposed in [33], which used the
position, speed, and acceleration of the surrounding vehicles to
select the optimal slot. However, the initial longitudinal spacing
between the front/rear cars on different lanes and the host
car were ignored, which is even more important than the gap
between the front and rear cars from the psychological point of
view of the drivers and passengers [34]. When the gap for LC is
sufficient, the human driver will prefer to choose the lane with
the front and rear cars farther away from him, rather than the
lane with a larger gap [35].
In this article, a hierarchical real-time motion planning system
is proposed, which can dynamically adapt to environmental
changes. The contributions are summarized as follows:
1) An APF-based decision-making module is proposed to
decide whether a replanning is necessary and select the
optimal target lane, subject to the states and movement
trends of the host car and surrounding cars.
2) Sampling and optimization methods are integrated to
select the optimal LC trajectory, with consideration of
safety, comfort, and efficiency. And an APF method is
designed to soften the hard constraints of safety and guide
the LC trajectory toward the center of safety zone, then
the safety of LC process in dynamic environments can be
much improved.
3) A piecewise linearization method is proposed to convert
nonlinear constraints for safe LC into piecewise linear
constraints, which can effectively improve the real-time
performance of the trajectory planning.
The rest of this article is organized as follows. In Section II, the
overall structure of this article is presented. A decision-making
module of whether the trajectory should be replanned is designed
in Section III. In Section IV, a dynamic adaptive planning algo-
rithm of LC trajectory is proposed. Testing results and analyses
of the proposed system are presented in Section IV. Finally,
Section V concludes this article.
II. FRAMEWORK OF THE HIERARCHICAL MOTION PLANNING
SYSTEM
The relative position between the host car (HC) and the other
cars in a common LC scenario is shown in Fig. 1. The HC is an
AV moving on the right lane, and the preceding car (PC) moves
in front of the HC at a slow speed. At this time, the HC has to turn
to the left lane or decelerate to follow PC, with consideration of
factors such as environmental safety, ride comfort, and travel
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.
Authorized licensed use limited to: Southeast University. Downloaded on November 23,2022 at 08:55:30 UTC from IEEE Xplore. Restrictions apply.
YAN et al.: HIERARCHICAL MOTION PLANNING SYSTEM FOR DRIVING IN CHANGING ENVIRONMENTS 3
Fig. 2. Framework of the hierarchical real-time motion planning sys-
tem.
efficiency, which is a motion planning problem. The left lane is
the target lane of LC, then the two cars moving on the left lane
that is closest to the HC must be considered, i.e., the preceding
car (TP) and the following car (TF). Since the movement of the
following car on the original lane (FC) is affected by the HC,
thus, the FC is not taken into account.
The proposed hierarchical real-time motion planning frame-
work is illustrated in Fig. 2, which mainly consists of four
modules, i.e., safety spacing, decision-making, trajectory plan-
ning, and path tracking. The safety boundaries of the HC are
described with a longitudinal safety spacing model, and then, the
nonlinear safety constraints are converted into linear constraints
with a piecewise linearization method, which effectively im-
proves the solution efficiency. In the decision-making module,
it is determined whether the trajectory needs to be replanned
through prediction of the surrounding environmental informa-
tion. And the APF method is utilized to select the optimal lane.
In the trajectory planning module, the terminal-state sampling
method based on the quintic polynomial curves is adopted to
spread the longitudinal and lateral trajectory candidates, and
the quadratic programming (QP) method is utilized to obtain
the optimal candidates of LC trajectories, with consideration of
safety, comfort, and the preferences of different passengers. In
the path tracking model, the MPC method is adopted to track
the planned trajectory.
III. DECISION-MAKING BASED ON ENVIRONMENTAL SAFETY
CONSTRAINTS
In a dynamic traffic environment, the motion states of sur-
rounding vehicles may change at any time, making the last
planned trajectory no longer safe. Thus, adaptive replanning of
trajectories is required according to the environmental changes.
At this time, a decision-making module is essential.
A. Safety Constraints and Their Linearization
Improvement
In the module of decision-making and trajectory planning,
driving safety must be ensured first. Therefore, the constraints
of minimum safety spacing are utilized as the hard constraints
Fig. 3. Possible collision points between the host car and surrounding
cars.
to judge whether the trajectory should be replanned and ensure
the planned trajectory within the safe regions. At the same time,
the nonlinear constraints are piecewise linearized and simplified
to improve the efficiency of solution.
1) Constraints of Minimum Longitudinal Safety Spacing:
When the HC encounters a slow-moving car PC ahead, an LC
maneuver is required. The HC may have a collision with PC, TP,
and TF, as shown in Fig. 3, and the collision points are defined
as CPC,CTP , and CTF, respectively. As shown in Fig. 3(a),the
condition for no collision between HC and PC within the time
range 0 ≤t≤tccan be given by
∀t∈[0,t
c],x
PC(t)−1
2LPC >x
H(t)+1
2LH+WH∗sin(ψ)
(1)
where tcis the time instant when the HC leaves the current lane.
In this article, the initial target trajectory of the HC is centerline
of the right lane, which is chosen as the vertical axis, X-axis, and
the lateral offset from the X-axis is chosen as the horizontal axis
Y-axis. Then, y(tc)=WL
2,yis the lateral position of the HC,
WLis the width of one lane. xPC and LPC are the longitudinal
position and length of the PC. xH,LH,WH, and ψare the
longitudinal position, length, width, and heading angle of the
HC, respectively. More details of modeling and analysis of the
collision points can be found in [4]. In addition, an emergency
braking may happen to the car in front of the HC, which usually
leads to a crash. Therefore, the HC should keep a safety distance
GPC from the PC during the LC process [8], [9]
GPC =vx,HCTs+v2
x,HC
2bmax
−v2
x,PC
2bPC,max
(2)
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.
Authorized licensed use limited to: Southeast University. Downloaded on November 23,2022 at 08:55:30 UTC from IEEE Xplore. Restrictions apply.
4IEEE/ASME TRANSACTIONS ON MECHATRONICS
where vx,HC and vx,PC are the longitudinal speed of HC and PC,
respectively. bmax and bPC,max are the maximum deceleration
of HC and PC. vx,HCTsis the minimum safe distance that HC
should keep from the PC, which is related to the longitudinal
speed of the HC. Tsis the minimum safe time headway. At the
same time, the heading angle is generally a small angle, i.e.,
sinψ≈0. Equation (1) can be rewritten as
∀t∈[0,t
c],G
PC =vx,HCTs+v2
x,HC
2bmax −v2
x,PC
2bPC,max
xPC −1
2LPC >x
H+1
2LH+GPC.(3)
In the same way, as shown in Fig. 3(b), the condition for no
collision between HC and TP within the time range of tc≤t≤
tfcan be given by
∀t∈[tc,t
f],G
TP =vx,HCTs+v2
x,HC
2bmax −v2
x,TP
2bTP,max
xTP −1
2LTP >x
H+1
2LH+GTP
(4)
where tfis the terminal moment of the LC process. xTP,vx,TP ,
LTP,bTP,max , and GTP are the longitudinal position, longitudinal
speed, length, maximum deceleration, and the expected safe
distance of the TP, respectively. As shown in Fig. 3(c),theTF
is located behind the HC, thus the HC only needs to keep a safe
distance from it. Then, the condition for no collision between
HC and TF within the time range of tc≤t≤tfcan be given
by
∀t∈[tc,t
f],G
TF =vx,TFTs
xH−1
2LH>x
TF +1
2LTF +GTF
(5)
where xTF,vx,TF , and LTF are the longitudinal position, longitu-
dinal speed, and length of the TF, respectively. GTF =vx,TFTs
is the minimum safe distance that HC should keep from the TF,
which is related to the longitudinal speed of TF.
2) Piecewise Linearization of the Safety Constraints: The
constraints of longitudinal distances from HC to PC and TP are
nonlinear ones, as shown in (2)–(4), which will reduce the speed
of optimization solution for the objective function and affect
the real-time performance of the trajectory planning algorithm.
Thus, a piecewise linearization method is proposed in this article,
that deploys multiple straight lines to approximate the nonlinear
constraints, as shown in Fig. 4. The red dotted line in Fig. 4
represents the constraint of (4), the black straight lines l1,l2,
and l3are deployed to approximate the nonlinear constraint (4).
The number of the straight lines can change according to the
complexity of nonlinear constraints and the requirements of
constraint accuracy. At the same time, the longitudinal speed
of the HC should be constrained by the road speed limit. Then,
the nonlinear constraints for safe LC can be transformed into
kivx,HC +xHC ≤mi,(i=1,2,3)(6)
where kiand biare the slope and intercept of the ith straight line,
respectively. The shaded area in Fig. 4 is the safe region where
HC can move on the target lane, which is within the nonlinear
constraints and even safer than them.
Remark 1: The piecewise linearization method can convert
nonlinear constraints into piecewise linear constraints, which
effectively improves the solution efficiency. It should be noted
that the proposed method can also be applied to other nonlinear
Fig. 4. Piecewise approximation method for nonlinear constraints of
collision-avoidance emergency braking.
constraints, which provides a new idea for solving nonlinear
constraint problems under complex conditions.
B. Real-Time Decision Making
The decision-making module is divided into three parts. The
first one is to predict the future movements of surrounding cars,
the second one is to judge whether the trajectory needs to be
replanned, and the last one is to select the optimal target lane.
1) Trajectory Prediction and Replanning Decision Making:
Assuming that the acceleration of the HC and surrounding cars
remains constant, then their motion can be described as
xi(t)=xi
k+vi
x,kt+1
2ai
xt2,(i=HC,PC,TP,TF)(7)
where xi
k,v
i
x,k, and ai
xare the current longitudinal position,
speed, and acceleration of the three cars around HC. Define the
time interval of prediction as Δtand the number of prediction
steps as Np, then the prediction sequence of the longitudinal
position of the cars can be derived as
xi(tk+Δt|tk),x
i(tk+2Δt|tk),...x
i(tk+NpΔt|tk)(8)
where tkis the current time step.
The trajectory should be replanned, if there is a collision risk
between HC and PC within the prediction horizon. The condition
of collision can be expressed as
xPC(t|tk)−x(t|tk)<G
PC.(9)
If the safety constraints are not met, the LC trajectory planning
algorithm will be implemented, which is detailed in Section IV.
And the current time is selected as the start time t0of LC
process. In addition, the surrounding environments also need
to be predicted during the LC process. If a sudden change in
the surrounding environments during the LC process results in
a collision risk, the trajectory will be replanned again.
When the prediction sequence in the time horizon of LC, i.e.,
tk+jΔt<t
f,j =1,2...N
p, the position sequence of the HC
can be obtained from (17). If the prediction sequence is beyond
the time horizon of LC, it is assumed that the longitudinal speed
of the HC remains unchanged after the LC. Then, the prediction
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.
Authorized licensed use limited to: Southeast University. Downloaded on November 23,2022 at 08:55:30 UTC from IEEE Xplore. Restrictions apply.
YAN et al.: HIERARCHICAL MOTION PLANNING SYSTEM FOR DRIVING IN CHANGING ENVIRONMENTS 5
sequence of the longitudinal position of the HC can be described
as
⎧
⎪
⎪
⎪
⎪
⎪
⎪
⎪
⎪
⎨
⎪
⎪
⎪
⎪
⎪
⎪
⎪
⎪
⎩
x(tk+Δt|tk),x(tk+2Δt|tk),...,x(tk+NpΔt|tk)
tk+NpΔt<t
f
x(tk+Δt|tk),...,x(tf|tk),x(tf|tk)+vx,f ∗Δt,...
x(tf|tk)+(Np−Nf)vx,f ∗Δt,
tk<t
f<t
k+NpΔt
x(tk)+vx,f ∗Δt,...,x(tk)+Npvx,f ∗Δt
tf<t
k
(10)
where tfis the terminal time of LC, Nf=tf−tk
Δt,if tf>t
k.
Before entering the target lane, i.e., t=tk+jΔt<t
c,if
the HC may collide with the PC, TP, and TF in the prediction
horizon, the trajectory should be replanned. The three conditions
of collision can be expressed as
⎧
⎨
⎩
xTP(t|tk)−x(t|tk)<G
TP
xPC(t|tk)−x(t|tk)<G
PC
x(t|tk)−xTF(t|tk)<G
TF.
(11)
When the HC has entered the target lane, i.e., t=tk+jΔt>
tc, only the TP needs to be taken into consideration. Then, the
condition of collision can be expressed as
xTP(t|tk)−x(t|tk)<G
TP (12)
where GTP,GPC , and GTF are the safety constraints, and more
details can be found in (2)–(5).
2) Decision Making of the Optimal Lane: After predicting
the motion states of surrounding cars, if the safety conditions
are not met, the trajectory needs to be replanned. At this time,
it is most important to select the optimal target lane. In this
article, the APF method is adopted to describe the degree of
risk in each lane, with consideration of the longitudinal distance
between the HC and the cars on different lanes at the current
moment, as well as their speed differences. The acceleration
and deceleration behaviors of the front and rear cars are also
considered. The farther the front and rear cars are from the HC
on different lanes, the faster the front car is and the slower the
rear car is, the larger the acceleration of the front car is and
the smaller the acceleration of the rear car is, the safer the LC
behavior will be, so the smaller the potential field should be,
and vice versa. At the same time, the gap between the front
and rear cars must be greater than the safe distance of LC Gsaf
[36]. In the current lane, only the distance, speed difference,
and longitudinal acceleration of the preceding vehicle need to
be considered, and the following car does not. In terms of both
driving safety and travel efficiency, it is necessary to stay away
from the lane where large vehicles are located. Therefore, the
greater the vehicle mass is, the higher the potential field is. In
addition, the loss of LC should also be taken into account. The
more lanes the HC spans, the larger the potential field is. Then,
the APF of different lanes can be described as
⎧
⎪
⎪
⎪
⎨
⎪
⎪
⎪
⎩
Ji
APF =c1miP
xiP
0−xHC
0
+c1miF
xHC
0−xiF
0
−c2(viP
0−vx,0)+c3(viF
0−vx,0)
−c4aiP
x+c5aiF
x+c6i3if xiP
0−xiF
0>G
saf
Ji
APF =+∞if xiP
0−xiF
0≤Gsaf
J0
APF =c1mPC
xPC
0−xHC
0
−c2(vPC
0−vx,0)−c4aPC
x
(13)
where Ji
APF(i=1,2,,n)is the APF of the ith lane, J0
APF(i=0)
is the APF of the current lane. miP ,xiP
0,viP
0, and aiP
xare
the mass, longitudinal location, speed, and acceleration of the
preceding car on the ith lane. miF ,xiF
0,viF
0, and aiF
xare
the mass, longitudinal location, speed, and acceleration of the
following car. xPC
0and vPC
0are the longitudinal location and
speed of the preceding car on the current lane. cj(j=1,2,...6)
are the weight coefficients.
(J∗
APF,i
∗)=argmin(JAPF(i, xHC
0,v
x,0)) (14)
where i∗is the best lane selected by the decision-making module,
that is, the target lane of trajectory replanning.
Remark 2: It should be noted that, the potential field method
designed in this article is not only applicable to two-lane en-
vironments, but also can be applied to multilane environments,
which is a direction of our future researches.
IV. DYNAMIC ADAPTIVE TRAJECTORY PLANNING BASED ON
SAMPLING AND OPTIMIZATION METHODS
In this section, the trajectory planning algorithm is designed.
First, the traveling time during the LC process is sampled at equal
intervals, and then, various feasible candidates with different
terminal time tfcan be generated. Second, a trajectory planner
based on the QP method is designed to select the optimal
candidates under different LC times subject to safety and com-
fort. Finally, the preferences of travel efficiency for different
passengers are combined to choose the optimal trajectory.
A. Generation of LC Trajectories
1) Equidistant Sampling of the Traveling Times: The trav-
eling time during the LC process is an important indicator to
describe the driving safety, ride comfort, and travel efficiency of
AVs. If the LC time is short, the lateral acceleration aywill be
large and the ride comfort will be poor. If the LC time is long, the
travel efficiency will be reduced. The LC time should be within
the acceptable time frame for the passengers [37]
Tmin ≤tf−t0≤Tmax (15)
where Tmin and Tmax are the minimum and maximum LC times
acceptable to different passengers. t0and tfare the start time
and terminal time during the LC process. Since it is unreasonable
to set the value of LC time in an arbitrary manner, thus it is
equidistantly sampled between Tmin and Tmax as
ΔT=(Tmax −Tmin )/N
tm=mΔT+Tmin,(m=0,1,...,N)(16)
where ΔTis the sampling interval, and Nis the sampling
number of the LC time. tmis the LC time corresponding to the
mth sampling period. Then, the LC trajectory can be sampled
and optimized according to different LC times.
2) Sampling for Longitudinal and Lateral Movements: Dur-
ing the LC process on a structured road, the HC needs to make
a smooth transition to the center of the target lane. The quintic
polynomial has the advantages of high-order continuous deriva-
tion and simple calculation, and is often utilized to represent the
lateral movements of AVs [17]. At the same time, the motion
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.
Authorized licensed use limited to: Southeast University. Downloaded on November 23,2022 at 08:55:30 UTC from IEEE Xplore. Restrictions apply.
6IEEE/ASME TRANSACTIONS ON MECHATRONICS
states of the adjacent cars (i.e., the TP and TF) on the target lane
may also change during the LC process of the HC. Therefore, not
only the lateral movements, but the longitudinal movements of
the HC are also taken into consideration. The quintic polynomial
of longitudinal and lateral trajectories are described as
x(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5
y(t)=b0+b1t+b2t2+b3t3+b4t4+b5t5(17)
where ai,b
i(i=0,1,...5)are the coefficients of the quin-
tic polynomials, which can be solved through the initial
states S0=[x0,v
x,0,a
x,0;y0,v
y,0,a
y,0]Tand terminal states
Sf=[xf,v
x,f ,a
x,f ;yf,v
y,f ,a
y,f ]T.
x(t0)=x0,˙x(t0)=vx,0,¨x(t0)=ax,0
y(t0)=y0,˙y(t0)=vy,0,¨y(t0)=ay,0
(18)
x(tf)=xf,˙x(tf)=vx,f ,¨x(tf)=ax,f
y(tf)=yf,˙y(tf)=vy,f,¨y(tf)=ay,f
(19)
where x0,y
0,v
x,0,v
y,0,a
x,0, and ay,0represent the longitudinal
and lateral position, velocity, and acceleration of the HC at the
initial moment, respectively. xf,y
f,v
x,f ,v
y,f ,a
x,f , and ay,f
represent the longitudinal and lateral position, velocity, and ac-
celeration of the HC at the terminal moment of LC, respectively.
The HC changes from the current position to centerline of the
target lane. Thus, the terminal lateral position yfof the HC is
equal to 0 or WL. At the same time, to achieve smooth transition
from the initial lateral offset to the sampling terminal offset, the
replanned trajectories are set to be tangent to the centerline of
the target lane, i.e., the velocity and acceleration vy,f and ay,f
are set as 0.
Twelve linearly independent equations can be solved for the
12 unknown parameters, then the parameters aiand bicould be
obtained according to the initial states S0and terminal states
Sfof the HC, as well as the LC time tf. Since the initial states
are predetermined by the current states of the HC, which can be
acquired through GPS or sensors. Therefore, the LC trajectory
can be determined by the terminal states and LC time, which are
the convergence states and convergence time of the trajectory.
Since the initial and terminal lateral states are known, the lateral
trajectory for different LC times can be determined. Thus, only
the three terminal states of the HC, i.e., longitudinal position xf,
speed vx,f , and acceleration ax, are necessary to be optimized.
B. Optimization for the LC Trajectories
In this section, the unknown states xf,vx,f , and axand
known states are utilized to indirectly describe the coefficients
of the quintic polynomials, and to express the cost function for
optimization solution. Then, the QP method is deployed to solve
the optimal longitudinal states xf,vx,f , and ax.
1) Constraints of Vehicle Dynamics and Kinematics: During
the LC process, the HC should stay within the lane boundary
−WL
2+WH
2≤y(t)≤3WL
2−WH
2(20)
where y(t)is the lateral position of the HC, WLis lane width,
and WHis the width of the HC. The speed of the HC should be
restricted within the maximum speed vx,max allowed by the road
as follows:
0≤vx(t)≤vx,max.(21)
Taking into account the factors of comfort and vehicle dynamics,
these candidates should be restricted within a safe acceleration
limit as follows:
ax,min ≤ax(t)≤ax,max,a
y,min ≤ay(t)≤ay,max (22)
where ax,min and ax,max are the minimum and maximum lon-
gitudinal acceleration limits, and ay,min and ay,max are the
minimum and maximum lateral acceleration limits, respectively.
2) Cost Function and Its Optimal Solution: In this article, the
safety and comfort factors are deployed to solve the optimal
trajectories at different LC times. Ride comfort is a important
indicator to evaluate the performance of LC, and is generally
measured by jerk. The comfort cost JCcan be defined as
JC(xf,v
x,f ,a
f)=wlon tf
0
jx(t)2+wlat tf
0
jy(t)2(23)
where jxand jyare the longitudinal and lateral jerks, respec-
tively. wlon and wlat are the weights of them.
In previous works, the safety of trajectory planning is mainly
guaranteed by hard constraints [14], [16]. If only the comfort
cost is considered, the planned trajectory tends to be close to the
boundaries of safety constraints. However, in a dynamic traffic
environment, the safety constraints are changing. As a result,
the security requirements can easily no longer be met. If the cost
function of security is added, the planned trajectory will tend to
the center of the safety domain, which is safer and easier to deal
with dynamic environments. In this article, the APF method is
introduced, and the risk function of surrounding cars at the end of
LC is added to the objective function, which can be considered as
a soft constraint to improve the safety of the planned trajectory.
⎧
⎨
⎩
UTP =1
xTP−xf−vx,f Ts−
v2
x,f
2bmax +
v2
x,TP
2bTP,max
UTF =1
xf−xTF−vx,TF Ts
(24)
where UTP and UTF are the potential fields generated by the
preceding and following cars on the target lane. The APFs are
related to the speeds of TP and TF, as well as the distance
between the HC and the two cars. The closer the HC is to the
two cars, the lower the speed of TP or the higher the speed of TF,
the greater the value of APFs, and vice versa. Then, the safety
cost can be defined as
JS(xf,v
x,f )=wPUTP +wFUTF (25)
where wPand wFare the weights considering the safety of TP
and TF, respectively. Since the constructed APF function is not
a quadratic one, which is not suitable for the optimization of the
QP method. In order to improve the applicability of the func-
tion and the solution efficiency of trajectory optimization, the
second-order Taylor expansion method is deployed to simplify
(25) as follows:
JS(X)≈1
2(X−X0)TH(X0)(X−X0)
+[∇JS(X0)]T(X−X0)+JS(X0)(26)
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.
Authorized licensed use limited to: Southeast University. Downloaded on November 23,2022 at 08:55:30 UTC from IEEE Xplore. Restrictions apply.
YAN et al.: HIERARCHICAL MOTION PLANNING SYSTEM FOR DRIVING IN CHANGING ENVIRONMENTS 7
Fig. 5. Optimal LC trajectories with different sampling times.
where X=[xf,v
x,f ],H(X0)=
⎡
⎢
⎣
∂2JS(X0)
∂x12
∂2JS(X0)
∂x1∂x2
∂2JS(X0)
∂x2∂x1
∂2JS(X0)
∂x22
⎤
⎥
⎦.
Then, the trajectory planning problem at different sampling
times can be transformed into the optimization problem as
J∗=minJ(xf,v
x,f ,a
x,f )=JC+JS
s.t. −WL
2+WH
2≤y(t)≤3WL
2−WH
2,0≤vx(t)≤vx,max
ax,min ≤ax(t)≤ax,max,a
y,min ≤ay(t)≤ay,max
∀t∈[0,t
c],x
PC(t)−1
2LPC >x
H(t)+1
2LH+GPC
∀t∈[tc,t
f],x
TP(t)−1
2LTP >x
H(t)+1
2LH+GTP
∀t∈[tc,t
f],x
H(t)−1
2LH>x
TF(t)+ 1
2LTF +GTF
xTP −xf>G
TP,x
f−xTF >G
TF
(27)
where GPC,G
TP, and GTF are the safety constraints in (2)–(5).
The QP solver is utilized to obtain the optimal longitudinal
position, velocity, and acceleration of the HC at the end of
LC with different LC times tm,m=0,1,...,N. Then, the LC
trajectory can be determined.
3) Selection of the Optimal Trajectory: Based on the afore-
mentioned optimization solution, the trajectory clusters at differ-
ent LC times t0,t
1,...,t
Ncan be obtained, as shown in Fig. 5.
Different LC times can generate diverse safe and comfortable
LC trajectories. However, different groups of passengers have
different needs for the travel efficiency during the LC process. At
the same time, the safety and comfort of the planned trajectories
under different LC times are also different. Thus, the aggressive
preferences of different passengers, as well as the performance
indicators of driving safety and ride comfort are all considered
in this article, to select the synthetically optimal trajectory of
LC.
J=J∗+wd
tf
Tmax
(28)
where J∗is the performance of safety and comfort of the optimal
trajectory under different LC times. Tmax is the maximum LC
time acceptable to different passengers, wdis a variable driving
style factor. The stronger the driving aggressiveness, the larger
the parameter wd, the shorter the traveling time, and traveling
distance during the LC process, then a more aggressive LC
trajectory is generated [13].
TABLE I
MODEL AND CONTROLLER PARAMETERS
Remark 3: It should be noted that, if the result of decision
making is to decelerate and follow the preceding car on the
current lane, instead of LC, then the trajectory planning can
be regarded as a speed planning. A profile of longitudinal
speed would be generated subject to safety, comfort, and the
preferences of different and passengers.
C. Trajectory Replanning
If a sudden change of the surrounding environment during
the LC process leads to a collision risk, the decision-making
module will reselect the optimal lane and the trajectory planning
module will replan the trajectory. Since the previous LC behavior
is halfway through, the minimum and maximum allowable LC
times in (15) should be changed accordingly. Assuming that
the LC maneuver has been carried out for a time of Δτ, then
the minimum and maximum allowable LC times allowed by
replanning are
Tmin,p =Tmin −Δτ
Tmax,p =Tmax −Δτ. (29)
When the target lane and the time range of LC is determined,
the performance indicators of driving safety, ride comfort, and
travel efficiency favored by different passengers, are considered
to select the synthetically optimal trajectory of LC. More details
can be found in Sections IV-A and IV-B.
After the trajectory is planned, an MPC method based on the
kinematics vehicle model is utilized to track the planned lateral
trajectory, and a PID method is utilized to track the longitudinal
velocity. More details can be found in [22].
V. T ESTING RESULTS AND PERFORMANCE EVALUATI O N
To verify the feasibility and real-time performance of the
proposed hierarchical motion-planning system, two cases of
simulations/experiments are conducted and analyzed. The first
case is built and tested on the MATLAB/Simulink-Carsim joint
platform. The high-speed HC encounters a slow-moving PC,
and has to plan safe trajectories according to the changing envi-
ronments. In the second case, four miniature model vehicles are
built to perform the experiments of hierarchical motion planning,
which indicate the effectiveness and real-time performance of
the system. The main parameters of the vehicle model and the
controller are summarized in Table I.
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.
Authorized licensed use limited to: Southeast University. Downloaded on November 23,2022 at 08:55:30 UTC from IEEE Xplore. Restrictions apply.
8IEEE/ASME TRANSACTIONS ON MECHATRONICS
Fig. 6. Demonstration of the vehicles movements for scenarios A–C
(blue line indicates the trajectory of the HC, orange line, green line
and pink line indicate the trajectories of the first replanning, the second
replanning, and the third replanning, respectively. The same shapes
indicate the position of the four cars at the same moment. Blue shapes
indicate the HC, orange shapes indicate the TP, green shapes indicate
the TF, and pink shapes indicate the PC). (a) Longitudinal accelera-
tion of TF is 0.5 m/s2. (b) Longitudinal acceleration of TF is 2 m/s2.
(c) Longitudinal acceleration of TF is 3 m/s2.
A. MATLAB/Simulink-Carsim Joint Simulation
In this case, the HC and PC are driving along the right lane
at the speed of 20 and 16 m/s. The PC is 35 m in front of the
HC at the beginning. The TP and TF are driving along the left
lane at the speed of 20 m/s, and they are 20 m in front of and
behind of the HC, respectively. Since the HC is faster than PC,
it has to decide whether to change lanes or slow down to track
the PC, and plan a collision-free trajectory. At the same time, in
an actual traffic environment where human-driven vehicles and
AVs are mixed, if the autonomous HC wants to change lanes,
the human-driven TF may slow down to give way to the HC,
or accelerate to prevent the HC from merging into its lane. In
this article, when TF finds the LC behavior of the HC, it will
accelerate with accelerations of 0.5, 2, and 3 m/s2. Then, the HC
will perform decision-making and trajectory replanning based
on the predicted driving environments. In addition, to simulate
the realistic reactions of the surrounding cars, a classical “in-
telligent driver model” proposed in [38] is deployed. When car
i(i=PC,TP,TF)is far away from the car ahead, the car i
travels at the preferred speed. But when the car iis close to the
car ahead, the driver model will be utilized to adjust its speed to
track the car ahead. The results of several replannings and the
actual driving trajectory of the HC, as well as the positions of
the HC and surrounding cars at different moments are shown in
Fig. 6.
1) Scenario A: In this scenario, when the HC changes lanes,
the TF will accelerate at an acceleration of 0.5 m/s2.Asshown
in Fig. 6(a), when the HC (represented with the blue shapes)
detects the slow-moving PC (represented with the pink shapes),
it operates the first decision-making and trajectory replanning.
Since the speed of PC is lower than that of the HC, the speeds
of TP (represented with the orange shapes) and TF (represented
Fig. 7. Vehicle responses during scenario B. (a) Longitudinal speed
vx. (b) Longitudinal acceleration ax. (c) Front tire steering angle δfand
lateral acceleration ay.
with the green shapes) are the same as that of the HC, and the
distances between TP, TF, and the HC are safe enough. Thus, the
left lane is selected as the target lane. The planned trajectory is
represented by the orange line, the tracking trajectory of the HC
is represented by the blue line. Due to the small acceleration
of TF, the originally planned trajectory is still safe, and no
replanning is required during the LC process. After the LC
maneuver, the speed of the HC is slightly higher than that of
TP, and a second trajectory planning is performed to slow down
slightly to follow the TP, which is represented by the green line.
The same shapes indicate the position of the four cars at the
same moments, 1.5, 3.5, 5.5, and 7.5 s, respectively. As shown in
Fig. 6(a), the four cars do not collide with each other, indicating
that the driving safety is satisfied.
2) Scenario B: In this scenario, when the HC changes lanes,
the TF will accelerate at an acceleration of 2 m/s2.Asshownin
Fig. 6(b), the first decision-making and trajectory planning are
operated in the same way. However, due to the large acceleration
of the TF, if the HC continues to follow the originally planned
LC trajectory, it will crash into the TF. Therefore, a more urgent
trajectory is replanned by the HC, which is represented by the
green line. The HC continues to track the second planned trajec-
tory, and the actual driving trajectory of the HC is represented
by the blue line. As shown in Fig. 7(a), the speed of the HC does
not change much in the first planning. Since the acceleration of
TF is detected, the HC accelerates to merge into the target lane.
After entering the target lane, the HC will crash into the TP due
to its high speed, so the third planning is carried out to follow
the TP, which is represented by the pink line. Finally, the speed
of the HC is 20 m/s, which is the same as that of TP. The speed
of the TF is indicated by the black dotted line. When the HC
starts to change lanes, the TF accelerates to prevent the HC from
changing lanes, but when the HC enters the target lane (t>t
c),
the TF will follow the HC using the intelligent driver model.
Since the TF is close to the HC, it will slow down urgently to
maintain a safe distance. As shown in Fig. 6(b), the four cars do
not collide with each other, meeting the requirements of driving
safety. As shown in Fig. 7(b) and (c), the longitudinal and lateral
accelerations of the HC do not exceed 0.1 and 0.2 g, respectively,
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.
Authorized licensed use limited to: Southeast University. Downloaded on November 23,2022 at 08:55:30 UTC from IEEE Xplore. Restrictions apply.
YAN et al.: HIERARCHICAL MOTION PLANNING SYSTEM FOR DRIVING IN CHANGING ENVIRONMENTS 9
Fig. 8. Vehicle responses during scenario C. (a) Longitudinal speed
vx. (b) Longitudinal acceleration ax. (c) Front tire steering angle δfand
lateral acceleration ay.
at a speed of 20 m/s, indicating that the comfort of the HC can
be ensured.
3) Scenario C: In this scenario, when the HC changes lanes,
the TF will accelerate at an acceleration of 3 m/s2. The initial
planned trajectory of the HC is no longer safe, and a safe LC
trajectory cannot be replanned due to the particularly large
acceleration of TF. As a result, the HC switches back to the
original lane and follows the PC again. As shown in Fig. 8(a),
the HC slows down to avoid a collision with the PC, during and
after the process of returning to the original lane. Finally, the
speed of the HC remains at 16 m/s, the same as that of PC. The
TF performs an emergency acceleration first to prevent the HC
from changing lanes. When the HC returns to the original lane,
the FC decelerates slowly to follow the TP ahead. As shown in
Fig. 8(b) and (c), the longitudinal and lateral accelerations of the
HC both do not exceed 0.1 g, which meets the requirements of
traveling comfort.
Remark 4: In addition, the hierarchical motion planning
algorithms with nonlinear constraints in (2)–(4) and linearly
approximated constraints in (6) are compared in this article.
The time for decision-making and trajectory planning using
the algorithm with nonlinear constraints is 0.15336 s, which
is only 0.02455 s after linear approximation. The computational
efficiency is improved by six times. We believe that the com-
putational efficiency can be more improved with the linear ap-
proximation method when dealing with more complex nonlinear
constraints. At the same time, after converting the algorithm
with linearly approximated constraints into C++ code, it only
takes 0.001232 s to operate a decision-making and a trajectory
replanning, which fully meets the real-time requirements of the
experiment.
B. Experimental Results
In this section, the ability of the proposed system in a real
driving environment is verified. The experimental scene and the
LIDAR clustering effect are shown in Fig. 9. Four vehicles are
included in these experiments, three 1:3 miniature intelligent
vehicles, i.e., HC, TP, TF, and one 1:10 miniature intelligent
vehicle, i.e., slow-moving PC. The initial target speeds of the
Fig. 9. Experimental scene and LIDAR clustering effect.
TABLE II
MODEL AND CONTROLLER PARAMETERS IN THE EXPERIMENT
HC, TP, and TF are set to 15 km/h and that of PC is set to
2.8 km/h. The HC is equipped with Lidar, millimeter wave
radar (MMW), and global navigation satellite system (GNSS),
which provide fundamental information for the hierarchical
motion-planning system. A mini PC is deployed as the upper
layer controller for environment perception, decision-making,
trajectory planning, and path tracking. Finally, the planned safe
trajectory is transformed into steering angle and driving torque
commands, which are sent to the lower layer controller MCU in
the vehicle chassis. The vehicle chassis is fully wire-controlled,
and a controller area network (CAN) analyzer is utilized to
communicate the upper level USB data and the lower level CAN
data. Since the output frequency of the GNSS is 20 Hz, the path
tracking module is also executed at 20 Hz. Due to the changes in
the size and speed of the vehicles during the experiment, some
parameters of model and controller in Table I are scaled and
adjusted to ensure safety, which are shown in Table II.
1) Scenario D: In this scenario, when the HC changes lanes,
the TF accelerates with a small acceleration. As shown in
Fig. 10(a), when the HC detects the slow-moving PC, a smooth
and comfortable LC trajectory is generated. At this time, the TF
accelerates slightly, resulting in the first planned trajectory of
the HC not meeting the safety constraints. Therefore, the HC
replans a more urgent LC trajectory. After the LC maneuver, the
speed of the HC is slightly higher than that of TP, as shown in
Fig. 11(a). Thus, a third replanning is carried out to follow the
TP. The lateral offset eyand heading error eψbetween the real
trajectory and the planned trajectory are shown in Fig. 11(b).The
heading error eψis less than 0.05 rad, which satisfies the small
angle assumption of the kinematics-vehicle-model-based MPC
path tracker [22]. The lateral offset is less than 0.15 m, which
shows good performance of the MPC tracker. At the same time,
as shown in Fig. 11(c), the longitudinal and lateral accelerations
of the HC do not exceed 0.1 g, then the traveling comfort can be
satisfied.
2) Scenario E: In this scenario, when the HC changes lanes,
the TF accelerates with a large acceleration to closely follow TP,
and prevent HC from merging into its lane. If the HC forcibly
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.
Authorized licensed use limited to: Southeast University. Downloaded on November 23,2022 at 08:55:30 UTC from IEEE Xplore. Restrictions apply.
10 IEEE/ASME TRANSACTIONS ON MECHATRONICS
Fig. 10. Demonstration of the vehicles movements for scenarios D–E
(blue line indicates the trajectory of the HC, orange line, green line,
and pink line indicate the trajectories of the first replanning, the second
replanning, and the third replanning. The same shapes indicate the po-
sition of the four cars at the same moment. Blue shapes indicate the HC,
orange shapes indicate the TP, green shapes indicate the TF, and pink
shapes indicate the PC). (a) TF accelerates with a small acceleration.
(b) TF accelerates with a large acceleration.
Fig. 11. Vehicle responses during scenario D. (a) Longitudinal speed
vx. (b) Lateral offset eyand heading error eψ. (c) Longitudinal accelera-
tion axand lateral acceleration ay.
changes lanes at this time, it will lead to collision. Therefore,
the HC replans a trajectory to return to the original lane. As
shown in Figs. 10(b) and 12(a), the HC eventually decelerates
greatly to follow the PC. As shown in Fig. 12(b) and (c),the
lateral offset ey, heading error eψ, longitudinal acceleration ax,
and lateral acceleration aydo not exceed 0.16 m, 0.1 rad, 0.1 g,
and 0.1 g, respectively, verifying the good performance of the
hierarchical motion-planning system and tracking controller.
The same shapes indicate the position of the four cars at the
same moments. As shown in Fig. 10(a) and (b), the four cars do
not collide with each other, indicating that the driving safety is
satisfied.
In addition, the computational time of the path tracker in the
experiments is shown in Fig. 13. Due to the use of the kinematic
vehicle model and C-language, the computation time is much
less than the control execution time of 50 ms. Combined the fast
solution for decision-making and trajectory planning, real-time
performance can be achieved.
Fig. 12. Vehicle responses during scenario E. (a) Longitudinal speed
vx. (b) Lateral offset eyand heading error eψ. (c) Longitudinal accelera-
tion axand lateral acceleration ay.
Fig. 13. Computational time of tracking module in the experiments.
VI. CONCLUSION AND FUTURE WORK
In this article, a hierarchical real-time motion planning frame-
work was proposed. In the module of decision making, an APF-
based algorithm was designed to judge whether a replanning
of trajectory was needed, and select the optimal target lane,
according to the environmental safety constraints. In the module
of trajectory planning, the sampling and optimization methods
were integrated to generate feasible collision-free trajectories,
with consideration of safety, comfort, and the preferences of
different passengers. Finally, in the module of path tracking,
the MPC method was utilized to accurately track the planned
trajectory. Simulations and experiments on different scenarios
were conducted to demonstrate the feasibility and real-time
performance of the proposed system for motion planning in
dynamic environments.
In future research, the decision-making and trajectory replan-
ning will be further investigated for application in multilane en-
vironments. The intentions and preferences of different passen-
gers will also be taken into consideration in the decision-making
module. Validation of the hierarchical motion planning system
on full-size vehicles is another direction of our future work.
REFERENCES
[1] L. Claussmann, M. Revilloud, D. Gruyer, and S. Glaser, “A review of
motion planning for highway autonomous driving,” IEEE Trans. Intell.
Transp. Syst., vol. 21, no. 5, pp. 1826–1848, May 2020.
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.
Authorized licensed use limited to: Southeast University. Downloaded on November 23,2022 at 08:55:30 UTC from IEEE Xplore. Restrictions apply.
YAN et al.: HIERARCHICAL MOTION PLANNING SYSTEM FOR DRIVING IN CHANGING ENVIRONMENTS 11
[2] P. Hang, C. Lv, Y. Xing, and Z. Hu, “Human-like decision making for
autonomous driving: A noncooperative game theoretic approach,” IEEE
Trans. Intell. Transp. Syst., vol. 22, no. 4, pp. 2076–2087, Apr. 2021.
[3] Y. Xia, Z. Qu, Z. Sun, and Z. Li, “A human-like model to understand
surrounding vehicles’ lane changing intentions for autonomous driving,”
IEEE Trans. Veh. Technol., vol. 70, no. 5, pp. 4178–4189, May 2021.
[4] H. Jula, E. B. Kosmatopoulos, and P. A. Ioannou, “Collision avoidance
analysis for lane changing and merging,” IEEE Trans. Veh. Technol.,
vol. 49, no. 6, pp. 2295–2308, Nov. 2000.
[5] Y. Luo et al., “A dynamic automated lane change maneuver based on
vehicle-to-vehicle communication,” Transp. Res. C, Emerg. Technol.,
vol. 62, pp. 87–102, 2016.
[6] H. C. Hsu and A. Liu, “Kinematic design for platoon-lane-change ma-
neuvers,” IEEE Trans. Intell. Transp. Syst., vol. 9, no. 1, pp. 185–190,
Jan. 2008.
[7] J. J. Justo et al., “Fuzzy model predictive direct torque control of
IPMSMs for electric vehicle applications,” IEEE/ASME Trans. Mecha-
tronics, vol. 22, no. 4, pp. 1542–1553, Aug. 2017.
[8] P. G. Gipps, “A behavioural car-following model for computer simulation,”
Transp. Res. B. Methodol., vol. 15, no. 2, pp. 105–111, 1981.
[9] D.Yang et al., “A dynamic lane-changing trajectory planning model for au-
tomated vehicles,” Transp. Res. C, Emerg. Technol., vol. 95, pp. 228–247,
2018.
[10] S. Dixit et al., “Trajectory planning and tracking for autonomous over-
taking: State-of-the-art and future prospects,” Annu. Rev. Control, vol. 45,
pp. 76–86, 2018.
[11] M. Werling, S. Kammel, J. Ziegler, and L. Gröll, “Optimal trajectories for
time-critical street scenarios using discretized terminal manifolds,” Int. J.
Robot. Res., vol. 31, no. 3, pp. 346–359, Mar. 2012.
[12] J. Zhou, H. Zheng, J. Wang, Y. Wang, B. Zhang, and Q. Shao, “Multiob-
jective optimization of lane-changing strategy for intelligent vehicles in
complex driving environments,” IEEE Trans. Veh. Technol., vol. 69, no. 2,
pp. 1291–1308, Feb. 2020.
[13] Y. Yan, J. Wang, Y. Wang, C. Hu, H. Huang, and G. Yin, “A cooperative
trajectory planning system based on the passengers’ individual preferences
of aggressiveness,” IEEE Trans. Veh. Technol., early access, Aug. 31, 2022,
doi: 10.1109/TVT.2022.3203083.
[14] W. Lim, S. Lee, M. Sunwoo, and K. Jo, “Hybrid trajectory planning for
autonomous driving in on-road dynamic scenarios,” IEEE Trans. Intell.
Transp. Syst., vol. 22, no. 1, pp. 341–355, Jan. 2021.
[15] Y. Liu et al., “Dynamic lane-changing trajectory planning for autonomous
vehicles based on discrete global trajectory,” IEEE Trans. Intell. Transp.
Syst., vol. 23, no. 7, pp. 8513–8527, Jul. 2022.
[16] X. Li, Z. Sun, D. Cao, Z. He, and Q. Zhu, “Real-time trajectory planning
for autonomous urban driving: Framework, algorithms, and verifications,”
IEEE/ASME Trans. Mechatronics., vol. 21, no. 2, pp. 740–753, Apr. 2016.
[17] W. Lim, S. Lee, M. Sunwoo, and K. Jo, “Hierarchical trajectory planning
of an autonomous car based on the integration of a sampling and an
optimization method,” IEEE Trans. Intell. Transp. Syst., vol. 19, no. 2,
pp. 613–626, Feb. 2018.
[18] Y. Wang et al., “Dynamic trajectory planning of autonomous lane change
at medium and low speeds based on elastic soft constraint of the safety
domain,” Autom. Innov., vol. 3, no. 1, pp. 73–87, 2020.
[19] Y. Yan, J. Wang, K. Zhang, Y. Liu, Y. Liu, and G. Yin, “Driver’s individual
risk perception-based trajectory planning: A human-like method,” IEEE
Trans. Intell. Transp. Syst., vol. 23, no. 11, pp. 20413–20428, 2022.
[20] Y. Wang et al., “An event-triggered scheme for state estimation of preced-
ing vehicles under connected vehicle environment,” IEEE Trans. Intell.
Veh . , early access, Jun. 13, 2022, doi: 10.1109/TIV.2022.3181330.
[21] V. Jain, U. Kolbe, G. Breuel, and C. Stiller, “Reacting to multi-obstacle
emergency scenarios using linear time varying model predictive control,”
in Proc. IEEE Intell. Veh. Symp., 2019, pp. 1822–1829.
[22] J. Wang, Y. Yan, K. Zhang, Y. Chen, M. Cao, and G. Yin, et al., “Path
planning on large curvature roads using driver-vehicle-road system based
on the kinematic vehicle model,” IEEE Trans. Veh. Technol., vol. 71, no. 1,
pp. 311–325, Jan. 2022.
[23] M. Beglini, T. Belvedere, L. Lanari, and G. Oriolo, “An intrinsically
stable MPC approach for anti-jackknifing control of tractor-trailer ve-
hicles,” IEEE/ASME Trans. Mechatronics, early access, Mar. 23, 2022,
doi: 10.1109/TMECH.2022.3154295.
[24] B. R. Kiran et al., “Deep reinforcement learning for autonomous driving:
A survey,” IEEE Trans. Intell. Transp. Syst., vol. 23, no. 6, pp. 4909–4926,
Jun. 2022.
[25] S. Aradi, “Survey of deep reinforcement learning for motion planning of
autonomous vehicles,” IEEE Trans. Intell. Transp. Syst., vol. 23, no. 2,
pp. 740–759, Feb. 2022.
[26] H. Wang et al., “Risk assessment and mitigation in local path planning for
autonomous vehicles with LSTM based predictive model,” IEEE Trans.
Autom. Sci. Eng., vol. 19, no. 4, pp. 2738–2749, Oct. 2022.
[27] K. B. Naveed, Z. Qiao, and J. M. Dolan, “Trajectory planning for au-
tonomous vehicles using hierarchical reinforcement learning,” in Proc.
IEEE Int. Conf. Intell. Transp. Syst., 2021, pp. 601–606.
[28] S. Liu, Z. Hou, T. Tian, Z. Deng, and Z. Li, “A novel dual successive
projection-based model-free adaptive control method and application to
an autonomous car,”IEEE Trans. Neural Netw.Learn. Syst., vol. 30, no. 11,
pp. 3444–3457, Nov. 2019.
[29] C. Huang et al., “Personalized trajectory planning and control of lane-
change maneuvers for autonomous driving,” IEEE Trans. Veh. Technol.,
vol. 70, no. 6, pp. 5511–5523, Jun. 2021.
[30] J. Nilsson, J. Silvlin, M. Brannstrom, E. Coelingh, and J. Fredriksson, “If,
when, and how to perform lane change maneuvers on highways,” IEEE
Intell. Transp. Syst. Mag., vol. 8, no. 4, pp. 68–78, Oct. 2016.
[31] P. Hang, C. Lv, C. Huang, Y. Xing, and Z. Hu et al., “Cooperative decision
making of connected automated vehicles at multi-lane merging zone: A
coalitional game approach,”IEEE Trans. Intell. Transp.Syst., vol. 23, no. 4,
pp. 3829–3841, Apr. 2022.
[32] Z. Wang, X. Zhou, and J. Wang, “Extremum-seeking-based adaptive
model-free control and its application to automated vehicle path track-
ing,” IEEE/ASME Trans. Mechatronics, vol. 27, no. 5, pp. 3874–3884,
Oct. 2022.
[33] D. Marinescu, J. Curn, M. Bouroche, and V. Cahill, “On-ramp traffic
merging using cooperative intelligent vehicles: A slot-based approach,”
in Proc. IEEE Int. Conf. Intell. Transp. Syst., 2012, pp. 900–906.
[34] C. Huang, C. Lv, P. Hang, and Y. Xing, “Toward safe and personal-
ized autonomous driving: Decision-making and motion control with DPF
and CDT techniques,” IEEE/ASME Trans. Mechatronics., vol. 26, no. 2,
pp. 611–620, Apr. 2021.
[35] J. Weng, G. Li, and Y. Yu, “Time-dependent drivers’ merging behavior
model in work zone merging areas,” Transp. Res., C. Emerg. Technol.,
vol. 80, pp. 409–422, 2017.
[36] J. Nilsson, M. Brännström, E. Coelingh, and J. Fredriksson, “Lane change
maneuvers for automated vehicles,” IEEE Trans. Intell. Transp. Syst.,
vol. 18, no. 5, pp. 1087–1096, May 2016.
[37] Q. H. Do, H. Tehrani, S. Mita, M. Egawa, K. Muto, and K. Yoneda,
“Human drivers based active-passive model for automated lane change,”
IEEE Intell. Transp. Syst. Mag., vol. 9, no. 1, pp. 42–56, Jan. 2017.
[38] M. Treiber, A. Hennecke, and D. Helbing, “Congested traffic states in
empirical observations and microscopic simulations,” Phys.Rev.E, vol. 62,
no. 2, pp. 1805–1824, 2000.
Yongjun Yan received the B.S. degree in vehi-
cle engineering from Nanjing Agricultural Uni-
versity, Nanjing, China, in 2017, and the M.S.
degree in vehicle engineering from Southeast
University, Nanjing, in 2020. He is currently
working toward the Ph.D. degree in vehicle en-
gineering.
His current research interests include vehicle
dynamics and control, automotive active safety
control, intelligent vehicle decision-making and
control, and personalized driving.
Lin Peng received the B.S. degree in vehicle
engineering from the Hefei University of Tech-
nology, Hefei, China, in 2020. He is currently
working toward the M.S. degree in vehicle en-
gineering with the School of Mechanical Engi-
neering, Southeast University, Nanjing, China.
His research interests include trajectory plan-
ning and trajectory tracking for autonomous
vehicle.
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.
Authorized licensed use limited to: Southeast University. Downloaded on November 23,2022 at 08:55:30 UTC from IEEE Xplore. Restrictions apply.
12 IEEE/ASME TRANSACTIONS ON MECHATRONICS
Jinxiang Wang (Member, IEEE) received the
B.S. degree in mechanical engineering and au-
tomation and the Ph.D. degree in vehicle en-
gineering from Southeast University, Nanjing,
China, in 2002 and 2010, respectively.
From 2014 to 2015, he was a Visiting Re-
search Scholar with the Department of Me-
chanical and Aerospace Engineering, The Ohio
State University, Columbus, OH, USA. He is cur-
rently an Associate Professor with the School of
Mechanical Engineering, Southeast University,
Nanjing. His research interests include vehicle dynamics and control,
assisted-driving system, and control on autonomous vehicles.
Hui Zhang (Senior Member, IEEE) received the
B.Sc. degree in mechanical design manufactur-
ing and automation from the Harbin Institute of
Technology, Weihai, China, in 2006, the M.Sc.
degree in automotive engineering from Jilin Uni-
versity, Changchun, China, in 2008, and the
Ph.D. degree in mechanical engineering from
the University of Victoria, Victoria, BC, Canada,
in 2012.
He was a Research Associate with the De-
partment of Mechanical and Aerospace Engi-
neering, Ohio State University, Columbus, OH, USA. He is an author
or co-author of more than 80 peer-reviewed papers on journals and
conference proceedings. His research interests include diesel engine
aftertreatment systems, vehicle dynamics and control, mechatronics,
robust control and filtering, networked control systems, and signal
processing.
Tong Shen received the M.S. degree in vehicle
engineering, in 2021, from Southeast University,
Nanjing, China, where he is currently working
toward Ph.D. degree in vehicle engineering with
the School of Mechanical Engineering.
His research interests include vehicle dynam-
ics and control, active safety control, and au-
tonomous vehicle
Guodong Yin (Senior Member, IEEE) received
the Ph.D. degree in vehicle engineering from
Southeast University, Nanjing, China, in 2007.
He is currently a Professor with the School of
Mechanical Engineering, Southeast University.
His current research interests include vehicle
dynamics and control, connected vehicles, and
multiagent control, where he has authored and
co-authored a monograph and contributed three
book chapters, more than 150 papers, and ob-
tained 25 granted patents.
This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.
Authorized licensed use limited to: Southeast University. Downloaded on November 23,2022 at 08:55:30 UTC from IEEE Xplore. Restrictions apply.
