Sensor-based Formation Control of Autonomous Underwater ...€¦ · were astounding: around a...

Sensor-based Formation Control of Autonomous

Underwater Vehicles

Pedro Caldeira Abreu

Thesis to obtain the Master of Science Degree in

Aerospace Engineering

Supervisor: Prof. António Manuel dos Santos Pascoal

Examination Committee

Chairperson: Prof. João Manuel Lage de Miranda Lemos

Supervisor: Prof. António Manuel dos Santos Pascoal

Members of the Committee: Prof. 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′


)− ν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′


)]= −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


+ AUV

Vehicle 2

Pathgenerator


+ 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 5 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

Sensor-based Formation Control of Autonomous Underwater ...€¦ · were astounding: around a...

Documents

Transcript of Sensor-based Formation Control of Autonomous Underwater ...€¦ · were astounding: around a...