DF

Trajectory Planning for Automated
Driving of Articulated Heavy Vehicles
Using model predictive control

Master’s thesis in Systems, Control and Mechatronics

JULIUS DIESTRA

SPYRIDON SKOURAS

Department of Electrical Engineering
CHALMERS UNIVERSITY OF TECHNOLOGY
Gothenburg, Sweden 2019





Master’s thesis 2019:NN

Trajectory Planning for Automated
Driving of Articulated Heavy Vehicles

Using model predictive control

JULIUS DIESTRA SPYRIDON SKOURAS

DF

Department of Electrical Engineering
Division of Systems and Control

Chalmers University of Technology
Gothenburg, Sweden 2019



Trajectory Planning for Automated Driving of Articulated Heavy Vehicles
Using model predictive control
JULIUS DIESTRA SPYRIDON SKOURAS

© JULIUS DIESTRA, SPYRIDON SKOURAS, 2019.

Supervisor: Peter Nilsson and Patrik Nilsson, Volvo AB
Examiner: Jonas Fredriksson, Chalmers, Systems and Control

Master’s Thesis 2019:NN
Department of Electrical Engineering
Division of Systems and Control
Chalmers University of Technology
SE-412 96 Gothenburg
Telephone +46 31 772 1000

Typeset in LATEX, template by David Frisk
Printed by Chalmers Reproservice
Gothenburg, Sweden 2019

iv



Trajectory Planning for Automated Driving of Articulated Heavy Vehicles
Using model predictive control
JULIUS DIESTRA SPYRIDON SKOURAS
Department of Electrical Engineering
Chalmers University of Technology

Abstract
Automotive industries have shown great interest in designing autonomous heavy ve-
hicles in the last decades. Hence, the role of the driver will belong to the past in the
close future and control functions will handle their tasks. However, the development
of control functions for maneuvering is a challenging area of research due to the size
and motion characteristics of the heavy vehicles.

This project is focused on designing trajectory planners for a truck with a semi-
trailer. Since the aim of the planners is to make possible for the vehicle to cross
sharp corners and roundabouts, the design is restricted in low velocities. The gener-
ated trajectories satisfy also some additional constraints, which are related to good
performance and safety.

The method used for the generation of such trajectories is based on numerical opti-
mization and more specifically model predictive control. The selection of the method
can be explained by the fact that it solves optimization problems considering con-
straints. The software tool used to solve the optimization problems is Forces Pro.

In this study, three different trajectory planners are developed and tested. Each
planner is based on different equations describing the motion of the vehicle such
as the kinematics and dynamics. For all of the planners, the prediction horizon is
selected to be 8 seconds. The trajectory planners are also tested with a high fidelity
model (provided by Volvo GTT) in a real-time simulation environment.

Lastly, it can be observed from the results that all planners manage to generate
smooth and safe trajectories, making it possible at the same time for the truck to
cross two sharp corners and a roundabout in a satisfying manner.

Keywords: Model predictive control, trajectory planning, semi-trailer combination,
optimization, modelling, Matlab/Simulink, Forces Pro.

v





Acknowledgements

We would like to thank our supervisors from Volvo GTT Peter Nilsson and Patrik
Nilsson for supporting us during the conduction of this master thesis. Through
their experience and knowledge, they helped us overcome obstacles and learn from
them. Lastly, we would like to thank our supervisor from Chalmers University of
Technology Jonas Fredriksson, who was always willing to help our work and
provided us with useful advice.
Gothenburg, June 2019 Spyridon Skouras
Chalmers University of Technology Julius Diestra

vii





Contents

List of Figures xi

List of Tables xiii

1 Introduction 1
1.1 Objective . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2 Prerequisites . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.3 Limitations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.4 Outline of the thesis . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2 Background 5
2.1 Definitions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2.2 Trajectory planning techniques . . . . . . . . . . . . . . . . . . . . . 5
2.3 Trajectory planning for articulated heavy vehicles . . . . . . . . . . . 6
2.4 Motion characteristics of articulated vehicles . . . . . . . . . . . . . . 7
2.5 Optimization problem . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.6 State-space model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.7 Model predictive control . . . . . . . . . . . . . . . . . . . . . . . . . 10

3 Modelling 13
3.1 Road modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
3.2 Vehicle motion modeling . . . . . . . . . . . . . . . . . . . . . . . . . 14

3.2.1 System description . . . . . . . . . . . . . . . . . . . . . . . . 14
3.2.2 Rotation matrices . . . . . . . . . . . . . . . . . . . . . . . . . 15
3.2.3 Euler-Lagrange equation . . . . . . . . . . . . . . . . . . . . . 16

3.2.3.1 Generalized coordinates . . . . . . . . . . . . . . . . 18
3.2.3.2 Kinetic energy . . . . . . . . . . . . . . . . . . . . . 19
3.2.3.3 Generalized forces . . . . . . . . . . . . . . . . . . . 19

3.3 Vehicle motion in road coordinates . . . . . . . . . . . . . . . . . . . 21
3.4 Nonlinear state-space model: Lateral and longitudinal dynamics . . . 25
3.5 Nonlinear state-space model: Only lateral dynamics . . . . . . . . . . 26
3.6 Linear model: Lateral and longitudinal dynamics . . . . . . . . . . . 27
3.7 Linear model: Lateral dynamics . . . . . . . . . . . . . . . . . . . . . 28
3.8 Kinematic model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
3.9 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

4 Proposed trajectory planning system 33

ix



Contents

4.1 Longitudinal and lateral trajectory planner . . . . . . . . . . . . . . . 33
4.2 Lateral trajectory planner . . . . . . . . . . . . . . . . . . . . . . . . 35

5 Results 37
5.1 Cornering stiffness estimation . . . . . . . . . . . . . . . . . . . . . . 37
5.2 Model validation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

5.2.1 Model validation: Constant steering angle . . . . . . . . . . . 40
5.2.2 Model validation: Sinusoidal steering angle . . . . . . . . . . . 41

5.3 Trajectory planner: Simulations using Forces Pro solver and nonlinear
state-space model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

5.4 Trajectory planner: Real time simulations using Forces Pro solver
and HF model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47

6 Conclusions and Future work 53
6.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
6.2 Future work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

Bibliography 55

A Nomenclature I

x



List of Figures

1.1 Semi-trailer truck, reproduced from [1]. . . . . . . . . . . . . . . . . . 1
1.2 Schematic architecture of an automated vehicle, reproduced from [9]. 2

2.1 Truck with a semi trailer - Cornering Case - low speed, reproduced
from [20]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

2.2 Rearward amplification for an A-double combination, reproduced from
[22]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

2.3 Truck with a semi trailer - Cornering Case - high speed, reproduced
from [22]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

2.4 Model predictive control algorithm. . . . . . . . . . . . . . . . . . . . 11

3.1 Road model parameters. . . . . . . . . . . . . . . . . . . . . . . . . . 14
3.2 Semi-trailer combination [51]. . . . . . . . . . . . . . . . . . . . . . . 15
3.3 Two-track model with multiple non-steerable axles and dual tyres. . . 15
3.4 One-track model with multiple non-steerable axles and single tyres. . 15
3.5 One-track model of a semi-trailer combination. . . . . . . . . . . . . . 16
3.6 One-track model : Position vectors. . . . . . . . . . . . . . . . . . . . 17
3.7 One-track model : Forces. . . . . . . . . . . . . . . . . . . . . . . . . 18
3.8 Semi-trailer motion relative to the road. . . . . . . . . . . . . . . . . 22
3.9 Road and its parameters. . . . . . . . . . . . . . . . . . . . . . . . . . 23
3.10 Sigmoid function. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.11 A set of four subfigures. . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.12 Modeling procedure summary. . . . . . . . . . . . . . . . . . . . . . . 32

4.1 Diagram of the longitudinal and lateral trajectory planner in the system. 34
4.2 Diagram of the lateral trajectory planner in the system. . . . . . . . . 35

5.1 Cornering stiffness of unit 1. . . . . . . . . . . . . . . . . . . . . . . 38
5.2 Cornering stiffness of unit 2. . . . . . . . . . . . . . . . . . . . . . . 38
5.3 Model validation diagram. . . . . . . . . . . . . . . . . . . . . . . . . 39
5.4 Center of gravity of vehicle unit 1 and 2. Input δ̇11(t) = 0 and initial

condition δ110 = 5◦. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
5.5 Error estimation of the center of gravity of vehicle unit 1 and 2 by

the nonlinear and linear mode. Input δ̇11(t) = 0 and initial condition
δ110 = 5◦. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

5.6 Estimation of ψ1 and ∆ψ1. Input δ̇11(t) = 0 and initial condition
δ110 = 5◦. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

xi



List of Figures

5.7 Center of gravity of vehicle unit 1 and 2. Input δ̇11(t) = A·cos 2π · f · t
and initial condition δ110 = 0◦. A = 0.1053 rad/s and f = 0.08 Hz. . 42

5.8 Error estimation of the center of gravity of vehicle unit 1 and 2 by
the nonlinear and linear mode. Input δ̇11(t) = A · cos 2π · f · t and
initial condition δ110 = 0◦. A = 0.1053 rad/s and f = 0.08 Hz. . . . . 42

5.9 Estimation of ψ1 and ∆ψ1. Input δ̇11(t) = A · cos 2π · f · t and initial
condition δ110 = 0◦. A = 0.1053 rad/s and f = 0.08 Hz. . . . . . . . 42

5.10 A set of four subfigures. . . . . . . . . . . . . . . . . . . . . . . . . . 45
5.11 A set of four subfigures. . . . . . . . . . . . . . . . . . . . . . . . . . 46
5.12 Simulation using nonlinear model and Forces Pro: Planner solving

time. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
5.13 A set of four subfigures. . . . . . . . . . . . . . . . . . . . . . . . . . 48
5.14 A set of four subfigures. . . . . . . . . . . . . . . . . . . . . . . . . . 49
5.15 A set of four subfigures. . . . . . . . . . . . . . . . . . . . . . . . . . 50
5.16 Real time simulation using HF model, Forces Pro and ROS: Planner

solving time. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

xii



List of Tables

5.1 Cornering stiffness estimated parameters. . . . . . . . . . . . . . . . . 39
5.2 Values of Ts and T∆ for each planner. . . . . . . . . . . . . . . . . . . 43
5.3 Lateral trajectory planners: Tuning parameters and boundaries. . . . 43
5.4 Trajectory planner using linear longitudinal and lateral dynamics

model: Tuning parameters and boundaries. . . . . . . . . . . . . . . . 44

xiii



List of Tables

xiv



1
Introduction

During the last decades, automotive industries have shown a lot of interest in de-
veloping automated vehicles, see e.g. [2],[3],[4]. Even though, there is a lot of work
needed in order for autonomous vehicles to replace the existing cars and trucks,
experimental autonomous vehicles exist and are tested. The reason why automotive
industries tend to develop automated vehicles can be seen by several advantages,
some of them will be shortly mentioned below. According to [5], automated vehicles
could have a positive impact on traffic safety. More specifically human errors are
responsible for the highest percentage of car accidents [6]. Hence automated vehicles
aim at making transportation safer. Furthermore, automated vehicles could limit
the CO2 emissions. Truck platooning for aerodynamic drag reduction as well as
employing the most efficient driving speeds could result to less emissions and as a
result, automated vehicles could make transportation more environmental friendly
[7]. Automated vehicles could also impact traffic congestions. In [8], researchers
showed that autonomous vehicles managed to increase the traffic flow. This could
have a strong impact to people’s lives, as it would be much easier for people to
manage their schedules at both personal lives and work.

This master thesis is focused on an automated driving system for a tractor semi-
trailer combination, see Fig 1.1. The main goal is to design a trajectory planner
that will generate a safe trajectory and at the same time guarantee maneuverability
at low speeds. The planner is to be tested in simulation using a high - fidelity truck
model in simulink.

Figure 1.1: Semi-trailer truck, reproduced from [1].

1



1. Introduction

1.1 Objective
The main objective is to develop a trajectory planner, so that the vehicle combina-
tion can manoeuvre with respect to safety and efficiency. The trajectory planner
should be as fast as possible and make it possible for the vehicle combination, to
make sharp turns and negotiate roundabouts.

Model predictive control (MPC) method is selected for this purpose. In order to
accomplish the main objective, the steps below are needed:

• modeling of the truck with a semi-trailer,
• verification of the above model, by comparing it with the high-fidelity truck

model,
• development of a trajectory planner,
• trajectory planner test on the high-fidelity model.

1.2 Prerequisites
Trajectory planning is a one of the main functions for an automated vehicle Fig. 1.2.
Though in order to for the reader to obtain a general view of the total structure,
the rest of the the functionalities will be shortly explained. Firstly, the vehicle is
equipped with sensor systems which provide information regarding the positions,
velocities ,and orientations of the vehicle. Secondly, the prediction systems estimate
the trajectories of the surrounding vehicles, while the decision making functionality
is responsible for deciding when a manoeuvre should start. Then the trajectory
planner is responsible for the generation of a smooth and safe for the vehicle trajec-
tory. Lastly, the low level control systems are responsible for tracking the generated
trajectory.

Figure 1.2: Schematic architecture of an automated vehicle, reproduced from [9].

Though, in this research, several assumptions are made, as far as these functions
are concerned.

• All states information is available from already existing sensors and no further
investigation is done on this topic.

• No surrounding vehicles and obstacles are considered in this research. Hence
the traffic situation prediction is out of the scope of this thesis.

2



1. Introduction

• No investigation is done on decision making.
• Actuation controls such as propulsion force, steering angle, and steering angle

rate are assumed to be available for use.
• The road characteristics (curvature and angle of the road center line as well

as road width) are known.

1.3 Limitations
In this subsection, several limitations of the designed trajectory planner are briefly
presented.

• The following research is focused on a truck with semi-trailer. The trajectory
planner is designed specifically for this type of truck combination.

• The trajectory planner is designed to operate at low velocities (below 20km/h).
• The trajectory planner is designed for a specific route, provided by Volvo GTT.

1.4 Outline of the thesis
Chapter 2 In chapter 2, the background of this research is presented. Firstly sev-
eral trajectory planning algorithms are briefly presented. The trajectory planning
problem for automated heavy vehicles is described as well as previously researches
on this topic are mentioned. Lastly, the definitions of optimization problem and
state space representation of a model are explained.

Chapter 3 Chapter 3 presents the equations describing the dynamics and kine-
matics of the truck with a semi-trailer. More specifically, the nonlinear dynamic
model as well as a simplified version of the dynamic model is shown. Lastly, the
kinematics’s model equations are presented.

Chapter 4 In chapter 4, the proposed trajectory planning algorithm is shown. More
specifically, the selected optimization variables, cost function ,and constraints of the
optimization problem are presented.

Chapter 5 Chapter 5 shows verification tests of each model, as well as the simula-
tion results of the trajectory planner for each model. Lastly, simulation results with
the high fidelity model are also presented.

3



1. Introduction

4



2
Background

In this chapter a brief description of several trajectory planning techniques used
for automated heavy vehicles, are described. Also the characteristics of motion for
heavy vehicles are shortly presented. Lastly brief definitions of the optimization
problem, state space model and MPC are mentioned.

2.1 Definitions
According to the definitions, made in [1], a path is a geometric trace (including
orientation) that a specific position of the vehicle, say, the centre of the first or the
last vehicle axle, should follow whilst satisfying given constraints. Hence, the path
planning problem is to find a path in the configuration space that starts at the initial
configuration and reaches the target region whilst satisfying the given constraints.
On the other hand, state trajectory can be interpreted as a generalisation of a
path; prescribing the evolution of the states of a dynamic system in time. Finally,
trajectory planning (also known as trajectory generation or motion planning) is a
generalisation of path planning, involved with planning the state evolution in time
while satisfying given constraints on the states and possibly the actuation.

2.2 Trajectory planning techniques
Various techniques have been published for autonomous vehicles for trajectory plan-
ning problems. These techniques can be classified in four groups: graph search,
sampling, interpolating and numerical optimization [11].

• Graph search. The main goal of the graph search algorithms is to find the
best path from an initial point A to a final point B. For that purpose, the
state space is divided into grid cells. These algorithms visit every state in the
grid and decide if there is a path to be followed (not necessarily optimal) or
if there is no solution to the trajectory planning problem. The three follow-
ing algorithms for graph search have been implemented in automated driving:
Dijkstra algorithm, [12], A∗ algorithm, [13] and state lattice algorithm, [14].

• Sampling. In sampling based algorithms, the state space is randomly di-
vided into grid cells. A new cell is added into the graph if there is a path
which connects that cell with an already existed cell in the graph. Finally,
an optimal path is generated, connecting the start with the final point. The

5



2. Background

main algorithms use in SBP are Rapidly-Exploring Random Tree (RRT) and
Probabilistic Roadmap Method (PRM).

• Curve Interpolating. In general interpolation is the procedure in which
given a set of data, a new set of data is calculated and inserted inside the old
set (reference points). Interpolation curve planners generate smooth trajecto-
ries considering the vehicle constraints and changing surrounding environment.
More specifically these algorithms consider the previous way points of a global
road map and a new path is generated considering all limitations. Some of
the most popular interpolation methods are lines and circles, clothoid curves,
polynomial curves, spline curves and Bezier curves. Spline and clothoid meth-
ods and Bezier curves have been applied on automated driving, see e.g. [16]
and [17].

• Numerical Optimization. The main goal in this method is the minimization
of a cost function subject to several constraints of variables. Any specifications
of an optimization problem could be included into the constraints. The cost
function is weighted such that different variables of interest are penalized. In
trajectory planning problems, the solution of an optimization problem could be
path satisfying the desired requirements. Applications in automated driving
can be found in [18]. It is worth mentioning that MPC is based on solving an
optimization problem in each time instance.

2.3 Trajectory planning for articulated heavy ve-
hicles

A lot of interest has appeared on trajectory planning for automated heavy vehicles,
for both high-speed [22], [23] and [26] and low speed [25] scenarios, using MPC.
In each scenario different formulation of the problem is used. At high speeds, the
authors at [22], [23] and [26], are focused mostly on lane-changing. In these re-
searches, after simplifications a linear model describing the motion of the vehicle,
is used. An interesting approach is also the spatial formulation, which is used at
[23] and [26]. On the other hand, at low speeds scenarios, the kinematic model is
selected at [24] and [25], which shows good approximation in low velocities. The
latter formulation leads to a nonlinear state space model, but much simpler than
the nonlinear model describing the complete dynamics. The reason why different
models are used in each scenario, is the fact that the complete differential equations
describing the motion of the truck, are quite complex and contain many nonlinear
terms. Hence the goal of the authors is to approximate the equations of motion of
the heavy vehicle in each scenario with a simplified model. This goal has a strong
impact on the feasibility and speed of the MPC algorithm. As mentioned before,
the designed trajectory planner of this research is focused on maneuvering at low
velocities. Both the kinematic and the dynamic models are considered as it will
be seen at the next chapters. In a corner case (in which the truck maintains low
speed), the truck is expected to deviate from the center of the road, so that the
trailer will remain inside the road limits Fig. 2.1. Hence the trajectory planner is
expected to generate a similar trajectory, considering at the same time the mechan-

6



2. Background

Figure 2.1: Truck with a semi trailer - Cornering Case - low speed, reproduced
from [20].

ical limitations of the vehicle. Meanwhile the trajectory planners are designed for a
specific route (provided by Volvo GTT). Defining the route outside the road limits
as the obstacles and considering the above mentioned, the trajectory planning prob-
lem for automated heavy vehicles is described according to the trajectory planning
definition.

2.4 Motion characteristics of articulated vehicles

There are two lateral phenomena occurring due to the length of a heavy truck at lane
changing and cornering scenarios. These phenomena are the rearward amplification
and the lateral off-tracking. During a maneuver, the rearward amplification can be
observed, when the magnitude of the oscillation of the lateral acceleration of the last
unit of a heavy vehicle combination is larger than the lateral acceleration magnitude
of the truck, see illustration in Fig. 2.2. Hence, rearward amplification can lead
to roll over. Rearward amplification its more visible at high speed maneuvering.
Lateral off-tracking´can be seen by the different trajectories that the front axle and
the most rear axle follow when driving in a curve. At low velocities, the last unit
tends to move inwards from the curve Fig. 2.1, whereas at high velocities the most
rear position of the trailer, tends to move outwards Fig. 2.3.

7



2. Background

Figure 2.2: Rearward amplification for an A-double combination, reproduced from
[22].

Figure 2.3: Truck with a semi trailer - Cornering Case - high speed, reproduced
from [22].

2.5 Optimization problem
An optimization problem can be formulated as [27],

minimize
z

J(z)

subject to hi(z) = 0 for i ∈ E
gi(z) ≤ 0 for i ∈ I
z ∈ Z

(2.1)

z ∈ IRn : Decision variable vector zj, j = 1, 2, · · · , n;
J : IRn −→ IR⋃{±} : Objective function;
hi : IRn −→ IR : Equality constraint defining restriction on z;
gi : IRn −→ IR : Inequality constraint defining restriction on z;
Z ⊆ IRn : Set defined by the lower and upper bounds on z;
E = {1, ...,m} : Set of number of equality constraints;
I = {1, ..., l} : Set of number of inequality constraints.

8



2. Background

The feasible set (S ⊆ IRn) is defined as the solution set of all the constraints of the
system,

S = {z ∈ IRn
∣∣∣hi(z) = 0, i ∈ E , gi(z) ≤ 0, i ∈ I, z ∈ Z} (2.2)

A vector satisfying z ∈ S is called a feasible solution. The feasible solution z∗ which
gives the objective function its minimal value among S is called the optimal solution,
[28],

J(z∗) ≤ J(z) , z ∈ S (2.3)

The optimization problem (2.1) is denoted nonlinear when at least one of the func-
tions J , hi or gi is a nonlinear function. Moreover, when J , hi and gi are linear
functions, (2.1) is a linear optimization problem.

2.6 State-space model
A dynamic system can be described by the following differential equation [29],

ẋ(t) = f(x(t), u(t)) , x(0) = x0 (2.4)

where,

x ∈ IRn : State vector xj, j = 1, 2, · · · , n;
u ∈ IRp : Input vector uj, j = 1, 2, · · · , p;
f : IRn × IRp −→ IRn : Vector function fj, j = 1, 2, · · · , n;
x0 ∈ IRn : Initial state vector.

An equilibrium point (xe, ue) can be selected such that,

f(xe, ue) = 0 (2.5)

Then, the nonlinear system can be linearized around the equilibrium point,

ẋ(t) = Ax(t) +Bu(t) , x(0) = x0 (2.6)

where,

A = ∂f(x, u)
∂x

∣∣∣∣
(xe,ue)

, B = ∂f(x, u)
∂u

∣∣∣∣
(xe,ue)

(2.7)

The nonlinear and linear representations of the system can be sampled to obtain
their discrete-time version [30]. In this document, the formulation of the discrete-
time state-space model has the following form,

x(k + 1) = f(x(k), u(k)) (2.8)

where f can be a nonlinear or linear model. Moreover, the following notation will
be used,

xk+j = x(k + j) , j ∈ IN (2.9)

9



2. Background

2.7 Model predictive control
Model predictive control (MPC) uses the predicted behaviour of a system in a finite
horizon to compute a control input sequence that optimize a cost function subject
to constraints under the inputs and states of the system. The predicted behavior is
obtained by using the system model, see [32] and [33].

In order to formulate the MPC algorithm, the following matrices are defined,

ū =


uk
uk+1
...

uk+N−1

 (2.10)

x̄ =


xk
xk+1
...

xk+N

 (2.11)

where N is the horizon, ū is the control input sequence and x̄ is the state sequence.
The next step state sequence (x̄+) can be defined as,

x̄+ =


xk+1
xk+2
...

xk+N

 (2.12)

It can be observed that x̄+ can be created as a function of xk and ū using (2.8),

x̄+ =


xk+1
xk+2
...

xk+N

 =


f
(
xk, uk

)
f
(
xk+1, uk+1

)
...

f
(
xk+N−1, uk+N−1

)

 = f̄(xk, ū) (2.13)

MPC solves an optimization problem in each iteration over the interval [k, k+N−1]
using the current state (xk) as the initial state. Additionally, the optimization
problem is subject to state-space model equation as the equality constraint and the
limitations of the states and inputs as inequality constraints. The solution of the
optimization problem is the control input sequence ū∗. The problem solution in each
iteration can be formulated as [34] [35],

J∗(xk, ū∗) = minimize
ū

J(xk, ū)

subject to x̄+ = f̄(xk, ū);
x̄ ∈ X ;
ū ∈ U .

(2.14)

where X and U are sets defined by the limitations of the states and inputs, respec-
tively. The complete MPC algorithm is presented in Fig. 2.4.

10



2. Background

Figure 2.4: Model predictive control algorithm.

11



2. Background

12



3
Modelling

The equality constraint of the optimization problem formulation in (2.14) contains
the dynamics of system in its state-space representation. Changes or inaccuracies
in the model will induce changes in the feasible set and, therefore, in the optimal
solution obtained. Hence, a reliable system model is needed to obtain a reliable
solution in each iteration of the MPC problem.

The Euler-Lagrange equation has been used to model different systems such as DC-
DC power converters [36] [37], a robot manipulator [37], a quadrotor [38] and a
one-legged robot [39] in previous studies. In contrast to classical mechanics, the
Euler-Lagrange formalism decreases the complexity of the formulation when multi-
body systems are under study. This is because the reaction forces are usually absent
when the equations of motion are formulated [55].

Several studies using the Euler-Lagrange formalism to obtain the equations of mo-
tion of long combination vehicles (LCVs) has been published [40] [41] [42]. Also,
other studies focused on semi-trailer combination have been done, see e.g. [43] and
[44].

In this study the Euler-Lagrange equation is used to formulate the dynamics of the
system. Studies done by H. B. Pacejka [45] and P. Nilsson et al. [46] are used as
reference for modelling.

3.1 Road modeling

The Swedish Transport Administration regulates the design of the public roads in
Sweden. For horizontal road geometry, three shapes are used for design: straight
lines, circular arcs and clothoid segments [48]. Such a planar curve, formed by these
three shapes, is called a clothoid spline [49] [50]. In order to create smooth curves,
clothoid segments are always used as transitional curves between two straight lines,
a straight line and a circular arc or two circular arcs of different radii [49] [22]. The
curvature of each shape is described as [1],

c
(i)
h (s) = c

(i)
h,0 + c

(i)
h,1 · s (3.1)

13



3. Modelling

where s is the arc position, c(i)
h is the curvature of each segment i, c(i)

h,0 is the initial
curvature and c(i)

h,1 is the curvature rate. Moreover, c(i)
h is defined as,

c
(i)
h = dψ(i)

R (s)
ds (3.2)

where ψ(i)
R is the angle between the road tangent vector and the x-axis.

Figure 3.1: Road model parameters.

Fig. 3.1 shows a road segment represented as a clothoid spline (left) and the two
parameter that describe the road: ψR (middle) and ch (right). The road segment
is divided in five parts: straight line (1), clothoid (2), circular arc (3), clothoid (4)
and straight line (5). It is observed in Fig. 3.1 that the straight lines have zero
curvature, whereas circular arcs have constant curvature different than zero. In the
clothoid segments it is observed that the curvature is a linear function of s.

3.2 Vehicle motion modeling

3.2.1 System description
The vehicle combination consists of a 6 × 4 tractor and a 3-axle semi-trailer as
shown in Fig. 3.2. The characteristics of the semi-trailer used in this study follow
the modular concept explained by J. Aurell et al. [51]. The model should be able
to capture important motion characteristics and handle relevant driving scenarios.
This is possible by using the two-track model presented in Fig. 3.3. However, by
using this approach, a computationally demanding model is generated with many
states and multiple inputs [1]. Gillespie et al. [52] published a linear analysis of the
steady-state cornering performance of a multiaxle truck including the influence of
dual tires and multiple non-steerable axles [53]. This model is based on a one-track
model and it is presented in Fig. 3.4.

Figures 3.5, 3.6 and 3.7 show the one-track model of the vehicle combination and
variables used in the model formulation of this study. Fig. 3.5 illustrates relevant
dimensions, angles and coordinate frames. Fig. 3.6 illustrates position vectors of
the center of masses, axles and connection point. Additionally, Fig. 3.7 illustrates
motion variables and forces.

14



3. Modelling

The notation, presented in Appendix A, is according to the ISO 8855 norm, [54],
and P. Nilsson, [1].

Figure 3.2: Semi-trailer combination [51].

Figure 3.3: Two-track model with multiple non-steerable axles and dual tyres.

Figure 3.4: One-track model with multiple non-steerable axles and single tyres.

3.2.2 Rotation matrices
Fig. 3.5 shows four frames used in the modeling procedure,

(XE, YE) : Axis system fixed in the earth frame (Frame {E});
(XW11, YW11) : Axis system fixed in unit 1 axle 1 (Frame {W});
(XV 1, YV 1) : Axis system fixed in unit 1 (Frame {1});
(XV 2, YV 2) : Axis system fixed in unit 2 (Frame {2}).

The following rotation matrices of frames {W}, {1} and {2} with respect to frame
{E} are calculated,

Rv1 = Rz(ψ1) =
(

cosψ1 − sinψ1
sinψ1 cosψ1

)
(3.3)

Rv2 = Rz(ψ2) =
(

cosψ2 − sinψ2
sinψ2 cosψ2

)
=
(

cos (ψ1 −∆ψ1) − sin (ψ1 −∆ψ1)
sin (ψ1 −∆ψ1) cos (ψ1 −∆ψ1)

)
(3.4)

Rw11 = Rz(ψ1 + δ11) =
(

cos (ψ1 + δ11) − sin (ψ1 + δ11)
sin (ψ1 + δ11) cos (ψ1 + δ11)

)
(3.5)

15



3. Modelling

Figure 3.5: One-track model of a semi-trailer combination.

where,

Rv1 : Rotation matrix for vehicle unit 1 (Frame {1}) with
respect to earth frame (Frame {E}) ;

Rv2 : Rotation matrix for vehicle unit 2 (Frame {2}) with
respect to earth frame (Frame {E});

Rw11 : Rotation matrix for vehicle unit 1 axle 1 (Frame {W})
with respect to earth frame (Frame {E}).

3.2.3 Euler-Lagrange equation
The Euler-Lagrange equation is defined as [55],

d

dt

(
∂L(q, q̇)
∂q̇

)
− ∂L(q, q̇)

∂q
= Q (3.6)

16



3. Modelling

Figure 3.6: One-track model : Position vectors.

where q are the generalized coordinates, L(q, q̇) is the Lagrangian for the system
and Q are the generalized forces of the system. The Lagrangian is defined as,

L(q, q̇) = T (q, q̇)− V (q) (3.7)

where T (q, q̇) is the kinetic energy and V (q) is the potential energy. In this model it
is considered a flat road. Therefore, the potential energy is neglected, i.e. V (q) = 0.
Then, (3.6) can be expressed as,

d

dt

(
∂T (q, q̇)
∂q̇

)
− ∂T (q, q̇)

∂q
= Q (3.8)

By using the chain-rule, the following equivalent expression can be obtained,

d

dt

(
∂T (q, q̇)
∂q̇

)
= ∂

∂q

(
∂T (q, q̇)
∂q̇

)
dq

dt
+ ∂

∂q̇

(
∂T (q, q̇)
∂q̇

)
dq̇

dt
(3.9)

d

dt

(
∂T (q, q̇)
∂q̇

)
= ∂

∂q

(
∂T (q, q̇)
∂q̇

)
q̇ + ∂

∂q̇

(
∂T (q, q̇)
∂q̇

)
q̈ (3.10)

Using (3.10) in (3.8), the Euler-Lagrange equation can be rewritten as,

17



3. Modelling

Figure 3.7: One-track model : Forces.

∂

∂q

(
∂T (q, q̇)
∂q̇

)
q̇ + ∂

∂q̇

(
∂T (q, q̇)
∂q̇

)
q̈ − ∂T (q, q̇)

∂q
= Q (3.11)

solving (3.11) for q̈,

q̈ =
 ∂

∂q̇

(
∂T (q, q̇)
∂q̇

)−1∂T (q, q̇)
∂q

− ∂

∂q

(
∂T (q, q̇)
∂q̇

)
q̇ +Q

 (3.12)

The dynamics of the semi-trailer is described by the set of second order differential
equations obtained in (3.12). In the following subsections, q is defined, and then,
T (q, q̇) and Q are calculated in order to obtain the complete formulation of (3.12).

3.2.3.1 Generalized coordinates

The generalized coordinates of the system are defined as,

q =


x1
y1
ψ1

∆ψ1

 −→ q̇ =


ẋ1
ẏ1
ψ̇1

∆ψ̇1

 −→ q̈ =


ẍ1
ÿ1
ψ̈1

∆ψ̈1

 (3.13)

18



3. Modelling

3.2.3.2 Kinetic energy

The total kinetic energy for the semi-trailer combination is given by,

T (q, q̇) = 1
2m1v

T
1 v1 + 1

2m2v
T
2 v2 + 1

2J1ψ̇
2
1 + 1

2J2ψ̇
2
2 (3.14)

where mj are the masses of vehicle unit j, vj are the translational velocities vector
for center of gravity of vehicle unit j and Jj are the moment of inertia about the
z-axis. In order to calculate T (q, q̇); v1, v2 and ψ2 should be expressed as a function
of q and q̇.

From Figs. 3.5 and 3.6 is obtained,

r1
1c1 =

(
−l1c1

0

)
, r2

2c1 =
(
l2c1
0

)
(3.15)

Both vectors are represented in Frame {1}. In order to represent the same vectors
with respect to Frame {E}, the following transformations are done, [56],

rE1c1 = Rv1

(
−l1c1

0

)
, rE2c1 = Rv2

(
l2c1
0

)
(3.16)

In Fig. 3.6 is observed that r2 can be calculated as,

r2 = r1 + rE1c1 − rE2c1 (3.17)
r2 = r1 +Rv1r

1
1c1 −Rv2r

2
2c1 (3.18)

Therefore, the position vectors of each center of gravity can be expressed as a func-
tion of q,

r1(q) =
(
x1
y1

)
(3.19)

r2(q) = r1 +Rv1

(
−l1c1

0

)
−Rv2

(
l2c1
0

)
(3.20)

The translational velocities vector for center of gravity of vehicle unit j can be
calculated as,

vj(q, q̇) = ṙj = ∂rj
∂q

q̇ , j ∈ Nu (3.21)

Furthermore, from Fig. 3.5, ψ2 can be determined as,

ψ2(q) = ψ1 −∆ψ1 (3.22)

3.2.3.3 Generalized forces

Fig. 3.7 shows the forces acting on the system. It is observed that those forces act
on each axle and vehicle center of gravity. Then, the following general formulation
can be used to calculate the generalized forces, [55] and [57]:

Q =
∑
j∈Nu

∑
k∈Na

(
∂rjk
∂q

)T
Fjk +

∑
j∈Nu

(
∂rj
∂q

)T
Fj (3.23)

19



3. Modelling

For the semi-trailer combination, the following expression is obtained using (3.23),

Q =
(
∂r11

∂q

)T
F11 +

(
∂r12

∂q

)T
F12 +

(
∂r13

∂q

)T
F13+(

∂r21

∂q

)T
F21 +

(
∂r22

∂q

)T
F22 +

(
∂r23

∂q

)T
F23+(

∂r1

∂q

)T
F1 +

(
∂r2

∂q

)T
F2

(3.24)

From Figs. 3.5 and 3.6, all the position vectors

r1
11 =

(
l11
0

)
, r2

21 =
(
l21
0

)

r1
12 =

(
−l12

0

)
, r2

22 =
(
−l22

0

)

r1
13 =

(
−l13

0

)
, r2

23 =
(
−l23

0

) (3.25)

are expressed with respect the fixed body frame of vehicle unit 1 and 2. In order to
obtain the vector positions of each axle with respect to Frame {E}, the following
transformations are needed [56],(

rjk
1

)
= Hj

(
rjjk
1

)
=
(
Rvj rj
01×3 1

)(
rjjk
1

)
(3.26)

where Hj is the homogeneous transformation matrix of Frame {j} with respect to
Frame {E}. Then, the axles position vectors are calculated using,

rjk = rj +Rvjr
j
jk , (j, k) ∈ Nu ×Na (3.27)

The translational velocity vector of vehicle unit j axle k with respect to the earth
frame (vjk) can be define as,

vjk = ∂rjk
∂q

q̇ (3.28)

This vector expressed in a wheel fixed frame can be calculated as,
(
vXwjk
vY wjk

)
=

Rwjkvjk , (j, k) ∈ {(1, 1)}
Rvjvjk , (j, k) ∈ Nu ×Na − {(1, 1)}

(3.29)

Fig. 3.7 shows the forces acting on the semi-trailer combination. These forces are
expressed in the fixed body frame of each unit and wheel. The forces acting on each
center of mass are defined by,

FXvj =

Fair + Fgrav,j j = 1
Fgrav,j j = 2

(3.30)

FY vj = 0 (3.31)

20



3. Modelling

Assuming no longitudinal slip, small lateral slip and constant normal load, the lateral
tyre forces can be approximated using, [45] and [47],

FY wjk = −CY jk
vY wjk
vXwjk

(3.32)

where CY jk is the tyre cornering stiffness of unit j axle k.
The longitudinal tyre forces are generated by braking and propulsion and can be
calculated as,

FXwjk =

Fprop,jk + Fbrake,jk , (j, k) ∈ {(1, 2)}
0 , Nu ×Na − {(1, 2)}

(3.33)

In order to calculate Fj and Fjk, which are expressed with respect to Frame {E},
the force vectors (Fxvj, Fyvj) and (FXwjk, FY wjk) should be rotated,

Fj = Rvj

(
FXvj
FY vj

)
, j ∈ Nu (3.34)

Fjk =


Rwjk

FXwjk
FY wjk

 , (j, k) ∈ {(1, 1)}

Rvj

FXwjk
FY wjk

 , (j, k) ∈ Nu ×Na − {(1, 1)}
(3.35)

In this study the aerodynamic drag and gravitational forces are neglected. Therefore,

FXvj = FY vj = 0 , j ∈ Nu (3.36)

3.3 Vehicle motion in road coordinates
The position of any point in the earth frame can be expressed based on the road.
In order to do this, two parameters are defined: s as the position along the road of
the point projection on the road and d as the perpendicular distance from the road
[22].

Fig. 3.8 shows the semi-trailer and road. In this study, the front and rearmost axle
positions are described with respect to the the road as in previous studies done by
P. Nilsson [1] and N.J. van Duijkeren [23]. The distances along the road for the unit
1 axle 1 and unit 2 axle 3 are represented by sR11 and sR23, respectively. Moreover,
the distances dR11 and dR23 correspond to the lateral distance offset perpendicular
to the road for the unit 1 axle 1 and unit 2 axle 3, respectively.

The following expressions are obtained from P. Nilsson [1],

ṡR11 = vXv11 · cos(ψ1 − ψR(sR11))− vY v11 · sin(ψ1 − ψR(sR11))
1− dR11 · ch(sR11) (3.37)

ḋR11 = vXv11 · sin(ψ1 − ψR(sR11)) + vY v11 · cos(ψ1 − ψR(sR11)) (3.38)

21



3. Modelling

Figure 3.8: Semi-trailer motion relative to the road.

22



3. Modelling

Figure 3.9: Road and its parameters.

ṡR23 = vXv23 · cos(ψ2 − ψR(sR23))− vY v23 · sin(ψ2 − ψR(sR23))
1− dR23 · ch(sR23) (3.39)

ḋR23 = vXv23 · sin(ψ2 − ψR(sR23)) + vY v23 · cos(ψ2 − ψR(sR23)) (3.40)

where ch is the curvature of the road segment ψR is the angle between the road
tangent vector (XR11 or XR23) and the x-axis of the earth fixed frame (XE).

Fig. 3.9 shows the road used in this thesis and its parameters. In order to represent
ch and ψR as a function of s, a sigmoid function is used,

fsigmoid(x) = A

1 + e−m(x−xc) (3.41)

where parameters are defined as presented in Fig. 3.10. Then, ch and ψR can be
formulated as,

ch(s) =
N∑
j

Aj
1 + e−mj(s−sj) (3.42)

ψR(s) =
M∑
i

Ai
1 + e−mi(s−si)

(3.43)

Fig. 3.11 shows the road segment represented in ch and ψR.

23



3. Modelling

Figure 3.10: Sigmoid function.

(a) Curvature of the road (b) Angle of the road

Figure 3.11: Road parameters approximation using sigmoid functions.

24



3. Modelling

3.4 Nonlinear state-space model: Lateral and lon-
gitudinal dynamics

The following state and input vectors are selected for the semi-trailer nonlinear
model,

XNL =



vx1
vy1
ψ̇1

∆ψ̇1
ψ1

∆ψ1
δ11
sR11
dR11
sR23
dR23



, UNL =
(

δ̇11
FXw12

)
(3.44)

From (3.33), the longitudinal tyre force applied on unit 1 axle 2 is defined as,

FXw12 = Fprop,12 + Fbrake,12 (3.45)

where Fprop,12 and Fprop,12 are the propulsion force and brake force in unit 1 axle 2
in the wheel fixed frame.

The nonlinear state-space model is defined by,

ẊNL =



v̇x1
v̇y1
ψ̈1

∆ψ̈1
ψ̇1

∆ψ̇1
δ̇11
ṡR11
ḋR11
ṡR23
ḋR23



= fNL(XNL, UNL) (3.46)

From (3.12) and (3.13), the following expressions are obtained as a function of q and
q̇,

q̈ =


ẍ1
ÿ1
ψ̈1

∆ψ̈1

 (3.47)

In order to expressed the dynamics as a function of vXv1 and vY v1 instead of ẋ1 and
ẏ1, the following rotation is done,

25



3. Modelling

(
ẋ1
ẏ1

)
= Rv1

(
vXv1
vY v1

)
(3.48)

Moreover, calculating the derivative with respect to time,
(
ẍ1
ÿ1

)
= Ṙv1

(
vXv1
vY v1

)
+Rv1

(
v̇Xv1
v̇Y v1

)
(3.49)

Rewriting (3.49), (
v̇Xv1
v̇Y v1

)
= RT

v1

(ẍ1
ÿ1

)
− Ṙv1

(
vXv1
vY v1

) (3.50)

The expressions from ẍ1 and ÿ1 are obtained from (3.12). These expressions are
obtained as function of ẋ1, ẏ1, ψ1, ∆ψ1, ψ̇1 and ∆ψ̇1. The expression (3.48) is used
to expressed ẋ1 and ẏ1 as a function of vXv1 and vY v1. Finally, (3.50) is used to
obtain the expressions of v̇Xv1 and v̇Y v1 as a function of XNL.

3.5 Nonlinear state-space model: Only lateral dy-
namics

A simplified nonlinear model is formulated assuming constant velocity of vehicle
unit 1,

vXv1 : Constant (3.51)

i.e., it is assumed that all the longitudinal forces acting on this unit are zero.

FXw11 sin(δ11) + FXw12 + FXw13 = 0 (3.52)

The coupling forces between unit 1 and 2 are neglected in order to simplify the
calculation.

The following state vector and input are selected,

Xnl =



vY v1
ψ̇1

∆ψ̇1
ψ1

∆ψ1
δ11
sR11
dR11
sR23
dR23



, Unl = δ̇11 (3.53)

26



3. Modelling

and the state-space model is defined by,

Ẋnl =



v̇Y v1
ψ̈1

∆ψ̈1
ψ̇1

∆ψ̇1
δ̇11
ṡR11
ḋR11
ṡR23
ḋR23



= fnl(Xnl, Unl) (3.54)

3.6 Linear model: Lateral and longitudinal dy-
namics

In this case the following nonlinear state-space model is defined.

X =



vXv1
vY v1
ψ̇1

∆ψ̇1
ψ1

∆ψ1
δ11


, U =

(
δ̇11

FXw12

)
−→ Ẋ =



v̇Xv1
v̇Y v1
ψ̈1

∆ψ̈1
ψ̇1

∆ψ̇1
δ̇11


= F(X , U) (3.55)

The equilibrium point (Xe,Ue) is selected as,

Xe =



vXv1e
vY v1e
ψ̇1e

∆ψ̇1e
ψ1e

∆ψ1e
δ11e


=



vXv1d
0
0
0
0
0
0


, Ue =

(
δ̇11e

FXw12e

)
=
(

0
0

)
(3.56)

where vXv1d is the desired longitudinal velocity of unit 1. Additionally, around the
equilibrium point no acceleration is needed in order to keep constant longitudinal
velocity. Therefore, the longitudinal force in unit 1 at the equilibrium point is
defined as,

FXw12e = 0 (3.57)
Then, F can be linearized around the equilibrium point (Xe,Ue) using (2.7),

Ẋ = A(vXv1d)X +B(vXv1d)U (3.58)
Using (3.58) in (3.46), a partial linear state-space equation is obtained,

27



3. Modelling

ẊL =


A(vXv1d)X +B(vXv1d)U

ṡR11
ḋR11
ṡR23
ḋR23

 = fLINEAR(XL, UL) (3.59)

where,

XL =



vx1
vy1
ψ̇1

∆ψ̇1
ψ1

∆ψ1
δ11
sR11
dR11
sR23
dR23



, UL =
(

δ̇11
FXw12

)
(3.60)

3.7 Linear model: Lateral dynamics
The following expressions are defined,

X =



vY v1
ψ̇1

∆ψ̇1
ψ1

∆ψ1
δ11


−→ Ẋ =



v̇Y v1
ψ̈1

∆ψ̈1
ψ̇1

∆ψ̇1
δ̇11


= F(X , U) (3.61)

The equilibrium point (Xe,Ue) is selected as,

Xe =



vY v1e
ψ̇1e

∆ψ̇1e
ψ1e

∆ψ1e
δ11e


=



0
0
0
0
0
0


, Ue = δ̇11e = 0 (3.62)

Then, F can be linearized around the equilibrium point using (2.7),

Ẋ = A(vXv1)X +B(vXv1)U (3.63)

where A and B are expressed as a function of vXv1. Using (3.63) in (3.54), a partial
linear state-space equation is obtained,

28



3. Modelling

Ẋl =


A(vXv1)X +B(vXv1)U

ṡR11
ḋR11
ṡR23
ḋR23

 = flinear(Xl, Ul) (3.64)

where,

Xl =



vY v1
ψ̇1

∆ψ̇1
ψ1

∆ψ1
δ11
sR11
dR11
sR23
dR23



, Ul = δ̇11 (3.65)

3.8 Kinematic model
The kinematic model approach neglects the lateral forces, mass and inertia of the
vehicle [58] [24]. At low velocities, the tyre slip effects on the system can be neglected
[59]. Therefore, since kinematic model neglects mass and inertia, this approach
can give an accurate model if low velocity is used. Previous studies showed good
performance of the kinematic model at low velocities [60] [61]. In this study, the
methodology followed by Kalose [24] is used in order to obtain the kinematic model.
The rearmost axle of the first unit, i.e. unit 1 axle 3, is chosen as the reference
point. The position vector of the reference is defined as,

r13 =
(
x13
y13

)
(3.66)

A new set of generalized coordinates can be defined as,

qkin =


x13
y13
ψ1

∆ψ1

 −→ q̇kin =


ẋ13
ẏ13
ψ̇1

∆ψ̇1

 (3.67)

where,
ẋ13 = vXv1 cos(ψ1) (3.68)

ẏ13 = vXv1 sin(ψ1) (3.69)

The position vectors of the front and rearmost axles can be defined as,

r11 = r13 +Rv1

(
L11
0

)
(3.70)

29



3. Modelling

r23 = r1f −Rv2

(
L22
0

)
(3.71)

where,

r1f = r13 +Rv1

(
e1
0

)
(3.72)

and L11 is the distance between the 2 axles of unit 1, L22 is the distance between
the 2 axles of unit 2 and e1 is the distance between the rear axle of unit 1 and the
joint point.

The velocity vectors can be calculated by,

v11 = ∂r11

∂qkin
q̇kin (3.73)

v23 = ∂r23

∂qkin
q̇kin (3.74)

In every axle, the lateral velocity should be zero. Therefore, the following constraint
should hold,

ẏ cos(ω)− ẋ sin(ω) = 0 (3.75)

where
(
ẋ ẏ

)T
is the velocity vector of the axle in the Frame {E}. Then, the

following general expression is formulated,r̂(ψj + δjk) · vjk = 0 , (j, k) ∈ {(1, 1)}
r̂(ψj) · vjk = 0 , (j, k) ∈ Nu ×Na − {(1, 1)}

(3.76)

where r̂(ω) is a unit vector defined by,

r̂(ω) =
(
− cos(ω) sin(ω)

)
(3.77)

This constraint is used in the front and rearmost axles,

r̂(ψ1 + δ11) · v11 = 0 (3.78)

r̂(ψ2) · v23 = 0 (3.79)
and solving (3.78) and (3.79) for ψ̇1 and ∆ψ̇1 yields

ψ̇1 = sin(δ11)
L1 cos(δ11)vXv1 (3.80)

∆ψ̇1 = L2 sin(δ11)− L1 cos(δ11) sin(∆ψ1)− L1r sin(δ11) cos(∆δ11)
L1L2 cos(δ11) vXv1 (3.81)

where,
L1 = l11 + l13 (3.82)

30



3. Modelling

L2 = l23 + l2c1 (3.83)

L1r = l13 − l1c1 (3.84)

Additionally, lateral velocity of unit 1 can be approximated by,

vY v1 = l13ψ̇1 (3.85)

The following state and input vectors are defined,

Xkin =



ψ1
∆ψ1
δ11
sR11
dR11
sR23
dR23


, Ukin = δ̇11 (3.86)

The kinematic state-space model can be formulated as,

Ẋkin =



ψ̇1
∆ψ̇1
δ̇11
ṡR11
ḋR11
ṡR23
ḋR23


= fkin(Xkin, Ukin) (3.87)

3.9 Summary
Fig. 3.12 shows a summary of the modeling process. A nonlinear state-space model
that describes the longitudinal and lateral dynamics of the semi-trailer is obtained
(fNL) using the Euler-Lagrange formalism. A partial linear state-space model is
obtained from fNL. The dynamics of the semi-trailer are described by the first seven
states of fNL. These states are linearized around an equilibrium point (Xe, Ue) in
order to obtain fLINEAR.
If constant longitudinal velocity of unit 1 is assumed, the nonlinear state-space model
can be expressed as fnl. The lateral dynamics of the semi-trailer are described by
the first six states of fnl. These states are linearized around an equilibrium point
(Xe, Ue) in order to obtain flinear. Finally, the kinematic model approach is applied
under these conditions. Using this approach ψ̇1 and ∆ψ̇1 are estimated. Afterwards,
fkin is formulated.

31



3. Modelling

Figure 3.12: Modeling procedure summary.

32



4
Proposed trajectory planning

system

The aim of this study is road tracking considering physical limitations and safety of
the vehicle combinations. In order to do this, two trajectory planners using MPC are
formulated. Figures 4.1 and 4.2 show diagrams that include the trajectory planner,
vehicle combination and controllers. In this study, the value of the vehicle combina-
tion state (X) is known at any time instant and the characteristics of the road are
known (ψR and ch)

An optimization problem is formulated to be solved in every iteration of the MPC
problem. The decision variable is defined as,

Z =
(
ū
x̄

)
(4.1)

where ū and x̄ are defined in (2.10) and (2.11), respectively. Both vectors are con-
structed based on the state and input vectors selected for the state-space model
of the system. After selecting the decision variable, a cost function, equality and
inequality constraints should be defined as is presented in (2.14). The equality con-
straints are defined using the state-space equation of the model over the prediction
horizon as shown in (2.13). The inequality constraints are selected based on the
physical limitation of the system.

4.1 Longitudinal and lateral trajectory planner

The decision variable for the trajectory planner problem is built using XL and UL,
which are defined in (3.60). Fig. 4.1 shows a diagram of the longitudinal and lateral
trajectory planner in the system. For road tracking, two states of the system are
minimized: dR11 and dR23. The inputs are also penalized in this formulation. The
steering angle rate (δ̇11) is minimized in order to achieve smooth behavior [22]. In
order to achieve constant velocity in unit 1, the difference between vXv1 and the
reference velocity vrefXv1 is minimized. Finally, FXw12 is minimized to avoid violent

33



4. Proposed trajectory planning system

Figure 4.1: Diagram of the longitudinal and lateral trajectory planner in the
system.

changes of velocity. Thus, the cost function to be minimized is formulated as,

J(Z) =
k+N−1∑
i=k

Kδ̇11
·
(
δ̇11(i)

)2
+KFXw12 ·

(
FXw12(i)

)2

+KvXv1 ·
(
vXv1(i)− vrefXv1

)2

+KdR11 ·
(
dR11(i)

)2
+KdR23 ·

(
dR23(i)

)2


(4.2)

where Kδ̇11
, KFXw12 , KvXv1 KdR11 and KdR23 are tuning parameters used to penalize

the contribution in the cost function of δ̇11, FXw12, the deviation of vXv1 with respect
to vrefXv1, dR11 and dR23, respectively.

The cost function is minimized subject to constraints defined by the dynamics of the
system and physical limitations in the states and inputs of the system. The equality
constraints of the optimization problem includes the information of the dynamics of
the system as shown in (2.13). The equality constraints are defined using fLINEAR
as state-space model. Additionally, in order to formulate the inequality constraints,
boundaries in the states and inputs are defined based on physical limitations and
safety,

δ̇min11 ≤ δ̇11 ≤ δ̇max11 (4.3)

Fmin
Xw12 ≤ FXw12 ≤ Fmax

Xw12 (4.4)

δmin11 ≤ δ11 ≤ δmax11 (4.5)

dminR11 ≤ dR11 ≤ dmaxR11 (4.6)

dminR23 ≤ dR23 ≤ dmaxR23 (4.7)

vminXv1 ≤ vXv1 ≤ vmaxXv1 (4.8)

Bounds in dR11 and dR23 are defined in order to achieve road tracking, bounds in δ11
for prevention of rolling over and bound in δ̇11 based on actuator limitations.

34



4. Proposed trajectory planning system

The complete optimization problem of the longitudinal and lateral trajectory planner
(PLLlinear) is formulated as,

J∗(xk, ū∗) = minimize
ū

k+N−1∑
i=k

Kδ̇11
·
(
δ̇11(i)

)2
+KFXw12 ·

(
FXw12(i)

)2

+KvXv1 ·
(
vXv1(i)− vrefXv1

)2

+KdR11 ·
(
dR11(i)

)2
+KdR23 ·

(
dR23(i)

)2


subject to x̄+ = f̄LINEAR(xk, ū);
dminR11 ≤ dR11 ≤ dmaxR11 ;
dminR23 ≤ dR23 ≤ dmaxR23 ;
δ̇min11 ≤ δ̇11 ≤ δ̇max11 ;
δmin11 ≤ δ11 ≤ δmax11 ;
Fmin
Xw12 ≤ FXw12 ≤ Fmax

Xw12;
vminXv1 ≤ vXv1 ≤ vmaxXv1 .

(4.9)

The solution of the optimization problem (ū∗) is an actuation trajectory in a horizon
N .

4.2 Lateral trajectory planner
Fig. 4.2 shows a diagram of the lateral trajectory planner in the system. In this
formulation, it is assumed constant velocity in unit 1. Therefore, only lateral dy-
namics are considered to formulate this problem.

Figure 4.2: Diagram of the lateral trajectory planner in the system.

Two lateral trajectory planners are formulated in this study. The first one uses the
partial linear state-space model defined in (3.64) to build the equality constraints.
In this formulation, Xl and Ul are used as state and input vectors. The second one

35



4. Proposed trajectory planning system

uses the kinematic model defined in (3.87) to build the equality constraints. In this
case, Xkin and Ukin are used as state and input vectors.
The lateral trajectory planner using linear dynamics (PLlinear) or the kinematic
model (PLkinematic) can be formulated as,

J∗(xk, ū∗) = minimize
ū

k+N−1∑
i=k

Kδ̇11
·
(
δ̇11(i)

)2

+KdR11 ·
(
dR11(i)

)2
+KdR23 ·

(
dR23(i)

)2


subject to x̄+ = f̄lateral(xk, ū);
dminR11 ≤ dR11 ≤ dmaxR11 ;
dminR23 ≤ dR23 ≤ dmaxR23 ;
δ̇min11 ≤ δ̇11 ≤ δ̇max11 ;
δmin11 ≤ δ11 ≤ δmax11 .

(4.10)

where f̄lateral can be f̄linear or f̄kin.

36



5
Results

At the beginning of this chapter, the method to estimate the cornering stiffness
values of the one truck model is presented. In order to estimate these parameters,
different simulation tests are performed on the high-fidelity model. Then, simulation
tests are implemented in order to validate and compare every simplified model (that
was previously presented), with the high fidelity model.
Furthermore, each proposed trajectory planner is tested, using Forces Pro. During
the simulations, the plant model that is used in every case is the corresponding
dynamic/kinematic model used from the current trajectory planner.
In addition, the same simulation tests are performed using the high-fidelity model
as a plant model. The results obtained from these tests can give to the reader a
more realistic view of the performance of the proposed trajectory planners.

5.1 Cornering stiffness estimation
Tyre cornering stiffness value depends on several variables and tyre properties such
as vertical load, steering angle, longitudinal velocity and slip angle [47]. In this
study, the lateral tyre force is estimated using (3.32). Then, the cornering stiffness
can be estimated by,

CY jk = −FY wjk
vXwjk
vY wjk

(5.1)

where CY jk is the cornering stiffness of unit j axle k. The HF model provided by
Volvo Trucks is used to estimate the cornering stiffness values,

CY jk = −Fm
Y wjk

vmXwjk
vmY wjk

(5.2)

where Fm
Y wjk, vmXwjk and vmY wjk are measured in the HF model. Simulations during

20 seconds with different vXv1 and δ11 values are performed. The sampling time of
simulation is Tsimulation = 0.001 seconds. Values in the following interval are used,

vXv1 ∈ [1.5, 5.5] m/s (5.3)

δ11 ∈ [0.05, 0.5] rad (5.4)
Figures 5.1 and 5.2 show histograms of the estimations of each tyre. Finally, the
cornering stiffness used for the one-track model with multiple axles is estimated by,

CY jk = CY jk−left + CY jk−right (5.5)

Table 5.1 shows the cornering stiffness of each axle used in this model.

37



5. Results

Figure 5.1: Cornering stiffness of unit 1.

Figure 5.2: Cornering stiffness of unit 2.

38



5. Results

Cornering Stiffness Value
CY 11 35× 104

CY 12 20× 104

CY 13 20× 104

CY 21 13× 104

CY 22 13× 104

CY 23 13× 104

Table 5.1: Cornering stiffness estimated parameters.

5.2 Model validation
The high fidelity (HF) model is used to validate the nonlinear, linear and kinematic
models obtained in this thesis. The HF model has two inputs: steering angle and
acceleration request. In this study, FXw12 is used instead of acceleration request
since both values are linearly related,

FXw12 = (m1 +m2)areq (5.6)

where areq is the acceleration request.

Figure 5.3: Model validation diagram.

39



5. Results

Fig. 5.3 shows a diagram of the simulations performed to model validation. In
order to compare the different models, constant velocity (vrefXv1) of vehicle unit 1 is
used. To achieve this, a proportional controller is used in the HF model and fLINEAR
to keep vXv1 constant. Moreover, the same input signal δ̇11 is used for all the models.

The error of the center of gravity estimation of vehicle unit j with respect to the
HF model is calculated by,

ej−m(t) =
∣∣∣∣∣∣∣∣
(
xj−HF (t)
yj−HF (t)

)
−
(
xj−m(t)
yj−m(t)

) ∣∣∣∣∣∣∣∣
2

(5.7)

where
(
xj−HF (t) yj−HF (t)

)T
is the center of gravity of vehicle unit j calculated

by the HF model and
(
xj−m(t) yj−m(t)

)T
the center of gravity estimated by the

model.

5.2.1 Model validation: Constant steering angle
In the first simulation, a constant steering angle of δ11 = 5◦ is used. In order to
achieve this, the following input and initial conditions are considered,

δ̇11(t) = 0 (5.8)
δ110 = 5◦ (5.9)

where δ110 is the initial condition of δ11. The following condition are considered,
vrefXv1 = 2.5 m/s (5.10)
Tsimulation = 10 s (5.11)

Fig. 5.4 shows the estimated center of gravity of unit 1 and 2 using the different
models. Fig. 5.5 shows the estimation error calculated using (5.7). Fig. 5.6 shows
the estimation of ψ1 and ∆ψ1. The dashed black lines are the results from the HF
model. The blue lines are the estimations using the linear model of lateral and
longitudinal dynamics (fLINEAR). The dashed green lines are the estimation using
the linear model of lateral dynamics flinear. The red lines are the estimations using
the kinematic model (fkin).

Figure 5.4: Center of gravity of vehicle unit 1 and 2. Input δ̇11(t) = 0 and initial
condition δ110 = 5◦.

40



5. Results

Figure 5.5: Error estimation of the center of gravity of vehicle unit 1 and 2 by the
nonlinear and linear mode. Input δ̇11(t) = 0 and initial condition δ110 = 5◦.

Figure 5.6: Estimation of ψ1 and ∆ψ1. Input δ̇11(t) = 0 and initial condition
δ110 = 5◦.

5.2.2 Model validation: Sinusoidal steering angle

In the second test, uses the following input to the three models,

δ̇11(t) = A · cos 2π · f · t (5.12)

where A = 0.1053 rad/s and f = 0.08 Hz. Moreover, constant longitudinal velocity
of vehicle unit 1 is considered,

vrefXv1 = 2.5 m/s (5.13)

The simulation time is Tsimulation = 20 s and the results are shown in Figures 5.7,
5.8 and 5.9.

41



5. Results

Figure 5.7: Center of gravity of vehicle unit 1 and 2. Input δ̇11(t) = A ·cos 2π · f · t
and initial condition δ110 = 0◦. A = 0.1053 rad/s and f = 0.08 Hz.

Figure 5.8: Error estimation of the center of gravity of vehicle unit 1 and 2 by
the nonlinear and linear mode. Input δ̇11(t) = A · cos 2π · f · t and initial condition
δ110 = 0◦. A = 0.1053 rad/s and f = 0.08 Hz.

Figure 5.9: Estimation of ψ1 and ∆ψ1. Input δ̇11(t) = A · cos 2π · f · t and initial
condition δ110 = 0◦. A = 0.1053 rad/s and f = 0.08 Hz.

42



5. Results

5.3 Trajectory planner: Simulations using Forces
Pro solver and nonlinear state-space model

The discrete state-space models (fLINEAR, flinear or fkin) used as equality constraints
are obtained using the Forward-Euler method with a sampling time Ts. The state
sequence, input sequence and the next step state sequence are formulated using
steps intervals of T∆ seconds. The prediction horizon (Thorizon) of the MPC problem
is selected as,

Thorizon = 8 s (5.14)

The following equation is used to calculate the number of samples of horizon,

N = Thorizon
T∆

(5.15)

The Ts and T∆ values selected for the different planners are presented in Table 5.2.
Finally, the following longitudinal velocity reference of unit 1 is selected,

vrefXv1 = 3 m/s (5.16)

The lateral trajectory planners (PLlinear and PLkinematic) boundaries and tuning
parameters are presented in Table 5.3. Moreover, the nonlinear state-space fNL is
used for the simulations.

Trajectory Planner Ts [s] T∆ [s]
PLLlinear 0.01 0.25
PLlinear 0.01 0.25
PLkinematic 0.02 0.25

Table 5.2: Values of Ts and T∆ for each planner.

Constant Value
Kδ̇11

10
KdR11 30
KdR23 80
dminR11 −3.5
dmaxR11 +3.5
dminR23 +2.5
dmaxR23 +2.5
δ̇min11 −0.2
δ̇max11 +0.2
δmin11 −0.4
δmax11 +0.4

Table 5.3: Lateral trajectory planners: Tuning parameters and boundaries.

43



5. Results

The longitudinal and lateral trajectory planner (PLLlinear) is designed using the
formulation in (4.9). The diagram of the simulation is presented in Fig. 4.1. In this
formulation, the acceleration constraint of vehicle unit 1 is defined by,∣∣∣aXv1

∣∣∣∣ ≤ alimXv1 (5.17)

The following relationship can be considered around the stationary point used to
obtain the linear models,

FXw12 = (m1 +m2) · aXv1 (5.18)

The boundaries for FXw12 are defined by∣∣∣FXw12

∣∣∣∣ ≤ (m1 +m2) · alimXv1 (5.19)

The following acceleration limit is considered in this study,

alimXv1 = 2 m/s2 (5.20)

Table 5.4 shows the boundaries and tuning parameters. Fig. 5.10 shows the road
tracking results. Fig. 5.11 shows the optimal control input obtained by the planners
( ˙δ11) and δ11. It is observed that the three planners have similar results. Similar
dR11, dR23 and δ11 values are obtained. However, some differences in the optimization
problem solving time is observed in Fig. 5.12. PLkinematic is the fastest planner. It
has a mean solving time of 0.015 seconds, while PLlinear planner has a mean solving
time of 0.028 seconds and PLLlinear a mean solving time value of 0.032 seconds.

Constant Value
Kδ̇11

200
KFXw12 0.00002
KvXv1 500
KdR11 1
KdR23 10
dminR11 −3.5
dmaxR11 +3.5
dminR23 +2.5
dmaxR23 +2.5
δ̇min11 −0.2
δ̇max11 +0.2
δmin11 −0.4
δmax11 +0.4
Fmin
Xw12 −39482
Fmax
Xw12 +39482
vminXv1 1.5
vmaxXv1 3.5

Table 5.4: Trajectory planner using linear longitudinal and lateral dynamics model:
Tuning parameters and boundaries.

44



5. Results

(a) PLlinear : Road tracking (b) PLlinear : dR11 and dR23

(c) PLkinematic : Road tracking (d) PLkinematic : dR11 and dR23

(e) PLLlinear : Road tracking (f) PLLlinear : dR11 and dR23

Figure 5.10: Simulations using nonlinear model and Forces Pro solver: Road
tracking, dR11 and dR23.

45



5. Results

(a) PLlinear : δ̇11 (b) PLlinear : δ11

(c) PLkinematic : δ̇11 (d) PLkinematic : δ11

(e) PLLlinear : δ̇11 (f) PLLlinear : δ11

Figure 5.11: Simulations using nonlinear model and Forces Pro solver: δ11 and
δ̇11.

46



5. Results

Figure 5.12: Simulation using nonlinear model and Forces Pro: Planner solving
time.

5.4 Trajectory planner: Real time simulations us-
ing Forces Pro solver and HF model

The trajectory planners are tested using the HF model provided by Volvo Trucks.
Real time simulations using ROS are performed. The frequency publish ROS topic
is set to 20 hz.

The same tuning parameters are used in this simulation (Table 5.4). Fig. 5.13
shows the road tracking results. Fig. 5.14 shows the optimal control input obtained
by the planners ( ˙δ11) and δ11. As in the previous simulations, the three planners
have similar results for the optimal control input and states. In simulations using
PLlinear and PLkinematic, an additional longitudinal controller is used. On the other
hand, PLLlinear planner includes lateral and longitudinal predictions. Fig. 5.15
shows vXv1 values during simulations. Finally, PLkinematic is the fastest planner as
well. The optimization problem solving time is presented in Fig. 5.16. In this case,
PLkinematic has a mean solving time of 0.012 , while PLlinear planner has a mean
solving time of 0.032 seconds and PLLlinear a mean solving time value of 0.039
seconds. It is observed that the standard deviation of the solving time is higher
than in simulations using the nonlinear state-space model.

47



5. Results

(a) PLlinear : Road tracking (b) PLlinear : dR11 and dR23

(c) PLkinematic : Road tracking (d) PLkinematic : dR11 and dR23

(e) PLLlinear : Road tracking (f) PLLlinear : dR11 and dR23

Figure 5.13: Real time simulation using HF model, Forces Pro solver and ROS:
Road tracking, dR11 and dR23.

48



5. Results

(a) PLlinear : δ̇11 (b) PLlinear : δ11

(c) PLkinematic : δ̇11 (d) PLkinematic : δ11

(e) PLLlinear : δ̇11 (f) PLLlinear : δ11

Figure 5.14: Real time simulation using HF model, Forces Pro solver and ROS:
δ11 and δ̇11.

49



5. Results

(a) PLlinear : vXv1

(b) PLkinematic : vXv1

(c) PLLlinear : vXv1

Figure 5.15: Real time simulation using HF model, Forces Pro solver and ROS :
Longitudinal velocity vXv1.

50



5. Results

Figure 5.16: Real time simulation using HF model, Forces Pro and ROS: Planner
solving time.

51



5. Results

52



6
Conclusions and Future work

6.1 Conclusions

In this section, the conclusions obtained by the three trajectory planners will be
presented. The conclusions are related to the estimation errors of each model, the
solving time of each planner, the operating velocities of the planners, the selection of
the prediction horizon and the dependence of every planner on additional controllers.

In this thesis three different semi-trailer models are obtained: Linear model of lateral
dynamics, Linear model of longitudinal and lateral dynamics and kinematic model.
Low error estimation is observed in the three cases. The highest error is observed
in the kinematic model, however its formulation is much simpler than the other two
models.

In addition, three different planners are formulated using the three models obtained
as equality constraints: PLlinear, PLkinematic and PLLlinear. PLkinematic is the fastest
planner (lowest solving time) and PLLlinear the slowest (highest solving time). It
should be noted that the solving time can be influenced by several factors. These fac-
tors are the gains of the cost function, the hard inequality constraints, the complexity
of the equality constraints, the prediction horizon, the number of the optimization
variables, the discretization method, the time step as well as the number of times
that the solution search algorithm is repeated to solve the optimization problem.
By comparing all the trajectory planners, it should be noted that PLLlinear and
PLlinear are more general, as they can operate at both high and small velocities. On
the other hand, PLkinematic can perform well at low velocities. Also, even though
PLkinematic and PLlinear are designed to operate at constant velocities, they can ac-
count for small deviations. This can be verified from the trajectory planners’ results
with HF model.Long prediction horizons would, in theory, increase the stability of
the system. Though, as the trajectory planners use approximated to the HF mod-
els, using too long prediction horizons might lead to bad behavior. This can be
explained by the increased with time estimation errors (model validation). As a
result, the simplified models cannot be trusted for a long time. It should be noted
that in order to use PLkinematic and PLlinear, an additional longitudinal controller is
needed. On the other hand, when using planner PLLlinear, no additional controller
is needed.

53



6. Conclusions and Future work

6.2 Future work
Even though the trajectory planners performed in a good manner, several improve-
ments can be suggested for future work. Suggestions on how to improve the trajec-
tory planners’ performance (generated trajectories and driving comfort), the accu-
racy of the models and the way the road characteristics are used, will be shown in
the text below.

Firstly, it would be desirable to harden the constraint of lateral distance offset per-
pendicular to the road on the first axle to 2.5 m. In order for the trajectory planners
to generate smooth trajectories and not to violate this constraint, different tuning is
required. Hence, it is recommended to reformulate the optimization problem (with-
out decreasing the prediction horizon) and improve the performance of the trajectory
planners. However, neither of the trajectory planners considered minimizing or con-
straining the longitudinal or lateral jerk. Keeping the jerk in low values could lead
to more comfortable driving.

Furthermore, there are several suggestions on how to increase the accuracy of the
models. Including the aerodynamic drag and gravitational forces into the model
could lead to more robust performance. Hence, it is recommended to design and
test trajectory planners which can operate successfully on more realistic scenarios
(e.g. strong wind, uphill road). Also, in this research, the Euler method is used to
discretize the continuous systems. Since this method is in general poor, it is recom-
mended to try and obtain good performance using higher-order methods, such as
Runge-Kutta 2,3 and 4.

Lastly, instead of using sigmoid functions to describe the road characteristics (cur-
vature and angle of the road) as functions of the covered distance, the road charac-
teristics could be read from a look-up table. A way to achieve this is to predict the
road characteristics in each time instance using the predicted cover distances from
the previous solution of the optimization problem, which will be then applied on a
look-up table. This method is easier to apply on different road scenarios as well as
it may influence the overall performance of the trajectory planners.

54



Bibliography

[1] P. Nilsson, Traffic situation management for driving automation of articulated
heavy road transports - from driver behaviour towards highway autopilot, Ph.D.
thesis, Department of Mechanics and Maritime Sciences, Chalmers University
of Technology, Sweden, 2017.

[2] A. J. Hawkins. Toyota’s self-driving cars can now fit more junk in their trunks.
January 3, 2019. https://www.theverge.com/2019/1/3/18166854/toyota-
self-driving-car-test-vehicle-ces-2019. Accessed 3 June, 2019.

[3] A. Macduff and R. Aron. Autonomous trucks in real operation. Febru-
ary 20, 2019. https://www.volvotrucks.com/en-en/news/volvo-trucks-
magazine/2019/feb/bronnoy.html. Accessed 3 June, 2019.

[4] L. Papadopoulos. Renault’s new car is a shared self-driving all-electric luxury
dream. October 4, 2018. https://interestingengineering.com/renaults-
new-car-is-a-shared-self-driving-all-electric-luxury-dream. Ac-
cessed 3 June, 2019.

[5] Self-Driving Cars Explained. February 21, 2018. https://www.ucsusa.org/
clean-vehicles/how-self-driving-cars-work. Accessed 3 June, 2019.

[6] Human Error Causes 94 Percent of Car Accidents. June 9, 2017.
https://blog.lawinfo.com/2017/09/06/human-error-causes-94-
percent-of-car-accidents/. Accessed 3 June, 2019.

[7] Autonomous vehicles and the environment. August 6, 2018. https:
//teleroute.com/en-en/blog/article/autonomous-vehicles-the-
environment/. Accessed 3 June, 2019.

[8] M. Hutson. Watch just a few self-driving cars stop traffic jams. November 16,
2018. https://www.sciencemag.org/news/2018/11/watch-just-few-self-
driving-cars-stop-traffic-jams. Accessed 3 June, 2019.

[9] Volvotrucks.com. https://www.volvotrucks.com/en-en/home.html. Ac-
cessed 3 June, 2019.

[10] D.G. Bautista and V. Milanes and J. Pérez and F. Nashashibi, Review of motion
planning techniques for automated Vehicles, IEEE Transactions on Intelligent
Transportation Systems, November 2015.

[11] Julia Nilsson Trajectory Planning via Convex Optimization in the Model Pre-
dictive Control Framework. Department of Signals and Systems Chalmers Uni-
versity of Technology Goteborg, Sweden 2016.

[12] R. Kala and K. Warwick, Multi-level planning for semi-autonomous vehicles in
traffic scenarios based on separation maximization, J. Intell. Robot. Syst., vol.
72, no. 3/4, pp. 559–590, Dec. 2013.

55

https://www.theverge.com/2019/1/3/18166854/toyota-self-driving-car-test-vehicle-ces-2019
https://www.theverge.com/2019/1/3/18166854/toyota-self-driving-car-test-vehicle-ces-2019
https://www.volvotrucks.com/en-en/news/volvo-trucks-magazine/2019/feb/bronnoy.html
https://www.volvotrucks.com/en-en/news/volvo-trucks-magazine/2019/feb/bronnoy.html
https://interestingengineering.com/renaults-new-car-is-a-shared-self-driving-all-electric-luxury-dream
https://interestingengineering.com/renaults-new-car-is-a-shared-self-driving-all-electric-luxury-dream
https://www.ucsusa.org/clean-vehicles/how-self-driving-cars-work
https://www.ucsusa.org/clean-vehicles/how-self-driving-cars-work
https://blog.lawinfo.com/2017/09/06/human-error-causes-94-percent-of-car-accidents/
https://blog.lawinfo.com/2017/09/06/human-error-causes-94-percent-of-car-accidents/
https://teleroute.com/en-en/blog/article/autonomous-vehicles-the-environment/
https://teleroute.com/en-en/blog/article/autonomous-vehicles-the-environment/
https://teleroute.com/en-en/blog/article/autonomous-vehicles-the-environment/
https://www.sciencemag.org/news/2018/11/watch-just-few-self-driving-cars-stop-traffic-jams
https://www.sciencemag.org/news/2018/11/watch-just-few-self-driving-cars-stop-traffic-jams
https://www.volvotrucks.com/en-en/home.html


Bibliography

[13] M. Likhachev and D. Ferguson, Planning long dynamically feasible maneuvers
for autonomous vehicles, Int. J. Robot. Res., vol. 28, no. 8, pp. 933–945, Aug.
2009.

[14] O. Ljungqvist1 and D. Axehill1 and J. Lofberg, On stability for state-lattice tra-
jectory tracking control, 2018 American Control Conference (ACC), Milwaukee,
June 2018.

[15] Y. Kuwata et al., Real-time motion planning with applications to autonomous
urban driving, IEEE Trans. Control Syst. Technol., vol. 17, no. 5, pp. 1105–1118,
Sep. 2009

[16] B. Park and Y. Lee and W. Han Trajectory generation method using Bezier
spiral curves for ´ high-speed on-road autonomous vehicles2014 IEEE Inter-
national Conference on Automation Science and Engineering (CASE) Taipei,
Taiwan, August 18-22, 2014.

[17] L. Labakhua, U. Nunes, R. Rodrigues, and F. S. Leite Smooth Trajectory
Planning for Fully Automated Passengers Vehicles: Spline and clothoid based
methods and its simulationin Informatics in Control Automation and Robotics.
Berlin, Germany: Springer-Verlag, 2008, pp. 169–182.

[18] W. Lim and S. Lee and M. Sunwoo and K. Jo Hierarchical trajectory planning of
an autonomous car based on the integration of a sampling and an optimization
methodIEEE transactions on intelligent transportation systems, vol. 19, no. 2,
February 2018.

[19] Q. T. Dinh* and M. Diehl An application of sequential convex programming to
time optimal trajectory planning for a car motion Joint 48th IEEE conference
on decision and control and 28th Chinese control conference Shanghai, P.R.
China, December 16-18, 2009.

[20] C. Law, Off-tracking: how rear truck tires can crush pedestrians. https://www.
truckingwatchdog.com/2018/09/01/off-tracking-semi-truck/. Accessed
3 June, 2019.

[21] D. Gonzalez Bautista and V. Milanes and J. Pérez - F. Nashashibi. A Review
of Motion Planning Techniques for Automated Vehicles, IEEE Transactions on
Intelligent Transportation Systems, November 2015.

[22] P. Wallin and P. Nilsson. Trajectory planning for automated highway driving
of articulated heavy vehicles, Department of Electrical Engineering, Chalmers
university of technology, 2018.

[23] N. J. van Duijkeren, Real-time receding horizon trajectory generation for long
heavy vehicle combinations on highways, Faculty of Mechanical, Maritime and
Materials Engineering (3mE), Delft University of Technology, Netherlands,
2014.

[24] A. Kalose. Controller for Steering of Smart Dolly, HAN University, 2018.
[25] N. Andren and A. Gil Martın and K. Hoogendijk and L. Niklasson and F.

Sandblom - F.Slottner Seholm. Predictive Control for Autonomous Articulated
Vehicles, Department of Computer Science, Chalmers university of technology,
2017.

[26] N. van Duijkeren and T. Keviczky and P. Nilsson and L. Laine, KU
Leuven.Real-Time NMPC for Semi-Automated Highway Driving of Long Heavy

56

https://www.truckingwatchdog.com/2018/09/01/off-tracking-semi-truck/
https://www.truckingwatchdog.com/2018/09/01/off-tracking-semi-truck/


Bibliography

Vehicle Combinations, Delft University of Technology, Volvo Group Trucks
Technology.

[27] M. S. Bazaraa, H. D. Sherali and C. M. Shetty, Nonlinear programming, Third
Edition, John Wiley & Sons, 2006.

[28] N. Andréasson, A. Evgrafox and M. Patriksson, An Introduction to Continu-
ous Optimization: Foundations and Fundamental Algorithms, Third Edition,
Studentlitteratur, 2016.

[29] Ogata, Katsuhiko. Modern Control Engineering. Upper Saddle River, N.J:
Prentice Hall, 1997.

[30] Åström, Karl Johan and Richard M. Murray, Feedback Systems : An Introduc-
tion for Scientists and Engineers, Princeton University Press, Princeton and
Oxford, 2012.

[31] H. Kwakernaak and R. Sivan, Linear Optimal Control Systems, First Edition,
John Wiley & Sons, 1972.

[32] Rawlings, J. B., Tutorial Overview of Model Predictive Control, IEEE Contr.
Syst. Magazine, 2000.

[33] Rawlings, J. B., E. S. Meadows and K. R. Muske, Nonlinear Model Predictive
Control: A Tutorial and Survey, Proc. Int. Symp. Adv. Control of Chemical
Processes, p. 185, 1994.

[34] Mayne, D. Q., J. B. Rawlings, C. V. Rao, and P. O. M. Scokaert, Constrained
Model Predictive Control: Stability and Optimality, Automatica, 26(6), pp.
789 - 814, 2000.

[35] L. Grüne and J. Pannek, Nonlinear Model Predictive Control: Theory and
Algorithms, Springer Publishing Company, Incorporated, 2013

[36] H. Sira-Ramirez, R. Ortega and G. Escobar, Lagrangian Modeling of Switch
Regulated DC-to-DC Power Converters, IEEE Conference on Decisition and
Control, Kobe, Japan, December 1996, pp. 396-402.

[37] H. A. Yildiz and L. Goren-Sumer, Lagrangian modeling of DC-DC buck-boost
and flyback converters, Circuit Theory and Design 2009. ECCTD 2009. Euro-
pean Conference on, pp. 245-248, 2009.

[38] K. U. Lee, H. S. Kim, J. B. Park and Y. H. Choi, Hovering control of a quadro-
tor, Proc. 121 International Conference on Control, Automation and Systems
(ICCAS), pp. 162-167,2012.

[39] D. B. Tank, R. S. Jo and H. S. Jo, Dynamic modelling and control of self-
balancing one-legged robot, IEEE International Conference on Automatic Con-
trol and Intelligent Systems, pp. 19 - 23, 2018.

[40] D. d. Bruin, Lateral Guidance of All-Wheel Steered Multiple-Articulated Vehi-
cles. Dissertation, Technische Universiteit Eindhoven, 2001.

[41] C. Fletcher, C. Manzie, and M. Good, Trailer Steering: An Australian Research
Perspective and Application for By-Wire Control. Technical report, Ninth In-
ternational Symposium on Heavy Vehicle Weights and Dimensions. June 18-22,
Penn State, State College, Pennsylvania, 2006.

[42] M. K. Bahaghighat, S. Kharrazi, M. Lidberg, P. Falcone and B. Schofield,
Predictive Yaw and Lateral Control in Long Heavy Vehicles Combinations,
IEEE Conference on Decision and Control, pp. 6403-6408, 2010.

57



Bibliography

[43] C. Chen and M. Tomizuka, Dynamic Modeling of Tractor-Semitrailer Vehicle
in Automated Highway Systems, California Path Program Institute of Trans-
portation studies, University of California, Bekerley, 1995.

[44] Z. Yu and J. Li, Modeling and verification of heavy-duty semi-trailer, Interna-
tional Conference on Transportation, Mechanical, and Electrical Engineering
(TMEE), pp. 982 - 985, 2011.

[45] H. B. Pacejka. Tire and vehicle dynamics. Butterworth-Heinemann, 2006.
[46] P. Nilsson, K. Tagesson, Single-track models of an a-double heavy vehicle com-

bination, Tech. rep., Department of Applied Mechanics, Chalmers University
of Technology, Sweden, 2013.

[47] B. Jacobson. Vehicle Dynamics - Compendium for Course MMF062. Chalmers
University of Technology, 2016.

[48] Trafikverket, Krav för vägars och gators utformning, 2015.
[49] D. S. Meek and R. Thomas, A guided clothoid spline. Computer Aided Geo-

metric Design 8.2, pp. 163-174, 1991.
[50] J. Stoer, Curve fitting with clothoidal splines, J. Res. Nat. Bur. Stand., 87, pp.

317-346, 1982.
[51] J. Aurell and T. Wadman, Vehicle combinations based on the modular concept.

Tech. rep. 1. NVF-reports, Report 1/2007, 2007. DOI: ISSN:0347-2485.
[52] T. D. Gillespie and C. B. Winkler, On the directional response characteristics

of heavy vehicles, The Dynamics of Vehicles on Roads and Tracks, Amsterdam,
Swets&Zeitlinger, pp. 165- 187, 1978.

[53] C. B. Winkler. Simplified Analysis of the Steady-State Turning of Complex
Vehicles, Vehicle System Dynamics 29, pp. 141-180, 1998.

[54] S. S. Institute. SS-ISO 8855:2011. Road vehicles - Vehicle dynamics and road-
holding ability - Vocabulary. 2011.

[55] A. Boström, Rigid body dynamics, Chalmers University of Technology, 2012.
[56] B. Siciliano, L. Sciavicco, L. Villani and G. Oriolo, Robotics: Modelling, Plan-

ning and Control, Springer Publishing Company, 2008.
[57] P. Nilsson, K. Tagesson, Single-track models of an a-double heavy vehicle com-

bination, Tech. rep., Department of Applied Mechanics, Chalmers University
of Technology, Sweden, 2013.

[58] R. Kusumakar, K. Kural, A. S. Tomar and B. Pyman, Autonomous parking for
articulated vehicles, HAN University of Applied Science, Netherlands, 2017.

[59] J. Pauwelussen, Essentials of vehicle dynamics, Butterworth-Heinemann, 2014.
[60] R. Werner, S. Mueller, and G. Kormann, Path tracking control of tractors and

steerable implements based on kinematic and dynamic modeling, in Proc. 11th
Int. Conf. Precis. Agricult., pp. 15–18, 2012.

[61] B. A. Jujnovich and D. Cebon, Path-following steering control for articulated
vehicles, J. Dyn. Syst., Meas. Control, vol .135, no. 3, 2013.

58



I



A. Nomenclature

A
Nomenclature

{E} : Earth fixed frame.
{1} : Vehicle unit 1 body fixed frame.
{2} : Vehicle unit 2 body fixed frame.
{W} : Vehicle unit 1 axle 1 wheel fixed frame.
Nu = {1, 2} : Set of number of vehicle units.
Na = {1, 2, 3} : Set of number of axles.
δ11 : Steering angle of vehicle unit 1 axle 1.
ψj : Yaw angle of vehicle unit j;
∆ψ1 : Articulation angle of the trailing vehicle unit 1;
(XE, YE) : Earth fixed frame.
(XW11, YW11) : Unit 1 wheel 1 fixed frame.
(XV j, YV j) : Vehicle unit j body fixed frame;
(xj, yj) : Center of gravity of vehicle unit j;
vXvj : The x-axis component of the translational velocity

vector of vehicle unit j in a body fixed frame;
vY vj : The y-axis component of the translational velocity vec-

tor of vehicle unit j in a body fixed frame;
ljk : Distance from center of gravity of unit j to axle k;
ljc1 : Distance from center of gravity of unit j to the con-

nection point rear of unit 1;
FXwjk : The x-axis component of tyre force of vehicle unit j

axle k in a wheel fixed frame;
FY wjk : The y-axis component of tyre force of vehicle unit j

axle k in a wheel fixed frame;
Fgrav,j : Gravitational force of vehicle unit j;
Fair : Aerodynamic drag in a body fixed frame;
rj : Position vector for center of gravity of vehicle unit j

with respect to earth frame;
rjk : Position vector of vehicle unit j axle k with respect to

earth frame;
rjjk : Position vector of vehicle unit j axle k with respect to

frame j;
rjc1 : Position vector of vehicle unit j axle k with respect to

earth frame;
rjjc1 : Position vector of vehicle unit j axle k with respect to

frame j;

II


	List of Figures
	List of Tables
	Introduction
	Objective
	Prerequisites
	Limitations
	Outline of the thesis

	Background
	Definitions
	Trajectory planning techniques
	Trajectory planning for articulated heavy vehicles
	Motion characteristics of articulated vehicles
	Optimization problem
	State-space model
	Model predictive control

	Modelling
	Road modeling
	Vehicle motion modeling
	System description
	Rotation matrices
	Euler-Lagrange equation
	Generalized coordinates
	Kinetic energy
	Generalized forces


	Vehicle motion in road coordinates
	Nonlinear state-space model: Lateral and longitudinal dynamics
	Nonlinear state-space model: Only lateral dynamics
	Linear model: Lateral and longitudinal dynamics
	Linear model: Lateral dynamics
	Kinematic model
	Summary

	Proposed trajectory planning system
	Longitudinal and lateral trajectory planner
	Lateral trajectory planner

	Results
	Cornering stiffness estimation
	Model validation
	Model validation: Constant steering angle
	Model validation: Sinusoidal steering angle

	Trajectory planner: Simulations using Forces Pro solver and nonlinear state-space model
	Trajectory planner: Real time simulations using Forces Pro solver and HF model

	Conclusions and Future work
	Conclusions
	Future work

	Bibliography
	Nomenclature