INITIALIZATION OF ROBOTIC FORMATIONS USING DISCRETE PARTICLE SWARM OPTIMIZATION

24 Internatonal Symposum on on Automaton & Robotcs n n Constructon (ISARC 2007) Constructon Automaton Group, I.I.T. Madras INITIALIZATION OF ROBOTIC FORMATIONS USING DISCRETE PARTICLE SWARM OPTIMIZATION V.T. Ngo ARC Centre of Excellence for Autonomous Systems, Faculty of Engneerng Unversty of Technology, Sydney PO Box 123 Broadway NSW 2007 AUSTRALIA ngo652@eng.uts.edu.au N. M. Kwok and Q. P. Ha ARC Centre of Excellence for Autonomous Systems, Faculty of Engneerng Unversty of Technology, Sydney PO Box 123 Broadway NSW 2007 AUSTRALIA ABSTRACT In s paper, a Dscrete Partcle Swarm Optmzaton (DPSO) technque s appled as an optmal moton plannng strategy for e ntalzaton of moble robots to establsh some geometrcal patterns or formatons. Frstly, an optmal assgnment of locaton and orentaton for each robot n e formaton s performed. For s, a total travellng cost s defned, nvolvng e angular and translatonal movement by all robots from er ntal postons n e workspace to er goal postons. The objectve s en to determne e poston for each robot n e formaton n order to mnmze e cost functon to form a predefned shape. Once each robot has been assgned w a desred poston, a search scheme s mplemented to obtan a collson free trajectory for each robot to establsh e formaton. Smulaton results are presented to llustrate e valdty of e proposed approach. KEY WORDS Moton Plannng, Robotc Formaton Intalzaton, Poston Assgnment, Dscrete Partcle Swarm Optmzaton. 1. INTRODUCTION Robotc formaton s defned as e coordnaton of a group of moble robots to get nto and mantan a desred pattern w a certan geometrc shape. Mult-robot coordnaton has recently receved a consderable nterest as varous applcatons can be performed faster and more effcently w multple robots an w sngle robots operatng ndependently. Applcatons requrng multple robot coordnaton nclude mne sweepng [1], movng large objects n a constructon ste or mltary scouts [2-4], where each robot needs to be n a partcular poston correspondng to ts sensng capacty or geometrcal advantage to perform a gven cooperatve task. Such problems as ntalzng e formaton, mantanng e formaton confguraton, avodng statc or movng obstacles n e workspace, and changng formaton shapes to fulfl a specfc task or to deal w obstacles reman mportant ssues n robotc formaton control [5]. Several approaches to solve one or more of e above subtasks can be found n e lterature [6-9]. In [10], a drected potental feld meod was used for moton coordnaton of robots movng n formatons, however, e nherent drawback of potental feld meod n e cases of local optma was not mentoned. An archtecture for robotc formaton control was proposed n [5] to solve e four man ssues mentoned above. The auors focused on e problem of how to assgn e most sutable poston for each robot n e formaton usng a bounded dep-frst search w a prunng algorm to mnmze a cost functon compromsng e level of a follower robot n a formaton, e dstance and headng devaton of a follower w respect to ts leader robot. The search s dstrbuted over every robot n e formaton and e result w e lowest cost s

356 chosen. Ths meod s somewhat smlar to e work proposed by [11], n whch a robot n e group plans ts own pa ndependently and a D* search n a coordnaton dagram s performed to obtan e mnmum cost trajectory for each robot n a mult-robot system. Obvously, a search process for s purpose would requre a sutable optmsaton technque. Partcle Swarm Optmzaton (PSO), orgnally developed by Kennedy and Eberhart [12], has proven to be of great potental for optmzaton applcatons and has been used successfully n robotcs. For example, PSO was used for target searchng problems w sngle and multple target cases [13]. In [14], a modfed PSO algorm was ntroduced to fnd an optmal pa for moble robots. In constructon automaton, a PSO-based has been recently proposed for e coordnaton of a group of robotc vehcles [15]. Dealng n a smlar applcaton, e Generalzed Dscrete Partcle Swarm Optmzaton (DPSO), proposed by Clerc [16], was appled for e Travellng Salesman Problem. The auor ponted out at DPSO s easly mplemented for dscrete or combnatonal problems, partcularly when a suffcently good specalzed algorm s unavalable. Ths paper addresses e problem of how to ntalze a robotc formaton by seekng an optmal poston assgnment for each robot n e formaton. Each robot wll be desgnated a desred poston whle mnmzng e total cost whch ncludes e translatonal and angular movement of all robots n e formaton. The workspace s assumed to be obstacle-free so e shortest pa for each robot from ts ntal poston to ts assgned poston wll be a straght lne connectng e two postons. As each robot moves to ts goal, collson between two or more robots may occur. We propose a search scheme to construct e velocty profle for ose robots whch may potentally collde w e oers. The rest of e paper s organzed as follows. In Secton 2, a bref revew of DPSO s presented. The proposed robotc formaton ntalzaton approach s descrbed n Secton 3. Secton 4 gves some smulaton results to demonstrate e valdty of e approach. A concluson s gven n Secton 5 of e paper. V. Ngo, T. Ngo, V. T, N. N. M. M. Kwok & Q. P. Ha 2. DISCRETE PARTICLE SWARM OPTIMIZATION In Partcle Swarm Optmzaton, a set of movng partcles are ntally rown nto e search space. Each partcle, havng a poston and a velocty, knows ts own poston and e ftness functon to evaluate ts soluton qualty. Each partcle randomly searches rough e problem space by updatng ts own memory w ts poston and e socal nformaton gaered from oer partcles. At each tme step, e movement of a partcle s a compromse of ree behavours: to move randomly (ts own way), to go towards ts best prevous poston (memory), and to go towards ts neghbour (global) best poston. Ths evolutonary selecton s descrbed by e followng equatons for e partcle: v, new c1 v, old c2 p, new p, old v, new where v, new v, old p, new p, old p, b g b c, c 1 c2, 3 p p c g p, b, old New velocty calculated for e Velocty of e prevous teraton New poston calculated for e Poston of e teraton Best poston of e 3 b partcle partcle from e partcle partcle from e prevous partcle so far Best poston of from e neghbour so far Socal/cogntve confdence coeffcents, old (1) The PSO algorm descrbed above s known as tradtonal PSO and works well only n contnuous domans. Ths s a lmtaton of e classcal PSO because many applcatons are set n a space featurng dscrete varables. Naturally, Dscrete Partcle Swarm Optmzaton (DPSO) s developed to tackle ose dscrete optmzaton problems. It dffers from e tradtonal PSO n e sense at ts partcles do not represent ponts n e n - dmensonal Eucldean space [17]. In s paper, t represents, n a dscrete nature, a combnaton of selected poston assgnments for moble robots n a formaton. Equaton (1) s also used to update

Intalzaton of Robotc Formatons Usng Dscrete Optmzaton Partcle Swarm Optmzaton 357 partcle s velocty and poston n e DPSO. The partcle poston and velocty encodng vares from one specfc problem to anoer and wll be presented n Secton 3 for e case of poston assgnment. 3. ROBOTIC FORMATION INITIALIZATION APPROACH 3.1 Premses and Problem Statement The focus here s on e optmal poston assgnment and trajectory generaton for robots n a desred formaton, oer ssues such as pa plannng, choce of e leader robot n a formaton or moton control are beyond e scope of s paper. We assume at e planner knows e desred poston of a formaton and e envronment for ntalzaton s obstacle-free. Each robot s assumed to move at constant speeds and be able to swtch nstantaneously between a fxed speed and haltng, whch s typcal n multrobot moton planng [11]. Therefore, e shortest pa for a robot would be a straght lne connectng ts ntal poston and ts desred poston n e formaton. The formaton ntalzaton problem for N robots s now defned as follows: Gven e poston of e leader robot and a desred formaton confguraton, assgn e desred poston for each robot n e group and fnd a set of velocty profles for each robot R ( 1,..., N 1) to move from ts ntal poston to ts desred poston wout colldng w each oer, whle mnmzng e total tme for all robots to establsh e formaton. 3.2 Formaton Intalzaton Algorm We dvde e problem nto two sub problems. Frstly, DPSO s appled to fnd e optmal poston assgnment for each robot. Each robot s assgned a desred poston n a formaton so at total tme requred by all robots to form e formaton s mnmzed. Once each robot has known ts assgned poston, e next step s to perform a geometrc checkng to fnd a prorty scheme by whch a robot has to wat for oers n e case of a potental collson w anoer robot and e total watng tme for all robots wll be mnmzed. 3.2.1 Poston Assgnment Let us consder N robots ntally scatterng n e workplace. One robot has been chosen to be e reference robot and s called e leader robot whle e rest are called follower robots n s paper. The selecton of e leader robot may be based on ts geographcal advantage n e whole group or ts powerful sensng capacty. From e ntal poston of e selected leader robot togeer w e requred formaton confguraton, desred postons of all follower robots n e formaton are calculated. If ere are N robots, ncludng e leader, to enter a formaton n whch each desred poston n e formaton s ndexed as P ( x, y, ), 1.. N 1, or just P for convenence, and each robot excludng e leader has a unque dentfcaton (ID) as R 1,..., R N 1 or just 1,..., N 1 en each partcle poston and velocty are encoded as follows. - Partcle poston p x, x,..., x ), w ( 1 2 N 1 x j 1,.., N 1 means at robot R x s assgned to e desred poston P x n e formaton. Objectve functon As stated above, e crtera for e poston assgnment problem s to mnmze e total dstance traversed by all robots to form e formaton. To ease e computatonal burden of e planner as well as e mplementaton complexty n trajectory trackng for each robot, e trajectory of each robot when movng from an ntal poston to ts desred poston s separated nto ree phases. They are: () spnnng on ts wheels to algn w e straght lne connectng ts ntal poston to ts desred poston; () followng at lne, and () spnnng on ts wheels to algn w e formaton orentaton. Suppose e follower robot does not have to wat for oer robots n a potental collson or ere s no propensty of collson, en e tme needed to reach ts goal s 1 1 1,1,2 v T d, (2) where, 1,, 2 and d are respectvely angular and translatonal dsplacement correspondng to each phase. The objectve functon s chosen as:

358 N 1 mn ( F T ) v, (3) 1 s. t.safety The actual total tme executed by a follower robot to reach a desred poston may be dfferent from (2) as t may have to stop and wat for oer robots f nter-vehcle collson s to happen. As each robot s angular ) and translatonal d ) (, 1, 2 movement are assgned by e desred formaton shape, a soluton found w a mnmum cost defned by (3) wll guarantee at e poston assgnment s tme-optmal subject to e safety condton of nter-robot collson avodance, whch wll be descrbed n e next secton. 3.2.2 Moton Plannng As mentoned, e cost functon used n DPSO algorm gven n (3) should be subject to e collson-free condton. Ths constrant s resolved n s paper va a moton planner to construct a sutable trajectory for each robot where ts safety wll be preserved. For e safety purpose, e physcal robot s located as a crcle w radus rsafe r r m argn, where r s e dstance from e center of e robot to e ts furest pont and rm arg n s e margnal clearance around t [18]. W e segmentaton of a robot s trajectory correspondng to ree phases as stated above, e boundary of a robot pa wll be rectangular whch facltates e geometrcal collson checkng. To safely reach a target for a robot n e group, a quck geometrcal check s performed to dentfy robots whch potentally collde w each oer. Those robots whose pas do not geometrcally collde w oers wll be excluded from e search operaton and er velocty profles are constructed straghtforwardly. For ose robots whose geometrcal pas cross e oer pas may or may not collde w each, dependng on wheer or not ey reach e same pont at e same tme, er velocty profles are obtaned from e search by adoptng furer e followng ntervehcle collson avodance strategy: - For robot R, calculate e tme nstant and duraton t crosses e oer robots pas as ( 1 1 V. Ngo, T. Ngo, V. T, N. N. M. M. Kwok & Q. P. Ha 2 2 CT [( t0, t1, ndex1), ( t0, t1, ndex2 ),, ( t0, t1, ndex j )] whch means robot R crosses and occupes e j j pa of robot R durng t, t ). Once a robot ndex j ( 0 1 crosses anoer robot s pa, t s ncluded n e nter-vehcle collson avodance checkng even f t does not reach e crossng pont at e same tme w e oer robot. Thus, CT wll change over tme as explaned later. - The search result for robot R s of e form VP [ b1, b2,, b M ], where b {0, 1} and M s e number of tme steps n e search outcome for robot R. Ths wll depend on e leng of e robot pa, robot velocty, and e duraton of each tme step. Robot R wll follow ts planned pa w a constant velocty durng e tme slot f b 1 ; oerwse, t stops. When b 1, e velocty of robot R may be ( v 0, const) f t s spnnng on wheels, or ( v const, 0) f t s followng e straght lne to ts desred poston. - Collson between any two robots, f occurs, wll take place only one tme due to e feature of a robot pa. Hence, once e collson ssue has been resolved, ese two robots are collson-free w each oer so eer one wll be released from e safety check w respect to e oer. If a robot has no more potental collson w oers, t wll be released from e nter-robot collson checkng procedure and t wll safely follow ts pa. - Deadlock may occur f robot R crosses e target of robot R j [15]. In order to avod s deadlock, R j must wat untl R passes ts target. - If a robot reaches e crossng area before ts counter-part robot and no deadlock occurs when crossng e area frst, en t wll have a hgher prorty an ts counter-part. If deadlock occurs, t has to wat for ts counter-part to cross frst and erefore, actng n a lower prorty. - At each tme step, e current tme s compared w e tme n CT : 1. If R does not reach any crossng area, set b 1. j j

Intalzaton of Robotc Formatons Usng Dscrete Optmzaton Partcle Swarm Optmzaton 359 2. If R s at a crossng area: 2.1 If deadlock occurs when t moves, set b 0. 2.2 If no deadlock occurs and ts counter- part has not reached s crossng area, set 1 2.3 If no deadlock occurs and ts counter-part has reached s crossng area: If t has entered s area before ts counter-part en t wll contnue movng,.e., set, b 1. If a counter-part has entered s area before e consdered robot R, en check e collson potental between e two robots. If bo of em move to e next step, en set b accordngly,.e., b 1 f no collson exsts and b 0 oerwse. If R has passed e crossng area, en release R and ts counter-part from e checkng procedure n future steps. After all e search results have been found, desred velocty profles or safe trajectores for all robots n e formaton wll be constructed straghtforwardly. 4. SIMULATION RESULTS The proposed meod for robotc formaton ntalzaton usng DPSO and e behavoural collson avodance strategy s appled to smulatons for dfferent type of formaton confguratons w dfferent number of robots. Due to lmted space, e smulaton results for a case of 9 robots to form a lne formaton are presented. Fgure 1a rough 1d show e snapshots for robot locatons over tme. The robots from left to rght and top to bottom are e leader robot, e ntal locaton of follower robots R1, R4, R6, R5, R7, R8, and R 2, respectvely. The proposed algorm gves e best poston as P [1, 4, 3, 6, 5, 8,7, 2]. best b (a) (b) (c) (d) Fgure 1. Example of Nne Robots to Form a Lne Formaton. Snapshots of e Formaton Over Tme. 5. CONCLUSION A dscrete partcle swarm optmzaton (DPSO) algorm has been proposed for schedulng optmally e poston ass gnment to ntalze a formaton of vehcles, whch can be deployed for constructon automaton purposes. For e sake of smulaton, e ftness functon to mplement DPSO comprses e total tme requred for all vehcles to establsh e formaton, subject to e collson avodance condton. Smulaton results are ncluded to verfy e approach effectveness. Inpractce, furer crtera such as sensng capacty and geometrcal advantages can also be ncorporated n s mult-objectve optmzaton technque.

360 6. ACKNOWLEDGMENTS Ths work s supported by e Vetnam Mnstry of Educaton and Tranng and also by e ARC Centre of Excellence program, funded by e Australan Research Councl (ARC) and e New Sou Wales State Government. 7. REFERENCES [1] A. J. Healey, "Applcaton of formaton control for mult-vehcle robotc mnesweepng," n Proc 40 IEEE Conference on Decson and Control, Orlando, Florda, Dec. 2001, pp. 1497-1502. [2] B. Donald, L.Garepy, and D.Rus, "Dstrbuted manpulaton of multple objects usng ropes," n Proc IEEE Internatonal Conference on Robotcs and Automaton, San Francsco, CA, 2000, pp. 450-457. [3] D. Fox, W. Burgard, H. Kruppa, and S. Thrun, "A probablstc approach to collaboratve mult-robot localzaton," Autonomous Robots, vol. 8, pp. 325-344, 2000. [4] J. Feddema, C. Lews, and D. Schoenwald, "Decentralzed control of cooperatve robotc vehcles: Theory and applcaton," IEEE Transactons on Robotcs and Automaton, vol. 18, pp. 852-864, 2002. [5] M. Lemay, F. Mchaud, Letourneau.D, and J.-M. Valn, "Autonomous Intalzaton of robotc formatons," n Proc IEEE Int. Conf. on Robotcs and Automaton, New Orleans, LA, 2004, pp. 3018-3023. [6] J. P. Desa, J. P. Ostrowsk, and V. Kumar, "Control of changes n formaton for a team of moble robots," n Proc IEEE Int. Conference on Robotcs and Automaton, Detrot, MI, May 1999, pp. 1556-1561. [7] Y. Hao and S. K. Agrawal, "Plannng and control of UGV formatons n a dynamc envronment: A practcal framework w experments," n Proc IEEE Int. Conference on Robotcs and Automaton, Tape, Tawan, Sep. 2003, pp. 1209-1214. [8] H. Yamagucha and T. Ara, "Dstrbuted and autonomous control meod for generatng shape of multple moble robot group," n Proc IEEE Internatonal Conference on Robotcs and Automaton, San Dego, CA, 1994, pp. 800-807. V. Ngo, T. Ngo, V. T, N. N. M. M. Kwok & Q. P. Ha [9] B. Erkn, S. Onur, and S. Erol, "A revew: Pattern formaton and adaptaton n mult-robot systems," Robotcs Insttute - Carnege Mellon Unversty, Pttsburgh CMU-RI-TR-03-43 2003. [10] F. E. Schneder and D. Wldermu, "A potental feld based approach to mult robot formaton navgaton," n Proc IEEE Internatonal Conference on Robotc, Intellgent Systems and Sgnal Processng, Changsha, Chna, Oct.2003, pp. 680-685. [11] Y. Guo and L. E. Parker, "A dstrbuted and optmal moton plannng approach for multple moble robots," n Proc IEEE Internatonal Conference on Robotcs and Automaton, Washngton, DC, May, 2002, pp. 2612-2619. [12] J. Kennedy and R. Eberhart, "Partcle swarm optmzaton," n Proc IEEE Internatonal Conference on Neural Networks, Dec. 1995, pp. 1942-1948. [13] S. Doctor and D. K. Venayagamoory, "Unmanned vehcle navgaton usng swarm ntellgence," n Proc IEEE Internatonal Conference on Intellgent Sensng and Informaton Processng, 2004, pp. 249-253. [14] Y. Qn, D. Sun, N. L, and Y. Cen, "Pa plannng for moble robot usng e partcle swarm optmzaton w mutaton operator," n Proc 3rd Intl. Conf. on Machne Learnng & Cybernetcs, Shangha, Chna, Aug. 2004, pp. 2473-2478. [15] N. M. Kwok, Q. P. Ha, V. T. Ngo, and S.M. Hong, Partcle Swarm Optmzaton-based Coordnaton of a Group of Constructon Vehcles, n Proc. Int. Sym. Automaton and Robotcs n Constructon, Tokyo, Japan, Oct. 2006, pp. 840-845. [16] M. Clerc, "Bnary partcle swarm optmsers: toolbox, dervatons, and maematcal nsghts," n http://clerc.maurce.free.fr/pso/, accessed Feb 2005. [17] E. S. Correa, A. A. Fretas, and C. G. Johnson, "A new dscrete partcle swarm algorm appled to attrbute selecton n a bonformatcs data set," n Proc Genetc and Evolutonary Computaton Conference, 2006, pp. 35-42. [18] A. D. Nguyen, V. T. Ngo, N. M. Kwok, and Q. P. Ha, "Formaton Intalzaton w Vrtual-Head Robot Trackng and Three-Pont l-l Control," Internatonal Transactons on Systems Scence and Applcatons, vol. 2, no. 4, pp. 405-412, 2007.