机器人技术-第5讲-路径规划和避障

!!—• Roland!Siegwart!and!Illah!R.!Nourbakhsh.!!Introduc8on!to!Autonomous!Mobile!Robots.!The!MIT!Press,!2004.!• Where!am!I!?!!(?)!• Where!am!I!going!?!!(?)!• How!do!I!get!there!?!(?)2?• /• • • 1/!• /114• /9• • • – – – – • 路径规划1• /• /1• • • (Configura8on!space)• !• /!– 6!– !• 1!• /!– 73!• /!– 7/1• /– • /• /– resolutioncompleteness/– Probabilisticcompleteness:• /!– /– /– /• /!– PRMProbabilis8c!RoadMaps!– RRTRapidKExploring!Radom!Trees!!• /111!• /VisibilitygraphVoronoidiagram• !– – • /!– !– !• /!– 2!– 3!!Voronoi!diagram• /!Voronoi!diagram• /!– 10!– 30!– !!Voronoi!diagram!!Voronoi!diagram• /!• /))• !– 11)10– 0– • – – • !• /– 1– • /– • !– 26!• !– 3!• /!– /• /!– !– !– !5.35-3rFtF5-45-55-612r1Fr2rFF5-35-7Fr2FtFFr12FtF42• /(Ar8ficial!Poten8al!Field)!– /!ArtificialPotentialFieldsIntroductionArobotshouldbedesignedandcontrolledtonavi-gateandtraversewithoutcollidingtotheobstacleslocatedarounditselfwhichmaybestaticordynamic.Artificialpotentialfields(apf)isareactiveapproachinwhichtrajectoriesarenotplannedexplicitly.In-stead,agent’sinteractionswithitsenvironmentaresuperposedoremergedtomaketherobotflexiblycopewiththechangingenvironment.Althoughtheideabehindusingapfinpathplanningseemseasy,problemssuchaslocalminimaandoscilliatorymove-mentsmakesitdiculttofindapathconnectingtwo-endpositions.BasicArtificialPotentialFieldConceptsAPFmainlyconsistsofforcevectors,causedbytheobstaclesortargetpositions,whichmaybelinearortangentialandtheymayhavecharacteristicsofre-pulsive,attractiveorrandomdependingonthestateoftheagentwithrespecttoitsenvironment.Anex-ampleofapotentialfieldforcefunctionisasfollows;U=K⇥expxxo↵+yyoDirectionofthepotentialfieldforcecanbefoundbytakingit’sgradient;d(U)d(x)=K⇥(2⇥(xxo)↵)⇥expxxo↵+yyo⇥d(x)d(U)d(y)=K⇥(2⇥(yyo))⇥expxxo↵+yyo⇥d(y)Followingfigures,takenfrom[1],indicatesdi↵erentsortofpotentialfields.Figure1:AttractivelinearapfFigure2:RepulsivelinearapfFigure3:OnedirectiontangentialpotentialfieldsKadirFiratUyanik122||||()(2||)||addaattaadadaKdUKddd⎧−−≤=⎨−−−⎩xxxxxxxxxadaKdxx• /(Ar8ficial!Poten8al!Field)!– /– /ArtificialPotentialFieldsIntroductionArobotshouldbedesignedandcontrolledtonavi-gateandtraversewithoutcollidingtotheobstacleslocatedarounditselfwhichmaybestaticordynamic.Artificialpotentialfields(apf)isareactiveapproachinwhichtrajectoriesarenotplannedexplicitly.In-stead,agent’sinteractionswithitsenvironmentaresuperposedoremergedtomaketherobotflexiblycopewiththechangingenvironment.Althoughtheideabehindusingapfinpathplanningseemseasy,problemssuchaslocalminimaandoscilliatorymove-mentsmakesitdiculttofindapathconnectingtwo-endpositions.BasicArtificialPotentialFieldConceptsAPFmainlyconsistsofforcevectors,causedbytheobstaclesortargetpositions,whichmaybelinearortangentialandtheymayhavecharacteristicsofre-pulsive,attractiveorrandomdependingonthestateoftheagentwithrespecttoitsenvironment.Anex-ampleofapotentialfieldforcefunctionisasfollows;U=K⇥expxxo↵+yyoDirectionofthepotentialfieldforcecanbefoundbytakingit’sgradient;d(U)d(x)=K⇥(2⇥(xxo)↵)⇥expxxo↵+yyo⇥d(x)d(U)d(y)=K⇥(2⇥(yyo))⇥expxxo↵+yyo⇥d(y)Followingfigures,takenfrom[1],indicatesdi↵erentsortofpotentialfields.Figure1:AttractivelinearapfFigure2:RepulsivelinearapfFigure3:OnedirectiontangentialpotentialfieldsKadirFiratUyanik1ρρ02000111()20rrepKUρρρρρρ⎧⎛⎞⎪−≤⎜⎟=⎨⎝⎠⎪⎩x• /– 2()||()()2||||addaattattdaadadKdFUKdd−−−≤⎧⎪=−∇=−⎨−−⎪−⎩xxxxxxxxxxxx0200111()()0rreprepKFUρρρρρρρρ⎧⎛⎞∂−≤⎪⎜⎟=−∇=∂⎨⎝⎠⎪⎩xxx00Txyρρρρ⎛⎞−∂∂∂==⎜⎟∂∂∂⎝⎠xxxx• /()()()()()()attrepattrepFUUUFF=−∇=−∇−∇=+xxxxxx• 1!• 1!• /5.35-3rFtF5-45-55-612r1Fr2rFF5-35-7Fr2FtFFr12FtF42• 1NPKcomplete!• /!– 8Dijkstra!• !– !• A*,!D*,!Focused!D*!– !• !A*• !– 441413()()()fngnhn=+ng(n)h(n)dabce21.522334h(d)=4.5h(c)=4h(e)=2h(a)=4h(b)=2daf(a)=5.5f(d)=6.5bf(b)=5.5f(c)=10.5cf(e)=7ef=7Openlist7g(n),h(n),f(n)Openlist7f(n)48openlistcloselist7tg=g(n)4+444closelist74tg4=4g(n)442openlist7tgg(n)8g(n)=tg,h(n)f(n)2openlist7YNYYNN74!!4!!!!6060!!10!!5054!!4!!!!4060!!10!!5074!!4!!!!6060!!10!!5040!!0!!!!3054!!4!!!!4074!!24!!5068!!28!!!4074!!24!!5068!!28!!!4068!!38!!3062!!42!!2062!!52!!!!1062!!52!!1056!!56!!084!!24!!6094!!24!!!!7084!!24!!6094!!24!!!!70• 2Knorm!• ManhaZan!distance,!Block!distance,!1KnormL1!distance!• rChebychev!distance,!infinity!norm!• Minkowski• 2Knorm!22()iiixy−=−∑xyx=(x1,x2,…,xn)y=(y1,y2,…,yn)• ManhaZan!distance!1iiixy−=−∑xy8886r• !Chebychev!distance!maxiiixy∞−=−xy(Minkowski)• kk(,)||kkiiiMinkowskixy=−∑xyk1,2,,kkk==→∞(• PSOParticleSwarmOptimization!,.2,.1,O2XY,S,G..P={S,p1,p2,,pm,G},(1)(p1,p2,,pm),.pj:pj,pj.1,SGX,XSY,X,Y1.xy=cosA-sinAsinAcosAıxy+xSyS.(2):(x,y),(x,y)O2XYS2XY,AXX,(xS,yS)SO2XY.SG(m+1),,(l1,l2,,lm),P(p1,p2,,pm).Sp0,Gpm+1,PLPLP=LSp1+m-1j=1Lpjpj+1+LpmG=mj=0Lpjpj+1.(3)Lpjpj+1pjpj+1.(x,y),(3)LP=mj=0(xpj-xpj+1)2+(ypj-ypj+1)2=mj=0LSGm+12+(ypj-ypj+1)2.(4)(3),ypj(j=1,2,,m)LP.,,ypj,LP,ypjypj+1().3PSO3.1PSOPSO,2(v2x).,F(x).,.,:pBestgBest.n,m,[6]vt+1i,j=Xvti,j+c1r1(pti,j-xti,j)$t+c2r2(ptg,j-xti,j)$t,(5)xt+1i,j=xti,j+vt+1i,j$t.(6):vti,j,xti,ji(i=1,2,,n)j(j=1,2,,m)t;pti,jijt;ptg,jjt;$t;r1,r2(01);c1,c2,pBestgBest,c1=c2=2;X,X,X,