Controller Design for Autonomous Marine

Vehicle Executing Inspection Manoeuvre

Undergraduate Thesis

Submitted in partial fulfillment of the requirements of

BITS F421T Thesis

By

Rohan Godiyal
ID No. 2015A4TS0323G

Under the supervision of:

Prof. Antonio Pascoal

&
Dr. Kiran D. Mali

BIRLA INSTITUTE OF TECHNOLOGY AND SCIENCE PILANI, GOA CAMPUS

December 2018
 Declaration of Authorship
I, Rohan Godiyal, declare that this Undergraduate Thesis titled, ‘Controller Design for Au-
tonomous Marine Vehicle Executing Inspection Manoeuvre’ and the work presented in it are my
own. I confirm that:

 This work was done wholly or mainly while in candidature for a research degree at this
University.

 Where any part of this thesis has previously been submitted for a degree or any other
qualification at this University or any other institution, this has been clearly stated.

 Where 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.

 I have acknowledged all main sources of help.

 Where the thesis is based on work done by myself jointly with others, I have made clear
exactly what was done by others and what I have contributed myself.

Signed:

Date:

i
 Certificate
This is to certify that the thesis entitled, “Controller Design for Autonomous Marine Vehicle
Executing Inspection Manoeuvre” and submitted by Rohan Godiyal ID No. 2015A4TS0323G
in partial fulfillment of the requirements of BITS F421T Thesis embodies the work done by him
under my supervision.

Supervisor
Co-Supervisor
Prof. Antonio Pascoal
Dr. Kiran D. Mali
Professor,
Asst. Professor,
ISR, Institute Superior Technico(IST)
BITS-Pilani Goa Campus
Lisbon
Date:
Date:

ii
“What’s in the knowledge base only defines the outward expanse of ones potential boundaries, it’s
what one has put into experience that ensures the inward depth and subsequently excellence”

-Rohan Godiyal
 BIRLA INSTITUTE OF TECHNOLOGY AND SCIENCE PILANI, GOA CAMPUS

Abstract
Bachelor of Engineering (Hons.)

Controller Design for Autonomous Marine Vehicle Executing Inspection

Manoeuvre

by Rohan Godiyal

The thesis initially dwells upon various aspects of controlling Autonomous marine Vehicles(AMV)
by understanding theoretical background and practicing established methodologies based on
past research. Further post-midsem the research would narrow down to designing and simulating
control system for a sufficiently actuated AMV to execute an Inspection manoeuvre around
an object of interest in a planar path. The primary focus has been to achieve heading and
speed control while simulating the kinematics and dynamics on the mathematical model of
under-actuated Medusa class vehicle and later on expanding to a 4 thruster model to achieve
heading, course and speed control to perform a waypoint based path planning sequence. The
control architecture employs inner-outer loop approach considering current estimation and
compensation as well. Considering it’s very own challenges there have been insightful conclusions
and learning curves in topics related to time domain and frequency domain analysis, linear and
non-linear systems, linearization, state-space notation, PID Controller, root locus, inner-outer
loop approach, Kalman filter etc. Moreover there has been significant hands on experience
with some of the most advanced software tools such as MATLAB and ROS in simulating and
implementing aforementioned scenarios.. . .

Keywords: Course Control, Inner-outer loop control, Current estimation and Compensation,
Inspection manoeuvre
 Acknowledgements
As insightful this journey has been knowledge wise, it’s the support, wisdom and generosity
of my family, friends, colleagues and supervisors that has been far more influential especially
throughout the course of these past 5 months and beyond.
Firstly and foremost I would like to thank my parents and my sister who have undergone immense
emotional and financial hardships to get me through to this opportunity. Also I would like to
thank all my close family members including all my uncles and aunts who have been in no less
roles than my parents to me, my cousin brother and sisters for their unparalleled support and
sacrifices and commit myself to be always there for them.
A special thanks to my supervisor, Professor António Pascoal for his heart warming support
which really made me feel home in a foreign land and his wise guidance that facilitated the
whole research practice in a very convenient, efficient and insightful manner.
I would also like to thank my on-campus supervisor, Dr. Kiran D. Mali for holding trust in me
and imparting his wise guidance and support in accomplishing the milestones successfully.
At last I would like to take this opportunity to thank my fellowmates Shubham Kumar and
Abhinav Singh for their joyful company that made this whole journey a memorable experience
altogether . . .

v
Contents

Declaration of Authorship i

Certificate ii

Abstract iv

Acknowledgements v

Contents vi

List of Figures viii

List of Tables ix

Abbreviations x

1 Introduction 1
1.1 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

2 Theoretical Developments 3
2.1 Reference Frames and Notations . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2.2 Kinematics and Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2.2.1 Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2.2.2 Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.2.3 Simplified Mathematical Model . . . . . . . . . . . . . . . . . . . . . . . . 7

3 Control System Design and Developments 10

3.1 Vehicle Model Characterization . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
3.2 Motor Power and Thruster Modelling . . . . . . . . . . . . . . . . . . . . . . . . 11
3.2.1 Motor Power . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
3.2.2 Thruster modelling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
3.3 Inner Loop Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
3.3.1 Yaw Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
3.3.2 Speed(u-v) Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

vi
Contents vii

3.4 Outer Loop Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

3.4.1 Reference generation and waypoint control . . . . . . . . . . . . . . . . . 15
3.4.2 Guidance Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
3.5 Current Estimation and Compensation . . . . . . . . . . . . . . . . . . . . . . . . 19
3.5.1 Current observer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.5.2 Current Compensation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

4 Inspection Manoeuvre Simulation 23

4.1 Path following and Waypoint Generation . . . . . . . . . . . . . . . . . . . . . . 23
4.2 Simulink model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
4.3 Simulation Results and Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . 26
4.3.1 PID Tuning Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
4.3.2 Inspection Manoeuvre Simulation Results . . . . . . . . . . . . . . . . . . 27

5 Conclusion and Future Prospects 32

A Matlab-Simulink Code excerpts 34

A.1 Wapoint Generation Block . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
A.2 Reference Generator Block . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
A.3 Motor Power block . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

Bibliography 37
List of Figures

1.1 Medusa class vehicle performing surface mapping operation beneath Ice sheet
(source: https://vimeo.com/112711346 . . . . . . . . . . . . . . . . . . . . . . . . 2

2.1 AUV reference frames and notation. (source: dos Santos Ribeiro [3]) . . . . . . . 3

3.1 Thruster RPMs Starbord rear() and Portside rear() . . . . . . . . . . . . . . . . 11

3.2 Thruster Model Diagram (source: dos Santos Ribeiro [3]) . . . . . . . . . . . . . 12
3.3 Thruster Model Diagram (inspired by: dos Santos Ribeiro [3]) . . . . . . . . . . . 13
3.4 PID control Implementation for surge, sway and yaw (inspired by: dos Santos
Ribeiro [3]) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
3.5 Line of sight guidance representation (inspired by: dos Santos Ribeiro [3]), Please
note in the depiction hd is denoted rather than ψd as in medusa . . . . . . . . . . 18
3.6 Kalman filter block for current estimation . . . . . . . . . . . . . . . . . . . . . . 20
3.7 Current Compensation (inspired by: dos Santos Ribeiro [3]), Please note in the
depiction h and vn et are denoted rather than ψ and u respectively, as in medusa 21

4.1 Simulink Model for Inspection Manoeuvre (inspired by [3]) . . . . . . . . . . . . 24

4.2 Simscape multibody Dynamics block for graphic visualization of Inspection ma-
noeuvre . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
4.3 Simplified Model for PID tuning . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
4.4 x-y displacement, u-v velocities and yaw responses of the tuned PID controllers . 27
4.5 Actual and desired trajectory for Inspection Manoeuvre . . . . . . . . . . . . . . 28
4.6 x-y displacement, u-v velocities and yaw responses of the tuned PID controllers . 28
4.7 [V cx , V cy ] actual and [Vˆcx , Vˆcy ]estimated current velocities using Kalman filter
and GPS measured position and DVL measured velocities . . . . . . . . . . . . . 29
4.8 Simscape multibody simulation graphics snippets of some notable instants . . . 29
4.9 PID Control Signals . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
4.10 Forces and moment in Body Fixed Frame . . . . . . . . . . . . . . . . . . . . . . 30
4.11 Thruster RPMs Stabord front() and portside front() . . . . . . . . . . . . . . . . 31
4.12 Thruster RPMs Stabord rear() and portside rear() . . . . . . . . . . . . . . . . . 31

viii
List of Tables

2.1 SNAME notations for marine vehicles motion . . . . . . . . . . . . . . . . . . . . 4

3.1 AMV Model Parameters (Assumed) . . . . . . . . . . . . . . . . . . . . . . . . . 11

ix
Abbreviations

AMV Autonomous Marine Vehicle

DOF Degree Of Freedom
SNAME Society of Naval Architects and Marine Engineers
AHRS Attitude and Heading Reference System
DVL Doppler Velocity Log
LOS Line Of Sight
GPS Global Positioning System
PID Proportional Integral Derivative
RPM Rotations Per Minute

x
Dedicated to my dear parents and my sweet sister.

xi
Chapter 1

Introduction

Biological Sciences significantly suggest that primary lifeforms came into existence beneath the
vast realms of oceans, millions of years ago. To me, further than this idea of emergence of life,
even more intriguing is the prediction of much more species still unknown within these depths
than we have observed till date. We humans have fled those bodies such long back that our
body has lost almost all of it’s capabilities to sustain in that environment. But even better,
we have gained intellectual advancements far superior, which has outsourced fulfillment of our
exploratory tendencies to technologies far capable than we could have ever imagined ourselves to
be. This has been proven every now and then in the field of space exploration.
Still an Underwater or on-surface expeditions poses challenges far complex in it’s own way. It’s
theses challenges and current state of technological advancements that inspires and motivates the
researchers and developers of this decade to undertake substantial leap forward in this leading
domain of Marine Robotics.
Medusa class vehicles are highly sophisticated marine robots suitable for a wide range of
on-surface as well as underwater applications but limited DOF due to being under-actuated,
restricts it’s inspection and intervention capability significantly. Although this highly simplifies
the mechanism and it’s control as compared to AMVs with 3 or more DOF, the limitations
could be overcome primarily by adding thrusters to enable sway and heave motion.

1.1 Problem Statement

The research is inspired to take on the challenge of designing a controller for AMV with 4
thrusters and 3 Control parameters(Yaw, Surge and Sway). The 4 thruster model for 3 DOF
control is based on an assumption that it’s physical symmetry would offer system stability and
simplified mathematical modelling and more number of thrusters will lead to speed and control
advantages at the cost of over-actuation and related disadvantages that haven’t been considered

1
Chapter 1. Introduction 2

within the scope of this research.

Furthermore, the task is to simulate the execution of Inspection maneuver in a circular trajectory
around a point of interest thus demonstrating the Control System implementation of all 3 DOFs
of the AMV. This also justifies the necessity for a 3 DOF AMV because of the fact that while
performing the manoeuvre the AMV is supposed to be pointing towards the Point of interest
and simultaneously moving along a circular trajectory of defined curvature around that point.
This limits the use of Medusa class vehicles with only 2 DOF in this scenario because of the lack
of 3rd DOF(i.e. sway).
Also in order to incorporate real scenario factors such as ocean currents, current estimation and
compensation has been carried out in the final manoeuvre simulation.

Figure 1.1: Medusa class vehicle performing surface mapping operation beneath Ice sheet
(source: https://vimeo.com/112711346

1.2 Outline

• Chapter 2 : In this chapter we setup the preliminary background regarding general AMV
Mathematical modelling, setup some references and notations in order to be able to dive
further into practical manipulation of those systems.

• Chapter 3 : Once we have the mathematical model in hand we begin to establish control
within various sub systems within the system by practically designing and iterative testing
with software tools such as Simulink and MATLAB.

• Chapter 4 : Once the control is established on the system in all aspects, we start tackling
the task on hand i.e. simulating Inspection Manoeuvre. For this an algorithm or simply a
sequence of operations is put forward and further simulation results are observed.

• Chapter 5 : Finally, conclusions are drawn upon the observed results and further possible
developments are laid down.
Chapter 2

Theoretical Developments

Before diving into the control architecture design there are a few notations and references to be
defined in order to parameterize various kinematic and dynamic features of the AMV, also several
mathematical modelling tools and techniques need to be covered which help in sophisticated
manipulation of these kinematic and dynamic relations.

2.1 Reference Frames and Notations

In 3D space the AUV’s position and orientation can be completely described using six variables
along with two reference frames.

Figure 2.1: AUV reference frames and notation. (source: dos Santos Ribeiro [3])

3
Chapter 2. Theoretical Developments 4

• where {B} represents body-fixed frame and {I} represents earth-fixed or inertial frame

• position of {B} with respect to {I}:

η1 = [x, y, z]T

• orientation of {B}, in Euler-angles, with respect to {I}:

η2 = [φ, θ, ψ]T

• linear velocity of {B} with respect to {I} expressed in {B}:

ν1 = [u, v, w]T

• angular velocity of {B} with respect to the {I}, expressed in {B}:

ν2 = [p, q, r]T

• forces acting on the vehicle measured in the {B}:

τ1 = [X, Y, Z]T

• moments acting on expressed in {B}:

τ2 = [K, M, N ]T

Degree of Freedom Forces and Angular and Linear Position and Euler
Moments Velocities Angles
Motion in the x-direction (surge) X u x
Motion in the y-direction (sway) Y v y
Motion in the z-direction (heave) Z w z
Rotation about the x-axis (roll) K p φ
Rotation about the y-axis (pitch) M q θ
Rotation about the z-axis (yaw) N r ψ

Table 2.1: SNAME notations for marine vehicles motion

Chapter 2. Theoretical Developments 5

2.2 Kinematics and Dynamics

In the kinematics section the transformation relations among motion parameters such as linear
and angular velocities are presented, later in the dynamics section we deal with forces and
moments acting on the AMV.

2.2.1 Kinematics

The equation below depicts the transformation from body linear velocities measured in {B} to
body position derivative measured in {I}.

I (2.1)
η˙1 = RB (η2 )ν 1

I represents the rotation matrix obtained by multiplying a series of principal rotation

where RB
matrices. In our case the ZYX convention is used. This means that the axis are successively
rotated following that order: first a rotation on z, then y and finally x. Thus by multiplying
I mentioned below.
each principal rotation matrix in that order we obtain matrix RB

 
cψcθ −sψcφ + cψsθsφ sψsφ + cψcφsθ
I
 
RB (η2 ) = 
sψcθ cψcφ + sψsθsφ −cψsφ + sψcφsθ
 (2.2)
−sθ cθsφ cθcφ

where s*=sin(*) and c*=cos(*).

The equation below depicts the angular velocities expressed in the body frame with the derivative
of the Euler-angles by means of a transformation matrix, in this case also called attitude matrix

η˙2 = T (η2 )ν 2 (2.3)

 
1 sφtθ cφtθ
 
T (η2 ) = 
0 cψcφ −sφ 
 (2.4)
0 cθsφ cφ/cθ
Chapter 2. Theoretical Developments 6

2.2.2 Dynamics

The rigid-body dynamics can be expressed, in a compact manner, as

MRB ν̇ + CRB (ν)ν = τ RB (2.5)

• MRB is the inertia matrix. This matrix is positive definite, symmetric and constant, i.e.
T >0
ṀRB = 0 and MRB = MRB

• CRB is the Coriolis and centripetal matrix, which can always be parameterized such that
T (ν)
it is a skew-symmetrical matrix, i.e. CRB (ν) = −CRB ∀ ν ∈ R6

• τ RB = [τ1 , τ2 ]T is the generalized vector of external forces and moments

This last term, τ RB , can be seen as the sum of multiple torques and forces, which can be
classified according with its origins. Therefor we may have the decomposition

τ RB = τ + τ A + τ D + τ R + τ E (2.6)

where, τ is the vector of forces and moments produced by the thrusters and it is the control
input of the vehicle.
The τ A represents moments and forces due to hydrodynamic added mass. It may itself be
decomposed in added inertia matrix, MA and hydrodynamic added Coriolis and centripetal
contribution, CA (ν), to produce the result

τ A = −MA ν̇ − CA (ν)ν (2.7)

τ D are forces and moments generated by hydrodynamic damping, which is the energy carried
away due to lift, drag and skin friction, among others. It can also be expressed as

τ D = −D(ν)ν (2.8)

τ R is the contribution due to restoring forces, that is weigh and buoyancy mutual effect. It can
also be expressed as

τ R = −g(η) (2.9)
Chapter 2. Theoretical Developments 7

Finally we have the term τ E which sums the contributions from environmental forces and
moments such as wind, waves and ocean currents

Replacing (2.6) on (2.5), taking into account (2.7), (2.8) (2.9), the dynamic equations can be
written as

(MRB ν̇ + CRB (ν)ν) + (MA ν̇ + CA (ν)ν + D(ν)ν) + g(ν) = τ RB + τ dist (2.10)

M ν̇ + C(ν)ν) + D(ν)ν + g(η) = τ RB + τ dist (2.11)

where M ν̇ = MRB + MA , C(ν) = CRB (ν) + CA (ν)

2.2.3 Simplified Mathematical Model

As the vehicle possesses only 3 DOF allowing motion in a plane, therefore the kinematic and
dynamic models can be simplified significantly to

" #
cψ −sψ
R(ψ) = (2.12)
sψ cψ

therefore,

    
ẋ cψ −sψ 0 u
    
 ẏ  = sψ cψ 0 v  (2.13)
    
ψ̇ 0 0 1 r

considering current as an external factor

ṗ = R(ψ)ν + Vc (2.14)

where, ṗ = [ẋ, ẏ]T , ν = [u, v]T and Vc = [Vcx , V cy]T

Chapter 2. Theoretical Developments 8

on the other hand dynamics reduces down to,

h i
τ = τu τv τr (2.15)

 
mu 0 0
 
M =
 0 mv 0 
 (2.16)
0 0 mr

 
0 0 −mv v
 
C(ν) = 
 0 0 mu u 
 (2.17)
−mv v mu u 0

 
du 0 0
 
D(ν) = 
 0 dv 0 
 (2.18)
0 0 dr

 
0
 
g(η) = 0

 (2.19)
0

where

mu = m − Xu̇ du = −Xu − Xu|u| |u|

mv = m − Yv̇ dv = −Yv − Yv|v| |v|
(2.20)
mr = Iz − Nṙ dr = −Nr − Nr|r| |r|
muv = mu − mv

and g(η) is zero since in our case the weight and the buoyancy force are equal and the center
of mass is coincident with the center of buoyancy. The simplified dynamic model can then be
Chapter 2. Theoretical Developments 9

expressed as

mu u̇ − mv vr + du u = τu
mv v̇ + mu ur + dv v = τv (2.21)
mr ṙ − muv uv + dr r = τr
Chapter 3

Control System Design and

Developments

Once the theoretical background has been setup the next phase is to deal with the design of
control system algorithms to achieve the desired kinematics for the vehicle by controlled tweaking
of dynamic parameters. This chapter deals with the presented goal in a sequence of operations
such as vehicle model characterization, motor Power and thruster modelling, inner loop control
and outer loop control and finally, current estimator and compensator.

3.1 Vehicle Model Characterization

As mentioned in the problem statement that our final goal is to perform an inspection manoeuvre
around a point of interest which involves the vehicle pointing in a particular direction(i.e. towards
the point of interest) while continuing it’s movement in another direction(i.e. along a circular
trajectory around the point of interest). The AMV model is therefore inspired to overcome this
limitation of the 2 DOF Medusa class vehicles by incorporating another 3rd DOF, facilitating
controlled motion in the sway direction as well which further enhances the functional capabilities
of the AMV to perform aforementioned manoeuvre.
To perform this desired action a minimum of one additional actuator/thruster is required,
summing up to a total of 3 thrusters in the vehicle. However,a 4 thruster model has been adopted
instead of 3 because of the physical symmetry which would supposedly lead to added stability,
control and design advantages and also enhanced thrust capability.
The model parameters considered for the design and simulation are suitably self-assumed therefore
leaving a scope of correction and tuning in the parameters. The assumed model parameters have
been mentioned in the following table :

10
Chapter 3. Control System Design and Developments 11

Mass= 17 kg Iz = 1kg.m2 muv = mu − mv

mu = Mass - Xu̇ mv = Mass - Yv̇ mr = IZ - Nṙ
Xu = -0.2 kg/s Yv = -0.2 kg/s Nr = -4.14 kg.m/s dv = -Yv -Yv|v| |v|
Xu̇ = -20 kg Yv̇ = -30 kg Nṙ = -8.69 kg.m2 du = -Xu − Xu|u| |u|
Xu|u| = -25 kg/m Yv|v| = -25 kg/m Nr|r| = -6.23 kg.m dr = −Nr − Nr|r| |r|

Table 3.1: AMV Model Parameters (Assumed)

3.2 Motor Power and Thruster Modelling

Since we are designing a control system for the actual AMV, we are required to incorporate
motor power block which generates the RPM signals to be fed to individual thrusters from the
input control signals. Also, we would need thruster block which would convert the RPM signal
generated by the motor power block to forces developed by individual thrusters which in turn
combine together to give net forces and moments acting on the AMV.

Figure 3.1: Thruster RPMs Starbord rear() and Portside rear()

3.2.1 Motor Power

We have 3 control input signals for 3 DOF and 4 output thruster power signals for our model
therefore the derived thruster allocation is as follows :
Chapter 3. Control System Design and Developments 12

q
Smf = round( |Cmu |Cmu | − Dmy − Cmv |Cmv || ∗ sign(|Cmu |Cmu | − Dmy − Cmv |Cmv ||))
q
P mf = round( |Cmu |Cmu | + Dmy + Cmv |Cmv || ∗ sign(|Cmu |Cmu | + Dmy + Cmv |Cmv ||))
q
Smr = round( |Cmu |Cmu | − Dmy + Cmv |Cmv || ∗ sign(|Cmu |Cmu | − Dmy + Cmv |Cmv ||))
q
P mr = round( |Cmu |Cmu | + Dmy − Cmv |Cmv || ∗ sign(|Cmu |Cmu | + Dmy − Cmv |Cmv ||))
(3.1)

where,(∗ )
Smf = Starboard side front motor/thruster
Smr = Starboard side rear motor/thruster
P mf = Port side front motor/thruster
P mr = Port side rear motor/thruster

and,(∗ )
Cmu = Common mode x-axis velocity(surge) control signal
Cmv = Common mode y-axis velocity(sway) control signal
Dmy = Differential mode z-axis rotation(yaw) control signal

(*) : The notations above and henceforth are inspired by the Medusa class vehicle control system
therefore a symbolic correlation has been maintained for easier understanding, however in some
cases the notations won’t make literal sense as in case of Medusa vehicle. Eg. common mode
and differential mode don’t make direct sense in case of the current vehicle model.

3.2.2 Thruster modelling

The thruster model presented below is the SEABOTIX thruster model that has been primarily
derived from [3] implemented on Medusa and has been incorporated into 4 thruster model for
suitable operation in this case.

Figure 3.2: Thruster Model Diagram (source: dos Santos Ribeiro [3])
Chapter 3. Control System Design and Developments 13

The model includes a first order system with a pure delay characterized by
K0 = 7.2115 , Delay = 0.346s The complete thruster model diagram for combined action of all
the thrusters on the AMV has been depicted below.

Figure 3.3: Thruster Model Diagram (inspired by: dos Santos Ribeiro [3])

3.3 Inner Loop Control

As we know the inner-outer loop approach relies upon distribution of the dynamic and kinematic
control respectively therefore, in Inner loop we deal with the control of dynamic input parameters
to produce the desired kinematic outputs. Based on the dynamic model presented earlier
we have 3 dynamic input parameters i.e.τu , τv , τr to be controlled in order to drive 3 desired
kinematic parameters i.e. u(surge velocity), v(sway velocity), ψ(yaw) to some specific values.
The mentioned control was achieved using PD controller for Yaw and using PI controllers for
Surge and sway velocities.as depicted in the 3.4 below

3.3.1 Yaw Control

The objective of yaw control is to steer automatically the vehicle’s front to a given desired
direction (yaw). To this effect, the control algorithm accepts as inputs the reference signal ψd ,
the yaw ψ and yaw rate ψ˙d that are accessible by the vehicle’s AHRS. Let

ψ̃ = ψ − ψd (3.2)
Chapter 3. Control System Design and Developments 14

Figure 3.4: PID control Implementation for surge, sway and yaw (inspired by: dos Santos
Ribeiro [3])

be the heading error. A simple controller is the proportional-derivative feedback law:

Dmy = −kp ψ̃ − kd ψ (3.3)

where Dmy denotes the control signal weighing the moment acting on the AMV resulting in
turning movement which is later fed to the motor power block. In 3.2, since ψ ∈ [0, 2π]the error
ψ has to be redefined according to
if,

ψ̃ > π(rightside)
(3.4)
ψ̃ = ψ̃ − 2π

if,

ψ̃ < −π(lef tside)

(3.5)
ψ̃ = ψ̃ + 2π

This has been implemented in the model in the block preceding the ’PD-controller’ block3.4.

3.3.2 Speed(u-v) Control

The objective here is to drive the AMV’s surge and sway velocities to the desired values and as
a result steer the vehicles heading angle to the desired direction along with the net speed in that
direction assuming that the body-frame velocities are accessible by DVL. Let

ũ < û − ud (3.6)
Chapter 3. Control System Design and Developments 15

be the error between the measurement speed u and the desired surge speed ud . And
Z t
ξ= ũ(τ )dτ (3.7)
0

is the integration error that will be used to take into account the fact that the vehicle’s thrusters
are commanded in RPMs and without the integration part the controller can not learn the
relation between RPMs and nominal speed (this relation is not constant along time since there
are currents and other external perturbations, neither is linear).

With 3.6 and 3.7 we define a PI (proportional-integral) controller for the common mode control
signals of surge velocity as:

Cmu = −kpu ũ − kiu ξ where kpu , kiu > 0 (3.8)

the same applies to the swy velocity therefore,//

Cmu = −kpv ṽ − kiv ξ where kpv , kiv > 0 (3.9)

3.4 Outer Loop Control

The Outer loop serves the purpose of generating references based on the measured/estimated
kinematic parameters such as positions and speeds to feed the inner loop which in turn utilizes
those references to implement dynamic control as we saw in the earlier section.

3.4.1 Reference generation and waypoint control

The implementation of this approach is inspired by [1] and driven by the desire to implement
positional control over the AMV. So, far we have achieved yaw, surge speed and sway speed
controls and the next goal is to drive the vehicle to a desired position/coordinate and orientation
and hold to it until next waypoint is fed. This is absolutely necessary for controlling real vehicle
and later implementing the inspection manoeuvre.
Inspired by the point stabilization algorithm described in [1], the following strategy was imple-
mented:
1) while the vehicle is in a neighbourhood of radius d of the desired point[sd , yd ] set to zero the
Chapter 3. Control System Design and Developments 16

speed reference (ud = 0) and maintain a given orientation using the heading controller.
This implies that the vehicle is not wasting unnecessary batteries with the thrusters.
2) If the vehicle is outside the neighbourhood, use the speed controllers which in turn control
the heading direction to get to the expected waypoint.
The switching between these two modes of operation is done applying a convenient hysteresis
and avoid chattering.

In mode 2) generating references for the speed controllers gets quite tricky as compared to
Medusa because of the fact that now the heading is governed by the net action of u and v
velocities rather than the yaw as in case of Medusa. Therefore, it is absolutely necessary that
the references generated for both the surge and sway velocities are derived from the desired
heading angle for the AMV.

href = atan2(yd − y, xd − x) ∗ 180/π (3.10)

where, [xd , yd ] represents desired coordinates of the expected position(waypoint), p =[x, y]

represents current position and hr ef ∈ [−π, π] represents the heading direction. Also this
suggests that provided that the heading vector is defined, the ratio of the velocities becomes
constant thus defining a certain correlation among them which is given as

vref /uref = tan(href − ψ) (3.11)

where ψ is the current yaw. But still this directly does not give us the exact values for uref and
vref but only the ratio, because of the fact that we have 4 thrusters and we are trying to control
only 3 DOF, i.e. we have a redundancy in the system, We also have one more term which is the
net resultant of these velocities given by
q
sref = 2
u2ref + vref (3.12)

therefore due to the redundancy we have the freedom to choose values for either one of the uref
and vref and let the other one be derived by the relation 3.11,or choose value for sr ef and solve
for 3.11 and 3.12. The former one was chosen but this complicates the things a bit as we have
certain constraints limiting the references to be generated due to hardware capabilities of the
AMV, therefore a solution to this was devised by assigning value to the velocity reference which
is supposed to be higher in that part of the graph where the angle href − ψ ∈ [−2π, 2π] lies and
S
whether tan(href − ψ) ∈ [−1, 1] or [−∞, −1) (1, ∞].
Therefore,
Chapter 3. Control System Design and Developments 17

d
uref = ku .asin( )
d + ks
vref = uref ∗ tan(href − ψ) ∀ tan(href − ψ) ∈ [−1, 1]
or, (3.13)
d
vref = ku .asin( )
d + ks
uref = uref ∗ tan(href − ψ) ∀ tan(href − ψ) ∈ [−1, 1]

where,

p
d= (x − xd )2 + (y − yd )2 − d d , neighbourhood radius (3.14)

also, ku > 0 defines the upper limit on the speed reference ud , ks > 0 is the parameter for tuning
the vehicle speed with respect to distance error and d is the defined neighbourhood radius. for
model ku = 1 and ks = 0.1 and d = 1 was set.
The reference generation for yaw is quite straightforward as compared to speed references, and
is given by

ψref = atan2(yc − y, xc − x) ∗ 180/π (3.15)

where, p =[x,y] represents current position measurements and [xc , yc ] represents coordinates
towards which the AMV points whose value may or may not differ than the coordinates [xd , yd ]
towards which the AMV is headed unlike the Medusa, in which case the vehicle is always headed
to where it points.

3.4.2 Guidance Control

Guidance associates with higher level of control and generates heading references based on a
series of waypoints. Although there are a number of proposed Guidance Laws in the Literature
the LOS guidance was adopted. An integral term is usually included in order to cancel the
steady state error induced by the currents. However, in our case a method without the integral
term was implemented based on [2]

Consider a path parameterized by a series of waypoints connected through straight lines, as on

3.5, where wpk and wpk+1 are two consecutive waypoints. The line forms an angle χp with the
inertial referential and xe is sometimes called along track error. Because it is not in fact an error,
in the case of path-following, it is also called along path traveled distance. In the case of curved
paths is the curvilinear path distance. As it happens with all guidance laws, the objective is
to reduce the cross-track error, ye . Thus, ye is the input variable used to generate heading
Chapter 3. Control System Design and Developments 18

Figure 3.5: Line of sight guidance representation (inspired by: dos Santos Ribeiro [3]), Please
note in the depiction hd is denoted rather than ψd as in medusa

references in both guidance laws and it is abbreviated ’e’ in the equation. In the case of the
LOS guidance,to generate heading references to a line such as the one represented in 3.5, the
control is given by

−e
hLOS
d = arctan( ) + χp (3.16)
d

where d(set value d = 7 for the model) is called the lookahead distance, that is, the point in the
line to which the vehicle heads is then computed by adding to the vehicle’s nearest point in the
curve the lookahead distance. Thus 3.14 replaces the previous equation for heading reference i.e.
3.10.
The line’s angle, χp , can be computed as follows

χp = atan2(yk+1 − yk , xk+1 − xk ) (3.17)

To get the cross-track error and the traveled distance for given vehicle’s position we simply have
to compute
Chapter 3. Control System Design and Developments 19

" # " #
xe T x − xk
= R (χp ) (3.18)
e y − yk

3.5 Current Estimation and Compensation

One of the main issues when operating marine robots, be it in submerged or semi-submerged, in
less a controlled environment - one that is not a dock - is the effect of the element τe (mentioned
in 2.6) on the vehicles position and on its course, which in turn can affect the performance of
outer loop algorithms with a static error.
The solution devised in [2] estimates the current in the inertial frame. This estimation is used by
a compensation unit, that also takes into account the desired course angle, to produce a desired
heading that accounts for the ocean currents.

3.5.1 Current observer

Kalman filter was employed to achieve the goal of current estimation, using a simple state space
formulation of the system given by

" # " #" # " #

ẋ 0 1 xm 1 h i
= + V xm
V ˙cx 0 0 V cx 0
(3.19)
ẋ = V cx + V xm
V ˙cx = 0

" #
h i x
m
Y = 1 0
V cx (3.20)
Y =x

the above formulation represents the estimation for position and current velocity in x-direction
only, similar can be stated for y-axis as
Chapter 3. Control System Design and Developments 20

" # " #" # " #

ẏ 0 1 ym 1 h i
= + V ym
V ˙cy 0 0 V cy 0
(3.21)
ẏ = V cy + V ym
V ˙cy = 0

" #
h i y
m
Y = 1 0
V cy (3.22)
Y =y

The measured position has been assumed to be achieved through GPS with added white noise
corruption to simulate measurement noise and the velocity[V xm , V ym ] are assumed to be derived
indirectly from DVL which gives us [um , vm ] as.

[V xm , V ym ]T = R(ψ)[u, v]T (3.23)

the implementation in the model has been depicted in the figure3.6 below

Figure 3.6: Kalman filter block for current estimation

Chapter 3. Control System Design and Developments 21

3.5.2 Current Compensation

Once the inertial current vector is known we can control our heading in order to compensate for
the current and follow the desired course, set by the guidance law.

Figure 3.7: Current Compensation (inspired by: dos Santos Ribeiro [3]), Please note in the
depiction h and vn et are denoted rather than ψ and u respectively, as in medusa

where χ/ = chid = course angle, hd = χd + α= compensated/desired heading and ψ=current

yaw, from 3.7 ,

" #
u
VT = R(ψ) + Vc (3.24)
v

" # " # " #

VT x u.cψ − v.sψ V cx
= + (3.25)
VT y u.sψ + v.cψ V cy

where VT = ṗ.making all the computation in a reference frame anchored in the course vector Vt
simplifies the relations therefore multiplying both sides of equation3.25 by RT (χd ) gives

||
" # " # " #
VT x
cos(χ) u.cos(χd − ψ) + v.sin(χd − ψ) Vc
= + (3.26)
0 −u.sin(χd − ψ) + v.cos(χd − ψ) Vc⊥
Chapter 3. Control System Design and Developments 22

Notice that now the current is expressed as having one component parallel to the the course
vector and another perpendicular to the course frame given by simply

||
" # " #
Vc T V cx
= R (ψ) (3.27)
Vc⊥ V cy

√
Now solving for second line of equation 3.26 and dividing it by Vnet = u2 + v 2

Vc⊥ u v
=√ sin(χd − ψ) − √ cos(χd − ψ) (3.28)
Vnet u2 + v 2 u2 + v 2

notice from fig.3.7 that,

v
sin(α) = √ (3.29)
u2 + v2

therefore 3.28 becomes

Vc⊥
= sin(χd − (ψ + α)) (3.30)
Vnet

also, hd (heading) = ψ + α, hence,

V⊥ 
hd = −sin−1 c
+ χd (3.31)
Vnet

subsequently the compensation is applied to the Guidance law where hLOS

d from the Guidance
law is fed as χd in the equation 3.31 and the compensated heading is produced.
Chapter 4

Inspection Manoeuvre Simulation

So, far we have mathematically formulated all the algorithmic background necessary for imple-
mentation of any manoeuvre using waypoint approach. Now we use the designed inner-outer
loop control system to perform Inspection Manoeuvre as mentioned in the Problem statement
initially. The task on hand is to perform a surveillance/ inspection manoeuvre by making the
AMV revolve around certain point of interest acting as the centre of the circular path and at
the same time keep the AMV front pointing towards the centre. Such a manoeuvre may be
necessary in case a camera, sensor or any other intervention accessory is attached to the AMV
to act on a certain object of interest in the waterbody.

4.1 Path following and Waypoint Generation

Even though the path is circular the LOS Guidance enabled waypoint approach has been
employed rather than curved path guidance for simplified implementation.
The problem was approached in following steps:

• The waypoints are created over the whole path with the origin of earth-fixed frame as
the initial point[x0 , y0 ] and the point of tangency between the circular path(radius R,
centerpoint [xc , yc ]) and line passing through origin as the next waypoint[x1 , y1 ] as follows :

x0 = 0,
y0 = 0,
p
r2 (x0 − xc ) − r(y0 − yc ) (x0 − xc )2 + (y0 − yc )2 − r2 (4.1)
x1 = xc + ,
(x0 − xc )2 + (y0 − yc )2
p
r2 (y0 − yc ) + r(x0 − xc ) (x0 − xc )2 + (y0 − yc )2 − r2
y1 = yc +
(x0 − xc )2 + (y0 − yc )2

23
Chapter 4. Inspection Manoeuvre and Simulation Results 24

• From this point onwards the waypoints are defined on the circular path at an angular
displacement ∆θ = 10 deg(in the model) with initial value θ0 given as

θ0 = atan2d(y1 − yc , x1 − xc ) (4.2)

also, subsequent waypoints are given as

θn = θn−1 + 10 ∀, n ≥ 1
xn+1 = xc + r.cosd(θn ) (4.3)
yn+1 = yc + r.sind(θn )

4.2 Simulink model

Figure 4.1: Simulink Model for Inspection Manoeuvre (inspired by [3])

In fig.4.1 the simulation model has been depicted with the network of various algorithm blocks
of which the mathematical modelling has been discussed so far in order of labelling as follows :

• Block 1 : Waypoint generator block generate path following waypoints(as discussed in

section 4.1).

• Block 2 : Reference Generator block implements Guidance Law(section 3.4.2), Current

Compensation(section 3.5.2)and generates yaw-u-v references for waypoint control(section
3.4.1).

• Block 3 :Dynamics block simulates the dynamic action on the AMV of the waterbody and
it’s own thrust action and outputs resulting Kinematics(section 2.2).

• Block 4 :This is the motor power block that simulates the transformation from control
signals to the RPM signals driving the Thrusters.(section 3.2.1).
Chapter 4. Inspection Manoeuvre and Simulation Results 25

• Block 5 : This block generates Kinematic parameters such as position, velocity and
acceleration of the AMV in the x and y-directions and feeds them to the Simscape block
for motion dynamics simulation.

• Block 6 : Simscape multibody dynamics was necessary in order for graphic visualization of
Inspection manoeuvre simulation to keep track of yaw direction as the vehicle traverses
the circular path.

Figure 4.2: Simscape multibody Dynamics block for graphic visualization of Inspection
manoeuvre

• Block 7 :Kalman filter block performs the Current observing and estimation task(section
3.5.1).

• Block 8 :These are the PID blocks for the controlled response generation of yaw, u and v
control signals(section 3.3).

• Block 9 :This block simulates the thruster action by transforming the RPM signals to net
forces and moments in turn driving the AMV(section 3.2.2).
Chapter 4. Inspection Manoeuvre and Simulation Results 26

4.3 Simulation Results and Conclusions

There are numerous aspects to be reviewed and certain factors to be considered as a result of
model simulation such as, control responses of the tuned PIDs, yaw, u, v, heading and net speed
tracking, thruster responses, current estimation results etc.

4.3.1 PID Tuning Results

PID tuning was undertaken on simplified linear model(shown in fig.4.3) which is different than
the one in fig.3.4 by the absence of thruster and motor power block in order to use matlab PID
tuning tool. Therefore the PIDs were tuned and then tuned parameters were used to simulate
the fig.3.4 model, also in it the anti-windup was employed for u and v controllers which had
saturation limits set and integral component in the controllers. The results were as follows : for

Figure 4.3: Simplified Model for PID tuning

the simulation, inputs were as follows : u= 1m/s ; v= 1m/s ; yaw= 60deg ; simulation time=
50sec also, the tuned control parameters were as follows : yaw(kp = 14.45, kI = 0, KD = 19.93),
u(kp = 38, kI = 7.25, KD = −8), v(kp = 48, kI = 9.25, KD = −10). Further ks at = [−100, 100]
was set for u-v controllers. resulting responses were recorded as
Chapter 4. Inspection Manoeuvre and Simulation Results 27

Figure 4.4: x-y displacement, u-v velocities and yaw responses of the tuned PID controllers

4.3.2 Inspection Manoeuvre Simulation Results

The simulation inputs were as follows :

• Waypoint generation :
[Xc , Yc ] = [−200, 150] : point of interest/ centerpoint of trajectory.
R = 100 : radius of the followed trajectory.
• Guidance :
d = 1 : neighbourhood radius
d= 7 : lookahead distance
ku = 1 : speed reference upper limit
ks = 0.1 : tuning parameter for vehicle speed with respect to distance error
• Kalman Filter-Noise Covariances
" #
0 0
Q=
0 0.01

R= 10−4
Chapter 4. Inspection Manoeuvre and Simulation Results 28

Figure 4.5: Actual and desired trajectory for Inspection Manoeuvre

4.5 depicts the plot of the Inspection Manoeuvre and as it can be noticed there is slight
difference/shift(towards 45deg direction)in the actually followed and the desired path. This may
be because of the current vector exactly in that direction[0.15, 0.15]m/s as in fig.4.7.
The fig.4.6 depicts the actual u, v and yaw as compared to the respective references generated.
Even though the system traces references closely there is substantial deviation in the references
itself which further amplifies in the actual responses because of the fact that the current estimation
deviations which inturn get highlighted in references during compensation and minor incapability
of controllers to compensate the deviations. Therefore this presents a scope of improvement in
the control aspect.

Figure 4.6: x-y displacement, u-v velocities and yaw responses of the tuned PID controllers
Chapter 4. Inspection Manoeuvre and Simulation Results 29

Figure 4.7: [V cx , V cy ] actual and [V ˆcx , Vˆcy ]estimated current velocities using Kalman filter
and GPS measured position and DVL measured velocities

fig.4.7 depicts the current estimation and it can be very well observed that it lies well within the
limits [0.15-0.1,0.5+0.1] which can be significantly improved based on the tuning of process and
measurement covariances. 4.8 present several notable timelines for observation of rest of the
plots in reference to vehicle’s position.

Figure 4.8: Simscape multibody simulation graphics snippets of some notable instants
Chapter 4. Inspection Manoeuvre and Simulation Results 30

Figure 4.9: PID Control Signals

4.9 depicts the control responses from PID controllers controlling u, v and yaw for the AMV, as
it can be seen there are highly noisy corrective response specifically in yaw response, this may
be because of noise amplifying ’D’ component of it’s controller, certain other factors such as
non-linearity of the system, inappropriate tuning etc may be factors responsible.
In 4.10 we observe the net forces and moments acting on the vehicle as the course of manoeuvre
is undertaken. Noticeable is the moment responsible for yaw which is significantly low, still
effective, this may be because of the assumed vehicle characterization parameters leading to such
a response as we feed actual vehicle inertial parameters the response may become significantly
realistic.

Figure 4.10: Forces and moment in Body Fixed Frame

Chapter 4. Inspection Manoeuvre and Simulation Results 31

Figure 4.11: Thruster RPMs Stabord front() and portside front()

fig.4.11 and fig.4.12 depict the generated RPM signals to be fed to the thrusters and as can
be seen the responses are in accordance with the system requirements at every instant. The
RPM limitations were unknown and can be regulated by the controllers based on the vehicle
characterization parameters including inertial and drag coefficients.

Figure 4.12: Thruster RPMs Stabord rear() and portside rear()

Chapter 5

Conclusion and Future Prospects

There have been a number of insightful observations while designing the control system for a 3
DOF Autonomous Marine Vehicle. Even though the whole design process has been inspired by
the past research on Medusa Class Vehicle, there were a number of challenges to be dealt with
as the designing was undertaken and differences among the vehicles became significant. Still
analogous relationship was maintained throughout for easier understanding and implementation.
Regarding the simulation results several conclusions were drawn as follows :

• The results have been presented for some specific values of input variables such as the
neighbourhood radius(d = 1), lookahead distance(d= 7) and Path radius(R= 100),for
which the system performs considerably well. But, this isn’t the case for all the values.

• The manoeuvre involves a circular trajectory however it was implemented using LOS
Guidance and using waypoint approach therefore it’s accuracy is bound to be dependent
on the density of the waypoints and that in our case depends on the (θ) parameter as in
section4.1. Also this is one specific implementation of certain scenario and would have to
be customized as per the manoeuvre specification.

• PID controllers for generating a controlled response to the non-linear dynamics of the
system were tuned using the PID tuning addon of Simulink for ease and simplicity, this
may be the reason for anomalies in the control response as observed in 4.9.

• Also in real hardware implementation of the control system, several factors such as control
signal saturation limits and PID parameters would have to be compensated for appropriate
responses within hardware limitations and suitable functionality.

• Also as the model is based on a hypothetical vehicle characterization, therefore responses

would come close to realistic behaviour only when realistic vehicle parameters are fed to
the system. This may suitably justify the yaw response in 4.9 and moment action in 4.10.

32
Chapter 5. Conclusion and Future Prospects 33

With so much covered in the study there still remains a lot of improvements and further
developments necessary in order to take the system to a realistic scale and for a productive
implementation. Some of these foreseeable goals have been put into perspective below :

• Variation in system input parameters as mentioned in the conclusions considerably affects

the system behaviour and is necessary to be further analyzed. Respectively improvisations
would have to be made for more robust behaviour.

• The control algorithms as mentioned in conclusions are highly specific and customized as
per the problem statement. They need to be generalized further for enhancing accuracy
and upgrading capabilities to act in multiple scenarios. One such improvement would be
to incorporate Guidance control for curved path following.

• The designed simulation model can be further easily adapted to controlling an actual
vehicle. ROS implementation can very easily serve the purpose because of it’s highly
sophisticated hardware-software interfacing capabilities.
Appendix A

Matlab-Simulink Code excerpts

A.1 Wapoint Generation Block

function [Xc,Yc,x0,y0,x1,y1,Dn,l,d,theta] = waypoint gen(x,y,t,x0,y0,x1,y1,theta)

r= 100;
Xc= -200;
Yc= 150;
l= 7;
Dn=1;
if(t== 0)
x0= 0.0;
y0= 0.0;
x1= Xc +((x0- Xc)*(rˆ2) - r*(y0- Yc)*sqrt((x0-Xc)ˆ2 + (y0-Yc)ˆ2 -rˆ2))/((x0-Xc)ˆ2 + (y0-
Yc)ˆ2);
y1= Yc +((y0- Yc)*(rˆ2) + r*(x0- Xc)*sqrt((x0-Xc)ˆ2 + (y0-Yc)ˆ2 -rˆ2))/((x0-Xc)ˆ2 + (y0-
Yc)ˆ2);
theta= atan2d(y1- Yc, x1- Xc);
end
d= sqrt((x-x1)ˆ2 + (y-y1)ˆ2);
if(d < 5)
x0= x1;
y0= y1;
theta= theta+ 10;
x1= Xc+ r*cosd(theta);
y1= Yc+ r*sind(theta);

34
Appendix A. Matlab Code 35

end end

A.2 Reference Generator Block

function [Yref,Uref,Vref,Sref,href,d,A,Vcper,hrefC] = refernce gen(yaw,x,y,xc,yc,x0,y0,x1,y1,Dn,l,d0,Vcx,Vcy,Vne

Smax =1;
d = d0-Dn;
Xp =(atan2( y1 - y0,x1 - x0 ))*180/pi;
R = [cosd(Xp) -sind(Xp) ; sind(Xp) cosd(Xp)];
E = transpose(R)*[x - x0 ; y - y0];
href = atand(-E(2)/l)+ Xp;
Vcper = -Vcx * sind(href) + Vcy * cosd(href);
if(Vnet==0)
hrefC = href;
elseif(abs(Vcper)>Vnet)
hrefC= href ;
else
hrefC= -asind(-Vcper/Vnet) + href ;
Yref=atan2(yc-y, xc-x)*180/pi;
dif=hrefC-yaw;
A = tand(dif);
if(d <=2 && d>=-2)
Uref=0;
Vref=0;
else
if(A>-1 && A<1)
if((dif>-45 && dif<45)——(dif >315)——(dif<-315))
Uref = Smax*asin(d/(abs(d)+0.1))*2/pi;
Vref = Uref*A;
else
Uref =-Smax*asin(d/(abs(d)+0.1))*2/pi;
Vref = Uref*A;
end
else
if((dif>45 && dif<135)——(dif<-225 && dif>-315))
Vref = Smax*asin(d/(abs(d)+0.1))*2/pi;
Uref = Vref/A;
Appendix A. Matlab Code 36

else
Vref =-Smax*asin(d/(abs(d)+0.1))*2/pi;
Uref = Vref/A;
end
end
end

Sref = sqrt(Uref*Uref + Vref*Vref);

A.3 Motor Power block

function [Smf,Pmf,Smr,Pmr] = fcn(DmYaw,Cm uSpeed,Cm vSpeed)

Cm usqr=Cm uSpeed*abs(Cm uSpeed);

Cm vsqr=Cm vSpeed*abs(Cm vSpeed);
Dm ysqr=DmYaw;

Smf = round(sqrt(abs(Cm usqr-Dm ysqr-Cm vsqr))*sign(Cm usqr-Dm ysqr-Cm vsqr));

Pmf = round(sqrt(abs(Cm usqr+Dm ysqr+Cm vsqr))*sign(Cm usqr+Dm ysqr+Cm vsqr));
Smr = round(sqrt(abs(Cm usqr-Dm ysqr+Cm vsqr))*sign(Cm usqr-Dm ysqr+Cm vsqr));
Pmr = round(sqrt(abs(Cm usqr+Dm ysqr-Cm vsqr))*sign(Cm usqr+Dm ysqr-Cm vsqr));
Bibliography

[1] António Pascoal António Pedro Aguiar. “Dynamic positioning and way-point tracking of
underactuated AUVs in the presence of ocean currents”. In: International Journal of Control
(2007).

[2] Guilherme Manuel Vilela Sanches. “Sensor-Based Formation Control of Autonomous Marine
Robots”. In: Master’s thesis, Instituto Superior Técnico (2015).

[3] J. M. dos Santos Ribeiro. “Motion control of single and multiple autonomous marine
vehicles.” In: Master’s thesis, Instituto Superior Técnico (2011).

37