applied

sciences
Article
MPC and PSO Based Control Methodology for Path
Tracking of 4WS4WD Vehicles
Qifan Tan 1, * ID
, Penglei Dai 2 , Zhihao Zhang 3 and Jay Katupitiya 3
1 School of Mechanical, Electronic and Control Engineering, Beijing Jiaotong University, Beijing 100044, China
2 School of Mechanical and Mechatronic Engineering, University of Technology Sydney,
Sydney, NSW 2007, Australia; penglei.dai@uts.edu.au
3 School of Mechanical and Manufacturing Engineering, The University of New South Wales,
Sydney, NSW 2052, Australia; zhihao.zhang1@unsw.edu.au (Z.Z.); j.katupitiya@unsw.edu.au (J.K.)
* Correspondence: 14116355@bjtu.edu.cn; Tel.: +86-186-0124-6239

Received: 31 May 2018; Accepted: 13 June 2018; Published: 19 June 2018

Abstract: Four wheel steering and four wheel drive (4WS4WD) vehicles are over-actuated systems
with superior performance. Considering the control problem caused by the system nonlinearity
and over-actuated characteristics of the 4WS4WD vehicle, this paper presents two methods to
enable a 4WS4WD vehicle to accurately follow a predefined path as well as its reference trajectories
including velocity and acceleration profiles. The methodologies are based on model predictive
control (MPC) and particle swarm optimization (PSO), respectively. The MPC method generates
the virtual inputs in the upper controller and then allocates the actual inputs in the lower controller
using sequential quadratic programming (SQP), whereas the PSO method is proposed as a fully
optimization based method for comparison. Both methods achieve optimization of the steering angles
and wheel forces for each of four independent wheels simultaneously in real time. Simulation results
achieved by two different controllers in following the reference path with varying disturbances are
presented. Discussion about two methodologies is provided based on their theoretical analysis and
simulation results.

Keywords: 4WS4WD vehicle; force control; MPC; PSO; path tracking

1. Introduction
With the development of Autonomous Ground Vehicles (AGVs) in the last few decades,
the demand for accuracy, maneuverability and controllability in vehicle’s navigation is ever increasing.
For example, an AGV may be required to follow a path accurately under unstructured and uneven
terrain conditions, where a significant amount of wheel slip and unpredictable disturbance forces
occur at the vehicle’s wheels. The 4WS4WD vehicle, with four wheels that can be steered and driven
independently, is a revolutionary platform that has great potential to perform high maneuverability
and flexibility in harsh environments.
The main challenge in the control of 4WS4WD control is the number of control inputs (four steering
angles and four drive torques), which results in an over-actuated system, where only three outputs
including its degree of freedom (DOF) in the longitudinal, lateral and angular directions of the vehicle
are concerned. How to allocate all eight control inputs to achieve high path following performance has
not yet been effectively solved. The control allocation is proposed to handle the control problems of
over-actuated systems [1]. Generally, the control allocation can be treated as an optimization problem.
For controller designing, a model of the vehicle under control is generally required to facilitate
the selection of future control inputs. A dynamic model describes the states of the vehicle based
on the forces applied. However, the development of a detailed vehicle dynamic model is always
a challenging task due to the uncertainty of parameters and the complex disturbances from the external

Appl. Sci. 2018, 8, 1000; doi:10.3390/app8061000 www.mdpi.com/journal/applsci

Appl. Sci. 2018, 8, 1000 2 of 24

forces. The majority of existing control methodologies for 4WS4WD vehicles are proposed based on the
linearized dynamic model, which lead to the loss of input degree of freedom [2,3]. Meanwhile,
some other methods use nonlinear dynamic models in the control of 4WS4WD vehicles [4,5],
where control inputs are subjected to some relationship constraints to simplify the controller design.
However, in practice, the four independent wheels may interact with different terrain conditions,
where different slip, wheel forces and terrain disturbances are generated on the corresponding contact
patches. Hence, it is desirable to make four wheels individually controlled, thereby limiting and/or
overcoming different slip and disturbance on different contact patch.
There is an abundance of literature that presents kinematic modelling of ground vehicles [6–8],
in which the vehicles are assumed to operate at low speeds to reduce the dynamic effects. Most vehicle
kinematic models are developed based on the non-integrable kinematic constraints, known as
non-holonomic constraints. As a result, the wheel slip has to be ignored with the assumption of
zero relative velocity between wheels and terrain [9,10]. To utilize the kinematic model’s advantage of
keeping the steering control relatively independent of velocity control [11], it is desirable to incorporate
wheel slip in the vehicle kinematic model so as to facilitate the accurate vehicle control in complex
terrain conditions where the no-slip assumption is not applicable.
To realize force control, a novel type of 4WS4WD vehicle with force sensors at each wheel
has been designed as shown in Figure 1. This vehicle possesses the characteristics of independent
steering and drive control at each wheel and force measurements. In this work, the dynamic model is
partitioned into a hierarchy of three levels to facilitate the incorporation of force sensors and overcome
uncertainties in the model. The force sensors at the drive unit allow the measurement of actual force
data, while models of the drive unit and tire are used for simulation.

Power Unit
Steering Unit

Driving Unit

Force Sensor

Figure 1. The 4WS4WD (four wheel steering and four wheel drive) vehicle.

Model predictive control (MPC) is selected for its ability in handling linear constraints and
time-varying systems as well as its good performance in tracking problems. Particle swarm
optimization (PSO) is also selected for its fast searching speed in global optimization. MPC and
PSO have been successfully applied in the controller design of real-time control systems [12–17]. In this
work, MPC and PSO based control methods are proposed to realize the controlling aim of achieving
good path following performance as well as high motion quality via vehicle steering control and
independent force control at four wheels.
As novel contributions, the MPC methodology is applied to achieve precise path tracking of
4WS4WD vehicle. Based on the MPC theory, an offline control law is proposed to guarantee the
stability of the upper controller. An sequential quadratic programming (SQP) based control allocation
Appl. Sci. 2018, 8, 1000 3 of 24

is developed to control the 4WS4WD vehicle in the lower controller. The inclusion of full independent
force control and steering control on all four wheels enable the maximization of performance. In this
work, comparison of MPC and PSO on the same vehicle model is provided, in which the proposed
PSO control methodology is a further refinement of the PSO methodology presented previously
in [18]. The PSO control methodology in this paper simplifies the derivation and gives an algorithm
in a more general form, which facilitates the comparison with other control methods. In addition,
both methodologies are compared with the kinematic model based method proposed in [19].
The paper is organized as follows: Section 2 describes the 4WS4WD vehicle modelling. The MPC
and PSO control methodologies are presented in Section 3. In Section 4, simulation setup and the
reference path are presented. The result of the two controllers are compared. In Section 5, the discussion
about two methodologies are provided based on their theoretical analysis and simulation results.
Finally, conclusions are provided in Section 6.

2. Vehicle System Modelling

In order to develop suitable control methodology, the 4WS4WD vehicle must be modelled for
controller design and simulation. The model used for this work is a fully dynamic model that consists
of three components, vehicle body dynamics to estimate vehicle body movement, drive unit dynamics
to model the tire force acting on the drive unit in vehicle coordinate frame and a tire dynamic model to
model the force generated by the tire in a wheel coordinate frame. The separation of dynamic model
allows force sensors to be incorporated easily and reduces uncertainties by obtaining actual force
measurements. In the end, an offset error model is proposed for evaluating the tracking performance
and is used in the controller design.

2.1. Vehicle Body Dynamic Model

As shown in Figure 2, the vehicle body dynamic model describes the motion of the vehicle by
representing each drive unit as a pair of forces, with FSl in the longitudinal and FSL in the lateral
direction of the corresponding wheel. The reason for separating the dynamics of the vehicle at the drive
unit boundary is to allow force sensors to be incorporated to measure the forces acting on each wheel,
instead of relying on the tire model to estimate them. To facilitate the notations, the four wheels are
numbered by 1, 2, 3 and 4 in circles. In particular, the force acting at each force sensor is decomposed
into two components (i.e., FSli and FSLi , i = 1, . . . , 4) normal to each other.
The equation of the vehicle body dynamics can be expressed as
 
  R1 FAS1
ar  R F 
acc =  al  = M −1 C  2 AS2 , (1)
   
 R3 FAS3 
γ
R4 FAS4
where the acceleration vector denoted by acc consists of longitudinal acceleration al , radial acceleration
ar and angular acceleration γ.
The mass matrix is represented by M:
 
M − 4md 0 0
M= 0 M − 4md 0  , (2)
 
0 0 Jz
where M is the mass of the vehicle, Jz is the vehicle body inertia, and md is the mass of the drive unit.
Appl. Sci. 2018, 8, 1000 4 of 24

vw1
FSl1
δf

FSL1
1

vw2
YL FSl2
Vl X δf
Vr Ω L

δr vw4 γ 2

CG
al
FSl4 ar FSL2
OL
4
FSL4

Lh Lf
δr vw3
3
Y FSl3
θ Lh Lr
O X FSL3

Figure 2. Vehicle body dynamic model.

The matrix C is decided by vehicle dimension, written as

 
1 0 1 0 1 0 1 0
C= 0 1 0 1 0 1 0 1 . (3)
 
− L h L f L h L f L h − Lr − L h − Lr
The force vectors FASi are defined as
h iT
FASi = FSli FSLi , i = 1, . . . , 4. (4)

The transfer matrices Ri from wheel frame to vehicle local coordinate frame X L − OL − YL are
presented in
" #
cos δi − sin δi
Ri = , i = 1, . . . , 4. (5)
sin δi cos δi

The lateral components FSLi are determined by the lateral forces acting on tires, while the
longitudinal components FSli are viewed as intermediate control inputs of vehicle system, which can
be achieved by controlling the torques (i.e., Ti , i = 1, . . . , 4) applied on wheels. To simplify the coupled
issue of left and right wheel steerings, the steering angles are constrained by

δ1 = δ2 = δ f ,
(6)
δ3 = δ4 = δr .
Therefore, the vehicle system in this work considers six control inputs (i.e., δ f , δr and Ti , i = 1, . . . , 4)
in total.

2.2. Driving Unit Dynamic Model

While the force sensors provide measurement of the current forces acting on each wheel, controller
design requires prediction of future values based on control inputs. The drive unit dynamic model as
shown in Figure 3 is described in this section.
Appl. Sci. 2018, 8, 1000 5 of 24

YWi
FwLi
FSli
md awLi
FSLi

md awli XWi
OWi

Fwli

Figure 3. Driving unit dynamic model.

In each driving unit, the force sensor mounted on the wheel hub rotate with the wheel.
Force measurement is in the direction of the steering angle. According to Figure 3, the dynamics of
driving unit can be expressed by
h iT h iT
FASi = FSli FSLi = Fwi − md awli md awLi , (7)

where awli and awLi denote the longitudinal and lateral accelerations of each driving unit i in the wheel
coordinate system (XWi − OWi − YWi ), respectively. Using the transfer matrices Ri and the accelerations
al , ar and γ given in vehicle body dynamic model, awli and awLi can be deduced as

h i h i
awl1 awL1 = al − Lh γ ar + L f γ R1 ,
h i h i
awl2 awL2 = al + Lh γ ar + L f γ R2 ,
h i h i (8)
awl3 awL3 = al + Lh γ ar − Lr γ R3 ,
h i h i
awl4 awL4 = al − Lh γ ar − Lr γ R4 .

In Equation (7), Fwi represents the force vector acting on each wheel, which is written as
h iT
Fwi = Fwli FwLi . (9)

2.3. Tire Model

The actual generation of wheel forces are from the contact between tires and the ground. The wheel
forces depend on the surface friction, load, and the intrinsic properties of the tire. To analyze the wheel
forces Fwi in Equation (9), the tire model is built as shown in Figure 4.
Considering the generation of wheel forces and external disturbances, Fwi can be expressed as
h iT h iT
Fwi = Fwli FwLi = Fli − Fgli FLi − FgLi , (10)

where Fli and FLi (i = 1, . . . , 4) are longitudinal and lateral forces caused by wheel slip, respectively.
Fgli and FgLi denote the terrain disturbances corresponding to Fli and FLi .
According to [20], the longitudinal force can be considered proportional to the slip ratio in small
range, which is expressed by

 k F ς i , |ς | ≤ 0.1,
li Ni 0.1 i
Fli = (11)
 k F , |ς | > 0.1,
li Ni i
Appl. Sci. 2018, 8, 1000 6 of 24

where FNi is the weight force on wheel i. In the simulation, the load transfer due to the longitudinal and
lateral accelerations and roll angle are taken into consideration. FNi can be calculated by considering
longitudinal and lateral load transfer according to [21]. The longitudinal slip stiffness k li is determined
by the tire type and terrain condition. ς i is the longitudinal slip ratio of wheel i presented in [22]:

Rw ωi − vwi
, Rw ωi > vwi > 0,





 R w ωi
ς i = Rw ωi − vwi , Rw vwi > ωi > 0,

(12)


 vwi


 0, Rw vwi = ωi ≥ 0,


where Rw and ωi denote the tire radius and angular velocity of wheel i as shown in Figure 4b.
vwi represents the actual velocity of the wheel i, which can be calculated by the vehicle geometry as
well as velocities Vl , Vr and Ω shown in Figure 2. Note that the model is proposed only considering the
vehicle is moving forward. Thus, vwi and ωi are set to be nonnegative. As is discussed in Equation (12),
the slip ratio is zero when the vehicle is resting.
Finally, as per Figure 4b, the wheel dynamic equation indicating the relationship between Ti and
Fli can be written as

Ti = ( Fli + Fri ) Rw + ω̇i Jw , (13)

where Jw represents the wheel inertia, and Fri is the rolling resistance calculated by

Fri = FNi (Kr0 + Kr1 Vl2 ), (14)

where Kr0 = 0.015 and Kr1 = 7 × 10−6 s2 /m2 are the parameters for common car tires [23].

vi
vwi
Fli
δi
αi
XWi Fri ωi
βi
Ti
XL
YWi FLi
YL OWi vsi Rw vwi

Fli
OL

FNi
(a) (b)
Figure 4. Tire model. (a) Top view of the tire; (b) lateral view of the tire.

The steering motion of wheels result in the lateral slip velocities vsi , which causes the actual
direction of wheel velocity vwi to differ from the wheel center plane by the slip angle αi . According to
the tire lateral characteristic curve [24], a linear model for FLi is given in
Appl. Sci. 2018, 8, 1000 7 of 24


 − k F α i , | α i | ≤ 5◦ ,
 Li Ni 5


FLi = −k Li FNi , αi > 5◦ , (15)


 k F , α < −5◦ ,

Li Ni i

where k Li is the lateral slip stiffness of the wheel i. Note that, due to the sign conventions in
XWi − OWi − YWi and X L − OL − YL , a negative slip angle causes a positive lateral force and vice versa.
As shown in Figure 4a, the slip angle αi can be calculated by steering angle δi and side slip
angle β i ,

αi = β i − δi , (16)

where the expression of β i is available in our previous work presented in [25].

2.4. Offset Model

An offset error model is proposed for the evaluation of the performance of the controllers.
The reference position (RP) of the vehicle on the reference path is defined as the normal projection
of the heading of the vehicle at the centre of gravity on to the reference path, as shown in Figure 5.
The tracking error in vehicle coordinate frame consists of lateral offset error los and heading error θos ,
expressed as
h iT
p
g os = 0 los θos (17)

RP

alp alR
arR Vlp θ os VlR
VrR
~ γp θ
Vrp θ los
arp Ωp p
PP
YL
Vl X
Vr Ω L

γ
CG al
ar
OL

Y
θ
O X

Figure 5. Offset model.

The position errors are always in the lateral direction of the vehicle. The longitudinal position
error is set to 0. The heading error can be found by

θos = θ − θre f , (18)

Appl. Sci. 2018, 8, 1000 8 of 24

where θre f is the reference heading direction of the RP, tangential to the reference path.
The velocity error states vel
f is the difference between vehicle velocity vel and reference velocity
states velre f in the vehicle coordinate frame. vel
f can be expressed as

h iT
vel
f = Vlerr Vrerr Ωerr = vel − velre f , (19)

where Vlerr , Vrerr and Ωerr are the errors in longitudinal, lateral, and angular velocity, respectively.
Reference velocity states are defined as
h iT
velre f = Vlre f Vrre f Ωre f , (20)

where reference angular velocity Ωre f is the rate of change of the heading of RP on the reference path.
The reference longitudinal velocity Vlre f and lateral velocity Vrre f in vehicle coordinate frames
can be derived from the reference path coordinate frame by the equations:

Vlre f = Vl p cos(θos ) − Vrp sin(θos ),

(21)
Vrre f = Vl p sin(θos ) + Vrp cos(θos ),

where Vl p is the reference longitudinal velocity, taken as tangential to the reference path. Vrp is the
reference radial velocity normal to the reference path at RP.
Finally, the acceleration error states a
g cc are written as:
h iT
a
g cc = alerr arerr γerr = acc − accre f , (22)

where acc is the difference between vehicle acceleration acc and reference acceleration accre f .
The reference acceleration vector accre f is defined as
h iT
accre f = alre f arre f γre f , (23)

where alre f and arre f are, respectively, the reference longitudinal and lateral acceleration at point
RP, measured in the direction and normal to the vehicle heading, and γre f is the reference angular
acceleration of the point RP.
The reference acceleration in vehicle coordinate frame can be derived from the path coordinate
frame by
alre f = al p cos(θos ) − arp sin(θos ),
(24)
arre f = al p sin(θos ) + arp cos(θos ),
where al p and arp are the reference acceleration tangential and normal to the reference path.

3. Control Methodology
Two methods of finding the optimal control inputs for steering angles (i.e., δ f and δr ) and drive
forces FSli , (i = 1, ..., 4) are developed. The first method used model predictive control (MPC) with
a sequential quadratic programming (SQP) solver. The second method is particle swarm optimization
(PSO). The objective of the controller is to accurately follow a predetermined path through multiple
terrain types.
Appl. Sci. 2018, 8, 1000 9 of 24

3.1. MPC-SQP Method

3.1.1. Offset Model Linearization

In order to be applied in the MPC controller design, the offset model is linearized by a feedback
linearization method. By defining the state vector x = [ x1 , x2 , x3 , x4 , x5 ], as x1 = Vlerr , x2 = los , x3 = l˙os ,
x4 = θos and x5 = θ̇os , the offset model can be written in the following form:

ẋ1 = ar − ( al p cos x4 − arp sin x4 ),

ẋ2 = x3 ,
ẋ3 = ar − ( al p sin x4 + arp cos x4 ), (25)
ẋ4 = x5 ,
ẋ5 =γ − γre f .

In this model, ar , al and γ are considered as the inputs while other constants including reference
accelerations and velocities can be obtained from the vehicle states. The nonlinearity of the model
comes from trigonometric terms of x4 . For accurate path tracking problems, the yaw error is assumed
to vary smoothly within [−10° 10°]. Then, the model can be linearized by feedback linearization.
Define the input vector u = [ ar al γ] T ; then, the new input vector at time tk is obtained as
 
al p cos x4 − arp sin x4
v( x, t)|t=tk = u −  al p sin x4 + arp cos x4  , (26)
 
γre f
x4 = x4 ( t k )

and the offset model is expressed as

ẋ = A · x + v( x, t)|t=tk , (27)

where

 
0 0 0 0 0
0 0 1 0 0
 
A = 0 0 0 0 0 .
 
 
0 0 0 0 1
0 0 0 0 0

It can be seen that the model is time-varying but can be treated as a linear model at each sampling
step. The output vector is denoted by yc and the equation is written as

yc = Cc x, (28)

where

 
1 0 0
0 1 0
 
Cc = 0 0 0 .
 
 
0 0 1
0 0 0
Appl. Sci. 2018, 8, 1000 10 of 24

3.1.2. Model Predictive Control

To develop the MPC controller, the offset model as given in Equations (27) and (28) needs to be
discretized and its discrete-time state vector form can be written as

xd (k + 1) = Ad xd (k ) + Bd ud (k),
yd (k) =Cd xd (k), (29)

where xd (k), yd (k) and ud (k) are the state vector, output vector and input vector, respectively.
The coefficient matrices Ad , Bd and Cd are updated after discretization.
To eliminate undesirable oscillations, embedded integrator vectors ∆xd (k) = xd (k + 1) − xd (k),
∆u(k) = ud (k + 1) − ud (k) are defined, thereby an augmented state-space model can be expressed as

" # " #" # " #

∆xd (k + 1) Ad O T ∆xd (k) Bd
= + ∆u(k),
yd (k + 1) Cd Ad I yd (k) Cd Bd
" #
h i ∆x (k)
d
yd (k ) = O I , (30)
yd (k)

where O is a zero matrix, and I represents the identity matrix.

Defining the new state vector x(k) = [∆xd (k) T yd (k) T ] T , the augmented model can be written in
the following matrix form:

x(k + 1) = Ax(k ) + B∆u(k ),

y(k ) =Cx(k), (31)

where
" # " #
Ad OT Bd h i
A= , B= , C = OT I .
Cd Ad I Cd Bd

Theorem 1. Given a discrete time system following the form of Equation (31), the asymptotic stabilization of the
closed-loop system can be realized by substituting the first item of ∆U∗ as the control input ∆u(k) , when ∆U∗
is the optimal solution of the following optimization problem:

arg min J = (Rs − Y) T (Rs − Y) + ∆UT R̄∆U,

∆U
(32)
s.t. y Np (∆U) = 0,

where Y denotes the predicted output sequence, ∆U denotes the future input sequence, and Np is the prediction
horizon. Rs is the sequence of the control target vector. y Np (∆U) represents the error between the final predicted
output and target.

Proof of Theorem 1. To prove the stability, the Lyapunov function V ( xk ) is defined equal to the value
of the objective function Jk subjected to its optimal solution, which can be expressed as

V ( xk ) = min Jk
Np N p −1 (33)
= ∑ ykT+i yk+i + ∑ ∆ukT+i rw ∆uk+i ,
i =1 i =0
Appl. Sci. 2018, 8, 1000 11 of 24

where, ∆uk , ..., ∆uk+ Np −1 are obtained by the optimal solution of future inputs, and yk , ..., yk+ Np
represent the corresponding error sequence between future outputs and target. rw is the nonnegative
gain matrix.
According to the definition in Equation (33), the Lyapunov function at the next sample k + 1 is
written as

Np N p −1
V ( x k +1 ) = ∑ ykT+1+i yk+1+i + ∑ ∆ukT+1+i rw ∆uk+1+i . (34)
i =1 i =0

To facilitate the comparison between two neighboring Lyapunov function values, a intermediate
function V̄ is defined, which is formed by evaluating V ( xk+1 ) with a defined inputs sequence, which is
obtained by shifting the optimal inputs sequence of V ( xki ) one step forward, and setting its last input
∆uk+ Np as zero. It is obvious that the objective function value of non-optimal inputs sequence has to
be no less than V ( xki+1 ), which can be expressed as

V ( xk+1 ) ≤ V̄, (35)

thereby,

V ( xk+1 ) − V ( xk ) ≤ V̄ − V ( xk ). (36)

Since V̄ shares the same future inputs sequence and the predictive outputs sequence with V ( xk ) for
the sample time k + 1, ..., k + Np − 1, it can be easily derived that the difference between these two
functions is

V̄ − V ( xk ) = ykT+ Np yk+ Np − ykT+1 yk+1 − ∆ukT rw ∆uk . (37)

As is given in Equation (32), the optimization problem is subjected to the constraint yk+ Np = 0.
Then, it can be obtained that

V̄ − V ( xk ) ≤ −ykT+1 yk+1 − ∆ukT rw ∆uk . (38)

Then, the monotonicity of the Lyapunov function can be obtained by

V ( xk+1 ) − V ( xk ) ≤ −ykT+1 yk+1 − ∆ukT rw ∆uk < 0, (39)

which can prove the asymptotic stability of the system.

The model predictive control algorithm is realized by receding optimization. In order to apply
the MPC efficiently, we assume that the predicted outputs sequence is in a finite prediction horizon Np
and the inputs sequence is in a control horizon Nc , which is less than Np . The sequences mentioned
above can be expressed in the matrix form:
h iT
Y = y(k + 1|k) y(k + 2|k ) ... y(k + Np |k) ,
h iT (40)
∆U = ∆u(k ) ∆u(k + 1) ... ∆u(k + Nc − 1) ,

where y(k + n|k ) denotes the predicted outputs at time k + n based on the states at time k. Based on
Theorem 1 and the assumption above, the corollary about finite-time unconstrained MPC can be
obtained as follows.
Appl. Sci. 2018, 8, 1000 12 of 24

Corollary 1. Given the system without input and output constraints, the prediction and control horizon are
Np and Nc , respectively. Then, the following feedback control law ∆u(k) = Kx (k) can asymptotically stabilize
the closed-loop system, where

Nc
z }| { (41)
K = [1 0 ... 0] T (−Ξ[ Γ − (ΨΞ)−1 (CA Np + ΨΞΓ )]),

and,
T
CA Np −1 B
  
CA
 CA2   CA Np −2 B 
   
F =  . , Ψ = 
   ..  ,
 ..  .

 
Np Np − Nc
CA CA B
 
CB 0 ··· 0
 CAB
 CB ··· 0 
 (42)

CA 2B CAB · · · 0

Φ=  ,
..

 
 . 
N p −1 N p −2 Np − Nc
CA B CA B · · · CA B
Ξ = (Φ T Φ + R̄)−1 ,
Γ = Φ T F.

Proof of Corollary 1. According to Equations (31) and (40), the predicted output sequence Y can be
expressed by
Y = Fx (k ) + Φ∆U. (43)

Meanwhile, the final item of Y can be expressed as

y(k + Np |k) = CA Np x (k) + Ψ∆U. (44)

By substituting Equations (43) and (44) into Theorem 1, the optimization problem turns into the
following form:
arg min J = (Rs − Fx (k i ) − Φ∆U) T (Rs − Fx (k i ) − Φ∆U)
∆U
+ ∆UT R̄∆U, (45)
s.t. Ψ∆U + CA Np x (k i ) = 0,
where R̄ is a weighting matrix.
Note that in the application of offset model based path tracking, the target vector Rs should
be zero all the time. It can be seen that J meets an equality constrained quadratic programming.
Then, the objective function is expanded by Lagrange expression and simplified by omitting the
constant term,
J = 2∆UT Φ T Fx (k) + ∆UT (Φ T Φ + R̄)∆U
(46)
+ ξ T (Ψ∆U + CA Np x (k)),
where ξ is the Lagrange multiplier.
According to the Lagrange multiplier method, the optimal control input vector ∆U∗ can be found
by solving the following equation system. The solution is obtained by taking the first partial derivatives
of J with respect to the vectors ∆U and λ, and then equating these derivatives to zero:

∂J
= ∆UT (Φ T Φ + R̄) + Φ T Fx (k) + Ψ T ξ = 0,
∂∆U
(47)
∂J
= Ψ∆U + CA Np x (k) = 0.
∂ξ
Appl. Sci. 2018, 8, 1000 13 of 24

Then, its optimal solution can be obtained:

∆U∗ = −Ξ[ Γ − (ΨΞ)−1 (CA Np + ΨΞΓ )] x (k), (48)

where
Ξ = (Φ T Φ + R̄)−1 ,
Γ = Φ T F.
According to the receding horizon control principle, the first increment of ∆U∗ is applied as the
control inputs. Then, the control law in Equation (41) can be obtained.

It can be seen that Corollary 1 gives an offline solution of the MPC algorithm, which significantly
improves its computing efficiency. Based on Corollary 1, the desired accelerations A e] T can
e = [ aer , ael , γ
be obtained by integrating the optimal solution ∆u(k)∗ .

3.1.3. Sequential Quadratic Programming Based Control Allocation

Considering the vehicle body with the mass M and inertia J in this work, the command force
vector τ is defined as    
τl M 0 0
τ =  τr  =  0 M 0 A, (49)
    e
τγ 0 0 J
where τl , τr and τγ are the longitudinal and lateral forces and the moment about a vertical axis of the
vehicle, respectively.
According to the vehicle body dynamic model given in Equation (1), the actual actuating forces
come from the longitudinal forces FSl and lateral forces FSL on each wheel. Let the command forces τ
produced jointly by the wheel forces and steering angles be expressed as

τ ( FSl , FSL , δ) = Bu (δ) FSl + Bw (δ) FSL , (50)

where the i-th column of Bu (δ) and Bw (δ) can be written as

   
cos δi − sin δi
Bui (uδi ) =  sin δi ,
i
Bw (uδi ) =  cos δi .
   
− Lyi cos δi + L xi sin δi Lyi sin δi + L xi cos δi

( L xi , Lyi ) represents the location of each wheel in a coordinate system with its origin at the centre of
gravity and positive x-axis forward.
On a 4WS4WD vehicle, the drive forces FSl and the steering angles δ are the direct inputs while
the lateral forces FSL obtained from sensors are considered as a measured disturbance. In this work,
the steering angles δ are composed of δ f and δr , which represent the front and rear steering inputs,
respectively. Thus, the control problem is reduced to obtaining the feasible solution of Equation (50).
In order to facilitate the computation, a slack vector s is defined by

s = τ − Bu (δ) FSl + Bw (δ) F̂SL , (51)

which denotes the error between the commanded and actual generalized forces. The slack variable s
guarantees that there always exists a feasible solution in the following optimization [26].
Appl. Sci. 2018, 8, 1000 14 of 24

In order to solve this control allocation problem, the objective function is defined with respect to
FSl , δ and s,
T
J ( FSl , δ, s) = FSl Q f FSl + (δ − δ∗ ) T Qδ (δ − δ∗ ) + s T Qs s,
s.t. s = τ − Bu (δ) FSl + Bw (δ) F̂SL ,
s ∈ Bs , (52)
FSl ∈ BFSl ,
δ ∈ Bδ ,
where Bs , BFSl and Bδ are the search bounds of each variable.
In this function, the first term minimizes the magnitudes of the drive forces; the second term
is used to ensure the steering angle to search around its previous value. By penalizing the slack
variable s in the third term, the actual generalized force vector coincides as much as possible with the
commanded forces τ. The matrix Q f ∈ I 4×4 , Qδ ∈ I 2×2 and Qs ∈ I 3×3 are used to tune the objective.
The search bounds of all variables (i.e. FSl , δ and s) are specified by the constraints.
Based on the objective function presented above, the control allocation is converted to a nonlinear
constrained optimization problem. Using the sequential quadratic programming (SQP), the optimal
solution can be computed efficiently and reliably by standard numerical software.

3.2. PSO-Based Method

For the PSO-based method, an objective function including vehicle error states is proposed using
the sliding surfaces. In Sliding Mode Control (SMC), the time-varying sliding surface is normally
defined by the scalar equation s( x; t) = 0, in which s( x; t) is expressed by [27],

d
s( x; t) = ( + λ)n−1 xe, (53)
dt
where λ is a positive constant and xe is the error state vector.
According to the idea of SMC, the problem of maintaining xe = 0 is transformed into keeping s = 0.
In this work, the scalar quantities composed of vehicle error states are introduced into the definition of
objective function. Instead of designing the switching control law in SMC, the optimization is used to
maintain the scalar quantities on the sliding surface s = 0. The vehicle error state vectors including
p
g os, vel
f and a gcc are following the definitions in the offset model. Therefore, to track the trajectories
of the RP, the vector s = [sl , sr , s a ] needs to be defined correspondingly. Note that vector s follows
a different definition than that in SQP. It is defined to facilitate the notations in the local boundary part.
For p
g os presented in Equation (17), its first component, which represents the position error in the
longitudinal direction, is always zero. Therefore, according to Equation (53), the longitudinal scalar
quantity sl can be obtained as
sl = alerr + λl Vlerr , (54)

where alerr and Vlerr are the longitudinal components of a

g cc and vel,
f which are given in Equations (22)
and (19), respectively.
To maintain the vehicle on the RP geometrically, another first order scalar quantity sr is designed as

sr = Vrerr + λr los , (55)

which aims to minimize the offset errors los and Vrerr .

The angular acceleration error γ
e is included to consider the effects of forces and yaw movements
of vehicle, and thus a second order scalar quantity is chosen as

s a = γerr + 2λ a Ωerr + λ a 2 θos , (56)

where θos , Ωerr and γerr are given in Equations (17), (19) and (22), respectively.
Appl. Sci. 2018, 8, 1000 15 of 24

Then, the problem of following the trajectories of RP is transformed into maintaining s at 0. Using
the linear scalarization, an objective function is defined as

Jmin ( FSl , δ) = Cl |sl | + Cr |sr | + Ca |s a | , (57)

where Cl , Cr and Ca are the weighting coefficients strictly positive and constrained by

Cl + Cr + Ca = 1. (58)

The variables of objective function in Equation (57) consist of the forces FSli , steering angles δ f
and δr , which can be written as a variable vector,
h iT
vobj = δ f δr FSl1 FSl2 FSl3 FSl4 . (59)

First invented by Kennedy and Eberhart (1995), PSO has been successfully applied to solve
problems featuring nonlinearity, non-differentiable, and multiple optima. PSO is found to be capable of
generating high quality solutions with more stable and faster convergence characteristics, and shorter
calculation times than other stochastic methods [17]. For standard PSO at time t, the updating velocity
vi (t) and position xi (t) of the i-th particle are presented in the following equations:

vi (t + 1) = ζvi (t) + φ1 η1 ( pb − xi (t)) + φ2 η2 ( gb − xi (t)),

(60)
xi (t + 1) = xi (t) + vi (t + 1),

where vi (t) and xi (t) are vectors in multi-dimensional space. pb and gb denote the local optimal
position and the global optimal position, respectively. The particle inertia weight is represented by ζ.
The particle cognitive acceleration and social acceleration are denoted by φ1 and φ2 , which are defined
as positive constants. η1 and η2 are two stochastic parameters within [0 1].
The search space of PSO in this work is defined as a six-dimensional space corresponding to the
dimension of vobj . Therefore, the particle position vector xi (i ) in Equation (60) represents a possible
solution of the objective function in Equation (57).

3.3. Boundary Definition

Given that both methods are reduced to the optimization problems, the definition of the search
space determines the quality of the solution.

3.3.1. Global Boundaries

In this work, the global boundaries can be assigned based on the properties of each actuator,
which is defined as 
 δ ≤ δ f ≤ δmax ,
 min


Bg = δmin ≤ δr ≤ δmax , (61)


F ≤ F li ≤ F ,

d min S d max

where δmin and δmax are the minimum and maximum steering angles of each wheel. Fd min and Fd max
are the minimum and maximum drive forces provided by the driving unit, which can be calculated by

Tmax Tmax
Fd min = − , Fd max = , (62)
Rw Rw

where Tmax is the maximum torque that can be achieved by the driving unit of the vehicle.
Appl. Sci. 2018, 8, 1000 16 of 24

3.3.2. Local Boundaries

To realize real-time optimization, the computing time obtaining optimal values for variables
[ FSl , δ] needs to be constrained within the sample time of controller Ts . According to the properties of
each actuator, the maximum variations of [ FSl , δ] within Ts can be obtained. In this work, to improve
the computing efficiency, the local boundaries are also determined by the states of s.
According to ar in Equation (1), when sl < 0, the forces FSli need to be increased, thereby dragging
sl in Equation (54) towards the surface sl = 0. Similarly, when sl > 0, the forces FSli need to be
decreased. Therefore, the local boundaries of FSli can be written as

 F (t) ≤ F (t + T ) ≤ F (t) + ∆F sl < 0,
Sli Sli s Sli d max ,
BPFSli = (63)
 F (t) − ∆F ≤ F (t + Ts ) ≤ F (t), s ≥ 0,
Sli d max Sli Sli l

where ∆Fd max is the maximum change of FSli that can be achieved within Ts . At the time t + Ts ,
FSli (t + Ts ) are the possible optimal solutions.
For the steering angles δ f and δr in vobj , they affect the vehicle lateral and angular motions in a
coupled way. To solve this problem, an allocating method is specified that the vehicle lateral error
determines searching direction of δ f , while δr is related to vehicle angular error. When sr < 0, δ f needs
to be increased to drive sr back to the surface sr = 0, and vice versa. Thus, the local boundary of δ f is
summarized as, 
δ (t) ≤ δ (t + T ) ≤ δ (t) + ∆δ , s < 0,
f f s f max r
BδPf = (64)
δ (t) − ∆δ
f max ≤ δ f ( t + Ts ) ≤ δ f ( t ), sr ≥ 0,
where ∆δmax is the maximum change of steering angle that can be achieved within Ts . At the time
t + Ts , the possible optimal solution of front steering angle is δ f (t + Ts ).
Similarly, based on sr , the local boundary of δr is defined as

δ (t) − ∆δ
r max ≤ δr ( t + Ts ) ≤ δr ( t ), s a < 0,
BδPr = (65)
δ (t) ≤ δ (t + T ) ≤ δ (t) + ∆δ , s ≥ 0.
r r s r max a

For the PSO in particular, its particle velocity boundaries define the range of speed that particles
can achieve to search for the optimal solution. To improve search performance in PSO, the absolute
maximum particle velocity is normally set as a certain percentage of particle position range [28].
According to Equations (63)–(65), the particle velocity boundaries can be obtained as
h i
BV
FSli = σF − ∆Fd max ∆Fd max ,
h i
V
Bδ f = σδ f −∆δmax ∆δmax , (66)
h i
BV
δr = σδr − ∆δ max ∆δmax ,

where σF , σδ f and σδr are the particle velocity coefficients for FSli , δ f and δr , respectively.

4. Simulation

4.1. Simulation Setup

The working process of the vehicle control system is illustrated in Figure 6. The dashed box at
the top shows the operation of the vehicle dynamic model. Based on the actual inputs (i.e., Ti and δi )
as well as the vehicle velocity states vel, the tire model can generate the wheel forces, which is then
used in driving unit model. The driving unit model considers the wheel inertia and gives the actual
forces acting on the vehicle body. The dynamic states of the vehicle such as positions and velocities
are simulated by running the proposed vehicle body dynamic model in Matlab (R2017a, MathWorks,
Natick, MA, United States) using a Runge–Kutta method based solver with a time step of 8.33 µs.
Appl. Sci. 2018, 8, 1000 17 of 24

External Terrain
Disturbances Stiffness Profiles

Ti T
Vehicle FASi [FSli FSLi]
δf δr
Dynamic Model
T T
Pos [x y θ] Vel [Vl Vr Ω]
GPS Velocity
Noise Noise Force
Noise

^ ^
FSi [FSli F^SLi]
Kalman Filter Kalman Filter T

^ ^ ^ ^ ^ T
Pos [x^ y^ θ^]
T
Vel [Vl Vr Ω] ^ ^T
Trajectories of RP: Acc [a^l a^r γ]
Error States: ^
Acc [a^l a^r ^γ]
T
Posp [xp yp θp]T ~ ~ ~
Pos Vel Acc
Velp [Vlp Vrp Ωp]T Obtain States:
Accp [alp arp γp]T sl sr sa

Obtain Boundaries: ^
δf δr P P P
FSli
BF Sli
Bδf Bδr

BF
V V
Bδf Bδr
V
^
Sli FSLi

Offset Model
δf δr
Steering Controller

FSli Control
Torque Controller
Allocation
MPC

Main Controller

Figure 6. Flowchart of the vehicle control system.

In the simulation, both external disturbances and state measurement noises are involved in
validating the robustness of the proposed control methodology. The measured states are compared
with the reference profiles to generate the error states. Using the method proposed in Section 3.3,
the search boundaries of the drive forces and steering angles are obtained. Meanwhile, the error states
are transfered to the offset model. Virtual inputs calculated by the MPC algorithm are delivered to the
control allocation modual. Then, the actual control inputs including drive forces and steering angles
are generated and substituted into the dynamic model for the next iteration. The dashed box at the
bottom represents the main controller. PSO controller can be substituted in place of the MPC controller.
The PSO and MPC controllers are applied to drive the simulated vehicle to track the reference
path shown in Figure 7a. In the simulation, the varying terrain conditions are considered to verify the
validity and robustness of each controller. As presented in Figure 7b, the reference path is divided
into ten sections, which have different tire stiffnesses k li and k Li acting on each wheel. To simulate
the terrain disturbances, the disturbances Fgi applied to all four wheels were always in the vehicle
longitudinal direction. As shown in Figure 8, the disturbances are modeled as step signals and their
magnitudes are generated randomly to show the uncertainties.
Appl. Sci. 2018, 8, 1000 18 of 24

As a comparison, a kinematic model based control method is applied to drive the same vehicle.
According to relative kinematic path tracking research [8,19], the side slip is the main disturbance that
leads to the unpredicted tracking errors. Using the dynamic model based observer, the side slip of the
vehicle can be predicted with an error of 10% to 30%. In this simulation, a random side slip velocity
that is less than 10% of its longitudinal velocity is added in the kinematic model.

15 RP 1
SEC1
C8 Start point
10 SE SEC Junction points 0.9 SEC2 SEC10
9
5 SEC 0.8 SEC3 SEC9
10
SEC7

0
SEC1
0.7 SEC4 SEC8
Y (m)

−5
SEC5 SEC7
−10 0.6
SEC6
2

Wheel 1 (kl1, kL1)

SEC

−15 0.5
SE

Wheel 2 (kl2, kL2)

C6

−20 SEC
5 SEC
3
0.4 Wheel 3 (kl3, kL3)
SEC4
−25 Wheel 4 (kl4, kL4)
0.3
−40 −30 −20 −10 0 10 0 250 500 750 1000 1250 1500 1750 2000 2250
X (m) # of set-points
(a) (b)

Figure 7. Reference path (RP) and terrain coefficients along the RP. (a) Reference path; (b) terrain
coefficients.

20
Fg1
Fg2
15
Fg3
Terrain disturbance (N)

Fg4
10

−5
0 250 500 750 1000 1250 1500 1750 2000 2250
# of set-points

Figure 8. Terrain disturbance.

4.2. Parameters of Simulation

The first two sections of Table 1 list the parameters of vehicles used in simulation. The common
parameters in the objective functions are listed in the third section of Table 1, in which the vehicle mass
and inertia values used in the objective function are different from actual vehicle values to simulate
parametric uncertainties. This helps to verify the robustness of the vehicle control method in dealing
with variations in the system. In order to run in real time, both optimization methods are limited
to 15 ms of search time per iteration, less than the system sample time Ts = 20 ms. In optimization,
the optimal solution is searched within the boundaries until time reaches TI or the error gradient of
0.001 is achieved by the objective function. The last two sections present the particular parameters for
each method.
Appl. Sci. 2018, 8, 1000 19 of 24

Table 1. Parameters used in the simulation.

Para Value Unit Para Value Unit

Vehicle model
M 200.0 kg md 15.0 kg
Lf 0.85 m Lr 0.85 m
Lh 0.5 m Rw 0.25 m
Jz 45.0 kg·m2 Jw 0.8 kg·m2
System constraints
Fd max 250.0 N δmax 40.0 ◦
Fd min −250.0 N δmin −40.0 ◦
∆Fd max 0.8 N ∆δmax 0.35 ◦

Common parameters in objective function

Mc 205.0 kg Jzc 40.0 kg·m2
λl 2.0 - λr 1.5 -
λa 2.8 - TI 15 ms
MPC and SQP parameters
R̄ 0.1 × I 3×3 - Qf 1e−4 × I 4×4 -
Qδ 0.1 × I 2 × 2 - Qs I 3×3 -
PSO parameters
SS 24 - ζ 0.9 -
φ1 1.85 - φ2 1.85 -
σF 0.4 - σδ f 0.5 -
σδr 0.5 - Cl 0.35 -
Cr 0.35 - Ca 0.3 -
I i×i represents the i-th-order identity matrix. MPC: model predictive control; SQP: sequential quadratic
programming; PSO: particle swarm optimization.

4.3. Simulation Results

Figure 9 shows path tracking results by different controllers. The MPC-SQP controller has superior
performance with offset error less than 3.2 cm, and heading error less than 2◦ . The PSO controller has
maximum offset error of 6.1 cm and heading error of 3.2◦ . Both dynamic model based control methods
perform better than the kinematic model based one in constraining the offset errors.

0.15
MPC-SQP PSO Kinematic
RP
MPC-SQP path 0.1
Offset error (m)

PSO path
Kinematic path 0.05
10 Start point
0

-0.05
0 5 10 15 20 25 30 35 40 45
Y (m)

Time (s)
MPC-SQP PSO Kinematic
Heading error (degree)

-10 5

-5

0 5 10 15 20 25 30 35 40 45
-40 -30 -20 -10 0 10
X (m) Time (s)

Figure 9. Path following performance.

Appl. Sci. 2018, 8, 1000 20 of 24

The drive torques and steering angles applied on four wheels are presented in Figure 10. At each
cornering, the torques applied at the two outside wheels (i.e., T1 and T4 ) firstly increase and then
decrease, which are contrary to that of the two inside wheels (i.e., T2 and T3 ). The difference in torques
is used to compensate the insufficient angular accelerations of vehicle in corners. The steering angles
of MPC-SQP and PSO controller follow the same varying trend in which steering curves of MPC-SQP
show smoother variation. Thus, the MPC-SQP controller can provide more stable steerings compared
with the PSO controller.

Left-front wheel torque (T1) Right-front wheel torque (T2) Front wheel steering (δ f)
15
30 MPC-SQP 30 MPC-SQP MPC-SQP
10

Drive torque (Nm)

Drive torque (Nm)

PSO PSO PSO

Angle (degree)
20 20 5
0
10 10 -5

0 -10
0
-15
0 5 10 15 20 25 30 35 40 45 0 5 10 15 20 25 30 35 40 45 0 5 10 15 20 25 30 35 40 45
Time (s ) Time (s ) Time (s )

Left-rear wheel torque(T4) Left-rear wheel torque (T3) Rear wheel steering (δ r)
15
30 30
MPC-SQP MPC-SQP 10
Drive torque (Nm)
Drive torque (Nm)

Angle (degree)
PSO PSO 5
20 20
0
10 10 -5 MPC-SQP
0
-10 PSO
0
-15
0 5 10 15 20 25 30 35 40 45 0 5 10 15 20 25 30 35 40 45 0 5 10 15 20 25 30 35 40 45
Time (s ) Time (s ) Time (s )

Figure 10. Actual inputs.

In this work, another aim of control is to maintain high vehicle motion quality including its
velocity and acceleration performances. Figure 11a presents the longitudinal velocity curves achieved
by MPC-SQP and PSO controllers. Starting at 0.5 m/s, the two curves reach 3 m/s at 6.5 s and 4.8 s ,
respectively. Thus, the trajectory and error curves of the MPC controller responses are slower than the
PSO controller in Figure 9. Both controllers are capable of maintaining the longitudinal velocity around
3 m/s in the rest process. In Figure 11b, the longitudinal acceleration curve of PSO increases faster
than MPC-SQP and has a peak value of 1 m/s2 . The curve of MPC-SQP controller reaches 0.6 m/s2 at
a maximum and has smoother variation during the whole process.
From the offset model, lateral velocity is undesirable as it causes tracking error. As shown in
Figure 11c, both controllers maintain the lateral velocities fluctuating around zero. The MPC-SQP
controller has a maximum lateral velocity of 0.08 m/s, whereas PSO has a maximum value of
0.17 m/s. The lateral velocities are well constrained, which provides higher accuracy of path tracking.
The lateral acceleration curves follow the same varying trend in Figure 11d. It can be seen the PSO
controller causes a lateral acceleration oscillation of 0.6 m/s2 relative to the MPC-SQP. The smoother
change of lateral acceleration achieved by MPC-SQP controller reduces the jerk effect, decreasing the
deviation from the reference path. The angular velocities are demonstrated in Figure 11e, in which the
MPC-SQP has higher accuracy and a smoother manner, compared with that of the PSO. In Figure 11f,
the angular acceleration curves are presented. MPC-SQP and PSO curves vary with oscillations of
12 °/s2 and 20 °/s2 , respectively. The undesirable oscillations are eliminated in the process of the
integration, which can be demonstrated in Figure 11e. From all the acceleration plots given in Figure 11,
the MPC-SQP controller performs better in constraining oscillations of accelerations, which provides
better stability in vehicle motion control.
Considering that the optimization based method may lead to an expensive computation,
it is essential to analyze the computing efficiency for each controller. As shown in Figure 12,
the computing efficiency of each controller is compared. Figure 12a shows the computing time
used by the controllers in each sampling time Ts . It can be seen that both controllers achieve finishing
computing within Ts , which validates the feasibility of proposed controllers. In the box plot in
Appl. Sci. 2018, 8, 1000 21 of 24

Figure 12b, both controllers have an average computing time of 8 ms. The PSO controller gives a stable
variation range from 4 ms to 10 ms, as the particle velocity vector is decided by the limitation of the
sampling time, which guarantees relatively high quality solutions within a short time. The computing
time of MPC-SQP controller substantially increases and reaches 15 ms to 20 ms during each cornering
because the control allocation may encounter a complex optimization problem when the lateral forces
start to vary. It indicates that the PSO controller performs better in computing efficiency.

4
MPC-SQP PSO
MPC-SQP PSO
Longitudinal Velocity (m/s)

0
0 5 10 15 20 25 30 35 40 45
Time (s)

(a) (b)

0.2 2
MPC-SQP PSO
Lateral Acceleration (m/s 2 )

1
Lateral Velocity (m/s)

0.1
0
0
-1

-0.1
-2
MPC-SQP PSO
-0.2 -3
0 5 10 15 20 25 30 35 40 45 0 5 10 15 20 25 30 35 40 45
Time (s) Time (s)

(c) (d)

40
)

MPC-SQP PSO
Angular Velocity (degree/s)

20

-20
MPC-SQP
PSO
-40
0 5 10 15 20 25 30 35 40 45
Time (s)

(e) (f)

Figure 11. Acceleration and velocity performances. (a) longitudinal velocity; (b) longitudinal
acceleration; (c) lateral velocity; (d) lateral acceleration; (e) angular velocity; (f) angular acceleration.

0.03
MPC-SQP
Computing Time (s)

Computing Time (s)

PSO 0.02
0.02

0.01
0.01

0 0
0 5 10 15 20 25 30 35 40 45 MPC-SQP PSO
Time (s)
(a) (b)

Figure 12. Computing efficiency comparison. (a) computing time; (b) box plot of computing time.

5. Discussion
Based on the theoretical analysis and simulation results provided above, the discussion between
MPC based method and PSO method are given as follows:
Appl. Sci. 2018, 8, 1000 22 of 24

D1 Comparing the simulations, it is obvious that dynamic-based methods proposed in this paper
perform better than kinematic model based methods. In the kinematic model based methods,
it is difficult to measure the side slip directly. The majority of research works are trying to design
observers to predict its side slip. However, this kind of method can only get an approximate
estimate. Thus, it is not feasible to use a kinematic model based controller to completely
eliminate the error due to side slip. Both MPC-SQP and PSO methods are proposed based on
dynamic models and controlled by drive forces. In the dynamic model, the lateral forces can be
obtained easily using force sensors. This kind of method gives a practical way to avoid the side
slip estimation and achieve precise tracking along curved paths.
D2 Both MPC-SQP and PSO methods are proposed partly or completely based on optimizations.
As is compared in the simulation, the PSO controller achieves obtaining the solution with a stable
computing time. The algorithm can optimize the quality of solutions and the calculation times
at the same time, which guarantees the feasibility and capability of the controller. The MPC-SQP
controller may encounter the calculation timeout in the simulation results. This is because the
SQP solver spends more time when calculating a complex Hessian matrix. Thus, it is necessary
to set a sufficient sampling time when applying the MPC-SQP method.
D3 The MPC-SQP method gives a stability proof of the control system while the PSO method
has difficulty with the mathematical proof due to the limitation of intelligent optimization.
The stability analysis of MPC-SQP method provides a possibility to analyze the stable range of
its control parameters and margin of errors.
D4 Both control algorithms are reduced to constrained optimization problems with six input
variables. PSO is a global algorithm that performs better with searching for a global optimal
solution while SQP is a reliable solver but may be held in local optimal solutions. Thus, for each
method, it is important to define a proper search range. In this paper, the boundaries
are calculated based on the vector s, which improves the efficiency and stability of the
optimization process.

6. Conclusions
This paper presented two control methodologies applicable to four wheel steering and four wheel
drive vehicle systems to track paths accurately. The system model is a nonlinear coupled dynamic
model that is over-actuated. The first methodology determines the drive force inputs and steer inputs
using an MPC based method coupled with a control allocation based on SQP. The second methodology
is proposed as a fully optimization based method that is developed by refining a previously proposed
PSO based method. The performance of the MPC-SQP method has been compared with the PSO based
control method and a kinematic model based control method. In the simulation, the path tracking
results have proved the superior performance of the MPC-SQP based controller. In the cases when
computing times are considered, the PSO based controller offers more efficiency and better stability in
computing compared with MPC-SQP based controller.

Author Contributions: Conceptualization, Q.T. and J.K.; Methodology, Q.T. and P.D.; Writing—Original Draft
Preparation, Q.T. and Z.Z.; Writing—Review & Editing, J.K.; Supervision, J.K.; Project Administration, J.K.;
Funding Acquisition, Q.T.
Funding: This research was funded by the Fundamental Research Funds for the Central Universities, Grant No.
2017JBM051 and 2015JBC022, and the China Postdoctoral Science Foundation, Grant No. 2016M600910.
Acknowledgments: This work was carried out at the School of Mechanical and Manufacturing Engineering,
the University of New South Wales, Sydney, Australia.
Conflicts of Interest: The authors declare no conflict of interest. The founding sponsors had no role in the writing
of the manuscript.
Appl. Sci. 2018, 8, 1000 23 of 24

References
1. Oppenheimer, M.W.; Doman, D.B.; Bolender, M.A. Control Allocation for Over-actuated Systems.
In Proceedings of the 14th Mediterranean Conference on Control and Automation, Ancona, Italy,
28–30 June 2006; pp. 1–6.
2. Ackermann, J.; Sienel, W. Robust yaw damping of cars with front and rear wheel steering. IEEE Trans.
Control Syst. Technol. 1993, 1, 15–20. [CrossRef]
3. Itoh, H.; Oida, A.; Yamazaki, M. Numerical simulation of a 4WD-4WS tractor turning in a rice field.
J. Terramech. 1999, 36, 91–115. [CrossRef]
4. Wu, J.; Wang, Q.; Wei, X.; Tang, H. Studies on improving vehicle handling and lane keeping performance of
closed-loop driver-vehicle system with integrated chassis control. Math. Comput. Simul. 2010, 80, 2297–2308.
[CrossRef]
5. Wang, W.; Song, Y. A new high dimension nonlinear dynamics simulation model for four-wheel-steering
vehicle. J. Mech. Eng. Sci. 2013, 227, 29–37. [CrossRef]
6. Udengaard, M.; Iagnemma, K. Kinematic analysis and control of an omnidirectional mobile robot in rough
terrain. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS,
San Diego, CA, USA, 29 October–2 November 2007; pp. 795–800.
7. Grand, C.; Amar, F.B.; Plumet, F. Motion Kinematic analysis of wheeled-legged rover over 3D surface with
posture adaptation. Mech. Mach. Theory 2010, 45, 477–495. [CrossRef]
8. Han, G.; Fu, W.; Wang, W.; Wu, Z. The Lateral Tracking Control for the Intelligent Vehicle Based on Adaptive
PID Neural Network. Sensors 2017, 17, 1244. [CrossRef] [PubMed]
9. Qian, H.; Lam, T.; Li, W.; Xia, C.; Xu, Y. System and design of an Omni-directional vehicle.
In Proceedings of the IEEE International Conference on Robotics and Biomimetics (ROBIO), Bangkok,
Thailand, 22–25 February 2009; pp. 389–394.
10. Grepl, R.; Vejlupek, J.; Lambersky, V.; Jasansky, M.; Vadlejch, F.; Coupek, P. Development of 4WS/4WD
Experimental Vehicle: Platform for research and education in mechatronics. In Proceedings of the IEEE
International Conference on Mechatronics (ICM), Istanbul, Turkey, 13–15 April 2011; pp. 893–898.
11. Ramaswamy, S.A.P.; Balakrishnan, S.N. Formation control of car-like mobile robots: A Lyapunov function
based approach. In Proceedings of the American Control Conference, Seattle, WA, USA, 11–13 June 2008;
pp. 657–662.
12. Camacho, E.; Alba, C. Model Predictive Control; Advanced Textbooks in Control and Signal Processing;
Springer: London, UK, 2013.
13. Lenain, R.; Thuilot, B.; Cariou, C.; Martinet, P. Model predictive control for vehicle guidance in presence of
sliding: Application to farm vehicles path tracking. In Proceedings of the 2005 IEEE International Conference
on Robotics and Automation (ICRA 2005), Barcelona, Spain, 18–22 April 2005; IEEE: Piscataway, NJ, USA,
2005; pp. 885–890.
14. Lenain, R.; Thuilot, B.; Cariou, C.; Martinet, P. High accuracy path tracking for vehicles in presence of
sliding: Application to farm vehicle automatic guidance for agricultural tasks. Auton. Robots 2006, 21, 79–97.
[CrossRef]
15. Prasetya, D.A.; Yasuno, T. Cooperative control of multiple mobile robot using particle swarm optimization
for tracking two passive target. In Proceedings of the SICE Annual Conference (SICE), Akita, Japan,
20–23 August 2012; pp. 1751–1754.
16. Lin, L.; Sun, Q.; Wang, S.; Yang, F. Research on PSO based multiple UAVs real-time task assignment.
In Proceedings of the Chinese Control and Decision Conference (CCDC), Guiyang, China, 25–27 May 2013;
pp. 1530–1536.
17. Thomas, J. Integrating Particle Swarm Optimization with Analytical Nonlinear Model Predictive Control
for nonlinear hybrid systems. In Proceedings of the International Conference on Informatics in Control,
Automation and Robotics (ICINCO), Colmar, France, 21–23 July 2015; pp. 294–301.
18. Dai, P.; Katupitiya, J. Force control for path following of a 4WS4WD vehicle by the integration of PSO and
SMC. Veh. Syst. Dyn. 2018, 1–35. [CrossRef]
19. Wang, X.; Taghia, J.; Katupitiya, J. Robust Model Predictive Control for Path Tracking of a Tracked Vehicle
with a Steerable Trailer in the Presence of Slip. IFAC-PapersOnLine 2016, 49, 469–474. [CrossRef]
20. Rajamani, R. Vehicle Dynamics and Control; Springer Science: Berlin, Germany, 2006.
Appl. Sci. 2018, 8, 1000 24 of 24

21. Smith, D.; Starkey, J. Effects of Model Complexity on the Performance of Automated Vehicle Steering
Controllers: Model Development, Validation and Comparison. Veh. Syst. Dyn. 1995, 24, 163–181. [CrossRef]
22. Brach, R.; Brach, R. Tire Models for Vehicle Dynamic Simulation and Accident Reconstruction; SAE Technical
Paper; Society of Automotive Engineers: Warrendale, PA, USA, 2009; Volume 1.
23. Jazar, R.N. Vehicle Dynamics: Theory and Applications; Springer: New York, NY, USA; London, UK, 2008.
24. Milliken, W.F.; Milliken, D.L. Race Car Vehicle Dynamics; Society of Automotive Engineers: Warrendale,
PA, USA, 1995.
25. Dai, P.; Katupitiya, J. Force Control of a 4WS4WD Vehicle for Path Tracking. In Proceedings of the IEEE
International Conference on Advanced Intelligent Mechatronics (AIM), Busan, Korea, 7–11 July 2015;
pp. 238–243.
26. Johansen, T.A.; Fossen, T.I. Control allocation—A survey. Automatica 2013, 49, 1087–1103. [CrossRef]
27. Slotine, J.J.E.; Li, W. Applied Nonlinear Control; Prentice Hall: Upper Saddle River, NJ, USA, 1991.
28. Eberhart, R.C.; Shi, Y. Particle swarm optimization: Developments, applications and resources. Evol. Comput.
2001, 1, 81–86.

© 2018 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 (http://creativecommons.org/licenses/by/4.0/).