D* Lite - idm-lab.org

1 , weapplyLifelongPlanningA*to robotnavigationinunknownterrain, *Litealgorithmis im-plementsthesamebehaviorasStentz FocussedDynamicA* prove propertiesaboutD* believe thattheseresultsprovidea ,suchasDynamicSWSF-FP(Ramalingam&Reps199 6), is givenin(Frigioni,Marchetti-Spaccamela,&N anni2000).Heuristicsearchmethods,suchasA *(Nilsson1971),ontheotherhand,useheurist icknowledgeinformofapproximationsofthego aldistancestofocusthesearchandsolve givenin(Pearl1985).We recentlyintroducedLPA*(LifelongPlan-ning A*),thatgeneralizesbothDynamicSWSF-FPand A*andthususestwo differenttechniquestoreduceitsplanningti me(Koenig&Likhachev 2001).Inthispaper, weapplyLPA* , theresultingplanningtimescanbeontheorder ofminutesforthelargeterrainsthatareoften used,whichaddsuptosubstantialidletimes(S tentz1994).Fo-cussedDynamicA*(D*)(Stentz 1995)is a cleverheuris-ticsearchmethodthatachieves a speedupofonetotwo or-dersofmagnitudes(!)overrepeatedA*sear chesbymod-Copyrightc 2002,AmericanAssociationforArtificialInt elli-gence( ).

2 D*hasbeenexten-sivelyusedonrealrobots,in cludingoutdoorHMMWVs(Stentz&Hebert1995). Itiscurrentlyalsobeinginte-gratedintoMar sRoverprototypesandtacticalmobilerobotpr ototypesforurbanreconnaissance( ; ).However, it *,wethereforepresentD*Lite,a novelreplanningmethodthatimplementsthesa menavigationstrategyasD* *LiteissubstantiallyshorterthanD*,useson lyonetie-breakingcriterionwhencomparingp riorities,whichsimplifiesthemaintenanceo fthepriorities,anddoesnotneednestedif-st atementswithcomplex conditionsthatoc-cupy uptothreelineseach,whichsimplifiestheana lysisoftheprogramflow. Thesepropertiesalsoallow onetoextendit easily, forexample,touseinadmissibleheuristicsan ddif-ferenttie-breakingcriteriato gainefficiency. To gaininsightintoitsbehavior, wepresentvarioustheoreticalpropertiesofL PA*thatalsoapplytoD* thatLPA*is efficientandsimilartoA*,a *LiteisatleastasefficientasD*.We alsopresentanexperimentalevaluationofthe benefitsofcombiningincrementalandheurist icsearchacrossdifferentnavigationtasksin unknownterrain, believe thatourtheoreticalandempiricalanalysisof D*Litewillprovidea goal-directedrobot-navigationtaskinunkno wnterrain, alwayscomputesa reachesthegoalcell,inwhichcaseit stopssuccessfully, orit observesanuntraversablecell.

3 In whichcaseit recomputesa showsthegoaldistancesofalltraversablecel lsandtheshortestpathsfromitscurrentcellt o thegoalcellbothbeforeandaftertherobothas movedKnowledgeBeforetheFirstMove oftheRobot232236357765658127121313137814 1481414121212131413131312141418141414141 4131313121212464333334532482112357656432 481367666666811312999999612121314sstart7 7737777777778199161113121010101010117121 0101010101014411111111117121111111111111 5561235765643248111235765643248122235765 6432482333357656433483444457656444484555 557656555585688835768888848299sgoal6 KnowledgeAftertheFirstMove oftheRobot232236357765658191571414141572 0815158141412121213141313131214142114141 5151513131312121246433333453248211235765 6432481636766666681141369914161599991317 1010101214715101010101113174111111121471 5111111111213185613131412357656432481112 3576564324812223576564324823333576564334 834444576564444845555576565555856sstarts goal777377777777781911108883576888824826 Figure1 changedareshadedgray.

4 Thegoaldistancesareimportantbecauseoneca neasilydeterminea shortestpathfromitscurrentcelloftherobot to thegoalcellbygreedilydecreasingthegoaldi s-tancesoncethegoaldistanceshave smallandmostofthechangedgoaldistancesare irrelevantforre-calculatinga ,onecanefficientlyrecalculatea shortestpathfromitscurrentcelltothegoalc ellbyrecalculatingonlythosegoaldistances thathave changed(orhave notbeencal-culatedbefore) whatD* *LifelongPlanningA*(LPA*)is *isanincrementalversionofA*.It appliestofinitegraphsearchproblemsonknow ngraphswhoseedgecostsin-creaseordecrease overtime(whichcanalsobeusedtomodeledgeso rverticesthatareaddedordeleted). de-notesthefinitesetofverticesofthegraph . denotesthesetofsuccessorsofvertex . Similarly, denotesthesetofpredecessorsofvertex . ! " #$ %'&denotesthecostofmovingfromvertex tovertex #( ) * + , . LPA*alwaysdeterminesa shortestpathfroma givenstartvertex -/.10324. 5 toagivengoalvertex 76 8"0:9; < , use=+>, tode-notethestartdistanceofvertex ?

5 @ , thatis,thelengthofa shortestpathfrom . Like A*,LPA*usesheuristicsCD , : 76 84039 thatapproximatethegoaldistancesofthevert ices . Theheuristicsneedtobenonnegative andconsistent(Pearl1985),thatis,obey thetriangleinequalityCE 76 840:9B " 76 840:91 GF) andCD ! " 76 84039 H%I ! " #$ DJ<CD 7# : K6 8"0:91 forallvertices L and #M N with POFQ 76 8"0 withthesmallestpriorityofallverticesinpr iorityqueueU. (IfUisempty, [X;\.) ^]4_ `:Sinsertsvertex]intopriorityqueueUwithp riority`. ^]4_ `:Schangesthepriorityofvertex]inpriority queueUto`. (Itdoesnothingif thecurrentpriorityofvertex]alreadyequals `.) Finally, ]ASremovesvertex] CalculateKeyRT]ASa01breturnWcMde3R^fKR^] AS1_1gBh3]4RT]AS$S7i(h R^]4_j]AkAl/mBnjS1Y[cMdeKR^f RT]AS1_[g4h3]4R^]AS$So\;procedure InitializeRTSa02bpUrq s;a03bforall]MtvuEgBh3]4RT]AS,qrf RT]AS,q X;a04b g4h3]4R^]Aw xmzyxAS qZ{; ^]zw1xmzyx4_CalculateKeyRT]Aw xmByxAS$S;procedure UpdateVertexR^| Sa06bifRT|(}qZ]w1xmzyxS gBh3]4RT|7S,qrcMdew1~j yA B ^ R^f RT]~S iH BRT]~_1| S$S;a07bifRT| ^| S;a08bifR^f RT|7S }qZgBh3]4RT|7S$ | _CalculateKeyR^| S$S;procedure !)

6 CalculateKeyRT]kAl/mBnSORgBh3]4RT]kAl/m4 nS }qrf RT]kAl/mBnS$Sa10b |* ;a11bifRfKRT|7S rg4h3]4R^| S$Sa12bfKRT|7S q gBh ]BRT| S;a13bforall]Mtvu |K 4RT|7 SUpdateVertexRT]AS;a14belsea15bfKRT|7S q X;a16bforall]Mtvu |K 4RT|7S a| bUpdateVertexRT]AS;procedure MainRTSa17bInitializeRTS;a18bforevera19b ComputeShortestPathRVS;a20bWaitforchange sinedgecosts;a21bforalldirectededgesRT| _j "Swithchangededgecostsa22bUpdatetheedgec ost BRT| _j :S;a23bUpdateVertexR^ :S;Figure2:LifelongPlanningA*.LifelongPl anningA*:TheVariablesLPA*maintainsanesti mate=M ofthestartdistance=+> ofeachvertex . Thesevaluesdirectlycorrespondtotheg-valu esofanA* * *alsomaintainsa alwayssatisfythefollowingrelationship(In variant1): 7 1 M if G ? ]/ :gB T ]~t gz RT]AS $ 1 3 o 7 1 : 1 B A otherwise.(1)A vertex is calledlocallyconsistentiff itsg-valueequalsitsrhs-value,otherwiseit is ,thentheg-valuesofallverticesequaltheirr espective shortestpathfrom -/. vertex byalwaystransitioningfromthecurrentverte x , startingat , toany predecessor #thatminimizes=M # JQ # : (tiescanbebrokenarbitrarily)until reached.

7 (Thisis differentfromFigure1,wherethegoaldistanc esinsteadofthestartdistancesareusedtodet erminea shortestpathandonecanfollow a shortestpathfrom -/. 6 840:9byal-waysmovingfromthecurrentvertex , startingat -/.10324.,toany successor #thatminimizes , : #$ J@= # , until K6 8"0:9isreached.)However, LPA*doesnotmake allver-ticeslocallyconsistentaftersomeed gecostshave ,it usestheheuristicstofocusthesearchandupda tesonlytheg-valuesthatarerelevantforcomp utinga thisend,LPA*maintainsa (Invariant2).Thesearetheverticeswhoseg-v aluesLPA*potentiallyneedstoupdatetomake vertex inthepriorityqueueisalwaysthesameasitske y(Invariant3), whichisa vectorwithtwo components: M F ,where vF E [=M 3 C+ B JCD ! " 6 840:9 and GF E [=M 3 C+ B 01 (numbersinbracketsrefertolinenumbersinFi gure2).Thefirstcomponentofthekeys correspondsdirectlytothef-values TF@= >, *JCE ! " 6 84039 usedbyA*becauseboththeg-valuesandrhs-val uesofLPA*correspondtotheg-valuesofA*andt heh-valuesofLPA*correspondto theh-valuesofA*.]]

8 Thesec-ondcomponentofthekeys ! correspondsto theg-valuesofA*.Keysarecomparedaccording toa ,a key M islessthanorequaltoakey # , denotedby M % # , iff either # or( PF # and % # ).LPA*alwaysex-pandsthevertex inthepriorityqueuewiththesmallestkey(bye xpandinga vertex,wemeanexecuting 10-16 ). Thisis similarto A*thatalwaysexpandsthevertex in thepriorityqueuewiththesmallestf-valueif it *andA*isalsosimilar. ThekeysoftheverticesexpandedbyLPA*arenon decreasingover timejustlike thef-valuesoftheverticesexpandedbyA*(sin cetheheuristicsareconsistent).LifelongPl anningA*:TheAlgorithmThemainfunctionMain ()ofLPA*firstcallsInitialize()toinitiali zethesearchproblem 17 . Initialize()setstheg-valuesofallvertices toinfinityandsetstheirrhs-valuesac-cordi ngtoEquation1 03-04 . Thus,initially theonlylocallyinconsistentvertex andis insertedintotheoth-erwiseemptypriorityqu eue 05 . Thisinitializationguar-anteesthatthefirs tcalltoComputeShortestPath()performsexac tlyanA*search,thatis,expandsexactlythesa mever-ticesasA*inexactlythesameorder.

9 Notethat,inanactualimplementation,Initia lize()onlyneedstoinitializea vertexwhenit encountersit importantbe-causethenumberofverticescanb elargeandonlya few *thenwaitsforchangesin edgecosts 20 . To maintainInvariants1-3ifsomeedgecostshave changed,it callsUpdateVertex() 23 toupdatetherhs-valuesandkeysofthevertice spotentiallyaffectedbythechangededgecost saswellastheirmember-shipinthepriorityqu eueif they becomelocallyconsistentorinconsistent,an dfinallyrecalculatesa shortestpath 19 bycallingComputeShortestPath(), is calledlocallyovercon-sistentiff= ! C+ ! . WhenComputeShortestPath()ex-pandsa locallyoverconsistentvertex 12-13 , thenit setstheg-valueofthevertex toitsrhs-value 12 , whichmakesthevertex iscalledlocallyunderconsistentiff=M C+ .WhenComputeShortestPath()expandsa locallyundercon-sistentvertex 15-16 , thenit simplysetstheg-valueofthevertex toinfinity 15 . Thismakesthevertex theexpandedvertex waslo-callyoverconsistent,thenthechangeo fitsg-valuecanaffectthelocalconsistency ofitssuccessors 13.

10 Similarly, if theexpandedvertex waslocallyunderconsistent,thenit anditssuccessorscanbeaffected 16 . To maintainInvariants1-3,ComputeShortestPat h()thereforeupdatesrhs-valuesofthesevert ices,checkstheirlocalconsistency, andaddsthemto orremovesthemfromthepriorityqueueaccordi ngly 06-08 . ComputeShortestPath()expandsverticesunti l 6 8"0:9islocallyconsistentandthekey ofthevertex toexpandnextisnolessthanthekey of 638"0:9. Thisis similartoA*thatex-pandsverticesuntilit expands 638"0:9atwhichpointintimetheg-valueof 76 8"0:9equalsitsstartdistanceandthef-value ofthevertex toexpandnextisnolessthanthef-valueof 6 8"0:9. If=M 6 8"0:9 NF&afterthesearch,thenthereisnofinite-co stpathfrom o-/. 76 8"0 ,onecantracebacka shortestpathfrom -/. 7638"0:9byalwaystransitioningfromthecurr entvertex , startingat 638"0:9, toany predecessor #thatminimizes=M #$ *J # " (tiescanbebrokenarbitrarily)until -/. similartowhatA*candoif it nowpresentsomepropertiesofLPA*toshowthat itterminates,is correct,similarto A*, (Likhachev &Koenig2001).