预测行人运动的服务机器人POMDP导航

32120101ROBOTVol.32,No.1Jan.,20101002-0446(2010)-01-0018-08POMDP210096—POMDPPOMDPTP24BPOMDPNavigationofServiceRobotswithHumanMotionPredictionQIANKunMAXudongDAIXianzhongFANGfang(KeyLaboratoryofMeasurementandControlofCSE,MinistryofEducation,SchoolofAutomation,SoutheastUniversity,Nanjing210096,China)Abstract:Toimprovethenaturalpedestrian-avoidanceskillsofservicerobotsinindoordynamicenvironments,amethodofcombininglong-termandshort-termpredictionofpedestrian’smotionisintroducedonthebasisofmodelinghumans’motiontrajectorypatterns.Inordertoaccommodatetheuncertaintiesintheperception-controlloopofrobots,whicharemainlycausedbysensornoiseandtimedelayinnetworkandotherfactors,therelativepositionrelationbetweenhumanandrobotismodeledaspartiallyobservableMarkovstate.PartiallyobservableMarkovdecisionprocess(POMDP)isutilizedforprobabilisticdecision-makingundermulti-sourceuncertainties,andthebehaviormodulesoftheglobalpathplanner,themotionreactorandthespeedcontroller,arecoordinated.Experimentalresultsillustratetheperformanceofsafenavigationthatcanavoidconflictsinadvance,aswellastheimprovedrobotnavigationefficiencybyavoidingrepeatedzigzagingandwanderingmotion.Keywords:predictivenavigation;motionestimation;uncertainty;POMDP(partiallyobservableMarkovdecisionprocess)1Introduction[1-2]goal-orientedmotion[1-5]—Miura[3]Hoeller[4]Osentoski[1]EMexpectation-maximizationSchulz[6][7][7]POMDP[8-9][e-mail]2009-01-20/2009-04-20/2009-10-1218321POMDP19POMDP[7]Foka[10]POMDPgoal-oriented—POMDP2POMDPPrincipleandsystemframeworkofPOMDPnaviga-tionhuman-aware(1)(2)SimultaneousrobotLocalizationAndPeople-trackingSLAPSLAP10cm[11]50cm1(3)Pioneer100mm/saF400msd20mmd660mmd620mm60mmd6100mm70%20%10%1Fig.1Uncertaintyofsimultaneousrobotlocalizationandpeople-trackingSLAP—SLAP—POMDP2people’sactionobservationPAO—people-robotrelationobservationPROrobotstateobservationRSOwavefrontprop-agation[12]nearnessdiagram[13]20201012POMDPFig.2FrameworkofthePOMDPpredictivenavigationsystemPOMDPPOMDP33HumanmotionpredictionSLAPrrrt=(xr;yr;qr)(vr;wr)hhht=(xh;yh;qh)(vh;wh)qhxfYYYmgjMm=1MYYYmx-yKmYYYm=(qqqkm;pkm)jKmk=1qqqkm=(mkm;Skm)kpkmp(hhhtjYYYm)YYYm3x-y(a)(b)3Fig.3Examplesofhumanindoormotionpatterns[vmin;vmax][qmin;qmax]DThhh0=(x0;y0)person’sinstantaneousorientationPIOL=qmaxhhh=(r;a)porien(hhhjhhh0)=exp(¡a2)PIOa4thhhtpvel(hhht;t)5qh;0s20viitxt=x0+åti=1viDTcosqh;0yt=y0+åti=1viDTsinqh;0(1)4Fig.4Uncertaintyoftheinstantaneousorientation321POMDP215Fig.5Uncertaintyofthemotionvelocityv0;¢¢¢;vtfvigi=0;¢¢¢;tvi»U(vmin;vmax)s2steps2step=¥w¡¥(vi¡Evi)2f(vi)dvi=1vmax¡vminvmaxwvmin(vi¡vmax+vmin2)2dvi=112(vmax¡vmin)2(2)thhhtpvel(hhht;t)=12psx;tsy;texp·¡12µ(xt¡¯xt)2s2x;t+(yt¡¯yt)2s2y;t¶¸(3)¯xt=x0+t¯vDTcosqh;0¯yt=y0+t¯vDTsinqh;0s2x;t=s2y;t=s20+ts2step¯v=vmax+vmin20hhh0thhht(4)hp(hhhtjYYYm)m=1;¢¢¢;Mp(hhht;t)=hp(hhhtjYYYm)pvel(hhht;t)gg=porien(hhhtjhhh0)(4)q=jqh¡qrjqth1qpLsafePct0[Pc¡Dinsafe;Pc+Doutsafe][3]Dinsafe=LsafesinqÃsv2r+v2h¡vrvhcosqv2r+vhvr!(5)Doutsafe=LsafesinqÃsv2r+v2h¡vrvhcosqv2r¡vhvr!(6)vhvrt=t0+Dinsafe±vhPcLsafet0Pcpconflict=p(Pc;t)(7)q=pDinsafeDoutsafeqqth2p(stjYYYm)6Skmk=argmink=1;¢¢¢;Kfp(sjqqqkm;pkm)gsinsoutvhvrsinsouttinhtouthtinrtoutr¯¯tinr¡touth¯¯qth3sinpobstruct=hp(sinjYYYm)pvel(sin;tinr)g1pvel(sin;touth)g2g1=porien(sinjrrrt);g2=porien(sinjhhht)(8)¯¯tinh¡toutr¯¯qth4soutpobstruct=hp(soutjYYYm)p(sout;tinh)g1p(sout;toutr)g2g1=porien(soutjhhht);g2=porien(soutjrrrt)(9)h64POMDPPOMDPnavigation4.1—PAOfmove;staygPRORSOPRO8(1)[Pc¡Dinsafe;Pc+Doutsafe](2)[Pc¡Dinsafe;Pc+Doutsafe](3)[Pc¡Dinsafe=2;Pc¡Dinsafe](4)(5)¯¯tinr¡touth¯¯qth3(6)¯¯tinh¡toutr¯¯qth4(7)¯¯tinr¡touth¯¯qth3¯¯tinh¡toutr¯¯qth4(8)vh;t=0(1)(4)(5)(7)RSO2220101(a)(b)(c)(d)6Fig.6Predictionofhuman-robotmotionconflictandobstruction4.2POMDPPOMDPhS;A;T;R;O;BiSmove(PM)stay(PS)move(GM)stay(GS)DSDBDJDERNRFRSRRs1;¢¢¢;s64642£2£4£43232POMDPb(s)agentss0jSjBA=fa1;¢¢¢;a4g4anauasar(4)T(s0;a;s)agentsas0R(s;a)agentsa1XO(s0;a;o)as0oO(s0;a;o)=P(ojs0;a)EM[7]POMDPPerseus[14]32POMDPPentium4PC63.578s71Tab.1PartoftherewardmatrixstartstateendstateactionrewardPMGMDEXPMGMDSRRar100PMGMDBXPMGMDSRFau100PSGSDJXPSGSDSRFau¡100PSGSDSRNPSGSDSRSan¡20............7POMDPFig.7ConvergenceoftheerrorchangesduringthePOMDPmodeltraining321POMDP235Experiments7.8m£12m33(b)SICKLMS200People-bot53(b)qth1qth2—qth3qth4POMDP2Hz5.1—1m—3m89(d)ABII—III9(a)(c)9(d)633(a)(b)8—Fig.8Navigationresultsinthesituationofhuman-robotfrontalconflict(a)(b)(c)(d)9Fig.9Predictivenavigationusingrepetitivere-planninginthesituationofmultiplenarrowcorridors10Fig.10Predictivenavigationusingtheproposedmethodinthesituationofmultiplenarrowcorridors2420101113Fig.11Theaveragesuccessrateofcompletingthreetasks—10(d)CDII10(a)(c)10(d)342(1)(2)(3)420115.2POMDPMarkovdecisionprocessMDP12POMDPMDP12POMDP/MDPFig.12TheaverageaccumulatedrewardofPOMDP/MDP6ConclusionPOMDPReferences[1]OsentoskiS,ManfrediV,MahadevanS.Learninghierarchicalmodelsofactivity[C]//IEEE/RSJInternationalConferenceonIntelligentRobotsandSystems.Piscataway,NJ,USA:IEEE,2004:891-896.[2]BennewitzM,BurgardW,ThrunS