Model Predictive Control for Six Degrees-of-Freedom Station-Keeping of an Underwater...

IN DEGREE PROJECT ELECTRICAL ENGINEERING,SECOND CYCLE, 30 CREDITS

, STOCKHOLM SWEDEN 2017

Model Predictive Control for Six Degrees-of-Freedom Station-Keeping of an Underwater Vehicle-Manipulator System

CHRISTIAN E. S. KOCH

KTH ROYAL INSTITUTE OF TECHNOLOGYSCHOOL OF ELECTRICAL ENGINEERING

KTH Royal Institute of TechnologyStockholm, Sweden

Ocean Systems LaboratoryHeriot-Watt University

Edinburgh, Scotland

Master’s Thesis

Model Predictive Controlfor Six Degrees-of-Freedom

Station-Keeping of an UnderwaterVehicle-Manipulator System

Author:

Christian Ernst Siegfried Koch

[email protected]

Supervisor:

Dr. Matthew D. Dunnigan

Martin Biel

Examiner:

Prof. Dr. Mikael Johansson

A thesis submitted in

fulfilment of the requirements for the degree of

Master of Science in Systems, Control and Robotics

September 2017

i

Declaration of Authorship

I, Christian E. S. Koch, declare that the thesis entitled “Model Predictive Conrol

for Six Degrees-of-Freedom Station-Keeping of an Underwater Vehicle-Manipulator

System”and the work presented in this thesis are both my own, and have been

generated by me as the result of my own original research.

I confirm that:

• this work was done wholly or mainly while in candidature for a research

degree at this university;

• this thesis has not been published previously neither wholly nor in parts, and

has never been submitted in fulfillment of a degree or any other qualification

at this university or any other institution;

• were I have consulted the published work of others, this is always clearly

attributed;

• where I have quoted from the work of others, the source is always given.

With the exception of such quotations, this thesis is entirely my own work.

Signed:

Date:

ii

Abstract

Underwater robotics are a reliable and efficient mean for exploration and sur-

veys in a submarine environment. Albeit, intervention tasks, e.g. installation

and maintenance, require the expansive and hazardous deployment of professional

divers. Lightweight unmanned underwater vehicles equipped with a multi-degree-

of-freedom manipulator, have been proposed as an alternative. However, the con-

trol of these vehicle-manipulator systems is challenging due to their non-linear

high-dimensional coupled dynamics. The central problem explored in this thesis,

is station keeping of an underwater vehicle under the influence of a moving ma-

nipulator. The manipulator is represented by predictable disturbing forces and

moments. The proposed control scheme is a Model Predictive Control (MPC) al-

gorithm with preview of the disturbances. In simulation, performance of the MPC

scheme is evaluated for different degrees of knowledge about the disturbances.

Results are compared to a classical feedback controller.

Sammanfattning

Undervattensrobotik medfor ett palitligt och effektivt satt att utforska subma-

rina miljoer. Latta obemannade undervattensfarkoster, utrustade med verktyg,

har aven foreslagits som ett alternativ till professionella dykare for installation

och underhall under ytan. Styrning av verktygsutrustade farkoster ar en utman-

ing da de medfor hogdimensionell olinjar dynamik och korseffekter. Det centrala

problemet som behandlas i det har arbetet ar reglering av en undervattensfarkost

utrustad med en robotarm, vars rorelser stor farkosten. Robotarmen representeras

av storande krafter och moment som gar att forutsaga i modellramverket. Den

foreslagna styrlagen ar modell-prediktiv reglering (MPC) med storningspredik-

tion. Styrlagen utvarderas i simulering under olika vetskapsnivaer av storsignalen.

Resultaten jamfors aven med simuleringar dar klassisk aterkoppling anvands.

iii

Acknowledgement

I wish to acknowledge the great support I received from both my examiner Prof.

Mikael Johansson and my supervisor Martin Biel from KTH. Their willingness to

give their time so generously has been very much appreciated.

I would like to offer my special thanks to my supervisor Dr. Matthew D. Dunnigan

from Heriot-Watt University for his valuable help and patent guidance. I am very

grateful for the experiences I made during this project, which would not have been

possible without him.

Furthermore, I wish to thank the staff at Heriot-Watt University in general and

the members of the Ocean Systems Laboratory in particular for their assistance

and contributions to this thesis.

Contents

Contents iv

List of Figures vi

List of Tables viii

Abbreviations ix

1 Introduction 1

1.1 Thesis Objectives . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

1.2 Literature Review . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2 Modelling 6

2.1 Nomenclature . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2.2 The Vehicle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

2.3 Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

2.4 Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.4.1 Rigid-Body Dynamics . . . . . . . . . . . . . . . . . . . . . 10

2.4.2 Hydrodynamics . . . . . . . . . . . . . . . . . . . . . . . . . 12

2.4.3 The Complete Six-DoF Equation of Motion . . . . . . . . . 15

2.5 Thruster . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

2.6 Dynamic State-Space Model of the Vehicle . . . . . . . . . . . . . . 16

2.6.1 State-Space representation including disturbances . . . . . . 17

3 Control Schemes 18

3.1 Model Predictive Control . . . . . . . . . . . . . . . . . . . . . . . . 18

3.1.1 Prediction . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

3.1.2 Optimization . . . . . . . . . . . . . . . . . . . . . . . . . . 20

3.1.2.1 Cost Function . . . . . . . . . . . . . . . . . . . . . 20

3.1.2.2 Constraints . . . . . . . . . . . . . . . . . . . . . . 20

3.1.2.3 Quadratic Programming . . . . . . . . . . . . . . . 20

3.1.3 Receeding Horizon Implementation . . . . . . . . . . . . . . 21

3.1.4 Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

3.1.5 Disturbance Preview . . . . . . . . . . . . . . . . . . . . . . 22

3.1.6 Infinite Horizon Cost . . . . . . . . . . . . . . . . . . . . . . 23

3.2 PILIM Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

iv

Contents v

4 Simulation 25

4.1 Implementation Details . . . . . . . . . . . . . . . . . . . . . . . . . 25

4.1.1 Vehicle Simulator . . . . . . . . . . . . . . . . . . . . . . . . 25

4.1.2 MPC Controller and Disturbances . . . . . . . . . . . . . . . 26

4.1.3 PILIM Controller . . . . . . . . . . . . . . . . . . . . . . . . 27

4.2 Control Objective and Experimental Set-up . . . . . . . . . . . . . 28

4.3 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

4.3.1 Disturbing Moment on Pitch . . . . . . . . . . . . . . . . . . 29

4.3.1.1 Use of Input . . . . . . . . . . . . . . . . . . . . . 34

4.3.2 Disturbing Force on Surge . . . . . . . . . . . . . . . . . . . 34

4.3.2.1 Use of Input . . . . . . . . . . . . . . . . . . . . . 39

4.4 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

5 Conclusion 41

5.1 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42

A Model of the Fictional Vehicle 43

A.1 Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

A.2 Behaviour Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

A.2.1 Restoring Forces . . . . . . . . . . . . . . . . . . . . . . . . 44

A.2.2 Maximum Velocity . . . . . . . . . . . . . . . . . . . . . . . 46

A.2.3 Deceleration Curve . . . . . . . . . . . . . . . . . . . . . . . 49

A.2.4 Active Braking . . . . . . . . . . . . . . . . . . . . . . . . . 51

A.2.5 Drop-Dive Scenario . . . . . . . . . . . . . . . . . . . . . . . 52

B Linearisation and Discretisation 53

B.1 Linearisation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

B.2 Discretisation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

Bibliography 55

List of Figures

2.1 Nomenclature for submerged body . . . . . . . . . . . . . . . . . . 7

2.2 Nessie V AUV . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

2.3 Fictional vehicle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

2.4 Exemplary thruster allocation . . . . . . . . . . . . . . . . . . . . . 16

3.1 Receeding Horizon . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

3.2 MPC Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

3.3 PILIM Blockdiagram . . . . . . . . . . . . . . . . . . . . . . . . . . 24

4.1 MPC Closed Loop Block Diagram . . . . . . . . . . . . . . . . . . . 26

4.2 PILIM tuning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

4.3 Disturbance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

4.4 Pitch disturbance, Open loop . . . . . . . . . . . . . . . . . . . . . 30

4.5 Pitch disturbance, PILIM controller . . . . . . . . . . . . . . . . . . 30

4.6 Pitch disturbance, MPC controller with 100 % certainty . . . . . . . 31







4.13 Pitch disturbance, MPC controller with 0 % certainty . . . . . . . . 33

4.14 Input usage of MPC and PILIM in comparison . . . . . . . . . . . . 34

4.15 Pitch disturbance, Open loop . . . . . . . . . . . . . . . . . . . . . 35

4.16 Pitch disturbance, PILIM controller . . . . . . . . . . . . . . . . . . 35








4.25 Input usage and x-position curve for MPC and PILIM in comaprison. 39

A.1 Heave velocity and depth of the uncontrolled vehicle. . . . . . . . . 45

A.2 Restoring capabilities of roll and pitch angle . . . . . . . . . . . . . 45

vi

List of Figures vii

A.3 Surge under maximum thrust . . . . . . . . . . . . . . . . . . . . . 46

A.4 Sway under maximum thrust . . . . . . . . . . . . . . . . . . . . . . 46

A.5 Heave under maximum thrust (positive direction) . . . . . . . . . . 47

A.6 Heave under maximum thrust (negative direction) . . . . . . . . . . 47

A.7 Pitch under maximum thrust . . . . . . . . . . . . . . . . . . . . . 48

A.8 Pitch under maximum thrust . . . . . . . . . . . . . . . . . . . . . 48

A.9 Surge deceleration curve . . . . . . . . . . . . . . . . . . . . . . . . 49

A.10 Sway deceleration curve . . . . . . . . . . . . . . . . . . . . . . . . 49

A.11 Heave deceleration curve (maximum positive velocity) . . . . . . . . 50

A.12 Heave deceleration curve (maximum negative velocity) . . . . . . . 50

A.13 Yaw deceleration curve . . . . . . . . . . . . . . . . . . . . . . . . . 51

A.14 Surge under active braking . . . . . . . . . . . . . . . . . . . . . . . 51

A.15 Position and orientation in the drop-dive experiment. . . . . . . . . 52

List of Tables

2.1 Notation for underwater vehicle velocity . . . . . . . . . . . . . . . 7

2.2 Notation for underwater vehicle position . . . . . . . . . . . . . . . 8

4.1 MPC Tuning Parameter . . . . . . . . . . . . . . . . . . . . . . . . 27

4.2 PILIM Tuning Parameter . . . . . . . . . . . . . . . . . . . . . . . 28

viii

Abbreviations

AUV Autonomous Underwater Vehicle

DoF Degree of Freedom

MIMO Multiple Input, Mupltiple Output (System)

MPC Model Predictive Control

PILIM Proportional Integral LIMited (Control)

ROV Remotely Operated Unterwater Vehicle

SISO Single Input, Single Output (System)

USMS Underwater Vehicle-Manipulator System

UUV Unmanned Underwater Vehicle

ix

Symbols

Symbol Meaning

A linear state-space state matrix

B linear state-space input matrix

B body-fixed coordinate system

C(ν) Coriolis and centripetal matrix

CRB(ν) rigid-body Coriolis and centripetal matrix

CA(ν) added mass Coriolis and centripetal matrix

D(ν) damping matrix

E Earth-fixed coordiante system

Fi force produced by thruster i

f dynamic equation of non-linear state-space sys-

tem

g(η) restoring forces and moments

I0 inertia tensor matrix

Im×m Identity matrix with size m

J MPC cost function

J(η) kinematic coordinate transformation matrix

K moment on roll-DoF

M moment on pitch-DoF

M inertia matrix

MRB rigid-body inertia matrix

MA added mass inertia matrix

N moment on surge-DoF

x

Symbols xi

N MPC prediction horizon

p roll velocity

Q MPC state weighing matrix

q pitch velocity

R MPC input weighing matrix

r yaw velocity

~rG location of the centre of gravity, given in B

~rB location of the centre of buoyancy, given in B

T Thruster allocation matrix

Ts Sampling time

Uk sequence of inputs u, starting at time k

U? optimal input sequence

u surge velocity

u state-space notation: input vector

u, u upper and lower input boundary

v sway velocity

w heave velocity

Wk sequence of disturbances w, starting at time k

w state-space notation: disturbance vector

w estimated disturbance vector

X force on yaw-DoF

Xk sequence of states x, starting at time k

x position along the X-axis

x state-space notation: state vector

x, x upper and lower state boundary

Y force on sway-DoF

y position along the Y-axis

Z force on heave-DoF

z position along the Z-axis

η position and orientation vector

Symbols xii

θ orientation around the Y-axis

ν velocity vector

τ generalised forces and moment vector

τRB forces and moment associated with rigid-body dy-

namics

τA forces and moment associated with added mass

τD forces and moment associated with hydrody-

namic damping

τR forces and moment associated with gravity and

buoyancy

φ orientation around the X-axis

ψ orientation around the Z-axis

Chapter 1

Introduction

In his 2011 TED talk, oceanographer Paul Snelgrove said that the scientific com-

munity knows more about the surface of the Moon than about the floor of the

deep sea [24]. He led a ten-year project with the goal to explore the biodiversity

of the oceans. The knowledge he and his team were able to provide is immensely

useful for preservation and for our understanding of the impact of climate change

and other environmental challenges on the oceans, Earth’s largest habitat.

Exploring the submarine world with scuba divers is limited to a maximum depth of

about 50 metres. Manned submarines have been able to reach the Mariana Trench,

the deepest part of the ocean, but their use comes with high costs and potential

risks. In his research, Snelgrove made use of remotely operated underwater vehicles

(ROVs). These robots are equipped with visual and acoustic sensors and can

provide detailed insights into otherwise inaccessible ocean regions at a reasonable

price.

The use of unmanned underwater vehicles (UUVs) is steadily increasing and many

advances have been made in the field of underwater robotics. These submersibles

can be either remotely operated or autonomous. Since radio waves cannot pene-

trate water well enough to establish the necessary communication, ROVs need to

have a physical connection to the operator via a tether. The tether transmits data

and usually supplies electricity to the vehicle. The tether, often several kilometres

long, can affect the dynamics and manoeuvrability of the vehicle. Furthermore,

ROVs require a skilled operator on shore or ship. On the other hand, autonomous

underwater vehicles (AUVs) do not require a tether but rely on on-board sensors

and intelligence to make decisions and complete missions without human input.

AUV technology has seen massive progress over the last decades. Many tasks,

such as mapping, exploration and localisation, can be fulfilled by AUVs.

While researchers such as Snelgrove rely on underwater robotics, the big driver

for the developments in this field is the petroleum industry. Extracting resources

1

Chapter 1. Introduction 2

from the deep sea gets increasingly challenging since companies are required to

drill for deeper resource fields. These industries already profit from underwater

robotics for exploration and inspection purposes. However, for tasks that require

intervention, e.g. installation and maintenance of deep sea structures, industries

still have to rely on commercial diving. While techniques like Saturated diving

allows divers to perform tasks at depth of a few hundred metres, it is a dangerous

and costly endeavour.

A solution to this limitation would be to equip an UUV with a robotic manipulator

that can handle intervention tasks. In order to perform tasks on the same level as

professional divers, the vehicle needs to be compact and manoeuvrable while the

manipulator needs to have the necessary range and flexibility. Early examples of

such underwater vehicle-manipulator systems (UVMSs) are ODIN [8] and OTTER

[30] from the 1990s. Both systems consist of a fully actuated vehicle and a one-DoF

manipulator. In 1997 the SAUVIM AUV project was initiated at the University

of Hawaii [32]. This UVMS has a mass of 6500 kg and was equipped with a

seven-DoF manipulator. As recently as 2010 SAUVIM performed its first fully

autonomous intervention mission in open water.

The control of UVMS is challenging as they are characterised as high-dimensional

and non-linear systems with highly coupled dynamics. A major issue is station

keeping of the vehicle while the manipulator is employed due to interaction effects

between both subsystems [2].

1.1 Thesis Objectives

This topic of this thesis is station keeping of an underwater vehicle-manipulator

system. Corina Barbalata explored the interaction effects between vehicle and

manipulator [2]. By considering the kinematic chain in combination with both

rigid-body dynamics and hydrodynamic effects, she developed a method for mod-

elling the interaction forces. Based on this result, the underlying assumption in

this thesis is the following: Interaction effects caused by the manipulator and acting

on the vehicle can be predicted, when the motion of the manipulator is predictable.

The presumption that the motion of the manipulator is predictable holds for au-

tonomous and semi-autonomous missions, where tasks are planned in advance.

As a control structure, Model Predictive Control (MPC) is proposed. The basic

idea of MPC is to choose system inputs by predicting possible system trajectories

and determining the optimal one. As such, MPC is optimal control. MPC is

chosen since the interaction effects can easily be included in the perdiction scheme.

Furthermore, MPC offers control of coupled systems with multiple inputs and

outputs, which are two of the difficulties faced in underwater robotics. Another


strength of MPC is its ability to handle constraints natively. Constraints are

found in most applications and often lead to a degraded control performance.

In underwater robotics, constraints are always present in the form of thruster

limitations.

The objective of this thesis is to evaluate MPC for station keeping in six-DoF of

an underwater vehicle which is subject to interaction forces caused by a moving

manipulator. The MPC performance is evaluated in simulation and compared to

a PILIM 1 controller, a classical feedback control-scheme.

In this thesis an underwater vehicle is modelled including hydrodynamic effects.

An MPC controller is implemented and the closed loop system is tested in simula-

tion. The interaction effects are approximated by a predictable force disturbance.

The performance of the MPC scheme is compared to a traditional feedback con-

troller. Chapter 2 covers the vehicle modelling, while chapter 3 gives an expla-

nation on Model Predictive Control and the PILIM controller. Implementation

details along with the simulation set-up and results are presented in chapter 4.

The thesis end with a conclusion in chapter 5.

1.2 Literature Review

In the following, literature related to the objective of this thesis is reviewed. The

review is grouped into three parts: The first and most substantial part covers

Model Predictive Control for underwater robotics. In the second part, an example

of MPC being used for station-keeping in six-DoF from the domain of space-

engineering is presented. Lastly, the third part includes literature on underwater

vehicle-manipulator systems.

Model Predictive Control for underwater vehicles

Model Predictive control has shown promising results in underwater robotics and

research interest has been growing in recent years. The first paper on the topic is

by Wasif Naeem, published in 2002 [21]. Other research groups have followed and

investigated different MPC schemes for low-level as well as high-level control.

In his work, Naeem [21] explores MPC for yaw-control2 (heading-control) of a

rudder-steered AUV3. The MPC routine utilises a generic algorithm for the opti-

misation and a linearised second-order yaw-angle model for the prediction. While

1Proportional Integral Limited controller[3] Was developed especially for position control ofunderwater vehicles. See Section 3.2.

2For nautical notations see Section 2.1.3Torpedo-shaped vehicle with stern propulsion and rudders for steering.


Naeem motivates the choice of MPC by its ability to control MIMO systems, he

only considers a SISO system in his work. Another early example is the work of

Truong, Wang and Gawthrop from 2006 [29]. The authors also utilize a linearised

model for a rudder-steered vehicle but complexity is increased compared to the

work of Naeem. The model is expanded to include yaw-angle, pitch-angle as well

as position in y and (lateral) z (depth), all of which are controlled by the MPC.

Two rudders for steering are used as input, resulting in a MIMO system. To

handle the complexity, the authors propose an intermittent feedback approach to

compensate for longer sampling times. In both papers the authors conclude that

MPC shows promising results in simulation.

Standard MPC approaches can be found in the work of Budiyono [5] and Steenson

[25, 26]. Both author consider linearised models with reduced DoF. Budiyono,

again, describes control of a rudder-steered vehicle, while Steenson’s work covers a

hybrid AUV that is rudder-steered at high speeds (flight mode) as well as thruster-

actuated for lower speeds (hovering mode). Steenson evaluates the controller in

simulation as well as experimentally. Especially a controller for depth and pitch

in hovering mode shows excellent results and low sensitivity to modelling errors.

Heshmati-Alamdari et al. [13] explore non-linear MPC for low-level control. To

reduce computational complexity, an event-triggered aperiodic sampling is used.

Between sampling instances, the sequence of control inputs, as calculated with

MPC, will be fed to the vehicle in open loop. In experiments, the event-triggered

strategy performes slightly better than classical time-triggered MPC.

MPC has further been used for underwater vehicles to create a hybrid of low-level

and high-level control, especially as a combination of path planning and tracking.

Caldwell et al. [6] explore this approach with an MPC-like algorithm. The authors

use a graph-based search algorithm (e.g A?) instead of numerical optimization,

which offers advantages for path-planning. Shen et al. [23] realise dynamic path

planning and motion control with nonlinear MPC. Least curvature optimal path

planning is integrated into the MPC routine. In both papers, the concepts are

evaluated in simulation with a simplified four-DoF model. Another example of

high-level control in the underwater domain is given by Lobo Preira et al. [15],

who consider MPC for the coordinated control of a fleet of AUVs.

Due to the receding horizon fashion MPC is closed-loop feedback control. As such,

MPC can react to disturbances. However, when disturbances are predictable they

can be included in the model and thus that the controller can deal with them

in a preventive manner. This possibility is explored by Medagoda and Williams

[18]. The authors use sensor data to estimate water currents and include predic-

tions in the MPC algorithm. Fernandez and Hollinger [10] present a similar idea:

The authors explore MPC for station-keeping in surge and heave direction of an

submerged vehicle subject to waves. The influence of waves is often disregarded,

which is fair for sufficient depth. Close to the surface, however, the impact of


waves is significant. The waves are estimated and included in the predictive con-

trol scheme. The novel approach showed a 74% reduction in error compared to a

classical PD-controller.

Model Predictive Control for six-DoF station keeping

Examples of MPC for station-keeping in six-DoF can be found in the domain

of space engineering. For instance, Weiss et al. [31] consider station keeping of

an geostationary satellite. Furthermore, the control objective includes unloading

any momentum stored in three reaction wheels4. The resulting system is complex

with order 15 and nine inputs. The high complexity is compensated with by

linearisation and a very long sampling time of up to one hour.

Underwater vehicle-manipulator systems

Underwater vehicle-manipulator systems have been an area of active research for

over two decades. A variety of feedback and model based feed-forward controller

have been proposed for the control of UVMSs. McLain et al. [17] explore the

hydrodynamic effects caused by a single joint manipulator on the vehicle in order

to derive a dynamic model. The authors propose a model-based control strategy

to improve station-keeping capabilities of the vehicle during manipulator move-

ment. Barbalata [2] derives a generic non-linear model for an UVMS including

hydrodynamic effects from first principles. The model can be used for simulation.

The author further proposes and compares a variety of control schemes for track-

ing and coordinated control. While several control schemes have been tested, to

the authors best knowledge, MPC has not been applied to vehicle-manipulator

systems. This is likely due to the high complexity of the coupled model. However,

as MPC showed good results in underwater vehicle control, it is desirable to ex-

tend the application to vehicle-manipulator systems. Furthermore, with increasing

processing capabilities, MPC becomes feasible for complex systems.

4Reaction wheels are on-board devices used to change the orientation of a space-craft basedon the principle of conservation of momentum.

Chapter 2

Modelling

This chapter covers the modelling of an underwater vehicle. The model is derived

according to two books by Thor I. Fossen [11] and [12]. It comprises three parts:

the kinematic equation, the dynamic equation and the thruster allocation. The

chapter starts with an explanation of the notation used in this thesis, followed by

a description of the modelled vehicle. It continues with a detailed description of

the three parts of the model and concludes with a presentation of the state space

representation of the model.

2.1 Nomenclature

In marine engineering it is common to follow a nomenclature guideline given by

the Society of Naval Architects and Marine Engineers [28]. An overview is given

in Fig. 2.1. The state of a submerged body is described with three components

of translation and three components of rotation, i.e. an underwater vehicle has

six degrees of freedom. According to convention, the vehicles position/orientation

and the velocity will be represented in two distinct coordinate systems:

• Body-fixed coordinate system BThis coordinate system moves and rotates with the vehicle.

• Earth-fixed coordinate system EThis coordinate system is stationary relative to a flat earth and assumed to

be inertial. The Z-axis is defined such that it points towards the centre of

the earth, i.e. a high coordinate value is associated with a greater depth.

To describe the absolute position and orientation of the vehicle, the earth-fixed

coordinate system E is used. The orientation is specified using Euler-angles. An

overview is given in Table 2.2.

6

Chapter 2. Modelling 7

Surgeu

Rollp

Swayv

Pitchq

Heavew

Yawr

Earth-fixed EX

YZ

Body-fixed B

Figure 2.1: Nomenclature for submerged bodies according to [28]

Axis Translation Rotationof Motion Velocity Force Velocity Moment

(B) [m s−1] [N] [deg s−1] [N m]Longitudinal surge u X roll p K

Lateral sway v Y pitch q MVertical heave w Z yaw r N

Table 2.1: Notation for linear and angular velocities of a submerged bodyalongside their associated forces and moments. All entities are defined in respect

to the body-fixed reference frame.

By convention, velocities are described depending on the principle axes of the

vehicle. The axes are as follows:

• Longitudinal axis from stern to bow

• Lateral axis from port to starboard

• Vertical axis from topside to keel

These axes define the body-fixed coordinate system B, i.e velocities are given B.

The linear velocities along these axes are referred to as surge, sway and heave.

Angular velocities around the respective axes are referred to as roll, pitch and

yaw. An overview is given in Table 2.1.

2.2 The Vehicle

The model describes a fictional vehicle. However, in many aspects it is related

to Nessie V, an AUV developed at the Ocean Systems Laboratory at Heriot-Watt


Axis Position Orientation(E) [m] [deg]

X-axis x φY-axis y θZ-axis z ψ

Table 2.2: Notation for position and orientation of a submerged body. Allentities are defined in respect to the earth-fixed reference frame

University in Edinburgh [1], [22]. Nessie V has a torpedo shaped hull and its

propulsion system consists of six thrusters, four of which are located in tunnels

through the body.

Figure 2.2: Nessie V AUV with payload

The fictional vehicle is also torpedo shaped and assumed to have three planes of

symmetry. The centre of gravity coincides with the geometric centre and thus with

the origin of the body-fixed reference frame. This assumption allows to simplify

the model, as is exlained later in this chapter. The centre of buoyancy is placed on

top of the centre of gravity, resulting in a self-stabilising vehicle in roll and pitch.

This is due to the gravity and buoyancy forces acting on a submerged vehicle,

which always work to vertically align the centre of buoyancy and the centre of

gravity.

Also, the thruster configuration, shown in 2.3, is similar to Nessie V : A pair of

thrusters is pointing in surge, sway and heave direction respectively. The two

surge thrusters are located outside the hull while the remaining four are installed

in tunnels. With this configuration the vehicle can be actuated in 5-DoF with roll

being uncontrollable.

Furthermore, the fictional vehicle is modelled to be slightly buoyant, i.e. in order

to keep the vehicle underwater, the heave thrusters have to produce a force. This


Figure 2.3: Thruster configuration of the fictional torpedo shaped vehicle.The thruster pairs are named: Surge-thruster (blue), sway-thruster (red) and

heave-thruster (green)

is a common safety feature in underwater robotics, since the robot will simply rise

to the surface in case the actuation fails.

2.3 Kinematics

In this section, the first part of the model, the kinematic equation, is presented.

Essentially, this is a coordinate transformation from the body-fixed system B to

the earth fixed reference frame E .

In this model, the angular and linear velocity of the vehicle, given in B, is repre-

sented as a six-dimensional vector ν.

ν = [u v w p q r]T (2.1)

where u, v, w, p, q and r follow the notation in Table 2.1 in Section 2.1.

Similarly, the position and orientation of the vehicle, i.e. the origin of B relative

to E is given by the vector η.

η = [x y z φ θ ψ]T (2.2)

where, again, x, y, z, φ, θ and ψ follow the notation in Table 2.2. Although it

includes both position and orientation, η will be referred to as the position vector

in this thesis.

Another vector τ is used to represent the corresponding forces and moments acting

on the vehicle, according to Table 2.1.

τ = [X Y Z K M N ]T (2.3)


For simplification, τ will be referred to as the force-vector, although it includes

forces as well as moments.

The kinematic equation Eq. (2.4) relates the velocity vector ν and the position

vector η by expressing the velocity in E-coordinates. The velocity expressed in Ecan be written as the time derivative of the position vector, i.e. η.

η = J(η)ν (2.4)

The matrix J(η) is a coordinate transformation matrix from B to E . It is depen-

dant on the orientation, i.e. η, of the vehicle.

J(η) =

cψcθ −sψcφ+ cψsθsφ sψsφ+ cψcφsθ 0 0 0

sψcθ cψcφ+ sφsθsψ −cψsφ+ sθsψcφ 0 0 0

−sθ cθsψ cθcψ 0 0 0

0 0 0 1 sφtθ cφtθ

0 0 0 0 cφ −sφ0 0 0 0 sφ/cθ cφ/cθ

(2.5)

Here, sα, cα and tα are used as a shortened notions for sinα, cosα and tanα

respectively.

2.4 Dynamics

The second part of the model, the dynamic equation, is derived by considering

both rigid-body dynamics and hydrodynamics.

2.4.1 Rigid-Body Dynamics

The six-DoF equation of motion for a rigid body can be written as

τRB = MRBν + CRB(ν)ν (2.6)

where MRB is the rigid-body inertia matrix, and CRB(ν) represents Coriolis and

centripetal effects. The vector τRB is the generalised force-vector associated with

the rigid-body motion. Detailed information about rigid-body dynamics can be

found in [9].

The Rigid-Body Inertia Matrix

The inertia matrix MRB depends on


• the body’s mass m,

• the inertia tensor matrix I0 (given in B),

• and the location of the centre of gravity ~rG = |xG yG zG|T (given in B).

The inertia Matrix can be derived from first principles, e.g. using the Newton-

Euler or Lagrangian formulation as suggested in [11]. The derivation is thoroughly

treated in literature and not part of this thesis. Instead, the resulting matrix is

presented here:

MBR =

[mI3×3 −mS(~rG)

mS(~rG) I0

](2.7)

The matrix function S(λ), where λ ∈ R3×1, is defined as

S(λ) =

0 −λ3 λ2λ3 0 −λ1−λ2 λ1 0

(2.8)

Due to the assumptions made in Section 2.2, the inertia tensor has no off-diagonal

elements, i.e. I0 = diag {Ix, Iy, Iz}, and ~rG = [0, 0, 0]T since the centre of gravity

coincides with the origin of B. These simplifications lead to the following inertia

matrix

MBR =

m 0 0 0 0 0

0 m 0 0 0 0

0 0 m 0 0 0

0 0 0 Ix 0 0

0 0 0 0 Iy 0

0 0 0 0 0 Iz

(2.9)

The Coriolis and Centripetal Matrix

The CRB(ν)-matrix is calculated from the inertia matrix MRB and dependent

on the vehicle’s velocity ν. The derivation through Kirchhoff’s equation of mo-

tion, is detailed in [11] and skipped in this thesis. There are multiple equivalent

parametrisations for CRB(ν), one of which is presented here.

By rewriting MRB and ν to

MBR =

[M11 M12

M21 M22

](2.10)

ν =[νT1 νT2

]T(2.11)


CRB(ν) can be calculated in the following way:

CRB(ν) =

[03×3 −mS(ν1)−mS(ν2)S(~rG)

−mS(ν1) +mS(~rG)S(ν2) −S(I0ν2)

]. (2.12)

The function S(λ) is defined in Eq. (2.8).

Similarly to the inertia matrix, CRB(ν) can be simplified by exploiting the as-

sumptions made in Section 2.2:

CRB(ν) =

0 0 0 0 mw −mv0 0 0 −mw 0 mu

0 0 0 mv −mu 0

0 mw −mv 0 Izr −Iyq−mw 0 mu −Izr 0 Ixp

mv −mu 0 Iyq −Ixp 0

. (2.13)

2.4.2 Hydrodynamics

Hydrodynamic forces are difficult to model. However, the rigid-body equations of

motion can be extended to include hydrodynamic effects with a few simplifications.

The following hydrodynamic effects are modelled:

• Added Mass

• Damping forces

• Restoring forces

Added Mass

In addition to it’s own mass, the vehicle also has to move the mass of the sur-

rounding fluid. The forces and moments due to this additional mass τA act against

the direction of velocity. They can be modelled with an inertia matrix MA and

Coriolis and centripetal matrix CA(ν).

τA = −MAν −CA(ν)ν (2.14)

The matrix CA(ν) can be derived from MA. In a general case, determining the

off-diagonal elements of MA is a challenging task. However, in the special case of

a slender vehicle with three planes of symmetry which is fully submerged, MA can

be approximated by a diagonal matrix. In this case

MA = diag {A1, A2, A3, A4, A5, A6} (2.15)


CA(ν) =

0 0 0 0 A3w −A2v

0 0 0 −A3w 0 A1u

0 0 0 A2v −mu 0

0 A3w −A2v 0 A6r −A5q

−A3w 0 A1u −A6r 0 A4p

A2v −A1u 0 A5q −A4p 0

(2.16)

The elements A1 to A6 depend on the geometry of the body. The fictional vehicle

described in Section 2.2 can be well approximated by a cylinder. For a cylindrical

body with mass m, length l and radius r in a fluid with density ρ the following

expressions hold for the coefficients [2, 11]:

A1 ≈ 0.1m A4 = 0

A2 = ρπr2 A5 = 112ρπr2l3

A3 = A2 A6 = A5

(2.17)

Damping

Damping is caused by several hydrodynamic effects [11]. Among the most signif-

icant are skin friction and vortex shedding. Skin friction is the friction between

the vehicles boundary layer and the surrounding liquid. The vortex shedding phe-

nomenon describes an oscillating flow behind a moving object through a fluid.

This oscillation causes drag on the object. The magnitude depends on various

factors, e.g the geometry of the vehicle and the relative speed.

In the model, damping forces and moments τD are quantified by the damping

matrix D(ν) and depend on the velocity.

τD = −D(ν)ν (2.18)

A possible approximation for an underwater vehicles as described in Section 2.2,

is to neglect off-diagonal elements of D(ν). Furthermore, only first and second

order damping terms are considered. The matrix then becomes

D(ν) = −diag {Dl1 , Dl2 , Dl3 , Dl4 , Dl5 , Dl6}− diag {Dq1|u|, Dq2|v|, Dq3|w|, Dq4|p|, Dq5|q|, Dq6 |r|}

(2.19)

Since only limited theory is available, determining the coefficients would require

experimental evaluation. However, quadratic damping due to vortex shedding can

be used as a first approximation. This is based on Morison’s equation [20] and

described in [11]. The linear and quadratic coefficients are adjusted experimentally


by evaluating the models behaviour in simulation. A detailed study can be found

in Appendix A.

Restoring forces

Restoring forces are gravity and buoyancy acting on the vehicle. While these are of

course constant effects, the resultant forces and moments, τR, given in B, depend

on the orientation of the vehicle. The restoring forces are thus expressed as

τR = −g(η). (2.20)

The restoring forces are acting along the z-axis of the earth-fixed coordinate system

in opposite direction, i.e. gravity is in positive z-direction (downwards) while

buoyancy is in negative z-direction (upwards). The force due to gravity, i.e. the

weight of the vehicle, W , is acting on the centre of gravity, ~rG. On the other hand,

the buoyancy force, denoted B, is acting on the centre of buoyancy. Both forces

can be determined from first principle:

W = mg (2.21)

B = ρg∇ (2.22)

where m is the mass of the vehicle, g is Earth’s gravitational acceleration and ρ

and ∇ denote the density and the volume, respectively, of the displaced liquid.

The restoring forces W and B can be tranformed to B by using the coordinate

transformation matrix J(η) defined in Eq. (2.5), which yields

fG(η) = J−1(η)[0 0 W

]T(2.23)

fB(η) = −J−1(η)[0 0 B

]T. (2.24)

(2.25)

The resultant forces are simply the sum of fG and fB, while the resultant moment

depends on the centres of gravity and buoyancy:

g(η) = −[

fG(η) + fB(η)

~rG × fG(η) + ~rB × fB(η)

](2.26)

In the fictional vehicle from Section 2.2 the centre of gravity is assumed to be

in the origin of B, i.e ~rG =[0 0 0

]T. Furthermore, the centre of buoyancy is

vertically aligned with the centre of gravity, i.e. ~rB =[0 0 zB

]T. This leads to


a simplified equation for g(η) which can be written as

g(η) =

(W −B)sθ

−(W −B)cθsφ

−(W −B)cθcφ

−zBBcθsφzBBsθ

0

(2.27)

Again, sα and cα are used as a shortened notions for sinα and cosα respectively.

2.4.3 The Complete Six-DoF Equation of Motion

The complete model describing the six-DoF motion of the submarine including

both rigid-body dynamics and hydrodynamics is derived by considering the total

net forces and moments. It must hold that

τ + τRB + τA + τD + τR = 0 (2.28)

where τ is the generalised force-vector representing the vehicle actuation as defined

in Section 2.3. By substituting the remaining force-vectors according to Eqs. (2.6),

(2.14), (2.18) and (2.20), the following equation can be derived

τ = MRBν + CRB(ν)ν + MAν + CA(ν)ν + D(ν)ν + g(η) (2.29)

This equation can be simplified by introducing the augmented inertia and Coriolis

matrices:

M = MRB + MA (2.30)

C(ν) = CRV (ν) + CA(ν) (2.31)

The complete equation describing the six-DoF motion of the submarine including

both rigid-body dynamics and hydrodynamics is subsequently written as

τ = Mν + C(ν)ν + D(ν)ν + g(η) (2.32)

2.5 Thruster

In the dynamic equation (2.32), the generalised force-vector τ stands as the control

input. However, the vehicle is actuated by six thrusters, configured according to

Section 2.2.Furthermore, in this thesis it is assumed that thruster dynamics can

be disregarded. Thus it is desired to have the forces produced by the individual


thrusters as control input. This can easily be done by calculating τ as the resultant

force of all thruster forces [19].

Each thruster i produces a force fi = Fi ·~rFiwith strength Fi and the unit direction

vector ~rFi. The shortest distance between the line of thrust and the origin of B

is denoted ~rTi (see Fig. 2.4). The vector τ =[τ T1 τ T2

]Trepresents the resultant

forces and moments. They can be calculated from the force produced by each

thruster fi according to

τ1 =∑

fi (2.33)

τ2 =∑

fi × ~rTi (2.34)

This can be rewritten with a constant transformation matrix T to

τ = Tu (2.35)

where u =[F1 F2 F3 F4 F5 F6

]Tis the input vector containing the six

thruster forces.

line of thrustf1

~rT1

line of thrust

f2

~rT2Origin of B

Figure 2.4: Exemplary thruster allocation

2.6 Dynamic State-Space Model of the Vehicle

The kinematic equation (2.4) combined with the dynamic equation (2.32) yields

the complete dynamic model for the subsurface vehicle. It can be written as a

nonlinear state-space system of order 12.[ν

η

]︸︷︷︸x(t)

=

[M−1τ −M−1(C(ν) + D(ν))ν −M−1g(η)

J(η)ν

]︸︷︷︸

f(x(t),u(t))

(2.36)

with u = T−1τ as the input to the system. It is important to note, that thruster

dynamics are not included in this model.


2.6.1 State-Space representation including disturbances

the interaction effects acting on the vehicle and caused by the manipulator are

treated as disturbing forces and moments. They are represented by a generalised

force-vector in B and denoted

w =[w1 w2 w3 w4 w5 w6

]T. (2.37)

The disturbances are included in the model by considering

τ = Tu + w (2.38)

resulting in the complete state-space equation[ν

η

]︸︷︷︸x(t)

=

[M−1(Tu(t) + w(t))−M−1(C(ν) + D(ν))ν −M−1g(η)

J(η)ν

]︸︷︷︸

f(x(t),u(t),w(t))

(2.39)

Chapter 3

Control Schemes

This chapter is foremost an introduction into Model Predictive Control (MPC),

the proposed control scheme in this thesis (Section 3.1). However, it also includes

a short presentation of the PILIM control scheme, a classical feedback controller

for underwater vehicles which is used for comparison (Section 3.2).

3.1 Model Predictive Control

MPC is an optimal control strategy with a history that dates back to applications

in the process industry in the early 1970s [14]. The basic idea of MPC is as

following: The controller chooses the best possible control action according to the

desired progression by looking ahead and predicting the future. Usually, MPC can

be characterised by three elements:

Prediction: Based on knowledge about the system (i.e. a model), the current

state and potentially the environment, system trajectories along a future

time horizon are calculated for possible control action.

Optimisation: According to a performance index and possibly constraints, the

optimal system trajectory, in other words the optimal control action, is de-

termined.

Recceding Horizon Implementation: Only the beginning off the optimal con-

trol sequence is applied to the system before the process is repeated, i.e. the

prediction horizon is shifted one step into the future.

While this concept is rather intuitive, the calculative burden due to the optimisa-

tion made it impractical apart from slow processes. In recent years, with advances

18

Chapter 3. Control Schemes 19

in computer technology, MPC has gained popularity in industry and academia.

This can be attributed to some of MPC’s key features:

• MPC is optimal control

• Constraints are inherently enforced

• Applicable to systems with multiple inputs and outputs

• Possible to handle non-linearities

Serveral variations of MPC have been developed. The MPC scheme used in this

thesis is presented in this chapter. This particular scheme uses a discrete linear

model for prediction, a quadratic cost function and linear constraints.

3.1.1 Prediction

Future system trajectories are explicitly calculated based on a model of the system.

It is common to work in discrete time with a fixed sampling interval and to assume

linearised dynamics. The model can thus be expressed in a linear discrete state-

space equation

x[k + 1] = Ax[k] + Bu[k]. (3.1)

In the discrete notation, steps k are used rather than time t. The time at each

step is t = t0 + Tsk, where Ts is a uniform sampling time. Time is assumed to

begin with the start of the experiment, thus t0 = 0.

This model can be used to predict the behaviour of the system. For predicted

states and their corresponding input, the following notation is used: x[m|k] and

u[m|k] denote the predicted state (respectively the input) for step m as calculated

at step k. Consequently, x[k|k] = x[k] denotes a actual value of the states at step

k, i.e. the state measurement.

In MPC the trajectories are commonly predicted over a finite prediction horizon of

N steps. Assuming the prediction is performed at step k, the predicted trajectories

depend on the current state measurement, also referred to as the initial condition,

i.e. x[k|k] = x[k]. The prediction leads to a sequence of states, denoted Xk,

{x[k + 1|k], x[k + 2|k], x[k + 3|k], . . . , x[k +N ]} =: Xk (3.2)

and a sequence of corresponding inputs, denoted Uk,

{u[k|k], u[k + 1|k], u[k + 2|k], . . . , u[k +N − 1]} =: Uk. (3.3)

To summarise: A sequence of states Xk and inputs Uk define one future system

trajectory over a prediction horizon of length N based on the initial condition

x0 = x[k] at time k.


3.1.2 Optimization

MPC determines the optimal control action according to a performance index while

being limited to an optional set of constraints. In this thesis, the optimisation is

based on a quadratic cost function and constraints defined as linear inequalities.

This combination is common and describes a Quadratic Programming problem.

3.1.2.1 Cost Function

The performance index is stated as a quadratic cost function J of states and inputs,

e.g.

J(Xk,Uk) =∑x∈Xk

xTQx +∑u∈Uk

uTRu (3.4)

The weighing matrices Q and R are control parameter set by user. They are chosen

in correspondence to the control objective and shift the focus between penalising

state deviation and penalising control actions. However, in order for a quadratic

cost funcion to be convex, the matrices Q and R must be positive semi-definite.

Per definition, the optimal inputs sequence U? minimises the cost, i.e.

U? = arg minUk

J(Xk,Uk) = arg minUk

∑x∈Xk

xTQx +∑u∈Uk

uTRu (3.5)

3.1.2.2 Constraints

In control applications, states and inputs are usually subject to constraints. In

many classic control schemes, this leads to a degraded performance. A major

strength of MPC is that constrains can be included in the control scheme and

handled optimally. A simple way of defining constraints is in the form of linear

inequalities that limit for instance state and input values to an allowed range:

x ≤x ≤ x ∀x ∈ Xk (3.6)

u ≤u ≤ u ∀u ∈ Uk (3.7)

3.1.2.3 Quadratic Programming

From above we get an optimisation problem that requires minimising a quadratic

function subject to linear constraints. This defines a Quadratic Programming

problem and can be efficiently handled by a multitude of available solvers.


The complete optimisation problem can be written as follows

U?k (x[k]) = arg minUk

J(Xk,Uk)

s.t.

x[j + 1|k] = Ax[j|k] + Bu[j|k] ∀j = k . . . k +N

x[k|k] = x[k]

x ≤ x ≤ x ∀x ∈ Xku ≤ u ≤ u ∀u ∈ Uk

(3.8)

3.1.3 Receeding Horizon Implementation

At time step k the optimisation determines a sequence of optimal inputs U?k .

Instead of applying the entire sequence to the system in open loop, only the first

input of the sequence u?[k|k] is given to the system. The rest of the sequence is

discarded and the process is repeated at the next time step k+ 1. In this way, the

prediction horizon is shifted one step into the future, i.e the predictor is always

looking N steps ahead. This is called receding horizon and visualised in Fig. 3.1.

Prediction horizon at time k

Prediction horizon at time k + 1

kk + 1

k +Nk + 1 +N

u

x

time

time

Figure 3.1: Receeding horizon visualised [7]

3.1.4 Algorithm

The basic MPC algorithm consists of four steps, which are repeated at every

sampling instance:

1. Sampling

The current state of the plant is sampled at time k. This gives the state

measurement (initial conditions) x[k].


2. Optimisation

The optimal input sequence U?k is calculated by solving the optimisation

problem.

3. Select first input

Out of the optimal sequence, the first input, u?[k|k], is selected. The rest of

the series is discarded.

4. Application

The optimal control input u?[k|k] is applied to the plant.

The MPC algorithm is visualised in Fig. 3.2.

Sample state

Run optimisation

Select first input

Apply to plant

MPCPlant

x[k]

U?k

u?[k|k]

State measurement

Control action

Figure 3.2: MPC Algorithm

3.1.5 Disturbance Preview

Future external signals, which are known in advance, can be included in the pre-

diction. This is referred to as preview. In this thesis, preview of predictable

disturbances is introduced into the MPC. This is done by extending the linear

discrete prediction model to include predicted disturbances w:

x[k + 1] = Ax[k] + Bu[k] + Ww[k] (3.9)

The sequence of predicted disturbances over the prediction horizon

Wk := {w[k], w[k + 1], . . . , w[k +N ]} (3.10)

needs to be provided to the optimisation problem. Since there is no influence over

the disturbances the cost function J can remain unchanged. Equation (3.8) can


be rewritten:

U?k (x[k],Wk) = arg minUk

J(Xk,Uk)

s.t.

x[j + 1|k] = Ax[j|k] + Bu[j|k] + Ww[k] ∀j = k . . . k +N

x[k|k] = x[k]

x ≤ x ≤ x ∀x ∈ Xku ≤ u ≤ u ∀u ∈ Uk

(3.11)

3.1.6 Infinite Horizon Cost

In the quadratic cost function Eq. (3.4) all states are penalised with the same

weighing matrix Q. It is, however, often desired to associate a different cost

(terminal cost) with the final predicted state x[k + N |k] := xt (terminal state).

The idea behind this is to approximate the cost over an infinite time horizon.

Terminal cost can be included in the cost function by introducing the terminal

weighing matrix Qt:

J(Xk,Uk) =∑

x∈Xk\xt

xTQx +∑u∈Uk

uTRu + xTt Qtxt︸︷︷︸terminal cost

(3.12)

If Qt is chosen correctly, the terminal cost represent the cost of the unconstrained

control problem over an infinite horizon by assuming the linear feedback law

u[i] = Kx[i] ∀i = k +N, k +N + 1, . . . (3.13)

The terminal weighing matrix Qt needs to satisfy the equation [7]

Qt − (A + BK)TQt(A + BK) = Q + KTRK (3.14)

where A and B are the system matrices of the prediction model Eq. (3.1).

If the linear feedback law K is the Linear Quadratic Regulator (see [4]), the ter-

minal cost represents the optimal cost over an infinite horizon disregarding con-

straints.

3.2 PILIM Controller

The Performance of the MPC scheme is compared to a classical feedback controller.

The control strategy chosen for comparison is called Proportional Integral Limited


(PILIM) control. The PILIM controller was developed at Heriot-Watt University

by P. Bellec [3]. It was especially designed for unmanned underwater vehicles.

The PILIM-controller is visualised in Fig. 3.3. It is a position and velocity con-

troller hybrid, where all degrees-of-freedom are controlled separately. The con-

troller consists of two loops. The outer loop is the position controller with a

proportional component. The outer loop dictates the reference for the velocity,

depending on the distance to the goal position. The velocity reference is limited

by a saturation block to the maximum speed of the vehicle. The inner loop is a

PI-controller for velocity control. An important component to include is integrator

anti wind-up to account for constraints such as maximum velocity and thruster

limitations.

ηref ∑KηP

Speed Limit ∑KνP

KνI

∫∑ Thruster Limit

AUV

− −

η ν

η

ν

Position Control Speed Control

Figure 3.3: PILIM Blockdiagram.

Correctly tuned, the PILIM controller steers each degree-of-freedom to the refer-

ence position without any overshoot.

Chapter 4

Simulation

This chapter covers the simulation of the closed loop system with the purpose to

evaluate the Model Predictive Control approach for a station keeping scenario.

First, the experimental setup and procedure is explained. This also includes a

description of the PILIM controller, a classical feedback control scheme used for

comparison. The chapter continues with a presentation of the simulation results

and closes with a discussion.

4.1 Implementation Details

The MPC controller is evaluated in simulation. For comparison, the experiments

are repeated with a PILIM controller, a classical control scheme that was developed

especially for underwater vehicles. The closed loop simulations are implemented

in MATLAB SIMULINK. A description is given in this section.

4.1.1 Vehicle Simulator

A general presentation of the underwater vehicle model is given in Chapter 2

and the parametrised model is provided in Appendix A. The model is non-linear,

coupled and describes motion and absolute position of the vehicle in six-DoF.

The dynamic model is implemented with the state-space equation x = f(x, u,w)

(Eq. (2.39)) in a feedback loop with an integrator. This model is used to simulate

the underwater vehicle. In SIMULINK, the state-space equation is implemented

as an interpreted MATLAB function and called continuously. The block-diagram

representation of the vehicle simulator is shown as part of Fig. 4.1.

25

Chapter 4. Simulation 26

4.1.2 MPC Controller and Disturbances

The MPC scheme is described in Chapter 3 and based upon a discrete linear

prediction model. This model is obtained by linearising and discretising the non-

linear simulation model according to the methods described in Appendix B. The

MPC controller consists of a sampling block and the optimiser, which solves the

optimisation problem with preview Eq. (3.11).

A moving manipulator is included in the simulation by considering the interaction

forces and moments as a predictable disturbance. The disturbance is defined as

a continuous function w(t), which is given to the vehicle simulator. Additionally,

for disturbance preview (see Section 3.1.5), the sequence of predicted disturbances

over the prediction horizon Wk (Eq. (3.10)) is provided to the optimiser. In the

experiment performance is to be evaluated at different levels of certainty about

the disturbance. That is, the sequence of predicted disturbances is defined as

Wk = {w[k], w[k + 1], . . . , w[k +N ]}= {cw((kTs), cw((k + 1)Ts), . . . , cw((k +N)Ts)}

(4.1)

where w(t) is the real disturbance and c is the certainty factor. With a certainty

factor of c = 100%, the disturbance is completely included in the prediction, while

a factor of c = 0 means no preview of the disturbance at all.

A block diagram of the closed loop system is shown in Fig. 4.1.

TsOptimiser f(x, u,w)

∫x[k] u?[k] x(t)

x(t)

w(t)Wk

Disturbance

MPC Vehicle Simulator

Figure 4.1: MPC Closed Loop Block Diagram. The signal names follow thenotation introduced in Chapter 3.

In SIMULINK, the optimiser is implemented in the YALMIP toolbox [16] and uses

MATLAB’s quadprog [27] to solve the optimisation problem. The optimiser is run

as an interpreted MATLAB function at each sampling instance in SIMULINK. The

sampling is realised by framing the optimiser routine with two Zero-Order Hold

blocks.


MPC Tuning

The MPC tuning parameters are listed in Table 4.1. From the parametrisation of

the state weighing matrix Q it can be seen that velocity states ν are not penalised.

Hence, only position control is considered and velocity control is disregarded. Fur-

thermore, the use of inputs is penalised seven orders of magnitude less than state

deviation. This is partly attributed to the control objective, i.e. station keeping,

but also due to the range of values for thruster forces and expected state deviation.

The numerical value of the inputs are up to four orders of magnitude larger than

the state deviation. A sampling time of 0.2 seconds is chosen and the prediction

horizon has a length of ten time steps. Thus, the MPC considers trajectories that

reach two seconds into the future. The states are unconstrained. However, input

constrains are included, which refer to the thruster limitation with a maximum

force production of 50 Newton.

Parameter Symbol Values

State Weighing Matrix Q diag

0, 0, 0, 0, 0, 0︸︷︷︸Velocity ν

, 5, 5, 10, 1, 40, 1︸︷︷︸Position η

Input Weighing Matrix R 5 · 10−7 · I6×6Prediction Horizon N 10 time stepsSampling Time Ts 0.2 s

Input constraints u = −u[50 50 50 50 50 50

]TN

State constraints - unconstrained

Table 4.1: MPC Tuning Parameter

4.1.3 PILIM Controller

The basic idea of the PILIM controller is stated in 3. The control scheme is

implemented with continuous blocks in Matlab Simulink. The PILIM is tuned

individually for each of the six degrees-of-freedom. The tuning is performed such

that any overshoot is vanished. The performance is exemplary shown for position

control in surge and orientations control in pitch in Fig. 4.2. The parameter values

are given in Table 4.2. Parameter associated with the roll-DoF are set to zero,

hence roll control is not considered. This is because roll is uncontrollable with the

proposed thruster configuration.


time [s]0 2 4 6 8 10

posi

tion

[m]

0

1

2

3

4

5Surge Tuning

time [s]0 2 4 6 8 10

orie

ntat

ion

[deg

]

0

10

20

30

40

50Pitch Tuning

Figure 4.2: PILIM-controller performance for position (orientation) control.As can be seen, the reference is reached without any overshoot.

Parameter Symbol Values

Position Control: P-Part KηP

[1.85 1 1 0 4 5

]TVelocity Control: P-Part Kν

P

[3500 3500 3500 0 1000 3000

]TVelocity Control: I-Part Kν

I

[300 300 300 0 100 300

]T

Speed Limit νmax

1.25 m s−1

0.25 m s−1

0.17 m s−1

0 deg s−1

22.5 deg s−1

36 deg s−1

Thruster Limit umax

[50 50 50 50 50 50

]TN

Table 4.2: PILIM Tuning Parameter. The values for each parameter differdepending on the associated degree-of-freedom. Only the thruster limits are

independent from the degree-of-freedom and set for all thrusters.

4.2 Control Objective and Experimental Set-up

The control objective in the set of experiments is station-keeping: The vehicle

is supposed to hold a position and orientation as close as possible. The desired

position and orientation is

ηref =[xref yref zref φref θref ψref

]T=[0 0 0 0 0 0

]T(4.2)

The vehicle is affected by disturbing forces and moments. These disturbances are

meant to reflect the interaction effects caused by a moving manipulator. According

to [2], the most affected degree-of-freedom is pitch, i.e. rotation around the lateral

axis. For this reason, the experiments are performed with a disturbing moment


acting on the pitch. Furthermore, the experiments are repeated with the distur-

bance acting on a translatory degree-of-freedom. For this the disturbing force is

affecting the surge-DoF.

Both pitch and surge-disturbance have the same shape: they are initiated with

0 N m (respectively 0 N). At time t = 5 s they start to rise uniformly with a slope of

4 N m s−1 (respectively 4 N s−1) and reach their maximum of 20 N m (respectively

20 N) at time t = 10 s. The disturbances stay constant for the rest of the experi-

ment. Exemplarily, the disturbing moment acting on the pitch-DoF is visualised

in Fig. 4.3. The pitch-disturbance has identical numerical values but, of course,

the unit needs to be changed from newton-metre to newton.

Time [s]0 5 10 15 20

Mom

ent [

Nm

]

0

5

10

15

20

Figure 4.3: The disturbing moment acting on the vehicle during the experi-ment.

4.3 Results

In this section simulation results from the experiments with disturbances on pitch

and surge are presented and compared. Most MPC results will show a significant

offset. This is a characteristic behaviour of MPC in the presence of disturbances

due to the lack of integral action. This phenomenon is discussed later and not

considered in the performance evaluation. Instead, performance is evaluated in

terms of peak amplitude.

4.3.1 Disturbing Moment on Pitch

Open Loop

In open loop, the heave thrusters produce just the right amount of force to coun-

terbalance the restoring forces. This, however, is assuming a neutral position,


i.e. no roll or pitch inclination. The disturbance causes a pitch-angle (θ) of over

40 degrees. As a result, the force produced by the heave-thrusters is not directed

vertically any longer and causing the vehicle to accelerate in surge-direction (along

the x-axis). Furthermore, the restoring forces are not fully counterbalances, caus-

ing the vehicle to rise, i.e. move in negative z-direction, due to buoyancy.

time [s]0 10 20 30 40

posi

tion

[m]

-20

-10

0

10

20

30Open Loop

x y z (depth)

time [s]0 10 20 30 40

orie

ntat

ion

[deg

]

0

10

20

30

40

50Open Loop

? 3 A

Figure 4.4: Pitch disturbance, Open loop

PILIM Controller

The PILIM controller reduces the maximum pitch-angle to 0.2 degree. It further

limits the translational movement to a fraction of a millimetre.

time [s]0 10 20 30 40

posi

tion

[m]

#10-5

-1

0

1

2

3PILIM

x y z (depth)

time [s]0 10 20 30 40

orie

ntat

ion

[deg

]

-0.05

0

0.05

0.1

0.15

0.2

0.25PILIM

? 3 A

Figure 4.5: Pitch disturbance, PILIM controller


MPC Controller

When the controller has full knowledge about the disturbance, the station-keeping

performance is excellent. The effects of the manipulator are reduced to the order

of noise.

time [s]0 10 20 30 40

posi

tion

[m]

#10-4

-8

-6

-4

-2

0

2MPC (c=100%)

x y z (depth)

time [s]0 10 20 30 40

orie

ntat

ion

[deg

]

#10-3

-5

0

5

10

15MPC (c=100%)

? 3 A

Figure 4.6: Pitch disturbance, MPC controller with 100 % certainty

However, performance degrades quickly, when the prediction scheme is provided

with only partial knowledge of the disturbances.

time [s]0 10 20 30 40

posi

tion

[m]

#10-4

-8

-6

-4

-2

0

2MPC (c=99%)

x y z (depth)

time [s]0 10 20 30 40

orie

ntat

ion

[deg

]

#10-3

-5

0

5

10

15

20MPC (c=99%)

? 3 A



time [s]0 10 20 30 40

posi

tion

[m]

#10-4

-6

-4

-2

0

2MPC (c=95%)

x y z (depth)

time [s]0 10 20 30 40

orie

ntat

ion

[deg

]

-0.01

0

0.01

0.02

0.03

0.04

0.05MPC (c=95%)

? 3 A


time [s]0 10 20 30 40

posi

tion

[m]

#10-4

-6

-4

-2

0

2MPC (c=90%)

x y z (depth)

time [s]0 10 20 30 40

orie

ntat

ion

[deg

]

-0.02

0

0.02

0.04

0.06

0.08

0.1MPC (c=90%)

? 3 A


time [s]0 10 20 30 40

posi

tion

[m]

#10-4

-6

-4

-2

0

2MPC (c=80%)

x y z (depth)

time [s]0 10 20 30 40

orie

ntat

ion

[deg

]

-0.05

0

0.05

0.1

0.15

0.2

0.25MPC (c=80%)

? 3 A


With a certainty factor of c = 80 %, the MPC and the PILIM controller show

similar performance in terms of peak amplitude of the pitch-angle. At even lower

certainty levels, the peak pitch-angle rises to single digits values.


time [s]0 10 20 30 40

posi

tion

[m]

#10-4

-8

-6

-4

-2

0

2MPC (c=70%)

x y z (depth)

time [s]0 10 20 30 40

orie

ntat

ion

[deg

]

-0.1

0

0.1

0.2

0.3

0.4MPC (c=70%)

? 3 A


time [s]0 10 20 30 40

posi

tion

[m]

#10-4

-6

-4

-2

0

2MPC (c=50%)

x y z (depth)

time [s]0 10 20 30 40

orie

ntat

ion

[deg

]

-0.5

0

0.5

1

1.5MPC (c=50%)

? 3 A


Without preview (c = 0 %), the pitch-angle peaks at 5.4 degrees. Furthermore,

there is a 2 millimetre shift on the vertical axis.

time [s]0 10 20 30 40

posi

tion

[m]

#10-3

-1

0

1

2

3MPC (c=0%)

x y z (depth)

time [s]0 10 20 30 40

orie

ntat

ion

[deg

]

-2

0

2

4

6MPC (c=0%)

? 3 A



4.3.1.1 Use of Input

In Fig. 4.14, the input usage in terms of heave-thruster actuation is visualised

together with the pitch-angle development for the MPC and PILIM controller.

Both input curves show a similar characteristic overall: Right after the beginning

of the experiment, in both cases, the two heave-thrusters produce a force of about

30 N each. This is in order to counterbalance the restoring-forces. Between 5 and

10 seconds, the thruster output diverges, corresponding to the applied disturbance.

After 10 seconds, when the disturbance remains constant, the thruster outputs

settle with a difference of about 34 N.

Comparing the use of input under both controllers, the effects of the time discreti-

sation and zero-order-hold are clearly visible: While the MPC controller selects

a new input only at discrete time steps, resulting in the staircase characteristic

of the input curves, the PILIM input curve is smooth without any jumps. The

effect of the disturbance preview can be observed: The thruster start to react

to the disturbing moment before it is actually applied. On the other hand, the

PILIM controller can only react to the disturbance, thus thrusters are actuated

accordingly only after 5 seconds.

time [s]0 5 10 15 20 25

orie

ntat

ion

[deg

]

#10-3

-2

0

2

4

6

8

10

12

14

16MPC (c=100%)

thru

ster

forc

e (h

eave

) [N

]

10

15

20

25

30

35

40

45

50

time [s]0 5 10 15 20 25

orie

ntat

ion

[deg

]

0

0.05

0.1

0.15

0.2

0.25PILIM

thru

ster

forc

e (h

eave

) [N

]

10

15

20

25

30

35

40

45

50

Figure 4.14: Input usage and pitch-angle for MPC and PILIM in comparison.

4.3.2 Disturbing Force on Surge

Open Loop

In the case of a disturbing force on the surge-DoF, the orientation of the vehicle is

not affected. That is why, in contrast to the pitch-disturbance, the restoring forces

can still be counterbalanced in open loop. In consequence, the only observable


effect is on the surge-position, i.e. x-position. During the 40 seconds of the

experiment, the vehicle moves by 20 metres.

time [s]0 10 20 30 40

posi

tion

[m]

0

5

10

15

20Open Loop

x y z (depth)

time [s]0 10 20 30 40

orie

ntat

ion

[deg

]

-1

-0.5

0

0.5

1Open Loop

? 3 A

Figure 4.15: Pitch disturbance, Open loop

PILIM Controller

The PILIM controller confines the movement to a few millimetre. Other degrees-

of-freedom are not affected to a notable degree.

time [s]0 10 20 30 40

posi

tion

[m]

#10-3

-0.5

0

0.5

1

1.5

2

2.5PILIM

x y z (depth)

time [s]0 10 20 30 40

orie

ntat

ion

[deg

]

#10-7

-1.5

-1

-0.5

0

0.5

1

1.5PILIM

? 3 A

Figure 4.16: Pitch disturbance, PILIM controller

MPC Controller

When full knowledge about the disturbance is assumed, i.e. a certainty factor

of c = 100 %, the MPC controller shows excellent performance. The effects on

translatory DoF are in the order of a some tenth of a millimetre. Interestingly, the


heave-DoF seems to be more heavily affected than the surge-DoF. This, however,

is primarily caused by the restoring forces and not by the disturbance: The same

magnitude can be observed at lower certainty levels, or in the pitch-disturbance

experiment. Furthermore, MPC control introduces low-amplitude oscillations on

the pitch-angle.

time [s]0 10 20 30 40

posi

tion

[m]

#10-4

-8

-6

-4

-2

0

2MPC (c=100%)

x y z (depth)

time [s]0 10 20 30 40

orie

ntat

ion

[deg

]

#10-3

-2

-1

0

1

2MPC (c=100%)

? 3 A


At lower certainty levels, the peak shift in x-direction increases.

time [s]0 10 20 30 40

posi

tion

[m]

#10-4

-6

-4

-2

0

2

4MPC (c=99%)

x y z (depth)

time [s]0 10 20 30 40

orie

ntat

ion

[deg

]

#10-4

-15

-10

-5

0

5MPC (c=99%)

? 3 A



time [s]0 10 20 30 40

posi

tion

[m]

#10-4

-5

0

5

10MPC (c=95%)

x y z (depth)

time [s]0 10 20 30 40

orie

ntat

ion

[deg

]

#10-4

-15

-10

-5

0

5MPC (c=95%)

? 3 A


time [s]0 10 20 30 40

posi

tion

[m]

#10-3

-1

-0.5

0

0.5

1

1.5MPC (c=90%)

x y z (depth)

time [s]0 10 20 30 40

orie

ntat

ion

[deg

]

#10-4

-15

-10

-5

0

5MPC (c=90%)

? 3 A


time [s]0 10 20 30 40

posi

tion

[m]

#10-3

-1

0

1

2

3MPC (c=80%)

x y z (depth)

time [s]0 10 20 30 40

orie

ntat

ion

[deg

]

#10-4

-15

-10

-5

0

5MPC (c=80%)

? 3 A


Similar to the pitch-disturbance experiment, with a certainty factor of 80 % the

MPC and the PILIM controller show similar peak amplitude in x-position. Low-

ering the certainty level further, quickly leads to a deviation of several millimetre,

up to a few centimetre.


time [s]0 10 20 30 40

posi

tion

[m]

#10-3

-1

0

1

2

3

4

5MPC (c=70%)

x y z (depth)

time [s]0 10 20 30 40

orie

ntat

ion

[deg

]

#10-4

-15

-10

-5

0

5MPC (c=70%)

? 3 A


time [s]0 10 20 30 40

posi

tion

[m]

#10-3

-2

0

2

4

6

8

10MPC (c=50%)

x y z (depth)

time [s]0 10 20 30 40

orie

ntat

ion

[deg

]

#10-3

-1

-0.5

0

0.5

1

1.5MPC (c=50%)

? 3 A


Without any preview (c = 0 %), the vehicle diverges from the reference position

by 3 cm.

time [s]0 10 20 30 40

posi

tion

[m]

-0.01

0

0.01

0.02

0.03

0.04MPC (c=0%)

x y z (depth)

time [s]0 10 20 30 40

orie

ntat

ion

[deg

]

#10-4

-5

0

5

10

15MPC (c=0%)

? 3 A


4.3.2.1 Use of Input

In Fig. 4.25, input usage in terms of surge-thruster output and the curve for

x-position are shown in comparison for MPC and PILIM. Similar to the pitch-

disturbance experiment, the overall characteristics of the input curves for MPC

and PILIM are similar. Under both control laws, the two surge-thrusters, be-

have identical. As long as there is no disturbance there is no need for actuation,

thus the thrusters start with an output of 0 N. With the rising disturbance, the

thruster forces increase. The output settles at approximately 10 N per thruster to

counteract the 20 N disturbance.

In case of the MPC controller, it can again be seen that the thrusters are actuated

before the disturbance is applied due to preview.

time [s]0 5 10 15 20 25

posi

tion

[m]

#10-5

-5

0

5

10

15

20MPC (c=100%)

thru

ster

forc

e (s

urge

) [N

]

-12

-10

-8

-6

-4

-2

0

2

time [s]0 5 10 15 20 25

posi

tion

[m]

#10-3

0

0.5

1

1.5

2

2.5PILIM

thru

ster

forc

e (s

urge

) [N

]

-12

-10

-8

-6

-4

-2

0

2

Figure 4.25: Input usage and x-position curve for MPC and PILIM incomaprison.

4.4 Discussion

Looking at the performance of the PILIM controller, it can be said that it is a

potent solution to the station-keeping problem. The affected DoF is limited to

an order of magnitude that becomes undistinguishable from noise. Furthermore,

the PILIM controller does a great job at stabilising the remaining position and

orientation states.

The performance of the MPC controller on the other hand depends on the de-

gree of knowledge the predictive scheme has about the disturbance. With full

knowledge the performance is excellent. In both experiments, the MPC controller

outperforms the PILIM controller in terms of maximum amplitude of the affected


DoF for a certainty factor of 80 % or above. Unknown disturbances, however,

are not handled very well compared to the PILIM controller. Without preview,

the disturbances cause a pitch-inclination of 5 degrees and a translatory move-

ment of 3 centimetre. This offset is noticeable and, depending on the application,

potentially intolerable.

Another issue of the MPC scheme is its lack of integral action. As a result, a

steady-state error will remain in the presence of constant disturbances or modelling

errors. There are extensions to the standard scheme that enable offset-free control.

A detailed discussion is provided in [4]. In short, offset-free MPC is realised by

considering a disturbance model in the prediction and the cost-function. The

disturbance is estimated with an observer in the loop.

The performance issues of MPC can be attributed to differences between the linear

prediction model and the non-linear simulator. Furthermore, the sampling time

degrades performance in the presence of continuously changing disturbances.

Overall, the PILIM controller sufficiently handles the station-keeping problem.

The MPC routine performs only better when disturbances can be predicted with

a high degree of certainty. Additionally, the MPC controller is computationally

expensive. In conclusion, the PILIM controller is to be preferred over MPC in the

station-keeping application when disturbances are not known or the computational

burden plays an important role.

Chapter 5

Conclusion

In this thesis, a Model Predictive Control approach for station-keeping of an un-

derwater vehicle-manipulator system was evaluated. A disturbance representing

the movement of the manipulator was applied in simulation. The performance of

the MPC scheme when including the disturbance into the prediction to different

degrees was examined. Performance was evaluated in terms of peak deviation

from the desired position and orientation. Simulation showed excellent results

when disturbances are known to a high degree. When less than 80 % of the distur-

bance is included in the prediction, MPC is outperformed by a PILIM controller,

a classical feedback control scheme. MPC has the advantage over PILIM that con-

straints and, for example, thruster dynamics can easily be included in the control

scheme. The better performance of the PILIM controller can partly be attributed

to the assumption made in this thesis that thruster dynamics can be disregarded.

Another reason are the differences between the non-linear simulation model and

the linearised prediction model used by the MPC routine. A drawback of MPC is

its large computational burden due to the optimisation. In conclusion, the PILIM

controller is better suited for the station-keeping task than MPC when distur-

bances are unpredictable and under the assumption that thruster dynamics can

be disregarded.

Nonetheless, the work presented in this thesis suggests possibilities for the use of

MPC in the control of vehicle-manipulator systems. However, this will require

improvements on the MPC routine, such as the introduction of integral action for

offset-free control and a prediction scheme that better approximates the actual

vehicle dynamics.

41

Chapter 5. Conclusion 42

5.1 Future Work

Although station-keeping can be sufficiently handled by more simplistic control

strategies such as the PILIM controller, there are some advantages of MPC that

were not exploited in this thesis, e.g.:

• Thruster rate limitations, which have a significant impact, can easily be in-

cluded with MPC, either by defining constraints accordingly or by including

thruster dynamics into the model.

• While the PILIM controller considers each degree-of-freedom separately,

MPC has the ability for coupled control, thus considering the entire vehicle

in six-DoF.

• Although a complex problem, coordinated control of vehicle and manipulator

is possible with MPC. This opens many opportunities for control of vehicle-

manipulator systems.

When further considering MPC for vehicle-manipulator systems, it might be ad-

vantageous to consider work on coordinated control of both vehicle and manipu-

lator, and focusing on the strengths of MPC over traditional control schemes. In

the following, two suggestions for advanced coordinated control tasks for a UVMS

are given. In these examples, the system is charged with the task of drilling on a

horizontal surface.

• Instead of first moving the vehicle to proximity of the objective and then

deploying the manipulator, both subsystems can be actuated simultaneously

to save time and fuel.

• While drilling, only the endeffector needs to hold its position. The vehicle can

change position and orientation given that the kinematic chain can adjust

for these movements. This allows the vehicle to align with water currents.

Furthermore, when a certain tolerance is allowed, thrusters do not need to be

actuated abruptly. This reduces fuel consumption and prevents the thrusters

from wearing out prematurely.

Appendix A

Model of the Fictional Vehicle

A.1 Model

The non-linear model derived in Chapter 2 is presented here in parametrised form.The following parameter were used:

Environment :

Water density : ρ = 1000 kg m−3

Earth′s gravity : g = 9.8 m s−2

Vehicle :

Vehicle length : l = 1.5 m

Vehicle radius : r = 0.15 m

Vehicle mass : m = 100 kg

Centre of gravity : ~rG =[0 0 0

]Tm

Centre of bouyancy : ~rB =[0 0 −0.03

]Tm

Thruster allocation :

Surge thruster, port : ~rT1 =[0 0.2 0

]Tm, ~rF1 =

[1 0 0

]TSurge thruster, starboard : ~rT2 =

[0 −0.2 0

]Tm, ~rF2 =

[1 0 0

]TSway thruster, bow : ~rT1 =

[0.7 0 0

]Tm, ~rF1 =

[0 1 0

]TSway thruster, stern : ~rT1 =

[−0.7 0 0

]Tm, ~rF1 =

[0 1 0

]THeave thruster, bow : ~rT1 =

[0.6 0 0

]Tm, ~rF1 =

[0 0 1

]THeave thruster, stern : ~rT1 =

[−0.6 0 0

]Tm, ~rF1 =

[0 0 1

]T

The model follows state-space notation

x = f(x, u,w) (A.1)

43

Appendix A. Model of the Fictional Vehicle 44

with

States : x =[u v w p q r x y z φ θ ψ

]TInputs : u =

[F1 F2 F3 F4 F5 F6

]TDisturbances : w =

[w1 w2 w3 w4 w5 w6

]TThe individual functions for each state-derivative are given in Eqs. (A.2) to (A.13)

u = 9.1 · 10−3F1 + 9.1 · 10−3F2 + 9.1 · 10−3w1 + 0.54 sin θ . . . (A.2)

− 2.6qw + 2.6rv − 9.1 · 10−3u(28.3|u|+ 15)

v = 3.4 · 10−3F3 + 3.4 · 10−3F4 + 3.4 · 10−3w2 . . . (A.3)

− 3.4 · 10−3v(675|v|+ 20)− 0.2 cos θ ∗ sinφ+ pw − 0.38ru

w = 3.4 · 10−3F5 + 3.4 · 10−3F6 + 3.4 · 10−3w3 − 0.2 cosφ cos θ . . . (A.4)

− 3.4 · 10−3w(675|w|+ 20)− pv + 0.38qu

p = 0.3w4 − 1.5p− 9.2 cos θ sinφ (A.5)

q = 0.01F6 − 0.01F5 + 0.017w5 − 0.53 sin θ . . . (A.6)

− 0.017q(112.5|q|+ 20) + 0.94pr

r = 3.4 · 10−3F2 − 3.4 · 10−3F1 + 0.012F3 − 0.012F4 + 0.017w6 . . . (A.7)

− 0.017r(112.5|r|+ 20)− 0.94pq

x = w(sinφ sinψ + cosφ cosψ sin θ)− v(cosφ sinψ − cosψ sinφ sin θ) . . . (A.8)

+ u cosψ cos θ

y = v(cosφ cosψ + sinφ sinψ sin θ)− w(cosψ sinφ− cosφ sinψ sin θ) . . . (A.9)

+ u cosψ cos θ

z = w cosφ cos θ − u sin θ + v cos θ sinφ (A.10)

φ = p+ r cosφ tan θ + q sinφ tan θ (A.11)

θ = q cosφ− r sinφ (A.12)

ψ =r cosφ+ q sinφ

cos θ(A.13)

A.2 Behaviour Analysis

A.2.1 Restoring Forces

In the first set of experiments, no input to the thruster is given, i.e. only therestoring forces are acting on the vehicle.


time [s]0 2 4 6 8 10

velo

city

[m/s

]

-0.3

-0.25

-0.2

-0.15

-0.1

-0.05

0

heave

time [s]0 2 4 6 8 10

posi

tion

[m]

-3

-2.5

-2

-1.5

-1

-0.5

0

z (depth)

Figure A.1: Heave velocity and depth of the uncontrolled vehicle.

With no thruster input, the vehicle will start to rise with a maximum velocity of2.8 m s−1. After 10 seconds, the vehicle will rise 2.5 metres. This behaviour is tobe expected, since the vehicle is designed to be positively buoyant. Other DoF arenot affected.

The next two experiments demonstrate the restoring capabilities for the roll andpitch angle. Since the centre of buoyancy is placed above the centre of gravity,both angles are selfstabilising. In the experiments, the angles are initialised withan inclination of 45 degrees.

time [s]0 2 4 6 8 10

orie

ntat

ion

[deg

]

-20

-10

0

10

20

30

40

50

?

time [s]0 5 10 15 20 25 30

orie

ntat

ion

[deg

]

-10

0

10

20

30

40

50

3

Figure A.2: Restoring capabilities of roll and pitch angle

The roll angle restores quicker than the pitch angle due to a higher velocity andshows more oscillation. This can be attributed to the lack of added mass andsmaller damping on the roll axis compared to the pitch axis.


A.2.2 Maximum Velocity

In the following experiments, the acceleration and maximum velocity for differentDoF is to be determined. For this purpose, the corresponding thruster are set toproduce maximum force. The maximum force one thruster can produce is assumedto be 50 N.

Linear Motion

time [s]0 2 4 6 8 10

velo

city

[m/s

]

0

0.5

1

1.5

2

surge

time [s]0 2 4 6 8 10

posi

tion

[m]

0

5

10

15

x

Figure A.3: Surge under maximum thrust

The maximum surge velocity is 1.6 m s−1. Maximum speed is reached after ap-proximately 6 seconds.

time [s]0 1 2 3 4 5 6

velo

city

[m/s

]

0

0.1

0.2

0.3

0.4

sway

time [s]0 1 2 3 4 5 6

posi

tion

[m]

0

0.5

1

1.5

2

y

Figure A.4: Sway under maximum thrust

The maximum sway velocity is with 0.37 m s−1 significantly slower than the surgevelocity. This is due to the higher cross-sectional area in sway lateral direction.This increases the added mass and the damping.


The heave acceleration is affected by the restoring forces. Consequently, the max-imum velocity is different in positive and negative direction.

time [s]0 1 2 3 4 5 6 7

velo

city

[m/s

]

0

0.05

0.1

0.15

0.2

0.25

heave

time [s]0 1 2 3 4 5 6 7

posi

tion

[m]

0

0.2

0.4

0.6

0.8

1

1.2

1.4

z (depth)

Figure A.5: Heave under maximum thrust (positive direction)

In neutral position (i.e. no roll and pitch inclination), positive heave correspond toan increase in depth. The experiment show a maximum sinking-rate of 0.23 m s−1.

time [s]0 1 2 3 4

velo

city

[m/s

]

-0.5

-0.4

-0.3

-0.2

-0.1

0

heave

time [s]0 1 2 3 4

posi

tion

[m]

-2

-1.5

-1

-0.5

0

z (depth)

Figure A.6: Heave under maximum thrust (negative direction)

In negative heave direction, corresponding to a rising vehicle, the buoyancy-forceis added to the thruster force. In consequence, a higher speed of 0.47 m s−1 isobserved.

Angular motion

The roll angle is not included in this analysis since it is uncontrollable with thespecific thruster allocation.


time [s]0 5 10 15 20

angu

alr

velo

city

[deg

/s]

0

10

20

30

40

50

pitch

time [s]0 5 10 15 20

orie

ntat

ion

[deg

]

0

100

200

300

400

500

600

700

3

Figure A.7: Pitch under maximum thrust

The restoring forces acting on the pitch angle induce oscillation in the speed curve.The maximum speed depends on the orientation of the vehicle and is between25 deg s−1 and 46 deg s−1.

time [s]0 0.5 1 1.5 2 2.5 3

angu

alr

velo

city

[deg

/s]

0

10

20

30

40

50

yaw

time [s]0 0.5 1 1.5 2 2.5 3

orie

ntat

ion

[deg

]

0

20

40

60

80

100

120

140

A

Figure A.8: Pitch under maximum thrust

The maximum yaw speeds is 47 deg s−1. This can be reached by actuating allfour vertically aligned thruster. From standstill the vehicle can turn 90 degrees in2.3 seconds.

Summery

The following values for maximum velocity and acceleration time were obtainedfrom the experiments:


DoF Max. Velocity Acceleration time

Surge 1.6 m/s ≈ 6 sSway 0.37 m/s ≈ 4 sHeave (pos) 0.23 m/s ≈ 5 sHeave (neg) 0.47 m/s ≈ 3.5 sPitch 25 - 46 deg/s -Yaw 46 deg/s ≈ 1.5 s

A.2.3 Deceleration Curve

In this set of experiments, the vehicle is initialised with maximum speed in theaccording DoF. The thrusters are not actuated and the vehicle will decelerate tostandstill.

Linear Motion

time [s]0 10 20 30 40 50

velo

city

[m/s

]

0

0.5

1

1.5

2

surge

time [s]0 10 20 30 40 50

posi

tion

[m]

0

1

2

3

4

5

6

x

Figure A.9: Surge deceleration curve

time [s]0 10 20 30 40 50 60

velo

city

[m/s

]

0

0.1

0.2

0.3

0.4

sway

time [s]0 10 20 30 40 50 60

posi

tion

[m]

0

0.2

0.4

0.6

0.8

1

1.2

y

Figure A.10: Sway deceleration curve


With no forces acting on the vehicle, both surge and sway velocity show slowdeceleration rates. From full speed it will take the vehicle about 40 seconds to cometo a complete hold in surge direction, and 50 seconds in sway. This correspondsto a travelled distance of 5.5 metres for surge and 1.1 metres in sway.

time [s]0 1 2 3 4 5

velo

city

[m/s

]

-0.3

-0.2

-0.1

0

0.1

0.2

0.3

heave

time [s]0 1 2 3 4 5

posi

tion

[m]

-0.8

-0.6

-0.4

-0.2

0

0.2

z (depth)

Figure A.11: Heave deceleration curve (maximum positive velocity)

time [s]0 2 4 6

velo

city

[m/s

]

-0.5

-0.45

-0.4

-0.35

-0.3

-0.25

heave

time [s]0 1 2 3 4 5 6

posi

tion

[m]

-2

-1.5

-1

-0.5

0

z (depth)

Figure A.12: Heave deceleration curve (maximum negative velocity)

The heave velocity settles one order of magnitude quicker than surge or sway. Thiscan be explained by the restoring forces which quickly settle the heave velocity tothe rising rate of 0.28 m s−1 for the uncontrolled vehicle.

Angular motion

Only the yaw angle is analysed since roll is uncontrollable and the stabilisingproperties of the pitch angle were analysed previously.


time [s]0 5 10 15 20

angu

alr

velo

city

[deg

/s]

0

10

20

30

40

50

yaw

time [s]0 5 10 15 20

orie

ntat

ion

[deg

]

0

10

20

30

40

50

60

A

Figure A.13: Yaw deceleration curve

From full rotational speed, the vehicle comes to a rest after 15 seconds. The vehiclewill keep turning 52 degrees.

A.2.4 Active Braking

The deceleration times in the previous experiment were measure for the unactuatedvehicle. In this experiment, the vehicle actively brakes by producing full thrusterforce in opposite direction. The vehicle is initialised with full speed. In thisexperiment, only surge, arguably the most important DoF, is considered.

time [s]0 1 2 3 4

velo

city

[m/s

]

-1.5

-1

-0.5

0

0.5

1

1.5

2

surge

time [s]0 1 2 3 4

posi

tion

[m]

-1.5

-1

-0.5

0

0.5

1

x

Figure A.14: Surge under active braking

The vehicle manages to come to a hold after 1.3 seconds and a travelled distanceof just under 1 metre.


A.2.5 Drop-Dive Scenario

This experiment simulates the following scenario: The vehicle is dropped into thewater. It enters the water with a downward speed of 3 m s−1 and a pitch inclinationof -45 degrees. The thrusters are not actuated. The surface is assumed to be atzero depth.

time [s]0 2 4 6 8 10

posi

tion

[m]

-0.5

0

0.5

1

1.5

2

2.5

xyz (depth)

time [s]0 2 4 6 8 10

orie

ntat

ion

[deg

]-50

-40

-30

-20

-10

0

10

?

3

A

Figure A.15: Position and orientation in the drop-dive experiment.

The vehicle reaches a depth of 2 metres before starting to rise after 2.5 seconds.It breaks through the surface after 10.3 seconds. Although the vehicle has noinitial momentum in x-direction when it drops into the water, the inclination of45 degrees causes the vehicle to move in x-direction by 2.1 metres before reachingthe surface. Note that the model assumes the vehicle to be always under water, i.e.effects caused by breaking through the surface are not simulated.

Appendix B

Linearisation and Discretisation

This chapter briefly explains the process of linearising and discretising a non-linearstate-space system with disturbances.

B.1 Linearisation

A non-linear state-space system

x = f(x, u,w) (B.1)

can be approximated with a linear state-space system by linearising around aworking point [x, u,w] = [xp, up,wp]. The state, respectively the input, can beexpressed in terms of perturbation from the working point, i.e.

x = xp + x u = up + u w = wp + w (B.2)

For small perturbations x and u, the non-liner dynamics can be approximatedusing Taylor series expansion:

x = f(x, u,w)

≈ f(xp, up,wp)︸︷︷︸xp

+∂f

∂x(xp, up,wp)︸︷︷︸

Ac

·x +∂f

∂u(xp, up,wp)︸︷︷︸

Bc

·u +∂f

∂w(xp, up,wp)︸︷︷︸

Wc

·w (B.3)

With ˙x = x − xp the linear dynamics can be expressed in standard state-spacenotation:

˙x ≈ Acx + Bcu (B.4)

By choosing xp, up and wp such that

xp = f(xp, up,wp) = 0, (B.5)

53

Appendix B. Linearisation and Discretisation 54

that is the working point is an equilibrium of the system, the linear state-spacecan be expressed as

x ≈ Acx + Bcu + Wcw (B.6)

B.2 Discretisation

A continuous linear state-space model

x(t) = Acx(t) + Bcu(t) + Wcw(t) (B.7)

can be transformed to a discrete state-space system

x[k + 1] = Ax[k] + Bu[k] + Ww[k] (B.8)

where

A = eAcTs (B.9)

B =

∫ Ts

0

eAcsds ·Bc (B.10)

W =

∫ Ts

0

eAcsds ·Wc (B.11)

with the sampling time Ts. In this approximation it is assumed that inputs anddisturbances are constant during the sampling interval.

In case of the linear approximation Eq. (B.6), the linear offset needs to be included:

x[k + 1] = A(x[k] + xp[k]) + B(u[k] + up[k]) + W(w[k] + wp[k]) (B.12)

Bibliography

[1] Heriot-Watt University Ocean Systems Laboratory, Nessie 2012 AUV.https://osl.eps.hw.ac.uk/virtualPages/experimentalCapabilities/

Nessie%202012.php. Accessed: 2017-08-25.

[2] C. Barbalata. Modelling and Control of Lightweight Underwater Vehicle-Manipulator Systems. PhD thesis, School of Engineering and Physical Sci-ences, Heriot-Watt University, 2016.

[3] P. Bellec. Simulation of the six-degree-of-freedom motion of a remotely-controlled unmanned submersible. Master’s thesis, Electrical and ElectronicEngineering, Heriot-Watt University, 1980.

[4] F. Borrelli, A. Bemporad, and M. Morari. Predictive control for linear andhybrid systems, 2017.

[5] A. Budiyono. Model predictive control for autonomous underwater vehicle.Indian Journal of Geo-Marine Sciences, 40(2):191–199, 2011.

[6] C. V. Caldwell, D. D. Dunlap, and E. G. Collins. Motion planning for anautonomous underwater vehicle via sampling based model predictive control.In OCEANS 2010 MTS/IEEE SEATTLE, pages 1–6, Sept 2010.

[7] M. Cannon. C21 model predictive control, lecture notes. http://www.eng.

ox.ac.uk/~conmrc/mpc/. Accessed: 2017-05-13.

[8] H. T. Choi, A. Hanai, S. K. Choi, and J. Yuh. Development of an underwaterrobot, odin-iii. In Proceedings 2003 IEEE/RSJ International Conference onIntelligent Robots and Systems (IROS 2003) (Cat. No.03CH37453), volume 1,pages 836–841 vol.1, Oct 2003.

[9] R. Featherstone. Rigid Body Dynamics Algorithms. Springer US, 2008.

[10] D. C. Fernandez and G. A. Hollinger. Model predictive control for underwaterrobots in ocean waves. IEEE Robotics and Automation Letters, 2(1):88–95,Jan 2017.

[11] T. I. Fossen. Guidance and control of ocean vehicles. John Wiley & Sons,Ltd, 1994.

[12] T. I. Fossen. Handbook of marine craft hydrodynamics and motion control.John Wiley & Sons, Ltd, 2011.

55

https://osl.eps.hw.ac.uk/virtualPages/experimentalCapabilities/Nessie%202012.php

https://osl.eps.hw.ac.uk/virtualPages/experimentalCapabilities/Nessie%202012.php

http://www.eng.ox.ac.uk/~conmrc/mpc/

http://www.eng.ox.ac.uk/~conmrc/mpc/

Bibliography 56

[13] S. Heshmati-Alamdari, A. Eqtami, G. C. Karras, D. V. Dimarogonas, andK. J. Kyriakopoulos. A self-triggered visual servoing model predictive controlscheme for under-actuated underwater robotic vehicles. In 2014 IEEE Inter-national Conference on Robotics and Automation (ICRA), pages 3826–3831,May 2014.

[14] K. S. Holkar and L. M. Waghmare. An overview of model predictive control.International Journal of Control and Automation, 3(4):47–63, 2010.

[15] F. Lobo Pereira, J. Borges de Sousa, R. Gomes, and P. Calado. A Model Pre-dictive Control Approach to AUVs Motion Coordination, pages 9–18. SpringerInternational Publishing, Cham, 2015.

[16] J. Lofberg. Yalmip : A toolbox for modeling and optimization in matlab. InIn Proceedings of the CACSD Conference, Taipei, Taiwan, 2004.

[17] T. W. McLain, S. M. Rock, and M. J. Lee. Experiments in the CoordinatedControl of an Underwater Arm/Vehicle System, pages 139–158. Springer US,Boston, MA, 1996.

[18] L. Medagoda and S. B. Williams. Model predictive control of an autonomousunderwater vehicle in an in situ estimated water current profile. In 2012Oceans - Yeosu, pages 1–8, May 2012.

[19] F. Mestemacher. Grundkurs Technische Mechanik. Spektrum AkademischerVerlag.

[20] J. R. Morison, J. W. Johnson, and S. A. Schaaf. The force exerted by surfacewaves on piles.

[21] W. Naeem. Model predictive control of an autonomous underwater vehicle.In Proc. UKACC Conference on Control, 2002.

[22] T. Pennors, S. Batalov, and Y. Petillot. Modeling and control of underwatervehicle: Nessie v. Heriot-Watt University, Ecole des Mines de Nantes, 2011.

[23] C. Shen, Y. Shi, and B. Buckham. Model predictive control for an auv withdynamic path planning. In 2015 54th Annual Conference of the Society of In-strument and Control Engineers of Japan (SICE), pages 475–480, July 2015.

[24] P. Snelgrove. A census of the ocean. https://www.ted.com/talks/paul_

snelgrove_a_census_of_the_ocean#t-986622. Accessed: 2017-08-21.

[25] L. V. Steenson. Experimentally Verified Model Predictive Control of a Hover-Cabable AUV. PhD thesis, Faculty of Engineering and the Environment,University of Southamption, 2013.

[26] L. V. Steenson, S. R. Turnock, A. B. Phillips, C. Harris, M. E. Furlong,E. Rogers, L. Wang, K. Bodles, and D. W. Evans. Model predictive controlof a hybrid autonomous underwater vehicle with experimental verification.Proceedings of the Institution of Mechanical Engineers, Part M: Journal ofEngineering for the Maritime Environment, 228(2):166–179, 2014.

https://www.ted.com/talks/paul_snelgrove_a_census_of_the_ocean#t-986622

https://www.ted.com/talks/paul_snelgrove_a_census_of_the_ocean#t-986622

Bibliography 57

[27] The MathWork, Inc. Quadratic programming, matlab quadprog.https://de.mathworks.com/help/optim/ug/quadprog.html. Accessed: 2017-08-29.

[28] The Society of Naval Architects and Marine Engineers. Nomenclature fortreating the motion of a submerged body through fluid. In Technical andResearch Bulletin No 1-5. 1950.

[29] Q. Truong, L. Wang, and P. Gawthrop. Intermnittent model predictive controlof an autonomous underwater vehicle. In 2006 9th International Conferenceon Control, Automation, Robotics and Vision, pages 1–6, Dec 2006.

[30] H. H. Wang, S. M. Rock, and M. J. Lee. Otter: The design and developmentof an intelligent underwater robot. Autonomous Robots, 3(2):297–320, Jun1996.

[31] A. Weiss, U. Kalabic, and S. D. Cairano. Model predictive control for simul-taneous station keeping and momentum management of low-thrust satellites.In 2015 American Control Conference (ACC), pages 2305–2310, July 2015.

[32] J. Yuh, S. K. Choi, C. Ikehara, G. H. Kim, G. McMurty, M. Ghasemi-Nejhad,N. Sarkar, and K. Sugihara. Design of a semi-autonomous underwater vehi-cle for intervention missions (sauvim). In Proceedings of 1998 InternationalSymposium on Underwater Technology, pages 63–68, Apr 1998.

TRITA EE 2017:154

ISSN 1653-5146

www.kth.se

Model Predictive Control for Six Degrees-of-Freedom Station-Keeping of an Underwater...

Documents

Transcript of Model Predictive Control for Six Degrees-of-Freedom Station-Keeping of an Underwater...