Field Results of the Control, Navigation, and Mapping Systems of aHovering AUV

Nathaniel Fairfield, Dominic Jonak, George Kantor, David WettergreenThe Robotics Institute

Carnegie Mellon UniversityPittsburgh, Pennsylvania 15213 USA

Email: {than, dom, kantor, dsw}@cmu.edu

Abstract

This paper describes the evolution of the control, naviga-tion, and mapping capabilities of a hovering autonomousunderwater vehicle (AUV) designed to explore floodedcenotes in Mexico as part of the DEPTHX project. Thevehicle is equipped with a suite of navigation sensors thatallow it to localize itself and and create maps of the com-plex 3D environments in which it operates. It is passivelystabilized in the roll and pitch directions and equippedwith six thrusters that allow it to directly actuate eachof the remaining degrees of freedom (x, y, z, and head-ing). We describe the control system of the vehicle, whichprovides open-water and near-wall manuevering capabil-ities. Near-wall behaviors are used in support of scienceoperations such as visual wall survey and precise place-ment of a core sampling tool. We also present the vehi-cle’s navigation system, which combines dead-reckoning,sonar-based localization, and simultaneous localizationand mapping (SLAM). We demonstrate the performanceof these systems using experimental results from multipletest environments, including a test tank, a flooded lime-stone quarry, and the La Pilita cenote. Trajectory plotscomparing desired and actual motions will be used todemonstrate the various vehicle controllers. Data fromrepeated localization experiments will be used to providestatistically meaningful measures of the accuracy of thedead-reckoning, localization, and SLAM filters.

Figure 1: The DEPTHX AUV deployed in Poza La Pilita, withthe solid core sampling arm extended.

1 Introduction

The DEPTHX (DEep Phreatic THermal eXplorer) projectis a three-year NASA-funded effort whose primary objec-tive is to use an autonomous vehicle to explore and char-acterize the unique biology of the Zacaton cenote. Za-caton, the world’s deepest known limestone sinkhole, is awater-filled cavern that is at least 300 meters deep. Thedepths of Zacaton are geothermally heated with a highsulfur content and a lack of sunlight or dissolved oxy-gen, making this an ideal place to search for exotic micro-bial life [Gary, 2002]. The robotic exploration and searchfor microbial life in Zacaton is an analog mission for the

1

search for life in the liquid water ocean beneath the frozensurface of Europa.

The DEPTHX robot (Figure 1) is a hovering au-tonomous underwater vehicle (AUV) designed to exploreflooded caverns and tunnels while building 3D maps, col-lecting environmental data, and obtaining samples fromthe water column and cavern walls. To accomplish thesetasks, the vehicle is equipped with a Doppler velocity log-ger (DVL), a ring laser gyro-based inertial navigation sys-tem (INS), a depth sensor, and an array of 56 narrow beamsonar transducers.

The paper is organized as follows: related research ac-tivities in AUV control and the use of AUVs for scienceand mapping are briefly discussed in Section 2. Thenwe provide a description of the instrumentation of theDEPTHX vehicle in Section 3. An overview of the ve-hicle control system is provided in Section 4. Likewise,the navigation and mapping system is described in Sec-tion 5. Experimental results are presented in Section 6,followed by some conclusions and a discussion of futurework in Section 7.

2 Related WorkThere are a variety of techniques employed for determin-ing the position of underwater vehicles – unfortunatelyGPS does not work underwater – and they can be dividedinto those that utilize emplaced infrastructure and thosethat do not. (See Leonard et al. [1998] for a survey.)

When accurate position is needed underwater, manyAUVs and human-driven remotely operated vehicles(ROVs) rely on a surveyed array of acoustic beacons,known as a long base-line (LBL) array. Acoustic beaconsprovide a fixed frame of reference for positioning the ve-hicle [Whitcomb, 2000].

Over large distances, or in underwater caves and tun-nels, the performance of LBL systems is unknown due tosignal attenuation, reverberation and multipath. Withouta fixed LBL infrastructure, an AUV uses a combinationof depth sensors, inertial sensors, and Doppler velocitysensors to compute a dead reckoned estimate of its po-sition while at depth. With high accuracy attitude anddepth sensors the uncertainty in the AUV’s 3D pose (roll,pitch, yaw, x, y, z) is primarily in x and y. Most un-derwater navigation systems are based on a Kalman Filter

Figure 2: A model of the DEPTHX vehicle structure and com-ponents with buoyancy removed for clarity. Eleven pressurevessels house computing, batteries, sensors, and science instru-ments. Diameter is approximately 2m. c©Stone Aerospace,2006.

which combines Doppler velocity and inertial measure-ments. These systems report navigation errors as low as0.1% of distance traveled [Larsen, 2000], however perfor-mance degrades in situations where the DVL is unable tomake accurate velocity measurements.

The dead reckoned estimate will accumulate error, andwhen the drift exceeds what is required for the applica-tion, a correction must be made by (re)observing a knownreference. A common approach is to surface and obtainposition from a GPS. If LBL or surfacing is not an op-tion, the position error can be bounded by simultaneouslocalization and mapping (SLAM) [Williams et al., 2000][Dissanayake et al., 2001]. Williams and Mahon [2004]provide an example of near-bottom mapping with sonarsand cameras of coral reefs using SLAM. Roman [2005]uses multibeam sonar maps to do SLAM over varied to-pography.

3 Vehicle Description

The DEPTHX vehicle is a hovering AUV that has beendesigned for exploration of flooded caverns and tunnels.An oblate spheroid shape approximately 1.5 m in heightand 2 m in width (Figures 1 2), the vehicle’s dry mass is1400 kg. Vehicle roll and pitch are stabilized by the sep-

2

aration of buoyancy and ballast. The vehicle can movedirectly in the remaining four degrees of freedom (for-ward, starboard, down, and heading) using six thrustersdriven by brushless DC motors. The cruising speed of thevehicle is about 0.2 m per second. The vehicle is poweredby two 56-volt Lithium-Ion battery stacks with a total ca-pacity of 6.2 KWh, enough to supply the vehicle during afour hour exploration mission.

The DEPTHX vehicle has a full suite of underwaternavigation sensors, including a Honeywell HG2001ACINS, two Paroscientific Digiquartz depth sensors, and anRDI Navigator 600kHz DVL. The specifications for theINS are roll/pitch: 0.2◦ 2σ, yaw: 0.4◦2σ, for the DVLvelocities 0.3 cm/s 1σ, and for the depth sensors 0.01%of full range (10 cm for our 1000m rated sensors). Thetwo depth sensors are zeroed with respect to atmosphericpressure at the start of each day. The DVL is mountedto the front of the vehicle facing forward and tilted down30 degrees from horizontal, a nonstandard configurationfor this instrument. The usual DVL configuration pointsstraight down so that it can achieve lock on the oceanfloor. In our application, it is difficult to predict the rel-ative direction to surfaces useful for DVL lock. The top280 meters of Zacaton is known to be a chimney witha diameter of approximately 80 meters [Fairfield et al.,2005], so the forward-looking configuration should allowthe DVL to lock on to one of the vertical walls in mostsituations. This configuration can cause the DVL to losebottom lock in more wide-open waters. Loss of bottomlock can also occur at extremely short ranges, or whenpassing over highly irregular terrain.

For our purposes, the raw roll, pitch, and yaw mea-surements provided by the IMU and the depth measure-ments provided by the depth sensors are accurate enoughto be considered absolute measurements of those quanti-ties. The task of determining the location of the vehicle isthen reduced to the two dimensional problem of estimat-ing its position in the horizontal plane.

For mapping, the vehicle has an array of 56 2◦ beam-width sonars that provide a constellation of range mea-surements around the vehicle. This array is in the shapeof three great circles, a configuration that was arrived atafter studying the suitability of various sonar geometriesfor the purposes of SLAM [Fairfield et al., 2005]. Thesonars have long ranges (some 100m and others 200m)and the accuracy of the range measurements is fairly high

(about 10cm), and were usually fired at about 1 Hz. Thelow resolution, slow update rate, and sparse point densitymakes the mapping problem significantly more difficultthan it is with ranging sensors like a laser scanner thatprovide fast, accurate, and high density ranges.

4 ControlThe DEPTHX vehicle has a three-level control systemthat is used to guide the vehicle on its mission. The lowestlevel, aptly named the low level control system (LLCS),employs velocity feedback from the DVL and IMU in or-der to generate the thrust necessary to track a desired ve-hicle frame velocity command. The middle level, namedthe navigator, issues velocity commands to the LLCS inorder to achieve the immediate goal. In this paper, theimmediate goal is to drive the vehicle to a specified way-point, however the navigator also is capable of executingmore general behaviors such as wall following and obsta-cle avoidance. At the highest level is the system executivethat, among other things, issues a series waypoint com-mands to the navigator in order to accomplish the overallmission.

4.1 Low Level Control SystemThe LLCS performs two basic functions: it uses veloc-ity feedback to convert a vehicle frame velocity commandinto the vehicle frame thrust needed to track that veloc-ity and it implements a mixing table in order to convertthe vehicle frame thrust command into the necessary shafttorque 1 commands to each of the individual thrusters.

Velocity feedback is implemented in four independentloops, one for each of the vehicle’s four degrees of free-dom. Each loop contains an experimentally tuned PI con-troller. Note that this structure assumes that the compo-nents of vehicle frame velocity are not coupled by thedynamics of the vehicle, an assumption which not true.In particular, the forward and sideways velocity compo-nents of the vehicle will be highly coupled when the vehi-cle simultaneously rotates and moves in the lateral plane.

1The relationship between shaft torque and thrust is very nearly lin-ear, and we rely on the DriveBlokTM controller produced by MTS Sys-tems Corp to implement the desired shaft torque on the brushless DCthruster motors.

3

Hence we can enforce the decoupled assumption by sim-ply avoiding these type of motions, a restriction which iscompatible with the slow, deliberate types of missions thatthe vehicle will undertake. In practice, however, the con-troller performs well even when such coupling motionsare executed.

The thrust mixer maps the vehicle frame thrust com-mand vector Fv = [Fω, Fx, Fy, Fz]T into a vector of nindividual thruster commands T = [τ1, τ2, τ3, . . . , τn]T ,where n is the number of thrusters. It is implemented as amatrix multiplication, i.e., T = MFv , where M is com-puted as follows. First, let A be the 6 × n matrix whoseith column is given by

Ai =[pi ×Di

Di

],

where pi is a vector describing the location of the iththruster in the vehicle frame, and Di is a vector describ-ing the positive thrust direction of the ith thruster in thevehicle frame. Now let B be the 4 × n matrix that isthe bottom four rows of A. B is the matrix that mapsthe individual thruster thrusts T into the resulting vehicleframe thrust vector Fv . Assuming that the rows of B arelinearly independent, M can then be found by taking thepseudoinverse of B:

M = BT(BBT

)−1.

Note that in the nominal case, the number of thrusters isn = 6. However, this formulation allows the mixing ma-trix to easily be recomputed in the event of thruster failure.

4.2 Open-waterWaypoint(): Open water waypoint

FlyUpward(): Reactive abort strategy

StaionKeeping(): Holds position and heading

4.3 Near-wallThe vehicle maneuvers relative to a nearby wall using thefollowing behaviors:

ApproachWall(d0, θ0): Approach the nearest wall at astandoff distance of d0 and a relative heading of θ0

Figure 3: Velocity tracking performance of the LLCS for a com-manded velocity pulse. The duration of the leading and fallingedge transients is on the order of 4 seconds.

while maintining constant depth. The relative head-ing is defined to be zero when the vehicle directlyfaces the wall.

WallFollow(∆`,∆z): Move rightward ∆` and down-ward along wall by the distance ∆z while maintain-ing constant standoff distance and relative heading.

In order to provide these capabilities, we have devel-oped a collection of reactive motion primitives that wecall the proxops controller. The proxops controller usesfeedback from the mapping sonars to fit a plane to thenearest wall, then issues a velocity command to the ve-hicle in order to achieve the desired goal. The velocitycommand is sent to a low-level velocity controller thatuses IMU and DVL feedback to determine the thruster in-puts necessary to track the desired velocity command.

At the heart of the proxops controller is an inverted dy-namics controller derived from the kinematic equationsof motion of the vehicle. The equations of motion are de-rived from the assumption that the wall in front of the ve-hicle is approximately planar. Under this assumption, thestandoff distance d, relative heading θ, and lateral position` can be defined as shown in Figure 4. This geometry canbe used to determine the derivatives of these quantities asa function of vehicle velocity:

d = −vx + vy tan θ + dω tan θ,

4

Figure 4: Proximity operations - Definition of control parame-ters.

controller commandsd0 desired wall distance`0 desired lateral travelθ0 desired wall angle

controller parametersγd wall distance convergence rateγ` lateral convergence rateγθ wall angle convergence ratekdx forward damping gainkdy lateral damping gainkdω wall angle damping gain

Table 1: Controller notation

˙ = vx sin θ + vy cos θ + dω cos θ + d sin θ,

θ = ω

where vx, vy , and ω are respectively the forward, star-board, and angular velocities of the vehicle.

Using these equations of motion, we determine a prox-ops control law that maps current wall geometry (d, θ, and`) and vehicle velocity measurements (vx, vy , and ω) intodesired vehicle velocities (vxd, vyd, and ωd):

vxd = γd(d− d0) + vyωtanθ + dωtanθ − kdxvx,

vyd = −ωd (cosθ + sinθtanθ)− γ`(`− `0)cosθ + ωsinθtanθ

− kdyvy,

ωd = −γθ(θ − θ0)− kdωω,

where the controller parameters and commands are de-fined in Table 1.

This controller assures exponential convergence of theproxops variables d, `, and θ to d0, `0, and θ0, respec-tively. This can be verified by substituting the desired ve-locities into the equations of motion. The damping terms

Figure 5: Vehicle response to ApproachWall command.

are hand tuned to account for the fact that the underlyingvelocity controller is not perfect (e.g., that ωd is not equalto ω). This control law can be used to achieve all of thedesired proxops behaviors by choosing appropriate valuesfor d0, `0, and θ0.

Figures 5 and 6 depict the vehicle response to two typesof proxops commands. In both cases these tests were con-ducted in the same 15 m diameter neutral buoyancy facil-ity used for the SLAM navigation results reported earlier.

In Figure 5 the plots show the vehicle response to anApproachWall command. The vehicle starts at a distanceof 4 m from the wall and is commanded to move to astandoff distance of 2 m at approximately t = 4 seconds.The vehicle reaches its goal at approximately t = 46 sec-onds. The relative heading and lateral motion are main-tained at zero during this maneuver.

In Figure 6 the response to a WallFollowLateral com-mand is shown. Here the vehicle is commanded to followthe wall laterally in the starboard direction for a distanceof 5 m. The command is issued at t = 20 seconds, andthe goal is reached at about t = 190 seconds. Wall dis-tance and angle are held constant. Note that there is sig-nificant noise in the distance and angle measurements dueto noisy sonar data (a function of the test tank geometryand the frequency of the tranducers). A noise filter is cur-rently being implemented, but nonetheless the controllerperforms well in the presence of this noise.

5

Figure 6: Vehicle response to WallFollow command.

5 Navigation

5.1 Navigation Architecture

The DEPTHX vehicle used three different navigationmethods: dead reckoning, localization with a prior map,and simultaneous localization and mapping. These meth-ods are described below, and are listed in order of com-plexity. Onboard the vehicle, a simple switching processdecided which of the three navigation solutions to use.

5.2 Dead Reckoning

Dead reckoning is the process of integrating the vehiclevelocities over time in order to produce a position esti-mate. In our implementation of dead-reckoning, the algo-rithm falls back to the IMU velocity measurement whenDVL measurements are not available. Unlike the DVL ve-locities, the IMU velocities drift over time since they arethe result of the integration of accelerometers, and thisleads to a drift in the position estimate. When the DVLvelocities are good, they are used to estimate and correctfor the IMU drift using a standard Kalman filter. This al-lows the IMU to fill in for brief DVL dropouts from noise,switching from water column tracking to wall tracking,and loss of lock when too close to a surface.

Under normal circumstances dead reckoning can pro-vide navigation with a divergence rate of 0.5% of dis-

Figure 7: A single 6◦ sonar beam as represented in an evidencegrid.

tance traveled. The DEPTHX vehicle can run a 4 hourmission at an average cruising speed of 0.2 m/s, and thusan exploration range of approximately 3000 m. Over thisrange, the dead-reckoning error would be on the order of15 m. DVL dropouts significantly degrade this positionestimate.

5.3 3D Map

Sonar measurements are noisy and unable to resolve finefeatures, but over time they do provide information aboutthe environment around the vehicle. In order to combinethe individual sonar measurements, the DEPTHX vehicleuses a 3D evidence grid (see Martin and Moravec [1996]for a description of the classic 2D evidence grid). In a 3Devidence grid, space is uniformly discretized into cubicvoxel elements.

A measurement model is used to determine how a par-ticular sonar measurement affects the map. We modeledthe 56 individual sonars as producing 2-degree cones pro-jecting from the vehicle (see Figure 7). Given a particularrange measurement, our model states that voxels withinthe cone are probably empty and voxels at the end of thecone are probably occupied. A modified Bresenham 3Dray tracing algorithm [Bresenham, 1965] is used to mergethese probabilities with the information already containedin the 3D evidence grid, according to a Bayesian updaterule.

A major drawback of the 3D evidence grid approachis that the memory required to store them increases asthe cube of the size of the map. For reasonable mapsizes and resolutions, the memory requirements quicklybecome intractible, especially considering that the parti-cle filter described below requires hundreds of maps. Tocope with this storage and processing problem we use the

6

Deferred Reference Counting Octree (DCRO) data struc-ture described by Fairfield et al. [2007]. The DRCO is adrop-in replacement for a standard 3D evidence grid thatexploits shared regions between particle maps and effi-ciently represents sparse volumes, yielding a significantperformance boost that allows us to represent maps thatwould not even fit into memory as a uniform array.

5.4 LocalizationIf the vehicle already has a map of the environment then itcan use that map, together with its range sensor measure-ments, to localize itself. The particle filter localizationalgorithm has the following steps:

Initialize The particles start with their poses s0 initial-ized according to some initial distribution.

Predict The dead-reckoned position innovation ut iscomputed using the navigation sensors (IMU, DVLand depth sensor). A new position st is predicted foreach particle using the vehicle motion model (see Ta-ble 3):

st = h(st−1, ut, N).

This new distribution of the particles is called theproposal distribution.

Weight The weight w for each particle is computed usingthe measurement model and the sonar range mea-surements (from #obs different simultaneous sonarobservations):

w = η

#obs∏n=1

p(nzt|st,Θ)

where η is some constant normalizing factor (dif-ferent than the one used in the expression for theBayesian filter). In our implementation, the realrange measurements z are compared to ray-tracedranges z using the particle pose and map. We com-pare the simulated and real ranges using the mea-surement model

z = g(st, ut, N(0, σz)),

which is assumed to have a normal noise model, so

p(z|s,Θ) =1√2πσ2

z

e−(z−z)2

2σ2z .

Substituting into the expression for particle weightand taking the logarithm of both sides shows thatmaximizing this weight metric is very similar to min-imizing the intuitive sum squared error metric:

log w = C − 12σ2

#obs∑i=1

(iz − iz)2,

where C = #obs × log(√

2πσ2)

. An alternativeweighting method, called “point correlation” wasfound to be slightly less informative [Fairfield et al.,2005].

Resample The O(n) algorithm described in [Arulam-palam et al., 2002] is used to resample the set of par-ticles according to the weights w such that particleswith low weights are likely to be discarded and par-ticles with high weights are likely to be duplicated.The set of particles is now our new estimate of thenew vehicle position posterior.

Estimate Generate a position estimate from the particles:when the PF is being used to provide a pose for therest of the vehicle control software, we usually wantto turn the set particles into a single point estimate.

Repeat from Predict

5.5 SLAMSimultaneous Localization and Mapping (SLAM) is thetask of building a map of the environment from sensordata and simultaneously using that map to localize, orrecover the robot’s actual trajectory. In most cases therobot uses various sensors to measure its own motion andsense its local surroundings. This sensor data is inevitablynoisy, and must be appropriately filtered as part of SLAM.

There are a number of different methods which are usedto perform SLAM, the most common being based on thewell-known Kalman Filter. We present a SLAM methodbased on a particle filter, another standard approach that isoften used when certain requirements of the Kalman Fil-ter formulation (unimodal position distributions, featuredetection) cannot be satisfied.

In the most general sense, particle filters sample overthe entire state space of the vehicle. As long as this state

7

s(m)t vehicle pose of the m-th particle at time t

= (roll, pitch, yaw, x, y, z)T

S(m)t trajectory of m-th particle from time 1 to t

= {s(m)1 , s

(m)2 , . . . , s

(m)t }

zt sonar measurements at time tZt history of measurements from time 1 to t

= {z1, z2, . . . , zt}ut vehicle dead-reckoned innovation at time tUt history of dead-reckoning from time 1 to t

= {u1, u2, . . . , ut}w

(m)t m-th particle weight at time t

Table 2: Particle filter notation.

N(µ, σ) normally distributed noise withmean µ and std dev σ

h(st−1, ut, N(0, σs)) vehicle motion model= p(st|ut, st−1)

g(st, Θ, N(0, σz)) sonar measurement model= p(zt|st, Θ)

Table 3: Model notation.

space is fairly low dimensional, as in the case of estimat-ing the 3D position of a vehicle, a few hundred sampleswill adequately represent the true distribution. But forSLAM, the state space of the vehicle is both the vehicleposition and the map. This is a very high dimensionalspace, so we can’t possibly apply the basic particle filter:each particle would be a sample from all possible mapsand all possible positions within that map.

The goal of SLAM is to estimate the probability dis-tribution at time t over all possible vehicle states s andworld maps Θ using all previous sensor measurements Zt

and control commands Ut (for a complete list of notation,see Table 2):

p(s,Θ|Zt, Ut).

This distribution is called the SLAM posterior. Therecursive Bayesian filter formulation of the SLAM prob-lem is straightforward (see Montemerlo et al. [2002] fora derivation) but the integral is usually computationally

intractable to solve in closed form:

new posterior︷ ︸︸ ︷p(st,Θ|Zt, Ut) = η ×

measurement model︷ ︸︸ ︷p(zt|st,Θ) ×∫

p(st|st−1, ut)︸ ︷︷ ︸motion model

p(st−1,Θ|Zt−1, Ut−1)︸ ︷︷ ︸old posterior

dst−1,

where η is a constant scale factor from Bayes’ rule.The key insight of Murphy [1999] is that the SLAM

posterior distribution can be factored into two parts, ormarginals: the path distribution and the map distribution.Furthermore, knowing the vehicle’s trajectory St makesthe observations Ut conditionally independent, so that themap sample Θ can be computed in a closed form. Theprocess of factoring a distribution such that one part canbe computed analytically is known as Rao-Blackwell fac-torization [Doucet et al., 2000]. As a result, followingMontemerlo et al. [2002] we compute the posterior overtrajectories, and factor the distribution as

p(St,Θ|Zt, Ut) = p(St|Zt, Ut)p(Θ|St, Zt).

Particle filters are a Monte Carlo approximation to theBayesian filter. The particle filter maintains a discrete ap-proximation of the SLAM posterior using a (large) set ofsamples, or particles. The m-th instance of the #par par-ticles represents both a sample pose S

(m)t from the distri-

bution of vehicle trajectories, and the sample map Θ(m)

which results from that trajectory combined with the sen-sor measurements Zt. Since we update the particle mapsat every time-step, they represent the combination of sen-sor measurements and vehicle trajectory – so each particleonly needs to store the current map Θ(m) and pose s

(m)t

(rather than the whole trajectory S(m)t ).

For practical purposes, when SLAM is being used toprovide a pose for the rest of the vehicle control software,we usually want to turn the set particles into a single pointestimate. If the posterior distribution is Gaussian, thenthe mean is a good estimator, but other estimators may bebetter if the distribution becomes non-Gaussian.

The particle filter SLAM algorithm maintains a map foreach particle (the maps are initialized either blank or witha partial map of the world), and appends the map updatestep after the resample step in the localization sequencedescribed above:

8

Figure 8: A map of quarry bathymetry generated using weightedk-nearest neighbor interpolation between sonar points. Coordi-nates are in truncated UTM northings and eastings. Very shal-low regions were manually mapped with a canoe. Backgroundimage courtesy Google, c©Europa Technologies, 2007.

Update The measurements z are inserted into the particlemaps Θ(m) to update the evidence of all the voxelsθ which lie in the conic sonar beam model of eachmeasurement relative to the particle position. Thisis when maps must be copied and updated. We saveduplicate insertions by inserting before copying suc-cessfully resampled particles.

6 Experiments

6.1 Hyde Park Baptist Church Quarry

We ran field tests of the DEPTHX dead reckoning systemin a flooded limestone quarry in Austin, Texas (Figure 8.The quarry is a triangle roughly 200 m by 300 m, with amaximum depth in the south corner of 15 m.

Figure 9 shows the performance of the dead reckoningsystem during a long raster mission to map the quarry.This mission had a total path length of over a kilometer,at the end of which the positioning error was 3.16 me-ters. This gives a respectable dead reckoning accuracyof less than half a percent of distance traveled. It shouldbe noted that GPS fixes used as ground truth in this ex-periment were taken using a hand held non-differential

Figure 9: A comparison of dead reckoning localization perfor-mance with and without the IMU velocity Kalman filter over araster scan mission of the quarry. Coordinates are in truncatedUTM northings and eastings.

GPS receiver from a moving boat, so it is likely that thedead reckoning estimate is actually more accurate than the“ground truth”.

The red dots in Figure 9 denote locations where theDVL failed to achieve bottom lock. During this 6300 sec-ond mission, the vehicle was without DVL measurementsfor a total of about 780 seconds. Some of these dropoutperiods were as long as sixty seconds. Given this DVLperformance, it would have been impossible to achieveany reasonable dead reckoning estimate without patchingin the IMU velocities as estimated by the Kalman filter.

6.2 ARL Tank Test

We first tested SLAM in a cylindrical tank (Figure 10),where the vehicle drove three cycles around a 3D box pat-tern (Figure 11), using dead-reckoning for real-time nav-igation. The box pattern was 8m on a side and 5m deep,and each cycle took about 13 minutes for a total run timeof 40 minutes. The vehicle rotated ∼ 5◦/s during ascentand descent in order to obtain better sonar coverage of thewalls.

To establish the ground truth trajectory of the vehi-cle, we localized with 3000 particles and a manually con-structed 0.25m resolution prior map of the ARL tank. Thedead-reckoned trajectory drifted from the ground-truth by

9

Figure 10: The large wooden test tank at the University of Texasat Austin Applied Research Lab (ARL) is a cylinder 11.6 m deepand 16.8 m in diameter. Shown here is the vehicle next to theoperator hut, which is in the middle of a bridge over the tank.

∼0.5m, which agreed with our observations during thetest. We then ran SLAM using 500 particles (with noprior map), which yielded a bounded localization error of∼0.1m (Figures 12 and 13).

6.3 La Pilita

La Pilita is a smaller, much more easily accesible forma-tion that is part of the same system of cenotes as Zacaton(Figure 14). Although it exhibits much of the same mor-phology and biology as Zacaton, La Pilita is only about100 m deep, and thus within the range of specializeddivers if the vehicle were lost.

We successfully ran SLAM onboard the vehicle in LaPilita with 300 particles and used it to provide the real-time navigation to control the vehicle (Figure 15). Due tothe lack of ground truth, we can’t make any strong asser-tions as to the accuracy of the SLAM solution. Likewise,we can’t claim any great improvement over dead reckon-ing, as both solutions accurately returned to the vehicle’sstarting location (our only ground truth point) and usuallydiffered by less than a meter (Figure 16).

Figure 11: This figure shows the 3D trajectory of the vehicle inthe ARL test tank, as well as a rendering of the vehicle and itssonar beams. The vehicle is surrounded by the cloudy evidencemap constructed by SLAM, where opacity indicates occupancy.

7 Conclusion

The DEPTHX vehicle is a demonstrated platform forperforming exploration of fully 3D underwater environ-ments. We have described the control and navigation sys-tems, and demonstrated the performance of these systemsusing experimental results from multiple test environ-ments, including a test tank, a flooded limestone quarry,and a cenote. Our particle filter approach to SLAM ap-pears to work reliably, despite the low resolution, noisy,sparse, and low rate range data afforded by the pencilbeam sonars – however we lack the ground truth datanecessary to make definitive statements about its perfor-mance.

Thanks

We would like to thank the staff at ARL and the HydePark Baptist Church in Austin, Texas. We would alsolike to thank the other members of the DEPTHX teamfor their roles in bringing the DEPTHX vehicle together.In particular, we thank John Kerr and Bill Stone atStone Aerospace and Marcus Gary at the Departmentof Geology at the University of Texas, Austin. Thiswork was funded by the NASA ASTEP program, grantNNG04GC09G.

10

Figure 12: Planar XY view of the trajectories of the various lo-calization solutions in the ARL test tank. The deadReck solutionlooks quite square as it was used to navigate during the test, butthe true vehicle trajectory is shown by locOnly (localization witha prior map and 3000 particles).

ReferencesS. Arulampalam, S. Maskell, N. Gordon, and T. Clapp.

A tutorial on particle filters for on-line non-linear/non-gaussian bayesian tracking. IEEE Transactions on Sig-nal Processing, 50(2):174–188, February 2002.

J. Bresenham. Algorithm for computer control of a digitalplotter. IBM Systems Journal, 4(1):25–30, 1965.

G. Dissanayake, P. Newman, S. Clark, H. F. Durrant-Whyte, and M. Csorba. A solution to the simultane-ous localisation and map building (SLAM) problem.IEEE Transactions on Robotics & Automation, 17(3):229–241, 2001.

A. Doucet, N. de Freitas, K. Murphy, and S. Russell. Rao-blackwellised particle filtering for dynamic bayesiannetworks. In Proc. of the Sixteenth Conf. on Uncer-tainty in AI, pages 176–183, 2000.

N. Fairfield, G. Kantor, and D. Wettergreen. Three dimen-sional evidence grids for SLAM in complex underwa-ter environments. In Proc. of the 14th Intl. Symposium

Figure 13: Distance between various localization solutions inthe ARL test tank. The ground truth was established using lo-calization with a prior map and 3000 particles (and agrees withour coarse observations during the test) – dead reckoning driftsaway while SLAM error is bounded.

of Unmanned Untethered Submersible Technology, Au-gust 2005.

N. Fairfield, G. Kantor, and D. Wettergreen. Real-timeslam with octree evidence grids for exploration in un-derwater tunnels. Journal of Field Robotics, 24:3–21,February 2007.

M. Gary. Understanding Zacaton: Exploration and initialinterpretation of the world’s deepest known phreaticsinkhole and related karst features, southern Tamauli-pas, Mexico. Karst Frontiers, Karst Waters InstituteSpecial Publication, 7:141–145, 2002.

M. B. Larsen. High performance doppler-inertial navi-gation experimental results. In Proc. of IEEE/MTSOCEANS, pages 1449–1456, 2000.

J. Leonard, A. Bennett, C. Smith, and H. Feder. Au-tonomous underwater vehicle navigation. Technical re-port, MIT Marine Robotics Laboratory, 1998.

M. Martin and H. Moravec. Robot evidence grids. Tech-nical Report CMU-RI-TR-96-06, Robotics Institute,Carnegie Mellon University, Pittsburgh, PA, March1996.

11

Figure 14: La Pilita aerial view, with the DEPTHX vehicle atthe surface. The mouth of the cenote is only about 30 m acrossalthough it opens up considerably at depth. Courtesy RobynGary and Jason Sahl.

M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit.FastSLAM: A factored solution to the simultaneous lo-calization and mapping problem. In Proc. of the AAAINational Conference on Artificial Intelligence, pages593–598, 2002.

K. Murphy. Bayesian map learning in dynamic envi-ronments. In Neural Information Processing Systems,pages 1015–1021, 1999.

C. Roman. Self Consistent Bathymetric Mapping fromRobotic Vehicles in the Deep Ocean. PhD thesis,Massachusetts Institute of Technology & Woods HoleOceanographic Institution, May 2005.

L. Whitcomb. Underwater robotics: Out of the researchlaboratory and into the field. In IEEE 2000 Interna-tional Conference on Robotics and Automation, May2000.

S. Williams and I. Mahon. Simultaneous localisation andmapping on the great barrier reef. In Proc. of IEEE Intl.Conf. on Robotics and Automation, volume 2, pages1771–1776, April 26-May 1 2004.

S.B. Williams, P. Newman, G. Dissanayake, andH. Durrant-Whyte. Autonomous underwater simulta-neous localisation and map building. In Proc of IEEEConf. on Robotics and Automation, May 2000.

Figure 15: An east-facing orthogonal view of a SLAM evidencegrid of La Pilita. The maximum depth of La Pilita is 117 m, andits width is about 100 m, while the narrow neck that opens to thesurface is only about 30 m wide. In this figure, yellow indicatesan occupancy isosurface, and red indicates a vacancy isosurface.Individual beams, caused by spurious sonar measurements, canbe seen projecting through the surface of the cenote.

Figure 16: A perspective view of the nested star path that thevehicle followed to map La Pilita, the dead-reckoned trajectoryin yellow, and the SLAM trajectory in green.

12