Hybrid localization approach of a bi-steerable mobile robot based on grids matching and extended Kalman filter

Transcription

1 Hybrd localzaton approach of a b-steerable moble robot based on grds matchng and extended Kalman flter S Bourane, A O Djekoune and O Azouaou Abstract Ths paper presents a moble robot self localzaton method used to determne the poston of the moble robot obucar The localzaton approach s based on usng both grds matchng method and Extended Kalman lter (EK) method The grds matchng method provdes accurate results but requres a large computatonal tme that s why the EK s ntroduced EK fuses odometrc data and laser data to estmate the robot poston The developed algorthms are mplemented and tested on the moble robot obucar Keywords Mobles obots, Localzaton, grds matchng, Extended Kalman lter and Certanty grd T I INTODUCTION HE development of outdoor vehcles able to overcome the problems of congeston, polluton and safety, and that operate n complex and dynamc envronment, has receved tremendous nterest from robotcs n recent years [1,2] These vehcles must use advanced control technques for autonomous navgaton, whch requre a robust localzaton algorthm The localzaton s the process of determnng the poston and orentaton of the moble robot wthn the operatng envronment In ths context, several researches have been acheved Some works as n [3] are based on dead-reckonng usng pure odometry, where the robot postons are calculated relatvely to the ntal poston But, t s known that ths approach s not a very robust localzaton for robots that cover long dstances Ths s because errors n odometry accumulate over tme Other works use absolute localzaton approach that s ndependent of the ntal poston and relay on the robot envronment, lke the localzaton by beacons [4], ths uses artfcal or natural beacons to locate the robot Some other approaches are not constrant to land marks of the envronment, and use the robot s sensors (ultrasonc, laser, camera, GPS ) to buld t, such as the grds matchng method [5] Ths method searches the best correspondence between a local map representng the part of envronment percevable snce the robot poston, and the global map Manuscrpt receved July 25, 2008 S Bourane s wth the centre de Développement des Technologes Avancées, Lotssement 20 Août 1956, BP 17, Baba Hassen, Alger, Algére (Phone: 213 (21) ; fax 213 (21) ; e-mal: s_bourane@yahoofr) O Djekoune s wth the centre de Développement des Technologes Avancées, Lotssement 20 Août 1956, BP 17, Baba Hassen, Alger, Algére (Phone: 213 (21) ; fax 213 (21) ; e-mal: djekoune@yahoofr) O Azouaou s wth the centre de Développement des Technologes Avancées, Lotssement 20 Août 1956, BP 17, Baba Hassen, Alger, Algére (Phone: 213 (21) ; fax 213 (21) ; e-mal: azouaou@hotmalfr) representng the envronment where the robot moves, to estmate the real poston of the robot Generally, to buld the envronment, dfferent approaches can be used; for nstance occupancy grds method [6], geometrc method [7], and topologcal method [8] The grds matchng method provdes accurate result comparng to the relatve approaches, but ts major nconvenent s the computng tme that t requres Ths s why; a hybrd approach that combnes relatve and absolute methods s more adapted to be used, t takes the advantages of the two methods Some works use global methods lke multple hypotheses trackng method [9], Markov method [10] or more accurate lke the partcle flter method [11] to localze the robot n a pror or prevously learned map and gven no other nformaton that the robot s somewhere n ths map But as a drawback, t can be dffcult to regulate and very expansve n tme of calculaton, for ths rasons a local approach s more approprate to be used Lke n [12], where an extended Kalman flter s employed; t combnes odometrc measurement and GPS measurement to estmate the robot poston Ths sensor s lmted n urban envronment covered wth buldngs Other works use ultrasonc sensor [13] that are less accurate, or laser sensor as n [14], where the robot fuses odometrc postons and postons calculated from the laser measurement The robot uses the laser measurement to detect corners of an ndoor envronment and uses ths nformaton to calculate the robot postons But generally n outdoor envronment, beacons are used to calculate these postons, lke n [15] These technques requre hard equpment, and restrct the robot dsplacement n a predefned area To overcome these constrants (relatvely to our applcaton that requres accuracy and real tme mplementaton), a grds matchng method combned wth extended Kalman flter s proposed n ths paper to localze the moble robot n an outdoor envronment The grds matchng approach requres an envronment map Generally, a geometrc model s more adapted where usng EK algorthm [16] But n our case, the certanty grds method s used to model the envronment, because of ts mplementaton smplcty, and ts adaptaton to dfferent forms of envronments The paper s organzed as follows: Secton 2 brefly descrbes the knematcs model of the moble robot obucar Secton 3 exposes the localzaton approach based on the grds matchng algorthm and the extended Kalman flter algorthm Secton 4 provdes detals about the expermental results mplemented on the obucar

2 II THE KINEMATICS MODEL The moble robot obucar s a b-steerable moble robot (fgure 1) Both ts front and rear wheels are orentable, such that the rear wheels steerng angleφ, s a functon of the front steerng angle ( φ = kφ ) (where 18 < φ < 18 ) III HYBID LOCALIZATION BASED ON GIDS MATCHING METHOD AND ETENDED KALMAN ILTE To localze the moble robot n ts envronment, as a frst step the robot explores ths envronment and rebulds t by means of certanty grd algorthm based on laser measurement Then, the rebult map s used n the grds matchng method to determne the measure nally, to estmate the robot poston, EK fuses ths measure wth odometrc postons (fgure 3) Laser Map buldng Grds matchng Measure g 1The moble robot obucar Conventonally, the robot poston s gven by the vector ( x, y, ϕ), where ( x, y) are ether the gravty center coordnates (the mddle pont of the rear axle (pont )), and ϕ s the headng of the robot (e the orentaton) (fgure 2) or non-holonomc modellng purposes, the steerng control φ (alt kφ ) s assumed to be appled to an magnary wheel placed at pont (alt ) so that the lnear velocty vector assocated to that pont v (alt v ) s collnear to the orentaton of ths wheel Y y O Y CI V φ ϕ kφ V x g2 obucar knematcs model The knematcs model of the robot, assumng wheels rollng wthout slppage, for a reference frame located at the mdpont of the rear axle, s gven by the followng equatons: x = v sn( ϕ kφ) y = v cos( ϕ kφ) (1) sn( φ + kφ) ϕ = v = Ω L cosφ Where; L s the length between the front and rear wheels axels, and k a constant ( 0 <= k <= 1) In ths paper we take k = 1; n order to enhance the maneuverablty of the robot by rasng the gyraton radus lmt The steerng angles become: φ = φ and φ = kφ L Odometers Odometrc poston g3 Localzaton approach EK obot poston A Grds matchng method 1) Map buldng In order to create a scene map from laser range measurements, the envronment must frst be scanned The moble robot obucar s equpped wth a sck laser range fnder lms-200 [17] that s mounted at forty centmetres from the floor on front of the robot It provdes dstance measurements over a 180 degree area, and ts scope can reach eght meters usng the mllmetre mode and eghty meters usng the centmetre mode The map buldng algorthm used to model the envronment s based on certanty grd [5] It uses a two dmensonal Cartesan hstogram grd for envronment representaton Each cell n the hstogram grd holds a Certanty Value CV, representng that ths cell s occuped or not by an obstacle Snce the laser s drectve and dvded nto 180 drectons, the measurement s nterpreted as the dstance to the nearest obstacle for each drecton To determne the cells correspondng to obstacles postons, frst the poston of the obstacle s calculated relatvely to the moble reference (related to the moble robot) accordng to the laser measurement (dstance and drecton)) Then, usng the relatonshp between the moble reference and the unverse reference (related to the envronment) where the poston of each cell s known, the poston of obstacle relatvely to the unverse reference s deduced to fnd the cell occuped by ths obstacle Cell contanng the value: C L =C L +1 Cell contanng the value: C L =C L -1 Cell contanng the value: C L =0 g4 Laser measurement model The CVs of cells correspondng to the detected obstacles are ncreased by 1 untl a predefned maxmum value s reached, and the CVs of cells correspondng to the free space

3 (between the laser and the obstacle) are decreased by 1 untl a predefned mnmum value s reached The CVs of the remanng cells are not modfed Intally, the hstogram grd world model s set to zero value (fgure 4) Ths s appled for each drecton α (0-180 degree) of the laser 2) Grds matchng prncple Ths approach requres a global grd and a local grd The global grd descrbes the envronment of the robot, and the local grd represents the part of envronment perceved snce the robot poston In ths paper the envronment used s partally known, where the global grd s updated durng the robot dsplacement θ b l u J a 0 L I k g5 Grds matchng Local Grd Global Grd The am of grds matchng s to fnd the robot poston and orentaton n ts envronment [5] Based on a predcted poston, the poston of the robot s calculated by fndng the locaton of the local grd n the global grd, for whch there are most matched cells wth the same type (occuped or free) (fgure 5) The sum product of all matched cells s calculated, whch represents the matchng crteron The robot poston s provded by a, b and θ (wth ( a, b, θ) poston of the local grd n the global grd) for whch the sum S(a,b,θ ) s maxmal, such that : S( a, b, θ ) = C ( I, J) C ( k, l) (2) L I, J loca lg rd C L (I,J) are local grd cells values and C G (k,l) are global grd cells values To determne a, b andθ values, (k,l) coordnates (of global grd) must be expressed accordng to (I,J) coordnates (of local grd), such that : k = a + ( I 0 )cosθ ( J + j0 ) snθ (3) l = b + I )snθ + ( J + ) cosθ ( 0 j0 Wth: ( 0, 0) are ndexes of local grd cell ( o centred) contanng local reference ( L ) orgn k and l are ndexes of the global grd cell, that s covered by the local grd cell wth ndexes I, J Durng the matchng step, the only nformaton about the moble robot obucar s poston s that provded by the odometrc system It s a predcted poston expressed by the vector ( xr, yr, θ r) The matchng research s acheved n an area fxed expermentally at ± 2,5m around the predcted poston and ± 45 around the predcted G orentaton These values are chosen n a way that the research area contans the actual poston of the robot To obtan a precse procedure, the sze of the grd cell s set to 10x10cm, and the research area s translated by a step of one cell n translaton and 5 n orentaton The grds matchng localzaton provdes accurate postons close to the real postons, but ts algorthm requres a lot of computng tme Then for real-tme applcatons, ths s a great drawback or ths reason, an extended Kalman flter s used to optmze the grds matchng algorthm and employed ts results for estmatng the robot poston B Extended Kalman flter method Extended Kalman flter (EK) s useful for estmatng the state vector, that represents the robot poston ( x, y, ϕ ) at the step [18]-[20] Its algorthm s gven n fgure 6 The robot s state evolves to the followng system of nonlnear stochastc dfference equatons: x = x δ d sn( ϕ kφ + δϕ / 2) + Wx 1 1 y = y + δ d cos( ϕ kφ + δϕ / 2) + Wy (4) 1 1 ϕ = ϕ + δϕ + Wϕ 1 δd andδϕ are the nternally sensed translaton and rotaton n the nterval between tmes and -1 These varables are expressed accordng to the odometrc data ( v, φ) at each samplng perod T, such as: δ d = Tv (5) v sn( φ + kφ) δϕ = T (6) L cosφ W=N(0 ;Q) represents normally dstrbuted nose n the state evoluton process, wth zero mean and covarance Q These equatons can be summarzed as follows: = f ( 1, U, W ) (7) Where: U = ( v, φ) s the vector obtaned from the odometers for the perod from tme -1 to tme Ths odometrc model represents the model system Laser measures are used to determne the measurement error between the current state and the nternally computed state of equaton (7) The measurement process s modelled as follows: m = h(, V ) (8) Where m s a poston ( x, y, ϕ), representng the measurement of the actual state at tme (the measure) V=N(0 ;) s the measurement nose vector wth zero mean and covarance, then : m = h(,0) (9) Equaton (9) can take the followng matrx form: xk xk yk = y (10) k ϕ k ϕk

4 rom equaton (9), we dstngush that the functon h s lnear Therefore, we haven t to calculate the jacoban of the matrx H, and then the measure takes the followng form: m = H (11) The EK combnes odmetrc data (model system) and laser data (measurement system) to estmate the robot s poston Ths poston s calculated from the predcton and update (measurement) steps The equatons for the predcton step at tme are: = f ( 1, U,0) (12) T P = AP 1 A + Q (13) stands for the predcton of the state vector for tme, gven obucar odometers data and knowledge of the state at tme -1 P s a pror estmate of the error covarance, e the covarance of the predcted state Durng the frst step, the ntal poston 0 s known, and ts covarance P 0 s chosen so large to cover all possble errors, snce ts value wll decrease wth the convergence of the flter A s the jacoban matrx It s calculated n order to lnearze the system model (evoluton model), and then lnearze the functon f, where: δ A = f = (14) δ 1 0 δd cos( ϕ kφ + δϕ / 2) Then: 1 (15) A = 0 1 δd sn( ϕ kφ + δϕ / 2) The measure m s calculated from the grds matchng algorthm (descrbed n secton III2) Where the EK parameters and P are used to determne the estmated poston and the research area n wtch the research s acheved, that are requred by ths algorthm, nstead of usng odometrc poston and a predefned research area Therefore, the predcted poston s expressed by the matrx and the dmenson of the research area s 2 determned by the covarance P The covarance σ ( P ) represents the nterval of error where the predcted poston s located Thus, t s sure that the real poston s located wthn a dsk of radus 2 σ around the predcted poston Then, wth convergence of the flter P value decrease, so the research area dmenson decreases too nally, the Kalman flter gan K s computed and used to correct the state and ts covarance P as follows: T T 1 K = P H ( HP H + ) (16) + K ( m H ) (17) 1 = = P K HP P (18) K s the wegh allocated to the state and error covarance correcton s the fnal estmated poston wth covarance P The covarance value must converge to a best result as the localzaton cycle runs In equaton (17), expresson combnes measure predcton, where ( m H ) s the nnovaton NO Begn Acquston of the global grd Intalzaton of EK (H, Q,, 0 and P 0) Predcton (, P ) nal estmaton (, P ) Destnaton reached? Yes Measure ( m ) (Grds matchng algorthm) End g6 EK algorthm IV EPEIMENTAL ESULTS m and The proposed algorthms are mplemented and tested on the moble robot obucar The objectve of our approach s to localze the moble robot obucar n a partally known envronment The robot s equpped of an odometrc system, however because of accumulatve errors a robust localzaton method s requred, frst the grds matchng method s mplemented to overcome ths drawback But ths method s expansve n tme of calculaton; ths s why the EK s used n the localzaton process The EK algorthm fuses odometrc data and grds matchng algorthm results to estmate the robot poston The grds matchng algorthm requres an envronment map, so the obucar explores the envronment and collects nformaton to buld t (ths envronment s updated durng the localzaton process) The robot makes correspondence between the local grd representng the part of the envronment perceved snce ts poston and the global grd representng the envronment where t moves, n order to determne the poston where t s As a frst experment, the robot moves along a straght path (fgure 7) In ths test, an error n the ntal poston of the robot s ntroduced Consequently, all the odometrc postons are calculated relatng to ths poston And then, the trajectory s shfted from the true path (fgure 7a) But by the grds matchng, ths poston s corrected In ths case the robot poston

5 corresponds to the best matchng (fgure 7b) The results confrm the accuracy of the method whch corrects odometrc s accumulatve errors Another example for a curve path s gven n fgure 8 wth the same condton and yelds to the same results g7 Grds matchng results; frst experment (straght path): postons of the moble robot obucar provded by odometers, postons of the moble robot obucar provded by grds matchng algorthm (red ponts represent robot poston) g8 Grds matchng results, second experment (curve path): postons of the moble robot obucar provded by odometers, postons of the moble robot obucar provded by grds matchng algorthm (red ponts represent robot poston) The grds matchng method s accurate dependng on the research area sze Indeed, the real poston can be near the estmated one and wth a large area sze, n ths case the research process wll take a lot of tme to proceed In another way, ths poston can be far and wth a small area sze, the measurement can be mssed So, the area sze must be chosen carefully to gan tme and avod mssng nformaton, as a concluson, the optmal sze of research area can t be predcted n advance To optmze the grds matchng algorthm, an extended Kalman flter s ntroduced n the localzaton process Ths leads to a hybrd approach that uses the two methods to determne the robot poston or the hybrd approach mplementaton, the same test condtons as the grds matchng mplementaton are used Accordng to fgure 9 and fgure 10, EK explots accuracy of grds matchng algorthm to correct odometrc s accumulatve errors Where, the grds matchng algorthm takes postons correspondng to the best matchng Moreover, wth an error on the ntal poston, EK doesn t count on odometrc data and corrects ths error At the begnnng of the path, the postons are naccurate, but durng robot s dsplacement the accuracy ncreases and the flter converges To better show the dfference between postons provded by EK, odometers and measure, these postons are represented followng x, y andϕ coordnates, accordng to the number of sample (fgure 11) or nstance, the trajectory of fgure 10 s taken, where the robot moves along a curve path or the poston x, odometrc curve s shfted from the two other curves (EK and measure), whle havng the same general shape Ths means that there s an error on the ntal odometrc poston, and the entre robot trajectory s calculated n relaton wth t Thus EK uses the measure (calculated from grds matchng algorthm) to correct ths error It s the same case for the orentaton ϕ but wth a smallest error or the poston y, the three curves have the same ntal poston wth no error and the three curves are almost superposed, whch means that there s no error As t s shown n gure 11, the EK output has almost the same general shape as the measure s curve, but t s more stable wth oscllatons havng smallest ampltude Ths confrms that EK s more accurate than grds matchng And thus EK ncreases ts accuracy The mplementaton results show that the extended Kalman flter s much faster than grds matchng algorthm As an example, for grds matchng algorthm wth research area sze equals 5x5m, the EK s forty tmes faster EK tself doesn t requre a lot of computng tme, and then usng grds matchng algorthm to determne the measure, EK optmzes ts research area dmenson whch decreases wth convergence of the algorthm, and then ncreases ts accuracy (c) g9 Extended Kalman flter results, thrst experment (straght path): postons of the moble robot obucar provded by odometers, postons of the moble robot obucar provded by grds matchng algorthm (measure), (c) postons of the moble robot obucar provded by EK algorthm (red ponts represent robot poston)

6 our approach, the percepton system can be enrched by usng the obucar s camera, whch provdes more nformaton about the envronment where the robot moves and complements laser measurements (c) g10extended Kalman flter results, fourth experment (curve path): postons of the moble robot obucar provded by odometers, postons of the moble robot obucar provded by grds matchng algorthm (measure), (c) postons of the moble robot obucar provded by EK algorthm(red ponts represent robot poston) EK postons The measure Odometrc postons (c) g11 Comparson between postons provded by odometers, EK and measure: Postons x, Postons y, (c) orentatonϕ V CONCLUSION In ths paper, a robust localzaton algorthm to determne the robot poston n ts envronment s developed Ths approach s based on usng both grds matchng and extended Kalman flter methods rst, the grds matchng method s mplemented; where matchng between local grd and global grd s acheved for all possble translaton and rotaton between the two grds The best matchng s evaluated to determne the robot s poston After mplementaton of ths method, the results are satsfactory n term of accuracy, but the man problem s the computng tme that depends on the research area sze Indeed, when the research area sze ncreases, the accuracy ncreases, but the computng tme ncreases too As a soluton, an EK algorthm s developed that fuses odometrc data (system model) and laser data (measure) to estmate the robot s poston To determne the measure, the EK uses the grds matchng algorthm, where he estmates the sze of the research area and the poston around whch the research s acheved The mplementaton of EK algorthm reveals that EK corrects odometrc errors and optmzes the grds matchng method Ths makes t very sutable for real-tme applcaton To ncrease accuracy of EEENCES [1] S Pett, T rachard, «Safe navgaton of a car-lke robot wthn a dynamc envronment», Sprnger Tracts n Advanced obotcs Seres, Sprnger, 2007, vol 35 [2] B Wagner, ELOB 2006, Techncal paper, Unversty of Hannover, May 2006 [3] J Borensten, «Internal Correcton of Dead-reckonng Errors Wth the Complant Lnkage Vehcle», Journal of obotc Systems, Vol 12, No 4, pp , Unversty of Mchgan, Ann Arbor, USA, Aprl 1995 [4] EBrassart, «Localsaton absolue d un robot moble autonome par des balses actves et un système de vson monoculare», Thèse de l unversté de Compègne, rance, Janver 1995 [5] A Courcelle, «Localsaton d un robot moble : Applcaton à l ade à la moblté des personnes handcapées moteur», Doctorat de l unversté de METZ, rance, Janver 2000 [6] A Elfes, «Usng Occupancy Grds for Moble obot Percepton and Navgaton», IEEE Computer, pp46-57, Los Alamtos, USA, 1989 [7] JL Crowley, «World modellng and poston estmaton for a moble robot usng ultrasonc rangng», Proceedngs of the IEEE Internatonal Conference on obotcs and Automaton 1998 [8] G Dedeoglu, M Matarc, and G S Sukhatme, «Incremental onlne topologcal map buldng wth a moble robot», In Proceedngs of Moble obots IV - SPIE, pages , 1999 [9] [16] P Jensfelt and S Krstensen, Actve global localsaton for a moble robot usng multple hypotheses trackng, IEEE Trans on obotcs and Automaton, 17(5):748760, October 2001 [10] Song, Probablstc Localzaton Methods for a Moble obot Navgaton, Systems Desgn Engneerng, Unversty of Waterloo, Waterloo, ON May 13, 2002 [11] Sebastan, Thrun, Deter ox, Wolfram Burgard, and rank Dellaert, obust Monte Carlo Localzaton for Moble obots, School of Computer Scence Carnege Mellon Unversty, Pttsburgh, PA Aprl 2000, CMU-CS [12] Thrapp, C Westbrook et D Subramanan, «obust localzaton algorthms for an autonomous campus tour gude», Proceedngs of IEEE Internatonal Conference on obotcs and Automaton (ICA 01), USA, 2001, pp vol2 [13] G Lawtzky, W eten, M Moller, «Sonar sensng for low-cost ndoor moblty», obotcs and Autonomous Systems, vol 14, pp , 1995 [14] L Wang, L Yong, «Moble obot Localzaton for Indoor Envronment», SIMTech Techncal eport, Sngapore Insttute of Manufacturng Technology, Mechatroncs Group, 2002 [15] JM Armngol, L Moreno, A de la Escalera and MA Salchs "Landmark percepton plannng for moble robot localzaton" IEEE Internatonal Conference on obotcs and Automaton,ICA 98, Leuven, Belgque, ma 1998 [16] C Drocourt, «Localsaton et modélsaton de l'envronnement d'un robot moble par coopératon de deux capteurs omndrectonnels», thèse, Unversté de Technologe de Compègne Centre de obotque, d Electrotechnque et d Automatque, 2002 [17] www Sckoptccom [18] Thrapp, C Westbrook et D Subramanan, «obust localzaton algorthms for an autonomous campus tour gude», Proceedngs of IEEE Internatonal Conference on obotcs and Automaton (ICA 01), USA, 2001, pp vol2 [19] Madhavan, K regene and L E Parker, «Destrbuted heterogeneous outdoor mult-robot localzaton», Proceedngs of IEEE Internatonal Conference on obotcs and Automaton (ICA 02), USA, 2002, pp vol1 [20] P Bonnfat, «Localsaton précse en poston et atttude des robots mobles d'extéreur à évolutons lentes», thèse, Ecole Doctorale Scence pour l ngéneur de Nantes, Spécalté : Automatque et Informatque Applquée, le 24 Novembre 1997