Geometric Motion Planning and Formation Optimization for a Fleet of Nonholonomic Wheeled Mobile Robots

Transcription

1 Proceedgs of the 4 IEEE Iteratoal Coferece o Robotcs & Automato New Orleas, LA Arl 4 Geometrc oto Plag ad Formato Otmzato for a Fleet of Noholoomc Wheeled oble Robots Rajakumar Bhatt echacal & Aerosace Egeerg State Uversty of New York at Buffalo Buffalo NY, USA rmbhatt@eg.buffalo.edu Ch Pe ag echacal & Aerosace Egeerg State Uversty of New York at Buffalo Buffalo NY, USA chtag@eg.buffalo.edu Vekat Krov echacal & Aerosace Egeerg State Uversty of New York at Buffalo Buffalo NY, USA vkrov@eg.buffalo.edu Abstract I ths aer, we reset a geometrc method for moto lag for a fleet of dfferetally-drve wheeled moble robots movg formato, that exlctly takes to accout ther oholoomc costrats. he relatve osto wth the formato duces dfferet moto las each dvdual wheeled moble robot (WR). We ca quattatvely evaluate the erformace of such duced moto las usg sutable metrcs defed for the motos of each WR. hese erformace metrcs cumulatve form or dvdual form are used to otmze the overall formato (.e. ther relatve ostos) for erformg a gve task. he aroach s well suted for ole mlemetato ad s demostrated usg case studes. Keywords- eam-based mult-aget moto lag, oholoomy, Performace evaluato, Formato otmzato. I. INRODUCION I recet years, there has bee cosderable terest teamg ad cooerato of multle moble robotc agets, targetg alcatos survellace, exlorato ad cooeratve maulato. Iterest such cooeratg systems arses whe the tasks may be heretly too comlex for a sgle system to accomlsh; or whe buldg ad usg several smle systems ca be more flexble, fault-tolerat or cheaer tha usg a sgle large system. Several aroaches to coordato of such fleet of robotcagets are reorted the lterature see []-[3] for surveys. Behavor -based cotrol adherets address the comlexty of mult-aget coordato by decomosg the hgh-level goals to rmtve tasks ad mlemetg smle cotrollers for them, wth the oto of a emerget tellgece as the sets of teractg behavors grow. he beefts of such behavor based cotrol clude ease of decetralzato, lmted commucato eeds ad surrsgly good erformace usg relatvely smle behavors [4],[5]. However, a systematc method for sytheszg desred emerget behavor ad aalyzg ts erformace/robustess has rove dffcult. I arallel, the formato aradgm has also emerged as a coveet mechasm for abstracto ad coordato wth aroaches ragg from leader-followg [6],[7], vrtual structures [8],[9] ad vrtual leaders [],[]. he grou cotrol roblem ow reduces to a well-kow sgle-aget cotrol roblem from whch the other agets derve ther cotrol laws. However, ths aroach requres commucato of some coordato formato. he formato aradgm has evolved to allow rescrto of arameterzed formato maeuvers [],[] ad grou feedback []-[4]. From these seemgly dsarate aroaches, a dyamc system-theoretc ersectve has emerged for examg the decetralzed multaget behavoral cotrol of formatos []-[6] ad our work s set ths cotext. I ths aer, we develo the formato ad dvdual aget moto lag algorthms for a system of dfferetallydrve moble robots movg the lae. ay formato lag aroaches treat the moble agets smly as ot objects wth ucostraed motos. Noholoomc costrats, f cosdered, are tackled by defg a sutable ed-effector ot for the wheeled base ad alyg olear ut-outut feedback learzato techques. he lag roblem s coverted back to oe of lag the ucostraed Cartesa motos of ths ed-effector ot, wth guarateed-stable zero dyamcs [7]. oto las are frst created for the ed effector ostos of the multle robots wth the formato ad a secod stage traslated to moto las for each dvdual robots wth resect to ts ed effector. I cotrast, we focus o develog a kematc moto lag roblem for rgd formatos of dfferetally drve moble robots, whle retag ther full oholoomc form. Whle [8] resets moto lag aroach for formatos of moble robots wth oholoomc costrats, we also otmze the formatos for secfc moto aths. I artcular, we cosder the dfferetally drve moble robots to be attached to the vrtual structure at ther ceter of axle (a ot that tradtoally has bee avoded because of the sgularty duced the ut-outut feedback learzato). For ay gve (laar) ath, we frst determe the stataeous moto of the Serret-Freet frame ad the corresodg ole of the moto. Algg a team-fxed frame wth ths Serret- Freet frame ow duces a helcodal velocty vector feld for the vertces of the vrtual structure. Our lag algorthm for the dvdual moble robots the algs the forward drecto of travel wth ths helcodal velocty vector feld /4/$7. 4 IEEE 376

2 he relatve osto of each moble robot wth the vrtual structure duces dfferet moto las for the dvdual moble robots. By usg a sutable metrc, defed o the motos of each moble robot, we ca evaluate the erformace of such duced moto las. hese erformace metrcs cumulatve form or dvdual form are used to otmze the overall formato (.e. ther relatve ostos) for erformg a gve task. hs otmzato ca be carred out for a gve stat of tme or for the etre moto. Whle a global formato otmzato ca be carred out for a etre rescrbed ath, we also dscuss local otmzato (at a gve tme-stat/osto) that s well suted for ole mlemetato. he orgazato of aer s as follows: I secto II, we defe our otato ad dscuss some of the metrcs tradtoally used for erformace evaluato. I secto III, we dscuss the algorthm that s used to determe the moto las for dvdual oholoomc robots from moto of a teamfxed frame. I secto IV, we reset case studes of otmal formato lag for executg a screw moto ad a geeral moto ad dscuss these results. Secto V cocludes the aer. II. PROBLE FORULAION A. Backgroud he set of rgd body dslacemets the lae ca be cosdered as elemets of a Le grou, A ( R, ) SE( ) ad deoted by: R SE A A R RR I R,,,det, he Le algebra of SE ( ), deoted by se (), s gve by Le algebra se () ca be assocated to the taget vector A ( st ( )) alog the curve at s() t by left traslato as: ˆ R s() t As() t A s() t where ˆ RR. he choce of assocatg the elemet of the Le Algebra wth a elemet from the taget sace by left traslato results a body-fxed reresetato of twsts that s varat wth resect to locato of the ertal frame (ad hece the referred descrto for locomoto systems). If t ( s) v s the vector ar reresetato of the matrx twst s, ( ) the corresods to the agular velocty of the vrtual structure whle v s the lear velocty of the org of { }, both exressed the team frame { }. A stataeous screw axs ca be assocated wth each twst. For a geeral moto alog a arbrtrary ath, stataeous screw axs chages wth tme. However, for a certa class of motos, the twst ad thus corresodg screw axs remas fxed. Such motos are kow as screw motos kematcs. For a laar case, the screw axs reduces to ole ad such motos reduce to rotatos about a fxed ole wth a costat agular velocty. C. Vrtual Structure We cosder a team of N dfferetally drve oholoomc moble robots the lae. We affx a bodyfxed frame { } to each moble robot at the ceter of the axle wth the x axs alged the drecto of forward travel as show Fgure. ˆ v, ˆ, ˆ se ˆ, v where ˆ s the skew-symmetrc matrx form of the vector. See [9] for a detaled descrto. B. oto Parameterzato Gve a curve A():[, s s ] SE() ad assumg a tmearameterzato, s():[ t t, t ] [, s ], the cofgurato of the body ca be rereseted by: Rst (()) st (()) () SE A s t A curve o SE () hyscally reresets a moto of the rgd body. I the rest of ths aer, we assume a costat seed of travel alog the curve wth s s() t of the. A elemet Fgure Vrtual structure ad formato of moble robots he cofgurato of each moble robot s gve by F F g ( R, ) SE( ) wth resect to some ertal frame 377

3 { F }. We assume that the orgs of all these frames O form the vertces of a rgd vrtual structure. A team fxed coordate frame { } ca ow be affxed at some coveet locato o ths vrtual structure. he cofgurato of ths coordate frame { } s the gve by F F g ( R, ) SE( ) wth resect to { F }. he relatve oretato of each dvdual frame { } wth resect to { } ca be wrtte as F F R R ad ther relatve ostos by F F F R. he cofgurato of the overall formato ca thus be wrtte terms of the cofgurato of the team-frame (determed by 3 arameters) ad N arameters defg the relatve locatos of the orgs of the dvdual robots as: q ( g,,,..., ) Q SE()... Note that we do ot cosder the relatve oretatos of the { } frame as art of the formato cofgurato for reasos dscussed later. We ote that a umber of aroaches have bee roosed for selectg the team coordate frame { } wth resect to varous robot affxed frames { }. For examle, [] choose the org of { } to cocde wth the org of { } ad oret the X axs the drecto of a earest eghbor { }. Others such as [] assg { } at the ceter of mass of the system oreted alog the rcal ertal drectos. Deste the added tal comutato burde, such a selecto makes the ketc eergy metrc dagoal ad allows ease of subsequet reeated evaluatos of the system ketc eergy. D. Path Parameterzato Gve a arc-legth arameterzed laar ath X :[, s ] ; X ( s ) ( x( s), y( s)), we comute the Serret-Freet frame at each ot. he corresodg evoluto equatos may be wrtte as E. Remaa metrcs o SE 3 ( SE ) We adot the otato of Zefra (ad refer the reader to [] for a detaled dscusso). A er roduct o se (3) ca be exteded to a Remaa metrc o SE (3). Let the er roduct of two elemets, se( 3) be gve by, twt I where t ad t are 6 vectors of comoets of ad wth resect to some bass ad W s a ostve defte matrx reresetg the metrc. If V ad V are taget vectors at a arbtrary ot A SE(3), the left-varat er roduct VV, A the taget sace SE A (3) ca be defed by: VV, AV, AV A I he matrx form for a famly of left-varat metrcs o se(3), arameterzed by 3 scalars, ad, s exressed as: I W I I I Several left varat metrcs ca be foud lterature by usg dfferet values for, ad. For examle, = = s called Kllg form whch essetally s a measure of the agular veloctes ( ) wth the sace of twsts. = = results the Kle form ad serves as a measure of ( v ). (However, whe secalzed to SE (), ths measure s detcally zero ad we avod ts use). = yelds the decouled Park metrc whch serves as a weghted quadratc combato of the lear ad agular veloctes ( ( vv). Smlarly, the cetrod fxed frame of referece alged alog the rcal ertal drectos, the ketc eergy metrc ca be wrtte as: e ˆ ( ) ( ) ˆ t s s et( s) eˆ ( ) ( s) eˆ ( s) s where e ˆt s the ut taget vector ad e ˆ s s the ut ormal vector. We ow attach our team frame { } to ths Serret- Freet frame, thereby creatg a arc-legth-arameterzed moto of the vrtual structure. hese twsts at { }, together wth the rgd formato codto duce a helcodal velocty vector feld at the orgs O of each of the robot fxed frames { }. H W mi he metrcs show Equato 9 ca be secalzed for SE (), ad wrtte as: W I Ad smlarly the metrc show Equato may be secalzed as: 378

4 I zz W mi where m ad I zz are the mass ad laar erta of the rgd body. III. FORAION OION PLANNING Gve a desred ath determes both, the locato of ICR as well as the curvature. Let the taget ad ormal drectos of the curve at ay ot be e ˆt ad e ˆ whch uquely defes the Serret-Freet frame. Let s be the magtude of velocty at whch the frame travels alog the ath. he forward velocty of the org of the Serret-Freet frame may be wrtte as v es ˆ t ad the agular as ˆ ˆ e t eks. I ths aer we assume that we travel alog the ath wth costat s leavg the case of ath- ad tme-varyg s ( st, ) for future work. he moto of the team frame { } as t traces the desred ath duces a helcodal velocty feld at the vertces of the vrtual structure. he velocty of the th robot wth resect to frame { } s comuted as: ( s) cos v F () s s As the formato maeuvers, matag the rgdty of ths vrtual structure, our moto lag strategy algs the drecto of forward travel (the X axs of each robot) wth the duced helcodal velocty vector feld. locato of stat ceter of the osculatg crcle s gve by (, ( ( s))). Further, grahcally, we see that the ICR of each robot s costraed to le o the le assg through the axle of each robot (Fgure ). hus, whe multle robots form art of a vrtual structure movg wth ts helcodal feld, the ICR of each robot must ow corresod to the stat ceter of the vrtual structure (ad thus the motos alog the uderlyg ath). hs s used to uquely determe the oretato of each moble base. he corresodg twsts of the frames { }, as they move to alg themselves wth ths moto la have a smlfed reresetato robot fxed frame { } as: F t v where v v ad are the magtudes of the lear ad agular veloctes. However, for ay gve ath, the relatve cofguratos ( ) duce dfferet moto las (wth dfferet erformace characterstcs) for the dvdual moble robots as they move formato towards the fal goal. Hece, usg the left varat metrcs dscussed Secto II.E we ca evaluate the erformace at each of these dfferet cofguratos ad use ths to otmze the relatve locato as wll be dscussed the ext secto. We also ote that whle the dscusso above was wth resect to a rescrbed ath, a ole mlemetato s also ossble gve oly the stataeous moto reresetato (twst) of a sutable task frame. IV. FORAION OPIIZAION CASE SUDIES We exame the otmzato of the overall formato usg case studes of a screw moto ad a geeral moto. I artcular, for both cases, we show the results of the otmzato usg the Kllg form ad the decouled ketc eergy metrc. he Kllg form metrc evaluates ( ( s )) whle the ketc eergy metrc evaluates ( I mv ) for each moble robot ( ts ow frame { }). zz Fgure Vsualzato of stataeous ceter of rotato costrat hs lag strategy for develog moto las for each moble robot ca be vsualzed usg the oto of the Istataeous Ceter of Rotato (ICR). I frame { }, the Fgure 3 Formato movg wth a costat v ad 379

5 We ote that a aalytcal soluto for the ketc eergy ca be comuted terms of the arc-legth arameterzato ad the geometry of the roblem as show Fgure 3. Let the moble robot be located at ot O ad the team frame located at O as show Fgure 3. he, the ketc eergy ca be wrtte as: E v m cos By cosderg otmzato varables to be ad dvdually, the codtos for mmzg the ketc eergy * ca be derved aalytcally. For a costat, the otmal s: For a costat * cos( ) *, the otmal s: * I each of the subsequet fgures, the frst sublot dects the desred trajectory the XY lae, the secod sublot dects () s vs s, the thrd sublot dects * () s vs s ad the fourth sublot dects * () s vs s. costat, the erformace measure also remas costat regardless of the relatve cofgurato of the moble base from the team-frame. hus, ay feasble relatve cofgurato s otmal. he ath wll be a crcle as show frst sublot Fgure 4. For ths case-study v ad ad so. oreover, the values of * () s are costat for gve () s for otmal cofgurato see Equato 6. hs s show thrd sublot where dfferet costat les corresods to dfferet values of () s. Also value * () s s costat for ay () s. B. Geeral moto y (m) * (m) Desred rajectory x (m) - - Otmal Radus chagg (/m) * (degrees) Curvature Otmal A. Screw oto y (m).5.5 Desred rajectory x (m) Otmal Radus (/m).5.5 Curvature Otmal Fgure 5 Desred trajectory ad corresodg results for a o-screw (Susodal) moto Fgure 5 dects the results obtaed for the case where the Serret-Freet frame travels alog a susodal ath alog the X axs of the ertal frame wth a costat lear seed s. For geeral motos (ad therefore ) are ot costats but fuctos of arameter s. Whle aalytcal exressos for () s may be dffcult to obta, t s always ossble to umercally comute these values for ay gve ath. However, for the case of a susodal desred ath, a aalytcal equato for the curvature () s exsts ad ca be wrtte as: * (m) chagg * (degrees) Fgure 4 Desred trajectory ad corresodg results for a screw moto Fgure 4 shows the results for the screw moto usg the ketc eergy metrc. For a screw moto, for whch s s v s () s v cos s hs exresso s grahcally show the secod sublot of Fgure 5. Further, from Equato 6, we ote that as * () s whch s reflected the thrd sublot. However, lease ote that we caed the value of * () s to. he fourth sublot dects the fact that otmal values for * () s for a costat radus always remas uchaged at. 38

6 V. CONCLUSION I ths aer we reseted method of moto lag that takes to cosderato the oholoomc costrats the lag hase. We also secalzed metrcs defed o SE 3 for use wth SE. Usg these metrcs, we otmzed the erformace of the formato of robots ad derve aalytcal solutos for the otmal values of arameters. he aalytcal exressos develoed for the otmal cofgurato terms of () s make t very easy to comute otmal formatos ad are well suted for ole mlemetato. ACKNOWLEDGEN We gratefully ackowledge the suort from he Research Foudato of State Uversty of New York ad Natoal Scece Foudato CAREER Award (IIS ) for ths research effort. REFERENCES [] R. C. Ark ad Bekey, G. A. (Eds.), Robot Coloes, Secal Issue of Autoomous Robots, rerted by Kluwer Academc Publshers, Bosto. Vol. 4, o.5, 997. [] Y. Cao, A. S. Fukuaga, ad A. B. Kahg, Cooeratve moble robotcs: Atecedets ad drectos, Autoomous Robots, vol. 4, o.,. 7-7,997 [3] L. E. Parker, Curret state of the art dstrbuted autoomous moble robotcs, Dstrbuted Autoomous Robotc Systems 4, Eds. L. E. Parker, G. Bekey, ad J. Barhe, Srger-Verlag:okyo,. 3-,. [4]. Balch ad R. C. Ark, Behavor-based formato cotrol for multaget robot teams, IEEE rasactos o Robotcs ad Automato, vol. 4, o. 6, , 998. [5] L. E. Parker, Allace: A archtecture for fault tolerat multrobot cooerato," IEEE rasactos o Robotcs ad Automato, vol. 4, o.,. -4, 998. [6] J. P. Desa., J. P. Ostrowsk, ad V. Kumar, odelg ad cotrol of formatos of oholoomc moble robots," IEEE rasactos o Robotcs ad Automato, vol. 7, o. 6, ,. [7] P. K. C. Wag, Navgato strateges for multle autoomous moble robots movg formato, Joural of Robotc Systems, vol. 8, o., , 99. [8] R. W. Beard, J. Lawto, ad F. Y. Hadaegh, A feedback archtecture for formato cotrol," IEEE rasactos o Cotrol Systems echology, vol. 9, o. 6, ,. [9]. A. Lews, ad K-H. a, Hgh recso formato cotrol of moble robots usg vrtual structures, Autoomous Robots, vol. 4, , 997. [] N. E. Leoard ad E. Forell, Vrtual leaders, artfcal otetals ad coordated cotrol of grous," Proceedgs of the IEEE Coferece o Decso ad Cotrol, Orlado, Florda, ,. URL: htt:// (Avalable February 4) [] P. Ögre, E. Forell, ad N. E. Leoard, Formatos wth a msso: Stable coordato of vehcle grou maeuvers, Proc.5th Iteratoal Symosum o athematcal heory of Networks ad Systems, August,. URL:htt:// mts.html (Avalable February 4) [] B. Youg, R. W. Beard, ad J.. Kelsey, A cotrol scheme for mrovg mult-vehcle formato maeuvers," I Proceedgs of Amerca Cotrol Coferece, Arlgto, VA, ,. [3]. Egerstedt ad X. Hu, Formato costraed mult-aget cotrol, IEEE rasactos o Robotcs ad Automato, vol. 7, o. 6, ,. [4] P. Ögre,. Egerstedt, ad X. Hu, A cotrol lyauov fucto aroach to mult-aget coordato, IEEE rasactos o Robotcs ad Automato, vol. 8, o. 5, ,. [5] J. Lawto, B. Youg, ad R. Beard, "A decetralzed aroach to elemetary formato maeuvers," I Proceedgs of IEEE Iteratoal Coferece o Robotcs ad Automato, Sa Fracsco, CA,. URL:htt:// LawtoYougBeard.s. (Avalable February 4) [6] R. Olfat-Saber ad R.. urray, "Flockg wth obstacle avodace: Cooerato wth lmted commucato moble etworks," I Proceedgs of 4 d IEEE Coferece o Decso ad Cotrol, 3. [7] B. J. Youg, R. W. Beard, ad J.. Kelsey, "A Cotrol scheme for mrovg mult-vehcle formato maeuvers," Amerca Cotrol Coferece, , Arlgto, VA, Jue 5-7,. [8]. D. Barfoot ad C.. Clark, oto Plag for Formatos of oble Robots, Robotcs ad Autoomous Systems, vol. 46, , 4. [9] R.. urray, Z. L, ad S. S. Sastry, A athematcal Itroducto to Robotc aulato. CRC Press, 994 [] R. Olfat-Saber ad R.. urray, "Grah rgdty ad dstrbuted formato stablzato of mult-vehcle systems," I Proceedgs of the 4st IEEE Coferece o Decso ad Cotrol, Las Vegas, NV, Dec.. [] C. Belta ad V. Kumar, rajectory desg for formatos of robots by ketc eergy shag, Proc. IEEE It. Cof. Robot. Automat., , Washgto, DC, ay. []. Zefra, Cotuous methods for moto lag. PhD thess, Uversty of Pesylvaa, Phladela, PA,