A Comparison of EKF, UKF, FastSLAM2.0, and UKF-based FastSLAM Algorithms

Transcription

1 A Comparison of,, FasSLAM., and -based FasSLAM Algorihms Zeyneb Kur-Yavuz and Sırma Yavuz Compuer Engineering Deparmen, Yildiz Technical Universiy, Isanbul, Turkey Absrac This sudy aims o conribue a comparison of various simulaneous localizaion and mapping (SLAM) algorihms ha have been proposed in lieraure. The performance of Exended Kalman Filer () SLAM, Unscened Kalman Filer () SLAM, -based FasSLAM version., and -based FasSLAM (ufasslam) algorihms are compared in erms of accuracy of sae esimaions for localizaion of a robo and mapping of is environmen. The algorihms were run using he same ype of robo on Player/Sage environmen. The resuls show ha he -based FasSLAM has he bes performance in erms of accuracy of localizaion and mapping. Unlike mos of he previous applicaions of FasSLAM in lieraure, no waypoins are used in his sudy. I. INTRODUCTION The simulaneous localizaion and map building (SLAM) echniques ries o solve he problem where an auonomous vehicle sars in an unknown locaion in an unknown environmen o incremenally build a map of his environmen. The robo uses his map o compue is own locaion simulaneously. Uncerainy is wha makes he SLAM problem hard o solve. The robo can only make noisy, probabilisic observaions of is surroundings wihou knowing is exac locaion. Every ime he robo moves uncerainy is added ino an already uncerain pose. Among he mehods proposed in lieraure o solve he SLAM problem, he mehods based on Bayesian esimaion heory has been he mos successful ones. Many successful applicaions exis in lieraure using o solve nonlinear esimaion problems as posiion racking, localizaion and SLAM [-] problems. Bu he quadraic compuaional complexiy of he makes i difficul o apply in real ime. is a more reliable esimaor han while he sysem model is highly nonlinear. The pas of he is relaively shor compared o. By approximaing he probabiliy densiy funcion insead of he nonlinear funcion iself, SLAM [,] received a considerable aenion. Ye i did no make any improvemen o he compuaional complexiy of he. FasSLAM [3-7] uilizes paricle filers and improves he compuaional complexiy considerably compared o and. based FasSLAM (ufasslam), he newes approach proposed in lieraure [-3], combines he more reliable esimaion abiliy of and reduced compuaional complexiy feaure of FasSLAM. In his sudy, he localizaion and mapping accuracy of,, FasSLAM version. and -based FasSLAM (ufasslam) were compared by using several paricle numbers in FasSLAM and ufasslam o achieve more reliable observaion abou hose algorihms. Furhermore, he effecs of angular velociy noise changing on he roo mean square errors (RMSE) of he robo posiion, obained by hose algorihms, were compared. Beween he second and sixh secion, Bayes Filer,,, FasSLAM. and ufasslam are described, respecively. Secion seven illusraes he experimenal resuls; conclusion is given in he final secion. II. BAYES FILTERS All mehods realized in his sudy are based on Bayes Filers. The sae of he sysem, he sae of he robo and he environmen, a ime is expressed by random variables x. In probabilisic SLAM mehods, he sae of he robo and he environmen can only be expressed hrough he condiional probabiliy disribuions of he sensor daa which are available for he robo. The probabiliy disribuion represening he uncerainy a each poin in ime is called belief, bel ( x ). Bayes filers apply wo updae rules successively o predic he sysem sae. The predicive belief a ime is calculaed jus before he observaion ( z ) and uses he conrol daa ( u : ) by he ime. This sep is also called conrol updae, and expressed as in (). bel x ) P( x z, u ) ( : : The sae esimae given in () is correced according o (), using sensor measuremens ( z : ) and he conrol daa ( u : ) by he ime. This sep is called as measuremen updae or he poserior belief of he sysem sae and calculaed whenever a sensor provides a new observaion. bel( x ) P( x z:,u: ) This research has been suppored by Yıldız Technical Universiy Scienific Research Projecs Coordinaion Deparmen. Projec Number: - --DOP //$3. IEEE 37

2 Z. Kur-Yavuz and S. Yavuz A Comparison of,, FasSLAM., and -based FasSLAM Algorihms The sysem is assumed o be Markov, so he observaions and conrols are condiionally independen of pas measuremens and conrol readings, given he rue sae. The SLAM algorihms presened in his sudy srongly differ in how hey represen hese probabiliy densiies and he belief bel ( x ). These differences of he algorihms are briefly explained in following secions. III. SLAM Kalman filers are he mos widely used varian of Bayes filers. Sandard Kalman filer assumes linear sae ransiions and linear measuremen ransiions wih added Gaussian noise. The exended Kalman filer () overcomes he lineariy assumpion by linearizing he nonlinear sae and measuremen ransiion funcions via Taylor expansion. The nonlinear sae ransiion funcion g and measuremen ransiion funcion h is expressed as in (3) and () respecively wih added noises and. IV. SLAM The unscened Kalman filer () is a direc applicaion of he unscened ransform. Unscened ransform is based on he idea ha i is easier o approximae he probabiliy funcion insead of a nonlinear funcion []. The mehod firs used in SLAM problem by Marinez-Canin and Casellanos [] Insead of approximaing he sae and measuremen ransiion funcions by Taylor series expansion, he i exracs n+ sigma poins, X, from he Gaussian as in (9) and passes hese hrough he nonlinear sae and measuremen funcions. n is he dimension of feaure sae, λ is compued as in () where α and k scaling parameers deermine how far he sigma poins are separaed from he mean. X i X ( i X ( ( n ) ), ( n ) ) i in, for i,...,n for i n,..., n x g ( u, x ) (3) ( n k) n z h( ) x The exended Kalman filer represens beliefs by he mean vecor and he covariance marix Σ a ime. The Taylor expansion of sae ransiion funcion g is given as in (5) and (). The Jacobian G is he value of firs derivaive of g a he poin µ -. ' g( u, x) g( u, ) g ( u, )( x ) g( u, x ) g( u, ) G ( x ) Similarly, he Taylor expansion of measuremen ransiion funcion h is given as in (7) and (). The Jacobian G is he value of firs derivaive of g a he poin µ -. ' h( x ) h( ) h ( )( x ) h( x ) h( ) H ( x ) The updae rules corresponding o he predicion and correcion seps of he Bayes filer and oher deails of he algorihm can be found in [-]. μ is he mean vecor in previous sep, χ[i] is he sigma poins on Caresian coordinae and Σ is he covariance marix of previous sep. Afer he compuaion, each sigma poin is ransformed via he nonlinear sae ransiion funcion g as in (). i ( i y g X ) Then, he mean vecor and he covariance marix are prediced by muliplying he exraced sigma poins and heir weighs. Prediced measuremens are obained by he measuremen ransiion funcion h as given in (). i ( i Z h X ) Poserior esimaion seps of he mean vecor and he covariance marix can be found in [,,]. Unlike, does no employ a linearizaion process via Taylor expansion which causes incomplee represenaion of he nonlinear funcions and does no employ Jacobian marices for calculaing feaure covariance []. V. -BASED FASTSLAM The FasSLAM algorihm uses paricle filers o esimae robo posiion and uses o esimae landmark posiions [, 3-7]. FasSLAM algorihm mainains a se of paricles, each of hese paricles has is own belief regarding posiions of he robo and N landmarks. These beliefs are he local Gaussians and each paricle uses for predicions and updaes of he landmark posiions. Each one of he M paricles in he sysem has he form as given in (3). The noaion [m] is he index of, m T he paricle while x ( x y ) represens paricle s pah 3

3 esimae and m m i, and are he mean vecor and covariance i, marix of he Gaussian represening he ih landmark. m m m m m m m m x,,,...,,,,..., X,, i, i, N,, N, In FasSLAM., he conrol u is used o sample new robo pose for each paricle according o he moion model. FasSLAM. uses he measuremen z and he conrol u ogeher o sample new robo pose, herefore FasSLAM. is more efficien han FasSLAM.. However, is implemenaion is more difficul han he implemenaion of FasSLAM.. Deails can be found in [, 3-7]. If a landmark m n is observed again, measuremen updae equaions are used for updaing he landmark esimaions. FasSLAM linearizes he measuremen model in he same way as does. Temporary paricle se, conaining M paricles, is resampled according o an imporance facor. Sraified resampling is used in his sudy []. and performs only one daa associaion hypohesis over all sae space, however in he FasSLAM each paricle has is own hypohesis for his problem. FasSLAM provides several local soluions for localizaion and mapping by using he paricles. VI. -BASED FASTSLAM There are number of FasSLAM and based paricle filer applicaions[3-3,5]. Generally, hey use he simulaion provided by Bailey e. al.[]. This simulaion assumes an array of waypoins wih known coordinaes and he conrol signals are generaed o direc he robo from one waypoin o anoher. Uilizaion of waypoins prevens hese applicaions o be used in compleely unknown environmens like search and rescue sies. Such waypoins are no used in his sudy. In -based FasSLAM, measuremen updae equaions ake place insead of measuremen updae operaions of FasSLAM.. -based FasSLAM equaions is ou of scope of his paper, he deails can be found in [3] widely. VII. EXPERIMENTAL RESULTS In his secion he ools used for he experimens are explained and he resuls are given in a classified manner. A. Experimenal Seup Algorihms presened in his sudy are coded in C++ o be used on mobile robo developed by our eam. The iniial resuls presened here are obained in Player/Sage environmen [7]. Player is a nework server used for robo conrol, conrol algorihms connec o he server as cliens o send conrol signals o he robo or o receive sensor daa from he robo. Kinemaic models are based on a hree-wheeled robo as shown in Fig.. The proximiy daa are provided by a laser range finder placed on op of he robo and he odomeer daa are provided by a wheel encoder. A simple wall-following mehod as presened in [] is used for exploraion. Figure. Three-wheeled robo model used for kinemaic models. Three differen environmen seup, shown in Fig. were used o evaluae he performance of differen SLAM echniques. (a) (b) (c) Figure. Tes environmens used for mapping In following secions, firsly, he resuling maps for all hree environmens are given. Afer ha, he effec of paricle numbers in FasSLAM. and ufasslam algorihms on he RMS error of robo posiion esimaes under same velociy and noise parameers are analyzed. In he same figure RMSEs of and algorihms are also given. Then, he posiion and orienaion errors of he algorihms according o he loop size (ravelled disance in erms of meers) are given, when he paricle number of FasSLAM. and ufasslam is 5. Finally RMS posiion errors of he algorihms according o changing of angular velociy noise parameer is presened. To obain he resuls, described above, he average RMSE values of each algorihm were acquired by running each of he algorihms imes. B. Maps Generaed by he Algorihms Maps generaed by he,, FasSLAM., and ufasslam algorihms for he firs es environmen are shown in Figs. 3,, 5, and respecively. predics he robo pah more fauly han he oher mehods, as i is examined in following secions oal posiion errors achieved by he algorihm are no he smalles and shows high deviaions from he ground ruh. In he map generaed by, i can be seen ha he accuracy of he map is bigger han he and FasSLAM. (when paricle number is 5). ufasslam gives he bes accuracy resuls for localizaion and mapping. 39

4 Z. Kur-Yavuz and S. Yavuz A Comparison of,, FasSLAM., and -based FasSLAM Algorihms he ufasslam is visibly more accurae han he oher algorihms Predicion True Pah Figure 3. Map of he environmen (a) generaebd by SLAM. Predicion True Pah Figure 7. Map of he environmen (b) generaebd by SLAM Predicion True Pah Predicion True Pah Figure. Map of he environmen (a) generaebd by SLAM. Figure. Map of he environmen (b) generaebd by SLAM Predicion True Pah Predicion True Pah Figure 5. Map of he environmen (a) generaebd by FasSLAM.. Figure 9. Map of he environmen (b) generaebd by FasSLAM Predicion True Pah Predicion True Pah Figure. Map of he environmen (a) generaebd by ufasslam. Figs. 7,, 9, and show he maps generaed for he second es environmen. Resuls show ha he performance of Figure. Map of he environmen (b) generaebd by ufasslam

5 - - - Predicion True Pah Figure. Map of he environmen (c) generaebd by SLAM Predicion True Pah Figure. Map of he environmen (c) generaebd by SLAM. For he hird circular environmen, Figs.,, 3, and show he maps generaed by he algorihms. The map generaed by he ufasslam algorihm is successful visually, han he oher algorihms in circular formed environmen. C. RMS Robo Posiion Errors respec o Paricle Numbers In his secion average RMS errors of esimaed robo posiions are examined. Each algorihm was run imes and average RMSE of posiion was calculaed. Number of paricles was chosen as, 5,, 5,, and 5. Translaional velociy is.3m/sec; sd. dev. of he ranslaional and roaional velociies are.m/sec and.5 rad/sec respecively. RMSE of Robo Posiion Number of Paricles vs. RMS Posiion Error FasSLAM. ufasslam Number of Paricles Figure 5. The effec of he paricle numbers on he RMSE of robo posiions Predicion True Pah Figure 3. Map of he environmen (c) generaebd by FasSLAM.. TABLE I. ESTIMATED ROBOT POSITION RMS ERRORS OF THE ALGORITHMS Algorihm RMSE Algorihm RMSE FasSLAM,.53 ufasslam,. M= M= FasSLAM,.9 ufasslam,.9 M=5 M=5 FasSLAM,.93 ufasslam,.9 M= M= FasSLAM,.5 ufasslam,.3 M=5 M=5 FasSLAM,.3 ufasslam,.5 M= M= FasSLAM,.99 ufasslam,.9 M=5 M= Predicion True Pah Figure. Map of he environmen (c) generaebd by ufasslam. Fig. 5 shows he RMS errors of esimaed robo posiions for all four algorihms. Since he number of paricles (M) affecs he performance of he FasSLAM algorihms, resuls are given for differen number of paricles for hose algorihms. The noise parameers are same for all experimens. The effec of he paricle number on he RMSE is also given in Table I. Table I and Fig. 5 clearly illusrae ha, up o a paricular paricle number, FasSLAM algorihms canno accomplish beer resuls han and. FasSLAM. ouperform and when he paricle number is bigger han 5; however ufasslam ouperform he oher hree algorihms when he paricle number is equal and bigger han 5. Thus he

6 Z. Kur-Yavuz and S. Yavuz A Comparison of,, FasSLAM., and -based FasSLAM Algorihms mos efficien algorihm is ufasslam when he paricle number is chosen appropriaely. D. RMS Robo Posiion and Orienaion Errors During he runime of he algorihms posiion and orienaion esimaion errors wih respec o he ravelled disance (in erms of m.) are given in Figs. and 7 respecively. Esimaed Pos. Err (m) Esimaed Posiion Error (meer) respec o Loop size (m) FasSLAM ufasslam Loop size (m) Sd. of he Angular Noise TABLE II. RMS ERRORS OF POSITION ESTIMATIONS RMSE of Posiion Esimaion FasSLAM (M=5) ufasslam (M=5), , , , , , , , , , Figure. Posiion esimaion errors of four algorihms during he runime. Esimaed Orien. Err (r) Esimaed Orienaion Error (radian) respec o Loop size (m) FasSLAM ufasslam Loop size (m) RMSE of Robo Posiion Esimaion Roaional velociy noise vs. RMSE of Robo Posiion Esimaion FasSLAM ufasslam Figure 7. Orienaion esimaion errors of four algorihms during he runime Figs. and 7 clearly indicae ha he minimum posiion and orienaion esimaion error is achieved by ufasslam, while he paricle number is 5. is he second efficien algorihm, since is posiion and orienaion error is less han and FasSLAM. while he number of paricles is 5. E. RMS Robo Posiion Errors according o Roaional Velociy Noise Parameer The las comparison was made on all four algorihms which were run under differen amouns of roaional velociy noise and he RMS errors of each are given in Table II and Fig.. Table II depics ha; ufasslam is significanly he mos reliable algorihm in case of roaional velociy noise changing. is more reliable and efficien han he and FasSLAM.. is he leas efficien one among hem. Maximum likelihood unknown daa associaion was applied in all of he experimens for each algorihm. The second environmen in Fig. (b), ha is he mos complicaed one, was used in he experimenal resul secions C, D, E Roaional Velociy Noise Sd. Figure. RMS errors of robo posiions under differen roaional noises VIII. CONCLUSION Alhough remains o be a popular choice for he soluion of SLAM problem, as a resul of he experimens in his sudy, -based FasSLAM is observed o be he mos efficien algorihm among sandard,, FasSLAM., and ufasslam. Our resuls also show ha -based FasSLAM can perform as well as SLAM algorihm, while he paricle number is equal and greaer han, or algorihm iself. Theoreical complexiies of he and are same bu in applicaion he selecion and compuaion of he sigma poins caused algorihm o be a slow algorihm compared o he algorihm. ufasslam is he slowes one; however i is he mos accurae one among hem. Also in case of angular velociy noise's increasing, ufasslam gives he leas RMSE value for robo posiion esimaion.

7 REFERENCES [] P. Jensfel, D. Kragic, J. Folkesson, and M. Björkman, "A Framework for Vision Based Bearing Only 3D SLAM", IEEE Inernaional Conference on Roboics and Auomaion (ICRA), pp. 9-95,. [] T. Lemaire and S. Lacroix, (7), "SLAM wih Panoramic Vision", Journal of Field Roboics, Vol., no. -, pp. 9 -, Feb 7. [3] J.-H. Kim, and M.J. Chung, "SLAM wih omni-direcional sereo vision sensor" In Proceedings of he Inernaional Conference on Inelligen Robos and Sysems, Vol. 5, no., pp. 3-3, January,. [] J. Arieda, J. M. Sebasian, P. Campoy, J. F. Correa, I. F. Mondragón, C. Marínez and M. Olivares, "Visual 3-D SLAM from UAVs", Journal of Inelligen and Roboic Sysems, Vol. 55 (-5), pp. 99-3, Aug 9. [5] D. Scaramuzza, R. Siegwar and A. Marinelli, (9), "A Robus Descripor for Tracking Verical Lines in Omnidirecional Images and Is Use in Mobile Roboics", Inernaional Journal of Roboics Research, Vol., no., pp. 9-7, Feb 9. [] J. Sola, A. Monin, M. Devy and T. Vidal-Calleja, Fusing Monocular Informaion in Mulicamera SLAM, IEEE Transacions on Roboics, Vol., no. 5, pp. 95-9,. [7] A.P. Gee, D. Chekhlov, A. Calway and W. Mayol-Cuevas, "Discovering Higher Level Srucure in Visual SLAM", IEEE Transacions on Roboics, Vol., no. 5, pp. 9 99, Ocober. [] S. Thrun, W. Burgard and D. Fox, Probabilisic Roboics, The MIT Press, 5. [9] P. Abbeel, A. Coaes, M. Monemerlo, A. Ng, and S. Thrun, "Discriminaive raining of Kalman filers". In Proceedings of Roboics Science and Sysems, Cambridge, MA, 5. MIT Press. [] Q.-H. Meng, Y.-C. Sun, and Z.-L. Cao, Adapive Exended Kalman Filer (A)-based Mobile Robo Localizaion Using Sonar, Journal of Roboica, Vol (5), pp , Sepember. [] S. J. Julier and J. K. Uhlmann, Unscened filering and nonlinear esimaion, Proceedings of he IEEE, Vol. 9, No. 3, March, pp-. [] R. Marinez-Canin and J. A. Casellanos, Unscened slam for largescale oudoor environmens, IEEE/RSJ Inl. Conf. On Inelligen Robos and Sysems, 5, pp [3] M. Monemerlo, S. Thrun, D. Koller, and B. Wegbrei, "FasSLAM: A Facored Soluion o he Simulaneous Localizaion and Mapping Problem", In he Proceedings of he AAAI Naional Conference of Arificial Inelligence, Canada,. [] C. Sachniss, D. Hahnel, and W. Burgard, W., Exploraion wih acive loop-closing for FasSLAM, Proceedings of Inernaional Conference on Inelligen Robos and Sysems (IROS), pp. 55-5, Oc. [5] J. Z. Sasiadek, A. Monjazeb and D. Necsulescu, Navigaion of an auonomous mobile robo using -SLAM and FasSLAM, h Medierranean Conference on Conrol and Auomaion, pp. 57-5, 5-7 June, Ajaccio, France. [] M. Monemerlo, and S. Thrun, Simulaneous localizaion and mapping wih unknown daa associaion using FasSLAM, in he Proceedings of he 3 IEEE Inernaional Conference on Roboics & Auomaion (ICRA), pp , -9 Sepember 3, Taipei, Taiwan. [7] T. Bailey, J. Nieo and E. Nebo, Consisency of he FasSLAM algorihm, IEEE Inernaional Conference on Roboics and Auomaion (ICRA), pp. - 9, 5-9 May, Orlando, FL. [] C. Kim, R. Sakhivel and W. K. Chung, Unscened FasSLAM: A Robus and Efficien Soluion o he SLAM Problem, IEEE Transacions on Roboics, Vol., no., Augus, pp. -. [9] M. Cugliari and F. Marinelli, A FasSLAM algorihm based on he Unscened Filering wih Adapive Selecive Resampling, h Inernaional Conference on Field and Service Roboics (FSR 7), France 7. [] X. Wang and H. Zhang, A UPF- Framework for SLAM, IEEE Inernaional Conference on Roboics and Auomaion - ICRA 7, pp. -9, April 7, Rome, Ialy. [] L. Zhang, X. Meng and Y. Chen, "Unscened Transform for SLAM Using Gaussian Mixure Model wih Paricle Filer", Inernaional Conference on Elecronic Compuer Technology, pp. -7, - Feb. 9, Macau. [] S. Zandara and A. Nicholson, "Square Roo Unscened Paricle Filering for Grid Mapping", in he Proceedings of he nd Ausralasian Join Conference on Advances in Arificial Inelligence, 9. [3] A. Sakai, T. Saioh and Y. Kuroda, "Robus Landmark Esimaion and Unscened Paricle Sampling for SLAM in Dynamic Oudoor Environmen", Journal of Roboics and Mecharonics, pp. -9, Vol. No. Apr.. [] G. Kiagawa. Mone-Carlo filer and smooher for non-gaussian nonlinear sae space models, J. Compu. Graph.Sais., : 5, 99. [5] R. Merwe, A. Douce, N. Freias and E. Wan, The Unscened Paricle Filer, Technical Repor CUED/F-INFENG/TR 3, Cambridge Universiy Engineering Deparmen,. [] hp://www-personal.acfr.usyd.edu.au/bailey/sofware/index.hml [7] B. Gerkey, R. T. Vaughan and A. Howard. "The Player/Sage projec: ools for muli-robo and disribued sensor sysems". In Proceedings of he h Inernaional Conference on Advanced Roboics (ICAR 3), pp , June 3, Coimbra, Porugal. [] K. Beevers, Loop closing in opological maps, Proceedings of he Inernaional Conference on Roboics and Auomaion, pp , April 5. 3