Title Page
 Table of Contents
 Existing obstacle avoidance...
 Reflexive mine avoidance appro...
 Estimating sonar target positions...
 Comparison of the reflexive mine...
 Biographical sketch

Group Title: reflexive mine avoidance approach for autonomous underwater vehicles
Title: A reflexive mine avoidance approach for autonomous underwater vehicles
Full Citation
Permanent Link: http://ufdc.ufl.edu/UF00082407/00001
 Material Information
Title: A reflexive mine avoidance approach for autonomous underwater vehicles
Physical Description: vi, 270 leaves : ill., photos (some col.) ; 29 cm.
Language: English
Creator: Hyland, John Charles
Publication Date: 1993
Subject: Oceanographic submersibles   ( lcsh )
Remote submersibles   ( lcsh )
Electrical Engineering thesis Ph. D
Dissertations, Academic -- Electrical Engineering -- UF
Genre: bibliography   ( marcgt )
non-fiction   ( marcgt )
Thesis: Thesis (Ph. D.)--University of Florida, 1993.
Bibliography: Includes bibliographical references (leaves 259-269).
Statement of Responsibility: by John Charles Hyland.
General Note: Typescript.
General Note: Vita.
 Record Information
Bibliographic ID: UF00082407
Volume ID: VID00001
Source Institution: University of Florida
Holding Location: University of Florida
Rights Management: All rights reserved by the source institution and holding location.
Resource Identifier: aleph - 001897852
oclc - 29860925
notis - AJX3149

Table of Contents
    Title Page
        Page i
        Page ii
    Table of Contents
        Page iii
        Page iv
        Page v
        Page vi
        Page 1
        Page 2
        Page 3
        Page 4
        Page 5
        Page 6
    Existing obstacle avoidance approaches
        Page 7
        Page 8
        Page 9
        Page 10
        Page 11
        Page 12
        Page 13
        Page 14
        Page 15
        Page 16
        Page 17
        Page 18
        Page 19
        Page 20
        Page 21
        Page 22
        Page 23
        Page 24
        Page 25
        Page 26
        Page 27
        Page 28
        Page 29
        Page 30
        Page 31
        Page 32
        Page 33
    Reflexive mine avoidance approach
        Page 34
        Page 35
        Page 36
        Page 37
        Page 38
        Page 39
        Page 40
        Page 41
        Page 42
        Page 43
        Page 44
        Page 45
        Page 46
        Page 47
        Page 48
        Page 49
        Page 50
        Page 51
        Page 52
        Page 53
        Page 54
        Page 55
        Page 56
        Page 57
        Page 58
        Page 59
        Page 60
        Page 61
        Page 62
        Page 63
        Page 64
        Page 65
        Page 66
        Page 67
        Page 68
        Page 69
        Page 70
        Page 71
        Page 72
        Page 73
        Page 74
    Estimating sonar target positions with an extended kalman filter
        Page 75
        Page 76
        Page 77
        Page 78
        Page 79
        Page 80
        Page 81
        Page 82
        Page 83
        Page 84
        Page 85
        Page 86
        Page 87
        Page 88
        Page 89
        Page 90
        Page 91
        Page 92
        Page 93
        Page 94
        Page 95
        Page 96
        Page 97
        Page 98
        Page 99
        Page 100
        Page 101
        Page 102
        Page 103
        Page 104
        Page 105
        Page 106
        Page 107
        Page 108
    Comparison of the reflexive mine avoidance approach with other mine avoidance approaches
        Page 109
        Page 110
        Page 111
        Page 112
        Page 113
        Page 114
        Page 115
        Page 116
        Page 117
        Page 118
        Page 119
        Page 120
        Page 121
        Page 122
        Page 123
        Page 124
        Page 125
        Page 126
        Page 127
        Page 128
        Page 129
        Page 130
        Page 131
        Page 132
        Page 133
        Page 134
        Page 135
        Page 136
        Page 137
        Page 138
        Page 139
        Page 140
        Page 141
        Page 142
        Page 143
        Page 144
        Page 145
        Page 146
        Page 147
        Page 148
        Page 149
        Page 150
        Page 151
        Page 152
        Page 153
        Page 154
        Page 155
        Page 156
        Page 157
        Page 158
        Page 159
        Page 160
        Page 161
        Page 162
        Page 163
        Page 164
        Page 165
        Page 166
        Page 167
        Page 168
        Page 169
        Page 170
        Page 171
        Page 172
        Page 173
        Page 174
        Page 175
        Page 176
        Page 177
        Page 178
        Page 179
        Page 180
        Page 181
        Page 182
        Page 183
        Page 184
        Page 185
        Page 186
        Page 187
        Page 188
        Page 189
        Page 190
        Page 191
        Page 192
        Page 193
        Page 194
        Page 195
        Page 196
        Page 197
        Page 198
        Page 199
        Page 200
        Page 201
        Page 202
        Page 203
        Page 204
        Page 205
        Page 206
        Page 207
        Page 208
        Page 209
        Page 210
        Page 211
        Page 212
        Page 213
        Page 214
        Page 215
        Page 216
        Page 217
        Page 218
        Page 219
        Page 220
        Page 221
        Page 222
        Page 223
        Page 224
        Page 225
        Page 226
        Page 227
        Page 228
        Page 229
        Page 230
        Page 231
        Page 232
        Page 233
        Page 234
        Page 235
        Page 236
        Page 237
        Page 238
        Page 239
        Page 240
        Page 241
        Page 242
        Page 243
        Page 244
        Page 245
        Page 246
        Page 247
        Page 248
        Page 249
        Page 250
        Page 251
        Page 252
        Page 253
        Page 254
        Page 255
        Page 256
        Page 257
        Page 258
        Page 259
        Page 260
        Page 261
        Page 262
        Page 263
        Page 264
        Page 265
        Page 266
        Page 267
        Page 268
        Page 269
    Biographical sketch
        Page 270
        Page 271
        Page 272
Full Text








I would like to express my sincere thanks to my wife, Joan, for her continued

support, patience and devotion and to Dr. Fox, Dr. Dobeck, Dr. Tacker and the Coastal

Systems Station for their encouragement and technical support. I would also like to thank

my supervisory committee chairman, Dr. Taylor, and my supervisory committee members,

Dr. Wilbur, Dr. Principe, Dr. Miller and Dr. Vining for their input and experienced



ACKNOWLEDGEMENTS ........................................ ii

ABSTRACT .................................................. v


1 INTRODUCTION ...................................... 1

Unmanned Underwater Vehicle Applications and
Research Trends ...........................,...... 1
Future AUV Missions and Likely Research Areas ............. 2
Purpose and Scope .................................. 5


Problem Definition and Technical Considerations ............. 7
Mine and Obstacle Avoidance Research Summary ............ 13


Overview ......................................... 34
Vehicles Dynamics and Navigation Error ................... 35
Energy-Based Sonar Model ............................ 37
Reflexive Mine Avoidance Logic ...................... 38
Local, Reflexive Mine Avoidance Logic ................... 48
Control M ode Structure ............................... 71

EXTENDED KALMAN FILTER ........................... 75

Introduction ...................................... 75
Continuous-Discrete Extended Kalman Filter ................ 76
Coordinate System and Sonar Functional
Relationships ..................................... 81

Target Position Estimation With No
Navigation Error .................................. 85
Required Modifications when Navigation
Error is Present ................................... 90
Associating New Measurements with Previously
Detected Targets ................................. 104
Sum m ary ........................................ 107


Overview ...................................... 109
Simulation Scenario and Modifications ................. .. 110
Simulated Minefield Scenarios ......................... 112
Comparison Summary ............................... 141


Conclusions ...................................... 143
Future Research Recommendations ...................... 145



B FORWARD LOOKING SONAR MODEL .................... 156

C SIMULATION RESULTS ............................... 186

REFERENCES .............................................. 259

BIOGRAPHICAL SKETCH ..................................... 270

Abstract of Dissertation Presented to the Graduate School
of the University of Florida in Partial Fulfillment of the
Requirements for the Degree of Doctor of Philosophy




May, 1993

Chairman: Fredrick J. Taylor
Major Department: Electrical Engineering

A three-dimensional reflexive mine avoidance capability for autonomous

underwater vehicles has been developed. This reflexive approach is a hybrid strategy

formed by combining a global path planner with a local swept volume method. This

combination offers the advantages of both approaches. The global path planner makes

effective use of both global and a priori mine position estimates to plan paths that are

safe and optimum with respect to known mine positions. The local swept volume method

then enhances the mine avoidance performance along the planned path by modifying the

vehicle trajectory as necessary when the sonar detects new mines.

By implementing the reflexive mine avoidance techniques, an underwater vehicle

equipped with an obstacle avoidance sonar and a navigation system can safely navigate

an unknown minefield. The mine avoidance techniques take into account the physical

limitations of the sonar and the navigation system, the maneuverability constraints on the

underwater vehicle and the required safe standoff distance from all mines. The mine

avoidance capability was tested extensively within a simulation environment that

accurately models the major difficulties associated with the sonar, the navigation system

and the vehicle dynamics. The different simulated minefield scenarios include mazes,

boxes, canyons and random configurations. In all scenarios, despite navigation errors and

limited sonar performance, the mine avoidance logic protects the vehicle by guiding it to

a predetermined end point and always maintaining at least a specified, minimum safe

standoff distance from each mine. Simulation results have also shown that the reflexive

mine avoidance method performs exceptionally well when compared to the merit function



Unmanned Underwater Vehicle Applications and Research Trends

Military and civilian interest in unmanned underwater vehicle (UUV) research has

steadily increased over the past few years. The research trend has steadily progressed

from unmanned submersibles, to remotely operated vehicles (ROVs), to fully autonomous

underwater vehicles (AUVs). Current ROV capabilities include deep water exploration

[1], deep underwater surveying [2] and search and salvage operations. In 1989, for

example, the GEMINI 6000 ROV recovered the cockpit voice recorder from the wreckage

of a South African Airway's 747 jumbo jet in 14,800 feet of sea water [3]. AUVs have

been used for deep water surveying, under-ice surveying [4], deep water exploration of

hydrothermal vents and tunnels [5] and as test platforms for ROV/AUV navigation,

guidance, control and communication subsystems [6].

Unlike ROVs, AUVs do not require a tether for control or power. Hence, they

operate independently from any parent craft and can execute a wider range of military

missions. As a result, the majority of the U.S. Navy's research and development (R&D)

expenditures in the UUV area have been on AUVs instead of ROVs. To date, total Navy

AUV R&D funding estimates range from $1 to $2 billion. And in 1990, 26 of the 36

different U.S. organizations involved with UUV research dealt exclusively with AUVs


[7]. Recent major AUV research areas are navigation [8], [9], guidance [10],

control [11], [12], [13], knowledge-based systems [14], [15], fault

recovery [16], three-dimensional environmental modeling [17], mission planning

[18], mission control [19], mission simulation [20], multiple AUV missions [21]

and power systems [22]. In 1988, a $23 million UUV program was initiated in which

a 36-foot long, 44-inch diameter underwater vehicle would be built and outfitted with

several tactical software packages including a mine search system package [23].

Table 1-1 summarizes AUV programs from 1963 through 1993 which actually proposed

new AUV design and fabrication [24]. Some of the programs never advanced past

the concept phase. Many are still in development while others have resulted in a fully

operational AUV.

Future AUV Missions and Likely Research Areas

AUVs are now being considered for a variety of future Navy missions because

they are cost effective, hard to detect and provide minimal risk to personnel and valuable

fleet assets [25]. A joint Navy-Marine Corps steering committee has recently

identified AUVs equipped with sensors and mine clearing devices as the far term solution

to shallow water mine countermeasures [26]. Recent research proposal requests by

Coastal Systems Station, a Navy research laboratory, for critical technology

demonstrations involving shallow-water mine countermeasures, mine detection and mine

avoidance confirm support of this new mission [27]. Another mission concept being

considered is multiple AUVs functioning as a coordinated unit to perform rapid mine

Table 1-1. AUV Program Summary

Year Vehicle Purpose (meters) Developer

1963 SPURV 1 water measurements 6,000 University of Washington Applied Physics Laboratory

1972 UARS under-ice mapping 457 UW/APL (USA)

1973 SPURV 2 water measurements 6,000 UW/APL (USA)

1975 SKAT ocean research n/a Shirshov Institute of Oceanology, USSR Academy of
Science (USSR)

OSR-V ocean research 250 Japan Society of Promotion Marine Industry (Japan)

1977 unnamed testbed 100 Japan Marine Science & Technology Center
(JAMSTEC) (Japan)

1979 EAVE III testbed 914 University of New Hampshire Marine Systems
Engineering Lab. (UNH/MSEL) (USA)

EAVE West testbed 610 Naval Ocean Systems Center (USA)

RUMIC mine countermeasures n/a Naval Coastal Systems Center (USA)

UFSS search 357 Naval Research Laboratory (USA)

1980 Pinguin Al search 200 MBB GmbH (Bremen, West Germany)

CSTV submarine control tests n/a Naval Coastal Systems Center (USA)

1982 Rover structure inspections 100 Heriot-Watt University (Edinburgh, Scotland)

Robot II bottom surveys 91 Massachusetts Institute of Technology (USA)

B-I drag characteristic studies 90 Naval Underwater Systems Center (USA)

1983 AUSS search 6,000 Naval Ocean Systems Center (USA)

Telemine vessel destruction 150 Teksea (Lugano, Switzerland)

TM 308 structure inspections 400 Tecnomare SpA (Venice, Italy)

Epaulard bottom photography/topography 6,000 IFREMER (Paris, France)

1984 ARCS under-ice mapping 400 Intl. Submarine Engineering Ltd. (Port Moody,B.C.

AUV hydrodynamic drag studies n/a Rockwell International (USA)

1985 Submarine testbed/hydrodynamic flow tests 500 JAMSTEC (Yokosuka, Japan)

PLA2 nodule collection 5,000 C.E.A./IFREMER (France)

1986 ELIT structure inspections 1,000 IFREMER/Comex (Paris, France)

unnamed feasibility study n/a Simrad Subsea A/S (Horten, Norway)

1987 LSV submarine testing n/a Naval Coastal Systems Center (USA)

Table 1-1.--continued

Year Vehicle Purpose (meters) Developer

1988 XP-21 testbed 610 Applied Remote Technology (San Diego, California,

MUST testbed 610 Martin Marietta Corp. (Baltimore, Maryland, USA)

Sea Squirt tested 61 M.I.T. Sea Grant (Cambridge, Massachusetts, USA)

RUV krill research 250 University of Washington (USA)

ACTV water measurements 250 UW/APL (USA)

1989 UUV(I) tested n/a Draper Laboratory (Cambridge, Massachusetts)

FSMNV mine neutralization n/a Naval Ocean Systems Center (USA)

MT-88 bottom/water surveys 6,000 Institute of Marine Technology Problems (Vladivostok,

PTEORA survey 6,000 Institute of Industrial Science (Tokyo, Japan)

Waterbird survey 100 Sasebo High Tech Company (Sasebo, Japan)

1990 UUV(II) tested n/a Draper Laboratory (Cambridge, Massachusetts, USA)

UROV-2000 bottom survey 2,000 Japan Marine Science & Technology Centre (Yokosuka,

unnamed testbed/precise control vehicle 10 Japan Marine Science & Technology Centre (Yokosuka,

MacAROV anti-submarine warfare target n/a SUTEC (Linkoping, Sweden)

1991 AROV search, mapping n/a SUTEC (Linkoping, Sweden)

Ocean testbed n/a Florida Atlantic University/Perry Offshore (USA)

1992 Mine tested n/a Lockheed Missiles & Space Co. (Sunnyvale, California,
Avoidance USA)

Doggie bottom/sub-bottom survey 6,000 Yard Ltd. (Glasgow, Scotland)

Dolphin temperature/salinity/depth 6,000 Yard Ltd. (Glasgow, Scotland)
monitoring for 30-day periods

1993 ARUS bottom survey n/a EUREKA (European consortium)

sweeping operations [28]. There has also been some speculation that smaller AUVs

could be covertly deployed directly from a submerged submarine's torpedo tube to

perform anti-submarine warfare (ASW) and other missions [29].


In addition to mine countermeasures, mine hunting and ASW, experts in the AUV

community believe other important future AUV missions will include covert surveillance,

tactical probing to locate and unmask defensive forces, weapon delivery [30], mine

laying, escorting parent vehicles through minefields and covert deployment of long range

standoff weapons [31]. A reliable mine avoidance capability has been identified as

a requirement for successful completion of all of these mission scenarios [32].

However, despite continuing research programs and renewed military interest, current

UUV and AUV capabilities still suffer from one fundamental limitation: they cannot

effectively navigate in the presence of unknown mines and obstacles. A reliable

mine/obstacle avoidance capability, then, is a critical link in the quest for true underwater

vehicle autonomy. It will greatly expand the AUV's role by realizing a critical functional

requirement necessary for future AUV missions.

Purpose and Scope

The purpose of this research is to develop a mine avoidance capability for AUVs.

Chapter 2 motivates the research by describing the mine/obstacle avoidance problem and

the associated difficulties. It also summarizes current mine/obstacle avoidance

approaches, their strengths and their weaknesses. Chapter 3 provides a detailed

explanation of the reflexive mine avoidance approach. Chapter 4 gives a thorough

account of an extended Kalman filtering algorithm used to track mine position estimates.

Chapter 5 provides a quantitative and qualitative comparison between the performance of

the reflexive mine avoidance logic and that of the merit function approach [33].


Appendix C verifies the mine avoidance capability by presenting simulation results of an

AUV, guided by the reflexive mine avoidance logic, navigating several unmapped

minefields. Finally, Chapter 6 summarizes the research and identifies areas of future



Problem Definition and Technical Considerations

Mine Avoidance Problem

A successful mine avoidance algorithm must guide an AUV through an unmapped

minefield to a specified end point such that the AUV maintains at least a minimum

prescribed standoff distance from all mines. For example, Figure 2-1 shows an AUV

escorting a surface vessel through an unmapped minefield; the circles around the mines

represent projections of spherical standoff distances or kill zones onto the sea floor. If

the mine avoidance algorithm fails and the AUV enters any of the standoff regions, the

mine destroys the AUV.

Technical Considerations

Overview. Mine avoidance for AUVs presents many difficult problems. First, the

AUV may have little initial information about the minefield; it must ascertain mine

position data from its sonar and navigation sensors. The sonar data, though, is plagued

by missed detections, false alarms, poor angular resolution, limited coverage, etc. Second,

error sources in the navigation system make it difficult to distinguish new mines from any

previously detected mines and also create an uncertainty in vehicle position with respect



Figure 2-1. AUV Escorting a Surface Vessel Through Minefield

to previously detected mines. And finally, since a computer on-board the AUV will

implement the mine avoidance algorithms, the resulting mine avoidance software must

execute in real-time. The specific, key properties necessary to overcome these problems

and implement an effective, realistic mine avoidance capability are discussed below in

three separate sections entitled, mine positions, sonar, navigation and tracking, and other

implementation issues. A final section summarizes by enumerating the necessary

properties of a successful mine avoidance capability.

Mine positions. In some instances, there may be complete a priori mine position

data while in others there may be little or none. When there is complete information, a

mine avoidance approach that considers the entire AUV path from start to finish is the


logical choice. However, when little position information is available such an approach

would waste valuable resources planning paths through unexplored areas; the resulting

paths would likely be of little value. The mine avoidance approach must handle either


Sonar. In the general case when the AUV has incomplete a priori mine position

data, the AUV discovers new mine positions only when its sonar detects a new mine.

The sonar, though, has limitations. Environmental and electronic noises affecting the

sonar give rise to false alarms and missed detections. The sonar cannot indicate that it

has detected a mine or that it has not detected a mine with absolute certainty.

Furthermore, the sonar has a finite scanning time; at a range of 1500 meters, for

example, it can provide periodic data at a maximum scan rate of 0.5 hertz. The mine

avoidance approach must address these sonar limitations (or the limitations of another

sensor, if used).

Another major limitation inherent to sonar is its limited field of view. This creates

a turn radius restriction for the AUV given by

TR = SOtotal (2-1)
mm 1-cos(0/2)


TRmn-minimum turn radius (feet) (2-2)

SO ,,,,-total desired standoff distance from center of mines (feet) (2-3)


0=total sonar angular bearing coverage (radians)


AUV movement as severely as does the turn radius restriction. Both restrictions,

however, must be addressed.

Navigation and tracking. AUVs employ on-board navigation systems to estimate

their position and orientation. Navigation system errors, though, make it impossible for

the AUV to know its position precisely. This makes it difficult for the AUV to track

mine positions because proper mine tracking must differentiate newly detected mines from

previously detected mines.

Other implementation issues. A few issues still remain for discussion. Since the

AUV will operate in a three-dimensional environment, the mine avoidance capability must

deal with three dimensions. Also, a computer on board the AUV will implement the

mine avoidance capability. Therefore, if this research is to progress into a demonstration

phase, the mine avoidance software must reside on such a computer and must execute in


Summary of mine avoidance requirements. Requirements necessary for a realistic

and practical mine avoidance capability can now be summarized. A mine avoidance

capability must

1) accommodate a priori mine position information,

2) make prudent use of global mine position information,

3) make prudent use of local mine position information,

4) properly track mine positions,

5) differentiate between newly and previously detected mines,

6) accept new mine position data,


Figure 2-2 demonstrates the problem. In the figure the dashed arc represents the

AUV's path and the four triangular figures represent four snapshots in time of the ideal

sonar coverage along the path. Note that although the AUV's path passes well within the

undetected obstacle's standoff distance, the sonar cannot detect the obstacle. The sonar's

coverage never encompasses it. This predicted phenomenon was observed in several two-

dimensional simulation runs [34].

F 1

Figure 2-2. Turn Radius Problem

Executing turns with radii larger than TRm,,, will ensure that this situation cannot

arise. Figure 2-3 shows such an example. Although the AUV does not detect the

obstacle, its path does not violate the standoff circle.

Vehicle Path

Standoff Distance

Undetected Obstacle

Vehicle's Path Intersects
with Standoff Region


Ideal Sonar Coverage


Standoff Distance
Vehicle's Path Just
Misses Standoff Region
Undetected Obstacle

S.. . ..........

Ideal Sonar Coverage
1 2

Figure 2-3. Solution to Turn Radius Problem

An analogous pitch radius restriction exists as well

PR .= SO, (2-5)
mm 1-cos(0/2)


PRm-=minimum pitch radius (feet) (2-6)


=--total sonar angular elevation coverage (radians) (2-7)

Equations (2-1) and (2-5) are only valid for a forward looking sonar system that

has symmetric elevation and bearing coverage about the AUV's centerline. Since AUVs

typically have pitch angle restrictions anyway, the pitch radius restriction does not impact


7) maintain the minimum prescribed standoff distance from all mines,

8) reduce effects of sensor inaccuracies (missed detections and false alarms),

9) compensate for sensor limitations (turn radius restriction/limited field of view),

10) be robust with respect to reasonable navigation errors,

11) consider vehicle maneuverability constraints,


12) operate in three dimensions.

In addition, the mine avoidance logic should

1) execute in real-time,


2) be capable of residing in a computer system on board the AUV.

Mine and Obstacle Avoidance Research Summary


To date, there has been little research directed specifically at the area of mine

avoidance for AUVs. However, a significant body of obstacle and collision avoidance

research has been produced in the robotics area. The general obstacle avoidance problem

for robotic vehicles naturally decomposes into three steps: curve planning, trajectory

planning and control implementation [35, p. 502]. Given a starting point, an ending

point and known obstacle positions, a curve planning procedure calculates a collision-free

curve in the robotic vehicle's n-dimensional space. The planned curve does not take the

robotic vehicle's dynamics into consideration. A trajectory planning procedure then


adjusts the planned curve according to the vehicle's maneuverability constraints. Finally,

a control implementation that makes the robot follow the planned trajectory must be

developed. The control problem is usually handled separately because it is highly vehicle


All of the various obstacle avoidance techniques use one of three basic methods

to implement this general approach. These methods are path planning, penalty functions

and swept volumes. Briefly summarizing the three basic obstacle avoidance techniques

and then examining some specific, significant approaches will lend insight into the mine

avoidance problem. It will also determine how well the existing methods can be applied

to mine avoidance.

Three Basic Approaches to Obstacle Avoidance

Path planning. The path planning approach uses obstacle position information to

divide the robot's n-dimensional space into safe and unsafe areas or nodes. The safe

nodes form a graph. A search strategy then searches the graph for a safe path. Pearl

provides a thorough discussion of the different search strategies available and their

properties [36].

If an optimal search strategy is used, the path planner will generate an optimal

collision-free path. Also, these approaches are easily implemented in three dimensions

and will converge to a solution if a safe path exists. However, path planning has some

major drawbacks. First, the technique can be extremely slow and can require enormous

amounts of computer memory storage thereby making a real-time implementation on

board an AUV quite difficult. Also, the approach is only as good as the available


obstacle position information For instance, if global obstacle positions are not available,

the planned paths cannot be globally optimum [37, p. 548]. As a result, path planning

methods perform best when working with complete obstacle position data. Unfortunately,

planning paths with incomplete obstacle information is still quite slow and memory

intensive. Executing path planning locally, over a small known region of the n-

dimensional space, escapes the problems inherent to incomplete information. It does not,

though, address the global path planning problem.

Penalty functions. Penalty function approaches mathematically quantify the cost

of the vehicle's position in relation to the obstacles and the end point throughout the

vehicle's space. The goal is to minimize the cost to reach the end point. Factors like

proximity to obstacles, speed, distance to the endpoint, range to the closest obstacle

detected by the sensors, etc., determine the cost. For example, in the potential field

penalty function approach the desired end point exhibits an attractive force on an object

moving through the obstacle field. The obstacles, on the other hand, exhibit a repulsive

force on the object that pushes it away; the negative force increases as the object moves

towards an obstacle and decreases as it moves away. This results in an overall gradient

vector field. To safely reach the end point, the vehicle just follows along the minimum

of the gradient. Frost provides a detailed account of the potential field approach [38].

Penalty functions are the fastest of the three obstacle avoidance approaches and

are easily implemented in real-time. Additionally, some penalty functions can be

designed to simultaneously implement all three parts of the general obstacle avoidance

problem (course planning, trajectory planning and control implementation). However,


they cannot distinguish between global minima and local minima; penalty functions'

overwhelming limitation is their susceptibility to local minima. When a robotic vehicle

guided by a penalty function hits a local minimum, it becomes trapped. Penalty functions

also suffer from incomplete obstacle position knowledge; the gradients are only as good

as the global information.

Subdividing penalty functions into global and local classes is difficult because of

the repulsive forces' mathematical description. For instance, a penalty function approach

may appear to be global but the repulsive forces often reach a constant zero value beyond

a small range from each obstacle. In effect the penalty functions behave as local ones.

But on the other hand, the attractive force remains in effect at all ranges.

Swept volume. In general, these methods use a number of geometrical approaches

to calculate how an object with a given shape can move through a field of obstacles

without touching any of the obstacles. Usually the method proceeds by proposing a

simple path, calculating the swept volume, checking for overlap, proposing a new path

and repeating until the path is safe. Points, circles, spheres, rectangles, convex polygons,

etc., typically approximate object and obstacle shapes.

The particular forms of the swept volume method frequently take both the object's

and the obstacles' orientations into account. And when the method does converge, it

usually does so very fast. However, the swept volume method often fails to converge on

a solution when obstacle density is moderately high. And like path planning and penalty

functions, the solutions rely on the completeness of obstacle position knowledge.


Swept volume methods are easily subdivided into global and local methods. The

local methods typically use only current sensor data, while global methods either assume

complete a priori knowledge or remember obstacle positions after the vehicle's sensor

detects them. However, the swept volume method's proposed path provides only local

information about possible collisions. Using local collision information to change the

path does not determine if a substantially different path would be better [39, p. 560].

Neural Networks. Neural networks form the basis of a number of obstacle

avoidance approaches (described later). Upon closer scrutiny, though, the neural networks

implement one of the three basic obstacle avoidance approaches. The neural networks

merely provide a convenient, simple functional implementation.

Specific Obstacle Avoidance Approaches

Range-based robot navigation. In 1975, Cahn and Phillips developed the

ROBNAV algorithm [40]. ROBNAV is a two-dimensional local penalty function

approach that continually guides a mobile robot through an obstacle filled room. A range

sensor divides the field of view into equi-angular sections and determines the range to any

obstacles. The algorithm only reacts to the closest obstacle in each angular section.

ROBNAV's penalty function is quickly computed and requires little memory.

However, the approach only uses current sensor information (does not remember where

obstacles are located). In two-dimensional simulations ROBNAV exhibits the local

minimum problem by going into a turning limit cycle, making 90 degree turns continually

and never escaping deep channels. In sparse obstacle field scenarios ROBNAV performs



Polyhedral obstacles. In 1979, Lozano-Perez and Wesley describe an algorithm

that plans collision-free paths for a polyhedral objects moving through a known set of

polyhedral obstacles in a "configuration space" [41]. It was developed for a robotic

arm. Their algorithm "expands" the obstacles by the size of the object thereby creating

"grown" obstacles. The expansion takes into account the object orientation; a rectangular

object will cause a different obstacle expansion when it is rotated 90 degrees. Additional

obstacle vertices are then introduced along the obstacles' edges. A search strategy

searches for a safe path along the vertices. The optimal A* search strategy is

recommended [42].

The polyhedral obstacle method offers some advantages by taking into account

object and obstacle shapes. It only works with a prior obstacles, though, and does not

accommodate any sensors. Three-dimensional path planning is quite slow. Simulation

and demonstration results show that the method works for a robotic manipulator with a

limited number of obstacles.

Generalized cylinders. Brooks developed a swept volume method in 1983 that

uses convex polygons to define safe areas or "generalized cylinders" [43]. The

method bisects all of the safe regions and determines the bisectors' intersection points,

called nodes. At each node the procedure calculates the orientation angles which allow

the object to safely move to the next node. By using an A* search, Brooks then

calculates the optimum path through the safe nodes.

Brook's method is computationally fast and handles complex object and obstacle

shapes. Unfortunately, it was developed for only two dimensions, requires complete a


priori obstacle positions and does not provide for sensory input. The method also does

not work well in an obstacle-dense environment. For the case of a few sparsely spaced

obstacles, Brooks' generalized cylinders may guide the object much further from the

obstacles than necessary [44]. Kuan, Brooks, Zamski and Das use this same method

with two different path planning strategies in what they refer to as a "mixed

representation of free space" [45].

Stanford Cart. The Stanford Cart, developed by Moravec in 1983, is remote

controlled mobile robot equipped with a television camera [46]. The cart uses camera

images to form an environmental world model. It models all detected obstacles as circles.

It calculates the four paths between the tangent points of each pair of obstacles. A graph

searching algorithm uses each tangent point as a node vertex and computes the optimum

path between vertices; arcs are used to connect the four nodes on a circle. In

demonstrations the cart lurches forward three feet, scans the area with the camera, replans

the path, and repeats the process until it reaches the end point.

The television camera sensor overcomes the incomplete obstacle position problem

and the periodic path planning alleviates the poor local obstacle avoidance properties

inherent to global path planning. However, the path planning is extremely slow; the

Stanford Cart required five hours to negotiate a 20-meter long course. Furthermore,

positions errors caused the system to replicate the same obstacles (at different positions)

in the system's memory thereby incorrectly perceiving some pathways as blocked. The

algorithm was not always successful. And although the cart performed an actual

demonstration, the algorithm is essentially two-dimensional.


In related work, Frohn and Seelen developed VISOCAR, an autonomous industrial

transport vehicle guided by visual navigation [47]. Because VISOCAR was designed

exclusively for a well structured, industrial environment, it has no AUV mine avoidance


Rotation mapping graph. In 1984, Chien et. al introduce the idea of a rotation

mapping graph (RMG) to plan collision-free paths for a robotic arm among obstacles

[48]. The RMG is made by first subdividing a two-dimensional space into regions

and then determining what areas, measured as angles from each region's center, are

collision-free. The collision-free regions of adjacent regions are merged by taking the

intersections of clear paths. Drawing the connected region creates the overall connected

graph network which, in theory, can be easily solved with existing techniques. Their

explanation shows an example but provides no details of how to solve the connected

graph problem.

Generalized potential field. Krogh implemented a generalized potential field

obstacle avoidance approach in 1984 for a point object moving through a two-dimensional

space containing convex polygons [49]. Krogh's technique expands the penalty

function approach by including the object's velocity in the potential fields. The approach

was tested in a two-dimensional simulation with rectangular obstacles. Krogh's method

has the typical advantages and disadvantages inherent to global penalty functions.

Path relaxation. Thorpe improved the global path planning procedure by

incorporating "path relaxation" [50]. After planning a global path with a grid

searching strategy, Thorpe adjusts the grid node locations to lower the overall path cost.


Although this idea is analogous to path planning followed by trajectory planning, the

major contribution by Thorpe is his consideration of obstacle avoidance constraints. In

this paper, Thorpe discusses-the difficulties caused by limited vehicle maneuverability,

imprecise control, cumulative navigation error, unmapped areas and grid size. Most other

obstacle avoidance approaches ignore many of these considerations.

A simulation example using actual data from a video system illustrates that the

path relaxation approach handles the considerations by moving the graph nodes further

from the obstacles. Note, though, that the technique is two-dimensional. Moving the

nodes further and further away could also create problems when the planned path is not


Distance functions. Gilbert and Johnson formulated the obstacle avoidance

problem for a robotic manipulator as an optimal control problem [51]. The distances

between the moving object and the stationary obstacles are the major cost function

components. By modeling the object and obstacles as convex polygons, the distance

functions become continuously differentiable.

Their problem development is presented in three dimensions but their simulation

results are in two dimensions. The approach has the usual global penalty function

problems. The cost functions are particularly advanced because they take into account

object orientation, movement and rotations.

Intelligent mobile robot. In 1985, Crowley combined a global path planner with

a local path planner for robot navigation [52]. Using his techniques, a mobile robot

equipped with a rotating range sensor and touch sensors explores and maps out its


environment by entering an active learning mode. It then generates a global planned path

using Dijkstra's algorithm [53]. Afterwards, a "composite local model" integrates the

sensor information with the-planned path. The approach circumvents newly detected

obstacles by performing local path planning around the obstacles.

Conceptually, Crowley's method is extremely valuable because it addresses both

the local and global path planning problems simultaneously. It also accommodates new

sensor data and employs an ad hoc method to match sensor data with previously detected

obstacles. Crowley's approach is not optimal and it was simulated in two dimensions


Clash detection problem. In 1985, Cameron summarized three "clash detection"

approaches for a robotic arm: multiple interference detection, four-dimensional

interference detection and sweeping [54]. Cameron describes multiple interference

detection as periodically sampling points on obstacles to determine if they intersect. The

difficulty in this method is choosing the discrete sampling time. The four-dimensional

intersection detection has a different emphasis. This approach uses four-dimensional (x,

y, z, and time) mathematical descriptions of the obstacles and object. Mathematical

formulae solve the intersection problem directly. He summarizes sweeping as using a

three-dimensional object to create or sweep out a three-dimensional volume.

Mathematical tests find intersections between the three-dimensional volume and objects.

Since this work is mainly a summary of these three implementations, the summary table

at the end of this section does not contain the work.


Autonomous land vehicle. Nitao and Parodi developed an obstacle avoidance

autopilot for an autonomous land vehicle (an M113A2 armored personnel carrier) and

demonstrated that it could safely guide the M113A2 through a sparse obstacle course at

a speed of five miles per hour [55]. Oil drums and a large bale of hay comprised the

obstacles. Their approach guides the vehicle as closely as possible on a planned path

which is based on a digitized map of a priori terrain and elevation features. A sonic

imaging sensor with a range of 32 miles is then used to detect any obstacles that were

unknown when the planned path was created. The autopilot then combines the planned

path, the a priori terrain and elevation data, and the sonic imaging sensor data to generate

an obstacle-free trajectory. The autopilot negotiates newly detected obstacles by

employing an edge following approach. More specifically, when the vehicle encounters

an obstacle like a wall, the autopilot goes around it by following along the wall's edge

until the wall no longer blocks the path. In an actual field test, the M113A2 successfully

navigated a limited obstacle field.

Edge following adapts the path once new obstacles are detected. However, special

provisions must be made so that a vehicle using this algorithm does not become trapped

in deeply concave obstacles. These provisions usually fail in a maze-like obstacle

configuration or when two or more deeply concave obstacles are nested together.

Intersecting convex polygons. Singh and Wagh modify the global path planning

approach by breaking up the two-dimensional grid space into a series of non-uniform

rectangular regions [56]. Object size determines the size of the rectangular regions.

That is, the method sizes the grids in an area according to the size of the obstacles.


Graph searching then calculates a safe path through the grids. This solves the tradeoff

between using fine or course grids. However, it creates problems with the graph

searching procedure in that -the cost to move from one node to another node is not

uniform. The method is only two-dimensional and does not allow sensory input.

Artificial potential field. Khatib uses an artificial potential field approach to guide

a robotic arm through an array of obstacles [57]. Analytical equations describe both

the object and the obstacles. The obstacles exhibit a repulsive force only in a small

region immediately around themselves. Khatib states that real-time obstacle avoidance

demonstrations have been performed by his system using a visual sensor. However, the

paper gives no details of the demonstration.

This approach has the usual problems and benefits of penalty functions. Khatib

does propose integrating a high level, global path planning strategy with his low level

potential field approach to overcome any limitations.

Sonar-based navigation and mapping. Elfes developed a sonar-based mapping and

navigation system for a mobile robot [58]. The sonar mapping system statistically

combines the sonar returns from an ultrasonic sonar system to generate a two-dimensional

occupancy probability map. The probability values in each grid are the probability that

the grid is occupied. Afterwards, the navigation system uses an A* based path planner

to calculate the best path through the grid. The system successfully navigated a 10-foot

by 25-foot indoor area covered with chairs, tables, boxes, workstations and filing cabinets

and a 50-foot by 30-foot outdoor area covered with trees.


Elfes's approach has the typical advantages and disadvantages of a global path

planning method. Taking into account the probability of occupancy for each grid is a

distinctive feature of this approach. However, Elfes' sensor processing architecture does

not take the sonar's angular width or navigation error into account

Learned visibility graph. Oommen et al. present an algorithm that guides a mobile

robot through an unexplored terrain arbitrarily populated with disjoint convex polygon

obstacles [59]. Their approach combines a global path planning procedure with a

local obstacle avoidance approach called a "learned visibility graph." The "learned

visibility graph" is a combination of hill climbing and edge following. The technique,

however, fails to converge in moderately restricted areas. The authors state that their

approach learns or maps out an area with its sensors but they provide few details about

the sensors. Also, the approach is two-dimensional.

AUV obstacle avoidance. Hyland developed a two-dimensional obstacle avoidance

algorithm specifically for AUVs outfitted with an obstacle avoidance sonar [60]. The

algorithm is unique in that it is the first obstacle avoidance algorithm to detect stationary

obstacles with a sonar and then track then with an extended Kalman filter.

The method exhibits the typical swept volume difficulties. Simulation results

show the AUV effectively navigates several minefields. However, the method is only

two-dimensional and the AUV could become trapped in certain box canyon situations.

Collision detection. Gilbert and Hong present a numerical algorithm that detects

whether or not two convex polygons, each following a specified path, will collide

[61]. If a collision will take place, the collision point is given. The algorithm was

simulated for two robotic arms.

Since the approach only works for one object and one obstacle, it is not well

suited to AUV obstacle avoidance. By combining it with another approach, though, it

could have some obstacle avoidance uses.

Unstructured environment navigation. Kue and Barshan showed that a mobile

robot equipped with a range sensor could safely travel through a room filled with

obstacles [62]. Their procedure divides the two-dimensional floor space into grids

and uses a range sensor to detect obstacles and classify grids as blocked or not blocked.

A local swept volume method decides whether or not the grid is safe. Their indoor

demonstration indicated that the robot could navigate through a cluttered office without

bumping into any obstacles. Their unstructured environment navigation routine is

essentially two-dimensional, is not global and does not consider navigation error.

AUV route planning. Warren modified the potential field approach so that it was

less susceptible to local minima [63]. Warren first establishes a trial path through the

obstacle field. Afterwards, he modifies the entire path, rather than the local paths,

according to the potential field's influence. The method was developed for mobile robots

and robotic manipulators.

The method does not converge well in highly cluttered environments. However,

it is extremely fast and the divergence is easily detected. In 1990, Warren extended this

approach to AUVs [64].


Optimal AUV obstacle avoidance. Hyland expanded his earlier two-dimensional

work to three dimensions, added a global dynamic programming based path planner to

the local swept volume method and enhanced the sonar model [65]. This eliminated

the problem of the AUV becoming trapped in certain box canyon obstacle fields. In

1990, Hyland and Fox improved validation and testing of the approach by including a

navigation error model in the vehicle simulation and actual receiver and projector beam

patterns in the sonar model [66]. In 1993, Hyland and Taylor provided a detailed,

comprehensive description of the mine avoidance approach [67]. They also compared

two different path planners, the optimal A* and the suboptimal breadth-first [68], in

terms of execution speed and distance of planned path.

Neural network. A neural network-based local swept volume method was

developed by Springsteen and DeMuth for AUVs [69]. The system uses current

forward looking sonar data in the form of distance and depth from azimuth.

The neural network was only tested in one and two obstacle scenarios. The AUV

cut corners rather closely around obstacles because it only uses current sonar data;

without obstacle tracking and sonar peripheral vision, the neural network does not realize

that the obstacles are still present once the AUV's sonar no longer detects them.

Merit function. Williams, Lagace and Woodfin created a merit function approach

to obstacle avoidance by augmenting a potential field approach with a "visit counter"

heuristic. This approach is designed for an AUV outfitted with a sonar and is based on

Hyland's two-dimensional work [70]. The "visit counter" reduces the susceptibility

to local minima by increasing an obstacle's repulsive force according to how many times


the sonar detects it; this tends to drive the AUV away from certain areas in which it

would normally become trapped. Simulation results support this claim.

The merit function tracks both stationary and moving obstacles. Although the

development is in three dimensions, the simulation results are in two.

Collision detector. Schaffer and Herb developed a real-time collision avoidance

safety system for a computer simulated robot arm [71]. The arm is modelled as set

of cylinders and spheres. Their safety system employs an octree structure that continually

subdivides an octree region until each new region contains no more than a predetermined

number of obstacles. Each time an octree node changes size or contents, the system

checks for possible collisions. By only checking for collisions in updated octree nodes,

the approach can be very fast when the robot arm's position changes slowly. Details of

how the collision detector works, though, are not given.

The use of cylinders and spheres permits fairly complicated obstacles to be

represented. Also, the octree structure is extremely efficient in terms of memory

requirements. However, because the system only accepts a priori obstacle positions, the

path cannot progress if new obstacles are found.

Harmonic potential fields. Kim and Kholsa use harmonic potential fields to

eliminate the local minima problem associated with traditional potential field approaches

[72]. A series of panels form polyhedrons that model the obstacles. Each panel has

an associated harmonic function that repels a mobile robot traveling through the obstacle



Despite their efforts, the harmonic potential field still exhibits local minima

problems for mobile robots that cannot be approximated by a point. The local minima

appear as stagnation points where the robot cannot move. Furthermore, the approach

assumes complete knowledge of obstacle positions. The algorithm development in the

paper is three-dimensional while the simulation results are two-dimensional.

Navigation functions. Rimon and Koditschek introduce "navigation functions" as

a restricted class of potential field functions [73]. When given a point object and

known circular obstacles, the navigation functions create potential fields that are free from

local minima. The navigation functions take into full consideration any torque limitations

of the robotic manipulator for which it they were developed. Two-dimensional

simulations used the navigation functions to calculate gradients that are free from local

minima and that lead to the goal.

Although there are no local minima in this approach, the calculated gradients often

hug the obstacle boundaries too tightly. This, in turn, causes the gradient vector fields

to vary too abruptly to be implemented in a practical setting.

Other methods and related research. Other obstacle avoidance methods in the

literature are quite similar to one or more of the methods previously described. Assorted

path planning methods include timing investigations into three-dimensional searching

methods by Bonsignore [74]; heuristic searching by Ong [75]; Wong and Fu's

two-dimensional orthogonal space planning [76]; Bien and Lee's time-optimal dual

robot control [77]; Shin and Zheng's multiple robot work [78]; Wilfong's


consideration of steering angle limitations when planning paths [79]; and Richbourg's

performance study of different two-dimensional path planning techniques [80].

Other penalty function methods have been investigated by Langley Research

Center [81], The Naval Research Laboratory [82] and the U.S. Army [83].

Smith et al. developed a rudimentary swept volume method [84] and Donald uses a

C-Voranoi diagram for solving the motion planning problem [85]. Neural network

approaches have been studied by Norton [86].

Several researchers have developed hybrid methods. Herman combines "hill-

climbing, generate and test, A* and octrees" [87]. Both Iyengar et al. [88] and

Keirsey et al. [89] use global path planning with local swept volumes. Krogh and

Thorpe merge global path planning with local penalty functions [90].

Spouge [91], Witkowski [92], Meystel et al. [93], and Richbourg et

al. [94] have been developing ways to speed up dynamic programming and path

planning methods. Other efforts towards finding better ways to utilize and store sensor

data [95] and integrating sensor data with navigation data are underway [96].

Summary of Obstacle Avoidance Methods

Table 2-1 summarizes the obstacle avoidance method discussion. Because none

of the techniques described satisfy all of the mine avoidance requirements previously

listed, no one method can be directly applied to AUV mine avoidance without significant

improvements. Nevertheless, the discussion leads to several observations.

Table 2-1. Obstacle Avoidance Research Summary

Date Developer/ Algorithm Dimension Object Obstacle Accepts Accepts Validation/
Method Type Shape Shapes A Priori Sensor Testing
Information Data

9/75 Cahn & Phillips/ Local 2-d Rectangle Polygons No Yes Simulation
Range-Based Penalty (Range
ROBNAV Function Sensor)

10/79 Lozano-Perez & Global 3-d Point Polhedrons Yes No Simulation
Welsey/ Path (Required) & Lab.
Polyhedral Planning Demo.

4/83 Brooks/ Global 2-d Polygon Polygons Yes No Simulation
Generalized Path (Required)
Cylinders Planning

7/83 Moravec/ Global 2-d Circle Circles Yes Yes Simulation
Stanford Cart Path (TV & Field
Planning Camera) Demo.

1/84 Chien, Zhang & Global 2-d Circle Polygons Yes No Numerical
Zhang/Rotation Swept (Required) Examples
Mapping Graph Volume

8/84 Krogh/ Global 2-d Point Convex Yes No Simulation
Generalized Penalty Polygons (Required)
Potential Field Function

8/84 Thorpe/ Path Global 2-d Point Circles Yes Yes Simulation
Relaxation Path (Video Using Real
planning Images) Video Data

2/85 Gilbert & Global 3-d Convex Convex Yes No Simulation
Johnson/ Penalty Polygon Polygons (Required) (2-d only)
Distance Function

2/85 Crowley/ Global 2-d Point Polygons Yes Yes (Ultra Simulation
Intelligent Path sonic
Mobile Robot Planning Range
& Local Sensor &
Path Touch
Planning Sensors)

2/86 Nitao & Parodi/ Global 2-d Point Polygons Yes Yes Simulation
Autonomous Path (3-d (Required) (Utira- & Field
Land Vehicle Planning world) sonic Demo.
& Local Imaging
Penalty Sensor)

2/86 Singh & Wagh/ Global 2-d Point Convex Yes No Simulation
Intersecting Path Polygon (Required)
Convex Shapes Planning

4/86 Khatib/ Artificial Local 3-d Analytic Analytic Yes Yes Lab. Demo.
Potential Field Penalty Equation Equation (Required) (Vision
Function System)

6/87 Elfes/ Sonar- Global 2-d Grid Grid Yes Yes Simulation.
Based Mapping Path Square Squares (Ultra- Indoor &
& Navigation Planning sonic Outdoor
Sonar) Demos.

Table 2-1.--continued

Date Developer/ Algorithm Dimension Object Obstacle Accepts Accepts Validation/
Method Type Shape Shapes A Priori Sensor Testing
Information Data

12/87 Oommen, Global 2-d Convex Convex No Yes Example
lyengar, Rao & Path Polygon Polygons (Few
Kashyap/ Planning Details)
Learned & Swept
Visibility Graphs Volume

11/88 Hyland/ AUV Local 2-d Circle Circles Yes Yes Simulation
Obstacle Swept (Sonar)
Avoidance Volume

5/89 Gilbert & Hung/ Local 3-d Convex Convex Yes No Simulation
Collision Swept Polygon Polygons (Only One)
Detection Volume

5/89 Kue & Barshan/ Local 2-d Circle Grid No Yes Indoor
Unstructured Swept Square (Sonic Demo.
Environment Volume Range
Navigation Sensor)

5/89 Warren/ AUV Global 3-d Point Poly- Yes Yes Simulation
Route Planning Penalty hedron (No (2-d Only)
Function Details)

6/89 Hyland/ Optimal Global 3-d Point Spheres Yes Yes Simulation
AUV Obstacle Penalty (Sonar)
Avoidance Function
& Local

6/89 Sciller & Tench/ Neural 2-d Details Polygons No Yes Simulation
Neural Network- Network Unknown (Sonar) (Limited)
Based AUV (Local
Guidance Swept

6/90 DeMuth & Neural 2-d Polygons Polygons No Yes Simulation
Springsteen/ Network (Sonar)
Neural Network (Local
Obstacle Swept
Avoidance Volume)

6/90 Williams, Global 3-d Sphere Spheres Yes Yes Simulation
Lagrace & Penalty (Sonar) (2-d only)
Woodfin/ Merit Function

4/92 Schaffer & Herb/ Global 3-d Cylinder Cylinder, Yes No Simulation
Robot Arm Path & Sphere Sphere & (Required)
Collision Planning Composite Rectangle
Detector Composite

6/92 Kim & Kholsa/ Global 2-d Point Polygons Yes No Simulation
Harmonic Penalty (Required)
Potential Fields Function

9/92 Rimon & Global 2-d Point Circles Yes No Simulation
Koditschek/ Penalty (Required)
Navigation Function
Function ____


First, the simulation and demonstration results described in the literature support

the stated advantages and disadvantages of each method. Second, as one would expect,

all three basic approaches perform only as well as the available obstacle position

information. And third, the hybrid methods formed by combining two of the methods out

perform any one approach. Developing a hybrid approach is the logical choice for AUV

Mine Avoidance.



Considering the technical difficulties associated with mine avoidance and the host

of particular obstacle avoidance approaches, a new reflexive mine avoidance approach has

been developed. The reflexive approach combines a global path planner with a local,

reflexive swept volume method. This combination offers definite advantages. Global

path planning makes effective use of both global and a priori mine positions. The local,

reflexive swept volume method makes prudent use of local mine position data and when

properly designed, exhibits robust behavior with respect to reasonable navigation error.

The remaining requirements enumerated in Chapter 2 have been addressed by separating

the mine avoidance approach into three distinct parts: tracking mine positions, planning

paths through the estimated minefield and avoiding remembered mine positions. Figure

3-1 shows a high level block diagram of this structure. Note that the mine avoidance

logic also requires sonar data and vehicle position information. Hence, testing the mine

avoidance capability in a simulation environment requires models for the sonar, the

vehicle dynamics and the inertial navigation system (INS).


r -----------------------------
Pre-processed sonar data

Mine INS Model
Position Comsaded
Tr scking p Local heading.
WIlaan Filter Esti ted Reflexive speed,
Mile mine pitch. etc.
Pitio Positions Avoidance
-- Planned
Recollection path

I -- Dynsamic
Vehicle Based
ftatus Path Plsaner


Figure 3-1. Block Diagram Overview

This chapter begins by describing models for the vehicle dynamics, the navigation

system and the sonar. It then provides a complete description of the reflexive mine

avoidance logic and discusses how the approach satisfies the remaining requirements in

Chapter 2.

Vehicles Dynamics and Navigation Error

Vehicle Dynamic Model

The vehicle dynamic model simulates the dynamics of an underwater vehicle.

Currently, the model is tuned to correspond to the aforementioned Control System Test

Vehicle: a 30 foot long, fully autonomous 1/12-scale model of a LOS ANGELES class

submarine [97, pp. 731-733]. The dynamic model is relatively simple. Using

commanded heading, pitch and roll as inputs, second order difference equations propagate


true heading, pitch and roll from the current vehicle state at time t to the next vehicle

state at time t + dt. Because the resultant angular velocities, angular accelerations, turn

diameters and vehicle positions simulate those of an actual AUV, the dynamic model

lends itself to a more thorough testing of the mine avoidance logic. Also, the model's

ease of implementation allows other vehicles to be readily tested in simulation.

Navigation Error

Rigorous testing of the mine avoidance techniques dictates that navigation errors

be included in the simulation. Therefore, a Doppler/INS model has been incorporated

into the simulation. Although the simulation does not specifically model all of the

dynamics associated with the Doppler/INS system, it does model its major sources of

error. The modeled error sources include

1. A 1.0% scale factor error for the Doppler/INS.

2. A 1.0% scale factor error for the depth sensor.

3. A fixed 0.3 degree uncompensated heading bias.

4. Random noise on roll, pitch and yaw (uncorrelated and normally

distributed with zero mean and 0.12 variance (radians2)).

5. Random Noise on depth (uncorrelated and normally distributed with

zero mean and 0.252 variance feett2).


6. Random noise on Doppler velocity (uncorrelated and normally

distributed with zero mean and 0.012 variance (feet/second)2).

Many currently available Doppler/INS systems have better performance

characteristics than this model. Conservatively, then, this model provides realistic

navigation data for the mine avoidance logic. Appendix A provides a detailed description

of the vehicle dynamic model and the Doppler/INS model.

Energy-Based Sonar Model

Considerable effort has been spent on developing a realistic sonar model to drive

the mine avoidance simulation. The resulting energy-based sonar model contains both

sufficient detail to accurately represent the sonar's first order characteristics and enough

flexibility to evaluate virtually any forward-looking sonar configuration. The sonar

model's superior performance and novel design have made it the basis for a side scan

sonar model that generates highly realistic simulated side scan sonar images [98],


The energy-based approach uses the standard sonar equations for surface, bottom

and volume reverberation; thermal, ambient and flow noises; two-way transmission loss;

directivity index; etc. Additionally, the sonar simulation pre-computes many of these

sonar equations and stores the results in look up tables. This makes the sonar

simulation's execution speed relatively fast for the exceptional level of detail contained

in the model. The sonar model also contains actual receiver and projector beam patterns,

complete with side lobes. The signal-to-noise ratios (SNRs) produced, in turn, yield

realistic probabilities of detection (that vary with range and angle) and a constant false


alarm rate. The generated sonar data includes the uncertainties caused by the coarse

angular resolution and overlapping beams typical of forward-looking sonars.

Input parameters to the sonar model, such as environmental conditions, side lobe

level, number of beams, source level, beam patterns, etc., enable a variety of sonar

configurations to be tested in a wide range of environmental conditions. The research has

focused primarily on Sonatech Inc.'s Terrain and Obstacle Avoidance Sonar (TOAS)

which is a three-row by five-column beam configuration that encompasses a total of 33

degrees by 55 degrees [100]. Finally, the sonar model incorporates some data

pre-processing. Before the sonar conveys any sonar data to the mine avoidance logic, the

pre-processing procedure threshold-detects the data; only returns above a specified

detection threshold are categorized as detections and subsequently passed on to the mine

avoidance logic. Appendix B gives a thorough account of the energy-based sonar model.

Reflexive Mine Avoidance Logic


The mine avoidance logic is divided into three parts: (1) mine position tracking

(MPT); (2) global path planning (GPP); and (3) local reflexive mine avoidance (LRMA).

Estimating and remembering mine positions greatly reduces the missed detection problem.

Once the sonar detects a target, it remembers its position. The path planning portion then

uses the estimated mine positions to plan a safe path through the minefield. Finally, the

LRMA logic, which is based on a local swept volume method, guides the AUV along the

planned path. When newly encountered targets obstruct the planned path, the LRMA


logic maneuvers the AUV around them and ensures that the AUV maintains at least the

minimum prescribed safe standoff distance.

Target Tracking, Target Size and Shape and Clustering Sonar Data

Target tracking. The sonar's characteristics give rise to false alarms and missed

detections. Overlapping beams and side lobes cause extraneous sonar detections while

navigation errors make exact mine position estimation impossible. Unfortunately, the

sonar cannot simultaneously maximize detections and minimize false alarms. There is

an inverse relationship between the two. To complicate the problem further, some

method to associate new sonar measurements with previously detected sonar targets is

also required. The mine position tracking logic has been designed to make this


False alarm reduction is achieved by setting the detection threshold sufficiently

high. Thus, when the sonar indicates a detection, the probability is high that it is in fact

a target and the probability is low that it is a false alarm. To reduce the effects of a low

probability of detection that result from a high detection threshold, the MPT logic

remembers the mine positions once the sonar detects them.

When the sonar indicates a target, an extended Kalman filtering algorithm

processes the sonar data and forms a statistical measure of how well each of the

previously detected mine position estimates matches the currently detected mine. Chapter

4 discusses the details of the Kalman filtering algorithm and the procedure that matches

sonar data with previously detected mines. If the current detection is not close enough

to any previously detected mine, the procedure adds a new position estimate and


covariance matrix to its table and initializes a new detection counter to one. If the

currently detected mine can be matched to a previously detected mine, the extended

Kalman filter uses the new position information to refine the prior position estimate. The

procedure also increments a corresponding detection counter. The mine avoidance logic

ignores mine position estimates until their corresponding detection counter reaches a

specified detection counter limit, MinDetectCount. In this manner, the mine tracking

logic continually improves its mine position estimates, maintains the position estimates'

accuracies and keeps count of how many times each mine has been detected. This

procedure sequentially matches the sonar measurements with the most likely mine

position estimates.

Target size and shape and clustering sonar data. Remembering complex target shapes in

three dimensions is impractical and not necessary for successful mine avoidance. The

mine position estimation logic only needs to remember where the centers of the mines are

located. Additionally, the estimation logic structure permits it to accommodate

non-mine-like targets as well. Large obstructions such as reefs and unusual underwater

terrain are broken down and remembered as many smaller objects; the logic organizes

the sonar detections so that each target is remembered as a sphere with radius no larger

than a predetermined limit, Maxsize. Furthermore, if these objects can be categorized as

mine-like or non-mine-like by some external computer aided classification routine, the

mine avoidance logic can associate the correct standoff distance to each sonar target.

Actual sonar systems measure the returned energy in each of the sonar's sampled

range cells. This forms a three-dimensional array of sonar data. Two of the array


dimensions identify the sonar beam row and column number and the third identifies range

cell number. Actual target range is easily calculated from range cell number and range

cell spacing. However, this data is not in the proper format for the mine tracking

algorithms. A sonar pre-processing algorithm first reduces the data by threshold-detecting

all of the sonar data after each sonar ping cycle. Energy levels above a specified

detection threshold are categorized as detections and subsequently passed on to a

clustering routine. Figure 3-2 shows what a threshold-detected sonar display might look

like. The narrow arcs in the figure separate the range sample cells. The dark cells

represent cells that contain returned energy levels above the detection threshold.

Head 1 ng

Figure 3-2. Threshold Detected Sonar Display Example

The sonar could detect a target in many contiguous range cells. For instance, if

a mine is six feet long and the sonar's range sample cell is one-half a foot long, the sonar

could detect the mine in up to twelve contiguous range sample cells. Or if two or more


mines were very close, the sonar might indicate an even longer series of contiguous range

cells with detections. In another situation, a single mine could be simultaneously detected

in multiple sonar beams. In these instances, should the range cells be interpreted as a

single target or a group of targets?

The clustering approach recognizes that in such an instance, the sonar cannot

discriminate between a single target and multiple targets. Furthermore, the sonar cannot

resolve any target's angular width to a finer degree than receiving beam's angular width.

To attempt to resolve a target size to a finer degree would be inconsistent with the sonar's

capabilities. Therefore, the clustering logic groups all contiguous range cell detections

within a receiver beam into a single target. There is, however, a maximum number of

range cells, Maxlink, that can be grouped into a single target. Contiguous cells longer

than Maxsize are broken down into multiple tracked targets. Maxsize and Maxlink are

determined by the following relationships.

Maxsize=R sin() (feet) (3-1)


Maxlink=MaxsizelCell_Size (integer number) (3-2)


Maxsize =Maximum perceived size of tracked target (feet) (3-3)

R=Maximum sonar range (feet) (34)

=One-half of the maximum of the receiving beam 's
angular bearing coverage and the receiving (3-5)
beam 's angular elevation coverage (radians)

Maxlink=Maximum number of contiguous range cells that can (3-6)
be grouped together to form one target (integer number)


Cell_size=Sonar range sample cell size (feet) (3-7)

This process subdivides all targets into a series of spheres. The tracking routine

remembers each sphere as a point with spherical radius Maxsize. Figure 3-3 illustrates

how the clustering procedure would cluster the threshold detected returns in Figure 3-2.

The dots represent each sphere's center. Note that to minimize the error in the target

location estimate, the sphere centers are placed in the middle of the sonar beams and the

middle of the contiguous cells.

Global Path Planning

Overview. Previous research into mine avoidance revealed that a purely reflexive

mine avoidance algorithm could allow the host vehicle to become trapped in certain box

canyon situations [101]. Although the reflexive algorithm would not allow the

vehicle to navigate too close to any mines, it lacked the sophistication necessary to escape

box canyon scenarios. Hence, it became necessary to integrate a high level path planning

procedure with the purely reflexive mine avoidance logic. The global path planner

divides the entire three-dimensional space into rectangular volume elements (voxels).

Based on each voxel's proximity to estimated positions, the GPP deems each voxel as

Head I ng

Figure 3-3. Clustered Target Example

safe or unsafe. A dynamic programming based search strategy then calculates a safe path

from the AUV's current position, through the safe voxels, to the desired end point. Note

that because the path planning strategy only accounts for mine position estimates of

detected mines, the planner typically works with an incomplete set of mine position

information. Hence, a planned path could pass too close to an undetected mine.

However, our path planning strategy reduces such possibilities somewhat by

accommodating a priori information such as known mine or underwater obstruction

positions. Figure 3-4 shows a two-dimensional path planning example.

The large dots represent the estimated mine positions and the circles represent the

required standoff distances. All voxels that touch any portion of a standoff region are

classified as unsafe (indicated by darkened voxel). The GPP then calculates a safe path

Figure 3-4. Path Planning Example with Complete Mine Position Data

from the start, through the safe voxels, to the goal. The safe path consists of a series of

weigh points. Each weigh point is located at the center of a safe voxel.

One drawback of the path planning function is that the dynamic programming

employs massive amounts of memory and consumes considerable processing time. This

combination requires an effort towards increasing execution speed so that a real-time

implementation of the mine avoidance capability can be demonstrated. Chapter 5 presents

some preliminary results that address these problems. Figure 3-5 illustrates a situation

in which the path planner has incomplete mine position information.

Figure 3-5. Path Planning Example with Incomplete Mine Position Data

The mine positions in Figure 3-5 are the same as in Figure 3-4. However, the area

around the undetected mine is not shaded because the path planner is unaware of its

existence. Hence, the planned path goes through the unsafe standoff region. This

example clearly demonstrates the GPP's dependence on how well global mine positions

are known.

Node searching methods. As previously described in Chapter 2, global path

planners normally model the safe voxels in an obstacle field as nodes. The path planner

then searches the nodes for a safe path. The particular search strategy is usually based

on some sort of cost function where the cost of a path is directly related to the planned


path's distance. Many node searching strategies exist. This research focused on two

node searching strategies, A* and breadth-first [102]. With respect to planned path

distance, A* is optimal and breadth-first is suboptimal. Because these techniques are well

documented in the literature, the details of the node searching algorithms are not

contained in this dissertation.

Three dimensions and path relaxation. In the two-dimensional path planning

examples, the voxel structure allows each node to have eight neighbors; if the planned

path goes through a node, the next node on the path must be one of the node's eight

neighbors. The literature refers to the first node as a parent node and the neighbor nodes

as child nodes. The global path planner utilized in this simulation is three-dimensional.

Hence, using a similar voxel structure in three dimensions, a parent node has 26 children.

The search strategy is basically the same. There are just more child nodes to search.

However, because an AUV would have great difficulty progressing to a node immediately

above or below the AUV's current node, the search strategy used in this simulation only

considers 24 child nodes. The search strategy is not permitted to search immediately

above or below the current node.

The node searching mechanism creates planned paths comprised of a series of

weigh points that form straight line segments. Each segment's orientation is some integer

multiple of forty-five degrees. However, by adapting Thorpe's path relaxation idea

[103], some intermediate weigh points can be eliminated.

The adapted path relaxation procedure systematically tests all intermediate weigh

points for possible removal. The procedure tests a weigh point, wi, by determining if the


straight line segment from weigh point wi.I to wi+j is still safe. If the new line segment

does not pass through any voxels known to be unsafe, the weigh point is removed. This

process smooths the planned paths and reduces the planned distance. Figure 3-6 shows

the resulting path when the path relaxation procedure is applied to the path in Figure 3-4.

The solid line in the figure indicates the relaxed path. The figure still shows the

weigh points that were removed from the original planned path.

Both of the path planners used in this simulation employ this path relaxation

technique. However, the A* planner exhibits better path relaxation performance than the

breadth-first planner. This is because A* initially has a weigh point located in every

voxel through which the planned path passes. Breadth-first, on the other hand, only has

nodes in voxels where the planned path changes direction. Therefore, for the same

overall planned path, A* would have more nodes than breadth-first. This performance

difference is entirely due to the weigh point implementation differences. The A* planner

was developed more recently than the breadth-first planner. Hence, its design benefitted

from the results of the earlier breadth-first design.

Local, Reflexive Mine Avoidance Logic

Safety Spheres

The local, reflexive mine avoidance logic is based on a local swept volume

method. It has three basic requirements. First, it must maintain at least the minimum

safe standoff distance from all mines (or more generally, all obstacles). Second, it must

guarantee that sufficient room exists for the AUV to perform a 360 degree turn. This

Figure 3-6. Path Relaxation Example

enables the sonar to map out the area immediately around the AUV before the logic

decides the best evasive action. And finally, if depth changing is desired, it must ensure

that the AUV has adequate elevation clearance to maneuver vertically. Figures 3-7 and

3-8 illustrate these three conditions.

Figure 3-7 shows a top view the AUV, its ideal sonar coverage and the two safety

regions around the vehicle. Guaranteeing that the AUV does not navigate too close to

a mine is equivalent to maneuvering the AUV so that no mine penetrates the inner circle.

Note that to account for the uncertainties associated with mine position, mine size and

vehicle position and to add an additional safety margin, the inner circle has been enlarged


by an amount U + SM1 beyond the standoff distance. Ensuring that no mines enter this

region is a primary requirement of the LRMA logic. The outer circle in Figure 3-7

includes an additional turn diameter requirement and a second safety margin. Note that

because the AUV can turn around either to the right or to the left, the outer turn diameter

circle's integrity only has to be maintained on one side of the vehicle at any particular

time. This divides the outer turn diameter circle into two semicircles.

The inner safety circle in Figure 3-8 is the analogous situation in the vertical

plane. However, the outer circle has been extended by any desired elevation clearance

instead of the turn diameter of the AUV. The avoidance logic only has to maintain one

half of the outer circle (either above or below the AUV).


Figure 3-7. Top View of Ideal Sonar Coverage and Safe Area Encompassing AUV

I Head I ng

SD = Standoff Distance
U = Size Uncertainty
TD = Turn Diameter
SM1 = Safety Margin 1
SM2 = Safety Margin 2


Figure 3-8. Side View of Ideal Sonar Coverage and Safe Area Encompassing AUV

In three dimensions, these conditions form three spherical regions around the

vehicle: a safety sphere; a turn diameter sphere; and an elevation clearance sphere.

Similar to the outer safety circles, the turn diameter sphere and the elevation clearance

sphere can each be subdivided into two hemispheres. The mine avoidance problem can

now be viewed as moving these three spheres through the minefield under the conditions

that: (1) no mines may enter the inner safety sphere; (2) no mines may simultaneously

penetrate both hemispheres (left and right) of the turn diameter sphere; and (3) no mines

may simultaneously penetrate both hemispheres (upper and lower) of the elevation

clearance sphere. The LRMA logic closely guides the AUV along the planned path and

continually maneuvers the vehicle as necessary to maintain the integrity of these three


SD = Standoff Distance
U = Size Uncertainty
EC = Elevation Clearance
SM1 = Safety Margin 1
SM3 = Safety Margin 3


The inner safety sphere is the smallest volume that satisfies the requirement that

the AUV must maintain a minimum standoff distance from all mines. However, both

outer spheres are larger than necessary. For instance, when performing a 360 degree turn,

the vehicle only needs the extra turn diameter clearance in a narrow region of height h

around the plane in which the turn is executed. This creates a disc-like region around the

AUV as illustrated by the shaded area in Figure 3-9. Extending a standoff distance

around this region creates a rectangular volume around the vehicle.

SD = Standoff Distance
U = Size Uncertainty
SM1 = Safety Margin 1
TD = Turn Diameter
h = Height of Disc-Like Region

Figure 3-9. Side View of Disc-Like Turn Region Around AUV

An analogous situation exists with the elevation clearance. A major problem with

using such shapes is that they depend on vehicle orientation. For instance, if the AUV

was descending when it made a flat turn, the rectangular volume region would no longer

be parallel the x-y plane. Likewise, the region required for elevation clearance moves as


the vehicle changes heading. Using spheres, however, creates regions that are

independent of vehicle orientation. Rotations do not affect them. This greatly simplifies

the mathematics involved with computing the regions and detecting when any standoff

regions violate the spheres. Therefore, spheres are the logical choice for the safety

regions around the AUV.

Synthetic Sonar

Overview. Figure 3-10 depicts the basis of the LRMA logic.

Synthetic Sonar Coverage

Figure 3-10. Top View i


First, the algorithm creates a synthetic sonar. That is, it hypothetically extends the

sonar coverage well beyond the actual sonar coverage. Since the AUV constantly moves

forward, a synthetic sonar elevation and bearing coverage of 180 degrees is sufficient;

the synthetic sonar need not look behind the vehicle. Also, to be consistent with the

actual sonar system, the synthetic beam widths should be an integer multiple of the actual

beam widths.

The synthetic sonar is readily accomplished by having estimates for both the

AUV's position and the mine positions. Then, for each and every synthetic sonar beam,

the procedure calculates the ranges to all of the estimated mine positions (if any) that

intersect with the corresponding synthetic sonar beam. Finally, the LRMA logic uses the

closest range in each synthetic beam to determine how to maneuver to maintain the

spheres' integrities. Figure 3-10 shows that when the AUV turns at a constant turn rate,

the safety circles intersect the synthetic sonar beams at known points. The constant turn

rate corresponds to the AUV's minimum turn radius, MinTurnRadius. In the figure these

intersection points are located where the dashed arcs intersect with the edges of the ideal

sonar beams. A similar situation exists in the side view. The LRMA approach

determines these ranges in three dimensions for all of the synthetic sonar beams. Thus,

if the range to the closest mine in each beam is known, the algorithm can easily

determine a number of logical conditions related to both the inner and outer spherical

regions. The LRMA logic uses two independent control functions, a heading control

function (HCF) and a pitch control function (PCF), to combine these often conflicting

logical conditions and generate commanded roll, pitch and yaw that maintain the three


basic requirements previously listed. The heading control function is always active while

the pitch control function can optionally be active or inactive.

Synthetic sonar beam-numbering and inner/outer safety areas. Continuing with the

explanation of the LRMA approach first requires some definitions. Figures 3-11 and 3-12

illustrate how to identify each synthetic sonar beam.

Figure 3-11 indicates that the sonar beam row numbers range from -NSR to NSR.

The sonar beam column numbers in Figure 3-12 range from -NSC to NSC. The row and

column numbers uniquely identify each beam. This creates an array of sonar beams

Beam(ij); i=-NSR,...,NSR; j=-NSC,...,NSC (3-8)

Now define the range to the closest target in each Beam(ij) as

TRange(ij); i=-NSR,...,NSR; j=-NSC,...,NSC (3-9)

Figure 3-13 shows a top view of the critical turn diameter area.

For each beam, beam(ij), let the range to the edge of this turn diameter sphere

area be

TDRange(ij); i=-NSR,...,NSR; j=-NSC,...,NSC (3-10)

Likewise, Figure 3-14 shows a top view of the critical safety sphere area.

For each beam, beam(i,j), let the range to the edge of this safety sphere area be

SSRange(ij); i=-NSR,...,NSR; j=-NSC,...,NSC (3-11)

Figure 3-15 shows a side view of the critical elevation clearance sphere area.

Co I umn 0

Figure 3-11. Synthetic Sonar Beam Row Numbering

Figure 3-12. Synthetic Sonar Beam Column Numbering

Co I umn NSR

SD = Standoff Distance

U = Size Uncertainty
TD = Turn Diameter
SM1 = Safety Margin 1

SM2 = Safety Margin 2

Figure 3-13. Top View of Critical Turn Diameter Sphere Area

Head i ng



SD = Standoff Distance
U =Size Uncertainty
EC = Elevation Clearance
SM1 = Safety Margin 1
SM3 = Safety Margin 3

Figure 3-14. Top View of Critical Elevation Clearance Sphere Area

SHead I ng

SD = Standoff Distance
U = Size Uncertainty
TD = Turn Diameter
SM1 = Safety Margin 1
SM2 = Safety Margin 2

Figure 3-15. Top View of Critical Safety Sphere Area

For each beam, beam(i,j), let the range to the edge of this elevation clearance

sphere area be


ECRange(ij); i=-NSR,...,NSR; j=-NSC,...,NSC

Logical Conditions Related to Three Safety Spheres

Now, the previously mentioned logical conditions related to the three safety

spheres can be described in terms of Beam(ij), TRange(ij), SSRange(i,j), TDRange(ij)

and ECRange(i,j). These logical conditions are:

GORTSS: The AUV must turn right to maintain the safety sphere

GOLTSS: The AUV must turn left to maintain the safety sphere

GORTTD: The AUV must turn right to maintain the turn
diameter sphere

GOLTTD: The AUV must turn left to maintain the turn
diameter sphere

The AUV must pitch up to maintain the elevation
clearance sphere

The AUV must pitch down to maintain the elevation
clearance sphere

The AUV can turn right without violating the turn
diameter sphere

The AUV can turn left without violating the turn
diameter sphere

The AUV can pitch up without violating the elevation
clearance sphere















CANGDN: The AUV can pitch down without violating the
elevation clearance sphere


These logical conditions have either a true or false value. Mathematically, they

are defined as follows

GORTSS = TRange(ij) < SSRange(ij); i=-NSR,...,NSR; j=O,...,-NSC

GOLTSS = TRange(ij) < SSRange(ij); i=-NSR,...,NSR; j=O,...,NSC

GORTTD = TRange(ij) < TDRange(ij); i=-NSR,...,NSR; j-O,...,-NSC

GOLTTD = TRange(ij) 5 TDRange(ij); i=-NSR,...,NSR; j-O,...,NSC

GOUPEC = TRange(ij) < ECRange(ij); i=-l,...,-NSR; j=-NSC,...,NSC

GODNEC = TRange(ij) 5 ECRange(ij); i=1,...,NSR; j=-NSC,...,NSC







CANGRT = TRange(ij) 5 TDRange(ij); i=-NSR,...,NSR; j=1


CANGLT = TRange(ij) < TDRange(ij); i=-NSR,...,NSR; j=-l

CANGUP = TRange(ij) < ECRange(ij); i=1; j=-NSC,...,NSC

CANGDN = TRange(ij) < ECRange(ij); i=-l; j=-NSC,...,NSC



Some additional metrics associated with the safety spheres are

GRSSMN = Minimum(TRange(ij)-SSRange(ij));
i=-NSR,...,NSR; j=-1,...,-NSC (feet)

GLSSMN = Minimum(TRange(ij) -SSRange(ij)):
i=-NSR,...,NSR; j=1,...,NSC (feet)


= Minimum(TRange(ij) -TDRange(ij)):
i=-NSR,...,NSR; j=-1,...,-NSC (feet)





GLTDMN = Minimum(TRange(ij)-TDRange(ij));
i=-NSR,...,NSR-j=-1,...,NSC (feet)

RClosest = Minimum(TRange(ij)) i=-NSR,...,NSR; j=0,...,NSC (feet)



LClosest = Minimum(TRange(ij)) i=-NSR,...,NSR; j=0,...,-NSC (feet) (3-38)

8, = heading error = y,,des meurd (3-39)
= desired heading measured heading (radians)

50 = pitch error = Od, ir-- measured (3-40)
= desired pitch measured pitch (radians)


8 = roll error = mid O mared (3-41)
= desired roll measured roll (radians)

Expanding Remembered Mine Positions

Creating a safety sphere around the AUV of some radius r + t and guiding it

through the minefield so that no mine enters its volume is mathematically equivalent to

using a sphere of radius r and expanding the mines by radius t [104, p. 876]. Using

this property, the reflexive mine avoidance logic expands all estimated mine positions by

SD + U, the standoff distance plus the total size uncertainty. The previous discussion

of safety spheres, synthetic sonar, logical conditions and metrics still applies. However,

the three radii of the safety spheres must be reduced by SD + U. With this change, the

logical conditions and metric definitions are still valid.

Heading Control Function

The heading control function combines the logical conditions and turn diameter

and safety sphere metrics to determine a commanded heading that safely guides the AUV

through the minefield. Let the rate-limited heading error be

e-limited = minimum(68,8.mai 8); SV>0.0 (3-42)
= maximum(8&,- m-aGx,,); 8i<0.0 (radians)


mai, = the maximum possible positive heading (3-43)
change in one control update cycle (radians)

and let

Vrate-limited = rate-limited commanded heading change (radians) (3-44)

The HCF combines these conditions to generate a commanded heading change.

A FORTRAN-like description of the heading control function algorithm is indicated


command maximum
command = maximum
End If
Else If (GORTSS) Then
B'command = maximum
Else If (GOLTSS) Then
-command = 'maximum

If (CANGRT) Then
command maximum
Else If (CANGLT) Then
command = -maxinmum
command = maximum
End If
If (CANGLT) Then
command = maximum
Else If (CANGRT) Then
'Vcommand = maximum
command = mxmium
End If
End If
klcommand = maximum
T command = -maxinum
~Wcommand = 8rate-limited
End If

The HCF structure gives priority to the safety sphere. If a mine's standoff

distance is about to penetrate either hemisphere of the safety sphere, the heading control

function maneuvers the vehicle to the opposite side of the threatening mine. And if mines

simultaneously threaten both hemispheres, the heading control function maneuvers to

prevent the closest mine from entering the corresponding hemisphere. Note that the

calculations of GRSSMN and GLSSMN do not involve the center column of synthetic

beams. This prevents GRSSMN and GLSSMN from equalling each other when a standoff

distance in the center column is the closest standoff distance to the critical safety sphere


area. Such an instance could prevent the heading control function from determining the

best maneuver to preserve the safety sphere's integrity.

If no standoff region- is about to enter the critical safety sphere area, the HCF

reacts to preserve the turn diameter sphere critical area. Like the safety sphere critical

area, the HCF turns the vehicle away from mines whose standoff distances are about to

enter any of the turn diameter sphere's hemispheres. And when mines threaten both

hemispheres, the heading control function responds to the more imminent danger. Finally,

when none of the regions are threatened, the vehicle is allowed to turn towards its desired


Flat Turn Maneuver

Whenever a standoff region causes a safety sphere violation, the AUV performs

a flat turn maneuver. During the flat turn, the AUV levels out and turns at its minimum

turn radius. The sonar sweeps out a predetermined volume immediately around the

vehicle. If the AUV holds a constant depth after the turn, the turn radius restriction

imposed by the limited sonar bearing coverage no longer applies while the AUV remains

in the swept out volume. Figure 3-16 shows a top view of this swept out region.

The dashed circle in the figure represents the outer boundary of the swept out

region. It has diameter SVD. The inner circle corresponds to the turn diameter sphere.

Figure 3-17 shows a side view of this region.

Note that the interior of the region is not swept out by the sonar. However, the

three safety spheres maintained by the mine avoidance logic already ensure that an

TD = Turn Diameter
SVD = Sweep Volume Diameter -

Figure 3-16. Top View of Area Swept Out By Sonar During Flat Turn Maneuver

interior region of radius SD + TD is free from mines. As long as the AUV maintains a

constant depth and remains within the standoff distance, SD, from the edge of this region,

the mine avoidance logic does not have to restrict the AUV's turn radius. The sonar has

already detected the mines in the area. Also, maintaining a constant heading immediately

after a flat turn extends the swept out volume region.

Turn Diameter Sphere

TD = Turn Diameter

Figure 3-17. Side View of Area Swept Out By Sonar During Flat Turn Maneuver

Vehicle Speed

The reflexive mine avoidance logic offers an optional reflexive/flat turn mode of

operation. In this mode, the mine avoidance logic removes the turn radius restriction by

executing periodic flat turns whenever the AUV nears the boundary of the sonar swept

volume. This mode is sometimes useful in a dense minefield environment when total

sonar bearing coverage is limited. The removed turn radius restriction permits the mine

avoidance logic to turn sharply around newly detected mines at the expense of periodic

flat turns. In the reflexive mode, the additional flat turns are not executed. The AUV

operates with a restricted turn radius.

Unfortunately, the corresponding pitch radius restriction cannot be eliminated.

Underwater vehicles typically have minimum and maximum pitch limits.


In practice, the sonar cannot be guaranteed to detect all mines that come within

its coverage area. However, the vehicle's speed is an important factor that determines

how well the sonar performs- mine detection. If the AUV moves too quickly, the sonar

may not perform adequately. Unless the sonar detects each target at least

MinDetectCount times, the mine avoidance logic ignores the estimated mine position. If

it moves too slowly, multiple false alarms are more likely to arise in the same vicinity.

The MPT logic could incorrectly interpret the false alarms as a single mine thereby

causing a false alarm's detection counter to reach the MinDetectCount limit. The

vehicle's speed must be chosen carefully so the sonar performs mine detection adequately.

Pitch Control Function

The pitch control function combines the logical conditions and elevation clearance

sphere metrics to determine a commanded pitch that safely guides the AUV through the

minefield. Let the rate-limited pitch error be

rate-limited = minimum(, x); 0.0 (3-45)
= maximum(56,-58m); 80<0.0 (radians)


50mx = the maximum possible positive pitch (3-46)
change in one control update cycle (radians)

and let


80rate-lmited = rate-limited commanded pitch change (radians)


The PCF combines these conditions to generate a commanded pitch change. A

FORTRAN-like description of the pitch control function algorithm is indicated below.

If (CANGUP) Then
command maximum
Else If (CANGDN Then
580 = -650
command maximum
command maximum
End If
590 = 590.
command maximum
command = maximum
Else If (GOUPEC) Then
590 = 590 ,
command maximum
Else If(GODNEC) Then
59 = -59
command Maximum
Else If (5080.0 .AND. CANGUP) Then
command rare-limited
Else If (50<0.0 .AND. CANGDN) Then
command = rate-limited
If (020.0) Then
8_ = minimum(-O,-850,,.m)
50command = maximum(-0,'80xi,.m)
End If
End If

Certain factors limit the PCF's range of pitch control. First, the minimum pitch

radius restriction caused by the limited sonar elevation coverage limits the vehicle's pitch

rate. Second, if the AUV is performing a flat turn or operating in the reflexive/flat turn

mode, it must maintain a constant depth. And finally, in some instances it is not

advisable to go under or over obstacles. The standoff region may extend all the way to


the surface [105, pp. 180-183]. Therefore, the pitch control function is considerably

less complex than the heading control function.

Since the heading control function has fewer maneuverability constraints and

already preserves the inner safety sphere, the pitch control function only maintains the

elevation clearance sphere. The FORTRAN-like description indicates that the pitch

control tests for mines penetrating the upper and lower elevation sphere hemispheres. It

then gives priority to maintaining the lower hemisphere.

If the elevation clearance sphere's integrity is not threatened, the pitch control

function allows the vehicle to establish the desired pitch when possible. Otherwise, the

function controls the vehicle pitch to 0.0 degrees.

The mine avoidance logic does not require a roll control function. It simply

commands the roll to 0.0 degrees.

Control Mode Structure

Thus far, Chapter 3 has described the various components of the reflexive mine

avoidance logic in great detail. Examining the interaction between the components is

critical to understanding precisely how the overall mine avoidance approach functions.

Figure 3-18 illustrates the interaction in the form of a control mode transition diagram.

The figure shows that overall reflexive mine avoidance logic has four basic modes

of operation: on track (OT) mode; local reflexive (LR) mode; flat turn (FT) mode; and

acquire new track (ANT) mode. Initially, let the AUV be in the on track mode. In this

case the AUV is closely following the weigh points on the planned path and no mines or

Re-Acquire Track

Figure 3-18. Control Mode Transition Diagram

obstacles obstruct the path. If a mine obstructs the planned path, the mine avoidance

logic switches to the LR mode. After this mode guides the vehicle around the

obstructions and the AUV re-acquires the planned path, the mine avoidance logic's mode

switches back to the OT mode.

However, if any of the safety sphere violations occur or if the AUV deviates from

the planned path by more than a specified off-track distance limit, the AUV immediately

enters a flat turn mode. In this mode, the vehicle levels out and performs a 360-degree

turn at its minimum turn radius. This enables the sonar to sweep out a volume

immediately around the AUV. Upon completion of the turn, the path planner re-plans a

safe path through the minefield and the avoidance logic enters the acquire new track



In the ANT mode, the extra safety margins, SM1, SM2 and SM3 are temporarily

reduced. This is required because the nature of the local, reflexive mine avoidance logic

guides the AUV to the edges of standoff regions. In limited maneuverability situations

this frequently causes one of the three safety sphere violations. After the flat turn is

completed, though, nothing has changed; the AUV and mine positions remain virtually

unchanged. Without relaxing the extra safety margins, the mine avoidance logic would

again detect a safety sphere violation and would initiate another flat turn. Temporarily

reducing the safety margins gives the mine avoidance logic sufficient time to acquire the

new track and remove the AUV from this situation. Once the AUV travels an acquire

new track distance limit, the mine avoidance logic re-enters the on track mode. While

in the acquire new track mode, Figure 3-18 shows that the mine avoidance logic can still

enter the LR mode to avoid mines. After re-acquiring the new track, the mine avoidance

logic re-enters the acquire new track mode.

The conditions that caused the AUV to initiate a flat turn could create problems

for the path planner. The GPP could be in situation in which it believes the voxel that

the AUV is currently in is unsafe. The GPP then cannot plan a path through safe voxels.

To overcome this phenomenon the mine avoidance logic stores all of the voxels through

which the AUV has passed, in a last in first out (LIFO) buffer. If the AUV's current

voxel is unsafe, the path planner searches the LIFO buffer for a safe voxel. This safe

voxel is then used as the ending point for the node searching strategy. The path planner

then simply adds the current voxel to the beginning of the weigh point list to complete

the path.


When the AUV is in either the on track mode or the local reflexive mode, the

mode can change directly to the flat turn mode if a flat turn is required to perform a sonar

volume sweep. This occurs when the local, reflexive mine avoidance logic is operating

in the optional reflexive/flat turn mode and the AUV is approaching the boundary of the

previously swept out sonar volume.



Sonar systems provide range, elevation and bearing measurements to underwater

targets. The accuracy of these measurements is typically known. This chapter addresses

the problem of using these nonperiodic, noisy measurements from a sonar mounted to the

front of an underwater vehicle to estimate and refine estimates of sonar target positions

in a world cartesian coordinate system. Because the relationship between the

measurements and the target position is highly nonlinear, the conventional linear Kalman

filter cannot be used. However, the extended Kalman filter provides an appropriate

recursive filtering mechanism that accommodates the nonlinear relationship between the


This chapter first summarizes the continuous-discrete Kalman filter in the general

case. Second, filter initialization procedures are derived. Third, the coordinate system

and the functional relationships for this specific extended Kalman filter implementation

are addressed. The case of no navigation error is discussed next followed by the case

with navigation error. Finally, this chapter completes the entire estimation problem by

considering how new measurements should be associated with previously detected targets.


Continuous-Discrete Extended Kalman Filter

Mathematical Summary

A detailed summary of a continuous extended Kalman filter with discrete

measurement updates is given below [106]. Note that upper case letters indicate

matrices, underlined lower case letters indicate vectors, and lower case letters indicate









k=h (tk)) +Zk; 8z_-(0,Rk); k=1,2,...

(0)-N (,Po)

E[w(t)k]-=0 for all k and all t

A ^
-x--Z(t),t); t :5k+,

(tk)= +); (-)k+l,=(tk+l)

P(t) =E[ (t) --x(t)) ( (t) -_(t))]

P(t)=Fx(t),t)P(t)+P(t)F (C(t),t)+Q(t); tk
P(tk=P,(+); Pk,(-)=P(tkl,)






x'(t)=fx(t),t)+w(t); w(t)-N(0,Q(t))







k(+)= (-)+Kk -h k,(-))]

Pk( +) =[-KkHk( k(-))]Pk(-)

Kk= k(-)HkT(k -))
[H k (-))P(-)Hk k (-)) +R ,]-'}

FC(t),t)= = (t),t)
ax (t) I

Hk( -))= ) -)


ahi (tk))







Filter Initialization

Equations (4-1) through (4-13) provide a detailed summary of the extended

Kalman filter and its implementation. However, filter initialization has not been


addressed. Specifically, how should the initial estimates of the state x and the error

covariance P be chosen? If the true state estimate and measurement errors are known,

the true initial state can be calculated according to

x(0) =-(0) -Sx(0) (4-14)


x(0)=g -8Z) (4-15)

where the function g(.) maps the measurements to the state.

Since the expected value of the measurement error is zero, the initial state estimate

may be calculated based entirely on the noisy initial measurement to as indicated in

Equation (4-16).

State (0) =g () (4-16)

Initializing the covariance matrix P is more involved. In typical Kalman filter

implementations, the initial covariance estimate is set artificially high. After repeated

measurement updates, the initial filter transients become small and the covariance estimate

is less dependent on the initial estimate. While these transients are settling down the

filter may not estimate accurately. To avoid this estimation delay, an immediate, more


realistic initial covariance estimate is required. A reasonable procedure to estimate the

initial covariance is developed below [107].

Substituting Equation (4-14) into (4-15) yields


Expanding Equation (4-17) in a Taylor series and dropping

higher terms produces a first order approximation for the initial state

second order and




zi = the i'th element of vector zk at time k.


Substituting Equation (4-14) into Equation (4-18) and solving for the state estimate

error results in


(o) -s(O) S(KO 8Z )

x(0)=1(0)-4x(0)=g -8 )=g)Q-A )8j

A(k)= -----_; ai,= g-Z
k Lzk> azi

Now note that the state estimation error covariance matrix is defined as

P(t)=E(Gx(t)GxT(t)) (4-22)

Substituting Equation (4-21) into Equation (4-22), rearranging terms and moving

the expectation operation inside produces the desired initial covariance estimate

Po-A )E(zS_)A Tr) (4-23)

Note that the measurement error covariance is

R=E(8zkZ5z) (4-24)

so that Equation (4-23) can be simplified to

Error Po=A(o)RA T() (4-25)

Equations (4-1) through (4-13) summarize the continuous extended Kalman filter

with discrete measurements while Equations (4-16) and (4-25) provide initial state and

covariance estimates.


Coordinate System and Sonar Functional Relationships


Before the specific application of estimating sonar target positions with an

extended Kalman filter can be discussed, the coordinate system and functional

relationships that define the system and measurement models must first be defined.

Coordinate System

Figure 4-1 shows the three coordinate systems which define the positions

necessary for estimating sonar target coordinates. The three coordinate systems are world

coordinates, vehicle coordinates and sonar coordinates as indicated by the w, v and s

subscripts on the corresponding coordinate axis. Necessary target position definitions are

indicated below. All positions in the following discussion are measured in feet.

Figure 4-1. Submarine Coordinate Systems

Target Position in
World Coordinates

Target Position in
Sonar Coordinates

Target Position in
Vehicle Coordinates

x,=[x, y zJT

,=[x y, z]T

x,=[x, yv zV]T

Sonar Coordinate System Origin
in World Coordinates

Sonar Coordinate System Origin
in Vehicle Coordinates

Vehicle Coordinate System Origin
in World Coordinates

b [Xb Yb Zb]

bv=[xb Yb, ZbT

C,=[Xc Yc Zr]T

In this problem formulation the sonar coordinate system is not rotated with respect

to the vehicle coordinate system; a simple translation transforms between vehicle and

sonar coordinates. A transformation from vehicle to world coordinates is achieved by

both a rotation and a translation as indicated in Equation (4-32)

y =D y +c
zL LZ~w








where the rotation matrix is

D= sinosin0cosW-cososinNI

cosocosy, +sinosinnsisin
cososin0sin -sin4cosV



O=vehicle roll (radians)

0=vehicle pitch (radians)

--vehicle yaw (radians)



Sonar Functional Relationships

The sonar system provides a three by one measurement vector consisting of range,

elevation angle and bearing angle to the target. The functional relationship that maps the

target position in sonar coordinates to the measurement vector is

range (feet) r 2-x, 2 2 + (4-37)
z elevation (radians) = a Tan -I(-x (3)/x (1)) Tan -(-zjx)
bearing (radians) J Tan (2)x(1)) lTan-l(yx)

The relationship that maps the measurements to the target position in sonar

coordinates is

z(1) r
1 +Tan 2(2)) +Tan 2(3)) 1 +Tan2(+Tan2()
z (1)Tan(z(3)) rTan(p) (4-38)
S +Tan 2((2))+Tan 2((3)) 1 +Tan2( Tan2()
-z(1)Tan(z(2)) -rTan(a)

1 +-Tan2((2))+Tan 2(3)) 1 +Tan 2(a)+Tan2()

Equations (4-37) and (4-38) relate target position in sonar coordinates to the sonar

measurements. However, the Kalman filter requires these relationships to map between

world coordinates and sonar measurements. By first writing the rotation matrix as a

matrix of three vectors and then utilizing the previous definitions these relationships can

be expressed in the desired format

Dd,, d23 dT (4-39)
d,1 d,, d,3
d3jL d3,2 d3,3 dT
^ ,<, I,^ I

r w(D(x' -c )-b )T(D(x,-c )-bQ)
( WT (4-40)
z_-x-h (0 Tan -'((-dr(x -c )+Zb )(LT( > -)Xb) (4-40)
yz w "w x w w
A Tan -'((d(-C ) -yb.)l w -) -X



1 +Tan 2(a) +Tan 2()
rTan(P) b (4-41)
x =g()= y =D -v + c ~ (4-41)
1-() = y. D T l +Tan 2(a)+Tan 2(0) C
J -rTan(a)

1 +Tan 2(a)+Tan 2(p)

Target Position Estimation With No Navigation Error

Model Parameters

Note that as the problem has been stated, the filter is designed to only estimate

stationary target positions. Using the previously developed notation, the filter state is just

a three by one column vector that indicates the target position in world cartesian

coordinates. Since the targets are stationary, the true state remains constant. The state

vector and the parameters of the system model in Equation (4-1) may now be defined

Vector X=X,=[w y, zJ]T (4-42)

Model _(t)=0; f(~(t),t)=0; (t)=0; and Q(t)=[0] (4-43)

The measurement vector and measurement model of Equation (4-2) have already

been defined in Equation (4-40). Since the sonar produces independent range, bearing

and elevation measurements, the measurement covariance matrix is diagonal. The

diagonal elements of the covariance matrix have units of feet2, radians2 and radians2.

Measurement 0 0
Covariance (444)
Matrix R0 (4-44)
0 0

The initial conditions of Equation (4-3) and the assumption of Equation (4-4) are

still valid. Also, note that because the system model parameters listed in Equation (4-43)

are all zero, Equations (4-5), (4-6), (4-10) and (4-12) are not used. Before the Kalman

filter update and gain equations described in Equations (4-7) through (4-9) can be

implemented, the Jacobian matrix in Equation (4-11) must first be calculated.



aX (tk

S[D (D(,-c)-b ]
I -))-^ -C -w -v
(D(x_ -cw) -b-) )(D w -c-) -b) (4-45)

1 -d w-c) -xb,) +dT(T(- ,)-zT,)
(d -Cxb) 2
-d~w-c w)+Xb-

+ )x _)WD -,) Xb
!(x-c W( ) Xb b r-c )-Xb)

1[-dT -c I -c
g(x -c )-xb


The initial covariance matrix estimate in Equation (4-25) requires the calculation

of the Jacobian matrix described in Equation (4-19). Because this calculation is quite

cumbersome, a separate equation is given below for each matrix element.

Sagk 1 ak 1 (4-46)
Ak)Z k az=2,1 2,2 a2,3
a3,l a3,2 a3,3


ax d,1 +d2,Tan(p)-d3,1Tan(a)
a11 -- (4-47)
a r 1 +Tan 2(a) +Tan 2(P)

y dl2+d2,zTan() -d3,Tan(a)
a2,, - (4-48)
ar v +Tan 2(a)+Tan 2(p)

azw d13 +d2,3Tan(P)-d33Tan(a)
a3,1 - (4-49)
r +Tan 2(a) +Tan 2(o)

a xw -dl,r Tan(a) -d2r Tan(a) Tan(f)
a"a cos2(a)(1 +Tan 2(a) +Tan 2(p))32


Sy, -dJr Tan(a)-d2r Tan(a) Tan(f)
a22 a cos2(a)(1+Tan 2(a)+Tan 2(p))3/2

d3 1- Tan (a)
S (1 +Tan 2(a) +Tan 2()
cos2(a) 1 +Tan 2(a) +an 2(p)

Sz, -dlr Tan(a)-d,3rTan(a) Tan(1)
"3a cos2(a)(1 +Tan 2(a)+Tan 2(3))312

an 2(a) (4-52)
3 (1+Tan 2() +Tan 2(p)

cos2(a)1 +Tan 2(a)+Tan 2(3)

ax, -d,,r Tan(P) +d3,rTan(a) Tan(P)
al P cos2(p)(1 +Tan 2(a) +Tan 2(3))32

Sd2r1 1 Tan( (4-53)
+ (1 +Tan 2(a) +Tan 2)
cos 2(J) 1 +Tan 2(a)+Tan 2(1)

_y, -dr Tan(P) +d3r Tan(a) Tan(p)
a2'3 p cos 2()(1 +Tan 2(a) +Tan 2(P))3t2

Tan 2(3) (4-54)
S (1 +Tan 2(a)+Tan 2(1)
cos 2(c13) +Tan 2(a) +Tan 2()


az_ -d,3r Tan(P) +d3,r Tan(a) Tan(3)
a- -
cos2(P)(I +Tan 1(a) +Tan (P))3/2

d ( 1 Tan 2() (4-5)
(1 +Tan 2(a) +Tan 2()

cos() 1 +Tan 2(a) +Tan 2()

Required Modifications when Navigation Error is Present


In the presence of navigation error, true vehicle position (x, y, z) and orientation

(roll, pitch, yaw) information is no longer available. Instead, an on-board inertial

navigation system (INS) provides noisy estimates of these six time varying quantities.

Under such circumstances the Kalman filter must be modified to account for the effect

these additional noise sources has on the measurement covariance matrix, the initial state

estimate and the initial state error covariance estimate.

Measurement Covariance Matrix Modifications

Some new definitions and notation help formulate the required modifications to

the measurement covariance matrix. First, let


tue [X. Y=[x Z, 4c Oe Vc]T = [Xe Ytu Z~'E e 0true Vtrue]T


represent the true vehicle states (position and orientation) and let


represent the measured vehicle states as indicated by the on-board INS; the error in the

vehicle states is defined as



The measurement model in Equation (4-40) can now be rewritten -to express the

dependence on both the desired state and the vehicle state.



As expected, the measurement estimate used in Equation (4-7) is calculated by

evaluating Equation (4-40) at the current filter state and vehicle state estimates

z^ =h (x tk)' k t(_),() ) (4-60)
k "k ( mX t Y )1(0).L f 'd

.=. ,'-[Xis Yi, Zi, 4i, ins 6 i j'T

zk k (t)) +8Z k

Substituting Equation (4-58) into (4-59) yields

Z.k=h k (tk) -,k (ttk)) +8 k

which can be expanded in a Taylor series to produce

z Kh (tk),Y. (tk)) +Hy(-5g +5zk


H k(x(tk )' ,,, ())




Using the covariance definition and simplifying produces the modified covariance



Rmod=E[ [Hy(-8y)+8 z [H/(- ) +Sz

=HyE[8y8y T]H,r+E[Sz8z I]



where the INS error covariance matrix is

.0 0 0 0 0

0 0 0 0 0
INS Error
Covariance 0 0 (- 0 0 0
Matrix Ry0 (4-65)

0 0 0 0 0 0

0 0 0 0 0 o2

Substituting Rod, the modified measurement error covariance matrix defined in

Equation (4-63), in place of R, the measurement error covariance matrix used in Equation

(4-9) will now account for the INS noise sources' effect on the measurement covariance.

For completeness, the 18 elements of the remaining Hy matrix in Equation (4-64) have

been calculated according to Equation (4-63) and are listed below.

h,i h h3 h1,4 h1,5 h ,6
H=h2,1 h2, h h2, h2,5 h26 =x -h h hh h (4-66)
Lh3,1 h3, h3,4 h 3,4 h3,6 h

= r -[d11 d2, d3,1,](D(x-c)--b)
h1- = (4-67)
xt (D( -c )-b )'(D(x -c )-b )
y*^w -w -v -w -w -v

h2ar -[d1,2 d2, d3,2]T(D -c )-b)(
h1,2=- (4-68)
ytre (D~ -c )-b)T(Dw, -c ) -b)

S r -[d13 d,, dd,33](D( X-,)-b (
hi.=-- =-- (4-69)
a',e (D(x -c ) -b )T(D(x_ c )-b )
V w -w -v w -w -v

[ar I -c () ]"^(D(x -c )- )
h a-r [D -w -w)- (4-70)
D J(D( -c )_-bc)r(D(x -c )) -


0 0 0 T
D,= os sincosy+sinosinw -sinscosi/+coscsinesinov coscosi (4-71)
cososinVy-sinosin0cosi -sin)sinesinV-cos4cosiv -cossin d T

S r [De'(x -c )]T(Dwx-c_ )-b )
h, tre = (D(x ) (4-72)
0 (D(x -c )_-b )(Dx -c )-b )
y'-w -w -v -w -w -v

University of Florida Home Page
© 2004 - 2010 University of Florida George A. Smathers Libraries.
All rights reserved.

Acceptable Use, Copyright, and Disclaimer Statement
Last updated October 10, 2010 - - mvs