Controlling formations of multipl mobil robots with intr-robot collision avoidanc H.M. Ha, A.. Nguyn and Q.P. Ha ARC Cntr of Excllnc for Autonomous Systms, Faculty of Enginring, Univrsity of Tchnology, Sydny PO Box 123 Broadway NSW 2007 AUSTRALIA Email: {Huy.M.Ha@studnt,adnguyn@ng,quangha@ng}.uts.du.au Abstract In this papr, w invstigat th problm of intr-robot collision avoidanc in multipl mobil robot formation control. Two mthodologis ar utilizd, namly Virtual Robot tracking by [Jongusuk and Mita, 2001] and l-l control by [sai t al., 1998] to stablish formation and avoid collision among robots. W point out that th framwork in Virtual Robot tracking is potntially subjct to collision among robots. This drawback is ovrcom in our dsign by incorporating a diffrnt ractiv schm in th incidnt possibility of collision. To prov th advantags of our framwork, w dmonstrat in simulation th cas of thr robots moving in formation and avoiding intr-robot collisions. 1 Introduction Th issu of control and coordination for multipl mobil robots hav rvolvd around two major tasks; first, th robot platoon must maintain dsird shaps such as a lin, a column or a ring formation. Th motivation is that multipl robots ar capabl of prforming many applications that singl robots cannot. Exampls of ths applications includ box pushing [Lwis and Tan, 1998], load transportion [Johnson and Bay, 1995] and capturing/nclosing an invadr [Yamaguchi, 1999].Scond, th robots hav to simultanously avoid collisions btwn thmslvs and with obstacls in th nvironmnt. Essntially, thr ar thr approachs in controlling multipl mobil robots formation, namly: ladrfollowing, bhavior-basd and virtual-structur, s [Nguyn t al., 2004] and rfrncs thrin. Whil th virtual structur approach utilizs cntralizd controllrs, th ladr-following and bhavioral approachs oftn apply dcntralizd controllrs using local information. To dal with collision avoidanc, som rsarchrs usd optimal motion planing [Rds and Shpp, 1990; Kavraki t al., 1996], which can b vry computationally xpnsiv, whil othrs usd som typ of fdback control with ractiv schms [sai t al., 1998; Bicho and Montiro, 2003; Ögrn and Lonard, 2003]. Ths fdback controls com with formal proofs of satisfactory systm prformanc and formation acquisition. On advantag of ths schms is that thy can b applid to small, htrognous robots with limitd communication rang. In th contxt of ladr-following control, th problm of collision btwn robots in transint phas is important, although has not bn xplicitly addrssd. Jongusuk and Mita [2001] hav introducd an intrsting ida for tracking control of multipl mobil robots using virtual robots (VR) combind with l-l control, by [sai t al., 1998], in a obstacl-fr nvironmnt. Howvr, th VR control mthod dos not ncssarily guarant accptabl collision avoidanc among robots in som cass. In this papr, a rmdy for th problms associatd with thir mthod is proposd. A ractiv control switching schm is utilizd to avoid collisions among robots. This ractiv control switching schm applis diffrnt paramtrs to l-l control to lad th robots to saf positions for formation achivmnt with minimum numbr of control switchings. Th rst of th papr is organizd as follows: in sction 2 w prsnt th VR control framwork and discussions on its waknsss. Sction 3 shows our control dsign and sction 4 discusss how it accounts for th VR control problms. Simulation rsults ar prsntd in sction 5 and finally in sctions 6, a conclusion is drawn togthr with dicussion on futur rsarch dirctions. 2 Tracking Control of Multipl Mobil Robots 2.1 Problm Formulation Th VR problm formulation is adoptd, a platoon of unicycl mobil robot is considrd, whos kinmatic
modl of ach robot is givn by q i = B i u i = cos θ i 0 sin θ i 0 u i, i = 1, 2,..., n; (1) 0 1 whr q i = [x i, y i, θ i ] T is th stat vctor, (x i, y i ) is th position in global fram and θ i is th orintation; and u i = [v i, ω i ] T is th control input, with v i bing th translational vlocity and w i is th angular vlocity, of robot i. In addition, th robots satisfy non-holonomic vlocity constraints, which ncompass pur-rolling and non-slipping conditions, non slipping : ẋ i sin θ i ẏ i cos θ i = 0, (2) pur rolling : ẋ i cos θ i + ẏ i sin θ i = v i. (3) Assumptions (i) Robots ar of th sam modl and satisfy nonslipping and pur-rolling constraints. (ii) Th workspac is flat and contains no obstacl. (iii) Th rfrnc robot follows a smooth trajctory and maintains positiv vlocity. (iv) Each followr robot is indxd by a distinctiv priority numbr and awar of othrs indxs. (v) Each robot can xtract ncssary information via its communication quipmnt. Problm statmnt Giving initial positions and orintations of th followr robots and th motion of th rfrnc robot, th objctiv is to dsign for followr i such that as t, 1. Formation is stablishd. 2. No collision among robot i and any robot j. 3. A ovrall motion that satisfis th limitation of communication rang. 2.2 Virtual Robot Tracking Th concpt of VR is usd to avoid collisions btwn th followr robots and th rfrnc robot. Th virtual robot is a hypothtical robot bing placd such that it has r-l clarancs from th followr and th sam orintation. In this cas, l dfins longitudinal claranc and r dfins claranc along th whl axis. not q r = [x r, y r, θ r ] th rfrnc robot s stat vctor, q i = [x v i, y v i, θ v i] th VR of followr i, i = 1, 2,..., n Th rlationship btwn VR and th followr robot is as follows, x vi = x i r sin θ i + l cos θi y vi = y i + r cos θ i + l sin θi θ vi = θ i. (4) Th kinmatic modl of VR is thn, q vi = cos θ i r cos θ i l sin θ i sin θ i r sin θ i + l cos θ i u i 0 1 [ ] B vi = u i. (5) 0 1 Th ida of VR tracking is to us VR to track th rfrnc robot, thn th followr will approach th dsird position in th formation as its VR approachs th rfrnc robot. In Figur 1, it is shown that th VR approach th rfrnc robot in a intrnal shap x and y. tails about th controllr can b found in [Jongusuk and Mita, 2001]. Not that B vi must b non-singular or l must b diffrnt from zro, which mans a lin formation cannot b achivd. y q r x l Followr i VR i Figur 1: VR tracking modl 2.3 l-l Control This control concpt was introducd in [sai t al., 1998] for stablishing multipl mobil robot formation. Th aim of this control is to maintain th dsird lngths, l d 13 and l d 23 of a robot (robot numbr 3) from its two ladr robots (robot numbr 1 and 2 in Figur 2). Th kinmatic quations for robot 3 ar givn as follows, l 13 = v 3 cos γ 1 v 1 cos ψ 13 + ω 3 sin γ 1 l 23 = v 3 cos γ 2 v 2 cos ψ 23 + ω 3 sin γ 2 θ = ω 3. r q i (6) whr γ i = θ i + ψ i3 θ 3, (i = 1, 2). tails about th control can b found in [sai t al., 1998]. Also not th singularity cas whn sin(γ 1 γ 2 ), th control law is undfind. 2
Prpar initial paramtrs for robot i-th y 13 q 1 Robot 1 l 13 Collision? YES q 3 NO High Priority? Low Robot 3 l 23 y 23 q 2 Formation Controllr (l-l control) Collision Controllr (l-l control) Robot 2 Figur 2: Notation for l-l control modl 2.4 Controllr sign tcting Collision In Figur 3, th solid circl, which covrs th whol robot cntring at th control point, has radius and d is th rquird clarancs btwn robots. Lt (x i, y i ) and (x j, y j ) dnot th control points of robot i and j, thn th distanc btwn robot i and j is: ρ ij = (x i x j ) 2 + (y i y j ) 2. (7) On has thrfor, ρ ij > 2( + d) saf (8) ρ ij 2( + d) collision (9) t = tfinal? YES EN Figur 4: Algorithm flow chart th lattr do not hav to chang thir control laws. Th algorithm is outlind in Figur 4. With rgards to how to choos dsird lngths for l-l control, Jongusuk and Mita dfins two situations, whn th targt is insid th accssibl ara of low priority robot, th shadd aras in Figur 5 ;and whn its outsid that ara. T G 1 and T G 2 ar th targts of followr 1 and 2, rspctivly; and is whr followr 2 will b ld to using l-l control with paramtrs d and l23. d pnding on ach situation, th dsign of d and l23, d which is quivalnt to dsign of, will b chosn accordingly. Followr 1 d TG 2 TG 1 TG 2 TG 1 Followr 1 _ R _ R Figur 3: Collision Avoidanc Modl Control Algorithm Jongusuk and Mita [2001] us VR tracking for formation stablishmnt and collision avoidanc btwn th followr robots and th rfrnc robot; and l-l control for collision avoidanc among th followr robots. Essntially, whn collisions ar dtctd, th followr robots with lowr priority should switch to l-l control to avoid collision with thos robots having highr prioritis, whil Figur 5: [Lft] Targt TG 2 is outsid accssibl ara [Right] Targt TG 2 is insid accssibl ara 2.5 iscussion Whn a VR tracks th rfrnc robot, th solution xponntially convrgs to an rgion boundd by x and y, shown in Figur 1. Howvr, this dos not ncssarily guarant that a followr robot will not collid with th rfrnc robot. It is obsrvd that during th track- 3
ing procss, th VR rotats and th followr gos insid th intrnal rgion x and y, or initially th followr robot is insid th intrnal rgion, as shown in Figur 6, thn it may collid with th rfrnc robot. y q r x q i r VR i Followr i Figur 6: Potntial collision with VR Anothr problm ariss with th dsign of whn collision happns. Sinc can only b within th accssibl ara and TG 2 may b outsid of that ara, it is not guarantd that whn switching back to VR control, followr 2 can track TG 2 without colliding with followr 1 and this can happn rpatdly. For xampl, whn TG 2 is vry nar th lin conncting th rfrnc robot and TG 1, sinc followr 2 cannot dirctly go around followr 1 to th targt, probably many collisions will occur bfor followr 2 rach its dsird position. To dal with ths problms, w propos ractiv anothr schm combining VR and l-l control that can guarant collision avoidanc btwn robots. Firstly, this schm first apply l-l control to dal with collisions btwn th followr robots and th rfrnc robot. Scondly, in cas of collisions among th followr robots, it will minimiz th numbr of collisions by using l-l control intlligntly. 3 Proposd Control Framwork sign Th control framwork hr is dsignd for th cas of thr robots; on rfrnc robot and two followr robots. In sction 2.5, it has bn pointd out that VR tracking dos not guarant collision avoidanc btwn followr robots and th rfrnc robot, thus anothr mchanism to nsur collision avoidanc is ndd and l-l control will b usd for this purpos. Th ida is that should collision occur btwn any followr robot i and th rfrnc robot according to th collision dtction critria proposd by [Jongusuk and Mita, 2001], l-l control will l b usd to driv th followr robot to divrg but hads to targt position so that collision will most likly not happn aftr switching back to VR tracking. In th control framwork, four main cass will b considrd as follows. In cas 1, thr is a potntial collision btwn a followr forbot and th rfrnc robot. In cas 2, thr is a potntial collision btwn th two followrs and th distanc btwn th high priority followr and th rfrnc is sufficint for th lowr priority robot to go btwn. Cas 3 covrs similar situations in cas 2 xcpt that th distanc btwn th high priority followr and th rfrnc is insufficint. Lastly, cas 4 includs situations whn th low priority followr has potntial collisions with both high priority followr and th rfrnc robot. To dal with ths cass, th VR of th rfrnc robot (VRR) is introducd as a VR with -r and -l clarancs from th rfrnc robot, whr r-l ar th dsir clarancs of followr i. This virtual robot will b at th dsird position of followr i in th formation. Th dtail control for ach cas will b dscribd in th following sctions. Cas 1 Th followr robot will switch to l-l control in ordr to go to, as illustratd in Figur 7, by using two ladrs: th rfrnc robot and th VRR, with l d 13 and l d 23 dsignd as follows, d = 2( + d) + + δ l23 d = r 2 + l 2 2( + d) (10) + δ. Followr i l 2(+d) r VRR Figur 7: Using l-l control in collision btwn followr i and th rfrnc robot 4
Using l-l control with th abov paramtrs will driv followr i closr to th dsird position whil going around th safty boundary of th rfrnc robot, which is a circl whos diamtr is 2( + d) cntring at th rfrnc robot s control point. l-l control is usd to driv followr i not dirctly to its dsird position bcaus driving followr i to go nar th safty boundary of th rfrnc robot will mak it lss likly to collid with th othr robot. Th rason that P is 2( + d) + away from th rfrnc robot rathr than 2( + d) is whn using l-l control, th distanc from th front castor of th third robot, or followr i, to th control point of its ladr, or th rfrnc robot, is considrd instad of th distancs btwn thir control points, or ρ ri. Thus in ordr to nsur ρ ri will b sufficint for collision avoidanc, w hav to incras d by th distanc from th third robot s control point to its front castor, which is. In practic, on can not driv followr i to P bcaus P lis on th lin conncting th rfrnc robot and its VR, whr th control is undfind du to singularity. Anothr rason is that w nd to nsur th distanc btwn followr i and th rfrnc robot to b stricly gratr than 2( + d) + to thoroughly avoid collision. For ths rasons, a small positiv amount δ is dlibratly augmntd to both d and l23. d Cas 2 Thr is a potntial collision btwn th two followrs and ρ r1 > 4( + d), as dpictd in Figur 8. Followr 2 (lowr priority) will hav to apply l-l control to avoid collision. In this cas, ladr 1 is th followr 1 and ladr 2 is th VR of th rfrnc robot. d is th sam as in (10) and l23 d is similar to th on in (10) xcpt that r 2 + l 2 is rplacd by distanc btwn VRR and followr 1, or ρ rr1. Thrfor, d = 2( + d) + + δ l23 d = ρ rr1 2( + d) + δ. (11) Howvr, thr ar situations whr if (11) is applid, thn it is most likly that followr 2 will collid with both th rfrnc and followr 1 in attmpting to go to th dsird position, as dpictd in Figur 9. This usually happns whn targt TG 2 is in opposit half plan dividd by th lin conncting followr 1 and th rfrnc robot, and th distanc btwn th rfrnc robot and followr 1 is lss than or qual to 4( + d). Hnc th nd for spcial tratmnts in such situations. Cas 3 Thr is a potntial collision btwn followr robot 2 and followr robot 1, and ρ r1 4( + d). l-l control will b usd to driv followr 2 to go bhind followr 1. Th spcific mthod is shown in Figur 9, whr followr 2 is drivn to and th lin conncting and followr TG 2 Followr 1 2(+d) Figur 8: Using l-l control in collision btwn followrs 1 is always prpndicular to th lin conncting followr 1 and followr 2. This is accomplishd by stting ladr 1 in l-l control to b followr 1, and ladr 2 to b a virtual robot placd at, with l d 13 and l d 23 ar as follows, d = 2( + d) + + δ l23 d = δ. (12) Effctivly, followr 2 will rvolv around followr 1 safty rgion clockwis, or countr-clockwis whn followr 2 is on th lft sid of TG 2 in Figur 9. This control is applid until followr 2 scaps collision with followr 1, and TG 2 and followr 2 ar on th sam sid with rspct to th lin conncting followr 1 and th rfrnc robot. This also mans followr 2 hav bn drivn to go outsid of th accssibl ara in Figur 5. Thn w can apply VR tracking again to driv followr 2 to its targt TG 2. This will guarant that followr 2 can always go to th dsird position rgardlss of th positions of th rfrnc robot and followr 1. Cas 4 should apply l-l control to go to a saf position. That position should b at last 2( + d) + away from othr robots. δ is augumntd to l d 13 and l d 23 for th sam rason as xplaind abov. Thrfor, d = 2( + d) + + δ l23 d = 2( + d) + + δ. (13) 5
TG 2 Followr 1 robot cas. Th simulation is implmntd in Matlab and Simulink. Th aim of this simulation is to validat if collision is dtctd and avoidd proprly using our approach. W assum that thr is no paramtr variations or xtrnal disturbancs. Initial paramtr ar st as follows, - Common : = 3, d = 1, δ = 0.1 - : q r (0) = [100, 0, 0] T, u r = [2.5, 0] T - Followr 1 : q 1 (0) = [90, 10, 0] T, (r, l) = (10, 10), - : q 2 (0) = [90, 30, 0] T, (r, l) = ( 10, 1) 2(+d) Figur 9: Using l-l control to driv followr 2 tak rar rout around followr 1 4 iscussion Using this dsign framwork, th drawbacks with th original dsign by [Jongusuk and Mita, 2001] hav bn ovrcom and a collision-fr movmnt for a group of thr robots is achivd. Th framwork hav combind VR tracking control and l-l control to avoid collision btwn robots, at th sam tim driving robots to thir dsird positions with minimum ffort. Whn followr 2 has to tak th rar rout around followr 1, it attmpts maintains th minimum distanc with followr 1, which also mans that it has maintains minimal communication rang with th rfrnc robot whil avoiding collision with followr 1. This should sufficintly satisfy th limitd communication rang rstriction. This framwork can b xtndd to th multipl robot cas, whr th platoon of robot can b dividd into multipl thr-robot groups. Thn ach of th thrrobot groups can b tratd as an individual unit, with control points bing its th rfrnc robot. Th safty boundary can b xtndd from th rfrnc robot so that it covrs all thr robots in th group; and th sam or anothr control framwork can b applid to avoid collision btwn groups in a obstacl-fr nvironmnt. To incorporat obstacl avoidanc, w can us similar stratgy found in [sai t al., 1998] using l-l control, whr on of th distancs is th distanc to th obstacl. 5 Simulation rsult In this sction, w show th implmntation of our controls and control framwork in simulation for th thr Assum that followr robot 2 has th lowst priority and th rfrnc robot has th highst on. Figur 10 dpicts how followr 2 dtcts collision with followr 1 and try to go bhind followr 1. In this figur, R stands for th rfrnc robot, F1 stands for followr robot 1 and F2 stands for followr robot 2. Figur 11 shows th distancs btwn followr 2 and rspctivly th rfrnc robot and followr 1 during that tim, whr l 13 is th distanc btwn th rfrnc robot and followr 2 and l 23 is th distanc btwn followr 1 and followr 2. First, followr 2 had apparntly an incidnc of collision with followr 1 nar x = 100 and y = 20 and at around th 2.5 th scond, whn attmpting to go to th dsird position. It thn dcidd that it must tak th rar rout bcaus followr 1 and th rfrnc ar too clos, by applying th mthod dscribd in Figur 9. Aftr taking th rar rout, followr 2 wnt to th sam sid as its dsird position with rspct to th lin conncting followr 1 and th rfrnc robot. It applid VR tracking control again, rsulting in anothr collision possibility with followr 1 nar x = 100 and y = 20 and at th 6 th scond. At this momnt, it applid th mthod dscribd in Figur 8 to avoid collision. Aftr that, whn switching back to VR control, it had an incidntal collision with rfrnc robot nar x = 130 and y = 0 and at th 14 th scond. This tim it applid th mthod dscribd in Figur 7 to go around th rfrnc and finally, followr 2 approachd th dsird position. 6 Conclusion In this papr, w hav prsntd a nw approach for controlling multipl mobil robots (thr-robot cas) in formation using ladr-following stratgy whil nsuring collision-fr movmnt. W introducd a nw framwork using th combination of VR tracking control and l-l control. This framwork is provn to ovrcom prvious shortags including potntial collision with th rfrnc robot and too many collisions among th followr robots. W hav illustratd som of our framwork s advantags in simulation. 6
y(dm) 30 20 10 0 10 20 30 40 80 100 120 140 160 180 200 220 240 x(dm) Figur 10: avoid collision with followr 1 and th rfrnc robot. Rlativ distancs(dm) 35 30 25 20 15 10 5 0 5 10 15 20 25 30 35 40 45 50 Tim(sc) Figur 11: istancs from followr 2 to th rfrnr robot and followr 1. W plan to xtnd our framwork to multipl robots and incorporation of collision avoidanc as w hav mntiond in our discussion. This is possibl sinc th us of l-l control for collision avoidanc hav bn dmonstratd in [sai t al., 1998]. Graph thory will also b usful in dsigning formation chang for obstacl avoidanc; for instanc, chang to column formation whn moving through narrow passag way. Work is undr way towards tsting our framwork s adaptation capability to such formation chang. W ar also intrstd in incorporating som tratmnt for paramtr prturbations and xtrnal disturbancs. Inspird by Ögrn and Lonard [2003], who dalt with paramtr uncrtaintis indirctly by dfining a uncrtainty rgion around ach robot, w plan to us Variabl Structur Systms (VSS) approach as a tratmnt to th problm of paramtr uncrtaintis and disturbancs, making us of th wllrcognisd proprty of robustnss of th VSS approach. Ths topics ar th subjct of our futur study. Acknowldgmnts This work is supportd, in part, by th ARC Cntr of Excllnc programm, fundd by th Australian Rsarch Council (ARC) and th Nw South Wals Stat F1 F2 R Govrnmnt. s [Bicho and Montiro, 2003] Estla Bicho and Srgio Montiro. Formation control for multipl mobil robots: a non-linar attractor dynamics approach. In Procdings of th 2003 IEEE/RSJ Intl. Confrnc on Intllignt Robots and Systms, pags 2016 2022, Las Vgas, Nvada, Octobr 2003. [sai t al., 1998] Jaydv P. sai, J. Ostrowski, and Vijay Kurma. Controlling formations of multipl mobil robots. In Procdings of th 1998 IEEE Intl. Confrnc on Robotics and Automation, pags 2864 2869, May 1998. [Johnson and Bay, 1995] P. Johnson and J. Bay. istributd control of simulatd autonomous mobil robot collctivs in payload transportation. Autonomous Robots, 2(1):43 64, 1995. [Jongusuk and Mita, 2001] Jurachart Jongusuk and Tsutomu Mita. Tracking control of multipl mobil robots: A cas study of intr-robot collision-fr problm. In Procdings of th 2001 IEEE Intl. Confrnc on Robotics and Automation, pags 2885 2890, Soul, Kora, May 2001. [Kavraki t al., 1996] L. Kavraki, P. Svstka, J. C. Latomb, and M. H. Ovrmars. Probabilitic roadmaps for path planning in high dimnsional configuration spac. IEEE Transactions of Robotics and Automation, 12(4):566 580, 1996. [Lwis and Tan, 1998] M. A. Lwis and K. Tan. High prcision formation control of mobil robots using virtual structurs. Autonomous Robots, 4:387 403, 1998. [Nguyn t al., 2004] A. Nguyn, Q. P. Ha, S. Huang, and H. Trinh. Obsrvr-basd dcntralizd approach to robotic formation control. In Procdings of th 2004 Australian Confrnc of Robotics and Automation, pags 1 8, Canbrra, Australia, cmbr 2004. [Ögrn and Lonard, 2003] Ptr Ögrn and Naomi E. Lonard. Obstacl avoidanc in formation. In Procdings of th 2003 IEEE Intl. Confrnc on Robotics and Automation, pags 2492 2497, Taipi, Taiwan, Sptmbr 2003. [Rds and Shpp, 1990] J. A. Rds and L. A. Shpp. Optimal paths for a car that gos both forwards and backwards. Pacific. Journal of Math., 145(2):367 393, 1990. [Yamaguchi, 1999] H. Yamaguchi. A cooprativ hunting bhavior by mobil-robot troops. Th Intl. Journal of Robotics Rsarch, 18(8):931 940, Sptmbr 1999. 7