Motion Control of Single and MultipleAutonomous Marine Vehicles

Jorge Ribeiro, Antonio Pedro Aguiar (Advisor),Antonio Pascoal (Co-advisor),

Instituto Superior Tecnico - Institute for Systems and Robotics(IST-ISR) Lisbon 1049-001, Portugal

Abstract: Worldwide, in the field of ocean exploration, there has been a surge of interestin the development of autonomous marine robotic vehicles equipped with advanced systemsto steer them accurately and reliably in the harsh marine environment and allow them tocollect data at the surface and underwater. Motivated by this fact, the first part of this paperproposes motion control algorithms (heading, speed, positioning, way-point and path-followingcontrollers) for single marine vehicles. For the path-following controller, the solution proposedis based on an outer loop control structure designed at the kinematic level responsible forcomputing the heading commands to make the vehicle move along a desired path, and an innerloop that regulates the actuators so that a given heading reference signal is tracked. Simulationand experimental results with the Medusa vehicles illustrate the proposed control systems.The second part of the paper is dedicated to the cooperative control of multiple marine vehicles.The cooperation of multiple vehicles offers several advantages and leads to safer, faster, and farmore efficient ways of exploring the ocean frontier, especially in hazardous conditions.The third part of the paper focuses on practical issues such as the implementation of theproposed algorithms in the vehicles, presents the software architecture adopted, and describessome graphical tools to supervise and manage the sea tests.

Keywords: Autonomous Marine Robotic Vehicles, Motion Control, Path following, CooperativeControl, Experimental tests.

1. INTRODUCTION

The ocean covers approximately 71% of the Earth’s surfaceand harbour extensive biological diversity. With 200.000marine species currently known, scientists estimate thatthe number of species yet to be found could probably betenfold [11]. The oceans are an enormous source of food,minerals, and energy waiting to be harvested. They alsoplay a key role in regulating the climate of the planet.For these reasons, there is considerable interest in theexploration and exploitation of the oceans to better un-derstand its dynamics and to promote the rational andsustainable exploitation of marine resources. This callsfor the development of advanced systems and methods tosample the ocean at an unprecedented scale, so as to geta synoptic view of the physical, chemical, geological, andbiological phenomena that occur in the water column andat the interfaces between the ocean and the atmosphere aswell as the seabed [3].In practice, this mandates the use of increasingly sophis-ticated autonomous marine robots capable of roaming theocean and accessing sites that have hitherto been out ofreach of humans or remotely operated vehicles. Even forsites that can be accessed with simple robots, the useof autonomous systems has the potential to substantiallydecreased the costs and increase the efficiency of scien-tific and commercial operations at sea. This change ofparadigm is steadily being brought about due to the avail-ability of advanced technology that includes embeddedcomputer systems, new communication systems, sensors,and actuators. Central to the development and operationof advanced systems for ocean exploration and exploitationis the availability of reliable methods for robot motioncontrol that can yield robust performance in the faceof stringent communication limitations, external distur-

bances, and reduced actuator control authority. Suffice itto remember that these robots are required to operate ina very hostile environment, with limited access to GPSsignals, strong attenuation of electromagnetic waves, andcomplex underwater propagation scenarios. There is alsoadded difficulty in controlling under-actuated robots (thatis, robots with a smaller number of control inputs than thenumber of degrees of freedom) in the presence of unpre-dictable currents. In order to overcome the difficulties thatarise during the operation of autonomous marine robots,there has been over the past decade a flurry of activityaimed at the development of new methods for robustsingle vehicle control. Identical efforts have been witnessedin a number of areas where under-actuated systems arecommonly used. Together, they address challenging prob-lems involved in the control of hovercraft, spacecraft, heli-copters, missiles, surface vessels, and underwater vehicles[2].Recently, the attention has been focused on the designand operation of groups of autonomous robots that mustcooperate to achieve a given task. The key concept is todistribute among a network of vehicles different sensors orcapabilities in order to reduce the time involved in carryingout a mission and increase its efficiency and reliability and,in some cases, to allow for the execution of tasks that areout of reach of a single vehicle. It is against this backdropof ideas that this paper addresses a number of challengingissues that have been defined in the scope of the ECCO3AUVs 1 project [2009-2012], which aims at the devel-opment, implementation, and testing of cognitive systemsfor coordination and cooperative control of multiple Au-tonomous Marine Vehicles (AMVs), in a scenario referred

1 Cognitive Cooperative Control for Autonomous Underwater Vehi-cles

to as Cooperative Control and Navigation of MultipleMarine Robots for Assisted Human Diving Operations.In this scenario, a number of autonomous surface craftare required to maintain a desired formation and guidean underwater target (a human or an underwater vehicle)along a desired path using acoustic communications. Thediver should follow the vehicles commands and at thesame time explore and examine seabed (check Figure 1).Another practical scenario requires that the vehicle fleetfollow a diver to track and log its position as the diverexplores the underwater world at leisure.

Fig. 1. Diver localization and guidance (Left Source: Artist- Dinsa Cunha Silva,2008; Right Source:[1]).

This paper describes the design and full testing at seaof a number of systems required for multiple vehiclecooperation in the face of external disturbances. As such,the paper bridges the gap between theory and practice.To this effect, the paper affords the reader a concisepresentation of the three main issues involved in the designand implementation of cooperative control systems.Namely,

• Vehicle Modelling - The objective is to obtain arealistic mathematical model for each of the vehiclesinvolved, by resorting to first physics principles andsimple identification methods. The models developedallow for extensive simulations as well as controllerparameter tuning in a hardware-in-the-loop simula-tion environment, prior to deployment at sea.

• Single Vehicle Motion Control - This task ad-dresses the problem of designing, for each of the ve-hicles involved, inner loops for yaw and speed controltaking into account the vehicle dynamics, actuatorconstraints, and sensor measurement noise. The per-formance of the resulting control laws must necessar-ily be assessed in simulation, prior to implementationin the real vehicles that are property of IST.

• Mutiple Vehicle Cooperative Motion Control- In this topic, the emphasis is placed on the de-velopment of control structures effectively enabling anumber of vehicles to jointly maneuver along desiredpaths while maintaining a specified geometric pattern(Cooperative Path Following). In the set-up adopted,cooperation among the vehicles is achieved by ex-changing data on the ”normalized length” of the pathtraversed by each vehicle and adjusting their speedsaccordingly so as to reach formation. Path followingof each vehicle is done at a single vehicle level, bygenerating appropriate yaw references. The resultingcontrol architecture displays an inner-outer controlloop structure, whereby speed and yaw commands areto be tracked by the inner loops developed for singlevehicle control.

• Implementation and tests at sea - This task in-volves the design and implementation of a software ar-chitecture for vehicle control, the sensor and actuatorinterfaces, a communications layer for inter-vehiclecommunications, a console for mission programmingand execution, software safety features, and the per-formance evaluation by carrying several tests at sea.

The paper is organized as follows. Section 2 introducesthe MEDUSA class of autonomous marine robots usedin the work and describes the dynamic model adoptedfor simulation and control systems design. Section 3describes the algorithms used for single motion vehicle con-trol. Section 4 is an exposition of the algorithm adoptedfor multiple vehicle cooperative motion control. Section 5describes the hardware and software architectures used forvehicle and mission control at sea. Section 6 summarizesthe results obtained and discusses topics that warrantfurther research and development effort.

2. AUTONOMOUS UNDERWATER VEHICLEMODEL

To derive the equations of motion for a marine vehicle it isstandard practice to define two coordinate frames; Earth-fixed inertial frame {U} composed by the orthonormalaxes {xU , yU , zU} and the body-fixed frame {B} composedby the axes {xB, yB, zB}, as indicated in Figure 2.The body axes are defined as follows

• xB is the longitudinal axis (directed from the sternto fore);

• yB is the transversal axis (directed from port tostarboard);

• zB is the normal axis (directed from top to bottom).

Fig. 2. Body-fixed and Inertial reference frames.

In general, six independent coordinates are necessary todetermine the evolution of the position and orientation (6degrees of freedom), three position coordinates (x,y,z ) andusing the Euler angles three orientation angles (φ,θ,ψ).The six motion components are defined as surge, sway,heave, roll, pitch, and yaw, and adopting the SNAME 2

notation we can introduce the following

• η1 = [x, y, z]T denotes the position of the origin of{B} expressed in {U}, North(x), East(y), Down(z);

• η2 = [φ, θ, ψ]T is orientation of {B} with respect to{U}, Roll(φ), Pitch(θ), Yaw(ψ);

• ν1 = [u, v, w]T the linear velocity of the origin of {B}relative to {U}, expressed in {B};

• ν2 = [p, q, r]T the angular velocity of {B} relative to{U}, expressed in {B};

2 The Society of Naval Architects & Marine Engineers -http://www.sname.org/

• τRB1 = [X,Y, Z]T actuating forces expressed in {B}and τRB2 = [K,M,N ]T the actuating moments ex-pressed in {B}.

Compacting this, η = [ηT1 , ηT2 ]

T , ν = [νT1 , νT2 ]

T and τRB =[τTRB1, τ

TRB2]

T .The motion control of {B} (that corresponds to the motionof the vehicle) is described relative to the inertial frame{U}. To simplify the model equations the origin of thebody-fixed frame is normally chosen to coincide with thecenter of mass of the vehicle.The vehicle in question will operate only in the horizontalplane. In that case, we only have three degrees of freedom[x,y,ψ] and the kinematics equations take the simple form

x = u cosψ − v sinψ,y = u sinψ + v cosψ,ψ = r.

(1)

For the dynamic equations and since the hydrodynamicforces and moments have simpler expressions if written inthe body frame because they are generated by the relativemotion between the body and the fluid, it is convenientto formulate Newton’s law in {B} frame. In that case, therigid-body equation can be expressed as

MRB ν + CRB(ν)ν = τRB , (2)where MRB is the rigid body inertia matrix, CRB rep-resents the Coriolis and centrifugal terms and τRB is ageneralized vector of external forces and moments and canbe decomposed as

τRB = τ + τA + τD + τR + τdist, (3)

where τ is the vector of forces and torques due tothrusters/surfaces [3], which usually can be viewed as thecontrol input, τA the force and moment vector due to thehydrodynamic added mass, τD the hydrodynamics termsdue to lift, drag, skin friction and others, τR restoringforces and torques due to gravity and fluid density, andτdist represents the external disturbances (waves, wind andothers). Replacing (3) on (2), the dynamic equations canbe written as

Mν + C(ν)ν +D(ν)ν + g(η) = τ, (4)where M includes the rigid body inertia matrix andadded mass terms, C(ν) the hydrodynamic Coriolis andcentripetal matrix and D(ν) the hydrodynamic dampingmatrix. Denoting Fsb and Fps as the starboard and port-side thruster forces respectively, and l the length of thearm with respect to the center of mass, we obtain

τu = Fps + Fsb,τr = l(Fps − Fsb),

(5)

where τu is the external force in surge (common mode),and τr the external torque about the z-axis (differentialmode between thrusters). In the horizontal plane, the sim-plified dynamic equations of motion of an underactuatedmarine vehicle can be expressed as

muu−mvvr + duu = τu,mvv +muur + dvv = 0,mr r −muvuv + drr = τr,

(6)

where

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

mv = m− Yv dv = −Yv − Y|v|v|v|

mr = Iz −Nr dr = −Nr −N|r|r|r|

muv = mu −mv

(7)

In (7), the symbols mu, mv, mr and muv are mass andhydrodynamic added mass, and the symbols du, dv, drrepresent hydrodynamic damping effects.

2.1 Vehicle Characterization - The Medusa

Fig. 3. Medusa dimensions.

The vehicles, named Medusa, are autonomous semi-submerged robotic vehicles developed at the Laboratoryof Robotics and Systems in Engineering and Science(LARSyS)/ISR of the Instituto Superior Tecnico of Lis-bon, Portugal.Each MEDUSA-class vehicle weights approximately 30Kgand consists of two acrylic housing tubes of size 0.15m by1.035m (diameter x length) with aluminum end caps, at-tached to a central aluminum frame. The distance betweenthe two bodies is 0.45m. The lower underwater tube con-tains two packs of 7-cell lithium polymer batteries togetherwith the thruster electronics, an underwater camera, anacoustic modem (Tritech) and other minor sensors. Theupper body, partially out of the water (see Figure 3 forthe vehicle dimensions with more detail), contains themain computer unit (Epic computer NANO PV D5251,with an Intel Atom D525 dual-core processor, low power,1.8GHz with 2GB RAM) together with navigation sensors(an attitude sensor - VECTORNAV VN-100) and a GlobalPositioning System (GPS - Ashtech MB100).Attached to the main frame there are two stern thrusters(SEABOTIX HPDC1507 Brushless thrusters), which con-trol directly the surge and yaw motions. At the nominalspeed of 1.0 m/s, the vehicles have an autonomy of 12hours. The maximum speed of the vehicles is 1.5 m/s. In-ner vehicle and vehicle/underwater target communicationsare enabled via Wi-Fi and an acoustic modem network,respectively.Roll and pitch does not need to be actuated because ofthe buoyancy, which guarantee that the vehicle has largeenough metacentric height. Since this vehicle is intended tobe a surface craft the heave motion is not being controlledas well.The final vehicle can be seen in Figure 4 with differentviews, as well as the three vehicles (red, black, and yellow)after going to water (Figure 5) in Sesimbra.

2.2 Thruster Model

With the objective of getting a realistic model of thethrusters, several experimental tests were conducted ina swimming pool in Tagus campus of Instituto SuperiorTecnico. From the test results we concluded that a goodapproximation is the model with the schematic presentedin Figure 6. The model includes a first order system witha pure delay characterized by

K0 = 7.2115 , Delay = 0.346s

a saturation and a rounding block, since the thrusters arecommanded by integer numbers between -100 and 100.

(a) (b) (c)

Fig. 4. The Medusa vehicle in different views (a) Frontview (b) Side view (c) Top view.

Fig. 5. Three Medusas in Sesimbra after a full day of trials.

100% −> 4500 rpm

F

1

rpm −> N

eta * abs(u)* u

Transport

Delay

Transfer Fcn

K0

s+K0

Sat [−100,100]Sat [−100,100] Gain

45round round

w RPM

1

Fig. 6. Thruster Model Diagram.

0 20 40 60 80 100 120 140−30

−20

−10

0

10

20

30

40

Time (s)

Thr

uste

r R

PM

(%

)

19 20 21 22 23 24 25 26

−30

−20

−10

0

Time (s)

Thr

uste

r R

PM

(%

)

21 21.5 22 22.5 23 23.5 24 24.5

−30

−25

−20

Time (s)

Thr

uste

r R

PM

(%

)

Real Thruster InputSimulated Thruster OutputReal Thruster Output

Fig. 7. Thruster characterization real data versus simu-lated data with different zoom levels.

The conversion between percentage to RPM (gain), andthe conversion from RPM to force was taken from [12].

From Figure 7 it is easy to see that the obtained model isvery close to the reality.

2.3 Vehicle Model Parameters

At this point we have a realistic thruster model but westill need to find the Medusa physical parameters forthe motion equations. To this end, the first step was to

apply a normalization using the Prime-System [7] of thenon-dimensional parameters obtained for the SIRENE 3

vehicle. The second step was to tune the values obtainedusing real data (See Figure 8 and Figure 9). At the end ofthis process we have arrived to the following values:

Xu = 25kg Yv = −2.325kg Nr = −8.690kg.m2

Xu = −0.2kg/s Yv = −55.117kg/s Nr = −4.14kg.m/sX|u|u = −19.5kg/m Y|v|v = −147.900kg/m N|r|r = −6.23kg.m

Figures 8 and 9 shows experimental results. Notice that

0 200 400 600 800 1000 1200 14000

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

Time (seconds)

Spe

ed (

m/s

)

Model performance (Speed)

Real SpeedSimulated Speed

Fig. 8. Surge velocity model analysis: simulated and ex-perimental results.

0 50 100 150 200 250−20

−15

−10

−5

0

5

10

15

20

Time (seconds)

Yaw

rat

e (d

egre

es/s

)

Model Performance (Yaw Rate)

Real Yaw_RateSimulated Yaw_Rate

Fig. 9. Yaw velocity model analysis: simulated and exper-imental results.

the real commands used for the real thrusters were used asinputs for the Simulink model. At this stage, in the modelthe external perturbations were not taken into accountsuch as currents, waves, sensor noise. These perturbationscan be seen in the Figure 8 were the vehicle did a straightline along the current (0-200 seconds) and another onetowards the current (200-450 seconds) with the same com-mon mode. Overall, from the figures it can be seen thatthe tuned model give similar results as obtained using thereal vehicle.

3 Autonomous Underwater Shuttle designed for the transportationof Benthic Laboratories, more information in [4]

3. SINGLE VEHICLE MOTION CONTROL

This section presents the motion control algorithms for sin-gle marine vehicles. The proposed algorithms include theheading controller, the speed controller, the way-point andhold positioning controller and path-following controller.For all of them, simulation results and experimental testswith the Medusa vehicles are described.

3.1 Heading Controller

The objective of the heading controller is to steer auto-matically the vehicle to a given desired direction (yaw).To this effect, the control algorithm accepts as inputs thereference signal ψd, the yaw ψ and yaw rate ψ that areaccessible by the vehicle’s IMU. Let

ψ = ψ − ψd (8)

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

ud = −kpψ − kdψ (9)where ud denotes the differential mode between thethrusters.In (8), since ψ ∈ [0, 2π] the error ψ has to be redefinedaccording to

if ψ > π (rightside)ψ = ψ − 2π

if ψ < −π (leftside)ψ = ψ + 2π

(10)

Figure 10 illustrates the performance of the heading con-troller in simulation as well as with real data using the RedMedusa vehicle. This experimental test was done with thecommon mode set to zero, that is, the vehicle was turningaround its center of rotation.

0 50 100 150 200 2500

50

100

150

200

250

300

350

Time (seconds)

Yaw

(de

gree

s)

Heading Controller

Real YawSimulated YawDesired Yaw

Fig. 10. Heading Controller Test: simulated and experi-mental results.

3.2 Speed Controller

The speed controller is responsible to keep the vehicle ata certain desired speed and be robust as much as possibleto external perturbations like ocean waves and currents.For estimating the vehicle’s surge velocity it was includedan estimator that receives the position given by the GPS(observer diagram in Figure 11), where K1 and K2 are

_______ Filter

_______ Output s

_______ Inputs

Vest

2

Pest

1

Step

Integrator

1

s

xo

Integrator

1

s

xo

Gain

K2

Gain

K1

Constant

0

P

1

Fig. 11. Implemented filter for position and velocity esti-mation.

positive gains. Having the estimates Vx and Vy we cannow easily derive the vehicle’s velocity expressed in bodyframe by rotating them, that is, u and v: u = sin(ψ)Vx +cos(ψ)Vy . Note that the vehicle movement due to currentsand other perturbations will be measured in the inertialframe and will take part in u and v. At this point we havethe ingredients to design the speed controller.Let

u = u− ud (11)be the error between the estimated speed u and the desiredspeed ud, and And

ξ =

∫ t

0

u(τ) dτ (12)

be the integration error. With (11) and (12) we define aPI(proportional-integral) controller for the common mode:

uc = −kpuu− kiuξ, kpu, kiu > 0. (13)

0 50 100 150 200 250 300 350 4000

0.1

0.2

0.3

0.4

0.5

0.6

0.7Speed Controller

Time (s)

Vel

ocity

(m

/s)

Simulated SpeedReference Speed

Fig. 12. Speed Controller simulation test.

Figure 12 shows simulations results using the PI controller.It can be seen that the vehicle needs about 50 secondsto reach the reference speed, witch is too slow. Thisis due to the fact the thruster model contains a time-delay. To compensate the time-delay and obtain a betterperformance we decided to implement a Smith Predictor[8], [9].

Smith PredictorThe main goal of the Smith Predictor algorithm is tocompensate the time-delay in the process by including aninner-loop that tries to predict the future vehicle’s statebased on the inputs. To implement the Smith predictorwe used a simple version of the model described in Section

2 for surge velocity, described by the system (in discretetime)

H(z) =0.0006641z

z2 − 0.6368z − 0.2936(14)

Then, it is delayed by a few samples and given as compen-sation of the error u, reducing the control oscillations [8].Figure 13 shows the implementation details of the speedcontroller using a Smith predictor. The simulation resultsfor the PI controller with and without compensation of thedelay is presented in Figure 14.

Error

Proportional Part

Integration Part

Smith Predictor

Common Mode

1

Surge Model

0.0006641z

z −0.6368z−0.29362

Gain

−Kiu

Gain

−Kpu

Delay

1/z

Delay

−7

Z

u

2

Uref

1

Fig. 13. Smith and Speed controller Diagram

Comparing the two it can be understood that with theSmith Predictor it is possible to increase the gains, withouthaving the problem of oscillations or too much overshoot.

3.3 Way-point/Hold Position Controller

Controlling a marine robotic vehicle in a real scenario isnot an easy task because the human operators need timeto plan the missions and in the meanwhile the vehiclecan drift due to waves and ocean currents. Motivated bythis fact, a point stabilization controller was developed tosteer the vehicle to a given point (xd,yd) and keeps it in aneighbourhood around the desired point.Inspired by the point stabilization algorithm describedin [5] we implemented the following strategy: while thevehicle is in a neighbourhood of radius ǫd of the desiredpoint set to zero the speed reference (ud = 0) and maintaina given orientation using the heading controller. Thisimplies that the vehicle is not wasting unnecessary batteryenergy with the thrusters; if the vehicle is outside theneighbourhood use the heading and speed controllers tobring it back. The switching between these two modes ofoperation is done applying a convenient hysteresis to avoidchattering. In second mode the references for the speed andheading controller are given by

ud = ku sin−1(

d|d|+ks

)

2

π,

ψd = atan2(yd − y, xd − x),d =

√

(x− xd)2 + (y − yd)2 − ǫd,(15)

ku > 0 defines the upper limit on the speed reference udand ks > 0 is the parameter for tuning the vehicle speedwith respect to distance error.Figure 15 shows an experimental result for one scenariowhere the vehicle starts at a distance of 65 meters andthe radius ǫd was set to ǫd = 1m. From the Figure 15 itcan be concluded that the vehicle converges to the desiredposition even with the presence of ocean currents or otherperturbations.

0 50 100 150 200 250 300 350 400−0.4

−0.2

0

0.2

0.4

0.6

0.8

1Speed Controller Increased Gains without Smith Predictor

Time (s)

Vel

ocity

(m

/s)

Simulated SpeedReference Speed

(a) Higher Gains

0 50 100 150 200 250 300 350 400 450

0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

Vel

ocity

(m

/s)

Time (s)

Speed Controller with Smith Predictor

Simulated SpeedReference Speed

(b) Same Gains with Smith Predictor

Fig. 14. Comparing the Smith Predictor performance

−40 −20 0

−20

−10

0

10

20

30

40

50

60

70

X (m)

Y

Position

0 50 100 150

−50

0

50

Differential Mode

Time (s)

RP

M (

%)

0 50 100 150

−50

0

50

Common Mode

Time (s)

RP

M (

%)

Vehicle PathBoundary

Fig. 15. Performance of the way-point Controller in realconditions.

3.4 Path-Following Controller

In ocean missions of interest, marine vehicles are requiredto converge and follow spatial paths accurately. Oncein the path, the vehicle should follow it with a desired

speed. In this case, no temporal constrains were imposed,contrasting with trajectory tracking controllers where thereference for the vehicle motion is time parametrized. Thismay lead to situations where the required speed with re-spect to the water may be to small (leading to an unstablecontrol) or to high (exceeding the capability of the vehicle)[10].In this work, the path following controller implementeduses the Heading and Speed controllers described in Sec-tion 3.1 and Section 3.2, respectively. The Path-followingalgorithm works in an outer-loop control design and itis responsible to issue the convenient heading and speedcommands based on the actual position and desired mis-sion. For simplicity, we consider that the mission is onlycomposed by straight lines and arcs. Let e, be the crosstrack error (the distance to the nearest point in the path)and β the angle between the tangent to the point in thepath and the vertical axis (see Figure 16). The desired

β

xI

xI

xB

yB

yI

Straight line to be

followed

cross track error e

Fig. 16. Path-following nomenclature for a straight line.

heading and speed commands are given by

ψd = β + sin−1(sat(ψc))ud = vL

(16)

where,

sat(ψc) =

{

ψc if|ψc| < 11 if|ψc| ≥ 1−1 if|ψc| ≤ −1

(17)

and

ψc = −

∫ t

0

(k1

vLe(τ) +

k2

vLe(τ) ) dτ (18)

In (18) the gains k1 > 0 and k2 > 0 are chosen as functionof the desired natural frequency and damping factor fora second order system and vL is the mission nominalvelocity. In [10], it is proved that the cross track errorconverges to zero assuming that the heading controller isfast enough. Figure 17 illustrates in simulation the path-following controller for a lawn mower path (a maneuvercommonly used to cover an area with one sensor for ex-ample) composed by straight lines and arcs.In Figure 18 the results of a similar lawn mower maneuveris displayed for a real scenario where one Medusa vehiclewas collecting bathymetric data in Parque das Nacoes.In Figure 17 is shown the behaviour of the path-followingcontroller in arcs and straight lines, distance error (e) con-verges to zero as well as Heading of the vehicle convergesto the desired one (ψd).

4. MULTIPLE VEHICLE MOTION CONTROL

This section starts to describe a very general architecturefor multiple vehicle cooperative motion control. Then, wepresent the cooperative path-following maneuver and illus-trate it through computer simulations and experimentaldata obtained in a series of tests carried out in SesimbraOctober 2011.

0 500 1000 1500−6

−4

−2

0

2

Dis

tanc

e er

ror

(m)

Time (s)

Cross Track Error (e)

0 500 1000 15000

100

200

300

Time (s)

Ang

le (

degr

ees)

Heading (ψd)

Heading ReferenceActual Heading

−200 −150 −100 −50 0 50 100−50

0

50

X (m)

Y (

m)

Path

Simulated PathMission path

Fig. 17. Path-following for a lawn mower maneuver.

0 20 40 60 80 100 120 140

−50

−40

−30

−20

−10

0

10

20

30

40

50

X (m)

Y (

m)

Real PathMission Path

Fig. 18. Path-following for a lawn mower maneuver in”Parque das Nacoes” dock.

4.1 General architecture for multiple vehicle cooperation

Figure 19 illustrates the proposed architecture for multiplevehicle cooperation that is composed by the followingblocks:

(1) Path-following controller (described in Section 3.4)a dynamical system whose inputs are a path Pdi, adesired speed profile that is common to all vehicles,and the vehicle’s output yi. Its outputs are thevehicles input vL, computed so as to make it followthe path at the assigned speed, an heading referenceψd, and a generalized path-variable γi.

(2) Coordination Controller (described in this Section) isthe dynamical system that enforces coordination withother vehicle, receiving has inputs the generalizedpath-variable γi, and the generalized coordinationstates γj of the n agents it communicates with. Itpasses on to the vehicle’s inner-loops the associatedspeed profile ul, coupled with the correction speedsignal vcorri which is used to synchronize vehicle iwith its neighbours.

(3) The path generator is, in this case, responsible forgenerating the desired path Pdi(γi) : R → R

n

parametrized by γi ∈ R, and feed it to the path-following controller.

Coordination

Controller

Vehicle+

Inner-Loop

Communications

Coordination

Controller

Path-Following

Controller

Path

Generator

data Pdi

Vcorri

VL

ud

d

Vehicle

Statex,y, ,

Other Vehicles

Statex,y,

Network Layer

Fig. 19. Vehicle general control architecture

4.2 Cooperative Path-Following

Cooperative path-following refers to the maneuver wherea fleet of marine vehicles is required to track a seriesof pre-defined paths, while holding a desired formationpattern at a desired formation speed. The implementationof the algorithm corresponds for each vehicle to execute apath-following control law together with a synchronizationscheme that changes the nominal speeds of the vehiclesso as to achieve the desired temporal synchronism. Inthis work, the path-following controller used was the onedescribed in Section 3.4.Let I := {1, ..., n} be a group of vehicles, each onewith its own parametrized path Pdi(γi) and a speedprofile vL that is common to all entities to guaranteecoordination. The parameter γ of each vehicle is seen as acoordination state, such that coordination exists betweenvehicles i and j if and only if γi = γj . The controllerdesigned for coordination yields a correction term, vcorr =

[vcorr1 , · · · , vcorrn ]T, that is added to the desired speed

of each vehicle, with the goal of driving the coordinationerror to zero. Thus, the desired velocity that enters theinner-loop is

vd = vL + vcorr (19)

A decentralized control strategy is designed where thecoordination speed vcorr of vehicle i is determined basedon the available measurements of the γi parameters, thatis, based on the coordination states of the vehicles that can

communicate with vehicle i. Let γ = [γ1, · · · , γn]Tbe the

vector containing the coordination states of each vehicleincluding the target to follow and Ni denote the set ofvehicles that vehicle i can communicate with.Defining the coordination error as

γi = γi −1

|Ni|

∑

j∈Ni

γj (20)

that is basically the error between the vehicle gamma withthe mean of the gammas from the other vehicles, a simplecontroller can be designed to lead γi to zero as

vcorri = −ksync sin−1

(

γi

|γi|+ ks

)

2

π(21)

In (21) ksync defines the upper limit in the speed correction(cannot be higher than the nominal path speed, becausethe vehicle could run backwards) and ks is the parameterto tune the reactivity to variations in gamma error.

4.3 Results

The algorithm for cooperative path following was imple-mented and tested with great success with three vehicles.Before testing this algorithm on the field, simulationswere performed, with different missions under differentconditions like currents. The vehicles followed the pathin each of them in very good conditions, Figures 20 and21 present a lawn mower and straight line manoeuvres inan real scenario at Sesimbra sea. It is observable in someparts of the mission, that the controller is compensatingthe ocean currents.

0 20 40 60 80 100 120

20

40

60

80

100

120

X (m)

Y (

m)

Vehicle 1

Vehicle 2

Vehicle 3

Formation

Fig. 20. Lawn-mower Mission with a formation in line(experimental results).

120 140 160 180 200 220 240 260

200

220

240

260

280

300

X (m)

Y (

m)

Vehicle 1

Vehicle 2

Vehicle 3

Formation

Fig. 21. Line Mission with a formation in line (experimen-tal results).

5. IMPLEMENTATION

This section describes the software architecture adopted toimplement the controllers developed in the real vehicles.The main idea for the middleware software was to make iteasily changed and configured to support new controllers,sensors or hardware changes. To this effect, was imposedthat each vehicle will run Matlab in real time.

5.1 Software Architecture

For control, navigation, and mission control systems im-plementation, each vehicle runs a standard Linux oper-ative system on a 2GHz Intel dual-core - 2 GB RAM

Write the last values

every 500ms

Write the last

values in Simulink

every 200ms

MOOSDB

THRUSTERS GPS IMU MODEM BATTERIES

MATLAB Server

k

Safety Feature Display

External PC

2 Hz

M

O

O

S

HTTP

Server

By Query

External PC Browser

Matlab Commands (RPMs)

Safety Feature

Sensor Data

Hardware Communication

5 Hz 10 Hz 0.2 Hz 1 Hz5 Hz

5 Hz

Co

mm

un

ica

tio

ns

Broadcast

UDP

5 Hz

Fig. 22. Communications between all the processes in-volved.

platform. The architecture adopted allows for seamlessimplementation and hardware-in-the-loop testing of solu-tions developed in the Matlab enviorment. For this effectindependent processes were developed for each sensor andactuator, to allow a communication interface with thehardware. A general block diagram of the inter-processcommunication architecture is shown in Figure 22. Themiddleware software infrastructure responsible for coor-dinating and communicating with the processes in theopen source MOOS 4 . The core of MOOS is a robustnetwork based communication architecture that simplifiessubstantially the effort of building an application andwhere all communications are made through a DataBasecalled MOOSDB. Whith this set-up, each process mustonly analyse the sensor data and post the important vari-ables on this MOOSDB. In MOOS software package thereare a huge variety of additional tools/applications to coversome basic tasks like visually debug a set of communicatingprocesses, logging all the necessary data and missioningcontrol. In Figure 22, the blocks marked as ellipses are themain processes implemented on the vehicle. The top partof the diagram shows the actuator and sensor processeswith their sampling frequencies. Bellow MOOSDB are thecontroller and the visualization parts. The role of someblocks is now briefly described.

• The THRUSTERS module handles the bidirectionalcommunication between the thrusters and DataBase.It constantly polls the status of both thrusters andposts the result on the database. Likewise it sendsto the thruster the speed commands as soon as theybecome available in the database (posted by theMATLAB Server).

• The IMU, GPS, MODEM and BATTERIES mod-ules, parse the data coming from the sensors and postit to the database.

• The Communications block is responsible for acquir-ing selected variables and broadcasting them via UDPto all the other vehicles. It also does the inverse roleof receiving the variables from the remaining vehiclesand posting them in the DataBase to be accessed bythe other processes (e.g. the MATLAB Server).

• The MATLAB Server block plays a central role inthe architecture because it broadcasts important dataand sets the clock of Matlab/Simulink (turning it

4 Mission Orientated Operating Suite developed by Oceanai MIT -http://www.moos-ivp.org

into a real clock instead of the normal virtual clock)and this has to be as precise as possible, because ofintegrations used in the different controllers describedbefore are considering a constant time step.

• The Safety Feature process communicates periodi-cally every 2 seconds through a Wi-Fi link with acomputer outside the vehicle and expects a replyfrom that computer. When that reply fails to arrivethe vehicle becomes aware that the link is brokenand stops the thrusters after a timeout. This safetyfeature is intended to stop the vehicle whenever thecommunication is lost thus it is impossible to senda command to stop the vehicle. This way it can beassured that the vehicle will not go inadvertently andwithout control against nearby obstacles, neither runashore.

• The Display and Http Server are responsible for ex-porting all the variables contained in the DataBase,periodically or by querying. As an example of the ca-pabilities offered by this server, subsection 5.3 showsthe browser console implemented.

• The MATLAB/SIMULINK block is a software pack-age developed by MathWorks for modelling, simu-lating and analysing dynamical systems. It has agraphical block diagramming tool and a customizableset of block libraries. MATLAB/SIMULINK can beused to implement all the algorithms for cooperativemotion control described before.

This structure is very helpful in development stages sinceit is possible to replace/add one block (e.g. Sensor Block)by a simulated one, or increasingly remove blocks fromMatlab and add new blocks without having to changethe others. For instance, if the Heading controller is fullytested and working, it may be easily attached as new blockas a standalone process.

5.2 Hardware in the loop Simulation

Hardware in the loop simulation is a process that combinesin the loop real time simulation, with real components(sensors and actuators) and simulated vehicle dynamics.With this technique it is possible to test every block ona friendly and controlled environment (e.g. Laboratory)in a fast and safe way. To achieve the above mentionedgoals it is desirable to keep as many original processesrunning in real conditions as possible, without performingany change. On that ground, GPS and IMU are simulatedby a laptop connected through serial port to the vehiclecomputer exactly at the same ports as GPS and IMUare. The vehicle runs the same applications except forthe thrusters module which is slightly modified: insteadof sending the speed commands to the hardware only, italso sends the RPMs through TCP-IP connection to anexternal laptop (shown in Figure 23). That laptop runs thevehicle model and simulates the real hardware messages,while in the vehicle it is running in the same way as in areal mission.s

5.3 Browser Console

One important aspect in a real mission at sea is to knowthe location of the vehicles at all time and obtain theirstatus from time to time. Typically examples include thestatus of the batteries and, if there is a failure in thethrusters (like a plastic bag rolled around the propellersor a temperature problem). The solution chosen was tohave a console implemented in HTML so that it is possibleto run it in any pc with a browser installed. Figure 24shows the console running in a browser (Google Chrome)

TCP-IP

RPMs

Fig. 23. Hardware in the loop Configuration

Fig. 24. Browser Console for a trial where the vehicle wereacquiring bathymetric data

on a laptop, when the vehicles were doing a cooperativepath following mission with a line formation in Parque dasNacoes, Portugal.

6. CONCLUSIONS

This paper addressed the problem of developing the soft-ware architecture and system motion control for single andmultiple robotic marine vehicles. To this end, a dynamicmodel that governs the motion of the vehicle Medusa wasproposed. Next we presented a series of controllers forguidance/navigation of a single vehicle to perform simpletasks like follow a specified orientation, with a desiredspeed or go to a point and stay there even in the presence ofcurrents. These controllers were fully tested in simulationand in open sea with good results.For the path-following and cooperation of multiple ve-hicles it was designed and implemented an inner-outerloop structure to be possible to separately test each blockindividually and obtain a simple way to tune the gains. Inthe paper we show the experimental results for three ve-hicles performing missions in Sesimbra’s sea with differentformation patterns.To implement the motion controllers proposed, a softwarearchitecture was designed taking into account it shouldbe easy to add new sensors and actuators with totallyindependence on the vehicle in question. The architecturecan be exported to a completely different vehicle withoutsignificant changes (only the processes responsible by in-terfacing with the hardware) turning it into a flexible androbust architecture. Some graphical tools to help managethe real sea tests were also developed.

6.1 Future work

The problems addressed while developing this work covera vast number of fields. Some of the results obtain are

preliminary and point out to possible avenues for futureresearch.

• Collision avoidance - To be possible to controlmultiple vehicles safely in ocean, it is important tohave a mechanism to avoid collisions between vehiclesand with obstacles in the vehicles path. Recent resultsin this direction can be found in [6].

• Dynamic formations - One future objective is toallow the vehicles to be capable of tracking a diver,following a path and maintain a formation patternthat is dependent of diver’s depth. In this case thevehicles need to adapt the formation to have betterreadings. This paper presented a cooperative path-following using a constant formation along the path.The next step is to extend it to dynamic formations.

REFERENCES

[1] Antonio P. Aguiar, Joao Botelho, Antonio M.Pascoal, Manuel Rufino, Jorge Ribeiro, and LuisSebastiao. Cooperative cognitive control for au-tonomous underwater vehicles - mission scenarios andplatforms (internal report). http://robotics.jacobs-university.de/projects/Co3-AUVs/index.htm.

[2] Antonio Pedro Aguiar and Joao P. Hespanha.Trajectory-tracking and path-following of underactu-ated autonomous vehicles with parametric modelinguncertainty. Automatic Control, IEEE Transactionson, 52(8):1362 –1379, aug. 2007.

[3] Antonio Pedro Aguiar and Antonio Pascoal. Coopera-tive control of multiple autonomous vehicles: missionsscenarios, theoretical foundations, and practical is-sues. http://users.isr.ist.utl.pt/ pedro/EECI/. Courseslides of EECI graduate school on control Supelec,France, 2010.

[4] Antonio Pedro Aguiar and Antonio Pascoal. Modelingand control of an autonomous underwater shuttlefor the transport of benthic laboratories. Proc ofOCEANS97, 1997.

[5] Antonio Pedro Aguiar and Antonio Pascoal. Dynamicpositioning and way-point tracking of underactuatedauvs in the presence of ocean currents. InternationalJournal of Control, 2007.

[6] Sergio Carvalhosa. Cooperative motion control of mul-tiple autonomous robotic vehicles. Master’s thesis, In-stituto Superior Tecnico, 2009.

[7] Thor I. Fossen. Guidance and control course, identi-fication and control of UUVs. Lectures notes. Norwe-gian University of Science and Technology - Dept. ofEngineering Cybernetics. Trondheim, Norway, 2010.

[8] Zdenek Hurak and Martin Rezac. Delay compensationin a dual-rate cascade visual servomechanism. 49thIEEE Conference on Decision and Control, 2010.

[9] K. L. Koay and G. Bugmann. Compensating inter-mittent delayed visual feedback in robot navigation.2004.

[10] Pramod Maurya, Antonio Pedro Aguiar, and AntonioPascoal. Marine vehicle path following using inner-outer loop control. Proc. of MCMC09 - 8th Conferenceon Manoeuvring and Control of Marine Craft, 2009.

[11] Camilo Mora, Derek P. Tittensor, Sina Adl, AlastairG. B. Simpson, and Boris Worm. How many speciesare there on earth and injo the ocean? PLoS Biology,2011.

[12] Roger Skjetne, Oyvind Smogeli, and Thor Fossen.A nonlinear ship manouvering model: Identificationand adaptive control experiments for a model ship.Modeling, Identification and Control, 2004.