Mapping Partially Observable Features from Multiple...

Mapping Partially Observable Features from Multiple UncertainVantage Points

JohnJ.Leonard,RichardJ.Rikoski,Paul M. Newman,andMichaelBosseMIT Dept.of OceanEngineeringCambridge,MA 02139,U.S.A

[email protected],[email protected],[email protected],[email protected]

Abstract

This paperpresentsa techniquefor mappingpartially observable featuresfrom multiple un-certainvantagepoints. The problemof concurrentmappingandlocalization(CML) is statedasfollows: startingfrom aninitial known position,a mobilerobottravelsthroughasequenceof posi-tions,obtainingasetof sensormeasurementsateachposition.Thegoalis to processthesensordatato produceanestimateof thetrajectoryof therobotwhile concurrentlybuilding a mapof theenvi-ronment.In this paper, we describea generalizedframework for CML that incorporatestemporalaswell asspatialcorrelations.Therepresentationis expandedto incorporatepastvehiclepositionsin thestatestatevector. Estimatesof thecorrelationsbetweencurrentandprevious vehiclestatesareexplicitly maintained.Thisenablestheconsistentinitializationof mapfeaturesusingdatafrommultiple timesteps.Updatesto themapandthevehicletrajectorycanalsobeperformedin batchesof dataacquiredfrom multiple vantagepoints. The methodis illustratedwith sonardatafrom atestingtankandvia experimentswith aB21landmobilerobot,demonstratingtheability to performCML with sparseandambiguousdata.

1 Introduction

This paperpresentsa generalizedframework for feature-basedconcurrentmappingand localization

(CML) thatenablesmappingof partially observablefeaturesfrom multiple uncertainvantagepoints.

This enablesCML to be performedin situationswhereindividual measurementsprovide weakgeo-

metric constraints,suchaswith wide-beamsonarsensors.The problemof CML, alsoreferredto as

simultaneouslocalizationandmapping(SLAM), is statedasfollows: startingfrom aninitial known po-

sition, a mobile robottravels througha sequenceof positions,obtaininga setof sensormeasurements

at eachposition. Thegoal is to processthesensordatato produceanestimateof thetrajectoryof the

robotwhile concurrentlybuilding amapof theenvironment.

D�

oor D�

oorD�

oorD�

oor

Door Door Door Door D�

oorDoor

D�

oor

Figure1: Hand-measuredmodelof acorridor(total lengthapproximately25 meters).

Figure2: Laser(left) andsonar(right) datataken with a B21 mobile robot in the corridor shown in Figure1,referencedto the dead-reckoning positionestimate.The vehicletraveledback-and-forththreetimesfollowingroughly thesamepath. Eachsonarandlaserreturnis shown referencedto odometry. The laserdatais slightlysmearedduringturneddueto a latency betweentheodometryandthelaserdata.

The key technicaldifficulty in performingCML is coping with uncertainty(Brooks 1984). For

example,Figures2 shows SICK laserscannerdataandPolaroidsonardatacollectedby a B21 mobile

robotduringseveralback-and-forthtraversesof a shortcorridor (about60 meterstotal travel length).

Onecanseethatthedead-reckoningerrorof thevehiclebecomesintermingledwith uncertaintyin the

valuesof measurements(noise)anduncertaintyin theorigins of measurements(spuriousreflections,

ambiguousassociations).Thesethreedistinctformsof uncertainty– navigationerror, sensornoise,and

dataassociationambiguity– combineto presentachallengingdatainterpretationproblem.

The CML problemcanbe addressedusinga variety of differentrepresentations,suchasevidence

grids (Schultzand Adams1998) and topologicalmodels(Kuipers2000). We advocatethe useof

a feature-based,probabilisticrepresentation.Smith, Self, and Cheeseman(1987) were the first re-

searchersto casttheproblemof feature-basedCML usingavariable-dimensionstateestimationformu-

lation. In their approach,the locationsof boththerobotanda numberof objects(geometricfeatures)

2

in theenvironmentarecombinedinto a singlestatevectorin anappropriateparameterspace.Thelo-

cationsof therobotandthefeaturesareconcurrentlyestimatedusingrecursive stateestimation.From

thetitles of thesetwo papers,we usetheterm“stochasticmapping”to referto this typeof algorithm.1

Examplesof recentwork on CML in roboticsthat follow this formulationincludeDavison usingvi-

sion (1998), Guivant and Nebot (2001), Castellanoset al. (2000), Dissanayake et al. (1999), and

Jensfelt(2001)usinglasersensing,andLeonardandFeder(2001),Tard́oset al. (2001),andWilliams

(2001)usingsonar. Gibbenset al. have analyzedtheclosed-formsolutionof thelinear, singledegree-

of-freedomCML problem,yielding someinsightsinto convergence.This body of work canbe con-

trastedwith otherapproachesto CML thatdo not rely on feature-basedmodels,suchasGutmannand

Konolige(1999),Thrun(2001)andChosetandNagatani(2001).

Thegeneralproblemof CML incorporatingdataassociationambiguitycanbecastasahybrid(mixed

continuous/discrete)estimationproblemin which trackinganddataassociationareintertwined(Bar-

ShalomandFortmann1988).Generaltheoreticalmodelsfor hybrid stateestimation,suchasmultiple

hypothesistracking(Reid 1979; Mori, Chong,Tse,andWishner1986),presenta staggeringlylarge

computationalburdenwhenappliedto CML. Thenavigationerrorof theplatform preventsonefrom

splitting the solution into separatesubgroups(known as“clusters” in the tracking literature(Kurien

1990)) correspondingto different partsof the environment(Cox and Leonard1994). Most imple-

mentationsof stochasticmappingdecouplethe discrete(decision-making)andcontinuous(filtering)

partsof theproblem.TheextendedKalmanfilter (EKF) is typically usedfor continuousstateestima-

tion (Smith,Self,andCheeseman1987;Feder1999;GuivantandNebot2001;CastellanosandTardos

2000). This is the methodusedin this paper, but it is not the only stateestimationalgorithmwhich

couldbeused.For instance,sequentialMonteCarloalgorithms(Doucet,deFreitas,andGordan2001;

Thrun2001)couldbechoseninstead.

Discretestateestimation— makingdecisionsabouttheoriginsof measurements— is usuallyper-

formed in CML with maximumlikelihoodmethodssuchas “nearest-neighborgating” (Bar-Shalom

andFortmann1988). Suchmethodsencounterdifficultieswhenthe distancebetweenfeaturesin the

environmentis smallerthantheuncertaintyin therobotposition.Unfortunately, thissituationcanarise

frequentlyin practice.For example,in the datafor the corridor shown in Figure2, the doorsarere-

cessedabout10 centimetersfrom thecorridorwall. Theodometricuncertaintyis muchlarger. In this

situation,it is very difficult to associatemeasurementswith featureswhenconsideredin isolation. A1Notethatthisuseof thetermstochasticshouldnotbeconfusedwith randomizedalgorithmsthatthemselvesarestochas-

tic, suchasprobabilisticalgorithmsfor motionplanning(Kavraki, Svestka,Latombe,andOvermars1996).

3

morepowerful techniquethat teststhe joint compatibility of multiple sensormeasurements,usinga

branchandboundalgorithm,hasbeendevelopedby NeiraandTard́os(2001)andsuccessfullyapplied

to CML usinglaserdata(Newman,Leonard,Neira,andTard́os2002)andsonardata(Tard́os,Neira,

Newman,andLeonard2001).Dataassociationis not theprimaryfocusof thispaper, howeverthetopic

is intimatelylinkedwith theproblemof mappingpartiallyobservablefeatures.It is desirableto employ

methodsfor perceptualgroupingthatcanrejectoutlierswhile findingsetsof measurementsthat,when

interpretedtogether, yield a consistentexplanation.This paperfocuseson thestateestimationaspects

of thisproblem,describingarepresentationthatallowstheoutputof perceptualgroupingroutinesto be

consistentlyappliedfor mappingfrom multipleuncertainvantagepoints.

In thecomputervision community, theanalogousproblemto CML is Structurefrom Motion (SFM)

(Faugeras1993; Taylor and Kriegman1995; Chiuso,Favaro, Jin, and Soatto2000; Yagi, Shouya,

andYachida2000;Hartley andZisserman2001). An early approachto SFM thatusedthe EKF was

developedbyAyacheandFaugeras(1989).Thisworkwasoneof thefirst to applygeometricconstraints

in the stateestimationprocess,suchasthe fusion of two featuresin the mapthat areassertedto be

the same. Recently, constraintapplicationhasbeenincorporatedin stochasticmappingalgorithms

by ChongandKleeman(1997a),Tard́os et al. (2001)andWilliams et al. (2001). Of recentwork in

computervision, thework of Chiusoet al. (2000)andMcLauchlan(2000)arethemostcloselyrelated

to themethodpresentedin this paper.

TheSmith,Self andCheeseman(1987)algorithmfor CML usesthreemodels:(1) a robot motion

model,(2) a featuremappingmodel,and(3) a measurementmodel,which wereferto asthefunctions��, � �� , and �� respectively. Therobotmotionmodelusesknowledgeof therobot’s dynamicsfor

stateprojection.Thefeaturemappingmodelusessensorobservationsto estimatethelocationof anew

geometricfeature,so that it maybeaddedto themap. Themeasurementmodelpredictsobservations

of mappedfeatures. The basicmethodassumesthat featuresare stationary, and that there is only

onerobot,but it canbeextendedto accommodatedynamicfeaturesand/ormultiple robots(Fenwick,

Newman,andLeonard2002).

A significantlimitation of Smith, Self and Cheeseman(1987) relatesto the initialization of new

features.The methodassumesthat the full stateof an objectcanbe completelyinitialized usingthe

measurementdataavailablefrom a singlevehicleposition,obtainedby a singlerobot. However, this

situation is not satisfiedin many importantcasesof practical interest. In this paper, we presenta

generalizedframework for CML that permitsmappingof partially observablefeatures.This canbe

appliedto situationswhenthe measurementdatafrom a single locationis insufficient to completely

4

estimatethefeaturelocation,suchaswith wide-beamsonarmeasurementsorangle-onlymeasurements.

It alsoenablescompositefeatures,comprisedof multiplesimplefeatures,to bemapped,suchasjoining

pointsandlines togetherto form polygons.Themethodalsohasapplicationto mappingby multiple

robots.

Theissueof estimatingpartiallyobservablefeatureswith measurementsobtainedfrom multipleun-

certainvantagepoints is clearly sharedin both the CML andSFM problems.State-of-the-artvision

algorithmsfor SFM usebundleadjustment(non-linearleastsquaresoptimization)to concurrentlyes-

timatescenestructureandcameramotion(Triggs,McLauchlan,Hartley, andFitzgibbon2000).These

techniquestypically solvetheassociationproblembyusingrandomsampleconsensus(RANSAC) (Fis-

chlerandBolles1981)or LeastMedians(Faugeras,Luong,andPapadopoulo2001)to find groupsof

measurementsthatoriginatefrom thesamepoint in thesceneacrossmultiple images.

While mostwork hasconsideredthe batchSFM problem,therehave beensomeapproachesthat

adopta recursiveapproach,which is highly importantfor navigationapplications.Chiusoet al. (2000)

have developeda real-timeSFM systemthat candealwith occlusion. In their system,tracked fea-

tures are not included into the full SFM solution until there is a high certainty that they provide

good quality data. McLauchlanhas developedthe variable statedimensionfilter (VSDF), which

is a hybrid batch/recursive techniquethat combinesthe characteristicsof the EKF and bundle ad-

justment(McLauchlan2000; McLauchlanandMurray 1996; McLauchlanandMurray 1995). The

VSDF maintainsa dynamictime-window of observationsand cameramotion parameters.We em-

ploy a similar ideain this paper. DeansandHebert(2000)have developeda relatedmethodfor CML

with bearing-onlymeasurementsandhaveperformedanexperimentalinvestigationof its performance.

More recently, Deans(2002)hasprovidedseveral improvementsto theVSDF, includinganinterpola-

tion schemethat reducesthe linearizationerror anda factorizationmethodthat yields computational

efficiency.

The structureof this paperis asfollows: Section2 formally definesthe problemunderconsidera-

tion. Previouswork is reviewedandtheproblemof mappingwith partialobservability is formulated.

Section3 describesour new approachto this problem. The key ideais to addpastvehiclepositions

to thestatevectorandto maintainexplicitly estimatesof the correlationsbetweencurrentandprevi-

ousvehiclestates.By incorporatingpastvehiclelocationsin the statevector, it becomespossibleto

consistentlyinitialize new mapfeaturesby combiningdatafrom multiplevantagepoints.

We presenttwo differenttypesof experimentalresultswith themethod.In Section4, we presenta

seriesof simplifiedexamplesthatusemanualdataassociationto demonstratetheprocessesof multi-

5

vantagepoint initialization andbatchmeasurementprocessing.Theresultsalsodemonstratemapping

of compositefeaturesandtheinitialization of anew robotpositioninto astochasticmap.In Section5,

we describethe useof the methodwithin a complete,real-timeimplementationof CML that uses

the Hough transform (Tard́os, Neira, Newman, and Leonard2001) for perceptualgrouping. The

implementationis beingdevelopedto enableautonomousunderwatervehicles(AUVs) to performCML

usingsyntheticaperturesonar(Schmidt1998).Theresultsin thispaper, however, arefor aB21mobile

robotnavigatingin typical indoorenvironments,suchasacorridor, usingodometryandPolaroidsonar

data.Finally, in Section6 weprovidea furtherdiscussionof relatedresearchanddescribeanumberof

interestingtopicsfor futureresearch.

2 Problem statement

2.1 General formulation of the problem

CML is somewhatunconventionalasa stateestimationproblemfor two reasons:(1) dataassociation

uncertainty, and (2) variabledimensionality. Initially, the numberof featuresin the environmentis

unknown andtherearenoinitial locationestimatesfor any features.Theinitial statevectoris restricted

to containonly the initial stateof therobot. As therobotmovesthroughits environment,it usesnew

sensormeasurementsto performtwo basicoperations:(1) addingnew featuresto its statevector, and

(2) updatingconcurrentlyits estimateof its own stateandthelocationsof previouslyobservedfeatures

in the environment. The robot alsohasto maintainits map,which canincorporatethe fusing of two

featuresthatarehypothesizedto bethesameobject(AyacheandFaugeras1989;ChongandKleeman

1997a)andthedeletionof featuresthatarehypothesizedto no longerbe present(Leonard,Cox, and

Durrant-Whyte1992). In this manner, the numberof elementsin the stochasticmap(andhencethe

sizeof thestatespace)variesthroughtime.

Let usassumethatthereare featuresin theenvironment,andthatthey arestatic.Thetruestateat

time � is designatedby � � � �� , where�� representsthelocationof therobot,and�� ! " " ��$# � � ��%� representsthe locationsof theenvironmentalfeatures.We assume

that the robot movesfrom time � to time �'&)( in responseto a known control input, * � � � , that is

corruptedby noise.Let +-, designatethesetof all controlinputsfrom time . throughtime � .Thesensorsontherobotproduce/ , measurementsateachstep� of discretetime. Thesetof sensor

measurementsat time � is designatedby 0 � � � , which is theset 13234 � � �65 78 ( " " / , 9 . Let 0 , designate

6

thesetof all measurementsobtainedfrom time . throughtime � . We assumethateachmeasurement

originatesfrom a single feature,or it is spurious. For eachmeasurements234 � � �;: 0 � � � , thereis a

correspondingassignmentindex <=4 . Thevalueof <=4 is > if measurement234 � � � originatesfrom feature> , andit is zeroif 234 � � � is a spuriousmeasurement.Let ?@, designatethesetof all assignmentindices

from time . throughtime � . Thecardinalityof thesets0 , and ? , arethesame.Let , designatethe

numberof featuresthathavebeenmeasuredupthroughtime � (thenumberof featuresthathaveat least

onemeasurementin ? , ).Theobjective for CML is to computerecursively theprobabilitydistribution for the locationof the

robotandthefeaturesandtheassignments,giventhemeasurementsandthecontrol inputs:A � � � � �CB ? , 5 0 , B + ,EDGF �� A � �� CB �� HBI " " HB ��$#KJ � � �HB ? , 5 0 , B + ,HDGF �H (1)

Beforeconsideringstrategiesfor computingEquation1, considerfirst themorerestrictive problem

of localizationand mappingwith prior knowledgeof all the featuresand with no dataassociation

uncertainty. With perfectknowledgeof ? � � � , onecoulddiscardtheoutliersandcombinetheremain-

ing measurementsof 0 � � � into a compositemeasurementvector 2 � � � . With prior knowledgeof the

numberof features,andprior stateestimatesfor all features,we areleft with a “conventional”,fixed-

dimensionstateestimationproblem.Thegeneralrecursivesolutionapplicablefor fully non-linearand

non-Gaussiansystemsis well-known (Bucy andSenne1971;Sorenson1988)andgivenby thefollow-

ing two equations:

A � � � � �65 0 ,EDGF B + ,EDGF �� ML A � � � � �65 � � �ONP( �HB * � �ONQ( �K� A � � � �RNP( �35 0 ,EDGF B + ,EDTS �VU � � �ONP( � (2)

and A � � � � �35 0 , B + ,EDGF �W YX , A � 2 � � �35 � � � �K� A � � � � �35 0 ,EDGF B + ,HDGF �HB � ( BCZ[B" " I (3)

where F\ J ^] A � 2 � � �35 � � � �K� A � � � � �35 0 ,EDGF B + ,HDGF �VU � � � �H Equation2 is theChapman-Komolgorov equa-

tion, andrepresentsthe useof the dynamicmodel A � � � � �65 � � �_N�( �HB * � �_N�( �K� for stateprojection.

Equation3 is Bayestheorem,where A � 2 � � �65 � � � �K� is the measurementmodel. The direct applica-

tion of Equations2 and3 entailsa computationalburdenthat grows exponentiallywith the number

of features,renderingsuchapplicationcomputationallyintractablefor typical feature-basedCML ap-

plicationsin environmentswith hundredsor more features.Recentwork in sequentialMonte Carlo

methods(Doucet,deFreitas,andGordan2001)have achievedsuccessfulperformancefor many chal-

7

lengingnonlinear, non-Gaussianstateestimationproblems;difficultiesareencountered,however, in the

applicationof sequentialMonteCarlomethodsin high-dimensionalstatespaces(MacCormick2000).

Equations2 and3 assumethat the correspondenceproblemis known. Whendataassociationun-

certainty(the correspondenceproblem)is addedto the formulation,oneis left with a hybrid (mixed

continuous/discrete)estimationproblem. Mori et al. (1986)publisheda generalrecursive non-linear,

non-Gaussianalgorithmfor stateestimationwith assignmentambiguity. Their solutiongeneralizedan

earlier linear-Gaussianmethodby Reid (1979),known asmultiple hypothesistracking (MHT). The

solutionbuilds an exponentiallygrowing treeof hypotheses,with eachleaf of the treeimplementing

a differentsolutionto Equations2 and3, basedon differenthypothesizedassignments.Probabilities

areassignedrecursively to eachdiscretehypothesis,andpruningis usedto restrictthenumberof hy-

potheses.While the Mori et al. (1986)solutioncanaccommodategeneralnon-linear, non-Gaussian

models,to our knowledgeit hasnever beenimplementedwithout simplifying assumptions.Evenwith

the linear-Gaussianassumptionsmadeby Reid’s algorithm,themethodis exponentiallycomplex due

to the combinatoricsof discretedecisionmaking. The problembearssomeresemblanceto object

recognitionin computervision (Grimson1990).

It is unclearhow to incorporatevariable-dimensionality(initializationof new featuresbasedonstate

estimatesfor the robotandotherfeaturesin themap)into theMori et al. (1986)algorithm. Hence,it

is unclearif onecanconsidertheMori et al. (1986)asthegeneralsolutionto Equation1 for theCML

problem.Ourcurrentopinionis that,becauseof theinteractionsbetweenuncertaintyandcomputational

complexity, from ageneraltheoreticalperspectiveCML is an“unsolved” problem.

2.2 Linear-Gaussian Approximate Algorithms for CML

The methodpublishedin Smith, Self, andCheeseman(1987) is a linear-Gaussianapproximationto

the generalsolutionof Equations2 and3. Nonlinearfunctionsarelinearizedvia a Taylor seriesex-

pansionandall probabilitydistributionsareapproximatedby Gaussiandistributions.Stateupdatesare

performedwith theEKF. With theseapproximations,andassumingthatdataassociationis known, the

computationalcomplexity is reducedto ` � �S � (MoutarlierandChatila1989b).

The methodrecursively computesa stateestimate a� � � 5 � �b � a�� 5 � �� a�� at eachdiscrete

time step � , where a�� 5 � �� and a�� ^ � a�� ! " I a��$# � � ��%� are the robot and featurestate

estimates,respectively. Basedon assumptionsaboutlinearizationanddataassociation,this estimateis

8

theapproximateconditionalmeanof A � � � � �35 0-, B +@,HDGF � :a� � � 5 � �Wced'� � � � �35 0 , B + ,EDGF � (4)

Associatedwith this statevectoris anestimatederrorcovariance,f � � 5 � � , which representstheerrors

in therobotandfeaturelocations,andthecross-correlationsbetweenthesestates:

f � � 5 � �W hg fi�� 5 � � fi�j� � � 5 � �f8�k� � � 5 � � f8�C� � � 5 � ��l mnnno fi�� 5 � � fi�j�� 5 � � �6�6� fi�j� # � � 5 � �f!��p� � � 5 � � f8�j�q�� 5 � � �6�6� f!��pr � � 5 � �...

.... . .

...f8�s#H� � � 5 � � f8�$#E�� 5 � �t�6�6� f!�$#E�$# � � 5 � �uwvvvx (5)

Themethodusesthreemodels,a plantmodel��

, a measurementmodel �j�w� , anda featureinitial-

izationmodel � �� . Thispaperfocuseson � �j�w� , presentingageneralizedmodelfor featureinitialization

from multiple uncertainvantagepoints.Theplantmodel��j�w�

is usedto makepredictionsof futureve-

hicle positionsbasedon a controlinput. Theobservationmodel, �� , definesthenonlinearcoordinate

transformationfrom stateto observationcoordinates.For a moregeneraldiscussionof thesemodels,

seeFederandLeonard(1999)or oneof the other referenceson feature-basedCML listed above in

Section1. Beforeconsideringtheproblemof featureinitialization in moredetail,we know provide a

discussionof thedataassociationproblemfor CML.

2.3 Data Association

To usethemodels �j�w� and � �� properly, stochasticmappingalgorithmsmustmakedecisionsaboutthe

originsof measurements.Spuriousmeasurementsmustbe ignored,however it is oftenunclearwhich

measurementsarespurious.Measurementsthat aredeterminedto originatefrom previously mapped

featuresareusedvia ��w� to performa stateestimatedupdate.Measurementsthat aredeterminedto

originatefrom anew featureareusedwith � ��w� to addthefeatureto themap.

While thereis no mentionof thedataassociationproblemin Smith,Self andCheeseman(1987),it

is a crucialaspectof theCML problem.Theoptionsfor dataassociationareratherlimited. Powerful

toolsexist, suchasMHT (Reid1979)or probabilisticdataassociationfilter (PDAF) (Bar-Shalomand

Fortmann1988),but thecomputationalburdenof theseapproachesis veryhighwhenthesetechniques

areappliedto CML. Theusualalternative is to employ “nearest-neighbor”gatingtechniques.For each

featurein the statevector, predictedrangeandanglemeasurementsaregeneratedandarecompared

againstthe actualmeasurementsusinga weightedstatisticaldistancein measurementspace.For all

9

measurements234 � � � thatcanpotentiallybeassociatedwith feature y��$z � � � , theinnovation, {}|~4 � � � , and

the innovationcovariance,��|~4 � � � , areconstructedandtheclosestmeasurementwithin the “gate” de-

finedby theMahalanobisdistance {}|~4 � � � � ��|~4 � � � DGF {}|�4 � � ��Q�}B (6)

is consideredthemostlikely measurementof that feature(Bar-ShalomandFortmann1988). Suchan

approachwill fail if thefeaturesin theenvironmentaretoo closeto oneanother.

In addition,simply testingtheproximity of observationsto predictedmeasurementsfor previously

mappedfeaturesprovidesno indicationof whena measurementcomesfrom a new feature. Feature

initialization is typically basedon looking for severalconsecutive unexplainedmeasurementsthatare

closeto oneanother, andfar from any previouslymatchedfeatures.Thispolicy is referredto asdelayed

track initiation (Leonardand Durrant-Whyte1992; Feder, Leonard,and Smith 1999; Dissanayake,

Newman,Durrant-Whyte,Clark,andCsorba2001).In general,thereis a tradeoff betweenbeingmore

likely to assignameasurementto anold feature,vs. usingit to initialize anew feature.If oneis is able

to performfeaturefusion(ChongandKleeman1997a),thenit is probablybetterto err on thesideof

new featurecreation.This is thestrategy employedin ourexperimentsin Section5.

A varietyof methodsfor attackingthecorrespondenceproblemhavebeendevelopedin vision,such

asRANSAC (FischlerandBolles 1981). The generalideais to usetechniquesfrom robust statistics

to find setsof measurementsthat collectively reinforceoneanotherandyield a single,consistentin-

terpretation.Recently, Neiraet al.(2001)have presenteda joint compatibility testingmethodfor data

associationthatexploits correlationinformationwhenconsideringpotentialassignmentsfor groupsof

measurements.Themethodsucceedsin ambiguoussituationswhenstandardnearestneighborgating

fails. Thegeneralpolicy of looking for consensusamongmultiplemeasurementsto resolveambiguity

is similar in spirit to RANSAC. Otherdataassociationstrategiesspecificto sonarhavebeenproposed;

for example,Wijk andChristensen(2000)have recentlydevelopeda techniquecalledTriangulation-

BasedFusion(TBF) thatprovidesexcellentperformancefor detectionof point featuresfrom ring sonar

data. TheTBF methodlooks for setsof sonarreturnsobtainedfrom adjacentpositionsthatcouldall

have originatedfrom thesamepoint object,by efficiently computingcircle intersectionpointsandap-

plying angleconstraints.Themethodrunsin real-timeandhasbeensuccessfullyusedfor occupancy

grid mapping,model-basedlocalization,andrelocation(Wijk andChristensen2000). CML hasalso

beenimplementedusingtheTBF for pointsfeatures(ZuninoandChristensen2001).

In this paper, we usemanualdataassociationin Section4 to illustratevariousnew typesof feature

10

initialization,andweuseaHoughtransformvotingtechnique(fully documentedin Tard́osetal. (2001))

to performinitialization of new point andline featureswhenperformingreal-timeCML in Section5.

2.4 Feature Initialization in Smith, Self, and Cheeseman

The Smith, Self andCheesemanalgorithm addsnew featuresto the map using the linear-Gaussian

approximationin thefollowing manner. Themethodassumesthatthestateof thenew feature, y�� #k� � � � �canbecomputedusingthemeasurementdataavailablefrom a singlevehicleposition,usinga feature

initialization function � �� : y�� #K� � � � �� y� � � 5 � �CB 234 � � �V�H (7)

For example,for a sensorproviding rangeand bearingmeasurements,234 � � �_ �~��, the feature

initialization functionfor apoint � �� takesthefollowing form:y�� #K� � � � �� y� � � 5 � �HB 234 � � �K�� g�� & ��E��3�s� & �� & ��V��s� & ��"l (8)

Thenew featureis integratedinto themapby expandingthestatevector y� � � 5 � � andcovariancef � � 5 � �asshown below: y� � � 5 � �� g y� � � 5 � �y�� #k� � � � ��l B (9)

f � � 5 � �W� mo fi�� 5 � � f��j� � � 5 � � f��j�s#k� � � � 5 � �f8�k� � � 5 � � f!�C� � � 5 � � f!�C�$#k� � � � 5 � �f!�$#k� � � � � 5 � � f8�$#K� � � � � 5 � � f!�$#K� � �$#k� � � � 5 � �ux B (10)

where f8�s#k� � �$#k� � � � 5 � �W �� f � � 5 � �k� � � & �'�C� � � �k� �� B (11)¡ f8�s#k� � � � � 5 � � f!�$#k� � � � � 5 � ��¢ g f!�$#k� � � � � 5 � �f8�s#k� � � � � 5 � � l � �� f � � 5 � �CB (12)��is theJacobianof � �j�w� with respectto thestatevector, and

�'�is theJacobianof � �j�w� with respect

to themeasurement.

3 Mapping Partially Observable Features using an Extended Representation

As mentionedabove,themethodof Smith,Self andCheeseman(1987)assumesthatthereis sufficient

informationin thesetof measurementsavailablefrom a singlerobotpositionto completelyandcon-

sistentlyinitialize a new featureinto themap.To enableCML in situationswherethis is not thecase,

11

we addpastvehiclepositionsto the statevectorandmaintainexplicitly estimatesof the correlations

betweencurrentandpreviousvehiclestates.By incorporatingpastvehiclelocationsin thestatevector,

it becomespossibleto makeimprovedprobabilisticdataassociationandfeatureclassificationdecisions

andto initialize new mapfeaturesby consistentlycombiningdatafrom multiplevantagepoints.

The motivation for the new approachis the following: if the sensorobservationsavailablefrom a

single time stepdo not provide sufficient information to initialize the stateestimateof a newly de-

tectedfeature,theninformationfrom multiple vehiclepositionsmustbeused.To maintainconsistent

errorbounds,correlationsbetweendifferentvehiclelocationsmustbetakeninto accountby theCML

algorithm. Further, decisionsthat aredifficult basedon the datafrom a singleposition(suchasthe

dispositionof an individual sonarreturn)canbemademucheasierwhenconsideredasdelayeddeci-

sions,usingdatafrom multiple vehiclepositions.Measurementscanalsobeappliedasynchronously,

in batchesof datafrom sequencesof positions.

To achieve thesecapabilities,we expandthe representationto add a numberof previous vehicle

locationsto the statevector. We refer to pastvehicle statesthat are part of the stochasticmap as

“trajectorystates”.We introducethenotation ��£ J (equivalentto �� ) to refer to thetruestate(pose)

of therobotat time � ¥¤.

Usingtrajectorystates,theCML problemembodiedby Equation1 is restatedin expandedform as

therecursivecomputationof:A � � � � �HB ? , 5 0 , B + ,EDGF �¦ A � ��£�§ B ��£ � B" I " EB ��£ J�¨ � B �� HB �� HB" I " EB �� # J � � �HB ? , 5 0 , B + ,EDGF �H (13)

Expandingtherepresentationin thismannerprovidesanew generalframework for feature-basedCML.

While theapproachis generallyapplicableusingany stateestimationframework, in this paperwe

describetheimplementationof theapproachin thecontext of stochasticmapping,yieldingamethodwe

referto as“delayedstochasticmapping”.Thenew methodis summarizedin Figure3. Thenew com-

ponentsof theframework includetrajectorystatemanagement,perceptualgrouping,multiple vantage

point initialization,andbatchupdating.

Eachtime thevehiclemoves,thepreviousvehiclelocationis addedto thestatevector. We introduce

thenotation a��£�z � � � (equivalentto a�� > 5 � � ) to refer to theestimateof thestate(position)of therobotat

time > givenall informationup to time � . Thecompletetrajectoryof therobotfor time step . through

timestep ��N�( is givenby thevector a��£ � � �� )� a��£�§ � � � � a��£ � � � � � a��£%© � � � � " " a��£ Jj¨ � � � �j� � . Thecomplete

12

1: while activemissiondo2: y� � � 5 �ONP( � =

�=� y� � �RNP( 5 �ONQ( �CB * � � �K� 1 stateprojection9

3: f Mª«� f ª¬�� &P^1 covarianceprojection9

4: y2 � � � = � y� � � 5 �ONP( �K� 1 sensorprediction9

5:� < BC® < �¯� ( 2 � � � , y2 � � � ) 1 dataassociation

96: � M°;� f ° � � & � 1 innovationcovariance

97: ± f °²� � �}DGF¯1 Kalmangain

98: y� � � 5 � �� y� � � 5 �ONP( � &³± � 2�´µNMy2�´ � 1 Kalmanstateupdate

99: f fMN¶± ��± � 1 Kalmancovarianceupdate

910: y� � � 5 � �� g y� � � 5 � �� y� � � 5 � �HB 2�·6´ ��l 1 mappingstate

911: f � g f f � � ��'� f �'� f �'�� & �'�E��¸�� l 1 mappingcovariance

912: � = ��&¥(13: end while

1: while activemissiondo

2: y� � � 5 �ONP( �W� g �� y�� RN¹( 5 �ONP( �CB * � � �K�y� � �RNQ( 5 �ONP( � l 1 stateaugmentationwith projection9

3: f � g ª«� f ª¬�� &P ª«� ff ª«� � f l 1 covarianceaugmentationwith projection9

4: y2 � � � = � y� � � 5 �ONP( �K� 1 sensorprediction9

5:� < BC® < �¯� ( 2 � � � , y2 � � � ) 1 dataassociation

96: � M°;� f ° � � & � 1 innovationcovariance

97: ± f °²� � �}DGF¯1 Kalmangain

98: y� � � 5 � �� y� � � 5 �ONP( � &³± � 2�´µN y2�´ � 1 Kalmanstateupdate

99: f fMN¶± ��± � 1 Kalmancovarianceupdate

910: y� � � 5 � �� g y� � � 5 � �� y� � � 5 � �CB 2�·3´ ��l 1 mappingstate

911: f � g f f �'��'� f �'� f �'�� & �'�E��¸�� l 1 mappingcovariance

912: Contractthestate� andcovariancef to removeunecessarytrajectorystatesandassoci-

atedtermsin thecovariancematrix13: � = ��&¥(14: end while

Figure3: Comparisonof conventionalstochasticmapping(top) anddelayedstochasticmapping(bottom). Thenotationis summarizedasfollows: º is thestatevector, » is thecovariance,¼µ½k¾ , and ¿ aretheJacobiansoftheir respective nonlinearfunctions,À and Á arethepropagationandmeasurementcovariancematrices,Â is thecontrolinput, Ã aretheobservations,Ä labelsassociatedobservations, Å�Ä labelsunmatchedobservations.

13

statevectoris:

a� � � 5 � �� mo a�� 5 � �a��£ � � �a�� ux

mnnnnnnnnnnnnnnnnnnno

a�� 5 � �a��£ § � � �a��£�� a��£ © � � �...a��£ Jj¨ � � � �a�� a��© � � �a��$Æ � � �...a�� # ¨ � � � �a��$# � � �

u vvvvvvvvvvvvvvvvvvvx

(14)

Theassociatedcovariancematrix is:

f � � 5 � �W mo fi�� 5 � � fi��£ � � 5 � � f�� 5 � �fi£w� � � 5 � � fi£w£ � � 5 � � f�£%� � � 5 � �f!�Ç� � � 5 � � f8�k£ � � 5 � � f!�C� � � 5 � �ux B (15)

or equivalently,

ÈÊÉÌË�Í Ë3ÎÐÏÑÒÒÒÒÒÒÒÒÒÒÓÈWÔ$ÔCÉÌËTÍ Ë3Î ÈWÔsÕ�ÖCÉÌË�Í Ë3Î ×V×�× ÈWÔqÕ%Ø�ÙIÚÇÉÌËTÍ Ë3Î ÈWÔ$Û Ú É�Ë�Í Ë�Î ×�×V× ÈWÔ�ÛÝÜ=ÉÌËTÍ Ë3ÎÈ¯Õ�ÖsÔEÉÌË�Í Ë3Î È¯Õ�ÖsÕ�ÖHÉ�Ë�Í Ë3Î ×V×�× È¯Õ�ÖsÕ%Ø�ÙIÚkÉ�Ë�Í Ë3Î È¯Õ�Ö�Û Ú ÉÌË�Í Ë3Î ×�×V× È¯Õ�Ö$ÛqÜ=ÉÌË�Í Ë3Î

......

. .....

.... . .

...È Õ Ø�ÙIÚ Ô É�Ë�Í Ë�ÎÞÈ Õ Ø�ÙIÚ Õ ÖCÉÌËTÍ Ë3Îß×V×�×¥È Õ Ø�ÙIÚ Õ Ø�ÙIÚÇÉ�Ë�Í Ë�ÎàÈ Õ Ø�ÙIÚ ÛsÚ É�Ë�Í Ë3Îß×�×V×¥È Õ Ø�ÙIÚ Û Ü=É�Ë�Í Ë�ÎÈ ÛsÚqÔ É�Ë�Í Ë�Î È ÛsÚÝÕ ÖCÉÌËTÍ Ë3Î ×V×�× È Û$ÚpÕ Ø�ÙIÚÇÉ�Ë�Í Ë�Î È ÛsÚsÛsÚ É�Ë�Í Ë3Î ×�×V× È ÛsÚsÛ Ü=É�Ë�Í Ë�Î...

.... ..

......

. . ....È Û Ü Ô É�Ë�Í Ë�Î È Û Ü Õ ÖCÉÌË�Í Ë3Î ×V×�× È Û Ü Õ Ø�ÙIÚÇÉÌËTÍ Ë3Î È Û Ü Û$Ú É�Ë�Í Ë�Î ×�×V× È Û Ü Û Ü�É�Ë�Í Ë�Î

áãââââââââââä × (16)

New trajectorystates a��£ J � � �O a�� 5 � � aregeneratedat eachtime stepandareaddedto the state

vector:

y� � � 5 � ��mnnnnnnnnnnnoa�� 5 � �a��£ § � � �a��£ � � � �a��£%© � � �

...a��£ Jj¨ � � � �a��£ J � � �a��

uwvvvvvvvvvvvx

(17)

14

Thestatecovarianceis expandedasfollows:

ÈÊÉÌË�Í Ë3Î�å ÑÒÒÒÒÒÒÒÓ È ÔsÔ ÉÌË�Í Ë3Î È ÔqÕ ÖCÉ�Ë�Í Ë3Î ×�×V× È ÔsÕ Ø�ÙIÚCÉÌË�Í Ë3Î È ÔsÕ Ø6É�Ë�Í Ë�Î È Ô�Û É�Ë�Í Ë�ÎÈ Õ Ö Ô É�Ë�Í Ë�Î È Õ Ö Õ ÖHÉÌËTÍ Ë3Î ×�×V× È Õ Ö Õ Ø�ÙIÚkÉÌËTÍ Ë3Î È Õ Ö Õ Ø"ÉÌË�Í Ë3Î È Õ Ö Û ÉÌË�Í Ë3Î...

.... . .

......

...È Õ Ø�ÙIÚ Ô ÉÌË�Í Ë3ÎÞÈ Õ Ø�ÙIÚ Õ ÖEÉÌË�Í Ë3Îß×�×V×¥È Õ Ø�ÙIÚ Õ Ø�ÙIÚCÉÌË�Í Ë3ÎàÈ Õ Ø�ÙIÚ Õ Ø6É�Ë�Í Ë3ÎàÈ Õ Ø�ÙIÚ Û É�Ë�Í Ë3ÎÈ Õ Ø Ô ÉÌË�Í Ë3Î È Õ Ø Õ ÖCÉ�Ë�Í Ë�Î ×�×V× È Õ Ø Õ Ø�ÙIÚkÉ�Ë�Í Ë3Î È Õ Ø Õ Ø6ÉÌËTÍ Ë3Î È Õ Ø Û ÉÌËTÍ Ë3ÎÈ ÛKÔ É�Ë�Í Ë�Î È ÛVÕ ÖHÉÌËTÍ Ë3Î ×�×V× È Û�Õ Ø�ÙIÚÇÉ�Ë�Í Ë�Î È ÛVÕ Ø6ÉÌË�Í Ë3Î È ÛkÛ ÉÌË�Í Ë3Îá âââââââäiæ (18)

wherefi£ J £�z � � 5 � �� fi��£�z � � 5 � � , f�£ J � � � 5 � �W fi�j� � � 5 � � , and f�£ J £ J � � 5 � �W f�� 5 � � .Thegrowthof thestatevectorin thismannerincreasesthecomputationalburdenas ` � S � , socaution

mustbetaken. Thenew problemof trajectorystatemanagementis introduced.Old vehicletrajectory

statesandassociatedtermsin thecovarianceneedto bedeletedonceall themeasurementsfrom agiven

time stephavebeeneitherprocessedor discarded.In practice,we have seenexcellentperformanceby

keepingthenumberof trajectorystatesrestrictedto a fixedsizeof 40. With a fixedwindow size,the

processof addingpaststatesis similar to afixed-lagKalmansmoother(AndersonandMoore1979).

3.1 Perceptual Grouping

Thenext stepin theframework is to applyaperceptualgroupingalgorithmto examinecollectively the

entiresetof datathat camefrom the currentandpastvehiclepositionscurrentlyin the map. Instead

of beingforcedto make aninstantaneousdecisionabouttheoriginsof currentmeasurements,delayed

decisionmaking is now possible. In general,a wide variety of perceptualgroupingalgorithmsare

possible,suchasthe RANSAC (FischlerandBolles 1981)andLeastMedian(Faugeras,Luong,and

Papadopoulo2001)methodsthathave beensuccessfullyemployed in vision. The subjectof delayed

decisionmakingis very broadin scope,andin this paperwe focuson thestateestimationaspectsof

theproblemonly. For now, we assumethatsomeperceptualgroupingalgorithmhasbeenemployedto

classifyandassociatemeasurements.Furtherdiscussionof perceptualgroupingis containedbelow in

Section5.

3.2 Feature initialization using data from multiple vantage points

Givendecisionsabouttheoriginsof measurementsfrom multiplevantagepoints,thenext stepis to use

the trajectorystatesandassociatedcorrelationinformation to initialize new featuresusingmeasure-

mentsfrom multiple vantagepoints.Supposethata perceptualgroupingmethodhasproduceda setof

15

z[k] z[k-1]

x r [k-1|k]

P rr [k-1|k]

P rr [k|k]

Stored, Uncertain Vehicle Trajectory, T

New feature initialised by combining observations from ç

multiple uncertain vantage points

Stored Observations Z

x èr [k|k]

x f [k|k]=g(Z,T)

P ff [k-1|k]

Figure4: Illustrationof initialization from multiple vantagepoints.

candidatemeasurements,0¦é , from a setof vehiclepositionsthatarecurrentlycontainedin thesetof

activetrajectorystatesof thestochasticmap.Initializationcanthenbeperformedby pickingaminimal

subset0�ê of “seed” featuresfrom 0¦é thataresufficient theestimatethestateof thenew feature.Letë ê be thesetof vehiclepositionsfor themeasurementsin 0�ê . Then,the locationof thenew feature

canbecomputedas: a��s#k� � � � ë ê B 0�ê � (19)

For example,considerthe initialization of a new point featurein two dimensions,usingtwo range

measurements,� F and

� S , takenat time steps� F and � S . Thefunction � �j�w� representsa solutionfor the

intersectionof two circles.Thealgebrais verysimpleandis reviewedin AppendixA. Thebeamwidth

of the sonarsensorcan be usedas an angleconstraintto rule out multiple solutions(Leonardand

Durrant-Whyte1992;Wijk andChristensen2000).Thecovariancefor thenew featureis initialized in

asimilar fashionasshown abovein Equations10to 12,exceptthattheJacobianmatrix�'�

will contain

additionalnon-zerotermscorrespondingto thetrajectorystatesandtheJacobianmatrix�'�

. Thedirect

differentiationof themultiple vantagepoint initialization functioncanbecumbersome.It is possible,

however, to computetheinitializationJacobiansby invertingtheJacobianof thecompositefunctionof

minimalobservations,asfollows: ��µ ¥° DGFì B(20)

16

and ��! N ° DGFì °;�íB (21)

where î �� #k� � is the stateestimatefor the new feature. This is possiblebecause° ì is a square

matrixwith full rank.

An alternative is to computethe Jacobiansnumerically(Julier, J. K. UhlmannandH. F. Durrant-

Whyte1996);this is theapproachusedfor theresultsin this paper. For initialization of a line feature

from two positionswith two sonarmeasurements,the function � �� representsthe solution for the

commontangentsof two circles.Thegeneralprocedureis thesameif thefeatureinitialization function� �j�w� is a functionof measurementsfrom morethantwo time steps,for exampleto initialize a cylinder

usingthreerangevalues.

3.3 Batch Updating

Oncea new featureis initialized, the mapcanbe simultaneouslyupdatedusingall otherpreviously

obtainedmeasurementsthat canbe associatedwith the new feature(measurementsthat areelements

of 0�é but not elementsof 0¦ê .) This updateis performedusing a compositemeasurementvector,

consistingof measurementsobtainedfor differenttimesteps.We call this procedurea “batchupdate”.

It allowsthemaximumamountof informationto beextractedfrom all pastmeasurements.Empirically,

wehaveobservedthattheEKF canbelessproneto divergencewhenbatchupdatesareperformed.

To updatetherobothistoryandfeaturesusingabatchof measurements,ameasurementvector 2=ï is

constructedfrom anappropriatetemporalobservationset.

2�ð mnnnnno2 � �ON¶ñHò �2 � �ON¶ñ F �2 � �ON¶ñ S �...2 � �ON�ñHr �

uwvvvvvx (22)

An appropriatepredictedmeasurementvectoris constructed:

a2�ð mnnnnno � a�� ON�ñHò �HB a�� K� � a�� ON�ñ F �HB a�� K� � a�� ON�ñ S �HB a�� K�... � a�� RN¶ñHr �HB a�� V�

uwvvvvvx (23)

17

This leadsto aninnovationwhich includesobservationsfrom acrossmultiple timesteps:

ó mnnnnno2 � �ON�ñEò � N� � a�� ON�ñHò �HB a�� K�2 � �ON�ñ F � N� � a�� ON�ñ F �HB a�� K�2 � �ON�ñ S � N� � a�� ON�ñ S �HB a�� K�...2 � �RN�ñCr � N� � a�� ON�ñCr �CB a�� K�

uwvvvvvx (24)

Similarly, themeasurementJacobian°²ô

is constructed,asis themeasurementcovariance�

:

°;�� mnnnnno°;�"õKö ,EDT÷ §�øpù �"úEö , ø° � õ ö ,EDT÷ � øpù � ú ö , ø° � õ ö ,EDT÷ ©�øpù � ú ö , ø...°²�IõKö ,HDT÷ # ø�ù �6úEö , ø

u vvvvvx (25)

�û mnnnnno� � �RN�ñHò � . . I " .. � � �ON�ñ F � . I " .. . � � �ON�ñ S �ü I " .

......

.... . .

.... . . I " �� ON¶ñCr �uwvvvvvx (26)

Using thesecomponentsandthe standardKalmanupdateequations,all robot trajectorypositions

andall featuresthatarecurrentlyin mapareconcurrentlyupdated.

3.4 Composite Initialization

In general,thefeatureinitializationfunctioncanuseanyof theinformationin thestateestimationa� for

thestochasticmap,includingthelocationsof otherpreviously mappedfeaturesaswell asasprevious

vehiclestates.Thenew featurelocationcanbea functionof oneor morepreviously mappingfeature,a��$z , oneor moremeasurements,andtherobotstateestimatecorrespondingto thesemeasurements.For

example,onecaninitialize a line that passesthrougha point feature a��$z andis tangentto onesonar

return 234 � � � . In this case,thefeatureinitialize functionis of theform:a��$#k� � � � a��$z B a�� CB 234 � � �V� (27)

Alternatively, onecaninitializationapoint thatliesat theintersectionof a line currentlyin themapand

a new sonarreturn.Theequationsfor thesetwo initialization scenariosaredescribedin Appendix1.3.

18

Onecanalsoinitialize a new featurewithout any measurements,for example,hypothesizingthecon-

straint that a new point featureexists at the intersectionof two line segmentscurrently in the map.

Exampleswith realdatafor severalof thesescenariosaregivenbelow in Section4.

3.5 Extension to Mapping by Multiple Robots

Cooperative stochasticmappingwith perfectcommunicationsrequiresaddingrobotsto thestatevec-

tor (Fenwick,Newman,andLeonard2002).Althoughtherearenumeroustechnicalissuesto overcome,

from astateestimationperspectiveit is relatively straightforwardto performfeatureinitializationusing

datafrom multiple robots.By usingtrajectorystates,suchinitializationscanoccuron adelayedbasis,

removing the requirementfor the two robotsto sensethe featuresin questionat preciselythe same

time. In is alsopossiblefor onerobot to “map” the locationof anotherrobot (addtheotherrobot to

its statevector),throughprocessingof measurementsof commonfeatures(assumingthatassociation

is known). A simplifiedexampleof how this processcanwork is shown below in Section4.

4 Examples using manual association

We now presentseveral illustrationsof the conceptspresentedabove usingreal sonardatasetswith

manualdataassociation.Thefirst experimentuses500kHz underwatersonardataacquiredin a test-

ing tank, andthe secondusesexperimentusesdatafrom a ring of 24 Polaroidsonarsensors.Both

experimentsusemanually-guideddataassociationstrategies that exploit a priori knowledgeof the

environment. While both environmentsarehighly simplified, they areuseful in illustrating the state

estimationprocessfor mappingfrom multiple uncertainvantagepoints.Fully automaticdataassocia-

tion is usedin theexperimentsin Section5, aspartof an integratedsystemthatcanperformCML in

real-timeusingodometryandwide-beamsonarmeasurements.

4.1 Tank Experiment

An experimentwasconductedusingaroboticgantryto emulatethemotionandsensingof anunderwa-

ter vehicle. A 500KHz binauralsonarwasused(Kuc 1996;Au 1993). To show mappingof partially

observablefeatures,bearinginformationwasdiscarded.Two objectswereplacedin thetank,a metal

triangleandapoint likeobject(a fishingbobber).Thegantrywasmovedthroughtwo trajectories,one

19

to the left andoneto the right of the objects,emulatingcooperative CML by two vehicles. All pro-

cessingwaspostprocessing.Dataassociationwasdoneby handsinceit is not thefocusof this paper.

Themanually-associatedreturnsusedfor featureinitialization arelabeledin Figures7 and 10 andare

listed in Table1. The initialization strategiesusedfor eachfeatureandfor thepositionof thesecond

robotarelisted in Table2. We considerthesetof measurementsfrom theright sideof theobjectsto

originatefrom “robot 1” andthemeasurementsfrom theleft sideto befrom “robot 2”.

Thegantryoperatesin atankthatis 10meterlongby 3 meterwideby 1 meterdeep.Themechanism

providesground-truthgoodto a few millimeters. Simulatedspeedandheadingmeasurementswere

generatedandusedfor dead-reckoning. Initially, thesensordead-reckonedthrougha trajectoryof 11

positions,as shown in Figure 5. Upon completingthis trajectory, the robot had a statevector and

covariancematrix which containedonly robot states,one estimatefor eachposition. In Figure 6,

the correlationcoefficients for the x componentsof the trajectoryare plotted. Eachline represents

the correlationsbetweenone timestepand all other saved timesteps. Becausethis is a short dead-

reckonedtrajectory, eachcurve hasonly onemaxima;morecomplex trajectoriesmayhave numerous

local maxima.

Theassumedrangemeasurementstandarddeviation was3 centimetersfor eachmeasurement.The

addedprocessnoisehada standarddeviation of 1 cm per time stepin � and � and2 degreesper time

stepin heading.

Thedataprocessingwasperformedasfollows. First, stateprojectionwasperformedandtrajectory

stateswerecreatedfor robot1 for timesteps1 through11,withoutany measurementsbeingprocessed.

Thethree-sigmaerrorboundsfor thedead-reckoned� � B � � trajectoryof robot1 areshown in Figure5.

Thecorrelationcoefficientsbetweenthe � coordinatesof therobot trajectorystatesareplottedin Fig-

ure6.

Having anentiretrajectoryof positions,robot1 startsto constructits map. Becausetherobotuses

a rangemeasurementsonly, featuresmustbeobservedfrom multiplevantagepointsto bemapped.By

combiningreturns1 and5 (labeledin Figure7), therobot initializes thepoint objectat thebottomof

its map(Figure7). Similarly, by intersectingtwo morearcs(returns2 and6), thebottomcornerof the

triangleis mapped.Theequationsfor arcintersectionaregivenin Appendix1.1.Next, return4 is used

in conjunctionwith theestimatedlocationof thebottomcornerof thetriangleto addtheright wall of

the triangleto themap. Finally, by intersectingreturn3 with the estimatedline correspondingto the

right wall, the top corneris addedto themap. Using theseobservations,thepoint objectandtheside

of thetriangleareinitialized (Figure8).

20

Thewall is representedin mapby aninfinite line with two parameters,ý and�, which aretheangle

and offset of the normal with respectto the origin. The dashedline in Figures7 and 8 show the

extensionof theestimatedline.

After initializing thefeatures,all otherobservationsareusedfor a batchupdateof thenewly initial-

izedfeatures(Figure9). Mappingandnavigationareimprovedsubstantially. Robot1 obtained29total

measurements.Of these,six wereusedfor initializationand23wereusedbatchupdating.

Next, robot1 triesto useinformationfrom robot2. We assumethatthereis no a priori information

for theinitial locationof robot2, andthathencerobot2 mustbeinitialized into themapusingshared

measurementsof featuresseenby bothrobots.

It is determinedthat robot 2 hasobserved featuresin robot 1’s map. From the rangesto the point

objectandthe bottomcornerof the triangle,(returns7 through10 aslabeledin Figure10) robot 2’s

first two positionscanbeobserved.Thosetwo positionsareinitialized into themap,their initialization

function is of the form � � �� õ � B 2�� S � , meaningthat robot 2 is addedto robot 1’s mapusingrobot 2’s

observationsof featuresthathavebeenpreviouslymappedby robot1 (Figure11).

Next, usingthefirst two estimatedpositionsfor robot2, theinitial headingandvelocityof robot2 are

mapped.Usingthis information,alongwith thecontrolinputs,adead-reckonedtrajectoryfor robot2 is

established(Figure12). Becauseneithercompassnor velocity observationsareavailable,andbecause

the initial estimatesof velocity andheadingfor robot 2 areimprecise,the trajectoryis impreciseand

haslargeerrorbounds.

Having a dead-reckonedtrajectoryfor robot2 andits measurements,robot1 thenmapsthe(other-

wiseunobservable)backsideof the triangleandperformsa batchupdateto getan improvedmapand

animprovedestimateof whererobot2 traveled(Figure13) . Robot2 obtained22 totalmeasurements.

Of these,four measurementswereusedto initialize the positionof robot 2 in the stochasticmapof

robot 1. Two measurementswereusedin initializing featureson the left sideof the triangle,andthe

remaining16 returnswereusedin batchupdating.

Theerrorboundsfor robot2 donotexhibit thegrowth profile thatis normallyseen.Normally, since

the robot startswith an initial positionestimateandthenmoves,theuncertaintygrows with time. In

this case,sincethesecondrobot is mappedandlocalizedwith respectto previously mappedfeatures,

thesmoothedestimateof its trajectoryhastheleastuncertaintyin themiddle(Figure14).

Thenext experimentis for datafrom a simple“box” environmentmadeof plywood,demonstrating

theprocessof multiplevantagepoint initialization,batchupdating,andcompositefeatureinitialization.

Thedataassociationandfeaturemodelingtechniquesutilize thea priori knowledgeof thestructureof

21

5þ

4ÿ

3�

2�

1 0�

1 1

0�

. 5þ

0�

0�

. 5þ

1

1. 5þ

2

2. 5þ

3�

3�

. 5þ

4

x� , in meter s�

y, in

met

er s

Figure5: Dead-reckonedtrajectoryof robot1. Therobotstartedat thebottomandmovedupwards.Theellipsesrepresentthe99%confidenceinterval for eachposition.Thetriangleis analuminumsonartarget,andthesmallfilled circleat theposition � =-1.0and � =0.0is afishingbobber, whichservedasa “point” object.

Table1: Detailsfor thetenmanually-selectedreturnsusedfor featureinitialization.Returnnumber Robot TimeStep OdometryX (m) OdometryY (m) Range(m)

1 1 1 0.0 0.0 1.00362 1 1 0.0 0.0 1.80583 1 3 .0045 .4992 1.83804 1 8 -.0399 1.7637 .95045 1 11 -.0235 2.537 2.72056 1 11 .0235 2.5373 1.41927 2 1 (unused) (unused) .97738 2 1 (unused) (unused) 1.78519 2 2 (unused) (unused) 1.005410 2 2 (unused) (unused) 1.5692

22

1 2 3 4 5 6 7 8 9 100.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

k

ρ x(j)x

(k)

ρ1,k

ρ2,k

ρ3,k

ρ4,k

ρ5,k

ρ6,k

ρ7,k

ρ8,k

ρ9,k

ρ10,k

Figure6: Correlationcoefficientsbetweenthe x componentsof robot 1’s trajectory. Eachline representsthecorrelationsbetweenaspecifictimestepandall othertimesteps.

Figure7: Setof observationsusedto initialize the sideof the triangleandthe point object. Two observationsareneededto initialize thepoint target, two moreareneededto initialize thecornerof the triangle. Given theconstraintof the corner, only onemeasurementwasneededto mapthe wall. Given the wall, only onemoremeasurementwasneededto mapthetopcornerof thetriangle.

Table2: Methodof initialization for features.

Feature InitializationMethod

Pointobject Return1 andReturn5Lower right vertex of triangle Return2 andReturn6

Rightplaneof triangle Lower right vertex of triangleandReturn4Upperright vertex of triangle Right planeof triangleandReturn3

Position1 for robot2 PointobjectandReturns7 and8Position2 for robot2 PointobjectandReturns9 and10

Table3: Comparisonof hand-measuredandestimatedfeaturelocationsfor pointsfeatures(in meters).

Handmeasured CML estimatedFeature x y x y

Pointobject -1.0 0.0 -1.0054 .0109Lower right vertex of triangle -1.0 1.5 -.99 1.5045Upperright vertex of triangle -1.0 2.05 -.9922 2.0837

Left vertex of triangle -1.4763 1.77 -1.4908 1.7846

23

−5 −4 −3 −2 −1 0 1−1

−0.5

0

0.5

1

1.5

2

2.5

3

3.5

4

x, in meters

y, in

met

ers

Figure8: Initial map.99%confidenceintervals for thecornersandthepoint objectareshown.

−5 −4 −3 −2 −1 0 1−1

−0.5

0

0.5

1

1.5

2

2.5

3

3.5

4

x, in meters

y, in

met

ers

experiment 4: complex initializations of partially observable objects

Figure9: Mapafterabatchupdate.Notetheimprovedconfidenceintervals for thefeaturesandtherobot.

24

5 4�

3�

2

1 0 11

0. 5

0

0.5

1

1.5

2

2.5

3�3

�.5

4

x , in meters�

y, in

met

ers

7

8

9

10

Figure10: Adding in thesecondrobot.Thetruetrajectoryfor robot2 is theredline on theleft. Observationsofthebottomcornerof thetriangleandof thefishingbobberarereversedto find thefirst two vantagepoints.

−5 −4 −3 −2 −1 0 1−1

−0.5

0

0.5

1

1.5

2

2.5

3

3.5

4

x, in meters

y, in

met

ers

Figure11: First two positionfor robot 2 aremapped.Using thesetwo positionsis is possibleto estimatetheinitial headingandvelocity. Confidenceintervalsfor robot2’s positionsshown in blue.

25

−5 −4 −3 −2 −1 0 1−1

−0.5

0

0.5

1

1.5

2

2.5

3

3.5

4

x, in meters

y, in

met

ers

Figure12: Using the estimatedinitial headingandvelocity for robot 2, alongwith its control inputs,a dead-reckonedtrajectoryis constructed.With poorinitial estimatesof headingandvelocity, andnocompassorvelocitymeasurementsfor updates,thetrajectoryis very imprecise.

−5 −4 −3 −2 −1 0 1−1

−0.5

0

0.5

1

1.5

2

2.5

3

3.5

4

x, in meters

y, in

met

ers

experiment 8: cooperative using a robot of convenience

Figure13: Usinginformationfrom robot2, robot1 is ableto mapthebacksideof thetriangle,whichit otherwisecouldnotobserve. After thebatchupdateits estimateof robot2 improvesconsiderably.

26

1 2 3 4 5 6 7 8 9 10 11−0.15

−0.1

−0.05

0

0.05

0.1

0.15

timestep

met

ers

Figure14: Error in thesmoothedestimateandthree-� confidenceinterval for robot2’s � position.Theminimumuncertaintyis in themiddleof thetrajectorydueto forward/backwardsmoothing.

Figure15: B21mobilerobotin theplywoodbox.

27

thebox,namelythateachcornerof thebox wascreatedby two walls,andthateachwall wasbounded

at eachendby a corner. The input dataconsistsof 600sonarreturnsacquiredfrom a sequenceof 50

positionsthatform one-and-a-halfloopsaroundtheinsideof thebox. Thevehiclestartedin thelower

left cornerfacingupward.

Thedataprocessingproceededasfollows. First,stateprojectionwasperformedandtrajectorystates

werecreatedfor timestep1 throughtimestep50,withoutany measurementsbeingprocessed.

At eachprocessingcycle, a new vehicletrajectorystatewasaddedto thesystemstatevector, using

Equations17and18. Thedead-reckonedvehicletrajectoryis shown in Figure16(b).After fifty cycles,

a manually-guidedsearchstrategy wasperformedto find ninereturnsthatwereusedto initialize nine

features(thefour cornersandfour wallsof theboxandaprominent“crack” on thebottomwall”). The

nine returnsandthenine featuresareeachlabeledin Figure17, anddetailsof initialization sequence

areshown in Tables4 and5. Azimuth informationfrom thesonarreturnswasnot usedfor gatingbut

not for estimation.Theprocessingproceededasfollows: first, Returns1 and2 wereusedto initialize

Corner1 usingatwo positionrange-onlyinitialization(circle intersection).Next, Return3 wasusedin

conjuctionwith thestateestimatefor Corner1 to initialize Plane1, andReturn4 wasusedin conjuction

with thestateestimatefor Corner1 to initialize Plane2. After this, Return5 wasusedin conjuction

with the stateestimatefor Plane1 to initialize Corner2, andReturn6 wasusedin conjunctionwith

the stateestimatefor Plane2 to initialize Corner3. Likewise,Return7 wasusedin conjuctionwith

thestateestimatefor Corner2 to initialize Plane3,andReturn8 wasusedin conjuctionwith thestate

estimatefor Corner3 to initialize Plane4. Next, Return9 andPlane4 wereusedto initializedthecrack

on plane4. Finally, thestateestimatesfor Plane3 andPlane4 wereusedto initialize thefinal feature,

Corner4.

After the initializations,nine constrainedfeatures(shown in Figure 17) were mappedusingnine

rangemeasurements(shown in Figure18). Oncethesefeatureswereinitialized,nearest-neighborgat-

ing wasperformedbetweenall of the remainingsonarmeasurementsandthe newly initialized map

features.A total of 217of theoriginal 600 measurementswereuniquelymatchedto oneof thenine

featuresshown in Figure19(a). Finally, Figure19(b) shows the resultwhenall thesemeasurements

areappliedin asinglebatchupdate,resultingin adramaticreductionin theuncertaintyellipsesfor the

estimatedfeaturelocationsandin thecompletetrajectoryof thevehicle.

28

−2 −1.5 −1 −0.5 0 0.5 1 1.5 2 2.5 3−2

−1.5

−1

−0.5

0

0.5

1

1.5

2

2.5

3

(a)

−2 −1 0 1 2 3−2

−1

0

1

2

3

East (m)

Nor

th (

m)

(b)

Figure16: (a) Setof all measurementsprocessed,from 50 vehiclepositions. Eachsonarreturnis shown asacirculararc,with raysdrawn from thecenterof thedead-reckonedrobotpositionto thecenterof eacharc. (b)Dead-reckonedvehicletrajectory, with 3-� errorellipses.

29

7

1

2

Cor ner 1

Cor ner 3

Cor ner 2

Cor ner 4

Pl ane 1

Cr ack

Pl

an

e

2

4

Pl ane 4

Pl

an

e

3

9

5

6

8

3

(b)

Figure17: Nine measurementsusedto initialize nine new features,startingwith the cornerin the upperleftof the figure,andbuilding in both directionsaroundthe room, closingthe box in the lower right handcorner.Three-sigmaerrorellipsesareshown for thedead-reckonedvehiclepositionsfor eachof thereturns.

Table4: Detailsfor theninemanually-selectedreturnsusedfor featureinitialization.Returnnumber TimeStep OdometryX (m) OdometryY (m) OdometryHeading(deg) Range(m) Azimuth (deg)

1 27 .0058 .0002 -3.8264 1.5486 -.39272 44 1.1949 .5997 -7.8248 2.0303 4.31973 44 1.1949 .5997 -7.8248 .8706 3.27254 49 1.1998 .0044 -8.9705 1.8202 -.39275 16 1.1990 .1490 -1.5643 1.4352 2.74896 36 .0004 .5950 -6.2571 1.3806 -1.96357 21 1.1995 .0063 -3.1013 .6280 3.27258 42 1.1949 .5997 -7.1450 1.1788 -.65459 50 1.1998 .0044 -9.3471 .8658 .6545

Table5: Methodof initialization for theninefeatures,andcomparisonof hand-measuredandactuallocationsforthefour cornersof thebox.

Handmeasured CML estimatedFeature InitializationMethod x y x y

Corner1 Returns1 and2 -0.6240 1.4153 -0.6564 1.4287Plane1 Corner1 andReturn3Plane2 Corner1 andReturn4Corner2 Plane1 andReturn5 1.7652 1.4153 1.7838 1.4605Corner3 Plane2 andReturn6 -0.6240 -0.5550 -0.6050 -0.6243Plane3 Corner2 andReturn7Plane4 Corner3 andReturn8Crack Plane4 andReturn9

Corner4 Plane3 andPlane4 1.7652 -0.5550 1.7652 -0.5550

30

−2 −1 0 1 2 3−2

−1

0

1

2

3

East (m)

Nor

th (

m)

Figure18: Stateestimatesand3-� errorellipsesfor thenineinitialized features.

5 Integrated Concurrent Mapping and Localization Results

Theframework describedabove in Section3 hasbeenimplementedaspartof anintegratedframework

for real-timeCML, which incorporatesdelayedstatemanagement,perceptualgrouping,multiple van-

tagepoint initialization,batchupdating,andfeaturefusion.Figure20 givesanoverview of theflow of

informationin thesystem.

For theseexperiments,afixedsizeof 40 trajectorytimestepswasutilized. Every40 timesteps,per-

ceptualgroupingwasperformedusingthesonarreturnsfrom thepast40 timesteps.In general,awide

varietyof strategiesfor makingdelayeddataassociationdecisionsarepossiblewithin this framework.

In this paper, we do not attemptto describea singledefinitivedecision-makingpolicy, ratherour goal

is to illustratetheprocesswith a few representativeexampleswith sonardata.For theresultsreported

here,weuseaHoughtransformtechniquedocumentedin Tard́osetal. (2001).A brief summaryof the

techniqueis asfollows (Leonard,Newman,Rikoski,Neira,andTard́os2001):

The datafrom a standardring of Polaroidsonarsensorscan be notoriouslydifficult to interpret.

31

−2 −1.5 −1 −0.5 0 0.5 1 1.5 2 2.5 3−2

−1.5

−1

−0.5

0

0.5

1

1.5

2

2.5

3

(a)

−2 −1 0 1 2 3−2

−1

0

1

2

3

East (m)

Nor

th (

m)

(b)

Figure19: (a) Sonarmeasurementsthatuniquelygatedwith thenineinitialized features,to beusedin thebatchupdate.(b) Featurelocationestimates,vehicletrajectory, anderrorellipsesafterthebatchupdate.

32

ASYNCHRONOUS DATA AQUISITION

Data Collection

Data Storage

Perceptual Grouping

START

State Projection

DATA ASSOCIATION

Delayed State Management

Feature Manufacture

Batch Update

Map Update

Feature Management

EXIT

Indi

vidu

al O

bser

vatio

ns

grouped obsevations

positive associations

new feature

no new data

raw sensor data

new data available

SYNCHRONOUS PROCESSING

stable initialisation ambiguity

new and existing unexplained data is combined with a history �of vehicle states to search for a stable initialisation of a new feature

Addition and removal of past vehicle states �

unexplained observations

all newly explained observations are used during initialisation �Features can be fused with each other (equivalence assertion). Stagnant features can be removed (garbage collection). Compound features can be built. �

Any applicable technique can �be applied here, for example Hough transform, RANSAC, least medians, maximum likely-hood

Figure20: Informationflow for cycle integratedreal-timeCML incorporatingdelayedstatemanagement,per-ceptualgrouping,multiple vantagepoint initialization,batchupdating,andfeaturefusion.

33

odometry trajectory and measurements referenced to the odometry trajectory

(a)

−0.5 0 0.5 1 1.5 2 2.5 3 3.5 4

−2.5

−2

−1.5

−1

−0.5

0

0.5

1

cml trajectory and measurements referenced to the cml trajectory

(b)

Figure21: (a) Raw sonardatafor experimentwith two point objects,referencedto odometry. (b) sonarreturnsmatchedto thetwo features,referencedto theCML estimatedtrajectory. Theexperimentwas50 minuteslong.The vehiclemoved continuouslyundermanualcontrol at a speedof 0.1 metersper second,makingabout15loopsof thetwo cylinders. 34

−1

0

1vehicle east error bound vs. time

met

ers

−1

0

1vehicle north error bound vs. time

met

ers

−1

0

1xy cor. coefficient vs. time

−50

0

50

degr

ees

Vehicle heading error bound

−0.5

0

0.5

met

ers

Feature 0 east error bound vs. time

−1

0

1Feature 0 north error bound vs. time

met

ers

−1

0

1

met

ers

Feature 0 east error bound vs. time

0 500 1000 1500 2000 2500 3000 3500−0.5

0

0.5Feature 0 north error bound vs. time

met

ers

time (seconds)

Figure22: Estimatederrorboundsfor theexperiment.(Topplots: three-sigmaboundsfor � and � of thevehicle;next plot: � - � correlationcoefficient; next plot: three-sigmaboundsfor vehicle heading;bottom four plots:three-sigmaboundsfor the � and � locationsof thetwo features.Thereis no ground-truthfor this experiment,however thevehiclereturnedto within a few inchesof thestartposition,commensuratewith theCML algorithmstateestimationerror.

35

Figure23: Raw datafor corridorexperiment,referencedto odometry.

36

(a)

(b)

(c)

Figure 24: (a) CML estimatedtrajectoryfor corridor sceneand estimatedmap consistingof points and linesegments. Three-sigmaerror boundsareshown for the location of points. (b) Sameplot as in (a), but withthree-sigmaerrorboundsfor linesadded.(c) Sameplot asin (a),but with hand-measuredmodeloverlaid.

37

This leadsmany researchersaway from a geometricapproachto sonarmapping. However, usinga

physics-basessensormodel,the geometricconstraintsprovidedby an individual sonarreturncanbe

formulated(Leonardand Durrant-Whyte1992). Eachreturn could originatefrom varioustypesof

features(point, plane,etc.) or couldbespurious.For eachtypeof feature,thereis a limited rangeof

locationsfor apotentialfeaturethatarepossible.Giventheseconstraints,theHoughtransform(Ballard

andBrown 1982)canbeusedasavotingschemeidentify pointandplanarfeatures.Moredetailonthis

techniqueis containedin Tard́oset al. (2001).Themethodis similar in spirit to theTBF methodWijk

andChristensen(Wijk andChristensen2000),but canalsodirectly identify specularplanarreflectors

from sonardata,whichis vitally importantin typicalman-madeenvironmentswith many smoothwalls.

Theoutputfrom theHoughtransformgivessetsof measurementswith ahigh likelihoodto originate

from asinglepoint or planefeature.Eachcandidatesetfrom theHoughtypically containsbetween10

and40 sonarreturnshypothesizedto originatefrom a new feature.For eachcandidateset,two returns

arechosento serve as “seed” featuresfor the initialization, to be usedin the function �� , andthe

remainingreturnsareusedin abatchupdate.Thefirst of thetwo “seed”measurementsis chosento be

thereturnin thecandidatesetthatoriginatesfrom theearliestvehiclepositionfrom thesetof trajectory

states.The otherseedmeasurementis chosento be the earliestreturnthat,whencombinedwith the

first seedmeasurement,achievesa sufficient minimumbaselinefor featureinitialization (typically 0.6

meters).Onceanew featureis initialized,it is discardedif it hastoosmallof abaseline.To successfully

distinguishdoorsfrom thewalls in thecorridorexperimentshown in Figure1, a minimum valid line

lengthof 1.2 metersfor addinga featureinto the map. (This restrictioncanbe removed if the joint

compatibilitymethodof NeiraandTard́os(2001)is applied.)

For stateestimation,onehasa choicebetweentwo basicstrategies: (1) attemptto matchindividual

measurementsto pre-existing features,or (2) usemeasurementsexclusively for new featureinitializa-

tion andbatchupdating,followed by featurefusion with previously mappedfeaturesto obtainerror

reduction.A hybridstrategy thatmixesbothpoliciesis alsopossible.While wehavehadgoodsuccess

with either(1) only, (2) only, or amix of both,in thispaperwefocusonoption(2), namelynew feature

mappingfollowedby featurefusion. SeeAyacheandFaugeras(1989),ChongandKleeman(1997a),

Tard́oset al. (2001)andWilliams et al. (2001)for furtherdiscussionof featurefusion. To determine

whenfeaturesshouldbefusedtogether, we usetheMahalanobisdistanceandnearestneighbor.

To illustratetheperformanceof theimplementation,wepresentresultsfrom two differentsimplified

settings,oneexperimentwith two pointobjectsonly (cylindersof known radii), andoneexperimentin

thecorridorshown in Figures1 and2. Videosof thereplayof dataprocessingfor thesetwo experiments

38

areaccessibleat:

http://oe.mit.edu/˜jleonard/d ata/ video shttp://oe.mit.edu/˜jleonard/d ata/ /cyli nder s/cyl inde rs7.m pghttp://oe.mit.edu/˜jleonard/d ata/ video s/ja n24_a ll_s tart. mpghttp://oe.mit.edu/˜jleonard/d ata/ corri dor/ corri dor_ all.m pg .

All of theraw datafor theseexperimentshasbeenmadepublicly availableon thewebat:

http://oe.mit.edu/˜jleonard/d ata/

The methodhasalso beenimplementedrunning in real-timeundermanualcontrol. To our knowl-

edge,this is thefirst successfulfeature-basedCML implementationusingsonarsensingfor which the

robot wascontinuallyin motion andthe CML outputwasgeneratedin real-time. (ChongandKlee-

man(1997a)implementedsonar-basedmappingwith a high-performancesonararraythatstoppedto

performmechanicalscanningfor eachdataacquisitioncycle.) ThemethodusesthestandardPolaroid

sonararrayontheB21robotandcouldbereadilyportedto any B21mobilerobot.Sucharesulthasnot

beenachievedbeforebecauseit hasnot beenpossiblewithout theexpandedrepresentationaccounting

for temporalcorrelations,describedin Section3.

Themethodpresentedin this paperalsobeenusedextensively in two otherexperimentalsystems.

With sonar, using RANSAC for perceptualgroupingand the ATLAS framework for scalablemap-

ping (Bosse,Newman,Leonard,andTeller2002),wehavemappeda largeportionof theMIT campus

anddemonstratedclosureof large loops,usingonly sonarandodometrydata.A representative result

is shown in Figure25. We have alsoextendedtheframework presentedin this paperto achieve robust

3-D localmappingfrom omnidirectionalvideodata(Bosse,Rikoski,Leonard,andTeller2002).

6 Conclusion

Thispaperhasdescribedageneralizedframework for CML thatincorporatestemporalaswell asspatial

correlations,allowing featuresto beinitialized from multiple uncertaintyvantagepoints. Themethod

hasbeenappliedto Polaroidsonardatafrom aB21 mobilerobot,demonstratingtheability to perform

CML with sparseandambiguousdata.Theseexperimentsillustratethebenefitsof addingpastvehicle

positionsto thestatevector, enablingstochasticmappingto beperformedin situationswherethestate

of afeaturecanonlyby partiallyobservedfromasinglevehiclepositionandtheambiguityof individual

measurementsis high.

39

0 20 40 60 80 100 120 140 160-80

-60

-40

-20

0

20

40

60

0 20 40 60 80 100 120 140 160-80

-60

-40

-20

0

20

40

60

Figure25: Map producedfrom B21 sonardatafor severalcorridorsof theMIT campus,createdusingthestateestimationframework describedin Section3 combinedwith RANSAC for perceptualgroupingandtheATLASframework for large-scalemapping(Bosse,Newman,Leonard,andTeller2002).Themissionwas50minutesindurationandthevehicletraveleda distanceof 481meters,with a peakvelocity of 0.3meterspersecondandanaveragevelocityof 0.163meterspersecond.

40

6.1 Related Research

The notion of incorporatingsegmentsof the robot trajectoryin the statevector (insteadof just the

currentrobotstate)employedin this papers similar in somerespectsto thework of Thrun(2001)and

GutmannandKonolige(1999),which alsousethevehicletrajectoryasoneof thekey elementsof the

maprepresentation.In our work, we only save partial segmentsof the vehicletrajectory, on an “as-

needed”basisto resolvedataassociationandfeaturemodelingambiguity. Webelievethatit is possible

to posethe problemof stochasticmappingwithout features,usingonly trajectorystates.The basic

updateoperationwould be to correlatetheobservedsensordatafrom onepositionwith thatobserved

at anotherposition,andto formulatea measurementupdatefunction �� that involvesonly trajectory

states.For example,CarpenterandMedeiros(2001)havereportedCML resultsusingmultibeamsonar

images.Fleischerhasemployedsmoothingto goodeffect in astochasticframework for underseavideo

mosaicking(Fleischer2000).

Themethodsof Thrun(2001)andGutmannandKonolige(1999)cancomputepositionoffsetsfor

therobotby correlatingthecurrentlaserscanswith anotherpreviously obtainedlaserscan.A benefit

of this typeof approachis that thedataassociationproblemdoesnot needto besolvedfor individual

sensormeasurements.Very impressiveexperimentalresultshave beenobtainedwith bothapproaches.

With sonar, theraw datais usuallytoo noisyandambiguousfor thesecorrelation-basedapproachesto

work.

Recentwork in feature-basedCML hasshown theimportanceof maintainingspatialcorrelationsbe-

tweenthestateestimatesfor differentfeatures,in orderto maintainconsistenterrorbounds(Castellanos

andTardos2000;Dissanayake, Newman,Durrant-Whyte,Clark, andCsorba1999). The representa-

tion of spatialcorrelationsresultsin an �� "!#� growth in computationalcost(Moutarlier andChatila

1989b),where � is the numberof featuresin the environment. This hasmotivatedtechniquesto ad-

dressthemapscalingproblemthroughspatialandtemporalpartitioning(Davison1998;Leonardand

Feder2001;GuivantandNebot2001).Thecurrentpaperhasnot addressedthemapscalingproblem,

however it providesa framework for increasingthe reliability of local mapbuilding. This is antici-

patedto greatlyexpandtherangeof environmentsin which CML canbesuccessfullyperformed.For

a givennew typeof environment,it is essentialto establishreliablelocal mappingbeforeconsidering

thelarge-scalemappingproblem.

An alternative to achieve the sonarmappingresultspresentedhereis to usea customsonararray

thatcanclassifyandinitialize geometricprimitivesfrom a singlevantagepoint. Thestate-of-the-artin

41

this areais exhibitedby thework of Kleemanet al. (Kleeman2001;HealeandKleeman2001;Chong

andKleeman1997a;KleemanandKuc 1993). For example,ChongandKleeman(1997b)have used

customadvancedsonararraysto very goodeffect in testinglarge-scaleCML algorithms. Sincethis

wasa scanningsonar, the robot hasto stopandscanat eachlocation,however, morerecently, Heale

andKleeman(2001)havedemonstratedasmall,multi-elementsensorthatperformsrapidclassification

to enablemappingwhile moving.

None-the-less,attemptingto performCML with thestandardring of polaroidsensorsis aninterest-

ing andimportantproblemfrom both a practicalanda basicscienceperspective. The challengesof

range-onlyinterpretationexplicitly capturemany importantuncertaintymanagementproblemsposed

by CML. Thefundamentalessenceof sonarasa range-onlysensorproviding only sparseinformation

is maintainedin a mannerthat canbe appliedto alternative, moregeneralsituations,suchasmulti-

robotmapping.Much further researchis necessaryto extendtheapproachto complex environments,

suchasthemappingof underwaterterrain;however, weanticipatethegeneralizedframework for CML

presentedhereto bebroadlyapplicablein a varietyof environments.

6.2 Future Work

A numberof importanttopicswarrantconsiderationin future work. In the resultspresentedin this

paper, new featuresweremappedusingan initialization function �$�� thatdeterminedthe locationof

a new mapfeaturelocationasan explicit function of measurementsfrom multiple uncertainvantage

points. An alternative is to usenon-linearleastsquaresperformedon many measurements,suchin a

similar mannerto bundleadjustment(Triggs,McLauchlan,Hartley, andFitzgibbon2000). Faugeras

summarizesthenecessaryformulaefor computingthecovariancematrix to accompany thestateesti-

matederivedfrom theleastsquaresoptimization(Faugeras1993),andthiscanbereadilyincorporated

into thestochasticmap.

Anotherinterestingtopic is adaptive featureinitialization andthe integrationof CML with closed-

looptrajectorycontrol.Strategiesshouldbedevelopedfor controllingtherobotduringtheinitialization

of a feature,and in selectingwhich measurementsto usefor performingthe initialization. Onecan

incorporatean adaptive motion control stepto direct the robot to move to a bettervantagepoint that

will improve the informationavailable to the robot in attemptingto initialize a feature. To provide

improvedstability, theadditionof new featuresto thestatevectorcanbedelayedto occuronly whenthe

initializing Jacobiansindicatethatthenew featureestimateis well-conditioned.It wouldbeinteresting

42

to couplethis backto controltherobot’smotionfor dataacquisition.

It is currentlyunclearhow well the techniquewill performin situationswith extremelypoordead-

reckoning. As long asthe linearizationassumptionsof the EKF aresatisfied,then the stateestima-

tion framework presentedhereshouldbe expectedto provide consistentinitialization despitedead-

reckoning error, by maintainingcorrelationsbetweenpastandcurrentvehiclestates. However, we

expectany approachthatusestheEKF to fail whenvery largeangularerrorsareencountered.In addi-

tion, thetaskof perceptualgroupingis reliant to someextenton reasonablyaccuratedead-reckoning.

Futurework is necessaryto quantify theserelationshipsover a rangeof differentoperatingconditions

(e.g.,environmentwith sparsefeatures).

In ongoingresearch,we have areextendingthe framework presentedin this paperto enablelarge-

scaleautonomousexplorationof unknown environments(Bosse,Newman,Leonard,andTeller 2002;

Newman,Bosse,andLeonard2002).Theapproachis alsobeingextendedto enablereal-timecooper-

ativenavigationby multipleAUVs (Fenwick,Newman,andLeonard2002).

A Functions for Initialization of Features from Multiple Vantage Points

This appendixdescribesfunctionsfor initialization of point andline measurementsfrom multiple po-

sitions.

1.1 Initializing a point from two range measurements

Observingthe rangefrom the robot to a point definesa circle which the point must lie upon. Two

observationsdefinetwo circles,theintersectionof whichdefinestwo points.Theambiguityis resolved

throughtheuseof additionalinformation,usuallyathird rangemeasurementor abeamwidthconstraint.

If therobotobservesa point feature �&%('*)+� from vantagepoints �&%-,.'/)0,1� and �&% ! '/) ! � , measuringranges2 , and 2 ! , two circlescanbedefined:�&%435%-,*� !76 �&)839):,1� ! 3 2 ! ,<; . (28)�&%435% ! � ! 6 �&)839) ! � ! 3 2 !! ; . (29)

Thesolutioncanbecomputedasfollows:

%:= ; > � ) ! 39):,1�@?A ! 3 � 2 !! 3 2 !, �B�C% ! 3D%-,*�A ! 6 %-, 6 % !E (30)

43

)F= ; G �&% ! 35%-,*�0?A ! 3 � 2 !! 3 2 !, �-�&) ! 39)0,��A ! 6 )0, 6 ) !E (31)

where ? ; H I � 2 ! 6 2 ,1� ! 3 A !.J I A ! 3K� 2 ! 3 2 ,1� ! J (32)

and A ! ; �&% ! 3D%-,*� ! 6 �&) ! 35):,1� !(L (33)

If ? is imaginary, thenthecirclesdo not intersect.SeealsoWijk andChristensen(Wijk andChris-

tensen2000).

1.2 Initializing a line from two range measurements

Therecanbeasmany asfour lineswhich aretangentto two circles.Consideringonly thecaseswhere

thetwo circlesaretangentto thesamesideof theline, thetwo solutionsare:

M ;ONQP*RFS/NUT G �&% ! 35%-,*�@?> �&) ! 39)0,1�@?V3K�&% ! 35%-,*�-� 2 ! 3 2 ,*� (34)

W ; > �&%-,�) ! 3D%-,�)0,1�@? 6 �&) ! 39)0,1�X� 2 ,�) ! 3 2 ! )0,1��C% ! 35%-,*� ! 6 � ) ! 39):,1� ! ' (35)

where ? ; H �C% ! 35%-,1� ! 6 � ) ! 39):,1� ! 3Y� 2 ! 3 2 ,1� ! ' (36)

andM

designatestheangleof thenormalof the line and W is theperpendicularoffsetof the line from

theorigin. When ? is imaginarythenthecirclesareconcentricanddonothaveacotangent.

1.3 Initializing a line from a range measurement and a colinear point

Initializing theline which is tangentto circle �C%-,#'/)0,.' 2 ,1� andpassesthroughpoint �&% ! '/) ! � is equivalent

to findingtheline which is tangentto two circleswhenoneof thecircleshaszeroradius.Therearetwo

solutions.Thetwo results,� W ,.' M ,*� and � W ! ' M ! � , are:A ; H �C% ! 3Z%-,*� ! 6 � ) ! 35)0,1� ! (37)M =\[ ;ONUP1RFS.NQT E �C) ! 35):,\'*% ! 35%-,1� (38)

44

] ;^NQP*R_RF`badc 2A(e (39)M , ;Yf 6 ](40)M ! ;^f 3 ](41)g %h[i,)U[j,1k ; g %-, 6 2 , RF`ba M ,):, 6 2 , a*lmT M ,nk (42)g %h[ !)U[ ! k ; g %-, 6 2 , RF`ba M !):, 6 2 , a*lmT M ! k (43)

f , ; H % ![j, 6 ) ![j, (44)

f ! ; H % ![ ! 6 ) ![ ! (45)] , ;YNUP*R#S.NUT E �C)U[j,\'*%h[j,1� (46)] ! ;YNUP*R#S.NUT E �C)U[ ! '*%h[ ! � (47)W , ;^f , RF`ba � M ,o3 ] ,*� (48)W ! ;^f ! RF`ba � M ! 3 ] ! � (49)

1.4 Initializing a point from a line and a range measurement

A robotpositionanda rangedefinea circle �C%('/)"' 2 � . Providedthat thecircle intersectstheline � W ' M � ,we want to find the intersectionpoints. First, we find thedistancefrom thecenterof thecircle to the

line, which is definedas: A ;qp W 35% a*lmT � M �o39) RF`ba � M � p (50)

This is thefirst leg of a right triangle. Thehypotenuseis therangemeasurement.Theanglebetween

two is: ] ;ONUP1R_R_`Uasr A2Bt (51)

Knowing thebearingto theline is, by definition,M, thetwo intersectionsaretherefore:g %-,)0,1k ; g % 6 W R_`ba � M 3 ] �) 6 W a1luT � M 3 ] �#k (52)g %-,)0,*k ; g % 6 W R_`ba � M 6 ] �) 6 W a*lmT � M 6 ] �#k (53)

45

Acknowledgments

This researchhasbeenfundedin partby NSFCareerAwardBES-9733040,theMIT SeaGrantCollegeProgramundergrantNA86RG0074(projectRCM-3), and the Office of Naval ResearchundergrantN00014-97-0202.Theauthorswould like to thankJ.D. Tard́os,J.Neirafor helpful discussionsandthecontribution of theHoughtransformfeatureclassificationtechniqueto theresultsreportedin Section5.

References

Anderson,B. D. O. andJ.B. Moore(1979).Optimalfiltering. EnglewoodCliffs, N.J.: Prentice-Hall.

Au, W. (1993).TheSonarof Dolphins. New York: Springer-Verlag.

Ayache,N. andO. Faugeras(1989).Maintainingrepresentationsof theenvironmentof a mobilerobot.IEEETrans.RoboticsandAutomation5(6), 804–819.

Ballard,D. andC. Brown (1982).ComputerVision. Prentice-Hall.

Bar-Shalom,Y. andT. E. Fortmann(1988).TrackingandDataAssociation. AcademicPress.

Bosse,M., P. Newman,J.Leonard,andS.Teller (2002).An atlasframework for scalablemapping.TechnicalReportMarineRoboticsLaboratoryTechnicalMemorandum02-04,MassachusettsInstituteof Technol-ogy.

Bosse,M., R. Rikoski, J. Leonard,and S. Teller (2002, September).Vanishingpoints and 3d lines fromomnidirectionalvideo.In InternationalConferenceon Image Processing, Rochester, NY.

Brooks,R. A. (1984).Aspectsof mobile robotvisualmapmaking.In SecondInt. Symp.RoboticsResearch,Tokyo, Japan,pp.287–293.MIT Press.

Bucy, R. S.andK. D. Senne(1971).Digital synthesisof nonlinearfilters.Automatica7, 287–298.

Carpenter, R. N. (2001).Concurrentmappingandlocalizationandmapmatchingon autonomousunderwatervehicles.In IEEE Oceans, Hawaii, pp.380–389.

Castellanos,J. A. and J. D. Tardos(2000).Mobile RobotLocalizationand Map Building: A MultisensorFusionApproach. Boston:Kluwer AcademicPublishers.

Chiuso,A., P. Favaro, H. Jin, and S. Soatto(2000).3-d motion and structurefrom 2-d motion causuallyintegratedover time: Implementation.In SixthEuropeanConferenceon ComputerVision.

Chong,K. S.andL. Kleeman(1997a).Sonarbasedmapbuilding in largeindoorenvironments.TechnicalRe-portMECSE-1997-1,Departmentof ElectricalandComputerSystemsEngineering,MonashUniversity.

Chong,K. S.andL. Kleeman(1997b).Sonarfeaturemapbuilding for amobilerobot.In Proc.IEEEInt. Conf.RoboticsandAutomation.

Choset,H. andK. Nagatani(2001,April). Topologicalsimultaneouslocalizationandmapping(slam):towardexact localizationwithout explicit localization. IEEE Transactionson Roboticand Automation17(2),125–137.

Cox, I. J. andJ. J. Leonard(1994,April). Modeling a dynamicenvironmentusinga Bayesianmultiple hy-pothesisapproach.Artificial Intelligence66(2), 311–344.

Davison,A. J. (1998).MobileRobotNavigationUsingActiveVision. Ph.D. thesis,Universityof Oxford.

Deans,M. (2002,May). Maximally informative statisticsfor localizationandmapping.In Proc. IEEE Int.Conf. RoboticsandAutomation, pp.1824–1829.

Deans,M. and M. Hebert(2000, December).Experimentalcomparisonof techniquesfor localizationandmappingusing a bearingsonly sensor. In Proc. of the ISER’00 Seventh International SymposiumonExperimentalRobotics.

46

Dissanayake, M. W. M. G., P. Newman,H. F. Durrant-Whyte,S. Clark, andM. Csorba(1999,March).Anexperimentalandtheoreticalinvestigationinto simultaneouslocalizationandmapbuilding. In SixthInter-nationalSymposiumon ExperimentalRobotics, pp.265–274.

Dissanayake, M. W. M. G., P. Newman, H. F. Durrant-Whyte,S. Clark, and M. Csorba(2001, June).Asolutionto thesimultaneouslocalizationandmapbuilding (slam)problem.IEEETransactionsonRoboticandAutomation17(3), 229–241.

Doucet, A., N. de Freitas,and N. Gordan(Eds.) (2001). SequentialMonte Carlo Methodsin Practice.Springer-Verlag.

Faugeras,O. (1993).Three-DimensionalComputerVision: A GeometricViewpoint. MIT Press.

Faugeras,O.,Q.-T. Luong,andT. Papadopoulo(2001).TheGeometryof Multiple Images. MIT Press.

Feder, H. J.S.(1999).SimultaneousStochasticMappingandLocalization. Ph.D. thesis,MassachusettsInsti-tuteof Technology.

Feder, H. J.S.,J. J.Leonard,andC. M. Smith(1999,July). Adaptive mobilerobotnavigationandmapping.Int. J. RoboticsResearch 18(7), 650–668.

Fenwick,J.,P. Newman,andJ.Leonard(2002,May). Collaborative concurrentmappingandlocalization.InProc. IEEEInt. Conf. RoboticsandAutomation, Washington,DC, pp.1810–1817.

Fischler, M. A. and R. C. Bolles (1981).Randomsampleconsensus:A paradigmfor model fitting withapplicationsto imageanalysisandautomatedcartography. Communicationsof theACM 24(6), 381–395.

Fleischer, S.D. (2000).Bounded-Error Vision-BasedNavigationof AutonomousUnderwaterVehicles. Ph.D.thesis,StanfordUniversity.

Grimson,W. E. L. (1990).ObjectRecognition by Computer:TheRoleof GeometricConstraints. MIT Press.(With contributionsfrom T. Lozano-PerezandD. P. Huttenlocher).

Guivant,J.andE. Nebot(2001,June).Optimizationof thesimultaneouslocalizationandmapbuilding algo-rithm for realtime implementation.IEEETransactionson RoboticandAutomation17(3), 242–257.

Gutmann,J.-S.andK. Konolige(1999).Incrementalmappingof largecyclic environments.In InternationalSymposiumon ComputationalIntelligencein RoboticsandAutomation.

Hartley, R. I. andA. Zisserman(2001).Multiple View Geometryin ComputerVision. CambridgeUniversityPress,ISBN: 0521623049.

Heale,A. and L. Kleeman(2001).Fast target classificationusing sonar. In Proc. IEEE Int. WorkshoponIntelligentRobotsandSystems, pp.1446–1451.

Jensfelt,P. (2001).Approachesto Mobile RobotLocalizationin Indoor Environments. Ph.D. thesis,RoyalInstituteof Technology, Stockholm,Sweden.

Julier,J.K. UhlmannandH. F. Durrant-Whyte,S.J.(1996).A New Approachfor theNonlinearTransforma-tion of MeansandCovariancesin LinearFilters.IEEE Transactionson AutomaticControl.

Kailath,T., A. Sayed,andB. Hassibi(2000).Linear Estimation. Prentice-Hall.

Kavraki, L., P. Svestka,J.-C.Latombe,andM. Overmars(1996,August).Probabilisticroadmapsfor pathplanningin high-dimensionalconfigurationspaces.IEEE Trans.RoboticsandAutomation12, 566–580.

Kleeman,L. (2001).Advancedsonarsensing.In R. JarvisandA. Zelinsky (Eds.),RoboticsResearch: TheTenthInternationalSymposium, Lorne,Australia.Proceedingsto bepublishedby SpringerVerlag.

Kleeman,L. andR.Kuc(1993).Mobile robotsonarfor targetlocalizationandclassification.TechnicalReportISL-9301,IntelligentSensorsLaboratory, YaleUniversity.

Kuc, R. (1996).Fusingbinauralsonarinformation for object recognition.In IEEE/SICE/RSJInternationalConferenceon MultisensorFusionandIntegration for IntelligentSystems, pp.727–735.

Kuipers,B. J. (2000).Thespatialsemantichierarchy. Artificial Intelligence.

47

Kurien,T. (1990).Issuesin thedesignof practicalmultitarget trackingalgorithms.In Y. Bar-Shalom(Ed.),Multitarget-MultisensorTracking: AdvancedApplications, pp.43–83.Boston:ArtechHouse.

Leonard,J.J.,I. J.Cox,andH. F. Durrant-Whyte(1992,August).Dynamicmapbuilding for anautonomousmobilerobot.Int. J. RoboticsResearch 11(4), 286–298.

Leonard,J.J.andH. F. Durrant-Whyte(1992).DirectedSonarSensingfor MobileRobotNavigation. Boston:Kluwer AcademicPublishers.

Leonard,J. J. andH. J. S. Feder(2001).Decoupledstochasticmapping.IEEE J. OceanEngineering26(4),561–571.

Leonard,J. J., P. Newman,R. Rikoski, J. Neira,andJ. Tard́os (2001).Towardsrobust dataassociationandfeaturemodelingfor concurrentmappingandlocalization.In RoboticsResearch: TheTenthInternationalSymposium. To Appear.

MacCormick,J. (2000).Probabilisticmodellingandstochasticalgorithmsfor visual localisationandtrack-ing. Ph.D. thesis,Universityof Oxford.

McLauchlan,P. F. (2000).A batch/recursive algorithmfor 3d scenereconstruction.In Int. Conf. ComputerVisionandPatternRecognition, Volume2, Hilton Head,SC,USA, pp.738–743.

McLauchlan,P. F. andD. W. Murray (1995).A unifying framework for structureandmotionrecovery fromimagesequences.In Proc. Int. Conferenceon ComputerVision, Boston,Massachusetts,pp.314–320.

McLauchlan,P. F. andD. W. Murray(1996,January).Activecameracalibrationfor ahead-eyeplatformusingthevariablestate-dimensionfilter. IEEE Trans.PatternAnalysisandMachineIntelligence18(1), 14–22.

Mori, S.,C. Chong,E. Tse,andR. Wishner(1986,May). Trackingandclassifyingmultiple targetswithout apriori identification.IEEETrans.onAutomaticControl AC-31(5).

Moutarlier, P. andR. Chatila(1989a,June).An experimentalsystemfor incrementalenvironmentmodelingby anautonomousmobilerobot.In 1stInternationalSymposiumon ExperimentalRobotics, Montreal.

Moutarlier, P. and R. Chatila (1989b).Stochasticmulti-sensorydatafusion for mobile robot location andenvironmentmodeling.In 5th Int. SymposiumonRoboticsResearch, pp.207–216.

Neira,J.andJ.Tard́os(2001).Dataassociationin stochasticmappingusingthejoint compatibilitytest.IEEETrans.on RoboticsandAutomation17(6), 890–897.

Newman, P., M. Bosse,and J. Leonard(2002). Autonomousfeature-basedexploration. TechnicalReportMarineRoboticsLaboratoryTechnicalMemorandum02-02,MassachusettsInstituteof Technology.

Newman,P., J. Leonard,J. Neira, andJ. Tard́os (2002,May). Exploreandreturn: Experimentalvalidationof real time concurrentmappingand localization.In Proc. IEEE Int. Conf. Roboticsand Automation,Washington,DC, pp.pages1802–1809.

Reid,D. B. (1979,Dec.).An algorithmfor trackingmultiple targets.IEEE Trans.on AutomaticControl AC-24(6), 843–854.

Schmidt,H. (1998).GOATS-98— AUV network sonarconceptsfor shallow watermine countermeasures.TechnicalReportSACLANTCEN SR-302,SACLANT UnderseaResearchCentre.

Schultz,A. C. andW. Adams(1998).Continuouslocalizationusingevidencegrids.In Proc. IEEE Int. Conf.RoboticsandAutomation, pp.2833–2839.

Smith, R., M. Self, andP. Cheeseman(1987).A stochasticmap for uncertainspatialrelationships.In 4thInternationalSymposiumon RoboticsResearch. MIT Press.

Sorenson,H. W. (1988).Recursive estimationfor nonlineardynamicsystems.In J. C. Spall (Ed.), BayesianAnalysisof TimeSeriesandDynamicModels, pp.127–166.New York: MarcelDekker.

Tard́os, J., J. Neira, P. M. Newman,andJ. J. Leonard(2001).Robust mappingand localizationin indoorenvironmentsusingsonardata.TechnicalReportMarine RoboticsLaboratoryTechnicalMemorandum2001-4,MIT. http://oe.mit. edu/˜ jl eonard /pu bs /t ar dos_e ta l_ ij rr _2002.p df .

48

Taylor, C. J. andD. J. Kriegman(1995,November).Structureandmotion from line segmentsin multipleimages.IEEETrans.PatternAnalysisandMachineIntelligence17(11),1021–1032.

Thrun,S. (2001,May). A probabilisticonlinemappingalgorithmfor teamsof mobilerobots.Int. J. RoboticsResearch 20(5), 335–363.

Triggs,W., P. McLauchlan,R. Hartley, andA. Fitzgibbon(2000).Bundleadjustmentfor structurefrom mo-tion.

Wijk, O. andH. Christensen(2000,December).Triangulationbasedfusionof sonardatawith applicationinrobotposetracking.IEEE Trans.RoboticsandAutomation16(6), 740–752.

Williams, S.(2001).Efficientsolutionsto autonomousnavigationandmappingproblems. Ph.D. thesis,Uni-versityof Sydney.

Williams, S.,G. Dissanayake, andH. Durrant-Whyte(2001,June).Constrainedinitialisationof thesimulta-neouslocalisationandmappingalgorithm.In Field andServiceRobot2001, Finland,pp.309–314.ISBN952-5183-13-0.

Yagi,Y., K. Shouya,andM. Yachida(2000,May). Environmentalmapgenerationandego-motionestimationin a dynamicenvironmentfor an omni-directionalimagesensor. In Proc. IEEE Int. Conf. RoboticsandAutomation, pp.3493–3498.

Zunino,G. andH. I. Christensen(2001,July).Slamin realisticenvironments.In M. Devy (Ed.),Symposiumon IntelligentRoboticSystems, Toulouse,F.

49

Mapping Partially Observable Features from Multiple...

Documents

Transcript of Mapping Partially Observable Features from Multiple...