Hans Jacob S. Feder Adaptive Mobile Robot John J. Leonard … · 2015. 6. 2. · sensing (Leonard and Durrant-Whyte, 1992), active infor-mation gathering (Hager, 1990), adaptive sampling

Hans Jacob S. FederJohn J. LeonardMarine Robotics LaboratoryDepartment of Ocean EngineeringMassachusetts Institute of TechnologyCambridge, Massachusetts 02139, USA{feder,jleonard}@deslab.mit.edu

Christopher M. SmithCharles Stark Draper LaboratoryCambridge, Massachusetts 02139, [email protected]

Adaptive Mobile RobotNavigation and Mapping

Abstract

The task of building a map of an unknown environment and con-currently using that map to navigate is a central problem in mobilerobotics research. This paper addresses the problem of how to per-form concurrent mapping and localization (CML) adaptively usingsonar. Stochastic mapping is a feature-based approach to CMLthat generalizes the extended Kalman filter to incorporate vehiclelocalization and environmental mapping. The authors describe animplementation of stochastic mapping that uses a delayed nearestneighbor data association strategy to initialize new features into themap, match measurements to map features, and delete out-of-datefeatures. The authors introduce a metric for adaptive sensing thatis defined in terms of Fisher information and represents the sum ofthe areas of the error ellipses of the vehicle and feature estimatesin the map. Predicted sensor readings and expected dead-reckoningerrors are used to estimate the metric for each potential action of therobot, and the action that yields the lowest cost (i.e., the maximuminformation) is selected. This technique is demonstrated via simula-tions, in-air sonar experiments, and underwater sonar experiments.Results are shown for (1) adaptive control of motion and (2) adaptivecontrol of motion and scanning. The vehicle tends to explore selec-tively different objects in the environment. The performance of thisadaptive algorithm is shown to be superior to straight-line motionand random motion.

NomenclatureF dynamic modelH observation modelM transformation relating the Fisher information

between time steps recursively

The International Journal of Robotics ResearchVol. 18, No. 7, July 1999, pp. 650-668,©1999 Sage Publications, Inc.

F Jacobian of the dynamic modelG process noise scaling matrix (dependent onu)H Jacobian of the state-to-observation

transformationhI Fisher information matrixK Kalman filter gain matrixL Jacobian ofl with respect toxr , evaluated atxrk|kN number or element in the state vector; number of

features in the mapP system covariance matrixR observation covariance matrixT transformation used in the compounding operationdx process noise (assumed to be white and Gaussian)dz observation noise process (assumed to be white

and Gaussian)h state-to-observation transformationi subscript index identifying a feature numberk time index is shown in subscriptl transformation used for entering a new element

to the state vectorp(·) a probability density functionu actions, or system control inputUθ set of sonar scanning anglesx actual system state vectorx estimated system state vectorxk|k estimated state at time stepk given all the

information up to time stepkxr Vehicle state—location(xr , yr ) and directionφ;

that is,xr = [xr yr φ]z measurement vector (range and bearing

measurements)ˆ signifies an estimated variable

650

Feder, Leonard, and Smith / Mobile Robot Navigation and Mapping 651

1. Introduction

The goal of concurrent mapping and localization (CML) is tobuild a map of the environment while simultaneously usingthat map to enhance navigation performance. CML is criticalfor many field and service robotics applications. The long-term goal of our research is to develop new methods for CML,enabling autonomous underwater vehicles (AUVs) to navi-gate in unstructured environments without relying on a priorimaps or acoustic beacons. This paper presents a techniquefor adaptive CML and demonstrates that technique using realsonar data.

Navigation is essential for successful operation of under-water vehicles in a range of scientific, commercial, and mili-tary applications (Leonard et al., 1998). Accurate positioninginformation is vital for the safety of the vehicle and for theutility of the data it collects. The error growth rates of inertialand dead-reckoning systems available for AUVs are usuallyunacceptable. Underwater vehicles cannot use the global po-sitioning system (GPS) to reset dead-reckoning error growthunless they surface, which is usually undesirable. Positioningsystems that use acoustic transponders (Milne, 1983) are oftenexpensive and impractical to deploy and calibrate. Naviga-tion algorithms based on existing maps have been proposed(Tuohy et al., 1996; Lucido et al., 1996), but sufficiently ac-curate a priori maps are often unobtainable.

Adaptive sensing strategies have the potential to save timeand maximize the efficiency of the CML process for an AUV.Energy efficiency is one of the most challenging issues inthe design of underwater vehicle systems (Bellingham andWillcox, 1996). Techniques for building a map of sufficientresolution as quickly as possible would be highly benefi-cial. Survey class AUVs must maintain forward motion forcontrollability (Bellingham et al., 1994); hence, the abilityto adaptively choose a sensing and motion strategy that ob-tains the most information about the environment is especiallyimportant.

Sonar is the principle sensor for AUV navigation. Pos-sible sonar systems include mechanically and electronicallyscanned sonar, side-scan sonar, and multibeam bathymet-ric mapping sonar (Urick, 1983). The rate of informationobtained from a mechanically scanned sonar is low, mak-ing adaptive strategies especially beneficial. Electronicallyscanned sonar can provide information at very high data rates,but enormous processing loads make real-time implementa-tions difficult. Adaptive techniques can be used to limit sens-ing to selected regions of interest, dramatically reducing com-putational requirements.

In this paper, adaptive sensing is formulated as the eval-uation of different actions that the robot can take and theselection of the action that maximizes the amount of informa-tion acquired. This general problem has been considered in avariety of contexts (Manyika and Durrant-Whyte, 1994; Rus-sell and Norvig, 1995), but not specifically for CML. CML

provides an interesting context within which to address adap-tive sensing because of the trade-off between dead-reckoningerror and sensor data acquisition. The information gainedby observing an environmental feature from multiple vantagepoints must counteract the rise in uncertainty that results fromthe motion of the vehicle.

Our experiments use two different robot systems. One isan inexpensive wheeled land robot equipped with a singlerotating sonar. Observations are made of several cylindri-cal targets whose location is initially unknown to the robot.Although this is a simplified scenario, these experiments pro-vide a useful illustration of the adaptive CML process andconfirm behavior seen in simulation. The second system is aplanar robotic positioning system that moves a sonar withina 9× 3 × 1 m testing tank. The sonar is a mechanicallyscanned 675-kHz Imagenex model 881 sonar with a 2◦ beam.These characteristics are similar to alternative models used inthe marine industry. Testing tank experimentation provides abridge between simulation and field AUV systems. Repeat-able experiments can be performed under identical conditions;ground truth can be determined to high accuracy.

Section 2 reviews previous research in concurrent mappingand localization and adaptive sensing. Section 3 develops thetheory of adaptive stochastic mapping. Sections 4, 5, and6 describe testing of the method using simulations, air sonarexperiments, and underwater sonar experiments, respectively.Finally, Section 7 provides concluding remarks and sugges-tions for future research.

2. Background

2.1. Stochastic Mapping

Stochastic mapping is a technique for CML that was intro-duced by Smith, Self, and Cheeseman (1990). In stochasticmapping, a single state vector represents estimates of the loca-tions of the robot and the features in the environment. An as-sociated error covariance matrix represents the uncertainty inthese estimates, including the correlations between the vehi-cle and feature state estimates. As the vehicle moves throughits environment taking measurements, the system state vectorand covariance matrix are updated using an extended Kalmanfilter (EKF). As new features are observed, new states areadded to the system state vector; the size of the system co-variance matrix grows quadratically. Implementing stochas-tic mapping with real data requires methods for data associa-tion and track initiation. Smith, Self, and Cheeseman describeprocesses for adding new features to the map and enforcingknown geometric constraints between different map featuresbut do not perform simulations or experiments using thesetechniques.

Moutarlier and Chatila (1989) provided the first imple-mentation of this type of algorithm with real data. Their im-plementation uses data from a scanning laser range findermounted on a wheeled mobile robot operating indoors. They

652 THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH / July 1999

implemented a modified updating technique, called relocationfusion, which reduces the effect of vehicle dynamic modeluncertainty. More recently, Chatila and colleagues developedimproved implementations for outdoor vehicles in natural en-vironments and investigated new topics such as the decou-pling of local and global maps (Betgé-Brezetz et al., 1996;Hébert, Betgé-Brezetz, and Chatila, 1996). The number ofother researchers who have implemented stochastic mappingwith real data has been limited. Using sonar on land robotsoperating indoors, Rencken (1993) and Chong and Kleeman(1997) implemented variations of stochastic mapping usingsonar sensing. Rencken used the standard Polaroid rangingsystem, whereas Chong and Kleeman used a precision customsonar array that provided accurate classification of geometricprimitives. Current implementations of stochastic mappingare limited to ranges of tens of meters and/or durations of afew hours.

Stochastic mapping assumes a metrical, feature-based en-vironmental representation in which objects can be effec-tively represented as points in an appropriate parameter space.Other types of representations are possible and have been em-ployed with success. For example, Thrun, Fox, and Burgard(1998) have demonstrated highly successful navigation of in-door mobile robots using a combination of grid-based (Elfes,1987) and topological (Kuipers and Byun, 1991) modeling.For marine robotic systems, the complexity of representingthree-dimensional natural environments with geometric mod-els is formidable. Our hypothesis, however, is that salientfeatures can be found and reliably extracted (Medeiros andCarpenter, 1996), enabling CML. Exhaustively detailed envi-ronmental modeling should not be necessary for autonomousnavigation.

2.2. Adaptive Sonar Sensing

Adaptive sensing has been a popular research topic in manydifferent areas of robotics. Synonymous terms that have beenused for these investigations include active perception (Ba-jcsy, 1988), active vision (Blake and Yuille, 1992), directedsensing (Leonard and Durrant-Whyte, 1992), active infor-mation gathering (Hager, 1990), adaptive sampling (Belling-ham and Willcox, 1996), sensor management (Manyika andDurrant-Whyte, 1994), and limited rationality (Russell andWefald, 1995). A common theme that emerges is that adap-tive sensing should be formulated as the process of evaluatingdifferent sensing actions that the robot can take and choos-ing the action that maximizes the information acquired by therobot. The challenge in implementing this concept in prac-tice is to develop a methodology for quantifying the expectedinformation for different sensing actions and evaluate themin a computationally feasible manner given limited a prioriinformation.

Our approach is closest to Manyika and Durrant-Whyte(1994), who formulated a normative approach to multisensordata fusion management based on Bayesian decision theory

(Berger, 1985). A utility structure for different sensing ac-tions was defined using entropy (Shannon information) as ametric for maximizing the information in decentralized multi-sensor data fusion. The method was implemented for model-based localization of a mobile robot operating indoors usingmultiple-scanning sonar and an a priori map. Feature locationuncertainty and the loss of information due to vehicle motionerror, which are encountered in CML, were not explicitly ad-dressed.

Examples of the application of adaptive sensing in marinerobotics include Singh (1995) and Bellingham and Willcox(1996). Singh formulated an entropic measure of adaptivesensing and implemented it on the Autonomous Benthic Ex-plorer. The implementation was performed using stochasticback-projection, a grid-based modeling technique developedby Stewart (1988) for marine sensor data fusion. Belling-ham and Willcox have investigated optimal survey design forAUVs making observations of dynamic oceanographic phe-nomena (Willcox et al., 1996). Decreased vehicle speeds savepower due to the quadratic dependence of drag on velocity,but susceptibility to space-time aliasing is increased.

An interesting motivation for adaptive sonar sensing isprovided by the behavior of bats and dolphins performingecholocation. For example, dolphins are observed to movetheir heads from side to side as they discriminate objects (Au,1993). Our hypothesis for this behavior is that sonar is morelike touch than vision. A useful analogy may be the mannerin which a person navigates through an unknown room in thedark. By reaching out for and establishing contact with walls,tables, and chairs, transitions from one object to another canbe managed as one moves across the room. Whereas man-made sonar tends to use narrow-band waveforms and narrowbeam patterns, bat and dolphin sonar systems use broad-bandwaveforms and relatively wide beam patterns. A broad beampattern can be beneficial because it provides a greater rangeof angles over which a sonar can establish and maintain “con-tact” by receiving echoes from an environmental feature. Thetask for CML is to integrate the information obtained fromsonar returns obtained from different features as the sensormoves through the environment to estimate both the geome-try of the scene and the trajectory of the sensor.

3. Adaptive Delayed Nearest Neighbor Stochas-tic Mapping

This section reviews the theory of stochastic mapping, derivesa metric for performing stochastic mapping adaptively, anddescribes the delayed nearest neighbor (DNN) method fortrack initiation and track deletion.

3.1. Stochastic Mapping

Stochastic mapping is simply a special way of organizing thestates in an EKF for the purpose of feature relative navigation.


An EKF is a computational efficient estimator for the statesof a given nonlinear dynamic system, assuming that the noiseprocesses are well modeled by Gaussian noise and the errorsdue to linearization of the nonlinear system are small. Thatis, the EKF for a system provides an estimate of both the stateof the system, sayx, and an estimate of the covariance of thestate, sayP. The covariance of the state provides an estimateof the confidence in the estimatex. A dynamic system isdescribed by a dynamic model,F, which defines the evolutionof the system (robot) through time, and an observation model,H, which relates observations (measurements) to the state ofthe system (robot).

Our implementation of stochastic mapping is based onSmith, Self, and Cheeseman (1990) and is described in moredetail in the appendix. We usexk = xk|k + xk to representthe system state vectorx = [xT

r xT1 . . . xT

N ]T , wherexr andx1 . . . xN are the robot and feature states, respectively,x is theestimated state vector, andx is the error estimate. Measure-ments are taken everyt = kT seconds, whereT is a constantperiod andk is a discrete time index. The measurementsare related to the state by a state-to-observation transformzk = H(xk, dz), wheredz is the measurement noise pro-cess. The a posteriori PDF ofxk, given a set of measurementsZk ≡ {zk, . . . , z1}, can be found from Bayes’s rule as

p(xk|Zk) = p(zk|xk)p(xk|Zk−1)

p(zk|Zk−1). (1)

The distributionp(zk|xk) is defined as the likelihood func-tion using the likelihood principle (Berger, 1985). By know-ing p(xk|Zk), we can form an estimatexk of the state.

To perform CML, the state transition (dynamic model)xk+1 = F(xk, uk, dx), in addition to the observation trans-formationH, must be known. Here,uk is the control input(action) at timek anddx is the process noise. If the stochasticprocessesdz anddx are assumed to be independent, white, andGaussian, and the state transitionF and observation modelH are both linear, the Bayes least squares (BLS) estimatorxk+1|k = F(E{xk|Zk}, u) will be an efficient estimator ofx. However, in the general problem of CML, neither the dy-namic model nor the observation model will be linear. Thus,an efficient estimator cannot be obtained. Furthermore, prop-agating the system’s covariance through nonlinear equationsdoes not guarantee that the statistics will be conserved. Thus,to circumvent the problem of transformation of nonlinearities,the nonlinear models ofF andH are approximated througha Taylor series expansion, keeping only the first two terms.That is,

F(xk, uk, dx) ≈ F(xk|k, uk, dx) + Fx xk|k⇒ xk+1|k ≈ E{F(xk, uk, dx)|Zk}, (2)

whereFx = dF(x, uk)/dx|x=xk|k is the Jacobian of dynamicmodelF with respect tox, evaluated atxk|k. Similarly, theobservation model is approximated by

H(xk, dz) ≈ H(xk|k−1, dz) + Hx xk|k−1, (3)

whereHx = dH(x)/dx|x=xk|k−1is the Jacobian of the obser-

vation modelH with respect tox evaluated atxk|k−1. Theseapproximations are equivalent to the assumption that the esti-mated mean at the previous time step,xk|k, is approximatelyequal to the system statexk at the previous time step. Oncethese linearizations have been performed and assuming thatthe approximation error is small, we can now find the BLSfor this linearized system. The result is equivalent to that ofthe EKF (Gelb, 1973; Bar-Shalom and Li, 1993; Uhlmann,Julier, and Csorba, 1997).

The estimate error covariancePk|k = E{xk xTk } of the sys-

tem takes the form

Pk|k =

Prr Pr1 · · · PrN

P1r P11 · · · P1N

......

. . ....

PNr PN1 · · · PNN

k|k

. (4)

The submatricesPrr , Pri , and Pii are the vehicle-vehicle,vehicle-feature, and feature-feature covariances, respectively.This form is significant, since it allows us to separate the un-certainty associated with the robot,Prr , as well as the individ-ual features,Pii , and this separation will be used in obtaininga measure of the information in our system.

Thus, the robot and map are represented by a single statevectorx with an associated estimate error covariancePat eachtime step. As new features are added,x andP increase in size.

In our implementation, we denote the vehicle’s state byxr = [xr yr φ]T , and the control input to the vehicle is givenby u = [ux uy uφ]. For the dynamic modelF, we use

xrk+1 =f (xk, uk) + G(uk)dx

=xrk + Tφkuk + G(uk)dx,

(5)

whereG(uk) scales the noise processdx as a function of thedistance traveled; that is,

G(uk) =

γx

√u2

xk+ u2

yk0 0

0 γy

√u2

xk+ u2

yk0

0 0 γφ

(6)

and

Tφk=

cos(φk) − sin(φk) 0

sin(φk) cos(φk) 00 0 1

. (7)

The φ in this expression comes from the robot positionxrk . The covariance ofdx is for convenience set equal to theidentity matrix, since any scaling is placed inG. The matrixG is diagonal by assumption. If the correlations between the


noise processes in thex, y, andφ directions are known, itshould be included. However, this is a minor point, and thisform has been shown to work well for our systems.

Equation (5) does not take into account any of the vehicle’sreal dynamics. Thus, the model is very general. However, ifthe vehicle’s dynamics were known, they could be used toensure that the vehicle movements remain realistic. In ourexperiments and simulation, we will constrain the robot tomove only a certain distance each time step, thus makingux

anduy dependent.For the observation model, we use

zk = H(xk, dz) = h(xk) + dz, (8)

wherezk is the observation vector of range and bearing mea-surements. The observation modelh defines the nonlinear co-ordinate transformation from state to observation coordinates.The stochastic processdz is assumed to be white, Gaussian,and independent ofx0 anddx, and has covarianceR.

In our experiments, a sector including an object is scannedover multiple adjacent angles to yield multiple sonar returnsthat originate from the object. That is, isolated features andsmooth surfaces appear as circular arcs, also known as re-gions of constant depth (Leonard and Durrant-Whyte, 1992),in sonar scans. For the specular returns, an improved estimateof the range and bearing to the object is obtained by group-ing sets of adjacent returns with nearly the same range, thentaking the mode of this set of angles as the bearing measure-ment and the range associated with this bearing as the rangemeasurement to the object (Moran, Leonard, and Chryssosto-midis, 1997). Rough surfaces (Bozma and Kuc, 1991) yieldadditional returns at high angles of incidence. These occurfrequently in the data from our underwater sonar, but are notprocessed in the experiments reported in this paper.

In our work, all features are modeled as point features.More complex objects can be described by a finite set of pa-rameters and estimated by using the associated observationmodel for this parameterization in stochastic mapping. Forinstance, the modeling of planes, corners, cylinders, and edgesis relatively straightforward (Leonard and Durrant-Whyte,1992).

3.2. Adaptation Step

The goal of adaptive CML is to determine the optimal actiongiven the current knowledge of the environment, sensors, androbot dynamics in a CML framework. To provide an intuitiveunderstanding of this goal, imagine an underwater vehiclewith no navigational uncertainty estimating the position ofa feature as depicted in Figure 1. As can be seen from thissimple example, it is clearly advantageous for the vehicle totake the next measurement from a new direction. By doingso, more information about the feature is extracted, and thusa better estimate can be obtained.

Actual feature position.

Estimated feature position

1

2

Fig. 1. An autonomous underwater vehicle with no naviga-tional uncertainty estimating the position of an environmentalfeature. The ellipses denote the certainty (error ellipse) withwhich the feature position is known. (a) The initial estimateof the feature. (b) Given two possible new allows for a moreaccurate estimate of the feature’s position, as the measure-ment of the feature is taken from a new angle, resulting in thetilted error ellipse associated with this measurement. Com-bining this (tilted error ellipse) with the error ellipse from (a),the small dotted circular error ellipse is achieved. Taking thenext observation slightly smaller than that of the previous timestep in (a), shown by the dotted ellipse.


The essence of our model is to determine the action thatmaximizes the total knowledge (i.e., the information) aboutthe system in the presence of measurement and navigationaluncertainty. Byadaptivelychoosing actions, we mean that thenext action of the robot is chosen so as to maximize the robot’sinformation about its location and all the features’ locations(the map).

The “amount” of information contained in eq. (1) can bequantified in various ways. Fisher information is of particularinterest and is related to the estimate of the statex given theobservations. The Fisher information for a random parameteris defined (Bar-Shalom and Fortmann, 1988) as the covarianceof the gradient of the total log probability; that is,

I k|k ≡ E{(∇x ln p(x, Zk))(∇x ln p(x, Zk))T }

= − E{∇x∇Tx ln p(xk, Zk)},

(9)

where∇x = [ ddx1

· · · ddxN

]T is the gradient operator with

respect tox = [x1 · · · xN ]; thus,∇x∇Tx is the Hessian matrix.

Strictly speaking, the Fisher information is defined only inthe non-Bayesian view of estimation (Bar-Shalom and Fort-mann, 1988); that is, in the estimation of a nonrandom param-eter. In the Bayesian approach to estimation, the parameter israndom with a (prior) probability density function. However,as in the non-Bayesian definition of the Fisher information,the inverse of the Fisher information for random parameter es-timation is the Cramér-Rao lower bound for the mean squareerror.

Applying Bayes’s rule to eq. (9)—that is,p(xk, Zk) =p(xk, z, Zk−1) = p(xk, Zk−1) · p(z|xk)—and noting the lin-earity of the gradient and expectation operators, we obtain aFisher information update:

E{∇x∇Tx ln p(xk, Zk)} = E{∇x∇T

x ln p(xk, Zk−1)}+ E

{∇x∇T

x ln p(zk|xk)}

− I k|k = I k|k−1 − E{∇x∇T

x ln p(zk|xk)}

.

(10)

The first term on the right represents the Fisher information,I k|k−1, before the last measurement, whereas the second termcorresponds to the additional information gained by measure-mentzk.

If we obtain an efficient estimator forx, the Fisher informa-tion will simply be given by the inverse of the error covarianceof the state. Thus, under the assumption that eqs. (2) and (3)hold, the inverse of error covarianceP will be an estimate forthe Fisher information of the system; that is

I ≈ P−1 .

Under this assumption, and using eq. (5), the transformationM relatingI k|k to I k+1|k can be found. By combining this witheq. (10), we have a recursive Fisher information update that

depends on the actionsu (inputs). M will generally dependon the statexk+1 as well, which is not available. By invokingthe assumption of eq. (2),xk+1 is replaced by the estimatexk+1|k. Thus,M can be used to give us the optimal actionuk

to take given our model and assumptions.By optimal we mean that, under the assumption that the

EKF is the best estimator for our state, the actionu that max-imizes the knowledge (information) of the system given thecurrent knowledge of the system can be determined. This isnot necessarily the optimal action for the actual system.

At each time step, the algorithm seeks to determine thetransformationM and, from this, infer the optimal actionuk.Combining the vehicle prediction and EKF update step ofstochastic mapping,M is

I k+1|k+1 = (FxI−1k|kFT

x + G(uk)G(uk)T )−1 + HT

x R−1Hx,

(11)

whereFx andHx are the Jacobian off andh with respect tox evaluated atxk|k andxk+1|k, respectively. The first term onthe right of eq. (11) represents the previous information ofthe system, as well as the loss of information that occurs dueto the actionuk. The second term represents the additionalinformation gained by the system due to observations after theactionuk. Since this quantity is a function ofxk+1, which isunknown, we approximatexk+1 by xk+1|k by the assumptionof eq. (2). The action that maximizes the information can beexpressed as

uk = maxu

I k+1|k+1 = minu

Pk+1|k+1. (12)

The information is a matrix, and we require a metric to quan-tify the information. Furthermore, it is desired that this metrichave a simple physical interpretation.

For CML, it is desirable to use a metric that makes explicitthe trade-off between uncertainty in feature locations and un-certainty in the vehicle position estimate. To accomplish this,we define the metric by a cost functionC(P), which givesthe total area of all the error ellipses (i.e., highest probabilitydensity regions) and is thus a measure of our confidence inour map and robot position. That is,

C(P) = π∏

j

√λj (Prr ) + π

∑Ni=1

∏j

√λj (Pii )

= π√

det(Prr ) + π∑N

i=1√

det(Pii ) ,

(13)

whereλj (·) is thej th eigenvalue of its argument and det isthe determinant of its argument. Turning back to Figure 1b,eq. (13) minimizes the error ellipses, thus yielding a lower costfor position 1 than for position 2 as makes intuitive sense.

The action to take is obtained by evaluating eq. (12) overthe action space of the robot using the metric in eq. (13).This yields an adaptive stochastic mapping algorithm. Thisprocedure optimizes the information locally at each time step;thus, the adaptation step performs a local optimization. Note


that the action space of the robot is not limited to motioncontrol inputs. Other actions and constraints can readily beincluded in the control inputu, such as the measurements thatshould be taken by the sonar. For this, we control the set ofanglesUθ that are scanned by the sonar.

In principle, eq. (12) can be solved in symbolic form usingLagrange multipliers. However, the symbolic matrix inver-sion required in eq. (11) is very tedious and results in a verylarge number of terms. Furthermore, as more features areadded, the inversion scales exponentially in the number ofcalculations. Numerical methods, however, can be used toevaluate eq. (12) quite efficiently. The experiments below donot use a numerical technique such as the simplex methodto perform this optimization; however, this could be readilyincorporated in the future.

3.3. Data Association, Track Initiation, and Track Deletion

Section 3.1 outlined the stochastic mapping approach tofeature-based navigation. To employ stochastic mapping in areal-world scenario, we have to be able to extract features fromthe environment. Sonar is notorious for exhibiting dropouts,false returns, no returns, and noise. Thus, addressing theproblem of data association is critical for the validity of theobservation model (eq. (8)) and in employing a stochastic-mapping-based approach to CML. For this purpose, data as-sociation, initiation of new feature estimates (i.e., track ini-tiation), and the removal of outdated feature estimates (trackdeletion) for the mobile robot are described here.

The problem of data association is that of determining thesource of a measurement. Obtaining correct data associationcan pose a significant challenge when making observationsof environmental features (Smith, 1998). In our implemen-tation, data association is performed using a gated nearestneighbor technique (Bar-Shalom and Fortmann, 1988). Theinitiation of new feature tracks is performed using a DNN ini-tiator. The DNN initiator is similar in spirit to the logic-basedmultiple target track initiator described by Bar-Shalom andFortmann (1988). One important difference in our method isthat the vehicle’s position is uncertain, and this uncertaintyhas to be included when performing gating and finding thenearest neighbor. It is assumed that a sonar return originatesfrom not more than one feature.

To include the vehicle’s uncertainty in performing data as-sociation using a nearest neighbor gating technique, we needto transform the vehicle’s uncertainty into measurement spaceand add this uncertainty to the measurement uncertainty. Weassume that the true measurement of featurei at timek condi-tioned onZk−1 is normally distributed in measurement space.Furthermore, it is assumed that the transformation from ve-hicle space to measurement space retains the Gaussianity ofthe estimated state. Under these assumptions, one may definethe innovation matrixSi for featurei as

Si = Hxi

[Prr Pri

Pir Pii

]k|k

HTxi

+ R,

with

Hxi= dhi ([xr xi])

d[xr xi]∣∣∣∣[xr xi ]=[xrk+1|k xik+1|k ]

, (14)

wherehi is the observation model for featurei. Hxiis the

linearized transformation from vehicle space to measurementspace. The nearest neighbor gating is performed in innova-tion space. That is, defining the innovationν = z − zi , thevalidation region, or gate, is given by

νT S−1i ν ≤ γ. (15)

The value of the parameterγ is obtained from theχ2 distri-bution. For a system with 2 DoF, a value ofγ = 9.0 yieldsthe region of minimum volume that contains the measure-ment with a probability of 98.9% (Bar-Shalom and Fortmann,1988). This validation procedure defines where a measure-ment is expected to be found. If a measurement is outsidethis region, it is considered too unlikely to arise from featurei. If several measurements gate with the same featurei, theclosest (i.e., most probable) one is chosen.

In performing track initiation, all measurements that havenot been matched with any feature over the lastN time stepsare stored. That is, any measurement that was not matched toa known feature is a potential new feature. At each time step,a search for clusters of more thanM ≤ N measurements overthis set of unmatched measurements is performed. For eachof these clusters, a new feature track is initiated. A clusteris defined as at most one measurement at each time step thatgates according to eq. (15) with all other measurements in thecluster. For our systems, where the probability of false returnsis relatively low and the probability of detection is relativelyhigh, values ofM = 2 or 3 andN = M + 1 are sufficient.

A track deletion capability is also incorporated to provide alimited capability to operate in dynamic environments. Whena map feature is predicted to be visible but is not observedfor several time steps in a row, it is removed from the map(Leonard, Cox, and Durrant-Whyte, 1992). This is motivatedby assuming a probability of detectionPD < 1. Thus, ifthe feature has not been observed over the lastr time stepsduring which an observation was expected of the feature, theprobability of the feature being at the expected location is(1 − PD)r , assuming that the observations are independent.Thus, setting a threshold onr is equivalent to setting a thresh-old on the probability that the feature still exists at the pre-dicted location.

With the incorporation of these data association methods,the complete adaptive DNN stochastic mapping algorithm issummarized as follows:


1. State projection. The system state (vehicle and fea-tures) is projected to the next time step using the statetransition modelF along with the control inputuk.

2. Gating. The feature closest to each new measurement isdetermined and gated with the feature, and nonmatch-ing measurements are stored.

3. State update. Reobserved features update the vehicleand feature tracks using the EKF.

4. New feature generation. New features are initializedusing the DNN data association strategy.

5. Old feature removal. Out-of-date features are deleted.6. Adaptation step. Determine the next action to take,uk,

by optimizing eq. (13).

An outline of the algorithm for performing adaptive aug-mented stochastic mapping is shown in Figure 2.

4. Simulation Results

The algorithm described above has been extensively testedin simulation. In these simulations, as well as in the exper-iments to follow, a numerical approximation was performedby evaluating eq. (13) at a fixed number of points in the actionspace. The robot was constrained to move a distance of 0, 10,or 20 cm at each time step, and could only turn in incrementsof 22.5◦. Furthermore, the vehicle was constrained to get nocloser than 40 cm to the features (PVC tubes), since the sonarsignal in this range becomes unreliable. In all these simula-tions, range error was assumed to have a standard deviationof 2 cm, whereas the standard deviation for the bearing was10◦. Furthermore, the sonar could move in increments of 0.9◦between each measurement. Thus, a complete scan of 360◦consisted of 400 sonar returns. The standard deviation forthe vehicle odometry was set to 5% of the distance traveled.The angle uncertainty was set to 1◦. These parameters werechosen because they resemble the situation for the air sonar

adaptation

SM update

track deletiontrack initiation

SM update

feature integration

state prediction

data association

feature deletion

sonar

controlinput

Fig. 2. Structure of the adaptive delayed nearest neighborstochastic mapping algorithm.

experiments in Section 5. Two different types of simulationare described.

4.1. Adaptive Vehicle Motion

In these simulations, it was assumed that the robot stoppedand took a complete 360◦ scan of the environment before con-tinuing. The algorithm chose where to move adaptively. Thealgorithm took advantage of the fact that the measurementshave different certainties in range and bearing, thus formingan error ellipse. Note, however, that if the observations of atarget have nearly equal standard deviations in all directions,the robot will not move, since the loss of information fromodometric error is larger than the information gained by takinga measurement from a different location.

Figure 3 shows the three typical paths of the robot in thepresence of two features (8.4-cm radius PVC tubes) as a re-sult of adaptation. The robot started at (0,0), and the paths(solid, dashed, and dashed-dotted lines) occurred with ap-proximately equal frequency. The dotted lines around thePVC tubes denote the constraints placed on the robot for howclose it could come to the PVC tubes while still obtaining validsonar returns. (This is a limitation of the standard Polaroidsonar driver circuit.) The resulting 95% confidence ellipsesare drawn for the middle path, for the estimated position ofthe center of the features, and for the robot’s position. Therobot’s true position is indicated by a +, whereas the esti-mated position is shown by a×. The robot stopped movingafter about 15 time steps, since more information would belost than gained by moving. Also note that the robot movedgreater distances in the beginning before taking the next scanthan it did at the end of the run; more information was gainedby moving and taking a measurement from a different locationin the beginning than near the end of a run.

Figure 4 shows the average, over 2000 runs, of the totalcost (as defined by eq. (13)) of the system under adaptation asa percentage of the total cost of the system when moving ran-domly (solid line) or moving along the negativex-axis (dashedline) without adaptation. Moving along the negativex-axisis a worst case scenario; moving in a straight line from theinitial position in a direction between the two features wouldpractically be identical to the adaptive run, thus presenting anupper bound for straight-line motion. In all the runs withoutadaptation, the robot moved a distance of 10 cm at each timestep. As can be seen, the adaptation procedure obtains a costof about 60% of the nonadaptive strategies after about 8 timesteps. The lower cost signifies that the adaptive strategy hasobtained more information about the environment and, thus,has produced a more accurate estimate of the robot’s, as wellas the features’, position. The random motion slowly startedcatching up after the 8th time step, and by the 50th time step,it had achieved a cost about 15% higher than that of the adap-tive strategy. When moving along the negativex-axis, thecost actually starts increasing after about the 15th time step,


Fig. 3. Three typical paths taken by the robot in simulations ofadaptive concurrent mapping and localization in the presenceof two PVC tubes (filled circles) of radius 8.4 cm. Of 1000simulated runs, the robot chose to go to the left, to the right,and through the middle with about equal frequency. The 95%confidence level of the map for the middle path is shown bythe ellipses. The dotted circles define how close to the PVCtubes the robot is allowed to be (see text for symbols).

since the robot was so far away from the features that it lostmore information by moving than it gained by sensing at eachtime step. This is due to the poor angular resolution of thesonar.

Figure 5 shows the sensor trajectory for a simulation in-volving eight objects. Figure 6 shows the east and north vehi-cle location errors and associated error bounds as a functionof time during the simulation. After an initial loop aroundfour of the objects is executed, the error bounds converge andthe sensor wanders back and forth over a small area.

4.2. Adaptive Scanning and Motion

In using sonar for mapping an environment, one is limited bythe relatively slow speed with which measurements can beacquired. Thus, we imposed the additional constraint that thesonar could scan only an angle of 15◦ (i.e., 50% more than themeasurement bearing standard deviation) at each time step.The algorithm was required to decide where to direct the atten-tion of the robot. The algorithm thereby adaptively decidedwhere to look and where to move. This was implemented inthe framework outlined above by adding an additional actionuθ to be controlled and solving eq. (13) given the constraintsof the scan angle.

To compare the simulation results to the experiments andthe previous simulations, the simulations were, as before, con-ducted over 50 time steps. However, under the adaptive strat-

Fig. 4. Simulation result showing the cost functionC(P) whenperforming adaptation divided by the cost function when notperforming adaptation. Two cases in which no adaptation wasperformed are shown: the solid line denotes the case in whichthe robot moved randomly, whereas the dashed line denotesthe case in which the robot moved along the negativex-axis.

Fig. 5. Path of vehicle performing adaptive motion amongmultiple objects.


Fig. 6. Position estimate and 3σ bounds for adaptive motionamong multiple features. As can be seen, a steady state isreached after about 500 s.

egy, only a 15◦ scanning angle was chosen at each time step.This is equivalent to obtaining 17 sonar returns. Withoutadaptation, a complete scan was taken at every time step,generating 400 sonar returns. Figure 7 shows the relative costof adaptive sensing and motion with and without adaptation.The solid line denotes the case in which the robot moved ran-domly without adaptation, whereas the dashed line denotesthe case in which the robot moved along the negativex-axiswithout adaptation as a function of sonar returns. The dottedvertical line indicates the point at which the adaptive case wasterminated as 50 time steps was reached. As can be seen fromFigure 7, the adaptive method obtained a map with high con-fidence after relatively few sonar returns. After obtaining atotal of 20,000 returns, moving randomly and along the neg-ative x-axis achieved only 93% and 33% of the confidencelevels, respectively, that the adaptive method obtained with850 sonar returns. Thus, the adaptive strategy required fewermeasurements and achieved a higher confidence level thanany of the strategies without adaptation. The actual vehicle’smotion was similar to that shown in Figure 3.

Fig. 7. Advantage of adaptive sensing and motion control.The cost function when performing adaptation divided by thecost function when not performing adaptation is plotted. Twocases in which no adaptation was performed are shown: thesolid line denotes the case in which the robot moved randomly,whereas the dashed line denotes the case in which the robotmoved along the negativex-axis. The vertical dotted line in-dicates the point at which the adaptive method had completedits 50 time steps and terminated.

Table 1 compares all the strategies on the basis of howmany sonar returns and how many time steps are requiredbefore the map reaches a specified confidence level. As ex-pected, the adaptive sensing and motion strategy required thefewest number of sonar returns to reach a given confidencelevel. However, it used more time steps than the adaptivemotion strategy alone, since under the adaptive motion andsensing strategy only one feature was measured at each timestep, whereas under the adaptive motion strategy, both fea-tures were measured at each time step.

5. Air Sonar Experimental Results

The algorithm was implemented on a Nomad Scout robotequipped with a 50-kHz Polaroid 6500 series ultrasonic sen-sor mounted on a stepping motor that rotated the sensor in0.9◦ increments (see Figure 8). The error in the sensor andthe vehicle odometry was assumed to be the same as that usedin the simulations. The same constraints were employed aswell. The sonar returns were assumed to be mainly specular;therefore, regions of constant depth were extracted from thescans (Leonard and Durrant-Whyte, 1992). In these experi-ments, rather than using the DNN track initiator, tracks wereinitiated from the first scan only. Figure 9 shows a roughmodel of the room and the configuration of the robot and


Table 1. Resources Needed to Achieve a Given CostC in SimulationC = Ce C = Cr

Strategy Returns Steps Returns StepsAdaptive sensing and motion 100 6 300 18Adaptive motion 1600 4 5200 13No adaptation, random motion 3200 8 20000 50No adaptation, line motion 3600 9 ∞ ∞

Ce = 0.038 m2 is the minimum cost achieved when moving in one direction during the experiment.Cr = 0.0019 m2 is theminimum cost achieved in the simulations when moving randomly.

Fig. 8. The Nomad Scout robot with the Polaroid ultrasonicsensor mounted on top.

features (PVC tubes of known radius) in the experimentalsetup. The dots indicate individual sonar returns of the Po-laroid sensor from a complete scan of the environment. Eachcomplete scan of the environment consisted of 400 sonar re-turns. In these experiments, 15 motion steps were performedin each run.

Figure 10 shows the advantage of adaptation for a repre-sentative Nomad run, which is similar to the simulation ofFigure 4. As can be seen from this figure, the advantage ofperforming adaptation is clear. Furthermore, the experimen-tal result was well within the one-standard-deviation boundof the simulated predicted result over 2000 runs.

Figure 11 shows the advantage of performing adaptive mo-tion and sensing over a straight-line movement along the neg-ativex-axis for the Nomad Scout robot (solid line). The sim-

Fig. 9. The returns from the Polaroid sensor (dots) with arough room model superimposed. The robot is drawn asa triangle, and the PVC tubes are indicated by small filledcircles.

ulated result is shown by a dashed line along with the one-standard-deviation bound for 2000 simulated runs. As canbe seen, the experimental cost ratio was within the one stan-dard deviation of the simulated value. The adaptive strategyproduced a high confidence map after relatively few measure-ments, which is consistent with the simulations in Figure 7.The nonadaptive method reached a confidence level of only50% of the adaptive level, even after more than 20 times asmany measurements were taken.

Table 2 shows the number of time steps and sonar returnsrequired under each strategy to reach a map with some maxi-mum specified costC. As expected, the adaptive sensing andmotion strategies required orders of magnitude less sonar re-turns than any of the other strategies. However, the adaptivemotion strategy also used fewer time steps to reach a specifiedconfidence level. When comparing this table of experimentalresults to Table 1, we see that they are consistent.


Fig. 10. Comparison of cost functionC(P) with and withoutadaptation for a representative Nomad experiment. The solidline is the ratio of the cost when performing adaptation dividedby the cost of moving in a straight line along the negativex-axis. The dashed line denotes the average simulation resultover 2000 runs, and the dotted line denotes the one-standard-deviation bound.

Fig. 11. Advantage of adaptive sensing and motion in a rep-resentative Nomad experiment. The solid line is the cost ofadaptive sensing and motion divided by the cost of movingin a straight line along the negativex-axis. The dashed lineis the average of 2000 simulated runs, and the dotted linesshow the one-standard-deviation bound. The dotted verticalline indicates the time at which the adaptive case terminatedas 15 time steps were completed.

Table 2. Number of Scans and Time Steps Required toAchieve a Cost of Ce = 0.038 m2 or Less for a Repre-sentative Experiment

Strategy,C = Ce Returns Steps

Adaptive sensing and motion 100 6Adaptive motion 2000 5No adaptation, line motion 6000 15

The costCe was chosen to be the minimum cost achievedduring the no-adaptation strategy.

6. Underwater Sonar Experimental Results

The second type of experiments conducted to test the adaptivestochastic mapping algorithm used a narrow-beam 675-kHzsector scan sonar mounted on a planar robotic positioningsystem, as shown in Figure 12. The positioning system wascontrolled by a Compumotor AT6450 controller card. Thesystem was mounted on a 3.0×9.0×1.0 m testing tank. Thesystem was executed on a PC, and controller software andCML code were integrated to obtain a closed-loop system forperforming CML. At each time step, eq. (13) was minimizedover the action space of the robot to choose the motion andscanning angles of the sensor.

The experiments were designed to simulate an underwatervehicle equipped with a sonar that can scan at any directionrelative to the vehicle at each time step. Conducting complete360◦ scans of the environment at every time step is slow witha mechanically scanned sonar and computationally expensivewith an electronically scanned sonar. For these experiments,we envisioned a vehicle mounted with two sonar systems, oneforward looking for obstacle avoidance and one that can scanat any angle for localization purposes. The forward-lookingsonar was assumed to scan an angle of±30◦. The scanningsonar was limited to scan overUθ = [−15◦, 15◦] at each timestep. The scanning was performed in intervals of 0.15◦. Thesonar systems were modeled to have a standard deviation inbearing of 10.0◦ and 2.0 cm in range. Between each scanby the sonar, the vehicle could move between 15 cm and 30cm. The lower limit signifies a minimum speed at whichthe vehicle could move before loosing control, whereas theupper limit signifies the maximum speed of the vehicle. Thevehicle was constrained to turn in increments of only 22.5◦.Furthermore, we assumed that the vehicle was equipped witha dead-reckoning system with an accuracy of 10% of distancetraveled and an accuracy of 1.0◦ in heading.

Figure 13 shows a typical scan taken by the sonar from theorigin. The crosses show individual sonar returns. The circlesshow the features (PVC tubes). The dotted circles around thefeatures signify the minimum allowable distance between thevehicle and the features. The triangle shows the position ofthe sensor. Circular arc features were extracted from the sonarscans using the technique described by Leonard and Durrant-Whyte (1992).


Fig. 12. The planar robotic positioning system and sector scansonar used in the underwater sonar experiments. The waterin the tank is approximately 1 m deep. The transducer wastranslated and rotated in a horizontal plane midway throughthe water column.

Figure 14 shows the sensor trajectory for a representativeunderwater sonar experiment with the complete algorithm.The sensor started at the origin and moved around the tankusing the adaptive stochastic mapping algorithm to decidewhere to move and where to scan. Based on minimization ofthe cost function in eq. (13), the vehicle selected one targetto scan to provide localization information. In addition, ateach time step, the sonar was also scanned in front of thevehicle for obstacle avoidance. Figure 15 shows thex andy errors for the experiment and associated error bounds. Nosonar measurements were obtained from approximately timestep 75 to time step 85 due to a communication error betweenthe sonar head and the host PC. Figure 16 shows the cost asa function of time. Solid vertical lines in Figures 15 and 16indicate the time steps when features of the environment wereremoved. Figure 17 plots the vehicle position error versustime for the stochastic mapping algorithm in comparison todead reckoning.

Figures 18 through 20 show the results of a nonadaptiveexperiment in which the vehicle moved in a straight line in thenegativex-direction with the two objects present throughoutthe experiment. Without adaptive motion, the observabilityof the features was degraded and they estimate seemed todiverge. Comparing Figure 19 with Figure 15, we observethat the nonadaptive strategy had a maximum 3σ confidencelevel of about 0.8 m and 0.2 m in thex andy positions, re-spectively, whereas the maximum 3σ confidence level underadaptation was about 0.17 m and 0.15 m in they andx po-sitions, respectively. A further indication of the advantage ofthe adaptive approach can be seen by comparing Figure 20with Figure 17.

Interestingly, over a range of different operating condi-tions, the system selectively explores different objects in the

Fig. 13. The returns from the underwater sonar for a 360◦ scanof the tank from the origin. The crosses show individual re-turns. The small circles identify the positions of the features(PVC tubes), with a dotted 5-cm outside circle drawn aroundthem to signify the minimum allowable distance between thesonar and the features. The sonar was mounted on the car-riage of the positioning system, which served as a simulatedautonomous underwater vehicle. The location of the sonar isshown by a triangle. The outline of the tank is shown in gray.

environment. For example, Figure 21 shows the sensor pathfor two different underwater sonar experiments under similarconditions in which an exploratory behavior can be observed.

7. Conclusion

This paper considered the problem of adaptive sensing forfeature-based CML by autonomous underwater vehicles. Anadaptive sensing metric was incorporated within a stochasticmapping algorithm and tested via air and underwater experi-ments. This is the first time, to our knowledge, that a feature-based CML algorithm has been implemented with underwatersonar data.

We introduced a method for performing adaptive CML in apriori unknown environments for any number of features. Theadaptive method was based on choosing actions that, given thecurrent knowledge, would maximize the information gainedin the next measurement. This approach can easily be imple-mented as an extra step in a stochastic mapping algorithm forCML. The validity and usefulness of the approach were veri-fied in simulation and in experiments with air and underwatersonar data.

Based on the air sonar experiments, we feel confident thatthe simulation can accurately predict experimental outcomes.For example, the three typical paths for the robot shown in Fig-ure 3 are representative of both adaptive simulations and realdata experiments, and the plots comparing the performance


Fig. 14. Time evolution of the sensor trajectory for an adaptive stochastic mapping experiment with the underwater sonar. Thefeature on the right was added at the 40th time step, and the two features on the left were removed near the end of the experi-ment. The vehicle started at the origin and moved adaptively through the environment to investigate different features in turn,maximizing eq. (13) at each time step. The filled circles designate the feature locations and are surrounded by dotted circles thatdesignate the standoff distance used by an obstacle avoidance routine. Similarly, the dashed-dotted rectangle designates thestandoff distance to the tank walls. The dashed-dotted line represents the scanning region selected by the vehicle at the last timestep. The large triangle designates the vehicle’s position. The vehicle was constrained from moving outside the dashed-dottedlines to avoid collisions with the walls of the tank. Sonar returns originating from outside the dashed-dotted lines were rejected.

with and without adaptation are similar. The advantage ofperforming adaptive CML is notable when adapting only themotion of the vehicle (Figs. 4 and 10). However, more sub-stantial gains are obtained when performing adaptive motionandsensing. This is apparent from Figures 7 and 11, wherethe number of sonar returns required to obtain a given con-fidence level is an order of magnitude fewer than when notperforming adaptation.

The adaptive sensing technique employed here is a localmethod. At each cycle, only the next action of the robot isconsidered. By predicting over an expanded time horizon,one can formulate global adaptive mapping and navigation.For example, one can consider how to determine the best pathbetween the robot’s current position and a desired goal posi-tion. In this case, the space of possible actions grows tremen-dously, and a computationally efficient method for searchingthe action space will be essential.

In future research, we will integrate adaptive sensingwithin a hybrid estimation framework for CML in develop-ment in our laboratory (Smith and Leonard, 1997; Smith,1998). In addition, we will perform experiments with morecomplex objects and develop methods that incorporate addi-tional criteria for adaptation to address effects such as occlu-sion, rough surface scattering, and multiple reflections.

CML is a compelling topic for investigation because thereare so many open issues for future research. Key questionsinclude the following:

• How can representation of complex, natural environ-ments be incorporated within a metrically accurate,feature-based mapping approach?

• How can we reliably extract features from sensor data?• What are the trade-offs between global versus local,

absolute versus relative, and metrical versus topologicalmapping?

• How can we select which types of environmental fea-tures are most useful as navigation landmarks?

• How can data association ambiguity be effectivelyaddressed?

• How can computational complexity be overcome inextending CML to long duration missions over largeareas?

• In which situations can long-term bounded position er-rors be achieved?

• How can CML be extended to dynamic environmentsin which previously mapped environmental objects canmove or disappear and new features can appear in pre-viously mapped areas?

• How can CML be integrated with path planning andobstacle avoidance?


Fig. 15. Position errors in thex andy directions and 3σ con-fidence bounds for adaptive underwater experiment.

Fig. 16. Cost as a function of time for the adaptive underwaterexperiment. The cost increased at approximately the 40thtime step when the third object was inserted into the tankand was observed for the first time. During the time intervalbetween the two dashed vertical lines, no sonar data wereobtained due to a serial communications problem between thePC and the sonar head. The two solid vertical lines designatethe time steps during which features were removed from thetank, to simulate a dynamic environment.

Fig. 17. Vehicle position error versus time for dead reckon-ing (dashed line) and the adaptive delayed nearest neighborstochastic mapping algorithm (solid line). During the timeinterval between the two dashed vertical lines, no sonar datawere obtained due to a serial communications problem be-tween the PC and the sonar head. The two solid vertical linesdesignate the time steps when features were removed fromthe tank, to simulate a dynamic environment.


Fig. 18. Sensor trajectory for a nonadaptive experiment inwhich the vehicle moved in a straight line. While accuratelocation information was obtained in thex direction, the con-current mapping and localization process diverged in they-direction. The dashed-dotted rectangle indicates the standoffdistance from the tank walls used by an obstacle avoidanceroutine.

Fig. 19. Position errors in thex andy directions and 3σ con-fidence bounds for the nonadaptive underwater experiment.

Fig. 20. Vehicle error as a function of time for the nonadaptiveunderwater experiment.

Although this paper has focused on an adaptive sensing met-ric for improved state estimation performance in CML, webelieve that other types of sensing strategies can help answermany of these questions. We have shown that adaptive sens-ing can save time and energy and reduce the amount of datathat needs to be acquired. However, we believe that adaptivestrategies have the potential to improve state estimation ro-bustness, ease data association ambiguity, prevent divergence,and facilitate recovery from errors. Investigation of adap-tive strategies for these purposes will be addressed in futureresearch.

Appendix: Stochastic Mapping

This appendix provides more detail on the stochastic mappingmethod, based on the work of Smith, Self, and Cheeseman(1990). Stochastic mapping is simply a special way of orga-nizing the states in an EKF and providing a consistent way ofadding new states to the systems as more features are observedand estimated. In a Kalman filter, through the combinationof a dynamic model and an observation model, an estimatedstatex and associated estimated covarianceP are produced.In this paper, the dynamic model for the robot was given by

xrk+1|k = f (xrk , uk) + G(uk)dx= xrk|k + Tφk

+ G(uk)dx,

where, as in the main text,k defines the time index. Since allfeatures are assumed to be static, there are no dynamics forthe features; thus,

xik = xik .

The noise scaling matrixG is defined in eq. (6), and the trans-formation matrixT is defined in eq. (7). As in the main text,


Fig. 21. Two representative stochastic mapping experimentsthat exhibited adaptive behavior. In each figure, the solid lineshows the estimated path of the sensor and the dashed lineshows the actual path. The triangle indicates the final posi-tion of the sensor. The filled disks indicate the locations ofthe features (PVC tubes). The ellipses around the featuresand the sensor are the 3σ contours; that is, the 99% high-est confidence regions. The sonar view is indicated by thedashed-dotted line. In the left figure, the sensor had a scan-ning angle of[−30◦, 30◦]. The sensor started at (0,0) andadaptively determined the path to take and the direction toscan, resulting in “exploratory” behavior. The sensor firstmoves over to one of the objects, turns, and moves aroundthe second. The experiment illustrated in the right figure wassimilar, with the exception that the sonar was able to scan anarea of only[−7.5◦, 7.5◦] each time step. Again, we can seethat exploratory behavior emerged as the sensor attempted tomaximize the information it obtained about the environment.In these experiments, rather than using the delayed nearestneighbor track initiator, tracks were initiated from the firstscan only. In addition, the robot was constrained to turn amaximum of 30◦ at each time step in 15◦ increments.

the state vectorx = [xr x1 . . . xN ] consists of the robot’s statexr and all theN feature states denoted byx1 . . . xN . Thecontrol input is given byu, and the process noise is given bydx.

As the robot moves around in its environment, the fea-tures are observed through the robot’s sensor. The relationbetween the current state and the observations is given by theobservation model

zk = h(xk) + dz, (16)

wherez is range and bearing measurements to the features inthe environment andh is the nonlinear transformation fromthe Cartesian coordinates ofx and the polar coordinates ofz.

As the robot moves around in its environment, the systemevolves through (1) robot displacement, (2) new feature inte-gration, and (3) reobservation of features. Each of these stepsof stochastic mapping is presented below.

Robot Displacement

When the robot moves a distance given byuk, the robot state,xr , estimated at time stepk + 1, is given by taking the expec-tation of the dynamic model given above:

xrk+1|k = xrk|k + Tφk|k uk andxik+1|k = xik .

Under the assumption of eq. (2), that this approximationerror is small, the Fisher information is updated through thelinearized system by

I k+1|k = (FxI−1k|kFT

x + G(uk)G(uk)T )−1.

Here,Fx, is the Jacobian off with respect tox evaluated atxk|k.

New Feature Integration

If the robot observes a new featureznew = [r θ ]with respect tothe its reference frame, a new feature statexN+1 is estimatedand incorporated by

xN+1 = l(xk|k, znew) =[xr + r cos(φ + θ )

yr + r sin(φ + θ )

].

The new feature is integrated into the map by adding this newstate tox andI ; that is,

xk+1|k =[

xk|kxN+1

]

PN+1N+1 = Lxr PrrLTxr

+ LzRLTz

PN+1 i = PTi N+1 = Lxr Pri ,


whereLxr andLz constitute the Jacobian ofl with respectto the robot statexr evaluated atxrk|k andznew evaluated atznew. The covariance of the state of the new features is givena priori byR. The structure of the covariance for the system,P, is given in eq. (4), and its inverse is a good estimate for theFisher information of the system; that is,P = I−1.

Reobservation of Features

When a featurei is reobserved, we use the prediction stepof the EKF to update the vehicle’s state and the map. Byintroducing the following definitions,

xi = xik+1|k − xrk+1|kyi = yik+1|k − yrk+1|k ,

the observation modelh (eq. (16)) for featurei takes the form

zik =[rikθik

]=

[ √x2i + y2

i

tan−1(yi/xi) − φk

]+ dzi

= hi (xk) + dzi .

The noise processdziis assumed to be white and Gaussian

with covarianceRi . If the n featuresi1 . . . in are reobserved,the observation model becomes

zk =

zi1k

...

zink

, h =

hi1...

hin

, R =

Rin · · · 0...

. . ....

0 · · · Rin

,

with the Jacobian ofh given by Hx = Hx(xk+1|k) =dh(x)/dx|x=xk+1|k . These matrices are used in the updatestep of the EKF as follows:

xk+1|k+1 = xk+1|k + K k+1(zk+1 − h(xk+1|k))I k+1|k+1 = I k+1|k + HT

x R−1Hx,

whereK k+1 is the EKF gain given by

K k+1 = I−1k+1|kHT

x (HxI−1k+1|kHT

x + R)−1.

Computational Issues

To prevent instability in the coding of the EKF, it is advisedthat the square root filter or the Josephson form covarianceupdate (Bar-Shalom and Li, 1993) be employed. In these im-plementations, the Josephson form was used, which ensuresthe positive definiteness of the EKF.

To prevent the EKF from becoming overconfident, extraprocess noise was added to the feature position. That is, ateach time step, each feature was assumed to have a processnoise including the same order of magnitude asdx. This wasplaced in theG matrix.

Acknowledgments

This research was funded in part by the Naval Undersea War-fare Center, Newport, Rhode Island. The first author ac-knowledges the support of NFR (Norwegian Research Coun-cil) through grant 109338/410. The second author acknowl-edges the support of the Henry L. and Grace Doherty As-sistant Professorship in Ocean Utilization and NSF CareerAward BES-9733040. Furthermore, the authors would liketo thank Seth Lloyd for fruitful discussions and comments.The authors would also like to thank Jan Meyer, Jong H. Lim,Marlene Cohen, and Tom Consi for their help with the Nomadhardware integration, Professor J. Milgram for the use of thetesting tank, Paul Dambra for the installation of the position-ing system, and Imagenex Technology Corporation and PaulNewman for their help with the sonar integration.

References

Au, W. 1993.The Sonar of Dolphins. New York: Springer-Verlag.

Bajcsy, R. 1988. Active perception.Proc. IEEE76:996–1005.

Bar-Shalom, Y., and Fortmann, T. E. 1988.Tracking andData Association. New York: Academic Press.

Bar-Shalom, Y., and Li, X.-R. 1993.Estimation and Track-ing: Principles, Techniques, and Software. Artech House.

Bellingham, J. G., Goudey, C. A., Consi, T. R., Bales, J. W.,Atwood, D. K., Leonard, J. J., and Chryssostomidis, C.1994. A second generation survey AUV.IEEE Conferenceon Autonomous Underwater Vehicles, Cambridge, MA.

Bellingham, J. G., and Willcox, J. S. 1996. Optimizing AUVoceanographic surveys.IEEE Conference on AutonomousUnderwater Vehicles, Monterey, CA.

Berger, J. O. 1985.Statistical Decision Theory and BayesianAnalysis/, 2nd ed. Springer-Verlag.

Betgé-Brezetz, S., Hébert, P., Chatila, R., and Devy, M. 1996.Uncertain map making in natural environments.Proc.IEEE Int. Conf. Robotics and Automation, pp. 1048–1053.

Blake, A., and Yuille, A., eds. 1992.Active Vision. Cam-bridge, MA: MIT Press.

Bozma, O., and Kuc, R. 1991. Characterizing pulses reflectedfrom rough surfaces using ultrasound.J. Acoustical Soc.Am. 89(6):2519–2531.

Chong, K. S., and Kleeman, L. 1997. Sonar feature mapbuilding for a mobile robot.Proc. IEEE Int. Conf. Roboticsand Automation.

Elfes, A. 1987. Sonar-based real-world mapping and naviga-tion. IEEE J. Robotics AutomationRA-3:249–265.

Gelb, A. C. 1973.Applied Optimal Estimation. Cambridge,MA: MIT Press.

Hager, G. 1990.Task-Directed Sensor Fusion and Planning:A Computational Approach. Boston: Kluwer Academic.


Hébert, P., Betgé-Brezetz, S., and Chatila, R. 1996. Decou-pling odometry and exteroceptive perception in buildinga global world map of a mobile robot: The use of localmaps. Proc. IEEE Int. Conf. Robotics and Automation,pp. 757–764.

Kuipers, B. J., and Byun, Y. 1991. A robot exploration andmapping strategy based on a semantic heirarchy of spatialrepresentations.Robotics Autonomous Sys., pp. 47–63.

Leonard, J. J., Bennett, A. A., Smith, C. M., and Feder, H.J.S.1998. Autonomous underwater vehicle navigation.IEEEICRA Workshop on Navigation of Outdoor AutonomousVehicles, Leuven, Belguim.

Leonard, J. J., Cox, I. J., and Durrant-Whyte, H. F. 1992. Dy-namic map building for an autonomous mobile robot.Int.J. Robotics Research11(4):286–298.

Leonard, J. J., and Durrant-Whyte, H. F. 1992.Directed SonarSensing for Mobile Robot Navigation. Boston: KluwerAcademic.

Lucido, L., Popescu, B., Opderbecke, J., and Rigaud, V. 1996.Segmentation of bathymetric profiles and terrain matchingfor underwater vehicle navigation.Proc. of Second AnnualWorld Automation Conf., Montpellier, France.

Manyika, J. S., and Durrant-Whyte, H. F. 1994.Data Fusionand Sensor Management: A Decentralized Information-Theoretic Approach. New York: Ellis Horwoord.

Medeiros, M., and Carpenter, R. 1996. High resolution arraysignal processing for AUVs.AUV 96, pp. 10–15.

Milne, P. H. 1983.Underwater Acoustic Positioning Systems.London: E.F.N. Spon.

Moran, B. A., Leonard, J. J., and Chryssostomidis, C. 1997.Curved shape reconstruction using multiple hypothesistracking. IEEE J. Ocean Eng.22(4):625–638.

Moutarlier, P., and Chatila, R. 1989. Stochastic multisen-sory data fusion for mobile robot location and environmentmodeling.5th Int. Symp. on Robotics Research, Tokyo.

Rencken, W. D. 1993. Concurrent localisation and map build-ing for mobile robots using ultrasonic sensors.Proc. IEEE

Int. Workshop on Intelligent Robots and Systems, Yoko-hama, Japan, pp. 2192–2197.

Russell, S., and Norvig, P. 1995.Artificial Intelligence: AModern Approach. Englewood Cliffs, NJ: Prentice Hall.

Russell, S., and Wefald, E. 1995.Do the Right Thing: Studiesin Limited Rationality. Cambridge, MA: MIT Press.

Singh, H. 1995. An entropic framework for AUV sensor mod-elling. Ph.D. thesis, Massachusetts Institute of Technology.

Smith, C. M. 1998. Integrating mapping and navigation.Ph.D. thesis, Massachusetts Institute of Technology.

Smith, C. M., and Leonard, J. J. 1997. A multiple hypoth-esis approach to concurrent mapping and localization forautonomous underwater vehicles.Int. Conf. on Field andService Robotics, Sydney.

Smith, R., Self, M., and Cheeseman, P. 1990. Estimatinguncertain spatial relationships in robotics. In Cox, I., andWilfong, G. (eds.)Autonomous Robot Vehicles. Springer-Verlag.

Stewart, W. K. 1988. Multisensor modeling underwater withuncertain information. Ph.D. thesis, Massachusetts Insti-tute of Technology.

Thrun, S., Fox, D., and Burgard, W. 1998. A probabilistic ap-proach to concurrent mapping and localization for mobilerobots.Machine Learning31:29–53.

Tuohy, S. T., Leonard, J. J., Bellingham, J. G., Patrikalakis,N. M., and Chryssostomidis, C. 1996. Map based naviga-tion for autonomous underwater vehicles.Int. J. OffshorePolar Eng.6(1):9–18.

Uhlmann, S. J., Julier, M., and Csorba, J. K. 1997. Nondi-vergent simultaneous map building and localisation usingcovariance intersection. Navigation and control technolo-gies for unmanned systems II.

Urick, R. 1983.Principles of Underwater Sound. New York:McGraw-Hill.

Willcox, J. S., Zhang, Y., Bellingham, J. G., and Marshall, J.1996. AUV survey design applied to oceanic deep con-vection. IEEE Oceans, pp. 949–954.

Documents

Hans Jacob S. Feder Adaptive Mobile Robot John J. Leonard … · 2015. 6. 2. · sensing (Leonard and Durrant-Whyte, 1992), active infor-mation gathering (Hager, 1990), adaptive sampling