Randomized Kinodynamic Motion Planning with Moving Obstacles

withMovingObstacles

DavidHsu

RobertKindel

Jean-ClaudeLatombe

StephenRock

DepartmentofComputerScience

DepartmentofAeronautics&Astronautics

StanfordUniversityStanford,CA94305,U.S.A.

Abstract

Thispaperpresentsanovelrandomizedmotionplannerforrobotsthatmustachieveaspecifiedgoalunderkinematicand/ordynamicmotionconstraintswhileavoidingcollisionwithmovingobstacleswithknowntrajectories.Theplannerencodesthemotionconstraintsontherobotwithacontrolsystemandsamplestherobot’sstatetimespacebypickingcontrolinputsatrandomandintegratingitsequationsofmotion.Theresultisaprobabilisticroadmapofsampledstatetimepoints,calledmilestones,connectedbyshortadmissibletrajectories.Theplannerdoesnotprecomputetheroadmap;instead,foreachplanningquery,itgeneratesanewroadmaptoconnectaninitialandagoalstatetimepoint.Thepaperpresentsadetailedanalysisoftheplanner’sconvergencerate.Itshowsthat,ifthestatetimespacesatisfiesageometricpropertycalledexpansiveness,thenaslightlyidealizedversionofourimplementedplannerisguaranteedtofindatrajectorywhenoneexists,withprobabilityquicklyconvergingto1,asthenumberofofmilestonesincreases.Ourplannerwastestedextensivelynotonlyinsimulatedenvironments,butalsoonarealrobot.Inthelattercase,avisionmoduleestimatesobstaclemotionsjustbeforeplanningstarts.Theplanneristhenallocatedasmall,fixedamountoftimetocomputeatrajectory.Ifachangeintheexpectedmotionoftheobstaclesisdetectedwhiletherobotexecutestheplannedtrajectory,theplannerrecomputesatrajectoryonthefly.Experimentsontherealrobotledtoseveralextensionsoftheplannerinordertodealwithtimedelaysanduncertaintiesthatareinherenttoanintegratedroboticsysteminteractingwiththephysicalworld.

1Introduction

Initssimplestform,motionplanningisapurelygeometricproblem:giventhegeometryofarobotandstaticobstacles,computeacollision-freepathoftherobotbetweentwogivenconfigurations.Thisformulationignoresseveralkeyaspectsofthephysicalworld.Inparticular,robotmotionsareoftensubjecttokinematicanddynamicconstraints(kinodynamicconstraints[DXCR93])thatcannotbeignored.Unlikeobstructionbyobstacles,suchconstraintscannotberepresentedasforbiddenregionsintheconfigurationspace.Moreover,theenvironmentmaycontainmovingob-stacles,requiringthatcomputedpathsbeparametrizedbytimetoindicatewhentherobotisto

1

Figure1:Robottestbedconsistingofanair-cushionedrobotamongmovingobstacles.

achieveaparticularstate.Inthispaper,weconsidermotionplanningproblemswithbothkin-odynamicconstraintsandmovingobstacles,andproposeanefficientalgorithmforthisclassofproblems.Inpractice,wealsoneedtoconsidernumerousotherissues(e.g.,uncertaintyabouttheenvironment),someofwhichwillbeexaminedhere.

Ourworkextendstheprobabilisticroadmap(PRM)frameworkoriginallydevelopedforplan-ˇˇningcollision-freegeometricpaths[Kav94,KSLO96,Sve97].APRMplannersamplestherobot’s

configurationspaceatrandomandretainsthecollision-freesamplesasmilestones.Itthentriestoconnectpairsofmilestoneswithpathsofpredefinedshape(typicallystraight-linesegmentsinconfigurationspace)andretainsthecollision-freeconnectionsaslocalpaths.Theresultisanundi-rectedgraph,calledaprobabilisticroadmap,whosenodesarethemilestonesandtheedgesarethe

ˇlocalpaths.Multi-queryPRMplannersprecomputetheroadmap(e.g.,[KSLO96]),whilesingle-queryplannerscomputeanewroadmapforeachquery(e.g.,[HLM97]).Ithasbeenproventhat,

underreasonableassumptionsaboutthegeometryoftherobot’sconfigurationspace,arelativelysmallnumberofmilestonespickeduniformlyatrandomaresufficienttocapturetheconnectivityoftheconfigurationspacewithhighprobability[HLM97,KLMR95].

Theplannerproposedinthispaperrepresentskinodynamicconstraintsbyacontrolsystem,whichisasetofdifferentialequationsthatdescribesallthepossiblelocalmotionsofarobot.Foreachquery,theplannerbuildsanewroadmapinthecollision-freesubsetoftherobot’sstatetimespace,whereastatetypicallyencodesboththeconfigurationandthevelocityoftherobot.Tosam-pleanewmilestone,itfirstselectsacontrolinputatrandominthesetofadmissiblecontrolsandthenintegratesthecontrolsystemwiththisinputoverashortdurationoftime,fromapreviouslygeneratedmilestone.Byconstruction,thelocaltrajectorythusobtainedautomaticallysatisfiesthekinodynamicconstraints.Ifthistrajectorydoesnotcollidewiththeobstacles,itsendpointisaddedtotheroadmapasanewmilestone.Thisiterativeincrementalprocedureproducesatree-shapedroadmaprootedattheinitialstatetimepointandorientedalongthetimeaxis.Itterminateswhenamilestonefallsinan“endgame”regionfromwhichitisknownhowtoreachthegoal.Thisendgameregionmaybespecificallydefinedforagivenrobot.Itmayalsobegeneratedbytheplannerbyconstructingasecondtreeofmilestonesrootedatthegoalandintegratingtheequationsofmotionbackwardsintime.

2

Ourplannerexploitsthesynergyofpreviouslyproposedideas(seeSection2).Itmakestwokeycontributions,onetheoreticalandoneexperimental:

Weprovideanin-depthanalysisoftheplanner’sconvergencerate.Itshowsthat,ifthestatetimespacesatisfiesageometricpropertycalledexpansiveness,thenundersuitableas-sumptions,theprobabilitythattheplannerfailstofindatrajectory,whenoneexists,quicklygoesto0,asthenumberofmilestonesincreases.Theexpansivenesspropertydefinedheregeneralizesasimilarnotionintroducedin[HLM97]forholonomicrobotsinstaticenviron-ments.Theproofofconvergence,however,isdifferentfromtheonein[HLM97].Theearlierproofassumesthatlocalmotionsoftherobotaretotallyunconstrained.Italsocriticallyusesthesymmetryoftheconnectivityrelationshipinconfigurationspace—ifapointisreach-ablefromapoint,thenisalsoreachablefrom.Thissymmetricrelationshipnolongerholdswhentherobothasanasymmetriccontrolsystem(e.g.,acar-likerobotthatcanonlymoveforward)orwhenobstaclesaremoving.Currentlywedonotknowhowtoestimateapriorithedegreeofexpansivenessforagivenstatetimespace.Hence,ouranalysisisonlyonesteptowardunderstandingtheconvergenceofrandomizedmotionplanners.However,webelievethatexpansivenessisaveryusefulconceptforcharacterizingthespacesinwhichrandomizedplannersarelikelytoworkwell(ornotsowell).Itmayalsohelpindesigningbettersamplingstrategies.

Wealsodescribesourexperiencesinintegratingtheplannerintoahardwarerobottestbed(Figure1).Inthisintegratedsystem,avisionmoduleestimatesobstaclemotionsjustbeforeplanningstarts.Theplanneristhenallocatedasmall,fixedamountoftime(afractionofasecond)tocomputeatrajectory.Ifachangeintheexpectedmotionoftheobstaclesisdetectedwhiletherobotexecutestheplannedtrajectory,theplannerrecomputesatrajectoryonthefly.Experimentsontherealrobotledtoseveralextensionsoftheplannertodealwithtimedelaysanduncertaintiesthatareinherenttoanintegratedroboticsysteminteractingwiththephysicalworld.Thisisparticularlyimportantbecausekinodynamicconstraintsarenotoriouslydifficulttomodelaccurately.Evenmoredifficultistobuildanaccuratemodelforpredictingfutureobstaclemotion.Ourexperimentalworkdemonstratesthatafastplannercanreliablyhandledynamicenvironments,evenwithuncertaintyinthefuturemotionsoftheobstacles.

Therestofthepaperisorganizedasfollows.Section2reviewspreviouswork.Section3describestheplanningalgorithm.Section4developsthetheoreticalanalysisoftheplanner’sconvergence.Sections5through7describeourexperimentswiththeplanneronanonholonomicrobotandonadynamically-constrainedrobotdevelopedtoinvestigatespaceroboticstasks.Insimulation(Sections5and6),weverifiedthattheplannercanreliablysolvetrickyproblems.Inthehard-warerobottestbed(Section7),weverifiedthattheplannercanoperateeffectivelydespitevariousuncontrollableuncertaintiesandtimedelays.

Thispapercombinesandextendstheresultspreviouslyreportedin[HKLR00,KHLR00].Formoredetails,see[Hsu00,Kin01].

3

2PreviousWork

2.1Motionplanningbyrandomsampling

Sampling-basedmotionplanningisaclassicconceptinmotionplanning(e.g.,see[Don87]).Orig-inally,theapproachwasproposedtobothavoiddifficultiesencounteredinimplementingcompleteplanners(e.g.,floating-pointapproximations)andfacilitatetheincorporationofsearchheuristics(e.g.,potentialfields).Samplesareorganizedintoregulargridsorhierarchicalones(e.g.,quadtreesin2-Dconfigurationspaces).Thesegridsprovideanexplicitrepresentationoftherobot’sfreespaceandhelpthesearchalgorithmtorememberthepointsalreadyvisited.Theirsize,however,growsexponentiallywiththedimensionalityoftheconfigurationspace,i.e.,thenumberofdegreesoffreedom(dofs)oftherobot.Moreover,explicitlycomputingthegeometryofthefreesubsetofaconfigurationspacewithdimensiongreaterthanfourorfiveturnsouttohaveaprohibitivelyhighcost.

Randomsampling—morespecifically,PRMmethods—wasintroducedtosolve(geometric)pathplanningproblemsforrobotswithmanydofs[ABD98,BK00,BKL97,BL91,BOvdS99,

ˇˇHLM99,HST94,Hsu00,Kav94,KSLO96,Kuf99,LK01,LH00,SLL01,Sve97].Thecostlycompu-tationofanexplicitrepresentationofthefreespaceisreplacedbyacollisiontestoneveryran-domlypickedsampleandconnectionbetweensamples.This,ofcourse,canbedonewithregular

andhierarchicalgrids,too.Moreinterestingly,randomsamplingprovidesanincrementalplan-ningschemewhichdoesnotartificiallydependonthedimensionalityoftheconfigurationspace.TheanalysisoftheconvergencerateofseveralPRMalgorithmsrevealsthetruevalueofrandom

ˇsampling[Hsu00,HLM97,KKL98,KLMR95,Sve97]:eachnewmilestoneaddedtoaprobabilistic

roadmaprefinesthetheconnectivityrelationshipcapturedinandreducestheprobabilitythattheplannerfailstofindasolutionpath,whenoneexists(seeSection2.3).

Variousapplicationsofrandomizedplannersarereviewedin[Lat99],includingrobotics,de-signformanufacturingandservicing,graphicanimationofdigitalactors,surgicalplanning,andpredictionofmolecularmotion.

Otherplanningapproaches(e.g.,[Ahu94,HXCW98])attempttocapturetheglobalconnectivityofarobot’sfreespacebycombiningexplorationandsearchinamannersimilartographsearchinartificialintelligence.

2.2Samplingstrategies

ProposedPRMtechniquesdifferintheirsamplingstrategies.Animportantdistinctionexistsbe-ˇtweenmulti-querystrategies(e.g.,[KSLO96])andsingle-queryones(e.g.,[HLM97]).Amulti-query

plannerprecomputesaroadmapforagivenrobotandworkspaceandthenusesthisroadmaptoprocessmultiplequeries.Ingeneral,thequeryconfigurationsarenotknowninadvance.Sothesamplingstrategymustdistributethemilestonesovertheentirefreespace.Incontrast,asingle-queryplannercomputesanewroadmapforeachquery.Herethegoalistofindacollision-freepathbetweenthetwoqueryconfigurationsbyexploringaslittlespaceaspossible.Multi-querystrategiesareappropriatewhenthecostofprecomputingaroadmapcanbeamortizedoveralargenumberofqueries.Single-queryonesareappropriatewhenthenumberofqueriesinagivenspaceissmall.Intermediatestrategies,whichprecomputepartialroadmapsandcompletethemtopro-4

cessspecificqueries,havealsobeenproposed[BK00,SMA01].Theplannerproposedinthispaperfollowsthesingle-querysamplingparadigm.

Single-querystrategiesoftenbuildanewroadmapforeachquerybygrowingtreesofsampledmilestonesrootedattheinitialand/orgoalconfigurations[AG99,HLM97,Hsu00,Kuf99,LK99],buttheydifferinthewaytheysamplethemilestonesthatformthenodesofthetrees.Similartotheplannerin[HLM97],ouralgorithmselectsamilestoneinatreetoexpandatrandom,withprobabilityinverseproportionaltothecurrentdensityofmilestonesaround(seeSection3.2).Anewmilestoneisthenpickedbysamplingtheneighborhoodofatrandom.Thisguaranteesthattheroadmapeventuallydiffusesthroughthecomponent(s)ofthefreespacereachablefromthequeryconfigurationsandthatthemilestonedistributionoverthesecomponentsconvergestoauniformone.Thisconditionisneededfortheanalysisoftheplanner’sconvergencedevelopedinSection4.Analternativeistofirstpickaconfigurationintheconfigurationspaceatrandom,selecttobethemilestoneinthetreeclosestto,andthenpickanewmilestonealongthelineconnectingto[LK99].Thistechniqueisslightlysimplertoimplementthanoursandworkswellwhenthequeryadmitsasolutionthatdoesnotrequirelongdetours.However,thissamplingstrategybiasesthedistributionofmilestonestowardthoseregionsintheconfigurationspacewithlargeobstacles.Thismaybeundesirableandseverelyslowdowntherateofconvergenceoftheplanner.Anotherpossibilityistogrowthesearchtreebypickingeachnewmilestoneasfarawayaspossiblefromthecurrentmilestones[AG99].Othertechniquesorrefinementsofthesetechniquesareclearlypossible.Ourexperienceisthat,althoughonemayimprovetheperformanceofaPRMplanneronsomeexamplesbybiaisingthedistributionofmilestones,asamplingstrategythatyieldsauniformdistributionofmilestonesoverthereachablefreespaceavoidspathologicalcasesandgivesthebestresultsontheaverage.

2.3Probabilisticcompleteness

Acompletemotionplannerisonethatreturnsasolutionwheneveroneexistsandindicatesthatnosuchpathexistsotherwise.However,aswasshownin[Rei79],pathplanningisPSPACE-hard,whichisstrongindicationthatanycompleteplannerislikelytobeexponentialinthenumberofdofsofarobot.Addingkinodynamicconstraintsandmovingobstaclesfurtherincreasesthecomplexityoftheproblem[DXCR93,RS85].

Aplannerbasedonrandomsamplingcannotbecomplete.However,aweakernotionofcom-pleteness,calledprobabilisticcompleteness,wasintroducedin[BL91]:aplannerisprobabilis-ticallycompleteiftheprobabilitythatitreturnsacorrectanswergoesto1astherunningtimeincreases.Supposethatarandomizedplannerreturnsasolutionpathassoonasitfindsone,andindicatesthatnosuchpathexistsifitcannotfoundoneafteragivenamountoftime.Iftheplannerreturnsapath,theanswermustbecorrect.Ifitreportsthatnopathexists,theanswermaybesome-timeswrong.Ithasbeenshownthattheprobabilitythattherandomizedpotentialfieldplannerfailstofindasolutionpathwhenoneexistsgoesto0astherunningtimeincreases,henceprovingthattheplannerisprobabilistically(resolution)complete[BL91].Severalotherrandomizedplanners

ˇhavealsobeenproventobeprobabilisticallycomplete[AG99,LK01,LL96,Sve97].

Probabilisticcompleteness,however,isaweakconcept,asitsaysnothingaboutaplanner’srateofconvergence.InordertounderstandwhyPRMplannersworkwellinpracticeandidentifythecaseswheretheymaynotworkwell,weneedtoshowthattheyhaveafastconvergencerate.

5

Thisrequiresustodevelopacharacterizationofthecomplexityoftheinputgeometrythatissuitableforrandomsampling.Thischaracterizationshouldnotdependonthedimensionalityoftheconfigurationspaceinanartificialway.Afterall,isitreallymoredifficulttosampleanempty-dimensionalhypercubethantosampleanemptysquare?Alongtheselines,ithasbeenshownthat,undersuitableassumptions,certainidealizedversionsofmulti-queryPRMpathplannershaveaconvergencerateexponentialinthenumberofsampledmilestones[HLM97,HLMK99,KKL98,

ˇˇKLMR95,Sve97,SO98].

Morespecifically,thenotionof-goodnesswasintroducedtocharacterizethecomplexityofarobot’sconfigurationspace[KLMR95,BKL97].Ifaspaceis-good,thenwithsomelimitedhelpfromacompleteplanner,amulti-queryPRMplannerthatsamplesmilestonesuniformlyatrandomfromtheconfigurationspaceconvergesatanexponentialratewithrespecttothenumberofsampledmilestones.TheproofofthisresultrelatesPRMmethodstotheissueofvisibilitysetsstudiedincomputationalgeometry,inparticular,theart-galleryproblem[O’R97]:eachmile-stoneisregardedasaguardthatseesasubsetoftherobot’sfreespace,themilestone’svisibilityregion[KLMR95].Thisinsightwasrecentlyexploitedtogeneratesmallerroadmaps[SLL01].

Toremovetheneedforacompleteplannerintheproofpresentedin[BKL97],expansivenesswasintroducedasamorerefinedcharacterizationoftherobot’sfreespace.Whilethecomputa-tionalcomplexityofacompleteplannerisusuallyexpressedasafunctionofthenumberofdofsandthenumberandthedegreeofpolynomialsdescribingtheboundarysurfaceofarobotandobstacles,therateofconvergenceofaPRMplannerisexpressedasafunctionofparametersmea-suringthedegreetowhicharobot’sfreespaceisexpansive.Importantly,theexpansivenessofafreespacecapturesthe“narrowpassage”issuestudiedin[HKL98].Itrevealsthetruenarrow-nessofapassageandisabettermeasurethanthewidthofthepassagetocapturethedifficultyofsamplinginamulti-dimensionalnarrowpassage[HLM99].

Inthispaper,wefurthergeneralizethenotionofexpansivenessandextendittostatetimespace.Weprovethatifthestatetimespaceisexpansive,thenundersuitableassumptions,ournewrandomizedplannerforkinodynamicplanningwithmovingobstaclesisprobabilisticallycompletewithaconvergencerateexponentialinthenumberofsampledmilestones.

2.4Geometriccomplexity

OnetenetofthePRMapproachtomotionplanningisthatafastalgorithmexiststochecksampledconfigurationsandconnectionsbetweenthemforcollision.Whenboththerobotandtheobstacleshavesimplegeometricshapes,whichisthecaseofmostexamplesinthispaper,thisassumptionisclearlysatisfied.However,doesthisremaintruewhentherobotandtheobstaclesarecomplex3Dobjectsdescribedby100,000trianglesormore?

Duringthepastdecade,anumberofefficientcollisioncheckinganddistancecomputationalgorithmshavebeendeveloped.Themostpopularonesarehierarchicaldecompositionalgo-rithms,whichprecomputeamulti-levelboundingapproximationofeveryobjectinanenviron-ment,usingprimitivevolumessuchasspheres,axis-alignedboundingboxes,ororientedboundingboxes[CLMP95,GLM96,Hub96,KHM98,KPLM98,Qui94].Experimentsreportedin[SA02]indi-catethatthePQPpackage[GLM96]teststwoobjects,describedby500,000triangleseach,intimesrangingbetween0.0001and0.0025seconds(onanIntelPentiumIII1GHzprocessor),dependingontheactualdistancebetweenthetwoobjects.

6

TheuseofefficientcollisioncheckersinPRMplannershasbeenreportedin[BK00,CL95,HLM97,SA02,SLL01].Theseplannersarecapableofefficientlyandreliablyprocessingplanningquerieswithgeometricmodelscontaininghundredsofthousandsoftriangles.

2.5Movingobstacles

Whenobstaclesaremoving,theplannermustcomputeatrajectoryparametrizedbytime,insteadofsimplyageometricpath.Thisproblemhasbeenproventobecomputationallydifficultevenforrobotswithfewdofs[RS85].

Anumberofheuristicalgorithms(e.g.,[FS96,Fuj95,KZ86])havebeenproposed.Thetechniquein[KZ86]isatwo-stageapproach:inthefirststage,itignoresthemovingobstaclesandcomputesacollision-freepathoftherobotamongthestaticobstacles;inthesecondstage,ittunestherobot’svelocityalongthispathtoavoidcollidingwithmovingobstacles.Theresultingplannerisclearlyincomplete,butitoftengivesgoodresultswhenthenumberofmovingobstaclesissmalland/ortheworkspaceisnottoocluttered.Theplannerin[Fuj95]triestoreducetheincompletenessbygeneratinganetworkofpaths.Theplannerin[FS96]introducesthenotionofavelocityobstacle,definedasthesetofvelocitiesthatwillcausetherobottocollidewithanobstacleatafuturetime.Velocityobstaclesareusedtogenerateaninitialfeasibletrajectoriesfortherobot,whichislateroptimized.Theplannercanhandleactuatorconstraintssuchasboundedacceleration.

Thenotionofaconfigurationtimespacewasintroducedin[ELP87]tocoordinatethemotionofmultiplerobots.Itwaslaterextendedin[Fra93]tothatofastatetimespace,whereastateencodesarobot’sconfigurationandvelocity,toplanrobotmotionswithbothmovingobstaclesandkinodynamicconstraints.

2.6Kinematicanddynamicconstraints

Kinodynamicmotionplanningreferstoproblemsinwhichtherobot’smotionmustsatisfynon-holonomicand/ordynamicconstraints.

Planningfornonholonomicrobotshasattractedconsiderableinterest(e.g.,[BL89,Lau86,LCH89,

ˇˇLJTM94,LM96,SO94,SSLO97]).Oneapproach[Lau86,LJTM94]istofirstgenerateacollision-freepath,ignoringthenonholonomicconstraints,andthenbreakthispathintosmallpiecesand

replacethembyadmissiblecanonicalpaths(e.g.,ReedsandSheppcurves[RS90]).Anexten-sionistoperformsuccessivepathtransformationsofvarioustypes[Fer98,SL98].Arelatedap-ˇˇproach[SSLO97,SO94]usesamulti-queryPRMalgorithmthatconnectsmilestonesbycanonical

pathsegmentssuchasReedsandSheppcurves.Allthesemethodsrequiretherobotstobelocallycontrollable[BL93,HK77,LCH89,LM96].Analternativeapproach,introducedin[BL89,BL93],istogenerateatreeofsampledconfigurationsrootedattheinitialconfiguration.Ateachiteration,asampleisselectedfromthetreeandexpandedtoproducenewsamples,byintegratingtherobot’sequationsofmotionoverashortdurationoftimewithdeterministicallypickedcontrols.Aspacepartitioningschemeregulatesthedensityofsamplesinanyregionoftheconfigurationspace.Thisapproachworkswellforcar-likerobotsandtractor-trailorrobotswithtwotofourdofsandisap-plicabletorobotsthatarenotlocallycontrollable.Ournewplannertakesasimilarapproach,butpickscontrolsatrandom.Neithertheplannernortheanalysisofitsconvergenceraterequirestherobottobelocallycontrollable.Comparedtotheplannerin[BL93]andtheplannerpresentedin

7

thispaper,thetwo-stepapproachof[Lau86,LJTM94]hastheadvantagethatitcanreachthegoalconfigurationexactly,whicheliminatestheneedtodefineanendgameregion,butitisapplicableonlytolocallycontrollablerobots.

Algorithmsfordealingwithdynamicconstraintsarecomparabletothosedevelopedfornon-holonomicconstraints.In[BDG85,SD91],acollision-freepathisfirstcomputed,ignoringthedynamicconstraints;avariationaltechniquethendeformsthispathintoatrajectorythatbothcon-formstothedynamicconstraintsandoptimizesacriterionsuchasminimalexecutiontime.Thesemethodsworkwellonmanypracticalexamples;however,noformalguaranteeofperformancehasbeenestablishedforthem.Infact,itisnotalwayspossibletotransformthepathgeneratedinthefirstphaseintoanadmissibletrajectory,duetolimitsontheactuatorforcesortorques.Theap-proachin[DXCR93]placesaregulargridovertherobot’sstatespaceanddirectlysearchesthegridforanadmissibletrajectoryusingdynamicprogramming.Itoffersprovableperformanceguaran-tees(resolutioncompletenessandanasymptoticboundonthecomputationtime),butitisonlyapplicabletorobotswithfewdofs(typically,twoorthree),asthesizeofthegridgrowsexponen-tiallywiththenumberofdofs.Theplannerin[Fra93]usesasimilarapproachinthestatetimespaceoftherobotanddealswithmovingobstaclesaswell.Bothourplannerandtheonein[Kuf99,LK99,LK01]havemanysimilaritieswiththeapproachtakenin[BL93,DXCR93,Fra93].Ourplannerdiscretizesthestatetimespaceviarandomsampling,insteadofplacingaregulargridoverit.Thismakesitpossibletodealwithrobotswithmanymoredofs.Ontheotherhand,ourplannerdoesnotachieveresolutioncompletenessastheonein[DXCR93].Instead,undersuitableassumptions,itachievesprobabilisticcompletenesswithanexponentialconvergencerate(Section4).

Therepresentationandthealgorithmusedinourplannerbuilduponseveralexistingideas,inparticular:single-queryrandomsamplingofconfigurationspace[HLM97],statetimespaceformulation[BL93,DXCR93,ELP86,Fra93],andrepresentationofkinodynamicconstraintswithacontrolsystem[BL93,DXCR93,Fra93].Themostsalientcontributionsofthisworkarethegeneral-izationofexpansivenesstostatetimespace,thetheoreticalanalysisoftheplanner’sconvergencerate,andtheintegrationandexperimentsoftheplanneronarealrobot.

3Planningframework

Ouralgorithmbuildsaprobabilisticroadmapinthecollision-freesubsetofthestatetimespaceoftherobot.Theroadmapiscomputedintheconnectedcomponentofthatcontainstherobot’sinitialstatetimepoint.

3.1State-spaceformulation

MotionconstraintsWeconsiderarobotwhosemotionisgovernedbyanequationoftheform

(1)

whereThesets

istherobot’sstate,isitsderivativerelativetotime,andisthecontrolinput.andaretherobot’sstatespaceandcontrolspace,respectively.Weassumethatand

8

Figure2:Asimplemodelofacar-likerobot.

areboundedmanifoldsofdimensionsandwith.Bydefiningappropriatecharts,wecantreatandassubsetsofRandR.

Eq.(1)canrepresentbothnonholonomicanddynamicconstraints.Themotionofanon-holonomicrobotisconstrainedbyindependent,non-integrablescalarequationsoftheform

,,whereanddenotetherobot’sconfigurationandvelocity,re-spectively.Definetherobot’sstatetobe.Itisshownin[BL93]that,underappropriate

conditions,theconstraintsareequivalenttoEq.(1)inwhichisavectorinRR.Inparticular,Eq.(1)canberewrittenasindependentequationsoftheform.Dynamicconstraintsarecloselyrelatedtononholonomicconstraints.InLagrangianmechanics,dynamicsequationsareoftheform,where,,andaretherobot’sconfiguration,velocity,andacceleration,respectively.Definingtherobot’sstateas,wecanrewritethedynamicsequationsintheform,which,asinthenonholonomiccase,isequivalenttoEq.(1).

andRobotmotionsmayalsobeconstrainedbyinequalitiesoftheforms

.These-constraintsrestrictthesetsofadmissiblestatesandcontrolstosubsetsofRandR.

ExamplesThesenotionsareillustratedbelowwithtwoexamplesthatwillalsobeusefullaterinthepaper:

Nonholonomiccarnavigation.ConsiderthecarexampleinFigure2.Letbethepositionofthemidpointbetweentherearwheelsoftherobotandbetheorientationoftherearwheelswithrespecttothe-axis.Definethecar’sstatetobeR.Thenonholonomicconstraint

isequivalenttothesystem

Thisreformulationcorrespondstodefiningthecar’sstatetobeitsconfigurationandchoos-ingthecontrolinputtobethevector,whereandarethecar’sspeedandsteeringangle.

9

BoundsonandcanbeusedtorestrictandtosubsetsofRandR,respectively.Forinstance,ifthemaximumspeedofthecaris1,thenwehave.Point-massrobotwithdynamics.Forapoint-massrobotmovingonahorizontalplane,wetypicallywanttocontroltheforcesappliedto.Thisleadsustodefinethestateofas

,whereandarethepositionandthevelocityof.Thecontrolinputs

arechosentobetheforcesappliedtointhe-and-direction.Hencetheequationsofmotionare

(2)

whereisthemassofandistheappliedforce.ThevelocityandforcearerestrictedtosubsetsofRduetolimitsonthemaximumvelocityandforce.

PlanningqueryLetdenotethestatetimespace.Obstaclesintherobot’sworkspacearemappedintothisspaceasforbiddenregions.Thefreespaceisthesetofallcollision-freepoints.Acollision-freetrajectoryisadmissibleifitisinducedbyafunctionthroughEq.(1).Aplanningqueryisspecifiedbyaninitialstatetimeandagoalstatetime.Asolutiontothequeryiseitherafunctionthatinducesacollision-freetrajectory

,suchthat,,oranindicationthat

noadmissibletrajectoryexistsbetweenand.Thisformulationcanbeextendedtoallowtobeanyinstantinsomegiventimeinterval,ortobetheearliestpossiblearrivaltime.Inthefollowing,weconsiderpiecewise-constantfunctionsonly.

3.2Theplanningalgorithm

Ourplanningalgorithmisanextensionofthealgorithmpresentedin[HLM97].Ititerativelybuildsatree-shapedroadmaprootedat.Ateachiteration,itfirstpicksatrandomamilestonefrom,atimewith,andacontrolfunction.ItthencomputesthetrajectoryinducedbybyintegratingEq.(1)from.Ifthistrajectoryliesin,itsendpointisaddedtoasanewmilestone;adirectededgeiscreatedfromto

,andisstoredwiththisedge.Thekinodynamicconstraintsarethusnaturallyenforcedinalltrajectoriesrepresentedin.Theplannerexitswithsuccesswhenthenewlygeneratedmilestonefallsinan“endgame”regionthatcontains.

toeachmilestonein.TheweightMilestoneselectionTheplannerassignsaweight

ofisthenumberofothermilestoneslyingintheneighborhoodof.Soindicateshowdenselytheneighborhoodofhasalreadybeensampled.Ateachiteration,theplannerpicksanexistingmilestoneinatrandomwithprobabilityinverselyproportionalto.Hence,amilestonelyinginasparselysampledregionhasagreaterchanceofbeingselectedthanamilestonelyinginanalreadydenselysampledregion.Thistechniqueavoidsoversamplinganyparticularregionof.

ControlselectionLetbethesetofallpiecewise-constantcontrolfunctionswithatmostconstantpieces.Soeveryadmitsafinitepartitionsuchthatisaconstantoverthetimeinterval,for.Wealsorequire

10

,whereisaconstant.Ouralgorithmpicksacontrol,forsomepre-specifiedand,bysamplingeachconstantpieceofindependently.Foreachpiece,andareselecteduniformlyatrandomfromand,respectively.Thespecific

choicesoftheparametersandwillbediscussedinSection4.5.Intheactualimplementationofthealgorithm,however,onemaychose,becauseanytrajectorypassingthroughseveralconsecutivemilestonesinthetreeisobtainedbyapplyingasequenceofconstantcontrols.

EndgameconnectionUnlikesomeotherplanningtechniques(e.g.,[Lau86,LJTM94]),theabove“control-driven”samplingtechniquedoesnotallowustoreachthegoalexactly.Weneedto“expand”thegoalintoanendgameregionthatthesamplingalgorithmwilleventuallyattainwithhighprobability.Thereareseveralwaysofcreatingsucharegion:

In[BL93],theendgameregionisdefinedtobeaballofsmallradiuscenteredatthegoal.Anypointinthisballisconsideredtobeasufficientlygoodapproximationofthespecifiedgoal.Thistechniqueispracticalonlyinspacesofsmalldimensionality,astherelativevolumeofaballofsmallfixedradiusgoestoward0asthedimensionalityincreases.Wecouldneverthelessadaptthistechniquebysettingtheparameterproportionaltothedistancebetweenthemilestonepickedfromandthegoal,allowingthedensityofmilestonestoincreaseinthegoal’svicinity,andterminatingwithsuccesswhentheplannersamplesanewmilestonecloseenoughtothegoal.

Forsomerobots,itispossibletoanalyticallycomputeoneorseveralcanonicalcontrolfunc-tionsthatexactlyconnecttwogivenpointswhileobeyingthekinodynamicconstraints.AnexampleistheReedsandSheppcurves[RS90]developedfornonholonomiccar-likerobots.Ifsuchcontrolfunctionsareavailable,onecantestifamilestonebelongstotheengameregionbycheckingwhetheracanonicalcontrolfunctiongeneratesacollision-freetrajectoryfromto.

Amoregeneralmethodistobuildasecondarytreeofmilestonesfromthegoalinthesamewayasthatfortheprimarytree,exceptthatEq.(1)isintegratedbackwardsintime.Letbeanewmilestoneobtainedbyintegratingbackwardsfromanexistingmilestone

in.Byconstruction,ifthetimegoesforward,thecontrolfunctiondrivestherobotfromstateattimetostateattime(Figure3).Thusthereisaknowntrajectoryfromeverymilestoneintothegoal.Thesamplingprocessterminateswithsuccesswhenamilestoneisintheneighborhoodofamilestone.Inthiscase,theendgameregionistheunionoftheneighborhoodsofmilestonesin.Togeneratethefinaltrajectory,wesimplyfollowtheappropriateedgesofand;however,thereisasmallgapbetweenand.Thegapcanoftenbedealtwithinpractice.Forexample,beyond,onecanuseaPDcontrollertotrackthetrajectoryextractedfrom.Constructingendgameregionsbybackwardintegrationisaverygeneraltechniqueandcanbeappliedtoanysystemdescribedby(1).

InSections5–7,wewillpresentimplementationsoftheplanner,usingthelasttwotechniquesdescribedabove.

Endgameregioncanalsobeusedwhenthegoaldoesnothaveauniqueconfiguration.Forexample,in[AG99],thegoalregionisdefinedtobethesubsetofconfigurationsofaredundantrobotsuchthattheend-effectorachievesagivenposture.

11

Figure3:Buildingasecondarytreeofmilestonesbyintegratingbackwardsintime.

Algorithminpseudo-codeTheplanningalgorithmissummarizedinthefollowingpseudo-code.1.Insertinto;.2.repeat3.Pickamilestonefromwithprobability.4.Pickacontrolfunctionfromuniformlyatrandom.5.PROPAGATE.6.ifthen7.Addto;.8.Createanedgefromto;storewith.9.ifENDGAMEthenexitwithSUCCESS.10.ifthenexitwithFAILURE.

Figure4:Afreespacewithanarrowpassage

4AnalysisofthePlanner

TheexperimentstobedescribedinSections5–7demonstratethatAlgorithm1providesaneffi-cientsolutionfordifficultkinodynamicmotionplanningproblems.Neverthelesssomeimportantquestionscannotbeansweredbyexperimentsalone.Whatistheprobabilitythattheplannerfailstofindatrajectorywhenoneexists?Doesconvergetoasthenumberofmilestonesin-creases?Ifso,howfast?Inthissection,wegeneralizethenotionofexpansiveness,originallyproposedin[HLM97]for(geometric)pathplanning.Weshowthatinanexpansivespace,thefail-ureprobabilitydecreasesexponentiallywiththenumberofsampledmilestones.Hence,withhighprobability,arelativelysmallnumberofmilestonesaresufficienttocapturetheconnectivityofthefreespaceandanswerthequerycorrectly.

4.1Expansivestatetimespace

Expansivenesstriestocharacterizehowdifficultitistocapturetheconnectivityofthefreespacebyrandomsampling.Tobeconcrete,considerthesimpleexampleshowninFigure4.Assumethattherearenokinodynamicconstraintsandapointrobotcanmovefreelyinthespaceshown.Letussaythattwopointsinthefreespaceseeeachother—equivalently,aremutuallyvisible—ifthestraightlinesegmentbetweenthemliesentirelyin.ThefreespaceinFigure4consistsoftwosubsetsandconnectedbyanarrowpassage.Fewpointsinseealargefractionof.RecallthataclassicPRMplannersamplesuniformlyatrandomandtriestoconnectpairsofmilestonesthatseeeachother.Letthelookoutofbethesubsetofallpointsinthatseesalargefractionof.Ifthelookoutofwerelarge,itwouldbeeasyfortheplannertosampleamilestoneinandanotherinthatseeeachother.However,inourexample,hasasmalllookoutduetothenarrowpassagebetweenand:fewpointsinseealargefractionof.Thusitisdifficultfortheplannertogenerateaconnectionbetweenand.Thisexamplesuggeststhatwecancharacterizethecomplexityofafreespaceforrandomsamplingbythesizeoflookoutsets.In[HLM97],afreespaceissaidtobeexpansiveifeverysubsethasalargelookout.Ithasbeenshownthatinanexpansivespace,aclassicPRMplannerwithuniformrandomsamplingconvergesatanexponentialrateasthenumberofsampledmilestonesincreases.Whenkinodynamicconstraintsarepresent,thebasicissuesremainthesame,butthenotionofvisibility(connectingmilestoneswithstraight-linepaths)isinadequate.Algorithm1generatesadifferentkindofroadmaps,inwhichtrajectoriesbetweenmilestonesmaybeneitherstraight,nor

13

Figure5:Thelookoutofaset.

reversible.Thisleadsustogeneralizethenotionofvisibilitytothatofreachability.

andin,isreachablefromifthereexistsaGiventwopoints

controlfunctionthatinducesanadmissibletrajectoryfromto.Ifremainsreachablefrombyusing,apiecewise-constantcontrolwithatmostconstantpiecesasdefinedinSection3.2,thenwesaythatislocallyreachable,or-reachable,from

.Letanddenotethesetofpointsreachableand-reachablefromsomepoint,respectively;wecallthemthereachabilityandthe-reachabilitysetof.Foranysubset,thereachability(-reachability)setofistheunionofthereachability(-reachability)setsofallthepointsin:

Wedefinethelookoutofasetasthesubsetofallpointsinwhose-reachabilitysetsoverlapsignificantlywiththereachabilitysetofthatisoutside(Figure5):

Definition1Let

beaconstantin

.The-lookoutofaset

is

-LOOKOUT

where

denotethevolumeofaset

.

Thefreespaceisexpansiveifforeverypoint

,everysubset

hasalargelookout:

Definition2Letandbetwoconstantsinexpansiveifforeveryconnectedsubset,

.Forany

,thesetis

-

-LOOKOUT

Thefreespace

is

-expansiveifforevery

,

is

-expansive.

Tobettergraspthesedefinitions,thinkofinDefinition2astheinitialmilestoneandasthe-reachabilitysetofasetofmilestonessampledbyAlgorithm1.If

and

are

14

bothreasonablylarge,thenAlgorithm1hasagoodchancetosampleanewmilestonewhose-reachabilitysetaddssignificantlytothesizeof.Infact,weshowbelowthatwithhighprobability,the-reachabilitysetofthesampledmilestonesexpandsquicklytocovermostof;hence,ifthegoalliesin,thentheplannerwillquicklyfindanadmissibletrajectorywithhighprobability.

4.2Idealsampling

Tosimplifyourpresentationandfocusonthemostimportantaspectsofourplanner,letusassumefornowthatwehaveanidealsamplerIDEAL-SAMPLEthatpicksapointuniformlyatrandomfromthe-reachabilitysetofexistingmilestones.Ifitissuccessful,IDEAL-SAMPLEreturnsanewmilestoneandatrajectoryfromanexistingmilestoneto.Withidealsampling,theplanningalgorithmcanberestatedasfollows:

1.2.3.4.5.6.7.8.

Insertintoatree;.repeat

InvokeIDEAL-SAMPLE,whichsamplesanewmilestoneandreturnsatrajectoryfromanexistingmilestonetoifthetrajectoryisadmissible.ifnilthenInsertinto.

Createadirectededgefromto,andstorethetrajectorywith.

;.

ifENDGAMEthenexitwithSUCCESS.

Figure6:Asequenceofsampledmilestones.

Lemma1Ifasequenceofmilestones

contains

lookoutpoints,then

.,

bethesubsequenceoflookoutpointsin,whereProof.Let

givetheindicesofthelookoutpointsinthesequence.Foranywehave

(3)

Thus

forall

,inparticular,

(4)

where

isalookoutpoint,weget

.Using(3)with

incombinationwiththefactthat

Let

.Since

,whichcanberewrittenas

,wehave

(5)

Note

(Figure6)andthus

.Itfollowsfrom(5)that

Setting

leadstotherecurrence

,withthesolution

Since

and

,weget

.Combinedwith(4),ityields

Lemma2Asequenceof

.

milestonescontains

lookoutpointswithprobabilityatleast

Proof.Letbeasequenceofmilestones,andbetheeventthatcontainslookoutpoints.WedivideMintosubsequencesofconsecutivemilestoneseach.Denotebytheeventthattheithsubsequencecontainsatleastonelookoutpoint.Sincetheprobabilityofhaving

16

lookoutpointsisgreaterthantheprobabilityofeverysubsequencehavingatleastonelookoutpoint,wehave

PrPr

whichimplies

Pr

Pr

ofhavingnolookoutpointintheithsubsequenceisatmost

.Hence

Pr

Pr

answertothequery,hasprobabilitylessthan.SincetheboundinTheorem1containsonlylogarithmictermsof,theprobabilityofanincorrectanswerconvergesto0exponentiallyinthenumberofmilestones.

TheboundgivenbyTheorem1alsodependsontheexpansivenessparameters,andthevolumeoftheendgameregion.Thegreater,,and,thesmallerthebound.Inpractice,itisoftenpossibletoestablishalowerboundfor.However,andaredifficulttoestimate,exceptforeverysimplecases.Thispreventsusfromdeterminingtheparameter,themaximalnumberofmilestonesneededforAlgorithm1apriori.Neverthelesstheseresultsareimportant.First,theytellusthatthefailureprobabilityofourplannerdecreasesexponentiallywiththenumberofmilestonessampled.Second,thenumberofmilestonesneededincreasesonlymoderatelywhenanddecrease,i.e.,whenthespacebecomeslessexpansive.

4.4ApproximatingIDEAL-SAMPLE

TheaboveanalysisassumestheuseofIDEAL-SAMPLE,whichpicksanewmilestoneuniformlyatrandomfromthe-reachabilitysetoftheexistingmilestones.OnewaytoimplementIDEAL-SAMPLEwouldberejectionsampling[KW86],whichthrowsawayafractionofsamplesinregionsthataremoredenselysampledthanothers.However,rejectionsamplingisnotefficient:manypotentialcandidatesarethrownawayinordertoachievetheuniformdistribution.

Soinstead,ourimplementedplannerstrytoapproximatetheidealsampler.Theapproximationismuchfastertocompute,butgeneratesaslightlylessuniformdistribution.Recallthattosampleanewmilestone,wefirstchooseamilestonefromtheexistingmilestonesandthensampleintheneighborhoodof.Everynewmilestonethuscreatedtendstoberelativelycloseto.Ifweselecteduniformlyamongtheexistingmilestones,theresultingdistributionwouldbeveryuneven;withhighprobability,wewouldpickamilestoneinanalreadydenselysampledregionandobtainanewmilestoneinthatsameregion.Thereforethedistributionofmilestonestendstoclusteraroundtheinitialstatetimepoint.Toavoidthisproblem,weassociatewitheverymilestoneaweight,whichisthenumberofmilestonesinasmallneighborhoodof,andpickanexistingmilestonetoexpandwithprobabilityinverselyproportionalto.Soitismorelikelytosamplearegioncontainingasmallernumberofmilestones.Thedistributioncontributestothediffusionofmilestonesoverthefreespaceandavoidsoversamplinganyparticularregion.

astheroadmapisbeingbuiltincursamuchsmallerIngeneral,maintainingtheweights

computationalcostthanperformingrejectionsampling.

Thereisalsoaslightlygreaterchanceofgeneratinganewmilestoneinanareawherethe-reachabilitysetsofseveralexistingmilestonesoverlap.However,milestoneswithoverlapping-reachabilitysetsaremorelikelytobeclosetooneanotherthanmilestoneswithnosuchoverlap-ping.Thusitisreasonabletoexpectthatusingkeepstheproblemfromworseningasthenumberofmilestonesgrows.

Iftherearenokinodynamicconstraintsontherobot’smotion,thenotherthanthetwoissuesmentionedabove,Theorem1givesanasymptoticboundthatcloselycharacterizestheamountofworkthattheplannermustdoinordertoguaranteefindingatrajectorywithhighprobabilitywheneveroneexists.Inparticular,theresultappliesto(geometric)pathplanningproblems.

Thereis,however,onemoreissuetoconsiderwhenkinodynamicconstraintsarepresent.Al-inthoughline4ofAlgorithm1selectsuniformlyatrandomfrom,thedistributionof

18

isnotuniformingeneral,becausethemappingfromtomaynotbelinear.In

somecases,onemayprecomputeadistributionsuchthatpickingfromwithprobability

yieldsauniformdistributionofin.Inothercases,rejectionsamplingcanbeusedlocally.Firstpickseveralcontrolfunctionsandcomputethecorresponding,theendpointofthetrajectoryinducedby.Thenthrowawaysomeofthemtoachieveauniformdistributionamongtheremaining’s,andpickaremainingatrandom.

4.5Choosingsuitablecontrolfunctions

Tosamplenewmilestones,Algorithm1picksatrandomapiecewise-constantcontrolfunctionfrom.Everyhasatmostconstantpieces,eachofwhichlastsforatimedurationlessthan.Theparametersandarechosenaccordingtospecificpropertiesofeachrobot.Intheory,mustbelargeenoughsothatforany,hasthesamedimensionas.Otherwise,haszerovolumerelativeto,andcannotbeexpansiveevenforarbitrarilysmallvaluesofand.Thiscanonlyhappenwhensomedimensionsof

arenotspanneddirectlybybasisvectorsinthecontrolspace,butthesedimensionscan

thenbegeneratedbycombiningseveralcontrolsinusingLie-brackets[BL93].ThemathematicaldefinitionofaLiebracketcanbeinterpretedasaninfinitesimal“maneuver”involvingtwocontrols.SpanningallthedimensionsofmayrequirecombiningmorethantwocontrolsofbyimbricatingmultipleLiebrackets.AtmostLiebracketsareneeded,whereisthedimensionofthestatespace.Henceitissufficienttochoose.

Ingeneral,thelargeris,thegreaterandtendtobe.Soaccordingtoouranalysis,fewermilestonesareneeded;ontheotherhand,thecostofintegrationandcollisioncheckingduringthegenerationofanewmilestonebecomesmoreexpensive.Thechoiceofissomewhatrelated.Alargermayresultingreaterand,butalsoleadtheplannertointegratelongertrajectoriesthataremorelikelytobeinadmissible.Experimentsshowthatandcanbeselectedinrelativelywideintervalswithoutsignificantimpactontheperformanceoftheplanner.However,ifthevaluesforandaretoolarge,theapproximationtoIDEAL-SAMPLEbecomesverypoor.

5Nonholonomicrobots

WeimplementedAlgorithm1fortwodifferentrobotsystems.Oneconsistsoftwononholo-nomiccartsconnectedbyatelescopiclinkandmovingamongstaticobstacles.Theotherisanair-cushionedrobotthatisactuatedbyairthrustersandoperatesamongmovingobstaclesonaflattable.Theair-cushionedrobotissubjecttostrictdynamicconstraints.Inthissection,wediscusstheimplementationofAlgorithm1forthenonholonomiccarts.Inthenexttwosections,wewilldothesamefortheair-cushionedrobot.

5.1Robotdescription

Wheeledmobilerobotsareaclassicalexamplefornonholonomicmotionplanning.Therobotconsideredhereisanewvariationonthistheme.Itconsistsoftwoindependently-actuatedcartsmovingonaflatsurface(Figure7).Eachcartobeysanonholonomicconstraintandhasnon-zerominimumturningradius.Inaddition,thetwocartsareconnectedbyatelescopiclinkwhose

19

Figure7:Two-cartnonholonomicrobots.Cooperativemobilemanipulators.nonholonomicrobotsthatmaintainadirectlineofsightandadistancerange.

Twowheeled

lengthislowerandupperbounded.Thissystemisinspiredbytwoscenarios.OneisthemobilemanipulationprojectattheUniversityofPennsylvania’sGRASPLaboratory[DK99];thetwocartsareeachmountedwithamanipulatorarmandmustremainwithinacertaindistancerangesothatthetwoarmscancooperativelymanipulateanobject(Figure7).Themanipulationareabetweenthetwocartsmustbefreeofobstacles.Intheotherscenario,twocartspatrollinganindoorenvironmentmustmaintainadirectlineofsightandstaywithinacertaindistancerange,inordertoallowvisualcontactorsimpledirectionalwirelesscommunication(Figure7).Weprojectthegeometryofthecartsandtheobstaclesontothehorizontalplane.For,letbethemidpointbetweentherearwheelsofthethcart,bethemidpointbetweenthefrontwheels,andbethedistancebetweenand.Wedefinethestateofthesystemby

,wherearethecoordinatesof,andistheorientationofthe

rearwheelsofthcartrelativetothe-axis(Figure2).Tomaintainadistancerangebetweenthetwocart,werequire

Figure8:Computedexamplesfornonholonomiccarl-likerobots.

naivemethodthatcheckseverynewmilestoneagainstallthemilestonescurrentlyin.Thus,foreverynewmilestone,updatingtakeslineartimeinthenumberofmilestonesin.Moreefficientrangesearchtechniques[Aga97]wouldcertainlyimprovetheplanner’srunningtimeforproblemsrequiringverylargeroadmaps.

ImplementingPROPAGATEGivenamilestoneandacontrolfunction,PROPAGATEusestheEulermethodwithafixedstepsizetointegrate(6)fromandcomputesatrajectoryofthesystemunderthecontrol.Moresophisticatedintegrationmethods,e.g.,fourth-orderRunge-Kuttaorextrapolationmethod[PTVP92],canimprovetheaccuracyofintegration,butatahighercomputationalcost.

Wethendiscretizeintoasequenceofstatesandreturnsnilifanyofthesestatesisincollision.Foreachcart,weprecomputea3-Dbitmapthatrepresentsthecollision-freeconfigurationsofthecartpriortoplanning.Itthentakesconstanttimetocheckwhetheragivenconfigurationisincollision.Awell-knowndisadvantageofthismethodisthatiftheresolutionofthebitmapisnotfineenough,wemaygetwronganswers.Intheexperimentsreportedbelow,weusedan

bitmap,whichwasadequateforourtestcases.

EndgameregionWeobtaintheendgameregionbygeneratingasecondarytreefromthegoal.

ofmilestones

5.3Experimentalresults

Weexperimentedwiththeplannerinmanyworkspaces.Eachoneisa10m10msquareregionwithstaticobstacles.Thetwocartsareidentical,eachrepresentedbyapolygoncontainedina

m.Thespeedofthecartsrangesfromm/stocirclewithdiameter0.8m,and

m/s,anditssteeringanglevariesbetweenand.Theallowabledistancebetweenandrangesfrommtom.

Figure8showsthreecomputedexamples.Environmentisamaze;therobotcartsmustnavigatefromonesideofittotheother.Environmentcontainstwolargeobstaclesseparatedbyanarrowpassage.Thetwocarts,whichareinitiallyparalleltooneanother,changeformation

21

Time(sec.)mean1.390.740.540.55

3.92

14.090.92

mean62402435643596038384

61836

28782856367

4473

2131615315128151406610739320250

353025201510500123running time (seconds)

45Figure9:Histogramofplanningtimesformorethan100runsonaparticularquery.Theaveragetimeis1.4sec,andthefourquartilesare0.6,1.1,1.9,and4.9seconds.

center,and

isthevelocity.Theequationsofmotionare

whereistherobot’smass,andandarethemagnitudeanddirectionoftheforcegeneratedbythethrusters.Wehavem/sand.Themaximumspeedoftherobotis0.18m/s.

Forplanningpurposes,theworkspaceisrepresentedbya3m4mrectangle,therobotbyadiscofradius0.25m,andtheobstaclesbydiscsofradiibetween0.1and0.15m.Theplannerassumesthattheobstaclemovesalongastraight-linepathatconstantspeedbetweenand

m/s(morecomplextrajectorieswillbeconsideredinSection7.4).Whenanobstaclereachestheworkspace’sboundary,itleavestheworkspaceandisnolongerconsideredathreattotherobot.

6.2Implementationdetails

Theplannerbuildsaroadmapintherobot’s5-Dstatetimespace,Itisgivenaninitialstatetime

andagoalstatetime,whereisanytimelessthanagiven.Inaddition,theplannerisgiventheobstacletrajectories.Unliketheexperimentswiththerealrobotinthenextsection,planningtimeisnotlimitedhere.Thisisequivalenttoassumingthattheworldisfrozenuntiltheplannerreturnsatrajectory.

ComputingtheweightsThe3-Dconfigurationtimespaceoftherobotispartitionedintoan81110arrayofidenticallysizedrectangularboxescalledbins.Whenamilestoneisinsertedin,theplanneraddsittothelistofmilestonesassociatedwiththebininwhichitfalls.Toimplementline3ofAlgorithm1,theplannerfirstpicksatrandomabincontainingatleastonemilestoneandthenamilestonefromwithinthisbin.Bothchoicesaremadeuniformlyatrandom.Thiscorrespondstopickingamilestonewithprobabilityapproximatelyproportionaltotheinverseofthedensityofsamplesintherobot’sconfigurationtimespace(ratherthanits5-Dstatetimespace).Wedidsomeexperimentswithbinsinstatetimespace,buttheresultsdidnotdiffersignificantly.

23

Time(sec)mean0.249

0.285

0.002

std2229

1946

25

T = 0.0 secsT = 11.2 secsT = 22.4 secsT = 33.7 secsT = 44.9 secsT = 0.0 secsT = 9.0 secsT = 20.0 secsT = 30.0 secsT = 39.2 secsT = 0.0 secsT = 8.0 secsT = 16.1 secsT = 24.1 secsT = 32.1 secsFigure10:Computedexamplesfortheair-cushionedrobot.tomovewithconstantlinearvelocity,theymapintocylinders.Thevelocityandaccelerationconstraintsrequireeverysolutiontrajectorytopassthroughasmallgapbetweenthecylinders.Exampleismuchsimpler.Therearetwostationaryobstaclesobstructingthemiddleoftheworkspaceandthreemovingobstacles.Planningtimeiswellbelow0.01second,withanaverageof0.002second.Thenumberofmilestonesisalsosmall,confirmingtheresultofTheorem1thatwhenthespaceisexpansive,Algorithm1isveryefficient.Asintheexperimentsonnonholonomicrobotcarts,therunningtimedistributionoftheplannertendstohavealongandthintailduetolongexecutiontimeinasmallnumberofruns,butoveralltheplannerisveryfast.

25

Figure11:ConfigurationspacefortheexampleinFigure10.

7Experimentswiththerealrobot

Tofurthertesttheperformanceoftheplanner,weconnectedtheplannerdescribedintheprevioussectiontotheair-cushionedrobotinFigure1.Inthesetests,weexaminedthebehaviorofAl-gorithm1runninginreal-timemodeonasystemintegratingcontrolandsensingmodulesoveradistributedarchitectureandoperatinginaphysicalenvironmentwithuncertaintiesandtimedelays.

7.1Testbeddescription

TherobotshowninFigure1isuntetheredandmovesfrictionlesslyonanairbearingona3m4mtable.Gastanksprovidecompressedairforboththeair-bearingandthrusters.AnonboardMo-torolappc2604computerperformsmotioncontrolat60Hz.Obstaclesarealsoonair-bearings,buthavenothrusters.Theyareinitiallypropelledbyhandfromvariouslocationsandthenmovefrictionlesslyonthetableatroughlyconstantspeeduntiltheyreachtheboundaryofthetable,wheretheystopduetothelackofairbearing.

Anoverheadvisionsystemestimatesthepositionsoftherobotandtheobstaclesat60HzbydetectingLEDsplacedonthemovingobjects.Themeasurementisaccurateto5mm.Velocityestimatesarederivedfrompositiondata.

Ourplannerrunsoffboardona333MhzSunSparc10.Theplanner,therobot,andthevisionmodulecommunicateovertheradioEthernet.

7.2Systemintegration

Implementingtheplanneronthehardwaretestbedraisesseveralnewchallenges.

TimedelaysVariouscomputationsanddataexchangesoccurringatdifferentpartsofthesystemleadtodelaysbetweentheinstantwhenthevisionmodulemeasuresthemotionoftherobotandtheobstaclesandtheinstantwhentherobotstartsexecutingtheplannedtrajectory.Thesedelays,

26

ifignored,wouldcausetherobottobeginexecutingtheplannedtrajectorybehindthestarttimeassumedbytheplanner.Therobotmaynotthenbeabletocatchupwiththeplannedtrajectorybeforeacollisionoccurs.Todealwiththisissue,theplannercomputesatrajectoryassumingthattherobotwillstartexecutingit0.4secondintothefuture.Italsoassumesthattheobstaclesmoveatconstantvelocities,asmeasuredbythevisionmodule,andextrapolatestheirpositionsaccordingly.The0.4secondincludesallthedelaysinthesystem,inparticular,thetimeneededforplanning.ThistimecouldbefurtherreducedbyimplementingtheplannermorecarefullyandrunningitonamachinefasterthantherelativelyslowSunSparc10currentlybeingused.

SensingerrorsAlthoughtheplannerassumesthattheobstaclesmovealongstraightlinesatcon-stantvelocitiesmeasuredbythevisionmodule,theactualtrajectoriesareslightlydifferentduetoasymmetryinair-bearingsandinaccuracyinthemeasurements.Theplannerdealswiththeseerrorsbygrowingtheobstacles.Astimeelapses,theradiusofeachmovingobstacleisincreasedby,whereisafixedconstant,isthemeasuredvelocityoftheobstacle,andisthetime.Sotheplannercanavoiderroneouslyassertingthatastatetimepointiscollision-freewhenitisactuallynot.

TrajectorytrackingTherobotreceivesfromtheplanneratrajectorythatspecifiestheposition,velocity,andaccelerationoftherobotatalltimes.APD-controllerwithfeedforwardisusedtotrackthistrajectory.Themaximumtrackingerrorsforthepositionandvelocityare0.05mand0.02m/s,respectively.Asaresult,weincreasethesizeofthediscmodelingtherobotby0.05mduringtheplanningtoguaranteethatthecomputedtrajectoryiscollision-free.

TrajectoryoptimizationSincetheplannerisveryefficientingeneral,the0.4secondallocatedisoftenmorethanwhatisneededforfindingafirstsolution.Sotheplannerexploitstheextratimetogenerateadditionalmilestonesandkeepstrackofthebesttrajectoryfoundsofar.Thecost

,whereisthenumberofsegmentsinthefunctionforcomparingtrajectoriesis

trajectory,isthemagnitudeoftheforceexertedbythethrustersalongthethsegment,isafixedconstant,andisthedurationofthethsegment.Thecostfunctiontakesintoaccountbothfuelconsumptionandexecutiontime.Alargeryieldsfastermotion,whileasmalleryieldslessfuelconsumption.Inourexperiments,thecostoftrajectorieswasreduced,ontheaverage,by14%withthissimpleimprovement.

Safe-modeplanningIftheplannerfailstofindatrajectorytothegoalwithintheallocatedtime,wefounditusefultocomputeanescapetrajectory.Theendgameregionfortheescapetrajectoryconsistsofallthereachable,collision-freestateswithforsometime.Anescapetrajectorycorrespondstoanyacceleration-bounded,collision-freemotionintheworkspaceforasmalldurationoftime.Ingeneral,isverylarge,andsogeneratinganescapetrajectoryoftentakeslittletime.Toensurecollision-freemotionbeyond,anewescapetrajectorymustbecomputedlongbeforetheendofthecurrentescapetrajectorysothattherobotcanescapecollisiondespitetheaccelerationconstraints.Wemodifiedtheplannertocomputesimultaneouslyanormalandanescapetrajectory.Themodificationincreasedtherunningtimeoftheplannerbyabout2%inourexperiments,butitleadstoasystemthatismuchmoreusefulpractically.

27

Figure12:Snapshotsoftherobotexecutingatrajectory.

7.3Experimentalresults

Theplannersuccessfullyproducedcomplexmaneuversoftherobotamongstaticandmovingob-staclesinvarioussituations,includingobstaclesmovingdirectlytowardtherobotorperpendiculartothelineconnectingitsinitialandgoalpositions.Thetestsalsodemonstratedtheabilityofthesystemtowaitforanopeningtooccurwhenconfrontedwithmovingobstaclesintherobot’sde-sireddirectionofmovementandtopassthroughopeningsthatarelessthan10cmlargerthantherobot.Inalmostallthetrials,atrajectorywascomputedwithintheallocatedtime.Figure12showssnapshotsoftherobotduringoneofthetrials,inwhichtherobotmaneuversamongthreeincomingobstaclestoreachthegoalatthefrontcornerofthetable.

Severalproblemslimitedthecomplexityoftheplanningproblemswhichwecouldtryinthistestbed.Twoarerelatedtothetestbeditself.Firsttheaccelerationsprovidedbytherobot’sairthrustersarequitelimited.Secondthesizeofthetableissmallrelativethatoftherobotandtheobstacles,whichlimitstheavailablespacefortherobottomaneuver.Thethirdproblemresultsfromthedesignofoursystem.Theplannerassumesthatobstaclesmoveatconstantlinearveloci-tiesanddonotcollidewithoneother,anassumptionwhichislikelytofailinpractice.Toaddressthislastandimportantissue,weintroduceon-the-flyreplanning.

7.4On-the-flyreplanning

Anobstaclemaydeviatefromitspredictedtrajectory,becauseeithertheerrorinthemeasurementsislargerthanexpected,ortheobstacle’sdirectionofmotionhassuddenlychangedduetoacol-lisionwithotherobstacles.Wheneverthevisionmoduledetectsthis,italertstheplanner.Theplannerthenrecomputesatrajectoryontheflywithinthesameallocatedtimelimit,byprojectingthestateoftheworld0.4secondintothefuture.On-the-flyreplanningallowsmuchmorecomplex

28

T = 2.1 secs

1.51x [meters]x [meters]0.50−0.5−1−1.5−11.510.50−0.5−1−1.5−1T = 14.6 secs

1.51x [meters]0.50−0.5−1−1.5−1T = 19.8 secs

2201x1 [meters]T = 33.2 secs

01x1 [meters]T = 50.2 secs

201x1 [meters]T = 75.0 secs

1.51x [meters]x [meters]0.50−0.5−1−1.5−11.510.50−0.5−1−1.5−1x [meters]01x1 [meters]

1.510.50−0.5−1−1.5−12201x1 [meters]

201x1 [meters]

Figure13:Acomputedexamplewithreplanninginasimulatedenvironment.

experimentstobeperformed.Weshowtwoexamplesbelow,oneinsimulationandoneontherealrobot.

IntheexampleshowsinFigure13,eightreplanningoperationsoccurredovertheentirecourse(75seconds)oftheexperiment.Initiallytherobotmovestothelefttoreachthegoalatthebottommiddle(snapshot1).Thentheupper-leftobstaclechangesitsmotionandblockstherobot’sway,resultinginareplan(snapshot2).Soonafter,themotionoftheupper-rightobstaclealsochanges,forcingtherobottoreversethedirectionandapproachthegoalfromtheothersideoftheworkspace(snapshot3).Intheremainingtime,newchangesinobstaclemotioncausetherobottopause(seethesharpturninsnapshot5)untiladirectapproachtothegoalispossible(snapshot6).

TheefficacyofthereplanningprocedureontherealrobotisdemonstratedbytheexampleinFigure14.Therobot’sgoalistomovefromthebackleftofthetabletothefrontmiddle.Initiallytheobstacleinthemiddleisstationary,andtheothertwoobstaclesaremovingtowardtherobot(snapshot1).Therobotdodgesthefaster-movingobstaclefromtheleftandproceedstowardthegoal(snapshot2).Theobstacleisthenredirectedtwice(insnapshots3and5)toblockthetrajectoryoftherobot,causingittoslowdownandstaybehindtheobstacletoavoidcollision(snapshots3–6).Rightbeforesnapshot7,therightmostobstacleisdirectedbacktowardtherobot.Therobotwaitsfortheobstacletopass(snapshot8)andfinallyattainsthegoal(snapshot9).Theentiremotionlastsabout40seconds.Throughoutthisexperiment,otherreplanningoperations(notshown)occurredasaresultoferrorsinthemeasurementoftheobstaclemotions.However,noneresultedinamajorredirectionoftherobot.

29

Figure14:Anexamplewiththerealrobotusingon-the-flyreplanning.

8Conclusion

Wehavepresentedasimple,efficientrandomizedplannerforkinodynamicmotionplanninginthepresenceofmovingobstacles.Ouralgorithmrepresentsthemotionconstraintsbyanequationof

andconstructsaroadmapofsampledmilestonesinthestatetimespaceoftheform

arobot.Itsamplesnewmilestonesbyfirstpickingatrandomapointinthespaceofadmissiblecontrolfunctionsandthenmappingthepointintothestatespacebyintegratingtheequationsofmotion.Thusthemotionconstraintsarenaturallyenforcedduringtheconstructionoftheroadmap.Thealgorithmisgeneralandcanbeappliedtoawideclassofsystems,includingonesthatarenotlocallycontrollable.Theperformanceofthealgorithmhasbeenevaluatedthroughboththeoreticalanalysisandextensiveexperiments.

Wehavegeneralizedthenotionofexpansiveness,originallyproposedin[HLM97]for(geo-metric)pathplanning.Themainpurposeofthegeneralizationistoaddressthecomplicationsintroducedbykinematicanddynamicconstraints.Usingtheexpansivenesstocharacterizethecomplexityofthestatespace,wehaveproventhat,undersuitableassumptions,thefailureprob-30

abilityoftheplannerconvergeto0atanexponentialrate,whenasolutionexists.Thisresultalsoholdsforrobotsthatarenotlocallycontrollable.

Experimentallytheplannerhasdemonstrateditseffectivenessbothinsimulationandonarealrobot.Theexperimentsontherealrobotindicatesthattheplannerworkswelldespitemanyadversarialconditions,including(i)severedynamicconstraintsonthemotionoftherobot,(ii)movingobstacles,and(iii)varioustimedelaysanduncertaintiesinherenttoanintegratedsystemoperatinginaphysicalenvironment.Inparticular,theydemonstratethattheefficiencyoftheplannerenablesittobeusedinrealtimewhenobstaclestrajectoriesarenotknowninadvance.Inthefuture,weplantoapplytheplannertoenvironmentswithmorecomplexgeometryandrobotswithhigherdofs.Geometricalcomplexityincreasesthecostofcollisionchecking,butasdiscussedinSection2.4,hierarchicalalgorithmscandealwiththisissueeffectively.Infact,asimilar,butsimplerplannerhasbeenusedsuccessfullytocomputegeometricdisassemblypathswithCADmodelshavingupto200,000triangles[HLM99].

Wearealsointerestedinreducingthestandarddeviationofrunningtimesforourrandomizedplanner.Quitepossibly,thethinandlongtailoftherunningtimedistributionshowninFigure9istypicalofallPRMplannersdevelopedsofar.However,itismoreimportanttoreduceitforsingle-queryplanners,becausetheyareintendedtobeusedinteractivelyorinrealtime.Largestandarddeviationsinthesesettingsareclearlyundesirable.

Moreimportantly,weneedtofurtherdeveloptoolstoanalyzetheefficiencyofrandomizedmotionplanners.Thenotionofexpansivenessisastepforwardinthatdirection.However,thepa-rameterscharacterizinganexpansivespacecannotbeeasilydetermined,andsowecannotdecide,inadvance,thenumberofmilestonesneededforagivenquery.Itisimportanttocontinuelookingfornewanalysistools;ifwecannotmeasuretheperformanceofthesealgorithmsquantitatively,wewillnotbeabletocompare,improve,andthusadvanceourunderstandingofthem.

Acknowledgments:ThisworkwassupportedbyAROMURIgrantDAAH04-96-1-007,NASATRIWGCoop-AgreementNCC2-333,Real-TimeInnovations,andtheNISTATPprogram.DavidHsuhasalsobeentherecipientofaMicrosoftGraduateFellowship,andRobertKindel,therecipientofanNSFGraduateFellowship.

References

[ABD98]N.M.Amato,O.B.Bayazit,L.K.Dale,C.Jones,andD.Vallejo.OBPRM:An

obstacle-basedPRMfor3Dworkspaces.InP.K.Agarwaletal.,editors,Robotics:TheAlgorithmicPerspective:1998WorkshopontheAlgorithmicFoundationsofRobotics,pages155–168.A.K.Peters,Wellesley,MA,1998.

[AG99]

J.M.AhuactzinandK.K.Gupta.Thekinematicroadmap:Amotionplanningbasedglobalapproachforinversekinematicsofredundantrobots.IEEETransactionsonRoboticsandAutomation,15(4):653–669,1999.

P.K.Agawal.Rangesearching.InJ.E.GoodmanandJ.O’Rourke,editors,Handbookofdiscreteandcomputationalgeometry,pages575–598.CRCPress,BocaRaton,FL,1997.

[Aga97]

31

[Ahu94]

J.M.Ahuactzin.LeFild’Ariane.Unem´ethodedeplanificationg´en´eral.Applica-tiona`laplanfificationdestrajectoires.PhDthesis,NationalPolytechnicInstituteofGrenoble,France,1994.

J.E.Bobrow,S.Dubowsky,andJ.S.Gibson.Time-optimalcontrolofroboticmanip-ulatorsalongspecifiedpaths.InternationalJournalofRoboticsResearch,4(3):3–17,1985.

R.BohlinandL.E.Kavraki.PathplanningusinglazyPRM.InProc.IEEEInt.Conf.onRoboticsandAutomation,2000.

[BDG85]

[BK00]

[BKL97]J.Barraquand,L.Kavraki,J.C.Latombe,T.-Y.Li,R.Motwani,andP.Raghavan.

Arandomsamplingschemeforpathplanning.InternationalJournalofRoboticsResearch,16(6):759–774,1997.

[BL89][BL91][BL93]

J.BarraquandandJ.C.Latombe.Onnonholonomicrobotsandoptimalmaneuvering.Revued’IntelligenceArtificielle,3(2):77–103,1989.

J.BarraquandandJ.C.Latombe.Robotmotionplanning:Adistributedrepresentationapproach.InternationalJournalofRoboticsResearch,10(6):628–649,1991.J.BarraquandandJ.C.Latombe.Nonholonomicmultibodymobilerobots:Control-labilityandmotionplanninginthepresenceofobstacles.Algorithmica,10(2-4):121–155,1993.

[BOvdS99]V.Boor,M.H.Overmars,andF.vanderStappen.Thegaussiansamplingstrategyfor

probabilisticroadmapplanners.InProc.IEEEInt.Conf.onRoboticsandAutomation,pages1018–1023,1999.[CL95]

H.ChangandT.-Y.Li.Assemblymaintainabilitystudywithmotionplanning.InProc.IEEEInt.Conf.onRoboticsandAutomation,pages1012–1019,1995.

[CLMP95]J.Cohen,M.Lin,D.Manocha,andM.Ponamgi.I-Collide:Aninteractiveandexact

collisiondetectionsystemforlargescaleenvironments.InProc.ACMInteractive3DGraphicsConf.,pages189–196,1995.[DK99][Don87]

J.P.DesaiandV.Kumar.Motionplanningforcooperatingmobilemanipulators.Jour-nalofRoboticSystems,16(10):557–579,1999.

B.R.Donald.Asearchalgorithmformotionplanningwithsixdegreesoffreedom.ArtificialIntelligence,31(3):295–353,1987.

[DXCR93]B.R.Donald,P.Xavier,J.Canny,andJ.Reif.Kinodynamicmotionplanning.Journal

oftheACM,40(5):1048–1066,1993.[ELP86]

M.ErdmannandT.Lozano-P´erez.Onmultiplemovingobjects.InProc.IEEEInt.Conf.onRoboticsandAutomation,pages1419–1424,1986.

32

[ELP87][Fer98][Fra93]

M.ErdmannandT.Lozano-P´erez.Onmultiplemovingobjects.Algorithmica,2(4):477–521,1987.

P.Ferbach.Amethodofprogressiveconstraintsfornonholonomicmotionplanning.IEEETransactionsonRoboticsandAutomation,14(1):172–179,1998.

T.Fraichard.Dynamictrajectoryplanningwithdynamicconstraints:A“state-timespace”approach.InProc.IEEE/RSJInternationalConferenceonIntelligentRobotsandSystems,volume2,pages1394–1400,1993.

P.FioriniandZ.Shiller.Timeoptimaltrajectoryplanningindynamicenvironments.InProc.IEEEInt.Conf.onRoboticsandAutomation,pages1553–1558,1996.K.Fujimura.Time-minimumroutesintime-dependentnetworks.IEEETransactionsonRoboticsandAutomation,11(3):343–351,1995.

S.Gottschalk,M.Lin,andD.Manocha.OBB-Tree:Ahierarchicalstructureforrapidinterferencedetection.InSIGGRAPH96ConferenceProceedings,pages171–180,1996.

R.HermannandA.J.Krener.Nonlinearcontrollabilityandobservability.IEEETrans-actionsonAutomaticControl,22(5):728–740,1977.

[FS96][Fuj95][GLM96]

[HK77]

[HKL98]D.Hsu,L.Kavraki,J.C.Latombe,R.Motwani,andS.Sorkin.Onfindingnar-rowpassageswithprobabilisticroadmapplanners.InP.K.Agarwaletal.,editors,Robotics:TheAlgorithmicPerspective:1998WorkshopontheAlgorithmicFounda-tionsofRobotics,pages141–154.A.K.Peters,Wellesley,MA,1998.

[HKLR00]D.Hsu,R.Kindel,J.C.Latombe,andS.Rock.Randomizedkinodynamicmotion

planningwithmovingobstacles.InB.R.Donaldetal.,editors,AlgorithmicandComputationalRobotics:NewDirections:TheFourthInternationalWorkshopontheAlgorithmicFoundationsofRobotics,pages247–264.A.K.Peters,Wellesley,MA,2000.[HLM97]

D.Hsu,J.C.Latombe,andR.Motwani.Pathplanninginexpansiveconfigurationspaces.InProc.IEEEInt.Conf.onRoboticsandAutomation,pages2719–2726,1997.

D.Hsu,J.C.Latombe,andR.Motwani.Pathplanninginexpansiveconfigurationspaces.InternationalJournalofComputationalGeometry&Applications,9(4&5):495–512,1999.

[HLM99]

[HLMK99]D.Hsu,J.C.Latombe,R.Motwani,andL.E.Kavraki.Capturingtheconnectivityof

high-dimensionalgeometricspacesbyparallelizablerandomsamplingtechniques.InP.M.PardalosandS.Rajasekaran,editors,Advancesinrandomizedparallelcomput-ing,pages159–182.KluwerAcademicPublishers,Boston,MA,1999.

33

[HST94]

T.Horsch,F.Schwarz,andH.Tolle.Motionplanningformanydegreesoffreedom—randomreflectionsatC-Spaceobstacles.InProc.IEEEInt.Conf.onRoboticsandAutomation,pages3318–3323,1994.

D.Hsu.RandomizedSingle-queryMotionPlanninginExpansiveSpaces.PhDthesis,Dept.ofComputerScience,StanfordUniversity,Stanford,CA,May2000.

P.M.Hubbard.Approximatingpolyhedrawithspheresfortime-criticalcollisionde-tection.ACMTransactionsonGraphics,15(3):179–210,1996.

[Hsu00][Hub96]

[HXCW98]Y.K.Hwang,P.G.Xavier,P.C.Chen,andP.A.Watterberg.Motionplanningwith

SANDROSandtheconfigurationspacetoolkit.InK.K.GuptaandA.P.delPobil,editors,PracticalMotionPlanninginRobotics,pages55–77.JohnWiley&Sons,1998.[Kav94]

L.E.Kavraki.RandomNetworksinConfigurationSpaceforFastPathPlanning.PhDthesis,Dept.ofComputerScience,StanfordUniverity,Stanford,CA,December1994.

[KHLR00]R.Kindel,D.Hsu,J.C.Latombe,andS.Rock.Kinodynamicmotionplanningamidst

movingobstacles.InProc.IEEEInt.Conf.onRoboticsandAutomation,pages537–543,2000.[KHM98]J.Klosowski,M.Held,J.S.B.Mitchell,H.Sowizral,andK.Zikan.Efficientcolli-siondetectionusingboundingvolumehierarchiesofk-dops.IEEETransactionsonVisualizationandComputerGraphics,4(1):21–37,1998.

[Kin01]

R.Kindel.MotionPlanningforFree-FlyingRobotsinDynamicandUncertainEn-vironments.PhDthesis,Dept.ofAeronautics&Astronautics,StanfordUniversity,Stanford,CA,October2001.

L.Kavraki,M.N.Kolountzakis,andJ.C.Latombe.Analysisofprobabilisticroadmapsforpathplanning.IEEETransactionsonRoboticsandAutomation,14(1):166–171,1998.

[KKL98]

[KLMR95]L.Kavraki,J.C.Latombe,R.Motwani,andP.Raghavan.Randomizedqueryprocess-inginrobotpathplanning.InProc.ACMSymposiumonTheoryofComputing,pages353–362,1995.[KPLM98]S.Krishnan,A.Pattekar,M.Lin,andD.Manocha.Sphericalshell:Ahigherorder

boundingvolumeforfastproximityqueries.InRobotics:TheAlgorithmicPerspec-tive:1998WorkshopontheAlgorithmicFoundationsofRobotics,pages177–190,1998.ˇˇ[KSLO96]L.Kavraki,P.Svestka,J.C.Latombe,andM.H.Overmars.Probabilisticroadmaps

forpathplanninginhigh-dimensionalconfigurationspace.IEEETransactionsonRoboticsandAutomation,12(4):566–580,1996.

34

[Kuf99][KW86][KZ86][Lat99][Lau86]

[LCH89]

[LH00]

[LJTM94]

[LK99][LK01][LL96][LM96][O’R97]

[PTVP92]

J.J.Kuffner.AutonomousAgentsforReal-TimeAnimation.PhDthesis,Dept.ofComputerScience,StanfordUniversity,Stanford,CA,December1999.

M.H.KalosandP.A.Whitlock.MonteCarloMethods,volume1.JohnWiley&Son,NewYork,1986.

K.KantandS.W.Zucker.Towardefficienttrajectoryplanning:Thepath-velocitydecomposition.InternationalJournalofRoboticsResearch,5(3):72–89,1986.J.C.Latombe.Motionplanning:Ajourneyofrobots,molecules,digitalactors,andotherartifacts.InternationalJournalofRoboticsResearch,18(11):1119–1128,1999.J.-P.Laumond.Feasibletrajectoriesformobilerobotswithkinematicandenviron-mentalconstraints.InProc.Int.Conf.onIntelligentAutonomousSystems,pages346–354,1986.

Z.Li,J.F.Canny,andG.Heinzinger.Robotmotionplanningwithnonholonomicconstraints.InH.Miuraetal.,editors,RoboticsResearch:TheFifthInternationalSymposium,pages309–316.MITPress,Cambridge,MA,1989.

P.LevenandS.Hutchinson.Towardreal-timepathplanninginchangingenviron-ments.InB.R.Donaldetal.,editors,AlgorithmicandComputationalRobotics:NewDirections:TheFourthInternationalWorkshopontheAlgorithmicFoundationsofRobotics,pages363–376.A.K.Peters,Wellesley,MA,2000.

J.-P.Laumond,P.E.Jacobs.,M.Ta¨ıx.,andR.M.Murray.Amotionplannerfornonholonomicmobilerobots.IEEETransactionsonRoboticsandAutomation,10(5):577–593,1994.

S.M.LaValleandJ.J.Kuffner.Randomizedkinodynamicplanning.InProc.IEEEInt.Conf.onRoboticsandAutomation,pages473–479,1999.

S.M.LaValleandJ.J.Kuffner.Randomizedkinodynamicplanning.InternationalJournalofRoboticsResearch,20(5):278–400,May2001.

F.LamirauxandJ.-P.Laumond.Ontheexpectedcomplexityofrandompathplan-ning.InProc.IEEEInt.Conf.onRoboticsandAutomation,pages3014–3019,1996.K.M.LynchandM.T.Mason.Stablepushing:Mechanics,controllability,andplan-ning.InternationalJournalofRoboticsResearch,15(6):533–556,1996.

J.O’Rourke.Visibility.InJ.E.GoodmanandJ.O’Rourke,editors,Handbookofdiscreteandcomputationalgeometry,pages467–479.CRCPress,BocaRaton,FL,1997.

W.H.Press,S.A.Teukolsky,W.T.Vetterling,andB.P.Plannery.NumericalRecipesinC.CambridgeUniversityPress,2ndedition,1992.

35

[Qui94][Rei79][RS85][RS90][SA02][SD91]

[SL98]

[SLL01]

[SMA01][SO94]

ˇ[SO98]

ˇ[SSLO97]

ˇ[Sve97]

ˇS.Quinlan.Efficientdistancecomputationbetweennon-convexobjects.InProc.IEEEInt.Conf.onRoboticsandAutomation,pages3324–3329,1994.

J.H.Reif.Complexityofthemover’sproblemandgeneralizations.InProc.IEEESymposiumonFoundationsofComputerScience,pages421–427,1979.

J.H.ReifandM.Sharir.Motionplanninginthepresenceofmovingobstacles.InProc.IEEESymposiumonFoundationsofComputerScience,pages144–154,1985.J.A.ReedsandL.A.Shepp.Optimalpathsforacarthatgoesforwardsandbackwards.PacificJournalofMathematics,145(2):367–393,1990.

G.S´anchez-Ante.Single-QueryBi-DirectionalMotionPlanningwithLazyCollisionChecking.PhDthesis,ITESM,CampusCuernavaca,Mexico,2002.

Z.ShillerandS.Dubowsky.Oncomputingtheglobaltime-optimalmotionsofroboticmanipulatorsinthepresenceofobstacles.IEEETransactionsonRoboticsandAu-tomation,7(6):785–797,1991.

S.SekhavatandJ.-P.Laumond.Topologicalpropertyforcollision-freenonholonomicmotionplanning:Thecaseofsinusoidalinputsforchainedformsystems.IEEETransactionsonRoboticsandAutomation,14(5):671–680,1998.

T.Sim´eon,J.P.Laumond,andF.Lamiraux.Move3D:Agenericplatformformotionplanning.InProc.IEEEInternationalSymposiumonAssemblyandTaskPlanning,2001.

G.Song,S.Miller,andN.Amato.CustomizingPRMroadmapsatquerytime.InProc.IEEEInt.Conf.onRoboticsandAutomation,2001.

P.SvestkaˇandM.H.Overmars.Motionplanningforcar-likerobotsusingaprob-abilisticlearningapproach.TechnicalReportRUU-CS-94-33,Dept.ofComputerScience,UtrechtUniversity,Utrecht,TheNetherlands,1994.

P.SvestkaˇandM.H.Overmars.Probabilisticpathplanning:Robotmotionplanningandcontrol.InLectureNotesinControlandInformationSciences,volume229,pages225–304.Springer-Verlag,1998.

S.Sekhavat,P.Svestka,ˇJ.-P.Laumond,andM.H.Overmars.Multi-levelpathplan-ningfornonholonomicrobotsusingsemi-holonomicsubsystems.InJ.-P.Laumond

etal.,editors,AlgorithmsforRoboticMotionandManipulation:1996WorkshopontheAlgorithmicFoundationsofRobotics,pages79–96.A.K.Peters,Wellesley,MA,1997.

P.Svestka.ˇRobotMotionPlanningUsingProbabilisticRoadmaps.PhDthesis,Dept.ofComputerScience,UtrechtUniversity,TheNetherland,1997.

36