Look-ahead Proposals for Robust Grid-based SLAM

Transcription

1 Look-ahead Proposals for Robus Grid-based SLAM Slawomir Grzonka, Chrisian Plagemann, Giorgio Grisei, Wolfram Burgard To cie his version: Slawomir Grzonka, Chrisian Plagemann, Giorgio Grisei, Wolfram Burgard. Look-ahead Proposals for Robus Grid-based SLAM. 6h Inernaional Conference on Field and Service Roboics - FSR 2007, Jul 2007, Chamonix, France. Springer, 42, 2007, Springer Tracs in Advanced Roboics. <inria > HAL Id: inria hps://hal.inria.fr/inria Submied on 24 Apr 2008 HAL is a muli-disciplinary open access archive for he deposi and disseminaion of scienific research documens, wheher hey are published or no. The documens may come from eaching and research insiuions in France or abroad, or from public or privae research ceners. L archive ouvere pluridisciplinaire HAL, es desinée au dépô e à la diffusion de documens scienifiques de niveau recherche, publiés ou non, émanan des éablissemens d enseignemen e de recherche français ou érangers, des laboraoires publics ou privés.

2 Auhor manuscrip, published in "6h Inernaional Conference on Field and Service Roboics - FSR 2007, Chamonix : France (2007)" Look-ahead Proposals for Robus Grid-based SLAM Slawomir Grzonka, Chrisian Plagemann, Giorgio Grisei, and Wolfram Burgard Universiy of Freiburg Deparmen of Compuer Science Georges-Koehler-Allee, Freiburg, Germany {grzonka,plagem,grisei,burgard}@informaik.uni-freiburg.de Summary. Simulaneous Localizaion and Mapping (SLAM) is one of he classical problems in mobile roboics. The ask is o build a map of he environmen using on-board sensors while a he same ime localizing he robo relaive o his map. Rao-Blackwellized paricle filers have emerged as a powerful echnique for solving he SLAM problem in a wide variey of environmens. I is a well-known fac for sampling-based approaches ha he choice of he proposal disribuion grealy influences he robusness and efficiency achievable by he algorihm. In his paper, we presen a significanly improved proposal disribuion for grid-based SLAM, which uilizes whole sequences of sensor measuremens raher han only he mos recen one. We have implemened our sysem on a real robo and evaluaed is performance on sandard daa ses as well as in hard oudoor seings wih few and ambiguous feaures. Our approach improves he localizaion accuracy and he map qualiy. A he same ime, i subsanially reduces he risk of mapping failures. 1 Inroducion The abiliy o consruc models of naural and human-build environmens is widely regarded as a precondiion for auonomous service robos. Such models are required for basic asks such as localizaion and moion planning. The combinaion of a Rao-Blackwellized paricle filer (RBPF) wih occupancy grid maps represens an effecive and flexible soluion o he SLAM problem as i only makes mild assumpions abou he srucure of he environmen. Theoreically, given infiniely many paricles, RBPFs always converge o he correc map. In pracice however, only a finie number of paricles can be used. This number inherenly limis he uncerainy which can be represened by he filer and in his way can lead o divergence. Consider, for example, he environmen depiced in he upper picure of Figure 1. This environmen consiss of wo box-like landmarks, which canno be perceived a he same ime due o a limied sensor range. The geomeric shape of hese boxes needs o be represened accuraely in he map as oherwise he esimae abou he orienaion of he robo relaive o he landmark quickly ges los. When he robo drives around a box several imes he sandard RBPF mapping approach ypically urns he squared shape ino a circle, since sligh pose errors are inroduced by he limied number of paricles and by he subopimal proposal disribuion used. As a consequence,

3 2 Slawomir Grzonka, Chrisian Plagemann, Giorgio Grisei, and Wolfram Burgard Box 2 Box 1 Box 2 Box 1 Box 2 Box 1 Box 1 Fig. 1. A mobile robo wih a limied sensor range navigaes hrough he low srucured environmen depiced in he upper image, which consiss of wo cardboard boxes placed on fla, open errain. The robo sars a Box 1, moves o Box 2, and revolves around i several imes before reurning o is origin. Sandard mapping approaches canno deal wih he large amoun of pose uncerainy build up when circling around he second box, which resuls in a seriously diverged map (lower righ). In comparison, our approach using k-sep look-ahead proposals reains he squared shape of he boxes and yields an accurae map (lower lef). Noe, ha due o he limied laser range he hedge was visible only a Box 1. he paricle filer loses rack of he heading of he robo and yields seriously wrong maps like he one depiced in he lower righ diagram of Figure 1. In his paper, we presen he so-called look-ahead mapping approach which uses improved proposals for he robo pose o increase he accuracy of he paricles afer he proposal sep. This is achieved by performing independen, k-sep localizaion runs for he individual paricles o beer align hem o heir local maps. In his way, he filer can deal wih higher levels of noise and operae in less srucured environmens. Experimens wih real robos demonsrae ha our approach can handle siuaions in which sae-of-he-ar RBPF-based approaches, like he one using a moion model based on he odomery error [10] or laser scan-maching error [5] or he one using dynamically adaped proposal disribuions based on local laser scan-maching [4] fail.

4 Look-ahead Proposals for Robus Grid-based SLAM 3 2 Relaed Work Originally, Murphy e al. [1, 10] have inroduced Rao-Blackwellized paricle filers as an effecive means o solve he SLAM problem. In a RBPF, each paricle represens a possible robo rajecory and a map. The framework has been subsequenly exended by Monemerlo e al. [8, 9] for SLAM wih discree ses of landmarks. To efficienly learn accurae grid maps, Eliazar and Parr [3] described an efficien map represenaion. Addiionally, Hähnel e al. [5] proposed an improved moion model ha subsanially reduces he number of paricles required. Recenly, Howard presened an exension owards muli-robo sysems [6] in which he describes how o effecively merge he informaion obained by differen robos. Furhermore, Grisei e al. [4] proposed an exension of he approach by Hähnel e al. [5]. Insead of using a fixed proposal disribuion, heir algorihm compues an improved proposal disribuion on a per-paricle basis, in a way similar o FasSLAM-2, as presened by Monemerlo e al. [7] for he case of landmark-based mapping. All of he above menioned approaches perform mapping and localizaion as he daa is available, ha is, he pose and he map uncerainy resul from he very same se of observaions processed. In he work presened here, we use k fuure measuremens o build up a look-ahead proposal disribuion for he robo pose. In his way, he uncerainy in he robo pose is reduced and consequenly, less map uncerainy has o be represened by he filer. 3 Mapping wih Rao-Blackwellized Paricle Filers The key idea of he Rao-Blackwellized paricle filer for SLAM is o reason abou possible robo rajecories and he corresponding maps using a sample-based represenaion [5]. Formally, he ask is o esimae he join poserior p(x 1:,m z 1:,u 1: ) of he map m and he rajecory x 1: = x 1,...,x of he robo, given observaions z 1: = z 1,...,z and odomery measuremens u 1: = u 1,...,u. In he paricle filer framework, he poserior afer each ime sep is represened by a se of weighed rajecories x [j] 1: and he corresponding maps m[j] generaed from hese rajecories. Using he facorizaion p(x 1:,m z 1:,u 1: ) = p(m x 1:,z 1: ) p(x 1: z 1:,u 1: ), (1) we can derive a recursive filer ha in each ieraion updaes he rajecory samples x i 1: and hen analyically builds he corresponding map mi. Each filer ieraion consiss of he following seps: 1. Sampling: The nex generaion of paricles {x [j] } is obained from he generaion {x [j] 1 } by sampling from a proposal disribuion π. Ofen, a probabilisic odomery-based moion model is used for π.

5 4 Slawomir Grzonka, Chrisian Plagemann, Giorgio Grisei, and Wolfram Burgard 2. Imporance Weighing: Imporance weighs w [j] are assigned o he individual paricles according o w [j] = p(x[j] 1: z 1:,u 1: ) π(x [j] 1: z 1:,u 1: ) p(z m [j] 1,x[j] )p(x[j] x [j] 1,u ) π(x x [j] 1: 1,z 1:,u 1: ) w [j] 1. (2) The weighs accoun for he fac ha he proposal disribuion π is in general no equal o he arge disribuion of successor saes [2]. 3. Resampling: Paricles are drawn wih replacemen proporional o heir imporance weigh. 4. Map Esimaion: For each paricle, he corresponding map esimae p(m [j] x [j] 1:,z 1:) is compued based on he rajecory x [j] 1: of ha sample and he hisory of observaions z 1:. The robusness and efficiency of his procedure srongly depends on he proposal disribuion π ha is used o sample he new sae hypoheses in he selecion sep. If he proposal disribuion differs oo much from he rue poserior, here is a high risk of filer divergence. In he following secion, we inroduce a concree proposal disribuion π ha uilizes a se of fuure sensor measuremens o yield beer pose esimaes. The resuling weigh updae equaion is sraighforward o implemen, while he new approach is more robus in sandard and hard, poorly-srucured environmens. 4 Look-ahead Proposals The sandard RBPF mapping approach fails in siuaions, in which he paricle disribuion significanly differs from he rue poserior. This can happen when he proposal disribuion π provides a bad approximaion o he rue one, or when he environmen does no provide enough srucure o allow proper paricle weighing. The laer siuaion is illusraed in he lower righ diagram of Figure 1. Due o he limied srucure of he environmen, he robo is unable o localize iself properly and looses he fine srucure of Box 2. Before reurning o is saring locaion, he robo is clearly delocalized, such ha Box 1 appears wice in he resuling map. Such a divergence can eiher be avoided by using an exremely large number of paricles or by direcing he given number of paricles o more accurae locaions. We follow he laer sraegy by compuing he pose predicion in each ieraion based on he k nex sensory inpus insead of jus one. These k measuremens are used o beer localize each paricle wihin is own map. Concreely, for each mapping paricle a ime 1, we draw l localizaion paricles and localize hem k seps ahead wihin heir map. The resuling pose poserior a ime +k is hen used o sample he successor pose of he mapping paricle a ime. This process is visualized in Figure 2.

6 Look-ahead Proposals for Robus Grid-based SLAM 5-1 +k Fig. 2. The algorihm for look-ahead pose sampling using k seps: For each mapping paricle a ime 1 (firs diagram), draw l localizing paricles (second). Move hem according o u +i and weigh hem according o z +i unil + k (hird diagram). Propagae back he weighs of he paricles o he iniial siuaion a ime (forh) and draw he successor sae according o his disribuion (righ diagram). The chars above he diagrams visualize he curren weighs of he individual localizaion paricles a he highlighed imeindex. By using he addiional sensory inpu, a more informed and hus more accurae proposal for he robo pose can be compued. Formally, he idea is o compue a beer esimae for he pose x, using he previous posiion x 1, he commands (odomery) u :+k, and he measuremens z :+k up o ime-index + k. As saed in Equaion (1), he Rao-Blackwellized paricle filer is an approach for sequenially esimaing he disribuion p(m x 1:,z 1: ) p(x 1: z 1:,u 1: ). (3) Here, p(x 1: z 1:,u 1: ) is approximaed using a paricle filer, whose samples we will call SLAM paricles. In he sandard approach, he SLAM paricles are drawn from a proposal disribuion based on he moion model p(x x 1,u ). In our approach, we use he more informed proposal p(x x 1,u,z :+k,u +1:+k,m 1 ). Such a proposal resuls from performing an independen k-sep localizaion for each SLAM paricle. The localizaion algorihm is iniialized wih he map and he robo pose of he corresponding SLAM paricle a ime 1. Our proposal can be rewrien more compacly, omiing m 1, as p(x x 1,u :+k,z :+k ), which we can rewrie as p(x x 1,u :+k,z :+k ) = p(x :+k x 1,u :+k,z :+k ) dx +1:+k.(4) Given he poses x...x +k and he map m 1 he obsersavions z...z +k are independan. Therefore, we can rewrie he erm inside he inegral as +k +k p(x :+k x 1,u :+k,z :+k ) = η p(z j x j ) p(x j x j 1,u j ). (5) j= j=

7 6 Slawomir Grzonka, Chrisian Plagemann, Giorgio Grisei, and Wolfram Burgard Here η = p(z :+k x 1 u :+k ) 1 is he Bayes normalizer. A paricle approximaion of Equaion 5 (he localizer paricles) can be obained by sampling a sequence of poses according o he sequence of moion commands. Each sample i has a weigh v (i) :+k proporional o he likelihood of he chain of observaions saring a 1. x (i) +k :+k j= p(x (i) j x(i) j 1,u j) v (i) +k :+k = p(z (i) j x(i) j j= ). (6) A sampled approximaion of he inegral in Eq. 4 p(x x 1,u :+k,z :+k ) x (i),v (i) (7) x (j) is recovered from he samples in Eq. 6. This is done by runcaing each rajecory x (i) :+k a ime and by back-propagaing he weighs of he rajecory, according o Fig. 2. Due o resampling operaions, he evoluion of hese rajecories can be described as a ree [3], hus he sample x (i) receives a weigh v (i) which { is he sum } of he weighs of is successors. The se S = x (i),v (i) is a sampled approximaion of our proposal { } disribuion. We can draw from his se N new SLAM paricles for ime, according o he imporance weighs. The rue poserior p(x (j) x (j) 1,z,u ) (he SLAM paricles) is recovered from his se by assigning o each newly drawn sample x (j) a weigh w [j] according o he imporance sampling principle (see Eq. 2): w [j] w [j] 1 w [j] 1 p(x (j) x (j) 1,z,u ) p(x (j) x (j) 1,u :+k,z :+k ) p(z x (j) )p(x (j) x (j) 1,u ) p(x (j) x (j) 1,u :+k,z :+k ) w [j] p(z x (j) ) 1 v (j) (8) (9) (10) The las sep follows direcly from Equaion 7 and he fac ha he rajecories x (i) :+k had been drawn including he moion u beween 1 and. Noe ha for updaing he weigh of a SLAM paricle, we jus need he weigh of he drawn successor sae a ime (firs ieraion of he nesed localizaion run) and he corresponding weigh a ime + k(= v ). Boh values are readily available from he localizaion run. Equaion (10) assures, ha no informaion (odomery, measuremens) is inegraed more

8 Look-ahead Proposals for Robus Grid-based SLAM 7 han once. Thus, he presened approach does no change he esimaed poserior disribuion, bu only he way i is represened by he limied paricle se of he filer. 5 Experimens In his secion, we presen a se of experimens demonsraing ha our mapping approach can be applied in boh indoor and oudoor scenarios. Furhermore, we compared our approach using localizaion proposals (ermed LP in his secion) o a sae-of-he-ar echnique [4], which uses a dynamically adaped proposal based on scan maching (SMP) and o a sandard RBPF using odomery-based proposals (OP). The resuls show ha our approach is more robus in poorly srucured siuaions, while also providing high qualiy maps in more srucured environmens. Measuring Map Qualiy To assess he qualiy of he resuling maps we evaluae a measure denoed as revisiing accuracy (RA), which reflecs he error in he robo pose esimae a revisied places relaive o previous visis. This measure capures he inernal consisency of a map. I herefore beer reflecs he pracical usefulness of he map han for example he absolue mean squared error of he correced rajecory relaive o an assumed rue one. The laer measure is overly sensiive o disorions on a global level, which does in general no lead o pracical problems, and i requires a ground ruh rajecory. For calculaing he revisiing accuracy, we add color markers o he ground a places ha are o be raversed several imes during he experimen. We hen recorded he imesamps a which he robo passed over hese checkpoins. Le p be a checkpoin visied a imes 1 and 2. The revisiing accuracy for his locaion is defined as ǫ p 1, 2 = w [j] (1 λ)((x [j] 1 x [j] 2 ) 2 + (y [j] 1 y [j] 2 ) 2 ) + λ(θ [j] 1 θ [j] 2 ) 2. j Here, x,y, and θ are he componens of he pose vecor and λ [0,1] is a weighing facor for he roaional componen. Inuiively, he revisiing accuracy for a given checkpoin is he (weighed) disance beween he esimaed posiions wihin every map, weighed by he corresponding mapprobabiliy. Mapping an Office Environmen We esed our approach in he highly srucured office environmen depiced in Figure 3. Given his daase, we compared our approach (LP) o SMP and OP in erms of he revisiing accuracy measure defined above. Figure 3

9 8 Slawomir Grzonka, Chrisian Plagemann, Giorgio Grisei, and Wolfram Burgard Fig. 3. Maps obained from he indoor environmen using OP (lef) and LP (righ). Fig. 4. Mapping a low-srucured environmen (upper lef): Maps obained by OP (upper middle) and our approach (upper righ). Traveled pahs of he robo using OP (lower lef), SMP (lower middle) and our LP approach (lower righ). All approaches used 50 mapping paricles. Our approach used addiionally 100 localizaion paricles per map and a lookahead of k = 5. depics he maps obained using OP (lef diagram) and our approach (righ diagram) for 20 mapping paricles. In our approach, we used 50 localizaion paricles per map paricle and a look-ahead of k = 3. The map generaed using he SMP is similar o ours. The localizaion accuracy is beween 5 cm and 10 cm for OP and less han 5 cm for boh, our approach (LP) and SMP. Overall, a localizaion error of less han 10 cm is considerably small wih he given grid resoluion of 5 cm per cell. Mapping a Low-Srucured Oudoor Environmen We compared our approach o hose proposed by Hähnel e al. [5] and Grisei e al. [4] in he oudoor environmen depiced in he upper lef picure of Figure 4. The robo sared a he checkpoin X, moved around he cardboard box 15 imes. Thereby i reurned o he checkpoin every 5h run. The laser range was limied o 4 meers, so ha he box was he only visible map elemen. Figure 4 depics he mapping resuls of he OP and our

10 Look-ahead Proposals for Robus Grid-based SLAM 9 error (weighed disance) OP (k=0) SMP (k=1) LP (k=2..9) look-ahead seps successrae OP SMP LP (50M,100L) LP (20M,100L) look-ahead seps Fig. 5. Mean and sandard deviaion for he approaches (lef) and he raio of runs wih an error less han 0.2 (righ). Noe ha he OP-RBPF and he SMP-RBPF approach never had an revisiing accuracy error below 0.2. Thus he success raes of boh approaches is 0. LP approach (second and hird upper diagram) using 50 SLAM paricles. For generaing he map, we used a look-ahead of k = 5 and 100 localizaion paricles per SLAM paricle. The hree lower diagrams in Figure 4 depic ypical pahs esimaed by he hree mapping approaches. The disance beween he checkpoin X and he box is around 4 m. As can be seen from hese resuls, OP fails o map his environmen and yields a highly inconsisen map. Alhough, when using SMP, he map qualiy is higher and he box has reained is squared shape, he resuling map is sill inconsisen. Our approach, in conras, is able o deal wih his hard siuaion and generaes accurae maps. To quaniaively evaluae he performance of he approaches, we analyzed he mapping resuls for differen numbers of paricles. Each approach was execued 30 imes for each seing. We measured ǫ X firs, las, wih firs and las being he imesamps, when he robo moved over he checkpoin X for he firs and he las ime respecively. Figure 5 (lef) gives he resuls of his experimen. For reasons of beer readabiliy we depiced he resuls obained by 50 mapping paricles only. Furhermore, Figure 5 (righ) depics he success rae of he runs, defined as he raio of runs where he revisiing error ǫ was less han 0.2. I can be seen from hese experimens, ha in siuaions wih hardly idenifiable feaures, he performance obained using our proposal is higher han he one obained by he oher grid based RBPF mappers [4, 5]. Alhough he RBPF based on SMP ouperforms he sandard RBPF, i achieves significanly less accurae maps han he echnique proposed in his paper. Our approach is able o look hrough gaps of low srucure unil enough srucure is provided for proper localizaion. In his way, i can handle such a hard environmen even wih less mapping paricles (see figure 5 (righ), curve obained wih 20 mapping paricles and 100 localizaion paricles for each map).

11 10 Slawomir Grzonka, Chrisian Plagemann, Giorgio Grisei, and Wolfram Burgard 6 Conclusions In his paper, we presened an exension o he Rao-Blackwellized paricle filer for simulaneous localizaion and mapping (SLAM), which significanly improves he mapping in environmens which are poorly srucured. Our approach uses a novel proposal disribuion ha looks k seps ahead in ime o yield a more informed pose esimae for he mapping decision. We provided a mahemaical derivaion of he approach and showed, ha he weigh updae for our improved proposal akes a simple form. Our mehod has been implemened and esed on real robo daa ses. We compared our echnique o wo sae of he ar mapping echniques. Experimenal resuls sugges ha our approach yields beer maps, especially in environmens wih sparse feaures ha faciliae accurae localizaion of paricles. Acknowledgmen This work has been suppored by he EC under conrac number FP6-IST , Acion line: Micro/Nano Based Sub-Sysems References 1. A. Douce, J.F.G. de Freias, K. Murphy, and S. Russel. Rao-Blackwellized parcile filering for dynamic bayesian neworks. In Proc. of he Conf. on Uncerainy in Arificial Inelligence (UAI), pages , Sanford, CA, USA, A. Douce, N. de Freias, and N. Gordan, ediors. Sequenial Mone-Carlo Mehods in Pracice. Springer Verlag, A. Eliazar and R. Parr. DP-SLAM: Fas, robus simulainous localizaion and mapping wihou predeermined landmarks. In Proc. of he In. Conf. on Arificial Inelligence (IJCAI), pages , Acapulco, Mexico, G. Grisei, C. Sachniss, and W. Burgard. Improving grid-based slam wih Rao-Blackwellized paricle filers by adapive proposals and selecive resampling. In Proc. of he IEEE In. Conf. on Roboics & Auomaion (ICRA), pages , Barcelona, Spain, D. Hähnel, W. Burgard, D. Fox, and S. Thrun. An efficien FasSLAM algorihm for generaing maps of large-scale cyclic environmens from raw laser range measuremens. In Proc. of he IEEE/RSJ In. Conf. on Inelligen Robos and Sysems (IROS), pages , Las Vegas, NV, USA, Andrew Howard. Muli-robo simulaneous localizaion and mapping using paricle filers. In Roboics: Science and Sysems, Cambridge, MA, USA, M. Monemerlo, S. Thrun D. Koller, and B. Wegbrei. FasSLAM 2.0: An improved paricle filering algorihm for simulaneous localizaion and mapping ha provably converges. In Proc. of he In. Conf. on Arificial Inelligence (IJCAI), pages , Acapulco, Mexico, M. Monemerlo and S. Thrun. Simulaneous localizaion and mapping wih unknown daa associaion using FasSLAM. In Proc. of he IEEE In. Conf. on Roboics & Auomaion (ICRA), Taipei, Taiwan, M. Monemerlo, S. Thrun, D. Koller, and B. Wegbrei. FasSLAM: A facored soluion o simulaneous localizaion and mapping. In Proc. of he Naional Conference on Arificial Inelligence (AAAI), pages , Edmonon, Canada, K. Murphy. Bayesian map learning in dynamic environmens. In Proc. of he Conf. on Neural Informaion Processing Sysems (NIPS), pages , Denver, CO, USA, 1999.