LQGObstacles Feedback Control with Collision Avoidance for Mobile Robots with Motion and - PDF document

LQG-Obstacles:FeedbackControlwithCollisionAvoidanceforMobileRobotswithMotionandSensingUncertaintyJurvandenBergDavidWilkieStephenJ.GuyMarcNiethammerDineshManochaAbstract—ThispaperpresentsLQG-Obstacles,anewconceptthatcombineslinear-quadraticfeedbackcontrolofmobilerobotswithguaranteedavoidanceofcollisionswithobstacles.OurapproachgeneralizestheconceptofVelocityObstacles[3]toanyroboticsystemwithalinearGaussiandynamicsmodel.WeintegrateaKalmanlterforstateestimationandanLQRfeedbackcontrollerintoaclosed-loopdynamicsmodelofwhichahigher-levelcontrolobjectiveisthe“controlinput”.WethendenetheLQG-Obstacleasthesetofcontrolobjectivesthatresultinacollisionwithhighprobability.SelectingacontrolobjectiveoutsidetheLQG-Obstaclethenproducescollision-freemotion.WedemonstratethepotentialofLQG-Obstaclesbysafelyandsmoothlynavigatingasimulatedquadrotorhelicopterwithcomplexnon-lineardynamicsandmotionandsensinguncertaintythroughthree-dimensionalenvironmentswithobstaclesandnarrowpassages.I.INTRODUCTIONPlanningunderuncertaintyhasreceivedconsiderableat-tentioninroboticsinrecentyears.Motionuncertainty(duetoexternaldisturbances)andimperfectstateinformation(duetopartialandnoisymeasurements)ariseinmostreal-worldrobotictasks.Thisisespeciallytrueforhighlydy-namicmobilerobotsthatmustreachatargetcongurationwhileavoidingcollisionswithobstacles,suchasquadrotorhelicopters[11].Traditionalplanningapproaches[15],[9]maynotsufceinthesecases,astheyassumedeterministicmotionandfullknowledgeofthestate,andoftenproducejerkypathsduetotherandomnatureofthealgorithm.Feedbackcontrollers,ontheotherhand,cancompen-sateformotionandsensinguncertaintywhilesmoothlycontrollingamobilerobottowardsatargetconguration,eitherdirectlyorbytrackingapre-plannedpath.Thelinear-quadraticGaussian(LQG)controllerdoessooptimallyforlinearsystemswithGaussiannoise[2],andiswidelyusedfornon-linearmodelsaswell[27].LQGcontroldoesnot,however,accountfortheriskofcollidingwithobstaclesintheenvironment.Toaddressthisshortcoming,wepresentthenovelconceptofLQG-ObstaclesforcombiningLQGfeedbackcontrolwithcollisionavoidance.LQG-ObstaclesgeneralizeVelocityObstacles[3]toanylinearGaussiansystem.OurapproachintegratesaKalmanlterforstateestimationandanLQRfeedbackcontrollerintoaclosed-loopdynamicsmodelofJurvandenBergiswiththeSchoolofComputing,UniversityofUtah.E-mail:berg@cs.utah.edu.DavidWilkie,StephenJ.Guy,MarcNiethammer,andDineshManochaarewiththeDepartmentofComputerScience,UniversityofNorthCarolinaatChapelHill.E-mail:fwilkie,sjguy,mn,dmg@cs.unc.edu.ThisworkwassupportedbyAROContractW911NF-10-1-0506,NSFawards1000579and1117127. Fig.1.The3-Windowscenarioforourquadrotorsimulationexperiments.Theredlineistheguidingpath.Ourapproachsmoothlycontrolsthequadro-tor(shownat4Hz)throughthewindowswithoutcollidingwiththewalls.Videosareavailableathttp://gamma.cs.unc.edu/CA/LQGObs/.whichahigher-levelcontrolobjective(atargetconguration)isthe“controlinput”.LQG-Obstaclesthenidentifyalltargetcongurationstowhichamobilerobotcansafelybecon-trolledusingtheLQGcontroller.Fordeterministicdynamicsmodels,theLQG-Obstacleguaranteesthattherobotwillnotcollidewithanobstacle.WeconsiderthistobeaspecialcaseandrefertotheLQG-ObstacleasanLQR-Obstacleforthisclassofmodels.Forstochasticmodels,thegeneralcase,theLQG-Obstacleboundstheaprioriprobabilityofcollidingwithobstaclestoauser-speciedlimit,explicitlyaccountingformotionandsensinguncertainty.Ourconceptcanbeusedtosafelyandsmoothlynavigatearobotwithcomplexdynamicsthroughanenvironmentwithobstacles,bycontinuallyselectingatargetcongurationallowedbythecomputedLQG-Obstacle.TheLQGcontrollerthendeterminesthecorrespondingcontrolinput.Whileourapproachisdesignedforrobotswithlineardynamicsandobservationmodels,itcanalsobeappliedtorobotswithnon-linearmodels,bycontinuallylinearizingthemaroundtheneareststeadystate.TheLQG-Obstacleautomaticallyadaptstothelocalamountofmotionandsensinguncertainty:itforcestherobottochoosesafercontrolobjectivesifthisuncertaintyishigh,andallowsformoreaggressivemotionwhenthereislessuncertainty.Incontrasttoexistingcollisionavoidanceand(re)planningapproaches,ourapproach(i)pro-ducessmoothmotion,(ii)worksatreal-timeratesevenforrobotswithhigh-dimensionalstatespaces,(iii)isapplicabletorobotsrequiringhigh-frequencycontrolfeedbackloops,and(iv)explicitlyconsidersmotionandsensinguncertainty.Whileourapproachnaturallyextendstomovingobstacles,wefocusonenvironmentswithstaticobstaclesinthispaper.Weassumethatthegeometryoftheobstaclesintherobot'scongurationspacearegivenexplicitly.Further,weassumethedynamicsandobservationmodelsoftherobottobegiven,andthattheiruncertaintycanbemodeledbyknownGaussiandistributions.Weimplementedourapproachfor twoillustrativecases.Therstisalinearmobilerobotintheplanewhoseaccelerationcanbecontrolled.Thesecondisaquadrotorhelicopterwithathirteen-dimensionalstatespaceandnon-lineardynamicsyinginthree-dimensionalworkspaces.Wevalidateourapproachusingsimulationexperimentswithsyntheticmotionandsensornoiseinen-vironmentswithobstaclesandnarrowcorridors.OurresultssuggestthatLQG-Obstaclescanworkwellforsafe,real-timeLQGcontrolofmobilerobotstowardsagoalcongurationamidstobstacles(seeFig.1).Theremainderofthispaperisorganizedasfollows.InSectionIIwediscussrelatedwork.InSectionIIIweintro-duceLQR-Obstaclesforrobotswithdeterministicdynamics.InSectionIVweextendthistoLQG-Obstaclesforrobotswithstochasticdynamicsandobservationmodels.WereportsimulationresultsinSectionVandconcludeinSectionVI.II.BACKGROUNDANDRELATEDWORKA.PlanningandControlunderUncertaintyTheproblemofplanningundermotionandsensinguncer-taintyismostgenerallydescribedasapartially-observableMarkovdecisionprocess(POMDP)[10].POMDPsprovideaformulationoftheobjectivetooptimizetheoverallprob-abilityofarrivingatagoalcongurationwithoutcollisions.Unfortunately,intheirgeneralform,POMDPsareknowntobeofextremecomplexity,whichmakesitchallengingtosolvethemforlargeorhigh-dimensionalstatespaces[18].MostapproachesthatapproximateanoptimalsolutiontoPOMDPsrelyondiscretizationorrandomsamplingofthebeliefspace,oracombinationofboth[12],[20],[26].However,discretizationerrorandhighcomputationalcostsmayprohibittheirapplicationtomobilerobotswithhigh-frequencycontrolfeedbackloops.Anotherclassofalgorithmsassumelinear(ized)Gaussiandynamicsandobservationmodels.LQG-MP[28]calculatestheprobabilityofsuccessfulexecutionofagivenpathbasedoncomputeda-prioriprobabilitydistributions.BeliefspaceplanningapproachesbasedontheLQGmodel[8],[21],[22]attempttooptimizethelikelihoodofarrivingatthegoal,butdonottakeintoaccounttheprobabilityofcollidingwithobstacles.Morerecentwork[30]doesaccountforobstacles,butitsapplicabilityislimitedtorobotswithlow-dimensionalstatespaces.Theseapproachesfocusontheabilitytogenerateinformationgatheringactionsastominimizeuncertaintyabouttherobot'sstate.Incontrast,ourapproachdoesnotlookaheadtoselectactionsthatprovidemostinformation,ratheritboundstheprobabilityofcollisionwithobstaclesbasedonthelocalsensingandactingcapabilitiesoftherobot.Thisgoaliscomplementarytothatofbeliefspaceplanningapproaches.OurworksharessimilaritieswiththefeedbackmotionplanningapproachofLQR-trees[25],whichcoverthestatespacewithstabilizingLQRcontrollersaroundpathsinatreethatguaranteestherobottoreachthegoalfromanypointinthestatespace.OurapproachmaycomplementLQR-trees,astheydonotexplicitlyaccountforthepresenceofobstaclesincombinationwithmotionandsensinguncertainty.Ourworkisalsorelatedtomodel-predictivecontrolapproaches[23],whichdoaccountforconstraintsonthestateinanoptimalcontrolformulation.However,theyoftenrequiresolvinghigh-dimensionalmixed-integerprogramsineverycontrolcycle,eveniftheconstraintsarelinearandconvex.B.CollisionAvoidanceOurapproachgeneralizesconceptsinreactivecollisionavoidance,inparticularVelocityObstacles[3].Velocityobstaclesidentifyallvelocitiesthatleadtoacollisionatanytimeinthefuture.Velocitiescanthenbechosenoutsideofthevelocityobstacletoensurethatnocollisionwilloccur.Stochasticvariantsofthevelocityobstacledealwithmotionuncertaintybyenlargingthevelocityobstacle[24]orconsideruncertaintyinthemotionoftheobstacles[6].Alimitationofapproachesbasedonvelocityobstaclesisthattheyonlyworkforrobotswhosevelocitycandirectlybecontrolled.Extensionsexistforrobotsofwhichtheaccelerationcanbecontrolled[29]andforcar-likerobots[17],[32].OurapproachgeneralizesthesemethodstoanylinearGaussiansystem.Analternativetofeedbackcontrolandreactivecollisionavoidanceiscontinual(partial)replanning[19],potentiallyincombinationwithdynamicwindows[4]orICSchecking[5].Thelatterconcepthasrecentlybeenextendedtoaccountformotionuncertaintyoftheobstacles[1].Theachievableplanningfrequencyandpathqualitymaynotbehighenoughforhighlydynamicsystemssuchasquadrotors,though.C.NotationWeusethefollowingnotationalconventionsinthispaper.VectorsetsAaredenotedusingcalligraphics,vectorsaaredenotedusingboldface,matricesAaredenotedusinguppercaseitalics,andscalarsaaredenotedinlower-caseitalics.Scalarandmatrixmultiplicationofsetsaredenedas:aX=faxx2Xg;AX=fAxx2Xg(1)TheMinkowskisumoftwosetsisdenedas:XY=fx+yx2Xy2Yg(2)ItfollowsthatXfxgdenotesatranslationofasetXbyavectorx.III.LQR-OBSTACLESFORDETERMINISTICSYSTEMSInthissectionwediscusshowLQRfeedbackcontrolandcollisionavoidancearecombinedforrobotswithdeter-ministiclineardynamicsandperfectstateinformation.WerstdiscussLQRcontrolandderivetheclosed-looplineardynamics.WethendenetheLQR-Obstacleforcollision-freeLQRcontrol.A.LQRFeedbackControlLetXRxbethestatespaceoftherobot,whichisthespaceofvectorscontainingallinformationrelevantforthemotionoftherobot,andletCRcbethecongurationspaceoftherobot(cx),whichisthespaceofvectorscontainingallinformationrelevantforthe geometricappearanceoftherobotintheworkspace.LetagivenmatrixC2Rcxmapthestatex2XoftherobottoitscorrespondingcongurationCxinC.LetURubethecontrolinputspaceoftherobot.Letthedynamicsoftherobotbegivenbythedeterministiclinearmodel,whichweassumeisformallycontrollable:x=Ax+Bu(3)wherex2Xandu2Uarethestatevectorandcontrolinputvector,respectively,oftherobot,andA2RxxandB2Rxuaregivenconstantmatrices.Letc2Cdenoteatargetcongurationtherobotwishestoreach.Forsystemswithlineardynamics,anLQRfeedbackcontrollercanoptimallycontroltherobottowardsthistargetstateifaquadraticcostfunctionisspeciedthattrades-offreachingthetargetquickly,versusnotapplyingextremecontrolinputs[2]:Z10(Cxc)TQ(Cxc)+uTRu
dt;(4)whereQ2RccandR2Ruuaregivenconstantweightmatrices,forwhichQ=QT0andR=RT�0.Thefeedbackcontrolpolicythatminimizesthiscostfunctionisgivenby:u=Lx+Ec(5)whereL=R1BTS;E=R1BT(BLA)TCTQ;(6)withSbeingthepositive-denitesolutiontothecontinuous-timealgebraicRiccatiequation:ATS+SASBR1BTS+CTQC=0(7)Thisisthestandardcontinuous-timeinnite-horizonLQRderivation[2].NotethatLandEareconstantandcanbecomputedgiventhematricesA,B,Q,andR[16].Wecreatetheclosed-loopdynamicsoftherobotbysubstitutingEq.(5)intoEq.(3),whichgives:x=~Ax+~Bc(8)with~A=ABL;~B=BE:(9)Thetargetcongurationcisthehigher-level“controlinput”oftheclosed-looplineardynamics.Weusetheclosed-loopdynamicstodeneLQR-Obstaclesbelow.B.ConstructingLQR-ObstaclesLetOCdenotetheforbiddenregioninthecongurationspaceoftherobotthatisoccupiedbyobstacles.Then,theLQR-Obstaclefortherobotisdenedas:Denition1Giventhecurrentstatex,theLQR-ObstacleLQR(x)CisthesetoftargetcongurationscthatlettherobotcollidewithanobstacleatsomepointintimewhentheLQRcontrolpolicyisusedtocontroltherobottoc. Fig.2.TheLQR-ObstacleLQR(x)shownasaunionoftransformedconguration-spaceobstaclesforaplanarrobotofwhichtheaccelerationcanbecontrolled(seeSectionV-B)withcurrentstatex=(p;v).Theconguration-spaceobstacleOisadiscshownbythedashedcircle.TheLQR-ObstacleconsistsofalltargetcongurationscthatresultinacollisionwhenanLQRcontrollerisusedtocontroltherobottothatconguration.WeconstructtheLQR-Obstacleasfollows.Integratingtheclosed-loopdynamicsofEq.(8)assumingaconstanttargetcongurationcgives:x(t)=F(t)x(0)+G(t)c(10)withF(t)=exp(t~A);G(t)=~A1(exp(t~A)I)~B:(11)Then,therobotwillcollidewithanobstacleattimetif:Cx(t)2O(12)()CF(t)x(0)+CG(t)c2O()c2(CG(t))1(OfCF(t)x(0)g)whereweassumethatCG(t)2Rccisinvertible.Hence,theLQR-ObstacleLQR(x)isdenedasaunionoftrans-formedconguration-spaceobstacles(seeFig.2):LQR(x)=[t�0(CG(t))1(OfCF(t)xg)(13)ThedenitionoftheLQR-ObstacleimpliesthatiftherobotchoosesitstargetcongurationcoutsideLQR(x),therobotwillnotcollidewithanyoftheobstacleswhileitiscontrolledtowardsc.Theaboveformulationgeneralizesear-liercollisionavoidanceconcepts,suchasvelocityobstacles[3]andaccelerationobstacles[29],tosystemswitharbitrarylineardynamics.C.PropertiesandComplexityofLQR-ObstaclesIfthecongurationspaceobstacleOisofO(1)geometriccomplexity,thenLQR(x)isofO(1)complexityaswell.Aclosed-formexpressioncanbederivedfortheboundaryoftheLQR-ObstacleifOiscircularoraline-segment,andCG(t)isascalarmatrix[29].Further,ifO=O1[O2,thenLQR(x)=LQRO1(x)[LQRO2(x).ItfollowsthatifOconsistsofO(n)geometricprimitivesofO(1)complexityeach,theLQR-ObstacleforOisaunionofO(n)primitiveLQR-Obstacles.Thisunionhasaworst-casecomplexityofO(n2),butwedonotsuspectthatthisboundistight:theLQR-Obstaclesmayobserveapseudo-discpropertywhichwouldallowforalowertotalcomplexity.Weleavethisasanopenquestion. Fig.3.Navigatingacircularrobotwhoseaccelerationcanbecontrolled(seeSectionV-B)throughanarrowcorridorusingLQR-Obstacles.Theworkspaceobstaclesareshownusingthicklines.Theguidingpathisshownbyadashedline.Ineachframe,theLQR-ObstacleLQR(x)fortheparticularstatex=(p;v)isshown.Thevalidcongurationcfarthestalongtheguidingpathischosenastargetconguration.D.Collision-FreeFeedbackControlwithLQR-ObstaclesLQR-Obstaclescanbeusedtosafelycontrolarobotamongobstaclesinacontinualcyclewithasmalltimestep
t:ineachtimestep,theLQR-ObstacleLQR(x)iscomputed,andatargetcongurationc62LQR(x)isselected.ThecontrolinpututhatisappliedisthengivenbyEq.(5).NotethatthefrequencyofselectinganewtargetcongurationcmaywellbelowerthantheLQRcontrolfrequency.Ifagoalcongurationcisgiven,onemaycontinuallyselectthetargetcongurationc62LQR(x)thatisclosesttoc.ForcircularcongurationspaceobstaclesO(e.g.Fig.2),thiswilllettheroboteventuallyreachthegoal.Formoregen-erallyshapedobstacles,however,thisapproachmayeasilyleadtherobotintoalocalminimum.Apossiblealternativeinthiscaseistodeneaguidingpath:[01]!CnOinthefreecongurationspace,with(1)=c,thatindicatestheglobaldirectionofmotionoftherobot.TherobotmaythencontinuallyselectthetargetcongurationfarthestalongthatisoutsidetheLQR-Obstacle,i.e.c=(maxfs(s)62LQR(x)g).Notethattheguidingpathneednotsatisfyanydifferentialconstraints;aseriesofwaypointssufces.Itcanthereforeeasilybeplannedorconstructed,e.g.byextractingitfromaroadmaportreecoveringthefreecongurationspace[15],[9].TheLQRcontrollerensuresthatcontrolinputsarechosenthatresultinsmoothmotionoftherobot(seeFig.3).IV.LQG-OBSTACLESFORSTOCHASTICSYSTEMSAbove,wehaveassumedthatthemotionoftherobotisdeterministic,andthattherobothasperfectinformationaboutitsstate.Here,weextendthemethodtodealwithuncertaintyinboththerobot'sdynamicsandsensing.WewillrstdiscussLQGcontrolandderivetheclosed-looplinearGaussiandynamics,andthendeneLQG-ObstaclesforLQGcontrolwithboundedprobabilityofcollisionswithobstacles.Wediscussitsapplicationtonon-linearsystemsaswell.A.LQGControlwithStateEstimationLetthedynamicsandobservationmodelsoftherobotbegivenbythefollowinglinearGaussiansystem:x=Ax+Bu+mmN(0;M)(14)z=Hx+nnN(0;N)(15)whereEq.(14)issimilarto(3),exceptthatthemotionoftherobotiscorruptedbynoisem2Rxdrawnfromanindependentzero-meanGaussiandistributionwithgivenconstantvarianceM2Rxx.Intheobservationmodel,z2Rzisthevectorofsensormeasurements,andH2Rzxisagivenconstantmatrix.Thesensormeasurementsarecorruptedbynoisen2Rzdrawnfromanindependentzero-meanGaussiandistributionwithgivenconstantvarianceN2Rzz.LetthecontrolcostfunctionbeasinEq.(4),givenatargetcongurationc2Ctherobotwishestoreach.ForlinearGaussiansystems,anLQGcontrollerisoptimal.AnLQGcontrollerusesanLQRfeedbackcontrollerinparallelwithaKalmanlterforstateestimation.TheKalmanlterprovidesanoptimalestimate^xofthestatex,whichevolvesgivensensormeasurementszas[7]:^x=A^x+Bu+K(zH^x)(16)whereKistheKalmangain,whichisgivenby:K=PHTN1(17)wherePisthevarianceofthestatexgiventhestateestimate^x.Sinceourdynamicsandobservationmodelarestationary(i.e.thematricesA,H,M,andNareconstant),thisvarianceconvergesovertimetothepositive-denitesolutionofthecontinuous-timealgebraicRiccatiequation:AP+PAT+MPHTN1HP=0(18)Hence,theKalmangainKisconstant,andcanbecomputedgiventhematricesA,H,M,andN[16].Throughtheseparationprinciple[2],theLQRcontrolpolicycanbederivedindependentlyfromthestateestimator,andisthereforethesameasinEq.(5),withthedifferencethatthestateestimate^xisusedinsteadofthe(unknown)truestatex:u=L^x+Ec(19)withLandEasdenedinEq.(5).Tocreatetheclosed-loopdynamicsthatincorporatesboththestateestimationandthefeedbackcontroller,wedeneanaugmentedstateythatcontainsbothxand^x,forthetruestatexandthestateestimate^xevolveasfunctionsofeachother[28].SubstitutingEq.(19)into(14)andEqs.(15)and(19)into(16)gives:y=~Ay+~Bc+~m~mN(0~M)(20) Fig.4.TheLQG-ObstacleLQGp(^x)inthesamecongurationasinFig.2accountingformotionandsensinguncertaintyoftherobot(seeSectionV-B).TheconservativeapproximationofEq.(25)isshownforvariousvaluesoftheprobabilityboundp.Forp=1,itisequivalenttotheLQR-Obstacle.withy=x^x~A=ABLKHABLKH~B=BEBE~M=M00KNKT(21)Alsointhiscasethetargetcongurationcisthe“controlinput”oftheclosed-looplinearGaussiandynamics.B.ConstructingLQG-ObstaclesWenowfollowasimilarapproachasinSectionIII-BtodeneLQG-Obstacles.Denition2Giventhecurrentstateestimate^x,theLQG-ObstacleLQGp(^x)Cforprobabilityboundpisdenedasthesetoftargetcongurationscforwhichthereisatimet�0atwhichtheprobabilitythattherobotcollideswithanobstacleisgreaterthanpwhenLQGcontrolisusedtocontroltherobottoc.WeconstructtheLQG-Obstacleasfollows.Integratingtheclosed-loopstochasticdynamicsofEq.(20)givenatargetcongurationcgivesy(t)N(^y(t);Y(t)),with:^y(t)=F(t)^y(0)+G(t)c(22)Y(t)=F(t)Y(0)FT(t)+Zt0F()~MFT()d;(23)whereF(t)andG(t)areasinEq.(10)forthematrices~Aand~BofEq.(21).Sincethetruestatexisunknown,theinitialconditionsare^y(0)=^x^xandY(0)=P000,where^xisthecurrentstateestimate.RecallthatPisthevarianceofthetruestatexgivenitsestimate^x(seeEq.(18)).Tomaptheaugmentedstateytothecongurationoftherobot,wedenetheaugmentedmatrix~C=C0.Now,followingasimilarderivationasinEq.(12),therobotwillcollidewithanobstacleataspecictimetif:~Cy(t)2O()~c2(~CG(t))1(Of~CF(t)^y(0)g)(24)where~cN(c(t)),with(t)=(~CG(t))1~CY(t)~CT(~CG(t))T.LetEp()denotethecontourellipsoidofa Fig.5.Controllingtherobot(seeSectionV-B)usingLQG-ObstaclesinthesameenvironmentasFig.3.(a)Tracesofmaximumlikelihoodexecutionsforp=f0:01;0:02;0:03;0:05;1g.(b)Tracesofveactualexecutionswithsyntheticmotionandobservationnoiseforp=0:01.zero-meanGaussiandistributionwithvariancethatcon-tainsafraction1pofitsinstances.Then,theprobabilitythattherobotwillcollidewithanobstacleattimetislessthanpifc62(~CG(t))1(Of~CF(t)^y(0)g)Ep((t)).Hence,aconservativeapproximationoftheLQG-Obstaclecanbeconstructedas(seeFig.4):LQGp(^x)[t�0(~CG(t))1(Of~CF(t)^x^xg)Ep((t))(25)ItfollowsthatiftherobotchoosesitstargetcongurationcoutsideLQGp(^x),theprobabilityofcollidingwithanobstacleatanygiventimet�0islessthanp,ifLQGcontrolisusedtocontroltherobottowardsc.LQG-ObstaclescanbeusedfornavigationinasimilarwayasLQR-Obstacles(seeFig.5).C.LQG-ObstaclesforNon-LinearSystemsTheabovederivationsonlyworkforlinearsystems.Letusconsideranon-linearGaussiansystemoftheform:x=f(xum)mN(0;M)(26)z=h(xn)nN(0;N)(27)Inthiscase,wecanapproximatetheLQG-Obstaclebylinearizingthesystem.Itisconvenienttolinearizearoundasteadystatexforwhichf(x00)=0.Typically,onechoosesxclosesttothecurrentstateestimate^x.Linearizingthengives:~x=A~x+Bu+~m~mN(0;VMVT)(28)~z=H~x+~n~nN(0;WNWT)(29)where~x=xxistheredenedstate,~z=zh(x0)istheredenedmeasurementvector,andA=@f @x(x00),B=@f @u(x00),V=@f @m(x00),H=@h @x(x0),andW=@h @n(x0)aretheJacobianmatricesoffandh.Ifthelinearizedsystemiscontrollable,wecanconstructtheLQG-Obstacleasabove.Asthelinearizedsystemisonlyvalidinasmallregionaroundthelinearizationpoint,themodelsshouldbecontinuallyrelinearizedtogetmeaningfulcontrol. V.IMPLEMENTATIONANDEXPERIMENTATIONA.ImplementationDetailsWeimplementedourapproachusingacollision-checkercapableofperforminglineartransformationsonthegeometry[31].Ineachtime-stepofacontinuouscycle,weselectatargetcongurationc62LQGp(^x),andapplythecontrolinputuaccordingtoEq.(19).Inourcurrentimplementation,weselectthecongurationcusingabrute-forceapproachasfollows.GivenanexplicitrepresentationofOinthecollision-checker,andanitesetTofcandidatetargetcongurationsc,weiterateovertime0t1insmallsteps
t,andtransformtheobstacleOinthecollision-checkertoQ(t)=(~CG(t))1(Of~CF(t)^x^xg)(seeEq.(25)).Then,weiterateoverallcandidatecongurationsc2T,andusethecollision-checkertocheckwhethertheellipseEp((t))centeredatcintersectsthetransformedobstacleQ(t).Ifso,cisinsidetheLQG-Obstacle,andisremovedfromthesetT.Obviously,wecannotiteratetimetoveraninnitedomain,butthetransformedobstacleQ(t)convergesexponentiallyfasttoOfort!1(thisfollowsfromthefactthattheLQGcontrollerreachesthetargetexponentiallyfast[2]).So,wecansafelyboundthetimedomain;inourexperiments,weused0t4inourexperiments.Ingeneralonewouldchooseatime-boundbasedontheeigenvaluesofmatrix~A,astheydeterminethepreciserateofconvergencetothetarget.Afterthis,weareleftwithareducedsetTofcandidatetargetcongurationscwhichareoutsideLQGp(^x).Fromthisset,weselectthemostpreferableone.Inourimplemen-tation,thesetTinitiallyconsistsofthecongurationsalongaguidingpath2C,andtheonefurthestalongthepaththatremainsischosen.B.RobotModelsWeimplementedourapproachfortworobotmodels;alinearplanarrobotwhoseaccelerationcanbecontrolled,andaquadrotorhelicopterwithnon-lineardynamicsandobservationsyingin3-D.TheformerwasusedtogenerateFigs.2,3,4,and5forillustrationpurposes.Thelatterisusedtoreportsimulationresults.1)PlanarRobotwithAccelerationControl:Therobotisadiscintheplanethatiscapableofacceleratingomni-directionally.Its(linear)dynamicsaredenedby:x=pvu=a;A=0I00;B=0I;C=I0wherep2R2istheposition,v2R2thevelocity,anda2R2theaccelerationoftherobot.Thecongurationspaceconsistofallpositionsoftherobot(thevelocitydoesnotchangeitsgeometryintheworkspace),sothematrixCasgivenaboveprojectsthestatetotherobot'sconguration.Weusedthefollowingsettingsforthecontrollerandforthestochasticcasewithnoisymotionandpartialobservation:Q=I;R=I;H=I0;M=001I;N=001I:Thatis,therobotreceivesmeasurementsofonlyitsposition. Fig.6.ProbabilityofcollisionovertimefortheLQG-Obstaclewithp=0:01.2)QuadrotorHelicopter:OurquadrotormodelisbasedonAscendingTechnologies'ResearchPilot.Its13-Dstateand4-Dcontrolinputaredenedby:x=pTvTrTwT'Tu=wT'Twherep2R3istherobot'sposition,v2R3itsvelocity,r2R3itsorientation(axisrandanglekrk),w2R3itsangularvelocity,and'thecombinedforceoftherotors.Thecontrolinputconsistsofthedesiredangularvelocitywandforce'.Alow-levelon-boardcontrollertransformstheseintovoltagesforthemotorsofeachoftherotors.Therobot'sgeometryismodeledbyanencapsulatingsphere,suchthatthecongurationspaceconsistsofonlythepositionsp2R3.Itsnon-lineardynamicsaremodeledafter[14],augmentedwitheffectsofrotordragandinducedinowthatcauseaforceintheoppositedirectionofthevelocity[13]:p=vv=h00gi+(exp([r])h00'ikvv)=m;r=w+[rw=2w=kw(ww)'=k'('')whereg=98m/s2isthegravity,mthemassoftherobot,andkv,kw,andk'scalingconstants.Anoverheadcamerapositionedatpmeasurestheapparentpositionb2R2andradiusinthecameraimageofaballwithradiusrthatisxedontopofthequadrotor.Inaddition,thequadrotorisequippedwithanaccelerometer,amagnetometer,arate-gyro,andanaltimeterthatproducemeasurementsa2R3,d2R3,g2R3,and,respectively,accordingtothefollowingnon-linearobservationmodel:b=(pxpx)(pypy)=(pzpz);=arcsin(r=kppk)a=(h00'iexp([r])Tkvv)=m;d=exp([r])Tkdg=w;=pzwherekd2R3isthedirectionofEarth'smagneticeld.C.SimulationResults1)CollisionProbabilityExperiment:Intherstexperi-ment,weexploretherelationbetweentheprobabilityboundparameterpofLQG-Obstaclesandactualprobabilitiesofcollision.SinceourLQG-Obstacleformulationisconserva-tive,weexpectthatthecollisionprobabilityataspecic Fig.7.TheS-Mazescenarioforourquadrotorsimulationexperiments.Thethinredlineistheguidingpath.Thequadrotorisshownat6Hz.Videosareavailableathttp://gamma.cs.unc.edu/CA/LQGObs/.timetwillbefarlowerthanourbound,andthatthecumulativeprobabilityofcollisionwillslowlygrowoverthedurationoftheexperiment.Givenalongenoughexperimentduration,wesuspectthisprobabilitywillapproachone,asevenaftertherobotreachesitsgoalcongurationnoiseinitsmotionmodelwillcauseittomoveunpredictably,eventuallybringingtherobotintocontactwiththeobstacle.Totestthis,weusetherobotasdescribedinSectionV-BinthescenarioofFig.4.WeselectatargetcongurationconceontheboundaryoftheinitialLQG-ObstacleofFig.4forp=001,andcontroltherobotusingtheLQG-controllertowardscforthedurationoftheexperimentwithsyntheticmotionandsensingnoise.InFig.6,weshowtheresultsofthisexperimentaveragingover100,000trials.Theseresultsconrmthatourboundis(very)conservative:themaximalprobabilityofcollidingataspecictimetseemstobeafactor100lowerthanourbound.2)QuadrotorSimulationExperiments:Toanalyzetheef-fectivenessoftheLQG-Obstacletechniquefornavigatingthesimulatedquadrotorwecreatedtwoexperimentalscenarios.Forbothscenariosweusedasimulatedversionofalabspaceofapproximately10m5m.Asimulatedoverheadcamerawhichrefreshesat30Hzisusedtoaugmentthequadrotor'son-boardsensingtoallowlocalizationintheenvironment.Itisassumedthatthelocationofallobstaclesareknowninadvance.Forbothscenarioswesetp=003andusedasrealisticmodelparametersandlevelsofmotionandsensoruncertaintyaspossible.Inthe3-Windowscenario(seeFig.1),thequadrotormustnavigatefromtheeastsideoftheroomtothewestside.Inbetweenthereisaseriesofthreesmallwindowsthequadrotormustpassthroughatvariousheightsandpositions.Asimpleguidingpathisgivenconsistingofseveralshort,straightsegmentsthroughthecenterofeachwindow.IntheS-Mazescenario(seeFig.7),thequadrotorstartsontheoorinthesoutheastcornerofaroomandisgivenagoalcongurationintheairinthenorthwestcorner.Betweenthesetwopositionsareseveralwallscreatingan (a)(b)Fig.8.(a)Thex,y,andzpositionovertimeofthequadrotorinthe3-Windowscenario(seeFig.1).(b)Thexy-projectionsofthemaximumlikelihoodtracesofthequadrotorintheS-Mazescenario(seeFig.7)for0,1,2,and5therealisticamountofuncertainty.S-shapedcorridorthequadrotormustnavigatethrough.Asimpleguidingpathisgivenconsistingoffourstraightlinesthroughthecenterofthecorridor.Inbothscenarios,thequadrotorisabletosmoothlyandconsistentlynavigatetoitsgoalpositionwith-outcollidingintothewalls.ThegraphsinFig.8(a)showthe3-Dmotionofthequadrotorinthe3-Windowscenario.Videosofbothscenariosareavailableathttp://gamma.cs.unc.edu/CA/LQGObs/.TodemonstratetheeffectofmotionandsensingnoisewerantheS-Mazescenariowithvariouslevelsofnoise.Firstwithnosimulatednoise,secondwithrealisticlevelsofnoise,andthenwith2and5morenoisethantherealisticamountusedinotherexperiments.Beyond5thequadrotorfailedtoreachitsgoal,andinsteadhoveredneartheentranceofthemaze.Fig.8(b)showsanxy-projectionofthemaximumlikelihoodpathforeachoftheserunsofincreasingnoise.Bycomparingtheno-noiseruntothestandardnoiseruntheeffectoftheLQG-Obstacleisapparent.Thequadrotortakesaclearlywiderturnaroundtherstbendtoavoidcomingtooclosetothewall.Athigherlevelsofnoise,thequadrotortakesaveryconservativepath,stayingclosethecenterofthecorridorformuchoftherun.Whiletheconstraintsoftheightdynamicsreducethevariationinpathsduringthesecondcurve,thehighernoiserunsstillstayfurtherawayfromwalls.Ingeneral,planningformorenoiseallowedsmoother,gentlerpaths.3)TimingResults:AbenetofLQG-Obstaclesistheabilitytorunfastenoughforfeedbackcontrol.Forthequadrotorscenarios,thecomputationcannottakemorethan33.3mswithoutframesfromthecamerabeinglost.Herewereporttheexecutiontimeforplanningonan3.2GHzIntelCorei7.TableIshowsthetimetakenbytheLQG-Obstaclefeedbackplannerforthethreescenariosdiscussedinthispaper.Inallcasesthecomputationistimefasterthanthecontrolfrequencyof30Hz.Thenumberofconsideredtargetcongurations–whichinourcaseisproportionaltothelengthoftheguidingpath–isthemainfactordeterminingthecomputationtime. TABLEICOMPUTATIONTIMEFORTHREEDIFFERENTSCENARIOS. Robot Scenario ComputationTime Planar L-Corridor 9.8ms(102Hz) Quadrotor S-Maze 21.4ms(47Hz) Quadrotor 3-Window 24.8ms(40Hz) VI.CONCLUSION,LIMITATIONS,ANDFUTUREWORKInthispaper,wehaveintroducedthenewconceptofLQG-ObstaclesforcombiningLQGfeedbackcontrolwithcollisionavoidance.Wehaveshownusingsimulationsthatourapproachcanbeusedtosmoothlyandsafelyyaquadrotorhelicopterwithmotionandsensinguncertaintythroughanenvironmentwithobstaclesandnarrowcorridors.Wearecurrentlyworkingtoimplementourapproachonarealquadrotor.Ourapproachhasanumberoflimitations.First,itrequiresthegeometryoftheobstaclesinthecongurationspaceoftherobottobegivenexplicitly.Whileintheoryourapproachworksforanyrobot,inpracticeitsapplicabilityislimitedtorobotswithsimplegeometry,suchasmobilerobotsthatcanbemodeledasadiscorasphere.Also,ourapproachworksfornon-linearsystemsonlyifthelinearizeddynamicsarecontrollable.Forthequadrotor,welinearizedaboutitsnear-eststeadystate,butforcar-likerobotsordifferential-drivesonehastochoosethelinearizationpointmorecarefully,asinthesecaseslinearizingaboutasteadystateresultsinnon-controllabledynamics.Further,ourcurrentimplementationletstherobotselectitstargetcongurationalongaguidingpath.Thisisneithertheoptimalway,northeonlywaytouseLQG-Obstacles.Investigatingalternativestrategiesforselectingcontrolobjectivesissubjectofongoingwork.Thereareafewrelativelystraightforwardextensionstoourmethodwedidnotdiscussinthispaper.Firstly,ourapproachcanhandleconstraintsonthecontrolinput.Thesetranslatetoconstraintsonthetargetcongurationthroughthecontrolpolicy,andcanbeincludedintheLQG-Obstacle.Also,ourapproachcanbeextendedtoworkformovingobstacles,byreplacingObyO(t)inthederivation.Extensionsthatrequirefurtherstudyincludeaccountingforuncertaintyinthegeometryandmotionofobstacles,andon-board(local)sensingofobstacles.Ourapproachmayalsoextendtoreciprocalcollisionavoidance[29]formultiplerobots.REFERENCES[1]D.Althoff,M.Althoff,D.Wollherr,M.Buss.Probabilisticcollisionstatecheckerforcrowdedenvironments.IEEEInt.Conf.onRoboticsandAutomation,2010.[2]M.Athans,P.Falb.OptimalControl:AnIntroductiontotheTheoryandItsApplications.DoverPublications,2006.[3]P.Fiorini,Z.Shiller.Motionplanningindynamicenvironmentsusingvelocityobstacles.Int.JournalofRoboticsResearch17(7):760,1998.[4]D.Fox,W.Burgard,S.Thrun.Thedynamicwindowapproachtocollisionavoidance.IEEERoboticsandAutomationMagazine4:23–33,1997.[5]T.Fraichard,H.Asama.Inevitablecollisionstates-asteptowardssaferrobots?AdvancedRobotics18(10):10011024,2004.[6]C.Fulgenzi,A.Spalanzani,C.Laugier.DynamicobstacleavoidanceinuncertainenvironmentcombiningPVOsandoccupancygrid.IEEEInt.Conf.onRoboticsandAutomation,2007.[7]A.Gelb.Appliedoptimalestimation.TheAnalyticScienceCorpora-tion,1974.[8]V.Huynh,N.Roy.icLQG:combininglocalandglobaloptimizationforcontrolininformationspace.IEEEInt.Conf.onRoboticsandAutomation,2009.[9]S.LaValle,J.Kuffner.Randomizedkinodynamicplanning.Int.Jour-nalonRoboticsResearch20(5):378–400,2001.[10]L.Kaelbling,M.Littman,A.Cassandra.Planningandactinginpartiallyobservablestochasticdomains.ArticialIntelligence101(1-2):99–134,1998.[11]V.Kumar,N.Michael.Opportunitiesandchallengeswithautonomousmicroaerialvehicles.Int.Symp.onRoboticsResearch,2011.[12]H.Kurniawati,D.Hsu,W.Lee.SARSOP:Efcientpoint-basedPOMDPplanningbyapproximatingoptimallyreachablebeliefspaces.Robotics:ScienceandSystems,2008.[13]P.Martin,E.Sala¨un.Thetrueroleofaccelerometerfeedbackinquadrotorcontrol.IEEEInt.Conf.onRoboticsandAutomation,2010.[14]N.Michael,D.Mellinger,Q.Lindsey,V.Kumar.TheGRASPmultiplemirco-UAVtestbed:experimentalevaluationofmultirobotaerialcon-trolalgorithms.IEEERoboticsandAutomationMagazine17(3):56-65,2010.[15]L.Kavraki,P.Svestka,J.-C.Latombe,M.Overmars.Probabilisticroadmapsforpathplanninginhighdimensionalcongurationspaces.IEEETrans.onRoboticsandAutomation12:4(566–580),1996.[16]L.Lu,C.Pearce.Onthesquare-rootmethodforcontinuous-timealgebraicRiccatiequations.JournaloftheAustralianMathematicalSociety,SeriesB,40:459–468,1999.[17]E.Owen,L.Montano.Motionplanningindynamicenvironmentsusingthevelocityspace.IEEE/RSJInt.Conf.onIntelligentRobotsandSystems,2005.[18]C.Papadimitriou,J.Tsisiklis.ThecomplexityofMarkovdecisionprocesses.MathematicsofOperationsResearch,12(3):441–450,1987.[19]S.Petti,T.Fraichard.Safemotionplanningindynamicenvironments.IEEE/RSJInt.Conf.onIntelligentRobotsandSystems,2005.[20]J.Porta,N.Vlassis,M.Spaan,P.Poupart.Point-basedvalueiterationforcontinuousPOMDPs.JournalofMachineLearningResearch7:2329–2367,2006.[21]R.Platt,R.Tedrake,L.Kaelbling,T.Lozano-Perez.Beliefspaceplan-ningassumingmaximumlikelihoodobservations.Robotics:ScienceandSystems,2010.[22]S.Prentice,N.Roy.Thebeliefroadmap:Efcientplanninginbe-liefspacebyfactoringthecovariance.Int.J.ofRoboticsResearch28(1112):1448-1465,2009.[23]J.Rawlings.TutorialoverviewofModelPredictiveControl.IEEEControlSystemsMagazine20(3):38–52,2000.[24]J.Snape,J.vandenBerg,S.Guy,D.Manocha.Independentnavi-gationofmultiplerobotswithHybridReciprocalVelocityObstacles.IEEE/RSJInt.Conf.onIntelligentRobotsandSystems,2009.[25]R.Tedrake.I.Manchester,M.Tobenkin,J.Roberts.LQR-trees:Feedbackmotionplanningviasums-of-squaresverication.Int.J.onRoboticsResearch29(8):1038-1052,2010.[26]S.Thrun,W.Burgard,D.Fox.ProbabilisticRobotics,MITPress,2005.[27]E.Todorov,W.Li.AgeneralizediterativeLQGmethodforlocally-optimalfeedbackcontrolofconstrainednonlinearstochasticsystems.AmericanControlConference,2005.[28]J.vandenBerg,P.Abbeel,K.Goldberg.LQG-MP:Optimizedpathplanningforrobotswithmotionuncertaintyandimperfectstateinformation.Int.J.ofRoboticsResearch30(7):895-913,2011.[29]J.vandenBerg,J.Snape,S.Guy,D.Manocha.Reciprocalcollisionavoidancewithacceleration-velocityobstacles.IEEEInt.Conf.onRoboticsandAutomation,2011.[30]J.vandenBerg,S.Patil,R.Alterovitz.Motionplanningunderuncertaintyusingdifferentialdynamicprogramminginbeliefspace.Int.Symp.onRoboticsResearch,2011.[31]G.vandenBergen.Collisiondetectionininteractive3Denvironments.MorganKaufmannPublishers,2004.[32]D.Wilkie,J.vandenBerg,andD.Manocha.Generalizedvelocityobstacles.IEEE/RSJInt.Conf.onIntelligentRobotsandSystems,2009.