1

Computer-Based Cooperative Motion Planning,Programming, and Control of Autonomous Robotic

VehiclesMiguel Ribeiro

Instituto Superior Técnico - Institute for Systems and Robotics (IST-ISR)Lisbon, Portugal

October 2013

Abstract—The human interest in the Sea is not new. However,human lives were always endangered due to the extreme marineenvironment, either at the surface or more recently underwater.The past decades electronics’ evolution has supported a promis-ing beginning of the development of autonomous marine roboticsperforming increasingly complex tasks. Nevertheless very robustsystems have to be done to deal with this harsh environment.One of the challenges is to steer accurately robots to a pathregardless of the external disturbances such as ocean currents.In that sense, the path following problem is addressed in thispaper.

A new path following controller is proposed with an inner-outer loop strategy. One feature of the outer loop is that instead ofusing a proportional integral controller on the cross track error, ittries to compensate ocean currents with an observer in the inertialframe. As for the inner loop, it takes into account the dynamicsof the vehicle and thrusters and its gains are computed resortingto the Linear-Quadratic Regulator. Results from simulations andreal tests done in the water are shown throughout this while andits performance is assessed.

The final part addresses software that was developed duringthis work, such as a migration of control related software, agraphical user interface to make it easier to program missionfor the vehicles and the integration of a tool to simulate multiplevehicles.

Index Terms—Autonomous Marine Vehicles; Control; Pathfollowing; Ocean Current estimation

I. INTRODUCTION

A. Background

In the early 1960s, the US President John F. Kennedy setthe goal of landing a man on the moon till the end of thatdecade. On 24th July 1969, Apollo 11 landed on the moonand Neil Armstrong was the first man to walk on the Earth’snatural satellite.

Despite the fact that the man have already gone to themoon, our planet is not fully explored and beneath the 71% ofEarth’s surface that is salt water there is still a lot awaiting tobe found. However, possibly due to cultural or subconsciousreasons the interest in the sea is far from the interest inspace. In fact, one-year budget of the National Aeronauticsand Space Administration (NASA) would finance 1600 yearsof its equivalent for the seas exploration, the National Oceanicand Atmospheric Administration (NOAA), [1].

The exploration of the sea has an extreme importance eitherto discover wrecks of airplanes or ancient ships, to extend ourknowledge on fauna and flora or to study with greater detailthe Plate tectonics. There are numerous examples of recentsea-related findings. In the beginning of September 2013, agiant volcano, named Tamu Massif, was discovered 1.600kilometer east from Japan [2], [3]. Covering an estimated areaof 292.500 km2, it is the largest known volcano on Earth andit can be compared with the largest known in the Solar System,the Olympus Mons on Mars. Its size is about the same as theBritish islands or three times the area of Portugal.

Hidden in the ocean there are not only huge scientificfindings to be done, but also huge economical benefits, waitingto be explored and exploited. If our resources are becomingscarce, why should not we search for them intensively in theother three quarters of the planet? From fishing, minerals,energy, gas or oil, there are many resources that have notbeen used due to the extreme conditions characteristic of theseregions.

In order to make this possible, there is an urgent demandfor robust and sophisticated autonomous marine vessels able toperform more and more complex tasks to explore places wherethe human has never been before. Thus leading sea explorationto get further, to be more efficient and cost effective and pro-tecting human beings from the harsh environmental conditions.All of this is being boosted by a flourishing and constantdevelopment in the electronics’ area providing increasinglyaccurate and or cheaper sensors, actuators, communication andprocessing systems.

The underwater environment has various unique featuresthat make it quite challenging. The water itself introducesstrong attenuation to electromagnetic waves, thus it is impos-sible to use useful technologies such as Wi-Fi communication,the Global Positioning System (GPS) localization and on theother hand there are external disturbances as ocean currentsthat can change quickly.

The 90s started an era of great research activity with thepurpose of solving problems as the ones mentioned previouslybut more advanced problems are now being addressed. Inthis context, the MORPH European project aims to create anunderwater robotic system composed of a group of heteroge-

2

neous autonomous marine vehicles with different capabilitiesthat change their formation over the time in other to map richtextured terrains with negative slop.

The possible applications of MORPH range from scientific,as mapping and monitoring natural habitats, or commercial asthe inspection of dams, harbors or oil and gas pipelines. Thecommon feature of these scenarios is the requirement to resortto different and complementary instruments at very close rangeto examine any arbitrarily-shaped structure.

Part of the problem of navigation is the path following ortrajectory tracking. Very robust systems have to be done to dealwith this harsh environment and one of the biggest challengesis to steer accurately robots to a path regardless of the externaldisturbances such as ocean currents. In that sense, the pathfollowing problem is addressed in this paper.

The problem of controlling autonomous vehicles has beenthe center of continuous research. There are three main areas:

1) point stabilization - the aim is to stabilize the vehiclearound a given point;

2) trajectory tracking - the goal is to track a time parame-terized trajectory;

3) path following - the vehicle is required to follow a path,however without time parameterization

From these, path following will be the focus in this text.Path following control has not been as in focus as the othertwo problems. Assuming that the surge speed of the vehicleis always positive and has an arbitrary profile, the aim of thecontroller is to steer the vehicle to the path. When comparingwith trajectory tracking, usually smoother convergence isachieved.

When looking for path following strategies, one of the mostcommon is to use a projection on the path and based on itgenerate commands for the vehicle. This strategy may raiseproblems such as ambiguities. When for instance, finding theclosest point of the path and it is possible to exist more thanone at the same distance.

Such problems have been solved by using a virtual targetwith its own dynamics, the so called rabbit, [4]. This waythere is no longer the need to do the projection to the pathand references are less prone to have rapid changes due tojumps in the target to follow.

Another topic that usually does not have much emphasison the literature is the design of controllers that are resilientto external disturbances, such as ocean currents. Possibly themost common solution is to have a proportional and integral(PI) controller on the cross track error, so that the proportionalpart drives the vehicle to the trajectory and the integral partcreates the needed heading offset making the total velocity ofthe vehicle tangent to the path as in [5], [6] or [7].

If the vehicle is in a straight line and once it converges,this solution is very robust. However if the vehicles changesits direction, the integral part has to converge to another set-point and no information from the previous navigation is used.

An alternative to the PI controller is to have a currentobserver in the inertial world-fixed frame and generate ref-erences for the heading of the vehicle based on it. Thisdifferent approach besides theoretically solving the PI transient

problem, produces an estimate of the ocean current that maybe useful to other vehicles or other applications.

The paper is organized as follows. Section II introducesthe two types of autonomous marine vehicles used in thiswork, the Delfim and Medusa and a model used for simulationis presented. Section III describes a new path followingstrategy an inner-outer loop strategy. Section IV introducesthe ocean currents in the path following control and analyzesthe robustness of the algorithm. Section V shows the resultsobtained in real tests and assesses the performance. SectionVI describes some software developed to control, plan themotion and simulate vehicles. Section VII summarizes theresults obtained and suggests topics for further research.

II. VEHICLES’ CHARACTERIZATION

In the context of this paper two types of vehicles were used:Delfim and Medusa. Both of them were designed and builtin the Dynamical Systems and Ocean Robotics Laboratory(DSOR), a group of the Institute for Systems and Robotics(ISR) which is affiliated to Instituto Superior Técnico inPortugal. A brief characterization of the vehicles is made inthis chapter. In the end of this chapter, a model for the Medusais also presented as it is the vehicle used for the path followingcontroller design.

A. Delfim’s description

The Delfim is an autonomous catamaran. It was designedto be a robust Autonomous Surface Vehicle (ASV) capable ofworking in cooperation with multiple Autonomous UnderwaterVehicles (AUVs), like in ASIMOV project. A picture of thevehicle can be seen in Figure II.1.

Figure II.1: Delfim

Measuring 3.5 meter long and 2.0 meter wide and weightingabout 250 kg, this vehicle has the capability to take a fairamount of sensor payload. The basic set-up for the sensorsincludes a GPS and Inertial Measurement Unit (IMU) installedin its starboard hull.

In its central body, an acoustic modem could be installed.In that position the acoustic interferences are reduced (createdby the motors or caused by air bubbles from either hull orsurface proximity). Each hull has an electric motor that cancontrol surge and yaw motions (by common and differentialmode, respectively).

B. Medusa’s characterization

Contrary to Delfim, the Medusas are much smaller andlighter vehicles (about 30 kg) that can be easily transported,placed in and out of the water. This takes special importance

3

when there is the need to test cooperative algorithms, thuswith multiple vehicles.

At the moment this paper was written, there were 3Medusas: two surface and one diver version. They are made oftwo acrylic tubes, one on the top of the other spaced by 0.15m, ended by aluminum caps and fixed by a aluminum frame.Each tube measures about 1 m of length and its diameter is0.15 m.

The top tube, that is semi submerged in the surface version,holds the majority of the systems such as computer, GPS, IMUand a Wi-Fi access point. As for the lower tube, it holds thebatteries, the acoustic modem and some other sensors.

Regarding propulsion, two thrusters are mounted on the alu-minum frame to control the surge and yaw motion (horizontalplane). The diver version has two extra ones, in order to controlthe heave motion. In the case of the surface version, there isno need to control these motion due to the buoyancy as seen inFigure II.2. The normal speed of the vehicles is 0.5 ms−1 inorder to increase its autonomy and they can go up to 1.5 ms−1.

Figure II.2: Medusa in MORPH trials, Toulon, July 2013

C. Model of Medusa

1) Coordinate Frames and conventions: First thing to dowhen addressing any problem is to define conventions. Itis common in Robotics to use two orthonormal coordinateframes: an inertial reference frame, or world frame, namingits axes as {xI , yI , zI} and a body-fixed frame, {xB , yB , zB}.Figure II.3 represents both frames, {I} and {B}.

Figure II.3: Coordinate frames used

Regarding the body axes, they are defined as:• xB longitudinal axis (directed from the stern to fore);• yB transversal axis (directed from portside to starboard);• zB normal axis (directed from top to bottom).For the sake of simplicity the body frame origin was

chosen to coincide with the center of gravity, as the dynamicsequations are simpler to write.

When referencing marine vessels in an inertial frame, vari-ous systems are used. Two of the most frequent are UniversalTransverse Mercator (UTM) and Local Tangent Plane (LTP).Both allow to represent the position in meters relative to some

Degrees Of Freedom Forces Velocities Positionx-direction (surge) X u xy-direction (sway) Y v yz-direction (heave) Z w zaround x-axis (roll) K p φaround y-axis (pitch) M q θaround z-axis (yaw) N r ψ

Table II.1: The notation of SNAME (1950) for marine vessels

reference point on Earth. The convention used in this text isthat xI heads East; yI points North; zI points down and theangle ψ is zero when pointing North and increases clockwise.

As marine vehicles typically move in the six degrees offreedom, 6 independent coordinates are needed to describethe position and orientation. The first three define the threedimensional position [x, y, z]T and the last three are the Eulerangles (φ, θ, ψ). The convention adopted is shown in compactform in Table II.1 and explained below:• η1 = [x, y, z]T - Position of the origin of {B} expressed

in {I}, North (x), East (y), Down (z)• η2 = [φ, θ, ψ]T - Roll (φ), Pitch (θ), Yaw (ψ) angles

expressing the orientation of {B} with respect to {I}(zyx convention),

• ν1 = [u, v, w]T - Linear velocity of the origin of {B}relative to {I}, expressed in {B}.

• ν2 = [p, q, r]T - Rotational velocity of {B} with respectto {I}, expressed in {B}.

• τ1 = [X,Y, Z]T - Actuating forces expressed {B}.• τ2 = [K,M,N ]T - Actuating moments expressed {B}.In compact form yields:

η = [ηT1 ,ηT2 ]T

ν = [νT1 ,νT2 ]T

τRB = [τT1 , τT2 ]T

(II.1)

2) Dynamics: The Newton-Euler laws for rigid bodies arepresented in this section. It is convenient to formulate themin the {B} frame as they become simpler. The generalizedrigid-body dynamics as suggested in [8] can be expressed as:

MRB ν +CRB(ν) ν = τRB (II.2)

whereMRB is the rigid body inertia matrix,CRB the Coriolisand centrifugal terms and τRB is a generalized vector ofexternal forces and moments.τRB can be decomposed into several terms as follows:

τRB = τ + τA + τD + τR + τ dist (II.3)

where τ is the vector of forces and moments due to eitherthrusters or surfaces; τA = −MA ν − CA(ν) ν the termsdue to added masses; τD = −D(ν) ν the hydrodynamicsterms due to lift, drag, skin friction; τR = −g(η) restoringforces and torques to do with gravity and buoyancy forces andτ dist the terms due to external disturbances: waves, wind, etc

Merging this information with Equation II.2, yields:

Mν +C(ν)ν +D(ν)ν + g(η) = τ + τ dist (II.4)

4

MRB parametersm 17 [kg]Iz 1 [kgm2]

Table II.2: Important Inertial Matrix Coefficients

Added MassXu −20 [kg]Yv −30 [kg]Nr −0.5 [kgm2]

Table II.3: Added Mass coefficients

3) Simplified equations of motion: Throughout this work,it was considered that the vehicle only moves in the horizontalplane as the vertical control was being developed in parallel.Therefore a simpler two-dimensional model was used whichassumes that φ ≈ 0, θ ≈ 0. The state is defined as [x, y, ψ]T

and Equation II.5 describes the kinematic model.x = u cosψ − v sinψ + Vcxy = u sinψ + v cosψ + Vcyψ = r

(II.5)

were Vci represent the ocean current velocity in the i compo-nent.

The two thrusters actuating in the horizontal plane cangenerate two important forces: τu that commands the surgespeed and τr that controls the rotation about z-axis, r. Theseare defined in Equation II.6

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

(II.6)

where Fps and Fsb represent the forces produced by eachthruster, from portside and starboard respectively. In the sec-ond line of Equation II.6, l represents the length of the armwith respect to the center of gravity.

Taking into account a constant irrotational ocean current,the dynamics equations for the horizontal plane, neglectingroll, pitch and heave motion are the following:

muu−mv(vr + vc)r + duur = τumv v +mu(ur + uc)r + dvvr = 0

mr r −muv(ur + uc)(vr + vc) + drr = τr

(II.7)

mu = m−Xu du = −Xu −X|u|u |ur|mv = m− Yv dv = −Yv − Y|v|v |vr|mr = Iz −Nr dr = −Nr −N|r|r |r|muv = mu −mv

(II.8)

where mi parameters represent mass and hydrodynamic addedmass coefficients and di the drag terms. Moreover ur and vrare the surge and sway speeds with respect to the water and ucand vc the velocity components of the ocean current expressedin body frame.

The parameters previously identified in [9] and used in thiswork are shown in Tables II.2, II.3 and II.4.

Linear Terms Quadratic TermsXu −0.2 [kg s−1] X|u|u −25 [kgm−1]

Yv −50 [kg s−1] Y|v|v −0.01 [kgm−1]

Nr −0.1 [kgms−1] N|r|r −21 [kgm]

Table II.4: Drag Coefficients

4) Motors: The Seabotix thrusters used on the Medusawere previously identified in [9]. The model used to theidentification was a pure delay, followed by a first order systemand then a quadratic conversion from Revolutions per Minute,RPM, to torque.

The pole used was K0 = 7.21 rad s−1, the delay = 0.346 sand the conversion from RPM to torque coefficient η = 1.78 ·10−6 kgms−1.

III. PATH FOLLOWING WITHOUT OCEAN CURRENTS

Continuing the work in [4], the algorithm presented inthis text has some differences. One of them is that thecontroller only generates references to the inner loops insteadof producing directly the torques for each actuator. The innerloops are therefore dependent on the dynamics of the vehicle.

In this chapter, a new path following controller for under-actuated marine vehicles is proposed. The strategy adoptedis to decouple the into a inner-outer loop control. The outerloop controller works only on a kinematic level, generatingreferences to inner loop.

A. Rabbit path following without ocean currents

The aim of the path following problem is to drive the vehicleto a path, taking the cross track error (perpendicular to thetrajectory) to zero with non time parameterized paths.

1) Kinematic Equations in the Serret-Fernet Frame withoutocean currents: Firstly, it is important to define the framesused to derive the path following control law.

Consider a point P on the desired path and the correspond-ing Serret-Fernet frame {F} as indicated in Figure III.1. Thecenter of mass of the vehicle to control is denoted as Q andthe signed curvilinear abscissa of P along the path as s. Inother words, s is a measure of the length of the path.

θm

{I}

O

{F}

P

θcq

p

r

s1

y1

yI

xI

t

Q

Figure III.1: Frame definition

It is now clear that Q can be expressed (in the form[x, y, ψ]T ) as q = [X,Y, 0]T in the inertial frame {I} and as

5

[s1, y1, 0]T in {F}, where the pair (s1, y1) can be interpretedas along and cross track error, respectively.

The rotation matrix from {I} to {F}, parametrized by theangle of the trajectory, θc, is:

R =

[cos θc sin θc− sin θc cos θc

](III.1)

Denoting θc as ωc, cc(s) as the path curvature and gc(s) =dcc(s)ds as its derivative, then:{

ωc = θc = cc(s)s

cc(s) = gc(s)s(III.2)

The velocity of P in {I} can be expressed in {F} as:(dpdt

)F

=[s 0 0

]T(III.3)

The velocity of the center of mass Q in {I} is given by:(dqdt

)I

=

(dpdt

)I

+ R−1(drdt

)F

+ R−1(ωc × r) (III.4)

where r is the vector from P to Q, as in Figure III.1. To getthe velocity of Q in {I} expressed in {F}, it is just take tomultiple Equation III.4 on the left by R, yielding:

R(dqdt

)I

=

(dpdt

)F

+

(drdt

)F

+ (ωc × r) (III.5)

Each term of Equation III.5 can be written as:(dqdt

)I

=[X Y 0

]T, (III.6)(

drdt

)F

=[s1 y1 0

]T, (III.7)

and

ωc × r =

00

θc = cc(s)s

× s1y1

0

=

−cc(s) s y1cc(s) s s10

, (III.8)

Merging Equation III.5 with III.3, III.6, III.7 and III.8

R

XY0

=

s1 + s(1− cc(s)y1)y1 + cc(s) s s1

0

. (III.9)

Solving for s1 and y1:s1 =

[cos θc sin θc

] [ XY

]− s(1− cc y1)

y1 =[− sin θc cos θc

] [ XY

]− cc s s1.

(III.10)

In [10], it was assumed that s1 = 0 for all time, this waythe point P used is the projection of Q on the path, assumingit is well defined. Then, one was forced to solve for s, yieldinga singularity in y1 = 1

cc. As a consequence of this choice, the

initial position of Q had to be confined to a tube around thepath with radius smaller than 1

ccmax.

Contrary to [10], in [11] s1 is not forced to equal zero, thuscreating an extra degree of freedom to take into account whendesigning the controller. P is no longer a projection, but hasits own dynamics given by some control law. Therefore theabove mentioned constraint is lifted.

The kinematic model for the vehicle, relating the body framesurge speed with the velocity in {I} is given by:[

X

Y

]= u

[cos θmsin θm

](III.11)

where θm is the heading of the vehicle as defined in FigureIII.1. Denoting the error in heading as:

θ = θm − θc (III.12)

the kinematic model for the robot in {F} is given by:{s1 = −s(1− cc y1) + u cos θ

y1 = −cc s s1 + u sin θ(III.13)

2) Non-linear Control Law: As a consequence of theadopted inner-outer loop strategy, the choice of the Lyapunovcandidate V with a term regarding the heading error was notused. When deriving V a term would require to specify thevelocity of the vehicle and due to the inner outer-loop structureof the controller the command variable is the heading, not therate.

The goal of this path following law is to drive the errorss1, y1 to zero. With this purpose, the Lyapunov functioncandidate in Equation III.14 was considered.

V =1

2(s21 + y21) (III.14)

Denoting the time derivative of V as V :

V = s1 · s1 + y1 · y1 (III.15)

From Equation III.15 and Equation III.13:

V = s1 (−s+ s cc y1 + u cos θ) + y1 (−cc s s1 + u sin θ)(III.16)

That can be simplified as:

V = s1 (−s+ u cos θ) + y1 u sin θ (III.17)

In order to force the first part of Equation III.17 to be nonpositive, the control law for the virtual target, s, was chosento be:

s = u cos θ + k1 · s1, k1 > 0 (III.18)

From Equation III.17 and Equation III.18:

V = −k1 s21 + y1 u sin θ (III.19)

Assuming the inner-loop is infinitely quick and in order tomake the system stable, θ is defined as:

θ = − arcsin

[sat

(y1γ

)], γ > 0 (III.20)

6

In Equation III.20, γ is a tuning knob, defining how smooththe vehicle should approach the path. With this control law itis straightforward from Equation III.19 that:

V = −k1 s21 −1

γy21 u (III.21)

From Equation III.21, it is easy to conclude that the system,when not saturated, is assymptotically stable if the nominalspeed u > 0. When saturated, the heading of the vehicleis driven to be perpendicular to the trajectory, thus it alsoconverges.

B. Inner loop for heading control

Having already designed the outer loop which generatesreferences, in this section will be described how the innerloop was designed.

1) System Discretization: As it is important to make thisinner loop quick, it has embedded some knowledge of thethrusters. The sample time used is imposed by the motors’specifications that work at the frequency of fs = 5 Hz orsample time h = 0.2 s. Moreover from many experimentscarried out it is known that they have a delay of τ = 0.346s. With that knowledge, one can project a controller whichembodies that delay, being able to increase the gains withoutoscillations.

A system with a delay of τ in the actuators, as this one,has the state space representation as in Equation III.22.

dx(t)dt = Ax(t) +Bu(t− τ)

y(t) = Cx(t) +Du(t)(III.22)

Before discretizing the system, one simplification was made:the sway motion was neglected, due to the vehicles’ dynamics.Linearizing about r = 0, the dynamics’ equation for yaw,based on Equation II.8, is expressed in Equation III.23.

ψ =τrmr

+Nrmr

ψ (III.23)

Defining the state x(t) = [ψ, ψ]T and the input as u(t) =τr, the state space representation of the system is complete bydefining matrices A,B,C and D:

A =

[0 10 Nr

mr

]B =

[01mr

]C =

[1 0

]D = 0

(III.24)It is easier to design a controller for systems with actuators’

delay in discrete time as its state representation is finite-dimensional.

As a result of the signal u(t) being piecewise constant overthe sampling period, its delayed version u(t− τ) will also be.However, if τ is not a multiple of h, from the system point ofview the command u(t) changes between sample instants.

As in [12] and [13], the first step is to compute how manyinteger delay, d− 1, there are and the modulus after divisionof τ by h, τ ′.

τ = (d− 1)h+ τ ′ 0 < τ ′ ≤ h (III.25)

in this case, d = 2 and τ ′ = 0.146 s.

The zero-order-hold sampling of the system yields EquationIII.26.

x(kh+h) = Φx(kh)+Γ0u(kh−h)+Γ1u(kh−2h) (III.26)

whereΦ = eAh

Γ0 =∫ h−τ ′

0eAsds B

Γ1 = eA(h−τ ′)∫ τ ′

0eAsds B

(III.27)

Therefore, the discretized system has the state space repre-sentation shown in Equation III.28 and Equation III.29.

x(kh+ h) = Az x(kh) +Bz u(kh) (III.28)

where

Az =

Φ Γ1 Γ0

01×2 0 10 0 0

Bz =

0001

Cz =

1000

T

Dz = 0

x(k) =[ψ(kh) ψ(kh) u(kh− 2h) u(kh− h)

]T(III.29)

2) Precise reference following: With the purpose ofsmoothing as much as possible the tracking of a reference,it is important to take into account the vehicle’s dynamics.

Defining the error in yaw as ψ := ψ − ψref and taking itssecond derivative:

¨ψ = ψ − ψref (III.30)

The desired behavior of the controller is to be a exponen-tially stable as shown in Equation III.31.

¨ψ = −K1ψ −K2

˙ψ (III.31)

Replacing ¨ψ with the vehicle’s model, Equation II.7, and

once more neglecting v, in Equation III.30, it yields EquationIII.32.

¨ψ =

1

mr(τr − drr)− ψref (III.32)

The desired angular acceleration is given by ψref = ccs+gcs

2. Merging this knowledge with Equation III.31 and theEquation III.32 we conclude:

τr = −K1mrψ −K2mr˙ψ + drr + ccs+ gcs

2 (III.33)

3) Inner Loop Design: With the purpose of computing thefeedback gains, there are two main options: either the classicalpole placement or techniques derived from optimal control.

The pole placement approach may be difficult to largeorder systems as there are not easy analytical and systematicalgorithms to choose them. Whereas, a optimal control ap-proach like Linear Quadratic Regulator may be more scalable.However, by changing the weight factor of a state its effect isnot decoupled from the other states, so it is also an iterativeprocess.

The formulation of the infinite horizon discrete time LinearQuadratic Regulator problem is to minimize the cost functionJ in Equation III.34 over the input u.

7

J =

∞∑k=0

[x(k)TQx(k) + u(k)TRu(k)

](III.34)

For a discrete time linear system defined as:

x(kh+ h) = Az x(kh) +Bz u(kh) (III.35)

The solution for this problem is:

u = −KLQR xKLQR = (R+BTz PBz)

−1BTz PAz(III.36)

where P is the unique positive definite solution to the discrete-time algebraic Riccati equation, Equation III.37.

P = Q+ATz

(P − PBz

(R+BTz PBz

)−1BTz P

)Az(III.37)

As a requirement, the pair Az, Cz has to be detectableand the pair Az, Bz stabilizable. The observability O andcontrollability C matrix are as defined in Equation III.38. FromEquation III.29, both matrices are full rank, thus the system isnot only detectable and stabilizable, but also observable andcontrollable.

C =[Bz AzBz A2

zBz A3zBz

]O =

[Cz CzAz CzA

2z CzA

3z

]T (III.38)

The control input is defined by:

u = ufb + uffufb = KLQR · (xref − x)uff = −Nrr −N|r|r |r| r + ccs+ gcs

2(III.39)

where ufb is the term due to feedback and uff due to feed-forward.

IV. PATH FOLLOWING WITH CURRENTS

In this chapter, a technique to make the control law dealwith ocean currents is proposed. The method consists in anobserver in the fixed world frame and then generate referencesto the yaw controller to cancel the perpendicular componentof the current to the trajectory.

A. Rabbit path following with ocean currents

Usually, besides forcing the cross track error to go to zero,it is also specified Equation III.14 it is also required to forcethe yaw of the vehicle to be the same as the trajectory.

When designing a path following controller for marinevessels, it is very important to take into account ocean currents.With currents the vehicle’s heading has to be mismatched sothat the total velocity vector is aligned with the trajectory,usually achieved with a PI controller on the cross track error.This mismatch is from now on called relative course angle.

The adopted notation for ocean current was the following:Vc is the velocity vector of the ocean current; V ‖c is the parallelcomponent of the ocean current to the trajectory (along withthe x axis of the Serret-Frenet) and V ⊥c the perpendicularcomponent.

Going back to the kinematic model of the robot in theSerret-Fernet, Section III-A1 and taking ocean currents intoaccount, Equation III.13 is modified and the new kinematicmodel is:

{s1 = −s(1− cc y1) + ur cos θ + V

‖c

y1 = −cc s s1 + ur sin θ + V ⊥c(IV.1)

where ur is the relative surge speed of the vehicle with respectto the water.

Taking the same approach as in Section III-A1,

V = s1 (−s+ ur cos θ + V ‖c ) + y1(ur sin θ + V ⊥c ) (IV.2)

The control law for the rabbit is then given by:

s = ur cos θ + k1 · s1 + V ‖c , k1 > 0 (IV.3)

To drive the cross track error to zero, the heading error θshould be:

θ = − arcsin

[sat

(y1γ

+V ⊥cur

)], γ > 0 (IV.4)

With this control law and assuming that we know theperpendicular component of the current to the path it isstraightforward the results for V would be the same as inEquation III.21.

It is important to analyze the system behavior when sat-urated. If V ⊥c is bigger than ur it is impossible to controlthe vehicle as its speed is not even enough to compensate thecurrent and stabilize the system. Consequently the situationsto analyze are when it is saturated due to large absolute valuesof y1, either positive or negative.

In these cases, when θ is saturated it takes values of ±90◦,that means the vehicle is going perpendicular to the trajectory,due to the cross track error. As ur > V ⊥c , this term is only anoffset and the term due to the cross track error will be drivento zero.

B. Current Observer

1) Theoretical formulation: Not having access to the realvelocity of the current, an observer was used and it is presentedin this section. The world fixed frame observers’ equations arein Equation IV.5 and Equation IV.6, taking the same approachas in [14].

Defining the notation x as an estimate of the inertial x andx := x− x as the error of the estimate (the same notation fory and the currents vci ), the equations for the current observerare: {

˙x = ur cosψ − vr sinψ + vcx + kx1 x˙vcx = kx2

x(IV.5)

{˙y = ur sinψ + vr cosψ + vcy + ky1 y˙vcy = ky2 y

(IV.6)

where ur and vr are, respectively, the surge and sway speedswith respect to the water and ψ the heading of the vehicle.

The error of the observer can then be defined as:

8

[˙x

˙vcx

]=

[−kx1

1−kx2

0

] [xvcx

](IV.7)

Clearly, the estimate errors x and vcx := vcx − vcx areasymptotically exponentially stable if all roots of the charac-teristic polynomial p(s) = s2 +kx1s+kx2 associated with thesystem have strictly negative real parts.

In order to know the relative velocities to the water a DVLwould be useful. However, due to its cost and dimensions itis preferable not to use one. So simplifying assumptions weremade.

Firstly the relative sway speed is harder to estimate, but dueto the Medusa’s dynamics it can be neglected. As for the surgespeed, having a model for both the thrusters and the vehicle,a rough estimate ur can be computed.

The strategy used to compute ur is to to compute τu basedon the thruster model. Then, neglecting v from the dynamicof u in Equation II.7 and solving for the stationary situationu = 0 :

duur = τu ⇔τu +X|u|u |ur|ur +Xu ur = 0

(IV.8)

As ur is assumed to be always positive (vehicle is alwaysmoving forward), Equation IV.8 is simply a quadratic equation.

For the next simulations, Vcx = Vcy = 0.2 ms−1.

C. Robustness of the controller

In this section, simulations were done to demonstrate thebehavior of the controller in the presence of measurementnoise or error in the parameters of the model.

1) Error in the model parameters and bias in the yawsensor: The major concern raised by the strategy used is theestimate of ur. If the model is perfect, there would be noproblem however, the controller should be robust to error upto 20, 30 % on the parameters used.

To test this, the results of a simulation are here presented,where the drag coefficients used in Equation IV.8 to get urwhere multiplied by 0.5 and 2 and the results were similar.

When simulating this, it is possible to see that the currentlearnt by the observer is no longer correct, Figure IV.1c,however the algorithm still drives the cross track error to zero,Figure IV.1b.

In the interval [500, 1600] s, the vehicle is moving to thenegative side of the x axis, against the x component of thecurrent, whilst from [1900, 2100] s is moving to the positiveside, helped by the current. It is curious to notice that themean value of the estimated values of the x component of thecurrent is the true value of the current.

As for the bias in the yaw sensor, its behavior is similar tothe error in the model parameters. In simulations done with abias of 10◦ in a lawn mower maneuver, the estimated currentoscillated between two values and the cross track error is alsodriven to zero, with peaks in the beginning of a curve.

2) Performance of the controller with noise in the sensors:For this simulation, the measurement noise is white Gaussiannoise with a standard deviation of 5 cm for the x and y posi-tion and 2◦ for the heading readings noise. As a consequenceof the noise, the actuation becomes more nervous due to the

actuation from the cross track error term, but as the currentobserver is sufficiently slow, the noise influence is not evident.

0 20 40 60 80 100

−70

−60

−50

−40

−30

−20

−10

0

10

Vehicle’s trajectory

Easting [m]

Nort

hin

g [m

]

desired

simulated

(a) XY plot of trajectory

0 500 1000 1500 2000 2500 3000−1

−0.8

−0.6

−0.4

−0.2

0

0.2

0.4

0.6

0.8

1

time [s]

Dis

tance [m

]

Cross track error evolution

y1

(b) Cross track error

0 500 1000 1500 2000 2500 3000−0.5

−0.4

−0.3

−0.2

−0.1

0

0.1

0.2

0.3

0.4

0.5

time [s]

Velo

city [m

s−1]

Current estimates

Vcx_est

Vcy_est

(c) XY components of the ocean current esti-mates

Figure IV.1: Simulations results with sensor noise and error indrag parameters

V. REAL TESTS

When simulating, many assumptions are made and some-times bad models for both the system and noise could givevery different results. With the purpose of testing if thoseassumptions were valid, some trials have been carried out onthe Tagus river and on the Olivais’s Dock.

A. Rabbit Controller testsHaving tested the current observer part in open loop, it was

time to close the loop and test the whole controller.

9

Medusas’ middleware system is the Robot Operating Sys-tem (ROS) which has many features, one of them is the abilityto test controllers directly from Simulink, making it easier todebug in a development stage of the algorithm.

This way one can use part of the modules of the Medusaand override others. In this case, the pose filter was used sothe output of the controller is the command for the thrustersand the input from the vehicle is the pose. During all thefollowing missions, the reference for the inertial surge speedwas 0.5 ms−1.

1) Lawn mower maneuver: The first mission tested was alawn mower maneuver with leg length of 100 meter and 10meter radius curves. The starting behavior with oscillationsis due to the current observer initial transient. Later bymultiplying the reference to compensate the current by a stepsignal, disabling it for the first 30 seconds, that behavior wasgone.

The maneuver is the same as in Figure ?? and the crosstrack error in the straight lines converges to zero as seen inFigure V.1a. During the curves it has a steady state differentfrom zero, doing the curve on the inside, behavior that wasnot revealed in the simulations. This is possibly caused by thecurrent compensation and with more trials, only adjusting thequickness of the current observer is thought to be possible tosolve.

There was one thing that it was not noticed in simulationbut that also happen is the overshoot of the yaw in the endof a curve, producing a characteristic behavior. This is due tothe current canceling term’s transient.

2) Other trajectories: To show the capability of performingany smooth path, some different trajectories were tested.

The performance previously pointed out of the controller incurves is similar, Figure V.2. The mean cross track error isabout 20 to 30 cm and is not extinguished as in simulation.

VI. SOFTWARE

During this work, software was developed and a briefdescription of it is made in this chapter.

A. Changes in Delfim Architecture

Delfim has been used in several projects, but with its ownarchitecture. With the appearance of ROS never has so easyto reuse code and exchange modules from different vehicles.

Many tools were developed for the Medusas and makingthem available for Delfim was a request. In order to operateDelfim like a Medusa, there was no need to change the mod-ules dealing with sensors and the pose estimation. Everythingelse was changed taking into account the fact that the processis easily reversible. No files were changed, only launchingdifferent modules.

Some of the modules imported to Delfim were: module fortesting controllers with MATLAB (useful in a developmentstage of the controllers); module to communicate with othervehicles for cooperative missions; controllers: Path Following,WayPoint, Inner loops; mission Handler: node that parses themission file and activates the proper controller; Module tocommunicate with the operating console.

0 200 400 600 800 1000 1200−1

−0.8

−0.6

−0.4

−0.2

0

0.2

0.4

0.6

0.8

1

time [s]

Dis

tance [m

]

Cross track error evolution

y1

(a) y1 during the mission

0 200 400 600 800 1000 1200−0.5

−0.4

−0.3

−0.2

−0.1

0

0.1

0.2

0.3

0.4

0.5

time [s]

Velo

city [m

s−1]

Current estimates

Vcx_est

Vcy_est

(b) XY components of the ocean current esti-mates

Figure V.1: Lawn Mower mission with 100 meter of leg lengthand 10 meter radius curves

−140 −120 −100 −80 −60 −40 −20 0

−50

−40

−30

−20

−10

0

10

20

30

40

50

Vehicle’s trajectory

Easting [m]

No

rth

ing

[m

]

desired

real trajectory

Figure V.2: Lemniscate of Bernoulli mission

B. Mission Programmer

When in a trial, it is crucial to be prepared to change theplans because of weather conditions or other reasons. For thatreason, having software that produces the missions for thevehicles prevents lots of errors, while allowing to preview themission itself to check if everything is correct.

Figure VI.1: Mission Programmer

10

The Mission Programmer can be seen in Figure VI.1, butsome menus are not shown because of its dynamic nature.Some of its features are: change the scenario; generate a lawnmower maneuvers; defining the formation of the mission; editthe mission; create a mission by click in the map; moveMission to a specific point of dragging and dropping; loada previously generated mission file, save the mission to a textfile so that the Medusas can perform it; preview the mission,etc.

C. UWSim Simulator Integration

In a project with multiple partners, different vehicles anddifferent software architectures it is important to simulate asmuch as possible to check the well functioning of most of thesubsystems involved.

In the context of the MORPH project, it was agreed touse the UnderWater SIMulator (UWSim), [15], that is alsodeveloped in ROS. Underwater scenarios can be created bystandard software and then be imported to this visualizer.Besides surface and underwater vessels, it is also possible tosimulate robotic manipulators or sensors and test them withthe whole software architecture of the vehicles.

To test the integration of the UWSim visualizer, a Coop-erative Path Following (CPF) mission was simulated and theresult is shown in Figure VI.2. The scenario is the dock usedin MORPH trials in Toulon, July 2013.

Figure VI.2: CPF simulation in UWSim

VII. CONCLUSION

This paper aim was to develop a control strategy forautonomous robotic vehicles, with emphasis on marine vessels.When designing a path following controller for this type ofvehicles it is extremely important to take into account externaldisturbances as ocean currents.

Contrary to what is usual, two nuances are used in thiscontroller. One of them is that the point used to compute thereferences is not the closest point to the trajectory, or a line ofsight. Instead, this point, called rabbit, has its own dynamics.This way, problems that arise when using the closest point ofthe trajectory, such as the existence of more than one at thesame distance causing sudden changes in the references, aresolved.

Another difference is that in order to compensate for thecurrent, instead of using a PI controller on the cross track error,a current observer was used. The PI controller has the problemthat happens when changing the heading of the vehicle, thecontroller has a transient as the integral part has to converge

to a new set-point. Theoretically, by knowing the current onecan give references to the heading of the vehicle without thistransient problem. The current observer used in this thesisneeds an estimate of the relative surge speed to the water, thatis estimated with the vehicle’s parameters. However, as thereis error in the knowledge of those parameters, the transientproblem was not solved.

Nevertheless, a curious behavior was shown, typical ofadaptive control that is even without the correct parameters,the controller stabilizes the system, with a cross track errorof zero in straight lines. Besides the advantage mentionedon the previous paragraph, this controller naturally enablespath following of any arbitrary path, provided that is smoothenough regarding the vehicle’s dynamics.

REFERENCES

[1] R. Ballard, “On exploring the oceans,” 2008. [Online]. Available:http://ed.ted.com/lessons/on-exploring-the-oceans-robert-ballard

[2] W. W. Sager, J. Zhang, J. Korenaga, T. Sano, A. A. P.Koppers, M. Widdowson, and J. J. Mahoney, “An immenseshield volcano within the Shatsky Rise oceanic plateau, northwestPacific Ocean,” Nature Geoscience, Sep. 2013. [Online]. Available:http://dx.doi.org/10.1038/ngeo1934

[3] B. C. Howard, “New Giant Volcano Below SeaIs Largest in the World,” 2013. [Online]. Avail-able: http://news.nationalgeographic.com/news/2013/09/130905-tamu-massif-shatsky-rise-largest-volcano-oceanography-science/

[4] L. Lapierre, D. Soetanto, and A. M. Pascoal, “Non-Singular Path-Following Control of a Unicycle in the Presence of Parametric ModelingUncertainties,” International Journal of Robust and Nonlinear Control,vol. 16, no. 10, pp. 485–503, 2006.

[5] E. Borhaug, A. Pavlov, and K. Y. Pettersen, “Integral LOS Controlfor Path Following of Underactuated Marine Surface Vessels in thePresence of Constant Ocean Currents,” in 47th IEEE Conference onDecision and Control, Cancun, Mexico, 2008, pp. 4984–4991. [Online].Available: http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=4739352

[6] W. Caharija, K. Y. Pettersen, J. T. Gravdahl, and E. Borhaug, “Inte-gral LOS Guidance for Horizontal Path Following of UnderactuatedAutonomous Underwater Vehicles in the Presence of Vertical OceanCurrents,” in American Control Conference, Fairmont Queen Elizabeth,Montréal, Canada, 2012, pp. 5427–5434.

[7] P. Maurya, A. P. Aguiar, and A. M. Pascoal, “Marinevehicle path following using inner-outer loop control,” inConference on Manoeuvring and Control of Marine Craft,Guarujá (SP), Brazil, 2009, pp. 38–43. [Online]. Available:http://paginas.fe.up.pt/ apra/publications/MCMC09_PM.pdf

[8] T. I. Fossen, Marine Control Systems: Guidance, Navigation and Controlof Ships, Rigs and Underwater Vehicles. Marine Cybernetics AS, 2002.

[9] J. Ribeiro, “Motion Control of Single and Multiple Autonomous MarineVehicles,” Instituto Superior Técnico, Lisbon, Nov. 2011.

[10] A. Micaelli and C. Samson, “Trajectory tracking for unicycle-type andtwo-steering-wheels mobile robots,” Institut National de Recherche enInformatique et en Automatique, Tech. Rep., 1993.

[11] D. Soetanto, L. Lapierre, and A. M. Pascoal, “Adaptive, non-singular path-following control of dynamic wheeled robots,” in42nd IEEE Conference on Decision and Control, vol. 2, no.December. IEEE, 2003, pp. 1765–1770. [Online]. Available:http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=1272868

[12] K. J. Aström and B. Wittenmark, Computer-Controlled Systems: Theoryand Design, 3rd ed. Dover Publications, 2011.

[13] G. F. Franklin, M. L. Workman, and D. Powell, Digital Control ofDynamic Systems, 3rd ed. Addison-Wesley Longman Publishing, 1997.

[14] A. P. Aguiar and A. M. Pascoal, “Dynamic Positioning and Way-PointTracking of Underactuated AUVs in the Presence of Ocean Currents,”International Journal of Control, vol. 80, no. 7, pp. 1092–1108, 2007.

[15] M. Prats, J. Perez, J. Fernandez, and P. Sanz, “An open source tool forsimulation and supervision of underwater intervention missions,” 2012IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS), pp. pp. 2577–2582, 2012.