Collision Avoidance for Multiple Robots till Next Waypoints through Collision Free Polygons

Transcription

1 Collson Avodance fo Multple Robots tll Next Wayponts though Collson Fee Polygons Satsh Peddu* K. Madhava Kshna* * Robotcs Reseach Cente, Intenatonal Insttute of Infomaton Technology, Hydeabad, Inda peddu@eseach.t.ac.n mkshna@t.ac.n Abstact Knematcally consstent paths fo multple moble obots that ae collson fee fo the next T steps ae geneated by fomng collson fee polygons (CFP). The polygons can be geneated n a dstbutve fashon fo each obot. All paths nsde CFP that ae knematcally feasble fo a cetan obot ae collson fee wth all othe obots n the same collson cluste fo the next T tme samples and wth any othe obot n the wokspace n geneal. The constucton of CFP s though two log-lnea complex opeatons. The fst one nvolves computng the ntesecton of the path contane polygon (PCP) of a obot wth PCP of othe obots n the cluste. The second nvolves computng the convex hull of ntesectng ponts asng fom the ntesecton of PCPs. Once CFP s computed fo a obot t chooses a pont wthn the CFP (ts next waypont) that mnmzes a cost functon and moves towads the pont. The pocess s epeated tll the goal s eached. The man advantage of ths method s that the best and wost-case tmes fo fndng a T-step ahead collson fee path s always log-lnea. In many andomzed seach methods, fndng the paths tll the next waypont esults n a wost case complexty exponental n numbe of obots. The effcacy of the cuent method s well potayed though smulatons. Compaatve esults show that the followng method fnds a collson fee path to next way pont much qucke. 1. INTRODUCTION Mult obotc systems have been an actve aea of eseach, whee multple obots pefom a task n a coopeatve o ndvdual fashon. Whle pefomng mult obotc tasks, t s often desable that the system s collson fee. Collsons can happen wth the co-obots o wth the statc and dynamc obstacles, and these collsons also called as conflcts can be hazadous fo the obots. In ode to ovecome these conflcts, we devse an algothm, whch can be used n vaous applcatons. The man advantage and novelty of ths method s that the best and wost-case tmes fo fndng collson fee path tll next waypont s always log-lnea. Any pont on ths path cannot be accessed by any othe obots fo the next T Steps. In many methods that nvolve a andomzed seach fo collson fee paths the wost-case complexty s exponental n the numbe of obots. The effcacy of ths method and ts advantages ae well potayed though smulatons. Compasons wth othe heustcs that ae used n andomzed methods show that the followng method fnds a collson fee path to the next waypont much qucke than those heustcs. The pvot of ths algothm s the computaton of the Collson Fee Polygons (CFP) fo a obot. The CFP nvolves computng the ntesecton of Path Contane Polygon (PCP) of a obot wth all othe obots. The PCP, a convex polygon can be consdeed as a contane of a numbe of knematcally feasble paths fo the next T nstants. The vetces of the PCP (CP fo shot hencefoth) ae the cuent poston of the obot, the coodnates that ae eached wth maxmum lnea acceleaton and maxmum angula deceleaton (f the cuent angula acceleaton s zeo then that s mantaned), the coodnates obtaned by maxmum lnea and angula acceleaton and those obtaned by maxmum lnea deceleaton and angula acceleaton. The ntesecton of CPs of obots n collson gves se to a numbe of ntesecton ponts on the bounday of the PCP as well as nsde t fo a obot. The convex hull of these ponts s consdeed. Then all ponts that ae outsde the hull but nsde the CP ae guaanteed to be collson fee fo next T samples. The CFP s the bounday of all such collson fee ponts. The whole opeaton can be pefomed n two log-lnea opeatons. Ponts wthn CFPs of all the obots that belong to a collson cluste ae chosen such that a cost functon s mnmzed. The seach fo a pont wthn the CFP can be made abtaly fast by choosng abtaly a small numbe of ponts to mnmze the cost functon. In fact choosng one pont would suffce snce any pont n a CFP and path to t s collson fee fo next T steps. The obots belongng to a collson cluste ae obtaned though the collson dependency gaph. Robot A that pedcts a collson wth anothe obot B wthn a cetan stpulated tme s sad to have a collson dependency wth that obot and s shown n the gaph by a bdectonal lnk between the two. The algothm cuently opeates n a sem-centalzed fashon n that t s centalzed wth espect to the obots n a cluste whle decentalzed acoss clustes. Howeve, a completely

2 decentalzed mplementaton can also be acheved wth exta bandwdth, allowng fo moe exchange of messages between the obots. Mult-obotc navgaton algothms ae tadtonally classfed as centalzed o decentalzed appoaches. In the centalzed plannes [1, 2], the confguaton spaces of the ndvdual obots ae combned nto one composte confguaton space, whch s then seached fo, to obtan a path fo the whole composte system. In case of centalzed appoaches that compute all possble conflcts ove the ente taectoes, the numbe of collson checks to be pefomed and the plannng tme tend to ncease exponentally as the numbe of obots n the system nceases. Complete ecalculaton of paths s equed even f one of the obot s plan s alteed o the envonment changes. Howeve, centalzed plannes can guaantee completeness and optmalty of the method, at least theoetcally. Decentalzed appoaches, on the othe hand, ae less computatonally ntensve as the computatonal buden s dstbuted acoss the agents and, n pncple, the computatonal complexty of the system can be made ndependent of the numbe of agents n t, at-least to the pont of computng the fst ndvdual plans. It s moe toleant to changes n the envonment o alteatons n the obectves of the agents. Conflcts ae dentfed when the plans o commands ae exchanged and some knd of coodnaton mechansm s employed to avod the conflcts. Howeve, they ae ntnscally ncapable of satsfyng optmalty and the completeness cteon. Pomnent among the decentalzed appoaches ae the decoupled plannes [3], [4], [5]. The decoupled plannes fst compute sepaate paths fo the ndvdual obots and then esolve possble conflcts of the geneated paths by a hll clmbng seach [3] o by plan megng [4] o though dvdng the oveall coodnaton nto smalle sub poblems [5]. Howeve many of these plannes ae not vable n pesence of non ntegable constants, movng obstacles and fast eplannng. Thee s a vast volume of lteatue that addesses the poblem of moton plannng n pesence non ntegable constants by ncopoatng the knematcs of the obot [6, 7] and the vaous extensons that addess the poblem of fast eplannng [8] and movng obstacles [9] and multple obots [10]. They do so by makng use of the exstng oadmap of cuves constucted whle computng the ntal plan. In the pesent method thee s no ntal plan fo the obot snce thee s no map avalable and s close to eactve collson avodance appoaches n ths egad. It s cucally dffeent fom othes n that the focus has been on computng a collson fee path tll the next waypont that has the same log-lnea complexty espectve of the natue of the envonment. The pesent appoach can be dovetaled wth the methods cted above snce fndng a path to the goal state though a sequence of paths tll the next waypont s a ecung theme n all. Heustcs ae geneally esoted to educe the seach n fndng the path fo multple obots n collson [10]. Howeve t s not guaanteed that these heustcs ae effcent acoss the boad fo vaous stuatons unlke the cuent appoach whee a seach fo a collson fee path s equvalent to the computaton of CFP. Beng log-lnea t easly scales up gacefully ove numbe of obots. It can also be dovetaled wth appoaches that do not ncopoate a known map such as the Dynamc Wndow [11] to a mult obotc settng, whee the seach fo a collson fee path s fo the next tme sample only. II PROBLEM FORMULATION Gven a set of obots R R, R, 2, 1 R n, each assgned a stat and goal confguaton, the obectve s to navgate the obot such that they each the goal confguaton avodng all collsons. Whle collsons could occu wth statonay and movng obects, n ths pape we focus pmaly on how the obots could avod collsons that occu amongst them n a coopeatve fashon. Fo ths pupose the followng pemses have been made: a. Each obot R s assgned a stat and goal locaton and t has access to ts cuent state and ts cuent and aspng veloctes. The cuent state of R s epesented as vc, vn, c, n whee vc, vn epesent ts cuent and aspng veloctes and c, n ts cuent and aspng angula veloctes. b. Robots ae capable of boadcastng the cuent states to each othe. They do so only to those obots that ae wthn a patcula ange of communcaton. c. Robots acceleate and deceleate at constant ates that ae same fo all. Hence a obot R can pedct, when anothe obot R would attan ts aspng velocty vn fom ts cuent velocty vc f t does not change ts decton. Assumpton a s a standad assumpton made n a typcal moble obot system. In geneal senso based localzaton algothms ae used to coect dscepances between the expected estmate and the actual eadngs epoted by senso. Cuent and futue velocty estmates obtaned though such technques have been used to acheve desed esults such as n [11], whee the authos epot a successful eal-tme algothm fo fast navgaton. Assumpton b s also common to mult obotc systems wth successful mplementatons [10]. Assumpton c s done essentally to facltate cetan smplcty n the appoach to educe the amount of data tansfe between obots. The algothm s pefomance would not be affected f the ates of acceleaton and deceleaton vay between obots povded they ae communcated to one anothe.

3 III METHODOLOGY We consde a dffeental dve obot that move along contnuous cuvatue paths. We assume that fo a small nteval t the lnea and angula veloctes ae constant. The path of a obot conssts of pecewse ccula acs (lnea segment ae made of adus). Fom the cuent pose of the obot we get vaous cuves coespondng to dffeent values of fnal angula and lnea veloctes eached fom the cuent state. These paths ae geneated such that thee s only one change n the decton of acceleaton,.e. ethe the obot acceleates o deceleates but does not do both. In essence fo cetan T tme steps nto the futue we get a set of eachable poses. Among these we select the pose eached though maxmum lnea and angula acceleatons, along wth the cuent poston we fom a polygon fom these ponts that s convex and call t the PCP (fgue 1). It can be shown that the set of eachable postons nsde the PCP s much dense than those outsde. (Fgue 2) shows AFP fo obot A as the shaded egon. C P ~ fo obots B and C ae also shown. Fg. 2. The shaded egon shows AFP fo obot A. Any pont n the egon s naccessble fom B o C f values of acceleaton ae slghtly dffeent fom the exteme values. Fg. 3. CFP of the polygon A fomed by convex hull. A. Domnantly Inaccessble Aeas We fst fnd what we call as domnantly naccessble aeas wthn PCP of obot wth espect to all othe obots 1 n; wth whch t has a collson wthn next T samples. Let PCP of constucted fo the next t sample be t CP we emove supescpt t fo ease of notaton. Let A CP be the aea enclosed bycp. We extend the sde vetces of CP be a facto and fom a new CP called CP. The facto futhe educes the numbe of ponts accessble fom cuent state of that le outsde ACP 1 n. We futhe gow all CP by adus of obot and educe to a pont. We denote the gown CP as C P ~. We fnd the followng aea AFP such that any pont n AFP s almost naccessble fom the cuent state of 1 n;. We say almost n the sense that they ae accessble only fo exteme values of lnea and angula acceleaton o deceleaton f at all. They can be made naccessble by choosng an acceleaton o deceleaton slghtly dffeent fom exteme value o by changng. AFP Fg. 1. The PCP and the Paths and set of eachable postons contaned nsde. n 1 A CP A CP 1 Fg. 4. Ponts chosen nea V2 can pass the CP of B. B. Collson Fee Paths Whle any pont n AFP fo a obot s naccessble fom othes t does not guaantee that the path eachng any such pont fom the cuent state s collson fee. Fo ths we compute the convex hull of the followng ponts Pont due to ntesecton of CP wth CP and all ponts of CP nsdecp. All pont of CP except the cuent pont coespondng to cuent pose of the obot. Fo (fgue 2) the convex hull s plotted n (fgue3). Let us call the convex hull as Hu CP ). Then the collson fee polygon of Fg. 5. Each obot s cuent state contan the contane polygon of anothe obot. ( CFP s the polygon CFP CP Hu CP. The shaded egon n (fgue 3) s the CFP fo obot A. The chaactestcs of CFP s that any pont n the CFP when eached fom cuent pose of, C wth not moe than one change n decton of angula velocty of wll not ntesect CP and s hence collson fee fom all paths emanatng fom C ; 1 n;.

4 C. Tough cases: Case 1: When choosng the next waypont of obot wthn t s CFP at tmes the cuve onng the cuent pose to the waypont can cut CP of anothe obot. Ths happens f the pont s chosen on the bounday of CFP, close to one of the vetces of thecp of, usually when only one obot s ntesectng and the angle of appoach s acute. Ths s shown n (fgue 4). Howeve the pobablty of collson s neglgble snce The path even when t ntesects the CP of othe obot does t such that the length of the path nsde the CP s vey small. Snce the othe obot also chooses ts next waypont wthn ts CFP the actual pobablty of paths colldng s zeo. Case 2: In ths case we have a stuaton of each obot s cuent state contaned n the contane polygon of anothe obot. Ths s shown n (fgue 5). The stuaton can be ovecome by contnung the obot along cuent moton decton fo a few moe samples. If the stuaton contnues to pesst the polygon sze can be educed fo a lowe tme peod T. Ths method of educng polygon sze can be appled fo the pevous case as well. It must be emphaszed that the above stuatons ae extemely ae and have been notced not moe than a few tme n seveal uns. D. Obectve Functon Mnmzaton We choose sample ponts fo all obots wthn a cluste. These sample ponts fo a obot s on the bounday that concde wth CFP of that obot and the convex hull. We take the pont space of sample ponts of the obots n the cluste that mnmzng the obectve functon Ob p C / P k Hee p s the pependcula dopped on to the lne onng the obots cuent pose to the goal fom the sample pont. C k 1 fo any pa of obots, that k ae n collson n the cluste P k s the dstance between, at the espectve sample pont consdeed. The k fst tem n the obectve functon pevents lage devatons fom the lne onng the cuent pose to the taget, the second nceases spacng between the obots. It s to be noted agan that the obectve of evaluatng Ob s only to obtan bette shape ove taectoes fo the obot. The seach space can be educed as small as possble by choosng abtaly small numbe of sample ponts wthout affectng the ablty of the method to k k chose a collson fee taectoy fo that s guded based on the theme dscussed n the subsectons A,B. E. Dependency Gaph and Collson Cluste Robots that have a collson amongst them s shown by a bdectonal lnk n the dependency gaph. Fo a snapshot shown n fgue 6a, obot pas (R1, R2), (R1, R3), (R4, R5), (R5, R6) detect collsons amongst them wthn a stpulated tme. The dependency gaph fo such a stuaton s shown n fgue 6b. All obots that ae n the same connected component of the dependency gaph belong to the same collson cluste. Hence fo the gaph of fgue 6b thee ae two clustes once consstng of obots (R1,R2,R3) and the othe (R4, R5, R6). Thus the algothm fo collson avodance on the obot takes the fom shown below ALGORITHM: Collson Avodance 1. Untl all obots each the goals, do steps 2 to 3 2. Fnd the dependency gaph of obots yet to each the goals and fom clustes 3. Fo each cluste C do step 4 4. Fo all obots R n C do steps 5 to 7 5. Compute the CFP and obtan sample ponts. 6. Evaluate Ob and fnd the collson fee pont fo each obot fom the sample ponts obtaned n 5 that mnmzes Ob. 7. Tavese to the pont obtaned n 6 fo each obot untl one o all the obots each to the obtaned ponts whle checkng fo the below condtons. )If Robot fom othe cluste fnds collson wth the cuent cluste, go to step 1. ) If goal s eached, go to step 1. F. Complexty Fg. 6a. Robots R1, R2 and R1, R3 ae n collson and ncluded n one cluste. Fg. 6b. Robots R4, R5 and R5, R6 ae n collson and ncluded n anothe cluste. The cuent algothm complexty: Fo the obots, each contane polygon fo the obot has 4 ponts. Hence thee ae n = 4 ponts to be consdeed. So when done n a dstbuted way a obot

5 fnds ntesecton wth emanng (-1) polygons; t does so n O nlog n tme [12]. Indeed [12] pesents an O n 4 / 3 c 1 algothm that fo lage n s qucke than n O nlog computaton. If thee ae m ntesecton ponts of the -1 polygons wth the cuent one, the convex hull of those m ponts can be found n O mlog m. Hence the oveall complexty s OnLogn mlogm c. Hee c s a constant compensatng the tme taken fo gowng the polygons and the constant c 1 fom the ntesecton computatons. Complexty fo choosng next waypont by exhaustve seach tempeed wth heustcs: Among obots each obot seaches along m paths fo the next waypont o mlestone. Thee ae O m seaches fo fndng a mlestone that s collson fee wth all othes n wost case. Each path to such a mle stone s dvded nto k sub locatons and collsons ae checked at each of those k locatons. Hence n the wost case the computaton tme s O km. If the fst path ntegal to the new mlestone s a collson fee path, f that s the case fo all the obots, fo the k sub dvsons of obots fst paths the numbes of collson checks that wee pefomed, s O k. Ths s the best case scenao, whee the fst path seached fo all the obots wee collson fee. Whle effcent heustcs can O km they can neve cetanly make thngs bette than guaantee O k except n vey exceptonal stuatons of obot confguatons. Howeve the tme complexty of the pesent method s always the same. IV. Smulaton Results pevously, had detected collson wth a obot comng towads t, and the total cluste of thee stated avodng the collsons cumulatvely. Fgue 7d depcts the obots havng eached the fnal goals though contnuous cuvatue paths afte avodng each othe. Fgues 8a-8c depcts 12 obots case wth multples of fou obots colldng smultaneously, and eachng towad the espectve goals. Fgue 7a depcts the ntal polygons computed mmedately afte fndng the collsons. Fgue 7b shows an ntemedate stage dung the collson avodance maneuve. Fgue 7c shows the obots eachng the goals and the espectve paths of the obots. It must be noted that n both the scenaos the paths of the obots espect the knodynamc constants of the obot. Fg. 7b. The 3 obots n top fomng collson cluste. Fg. 7c. Intemedate state of obots n 7a. Fg. 7a. Intally fve obots avng n to the scenao. Below ae the smulatons of 7 and 12 obots n dffeent scenaos, whee each obot havng ts goal locaton maked as astesk. Fgue 7a to 7d depcts the 7 obots case wth each obot encounteng collson wth few othe obots sequentally. Intally thee ae fve obots n the envonment, n whch the two obots n top left have mmedate collson and stated avodng each othe. Fgue 7b depcts a state whee the obot at top left whle avodng the collson wth a obot encounteed Fg. 7d. Robots n 7a wth smooth cuvatue paths eached the espectve goals. A. Compason wth heustcs: Table 1 depcts the tme consumed by the pesented algothm and the dffeent heustcs, fo clutteed and scatteed cases. We compaed wth thee dffeent heustcs the tme taken to fnd a collson fee path to the next waypont, on a Pentum-3 pocesso wth 256MB memoy, though MATLAB smulatons. Fst

6 heustc chooses the next way pont based on the followng ode: exteme left wth maxmum acceleatons, exteme ght wth maxmum acceleatons, exteme left wth aveage veloctes, exteme ght wth aveage veloctes, mmedate deceleaton to ethe left o ght. Smlaly n the second heustc we have the poty ode: exteme left wth maxmum acceleaton, exteme ght wth aveage veloctes, and mmedate deceleaton. The thd heustc smply deceleates to ethe left o ght. The fst ow shows the compason fo clutteed case, and the second ow shows the esults fo obots whch ae fa enough befoe detectng the collson. It can be seen that n all the cases except fo the thd heustc, the pesented algothm pefomed well. The pefomance gan of the cuent algothm s moe ponounced n a clutteed than a scatteed envonment. Ths s ndeed expected snce the seach begns to gow exponentally n heustc appoaches when clutte s moe. Note that the thd heustc s uttely tval as t smply stops all the obots and hence has neglgble utlty. Also t s obseved that these heustcs pefom well fo cetan cases only, fo the est of the cases the computaton tme gows exponentally. The compasons ae epoted n seconds. The tme fo each opeaton fo all methods would have been much faste had they been done n C/C++ athe than MATLAB. Fg. 8a. 12 Robots n clutteed stuaton fomng fou clustes, the ntal polygons ae shown n geen, and sample ponts n blue. Fg. 8c. Robots n 8a eachng the goals, along the contnuous smooth paths. V.CONCLUSIONS Fg. 8b. Robots n 8a at ntemedate collson avodance. Scenao Pesent algothm H1 H2 H3 clutteed scatteed Tab. 1. Compason between pesented method and vaous heustcs n scatteed and clutteed envonments. All values shown n seconds. A method fo collson avodance among multple obots though collson fee polygons has been pesented.. The essental contbuton of ths pape has been the computaton of a collson fee path tll the next way pont n a sequence of two log lnea opeatons. Heustc appoaches that compute a collson fee path to the next waypont n andomzed seach methods nvaably tend to become exponental n the numbe of obots n clutteed scenaos howsoeve good the heustcs ae desgned. The method has shown to be useful fo onlne collson avodance and goal eachng maneuves that espect the knodynamc constants of the obot. Extensve compasons wth caefully desgned heustcs show the pefomance gan of ths method ove such heustcs.

7 REFERENCES [1]J. Baaquand and J. C. Latombe., A monte-calo algothm fo path plannng wth many degees of feedom. Poceedngs of the IEEE Intenatonal Confeence on Robotcs & Automaton (ICRA),, [2]Svetska P. and Ovemas M, Coodnated moton plannng fo multple ca-lke obots usng pobablstc oadmaps Poceedngs of the IEEE Intenatonal Confeence on Robotcs & Automaton (ICRA), 1995 [3]Bennewtz M., Bugad W. and Thun S., Fndng and Optmzng Solvable Poty Schemes fo Decoupled Path Plannng Technques fo Teams of Moble Robots, Poceedngs Robotcs and Autonomous Systems, 41 (2), pp 89-99, 2002 [4]F. Gavot and R. Alam, An extenson of the plan-megng paadgm fo mult-obot coodnaton, Poceedngs of the IEEE Intenatonal Confeence on Robotcs & Automaton (ICRA), [5]S. Leoy, J. P. Laumond, and T. Smeon, Multple path coodnaton fo moble obots: A geometc algothm, Poceedngs of the Intenatonal Jont Confeence on Atfcal Intellgence (IJCAI), [6] O. Bock and L. Kavak, Decomposton-based moton plannng: a famewok fo eal-tme moton plannng n hgh-dmensonal confguaton spaces, Poceedngs of IEEE Intenatonal Confeence on Robotcs and Automaton, vol. 2, [7] S. LaValle and J. Kuffne, Randomzed knodynamc plannng, Intenatonal Jounal of Robotcs Reseach, vol. 20, no. 5, pp , May [8] J. Buce and M. Veloso, Real-tme andomzed path plannng fo obot navgaton, Poceedngs of Intenatonal Confeence on Intellgent Robots and System (IROS), Octobe 2002 [9] M Zucke, J Kuffne and M Bancky, Multpatte RRTs fo Rapd Replannng n Dynamc Envonments, ICRA 2007 to appea. [10] C.M. Clak and Stephen M. Rock and J.C. Latombe, Moton Plannng fo Multple Moble Robots usng Dynamc Netwoks Poceedngs of IEEE Intenatonal Confeence on Robotcs and Automaton, ICRA 2003: [11] Fox D, Bugad W, and Thun S., The Dynamc Wndow Appoach to Collson Avodance, IEEE Robotcs & Automaton Magazne, 4(1) [12] P. Gupta, R. Janadan and M. Smd, "Effcent algothms fo countng and epotng pawse ntesectons between convex polygons'', Poceedngs of Infomaton Pocessng Lettes, (69), pp , 1999.