Pat Plannng for Formaton Control of Autonomous Vecles 1 E.K. Xdas, 2 C. Palotta, 3 N.A. Aspragatos and 2 K.Y. Pettersen 1 Department of Product and Systems Desgn engneerng, Unversty of te Aegean, 84100 Ermoupols, Syros, Greece Emal: xdas@aegean.gr 2 Department of Engneerng Cybernetcs, Norwegan Unversty of Scence and Tecnology, NTNU, NO-7491 Trondem, Norway Emal: {claudo.palotta, krstn.y.pettersen}@tk.ntnu.no 3 Department of Mecancal Engneerng & Aeronautcs, Unversty of Patras, 82200 Patras Greece Emal: asprag@mec.upatras.gr Abstract. In ts paper a two-stage approac s ntroduced for optmum pat plannng of a team of autonomous vecles n an envronment cluttered wt obstacles. Te vecles are requested to move n formaton from an ntal pont to a fnal pont. Te Bump-Surface concept s used for te representaton of te envronment wle te formaton of te vecles s presented by a deformable Delaunay trangulaton. Te proposed approac s presented n detal and test cases wt multple vecles are smulated to demonstrate te effcency of te metod. Keywords. Pat Plannng, Formaton Control, Autonomous Vecles 1 Introducton Teams of autonomous vecles are wdely used n many applcatons, were te vecles are requested to meet formatons or oter constrants to accompls complex tasks, suc as transportaton of large obects [1], localzaton and mappng [2], searc and rescue mssons [3]. Ts nterest s motvated by te necessty of avng more vecles performng tasks wc are more dffcult to perform wt only one vecle, for nstance survellance mssons [4]. Furtermore, te moton n formaton s partcularly mportant wen spatally dstrbuted tasks ave to be accomplsed, lke for nstance, source seekng mssons [5]. In ts paper we present an approac for te pat plannng problem for a mult-vecle system. In partcular, we consder a mult-vecle system wc s consstng of autonomous vecles. Te obectve s to fnd an optmal pat for eac vecle wc
connects an ntal pont wt a fnal pont wle smultaneously te vecles sould be movng n a gven formaton. Te pat plannng problem for formatons control of a team of autonomous vecles as been nvestgated [6]-[8]. In [6], te autors presented a metod based on rapdly explorng random trees (RRT) for pat plannng of formatons wt under-actuated vecles. Ts metod randomly samples te envronment and cooses a free collson confguraton for eac vecle. Te autors revsed te classcal RRT to generate feasble pats for non-olonomc vecles. Furtermore, tey desgned a prorty strategy, wc makes te vecles to move n a gven formaton. Te work [7] descrbes a metod based on Vorono Fast Marcng (VFM) for formatons of fully actuated moble robots. Ts metod can be classfed as a potental feld metod but avods te drawbacks related to local mnma. In [8] an abstract manfold A was defned wc s te product of two manfolds G and S. Te manfold G s a Le Group, wc captures nformaton about te orentaton and poston of every vecle, wle S s a manfold, wc captures nformaton about te sape of te group of vecles. Te states n te two manfolds G and S are controlled ndependently. In ts paper we extend te metod proposed n [9] and [10] to mult-vecle systems consstng of autonomous vecles. Te vecles sould be movng n a gven formaton. By usng te proposed approac, t s possble to obtan smultaneously an optmum pat, for eac vecle. Eac pat s constructed consderng bot te envronment constrants and te formaton constrants. Te man contrbuton of ts paper s te ntroducton of a metod for te pat plannng of a flexble formaton of n autonomous vecles n an envronment cluttered wt statc obstacles. For te frst tme te formaton relatonsp s represented by a deformable Delaunay trangulaton, wc as te ablty to fnd a soluton even wen te vecles are requested to move troug narrow passages. Furtermore, te smootness of te pat s obtaned by controllng te angles between te controlpolygon segments, wc defne te system s pat. A multplcty of optmzaton crtera and constrants could be ncorporated easly to te formulated optmzaton problem accordng to te msson requrements of te team of vecles. 2 Basc assumptons and te two stage approac It s assumed tat a formaton of autonomous vecles sould move n a 2D envronment wc s cluttered wt known probted areas (obstacles-danger zones). A formal statement of te problem, te assumptons and te structure of te proposed metod are gven n ts secton. 2.1 Te pat plannng problem for a formaton of autonomous vecles Consder a team of 3 n ³ autonomous vecles wc sould move from an ntal to a goal locaton by keepng te desred formaton n a 2D envronment cluttered wt known statc obstacles. Te basc assumptons are:
Eac vecle s represented by a pont wc s movng only forward. Eac vecle s requested to move from an ntal pont S to a goal pont G nsde te desred formaton, wc sould not splt, wle te lengt of te pat sould be mnmum. Te pat of eac vecle sould be smoot. Te formaton s modeled as a deformable polygon. Te user defnes bot te mnmum and maxmum allowed formatons of te polygon. Te maxmum formaton s te desred one. 2.2 Te geometry and te representaton of te formaton We assume tat a team of autonomous vecles s enclosed n a deformable convex polygon, wle no splts are allowed. Te vertces of te convex polygon are te external vecles. Fg.1 sows a vsual representaton of a team of 15 pont-vecles. Te convex polygon s used n order to take te advantage of te fact tat te centrod R always les nsde te polygon. Furtermore, by usng convex polygon, we avod to ncrease te problem s complexty wc s appen wen we use non-convex polygons. Fg. 1. A team of 15 vecles (black crcles). Te correspondng: Delaunay trangles (das lnes) and te convex ull (black bold lne). Te Delaunay trangulaton [12] s used to facltate te geometrc relatons between te vecles wtn a geometry, were constrants could be defned easly. Generally, Delaunay trangulaton s caracterzed by ts smplcty and ts economy n data storage. Furtermore, te Delaunay trangulaton s ndependent of te order n wc te ponts are processed. For a team of n vecles we ave, 2n-2- m trangles and 3n-3-medges, were m s te number of vecles on te convex ull. Te lengt of mn max eac edge d, = 3,...,3n-3- m s assocated wt te constrant d d d, were mn max d s te mnmum safe dstance and d s te maxmum, wc s te desred dstance between a par of vecles.
3 Frst stage: te sub-optmal pat for te mnmum sze formaton In ts stage, te mnmum sze formaton s consdered as a fxed sape and te sortest pat s searced on te Bump-Surface. For te constructon of te Bump- Surface representng a gven 2D envronment, a normalzed workspace W s constructed by lnearly mappng te ntal envronment to [0,1]. Te constructon of te 2 correspondng Bump-Surface S s obtaned by a stragtforward extenson of te Z- value algortm [9]. It s assumed tat, te team as a reference pont wc les at te centrod of te polygon representng te mnmum sze formaton. Te reference pont traces a pat R() s n te normalzed W wc starts from te gven start pont and termnates at te desred goal pont. In order to defne R() s we use a B-Splne curve [13] to represent te pat of te fxed mnmum sze formaton: were, Q s te number of control ponts Q-1 2 () s = ån( s),0 s 1 = 0 R p (1) 2 p, N ( ) s are te B-Splne bass functons and 2 s te curve degree. Te goal of te proposed global pat plannng strategy s te determnaton of te poston of ( Q - 2) control ponts p wc defne te requested pat R () s. 3.1 Safe optmum moton of te mnmum formaton A safe pat R () s s one tat () does not collde wt te obstacles and () t s smoot. Followng te results from [10], te arc lengt of R () s approxmates te lengt L of ts mage SR ( ( s)) on te Bump-Surface S as long as R () s les onto te flat areas of S. Furtermore, n order to take nto account te sape of te formaton, a set of feature ponts A s selected on ts boundary accordng to ts sape and te requested accuracy [10]. Takng te above analyss nto consderaton, te pat plannng problem s formulated as an optmzaton problem wc s descrbed by, N ì m ï p ü a ï mnmze Ecomp = íl, åå H,1/ 1,...,1/ Q-2ý, subect to 180, = 1,..., Q -2 (2) ïî = 1 a= 0 ïþ were N denotes te number of ponts taken on R () s to dscretze t, s te -t p angle between te control-polygon segments and +1 R () s and H s te flatness
of ( s) A on S. Te R() s follows te sape of te defnng control polygon wc s derved by connectng te control ponts p [13]. Ten, a Genetc Algortm s adopted n order to searc for a soluton to te formulated optmzaton problem (Eq.(2)). A floatng pont representaton sceme s selected snce te coordnates of te control ponts and te angles of te control polygon are real numbers. A ftness assgnment strategy based on Pareto-optmal solutons called GPSIFF [14] s mplemented. Te followng tree genetc operators were selected. Reproducton: te proportonal selecton strategy s adopted, were cromosomes are selected to reproduce ter structures n te next generaton wt a rate proportonal to ter ftness. Crossover: te one-pont crossover was adopted. Mutaton: a boundary mutaton s used. 4 Stage 2: Determnng te smoot pat of eac vecle n te deformable formaton. Wt te reference pat R() s derved by te frst stage, te pat of every vecle s determned consderng te locaton of te vecles n te desred formaton. Snce te formaton as to pass troug areas, were te mnmum sze formaton s able to move wt safety, ten n te second pase a deformable formaton s consdered. A deformaton cost functon s formulated wt an optmum cost at te desred formaton. In ts stage eac vecle as ts own ndependent smoot pat but along ts pat t as to respect te desred formaton. In order to ensure tat te vecles n- m do not cross te border of te convex ull, te followng condton s taken nto account. At every pont R( s ), a= 1,..., N of te pat R() s te locaton of te vecle a p R, = 1,..., n-ms computed by te convex combnaton of te m vecles wc defne te convex ull. Terefore, were, te wegt factors m å R = w R, = 1,..., n-m (3) w satsfy, f f = 1 f f m wf ³ 0 and å wf = 1, = 1,..., n-m (4) Te goal of te proposed pat plannng strategy s te determnaton of te Q - 2 control ponts f = 1 q, wc defne te requested pat R () s for te -t vecle gven by te same equaton as Eq.(1). In order to take nto account tat te formaton of te vecles sould adapt to te geometrc caracterstcs of te envronment wle smultaneously tryng to keep te desred sape, te followng deformaton functon s proposed: max mn max ì k -, [, ] ï d d dî d d CD = e, = 3,...,3n -3- m, and k =í (5) ï îaverybg value (defned by te user), oterwse
Eq. (5) gves a penalzng functon, wc takes te optmum value wen d mn max and te worst wen d Ï [ d, d ]. Te mnmum sze polygonal sape of te formaton s obtaned wen d = d. mn Accordng to te above requrements te derved obectve functon s a vector wc s represented by, ì wf ³ 0 1 n ï mn E {,...,, 1,..., }, 3,...,3 3, m comp = L L CD CD = n- -m subecttoí ï å (6) wf = 1, = 1,..., n-m î f = 1 were L s te pat s lengt for te -t vecle wc s computed n a smlar way as n Stage 1. In te optmzaton problem defned by Eq. (6), te optmzaton varables are te control ponts wc defne te pat R () s of eac vecle and te wegt factors w. f A Mcro-GA s used to searc for a sub-optmum pat of eac vecle. Te man caracterstcs of te developed Mcro-GA are te followng: A floatng pont representaton sceme s selected for te cromosome syntax. Eac cromosome repre- sents a possble R () s as a sequence of te unknown control ponts q. A ftness assgnment strategy based on Pareto-optmal solutons s mplemented. It sould be notced tat te qualty of te ndvdual soluton generated n te ntal pase plays a crtcal role n determnng te qualty of te fnal optmal soluton. Tus, te soluton wc s derved from Stage 1 (te control ponts wc defne te pat R () s ), s used as an ntal soluton n te Mcro-GA (seedng) for eac vecle s pat R () s. Ts elps te Mcro-GA to converge n sort tme to te sub-optmal pat for eac vecle. Te same genetc operators as n Stage 1 are used except te mutaton operator wc s gnored. In most cases, a maxmum number of teratons (generatons) s defned n advance for te termnaton. However, t s dffcult to determne beforeand te number of generatons needed to fnd near-optmum solutons. Tus, an assessment of te qualty level of te Genetc Algortm s made on-lne. Te proposed algortm termnates eter wen te maxmum number of generatons s aceved or wen te same best cromosome appears for a maxmum number of generatons. = d max 5 Smulatons Te performance of te proposed metod s nvestgated troug a number of smulaton experments for a varety of formatons movng n 2D envronments. All smulatons are mplemented n Matlab. In all test cases, te grd sze s set to N g = 100. For te frst stage, te control parameters of te GA are te followng: populaton sze=250, maxmum number of generatons=500, crossover rate =0.75, boundary mutaton rate=0.004. For te Mcro-GA we set: populaton sze=50, maxmum number of generatons=30, crossover rate =0.75. It s wort notng tat te selecton of
te approprate control settngs s te result of extensve expermental efforts wt varous control scemes adopted followng te ndcatons of te lterature. Test Case: We assume te envronment of fg.2. Here, a team of 15 vecles s requested to move n formaton from te ntal ponts S to goal ponts G, = 1,...,15. Te convex ull s defned by sx vecles. A vsual representaton of bot te ntal and fnal formatons, te computed pat R() s and te correspondng Delaunay trangles are sown n Fg.2. Eac vecle s pat s defned by 8 control ponts. Te computed soluton takes about 10.24 mnutes. Furtermore, Fg. 2 sows te formaton of te convex ull wle te team of vecles s passng troug narrow passages, were te formaton s not ust srnked but t canged ts sape autonomously to adapt to te envronment. Fg. 2. Te resultng soluton pat R () s,te ntal and fnal convex ull wt te correspondng Delaunay trangles and te trace of te convex ull n two dfferent tme nstances. Despte te fact tat te problem under consderaton s off-lne, computatonal tme results wt respect to te number of te vecles s of mmense nterest. Te varaton of CPU s tme s ndcatve of te problem complexty. In tese experments, te envronment s te one sown n Fg.2 and te number of control ponts s constant, wle te number of vecles s canged from 3 to 15. Fg. 3 sows tat CPU tme ncreases almost lnearly wt te ncrease n te number of vecles. No of vecles Fg. 3. A CPU tme study
6 Concluson A new approac for te pat plannng of a deformable formaton of autonomous vecles s proposed. Te Delaunay trangulaton s proved to be very convenent for te modelng of a deformable formaton snce te extreme formatons (mnmum and maxmum allowed formatons) are defned by te user and tus we can easly determne te lmts of te dstances between te vecles. Furtermore, te smootness of te pat s obtaned by controllng te angles between te control-polygon segments, wc defne te system s pat. In future work te proposed approac sould be extended to sem-known 2D and 3D envronments, and n addton to te moton plannng te gudance and control of te vecles sould be consdered. References [1] Y. Wang and C. Slva, Sequental Q-learnng wt Kalman flterng for multrobot cooperatve transportaton, IEEE/ASME Trans. Mecatroncs, vol. 15, no. 2, pp. 261 268, Apr. 2010. [2] H. Cen, D. Sun, J. Yang, J. Cen, SLAM based global localzaton for mult-robot formatons n ndoor envronment, IEEE/ASME Trans. Mecatroncs, vol. 15, no. 4, pp. 561 574, Aug. 2010. [3] Z. Tang and U. Ozguner, Moton plannng for mult-target survellance wt moble sensor agents, IEEE Trans. Robot., vol. 21, no. 5, pp. 898 908, Oct. 2005. [4] M. Saska, J. Cudoba, L. Precl, J. Tomas, G. Loanno, A. Tresnak, V. Vonasek, and V. Kumar, Autonomous deployment of swarms of mcro-aeral vecles n cooperatve survellance, 2014 Int. Conf. Unmanned Arcr. Syst. ICUAS 2014 - Conf. Proc., pp. 584 595, 2014. [5] J. Han and Y. Cen, Multple UAV formatons for cooperatve source seekng and contour mappng of a radatve sgnal feld, J. Intell. Robot. Syst. Teory Appl., vol. 74, pp. 323 332, 2014. [6] S. Lu, D. Sun, and C. Zu, A dynamc prorty based pat plannng for cooperaton of multple moble robots n formaton formng, Robot. Comput. Integr. Manuf., vol. 30, no. 6, pp. 589 596, 2014. [7] S. Garrdo, L. Moreno, and P. U. Lma, Robot formaton moton plannng usng Fast Marcng, Rob. Auton. Syst., vol. 59, pp. 675 683, 2011. [8] C. Belta and V. Kumar, Moton generaton for formatons of robots: A geometrc approac, Proc. 2001 ICRA. IEEE Int. Conf. Robot. Autom. (Cat. No.01CH37164), vol. 2, 2001. [9] P.N. Azarads, N.A. Aspragatos, Obstacle representaton by Bump-surfaces for optmal motonplannng. Robotcs and Autonomous Systems 51 (2-3), pp. 129-150, 2005. [10] E.K. Xdas, P.N. Azarads, Msson desgn for a group of autonomous guded vecles. Robotcs and Autonomous Systems 59 (1), pp. 34-43, 2011. [11] M.S. LaValle, Plannng Algortms. Unversty of Illnos, 2004. [12] Preparata F. P., Samos M.I. Computatonal Geometry. Sprnger-Verlag, New York, 1985. [13] L. Pegl, W. Tller, Te NURBS Book. Sprnger-Verlag Berln Hedelberg, 1997. [14] J.H. Cen and S.Y. Ho, A novel approac to producton plannng of flexble manufacturng systems usng an effcent mult-obectve genetc algortm. Internatonal Journal of Macne Tools & Manufacture, no.45, pp.949-957, 2005.