ArticlePDF Available

LTV-MPC Approach for Automated Vehicle Path Following at the Limit of Handling

Authors:

Abstract and Figures

In this paper, a linear time-varying model predictive controller (LTV-MPC) is proposed for automated vehicle path-following applications. In the field of path following, the application of nonlinear MPCs is becoming more common; however, the major disadvantage of this algorithm is the high computational cost. During this research, the authors propose two methods to reduce the nonlinear terms: one is a novel method to define the path-following problem by transforming the path according to the actual state of the vehicle, while the other one is the application of a successive linearization technique to generate the state–space representation of the vehicle used for state prediction by the MPC. Furthermore, the dynamic effect of the steering system is examined as well by modeling the steering dynamics with a first-order lag. Using the proposed method, the necessary segment of the predefined path is transformed, the linearized model of the vehicle is calculated, and the optimal steering control vector is calculated for a finite horizon at every timestep. The longitudinal dynamics of the vehicle are controlled separately from the lateral dynamics by a PI cruise controller. The performance of the controller is evaluated and the effect of the steering model is examined as well.
Content may be subject to copyright.
Citation: Domina, Á.; Tihanyi, V.
LTV-MPC Approach for Automated
Vehicle Path Following at the Limit of
Handling. Sensors 2022,22, 5807.
https://doi.org/10.3390/s22155807
Academic Editor: Felipe Jiménez
Received: 9 June 2022
Accepted: 27 July 2022
Published: 3 August 2022
Publisher’s Note: MDPI stays neutral
with regard to jurisdictional claims in
published maps and institutional affil-
iations.
Copyright: © 2022 by the authors.
Licensee MDPI, Basel, Switzerland.
This article is an open access article
distributed under the terms and
conditions of the Creative Commons
Attribution (CC BY) license (https://
creativecommons.org/licenses/by/
4.0/).
sensors
Article
LTV-MPC Approach for Automated Vehicle Path Following
at the Limit of Handling
Ádám Domina * and Viktor Tihanyi
Department of Automotive Technologies, Budapest University of Technology and Economics,
1111 Budapest, Hungary; tihanyi.viktor@kjk.bme.hu
*Correspondence: domina.adam@edu.bme.hu
Abstract:
In this paper, a linear time-varying model predictive controller (LTV-MPC) is proposed
for automated vehicle path-following applications. In the field of path following, the application
of nonlinear MPCs is becoming more common; however, the major disadvantage of this algorithm
is the high computational cost. During this research, the authors propose two methods to reduce
the nonlinear terms: one is a novel method to define the path-following problem by transforming
the path according to the actual state of the vehicle, while the other one is the application of a
successive linearization technique to generate the state–space representation of the vehicle used for
state prediction by the MPC. Furthermore, the dynamic effect of the steering system is examined
as well by modeling the steering dynamics with a first-order lag. Using the proposed method, the
necessary segment of the predefined path is transformed, the linearized model of the vehicle is
calculated, and the optimal steering control vector is calculated for a finite horizon at every timestep.
The longitudinal dynamics of the vehicle are controlled separately from the lateral dynamics by a PI
cruise controller. The performance of the controller is evaluated and the effect of the steering model
is examined as well.
Keywords:
automated vehicle; model predictive controller; path following; successive linearization;
vehicle dynamics
1. Introduction
Collision avoidance systems, advanced driver-assistance systems, and the many other
types of automated vehicle functions are becoming more and more popular and have
become the most related topics in the field of automotive research. The advantages of
automatization of different vehicle functions include the potential to improve road safety,
reduce pollutant emissions and traveling times, and eliminate human errors, which are
the primary cause of accidents. An automated vehicle can avoid collisions by using the
steering and the braking system conventionally; furthermore, a new alternative solution
for accident prevention is to force the vehicle into an unstable state, in which a control
software can drive the vehicle at a level as high as a professional driver, for example, in
case of a drift maneuver [
1
3
]. Each of these studies aims to drive a vehicle at the level of a
professional human driver. In [
1
], an LQ controller is used for steady-state drifting, while
in [
2
], an MPC is applied for drifting in varying road surface conditions, and in [
3
], the drift
is realized by a reinforcement learning algorithm.
According to the structure of the automated vehicles, the motion planning and motion
control modules are essential to achieve these goals. In this paper, the authors focus on the
motion control layer by proposing an LTV-MPC structure for path-following tasks. Based
on the existing research and the experimental tests, the authors identify three features to
be considered in a path-following controller at higher vehicle speed, the inclusion of the
vehicle dynamics in the control law, and the knowledge of the path ahead of the vehicle.
Furthermore, the inclusion of the steering dynamics in the control law is identified as the
Sensors 2022,22, 5807. https://doi.org/10.3390/s22155807 https://www.mdpi.com/journal/sensors
Sensors 2022,22, 5807 2 of 23
third required feature, which has a high effect on the performance of the path-following
controller. Numerous path-following solutions are summarized in [
4
6
], e.g., geometric
controllers, such as Pure Pursuit and Stanley, linear quadratic regulators, model predictive
controllers, neural network based controllers, etc. The authors decided to apply MPC for
the path-following tasks, as the identified features can be incorporated into an MPC and
the MPC handles constraints well.
Three main types of MPC methods are used in the field of path tracking, the linear
parameter-varying MPC (LPV-MPC) [
7
9
] the linear time-varying MPC (LTV-MPC) [
10
13
],
and the nonlinear MPC (NLMPC) [
10
,
14
16
] solutions. The LPV-MPC applies a linear
vehicle model for state prediction and the structure of that model does not change over
time; however, a few of the variables, e.g., the velocity of the vehicle, can change. The
model is calculated at each timestep based on the current states. The main advantage
of the LPV-MPC method is the low computational cost; however, this technique reaches
the limit of applicability when the controlled system leaves the linear region, where the
system model is defined. Thus, the LPV-MPC method is unable to handle nonlinear system
dynamics in a wide range, e.g., tire dynamics in the nonlinear region of the tires. The change
in given parameters can be determined by measurements or estimated by state estimators.
The LTV-MPC [
10
12
,
17
] can handle the nonlinear behavior of the system by lineariz-
ing the nonlinear system at every timestep and estimating the states accordingly. The
predicted states are still based on a linear system model; however, these are extremely
close to the real states of the system, since the controller calculates the predicted states for
a short horizon, e.g., a few hundred ms, in which range the prediction is acceptable. In
the LTV-MPC structure, the current vehicle state is the basis of the linearization and an
additional transformation in the system matrices and the control input is required. The
LTV-MPC method claims a bit more computational cost than the LPV-MPC; however, it
still can run on an ECU or a rapid prototyping unit in real-time. When using LPV-MPC
or LTV-MPC, usually, a quadratic cost function is formulated, the optimum of which is
computationally cheaper to find.
Applying the NLMPC, the evolution matrices are calculated based on a nonlinear
system model without linearization. A nonlinear cost function is formulated, the optimum
of which is computationally expensive to find, and might limit the applicability of the
NLMPC, as discussed in [6].
In this paper, the authors propose an LTV-MPC method for automated vehicle path-
following scenarios. The aim of this research is to apply a nonlinear vehicle model for state
prediction, handled by a successive linearization technique, where the vehicle model is
linearized at every timestep by Jacobi linearization. The applied vehicle model is coupled
with a Pacejka tire model, the parameters of which are identified based on measurement
results. The application of a nonlinear vehicle model coupled with the Pacejka model is
expected to be suitable for handling the high sideslip angles of the tires, where the tires
are operating in the nonlinear region–which the LPV-MPC is unable to handle–hence, the
vehicle should be able to perform the path-following task at the limit of handling.
In the existing research, if the path is defined in a forward-looking way, it is per-
formed by defining the reference path in the global coordinate system by a series of (x,y)
points
[10,12,1821]
. While this solution is obvious, it incorporates high nonlinearity into
the plant model, since the motion of the vehicle needs to be transformed from the vehicle’s
coordinate system–i.e., the ego frame–to the global frame. A novel method for the definition
of the reference path is presented, by which the number of the nonlinear terms involved in
the entire control problem is successfully reduced. According to our proposal, the path is
transformed from the global coordinate system into the vehicle’s coordinate system and
the reference states, which need to be realized by the vehicle, are expressed according to
the state–space representation of the vehicle plant model used for state prediction by the
MPC. In the existing research, the reference path is either not defined in a forward-looking
way [
7
,
22
], or the forward-looking property is limited to the lateral displacement of the
Sensors 2022,22, 5807 3 of 23
vehicle [
8
,
12
]. In this article, the authors define both the lateral displacement and the
heading angle as a reference series for a finite horizon, as presented in Section 3.
Some studies considered the effect of the actuator fault [
23
] and the robustness of
the controlled system [
24
]; in this article, the effect of the steering system is analyzed.
The effect of the consideration of the steering dynamics in the vehicle model applied for
state prediction is discussed in this paper as well. The steering system is modeled as a
first-order lag. Moreover, the performance of the controller is evaluated with and without
the inclusion of the steering dynamics in the plant model by applying the control input to
the same vehicle model, which includes the steering dynamics.
The contribution of this paper is twofold, (1) The proposed LTV-MPC method for path
following includes a novel reference definition method that effectively reduces the nonlinear
terms of the path-following problem by transforming the path from the global frame to
the ego frame. By this transformation, the nonlinearity that needs to be managed during
the Jacobian linearization is significantly reduced as the state vector has two elements less,
since the position of the vehicle in the global coordinate system is not considered in the
plant model. This approach removes two equations from the model while retaining the
lookahead principle of the reference definition. (2) The analysis of the effect of the steering
dynamics on the path-following problem are incorporated in an LTV-MPC.
The structure of this paper is as follows. Section 2presents the vehicle model applied
in the MPC for state prediction, regarding the steering dynamics and the vehicle model
used for testing the controller. In Section 3, the generation of the reference path is presented.
The structure of the proposed MPC and the derivation of the quadratic programming (QP)
cost function is presented in Section 4. Section 5describes the results, while Section 6
provides an analysis, in which the controller is tested on a sine wave path and the effect
of the application of linear and nonlinear vehicle models in the state prediction is com-
pared. Finally, Section 7discusses the concluding remarks and suggestions for further
research directions.
2. Vehicle Modelling
In this section, the applied tire and vehicle models are presented. The authors use
different vehicle models for state prediction and for simulation testing. Furthermore, a
Pacejka tire model is applied in both models, the parameters of which are identified from
measurement data.
2.1. Vehicle Model for Testing
A four-wheel vehicle model is used for testing the path-following controller in a
simulation environment. The roll and pitch dynamics are neglected in the vehicle model
since they have no significant effect on the path-following problem. The model describes
the planar dynamics of the vehicle: the angular acceleration around the vertical axis (1), the
longitudinal (2), and the lateral acceleration (3).
.
r=1
Iz
Mz(1)
ax=1
mFV
x+Vy
.
r(2)
ay=1
mFV
yVx
.
r(3)
where ris the yaw rate, I
z
is the moment of inertia of the vehicle around the axis z,M
z
is the
resultant torque, which rotates the vehicle around axis z,a
x
is the longitudinal acceleration,
mis the mass of the vehicle, F
xV
is the resultant of the longitudinal forces, acting on the
vehicle, V
y
is the lateral velocity of the vehicle in the ego coordinate system, a
y
is the lateral
acceleration, F
yV
is the resultant of the lateral forces acting on the vehicle, and V
x
is the
longitudinal velocity in the ego frame.
Sensors 2022,22, 5807 4 of 23
The resultant torque and forces are calculated by (4), (5), and (6)
Mz=b(FyRL +FyRR) + sf(FxFLsin(δFL )+FxFRsin(δF R)) + sr(FxRL +FxRR)
+aFyFL cos(δFL)+FyFR cos(δFR )(4)
FV
x=FxRL +FxRR +FyFLsin(δF L )+FyFRsin(δFR )+FxFL cos(δF L)+FxFRcos(δFR )(5)
FV
y=FyRL +FyRR +FxF L sin(δFL )+FxFRsin(δF R )+FyFLcos(δFL )+FyFR cos(δFR)(6)
where aand bare the distances between the center of gravity (C.G.) and the front and the
rear axle, respectively, s
f
and s
r
are the front and rear track of the vehicle, respectively,
δFL
and
δFR
are the front-left and front-right steering angles of the individual wheels, F
xij
and
F
yij
are the longitudinal and lateral forces at the individual wheels, where imarks the front
and the rear axles, hence, i=Ras rear or i=Fas front, and jmarks the left and the right
wheels, hence, j=Las left or j=Ras right. In this model, the authors consider a rear-wheel
drive vehicle, with equally distributed traction force on the rear wheels, hence, F
xRL
and
F
xRR
are equal. Furthermore, the authors assume a front-wheel steering vehicle; hence, the
steering angle is solely interpreted at the front wheels.
In Figure 1, the tire sideslip angles and the sideslip angle of the vehicle at C.G. are also
shown. The vehicle sideslip angle is calculated by (7).
β=tan1Vy
Vx(7)
Figure 1. The four-wheel vehicle model applied for controller testing.
The sideslip angle of an individual wheel is calculated using the velocity vector of the
given wheel and the steering angle at the front axle. The velocity vector of the wheels is
described by (8), the position vector of the wheels is defined by (9), where the origin is the
C.G. of the vehicle, and the tire sideslip angles are described by (10) and (11).
vi=
vxi
vyi
vzi
=
Vx
Vy
Vz
+
0
0
r
×Pi(8)
PFL =ha sf0iT,PFR =hasf0iT,PRL =b sr0T,PRR =bsr0T(9)
αFL =tan1vy FL
vxFL δFL ,αFR =tan1vyFR
vxFR δFR (10)
αRL =tan1vyRL
vxRL ,αRR =tan1vyRR
vxRR (11)
Sensors 2022,22, 5807 5 of 23
where i=FL
FR
RL
RR and
α
denotes the tire sideslip angle. The horizontal velocity
V
z
in (8) is considered zero as the model focuses solely on the planar dynamics of the
vehicle, as stated previously.
A Pacejka tire model [
20
] is applied to calculate the lateral forces at each wheel. During
this research, the vehicle moves at constant speed; thus, the longitudinal forces are small,
especially compared to the lateral forces and, hence, solely a lateral tire model is used in
this article. The Pacejka tire model is described by Equations (12) and (13) [25].
Y=DsinCtan1(Bφ)+Sv(12)
φ=(1E)(α+Sh)+(E/B)tan1(B(α+Sh)) (13)
where Bis the stiffness factor, Cis the shape factor, Dis the peak factor, Eis the curvature
factor, S
v
and S
h
are the vertical and the horizontal shift, respectively, and the
α
sideslip
angle is defined in degrees. The Pacejka tire model and the meaning of the factor variables
are described in more detail in [25].
The entire vehicle model, including the tire model, is fitted to a test vehicle used for
automated vehicle function tests, such as automated drift, Moose test, and other path-
following tasks. Further details of the test vehicle setup can be found in [1].
To determine the characteristics of the tires, a ramp steer maneuver is taken by the test
vehicle and the necessary variables are measured with the data acquisition system. The
parameters in the Pacejka tire model are fitted to the measurement results, as shown in
Figure 2.
Figure 2. The measured and identified tire characteristics.
The characteristics of the rear tires have a greater slope at small sideslip angles, where
the tire model is nearly linear. That means the rear tires have greater cornering stiffness
than the front tires. This meets the expectations of the authors, since the front wheels
of the vehicle are mounted with 245/35 R19 tires, while 265/35 R19 tires are applied at
the rear wheels—using the same sidewall height, the wider tire becomes stiffer. During
the identification of the tire characteristics, the vertical load of the tires is assumed to
be constant.
The identified tire models describe the characteristics of the tires located on the same
axle, not the individual wheels; hence, the results in Figure 2contain the lateral forces
generated by the two front tire pairs and the two rear tire pairs, respectively. Thus, when
modeling one wheel, the value of the lateral force needs to be halved.
Sensors 2022,22, 5807 6 of 23
2.2. Vehicle Model for State Prediction
The states of the vehicle model applied in the MPC for state prediction are advisable
to choose in a way to correspond to the control purpose. In this case, the aim is to drive the
vehicle along a predefined path as fast as the vehicle is still able to execute the maneuver.
The applied vehicle model shown in Figure 3is a dynamic bicycle model, coupled with
different Pacejka tire models at the front and the rear wheels. Since the bicycle model
applies one wheel per axle, the identified Pacejka models can be used without halving the
values of the lateral forces of the tire characteristics. The applied vehicle model describes the
dynamics of the vehicle with four equations, enhanced by a fifth equation for the steering
dynamics. The steering dynamics are considered to take into account the dynamic lag of
the steering system, in which way the controller considers that the demanded steering
angle will only be realized with a time delay. The dynamics of the steering system are
modeled by a first-order lag, which has one tuning parameter, the time constant, to identify
the measurements made. The time constant is determined by measurements conducted on
the test vehicle.
Figure 3. The vehicle model applied to the MPC for state prediction.
The state vector of the system is chosen as x = [
v Vyϕrδact
]
T
, where yis the lateral
displacement of the vehicle in the ego frame,
ϕ
is the heading angle of the vehicle, and
δact
is the actual steering angle. The vehicle dynamics are described by (14)–(17), while the
steering dynamics by (18).
Vy=.
y(14)
ay=.
Vy= Fy f cos(δ) + Fyr
m!Vxr(15)
r=.
ϕ(16)
.
r=aFy f cos(δ)bFyr
Iz(17)
.
δact =1
Tst
δact +1
Tst
δdem (18)
where
δdem
is the steering angle required by the MPC and T
st
is the time constant of the
first-order steering model. To calculate the F
yf
and F
yr
lateral forces, the identified Pacejka
models are applied by determining the parameters of (12) and (13). The sideslip angles are
calculated by (19) and (20).
vyF =.
vy+ar,vyR =.
vybr (19)
αF=tan1vyF
vxF δF,αR=tan1vyR
vxR (20)
where v
yF
and v
yR
are the velocity of the front and the rear wheels and
αF
and
αR
are
the sideslip angles of the front and the rear wheels, respectively. The tests are conducted
without the consideration of the steering dynamics, in which case, the state vector is chosen
as x = [v Vyϕr]T, and (18) is neglected during the state prediction.
Sensors 2022,22, 5807 7 of 23
The future states of a vehicle using a vehicle model can be predicted for a finite time
horizon. In this article, the authors apply successive linearization to continuously generate
the state–space representation of the vehicle. The successive linearization allows linearizing
the nonlinear vehicle model at the current operating point, calculating the state–space
representation accordingly, and making the state prediction based on the linearized system.
The linearization is conducted at every timestep; hence, the MPC can use the latest state of
the vehicle as a basis of the state prediction.
The linearization of the vehicle model is solved by Jacobian linearization and leads to
the continuous-time [Ac,Bc,Cc,Dc] state–space representation of the system (21).
Ac(i,j)=Fi
xj
,Bc(i,j)=Fi
uj
,Cc(i,j)=zi
xj
,Dc(i,j)=zi
uj
(21)
where Fis the system of nonlinear equations (14)–(18), xis the state vector, uis the control
input, which, in this case, is the demanded steering angle
δdem
, while C
c
and D
c
are
considered constant matrices (22).
Cc=10000
00100,Dc=0
0(22)
Furthermore, matrix B
c
remains constant during the linearization in the following
form (23).
Bc=h0000 1
Tst iT(23)
The partial derivatives of the matrices are evaluated at the desired operation point,
which is specified by the state vector x
o
, the time derivative of the state vector
.
xo
, and the
control vector u
o
and, thereby, the partial derivatives in (21) need to be calculated based
on the initial conditions
x=xo
,
.
x=.
xo
and
u=uo
. The evolution of the continuous-time
system can be described by (24)
.
xL=.
xo+Ac(xLxo)+Bc(uLuo)(24)
where x
L
and u
L
are the linearized state of the system and the control input, respectively.
The constant terms of (24) are incorporated into K
c
, with which the continuous-time
representation of the state–space system becomes (25) and (26).
.
xL=AcxL+BcuL+Kc,Kc=.
xoAcxoBcuo(25)
yL=CcxL+DcuL(26)
where yLis the output of the system.
Since the MPC is a discrete-time-control technique, the state–space representation
needs to be discretized to get the discrete-time state–space representation of the system
[Ad, Bd, Cd, Dd] and Kd(27)–(30).
Ad=eAcTs(27)
Bd=A1
ceAcTsIBc(28)
Kd=A1
ceAcTsIKc(29)
Cd=Cc,Dd=Dc(30)
where T
s
is the discretization timestep, which is equal to the timestep value of the MPC.
Finally, the discrete-time representation of the system (31) can be used for state prediction
by the MPC.
x(k+1)=Adx(k) + Bcu(k) + Kd
y(k+1)=Cdx(k+1)+Ddu(k+1)(31)
Sensors 2022,22, 5807 8 of 23
3. Reference Path Definition
As previously introduced, in numerous studies, the control problem is to minimize the
error between the spatial reference path and the position of the vehicle. In this research, the
state–space representation of the vehicle contains the position of the vehicle in the global
frame, according to the reference definition, where the path is defined by a series of (x,y)
points in the global frame. The controlled state is the spatial position of the vehicle—X
and Y coordinates in the global frame. The spatial path-following problem is possibly
supplemented with the tracking of the heading and the yaw rate. The problem with the
formulation when the X and Y coordinates are defined as a reference is that it introduces
several nonlinear terms into the vehicle model applied for state prediction by the MPC
and increases the dimension of the state vector by two. Therefore, if the control problem is
defined to follow spatial points, the vehicle needs to be transformed to the global frame in
the state–space representation—this transformation is responsible for higher nonlinearity.
Thus, if the increase in the dimension of state vector xby two—displacement in X and
Y directions—can be omitted, then the computational requirements of the MPC can be
reduced, which increases directly proportionally with the increase in the dimension of
state vector x, as presented in [
26
]. In this article, a novel method is proposed for the
reduction in nonlinear terms, which is to transform the path to the vehicle and describe the
path-following task in a way that retains the advantageous property of the MPC, which
is the knowledge of the path ahead of the vehicle—the knowledge of the reference for a
finite horizon. In this article, the lateral displacement and the heading of the vehicle are
defined as a reference to be followed, in accordance with the states of the vehicle model
used for state prediction. The reference state variables are chosen as they clearly define the
relationship between the vehicle and the path, with respect to the lateral terms. To calculate
the reference for a finite horizon, firstly, the lateral and angular errors are defined, as shown
in Figure 4. The lateral error eis the distance between the C.G. point of the vehicle and the
intersection point Mon the yaxis of the ego frame and the reference path.
Figure 4. The interpretation of the lateral and angular errors.
The angular error
α
is the angle between the heading of the vehicle and the tangent of
the path at the intersection point M. To generate the reference for the lateral displacement
state yand for the heading angle state
ϕ
, the vehicle, the reference path, the ego frame, and
the global frame need to be considered, as shown in Figure 5. In Figure 5, the lateral errors
and the angle errors are shown for a finite N
p
horizon, where N
p
is the prediction horizon of
the MPC. If the lateral error values from the ego coordinate system can be seen, these errors
can be interpreted as reference lateral displacements, which corresponds to y, the first state
of the state–space representation. In a similar way, the angular errors can be interpreted
as
ϕ
reference heading angles. By applying this method for reference generation, the
Sensors 2022,22, 5807 9 of 23
transformation of the vehicle coordinates from the ego frame to the global frame becomes
avoidable, resulting in omitting several nonlinear terms from the vehicle model.
Figure 5.
The interpretation of the lateral error reference and orientation angle reference in both ego
frame and global frame.
As stated before, if the vehicle model is not transformed into the global frame, the path
needs to be transformed into the ego frame of the vehicle. The transformation is defined by
Equations (32)–(34),
γ=α1+ϕ(32)
H=0
e1(33)
Pre f ,i=H+cos(γ)sin(γ)
sin(γ)cos(γ)Pi(34)
where
γ
is the transformation angle, e
1
and
α1
are the lateral and angular errors at the M
point, respectively, His an offset vector, which shifts the entire reference trajectory to the
necessary lateral displacement, P
i
is the (x,y) coordinate of the ith path point described in
the global frame, and P
ref,i
is the coordinate of the reference path point in the ego frame.
After the transformation, the lateral error references and the angular error references can be
calculated; the lateral error reference is the y-direction component of P
ref,i
, which determines
the series of e
1
, e
2
,
. . .
e
Np
, while the angular reference can be calculated considering the
tangent of the transformed path at every point where the lateral displacements are defined.
After the reference values are calculated, the reference vector can be generated by
(35) and (36).
t=ei
αi(35)
T=t1t2· · · tNpT=he1α1e2α2· · · eNpαNpiT(36)
While the original reference path is described by N
p
points, the reference lateral
displacements and the heading angles need to be calculated for every path point. The result
is a stacked matrix Twith a dimension of 2
×
N
p
. Using this method of reference definition,
several nonlinear terms are omitted from the vehicle model, the reference is coherent to
the states of the vehicle model, the reference lateral displacement is coupled with the first
state in the state–space model, and the angle reference is coupled with the third state.
Furthermore, the presented reference generation method preserves the forward-looking
nature of the problem definition, which is necessary for the path-following task.
Sensors 2022,22, 5807 10 of 23
4. MPC Structure
As stated in Section 2, the authors apply a successive linearization technique, similar
to [
12
,
14
,
17
], to handle the nonlinear vehicle model. During the successive linearization,
the nonlinear vehicle model is linearized at every timestep based on the actual state vector
of the vehicle. The resulting state–space representation is applied to predict the future
vehicle states for a finite prediction horizon, the length of which is N
p
. Using an MPC
controller, the objective is to minimize the difference between the given reference states
in the system and the predicted states in the systems by calculating the optimal control
input vector as a result of an optimization process, while meeting a set of constraints. The
optimization process requires a cost function to minimize. In this section, the derivation
of the cost function is presented. The cost function depends on the tracking error and
the amount of the control input. The tracking error e(k) in the k-th timestep is defined as
e(k) = y(k)r(k)
, where y(k) is the current state of the system and r(k) is the given reference.
Then, the evolution of the error can be defined for the k-th, k+ 1-th, and k+ 2-th timesteps,
etc., by (37).
e(k) = Cdx(k) + Ddu(k)r(k)
e(k+1)=Cdx(k+1)+Ddu(k+1)r(k+1)
=CdAdx(k) + CdBdu(k) + CdKd+Ddu(k+1)r(k+1)
e(k+2)=Cdx(k+2)+Ddu(k+2)r(k+2)=
CdA2
dx(k) + CdAdBdu(k) + CdBdud(k+1)+CdAdKd+CdKd+
Ddu(k+2)r(k+2)
(37)
The evolution of the error needs to be calculated for the entire prediction horizon,
resulting in the error vector
eRNpNo
, where N
o
is the number of outputs in the system,
which is equal to the rows of matrix C
d
. In this case, N
o
= 2 corresponds to the lateral
displacement and the heading angle references.
e==
Px(k) + =
Hu +=
EKdr(38)
e=
e(k)
e(k+1)
e(k+2)
.
.
.
ek+Np1
,=
P=
Cd
CdAd
CdA2
d
.
.
.
CdANp1
d
(39a)
=
H=
Dd0 0 . . .
CdBdDd0 . . .
CdAdBdCdBdDd. . .
.
.
..
.
..
.
....
CdANp2
dBdCdANp3
dBdCdANp4
dBd. . .
(39b)
u=
u(k)
u(k+1)
u(k+2)
.
.
.
uk+Np1
,=
E=
0
Cd
Cd(I+Ad)
.
.
.
CdI+Np2
i=1Ai
d
(39c)
Sensors 2022,22, 5807 11 of 23
r=
r(k)
r(k+1)
r(k+2)
.
.
.
rNp1
(39d)
where
uRNpNu
is the control input vector, i.e., the result of the optimization process, N
u
is the number of the control inputs, which is equal to the columns of matrix B
d
—in this
case—N
u
= 1 corresponding to the steering input,
=
PRNpNo×Nx
,N
x
is the dimension of
the state vector,
=
HRNpNo×NpNu
,
=
ERNpNo×Nx
,
rRNpNo
is the reference matrix, which
defines the reference for the N
p
horizon. In (39), P,H, and Eare the error evolution matrices.
The constant terms in (38) can be combined as
=
K==
EKdr
, which leads to a more compact
form of e.
e==
Px(k) + =
Hu +=
K(40)
As the evolution of the tracking error is calculated, the cost function can be defined as
J(k) = 1
2e(k)T=
Qe(k) + u(k)T=
Ru(k)(41)
where
=
QRNpNo×NpNo
penalizes the deviation from the reference states and
=
RRNpNu×NpNu
penalizes the number of the control input. Both
=
Q
and
=
R
are diagonal square matrices, built
by using matrix
=
q
and scalar
r
, according to (42) and (43), where q
1
and q
2
are the weights
of the deviation from the reference lateral distance and the heading angle, respectively, and
ris the weight of the control input.
=
Q=
=
q0 . . . 0
0=
q. . . 0
.
.
..
.
....0
0 0 . . . =
q
,=
R=
r0 . . . 0
0r. . . 0
.
.
..
.
....0
0 0 . . . r
(42)
=
q=q10
0q2,r= [r](43)
In this article, the authors apply a quadratic cost function (41), the optimum of which
is found by the built-in numerical solver in the MATLAB software package, called quadprog.
Substituting (40) into (41) results in
J(k) = 1
2=
Px(k) + =
Hu +=
KT=
Q=
Px(k) + =
Hu +=
K+u(k)T=
Ru(k))(44)
The following task is to aggregate the terms that do not depend on the control input
u
and aggregate the quadratic and linear terms of (44), which leads to (45).
J(k) = 1
2u(k)T
=
G
z }| {
=
HT=
Q=
H+=
Ru(k) +
=
WT
z }| {
x(k)T=
PT+
=
KT=
Q=
Hu(k)(45)
A more compact form of the cost function is in (46).
J(k) = 1
2u(k)T=
Gu(k) +
=
WTu(k)(46)
Sensors 2022,22, 5807 12 of 23
When solving optimal control problems, the aim is to minimize the tracking errors
and the amount of control input. However, a steady-state error might occur while tracking,
since the cost of the steady-state error might be less than the cost of the higher value of the
control inputs. To avoid the steady-state error, the cost function is defined by the increments
in the current control input, rather than defining the individual control inputs. Applying
this solution, the effect of the control input values on the cost function is minimal. The
control input vector can be transformed as
u(k) = u(k1)+ρu1
u(k+1)=u(k) + ρu2=u(k1)+ρu1+ρu2
uk+Np1=uk+Np2+ρuNp=u(k1)+ρu1+ρu2+. . . +ρuNp
(47)
which can be written as
u
z }| {
u(k)
u(k+1)
.
.
.
uk+Np1
=
u(k1)
z }| {
u(k1)
u(k1)
.
.
.
u(k1)
+
=
D
z }| {
I0 . . .
I I . . .
.
.
..
.
....
I I . . .
ρu(k)
z }| {
ρu1
ρu2
.
.
.
ρuNp
(48)
where
=
DRNpNu×NpNu
is a lower diagonal matrix built of
IRNu×Nu
matrices and zero
matrices of the same dimensions. Performing the transformation, the evolution of the
tracking error can be written as
e==
Px(k) + =
Hu(k1)+=
Dρu(k)+=
K(49)
where
u(k1)
is constant for the entire prediction horizon, thus, it can be included in the
constant term
=
K
. The new const function, the solution of which is the optimal series of
control input increments, is given in the following form
JD(k) = 1
2ρu(k)T=
GDρu(k) +
=
WT
Dρu(k)(50)
where the matrices are defined as =
GD=DT=
GD and =
WT
D=uT(k1)=
GD.
5. Results
A reference path is defined for testing the controller shown in Figure 6. The path
contains a double-lane-change section, which is considered an evasive maneuver [
27
], and
a U-turn section, which is built by a clothoid segment, marked with Aand Bin Figure 6. As
the vehicle drives along the U-turn section, the curvature of the path is the linear function
of the length of the arc. During the simulations, the vehicle is started from the left side of
the path, using a 0.2 m offset, which is applied to examine how the vehicle can find the path.
The simulations are conducted at 50, 60, and 70 km/h, using vehicle models, excluding
and including the steering dynamics; hence, in total, six cases are examined. During the
simulations, the identified model of the steering system is applied to the vehicle model in
every case; hence, the consideration of the steering dynamics in the prediction model is
expected to lead to better results. The value of N
p
and N
c
is 10 and the sampling time is
0.05 s.
The results are summarized in Table 1, where eis the lateral error and
φ
is the orien-
tation error. As the velocity of the vehicle is increased, the average and maximal errors
are also increased; furthermore, the application of the steering dynamics can reduce the
errors significantly.
Sensors 2022,22, 5807 13 of 23
Please note that the figures below sometimes do not show the difference between
the reference and the real steering angle, which is due to the accurate reference tracking.
As shown in Figures 7and 8, at 50 km/h, the consideration of the steering dynamics can
increase the accuracy of the controller and result in a smoother steering command, which
increases the stability of the vehicle and provides better ride comfort to the passengers.
Furthermore, smaller steering interventions result in smaller sideslip angles.
Figure 6. The reference path.
Figure 7. Simulation results at 50 km/h, without steering dynamics.
Sensors 2022,22, 5807 14 of 23
Table 1. Simulation results.
Speed [km/h] Steering Dynamics eavg [m] emax [m] φavg [deg] φmax [deg]
50 No 0.066 0.220 0.516 2.561
Yes 0.023 0.200 0.510 3.862
60 No 0.085 0.425 0.407 6.159
Yes 0.077 0.362 0.378 5.363
70 No 0.544 4.672 1.942 25.336
Yes 0.522 4.669 1.937 25.869
The result at 60 km/h is shown in Figures 9and 10. The results show similar behavior
to the 50 km/h cases; however, the errors are a little higher, although still in an acceptable
range. As shown in Figure 2, the tires become saturated when reaching about 4 deg and
2 deg of sideslip at the front and the rear wheels, respectively.
The tire sideslips reach about 7 deg at the front wheels and 5 deg at the rear wheels,
which means the tires operate in a saturated state when the vehicle drives at the ending
section of the U turn, resulting in a higher lateral error there.
The results at 70 km/h are shown in Figures 1114. Figures 12 and 14 show the
same results as Figures 11 and 13, respectively, but in a smaller time window to make the
results more visible. The vehicle reaches and exceeds the friction limit at the U turn, which
leads to high lateral errors—the vehicle is physically not able to follow the path due to its
great curvature.
Figure 8. Simulation results at 50 km/h, including steering dynamics.
Sensors 2022,22, 5807 15 of 23
Figure 9. Simulation results at 60 km/h, without steering dynamics.
Figure 10. Simulation results at 60 km/h, including steering dynamics.
Sensors 2022,22, 5807 16 of 23
Figure 11. Simulation results at 70 km/h, without steering dynamics.
The importance of the 70 km/h case is to show whether the controller can handle
the vehicle at the friction limit or increase the steering angle to a high value, applying
unnecessarily large sideslip angles, which can not decrease the path-following errors.
According to the simulation results, the controller can handle the nonlinear characteristics
of the tires and operates stably, even when the sideslip angles reach high values. As shown
in Figures 11 and 13, the sideslip angles reach a maximal 8 deg and 5 deg at the front and
the rear wheels, respectively. The further increase in the sideslip would not generate a
larger lateral force. As the cost function penalizes the value of the steering intervention, the
steering angle does not increase. In this research, the authors do not apply constraints on
the sideslip angle; however, the controller does not generate too-large values due to the
inclusion of the nonlinear Pacejka tire model in the plant model.
In the double-lane-change section, the errors do not show a significant increase com-
pared to the lower-speed scenarios; the controller could drive the vehicle faster in this
section, but the higher speed would result in a greater error at the U-turn. Please consider
that in Table 1, the high values of the average errors at 70 km/h are caused by the large
errors at the U-turn, which has a serious effect when calculating the average errors.
Overall, the inclusion of the steering dynamics in the plant model leads to a more
accurate path following, with a smoother steering intervention, while the complexity of
the model is not seriously increased. Furthermore, as shown by the fourth subgraphs in
Figures 714, the inclusion of the steering dynamics in the plant model results in a more
accurate steering demand tracking, where the tracking error is decreased in every case.
Sensors 2022,22, 5807 17 of 23
Figure 12. Simulation results at 70 km/h, without steering dynamics in 0–15 sec interval.
Figure 13. Simulation results at 70 km/h, including steering dynamics.
Sensors 2022,22, 5807 18 of 23
Figure 14. Simulation results at 70 km/h, including steering dynamics in 0–15 sec interval.
6. Evaluation of the Effectiveness of the Controller and Comparison of the
Linear/Nonlinear Plant Model
In this section, the effectiveness of the controller is evaluated by testing on a sine wave
path. Furthermore, the effect of the vehicle plant model is also evaluated by comparing
the results of a linear vehicle model and a nonlinear vehicle model. The reference path is
shown in Figure 15. The distance between the cones is 30 m, which means 60 m wavelength,
and the lateral peak value of the sine wave is 2.5 m in both directions. Each test scenario
presented in this section is conducted using the four-wheel vehicle model presented in
Section 2.1 and the plant model contains the steering dynamics in each test case.
Figure 15. Sine wave reference path.
Sensors 2022,22, 5807 19 of 23
The results of the scenarios conducted on the sine wave path are summarized in
Table 2. Please note that when the linear plant model is applied in the 70 km/h scenario,
the results are calculated solely for the time interval of 0–4.2 s, since at 4.2 s, the vehicle
left the path. In the first scenario, the controller presented in Section 4is tested, using the
vehicle model presented in Section 2.2 for state prediction. The results at 70 km/h vehicle
velocity are shown in Figure 16. The controller drives the vehicle accurately and stably,
the maximum sideslip angles are 4 deg, and the small values in the lateral and the angular
errors also demonstrate the accuracy of the controller.
Table 2. Simulation results on the sine wave path.
Speed [km/h] eavg [m] emax [m] φavg [deg] φmax [deg]
Nonlinear plant model
70 0.098 0.192 0.689 2.414
Linear plant model 60 0.390 0.687 2.442 4.259
70 1.225 2.903 15.307 71.385
Figure 16.
Simulation results on the sine wave path at 70 km/h, including steering dynamics, using
the nonlinear vehicle model for state prediction.
In the next scenario, the nonlinear vehicle plant model is replaced by a linear one, in
which small angle assumptions are applied during the linearization of the model, and the
vehicle model is coupled with a linear tire model, while the model of the steering dynamics
remains unchanged. The resulting linear vehicle model is described by Equations (51)–(55),
Vy=.
y(51)
ay=.
Vy=cfvycflfr
mVx
+cfδ
m+crvy+crlrr
mVx
Vxr(52)
r=.
ϕ(53)
Sensors 2022,22, 5807 20 of 23
.
r=
lfcfvyl2
fcfr
IzVx
+lfcfδ
Iz
+lrcrvyl2
rcrr
IzVx(54)
.
δact =1
Tst
δact +1
Tst
δdem (55)
where the lateral force is a linear function of the sideslip angle (56).
Fy f =cαfαf,Fyr =cαrαr(56)
In (56),
cαf
and
cαr
are the cornering stiffness of the front and rear tires, respectively,
which are identified from the measurement results (Figure 2), and the state vector remains
unchanged, x= [v Vyϕrδact]T.
The first test using the linear plant model is conducted at 60 km/h and the results are
shown in Figure 17. Both the lateral and angular errors are large, while the controller is
unable to drive the vehicle along the path when the velocity is increased to 70 km/h, as
shown in Figure 18.
Figure 17.
Simulation results on the sine wave path at 60 km/h, including steering dynamics, using
the linear vehicle model for state prediction.
In the 70 km/h scenario, the vehicle is unable to follow the path anymore, resulting in
leaving the path. The reason for the poor performance of the linear-model-based control is
the neglection of the nonlinearities of the controlled vehicle in the state prediction. Both the
nonlinear terms in the vehicle model and the tire model are neglected, which results in a
less accurate state prediction; hence, in the 60 km/h scenario, performance degradation of
the controller is realized, and at 70 km/h, the controller is unable to drive the vehicle along
the path.
The results clearly show that the application of the nonlinear vehicle model coupled
with the nonlinear tire model for state prediction has a significant advantage over the linear
vehicle model and linear tire model couple. Using the nonlinear model, the controller is
able to drive the vehicle stable at higher speeds and more accurately than with the linear
Sensors 2022,22, 5807 21 of 23
model. Furthermore, the effectiveness of the proposed LTV-MPC method is also confirmed
in this section by testing the controller in the sine wave path.
Figure 18.
Simulation results on the sine wave path at 70 km/h, including steering dynamics, using
linear the vehicle model for state prediction.
7. Conclusions
In this paper, an LTV-MPC structure is proposed for path-following scenarios. The
controller uses a nonlinear vehicle model coupled with a Pacejka tire model for state
prediction. Furthermore, the dynamics of the steering system are also incorporated into the
prediction model. The reference definition is embedded into the LTV-MPC, corresponding
to the states in the plant model, which results in a simplified plant model, removing two
equations from the model, which are responsible for the coordinate transformation. The
presented reference definition method can reduce the nonlinear terms in the path-following
problem definition by transforming the path into the ego frame, resulting in a simplified
description of the vehicle dynamics, the nonlinearities of which are easier to handle. Using
this method, the transformation from the ego frame to the global frame is practically
conducted separately from the vehicle model.
During the simulation tests, the inclusion of the steering dynamics results in a more
accurate path-following performance with smaller errors and smoother steering command
demanded by the controller. Furthermore, the proposed reference definition is proven
to be effective, while the nonlinear terms in the plant model are reduced. The proposed
controller structure coupled with the plant model can handle the vehicle at sharp corners
when the tires operate in the nonlinear region. The vehicle drives stably during the tests,
while the errors remain low. According to the results, the controller is applicable for even
emergency scenarios, e.g., for a double-lane change. The effectiveness of the proposed
controller is tested in a sine wave path, where the controller is proven to be as accurate as
on the double-lane-change path. Furthermore, the effect of the linear and nonlinear plant
Sensors 2022,22, 5807 22 of 23
model is also analyzed and the advantage of applying the nonlinear plant model is clearly
proven on the sine wave path.
Regarding future research opportunities, the vehicle plant model and the model of
the steering system, as an actuator, could be further detailed, which could result in more
accurate state prediction. The dynamics of the steering system could be modeled as a func-
tion of the vehicle velocity, the yaw rate, and the lateral acceleration, and the self-aligning
torque of the tires could be incorporated into the plant model as well. Furthermore, the
proposed reference definition may reduce the computational requirements of an NLMPC,
as the plant model has fewer nonlinear terms.
Author Contributions:
Conceptualization, Á.D. and V.T.; Formal analysis, Á.D. and V.T.; Inves-
tigation, Á.D.; Methodology, Á.D.; Project administration, V.T.; Resources, V.T.; Software, Á.D.;
Supervision, V.T.; Validation, Á.D.; Writing—review & editing, Á.D. and V.T. All authors have read
and agreed to the published version of the manuscript.
Funding:
The research reported in this paper and carried out at the Budapest University of Technol-
ogy and Economics was supported by the National Research Development and Innovation Fund
(TKP2020 National Challenges Subprogram, Grant No. BME-NC) based on the charter of bolster
issued by the National Research Development and Innovation Office under the auspices of the
Ministry for Innovation and Technology.
Institutional Review Board Statement: Not applicable.
Informed Consent Statement: Not applicable.
Data Availability Statement: Not applicable.
Conflicts of Interest: The authors declare no conflict of interest.
References
1.
Bárdos, Á.; Domina, Á.; Tihanyi, V.; Szalay, Z.; Palkovics, L. Implementation and experimental evaluation of a MIMO drift-
ing controller on a test vehicle. In Proceedings of the 2020 IEEE Intelligent Vehicles Symposium (IV), Las Vegas, NV, USA,
19 October–13 November 2020; pp. 1472–1478. [CrossRef]
2.
Czibere, S.; Domina, Á.; Bárdos, Á.; Szalay, Z. Model predictive controller design for vehicle motion control at handling limits in
multiple equilibria on varying road surfaces. Energies 2021,14, 6667. [CrossRef]
3.
Orgován, L.; Bécsi, T.; Aradi, S. Autonomous drifting using reinforcement learning. Period. Polytech. Transp. Eng.
2021
,49,
292–300. [CrossRef]
4.
Lu, Z.; Shyrokau, B.; Boulkroune, B.; van Aalst, S.; Happee, R. Performance benchmark of state-of-the-art lateral path-following
controllers. In Proceedings of the 2018 IEEE 15th International Workshop on Advanced Motion Control (AMC), Tokyo, Japan,
9–11 March 2018; pp. 541–546. [CrossRef]
5.
Viana, I.B.; Kanchwala, H.; Ahiska, K.; Aouf, N. A comparison of trajectory planning and control frameworks for cooperative
autonomous driving. J. Dyn. Syst. Meas. Control 2021,143, 071002. [CrossRef]
6.
Viana, Í.B.; Kanchwala, H.; Aouf, N. Cooperative trajectory planning for autonomous driving using nonlinear model predictive
control. In Proceedings of the 2019 IEEE International Conference on Connected Vehicles and Expo (ICCVE), Graz, Austria,
4–8 November 2019; pp. 1–6. [CrossRef]
7.
Kianfar, R.; Falcone, P.; Frederikson, J. A distributed model predictive control approach to active steering control of string stable
cooperative vehicle platoon. IFAC Proc. Vol. 2013,46, 750–755. [CrossRef]
8.
Wu, N.; Huang, W.; Song, Z.; Wu, X.; Zhang, Q.; Yao, S. Adaptive dynamic preview control for autonomous vehicle trajectory
following with DDP based path planner. In Proceedings of the 2015 IEEE Intelligent Vehicles Symposium (IV), Seoul, Korea,
28 June–1 July 2015; pp. 1012–1017.
9.
Alcalá, E.; Puig, V.; Quevedo, J.; Rosolia, U. Autonomous racing using Linear Parameter Varying-Model Predictive Control
(LPV-MPC). Control Eng. Pract. 2020,95, 104270. [CrossRef]
10.
Falcone, P.; Borelli, F.; Asgari, J.; Tseng, H.E.; Hrovat, D. Predictive active steering control for autonomous vehicle systems. IEEE
Trans. Control Syst. Technol. 2007,15, 566–580. [CrossRef]
11.
Katriniok, A.; Maschuw, J.P.; Christen, F.; Eckstein, L.; Abel, D. Optimal vehicle dynamics control for combined longitudinal
and lateral autonomous vehicle guidance. In Proceedings of the 2013 European Control Conference (ECC), Zurich, Switzerland,
17–19 July 2013; pp. 974–979.
12.
Xu, Y.; Tang, W.; Chen, B.; Qiu, L.; Yang, R. A model predictive control with preview-follower theory algorithm for trajectory
tracking control in autonomous vehicles. Symmetry 2021,13, 381. [CrossRef]
Sensors 2022,22, 5807 23 of 23
13.
Wang, Z.; Bai, Y.; Wang, J.; Wang, X. Vehicle path tracking LTV-MPC controller parameter selection considering CPU computational
load. J. Dyn. Syst. Meas. Control 2018,141, 051004. [CrossRef]
14.
Borrelli, F.; Falcone, P.; Keviczky, T.; Asgari, J.; Hrovat, D. MPC-based approach to active steering for autonomous vehicle systems.
Int. J. Veh. Auton. Syst. 2005,3, 265–291. [CrossRef]
15.
Yu, C.; Zheng, Y.; Shyrokau, B.; Ivanov, V. MPC-based path following design for automated vehicles with rear wheel steering. In
Proceedings of the IEEE International Conference on Mechatronics, ICM 2021, Piscataway, NJ, USA, 7–9 March 2021. [CrossRef]
16.
Chowdhri, N.; Ferranti, L.; Iribarren, F.S.; Shyrokau, B. Integrated nonlinear model predictive control for automated driving.
Control Eng. Pract. 2021,106, 104654. [CrossRef]
17.
Falcone, P.; Borrelli, F.; Tseng, H.E.; Asgari, J.; Hrovat, D. Linear timevarying model predictive control and its application to active
steering systems: Stability analysis and experimental validation. Int. J. Robust Nonlinear Control 2007,18, 862–875. [CrossRef]
18.
Falcone, P.; Tseng, H.E.; Borrelli, F.; Asgari, J.; Hrovat, D. MPC-based yaw and lateral stabilisation via active front steering and
braking. Veh. Syst. Dyn. 2008,46, 611–628. [CrossRef]
19.
Ducajú, J.M.S.; Llobregat, J.J.S.; Cuenca, Á.; Tomizuka, M. Autonomous ground vehicle lane-keeping LPV model-based control:
Dual-rate state estimation and comparison of different RealTime control strategies. Sensors 2021,21, 1531. [CrossRef] [PubMed]
20.
Zhang, W.; Wang, Z.; Drugge, L.; Nybacka, M. Evaluating model predictive path following and yaw stability controllers for over
actuated autonomous electric vehicles. IEEE Trans. Veh. Technol. 2020,69, 12807–12821. [CrossRef]
21.
Wei, S.; Zou, Y.; Zhang, X.; Zhang, T.; Li, X. An integrated longitudinal and lateral vehicle following control system with radar
and vehicle-to-vehicle communication. IEEE Trans. Veh. Technol. 2019,68, 1116–1127. [CrossRef]
22.
Ni, L.; Gupta, A.; Falcone, P.; Johannesson, L. Vehicle lateral motion control with performance and safety guarantees. IFAC-
PapersOnLine 2016,49, 285–290. [CrossRef]
23.
Moradi, M.; Fekih, A. A stability guaranteed robust fault tolerant control design for vehicle suspension systems subject to actuator
faults and disturbances. IEEE Trans. Control Syst. Technol. 2015,23, 1164–1171. [CrossRef]
24.
Jin, X.; Wang, J.; Yan, Z.; Xu, L.; Yin, G.; Chen, N. Robust Vibration Control for Active Suspension System of In-Wheel-Motor-
Driven Electric Vehicle Via µ-Synthesis Methodology. J. Dyn. Syst. Meas. Control 2022,144, 051007. [CrossRef]
25.
Pacejka, H.B.; Bakker, E.; Nyborg, L. Tyre Modelling for Use in Vehicle Dynamics Studies; SAE Technical Paper Series; SAE
International: Warrendale, PA, USA, 1987; p. 870421.
26.
Schwenzer, M.; Ay, M.; Bergs, T.; Abel, D. Review on model predictive control: An engineering perspective. Int. J. Adv. Manuf.
Technol. 2021,117, 1327–1349. [CrossRef]
27.
Rákos, O.; Aradi, S.; Bécsi, T. Lane Change Prediction Using Gaussian Classification, Support Vector Classification and Neural
Network Classifiers. Period. Polytech. Transp. Eng. 2020,48, 327–333. [CrossRef]
... MPC (Model Predictive Control) has proven successful for both stabilization and track-following drift tasks [18,19]. It also performed well when simulating changing road conditions, thus indicating a good adaptive property. ...
... This paper presents novel results on applying reinforcement learning agents for autonomous drifting on a full-scale, real test vehicle. This is the same car that was used in [13,19]. To the authors' best knowledge, this is the first time such an agent was successfully deployed to perform autonomous drifting on a real vehicle. ...
... Typically, most of these are rear-wheel drive (RWD) vehicles, so the chosen model is also RWD, but, at the same time, drift-like behavior is not impossible with front-wheel or four-wheel drive vehicles either. The vehicle model features presented throughout this section are validated based on previous works [13,19]. ...
Article
Full-text available
Enhancing the safety of passengers by venturing beyond the limits of a human driver is one of the main ideas behind autonomous vehicles. While drifting is mostly witnessed in motorsports as an advanced driving technique, it could provide many possibilities for improving traffic safety by avoiding accidents in extreme traffic situations. The purpose of the research presented in this article is to provide a machine learning-based solution to autonomous drifting as a proof of concept for vehicle control at the limits of handling. To achieve this, reinforcement learning (RL) agents were trained for the task in a MATLAB/Simulink-based simulation environment, using the state-of-the-art Soft Actor–Critic (SAC) algorithm. The trained agents were tested in reality at the ZalaZONE proving ground on a series production sports car with zero-shot transfer. Based on the test results, the simulation environment was improved through domain randomization, until the agent could perform the task both in simulation and in reality on a real test car.
... Researching steady-state drift problems with linear control methods started at the beginning of the previous decade (Voser et al., 2010;Velenis et al., 2011), but not later than ten years later that a MIMO controller was successfully applied in a realworld environment (Bárdos, 2020). Also, Model Predictive Controls (MPCs) were considerably successful even in handling more complex, trajectory-following tasks and changing environmental conditions (Czibere et al., 2021;Domina & Tihanyi, 2022). Furthermore, supervised neural network-based hierarchical control solutions have also been applied and evaluated on RC cars (Acosta & Kanarachos, 2018;Yang et al., 2022). ...
... They use the global path as a general direction while dynamically avoiding obstacles and performing short-distance path planning with the assistance of local path planning [4]. Based on algorithmic principles, path planning methods can be classified into raster mapbased, sampling-based, and bionic heuristic-based approaches [5][6][7][8][9][10][11]. Map-based methods include algorithms, such as Dijkstra's algorithm, A*, and D*. ...
Article
Full-text available
Autonomous vehicles can reduce labor power during cargo transportation, and then improve transportation efficiency, for example, the automated guided vehicle (AGV) in the warehouse can improve the operation efficiency. To overcome the limitations of traditional path planning algorithms in unknown environments, such as reliance on high-precision maps, lack of generalization ability, and obstacle avoidance capability, this study focuses on investigating the Deep Q-Network and its derivative algorithm to enhance network and algorithm structures. A new algorithm named APF-D3QNPER is proposed, which combines the action output method of artificial potential field (APF) with the Dueling Double Deep Q Network algorithm, and experience sample rewards are considered in the experience playback portion of the traditional Deep Reinforcement Learning (DRL) algorithm, which enhances the convergence ability of the traditional DRL algorithm. A long short-term memory (LSTM) network is added to the state feature extraction network part to improve its adaptability in unknown environments and enhance its spatiotemporal sensitivity to the environment. The APF-D3QNPER algorithm is compared with mainstream deep reinforcement learning algorithms and traditional path planning algorithms using a robot operating system and the Gazebo simulation platform by conducting experiments. The results demonstrate that the APF-D3QNPER algorithm exhibits excellent generalization abilities in the simulation environment, and the convergence speed, the loss value, the path planning time, and the path planning length of the APF-D3QNPER algorithm are all less than for other algorithms in diverse scenarios.
Article
Model predictive control (MPC) is a closed-loop optimization framework that can solve the real-time control challenges of inverter-based distributed energy resources (DERs) in smart grids. This paper addresses the challenge of heavy reliance of model predictive controllers on physics-based dynamic models by proposing a data-driven MPC framework via sparse regression (SR) theory and nonlinear model predictive control (NLMPC) framework. Unlike existing approaches that rely on approximate models based on physical principles or experiments, the proposed framework directly captures the dynamics of the DERs using measurements. This capability enables power sharing among DERs and active/reactive load support with high precision. The framework can capture uncertainties and drift dynamics of DERs by updating the data-driven model on a timely manner for running the MPC for effective power sharing. By employing this approach, the overall effectiveness of active and reactive power sharing is enhanced without compromising voltage and frequency control. The proposed optimal control strategy is validated through real-time simulations conducted on a 3-DER microgrid (MG) using OPAL-RT. The results demonstrate the successful estimation of DER dynamics using the SR method and accurate power sharing through NLMPC. Furthermore, NLMPC not only achieves a high degree of precision in power tracking but also outperforms other MPC strategies that rely on successive linearization, with a mean absolute percentage error (MAPE) of 6.83% for active power and 5.71% for reactive power.
Article
This article is concerned with the robust model predictive control (RMPC) problem for polytopic uncertain systems under the round-robin (RR) scheduling in the high-rate communication channel. From a set of sensors to the controller, several sensors transmit the data to the remote controller via a shared high-rate communication network, data collision might happen if these sensors start transmissions at the same time. For the sake of preventing data collision in the high-rate communication channel, a communication scheduling known as RR is used to arrange the data transmission order, where only one node with token is allowed to send data at each transmission instant. In accordance with the token-dependent Lyapunov-like approach, the aim of the problem addressed is to design a set of controllers in the framework of RMPC such that the asymptotical stability of the closed-loop system is guaranteed. By taking the effect of the underlying RR scheduling in the high-rate communication channel into consideration, sufficient conditions are obtained by solving a terminal constraint set of an auxiliary optimization problem. In addition, an algorithm including both off-line and online parts is provided to find a sub-optimal solution. Finally, two simulation examples are used to demonstrate the usefulness and effectiveness of the proposed RMPC strategy.
Article
Full-text available
Electronic vehicle dynamics systems are expected to evolve in the future as more and more automobile manufacturers mark fully automated vehicles as their main path of development. State-of-the-art electronic stability control programs aim to limit the vehicle motion within the stable region of the vehicle dynamics, thereby preventing drifting. On the contrary, in this paper, the authors suggest its use as an optimal cornering technique in emergency situations and on certain road conditions. Achieving the automated initiation and stabilization of vehicle drift motion (also known as powerslide) on varying road surfaces means a high level of controllability over the vehicle. This article proposes a novel approach to realize automated vehicle drifting in multiple operation points on different road surfaces. A three-state nonlinear vehicle and tire model was selected for control-oriented purposes. Model predictive control (MPC) was chosen with an online updating strategy to initiate and maintain the drift even in changing conditions. Parameter identification was conducted on a test vehicle. Equilibrium analysis was a key tool to identify steady-state drift states, and successive linearization was used as an updating strategy. The authors show that the proposed controller is capable of initiating and maintaining steady-state drifting. In the first test scenario, the reaching of a single drifting equilibrium point with −27.5° sideslip angle and 10 m/s longitudinal speed is presented, which resulted in −20° roadwheel angle. In the second demonstration, the setpoints were altered across three different operating points with sideslip angles ranging from −27.5° to −35°. In the third test case, a wet to dry road transition is presented with 0.8 and 0.95 road grip values, respectively.
Article
Full-text available
Autonomous vehicles or self-driving cars are prevalent nowadays, many vehicle manufacturers, and other tech companies are trying to develop autonomous vehicles. One major goal of the self-driving algorithms is to perform manoeuvres safely, even when some anomaly arises. To solve these kinds of complex issues, Artificial Intelligence and Machine Learning methods are used. One of these motion planning problems is when the tires lose their grip on the road, an autonomous vehicle should handle this situation. Thus the paper provides an Autonomous Drifting algorithm using Reinforcement Learning. The algorithm is based on a model-free learning algorithm, Twin Delayed Deep Deterministic Policy Gradients (TD3). The model is trained on six different tracks in a simulator, which is developed specifically for autonomous driving systems; namely CARLA.
Article
Full-text available
Model-based predictive control (MPC) describes a set of advanced control methods, which make use of a process model to predict the future behavior of the controlled system. By solving a—potentially constrained—optimization problem, MPC determines the control law implicitly. This shifts the effort for the design of a controller towards modeling of the to-be-controlled process. Since such models are available in many fields of engineering, the initial hurdle for applying control is deceased with MPC. Its implicit formulation maintains the physical understanding of the system parameters facilitating the tuning of the controller. Model-based predictive control (MPC) can even control systems, which cannot be controlled by conventional feedback controllers. With most of the theory laid out, it is time for a concise summary of it and an application-driven survey. This review article should serve as such. While in the beginnings of MPC, several widely noticed review paper have been published, a comprehensive overview on the latest developments, and on applications, is missing today. This article reviews the current state of the art including theory, historic evolution, and practical considerations to create intuitive understanding. We lay special attention on applications in order to demonstrate what is already possible today. Furthermore, we provide detailed discussion on implantation details in general and strategies to cope with the computational burden—still a major factor in the design of MPC. Besides key methods in the development of MPC, this review points to the future trends emphasizing why they are the next logical steps in MPC.
Conference Paper
Full-text available
Many studies have been recently exploited to discuss the path following control algorithms for automated vehicles using various control techniques. However, path following algorithm considering the possibility of automated vehicles with rear wheel steering (RWS) is still less investigated. In this study, we implemented nonlinear model predictive control (NMPC) on a passenger vehicle with active RWS for path following. The controller was compared to two other variations of NMPC where the rear steering angle is proportional to the front or fixed to zero. Simulation results suggested that the proposed controller outperforms the other two variations and the baseline controllers (Stanley and LQR) in terms of accuracy and responsiveness.
Article
Full-text available
Research on trajectory tracking is crucial for the development of autonomous vehicles. This paper presents a trajectory tracking scheme by utilizing model predictive control (MPC) and preview-follower theory (PFT), which includes a reference generation module and a MPC controller. The reference generation module could calculate reference lateral acceleration at the preview point by PFT to update state variables, and generate a reference yaw rate in each prediction point. Since the preview range is increased, PFT makes the calculation of yaw rate more accurate. Through physical constraints, the MPC controller can achieve the best tracking of the reference path. The MPC problem is formulated as a linear time-varying (LTV) MPC controller to achieve a predictive model from nonlinear vehicle dynamics to continuous online linearization. The MPC-PFT controller method performs well by increasing the effective length of the reference path. Compared with MPC and PFT controllers, the effectiveness and robustness of the proposed method are proved by simulations of two typical working conditions.
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
This paper proposes a robust vibration controller design for active suspension system of in-wheel-motor-driven electric vehicles based on unified µ-synthesis framework. First, multiple parameter uncertainties and unmodelled high-order dynamics of the suspension are analyzed and discussed. By applying the mixed uncertainties and linear fraction transformation, model perturbations are separated from the suspension system and their perturbation bounds can be also limited. Then the uncertain quarter-vehicle active suspension model with dynamic damping in-wheel motor driven system is established, in which in-wheel motor is suspended as a dynamic vibration absorber. The resulting robust µ-synthesis feedback controller of generalized closed-loop active suspension system is designed with the concept of structured singular value µ and µ-synthesis theoretics, and solved via comprehensive solution of the D-G-K iteration. The µ analysis results show that the µ controller possesses less conservative stability and performance margins as compared to the H8 method against system uncertainties. Furthermore, simulations of nominal and perturbed suspension systems are implemented and the corresponding frequency and time-domain responses are compared, and then simulations results confirm that the developed µ controller is capable of attenuating the negative vibration of the active suspension system compared with H8 controller and passive suspension.
Article
This work considers the cooperative trajectory-planning problem along a double lane change scenario for autonomous driving. In this paper we develop two frameworks to solve this problem based on distributed model predictive control (MPC). The first approach solves a single non-linear MPC problem. The general idea is to introduce a collision cost function in the optimization problem at the planning task to achieve a smooth and bounded collision function and thus to prevent the need to implement tight hard constraints. The second method uses a hierarchical scheme with two main units: a trajectory-planning layer based on mixed-integer quadratic program (MIQP) computes an on-line collision-free trajectory using simplified motion dynamics, and a tracking controller unit to follow the trajectory from the higher level using the non-linear vehicle model. Connected and automated vehicles (CAVs) sharing their planned trajectories lay the foundation of the cooperative behaviour. In the tests and evaluation of the proposed methodologies, MATLAB-CARSIM co-simulation is utilized. CARSIM provides the high fidelity model for the multi-body vehicle dynamics. MATLAB-CARSIM conjoint simulation experiments compare both approaches for a cooperative double lane change maneuver of two vehicles moving along a one-way three-lane road with obstacles.
Article
This article presents an innovative control approach for autonomous racing vehicles. Linear Parameter Varying (LPV) theory is used to model the dynamics of the vehicle and implement an LPV-Model Predictive Controller (LPV-MPC) that can be computed online with reduced computational cost. The optimal time problem is solved by an optimal off-line trajectory planner that calculates the best trajectory under the constraints of the circuit. An identification of the system model based on optimization is also carried out. The planning and control scheme is validated in simulation and experimentally in a real platform where the effectiveness of the proposed LPV-MPC is demonstrated.