Proposal of Algorithms for Navigation and Obstacles Avoidance of Autonomous Mobile Robot

Transcription

1 Proposal of Algorthms for Navgaton and Obstacls Avodanc of Autonomous Mobl Robot T. T. Hoang, D. T. Hp, P. M. Duong, N. T. T. Van, B. G. Duong and T. Q. Vnh Dpartmnt of Elctroncs and Computr Engnrng Unvrst of Engnrng and Tchnolog Vtnam Natonal Unvrst, Hano Abstract Ths papr prsnts algorthms to navgat and avod obstacls for an n-door autonomous mobl robot. A lasr rang fndr s usd to obtan 3D mags of th nvronmnt. A nw algorthm, naml 3D-to-2D mag prssur and barrrs dtcton (IPaBD), s proposd to crat a 2D global map from th 3D mags. Ths map s basc to dsgn th tractor. A tracng controllr s dvlopd to control th robot to follow th tractor. Th obstacl avodanc s addrssd wth th us of sonar snsors. An mprovd vctor fld hstogram (Improvd- VFH) algorthm s prsntd wth mprovmnts to ovrcom som lmtatons of th orgnal VFH. Exprmnts hav bn conductd and th rsult s ncouragd. Kwords-3D-lasr mag; 2D-lasr scannr; localzaton; navgaton; obstacls avodanc. I. INTRODUCTION Th navgaton procss for an autonomous mobl robot can b dvdd nto four stps: nvronmnt mappng, tractor dsgn, moton control, and obstacl avodanc. In ordr to obtan good navgaton prformanc, t s ncssar to hav two sparatd parts: th global and th local navgaton. Th global procss uss nformaton tan from snsors whch hav nough vsblt of th nvronmnt such as th 3D lasr and camra to gnrat a round path plannng and control stratg. For non-structural nvronmnt l ndoor, th robot must vw th whol nvronmnt b tslf to crat a map and dsgn a tractor whch can lad t to th dstnaton. Wth th rqurmnt of hgh prcson, th mag data from th camra s oftn nsuffcnt du to ts dpndnc on th lght condton and th surfac structurs of obcts. In addton, a vson sstm cannot drctl masur th gomtr paramtrs such dstancs btwn obcts. Th stro camra can partl ovrcom ths problm but th computaton s xpnsv and th accurac s low [1, 2]. Instad, th lasr rang fndr s an approprat choc. It masurs dstancs at hgh rat and prcson whl nsnstv to nvronmnt s condtons [3]. Wth th constrant that th robot alwas movs on th floor plan, th navgaton tas can b xcutd b drctl usng th pxl cloud of th 3D nvronmnt [4-5]. Howvr, th algorthm s complcatd and th computaton s rlatvl hgh. In ths papr, w propos an algorthm whch bulds a 2D global map from 3D mags for th navgaton problm. Th algorthm, naml 3D-to-2D mag comprsson, basd on th da that pxls at a sam poston but dffrnt hght can b prsntd b a sngl pxl dscrbd th occupanc or vacanc of th nvronmnt that th robot movs through. Basd on th cratd 2D map, th A* algorthm s mplod to dsgn th global tractor [6]. Th moton control to follow th dsgnd tractor s drvd b th Lapunov stablt crtron [7-9]. In th local navgaton, th robot nds to ract to th chang of th nvronmnt such as a suddn apparanc of obstacls. Sonar snsors hav bn provn to b ffctv for ths tas [10-13]. A wll-nown algorthm s vrtual forc fld mthod [11, 13]. Nvrthlss, ths mthod has th w pont that whn th robot movs from a squar to anothr n th nt graph, th sum of propulsv forcs s changd much n both th ntnst and th drcton so that thr xsts th cas th robot cannot pass vacanc spac such as an opnd door bcaus th propulsv forcs from two sds of th door ma a ont forc whch pushs th robot awa from th door [12]. In ordr to ovrcom ths problm, w appld th VFH mthod of J. Bornstn [13] and tund t to b sutabl to th robot s confguraton. Ths mthod, calld th Improvd-VFH, uss th polar coordnat to control th robot to avod obstacls n a rgon from 0,3m to 4m. II. CONSTRUCTING THE 2D MAP FROM THE CLOUD OF 3D PIXELS OF THE ENVIRONMENT A. Collctng data from lasr snsor Fgur 1 shows th mag of th lasr rang fndr LMS- 211 (LRF) n assocaton wth othr snsors n a mobl robot dsgnd at our laborator. Th masurmnt of th dstanc to a pont n th obstacl s basd on th prncpl of dtrmnng th gong-rturnng tm of th lasr puls rflctd from th obstaclc. In ordr to gt th 3D mag of th nvronmnt, th LRF s stacd n a confguraton that t can ptch up and down around a horzontal axs as dscrbd n [3]. In ths wor w adust paramtrs of LRF so that vr ccl of up ptchng, th snsor gts 94 2D photo frams wth ptch angls n th rang from -5º to +20º and th ncrasng stp of 0.266º. Each obtand 2D photo fram contans a data st corrspondng to th horzontal scan of th lasr ra wth a scannng angl β n th rang from 40º to 140º and th rsoluton 1º. Consquntl, th obtand 3D mag s a cloud wth 9,494 pxls. Ths mag has th lnart n th hght du to th constant rat of th ptchng rotaton.

2 Th rlaton btwn ths two paramtrs s not monovalnt n ach 2D fram. It mans that thr s possbl on or man ponts wth dffrnt R (assocatd to dffrnt hghts of z) n corrspondnc to a scannng horzontal angl β. R2 R1 β 1 Robot x Fgur 1. Th lasr rang fndr and sonar snsors n assocaton wth othr snsors attachd n a mobl robot.[14-15]. Snc th snsor s put on th hght 0.4m rlatv to th floor and at th mnmum ptch angl -5º, th snsor onl can dtct obcts at a dstanc at last 4.58m. Obstacls whch ar n th smallr dstanc to th robot ar dtctd b ght sonar snsors. Th combnaton of data from th lasr rang fndr and sonar snsors gvs th robot an mag of th whol nvronmnt. B. Constructng th 2D global map from 3D lasr mags At a ptch angl α, th robot obtans a 2D mag fram consstng of pxls dtrmnd b th horzontal angls β and th dstanc R to th obct. From a st of 2D mag fram, Cartsan coordnats of pxls that crats th 3D mag of th nvronmnt ar dtrmnd as blow [3]. x z,,, R cosα cos β R cosα sn β R snα Th procton of pxls of obcts on a plan x- paralll wth th floor can b usd to construct th 2D map. Tang th unon (U) of all pxls whch hav sam coordnats (x, ) along th drcton z of th nvronmnt crats a unqu 2D map on th plan x- as shown n fgur 2. (1) Fgur 3. Dtctng pxls whch hav th mnmum dstanc (crcular ponts n samlss lns) Th gnratd 2D mag prsnts th surfac mag of obstacls n front of th robot. It s ncssar to dtrmn th boundar of mag rgons as th wll b us for th path plannng and obstacl avodanc. As shown n fg.3, ths can b don b slctng pxls (crcl ponts) that hav th smallst dstanc to th robot among pxls rcvd at th sam lasr scannng angl. Th rmanng pxls (squard ponts) ar nglctd. Aftr ths procssng stp, th rlaton btwn β and R bcoms monovalnt. Th sgmntaton algorthm s thn appld. Th PDBS mthod s mplod [16]. To furthr smplf th mag, t s unncssar to procss th pxls at th hght hghr than th robot hght as thos pxls do not affct to th robot navgaton. Lt z lmt b th robot hght. W can rmov all pxls wth th hght hghr thn z lmt. In summar, all stps to construct th 2D map from a 3D mag ar dscrbd as follows: Stp 1: Startng from th cloud of 3D pxls, th unon of all pxls s tan along th vrtcal drcton z. Stp 2: For ach horzontal scannng angl β, fnd and slct th valu R R mn. Stp 3: Rmov ponts wth (corrspondng to th floor). z > z and z 0 Stp 4: Prformng th mag sgmntaton algorthm (f ncssar). W call ths algorthm 3D-to-2D mag prssur and boundar dtcton (IPaBD). lmt III. TRAJECTORY DESIGN AND MOTION CONTROL Fgur 2. Prssur of frams n th 3D mag to gt th 2D map It s a matrx of pxls n whch vr pxl s xprssd b a par of paramtrs ( β, R), whr β s th scannng angl of th -th lasr ra and R s th dstanc masurd to th pxl. A. Tractor dsgn Th nxt stp s to fnd th optmal path for th robot to rach th dstnaton from th ntal poston (th path plannng). In IPaBD algorthm, scannng pxls that prsnt th surroundng nvronmnt of th robot can b dscrbd as a st of ln sgmnts as n [17] whr ln sgmnts ar obstacls such as wall, door, or lobb Basd on ths ln sgmnts,

3 th floor spac s dscrtzd nto grd-cll map n whch th sz of ach cll s st to b (a a) cm 2, whr a quals to 1/3 damtr of th robot chasss. Ths grd map can b prsntd as a matrx wth lmnts 0 and 1 whr lmnt 0 mpls th apparanc of obstacl. A dlaton procss s usd n occupd rgon to xtnd two mor clls to nsur that th robot cannot colld wth th occupd rgon. Each ln sgmnt xtractd from scannng pxls s charactrzd n th Cartsan coordnat b two nodal ponts. Ln sgmnt paramtrs such as lngth, ln quaton, and angular coffcnt can b asl dtrmnd from ths two nodal ponts. In grd map, clls of ln sgmnt cratd b two x, and nodal ponts hav th valu of 1. Lt ( 1,L ) 1,L ( 2,L 2,L ) x, b coordnats of two nodal ponts prsntng th th obstacl ( 1... n), th coordnats of ths two ponts n grd map ar calculatd as blow: x,l x mn,l mn x G, G, cllsz cllsz ( 1... n) ( 1...2) whr cllsz s th sz of th grd cll. Clls occupd b th ln sgmnt prsntd b x,g and,g ar dtrmnd accordng to th followng condton: mx1, G x 2, G x 1, G T f x1, G x 2, G n 1, G 2, G 1, G mx1, G T f x 1, G x 2, G whr (m, n) s th coordnat of cll n grd map and T s th sz of th robot. From data of occupd clls, w mplod th A* algorthm [6] to fnd th shortst path to th dstnaton D(x D, D ) from th startng cll S(x S, S ) (fgur 9). B. Moton control Aftr th path plannng, control algorthm nd dvlopng to navgat th robot to follow th dsgnd tractor. In ths papr w ntroduc a nw control algorthm as follows. Y O Y p 2R P Fgur 4. A noholonomc mobl robot platform. d 2r Lt th tangnt and angular vlocts of th robot b vandω, rspctvl. W hav: θ X p X 1 R x cosθ 0 vr r r v v v ; snθ 0 v l 1 R ω q ω θ 0 1 r r Th obctv of th control problm s to dsgn an adaptv control law so that th poston vctor q and th vloct vctor q trac th dsrd poston vctor q r () t and vloct vctor q r () t undr condton that robot paramtrs ar not xactl nown. Th dsrd poston and vloct vctors ar rprsntd b: T qr [ xr r θr] x r vr cosθr wth v r > 0 for all t (3) r vr snθr θ ω r r Th tracng rror poston btwn th rfrnc robot and th actual robot can b xprssd n th mobl bass fram as blow [18]: 1 cosθ snθ 0xr x p 2 snθ cosθ 0 r (4) θr θ Th drvaton of th poston tracng rror can b xprssd as: 1 ω2 v+ vr cos 3 p 2 ω1 vr sn + 3 (5) 3 ωr ω Thr ar som mthods n th ltratur to slct th smooth vloct nput. In ths rsarch, w choos a nw control law for v, ω as: v cos + r v sn 3 ω ωr 3 3 v + + r 2 3 whr 1, 3 > 0. On can s that whn 3 0 sn thn so ω alwas b boundd. Wth th control law n quaton (6), t s as to prov th asmptotcal stablt of th sstm. Choosng a postv dfnt functon V p as follows: ( ) (2) (6) 1 T 1 Vp p p + + (7) 2 2 Th drvaton of V p wth rspct to tm V p s:

4 T V p p p v+ v v v+ v + v + ( ω rcos ) ( ω rsn ) ( ωr ω) ( cos ) sn ( ω ω) 1 r 3 2 r 3 3 r Rplacng (6) nto (8) gvs V ( v+ v cos ) + v sn + ( ω ω) p 1 r 3 2 r 3 3 r sn3 1( vrcos vrcos3) + v 2 rsn 3+ 3ωrωr 3 3v r It s abl to s that V p s contnuous and boundd accordng to th Barbalat thorm. It mans that V 0 whn t. Consquntl, 1 0, 3 0 whn t. Applng th Barbalat thorm agan for th drvaton, w gt: 0, 0 (10) 1 3 and th quaton (6) bcoms p (8) (9) v v r (11) Cas 2: If θ θ t 90 o, th algorthm slcts a slot whch has th drctonal anglθ d so that θd θ s mnmum. Th algorthm of control of th angl vloct s slctd n o a smpl wa as follows: ω 10( θ d θ) and ω 25 / s. Th algorthm to control th tangnt vloct s slctd as follows: If th robot s far from th obstacl, VV max 0,5m/s. If th robot s nar th obstacl, V5(d ) m/s, whr d30 s th dstanc from th robot to th narst obstacl n th rang -30 o 30 o n th polar coordnat sstm attachd to th robot. o If ω 10 / s, th tangnt vloct V s rplacd V bv. 2,5 ω ω r (12) Combnng (5), (10), (11), (12) gvs: 2 0, 2 0. Thus th control law (6) assurs th proxmt control sstm p 0 whn t. C. Obstacl avodanc As dscussd n th scton I, w dvlopd a mthod for obstacl avodanc naml Improvd-VFH. Ths mthod uss a nt chart as n VFH but th updatng law s changd as dscrbd n Fgur 5. At ach tm radng nformaton from th ultrasound snsor, th algorthm assgns valus of 2 or 3 to squars as n th fgur, othr squars ar assgnd th valu 0. Th data swtch law from th nt chart to th polar chart s nhrtd from VFH. Aftr that, th procss of calculaton of th drctonal angl θd s dvdd nto 2 stps as follows: Stp 1: Calculat slots wth rstrcton n th rang of sctors such that θ θ 100 o d <. If thr xsts a slot satsfng θ θ 100 o d <, th robot wll control th drctonal angl so thatθ θd. Stp 2: If thr s not an slot satsfng th abov condton, th robot wll prform a lft or a rght rotaton wth an angl 100º. Th robot wll rotat lft f θ < θt and vc vrs, whr θ t atan2 ( t x, t x) wth xt, t, x, ar coordnats of th targt and th currnt coordnats of th robot, rspctvl. Th algorthm thn sarch a nw slot n th rangθ ± 100 o,.. rturn to th Stp 1. If n th rang θ ± 100 o thr xts mor than on slot, thn th algorthm wll slct a slot dpndng on ach cas: Cas 1: If θ θ 90 o t <, th algorthm slcts a slot whch has th drctonal anglθ d so that θd θt s mnmum. Fgur 5. Envronmnt nformaton updatng structur n th nt chart n Improvd VFH. IV. EXPERIMENT AND DISCUSSION A. Th rsult of constructng th map b th IPaBD algorthm Fgur 6 shows an 3D mag obtand b th lasr rang fndr wth th startng pont S(0,0) and th dstnaton D(0,7.6). In th mag, thr s a gat (A) wth th grdr shortr than th robot hght and a gat (B) wth th grdr hghr, whr th robot hght s 1.2m n th xprmnt. Thr s also a corrdor nxt to th gats wth th hght that th robot can go through. Th robot has to choos tslf th shortst path from th startng poston S to th dstnaton poston D wthout colldng obstacls.

5 ncssar xtnd th occupd rgons (prsntd b th gr clls) to avod th collson. Th rsult path shows that th robot dos not pass th gat (A) snc t dtcts an obstacl whch s th grdr. It also dos not pass th corrdor (C) as th path s long. Instad, t gos through gat (B) as xpctd. Fgur 6. Th global 3D mag of th nvronmnt Fgur 7 shows th comprssd 2D map obtand b applng th unon (U) oprator to 9494 pxls of th 3D mag. Fgur 8 prsnts th map constructd b th IPaBD algorthm. Fgur 9. Robot navgaton b th algorthm A*. C. Control th robot to trac th tractor Wth th dsgnd tractor, w usd th control law (6) to control th robot to follow th tractor. Th rsult s shown n fgur 10. In th xprmnt, th tangnt vloct of th robot s around 0.2m/s to 0.4m/s. Fgur 7. Th 2D map wth all 3D pxls prssd onto th plan x-. Fgur 10.Th ral tractor of th robot(th rd curv) stcs to th dsgnd tractor(th blu curv). D. Avodng obstacls usng ultrasonc snsors Fgur 8.Th rsult of th obtand map b th IPaBD algorthm. B. Path plannng wth A* algorthm Th xprmnt s mplmntd b sndng an unxpctd obstacl O to th dsgnd tractor. Fgur 11 shows that th robot avods th obstacl and thn contnus to trac th global tractor to arrv at th dstnaton. Fgur 12 s a squnc of mags showng th robot moton durng th xprmnts. From th 2D map, th A* algorthm s appld to fnd th path from th startng poston S to th dstnaton D. As shown n fgur 9, th map s dvdd nto squars wth th sz of 40x40cm 2. Snc th sz of th robot s 80x80cm 2, t s

6 Fgur 11. Avodng an unxpctd obstacl on th tractor of th robot. Fgur 12. A squnc of mags showng th robot moton durng th xprmnts. V. CONCLUSION Ths papr prsnts nw algorthms for th problm of navgaton and obstacl avodanc for th autonomous mobl robot. Th navgaton s dvlopd from th nvronmnt data xtracton to th mappng, path plannng and tractor tracng. Th obstacl avodanc s accomplshd wth both nown and unnown obstacls. Th man contrbuton of th papr s th proposal of th IPaBD mappng algorthm for th path plannng, th tracng control law for th robot sstm wth partal unnown robot paramtrs, and th Improvd- VHF algorthm for th obstacl avodanc. Exprmnts n a ral robot sstm hav bn conductd and th rsult confrmd th ffctvnss of th proposd mthods. ACKNOWLEDGEMENT Ths wor was supportd b Vtnam Natonal Foundaton for Scnc and Tchnolog Dvlopmnt (NAFOSTED). REFERENCES [1] Z-xng Ca, Jn-xa Yu, Zhuo-hua Duan, Xao-bng Zou1Olvr, Brnardo Wagnr, Dsgn of th 3D prcptv sstm basd on lasr scannr for a mobl robot, IJCSNS Intrnatonal Journal of Computr Scnc and Ntwor Scurt, VOL.6 No.3B, March [2] Andrz TpaHartmut, Us of lassr rangrfndr to dtctng n surroundngs of mobl robot th obstacls, Th 25 th Intrnatonal Smposum on Automaton and Robotcs n Constructon, Jun 26-29, [3] T. T. Hoang, D. A. Vt, T. Q. Vnh, A 3D mag captur sstm usng a lasr rang fndr, IEICE Procdng of th 2th ntrnatonal confrnc on Intgratd Crcut Dsgn ICDV, Vtnam, Octobr, [4] P. d la Punt t al.. Extracton of Gomtrcal. Faturs n 3D Envronmnts for Srvc Robotc Applcatons. Sprngr-Vrlag Brln Hdlbrg [5] Johanns Strom, Andrw Rchardson, Edwn Olson. Graph-basd Sgmntaton for Colord 3D Lasr Pont Clouds. Th 2010 [6] R.Murph Robn.: Introducton to AI Rbotcs. Abradford Boo Th MIT Prss Cambrdg, London, England, [7] Aug Wdotratmo,Kum-Sh Hong, and Lafn H. Praudh, Robust stablzaton of a whld vhcl: Hbrd fdbac control dsgn and xprmntal valdaton, Journal of Mchancal Scnc and Tchnolog 24 (2) (2010) 513~520. [8] Thuan Hoang TRAN, Manh Duong PHUNG, Van Tnh NGUYEN and Quang Vnh TRAN, A Path Followng Algorthm for Whld Mobl Robot Usng Extndd Kalman Fltr, IEICE Procdng of th 3th ntrnatonal confrnc on Intgratd Crcut Dsgn ICDV (IEICE 2012), August 2012 Vtnam. [9] Tua Agustnus Tamba, Bongh Hong, and Kum-Sh Hong, A Path Followng Control of an Unmannd Autonomous Forlft Intrnatonal Journal of Control, Automaton, and Sstms (2009) 7(1): [10] Khatb, O., Ral-tm Obstacl Avodanc for Manpulators and Mobl Robots, 1985 IEEE Intrnatonal Confrnc on Robotcs and Automaton, St. Lous, Mssour, March 25-28, 1985, pp [11] usrs.sr.st.utl.pt/~mr/pub/obstaclavodanc.pdf. [12] Y. Korn, Snor Mmbr, IEEE and J. Bornstn, Potntal Fld Mthods and Thr Inhrnt Lmtatons for Mobl Robot Navgaton, Procdngs of th IEEE Confrnc on Robotcs and Automaton, Sacramnto, Calforna, Aprl 7-12, 1991, pp [13] Bornstn, J. and Korn, Y., Th Vctor Fld Hstogram-Fast Obstacl Avodanc for Mobl Robot, IEEE Journal of Robots and Automaton, Vol. 7, No. 3, Jun 1991, pp [14] T. T. Hoang, P. M. Duong, N. T. T. Van, D. A. Vt and T. Q. Vnh, Mult-Snsor Prcptual Sstm for Mobl Robot and Snsor Fusonbasd Localzaton, IEEE Intrnatonal Confrnc on Control, Automaton and Informatcs Scncs (ICCAIS 2012), Dcmbr 2012, HCM, VtNam. [15] T. T. Hoang, P. M. Duong, N. T. T. Van, D. A. Vt and T. Q. Vnh, Dvlopmnt of an EKF-basd Localzaton Algorthm Usng Compass Snsor and LRF, 2012 IEEE12th Intrnatonal Confrnc on Control, Automaton, Robotcs & Vson Guangzhou, Chna, 5-7th Dcmbr 2012 (ICARCV 2012) [16] Crstano Prmbda and Urbano Nuns, Sgmntaton and gomtrc prmtvs xtracton from 2D lasr rang data for mobl robot applcatons. Robótca 2005 Actas do Encontro Cnstsfco Combra, 29 d Abrl d [17] Tran Hp Dnh, Manh Duong Phung, Thuan Hoang Tran, Quang Vnh Tran, Localzaton of a Unccl-l Mobl Robot Usng LRF and Omn-drctonal Camra, 2012 IEEE Intrnatonal Confrnc on Control Sstm, Computng and Engnrng, Nov. 2012, Pnang, Malasa. [18] Y. Kanaama, Y. Kmura, F. Maza and T. Noguch, A stabl tracng control mthod for an autonomous mobl robot, n: Proc. IEEE Intl. Conf. on Robotcs Automaton, 1990, pp