/
Navigation in ThreeDimensional Cluttered Environments for Mobile Manipulation Armin Hornung Navigation in ThreeDimensional Cluttered Environments for Mobile Manipulation Armin Hornung

Navigation in ThreeDimensional Cluttered Environments for Mobile Manipulation Armin Hornung - PDF document

marina-yarberry
marina-yarberry . @marina-yarberry
Follow
406 views
Uploaded On 2015-03-07

Navigation in ThreeDimensional Cluttered Environments for Mobile Manipulation Armin Hornung - PPT Presentation

Gil Jones Maren Bennewitz Maxim Likhachev Sachin Chitta Abstract Collisionfree navigation in cluttered environ ments is essential for any mobile manipulation system Tra ditional navigation systems have relied on a 2D grid map pro jected from a 3D r ID: 42191

Gil Jones Maren Bennewitz

Share:

Link:

Embed:

Download Presentation from below link

Download Pdf The PPT/PDF document "Navigation in ThreeDimensional Cluttered..." is the property of its rightful owner. Permission is granted to download and print the materials on this web site for personal, non-commercial use only, and to display it on your personal computer provided you do not modify the materials and that you retain all copyright notices contained in the materials. By downloading content from our website, you accept the terms of this agreement.


Presentation Transcript

manipulationtasks,e.g.,taskswherearobotneedstocarrylargeobjectsinclutteredenvironments.Anotherimportantcontributionistheintegrationofanefcient3Drepresenta-tionforlarge-scaleindoorenvironmentswithfastrealtimemotionplanning,implementedandvalidatedonarealrobotoperatingwithnoisysensordatainaclutteredenvironment.Ourapproachprovidestherstimplementationofefcientnavigationplanningformobilemanipulationsystemsofdifferentshapesinarbitrarycongurations.Wevalidateourapproachthroughreal-worldexperimentsonthePR2mobilemanipulationrobot.Usingourapproach,thePR2isabletocarrylargeobjectsinaclutteredindooren-vironment.Itisabletonavigatecollision-freeandefciently,transportingobjectstopartsoftheworkspaceunreachablewithtraditional2Dnavigationplanningapproaches.OurframeworkbuildsuponandextendsthecapabilitiesoftheSearch-basedPlanningLibrary(SBPL,[2])andisavailableasopensourcesoftware1.II.RELATEDWORKNavigationinclutteredenvironmentsisawell-studiedproblemformobilerobots[1],[3],[4],[5].Mostapproachestothisproblem,however,havefocusedonnavigationfora2Dprojectedfootprintoftherobotmovinginaprojected2Drepresentationoftheworld.Therefore,theyareunsuitedforourmotivatingproblemofmobilemanipulationinaclutteredenvironmentsincetheywillrejectanycongurationoftherobotwherethe2Dprojectedfootprintoftherobotisincollisionwiththe2Denvironmentrepresentation.HornungandBennewitz[6]recentlyproposedanapproachforefcienthumanoidrobotnavigationbycombiningcoarse2Dpathplanninginopenspacesanddetailedfootstepplanninginthevicinityofobstacles.Thistechniqueallowsforndingsolutionswhereplanningbasedona2Dgridfails.LazycollisionchecksandanenlargedrobotmodelwereusedforfastRRT-basedmotionplanninginasimulatedkitchenenvironmentbyVahrenkampetal.[7].Theauthorsconcentratedonmanipulationtasksinasmallpartoftheenvironment.Theylaterextendedtheapproachtocoarsemotionplanningforthebasewhilefurtherawaywithanincreasedplannergranularityclosertothegoal[8].How-ever,theirapproachdoesnotcoverlongnavigationplansthroughaclutteredenvironment.Also,randomizedplanningapproachesgenerallygeneratenon-optimalsolutionsandrelyheavilyonsmoothingtoshortenthenalpath.Marder-Eppsteinetal.[1]useacompact3Drepresentationoftheenvironmentforalongrunningnavigationdemonstra-tion.Thisrepresentationallowedforquickupdatesfordy-namicobstaclesbutstillrestrictedthesystemtooperateonlyinaprojected2Denvironmentrepresentation.CartpushingwithamobilemanipulationsystemwasdemonstratedbyScholzetal.[9]butnavigationwasagainlimitedtogoalswherethe2Dprojectionofthecartandtherobotwasnotincollision.Thisdisallowed,forexample,actionsthatcouldmoveandstorethecartunderatable.1AspartoftheRobotOperatingSystem(ROS)athttp://www.ros.org/wiki/3d_navigationForefcientcollisioncheckingin3D,severalhierarchicalapproacheshavebeenproposedinthepastsuchasorientedboundingboxtrees[10]orspheretreehierarchies[11],[12].Theyallrelyonaspatialsubdivisionofthe3Dspace.Incontrasttotheseapproaches,ourcollisionchecksarerstperformedinamulti-layered2Dmapandweemployfull3Dcollisionchecksonlywhenrequired.Lauetal.[13]recentlyintroducedtechniquesforincre-mentalupdatesofcollisionmapsinanon-circularrobot'scongurationspace.Afterconvolvingamapwiththedis-cretizedshapeoftherobotinapre-computationstep,colli-sioncheckscorrespondtoasinglelookup.Whilethemapcanbeefcientlyupdatedfordynamicenvironments,anychangeintherobot'skinematicconguration(e.g.afterpickingupobjects)requiresafullrecomputation.III.THEPR2ROBOTPLATFORMWeimplementedoursystemonthePR2,whichisamobilemanipulationsystemequippedwithanomnidirectionalbase,anarticulatedsensorheadandtwo7-DoFarms.ThesensorsusedthroughoutthisworkaretheHokuyoUTM-30LX2Dlaserrangenderinthebaseforlocalizationandthestereosensorinthehead.Thestereocamerasareaugmentedbyatextureprojectortoprovidedensedata[14].Calibratedjointencodersprovideaccurateproprioceptivedataabouttherobot'skinematicconguration.Thestereosensorservesasthemainsensinginputfordetectingobstaclesintheenvironmentforourapproach.ThePR2'sdensestereopairhasanarroweldofviewof55.Tomakethatpracticalfornavigation,weimple-mentedanactivesensingbehaviorwheretherobot'ssensorheadalwayspointsinthecurrentdrivingdirection(whichmaybetothesideorevenbackwards).Thesensingpipelineforourapproachbuildsonexistingcomponentsdevelopedforatabletopmanipulationapplica-tion[15].ThedensedepthmeasurementsofthePR2'sstereoheadarerstrunthroughaselflter.Basedontheknownrobotmeshmodelandproprioception,pointsontherobot'sbodyoronobjectsthatarebeingmanipulatedorcarriedareremoved.AccuratetimesynchronizationbetweenstereoandproprioceptivedataonthePR2ensuresaneffectiveselfltering.Ashadowlterthenremovesallerrorsinherenttostereosensing:pointsthatareoccludedfromoneofthestereopair'scamerasandveilingpointsonsharpedgesandcornersofthejoints.Finally,aRANSAC-basedground-planedetectionlabelstheremainingpointsaseithergroundornotgroundforinsertionintotheoctree-based3Doccupancymapdescribedindetailnext.IV.ENVIRONMENTREPRESENTATIONMobilemanipulationrequirestheabilitytorepresentandprocessafairlylargeenvironmentefcientlyandaccurately.Therepresentationmustbecompactandeasytoincremen-tallyupdatetoaccountfordynamicobstaclesandchangingscenes. thetable).Usingmultiple2Dlayersinthismannerallowsmotionplannerstoavoidexpensive3Dcollisionchecking.AnexampleisillustratedinFig.3wheretherobothasitsarmsoveratableandthebaseunderatable.Whenusingonesingle2Dmapwitha2Dprojectedfootprint,thedown-projectedtablewillcollidewiththedown-projectedarmsandbase.Afull3Dcollisioncheckwouldbeneededtoconrmwhetheracollisionisactuallyoccurring.Withourapproach,however,thetableisonlyprojectedonthespinelayer(whichitdoesn'tcollidewith)andthearmandbasefootprintsarenotcomparedagainstit.Sincenoneofthefootprintsareincollision,anexpensivefull3Dcheckisnotneeded.Hence,usingmultiplelayersallowsustodeclaretherobotcongurationcollision-freemoreoftenwithoutusingexpensive3Dcollisionchecking.Butwemayalsobeabletodeterminethattherobotisdenitelyina3Dcollisionwithouthavingtodoanexplicit3Dcollisioncheck.Thisisexplainedinthefollowing.Somelayershavethepropertythatforeach(x;y)cellinthecorrespondingfootprintandeachziinthelayer,(x;y;zi)ispartoftherobot.Thisapplies,e.g.,tothebox-likebaseofthePR2andthetallspine.Inthesecases,ifa2Dprojectionofanobstacleisinsidetherobot'sfootprintattheappropriatelayer,therobotisdenitelyincollisionwiththeobjectin3D,regardlessofthezcoordinateanda3Dcheckisnotneeded.However,forcertainlayerssuchasthearmlayer,thispropertycannotbeusedbecausetheyarenotbox-likeandoftenchangetheirshape.Insomecases,wecanstilleliminate3Dchecksbyintroducingtheconceptofatallobstacle.Amapcell(x;y)ismarkedasatallobstacleifall(x;y;zi)cellswithinthelayerareoccupied.Now,whenthe2Dfootprintforthatlayerisincollisionwiththiscell,the3Dcongurationwillalwaysbeincollisionandafull3Dcollisioncheckisnotneeded.Notethatweapplyourmulti-layered2Dobstaclemapforrulingoutfull3Dcollisionchecks,nottoreplacethemcompletely.Inordertopreservethefullexibilityofanarbi-traryrobotconguration,itmaybestillnecessarytotestitskinematiccongurationfor3Dcollisions.Full3Dcollisionchecking,whennecessary,isperformedusingacollisionmodelrepresentationintheODEsimulationpackage[17].V.PLANNING&NAVIGATIONFRAMEWORKThe3Dmapandthemulti-layered2Dobstaclemapsserveasinputtoourplanningandnavigationframework.Weapplyaglobalplannertoconstructaplaninthepositionandplanarorientationspace(x;y;)toreachthegoal.Thisplanisthenexecutedbyalocalplanner.Tolocalizetherobotduringnavigation,wecurrentlyrelyon2DMonteCarlolocalizationbasedonlaserdata.Thislocalizationusesastaticmap,whichcontainswallsandotherstaticpartsoftheenvironment,combinedwithodometryinformationfromthewheelsandanIMUlocatedontherobot.A.GlobalPlanning1)Search-basedPlanningonLatticeGraphs:Theglobalplanneroperatesonalatticegraph[18],[19]corresponding Fig.4.Omni-directionalmotionprimitivesforthePR2.tothe2Dspacewithorientations(x;y;).Eachstateisconnectedtoneighborsresultingfromapplyingasetofmo-tionsprimitives.Motionprimitivesareshort,kinematicallyfeasiblemotionsequencesandthepathfoundbytheplannerisaconcatenationofthesemotions.Theadvantageofthisstatespacerepresentationisthattheresultingplanstendtobesmoothpathsthatcanhandlenon-holonomicconstraintsandnon-circularrobotfootprints.Fig.4showsthesetofmotionprimitivesweuseforthePR2,includingsidewaysandbackwardsmotionsinadditiontodrivingforwardandturning.Forefcient2Dcollisionchecking,wecomputease-quenceoffootprintcollisioncellsforeachmotionprimi-tivebyrollingouttheprojectedrobotfootprintalongtheprimitive'spath.Thissteptookapproximately3secondsinourtrialsandisonlynecessaryiftherobotcongurationchanges.Forknowncongurations,itcanbeprecomputed.Onthelatticegraph,wethenemploytheAnytimeRe-pairingA*(ARA*)search[20].ARA*runsaseriesofweightedA*searchesthatinatetheheuristiccomponentby"1whileefcientlyreusingpreviousinformation.Thesearchstartsoutwithalarge",causingittondanon-optimalinitialsolutionquickly.Then,astimeallows,thesearchrerunswithincrementallylowervaluesfor"whilereusingmuchoftheinformationfrompreviousiterations.Givenenoughtime,ARA*nallysearcheswith"=1,producinganoptimalpath.Iftheplannerrunsoutoftimebefore,thecostofthebestsolutionfoundisguaranteedtobenoworsethan"timestheoptimalsolutioncost.ThisallowsARA*tondsomesolutionfasterthanregularA*,andapproachoptimalityastimeallows.ARA*checksthestatesgeneratedateverystepintheplanningprocessforcollisionsusingthemulti-layered2Dmaprepresentationpresentedabove.A3Dcollisioncheckisperformedonlywhenapossiblecollisionisindicatedin2D.DuringtheARA*search,thecostsforapplyingamotionprimitivecorrespondtothelengthofthetrajectoryandadditionallydependontheproximitytoobstacles.Theheuristicfortheplannerusesa2DDijkstrasearchfromthegoalstate.Thisheuristiconlysearchesoverthe2Dgridmapofthebaselayerwithobstaclesinatedbythebaseinnercircle.Sinceweonlysearchthebaselayerwhere2Dcollisionsimply3Dcollisions,theheuristicstaysadmissibleandconsistent.B.PlanExecutionDuringexecution,theconcatenateddiscretemotionprimi-tivesfromtheglobalplannerhavetobeconvertedintomotorcommandsfortherobot'sbase.Additionally,thevalidity Fig.5.Successratefor60planningproblemsinclutteredspace.Thepercentageofsolvedproblemsafteracertaintimeisshownfortherstsolution(left)andforthenal,optimalsolution(right). chairs Fig.6.Theplanningenvironmentwithsixrobotcongurationschosenasstartandgoalposes.Anarrowpassageisformedbyatableandtwochairs,whichwereremovedtocreateasecond,easierscenario.oftheplanhastobecheckedwhileitisbeingexecutedbecauseobstaclepositionsmightchange.Todoso,thelocalplannercomputestheomnidirectionalvelocitiesrequiredtoreachthenext(x;y;)posealongthepathandperformsasingletrajectoryrolloutwhichischeckedforcollisionsintheupdatedobstaclemap.Asintheglobalplanner,collisionchecksarerstperformedin2D,andonlyin3Dwhenrequired.Incasecollisionsarepredicted,thelocalplannerrsttriestoscaledownthevelocitycommandtoreachthetargetposeascloseaspossible.Forexample,whendockingwithatable,afulldiscreteforwardmotionmightcollidewiththetablewhileashorter,slowapproachingmotionreachesthetablemoreclosely.Incasetheenvironmenthaschangedortherobothasdriftedfromitspath,therobotstopsandtheglobalplannerisinvokedagaintondanewpatharoundtheobstacle.VI.EXPERIMENTSWecarriedoutextensiveexperimentstoevaluateourapproachanddemonstrateitsusabilityforrealtimemobilemanipulationtasks.A.OfineMotionPlanningTestsTherstsetofexperimentswasdesignedtoshowthesuperiorperformanceofourapproachcomparedtoconven-tionaltechniques.Wegeneratedmultipleplanningproblemsinwhichtherobotwasposedinacongurationwithex-tendedarmsformanipulation.Thiscongurationrequirestousealarge2Dprojectedfootprintfortraditionalmotionplanning.Weevaluatedthefollowingthreedifferentapproaches:1)Single-layer2D:Atraditionalnavigationapproachusingonlyone2Dprojectedfootprintoftherobotanda2Dprojectedenvironmentmap.2)Single-layer3D:Anavigationapproachwhere3Dcol-lisionchecksarealwaysperformedwhenthe(single)2Dfootprintisincollisionwiththe2Dprojectedmap.3)Multi-layer3D:Ourapproachusingamulti-layered2Doccupancygridandperforming3Dcollisionchecksonlywhenabsolutelynecessary.Weperformedexperimentsintwodifferentscenarios.TherstscenarioisshowninFig.6.Itincludesanarrowpassagewaybetweentwochairsandatablethattherobotcouldonlynegotiatebymovingsidewayswithitsbaseunderthetable.Forthesecondscenariothechairswereremovedtomaketheplanningproblemeasier.NotethattheenvironmentisarealclutteredofcespaceandallthesensordatausedforgeneratingtheenvironmentrepresentationcamefromarealPR2robot.Sixdifferentstates(Fig.6)werechosenmanuallyandplanningwascarriedoutbetweeneachcombinationofstartandgoalfromthesestates.Thisresultsinatotalof60differentplanningproblemsacrossbothscenarios.Planningwasstartedwith"=10andallowedtocontinueuntileither"=1or5minuteshadpassed.ThesuccessrateforeachplanningapproachisshowninFig.5.NotethatonlytwoofthecongurationsinFig.6have2Dfootprintsthatarenotincollision,enablingthetraditionalSingle-layer2Dapproachtosucceedinonlyfourcases.Ourapproach(Multi-layer3D)alwayssucceededingen-eratingamotionplan,eveninthemostclutteredscenarios.ItwasabletoperformfarbetterthantheSingle-layer3Dapproachandfoundfarmoresolutionsbothinashortreal-timewindowandinthelongrun(seeFig5).Forourapproach,arstsolutionwasalwaysfoundwithinatmost7.3s.TableIcomparesourapproachandSingle-layer3Dinthe32planningattemptswherebothsucceeded.DatafromSingle-layer2Disomittedsinceitisonlyabletoplanbetweentwocongurations.Theresultsmakeitclearthatusingamulti-layeredrepresentationseriouslyimprovestheperformanceofthemotionplannerallowingittondaninitialsolutionveryquickly.Therenementofthesolutiontoafullyoptimalsolution("=1:0)takesmuchmoretimebutinpracticethesub-optimalsolutionsalreadyleadtoanefcientnavigationbehavioroftherobot.TableIIshowsdataaggregatedfromthemuchharder28planningattemptsforwhichonlyourapproach(Multi-layer3D)succeeded.InTableIandII,thenumberof2Dand3Dcollisionchecksreferstothenumberofmotionprimitivesthathadtobechecked.Amotionprimitivemayhaveseveralinterme-diatepositionsinadditiontothestartandendstate.Eachofourprimitivesmayrequireupto10collisionchecksforindividualrobotpositions.Theactualnumberofcollision TABLEIMEANANDSTANDARDDEVIATIONFORTHE32PLANNINGATTEMPTSONWHICHBOTH3DAPPROACHESSUCCEEDED. Single-layer3DMulti-layer3D Timetorstsolution[s]130.810.03Standarddeviation143.670.06Expandstorstsolution13910.911400.75Standarddeviation16572.311618.08"after5s1.51.002Dcollisionchecks(primitives)153375.0615408.253Dcollisionchecks(primitives)22017.090.00 TABLEIIMEANANDSTANDARDDEVIATIONFOROURAPPROACHINTHE28PLANNINGATTEMPTSONWHICHONLYMULTI-LAYER3DSUCCEEDED. Timetorstsolution[s]1.52Standarddeviation2.13Expandstorstsolution69915.39Standarddeviation107906.74"after5s6.282Dcollisionchecks(primitives)769069.323Dcollisionchecks(primitives)2.89 checksforeachmotionprimitivemaybelesssinceweabortassoonasoneintermediatepositionisincollision.Thenumberofmotionprimitivesthatcheckedfor3Dcollisionswithourapproachisverysmallbecausehavingmultiplelayersallowstheination(whichpenalizesthecostfunctionfortherobotasitgetsclosertoobstacles)tobemoreinformativeinkeepingthesearchawayfromobstaclesand3Dcollisionchecks.Additionally,ascanbeseenfromFig.2,thereareveryfewobstaclesinthearmlayeroftherobotthatarenotclassiedastallobstacles.However,astheplannerapproaches"=1:0,evenourapproacheventuallyhastodomore3Dcollisionchecks.Thisiswhy,onaverage,weonlyreach"=6:28in5sinsomeofthehardercasesandinmanyofthecases10or100thousandsofcollisionchecksarerequiredbeforewereachtheoptimalsolution.B.DockingManeuversWithRealRobotAsecondsetofexperimentsinvolvedrepeateddockingandundockingwithatableusingtherealPR2robotinasimilarclutteredofceenvironment.Thisisthekindofactionthatarobotwillhavetoexecutewhenitneedstopickuporplaceobjectsonthemiddleofthetable.Notethatcongurationsthatrequiretherobottoreachthemiddleofthetableareunachievableusingtraditional2Dnavigationapproaches.Weevaluatedthissetofexperimentssolelyforourapproachtomotionplanningwithamulti-layeredrepresentation.AtypicaldockingmaneuvertopickupalargeobjectfromatablecanbeseeninFig.7.Inourexperiments,allgoalcongurationswerereachedcollision-freewhiletherobotdockedandundockedatable12times.C.NavigationWhileCarryingLargeObjectsFinally,weevaluateourapproachonasetofexperimentsforadifcultscenarioinvolvingautonomousnavigationwithinanextremelyclutteredenvironmentwhilecarryinga goal Fig.7.Dockingwithatabletopickupalargeobject.Left:Goalconguration(shadedgreen)andresultingplans(blue).Right:ExecutionoftheplanbythePR2.laundrybasketwithbotharms.Inthisscenario,therobotoftenhadtopassthroughnarrowareas,forcingittomovesidewaysclosetoobstacles.Thelocationofthelaundrybasketwasprovidedaprioritotherobot.Therobotrstplannedapathtoagoalinfrontofthebasket.Itthenliftedupthebasketwithapre-denedmotionofthearms.Aboundingcylinderwasusedtolteroutthebasketfromtherobot'ssensordata.Thenalnavigationgoalfortherobotwasthennearanothertablewhereitwouldhavetoputthebasketdownagain.Weconductedmultipletrialsusingseveralcombinationsfromasetofsixgoalandstartstates.Therobotsuccessfullyexecutedeachtask.Fig.8showsaseriesofsnapshotswhileexecutingthistask,whereasthecompleteplannedpathisshowninFig.9.Theprecomputationtimeforthefootprintswas3.46sinthisscenarioandthecompleteplanningtime3.16s.Weplannedtotherstsolutioninthiscase("=10).Topickupthelaundrybasket,therobotinitiallyhadtomoveitsbaseunderthetable.Afterwards,thenarrowpassagewithchairsforcedittomovesidewaysunderthetablewiththebasketoverthetable.Finally,thePR2approachedthesecondtabletoputthebasketdown.Notethatlargepartsoftheworkspaceinthisscenarioareunreachablebytherobotusingtraditional2Dnavigationplanningapproaches.VII.CONCLUSIONSANDFUTUREWORKWepresentedanovelapproachforefcientnavigationwithamobilemanipulationrobotinthree-dimensional,clut-teredenvironmentssuchasofcesorhomes.Ourintegratedapproachcombinesanefcientoctree-based3Drepresen-tationandananytimesearch-basedmotionplanner.Forefcientcollisioncheckingduringplanning,ourapproachutilizesamulti-layered2Denvironmentrepresentation,al-lowingthegenerationofalmostrealtimeboundedsub-optimalplans.WedemonstratedtheperformanceofourframeworkonthePR2mobilemanipulationrobotinexhaustivereal-worldexperiments.Therobotwasabletonavigateefcientlyand