Real-time Clothoid Approximation by Rational Bezier Curves

Transcription

1 008 IEEE Intrnatonal onfrn on Robots and Automaton Pasadna A USA May Ral-tm lothod Approxmaton by Ratonal Bzr urvs olás Montés Alvaro Hrraz Lopoldo Armsto* and Josp Tornro* *Mmbr IEEE Abstrat- Ths papr prsnts a novl thnqu for mplmntng lothodal ral-tm paths for mobl robots. As frst stp Ratonal Bzr urvs ar obtand as approxmaton of th Frsnl ntgrals. By rsalng rotatng and translatng th prvously omputd RB an on-ln lothodal path s obtand. In ths pross offnts wghts and ontrol ponts ar pt nvarant. Ths on-ln approah guarants that an RB has th sam bhavor as th orgnal lothod usng a low urv ordr. Th rsultng lothodal path allows any two arbtrary poss to b jond n a plan. RBs worng as lothods ar also usd to sarh for th shortst boundd-urvatur path wth a sgnfant omputatonal ost rduton. In addton to ths th proposd thnqu s tstd on a ral mobl robot for trajtory gnraton and nmat ontrol. To th authors nowldg th prsnt approah s th frst thnqu whh allows ral-tm lothodal path omputaton. I. ITRODUTIO Trajtory gnraton for autonomous vhls has bn subjt to xtnsv rsarh n rnt dads. Th smplst soluton s to gnrat a trajtory onatnatng ln and ar sgmnts [] [3]. Th man dsadvantag of ths thnqu s th urvatur dsontnuty btwn sgmnts. Ths problm an b ovrom by usng smooth transton urvs btwn straght lns and ar sgmnts. In addton onstantly varyng ntrfugal alraton jr s dsrabl n ordr to mnmz whl slp problms. Ths typ of urvs s ommonly nown as lothod or ornu spral. In ths sns lothods hav bn usd n mobl robot trajtory gnraton [5] [6]. Anothr possblty s to ombn only pws lothods to jon two poss (xyθ) n a plan [7] [8]. In ths paprs authors ntrodud th onpt of Elmntary paths that s two qual onatnat pws lothods to jon symmtral poss. Thy also ntrodud th onpt of B-Elmntary paths to jon two arbtrary poss n a plan just only ombnng two dffrnt lmntary paths. It s ntrstng to not that lothods hav also bn usd Ths rsarh has bn partally sponsord by Th Spansh Govrnmnt MyT Projt Rf. DPI P-05 DPI DPI and BIA Montés s wth th Systms Engnrng and ontrol Dpartmnt Thnal Unvrsty of Valna Valna Span (orrspondng author to provd phon: ; -mal: nmonsan@upvnt.upv.s). A. Hrraz s wth th nsttut of Dsgn and Manufaturng of Valna Span (-mal: alhrmar@upvnt.upv.s). L. Armsto s wth th Systms Engnrng and ontrol Dpartmnt Thnal Unvrsty of Valna Span (-mal: loaran@sa.upv.s). J. Tornro s wth th Systms Engnrng and ontrol Dpartmnt Thnal Unvrsty of Valna Span (-mal: jtornro@sa.upv.s). for many yars as transton urvs n road dsgn [] gvn that onstant jr guarants passngr omfort. Unfortunatly lothods ar transndntal urvs dfnd n trms of Frsnl ntgrals whh annot b solvd analytally. For ths rason n rnt yars rsarh fforts hav bn fousd on fndng ontnuous funton approxmaton thnqus [9]-[]. Howvr ths thnqus sussful n AD platforms annot b usd n ral-tm path plannng gnraton du to th grat omputatonal ost of hgh ordr approxmaton urvs. In ths sns [0] uss a 6 th ordr ontnuous funton whh s unaptabl n ral-tm systms to th authors opnon. In [] a Fxd Pont traton thnqu s usd to fnd an approxmatd soluton of Frsnl ntgrals whh rqurs a hgh omputatonal ost. Th rsults obtand n [] do not guarant nhrnt proprts of lothods. In addton to mntond problms to approxmat a lothod jonng pws lothods rqurs addtonal tratv mthods [7] [8]. Th goal of our rsarh s to prsnt a thnqu that omputs a gnral ontnuous urv approxmaton of lothods at th lowst possbl dgr guarantng at th sam tm lothodal bhavor. Th pross gnrats frst an off-ln gnral approxmaton of Frsnl ntgrals and thn partularzs t by rsalng rotatng and translatng an on-ln urv s obtand. Thrfor lothodal path onstruton s arrd out wthout traton sutabl for ral-tm applatons. Our prvous wor [] prsntd a gnral off-ln approxmaton of th Frsnl ntgrals nto Ratonal Bzr urvs (RB). Ths papr rprsnts a ontnuous progrsson of [] n th sns that lmntary paths ar onstrutd smply by rsalng rotatng and translatng th gnral off-ln formulaton png offnts wghts and ontrol ponts of RBs nvarant. Ths papr s organzd as follows. In ston II a brf rvw of lothod urv proprts s prsntd. Ston III shows a brf rvw of th mthodology to approxmat Frsnl ntgrals by Ratonal Bzr urvs []. Ston IV stablshs th mthodology to omput B-lmntary paths usng th gnral off-ln lothodal formulaton. In Ston V th mthodology s tstd to sarh for th shortst boundd-urvatur paths. In Ston VI lothodal paths as ontrol rfrns ar tstd on a ral mobl robot. onlusons and futur wors ar prsntd n Ston VII /08/$ IEEE. 6

2 II. PROPERTIES OF LOTHOID URVES Th ornu spral or lothod urv s dfnd paramtrally n trms of Frsnl ntgrals as follows: π ξ os dξ () x() () 0 () Q () y S() π ξ sn dξ 0 whr s a postv ral numbr s a non-ngatv ral numbr. lothod urvs hav th followng proprts:. Angl of tangnt: τ π. urvatur: π Radus R 3. Ar lngth L: L π A whr A s th wll-nown lothod onstant paramtr.. Homotal fator π A Th most attratv proprty of th lothod urv s that: A R () L whr R s th radus of th urvatur. Ths proprty guarants smooth transtons stablshng at th sam tm a lnar rlaton btwn th urvatur and th ar lngth. In addton th varaton of th ntrfugal alraton J s dfnd by th topographrs as []: 3 V A J whr V s vhl vloty. As mntond n th ntroduton Frsnl ntgrals must b solvd numrally. Approxmaton mthods us polynomal and non-polynomal funtons. In partular all xstng thnqus nvolvng non-polynomal funtons [9] ar only usful whn approxmatng Frsnl ntgrals n a sngl pont. Howvr AD/AM systms or mobl robot trajtory gnraton moduls rqur a ontnuous funton. For ths purpos polynomal funtons ar th dal soluton. Th standard polynomal funtons ommonly usd n AD/AM ar Bzr Ratonal Bzr B-spln and URBS. Som of ths urvs hav bn usd for lothod approxmaton [0]-[]. III. PREVIOUS WORS Our prvous wor [] prsntd an off-ln mthodology to approxmat th lothod by Ratonal Bzr urv for a sltd worng ntrval. Th RB has th followng formulaton:! w ( u) ( u) 0! ( )! Pu! w ( u) ( u) 0! ( )! whr: : ontrol ponts : Ordr of th RB w : Wghts of th ontrol ponts u : Intrns paramtr [0 ] In ordr to onstrut a lothod-l Ratonal Bzr t s nssary to hang th varabl u whr and ar th lmts of th sltd worng ntrval for th Frsnl ntgrals whh an b alulatd from lothod proprts xpland n Ston II. Ths s basd on th tangnt angl urvatur and th ar lngth of th lothod as sn n Ston II and Fg.3. Th RB has two dgrs of frdom orrspondng to ontrol ponts and wghts. In [] frst th ontrol ponts ar omputd forng th wghts to. Ths translats th RB to a Bzr urv that an b xprssd as a lnar quaton of ths ontrol ponts as follows P B 0 B B whr B s th th Brnstn bass funton:! B! ( )! In that as Frsnl ntgrals P (() S()) ar obtand usng a non-polynomal approxmaton xpland n [9] that omputs th Frsnl ntgrals wth an auray of 0-0. Th rsultng lnar quatons an b solvd by last squars thnqus obtanng th ontrol ponts of th RB that approxmat th Frsnl ponts. Ths approxmaton dos not touh th start and nd ponts nor dos t guarant th rqurd ontnuty at th start pont. To ovrom ths problm th ontrol ponts hav to b ford at ths loatons at th ost of drasng th approxmaton auray unlss th ordr of th urv s nrasd. An altrnatv to nrasng th ordr of th Bzr urv s to omput th wghts usng th prvously omputd ontrol ponts and for th start and nd ponts to th orrt loatons. In that as th wghts ar also omputd usng last squars thnqus. As shown n [] th RB has th sam bhavour as th lothod wth homothtal fator qual to and rror n th 0 approxmaton lss than 0. Fg. shows a blo dagram of th mthodology. In ordr to obtan a gnral off-ln formulaton of th lothod on of th man proprts of th paramtr urvs that s transformaton nvaran [] s usd. Ths proprty allows th RB to b rotatd translatd and rsald through th ontrol ponts. ot that th homothtal fator of th lothod s nludd n th RB as a salng fator as shown n (3). P 0 ( ) w ( ) B 0 w B (3) 7

3 FRESEL POITS BY O-POLYOMIAL FUTIOS WORIG ITERVAL IITIAL RB ORDER SET OF LIEAR EQUATIOS WITH WEIGHTS EQUAL TO LEAST SQUARES ontrol ponts REALLOATIOI OF THE OTROL POITS IREASE THE RB ORDED From th splt pos th tangnt angls of th frst lothod(τ τ ) ar obtand as: ( θ s θ )/ ( θ θ )/ τ () τ (5) s Th sond lothod s qual and symmtr to th frst on. Ths mans that th ontrol ponts of th sond lothod an b obtand by smply tang th symmtr ontrol ponts of th frst lothod s Fg.. SET OF LIEAR EQUATIOS WITH THE RESULTIG OTROL POITS LEAST SQUARES wghts ERROR< *0-0 o Ys WEIGHTS AD OTROL POITS Fg. Blo dagram of th approxmaton thnqu Th sltd worng ntrval dpnds on th applaton. For a π rotaton four lothods wth a worng ntrval of [ 0 π ] hav to b ombnd. In ths as th approxmaton thnqu dtrmns that an th ordr RB guarants lothodal bhavor. Th orrspondng wghts and ontrol ponts ar shown n Tabl I. TABLE I OEFFIIETS OF THE RB FOR A PATH PLAIG S w w Th most ntrstng aspt to rmar s that th homothtal fator ats as a salng fator. Thrfor th offnts ar omputd just on for any group of lothods. IV. BI-ELEMETARY PATH In ths papr an lmntary path s onsdrd as a onatnaton of two qual pws lothods. Furthrmor two poss n th plan an b obtand by onatnatng two dffrnt lmntary paths to form a Blmntary path [7] [8]. In partular to ln th start pos p ( x y θ ) and nd pos p ( x y θ ) t s nssary to omput th splt pos p s ( xs ys θ s ) whh s a symmtr pos wth rspt to th start and nd poss [3]. Fg.. ontrol ponts symmtry: onstruton of an lmntary path. In addton a pvot ntrmdat pont ( S) dfnng a symmtral axs btwn th two lmntary paths s obtand. Ths pvot pont s alulatd ntrodung th tangnt angls (τ τ ) n th RB as dsrbd n Fg. 3. urvatur Ar Lngth Tangnt angl L τ π π! w ( ) ( ) 0!! P( ) n! w ( ) 0!! x( ) P( ) y( ) Fg 3 Blo dagram of RB ronfguraton As th tangnt angl dos not dpnd on th homothtal fator t s possbl to onstrut normal lmntary paths wth. Aftrwards th spf lmntary paths wll b romputd usng th homothtal fator as a salng fator as sn n Fg.. Wthout salng fator Wth salng fator Fg. Elmntary path onstruton wth nd pos ( ) n 8

4 Thrfor th nxt stp wll b to sal th ontrol ponts onsdrng that th last ontrol pont of th sond lothod of th frst lmntary path and th last ontrol ( ) S pont of sond lothod ( ) S blong to th sond lmntary path has to ond wth th splt pos and th nd pos rsptvly that s: x x y y s s (6) S x x y y s s (7) S Wthout losng gnralty both lmntary paths ar onstrutd wth rspt to th oordnat orgn. Fg.5 s a dagram of lmntary path onstruton. Fg 5. Dagram of th lmntary path onstruton V. THE SHORTEST BOUDED-URVATURE PATH As dmonstratd n [3] th lo of splt poss (ntrmdat poss) jonng th start and nd poss wth a B-lmntary path s a rl. Thrfor t xsts an nfnt st of solutons (B-lmntary paths wth dffrnt lngths and urvaturs) jonng start and nd poss. In partular w ar ntrstd n obtanng th shortst boundd-urvatur path that satsfs nmat urvatur onstrans of vhll mobl robots. Unfortunatly ths soluton an max not b obtand wth analytal mthods; rqurng hurst algorthms. Basd on th das from [5] w hav dvlopd a hurst mthod to fnd th shortst boundd-urvatur path. In ths papr th shortst boundd-urvatur path s obtand by onatnatng rular ars wth th maxmum allowd urvatur and straght lns. Howvr rsults from [5] do not satsfy urvatur ontnuty ondton. On th ontrary our approah solvs ths problm usng a B-lmntary path that nhrntly satsfs ths ondton. Obvously our soluton obtan on lmntary path wth th maxmum urvatur and mnmum salng fator (smlar to th maxmum urvatur rl) and anothr lmntary path wth mnmum urvatur and maxmum salng fator (smlar to a straght ln). Basd on [3] th ntr ( x y ) and radus r of th rl (lo of splt poss) dpnds on th start pos p and nd pos p gnrally xprssd as: ( x y r ) f ( p p ) Addtonally th splt pos dpnds on th ndpndnt varabl θ (angl th rl) as shown n Fg. 7: p h s ( p pθ ) Th last stp s to translat and rotat th ontrol ponts of th RBs untl th orrt pos dfnng a B-Elmntary path. Fgur 6 shows an xampl of B-lmntary path onstruton. Fg 6 B-lmntary path: Start pos:(000) End pos: (3000.) Th worng ntrval of th rsultng RBs for th four lothods ar [0 ( τ )] ] ( τ ) 0] ]0 ( τ )] ] ( τ ) 0] rsptvly. In ontrast to th mthod dsrbd n [7] and [8] ths mthod avods tratv produrs whn omputng lmntary paths. Fg 7. Dagram of th lmntary path onstruton Thrfor th problm s statd as follows: L mn ( Lngth( BElmntary( p p p ))). wth mn s max p It an b shown that th splt pos for th optmal soluton s always loatd on th non-shadowd ara H of th rl 9

5 dptd n Fg. 7 f only f th urvatur ondton s satsfd n th ara. Othrws th soluton wll b nssary loatd on th shadowd ara H. Thrfor our sarh produr trs frst to fnd a soluton n H and f dosn t xsts thn loos for soluton n H. Basd on [5] our sarh produr loos for th pos that satsfs th urvatur ondton and t s losst to thr start or nd pos. It s mportant to rmar that n our mthod t s not nssary to dvlop th whol B-lmntary path to omput th lngth of th path. Ths an b don frstly omputng th tangnt angl of th Elmntary paths usng Equatons () and (5). Latr trms and of th Frsnl ntgrals (salng fators of th RBs) ar alulatd through Equatons (6) and (7). In ths as only th last ontrol pont of th sond lothod of th Elmntary path ( ) S ( ) t s rqurd to omput th ar lngth whh an S b don smply by projtng th frst ontrol pont of th frst lothod. Fg. 8 dpts thr gomtr lo (possbl splt poss wthn boundd-urvatur) whr th optmal soluton has bn found n H. Fg. 9. Boundd-urvatur shortst path out of th shortst path Wth ths hurst mthod th omputatonal ost of th sarh produr s sgnfantly rdud ompard to brutfor sarh on th whol rl. In our smulatons usng a. GHz Pntum IV If th soluton xsts n H th man tm rqurd was 3 ms. Othrws svral tratons wll b rqurd at th ost of 0.6 ms/traton. VI. EXPERIMETAL RESULTS In ordr to tst th nw formulaton a trajtory gnraton modul has bn mplmntd on th dffrntal mobl robot shown n Ptur. Start pos (000) & End pos (030) Ptur Dffrntal mobl robot Start pos (000) & End pos (050.) Start pos (00.) & End pos (3050.) Fg. 8. Boundd-urvatur shortst path wth RBs as lothods Fg. 9 shows th as whr th soluton ould not b found n H and thrfor th sarh produr was fousd on H. Ths robot s quppd wth an ndustral P I PXI-886 wth a Pntum IV at. GHz. Wth ths prossor lothodal paths ar omputd n 5 ms. In ral applatons t s partularly mportant to rsampl th RB spally whn usng omputd paths as ontrol rfrns. In any as as th RB s a ontnuous funton ontnuous rsamplng s always possbl. urvatur ar lngth and tangnt angls dpnd on ah vhl and path rqurmnt as xpland n Ston IV. In partular for a tryl-l mobl robot th urvatur s rlatd to th strng whl turnng radus whl ar lngth s onsdrd for dffrntal mobl robots. Th man proprty of th lothod s that t guarants a onstant Jr whh mnmzs whl slp. Ths produs rrors that annot b masurd by th nodrs and thrfor th mobl robot s quppd wth nrtal snsors apabl of masurng varaton n th tangntal alraton. Th opnloop ontrol strutur usd n our xprmnts s shown n Fg

6 Fg. 0. Opn ontrol loop whr paramtr an b asly alulatd as V n T and n: numbr of sampls n th RB. T: samplng prod. V: mobl robot vloty. : salng fator. An xampl of th b-lmntary path followd by th robot s show n Fg.. (lft). Start pos s sltd to (000) and nd pos to (5). Th robot vloty s sltd to 0.5 m/s. th ntrfugal alraton suffrs by th robot s show n Fg. (rght). paramtr omputaton tas about 0.6 ms of omputatonal tm. As a rsult robot path plannng only rqurs about 5 ms to fnd th shortst boundd-urvatur path usng a hurst tratv mthod. In ral applatons t s partularly mportant to rsampl th RB spally whn usng th omputd paths as ontrol rfrns. Gvn that RBs ar ontnuous funtons ontnuous rsamplng s always possbl. In th papr t has bn shown dffrnt ass to rsampl a RB basd on: urvatur ar lngth and tangnt angl. In partular for tryl-l mobl robots th urvatur as sn t s rlatd to th strng whl turnng radus whl ar lngth as s onsdrd for dffrntal mobl robots. As a onluson t s also mportant to rmar that th thnqu prsntd n ths papr s a novl approah and to th authors nowldg t s th frst proposal to allow lothodal paths mplmntng ral-tm. As furthr rsarh t wll b ntrstng to xtnd D Frsnl ntgrals assoatd to lothods to 3D. Ths wll mply th applaton of th proposd path plannng mthod to robots wth 6 d.o.f. or vn mor. Ths mthod an also b usd to gnrat trajtors for hgh spd mahns. Fg.. lothodal path (rght). Inrtal snsor masurmnt (lft) VII. OLUSIOS AD FUTURE WORS Ths papr prsnts a mthod for obtanng ral-tm lothodal paths n mobl robots. Th mthod nvolvs two stps: ) to dfn off-ln approxmatons of lothods as Ratonal Bzr urvs (RB); ) To gnrat on-ln paths by smply rsalng rotatng and translatng th prvous off-ln formulaton. In whol pross offnts wghts and ontrol ponts ar pt nvarant. On of th man advantags of ths mthod s that a low ordr urv s obtand guarantng lothodal bhavor. In addton to ths th urvd s omputd off-ln whh s on of th most tm-onsumng tass. Anothr advantag of ths mthod s that onstruton of Elmntary paths and Blmntary path s omputd wthout rqurng tratv mthods. As a onsqun th mthod an b mplmntd n systms undr ral-tm rqurmnts. As t has bn shown thr ar nfnt solutons to jon two arbtrary poss n a plan usng B-Elmntary paths. In partular w ar ntrstd n th shortst-boundd urvatur path whh an b obtand basd on som hurst ruls. It has bn shown that th B-lmntary REFEREES [].Montés M..Mora and J.Tornro Trajtory gnraton basd on Ratonal Bzr urvs as lothods. IEEE Intllgnt Vhls Symposum.. vol.pp Jun 007 [] P. Jaobs and J. anny Plannng smooth paths for mobl robots Pro. IEEE Int. onf. Robots and Automaton vol. pp [3] J.P. Laumond J. Jaobs M. Tax and M.R. Murray A moton plannr for nonholonom mobl robots IEEE Trans. Robots& Automaton vol.0 ssu 5 pp [] I. d orral Manul d Vllna. Topografía d obras. SPUP 999. [5] T. Frahard A. Shur and R. Dsvgn From rds and shp s to ontnuous urvatur paths IEEE Int. onf. on Advand Robots pp [6] A. Shur and T. Frahard ontnuous-urvatur path plannng for multpl ar-l vhls IEEE Int. onf. on ntllgnt Robots. & Systms vol. pp [7] A. Shur and T. Frahard ollson-fr and ontnuous urvatur path plannng for ar-l robots IEEE Int. onf. on Robots& Automaton vol.. pp [8] A. Shur and T. Frahard Plannng ontnuous-urvatur paths for ar-l robots IEEE Int. onf. on Intllgnt Robots and Systms vol.3 pp [9]. D. Mlnz omputaton of Frsnl Intgrals II J.of Rsarh of th IST vol. 05 nº pp [0] L.Z. Wang.T. Mura E. aama T. Yamamoto and T.J. Wang. An approxmaton approah of th lothod urv dfnd n th ntrval [0 π/] and ts offst by fr-form urvs omputr Add Dsgn vol. 33 nº pp (0) 00. [] D.S. M and D.J. Walton An ar spln approxmaton to a lothod J. of omp.l and App. Math.vol.70()pp [] J. Sánhz-Rys and J.M. haón Polnomal approxmaton to lothods va s-powr srs AD vol.35nºpp [3] Y. anayama and B.I. Hartman Smooth loal path plannng for autonomous vhls IEEE Pro. Int. onf. on Robots and Automaton vol.3 pp [] Pgl L Tllr W. Th URBS boo. nd d. Brln: Sprngr997. [5] DubnsL.E.. On urvs of mnmal lngth wth a onstrant on avrag urvatur and wth prsrbd ntal and trmnal poston and tangnts Amran Journal of mathmats Vol 79 º3 pp