Deliberative Optimization of Reactive Agents forAutonomous Navigation

by

Anthony Douglas Jones

Bachelor of ScienceMechanical Engineering Technology

University of Arkansas at Little Rock2007

Associate of ScienceComputer Programming

University of Arkansas at Little Rock2008

A dissertation submitted to theCollege of Engineering at

Florida Institute of Technologyin partial fulfillment of the requirements

for the degree of

Doctor of Philosophyin

Ocean Engineering

Melbourne, FloridaJuly 2013

c©Copyright 2013 Anthony Douglas JonesAll Rights Reserved

The author grants permission to make single copies.

We the undersigned committee hereby approve the attached dissertation “De-liberative Optimization of Reactive Agents for Autonomous Navigation,” byAnthony Douglas Jones.

Stephen L. Wood, Ph.D., P.E.Major AdvisorProgram Chair, Ocean Engineering

Ronnal P. Reichard, Ph.D.Committee MemberProfessor, Engineering

Marius C. Silaghi, Ph.D.Committee MemberAssistant Professor, Computer Science

Hector M. Gutierrez, Ph.D., P.E.Committee MemberAssociate Professor, Mechanical and Aerospace Engineering

George A Maul, Ph.D.Department Head, Marine and Environmental Systems

Abstract

Deliberative Optimization of Reactive Agents for Autonomous Navigation

Author: Anthony Douglas Jones

Major Advisor: Stephen L. Wood, Ph.D., P.E.

This work develops a type of reactive agent that navigates according to a steering

behavior that is governed by the agent’s preferences. These preferences are

optimized during a deliberative planning phase, and the agent’s trajectory can

be recorded as a series of waypoints or encoded as the parameters governing its

preferences. The agent behaviors are tested in realistic ocean environments, and

their performance is compared against simple tracking and homing behaviors

as well as against an A*-based path planner.

The environmental model for testing uses historic oceanographic and bathy-

metric data in two regions of interest. The first region consists entirely of open

water in the Gulf of Mexico, and the second region lies in the South-Eastern

Caribbean, where it is obstructed by a chain of islands. The Gulf region has

no obstructions but faster local currents, while the Caribbean region has slower

currents, but obstructing land masses. In general the algorithm performs better

than simple tracking or homing behaviors, but not as well as the A* planner.

The algorithm shows its most promise in its simplistic encoding of mission be-

haviors that are much more complex than homing or tracking, but it falls short

iii

of replacing a synoptic mission plan.

To explore the feasibility of physically implementing one of these agents, a simple

Autonomous Surface Vehicle(ASV) was designed and constructed, which relies

primarily on commercial off-the-shelf electronics in a custom polycarbonate hull.

During a field test, the ASV successfully carried out a series of simple 250-meter

point to point navigation missions.

iv

Contents

1 Introduction 15

1.1 Intellectual Merit . . . . . . . . . . . . . . . . . . . . . . . . . . 18

1.2 Broader Impacts . . . . . . . . . . . . . . . . . . . . . . . . . . 19

2 Project Background and Literature Review 20

2.1 Graphs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

2.1.1 Dijkstra and A* Graph Search . . . . . . . . . . . . . . 22

2.2 Artifical Potential Fields . . . . . . . . . . . . . . . . . . . . . . 25

2.3 Swarm Intelligence . . . . . . . . . . . . . . . . . . . . . . . . . 27

2.3.1 MAKLINK Graph – Habib 1991 . . . . . . . . . . . . . . 27

2.3.2 Ant Colony Optimization – Bonabeau 1999 . . . . . . . . 28

2.3.3 Intensified ACO – Fan 2003 . . . . . . . . . . . . . . . . 29

2.3.4 Dijkstra and ACO – Tan 2006 . . . . . . . . . . . . . . . 30

2.3.5 Hybrid ACO with APF – Xing2011 . . . . . . . . . . . . 31

2.3.6 Particle Swarm Optimization – Kennedy 1995 . . . . . . 32

2.3.7 Optimized Reactive Systems – Rhoads 2010 . . . . . . . 33

2.4 Iterative Optimization – Perdomo 2011 . . . . . . . . . . . . . . 33

2.5 Steering Behaviors – Reynolds 1999 . . . . . . . . . . . . . . . . 34

2.6 Tracking and Homing . . . . . . . . . . . . . . . . . . . . . . . 35

v

3 Software Description 38

3.1 Concept of Operation . . . . . . . . . . . . . . . . . . . . . . . 38

3.2 Detailed Description of Operation . . . . . . . . . . . . . . . . 45

3.3 Supporting Software Utilities . . . . . . . . . . . . . . . . . . . 49

3.3.1 ncinfo . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49

3.3.2 nc2npz . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

3.3.3 toponc2npz . . . . . . . . . . . . . . . . . . . . . . . . . 50

3.3.4 GenWaypoints and GenWaypointsGulf . . . . . . . . . . 50

3.3.5 GrouperBehavior-P3d . . . . . . . . . . . . . . . . . . . 51

3.3.6 GrouperView-P3d . . . . . . . . . . . . . . . . . . . . . 51

3.3.7 popro . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

3.3.8 HullProp . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

4 Environmental Model and Regions of Interest 53

4.1 Bathymetric Data . . . . . . . . . . . . . . . . . . . . . . . . . 55

4.2 Regions of Interest . . . . . . . . . . . . . . . . . . . . . . . . . 56

4.2.1 Gulf of Mexico . . . . . . . . . . . . . . . . . . . . . . . 56

4.2.2 Caribbean . . . . . . . . . . . . . . . . . . . . . . . . . . 57

5 Software Development 60

5.1 2-D A* Planner . . . . . . . . . . . . . . . . . . . . . . . . . . 61

5.2 4-D A* Planner . . . . . . . . . . . . . . . . . . . . . . . . . . 63

5.2.1 4-D Grid Independent Design . . . . . . . . . . . . . . . 64

5.2.2 Testing of Prototype Assumptions . . . . . . . . . . . . . 65

5.2.3 Speed of Planning . . . . . . . . . . . . . . . . . . . . . 66

5.3 NetLogo Prototype . . . . . . . . . . . . . . . . . . . . . . . . . 69

5.4 Python Prototype in Gulf ROI . . . . . . . . . . . . . . . . . . 70

vi

5.4.1 Agents . . . . . . . . . . . . . . . . . . . . . . . . . . . 71

5.4.2 Test Conditions . . . . . . . . . . . . . . . . . . . . . . 73

5.4.3 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . 76

5.5 Modifications for Use in Obstructed

Environments . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81

5.6 Pseudo Scientific Tests and Modifications . . . . . . . . . . . . . 83

5.6.1 Selecting Values of M and T . . . . . . . . . . . . . . . 84

5.6.2 Exploration vs. Exploitation . . . . . . . . . . . . . . . 85

5.6.3 Simple Learning Does not Help . . . . . . . . . . . . . . 86

5.7 Impact of DORA’s Individual Parameters . . . . . . . . . . . . 87

5.7.1 The Influence of A . . . . . . . . . . . . . . . . . . . . . 87

5.7.2 The Need for C . . . . . . . . . . . . . . . . . . . . . . . 88

6 Platform 90

6.1 Hull Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90

6.2 Hull Construction . . . . . . . . . . . . . . . . . . . . . . . . . 95

6.3 Electronics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 96

6.4 Control Software . . . . . . . . . . . . . . . . . . . . . . . . . . 102

6.5 Estimated Speed . . . . . . . . . . . . . . . . . . . . . . . . . . 103

7 Experiment 107

7.1 Performance Evaluation . . . . . . . . . . . . . . . . . . . . . . 108

7.2 Choice of Parameters . . . . . . . . . . . . . . . . . . . . . . . 109

7.2.1 A* . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110

7.2.2 DORA . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111

7.2.3 All Agents . . . . . . . . . . . . . . . . . . . . . . . . . . 112

7.3 Program Environment Specifications . . . . . . . . . . . . . . . 113

vii

7.4 Gulf of Mexico ROI . . . . . . . . . . . . . . . . . . . . . . . . 113

7.4.1 Trial at 3.0 m/s . . . . . . . . . . . . . . . . . . . . . . . 114

7.4.2 Trial at 1.5 m/s . . . . . . . . . . . . . . . . . . . . . . 115

7.4.3 Trial at 0.4 m/s . . . . . . . . . . . . . . . . . . . . . . 115

7.5 Caribbean ROI . . . . . . . . . . . . . . . . . . . . . . . . . . . 117

7.5.1 Trial at 3.0 m/s . . . . . . . . . . . . . . . . . . . . . . 117

7.5.2 Trial at 1.50 m/s . . . . . . . . . . . . . . . . . . . . . . 119

7.5.3 Trial at 0.40 m/s . . . . . . . . . . . . . . . . . . . . . . 120

7.6 Field Trials . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 121

7.6.1 Impromptu Experiment . . . . . . . . . . . . . . . . . . 123

7.6.2 Design Improvements . . . . . . . . . . . . . . . . . . . . 126

8 Future Developments and Conclusions 129

8.1 Future Development . . . . . . . . . . . . . . . . . . . . . . . . 129

8.2 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132

A Arduino Source Code 143

B NetLogo Source Code 151

C Python Source Code 158

C.1 environment.py . . . . . . . . . . . . . . . . . . . . . . . . . . 158

C.2 navigation.py . . . . . . . . . . . . . . . . . . . . . . . . . . . 162

C.3 agents.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 164

C.4 asearch.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 167

C.5 nc2npz.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 173

C.6 toponc2npz . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 174

C.7 ncinfo.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 174

viii

C.8 GenWaypoints.py . . . . . . . . . . . . . . . . . . . . . . . . . . 175

C.9 GenWaypointsGulf.py . . . . . . . . . . . . . . . . . . . . . . . 176

C.10 GrouperBehavior-P3d.py . . . . . . . . . . . . . . . . . . . . . 177

C.11 GrouperView-P3d . . . . . . . . . . . . . . . . . . . . . . . . . . 180

C.12 popro.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 185

C.13 HullProp.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . 188

C.14 FieldTest.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . 189

C.15 csv2kml.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 190

C.16 ANav.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 190

C.17 GrouperNav.py . . . . . . . . . . . . . . . . . . . . . . . . . . . 191

C.18 GrouperNav0A.py . . . . . . . . . . . . . . . . . . . . . . . . . . 196

C.19 PNav.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 202

C.20 CNav.py . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 204

ix

List of Figures

2.1 Weighted Graph . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

2.2 A* vs. Dijkstra . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

2.3 MAKLINK Graphs . . . . . . . . . . . . . . . . . . . . . . . . . 28

2.4 Tracking and Homing . . . . . . . . . . . . . . . . . . . . . . . . 36

4.1 Gulf of Mexico Region of Interest . . . . . . . . . . . . . . . . . 54

4.2 Caribbean Region of Interest . . . . . . . . . . . . . . . . . . . . 55

4.3 Gulf of Mexico Velocity Field . . . . . . . . . . . . . . . . . . . 56

4.4 Gulf of Mexico Bathymetric Map . . . . . . . . . . . . . . . . . 57

4.5 Caribbean Velocity Field . . . . . . . . . . . . . . . . . . . . . . 58

4.6 Caribbean Bathymetric Map . . . . . . . . . . . . . . . . . . . . 59

5.1 2-D A* Path . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

5.2 Astar Island Navigation . . . . . . . . . . . . . . . . . . . . . . 64

5.3 NetLogo Prototype . . . . . . . . . . . . . . . . . . . . . . . . . 69

5.4 Tracking Vs. Dora . . . . . . . . . . . . . . . . . . . . . . . . . 78

5.5 PHD-Significance . . . . . . . . . . . . . . . . . . . . . . . . . . 83

6.1 Hull Plan . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94

6.2 ASV . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94

6.3 ASV Wiring . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101

x

6.4 ASV Schematic . . . . . . . . . . . . . . . . . . . . . . . . . . . 105

6.5 ASV Power Vs. Speed . . . . . . . . . . . . . . . . . . . . . . . 105

6.6 ASV Range Vs. Speed . . . . . . . . . . . . . . . . . . . . . . . 106

7.1 Example Path from Gulf . . . . . . . . . . . . . . . . . . . . . . 122

7.2 Field Trial 0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124

7.3 Field Trial 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127

xi

List of Tables

5.1 First Performance Trade Study . . . . . . . . . . . . . . . . . . 67

5.2 Results of Planar Earth Trial . . . . . . . . . . . . . . . . . . . 76

5.3 Results of Round Earth with High Speed Trial . . . . . . . . . . 79

5.4 Success Rates for Round Earth with Low Speed Trial . . . . . . 81

5.5 Results of Solved Trials on Round Earth with Low Speed . . . 81

6.1 ASV Hull . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93

6.2 ASV Performance . . . . . . . . . . . . . . . . . . . . . . . . . . 103

7.1 Results of Gulf Trials at 3.0 m/s . . . . . . . . . . . . . . . . . 114

7.2 Results of Gulf Trials at 1.5 m/s . . . . . . . . . . . . . . . . . 115

7.3 Results of Gulf Trials at 0.4 m/s . . . . . . . . . . . . . . . . . 117

7.4 Results of Caribbean Trials at 3.0 m/s . . . . . . . . . . . . . . 119

7.5 Results of Caribbean Trials at 1.5 m/s . . . . . . . . . . . . . . 119

7.6 Results of Caribbean Trials at 0.4 m/s . . . . . . . . . . . . . . 120

xii

Acknowledgements

I would like express my appreciation to everyone that has made this project

possible, including the following people and organizations in particular:

• My Major Professor, Dr. Stephen Wood

• The members of my doctoral committee:

– Dr. Ronnal Reichard

– Dr. Marius Silaghi

– Dr. Hector Gutierrez

• My colleagues from the Underwater Technology Laboratory

• The Department of Defense SMART Scholarship Program and my mentors

at SPAWAR Systems Center Pacific

– Marshal Dvorak

– Michael Tall

• My parents, Danny and Connie Jones

• My family and friends

xiii

Dedication

Dedicated to my wife, Jenna.

xiv

Chapter 1

Introduction

Most autonomous underwater vehicles (AUV’s) are severely limited in intelli-

gence. In general, they are only capable of carrying out a mission that is care-

fully scripted before deployment. They may be able to reactively avoid collisions

or make small departures from the plan, but they inevitably either return to the

plan as soon as possible or abort the mission entirely. Indeed, the AUV-related

algorithms described in the literature review all address pre-mission planners

rather than any onboard capability. As AUV’s navigate from one point to the

next, they usually do so by attempting to travel along the shortest distance

to their objectives. A number of planning methods are currently used to de-

velop these missions including a variety of A*-based graph searching methods,

some swarm-based techniques, and other optimization methods, which will be

discussed at length in Chapter 2.

However, these planners aspire to find an efficient path consisting of a number of

waypoints or bearings from a starting point to a goal. Conversely, this project

15

attempts to allow greater flexibility in path planning by using a more complex

steering behavior than simply moving toward the next objective. The is addi-

tional flexibility potentially reduces the need for a detailed list of waypoints. In

fact when successful and properly tuned, the behavior that is developed here al-

lows an agent to navigate around obstacles or opposing currents and to achieve

its objective without any global information other than the location of its ob-

jective. It makes all course adjustments based only on local information. This

steering behavior is purely reactive, but it is optimized by a deliberative mission

planner, so the algorithm is called Deliberative Optimization of Reactive Agents

(DORA), and is described in detail in Chapter 3.

Since the agent only has access to local data, it lacks the synoptic view of

the mission that is employed by waypoint-to-waypoint planning methods. This

has the potential benefit of reducing the memory required to store a mission,

since some of the trajectory is inherently a function of the agent’s environment.

Not withstanding these differences, an A*-based point-to-point planner is the

standard of comparison used here, as it is a well-proven and well-researched

method that has the benefits of mathematical elegance and successful use in

AUV navigation. But due to the fundamentally different nature of these two

systems, some difficulties of comparison arise. The A* algorithm searches in a

different problem space than DORA, and this must be borne in mind during any

comparative study of the two. Since both algorithms are being used to solve the

same physical problem, their comparison is still practical, but comparisons will

generally be made in terms of mission-related performance measures such as

computation time and the quality of the mission that is planned, rather than in

regard to metrics which are defined by the algorithms’ problem spaces. Both the

A*-based software that is used as the standard of comparison, and the current

16

version of the DORA software system have undergone much development and

iterative improvement, and this process is discussed in Chapter 5.

To evaluate the potential use of DORA aboard autonomous vehicles, a num-

ber of simulations were run that tasked both algorithms with planning paths

through historic ocean current fields in the Gulf of Mexico and the Caribbean.

The environmental data used to model these regions is described in Chapter 4.

In these simulations, the software was tasked with finding paths for agents oper-

ating at different speeds from much slower to slightly faster than the local ocean

currents. It was also required to navigate through a region that was divided by

an intervening island chain. In all cases, the success rates of the two methods

were compared as was the average quality of their solutions. In general, the

A*-based planner was more successful and planned better paths in shorter time

than did DORA. However, there were some exceptions in which DORA was the

better option, and the details of these cases are discussed in Chapter 7.

Since this algorithm is intended to be a path planner for a mobile robot, and

the agents are simplistic virtual approximations of autonomous vehicles, the

final test of the feasibility of this system is the deployment of an autonomous

surface vehicle (ASV) that navigates according to the steering behavior that is

developed by this research. The ASV’s navigation system instantiates a DOR

agent that is optimized before deployment in the Eau Gallie River, where it

successfully navigates to a waypoint 250 meters from its launch, and then returns

to its starting position. The design and construction of the ASV are described

in Chapter 6.

Finally, the conclusions drawn from the experiments address the questions that

began this research, but they also raised additional questions, and opened the

17

way for future improvements and developments. Chapter 8 then summarizes the

outcome of this project and describes some potential directions for its continued

development.

The preceding several paragraphs provide an overview of this project, however

this is an experiment, and as such it must be formalized before moving forward.

All of the following chapters serve to address this hypothesis:

An agent that is influenced by ocean currents and is subject to collisions with

terrain need not carry a detailed, synoptic mission plan, but rather can act

according to simple preferences based on its locally observable environment,

provided that these preferences are deliberatively optimized before its deploy-

ment. Further, the trajectory of such an agent will serve as a path plan that is

comparable in speed and quality to a plan generated by an A*-based mission

planner.

1.1 Intellectual Merit

The merit of this project lies primarily in the development of a simplistic means

of encoding a mission trajectory through a series of four floating point numbers.

This represents a lower memory requirement for a path plan than a long list of

waypoints. This would be more similar to the use of spline-based representa-

tions of mission paths, however fitting a spline to a mission plan requires either

neglecting environmental features, or arbitrarily adjusting the number of param-

eters used to describe the path. The use of optimized reactive agents, whose

reactions are based on environmental conditions allows for the construction of

18

trajectories, which are tuned to the particular mission.

1.2 Broader Impacts

The agents developed to enable this algorithm are simple enough to implement

with only high-school level math. More importantly, the construction of the

field testing ASV primarily from commercial off-the-shelf (COTS) components,

which are drawn largely from the hobby electronics community will enable it to

be easily and inexpensively reproduced and programmed. This will make the

ASV a viable platform for additional navigation-related research, as well as a

potential tool for educational outreach similar to terrestrial robotics kits that

are commonly used for that purpose. Beyond its potential use for robotics edu-

cation, the ASV’s mathematically derived hull allows for very simple calculation

of many parameters that are commonly used in Naval Architecture, which in

turn allows for simple trade studies between design parameters and hull prop-

erties. To support these broader impacts, the ASV is fully described in Chapter

6, which includes the description of the hull and all onboard electronics, and

Appendix A includes the source code for the ASV firmware.

19

Chapter 2

Project Background and

Literature Review

There is a large volume of literature regarding general path planning, path

planning for autonomous robots, and path planning for autonomous underwater

vehicles in particular. This section summarizes the concepts and papers that are

most relevant to the current research. Seto [34] provides an excellent reference

on this topic and includes a good overview of several common, current methods

for autonomous underwater vehicle navigation and path planning. In addition

to algorithms specifically related to maritime navigation, Millington and Funge

[25] describes a number of methods useful for real-time and high speed planning

including a discussion of some of the algorithms presented below, and Russel and

Norvig [32] is a popular reference regarding the subject of Artificial Intelligence

in general.

20

2.1 Graphs

A graph is a data structure that is used to represent the relationships between its

member elements or nodes. In the case of path planning, a graph is often used

as a convenient representation of an environment. Consider that any port on

an ocean or navigable river can eventually be accessed from any other port, but

in general major shipping lanes only connect the larger ports, whereas smaller

coastal ports are serviced by lighter shipping, and river ports are serviced by

barges and smaller riverine craft. Thus cargo’s travel from any port to any other

may require a number of transitions along typical shipping routes. Further,

transitioning from one port to another will incur some cost associated with the

distance traveled, the conditions en-route, and a number of other factors. This

type of problem lends itself well to representation by a weighted graph such as

the one in Figure 2.1.

“Graph” describes the data structure as consisting of a number of nodes tied

together via connections, and “weighted” refers to the fact that there is a cost

associated with a transition from one node to another [25]. The path planning

software described in Chapter 3 will use the positions of mission waypoints as

nodes and the time required to navigate along a straight line between them

will be the cost. Fortunately, graph problems are useful for many applications

beyond robotic mission planning, and there are a number of algorithms that have

been developed for their solution. Two of these that are particularly applicable

to the problem at hand will be described in the following section. Before moving

on, it should be noted that the AUV path planning problem’s graph is much

more complicated than the one shown because the cost of transitioning from

one node to another is not likely to be the same as transitioning back from that

21

Figure 2.1: Visual representation of a simple graph where the numbered cir-cles represent the nodes and the lines connecting them the edges. Costs oftransitioning from one node to another are indicated by the numbers along theedges.

node, and in fact, all costs in the graph continually vary with time due to the

changing ocean currents. Thus a node is not simply fixed in space, but also

in time. This problem is addressed through dynamic modification of the graph

by the search software, but the details of that process are better reserved for

Section 5.2.

2.1.1 Dijkstra and A* Graph Search

There are a number of methods that have been developed to solve the problem

of efficiently navigating from one node in a graph to another, but only two are

22

Figure 2.2: Example graph navigation problem that could be solved using A*or Dijkstra.

relevant to this this project. These are A* and Dijkstra’s algorithm.

Hart and Nils [19] introduce the A* algorithm for graph search problems, and

the technique is described well in several later publications including [32] and

[25]. A* and Dijkstra are both intended to find the minimum cost path through

a graph. A* works by choosing to explore nodes in an order based on a heuristic

function which estimates the total cost from the current node to the goal as the

cost of reaching a successor node plus some estimate of the remaining cost from

the successor node to the goal node. To ensure that an optimal path is planned,

the heuristic estimate must be “admissible.” An admissible heuristic is one

which predicts a cost that is always less than actual path cost. For instance,

in geographic problems, straight line distance from a successor node to the

goal is often used because whatever the best available path is, it can never be

shorter than the straight line distance. It is critical to note, that a heuristic’s

admissibility requirement is only necessary to ensure an optimal solution. If

a sub-optimal, “good” solution is acceptable, it may be advisable to relax the

admissibility requirement for a heuristic in the interest of speed of execution.

In robotic path planning, it is sometimes better to have good plan quickly than

an optimal plan that requires more time.

23

Dijkstra’s algorithm preceded the A* algorithm in publication [13], but is pre-

sented afterwards because it may be considered a special case of the A* ap-

proach. Dijkstra suggests expanding successor nodes in order of minimum cost,

so that no estimate of the total remaining distance to the goal is needed. In

practice, this can be faster than other search methods such as breadth-first, as

it is guided by cost, but is generally not as fast as A* since the path with the

total minimum cost is not always the same as the path with the minimum cost

between any two successors [25].

For comparison of the two methods, consider Figure 2.2, which shows a simple

graph problem. Here an agent needs to navigate from the start node to the goal

node, and must choose the shortest route. Clearly the shortest path lies along

the edges at the top, and this is the route that A* would explore first. It would

evaluate the cost of going from Node 0 to Node 1 and estimate the remaining

distance to Node 3. It would also evaluate the cost of traveling from Node 0

to Node 2 and estimate the remaining distance from Node 2 to Node 3. The

straight line distance from Node 2 to Node 3 is not much less than the distance

from Node 0 to Node 3, but Node 2 is very near Node 3, so A* chooses this

route.

Dijkstra’s algorithm on the other hand is based exclusively on the total cost

from the start node to the next node (not the goal). So Dijkstra’s algorithm

would evaluate the cost from Node 0 to Node 1 as well as the cost from Node 0

to Node 2. Seeing that the cost of reaching Node 2 was shorter, it would expand

Node 2, and begin the comparison again from that point. Dijkstra’s algorithm

would finally select the same route as A*, because eventually the cost incurred

along the lower route would become greater than the cost of moving to Node

24

1, but the critical difference is that Dijkstra’s algorithm would require all of

the additional processing time to explore a portion of the longer route before

finding the shorter route.

The A* algorithm has been applied to AUV navigation by several research

groups [10, 17, 16, 35]. Carroll et al. [10] used A* with a gridded discretiza-

tion of the ocean to plan AUV paths that took advantage of ocean currents,

avoided obstacles, and allowed for temporally varying exclusion zones. Garau

et al. [17] revisited A*-based AUV path planning, and used a different assump-

tion of vehicle behavior that employed constant thrust power throughout the

mission, rather than constant vehicle velocity. Fernandez-Perdomo et al. [16]

developed another A* algorithm specifically tailored to AUV’s, which was called

Constant Time Surfacing A* (CTS-A*). This algorithm differed from previous

similar systems by discretizing the environment in a way that assured constant

time between vehicle surfacing events rather than discretizing with respect to

environmental geometry. Smith and Dunbabin [35] implemented an A* path

planner not for AUV’s, but for drift buoys, which could only control their verti-

cal positions in the water column, and thus needed to take advantage of natural

currents for all navigation. All of these examples demonstrate the viability of

A* as a candidate mission planner for AUV’s.

2.2 Artifical Potential Fields

Khatib [23] describes the use of artificial potential fields(APF) in robotic ma-

nipulator motion, and discusses its extension to the field of mobile robotic path

planning. One of the most important contributions of this paper is the sim-

25

plification of the path planning problem, such that a simple agent could navi-

gate along collision-free trajectories without deliberative planning. Robots, and

robotic manipulators could then choose collision-free paths in real time instead

of relying on a separate deliberative process to pre-plan such paths off line. The

concept of simplifying the problem of navigation to one that is soluble in real

time is important to the algorithm that is proposed in Section 3.1.

To enable this simplified behavior, the method of APF models a robot’s envi-

ronment as a potential field in which obstacles carry a repulsive charge and the

objective carries an attractive charge. In this model, any point in the environ-

ment has an associated potential that acts to drive the agent from obstacles and

toward goals, thus the robot need not stop to deliberate about its next course

of action. It can simply move as if motivated by the artificial potential, and

assuming there are no local minima, it will eventually arrive at its objective

without collision.

However, for practical robot navigation in complex environments, the assump-

tion of no local minima is often invalid. Saska et al. [33] gives a simple example

of an obstacle that lies directly between a mobile robot and its objective. In

this case, the obstacle’s repulsive force is exactly balanced by the objective’s

attractive force, and a robot that is guided only by APF navigation will simply

stop at the balance point. The algorithm presented below bears some similar-

ities to APF, and in fact could be recast as a special case of APF navigation,

so the vulnerability to local minima or maxima is an important concern for the

agent behavior described in Section 3.1.

26

2.3 Swarm Intelligence

Another variety of optimization and path planning methods relies on the use of

swarm intelligence. Before exploring path planning literature that implements

swarm intelligence some discussion of the core concept is in order.

Several papers suggest the use of Ant Colony Optimization (ACO) techniques

[37, 14, 40] while others use Particle Swarm Optimization (PSO) to solve robotic

path planning problems [33, 24, 28, 11]. None of these applications is applied to

the marine environment, and most are not tested at all aboard physical robots,

but validated through simulation. A notably different approach is used by Witt

and Dunbabin [39] who employed a PSO algorithm in combination with other

optimization programs to develop a mission planner that was successfully tested

aboard an AUV in an estuary.

2.3.1 MAKLINK Graph – Habib 1991

While not a path planning algorithm in itself, the development of the MAK-

LINK graph is important because it demonstrates one method of discretizing

a continuous environment and modeling it as a searchable graph, specifically

intended for robot navigation [18]. As such, it is employed as an initial step in

some of the following methods .

The MAKLINK graph begins by mapping all obstacles in an environment. Each

obstacle is then expanded by a margin of safety that is specified for the type of

robot that will be used. A line then connects each vertex of an obstacle to all

other vertices that are visible from a given vertex, such that the newly drawn

27

Figure 2.3: Example MAKLINK graph discretizing navigable space for robotnavigation. Images directly reproduced from [18].

lines pass only from one vertex, through empty space to another vertex. Next,

a line is drawn from the midpoint of each of these new lines to the midpoints

of all other new lines. Finally, all of the initial construction lines are removed,

as are any lines that collide with objects, and the result is a graph that follows

through the center of the environment’s free space, thus avoiding collisions. An

example from [18] is shown in Figure 2.3.

2.3.2 Ant Colony Optimization – Bonabeau 1999

Bonabeau et al. [8] describe the principles of ACO. A large number of individ-

ual agents (“ants”) are initialized in a graph, and each independently wanders

between nodes. The agents in ACO choose successor nodes based on a prob-

abilistic choice between nodes that are close (like in the Dijkstra algorithm),

and nodes that have been commonly visited by other agents. The assumption

28

is that better routes will be chosen more often than worse routes, and eventu-

ally the population will tend to converge to good solutions of the problem. It is

important that the preference for close nodes or heavily trafficked nodes is prob-

abilistic, because this ensures that some agents will wander off of the established

routes, which limits the tendency to settle into local minima. ACO has proven

to produce good solutions to computationally difficult problems, but there is no

guarantee that it will find the optimal solution to any given problem.

2.3.3 Intensified ACO – Fan 2003

Fan et al. [14] uses a black and white image as a map of an environment.

In this model, black pixels represent obstructions, and white pixels represent

free space. An agent is considered to occupy one pixel at a time and can

choose to move from its current position to any of the four pixels immediately

adjacent to it. The agents search for an optimal path by wandering through their

environment guided by an attraction to simulated pheromone, until reaching

their objective.

To encourage faster solution when no pheromone is present, an ant will choose

its next position based on an heuristic estimate of the remaining distance to

the goal. When suitable paths are found, they are post-processed to smooth

them, and the algorithm iterates until an acceptable solution is discovered. Fan

et al. [14] demonstrates that this process converges to short, collision-free paths

through obstructed environments after some time.

This algorithm implements principles of ACO for path planning, but the act of

reducing the problem space to a 4-connected graph, and then weighting nodes

29

by heuristic estimates of remaining distance reduces the benefit of a stochastic

system like ACO. Since a heuristic estimate of distance is known for each node,

and the agent is only allowed to move along the four-connected grid, the prob-

lem is effectively collapsed to a graph search, and A* would find the shortest

unobstructed distance in a single iteration.

2.3.4 Dijkstra and ACO – Tan 2006

Tan et al. [37] uses a three-step approach to finding an optimal path through an

environment. First a MAKLINK graph of the environment is generated, so that

only paths through free space exist in the graph. Next, Dijkstra’s algorithm

is used to find the shortest path through the MAKLINK graph. Since the

MAKLINK graph keeps the path roughly centered between obstacles, it does

not permit an agent to take a shorter route that would pass closer to an obstacle.

So as a final step, the graph is expanded to include the unobstructed space on

either side of the chosen path, and the new graph of the free space is searched

using ACO.

The initial discretization according to the MAKLINK algorithm ensures the

safety of the robot, and Dijkstra’s algorithm, while less efficient than A*, does

find the shortest route though that graph. However, as in [14], the final formu-

lation of the environment is still a graph with known costs, and the reason for

abandoning proven graph searching algorithms is unclear.

30

2.3.5 Hybrid ACO with APF – Xing2011

Xing et al. [40] uses a hybrid of Artificial Potential Fields and Ant Colony

Optimization. In this algorithm, the environment is initialized with a certain

amount of pheromone, where the pheromone is distributed according to distance

from a goal and from obstacles. This method violates a core concept of swarm

algorithms–specifically it is not biologically feasible. But it also reduces the

initial amount of random wandering required by a pure ACO. The environment

is then searched for a viable route using normal ACO, where at the end of

each iteration, the pheromone strengths of all successful routes are reinforced

in inverse proportion to their lengths. This process is repeated until a solution

is found.

Then when the robot is deployed, it navigates according to an APF algorithm.

However, instead of only having an attraction to the objective and a repulsion

from the obstacles, it also has an attraction to the ACO-defined path. This

modification helps to overcome some of the local extrema problems associated

with APF navigation. To further help the robot avoid oscillating between local

extrema, the navigation system maintains a taboo list of previously visited

locations [40].

Hybrid ACO with APF differs from the preceding two methods in that it does

not use a graph-based model of the environment. Potential fields and pheromone

strengths can be modeled as continuous functions of space, so that discretization

is not required.

31

2.3.6 Particle Swarm Optimization – Kennedy 1995

Kennedy and Eberhart [22] introduces the PSO algorithm as a general function

optimization method. In this case, a large number of agents (“particles”) are

randomly distributed in problem space. Each of these agents is a potential

solution to the problem, and it is capable of moving in a space with the same

number of dimensions as the solution’s number of parameters. The fitness of

the agents is based on the quality of the solutions that they represent, and all

agents tend to move toward their more fit neighbors. The motion of the particles

in their space is roughly Newtonian such that each particle has a velocity and

acceleration in each dimension as well as some inertia. At each iteration, each

agent experiences an acceleration in the direction of the particle with the highest

fitness as well as an acceleration in the direction of the highest fitness solution

ever discovered by the group of agents. As time progresses, the particles tend

to settle into good solutions, and careful tuning of parameters of the algorithm

helps to avoid local minima. Like ACO, PSO does not guarantee an optimal

solution to a problem.

The papers reviewed here that use PSO for robotic path planning use some

variation of spline optimization. They construct a series of spline curves from

the robot’s position to its goal, and then use PSO to optimize the parameters of

the splines so that they minimize the overall cost. The cost generally includes

distance, time, or energy, as well as obstacle collision [33, 24, 28, 11]. One

problem common to all of these methods is that a spline can be of arbitrary

order, and the choice of order restricts the number of obstacles that can be

avoided. Some compromise is necessary, which either results in sub-optimal

paths, or unnecessarily long computation.

32

2.3.7 Optimized Reactive Systems – Rhoads 2010

Purely reactive navigation systems have the advantage of speed because they

have very specific responses mapped to stimuli, so that no “thinking” is re-

quired. However, since reactive systems do not deliberate, they have no means

of compensating for unexpected circumstances, so it is important to ensure that

a reactive system is well optimized for its intended environment. One example

of an optimized reactive system can be found in control systems whose parame-

ters are adjusted for their expected environments. These are described in terms

of mechatronic applications by Oberschelp and Hestermeyer [27]. Similarly,

Rhoads et al. [31] mathematically derives a feedback control law that would

allow an AUV to navigate through a known ocean current field in minimum

time.

While tuned control systems do use deliberative optimization to develop well-

suited reactive systems, they employ a different implementation than the one

proposed below. Control systems are largely built on detailed mathematical

models, which allow optimization by solution of the model’s equations. Whereas

the agents used by the DORA algorithm are optimized through stochastic ex-

ploration of the problem space.

2.4 Iterative Optimization – Perdomo 2011

Fernandez-Perdomo et al. [15] suggest an algorithm in which an agent is intro-

duced to a modeled ocean environment, and navigates to its goal by choosing

a bearing. At each step of the algorithm, the agent is allowed to move through

33

the forecast environment according to a simple kinematic particle model. The

agent travels in the direction of its bearing at a set speed until the next itera-

tion of the algorithm and its position and trajectory are evaluated. This process

is repeated until the agent arrives at its goal, and the sequence of bearings is

returned as the result. Iterative optimization is applied to improve the quality

of the solution. [15] find that this method works well, and provides realistic,

smooth trajectories for slow-moving AUV’s navigating in ocean currents.

2.5 Steering Behaviors – Reynolds 1999

Finally, one of the works that is most fundamental to this project is a description

of agent steering behaviors described by Reynolds [30]. Reynolds describes a

number of very simple, reactive behaviors that allow autonomous agents to

move through an environment in various useful ways without deliberation and

without careful scripting or planning by a human. These behaviors are an

expansion beyond his earlier work developing a system to model visually realistic

bird flocking by using a combination of simple steering behaviors [30]. The

emphasis throughout Reynolds’ work is on the development of mathematically

simplistic behaviors that can be used in combination to produce some desired

effect without user supervision. In particular in his work these are often applied

to cinematic rendering, and video game character behaviors [30].

Among the behaviors Reynolds’ describes are Seeking and Fleeing where an

agent attempts to either close on a position or evacuate from it, Pursuit and

Evasion, where an agent attempts to follow or escape from another agent, Arrival

where an agent decelerates on approach to an objective to avoid overshooting

34

it, Path Following, Obstacle Avoidance, and even Flow Field Following, where

agents attempt to follow paths, avoid collisions, and align themselves with vector

fields.

Reynolds describes a number of ways these behaviors can be combined to

create desired effects, and provides demonstration applets via a website at

http://wwww.red3d.com. These behaviors form the fundamental basis for some

of the key mechanics used in the steering behavior described in Section 3. An

important note is that the basis of the DORA algorithm is more closely related

to these simplistic steering behaviors than to theoretical control systems such as

those described by [31]. While this method lacks the mathematical rigor of the

control systems approaches, it has the benefit of very simple implementation,

which makes it more versatile than some other alternatives. Since the only math

required is algebra and trigonometry, the parameters of the algorithm can be

easily modified for different applications.

2.6 Tracking and Homing

Two trivial solutions to the problem of navigating in an ambient current field

will serve as standards of comparison in the experiments of Section 7. Homing

is a technique that ignores the local current, and always orients the agent to face

directly toward its objective. This has the advantage of simplicity, but making

no attempt to compensate for currents increases the length of the trajectory

traveled by a homing agent when compared to tracking. Tracking attempts to

minimize the distance traveled by compensating for cross-track components of

ambient current. Orienting the agent so that it faces upstream from its objective

35

Figure 2.4: Tracking and homing behaviors illustrated. Tracking compensatesfor local currents, while homing always orients directly toward its objective.

allows it to avoid being pushed off of a straight-line path by the current. These

two approaches are shown in Figure 2.4.

Since the ideas and algorithms of graph search, the bio-inspired aspects of swarm

intelligence, and the reactive steering behaviors of Reynolds’ work form the ba-

sis for the development of the algorithm described in the next chapter, these

preceding discussions provide useful context. However there is, as always, some

room for further exploration. Specifically, the swarming algorithms are con-

strained to biological feasibility, sometimes at the expense of optimal solutions.

The graph search algorithms require discretizing the environment into repre-

sentations that are amenable to graph-based methods, which may or may not

remain faithful to the original problem. The reactive systems are limited to

reactions to the stimuli for which they are designed. And all robotic naviga-

36

tion algorithms, including the one presented here, are constrained to solving an

abstract mathematical model of the navigation problem, rather than the prob-

lem itself. It is the hope of the following chapter that the proposed algorithm,

while hardly optimal, provides a new and useful balance of some of the concerns

associated with the works described above.

37

Chapter 3

Software Description

3.1 Concept of Operation

The algorithm evaluated here takes inspiration from the aggregate spawning be-

havior of some species of fish. To reproduce, such fish must navigate over large

distances without overall maps of their routes, and arrive at a specific time of

year. Fish that fail to complete the journey, or fail to arrive within a temporal

window also fail to reproduce, so they do not contribute to the next genera-

tion’s population. So the constraints on these creatures suggest a framework

for development of an artificial agent that operates in a similar environment and

with similar goals. Specifically, the agent is subject to navigation in temporally

and spatially variable ocean currents. It must navigate from a starting point to

an objective within a limited amount of time while using a limited amount of

energy, and it is incapable of making a global plan of its route.

While biology provides inspiration and an interesting context for the problem,

38

this system is not intended to model any actual organism, and biological feasibil-

ity is only taken as a guideline rather than a constraint. This work then could be

categorized as bio-inspired or swarm-based, but it is not truly biomimetic nor is

it a swarm intelligence method, as biomimicry is a prerequisite for swarms.

Toward this end, each agent will attempt to satisfy the four following prefer-

ences:

1. Avoid collisions with land.

2. Travel with strong currents.

3. Maintain heading.

4. Travel toward the objective.

Intuitively, each of these preferences will be more or less important depending on

the agent’s circumstances, so each is weighted based on some general conditions.

Then during optimization, the relative weighting of each preference is adjusted

to tailor the agent to the particular circumstances. Often these preferences will

conflict with one another, and while there are various schemes that can be used

to resolve such conflicts, the one chosen here is a simple weighted average of

vectors, which expresses the agent’s preferences. This summation is shown in

vector form in Equations 3.1-3.6.

39

p = a + b + c + d (3.1)

a = A

(1

1 + eMtc−MT

)(1− 1

1 + etc

)∇F (3.2)

b = B

(V∞

Va + V∞

)〈sin θ∞, cos θ∞〉 (3.3)

c = C〈sin θa, cos θa〉 (3.4)

d = D〈sin θT , cos θT 〉 (3.5)

tc =h

∇F · 〈sin θa, cos θa〉(3.6)

p Agent’s Preferred Direction Vectora Collision Avoidance Preference Vectorb Local Current Preference Vectorc Stability Preference Vectord Homing Preference VectorM Slope Parameter for Collision AvoidanceT Temporal Horizon for Collision Avoidanceθ∞ Heading of Local CurrentV∞ Local Current VelocityVa Agent’s Velocitytc Estimated Time to Collision∇F Gradient of Ocean Floorθa Agent’s Current HeadingθT Heading to Agent’s TargetA,B,C,D Behavioral Tuning Coefficients

The preference weighting parameters A,B,C,and D are each constrained to

any number between -1 and positive 1 inclusive, but are normalized so that

|A| + |B| + |C| + |D| = 1. This way the total preference is expressed as a unit

vector oriented in the direction that the agent would like to steer. These are

the parameters that the algorithm will optimize during execution. It should be

40

noted that a weight of zero eliminates a preference from consideration, and this

may be used to reduce the computation complexity. For instance in deep, open

water, constraining A to zero and searching in the space defined by the other

three preferences is perfectly viable, and is used as an early proof of concept

test for the algorithm.

Examining the preference vectors in turn, a represents the first preference of

the agent to avoid colliding with land. However, collisions with land are of little

concern in open water, or extremely deep water, so the term is weighted by two

sigmoid functions that reduce this preference’s consideration when the agent

considers collision to be unlikely. In particular, tc estimates a time to collision

based on the ocean floor’s gradient and depth and the agent’s speed as shown

in Equation 3.6.

The term ( 11+eMtC−MT ) is a sigmoid function that varies the weight of the prefer-

ence vector from zero to one. As the estimated time to collision becomes small,

the weight of the preference increases. The rate at which the weight changes

with respect to tc is determined by slope term M . Large values of M corre-

spond to vary rapid changes in weight, whereas small values of M correspond

to gradual changes. For all trials in presented here, M = 0.001. The temporal

horizon term, T is a measure, in seconds, of the value of tc for which the function

reaches 0.5. In general, a value of 2,700 seconds is used for these experiments.

This way, when the gradient of the ocean floor is small or distant, the agent has

very little preference to steer away from it, but as a threat becomes more im-

minent, or the margin of safety is reduced, steering to avoid collisions increases

in importance.

However, because of the definition of tc in Equation 3.6, if the ocean floor

41

is dropping away from the agent, tc becomes negative, which would give the

collision avoidance term a large weight, when collision is least likely. Since this

is counter to the behavior that is needed, the second term, ( −11+etc

+1) is defined.

This drives the weight to zero for negative values of tc. For a more rapid change

in weight, the exponential tc can be multiplied by an arbitrarily large number,

but for the tests here, its coefficient is left at 1.

The second preference, b, is weighted by the relative strength of the current

compared to the agent’s speed by the term V∞Va+V∞

this way, as the local current

speed increases in comparison to the agent’s speed, the preference to either fight

or avoid the local current becomes more important. In practice, this is a good

example of a preference whose weight is often optimized to a negative value.

With a negative value of B, the agent will tend to turn into the current by

an amount related to the strength, which applies some compensation to being

pushed off course by the current.

The stability preference, c acts as a means of reducing (or promoting) the

agent’s tendency to change course. Since, for positive values of C, the vector

acts in the direction that the agent is already facing, this can introduce an

artificial sense of inertia, and prevent the agent’s changing course very rapidly

due to a rapidly changing current field.

The homing preference d is the most important of the four preferences because

it is the only one that represents a valid general solution to the problem of

navigation without other influences. Since this vector always points toward the

objective, the agent could travel all the way to its destination while ignoring all

other preferences–assuming the agent could maintain sufficient speed and had

no intervening obstacles. While the other three preferences are independent of

42

one another, they all depend on the homing preference to drive the agent in the

direction of the goal.

Weighted averaging has the advantage of simplicity, but also has limitations for

navigation. The most significant limitation is that opposing preferences may be

averaged to a solution that does not sufficiently address either. If all other terms

are ignored and the weights for navigating with currents and toward the agent’s

objective are equal, then it easy to anticipate a situation where the local current

heads directly away from the objective, and the agent resolves this conflict of

preferences by setting a course perpendicular to the current and the objective.

Far from taking advantage of the benefits of either behavior, the agent simply

wanders off. Millington and Funge [25] discuss this phenomenon as well as some

solutions.

Carefully choosing preference weights and tailoring them to the environments

that the agent is likely to encounter should significantly reduce these occur-

rences, but will not completely eliminate them as possibilities. Since this al-

gorithm will employ a large number of agents, some anomalous behavior is

tolerable, but this limitation cannot be neglected.

One solution to the averaging problem is to execute a behavior based on only

one of the preferences at a time. This way the agent could, for example, take

immediate action to avoid a collision, and then when sufficiently far from a

threat could turn its attention to efficient navigation. Different schemes can be

used for prioritizing responses, and the averaging and prioritizing methods can

be combined to produce different behaviors.

An agent navigating with a prioritized response runs the risk of settling into

43

oscillation about a local minimum on the boundary of priority between conflict-

ing behaviors. As an example, if other preferences are ignored and an obstacle

lies directly between an agent and its objective, the agent could be prompted

to head directly toward the objective until it approached within a critical range

of the obstacle. Then the priority would change to obstacle avoidance and the

agent would head directly away from the objective until it exceeded the criti-

cal range. Priority would shift back to objective homing, and this cycle would

repeat indefinitely.

Trapping in local minima will probably be infrequent because of the temporal

variability of the current field, and the natural complexity of the environment.

Indeed the variability of the currents alone ensures that the agent’s problem

space is never constant, and simply repeating the same action in the same

location will lead to varying results as time passes. While temporal variations

combined with the use of stochastic behaviors will reduce the probability of

an agent’s becoming trapped, these same properties prevent any mathematical

claim to optimality or even guaranteed solution of the problem.

For this reason, the algorithm can only be demonstrated to be practical in many

cases, but cannot be shown to be universally applicable. Conversely, using

an algorithm such as A* is guaranteed to find the minimum cost path in the

minimum time, however it is practically limited by its model of the environment.

A* searches a graph very effectively, but the physical world is not a graph, and

any attempt to model it as one necessarily sacrifices some detail. Increasing

the quality of the model–by increasing the spacial or temporal resolution of

the graph–impacts the size of the graph and the time required to search it.

Some experimental results of these compromises will be discussed in Section

44

5.2.3.

The experiments in Section 7 will compare the proposed swarm-based stochastic

algorithm to A*, not in terms of absolute accuracy or mathematical optimality,

but in terms of practical implementation for real-world robotic path planning.

In this context the speed of solving the problem, and the ability to replan and

adapt to changing environments can quickly become more important than the

optimality of the solution. Likewise a small memory footprint can be criti-

cal to execution aboard a deployed vehicle with very limited computational

resources.

3.2 Detailed Description of Operation

This section details the internal structure of the present version of the software

used to test the DORA algorithm. Section 5 describes the progressive develop-

ment of the path planning software including iterative design modifications and

preliminary experiments, while Section 7 discusses trials and results.

The software is written the Python 2.7 programming language, and takes ad-

vantage of the third party libraries numpy, scipy[21], and matplotlib[20] as

well as the included Python standard libraries time, sys, and random.

The software begins by loading four-dimensional velocity and three-dimensional

bathymetric data for the operational area for a set of trials. These data are

stored in Velocity Field and Height Field data structures respectively. To be

successfully loaded, the data must be stored in numpy’s native compressed *.npz

format. Preprocessing to convert data from the NetCDF *.nc files that are

45

commonly used by the scientific community into a compatible format is per-

formed using a separate script, nc2npz.py whose source code appears in the

Appendix.

In addition to the data, a list of waypoints is loaded from a comma separated

values (CSV) file. Each line in the file consists of two latitude and longitude

pairs representing the starting and ending points of a trial. For the trials used

in this research, the CSV files are generated with a separate Python script

GenWaypoints.py.

Once the data are loaded, a number of global variables are used to configure

the trial, and the names and values of the configuration variables are exported

to an XML File.

Following the initial configuration, the software executes the trials enumerated

in the waypoints CSV file.

Each trial consists of two phases: Exploration and Exploitation. The explo-

ration phase uses a large number of randomly initialized agents to search for a

valid solution to the navigation problem. As soon as a viable solution is found,

exploration ends and the exploitation phase begins. Exploitation uses the solu-

tion found during exploration to begin a simulated annealing process. Specif-

ically, the preference parameters of the best solution found during exploration

are used as the mean for normally distributing the preferences of successive

generations of agents. In each epoch of exploitation, the standard deviation of

the normal distribution is reduced in order to refine the solution.

The internal mechanisms of the exploration and exploitation phases are similar.

46

Both initialize a number of agents at the trial’s origin, and they model the

motion of the agents according to their preferences and the local current from

the velocity field data. At each time step, each agent chooses a heading based on

its preferences and the local conditions of the environment. The agent’s speed is

resolved into East and North components, and added to the local components of

current. The agent’s position is updated based on the resulting velocity vector,

and the process repeats until all agents have been eliminated by meeting some

stop condition. The conditions which eliminate agents from consideration are

enumerated below.

1. Traveling outside of region of interest .

2. Exceeding time allowed to complete mission.

3. Expending all available energy.

4. Colliding with terrain.

5. Arriving at objective.

At each timestep, Condition 1 is evaluated by a simple comparison of the agent’s

position with the minimum and maximum latitude and longitude in the region

of interest. If the agent’s position is outside of these bounds, it is eliminated

from consideration.

Conditions 2 and 3 are both evaluated by a simple check of the agent’s mis-

sion timer and available energy respectively. If either is exceeded, the agent is

eliminated.

Condition 4 is evaluated by estimating the depth of the ocean floor at the agent’s

47

location using bilinear interpolation of the height field data set. If the agent’s

depth is greater than the estimated depth of of the ocean floor, then the agent

is eliminated.

Condition 5 is the only condition for which the agent’s preferences are recorded.

If the agent is within a specified range of its objective, it is considered to have

arrived. To ensure that the agent’s arrival is detected, the threshold is set to a

range equal to the sum of the agent’s speed and the maximum recorded ocean

current velocity, times the model’s timestep. If the range threshold was set

to a larger value, then it would be possible for the agent to “skip” across its

objective between timesteps. However, this range introduces a small error in

time tracking. To reduce the effect of the error, the agent’s travel time is not

simply recorded as the time required to reach the range threshold. Instead, the

agent’s rate of approach is calculated for the arrival timestep, and time required

to travel the exact remaining distance to the objective at the agent’s rate of

approach is added to the mission time when the arrival is first detected.

If the agent is the first to arrive at the objective, or if its travel time is less

than any previously recorded, the agent’s time and preference parameters are

recorded as the solution to the problem.

Once all agents have been eliminated from consideration, the epoch counter is

incremented. A new list group of agents is generated, and the modeling process

repeats for the specified number of epochs. Once all epochs have been com-

pleted, the relevant parameters from the trial are recorded in a CSV file.

48

3.3 Supporting Software Utilities

A number of separate Python scripts were written in order to facilitate pre-

and post-processing of data used in the path planning experiments. Each of

the scripts is described below, and their source code is recorded in Appendix

C.

Each script is designed for use by the program developer rather than an end

user, and as such requires modification of its source code for use.

3.3.1 ncinfo

The ncinfo.py script is used to provide information regarding the internal

structures and information stored in a NetCDF file. In general, the data pro-

vided include the names of the variables, the minimum and maximum values,

the number of entries in each data set, and the units of the data stored in

each variable. This is one of the few utility scripts that can be operated via

a command line interface (CLI). The name of the NetCDF file is passed as a

command line argument to the script, and the script then prints the internal

information to the console. While there is a CLI to this script, the structure

of NetCDF files varies significantly between sources, so modification of source

code may still be necessary for effective use.

49

3.3.2 nc2npz

The nc2npz.py script reads in a NetCDF (*.nc) file that stores four-dimensional,

regularly gridded ocean current data, and converts it into a numpy native for-

mat consisting of six arrays. Each array stores one of the following datasets:

latitudes, longitudes, times, depths, East components of velocity, and North

components of velocity. These arrays are then saved to a single compressed

file with the .npz file extension, which allows for easy import using the numpy

library.

3.3.3 toponc2npz

The toponc2npz.py script performs a function similar to the nc2npz.py script.

This script reads in topographic data saved in a NetCDF file, and converts it

to a set of numpy arrays, which store latitudes, longitudes, and depths, which

are then saved in a compressed .npz file.

3.3.4 GenWaypoints and GenWaypointsGulf

The GenWaypoints.py and GenWaypointsGulf.py scripts were used to gener-

ate a comma separated values (CSV) file consisting of pairs of latitude longitude

pairs. Modifying this file allows the programmer to specify the types of way-

points generated, and the CSV file is then taken as input to the path planner.

This allows for a randomized set of trials that can be repeated for consistency

between experiments.

50

3.3.5 GrouperBehavior-P3d

The GrouperBehavior-P3d.py program is a visualization program, used only

as a verification of agent behavior. It uses the free 3-D engine, Panda 3d to

render a model of an agent navigating through a simple current field. The user

can modify the source code to choose the behavioral preference parameters, the

local current strength and direction and the positions of starting and ending

waypoints. This allows a visual verification that the agent is behaving as ex-

pected in the presence of the current field. During early development this script

was used to find and correct navigation errors including those with great-circle

navigation and local current compensation.

3.3.6 GrouperView-P3d

The GrouperView-P3d.py script is used to visualize the behavior of an agent us-

ing the full four-dimensional ocean current data. This is similar to the function

of GrouperBehavior-P3d.py, except that the latter includes full ocean current

data rather than a simple, constant, uniform field used in the former.

3.3.7 popro

The popro.py script is used for numerical post processing. It reads in the data

from a trial log, which is generated as described in Section 3.2. The data are

stored in a numpy array, which allows simple processing for analysis of results.

The script can be adjusted to report success and failure rates for each of the

different types of agents, as well as average, standard deviation, and extrema of

51

any other parameters reported in the trial log. This is the software that is used

to compile the information reported in the tables in this dissertation.

3.3.8 HullProp

HullProp.py includes automation of the hull shape formulas described in Sec-

tion 6.1, such that design parameters can be changed in script, and a new hull

drawn and analyzed by the program. This script simplifies calculation of hull

shape and flotation properties as well as construction material requirements. It

also provides a table of offsets to facilitate building.

Having reviewed a conceptual model of the software, the following chapter ex-

plores its implementation. Much of engineering consists in the transition from

design to functional device, and the process is similar for software development.

So if this, Chapter 3 has been analogous to design, Chapter 5 documents the

initial stages of building a prototype and its iterative revisions and improve-

ments. However, a brief detour is in order. Before exploring the details of

DORA’s early development, a discussion of its intended operating environment

will provide useful context, and this is presented next in Chapter 4.

52

Chapter 4

Environmental Model and

Regions of Interest

Oceanographic data for the trials below are four-dimensional arrays of North

and East components of velocity. There are two regions of interest (ROI’s)

that are used in the experiments of Section 7, one in the Gulf of Mexico

that consists of entirely open water at the surface and one in the eastern

Caribbean, which is obstructed by a chain of islands. The time chosen for

these trials is the week beginning at midnight, May 5, 2012. This timeframe

was chosen because it showed a complex current field in the Gulf dataset,

which would provide a more challenging environment for navigation. The

data grid has a temporal resolution of three hours and a horizontal resolu-

tion of approximately 1/36 degree at standard depths from zero to 5000 meters

(http://www.northerngulfinstitute.org/edacNCOM AmSeas.php). The source

of the data was described in e-mail correspondence with Dr. John Harding of

the Northern Gulf Institute as follows:

53

The U.S. Navy AMSEAS nowcast/forecast retrospective data was

provided through a NOAA/Navy cooperative effort including the

Naval Oceanographic Office, NOAA’s National Coastal Data Devel-

opment Center and the Northern Gulf Institute, A NOAA Cooper-

ative Institute.

Since the data are provided on a regular grid and the paths to be planned by the

software will not be thus constrained, a four-dimensional linear interpolation

is used to estimate the local magnitudes of the East and North components

of velocity at an agent’s location given by its latitude, longitude, depth, and

mission time. For simplicity, all missions are assumed to begin at the same time

as the data.

Figure 4.1: Gulf of Mexico Region of Interest with line segments showing therandomly generated navigation problems posed to competing navigation sys-tems. Image from Google Earth Software overlays from custom generated .kml

54

Figure 4.2: Caribbean Region of Interest with line segments showing the ran-domly generated navigation problems posed to competing navigation systems.Image from Google Earth Software overlays from custom generated .kml

4.1 Bathymetric Data

Like the ocean current data, the bathymetric data used for collision detection

and agent preference are provided in the ETOPO1 gridded data set. These data

have a horizontal resolution of 1/60 degree. ETOPO1 is available in its entirety,

but only the data relevant to the regions of interest are used in each case, which

reduces the program’s memory requirement. These data are acquired through

ETOPO1’s project webpage at http://www.ngdc.noaa.gov/mgg/global/global.html.

Similar to the ocean current data, the depth at an arbitrary position is linearly

interpolated from the nearest points that lie on the grid.

55

4.2 Regions of Interest

4.2.1 Gulf of Mexico

Figure 4.3: Gulf of Mexico Velocity Field at Initial Timestep. Image generatedby ERDAP Service at http://wwww.northerngulfinstitute.org

For open water trials in which the agents cannot collide with the ocean floor,

the region of interest lies between 23.1◦ and 26.1◦ North latitude, and between

83.5◦ and 88.5◦ West longitude. These data are used in the Experiments in

Chapter 7, as well as in the software prototype described in Section 5.4.

56

Figure 4.4: Gulf of Mexico Velocity FieldGulf of Mexico ROI Bathymetric Map. Closely spaced contours on the rightof the image are at the boundary of Southwestern Florida’s continental shelf.Image generated using matplotlib Python library [20] with data from ETOPO1[7]

4.2.2 Caribbean

For obstructed water trials in which successful agents must avoid collisions with

land masses or bathymetric features, the region of interest is bounded by 14.5◦

and 16.5◦ North latitude and between −63.0◦ and −59.5◦ East longitude. This

region was chosen because there is a chain of islands running roughly North

to South in the center of the region. For these tests, all waypoint pairs are

oriented East-West, so that the island chain presents a series of obstacles, but

the starting and ending points are in open water.

57

Figure 4.5: Caribbean Velocity Field at Initial Timestep. Image generated byERDAP Service at http://wwww.northerngulfinstitute.org

58

Figure 4.6: Caribbean ROI Bathymetric Map, where white is at or above 0m el-evation, and gray is at or below 0m elevation. Image generated using matplotlibPython library [20] with data from ETOPO1 [7]

59

Chapter 5

Software Development

This project has seen much development over a fairly long period of time, with

early work on mission planners being conducted during summer internships of

2011 and 2012 at the Navy’s Space and Naval Warfare Systems Center Pa-

cific (SSC-Pac). This chapter provides a synoptic description of the software’s

progression from an early 2-D graph-based mission planner, which laid the foun-

dations for later work, through the 4-D A* planner that is used for this project’s

standard of comparison, and finally through the early concept tests of DORA.

As software development bares strong similarity to other varieties of engineering

design, there are some decisions that were made based on theory and calcula-

tion, some that were made as matters of reasoned assumptions and experiential

best guesses, and some that were absolutely arbitrary matters of preference.

This chapter documents the important milestones in the software development

and explains the motivations and justifications for decisions that were made

along the way.

60

5.1 2-D A* Planner

An initial prototype of graph-based path planning software was developed dur-

ing an internship at the Space and Naval Warfare Systems Center (SSC) Pacific.

This software imported oceanographic current data and used it to construct an

8-connected graph of an AUV’s operational area. The weights of the graph’s

edges were set equal to the time the vehicle would require to traverse them as-

suming a constant vehicle speed. An arbitrarily high cost was assigned to edges

with opposing currents that were greater than the vehicle’s operational speed,

as well as edges that resulted in collisions with land. Another cost term was

included as an inverse distance from user-specified points. This was to allow

the user to specify specific points for the AUV to avoid, such as areas of high

risk or areas occupied by shipping traffic. Once this graph was generated, the

user was able to specify an arbitrary-length sequence of waypoints for the AUV

to visit. The planner then used an A* search to find the minimum cost route

between the initial waypoint and the final one. An example of a path planned

by the software is shown in Figure 5.1. In this example, the current data was

read from a file produced by the U.S. Navy’s COAMPS-OS forecasting software.

The vehicle speed was set to 1 m/s, which is comparable to the maximum local

currents. The figure shows that the software planned a path that significantly

diverged from a straight line route between the start point to the East and the

goal point to the West. The divergence from the straight line path took advan-

tage of the local currents and avoided collision with the island between the two

waypoints.

The path planner seemed to work very well in the cases tested. The planned

path would consistently avoid tracks that collided with land or opposed strong

61

Figure 5.1: Output of 2-D A* path planning program showing velocity vectorsand planned route for a mission from East to West off the coast of California

62

currents, and would navigate away from the user-specified points as required.

This software was developed to support the Unmanned Underwater Vehicle

(UUV) Control Center at SSC Pacific, so the program also included a means

to export the planned paths to a custom xml format, to Google Earth’s kml

format, to bitmap images, and to the Navy’s Composable FORCEnet(CFN)

software.

5.2 4-D A* Planner

While the two-dimensional planner successfully planned good routes through its

problem space, the basic assumptions upon which it was built neglected signifi-

cant aspects of the ocean environment. In order to address these limitations, the

planner was rewritten to include ocean current variations with depth and time.

Additionally, the new planner searches a dynamically generated graph, that is

independent of the data grid and allows for arbitrary graph geometry.

The new algorithm begins with a single root node at the vehicle’s initial posi-

tion. It then searches for a path by building a tree in accordance with the A*

algorithm. The cost to travel between any two points is calculated by mod-

eling the vehicle’s motion through the current field according to user-specified

parameters.

Since the graph is generated independently from the source data grid, it is un-

likely that current data will be available for the particular point in space and

time that the software would need for a cost calculation. The local ocean cur-

rents are linearly interpolated between points from the source data. Higher order

63

interpolation schemes may be employed by later versions of the software.

An example of the algorithm’s dynamically generating a tree and choosing a

path to avoid collision with an island is shown in Figure 5.2..

Figure 5.2: Example of A* Search avoiding collision with an island using abranching factor of 3 and an included angle of 90◦. The left image shows con-struction of the graph, while the right image shows the route as returned.

5.2.1 4-D Grid Independent Design

The cost of transitioning between two nodes in the graph is calculated by model-

ing the agent’s motion through a velocity field defined by gridded ocean current

data. Specifically, the agent’s speed and heading is resolved into North and

East components independent of the ambient current. Then the ambient cur-

rent’s North and East components at the agent’s location and time are linearly

interpolated from gridded data and summed with the agent’s velocity. The

agent’s position is updated by calculating the action of the velocity over a user-

specified time step. The interpolation-calculation step is repeated until the

agent is within an acceptable range of its objective or until the agent reaches

another stop condition.

64

The agent’s speed and heading are determined by its navigational behavior.

Preliminary tests have assumed that the vehicle homes toward its objective by

changing its heading to directly face its target at each time step. To model the

changes of depth during gliding, the agent is capable of descending at a specified

angle. The agent begins at zero depth, then upon reaching a specified maximum

depth, the agent begins an ascent at the same angle. This cycle repeats until

the agent reaches its objective.

By dynamically generating the graph during planning, and by using interpola-

tion of data to predict current velocities at arbitrary points in time and space,

the planner remains independent of the data grid. This has the advantage of

allowing for increased data resolution without increased calculation time. This

represents a substantial advantage over the original prototype of the software,

which directly tied the graph resolution to the data resolution and imposed sig-

nificant time penalties on using increased data resolutions for a given plan.

Additionally, the separation of the planning graph from the data grid will allow

the plan to be easily cross-referenced against other data sets. For instance,

during the cost calculation, the agent’s predicted motion can be checked against

a bathymetric data set to predict and prevent collisions with the sea floor, or

to allow other environmental factors to be considered during planning.

5.2.2 Testing of Prototype Assumptions

Initially the four-dimensional planner was used to test the assumptions of the

two-dimensional planner by constraining the new software to paths along the

surface and in a constant current field, effectively collapsing it to two dimensions.

65

To ensure a fair comparison, the new planner was also restricted to planning

paths along cardinal and intercardinal directions, and at the same resolution as

the data grid.

These tests showed that the two-dimensional assumptions are wholly invalid.

The paths planned using only 2-D data often had higher costs than a simple

straight-line path. Ocean currents are highly variable by region, and it may

be feasible to use two-dimensional path planning in some areas, however, using

four-dimensional data provides a significant improvement in planner perfor-

mance. Indeed, in the cases tested, using no data at all improved performance

over using the 2-D approximation.

5.2.3 Speed of Planning

While v0.2 produced more cost effective plans than v0.1, the time required to

plan the routes was significantly longer. In order to make the planner practical

for real-world applications, a series of several paths were planned between the

same start and end points, and with the same environmental data. For each of

these trials, the branching factor, included angle, spacial resolution, or temporal

resolution varied.

Table 5.1 shows the results of these trials, which are based on the 72-hour

forecast of 18

degree resolution NCOM data for the period beginning at midnight,

July 2, 2012. All of these trials searched for routes from 32.9◦ N 119.2◦ W to

33.5◦ N, 119.2◦ W. All trials used a spacial resolution of 6.6 kilometers and a

speed of 0.5 m/s.

66

Table 5.1: First Performance Trade Study

Trial Branch Factor Angle Temp. Res. Spac. Res. Cost Comp. Time0 9 360.0 600.0 6.6 49.58 4275.531 3 90.0 600.0 6.6 49.58 59.672 3 90.0 60.0 6.6 49.58 166.713 3 90.0 600.0 3.3 48.76 11067.374 5 180.0 600.0 6.6 49.58 1240.085 5 90.0 600.0 6.6 49.58 312.34

The choices of branching factor, included angle, and temporal and spatial res-

olution clearly have significant impacts on the software’s performance both in

the time required to plan the path, and in the quality of the path that is

planned. Any specific choice of parameters is necessarily arbitrary, and the

parameters chosen for one particular problem may not be ideal for another.

However, this series of trials points to some useful, practical guidelines in pa-

rameter choice.

First, all seven trials planned paths with identical total costs with the exception

of Trial 3, whose higher resolution allowed for a better solution. However, in the

case of Trial 3, the predicted savings in mission time was 0.82 hours, or about

1.7%. A more meaningful observation arises from a comparison of the cost to

the benefit. While the higher resolution saved 0.82 hours of mission time, the

planner required 3.06 hours longer than Trial 1, which used exactly the same

parameters other than resolution. In other words, the higher resolution cost

substantially more time in planning than it saved in the mission.

Second, reducing the branching factor, or reducing the included angle reduces

the number of solutions that the planner considers and risks the exclusion of

the best route. To evaluate the loss of performance due to exclusion of optimal

67

paths, the included angle was varied between 360.0◦, 180.0◦, and 90.0◦. Similarly

the branching factor was varied from 9 to 5 to 3. In all cases, the planner found

the same route, but the computation time varied from a maximum of 4275.53

seconds down to 59.67 seconds. For this reason, the path planner is set to a de-

fault branching factor of three with an included angle of 90◦. Spacial resolution

is set between about 110

and 17

of the distance between waypoints.

Temporal resolution has a less significant impact on the calculation time than

the other parameters, but it does have a critical limit. Since the cost function

models the motion of the agent through space and considers the agent to have

completed a transit when it is within a certain threshold, the temporal resolution

must at least be sufficient to prevent the agent’s skipping past its objective

between time steps. As an example, if the agent is required to navigate at

0.5 meters per second to within 100 meters of an objective, with an ambient

current magnitude of 0.25 meters per second, then the temporal resolution may

be at most 100/(0.25 + 0.5) = 133.33 seconds. A finer resolution improves the

accuracy of the model, but a coarser resolution would allow the vehicle to enter

and exit its threshold range between timesteps and could continually oscillate

across its objective.

One common solution to this problem is to allow an agent to track its range to

target, and to consider its objective to be attained when range stops decreasing

and starts increasing. This is a good solution in the absence of ambient currents,

or in any case where the agent can close on its objective at will. However, since

the agent can be pushed away from the objective by the local currents, this

method is not useful for the current problem.

68

5.3 NetLogo Prototype

Figure 5.3: Example of Screenshots from the NetLogo prototype show the agentsconverging to a solution over time. The left frame shows the agents widelyscattered early during the experiment, while the frame to the right reflects theirnarrower dispersion about a valid solution.

To test the concept of iterative optimization of reactive agents using the agent

preference formula, a NetLogo[38] model was created, which generated a current

field either randomly or according to user preferences, and then modeled a

number of agents’ motion through the field. Figure 5.3 shows screenshots from

different stages of an experiment.

Since this program was only developed as a concept test, no real-world oceano-

graphic data were used. Instead, to model the current field, three nodes were

set at user-specified coordinates, and each node was assigned a vector that rep-

resented the current at that location. Then the current at any location in the

model space was then calculated as a distance-weighted average of the three

vectors. This ensured that the current measured at each node was equal to the

current specified at that node, and that the values transitioned smoothly from

69

one node to another throughout the space.

For this model, no concept of epoch was used, and the program simply main-

tained the number of agents in the field by introducing a new agent at the start-

ing point any time an agent either arrived at the objective or was eliminated.

Simulated annealing was used to improve the solutions that were found from

the initial random search. This algorithm showed good results, with agents

often solving problems that were not soluble with simple tracking or homing

behaviors, and consistently producing comparable results in cases that were

soluble with the simpler behaviors. The original concept for the DORA al-

gorithm incorporated genetic algorithms as the optimization method, however

the positive results using simulated annealing led to its continued use in later

experiments.

5.4 Python Prototype in Gulf ROI

To test the basis of the algorithm for operations in real-world environments, a

Python program was developed, which functioned very similarly to the NetLogo

prototype, except that it used data supplied by the Northern Gulf Institute’s

oceanographic data repository for estimating ocean currents. To simplify the

initial tests, no bathymetric data were used, so a simplified preference equation

was employed, which neglected the first term. In practice, no first term was

implemented, but mathematically this was equivalent to using the full formula,

but setting A = 0. To ensure that neglecting the bathymetric data would not

impact the validity of the test, all trials were conducted in the Gulf of Mexico

test area, at a depth less than the minimum in the area. The results of these

70

trials were positive with the DORA system solving difficult navigation problems

more frequently than simpler agents.

5.4.1 Agents

All experiments began with 50 randomly initialized DOR agents, as well as one

agent that used a homing behavior and one agent that used a tracking behavior.

The homing agent aligned its heading directly toward its objective. Since all

of its available speed was in the direction of its objective, it made no attempt

to compensate for local currents, which resulted in its traveling faster along its

path, but over greater distance than the tracking agent.

The tracking agent resolved the local current into vector components with one

oriented parallel to the agent’s path, and the other perpendicular. The parallel

component only acted to speed or slow the agent’s approach to its objective,

but did not alter the agent’s path. The perpendicular or cross-track component

pushed the agent off its course.

The tracking agent oriented itself to compensate for the cross-track component

when this component was slower than the agent’s speed. This compensation

minimized the distance that the agent was required to travel, and represented

an ideal solution in the case of a constant, uniform velocity field that was slower

than the agent’s speed. When the cross-track component was greater than or

equal to the agent’s speed, the agent used the homing behavior.

In some trials, these constraints on the tracking behavior produced the counter-

intuitive result that the agent would expend so much of its speed overcoming

71

the cross-track current, that it would be pushed away from its objective. To

evaluate the impact that this would have on performance, the tracking behavior

was modified so that the agent would only compensate for cross-track current

in cases when it was still able to make progress toward its objective, and the

experiment was repeated. This trial showed a negligible decrease in overall

performance, and the behavior was restored to its original state. This lack

of apparent impact is likely a result of the nature of the trials. They were

conducted in fields that did not have guaranteed solutions, so in cases where

the agent could not make definitive progress toward its target, it was unlikely

to be successful regardless of its response. In cases when it had sufficient speed

to close on its objective, its contingency behavior was irrelevant.

Physical Model

The agents in this algorithm are intended to model the behavior of physical

vehicles deployed on a real-world mission, so the agents operate in a simplistic

kinematic model of an ocean current field. Each agent has a heading, a speed,

and a position stored as internal, local variables. At each time step the agent’s

speed is resolved into East and North components of velocity. These are then

added to the East and North components of local current, and the agent’s

position is updated by multiplying the resulting total velocity vector by the

model’s timestep.

In experiments to evaluate the agents’ ability to solve known problems, the

model is constrained to moving and navigating on a planar surface in a constant,

uniform velocity field. However, in the more practical experiments, motion is

modeled along a spherical approximation of earth that assumes a radius of

72

6371.0 kilometers [9]. Agent position is tracked as latitude and longitude. Since

the kinematic model uses normal SI units, the agent’s change in position is

calculated in meters, then converted to a change in latitude and longitude,

where the conversion factor is based on the agent’s position before the update.

This introduces an error since the conversion from meters to degrees of longitude

changes continuously as latitude changes, but since the change in position during

a typical timestep of the model is small compared to the radius of Earth, the

error is also small.

5.4.2 Test Conditions

For these experiments, all trials were conducted using a fixed depth of 25.0

meters. Since the physical model and planner can run at an arbitrarily specified

resolution, four-dimensional linear interpolation is used to estimate the velocity

at any requested depth, time, and position within the data set. For any request

outside the valid range of the data, the interpolation function returns a non-

numeric error value. The first timestep is shown in Fig. 4.3.

The objective of each trial was to minimize the time required to navigate over

a 150 kilometer course. To ensure a wide variety of problems, the start and end

points of the test mission were selected using the programming environment’s

native pseudo-random number generator.

First, a latitude and longitude pair was chosen within a window of the data

set that was at least 150 kilometers from the boundary of the data. Next, a

heading was chosen, and the point that lay 150 kilometers from the first point in

that direction was selected. There was some small variation in the actual length

73

between the start and end points, because the 150 kilometer range was estimated

using the planar range approximation at the mean latitude of the operational

area. To prevent a bias toward outward bound courses, the software randomly

selected one of the two points as the starting point, and the other was set as

the destination.

In each case, 50 agents were initialized at the trial’s starting point, and their

positions and headings were updated at each timestep of the physical model

according to their respective behaviors. If an agent moved outside the bound-

aries of the data or arrived at the destination, the agent was removed from the

trial. If an agent reached the destination, its time was recorded, and the overall

minimum time for each type of agent was tracked for each trial. Additionally,

the behavioral tuning parameters for the DOR agent with the minimum time

was recorded. For this project, the starting point and the objective were only

varied between trials, so all agents for a given trial attempted to solve the same

problem.

The criterion for successfully reaching the intended destination was that the

agent must be no farther from its destination than the spatial resolution of the

physical model, which was calculated by adding the maximum agent speed to the

maximum local current speed, and multiplying by the model’s time step. While

there is some error introduced by using this simple range threshold criterion,

the error can be kept to an acceptable minimum by choosing a sufficiently high

temporal resolution. The range threshold criterion is described in 5.1.

To reduce the error produced by the range threshold approximation, the agent’s

rate of change of distance to its objective was calculated for the timestep preced-

ing the agent’s crossing the range threshold, as in 5.2. The remaining distance

74

to the objective was then divided by the rate of change to give an estimate of the

remaining time required to reach the destination, which is shown in 5.3. This

time estimate was then added to the time required to reach the range threshold

to produce the total estimated cost for a trial.

rT = (Va + Vmax)× δt (5.1)

sprev =(rprev − r)

δt(5.2)

tm = tT +

(r

sprev

)(5.3)

rT Range threshold for arrival criterionVa Magnitude of velocity of agentVmax Maximum magnitude of velocity in data setδt Incremental timestep of modelr Range to objectiverprev Range to objective at previous timestepsprev Average rate of change of range to objective

over the previous timesteptT Time when satisfaction of arrival criterion

is detectedtm Total mission time recorded for agent upon

arrival

A single epoch of a trial consisted of the span of time during which any agent was

still being modeled. Once all agents were removed due to success or failure, the

next epoch began with the introduction of another 50 DOR agents, one tracking

agent, and one homing agent. Each trial continued until it had executed seven

epochs.

75

At the end of the trial, relevant data were recorded to a file including the trial

number, the starting and ending points, the minimum time achieved by each

class of agent, and the tuning parameters of the best DOR agent.

5.4.3 Results

Planar Earth with High Velocity

To test the performance of the algorithm and of the physical model, an exper-

iment was run in which all navigation and physical modeling was carried out

using a planar model of the mission area. The local velocity field was set to

a constant 2.0 m/s directly from West to East, and the agents’ speed was set

at 3.0 m/s. This choice of a simplistic field and a relatively high agent speed

ensured that all trials would have valid, analytically calculable solutions. The

results of these trials are shown in Table 5.4.3

Table 5.2: Results of Planar Earth TrialTracking Solved 100.00 %Homing Solved 100.00 %DORA Solved 100.00 %Tracking Best 97.5 %Homing Best 0.00 %DORA Best 2.50 %Average Tracking Error 2.23× 10−6 sAverage DORA v. Tracking 2.37× 10−2 %Average DORA v. Homing −13.39 %Average Epoch 4.41Average Time 163.10 s

As expected, all agents found solutions in this simple case. Analytically, the

tracking agents should arrive at a point objective in a predictable time, which

76

is calculated by dividing the distance from the starting point to the objective

by the net speed remaining after correcting for a local current. Over the 200

trials in this experiment, the average absolute difference between the predicted

transit time and the modeled transit time was 2.23 microseconds for tracking

agents, while their average total mission duration was 18, 780.7 seconds.

The tracking agents implemented the best solution found in 97.5% of trials,

while the DOR agents found a better solution in 2.5% of trials. The reason

that the tracking solution was not the best possible solution found in all cases

is that the range threshold criterion for arrival permitted a slight improvement

in mission time by using more of the agents’ available speed to move toward

the objective, and accepting a small amount of leeward drift. Since the distance

remaining when an agent crosses the range threshold is based on the rate of

approach before crossing the threshold, the agent needs not contend with op-

posing currents for the final distance to the objective. This solution is shown

in Figure 5.4. Finding this slightly improved solution was a rare occurrence,

and on average the DOR agents performed about 2.37× 10−2% worse than the

tracking agents. Still this discovery by the software suggests a high degree of

optimality within the boundaries of the current trial, and also emphasizes the

importance of proper representation of the physical environment to ensure valid

solutions.

This result emphasizes some of the advantages and disadvantages of

the algorithm. Specifically, it consistently approached the optimal

solution, and produced results that were comparable to the tracking

agents’, but without any assurance of optimality.

The average amount of time required for each trial was 163.10 seconds for all

77

Figure 5.4: DORA found a slightly better solution than tracking in a problemspace that does not apply to physical space. This solution is shown schematicallywith the origin to the left and the destination to the right.

seven epochs, however the average number of epochs required to reach the DOR

agents’ best solution was 4.41. This suggests that some time may be saved in

future iterations by concluding the trials after fewer epochs. However, some

trials required the full seven epochs to find their solutions, so using fewer epochs

will necessarily sacrifice some optimality. The sensitivity of the solution to the

number of epochs permitted is left for later experimentation, and all trials for

this project are limited to seven epochs.

Spherical Earth with High Velocity

To verify that the agents’ performance and the physical model transitioned well

from the simplistic planar approximation to the spherical model of Earth, the

same initial configuration from the previous trial was retained, but all navigation

and physical model functions were replaced with the spherical model. Table

5.4.3 summarizes the results of this experiment.

78

Table 5.3: Results of Round Earth with High Speed TrialTracking Solved 100.00 %Homing Solved 100.00 %DORA Solved 100.00 %Tracking Best 83 %Homing Best 0 %DORA Best 17 %Average DORA - Tracking 3.70× 10−2 %Average DORA - Homing −15.07 %Average Epoch 4.47Average Time (s) 120.90

Again all three classes of agent found solutions in every trial. In this experiment,

the tracking agents found the best solution in 90.5% of cases, while the DOR

agents found the best solution in the remaining 9.50%.

The DOR agents performed comparably to the tracking agents with an average

performance 3.70 × 10−2% worse than the tracking agents’, but 15.07% better

than the homing agents’.

The average time required for execution of all seven epochs was 120.90 seconds,

while the DOR agents’ required an average 4.47 epochs to reach their best

solutions.

These results are very similar to those from the planar approximation, as would

be expected. The tracking behavior still represents the best solution for nav-

igation to a point, but the DORA optimization is again able to find a better

solution for navigation to a small circle around the objective in a few cases. The

overall result is that the DOR agents are nearly equivalent to the tracking agents

in this experiment, as they were in the preceding planar experiment.

79

Spherical Earth with Low Velocity

The preceding experiments were conducted to validate the performance of the

DORA algorithm in fields with known solutions, and to compare the model’s

results with the analytical solution to the tracking problem. Direct comparisons

with the tracking agents showed that on average, the DOR agents found com-

parable solutions to the problem, however since a constant, uniform current is a

trivial case, there is hardly justification for using any optimization at all.

A more practical problem includes the variations observed in currents in a real

ocean. Here, currents vary significantly in space and time, and are not guaran-

teed to be less than the vehicle’s speed. This presents a much more challenging

environment for navigation and a much more reasonable approximation of the

environment that an AUV employing this steering behavior might face. So for

the next trial, the data described in Section 5.4.2 were used as the source for

ocean currents, and the vehicle speed was limited to 0.50 m/s.

Since the maximum local velocity in the data set was slightly larger than 2.0

m/s, there was no guarantee that the slow-moving agents would be able to

navigate to their objectives. To ensure a sufficient number of successful results

for evaluation, this experiment was run for 1200 trials rather than the 200 in

the preceding experiments. The fact that no solution was guaranteed to exist

allowed for evaluation of the DOR agents’ robustness and ability to be adapted

for use in situations when simple tracking or homing behaviors failed. The

success rates of the three classes of agents are shown in Table 5.4.

Of the 1200 trials conducted, only 524 were solved by any of the agents. DOR

agents found solutions to 43.5% of cases, which was almost twice the number of

80

Table 5.4: Success Rates for Round Earth with Low Speed TrialTracking Solved 24.92 %Homing Solved 21.25 %DORA Solved 43.50 %Average Epoch 4.38Average Time (s) 124.55

solutions found by either of the other classes of agents. This is instructive when

considering the robustness of the agents, but for comparison between them, only

the cases when at least one class of agent found a solution are considered, which

is the basis of the percentages shown in Table 5.5.

Table 5.5: Results of Solved Trials on Round Earth with Low SpeedTracking Homing DOR

Successful Trials 299 255 522Success Rate (%) 57.06 48.66 99.62Best Solution 31 0 493Best Solution (%) 5.92 0.00 94.08

For the 524 trials with successful solutions, the DOR agents had the best perfor-

mance with both the highest number of solutions found and the highest number

of best solutions found.

5.5 Modifications for Use in Obstructed

Environments

Incorporating a collision avoiding behavior for use in obstructed waters required

modification of both the agent preference equation, and the simulation environ-

ment from those used in the open water trials. First, the simulation environment

was modified to incorporate bathymetric data. Specifically, the ETOPO2[2]

81

database was used for initial testing, then was replaced with the newer and

higher resolution ETOPO1 [7] database. These data were used to eliminate

agents that collided with terrain as well as to provide agents with local approx-

imations of bottom topography.

In the open water model, agents were eliminated from the simulation any time

that they strayed into areas that did not have good model data. This prevented

wandering out of bounds, and provided a crude means preventing agents’ travel

beneath the sea floor. However, since the ocean current model data were lower

resolution than the terrain data, a more accurate means of checking for colli-

sions was needed. First, if ocean current data were unavailable, the simulation

environment assumed that the local current was the same as the last valid cur-

rent information for the agent, so that an agent could maneuver in near-shore

areas without accurate current models and still survive, assuming the agent did

not collide with terrain.

To check for collisions with terrain, the agent’s depth was compared with

an interpolation-based estimate of ocean depth provided by the bathymetric

database. If the agent’s depth was greater than the depth estimate, the agent

was eliminated from consideration.

Since the agent preference formula was updated to include bathymetric data,

the simulation environment provided each agent an estimate of the local bottom

gradient vector as well as the depth at the agent’s position. To adhere to

the principle that agents only have access to local information, presumably

information that could be obtained in the field, the gradient calculation was

based on estimates of bottom depth within a 100-meter radius of the agent’s

position.

82

Finally, to keep agents restricted to the region of interest, each agent’s global

position was compared to the extrema of latitude and longitude in the data sets,

and agents outside of these bounds were eliminated.

5.6 Pseudo Scientific Tests and Modifications

Many of the design decisions during the development of the test software were

incidental to the operation of the algorithm itself. It was important to make

reasonable decisions, but these were matters of developer preference rather than

of scientific rigor. This section discusses a number of software design develop-

ments that were based solely on experience, intuition, statistically insignificant

trials, and whim. Nothing presented here is claimed to be optimal, ideal, or well-

tested, but it does provide insight to the justification for a number of design

decisions which are not, at their cores, the focus of this research.

Figure 5.5: “Piled Higher and Deeper” by Jorge Cham www.phdcomics.com

83

5.6.1 Selecting Values of M and T

a = A

(1

1 + eMtc−MT

)(1− 1

1 + etc

)∇F

The slope and temporal horizon terms in the obstacle avoidance vector (shown

above for reference) can take on any positive value, and are not among the

parameters that are optimized during normal execution of the program. In an

attempt to measure the influence of these terms on the algorithm’s probability of

success, a series of experiments was conducted in which 20 trials were run with

every combination of slope values M = {1.0, 0.1, 0.001, 0.0001} and temporal

horizon values T = {0, 900, 1800, 2700, 3600}. The success rates of the algorithm

were monitored, and the one with the highest rate was chosen. This combination

corresponded to M = 0.001 and T = 2700.

To evaluate the robustness of the conclusions drawn from the preceding test,

another experiment was conducted in which the same values of M and T were

used in successive trials with the same number of agents as in the previous

experiment. This test showed that success rates between tests often varied by

up to 5 trials. The largest difference between successive tests in the earlier

experiment was far fewer. Therefore, it is unreasonable to conclude that the

chosen values of M and T correspond to an optimal solution even among the

limited values considered in the first trial. However, since the choice of values

for these parameters is arbitrary, and the values used here worked for several of

the trials, no further attempt to optimize M or T is considered.

Since the first stage of the algorithm is a random exploration, increasing the

number of agents increases the effective coverage of the search space, so as the

84

number of agents is increased, the consistency between trials is also increased.

However, this also increases the computation time required for a given trial.

This trade between search space coverage and computation time led to the

developments described in Section 5.6.2.

5.6.2 Exploration vs. Exploitation

For all prior trials and for the first several hundred tests of the obstructed

environment agent preference formula, the number of agents in the environment

was kept constant. Fewer agents meant a lower likelihood of success, but also

less time required for execution. More agents improved the chances of success

at the expense of the amount of time required to execute.

Using this system, exactly the same amount of resources were used for explo-

ration as for exploitation; the algorithm spent exactly as much effort optimizing

its solutions as it did finding candidate solutions.

Since it is more important to have a valid solution than a good solution, a

short trial over the first ten waypoint pairs was conducted with a strong bias

toward exploration. The algorithm initialized with 600 agents, but on the epoch

following the discovery of a valid solution, the number of agents was reduced by

a factor of 10. Thus, 600 agents searched for a path, but once it was located,

the simulated annealing only operated with 60 agents per epoch. The results of

this trial were compared with a prior trial that was run on the same computer,

and the modification was found to slightly improve the number of solutions

discovered, but to reduce their average quality.

85

This was expected, as the resources were allocated to emphasize discovery over

optimization. However, the more important difference between the two tests

was that the average time required was reduced by approximately 91%. This

suggested an important concept for moving forward: by managing the num-

ber of agents allocated to different phases of the search, the quality

of solutions can be effectively traded for savings in computation time.

This also implies that for a practical case of real-world operations, it may be-

hoove an operator to bias a search strongly toward early discovery of a viable

solution, so that the rough solution can be used, and then improved during the

course of a mission.

5.6.3 Simple Learning Does not Help

In hopes of improving the algorithm’s performance through experience, a sim-

ple means of learning from previous successes was incorporated. This learning

consisted simply of maintaining a vector, which stored the mean value of all

successful trials.

To evaluate the influence of this change, a trial was conducted in which the mean

values of all behavioral parameters were recorded by the software, and 1/5 of

the agents generated during the exploration phase were initialized in the region

of the average according to a gaussian distribution with a standard deviation

of 0.25. During this trial, there was a negligible increase in success rate, but

also an increase in the number of epochs required to find an initial solution.

This result was discouraging, and further development of a learning component

was suspended. However, this does not imply that learning from experience

86

will not improve the algorithm’s performance. The small differences seen here

were only relevant to exactly the parameters of the trial, and it is possible that

a better learning system, or even the same system with more carefully chosen

parameters could perform quite well.

Further exploration of experiential learning is left for later research, as the

emphasis here is on the characterization of the algorithm itself.

5.7 Impact of DORA’s Individual Parameters

Before comparison against the A* algorithm, the necessity and influence of two

of the preference vectors were evaluated using the same 1.5 m/s speed and test

environment as the baseline trial described above.

5.7.1 The Influence of A

a = A

(1

1 + eMtc−MT

)(1− 1

1 + etc

)∇F

Since A increases the dimension of the search space for the algorithm, an ex-

periment was conducted in which A was constrained to zero. This trial showed

that exclusion of the terrain avoidance behavior had a small negative impact on

the algorithm’s ability to find a solution. In this trial, the success rate dropped

from about 67% to about 64%. However, this is a small difference, and the gains

associated with elimination of an ocean floor database or measuring from a de-

87

ployed platform would likely warrant this sacrifice. Since all other behaviors can

be implemented using a single GPS module and a simple microcontroller, and

the terrain avoidance behavior would need either a bottom tracking SONAR

or a terrain database for its operational area, it is likely that any agent simple

enough to warrant the use of this algorithm would be too simple to warrant the

monetary, computational, and energy expenses associated with terrain avoid-

ance.

This method of neglecting the terrain avoidance behavior is used in the field

trials described in later sections, as the test craft uses only a microcontroller

with a GPS and compass to navigate.

5.7.2 The Need for C

c = C〈sin θa, cos θa〉

The ability of the course stability preference to take on a negative value can lead

to very counter-intuitive behavior. The entire reason for adding this preference

is that changing directions expends energy.

Moreover, including this behavior introduces hysteresis into the agent’s trajec-

tory. To know which direction an agent will attempt to travel, it is insufficient

to know only its position and preference values. Instead, the agent’s course

at the previous update is also important. But to know that course, the state

preceding that one is necessary, and so on back to the beginning of the agent’s

travel. So beyond being merely counter-intuitive, the inclusion of the course

88

parameter is also mathematically inconvenient.

Three experiments were conducted with two sets of 20 trials each in the Eastern

Caribbean test area. One experiment was conducted with C constrained to the

normal range of −1 to 1 inclusive. Another experiment was conducted with

C constrained to positive values 0 to 1 inclusive, and in the final experiment,

the course stability preference was eliminated by setting C = 0. Eliminating

C reduced the algorithm’s ability to solve the navigation problems from a 75%

success rate down to 55%. Constraining C to the positive half of its range had

no apparent impact on success rates, but reduced the quality of the solutions,

by increasing the required transit time by about 4%. Given the small number of

trials, it is unlikely that this 4% holds any significance whatsoever, but without

a clear advantage to constraining C, it was left with its original definition, which

encompasses a larger variety of potential behaviors.

89

Chapter 6

Platform

To evaluate the potential use of the DORA system for real-world navigation,

a small-scale autonomous surface vehicle (ASV) was constructed, and was pro-

grammed to navigate according to the agent preference formula. This chapter

discusses the design, construction, and testing of the ASV, while the details of

its field trials are reserved for Section 7.6. Figure 6.2 shows the vehicle under

way.

6.1 Hull Design

The ASV design is built on a simple, wall-sided monohull. To facilitate analysis,

the hull shape was chosen so that it is represented by two geometric curves. The

forward section is described by a parabola, which is parallel to the vessel’s longi-

tudinal axis at the its widest point, and converges to a point on the longitudinal

axis at the bow. The aft section is described by a line that terminates at the

90

outer corner of the transom, and is tangent to the parabola. Since the line can

only pass through a fixed point and be tangent to the parabola at one point,

the hull geometry is fully constrained. To simplify modeling, the functions de-

scribing the hull shape were derived in terms of the following design parameters:

Length of Forward Section, Length of Aft Section, Maximum Beam, and Width

of Transom. Equation 6.1 shows the formula that defines the hull geometry. If

drawn on a Cartesian grid, these formulas plot a hull with its widest point at

x = 0, the transom at x = −A, the bow at x = F , and the point of tangency of

the two curves at XFA. The formulas can be easily transformed to a different

position if needed, but since useful design data are obtained directly from the

formulas, those transformations are not carried out here.

hF = −B2

(x2

F 2+ 1

)(6.1)

hA =B

F 2(−XFAx− AXFA) +

BA

2(6.2)

XFA = −A+F 2√

BF 2

(BA2

F 2 −B +BA

)B

(6.3)

F Length of Forward Hull SectionA Length of Aft Hull SectionB Maximum BeamBA Beam at TransomXFA Point of TransitionhF Half Breadth from XFA to BowhA Half Breadth from stern to XFA

Using a mathematically described hull shape facilitates analysis by eliminating

approximation. By integrating the curve from the transom to the bow, the area

is exactly calculated, and the displaced volume is found by multiplying that area

91

by the vessel’s draft. The formula for the waterplane area is given by Equation

6.4.

AW = AWA + AWF (6.4)

AWA = −BXFA

F 2

(X2

FA + AXFA + A2)

+BA (XFA + A) (6.5)

AWF = B

(X3

FA

3F 2−XFA +

F

3

)(6.6)

AW Waterplane areaAWA Waterplane area from −A to XFA

AWF Waterplane area from XFA to F

Similarly, the lengths of the side walls can be calculated using the formula for

arc length and suitable integration tables such as those given by Stewart [36].

The length of a side is shown in terms of design dimensions by Equation 6.7.

This length is useful for layout of material and for estimation of weight. Since

the sides of the hull are vertical, and are thin compared to the length and depth

of the vessel, the volume of material in the walls can be estimated by multiplying

the arc length of the side by the total height of the hull and the thickness of the

side walls.

92

L = −F2

2B

[B

F

(XFA

F

√C0 −

√C1

)+ ln

(−B

F+√C1

−BXFA

F 2 +√C0

)]+

√(XFA + A)2C0 (6.7)

C0 = 1 +

(BXFA

F 2

)2

(6.8)

C1 = 1 +

(B

F

)2

(6.9)

The particulars of the hull constructed for the ASV are shown in Table 6.1,

and the hull plan is shown in Figure 6.1. These dimensions were chosen to

provide ample internal space for the ASV’s electrical and control systems, as

well as sufficient beam to support the later addition of solar panels to the deck

without compromising the vessel’s stability. The following section describes the

construction of the hull as well as testing of a simple prototype to evaluate the

buoyancy and stability of the chosen shape.

Table 6.1: ASV Hull Properties; Coefficients calculated according to formulasfrom Rawson and Tupper [29].

F 14.0 inchesA 12.0 inchesB 8.0 inchesBA 4.0 inchesLOA 26.0 inchesAW 151.4 inches2

CWP 0.728CB 0.728CP 0.732CM 1.0CV P 1.0

93

Figure 6.1: ASV Hull Plan with scales shown in inches. Image generated usingmatplotlib[20]

Figure 6.2: ASV operating autonomously on the Indian River Lagoon duringfield trials.

94

6.2 Hull Construction

To validate the calculations described in the preceding section as well as to

evaluate vessel stability, a one-to-one scale model of the hull was constructed

using foam board with a laminate of Duck Tape to seal against water. The test

hull was massed and ballasted to match the fully loaded configuration of the

vehicle. The hull was then evaluated in FIT’s Southgate Pool, and it floated

at the predicted draft. It was also submerged to 18 inches without damage or

flooding. Stability was examined by manually rolling the hull and releasing it to

verify its ability to right itself, which it did at heel angles up to approximately 90

degrees, however it also exhibited strong stability after inversion suggesting that

if the vessel capsizes it will be unlikely to recover in the present configuration. If

necessary, this tendency can be mitigated by adding a weighted spar below the

hull to lower the vessel’s center of gravity, and bias it further toward recovering

to an upright attitude. The successful tests of this prototype suggest that for

simple field testing, the Duck Tape over foam board construction method is a

viable and inexpensive alternative to more elaborate means.

Following construction of the initial model, a more resilient hull of the same

shape was built from 0.085-inch thick polycarbonate (PC). The components

were all cut from a single sheet measuring 28 x 30 inches, and were structurally

joined with hot melt adhesive (HMA). The seems were sealed with marine sili-

cone adhesive/sealant. To facilitate maintenance, an access hatch was cut in the

upper deck, and a cover plate was attached with a pattern of six 6-32 x 1/2-inch

machine screws, which were permanently bonded to the upper deck plate with

cyanoacrylate (CA) adhesive. The cover plate was sealed with silicone grease

to prevent water ingress and enable easy removal of the plate for access to the

95

internal electronics.

Vertical structural supports were added at the center of the transom and on both

sides of the hull at 1/4, 1/2, and 3/4 of the vehicle’s length, which improved

the rigidity of the deck. This was largely a response to testing of the earlier

prototype hull, which used a single structural support at midship, and showed

minor buckling of the fore and aft decks after maintenance.

For steering, the ASV used a simple PC rudder, which was mounted to the

transom via a cabinet hinge, and fastened with 6-32 x 3/8-inch machine screws.

During pool and early field testing, the rudder’s 0.75-inch chord provided insuf-

ficient force to turn the ASV against ambient wind and currents, so an extension

was improvised, which increased the rudder’s chord to 1.25 inches and improved

its steering ability. A permanent improvement to the rudder will incorporate a

1.5-inch minimum chord below the waterline.

6.3 Electronics

The ASV uses a microcontroller and a number of peripheral systems to achieve

its autonomy. Each of these systems and modules will be discussed in turn,

but a wiring diagram of the entire electrical system appears in Figure 6.3, while

a schematic of the control board is shown in Figure 6.4. The description pre-

sented here represents a single snapshot in the ASV’s development, and many

improvements will be incorporated as the project continues. This section de-

scribes the vehicle in the state that it was during its field trials in the Indian

River Lagoon.

96

The ASV is controlled by a microcontroller compatible with the Arduino Mega

2560 Rev. 3 specifications and the Arduino IDE. This microcontroller is used

because it has four onboard serial ports, which enable simple interfacing with

TTL serial instruments and peripherals. In addition to simple interfacing, the

Arduino board and programming environment are commonly used in the hobby

electronics community, so this gives the ASV a basis of familiarity among other

potential users of the platform. Finally, the Arduino board includes a simple

USB programming and serial interface, which reduces the number of external

components and equipment needed to modify the ASV’s behavior. In short

the Arduino is the control board of choice because of its common use and its

simplicity, rather than for any other design consideration.

A remote communication capability is provided by a Roving Networks WiFly

module, which is connected to the Arduino’s first serial port. The WiFly mod-

ule enables the ASV to send and receive serial communication over an 802.11

wireless network connection[26], so that the ASV can be remotely monitored or

controlled by any computer with a normal wireless network interface. Since the

Arduino uses its first serial port for reprogramming, the ASV cannot be repro-

grammed when the WiFly module is connected, so while all of its supporting

circuitry and software is included in the ASV, the WiFLy module was removed

during field testing. To address this problem, the most recent revision of the

control board includes a power cutoff switch, which shuts down the commu-

nication board, and uses relays to disconnect its communications lines to the

microcontroller.

Since this vehicle is intended to provide a useful development platform for other

projects, it should be noted that the WiFly module communicates over a net-

97

work socket, and it is unclear whether it is capable of maintaining a connection

to more than one other similar module. This would be a significant limitation

for coordinated or swarm-oriented behaviors, and if necessary the WiFly can be

replaced with a Digi XBee wireless module. The XBee is pin-compatible with

the WiFly, and it supports a variety of network architectures [5], however it uses

a different wireless protocol that is not compatible with common wifi network

adapters. In either case, the range of the wireless connection will be short due

to the antenna’s proximity to the water’s surface.

For orientation the ASV uses a Devantech CMPS09 tilt-compensated compass,

which responds to TTL serial queries with the heading measured to 0.1 degrees

[1]. The compass is connected to the Arduino’s second serial port. When

queried, the compass provides heading pitch and roll, however only the heading

is currently used by the ASV’s control system.

During operation, the ASV records mission data to an onboard micro SD card,

which interfaces through a Sparkfun OpenLog serial adapter connected to the

Arduino’s third serial port. The current version of the ASV’s control software

does not allow for remote download of recorded data, so the deck panel must

be removed to physically access the card and retrieve recorded data.

The vessel fixes its position with a 50-channel GP-635T GPS module produced

by ADH Technology. The GPS provides updates of position at an interval of

1 Hz to an accuracy of 2.5 meters [3]. This GPS receiver was chosen for its

low cost and small size of 35 mm x 8 mm [3], which allows it to be mounted

directly to unused space on the control board. Since the DORA algorithm

requires heading updates at a rate that is slower than the time required for the

vehicle to maneuver, the relatively slow rate of 1 Hz does not cause a significant

98

problem for this application.

Control of the ASV’s rudder is provided by a Traxxas 2080 waterproof servo,

which is recessed into a cutout on the ASV’s after deck. Since the servo is

waterproof, its joints to the hull are sealed with silicone grease, but no further

action is taken to protect it from spray or overwash. It is powered by the ASV’s

regulated 5-Volt bus, and is controlled with a pulse-width modulated (PWM)

signal from the Arduino.

The ASV’s thrust is provided by a DC motor rated for 9-18 Volts, which is pot-

ted in wax and sealed in a film canister. This method is used by the SeaPerch

educational outreach ROV, and is described in detail in the SeaPerch Construc-

tion Manual [6]. The thruster is mounted external to the hull, which reduces

the complexity and the required tolerance of fittings by eliminating the need

to pass a rotating shaft through the hull. This also allows for a very modular

design. Since the thruster is attached to the hull with cable ties and connected

to the ASV’s controlling electronics by disconnectable waterproof fittings, it

can easily be replaced by different thruster configurations, or the controlling

electronics can be transplanted to a different hull. The motor is connected to

the ASV’s battery power supply, but its speed is controlled by a an N-type

MOSFET transistor whose gate is driven by a PWM signal from the Arduino.

To prevent the motor’s speed from changing with decreasing battery voltage,

the battery voltage is measured by the Arduino, and the PWM signal is scaled

to maintain a constant signal.

During operation, power is supplied by a lithium polymer battery, which is

rated for 6 Amp-Hours of capacity. Most of the ASV’s onboard systems receive

power from a 5-Volt Murata 78-S/R-5/2-C switching regulator, which is rated

99

for 2 Amps at up to 93% efficiency according to its datasheet. This switching

regulator replaced an LM78S05 linear regulator in order to improve the efficiency

of the system. The systems that are not powered by the 5-Volt regulator are the

wireless serial module, which is powered by an independent 3.3-Volt regulator,

and the Arduino, which has a built-in regulator and is powered directly from

the vehicle’s power supply. Also, the GPS module has an independent backup

power supply, which is provided by two AA batteries.

To allow for debugging and reprogramming the ASV without draining the bat-

tery, and to allow for recharging its battery without opening the deck access

panel, two power supply cables are passed through the hull. One provides shore

power directly the ASV’s systems, and the other provides power to the ASV’s

built-in battery charge controller. A center-off three-position switch mounted to

the vehicle’s deck allows the operator to shut down the ASV or select whether

to run from shore or battery power. To conserve power, the wireless commu-

nication board can be independently toggled using another switch mounted to

the deck. A final switch on the deck allows the user to reset the microcontroller

without toggling power to the entire ASV, which is helpful during operation

because it does not depower any of the peripheral systems. The GPS in par-

ticular is sensitive to cycling of its power, and often requires significant time to

reacquire a position fix after losing power.

All cables that penetrate the hull pass through waterproof cable fittings, and

all switches, fasteners, and temporary joints are potted with silicone grease to

prevent water from being admitted to the hull.

100

Figure 6.3: ASV system wiring diagram.

101

6.4 Control Software

The ASV’s software implements two layers of control: a reactive layer that

implements the DOR agent preference system, and a hardware control layer

that actuates the rudder to maintain a set heading. The reactive layer also

manages the mission by keeping track of the vehicle’s waypoints and its mission

completion criteria. Implementing the agent preference behavior and managing

the mission require data from the GPS, which can take more than a second to

acquire. Also, since DORA neglects the effects of vehicle dynamics, it is built

on the assumption that its update cycle is longer than the time required for the

vehicle to turn to a set heading. For this reason, the vehicle’s set heading is

updated at a much slower rate than its heading control system.

To maintain a heading that is set by the DOR preference calculation, the ASV

software uses a simple proportional feedback controller for the rudder servo,

where the rudder’s angle of deflection is proportional to the difference in degrees

between the ASV’s preferred heading and its measured heading (Kp = −3.75).

During each pass through the hardware control loop the ASV polls the compass

for heading, sets the deflection of the rudder, updates a mission timer and an

update timer, and checks the latter against the heading update interval.

If the timer has exceeded the update interval, the software acquires a new GPS

fix. It then estimates the local current by checking its GPS position against

its dead reckoning position, calculates a new preferred heading, logs relevant

mission parameters, and resets the update timer. This process repeats until the

ASV measures its range to its next waypoint as less than a preset threshold. At

this point, the update function carries out some completion action. The details

102

of the mission behavior will be described with the field test in Section 7.6.

6.5 Estimated Speed

Since the agent’s speed is an important parameter in the DORA navigation

system, the ASV’s still-water operating speed was measured in pool tests. The

size of a swimming pool was estimated from Google Earth images at 12 meters,

and the ASV was set to traverse the length of the pool using heading hold

autopilot, and the time required for the transit was recorded. To compensate for

directional influences, the ASV was then timed on its return, and the average of

the two speeds was recorded. This method is admittedly rough, but gave results

that were at least accurate enough for the algorithm to be successful. Certainly

much more careful measurement and characterization would be needed before

any current measurements by the ASV could be considered useful data, but for

simplistic estimates this method was sufficient.

Table 6.2: ASV performance data recorded from pool and tank tests.PWM (8-bit) Speed (m/s) Current (A) Duration (h) Range (km)

0 0.00 0.56 8.93 0.0064 0.32 0.84 5.95 6.8685 0.44 0.98 5.10 8.08

100 0.48 1.04 4.81 8.31128 0.56 1.26 3.97 8.00170 0.64 1.57 3.18 7.34191 0.66 1.76 2.84 6.75255 0.79 2.24 2.23 6.35

The ASV’s motor controller is open-loop, so its actual shaft speed is unknown.

For this reason the test data are recorded in terms of the PWM signal sent to

the motor. The results of the trials are shown in Table 6.2. Two useful results

103

of these experiments are shown in Figures 6.5 and 6.6. The first shows the

power required to achieve a given speed, and the second shows an estimate of

the range that is attainable at that speed. It is apparent from the figure that

the greatest range–at least within the values tested–corresponds to a speed of

0.48 m/s. Slower speeds require less power, but also cover less distance per

time, whereas higher speeds cover more distance but at a higher energy cost.

All of the trials conducted in the Indian River Lagoon were carried out at 50%

power for a speed setting of 0.56 m/s, which is a slightly higher speed than

for the maximum range, but also allows the ASV to maneuver against stronger

currents.

This chapter has provided an overview of the design, construction, and program-

ming of the ASV that is used for the field trial of the DORA algorithm, and

thus concludes all of the background information necessary before discussing

actual experiments. The next chapter describes the different cases examined to

evaluate DORA’s performance both in realistic simulations, and in a small-scale

real-world deployment.

104

Figure 6.4: ASV Control board schematic.

Figure 6.5: ASV Power Vs. Speed.

105

Figure 6.6: ASV Range Vs. Speed.

106

Chapter 7

Experiment

This chapter describes the experiments conducted using simulations of the

agents’ behaviors and the navigation system’s optimization as well as the field

test carried out using the ASV. While many experiments were conducted dur-

ing the course of the software’s development–some of which were documented

in Chapter 5–the experiments of this chapter were designed to provide the most

consistent comparison of the aspects under consideration and thus the most

useful insight regarding DORA’s benefits and liabilities as well as the circum-

stances under which it might or might not be a good candidate. Throughout,

four different methods are compared: DORA, A*-based navigation, Tracking,

and Homing.

107

7.1 Performance Evaluation

For each of the experiments, the following properties were examined: Success

Rate, Mission Time, Effective Speed, Computation Time, Successful Computa-

tion Time.

Success Rate is the fraction of trials that are solved by the specified method in

a given experiment. Since in some cases no solution is certain to exist, and since

in all cases the DORA algorithm is stochastic, this fraction gives a measure of

an algorithm’s robustness.

Mission Time is the average number of hours required to execute the success-

fully planned missions. This is a rough estimate of the quality of a solution.

Since it is important to not only find solutions that are possible, but to find

solutions that are as efficient as possible this measure provides some insight.

However, since path lengths are not standardized, a low mission time may only

reflect that an algorithm is better at solving easier problems. The effective

speed is introduced to address this concern.

Effective Speed is the average great circle distance between start and end

points divided by the mission time. This eliminates the potential for an algo-

rithm to prefer shorter problems and thus bias its results toward an appearance

of higher quality.

Computation Time is the average amount of time required for each algorithm

to complete its search for a solution, whether successful or not. For robotics and

applications that require real-time or short-term use of a mission plan, compu-

tation time is critically important. A plan is of no use to a robot if the data

108

upon which the plan is built have expired before the plan is developed. A related

concern is that computers aboard robotic vehicles in general, and AUV’s in par-

ticular, often have very limited processing power, so computational efficiency is

critical for any algorithm that will be deployed aboard a vehicle.

Successful Computation Time is the average amount of time required for

each algorithm to complete a successful search. This gives some measure of

comparison between the time taken to look for a solution, and the time re-

quired to actually find a solution. For instance, since it is complete, A* needs

to exhaustively search a graph before declaring that solution does not exist.

However, since DORA is based on a stochastic model, it searches for some spec-

ified amount of time or for the life of a population of agents, and has then either

succeeded or failed, which allows some control of the amount of time allocated

to searching for a solution.

7.2 Choice of Parameters

The tracking and homing behaviors are purely deterministic, and there is no

room for modification, so their cases are fairly trivial. However, both the A*

search algorithm and DORA have a great many factors that influence their

likelihood of success, the quality of their solutions, and the computational time

they require. Since an exhaustive study of all possible combinations of param-

eters is impossible, a specific subset is chosen based on experience, and these

parameters are used consistently throughout the different trials. This attempts

to capture the performance of not DORA vs. A* in general, but specifically, of

this implementation of DORA vs. this implementation of A*. So it is important

109

to remember that conclusions drawn from these trials cannot establish absolute

boundaries on either algorithm, but instead only aspire to provide some useful

insights about their suitability for problems that are fundamentally similar to

the ones discussed here.

7.2.1 A*

Since DORA is not complete or optimal, it was compared against the 4-D A*

planner described in Section 5.2. For all of the tests with A*, it is important

to remember that A* is guaranteed to find the best possible solution in the

minimum possible time for its search space. Since it is a graph search method,

its ability to find a path through a continuous, real-world environment as well

as the quality of that path and the time required to find it, are all very subject

to the environmental model, and the algorithm parameters that are used.

Increasing A*’s resolution either by increasing its branching factor or by re-

ducing the distance traveled during each branch improves the quality of the

solution that is produced, but at the expense of computation time. Therefore,

to be practical for use in the field, resolution must be reduced to a level that

allows for solution in a reasonably short amount of time, but it must be kept

high enough to ensure sufficient quality.

Because of these considerations, it would not be unreasonable to find cases in

which the A* search required more computation time or produced a higher cost

path than an alternative algorithm that was searching in a different problem

space. Choice of parameters for the comparison with A* are somewhat ar-

bitrary, and are based entirely on experience with the software that is being

110

tested. Caution must be used here, as the parameters could be artificially cho-

sen to prevent A*’s finding a solution, or finding one in a reasonable amount of

time.

For these tests, the parameters used are a branching factor of 3 with one branch

directly ahead of the agent, and one branch 45◦ to port and the other 45◦ to

starboard. To attempt to capture the influence of resolution on quality and

performance, two resolutions are tested for each trial, one at 10 kilometers of

length, and the other at 20 kilometers of length based on the planar earth

approximation at the point where the node branches. To ensure consistency,

the cost function uses the same simple kinematic simulation that is used in the

DORA software, and with the same time step.

7.2.2 DORA

The DORA system also depends on many parameters for operation. Two of

the most important are the size of the population, and the number of epochs

allowed for searching for a solution. Since it is built on a stochastic, rather

than a deterministic process, increasing the population or the duration of the

test increases the chances that a solution will be found if one exists, because

the random distribution of agents covers a larger portion of the search space.

However like A*, a practical limit is reached when the computation required to

process the search is greater than what is permissible for a particular mission.

Also, since DORA proceeds in two stages, one searching for an initial solution,

and the other attempting to improve on that solution, the number of agents

and epochs allocated to each phase is critically important. For the initial tests

111

of DORA, 500 agents are allocated to the initial search and are given 5 epochs

to find a solution. 50 agents are allocated to the improvement of the original

solution, and are allowed 10 epochs for their search. Next, to evaluate the

importance of the terrain avoidance term, two trials are conducted with its

preference coefficient constrained to zero, and a limit of fifty agents.

Typically, only varying one parameter at a time is better for identifying its

influence on a particular problem, however removal of one of the preference

terms simultaneously reduces the dimension of the search space.

7.2.3 All Agents

All agents are allocated a speed at the beginning of each trial, and maintain that

speed in their own frames of reference throughout the experiment. Three speeds

are chosen at 0.4 m/s, 1.5 m/s, and 3.0 m/s. Intuitively choosing speeds that

are related to the maximum speed of ocean current in a velocity field seems

like the most reasonable approach for evaluation of the algorithms, however

that is not the scheme chosen here. Rather, these speeds are chosen to more

accurately represent a practical application for this algorithm. There is little

physical value in exploring the software’s ability to solve a problem that only

exists for a hypothetical system.

The slowest speed is chosen to be in the range of operation for a common AUV

glider [12]. The next speed at 1.5 m/s, corresponds to the speed of a typical

thruster-driven AUV, or of a ship surveying with a tow-fish. Finally, 3.0 m/s

is more closely related to the velocity field, as it is faster than the maximum

speed in the Gulf region of interest. In general, an unobstructed environment

112

with a speed slower than the agents’ is the only condition where a solution is

guaranteed to exist for all four behaviors. Therefore, to evaluate the software’s

ability to find known solutions, a speed higher than the local current velocity

is necessary. While artificially based on the need to explore the algorithm’s

relationship to the current field, 3.0 m/s corresponds to roughly six knots, so it

is well within a range that is typical of ship operations.

7.3 Program Environment Specifications

All trials were conducted using the Python 2.7 programming language employ-

ing the included modules time, random, and sys. The third-party modules

numpy and scipy were used for most mathematical and data access operations.

The experiments were executed on a laptop computer with an Intel i7-2630QM

2.0GHz quad-core processor and 8 GB of RAM running the 64-bit Ubuntu 12.04

initially, and later upgraded to Ubuntu 12.10 operating system.

7.4 Gulf of Mexico ROI

The Gulf of Mexico test area has no obstructions but relatively high current

speeds, so it is a good field for evaluating the performance of the different

navigation systems under the influence of strong currents alone. It presents a

problem, which is in some ways simpler than the Caribbean test area, which

has intervening obstacles, but the relatively stronger currents in this region still

produce some cases in which there is no solution for slower agent speeds.

113

7.4.1 Trial at 3.0 m/s

This test evaluated the different navigation methods in an environment where a

solution was certain to exist for all of them. Since the agent velocity was faster

than the maximum local current, there are no cases where a path connecting

the starting point to the ending point could not be found. The results of the

experiment are shown in Table 7.1.

Table 7.1: Results of Gulf Trials at 3.0 m/sDORA DORA 0A-5 DORA 0A-15 A*(10km) A*(20km) Track Home

Success Rate 97% 94% 95% 100% 100% 100% 99%Mis. Time (hrs) 14.98 15.08 15.00 15.00 15.05 15.09 15.12Eff. Spd (m/s) 2.99 2.97 2.99 2.97 2.96 2.97 2.96Comp. Time (s) 534.8 246.3 399.9 553.8 30.6 0.8 0.7Suc. Cmp. Time (s) 519.7 254.3 401.4 553.8 30.6 0.8 0.7

This test provides a good baseline for discussion of the computational require-

ments and success rates of the different methods. Since there are no obstructions

and the local currents are slower than the agent speeds, the trivial solutions of

homing and tracking are both successful. Both provide good results and re-

quire less than one second of computation time. Similarly, the A* approaches

produce good results and a 100% success rate, but since they are deliberative

processes, they require substantial computation time. Finally, all three of the

DORA solutions produce results of similar quality and with high success rates,

but since they are stochastic processes, they lose the benefit of 100% success.

For this case, the overall best solution is tracking since it has 100% success,

requires less than one second of computation time, and achieves the highest

quality of solution.

114

Table 7.2: Results of Gulf Trials at 1.5 m/sDORA DORA 0A-5 DORA 0A-15 A*(10km) A*(20km) Track Home

Success Rate 92% 89% 90% 100 % 100% 90% 86%Mis. Time (hrs) 31.12 30.87 31.04 32.08 32.67 31.81 31.73Eff. Spd (m/s) 1.50 1.50 1.50 1.47 1.46 1.47 1.45Comp. Time (s) 588.1 300.2 484.6 2757.7 105.1 0.9 0.8Suc. Mis. Time (s) 535.32 318.2 483.5 2757.7 105.1 0.8 0.7

7.4.2 Trial at 1.5 m/s

In this experiment, the agents are running slower than the maximum current,

so no solution is certain to exist, but the high rate of success demonstrates that

this problem is still not particularly difficult for any of the algorithms. However,

there is a drop in DORA’s success rate, and the computation time required for

A* increases dramatically in both cases, which shows that the problem is no

longer trivial. The average solution found by DORA is minimally better than

the average solution found by either A* algorithm. However, the success rates

of DORA are comparable to the success rates of tracking and homing, which

concluded in less than one second of computation time.

7.4.3 Trial at 0.4 m/s

This experiment has all of the agents operating much slower than the maximum

local current, and it represents the only trial for which DORA showed a distinct

advantage over A*. For these problems, the 10-kilometer A* test was aborted

after the first six trials, which required in excess of eight hours to complete.

The time required for each search was considered too great to be of practical

use, and it was dropped from consideration. It is possible that the higher

resolution would have been more successful and found higher quality solutions,

115

but at the expense of prohibitively long computation times. This underscores

the importance of careful parameter choice, to which A* is more sensitive.

Since the time required for A* to find a solution is related to the number of

paths that it must explore during its search, changing properties of the flow

field can dramatically impact the time required to solve the same search prob-

lem. Whereas DORA’s computational requirements are most dependent on the

number of agents and the agents are constrained to a finite lifespan, so chang-

ing the flow field does not have as significant of an impact on its computation

requirements. This independence comes at the price of completeness. When A*

fails to find a solution, it is because no solution exists for its current parameters,

and this is known conclusively. However, when DORA fails to find a solution, it

could be a simple matter of insufficient exploration, and nothing can be inferred

about the existence of a solution that is not found.

As mentioned above, this trial is remarkable for DORA’s advantage over A*.

Again this does not imply, even in this flow regime, that a stochastic algorithm

is better suited than a graph-based one, but that this version of A* with it’s

model of the environment loses its advantage in this flow field. It is quite con-

ceivable that the same A* algorithm with a different resolution, or with different

parameters would exceed the performance of DORA in this environment, but

in the interest of consistency for comparison, the parameters are maintained for

all trials.

Another interesting observation of these trials is that the average effective speed

of all of the successful solutions is higher than the agents’ own speed, which

suggests that, on average, taking advantage of favorable currents was important

to the solution of these trials. With higher vehicle speeds, simply overcoming a

116

Table 7.3: Results of Gulf Trials at 0.4 m/sDORA DORA 0A-5 DORA 0A-15 A*(10km) A*(20km) Track Home

Success Rate 31% 26% 29% – 23% 14% 12%Mis. Time (hrs) 91.77 93.89 87.94 – 106.28 94.10 102.39Eff. Spd. (m/s) 0.59 0.57 0.62 – 0.46 0.59 0.58Comp. Time (s) 730.5 155.6 293.2 – 291.4 0.8 0.7Suc. Cmp. Time (s) 436.1 289.7 329.1 – 255.7 0.6 0.6

current is often sufficient, and this is reflected by the earlier Sections’ effective

speeds that were at most equal to the agents’ own speeds. These are only average

results, and all of these trials showed standard deviations that represented a

large fraction of the agents’ speed, so there were many exceptions, but this is

an example of an intuitive concept that is well supported by the results.

7.5 Caribbean ROI

The Caribbean trials represent a field where the velocities are lower, but since

islands obstruct many of the paths, the tracking and homing solutions are not

valid for all cases with high agent speeds.

7.5.1 Trial at 3.0 m/s

The tests at both 3.0 m/s and at 1.5 m/s are faster than the local velocities

in these fields, so a path should always exist, and this provides some measure

of the algorithms’ abilities to find paths when one is certain to exist, but when

intervening islands can obstruct the path. Therefore, this is most useful in the

context of measuring collision avoidance capabilities.

The A* algorithm’s synoptic view of the environment shows a great advantage

117

in the context of collision avoidance, where it is able to maintain a 100% success

rate for both the 3.0 m/s and 1.5 m/s experiments. The quality of the solutions

here is very slightly lower, which is almost certainly due to the inclusion of large

detours to avoid islands. Since A* shows a much higher success rate than the

others, it naturally includes many routes that were not found by any of the other

algorithms. Since the trivial solutions work for clear, straight-line paths, their

few successes would only include cases where no detour was necessary, and thus

would have shorter path lengths. Similarly, the DORA solutions have very high

success rates for straight-line paths, and lower for other cases, so their solutions

are likely to be weighted toward simplicity.

This is also a convenient time to discuss the tracking and homing behaviors’

surprisingly low success rates. Since they typically work well for straight-line

paths, at first it seems that the two should have similar numbers of success

that should align well with the probability of a collision free path between is-

lands. However, since the homing agents make no correction for local currents,

they have the possibility of drifting into islands while attempting to travel to-

ward their objectives, whereas the tracking agents maintain straight-line (or

more accurately great circle) routes directly from their starting points to their

objectives.

Another interesting result from this trial is that the full DORA solution has

almost twice the success rate of the versions that ignore the collision avoidance

term. This is probably because at this speed, the local currents can be dom-

inated by the agents’ own speed, so the ocean current preference is much less

important.

118

Table 7.4: Results of Caribbean Trials at 3.0 m/sDORA DORA 0A-5 DORA 0Z-15 A*(10km) A* (20km) Track Home

Success Rate 62% 37% 40% 100% 100% 22% 13%Mis. Time (hrs) 15.783 14.59 14.48 15.73 15.89 14.70 14.41Eff. Spd. (m/s) 2.73 2.90 2.93 2.67 2.66 2.89 2.96Comp. Time (s) 762.5 177.2 369.3 168.8 12.6 0.4 0.3Success Time (s) 479.7 276.9 371.7 168.8 12.6 0.7 0.7

Table 7.5: Results of Caribbean Trials at 1.5 m/sDORA DORA 0A-5 DORA 0A-15 A*(10km) A*(20km) Track Home

Success Rate 64% 42% 50% 100% 100% 22% 14%Mis. Time (hrs) 31.55 30.73 31.11 31.27 32.53 31.56 31.80Eff. Spd. (m/s) 1.39 1.44 1.42 1.33 1.38 1.42 1.40

Comp. Time (s) 858.6 204.0 371.9 455.7 23.4 0.4 0.3Suc. Cmp. Time (s) 661.6 308.8 388.7 455.7 23.4 0.8 0.7

7.5.2 Trial at 1.50 m/s

Many of the observations described in Section 7.5.1 still apply in this section

because the agents still have a travel speed greater than the maximum local

currents. In fact, in the original formulation of this experiment, no 3.0 m/s trial

was included because of its redundancy, however it was added later to keep the

trial speeds equal for both regions of interest.

Tracking, homing, and both A* algorithms performed almost identically for this

trial as for the last–with the exception of greater computation time required for

A*. However the DORA algorithms are more successful in this case than before.

At first, this seems counter intuitive because this is a more difficult planning

problem. However, recall that this is closer to the regime for which DORA was

designed–one with agent velocities that are lower than or close to local current

velocities. Since the ocean current term diminishes with decreased local velocity,

it is effectively eliminated from consideration for very low current speeds. As its

weight is increased here, it expands the range of solutions and thus the flexibility

of the algorithm.

119

Table 7.6: Results of Caribbean Trials at 0.4 m/sDORA DORA 0A-5 DORA 0A-15 A*(10km) A*(20km) Track Home

Success Rate 36% 35% 34% 68% 54% 9% 9%Mis. Time (hrs) 111.46 110.41 110.64 100.81 93.33 65.10 114.901Eff. Spd. (m/s) 0.44 0.44 0.44 0.47 0.49 0.67 0.40Comp. Time (s) 914.6 171.7 302.0 1516.3 46.1 0.5 0.3Suc. Cmp. Time (s) 585.6 284.6 326.7 1626.1 39.6 0.4 0.7

7.5.3 Trial at 0.40 m/s

This final experiment presents a very difficult environment to all of the planning

methods. The speed is lower than the local current, and the paths are often

obstructed by islands. A* still has the highest success rate over all. It is

very interesting to note that the performance of all three DORA algorithms

is almost identical at this speed. The performance is not improved; in fact

it is dramatically reduced, but it shows results that are very similar to those

from the slow speed trials in the Gulf ROI. Here it seems that the faster ocean

currents make the current-following preference dominant to the point that the

obstacle avoidance term does not meaningfully contribute to the likelihood of

success.

In addition to the general results described above, this trial gave rise to an

interesting emergent behavior that supports the need for the full range of values

of the course preference behavior as described in Section 5.7.2. In about thirty

percent of the successful trials from this experiment, a negative value of C was

chosen, which caused the agent to dramatically change its heading at every

timestep. The entire trajectory from Trial 1 is shown as the Northern line

around the island in Figure 7.1, and a closer view of a segment of this trajectory

is shown in the right frame. Since this is a counter-intuitive solution, it was

investigated further.

120

Examination of the ocean currents along the trajectory show that the currents

are generally from East to West, which should assist the agent along its course,

so the behavior was not an attempt to overcome an adverse current. Manually

constraining the value of C to small positive values or to zero consistently led

to the agent’s collision with the intervening island, so the choice of a negative

value of C was not simply a settlement into a successful, but poor solution.

Rather, the negative value of C–assuming the same combination of A,B, and

D–was critical to the success of the trial. This zigzagging pattern effectively

reduced the agent’s speed relative to the water, so that it drifted along the local

current, which naturally bypassed the island.

This result is intriguing because it demonstrates two useful properties. First, it

shows that DORA is able to find counter-intuitive solutions to the problem and

explains the observed improvement in performance when C is not constrained

to zero or positive values. Second, it suggests that rather than fix the agents’

speed as a constant in the test, a future version of this algorithm would benefit

from being able to adjust an agent’s speed directly, since this oscillation is

an effective,albeit indirect, means of throttling the agent. Indeed, the original

reason for disallowing negative values of C was an attempt to improve agent

speed.

7.6 Field Trials

The feasibility of implementing the agents’ steering behavior as a navigation

system was tested by deploying the ASV on the Eau Gallie River and the Indian

River Lagoon, and tasking it with simple point to point missions. Upon reaching

121

Figure 7.1: Left: Paths generated from Trial 0 in the Gulf of Mexico ROI. TheNorthern route was produced by DORA, the Southern route by the 20-km A*.Right: A closer view of DORA’s route shows the zigzagging trajectory thatresults from a negative value of C. Images from Google Earth

its destination, the vehicle was to return to the position of its initial GPS fix.

Since the current and wind fields were unknown, the NetLogo software prototype

was used to optimize preferences for agents acting in a constant, uniform field

with the current directly perpendicular to the agents’ path, and at 75% of the

agents’ speed. The preferences found from this optimization are shown below,

and were programmed into the ASV’s control software.

B = −0.422

C = 0.097

D = 0.491

While running the missions, a new GPS fix was acquired and a new preferred

heading was set every thirty seconds. During these updates, a number of pa-

rameters were recorded to the vessel’s onboard memory card for post-mission

122

analysis (PMA). One set of parameters that the ASV recorded was its GPS

positions, and the path that they define is shown in Figure 7.2, which is a

screenshot from Google Earth software.

The missions that the ASV executed were generated by the ASV during its

initial startup in the field. The ASV was programmed to acquire a GPS fix and

compass heading upon activation. It set this initial position as its home location,

and projected a waypoint 250 meters head of it. It then attempted to navigate

to this waypoint using the DORA preference formula with the parameters shown

above. Upon arrival within 15 meters of its target waypoint, it was programmed

to reset its objective back to its initial position. The mission was to be completed

when the ASV had returned to within 15 meters of its original position, and shut

down its drive motor. Since DORA makes no claim to symmetry, each mission

constituted two separate trials of the DORA preference-based navigation.

Early tests failed due to a mathematical error in the navigation software, where

the heading is calculated using arctan of the preference vector components.

In code, the X and Y components had been accidentally transposed, which

obviously returned an erroneous heading. Once this problem was corrected, the

ASV successfully completed a mission in the Eau Gallie River.

7.6.1 Impromptu Experiment

While the transposition error was destructive for the field tests, it inspired a

simple experiment with the deliberative software. Since the agent chose its

123

Figure 7.2: Path recorded from the ASV’s mission log. The Northern segmentrepresents the outbound leg and the Southern segment the return. Plotted usingGoogle Earth software.

124

heading based on the erroneous calculation, clearly it wouldn’t be able to suc-

cessfully navigate. However, the chosen heading was still a function of the local

current, so in principle the algorithm might still be able to find a solution, given

an accurate model of the agent’s behavior.

To test the potential for versatility, the NetLogo optimization software was

modified so that all of the agents would incorrectly calculate their headings

based on the transposed arguments to the arctangent function. However, the

physical model was unchanged, so that the incorrect agents were operating in a

correct model of the environment.

In this experiment, the software still converged to a solution that allowed the

agent to effectively navigate to its objective. This has potential implications

for DORA’s fault tolerance and versatility, since it appears that agents act-

ing on profoundly incorrect information can still be tuned by the deliberative

optimization system to solve the problem. This is not immediately useful, as

the deliberative system must first be able to model the problem that the agent

is experiencing before it can correct for it, and that implies a self awareness

that is well beyond the ambitions of this project. Further, fault management in

instrumentation and calculation would typically be addressed by some vehicle

or software system other than the path planning software, but the implication

that the path planner itself could be used to address some system malfunctions

is interesting in its own right.

This simple, single-run, virtual experiment is not included here as rigorous

evidence to support any claim to versatility, but merely as a sidenote that

some potential may exist to expand beyond the range of tasks that are more

traditionally within the purview of path planning software.

125

7.6.2 Design Improvements

After the first successful mission in the Eau Gallie River, the ASV was upgraded

with a higher voltage battery, and the motor power supply was rerouted so that

it drew power directly from the battery. These design modifications brought

the ASV up to the specifications described in Section 6.5.

Following these improvements, the ASV was deployed for a series of five missions

of two trials each in the Indian River Lagoon, just North of the Eau Gallie

Causeway. Using the same mission profile as in the initial field test, the ASV

successfully completed all trials by navigating to an objective 250-meters from

its starting point and then returning. Figure 7.3 shows a screenshot from Google

Earth, which displays the ASV’s mission log from one of the missions.

Since all twelve field trials of the DORA algorithm were successfully completed

by the ASV, it is apparent that implementing the steering behavior aboard a

physical platform is feasible. Further, since the NetLogo software was used to

optimize the parameters with no actual data regarding the physical velocity

fields that the ASV encountered, some measure of versatility can be implied. It

is unreasonable to assume that in all twelve trials, the velocity fields were similar

to the constant, steady, perpendicular field assumed during the optimization, so

it is a straight-forward conclusion that under some circumstances the algorithm

can tolerate environments that vary from those that were assumed, but there

is no reasonable way to quantify or predict that tolerance or variation without

much further investigation.

126

Figure 7.3: Path recorded from the ASV’s mission log during its third field trial.Track plotted using Google Earth software.

127

This chapter discussed the experiments used to evaluate DORA’s performance

as well as to explore the feasibility of deploying DORA’s steering behavior

aboard a practical vehicle. During the course of these experiments, the ca-

pabilities and limitations of the algorithm were demonstrated, and a number of

conclusions may be drawn from these results. Similarly, some areas for potential

further exploration and improvement were discovered. Both of these: questions

answered and questions raised will be discussed in the following chapter.

128

Chapter 8

Future Developments and

Conclusions

This final chapter summarizes some of the useful conclusions that can be drawn

from the results presented above. These outcomes will hopefully provide useful

information to support further development in artificially intelligent planners

and systems. Further, there are many questions that were raised during the

course of the research that remain for future development and characterization

of the DORA system, and some of these are discussed below.

8.1 Future Development

While this project has addressed its original hypothesis, it has also laid the

foundations for other future investigations. Following are some of the more

interesting areas, which bear further investigation.

129

• The agent preference formula has much potential for improvement. The

collision avoidance term in particular could be implemented in other ways

that may or may not improve its performance. Two such means would be

to shift it from downward-looking to forward-looking or some combination

of those, or to completely rewrite the collision avoidance behavior so that

instead of relying on the gradient to choose its direction, it could apply a

reduction to the stability term. This way it would not need to measure the

actual gradient of the ocean floor, but only log the change in depth with

time, and tend to shy away from directions that were leading to shallow

water. This latter option has the advantage of simplifying the equation

to three vectors rather than four, and reducing the amount of information

that is required for an agent to act. It is unclear whether this would help

or hurt an agent’s performance, and trials would need to be conducted to

evaluate both options.

• The algorithm should be able to benefit from experience. The simple

learning trial tested during the course of this project did not show a signif-

icant advantage, but that was only one method with one choice of param-

eters. A greater focus on tailoring the learning algorithm to the problem

might prove to be beneficial, particularly by way of reducing computa-

tion time. Since the initial random search takes a great deal of time, it

is reasonable to expect that a search that is focused in the area of prior

successes might dramatically reduce this time. At this point, however this

is mere conjecture, and needs to be validated through experimentation.

• The parameters to the collision avoidance term, M and T , were chosen

based on statistically insignificant optimization, as was the ratio of agents

130

used in exploration to those used in exploitation. The performance of the

software may be improved by a more rigorous study of the relationships

between each of these parameters and the outcome of the search.

• Simulated annealing was used for optimization of the parameters in the

exploitation phase of the search, however there are a great many other op-

timization algorithms that may prove better for this particular problem.

An additional study of optimization with respect to this problem may im-

prove the quality of the solutions that are produced, as well as the time

required to produce them. One potential candidate that is of particular

interest is genetic algorithms (GA). Genetic algorithms are good at find-

ing creative solutions to problems, and their random mutation helps to

minimize the tendency of an algorithm to become trapped in local min-

ima. The simulated annealing method used here does little to prevent

trapping in local minima, so this could represent a substantial improve-

ment. Also, the initialization of a random population would provide a

convenient means to implement the ability to learn from experience as

described above.

• The agents could be allowed to interact with one another. One of the

ways in which DORA departs from most true swarming algorithms is that

no agent directly influences any other agent. In most swarms, multiple

interactions are very beneficial to propagating useful information to other

agents. Adding this communication architecture to the system would

add computational overhead, but it is possible that it would improve the

performance of the system enough to offset that expense.

• The ASV shows promise as a lab-scale development platform, and the

131

hull form has potential for use in other field experiments, however it needs

further improvement and characterization. The hull is designed to be easy

to construct, but it sacrifices any attempt at hydrodynamic optimization.

Further studies to characterize the flow around the hull could improve

its performance. Also, using manufacturing processes that are capable of

producing three-dimensional curves would allow for another dimension of

optimization and the elimination of the sharp corners at the joint between

the side wall and the bottom plate of the vessel.

All of these areas for further exploration and improvement represent questions

raised by this work, but the next and final section aspires to briefly summarize

the questions that have been successfully addressed.

8.2 Conclusions

The original hypothesis that began this project was that the behaviors of sim-

ple agents using the preferences in Equation 3.1, could be optimized so that

they would navigate along good trajectories, which could then be used as path

plans for AUV’s. The experiments performed here showed that as a long range

path planner for an AUV, this algorithm failed to reliably solve the problem

of navigation, and when it did solve the problem, the paths found were often

of poorer quality than even a very rough A*-based algorithm. In some cases,

DORA performed quite well, finding good solutions in short times, but it was

not consistent enough to supplant existing techniques that are more robust.

Thus, as a competitor against modern path planners, DORA is not a favorable

alternative, particularly in complex environments with obstructions.

132

However, some interesting outcomes were discovered during the experimentation

and optimization phases of this work, which suggest some useful contributions

to navigation, and are considered to be the primary accomplishments of this

research.

First, the agents that were modeled here were not originally intended to rep-

resent any physical vehicle, but rather were intended as intermediaries used to

generate the trajectory that would be returned as the output of the algorithm.

However, since they were subject to the influence of a model of realistic ocean

currents and used only locally measurable information, it became feasible to

physically implement them as very simple unmanned vehicles with the agent

preference formula as a navigation system. This concept was tested with the

construction of an ASV, and in the limited cases that it was evaluated, the sys-

tem was very successful. The one series of field tests is not enough to draw any

conclusions beyond proving that physical implementation of the DORA system

is practical, but this practicality has strong implications for the merit of the

algorithm.

Second, the ASV itself provides an inexpensive, field-ready test bed for fur-

ther development. It is primarily built of inexpensive commercial off-the-shelf

technology that is already common in academic and hobby communities, and is

designed to be modular so that individual components can be used in other de-

signs. These features make it a viable platform for high volume construction, for

additional experiments in behaviors and navigation, as well as for swarm-based

projects.

Third, the agent preference formula itself, while not a strong basis for a long

range path planner, did prove to be much more versatile and optimal as the

133

basis for a steering behavior than simple homing or tracking. In general, homing

was not as reliable as tracking, and tracking was only successful in a fraction

of the cases for which tuned agent preferences were used. Further, the agent

preference-based navigation allows for much more complex paths than either of

these, and so fills a gap between control systems and full-fledged path planners

and navigation systems. The fact that it outperformed established steering

behaviors is significant, as its flexibility might allow for efficient graph-based

path planning with much more sparse graphs. If DORA can find very good

solutions to unobstructed point-to-point navigation problems, it may relieve an

A*-based algorithm of much of its need for resolution. Since A*’s computation

time increases dramatically with increased resolution, it is easy to envision a

system that would use a very sparse graph, searched by A* to find collision

free paths that could then be very efficiently navigated by DOR agents. This

however, is a question that is left for later explorations.

134

References

[1] CMPS09 Documentation. URL http://www.robot-electronics.co.uk/

htm/cmps09doc.htm.

[2] ETOPO2v2 Global Gridded 2-minute Database,. URL http://www.ngdc.

noaa.gov/mgg/global/etopo2.html.

[3] Data Sheet / GP-635T. URL http://www.adh-tech.com.tw/?12,

gp-635t.

[4] L78S05 Datasheet, 2006. URL https://www.jameco.com/Jameco/

Products/ProdDS/889461.pdf.

[5] XBee / XBee Pro Manual, 2007.

[6] SeaPerch Construction Manual. AUVSI Foundation, Arlington, VA, 2011.

URL http://www.seaperch.org/.

[7] C. Amante and B.W. Eakins. ETOPO1 1 Arc-Minute Global Relief Model:

Procedures, Data Sources and Analysis. Technical Report 11, National

Geophysical Data Center, Boulder, CO, 2009.

135

[8] Eric Bonabeau, Marco Dorigo, and Guy Theralaus. Swarm Intelligence:

From Natural to Artificial Systems. Oxford University Press, Santa Fe,

New Mexico, USA, 1999. ISBN 978-0-19-513159-8.

[9] Nathaniel Bowditch. THE AMERICAN PRACTICAL NAVIGATOR.

Number 9. National Imagery and Mapping Agency, Bethesda, MD, 2002.

[10] K.P. Carroll, S.R. McClaran, E.L. Nelson, D.M. Barnett, D.K. Friesen, and

GN William. AUV path planning: an A approach to path planning with

consideration of variable vehicle speeds and multiple, overlapping, time-

dependent exclusion zones. In Autonomous Underwater Vehicle Technol-

ogy, 1992. AUV’92., Proceedings of the 1992 Symposium on, pages 79–

84. IEEE, 1992. ISBN 0780307046. URL http://ieeexplore.ieee.org/

xpls/abs_all.jsp?arnumber=225191.

[11] Xin Chen and Yangmin Li. Smooth path planning of a mobile robot using

stochastic particle swarm optimization. In Mechatronics and Automation,

Proceedings of the 2006 IEEE International Conference on, pages 1722–

1727. IEEE, 2006. ISBN 1424404665. URL http://ieeexplore.ieee.

org/xpls/abs_all.jsp?arnumber=4026352.

[12] Russ E Davis, Charles C Eriksen, and Clayton P Jones. 3. autonomous

buoyancy-driven underwater gliders. 2003.

[13] E. W. Dijkstra. A Note on Two Problems in Connexion with Graphs.

Numerische Mathematik, 1(1):269–271, 1959.

136

[14] X Fan, Xiong Luo, Sheng Yi, and S Yang. Optimal path planning for

mobile robots based on intensified ant colony optimization algorithm. In

Proceedings of the 2003 IEEE International Conference on Robotics, In-

telligent Systems and Signal Processing, number October, pages 131–136,

2003. ISBN 078037925X. URL http://ieeexplore.ieee.org/xpls/abs_

all.jsp?arnumber=1285562.

[15] E Fernandez-Perdomo, D Hernandez-Sosa, J Isern-Gonzalez, J Cabrera-

Gamez, A C Dominguez-Brito, and V Prieto-Maranon. Single and multiple

glider path planning using an optimization-based approach. In OCEANS,

2011 IEEE - Spain, pages 1–10, June 2011. doi: 10.1109/Oceans-Spain.

2011.6003641.

[16] E Fernandez-Perdomo, J Cabrera-Gandmez, D Hernandndez-Sosa, J Isern-

Gonzandlez, A C Domiandnguez-Brito, A Redondo, J Coca, A G Ramos,

E A Fanjul, and M Garci anda. Path planning for gliders using Regional

Ocean Models: Application of Pinz #x00F3;n path planner with the ES-

EOAT model and the RU27 trans-Atlantic flight data. In OCEANS 2010

IEEE - Sydney, pages 1–10, May 2010. doi: 10.1109/OCEANSSYD.2010.

5603684.

[17] B Garau, M Bonet, A Alvarez, and S Ruiz. Path Planning for Autonomous

Underwater Vehicles in Realistic Oceanic Current Fields: Application to

Gliders in the Western Mediterranean Sea. Journal of Maritime Research,

VI(Ii):5–22, 2009. URL http://www.jmr.unican.es/pub/00602/00602.

pdf#page=7.

137

[18] Maki K. Habib and Hajime Asama. Efficient Method to Generate Colli-

sion Free Paths for an Autonomous Mobile Robot Based on New Free Space

Structuring Approach. In IEEE/RSJ International Workshop on Intelli-

gent Robots and Systems, number 91, 1991. URL http://ieeexplore.

ieee.org/xpls/abs_all.jsp?arnumber=174534.

[19] Peter E Hart and J Nils. A Formal Basis for the Heuristic Determina-

tion of Minimum Cost Paths. IEEE Transactions of Systems Science and

Cybernetics, (2):100–107, 1968.

[20] J D Hunter. Matplotlib: A 2D graphics environment. Computing In Science

& Engineering, 9(3):90–95, 2007.

[21] Eric Jones, Travis Oliphant, Pearu Peterson, and Others. {SciPy}: Open

source scientific tools for {Python}. URL http://www.scipy.org/.

[22] James Kennedy and R. Eberhart. Particle swarm optimization. In Neu-

ral Networks, 1995. Proceedings., IEEE International Conference on, vol-

ume 4, pages 1942–1948. IEEE, 1995. URL http://ieeexplore.ieee.

org/xpls/abs_all.jsp?arnumber=488968.

[23] O Khatib. Real-time obstacle avoidance for manipulators and mobile

robots. In Robotics and Automation. Proceedings. 1985 IEEE Inter-

national Conference on, volume 2, pages 500–505, March 1985. doi:

10.1109/ROBOT.1985.1087247.

138

[24] Li Lu and Dunwei Gong. Robot Path Planning in Unknown Environments

Using Particle Swarm Optimization. 2008 Fourth International Conference

on Natural Computation, pages 422–426, 2008. doi: 10.1109/ICNC.2008.

923. URL http://ieeexplore.ieee.org/lpdocs/epic03/wrapper.htm?

arnumber=4667318.

[25] Ian Millington and John Funge. Artificial Intelligence for Games. Morgan

Kaufmann Publishers, Burlington, MA, second edition, 2009.

[26] Roving Networks. User Manual and Command Reference WiFly GSX and

WiFly EZX, 2011.

[27] O Oberschelp and Thorsten Hestermeyer. Design of self-optimizing agent-

based controllers. In 3rd International Workshop on Agent-Based Simu-

lation, 2002. URL http://citeseerx.ist.psu.edu/viewdoc/download?

doi=10.1.1.11.8918&rep=rep1&type=pdf.

[28] Y.Q. Qin, D.B. Sun, N. Li, and Y.G. Cen. Path planning for mobile

robot using the particle swarm optimization with mutation operator. In

Machine Learning and Cybernetics, 2004. Proceedings of 2004 Interna-

tional Conference on, volume 4, pages 2473–2478. IEEE, 2004. URL

http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=1382219.

[29] K. J. Rawson and E. C. Tupper. Basic Ship Theory. International Marine

/ McGraw-Hill, Woburn, MA, 2001.

139

[30] Craig W Reynolds. Steering Behaviors For Autonomous Characters. In

Game Developers Conference, pages 763–782, San Jose, California, 1999.

Miller Freeman Game Group. URL http://www.red3d.com/cwr/papers/

1999/gdc99steer.html.

[31] Blane Rhoads, I Mezic̀I, and Andrew Poje. Minimum time feedback

control of autonomous underwater vehicles. In 49th IEEE Conference

on Decision and Control, pages 5828–5834, Atlanta, GA, 2010. IEEE.

ISBN 9781424477463. URL http://ieeexplore.ieee.org/xpls/abs_

all.jsp?arnumber=5717533.

[32] Stuart Russel and Peter Norvig. Artificial Intelligence: A Modern Ap-

proach. Prentice Hall, Upper Saddle River, NJ, second edition, 2002.

[33] Martin Saska, Martin Macas, Libor Preucil, and Lenka Lhotska. Robot

Path Planning using Particle Swarm Optimization of Ferguson Splines.

2006 IEEE Conference on Emerging Technologies and Factory Au-

tomation, pages 833–839, September 2006. doi: 10.1109/ETFA.2006.

355416. URL http://ieeexplore.ieee.org/lpdocs/epic03/wrapper.

htm?arnumber=4178249.

[34] Mae L Seto, editor. Marine Robot Autonomy. Springer. URL http://www.

springer.com/engineering/robotics/book/978-1-4614-5658-2.

140

[35] R.N. Smith and Matthew Dunbabin. Controlled Drift: An Investiga-

tion into the Controllability of Underwater Vehicles with Minimal Actu-

ation. Technical report, Queensland University of Technology, Brisbane,

Australia, 2008. URL http://robotics.usc.edu/~ryan/Publications_

files/acra.pdf.

[36] James Stewart. Calculus. Thompson Brooks/Cole, Belmont, CA, 6th edi-

tion, 2008.

[37] Guan-zheng Tan, Huan He, and Sloman Aaron. Global optimal path plan-

ning for mobile robot based on improved Dijkstra algorithm and ant system

algorithm. Journal of Central South University of Technology, 13(1):80–

86, February 2006. ISSN 1005-9784. doi: 10.1007/s11771-006-0111-8. URL

http://www.springerlink.com/index/10.1007/s11771-006-0111-8.

[38] U. Wilensky. NetLogo, 1999. URL http://ccl.northwestern.edu/

netlogo.

[39] Jonas Witt and Matthew Dunbabin. Go with the Flow : Optimal

AUV Path Planning in Coastal Environments Autonomous Systems.

Technical report, 2008. URL http://www.mendeley.com/research/

go-flow-optimal-auv-path-planning-coastal-environments/.

141

[40] Li-Ning Xing, Philipp Rohlfshagen, Ying-Wu Chen, and Xin Yao. A Hy-

brid Ant Colony Optimization Algorithm for the Extended Capacitated

Arc Routing Problem. IEEE transactions on systems, man, and cyber-

netics. Part B, Cybernetics : a publication of the IEEE Systems, Man,

and Cybernetics Society, pages 78–88, February 2011. ISSN 1941-0492.

doi: 10.1109/TSMCB.2011.2107899. URL http://www.ncbi.nlm.nih.

gov/pubmed/21896393.

142

Appendix A

Arduino Source Code

// Autonomous Surface Vehicle Control Software

// Anthony Jones

// April 15, 2013

/////////////////

// Import libraries and define global variables

#include<Servo.h>

#include<math.h>

#define REarth 6378000.0 //Typographical error--should be 6371000.0

#define Declination -6.36222222222 //Local Declination at

// the center of zip code 32935

// Declare buffers to store serial data

char s0buff[256];

char s1buff[6];

char s2buff[256];

char s3buff[256];

// Set range for length of leg

const float tgtRange=250.0; //Meters to Target

int upInterval=30; //Navigation Update Interval in Seconds

const float spd=12.0/18.0; //ASV Speed in m/s at full speed

float Heading,Pitch,Roll,SetHdg,t0,t1=0.0;

float B,C,D=1.0;

float Lat,Lon,LatPrev,LonPrev,tgtLat,tgtLon,hmeLat,

hmeLon,hh,mm,ss=0.0;

unsigned char SSpd=0;

Servo Rudder;

// Declare and initialize variables to store timing information

float misTime,startTime,iterTime=0.0;

// Set the range for successful arrival at objective

float RngThresh=15.0;

///////////////////////////////////////////////////////////////

//////////////// SETUP ////////////////////////////

///////////////////////////////////////////////////////////////

void setup()

143

{

// Configure Input and Output hardware

pinMode(3,OUTPUT);

Rudder.attach(2);

Serial.begin(9600); // XBee

if (Serial)

{

Serial.println("Serial Initialized");

}

Serial1.begin( 9600); // Compass

Serial1.setTimeout(100);

if(Serial1)

{

Serial.println("Serial1 Initialized");

}

Serial2.begin(9600); // OpenLog

if (Serial2)

{

Serial.println("Serial2 Initialized");

}

Serial3.begin(9600); // GPS

if(Serial3)

{

Serial.println("Serial3 Initialized");

} Serial.println("Getting GPS");

// Get initial GPS and Compass readings

getGPS();

Serial.println("Getting Heading");

Heading=getH();

Serial.println( Heading);

SetHdg=Heading;

hmeLat=Lat;

hmeLon=Lon;

float hmeLatM=REarth*rads(hmeLat);

float hmeLonM=REarth*cos(rads(hmeLat))*rads(hmeLon);

float tgtLatM=hmeLatM+tgtRange*cos(rads(SetHdg));

float tgtLonM=hmeLonM+tgtRange*sin(rads(SetHdg));

// Set target position

tgtLat=degs(tgtLatM/REarth);

tgtLon=degs(tgtLonM/(REarth*cos(rads(hmeLat))));

Serial.print(tgtLat,5);

Serial.print(’\t’);

Serial.println(tgtLon,5);

//////// Set the preference values//////////////

B=-0.422;

C=0.087;

D=0.491;

float BCDsum=B+C+D;

B=B/(BCDsum);

C=C/(BCDsum);

D=D/(BCDsum);

// Start Timers

144

startTime=millis();

iterTime=millis();

// Set motor drive PWM to full

SSpd=255;

}

///////////////////////////////////////////////////////////////

//////////////// CONTROL LOOP ///////////////////////////

///////////////////////////////////////////////////////////////

void loop()

{

while(Serial1.available()>0)

{

Serial1.read();

}

Serial1.write(0x23);

t0=millis();

while(Serial1.available()<4 and millis()-t0<100);

if (Serial1.available()==4)

{

Serial1.readBytes(s1buff,4);

}

// Update Vehicle Heading

Heading=getH();

misTime=millis()-iterTime;

// Check update timer

if(misTime>upInterval*1000)

{

// If the update timer has expired, use DORA steering

// behavior to calculate new heading, unless within

// RngThresh of the objective.

if (planeRTT()<RngThresh)

{

if(tgtLat==hmeLat && tgtLon==hmeLon)

{

SSpd=0;

}

tgtLat=hmeLat;

tgtLon=hmeLon;

}

LatPrev=Lat;

LonPrev=Lon;

// Update Position

getGPS();

// Calculate new preferred heading

float dt=(millis()-iterTime)/1000.0;

float LatCalc=(LatPrev+Lat)/2.0;

float LonM=cos(rads(LatCalc))*REarth*rads(Lon);

float LatM=REarth*rads(Lat);

float LatPrevM=REarth*rads(LatPrev);

float LonPrevM=cos(rads(LatCalc))*REarth*rads(LonPrev);

float dLonM=cos(rads(LatCalc))*REarth*rads((Lon-LonPrev));

float dLatM=REarth*rads(Lat-LatPrev);

145

float asvU=sin(rads(SetHdg))*spd;

float asvV=cos(rads(SetHdg))*spd;

float asvMag=sqrt(asvU*asvU+asvV*asvV);

float expLatM=LatPrevM+asvV*dt;

float expLonM=LonPrevM+asvU*dt;

float lclU=(expLonM-LonM)/dt;

float lclV=(expLatM-LatM)/dt;

float lclMag=sqrt(lclU*lclU+lclV*lclV);

float tgtLatM=REarth*rads(tgtLat);

float tgtLonM=REarth*cos(rads(LatCalc))*rads(tgtLon);

float tgtX=tgtLonM-LonM;

float tgtY=tgtLatM-LatM;

float tgtMag=sqrt(tgtX*tgtX+tgtY*tgtY);

tgtX=tgtX/tgtMag*D;

tgtY=tgtY/tgtMag*D;

float asvX=asvU/asvMag*C;

float asvY=asvV/asvMag*C;

float lclX=lclU/lclMag*B*(lclMag/(lclMag+spd));

float lclY=lclV/lclMag*B*(lclMag/(lclMag+spd));

float totalX=lclX+asvX+tgtX;

float totalY=lclY+asvY+tgtY;

// set heading to newly calculated direction

SetHdg=degs(atan2(totalX,totalY));

if (SetHdg<0)

{

SetHdg=360.0+SetHdg;

}

// update the mission log

Serial2.print((millis()-startTime)/1000.0,3);

Serial2.print(’,’);

Serial2.print(hh);

Serial2.print(’,’);

Serial2.print(mm);

Serial2.print(’,’);

Serial2.print(ss);

Serial2.print(’,’);

Serial2.print(Lat,5);

Serial2.print(’,’);

Serial2.print(Lon,5);

Serial2.print(’,’);

Serial2.print(tgtLat,5);

Serial2.print(’,’);

Serial2.print(tgtLon,5);

Serial2.print(’,’);

Serial2.print(dt,3);

Serial2.print(’,’);

Serial2.print(lclU,3);

Serial2.print(’,’);

Serial2.print(lclV,3);

Serial2.print(’,’);

Serial2.print(lclMag,3);

Serial2.print(’,’);

146

Serial2.print(RngThresh,3);

Serial2.print(’,’);

Serial2.print(SetHdg);

Serial2.print(’,’);

Serial2.print(Heading);

Serial2.print(’,’);

Serial2.print(SSpd);

Serial2.print(’,’);

Serial2.println(planeRTT(),3);

// Reset the update timer

iterTime=millis();

}

// Set steering direction and shaft speed

Rudder.write(Steering(Heading,SetHdg));

analogWrite(3,SSpd);

// check for any incoming commands on Serial

while(Serial.available()>0)

{

char con=Serial.read();

if(con==’r’)

{

Rudder.write(Serial.parseInt());

}

if(con==’s’)

{

SSpd=Serial.parseInt();

}

else

{

Serial.read();

}

}

}

//////////////////////END CONTROL LOOP/////////////////////////

// Calculate a rudder steering angle implementing

// P-Controller

int Steering(float NowHdg,float SetHeading)

{

int Signal;

float dHeading=Heading-SetHdg;

if (abs(dHeading)>360.0)

{

dHeading=dHeading-360.0;

}

Signal=90+(int)((dHeading)*-3.75);

if (Signal>125)

{

Signal=125;

}

if (Signal<55)

{

147

Signal=55;

}

return Signal;

}

// Update the global heading variable with a new

// reading from the CMPS09 compass

float getH()

{

char H[4];

float Hfloat=0.0;

while(Serial1.available()>0)

{

Serial1.read();

}

Serial1.write(0x23);

t0=millis();

while(Serial1.available()<4 and millis()-t0<100);

if (Serial1.available()==4)

{

Serial1.readBytes(H,4);

Hfloat=((byte)H[0]*256+(byte)H[1])/10.0;

Hfloat+=Declination;

if (Hfloat<0)

{

Hfloat+=360.0;

}

}

else

{

Hfloat=Heading;

}

return Hfloat;

}

// Update the global position variables with a new

// reading from the GPS

void getGPS()

{

char ID[]="GPXXX";

boolean KeepRun=true;

while(KeepRun==true)

{

while(ID[0]!=’$’)

{

ID[0]=Serial3.read();

}

Serial3.readBytes(ID,5);

if ( (String)ID=="GPGLL")

{

Serial3.readBytesUntil(13,s3buff,200);

if (s3buff[38]==’A’)

148

{

float LatDeg=(c2f(s3buff[1])*10.0+c2f(s3buff[2]));

float LatMin=(c2f(s3buff[3])*10.0+c2f(s3buff[4]));

LatMin+=c2f(s3buff[6])/10.0+c2f(s3buff[7])/100.0

+c2f(s3buff[8])/1000.0+c2f(s3buff[9])/10000.0

+c2f(s3buff[10])/100000;

Lat=LatDeg+LatMin/60.0;

if (s3buff[12]==’S’)

{

Lat=Lat*-1.0;

}

float LonDeg=(c2f(s3buff[14])*100.0+c2f(s3buff[15])*10.0+

c2f(s3buff[16]));

float LonMin=c2f(s3buff[17])*10.0+c2f(s3buff[18])

+c2f(s3buff[20])/10.0+c2f(s3buff[21])/100.0

+c2f(s3buff[22])/1000.0+c2f(s3buff[23])/10000.0

+c2f(s3buff[24])/100000.0;

Lon=LonDeg+LonMin/60.0;

if (s3buff[26]==’W’)

{

Lon=Lon*-1.0;

}

hh=c2f(s3buff[28])*10+c2f(s3buff[29]);

mm=c2f(s3buff[30])*10+c2f(s3buff[31]);

ss=c2f(s3buff[32])*10+c2f(s3buff[33])+c2f(s3buff[34])/10.0

+s3buff[35]/100.0;

KeepRun=false;

}

}

}

}

// Convert from an ASCII character representation of a number to

// its decimal equivalent

float c2f(char inchar)

{

return inchar-48.0;

}

// Convert from radians to degrees

float degs(float rads)

{

return rads/PI*180.0;

}

//Convert from degrees to radians

float rads(float degs)

{

return degs/180.0*PI;

}

// Calculate the planar approximated range in meters from

149

// current GPS fix to objective

float planeRTT()

{

float LatCalc=(tgtLat+Lat)/2.0;

float LatM=rads(Lat)*REarth;

float tgtLatM=rads(tgtLat)*REarth;

float LonM=cos(rads(LatCalc))*REarth*rads(Lon);

float tgtLonM=cos(rads(LatCalc))*REarth*rads(tgtLon);

return sqrt( pow((tgtLonM-LonM),2) +pow(tgtLatM-LatM,2));

}

150

Appendix B

NetLogo Source Code

; NetLogo Prototype of DORA software (written for NetLogo v5.0.3)

; Anthony Jones

; April 15, 2013

;

; Configure Global Variables

globals [ pidt pide crabt crabe groupert groupere grouperspd

threshrng bestA bestB bestC bestD bestF bestBeta bestSpd

mortality success total nowsdev]

; Establish custom breeds for waypoints, stations, and different

; types of agents

breed [stations station]

stations-own [u v]

breed [waypoints waypoint]

breed [crabs crab]

;Crabs use tracking behavior to close on objective

crabs-own [speed pwr energy time stick ]

breed [pigeons pigeon]

;Pidgeons use homing behavior to close on objective

pigeons-own[speed pwr energy time stick ]

breed [groupers grouper]

; Groupers use DORA behavior to close on objective

groupers-own[ B C D alpha beta speed stick pwr energy time ]

breed [markers marker]

markers-own[u v]

; Setup Configures the space according to preferences set in

; the GUI

to setup

clear-all

; Initialize preferences to zero

set bestB 0

151

set bestC 0

set bestD 0

set threshrng 0.5

ask patches [ set pcolor white ]

reset-ticks

set nowsdev sdev

create-stations 1

[

set xcor s0x

set ycor s0y

set u s0u

set v s0v

set color blue

set size 2.0

set shape "arrow"

ifelse u = 0 and v = 0 [ set heading 0][set heading atan u v]

]

create-stations 1

[

set xcor s1x

set ycor s1y

set u s1u

set v s1v

set color blue

set size 2.0

set shape "arrow"

ifelse u = 0 and v = 0 [ set heading 0][set heading atan u v]

]

create-stations 1

[

set xcor s2x

set ycor s2y

set u s2u

set v s2v

set color blue

set size 2.0

set shape "arrow"

ifelse u = 0 and v = 0 [ set heading 0][set heading atan u v]

]

create-waypoints 1

[

set shape "circle"

set color green

set size 1.5

set xcor wp0x

set ycor wp0y

]

create-waypoints 1

[

set shape "circle"

set color red

set size 1.5

152

set xcor wp1x

set ycor wp1y

]

create-crabs 0

[

set color red

set size 2.0

set xcor wp0x

set ycor wp0y

set speed spd

set pwr EnRate

set energy energycap

]

create-pigeons 0

[

set color yellow

set size 2.0

set xcor wp0x

set ycor wp0y

set speed spd

set pwr EnRate

set energy energycap

facexy wp1x wp1y

]

end

; Main Loop

to go

; Reduce the standard deviation for simulated annealing every

; 100 ticks

if (ticks mod 100 = 0)

[

set nowsdev nowsdev * 0.8

]

; create randomly initialized groupers until a solution is found

; then use simulated annealing to initialize new ones centered on

; best solution so far

create-groupers agents - ( count groupers )

[ set total total + 1

set energy energycap

set stick ticks

ifelse bestB = 0 and BestC = 0 and BestD = 0 and

BestF = 0 and bestBeta = 0

[

set B random-float 2.0 - 1

set C random-float 2.0 - 1

set D random-float 2.0 - 1

set speed spd ;random-float spd

]

[

set B random-normal bestB sdev

set C random-normal bestC sdev

153

set D random-normal bestD sdev

set speed spd ;random-normal bestSpd ( sdev * spd )

if speed > spd [set speed spd]

if speed < 0 [set speed 0]

]

if JustBest [ set B bestB set C bestC set D bestD ]

let bcdsum abs B + abs C + abs D

set B B / bcdsum

set C C / bcdsum

set D D / bcdsum

set color green

set size 2.0

set xcor wp0x

set ycor wp0y

set pwr EnRate * ( speed / spd ) ^ 3

facexy wp1x wp1y

]

; implement homing behavior by pidgeons

ask pigeons

[

set time ticks * tempres

if time mod upInt = 0

[

facexy wp1x wp1y

]

let pidu speed * sin heading

let pidv speed * cos heading

let tgtx xcor + (pidu + lclu) * tempres

let tgty ycor + (pidv + lclv) * tempres

set energy ( energy - ( pwr * tempres) )

if energy <= 0 [die]

ifelse (min-pxcor < tgtx and tgtx < max-pxcor and min-pycor <

tgty and tgty < max-pycor) [setxy tgtx tgty][ die ]

if distancexy wp1x wp1y < threshrng [ set pidt ticks *

tempres set pide energy die]

]

ask crabs

[

set time ticks * tempres

let xwind lclspd * sin ( subtract-headings wphdg lclhdg )

let hdgoff 0

ifelse abs ( xwind ) < speed [ set hdgoff asin ( xwind / speed)

][set hdgoff 0]

if time mod upInt = 0

[

set heading wphdg + hdgoff

]

show heading

let crabu speed * sin heading

let crabv speed * cos heading

let tgtx xcor + (crabu + lclu) * tempres

let tgty ycor + (crabv + lclv) * tempres

154

set energy ( energy - ( pwr * tempres) )

if energy <= 0 [die]

if distancexy wp1x wp1y < threshrng [ set crabt ticks * tempres

set crabe energy die]

ifelse (min-pxcor < tgtx and tgtx < max-pxcor and

min-pycor < tgty and tgty < max-pycor) [setxy tgtx tgty][ die ]

]

; implement DROA behavior

ask groupers

[

set time ( ( ticks - stick ) * tempres )

let reciphdg ( ( wphdg + 180 ) mod 360 )

if time mod upInt = 0

[

let Bu B * ( lclspd / (lclspd + speed) ) * sin lclhdg

let Bv B * ( lclspd / (lclspd + speed) ) * cos lclhdg

let Cu C * sin heading

let Cv C * cos heading

let Du D * sin wphdg

let Dv D * cos wphdg

let totU Bu + Cu + Du

let totV Bv + Cv + Dv

set heading atan totU totV

]

let grouperu speed * sin heading

let grouperv speed * cos heading

let tgtx xcor + (grouperu + lclu) * tempres

let tgty ycor + (grouperv + lclv) * tempres

set energy energy - ( pwr * tempres)

if energy <= 0 [die]

ifelse (min-pxcor < tgtx and tgtx < max-pxcor and

min-pycor < tgty

and tgty < max-pycor)

[setxy tgtx tgty][ set mortality mortality + 1 die ]

let outlist ( list "B: " B " C: " C

" D: " D " beta: " beta )

ifelse TnotE

[

if ( distancexy wp1x wp1y <= threshrng ) and

( ( time < groupert ) or ( groupert = 0 ) )

[

set groupert time

set groupere energy

set grouperspd speed

set bestB B

set bestC C

set bestD D

set bestBeta beta

file-open "netlogo.log"

file-print ( list bestB bestC bestD bestF )

file-close

155

]

]

[

if ( distancexy wp1x wp1y <= threshrng ) and

( ( energy > groupere ) or ( groupert = 0 ) )

[

set groupert time * tempres

set groupere energy

set grouperspd speed

set bestB B

set bestC C

set bestD D

set bestBeta beta

file-open "DisModel.log"

file-write ( list bestB bestC bestD bestF )

file-close

]

]

if distancexy wp1x wp1y <= threshrng [ set success

success + 1 show ( time ) die ]

]

tick

end

to-report lclu

let lu 0.0

let sumu 0.0

let sumd 0.0

foreach sort stations

[

set sumd sumd + 1.0 / (distancexy [xcor] of ? [ycor] of ?)

]

foreach sort stations

[

set lu lu + ( 1.0 / distancexy [xcor] of ? [ycor] of ? ) /

(sumd) * [u] of ?

]

report lu

end

; return the local u an v components of velocity based on

; weighted average of the stations’ values, weights by range

to-report lclv

let lv 0.0

let sumv 0.0

let sumd 0.0

foreach sort stations

[

set sumd sumd + 1.0 / (distancexy [xcor] of ? [ycor] of ?)

]

foreach sort stations

[

156

set lv lv + ( 1.0 / distancexy [xcor] of ? [ycor] of ? ) /

(sumd) * [v] of ?

]

report lv

end

; return the local heading of the currents

to-report lclhdg

if lclu = 0 and lclv = 0 [report 0]

report atan lclu lclv

end

; return the magnitude of the local current

to-report lclspd

report sqrt (lclu ^ 2 + lclv ^ 2)

end

; return the heading from the agent to the objective

to-report wphdg

report atan ( wp1x - xcor ) ( wp1y - ycor )

end

157

Appendix C

Python Source Code

C.1 environment.py

import sys

sys.path.append(’./’)

import scipy.io.netcdf as ncio

import numpy as np

import navigation as nav

#ALL DISTANCES ARE IN METERS,

#VELOCITIES IN METERS PER SECOND, TIMES IN HOURS

REarth=6378.0*1000 #Radius of Earth in meters

# Data structure to store velocity field data

class VField:

def __init__(self,ncfname):

# Parse .nc file

if ncfname.split(’.’)[len(ncfname.split(’.’))-1]==’nc’:

ncfile=ncio.netcdf_file(ncfname,’r’)

self.u=np.array(ncfile.variables.get(\

’water_u’).data,np.float)

self.v=np.array(ncfile.variables.get(\

’water_v’).data,np.float)

self.lats=np.array(ncfile.variables.get(\

’lat’).data,np.float)

self.lons=np.array(ncfile.variables.get(\

’lon’).data,np.float)

self.times=np.array(ncfile.variables.get(\

’time’).data,np.float)

self.depths=np.array(ncfile.variables.get(\

’depth’).data,np.float)

for i in xrange(self.u.shape[0]):

for j in xrange(self.u.shape[1]):

for k in xrange(self.u.shape[2]):

158

for l in xrange(self.u.shape[3]):

if self.u[i,j,k,l]==-30000:

self.u[i,j,k,l]=np.nan

if self.v[i,j,k,l]==-30000:

self.v[i,j,k,l]=np.nan

#self.u=self.u*(1.0/1000.0)

#self.v=self.v*(1.0/1000.0)

self.times=self.times-np.nanmin(self.times)

# Parse a data set that is already converted to a zipped set of

# numpy arrays. Note: npz must contain u,v,lats,lons,times, and

# depths arrays

elif ncfname.split(’.’)[len(ncfname.split(’.’))-1]==’npz’:

vfdata=np.load(ncfname)

self.u=vfdata[’u’]

self.v=vfdata[’v’]

self.lats=vfdata[’lats’]

self.lons=vfdata[’lons’]

self.times=vfdata[’times’]

self.depths=vfdata[’depths’]

# Record some useful global properties for use in algorithms

non_nan=np.count_nonzero(np.isnan(self.u))

self.maxmag=np.nanmax(np.sqrt(np.nanmax(np.abs(self.u))**2+\

np.nanmax(np.abs(self.v))**2))

self.meanmag=np.sqrt( (np.nansum(np.abs(self.u))/non_nan)**2+\

(np.nansum(np.abs(self.v))/non_nan)**2)

self.meanVmag=np.sqrt( (np.nansum(self.u)/non_nan)**2+\

((np.nansum(self.v))/non_nan)**2)

self.minLat=np.nanmin(self.lats)

self.minLon=np.nanmin(self.lons)

self.minD=np.nanmin(self.depths)

self.minT=np.nanmin(self.times)

self.maxLat=np.nanmax(self.lats)

self.maxLon=np.nanmax(self.lons)

self.maxD=np.nanmax(self.depths)

self.maxT=np.nanmax(self.times)

self.minu=np.nanmin(self.u)

self.minv=np.nanmin(self.v)

self.maxu=np.nanmax(self.u)

self.maxv=np.nanmax(self.v)

self.latcount=self.lats.shape[0]

self.loncount=self.lons.shape[0]

self.tcount=self.times.shape[0]

self.dcount=self.depths.shape

# Append another VField to the end of this one

def App(self,nVF):

print ’App’

’Concatenates along the Time Dimensions’

self.u=np.concatenate((self.u,nVF.u))

self.v=np.concatenate((self.v,nVF.v))

self.times=np.concatenate((self.times,nVF.times))

self.minLat=np.nanmin(self.lats)

self.minLon=np.nanmin(self.lons)

159

self.minD=np.nanmin(self.depths)

self.minT=np.nanmin(self.times)

self.maxLat=np.nanmax(self.lats)

self.maxLon=np.nanmax(self.lons)

self.maxD=np.nanmax(self.depths)

self.maxT=np.nanmax(self.times)

self.minu=np.nanmin(self.u)

self.minv=np.nanmin(self.v)

self.maxu=np.nanmax(self.u)

self.maxv=np.nanmax(self.v)

self.latcount=self.lats.shape[0]

self.loncount=self.lons.shape[0]

self.tcount=self.times.shape[0]

self.dcount=self.depths.shape[0]

# Linearly interpolate the velocity at a requested time,depth,lat,lon

def linterpTDLL(self,time,depth,lat,lon):

if lon<0:

lon=360.0+lon

# Check to make sure requested data is in dataset

if ( (self.minT <= time <=self.maxT)==False) or \

( (self.minD <= depth <=self.maxD)==False) or \

( (self.minLat <= lat <=self.maxLat)==False) or \

( (self.minLon <= lon <=self.maxLon)==False):

return np.array([np.nan,np.nan])

istep=(self.maxT-self.minT)/(self.tcount-1)

i = np.floor( (time-self.minT)/istep )

if i == self.times.shape[0]-1:

i=i-1

j = 0

while(self.depths[j]<depth):

j+=1

jstep=self.depths[j+1]-self.depths[j]

kstep=(self.maxLat-self.minLat)/(self.latcount-1)

k = np.floor( (lat-self.minLat)/( kstep ) )

lstep = (self.maxLon-self.minLon)/(self.loncount-1)

l = np.floor( (lon-self.minLon)/(lstep) )

# Do the quadrilinear interpolation for u

ulo0d0t0=(self.u[i,j,k+1,l]-self.u[i,j,k,l])*(lat\

-self.lats[k])*kstep+self.u[i,j,k,l]

ulo1d0t0=(self.u[i,j,k+1,l+1]-self.u[i,j,k,l+1])*(lat\

-self.lats[k])*kstep+self.u[i,j,k,l+1]

ud0t0=(ulo1d0t0-ulo0d0t0)*(lon\

-self.lons[l])/lstep+ulo1d0t0

ulo0d1t0=(self.u[i,j+1,k+1,l]\

-self.u[i,j+1,k,l])*(lat-self.lats[k])*\

kstep+self.u[i,j,k,l]

ulo1d1t0=(self.u[i,j+1,k+1,l+1]-self.u[i,j+1,k,l+1])*\

(lat-self.lats[k])*kstep+self.u[i,j+1,k,l+1]

ud1t0=(ulo1d1t0-ulo0d1t0)*(lon-self.lons[l])/lstep+ulo0d1t0

ut0=(ud1t0-ud0t0)*((depth-self.depths[j])/jstep)+ud0t0

ulo0d0t1=(self.u[i+1,j,k+1,l]\

-self.u[i+1,j,k,l])*(lat-self.lats[k])*\

160

kstep+self.u[i+1,j,k,l]

ulo1d0t1=(self.u[i+1,j,k+1,l+1]-self.u[i+1,j,k,l+1])*\

(lat-self.lats[k])*kstep+self.u[i+1,j,k,l+1]

ud0t1=(ulo1d0t1-ulo0d0t1)*(lon-self.lons[l])/lstep+ulo1d0t1

ulo0d1t1=(self.u[i+1,j+1,k+1,l]-self.u[i+1,j+1,k,l])*\

(lat-self.lats[k])*kstep+self.u[i+1,j,k,l]

ulo1d1t1=(self.u[i+1,j+1,k+1,l+1]-self.u[i+1,j+1,k,l+1])*\

(lat-self.lats[k])*kstep+self.u[i+1,j+1,k,l+1]

ud1t1=(ulo1d1t1-ulo0d1t1)*(lon-self.lons[l])/lstep+ulo0d1t1

ut1=(ud1t1-ud0t1)*((depth-self.depths[j])/jstep)+ud0t1

u=(ut1-ut0)*(time-self.times[i])/istep+ut0

# Do the quadrilinear interpolation for v

vlo0d0t0=(self.v[i,j,k+1,l]-self.v[i,j,k,l])*(lat\

-self.lats[k])*kstep+self.v[i,j,k,l]

vlo1d0t0=(self.v[i,j,k+1,l+1]-self.v[i,j,k,l+1])*(lat\

-self.lats[k])*kstep+self.v[i,j,k,l+1]

vd0t0=(vlo1d0t0-vlo0d0t0)*(lon-self.lons[l])/lstep+vlo1d0t0

vlo0d1t0=(self.v[i,j+1,k+1,l]-self.v[i,j+1,k,l])*(lat\

-self.lats[k])*kstep+self.v[i,j,k,l]

vlo1d1t0=(self.v[i,j+1,k+1,l+1]-self.v[i,j+1,k,l+1])*\

(lat-self.lats[k])*kstep+self.v[i,j+1,k,l+1]

vd1t0=(vlo1d1t0-vlo0d1t0)*(lon-self.lons[l])/lstep+vlo0d1t0

vt0=(vd1t0-vd0t0)*((depth-self.depths[j])/jstep)+vd0t0

vlo0d0t1=(self.v[i+1,j,k+1,l]-self.v[i+1,j,k,l])*(lat\

-self.lats[k])*kstep+self.v[i+1,j,k,l]

vlo1d0t1=(self.v[i+1,j,k+1,l+1]-self.v[i+1,j,k,l+1])*\

(lat-self.lats[k])*kstep+self.v[i+1,j,k,l+1]

vd0t1=(vlo1d0t1-vlo0d0t1)*(lon-self.lons[l])/lstep+vlo1d0t1

vlo0d1t1=(self.v[i+1,j+1,k+1,l]-self.v[i+1,j+1,k,l])*\

(lat-self.lats[k])*kstep+self.v[i+1,j,k,l]

vlo1d1t1=(self.v[i+1,j+1,k+1,l+1]-self.v[i+1,j+1,k,l+1])*\

(lat-self.lats[k])*kstep+self.v[i+1,j+1,k,l+1]

vd1t1=(vlo1d1t1-vlo0d1t1)*(lon-self.lons[l])/lstep+vlo0d1t1

vt1=(vd1t1-vd0t1)*((depth-self.depths[j])/jstep)+vd0t1

v=(vt1-vt0)*(time-self.times[i])/istep+vt0

return np.array([u,v])

class HField:

def __init__(self,HFName):

alldata=np.load(HFName)

self.lats=alldata[’y’]

self.lons=alldata[’x’]

self.z=alldata[’z’]

self.minz=np.nanmin(self.z)

self.maxz=np.nanmax(self.z)

self.minlat=np.nanmin(self.lats)

self.maxlat=np.nanmax(self.lats)

self.minlon=np.nanmin(self.lons)

self.maxlon=np.nanmax(self.lons)

def linterpll(self,latlon):

lat=latlon[0]

lon=latlon[1]

161

if self.minlat <= lat <= self.maxlat == False:

return np.nan

if self.minlon <= lon <= self.maxlon == False:

return np.nan

latstep=(self.maxlat-self.minlat)/(self.lats.shape[0]-1)

lonstep=(self.maxlon-self.minlon)/(self.lons.shape[0]-1)

k=int( (lat-self.minlat)/latstep) #Sometimes Wrong

l=int( (lon-self.minlon)/lonstep) #Sometimes Wrong

#Correct to here

k=int( (lat-self.minlat)/latstep+.000000000001)

l=int( (lon-self.minlon)/lonstep+.000000000001)

if k>self.z.shape[0]-2 or l>self.z.shape[1]-2:

return np.nan

zla0lo0=self.z[k,l]

zla0lo1=self.z[k,l+1]

zla1lo0=self.z[k+1,l]

zla1lo1=self.z[k+1,l+1]

zla0=(lon-self.lons[l])/lonstep*(zla0lo1-zla0lo0)+zla0lo0

zla1=(lon-self.lons[l])/lonstep*(zla1lo1-zla1lo0)+zla1lo0

z=(lat-self.lats[k])/latstep*(zla0-zla1)+zla0

dbgthresh=-2

return z

def grad(self,latlon,rng):

’return gradient in meters/meter with a resolution of\

\’range\’ in meters’

llW=np.array((latlon[0],latlon[1]-nav.pFromD(latlon[0],rng)))

llE=np.array((latlon[0],latlon[1]+nav.pFromD(latlon[0],rng)))

llN=np.array((latlon[0]+nav.lFromD(rng),latlon[1]))

llS=np.array((latlon[0]-nav.lFromD(rng),latlon[1]))

hW=self.linterpll((llW[0],llW[1]))

hE=self.linterpll((llE[0],llE[1]))

hN=self.linterpll((llN[0],llN[1]))

hS=self.linterpll((llS[0],llS[1]))

return np.array( ( (hE-hW)/rng,(hN-hS)/rng ) )

C.2 navigation.py

import numpy as np

REarth=6371008.770 #Radius of Earth in meters

def mxFromp(dlondegs,latdegs):

’Meters of Longitude from Degrees of Longitude’

return REarth*np.cos(np.radians(latdegs))*np.radians(dlondegs)

def myFroml(dlatdegs):

’Meters of Latitude from Degrees Lattitude’

dlat=np.radians(dlatdegs)

return REarth*dlat

def lFromD(dlatMeters):

’Degrees of Latitude from Meters of Latitude’

162

return np.degrees(dlatMeters/REarth)

def pFromD(Lat,dlonMeters):

’Degrees of Longitude from meters of Longitude’

return np.degrees(dlonMeters/(REarth*np.cos(np.radians(Lat))))

def CFromlp(l,p):

’Course From Change in Latitude and Change in Longitude’

return np.arctan2(p,l)

def lFromDC(D,C):

’Change in latitude due to traveling D meters on \

course C from current position’

return D*np.cos(C)

def pFromDC(D,C):

’Change in longitude due to traveling D meters on \

course C from current position’

return D*np.sin(C)

def gcRng(pt0,pt1):

’Calculate Great Circle Distance Between pt0 and pt1’

L1=np.radians(pt0[0])

L2=np.radians(pt1[0])

DL0=np.radians(pt1[1]-pt0[1])

if DL0>=np.pi:

DL0=2.0*np.pi-DL0

if DL0<= -1.0*np.pi:

DL0=DL0+2*np.pi

InTheFunc=(np.sin(L1)*np.sin(L2)+np.cos(L1)*np.cos(L2)*np.cos(DL0))

try:

D=REarth*np.arccos(InTheFunc)

except RuntimeWarning:

print InTheFunc, np.arccos(InTheFunc)

return D

def gcCrs(pt0,pt1):

’Calculate Initial Great Circle Course from pt0 to \

pt1 Returns Course in Radians’

L1=np.radians(pt0[0])

L2=np.radians(pt1[0])

DLo=np.abs((np.radians(pt1[1]-pt0[1])))

if DLo==0:

if L1>L2:

return np.pi

else:

return 0.0

A=np.pi/2-L2

B=np.pi/2-L1

cosC=np.cos(A)*np.cos(B)+np.sin(A)*np.sin(B)*np.cos(DLo)

C=np.arccos(cosC)

cosa=(np.cos(A)-np.cos(B)*np.cos(C))/(np.sin(B)*np.sin(C))

a=np.arccos(cosa)

if pt1[1]<pt0[1]:

C=2*np.pi-a

else:

C=a

return C

163

def plRngEq(pt0,pt1):

dl=pt1[0]-pt0[0]

dp=pt1[1]-pt0[1]

dl=np.radians(dl)

dp=np.radians(dp)

dl=dl*REarth

dp=dp*REarth

return np.sqrt(dl**2.0+dp**2.0)

def subHeadings(h1,h2):

hDiff=h1-h2

return hDiff

def headingUV(uv):

return np.degrees(np.arctan2(uv[0],uv[1]))

C.3 agents.py

import numpy as np

import navigation as nav

class Agent:

def __init__(self,pos,depth,heading,speed,tgt,rngThresh,energy,\

power,preuv=np.array((0.0,0.0)) ):

self.pos=pos

self.prepos=self.pos

self.heading=heading

self.speed=speed

self.tgt=tgt

self.rngThresh=rngThresh

self.energy=energy

self.power=power

self.posAge=0

self.headingAge=0

self.depth=depth

self.predepth=depth

self.preuv=preuv

self.headingAge=0.0

self.posAge=0.0

def subHeadings(self,h1,h2):

hDiff=h1-h2

return hDiff

def GCheadingTo(self,latlon):

return np.degrees(nav.gcCrs(self.pos,latlon))

def headingTo(self,latlon):

lp=np.array(latlon)-np.array(self.pos)

return np.degrees(np.arctan2(lp[1],lp[0]))

def headingUV(self,uv):

return np.degrees(np.arctan2(uv[0],uv[1]))

def updatePos(self,lcluv,bz,tstep):

if np.any(np.isnan(lcluv)):

164

lcluv=self.preuv

if self.depth*-1 < bz:

self.pos=(np.nan,np.nan)

else:

self.prepos=self.pos

aghdg=np.radians(self.heading)

aguv=np.array( (self.speed*np.sin(aghdg),\

self.speed*np.cos(aghdg)))

lcluv=np.array(lcluv)

uv=lcluv+aguv

dxdy = uv*tstep

self.pos=self.pos+np.array(( nav.lFromD(dxdy[1])\

,nav.pFromD(self.pos[0],dxdy[0])))

self.preuv=lcluv

def updateAll(self,lcluv,dzdll,bz,tstep,hRate):

if np.isnan(lcluv[0]) or np.isnan(lcluv[1]):

lcluv=self.preuv

if self.headingAge >= hRate:

self.updateHeading(lcluv,dzdll,bz)

self.updatePos(lcluv,bz,tstep)

self.headingAge=self.headingAge+tstep

self.energy=self.energy-self.power*tstep

class crabAgent(Agent):

’An agent that uses an angle to compensate for local currents’

def __init__(self,pos,depth,heading,speed,tgt,\

rngThresh,energy,power):

Agent.__init__(self,pos,depth,heading,speed\

,tgt,rngThresh,energy,power)

def updateHeading(self,lcluv,dzdll,bz):

if np.any(np.isnan(lcluv)):

lcluv=self.preuv

lclspd=np.sqrt(lcluv[0]**2+lcluv[1]**2)

dhdg=self.subHeadings(self.headingUV(lcluv), \

self.GCheadingTo(self.tgt))

xwind=-1*lclspd*np.sin(np.radians(dhdg))

offset=0.0

if np.abs( xwind ) < self.speed:

offset=np.degrees( np.arcsin( xwind/(self.speed ) ))

self.heading = self.GCheadingTo(self.tgt) + offset

self.headingAge=0.0

class pigeonAgent(Agent):

’An agent that homes in on its target’

def __init__(self,pos,depth,heading,speed,\

tgt,rngThresh,energy,power):

Agent.__init__(self,pos,depth,heading,\

speed,tgt,rngThresh,energy,power)

def updateHeading(self,lcluv,dzdll,bz):

if np.any(np.isnan(lcluv)):

lcluv=self.preuv

self.heading=self.GCheadingTo(self.tgt)

self.headingAge=0.0

165

class grouperAgent(Agent):

’An agent that chooses its heading based \

on preferences B,C, and D’

def __init__(self,A,B,C,D,pos,depth,heading,\

speed,tgt,rngThresh,energy,power):

Agent.__init__(self,pos,depth,heading,\

speed,tgt,rngThresh,energy,power)

ABCDsum= np.abs(A)+np.abs(B)+np.abs(C)+np.abs(D)

self.A=A / ABCDsum # Collision Coefficient

self.B=B / ABCDsum # Local Current Coefficient

self.C=C / ABCDsum # Agent Heading Coefficient

self.D=D / ABCDsum # Target Coefficient

def updateHeading(self,lcluv,dzdll,bz):

if np.any(np.isnan(lcluv)):

lcluv=self.preuv

’Update Grouper\’s heading’

spdwt=( 1 - self.speed/(self.speed +\

np.sqrt(lcluv[0]**2+lcluv[1]**2)) )

lclHdg=np.radians(self.headingUV(lcluv))

agentHdg=np.radians(self.heading)

tgtHdg=np.radians(self.GCheadingTo(self.tgt))

lclxy=np.array( (self.B*np.sin(lclHdg), \

self.B*np.cos(lclHdg)))

lclxy=lclxy*spdwt

agentxy=np.array( (self.C*np.sin(agentHdg), \

self.C*np.cos(agentHdg)) )

tgtxy = np.array((self.D*np.sin(tgtHdg),\

self.D*np.cos(tgtHdg)))

total=lclxy+tgtxy+agentxy

self.heading=self.headingUV((total[0],total[1]))

self.headingAge=0.0

class dorAgent(Agent):

’An agent that chooses its heading based\

on preferences A,B,C, and D’

def __init__(self,A,B,C,D,pos,depth,heading,\

speed,tgt,rngThresh,energy,power,M,T):

Agent.__init__(self,pos,depth,heading,\

speed,tgt,rngThresh,energy,power)

ABCDsum= np.abs(A)+np.abs(B)+np.abs(C)+np.abs(D)

self.A=A / ABCDsum # Collision Coefficient

self.B=B / ABCDsum # Local Current Coefficient

self.C=C / ABCDsum # Agent Heading Coefficient

self.D=D / ABCDsum # Target Coefficient

self.T=T #Temporal Threshold for Collision avoidance in seconds

self.M=M #Slope

def updateHeading(self,lcluv,dzdll,bz):

’Update Grouper\’s heading’

if np.any(np.isnan(lcluv)):

lcluv=self.preuv

lclmag=np.sqrt(lcluv[0]**2+lcluv[1]**2)

spdwt=(lclmag/(self.speed + lclmag) )

lclHdg=np.radians(self.headingUV(lcluv))

166

agentHdg=np.radians(self.heading)

tgtHdg=np.radians(self.GCheadingTo(self.tgt))

lclxy=np.array( (self.B*np.sin(lclHdg), \

self.B*np.cos(lclHdg)))

lclxy=lclxy*spdwt

agentxy=np.array( (self.C*np.sin(agentHdg), \

self.C*np.cos(agentHdg)) )

gradmag=dzdll[0]*np.sin(agentHdg)+dzdll[1]*np.cos(agentHdg)

t_c=(-1*self.depth-bz)/gradmag/self.speed #Time to Collision

gradwt0=1.0/(1.0+np.exp(self.M*t_c-self.T*self.M))

gradwt1=-1.0/(1+np.exp(1.0*t_c))+1.0

gradwt=gradwt0*gradwt1*self.A

gradxy=(dzdll)/(np.sqrt(dzdll[0]**2+dzdll[1]**2))

gradxy=gradxy*gradwt

tgtxy = np.array((self.D*np.sin(tgtHdg), \

self.D*np.cos(tgtHdg)))

total=gradxy+lclxy+agentxy+tgtxy

self.heading=self.headingUV((total[0],total[1]))

self.headingAge=0.0

C.4 asearch.py

import numpy as np

import environment as env

from navigation import *

# Class to record nodes in a planning graph

class pathNode:

def __init__(self,depth,lat,lon,heading=0,cost=0.0,parent=None):

self.root=False #True if node is the root.

#Currently unused

if parent==None:

self.root=True

self.time=None #This node’s time. Currently unused

self.cost=cost #The cost of reaching this node

self.heur=0.0 #This node’s heuristic value.

#Currently unused

self.depth=depth

self.lat=lat

self.lon=lon

self.heading=heading

self.parent=parent #Pointer to node’s parent.

#None if node is root

self.exp=False #True if this node has been expanded

self.childs=[] #Pointers to this node’s children

# Expands a node uniformly in space within the

# specified angle of heading and

# the given branching factor. All other arguments are

167

# used to calculate costs of reaching new nodes

def uExpSpace(self,depth,rng,spd,VF,tstep,tree,nodes=3,angle=90.0,\

descDepth=0.0,profiling=False,descAngle=0.0,HF=None):

’Expand Uniformly in space by adding <nodes> children \

distributed every 360/<nodes> degrees at <depth> meters\

from the surface and <range> horizontal meters from \

current location’

self.exp=True

for i in xrange(nodes):

course=self.heading-angle/2+i*angle/(nodes-1)

newlat=self.lat+lFromD(lFromDC(rng,np.radians(course)))

newlon=self.lon+pFromD(self.lat,pFromDC(rng,\

np.radians(course)))

newdepth=self.depth

addit=True

for j in xrange(len(tree)):

if(gcRng([newlat,newlon],[tree[j].lat,\

tree[j].lon])<0.25*rng):

if(self.cost+costT([[self.lat,self.lon,self.depth],\

[tree[j].lat,tree[j].lon,\

self.depth],spd,VF,tstep,\

self.cost,descDepth,\

profiling,descAngle,\

HF])>tree[j].cost):

addit=False

if addit==True:

newc=self.cost + costT([[self.lat,self.lon,self.depth],\

[newlat,newlon,newdepth],spd,VF,\

tstep,self.cost,descDepth,\

profiling,descAngle,HF])

if np.isnan(newc)==False:

self.childs.append(pathNode(newdepth,newlat,\

newlon,course,\

newc,self))

# Expand a node by adding a single node on a direct course

#between self and objective

def dExp(self,rng,spd,VF,tstep,target,descDepth,profiling,\

descAngle,HF):

tgtLat=target[0]

tgtLon=target[1]

tgtDep=target[2]

course=np.degrees(CFromlp(tgtLat-self.lat,tgtLon-self.lon))

newlat=self.lat+lFromD(lFromDC(rng,np.radians(course)))

newlon=self.lon+pFromD(self.lat,pFromDC(rng,np.radians(course)))

newc=self.cost + costT([[self.lat,self.lon,self.depth],\

[newlat,newlon,tgtDep],spd,VF,\

tstep,self.cost,descDepth,\

profiling,descAngle,HF])

if np.isnan(newc)==False:

self.childs.append(pathNode(tgtDep,newlat,newlon\

,course,newc,self))

168

# Calculate the cost of traveling from one node to another

def costT(args):

start=args[0] # Starting lat,lon,depth

end=args[1] # Ending lat,lon,depth

auvspd=args[2] # Vehicle Speed in m/s

VF=args[3] # Velocity Field

tstep=args[4] # time step in seconds

t0=args[5] # starting time in hours

descDepth=args[6] # Operating depth

profiling=args[7] # Boolean to control profiling

descAngle=args[8] # Descent Angle When Profiling

HF=args[9]

thresh=(auvspd+VF.maxmag)*tstep*np.sqrt(2) # Range from objective

# to consider objective reached

lcluvpre=np.array((0.0,0.0))

timeout=72.0*3.0 # Arbitrary timeout in hours

# to prevent getting stuck

# in infinite loop

stlat=np.float(start[0])

stlon=np.float(start[1])

# stdep=np.float(start[2])

enlat=np.float(end[0])

enlon=np.float(end[1])

#endep=np.float(end[2])

descent=True

zspd=auvspd*np.tan(np.radians(descAngle))

if profiling==True:

nowdep=0.0

else:

nowdep=descDepth

nowlat=stlat

nowlon=stlon

cost=0.0

prevrng=gcRng([stlat,stlon],[enlat,enlon])

RNG=prevrng

# Vehicle homes from start to objective in the current

# field, with model running at a temporal resolution of tstep

while(RNG > thresh):

prevrng=np.float(RNG)

auvC=CFromlp(enlat-nowlat,enlon-nowlon)

lcluv=VF.linterpTDLL(t0+cost,nowdep,nowlat,nowlon)

if np.any(np.isnan(np.array(lcluv))):

lcluv=lcluvpre

else:

lcluvpre=lcluv

###################################################

lclspd=np.sqrt(lcluv[0]**2+lcluv[1]**2)

dhdg=subHeadings(headingUV(lcluv),np.degrees(gcCrs((nowlat,\

nowlon),(enlat,enlon))) )

xwind= -1*lclspd*np.sin(np.radians(dhdg))

offset=0.0

if np.abs( xwind ) < auvspd:

169

offset=np.degrees( np.arcsin( xwind/(auvspd) ))

heading = np.degrees(gcCrs((nowlat,nowlon),\

(enlat,enlon))) + offset

auvC=np.radians(heading)

###########################

auvuv=[auvspd*np.sin(auvC),auvspd*np.cos(auvC)]

nowlon=nowlon+pFromD(nowlat,(auvuv[0]+lcluv[0])*tstep)

nowlat=nowlat+lFromD((auvuv[1]+lcluv[1])*tstep)

if profiling==True:

if nowdep==0:

descent=True

if nowdep==descDepth:

descent=False

if descent==True:

nowdep=np.min((nowdep+zspd*tstep,descDepth))

else:

nowdep=np.max((nowdep-zspd*tstep,0))

else:

nowdep=descDepth

cost=cost+tstep/3600.0

if np.isnan(lcluv[0]) or np.isnan(lcluv[1]) or np.any(\

np.isnan(np.array((nowlat,nowlon)))):

return np.nan

elif cost>timeout:

return np.nan

if HF!=None:

if HF.linterpll((nowlat,nowlon))>-1*nowdep:

return np.nan

RNG=gcRng([nowlat,nowlon],[enlat,enlon])

# Correct for the last bit of distance remaining when loop terminates

pmr=prevrng-RNG

if pmr!=0:

cost=cost+RNG/((pmr)/tstep)/3600.0

return cost

# Search for a direct route between st and en

def DirectSearch(st,en,legDist,t0,spd,tstep,VF,descDepth=0.0,\

HF=None,Q=None,profiling=False,descAngle=0.0):

Root=pathNode(st[2],st[0],st[1],np.degrees(\

CFromlp(en[0]-st[0],en[1]-st[1])))

Graph=[Root]

RNG=gcRng([Root.lat,Root.lon],[en[0],en[1]])

thresh=200

if RNG>thresh:

loopcon=True

else:

loopcon=False

while(loopcon):

Graph[len(Graph)-1].dExp(np.nanmin(np.array([legDist,RNG]))\

,spd,VF,tstep,en,descDepth,profiling,descAngle)

Graph=Graph+Graph[len(Graph)-1].childs

170

tgt=Graph[len(Graph)-1]

RNG=gcRng([tgt.lat,tgt.lon],[en[0],en[1]])

if RNG < thresh:

loopcon=False

tgt=Graph[len(Graph)-1]

tcost=tgt.cost

log=list()

while tgt.parent!=None:

log.append([tgt.lat,tgt.lon,tgt.depth])

tgt=tgt.parent

log.append([tgt.lat,tgt.lon,tgt.depth])

log.reverse()

log=np.array(log)

return [log,tcost]

# Dynamically build a tree and search it for a minimum time route

# between st and en

def AstarSearchTime(st,en,legDist,t0,spd,tstep,VF,descDepth=0.0,\

HF=None,timeout=None,Q=None,branchFactor=3,branchAngle=90.0\

,profiling=False,descAngle=0.0):

# Create the root of the tree

Root=pathNode(st[2],st[0],st[1],np.degrees(CFromlp(\

en[0]-st[0],en[1]-st[1])))

# Use a list to hold all of the nodes that are created

Graph=[Root]

# Use another list to hold the nodes that have not yet been visited

Candidates=[Root]

RNG=gcRng([Root.lat,Root.lon],[en[0],en[1]])

log=list()

thresh=200 # Arbitrary threshold of 200m from objective

# to stop planning

if RNG>thresh:

loopcon=True

else:

loopcon=False

while(loopcon):

# Create a list and store the cost+heuristic values of each

# of the candidate nodes

costs=list()

for i in Candidates:

costs.append(i.cost+gcRng([i.lat,i.lon],\

[en[0],en[1]])/(spd+VF.maxmag)/3600.0 )

try:

if np.nanmin(costs)*3600.0>=timeout:

print ’Timeout Exceeded’

return [None,None]

exploc=costs.index(np.nanmin(costs))

except (ValueError):

print ’No route found.’

return [None,None]

# Choose to expand the node with the lowest cost+heuristic value

tgt=Candidates[exploc]

171

RNG=gcRng([tgt.lat,tgt.lon],[en[0],en[1]])

# Add the current node to the interprocess

# queue to update the GUI

if Q!=None:

Q.put([’<currentNode>’,tgt])

if RNG > thresh:

loopcon=True

Candidates[exploc].uExpSpace(tgt.depth,\

np.nanmin(np.array([legDist,RNG])),\

spd,VF,tstep,Candidates,branchFactor,\

branchAngle,descDepth,profiling,descAngle,HF)

# When the search is close to the end, always

# consider a direct route to the objective

if RNG<legDist:

Candidates[exploc].dExp(np.nanmin(\

np.array([legDist,RNG])),spd,VF,tstep,\

en,descDepth,profiling,descAngle,HF)

# Add the children of the visited node to the Candidates

# and Graph

# lists, and remove the visited node and its cost from the

# candidate list

Candidates=Candidates+Candidates[exploc].childs

Graph=Graph+Candidates[exploc].childs

Candidates.pop(exploc)

costs.pop(exploc)

else:

loopcon=False

tcost=tgt.cost

if timeout!=None and tcost*3600.0>timeout:

print ’Timeout Exceeded’

return [None, None]

#log.append([en[0],en[1],en[2]])

while tgt.parent!=None:

log.append([tgt.lat,tgt.lon,tgt.depth])

tgt=tgt.parent

log.append([tgt.lat,tgt.lon,tgt.depth])

#log.append([st[0],st[1],st[2]])

log.reverse()

log=np.array(log)

if Q!=None:

Q.put([’<intRoute>’,log])

np.save(’plotlog.npy’,log)

return [log,tcost]

# Export routes as kml for display in Google Earth

def Route2KML(routes,filename):

xmlout=open(filename,’w’)

xmlout.write(’<?xml version="1.0" standalone="yes"?>\n’)

xmlout.write(’<kml xmlns="http://earth.google.com/kml/2.2">\n’)

xmlout.write(’ <Document>\n’)

xmlout.write(’ <Folder id="Tracks">\n’)

for j in xrange(len(routes)):

xmlout.write(’ <Placemark>\n’)

172

xmlout.write(’ <MultiGeometry>\n’)

xmlout.write(’ <LineString>\n’)

xmlout.write(’ <altitudeMode>clampToGround</altitudeMode>\n’)

xmlout.write(’ <coordinates>\n’)

for i in xrange(0,len(routes[j])):

outstring=str(routes[j][i,1])+’,’+str(routes[j][i,0])+’,’+\

str(0)+’\n’

xmlout.write(outstring)

xmlout.write(’ </coordinates>\n’)

xmlout.write(’ <tessellate>1</tessellate>\n’)

xmlout.write(’ </LineString>\n’)

xmlout.write(’ </MultiGeometry>\n’)

xmlout.write(’ <Snippet></Snippet>\n’)

xmlout.write(’ <Style>\n’)

xmlout.write(’ <LineStyle>\n’)

if j==0:

xmlout.write(’ <color>FF00FF00</color>\n’)

else:

xmlout.write(’ <color>FF00FF00</color>\n’)

xmlout.write(’ <width>3</width>\n’)

xmlout.write(’ </LineStyle>\n’)

xmlout.write(’ </Style>\n’)

xmlout.write(’ </Placemark>\n’)

xmlout.write(’ <name>Tracks</name>\n’)

xmlout.write(’ <open>0</open>\n’)

xmlout.write(’ <visibility>1</visibility>\n’)

xmlout.write(’ </Folder>\n’)

xmlout.write(\

’<Snippet><![CDATA[Path planned by APPv0.2]]></Snippet>\n’)

xmlout.write(’ <name><![CDATA[Planned Path]]></name>\n’)

xmlout.write(’ <open>1</open>\n’)

xmlout.write(’ <visibility>1</visibility>\n’)

xmlout.write(’ </Document>\n’)

xmlout.write(’</kml>’)

xmlout.close()

C.5 nc2npz.py

import sys

import environment as e

import numpy as np

# Read the NetCDF file passed as the command line argument and save it

# as a numpy zip file

VF0=e.VField(sys.argv[1])

np.savez(sys.argv[2],u=VF0.u,v=VF0.v,lats=VF0.lats,\

lons=VF0.lons,depths=VF0.depths,times=VF0.times)

173

C.6 toponc2npz

import scipy.io.netcdf as ncio

import numpy as np

# Read topographic data in a NetCDF file from the

# following line and save it as a numpy zip file

ncfname=’/home/aj/Dropbox/EclipseWorkspace/GrouperNav/etopo1_gulf0.nc’

ncfile=ncio.netcdf_file(ncfname,’r’)

z=np.array(ncfile.variables.get(’Band1’).data,np.float)

z=np.flipud(z)

print z.shape

lats=np.zeros(181)

lons=np.zeros(301)

for i in xrange(301):

lon=-88.5+1.0/60.0*i

lons[i]=-88.5+1.0/60.0*i

for i in xrange(181):

lat=23.1+1.0/60.0*i

lats[i]=lat

np.savez(’etopo1_gulf0.npz’,x=lons,y=lats,z=z)

C.7 ncinfo.py

import sys

sys.path.append(’./’)

sys.path.append(’./resources’)

import scipy as sp

import numpy as np

import scipy.io.netcdf as ncio

if len(sys.argv)!=2:

print "Usage: python ncinfo.py FILENAME.nc"

else:

ncname=sys.argv[1]

ncfile=ncio.netcdf_file(ncname, ’r’)

ncvars=ncfile.variables.items()

print ’dimensions: ’+str(ncfile.dimensions)

print ncname+’ Contains The Following Variables:’

print ’Name\t|\tEntries\t|\tMin\t|\tMax\t#|\tUnits’

print ’------------------------------------------------------’

for i in xrange(0,len(ncvars)):

varentry=list(ncfile.variables.get(ncvars[i][0]))

print ncvars[i][0]+’\t|\t’+str(sp.size(varentry))+\

’\t|\t’+str(sp.nanmin(varentry))+’\t|\t’+str(sp.nanmax(varentry))

174

C.8 GenWaypoints.py

import random

import numpy as np

import environment as env

import navigation as nav

WPFileName=’EWCarib150.csv’

VFileName=’Carib2012_05_01.npz’

HFileName=’ETOPO2.npz’

VF=env.VField(VFileName)

HF=env.HField(HFileName)

depth=2.0

trial=0

trials=100

fll=np.array( (14.5,-63.0,16.5,-59.5) )

Range=150000.0 # meters

Doff=np.degrees(Range/(nav.REarth*np.cos(\

np.radians((fll[0]+fll[2])/2.0))))

while(trial<trials):

valid=False

while valid==False:

temp0=(random.uniform(0.0,1.0)*(\

fll[2]-fll[0]) + fll[0],(fll[1]+\

fll[3])/2.0-Doff/2.0 ) #lat lon

temp1=(temp0[0],temp0[1]+Doff) # lat lon

if HF.linterpll(temp0)<(-1.0*depth) \

and HF.linterpll(temp1)<(-1.0*depth)\

and np.isnan(VF.linterpTDLL(0,depth,\

temp0[0],temp0[1])[0])==False\

and np.isnan(VF.linterpTDLL(0,depth\

,temp1[0],temp1[1])[0])==False:

valid=True

print "Depth at temp0: ",HF.linterpll(temp0),\

" Depth at temp1: ",HF.linterpll(temp1)," LCLUV Temp0: ",\

VF.linterpTDLL(0,depth,temp0[0],temp0[1])," LCLUV Temp1: ",\

VF.linterpTDLL(0,depth,temp1[0],temp1[1])

if random.uniform(0.0,1.0) >= 0.5:

spawn=temp0

target=temp1

else:

spawn=temp1

target=temp0

trial+=1

print trial

WPFile=open(WPFileName,’a’)

WPFile.write(str(spawn[0])+’,’+str(spawn[1])+’,’+str(target[0])+\

’,’+str(target[1])+’\n’)

WPFile.close()

175

C.9 GenWaypointsGulf.py

import random

import numpy as np

import environment as env

import navigation as nav

WPFileName=’Gulf100.csv’

VFileName=’SEGulf2012_05_01.npz’

HFileName=’etopo1_gulf0.npz’

VF=env.VField(VFileName)

HF=env.HField(HFileName)

depth=2.0

trial=0

trials=100

fll=np.array( (23.1,-88.5,26.1,-83.5 ))

Range=150000.0 # meters

Doff=np.degrees(Range/(nav.REarth*np.cos(np.radians(fll[2]))))

while(trial<trials):

valid=False

bearing=random.uniform(0.0,180.0)

DegLons=Doff*np.sin(np.radians(bearing))

DegLats=Doff*np.cos(np.radians(bearing))

while valid==False:

temp0=(random.uniform(23.1,26.1-DegLats),\

random.uniform(-88.5,-83.5-DegLons) )

DCrs=np.degrees(Range/(nav.REarth*np.cos(\

np.radians(temp0[0]))))

DegLons=DCrs*np.sin(np.radians(bearing))

DegLats=DCrs*np.cos(np.radians(bearing))

temp1=(temp0[0]+DegLats,temp0[1]+DegLons)

if HF.linterpll(temp0)<(-1.0*depth) \

and HF.linterpll(temp1)<(-1.0*depth)\

and np.isnan(VF.linterpTDLL(0,depth,\

temp0[0],temp0[1])[0])==False\

and np.isnan(VF.linterpTDLL(0,\

depth,temp1[0],temp1[1])[0])==False:

valid=True

print "Depth at temp0: ",HF.linterpll(temp0),\

" Depth at temp1: ",HF.linterpll(temp1)," LCLUV Temp0: ",\

VF.linterpTDLL(0,depth,temp0[0],temp0[1])," LCLUV Temp1: ",\

VF.linterpTDLL(0,depth,temp1[0],temp1[1])

if random.uniform(0.0,1.0) >= 0.5:

spawn=temp0

target=temp1

else:

spawn=temp1

target=temp0

trial+=1

print trial

WPFile=open(WPFileName,’a’)

WPFile.write(str(spawn[0])+’,’+str(spawn[1])+’,’+str(target[0])+’,’+\

176

str(target[1])+’\n’)

WPFile.close()

C.10 GrouperBehavior-P3d.py

import direct.showbase.ShowBase as psb

import direct.task as pt

from direct.gui.DirectGui import *

from panda3d.core import *

import random

import time as t

import numpy as np

import pylab as pl

import pygame

import agents

import environment as env

import navigation as nav

spawn=(-45.5,-0.5) # lat lon

target=(45.5,45.5) # lat lon

uv=(0.0000001,0.000001)

spd=130

hdg=np.degrees(nav.gcCrs(spawn,target))

energy=24*7.0*3600.0

power=1.0

timestep=60.0 # seconds

thresh=(spd+np.sqrt(uv[0]**2+uv[1]**2))*timestep # meters

missiontime=0.0

timeout=24*7.0*3600

Params=(0.0,-1.0,0.0,0.5)

tagent=agents.Agent()

tagent.pos=spawn

gcr=nav.gcRng(spawn,target)

meanlat=(spawn[0]+target[0])/2.0

meanlon=(spawn[1]+target[1])/2.0

gcr=gcr/nav.REarth*180.0/np.pi

fll=np.array((meanlat-gcr/2*1.5,meanlon-gcr/2*1.5,\

meanlat+gcr/2*1.5,meanlon+gcr/2*1.5))

fxy=np.array((-50,-50,50,50))

def lalo2xy(flalo,fxy,lalo):

x = (lalo[1] - flalo[1]) / (flalo[3]-flalo[1]) *\

(fxy[2]-fxy[0])+fxy[0]

y = (lalo[0] - flalo[0]) / (flalo[2]-flalo[0]) *\

(fxy[3]-fxy[1])+fxy[1]

return (x,y)

class Model(psb.ShowBase):

def __init__(self):

global k

psb.ShowBase.__init__(self)

177

render.setAntialias(AntialiasAttrib.MAuto)

self.setBackgroundColor(VBase3(1.0,1.0,1.0))

light=DirectionalLight(’Sun’)

light.setColor(VBase4(1.0,1.0,1.0,1.0))

lightNode=self.render.attachNewNode(light)

lightNode.setHpr(45.0,-45.0,0.0)

render.setLight(lightNode)

self.bestg=np.nan

self.bestp=np.nan

self.bestc=np.nan

self.startMark=self.loader.loadModel(’/home/aj/markerGreen.x’)

self.startMark.reparentTo(self.render)

txy=lalo2xy(fll,fxy,(spawn[0],spawn[1]))

self.startMark.setPos(txy[0],txy[1],0.0)

self.endMark=self.loader.loadModel(’/home/aj/markerRed.x’)

self.endMark.reparentTo(self.render)

self.bestgLabel=DirectLabel(text=’G: ’+\

str(self.bestg/3600),text_bg=(0.0,0.0,0.0,0.0),\

text_fg=(0.0,0.0,1.0,1.0),frameColor=(0.0,0.0,0.0,0.0)\

,text_scale=(0.08,0.08),text_pos=(-0.5,0.8))

self.bestpLabel=DirectLabel(text=’P: ’+str(self.bestp/3600),\

text_bg=(0.0,0.0,0.0,0.0),text_fg=(0.7,0.7,0.0,1.0),\

frameColor=(0.0,0.0,0.0,0.0),text_scale=(0.08,0.08),\

text_pos=(0.0,0.8))

self.bestcLabel=DirectLabel(text=’C: ’+str(self.bestc/3600),\

text_bg=(0.0,0.0,0.0,0.0),text_fg=(1.0,0.0,0.0,1.0),\

frameColor=(0.0,0.0,0.0,0.0),text_scale=(0.08,0.08),\

text_pos=(0.5,0.8))

txy=lalo2xy(fll,fxy,(target[0],target[1]))

self.endMark.setPos(txy[0],txy[1],0.0)

self.alist=list()

self.alist.append([agents.pigeonAgent(spawn,hdg,spd,target,\

thresh,energy,power),self.loader.loadModel(\

’/home/aj/agentYellow.x’)])

self.alist.append([agents.crabAgent(spawn,hdg,spd,\

target,thresh,energy,power),self.loader.loadModel(\

’/home/aj/agentRed.x’)])

self.alist.append([agents.grouperAgent(0,Params[1],\

Params[2],Params[3],spawn,hdg,spd,\

target,thresh,energy,power),\

self.loader.loadModel(’/home/aj/agentBlue.x’)])

self.taskMgr.add(self.moveAgents,"moveAgents")

self.missiontime=0

for i in self.alist:

i[1].reparentTo(self.render)

xy=lalo2xy(fll,fxy,i[0].pos)

i[1].setPos(xy[0],xy[1],0.0)

def moveAgents(self,task):

self.camera.setPos(0,0,200)

self.camera.setP(-90.0)

global bestg, bestParams, bestp, bestc,epoch,epochs,stdev

178

if self.missiontime>timeout or len(self.alist)==0:

self.alist.append([agents.pigeonAgent(spawn,hdg,spd,target,\

thresh,energy,power),self.loader.loadModel(\

’/home/aj/agentYellow.x’)])

self.alist.append([agents.crabAgent(spawn,hdg,spd,target,\

thresh,energy,power),self.loader.loadModel(\

’/home/aj/agentRed.x’)])

self.alist.append([agents.grouperAgent(0,Params[1],\

Params[2],Params[3],spawn,hdg,spd,target,thresh,\

energy,power),self.loader.loadModel(\

’/home/aj/agentBlue.x’)])

for i in self.alist:

i[1].reparentTo(self.render)

xy=lalo2xy(fll,fxy,i[0].pos)

i[1].setPos(xy[0],xy[1],0.0)

self.missiontime=0.0

else:

self.missiontime=self.missiontime+timestep

kills=list()

for i in range(len(self.alist)):

killi=False

self.alist[i][0].updateAll(uv,timestep,0)

if self.alist[i].__class__==agents.pigeonAgent or \

self.alist[i].__class__==agents.crabAgent:

print self.alist[i][0].heading,self.alist[i][0].pos

if (np.isnan(self.alist[i][0].pos[0]) or \

self.alist[i][0].energy <= 0.0 or ( ((fll[0]<\

self.alist[i][0].pos[0]<fll[2]) and \

(fll[1]<self.alist[i][0].pos[1]<fll[3]))!=True )):

killi=True

elif nav.gcRng(self.alist[i][0].pos,\

self.alist[i][0].tgt)<thresh:

drdt=(nav.gcRng(self.alist[i][0].prepos,\

self.alist[i][0].tgt)-nav.gcRng(\

self.alist[i][0].pos,self.alist[i][0].tgt))\

/timestep

dt=nav.gcRng(self.alist[i][0].pos,\

self.alist[i][0].tgt)/drdt

if self.alist[i][0].__class__==agents.crabAgent and\

((self.missiontime+dt < self.bestc) or \

(np.isnan(self.bestc))):

self.bestc = self.missiontime+dt

self.bestcLabel[’text’]=’C: ’+str(np.round(\

self.bestc/3600,2))

if self.alist[i][0].__class__==agents.pigeonAgent \

and ( (self.missiontime +dt < self.bestp ) or \

(np.isnan(self.bestp)) ):

self.bestp = self.missiontime+dt

self.bestpLabel[’text’]=’P: ’+str(np.round(\

self.bestp/3600,2))

179

if self.alist[i][0].__class__==agents.grouperAgent \

and ( (self.missiontime+dt < self.bestg ) or\

(np.isnan(self.bestg)) ):

self.bestg = self.missiontime+dt

self.bestgLabel[’text’]=’G: ’+str(\

np.round(self.bestg/3600,2))

killi=True

if killi==False:

xy=lalo2xy(fll,fxy,self.alist[i][0].pos)

self.alist[i][1].setH(-1*self.alist[i][0].heading)

self.alist[i][1].setPos(xy[0],xy[1],0.0)

else:

self.alist[i][1].removeNode()

kills.append(self.alist[i])

for i in range(len(kills)):

self.alist.remove(kills[i])

return pt.Task.cont

theProgram = Model()

theProgram.run()

C.11 GrouperView-P3d

import direct.showbase.ShowBase as psb

import direct.task as pt

from direct.gui.DirectGui import *

from panda3d.core import *

import random

import time as t

import numpy as np

import pylab as pl

import pygame

import agents

import environment as env

import navigation as nav

import xml.etree.ElementTree as et

depth=25.0

logfile=’Results/Carib_300cms/TrialLog_21Nov2012_2115.csv’

x=np.genfromtxt(logfile,delimiter=’,’)

tNumber=0

trial=x[:,0]

timeReq=x[:,1]

spnLat=x[:,2]

spnLon=x[:,3]

tgtLat=x[:,4]

tgtLon=x[:,5]

gcRng=x[:,6]

bestp=x[:,7]

bestc=x[:,8]

180

bestg=x[:,9]

bestEpoch=x[:,10]

bestA=x[:11]

bestB=x[:,12]

bestC=x[:,13]

bestD=x[:,14]

groupers=x[:,15]

epochs=x[:,16]

startStdev=x[:,17]

pconv=x[:,18]

cconv=x[:,19]

gconv=x[:,20]

testcfg=et.parse(logfile.split(’.’)[0]+’.xml’)

spd=np.float(testcfg.find(’speed’).text)

energy=np.float(testcfg.find(’energy’).text)

power=np.float(testcfg.find(’power’).text)

timestep=np.float(testcfg.find(’timeStep’).text) # seconds

thresh=np.float(testcfg.find(’threshold’).text) # meters

timeout=np.float(testcfg.find(’timeout’).text)

VF=env.VField(’Carib2012_05_01.npz’)

HF=env.HField(’ETOPO2.npz’)

k=0

for j in trial:

if j==np.float(tNumber):

k=int(j)

spawn=(spnLat[k],spnLon[k]) # lat lon

target=(tgtLat[k],tgtLon[k]) # lat lon

hdg=np.degrees(nav.gcCrs(spawn,target))

bestParams=(x[k,11],x[k,12],x[k,13],x[k,14])

meanlat=(spawn[0]+target[0])/2.0

meanlon=(spawn[1]+target[1])/2.0

fll=np.array((23.1,-88.5,26.1,-83.5))

fxy=np.array((-75,-75,75,75))

def lalo2xy(flalo,fxy,lalo):

x = (lalo[1] - flalo[1]) / (flalo[3]-flalo[1]) * \

(fxy[2]-fxy[0])+fxy[0]

y = (lalo[0] - flalo[0]) / (flalo[2]-flalo[0]) * \

(fxy[3]-fxy[1])+fxy[1]

return (x,y)

def settNumber(newtNumber):

global tNumber

tNumber=newtNumber

class Model(psb.ShowBase):

def __init__(self):

global k

psb.ShowBase.__init__(self)

render.setAntialias(AntialiasAttrib.MAuto)

self.setBackgroundColor(VBase3(1.0,1.0,1.0))

light=DirectionalLight(’Sun’)

light.setColor(VBase4(1.0,1.0,1.0,1.0))

lightNode=self.render.attachNewNode(light)

lightNode.setHpr(45.0,-45.0,0.0)

181

render.setLight(lightNode)

self.bc=np.nan

self.bg=np.nan

self.bp=np.nan

self.startMark=self.loader.loadModel(’/home/aj/markerGreen.x’)

self.startMark.reparentTo(self.render)

txy=lalo2xy(fll,fxy,(spawn[0],spawn[1]))

self.startMark.setPos(txy[0],txy[1],0.0)

self.endMark=self.loader.loadModel(’/home/aj/markerRed.x’)

self.endMark.reparentTo(self.render)

self.Tentry=DirectEntry(text=’’,command=settNumber,\

initialText=str(tNumber),numLines=1,text_bg=\

(0.0,0.0,0.0,0.0),scale=0.08,width=4,frameColor=\

(0.0,0.0,0.0,0.0),text_pos=(-1.0,0.7))

self.bestgLabel=DirectLabel(text=’G: ’+str(np.round(\

0/3600,2)),text_bg=(0.0,0.0,0.0,0.0),text_fg=\

(0.0,0.0,1.0,1.0),frameColor=(0.0,0.0,0.0,0.0)\

,text_scale=(0.08,0.08),text_pos=(-0.5,0.8))

self.bestpLabel=DirectLabel(text=’P: ’+str(np.round(\

0/3600,2)),text_bg=(0.0,0.0,0.0,0.0),text_fg=\

(0.7,0.7,0.0,1.0),frameColor=(0.0,0.0,0.0,0.0)\

,text_scale=(0.08,0.08),text_pos=(0.0,0.8))

self.bestcLabel=DirectLabel(text=’C: ’+str(np.round(\

0/3600,2)),text_bg=(0.0,0.0,0.0,0.0),text_fg=\

(1.0,0.0,0.0,1.0),frameColor=(0.0,0.0,0.0,0.0),\

text_scale=(0.08,0.08),text_pos=(0.5,0.8))

self.trialLabel=DirectLabel(text=’Trial: ’+str(tNumber)\

,text_bg=(0.0,0.0,0.0,0.0),text_fg=(0.0,0.0,0.0,1.0)\

,frameColor=(0.0,0.0,0.0,0.0),text_scale=(0.08,0.08)\

,text_pos=(-1.0,0.8))

self.rangeLabel=DirectLabel(text=’Range: ’,text_bg=\

(0.0,0.0,1.0,0.0),text_fg=(0.0,0.0,0.0,1.0),frameColor=\

(0.0,0.0,0.0,0.0),text_scale=(0.08,0.08),\

text_pos=(-0.2,-0.8))

self.rngvalLabel=DirectLabel(text=str(nav.gcRng(\

spawn,target)),text_bg=(0.0,0.0,1.0,0.0),text_fg=\

(0.0,0.0,0.0,1.0),frameColor=(0.0,0.0,0.0,0.0),\

text_scale=(0.08,0.08),text_pos=(0.2,-0.8))

self.mtimeLabel=DirectLabel(text=’MTime: ’,text_bg=\

(0.0,0.0,1.0,0.0),text_fg=(0.0,0.0,0.0,1.0),frameColor=\

(0.0,0.0,0.0,0.0),text_scale=(0.08,0.08),\

text_pos=(-0.2,-0.7))

self.mtvalLabel=DirectLabel(text=str(0.0),text_bg=\

(0.0,0.0,1.0,0.0),text_fg=(0.0,0.0,0.0,1.0),\

frameColor=(0.0,0.0,0.0,0.0),text_scale=(0.08,0.08)\

,text_pos=(0.2,-0.7))

txy=lalo2xy(fll,fxy,(target[0],target[1]))

self.endMark.setPos(txy[0],txy[1],0.0)

self.alist=list()

self.alist.append([agents.pigeonAgent(spawn,depth,hdg,\

spd,target,thresh,energy,power),self.loader.loadModel(\

’/home/aj/agentYellow.x’)])

182

self.alist.append([agents.crabAgent(spawn,depth,hdg,spd,\

target,thresh,energy,power),self.loader.loadModel(\

’/home/aj/agentRed.x’)])

self.alist.append([agents.grouperAgent(bestParams[0],\

bestParams[1],bestParams[2],bestParams[3],spawn,\

depth,hdg,spd,target,thresh,energy,power),\

self.loader.loadModel(’/home/aj/agentBlue.x’)])

self.taskMgr.add(self.moveAgents,"moveAgents")

self.missiontime=0

for i in self.alist:

i[1].reparentTo(self.render)

xy=lalo2xy(fll,fxy,i[0].pos)

i[1].setPos(xy[0],xy[1],0.0)

def moveAgents(self,task):

global k,bestg, bestParams, bestp, bestc,\

epoch,epochs,stdev,fll,fxy,meanlat,meanlon\

,gcr,spawn,target,hdg,energy,power

xy=lalo2xy(fll,fxy,(meanlat,meanlon))

self.camera.setPos(xy[0],xy[1],200)

self.camera.setP(-90.0)

kills=list()

if len(self.alist)==0:

if self.trialLabel[’text’].split(’ ’)[1]!=str(tNumber):

self.trialLabel[’text’]=’Trial: ’+str(tNumber)

k=0

for j in trial:

if j==np.float(tNumber):

k=int(j)

spawn=(spnLat[k],spnLon[k]) # lat lon

target=(tgtLat[k],tgtLon[k]) # lat lon

txy=lalo2xy(fll,fxy,(spawn[0],spawn[1]))

self.startMark.setPos(txy[0],txy[1],0.0)

txy=lalo2xy(fll,fxy,(target[0],target[1]))

self.endMark.setPos(txy[0],txy[1],0.0)

hdg=np.degrees(nav.gcCrs(spawn,target))

bestParams=(x[k,11],x[k,12],x[k,13],x[k,14])

gcr=nav.gcRng(spawn,target)

meanlat=(spawn[0]+target[0])/2.0

meanlon=(spawn[1]+target[1])/2.0

fll=np.array((14.5,-63.0,16.5,-59.5))

fxy=np.array((-75,-75,75,75))

self.bc=np.nan

self.bg=np.nan

self.bp=np.nan

self.bestgLabel[’text’]=’G: ’+str(np.round(0/3600,2))

self.bestcLabel[’text’]=’C: ’+str(np.round(0/3600,2))

self.bestpLabel[’text’]=’G: ’+str(np.round(0/3600,2))

self.alist.append([agents.pigeonAgent(spawn,depth,hdg,spd\

,target,thresh,energy,power),self.loader.loadModel(\

’/home/aj/agentYellow.x’)])

self.alist.append([agents.crabAgent(spawn,depth,hdg,spd,\

target,thresh,energy,power),self.loader.loadModel(\

183

’/home/aj/agentRed.x’)])

if bestParams!=(0.0,0.0,0.0,0.0):

self.alist.append([agents.dorAgent(bestParams[0],\

bestParams[1],bestParams[2],bestParams[3],spawn\

,depth,hdg,spd,target,thresh,energy,power),\

self.loader.loadModel(’/home/aj/agentBlue.x’)])

for i in self.alist:

i[1].reparentTo(self.render)

xy=lalo2xy(fll,fxy,i[0].pos)

i[1].setPos(xy[0],xy[1],0.0)

self.missiontime=0.0

self.mtvalLabel[’text’]=str(np.round(self.missiontime/3600.0,2))

self.missiontime=self.missiontime+timestep

for i in xrange(len(self.alist)):

killi=False

self.alist[i][0].updateAll(VF.linterpTDLL((self.missiontime\

-timestep)/3600.0,depth,self.alist[i][0].pos[0],\

self.alist[i][0].pos[1]),HF.grad(self.alist[i][0].pos,\

100.0),HF.linterpll(self.alist[i][0].pos),timestep,0)

if self.alist[i][0].__class__==agents.crabAgent:

drdt=(nav.gcRng(self.alist[i][0].prepos,\

self.alist[i][0].tgt)-nav.gcRng(\

self.alist[i][0].pos,self.alist[i][0].tgt))/timestep

self.rngvalLabel[’text’]=str(np.round(nav.gcRng(\

self.alist[i][0].pos,self.alist[i][0].tgt),0))

if (np.isnan(self.alist[i][0].pos[0]) or \

self.alist[i][0].energy <= 0.0 or ( ((fll[0]<\

self.alist[i][0].pos[0]<fll[2]) and (fll[1]<\

self.alist[i][0].pos[1]<fll[3]))!=True )):

killi=True

elif nav.gcRng(self.alist[i][0].pos,self.alist[i][0].tgt)\

<thresh:

drdt=(nav.gcRng(self.alist[i][0].prepos,\

self.alist[i][0].tgt)-nav.gcRng(self.alist[i][0].pos\

,self.alist[i][0].tgt))/timestep

dt=nav.gcRng(self.alist[i][0].pos,self.alist[i][0].tgt)\

/drdt

if self.alist[i][0].__class__==agents.crabAgent and\

((self.missiontime + dt < self.bc) or \

(np.isnan(self.bc))):

self.bc = self.missiontime+dt

self.bestcLabel[’text’]=’C: ’+str(np.round(\

self.bc/3600,4))

if self.alist[i][0].__class__==agents.pigeonAgent and\

( (self.missiontime + dt < self.bp ) or \

(np.isnan(self.bp)) ):

self.bp = self.missiontime+dt

self.bestpLabel[’text’]=’P: ’+str(np.round(self.bp\

/3600,4))

if self.alist[i][0].__class__==agents.dorAgent and (\

(self.missiontime + dt < self.bg ) or (np.isnan(\

self.bg)) ):

184

self.bg = self.missiontime+dt

self.bestgLabel[’text’]=’G: ’+str(np.round(\

self.bg/3600,4))

killi=True

if killi==False:

xy=lalo2xy(fll,fxy,self.alist[i][0].pos)

self.alist[i][1].setH(-1*self.alist[i][0].heading)

self.alist[i][1].setPos(xy[0],xy[1],0.0)

else:

self.alist[i][1].removeNode()

kills.append(self.alist[i])

for i in xrange(len(kills)):

self.alist.remove(kills[i])

del kills

return pt.Task.cont

theProgram = Model()

theProgram.run()

C.12 popro.py

import numpy as np

def none2Nan(isnone):

if isnone==’None’:

return np.nan

else:

return np.float(isnone)

logroot=’/home/aj/Dropbox/EclipseWorkspace/GrouperNav/Results/’

lognames=[’Carib_300cms/Series0/TrialLogAStar_20k_0.csv’,\

’Carib_300cms/Series0/TrialLogAStar_10k_0.csv’,\

’Carib_300cms/Series0/TrialLog_S500_I50_0.csv’,\

’Carib_300cms/Series0/TrialLog_C_0.csv’,\

’Carib_300cms/Series0/TrialLog_P_0.csv’,\

’Carib_300cms/Series0/TrialLogZeroA_S50_I50_0.csv’,\

’Carib_300cms/Series0/TrialLogZeroA15_S50_I50_0.csv’,\

’Carib_150cms/Series0/TrialLog_S500_I50_0.csv’ ,\

’Carib_150cms/Series0/TrialLogAStar_0.csv’,\

’Carib_150cms/Series0/TrialLogAStar_20k_0.csv’,\

’Carib_150cms/Series0/TrialLog_C_0.csv’,\

’Carib_150cms/Series0/TrialLog_P_0.csv’,\

’Carib_150cms/Series0/TrialLogZeroA_S50_I50_0.csv’,\

’Carib_150cms/Series0/TrialLogZeroA15_S50_I50_0.csv’,\

’Carib_40cms/Series0/TrialLog_S500_I50_0.csv’,\

’Carib_40cms/Series0/TrialLog_S500_I50_1.csv’,\

’Carib_40cms/Series0/TrialLog_S500_I50_2.csv’,\

’Carib_40cms/Series0/TrialLogAStar_1.csv’,\

’Carib_40cms/Series0/TrialLogAStar_20k_0.csv’,\

’Carib_40cms/Series0/TrialLog_C_0.csv’,\

’Carib_40cms/Series0/TrialLog_P_0.csv’,\

185

’Carib_40cms/Series0/TrialLogZeroA_S50_I50_0.csv’,\

’Carib_40cms/Series0/TrialLogZeroA15_S50_I50_0.csv’,\

’Gulf_300cms/Series0/TrialLog_S500_I50_0.csv’,\

’Gulf_300cms/Series0/TrialLogAStar_0.csv’,\

’Gulf_300cms/Series0/TrialLogAStar_20k_0.csv’,\

’Gulf_300cms/Series0/TrialLog_C_0.csv’,\

’Gulf_300cms/Series0/TrialLog_P_0.csv’,\

’Gulf_300cms/Series0/TrialLogZeroA_S50_I50_0.csv’,\

’Gulf_300cms/Series0/TrialLogZeroA15_S50_I50_0.csv’,\

’Gulf_150cms/Series0/TrialLogAStar_0.csv’,\

’Gulf_150cms/Series0/TrialLogAStar_20k_0.csv’,\

’Gulf_150cms/Series0/TrialLog_S500_I50_0.csv’,\

’Gulf_150cms/Series0/TrialLog_C_0.csv’,\

’Gulf_150cms/Series0/TrialLog_P_0.csv’,\

’Gulf_150cms/Series0/TrialLogZeroA_S50_I50_0.csv’,\

’Gulf_150cms/Series0/TrialLogZeroA15_S50_I50_0.csv’,\

’Gulf_40cms/Series0/TrialLog_S500_I50_0.csv’,\

’Gulf_40cms/Series0/TrialLogAStar_20k_0.csv’,\

’Gulf_40cms/Series0/TrialLog_C_0.csv’,\

’Gulf_40cms/Series0/TrialLog_P_0.csv’,\

’Gulf_40cms/Series0/TrialLogZeroA_S50_I50_0.csv’,\

’Gulf_40cms/Series0/TrialLogZeroA15_S50_I50_0.csv’

]

res=list()

print ’Suc Rate\t’,’Mtime\t’,’treq\t’,’tsuc’

for i in range(len(lognames)):

res.append(np.genfromtxt(logroot+lognames[i],delimiter=’,’))

conv=0

treq=0.0

treqsuc=0.0

mtime=0.0

effspd=0.0

FEpoch=0.0

minFEpoch=None

maxFEpoch=None

for j in xrange(res[i].shape[0]):

if res[i].shape[1]==27:

if res[i][j,26]==1.0:

conv+=1.0

if minFEpoch==None:

minFEpoch=res[i][j,17]

elif res[i][j,17]<minFEpoch:

minFEpoch=res[i][j,17]

if maxFEpoch==None:

maxFEpoch=res[i][j,17]

elif res[i][j,17]>maxFEpoch:

maxFEpoch=res[i][j,17]

FEpoch+=res[i][j,17]

treqsuc+=res[i][j,1]

mtime+=res[i][j,10]

effspd+=res[i][j,6]/res[i][j,10]

treq+=res[i][j,1]

186

if res[i].shape[1]==12:

if res[i][j,11]==1.0:

conv+=1.0

treqsuc+=res[i][j,1]

mtime+=res[i][j,10]

effspd+=res[i][j,6]/res[i][j,10]

treq+=res[i][j,1]

if res[i].shape[1]==13:

if res[i][j,12]==1.0:

conv+=1.0

treqsuc+=res[i][j,1]

mtime+=res[i][j,11]

effspd+=res[i][j,6]/res[i][j,11]

treq+=res[i][j,1]

if res[i].shape[1]==9:

if res[i][j,8]:

conv+=1.0

treqsuc+=res[i][j,1]

mtime+=res[i][j,7]

effspd+=res[i][j,6]/res[i][j,7]

treq+=res[i][j,1]

avgtreq=treq/res[i].shape[0]

avgtsuc=treqsuc/conv

avgmtime=mtime/conv

avgeffspd=effspd/conv

avgFEpoch=FEpoch/conv

sFEpoch=0.0

seffspd=0.0

conv=0.0

for j in xrange(res[i].shape[0]):

if res[i].shape[1]==27:

if res[i][j,26]==1.0:

conv+=1.0

sFEpoch+=np.sqrt( (res[i][j,26]-avgFEpoch)**2 )

seffspd+=np.sqrt( (res[i][j,6]/res[i][j,10]\

-avgeffspd)**2 )

if res[i].shape[1]==12:

if res[i][j,11]==1.0:

conv+=1.0

seffspd+=np.sqrt( (res[i][j,6]/res[i][j,10]\

-avgeffspd)**2 )

if res[i].shape[1]==13:

if res[i][j,12]==1.0:

conv+=1.0

seffspd+=np.sqrt( (res[i][j,6]/res[i][j,11]\

-avgeffspd)**2 )

if res[i].shape[1]==9:

if res[i][j,8]:

conv+=1.0

seffspd+=np.sqrt( (res[i][j,6]/res[i][j,7]\

-avgeffspd)**2 )

sFEpoch=sFEpoch/conv

187

seffspd=seffspd/conv

print ’----’+lognames[i]+’----’

print conv/res[i].shape[0],avgmtime/3600,avgeffspd,seffspd,\

avgtreq,avgtsuc,avgFEpoch,sFEpoch,’\n’

C.13 HullProp.py

import numpy as np

import navigation as nav

import environment as env

import pylab as pl

import Image as im

import random

def hf(B,F,x):

return -B/(2.0*F**2)*x**2.0 + B/2.0

def ha(B,F,A,Chi,x):

return -B*Xfa/F**2*x+Chi/2 - A*B*Xfa/F**2

WtBat=3.88/16.0 #lbf

NBats=2.0

WtBats=WtBat*NBats

WtMot=2.0/16.0

wsp=62.4/(12.0**3)

A=12.0 # inches

F=14.0 # inches

B=8.0 # inches

Chi=4.0 # inches

LWL=A+F

xMid=LWL/2.0-A

T=0.75

Bmid=2.0*hf(B,F,xMid)

Amid=Bmid*T

Xfa=-A + F**2.0*np.sqrt(B/F**2.0*(B*A**2/F**2-B+Chi))/B

Area=2.0*( -B*Xfa/(2.0*F**2)*Xfa**2 - B*A*Xfa/F**2*Xfa + Chi/2.0*Xfa\

+B*Xfa/(2.0*F**2.0)*(-A)**2.0 + B*A*Xfa/F**2.0*(-A) - \

Chi/2*(-A) )+2.0/3.0*B*F+B/(3*F**2.0)*(Xfa)**3.0-B*Xfa

print ’Area * T’,Area*T,’\tLWL*B*T’,LWL*T*B

print Area

thick=.25

VDisp=T*Area

VHull=Area*thick

WHull=1.2*wsp*VHull

w=WtBats+WtMot+WHull

C0=1+(B*Xfa/F**2)**2

C1=1+(B/F)**2

Lf=-1* F**2/(2.0*B)*( B/F*( (Xfa/F)*np.sqrt(C0) -np.sqrt(C1))\

+np.log((-B/F + np.sqrt(C1)) / (-B*Xfa/F**2 \

+np.sqrt(C0)) ) )

print ’WHull: ’,WHull,’Draft: ’,w/(wsp*Area),’Lf: ’,Lf,np.sqrt(B**2\

188

+F**2),Xfa

La=np.sqrt((Xfa+A)**2*C0)

print ’Side Length: ’,La+Lf

N=103

Lfnum=0.0

for i in xrange(1,N+1):

dx=(F-Xfa)/N

x=dx*i+Xfa

Lfnum+=np.sqrt(dx**2+(hf(B,F,x)-hf(B,F,x-dx))**2)

print Lfnum,La,np.sqrt( (ha(B,F,A,Chi,Xfa)-ha(B,F,A,Chi,-A))**2+\

(Xfa+A)**2 )

shape=np.zeros((N+1,2))

for i in xrange(N+1):

x=-A+(A+F)/N*(i)

shape[i,0]=x

shape[i,1]=(1.0/(1.0+np.exp(100.0*x-Xfa)))*(-1.0/(1.0+np.exp(\

100.0*x-A))+1)+(1.0/(1.0+np.exp(100.0*x-F)))*(-1.0/(1.0+\

np.exp(100.0*x+Xfa))+1 )

if x<=Xfa:

shape[i,1]=(-B*Xfa/(F**2.0)) * x - A*B*Xfa/(F**2) + Chi/2.0

else:

shape[i,1]=-B/(2.0*F**2)*x**2.0 + B/2.0

print 26-(shape[i,0]+12),np.round(2.54*shape[i,1],1)

print ’Bmid’,Bmid,’\tB ’,B

print ’C_WP: ’,Area/(B*LWL)

print ’C_B: ’,Area/(B*LWL)

print ’C_P: ’,VDisp/((T*Bmid*LWL))

print ’C_M: ’,Amid/(Bmid*T)

print ’C_VP: ’,1.0

pl.plot(shape[:,0],shape[:,1],’k’,linewidth=3.5)

pl.plot(shape[:,0],-shape[:,1],’k’,linewidth=3.5)

pl.plot((-A,-A),(-Chi/2,Chi/2),’k’,linewidth=3.5)

pl.axes().set_aspect(’equal’)

pl.show()

C.14 FieldTest.py

import numpy as np

import asearch

log=np.genfromtxt(’/home/aj/Dropbox/FieldTrials/04092013-\

Series2/LOG00314.TXT’,delimiter=’,’)

route=list()

#route.append( (28.12610,-80.63946))

for i in range(16,log.shape[0]):

route.append([log[i][4],log[i][5]])

route=np.array(route)

asearch.Route2KML([route], ’FieldTest-1.kml’)

189

C.15 csv2kml.py

import numpy as np

import asearch

WPFileName=’Gulf100.csv’

rp=np.genfromtxt(WPFileName,delimiter=’,’)

routes=list()

#for j in range(rp.shape[0]):

for j in range(100):

routes.append(np.array( [ (rp[j,0],rp[j,1]), (rp[j,2],rp[j,3]) ] ) )

for j in range(len(routes)):

print ’------------------’

for i in range(len(routes[j])):

print routes[j][i,1],routes[j][i,0]

asearch.Route2KML(routes,’Gulf100.kml’)

C.16 ANav.py

import random

import time

import numpy as np

import pylab as pl

import environment as env

import asearch as a

import navigation as nav

print ’Loading VField’

#VFileName=’SEGulf2012_05_01.npz’

VFileName=’Carib2012_05_01.npz’

#HFileName=’etopo1_gulf0.npz’

HFileName=’etopo1_carib0.npz’

#WPFileName=’Gulf100.csv’

WPFileName=’EWCarib150.csv’

VF=env.VField(VFileName)

print ’Velocity File Loaded.’

HF=env.HField(HFileName)

print ’Depth File Loaded.’

WPs=np.genfromtxt(WPFileName,delimiter=’,’)

print ’Waypoint File Loaded.’

print ’All Files Loaded.’

print VF.maxmag

fll=np.array( (14.5,-63.0,16.5,-59.5) ) #Carib

#fll=np.array( (23.1,-88.5,26.1,-83.5) ) #Gulf

Range=150000.0 # meters

random.seed(time.time())

timeout=24*7.0*3600

trials=100

spd=3.0

190

leg=10000.0 # meters/sec

tstep=360.0*0.5/(spd) # seconds

thresh=(spd+VF.maxmag)*tstep # meters

print ’Range Threshold: ’,thresh

depth=2.0

for t in xrange(1):

logroot=’Results/Carib_’+str(int(spd*100))+\

’cms/Series0/TrialLogAStar_’+str(int(leg/1000))+’k_’+str(t)

logname=logroot+str(’.csv’)

for trial in range(100):

########################## TRIAL BOUNDARY ############

spawn=(WPs[trial,0],WPs[trial,1],0.0)

target=(WPs[trial,2],WPs[trial,3],0.0)

t0=time.time()

mintime=a.AstarSearchTime(spawn,target, leg, 0, spd,\

tstep, VF,depth,HF,timeout)

t1=time.time()

if mintime[1]!=None:

outtime=mintime[1]*3600

else:

outtime=mintime[1]

aconv=0

if outtime!=None:

aconv=1

log=open(logname,’a’)

outString=str(trial)+’,’+str(t1-t0)+’,’+str(spawn[0])\

+’,’+str(spawn[1])\

+’,’+str(target[0])+’,’+str(target[1])\

+’,’+str(nav.gcRng(spawn,target))\

+’,’+str(spd)\

+’,’+str(leg)\

+’,’+str(tstep)\

+’,’+str(timeout)\

+’,’+str(outtime)\

+’,’+str(aconv)+’\n’

print outString

log.write(outString)

log.close()

C.17 GrouperNav.py

import random

import time

import numpy as np

import agents

import environment as env

import navigation as nav

#VFileName=’SEGulf2012_05_01.npz’

191

VFileName=’Carib2012_05_01.npz’

#HFileName=’etopo1_gulf0.npz’

HFileName=’etopo1_carib0.npz’

#WPFileName=’Gulf100.csv’

WPFileName=’EWCarib150.csv’

#fll=np.array( (23.1,-88.5,26.1,-83.5) )

fll=np.array( (14.5,-63.0,16.5,-59.5) )

VF=env.VField(VFileName)

HF=env.HField(HFileName)

WPs=np.genfromtxt(WPFileName,delimiter=’,’)

print ’File Loaded.’

print HF.maxz

Range=150000.0 # meters

Doff=np.degrees(Range/(nav.REarth*np.cos(np.radians(\

(fll[0]+fll[2])/2.0))))

random.seed(time.time())

trials=100

spd=0.4 # meters/sec

power=1.0

timestep=360.0*0.5/(spd) # seconds

thresh=(spd+VF.maxmag)*timestep # meters

print ’Range Threshold: ’,thresh

timeout=24*7.0*3600

StrayFactor=2.0 # Factor between still-water, straight-line time, and

# maximum allowable time

energy=np.nanmin((Range/spd*StrayFactor,timeout))#16#24*7*3600.0

Eepochs=5 #Maximum Epochs to search for a solution

Xepochs=10 #Maximum Epochs to optimize a solution

doras=500 #int(np.floor((groupers**(1.0/3.0))**4))

factor=10

depth=2.0

stdevstart=0.5

for t in (5,):

M=0.001#1.0/(10.0**float(m))

T=2700 #t*900/5-900+2700

logroot=’Results/Carib_’+str(int(spd*100))+\

’cms/Series0/TrialLog’+’_S’+str(doras)+’_I’+\

str(doras/factor)+’_’+str(t)

logname=logroot+str(’.csv’)

cfgout=open(logroot+’.xml’,’w’)

cfgout.write(’<?xml version="1.0"?>\n’)

cfgout.write(’<xml>\n’)

cfgout.write(’\t\t<Notes></Notes>\n’)

cfgout.write(’\t\t<velocityFile>’+VFileName+’</velocityFile>\n’)

cfgout.write(’\t\t<latlon>’+str(fll)+’</latlon>\n’)

cfgout.write(’\t\t<HeightFile>’+HFileName+’</HeightFile>\n’)

cfgout.write(’\t\t<M>’+str(M)+’</M>\n’)

cfgout.write(’\t\t<T>’+str(T)+’</T>\n’)

cfgout.write(’\t\t<range>’+str(Range)+’</range>\n’)

cfgout.write(’\t\t<speed>’+str(spd)+’</speed>\n’)

cfgout.write(’\t\t<energy>’+str(energy)+’</energy>\n’)

cfgout.write(’\t\t<power>’+str(power)+’</power>\n’)

192

cfgout.write(’\t\t<timeStep>’+str(timestep)+’</timeStep>\n’)

cfgout.write(’\t\t<threshold>’+str(thresh)+’</threshold>\n’)

cfgout.write(’\t\t<timeout>’+str(timeout)+’</timeout>\n’)

cfgout.write(’\t\t<Sepochs>’+str(Eepochs)+’</Sepochs>\n’)

cfgout.write(’\t\t<Oepochs>’+str(Xepochs)+’</Oepochs>\n’)

cfgout.write(’\t\t<doras>’+str(doras)+’</doras>\n’)

cfgout.write(’\t\t<stdev>’+str(stdevstart)+’</stdev>\n’)

cfgout.write(’\t\t<depth>’+str(depth)+’</depth>\n’)

cfgout.write(’</xml>\n’)

cfgout.close()

for trial in range(78,trials):

########################## TRIAL BOUNDARY #################

spawn=(WPs[trial,0],WPs[trial,1])

target=(WPs[trial,2],WPs[trial,3])

hdg=np.degrees(nav.gcCrs(spawn,target)) # ROUND EARTH

epoch=0

bestp=None

bestc=None

bestd=None

firstbest=None

firstsolEpoch=None

firstsolParams=(0,0,0,0)

bestDEpoch=None

bestDParams=(0,0,0,0)

alist=list()

missiontime=0.0

t0=time.time()

############EXPLORATION

while (epoch<=Eepochs and bestDParams==(0,0,0,0)):

if len(alist)==0:

alist.append([agents.pigeonAgent(spawn,depth,hdg,\

spd,target,thresh,energy,power),None])

alist.append([agents.crabAgent(spawn,depth,hdg,spd,\

target,thresh,energy,power),None])

for i in range(doras):

alist.append([agents.dorAgent(random.random()\

*2-1,random.random()*2-1,random.random()\

*2-1,random.random()*2-1,spawn,depth,hdg\

,spd,target,thresh,energy,power,M,T),None])

missiontime=0.0

epoch=epoch+1

else:

missiontime=missiontime+timestep

kills=list()

for i in xrange(len(alist)):

killi=False

alist[i][0].updateAll(VF.linterpTDLL(\

(missiontime-timestep)/3600.0,depth, \

alist[i][0].pos[0], alist[i][0].pos[1]),\

HF.grad(alist[i][0].pos,10.0),\

HF.linterpll(alist[i][0].pos),timestep,0)

if (missiontime>=timeout or np.isnan(\

193

alist[i][0].pos[0]) \

or alist[i][0].energy <= 0.0 \

or ( ((fll[0]<alist[i][0].pos[0]<fll[2]) \

and (fll[1] < alist[i][0].pos[1]<fll[3]))\

!=True )):

killi=True

elif nav.gcRng(alist[i][0].pos,alist[i][0].tgt)\

<thresh:

drdt=(nav.gcRng(alist[i][0].prepos,\

alist[i][0].tgt)-nav.gcRng(\

alist[i][0].pos,alist[i][0].tgt))\

/timestep

dt=nav.gcRng(alist[i][0].pos,\

alist[i][0].tgt)/drdt

if drdt<0:

print ’WARNING DRDT IS NEGATIVE!! \

THIS CAN\’T HAPPEN!!’

if alist[i][0].__class__==agents.crabAgent\

and (missiontime + dt < bestc \

or bestc == None):

bestc = missiontime+dt

if alist[i][0].__class__==agents.pigeonAgent\

and (missiontime + dt < bestp \

or bestp == None):

bestp = missiontime+dt

if alist[i][0].__class==agents.dorAgent \

and ( missiontime + dt < bestd \

or bestd == None ):

bestd = missiontime+dt

if bestDEpoch==None:

firstbest=missiontime+dt

firstsolEpoch=epoch

firstsolParams=(alist[i][0].A,\

alist[i][0].B,alist[i][0].C,\

alist[i][0].D)

bestDEpoch=epoch

bestDParams=(alist[i][0].A,\

alist[i][0].B,alist[i][0].C,\

alist[i][0].D)

killi=True

if killi==True:

kills.append(alist[i])

for i in xrange(len(kills)):

alist.remove(kills[i])

del kills

##################EXPLOITATION:

if bestDParams!=(0,0,0,0):

epoch=0

while (epoch < Xepochs):

if len(alist)==0:

stdev=stdevstart*(1-float(epoch)/Xepochs)

for i in range(doras/factor):

194

alist.append([agents.dorAgent(random.gauss( \

bestDParams[0],stdev),random.gauss( \

bestDParams[1],stdev),random.gauss(\

bestDParams[2],stdev),random.gauss(\

bestDParams[3],stdev),spawn,depth,\

hdg,spd,target,thresh,energy,power,\

M,T),None])

missiontime=0.0

epoch=epoch+1

else:

missiontime=missiontime+timestep

kills=list()

for i in xrange(len(alist)):

killi=False

alist[i][0].updateAll(VF.linterpTDLL((\

missiontime-timestep) \

/3600.0,depth, alist[i][0].pos[0],\

alist[i][0].pos[1]), \

HF.grad(alist[i][0].pos,10.0), \

HF.linterpll(alist[i][0].pos),\

timestep,0)

if (missiontime>=timeout or np.isnan(\

alist[i][0].pos[0]) \

or alist[i][0].energy <= 0.0 \

or ( ((fll[0]<alist[i][0].pos[0]\

<fll[2]) and (fll[1] < \

alist[i][0].pos[1]<fll[3]))!=True )):

killi=True

elif nav.gcRng(alist[i][0].pos,\

alist[i][0].tgt)<thresh:

drdt=(nav.gcRng(alist[i][0].prepos,\

alist[i][0].tgt)-nav.gcRng(\

alist[i][0].pos,\

alist[i][0].tgt))/timestep

if drdt<0:

print ’WARNING DRDT IS NEGATIVE!!\

THIS CAN\’T HAPPEN!!’

dt=nav.gcRng(alist[i][0].pos,\

alist[i][0].tgt)/drdt

if alist[i][0].__class__==\

agents.dorAgent and ( missiontime \

+ dt < bestd or bestd == None ):

bestd = missiontime+dt

bestDEpoch=epoch

bestDParams=(alist[i][0].A,\

alist[i][0].B,alist[i][0].C,\

alist[i][0].D)

killi=True

if killi==True:

kills.append(alist[i])

for i in xrange(len(kills)):

alist.remove(kills[i])

195

del kills

#######################TRIAL BOUNDARY####################

t1=time.time()

cconv=False

pconv=False

dconv=False

if bestc!=None:

cconv=True

if bestp!=None:

pconv=True

if bestd!=None:

dconv =True

log=open(logname,’a’)

outString=str(trial)+’,’+str(t1-t0)+’,’+str(spawn[0])\

+’,’+str(spawn[1])\

+’,’+str(target[0])+’,’+str(target[1])\

+’,’+str(nav.gcRng(spawn,target))\

+’,’+str(doras)+’,’+str(bestp)\

+’,’+str(bestc)+’,’+str(bestd)\

+’,’+str(bestDEpoch)+’,’+str(bestDParams[0])\

+’,’+str(bestDParams[1])+’,’+str(bestDParams[2])\

+’,’+str(bestDParams[3])+’,’+str(M)\

+’,’+str(firstsolEpoch)+’,’+str(firstbest)+’,’\

+’,’+str(firstsolParams[0])\

+’,’+str(firstsolParams[1])\

+’,’+str(firstsolParams[2])\

+’,’+str(firstsolParams[3])\

+’,’+str(int(pconv))+’,’+str(int(cconv))\

+’,’+str(int(dconv))+’\n’

print ’TRIAL: ’+str(t)

print outString

log.write(outString)

log.close()

C.18 GrouperNav0A.py

import random

import time

import numpy as np

import agents

import environment as env

import navigation as nav

#for i in (’G’,’C’):

for i in (’C’,):

#for j in (0.4,1.5,3.0):

for j in (3.0,):

if i==’G’:

base=’Results/Gulf_’

196

WPFileName=’Gulf100.csv’

VFileName=’SEGulf2012_05_01.npz’

HFileName=’etopo1_gulf0.npz’

fll=np.array( (23.1,-88.5,26.1,-83.5) )

elif i==’C’:

base=’Results/Carib_’

WPFileName=’EWCarib150.csv’

VFileName=’Carib2012_05_01.npz’

HFileName=’etopo1_carib0.npz’

fll=np.array( (14.5,-63.0,16.5,-59.5) )

WPs=np.genfromtxt(WPFileName,delimiter=’,’)

VF=env.VField(VFileName)

HF=env.HField(HFileName)

print ’File Loaded.’

Range=150000.0 # meters

Doff=np.degrees(Range/(nav.REarth*np.cos(np.radians((fll[0]+\

fll[2])/2.0))))

random.seed(time.time())

trials=100

spd=j # meters/sec

power=1.0

timestep=360.0*0.5/(spd) # seconds

thresh=(spd+VF.maxmag)*timestep # meters

print ’Range Threshold: ’,thresh

timeout=24*7.0*3600

StrayFactor=2.0 # Factor between still-water,

# straight-line time, and

# maximum allowable time

energy=np.nanmin((Range/spd*StrayFactor,timeout))#16#24*7*3600.0

Eepochs=15 #Maximum Epochs to search for a solution

Xepochs=15 #Maximum Epochs to optimize a solution

doras=50 #int(np.floor((groupers**(1.0/3.0))**4))

factor=1

depth=2.0

stdevstart=0.5

for t in xrange(1):

M=0.001#1.0/(10.0**float(m))

T=2700 #t*900/5-900+2700

logroot=base+str(int(spd*100))\

+’cms/Series0/TrialLogZeroA15’+’_S’+str(doras)+’_I’+\

str(doras/factor)+’_’+str(t)

logname=logroot+str(’.csv’)

cfgout=open(logroot+’.xml’,’w’)

cfgout.write(’<?xml version="1.0"?>\n’)

cfgout.write(’<xml>\n’)

cfgout.write(’\t\t<Notes></Notes>\n’)

cfgout.write(’\t\t<velocityFile>’+VFileName\

+’</velocityFile>\n’)

cfgout.write(’\t\t<latlon>’+str(fll)+’</latlon>\n’)

cfgout.write(’\t\t<HeightFile>’+HFileName\

+’</HeightFile>\n’)

cfgout.write(’\t\t<M>’+str(M)+’</M>\n’)

197

cfgout.write(’\t\t<T>’+str(T)+’</T>\n’)

cfgout.write(’\t\t<range>’+str(Range)+’</range>\n’)

cfgout.write(’\t\t<speed>’+str(spd)+’</speed>\n’)

cfgout.write(’\t\t<energy>’+str(energy)+’</energy>\n’)

cfgout.write(’\t\t<power>’+str(power)+’</power>\n’)

cfgout.write(’\t\t<timeStep>’+str(timestep)\

+’</timeStep>\n’)

cfgout.write(’\t\t<threshold>’+str(thresh)\

+’</threshold>\n’)

cfgout.write(’\t\t<timeout>’+str(timeout)\

+’</timeout>\n’)

cfgout.write(’\t\t<Sepochs>’+str(Eepochs)\

+’</Sepochs>\n’)

cfgout.write(’\t\t<Oepochs>’+str(Xepochs)\

+’</Oepochs>\n’)

cfgout.write(’\t\t<doras>’+str(doras)+’</doras>\n’)

cfgout.write(’\t\t<stdev>’+str(stdevstart)+’</stdev>\n’)

cfgout.write(’\t\t<depth>’+str(depth)+’</depth>\n’)

cfgout.write(’</xml>\n’)

cfgout.close()

for trial in range(trials):

########################## TRIAL BOUNDARY #########

spawn=(WPs[trial,0],WPs[trial,1])

target=(WPs[trial,2],WPs[trial,3])

hdg=np.degrees(nav.gcCrs(spawn,target)) # ROUND EARTH

epoch=0

bestp=None

bestc=None

bestd=None

firstbest=None

firstsolEpoch=None

firstsolParams=(0,0,0,0)

bestDEpoch=None

bestDParams=(0,0,0,0)

alist=list()

missiontime=0.0

t0=time.time()

############EXPLORATION

while (epoch<=Eepochs and bestDParams==(0,0,0,0)):

if len(alist)==0:

for i in range(doras):

alist.append([agents.dorAgent(0.0,\

random.random()*2-1,random.random()\

*2-1,random.random()*2-1,spawn,\

depth,hdg,spd,target,\

thresh,energy,power,M,T),None])

missiontime=0.0

epoch=epoch+1

else:

missiontime=missiontime+timestep

kills=list()

for i in xrange(len(alist)):

198

killi=False

alist[i][0].updateAll(VF.linterpTDLL(\

(missiontime-timestep)/3600.0,\

depth, alist[i][0].pos[0], \

alist[i][0].pos[1]),\

HF.grad(alist[i][0].pos,10.0), \

HF.linterpll(alist[i][0].pos),\

timestep,0)

if (missiontime>=timeout or np.isnan(alist[i][0].pos[0]) \

or alist[i][0].energy <= 0.0 \

or ( ((fll[0]<alist[i][0].pos[0]<\

fll[2]) and (fll[1] < \

alist[i][0].pos[1]<fll[3]))!=True )):

killi=True

elif nav.gcRng(alist[i][0].pos,\

alist[i][0].tgt)<thresh:

drdt=(nav.gcRng(alist[i][0].prepos,\

alist[i][0].tgt)-nav.gcRng(\

alist[i][0].pos,alist[i][0].tgt\

))/timestep

dt=nav.gcRng(alist[i][0].pos,\

alist[i][0].tgt)/drdt

if drdt<0:

print ’WARNING DRDT IS \

NEGATIVE!! THIS CAN\’T HAPPEN!!’

if alist[i][0].__class__==\

agents.crabAgent and (missiontime\

+ dt < bestc or bestc == None):

bestc = missiontime+dt

if alist[i][0].__class__==\

agents.pigeonAgent and (missiontime\

+ dt < bestp or bestp == None):

bestp = missiontime+dt

if alist[i][0].__class__==\

agents.dorAgent and ( missiontime\

+ dt < bestd or bestd == None ):

bestd = missiontime+dt

if bestDEpoch==None:

firstbest=missiontime+dt

firstsolEpoch=epoch

firstsolParams=\

(alist[i][0].A,\

alist[i][0].B,\

alist[i][0].C,\

alist[i][0].D)

bestDEpoch=epoch

bestDParams=(\

alist[i][0].A,\

alist[i][0].B,\

alist[i][0].C,\

alist[i][0].D)

killi=True

199

if killi==True:

kills.append(alist[i])

for i in xrange(len(kills)):

alist.remove(kills[i])

del kills

##################EXPLOITATION:

if bestDParams!=(0,0,0,0):

Xepochs=Eepochs-epoch

epoch=0

while (epoch < Xepochs):

if len(alist)==0:

stdev=stdevstart*(1-float(epoch)/Xepochs)

for i in range(doras/factor):

alist.append([agents.dorAgent(0.0,\

random.gauss(\

bestDParams[1],stdev),\

random.gauss(\

bestDParams[2],stdev),\

random.gauss(\

bestDParams[3],stdev),\

spawn,depth,hdg,spd,\

target,thresh,energy,power\

,M,T),None])

missiontime=0.0

epoch=epoch+1

else:

missiontime=missiontime+timestep

kills=list()

for i in xrange(len(alist)):

killi=False

alist[i][0].updateAll(\

VF.linterpTDLL((missiontime-timestep) \

/3600.0,depth,\

alist[i][0].pos[0],\

alist[i][0].pos[1]),\

HF.grad(alist[i][0].pos,10.0),\

HF.linterpll(alist[i][0].pos),\

timestep,0)

if (missiontime>=timeout \

or np.isnan(alist[i][0].pos[0]) \

or alist[i][0].energy <= 0.0 \

or ( ((fll[0]<alist[i][0].pos[0]\

<fll[2]) and (fll[1] < \

alist[i][0].pos[1]<fll[3]))!=True)):

killi=True

elif nav.gcRng(alist[i][0].pos,\

alist[i][0].tgt)<thresh:

drdt=(nav.gcRng(\

alist[i][0].prepos,\

alist[i][0].tgt)-nav.gcRng(\

alist[i][0].pos,\

alist[i][0].tgt))/timestep

200

if drdt<0:

print ’WARNING DRDT IS \

NEGATIVE!! THIS CAN\’T \

HAPPEN!!’

dt=nav.gcRng(alist[i][0].pos,\

alist[i][0].tgt)/drdt

if alist[i][0].__class__==\

agents.dorAgent and(missiontime\

+ dt < bestd or bestd == None ):

bestd = missiontime+dt

bestDEpoch=epoch

bestDParams=(\

alist[i][0].A,\

alist[i][0].B,\

alist[i][0].C,\

alist[i][0].D)

killi=True

if killi==True:

kills.append(alist[i])

for i in xrange(len(kills)):

alist.remove(kills[i])

del kills

#######################TRIAL BOUNDARY####################

t1=time.time()

cconv=False

pconv=False

dconv=False

if bestc!=None:

cconv=True

if bestp!=None:

pconv=True

if bestd!=None:

dconv =True

log=open(logname,’a’)

outString=str(trial)+’,’+str(t1-t0)\

+’,’+str(spawn[0])\

+’,’+str(spawn[1])\

+’,’+str(target[0])+’,’+str(target[1])\

+’,’+str(nav.gcRng(spawn,target))\

+’,’+str(doras)+’,’+str(bestp)\

+’,’+str(bestc)+’,’+str(bestd)\

+’,’+str(bestDEpoch)\

+’,’+str(bestDParams[0])\

+’,’+str(bestDParams[1])\

+’,’+str(bestDParams[2])\

+’,’+str(bestDParams[3])+’,’+str(M)\

+’,’+str(firstsolEpoch)\

+’,’+str(firstbest)+’,’\

+’,’+str(firstsolParams[0])\

+’,’+str(firstsolParams[1])\

+’,’+str(firstsolParams[2])\

+’,’+str(firstsolParams[3])\

201

+’,’+str(int(pconv))+’,’+str(int(cconv))\

+’,’+str(int(dconv))+’\n’

print outString

log.write(outString)

log.close()

trial+=1

C.19 PNav.py

import random

import time

import numpy as np

import agents

import environment as env

import navigation as nav

for i in (’G’,’C’):

for j in (0.4,1.5,3.0):

if i==’G’:

base=’Results/Gulf_’

WPFileName=’Gulf100.csv’

VFileName=’SEGulf2012_05_01.npz’

HFileName=’etopo1_gulf0.npz’

fll=np.array( (23.1,-88.5,26.1,-83.5) )

elif i==’C’:

base=’Results/Carib_’

WPFileName=’EWCarib150.csv’

VFileName=’Carib2012_05_01.npz’

HFileName=’etopo1_carib0.npz’

fll=np.array( (14.5,-63.0,16.5,-59.5) )

WPs=np.genfromtxt(WPFileName,delimiter=’,’)

VF=env.VField(VFileName)

HF=env.HField(HFileName)

print ’File Loaded.’

Range=150000.0 # meters

Doff=np.degrees(Range/(nav.REarth*np.cos(np.radians((fll[0]+\

fll[2])/2.0))))

random.seed(time.time())

trials=100

spd=j # meters/sec

power=1.0

timestep=360.0*0.5/(spd) # seconds

thresh=(spd+VF.maxmag)*timestep # meters

print ’Range Threshold: ’,thresh

timeout=24*7.0*3600

StrayFactor=2.0 # Factor between still-water, straight-line

# time, and

# maximum allowable time

energy=np.nanmin((Range/spd*StrayFactor,timeout))#16#24*7*3600.0

202

Xepochs=10 #Maximum Epochs to optimize a solution

depth=2.0

stdevstart=0.5

for t in xrange(1):

logname=base+str(int(100*spd))+’cms/Series0/TrialLog’+’_P_’\

+str(t)+’.csv’

for trial in range(trials):

spawn=(WPs[trial,0],WPs[trial,1])

target=(WPs[trial,2],WPs[trial,3])

hdg=np.degrees(nav.gcCrs(spawn,target)) # ROUND EARTH

epoch=0

bestp=None

bestc=None

alist=list()

missiontime=0.0

t0=time.time()

while (epoch<=1 ):

if len(alist)==0:

alist.append([agents.pigeonAgent(spawn,depth,\

hdg,spd,\

target,thresh,energy,power),None])

missiontime=0.0

epoch=epoch+1

else:

missiontime=missiontime+timestep

kills=list()

for i in xrange(len(alist)):

killi=False

alist[i][0].updateAll(VF.linterpTDLL((\

missiontime-timestep)/3600.0,depth,\

alist[i][0].pos[0], \

alist[i][0].pos[1]), \

HF.grad(alist[i][0].pos,10.0),\

HF.linterpll(alist[i][0].pos),\

timestep,0)

if (missiontime>=timeout or np.isnan(\

alist[i][0].pos[0]) \

or alist[i][0].energy <= 0.0 \

or ( ((fll[0]<alist[i][0].pos[0]<fll[2])\

and (fll[1] < alist[i][0].pos[1]<\

fll[3]))!=True )):

killi=True

elif nav.gcRng(alist[i][0].pos,\

alist[i][0].tgt)<thresh:

drdt=(nav.gcRng(alist[i][0].prepos,\

alist[i][0].tgt)-nav.gcRng(\

alist[i][0].pos,alist[i][0].tgt))\

/timestep

dt=nav.gcRng(alist[i][0].pos,\

alist[i][0].tgt)/drdt

if drdt<0:

print ’WARNING DRDT IS NEGATIVE!! \

203

THIS CAN\’T HAPPEN!!’

if alist[i][0].__class__==\

agents.crabAgent and (missiontime + dt\

< bestc or bestc == None):

bestc = missiontime+dt

if alist[i][0].__class__==\

agents.pigeonAgent and (missiontime\

+ dt < bestp or bestp == None):

bestp = missiontime+dt

killi=True

if killi==True:

kills.append(alist[i])

for i in xrange(len(kills)):

alist.remove(kills[i])

del kills

t1=time.time()

cconv=False

pconv=False

if bestc!=None:

cconv=True

if bestp!=None:

pconv=True

log=open(logname,’a’)

outString=str(trial)+’,’+str(t1-t0)+’,’+str(spawn[0])\

+’,’+str(spawn[1])\

+’,’+str(target[0])+’,’+str(target[1])\

+’,’+str(nav.gcRng(spawn,target))\

+’,’+str(bestp)\

+’,’+str(int(pconv))\

+’\n’

print outString

log.write(outString)

log.close()

trial+=1

C.20 CNav.py

import random

import time

import numpy as np

import agents

import environment as env

import navigation as nav

for i in (’G’,’C’):

for j in (0.4,1.5,3.0):

if i==’G’:

base=’Results/Gulf_’

WPFileName=’Gulf100.csv’

204

VFileName=’SEGulf2012_05_01.npz’

HFileName=’etopo1_gulf0.npz’

fll=np.array( (23.1,-88.5,26.1,-83.5) )

elif i==’C’:

base=’Results/Carib_’

WPFileName=’EWCarib150.csv’

VFileName=’Carib2012_05_01.npz’

HFileName=’etopo1_carib0.npz’

fll=np.array( (14.5,-63.0,16.5,-59.5) )

WPs=np.genfromtxt(WPFileName,delimiter=’,’)

VF=env.VField(VFileName)

HF=env.HField(HFileName)

print ’File Loaded.’

Range=150000.0 # meters

Doff=np.degrees(Range/(nav.REarth*np.cos(np.radians((fll[0]\

+fll[2])/2.0))))

random.seed(time.time())

trials=100

spd=j # meters/sec

power=1.0

timestep=360.0*0.5/(spd) # seconds

thresh=(spd+VF.maxmag)*timestep # meters

print ’Range Threshold: ’,thresh

timeout=24*7.0*3600

StrayFactor=2.0 # Factor between still-water, straight-line

# time, and

# maximum allowable time

energy=np.nanmin((Range/spd*StrayFactor,timeout))#16#24*7*3600.0

Xepochs=10 #Maximum Epochs to optimize a solution

depth=2.0

stdevstart=0.5

for t in xrange(1):

logname=base+str(int(100*spd))+’cms/Series0/TrialLog’+\

’_C_’+str(t)+’.csv’

for trial in range(trials):

spawn=(WPs[trial,0],WPs[trial,1])

target=(WPs[trial,2],WPs[trial,3])

hdg=np.degrees(nav.gcCrs(spawn,target)) # ROUND EARTH

epoch=0

bestp=None

bestc=None

alist=list()

missiontime=0.0

t0=time.time()

while (epoch<=1 ):

if len(alist)==0:

alist.append([agents.crabAgent(spawn,depth,hdg,spd,\

target,thresh,energy,power),None])

missiontime=0.0

epoch=epoch+1

else:

missiontime=missiontime+timestep

205

kills=list()

for i in xrange(len(alist)):

killi=False

alist[i][0].updateAll(VF.linterpTDLL((\

missiontime-timestep) \

/3600.0,depth, alist[i][0].pos[0],\

alist[i][0].pos[1]), \

HF.grad(alist[i][0].pos,10.0), \

HF.linterpll(alist[i][0].pos),\

timestep,0)

if (missiontime>=timeout or np.isnan(\

alist[i][0].pos[0]) \

or alist[i][0].energy <= 0.0 \

or ( ((fll[0]<alist[i][0].pos[0]<fll[2]) \

and (fll[1] < alist[i][0].pos[1]<fll[3]))\

!=True )):

killi=True

elif nav.gcRng(alist[i][0].pos,\

alist[i][0].tgt)<thresh:

drdt=(nav.gcRng(alist[i][0].prepos,\

alist[i][0].tgt)-nav.gcRng(\

alist[i][0].pos,alist[i][0].tgt))\

/timestep

dt=nav.gcRng(alist[i][0].pos,\

alist[i][0].tgt)/drdt

if drdt<0:

print ’WARNING DRDT IS NEGATIVE!!\

THIS CAN\’T HAPPEN!!’

if alist[i][0].__class__==\

agents.crabAgent and (missiontime + dt\

< bestc or bestc == None):

bestc = missiontime+dt

killi=True

if killi==True:

kills.append(alist[i])

for i in xrange(len(kills)):

alist.remove(kills[i])

del kills

t1=time.time()

cconv=False

if bestc!=None:

cconv=True

log=open(logname,’a’)

outString=str(trial)+’,’+str(t1-t0)+’,’+str(spawn[0])\

+’,’+str(spawn[1])\

+’,’+str(target[0])+’,’+str(target[1])\

+’,’+str(nav.gcRng(spawn,target))\

+’,’+str(bestc)\

+’,’+str(int(cconv))\

+’\n’

print outString

log.write(outString)

206

log.close()

trial+=1

207