Integration of Planning and Control in Robotic Formations

Transcription

1 Integraton of Plannng and Control n Robotc Formatons V.T. Ngo, A.D. Nguyen, and Q.P. Ha ARC Centre of Excellence for Autonomous Systems, Faculty of Engneerng, Unversty of Technology, Sydney, PO Box 13 Broadway NSW 7 AUSTRALIA Abstract Ths paper s devoted to plannng and control of a pattern formaton of moble robots when movng between goal ponts n a known and statc envronment. Path plannng s performed for a reference pont n the formaton usng the modfed A* search, coupled wth a proposed smoothng technque to generate a feasble trajectory wth nonholonomc constrants of moble robots taken nto account. Based on ths reference trajectory and the predefned formaton confguraton n curvlnear coordnates, each robot n the formaton computes ts trajectory. Formaton moton control s then ntegrated n the proposed framework to derve velocty profles for robots n the group, takng nto account dfferental geometry of the trajectores. Obstacle avodance s guaranteed by varyng the coordnates of those robots that are lkely n collson wth obstacles relatve to the reference one. Smulaton results are presented to llustrate the valdty of the proposed framework. Key words robotc formaton, modfed A* search, curvlnear coordnates, obstacle avodance. 1. Introducton The problem of control and coordnaton of mult-robot systems has receved a consderable nterest recently as varous applcatons can be performed faster and more effcently wth multple robots than wth a sngle robot. In many cases, mult-robot systems are much more robust and fault tolerant and can be easly expanded to a large scale. Some typcal applcatons are movng large objects [Donald et al., ], exploraton [Fox et al., ], survellance [Feddema and Schoenwald, 1], search and rescue [Jennngs et al., 1997]. In ths context, the control of robotc formatons s partcularly mportant n such applcatons as mne sweepng [Balch and Arkn, 1998; Healey, 1], mltary scout and agrcultural coverage tasks, where sensor assets are lmted as t allows each robot n the formaton to concentrate ts sensng capablty on a porton of the envronment, whle other robots n the formaton cover the rest. Research n robotc formatons has focused on ssues lke formaton generaton [Ara et al., 1989; Yamagucha and Ara, 1994], mantenance of a formaton shape [Balch and Arkn, 1998; Desa et al., 1], controllng and changng formatons [Das et al., ; Desa et al., 1999; Nguyen et al., 4]. Generally, there are three broad approaches to the robotc formaton problem avalable n the lterature. They nclude the combned reactve behavours [Balch and Arkn, 1998], leader follower strateges [Desa et al., 1999; Desa et al., 1], and vrtual structures [Jongusuk and Mta, 1; Lews and Tan, 1997]. A comprehensve revew of robotc formaton s gven n [Erkn et al., 3]. Our research objectve s to ntegrate the path plannng and control n movng towards a framework for the control and coordnaton of a group of moble robots. Toward ths goal ths paper proposes to combne path plannng and trajectory generaton for the control of multple robots n a gven pattern, stressng on the formaton dynamc behavour, n terms of velocty profles, partcularly when turnng. In ths work, nspred by [Barfoot and Clark, 4], we assume the avalablty of a grd cell map of an envronment and use a reference pont n the formaton as the startng pont to plan the path for the formaton. Ths reference may be the center of the formaton, one partcular robot n the group, or any other pont. Smoothng technques are then appled to acqure the shorter, less turns, and approprate turnng radus path. Based on ths path, the velocty profle s obtaned for the reference pont. The coordnaton between the robots n mantanng the formaton s guaranteed usng the moton plannng as proposed n [Barfoot and Clark, 4]. Changng the formaton shape to fulfl a specfc task or to deal wth obstacle collson s acqured by plannng the offsets from the reference trajectory n a curvlnear coordnates for each robot. The ratonale for the ntegraton of plannng and control n robotc formatons s stated n [Ngo et al., ], where a

2 generc archtecture s proposed for robotc formatons movng n a statc envronment. In ths work, the coordnaton of the moble robot group s mplemented n curvlnear coordnates, whch allows for mantanng formaton shape wth possbltes to adjust the formaton wdth or to change the formaton shape wth some concesson made when the formaton turns. The dea behnd the proposed mechansms for plannng and control of a robotc formaton can be llustrated n a flowchart shown n Fgure 1. After a reference pont has been chosen, the modfed A* search s performed to fnd a path for the whole formaton. If a path s found n ths step, t s optmal subject to the defned heurstcs and the path for the reference pont s always safe. The path found s smoothed out to reduce the number of turns and to satsfy the dynamc and knematc constrants of moble robots. A reference trajectory s then generated for the reference pont. Based on the formaton confguraton, and the reference trajectory, offsets for each robot n curvlnear coordnates are computed and the trajectores for all the robots n the formaton are then obtaned. Next, each robot performs ts moton accordng wth ts planned velocty profle untl the goal s reached. The paper s organsed as follows. In secton, path plannng and smoothng are presented. The trajectory generaton s dscussed n Secton 3 and the coordnaton strategy to avod obstacles and nter robot collson n Secton 4. Smulaton results are provded n secton. A concluson and future work are gven n Secton 6.. Path Planng and Smoothng.1 Path Plannng A grd map s assumed to be avalable at ths stage. Each cell s a node n the search process. The optmal path from the ntal pont to the goal pont can be found usng varous standard graph search methods. Amongst them, the A* method s frequently employed to search the free space for an optmal path. As the A* method may be computatonallyneffcent, the so-called modfed A* search, can be used to lessen the computatonal burden nvolved. Ths method results n a loose search, whch s llustrated n Fgure for k=4. Accordngly, when a node s expanded, ts chldren nodes are attached not adjacently but n k cells away, where k s plannng step. The vector approach s used to check the vsblty of from the expanded node to ts chldren nodes,.e. to check whether a straght lne from the expanded node to a chld node ntersects any obstacle. The vector approach s also used to check the vsblty between the goal and the nodes wth a mnmum cost beng opened. If the goal s vsble then the search process s termnated and the safe path s found. A drawback of ths method s that a safe path may not be found even there exsts one. An alternaton s to No Generate velocty profle for robot 1 VP 1 Robot 1 moton control Begn Compute path usng modfed A* search Path found? Yes Smooth path Generate velocty profle for reference pont VP r Compute offsets for all robot n the formaton Goal reached? Yes Qut Generate velocty profle for robot n VP n Robot n moton control Fgure 1. Flow chart of formaton plannng and control algorthm reduce the plannng step k f no path s found wth a specfc k. When k s reduced to 1, the method becomes the tradtonal A* search. In ths paper, the modfed A* search [Warren, 1993] s used to fnd the path for reference pont n the formaton. Assume that the center pont n the formaton s chosen as the reference pont, and then even f the plannng step k s chosen to be equal or greater than half of the formaton wdth, t does not guarantee that the formaton can traverse to the goal wthout colldng wth any obstacle due to the smoothng process, and the nature of generatng a chld node n the modfed A * search method. Here the cost functon used to determne the optmal path s defned as f = g + h, where g s the actual cost from the ntal poston to the current node, and h s the heurstc cost from the current node to the goal defned as: h = d( x,y) + c, (1) No

3 n whch d( x, y) s the dstance from the current node to the goal and c s the terran cost of the current node. To guarantee a suffcent dstance from the path to obstacles, those nodes wthn a set range of a known obstacle s assgned a hgher terran cost. Obvously, ths parameter decreases wth dstance. The vector approach s chosen here for checkng the vsblty between the node to be opened and the goal as the process nvolves a shorter searchng tme, can deal wth the problem of the goal node not lyng on loose grd [Warren, 1993], and helps elmnate the redundant way-ponts n the remander of the path. Ths also helps reduce the post processng tme to smooth the path, as wll be detaled n Secton.. As stated, ths paper s dealng wth only the case of known and statc envronments. However, t s recognsed that normally n order to deploy a robotc formaton, the envronment should not be too cluttered and that the robots can only reach the goal wth a predefned confguraton provded the envronment around the goal s obstacle free. These observatons together wth the advantage of a reduced tme for path searchng and post processng wth the modfed A* search enable the onlne replannng requrement for robotc formatons operatng n dynamc envronments... Path Smoothng The common knematc model for a non-holonomc threewheel robot s descrbed as: x& ( t) = vcosθ y& ( t) = vsnθ & θ ( t) = ω, where x (t) and y(t) denote the poston of the centre pont on the wheel axs, θ (t) represents the orentaton, and nputs v and ω = vk are the current translatonal and angular veloctes respectvely, K s the curvature of the trajectory the robot s followng. The moble robots have three degrees of freedom,.e. two postonal degrees of freedom ( x, y) and one orentatonalθ, whch are related to a non-holonomc constrant equaton mpled n the model: () x & sn θ + y& cosθ =. (3) To ease wth the navgaton and subject to the vehcles knematc and dynamc constrants, the paths resulted from the search process need to be smoothed out, usng the technque proposed as follows. As the A* algorthm searches the eght nodes around the current one, and then proceeds to the next node, t produces a zgzag path. Ths zgzag path s unacceptable for moble robots especally when movng n a formaton. An deal dfferentally drven robot can turn on the spot (spn on wheels), but ths does not Fgure. Modfed A* search n D wth plannng step k=4 mply that the formaton can turn on the spot whle mantanng the geometrc formaton. Here the lne of sght prncple s appled to smooth out the path obtaned from the A* search. Ths s summarsed n Algorthm 1 below. Algorthm 1 1. Choose the frst path node as the start node and ts successve node as the end node. Record the frst path node as the new way-pont.. Check the vsblty between the start node and the end node. 3. If they are not vsble to each other, record the mmedate precedent of the end node as a new way-pont, and choose ths node as the start node wth ts successve as the end node. Go to step. 4. If they are vsble to each other, then 4.1 If the end node s not the goal, choose the node successve to the end node as the new end node. Go to step. 4. If the end node s the goal, record t and ext. The resulted way-ponts of the path after applyng Algorthm 1 are shown n Fgure 3 n comparson wth the modfed A* search path. As noted n [Barfoot and Clark, 4], the square, rectangular, or even a wedge formaton can only be mantaned perfectly (ncludng the poston and orentaton of the robots) n a straght lne moton. When the formaton turns, a concesson must be made,.e., the formaton s mantaned n curvlnear coordnates rather than n the rectlnear coordnate system. Algorthm 1 results n the path wth smallest number of turns. As can be seen, the resulted path s smoother but stll contans sharp corners. Techncally, the robot can stop at those ponts and rotate on ts wheels untl t reaches the desred orentaton and then moves on but ths approach s tme-consumng and energyneffcent, especally for a formaton, and as noted above, ths turnng on the spot does not mantan the formaton perfectly. Consequently there s a need for further smoothng as suggested n the proposed second algorthm

4 where the turnng radus s taken nto consderaton. Ths s explaned n the followng. Gven a quadruple of the start pont, the robot orentaton at the start pont, the end pont and the requred orentaton at ths pont, there are possble shorted paths for the start pont to reach the end pont wth a fxed startng orentaton and arbtrary endng orentaton, and 4 possble shorted paths wth fxed startng and endng orentatons as llustrated n Fgure 4. In Case 1, the path between any two successve way-ponts conssts of an arc followed by a straght lne. The robot orentaton at the end pont wll be the orentaton for the next way-pont. In Case, the path between any two successve way-ponts conssts of an arc, followed by a straght lne, and then by an arc. If the translatonal velocty of the robots along the path s assumed to reman constant then the mnmum turnng radus s determned by the maxmum rotatonal velocty of the robot. Furthermore, f the orentaton of the formaton s requred to be fxed only at the startng and goal postons whle may be arbtrary at other way-ponts along the path, then Case can be appled only for fndng the path between th way-ponts ( n 1) and n th (the goal), where n s the way-pont number for the path resulted from Algorthm 1. Case 1 s appled for the rest pars of successve way-ponts. Ths s summarsed n Algorthm below. Algorthm 1. Record the frst way-pont (start poston), gven a predetermned turnng radus. For any par of successve way-ponts between the frst and th the ( n 1) way-ponts, perform the followng step 1.1 Calculate the two possble shorted paths usng Case If no possble paths exst or the exstng paths collde wth an obstacle, reduce the turnng radus. Go to Step If there exsts at least one possble path wthout obstacle collson, choose the shortest one (f there are two). Record the necessary data and go to step 1.1. th. For the path between the ( n 1) and n th wayponts, perform the same steps as 1.1 to 1.3, except that calculate the four possble shorted paths usng Case. Record the necessary data and ext. 3. Formaton Moton Control 3.1. Reference Trajectory Generaton After runnng the modfed A* search and Algorthm 1 and, the necessary data are avalable. For example, wth a path between two successve way-ponts consstng of an arc followed by a straght lne, the necessary data for trajectory generaton nclude the length of the arc, the centre of the correspondng crcle, the turnng radus of the arc, ts startng poston on the crcle, the length of the straght lne, ts startng poston, and the orentaton of the straght lne. Once these data are avalable, the robot poston and orentaton at a partcular tme,.e. the reference trajectory ( xr ( t), yr ( t), θ r ( t)) can be easly calculated. One alternaton s to obtan the reference trajectory n the form of velocty profle ( vr ( t), ω r ( t)) or ( vr ( t), Kr ( t)). As the smooth path obtaned from the proposed algorthm has been checked as collson-free, the resultng trajectory s safe for the reference pont of the formaton Way-ponts wth modfed A* search Straght lnes connectng smooth way ponts obtaned from Algorthm 1 Obstacle space Fgure 3. Smooth paths obtaned from proposed Algorthms (a) startng pont Fgure 4. Possble shortest paths consderng turnng radus (a). Case 1: Two optons wth fxed startng orentaton and arbtrary endng orentaton (b). Case : Four optons wth fxed startng and endng orentaton (b) goal pont

5 3.. Velocty profles Based on the reference pont chosen and the predefned geometrc formaton, each ndvdual robot n the group has predetermned offsets [ p, ] T q n the curvlnear coordnates relatve to the reference pont C as shown n Fgure. where S = sgn(1 q K ( s )) r dq = + ( 1 ( )) Q qkr s ds dq dq ( 1 qk r( s) ) + K ( ) r s S ds ds K = Kr( s), Q + Q (7) (a) (b) Fgure. Square formaton n a straght lne moton (a) and whle movng (b). Once the [ p ] T q coordnates of robot have been determned, then ts translatonal velocty v and the curvature K are obtaned as proposed by [Barfoot and Clark, 4] as follow. For convenence, the velocty profle of the reference pont C as a functon of tme, t, can be rewrtten as a functon of dstance, d r : vr( dr), Kr( d r), t where dr() t = v ( ) r τ dτ. (4) The dstance travelled by robot along the reference trajectory s s() t = dr() t + p() t, () where t s noted that [ p ] T q s functon of tme. The trajectory of robot s computed as the followng v( s) = SQvr( s) (6) ω ( s ) = v ( s ) K. n whch dq dq ds and ds are the frst and second dervatve of q wth respect to s, respectvely. Wth ths method, a square formaton would look lke Fgure b whle turnng. When the [ p ] T q coordnates are constant, equaton (6) s smply as ( ) v( s) = vr( s) 1 qkr( s) Kr( s) ω ( s ) = v ( s ). ( 1 qk ( s) ) r 4. Obstacle Avodance As noted prevously n Secton, the collson-free trajectory for the reference pont does not always guarantee the safety for the whole formaton. Our strategy s to change the trajectory of those robots whch are lkely to collde wth obstacles. However, n some worse cases, e.g. the wdth of the path s too narrow to allow for more than one robot, the formaton must be changed to a column. Whle avodng the statc obstacles, the robots also need to avod collson wth other moble objects, e.g. other robots of the formaton. When specfc robots need to change ther trajectores, the one wth the hghest prorty s planned frst. The trajectores for robots of lower prorty are planned n accordance wth of those of a hgher prorty. Wth the formaton plannng method presented n secton 3, f the formaton s statc,.e., [ p ] T q coordnates of each robot are constant, the ndvdual robot trajectores wll not collde provded that the formaton does not turn sharper than a threshold curvature. When the formaton needs to be changed from one shape to another shape or narrow ts wdth to accommodate the task, the [ p ] T q coordnates of each robot wll be planned as functons of tme or dstance. It s noted from formulae (6) and (7) that, the shape or wdth of the formaton can only be changed f the second dervatve dq ds (8) exsts,.e., offset q must be adequately smooth wth respect to the correspondng trajectory durng the transent from one confguraton to another. In ths

6 paper, when the formaton wdth needs to be narrower or larger by an amount q = q, f q, over an ncremental dstance s = s, f s,, q s chosen to take the form: q, s s, s s, s s, q( s) = q, + q 3 s, < s s, f. s s q, f s> s, f Wth ths trajectory of q, the wdth of the formaton wll be narrower f q, > q, f, larger f q, < q, f, and become a column f q, f =. It s also noted that, when the formaton changes to a column, two ponts havng a same coordnate wth respect to the reference pont wll apparently become the same pont whose dstance s p from the reference pont, whch may be problematc. To overcome ths undesred case for the moton planng method used, the p coordnates of those robots need to be adjusted so that those robots wll not collde wth each other and the column shape can be formed. Ths s accomplshed by decreasng or ncreasng the velocty of each robot n appropraton whle they stll follow the same trajectory.. Smulaton results In ths secton, the proposed framework s llustrated n two examples where the formaton of a wedge type needs to change ts confguraton to avod obstacles. The reference pont s chosen to concde wth robot R 1 and ths robot s desgnated as the leader as the motons of the other two robots ( R and R 3 ) are based on the trajectory of ths leader wth R has hgher prorty than R 3. p Typcal snapshots of the formaton are recorded over tme as shown n Fgure 9. The results demonstrate the capablty of the three robots n movng from an ntal poston to reach the goal whle mantanng the formaton shape (a wedge) and changng t accordngly to an envronment usng the proposed archtecture. In the second scenaro, as can be seen n Fgure 1, the formaton just needs to narrow ts wdth to go through a larger corrdor. Agan, Fgure 11 and Fgure 1 show respectvely the tme trajectores x (t) and y (t) for the three robots, whle the formaton snapshots over tme are presented n Fgure 13. The results ndcate the proposed framework can handle well stuatons when the formaton shape does not need to be changed, but only needs to adjust the dstance between the robots to meet the requrement. 'x' poston [m] R Tme [s] Fgure 7. Tme trajectores x(t) for three robots n the frst example. Fgure 6 shows that three robots traversng n a desred wedge have to form nto a column when movng through a narrow corrdor. The x (t) and y (t) trajectores of the three robots are shown respectvely n Fgure7 and Fgure R 'y' poston [m] 1 R 'y' poston [m] 'x' poston [m] Tme [s] Fgure 6. Paths for three robots movng n a wedge, then a column and then back to a wedge. Fgure 8. Tme trajectores y(t) for three robots n the frst example.

7 14 'y' poston [m] 'x' poston [m] R 'y' poston [m] R Fgure 9. Snapshots over tme for the formaton n the frst example Tme [s] 'y' poston [m] 1 R Fgure 1. Tme responses y (t) for three robots wth only F changng ts trajectory 'x' poston [m] Fgure 1. Paths for three robots n a wedge when passng a corrdor wth only F adjustng ts trajectory. 'y' poston [m] 1 R R 'x' poston [m] Fgure 13. Formaton snapshots over tme n the second example 'x' poston [m] Tme [s] Fgure 11. Tme responses x (t) for three robots wth only F changng ts trajectory path fndng, whch s acheved by usng the modfed A* search and the vector approach coupled wth the two proposed smoothng algorthms, takng nto account the knematc and dynamc constrants of moble robots, and () the mantenance and changng of formatons, whch s done n curvlnear coordnates to accomplsh the requred tasks whle formaton safety s concerned. Illustratve smulaton for a three-robot wedge was performed for two scenaros. The results obtaned together wth the advantage n fast path searchng and post processng suggest a possblty of extendng the work toward a generc archtecture for robotc formaton control n dynamc envronments. Ths wll be the topc of our future work. 6. Concluson and future work We have presented an effcent framework for plannng and control a robotc formaton movng n a statc envronment. The contrbuton of ths paper ncludes () fast and feasble Acknowledgement Ths work s supported by the Vetnam Mnstry of Educaton and Tranng and also by the ARC Centre of Excellence programme, funded by the Australan Research Councl (ARC) and the New South Wales State Government.

8 References [Ara et al., 1989] T. Ara, H. Ogata, and T. Suzuk, "Collson avodance among multple robots usng vrtual mpedance," Proceedngs of the IEEE/RSJ Internatonal Conference on Intellgent Robots and Systems, Tsukuba, Japan, pp , [Balch and Arkn, 1998] T. Balch and R. Arkn, "Behavor- based formaton control for mult-robotc teams," IEEE Transacton on Robotcs and Automaton, vol. 4, pp , [Barfoot and Clark, 4] T. D. Barfoot and C. M. Clark, "Moton plannng for formatons of moble robots," Journal of Robotcs and Autonomous Systems, vol. 46, pp. 6-78, 4. [Das et al., ] A. K. Das, R. Ferro, V. Kumar, J. P. Ostrowsk, J. Spletzer, and C. J. Taylor, "A vsonbased formaton control framework," IEEE Transacton on Robotcs and Automaton, vol. 18,. [Desa et al., 1999] J. P. Desa, J. P. Ostrawsk, and V. Kumar, "Control of changes n formaton for a team of moble robots," Proceedngs of the IEEE Internatonal Conference on Robotcs and Automaton, Detrot, MI, pp. 6-61, [Desa et al., 1] J. P. Desa, J. P. Ostrawsk, and V. Kumar, "Modellng and control of formaton of nonholonomc moble robots," IEEE Transacton on Robotcs and Automaton, vol. 17, pp. 9-98, 1. [Donald et al., ] B. Donald, L. Garepy, and D. Rus, "Dstrbuted manpulaton of multple objects usng ropes," Proceedngs of the IEEE Internatonal Conference on Robotcs and Automaton, San Francsco, CA, pp. 4-47,. [Erkn et al., 3.] B. Erkn, S. Onur, and S. Erol, "A revew: Pattern formaton and adaptaton n multrobot systems," Robotcs Insttute - Carnege Mellon Unversty, Pttsburgh CMU-RI-TR-3-43, 3. [Feddema and Schoenwald, 1] J. Feddema and D. Schoenwald, "Decentralzed control of cooperatve robotc vehcles," Proceedngs of the SPIE, Orlando, Florda, pp. 1-11, 1. [Fox et al., ] D. Fox, W. Burgard, H. Kruppa, and S. Thrun, "A probablstc approach to collaboratve mult-robot localzaton," Autonomous Robots, vol. 8, pp ,. [Healey, 1] A. J. Healey, "Applcaton of formaton control for mult-vehcle robotc mnesweepng," Proceedngs of the The 4th IEEE Conference on Decson and Control, Orlando, Florda, USA, pp , 1. [Jennngs et al., 1997] J. S. Jennngs, G. Whelan, and W. F. Evans, "Cooperatve search and rescue wth a team of moble robots," Proceedngs of the IEEE Internatonal Conference of Advanced Robotcs, Monterey, CA, pp. 19-, [Jongusuk and Mta, 1] J. Jongusuk and T. Mta, "Trackng control of multple moble robots: A case study of nter - robot collson free problem," Proceedngs of the IEEE Internatonal Conference on Robotcs and Automaton, Seoul, Korea, pp , 1. [Lews and Tan, 1997] M. A. Lews and K. H. Tan, "Hgh precson formaton control of moble robot usng vrtual structures," Autonomous Robots, vol. 4, pp , [Ngo et al., ] V. T. Ngo, A. D. Nguyen, and Q. P. Ha, "Toward a generc archtecture for robotc formatons: Plannng and control," Proceedngs of the Sxth Internatonal Conference on Intellgent Technologes, Phuket, Thaland,. [Nguyen et al., 4] A. D. Nguyen, Q. P. Ha, S. Huang, and H. Trnh, "Observer-based decentralsed approach to robotc formaton control," Proceedngs of the 4 Australan Conference on Robotcs and Automaton, Canberra, Australa, pp. 1-8, 4. [Warren, 1993] C. W. Warren, "Fast path plannng usng modfed a* method," Proc. IEEE Internatonal Conference on Robotcs and Automaton, Atlanta, Georga, USA, pp , [Yamagucha and Ara, 1994] H. Yamagucha and T. Ara, "Dstrbuted and autonomous control method for generatng shape of multple moble robot group," Proc. IEEE Int. Conf. on Robotcs and Automaton, San Dego, CA, USA, pp. 8-87, 1994.