TRAJECTORY OPTIMIZATION USING PSEUDOSPECTRAL METHODSFOR A MULTIPLE AUTONOMOUS UNDERWATER VEHICLE

TARGET TRACKING PROBLEM

By

ADAM J. FRANKLIN

A THESIS PRESENTED TO THE GRADUATE SCHOOLOF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT

OF THE REQUIREMENTS FOR THE DEGREE OFMASTER OF SCIENCE

UNIVERSITY OF FLORIDA

2012

c⃝ 2012 Adam J. Franklin

2

To my wife Kristie and my parents Mark and Kathy

3

ACKNOWLEDGMENTS

I would like to express my appreciation to my supervisory committee chair and

advisor Dr. Anil V. Rao for his support, encouragement and technical guidance

throughout my research. Also, I would like to thank my committee co-chair Dr. Warren E.

Dixon for his teaching and support during my course of study. I would like to also thank

Michael A. Patterson for helping me to better understand my research. Most importantly,

I would like to express my deepest appreciation to my wife Kristie and my parents Mark

and Kathy. Their love and support made this thesis possible.

4

TABLE OF CONTENTS

page

ACKNOWLEDGMENTS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

LIST OF TABLES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

LIST OF FIGURES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

ABSTRACT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

CHAPTER

1 INTRODUCTION . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

1.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101.2 Literature Review . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

1.2.1 Path Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101.2.2 Multiple AUVs Coordination . . . . . . . . . . . . . . . . . . . . . . 111.2.3 Target Detection and Localization . . . . . . . . . . . . . . . . . . . 13

1.3 About the Thesis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141.4 Thesis Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

2 MULTIPLE AUV TARGET TRACKING PROBLEM . . . . . . . . . . . . . . . . 15

2.1 Structure of an Optimal Control Problem . . . . . . . . . . . . . . . . . . . 152.1.1 First Order Optimality Conditions . . . . . . . . . . . . . . . . . . . 172.1.2 Transversality Conditions . . . . . . . . . . . . . . . . . . . . . . . . 17

2.2 Methods for Solving Optimal Control Problems . . . . . . . . . . . . . . . 182.2.1 Indirect Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . 182.2.2 Direct Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

2.3 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 192.3.1 Equations of Motion . . . . . . . . . . . . . . . . . . . . . . . . . . 202.3.2 Boundary Conditions . . . . . . . . . . . . . . . . . . . . . . . . . . 222.3.3 Path Constraints . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

2.3.3.1 State inequality constraints . . . . . . . . . . . . . . . . . 242.3.3.2 Target collision avoidance constraints . . . . . . . . . . . 252.3.3.3 Friendly collision avoidance constraints . . . . . . . . . . 252.3.3.4 Control constraints . . . . . . . . . . . . . . . . . . . . . . 252.3.3.5 Cost functional . . . . . . . . . . . . . . . . . . . . . . . . 26

3 PSEUDOSPECTRAL METHODS AND GPOPS . . . . . . . . . . . . . . . . . . 27

3.1 LG, LGR, and LRL Collocation Points . . . . . . . . . . . . . . . . . . . . 273.2 Formulation of Pseudospectral Method Using LGR Points . . . . . . . . . 283.3 hp-Adaptive Pseudospectral Method . . . . . . . . . . . . . . . . . . . . . 293.4 Problem Formulation in GPOS . . . . . . . . . . . . . . . . . . . . . . . . 30

3.4.1 Objective and Input Parameters . . . . . . . . . . . . . . . . . . . . 30

5

3.4.2 Cost Function Formulation . . . . . . . . . . . . . . . . . . . . . . . 313.4.3 Path Constraint Formulation . . . . . . . . . . . . . . . . . . . . . . 31

4 RESULTS AND DISCUSSION . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

4.1 Underwater Target . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 344.1.1 Analysis of Results . . . . . . . . . . . . . . . . . . . . . . . . . . . 344.1.2 Validation of Results . . . . . . . . . . . . . . . . . . . . . . . . . . 34

4.2 Surface Target . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 364.2.1 Analysis of Results . . . . . . . . . . . . . . . . . . . . . . . . . . . 364.2.2 Validation of Results . . . . . . . . . . . . . . . . . . . . . . . . . . 36

5 CONCLUSION . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47

REFERENCES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48

BIOGRAPHICAL SKETCH . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

6

LIST OF TABLES

Table page

2-1 Notation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

2-2 Equations of motion parameters . . . . . . . . . . . . . . . . . . . . . . . . . . 21

3-1 User-specified initial and final states of the four AUVs . . . . . . . . . . . . . . 30

4-1 Lagrange cost validation for an underwater target . . . . . . . . . . . . . . . . . 35

4-2 Lagrange cost validation for a surface target . . . . . . . . . . . . . . . . . . . . 36

7

LIST OF FIGURES

Figure page

3-1 Schematic showing LGL, LGR and LG collocation points . . . . . . . . . . . . . 33

4-1 Three-dimensional trajectory of AUVs tracking the underwater target . . . . . . 38

4-2 Top-down view of trajectory for tracking the underwater traget . . . . . . . . . . 38

4-3 Forward position versus time for tracking the underwater traget . . . . . . . . . 39

4-4 Lateral position versus time for tracking the underwater traget . . . . . . . . . . 39

4-5 Depth versus time for tracking the underwater traget . . . . . . . . . . . . . . . 40

4-6 The lagrange cost versus time for the underwater target . . . . . . . . . . . . . 40

4-7 Distance from AUVs to target versus time for the underwater target . . . . . . . 41

4-8 Average distance from one AUV to the others versus time for the underwatertarget . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42

4-9 Three-dimensional trajectory of AUVs tracking the surface target . . . . . . . . 42

4-10 Top-down view of trajectory for tracking the surface traget . . . . . . . . . . . . 43

4-11 Forward position versus time for tracking the surface traget . . . . . . . . . . . 43

4-12 Lateral position versus time for tracking the surface traget . . . . . . . . . . . . 44

4-13 Depth versus time for tracking the surface traget . . . . . . . . . . . . . . . . . 44

4-14 The lagrange cost versus time for the surface target . . . . . . . . . . . . . . . 45

4-15 Distance from AUVs to target versus time for the surface target . . . . . . . . . 45

4-16 Average distance from one AUV to the others versus time for the surface target 46

8

Abstract of Thesis Presented to the Graduate Schoolof the University of Florida in Partial Fulfillment of the

Requirements for the Degree of Master of Science

TRAJECTORY OPTIMIZATION USING PSEUDOSPECTRAL METHODSFOR A MULTIPLE AUTONOMOUS UNDERWATER VEHICLE

TARGET TRACKING PROBLEM

By

Adam J. Franklin

May 2012

Chair: Anil V. RaoCochair: Warren E. DixonMajor: Mechanical Engineering

In this work, a method is presented for solving a multiple AUV target tracking

problem using pseudospectral methods. The multiple AUV target tracking problem

involves generating a trajectory that minimizes the sum of the distances from each

AUV to the target over the entire trajectory. The trajectory generated for each vehicle

must not violate the distance constraint between any other vehicle or the target. The

multiple AUV target tracking problem is solved as an optimal control problem using the

open-source optimal control software GPOPS and a nonlinear programming problem

solver SNOPT. The results of the multiple AUVs target tracking problem for two different

target scenarios are analyzed and validated.

9

CHAPTER 1INTRODUCTION

1.1 Motivation

The Department of Transportation is responsible for the safety of more than 300

sea and river ports with more than 3,700 terminals [1]. The United States Navy has

285 deployable ships which, when underway in hostile waters, are possible targets for

terrorist attacks [2]. The protection of these assets is a top priority of the Departments

of Transportation and Defense, consuming money, equipment and manpower. Currently,

the responsibility of monitoring the sonar sweeps of ports and waters surrounding ships

for possible threats falls to the individual on watch. This dull and repetative task is better

suited for autonomous underwater vehicles (AUVs).

1.2 Literature Review

1.2.1 Path Planning

AUVs are being used to complete tasks such as ocean sampling, mapping, ocean

floor surveying, oceanographic data collection, minesweeping and port and ship security.

In order to accomplish these tasks, AUVs must be able to track a desired path. The two

most common divisions of path planning are trajectory tracking and path following. [3, 4]

Trajectory tracking refers to driving a vehicle to track a time-parameterized reference

curve. Ghommam, J. and others [3] proposed a trajectory tracking control that could

be applied to a group of underactuated AUVs. Lyapunov-based control techniques had

been used for trajectory tracking of a single AUV, but Ghommam, J. and others [3] were

able to decentralize the control to a group of AUVs by using graph theory. Through

decentralization, the control structure takes into account the dynamics of all vehicles

as well as the constraints of the inter-vehicle communications network. Their work was

validated by simulating a group of three underwater AUVs with good tracking results.

The goal of path following is to force a vehicle to converge to and follow a

desired spatial path, without regard to time. Wang, Y. and others [4] developed a

10

backstepping-based control law for accurate path following for an underactuated AUV.

They achieved this control law by developing a path tracking control from the kinematics

and then extending the control to the AUV dynamics through backstepping techniques.

Wang, Y. and others [4] proved the convergence of the vehicle’s trajectory to the desired

path via Lyapunov stability analysis. However, their controller relied heavily on complex

dynamics that may not always be known.

1.2.2 Multiple AUVs Coordination

In many applications, a given task is too complicated to be accomplished by a single

AUV. Therefore, multiple AUVs, working together, are required to complete the task.

Furthermore, a system of multiple AUVs is more robust than a single vehicle, because a

team of vehicles provides redundancy. However, one fundamental problem of a system

of AUVs is vehicle coordination while completing an assigned task. Vehicle coordination

strategies using either a behavioral, a virtual structure, queues and artificial potential

trenches or a leader-follower approach have been proposed in literature [5–10].

Behavioral coordination is a distributed autonomous control approach for a system

of multiple vehicles. Artificial force laws are defined between vehicles in a group.

These laws are inverse-power force laws, incorporating both attraction and repulsion.

The force laws are well-defined and they reflect the interactions among vehicles. An

individual vehicle’s motion is dictated by the forces imposed by other vehicles in the

system. Furthermore, this approach is distributed because each vehicle determines its

motion by observing the force laws between itself and the other vehicles. Reif, J. and

others [5] studied the behavioral cooperation approach teams of multiple robots. They

obtained computer simulated results where the system of multiple robots converged to

the desired formation.

In virtual structure coordination, the control is derived in three steps. First, the

desired dynamics of the virtual structure is defined. Second, the motion of the virtual

structure is translated into the desired motion for each vehicle. Finally, tracking controls

11

for each vehicle are derived. Beard, R. and others [6] developed a virtual structure

approach for the coordination of multiple spacecrafts. The strength of the virtual

structure approach is that it is fairly easy to prescribe a coordinated behavior for the

formation of spacecrafts. Furthermore, the feedback to the virtual structure is naturally

defined, for each spacecraft reacts according to the positions of others in the formation.

However, the disadvantage of this approach is that its current development lends itself to

a centralized implementation.

Ge. S, and others [8] developed an approach for representing a team of multiple

vehicles in terms of queues and vertices, rather than nodes, as well as the introduction

of a new concept of artificial potential trenches, for effectively controlling the formation.

This approach improves the scalability and flexibility of a vehicle team when the number

of vehicles in the formation changes and allows the formation to adapt to obstacles.

Furthermore, the team is protected against failures of individual vehicles, for the

formation scales and adapts automatically. Through simulation, Ge. S, and others

[8] show the communication bandwidth between vehicles places an upper bound on the

maximum number of vehicles in the formation.

In the leader-follower approach to multiple vehicle coordination, one vehicle is

designated as the leader. The leader moves along a predefined trajectory while the

other vehicles, the followers, maintain a desired distance and orientation with respect to

the leader [9]. In the work by Emrani, S. and others [10], the leader-follower formation

coordination of multiple AUVs is expanded upon by incorporating uncertainties in the

hydrodynamics. To deal with these uncertainties, an adaptive control law based on the

inverse dynamics of the plant was developed. They go on to provide a Lyaponov-based

closed-loop stability analysis for the proposed controller. Simulation results have

demonstrated the effectiveness of the proposed approach for leader-follower formation

control of the AUVs.

12

1.2.3 Target Detection and Localization

In applications where AUVs are minesweeping or providing security for ports or

ships, the ability to detect and localize targets is crucial. In recent years, a majority of

the research to improve these abilities has been in improving either the sensors and

data analysis techniques or the information sharing between multiple AUVs [11, 12].

Wiegert, R. and others [11] worked to develop a new magnetic sensor system for

an AUV in order to improve mine detection in litoral waters. The previous method, called

the scalar triangulation and ranging (STAR) method, used rotationally invariant scalar

quantities in order to detect magnetic targets. To improve upon this method, Weigert, R.

and others [11] used multi-tensor data from magnetic gradiometers, which exploited the

rotationally invariant and robust symmetry properties of the gradient contraction scalar

field. Simulation results showed an improvement in magnetic target detection over the

original STAR method.

Rauch, C. and others [12] worked to develop a module that could be added to

an AUV, allowing the vehicle to hold six to ten deployable sonar transponders. These

markers were to be jettisoned near a target and the deploying AUV would survey the

markers to determine their relative position to the target. Subsequent AUVs were then

given the best known coordinates of the target and the relative offsets to the markers,

allowing for guidance to the target. Rauch, C and others [12] also worked to improve the

acoustic signal processing required to survey the markers and then communicate their

relative locations to other AUVs.

Other work in target localization has been done not by improving sensors, but by

improving how information is shared between vehicles. Belbachir, A. and others [13]

worked to define a control strategy which adapts each AUV’s motion according to its

or other vehicles’ sensory information. Their goal was to minimize the range between

vehicles during information exchanges in order to reduce exchange time. Belbachir, A.

13

and others [13] where able to validated their control strategy through experiment, using

two AUVs and one surface vehicle to act as a relay between the two AUVs.

1.3 About the Thesis

In this research, multiple AUVs patrol a region of interest (ROI) for unidentified

contacts. Upon detection a unidentified contact, all vehicles are to close with and track

the target until it leaves the ROI. An hp-adaptive pseudospectral method that uses

Radau collocation points is implemented to generate the trajectories that minimizes

the sum of the distances from each AUV to the target. These trajectories satisfy the

dynamics of the vehicles. This work could be adapted so that instead of tracking the

target unitl it leaves the ROI, the AUVs could block, observe or even disable the target.

The research can be used to assist not only the Department of Transportation

in protecting domestic ports and waterways, but also the Department of Defense

in protecting ships in hostile waters. With future work in this area, near real-time

implementation of optimal control is possible.

1.4 Thesis Outline

This thesis is organized as follows. Chapter 1 is an introduction on the motivation

for the research of target tracking using multiple AUVs and a review of the previous

work related to the research. In Chapter 2, the multiple AUVs target tracking problem

is formulated along with a general description of optimal control problems with its

optimality and transversality conditions. Chapter 3 gives a brief description of the Radau

psedospectral and hp-adaptive methods and describes the formulation of the optimal

control problem. In Chapter 4 the results of this research are presented and discussed.

14

CHAPTER 2MULTIPLE AUV TARGET TRACKING PROBLEM

This chapter provides a brief description of and the methods for solving an optimal

control problem. Furthermore, the multiple AUV target tracking problem is described.

Finally, this chapter describes the equations of motion for the vehicles and the cost

function formulation.

2.1 Structure of an Optimal Control Problem

An optimal control problem is comprised of the dynamic equations of motion, the

objective functional, boundary conditions and path constraints. Below is a table of the

notation used in describing the formulation of an optimal control problem.

Table 2-1. NotationSymbol Descriptiont0 Initial Timetf Final Timex(t0) State Value at the Initial Time, t0x(tf ) State Value at the Final Time, tfJ Cost FunctionalJa Augmented Cost FunctionalQ Mayer CostL Lagrange Costϕ Boundary Conditiong Path ConstraintH Hamiltonianϑ Lagrange Multiplier for Boundary Conditionλ Costate of the Differential Equation

The dynamic equations of motion need to be continuous and differentiable. The

dynamic equations of motion are converted into state space representation in order

to have the individual state equations and their relation to the control input. Therefore,

the number of equations is equal to the number of states in the problem. The dynamic

equations of motion are represented as Equation (2–1) below,

x = f (x , u, t) (2–1)

15

where x(t) ∈ Rn is the state, u(t) ∈ Rp is the control, and t is the time.

The objective functional, also called the cost functional, is the parameter to be

optimized while solving the optimal control problem. The cost functional is a functional of

the state variables, control variables, and/or time. Specifically, the cost functional can be

the end point states, the end point time or the state and/or control over the entire period

of time. The part of the cost functional consisting of the end points is the Mayer cost

and the part consisting of state or control variables for the entire period of time is the

Lagrange cost.

J = Q(x(t0), t0, x(tf ), tf ) +

∫ tf

t0

L(x(t), u(t), t)dt (2–2)

In the above cost functional, Q(x(t0), t0, x(tf ), tf ) is the Mayer cost and L(x(t), u(t), t) is

the Lagrange cost.

The boundary conditions are the initial and terminal values of the states and

time, either known at the beginning of the problem or to be achieved. The boundary

conditions become the initial and terminal constraints of the optimal control problem.

ϕ(x(t0), t0, x(tf ), tf ) = 0 (2–3)

The path constraints are the linear or nonlinear constraints to be satisfied by the

trajectory of the system. They can be either equality or inequality constraints. The

nonlinear inequality constraints are represented as

g(x(t), u(t), t) ≤ 0 (2–4)

The nonlinear equality constraints are represented as

g(x(t), u(t), t) = 0 (2–5)

16

2.1.1 First Order Optimality Conditions

The augmented cost functional is formulated as

Ja = Q − ϑTϕ+

∫ tf

t0

[L+ λT (f − x)

]dt (2–6)

The Hamiltonian is represented as

H = L+ λT f = H(x ,λ, u) (2–7)

The augmented cost functional in terms of the Hamiltonian is given by

Ja = Q − ϑTϕ+

∫ tf

t0

[H − λT x

]dt (2–8)

The optimality condition for costate dynamics is given as

x =

[∂H

∂λ

]T(2–9)

The optimal control, u∗, is found using the condition given as[∂H

∂u

]T= 0 (2–10)

2.1.2 Transversality Conditions

The first variation of the augmented cost functional Ja is used to find the transversality

conditions. These conditions are used to solve the differential equations, which are

derived from the first order optimality conditions. There are different transversality

equations for the different boundary conditions on the state and time of the system. The

transversality equations are given below:

• For no boundary conditions on the state and time of the system, δϑ = 0, theboundary condition is ϕ(x(t0), t0, x(tf ), tf ) = 0

• For a fixed initial state, δx0 = 0, the costate at the initial time is given by

λ(t0) = −[

∂Q

∂x(t0)

]T+

[∂ϕ

∂x(t0)

]Tϑ

17

• For a fixed final state, δxf = 0, the costate at the final time is given by

λ(t0) =

[∂Q

∂x(tf )

]T−[

∂ϕ

∂x(tf )

]Tϑ

• For a fixed initial time, δt0 = 0, the Hamiltonian at the initial time is given by

H(t0) =∂Q

∂t0− ϑT

∂ϕ

∂t0

• For a fixed final time, δtf = 0, the Hamiltonian at the final time is given by

H(tf ) = −∂Q∂tf+ ϑT

∂ϕ

∂tf

2.2 Methods for Solving Optimal Control Problems

In order to solve an optimal control problem, the differential equations need to be

solved, subject to constraints, while optimizing a performance index. The two main

classifications of methods to solve such optimal control problems with path constraints

and boundary conditions are indirect methods and direct methods.

2.2.1 Indirect Methods

Indirect methods stem from the calculus of variations. Indirect methods require

solving a two-point boundary value problem. Here, the optimality conditions for solving

the optimal trajectory are derived. The first order optimality conditions are formed by

taking the first variation of the cost functional, using calculus of variations. This produces

the first-order differential equations for the states and the costates. The transversality

conditions often produce nonlinear equations which cannot be solved with the Ricatti

equation. Thus, these nonlinear equations form the nonlinear constraints of the problem.

Indirect methods convert the optimal control problem into a purely differential-

algebraic system of states, costates and dynamics. Hence, the optimal control problem

becomes a root finding problem, where a set of differential equations has to be solved

and roots have to be found to satisfy the constraints.

18

2.2.2 Direct Methods

With direct methods, all of the functions in the optimal control problem are

approximated then transcribed into a nonlinear optimization problem. By making

this approximation, the continuous time infinite-dimensional problem is conveted into a

nonlinear programming problem, which is a finite-dimensional problem. The quadrature

approximation of the integral is used in the continuous cost funtion as given below

J = Q +

∫ tf

t0

L dt ∼= Quadrature Approximation of Integral,

J ≈ Ja.(2–11)

The path constraints are evaluated only at specific points of the quadrature. The

pseudospectral method is a direct method of solving an optimal control problem, which

is explained in detail in Chapter 3.

2.3 Problem Formulation

Consider a homogenous group of A AUVs patrolling a region of interest (ROI). The

ROI for this problem is a rectangular prism. Each of the A AUVs is responsible for its

own subsection of the ROI, which is also a rectangular prism. The goal is for all of the

A AUVs to track a target from the time it enters the ROI to the time it leaves the ROI,

while obeying target and other AUV collision avoidance constraints and optimizing a

performance index. The performance index used in this problem is minimizing the sum

of the distance from each AUV to the target over the entire trajectory.

19

2.3.1 Equations of Motion

The translational kinematic equations for A AUVs maneuvering under the surface of

a flat body of water are

xi(t) = ui cos(ψi) cos(θi) + v1(cos(ψi) sin(ϕi) sin(θi)− cos(ϕi) sin(ψi))

+ wi(sin(ϕi) sin(ψi) + cos(ϕi) cos(ψi) sin(θi)),

yi(t) = ui cos(θi) sin(ψi) + vi(cos(ϕi) cos(ψi) + sin(ϕi) sin(ψi) sin(θi))

+ wi(− cos(ψi) sin(ϕi) + cos(ϕi) sin(ψi) sin(θi)),

zi(t) = −ui sin(θi) + vi cos(θi) sin(ϕi) + wi cos(ϕi) cos(θi),

(2–12)

where i = 1, ... ,A, xi(t) and yi(t) are the horizontal Cartesian components of position,

zi(t) are the inverted vertical Cartesian component of position (depth) and ϕi , θi and ψi

are the roll, pitch and yaw angles of the AUVs. The rotational kinematic equations for A

AUVs are

ψi(t) = qi sin(ϕi) sec(θi) + ri cos(ϕi) sec(θi),

θi(t) = qi cos(ϕi)− ri sin(ϕi),

ϕi(t) = pi + qi sin(ϕi) tan(θi) + ri cos(ϕi) tan(θi),

(2–13)

where i = 1, ... ,A, ϕi , θi and ψi are the roll, pitch and yaw angles, ui , vi and wi are the

velocities, and pi , qi and ri are the angular velocities of the AUVs.

The differential equations describing the six-degree of freedom motion of each AUV

is given below. The following equations of motion were adapted from the work of Arslan,

20

M. and others [14]

ui(t) =1

m(Fix + Xuuu1|ui |) + vi ri − wiqi ,

vi(t) =1

m(k2Miy + Yvvvi |vi |) + wipi − ui ri ,

wi(t) =1

m(k2Miz + Zwwwi |wi |) + uiqi − vipi ,

pi(t) =1

Ix(Mix + Kpppi |pi |)− (Iz − Iy)qi ri ,

qi(t) =1

Iy(Miy +Mqqqi |qi |)− (Ix − Iz)pi ri ,

ri(t) =1

Iz(Miz + Nrr ri |ri |)− (Iy − Ix)piqi ,

(2–14)

where i = 1, ... ,A. In this model, the control inputs to the AUVs are the thrust

Fix and the moments Mix , Miy and Miz . Below, Table 2-2 lists the parameters used in

Equation (2–14).

Table 2-2. Equations of motion parametersSymbol Value Descriptionm 30 Vehicle mass (kg)

Ix , Iy , Iz 0.177, 3.45, 3.45 Moments of inertia (kg m2)Xuu -3.9 Axial drag coef. (kg/m)Yvv -131 Cross-flow drag coef. (kg/m)Zww -131 Cross-flow drag coef. (kg/m)Kpp -0.13 Rolling resistance coef. (kg m2/rad2)Mqq -94 Rolling resistance coef. (kg m2/rad2)Nrr -94 Rolling resistance coef. (kg m2/rad2)k1 0.025 Ratio of the thrust to the rolling moment (m)k2 2 Reciprocal of dist. from fin shaft to CB (m−1)

21

2.3.2 Boundary Conditions

The initial conditions of each AUV’s position and orientation are given as

xi(t0) = xi0,

yi(t0) = yi0,

zi(t0) = zi0,

ϕi(t0) = ϕi0,

θi(t0) = ϕi0,

ψi(t0) = ϕi0,

(2–15)

where i = 1, ... ,A and t0 is the initial time. These are the initial positions and

orientations of the patrolling vehicles at the time the target enters the ROI. Next, the

initial conditions on the velocities and angular velocities of each vehicle are given as

ui(t0) = ui0,

vi(t0) = vi0,

wi(t0) = wi0,

pi(t0) = pi0,

qi(t0) = qi0,

ri(t0) = ri0,

(2–16)

where i = 1, ... ,A and t0 is the initial time. These are the initial velocities and angular

velocities of the patrolling AUVs at the time the target enters the ROI. Next, the terminal

22

conditions on the positions and the orientation for each vehicle are given as

xi(tf ) = xif ,

yi(tf ) = yif ,

zi(tf ) = zif ,

ϕi(tf ) = ϕif ,

θi(tf ) = ϕif ,

ψi(tf ) = ϕif ,

(2–17)

where i = 1, ... ,A and tf is the time when the target exists the ROI. Also, the terminal

conditions on the velocities and the angular velocities for each vehicle are given below

ui(tf ) = uif ,

vi(tf ) = vif ,

wi(tf ) = wif ,

pi(tf ) = pif ,

qi(tf ) = qif ,

ri(tf ) = rif ,

(2–18)

where i = 1, ... ,A and tf is the time when the target exists the ROI. At the start of the

problem, only the terminal velocities uif , vif and wif are known, and they are set to zero.

This is done to ensure the vehicles do not leave the ROI after the target egresses the

region. All of the remaining terminal conditions are left free and will be determined by

solving the tracking problem.

23

2.3.3 Path Constraints

2.3.3.1 State inequality constraints

In this problem, it is assumed the AUVs do not leave the ROI. Therefore, the state

inequality constraints are

xmin ≤xi(t) ≤ xmax ,

ymin ≤yi(t) ≤ ymax ,

0 ≤zi(t) ≤ zmax ,

(2–19)

where i = 1, ... ,A and xmin, xmax , ymin, ymax and zmax are the allowable limits on the

three-dimensional ROI. It should be noted that zmin is zero because it is assumed that

the AUVs will not break the surface of the water. Furthermore, the vehicles’ orientations,

velocities and angular velocities must stay within the operational limits given below

ϕmin ≤ϕi(t) ≤ ϕmax ,

θmin ≤θi(t) ≤ θmax ,

ψmin ≤ψi(t) ≤ ψmax ,

umin ≤ui(t) ≤ umax ,

vmin ≤vi(t) ≤ vmax ,

wmin ≤wi(t) ≤ wmax ,

pmin ≤pi(t) ≤ pmax ,

qmin ≤qi(t) ≤ qmax ,

rmin ≤ri(t) ≤ rmax ,

(2–20)

where i = 1, ... ,A and ϕmin, ϕmax , θmin, θmax , ψmin and ψmax are the limits on the

orientation of the vehicles, umin, umax , vmin, vmax , wmin and wmax are the velocity limits

of the vehicles and pmin, pmax , qmin, qmax , rmin and rmax are the angular velocity limits of

the vehicles.

24

2.3.3.2 Target collision avoidance constraints

It is a requirement that the AUVs do not collide with the target. The target collision

avoidance constraints are formulated as follows. Let (xi , yi , zi) be the Cartesian

coordinates of the A AUVs and (xT , yT , zT ) be the Cartesian coordinates of the target.

The number of target collision avoidance constraints will be A, where A is the number of

AUVs operating in the ROI. The target collision avoidance constraints are given as

(xi(t)− xT (t))2 + (yi(t)− yT (t))2 + (zi(t)− zT (t))2 ≤ d2T , (2–21)

where i = 1, ... ,A and dT is the minimum safe operating distance between an AUV and

the target.

2.3.3.3 Friendly collision avoidance constraints

Also, it is necessary that the AUVs do not collide with each other. The friendly

collision avoidance constraints are formulated as follows. Let (xa, ya, za) and (xb, yb, zb)

be the Cartesian coordinates of AUVs a and b, respectively, where a, b = 1, ... ,A,

and a = b. The number of friendly collision avoidance constraints will be A(A − 1)/2,

where A is the number of vehicles operating in the ROI. The friendly collision avoidance

constraints are given as

(xa(t)− xb(t))2 + (ya(t)− yb(t))2 + (za(t)− zb(t))2 ≤ d2A, (2–22)

where i = 1, ... ,A and dA is the minimum safe operating distance between two AUVs.

2.3.3.4 Control constraints

The following control constraints are applied to each AUV.

Fmin ≤ Fix (t) ≤Fmax ,

Mxmin ≤ Mix (t) ≤Mxmax ,

Mymin ≤ Miy (t) ≤Mymax ,

Mzmin ≤ Miy (t) ≤Mzmax ,

(2–23)

25

where i = 1, ... ,A and Fmin and Fmax correspond to the thrust limits on the AUVs and

Mxmin , Mxmax , Mymin , Mymax , Mzmin and Mzmax are the limits on the applied moments caused

by the AUVs’ fins about the vehicles’ body axes.

2.3.3.5 Cost functional

In this problem, the cost functional to be minimized is the sum of the distance from

each AUV to the target. The distance of each vehicle to the target is given by

di−t =√(xi − xt)2 + (yi − yt)2 + (zi − zt)2, (2–24)

where i = 1, ... ,A. Thus, the cost functional is

J =

∫ tf

t0

A∑i=1

di−t , (2–25)

26

CHAPTER 3PSEUDOSPECTRAL METHODS AND GPOPS

Pseudospectral methods are a class of direct trajectory optimization methods which

use direct collocation in order to transcribe the optimal control problem to a nonlinear

programming problem (NLP). Garg, D. and others [15] show that optimal control

problems can be solved using collocation at either Legendre-Gauss, Legendre-Gauss-

Radau or Legendre-Gauss-Lobatto points.

Psuedospectral methods use global polynomials to parameterize the state and

the control and then collocate the differential-algebraic equations using nodes from

a Gaussian quadrature. This method produces accurate solutions for problems

with smooth solutions. For problems with solutions which are not smooth, the time

interval from [−1, 1] is broken into several intervals, so that different global polynomial

approximations are used over each interval.

3.1 LG, LGR, and LRL Collocation Points

The three most common sets of collocation points are Legendre-Gauss (LG),

Legendre-Gauss-Radau (LGR), and Legendre-Gauss-Labatto (LGL) points. These

points are obtained from the roots of the Legendre polynomial and/or linear combinations

of a Legendre polynomial and its derivatives. All three sets of collocation points are

defined on the domain [−1, 1]. The difference between the sets is that the LG set does

not include either endpoint, the LGR set includes one endpoint and the LGL set includes

both of the endpoints. Also, the LGR set is asymmetric relative to the origin and can

be defined using the endpoint as either the initial point or the end point. Figure 3-1

illustrates the difference between LGL, LGR and LG collocation points [16].

Let N be the number of collocation points and PN(τ) be the Nth-degree Legendre

polynomial, then the LG, LGR, and LGL collocation points are obtained from the roots of

the polynomial. LG points are the roots obtained from PN(τ), LGR points are the roots

27

obtained from PN−1(τ) + PN(τ), and LGL points are the roots obtained from PN−1(τ)

together with points −1 and 1. [15]

3.2 Formulation of Pseudospectral Method Using LGR Points

Garg, D. and others [16] present the Radau Pseudospectral method for direct

trajectory optimization and costate estimation of finite-horizon and infintie-horizon

optimal control problems using global collocation at Legendre-Gauss-Radau (LGR)

points. The authors have shown the use of LGR collocation aids in finding accurate

primal and dual solutions for both finite and infitnite-horizon optimal control problems.

Therefore, the Radau Pseudospectral method is used in this work.

To simplify the problem, consider an unconstrained optimal control problem with

a terminal cost, on the time interval [−1, 1]. The time interval can be transformed from

[−1, 1] to the time interval [t0, tf ] via the affine transformation

t =tf − t02

τ +tf + t02

(3–1)

The goal is to determine the state x(τ) ∈ Rn and the control u(τ) ∈ Rm that

minimize the cost functional

Φ(x) = x(1), (3–2)

subject to the constraints below

dx

dτ= f (x(τ), u(τ)), x(−1) = x0, (3–3)

where f : Rn × Rm → Rn and x0 is the known initial condition.

Consider N LGR collocation points, τ1, τ2, τ3, ... , τN in the interval [−1, 1], where

τ1 = −1 and τn ≤ +1. An additional non-collocated point τN+1 = 1, which is used to

approximated the state variable, is introduced [16]. Each component of the state x is

28

approximated by a Lagrange polynomial, Li , as given in Equation (3–4) below,

Li(τ) =N+1∏j=1j =i

τ − τjτi − τj

, i = 1, ... ,N + 1 (3–4)

The j th component of the state is approximated as the series

xj(τ) =N+1∑j=1

xijLi(τ), (3–5)

Differentiating and evaluating the series in Equation (3–5) at the collocation points

τk , where k = 1, 2, ... ,N, gives

xj(τ)

N+1∑i=1

xij Li(τk) =

N+1∑i=1

Dkixij , (3–6)

where Dki = Li(τk).

The N × (N + 1) matrix D is called the differentiation matrix. Collocating the system

dynamics at each of the N collocation points, the discrete optimization problem becomes

DX = F (X ,U). (3–7)

From Equation (3–7), the finite-dimensional NLP is formulated as below,

Minimize Φ(XN) subject to DX = F (X ,U),

where X0 = x0.

The system dynamics are then rewritten as,

D1:NX1:N = F (X ,U)−D0x0, (3–8)

where D1:N is the N × N differentiation matrix, which is invertible.

3.3 hp-Adaptive Pseudospectral Method

An hp-adaptive pseudospectral method with collocation at Radau points is chosen

in this work. This method is proposed by Darby, C.L. and others [17], which iteratively

and simultaneously determines the number of segment breaks, the width of each

29

segment and the polynomial degree required in each segment for approximation, until

the user specified accuracy is achieved. This method leads to higher accuracy solutions

with less computational effort and memory than is required in global pseudospectral

methods.

3.4 Problem Formulation in GPOS

3.4.1 Objective and Input Parameters

The objective is to have the four AUVs track a target, while minimizing the distance

of each vehicle to the target and maintaining a safe operating distance. The final

positions and orientations of the AUVs are left free, for the problem ends when the target

has left the ROI. The ROI is defined to be [0 100; -50 -50; 0 100] in meters. The ROI

ranges from 0 to 100 meters along the x-axis (horizontal length), -50 to 50 meters along

the y-axis (horizontal width) and 0 to 100 meters along the z-axis (vertical depth). Table

3-1, shows the initial and terminal values of the states and time.

Table 3-1. User-specified initial and final states of the four AUVsInitial Conditions Terminal Conditionst0 = 0 s tf = distance traveled by target/target speed s

xi(t0) = 75, 75, 25, 25 m xi(tf ) freeyi(t0) = −25, 25, 25, −25 m yi(tf ) free

zi(t0) = 5 m zi(tf ) freeϕi(t0) = 0 deg ϕi(tf ) freeθi(t0) = 0 deg θi(tf ) free

ψi(t0) = −45, 45, 135, −135 deg ψi(tf ) freeui(t0) = 0 m/s ui(tf ) = 0 m/svi(t0) = 0 m/s vi(tf ) = 0 m/swi(t0) = 0 m/s wi(tf ) = 0 m/spi(t0) = 0 deg/s pi(tf ) freeqi(t0) = 0 deg/s qi(tf ) freeri(t0) = 0 deg/s ri(tf ) free

In this problem, it is assumed that the target’s trajectory is known. Because the

problem ends when the traget leaves the ROI, tf can be calculated by dividing the

distance traveled by the target by the speed of the target. Also, the final velocities are

30

set to zero to ensure that the AUVs do not leave the ROI after the target leaves the

region.

3.4.2 Cost Function Formulation

The cost functional for the problem is the sum of the distances from each of the four

AUVs to the target, which is formulated as

J =

∫ tf

t0

A∑i=1

√(xi − xt)2 + (yi − yt)2 + (zi − zt)2, (3–9)

where i = 1, ... ,A and xi , yi and zi are the Cartesian coordinates of the i th vehicle and

xt , yt and zt are the Cartesian coordinates of the target.

3.4.3 Path Constraint Formulation

For this problem, target and friendly collision avoidance constraints are needed.

Because the cost function is driving the distance between the AUVs and the target to

zero, four target collision avoidance constraints are required. Let di−T be the minimum

operating distance between the i th AUV and the target, where i = 1, ... , 4, then the

target-avoidance path constraints are

PathConsttarget = [d1−T ; d2−T ; d3−T ; d4−T ] ≥ [1; 1; 1; 1]. (3–10)

Once the four AUVs begin tracking a target, the vehicles are operating relatively

close together. In order to avoid collision among AUVs, friendly collision avoidance

constraints are needed. There are four vehicles, each avoiding the other three vehicles,

which produces 12 friendly collision avoidance path constraints. However, due to the

reciprocity of the distance between two vehicles, only six friendly collision avoidance

path constraints are needed. Let di−j be the minimum operating distance between the

i th and j th AUV, where i , j = 1, ... , 4 and i = j , then the friendly collision avoidance path

31

constraints are

PathConstfriendly1 = [d1−2; d1−3; d1−4] ≥ [1; 1; 1]

PathConstfriendly2 = [d2−3; d2−4] ≥ [1; 1]

PathConstfriendly3 = [d3−4] ≥ [1]

(3–11)

In Chapter 4, the results of the multiple AUV target tracking problem for two different

target scenarios are plotted and discussed. The trajectories of each of the vehicles are

plotted. Furthermore, the results are analyzed then validated.

32

Figure 3-1. Schematic showing LGL, LGR and LG collocation points

33

CHAPTER 4RESULTS AND DISCUSSION

In this chapter, the results of the multiple AUV target tracking problem for two target

scenarios are presented. In the first scenario, the target being tracked is an underwater

vehicle traveling at a depth of 30 meters with a speed of 7 knots. The target in the

second scenario is a surface vehicle with a speed of 20 knots. These problems are

solved by first converting the optimal control problems into NLPs using GPOPS. The

NLPs are then solved using the Sparse Nonlinear OPTimimzer (SNOPT) [18]. The

solutions obtained for the optimal control problems are then plotted. Furthermore, the

results are analyzed and validated.

4.1 Underwater Target

4.1.1 Analysis of Results

The trajectories of the four AUVs and the target are plotted in the three dimensional

plot in Figure 4-1. The plot shows the trajectories of the vehicles from the time the

underwater target enters the ROI to the time the target egresses from the ROI. Figure

4-10 provides a top-down view of the trajectories. Also, Figures 4-3, 4-4 and 4-5 display

the x , y and z-axis positions versus time plots of each AUV.

4.1.2 Validation of Results

The Lagrange cost of minimizing the sum of the distances from each AUV to the

target is satisfied and shown in Figure 4-6. Furthermore, the cost functional is verified

from Table 4-1, which displays the Lagrange cost and the average distance of the AUVs

to the target over the entire trajectory.

The target and friendly collision avoidance constraints defined in Chapter 3 are

validated from the results plotted in Figures 4-7 and 4-8. These plots validate that each

AUV avoids the target and the other AUVs by the minimum safe operating distance,

which is set at 1 meter.

34

Table 4-1. Lagrange cost validation for an underwater targetTime (s) Lagrange Cost (m) Average AUV Distance to Target (m)0.0000 252.4338 63.10840.0503 251.8992 62.97480.1684 250.6425 62.66060.3533 248.7121 62.17800.6042 246.1239 61.53100.9197 242.9849 60.74621.2984 239.2396 59.80991.7384 235.0198 58.75492.2376 230.2025 57.55062.7935 225.1430 56.28583.4034 219.6996 54.92494.0644 213.6914 53.42284.7732 205.7515 51.43795.5264 195.3580 48.83956.3203 182.1133 45.52837.1511 166.2769 41.56928.0146 147.4945 36.87368.9068 126.4502 31.61259.8232 104.6115 26.152910.7593 82.1849 20.546211.7107 60.1520 15.038012.6727 40.6519 10.163013.6406 25.6492 6.412314.6096 13.9165 3.479115.5751 5.9008 1.475216.5324 4.0807 1.020217.4767 5.5276 1.381918.4036 5.0488 1.262219.3084 4.2688 1.067220.1868 4.0000 1.000021.0344 4.0000 1.000021.8473 4.0000 1.000022.6213 4.0000 1.000023.3528 4.0000 1.000024.0381 4.0000 1.000024.6739 4.0000 1.000025.2572 4.0000 1.000025.7851 4.0000 1.000026.2549 4.0000 1.000026.6645 4.0000 1.000027.0119 4.0000 1.000027.2952 4.0000 1.000027.5132 4.0000 1.0000

35

Table 4-1. ContinuedTime (s) Lagrange Cost (m) Average AUV Distance to Target (m)27.6649 4.0034 1.000927.7494 4.0386 1.009727.7692 4.0431 1.0108

4.2 Surface Target

4.2.1 Analysis of Results

The trajectories of the four AUVs, as well as the target, are plotted in the three

dimensional plot in Figure 4-9. This plot shows the trajectories of the vehicles from the

time the surface target enters the ROI to the time the target egresses the ROI. Figure

4-10 provides a top-down view of the trajectories. Also, Figures 4-11, 4-12 and 4-13

display the x , y and z-axis positions versus time plots of each AUV.

4.2.2 Validation of Results

The Lagrange cost of minimizing the sum of the distances from each AUV to the

target is satisfied and shown in Figure 4-14. Furthermore, the cost functional is verified

from Table 4-2, which displays the Lagrange Cost and the average distance of the AUVs

to the target over the entire trajectory.

Table 4-2. Lagrange cost validation for a surface targetTime (s) Lagrange Cost (m) Average AUV Distance to Target (m)0.0000 300.3207 75.08020.0553 299.5625 74.89060.1852 297.7893 74.44730.3885 295.0320 73.75800.6643 291.3222 72.83061.0113 286.6288 71.65721.4277 280.8923 70.22311.9115 273.9425 68.48562.4603 265.7527 66.43823.0716 255.0547 63.76373.7422 241.5682 60.39204.4690 224.4197 56.10495.2484 204.1939 51.04856.0766 182.2937 45.57346.9496 158.5877 39.6469

36

Table 4-2. ContinuedTime (s) Lagrange Cost (m) Average AUV Distance to Target (m)7.8630 133.2311 33.30788.8126 110.5118 27.62809.7935 89.5891 22.397310.8011 69.3786 17.344711.8305 51.3470 12.836712.8766 34.7747 8.693713.9344 20.7136 5.178414.9986 13.9651 3.491316.0641 10.7207 2.680217.1257 8.8465 2.211618.1783 10.3702 2.592519.2166 10.3503 2.587620.2358 8.9006 2.225221.2307 6.8002 1.700022.1965 5.2279 1.307023.1286 4.3361 1.084024.0223 4.0000 1.000024.8734 4.0000 1.000025.6777 4.0000 1.000026.4312 4.0000 1.000027.1304 4.0000 1.000027.7717 4.0000 1.000028.3521 4.0000 1.000028.8688 4.0000 1.000029.3192 4.0000 1.000029.7011 4.0000 1.000030.0127 4.0000 1.000030.2524 4.0032 1.000830.4191 4.0694 1.017430.5120 4.2399 1.060030.5338 4.2572 1.0643

The target and friendly collision avoidance constraints defined in Chapter 3 are

validated from the results plotted in Figures 4-15 and 4-16. These plots validate that

each AUV avoids the target and the other AUVs by the minimumn safe operating

distance.

37

0

20

40

60

80

100

−20

−10

0

10

20

10

20

30

Forward Position (m)

AUV Three Dimensional Trajectory

Lateral Position (m)

Dep

th (

m)

TargetAUV

1

AUV2

AUV3

AUV4

Figure 4-1. Three-dimensional trajectory of AUVs tracking the underwater target

−50 −40 −30 −20 −10 0 10 20 30 40 500

10

20

30

40

50

60

70

80

90

100AUV Forward Position vs Lateral Position

For

war

d P

ositi

on (

m)

Lateral Position (m)

TargetAUV

1

AUV2

AUV3

AUV4

Figure 4-2. Top-down view of trajectory for tracking the underwater traget

38

0 5 10 15 20 250

10

20

30

40

50

60

70

80

90

100AUV Forward Position vs Time

For

war

d P

ositi

on (

m)

Time (s)

TargetAUV

2AUV

3

AUV4

Figure 4-3. Forward position versus time for tracking the underwater traget

0 5 10 15 20 25−30

−20

−10

0

10

20

30AUV Lateral Position vs Time

Late

ral P

ositi

on (

m)

Time (s)

TargetAUV

1

AUV2

AUV3

AUV4

Figure 4-4. Lateral position versus time for tracking the underwater traget

39

0 5 10 15 20 25

0

5

10

15

20

25

30

35

40

AUV Depth vs Time

Dep

th (

m)

Time (s)

TargetAUV

1

AUV2

AUV3

AUV4

Figure 4-5. Depth versus time for tracking the underwater traget

0 5 10 15 20 250

50

100

150

200

250

300

Lagr

ange

Cos

t (m

)

Time (s)

Lagrange Cost vs Time

Figure 4-6. The lagrange cost versus time for the underwater target

40

0 5 10 15 20 250

10

20

30

40

50

60

70

80

90

100

Dis

tanc

e to

Tar

get (

m)

Time (s)

Distance of AUV to Target vs Time

AUV

1

AUV2

AUV3

AUV4

Figure 4-7. Distance from AUVs to target versus time for the underwater target

41

0 5 10 15 20 250

10

20

30

40

50

60

Ave

rage

Dis

tanc

e B

etw

een

AU

Vs

(m)

Time (s)

Average Distance Between AUVs vs Time

AUV

1

AUV2

AUV3

AUV4

Figure 4-8. Average distance from one AUV to the others versus time for the underwatertarget

0

20

40

60

80

100

−50

0

50

05

Forward Position (m)

AUV Three Dimensional Trajectory

Lateral Position (m)

Dep

th (

m)

TargetAUV

1

AUV2

AUV3

AUV4

Figure 4-9. Three-dimensional trajectory of AUVs tracking the surface target

42

−50 −40 −30 −20 −10 0 10 20 30 40 500

10

20

30

40

50

60

70

80

90

100AUV Forward Position vs Lateral Position

For

war

d P

ositi

on (

m)

Lateral Position (m)

TargetAUV

1

AUV2

AUV3

AUV4

Figure 4-10. Top-down view of trajectory for tracking the surface traget

0 5 10 15 20 25 300

10

20

30

40

50

60

70

80

90

100AUV Forward Position vs Time

For

war

d P

ositi

on (

m)

Time (s)

TargetAUV

1

AUV2

AUV3

AUV4

Figure 4-11. Forward position versus time for tracking the surface traget

43

0 5 10 15 20 25 30−50

−40

−30

−20

−10

0

10

20

30

40

50AUV Lateral Position vs Time

Late

ral P

ositi

on (

m)

Time (s)

TargetAUV

1

AUV2

AUV3

AUV4

Figure 4-12. Lateral position versus time for tracking the surface traget

0 5 10 15 20 25 30

−50

−40

−30

−20

−10

0

10

20

30

40

50

AUV Depth vs Time

Dep

th (

m)

Time (s)

TargetAUV

1

AUV2

AUV3

AUV4

Figure 4-13. Depth versus time for tracking the surface traget

44

0 5 10 15 20 25 300

50

100

150

200

250

300

Lagr

ange

Cos

t (m

)

Time (s)

Lagrange Cost vs Time

Figure 4-14. The lagrange cost versus time for the surface target

0 5 10 15 20 25 300

20

40

60

80

100

120

Dis

tanc

e to

Tar

get (

m)

Time (s)

Distance of AUV to Target vs Time

AUV

1

AUV2

AUV3

AUV4

Figure 4-15. Distance from AUVs to target versus time for the surface target

45

0 5 10 15 20 25 300

10

20

30

40

50

60

Ave

rage

Dis

tanc

e B

etw

een

AU

Vs

(m)

Time (s)

Average Distance Between AUVs vs Time

AUV

1

AUV2

AUV3

AUV4

Figure 4-16. Average distance from one AUV to the others versus time for the surfacetarget

46

CHAPTER 5CONCLUSION

This work presented an approach to solve a multiple AUV target tracking problem

using pseudospectral methods. An example problem that generates the trajectory which

minimizes the sum of the distances from each vehicle to the target has been solved.

This trajectory satisfies the target and friendly avoidance constraints placed on the

AUVs. The problem has been formulated in Chapter 2. The optimal control problem has

been formulated in Chapter 3. From the results plotted in Chapter 4, it was found that

the solution satisfied all the constraints in the problem and gave an optimal solution.

This research work can be used to aid in the protection of U.S. ports and waterways,

as well as ships in foreign waters. Future work in optimizing the number of collocation

points used to solve the optimal control problem will decrease the computation time

required to these trajectories. Furthermore, improvements in CPU processors as well as

faster NLP solvers will significantly improve the computation time. This will help in near

real-time implementation of this research.

47

REFERENCES

[1] Frittelli, J., “Port and Maritime Security: Background and Issues for Congress,” CRSReport for Congress, May 2005.

[2] “Status of the Navy,” http://www.navy.mil/navydata/navy_legacy_hr.asp?id=146, Feb 2012.

[3] Ghommam, J., Calco, O., and Rozenfeld, A., “Coordinated Path Following forMultiple Underactuated AUVs,” OCEANS, April 2008, pp. 1–7.

[4] Wang, Y., Yan, W., Gao, B., and Cui, R., “Backstepping-based Path FollowingControl of an Underactuated Autonomous Underwater Vehicle,” InternationalConference on Information and Automation, June 2009, pp. 466–471.

[5] Reif, J. and Wang, H., “Social Potential Fields: A Distributed Behavioral Control forAutonomous Robots,” Robotics and Autonomous Systems, Vol. 27, No. 4, 1999,pp. 171–194.

[6] Beard, R., Lawton, J., and Hadaegh, F., “A Coordination Architecture for SpacecraftFormation Control,” IEEE Transactions on Control Systems Technology , Vol. 9,No. 6, Nov 2001, pp. 777–790.

[7] Fua, C., Ge, S., Do, K., and Lim, K., “Multi-robot Formations Based on theQueue-Formation Scheme With Limited Communication,” IEEE Transactionson Robotics, Vol. 23, No. 6, 2007, pp. 1160–1169.

[8] Ge, S. and Fua, C., “Queues and Artificial Potential Trenches for MultirobotFormations,” IEEE Transactions on Robotics, Vol. 21, No. 4, Aug 2005,pp. 646–656.

[9] Cui, R., Ge, S., How, B., and Choo, Y., “Leader-Follower Formation Control ofUnderactuated AUVs with Leader Position Measurement,” IEEE InternationalConference on Robotics and Automation, 2009, pp. 979–984.

[10] Emrani, S., Dirafzoon, A., Talebi, H., Nikravesh, S., and Menhaj, M., “An AdaptiveLeader-Follower Formation Controller for Multiple AUVs in Spatial Motions,” 36thAnnual Conference on IEEE Industrial Electronics Society , Nov 2010, pp. 59–64.

[11] Wiegert, R. and Oeschger, J., “Generalized Magnetic Gradient Contraction BasedMethod for Detection, Localization and Discrimination of Underwater Mines andUnexploded Ordnance,” OCEANS, Vol. 2, Sept 2005, pp. 1325–1332.

[12] Rauch, C., Austin, T., Grosenbaugh, M., Jaffre, F., Stokey, R., and MacDonald,J., “Generalized Magnetic Gradient Contraction Based Method for Detection,Localization and Discrimination of Underwater Mines and Unexploded Ordnance,”OCEANS, Sept 2008, pp. 1–5.

48

[13] Belbachir, A. and Perrier, M., “Cooperative-Adaptive Algorithms for TargetsLocalization in Underwater Environment,” Autonomous Underwater Vehicles(AUV), 2010 IEEE/OES, Sept 2010, pp. 1–7.

[14] Arslan, M., Fukushima, N., and Hagiwara, I., “Nonlinear Optimal Control of anAUV and its Actuator Failure Compensation,” International Conference on Control,Automation, Robotics and Vision, Dec 2008, pp. 668–673.

[15] Garg, D., Patterson, M., Hager, W., Rao, A., Benson, D., and Huntington, G., “AUnified Framework for the Numerical Solution of Optimal Control Problems UsingPseudospectral Methods,” Automatica, Vol. 46, No. 11, Nov 2010, pp. 1843–1851.

[16] Garg, D., Patterson, M., and Francolin, C., “Direct Trajectory Optimization; CostateEstimations of Finite-horizon; Infinite-horizon Optimal Control Problems Using aRadau Pseudospectral Method,” Computational Optimization and Applications, Oct2009, pp. 1–24.

[17] Darby, C., Hager, W., and Rao, A., “An hp-adaptive Pseudospectral Method forSolving Optimal Control Problems,” Optimal Control Applications and Methods, Aug2010, pp. 1–41.

[18] “Stanford Business Software Inc., SNOPT 7.2,” http://www.sbsi-sol-optimize.com/asp/sol_product_snopt.htm, 2011.

49

BIOGRAPHICAL SKETCH

Adam Franklin was born in Columbus, Mississippi in 1987. He earned his Bachelor

of Science degree in systems engineering and his commission in the United States

Marine Corps from the Naval Academy in May of 2010. He also graduated with a Master

of Science degree in mechanical engineering from the Department of Mechanical

and Aerospace Engineering at the University of Florida in May 2012. He completed

his research work and master’s thesis under the supervision of Dr. Anil V. Rao and

was co-advised by Dr. Warren E. Dixon. He now continues his officer training, with a

secondary military occupation specialty of ordnance systems engineer.

50