Colin Calas School of Engineering, Cardiff University August 2017

Abstract
This document presents a model prective control – MPC – algorithm aiming to autonoumously drive
an Unmanned Surface Vehicle – USV – towards a set of waypoints. The design of the algorithm has
been thinked to be robust to environmental disturbances encountered in the marine environment.

The modelling of the USV and disturbances have been simplified as this work is aimed to be a proof
of concept: a more accurate modelling should be considered for a real world implementation.

Introduction
This work follows a previous one where the goal was to autonomously control an USV over a set of
waypoints. A MPC algorithm with constant position reference - i.e. the current waypoint - tracking
was implemented but this control scheme was not achieving good tracking performances. The
problem could come from the optimization that was done over the position variables in the global
frame which expressions are non-linear. Thus it was decided to recast the MPC strategy to make the
optimization simpler using angular and forward speed variables – which expression are fully linear in
the boat’s frame - into the optimization problem.

In this new version, the algorithm tries to track both angle and forward speed references over a
constant time horizon. The boat model was rewritten in a simpler but yet accurate way and an
"intelligent" reference providing algorithm was written to make the system more robust to
disturbances.

The first point that will be addressed is the modelling of the USV, followed by the tracking strategy
used. Then the reference providing algorithm - including dealing with disturbances - will be explained
and finally the MPC algorithm and the results obtained.

USV modelling
This part explains the physics used to describe the USV behavior and how this was modeled so it can
be used into an MPC scheme.

Physical description
The USV studied here has a catamaran-like structure with two static propellers at the end of each
hull.

The figure below provides a graphic view of the boat and the forces that applies to him.

𝑤 ≈ 1𝑚
ℎ ≈ 1.5𝑚
DY
D
𝛼
Dx
Colin Calas School of Engineering, Cardiff University August 2017

Modelling
Using Newton's second law in translation and rotation the linear and angular accelerations can be
written in the boat local frame:
𝑚𝑥̈ = 𝑈2 + 𝑈2 − 𝑘𝑥̇
{ ̈ 𝐷
𝐼𝜃 = (𝑈1 − 𝑈2 )
2
The MPC algorithm will need to track a forward-speed and an angle reference, thus a model where
those are available is required.

The state-space formalism will be used to describe the system and the state and input vectors are
stated as follow1:

𝜃̈
𝜃̇ 𝑈
𝑋 = 𝜃 , 𝑈 = ( 1)
𝑈2
𝑥̇
(𝑥̈ )
Then a simple discrete-time state-space system is written:
𝐷 −𝐷
1 0 0 0 0 2𝐼 2𝐼
𝑇𝑒 1 0 0 0 0 0
𝑋𝑘+1 = 0 𝑇𝑒 1 0 0 𝑋𝑘 + 0 0 𝑈𝑘 = 𝐴𝑋𝑘 + 𝐵𝑈𝑘
0 0 0 1 𝑇𝑒 0 0
(0 0 0 0 1) 1 1
(𝑚 𝑚)
As explained in [1] an optimization made on input increments will be more tractable for tracking
problems. So the system was extended as follow:

𝜃̈
𝜃̇
𝜃 ∆𝑈
𝑋 = 𝑥̇ , ∆𝑈 = ( 1 )
∆𝑈2
𝑥̈
𝑈1
(𝑈2 )
𝐴 𝐵 0
𝑋𝑘+1 = ( ) 𝑋 + ( ) ∆𝑈𝑘
0 𝐼 𝑘 𝐼

1
The indices used for discrete time variables have been ignored in order to simplify the equations.
Colin Calas School of Engineering, Cardiff University August 2017

Tracking
The past section dealt with variables described in the boat local frame. But the boat position needs
to be estimated - or measured - in order to drive it towards the waypoint. To achieve this a technique
known as "dead reckoning" has been used.

Dead reckoning
Technically dead reckoning is equivalent to integrating the boat's speed over one time step with the
actual position as initial condition.

As the velocity of the boat is obtained in its own local frame, its position in the global frame depends
on the angle of the boat.

Again, a state-space system will be used to implement this. However, this system will be non-linear2:
𝑋
𝑌
𝜃
𝑈
𝑋 = 𝜃̇ , 𝑈 = ( 1 )
𝑈
𝜃̈ 2

𝑥̇
( 𝑥̈ )
𝑋 + 𝑇𝑒 𝑥̇ cos 𝜃
𝑌 + 𝑇𝑒 𝑥̇ sin 𝜃
𝜃 + 𝑇𝑒 𝜃̇
𝜃̇ + 𝑇𝑒 𝜃̈
𝑓(𝑋, 𝑈) = 𝐷
𝜃̈ + (𝑈1 − 𝑈2 )
2𝐼
𝑥̇ + 𝑇𝑒 𝑥̈
1
(𝑥̈ + 𝑚 (𝑈1 + 𝑈2 − 𝑘𝑥̇ ))
This set of state equations are linearized around the boat current angle to obtain:
0 0
1 0 0 0 0 𝑇𝑒 cos 𝜃 0 0 0
0 1 0 0 0 𝑇𝑒 sin 𝜃 0 0 0
0 0 1 𝑇𝑒 0 0 0 0 0
𝑋𝑘+1 = 0 0 0 1 𝑇𝑒 0 0 𝑋𝑘 + 𝐷 𝐷 𝑈𝑘
0 0 0 0 −
0 1 0 2𝐼 2𝐼
0 0 0 0 0 1 𝑇𝑒 0 0
(0 0 0 0 0 −𝑘/𝑚 1) 1 1
(𝑚 𝑚 )

Kalman filters
The previous paragraph provides a model to estimate the boat's position. However in real world
applications a GPS and a compass could be used to obtain this data.

A Kalman filter could be developed to compute a better estimate of the position using both model
and sensors data. This is known as sensor fusion.

2
The indices used for discrete time variables have been ignored in order to simplify the equations.
 Colin Calas School of Engineering, Cardiff University August 2017

Reference providing algorithm

When using a MPC algorithm, a reference over the prediction horizon is needed. The goal of this
algorithm is to provide such a reference and to anticipate the environmental disturbances that could
be encountered. In this algorithm an angle reference is needed and the speed reference will simply
be a constant.

The algorithm goal is to provide an angle reference, knowing the USV position and the environmental
disturbances. It works on a fairly simple way:

 𝑊ℎ𝑖𝑙𝑒 𝑖 < 𝑛𝑦 :
o 𝐶𝑜𝑚𝑝𝑢𝑡𝑒 𝑡ℎ𝑒 𝑎𝑛𝑔𝑙𝑒 𝑏𝑒𝑡𝑤𝑒𝑒𝑛 𝑡ℎ𝑒 𝑈𝑆𝑉 𝑝𝑜𝑠𝑖𝑡𝑖𝑜𝑛 𝑎𝑛𝑑 𝑡ℎ𝑒 𝑛𝑒𝑥𝑡 𝑤𝑎𝑦𝑝𝑜𝑖𝑛𝑡.
o 𝐶𝑟𝑒𝑎𝑡𝑒 𝑎 virtual 𝑠𝑡𝑎𝑡𝑒 𝑠𝑝𝑎𝑐𝑒 𝑚𝑜𝑑𝑒𝑙 𝑙𝑖𝑛𝑒𝑎𝑟𝑖𝑧𝑒𝑑 𝑎𝑟𝑜𝑢𝑛𝑑 𝑡ℎ𝑒 𝑎𝑛𝑔𝑙𝑒 𝑝𝑟𝑒𝑣𝑖𝑜𝑢𝑠𝑙𝑦 𝑐𝑜𝑚𝑝𝑢𝑡𝑒𝑑.
o 𝐼𝑛𝑝𝑢𝑡 𝑡𝑜 𝑡ℎ𝑒 “𝑣𝑖𝑟𝑡𝑢𝑎𝑙” 𝑚𝑜𝑑𝑒𝑙 𝑡𝑤𝑜 𝑒𝑞𝑢𝑎𝑙 𝑡ℎ𝑟𝑢𝑠𝑡𝑠 𝑐𝑜𝑚𝑝𝑢𝑡𝑒𝑑 𝑡𝑜 ℎ𝑎𝑣𝑒 𝑡ℎ𝑒 𝑑𝑒𝑠𝑖𝑟𝑒𝑑 𝑓𝑜𝑟𝑤𝑎𝑟𝑑 𝑠𝑝𝑒𝑒𝑑.
o 𝐴𝑑𝑑 𝑡ℎ𝑒 𝑎𝑛𝑔𝑙𝑒 𝑡𝑜 𝑡ℎ𝑒 𝑟𝑒𝑓𝑒𝑟𝑒𝑛𝑐𝑒 𝑣𝑒𝑐𝑡𝑜𝑟.
o 𝐼𝑓 𝑡ℎ𝑒 𝑏𝑜𝑎𝑡 𝑎𝑟𝑟𝑖𝑣𝑒𝑑 𝑡𝑜 𝑡ℎ𝑒 𝑤𝑎𝑦𝑝𝑜𝑖𝑛𝑡, 𝑐𝑜𝑛𝑠𝑖𝑑𝑒𝑟 𝑡ℎ𝑒 𝑛𝑒𝑥𝑡 𝑤𝑎𝑦𝑝𝑜𝑖𝑛𝑡.
o 𝐼𝑛𝑐𝑟𝑒𝑚𝑒𝑛𝑡 𝑖
 𝐸𝑛𝑑

Where 𝑛𝑦 is the MPC prediction horizon.

An illustrated example is provided below to help the understanding.

1 2
2 2

3 3

1 1

1. The USV is in a random position.

2. Compute the angle between the USV and the next waypoint and create a linearized model of
the USV – represented with dashed lines.
3. Input to this model constant and equal thrusts until it arrives to the waypoint and then
recompute the angle to the next waypoint and so on.
Colin Calas School of Engineering, Cardiff University August 2017

This algorithm outputs an angle between minus pi and pi, thus, when the difference between the
current angle and the next computed is greater than pi, this means that there is a shorter path to go
than what the algorithm outputs. A second algorithm is here to correct this.

It works as follow:

 𝐷𝑖𝑓𝑓𝑒𝑟𝑒𝑛𝑐𝑒 = 𝑎𝑏𝑠(𝑎𝑛𝑔𝑙𝑒 – 𝑛𝑒𝑥𝑡_𝑎𝑛𝑔𝑙𝑒)

 𝐼𝑓 𝑑𝑖𝑓𝑓𝑒𝑟𝑒𝑛𝑐𝑒 > 𝑝𝑖
o 𝐼𝑓 𝑎𝑛𝑔𝑙𝑒 < 0
 𝑁𝑒𝑥𝑡_𝑎𝑛𝑔𝑙𝑒 = 𝑎𝑛𝑔𝑙𝑒 – (2 ∗ 𝑝𝑖 – 𝑑𝑖𝑓𝑓𝑒𝑟𝑒𝑛𝑐𝑒)
o 𝐸𝑙𝑠𝑒
 𝑁𝑒𝑥𝑡_𝑎𝑛𝑔𝑙𝑒 = 𝑎𝑛𝑔𝑙𝑒 + (2 ∗ 𝑝𝑖 – 𝑑𝑖𝑓𝑓𝑒𝑟𝑒𝑛𝑐𝑒)
o 𝐸𝑛𝑑
 𝐸𝑛𝑑

Disturbances
In order to see the algorithm robustness to external disturbances, some were added to the model.

It was assumed that the disturbances were constant and that they could be measured with an
arbitrary precision, which is obviously false in a real scenario but more convenient here.

The disturbances in the global frame can be expressed as:

𝐷
𝐷𝑘 = ( 𝑋 )
𝐷𝑌
Let 𝛼 be the angle of the 𝐷𝑘 vector from the 𝑋 axis3. So in the boat frame this vector become:
𝐷𝑥 𝐷 cos(𝛼 − 𝜃)
𝐷𝑘 = (𝐷 ) = ( 𝑋 )
𝑦 𝐷𝑌 sin(𝛼 − 𝜃)

𝑦 axis velocity and acceleration had to be add to the state-space system used to provide the
reference; so it became:
𝑋
𝑌
𝜃
𝜃̇
𝑈
𝑋 = 𝜃̈ , 𝑈 = ( 1 )
𝑈2
𝑥̇
𝑥̈
𝑦̇
( 𝑦̈ )

3
See the USV figure in the « USV modelling » part for details.
Colin Calas School of Engineering, Cardiff University August 2017

With this state vector, the equations became:

𝑋 + 𝑇𝑒 𝑥̇ 𝑐𝑜𝑠(𝜃) − 𝑇𝑒 𝑦̇ 𝑠𝑖𝑛(𝜃)
𝑌 + 𝑇𝑒 𝑥̇ 𝑠𝑖𝑛(𝜃) + 𝑇𝑒 𝑦̇ 𝑐𝑜𝑠(𝜃)
𝜃 + 𝑇𝑒 𝜃̇
𝜃̇ + 𝑇𝑒 𝜃̈
𝐷
𝜃̈ + (𝑈1 − 𝑈2 )
𝑓(𝑋, 𝑈) = 2𝐼
𝑥̇ + 𝑇𝑒 𝑥̈
1
𝑥̈ + (𝑈1 + 𝑈2 − 𝑘𝑥̇ + 𝐷𝑥 )
𝑚
𝑦̇ + 𝑇𝑒 𝑦̈
1
( 𝑦̈ + (𝐷𝑦 − 𝑘𝑦̇ ) )
𝑚
And linearizing around 𝜃 gives the following Jacobians:

1 0 0 0 0 𝑇𝑒 cos(𝜃) 0 −𝑇𝑒 sin(𝜃) 0

0 1 0 0 0 𝑇𝑒 sin(𝜃) 0 𝑇𝑒 cos(𝜃) 0
0 0 1 𝑇𝑒 0 0 0 0 0
0 0 0 1 𝑇𝑒 0 0 0 0
𝐹𝑋 = 0 0 0 0 1 0 0 0 0
0 0 0 0 0 1 𝑇𝑒 0 0
0 0 0 0 0 −𝑘/𝑚 1 0 0
0 0 0 0 0 0 0 1 𝑇𝑒
(0 0 0 0 0 0 0 −𝑘/𝑚 1)
0 0
0 0
0 0
0 0
𝐷 𝐷
−
𝐹𝑈 = 2𝐼 2𝐼
0 0
1 1
𝑚 𝑚
0 0
(0 0 )
So the following recursive equation is obtained:
0
0
0
1 0
𝑋𝑘+1 = 𝐹𝑋 𝑋𝑘 + 𝐹𝑈 𝑈𝑘 + 0
𝑚 0
𝐷𝑥
0
𝐷
( 𝑦)
Using this system into the reference providing algorithm will automatically take disturbances into
account as far as they are measured.
Colin Calas School of Engineering, Cardiff University August 2017

MPC algorithm
This part will describe the MPC algorithm and how it has been implemented in Matlab.

The previous MPC control scheme was optimizing the boat’s position in the plane to drive it to the
next waypoint. This method has not given good results as the trajectory was not optimal in any
sense.

It is probable that this was produced by a bad optimization problem, as the boat position in the
global plane was given by a non-linear system – due to the referential change between the boat
frame and the global frame.

Thus it has been decided to use variables easily expressed in the boat’s frame. It has been noticed
that boat angle and cruise speed should be enough to control the boat. And as they can be expressed
easily in the boat’s frame they should give a more tractable optimization problem.

The extended state-space system described in the modelling part will be used for the MPC algorithm
– with the 𝐷𝑥 term of the disturbance added. The standard 𝐴, 𝐵 notations will be used for its
matrixes even if they were already used for the non-extended system.

Also, the output matrix has been chosen to match the optimized variables:
0 0 1 0 0 0 0)
𝐶=(
0 0 0 1 0 0 0
Cost function
The cost function used is the same than the one used in [1] for the tracking problem:
𝑁−1
1 1 𝑇
𝐽 = (𝑟𝑘+𝑁 − 𝑦𝑘+𝑁 )𝑇 𝑆(𝑟𝑘+𝑁 − 𝑦𝑘+𝑁 ) + ∑((𝑟𝑘+𝑛 − 𝑦𝑘+𝑛 )𝑇 𝑄(𝑟𝑘+𝑛 − 𝑦𝑘+𝑛 ) + ∆𝑢𝑘+𝑛 𝑅∆𝑢𝑘+𝑛 )
2 2
𝑛=0

Replacing the output term with its equation from the state-space representation gives:
1 𝑇
𝐽 = (𝑟𝑘+𝑛𝑦 − 𝐶𝑋𝑘+𝑛𝑦 ) 𝑆 (𝑟𝑘+𝑛𝑦 − 𝐶𝑋𝑘+𝑛𝑦 )
2
𝑛𝑦 −1
1 𝑇
+ ∑ [(𝑟𝑘+𝑛 − 𝐶𝑋𝑘+𝑛 )𝑇 𝑄(𝑟𝑘+𝑛 − 𝐶𝑋𝑘+𝑛 ) + ∆𝑈𝑘+𝑛 𝑅∆𝑈𝑘+𝑛 ]
2
𝑛=0

With 𝑆, 𝑄 and 𝑅 gain matrices.

It is show in [1] that the cost function can be expressed in a form that can be used into the
𝑞𝑢𝑎𝑑𝑝𝑟𝑜𝑔 function of Matlab:
1
𝐽 = 𝑥 𝑇 𝐻𝑥 + 𝑓 𝑇 𝑥
2
⃗⃗⃗⃗⃗ .
Where 𝑥 is the optimization variable – i.e. the 𝑛𝑢 future input increments written as ∆𝑈

It is also shown in [1] that 𝐻 and 𝑓 can be expressed as follow:

𝑇
𝐻 = 𝐶 𝑄𝐶+𝑅
{ 𝑇 𝑇
𝑇 𝑇 𝐴 𝑄𝐶
𝑓 = [𝑋𝑘 𝑟 ] [ ]
−𝑇 𝐶
Colin Calas School of Engineering, Cardiff University August 2017

With:

𝐶 𝑇 𝑄𝐶 0 ⋯ 0
⋱ ⋱ ⋮ )
𝑄=( 0 ⋱ 𝐶 𝑇 𝑄𝐶
⋮ 0
0 ⋯ 0 𝐶 𝑇 𝑆𝐶
𝐵 0 ⋯ 0
𝐶=( 𝐴𝐵 𝐵 ⋱ ⋮)
⋮ ⋱ ⋱ 0
𝐴𝑛𝑦 −1 𝐵 ⋯ 𝐴𝐵 𝐵
𝑅 0 ⋯ 0
𝑅 = (0 𝑅 ⋱ ⋮)
⋮ ⋱ ⋱ 0
0 ⋯ 0 𝑅
𝐴
2
𝐴=( 𝐴 )
⋮
𝐴 𝑛𝑦
𝑄𝐶 0 ⋯ 0
⋱ ⋮ )
𝑇=( 0 ⋱
⋱ 𝑄𝐶
⋮ 0
0 ⋯ 0 𝑆𝐶
And 𝑋𝑘 the current state vector and 𝑟 the reference vector.
Colin Calas School of Engineering, Cardiff University August 2017

Constraints
Constraints can be easily added to an MPC scheme as they are often included into the optimization
software. The 𝑞𝑢𝑎𝑑𝑝𝑟𝑜𝑔 function of Matlab has been used to solve the optimization problem, so
constraints of the following form have been added:

⃗⃗⃗⃗⃗ ≤ 𝐵
𝐴∆𝑈
As shown in [2], let’s pose:

𝑋≤𝑋
{
𝑋≥𝑋

So

𝑋≤𝑋
{
−𝑋 ≤ −𝑋

What can be written as:

𝑋
⋮
𝐼𝑛𝑦 𝑋
( )𝑋 ≤ −
−𝐼𝑛𝑦
−𝑋
⋮
(−𝑋)

Where 𝑋 represents the vector of the 𝑛𝑦 future states.

The following notation will be used:

𝐶𝑋 𝑋 ≤ 𝑑𝑋
And, as state predictions are given by:

𝑋 = 𝐴𝑋𝑘 + 𝐶 ⃗⃗⃗⃗⃗
∆𝑈
The previous inequality can be written as:

⃗⃗⃗⃗⃗ ) ≤ 𝑑𝑋
𝐶𝑋 (𝐴𝑋𝑘 + 𝐶 ∆𝑈

Which gives:

𝐶𝑋 𝐶 ⃗⃗⃗⃗⃗
∆𝑈 ≤ 𝑑𝑋 − 𝐶𝑋 𝐴𝑋𝑘
So in our constraint equation:

𝐴 = 𝐶𝑋 𝐶

𝐵 = 𝑑𝑋 − 𝐶𝑋 𝐴𝑋𝑘
Colin Calas School of Engineering, Cardiff University August 2017

Results
This part will present the results obtained with the MPC algorithm as described in this document.

The simulations used to validate this algorithm are based on a benchmark trajectory of 10 waypoints.
The USV has to reach the 10 waypoints in a defined order. This test has been made with and without
disturbances – on the second figure the pink arrow represents the direction of the disturbance.

In the figures below are represented – from left to right and up to down: the USV trajectory with the
waypoints, the thrust applied on each motor, the forward speed and its reference, the angle from
the state vector used in the reference providing algorithm, the angle from the state vector used in
the MPC algorithm and the 𝑦 axis boat speed.

In the first figure below – simulation without disturbances – the main objective is clearly reached: the
algorithm drives the boat around all the waypoints in a fairly optimized way. However an offset is
observed on the forward speed of the boat – adding an integral gain may solve the problem. And an
unexplained angle drift arises around the 1000th time step.

In the second figure – simulation with disturbances – the main objective is also clearly reached.
However, besides the two kinks pointed out in the previous paragraph, two more issued arises: as
the distance from the “optimal” path is not optimized, the MPC algorithm doesn’t care of its
trajectory as long as all the waypoints are reached thus leading to a non-optimal trajectory. Also, the
𝑦 speed of the boat is not an optimization criterion so all the disturbances pass through the MPC and
directly affects the boat. The problem is that the two motors are not controllable in angle, making
the correction of the 𝑦 axis speed difficult.
Colin Calas School of Engineering, Cardiff University August 2017

References

[1] Z. Hurák, "Optimal and Robust Control course at Faculty of Electrical Engineering, Czech
Technical University," [Online]. Available:
https://moodle.fel.cvut.cz/mod/folder/view.php?id=63655.

[2] J. A. Rossiter, "Constrained Predictive Control 5_3 - including constraints into GPC," [Online].
Available:
https://www.youtube.com/watch?v=mkVgvA_94HM&index=3&list=PLs7mcKy_nInGHNeV1RRTo
-tGFyzqOr7Nv.

Appendices
The code is available on GitHub:

https://github.com/ccalas/mpc/blob/master/final_mpc.m