Visual-based simultaneous localization and mapping and global positioning system correction

Transcription

1 Home Seach Collections Jounals About Contact us My IOPscience Visual-based simultaneous localization and mapping and global positioning system coection fo geo-localization of a mobile obot This aticle has been downloaded fom IOPscience. Please scoll down to see the full text aticle Meas. Sci. Technol ( View the table of contents fo this issue, o go to the jounal homepage fo moe Download details: IP Addess: The aticle was downloaded on 21/06/2012 at 13:17 Please note that tems and conditions apply.

2 IOP PUBLISHING Meas. Sci. Technol. 22 (2011) (9pp) MEASUREMENT SCIENCE AND TECHNOLOGY doi: / /22/12/ Visual-based simultaneous localization and mapping and global positioning system coection fo geo-localization of a mobile obot Sid Ahmed Beabah 1,2, Hichem Sahli 2 and Yvan Baudoin 1 1 Royal Militay Academy of Belgium (RMA), Av. de la Renaissance 30, B1000 Bussels, Belgium 2 Vije Univesiteit Bussel (VUB), Pleinlaan 2, 1050 Bussels, Belgium sidahmed.beabah@ma.ac.be Received 15 Febuay 2011, in final fom 10 August 2011 Published 15 Novembe 2011 Online at stacks.iop.og/mst/22/ Abstact This pape intoduces an appoach combining visual-based simultaneous localization and mapping (V-SLAM) and global positioning system (GPS) coection fo accuate multi-senso localization of an outdoo mobile obot in geo-efeenced maps. The poposed famewok combines two extended Kalman filtes (EKF); the fist one, efeed to as the integation filte, is dedicated to the impovement of the GPS localization based on data fom an inetial navigation system and wheels encodes. The second EKF implements the V-SLAM pocess. The linea and angula velocities in the dynamic model of the V-SLAM EKF filte ae given by the GPS/INS/Encodes integation filte. On the othe hand, the output of the V-SLAM EKF filte is used to update the dynamics estimation in the integation filte and theefoe the geo-efeenced localization. This solution inceases the accuacy and the obustness of the positioning duing GPS outage and allows SLAM in less featued envionments. Keywods: geo-localization, simultaneous localization and mapping (Some figues in this aticle ae in colou only in the electonic vesion) 1. Intoduction To be able to navigate in its envionment, a mobile obot is equied to infe its cuent position in elation to the outside wold using onboad sensoy eadings. Fo outdoo applications, the global positioning system (GPS) could be used to compute the obot s position in a geo-efeenced map of the envionment. Howeve, it is well known that GPS systems ae subject to seveal souces of eos, among them, ionosphee and toposphee delays, signal multi-path, numbe of visible satellites, satellite geomety/shading, etc. A typical GPS eceive, fo civil applications, povides 6 12 m accuacy, depending on the numbe of available satellites. This accuacy can be educed to 1 m when using a diffeential GPS (DGPS) system which employs a second eceive at a fixed location to compute coections to the GPS satellite measuements. Seveal solutions have been poposed in the liteatue to incease the accuacy of GPS localization by integating data fom othe sensos. In paticula, inetial navigation systems (INS) [1 3] and/o wheel encodes [4, 5] have often been used. This integation usually makes use of a Kalman filte (KF). Based on an eo model of the diffeent navigation system paametes, a KF solution may be capable of poviding a eliable estimate of the position, velocity and attitude components of the moving platfom [6]. Such a solution fo the integation of GPS and INS has been successfully used in pactice. Howeve, the accuacy of these systems deceases dastically duing long outage of the GPS eceive. On the othe hand, fo local navigation in unknown outdoo envionments, simultaneous localization and mapping (SLAM) techniques have been developed allowing obots to build up a map of thei envionment while at the same time keeping a tack of thei cuent location [8, 11] /11/ $ IOP Publishing Ltd Pinted in the UK & the USA

3 Combining GPS and SLAM has also been addessed in the liteatue. Lee et al [23] used the GPS and digital oad map infomation as pio constaints to aid thei SLAM algoithm in data association and loop closue. A simila idea was intoduced in [24], whee the authos built a steeo cameabased topological/metic hieachical SLAM fo vehicle localization in uban envionments. When available, the data fom GPS ae fused with the visual estimation using a Kalman filte. Yang et al [26] developed a SLAM-aided GPS/INS navigation system. In thei algoithm, if the GPS infomation is available, the SLAM-aided system woks in the way of INS/GPS, and at the same time, online building and updating the landmak-based map using INS/GPS solution. If the GPS data ae not available, the geneated map is used to constain the INS eos. In [26], Asma intoduced the VisSLAM appoach combining vision and INS using an EKF filte. In this study, we popose a diffeent multi-sensobased famewok combining visual SLAM and integated GPS/INS/Encodes filteing fo outdoo obot localization. The filtes ae combined in a feedback loop. Compaed to [24 26], the poposed method coects the GPS measuement (using the SLAM output and INS and encodes data) befoe using it to localize the built map and the SLAM estimation is helped by the integation filte. The following sections give a detailed desciption of the poposed famewok and discuss the obtained esults fo each pat of the algoithm. SABeabahet al 2. Global algoithm In this wok, the used obot is the ROBUDEM obot (figue 1), equipped with a camea, a GPS, an INS and wheel encodes. We define a local global coodinate system G (figue 1) fomed fom a plane tangent to the Eath s suface and fixed to a specific location with known geodetic coodinates (in ou case, it is supposed to be the initial obot position fo a null initialization of the covaiance matix in the SLAM pocess, see section 4). The X axis points towad the east, the Y axis points towad the noth and the Z axis points vetically upwad. We also define an inetial coodinate fame L elated to the INS senso, a coodinate system C fo the camea and a platfom (obot) fame R (figue 1). The axes of these fames ae paallel with the XY plane paallel to the gound and the X axis points towad the obot diection. The Z axis points vetically upwad. Fo geo-localization, a conventional coodinate fame called Eath-centeed Eathfixed (ECEF o ECF) is used. This fame has its oigin at the cente of the Eath (figue 1). The X axis passes though the equato at the pime meidian. The Z axis passes though the Noth Pole. The Y axis can be detemined by the ight-hand ule to be passing though the equato at 90 longitude. The geodetic coodinates expessed in tems of latitude, longitude Ɣ and altitude can be conveted into ECEF coodinates (x E,y E,z E ) using the following fomulas: x E = ג) + ) cos( ) cos(ɣ) y E = ג) + ) cos( ) sin(ɣ) (1) z E = 1 )ג) e 2 ) + ) sin( ), Figue 1. The ROBUDEM obot and coodinate fames. whee ג = a/ (1 e 2 sin 2 ( )) is the distance fom the suface to the Z axis along the ellipsoid nomal. a and e 2 ae the semi-majo axis and the squae of the fist numeical eccenticity of the ellipsoid, espectively (a = m and e 2 = ). The convesion of the coodinates of a location (x E,y E,z E ) in the ECEF fame to the coodinates (x G,y G,z G ) in the local global coodinate G fixed to a location O with geodetic coodinates (, Ɣ, ) and the ECEF coodinates (xo E,yE O,zE O ) is computed by x G s(ɣ O ) c(ɣ O ) 0 y G = s( O )c(ɣ O ) s( O )s(ɣ O ) c( O ) z G c( O )c(ɣ O ) c( O )s(ɣ O ) s( O ) x E xo E y E yo E, (2) z E zo E whee c( ) and s( ) stand fo cos( ) and sin( ), espectively. In ou application all measuements and computations ae tansfomed into the G coodinate system. Fo simplicity, the subscipt G is omitted in the following. 2

4 The state vecto of the obot x is defined with the 3D position vecto = (x,y,z ) in the wold fame coodinates and the obot s oientations yaw, oll and pitch (ω,θ,ϕ ): x y x = z ω. The dynamic model o motion model is the elationship between the obot s past state, x t 1, and its cuent state, x t, given a contol input u t : x t = f( x t 1,u t, v t), (3) whee f is a function epesenting the mobility, kinematics and dynamics of the obot (tansition function) and v is a andom vecto descibing the unmodeled aspects of the vehicle (pocess noise such as wheel sleep o odomety eo). The system dynamic model of the obot, consideing the contol u as identity, is given by x t 1 + (v t 1 + V)cos ( ) ω t 1 t x t = x t y t z t ω t θ t ϕ t = y t 1 θ ϕ + (v t 1 + V)sin ( ω t 1 ω t 1 z t 1 + ( ω t 1 θ t 1 ϕ t 1 ) t + ), (4) t whee v and ω ae the linea and the angula velocities, espectively, and V and ae the Gaussian distibuted petubations to the obot s linea and angula velocity, espectively. Figue 2 illustates the poposed famewok fo obot localization, whee two extended Kalman filtes (EKF) ae combined in a feedback loop. The fist EKF, efeed to as integation EKF (EKF-I), is dedicated to the impovement of the GPS localization based on data fom an inetial navigation system (INS) and wheels encodes. The second EKF implements the V-SLAM pocess. Both EKFs exchange motion paametes fo bette localization. The built V-SLAM algoithm uses an EKF to epesent a visual featue-based map. The linea ν and angula ω velocities in the dynamic model of the V-SLAM algoithm ae given by the GPS/INS/Encodes integation pocess. On the othe hand, the output of the V-SLAM is used to update the dynamics estimation in the EKF-I, and theefoe the geoefeenced localization. The poposed solution will allow inceasing accuacy and obustness of the positioning duing GPS outage as well as using fewe featues fo the V-SLAM. 3. GPS/INS/Encodes integation The GPS measuements ae called pseudo-anges (instead of anges) since the estimated times of tansmission ae coupted SABeabahet al Figue 2. The poposed famewok fo obot localization. by diffeent biases. The positioning equations fo n s satellites in sight at time instant t can be defined as (X i t t = i ) 2 ( xt + Y t i y) t 2 ( ) + Z t 2 i + b t + wi t (5) fo i = 1,...,n s, whee i t is the pseudo-ange between the GPS eceive and the ith satellite, [Xi t,yt i,zt i ]T is the position of the ith satellite, b t is the GPS eceive clock offset in metes, wi t is the measuement eo and [x,y ] is the vehicle position to be estimated (the vehicle altitude is z = 0 in ou application). The GPS clock offset dynamic model is defined by ḃ t = d t + ν t b, ḋt = ν t d (6) whee νb t and νt d ae the noise on GPS measuements. The inetial navigation system (INS) is a self-contained navigation technique in which measuements povided by acceleometes and gyoscopes ae used to tack the position and oientation of the obot elative to a known stating point, oientation and velocity. INS typically contains thee othogonal ate-gyoscopes and thee othogonal acceleometes, measuing angula velocity and linea acceleation, espectively. Usually, INS can only povide an accuate solution fo a shot peiod of time. As the acceleation is integated twice to obtain the position, any eo in the acceleation measuement will also be integated and will cause a bias on the estimated velocity and a continuous dift on the position estimate by the INS. The acceleometes delive a nongavitational acceleation (also efeed to as the specific foce f R ) and the gyometes measue the otation ate of the senso cluste LG in ode to keep tack of the vehicle oientation. 3

5 The diffeential equations elating the measued quantities to the dynamics ae defined as follows: v EG = R RG f R + g G ( EG +2 LE )v E ( LE ) 2 p G, (7) whee R RG is the otation matix fom the R fame to the local geogaphic fame W, LE is the otation ate fom the L fame to the E fame, EG is the otation ate fom the E fame to the G fame, v E is the velocity elative to the E fame and g G is the gavitational acceleation. The location p G of the vehicle in the G fame is given by ṗ G = ( Ɣ ) = 1 0 R 0 1 R Ɣ cos( ), (8) whee and Ɣ ae the latitude and longitude of the vehicle, R is the Eath s adius of cuvatue in the meidian and R Ɣ is the tansvese adius. The state model descibing the INS eo dynamics can be obtained by lineaizing equations (7) and (8) aound the INS estimation (see [22] fo moe details): δṗ G = S( EG ) δp G + δ v EG δ v EG = S(δρ) f R S( EG +2 LE ) δv EG + δg G S(δ EG +2 LE ) v EG δ ρ = δ LE S( LG ) δρ + R RG δ LE, (9) whee S( ) is the skew-symmetic matix and denotes the coss poduct. The linea speed v and the yaw ate (angula velocity) ω of the vehicle at time t can be computed based on the wheel encodes as follows: v = αt R + αl tr l 2 ω = αt R + αl tr l, L l (10) whee α t and αt l ae the angula velocities of the ight and left ea wheels, espectively, and R and R l thei coesponding adii. L l is the distance between ea wheels. The EKF-I filte pediction is done using equation (9) to estimate the state vecto composed of the vehicle position x, the linea and angula velocities, and the GPS bias tem x EKF-I = [x,p G,v, ω, b, d] T. Fo the update of the filte, the linea and angula velocities ae estimated as an aveage between the senso measuements and the V-SLAM estimations (as descibed in the following section). 4. Visual SLAM fo localization 4.1. Visual SLAM fomulation The SLAM poblem is tackled as a stochastic poblem and it has been addessed with appoaches based on Bayesian filteing. The most well-known Bayesian filtes fo teating the SLAM poblem ae (i) the extended Kalman filte (EKF) [7 9] whee the belief is epesented by a Gaussian distibution, and SABeabahet al Figue 3. Featues detected in a scene with moving objects. (ii) the paticle filtes [10, 11] whee the belief is epesented by multiple values (paticles). Wheneve a landmak is obseved by the obot s on-boad sensos, the system detemines whethe it has been aleady egisteed and updates the filte. Usually the featues used in vision-based localization algoithms ae salient and distinctive objects ae detected fom images. Typical featues might include egions, edges, object contous, cones, etc. In ou wok, the map featues ae obtained using the SIFT featue detecto [12]. These featues ae invaiant to image scale, otation and change in illumination [13]. To deal with the poblem of SLAM in dynamic scenes with a moving object, we use a peviously developed motion segmentation algoithm [14] to emove outlie featues which ae associated with moving objects. In othe wods, the detected featues which coespond to the moving pats in the scene ae not consideed in the built map. The appoach in [14] uses a Gaussian mixtue model backgound subtaction appoach to detect the moving objects mask and a Makov andom field famewok to optimize the detected masks based on the space and time dependences that moving objects impose on a fame pixel. The algoithm stats by estimating and compensating the camea motion. In anothe pape, we will show how we exploit the estimated 3D motion in the SLAM pocess fo the 2D camea motion compensation. Fo moe details, the eade is efeed to [14]. To deal with the eliability of the detected and tacked featues, we use a bounding box aound the moving objects (figue 3), and the newly detected featues should be detected and matched in at least j consecutive fames (in ou application, j = 5) befoe being added to the featues map. Featues ae epesented in the system state vecto by thei 3D location in the local wold coodinate system G: m i = (m 1,i,m 2,i,m 3,i ) T. The obsevation model of the EKF-SLAM is given by z t = [ z1 t T 2],zt = h(m t ) + w t, (11) whee z t is the obsevation vecto at time t and h is the obsevation model. The vecto z t i is an obsevation at instant t of the ith landmak location m t i elative to the obot s location 4

6 x t. Using a pespective pojection, the obsevation model in the obot coodinate system is obtained as follows: o x + f mt R 1,i z t i = h( m t m i) = t R 3,i o y + f mt R 2,i, (12) m t R 3,i whee o x and o y ae the image cente coodinates and f is the focal length of the camea. m R i = ( m R T 1,i,mR 2,i 3,i),mR ae the coodinates of the featue i in the obot coodinate fame R. They ae elated to m i by cos(ω ) sin(ω ) 0 m t m R 1,i x i = sin(ω ) cos(ω ) 0 m t 2,i y, (13) m t 3,i h whee h is the height of the camea. In EKF-based SLAM appoaches, the envionment is epesented by a stochastic map M = (x, P), whee x is the estimated state vecto, consisting of the n states epesenting the obot, x t, and the n states descibing the obseved landmaks, m t i, i = 1,...,n, and P is the estimated covaiance matix, whee all the coelations between the elements of the state vecto ae defined: x t x t m t 1 =. m t n P t P t 1 P t n P t P t 1 P t 11 P t 1n = (14) P t n P t n1 P t nn The sub-matices P t, Pt i and Pt ii ae, espectively, the obot to obot, obot to featue, and featue to featue covaiances. The sub-matices P t ij ae the featue to featue coss-coelations. x and P will change in dimension as featues ae added o deleted fom the map. The extended Kalman filte consists of two steps: (a) The pediction step (equations (15)), which estimates the system state accoding to the state tansition function f (equation (4)) and the covaiance matix P to eflect the incease in uncetainty in the state due to noise Q (unmodeled aspects of the system). The linea v and the angula ω velocities ae estimated by the GPS/INS/Encode integation KEF filte: x t t 1 =,u= 0) m t 1 t 1 1,... f(x t 1 t 1 P t t 1 = FP t 1 t 1 F T + Q t 1 (15) whee F = f x x t 1 t 1 = diag( f x t 1 t 1 x,i ) is the Jacobian of f with espect to the state vecto x and Q is the pocess noise covaiance. SABeabahet al Consideing a constant velocity model fo the smooth camea motion: 1 0 sin ( ) ω f t 1 (v t 1 + V) t x = 0 1 cos ( ) ω t 1 t 1 t 1 (v t 1 + V) t. x (16) (b) The update step uses the cuent measuement to impove the estimated state, and theefoe the uncetainty epesented by P is educed: whee x t t = x t t 1 + W t ε t P t t = P t t 1 W t S t W t T, W t = P t t 1 H T (S t ) 1 S t = HP t t 1 H + U t ε = z t h(x t t 1 ). (17) (18) Q and U ae block-diagonal matices (obtained empiically) defining the eo covaiance matices chaacteizing the noise in the model and the obsevations, espectively. H is the Jacobian of the measuement model h with espect to the state vecto. A measuement of featue m i is not elated to the measuement of any othe featue so [ ] h i x = hi h i 0 0, x m i whee h i is the measuement model fo the ith featue Initialization Seveal appoaches have been poposed fo the estimation of the initial state of the EKF-SLAM. Deans [15] combined Kalman filte and bundle adjustment in filte initialization, obtaining accuate esults at the expense of inceasing filte complexity. In [8], Davison uses an A4 piece of pape as a landmak to ecove the metic infomation of the scene. Then, wheneve a scene featue is obseved, a set of depth hypotheses ae made along its diection. In subsequent steps, the same featue is seen fom diffeent positions educing the numbe of hypotheses and leading to accuate landmak pose estimation. Solà et al [16] poposed a 3D beaing-only SLAM algoithm based on EKF filtes, in which each featue is epesented by a sum of Gaussians. In ou application, to estimate the 3D position of the detected featues, we use an appoach based on epipola geomety. This geomety epesents the geometic elationship between multiple viewpoints of a igid body and it depends on the intenal paametes and elative positions of the camea. The essence of the epipola geomety is illustated in figue 4 [24]. The fundamental matix F (a 3 3 matix of ank 2) encapsulates this intinsic geomety. It descibes the elationship between matching points: if a landmak M is imaged as m in the fist view, and m in the second, then the image points satisfy the elation m T Fm = 0 called the 5

7 SABeabahet al (a) Figue 4. Illustation of the epipola geomety. epipola constaint. m lies on the epipola line Fm and so the two ays back-pojected fom image points m and m lie in a common epipola plane. Since they lie in the same plane, they will intesect at one point. This point is the econstucted 3D scene point M. Analytically, the depth of the 3D point coesponding to x and x can be calculated by the following equation: z = (e m)(m m ) (19) m m 2 whee e is the epipole at the fist view satisfying the elation Fe = 0. The fundamental matix F is independent of scene stuctue and can be computed fom coespondences of imaged scene points, without equiing knowledge of the cameas intenal paametes o elative pose. Given a set of n pais of image coespondences (m j, m j ), j = 1,...,n, we compute the otation matix R between the two views and tanslation vecto t such that the epipola eo (equation (20)) is minimized. Fo the minimization, we use the andom sample Cconsensus (RANSAC) algoithm [21]: min F 4.3. Featue matching n m j Fm j. (20) j=1 At step t, the onboad senso obtains a set of measuements z t i (i = 1,...,k) of k envionment featues. Featue matching coesponds to data association, also known as the coespondence poblem, which consists in detemining the oigin of each measuement, in tems of the map featues m j, j = 1,...,n. In ou implementation, the measuement z t i can be consideed coesponding to the featue j if the following equation is satisfied: D 2 = Dij 2 + D2 desc + D2 epi <th (21) whee Dij 2 is the Mahalanobis distance between the new detected featue i and the map featues j, Ddesc 2 is the Euclidean distance between the descipto vectos of the featues i and j, and Depi 2 is the distance of the featue i fom the epipola line induced by the featue j. Figue 5 illustates the effectiveness and accuacy of the poposed appoach fo featue matching given by equation (21) (figue 5(c)), compaed to othe techniques using matching (b) (c) Figue 5. SIFT featue matching. (a) Featue matching based on the Mahalanobis distance with consistency hypothesis. (b) Featue matching based on the Euclidean distance between featue desciptos. (c) Featue matching based on equation (21). based on Mahalanobis distance with consistency hypothesis (figue 5(a)) and matching based on Euclidean distance between featue desciptos (figue 5(b)) SLAM in lage-scale aeas One of the poblems of the cuent state-of-the-at SLAM appoaches and paticulaly vision-based appoaches is mapping lage-scale aeas. Relevant shotcomings of this poblem ae, on the one hand, the computational buden, which limits the applicability of the EKF-based SLAM in lage-scale eal time applications and, on the othe hand, the use of lineaized solutions which compomises the consistency of the estimation pocess. The computational complexity of the EKF stems fom the fact that the covaiance matix P epesents evey paiwise coelation between the state vaiables. Incopoating an obsevation of a single featue will necessaily have an effect on evey othe state vaiable. This makes the EKF computationally infeasible fo SLAM in lage envionment. Methods like netwok coupled featue maps [17], sequential map joining [18] and the constained local submap filte (CRSF) [19] have been poposed to solve the poblem of SLAM in lage spaces by beaking the global map into submaps. This leads to a spase desciption of the coelations between map elements. When the obot moves out of one 6

8 submap, it eithe ceates a new submap o elocates itself in a peviously defined submap. By limiting the size of the local map, this opeation is constant time pe step. Local maps ae joined peiodically into a global absolute map in an O(N 2 ) step. Each appoach educes the computational equiement of incopoating an obsevation to constant time. Howeve, these computational gains come at the cost of slowing down the oveall ate of convegence. The constained elative submap filte [19] poposes to maintain the local map stuctue. Each map contains links to othe neighboing maps, foming a tee stuctue (whee loops cannot be epesented). The method conveges by evisiting the local maps and updating the links though coelations. On the othe side, in the hieachical SLAM [20], the links between local maps fom an adjacency gaph. This method allows us to educe the computational time and memoy equiements and to obtain accuate metic maps of lage envionments in eal time. To solve the poblem of SLAM in lage spaces, in ou study, we popose a pocedue to beak the global map into submaps by building a global epesentation of the envionment based on seveal size-limited local maps built using the peviously descibed appoach. The global map is a set of obot positions whee new local maps stated (i.e. the base efeences of the local maps). The base fame fo the global map is the obot position at instant t 0. Each local map is built as follows: at a given instant t k, a new map is initialized using the cuent vehicle location, x t k, as the base efeence B k = x t k, k = 0, 1,... being the local map ode. Then, the vehicle pefoms a limited motion acquiing senso infomation about the L i neighboing envionment featues. The kth local map is defined by M k = (x k, P k ), whee x k is the state vecto in the base efeence B k of the L k detected featues and P k is thei covaiance matix estimated in B k. The decision to stat building a new local map at an instant t k is based on two citeia: the numbe of featues in the cuent local map and the scene cut detection esult. The instant t k is called a key instant. In ou application, we defined two thesholds fo the numbe of featues in the local maps: a lowe Th and a highe Th + theshold. A key instant is selected if the numbe of featues n k in the cuent local map k is bigge than the lowe theshold and a scene cut has been detected o the numbe of featues has eached the highe theshold. This allows keeping easonable dimensions of the local maps and avoiding building too small maps. Fomally, the global map is defined as M B G = ( x 0, x1, x2,...), whee x k ae the obot coodinates in B 0, whee it decides to build the local map M k at instant t k : ) ) k ( x x k = T 1 k 0.( 1 t 0 = 0 and x 0 = xt 0 = (0, 0, 0). (22) Figue 6. Closing the loop. SABeabahet al The tansfomation matix T k 0 is obtained by successive tansfomations: T k 0 = T 1 0.T T k k 1, (23) whee T i i 1 = (R t) is the tansfomation matix coesponding to the otation R and tanslation t between B i and B i 1 : cos ( ) ω t i sin ( ) ω T i i 1 = t i sin ( ) ω t i cos ( ) ω t i 0 x t i 0 y t i (24) Fo featue matching at instant t, the obot uses the local map with the closest base fame to its cuent location: k ag min ( x x t ), (25) i whee x t is the obot position at instant t in B 0. The local maps ae consideed as nodes in a topological epesentation. Based on its cuent position, the obot selects the local map on which the featue matching will be done. If a matching is detected the two local maps ae fused in one local map. Since the elative efeence fames of both maps ae known, the main goal of the algoithm is to tansfom one of the maps and its featues into the efeence system of the othe one: M i+j = (x i+j, P i+j ), (26) whee x i+j and P i+j epesent the state vecto and the covaiance esultant of the fusion of the maps M i and M j in the efeence fame of the map M i. 5. Expeimental esults Figue 6 shows an example fo the detection of loops using the SLAM pocess. In this example, the obot wandes twice acoss a defined path (eal path dawn in ed in the figue). At the fist ound, the position eo exceeds 5 m and the uncetainty aound the obot position eaches 6.2 m (cyan ellipses in the figue). Afte loop detection, the uncetainty is 7

9 SABeabahet al Figue 7. Robot position eos and the coesponding 2σ vaiance bounds. (a) (b) Figue 9. Robot localization in a eal envionment. (a) Robot path supeimposed on a geo efeenced map. (b) The built local maps. Figue 8. Robot localization in the case of GPS outage. educed. Duing the second ound, the position eo is limited to a maximum of 1 m and the uncetainty to a maximum of 2 m (magenta ellipses in the figue) befoe the detection of the closue of the loop. Figue 7 epesents the obot position eo and its coesponding 2σ vaiance bounds obtained by the poposed algoithm. Position eos ae plotted as x and y distances of the obot location. The poposed famewok has been tested in eal envionments. Diffeent GPS outages wee simulated and analyzed. Figue 8 illustates an example whee the GPS signal was lost fo 30 s. The black cuve shows that the GPS localization eo is 3.6 m. The dashed blue cuve shows that even if the integation of GPS, INS and wheels encodes data educes the eo on the obot position to less than 1 m, it is not eliable duing the GPS outage whee the eo gows continuously, while the poposed famewok combining GPS/INS/Encodes localization and visual SLAM localization emains stable even duing the GPS outage (ed cuve). Figue 9 shows an example of the obot localization in a eal envionment. The blue/light cuve epesents the obtained obot path using the poposed algoithm, and the ed/dak cuve is the GPS data. The initial GPS position is the mean of the GPS measuements duing a few minutes of initialization. Figue 9(b) epesents the built local maps. Each local map is epesented by a diffeent colo. In this expeiment, the maximum numbe of featues in the local maps is fixed to 60 featues. 6. Conclusion In this pape, we pesented an algoithm fo obot localization in geoefeenced images. The poposed algoithm combines two localization techniques, one based on a GPS/INS/Wheel encodes integation appoach and the othe based on a visual SLAM appoach. The obtained esults ae inteesting and fo a futue wok we want to constain the featue matching fo the closue of the loop based on global positioning and study the influence of the GPS outage on the closing of the loop. Refeences [1] Wolf R, Eissfelle B and Hein G W 1997 A Kalman filte fo the integation of a low cost INS and an attitude GPS Poc. Int. Symp. on Kinematic Systems in Geodesy, Geomatics and Navigation pp [2] Shin E H 2001 Accuacy impovement of low cost INS/GPS fo land applications MSc Thesis Depatment of Geomatics Engineeing, Univesity of Calgay [3] Abdel-Hamid W 2005 Accuacy enhancement of integated MEMS IMU/GPS systems fo land vehicula navigation applications PhD Thesis Depatment of Geomatics Engineeing, Univesity of Calgay [4] Spangenbeg M, Calmettes V and Touneet J-Y 2007 Fusion of GPS, INS and odometic data fo automotive navigation 15th Euopean Signal Pocessing Conf. (EUSIPCO 2007) (Poznan, Poland, 3 7 Septembe 2007) pp

10 [5] Gao J, Petovello M G and Cannon M E 2006 Development of pecise GPS/INS/wheel speed senso/yaw ate senso integated vehicula positioning system Poc. ION NTM-06 (Monteey, CA, Januay 2006) pp 1 13 [6] El-Sheimy N 2004 Inetial suveying and INS/GPS integation ENGO 623 Lectue Notes Geomatics Depatment, Univesity of Calgay [7] Folkesson J, Jensfelt P and Chistensen H 2005 Gaphical SLAM using vision and the measuement subspace IEEE/JRS Int. Conf. on Intelligent Robotics and Systems (IROS) (Edmonton, August 2005) pp [8] Davison J, Reid I D, Molton N D and Stasse O 2007 MonoSLAM: eal-time single camea SLAM IEEE Tans. Patten Anal. Mach. Intell [9] Fenwick J W, Newman P M and Leonad J J 2002 Coopeative concuent mapping and localization IEEE Int. Conf. on Robotics and Automation (Washington) pp [10] Sim R, Elinas P, Giffin M and Little J J 2005 Vision-based SLAM using the Rao Blackwellised paticle filte Poc. IJCAI Wokshop on Reasoning with Uncetainty in Robotics pp 9 16 [11] Stachniss C, Gisetti G and Bugad W 2005 Recoveing paticle divesity in a Rao Blackwellized paticle filte fo slam afte actively closing loops IEEE Int. Conf. on Robotics and Automation pp [12] Lowe D G 2004 Distinctive image featues fom scale-invaiant keypoints Int. J. Comput. Vis [13] Mikolajczyk K and Schmid C 2003 A pefomance evaluation of local desciptos IEEE Comput. Sci. Conf. on Compute Vision and Patten Recognition, CVPR 03 vol 2 pp [14] Beabah S A, De Cubbe G, Enescu V and Sahli H 2006 MRF-based foegound detection in image sequences fom a moving camea Poc. Int. Conf. on Image Pocessing, ICIP2006 (Atlanta, GA, 8 11 Octobe 2006) pp [15] Deans M and Hebet M 2000 Expeimental compaison of techniques fo localization and mapping using a beaing-only senso 7th Int. Symp. on Expeimental SABeabahet al Robotics (Honolulu, HI, Decembe 2000) vol 271 pp [16] Solà J, Lemaie T, Devy M, Lacoix S and Monin A 2005 Delayed vs undelayed landmak initialization fo beaing only SLAM Poc. IEEE Int. Conf. on Robotics and Automation Wokshop on SLAM (Bacelona, Spain, Apil 2005) pp [17] Bailey I 2002 Mobile obot localization and mapping in extensive outdoo envionments PhD Thesis Austalian Cente fo Field Robotics, Univesity of Sydney, Austalia [18] Tados J D, Neia J, Newman P and Leonad J 2002 Robust mapping and localization in indoo envionments using sona data Int. J. Robot. Res [19] Williams S B 2001 Efficient solutions to autonomous mapping and navigation poblems PhD Thesis Austalian Cente fo Field Robotics, Univesity of Sydney, Austalia [20] Estada C, Neia J and Tados J D 2005 Hieachical SLAM: eal-time accuate mapping of lage envionments IEEE Tans. Robot [21] Nist D 2003 Peemptive RANSAC fo live stuctue and motion estimation Poc. IEEE Int. Conf. on Compute Vision (ICCV 2003) pp [22] Fael J A and Bath M 1999 The Global Positioning System and Inetial Navigation (New Yok: McGaw-Hill) [23] Lee K W, Wijesoma S and Guzman J I 2007 A constained SLAM appoach to obust and accuate localization of autonomous gound vehicles Robot. Auton. Syst [24] Schleiche D, Begasa L M, Ocaña M, Baea R and López E 2009 Real-time hieachical GPS aided visual SLAM on uban envionments 2009 IEEE Int. Conf. on Robotics and Automation (Kobe, Japan, May 2009) pp [25] Cao M-L and Cui P-Y 2007 Simultaneous localization and mapping aided INS/GPS navigation system J. Chin. Inetial Technol [26] Asma D 2006 Vision-inetial SLAM using natual featues in outdoo envionments PhD Thesis Univesity of Wateloo, Canada 9