Sensor-based Formation Control of Autonomous
Underwater Vehicles
Pedro Caldeira Abreu
Thesis to obtain the Master of Science Degree in
Aerospace Engineering
Supervisor: Dr. António Manuel dos Santos Pascoal
Examination Committee
Chairperson: Dr. João Manuel Lage de Miranda Lemos
Supervisor: Dr. António Manuel dos Santos Pascoal
Members of the Committee: Dr. António Pedro Rodrigues de Aguiar
December 2014
Abstract
This thesis addresses the subject of formation control of underwater vehicles, using either inter-vehicle
relative position measurements or range-only measurements. The work is motivated by the MORPH
project, aiming to use formations of vehicles with complementary capabilities to map the underwater
environment in challenging conditions. The problem studied is that of driving a vehicle (follower) to an
appropriately defined position with respect to another vehicle (leader), which moves along an unknown
trajectory. This control task is divided into two steps. Firstly, a trajectory-tracking controller is designed,
so that a follower vehicle can track a specified virtual target. Conditions under which the tracking error
converges to zero or remains bounded are derived using tools from nonlinear systems theory. Secondly, we
design path-generators that provide the position and velocity of the virtual target to be tracked, for both
the relative position measurements and the range-only measurements scenarios. These path-generation
strategies take into account the formation objectives, typical maneuver geometries and limitations of
underwater inter-vehicle communications. Applications envisioned in the scope of the MORPH project
are presented and discussed. The implementation of the strategies proposed for formation control with
relative position measurements is described, taking explicitly into account the filter architectures required
for relative vehicle navigation. Finally, results of simulations and sea trials are shown, with position errors
generally below 1 m in MORPH trials in Lisbon and the Azores.
Keywords: Autonomous marine vehicles, formation control, leader-follower formation, trajectory-
tracking.
Resumo
Esta tese aborda o tema de controlo de formação de veículos subaquáticos, usando medidas de posição
relativa ou apenas de distância entre os veículos. O trabalho é motivado pelo projecto MORPH, que
procura usar formações de veículos com capacidades complementares para mapear ambientes subaquáticos
em condições difíceis. Estuda-se o problema de conduzir um veículo (seguidor) a uma posição definida
em relação a outro veículo (líder) que descreve uma trajectória desconhecida a-priori. Este problema
de controlo é dividido em dois passos. Primeiramente, desenha-se um controlador de seguimento de
trajectórias para que um veículo seguidor consiga seguir um alvo virtual. Derivam-se condições para
que o erro de seguimento convirja para zero ou se mantenha limitado, usando ferramentas de teoria de
sistemas não-lineares. Seguidamente, desenham-se geradores de caminho que determinam a posição e
velocidade do alvo virtual a ser seguido, para os casos em que se usam medições de posição relativa ou
apenas de distância. Estes geradores de caminho têm em conta os objectivos da formação, geometrias
típicas de missões e limitações dos sistemas de comunicação subaquáticos. Apresentam-se aplicações
idealizadas no âmbito do projecto MORPH. Descreve-se a implementação das estratégias apresentadas
para controlo de formação com medidas de posição relativa, tendo em conta explicitamente a arquitectura
de estimação necessária para navegação relativa entre veículos. Finalmente, apresentam-se resultados de
simulação e na água, com erros de posição em geral inferiores a 1 m em testes do projecto MORPH, em
Lisboa e nos Açores.
Palavras-chave: Controlo de formação, formação líder-seguidor, seguimento de trajectórias, veículos
marinhos autónomos.
Contents
List of Tables I
List of Figures III
List of Acronyms V
List of Symbols VII
1 Introduction 1
1.1 Historical background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.1.1 Marine robotics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.1.2 Cooperation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.3 Problem statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.4 Previous work and main contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.4.1 Motion Control of Marine Vehicles . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.4.2 Formation Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
1.4.3 MORPH project . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
1.5 Thesis outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2 Mathematical Preliminaries 9
2.1 Norms . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.2 Quadratic forms . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.3 Nonlinear systems theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.3.1 Properties of differential equations . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.3.2 Lyapunov stability theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.3.3 Boundedness . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2.3.4 Input-to-state Stability . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
3 AUV Dynamics 19
3.1 Reference frames and naming conventions . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.2 Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
3.3 Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
3.4 Horizontal plane dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
3.5 The MEDUSA-class vehicle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
4 Trajectory-Tracking 27
4.1 Problem definition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
4.2 Outer-loop control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
4.3 Closed-loop dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
4.4 Application to Formation Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
4.4.1 Leader-follower formations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
4.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
5 Formation Control 43
5.1 Formation control with relative position measurements . . . . . . . . . . . . . . . . . . . . 43
5.1.1 Formation strategies . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
5.1.2 Path-generator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
5.1.3 Estimation of constant current . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
5.1.4 Applications to the MORPH upper segment . . . . . . . . . . . . . . . . . . . . . . 53
5.2 Formation control with range-only measurements . . . . . . . . . . . . . . . . . . . . . . . 54
5.2.1 The localization problem . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
5.2.2 Path-generator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
5.2.3 Estimation of virtual target velocity and constant current . . . . . . . . . . . . . . 58
5.2.4 Applications to the MORPH lower segment . . . . . . . . . . . . . . . . . . . . . . 59
5.3 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
6 Implementation 61
6.1 Trajectory-tracking controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
6.1.1 Issues with commanded heading . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
6.2 Communication . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
6.3 Filtering and estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
6.3.1 SSV path parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
6.3.2 Follower’s estimate of Leader’s path parameters . . . . . . . . . . . . . . . . . . . . 66
6.3.3 Currents and Follower-Leader relative position . . . . . . . . . . . . . . . . . . . . 66
6.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
7 Results 69
7.1 Trajectory-Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
7.2 Upper Segment Formation Control with currents . . . . . . . . . . . . . . . . . . . . . . . 71
7.3 Upper and Lower Segment Formation Control . . . . . . . . . . . . . . . . . . . . . . . . . 72
7.4 Real trial results: MEDUSAs in Lisbon . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
7.5 Real trial results: SeaCat in Lisbon . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74
7.6 Real trial results: MEDUSAs in the Azores . . . . . . . . . . . . . . . . . . . . . . . . . . 75
7.7 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
8 Conclusion and Future Work 79
8.1 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79
8.2 Future work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80
Bibliography 81
List of Tables
3.1 SNAME nomenclature and symbols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.2 Mass, inertia and hydrodynamic coefficients for a MEDUSA vehicle at the surface . . . . 25
I
List of Figures
1.1 A possible MORPH configuration as defined initially. . . . . . . . . . . . . . . . . . . . . . 4
1.2 The MORPH configuration on a negative-slope cliff. . . . . . . . . . . . . . . . . . . . . . 5
3.1 A schematic drawing of a non-diving MEDUSA vehicle and the inertial and body-fixed
frames. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.2 A MEDUSA vehicle operating at the surface during MORPH trials in Toulon, 2013. . . . 25
4.1 Effect of K and n in the tanh(e) function, in the scalar case. . . . . . . . . . . . . . . . . 30
4.2 Block diagram of the complete system, including the path generator. . . . . . . . . . . . . 39
4.3 Block diagram of the multi-agent system. . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
5.1 Desired position of a follower vehicle based on the previous path of the leader. . . . . . . 45
5.2 Position (in blue) and velocity of the point to be tracked, based on the motion of the leader
(in red). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
5.3 Information flow in the MORPH upper segment. . . . . . . . . . . . . . . . . . . . . . . . 53
5.4 Ambiguity when determining the position of a follower in {M} based on the ranges between
the follower and two upper segment vehicles. . . . . . . . . . . . . . . . . . . . . . . . . . 55
5.5 Follower vehicle and desired point, along with the reference frames and position vectors. . 57
5.6 Information flow in MORPH upper and lower segments. . . . . . . . . . . . . . . . . . . . 60
6.1 Estimator running on SSV (drawn in collaboration with Mohammadreza Bayat). . . . . . 66
6.2 Estimation and control in underwater followers of the upper segment (drawn in collabora-
tion with Mohammadreza Bayat). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
7.1 Results on simulated trajectory-tracking. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
7.2 Results on simulated upper segment formation control with currents. . . . . . . . . . . . . 71
7.3 Nominal formation geometry (left) and leader-follower topology (right) for this simulation.
Normal and dashed arrows denote following using relative position measurements or range-
only measurements, respectively. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
7.4 Results on simulated upper and lower segment formation control with currents. . . . . . . 73
7.5 Results on trials with MEDUSAs in Lisbon. . . . . . . . . . . . . . . . . . . . . . . . . . . 74
7.6 Results on trials with Atlas Elektronik SeaCat in Lisbon. . . . . . . . . . . . . . . . . . . 75
7.7 Trajectory of the SSV for the complete mission. . . . . . . . . . . . . . . . . . . . . . . . . 76
III
7.8 Results on trials with MEDUSAs in the Azores. . . . . . . . . . . . . . . . . . . . . . . . . 76
7.9 5-vehicle formation missions in the Azores. . . . . . . . . . . . . . . . . . . . . . . . . . . . 77
IV
List of Acronyms
ASV Autonomous Surface Vehicle
AUV Autonomous Underwater Vehicle
BIBS Bounded-input Bounded-state
C1V, C2V Camera Vehicles 1 and 2
CG Center of Gravity
CM Center of Mass
CPF Cooperative Path-Following
DOF Degrees of Freedom
DP Dynamic Positioning
DVL Doppler Velocity Log
GCV Global Communications Vehicle
GNSS Global Navigation Satellite System
GPS Global Positioning System
ISS Input-to-State Stable/Stability
LQR Linear Quadratic Regulator
LSV Local Sonar Vehicle
MBE Multibeam Echosounder
NED North East Down
PF Path-Following
PID Proportional Integral Derivative
ROF Range-only Formation
ROV Remotely Operated Vehicle
SSV Surface Support Vehicle
TDMA Time Division Multiple Access
TT Trajectory-Tracking
UAV Unmanned Aerial Vehicle
USBL Ultra-Short Baseline
V
List of Symbols
Symbol Definition
{B} Body-fixed frame
{I} Flat-earth navigation frame (local NED)
{M} Lower-segment mobile frame
A System matrix
B Input matrix
C Matrix of Coriolis and centrifugal terms; output matrix
d, d1, d2 Inter-vehicle distances in the lower segment
d Disturbance term including the velocity of a virtual target and the current
D Drag matrix
D′ Drag matrix without constant entries
e Position error in {B}
e′ Modified position error in {B}
eL, eU Error of relative position estimate in {I} for lower and upper segment vehicles respectively
In Identity matrix with n rows and columns
I32 Modified identity matrix, obtained by inserting a row of zeros in between the two rows of I2
K Outer-loop controller gain matrix
Kd Inner-loop controller gain matrix
Ko Observer gain matrix
M Mass and Inertia matrix
n Outer-loop controller sensitivity parameter
p Position of a follower vehicle in {I}
pd Position of a virtual target in {I}
pL Position of leader vehicle in {I}
pd Velocity of a virtual target measured and expressed in {I}Mp Position of a follower vehicle in {M}Mp1, Mp2 Position of leader vehicles in {M}Mpd Position of a virtual target in {M}
r Yaw rate
rd Commanded yaw rate
VII
R(·) Rotation matrix
S(·) Skew-symmetric matrix arising in time-differentiation of R(·)
t Time
u Surge speed
ud Commanded surge
u Vector of surge u and yaw rate r; Input vector
ud Vector of commanded surge ud and yaw rate rd
v Sway speed
V Lyapunov function
VL Magnitude of velocity of leader vehicle measured in {I}
v Vector of surge u and sway v
Vc Velocity vector of the current measured and expressed in {I}
x North-coordinate of the position of the origin of {B} in {I}
xB Longitudinal axis of {B}
x State vector
y East-coordinate of the position of the origin of {B} in {I}
yB Transverse axis of {B}
z Down-coordinate of the position of the origin of {B} in {I}
zB Normal axis of {B}
z Input vector (in estimators)
Γ Yaw rate to lateral speed scaling matrix
δ Distance between the CM of the vehicle and the point to be driven to the target, along xB
δx, δy Formation parameters
δ Vector of formation parameters
λ(·) Eigenvalue(s)
ν Vector of surge u, sway v, and yaw rate r
νd Vector of commanded surge ud, sway vd = 0, and yaw rate rd
τu Actuator force
τr Actuator moment
τ Vector of actuator force and moment
χL Course angle of leader vehicle
χL Course angle rate of leader vehicle
ψ Heading angle
ψd Commanded heading angle
VIII
Chapter 1
Introduction
1.1 Historical background
Ship navigation is one of the oldest scientific challenges. Ever since humans attempted to use vessels,
originally for both coastal fishing and travelling across small distances, a variety of techniques were
employed to monitor and control the motion of marine craft. Staying in sight of land and understanding
the winds were the most important navigation skills used by the first sailors. The Minoans, a Bronze Age
civilization that arose in the island of Crete, are thought to have been the first to use celestial navigation
techniques to travel both by night and across open water, albeit for short distances. The Phoenicians,
and later the Carthaginians, developed a number of techniques and used the position of the Polaris
star to navigate in open waters, allowing them to develop a maritime trading culture across the entire
Mediterranean. In the Pacific, the Polynesians also devised strategies to navigate between islands based
on the combined observation of stars, weather, wildlife species, characteristics of waves and clouds, and
colors of the sky and sea. In the medieval age, the Arabs introduced the magnetic compass and celestial
navigation instruments, like the quadrant, and the Chinese are also known to have used the magnetic
compass for navigation purposes.
The Portuguese expeditions in the early 15th century started an attempt to devise systematic ap-
proaches to navigation, in what has been considered by some the beginning of modern science [1]. This
scientific approach revolutionized navigation, with significant contributions in developing and perfect-
ing both navigation instruments and celestial observation tables and charts. The practical implications
were astounding: around a hundred years after the first Portuguese oceanic expedition, leading to the
discovery of Porto Santo in 1418, the first world circumnavigation was completed. More advanced tech-
niques and instruments were later introduced by the British in the 18th century. In the late 19th century
chronometers and wireless telegraphs were already used at sea.
The invention of the gyrocompass in the early 20th century was a key breakthrough in maritime
automation [2]. In 1911, Elmer Sperry produced the first closed-loop ship steering mechanism - the
Metal Mike, a gyroscope-guided autopilot. In 1922, Nicholas Minorsky analysed a position feedback
control system using a three-term law, known today as PID (Proportional-Integral-Derivative) control,
1
motivated by his observations of the behaviour of skilled pilots [3]. Advances in control and estimation
theory, as well as the introduction of local and later global navigation systems like GPS, led to increasingly
successful applications of a number of control systems throughout the 20th century, including autopilots,
dynamic positioning (DP) and way-point tracking systems.
1.1.1 Marine robotics
The increasing level of automation in marine vessels and the expansion of offshore oil and gas opera-
tions during the second half of the 20th century prompted an interest in unmanned marine vehicles [4].
The first remotely operated vehicle (ROV) was built in 1953, and ROVs eventually became established in
the offshore oil and gas sector in the 1980s, providing assistance in the development of offshore oil fields
when a big part of this development was beyond reach of human divers. More recently, technological de-
velopment in power supplies, computational capabilities and acoustic communications paved the ground
for autonomous underwater vehicles (AUVs), which are now becoming increasingly commonplace in a
number of scientific, military and commercial applications. Compared to ROVs, AUVs lift the restric-
tions imposed by tethers, at the expense of more difficult communications and the need to carry energy
supplies onboard.
In general, replacing traditional vessels (either at surface or underwater) with unmanned vehicles
results in less exposure of human lives to the harsh marine environment, more flexible and cheaper
operations, and usually increased maneuverability. The trend is that AUVs become cheaper and better
in terms of autonomy, reliability and decision-making capabilities. ROVs and AUVs will also become
increasingly closer in functionality, with the development of higher bandwidth acoustic systems that can
allow human supervision in real-time without tethers.
1.1.2 Cooperation
Within the more general scope of robotics, a wide number of successful applications of inter-vehicle
cooperation strategies have been reported. Swarms of cooperating ground robots have been used in a
variety of scenarios, ranging from purely academic competitions (e.g. RoboCup, a robotic football com-
petition) to large-scale industrial applications. In this context, one of the most influential breakthroughs
was the first permanent deployment and installation of the large-scale automated warehouse management
system Kiva [5], in 2006. Kiva is now used by a number of companies and its cooperating robots handle
millions of orders a year. A Kiva installation for a large distribution center typically employs more than
500 individual robots.
Cooperation of multiple Unmanned Aerial Vehicles (UAVs) has also seen increased interest in the
last years. From a technical standpoint, aerial vehicles have much more freedom of motion when com-
pared to ground robots, who often have to cope with obstacles or rough terrain. On the other hand,
when compared to underwater robotics, aerial vehicles can communicate much more effectively, since
electromagnetic communication channels have much higher bandwidth than their acoustic counterparts
that are used underwater. These two aspects make aerial vehicles a good platform for implementing
2
distributed, cooperative groups of robots. Research on military applications of groups of intelligent UAVs
has been ongoing for some time, but the rapidly decreasing costs of fixed-wing UAVs have spurred a
recent widespread interest in civil and commercial applications, including mapping and surveillance of
wide areas, border patrol, search and rescue, and formation flight to reduce aerodynamic drag [6]. More
specific work on cooperative motion control of UAVs can be found in [7, 8]. In [9], cooperative motion
control is applied to spacecraft formations.
This thesis, however, focuses on applications of robot cooperation in marine environments. The lim-
ited sensing and communication capabilities of AUVs make it especially challenging to perform distributed
high-level cooperation and decision-making. Hence, with some exceptions [10], most strategies for co-
operation of autonomous marine vehicles involve cooperation only on the level of motion control, as is
the case of formation control. Networks of marine vehicles moving cooperatively in formations have a
vast number of potential applications, and yield considerable operational advantages compared to using
a single vehicle. Regarding mapping and surveying missions, AUVs can move in formations so as to
cover large areas in minimum time. Formations can also be made robust to failure of one element of the
network. Another potential advantage is that vehicles are allowed to carry complementary equipment,
avoiding the use of a single heavily-equipped vehicle.
Specific applications of vehicle cooperation in marine environments reported in the literature in-
clude various scenarios and projects. The ASIMOV project [11] aimed at coordinated operations of an
Autonomous Surface Vehicle (ASV) and an AUV, and the ensemble was used to study shallow water hy-
drotermalism in the Azores. In [12], the problem of leaderless formation control with limited intervessel
communication is addressed, and an underway replenishment scenario between three ships is simulated
for demonstration. More recently, the GREX project [13] involved coordination and control of cooperat-
ing heterogeneous marine vehicles in uncertain environments. Three scenarios were reported as possible
applications of the work developed: search for hydrothermal vents, fish-data downloading and marine
habitat mapping.
1.2 Motivation
Within the framework of cooperation of marine robots for the purpose of ocean exploration, namely
surveying of the seabed, some difficulties arise. Scientifically interesting areas, from geological or biolog-
ical standpoints, usually occur in challenging, unstructured environments with rough terrain. In these
conditions, dead-reckoning navigation instruments like the Doppler Velocity Log (DVL) can provide un-
reliable results. ROVs can be used to tackle some of these problems, but the tether imposes limitations
regarding the motion of the vehicle and global positioning is still inaccurate when operating near complex
terrain features.
In this scope, the MORPH project (marine robotic system of self-organizing, logically linked physical
nodes) [14] brings together a consortium involving multiple partners from both the industry and academia.
It proposes a novel concept of a physically morphing robotic structure, composed of several intelligent,
self-propelled, heterogeneous robotic nodes with complementary capabilities. These nodes are linked via
3
acoustic communication channels, and not limited in any way by rigid links. The idea is that each robotic
node is a small and relatively cheap vehicle, but the overall system can overcome some of the difficulties
arising in challenging environments by cooperating and making use of complementary navigation, sensing
and communication capabilities.
Operating effectively in challenging environments with rough terrain and truly 3D features is then
the main difficulty that the MORPH concept attempts to overcome. One of the instrumental aspects in
controlling the motion of the robotic nodes in these environments is formation keeping. This is the main
motivation for this thesis: to develop formation control strategies that can be applied to MORPH. The
distributed robotic nodes should maintain formations according to the objectives of the project, using
either range measurements or relative position measurements obtained using an Ultra-Short Baseline
(USBL). These strategies should take into account the practical limitations posed by the environment
and onboard equipment, namely low sampling frequencies, low bandwidth and significant number of
failures and outliers in acoustic measurements and communications.
A possible configuration initially defined for the MORPH system is shown in Figure 1.1, consisting of
five heterogeneous vehicles. The example scenario is 3D mapping of the seabed. A flat bottom is shown
for simplicity of presentation.
Figure 1.1: A possible MORPH configuration as defined initially.
These five vehicles are divided into the upper segment and the lower segment of the MORPH structure.
The upper segment consists of three vehicles.
• SSV (Surface Support Vehicle). The SSV operates at the surface and its main role is global
navigation, by resorting to global navigation satellite systems (GNSS), which can only be used at the
surface. Global navigation is necessary for geo-referencing, ensuring that the region being mapped
can later be located again and that the MORPH system is operating in the planned location.
• GCV (Global Communications Vehicle). The GCV is responsible for maintaining the accuracy
of inter-vehicle navigation and relaying communications. It moves below the surface, far from the
4
influence of sea waves but still considerably above the seabed, and communicates directly with all
the other vehicles. Without the GCV at least one of the other underwater vehicles would have
to communicate directly with the SSV. The importance of this vehicle is better illustrated by
considering an example of operation in a challenging scenario, like the one shown in Figure 1.2.
In this case, the geometry of the environment could block the acoustic communication channel
between the SSV and the vehicles operating close to the cliff (LSV, C1V, C2V). The GCV solves
this problem by acting as a relay and staying within acoustic reach of all vehicles.
• LSV (Local Sonar Vehicle). The LSV carries the multibeam echosounder (MBE) for 3D mapping
of the terrain. Since the MBE is the most expensive equipment in the entire MORPH system, the
LSV should keep a safe distance from the seabed.
The lower segment consists of two other vehicles, which have identical roles.
• C1V, C2V (Camera Vehicles). The C1V and C2V carry onboard high-definition cameras.
The visibility and angle of view of the cameras are, in general, smaller than those of the MBE.
Consequently, the camera vehicles need to operate much closer to the bottom and perform accurate
bottom-tracking to avoid collisions. However, the risk involved in close bottom-tracking motivates
the idea that the camera vehicles should keep expensive onboard equipment to a minimum. Hence,
besides the acoustic information channel, they should keep the desired inter-vehicle formation by
using range-only measurements, as opposed to relative position measurements like those provided
by Ultra-Short Baseline (USBL) systems.
Figure 1.2: The MORPH configuration on a negative-slope cliff.
The example scenario in Figure 1.2, infeasible with current AUV technology, shows how the MORPH
project aims to use a cooperative multi-vehicle approach to tackle challenging geometries, such as that of
a negative-slope cliff.
5
1.3 Problem statement
This thesis addresses two formation control problems, motivated by the upper segment and lower
segment formation control problems in MORPH. These two problems are:
1. Formation control with relative position measurements; in the context of MORPH upper segment,
drive GCV and LSV (followers) to appropriate positions relative to SSV (leader), using relative
position measurements obtained from a USBL (that can be placed in any vehicle) without any
direct communication between the SSV and LSV.
2. Formation control with range-only measurements; in the context of MORPH lower segment, drive
C1V and C2V (followers) to appropriate positions relative to GCV and LSV (leaders), resorting to
range-only measurements.
These problems are tackled in terms of motion control, path-generation, and estimation. In both
problems, the motion of the leaders/neighbors is not known a-priori by the followers. The strategies
developed should be easily applicable to vehicles with different dynamics. The problems are addressed
considering motion on a horizontal plane only, under the assumption that the depth is kept constant by
an external controller.
1.4 Previous work and main contributions
In this section, prominent previous work regarding motion and formation control of marine vehi-
cles is introduced. Then, work previously developed in the scope of the MORPH project is presented.
Concerning each of these subjects, the main contributions of this thesis are pointed out.
1.4.1 Motion Control of Marine Vehicles
In the last decades, with ROVs and AUVs becoming increasingly commonplace for a number of
scientific and industrial purposes, the subject of motion control of unmanned underwater vehicles has
received increased attention. Numerous reports of applications of a variety of control techniques were
published, including PID control [15], Linear-Quadratic-Regulator (LQR) control [16], and robust control
techniques [17]. Recently, nonlinear control tools were applied to marine vehicles. Particularly prominent
work in this area was carried out by Thor Fossen; in [18], significant contributions were made in this
field, regarding both modelling and control using nonlinear techniques. Comprehensive studies of modern
research on guidance and control systems for marine vehicles are included in [2, 19].
Within the subject of motion control, trajectory-tracking (TT) and path-following (PF) are two
similar problems widely covered in the literature. While trajectory-tracking forces a vehicle to track a
desired time-parametrized trajectory, path-following only requires a vehicle to follow a prescribed path
with a desired speed profile but no associated timing law. Numerous examples of trajectory-tracking
controllers for marine vehicles have been reported. In this context, designs based on nonlinear Lyapunov-
based tools overcome some limitations imposed by feedback linearization or gain scheduling. In [20], the
6
problems of trajectory-tracking and path-following for the specific case of underactuated autonomous
vehicles are addressed using nonlinear tools, and convergence of the tracking error to an arbitrarily small
neighborhood of zero is achieved using backstepping. A model structure covering a wide class of vehicles
is considered; parametric uncertainty is addressed through the use of supervisory switching logic, which
overcomes some limitations of adaptive control techniques [21].
The problem of trajectory-tracking is of significant importance in this thesis. The approach taken
assumes that full state feedback is not possible, and hence the algorithms presented in [20] cannot be
used. Instead, a decoupled inner-outer-loop control architecture is proposed. Using a nonlinear model
structure based on [18], both the inner and outer-loop controllers are designed using simple nonlinear
control tools and borrowing from [22]. A stability analysis of the closed-loop is carried out, yielding
the main contribution of this thesis regarding trajectory-tracking in a decoupled inner-outer-loop control
architecture: a local bounded-input-bounded-state stability result, with respect to an estimation error on
the velocities of the current and of the virtual target, the estimate itself, and its time-derivative.
1.4.2 Formation Control
A number of different techniques for formation control are reported in the literature. In case all
vehicles have a-priori knowledge of the path to be followed and are intended to maintain a desired,
possibly time-varying geometrical formation pattern, the control problem is called cooperative path-
following (CPF). The problem is studied, for example, in [23,24]. Applications to underwater and aerial
vehicles are reported in [25] and [26] respectively. Important results for this problem are shown in [27],
where conditions are derived under which formation errors can be driven to a neighborhood of zero in
the presence of communication losses and time-delays.
If the path is not known a-priori, we distinguish several relevant techniques for formation control [28].
The potential field approach consists of using artificially created potential field functions to accomplish
formation-keeping [29]; in [30], the authors use potential field functions that arise naturally from formation
constraints and avoid collisions. The leader-follower approach [31] consists of controlling a follower vehicle
in such a way that its distances or angles with respect to one or more leader vehicles are driven to desired
values. In [32], Model Predictive Control (MPC) is used to control the vehicles in a formation via
optimization-based methods. In the virtual structure method, a virtual structure with desired dynamics
is defined. The motion of each vehicle is determined by the motion of this structure, and individual
tracking controllers are responsible for driving each vehicle to its intended position. These techniques
are, in general, well studied for linear systems; formation control solutions for nonlinear systems, on the
other hand, are usually targeted towards specific applications and restricted by some conditions.
For underwater vehicles, formation strategies are usually restricted by the bandwidth of acoustic
communication channels. This precludes the use of approaches that require frequent, heavy inter-vehicle
communications. The approach taken in this thesis in terms of formation control borrows from the virtual
structure method, in that a path-generator produces a virtual target for each follower vehicle; a tracking
controller is then used to drive the vehicle towards the virtual target. It also borrows from the leader-
follower approach, as the virtual targets for the follower vehicles are generated based on information from
7
a leader vehicle. The main contribution of this thesis in terms of formation control is a path-generation
strategy for leader-follower formations with desirable properties for underwater vehicles, targeted towards
typical mission geometries arising, for example, in seabed surveying. This path-generator is independent
of the motion of the follower itself, so it connects naturally with tracking controllers via a cascade
interconnection.
1.4.3 MORPH project
Within the MORPH project, most previous work focuses on range-only formation (ROF) control.
In [33–36], control systems for formation-keeping using range-only measurements are described, along
with its implementation and results in real conditions. This work was mainly targeted towards the
problem of lower segment control in MORPH.
This thesis contributes to the project by 1) proposing possible implementations of the analysed con-
trollers and estimators to both the upper and lower segment of MORPH, and 2) validating the use of the
proposed strategies in the upper segment with results obtained in sea trials, with different vehicles.
1.5 Thesis outline
Chapter 2 describes the notation used throughout the thesis and introduces important mathematical
concepts regarding nonlinear systems theory.
Chapter 3 starts by introducing nomenclature commonly used in the field of marine vehicles, proceeds
to briefly describe the procedure leading to a dynamic 6-degrees of freedom (DOF) model for MEDUSA-
class vehicles, and introduces simplifications to obtain a horizontal-plane, 3-DOF model.
Chapter 4 casts the trajectory-tracking problem in a mathematical setting and analyses a decoupled
inner-outer-loop control strategy within the framework of Lyapunov stability. The use of trajectory-
tracking algorithms to accomplish formation control objectives is clarified.
Chapter 5 addresses the problems of formation control with relative position measurements and with
range-only measurements. The solutions proposed generate appropriate commands to be fed to the
trajectory-tracking controller from Chapter 4. Some conclusions are drawn regarding the necessary
information flow between vehicles for the solutions to be applicable in the MORPH project.
Chapter 6 describes the implementation of the analysed control techniques for the upper segment of
MORPH, including changes in the control algorithms, estimators, and communication architecture.
Chapter 7 presents both simulated and real results for the control problems addressed throughout the
thesis. The real results were obtained at sea trials in Lisbon and the Azores, with different vehicles.
8
Chapter 2
Mathematical Preliminaries
This chapter presents some useful mathematical concepts and notation used throughout this thesis.
Notation involving vector and matrix norms is introduced firstly. Some properties regarding matrices and
quadratic forms follow. A very brief overview of Lyapunov stability theory is presented later, including
the main theorems used in the analysis of nonlinear systems.
Mathematical properties involving norms, matrices and quadratic forms were borrowed from a number
of references, including [22, 37]. The results and definitions regarding stability theory borrow especially
from [37,38].
2.1 Norms
A norm ‖x‖ of a vector x ∈ Rn is any real-valued function satisfying a set of properties [37]. In this
thesis, the class of p-norms is especially useful. A p-norm of a vector x ∈ Rn is defined as
‖x‖p =
(n∑i=1
|xi|p)1/p
(2.1)
where xi denotes the ith component of x, |xi| denotes its absolute value, and p ≥ 1. A particularly
case is p = 2, yielding the Euclidean or 2-norm. The norm symbol without subscript, ‖x‖, is used to
denote the Euclidean norm.
Another useful definition is the ∞-norm
‖x‖∞ = maxi|xi| (2.2)
Two important inequalities regarding norms are:
• the triangular inequality, ‖x+ y‖ ≤ ‖x‖+ ‖y‖ ∀x,y ∈ Rn
• the Hölder inequality, |xTy| ≤ ‖x‖p ‖y‖q ,1p + 1
q = 1, ∀x,y ∈ Rn
Due to the positiveness of norms, Young’s inequality is also useful, especially with exponent 2:
ab ≤ a2
2ε+εb2
2, ∀ a, b ≥ 0, ε > 0 (2.3)
9
Norms can also be defined for matrices. The induced p-norm of a matrix A ∈ Rn×m is defined as
‖A‖p = supx 6=0
‖Ax‖p‖x‖p
(2.4)
Given this definition, useful properties arise:
‖Ax‖p ≤ ‖A‖p ‖x‖p (2.5)
‖AB‖p ≤ ‖A‖p ‖B‖p (2.6)
For the specific case of p = 2, the induced norm can be computed by
‖A‖2 =(λmax(ATA)
)1/2(2.7)
Finally, we present the important property of the equivalence of norms: for every two p-norms ‖·‖αand ‖·‖β , there exist constants c1, c2 > 0 such that
c1 ‖x‖α ≤ ‖x‖β ≤ c2 ‖x‖α (2.8)
2.2 Quadratic forms
A quadratic form is a function f : Rn → R defined as
f = xTAx = xT(A+AT
2
)x (2.9)
Since the matrix A in the quadratic form can always be replaced by its symmetric part(A+AT
)/2,
we consider properties of the quadratic forms assuming that the matrix A is symmetric. Some useful
properties are
λmin(A) ‖x‖2 ≤ xTAx ≤ λmax(A) ‖x‖2 (2.10)
ST = −S (skew-symmetric)⇒ xTSx = 0 (2.11)
Matrix A is positive definite, A > 0, if xTAx > 0 and positive semi-definite, A ≥ 0, if xTAx ≥ 0. A
consequence of 2.10 is the property relating positive (semi-)definiteness and the minimum eigenvalue of
A:
A > 0⇔ λmin(A) > 0 (2.12)
A ≥ 0⇔ λmin(A) ≥ 0 (2.13)
Negative (semi-)definiteness is defined analogously, so A is negative (semi-)definite if −A if positive
(semi-)definite.
10
2.3 Nonlinear systems theory
2.3.1 Properties of differential equations
Differential equations are the basis of systems theory, and can be used to mathematically model a wide
variety of systems. In the context of this thesis, differential equations are used to model the deterministic
behaviour of a system. Considering a model and an initial condition in the form
dx
dt= f(t,x), x(t0) = x0 (2.14)
the response of the system x(t) for some t > t0 is expected to depend only on the initial condition.
Repeating the experiment with the same initial condition should produce the same response of the system.
It turns out that this is not always the case even if f(t,x) is continuous, so some stronger conditions are
necessary to ensure that a solution to the initial-value problem (2.14) exists and is unique. A key concept
regarding existence and uniqueness of solutions to initial-value problems is Lipschitz continuity.
Theorem 2.1 (Local Existence and Uniqueness). Consider the initial-value problem (2.14). Let f(t,x)
be piecewise continuous in t and satisfy
‖f(t,x)− f(t,y)‖ ≤ L ‖x− y‖ (2.15)
for all x,y ∈ B = {x ∈ Rn | ‖x− x0‖ ≤ r} with r > 0, and for all t ∈ [t0, t1]. Then there exists some
δ > 0 such that (2.14) has a unique solution over [t0, t0 + δ].
This theorem establishes a local result based on the Lipschitz condition (2.15), where L is called the
Lipschitz constant and f(t,x) is said to be Lipschitz in x. Notice that this result only holds on [t0, t0 + δ]
where we have no control over δ, which can actually be very small. At this point, to obtain a global result
that concerns existence and uniqueness beyond a neighborhood of the initial time t0, it is necessary to
distinguish between locally and globally Lipschitz functions.
Definition 2.1 (Lipschitz functions). A function f(t,x) is said to be:
• locally Lipschitz in x on an open and connected set D ⊂ Rn if each point of D has a neighborhood
D0 such that (2.14) is satisfied for all points in D0, uniformly in t ∈ [t0, t1];
• Lipschitz in x on a set W if (2.14) is satisfied for all points in W with the same Lipschitz constant
L, uniformly in t ∈ [t0, t1];
• globally Lipschitz in x if it is Lipschitz in x on Rn.
We can now state a more global result concerning existence and uniqueness.
Theorem 2.2 (Global Existence and Uniqueness). Consider the initial-value problem (2.14). Let f(t,x)
be piecewise continuous in t and globally Lipschitz in x, i.e. satisfy (2.15) for all x,y ∈ Rn and for all
t ∈ [t0, t1] with the same Lipschitz constant L. Then (2.14) has a unique solution over [t0, t1].
The drawback of this global result is the restrictiveness of the necessary globally Lipschitz condition.
In fact, as pointed out in [37], most models of real systems are continuously differentiable, and hence
11
satisfy the locally Lipschitz condition; the globally Lipschitz property, on the other hand, is a conservative
condition that many models of physical systems fail to satisfy, while still having unique solutions. A
global result can also be obtained by requiring only the locally Lipschitz property, but this requires
further knowledge of the trajectories of the system.
Theorem 2.3 (Global Existence and Uniqueness for locally Lipschitz functions). Consider the initial-
value problem (2.14). Let f(t,x) be piecewise continuous in t and locally Lipschitz in x on x ∈ D ⊂ Rn
for all t > t0. Let W be a compact subset of D, assume that x0 ∈ W and that every solution of (2.14)
remains entirely in W . Then (2.14) has a unique solution for all t > t0.
2.3.2 Lyapunov stability theory
Lyapunov stability theory is one of the frameworks to study stability of equilibrium points of systems
modelled by initial-value problems like (2.14). To introduce the main results on Lyapunov stability theory,
we first need to define concepts related to stability within this framework.
Definition 2.2 (Stability of autonomous systems). Consider the initial-value problem (2.14), and assume
that f does not depend explicitly on t. Such a system is called autonomous. Assume now, without loss
of generality1, that x = 0 is an equilibrium point of the differential equation; that is, f(0) = 0. Then, the
equilibrium point x = 0 is:
• stable if, for each ε > 0, there exists some δ(ε) > 0 such that
‖x(0)‖ < δ ⇒ ‖x(t)‖ < ε, ∀t > 0 (2.16)
• unstable if it is not stable;
• asymptotically stable if it is stable and δ can be chosen such that
‖x(0)‖ < δ ⇒ limt→∞
‖x(t)‖ = 0 (2.17)
• globally asymptotically stable if
limt→∞
‖x(t)‖ = 0, ∀x(0) (2.18)
Based on these definitions, we now state Lyapunov’s stability theorem. This is the basic result used
to prove stability properties in a Lyapunov sense.
Theorem 2.4 (Lyapunov stability). Consider the initial-value problem (2.14), assume that f does not
depend explicitly on t, and let x = 0 be an equilibrium point. Consider a domain D ⊂ Rn containing
x = 0, and a continuously differentiable Lyapunov function V : D → R such that
V (0) = 0 and V (x) > 0, ∀x ∈ D \ {0} (2.19)
V (x) ≤ 0, ∀x ∈ D (2.20)1As pointed out in e.g. [37], any equilibrium point can be translated to 0 by a coordinate transformation without affecting
its original stability properties.
12
Then x = 0 is stable. Additionally, if
V (x) < 0, ∀x ∈ D \ {0} (2.21)
then x = 0 is asymptotically stable. Moreover, if the equilibrium point is asymptotically stable with
D = Rn and
‖x‖ → ∞⇒ V (x)→∞ (2.22)
then x = 0 is globally asymptotically stable (GAS).
We now consider the case of non-autonomous systems, i.e. systems where the function f in 2.14
depends explicitly on t. The solution of the initial-value problem no longer depends only on t − t0 (as
was the case with autonomous systems); it depends on both t and t0. To take this into account, the
definitions of the stability concepts have to be rewritten for the case of non-autonomous systems.
Definition 2.3 (Stability of non-autonomous systems). Consider the initial-value problem (2.14), and
assume that f depends explicitly on t. The origin x = 0 is an equilibrium point of this system if
f(t, 0) = 0, ∀ t > 0 (2.23)
This equilibrium point is
• stable if, for each ε > 0, there exists some δ(ε, t0) > 0 such that
‖x(t0)‖ < δ ⇒ ‖x(t)‖ < ε, ∀ t ≥ t0 ≥ 0 (2.24)
• uniformly stable (US) if, for each ε > 0, there exists some δ(ε) > 0 independent of t0 such that
(2.24) is satisfied;
• unstable if it is not stable;
• asymptotically stable (AS) if it is stable and there is a constant c(t0) > 0 such that
‖x(t0)‖ < c⇒ limt→∞
‖x(t)‖ = 0 (2.25)
• uniformly asymptotically stable (UAS) if it is uniformly stable and there is a constant c > 0,
independent of t0, such that (2.25) is satisfied;
• globally uniformly asymptotically stable (GUAS) if it is uniformly stable, δ(ε) can be chosen such
that limε→∞
δ(ε) =∞ and, for each pair of positive numbers η and c, there is T (η, c) > 0 such that
‖x(t)‖ < η, ∀t ≥ t0 + T (η, c), ∀ ‖x(t0)‖ < c (2.26)
• exponentially stable if there exist positive constants c, k, λ, such that
‖x(t)‖ ≤ k ‖x(t0)‖ e−λ(t−t0), ∀ ‖x(t0)‖ < c (2.27)
• globally exponentially stable if (2.27) is satisfied for any x(t0).
13
We can now introduce a result relating these stability concepts for non-autonomous systems with the
Lyapunov function approach.
Theorem 2.5 (Lyapunov stability for non-autonomous systems). Consider the initial-value problem
(2.14), assume that f depends explicitly on t, and let x = 0 be an equilibrium point. Consider a domain
D ⊂ Rn containing x = 0, and a continuously differentiable Lyapunov function V : [0,∞)×D → R such
that
W1(x) ≤ V (t,x) ≤W2(x) (2.28)
∂V
∂t+∂V
∂xf(t,x) ≤ 0 (2.29)
for all t ≥ 0 and x ∈ D, where W1(x) and W2(x) are continuous positive definite functions on D. Then,
x = 0 is uniformly stable.
If these conditions are still verified with (2.29) replaced with the stronger inequality
∂V
∂t+∂V
∂xf(t,x) ≤ −W3(x) (2.30)
for all t > 0 and x ∈ D, where W3(x) is a continuous positive definite function on D, the equilibrium
point x = 0 is uniformly asymptotically stable.
Additionally, if D = Rn and W1(x) is radially unbounded, then x = 0 is globally uniformly asymptot-
ically stable.
A similar result exists for proving exponential stability using Lyapunov functions. It is not presented
here; see [37] for details.
2.3.3 Boundedness
For systems without equilibrium points, Lyapunov theory can still be used to prove boundedness of
the trajectory. Before introducing the Lyapunov-like theorem concerning boundedness, some definitions
are presented.
Definition 2.4 (Boundedness). Consider the initial-value problem (2.14). Its solutions are:
• uniformly bounded if there exists a positive constant c independent of t0 such that, for every a ∈
(0, c), there exists β = β(a) > 0 satisfying
‖x(t0)‖ ≤ a⇒ ‖x(t)‖ ≤ β, ∀t ≥ t0 (2.31)
• globally uniformly bounded if (2.31) holds for arbitrarily large a;
• uniformly ultimately bounded if there exist positive constants b and c independent of t0 such that,
for every a ∈ (0, c), there exists T = T (a, b) ≥ 0 satisfying
‖x(t0)‖ ≤ a⇒ ‖x(t)‖ ≤ b, ∀t ≥ t0 + T (2.32)
where b is the ultimate bound;
14
• globally uniformly bounded if (2.32) holds for arbitrarily large a.
We now introduce the concept of comparison functions. These functions simplify the statement of a
theorem relating boundedness with the Lyapunov function approach, to be presented later.
Definition 2.5 (Comparison functions). A continuous function α : [0, a) → [0,∞) is said to belong to
class K if it is strictly increasing and α(0) = 0, and to class K∞ if a =∞ and α(r)→∞ as r →∞.
A continuous function β : [0, a) × [0,∞) → [0,∞) is said to belong to class KL if, for each fixed s,
the mapping β(r, s) belongs to class K∞ with respect to r and, for each fixed r, the mapping β(r, s) is
decreasing with respect to s and β(r, s)→ 0 as s→∞.
Theorem 2.6 (Boundedness and Lyapunov functions). Consider a domain D ⊂ Rn containing the origin
and a continuously differentiable Lyapunov function V : [0,∞)×D → R such that
α1(‖x‖) ≤ V (t,x) ≤ α2(‖x‖) (2.33)
∂V
∂t+∂V
∂xf(t,x) ≤ −W3(x), ∀ ‖x‖ ≥ µ > 0 (2.34)
for all t ≥ 0 and x ∈ D, where α1 and α2 are class K functions and W3(x) is a continuous positive
definite function. Take some r > 0 such that a ball of radius r around the origin is contained in D,
Br ⊂ D. Suppose that
µ < α−12 (α1(r)) (2.35)
Then there exists a class KL function β and for every x(t0) satisfying ‖x(t0)‖ ≤ α−12 (α1(r)), there is
T ≥ 0, dependent on x(t0) and µ, such that the solution of (2.14) satisfies
‖x(t)‖ ≤ β(‖x(t0)‖ , t− t0), ∀t0 ≤ t ≤ t0 + T (2.36)
‖x(t)‖ ≤ α−11 (α2(µ)), ∀t ≥ t0 + T (2.37)
Additionally, if D = Rn and α1 belongs to class K∞, then (2.36) and (2.37) hold for any x(t0), with
no restriction on µ.
The main application of this theorem is the analysis of perturbed systems. It provides a way to ensure
that, given some conditions on the initial state, the norm of the solution of the system is bounded by a
function decreasing with time, and after a finite time T enters a set where its norm is bounded by the
ultimate bound α−11 (α2(µ)).
2.3.4 Input-to-state Stability
We now introduce the concept of input-to-state stability (ISS).
Definition 2.6 (Input-to-state Stability). Consider a system
x = f(t,x,u) (2.38)
where u(t) is an input signal and f is piecewise continuous in t and locally Lipschitz in x and u. This
system is said to be input-to-state stable if there exist a class KL function β and a class K function γ
15
such that for any initial state x(t0) and bounded input u(t), the solution x(t) exists and satisfies
‖x(t)‖ ≤ β(‖x(t0)‖ , t− t0) + γ
(sup
t0≤τ≤t‖u(t)‖
)(2.39)
Note that this definition implies that, for any bounded input, the state is bounded and, as t increases,
ultimately bounded by a class K function of the supremum of the input signal, since the initial time t0. In
other words, for large enough t, the solution asymptotically approaches a ball of radius proportional (via
the possibly nonlinear gain γ) to the maximum norm of the input. This radius is called the asymptotic
gain (AG). Another necessary condition for ISS is 0-stability, i.e. if the input is identically zero, u(t) = 0,
the system is stable. In [38], the ISS superposition theorem is presented, showing that the asymptotic
gain (AG) and 0-stability properties are necessary and sufficient conditions for a system to be ISS.
We now present a Lyapunov-like theorem concerning ISS, which follows from application of the global
version of Theorem 2.6.
Theorem 2.7 (ISS and Lyapunov functions). Let V : [0,∞)× Rn → R be a continuously differentiable
function such that
α1(‖x‖) ≤ V (t,x) ≤ α2(‖x‖) (2.40)
∂V
∂t+∂V
∂xf(t,x) ≤ −W3(x), ∀ ‖x‖ ≥ ρ(‖u‖) > 0 (2.41)
for all t ≥ 0, x ∈ Rn and u ∈ Rm, where α1 and α2 are class K∞ functions,W3(x) is a continuous positive
definite function, and ρ is a class K function. Then, (2.38) is input-to-state stable with γ = α−11 ◦α2 ◦ ρ.
The notion of input-to-state stability is defined here in a global sense, since the initial state and
the input signal can be arbitrarily large. A local version of this definition is now given, along with the
Lyapunov-like theorem.
Definition 2.7 (Local Input-to-state Stability). Consider Definition 2.6 and suppose that (2.39) is
satisfied only for ‖x(t0)‖ < k1 and supt≥t0 ‖u(t)‖ < k2, with k1 and k2 positive constants. The system
is said to be locally input-to-state stable, or (ε, δ)-ISS with ε = k1 and δ = k2 (see [39]). It can also be
said that the system is ISS for small inputs if it is locally input-to-state stable with restrictions on the
maximum norm of the input, but without restrictions on ‖x(t0)‖.
Theorem 2.8 (Local ISS and Lyapunov functions). Suppose the assumptions of Theorem 2.7 are satisfied
only for ‖x‖ < r and ‖u‖ < ru. Assume also that α1 and α2 are class K but not necessarily class K∞.
Then, the system is locally input-to-state stable, with k1 = α−12 (α1(r)) and k2 = ρ−1(min(k1, ρ(ru))).
2.4 Summary
In this chapter, important definitions and properties of norms and matrices were introduced. Then,
a brief overview of nonlinear systems theory was given, with emphasis on tools based on Lyapunov
functions. Concepts such as stability, boundedness and input-to-state stability, which are important
16
in the remainder of this thesis, were introduced. Important theorems that can be used to prove these
properties were presented.
17
Chapter 3
AUV Dynamics
In this chapter the equations governing the dynamics of an underwater vehicle are obtained. The
model given is kept general, so that it fits a wide range of vehicles. To achieve this, appropriate reference
frames are defined firstly. Equations are derived to obtain a three-dimensional, 6-DOF description. These
equations are then simplified and a reduced model for 3-DOF horizontal movement is obtained. Finally,
the MEDUSA-class vehicle is presented and numerical values of its coefficients in the 3-DOF model
structure are shown, yielding the final numerical model to be used in simulation.
The model structure used here was first introduced in [18]. Details on its derivation and underlying
simplifications regarding low-speed underwater vehicles are explained in detail in [2,18]. The parameters
of the MEDUSA-class vehicle are based on the identification work in [40].
3.1 Reference frames and naming conventions
In order to obtain a dynamic model of an AUV, it is usual practice to define both a body-fixed frame,
in which the dynamics of the vehicle are more conveniently described, and an earth-fixed frame, with
respect to which the position and orientation of the vehicle are described.
The body-fixed frame {B} consists of a right-handed, rectangular coordinate system. Its origin O is
chosen to coincide with the center of gravity (CG) of the vehicle. Its axes {xB , yB , zB} are defined in
accordance with the SNAME notation [41], and coincide with the principal axes of inertia at O:
• xB is the longitudinal axis, directed from the after to the forward end of the body;
• yB is the transverse axis, directed from port to starboard;
• zB is the normal axis, directed from top to bottom.
We now need to define the earth-fixed frame. In the conditions where this model will be used (low
speeds and only small variations in latitude and longitude), it is reasonable to disregard both the rotation
and the curvature of the Earth. In this situation, we define a flat Earth navigation frame {I}. This is a
local NED (North, East, Down) frame, defined in a plane tangent to the surface of the Earth at a fixed
point in the area of operation of the vehicle. {I} is then a right-handed, rectangular coordinate system,
19
Figure 3.1: A schematic drawing of a non-diving MEDUSA vehicle and the inertial and body-fixed frames.
and its axes xI , yI , zI point towards true North, East and Down respectively. Given the assumptions
stated earlier, {I} is an inertial frame, so Newton’s laws apply in this frame.
Having defined the two reference frames, a set of parameters can now be introduced to completely
define the position, orientation and velocity of the vehicle, along with the external forces and torques
applied:
• position of the origin of {B} expressed in {I}: η1 = [x, y, z]T ;
• orientation of the body axes relative to the earth-fixed axes, in Euler angles, using the zyx convention
for rotation order when rotating from {I} to {B}: η2 = [φ, θ, ψ]T ;
• linear velocity of the origin of {B} with respect to {I} expressed in {B}: ν1 = [u, v, w]T ;
• angular velocity of {B} with respect to {I} expressed in {B}: ν2 = [p, q, r]T ;
• external forces expressed in {B}: τ 1 = [X,Y, Z]T ;
• external torques expressed in {B}: τ 2 = [K,M,N ]T .
This notation is summarized in Table 3.1, along with the usual names for each parameter. The names
and symbols of these parameters are used interchangeably in this thesis.
Motion Force and Moment Velocity Position and Euler angles
motion along xB (surge) X u (surge speed) x
motion along yB (sway) Y v (sway speed) y
motion along zB (heave) Z w (heave speed) z
rotation about xB K p (roll rate) φ (roll angle)
rotation about yB M q (pitch rate) θ (pitch angle)
rotation about zB N r (yaw rate) ψ (yaw angle)
Table 3.1: SNAME nomenclature and symbols
20
3.2 Kinematics
Kinematic equations involve geometric aspects of movement. Specifically, kinematic equations of the
AUV motion relate velocities in the body-fixed frame with its inertial frame counterparts.
To relate linear velocity vectors expressed in these two reference frames, we resort to rotation matrices.
A matrix R ∈ R3×3 is a rotation matrix if RTR = RRT = I and det(R) = 1. An arbitrary vector
a ∈ R3 can be transformed from a frame to another through right-multiplication by a rotation matrix,
ato = Rtofroma
from. This rotation matrix Rtofrom is parametrized by the relative orientation of the two
frames.
In our particular case, we need the rotation matrix to transform vectors from {B} to {I} and vice-
versa. The Euler angles define the relative orientation of these two frames, and hence can be used to
parametrize the rotation matrix. This rotation matrix can be described by a sequence of three rotations
applied to {I}, about its current z, y, x axes over the Euler angles ψ, θ and φ respectively, so that
the directions of the axes of the rotated {I} frame coincide with those of {B}. The details of these
transformations are not shown here; for details see, for example, [2, 18].
The final rotation matrix from {B} to {I} parametrized by the Euler angles η2 = [φ, θ, ψ]T is:
RIB(η2) =
cψcθ −sψcφ+ cψsθsφ sψsφ+ cψcφsθ
sψcθ cψcφ+ sφsθsψ −cψsφ+ sθsψcφ
−sθ cθsφ cθcφ
(3.1)
where s(·) = sin(·) and c(·) = cos(·).
Using this rotation matrix, the kinematic equations related with linear velocities can now be written
in a vectorial setting:
η1 = RIB(η2)ν1 (3.2)
As for transformations of angular velocities, the Euler rates η2 relate to the body-frame angular
velocities ν2 via a transformation matrix:
T (η2) =
1 sφtθ cφtθ
0 cφ −sφ
0 sφ/cθ cφ/cθ
(3.3)
where t(·) = tan(·). Again, the details of the derivation of this matrix can be found in [2, 18], for
example. Note that T is not defined for θ = ±90◦, a singular point unavoidable when using Euler angle
representations. In this thesis, as discussed previously, the control problems will be formulated in the
horizontal plane. In this situation, the pitch angle θ is considered to remain close to 0◦ and the singular
point can be ignored. If complex 3D movement was involved, another approach would have to be used,
namely switching between two Euler angle representations with different singular point or resorting to
quaternions.
We can now write the kinematic equations related with angular velocities:
η2 = T (η2)ν2 (3.4)
21
Finally, combining (3.2) and (3.4), the 6-DOF kinematic equations can be written in compact form:η1
η2
=
RIB(η2) 0
0 T (η2)
ν1
ν2
⇔ η = J(η)ν (3.5)
3.3 Dynamics
Dynamic equations of movement capture the influence of forces and torques on movement. For
convenience, dynamic equations for mobile vehicles are usually expressed in the body-fixed frame, where
the inertia tensor is constant and the external forces (weight, hydrodynamic) are more easily expressed.
The derivation of the rigid-body dynamic equations of motion is based on application of Newton-
Euler’s law for both translational and rotational movement. These equations can be expressed in vectorial
form [2]:
MRBν + CRB(ν)ν = τRB (3.6)
M is the inertia matrix and C contains terms related to the Coriolis and centripetal accelerations.
Three important properties of these matrices are:
• MRB = 0, the inertia matrix is constant in the body-fixed frame;
• MTRB = MRB > 0, the inertia matrix is symmetric and positive-definite - in fact, since {B} is
centered at the CG of the vehicle and its axes are coincident with the principal axes of inertia,
MRB is diagonal;
• CTRB(ν) = −CRB(ν), the Coriolis and centripetal matrix can be parametrized so that it is skew-
symmetric.
The subscript RB indicates that these equations concern only the rigid-body dynamics, and all external
forces and moments are represented indistinguishably in τRB = [X,Y, Z,K,M,N ]T . To explicitly take
into account different types of external forces and moments, this vector can be decomposed in
τRB = τ + τA + τD + τR + τ e (3.7)
= τ −MAν − CA(ν)ν −D(ν)ν − g(η) + τ e (3.8)
The term τ contains forces and moments due to control inputs. τA is related with the inertia of
the surrounding fluid, and can be represented by added inertia and Coriolis matrices MA and CA. τD
concerns all drag forces and moments, including those due to skin friction and pressure differences, among
others. τR includes restoring terms caused by the interplay between weight and buoyancy. τ e contains
all the remaining external disturbances (wind, waves, currents).
The final dynamic model, now including hydrodynamic contributions but discarding τ e, is then
[MRB +MA] ν + [CRB(v) + CA(v)]v +D(v)v + g(η) = τ
⇔M ν + C(ν)ν +D(ν)ν + g(η) = τ (3.9)
22
where C(ν) can still be parametrized so to be skew-symmetric, D(ν) is positive-definite and, under some
assumptions, M is symmetric and positive-definite.
3.4 Horizontal plane dynamics
In this thesis, a vehicle is assumed to restrain its motion to a horizontal plane. This could be either
the surface or some fixed depth, through independent depth and pitch control. The roll motion is also
assumed to be negligible. Under these assumptions, a model describing the motion on a horizontal plane
with 3 degrees of freedom (surge, sway, yaw rate) would be more adequate for control purposes than the
full 6-DOF model.
The kinematics can be rewritten by dropping the pitch and roll angles, as well as the z coordinate,
from η. Analogously, the pitch and roll rates q, p and the heave w are dropped from ν, yielding:x
y
ψ
=
cosψ − sinψ 0
sinψ cosψ 0
0 0 1
u
v
r
(3.10)
We now introduce current velocities Vc =[Vcx , Vcy
]T , assumed not to vary in time or space. The
body-fixed frame velocities u and v are now redefined to be the velocities of the vehicle with respect to a
water-fixed frame {W}, identical to {I} but moving at velocity Vc. Since the current velocity is assumed
to be constant and uniform in space, this frame is still inertial. The new kinematic equations written in
compact form arep = R(ψ)v + Vc
ψ = r(3.11)
where p = [x, y]T and v = [u, v]
T . The rotation matrix R(ψ) is obtained by substituting θ = φ = 0 in
the original R matrix for the 6-DOF model.
Assuming that the center of buoyancy is coincident with the center of gravity of the vehicle, the
dynamic equations are reduced tomuu−mvvr + duu = τu
mv v +muur + dvv = τv
mr r −muvuv + drr = τr
(3.12)
with
mu = m−Xu mr = Iz −Nr du = −Xu −X|u|u |u|
mv = m− Yv muv = mu −mv dv = −Yv − Y|v|v |v|
dr = −Nr −N|r|r |r|
In compact form,
M ν + C(ν)ν +D(ν)ν = τ (3.13)
23
with
ν = [u v r]T
τ = [τu τv τr]T
M =
mu 0 0
0 mv 0
0 0 mr
> 0
C(ν) =
0 0 −mvv
0 0 muu
mvv −muu 0
D(ν) =
du 0 0
0 dv 0
0 0 dr
> 0
In this model, M is diagonal because MRB is diagonal and, for low-speed underwater vehicles with
three planes of symmetry, the off-diagonal elements in MA can be neglected. C(ν) is again parametrized
so to be skew-symmetric. D(ν) is also symmetric.
The model structure obtained is highly advantageous in terms of analysis and control design, when
compared with the classical models describing the motion of marine craft [18]. This is mainly because of
the properties of the matrices obtained, like symmetry, skew-symmetry and positive-definiteness. These
properties can be exploited in stability analysis and control design, and relate to physical properties of
the system. An example is the dissipativity of the dynamic model: it is straightforward, in the 3-DOF
model, to see that τTν ≥ νTM ν using only properties of quadratic forms related with (skew-)symmetry
and positive-definiteness.
3.5 The MEDUSA-class vehicle
The MEDUSA-class vehicles are autonomous semi-submersibles robotic marine vehicles, developed
at the Dynamical Systems and Ocean Robotics group (DSOR) of Instituto de Sistemas e Robótica from
Instituto Superior Técnico (ISR-IST). Currently, there are three available MEDUSAs, two of them with
diving capabilities.
These vehicles weigh around 17 kg (depending on the onboard sensors) and consist of two acrylic
tubes around 1 m long, with a diameter of 0.15 m, covered in both ends by aluminium caps. These two
tubes contain the batteries, all the onboard electronics and sensors, and the main computer unit. They
are attached to a central aluminium frame, and are 0.15 m apart.
The surface vehicles have two thrusters aligned along the longitudinal direction xB , one at each
side of the vehicle, each around 0.15 m from the center frame. These two thrusters generate forces Fs
(starboard) and Fp (port), that can be decomposed in a longitudinal force τu = Fs + Fp and a yaw
moment τr = 0.15(Fs − Fp). The diving MEDUSAs have two extra thrusters aligned along the normal
24
direction zB , again one at each side of the vehicle. They are used to control the heave motion. The
vehicle has large enough metacentric height, so the restoring moments are big enough for the roll and
pitch motions to be neglected.
Figure 3.2: A MEDUSA vehicle operating at the surface during MORPH trials in Toulon, 2013.
It is important to note that no thrusters in the transverse direction yB are available. The MEDUSAs
are underactuated vehicles, meaning that the number of control inputs is less than the number of degrees
of freedom - in this case, the sway motion is not actuated.
The final model of the MEDUSAs used throughout this thesis consists of the kinematic equations
(3.11) and dynamic equations (3.13), with τv = 0 because of the unactuated sway motion. The mass,
inertia and hydrodynamic coefficients are shown in Table 3.2, and were determined in [40] for motion on
the surface.
m = 17 kg Iz = 1 kg·m2
Xu = −20 kg Yv = −30 kg Nr = −8.69 kg·m2
Xu = −0.2 kg/s Yv = −50 kg/s Nr = −4.14 kg·m2/s
X|u|u = −25 kg/m Y|v|v = −0.01 kg/m N|r|r = −6.23 kg·m2
Table 3.2: Mass, inertia and hydrodynamic coefficients for a MEDUSA vehicle at the surface
3.6 Summary
In this chapter we first introduced notation to allow derivation of a dynamic and kinematic 6-DOF
model for the motion of an AUV. Some simplifications were made based on properties of the vehicle.
Then, a reduced model for horizontal plane motion was obtained. Properties concerning the structure of
the model, namely positive-definiteness or skew-symmetry of matrices, were highlighted; these properties
greatly simplify analysis of the system using tools from nonlinear systems theory.
Finally, the MEDUSA class of autonomous vehicles is presented, and its model parameters are shown
based on previous identification work in [40].
25
Chapter 4
Trajectory-Tracking
In this chapter the motion control problem of trajectory-tracking, briefly described in the introduction,
is defined rigorously. The problem is addressed using a decoupled inner-outer-loop control architecture.
The outer-loop, concerning the guidance of the vehicle (producing velocity commands, u and r), is
designed without considering any inner-loop dynamics.
Control laws based on full-state feedback, such as those designed using backstepping techniques in [20],
take into account the complete system dynamics, and can be extended to account for actuator dynamics.
The decoupled control strategy used here has obvious drawbacks in terms of performance and control
effort when compared with full-state feedback. The reason for this is that interactions between the two
loops are not taken into account in the design of the control laws, and in the worst case instabilities can
arise. However, this modular approach leads to simpler and more intuitive control laws, while generally
maintaining reasonable performance as long as the inner-loop dynamics are sufficiently faster than those
of the outer-loop. Another important reason for this decoupling is that the outer-loop can be used in
vehicles that already provide inner-loop control.
After the outer-loop controller design, a theoretical analysis is presented. A representative inner-
loop controller is designed so that some conclusions can be drawn regarding the influence of controller
parameters in closed-loop stability. The proposed control architecture borrows from [22], but extends the
controller to include an additional sensitivity parameter. The analysis assumes that an external estimator
is available, and accounts for estimation errors in the velocity of the target and in the velocity of the
current. A bounded-input bounded-state result is proved regarding the tracking error. The influence of
the controller parameters on closed-loop stability is studied.
Finally, the applications of trajectory-tracking to formation control are clarified. The fact that leader-
follower formation strategies avoid a feedback interconnection that would create additional formation
dynamics is highlighted. The problem of keeping a formation with a leader-follower topology is then
reduced to designing a path generator block that produces the commands to be fed to the trajectory-
tracking controller.
27
4.1 Problem definition
Trajectory-tracking consists on forcing a vehicle to reach and follow a time-parametrized reference
trajectory, by making the vehicle approach a virtual target that moves along the reference trajectory with
an associated timing law. We denote by p(t) the position of the vehicle and by pd(t) the desired position,
or position of the virtual target, at time t. In [20], this problem is defined for the 3D case. This definition
is adapted to the 2D case and presented below.
Problem 4.1 (Trajectory-tracking). Consider an underactuated autonomous underwater vehicle with
dynamic model (3.13) and kinematic model (3.11), with τv = 0. Let pd(t) : [0,∞) → R2 be a given
sufficiently smooth time-varying desired trajectory with bounded time-derivatives. Design a controller
such that all the closed-loop signals are bounded and the tracking error ‖p(t)− pd(t)‖ converges to a
neighborhood of the origin that can be made arbitrarily small.
It turns out that the decoupled inner-outer-loop strategy, under the assumptions that will be estab-
lished, will not allow us to solve this problem. This definition, however, is useful to define the outer-loop
control problem.
4.2 Outer-loop control
To tackle the trajectory-tracking problem according to the decoupled inner-outer-loop strategy as-
sumed above, we start by looking at the outer-loop control problem. In this setting, we discard the
dynamic model of the vehicle, taking u and r as control variables. The outer-loop control problem is now
formally defined.
Problem 4.2 (Outer-loop control). Consider the kinematic model of the system in (3.11), with inputs
ud = (ud, rd) (commanded surge speed and yaw rate). Assume that these commands are identically
satisfied, u(t) = ud(t). Let pd(t) : [0,∞) → R2 be a given sufficiently smooth time-varying desired
trajectory with bounded time-derivatives. Design a control law for u and r that produces bounded velocity
commands so that the tracking error ‖p(t)− pd(t)‖ converges to a neighborhood of the origin that can be
made arbitrarily small.
To address Problem 4.2 we start by defining the error variable to be driven to a neighborhood of zero.
We define it as the position error expressed in the body-fixed frame {B}.
e = RT (ψ) (p− pd) (4.1)
In this definition, the time-dependence of p and pd is not explicitly stated. This is common practice
in the remainder of this thesis, to make the equations less verbose; it does not change, of course, the fact
that both these signals depend on time.
Before computing the dynamics of e, we need to state important properties regarding the rotation
matrix R(ψ):d
dtR(ψ) = R(ψ) = R(ψ)S(ψ) = R(ψ)S(r) (4.2)
28
where
S(r) =
0 −r
r 0
(4.3)
Note that S(r) is skew-symmetric. The derivative of RT (ψ) is then
d
dtRT (ψ) = RT (ψ) = [R(ψ)S(r)]
T= ST (r)RT (ψ) = −S(r)RT (ψ) (4.4)
The time-derivative of e is
e = RT (ψ)(p− pd) + RT (ψ)(p− pd)
= RT (ψ)(p− pd)− S(r)RT (ψ)(p− pd)
= RT (ψ)(R(ψ)v + Vc − pd)− S(r)e
= v −RT (ψ)(pd − Vc)− S(r)e (4.5)
where v = (u, v) are the velocities with respect to the water along the body axes (surge and sway speed
respectively). Note that because pd and Vc may depend explicitly on time, the system is non-autonomous.
It would now be possible to achieve global uniform asymptotic stability (GUAS) of e = 0 through,
for example, the control law v = RT (ψ) (pd − Vc)−Ke, with a positive-definite symmetric gain matrix
K = KT > 0. However, the vehicle is underactuated and v cannot be directly controlled. Instead, we
resort to rotation to produce lateral motion. Consider that the control variable is instead u = (u, r), as
stated in Problem 4.2, and define a new error variable e′ = e − δ, as was done in [22], where δ = (δ, 0)
and δ is a constant parameter:
e′ = e− δ
=
uv
−RT (ψ)(pd − Vc)− S(r)(e− δ)− S(r)δ
=
uv
−RT (ψ)(pd − Vc)− S(r)e′ −
0 −r
r 0
δ0
=
u
−δr
+
0
v
−RT (ψ)(pd − Vc)− S(r)e′
=
1 0
0 −δ
︸ ︷︷ ︸
Γ
ur
︸︷︷︸u
+
0
v
−RT (ψ)(pd − Vc︸ ︷︷ ︸d
)− S(r)e′ (4.6)
where the term pd − Vc, which includes the velocities of the current and of the target in the inertial
frame, is now renamed as a disturbance d. In this formulation, if e′ is driven to zero, we are driving
to the target position a point on the vehicle that lies a distance δ behind the center of mass (which is
assumed to coincide with the center of rotation), along the longitudinal axis. To see this, notice that the
29
new tracking error variable can be written as
e′ = RT (ψ)(p− pd)− δ
= RT (ψ)(p−R(ψ) [δ, 0]T − pd) (4.7)
so δ should be negative if we want the vehicle to move forward towards the target point. From the
dynamics in (4.6), a control law can be defined in terms of the desired u, cancelling the effect of d and v:
ud = Γ−1
−0
v
+RT (ψ)d−K tanhe′
n
(4.8)
with a symmetric gain matrix K > 0 and n > 0. The purpose of the hyperbolic tangent is to saturate the
requested velocities due to the position error to a maximum value defined by the gain matrix K. Figure
4.1 illustrates the effect of the hyperbolic tangent function and the control parameters K and n, in the
scalar case. The parameter n adjusts the aggressiveness of the controller, since the slope of the hyperbolic
tangent around e = 0 is defined byK/n. This saturation mechanism ensures that the outer-loop controller
avoids very large velocity commands when the vehicle is far from the desired position. Other than the fact
that it is continuously differentiable, the hyperbolic function has no particularly important properties;
other saturation functions could have been used for the same purpose. The hyperbolic tangent is extended
to vector spaces in such a way that it scales only the norm, and preserves the direction of the vector:
tanhx = xtanh ‖x‖‖x‖
(4.9)
−5 0 5−2.5
−2
−1.5
−1
−0.5
0
0.5
1
1.5
2
2.5
e
Ktanh(e)
K=1, K/n=1K=2, K/n=2K=2, K/n=1slope=1slope=2
Figure 4.1: Effect of K and n in the tanh(e) function, in the scalar case.
Proposition 4.1. The control law (4.8) solves Problem 4.2 if K > 0 and n > 0.
Proof. Consider a positive-definite Lyapunov function V = 12e′Te′. Substituting the control law (4.8) in
the error dynamics (4.6) with u = ud, the time-derivative of V is
V = e′T e′
= e′T(−S(r)e′ −K tanh
e′
n
)= −e′TKe′ tanh ‖e′/n‖
‖e′‖< 0, ∀e′ 6= 0 (4.10)
30
The equilibrium point e′ = 0 is then asymptotically stable. Given that e′ = e− δ, this is equivalent
to stating that e = δ is an asymptotically stable equilibrium point. Since δ is a design parameter that
can be made arbitrarily small, we conclude that ‖p(t)− pd(t)‖ = ‖e‖ converges to a neighborhood of the
origin that can be made arbitrarily small.
We now relax the conditions of the problem by making two assumptions:
• the sway speed v is not available;
• we do not have complete knowledge of d.
The first assumption is reasonable because sway speeds are usually small and easily hidden by noise. The
second is relevant because we might have incomplete information regarding the velocity of the virtual
target, or some error in the estimate of the current. At the moment, we assume that some external
estimator is available; the problem of estimating d will be analysed later in the thesis.
With these two assumptions, the control law is modified so as not to include any terms concerning
sway speed, and d is replaced by its estimate d:
ud = Γ−1
(RT (ψ)d−K tanh
e′
n
)(4.11)
where d is affected by an error d = d− d.
We also consider that the speed commands are not identically satisfied, so u may differ from ud. This
is because ud contains requested velocities to be fed to an inner-loop, and it is reasonable to assume that
they are not tracked exactly. A variable u = u− ud is now introduced to take this inner loop error into
account.
Substituting u = u + ud and the control law (4.11) in the error dynamics (4.6), we compute the
time-derivative of a Lyapunov function V = 12e′Te′ along the trajectories of the system:
V = e′T e′ = e′T
Γu−K tanhe′
n+
0
v
+RT (ψ)(d− d
)= −e′TK tanh
e′
n+ e′T
Γu+
0
v
−RT (ψ)d
(4.12)
Using the definition of the vectorial hyperbolic tangent in (4.9),
V = −e′TKe′ tanh ‖e′/n‖‖e′‖
+ e′T
Γu+
0
v
−RT (ψ)d
≤ −(1− θ)e′TKe′ tanh ‖e′/n‖
‖e′‖− θe′TKe′ tanh ‖e′/n‖
‖e′‖+ ‖e′‖ ·
∥∥∥∥∥∥Γu+
0
v
−RT (ψ)d
∥∥∥∥∥∥ (4.13)
with 0 < θ < 1. Now, using one of the negative definite terms to dominate the last term,
V ≤ −(1− θ)e′TKe′ tanh ‖e′/n‖‖e′‖
(4.14)
31
when
θλmin(K) tanh ‖e′/n‖ ≥
∥∥∥∥∥∥Γu+
0
v
−RT (ψ)d
∥∥∥∥∥∥ (4.15)
This condition is implied by
1
θλmin(K)·
∥∥∥∥∥∥Γu+
0
v
−RT (ψ)d
∥∥∥∥∥∥ < 1 (4.16)
‖e′‖ ≥ n arctanh
1
θλmin(K)·
∥∥∥∥∥∥Γu+
0
v
−RT (ψ)d
∥∥∥∥∥∥ (4.17)
Proposition 4.2. Consider the system with state e′, with dynamics expressed in (4.6), in closed-loop
with the feedback controller (4.11). This system is ISS for small inputs, with respect to input Γu +
[0 v]T −RT (ψ)d, with a restriction on the norm of the input given by (4.16).
Proof. Consider the Lyapunov function V = 12e′Te′, and the properties concerning its derivative expressed
in (4.14) to (4.17). Through direct application of Theorem 2.8, we conclude the ISS property.
We can draw some conclusions on the influence of the controller parameters n and K on the ISS
properties of the system. Condition (4.16) shows that the gain matrix K influences the restrictions on
the maximum norm of the input, with a larger K allowing for bigger sway speeds, inner-loop errors and
disturbance estimation errors. On the other hand, for a fixed K, increasing n increases the ultimate
bound on the tracking error, as condition (4.17) shows.
Note that these parameters are also restricted by physical limitations. K is restricted by the maximum
surge speed and rotational velocity. K/n is restricted by how fast the inner loop is, which in turn is limited
by the maximum allowed actuation levels and the dynamics of the actuators (not considered explicitly
in this analysis).
4.3 Closed-loop dynamics
The previous analysis of the outer-loop and its control parameters is useful when the vehicle is already
equipped with an inner-loop for tracking the velocity references. Then, based on some knowledge of the
characteristics of the inner-loop, the outer-loop can be tuned by adjusting both K and n. This approach,
however, does not explicitly take into account the influence of K and n in the disturbance terms u and
v. To clarify this, we need to define a representative inner loop control law and look into the dynamics
of these two variables.
We start the analysis of the inner-loop velocity error using the dynamic model of the vehicle in (3.13)
to design a representative inner-loop controller. We define the extended inner-loop error ν = ν − νd,
32
where
νd =
ud
0
rd
=
1 0
0 0
0 1
︸ ︷︷ ︸
I32
ud (4.18)
so that
ud =
1 0 0
0 0 1
︸ ︷︷ ︸
I23
νd (4.19)
The dynamics of ν are
M ˙ν = M(ν − νd)
= −C(ν)ν −D(ν)ν + τ −M νd
= −C(ν) (ν + νd)−D(ν) (ν + νd)−M νd + τ
= − [C(ν) +D(ν)] ν − [C(ν) +D(ν)]νd −M νd + τ (4.20)
An inner-loop control law such as
τ = −Kdν + [C(ν) +D(ν)]νd +M νd (4.21)
would asymptotically stabilize ν = 0 for any Kd = KTd > 0 (recall that C(ν) is skew-symmetric).
However, given the decoupled inner-outer-loop control strategy considered (where the only information
fed from the outer- to the inner-loop is ud), the assumption that the sway speed v is not available, and
the impossibility to actuate on the second component of τ , we cannot use this control law. A possible
inner-loop control law that does not use v or νd and does not actuate on τv is
τ = −
ku 0 0
0 0 0
0 0 kr
︸ ︷︷ ︸
Kd
ν +D(ν)νd (4.22)
Substituting the control law (4.22) in the dynamics of the inner-loop error (4.20), we obtain:
M ˙ν = − [C(ν) +D(ν) +Kd] ν − C(ν)νd −M νd (4.23)
where
νd = I32ud
= I32Γ−1
(−S(r)RT (ψ)d+RT (ψ)
˙d−K d
dt
(tanh
e′
n
))(4.24)
33
The derivative of the term with the hyperbolic tangent is computed separately:
d
dt
(tanh
e′
n
)=
d
dt
(e′
n
tanh ‖e′/n‖‖e′/n‖
)= B(e′/n)
e′
n
=B(e′/n)
n
Γu−K tanhe′
n+
0
v
−RT (ψ)d− S(r)e′
=B(e′/n)
n
1 0 0
0 1 −δ
︸ ︷︷ ︸
Γ23
ν −K tanhe′
n−RT (ψ)d− S(r)e′
(4.25)
We do not expand B(e′/n). It is sufficient to present two important properties of this function: both
B(e′/n) and B(e′/n) · e′/n are bounded uniformly in e′, i.e. there exist two constants ε1 and ε2 such
that, for any e′,
‖B(e′/n)‖ < ε1 (4.26)
‖B(e′/n)‖ · ‖e′/n‖ < ε2 (4.27)
Before introducing the complete closed-loop analysis, we first need to state a result on the boundedness
of νd.
Proposition 4.3. If d is bounded, then νd(t) is bounded.
Proof. The norm of νd verifies
‖νd‖ ≤ ‖I32‖ ·∥∥Γ−1
∥∥(‖d‖+ ‖K‖)
(4.28)
where ‖I32‖ = 1 and∥∥Γ−1
∥∥ = max(1, 1|δ| ). The gain matrix K is a control parameter, so its norm must
be bounded. Hence, νd is bounded.
We can now analyse the stability of the closed-loop by considering a system with the tracking error
e′ and the extended inner-loop error ν as states. Note that ν includes both the inner-loop error and the
sway. We define a closed-loop Lyapunov function:
V =1
2e′Te′ +
1
2νTM ν (4.29)
We can now compute its derivative with respect to time, taking into account that M is symmetric.
Differentiating each term and grouping into quadratic and linear terms in each of the states, and terms
34
involving the product of e′ and ν, we obtain
V = e′T e′ + νTM ˙ν
= e′T(
Γ23ν −K tanhe′
n−RT (ψ)d− S(r)e′
)− νT [D(ν) +Kd] ν − νTC(ν)νd − νTM νd
= e′T(
Γ23ν −K tanhe′
n−RT (ψ)d
)− νT [D(ν) +Kd] ν − νTC(ν)νd
− νTMI32Γ−1
[−S(r)RT (ψ)d+RT (ψ)
˙d−KB(e′/n)
n
(Γ23ν −K tanh
e′
n−RT (ψ)d− S(r)e′
)]= −e′TKe′ tanh ‖e′/n‖
‖e′‖+ e′T b− νT
(D(ν) +Kd −MI32Γ−1K
B(e′/n)
nΓ23
)ν + νTa+ νTAe′
(4.30)
where
a = −C(ν)νd −MI32Γ−1
(−S(r)RT (ψ)d+RT (ψ)
˙d+K
B(e′/n)
n
(K tanh
e′
n+RT (ψ)d+ S(r)e′
))(4.31)
b = −RT (ψ)d (4.32)
A = ΓT23 (4.33)
Note that ‖b‖ = ‖d‖. Taking norms and applying Young’s inequality to the term νTAe′, we obtain:
V ≤ −λmin(K) ‖e′‖ tanh ‖e′/n‖+ ‖e‖ ‖d‖+ε ‖e′‖2
2
− νT
Kν︷ ︸︸ ︷(D(ν) +Kd −MI32Γ−1K
B(e′/n)
nΓ23 −
∥∥ΓT23
∥∥2
2εI
)ν + ‖ν‖ ‖a‖
= Ve′ + Vν (4.34)
for all ε > 0.
If K,Kν > 0 and there exists some 0 < θe′ < 1 such that
‖d‖+ε‖e′‖
2
θe′λmin(K)< 1 (4.35)
then Ve′ < 0 when
‖e′‖ > n arctanh‖d‖+
ε‖e′‖2
θe′λmin(K)(4.36)
Note that, for a fixed K and ‖d‖, (4.35) cannot be satisfied for arbitrarily large e′ because of the termε‖e′‖
2 . Indeed, this condition implies a restriction on the norm of the initial state e′(0). To relax this
restriction, in the analysis ε can be chosen small enough, at the expense of more conservative conditions
for ν.
We now look at Vν . Two important aspects should be noted, regarding the structure of the D and C
matrices:
• The entries of the D matrix contain only terms that are constant or linear in ν. Hence, because
ν = ν + νd, it can be decomposed by D(ν) = D′(ν) +D(νd) where D′ is a modified drag matrix
35
obtained by removing the constant terms.
• Similarly, the entries of the C matrix contain only terms that are linear in ν. Hence, it can be
decomposed in C(ν) = C(ν) + C(νd).
We now prove two propositions concerning the terms in Vν that will be instrumental in proving the
stability result.
Proposition 4.4. The term νTD(ν)ν is lower-bounded by a term proportional to ‖ν‖3 as ‖ν‖ → ∞.
Proof. Using the decomposition introduced before,
νTD(ν)ν = νT [D′(ν) +D(νd)] ν
= νTD′(ν)ν + νTD(νd)ν (4.37)
Since νd is bounded for a given maximum d, the entries of D(νd) are bounded, so its norm must also
be bounded:
‖D(νd)‖ ≤ ‖D(νd)‖max (4.38)
and hence we can conclude that
νTD(νd)ν ≤ ‖D(νd)‖max ‖ν‖2 (4.39)
Regarding the other term, for constants a, b, c, k > 0,
νTD′(ν)ν = νT
a |u| 0 0
0 b |v| 0
0 0 c |r|
ν= a |u| u2 + b |v| v2 + c |r| r2
≥ min(a, b, c)(|u|3 + |v|3 + |r|3
)= k ‖ν‖33 (4.40)
From the property of equivalence of norms, there exists a constant α > 0 such that
‖ν‖3 ≥ α ‖ν‖ (4.41)
where the norm operator ‖·‖ without subscript denotes a 2-norm. We can now conclude that
νTD′(ν)ν ≥ k′ ‖ν‖3 (4.42)
with k′ > 0. For large enough ‖ν‖, this cubic term dominates over the quadratic one, and the asymptotic
behaviour is cubic.
Proposition 4.5. All the terms in νTKν ν, other than the drag-related ones, are upper-bounded by a
term proportional to ‖ν‖2 as ‖ν‖ → ∞. The same happens with ‖ν‖ ‖a‖.
36
Proof. We start by analysing the terms in νTKν ν, other than the drag-related ones:
νT (Kν −D(ν)) ν ≤ ‖ν‖2(‖Kd‖+
∥∥MI32Γ−1K∥∥ · ‖Γ23‖
‖B(e′/n)‖n
+
∥∥ΓT23
∥∥2
2ε
)(4.43)
Recall from (4.26) that ‖B(e′/n)‖ < ε1. Hence, all the terms multiplying ‖ν‖2 are bounded, so there
exists a constant c1 > 0 such that
νT (Kν −D(ν)) ≤ c1 ‖ν‖2 (4.44)
Note that this constant c1 increases as ε→ 0, which confirms that relaxing the restriction on the initial
tracking error e′(0) implies that the conditions for negative-definiteness of Vν are stronger.
We now determine an upper-bound for ‖a‖:
‖a‖ ≤ ‖C(ν)‖ ‖νd‖+∥∥MI32Γ−1
∥∥(‖S(r)‖ ‖d‖+ ‖ ˙d‖+ ‖K‖ ‖B(e′/n)‖
n
(‖K‖+ ‖d‖+ ‖S(r)‖ ‖e′‖
))(4.45)
where we used the properties ‖R(ψ)‖ = 1 and ‖tanhx‖ = 1 for any x. Now, we introduce two properties
regarding the norms of C(ν) and S(r):
‖S(r)‖ = |r| ≤ ‖ν‖ ≤ ‖ν‖+ ‖νd‖
‖C(ν)‖ ≤ α
∥∥∥∥∥∥uv
∥∥∥∥∥∥ ≤ α ‖ν‖ ≤ α(‖ν‖+ ‖νd‖) (4.46)
Recalling properties (4.26) and (4.27), and the fact that νd is bounded, we can state that there exist
constants a1, a2 > 0 such that
‖ν‖ ‖a‖ ≤ a1 ‖ν‖+ a2 ‖ν‖2 (4.47)
As ‖ν‖ → ∞, the quadratic term dominates and hence the asymptotic behaviour is quadratic.
These observations allow us to conclude that there must exist some sufficiently large ν such that
νTKν ν is greater than νTa. Since the only term that grows with the third power of ‖ν‖ is positive
definite, this implies that Vν < 0 for sufficiently large ν. We are now in conditions of stating the
closed-loop result.
Proposition 4.6. Consider the system with states e′ and ν, whose dynamics are expressed respectively
in (4.6) and (4.20), in closed-loop with the feedback control laws (4.11) and (4.22). Consider as inputs:
• the disturbance estimation error d, which contains the estimation error regarding the current and
the velocity of the target;
• the disturbance estimate d;
• the time-derivative of the disturbance estimate ˙d.
Then, for appropriately chosen controller parameters K, n, Kd and δ, the system is bounded-input
bounded-state (BIBS) with restrictions on ‖d‖.
37
Proof. Consider the Lyapunov function V defined in (4.29) and its time-derivative (4.30). Note that if
‖d‖ and ε are small enough for condition (4.35) to be verified, then for sufficiently large e′ satisfying
(4.36), the derivative of the terms of the Lyapunov function concerning e′ is negative definite:
Ve′ ≤ −(1− θe′)e′TKe′tanh ‖e′/n‖‖e′‖
< 0 (4.48)
Consider now the derivative of the terms of the Lyapunov function concerning ν:
Vν ≤ −(1− θν)νTKν ν − θν νTKν ν + ‖ν‖ ‖a‖ (4.49)
Note the observations stated previously concerning how these terms grow with ‖ν‖. Because of the
nonlinear drag, for large enough ν, the higher-order drag terms dominate over ‖ν‖ ‖a‖. Then, for large
enough ‖ν‖,
Vν ≤ −(1− θν)νTKν ν < 0 (4.50)
Since V ≤ Ve′ + Vν , as long as condition (4.35) is satisfied, Theorem 2.8 can be applied to conclude
the BIBS property.
Note that the result does not contain any explicit expressions for ultimate bounds as a function of
the controller parameters. Instead, it is derived for the model structure given before, without considering
any numerical values. Consequently, the result is general enough to apply to a wide range of vehicles.
We can now make some observations regarding the influence of the controller parameters taking into
account the dynamics of the complete closed-loop system, by looking at (4.30). As stated in the outer-
loop analysis, higher eigenvalues of K make the first term in Ve′ more negative, and hence help reject
the effect of d. On the other hand, they increase the sensitivity of the outer-loop controller and produce
commands for the inner-loop that vary faster, reducing stability of the inner-loop. The parameter n
adjusts this sensitivity: lower values make the outer-loop controller provide commands that vary faster,
again helping reject d but reducing the stability of the inner-loop. A higher inner-loop gain Kd increases
stability of two inner-loop variables u and r, but because of its structure, it cannot directly affect v (the
stability of the sway motion is only due to the drag forces). Finally, δ acts as a gain on r for both
the feedforward and feedback terms of the outer-loop. Decreasing it produces more aggressive yaw rate
commands rd, which require the inner-loop to be faster.
Remark 1. In practice, the tracking error converges to zero when the virtual target moves with a known
constant velocity vector. This is not captured by the closed-loop result stated above because the Lyapunov
function used is not always strictly decreasing along the trajectories of the system. Had the closed-
loop system been analysed using different Lyapunov functions, or theory of interconnected systems, we
might have been able to prove convergence at the expense of a much more cumbersome analysis. This
convergence, however, is still proved by the outer-loop analysis in Section 4.2, if we ignore the dynamics
of the vehicle by setting u = 0 and v = 0.
38
4.4 Application to Formation Control
The trajectory-tracking algorithm presented above can be fed signals from a new block, called a path
generator. The block diagram in Figure 4.2 illustrates the structure of the entire system, including this
new path generator block.
pd
V c
pd e
ud u τ u, v, r p, ψPathGenerator
Outer-loopcontroller(TT)
Inner-loopcontroller
AUVdynamics
AUVkinematics
Trajectory-tracking + Inner-loop
Inner-loop
Figure 4.2: Block diagram of the complete system, including the path generator.
The required outer-loop controller inputs include only instantaneous measurements (or estimations
of) pd(t), pd(t), Vc(t), and no prior knowledge of the trajectory to be tracked is assumed. Hence, if we
feed the trajectory-tracking controller with pd(t), pd(t) commands appropriately produced by the path
generator block, the vehicle will be able to track the desired trajectory within the bounds and restrictions
determined above.
Consider now the case of a multi-agent system, composed of several, possibly heterogeneous vehicles,
where the dynamics of each vehicle and its controllers can be represented by a block diagram like the
one shown in Figure 4.2. Assume that a control objective is to steer each vehicle in such a way that
the multi-agent system moves in a formation, i.e. according to formation objectives that specify how
each vehicle should move relative to others. Suppose that the path generator block in each vehicle can
sense information about the remaining vehicles, and based on that information determine a position and
velocity of the target point. Then, we can call the path generator a formation controller.
Remark 2. Note that path-following techniques could not be used instead of trajectory-tracking, because
that would require each vehicle to have a-priori knowledge of the trajectory it should follow. This is not
possible because the trajectory each vehicle should follow depends on the motion of the remaining vehicles
in the formation. Trajectory-tracking, on the other hand, requires only knowledge of the instantaneous
position and velocity of the target point.
Given the stability results concerning the trajectory-tracking controller, we would then expect the
overall formation to be stable, that is, the position error of each vehicle with respect to a position
satisfying the formation objectives would remain bounded. This is not always true in the general case.
To see this, consider as an example a formation with two vehicles A and B. Consider also that both
vehicles can measure its relative position with respect to the other vehicle, and move based on that
39
information to try to maintain a desired relative position. In vehicle A, the pd(t), pd(t) commands are
generated based on information sensed from B, which in turn is moving based on information sensed from
A. The motion of each vehicle is affected by the other, possibly rendering the overall formation dynamics
unstable or poorly performing. In the more general case of a formation of n vehicles, and denoting by xi
the state of each vehicle, the dynamics of each vehicle depend on the state of the remaining vehicles in
the formation. The dynamics can be represented in general form as
xi = fi(x1, ...,xi, ...,xn), i = 1, ..., n (4.51)
and illustrated in Figure 4.3. In this general case where the motion of each vehicle in the formation
can depend on the motion of all the others, it is clear from both (4.51) and Figure 4.3 that feedback
interconnections exist between the systems corresponding to each individual vehicle.
Pathgenerator
pd, pdOuter-loop+ Inner-loop
+ AUV
Vehicle 1
Pathgenerator
pd, pdOuter-loop+ Inner-loop
+ AUV
Vehicle 2
Pathgenerator
pd, pdOuter-loop+ Inner-loop
+ AUV
Vehicle n
Formationstate
x1
x2
xn
Figure 4.3: Block diagram of the multi-agent system.
4.4.1 Leader-follower formations
A formation or sensing graph specifies what vehicles each element in a formation can obtain infor-
mation from, or measure its relative position to. It turns out that, for topologies of the formation graph
that do not contain cycles1, composed of leader-follower pairs, no feedback interconnections exist between
controllers of different vehicles. As a consequence, the path generator may be seen as forming a cascade
interconnection with the trajectory-tracking controller.
Although leader-follower formations have well-known drawbacks concerning over-reliance on a single
vehicle, they are also very appreciated for their simplicity and scalability. Avoiding feedback intercon-
nections to the path generator significantly simplifies the system, since no formation dynamics have to1Graph theory and related concepts are not introduced in this thesis; for details, see [43].
40
be included in the stability analysis. Given the BIBS result concerning the trajectory-tracking controller
and the inner-loop, we can easily include the path generator in the stability analysis. This consists of
simply enforcing that the signals fed by the path generator to the trajectory-tracking controller satisfy
the conditions imposed in the BIBS result; in that case, the states e′ and ν will remain bounded.
The problem of achieving a position relative to a leader is now reduced to designing the path generator
block, or formation controller, in accordance with the formation objectives. This is the purpose of the
next chapter. Additionally, until now we have assumed that an estimate of d = pd − V c exists; the
problem of estimating this term will also be addressed in the next chapter.
4.5 Summary
In this chapter, the problem of trajectory-tracking was studied. An inner-outer-loop control approach
was used, with intuitive control laws. A local bounded-input bounded-state result was obtained for
tracking and inner-loop errors. Finally, the application of trajectory-tracking to formation control was
clarified, specifically in the case of leader-follower formations.
41
Chapter 5
Formation Control
This chapter builds on the trajectory-tracking algorithm introduced in the previous chapter, and
addresses the problems of keeping formations based on 1) relative position measurements or 2) range-only
measurements. As mentioned in previous chapters, only the case where the formation has a leader-follower
topology is considered. In this situation, the path-generator block that produces the commands fed to the
trajectory-tracking algorithm (position and velocity of the virtual target) forms a cascade interconnection
with the trajectory-tracking algorithm. The motion of the leader is also considered unknown a-priori to
the followers.
The context where these two problems arise within the scope of the MORPH project is taken into
account, and constraints are considered in terms of equipment available. Control strategies are proposed
for formation control in the MORPH upper and lower segments. Some conclusions are drawn regarding
the necessary flow of information between vehicles and the necessary equipment onboard each vehicle.
Throughout this chapter, the outputs of the sensors and communication channels are assumed to be
continuous signals. Although this is not the case in the real setup, this approach simplifies the analysis
of the control problems by avoiding aspects related to estimating these signals in between samples. The
discrete nature of these signals is taken into account in later chapters.
5.1 Formation control with relative position measurements
We start by considering the formation control problem that arises in the context of the MORPH
upper segment. Recall from Section 1.3 that this problem is described in loose terms as driving GCV and
LSV (followers) to appropriate positions relative to SSV (leader), using relative position measurements
obtained from one USBL (that can be placed in any vehicle), without any direct communication between
the SSV and LSV.
Before delving into the definition of the control objectives in more detail, we make some assumptions
regarding the onboard equipment and control blocks already available in the upper segment vehicles
(SSV, GCV and LSV):
1. a trajectory-tracking system with a basic inner-loop controller, such as the representative one shown
43
in the last chapter, is already available;
2. the GCV has a bidirectional acoustic communication link with both the SSV and the LSV, albeit
with very limited bandwidth;
3. one USBL transceiver and at least two transponders are available and can be mounted in any of
the three vehicles;
4. all vehicles are equipped with sensors that provide measurements of heading angle ψ with respect
to a common reference, coincident with the direction of the x axis in {I};
5. the SSV is equiped with a GPS receiver that provides measurements of its position in {I}.
5.1.1 Formation strategies
We now proceed to define the appropriate positions relative to SSV to which the GCV and LSV should
be driven to, by selecting the type of leader-follower formation to be used. Three strategies are evaluated
considering the objectives of the MORPH project. Lawnmower trajectories, typically used for sea bottom
mapping and surveying purposes, consist of straight lines and arcs. The analysis of formation strategies
takes this into account by looking at the expected behaviour of the vehicles in these two types of path
geometries. For simplicity, these strategies are discussed using a single follower, but are applicable to the
real problem (with two followers, GCV and LSV, and one leader, SSV).
1. Formation defined by constant leader position in the follower’s body-fixed frame {B}.
This type of formation defines the desired relative position of the follower with respect to the leader
to be constant in {B}, as was done in [44] for a unicycle model. The formation is then defined by
the coordinates of the desired leader position in {B}, δ = [δx, δy]T . Note that with this formation
strategy the follower position is not rigidly fixed to the leader; the follower may lie anywhere in a
circle centered in the leader, with radius ‖δ‖.
Some problems arise when using this strategy in marine vehicles. In the presence of currents or
sideslip, the vehicle is in general not aligned with the path followed by the leader. On straight lines,
this makes the follower move parallel to the leader.
2. Rigid formation in a frame moving with the leader and aligned with its course angle.
With this formation strategy, the desired position of the follower relative to the leader is instead
defined in a frame aligned with the leader’s velocity vector measured in {I}. Since the leader’s
trajectory is independent of the motion of the follower, the formation is rigid: the position of the
virtual target to be tracked by the follower is completely specified at any time by the motion of the
leader. Currents and sideslip no longer influence the trajectory of the follower, but two additional
problems arise:
• When the leader moves along an arc, the follower moves along a concentric arc with bigger
radius, forcing the follower vehicle to move faster to keep the formation.
44
• Transitions from straight lines to arcs, in the trajectory of the leader, make the trajectory of
the follower non-smooth.
3. Non-rigid formation based on the previous path of the leader. This option departs from
the two previous strategies by considering that the follower should position itself relative to the path
of the leader, and not just its current position or direction of motion. The formation can again be
specified by two parameters, δx and δy. The first parameter, δx, defines the desired distance between
the leader and the follower along the path of the leader. After moving this distance backwards along
the path, the position of the virtual target is then obtained by moving a distance of δy perpendicular
to the path of the leader. Figure 5.1 clarifies how these two parameters define the position of the
virtual target, at two time instants during a lawnmower mission. The formation parameters δx and
δy are both positive in this formation.
δyδx
δx
δy
Figure 5.1: Desired position of a follower vehicle based on the previous path of the leader.
5.1.2 Path-generator
Considering the three alternative strategies described above, the third option is clearly advantageous
in that the issues identified with the other strategies are avoided. The disadvantage is that the follower
needs information about the path of the leader. Since we assumed that the path of the leader is not known
a-priori by the followers, some information has to be transmitted from the leader to the follower. One
option would be the leader broadcasting the position and velocity of the virtual target. This approach,
however, is not scalable because the amount of information to be broadcast grows linearly with the
number of followers. To minimize the information exchanged between vehicles and adopt an approach
that scales easily with the number of followers, some assumptions are enforced:
1. The path of the leader contains only alternating straight-line and constant-curvature segments.
An important example of a path satisfying this assumption is the lawnmower maneuver presented
before;
2. The δx distance that specifies the desired along-path distance between the leader and the follower
45
is small enough such that, at any time during a mission, the virtual target of the follower is in the
same segment as the leader.
Assumption 1 is reasonable for usual mission scenarios that concern surveying or mapping of the
seabed, since they usually involve lawnmower or similar maneuvers. Assumption 2 essentially implies
that the follower is not intended to follow the leader a big distance behind. Note, however, that it
is impossible to satisfy this assumption during a complete mission unless it only contains one segment.
When the leader vehicle changes its motion from one segment to another, the follower is still in a previous
segment for any δx > 0. This has important implications concerning transient behaviour when changing
between segments, causing discontinuities in the path. The advantage of enforcing these two assumptions,
however, is that the follower can keep a non-rigid formation based on the path of the leader (formation
strategy 3), and the leader only needs to broadcast three parameters: its total velocity VL in {I}, course
angle χL and its derivative χL. The path can be generated in such a way that it only depends on the
present values of VL, χL and χL, so integration of past data is avoided. The amount of data to be
broadcast is small and does not increase with the number of follower vehicles, overcoming the limitation
imposed by limited bandwidth on inter-vehicle communication channels.
We now look at how the path-generator block in each of the follower vehicles should process this
information so as to feed the trajectory-tracking algorithm with appropriate commands pd, pd. The
discussion is again focused on a one-leader one-follower situation, and then adapted to the actual MORPH
upper segment formation control problem. We focus the discussion on how to solve the problem with
the two assumptions stated above; later, assumption 2 will be relaxed to avoid discontinuities in the
generated path.
The path-generation problem
Having defined the desired formation strategy and enforced simplifying assumptions, we can now
define the problem to be solved by the path-generator block in a clear manner.
Problem 5.1 (Path-generation with relative-position measurements). Consider a leader vehicle per-
forming a mission consisting of a sufficiently smooth concatenation of path segments. The path segments
alternate between straight lines and constant-radius turns. Consider now a virtual target to be tracked by
the follower vehicle, assumed to be in the same path segment as the leader vehicle at all times. Assume
that the total velocity VL, course angle χL and course angle rate χL of the leader vehicle are known to
the follower at all times.
Derive a path-generation strategy that computes the position and velocity of the virtual target to be
tracked by the follower vehicle, according to formation strategy 3 modified by the simplifying assumptions
presented before. The position and velocity of the virtual target should be provided by the path-generator
as an error vector in the follower’s body-frame {B} and an inertial velocity in {I}, i.e. the parameters e
and pd as defined in Section 4.2.
To address this problem, note that the three parameters broadcast by the leader do not contain infor-
mation about its position. An immediate conclusion is that the follower vehicle needs extra information
46
about the position of the leader. This can be provided by placing a USBL transceiver in the follower
vehicle and a transponder in the leader vehicle. We can now assume that the position of the leader in
the follower’s body-frame {B} is available via USBL measurements. Denoting by pL the position of the
leader and by p the position of the follower in {I}, the measurement provided by the USBL can be
written as RT (ψ) (p− pL).
We now derive the equations to obtain e and pd. Figure 5.2 illustrates the process and contains the
necessary vectors and angles for easier understanding of the derivation.
δx
δyχ1
χLpL
pd1pd
Figure 5.2: Position (in blue) and velocity of the point to be tracked, based on the motion of the leader(in red).
The body-frame error vector as defined in Section 4.2 is
e = RT (ψ) (p− pd) (5.1)
To compute e, we first need to determine pd. To this end, we first integrate the leader’s velocity
vector backwards over a path with course angle rate χ, over a distance of δx:
pd1 = pL −
δxVL∫0
R(−χLt)
leader’s velocity vector︷ ︸︸ ︷R(χL)
VL0
dt
= pL −
δxVL∫0
R(χL − χLt)
VL0
dt (5.2)
Although this integral has a closed-form solution, it is singular for χL = 0, i.e. for straight lines.
This singularity is removable, but using the closed-form would only make calculations more cumbersome.
Implementation-wise, simple numeric integration yields good results, so we keep the formulation using
integrals.
At this point, we know pd1 . The velocity vector of that point is the integrand in (5.2) with t = δxVL
:
pd1 = R(χL − χLδxVL
)
VL0
(5.3)
47
The direction of this vector is
χ1 = χL − χLδxVL
(5.4)
We now obtain the final position of the virtual target pd. This is done by adding a vector of length
δy in a direction rotated π2 radians clockwise from χ1,
pd = pd1 +R(χ1 +π
2)
δy0
(5.5)
= pd1 +R(χ1)
0
δy
(5.6)
The final expression for pd as a function of pL, VL, χL, χL and the formation parameters δx, δy is
pd = pL −
δxVL∫0
R(χL − χLt)
VL0
dt+R(χL − χLδxVL
)
0
δy
(5.7)
Note, however, that the follower vehicle cannot measure the position of the leader in the {I} frame,
pL. However, we only need to compute e and not pd itself:
e = RT (ψ) (p− pd) (5.8)
= RT (ψ)
p−pL −
δxVL∫0
R(χL − χLt)
VL0
dt+R(χL − χLδxVL
)
0
δy
(5.9)
= RT (ψ) (p− pL) +RT (ψ)
δxVL∫0
R(χL − χLt)
VL0
dt−R(χL − χLδxVL
)
0
δy
(5.10)
= RT (ψ) (p− pL)︸ ︷︷ ︸USBL measurements
+
δxVL∫0
R(χL − χLt− ψ)
VL0
dt−R(χL − χLδxVL− ψ)
0
δy
(5.11)
The computation can now be performed using only the formation parameters δx, δy, the USBL
measurements, the heading ψ provided by a magnetometer, and the three parameters broadcast by the
leader VL, χL and χL.
Finally, to obtain pd, we differentiate (5.6) and substitute (5.3) and (5.4),
pd = pd1 +d
dt
R(χL − χLδxVL
)
0
δy
(5.12)
= R(χL − χLδxVL
)
VL0
+ R(χL − χLδxVL
)
0
δy
+R(χL − χLδxVL
)
0
δy
(5.13)
= R(χL − χLδxVL
)
VL0
+R(χL − χLδxVL
)S(χL)
0
δy
(5.14)
where we assumed that the formation parameters are constant, δx = δy = 0, and that the radius of
curvature of the leader’s path is constant, ddtχLVL
= 0.
48
Proposition 5.1. Problem 5.1 is solved by (5.11) and (5.14): the position and velocity of the virtual
target that the path generator needs to feed to the trajectory-tracking controller can be computed from the
available information using these two equations.
Remark 3. Despite the apparent complexity of (5.11) and (5.14), note the simplicity of the path-generation
strategy illustrated in a geometric sense in Figure 5.2. During turns, keeping the formation structure
implies that the follower moves with the same angular velocity as the leader. As a consequence, if δy 6= 0,
the velocity of the follower needs to be scaled. The term responsible for this scaling is the second term
in (5.14).
Remark 4. Throughout this derivation we assumed that the leader vehicle is performing a turn, i.e.
χL 6= 0. However, it is also valid when the leader vehicle is moving in a straight-line, in which case some
terms vanish and the equations simplify considerably.
Avoiding path discontinuities
The second assumption in Section 5.1.2 states that both the leader and the virtual target of the
follower lie on the same segment of the path. As noted before, for any δx > 0 this cannot be satisfied at
all times, causing undesired discontinuities in the generated path when the leader changes segment. In
order to address the problem, we relax this assumption, replacing it by a less restrictive one that avoids
path discontinuities:
The δx distance that specifies the desired along-path distance between the leader and the
follower is small enough such that, at any time during a mission, the virtual target of the
follower is either in the same segment as the leader, or in the previous one.
This assumption can be satisfied at all times if δx is smaller than the length of the smallest segment in
the leader’s path. Hence, in this situation, no discontinuities arise in the path generated for the follower
to track. Relaxing this assumption, however, requires a more complex algorithm for the path-generator
that needs case-testing to address situations where the leader and virtual target of the follower do not lie
in the same segment. The algorithm used to generate the path is now briefly described.
1. Generate the relative position e and the velocity pd of the virtual target according to the procedure
described above, i.e. using (5.11) and (5.14).
2. If the generated position of the virtual target deviates more than a threshold ε from its current
position, then there is a discontinuity in the generated path, so the leader and the virtual target are
not in the same segment - we proceed to step 3. Otherwise, they are in the same segment, so the
position and velocity of the virtual target computed in step 1 are correct and the algorithm stops.
3. If the present value of χL broadcast by the leader is non-zero, then the leader is in a turn. The
virtual target is in the previous segment, which must be a straight line because of the assumption
that path segments alternate between straight lines and arcs. In this case, to compute the relative
position of the virtual target e, we generate the path by integrating backwards along the path of
49
the leader in a turn until a certain point, and then in a straight line. To this end, we use (5.11)
with two modifications:
• The integral term in (5.11) is computed with the present value of χL until the direction of
the path being generated matches the direction in which the virtual target is moving. The
remainder of the integral, i.e. until the total integration time reaches δx/VL, is computed with
χL = 0.
• The last term in (5.11) is computed with χL− χL δxVL
replaced by the course angle of the virtual
target.
Regarding the computation of the velocity of the virtual target pd, we use (5.14) with two modifi-
cations:
• In the first term, χL − χL δxVL
is replaced by the course angle of the virtual target.
• The last term vanishes.
4. If the present value of χL broadcast by the leader is zero, then the leader is in a straight line.
The follower is in the previous segment, which must be a turn. In this case, we should integrate
backwards along the path of the leader in a straight line until a certain point, and then in a turn.
A procedure similar to the one in step 3 is used to compute e and pd.
Using this path-generation algorithm, if the trajectory of the leader satisfies the two assumptions
given above, then the commands produced are in accordance with formation strategy 3 as described in
Section 5.1.1, without any discontinuities in the generated path.
5.1.3 Estimation of constant current
Recall from Chapter 4 that the trajectory-tracking controller also needs an estimate of velocity of the
current measured and expressed in {I}, Vc. We now derive an estimator for the current that can be
analysed in a cascade interconnection with the controller. The derivation is carried out in a deterministic
framework, so no random variables are considered; in the implementation phase, which is described in the
next chapter, the estimator derived in this section is replaced by a Kalman filter with an identical model,
to take measurement noise into account. The terms estimator and observer are used interchangeably in
this section.
At this point, we know that each follower should have access to its position relative to the leader. For
simplicity, we consider that this relative position is available, expressed in {I}. If it is only available in
{B}, it can be transformed to {I} through left-multiplication by R(ψ). We denote this relative position
in {I} by
eU = p− pL (5.15)
where the subscript U is used to clarify that this estimation concerns the upper segment vehicles. The
50
dynamics of eU are
eU = p− pL
= R(ψ)
uv
+ Vc − pL
= R(ψ)
u0
− pL + Vc +R(ψ)
0
v
(5.16)
The velocity vector of the leader vehicle pL can be computed using information that the leader
broadcasts:
pL = R(χL)
VL0
(5.17)
Then, the first two terms in (5.16) are available, and will be considered an input to the estimator:
zU = R(ψ)
u0
− pL (5.18)
The sway cannot be measured and is considered unknown; the velocity of the current is what we wish
to estimate. Consider now the dynamics of a system with xU = [eL V c]T as states, where V c = 0 in
accordance with the constant current assumption:
xU =
0 I2
0 0
︸ ︷︷ ︸
A
xU +
I20
︸ ︷︷ ︸B
zU +
R(ψ)
0
0
v
(5.19)
If we define an observer with identical dynamics (except for the unknown sway) and include a pro-
portional feedback on eU ,
˙xU = AxU +BzU +Ko
[I2 0
]︸ ︷︷ ︸
C
(xU − xU ) (5.20)
then the dynamics of the estimation error xU = xU − xU are
˙xU = (A−KoC)xU + b (5.21)
where
b =
R(ψ)
0
0
v
(5.22)
If we ignore b, the system is linear with observability matrix
O =
C
CA
CA2
CA3
=
I40
(5.23)
51
which has rank 4. Since there are 4 states in the system, we conclude that the eigenvalues of the matrix
L = A − KoC can be chosen arbitrarily by appropriate choice of Ko. However, the term b acts as a
non-vanishing disturbance. Hence, we can say that the estimator does not converge to zero error. If Ko
is chosen so that all eigenvalues of L have negative real part, we know from linear systems theory that
for all Q = QT > 0 there is a unique solution P = PT > 0 to the Lyapunov equation:
LTP + P L+Q = 0 (5.24)
Consider now a Lyapunov function V = xTU P xU . Its time-derivative is
V = ˙xTU P xU + xTU P ˙xU
= (L xU + b)TP xU + xTUP (L xU + b)
= xTU (LTP + P L)xU + 2xTUP b
= −xTU Q xU + 2xTU P b
≤ −(1− θ)xTU Q xU − θxTU Q xU + 2xTU P b (5.25)
with 0 < θ < 1. Since Q > 0,
V ≤ −(1− θ)xTU Q xU < 0 (5.26)
when
‖xU‖ ≥2λmax(P ) ‖b‖θλmin(Q)
(5.27)
Since ‖b‖ = |v|, we conclude that the observer error system is ISS with respect to input v.
At this point, we can use this estimator to feed the trajectory-tracking controller with an estimate
of the current V c. Because the estimator dynamics are perturbed by v, the interconnection with the
controller is not a cascade, but a feedback. Ideally, the system including both the controllers and the
estimator should be analysed to guarantee stability.
However, introducing the estimator dynamics in the loop would make the analysis in Chapter 4
significantly more complex. As such, we make the simplifying assumption that no considerable interaction
exists between the controller and the estimator. This could be a consequence of ignoring the vehicle
sideslip by assuming v = 0; or, less restrictively, of assuming a small initial v and x, and an observer gain
that does not produce very aggressive variations of the estimate. We can now state a result concerning
boundedness of the tracking error in the upper segment vehicles.
Proposition 5.2. Under the assumptions stated above, using the path-generator described in Section
5.1.2, the inner and outer-loop controllers described and analysed in Chapter 4, and the estimator de-
scribed in the present section, the formation error of the upper segment vehicles (position error with
respect to where a vehicle should be to satisfy the formation constraints) remains bounded.
Proof. Recall that the result from Proposition 4.6 states that the system with tracking error e′ and
inner-loop error v as states is BIBS with respect to inputs d (with restrictions), d and ˙d. Under the
assumptions stated above, i.e. that the observer gain Ko is appropriately chosen, the estimation error
(which corresponds to d) will converge to a small neighborhoood of the origin. Also, the estimate d and
52
its derivative ˙d are bounded. Then, as a consequence, e′ and v are bounded. The signal e′ is exactly the
formation error of the vehicle. Hence, the formation error remains bounded.
Finally, recall the remark from the closed-loop analysis in Section 4.3: although the result obtained
does not prove asymptotic stability when the target moves with a constant known velocity vector, this
is observed in practice for small enough target and current velocities. It can also be concluded from the
outer-loop analysis in Section 4.2 if the dynamics of the vehicle are ignored by setting u = 0 and v = 0.
Under these assumptions, if we assume that the velocity vector of the target is not known but estimated
using the observer presented above, the tracking error converges to zero.
5.1.4 Applications to the MORPH upper segment
The path-generation strategy and the estimator derived above can be used in the MORPH upper
segment by considering that GCV and LSV both follow the SSV. Both followers should have access to
their position relative to the leader and the necessary leader path parameters. Then, each of the followers
can use its own path-generator and trajectory-tracking controller to keep a formation relative to the SSV,
but obviously with different δx and δy to avoid collisions.
Note, however, that according to the assumption made in Section 5.1 when setting up the problem,
only one USBL is available and the SSV cannot communicate directly with the LSV. A solution that
allows both followers to know their positions relative to the leader and the leader path parameters is to
use the GCV to relay information to the LSV. This can be accomplished by placing the USBL on the
GCV and equipping the SSV and LSV with transponders. Then:
1. the leader path parameters are simply received by the GCV and passed on to the LSV via acoustic
communications;
2. the position of the LSV relative to the leader can be calculated by the GCV and sent to the LSV
via acoustic communications.
Surface2Vehicle2(Leader)GPS2+2Acoustic2Comms
1
p - p1 2
p - p3 2
p - p1 3
acoustic2commsUSBLGPS
Underwater2Vehicle2(Follower)USBL2+2Acoustic2Comms
2Underwater2Vehicle2(Follower)
Acoustic2Comms
3
p 1
V, χ, χt
of2the2leader2(estimated2from2GPS)
Information2Flow
V, χ, χt
of2the2leader2(estimated2from2GPS)
SSV
GCV LSV
Figure 5.3: Information flow in the MORPH upper segment.
53
Figure 5.3 illustrates the information flow between the three vehicles in the MORPH upper segment.
This information flow ensures that both the GCV and LSV can generate and track a virtual target that
moves according to the formation objectives.
5.2 Formation control with range-only measurements
The problem of formation control with range-only measurements arises when a follower vehicle does
not have access to its position relative to the leader vehicle, but only to the range between them. In the
context of marine robotics, this problem is of considerable interest. The reason for this is that inter-vehicle
underwater relative position measurements are usually provided by USBLs. Range measurements, on the
other hand, can be obtained using acoustic pingers and transponders, which are considerably cheaper.
Within the MORPH project, this problem arises on the lower segment. Neither of the camera vehicles,
C1V and C2V, carry USBL transceivers, but both are able to measure the ranges to GCV and LSV. In
this section, we look at how to drive C1V and C2V to appropriate positions with respect to GCV and
LSV, i.e. make C1V and C2V move in formation with the upper segment.
5.2.1 The localization problem
We start by discussing the problem of localization of C1V and C2V. To be able to drive them to
convenient positions in the formation, these vehicles need information regarding their position. However,
since neither of the vehicles carry sensors providing position measurements, some discussion is necessary
to determine how C1V and C2V should obtain this information.
At this point, we discuss three alternative options, assuming that C1V and C2V only have access to
range measurements to GCV and LSV, an internal dead-reckoning model, and an acoustic communication
link to GCV and LSV. For simplicity, we refer to C1V and C2V as followers and GCV and LSV as leaders.
1. Position broadcast by GCV. As has been concluded in the discussion about the upper segment,
the formation can be maintained using a single USBL transceiver carried by the GCV. Assuming
that all the remaining vehicles carry transponders, including the lower segment ones, GCV has
access to the position of the followers relative to itself. This information could be broadcast to the
followers. The disadvantages of this are that 1) the amount of data broadcast is significant and
grows linearly with the number of followers, and 2) the range measurements available are not used.
2. Active single-beacon localization. This strategy consists of making each of the followers move
in such a way that the position with respect to one leader vehicle (acting as a range beacon) can be
estimated using the range measurements and the internal dead-reckoning model. In practical terms,
this is a problem of single-beacon range navigation. It is important to note that, in this situation,
the beacon (the leader vehicle) is moving with a velocity unknown to the follower. This renders
the system unobservable if the velocity of the beacon is considered to be arbitrary and unknown.
To overcome this problem, in [45] a cyclic stop-and-go strategy is used. This consists of making
subsets of vehicles in a formation stop at specific time intervals so that the remaining vehicles can
54
localize themselves using only range measurements. The clear disadvantage, in the scope of the
MORPH project, is that the entire formation of vehicles would have to stop periodically for the
lower segment followers (C1V and C2V) to localize themselves.
3. Two-vehicle range localization. Since the lower segment followers have access to range mea-
surements to two leaders (GCV and LSV), a possible strategy for localization consists of combining
this information. In fact, if the distance d between the two leaders is known, a follower can infer
its position in a mobile frame {M}, apart from a reflection ambiguity. This frame is oriented with
the line between the two upper segment vehicles, with the origin at the midpoint of that line. This
is illustrated in Figure 5.4, with the two leader vehicles shown in green and red, and the two pos-
sible positions of the follower vehicle in blue. The ranges between the follower and the leaders are
denoted by d1 and d2.
A simple workaround to disambiguate the position of the follower is to make one of the leaders -
the GCV vehicle, which carries a USBL transceiver - broadcast a bit that determines whether the
follower lies on the semi-plane with positive or negative yM . Obviously, with more than one follower,
one bit per follower would have to be broadcast, but this would still be a very small amount of data,
without implications concerning the bandwidth of the communication link. As for the distance d
between the leaders, in the MORPH project this information is available to C2V and C2V as a
consequence of the communication architecture currently implemented.
d1
d2
d1
d2
{M}xM
yMd
Figure 5.4: Ambiguity when determining the position of a follower in {M} based on the ranges betweenthe follower and two upper segment vehicles.
The two-vehicle range localization scheme (option 3) is advantageous, in that it makes use of both
range measurements and the communication link, while requiring very small amounts of data to be
transmitted over acoustic channels. More importantly, stop-and-go strategies are avoided, which would
have implications with the entire formation.
We now determine closed-form equations for the position of a follower in {M} given d, d1 and d2.
55
This position is denoted by Mp = (Mpx,M py). The positions of the two upper segment vehicles in {M}
are Mp1 = (d/2, 0) and Mp2 = (−d/2, 0). Then,
∥∥Mp−Mp1
∥∥ = d1 (5.28)∥∥Mp−Mp2
∥∥ = d2 (5.29)
Expanding the norms and squaring both sides yields(Mpx −
d
2
)2
+(Mpy
)2= d2
1 (5.30)(Mpx +
d
2
)2
+(Mpy
)2= d2
2 (5.31)
Subtracting (5.31) from (5.30), we obtain an equation from which Mpx can be determined:(Mpx −
d
2
)2
−(Mpx +
d
2
)2
= d21 − d2
2 (5.32)
⇔ Mpx = −d21 − d2
2
2d(5.33)
Having determined Mpx, we can solve (5.30) for Mpy:
Mpy = ±
√d2
1 −(Mpx −
d
2
)2
(5.34)
The ambiguity in using this two-vehicle range localization technique is now obvious in (5.34), with
the ± sign. We now assume that, according to previous discussion, GCV broadcasts a disambiguating
bit and hence each follower knows its position in the mobile frame, Mp, without ambiguities.
5.2.2 Path-generator
The problem of driving the lower-segment vehicles C1V and C2V to appropriate positions with respect
to GCV and LSV is now tackled. Since C1V and C2V localize themselves in the mobile frame {M}, the
problem is defined as moving C1V and C2V to constant positions in {M}.
Problem 5.2 (Path-generation with range-only measurements). Consider two leader vehicles moving
along arbitrary trajectories. Consider now a follower vehicle that has knowledge of its position Mp in a
mobile frame {M} as defined above.
Derive a path-generation strategy that computes the position and velocity of the virtual target to be
tracked by the follower vehicle, such that the follower vehicle is driven to a constant position in {M}.
This position and velocity of the virtual target should be provided by the path-generator as an error vector
in {B} and an inertial velocity in {I}, i.e. the parameters e and pd as defined in Section 4.2.
We start by introducing some notation:
• pM is the position of the origin of {M}, expressed in {I};
• ψM is the angle between the x axis of {I} and the x axis of {M};
• Mp is the position of the follower vehicle, expressed in {M};
56
• Mpd is the position of the virtual target, expressed in {M}, and assumed to be constant.
This notation is clarified in Figure 5.5, where the current position of the follower vehicle is shown in
blue and the virtual target in green.
p
pM
Mp
{I}
{M}
Mpd
ψM
pd
Figure 5.5: Follower vehicle and desired point, along with the reference frames and position vectors.
Additionally, the assumption that the two leaders move along arbitrary trajectories is equivalent to
assuming that {M} moves arbitrarily. We introduce some parameters concerning this motion:
• pM is the velocity of the origin of {M} with respect to {I}, expressed in {I};
• ψM is the angular rate of {M} about its origin.
We can now write the position of the virtual target in {I}:
pd = pM +R(ψM )Mpd (5.35)
Similarly, the position of the follower in {I} can be written as:
p = pM +R(ψM )Mp (5.36)
The body-frame position error can now be expanded:
e = RT (ψ) (p− pd)
= RT (ψ)[(pM +R(ψM )Mp
)−(pM +R(ψM )Mpd
)]= RT (ψ)R(ψM )
(Mp−Mpd
)(5.37)
where Mp is known, considering that the two-vehicle range localization approach is used (see Section
5.2.1), and Mpd is a parameter of the formation.
Now, to determine pd as a function of known parameters, we differentiate (5.35):
pd = pM + R(ψM )Mpd +R(ψM )M pd
= pM +R(ψM )S(ψM )Mpd (5.38)
57
It is now visible that, for the path-generator to be able to produce the signals e and pd that are fed to
the trajectory-tracking block, three new parameters are needed besides those already discussed in Section
5.2.1: the orientation the mobile frame ψM , its angular rate ψM and its velocity vector expressed in {I},
pM . The orientation of the mobile frame ψM can be computed by the GCV, since it has access to its
position relative to the LSV; it is then broadcast to the lower-segment vehicles. However, ψM and pMcannot be computed by any vehicle without introducing extra communications between upper segment
vehicles. This implies that lower segment vehicles will not have complete access to the velocity of their
virtual targets - they will have to rely on an estimate.
5.2.3 Estimation of virtual target velocity and constant current
We now derive an estimator, similar to the one proposed for the upper segment, but modified to take
into account the information available to the lower segment vehicles. An important difference is that,
in the upper segment, the vehicles have complete knowledge of the velocity of their virtual targets pd,
and only need to estimate Vc. In the lower segment, both need to be estimated. As such, we estimate
directly the term d = pd − Vc, as defined in the controllers in Chapter 4. The derivation proceeds in a
similar fashion to that of the upper segment estimator, so only the main differences will be highlighted.
We start by using the relative position of the follower with respect to the virtual target. As was done
when designing the estimator for the upper segment vehicles, we assume this relative position is available
in {I}. Again, if it is only available in {B}, it can be transformed to {I} through left-multiplication by
R(ψ). We denote this relative position in {I} by
eL = p− pd (5.39)
where the subscript L clarifies that this estimator concerns lower segment vehicles. The dynamics are
eL = R(ψ)
u0
− pd + V c +R(ψ)
0
v
= R(ψ)
u0
− d+R(ψ)
0
v
(5.40)
The first term is available, so it is used as an input to the estimator:
zL = R(ψ)
u0
(5.41)
As for the other terms, d is what we wish to estimate, and the sway cannot be measured. Consider
the dynamics of a system with xL = [eL d]T as states:
xL =
0 −I20 0
︸ ︷︷ ︸
A
xL +
I20
︸ ︷︷ ︸B
zL +
R(ψ) 0
0 I2
0
v
pd
(5.42)
58
where d = pd because the current is assumed to be constant. Now, we design an observer with identical
dynamics except for the last term, which cannot be measured, and add a proportional feedback on the
estimation error of eL:
˙xL = AxL +Bz +Ko
[I2 0
]︸ ︷︷ ︸
C
(xL − xL) (5.43)
The dynamics of the estimation error xL = xL − xL are
˙xL = (A−KoC)xL + b (5.44)
where
b =
R(ψ) 0
0 I2
0
v
pd
(5.45)
Now, the remarks on observability and ISS made in Section 5.1.3 regarding the estimator for the
upper segment still apply. The only relevant difference is that the disturbance to the observer has an
extra term concerning the acceleration of the virtual target pd, which can be obtained by differentiating
(5.38).
Given this, we can recover the result on boundedness of the formation error for the vehicles in the
upper segment, and adapt it to the lower segment by enforcing an additional condition on pd.
Proposition 5.3. Assume that the acceleration of the virtual target is small enough for the error of
the estimator described above to converge to a small neighborhood of zero. Under the same assumptions
established in Proposition 5.2, using the path-generator described in Section 5.2.2, the inner and outer-
loop controllers described and analysed in Chapter 4, and the estimator described in the present section,
the formation error of the lower segment vehicles (position error with respect to where a vehicle should
be to satisfy the formation constraints) remains bounded.
5.2.4 Applications to the MORPH lower segment
The path-generator and estimator described above for the lower segment vehicles can be directly used
in the lower segment of MORPH by making the camera vehicles C1V and C2V use GCV and LSV as two
leaders, as was made clear throughout this section. Each camera vehicle has a defined desired position
in the mobile frame {M}, which is fixed to the center point of a segment between the GCV and LSV.
The desired positions for C1V and C2V should be separated by a reasonable distance to avoid collisions.
Regarding inter-vehicle communication, two parameters need to be broadcast by the GCV to the lower
segment to enable the camera vehicles to keep the formation: the orientation of the mobile frame ψM ,
and one disambiguation bit per camera vehicle (see Section 5.2.1). The range between the two leaders d is
also needed, but is automatically available to the camera vehicles as a consequence of the communication
architecture used.
59
Figure 5.6 shows an information flow diagram for the lower segment, and includes also the upper
segment from Figure 5.3.
Surface2Vehicle2(Leader)GPS2+2Acoustic2Comms
1
p - p1 2
p - p3 2
p - p1 3
acoustic2commsUSBLGPS
Underwater2Vehicle2(Follower)USBL2+2Acoustic2Comms
2Underwater2Vehicle2(Follower)
Acoustic2Comms
3
p 1
V, χ, χtof2the2leader2
(estimated2from2GPS)
Information2Flow
V, χ, χtof2the2leader2
(estimated2from2GPS)
SSV
GCV LSV
Underwater2Vehicle2(Follower)Acoustic2Comms
4CV1
Underwater2Vehicle2(Follower)Acoustic2Comms
5CV2
ψ M,
disambiguation bit ψ M,
disambiguation bit
|| p - p ||3 4
|| p - p ||2 4
|| p - p ||2 5
|| p - p ||3 5
ranges
Upp
er2S
egm
ent
Low
er2S
egm
ent
Figure 5.6: Information flow in MORPH upper and lower segments.
5.3 Summary
In this chapter, the problem of path-generation for follower vehicles was addressed in two different
scenarios: using relative position measurements and using range-only measurements. Desirable properties
of the trajectories of the vehicles in typical missions were taken into account. Reasonable assumptions were
imposed on the trajectories of the leader vehicle in the formation, so that inter-vehicle communication was
reduced and the approaches obtained were scalable to a bigger number of follower vehicles. Estimators
were also proposed for both scenarios. Finally, possible information flow strategies were presented, which
would allow the proposed solutions to be applied to the MORPH upper and lower segments.
60
Chapter 6
Implementation
In this chapter the control systems and estimators designed and analysed from a theoretical perspective
in the previous chapters are adapted to fit the real vehicles in the upper segment of MORPH. We discuss
implementation as the necessary changes to allow the systems to be used in the real vehicles. The low-
level implementation itself was done by MORPH team members Henrique Silva, Jorge Ribeiro and Miguel
Ribeiro, using an existing ROS (Robot Operating System) architecture.
The implemented control system keeps the same structure with an inner-loop, a trajectory-tracking
outer-loop, and a path generator. However, due to constraints and limitations of the real systems, these
blocks and the signals sent between them differ slightly from the strategies proposed in the previous chap-
ter. Also, additional issues that were ignored in the theoretical analysis for simplicity purposes had to be
taken into account in the implementation, like the discrete nature of measurements and communications.
In this setting, the implementation is described taking into account three main issues. Firstly, the
trajectory-tracking controller is modified to provide commands in ψ and u. This modification makes this
controller compatible with all the vehicles involved in the project. Secondly, the necessary inter-vehicle
communication schemes are clarified. Finally, the subject of filtering and estimation is addressed, with
highlighted relevance because of the long communication cycles and delays involved.
The implementation described here illustrates the control and estimation systems used in the upper
segment vehicles in the MORPH project trials in Lisbon, Girona, and Faial, Azores, in the summer of
2014. The lower segment control architecture described has not yet been implemented in the vehicles.
6.1 Trajectory-tracking controller
Due to compatibility issues between all the vehicles involved in the MORPH project, the inner-loop
controller accepting velocity commands ud and rd, as analysed in the previous chapters, could not be used.
For the MEDUSA vehicles, a PID inner-loop controller was already available. This controller accepts
commands in u and ψ. The heading controller has a feedforward term that allows it to track ramp signals
in the requested heading ψd. This is important to be able to track constant-curvature trajectories, as the
heading varies linearly.
61
With this different inner-loop controller, the outer-loop has to be adapted so that it provides surge
speed and heading requests (ud and ψd) instead of surge speed and yaw rate. Recall the trajectory-
tracking controller used in the analysis in Chapter 4:
ud =
udrd
= Γ−1(RT (ψ)d−K tanh
e
n
)(6.1)
The matrix Γ was introduced to overcome the fact that the sway speed v can not be actuated directly.
The purpose of Γ is to scale a desired sway speed to a desired yaw rate by considering that the point
to be driven to the target is not the center of rotation of the vehicle, but a point slightly ahead. This
scaling is quantified by the parameter δ; see Chapter 4 for more details.
Based on this discussion, we can obtain a desired velocity vector with respect to the water, expressed
in {B}, if we take (6.1) and ignore the left-multiplication by Γ−1:
BV d = RT (ψ)d−K tanhe
n(6.2)
which can be expressed in {I} by
V d = d−R(ψ)K tanhe
n(6.3)
We now define the commanded surge speed and heading as the norm and angle of V d,
ud = ‖V d‖ (6.4)
ψd = ∠V d (6.5)
where ∠x : R2 → ]−π, π] is a vector argument function such that
R(∠x)
‖x‖0
= x (6.6)
This function was implemented using the 4-quadrant arctangent function, atan2. Note that this function
is not defined for a vector of norm zero.
At this point, we assume that V d varies slowly. This is a reasonable assumption because in the
MORPH project the nominal speeds of the vehicles are low, and the radii of curvature are considerable.
Then, we can assume that the inner-loop PID controllers of the vehicles can track the desired surge and
yaw commands with small error. We formalize this assumption through∥∥∥V d
∥∥∥‖V d‖
<< 1 (6.7)
where the inner-loop error is defined as
V = R(ψ)
u0
−R(ψd)
ud0
= R(ψ)
u0
− V d (6.8)
62
Again, because of the low speeds used in the MORPH project, we can also assume that the sway speed
is negligible when compared with the commanded velocity vector:
|v|‖V d‖
<< 1 (6.9)
Remark 5. For assumptions (6.7) and (6.9) to be valid, ‖V d‖ cannot approach 0.
To analyse the dynamics of the tracking error e with this controller, we first write the velocity of the
vehicle in {I} as
p = R(ψ)
u0
+
0
v
+ V c
= V + V d +
0
v
+ V c
≈ V d + V c (6.10)
Recovering the dynamics in (4.5) and substituting V d by the control law (6.3) yields
e = −K tanhe
n−RT (ψ)d− S(r)e (6.11)
These dynamics are identical to those in (4.12), but with no inner-loop error and no sway speed. Hence,
we can recover the result from Proposition 4.2: the system with state e′ is ISS with respect to input d,
with restrictions on the norm of the input. A particularly important implication of this result is that if
d→ 0 then e→ 0; even if d does not approach zero, the tracking error remains bounded.
This controller was run in discrete-time, at a frequency of 5 Hz.
6.1.1 Issues with commanded heading
Some issues arise with the controller presented above as ‖V d‖ approaches zero. As stated in a remark
made above, assumptions (6.7) and (6.9) no longer hold in this case, and both the inner-loop error and the
sway speed may become non-negligible. More importantly, the angle function used to produce heading
commands ψd is singular for V d = 0. This singularity is not removable, and as a consequence there is no
value that could be adopted for ψd(V d = 0) that would make ψd continuous at V d = 0. On a practical
level, the main issues with this singularity are:
• the increasing sensitivity of ψd to variations in a direction normal to V d as ‖V d‖ → 0;
• the possibility of discontinuities in the ψd(t), even if V d(t) is continuous.
To illustrate the first problem, we compute the derivative of the commanded heading with respect to
a variation on the normal direction of V d, and show that it increases as ‖V d‖ → 0:
∠V d = arctanV dx
V dy
(6.12)
V d = (δ, Vdy ) (6.13)
63
∂ψd∂δ
∣∣∣∣δ=0
=∂
∂δ
(arctan
δ
V dy
)∣∣∣∣δ=0
=V dy
V 2dy + δ2
∣∣∣∣∣δ=0
=1
V dy
=1
‖V d‖(6.14)
As for the second issue, an example would be V d(t) = (−1 + t, 0) for t ∈ [0, 2]. While this signal is
continuous in time, the corresponding commanded heading would be
ψd =
π, if t < 1
undefined, if t = 1
0, if t > 1
(6.15)
Note that, although seemingly critical, this problem does not arise regularly in typical missions of the
MORPH project. This is because, for ‖V d‖ to approach zero, the feedforward term d and the feedback
term R(ψ)K tanh en in (6.3) would need to have opposite signs. This means that the follower vehicle would
be in front of the virtual target, which does not happen during a mission unless possibly during the initial
transients. Hence, no workaround was implemented to overcome this issue. However, a simple possible
solution could be keeping ψd constant when V d is inside a ball around the origin, i.e., when ‖V d‖ < λ,
where λ defines the radius of the ball; a hysteresis mechanism on λ could also be implemented.
6.2 Communication
The inter-vehicle communication scheme is used to transmit information necessary for the control
algorithms to work as expected. In Chapter 5, we concluded that the information to be transmitted
between vehicles can be summarized by Figure 5.6.
The communication architecture supporting this information flow is a TDMA (time division multiple
access) scheme. Details on this communication architecture are not relevant and are omitted here. It is
important to note, however, that all transmitted information is quantized with non-negligible quantization
steps, and that the cycle has a duration of 5 seconds. This means that acoustic communications, relative
position measurements and range measurements arrive with a period of 5 seconds. Both the quantization
and the duration of the cycle were taken into account when tuning the estimators.
6.3 Filtering and estimation
Until now, all the measurement signals have been assumed to be continuous-time signals and contain
no noise or delays. This is not the case in the real system, where measurements are obtained from sensors
with long sampling periods or communication channels where data is quantized and delayed. These issues
require filters, which take discrete-time, noisy, delayed measurements and produce estimates at rates high
enough for control purposes. In this section, we describe the necessary filtering architecture implemented
in the upper segment vehicles, consisting of three Kalman filters. The first two of these filters were
developed in collaboration with Mohammadreza Bayat. The third filter is an adapted version of a filter
used in the project, previously designed mainly by Mohammadreza Bayat. The three filters run at 5 Hz.
The conditions for optimality of the estimates provided by the Kalman filters were not checked.
64
Kalman filters were used instead of more complex, non-linear filters to keep the implemented system
simple and easily tunable. In practice, the performance of the filters was adequate for the purposes of
the project.
6.3.1 SSV path parameters
As stated previously, for the upper segment followers (GCV and LSV) to maintain the desired position
in the formation, the path generator block needs three parameters concerning the movement of the leader
(SSV): its total velocity VL, course angle χL and course angle rate χL. These parameters are broadcast by
the SSV. The SSV has a GPS receiver, so these states could be estimated from the position measurements
provided by the GPS.
In accordance with the typical mission scenarios in the MORPH project, two assumptions were made
regarding the motion of the SSV:
• its total velocity remains constant, VL = 0;
• its trajectory consists of a concatenation of straight lines and arcs of constant curvature, so in each
segment the course angle rate remains constant, χL = 0.
The continuous-time state space model is then
x =d
dt
VL
χL
χL
x
y
=
0
χL
0
R(χL)
VL0
(6.16)
y =
xy
=
0 0 0 1 0
0 0 0 0 1
x (6.17)
The corresponding discrete-time model is obtained by replacing the derivative operator by a finite-
difference approximation:d
dtx(t) =
x(t+ ∆t)− x(t)
∆t=xk+1 − xk
∆t(6.18)
where ti denotes the ith discrete time instant, xi denotes the state at the ith time instant x(ti), and ∆t
denotes the difference between consecutive time instants. The discrete-time state space model is then
obtained by
xk+1 = x ·∆t+ xk (6.19)
With this formulation, an extended Kalman filter was implemented. Note that the need for the
extended version of the filter is due to the nonlinearity introduced by the rotation matrix R(χL). Figure
6.1 illustrates the inputs and outputs to this filter, where ASV stands for Autonomous Surface Vehicle -
which plays the role of SSV in MORPH.
This filter is necessary when the motion of the SSV itself is not known prior to the mission, so that
the estimated VL, χL and χL can be broadcast to the followers. However, in case the entire trajectory
65
ASV
GPS toTargetVelocitypasv(t)
GPS
‖ ˙pasv(t)‖
χasv(t)
˙χasv(t)
Acoustic
Comms
Figure 6.1: Estimator running on SSV (drawn in collaboration with Mohammadreza Bayat).
of the SSV is planned beforehand, an alternative to using this filter is to broadcast the nominal values
of VL, χL and χL. This leads to better performance and has been used in the missions at the MORPH
trials shown in the next chapter.
6.3.2 Follower’s estimate of Leader’s path parameters
The previously designed filter runs in the SSV and outputs estimates of its path parameters based
on GPS measurements. These estimates are broadcast to the followers (GCV and LSV). However, this
broadcasting occurs at a rate smaller than needed by the controller. Hence, each follower should run its
own filter to estimate the path parameters of the leader in between broadcastings, at rates sufficiently
high for control purposes.
We formulate a system consisting of the first three states in (6.16), using as measurements the pa-
rameters broadcast by the SSV (which coincide with the states):
d
dt
VL
χL
χL
=
0
χL
0
(6.20)
y = x (6.21)
Again, a discrete-time formulation of this system can be obtained through the finite-difference ap-
proximation by applying (6.19). A Kalman filter with this model was used as estimator for the leader’s
path parameters.
6.3.3 Currents and Follower-Leader relative position
Two other important signals needed for control are the relative position of the follower with respect
to the leader, p − pL, and the velocity of the current, Vc. To design an estimator for these signals, we
start by writing the dynamics of the relative position:
p− pL = R(ψ)
uv
+ Vc − pL (6.22)
From the previously described filter, the follower has an estimate of the leader’s total velocity, course
66
angle and course angle rate. From this, we can use this estimate as the leader’s velocity vector,
pL = ˆpL = R(χL)
VL0
(6.23)
The assumption that v = 0 has also been discussed before and can be applied. Similarly, the assump-
tion that the velocity of the current is invariant, V c = 0 is also used. This yields the model
x =d
dt
p− pLV c
=
R(ψ)
u0
+ Vc − pL
0
(6.24)
y =
yUSBLyDV L
=
p− pLp
(6.25)
where ψ, u and pL are inputs. Note that this formulation assumes that the vehicle carries a DVL, capa-
ble of measuring p. This is not generally the case with the MEDUSAs, but the system is observable even if
no DVL measurements are available. If we now define a measurement y′DV L = yDV L−R(ψ)[u, 0]T = Vc,
we obtain an alternative formulation that makes the outputs coincide with the states:
y′ =
p− pLV c
= x (6.26)
Again, a discrete-time model was obtained using the finite-difference approximation. Figure 6.2
illustrates the structure of the estimation and control blocks running in both underwater followers of the
upper segment (GCV and LSV).
Remark 6. Although u is an input to the filter, the MEDUSAs currently have no onboard sensor than
can measure the true surge speed. Instead, a static equation is used to determine an approximate value
of u from the thruster RPMs. This implies that the acceleration/deceleration dynamics of the vehicle
along xB are ignored, and has some practical consequences that will be exposed in the next chapter.
6.4 Summary
In this chapter we described how the strategies proposed in earlier chapters were adapted to allow
implementation in real vehicles. The two main issues discussed were 1) the modification of the trajectory-
tracking controller to be compatible with the existing inner-loop in the MEDUSAs, and 2) the introduction
of discrete-time Kalman filters.
67
AUV+
Inner loop
TargetVelocityEstimator
RelativePositionEstimator
ψd(t)
Yaw command
ud(t)
surge speedcommand
vc(t)
‖ ˙pasv(t)‖,χasv(t)
pr(t)
˙χasv(t)
FormationController
‖ ˙pasv(tk−τd)‖
χasv(tk−τd)
˙χasv(tk−τd)
ψauv(t)
Yaw
pr(tk − τp)USBL
uasv(t)
Surge speed
pauv(t)
DVL
AUV
Acoustic
Comms
Figure 6.2: Estimation and control in underwater followers of the upper segment (drawn in collaborationwith Mohammadreza Bayat).
68
Chapter 7
Results
In this chapter, results of both computer simulations and real trials are presented. The simulation
results are obtained with the continuous-time controllers and estimators from Chapters 4 and 5, using the
model of the MEDUSA vehicles derived in Chapter 3. In the real trials, the proposed solutions for the
follower vehicles in the upper segment of MORPH are validated, with the controllers and filters shown in
Chapter 6; the leader vehicle uses a path-following controller based on a preplanned mission. The lower
segment strategies were not implemented yet. The missions used for illustrating the performance of the
controllers consist of lawnmower and U-turn geometries with different turn radii and nominal speeds.
The structure of the chapter in presenting results follows the order in which the content was introduced
throughout the thesis. The missions presented are, in this order:
• One computer-simulated 1-vehicle trajectory-tracking lawnmower mission, with a nominal speed of
0.5 m/s and turn radius of 10 m and no currents.
• One computer-simulated 3-vehicle MORPH upper segment lawnmower mission, with a nominal
speed of 0.5 m/s and turn radius of 10 m for the leader vehicle, with a current velocity of 0.5 m/s
in the North direction.
• One computer-simulated 5-vehicle MORPH upper and lower segment lawnmower mission, at nom-
inal speed of 0.5 m/s, with a turn radius of 10 m for the leader vehicle, with a current velocity of
0.5 m/s in the North direction.
• One upper segment U mission executed at sea with three MEDUSA vehicles, in Lisbon, at a nominal
speed of 0.3 m/s with a turn radius of 20 m.
• One two-vehicle U mission executed at sea with one MEDUSA vehicle as leader and an Atlas
Elektronik SeaCat vehicle as follower, in Lisbon, at a nominal speed of 0.3 m/s with a turn radius
of 20 m.
• One five-vehicle loop mission executed at sea, with three MEDUSAs playing the roles of the upper
segment vehicles, executed in the MORPH Azores trials.
69
7.1 Trajectory-Tracking
A trajectory-tracking mission consisting of a lawnmower-like trajectory is simulated. The nominal
speed is 0.5 m/s and the turn radius is 10 m. These results show the performance of the trajectory-
tracking controller analysed in Section 4.3. The trajectory of the vehicle, the tracking error in {B}, the
speeds with respect to the water in {B} (surge u and sway v) and the yaw rate r are shown in Figure
7.1, along with the ud and rd commands produced by the outer-loop.
The controller parameters are δ = −0.02 m, kx = 0.5, ky = 0.5, n = 5, ku = 10, kr = 10. The mission
starts near the top left corner of the trajectory plots.
−20 0 20 40 60
−50
−40
−30
−20
−10
0
10
Easting (m)
Northing(m
)
Desired TrajectoryTrajectory
(a) Trajectories
0 100 200 300 400−10
−5
0
5
10
15
Time (s)
Trackingerror(m
)
exeyet
(b) Tracking error in {B}
0 100 200 300 400−0.1
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
Time (s)
Speed(m
/s)
uudv
(c) Surge and sway speeds
0 100 200 300 400−15
−10
−5
0
5
10
15
Time (s)
Yaw
rate
(◦/s)
rrd
(d) Yaw rate
Figure 7.1: Results on simulated trajectory-tracking.
As expected, during turns the tracking error is not zero. This is because the vehicle has some sway
speed, which cannot be measured and is not counteracted by the controller. Consequently, some error
is also introduced in the tracking of the commands ud and rd by the inner-loop. However, as predicted
by the ISS result proven in Chapter 4, both the tracking error and the inner-loop error remain in a
neighborhood of the origin.
70
7.2 Upper Segment Formation Control with currents
These results concern a simulated formation mission including the vehicles in the MORPH upper
segment: one leader and two followers. The leader tracks the same trajectory as in the previous mission.
The followers use the trajectory-tracking controller with commands fed from the upper segment path-
generator described in Section 5.1.2, and from the upper segment current estimator described in Section
5.1.3. A current with a velocity of 0.5 m/s in the North direction is introduced, and unknown to the
follower vehicles in the start of the mission. Since the goal is to evaluate the performance of the formation
controller (running in the follower vehicles), the trajectory of the leader is not affected by current and no
estimator is simulated. This is also useful to assess the influence of the current estimator in the tracking
error.
The controller parameters are the same as in the previous mission. The observer gain matrix Ko is
designed such that the four eigenvalues of A −KoC correspond to poles with a damping factor ξ = 0.7
and frequencies ω = 0.7 rad/s and ω = 1.4 rad/s.
−20 0 20 40 60
−50
−40
−30
−20
−10
0
10
Easting (m)
Northing(m
)
LeaderFollower 1Follower 2
(a) Trajectories
0 100 200 300 400−2
0
2
4
6
8
10
12
Time (s)
Trackingerror(m
)
LeaderFollower 1Follower 2
(b) Norm of tracking errors
0 100 200 300 400−0.1
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
Time (s)
Currentestimates
(m/s)
Follower 1, NorthFollower 1, EastFollower 2, NorthFollower 2, East
(c) Current velocity estimates
0 100 200 300 40080
100
120
140
160
180
200
220
240
Time (s)
Heading(◦)
Follower 1Follower 2
(d) Heading
Figure 7.2: Results on simulated upper segment formation control with currents.
71
The formation parameters are δx = 10, δy = 2 for follower 1 (10 m behind and 2 m to the left) and
δx = 10, δy = −2 for follower 2 (10 m behind and 2 m to the left). The mission starts near the top left
corner of the trajectory plots.
The current estimates converge to the correct values in straight segments. During turns, due to the
perturbation introduced by sway, the estimate deviates from the correct value but the estimation error
remains bounded. Note that while the controller parameters are the same for all three vehicles, the
tracking error is smaller for the follower vehicles, running the current estimator, than for the leader.
This is because, during turns, the estimate of the current also includes a term due to the sway expressed
in {I}. The sway is then partly counteracted by the controller. Also note that the estimate does not
converge to a constant value during turns because the sway, expressed in {I}, varies as the vehicle turns.
As predicted in previous chapters, the tracking error converges to zero during straight segments and
remains bounded during turns.
The controller action to cancel the current is visible by looking at the heading of the vehicle. This
is most easily seen along the three straight lines, where we would expect headings of 90◦, 270◦ and
90◦ respectively if no current was present. The actual values are 135◦, 225◦ and 135◦: as the current is
pushing the vehicle North at a speed equal to the nominal speed, the heading is adjusted by the controller
by 45◦ in the South direction.
7.3 Upper and Lower Segment Formation Control
These results concern a lawnmower mission including five vehicles: three in the upper segment and
two in the lower segment. The nominal speed and turn radius for the leader vehicle are 0.5 m/s and 10
m. The formation geometry and topology are clarified in Figure 7.3. The controller parameters are the
same as in the previous missions. The formation parameters are δ = (10, 0) for vehicle 2, δ = (20, 0) for
vehicle 3, Mpd = (0,−2) for vehicle 4 and Mpd = (0, 2) for vehicle 5.
For simplicity, the plots do not show the trajectory of the leader. Similarly, tracking errors are shown
only for the lower segment vehicles. Finally, since the estimators on lower segment vehicles estimate
pd − Vc whereas the upper segment ones estimate only Vc, they are not directly comparable - only the
estimates for lower segment vehicles are shown.
10 m 10 m5 m
5 m123
4
5
2 3
1
4 5
Figure 7.3: Nominal formation geometry (left) and leader-follower topology (right) for this simulation.Normal and dashed arrows denote following using relative position measurements or range-only measure-ments, respectively.
72
−20 0 20 40 60−50
−40
−30
−20
−10
0
10
Easting (m)
Northing(m
)
Vehicle 2Vehicle 3Vehicle 4Vehicle 5
(a) Trajectories
0 100 200 300 400−1.5
−1
−0.5
0
0.5
1
1.5
Time (s)
Estim
ates
(m/s)
Vehicle 4, NorthVehicle 4, EastVehicle 5, NorthVehicle 5, East
(b) Estimate: target velocity minus current pd−V c
0 100 200 300 400−2
0
2
4
6
8
10
12
14
16
18
Time (s)
Trackingerror(m
)
Vehicle 4Vehicle 5
(c) Norm of tracking errors
0 100 200 300 400−0.5
0
0.5
1
Time (s)
Trackingerror(m
)
Vehicle 4Vehicle 5
(d) Magnified norm of tracking errors
Figure 7.4: Results on simulated upper and lower segment formation control with currents.
The estimates of pd−Vc converge to the true values during straight segments; when turning, the sway
and the acceleration of the virtual target perturb the estimates. Note that the tracking error is slightly
bigger than for the upper segment vehicles (shown in the previous mission), as the lower segment vehicles
have no information regarding the velocity of the virtual target and need to estimate it. Also, note that
the amplitude of the initial transients in the estimate is visibly larger than for the upper segment vehicles.
Finally, as was the case with the previous mission, the tracking error converges to zero during straight
segments and remains bounded during turns.
7.4 Real trial results: MEDUSAs in Lisbon
These results concern a mission executed with three MEDUSA vehicles. The MEDUSA black, yellow
and red vehicles played the roles of SSV, GCV and LSV respectively, simulating the upper segment of
MORPH. The mission was executed at Marina dos Olivais, Lisbon, on 19 June 2014. No considerable
currents were present.
The formation configuration was δx = 17 m, δy = 5 m for the GCV (17 m behind the leader, 5 m to
73
the right) and δx = 2 m, δy = 5 m for the LSV (2 m behind the leader, 5 m to the right). The controllers
and estimators implemented were the ones described in Chapter 6. The GCV carried a DVL. Data on
controller errors is not shown because the logs were unavailable.
−120 −100 −80 −60 −40 −20 0 20−60
−40
−20
0
20
40
Easting (m)
Northing(m
)
SSV (GPS)GCV (USBL)GCV (estimated)LSV (USBL)LSV (estimated)
(a) Trajectories0 200 400 600
−0.2
−0.15
−0.1
−0.05
0
0.05
0.1
0.15
0.2
Time (s)Currentestimates
(m/s)
GCV, EastGCV, NorthLSV, EastLSV, North
(b) Current estimates
Figure 7.5: Results on trials with MEDUSAs in Lisbon.
Without controller errors, we cannot easily assess the performance of the controller. However, the
trajectories described by the follower vehicles seem to comply with the formation objectives. Because
the strategy proposed in Section 5.1.2 to avoid discontinuities in the generated path had not yet been
implemented, a deviation is visible in the trajectory of the GCV, before the turn.
We can also see that, compared to the LSV, the GCV has a significantly better estimate of the current,
more consistent and closer to zero (the currents were visibly very small at the site). The reason is that
the GCV was equipped with DVL during these trials, so its Kalman filter that estimates the current had
an additional measurement. The current estimate did not converge to a constant value, even on straight
segments, because 1) the filter input u is an approximation of the real surge speed, and 2) the filter input
pL is an estimate of the actual velocity of the leader, so it may be affected by some error.
7.5 Real trial results: SeaCat in Lisbon
These results concern a mission executed with one MEDUSA vehicle, playing the role of SSV, and
an Atlas Elektronik SeaCat vehicle playing the role of GCV, running the formation controllers and
estimators. The mission was executed at Marina dos Olivais, Lisbon, on 6 June 2014. No considerable
currents were present.
The formation configuration was δx = 17 m, δy = 5 m for the GCV (17 m behind the leader, 5 m to
the right) and δx = 2 m, δy = 5 m for the LSV (2 m behind the leader, 5 m to the right). The controllers
and estimators implemented were the ones described in Chapter 6.
The deviations when entering and exiting the turn are again due to the fact that the strategy to avoid
path discontinuities was not implemented at the time. Other than that, the trajectories seem to comply
with the formation objectives.
74
−120 −100 −80 −60 −40 −20 0 20−60
−40
−20
0
20
40
Easting (m)
Northing(m
)
SSV (GPS)GCV (USBL)GCV (estimated)
(a) Trajectories
0 100 200 300 400 500 600−8
−6
−4
−2
0
2
4
6
Time (s)
Trackingerrors
(m)
GCV along xB
GCV along yB
(b) Tracking errors
0 100 200 300 400 500 600−0.2
−0.15
−0.1
−0.05
0
0.05
0.1
0.15
0.2
Time (s)
Currentestimates(m
/s)
GCV, EastGCV, North
(c) Current estimates
Figure 7.6: Results on trials with Atlas Elektronik SeaCat in Lisbon.
We now analyse the controller error. The two peaks at roughly t = 250 s and t = 450 s are also due to
path discontinuities when entering and exiting the turn. The main difference from the simulated results,
however, is that the controller error does not converge exactly to zero, even on straight segments. This
is because the true controller error is only available when the followers know their exact position relative
to the leader, which happens once every 3 seconds. In between samples, the computed controller error
is based on the estimates provided by the Kalman filters, which are naturally affected by some error. In
general, however, the position error is under 1 m.
7.6 Real trial results: MEDUSAs in the Azores
These results concern a mission performed at the MORPH trials in Horta, Azores, on 18 September
2014. Five vehicles were involved; three MEDUSAs played the role of MORPH upper segment vehicles,
running the controllers and estimators as described in Chapter 6.
The planned trajectory for the leader (SSV) is shown in Figure 7.7, consisting of several loops offset
by 2 m. No considerable underwater currents were noticeable, but some currents and wind were present
75
at the surface. This visibly affected the trajectory of the SSV, when compared to its performance in the
Lisbon trials.
For simplicity of presentation, we only show the trajectories of the upper segment vehicles, in only
one of the loops.
−60 −40 −20 0 20−20
−10
0
10
20
30
40
50
Easting (m)
Northing(m
)
Figure 7.7: Trajectory of the SSV for the complete mission.
−60 −50 −40 −30 −20 −10 0 10−40
−30
−20
−10
0
10
20
Easting (m)
Northing(m
)
SSV (GPS)GCV (USBL)GCV (estimated)LSV (USBL)LSV (estimated)
(a) Trajectories
0 100 200 300 400−8
−6
−4
−2
0
2
4
6
8
Time (s)
Trackingerrors
(m)
GCV along xB
GCV along yBLSV along xB
LSV along yB
(b) Tracking errors
Figure 7.8: Results on trials with MEDUSAs in the Azores.
In the trajectories shown, we can observe deviations when entering and exiting turns, as happened
in the previous missions. The position error is again around 1 m along xB , but generally very close to
zero along yB for both vehicles. A possible explanation for the small error along yB , when compared to
the previous mission, is that the MEDUSAs have better maneuverability than the SeaCat. As for the
consistent error on xB , it is again due to the fact that in between samples the position is estimated with
a Kalman filter, as was explained in the previous mission.
However, with the MEDUSAs this problem is aggravated. Because the filter input u is an approxi-
76
mation of the real surge speed based on the thruster RPMs, as mentioned in Chapter 6, the vehicle is
considered to accelerate/decelerate instantaneously. When the controller acts to reduce a position error,
the RPMs vary almost immediately, even though the acceleration or deceleration is not instantaneous.
The filter then produces optimistic estimates of the position error along xB , producing the sawtooth-like
shape seen in Figure 7.8b.
Figure 7.9 shows two formation missions at the MORPH trials in the Azores. The formation geometries
and topology are similar to those shown in Figure 7.3. The results shown in this section concern the
mission seen in the underwater picture. The pictures include three MEDUSA vehicles from IST, two
vehicles from Universitat de Girona (Sparus and G500), and SeaCat from Atlas Elektronik.
(a) Underwater view.
(b) Aerial view.
Figure 7.9: 5-vehicle formation missions in the Azores.
77
7.7 Summary
This chapter presented both simulation and sea trial results. Simulations illustrate the performance
of the controllers, estimators and path-generators proposed in Chapters 4 and 5. Sea trials demonstrated
the performance of the modified controllers and estimators presented in Chapter 6. The results were
commented and the limitations of the systems were identified.
78
Chapter 8
Conclusion and Future Work
8.1 Conclusion
This thesis focused on the problem of formation control of autonomous underwater vehicles, motivated
by multiple application scenarios. In particular, the main motivation to tackle this problem was the
MORPH project, aiming to integrate in a formation multiple heterogeneous AUVs with complementary
capabilities.
The problem of formation control was studied under the assumption of fixed depth, i.e. concerning
only motion in an horizontal plane. Only one leader vehicle was assumed to have a-priori knowledge of
the mission to be executed. The problem of controlling the followers so as to move in a formation was
first tackled in a theoretical framework, based on tools from nonlinear systems theory. It was divided
into three main tasks: driving a follower vehicle to a virtual target (motion control), generating the
virtual target according to the formation objectives (path-generation), and estimating currents or virtual
target velocities (estimation). The path-generation and estimation tasks were analysed for two different
scenarios: formation control with relative position measurements, and with range-only measurements.
The controllers designed were based on a model structure for underactuated autonomous underwater
vehicles, and hence are valid for a wide range of AUVs. Stability and boundedness results were obtained
from this analysis.
These results were applied to the specific scenario of the MORPH project, including upper segment
vehicles with inter-vehicle relative position measurements and lower segment vehicles with range-only
measurements. An information flow strategy was proposed, under which the controllers analysed previ-
ously could be applied under the restrictions imposed by the MORPH project.
The controllers and estimators designed for the upper segment of MORPH were then implemented
on three MEDUSA vehicles and other vehicles from MORPH project partners, with some adaptations to
take into account existing inner-loop controllers. Real trials were conducted in Lisbon, Girona and the
Azores, as part of the MORPH project, demonstrating the performance of the estimators and controllers
in sea trials.
79
8.2 Future work
Many results obtained in this thesis are preliminary and point to possible directions of research. We
distinguish some subjects that warrant further investigation.
General-topology formation control of underwater vehicles. This thesis addressed only leader-
follower formations. Shortcomings of this approach, mainly concerning over-reliance on a single vehicle,
are well-known and exposed in the literature. In that sense, using other formation topologies would
be beneficial. The problem is well studied for formations of linear systems. Applications to non-linear
systems, on the other hand, are usually ad-hoc and no general tools exist for stabilizing such formations in
a decentralized fashion. New results on this front could have significant impact, especially if the practical
limitations inherent to underwater communication and navigation systems were taken into account.
Cooperation in challenging scenarios. In the scope of the MORPH project, one of the initial goals
was that the system could perform better than a single vehicle on challenging environments where sensor
measurements are considerably affected, through inter-vehicle cooperation. During the next months, the
research work will be focused on controlling the formation on a fully three-dimensional space, possibly by
including formation operators such as tilt and scale. If successful, this would have considerable impact
by paving the way for AUVs to be used in mapping and surveying of environments such as negative-slope
walls. These environments are particularly exciting from a scientific standpoint.
Cooperative decision-making. Current work with formations of AUVs focuses mainly on motion
control. Limitations on acoustic communication systems preclude higher-level cooperation from being
employed. With further development of acoustic communications and the recent introduction of optical
systems, which support much higher bandwidth, the cooperation may shift from the motion level to the
level of autonomous decision-making in a not too distant future. This would significantly increase the
autonomy of the formation, allowing missions to be carried out without human supervision.
80
Bibliography
[1] H. Leitão and W. Alvarez, “The Portuguese and Spanish voyages of discovery and the early history
of geology,” Geological Society of America Bulletin, vol. 123, no. 7-8, pp. 1219–1233, 2011.
[2] T. Fossen, Marine control systems: guidance, navigation and control of ships, rigs and underwater
vehicles. Trondheim, Norway: Marine Cybernetics, 2002.
[3] S. Bennett, “The past of PID controllers,” Annual Reviews in Control, vol. 25, pp. 43–53, 2001.
[4] G. Antonelli, T. Fossen, and D. Yoerger, “Underwater robotics,” in Springer Handbook of Robotics,
B. Siciliano and O. Khatib, Eds. Springer Berlin Heidelberg, 2008, pp. 987–1008.
[5] P. R. Wurman, R. D’Andrea, and M. Mountz, “Coordinating hundreds of cooperative, autonomous
vehicles in warehouses,” in Proceedings of the 19th National Conference on Innovative Applications
of Artificial Intelligence - Volume 2, ser. IAAI’07. AAAI Press, 2007, pp. 1752–1759.
[6] A. Ryan, M. Zennaro, A. Howell, R. Sengupta, and J. Hedrick, “An overview of emerging results in
cooperative UAV control,” in Decision and Control, 2004. CDC. 43rd IEEE Conference on, vol. 1,
Dec 2004, pp. 602–607 Vol.1.
[7] I. Kaminer, O. Yakimenko, V. Dobrokhodov, A. Pascoal, N. Hovakimyan, C. Cao, A. Young, and
V. Patel, “Coordinated path following for time-critical missions of multiple UAVs via L1 adaptive
output feedback controllers,” in AIAA Guidance, Navigation and Control Conference and Exhibit,
2007.
[8] A. P. Aguiar, I. Kaminer, R. Ghabcheloo, A. M. Pascoal, E. Xargay, N. Hovakimyan, C. Cao, and
V. Dobrokhodov, “Coordinated path following of multiple UAVs for time-critical missions in the
presence of time-varying communication topologies,” 2008.
[9] R. W. Beard, J. Lawton, F. Y. Hadaegh et al., “A coordination architecture for spacecraft formation
control,” IEEE Transactions on control systems technology, vol. 9, no. 6, pp. 777–790, 2001.
[10] N. Carlési, F. Michel, B. Jouvencel, and J. Ferber, “Generic architecture for multi-AUV cooperation
based on a multi-agent reactive organizational approach,” in Intelligent Robots and Systems (IROS),
2011 IEEE/RSJ International Conference on. IEEE, 2011, pp. 5041–5047.
[11] A. Pascoal, P. Oliveira, C. Silvestre, L. Sebastião, M. Rufino, V. Barroso, J. Gomes, G. Ayela,
P. Coince, M. Cardew et al., “Robotic ocean vehicles for marine science applications: the european
81
ASIMOV project,” in Oceans 2000 MTS/IEEE Conference and Exhibition, vol. 1. IEEE, 2000, pp.
409–415.
[12] R. Skjetne, I.-A. F. Ihle, and T. I. Fossen, “Formation control by synchronizing multiple maneuvering
systems,” Proceedings IFAC MCMC, pp. 280–285, 2003.
[13] A. Aguiar, J. Almeida, M. Bayat, B. Cardeira, R. Cunha, A. Hausler, P. Maurya, A. Oliveira,
A. Pascoal, A. Pereira et al., “Cooperative autonomous marine vehicle motion control in the scope of
the EU GREX project: theory and practice,” in OCEANS 2009-EUROPE. IEEE, 2009, pp. 1–10.
[14] J. Kalwa, M. Carreiro-Silva, F. Tempera, J. Fontes, R. Santos, M.-C. Fabri, L. Brignone, P. Ridao,
A. Birk, T. Glotzbach, M. Caccia, J. Alves, and A. Pascoal, “The MORPH concept and its application
in marine research,” in OCEANS - Bergen, 2013 MTS/IEEE, June 2013, pp. 1–8.
[15] A. Alvarez, A. Caffaz, A. Caiti, G. Casalino, L. Gualdesi, A. Turetta, and R. Viviani, “Folaga: A low-
cost autonomous underwater vehicle combining glider and AUV capabilities,” Ocean Engineering,
vol. 36, no. 1, pp. 24–38, 2009.
[16] M. M. Noh, M. R. Arshad, and R. M. Mokhtar, “Depth and pitch control of USM underwater glider:
performance comparison PID vs. LQR,” Indian Journal of Geo-Marine Sciences, vol. 40, no. 2, pp.
200–206, 2011.
[17] D. R. Yoerger and J.-J. Slotine, “Robust trajectory control of underwater vehicles,” Oceanic Engi-
neering, IEEE Journal of, vol. 10, no. 4, pp. 462–470, 1985.
[18] T. I. Fossen, “Nonlinear modelling and control of underwater vehicles,” Ph.D. dissertation, Norwegian
University of Science and Technology, Department of Engineering Cybernetics, 1991.
[19] ——, Guidance and control of ocean vehicles. Wiley New York, 1994, vol. 199, no. 4.
[20] A. Aguiar and J. Hespanha, “Trajectory-tracking and path-following of underactuated autonomous
vehicles with parametric modeling uncertainty,” IEEE Transactions on Automatic Control, vol. 52,
no. 8, pp. 1362–1379, 2007.
[21] J. P. Hespanha, D. Liberzon, and A. S. Morse, “Overcoming the limitations of adaptive control by
means of logic-based switching,” Systems & Control Letters, vol. 49, no. 1, pp. 49–65, 2003.
[22] F. I. Vanni, “Coordinated Motion Control of Multiple Autonomous Underwater Vehicles,” Master’s
thesis, 2007.
[23] A. P. Aguiar, R. Ghabcheloo, A. Pascoal, C. Silvestre, J. Hespanha, and I. Kaminer, “Coordinated
path-following of multiple underactuated autonomous vehicles with bidirectional communication
constraints,” in Proc. International Symposium on Communications, Control and Signal Processing,
2006.
82
[24] R. Ghabcheloo, A. P. Aguiar, A. Pascoal, C. Silvestre, I. Kaminer, and J. Hespanha, “Coordinated
path-following control of multiple underactuated autonomous vehicles in the presence of communica-
tion failures,” in Decision and Control, 2006 45th IEEE Conference on. IEEE, 2006, pp. 4345–4350.
[25] E. Borhaug, A. Pavlov, R. Ghabcheloo, K. Pettersen, A. Pascoal, and C. Silvestre, “Formation
control of underactuated marine vehicles with communication constraints,” in Proceedings 7th IFAC
Conference on Manoeuvring and Control of Marine Craft, 2006.
[26] I. Kaminer, O. Yakimenko, A. Pascoal, and R. Ghabcheloo, “Path generation, path following and
coordinated control for timecritical missions of multiple UAVs,” in American Control Conference,
2006. IEEE, 2006, pp. 4906–4913.
[27] R. Ghabcheloo, A. P. Aguiar, A. Pascoal, C. Silvestre, I. Kaminer, and J. Hespanha, “Coordinated
path-following in the presence of communication losses and time delays,” SIAM Journal on Control
and Optimization, vol. 48, no. 1, pp. 234–265, 2009.
[28] Y. Q. Chen and Z. Wang, “Formation control: a review and a new consideration,” in Intelligent Robots
and Systems, 2005.(IROS 2005). 2005 IEEE/RSJ International Conference on. IEEE, 2005, pp.
3181–3186.
[29] N. E. Leonard and E. Fiorelli, “Virtual leaders, artificial potentials and coordinated control of
groups,” in Decision and Control, 2001. Proceedings of the 40th IEEE Conference on, vol. 3. IEEE,
2001, pp. 2968–2973.
[30] R. Olfati-Saber and R. M. Murray, “Distributed cooperative control of multiple vehicle formations
using structural potential functions,” in IFAC World Congress, 2002, pp. 346–352.
[31] J. P. Desai, J. Ostrowski, and V. Kumar, “Controlling formations of multiple mobile robots,” in
Robotics and Automation, 1998. Proceedings. 1998 IEEE International Conference on, vol. 4. IEEE,
1998, pp. 2864–2869.
[32] W. B. Dunbar and R. M. Murray, “Model predictive control of coordinated multi-vehicle formations,”
in Decision and Control, 2002, Proceedings of the 41st IEEE Conference on, vol. 4. IEEE, 2002,
pp. 4631–4636.
[33] J. M. Soares, A. P. Aguiar, A. M. Pascoal, and A. Martinoli, “Joint ASV/AUV range-based forma-
tion control: Theory and experimental results,” in Robotics and Automation (ICRA), 2013 IEEE
International Conference on. IEEE, 2013, pp. 5579–5585.
[34] J. M. Soares, A. P. Aguiar, A. M. Pascoal, and M. Gallieri, “Triangular formation control using range
measurements: An application to marine robotic vehicles,” in 3rd IFAC Workshop on Navigation,
Guidance and Control of Underwater Vehicles (NGCUV 2012), no. EPFL-CONF-180991, 2012.
[35] J. M. Soares, A. P. Aguiar, A. M. Pascoal, and A. Martinoli, “Design and implementation of a range-
based formation controller for marine robots,” in ROBOT2013: First Iberian Robotics Conference.
Springer, 2014, pp. 55–67.
83
[36] F. Rego, J. M. Soares, A. Pascoal, A. P. Aguiar, and C. Jones, “Flexible triangular formation keeping
of marine robotic vehicles using range measurements,” 2014.
[37] H. Khalil, Nonlinear Systems. Prentice Hall PTR, 2002.
[38] E. D. Sontag, “Input to state stability: Basic concepts and results,” in Nonlinear and Optimal
Control Theory, ser. Lecture Notes in Mathematics, P. Nistri and G. Stefani, Eds. Springer Berlin
Heidelberg, 2008, vol. 1932, pp. 163–220.
[39] A. Teel and L. Praly, “Tools for semiglobal stabilization by partial state and output feedback,” SIAM
Journal on Control and Optimization, vol. 33, no. 5, pp. 1443–1488, 1995.
[40] J. I. Ribeiro, “Motion Control of Single and Multiple Autonomous Marine Vehicles,” Master’s thesis,
2011.
[41] The Society of Naval Architects and Marine Engineers (SNAME), “Nomenclature for treating the
motion of a submerged body through a fluid,” New York, Tech. Rep., 1950.
[42] A. P. Aguiar and A. M. Pascoal, “Dynamic positioning and way-point tracking of underactuated
AUVs in the presence of ocean currents,” International Journal of Control, vol. 80, no. 7, pp. 1092–
1108, 2007.
[43] S. Russell and P. Norvig, Artificial Intelligence: A Modern Approach, 3rd ed. Upper Saddle River,
NJ, USA: Prentice Hall Press, 2009.
[44] L. Consolini, F. Morbidi, D. Prattichizzo, and M. Tosques, “Leader-follower formation control of
nonholonomic mobile robots with input constraints,” Automatica, vol. 44, no. 5, pp. 1343–1349,
2008.
[45] M. Cao, C. Yu, and B. Anderson, “Formation control using range-only measurements,” Automatica,
vol. 47, no. 4, pp. 776–781, 2011.
84