A REFLEXIVE MINE AVOIDANCE
APPROACH FOR AUTONOMOUS UNDERWATER VEHICLES
By
JOHN CHARLES HYLAND
A DISSERTATION PRESENTED TO THE GRADUATE SCHOOL
OF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT
OF THE REQUIREMENTS FOR THE DEGREE OF
DOCTOR OF PHILOSOPHY
UNIVERSITY OF FLORIDA
1993
ACKNOWLEDGEMENTS
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
guidance.
TABLE OF CONTENTS
ACKNOWLEDGEMENTS ........................................ ii
ABSTRACT .................................................. v
CHAPTERS
1 INTRODUCTION ...................................... 1
Unmanned Underwater Vehicle Applications and
Research Trends ...........................,...... 1
Future AUV Missions and Likely Research Areas ............. 2
Purpose and Scope .................................. 5
2 EXISTING OBSTACLE AVOIDANCE APPROACHES ........... 7
Problem Definition and Technical Considerations ............. 7
Mine and Obstacle Avoidance Research Summary ............ 13
3 REFLEXIVE MINE AVOIDANCE APPROACH ................ 34
Overview ......................................... 34
Vehicles Dynamics and Navigation Error ................... 35
EnergyBased Sonar Model ............................ 37
Reflexive Mine Avoidance Logic ...................... 38
Local, Reflexive Mine Avoidance Logic ................... 48
Control M ode Structure ............................... 71
4 ESTIMATING SONAR TARGET POSITIONS WITH AN
EXTENDED KALMAN FILTER ........................... 75
Introduction ...................................... 75
ContinuousDiscrete 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
5 COMPARISON OF THE REFLEXIVE MINE AVOIDANCE
APPROACH WITH OTHER MINE AVOIDANCE APPROACHES 109
Overview ...................................... 109
Simulation Scenario and Modifications ................. .. 110
Simulated Minefield Scenarios ......................... 112
Comparison Summary ............................... 141
6 CONCLUSIONS AND POSSIBLE FUTURE RESEARCH ........ 143
Conclusions ...................................... 143
Future Research Recommendations ...................... 145
APPENDICES
A VEHICLE DYNAMIC MODEL AND DOPPLER/INS MODEL .... 146
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
A REFLEXIVE MINE AVOIDANCE
APPROACH FOR AUTONOMOUS UNDERWATER VEHICLES
By
JOHN CHARLES HYLAND
May, 1993
Chairman: Fredrick J. Taylor
Major Department: Electrical Engineering
A threedimensional 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
approach.
CHAPTER 1
INTRODUCTION
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, underice 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
2
[7]. Recent major AUV research areas are navigation [8], [9], guidance [10],
control [11], [12], [13], knowledgebased systems [14], [15], fault
recovery [16], threedimensional 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 36foot long, 44inch diameter underwater vehicle would be built and outfitted with
several tactical software packages including a mine search system package [23].
Table 11 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 NavyMarine 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 shallowwater 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 11. AUV Program Summary
Depth
Year Vehicle Purpose (meters) Developer
1963 SPURV 1 water measurements 6,000 University of Washington Applied Physics Laboratory
(UW/APL) (USA)
1972 UARS underice 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)
OSRV 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 HeriotWatt University (Edinburgh, Scotland)
Robot II bottom surveys 91 Massachusetts Institute of Technology (USA)
BI 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 underice mapping 400 Intl. Submarine Engineering Ltd. (Port Moody,B.C.
Canada)
AUV hydrodynamic drag studies n/a Rockwell International (USA)
1985 Submarine testbed/hydrodynamic flow tests 500 JAMSTEC (Yokosuka, Japan)
Robot
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 11.continued
Depth
Year Vehicle Purpose (meters) Developer
1988 XP21 testbed 610 Applied Remote Technology (San Diego, California,
USA)
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)
MT88 bottom/water surveys 6,000 Institute of Marine Technology Problems (Vladivostok,
USSR)
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)
UROV2000 bottom survey 2,000 Japan Marine Science & Technology Centre (Yokosuka,
Japan)
unnamed testbed/precise control vehicle 10 Japan Marine Science & Technology Centre (Yokosuka,
Japan)
MacAROV antisubmarine 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)
Voyager
1992 Mine tested n/a Lockheed Missiles & Space Co. (Sunnyvale, California,
Avoidance USA)
AUV
Doggie bottom/subbottom survey 6,000 Yard Ltd. (Glasgow, Scotland)
Dolphin temperature/salinity/depth 6,000 Yard Ltd. (Glasgow, Scotland)
monitoring for 30day 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 antisubmarine warfare (ASW) and other missions [29].
5
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].
6
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
research.
CHAPTER 2
EXISTING OBSTACLE AVOIDANCE APPROACHES
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 21 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
8
k_
Figure 21. AUV Escorting a Surface Vessel Through Minefield
to previously detected mines. And finally, since a computer onboard the AUV will
implement the mine avoidance algorithms, the resulting mine avoidance software must
execute in realtime. 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
9
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
case.
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 (21)
mm 1cos(0/2)
where
TRmnminimum turn radius (feet) (22)
SO ,,,,total desired standoff distance from center of mines (feet) (23)
and
(24)
0=total sonar angular bearing coverage (radians)
12
AUV movement as severely as does the turn radius restriction. Both restrictions,
however, must be addressed.
Navigation and tracking. AUVs employ onboard 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 threedimensional 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
realtime.
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,
10
Figure 22 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 22. Turn Radius Problem
Executing turns with radii larger than TRm,,, will ensure that this situation cannot
arise. Figure 23 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
2
Ideal Sonar Coverage
11
Standoff Distance
Vehicle's Path Just
Misses Standoff Region
Undetected Obstacle
S.. . ..........
3
Ideal Sonar Coverage
1 2
Figure 23. Solution to Turn Radius Problem
An analogous pitch radius restriction exists as well
PR .= SO, (25)
mm 1cos(0/2)
where
PRm=minimum pitch radius (feet) (26)
and
=total sonar angular elevation coverage (radians) (27)
Equations (21) and (25) 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
13
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,
and
12) operate in three dimensions.
In addition, the mine avoidance logic should
1) execute in realtime,
and
2) be capable of residing in a computer system on board the AUV.
Mine and Obstacle Avoidance Research Summary
Overview
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 collisionfree
curve in the robotic vehicle's ndimensional space. The planned curve does not take the
robotic vehicle's dynamics into consideration. A trajectory planning procedure then
14
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
dependent.
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 ndimensional 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
collisionfree 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 realtime implementation on
board an AUV quite difficult. Also, the approach is only as good as the available
15
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 realtime. 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,
16
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.
17
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
Rangebased robot navigation. In 1975, Cahn and Phillips developed the
ROBNAV algorithm [40]. ROBNAV is a twodimensional local penalty function
approach that continually guides a mobile robot through an obstacle filled room. A range
sensor divides the field of view into equiangular 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 twodimensional 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
well.
18
Polyhedral obstacles. In 1979, LozanoPerez and Wesley describe an algorithm
that plans collisionfree 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. Threedimensional 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
19
priori obstacle positions and does not provide for sensory input. The method also does
not work well in an obstacledense 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 20meter 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 twodimensional.
20
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
applications.
Rotation mapping graph. In 1984, Chien et. al introduce the idea of a rotation
mapping graph (RMG) to plan collisionfree paths for a robotic arm among obstacles
[48]. The RMG is made by first subdividing a twodimensional space into regions
and then determining what areas, measured as angles from each region's center, are
collisionfree. The collisionfree 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 twodimensional
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 twodimensional 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.
21
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 discussesthe 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 twodimensional. Moving the
nodes further and further away could also create problems when the planned path is not
convex.
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
22
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 theplanned 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
only.
Clash detection problem. In 1985, Cameron summarized three "clash detection"
approaches for a robotic arm: multiple interference detection, fourdimensional
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 fourdimensional
intersection detection has a different emphasis. This approach uses fourdimensional (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
threedimensional object to create or sweep out a threedimensional volume.
Mathematical tests find intersections between the threedimensional 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.
23
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 obstaclefree 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 mazelike 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 twodimensional grid space into a series of nonuniform
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.
24
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 twodimensional 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 realtime 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.
Sonarbased navigation and mapping. Elfes developed a sonarbased 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 twodimensional
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 10foot
by 25foot indoor area covered with chairs, tables, boxes, workstations and filing cabinets
and a 50foot by 30foot outdoor area covered with trees.
25
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 twodimensional.
AUV obstacle avoidance. Hyland developed a twodimensional 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
twodimensional 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
26
[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 twodimensional 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 twodimensional, 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].
27
Optimal AUV obstacle avoidance. Hyland expanded his earlier twodimensional
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 breadthfirst [68], in
terms of execution speed and distance of planned path.
Neural network. A neural networkbased 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 twodimensional work [70]. The "visit counter" reduces the susceptibility
to local minima by increasing an obstacle's repulsive force according to how many times
28
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 realtime 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
field.
29
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 threedimensional while the simulation results are twodimensional.
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. Twodimensional
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 threedimensional searching
methods by Bonsignore [74]; heuristic searching by Ong [75]; Wong and Fu's
twodimensional orthogonal space planning [76]; Bien and Lee's timeoptimal dual
robot control [77]; Shin and Zheng's multiple robot work [78]; Wilfong's
30
consideration of steering angle limitations when planning paths [79]; and Richbourg's
performance study of different twodimensional 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
CVoranoi 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 21 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 21. 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 2d Rectangle Polygons No Yes Simulation
RangeBased Penalty (Range
ROBNAV Function Sensor)
10/79 LozanoPerez & Global 3d Point Polhedrons Yes No Simulation
Welsey/ Path (Required) & Lab.
Polyhedral Planning Demo.
Obstacles
4/83 Brooks/ Global 2d Polygon Polygons Yes No Simulation
Generalized Path (Required)
Cylinders Planning
7/83 Moravec/ Global 2d Circle Circles Yes Yes Simulation
Stanford Cart Path (TV & Field
Planning Camera) Demo.
1/84 Chien, Zhang & Global 2d Circle Polygons Yes No Numerical
Zhang/Rotation Swept (Required) Examples
Mapping Graph Volume
8/84 Krogh/ Global 2d Point Convex Yes No Simulation
Generalized Penalty Polygons (Required)
Potential Field Function
8/84 Thorpe/ Path Global 2d Point Circles Yes Yes Simulation
Relaxation Path (Video Using Real
planning Images) Video Data
2/85 Gilbert & Global 3d Convex Convex Yes No Simulation
Johnson/ Penalty Polygon Polygons (Required) (2d only)
Distance Function
Functions
2/85 Crowley/ Global 2d Point Polygons Yes Yes (Ultra Simulation
Intelligent Path sonic
Mobile Robot Planning Range
& Local Sensor &
Path Touch
Planning Sensors)
2/86 Nitao & Parodi/ Global 2d Point Polygons Yes Yes Simulation
Autonomous Path (3d (Required) (Utira & Field
Land Vehicle Planning world) sonic Demo.
& Local Imaging
Penalty Sensor)
Function
2/86 Singh & Wagh/ Global 2d Point Convex Yes No Simulation
Intersecting Path Polygon (Required)
Convex Shapes Planning
4/86 Khatib/ Artificial Local 3d Analytic Analytic Yes Yes Lab. Demo.
Potential Field Penalty Equation Equation (Required) (Vision
Function System)
6/87 Elfes/ Sonar Global 2d Grid Grid Yes Yes Simulation.
Based Mapping Path Square Squares (Ultra Indoor &
& Navigation Planning sonic Outdoor
Sonar) Demos.
Table 21.continued
Date Developer/ Algorithm Dimension Object Obstacle Accepts Accepts Validation/
Method Type Shape Shapes A Priori Sensor Testing
Information Data
12/87 Oommen, Global 2d Convex Convex No Yes Example
lyengar, Rao & Path Polygon Polygons (Few
Kashyap/ Planning Details)
Learned & Swept
Visibility Graphs Volume
11/88 Hyland/ AUV Local 2d Circle Circles Yes Yes Simulation
Obstacle Swept (Sonar)
Avoidance Volume
5/89 Gilbert & Hung/ Local 3d Convex Convex Yes No Simulation
Collision Swept Polygon Polygons (Only One)
Detection Volume
5/89 Kue & Barshan/ Local 2d Circle Grid No Yes Indoor
Unstructured Swept Square (Sonic Demo.
Environment Volume Range
Navigation Sensor)
5/89 Warren/ AUV Global 3d Point Poly Yes Yes Simulation
Route Planning Penalty hedron (No (2d Only)
Function Details)
6/89 Hyland/ Optimal Global 3d Point Spheres Yes Yes Simulation
AUV Obstacle Penalty (Sonar)
Avoidance Function
& Local
Swept
Volume
6/89 Sciller & Tench/ Neural 2d Details Polygons No Yes Simulation
Neural Network Network Unknown (Sonar) (Limited)
Based AUV (Local
Guidance Swept
Volume)
6/90 DeMuth & Neural 2d Polygons Polygons No Yes Simulation
Springsteen/ Network (Sonar)
Neural Network (Local
Obstacle Swept
Avoidance Volume)
6/90 Williams, Global 3d Sphere Spheres Yes Yes Simulation
Lagrace & Penalty (Sonar) (2d only)
Woodfin/ Merit Function
Function
4/92 Schaffer & Herb/ Global 3d Cylinder Cylinder, Yes No Simulation
Robot Arm Path & Sphere Sphere & (Required)
Collision Planning Composite Rectangle
Detector Composite
6/92 Kim & Kholsa/ Global 2d Point Polygons Yes No Simulation
Harmonic Penalty (Required)
Potential Fields Function
9/92 Rimon & Global 2d Point Circles Yes No Simulation
Koditschek/ Penalty (Required)
Navigation Function
Function ____
33
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.
CHAPTER 3
REFLEXIVE MINE AVOIDANCE APPROACH
Overview
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
31 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).
SONAR
REFLEXIVE MINE AVOIDANCE LOGIC DYNAMICS and INS
r 
Preprocessed sonar data
Model
Mine INS Model
Position Comsaded
Tr scking p Local heading.
WIlaan Filter Esti ted Reflexive speed,
Mile mine pitch. etc.
Pitio Positions Avoidance
Loggiag
 Planned
Recollection path
SGlobal
I  Dynsamic
Progresdng
Vehicle Based
ftatus Path Plsaner
I
Figure 31. 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/12scale model of a LOS ANGELES class
submarine [97, pp. 731733]. The dynamic model is relatively simple. Using
commanded heading, pitch and roll as inputs, second order difference equations propagate
36
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).
and
6. Random noise on Doppler velocity (uncorrelated and normally
distributed with zero mean and 0.012 variance (feet/second)2).
37
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.
EnergyBased Sonar Model
Considerable effort has been spent on developing a realistic sonar model to drive
the mine avoidance simulation. The resulting energybased sonar model contains both
sufficient detail to accurately represent the sonar's first order characteristics and enough
flexibility to evaluate virtually any forwardlooking 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],
[99].
The energybased approach uses the standard sonar equations for surface, bottom
and volume reverberation; thermal, ambient and flow noises; twoway transmission loss;
directivity index; etc. Additionally, the sonar simulation precomputes 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 signaltonoise ratios (SNRs) produced, in turn, yield
realistic probabilities of detection (that vary with range and angle) and a constant false
38
alarm rate. The generated sonar data includes the uncertainties caused by the coarse
angular resolution and overlapping beams typical of forwardlooking 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 threerow by fivecolumn beam configuration that encompasses a total of 33
degrees by 55 degrees [100]. Finally, the sonar model incorporates some data
preprocessing. Before the sonar conveys any sonar data to the mine avoidance logic, the
preprocessing procedure thresholddetects 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 energybased sonar model.
Reflexive Mine Avoidance Logic
Overview
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
39
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
association.
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
40
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
nonminelike 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
minelike or nonminelike 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 threedimensional array of sonar data. Two of the array
41
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 preprocessing algorithm first reduces the data by thresholddetecting
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 32 shows what a thresholddetected 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 32. 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 onehalf a foot long, the sonar
could detect the mine in up to twelve contiguous range sample cells. Or if two or more
42
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) (31)
and
Maxlink=MaxsizelCell_Size (integer number) (32)
where
Maxsize =Maximum perceived size of tracked target (feet) (33)
R=Maximum sonar range (feet) (34)
=Onehalf of the maximum of the receiving beam 's
angular bearing coverage and the receiving (35)
beam 's angular elevation coverage (radians)
Maxlink=Maximum number of contiguous range cells that can (36)
be grouped together to form one target (integer number)
and
Cell_size=Sonar range sample cell size (feet) (37)
This process subdivides all targets into a series of spheres. The tracking routine
remembers each sphere as a point with spherical radius Maxsize. Figure 33 illustrates
how the clustering procedure would cluster the threshold detected returns in Figure 32.
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 threedimensional 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 33. 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 34 shows a twodimensional 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 34. 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 realtime
implementation of the mine avoidance capability can be demonstrated. Chapter 5 presents
some preliminary results that address these problems. Figure 35 illustrates a situation
in which the path planner has incomplete mine position information.
Figure 35. Path Planning Example with Incomplete Mine Position Data
The mine positions in Figure 35 are the same as in Figure 34. 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
47
path's distance. Many node searching strategies exist. This research focused on two
node searching strategies, A* and breadthfirst [102]. With respect to planned path
distance, A* is optimal and breadthfirst 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 twodimensional 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 threedimensional.
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 fortyfive 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
48
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 36 shows
the resulting path when the path relaxation procedure is applied to the path in Figure 34.
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
breadthfirst planner. This is because A* initially has a weigh point located in every
voxel through which the planned path passes. Breadthfirst, 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 breadthfirst. This performance
difference is entirely due to the weigh point implementation differences. The A* planner
was developed more recently than the breadthfirst planner. Hence, its design benefitted
from the results of the earlier breadthfirst 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 36. 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 37 and
38 illustrate these three conditions.
Figure 37 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
50
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 37
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 38 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).
*1
Figure 37. 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
Pitch
Figure 38. 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
spheres.
SD = Standoff Distance
U = Size Uncertainty
EC = Elevation Clearance
SM1 = Safety Margin 1
SM3 = Safety Margin 3
52
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 disclike region around the
AUV as illustrated by the shaded area in Figure 39. 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 DiscLike Region
Figure 39. Side View of DiscLike 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 xy plane. Likewise, the region required for elevation clearance moves as
53
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 310 depicts the basis of the LRMA logic.
Synthetic Sonar Coverage
Figure 310. Top View i
54
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 310 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
55
basic requirements previously listed. The heading control function is always active while
the pitch control function can optionally be active or inactive.
Synthetic sonar beamnumbering and inner/outer safety areas. Continuing with the
explanation of the LRMA approach first requires some definitions. Figures 311 and 312
illustrate how to identify each synthetic sonar beam.
Figure 311 indicates that the sonar beam row numbers range from NSR to NSR.
The sonar beam column numbers in Figure 312 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 (38)
Now define the range to the closest target in each Beam(ij) as
TRange(ij); i=NSR,...,NSR; j=NSC,...,NSC (39)
Figure 313 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 (310)
Likewise, Figure 314 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 (311)
Figure 315 shows a side view of the critical elevation clearance sphere area.
Co I umn 0
Figure 311. Synthetic Sonar Beam Row Numbering
Figure 312. 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 313. Top View of Critical Turn Diameter Sphere Area
Head i ng
58
Pitch
SID:
SD = Standoff Distance
U =Size Uncertainty
EC = Elevation Clearance
SM1 = Safety Margin 1
SM3 = Safety Margin 3
Figure 314. 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 315. 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
(312)
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
(313)
(314)
(315)
(316)
(317)
(318)
(319)
(320)
(321)
GOUPEC:
GODNEC:
CANGRT:
CANGLT:
CANGUP:
CANGDN: The AUV can pitch down without violating the
elevation clearance sphere
(322)
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; jO,...,NSC
GOLTTD = TRange(ij) 5 TDRange(ij); i=NSR,...,NSR; jO,...,NSC
GOUPEC = TRange(ij) < ECRange(ij); i=l,...,NSR; j=NSC,...,NSC
GODNEC = TRange(ij) 5 ECRange(ij); i=1,...,NSR; j=NSC,...,NSC
(323)
(324)
(325)
(326)
(327)
(328)
CANGRT = TRange(ij) 5 TDRange(ij); i=NSR,...,NSR; j=1
(329)
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
(331)
(332)
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)
GRTDMN
= Minimum(TRange(ij) TDRange(ij)):
i=NSR,...,NSR; j=1,...,NSC (feet)
(333)
(334)
(335)
(336)
GLTDMN = Minimum(TRange(ij)TDRange(ij));
i=NSR,...,NSRj=1,...,NSC (feet)
RClosest = Minimum(TRange(ij)) i=NSR,...,NSR; j=0,...,NSC (feet)
(330)
(337)
LClosest = Minimum(TRange(ij)) i=NSR,...,NSR; j=0,...,NSC (feet) (338)
8, = heading error = y,,des meurd (339)
= desired heading measured heading (radians)
50 = pitch error = Od, ir measured (340)
= desired pitch measured pitch (radians)
and
8 = roll error = mid O mared (341)
= 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 ratelimited heading error be
elimited = minimum(68,8.mai 8); SV>0.0 (342)
= maximum(8&, maGx,,); 8i<0.0 (radians)
where
mai, = the maximum possible positive heading (343)
change in one control update cycle (radians)
and let
Vratelimited = ratelimited commanded heading change (radians) (344)
The HCF combines these conditions to generate a commanded heading change.
A FORTRANlike description of the heading control function algorithm is indicated
below.
If (GORTSS AND. GOLTSS) Then
If (GRSSMN < GLSSMN) Then
command maximum
Else
command = maximum
End If
Else If (GORTSS) Then
B'command = maximum
Else If (GOLTSS) Then
command = 'maximum
Else If (GORTTD .AND. GOLTTD) Then
If (GRTDMN < GLTDMN) Then
If (CANGRT) Then
command maximum
Else If (CANGLT) Then
command = maxinmum
Else
command = maximum
End If
Else
If (CANGLT) Then
command = maximum
Else If (CANGRT) Then
'Vcommand = maximum
Else
command = mxmium
End If
End If
Else If (GORTTD .AND. CANGRT) Then
klcommand = maximum
Else If (GOLTTD .AND. CANGLT) Then
T command = maxinum
Else
~Wcommand = 8ratelimited
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
66
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
heading.
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 316 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 317 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 316. 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 317. 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.
69
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 ratelimited pitch error be
ratelimited = minimum(, x); 0.0 (345)
= maximum(56,58m); 80<0.0 (radians)
where
50mx = the maximum possible positive pitch (346)
change in one control update cycle (radians)
and let
(347)
80ratelmited = ratelimited commanded pitch change (radians)
70
The PCF combines these conditions to generate a commanded pitch change. A
FORTRANlike description of the pitch control function algorithm is indicated below.
If (GOUPEC AND. GODNEC) Then
If (CANGUP) Then
command maximum
Else If (CANGDN Then
580 = 650
command maximum
Else
command maximum
End If
Else If (GOUPEC .AND. CANGUP) Then
590 = 590.
command maximum
Else If (GODNEC .AND. CANGDN) Then
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 rarelimited
Else If (50<0.0 .AND. CANGDN) Then
command = ratelimited
Else
If (020.0) Then
8_ = minimum(O,850,,.m)
Else
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
71
the surface [105, pp. 180183]. 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 FORTRANlike 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 318 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
ReAcquire Track
Figure 318. 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 reacquires 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 offtrack distance limit, the AUV immediately
enters a flat turn mode. In this mode, the vehicle levels out and performs a 360degree
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 replans a
safe path through the minefield and the avoidance logic enters the acquire new track
mode.
73
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 reenters the on track mode. While
in the acquire new track mode, Figure 318 shows that the mine avoidance logic can still
enter the LR mode to avoid mines. After reacquiring the new track, the mine avoidance
logic reenters 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.
74
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.
CHAPTER 4
ESTIMATING SONAR TARGET POSITIONS
WITH AN EXTENDED KALMAN FILTER
Introduction
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
two.
This chapter first summarizes the continuousdiscrete 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.
76
ContinuousDiscrete 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
scalars.
System
Model
(41)
Measurement
Model
Initial
Conditions
Other
Assumptions
State
Estimate
Propagation
Error
Covariance
Propagation
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 ^
xZ(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,)
(42)
(43)
(44)
(45)
(46)
x'(t)=fx(t),t)+w(t); w(t)N(0,Q(t))
State
Estimate
Update
Error
Covariance
Update
Gain
Matrix
Definitions
where
(47)
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
xh(x(tk,))
Hk( ))= ) )
af.jx(t),t)
Saxi(t)
ahi (tk))
a'
(48)
(49)
(410)
(411)
(412)
(413)
Filter Initialization
Equations (41) through (413) provide a detailed summary of the extended
Kalman filter and its implementation. However, filter initialization has not been
78
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) (414)
and
x(0)=g 8Z) (415)
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 (416).
Initial
State (0) =g () (416)
Estimate
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
79
realistic initial covariance estimate is required. A reasonable procedure to estimate the
initial covariance is developed below [107].
Substituting Equation (414) into (415) yields
(417)
Expanding Equation (417) in a Taylor series and dropping
higher terms produces a first order approximation for the initial state
second order and
(418)
where
(419)
zi = the i'th element of vector zk at time k.
(420)
Substituting Equation (414) into Equation (418) and solving for the state estimate
error results in
(421)
(o) s(O) S(KO 8Z )
x(0)=1(0)4x(0)=g 8 )=g)QA )8j
A(k)= _; ai,= gZ
k Lzk> azi
80
Now note that the state estimation error covariance matrix is defined as
P(t)=E(Gx(t)GxT(t)) (422)
Substituting Equation (421) into Equation (422), rearranging terms and moving
the expectation operation inside produces the desired initial covariance estimate
PoA )E(zS_)A Tr) (423)
Note that the measurement error covariance is
R=E(8zkZ5z) (424)
so that Equation (423) can be simplified to
Initial
Error Po=A(o)RA T() (425)
Covariance
Estimate
Equations (41) through (413) summarize the continuous extended Kalman filter
with discrete measurements while Equations (416) and (425) provide initial state and
covariance estimates.
81
Coordinate System and Sonar Functional Relationships
Overview
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 41 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 41. 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 (432)
y =D y +c
zL LZ~w
(432)
(426)
(427)
(428)
(429)
(430)
(431)
where the rotation matrix is
cosOcosyv
D= sinosin0cosWcososinNI
sinmsinyW+cos(sinecosw
cos0sinx
cosocosy, +sinosinnsisin
cososin0sin sin4cosV
(433)
(434)
O=vehicle roll (radians)
0=vehicle pitch (radians)
vehicle yaw (radians)
(435)
(436)
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 2x, 2 2 + (437)
z elevation (radians) = a Tan I(x (3)/x (1)) Tan (zjx)
bearing (radians) J Tan (2)x(1)) lTanl(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()
x,
z (1)Tan(z(3)) rTan(p) (438)
S +Tan 2((2))+Tan 2((3)) 1 +Tan2( Tan2()
Zs
z(1)Tan(z(2)) rTan(a)
1 +Tan2((2))+Tan 2(3)) 1 +Tan 2(a)+Tan2()
Equations (437) and (438) 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 (439)
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 (440)
z_xh (0 Tan '((dr(x c )+Zb )(LT( > )Xb) (440)
yz w "w x w w
A Tan '((d(C ) yb.)l w ) X
and
r
1 +Tan 2(a) +Tan 2()
wx
rTan(P) b (441)
x =g()= y =D v + c ~ (441)
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 (41) may now be defined
State
Vector X=X,=[w y, zJ]T (442)
System
Model _(t)=0; f(~(t),t)=0; (t)=0; and Q(t)=[0] (443)
Parameters
The measurement vector and measurement model of Equation (42) have already
been defined in Equation (440). 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 (444)
0 0
The initial conditions of Equation (43) and the assumption of Equation (44) are
still valid. Also, note that because the system model parameters listed in Equation (443)
are all zero, Equations (45), (46), (410) and (412) are not used. Before the Kalman
filter update and gain equations described in Equations (47) through (49) can be
implemented, the Jacobian matrix in Equation (411) must first be calculated.
87
T
ah(x(t),t)
aX (tk
S[D (D(,c)b ]
I ))^ C w v
(D(x_ cw) b) )(D w c) b) (445)
1 d wc) xb,) +dT(T( ,)zT,)
(d Cxb) 2
d~wc w)+Xb
+ )x _)WD ,) Xb
!(xc W( ) Xb b rc )Xb)
1[dT c I c
g(x c )xb
Initialization
The initial covariance matrix estimate in Equation (425) requires the calculation
of the Jacobian matrix described in Equation (419). Because this calculation is quite
cumbersome, a separate equation is given below for each matrix element.
Sagk 1 ak 1 (446)
Ak)Z k az=2,1 2,2 a2,3
a3,l a3,2 a3,3
where
ax d,1 +d2,Tan(p)d3,1Tan(a)
a11  (447)
a r 1 +Tan 2(a) +Tan 2(P)
y dl2+d2,zTan() d3,Tan(a)
a2,,  (448)
ar v +Tan 2(a)+Tan 2(p)
azw d13 +d2,3Tan(P)d33Tan(a)
a3,1  (449)
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
(450)
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) (452)
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( (453)
+ (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) (454)
S (1 +Tan 2(a)+Tan 2(1)
cos 2(c13) +Tan 2(a) +Tan 2()
and
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() (45)
(1 +Tan 2(a) +Tan 2()
cos() 1 +Tan 2(a) +Tan 2()
Required Modifications when Navigation Error is Present
Overview
In the presence of navigation error, true vehicle position (x, y, z) and orientation
(roll, pitch, yaw) information is no longer available. Instead, an onboard 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
(456)
tue [X. Y=[x Z, 4c Oe Vc]T = [Xe Ytu Z~'E e 0true Vtrue]T
91
represent the true vehicle states (position and orientation) and let
(457)
represent the measured vehicle states as indicated by the onboard INS; the error in the
vehicle states is defined as
X7~sry'.Ypu
(458)
The measurement model in Equation (440) can now be rewritten to express the
dependence on both the desired state and the vehicle state.
Modified
Measurement
Model
(459)
As expected, the measurement estimate used in Equation (47) is calculated by
evaluating Equation (440) at the current filter state and vehicle state estimates
z^ =h (x tk)' k t(_),() ) (460)
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 (458) into (459) 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
where
H k(x(tk )' ,,, ())
(461)
(462)
(463)
Using the covariance definition and simplifying produces the modified covariance
estimate
Modified
Covariance
Estimate
Rmod=E[ [Hy(8y)+8 z [H/( ) +Sz
=HyE[8y8y T]H,r+E[Sz8z I]
(464)
=HR H T+R
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 (465)
0 0 0 0 0 0
0 0 0 0 0 o2
Substituting Rod, the modified measurement error covariance matrix defined in
Equation (463), in place of R, the measurement error covariance matrix used in Equation
(49) 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 (464) have
been calculated according to Equation (463) 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 (466)
Lh3,1 h3, h3,4 h 3,4 h3,6 h
= r [d11 d2, d3,1,](D(xc)b)
h1 = (467)
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= (468)
ytre (D~ c )b)T(Dw, c ) b)
S r [d13 d,, dd,33](D( X,)b (
hi.= = (469)
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 ar [D w w) (470)
D J(D( c )_bc)r(D(x c )) 
where
0 0 0 T
D,= os sincosy+sinosinw sinscosi/+coscsinesinov coscosi (471)
cososinVysinosin0cosi sin)sinesinVcos4cosiv cossin d T
S r [De'(x c )]T(Dwxc_ )b )
h, tre = (D(x ) (472)
0 (D(x c )_b )(Dx c )b )
y'w w v w w v
