Differential Evolution solution to the SLAM

problem

Luis Moreno, Santiago Garrido, Dolores Blanco

Department of Systems Engineering and Automation, Universidad Carlos III,Avda. de la Universidad 30 , 28911 Leganes, Madrid, Spain,e-mail:moreno@ing.uc3m.es,sgarrido@ing.uc3m.es

M. Luisa Munoz

Department of Computer’s Architecture, Universidad Politecnica de Madrid,Campus de Montegancedo, Facultad de Informatica, Madrid, Spain,

e-mail:mmunoz@fi.upm.es

Abstract

A new solution to the Simultaneous Localization and Modelling problem is presentedin this paper. The algorithm nucleus is based on the stochastic search for solutionsin the state space to the global localization problem by means of a differentialevolution algorithm. This non linear evolutive filter, called Evolutive LocalizationFilter (ELF), searches stochastically along the state space for the best robot poseestimate. The set of pose solutions (the population) focus on the most likely areasaccording to the perception and up to date motion information. The populationevolves using the log-likelihood of each candidate pose according to the observationand the motion errors derived from the comparison between observed and predicteddata obtained from the probabilistic perception and motion model.

The proposed SLAM algorithm operates in two steps: in the first step the ELFfilter is used at local level to re-localize the robot based on the robot odometry, thelaser scan at a given position and a local map where only a low number of the lastscans have been integrated. In the second step the aligned laser measures and thecorrected robot poses are used to detect when the robot is revisiting a previouslycrossed area (ie a cycle in the robot trajectory exists). Once a cycle is detected, theEvolutive Localization Filter is used again to estimate the accumulated residual driftin the detected loop and then to re-estimate the robot poses in order to integratethe sensor measures in the global map of the environment.

The algorithm has been tested in different environments to demonstrate the ef-fectiveness, robustness and computational efficiency of the proposed approach.

Preprint submitted to Elsevier Science 13 June 2008

1 Introduction

Localization and map building are key components in robot navigation andare required to successfully execute a path generated by a global planner. Bothproblems are closely linked, and learning maps required to solve simultaneouslyboth problems. These simultaneous problems are often referred as simultane-ous localization and mapping(SLAM). Building maps when robot poses areknown is a tractable problem with limited computational complexity becauseit is only necessary to manage the error associated to each individual sensormeasurement at modelling time. However, in the SLAM case, uncertainty inmeasures, uncertainty in robot pose estimates and a partially learned mapwhich contains the residual errors unsolved at integration or re-localizationprocesses makes the SLAM problem complex.

The two main approaches to effectively solve the simultaneous localization andmapping problem have been consolidates in the last decade. The first approachuses a feature-based model of the environment and the extended Kalman filterto manage the associated uncertainty. This approach is extremely compact,and its computational cost has been considerably improved in the most re-cent work. On the other hand, however, the linear nature of the basic methodrequires linearization of the motion and perception models which causes prob-lems in the long term. Moreover, the technique has difficulties modelling manyenvironment areas due to the limited set of feature models used.

The second group of solutions use particle filters to obtain a solution to theSLAM problem. This group of solutions can use certainty grid map modelsor a feature map to represent the environment [20], and the sequential MonteCarlo methods to estimate the posterior probability distribution functions.This approach has proved to be very robust from a statistical point of view inthe management of the uncertainties present in the problem. Its disadvantageis that the number of particles required increases the computational cost andthe algorithm robustness is heavily dependent on this because each particlehas a statistical significance associated with it.

In spite of the advances in stochastic search optimization methods in the lastdecade, they have received limited attention in the field of localization andSLAM problems. In this work we present a new solution to the grid-basedSLAM problem based on the stochastic search of the best pose estimate. Thisapproach uses a differential evolution method [17] to perturb the possible poseestimates contained in a given set until the optimum is obtained. By properlychoosing the cost function, a maximum a posteriori estimate is obtained. Thismethod is applied at local level to re-localize the robot and at global level tosolve the data association problem. The method proposed integrates sensorinformation in the map only when cycles are detected and the residual error

2

are eliminated, avoiding a high number of modifications in the map or theexistence of multiple maps, thus decreasing the computational cost comparedto other solutions.

Our approach has been validate on a set of data obtained from our experimentsand data extracted from the Radish repository data. The results show theproposed method is able to satisfactorily close environment cycles to generateaccurate metric maps.

The paper is organized as follows. A global evolutive localization filter (ELF)is formulated and explained in section 3. Next, section 4 introduces the ELFalgorithm details, Section 5 deals with the key aspects of the ELF algorithmapplication to the SLAM problem and the SLAM algorithm proposed. Thefollowing sections present the experimental results and discuss the advantagesand disadvantages of the algorithm.

2 State of the art

The SLAM problem has been one of the most interesting theoretical problemsin mobile robotics since the 90’s when the seminal work of Smith, Self andCheeseman [15] [16] introduced the concept of a stochastic map to establish un-certain spatial relationship between features detected in the environment. TheKalman filter provides the mechanism for integrating and updating the map.The stochastic map idea has been widely used in the last ten years but twoproblems have attracted the attention of many researchers: the convergenceand the scaling properties of this mapping method. With regard to the conver-gence of this solution there are some controversial aspects. If we assume thenoises are Gaussian and the observation and motion models can be linearizedalmost perfectly, Dissanayake’s work [5] has demonstrated three importantconvergence properties: first, ”the decreasing of the determinant of any sub-matrix of the map covariance matrix as observations are made successively”;second, ”the landmark estimates become fully correlated in the limit”; third,”the covariance associated with any single landmark pose estimate reaches alower bound determined only by the initial covariance in the vehicle locationestimate at the time of the first sighting of the first landmark”. On the otherhand, Julier and Uhlmann [8] have shown two important aspects: first that ”itis questionable whether the Kalman filter framework is as robust and rigor-ous a solution to the stochastic mapping of the environment”, and the secondthat ”the consistency of any map building algorithm cannot be fully assessedthrough experimental runs which are of short duration”. These conclusionsderive from the fact that in a general nonlinear system where the Jacobianmatrices are functions of noisy observations and erroneous estimates, even inthe case where all observation covariance are finite, the initial pose estimate is

3

initialized with a non-zero covariance and the state does not change, the stateestimate does not remain unchanged for all timesteps. The linearization prob-lem is structural and cannot be avoided. Castellanos et al. have also shown [3]that linearization errors produce inconsistency problems in the standard EKFsolution for SLAM. These problems can be reduced, but cannot be eliminatedin the long-term as the basic problem is nonlinear.

A second widely investigated aspect is the scaling properties of the stochas-tic map solution to the SLAM problem. The O(n2) complexity of the basicsolution (where n is the number of features in the map) has been a seriousbottleneck. Several researchers have developed techniques to alleviate the com-putational burden: the sparse extended information filters [19], the decoupledstochastic mapping [9], the compressed filter [7], the sequential map joining[18] and the constrained local submap filter [21] have improved the scalabilityproblem of the standard EKF-based version the SLAM problem. However, thelinearization problem and the data association problem ambiguities are notcompletely solved.

A third point rarely considered in literature is the inherent difficulty of feature-based models to map cluttered and irregular areas and to be used by othertasks in a mobile robot, particularly in planning and navigation activities. Thisnecessitates the introduction of additional software modules whose computa-tional cost is ignored. Feature-based representations are attractive because oftheir compactness but are difficult to use for path-planning purposes and areunable to model arbitrary features

A second group of SLAM algorithms are based on the occupancy grid map-ping method. Grid-based techniques are computationally expensive and havea larger memory requirement than EKF-based SLAM methods. This group ofsolutions uses the Rao-Blackwellized particle filters to estimate the posteriorprobability distribution functions [12] [11]. The algorithm complexity in fea-ture map implementations is O(M logK), where M is the number of particlesan K is the number of landmarks. When a grid-based map is used, the costof evaluating the map for each potential trajectory considerably increases thecomputational cost of the method. Rao-Blackwellized particle filters have of-fered an effective method of solving the data association problem for detectingloop closures. However, the main problem of this approach derives from thefact that each particle of the filter represents a different map of the environ-ment. This requires the obtaining of as many maps as particles being usedin the filter at each algorithm step. A second problem, common in Bayesianfilters, is the difficulty of estimating an adequate number of particles. If thenumber is low, the algorithm cannot estimate the posterior properly, and ifthe number is large the computational cost increases rapidly. This difficulty isusually solved through trial and error.

4

In this article we present an alternative solution to the SLAM problem thatavoids the EKF linearization problems of the perception and motion models,and the inability of feature-based maps to obtain a complete environmentmodel. It also avoids the computational cost problem in grid-based mappingwith Rao-Blackwellized approaches. The technique proposed uses a stochasticsearch optimization approach based on a solution perturbation method calleddifferential evolution to obtain the best pose estimate. This technique is usedat local level to re-localize the robot’s pose and at global to solve the dataassociation problem.

The advantage of our approach is twofold. First, the solution proposed buildsonly one certainty grip map, thus avoiding unnecessary operations. This mapis built at loop closing time, when the data association has been solved andthe residual error can be eliminated. Second, the computational efficiency ofthe method reduces the computational cost.

3 Localization Problem formulation and solution.

The SLAM solution we propose is based on the application at local or globalscale of a differential evolution search method to obtain optimal solutions tothe localization problem. In this section, the localization problem is formu-lated as an optimization problem, and the stochastic search method calledEvolutionary Localization Filter (ELF) developed.

From a Bayesian point of view, the localization problem can be formulated asa probability density estimation problem where the robot seeks to estimate aposterior distribution over the space of its poses conditioned on the availabledata. The sensor data can be divided into two groups of data Yt ≡ {z0:t, u1:t}where z0:t = {z0, . . . , zt} contains the perception sensor measurements andu1:t = {u1, . . . , ut} contains the odometric information. To estimate the pos-terior distribution p(xt|Yt), probabilistic approaches resort to the Markov as-sumption, which states that future states only depend on the knowledge of thecurrent state and not how the robot got there, i.e. they are independent ofpast states. The recursive determination of the posterior probability densitycan be computed in two steps:

• Measurement update. Applying the Bayes’s rule to the last element of themeasurement vector Yt and assuming that the observation zt is conditionallyindependent of the previous measurements given the state xt, yields

5

p(xt|Yt)=p(zt|xt, Yt−1)p(xt|Yt−1)

p(zt|Yt−1)

=p(zt|xt)p(xt|Yt−1)p(zt|Yt−1)

(1)

where the denominator of 1 is obtained by marginalization in equation 2.

p(zt|Yt−1) =∫

<np(zt|xt)p(xt|Yt−1)dxt (2)

• Prediction. The effect of a time step over the state given the observationsup to time t is obtained by observing that

p(xt+1|Yt)=∫

<np(xt+1|xt, ut, Yt)p(xt|Yt)dxt

=∫

<np(xt+1|xt, ut)p(xt|Yt)dxt (3)

where the assumption that the process xt is Markovian, and then xt+1 isindependent of Yt has been considered.

Equations 1, 2 and 3 are the solution to a recursive Bayesian estimation prob-lem. These equations integrate all available statistic and sensorial informationabout the system. In general, the multidimensional integrals in expressions2 and 3 have no explicit analytical solutions for nonlinear and non-Gaussianmodels. The posterior is a general statistical solution to the estimation prob-lem but is difficult to obtain and to manage for general non-linear and non-Gaussian problems. Each candidate parameter value in <n yields a value ofp(x|y), reflecting the posterior probability of the robot pose given the data upto time t. This posterior needs to be weighted according to a given criterion todetermine an estimate x of the true pose value. Two common choices of costfunction are the mean-square error and the maximum a posteriori estimators.The minimum mean-square estimate is defined by

xMS = argminx∗t

∫

<n(xt − x

∗t )T (xt − x

∗t )p(xt|Yt)dx (4)

It is well known that the optimal estimate in the mean-square sense is theconditional mean. This estimate is also referred to as the conditional meanestimate. Since the posterior probability distribution is multi-modal in theglobal localization problem, this estimate is inconvenient. Another commonchoice is the maximum a posteriori estimator,

xMAP = argmaxxp(xt|Yt) (5)

The solutions derived from the Bayesian approach concentrate on maintainingthe statistical coherency in the posterior probability distribution according toall received information up to a given moment. The localization algorithm used

6

concentrates on obtaining the better maximum a posteriori estimator. Thisapproach is less dependent on statistical assumptions, has a simpler imple-mentation, is robust from a statistical point of view and has a computationalcost lower than Bayesian methods.

3.1 Localization as a MAP optimization problem

The localization problem is basically an optimization problem, where the robotseeks to estimate the pose which maximizes the a posteriori probability den-sity.

xMAPt =argmaxxp(xt|Yt)

= argmaxxp(zt|xt, ut−1, Yt−1)p(xt|xt−1, ut−1, Yt−1)

= argmaxxp(zt|xt)p(xt|xt−1, ut−1)p(xt−1|Yt−1)

= argmaxx

t∏

i=1

p(zi|xi)t∏

i=1

p(xi|xi−1, ut−1)p(x0) (6)

The maximum a priori (MAP) estimate expression can be easily stated asan optimization problem subject to constraints (the motion and observationmodels of the robot). The MAP estimate is the solution xt to the followingproblem,

maxx

t∏

i=1

pe(zi|xi)t∏

i=1

pv(xi|xi−1, ut−1)p(x0)

subject to:

xt+1 = f(xt, ut) + vt, t = 0, 1, . . .

zt = h(xt) + et, t = 0, 1, . . . (7)

where pe express the probability density function for the observation noise e,and pv indicates the probability density function for the motion noise v. Wewill use the notation f0(x) to refer the objective function to maximize. Theproblem of finding an x that maximizes f0(x) among all x that satisfy theconditions xt+1 = f(xt, ut) + vt and zt = h(xt) + et is limited to finding theoptimal value between the set of all feasible solutions.

In general, the calculation of estimates for this optimization problem haveno explicit analytical solutions for nonlinear and non-Gaussian models. Thedifficulties are due to: non linearities in motion and perception models, envi-ronment symmetries and sensor limitations (observational symmetries). The

7

problem is highly simplified when the initial probability distribution is Gaus-sian because the problem becomes uni-modal and then it is possible to obtain,even analytically, an estimate (since the problem can be converted into aquadratic minimization problem if non linear motion and observation modelscan be approximated by a linear Taylor series expansion about the currentestimate xt). This situation lead to the well known Extended Kalman Filtersolution of the pose tracking problem.

3.2 Recursive formulation of the optimization problem

The MAP estimate formulated as an optimization problem subject to con-straints 7 is not practical from a computational point of view. To implementthe global localization algorithm in a robot, a recursive formulation is required.If we observe the objective function f0(xt), it can be reformulated in a moreconvenient form by taking logarithms and then expressed recursively in termsof f0(xt−1) in the following way:

f0(xt)=t∑

i=1

log pe(zi|xi) +t∑

i=1

log pv(xi|xi−1, ut−1) + log p(x0)

= log pe(zt|xt) + log pv(xt|xt−1, ut−1) + f0(xt−1) (8)

If we are able to solve the optimization problem at time t − 1, we have a setof m possible solutions {xt−1}1:m which satisfy the optimization problem upto time t− 1. The MAP optimization problem can be reformulated as

maxxlog pe(zt|xt) + log pv(xt|xt−1, ut−1) (9)

subject to conditions 7 and starting with {xt−1}1:m as previous feasible solu-tions set. Then, by perturbing and searching new solutions to 9, we obtain arecursive version of the MAP estimate. In the following section an evolutivealgorithm is proposed to obtain the evolutionary MAP estimate for the globallocalization problem according with the ideas introduced in this section.

4 Evolutive Localization Filter algorithm

The algorithm proposed to implement the evolutive localization filter is basedon the differential evolution method proposed by Storn and Price [17] for globaloptimization problems over continuous spaces, and Moreno [13] proposed us-ing it to solve the global localization problem by . The Evolutive Localization

8

Filter (ELF) uses a parallel direct stochastic search method which utilizes ndimensional parameter vectors xki = (x

ki,1, . . . , x

ki,n)T to point each candidate

solution i to the optimization problem at iteration k for a given time step t.This method utilizes N parameter vectors {xki ; i = 1, . . . , N} as a sub-optimalfeasible solutions set (population) for each generation t of the optimizationprocess. Depending on the kind of localization problem we are trying to solve,the initialization changes. When no information about the initial pose is avail-able, the initial population is chosen randomly to cover the entire parameterspace uniformly (the area where the solution is located). If the initial pose isknown, the initial population of possible solutions is obtained by distributingthe poses according to a Gaussian distribution around the initial pose.

4.1 Fitness function

According to the recursive optimization problem under consideration, the nat-ural choice for fitness function is the objective function.

f0(xt)= log pe(zt|xt) + log pv(xt|xt−1, ut−1) (10)

This expression contains the state transition probability density pv(xt|xt−1, ut−1)and the observation probability density pe(zt|xt), derived from the robot mo-tion model and the observation model. The observation probability pe(zt|xt)can be calculated by predicting the observation value of the noise-free sensorassuming the robot pose estimate is xt, the sensor relative angle with respectto the robot axis is αi, and a given environment model m, that in our case isestimated. Let zt,i = h(xt,m, αi) denote this ideal predicted measurement. Forpractical purposes, zt,i = h(xt, m, αi) is computed using a ray tracing method.Assuming the measurement error et,i is Gaussian, centered at h(xt, αi) andwith a σe standard deviation (that is et,i ≈ N(h(xt, m, αi), σe)), then theprobability of observing zt,i with sensor i can be expressed as,

pe(zt,i|xt) =1

(2πσ2e)1/2e−1/2

(zt,i−zt,i)2

σ2e (11)

And, assuming conditional independence between the individual measure-ments, the individual sensor beam probabilities are integrated into a singleprobability value:

pe(zt|xt)=Ns∏

i=0

p(zt,i|xt) =Ns∏

i=0

1

(2πσ2e)1/2e−1/2

(zt,i−zt,i)2

σ2e (12)

9

where Ns is the number of sensor observations.

The second probability required to calculate the objective function, pv(xt|xt−1, ut−1),can be obtained in two steps: the prediction of the noise free value of the robotpose xt = f(xt−1, ut−1) assuming the robot pose estimate is xt and the motioncommand at t is ut. Assuming the motion error is Gaussian with covariancematrix P (that is v ≈ N(f(xt−1, ut−1), P ), then the pv(xi|xi−1, ut−1) probabil-ity can be expressed as

pv(xi|xi−1, ut−1) =1

√|P |(2π)n

e−1/2(xi−xi)P−1(xi−xi)T (13)

Introducing the expressions of pv and pe into the objective function to optimizeat iteration t,

f0(xt)= logNs∏

i=0

(2πσ2e)−1/2e

−(zt,i−zt,i)

2

2σ2e

+ log(|P |(2π)n)−1/2e−12(xi−xi)P−1(xi−xi)T

=Ns∑

i=0

log(2πσ2e)−1/2 −

Ns∑

i=0

(zt,i − zt,i)2

2σ2e

+ log[(|P |(2π)n)−1/2]−1

2(xi − xi)P

−1(xi − xi)T (14)

which can be reduced to minimize the following function

f ′0(xt) =Ns∑

i=0

(zt,i − zt,i)2

2σ2e+1

2(xi − xi)P

−1(xi − xi)T (15)

The differential evolutive filter will minimize iteratively the fitness function15.

4.2 Evolutive Localization Filter algorithm

The Evolutive Localization Filter (ELF) uses n dimensional parameter vectorsxki = (x

ki,1, . . . , x

ki,n)T to point each candidate solution i to the optimization

problem at iteration k for a given time step t. The filter will generate new set ofparameter vectors by perturbing an existing vector through the addition of oneor more weighted difference vectors to it (see figure 1). If the resulting vectoryields a lower objective function value than the original unperturbed vector,the newly generated vector replaces the vector with which it was compared;

10

xt

xtr3

i

new parameter vector

v

F( - )xtr2 r3

xt

xtr2

minimum

Fig. 1. New population member generation.

otherwise, the old vector is retained. The perturbation scheme generates avariation v according to the following expression,

v = xki + F (xkr2− xkr3) (16)

where xki is the parameter vector to be perturbed at iteration k, xkr2and xkr3 are

parameter vectors chosen randomly from the population and are different fromrunning index i. F is a real and constant factor which control the amplificationof the differential variation (xkr2 − x

kr3).

To increase the diversity of the new generation of parameter vectors, a crossovermechanism is introduced. Denoted by uki = (u

ki,1, u

ki,2, . . . , u

ki,n)T the new pa-

rameter vector with

uki,j =

vki,j if pki,j < δ

xki,jotherwise

where pki,j is a randomly chosen value from the interval [0, 1] for each parameterj of the population member i at step k and δ is the crossover probability andconstitutes the crossover control variable. The random values pki,j are madeanew for each trial vector i.

To decide whether or not vector uki should become a member of generationi+1, the new vector is compared to xki . If vector u

ki yields a better value for the

objective fitness function than xki , it is replaced by uki for the new generation;

otherwise , the old value xki is retained for the new generation.

In accordance with the previous ideas, the basic evolutive localization filteralgorithm consists of the following steps:

• Step 1: Initialization.The initial set of solutions is calculated and the fitness value associated

11

with each of the points in the state space is evaluated. If no informationabout initial position is available, the initial set of pose solutions is obtainedby drawing the robot poses according to a uniform probability distributionover the state space. The initial robot pose estimate is fixed at an initialvalue.• Step 2: Evolutive search.∙ (a) For each element of the set of robot pose solutions and according to themap, the expected sensor observations are obtained zt,i = h(xt, αi,m), i =1, . . . , Ns. The expected observations, the sensor observations, the robotpose estimate and the robot pose element are used to evaluate the lossfunction for each robot pose element in the set of solutions.∙ (b) A new generation of perturbed pose solutions are generated accord-ing to the perturbation method described in the previous subsection. Foreach perturbed solution, the expected observations are calculated and theloss function evaluated. If the perturbed solution results in a better lossfunction, this perturbed solution is selected for the following iteration,otherwise the original is maintained.∙ (c) The crossover operator is applied to the resultant population (solutionsset).∙ (d) The robot pose element of the set with lower value of the loss functionis marked as best robot pose estimate. Go to step 2b a given number ofiterations.

• Step 3: Updating. The best robot pose element of the population is usedas the updated state estimate and then used in state transition model topredict the new state according to the odometry information. Next, thedisplacement is evaluated and the whole population is moved according tothis displacement. Then go to step 2.

5 Differential evolution approach to SLAM problem

In the simultaneous localization and mapping problem case, the map needs tobe estimated concurrently with the pose to generate the expected measure-ments. In figure 2, the data flow in SLAM is shown. Note how the SLAMproblem solution depends on the data association problem at local and globallevel.

The most extended Bayesian formulation for the SLAM problem is to estimatethe posterior probability of the trajectories, the map associated to it given theobservations z1:t and the odometry measurements u0:t:

p(x1:t,mt1|u0:t, z1:t) = p(m

t1|x0:t, z1:t)p(x1:t|z1:t, u0:t) (17)

In this expression 17, the posterior over maps p(m|x0:t, z1:t) can be easily com-

12

puted when x0:t and z1:t are known by means of a mapping process. Obtainingthe posterior over the trajectories p(x1:t|z1:t, u0:t) is much more complex. Thisapproach has shown to be effective but is computationally very expensive aseach pose sequence has an associated map which needs to be calculated tomake sensor predictions. At each iteration step the probability associated toaugmented states, including a candidate sequence and the map obtained byfusing the sensor data with this sequence, is calculated. The map associatedto a given sequence (an occupancy grid map in our case) is obtained by fusingthe observed data up to time t, z1:t according the estimated robot’s pose, x0:t,as mt0[x0:t, z1:t], where p(mij) expresses the occupancy probability of the celli, j in the map m according to the data received up to time t. For notationconvenience we express it as mt0[x0:t, z1:t] = m

t0.

mi^ mi+1

^ mi+n^

x i^ x i+1

^ x i+n^

z i+1 z i+n

u i+1 ui+n

z i+1

^

z i+n

^

Fig. 2. SLAM data flow

The method proposed in this paper is quite different in that it exploits theELF algorithm capability to operate at local and global level. The algorithmuses a two step approach. The first step exploits the local data coherency ideato partially eliminate the model inconsistency problem by using local modelsto re-localize the robot. Even if the global position is not correct these localmodels maintain the local data consistency. This local data coherency is usedto estimate the robot’s pose precisely but cannot completely eliminate thepose error, and the residual remaining error is accumulated over motion. Thisidea is in some sense similar to scan matching but instead of using the sensordata directly a local model is built and the robot is re-localized. This avoidsthe lack of robustness present in most scan matching approaches when thepose difference between scans is significant, particularly in orientation.

The second step of the algorithm exploits a different concept to avoid the

13

global inconsistency problem. The key idea is to delay the sensor data inte-gration in the global map until the residual error is eliminated. The residualerror can be eliminated when a loop is detected and the global data associ-ation done. Both ideas are combined in the solution proposed in this paperto obtain an accurate global model based on the application of the EvolutiveLocalization Filter at local and global level.

5.1 Local data association

One way to eliminate the fast degradation of the estimation process, andconsequently of the mapping process, is to re-localize the robot by using alocal map which integrates the last n observations perceived by the robot.This process constitutes a local data association and the pose estimated isreferred to as xLt .

xLt =argmaxx p(xt|Yt, mt−1t−n)

= argmaxxp(zt|xt, m

t−1t−n)p(xt|xt−1, ut−1)p(xt−1|Y

t, mt−1t−n)

≈ argmaxxp(zt|xt, m

t−1t−n)p(xt|xt−1, ut−1) (18)

The last term of expression 18 , p(xt−1|Y t, mt−1t−n) is not considered in this

step because its only objective is to reduce the residual error to a tractablelevel. This solution allows the robot to be re-localized efficiently and avoidspose estimate degradation. This re-localization is equivalent to a maximumlikelihood estimate but is obtained with a stochastic search method, avoidingthe noise problems of gradient descend methods. An equivalent method whichis gradient based is the iterative closest point proposed by [2] used in differentscan matching approaches [10]. Other iterative point matching methods userelated concepts [22] and have similar problems.

In figure 3 the re-alignment effect on the robot’s poses for the environment usedin test 1 (section 6 in this paper) can be observed. The data has been obtainedby recording odometry and sensor information (without any re-localization)and it has been aligned using the ELF filter locally and a local map of theenvironment where the last 20 laser sensor scans are integrated. Obviously,this local association does not eliminate the residual error accumulation whichtends to increase with distance travelled. This problem is addressed in thefollowing subsection.

14

800 1000 1200 1400 1600

700

800

900

1000

1100

1200

1300

1400

1500

1600

800 1000 1200 1400 1600

600

700

800

900

1000

1100

1200

1300

1400

1500

Fig. 3. Initial odometry and re-aligned poses for test environment 1

5.2 Global data association

According to the previous notation, we can also estimate the robot’s pose byusing the global map up to a given time to obtain the maximum a posterioriestimate over the full pose sequence. This we refer to as xGt .

xGt =argmaxx p(x0:t|Yt, mk−10 )

= argmaxxp(zt|x0:t, m

t−10 )p(xt|xt−1, ut)p(x0:t−1|Yt−1, m

k−10 ) (19)

The objective of the global data association is to estimate the residual erroraccumulated in a loop. This global data association does not require a completeglobal map, in fact, only the area of the global map which is currently beingrevisited is required in order to localize the residual error accumulated in

15

a given loop. This characteristic is used to integrate sensor data in a lazymode into the global map. Figure 4 shows the robot’s pose at the end of adetected cycle (test example number 1) and the new pose estimate accordingthe observed data and the global map modelled up to this moment. Note thatthe global map does not include data observations corresponding to robot’sposes included in the detected loop.

1000 1100 1200 1300 1400 1500500

600

700

800

900

1000

1100

Fig. 4. Global pose estimate at the end of a detected loop

5.3 Residual error correction

Assuming the modelled global map up to a given time t is mk−10 , which inte-grates the sensor measurements up to the start of the environment loop k−1,to detect if xt corresponds to a possible closing of a loop, a combined testbased on the distance between xt and points in the pose sequence, and theoverlapping area between sensor scanned area and the modelled global mapis used. If the test is positive, a global data association is started. When thesolutions set converge and the fitness function value of the best estimate is

16

below a given threshold, the global estimate is used to correct the residualerror accumulated in the detected loop pose sequence (from poses k− 1 to t).The error Δxt = x

Gt − x

Lt is proportionally attributed to poses in the loop,

and a new re-localization is done for each robot’s pose using the global mapup to that moment instead of the local environment (this time with a verynarrow optimization area).

Using these new estimates, xMAP0:t , to integrate observations z0:t in the map,mt0, it is possible to maintain the robot properly localized and to maintain aglobally coherent representation of the environment observed along the robotmotion up to time t. In that situation, the estimate obtained through theprevious process is the following

xMAP0:t =argmaxxp(x0:t|Yt, m

t−k0 )

= argmaxxp(zt|x0:t, m

t−k0 )p(xt|xt−1, ut)p(x0:t−1|Yt−1, m

t0) (20)

A proportional error attribution has been adopted because after the initial re-localization process the residual remaining error can not be considered Gaus-sian.

5.4 ELF based SLAM algorithm

According to previously explained ideas, the ELF grid map based SLAM al-gorithm operates as follows: if the sensor data has been obtained directly froma laser scanner and using only the odometry, a first re-alignment step is done.In this step a re-localization of the robot’s pose is done by using the ELFlocalization algorithm operating with a local map which includes the last 20robot poses and the laser data associated to them.

Once the robot’s poses have been realigned, a second step is executed to obtainthe global map and the globally corrected pose sequence. The global mappingalgorithm steps are the following:

(1) Current pose index is initialized to 1, and the first scan is integrated intothe global map .

(2) The poses of the following scans in the sequence are compared with thecurrent pose. If the geometrical distance between them is below a giventhreshold or if there is an overlap between current scan area and theglobal map, there a potential cycle exists. If there is no cycle, the nextpose in the sequence is explored until a cycle is detected.

(3) If a potential cycle has been detected, a global localization is started. Thesize of the explored area where the pose is re-localized depends on the

17

length of the detected cycle. The number of elements in the solution setis related to the area to be explored.• If the fitness value (loss function) of the new pose estimate is belowan acceptance threshold, the new pose is accepted. The detected errorbetween the realigned pose and the re-estimated pose at the end of thecycle is the accumulated residual error remaining along the cycle afterthe realignment process.• The accumulated residual error is distributed proportionally betweenall poses along the cycle to correct the accumulated error at the end ofthe detected cycle. The error is distributed proportionally according tothe magnitude of the displacement and turn at each motion.• For all poses in the loop:

∙ The ELF algorithm is started, limiting its search area to a smallarea around the corrected pose. This time the global map is used.This readjusts the estimate around the corrected pose and theupdated global map.∙ With the corrected pose the new scan data of the detected cycleare integrated into the global map.

(4) If no cycle is detected but the area has been previously modelled, a re-localization is done, the error corrected and the scan integrated in theglobal existent map.

(5) Increment current pose and go to step 2.

In the following section, the results obtained with the proposed algorithm areshown.

6 Experimental results

To demonstrate the algorithm, two different test environments have been used.The data for the first test environment have been obtained from the RoboticsData Set Repository (Radish) [14]. They are part of the Intel Jones FarmsCampus (Oregon) and were provided for M. Batalin (we thank him). Thisenvironment is an excellent test to show the cycle resolution capability of thealgorithm due to the presence of different cycles in the environment.

In Figure 4, the first cycle is detected in the mapping process, the cycle starts inpose 44 and finishes in pose 149. Figure 4 shows the pose at the end of the cycle,the new pose estimate in the global data association, together with the lasermeasures at original and re-estimated poses and the measures prediction atre-estimated pose. After the accumulated error is distributed between all posesincluded in the cycle and re-estimated around this new position according tothe global map, the global data integration process in the global map is done,and the result is shown in figure 5, together with the next scan.

18

600 700 800 900 1000 1100 1200 1300 1400 1500

400

500

600

700

800

900

1000

1100

1200

1300

1400

Fig. 5. Global map after error attribution and fusion at the end of the first detectedloop

Figure 6 shows a different cycle situation. In this situation, the robot trajec-tory does not pass close to previous trajectory poses, but there is an overlapbetween the perceived areas. In this case, a test indicates the perceptual over-lapping and the global localization algorithm is started to obtain the bestglobal estimate for new pose. This new global estimate allows calculation andcorrection of the residual alignment error up to that pose.

Figure 7 shows the state of the global map after integrating 451 scans. Fourcycles have been detected in addition to the revisiting of previously crossedareas. Note that the algorithm can solve satisfactorily the loops in the mappingof this first test.

A second test has been done with data obtained from experiments developedat our University laboratories, offices and corridors. The test site is around 60meters length. The data scans have been obtained each 80cm approximatelyusing a stop and go method. In this test example there are no cycles present,but there is a high degree of symmetry (a long corridor with office doors

19

600 700 800 900 1000 1100 1200 1300 1400 1500500

600

700

800

900

1000

1100

1200

1300

1400

1500

Fig. 6. New pose estimate at the end of the second detected loop

regularly distributed).

Figure 8 shows the global map up to motion 104.

An interesting aspect of the algorithm is the low number of pose solutions re-quired in the population to achieve satisfactory results. For the re-localizationtask, a population of 30 members with 20 iteration steps is enough, whilefor the global localization task the population number used is dynamicallyadapted depending on the loop travelled distance. A more detailed considera-tion of the accuracy and convergence properties of the localization algorithmcan be found in [13].

The complexity of the ELF algorithm is O(N.M), where N is the populationsize, M is the iteration number, N parameter is incremented linearly withthe radius of a ball around the robot’s pose estimate where the solution tothe data association is searched, and the iteration number M is incrementedlinearly with the number of poses included in the detected loop. In spite ofthis complexity appearing to be substantial, in practical terms it is very mod-

20

600 800 1000 1200 1400 1600400

600

800

1000

1200

1400

1600

Fig. 7. Global map after 451 scans fusion in environment test 1

200 400 600 800 1000 1200 1400 1600 1800 2000 2200

200

300

400

500

600

700

800

900

Fig. 8. Global map after 103 scans fusion in environment test 2

erate. This is because only in the case of a loop detection is the global dataassociation started, and in case of local correction, the population set is very

21

low. To illustrate this computational cost, using a cell size of 3cm and for theenvironment test1 the time required for the first global localization at the firstdetected loop (105 poses in the loop) is 15.8 seconds, for the second detectedloop (30 poses in the loop) it is 5.6 seconds, and for local optimizations it is0.47 seconds on average. The algorithm is not fully optimized: for instance,sines and cosines are calculated using conventional functions instead of usinga tabulated version to decrease the computational cost of the algorithm.

The initial realignment done left an estimated residual error lower than 0.25degrees in orientation and lower than 1 centimeter in position (on average).This error is eliminated in the global data association process.

7 Conclusion

The differential evolution-based solution to the grid-based SLAM problempresented in this article introduces a new possibility to accurately solve theSLAM problem. At a local scale, the ELF localization algorithm provides afast and accurate maximum likelihood estimate with results equivalent to otherre-localization methods like scan matching approaches. And at a global scale,the algorithm only incurs substantial cost at cycle closing detection time andthe cost increases linearly with the number of poses included in the detectedloop, remaining constant for the re-localization and integration of the scansin the global map which is lower than one second for the re-location and themapping of the scan data into the map.

The Markov chain nature of evolutionary algorithms is exploited by introduc-ing into the loss function the sensor error innovation together with motionerror innovation. In the ELF method used here each individual in the evolu-tive algorithm represents a possible solution to the localization problem andthe value of the loss function represents the error in explaining the perceptualand motion data. The search for this solution is done stochastically, employingan evolutive search technique. At a global scale the proposed approach is ableto detect and estimate environment loops to eliminate the residual error. Thisallows the algorithm to accurately map large environment maps. The algo-rithm has been tested with data acquired with different robots equipped withlaser range scanners. Tests performed with our algorithm have demonstratedthe algorithm’s robustness and accuracy generating maps.

The evolutive optimization technique constitutes a probabilistic search methodthat avoids derivatives. The use of derivatives present two types of problems:

• It causes strong numerical oscillations when noise to signal ratio is high.• It requires differentiable functions as otherwise the derivatives can be dis-

22

continuous or not even exist.

Due to the low number of solutions required in the algorithm, it is possible touse the algorithm to model large environment at low computational cost.

The advantages over other SLAM algorithms are:

(1) Compared with other techniques used at a local level to re-localize therobot, it is very robust because it does not use a gradient-based optimiza-tion like scan-matching techniques based on the iterative closest pointmethod or classic Extended Kalman Filters solutions. The robustness ofthe algorithm proposed derives from its stochastic search nature.

(2) The method can accommodate arbitrary non-linear system dynamics,sensor characteristics and non-Gaussian noise, avoiding unnecessary lin-earizations. By introducing in the fitness function the sensor innovationtogether with the motion innovation, and due to the Markov Chain behav-ior of the evolutive search algorithm, the set of particles evolves graduallyalong the most probable environment areas. This allows the algorithm tosolve large loops where one scan is insufficient to eliminate the ambiguity(a similar problem occurs in the global localization problem).

(3) Since the set of solutions does not try to approximate posterior densitydistributions, it does not require any assumptions about the shape of theposterior density, unlike parametric approaches.

(4) The evolutive filter focuses its computational resources on the most rele-vant areas by addressing the set of solutions to the most interesting areasaccording to the fitness function obtained.

(5) The size of the minimum solution set required to guarantee the conver-gence of the evolutive filter to the true solution is low.

(6) The algorithm is easy to implement. The ELF localization algorithm isused at a local or global level. Moreover, the low computational costmakes allows online operation even in relatively large areas.

(7) Due to the stochastic nature of the algorithm search of the best robotpose estimate, the algorithm is able to cope with a high level of sensornoise with low degradation of the estimation results.

(8) The delayed mapping in case of loops eliminates the necessity of main-taining multiple maps or re-mapping each time a significant global erroris detected.

8 Acknowledgements

The authors gratefully acknowledge the funds provided by the Spanish Gov-ernment through the MCYT project DPI2004-0594 .

23

References

[1] Bar-Shalom, Y., Fortmann, T.E., Tracking and data association AcademicPress,1988.

[2] Besl, P.J., and McKay, N.D., A method for registration of 3-d shapes, in: IEEETransactions on Pattern Analysis and Machine Intelligence, 14, pp. 239-256,1992.

[3] Castellanos, J.A., Neira, J., and Tardos, J.D., Limits to the consistency of EKF-based SLAM, in: Proc. of the 5th IFAC/EURON Symposium on Intelligent andAutonomous Vehicles IAV-2004, Lisboa, Portugal, 2004.

[4] Dellaert, F., Fox, D., Burgard, W., Thrun, S., Monte Carlo Localization forMobile Robots, in: Proceedings of the 1999 International Conference on Roboticsand Automation, pp. 1322-1328 , 1999.

[5] Dissanayake, M.W.M.G., Newman, P., Durrant-Whyte, H.F., Clark, S. andCsorba, M., A solution to the simultaneous localization and map building(SLAM) problem, in: IEEE Transactions on Robotics and Automation, 17 (3),pp 229-241, June, 2001.

[6] Doucet, A., Freitas, N., Murphy, K., and Russel, S., Rao Blackwellised particlefiltering for dynamic Bayesian networks, in: Proc. of the UAI, UAI-2000, 2000.

[7] Guivan, J. and Nebot, E., Optimization of the simultaneous localization and mapbuilding algorithm for real time implementation, in: EEE Trans. on Robotics andAutomation, 17 (3), pp 242-257, 2001.

[8] Julier, S.J. and Uhlmann, J.K., A counter example to the theory of simultaneouslocalization and map building, in: Proc. of the Int. Conference on Robotics andAutomation ICRA-01, Seoul, Korea, pp 4238-4243, 2001.

[9] Leonard, J. and Feder, H., Decoupled stochastic mapping, in: IEEE J. OceanEngineering, 26 (4), pp 561-571, 2002.

[10] Lu, F., and Milios, E., Robot pose estimation in unknown environments bymatching 2-d range scans, in: Intelligent and Robotics Systems, 18, pp 249-2715,1997.

[11] Montemerlo, M., and Thrun, S., Simultaneous localization and mapping withunknown data association using FastSLAM, in: Proc. of the Int. Conference onIntelligent Robotics and Automation ICRA-03, Taipei, Taiwan, vol 2, pp 1985-1991, 2003.

[12] Montemerlo, M.,Thrun, S.,Koller, D. and Wegbreit, B., FastSLAM: A factoredsolution to simultaneous localization and mapping, in: Proc. of the Nat.Conference on Artificial Intelligence AAAI, 2002.

[13] Moreno, L., Garrido, S, and Munoz, M.L., Evolutive filter for robust mbile robotglobal localization, in: Robotics and Autonomous Systems, 54, pp 590-600, 2006.

24

[14] Batalin, M., The Robotics Data Set Repository (Radish), in:http://radish.sourceforge.net.

[15] Smith, R., Self, M., and Cheeseman, P., Estimating uncertain spatialrelationships in robotics, in: I. Cox and G. Wilfong (Eds.), Autonomous RobotsVehicles, pp 167-193, Springer-Verlag, 1990.

[16] Smith, R. and Cheeseman, P., On Representation and Estimation of SpatialUncertainty The International Journal of Robotics Research, Vol 5, No 4, pp.56-67, 1986.

[17] Storn, R. and Price, K., Differential Evolution- A simple and efficient adaptiveheuristic for global optimization over continuous spaces, in : Journal of GlobalOptimization, 11, pp 341-359 , 1997.

[18] Tardos, J., Neira, J., Newman, P., and Leonard, J., Robust mapping andlocalization in indoor environments using sonar data, in: Int. Jou. RoboticsResearch, 21 (4), pp 311-330, 200.

[19] Thrun, S., Simultaneous mpping and localization with sparse extendedinformation filters: theory and initial results. in: CMU-CS-02-112, 2002.

[20] Thrun, S., Burgard, W. and Fox, D., in: Probabilistic Robotics, The MIT Press,2005.

[21] Williams, S., Dissanayake, G. and Durrant-Whyte, H., An efficient approach tothe simultaneous localization and mapping problem, in: Proc. of IEEE Int. Confon Robotics and Automation ICRA-02, pp 406-411, 2002.

[22] Zhang, Z., Iterative point matching for registration of free-form curves andsurfaces, in: Int. Journal of Computer Vision, 13 (2), pp 119-152, 1994.

25