Conference PaperPDF Available

Path Planning using a Kinematic Driver-Vehicle-Road Model with Consideration of Driver's Characteristics

Authors:

Figures

Content may be subject to copyright.
Path Planning using a Kinematic Driver-Vehicle-Road Model with
Consideration of Driver’s Characteristics
Yongjun Yan, Jinxiang Wang, Kuoran Zhang, Mingcong Cao, and Jiansong Chen
Abstract A driver-vehicle-road (DVR) model based on kine-
matic vehicle model is proposed in this paper. In this DVR
model, the kinematics vehicle-road model is adopted, and the
driver model considering the human driver’s characteristics is
also included. Thus the behaviors of human driver’s preview
and neuromuscular delay can be considered in design of
path planner and controller by using this DVR model. The
repulsive force field based on the artificial potential field (APF)
and the circle decomposition of vehicle shape are used to
describe the constraints of obstacle avoidance and the road
departure avoidance. Based on the proposed DVR model, a
trajectory planer using model predictive control (MPC) is
designed with consideration of collision and lane-departure
avoidance, driver’s intention, and vehicle occupant comfort.
Simulation results show that with the proposed planner, the
vehicle can successfully avoid static/moving obstacles and return
to the original lane without lane departure. Simulation results
indicate that the proposed kinematic vehicle model based DVR
model can be used to design the path planner in normal driving
and some typical driving scenarios. And the proposed path
planner can provide the vehicle driven by different human
drivers with individually safe trajectories in typical scenarios
of obstacle avoidance.
I. INTRODUCTION
Road traffic death is one of the leading causes of death in
the world, according to the road safety report of the World
Health Organization [1]. One solution to reduce road deaths
effectively is development and implementation of the fully
autonomous vehicles, but compared with the autonomous
vehicles, it is more practical to use semi-autonomous vehicles
currently. In semi-automated driving, the driver’s role is still
very important. So the driving characteristics of different
drivers were taken into consideration in model construction
for controller design [2], [3].
Dynamic and kinematic vehicle model are generally used
to design the vehicle lateral motion controllers [4]. Dynamic
model is more widely used than the kinematic one, because
the former can reflect the lateral motion of vehicles more
precisely. However, the dynamic model requires many vehi-
cle parameters, and most of these parameters have nonlinear
characteristics such as side slip angle, cornering stiffness,
tire parameters, etc. This will increase the computational
complexity and reduce the real-time performance of the
vehicle motion control [5]. In [6], [7], the effectiveness of
*This work is partially supported by National Natural Science Foundation
of China (NSFC) under Grant 51675099 and U1664258, National Key R&D
Program of China under Grant 2018YFB1201602, and Qing Lan Project of
Jiangsu Province. All correspondence should be sent to J. Wang (Email:
wangjx@seu.edu.cn).
The authors are with the School of Mechanical Engineering, Southeast
University, Nanjing, 211189, China
the kinematic model in both path planning and path tracking
at different vehicle speeds was verified. Kang et al [8] found
that There was small change of front tire steering angle
in highway autonomous driving, and the kinematic vehicle
model was competent for lateral motion control for highway
autonomous vehicles.
For designing the assistance controller in path planning
and path tracking, many researchers considered the combi-
nation of driver and vehicle dynamic model [9]. Parameters
of the driver’s steering characteristics such as delay time,
preview time, and steering gain were used to represent
drivers with different driving behaviors [10]. By consid-
ering these characteristics of drivers in controller design,
the performance of the assistance controller can be more
human-like. Thus in human-machine shared control, the
behavior of the controller is closer to the behavior of the
real human driver, which can make the human driver feel
more comfortable. On the other hand, with consideration of
the computational complexity by using the vehicle dynamic
model, the kinematic vehicle model is more efficient to be
applied for building the driver-vehicle-road (DVR) model for
path planning.
The methods to realize real-time path planning is im-
portant for collision avoidance. The artificial potential field
(APF) has been used to make collision-avoidance decisions
[11]–[13]. The APF method was implemented in generating
safety-oriented trajectory considering of both vehicle stabil-
ity and environmental constraints [11], [12]. In [13], imag-
inary mountains of repulsive force fields were introduced
to detect collision threat and lane boundaries. In order to
describe the relationship between the ego vehicle and the
obstacle vehicle better, the method of circle decomposition
was adopted to describe the shape of vehicles. In [14], the
collision-avoidance constraints between vehicles and obsta-
cles were transformed into limit of the distance between
center of the circles and the constraint polygons of obstacle.
The APF method and the circle decomposition method were
combined to solve the collision-avoidance problem under
extreme driving conditions in [13].
In recent years, Model predictive control (MPC) has been
used in local trajectory planning due to its ability to better
deal with system constraints and nonlinearities [15]. In [16],
a nonlinear MPC was utilized for obstacle avoidance of
high-speed autonomous ground vehicles in unstructured en-
vironments. In [17], an MPC method was adopted to realize
trajectory planning by dealing with the interactions between
different driver-vehicle systems. However, the computational
burden of solving MPC problems in real time increased [15].
2019 IEEE Intelligent Vehicles Symposium (IV)
Paris, France. June 9-12, 2019
978-1-7281-0560-4/19/$31.00 ©2019 IEEE 2259
So the linear kinematic vehicle model and point-mass vehicle
model were used to reduce the computational burden in MPC
[18], [19].
In this paper, a kinematic vehicle model based driver-
vehicle-road (DVR) model is proposed for path planning,
with consideration of driver’s steering characteristics. The
repulsive force field based on the artificial potential field
(APF) method and the circle decomposition of vehicle shape
are used to represent the constraints of collision-avoidance
and road departure. MPC method is adopted to synthesize the
methods poposed above to design the trajectory replanning
algorithm.
The rest of this paper is organized as follows. In Section
2, the kinematic vehicle model based DVR model is build,
and the path tracking performance of the kinematic vehicle
model based DVR model and the Carsim vehicle model
based DVR model are compared. Section 3 presents the path
planner design through the multi-objective MPC. Section 4
presents the simulation results of the proposed path planning
method in scenarios of avoiding obstacle vehicle. Section 5
is a conclusion of this paper.
II. DRIVER-VEHI CLE-ROAD MO DEL
In this section, in order to make the design of vehicle
controller more individual and human-like, the driver and
vehicle are considered as a whole system, i.e., the DVR
model. Then performance of the proposed DVR model is
validated through simulation.
A. Kinematic vehicle model based DVR model
In order to simplify the complexity of calculation, the
kinematic vehicle-road model proposed in [5] is adopted to
substitute the more generally used vehicle dynamic model.
As indicated in Fig.1, by assuming that Vxis a constant,
the front tire steering angle
δ
fand yaw angle
ψ
of vehicle
are small angles, cos
ψ
=1, tan
ψ
=
ψ
, cos
δ
=1, tan
δ
=
δ
,
the kinematic vehicle-road model can be approximated as
˙ey=˙y˙ydes
=lr
lf+lrVxtan
δ
f+Vx
ψ
Vx
ψ
des
=lr
lf+lrVx
δ
f+Vxe
ψ
(1)
The yaw motion of vehicle can be described as
˙
ψ
=Vx
lf+lrtan
δ
f=Vx
lf+lr
δ
f
¨
ψ
=Vx
lf+lr
˙
δ
f
(2)
Fig. 1. Kinematic vehicle-road model with look-ahead distance
Where lfand lrare distances from front and rear axles to
the vehicle’s center of gravity (CG), respectively. Vxis the
speed of vehicle. ydes and yare the target and current lateral
positions of the vehicle CG , respectively. ey=yydes repre-
sents the error between the current position of the vehicle and
the reference path.
ψ
is the yaw angle of the vehicle’s current
position,
ψ
des is the tangent of the corresponding position on
the reference path, and e
ψ
=
ψ
ψ
des is the current angle
deviation of the vehicle.
To consider the driver’s steering characteristics in the DVR
model, the following driver model introduced in [10] is
adopted.
¨
δ
f=1
a0T2
d
δ
f1
a0Td
˙
δ
f+RgGh
a0T2
d
[Yp(y+TpVx
ψ
)] (3)
Where, Td,Tp, and Ghare the delay time, preview time,
and the steering proportional gain, respectively. For conve-
nience, they are described as
ρ
1,
ρ
2,
ρ
3in the following. a0
is a constant, and Rgis gear ratio of the steering system. Yp
and y+TpVx
ψ
describe the lateral position of the preview
point and the predicted position of the vehicle, respectively.
The discrete-time system state vector of
the DVR model is defined as x(k)=
[x1(k),x2(k),x3(k),x4(k),x5(k),x6(k),x7(k)]T, with
x1(k) = ey(k),x2(k) = e
ψ
(k),x3(k) = ˙
ψ
(k),x4(k) =
ψ
(k),
x5(k) = y(k),x6(k) =
δ
f(k), and x7(k) = ˙
δ
f(k). And the input
of the DVR model is defined as u(k)= [ ˙
ψ
des (k),Yp(k)]T.
Then by assembling (1) to (3), the DVR state-pace model
can be described as
x(k+ 1) =Ax(k) + Buu(k)(4)
where
A=
1VxT0 0 0 lr
lf+lrVxT0
0 1 T0 0 0 0
0 0 1 0 0 0 Vx
lf+lrT
0 0 T1 0 0 0
0 0 0 VxT1lr
lf+lrVxT0
0 0 0 0 0 1 T
0 0 0 Rg
ρ
2
ρ
3
a0
ρ
2
1
VxTRg
ρ
3
a0
ρ
2
1
TT
a0
ρ
2
1
1T
a0
ρ
1
(5)
Bu=0T0 0 0 0 0
0 0 0 0 0 0 Rg
ρ
3
a0
ρ
2
1
TT
(6)
A diagram of the proposed kinematic vehicle model based
DVR model for tracking the desired path is shown in Fig. 2.
Fig. 2. Kinematic vehicle model based DVR model
Remark: With consideration of steering characteristics of
the human driver in the DVR model, handling behaviors and
preference of the human driver can be considered when the
2260
DVR model is applied in path planning or path tracking.
On the other hand, because the kinematic vehicle model is
applied, the proposed DVR model is essentially linear and
the computation cost is reduced when using this model for
intricate path planning.
B. Validation of the kinematic vehicle model based DVR
model
In order to verify effectiveness of the DVR model, the path
tracking performance of the proposed kinematic model based
DVR model and the nonlinear DVR model are compared. In
the nonlinear DVR model, the part of vehicle model (2) is
replaced by the high-fidelity, CarSim, full vehicle model, and
the parts of vehicle-road and driver model are remained as
(1) and (3). The path for tracking is chosen as trajectory of
single lane change. A polynomial trajectory of single lane-
changing maneuver proposed in [20] is adopted, from which
the system input of the DVR models u(k)can be calculated.
The comparison of tracking effects are shown in Fig. 3-4.
40 60 80 100 120 140 160
X(m)
0
1
2
3
4
Y(m)
Reference path
Non-linear model,Vx=10m/s
Kinematics model,Vx=10m/s
Non-linear model,Vx=20m/s
Kinematics model,Vx=20m/s
Non-linear model,Vx=25m/s
Kinematics model,Vx=25m/s
Fig. 3. Trajectory of vehicle CG
40 60 80 100 120 140 160
X(m)
-0.6
-0.4
-0.2
0
0.2
0.4
ey (m)
Non-linear model,Vx=10m/s
Kinematics model,Vx=10m/s
Non-linear model,Vx=15m/s
Kinematics model,Vx=15m/s
Non-linear model,Vx=25m/s
Kinematics model,Vx=25m/s
Fig. 4. Path tracking errors
In Fig. 3, the path tracking performance of the two DVR
models in different speeds are compared. As indicated in
Fig.4, without the consideration of the vehicle’s nonlinear
dynamic characteristics such as cornering stiffness and side
slip, the tracking offsets for the kinematics vehicle model
based DVR model are larger than those of the non-linear
DVR model. However, it can be observed from Fig. 3-
5 that the path tracking error for the kinematics vehicle
model based DVR model is acceptable when the vehicle
side slip angle is not larger than 0.6 deg, at vehicle speeds
smaller than 20m/s in this single-lane-change scenario. This
results indicate that the proposed DVR model can be used
to substitute the nonlinear model for path tracking and path
planning in scenarios of normal driving and in conditions
with large margin of vehicle lateral stability.
0 20 40 60 80 100 120 140 160 180
X(m)
-1.5
-1
-0.5
0
0.5
1
1.5
Side slip angle(deg)
Non-linear model,Vx=10m/s
Non-linear model,Vx=15m/s
Non-linear model,Vx=20m/s
Non-linear model,Vx=25m/s
Non-linear model,Vx=30m/s
Fig. 5. Response of the vehicle side slip angle for results with the nonlinear
DVR model
III. PATH PLANNING USIN G THE DVR MODE L
A. Constraints of the obstacle avoidance and lane departure
avoidance
In this paper, a circle decomposition method proposed
in [14], [21] was adopted. As shown in figure 6, the cir-
cle decomposition method can better describe the size of
vehicles, and the constraints of safe distance between two
vehicles. The heading angle of vehicle can be determined
by the center of these circles, which is helpful to reduce the
vehicle’s course deviation [13].
Fig. 6. Environmental constraints to keep a safe distance from obstacle
vehicle
When the circle decomposition method is used, size of the
vehicle should be taken into consideration. Too few circles
will increase the safety margin of the vehicle, which is not
conductive to avoid obstacle vehicle. At the same time, too
many circles will increase the complexity of calculation. In
Fig. 6, we divide the ego vehicle and the obstacle vehicle
into three circles. The distance between the center points of
circles covering on ego vehicle and obstacle vehicle must be
larger than the sum of the radius of these two circles, thus
the environmental constraint can be defined as
dirveh +robs i=1,2..., 9 (7)
Where, diis the distance between every two circles
covering on ego vehicle and obstacle vehicle, and i=33 is
the number of the pairs of two circles from obstacle vehicle
and ego vehicle. rveh and robs are the radius of the circles
covering on ego vehicle and obstacle vehicle, respectively.
Besides the requirement of avoiding the obstacle vehicles,
the ego vehicle should not violate the lane boundaries as
shown in Fig 6 , thus the following constraints should be
satisfied.
yiLanerrveh i=1,2,3
Lanelyirveh
(8)
Where, yiis the ordinate of each circle center of ego vehicle,
and Laneland Lanerare the ordinate of left and right lane
2261
boundaries, respectively.
B. Path planning using multi-objective MPC
For avoiding obstacles, the following objectives should be
realized: a) successfully avoid obstacles; b) cannot exceed
the lane; c) ensure comfort of the drivers; d) return to the
original lane after obstacle avoidance. To consider all of these
objectives, the objective function JMotion should consist of
five terms as follows.
JMotion =min(JRe f ,t+JObs,t+JLane,t+JLoad,t+Ju,t)(9)
Where t is the current step, JRe f is the cost for tracking
the driver’s original driving intention, JObs and JLane repre-
sent the costs considering constraints of obstacle and lane
departure avoidance respectively, JLoad is cost to reduce the
driver workload, and Juis the cost to reduce the system input.
In order to satisfy objectives a) and b), which were
described as inequality constraints in previous section, the
repulsive force field based on the APF method is applied.
The repulsive force becomes greater as the ego vehicle
approaches the obstacle vehicle and the lane boundary. The
objective functions JObs,JLane based on APF can be found
in [13].
Ji
Lp,k(Xi
t,Yi
t) = aq(Di
Lp,k(Xi
t,Yi
t)rveh))
κ
(10)
JLanek,t=
p=l,r
(max
1iMcir
Ji
Lp,k(Xi
t,Yi
t)),(11)
where DLp,kdenotes the distance between the center of it h
circle covering the ego vehicle and the lane boundaries at
the time step k.rveh denotes the radius of the circle covering
the ego vehicle. p=l,rrepresents the right lane or left lane,
aqand
κ
are weight and parameter of the repulsive force,
respectively. Mcir =3 is the number of the circles covering
on the ego vehicle.
The obstacle vehicle is considered to be the preceding
vehicle on the current lane of the ego vehicle. The repulsive
force field is designed to avoid collision while maintaining
the stability of the ego vehicle.
Ji
O,k(Xi
t,Yi
t) =
Moc
j=1
(a0eb0Dj
ON,k(Xi
t,Yi
t))(12)
JObsk,t=max
1iMcir
Ji
O,k(Xi
t,Yi
t),(13)
Where Dj
ON,kis the distance between the center of ith circle
of the ego vehicle and the center of jth circle of the obstacle
vehicle. Moc represents the number of circles covering the
obstacle vehicle, a0and b0are constant weights.
In order to reduce the error between the current path and
the driver’s original intention path, ey,e
ψ
, and yare assumed
to follow their references, described as follows.
JRe f ,t=
t+Np
k=t+1
||
η
k,t
η
re fk,t||2
Q(14)
η
k,t=C1xk,t,
η
re fk,t=0
C1=
1000000
0100000
0000100
(15)
where, Npis to the predictive horizon, C1is the selective
matrix,
η
k,t= [ey,e
ψ
,y]T.
For the objective of c), the driver’s physiological and
psychological workload should be taken into account in path
replanning as follows.
JLoad,t=
t+Np
k=t+1
||
ξ
k,t
ξ
re fk,t||2
P(16)
ξ
k,t=C2xk,t,
ξ
re fk,t=0
C2=0000010
0000001(17)
Where C2is the selective matrix,
ξ
k,t= [
δ
f,˙
δ
f]T.
In order to restrict the increment of the system input on
each time step to obtain smoother path, the following cost
function is adopted.
Ju,t=
t+Np
k=t+1
||uk,t||2
R(18)
In conclusion, by applying the kinematic vehicle model
based DVR model, the path planner for collision avoidance
using APF and MPC can be obtained as
min
Ut
t+Np
k=t+1
(||
η
k,t
η
re fk,t||2
Q+||
ξ
k,t
ξ
re fk,t||2
P+ (19)
||uk,t||2
R+JLanek,t+JObsk,t)
s.t.xk+1,t=Axk,t+Buuk,t
umin uk,tumax
umin uk,tumax
uk,t=uk,tuk1,t
k=t,t+1,...,t+Np1
ut1,t=u(t1)
xt,t=x(t).
Where, Q, P, and R are the weight parameters for path track-
ing error, driver’s physiological and psychological workload,
and the system input, respectively.
IV. SIMULATION RESULTS
In this section, numerical simulations of path planning for
static/moving obstacle avoidance were conducted, with con-
sideration of different human drivers. The main parameters
of the vehicle kinematic vehicle model based DVR model
are listed in Table 1 .
In order to verify effectiveness of the proposed path
planner based on APF and multi-objective MPC, two drivers
with different steering characteristics shown in Table 2 are
selected to establish the proposed kinematical DVR model.
The introduction of driver characteristics can be found in
[22]. Driver A is selected as an experienced driver, due to
the shorter delay time Tdand larger steering proportional
2262
TABLE I
PARAMETER VAL UE S OF T HE VE HI C LE MO DE L
Symbol Meaning Value
lfDistance between CG and front axle 1.035m
lrDistance between CG and rear axle 1.665m
LVehicle length 4.5m
rveh Circle radius of ego vehicle 1.25m
robs Circle radius of obstacle vehicle 1.25m
Rg Gear ratio of steering angle 1/16
TABLE II
CHARACTERISTIC PARAMETERS OF DRIVE RS
Driver Td(s) Tp(s) Gh
A 0.15 0.78 0.85
B 0.3 0.88 0.5
gain Gh, while Driver B is less experienced driver with
longer delay time. Besides, we determined weights in the
cost function of the path planner as Q=diag(100,100,5),
P=diag(500,1000),R=diag(100,400). The road width is
7.2m, with double lanes in the same direction. Center line
of the right lane is taken as the X axis. The sample time
T=0.05s, the prediction horizon Npand control horizon Nc
are both set as 30.
In the following simulations of scenarios with both static
and moving obstacles, we choose the ego vehicle speed as
25 m/s. With this vehicle speed, the vehicle side slip angles
in the process of the obstacle avoidance in both scenarios are
not larger than 0.6 deg, which is satisfied for the proposed
DVR model to be used for path planning.
A. Static obstacle avoidance with consideration of different
drivers’ characteristics
In Fig. 7-10, performances of path planning for static ob-
stacle avoidance considering different drivers are compared.
0 50 100 150 200 250
X(m)
-2
0
2
4
6
Y(m)
Real path of Driver A
Real path of Driver B
Fig. 7. Path planning trajectory of vehicle CG.
In Fig. 7, the real trajectory of the vehicles are shown.
It can be seen, both of the drivers could avoid the static
obstacle vehicle along the planned path. As shown in Fig.
8, the obstacle vehicle is represented by three black circles,
and the ego vehicle maneuvered by Driver A is represented
with three red circles for showing the process of obstacle
avoidance more evidently.
70 80 90 100 110 120 130
X(m)
-5
0
5
10
Y(m)
Real path of Driver A
Real path of Driver A
Road bountry
Fig. 8. Circle decomposition method used in static obstacle avoidance.
0246810
Time(s)
-1.5
-1
-0.5
0
0.5
1
Steering angle(deg)
Driver A
Driver B
Fig. 9. Vehicle front tire steering angle.
Fig. 10. Response of vehicle side slip angle.
In Fig. 9 and 10 show the front tire steering angle and side
slip angle of the vehicle in process of obstacle avoidance.
The side slip angle of the vehicle is less than 0.6 deg, which
satisfies the assumption of non/small vehicle slip for using
the kinematic vehicle model.
B. Moving obstacle avoidance with consideration of different
drivers’ characteristics
In Fig. 11-14, performances of path planning for moving
obstacle avoidance considering different drivers are com-
pared. The speed of obstacle vehicle is set as 15m/s.
0 100 200 300 400 500
X(m)
-2
0
2
4
6
Y(m)
Real path of Driver A
Real path of Driver B
Real path of Obstacle
Fig. 11. Trajectory of vehicle CG.
As indicated in Fig. 11, both of the drivers could avoid
the moving obstacle vehicle along the planned pathes. The
black boxes represent the trajectory of the obstacle vehicle in
the process of collision-avoidance. Fig. 12 depicts the real
trajectories of the two vehicles in obstacle avoidance. The
positions of the ego vehicle and the moving obstacle vehicle
at the same time are connected by double-headed arrows. It
can be seen in Fig. 12 that the ego vehicle can avoid collision
with the moving obstacle vehicle. Fig. 13 and 14 show the
front tire steering angle and the side slip angle of the ego
2263
vehicle with different drivers. The maximum side slip angle
of the vehicles is also smaller than 0.6 deg.
Fig. 12. Circle decomposition method used in moving obstacle avoidance.
0 5 10 15 20
Time(s)
-1
-0.5
0
0.5
1
Steering angle(deg)
Driver A
Driver B
Fig. 13. Response of vehicle front tire steering angles.
Fig. 14. Response of vehicle side slip angle.
As indicated in Fig. 9 and Fig.13, Driver A was an
experienced driver with a smaller reaction time, who steered
the front steering wheel earlier than Driver B, although
the preview time of Driver A is 0.1s less than that of
Driver B. The results show that the proposed DVR model
can consider the drivers’ different steering characteristic in
path planning. Thus the proposed path planner can provide
different drivers with individual safe path to follow. When
the driver assistance steering controller [10] is applied,
the path-planning/path-tracking controller can provide more
personalized driving assistance for the drivers.
V. CONCLUSION AND FUT UR E WO RK
In this paper, the kinematic vehicle model is applied to
established the DVR dynamic model, with consideration of
driver’s steering characteristics. The repulsive force field
based on the APF method and the circle decomposition of
vehicle shape are used to represent the constraints of collision
avoidance. MPC method is adopted to design the trajectory
replanning algorithm considering these constraints based on
the proposed DVR model. Simulation results show that the
proposed path planner can assist different human drivers
with individual safe paths to avoid static/moving obstacle. In
our future work, the proposed DVR model will be used to
deal with more intricate conditions of path planning, such as
scenarios with multi surrounding vehicles for the ego vehicle.
REF ER E NC ES
[1] World Health Organization, Global status report on road safety 2015,
World Health Organization, 2015.
[2] Abe M, Vehicle handling dynamics: theory and application,
Butterworth-Heinemann, 2015.
[3] Oh K, Yi K, A predictive driver model with physical constraints
for closed loop simulation of vehicle-driver system, IEEE, Intelligent
Transportation Systems, 2014: 3126-3131.
[4] Rajamani R, Vehicle dynamics and control, Springer Science Business
Media, 2011.
[5] Kang C M, Lee S H, Chung C C, A comparative study of lane keeping
system: Dynamic and kinematic models with look-ahead distance,
IEEE, Intelligent Vehicles Symposium, 2015: 1038-1043.
[6] Polack P, Altché F, d’Andréa-Novel B, The kinematic bicycle model:
A consistent model for planning feasible trajectories for autonomous
vehicles? IEEE, Intelligent Vehicles Symposium , 2017: 812-818.
[7] Kong J, Pfeiffer M, Schildbach G, Kinematic and dynamic vehicle
models for autonomous driving control design, Intelligent Vehicles
Symposium, 2015: 1094-1099.
[8] Kang C M, Lee S H, Chung C C, Comparative evaluation of dynamic
and kinematic vehicle models, Decision and Control, IEEE, 53rd
Annual Conference, 2014: 648-653.
[9] Sentouh C, Nguyen A T, Benloucif M A, Driver-automation coop-
eration oriented approach for shared control of lane keeping assist
systems, IEEE, Transactions on Control Systems Technology, 2018
(99): 1-17.
[10] Wang J, Zhang G, Wang R, A gain-scheduling driver assistance
trajectory-following algorithm considering different driver steering
characteristics, IEEE Transactions on Intelligent Transportation Sys-
tems, 18(5):1097-1108, 2017.
[11] Abdul Hamid U Z, Zamzuri H, Yamada T, Modular design of
artificial potential field and nonlinear model predictive control for
a vehicle collision avoidance system with move blocking strategy,
Proceedings of the Institution of Mechanical Engineers, Part D: Journal
of Automobile Engineering, 232(10): 1353-1373, 2018.
[12] Rasekhipour Y, Khajepour A, Chen S K, A potential field-based model
predictive path-planning controller for autonomous road vehicles,
IEEE Transactions on Intelligent Transportation Systems, 18(5): 1255-
1267,2017.
[13] Cao M, Wang R, Wang J, An Integrated MPC Approach for FWIA
Autonomous Ground Vehicles with Emergency Collision Avoidance,
IEEE, 21st International Conference on Intelligent Transportation
Systems, 2432-2437, 2018.
[14] Ziegler J, Bender P, Dang T, Trajectory planning for BerthaA local,
continuous method, IEEE, Intelligent Vehicles Symposium Proceed-
ings, 450-457, 2014.
[15] Dixit S, Fallah S, Montanaro U, Trajectory planning and tracking for
autonomous overtaking: State-of-the-art and future prospects, Annual
Reviews in Control, 2018.
[16] Liu J, Jayakumar P, Stein J L, A nonlinear model predictive control
formulation for obstacle avoidance in high-speed autonomous ground
vehicles in unstructured environments, Vehicle System Dynamics,
6(6): 853-882, 2018.
[17] Zhang K, Wang J, Chen N, A Comparison of Two Distributed
V2V Trajectory-planning Algorithms with Consideration of Drivers’
Characteristics, IEEE, Annual American Control Conference, 4014-
4019, 2018.
[18] Kim B, Kim D, Park S, Automated Complex Urban Driving based on
Enhanced Environment Representation with GPS/map, Radar, Lidar
and Vision, IFAC-PapersOnLine, 49(11): 190-195, 2016.
[19] Schildbach G, Borrelli F, Scenario model predictive control for lane
change assistance on highways, Intelligent Vehicles Symposium, 611-
616, 2015.
[20] Chee W, Tomizuka M, Vehicle lane change maneuver in automated
highway systems, 1994.
[21] Lim W, Lee S, Sunwoo M, Hierarchical Trajectory Planning of an
Autonomous Car based on the Integration of a Sampling and an
Optimization Method, IEEE Transactions on Intelligent Transportation
Systems, 19(2): 613-626, 2018.
[22] Wun Chai Y, Abe Y, Kano Y, A study on adaptation of SBW param-
eters to individual drivers steer characteristics for improved driverve-
hicle system performance, Vehicle System Dynamics, 44(sup1): 874-
882, 2006.
2264
... This method could convert the positional relationship between the host vehicle, obstacles, and traffic line marks into distance relationships between centers of the circles and the constraint polygons of the obstacles, and relationships between centers of the circles and the straight lane boundaries. Compared with the traditional method of treating the vehicle as a rectangle, the circle decomposition method could more easily describe whether the host vehicle has a potential crash with the surrounding vehicles, and whether the host vehicle has exceeded the traffic line marks [29], [30]. ...
Article
Full-text available
Lane-changing is a critical issue for autonomous vehicles (AVs), especially in complex environments. In addition, different drivers have different handling preferences. How to provide personalized maneuvers for individual drivers to increase their trust is another issue for AVs. Therefore, a framework of human-like path planning is proposed in this paper, considering driver characteristics of visual-preview, subjective risk perception, and degree of aggressiveness. In the decision making module, a model is built to select the most suitable merging spot, with respect to safety factors and the driver’s degree of aggressiveness. And a novel environmental potential field (PF) suitable for arbitrary road structures is designed to describe the driver’s individual risk perception. In the trajectory planning module, a model predictive control (MPC) based path planner is designed according to the decisions in coincidence with the driver’s individual intentions of collision avoidance. Simulation results have demonstrated that the proposed path planner can provide with personalized trajectories for different combinations of driver preferences and steering characteristics, in scenarios of curved roads with different risks of collision.
... Potential field collision avoidance constraint has been employed in former researches [8][9][10][11][12][13][14][15][16][17][18][19][20][21][22], with only the vehicle body potential field for collision avoidance being considered. In fact, with an appropriate distance between vehicles, passengers will have a sense of safety. ...
Article
Full-text available
In this paper, a vehicle‐to‐vehicle (V2V) trajectory planning algorithm for two vehicles driving in an arc‐shaped road is represented, in which the characteristics of human drivers are taken into consideration. Safety constraints including vehicle stability, road boundaries, and vehicles collision avoidance are considered. The boundary constraint of the arc‐shaped road is realized with a potential field method. The potential field method is also employed to realize the vehicles collision avoidance constraint and vehicle distance margin. The concept of partial cooperative control is presented, which can depict the collaboration of the vehicles more accurately. The algorithm is designed with a partial cooperative control pattern, employing model predictive control (MPC). The effectiveness of the algorithm is verified by software simulation in a scenario of overtaking in arc‐shaped roads. The algorithm can accomplish the V2V collaboration trajectory planning task successfully. The performance of the algorithm representing different collaboration degrees and phases grouping in complex scenarios is also studied.
... Model predictive control can be used for lateral vehicle control [8,20,21]. A linear model of the system should be considered to implement an MPC controller in real time in order to avoid computational delays [21,35,36]. The lateral dynamics' model that is used for this controller was presented in [27]: ...
Article
Full-text available
In this contribution, we suggest two proposals to achieve fast, real-time lane-keeping control for Autonomous Ground Vehicles (AGVs). The goal of lane-keeping is to orient and keep the vehicle within a given reference path using the front wheel steering angle as the control action for a specific longitudinal velocity. While nonlinear models can describe the lateral dynamics of the vehicle in an accurate manner, they might lead to difficulties when computing some control laws such as Model Predictive Control (MPC) in real time. Therefore, our first proposal is to use a Linear Parameter Varying (LPV) model to describe the AGV’s lateral dynamics, as a trade-off between computational complexity and model accuracy. Additionally, AGV sensors typically work at different measurement acquisition frequencies so that Kalman Filters (KFs) are usually needed for sensor fusion. Our second proposal is to use a Dual-Rate Extended Kalman Filter (DREFKF) to alleviate the cost of updating the internal state of the filter. To check the validity of our proposals, an LPV model-based control strategy is compared in simulations over a circuit path to another reduced computational complexity control strategy, the Inverse Kinematic Bicycle model (IKIBI), in the presence of process and measurement Gaussian noise. The LPV-MPC controller is shown to provide a more accurate lane-keeping behavior than an IKIBI control strategy. Finally, it is seen that Dual-Rate Extended Kalman Filters (DREKFs) constitute an interesting tool for providing fast vehicle state estimation in an AGV lane-keeping application.
Article
Full-text available
We propose a novel personalized planning and control approach for lane change assistance system (LCA) which can efficiently learn a model prediction control (MPC)-based driver-specific lane-changing policy via end-to-end imitation learning from a few driver demonstrations. Specifically, we build a novel learnable predictive model of the vehicle-driver system and design an adaptable cost function for the MPC-based lane change controller. We then calculate the gradient of the imitation loss with respect to the personalization parameters of the model and cost function via differentiating the optimality conditions, and update those parameters to minimize the imitation loss in an end-to-end fashion. A semi-physical simulation on a driving simulator and a closed-loop test on a real vehicle are conducted to validate the learning ability and personalized control performance. The results show that 1) the proposed method can automatically implement both the generalized and the personalized lane change planning and control by learning from demonstration data; 2) the proposed controller can adapt to different driver-specific behaviors; and 3) the proposed approach outperforms the model-free learning approach in terms of imitation accuracy, interpretability, data efficiency, and generalized performance. Index Terms-Lane change, model predictive control, imitation learning, differentiable optimization, personalized driver assistance system.
Article
In this paper, path planning for intelligent vehicle collision avoidance of dynamic pedestrian using Attention Mechanism-Long Short Term Memory Network (Att-LSTM), Modified Social Force Model (MSFM), and Model Predictive Control (MPC) is systematically investigated, and pedestrian-dynamic vehicle conflict scene at an un-signalized crosswalk is covered. Firstly, a data-driven stacking fusion model based on the Att-LSTM and MSFM is proposed for pedestrian path prediction. Pedestrian heterogeneity (age and gender) is taken into account for the first time. The data-driven stacking fusion model is verified with the existing methods. Then a MPC-based path planning-tracking system considering pedestrian path prediction is developed. The predicted path of pedestrian is treated as a pedestrian-safety-region with the combination of a semi-ellipse and semi-circle, and the front wheel steering angle is calculated to prevent the intelligent vehicle from colliding with the dynamic pedestrians. Simulink-Carsim simulations are presented to simulate the real dynamic environment where crossing pedestrians exist. The results illustrate that the developed MPC system considering pedestrian path prediction can provide dynamic path planning performance acceptably and effectively, and make it possible for intelligent vehicle to present the great feasibility for pedestrian safety protection and traffic efficiency improvement.
Article
Full-text available
This paper presents a novel shared control concept for lane keeping assist (LKA) systems of intelligent vehicles. The core idea is to combine system perception with robust control so that the proposed strategy can successfully share the control authority between human drivers and the LKA system. This shared control strategy is composed of two parts, namely operational part and tactical part. Two local optimal-based controllers with two predefined objectives (i.e. lane keeping and conflict management) are designed in the operational part. The control supervisor in the tactical part aims to provide a decision-making signal which allows for a smooth transition between two local controllers. The control design is based on a human-in-the-loop vehicle system to improve the mutual driver-automation understanding, thus reducing or avoiding the conflict. The closed-loop stability of the whole driver-vehicle system can be rigorously guaranteed using Lyapunov stability argument. In particular, the control design is formulated as an LMI optimization which can be easily solved with numerical solvers. The effectiveness of the proposed shared control method is clearly demonstrated through various hardware experiments with human drivers.
Article
Full-text available
This paper presents a hierarchical trajectory planning based on the integration of a sampling and an optimization method for urban autonomous driving. To manage a complex driving environment, the upper behavioral trajectory planner searches the macro-scale trajectory to determine the behavior of an autonomous car by using environment models, such as traffic control device and objects. This planner infers reasonable behavior and provides it to the motion trajectory planner. For planning the behavioral trajectory, the sampling-based approach is used due to its advantage of a free-form cost function for discrete models of the driving environments and simplification of the searching area. The lower motion trajectory planner determines the micro-scale trajectory based on the results of the upper trajectory planning with the environment model. The lower planner strongly considers vehicle dynamics within the planned behavior of the behavioral trajectory. Therefore, the planning space of the lower planner can be limited, allowing for improvement of the efficiency of the numerical optimization of the lower planner to find the best trajectory. For the motion trajectory planning, the numerical optimization is applied due to its advantages of a mathematical model for the continuous elements of the driving environments and low computation to converge minima in the convex function. The proposed algorithms of the sampling-based behavioral and optimization-based motion trajectory were evaluated through experiments in various scenarios of an urban area. IEEE
Article
Trajectory planning and trajectory tracking constitute two important functions of an autonomous overtaking system and a variety of strategies have been proposed in the literature for both functionalities. However, uncertainties in environment perception using the current generation of sensors has resulted in most proposed methods being applicable only during low-speed overtaking. In this paper, trajectory planning and trajectory tracking approaches for autonomous overtaking systems are reviewed. The trajectory planning techniques are compared based on aspects such as real-time implementation, computational requirements, and feasibility in real-world scenarios. This review shows that two important aspects of trajectory planning for high-speed overtaking are: (i) inclusion of vehicle dynamics and environmental constraints and (ii) accurate knowledge of the environment and surrounding obstacles. The review of trajectory tracking controllers for high-speed driving is based on different categories of control algorithms where their respective advantages and disadvantages are analysed. This study shows that while advanced control methods improve tracking performance, in most cases the results are valid only within well-regulated conditions. Therefore, existing autonomous overtaking solutions assume precise knowledge of surrounding environment which is not representative of real-world driving. The paper also discusses how in a connected driving environment, vehicles can access additional information that can expand their perception. Hence, the potential of cooperative information sharing for aiding autonomous high-speed overtaking manoeuvre is identified as a possible solution.
Article
This paper presents a nonlinear model predictive control (MPC) formulation for obstacle avoidance in high-speed, large-size autono-mous ground vehicles (AGVs) with high centre of gravity (CoG) that operate in unstructured environments, such as military vehicles. The term ‘unstructured’ in this context denotes that there are no lanes or traffic rules to follow. Existing MPC formulations for passenger vehicles in structured environments do not readily apply to this context. Thus, a new nonlinear MPC formulation is developed to navigate an AGV from its initial position to a target position at high-speed safely. First, a new cost function formulation is used that aims to find the shortest path to the target position, since no reference trajectory exists in unstructured environments. Second, a region partitioning approach is used in conjunction with a multi-phase optimal control formulation to accommodate the complicated forms the obstacle-free region can assume due to the presence of multiple obstacles in the prediction horizon in an unstructured environment. Third, the no-wheel-lift-off condition, which is the major dynamical safety concern for high-speed, high-CoG AGVs, is ensured by limiting the steering angle within a range obtained offline using a 14 degrees-of-freedom vehicle dynamics model. Thus, a safe, high-speed navigation is enabled in an unstructured environment. Simulations of an AGV approaching multiple obstacles are provided to demonstrate the effectiveness of the algorithm.
Article
The collision avoidance (CA) system is a pivotal part of the autonomous vehicle. Ability to navigate the vehicle in various hazardous scenarios demands reliable actuator interventions. In a complex CA scenario, the increased nonlinearity requires a dependable control strategy. For example, during collisions with a sudden appearing obstacle (i.e. crossing pedestrian, vehicle), the abrupt increment of vehicle longitudinal and lateral forces summation during the CA maneuver demands a system with the ability to handle coupled nonlinear dynamics. Failure to address the aforementioned issues will result in collisions and near-miss incidents. Thus, to solve these issues, a nonlinear model predictive control (NMPC)-based path tracking strategy is proposed as the automated motion guidance for the host vehicle CA architecture. The system is integrated with the artificial potential field (APF) as the motion planning strategy. In a hazardous scenario, APF measures the collision risks and formulates the desired yaw rate and deceleration metrics for the path replanning. APF ensures an optimal replanned trajectory by including the vehicle dynamics into its optimization formulation. NMPC then acts as the coupled path and speed tracking controller to enable vehicle navigation. To accommodate vehicle comfort during the avoidance, NMPC is constrained. Due to its complexity as a nonlinear controller, NMPC can be time-consuming. Therefore, a move blocking strategy is assimilated within the architecture to decrease the system’s computational burden. The modular nature of the architecture allows each strategy to be tuned and developed independently without affecting each others’ performance. The system’s tracking performance is analyzed by computational simulations with several CA scenarios (crossing pedestrian, parked bus, and sudden appearing moving vehicle at an intersection). NMPC tracking performance is compared to the nominal MPC and linear controllers. The effect of move blocking strategies on NMPC performance are analyzed, and the results are compared in terms of mean squared error values. The inclusion of nonlinear tracking controllers in the architecture is shown to provide reliable CA actions in various hazardous scenarios. The work is important for the development of a reliable controller strategy for multi-scenario CA of the fully autonomous vehicle.
Article
In this paper, a gain-scheduling, robust, and shared controller is proposed to assist drivers in tracking vehicle reference trajectory. In the controller, the driver steering parameters such as delay time, preview time, and steering gain are assumed to be varying with respect to the different characteristics of drivers, vehicle states, and driving scenarios. Meanwhile, the modeling errors and uncertainties in the tire cornering stiffness are also considered in the driver–vehicle system model and the controller design. A global objective function, considering the tracking error, the driver's physical and mental workloads, and the control effort, is designed to optimize the overall performance of the driver–vehicle system. Constraint on eigenvalue placement is added to the controller design to improve the performance of the closed-loop driver–vehicle system. Simulation results under different maneuvers show that the controller can significantly improve the system performance and reduce the driver's workloads. The controller can reduce the delay time of the driver–vehicle system in emergency maneuvers, particularly for inexperienced drivers.