1

Motion Planning for Multiple Autonomous RoboticVehicles: an Application of Optimal Control Theory

Andre Goncalves, Antonio Pascoal (Supervisor) and A. Pedro Aguiar (Co-Supervisor)

Abstract—Over the past few years we have witnessed anincreasing use of multiple autonomous robotic vehicles in missionscenarios with increasing complexity and associated risks. Theuse of multiple vehicles provides a paradigm shift which involvesusing, instead of a large monolithic vehicle, a set of simplervehicles, further enhancing flexibility and robustness againstfailures, while potentially lowering operation costs. For thesereasons, there is great interest in developing efficient motionplanning algorithms for cooperative missions.

This thesis aims to contribute to the development of a newgeneration of systems for mission planning that incorporates in itsformulation the dynamics of the vehicles. The first part describesthe development of a trajectory generator for a single vehicle. Forthis, we make use of the so-called Projection Operator Approach,a mathematical tool proposed by Professor John Hauser of theUniv. Colorado, USA to solve optimal control problems. Thegenerator synthesizes trajectories that minimize time or energyusage, avoiding, at the same time, collisions with fixed obstacles.

The second part of the thesis focuses on the extension of thedeveloped algorithm to the motion planing of multiple vehicles.We consider the case in which a group of vehicles is expectedto reach a target position simultaneously, while minimizingexpended energy and avoiding collisions with each other. Further,we consider the important case in which the vehicles need to movein formation.

The overall system performance is assessed via computationalsimulations with realistic models of the Medusa marine vehicle.

Keywords—Trajectory Generation, Collision Avoidance,Time-Minimal Optimization, Energy-Minimal Optimization, ProjectionOperator

I. INTRODUCTION

The past few years have witnessed an increasing in thedesign, development, and operation of remotely operated andautonomous vehicles. These vehicles are highly mobile robotsthat can reach otherwise inaccessible places and perform tasksthat are too hard or too dangerous for human beings to perform.They also have the potential to reduce the man-power andcost required to perform increasingly challenging scientificand commercial operations at sea. As the costs needed toproduce autonomous vehicles have decreased significantly, thenumber of missions in which more than one vehicle is usedhas increased. This allows them not only to carry out morecomplex tasks than a single robot can handle, but also increasesthe efficacy and efficiency of the missions.

An important part in the success of any mission is mo-tion planning, which consists of evaluating a collision freetrajectory from an initial state to a desired final state takinginto account geometrical, physical, and temporal constraints.

The planner complexity increases in multiple vehicle missionscenarios, where the vehicles need to cooperate with eachother, while at the same time avoiding collisions with oneanother.

The range of applications where single and multiple au-tonomous vehicles are used is very diversified.

A representative example, is the use of unmanned aerialvehicles to detect and prevent fires[1]. The objectives are todetermine the position of potential fire alarms and also toreduce the number of false alarms by means of cooperation.

Another example is the Nasa’s Mars Exploration Rover, Cu-riosity, that landed on Mars in 2012. This vehicle can analyseimages taken during a drive to compute a safe driving path.This will enable the rover to cover the remaining ground ofMars, giving crucial information about the geological featuresof that planet.

Another field where multiple autonomous vehicles play animportant role is ocean exploration. Exploring the oceans canbe costly, hard and potentially dangerous, so the best option isto deploy a large number of unmanned autonomous vehicles.

An example of a multiple vehicle cooperation project usingAUVs is Cooperative Cognitive Control for Autonomous Un-derwater Vehicles[2]. In this project, a number of autonomoussurface craft are required to maintain a desired formationto guide a human diver. For this mission’s objective to becompleted is not only necessary to have a reliable motioncontroller, but also a good mission planner.

A. General StatementThe general problem of motion planning can be divided

in three different aspects: path planning, manoeuvre planning,and trajectory generation. In this thesis we will focus on theproblem of generating trajectories that are feasible in terms ofthe vehicle’s dynamic model. Automatic trajectory generationis one of the most important functions which an autonomousvehicle must perform, since it must be able to move in hisworkspace whilst avoiding obstacles and reaching specific goalconfigurations.

New methods have been proposed for this problem, whichinclude differential geometric and differential algebra tech-niques, control input parametrization, and optimal controlapproaches ([3]).

In this work, we adopt a control theoretic approach thatbuilds on optimal control theory and yields trajectories formissions with single and multiple vehicles. To this effect, weborrow from and extend the formalism described in [4] and [5]that relies on the so-called Projection Operator approach forthe optimization of dynamic systems. We consider problems

2

whereby a fleet of vehicles must manoeuvre from their initial totarget positions and arrive at their destinations simultaneously,while minimizing the time to travel or energy expenditure.In contrast to what is normally reported in the literature, weinclude in the planning algorithm the explicit computation ofthe actual electrical energy consumed by the thrusters thatpower the Medusa vehicles considered in this thesis. Wefurther consider the case where the vehicles are required tomove in formation, that is, keeping a desired inter-vehiclegeometric pattern.

B. Main Contributions

In this work we develop a trajectory generator for singleand multiple vehicles, based on the mathematical frameworkadopted, the Projection Operator Approach[4]. The trajectoriesgenerated are feasible in terms of the vehicle’s dynamicsmodels, as well as the state and input constraints. They can alsoavoid static obstacles and inter-vehicles collisions. A strategyfor mission scenarios where the vehicles have to move whileholding a desired geometrical formation is also proposed.

The work is based in a dynamical model for the Medusavehicle but can also be extended to other types of vehicles.

C. Outline

The paper is structured as follows: Section II presents themodel of the vehicles used throughout the work, the Medusaclass of autonomous marine vehicles. Section III introducesthe mathematical framework used to solve optimal controlproblems. Section IV describes the type of optimal controlproblems that will be solved in the simulations. Section V de-scribes the implementation details of multiple vehicles missionscenarios. Section VI presents and comments the results ofseveral simulations made to test the proposed implementation.Section VII concludes the work with a summary of the resultsobtained and some topics of future work.

II. AUTONOMOUS UNDERWATER VEHICLE MODEL

To obtain the model equations it is common practice todefine two coordinate frames: an inertial reference frame {I},composed by the orthonormal axes {xI , yI , zI} and a body-fixed reference frame {B}, composed by the orthonormal axes{xB , yB , zB}.

Using the notation in [6], the kinematic equations of motionof a vehicle moving in the horizontal plane are

x = u cosψ − v sinψy = u sinψ + v cosψ

ψ = r

(1)

which can be written in the more compact form

p = R(ψ)ν (2)

where p = [x, y]>, ν = [u, v]

> and R(ψ) is the transfor-mation matrix from {R} to {I}.

Neglecting roll, pitch and heave rates, the equations forsurge, sway, and heading of an underactuated vehicle become

muu−mvvr + duu = τumv v −muur + dvv = 0

mr r −muvuv + drr = τr

(3)

where mu = m − Xu, mv = m − Yv , mr = Iz − Nr andmuv = mu − mv are mass and hydrodynamic added massterms, and du = −Xu − X|u|u|u|, dv = −Yv − Y|v|v|v| anddr = −Nr −N|r|r|r| are hydrodynamic damping effects.

The propelling thrust τu and steering torque τr are in turndefined by

τu = Tps + Tsb (4)τr = l(Tps − Tsb) (5)

where Tps and Tsb are the port side and starboard thrusterforces on the horizontal plane, respectively, and l is thedisplacement of the propellers from the center of {B} (seeFigure 2).

When we introduce a irrotational ocean current, vc, formingan angle φc with respect to {I}, the kinematic equations (1)hold but with u = ur + uc and v = vr + vc, where ur and vrare the components of the vehicle velocity with respect to thecurrent and uc and vc are the components of the ocean currentvelocity in {B}. Furthermore, dynamic equations (3) become

muur −mvvrr + duur = τumv vr −muurr + dvvr = 0

mr r −muvurvr + drr = τr

(6)

where now du = −Xu−X|u|u|ur| and dv = −Yv −Y|v|v|vr|.

A. The Medusa Autonomous Marine VehicleThroughout this work we use a full dynamic model of

the MEDUSA-class of autonomous semi-submersible roboticvehicles, developed at the Laboratory of Robotics and Systemsin Engineering and Science (LARSyS), Instituto Superior Tc-nico, shown in Figure 1. Each MEDUSA-class vehicle weighsapproximately 30 Kg and consists of two longitudinal acrylichousings with a total length of around 1 m. The upper body ispartially above the surface and carries an EPIC single-boardcomputer, an RTK-enabled GPS receiver, a full navigationsensor suite and an underwater camera. Most of the lowerbody is taken up by the batteries. An 802.11 interface isused for surface communications, while a Tritech acousticmodem enables underwater communication. The vehicle ispropelled by two side-mounted, forward-facing stern thrustersthat directly control surge and yaw motion, and is capable ofspeeds up to 1.5 m/s. Roll and pitch motion are not actuateddirectly.

B. Thruster ModelThe Medusa vehicle uses two Seabotix brushless HPDC1507

thrusters, with a maximum rotational speed of approximately75 rotations per second.

3

Fig. 1. Three Medusa vehicles

For our objective of minimizing the electrical energy that thethrusters consume, a mapping between the rotational velocitiesof the propellers (nps and nsb for the portside and starboardpropeller, respectively) and the thrust Tps and Tsb is necessary.Classically, this is done by computing the so-called open watercoefficients [6] defined in terms of the open-water advanceratio

J0 =vanD

(7)

where va is the propeller’s advance speed and D its diameter. Itis important to remark that the open-water coefficients can onlybe used when the advance velocity is non-zero together witha non-zero propeller velocity, where the rotational directionmust be the one that drives the vehicle forward, i.e. n > 0.This stems from the fact that (7) is not defined for n = 0. Toovercome this problem, a full four-quadrant model could havebeen used, as in [5] and [7]. For the sake of simplicity, wedecided to use the open-water coefficients and constrain thepropellers rotational velocities to be positive.

In the model chosen, the thrust and torque equations aregiven by

T = ρD4KT (J0)n|n| (8)Q = ρD5KQ(J0)n|n| (9)

where ρ is the water density (which we assume is constant) andKT and QT are non-dimensional thrust and torque coefficients.These coefficients are obtained from self-propulsion tests,where T , Q and n are measured.

To compute the advance speed, he have to take into accountthat they are not the same for each propeller when the vesselis rotating. From [5], the following expressions are obtained:

vaps = sin(atan2(−py, px))lr + u = −pyr + u (10)vasb = sin(atan2(py, px))lr + u = pyr + u (11)

where (px, py) is the offset of the propellers from the vehiclecenter of mass in body coordinates and l is their absolutedistance to the center of mass (see Figure 2).

III. PROJECTION OPERATOR APPROACH

This section provides a brief overview of the mathematicalframework used to solve the optimization problems. In particu-lar,the ones that involves solving finite-horizon optimal control

Fig. 2. Conceptual drawing of the Medusa as seen from above. Based on [5]

problems of the form

minimize∫ T

0

l(τ, xs(τ), us(τ))dτ +m(xs(T ))

subject to xs(t) = f(xs(t), us(t)), xs(0) = x0

(12)

where x ∈ Rn is the state-vector, u ∈ Rm the control-vector,l(·) is the running cost and m(·) the terminal cost.

The Projection Operator based Newton method for Tra-jectory Optimization (PRONTO) approach, as introduced in[4], will be discussed along with the necessary formulation toimplement it.

A. PRONTO

The main idea behind PRONTO is to define a projectionoperator to convert the constrained optimal control problem(12) into an unconstrained one to be solved using Newton’smethod. To this end and since the vector field f can beunstable, the PRONTO method takes a trajectory-trackingapproach. Suppose that ξ(t) = (α(t), µ(t)), t ≥ 0, is abounded curve (e.g., an approximate trajectory of f ) and letη(t) = (xs(t), us(t)), t ≥ 0, be the trajectory of f determinedby the nonlinear feedback system

xs(t) = f(xs(t), us(t)), xs(0) = α(0)

us(t) = µ(t) +Kr(t)(α(t)− xs(t))(13)

Under certain conditions, this feedback system defines a non-linear projection operator

P : ξ = (α, µ) 7→ η = (xs, us) (14)

that maps a certain trajectory to a trajectory that belongs tothe trajectory manifold T (the set of trajectories that satisfythe system dynamics).

Making g(ξ) := h(P(ξ)), with

h(ξ) =

∫ T

0

l(τ, α(τ), µ(τ))dτ +m(α(T )) (15)

we define the following Newton based method.

4

Algorithm 1 Projection Operator Newton Trajectory Opti-mization

given initial trajectory ξ0 ∈ Tfor i = 0, 1, 2, . . . do

1. Compute a regulator Kr for ξi2. Compute the search direction:

ξi = arg minζ∈TξiT

Dg(ξi) · ζ +1

2D2g(ξi) · (ζ, ζ)

with g(ξi) = h(P(ξi))3. Compute the step size: γi = arg min

γ∈(0,1]g(ξi + γζi)

4. Update: ξi+1 = P(ξi + γiζi)end for

This algorithm is quite similar to the usual Newton methodfor unconstrained optimization of a function g(·). The maindifferences are that the search direction minimization is per-formed on the tangent space (TξiT ) of the trajectory manifoldand the update step projects each iterate on to the trajectorymanifold.

A suitable feedback gain Kr may be constructed by solvinga finite horizon linear regulator problem (LQR) [8] about atrajectory η = (xs(t), us(t)). The matrices Qr = Q>r > 0and Rr = R>r > 0 that penalize the state and control errors,respectively, are defined in order to ensure the desired stability-like property. Also, to ensure good regulation all the way up tothe final time T , we include a positive definite terminal cost,Pr1 = P>r1 > 0. The feedback is thus chosen as Kr(t) =R−1r B(t)>Pr(t), t ∈ [0, T ] where P is the solution of theusual Riccati differential equation.

To compute the step size of the optimization algorithm abacktracking Armijo line search will be used [9]. It couldalso have been used a fixed step size of γi = 1, as a pureNewton method would, but by using the line search the regionof convergence is expanded.

B. Approximate Barrier FunctionalIn most optimal control problems, the system dynamics

(xs(t) = f(xs(t), us(t)), xs(0) = x0) are not the onlyconstraints of the problem. Given finite k constraints on thestate and/or control variables, the constrained optimal controlproblem can be cast as

min.∫ T

0

l(τ, xs(τ), us(τ))dτ +m(xs(T ))

s.t. xs(t) = f(xs(t), us(t)), xs(0) = x0cj(xs(t), us(t), t) ≥ 0, t ∈ [0, T ], j ∈ {1, . . . , k}

(16)where c(·) is the constraint function.

To solve this problem we incorporate the constraints usingthe barrier functional method [10]. The direct translation isclearly

min.∫ T

0

l(τ, xs(τ), us(τ))

− ε∑j

log(cj(τ, xs(τ), us(τ)))dτ +m(xs(T ))

s.t. xs(t) = f(xs(t), us(t)), xs(0) = x0

(17)

The main difficulty of solving this problem is the fact thatit is not possible to evaluate the objective in (17) unless ξis a feasible curve (i.e. satisfies the constraints), since thelogarithmic function is not defined for non-positive arguments.To overcame that we define an approximate log barrier functionβδ(·), 0 < δ ≤ 1

βδ(z) =

− log z z > δ

k − 1

k

[(z − kδ(k − 1)δ

)k− 1

]− log δ z ≤ δ

(18)where k > 1 is an even integer. Usually, k = 2 is used. Thisfunction is similar to the log barrier function for z < 0 butexpands its domain from (0,∞) to (−∞,∞).

The final formulation of of the constrained optimal controlproblem (16) becomes

min.∫ T

0

l(τ, xs(τ), us(τ))

+ ε∑j

βδ(cj(τ, xs(τ), us(τ)))dτ +m(xs(T ))

s.t. xs(t) = f(xs(t), us(t)), xs(0) = x0

(19)

where we start by choosing reasonably large values for ε andδ and reduce them in every algorithm iteration to force thetrajectories in to the valid region.

The fact that (18) tends to infinity as z tends to infinitymay cause some undesired effects. For example, in a obstacleavoidance constraint this fact may result in a domination ofthe negative part in the cost integral and effectively putting areward on staying away from de obstacle as far as possible.To overcome that, we make use of the ”hockey stick” function[11],[5]

σ(z) =

{tanh(z) z ≥ 0

z otherwise (20)

In this case, for the constraints that have this problem, wedefine a new barrier functional βδ(z) = βδ(σ(z)) that behavesas the standard approximate barrier function (18) for small andnegative z, but goes to zero as z goes to infinity.

IV. PROBLEM FORMULATION

In this section we will define the various optimizationproblems that we are interested to solve, namely by definingthe cost functional. The constraints added and the parametersneeded by the optimization algorithm (Section III) is alsodiscussed here.

A. Minimum Time TrajectoriesIn minimum time optimal control problems, the goal is to

drive the vehicle from a given initial state to a desired finalone in minimum time. A valid choice for the integral cost termin (16) can be

l(t, xs(t), us(t)) = 1 (21)

and the optimal solution would be computed by optimizingthe final time T . However, the final time T cannot be directlyused as an optimization variable in PRONTO. So, to compute

5

Algorithm 2 Minimum Time Algorithm using PRONTOgiven initial small final time Tfor i = 1, 2, 3, . . . do

Solve problem using PRONTO, with final time Tif converged then

Stop for cycleend ifIncrease T

end for

it we solve several optimization problems with different finaltimes. The algorithm is the following

The idea is to start with a small final time T and keepincreasing it until Pronto converges, in which case we have theoptimal solution. However, sometimes the PRONTO algorithmcan converge even when the solution does not satisfy theconstraints or the final position of the vehicle is far from thedesired one. In those cases we do not consider it as validsolution and continue to increase T .

Another issue that arises with the chosen integral cost term(21) is that it needs to be twice differentiable in us, asexpected by PRONTO. As we can see from (21), this is notthe case. Although, as we will need to add constraints to theoptimization problem (Section IV-C) and convert it to (19), thenew integral cost term will become twice differentiable in us.

The terminal cost will be

m(xs(T )) = ‖xs(T )− xsdes(T )‖2P1/2 (22)

where ‖a‖2K = a>Ka.

B. Minimum Energy Trajectories

In these type of problems, the idea is to generate trajectoriesthat minimize the energy usage throughout the mission. Moreprecisely, we want to minimize the electrical energy thebatteries need to use in order to drive the vehicle.

Taking the standard DC motor equation

LadI

dt+RaI = V −Keω (23)

and neglecting the dynamics, since the terminal inductance Lais small, we get the relation between the rotational velocitiesof the propellers and the armature voltage as

Vps,sb = RaIps,sb+Keωps,sb = RaIps,sb+Ke2πnps,sb (24)

where Ke is the electrical constant of the DC motor, Ra isthe resistance of the DC motor, and Vps,sb and Ips,sb arethe armature voltage and current of the portside/starboard DCmotor, respectively.

To determine the armature currents as a function of therotational velocities, some experimental measures where madewith the Medusa vehicle and a 3rd order polynomial was fitted

to the data obtained [12]

Ips = 0.1142e−3n3ps + 0.4355e−3n2ps + 23.5331e−3nps(25)

+ 8.2628e−3

Isb = 0.1175e−3n3sb + 0.5602e−3n2sb + 29.4185e−3nsb(26)

+ 14.3139e−3

The integral cost term for the optimization problem (16)then becomes

l(t, xs(t), us(t)) = Vps(t)Ips(t) + Vsb(t)Isb(t) =

= (RaIps(t) +Ke2πnps(t))Ips(t)

+ (RaIsb(t) +Ke2πnsb(t))Isb(t)

(27)

and the terminal cost is the same as in (22).

C. Constraints1) Constraints on the inputs: The use of the open-water

coefficients for the propellers’ model implies that their rota-tional velocity has to be positive. Also, due to the thruster’sspecifications we know that their top speed is approximately75 rotation per second.

So, the constraint function for nps becomes

cnps(xs(t), us(t)) = −(nps − nmax)(nps − nmin) (28)

which is a second order polynomial that is positive betweennmin and nmax and negative otherwise, as expected by theoptimization algorithm. We chose nmax = 75 and nmin = 1.The constraint for the other input (nsb) is defined in the sameway.

2) Obstacle Avoidance: One of the requirements is thatthe generated trajectories are collision free. To that end, it isnecessary to add a constraint on the vehicle spatial position.

Considering an obstacle with spatial coordinates (xobs, yobs)in reference frame {I}, we can define the constraint as

cobs(xs(t), us(t)) =(x(t)− xobs)2

D2obs

+(y(t)− yobs)2

D2obs

− 1

(29)with Dobs = robs+rv , where robs is the radius of the obstacleand rv is the safety distance that must be kept between theobstacle and the vehicle.

There will be as much constraints as there are obstacles.

V. MULTIPLE VEHICLES

In multiple vehicles mission scenarios, the trajectory plannerhas to take into account the different dynamics of each vehicleas well as making sure they do not collide with each other.

In this work we will focus mainly on two types of cooper-ative missions. One of them is when the goal of each vehicleis to go from an initial to a final position independently. Inother words, the vehicles only need to cooperate in order toavoid collisions with each other, while optimizing the overallmission. Another type of mission is when we want to movea group of vehicles from one area to another, but in a way

6

that the vehicles maintain a specified geometrical formationbetween each other.

The simplest way to take into account multiple vehiclesin the optimization problem is by increasing the state vector.More precisely, consider Nv vehicles then the new state vectorbecomes

xs =

x[1]s

x[2]s

...x[Nv ]s

(30)

where x[i]s , i ∈ {1, . . . , Nv} is the state vector of the i-

th vehicle. The control vector and other vectors and matricesneeded by PRONTO (Qr, Rr, P1, . . .) are also increased in thesame way. With this approach is possible to include vehiclesthat have different kinematic and dynamic equations.

A. Inter-vehicle collision avoidance

As said before we need to make sure that the optimaltrajectory is collision free. We do that by adding constraintsto the position of the vehicles, similar to what was done inSection IV-C2.

Thus, for each pair of vehicles {i, j} we define the constraint

ccol(x[i]s (t), x[j]s (t)) =

(x[i](t)− x[j](t))2

D2v

+(y[i](t)− y[j](t))2

D2v

− 1

(31)with i, j ∈ {1, . . . , Nv} and where Dv is the minimum safetydistance that must be kept between two vehicles. The totalnumber of constrains added is

(Nv2

).

B. Formation Motion

In some interesting scenarios, it is necessary for the vehiclesto perform trajectories where they need to keep a certainconfiguration between each other. We do that by defining aleader vehicle that the others have to follow from a certainrelative position, while the leader moves from an initial toa final position. In PRONTO, this is done by adding apenalization to the integral cost term, l(xs(t), us(t)), in (19).

Defining (x[i]w (t), y

[i]w (t)) as the desired position, expressed

in {I}, the i-th vehicle should be at time t (see Figure 3), theadded penalization term becomes

lform(xs(t),us(t)) =Nv−1∑i=1

(x[i](t)− x[i]w (t))2 + (y[i](t)− y[i]w (t))2

(32)To compute x

[i]w (t) and y

[i]w (t) in each instant, a coordinate

(x[i]b , y

[i]b ) is defined for each vehicle in the leader body

reference frame, which depends on the desired formation (e.g.,in a side-by-side formation we could have x

[i]b = 0 and

y[i]b = 10).

Fig. 3. Diagram that represents the vectors used in the formation missions.

The mapping between them becomes

x[i]w (t) = x[l] + d[i]l cos(θ[i])

y[i]w (t) = y[l] + d[i]l sin(θ[i])

(33)

with

d[i]l =

∥∥∥[x[i]b y[i]b

]∥∥∥ (34)

θ[i] = ψ[l] + atan2(y[i]b , x

[i]b ) (35)

where (x[l], y[l]) and ψ[l] are, respectively the position andheading of the leader vehicle (see Figure 3).

VI. RESULTS

To assess the performance of the proposed implementationseveral simulations were made with simple examples thathighlight the different aspects of the planner.

A. Example 1The objective of this first example is to compare the optimal

solutions for the time and energy minimization problems in thepresence of space variant water currents. There will be onlyone vehicle. and the components of the ocean current velocityare

uc = −0.8 tanh yvc = 0

(36)

In other words, the current velocity only has a componentin x that depends on the y position. The maximum value is0.8ms−1. This example is quite similar to one in [13].

Figure 4 shows the obtained results for the minimum timeof 22.5s.

By looking at the results, we can see that the vehicle uses thewater current to its advantage by trying to get as fast as it can toa place with favourable currents (positive y), instead of trying

7

0 5 10 15 20 25 30 35 40

−2

−1

0

1

2

3

x [m]

y [m

]

T = 22.5sT = 33sT = 43s

(a) Vehicles’ trajectories.

0 5 10 15 20 2510

20

30

40

50

60

70

80

time [s]

rota

tiona

l vel

ocity

[rps

]

npsnsb

(b) Propellers’ rotational velocities.

24 26 28 30 32 34 36 38 40 42 440

2

4

6

8

10

12

14

Time [s]

Ene

rgy

[Wh]

(c) Trade-off between time and energy.

Fig. 4. Simulation results for Example 1

to go against them. We can also see that in order to minimizethe consumed energy, the vehicle opts to do a wider curve soit does not need to turn so fast, which is energy expensive. Thevehicle also goes upper to obtain stronger favourable currents.

It should also be noted that the constraints on the propellers’rotational velocities are satisfied and the trade-off between timeand energy is in accordance with the expected results, sinceas time increases a smaller energy is used.

B. Example 2The objective of this example is to see how well the

obstacle avoidance constraint works. In this scenario, to make a

0 5 10 15 20 25 30 35 40

−2

−1

0

1

2

3

4

5

x [m]

y [m

]

Fig. 5. Vehicles’ trajectories for Example 2.

comparison with the previous case, this is example is similarto the previous one with the exception that an obstacle wasadded.

Figure 5 shows the optimal trajectory for the minimumenergy problem with a simulation time of T = 35s.

As can be seen, the added obstacle was clearly in the wayof the previous solution (Figure 4(a)). The new trajectorysuccessfully avoids the obstacle by passing as close to it asit can. This solution, however, is much more energy expensivethan the previous one, since the vehicle needs to rotate faster,especially after passing the obstacle.

C. Example 3In this simulation we will have two vehicles that need to go

from an initial position to a final one. These positions werechosen in way that the vehicles need to cooperate in orderto not crash whit each other. This way, we can evaluate theperformance of the inter-vehicle collision constraint. There isno water current in this example.

Figure 6 shows the optimal trajectory obtained for theminimum time (56s) and minimum energy (65.5s) problems.

As can be seen, to avoid having a collision the vehiclesdo not go in a straight line, but instead they opt to do alittle curved trajectory. In the energy minimization problemthe curve is wider so that the variation of angular speed (yawrate) is not as big. This conclusion is actually similar to theone made in Example 1.

D. Example 4For the final example, we will solve a minimum time optimal

control problem where the vehicles need to pass trough severalway-points before reaching the final position. More precisely,we want to do a square movement. To do that we are going tosolve several optimization problems between each consecutiveway-point.

In this example, there will be 3 vehicles that have to movein a triangular formation. The desired coordinates that thevehicles need to be on the leader body reference frame (seeSection V-B) are

x[1]b = 12m y

[1]b = 6m

x[2]b = 0m y

[2]b = 12m

(37)

8

−40 −35 −30 −25 −20 −15 −10 −5 0−20

−15

−10

−5

0

5

10

15

20

x [m]

y [m

]

vehicle 1vehicle 2

(a) Vehicles’ trajectories for the minimum time problem.

−40 −35 −30 −25 −20 −15 −10 −5 0−20

−15

−10

−5

0

5

10

15

20

x [m]

y [m

]

vehicle 1vehicle 2

(b) Vehicles’ trajectories for the minimum energy prob-lem.

Fig. 6. Simulation results for Example 3

−20 −10 0 10 20 30 40 50 60

−40

−30

−20

−10

0

10

20

30

40

50

x [m]

y [m

]

vehicle 1vehicle 2vehicle 3

Fig. 7. Vehicles’ trajectories for Example 4.

which means that one of the vehicles has to be in front of theformation while the other stays in side with the leader. Thereis no water current.

Figure 7 shows the optimal trajectory obtained. The mini-mum time was 251.5s.

Looking at the results, we see that the approach introducedto do formation movements works quite well. In fact, only inthe curve part of the generated trajectory is that the outsidevehicle (vehicle 3) seems to get out of formation, but only bya small distance.

VII. CONCLUSIONS

In summary, this work addressed the problem of developinga motion planner for single and multiple autonomous vehicles.A simple model for an AUV was used, along with a pro-peller model based on the open-water coefficients. We solvedminimum-time and minimum-energy optimal control problemsmaking use of the Projection Operator Approach with somemodifications so constraints could be added.

The generated trajectories were feasible in terms of thevehicles’ dynamics and input constraints and they were alsofree of obstacles and inter-vehicles collisions. An approach toperform mission with formation motions was developed byadding a term to the integral cost of the optimization problem.The proposed implementation has shown good behaviour inthe computational simulations made.

A. Future WorkThere are several issues that could be addressed in future

research in this area, such as the study of time variantcurrents and other current configurations and the case ofmoving obstacles and communication constraints. Also, thetime performance of the implementation should be improvedfor the planning to be done in real time so it can adapt toenvironment changes.

REFERENCES

[1] L. Merino, F. Caballero, J. Martnez-de Dios, and A. Ollero, “Co-operative Fire Detection using Unmanned Aerial Vehicles,” in IEEEInternational Conference on Robotics and Automation, April 2005.

[2] A. Birk, G. Antonelli, A. Pascoal, and A. Caffaz, “Cognitive Coop-erative Control for Autonomous Underwater Vehicles: An overviewof achievements in the first project year,” in Proc. 9th InternationalConference on Computer Applications and Information Technology inthe Maritime Industries (COMPIT), April 2010.

[3] W. Dong and Y. Guo, “New Trajectory Generation Methods for Non-holonomic Mobile Robots,” in Proceedings of the 2005 InternationalSymposium on Collaborative Technologies and Systems, May 2005.

[4] J. Hauser, “A projection operator approach to optimization of trajectoryfunctionals,” in IFAC World Congress, Barcelona, 2002.

[5] A. J. Hausler, A. Saccon, A. P. Aguiar, J. Hauser, and A. M. Pascoal,“Cooperative Motion Planning for Multiple Autonomous Marine Vehi-cles,” in 9th Conference on Maneuvering and Control of Marine Craft(MCMC), Sep. 2012.

[6] T. I. Fossen, Marine Control Systems: Guidance, Navigation andControl of Ships, Rigs and Underwater Vehicles. Marine Cybernetics,2002.

[7] A. J. Hausler, A. Saccon, J. Hauser, A. M. Pascoal, and A. P.Aguiar, “Four-Quadrant Propeller Modeling: A Low-Order HarmonicApproximation,” in Proceedings of the 9th IFAC Conference on ControlApplications in Marine Science (CAMS), Osaka, Japan, Sep. 2013.

[8] B. Anderson and J. B. Moore, Optimal Control: Linear QuadraticMethods. Pentrice-Hall Internacional, Inc., 1989.

[9] J. Nocedal and S. J. Wright, Numerical optimization, 2nd ed. Springer,2006.

[10] J. Hauser and A. Saccon, “A Barrier Function Method for the Optimiza-tion of Trajectory Functionals with Constraints,” in IEEE Conferenceon Decision and Control, 2006.

[11] A. Saccon, A. P. Aguiar, A. J. Hausler, J. Hauser, and A. M. Pascoal,“Constrained Motion Planning for Multiple Vehicles on SE(3),” in 51stIEEE Conference on Decision and Control, 2012.

9

[12] A. J. Hausler, “A Propeller Model for the Seabotix HPDC1507Thruster,” Instituto Superior Tecnico, ISR/IST Internal Report, Nov.2011.

[13] A. E. Bryson, Jr. and Y.-C. Ho, Applied Optimal Control: Optimization,Estimation and Control. Taylor & Francis, 1975.

[14] S. Boyd and L. Vandenberghe, Convex Optimization. CambridgeUniversity Press, 2004.

[15] J. Hauser and D. Meyer, “The trajectory manifold of a nonlinear controlsystem,” in IEEE Conference on Decision and Control, 1998.

[16] 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, 2007.

[17] J. Hauser, “On the computation of optimal state transfers with appli-cation to the control of quantum spin systems,” in Proceedings of the2003 American Control Conference (ACC), 2003, pp. 2169–2174.

[18] A. Saccon, “Minimum time maneuver for a nonholonomic car with ac-celeration constraints: Preliminary results,” in Proceedings of the 2005IEEE International Symposium on Intelligent Control, MediterraneanConference on Control and Automation, 2005, pp. 1337–1342.

[19] A. Rucco, G. Notarstefano, and J. Hauser, “Computing minimum lap-time trajectories for a single-track car with load transfer,” in 51stConference on Decision and Control, 2012, pp. 6321–6326.

[20] I. Spangelo and O. Egeland, “Trajectory Planning and Collision Avoid-ance for Underwater Vehicles Using Optimal Control,” in IEEE Journalof Oceanic Engineering, 1994.

[21] D. M. O. Martnez, “Nonlinear optimal control strategies for maneuver-ability analysis of manned submarines,” Ph.D. dissertation, UniversidadPolitcnica de Cartagena, 2011.

[22] T. F. B. Mestre, “Cooperative control of multiple marine robots forassisted human diving operations,” Master’s thesis, Instituto SuperiorTecnico, 2010.

[23] A. F. S. Meira, “Cooperative navigation of multiple autonomous un-derwater vehicles with logic based communication,” Master’s thesis,Instituto Superior Tecnico, 2011.

[24] J. M. dos Santos Ribeiro, “Motion control of single and multiple au-tonomous marine vehicles,” Master’s thesis, Instituto Superior Tecnico,2011.

[25] J. F. M. Almeida, “Controlo cooperativo de mltiplos veculos ocenicos,”Master’s thesis, Instituto Superior Tecnico, 2007.