sonar-based iceberg-relative auv localization...state space. in this way, the particle lter uses a...

9
SONAR-BASED ICEBERG-RELATIVE AUV LOCALIZATION Peter Kimball 1 [email protected] Stephen Rock 1,2 [email protected] 1 Dept. of Aeronautics & Astronautics Stanford University 496 Lomita Mall Stanford, CA 94305 2 Monterey Bay Aquarium Research Institute 7700 Sandholdt Road Moss Landing, CA 95039 Abstract This paper discusses the localization portion of a new iceberg-relative navigation technique for Au- tonomous Underwater Vehicles (AUVs). An esti- mator is presented which correlates incoming sonar range returns with an a-priori iceberg surface map in order to provide iceberg-relative AUV position and orientation estimates. The technique works by maintaining estimates not only of iceberg-relative AUV position and orientation, but also of the trans- lational and rotational velocities of the iceberg it- self. In addition to the a-priori iceberg surface map, required inputs to the estimator are measured iner- tial vehicle displacements and measured sonar range vectors from the vehicle to the iceberg. Details of a particle filter implementation of the estimator are provided along with example localiza- tion results. The example data were collected from the RVIB Nathaniel B Palmer during circumnaviga- tion of an iceberg in the Scoita Sea. In the example results, the vehicle position with respect to the berg is estimated to within 2 m, and the iceberg trans- lational and rotational velocities are estimated to within 20%. 1 Introduction Large free-floating icebergs are traveling ecosys- tems of great interest to science. Because they are relatively difficult and dangerous to approach, they are attractive targets for exploration by Au- tonomous Underwater Vehicles (AUVs). However, because they are moving and rotating, standard AUV navigation methods are unable to provide iceberg-relative position estimates. These estimates can be achieved by a new AUV navigation solution comprised of two steps: mapping and localization. The mapping step was described in a recent pa- per [1]. The localization step is presented here. Figure 1: Photograph of iceberg used for experiment To operate in the under-ice environment, an AUV must possess some capability of navigation with re- spect to the ice surface. Existing under-ice AUV navigation systems have typically relied on dead- reckoning using inertial measurements and/or ve- locity measurements from a Doppler sonar (both seafloor-relative and ice-relative). These systems have been used successfully to move between pre- programmed waypoints in horizontal inertial space, with depth determined by reactive terrain-following (or ice-following) and obstacle avoidance behaviors. However, these existing systems cannot provide ac- curate ice-relative position estimates around free- floating, rotating icebergs. Specifically, accelerom-

Upload: others

Post on 24-Feb-2021

0 views

Category:

Documents


0 download

TRANSCRIPT

Page 1: SONAR-BASED ICEBERG-RELATIVE AUV LOCALIZATION...state space. In this way, the particle lter uses a nite number of particles e ciently - only in likely regions of the state space. 3

SONAR-BASED ICEBERG-RELATIVE AUV LOCALIZATION

Peter Kimball1

[email protected] Rock1,2

[email protected]

1Dept. of Aeronautics & AstronauticsStanford University

496 Lomita MallStanford, CA 94305

2Monterey Bay Aquarium Research Institute7700 Sandholdt Road

Moss Landing, CA 95039

Abstract

This paper discusses the localization portion of anew iceberg-relative navigation technique for Au-tonomous Underwater Vehicles (AUVs). An esti-mator is presented which correlates incoming sonarrange returns with an a-priori iceberg surface mapin order to provide iceberg-relative AUV positionand orientation estimates. The technique works bymaintaining estimates not only of iceberg-relativeAUV position and orientation, but also of the trans-lational and rotational velocities of the iceberg it-self. In addition to the a-priori iceberg surface map,required inputs to the estimator are measured iner-tial vehicle displacements and measured sonar rangevectors from the vehicle to the iceberg.

Details of a particle filter implementation of theestimator are provided along with example localiza-tion results. The example data were collected fromthe RVIB Nathaniel B Palmer during circumnaviga-tion of an iceberg in the Scoita Sea. In the exampleresults, the vehicle position with respect to the bergis estimated to within 2 m, and the iceberg trans-lational and rotational velocities are estimated towithin 20%.

1 Introduction

Large free-floating icebergs are traveling ecosys-tems of great interest to science. Because theyare relatively difficult and dangerous to approach,they are attractive targets for exploration by Au-tonomous Underwater Vehicles (AUVs). However,because they are moving and rotating, standardAUV navigation methods are unable to provideiceberg-relative position estimates. These estimates

can be achieved by a new AUV navigation solutioncomprised of two steps: mapping and localization.The mapping step was described in a recent pa-per [1]. The localization step is presented here.

Figure 1: Photograph of iceberg used for experiment

To operate in the under-ice environment, an AUVmust possess some capability of navigation with re-spect to the ice surface. Existing under-ice AUVnavigation systems have typically relied on dead-reckoning using inertial measurements and/or ve-locity measurements from a Doppler sonar (bothseafloor-relative and ice-relative). These systemshave been used successfully to move between pre-programmed waypoints in horizontal inertial space,with depth determined by reactive terrain-following(or ice-following) and obstacle avoidance behaviors.However, these existing systems cannot provide ac-curate ice-relative position estimates around free-floating, rotating icebergs. Specifically, accelerom-

Page 2: SONAR-BASED ICEBERG-RELATIVE AUV LOCALIZATION...state space. In this way, the particle lter uses a nite number of particles e ciently - only in likely regions of the state space. 3

eters, gyros, and compasses provide informationabout how the AUV moves with respect to an iner-tial frame, not with respect to the ice. And whileDoppler Velocity Loggers (DVLs) can provide ice-relative velocity measurements, they cannot accountfor any rotation of the ice. Hence, extensions to thenavigation system are required to enable AUV nav-igation around free-floating icebergs.

Using the localization technique presented in thispaper, an AUV revisiting a previously-mapped ice-berg can know its position in the map, enabling safeoperation near the iceberg, and the capability to re-turn to sites of interest. The localization techniqueis terrain-relative, using correlation between incom-ing sonar measurements and the a-priori iceberg ter-rain map to provide an iceberg-relative vehicle posi-tion estimate. The technique works by maintainingestimates not only of iceberg-relative vehicle posi-tion and orientation, but also of the translationaland rotational velocities of the iceberg itself. Thispaper presents details of the algorithm along withexample localization results performed using a setof ship-based multibeam sonar data taken from anAntarctic iceberg (pictured in Figure 1). In this ex-ample, the vehicle position with respect to the bergis estimated to within 2 m, and the iceberg trans-lational and rotational velocities are estimated towithin 20%.

2 Background

2.1 AUV Navigation Beneath Sea Ice

The inherent difficulty of accessing the under-iceenvironment has driven an increase in the capabil-ity of AUVs to operate at high latitudes and undersea ice [2]. AUV navigation in this environment ismade challenging by the inability of AUVs to ob-tain GPS navigation fixes while beneath ice, by thereduced performance of navigation instruments athigh-latitudes [3], and by the hazardous presence ofboth ice and seafloor obstacles. Nevertheless, manysuccessful AUV missions have been performed be-neath sea ice. These missions have all utilized someform of dead-reckoning navigation, with a variety ofdrift-correction techniques.

In its Arctic configuration, MIT Sea Grant’sOdyssey II vehicle used only angular rate and accel-eration sensors to derive vehicle attitude and head-ing. Drift in this estimate was corrected by mag-netic field intensity measurements in all three vehi-cle axes, however this was done post-mission andwas not an aid to navigation. Position estima-tion for vehicle navigation was achieved through the

use of pre-deployed long baseline (LBL) or ultra-short baseline (USBL) acoustic systems. In addi-tion, Odyssey II used an acoustic homing beacon asa directional reference when returning to a recov-ery location. During successful deployment in theBeaufort Sea (Arctic), an upward-looking sonar onOdyssey II created a record of ice-draft above thevehicle’s navigation track [4].

The Canadian AUV, Theseus, was designed tolay several kilometers of fiber-optic cable beneathArctic ice. Theseus navigated to predetermined po-sition waypoints using dead-reckoning, based on ac-celeration measurements from an IMU and velocitymeasurements from bottom-pinging Doppler sonar.The dead-reckoned position estimate drifted at arate of 0.5% of distance traveled (%DT). To achievean order of magnitude improvement in navigation(to 0.05%DT), the dead-reckoned estimate was up-dated with position fixes from multiple acoustic bea-cons, installed at pre-surveyed locations along thedesired vehicle path [5].

In October 2001, the Monterey Bay AquariumResearch Institute operated the ALTEX AUV in theArctic to test the operation of various navigation in-struments at high latitudes. Dead-reckoning usingbottom-pinging (or ice-pinging) Doppler sonar, theALTEX vehicle achieved position error better than0.05%DT, without the use of external navigationaids (such as an LBL array). Under-ice missions onthis deployment were approximately 5km long [3].

In February 2002, a Cambridge team deployed aMartin 150 AUV in East Greenland pack ice to ob-tain the first AUV-based sidescan sonar imagery ofarctic pack ice. The Martin Positioning (MARPOS)system relied on dead-reckoning integration of in-ertial accelerations, angular rates from a ring-lasergyro, and velocities from bottom-pinging Dopplersonar. This system achieved 0.1%DT horizontal er-ror, as long as bottom-pinging was possible. Thevehicle used differential GPS at the surface to ob-tain periodic position fixes [6]. In April 2007, an-other Cambridge team used an ice-lanuched Gaviavehicle to obtain three-dimensional digital terrainmaps of the underside of sea ice in the BeaufortSea. This vehicle maintained a position estimate bydead-reckoning with ice-pinging Doppler sonar, andwas recovered via a tether [7].

The British Autosub Under Ice (AUI) programhas deployed the Autosub AUV under both Arcticand Antarctic sea ice. AUI missions in Greenland [8]and Antarctica [9] returned three-dimensional im-ages of under-ice topography, taken with upward-looking swath sonar from the AUV. Autosub uti-lized reactive obstacle avoidance for these missions,

Page 3: SONAR-BASED ICEBERG-RELATIVE AUV LOCALIZATION...state space. In this way, the particle lter uses a nite number of particles e ciently - only in likely regions of the state space. 3

but was navigated using dead-reckoning techniquesin which velocity measurements were made usingbottom-pinging or ice-pinging Doppler sonar. Au-tosub navigated to its recovery point by following aship-deployed acoustic homing beacon.

2.2 Terrain-Relative Navigation

TRN systems are used to eliminate accumulateddrift error in vehicle position estimates by correlat-ing terrain contour measurements with a-priori ter-rain maps. The Terrain Contour Matching (TER-COM) algorithm was one of the first implemen-tations of this principle, and was used to correctdrift in cruise missile navigation systems for manyyears [10]. Sequences of single-beam altitude mea-surements from along missile trajectories form theinputs to the TERCOM algorithm.

More recently, TRN techniques have been used(in place of GPS or acoustic transponder arrays)to correct drift error in underwater vehicle systems.Implementations have been fielded which providereal-time corrections to vehicle navigation, whileothers are used in post-processing [11–13]. In allimplementations, the achievable precision and accu-racy of the navigation estimates are limited by theresolution and quality of the a-priori map. Further-more, the terrain in the operating area must havesufficient texture to be used in identifying vehicleposition - TRN techniques provide no benefit overlarge areas of completely flat terrain.

2.3 Bayesian Estimators

In many applications (including underwater TRN),vehicle position estimates are often multimodalprobability distributions, which are not well approx-imated by unimodal Gaussians or other parame-terized distributions. A number of Bayesian tech-niques have been developed for estimation problemswith nonlinear belief distributions. The goal of anyBayesian estimator is to approximate the posteriorprobability distribution, p(xk|z1:k, u1:k). In vehiclelocalization, this is the probability that a given vehi-cle state, xk, represents the truth at epoch, k, condi-tioned upon all previous sensor measurements, z1:k,and all previous vehicle controls, u1:k.

Particle filters and point mass filters are examplesof Bayesian estimators. [14] shows that each of theseis suitable for underwater TRN, and that the local-ization accuracy of either method is approximatelyequal to the horizontal resolution of the terrain mapused. [14] also concludes that the point mass fil-ter is slightly more accurate and robust than the

particle filter, but that its computational expenseincreases dramatically with additional state dimen-sions. This work uses a particle filter due to theadditional states used to estimate terrain motion.

2.4 Particle Filters

Particle filters represent the posterior,p(xk|z1:k, u1:k), with a set of random statesamples drawn from the posterior. Ideally, theprobability of a particular state, xk, being includedin the set of samples is proportional to its Bayesposterior [15]. To achieve this, particle filters relyon incorporation of vehicle sensor measurementsinto a periodic resampling of the particle distri-bution. After every measurement, each particleis assigned a weight equal to the probability ofmaking that measurement, zk given that particle’sstate, xk. These weights are often expressedas exponentials, allowing them to be updatedmultiplicatively following each new measurement.When appropriate, a resampling step then throwsout particles with low weights, and replaces themwith new particles in highly-weighted regions of thestate space. In this way, the particle filter uses afinite number of particles efficiently - only in likelyregions of the state space.

3 Problem and Approach

For TRN, map-correlation of a single (2-dimensional) multibeam sonar ping is generallyinsufficient to uniquely identify the position ofthe vehicle. Rather, several pings (giving a 3-dimensional representation of the surface) arerequired to localize a vehicle. The central problemin using TRN around a free-floating iceberg isthat in order to use multiple pings, the ice-relativedisplacement of the vehicle between pings must beknown, yet only the inertial displacement of thevehicle is readily measured.

Typical iceberg motion (although varied geo-graphically) involves 0.03–0.08 m/s translationalspeeds, and 5–10 deg/hr rotation rates, while trans-lational speeds up to 0.27 m/s and rotation rates upto 40 deg/hr have been observed [16], [17]. Givenan AUV operating speed of 1.75 m/s, this typicaliceberg motion represents a drift rate of 1.7 to 4.6percent of distance traveled (%DT). Compared withthe 1%DT error achieved by high-accuracy dead-reckoning positoning systems, this motion is signif-icant.

However, while iceberg velocities are significant,they also change very slowly compared to the up-

Page 4: SONAR-BASED ICEBERG-RELATIVE AUV LOCALIZATION...state space. In this way, the particle lter uses a nite number of particles e ciently - only in likely regions of the state space. 3

date rate of a TRN estimator. Hence, they can beestimated explicitly as slowly-changing parameters,and accounted for during each motion update. Thisway, after several motion updates, measurement up-dates will allow the estimator to learn not only theberg-relative vehicle position, but also the inertialvelocities of the berg.

Specifically, the estimator presented here esti-mates the following nine degrees of freedom: allthree iceberg-relative vehicle position states, allthree iceberg-relative vehicle orientation states, anEasterly iceberg velocity, a Northerly iceberg veloc-ity, and an iceberg heading rate. It requires inertialmeasurements of vehicle position and orientation(for motion updates), as well as multibeam sonarmeasurements of range to the iceberg surface (formap-correlation in measurement updates).

AUVs are commonly equipped with Doppler ve-locity loggers (DVLs) which can provide ice-relativevehicle velocity information. Lacking an iceberg-relative heading reference, this sensor does not com-pletely solve the iceberg-relative navigation prob-lem, but does provide a measurement which willbe useful in future versions of the localization al-gorithm. Because no DVL instrument was availableduring collection of the data presented here, the al-gorithm detailed below does not utilize Doppler ve-locity measurements.

4 Algorithm

While various Bayesian estimator TRN formula-tions could appropriately be extended to estimateiceberg velocities, this work utilizes the particle fil-ter formulation due to the ease and efficiency withwhich particle filters handle the estimation of largernumbers of state variables. Each particle repre-sents a hypothesized system state, and is definedcompletely by its weight and the values of its ninestates - position and orientation with respect to theiceberg and iceberg northerly, easterly, and head-ing rates. Together, all the particle weights form asample-based approximation to the posterior prob-ability density distribution, p(xk|z1:k, u1:k), whereeach z is a multibeam sonar measurement, and eachu is a measured vehicle displacement.

The particle filter algorithm consists of threesteps, and is run every time a new multibeam sonarmeasurement is recorded (each epoch, k). First, theparticles undergo an iceberg-relative motion update.Second, the particle weights undergo a measurementupdate based on map-correlation of the new multi-beam measurement. Third, the particle distribution

may be resampled in order to allocate particles tothe most meaningful regions of the state space.

The motion update step of the particle filter al-gorithm adds the ice-relative vehicle displacement(since the last update) to each particle. This relativedisplacement is calculated as the difference betweenthe measured inertial vehicle displacement and theestimated inertial iceberg displacement. The dis-placement added to each particle also includes anadditive Gaussian random noise term, sized to rep-resent the uncertainty in the measured inertial ve-hicle displacement. The contribution to the motionupdate due to inertial iceberg displacement is dif-ferent for each particle since each particle maintainsits own iceberg velocity estimates. For each motionupdate, these velocities are treated as constant overthe period of the update. However, small Gaussianrandom noise is added to each particle’s iceberg ve-locity estimates between motion updates such thatslow changes in the iceberg motion can be learned.

Equation (1) details the motion update step forthe mth particle between epochs k − 1 and k. Eachparticle’s state vector represents a hypothesis overiceberg-relative vehicle position (x, y, and z) andorientation (φ, θ, and ψ), as well as the Northerly,Easterly, and heading rates of the iceberg throughinertial space (Xice, Yice, and Ψice). The measureddisplacements in the six vehicle pose states (∆xthrough ∆ψ) come from an inertial measurementunit. ν is a zero-mean, unit-variance normally dis-tributed random noise vector, sized by Σ6x6

IMU (usu-ally diagonal) to account for uncertainty in the iner-tial displacement measurements, and by Σ3x3

ice (usu-ally diagonal) to allow for slow changes in icebergmotion parameters. R[m]

k is the 2x2 rotation matrixwhich transforms vectors from the inertial frame tothe iceberg frame. This rotation is based on the theiceberg’s heading, which is not explicitly estimatedas a state, but rather computed for the mth particleas the difference between the vehicle (inertial) com-pass heading and ψ[m]

k , the particle’s estimate of theiceberg-relative vehicle heading.

The measurement update step computes and nor-malizes the weights of the particles. For each parti-cle, the weight is proportional to the probability ofmaking all multibeam measurments since the lastresampling operation, given the trajectory of thatparticle. This update is recursively computed af-ter each measurement by multiplicatively updatingeach particle weight based on the likelihood of mak-ing only the current measurement given the parti-cle’s current position. For the mth particle, eachupdate term takes the form given in Equation (2),where zk is a the current measurement vector, and

Page 5: SONAR-BASED ICEBERG-RELATIVE AUV LOCALIZATION...state space. In this way, the particle lter uses a nite number of particles e ciently - only in likely regions of the state space. 3

xyzφθψ

Xice

Yice

Ψice

[m]

k

=

xyzφθψ

Xice

Yice

Ψice

[m]

k−1

+

∆x∆y∆z∆φ∆θ∆ψ000

− (tk − tk−1)

[R[m]

k 00 0

]

Xice

Yice

000

Ψice

000

[m]

k−1

+[Σ6x6

IMU 00 Σ3x3

ice

]ν (1)

w[m]k = p(zk|x[m]

k ) =1√

2π|Σ|exp

[−1

2(z[m]

k − a[m]k )TΣ−1(z[m]

k − a[m]k )

](2)

z[m]k is the projection of the measurement into the

map frame under the particle’s state, x[m]k , and

a[m]k is the vector of points on the iceberg surface

which are closest to their corresponding measure-ment points in z[m]

k . Thus, the vector z[m]k − a[m]

k

is the measurement error vector, representing thedistance from the mapped iceberg face of each pro-jected sounding in the current measurement.

In this common Gaussian model for measurementerror, the particle weight is assigned according tothe assumption that the probability of a set of mea-surement errors is a zero-mean Gaussian, with co-variance given by Σ. Here, |Σ| becomes σ2n (wheren is the number of beams being used in the measure-ment) under the assumptions that the beam mea-surement errors are uncorrelated and that the mea-surement variance, σ2, is the same for each beam.

Resampling is only performed when two condi-tions have been met. First, a sufficiently high num-ber of soundings must have been used to calculatethe particle weights. Second, the variance of theparticle weights must be sufficiently high. Thesetwo requirements prevent particle deprivation - aloss of diversity in the particle distribution. Whensampling is performed, it is done according to thelow-variance sampling algorithm presented in [15],which avoids particle deprivation and is inexpen-sive to perform. A small, normally distributed noiseterm is added to each state of a resampled particlein order to explore the nearby state space.

5 Experimental Results

5.1 The Data Set

In June 2008, multibeam sonar data were takenfrom an iceberg in the Scotia Sea. During datacollection, the berg translated at approximately

0.09 m/s (0.18 kts), and rotated at approximately20 deg/hr. The sonar data were recorded by asideways-looking 400KHz Reson multibeam sonarhead mounted on a pole 10 m beneath the RVIBNathaniel B Palmer (see Figure 2).

Figure 2: Sideways-looking multibeam sonar be-neath the R.V. Nathaniel B Palmer

The ship’s track in inertial space was measuredby GPS. Sonar imagery of the shallowest 250 m (ap-proximately) of the iceberg’s submerged perimeterwas collected as the ship made a steady 400 ◦ cir-cumnavigation of the berg in just under 40 minutes.A photograph of the iceberg appears in Figure 1.

5.2 Data Processing

Each multibeam sonar ping is tagged with the ori-entation and GPS position of the ship at the timeof the ping. Raw sonar data were edited using theopen-source, NSF-supported MB-System software

Page 6: SONAR-BASED ICEBERG-RELATIVE AUV LOCALIZATION...state space. In this way, the particle lter uses a nite number of particles e ciently - only in likely regions of the state space. 3

package. Bad beams (e.g. max range, zero range,or excessive noise) were flagged using MB-System.The differential location between the ship’s positionsensors and the multibeam sensor was added to thevehicle-frame sonar range vectors.

5.3 The A-Priori Map

The multibeam sonar soundings were assembled intoa self-consistent map, representing the shape of theiceberg (as described in [1]). This map appears inFigure 3. Since the publication of [1], the maphas been updated to include further cleaning ofthe sonar data and a refined approximation of er-ror smoothing (corresponding with an assumed con-stant iceberg motion) used for the mapping. Notethat there are sections of the iceberg where sufficientmap data could not be collected, and the map sur-face is left undefined. As the vehicle passes by theseregions of the berg, measurement updates (map cor-relations) for localization are not possible.

In order to speed the terrain correlation compu-tation, and allow for surface plotting of the icebergmap, the map data have been reduced by the follow-ing method. All soundings were transformed intoa cylindrical coordinate system (azimuth, depth,range) centered over the iceberg. The range datawere then gridded over azimuth and depth. In eachcell with five or more soundings, the average rangevalue forms a surface point estimate along with theazimuth and depth values for that cell. Grid cellswith fewer than five soundings are not assigned asurface point. The Delaunay triangulation over allsurface points defines the triangular surface patchesused for plotting. Superior (more probabilisticallyappropriate) methods for surface recovery and deci-mation representation exist in the computer graph-ics literature, and must be implemented in map-ping icebergs whose surfaces cannot be well approx-imated by cylinders.

In this experiment, the same multibeam sonardata are being used to localize the vehicle withinthe map as were used to create the map in the firstplace. This means that the assumed iceberg mo-tion used to create the map can be considered truth,since the map projection depends entirely on thatmotion. Thus, if the estimator were to perform per-fectly, it would yield an iceberg motion estimate ex-actly matching the assumed motion. Ideally, therewould also exist a second sonar data set, along withGPS ground-truth for the iceberg motion so thatthe envisioned operational case of localization dur-ing a re-visit to a previously-mapped iceberg couldbe tested. Unfortunately, such a data set was not

collected.

Figure 3: Iceberg Map and Segment of Vehicle Tra-jectory

5.4 Localization Results

In this experiment, the estimator is initialized to abroad distribution, and converges to a useful esti-mate of system state after a series of motion andmeasurement updates. Approximately four secondspass between measurement updates. Figures 4 and5 show the vehicle pose and iceberg velocity esti-mation errors, respectively. Each of the smaller,magenta dots represents a hypothesized state valuecorresponding to a single particle. The larger, bluedots represent the weighted mean of the distribu-tion. (Since depth, roll, and pitch are readily mea-sured, estimation errors for these parameters are notshown).

Figure 4: Iceberg-relative vehicle pose estimationerrors

Page 7: SONAR-BASED ICEBERG-RELATIVE AUV LOCALIZATION...state space. In this way, the particle lter uses a nite number of particles e ciently - only in likely regions of the state space. 3

Figure 5: Iceberg velocity estimation errors

Following convergence, the error in the weightedmean of the distribution is less than 2 m for theiceberg-relative position states, less than 1 degreefor the iceberg-relative vehicle heading, less than10% for the iceberg translational velocity estimates,and less than 20% for the iceberg heading rate es-timate. Note that very few map correlations (mea-surement updates) are possible due to holes in themap between updates 125-170. Thus, the distri-bution isn’t resampled during this period, and itspreads out. However, when measurement updatesbecome available again, the distribution can be re-sampled, and converges once again. The ability ofthe motion updates to keep the position estimatesreasonable in the absence of measurement updatesdepends on how well the iceberg motion state esti-mates have converged before the measurement up-dates become impossible.

5.5 Initialization

The more closely an estimator can be initialized tothe true distribution, the less time it will take toconverge. However, the initial distribution must beset realistically based on the capability of AUV op-erators to estimate the system state at the time ofan AUV’s deployment. While this capability variesbased on experience and available instrumentation,the independent Gaussian parameter distributionsused for initialization in the present example arelisted in Table 1. These distributions can also beseen visually in Figures 4 and 5 as the magentapoints in the first column.

Table 1: Initial DistributionParameter Units Mean Var

Relative Vehicle Pos. 1 m 27.84 20

Relative Vehicle Pos. 2 m 292.30 20

Vehicle Depth m 0.13 0.1

Vehicle Roll deg 1.14 0.1

Vehicle Pitch deg 5.34 0.1

Relative Vehicle Heading deg 179.47 10

Iceberg North Velocity m/s -0.05 0.02

Iceberg East Velocity m/s 0.08 0.04

Iceberg Heading Rate deg/hr 20.63 6.55

5.6 Tradeoffs

As with any estimator implementation there aretradeoffs to be considered in implementing this par-ticle filter. Map correlation of each sounding foreach particle is the most expensive step in the al-gorithm. Three parameters which strongly influ-ence the computation time are the number of par-ticles (300 were used here), the measurement up-date period (4 seconds was used here, approximately10 times slower than the maximum update rate ofthe sonar), and the number of beams from eachmultibeam ping which are actually used for corre-lation (492 were used here). The estimation per-formance tradeoff between precision and robustnessis affected by the minimum number of soundings(individual beam measurements) required before re-sampling (5000 was used here), and the size of theexploratory noise added to particle states during re-sampling. Choices in these trades depend stronglyon the uncertainty characteristics of the availablesensors.

6 Conclusion

Typical iceberg motion is significant enough thatexisting AUV navigation methods are insufficientfor navigation with respect to free-floating icebergs.A terrain-relative navigation (TRN) estimator hasbeen presented which performs measurement up-dates using correlation between incoming multi-beam sonar ranges and an a-priori iceberg surfacemap. A key feature of this technique is the explicitestimation and incorporation into the motion up-date of iceberg translational and heading velocities.Once the estimator has converged to a correct esti-mate of terrain motion, it is robust to areas of tex-tureless terrain and to periods of time when multi-beam measurements may be unavailable. Successfulimplementation of the estimator was demonstrated

Page 8: SONAR-BASED ICEBERG-RELATIVE AUV LOCALIZATION...state space. In this way, the particle lter uses a nite number of particles e ciently - only in likely regions of the state space. 3

using ship-based sonar data taken from an icebergin the Scotia Sea.

During initialization (and assuming sufficientlytextured terrain), the estimates of position statesconverge quickly because the measurement updatedepends directly on position. However, several mo-tion updates must occur before errors in iceberg mo-tion state estimates are manifested as position errorsand therefore penalized during during the measure-ment update. Estimates of the iceberg velocitiestherefore require more time to converge. A furtherextension to the estimator would allow separate re-sampling of the vehicle position and iceberg motionstates based on whether the position estimate hadconverged.

In this experiment, a very rough grid-based tech-nique was used to reduce the map data to a surfacerepresentation. A better surface representation ofthe map would improve the performance of the al-gorithm, and will be required in order for the algo-rithm to be used with icebergs whose shapes are notwell approximated by cylinders. Automation of thesonar data cleaning process will also be required fora truly autonomous, deployable version of the algo-rithm.

Acknowledgment

This work was supported by grant ANT-0811399from the National Science Foundation. The au-thors would like to thank Reson Inc for the use oftheir 7125 sonar system, Brett Hobson for deployingand operating the sonar system, Joanna O’Neill forcollecting and processing the multibeam data, andDave Caress for his expertise and help with MBSys-tem.

References

[1] Peter Kimball and Stephen Rock, “Sonar-Based Iceberg-Relative AUV Navigation”, inProceedings Of AUV2008, Woods Hole, MA,October 2008, IEEE.

[2] Nicole Tervalon and Richard Henthorn, “Iceprofiling sonar for an auv: Experience in thearctic”, in OCEANS. MTS/IEEE, October2002, vol. 1, pp. 305–310.

[3] Rob McEwen, Hans Thomas, Don Weber, andFrank Psota, “Performance of an auv naviga-tion system at arctic latitudes”, IEEE Journalof Oceanic Engineering, vol. 30, no. 2, pp. 443–454, April 2005.

[4] J.G. Bellingham, C.A. Goudey, T.R. Consi,J.W. Bales, D.K. Atwood, J.J. Leonard, andC. Chryssostomidis, “A second generation sur-vey auv”, in Symposium on Autonomous Un-derwater Vehicle Technology. AUV Technology,July 1994, pp. 148–155.

[5] Bruce Butler and Ron Verrall, “A precisionhybrid inertial/acoustic navigation system fora long-range autonomous underwater vehicle”,in AM 98. Institute of Navigation, 1998, pp.561–572.

[6] P. Wadhams, J. P. Wilkinson, and A. Kaletzky,“Sidescan sonar imagery of the winter marginalice zone obtained from an auv”, AMS Journalof Atmospheric and Oceanic Technology, vol.21, pp. 1462–1470, 2004.

[7] P. Wadhams and MJ Doble, “Digital terrainmapping of the underside of sea ice from a smallAUV”, Geophysical Research Letters, vol. 35,no. 1, 2008.

[8] P. Wadhams, J.P. Wilkinson, and S.D.McPhail, “A new view of the underside of arc-tic sea ice”, Geophysical Research Letters, vol.33, February 2006.

[9] K. W. Nicholls, E. P. Abrahamsen, J. J. H.Buck, P. A. Dodd, C. Goldblatt, G. Griffiths,K. J. Heywood, N. E. Hughes, A. Kaletzky,G. F. Lane-Serff, S. D. McPhail, N. W. Mil-lard, K. I. C. Oliver, J. Perrett, M. R. Price,C. J. Pudsey, K. Saw, K. Stansfield, M. J.Stott, P. Wadhams, A. T. Webb, and J. P.Wilkinson, “Measurements beneath an antarc-tic ice shelf using an autonomous underwatervehicle”, Geophysical Research Letters, vol. 33,April 2006.

[10] J. P. Golden, “Terrain contour matching (ter-com) - a cruise missile guidance aid”, in ImageProcessing for Missile Guidance. SPIE, July1980, vol. 238, pp. 10–18.

[11] Ingemar Nygren and Magnus Jansson, “Terrainnavigation for underwater vehicles using thecorrelator method”, IEEE Journal of OceanicEngineering, vol. 29, no. 3, pp. 906–915, July2004.

[12] Ryan Eustice, Richard Camilli, and Hanu-mant Singh, “Towards bathymetry-optimizeddoppler re-navigation for auvs”, in OCEANS.MTS/IEEE, 2005, vol. 2, pp. 1430–1436.

Page 9: SONAR-BASED ICEBERG-RELATIVE AUV LOCALIZATION...state space. In this way, the particle lter uses a nite number of particles e ciently - only in likely regions of the state space. 3

[13] S. Williams and I. Mahon, “A terrain-aided tracking algorithm for marine systems”,The International Conf on Field and ServiceRobotics, pp. 112–118, 2003.

[14] Kjetil Bergh Anonsen and Oddvar Hallingstad,“Terrain aided underwater navigation usingpoint mass and particle filters”, in Position,Location, And Navigation Symposium, 2006.IEEE/ION, April 2006, pp. 1027– 1035.

[15] S. Thrun, W. Burgard, and D. Fox, Proba-bilistic Robotics (Intelligent Robotics and Au-tonomous Agents), MIT press, Cambridge,Massachusetts, USA, 2005.

[16] R. Gladstone and G.R. Bigg, “Satellite track-ing of icebergs in the weddell sea”, AntarcticScience, vol. 14, no. 03, pp. 278–287, 2002.

[17] P. Wadhams and M. Kristensen, “The responseof antarctic icebergs to ocean waves”, Journalof geophysical research, vol. 88, no. C10, pp.6053–6065, 1983.