trajectory optimization using pseudospectral methods...
TRANSCRIPT
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