A Novel Method for Trajectory Planning of Cooperative Mobile Manipulators

Transcription

1 Originl Aricle A Novel Mehod for recory Plnning of Cooerive Moile Mniulors Hossein Bolndi, Amir Frhd Ehyei College of Elecricl Engineering, Irn Universiy of Sciences nd echnology, Nmk, ehrn, Irn ABSRAC We hve designed wo-sge scheme o consider he recory lnning rolem of wo moile mniulors for cooerive rnsorion of rigid ody in he resence of sic oscles. In he firs sge, wih regrd o he sic oscles, we develo mehod h serches he worksce for he shores ossile h eween he sr nd gol configurions, y consrucing grh on orion of he configurion sce h sisfies he collision nd closure consrins. he finl sge is o clcule sequence of ime-oiml recories o go eween he consecuive oins of he h, wih regrd o he nonholonomic consrins nd he mximum llowed oin ccelerions. his roch llows geomeric consrins such s oin limis nd closed-chin consrins, long wih differenil consrins such s nonholonomic velociy consrins nd ccelerion limis, o e incorored ino he lnning scheme. he simulion resuls illusre he effeciveness of he roosed mehod. Key wor: Closed kinemic chins, collision voidnce, cooerive mniulion, high-dimensionl sysems, moile mniulors, nonholonomic moion lnning, roilisic comlee lnner INRODUCION Cooerive moile mniulors hve received exensive enion in recen yers due o heir wide rnge of licions including rnsoring long nd hevy merils. [-6] While cooerive moile mniulors cn offer dvnges over single moile mniulor in erms of heir ciliy o crry ou difficul sks, moion lnning of hese sysems is comliced y he need o minin closed-chin srucure, he so-clled closure consrin. A cooerive moile mniulor sysem, in ddiion o closed-chin consrins, is suec o kinodynmic nd nonholonomic consrins in he form of differenil equions, [7] long wih collision consrins, which mke moion lnning even more difficul. However, hese consrins re necessry o find relile nd efficien soluion, ofen clled recory. Mny revious sudies hve invesiged he recory generion for cooerive moile mniulors, under vrious condiions. [-6] wo roches for soluion o his rolem cn e disinguished, [8,9] nmely: he decouled nd direc roches. he decouled roch, involves firs serching for h in he configurion sce nd hen finding ime-oiml ime scling for he h, suec o he cuor limis. [8] his hs he desirle enefi of decomosing he comlexiy of moion lnning rolems in wo ses, s menioned erlier. However, he h from he firs sge migh no e rnsformle ino n execule recory nd he cos ssocied wih he finl recory could e exensive. he min reson for hese rolems is h he roch ignores he differenil consrins of he cion model in he firs sge. [9] o overcome hese deficiencies, direc roch hs een develoed, in which, differenil consrins re considered in he lnning rocess. Mos of he lierures ville on he direc roch, comue exc recories using he oiml conrol heory nd nonliner oimizion meho for some secific low-dimensionl rolems. [-] he min drwck of hese meho is h he numer of vriles nd he comlexiy of he formulion ridly increse for rolems wih high degrees of freedom. o his end, in recen yers, roilisic comlee meho hve een exensively sudied wihin he direc frmework. [3-6] In conrs o oher ville meho h consruc glol reresenion of he configurion sce, hese meho discreize heir reresenion of free configurion sce o hndle high-dimensionl configurion sces. Due o he effeciveness of hese meho in cooerive mniulion sysems, [,,] some recen effors hve een direced o generlize hem for cooerive moile mniulors. [3-6] For insnce, in [3] single query mehod sed on he Ridly-exloring Rndom ree lgorihm (RR hs een resened. he mehod consrucs rndomized ree eween he sr nd gol configurions nd serches Address for corresondence: Hossein Bolndi, College of Elecricl Engineering, Irn Universiy of Science nd echnology, Nrmk, P.O.Box 68, ehrn, Irn E-mil: h_olnd@ius.c.ir Vol Issue Jn-Ar

2 Bolndi nd Ehyei: A novel mehod for recory lnning of cooerive moile mniulors for n oiml h during he consrucion hse. A disdvnge of his mehod is h he genered ree is only vlid for cerin sr nd gol configurions; hence, for new query, noher ree will hve o e consruced, which resrics he mehod o limied rnge of licions. [] herefore, [] rooses mulile query wo-sge mehod, in which firs grh, nmely, he Rodm is consruced, sed on he Proilisic Rodm Mehod (PRM y neglecing he resence of oscles nd ssuming fixed locion for he ses of moile mniulors. hen, i oules he environmen wih coies of he kinemic rodm in rndom locions nd connecs he collisionfree configurions of he sme closure ye o uild he finl rodm. he mehod, ecuse of is higher seed in he query hse nd iliy o chnge is gol configurion in n online mnner, is more execule in rel-ime rcicl siuions. However, in his mehod, he roiliy of sisfying loo closure equions in rndomly smled configurion is ner zero nd his fc lowers he erformnce of he lgorihm. o solve his rolem in [5,6] simle nd generl geomeric-guided smling lgorihm clled Rndom Loo Generor (RLG hs een roosed, which noly increses he roiliy of oining rel soluions when solving he loo closure equions. he min deficiency of he mehod resened for rndomized moion lnning of cooerive moile mniulors is h hey ignore differenil consrins in he moion lnning rocess. [3] However, hese consrins will e considered o clcule relile nd efficien soluion. herefore, in his sudy we roose novel wo sge scheme h considers he recory lnning rolem of wo moile mniulors for cooerive rnsorion of rigid ody. he environmen is suosed o e 3-D sce h includes sic oscles. he mehod fin n oiml recory in which such consrins s nonholonomic nd closed-chin consrins, long wih he oin nd ccelerion limis cn e esily e del wih in he resence of sic oscles. he mehod uilizes he dvnges of direc nd decouled roches long wih he ower of roilisic meho in hndling high dimensionl configurion sces o hve relile nd fs recory lnning lgorihm. he ricle is orgnized s follows: Secion inroduces he model of he cooerive sysem. In secion 3, we design he deils of our new mehod for recory lnning of he cooerive moile mniulors under differenil consrins in he resence of sic oscles. Finlly, we resen simulion resuls in secion, o show he effeciveness of he lgorihm, nd some concluding remrks in secion 5. MODEL OF COOPERAIVE SYSEM Figure shows our seleced model, including cooerive sysem of wo-wheeled moile mniulors Figure : Cooerive rnsorion y dul moile mniulor sysem rnsoring common ylod. Ech moile mniulor module consiss of wheeled moile roo wih 5-DOF mouned revolue-oin mniulor. wo roionl oins hve een considered he ooms nd he is of oh he mniulors, wih he im of mking he sysem licle o more rcicl siuions. In he sequel, i is ssumed h he moion of vehicles is resriced o he horizonl lne nd oh he end effecors cch he oec ighly. he rolem cn now e sed s follows: Given grou of wo nonholonomic moile mniulors grsing rigid ody, we should find recory o seer he sysem in cooerive mnner eween wo configurions, in n environmen wih sic oscles such h he ccelerion of ech vrile in he configurion sce remins wihin cerin oun. I should e noed h he cominion of differen yes of consrins (including holonomic, nonholonomic, nd dynmic consrins in such sysem mkes he moion lnning rolem comliced nd i requires creful evluion o relize he ylod mniulion sk efficienly. herefore, in he remining secion we use he ove-menioned model o genere he consrin equions of he sysem. Closed-chin Consrins When collecion of links is rrnged so h i forms loo, he configurion sce ecomes much more comliced ecuse he oin ngles mus e chosen in wy h he loos remin closed. his le o consrins in which some links mus minin secified osiions relive o ech oher. o derive hese consrins, we consider he cooerive sysem in more deil, s shown in Figure. Nomenclures in his figure re defined s follows: li ( i =,, 6 : Lengh of i h link in he closed kinemic chin θ i ( i =,, 6 : he i h oin ngle roing in he vericl lne in he closed kinemic chin θ i ( i =,, : he i h oin ngle roing in he horizonl lne in he closed kinemic chin l o : Lengh of oec eween wo end effecors Vol Issue Jn-Ar 5

3 Bolndi nd Ehyei: A novel mehod for recory lnning of cooerive moile mniulors ( l, l i i =,, : Lengh of links ched o he i h oin i roing in he horizonl lne From his figure nd he ssumions menioned efore, one cn conclude h he moile mniulor sysem is suec o holonomic consrins, exressed s: f ( θ, θ, θ3, θ, θ5, θ6, θ, θ, θ, θ, θ, θ, d, α = ( 3 where θ nd θ reresen he orienion of he se srucures (deiled derivion of his equion cn e found in Aendix A. However, in order o lower he comuionl cos of our mehod, we hve designed he desired recory in mnner h oh he moile ses hve fixed osiion nd orienion relive o ech oher. o his end, we define he following condiions: d = g α = ( θ = θ π = θ where g is consn osiive vlue. We lso mke he mouned mniulors o cooere in single lne. By using (, his le o he following desired recories: θ = θ = θ = θ = (3 3 herefore, y susiuing ( nd (3 in ( nd fer furher mniulion one ges: θ+ θ + θ3 = θ + θ5 + θ6 l s + l s + l s l c = l s + l s + l s ( 3 3 o l c + l c + l c + l s = l c + l c + l c + g 3 3 o in which, we define he following rmeers: ( = ( + = ( ( θ θ θ = ( θ + θ = ( θ c = cos θ + θ + θ, c cos θ θ, c cos θ ik i k i i i i s = sin + +, s sin, s sin ik i k i i i i l = l + l, l = l + l ( Differenil Consrins Differenil consrins exis in model of every nonholonomic moion lnning rolem nd resric dmissile velociies nd ccelerions. Here, we consider n uer limi on he ccelerion of ll oin sce vriles. In ddiion, he moile lform used in his ricle is kind of cr-le moile roo s shown in Figure 3; herefore, we suose h he wheels re rolling wihou skidding nd sliing. 6 Figure : Cooerive sysem wih is ched coordine sysem Figure 3: Bse srucure susysem of he cooerive sysem o derive he equions due o hese consrins, we need o comue he velociy of ech wheel in is ched coordine sysem [Figure 3]. In his regrd, we cn se h: [7,] w w v E v = x + y + θ i x y z l (6 i i i ( where x i nd y i denoe he osiion nd θ i reresens he orienion of he i h se srucure (i =, nd l is he osiion vecor from he origin of { v i } o he conc oin of he wheel wih he ground [Figure ]. Hence, he velociy of ech wheel cn e wrien s: x cos y l i ( θ + θ i + sin i ( θ + θ i θ i ( y cosθ v = y cos θ θ sin θ θ θ sinθ i ( + i x i + ( i + l i ( y (7 nd θ is he seering ngle of he h wheel ( =,, nd lso: y l ( = l (8 y he non-skidding condiion imlies h he second erm of he velociy vecor in (7 vnishes. herefore, for ech moile mniulor module hese consrins cn e ken Vol Issue Jn-Ar

4 Bolndi nd Ehyei: A novel mehod for recory lnning of cooerive moile mniulors ino ccoun s: ( + + ( + + x sin θ θ y ( l i i cos θ θ i i θ i y sinθ = (9 Consequenly, he seering ngle of he wheels is uniquely deermined hrough he following equions: y cosθ x sinθ x cosθ + y sinθ θ ly n( θ = y cosθ x sinθ + g θ x cosθ + y sinθ + θl y nd? θ ecomes: θ + n ( θ = + n ( θ d d =, = 3, y cosθ x sinθ x cosθ + y sinθ θ l y y y cosθ x sinθ + gθ x cosθ y sinθ θ l + + = 3, ( =, ( Furhermore, y defining r s he rdius of he h wheel, he non-sliing consrins cn e wrien s: [7,] i i v = r z ( Now, we cn oin he ngulr velociies corresonding o he driving orques s follows: x θ θ ( + y θ θ + + cos sin ( r l θ y cosθ = ψ, (3 = x cos( θ + θ + y ( + sin θ θ + r θ gsinθ + l y θ cos θ = 3, DESIGN OF HE RAJECORY UNDER CLOSED-CHAIN AND DIFFERENIAL CONSRAINS he generl mehodology of he PRMs is o consruc grh (rodm during rerocessing sge h reresens he conneciviy of he roo s free configurion sce nd hen query he rodm using n oiml grh-serch lgorihm (e.g., A*, in order o find he shores ossile h eween sr nd gol configurions. However, mos of he moion lnning echniques for closed-chin mechnisms could no direcly ccoun for he differenil consrins, which could render he lnned recory infesile. Here, we hve o find recory eween inermedie oins, genered during he h lnning rocess, considering differenil consrins. Rodm Consrucion Some effors hve een recenly mde o ly he PRM mehod o closed chin sysems. [-6,,] he min drwck of hese roches is h oo mny smles my hve o e esed efore finding fesile configurion nd oo much comuing ime is sen in solving closure equions leding o imginry vlues. o solve his rolem, we uilize he PRM roch, wih geomericlly guided smling mehod clled he Rndom Loo Generor (RLG, [5] which noly increses he roiliy of oining rel soluions when solving he loo closure equions [Algorihm in le ]. In ddiion, in he lrge worksce of moile mniulor sysem, PRM meho require severl hours of comuion ime o genere well-conneced rodm. hus, we fix he osiion nd orienion of oh he mniulor ses firs, nd consruc rodm (fixedse rodm, which conins n differen self-collision-free closure configurions. hen we oule he environmen in m rndom locions wih coies of he kinemic rodm nd connec he configurions of he sme closure ye. he sic rincile of he RLG mehod, s sed in lgorihm, is grounded on sering he configurion vriles ino wo ses: Acive (q nd ssive (q. here re some limiions in defining cive nd ssive vriles, [5] for exmle:. he oin sce vriles in q nd q corresond o he consecuive oins in he mechnism.. he numer of ssive vriles is equl o he oec s degrees of freedom. If mulile choices exis for hese vriles h sisfy he ove-menioned limiions, ll of hem cn e used in he mehod. In he remining r of his ricle, we refer o cive nd ssive vriles s follows: le : RLG mehod for guided rndom smling in he rodm consrucion hse (Algorihm. Secify cive nd ssive suchins s q nd q. for ech cive vrile 3. comue on inervl h closed - chin consrin equlions hve soluion in i.. Choose he cive vrile rndomly form he comued inervl 5. end for 6. solve he closed - chin consrin equions for ssive vriles fer susiuing cive vriles. 7. if here is soluion 8. use q nd q o consruc he closed - chin rndom configurion (ree unil ining rel soluion Vol Issue Jn-Ar 7

5 Bolndi nd Ehyei: A novel mehod for recory lnning of cooerive moile mniulors q q = [ θ, θ, θ6 ] 3,, 5 = [ θ θ θ ] ( he lnner direcly cs on he cive vriles while he ssive ones re oined y solving loo-closure equions. An imorn r of lgorihm is o comue n inervl for ech of he cive vriles, incresing he roiliy of hving rel soluions for loo closure equions. herefore, we should find suse of vlues for ech cive vrile h mkes is worksce rechle y he remining chin of he sysem. o his end, we require o illusre he worksce of he closed chin sysem, which is usully very comliced sk. hus, we hve o use n roxime roch. In his regrd, we simlify he model of our sysem s shown in Figure. Figure : A simlified model of he fixed se sysem o e used in RLG lgorihm herefore, we cn exress he rechle worksce for ech cive vrile s he inersecion re illusred in Figure 5. he exernl nd inernl rdii, r ex nd r in, in he figure corresond o he mximum nd minimum exensions of he chin resecively nd re roximed s follows: r r ex in = k i= L i L rex L > mx mx r = ow.. 8 ex (5 where L i nd L mx re he lenghs of he i h nd he longes link in he chin resecively. he roof of his equion cn e found in Aendix B. hen, fer choosing ech cive vrile in is comued inervl, we solve he closed-chin consrin equions for ssive vriles. Now, he configurion vecor of he fixed-se sysem, is wrien s: θ= q, q (6 Moreover, in order o consider he se srucure moiliy, uilizing lgorihm [le ], we oule he enire worksce wih rndomly seleced rs of he iniil rodm. Le us choose he se configurion, g, s rndom vecor including he osiion nd orienion of he firs roo, relive o he world frme: g = ( x, y, θ (7 hen, we smle node, θ, rndomly from he iniil rodm nd check he comined vecor of ( g, θ for collision. If he node is collision-free, we dd i o he new rodm. his rouine coninues for ll neighors of θ nd is reeed for m differen osiions, o cover he enire worksce. We colleced ll rodm nodes wih he sme closed configurion in se nd used he PRM connecion mehod Figure 5: Suse of vlues for n cive oin o e rechle y he remining chin of he sysem le : Pouling he environmen wih coies of he fixed-se rodm (Algorihm. Genere rndom se configurion g. choose rndom verex reeedly from he fixed se rodm unil ining collision free configurion (g, 3. if here exis collision free configurion. rein (g, s rodm verex 5. For ech neigher of, sy, in he fixed se rodm 6. if (g, is collision free 7. rein (g, s rodm verex 8. rerieve he h ( connecing nd from he fixed sed rodm 9. if (g, ( is collision free for ll inermedie configurions long he h,. dd n edge eween (g, nd (g, (ree s desired o connec he nodes in he se, s illusred in Figure 6. Finlly, we serched he grh for he shores ossile h eween he sr nd gol configurions, wih n oiml grh serch lgorihm. Avoiding Collision wih Fixed Oscles Smling-sed lnners mus erform mny collision checks in order o uild rodm nd send mos of heir running ime erforming such checks. herefore, Vol Issue Jn-Ar

6 Bolndi nd Ehyei: A novel mehod for recory lnning of cooerive moile mniulors heir collision deecion mehod mus e very fs, wihou missing ny collision or incorrecly deecing collisions, even when he worksce hs comlex geomery. We used wo yes of collision checks in our moion lnner: Sic checks, which were used o es wheher smled configurion in he rodm ws in he free sce, nd dynmic checks o es is locl hs, which were coninuous ses of configurions. In he sic meho common mehod ws o rek comlex oecs (roo link, oscle, ec. down, using ounding volume hierrchy (BVH, which ws hierrchy of BVs (e.g., sheres h roximed he geomery of he oec successive levels of deil. [5] BVH model of he cooerive sysem he choice of he ye of ounding volume for given licion is rde-off eween he ighness of fi nd he seed of oerions eween wo such volumes. herefore, o uild BVH for he cooerive moile mniulor sysem we use ounding sheres, which re very quick o es for collision wih ech oher in conuncion wih more recise, u lso more exensive ye of ounding volume ccording o Figure 7. Sic collision checking he sic ide ehind his mehod is, o check wo oecs for collisions, heir BVHs re serched from o down, mking i ossile o quickly discrd lrge suses of he oecs conined in disoined BVs. In oher wor, if he BVs he o level re in collision hen heir children re lso checked for collision. Oherwise, he lgorihm will no serch ny of he children. Figure 6: ( Iniil rodm wihou se srucure moiliy; ( Disriuing he iniil rodm o consider he se srucure moiliy Dynmic collision checking he clssicl roch o erform dynmic checks is o smle ech h some fixed resoluion nd siclly check ech smled configurion for collision. his roch is roxime nd cn miss collisions. herefore, we use newly resened dynmic checker [6] h excly deermines wheher h lies in free sce, y choosing n dive smling resoluion long he locl h. his checker uomiclly decides wheher h segmen eween wo collision-free configurions nee o e iseced furher. Le A q i ( denoe n oec A i from collecion of rigid oecs (including ech of he moile mniulors, ylod, oscles, ec. configurion q. We define ni ( q o e ny non-rivil lower ound on he Euclidin disnce eween Ai ( q nd A ( q. Le λ i ( q, q e n uer ound on he lenghs of he curves rced y ll oins in A i eween configurions q nd q long h segmen. A sufficien condiion for wo oecs A i nd A no o collide long h in he configurion sce is: [6] λ ( q, q + λ ( q, q < n ( q + n ( q (8 i i i Figure 7: A BVH model for he moile mniulor sysem If his inequliy is verified for ll irs of oecs A i nd A, hen he h segmen is collision-free, oherwise, i mus e iseced. Furhermore, o comue lower ound on he disnce eween wo oecs we work ccording o he Vol Issue Jn-Ar 9

7 Bolndi nd Ehyei: A novel mehod for recory lnning of cooerive moile mniulors clssicl BVH collision checker. However, insed of esing if wo BVs inersec, we comue he disnce eween hem nd find he smlles disnce found. Design of he recory A chllenging rolem during he lnning rocess is h he differenil consrins shll e sisfied wihou missing closed-chin consrins. Here, we resen n exension of he RLG lgorihm o solve his recory lnning rolem. Firs, we descrie he recory of ech cive vrile eween he sequenil irs of wy oins using he hird order olynomil equions, which re exressed in normlized inervl of ime s follows: q (= s + s+ s + 3s, i { x, y, θ, θ, θ, θ6} (9 k =,,..., nseg, s 3 where, n seg is he numer of h segmens, nd: s = ( f where, f is he ime required o rverse he k h segmen of he h. Now, we use he osiion nd velociy vlues he endoins of he h segmen o clcule he coefficiens of q s follows: q = (, = s= = 3( q ( q ( s= s= = + q ( q ( s= s= ( ( In his regrd, we use simle numericl roch o find he velociy vecor in he inermedie oins of he h: If he sloe sign of he srigh lines eween ech oin nd he revious nd nex ones chnge, he velociy is equl o zero, oherwise i is comued s he verge of he wo sloes. Nex, uilizing loo closure equions we exrc closed form o find ssive vriles from (: ( l3 l lo l5 ( ξ θ l5 lo ( l3 l = n (, n ( dd, + + β γ sin + = + θ3 β γ θ5 = θ+ θ + θ3 θ θ6 where: ( l3 l ξ = rcn l o β = + θ θ θ + θ γ = θ θ θ + θ d = l l + + l + + d = l l + + l + 5 g l6cos( 6 lcos( lcos( l6sin( 6 lsin( lsin( ( 3 cos( θ θ o sin( θ θ l5cos( θ θ θ ( 3 sin( θ θ o cos( θ θ + l sin( θ + θ θ (3 Now, we clcule he minimum ime o go from one inermedie oin o he nex wih regrd o he mximum llowed ccelerions. owrd his end, he following heorem eslishes lower limi for he recories, in he form of cuic olynomils such s hose in (9. heorem. Consider he cuic olynomil given y he following equion: 3 3 q (= s + s+ s + s, s ( wih s = nd α. hen he lower ound on f f h gurnies he ccelerion limis of q is: f mx (, + 33 α (5 Proof. As he second derivive of cuic olynomil is line, is mximum vlue occurs in one of he corresonding endoins: mx(, (6 In oher wor: s= s= 3 mx(, + 6 (7 By using f = we hve: 3 f mx(, +3 (8 Furhermore, in view of he consrin following condiion shll e sisfied: Vol Issue Jn-Ar α, he mx(, + 3 α (9 3 f nd (5 is concluded.

8 Bolndi nd Ehyei: A novel mehod for recory lnning of cooerive moile mniulors However, due o he nonlineriy nd comlexiy of he equions for he wheel velociies nd he ssive vriles, solving he ccelerion condiion for he exc lower ound on f cn e rohiiively exensive nd furhermore ose numericl rolems. herefore, uilizing equions (, (3, nd ( we comue liner roximion of he ccelerion nd wrie he following condiion: k α s= s= fk βins, k { θ3, θ, θ5, θ, ψ, θ,, ψ 3, 3, } k = k,,,,..., n seg (3 where α k is he mximum llowed ccelerion nd β ins > is n insurnce fcor h comenses for he roximion error nd minins he cuor ccelerion in he sfe ound. Oviously, using h-smoohing lgorihms nd incresing he numer of h segmens lowers he corresonding deviion from he exc vlue of he ccelerion. Consequenly, he ol lower ound on he ime o move eween dcen oins in ech secion of he recory would e he mximum of he lower oun comued in (5 nd (3: mx, fk = ( f fk i, (3 However, o find hese lower oun, ecuse of he lck of ime resonse informion, we uilized n roximion of in ech inermedie oin of he h, in lce of he cul velociies, nd ssumed h: s= = ( + s= Hence, from ( we cn wrie: fk = fk ( + = f ( k+ = (3 (33 which shows h wih his ssumion here re disconinuous velociies when we move eween dcen segmens of he h. herefore, we ly he oun comued in (3 o find n roximed velociy in ech inermedie oin hrough he reviously menioned numericl roch. hen, we cn wrie: Finlly, susiuing (3 ino (5 nd using (3 nd (3 we cn comue new vlues for fk. SIMULAION RESULS Le us consider wo sme moile mniulors, s shown in Figure. o verify he effeciveness of our mehod we will conduc simulions sed on he following ssumions: Ech moile mniulor is sueced o he differenil consrins menioned in (9 nd (3 nd is gol is o cooere wih he oher o sisfy he closed-chin consrins in ( he rnsoring oec is rigid ody h cnno e deformed he sr nd gol configurions re se o: q q sr gol = π π,,,,,,,, π π π =,,,,,,,, (3 he environmen is ouled wih six sic oscles in he form of sheres wih secified osiion nd rdius, s shown in Figure 8 nd c Some imorn rmeers used in he lnner re chosen s er le 3: Figure 8 shows 3-D visulizion for he recory of he oec wih nd wihou oscles. As shown here, here is smooh recory eween he sr nd gol configurions in oh cses. Furhermore, ccelerions of he comued recories for oin sce vriles re shown in Figure 9, nd i cn e seen h hey re ounded wihin he mximum llowed ccelerions ( rd/s. From (9- he recory of ech cive vrile eween he sequenil irs of wy oins is descried y he hird order olynomil equions. herefore, in Figure 9 he ccelerions of he cive oins re in he form of rm signls; however, his is no he cse for he ssive vriles. he quliy of he designed recory is shown in Figure. his figure illusres he deviion of he oin sce vriles from heir finl vlues. q = (, = fk = = 3( q ( fk q ( fk + 3 = fk + = = fk = = fk q fk q ( ( ( (3 le 3: Simulion rmeers Prmeer Vlue n 3 m g, l o. (m l, l 6.8 (m l, l 5.5 (m l 3, l.3 (m Vol Issue Jn-Ar 3

9 Bolndi nd Ehyei: A novel mehod for recory lnning of cooerive moile mniulors Figure 9: Join ccelerions of he mniulors CONCLUSIONS A novel recory lnning mehod hs een designed for dul moile mniulors grsing common oec. Becuse of is srucure, our mehod cn hndle high dimensionl configurion sces efficienly. I lso uilizes he dvnge of decouled meho o decomose he comlexiy of he recory lnning rolem in wo ses, which increses he simliciy of he lnning rocess. he min dvnge of his mehod is h such consrins s nonholonomic nd closed-chin consrins, long wih he oin nd ccelerion limis, cn e esily del wih in he resence of sic oscles. In comrison wih he oher roches discussed in lierure, he dvnges of he roosed mehod hve een shown in le. Furhermore, he resuls of comuer simulions confirmed he effeciveness of he mehod. A good ide o imrove he lgorihm roosed in his sudy is o develo mehod o kee he sysem from colliding wih he moving oscles. Also, invesiging he recory lnning of more hn wo cooering moile mniulors o erform he crrying sk seems o e n ineresing roue o follow. APPENDIX Deil Derivion of Equion ( Figure 8: Oec s recory eween he redefined sr nd gol configurions: ( wihou oscle, ( wih oscles, (c wih oscles (o view I cn e seen h he errors converge o zero s ime goes on. Figure illusres he ime required o rech he gol configurion. 3 Uilizing he coordine sysems in Figure long wih he conce of rnsformion mrices, we cn wrie: v v v v 3 3 e = e (A. in which B A is mrix h rnsforms he coordine Vol Issue Jn-Ar

10 Bolndi nd Ehyei: A novel mehod for recory lnning of cooerive moile mniulors Figure : Errors of he oin sce vriles: ( wihou oscle, ( wih oscles Figure : he disnce eween curren nd gol osiions of he firs end effecor: ( wihou ny oscle, ( wih sic oscles le : Comrison of recen roches on moion lnning for cooerive moile mniulors Mehod Comlee meho Oiml conrol sed, Numericl oimizion,3 Arificil oenil fiel 6 Rndomized or roilisic comlee meho Single query meho 3 Mulile query meho 3-6 Comuionl comlexiy Very high High Very high Crieri Considering differenil consrins in he recory Gurnees o find soluion if i exiss Modere No Low No Our mehod Low No Alicle o high dimensionl sysems No No No sysem { B} ino { A}, nd he differen coordine sysems in he ove equion re defined s follows: herefore, from (A., fer furher mniulion, he vlidiy of ( cn e verified. cn e ounded y wo concenric sheres, s shown in Figure 5. he rdii of hese sheres corresond o he lengh of he mechnism in he cse of minimum nd mximum exensions. his lengh is defined y he disnce eween he origins of he se-frme nd he end-frme s: Proof of Equion (5 he rechle worksce of ny riculed mechnism r = k i= L i i (B. Vol Issue Jn-Ar 33

11 Bolndi nd Ehyei: A novel mehod for recory lnning of cooerive moile mniulors v ( θ sin( θ ( θ cos( θ sin = l θ sin θ l ( ( = sin θ cos θ ( ( 3 = l ( ( θ3 sin θ3 l ( 3 ( 3 = sin θ cos θ = ( ( θ sin θ sin( θ cos( θ 3 l = v v ( θ sin( θ ( θ cos( θ = sin sin cos = sin e = ( ( ( ( ( θb θb θb θb d α ( θb θb cos θb θb d sin α lo l v 3 e = l 3 ( θ sin( θ ( θ cos( θ sin = l = l = 6 ( ( θ6 sin θ6 sin( θ6 cos( θ6 θ5 sin θ5 l6 ( 5 ( 5 = sin θ cos θ 6 5 ( ( θ sin θ l5 ( ( = sin θ cos θ 5 ( ( l 3 = 3 3 ( θ sin( θ 3 3 ( θ cos( θ = sin where L i (i =,, k is he lengh of i h link nd i, is fcor h ms hese lenghs in he direcion of he vecor, connecing he origins of he se-frme nd he end-frme. hus, y susiuing i = or i =, i {,, k} in (B. we cn comue mximum vlue for r s follows: k mx = L i i= r 3 [ ] (B. Now, suose h he h link of he sysem hs he longes lengh, L mx ; hen, from (B. one ges: r = L+ L+ L33+ n+ Lmx + n+ Lkk (B.3 herefore, we cn wrie he following equion o comue minimum vlue for r: r min L L n L + Lmx L+ n Lk if Lmx > L+ L + n+ L + L + n+ Lk = if Lmx L + L + n + L + L+ n + Lk nd from (B. we cn conclude h: r min L r if L > r = if Lmx r mx mx mx mx which shows he vlidiy of (5. mx (B. (B.5 Vol Issue Jn-Ar

12 Bolndi nd Ehyei: A novel mehod for recory lnning of cooerive moile mniulors REFERENCES. J. P. Desi, C. C. Wng, M. Zefrn, nd V. Kumr. Moion lnning for mulile moile mniulors. In Proc. IEEE Inernionl Conference on Rooics nd Auomion,. 73-8, J. P. Desi, nd V. Kumr. Moion lnning for cooering moile mniulors. J Roo Sys 6, , S. Furuno, M. Ymmoo, nd A. Mohri. recory lnning of cooerive mulile moile mniulors. In Proc. 3 IEEE Inernionl Conference on Inelligen Roos nd Sysems. vol.,. 36-, 3.. Y. Ymmoo, nd S. Fukud. recory lnning of mulile moile mniulors wih collision voidnce ciliy. In Proc. IEEE Inernionl Conference on Rooics nd Auomion. vol., ,. 5. J. Alric, nd R. Z. Moion lnning of cooerive nonholonomic moile mniulors. In Proc. IEEE Inernionl Conference on Sysems, Mn, nd Cyerneics. vol. 6,. 39-3,. 6. H. G. nner, S. G. Loizou, nd K. J. Kyrikooulos. Nonholonomic nvigion nd conrol of cooering moile mniulors. IEEE rns Ro Auom 9,. 53-6, J. Vnnoy, nd X. Jing. Rel-ime moion lnning of mulile moile mniulors wih common sk oecive in shred work environmens. In Proc. 7 IEEE Inernionl Conference on Rooics nd Auomion,. -6, M. Aou-Smh, C. P. ng, R. M. Bh, nd V. Krovi. A kinemiclly comile frmework for cooerive ylod rnsor y nonholonomic moile mniulors. Auon Roos,. 7-, R. M. Bh. owr modulr cooerion eween mulile nonholonomic moile mniulors, Ph.D. hesis. Bufflo: Se Universiy of New York; 7.. C. P. ng, R. Bh, nd V. Krovi. Decenrlized kinemic conrol of ylod rnsor y sysem of moile mniulors. In Proc. IEEE Inernionl Conference on Rooics nd Auomion,. 6-7,.. J. P. Desi, nd V. Kumr. Nonholonomic moion lnning for mulile moile mniulors. In Proc. IEEE Inernionl Conference on Rooics nd Auomion,. 39-, Y. L. Fu, Q. H. Yn, nd Y. L. M. recory lnning of moile mniulors in consrined worksce. J Hrin Insiue echnol (New Series 5,. 5-, J. Cores. Moion lnning lgorihms for generl closed-chin mechnisms, Ph.D. hesis. oulouse, Frnce: Insiue Nionl Polyechnique de oulouse, 3.. L. Hn, nd N. Amo. A kinemics-sed roilisic rodm mehod for closed chin sysems. In Proc. Inernionl Worksho on Algorihmic Founions of Rooics (WAFR,. 33-5,. 5. J. Cores,. Simeon, nd J. P. Lumond. A rndom loo generor for lnning he moions of closed kinemics chins using PRM meho. In Proc. IEEE Inernionl Conference on Rooics nd Auomion,. -6,. 6. J. Cores, nd. Simeon. Smling-sed moion lnning under kinemic loo-closure consrins. In Proc. 6 h Inernionl Worksho on Algorihmic Founions of Rooics,. 7. H. G. nner, K. J. Kyrikooulos, nd N. I. Krelis. Modeling of mulile moile mniulors hndling common deformle oec. J Roo Sys 5, , E. o, G. Rush, nd R. Surez. Anlysis nd clssificion of mulile roo coordinion meho. In Proc. IEEE Inernionl Conference on Rooics nd Auomion, ,. 9. H. Chose, K. Lynch, S. Huchinson, G. Knor, W. Burgrd, L. Kvrki, e l. Princiles of roo moion: heory, Algorihms, nd Imlemenion. Unied Ses: MI Press; 5.. S. M. LVlle, J. H. Ykey, nd L. E. Kvrki. A roilisic rodm roch for sysems wih closed kinemic chins. In Proc. 999 IEEE Inernionl Conference on Rooics nd Auomion,. 67-6, J. H. Ykey, S. M. LVlle, nd L.E. Kvrki. Rndomized h lnning for linkges wih closed kinemic chins. IEEE rns Ro Auom 7,. 95-8,.. J. P. Lumond. Roo moion lnning nd conrol. Sringer, S. M. Lvlle. Plnning lgorihms. Cmridge: Cmridge Universiy Press; 6.. H. G. nner, nd K. J. Kyrikooulos. Moile mniulor modeling wih Kne s roch. Rooic 9, , G. Bergen. Collision deecion in inercive 3D environmens. Amserdm: Elsevier;. 6. F. Schwrzer, M. Sh, nd J. C. Lome. Adive dynmic collision checking for single nd mulile riculed roos in comlex environmens. IEEE rns Roo, , 5. How o cie his ricle: Bolndi H, Ehyei AF. A novel mehod for recory lnning of cooerive moile mniulors. J Med Sign Sens ;:-35 Source of Suor: Nil, Conflic of Ineres: None declred BIOGRAPHIES Hossein Bolndi received his D.Sc. degree in elecricl engineering from George Wshingon Universiy, Wshingon, D.C., in 99. Since 99, he hs een wih he College of Elecricl Engineering, Irn Universiy of Science nd echnology, ehrn, Irn, where he is n ssocie rofessor. His reserch ineress re in iude deerminion nd conrol susysems of sellies, rooics, nd dive conrol. Dr. Bolndi is he uhor of 5 ournl ricles nd 5 ricles in he inernionl nd Irnin conference roceedings. Amir Frhd Ehyei received he B.Sc. degree in conrol engineering from he Shrif Universiy of echnology, ehrn, Irn, in. He received he M.Sc. degree in conrol engineering in 3, from he Irn Universiy of Science nd echnology, ehrn, Irn, where he is currenly Ph.D. cndie. His min fiel of ineres include nvigion nd conrol of muli-roo sysems. Since, he hs worked wih severl comnies in he field of Insrumenion nd Process conrol including Power Engineering Consulns of he Minisry of Energy (MOSHANIR nd Irn Inernionl Engineering Comny (IRIEC. Vol Issue Jn-Ar 35