Fast laser scan matching approach based on adaptive curvature estimation for mobile robots P. Núñez,R.Vázquez-Martín, A. Bandera andf.

Transcription

1 Robotia (2009) volume 27, pp Cambidge Univesity Pess doi: /s Pinted in the United Kingdom Fast lase san mathing appoah based on adaptive uvatue estimation fo mobile obots P. Núñez,R.Vázquez-Matín, A. Bandea andf.sandoval Gupo de Teleomuniaiones, Dept. Tenología de los Computadoes y las Comuniaiones, Univesidad de Etemadua, Cáees, Spain. Gupo de Ingenieía de Sistemas Integados, Dept. Tenología Eletónia, Univesidad de Málaga, Málaga, Spain. (Reeived in Final Fom: May 29, Fist published online: July 7, 2008) SUMMARY This pape desibes a omplete lase-based appoah fo taking the pose of a obot in a dynami envionment. The main novelty of this appoah is that the mathing between onseutively aquied sans is ahieved using thei assoiated uvatue-based epesentations. The poposed san mathing algoithm onsists of thee stages. Fistly, the whole aw lase data is segmented into goups of onseutive ange eadings using a distane-based iteion and the uvatue funtion fo eah goup is omputed. Then, this set of uvatue funtions is mathed to the set of uvatue funtions assoiated to the peviously aquied lase san. Finally, haateisti points of paiwise uvatue funtions ae mathed and used to oetly obtain the best loal alignment between onseutive sans. A losed fom solution is employed fo omputing the optimal tansfomation and minimizing the obot pose shift eo without iteations. Thus, the system is outstanding in tems of auay and omputation time. The implemented algoithm is evaluated and ompaed to thee state of the at san mathing appoahes. KEYWORDS: San mathing, Adaptive uvatue funtion, Mobile obotis. 1. Intodution One of the key funtions in an autonomous mobile obot is to keep tak of its pose position and oientation while moving. To ahieve this elative loalization and to edue the ineasing eo fom the use of dead ekoning, it is ommon that the obot aies etenal sensos, like a 2-D lase ange findes, to peeive the envionment. Envionment peeptions taken fom diffeent loations and instants of time ould be mathed and then it would be possible to update the position estimate aoding to the mathing esults. It an be noted that the goal of this mathing poess would not be dietly to build an auate map, but athe to ensue a stable and fast loalization, egadless of the obot speed and without any estitions of the oveed distane. 9 To ahieve that, the san mathing tehniques estimate the obot displaement between two instants of time * Coesponding autho. pmnt@uma.es by dietly ompaing the peeived sans. Thus, they an be used, fo instane, to impove the pefomane of navigation appoahes whih ty to solve the simultaneous loalization and mapping (SLAM) poblem. The SLAM poblem has eeived onsideable attention ove the last deade and diffeent solutions have been poposed. 3,6,11 The diffiulty of the SLAM appoah lies in the fat that an auate estimation of the obot tajetoy is equied to obtain a good map. As has been pointed out by seveal authos, it is possible to edue the loalization eo due to slippage using san mathing algoithms as an impoved odomety. 8,12,15 One of the main diffeenes between the eisting san mathing algoithms is thei usage and not thei featues etated fom the aw data, as line segments o ones. 9 Thus, in stutued envionment it an be assumed that the eistene of polygonal elements an be etated fom the data and an be used to align the sans. 5,7 These methods ae effiient and eliable and, theefoe, they have been widely used to povide obot self-loalization in these envionments. Howeve, these geometi items ae not pesent in unstutued envionments. To deal with these situations, a seond ategoy of aw data mathing tehniques whih wok without epliit geometi intepetation has been developed. 14,16 Howeve, most of these point-to-point methods have been designed fo situations in whih the envionment is stati duing the measuement poess. Theefoe, dynami items (e.g. people o objets moving aound the obot) an lead to seious eos in the pose estimation. 2 Independently of the hosen stategy, it would be desiable that a san mathing algoithm handle the following thee demands 2 : 1. Eliminate san points assoiated to dynami objets in the envionment o othewise the esult will be inoet. 2. Math in stutued and unstutued envionments. 3. Povide infomation about the unetainty of the estimated obot pose. This pape poposes a novel san mathing appoah based on the uvatue infomation assoiated to the san (named CF-SMA, fom Cuvatue Funtion San Mathing Algoithm). This algoithm an deal with dynami, stutued o unstutued envionments. Besides, it obtains Downloaded fom Univesidad de Etemadua.Bibliotea, on 20 Feb 2017 at 11:13:05, subjet to the Cambidge Coe tems of use, available at

2 470 Fast lase san mathing appoah fo mobile obots an unetainty mati fo the obot pose. In this appoah, the fast and obust oespondene between two onseutive sans is ahieved fom the mathing of haateisti points etated fom paiwise uvatue funtions. The epesentation of the lase san data using a set of adaptively estimated uvatue funtions was oiginally poposed by the authos in a pevious wok. 13 In this pape, this onept is applied to the effiient mathing of lase sans. Epeimental esults show that the pefomane of the poposed appoah is high in tems of peision and omputational ost. Thus, the method has been suessfully ompaed to othe simila appoahes. 2. Poposed Method Let pt = (t,y t,θ t )T be the t-th pose of the obot, whee (t,y t ) ae the o-odinates in the XY hoizontal-fontal plane and θt is the oientation with espet to the vetial ais Z. The aim of san mathing appoah is to alulate an estimation of the obot displaement between two diffeent instants of time by ompaing onseutive sans povided by a lase ange finde. This elative displaement, p = (, y, θ ) T, an be used to update the obot pose and, theefoe to povide an altenative soue of infomation to the obot odomety (in fat, this elative displaement will be usually moe eliable than the odometi infomation povided by popioeptive sensos, whih ould be affeted by sliding eos). Given a pose and the estimated elative shift p, the updated pose pt+1 an be alulated aoding to the kinematis model: t+1 yt+1 θt+1 = t yt θt os θt sin θt 0 + sin θt os θt 0 y. (1) θ The main limitation of san mathing appoahes is that the quality of the mathing poess is uial fo an auate estimation. In fat, a bad assoiation between two onseutive sans povokes a signifiant obot pose eo whih is non-eoveable in most of the situations. 2 Thus, a signifiant advane in these san mathing algoithms is the possibility of emoving those outlies points o featues whih ineases the pose eo. The appoah desibed in this pape poposes a uvatuebased san mathing algoithm whih emoves those featues whih an inease the obot pose eo. As an be noted in Fig. 1, the poposed appoah onsists of thee independent stages. Net, a bief desiption of the modules depited in Fig. 1 is povided: CF-SMA1. At the fist stage, the uent san, C, is segmented into goups using a distane-based iteion. Then, the uvatue funtion fo eah goup is omputed (see Fig. 1). This uvatue funtion is alulated using an estimato whih adapts itself to the loal suounding of the studied ange eading. This segmentation poess is desibed with details in a pevious wok. 13 CF-SMA2. A oelation mati whih measues the similaity between the sets of uvatue funtions assoiated to the uent san and to the peviously aquied san (denoted as efeene san, R) is built. This mati allows to selet paiwise uvatue funtions and emoves isolated goups whih will not be inluded in the obot pose estimate poess. In ode to pefom this alulation in a fast and obust way, the appoah implements a FFT algoithm whih allows to ompae uvatue funtions only using poduts. The output of this stage ae two subsets of oesponding points, C C and R R, whih pemit to ompute the pose shift as the optimal tansfomation mapping C onto R. CF-SMA3. Finally, the best loal alignment between onseutive sans is obtained using C and R. Hee, a losed fom solution is employed fo omputing the optimal tansfomation, impoving omputational time of the san mathing algoithm. Robot s pose is updated aoding to Eq. (1) and uent san is saved as the new efeene san (R C). Net subsetions pesent a detailed desiption of eah stage. In ode to impove the undestanding of the poposed algoithm, Table I summaizes the main symbols used in this pape. Fig. 1. Oveview of the san mathing algoithm poposed in this pape. The method uses thee onseutive stages in ode to obtain the best alignment between onseutive sans. Downloaded fom Univesidad de Etemadua.Bibliotea, on 20 Feb 2017 at 11:13:05, subjet to the Cambidge Coe tems of use, available at

3 Fast lase san mathing appoah fo mobile obots 471 Table I. Main symbols used in this pape. Symbol Desiption pt = (, y, θ ) Robot s position at time instant t p = (, y, θ ) Relative displaement between two onseutive time instants (ρ, ϕ) l and (, y) l Pola and Catesian o-odinates of the l ange eading, espetively σρ 2 and σ ϕ 2 Vaiane of the ange and oientation measuement eos (lase ange finde) N R Numbe of data points of the whole san C and R Cuent and efeene sans N and N Numbe of segments afte distane-based segmentation (uent and efeene sans) NR i and NR i Numbe of data points of the i segment (uent and efeene sans) k l Adaptive k value fo alulating the uvatue funtion η l Cuvatue inde assoiated to the data point l ηl i Cuvatue inde assoiated to the data point l, whih belong to the segment i of the san C f and R f Set of uvatue funtions assoiated to the sans (uent and efeene) P f Set of paiwise uvatue funtions υ ij Coelation inde between uvatue funtions i C f and j R f υ ma Maimum oelation inde whih desibes paiwise uvatue funtions N k Numbe of mathed uvatue funtions N t Numbe of possible mathed points N t Numbe of eal mathed points N l Numbe of haateisti points Nl and Nl Numbe of haateistis points fo the uent and efeene san, espetively n Numbe of point at ight and left sides of eah haateisti point 2.1. CF-SMA1. Calulation of the set of uvatue funtions assoiated to the lase san In oboti field, san data ae typially in the fom {(ρ,ϕ) l l=1...nr }, on whih (ρ, ϕ) l ae the pola o-odinates of the l-th ange eading (ρ l is the measued distane of an obstale to the senso otating ais at dietion ϕ l ) and N R is the numbe of ange eadings. It is assumed that eah ange eading is independently affeted by Gaussian noise in both o-odinates, ange and beaing, with zeo mean and vaiane σρ 2 and σϕ 2, espetively. The poposed san mathing appoah is based on the mathing of physial entities of the sene whih ae pesent in both sans. Fom this mathing, the seah of paiwise points an be deived. Theefoe, it will be neessay to obtain a omplete desiption of the envionment whih allows to identify a set of distinguished entities. To obtain this desiption, the san is fistly segmented using the adaptive beakpoint deteto. 4 This deteto poposes that two onseutive ange eadings belong to the same segment if the distane between them is less than a vaiable theshold, instead of a onstant one. Sine the senso angula inement is onstant, this theshold will only depend on the ange san distane. The theshold value an be detemined fom the geometial sheme shown in Fig. 2. In this figue, it is defined as a vitual line whih passes though the san point ρ l 1 making an angle λ with espet to the sanning dietion ϕ l 1. This angle will epesent the wost ase of inidene angle of the lase san ay with espet to a line fo whih ange eadings ae still eliable. Its value will be detemined with use epeiene. Unde this onstaint, with some assumptions and futhe mathematial development, the adaptive beakpoint deteto speifies that two onseutive ange eadings belong to diffeent segments if (ρ,ϕ) l (ρ,ϕ) l 1 >ρ l 1 sin ϕ sin(λ ϕ) + 3σ ρ, (2) whee ϕ is the lase angula esolution, λ is a use-speified onstant paamete and σ ρ the ange standad deviation of the senso measues. The pesene of this value in the equation allows to enompass the stohasti behaviou of ρ l. 4 Points whih belong to the same segment i in the uent san ae assoiated to one element of the envionment. If eah goup of points is haateized by a uvatue funtion, i ={ηl i l = 1,...,N R i }, whee ηl i value is the uvatue inde assoiated to the ange eading l of the segment i, then the whole san will be desibed by a set of N uvatue funtions, C f ={ i i = 1,...,N }. Cuvatue funtions basially desibe how muh a uve bends at eah point. Peaks of the uvatue funtion oespond to the ones of the epesented uve and thei height depends on the angle at these ones. Flat segments whose aveage value is lage than zeo ae elated to uve segments and Fig. 2. Geometial sheme assoiated to the adaptive beakpoint deteto. Setion 2.1 eplains with details the algoithm. Downloaded fom Univesidad de Etemadua.Bibliotea, on 20 Feb 2017 at 11:13:05, subjet to the Cambidge Coe tems of use, available at

4 472 Fast lase san mathing appoah fo mobile obots those whose aveage value is equal to zeo ae elated to staight line segments. Thus, the uvatue funtion an be used as a desipto of the uent san segment. In this ase, the uvatue estimato is a modified vesion of the adaptive method desibed in ef. [13]. With espet to the pevious vesion, this new algoithm edues the omputational load haateizing eah ange eading by a unique value (k i ). In the pevious wok, the algoithm alulated the maimum lengths of the lase san pesenting no disontinuities on the ight and left sides of the ange eading (named k f and k b, espetively), and used them to obtain two vetos whih allowed to alulate the uvatue value. Let {(,y) l l=1,...,nr } be the Catesian epesentation of the ange images, whee l = ρ l os ϕ l and y l = ρ l sin ϕ l ; the method used to alulate the adaptive uvatue funtion fo eah etated segment onsists of the following steps: 1. Calulation of the maimum length of lase san pesenting no disontinuities on the ight side of the woking ange eading l, k l. This is obtained as the lagest value that satisfies: k l +l 1 j=l d(j,j + 1) d(l,l + k l ) <U k, (3) whee d(l,j) is the Eulidean distane fom ange eading l to ange eading j, and U k is a theshold value whih allows the smoothing of the noise assoiated to the lase data aquisition. As an be noted in Eq. (3), the k l value is then adaptively seleted aoding to the suounding of eah ange eading l. 2. Estimation of the uvatue angle assoiated to eah ange eading l, denoted as ϑ l. Aoding to the pevious k l value, this angle ϑ l an be obtained as (see Fig. 3): ϑ l = atan ( kl +l 1 ) j=l ( j+1 j ) kl +l 1 j=l (y j+1 y j ) (4) 3. Calulation of the uvatue inde assoiated to eah ange eading l. This value an be estimated using the equation: η l = ϑ l+1 ϑ l. (5) Figue 4a illustates this segmentation stage fo a eal san of a stutued envionment. As is shown, eight distint segments ae defined in the figue. Isolated goup of points ae emoved using a onstaint elated to the size of eah segment. 13 In Fig. 4b, the absolute value of the uvatue funtions assoiated to eah san segment in Fig. 4a is shown CF-SMA2. Points mathing fom two onseutive sans As afoementioned, the poposed appoah is based on the idea that the taking of physial entities of the sene an be aied out by mathing the sets of uvatue funtions assoiated to two suessively aquied sans. Thus, C f is ompaed to the set of uvatue funtions fom efeene san, R f. If the numbe of goups in this efeene san is N, and being NR i the numbe of ange eadings fo eah one of these segments, R f is defined as R f ={ i i = 1,...,N }, being i ={ηl i l = 1,...,N R i }. Fo the sake of effiieny and obustness, instead of onsideing all the points of the set of paiwise uvatue funtions, P f, a seond mathing step is used to seah fo oesponding points in P f. Then, in ode to ensue the quality of the san mathing algoithm, the poposed appoah divides the mathing stage in two steps. Fistly, the mathing of uvatue funtions is obtained in a fast way using the Fouie domain. Net, the seond mathing step onsists of mathing points fom the set of oesponding uvatue funtions. Only these paiwise points ae used to obtain the estimated displaement p. (1) Mathing of the uvatue funtions assoiated to the uent and efeene lase sans. One the uvatue Fig. 3. Calulation of the uvatue angle assoiated to eah ange eading l, ϑ l. The maimum length of lase san pesenting no disontinuities on the ight side of the woking ange eading l, k l, was adaptively estimated aoding to the suounding of the ange eading l. Fig. 4. CF-SMA1 desiption: (a) The envionment san is segmented using the distane-based iteion and (b) uvatue funtions of eah segment ae omputed. In this ase, N = 8. Downloaded fom Univesidad de Etemadua.Bibliotea, on 20 Feb 2017 at 11:13:05, subjet to the Cambidge Coe tems of use, available at

5 Fast lase san mathing appoah fo mobile obots 473 funtions assoiated to eah goup of the efeene san R and uent san C have been obtained (Setion 2.1), a mathing is pefomed between both the sets to obtain a fist estimation of the tansfomation that maps C onto R. This tansfomation will oughly oespond to the movement of the obot between the aquisition of these sans. To math these two sets of uvatue funtions, a mati of possible mathing pais is built. The element υ ij of this mati is defined by the similaity measue between two uvatue funtions. A fast way to measue the similaity of two uvatue funtions is to wok on the Fouie domain, whee oelations ae tansfomed into poduts. 1 In ode to ahieve this tansfomation with a low omputational time, FFT is used. It implies that the length of eah uvatue funtion must be peviously nomalized to a value of 2 n (e.g. 64, 128 o 256). Then, the oelation inde o distane funtion, υ ij, is defined as υ ij = IFFT(FFT[ i ] FFT[ j ]) i=1,...,n,j=1,...,n, (6) whee IFFT is the Invese Fast Fouie Tansfom, and i C f and j R f. When all oelation indees have been alulated, the maimum value of this oelation mati is ) and the oesponding uvatue funtions of C and R ae mathed if this value is highe than a peviously defined theshold U. This value U is imposed to eliminate mathed uvatue funtions with a low value of oelation inde. The desibed poess is iteatively applied to obtain a set of paiwise uvatue funtions, P f ={p k = ( i, j ) k i C f, j R f, υ ij = found fo eah ow (υ ma ij ma{υ in n = 1,...,Nl }, k = 1,...,N k }, whee N k is the numbe of mathed uvatue funtions. (2) Mathing paiwise points fom the set of oesponding uvatue funtions. Eah point of a uvatue funtion has an assoiated ange eading. Thus, the set of paiwise possible mathed points, M t f. Let m l ={( l,y l )} l=1,...,nt be the set of ange eadings assoiated to the mathed uvatue funtions of the uent san. In the same way, let m l = {( l,y l )} l=1,...,nt be the set of ange eadings assoiated to the mathed uvatue funtion of the efeene san. Theefoe, M t f ={(m l,m l ) m l, m l R 2, l = 1,...,N t }. In ode to edue and to impove the quality of the mathing poess, this seond step will selet and math only N t ange eadings fom the set of N t possible mathed points, M t f M t f. The seletion of these N t points is based on the detetion uvatue funtions, P f, defines a set of N t of haateisti points on the uvatue funtion. A point of the uvatue funtion is a haateisti one if its value is highe than a fied theshold U p, and is also a loal maimum in the uvatue funtion, that is η i = ϑ i+1 ϑ i U p η i 1 <η i η i+1. (7) Figue 5 shows a fagment of the san pesented in Fig. 4 and the assoiated uvatue funtion. Fo this san, in Fig. 5b, the algoithm detets two haateisti points (values of uvatue funtion upon U p ). Range eadings A = ( a,y a ) and B = ( b,y b ) maked in Fig. 5a ae assoiated to Fig. 5. (a) Fagment of a lase san aptued by the 2-D lase ange finde and (b) uvatue funtion assoiated to san (a). Two haateisti points ae deteted and maked (A and B). the haateisti points deteted on the uvatue funtion shown in Fig. 5b. Let Nl and Nl be the numbe of haateistis points fo the uent and efeene san, espetively. Fo eah paiwise uvatue funtions a mathing between eah haateisti point, m i ={( i,y i )} i=1,...,n l and m j = {( j,y j )} j=1,...,n l, and a similaity oeffiient, S(m i,m j ), is omputed. This similaity oeffiient is based on the Peason oelation and it is obtained using the n neighbouing points on the left and ight sides of the haateisti point: S ( m i j),m = S ( m ) i,m j S( m ) i,m j, (8) y whee S(m i,m j ) and S(m i,m j ) y ae the Peason oeffiients fo the and y o-odinates, espetively, S ( ) m i,m j = S ( ) m i,m j y = n n k=1 n k=1 n k=1( ik i )( jk j ) ( ik i ) 2 ( jk j ), 2 ( y ik ȳi )( y jk ȳj ) k=1 ( y ik ȳi ) 2 ( y jk ȳj ) 2 being (i,y i ) and ( j,y j ) the Catesian o-odinates fo eah point ( and indiate uent and efeene san, espetively), ( i, ȳ i ) and ( j, ȳ j ) the aveage values fo the set of n points. S(m i,m j ) is a positive value in the ange fom 0 (thee is no oelation) to 1 (both sans ae equal). In ode to impove this seond mathing step, the algoithm ejets andidates that satisfy these onditions: (9) Downloaded fom Univesidad de Etemadua.Bibliotea, on 20 Feb 2017 at 11:13:05, subjet to the Cambidge Coe tems of use, available at

6 474 Fast lase san mathing appoah fo mobile obots (a) Low stength. This onept is deteted when S(m i,m j ) S min, being S min a theshold whih is used to eliminate andidates with low oelation inde. (b) Low distintiveness. Thee eists m l satisfying S(m i,m l )/S(m i,m j ) 1. In othe wods, those andidates that have simila Peason oeffiients ae ejeted, eduing the ambiguity in the mathing poess. () Unidietionality. Fo m m, the math maimizing {S(m m,m j )} j=1,...,nl is ˆm n, but fo this latte one, the math maimizing {S(m i,m n )} i=1,...,nl is ˆm p ˆm m. Theefoe, the final poess obtains a set of N l mathed haateisti points. Finally, aound eah haateisti point, the algoithm defines a ange of n mathed points at ight and left sides. These N t paiwise points (N t = N l (2n + 1)), onstitute the set of final mathed points, M t f = {(m l,m l ) m l, m l R 2, l = 1,...,N t }. The obustness and auay of the poposed method is based on these two desibed mathing steps. Figue 6 shows two fagment of onseutive sans and thei assoiated uvatue funtions. Mathed points of these uvatue funtions ae pesented in Fig. 6b. Coesponding Catesian o-odinates points ae maked in Fig. 6a. In this ase, N t = 65 and N l = 2 (being n value equal to 30. Obviously, the N t value depends on the numbe of data points on the left and ight sides of the haateisti points) CF-SMA3. Calulation of the elative tanslation and otation The pupose of the mathing poess desibed in Setion 2.2 is to povide a set of good quality mathes whih allows to alulate the estimated pose of the obot. Two onstaints ae imposed to an optimal san mathing algoithm: (i) to be fast and (ii) to be auate in the mathing poess in ode to avoid mistakes in the alulation of the elative displaement. Fig. 6. (a) Two onseutive lase sans and mathed points (N t = 65) and (b) mathing of uvatue funtions assoiated to san (a). These two onstaints ae solved using the two mathing steps defined in Setion 2.2. Besides, the poposed method obtains an optimal tansfomation fo mapping the uent san onto the efeene san using a losed fom solution. In this way, the algoithm minimizes the displaement eo without iteations, impoving the omputational ost. 9 The solution of this poblem onsists of solving the minimization of the eo funtion: N t N t E(R θ, T) = ω ij m i ( R θ m j + T ) 2, i=1 j=1 (10) whee m i and m j ae the mathed ange eadings belonging to M t f, and ω ij is a binay value defined as 1 if m i and m j have been mathed o 0 if they have not been mathed. R θ and T ae the seahed otation and tanslation maties whih elate the uent and efeene sans. They ae defined as ( os θ sin θ R θ = sin θ os θ ( T = ), (11) y ), (12) whee p = (, y, θ ) T desibes the estimated elative shift whih allows to estimate pt+1 fom p t.an etended development of the optimization poblem desibed in Eq. (10) is detailed in ef. [9]. In that wok, authos obtain a losed-fom epession whih allows to ompute the otation and tanslation maties aoding to the set of mathed points, instead of employing an iteative minimization algoithm. Thus, afte omputing the pose shift p as the optimal tansfomation, the obot pose is updated using the Eq. (1) and the uent san is saved as a new efeene san (C R). Convegene of the solution is ensued due to the non-iteative method used by the san mathing algoithm to estimate the obot displaement. 9 Finally, to oetly integate the san mathing poess into othe navigation tasks, suh as a SLAM algoithm, it is neessay to know the statistial popeties of the estimate obot displaement, p. Thus, in SLAM, the pobability eo distibution assoiated to the poposed appoah is needed to use the pose povided by the san mathing algoithm in the SLAM stohasti poess. This eo distibution is onsideed as Gaussian noise, and it is epesented by a ovaiane mati, whih indiates the quality of the pose estimation. In fat, high values on the main diagonal of the ovaiane eflets an inease in the unetainty. In ode to obtain the eo ovaiane mati of the pose estimation, the tehnique desibed by Lu and Milios 10 is used. In this wok, the theoy of linea egession is applied on a san mathing algoithm to deive the ovaiane mati of the obot displaement analytially. Equation (10) defines a non-linea eo funtion of the displaement (R θ, T ), whee the non-lineaity is due to the obot otation, θ. Let (i, y i ) and ( i, y i ) be the Catesian o-odinates of two mathed points, that is, both data point denote the same Downloaded fom Univesidad de Etemadua.Bibliotea, on 20 Feb 2017 at 11:13:05, subjet to the Cambidge Coe tems of use, available at

7 Fast lase san mathing appoah fo mobile obots 475 Fig. 7. (a) Pionee 2-AT obot equipped with a LMS200 lase senso; (b) one eample of uent and efeene sans used fo the fist epeiment (see tet fo details) and () esulting maps afte testing the poposed algoithm in the seond epeiment (dak gey olo). Map based on the odomety infomation (light gey). point in the eal sene. Theewith Eq. (10) simplifies to N t E(R θ, T) = m i ( R θ m i + T ) 2 (13) i=1 whih an be lineaized fo small θ values,as ( ) ( )( ) ( ) i os( θ ) sin( θ ) yi i sin( θ ) os( θ ) yi y ( ) ( )( ) ( ) i 1 θ yi i θ 1 yi y ( ) ( ) i i 1 0 y yi yi i 0 1 i y θ = (Y M ˆb) T (Y M ˆb) (14) being ˆb = (, y, θ ) T, the estimate obot displaement, and M and Y ae defined as ( ) Y = i i yi yi, ( ) 1 0 y M = i 0 1 i. (15) The above equation is linea, and thus, it is possible to use the linea egession analysis to alulate an optimal estimate of the ovaiane mati assoiated to the obot displaement 10 : C R θ T = (M T M) 1 σ 2 (16) being σ 2 the vaiane of the measuement lase eo, supposing Gaussian with zeo mean. In ef. [10], the authos Typial values fo θ ae small due to the fequeny of aquisition of onseutive lase sans. popose an estimate of the σ 2 value: σ 2 = 1 N t 3 E min( ˆb), (17) whee E min ( ˆb) is the minimum eo aoding to the estimate obot displaement ˆb. Thus, the ovaiane mati of the estimato ˆb is a 3 3 symmeti mati whose diagonal elements ae the vaianes of the thee-paamete estimato and whose off-diagonal elements ae thei oelations: σ 2 σ y σ θ C R θ T = σ y σ y 2 σ y θ. (18) σ θ σ y θ σ 2 θ 3. Results The desibed san mathing algoithm has been tested in vaious eal envionments. Ou Pionee 2-AT obot is equipped with a SICK Lase Measue System LMS200 and an embedded Pentium III-1000 MHz. The field of view is 180 in font of the obot and up to 8 m distane. The ange samples ae spaed 0.5, all within the same plane. Figue 7a shows the obot used in the epeiments. The epeiments ae foused on the evaluation of the poposed method in tems of obustness, auay and poessing time in dynami envionments. Besides, a ompaative study with the main san mathing algoithms has been povided. Peviously, this setion eplains how the paametes of the poposed algoithm have been adjusted Estimation of paametes The poposed method equies hoosing values fo a set of paametes. The main paametes desibed in this pape ae: 1. The paametes σ ρ and λ used by the beakpoints deteto. 2. The theshold value whih detemines the noise level toleated by the adaptive uvatue deteto, U k. 3. The minimum oelation inde value to be onsideed as paiwise uvatue funtions, U. 4. The paamete U p, defined as the minimum value of a uvatue funtion to be onsideed as a haateisti point of the san lase. Downloaded fom Univesidad de Etemadua.Bibliotea, on 20 Feb 2017 at 11:13:05, subjet to the Cambidge Coe tems of use, available at

8 476 Fast lase san mathing appoah fo mobile obots 5. The numbe of points at ight and left sides of the haateisti point used by the losed fom to detemine the elative displaement, n. 6. The theshold S min whih ejets paiwise haateisti points with low stength. As was desibed in Setion 2.1, the poess to obtain the values fo σ ρ and λ is based on the pevious wok of Boges and Aldon. 4 These values have been fied to m and 10, espetively. The theshold value U k is used to eliminate spuious noise of the lase san. In ode to set it oetly, a set of eal plane sufaes has been sanned at diffeent distanes fom the obot. In these plana sufaes, the values must be fied to do not detet any loal peak. This simple epeiment has povided a U k value equal to 1.0. This value has been used in the epeiments desibed in this pape. A simila epeimental poess has been used to obtain the values fo U and U p. The fist one is employed to impove the oelation poess, thus paiwise uvatue funtions whose assoiated oelation inde is lowe than U ae disaded (esidual noise in the FFTs). On the othe hand, U p is used to detet haateisti points of the lase san. In this ase, a set of eal sufaes has been sanned at diffeent distanes. In these situations, the values must be fied to math both, the uvatue funtions and to detet most of the haateisti points of the sans. Fom this test, U and U p values have been fied to 0.4 and 0.2 ad, espetively. Net, the value of n is alulated. It must be noted that a low value of n implies few points to be used in Eq. (8) and thus, estimated pose esults an be inauate. On the ontay, a high value of n inludes too muh points in the estimation poess, ineasing the omputational load of the algoithm. In shot, the value of n must be seleted taking into aount these situations. Afte seveal tests whee the gound tuth was known, it has been found that an n value equal to 30 edues the displaement eos at an aeptable omputation time. Finally, the theshold S min value was seleted to 0.2. To do that, a set of onseutive sans has been onsideed at diffeent obot position (distane and oientation). The mathing poess between haateisti points was analysed with details, S min being seleted in ode to ejet paiwise points with small oelation value whih would be wong solution Epeiments with gound tuth The epeiments epoted in this setion fous on two aspets of the estimation pefomane: the auay of the displaement estimates and the obustness with espet to eos due to dynami objets in the sene. To test the poposed algoithm, two epeiments ae ahieved. The fist epeiment onsists of mathing two onseutive sans aquied in the same obot loation, (,y,θ ) T = (0, 0, 0) T, the diffeene being between one san to anothe due to the senso s noise and the pesene of people moving in the obot suounding. Net, to the seond san is applied a andom otation and tanslation (the aveage values of the andom tanslation and otation ae 100 m and 15 espetively, with standad deviation σ = σ y = 50 mm and σθ = 5 (see Fig. 7b). Afte applying the epeiment 1000 times in this senaio, the final aveage displaement eo was limited to = 12 mm, y = 22 mm and θ = In this patiula ase, the aveage numbe of mathed points N t assoiated to the set of paiwise uvatue funtions was 245. The seond epeiment ompaes the oveall auay of the algoithm. The epeiment onsists of a set of onseutive san (1100 sans) aquied in a stati envionment. The obot was tele-opeated with small displaements (otations and tanslations) aound its initial position pt=0 = (0, 0, 0). Finally the obot was stopped and the final loation (pose and oientation) was measued with an unetainty less than 2 mm in displaement and 0.2 in oientation. The tue obot pose at the end of the epeiment was a = 46 mm, y a = 52 mm and θa = Results of the estimate obot pose wee = 29 mm, y = 26 mm and θ = 3.9. Figue 7 illustates the esulting map afte applying the poposed san mathing algoithm (dak gey olo) and the esulting map only using the odometi infomation (light gey olo) A ompaative study Given the settings and speifiations desibed in Setion 3.1, the Pionee 2-AT obot has been tele-opeated into two diffeent indoo senaios: the eseah aea at the Teleommuniation Engineeing Shool (Univesity of Málaga) and the ISIS Goup Laboatoies at the Andalusian Tehnology Pak (Málaga). The fist test aea is used to test the desibed appoah in a stati envionment, meanwhile in the seond epeiment some people wee walking aound the obot, simulating a dynami envionment. The ompaative study is based on the auay, obustness, poessing time and easiness of the implementation. The methods used in this ompaative study have been povided by Gutmann s implementation (see ef. [7] fo moe details): Iteative Dual Coespondene (IDC), Co Algoithm 5 and Histogam san mathing. 16 IDC onsists of mathing aw data of both sans without featues detetion, e.g. all points of the sans ae used on the mathing poess. The mathing poblem is solved by two ules: the losest point ule and the mathing ange ule. On the ontay, Co Algoithm etats lines fom efeene san and subsequently mathes data points of uent san to these lines. Finally, the Histogam san mathing defines an angle and -y histogams fom both sans and ompae them using a oss oelation funtion. The ovaiane mati of the obot displaement estimate is dietly povided by Gutmann s implementation, whih uses the same method desibed in ef. [10]. This was one of the easons to use this method in the poposed san mathing algoithm (Setion 2.3). Thus, the displaement estimate eo of the poposed appoah is easily ompaed with the othe san mathing algoithms of this ompaative study. Figue 8 shows the esults of the poposed method fo the fist test aea. The epeiment was made up of 325 onseutive sans whee the obot moves along appoimately 60 metes. The olleted data was analysed by the diffeent san mathing algoithms. As is illustated in Fig. 8a-d, IDC, Co and the poposed algoithm have simila esults, although Co algoithm pesents a slight oientation eo. On the othe hand, Histogam method is Downloaded fom Univesidad de Etemadua.Bibliotea, on 20 Feb 2017 at 11:13:05, subjet to the Cambidge Coe tems of use, available at

9 Fast lase san mathing appoah fo mobile obots 477 Table II. Auay and aveage time ompaison of san mathing algoithms in the fist test aea (Fig. 8). p t=end Covaiane mati Algoithm Runtime t (ms) (mm) y (mm) θ (ad) σ 2 (mm2 ) σ 2 y (mm2 ) σ 2 θ (ad2 ) Poposed method IDC Co Histogam not an auate method in this test aea and the obot pose estimate suffes fom abupt eos. Table II ompaes the auay of the appoah and the aveage time needed to alulate the oveall tajetoies fom the stati envionment desibed in this setion. In this table, p = (, y, θ ) T desibes the final obot pose estimate, σ 2, σ 2 y and σ θ 2 epesent the aveage values of the main diagonal of the ovaiane mati assoiated to the obot displaement estimate. Table III desibes the maimum values of the main diagonal ovaiane mati along the obot tajetoy, σ 2, ma σ y 2 and σ 2 ma θ. Tables II and III allow to ompae the ma auay of the poposed method with espet to the lassi san mathing algoithms fo this fist epeiment. Finally, Fig. 8a-d shows the esulting map povided by eah one of the san mathing algoithms. The seond epeiment pesents people moving though the obot tajetoies. The obot moves appoimately 80 m whee it aquies 525 onseutive sans. Results of the poposed appoah fo this test aea ae shown in Fig. 9. Thee ae not additional oetive algoithms in this epeiment and the esults ae obtained dietly by the poposed method. It must be noted that dynami objets ae also epesented in the figue (gey points), but this infomation has been oetly disaded by the poposed algoithm. Figue 9b illustates the tajetoies alulated by diffeent algoithms, inluding the poposed appoah. Table IV ompaes the aveage time needed to alulate the tajetoies fom the dynami envionment desibed in this setion. Besides, Table III. Maimum eos in the fist test aea (Fig. 8). Maimum eos Algoithm σ 2 ma (mm2 ) σ 2 y ma (mm2 ) σ 2 θ ma (ad2 ) Poposed method IDC Co Histogam Fig. 8. San mathing algoithms ompaative study: (a) Poposed Algoithm; (b) the IDC algoithm; () Co s algoithm; and (d) the histogam san mathing appoah. Downloaded fom Univesidad de Etemadua.Bibliotea, on 20 Feb 2017 at 11:13:05, subjet to the Cambidge Coe tems of use, available at

10 478 Fast lase san mathing appoah fo mobile obots Table IV. Auay and aveage time ompaison of san mathing algoithms in the fist test aea (Fig. 9). p t=end Covaiane mati Algoithm Runtime t (ms) (mm) y (mm) θ (ad) σ 2 (mm2 ) σ 2 y (mm2 ) σ 2 θ (ad2 ) Poposed method IDC Co Histogam Table V. Maimum eos in the seond test aea (Fig. 9). Maimum eos Algoithm σ 2 ma (mm2 ) σ 2 y ma (mm2 ) σ 2 θ ma (ad2 ) Poposed method IDC Co Histogam Table VI. Easiness of the implementation ompaative: Numbe of paametes needed to be tuned. Poposed method 7 IDC 4 Co 5 Histogam 2 τ Tables IV and V summaize the numeial esults of the auay of eah san mathing algoithm simila to Tables II and III. It an be appeiated that the poposed algoithm is omputationally faste and auate than the othe methods. This is due to the non-iteative natue of the algoithm and due to the fat that it uses a few but auate mathed points fo the alulation of the elative displaement. Robustness to dynami objets in the sene and possible oespondene eos is impoving in the desibed san mathing algoithm, as an be appeiated in Fig. 9b. Finally, Table VI ompaes the easiness of eah implementation (defined as the numbe of paametes in the algoithm needed to be tuned, τ). Classi IDC appoah and Co s algoithm employ as input paametes the maimum numbe of iteations and the minimum eo whih desibes the onvegene of the method (tanslation and otation minimum eos). Besides, both the methods inlude a set of filtes to impove the esults (a pojetion filte and a median filte, basially). Finally, Co s method uses a line etation method fo making the Co algoithm suitable fo the san mathing. On the othe hand, Histogam san mathing algoithm also inludes a line etation method fo eduing eos due to isolated peaks in the histogam assoiated to measuement eos. Then, it only needs to apply histogams to the onseutive sans in ode to estimate the displaement. In this ompaative study, only those paametes whih Fig. 9. San mathing algoithms ompaative study: (a) Resulting map obtained with the poposed algoithm in the seond test aea and (b) tajetoies omputed by algoithms applied in this ompaative study. (Squae: poposed method, ile: Co s solution, tiangle: IDC algoithm and sta: Histogam appoah.) desibe a oet opeation of the algoithms have been taken into aount. As was ommented in Setion 3.1, the designe has to selet values fo the distint san mathing steps in ode to obtain a eliable set of mathed points. Fo this eason, the poposed appoah needs to set moe paametes (τ = 7) than the othe appoahes. These paametes ae easy to design and thei values ae seleted aoding to the eplanation desibed in Setion 3.1. Howeve, thee ae some onsideations to be taken into aount, sine the suess of the poposed appoah depends on the numbe of Downloaded fom Univesidad de Etemadua.Bibliotea, on 20 Feb 2017 at 11:13:05, subjet to the Cambidge Coe tems of use, available at

11 Fast lase san mathing appoah fo mobile obots 479 points whih takes pat in the mathing poess and this value is dependent on some paametes (e.g. U p, n o S min ). The epeiments desibed in this setion demonstate that the oet seletion of these values leads to aeptable opeation in diffeent envionments. 4. Conlusions This pape poposes a new san mathing appoah whih omputes the tansfomation between the uent and the efeene sans in thee stages. Fistly, a segmentation stage is onduted to divide the whole san in a set of segments assoiated to physial entities of the obot suounding. Then, the uvatue funtion assoiated to eah segment is omputed aoding to a modified algoithm of the pevious wok pesented in ef. [13]. Cuvatue funtions ae invaiant with espet to otation and tanslation and they an be quikly omputed. Theefoe, the seond stage of the appoah mathes uvatue funtions of both sans poviding a global estimation of the san mathing. Net, the algoithm hooses haateisti points in oesponding uvatue funtions as featues to estimate the final obot pose shift. Finally, the use of a losed-fom solution to alulate the updating of the pose and the definition of paiwise points using uvatue funtions impove the omputational load of the omplete algoithm. Besides, the unetainty of the estimate obot displaement is povided in ode to test the auay of the appoah. We have implemented and tested the tehnique in seveal epeiments, in ode to measue the obustness, auay and omputational time of the whole san mathing algoithm. The appoah has been ompaed with othe lassi san mathing algoithms into eal dynami and stati envionments (IDC, Co and Histogam methods povided by Gutmann s implementation 7 ). Aoding to the ompaative study esults, the desibed appoah impoves these lassi san mathing algoithms woking in stati and dynami envionments in auay, obustness and time poessing. Futue woks ae foused on the implementation of a SLAM algoithm using the pose infomation estimate and on testing it in a dynami and unstutued envionment. Aknowledgements We would like to thank the Steffen Gutmann s san mathing implementations fo the ompaative study setion. The authos would also like to thank the anonymous efeees fo thei suggestions and insightful omments. This wok has been patially ganted by the Spanish Ministeio de Eduaión y Cienia (MEC) and FEDER funds, Pojet n. TIN and by the Junta de Andaluía Pojet n. P07-TIC Refeenes 1. A. Bandea, C. Udiales, F. Aebola and F. Sandoval, On-line Unsupevised Plana Shape Reognition based on Cuvatue Funtions, 24th Annual Confeene of the IEEE Industial Eletonis Soiety (IECON 98), Aahen, Gemany (1998), Vol. 3, pp O. Bengtsson and A. J. Baeveldt, Robot loalization based in san-mathing estimating the ovaiane mati fo the IDC algoithm, Robot. Autonom. Syst. 44(4), (2003). 3. J. L. Blano, J. A. Fenández-Madigal and J. A. González, Appoah fo Lage-Sale Loalization and Mapping: Hybid Meti-Topologial SLAM, in IEEE Intenational Confeene on Robotis and Automation, Rome Italy (2007) pp G. A. Boges and M. Aldon, Line etation in 2-D ange images fo mobile obotis, J. Intell. Robot. Syst. 40, (2004). 5. I. J. Co, Blanhe an epeiment in guidane and navigation of an autonomous obot vehile, IEEE Tans. Robot. Automation, 7 (2), (1991). 6. M. Dissanayake, P. Newman, S. Clak, H. Duant-Whyte and M. Csoba, A solution to the simultaneous loalisation and map building poblem, IEEE Tans. Robot. Automation 17 (3), (2001). 7. J. S. Gutmann and C. Shlegel, AMOS: Compaison of San Mathing Appoahes fo Self-Loalization in Indoo Envionments, in 1st Euomio Wokshop on Advaned Mobile Robots, Kaiseslauten, Gemany (1996) pp D. Hähnel, D. Fo, W. Bugad and S. Thun, A highly effiient FastSLAM algoithm fo geneating yli maps of lagesale envionments fom aw lase ange measuements, in IEEE/RSJ Intenational Confeene on Intelligent Robots and Systems, Las Vegas, Nevada, USA. (2003) pp K. Lingemann, A. Nuhte, J. Hetzbeg and H. Sumann, High-speed lase loalization fo mobile obots, Robot. Autonom. Syst. 51(4), (2005). 10. F. Lu and E. E. Milios, Robot pose estimation in unknown envionments by mathing 2-D ange sans, Tehnial Repot No. RBCV-TR-94-46, pp Univesity of Toonto, Toonto (1994). 11. M. Montemelo, FastSLAM: A Fatoed Solution to the Simultaneous Loalization and Mapping Poblem with Unknown Data Assoiation Ph.D. Thesis (Canegie Mellon Univesity, 2003), Pittsbugh, Pensilvania, USA. 12. J. Nieto, T. Bailey and E. Nebot, Reusive san-mathing SLAM, Robot. Autonom. Syst. 55, (2007). 13. P. Núñez, R. Vázquez-Matín,J.C.delToo,A.Bandeaand F. Sandoval, Natual landmak etation fo mobile obot navigation based on an adaptive uvatue estimation, Robot. Autonom. Syst. 56(3), (2008). 14. S. Pfiste, Algoithms fo Mobile Robot Loalization and Mapping, Inopoating Detailed Noise Modeling and Multi- Sale Featue Etation Ph.D. Thesis (Califonia Institute of Tehnology, 2006), Pasadena, Califonia, USA. 15. C. C. Wang, C. Thope and S. Thun, Online simultaneous loalization and mapping with detetion and taking of moving objets: Theoy and esults fom a gound vehile in owded uban aeas, Poeedings of the IEEE Intenational Confeene on Robotis and Automation (ICRA), Taipei, Taiwan (2003) pp G. Weiss and E. Puttkame, A map based on lase sans without geometi intepetation, Intell. Autonom. Syst (1995). Downloaded fom Univesidad de Etemadua.Bibliotea, on 20 Feb 2017 at 11:13:05, subjet to the Cambidge Coe tems of use, available at