Mobile robot localization using a non-linear evolutionary� lter

J. M. ARMINGOL ¤, A. DE LA ESCALERA, L. MORENO and M. A. SALICHSDivision of Systems Engineering and Automation, Universidad Carlos III de Madrid,C/Butarque 15, 28911 Leganés, Madrid, Spain

Abstract—This article describes a localization system for autonomous mobile robot navigation in anindoor semi-structured environment. A peripheral ring of 24 ultrasonic sensors and a camera with amotorized zoom on a pan– tilt platform are used to obtain the information required for the localizationprocess. A non-linear � lter based on a genetic algorithm as an emerging optimization method tosearch for optimal positions is presented. The proposed algorithm is based upon an iterative extendedKalman � lter (EKF), which utilizes matches between observed geometric beacons and a generic mapof beacon locations and the detection of arti� cial landmarks, to correct the position and orientationof the vehicle. No exhaustive map of the environment is provided to the mobile robot. It must workwith a generic description of the kinds of entities in the environment. The resulting self-localizationmodule has been integrated successfully in a more complex navigation system based on a reactivearchitecture. Various experimental results show the effectiveness of the presented algorithm, includinga comparison with the EKF method.

Keywords: Mobile robots; genetic algorithms; Kalman � lter; ultrasonic sensors; visual localization.

1. INTRODUCTION

Autonomous mobile robots are currently receiving increasing attention in thescienti� c community as well as in industry. This is due to the upcoming applicationsin the � eld of service robotics. Mobile robots have many potential applications inroutine or dangerous tasks such as operations in nuclear plants, delivery of suppliesin hospitals, and cleaning of of� ces and houses. The � rst requirement for a robotto be able to do such tasks is to navigate successfully to desired locations in theenvironment.

A prerequisite for geometric navigation of a mobile robot is a position-� ndingmethod. Odometry is the most used navigation method for mobile robot position-

¤E-mail: armingol@ing.uc3m.es

ing. The fundamental idea of odometry is the integration of incremental motioninformation over time, which leads inevitably to the accumulation of errors. Theaccumulation of orientation errors will cause large position errors, which increaseproportionally with the distance traveled by the mobile robot. Wheel slippage andunequal wheel diameters are the most important source of error. To reduce the un-certainty and the error of the estimated location, localization techniques have beendeveloped and three different approaches are used:

² Detection of natural or arti� cial landmarks.

² Matching between the sensorial information and a map of the environment.

² Position estimation using occupancy grids, etc.

A landmark is a localized physical feature that the robot can sense and use toestimate its own position in relation to a map that contains the landmark’s absoluteposition. D’Orazio et al. [1] use a landmark that consists of panels on whichtwo groups of four LEDs are � xed. The camera can see the con� dence points atany moment, and they can be located easily in the image plane because of theirbrightness and arrangement. The intensity of the observed LEDs depends on thecamera position. Ricotti and Liotta [2] have used a guide mark (two circles andidenti� cation codes); they designed a landmark that could be easily characterizedby geometric invariant features in respect of scale factors and perspective views.The perception system localizes the landmark in the environment and tracks it. Abar code reader is used to read the information related to the landmark stored in abar code label. The errors in the orientation estimates are constant and the meanvalue is about 1.5±. In [3], a solid black circle with two interior white dots is usedto obtain the position of the mobile robot. The motive for using a circle with twodots is to reduce the effect of image noise and blur. The guide landmark can beplaced either on a ceiling or on a wall. The maximum absolute position error isabout 7.5 cm. Byler et al. [4] have developed an intelligent mobile sensing systemfor the automated inspection of radioactive containers. Each landmark is a � at pieceof metal or paper and is composed of a set of high contrast concentric circles in apredetermined pattern. The average position error was 2 cm with a 0.24± angularerror with respect to three different sightings.

The traditional approach to robot navigation is to provide a detailed and speci� cmap to the robot. Sensed data are matched to this map to determine the robot’sposition and orientation. A typical system is described in Escalera et al. [5],where a combination of a laser diode and a CCD camera is used. The sensorialinformation is modeled as straight lines that will be matched with a prior map ofthe environment; the matching process is accomplished within an extended Kalman� lter (EKF). In [6], a sonar localization system for mobile robot navigation in aknown environment is described; state corrections are applied through a standardKalman � lter. The system tries to extract as much information as possible fromthe sensors by building a detailed probabilisti c model of each sonar event. In [7],

an application of the EKF to the problem of mobile robot navigation in a knownenvironment is presented. This navigation algorithm is based around an EKF, whichutilizes matches between observed geometric beacons and an a priori map of beaconlocations. Betke and Gurvits [8] describe an ef� cient method for localizing a mobilerobot in an indoor environment. The robot can identify the landmarks and measuretheir bearings relative to each other. The localization algorithm estimates the robot’sposition and orientation with respect to the map of the environment. Bonnifait andGarcia [9] present a two-dimensional (2D) localization system for mobile robotsin outdoor environments which computes, by EKF, the position and orientationof a vehicle using odometry and the measurements of the relative azimuth anglesof known landmarks. However, this approach has several limitations . A detailedmap of the environment is dif� cult and tedious to create. Furthermore, the indoorenvironment changes often as objects in it, such as furniture, are moved.

Several techniques for position estimation using occupancy grids are studied inSchiele and Crowley [10]. Lessons learned from position estimation using a localmodel based on line segments are applied. The algorithm attempted to solve theposition estimation problem by multi-resolution matching [11].

This article introduces a new view of estimating the absolute position andorientation of a mobile robot in an indoor semi-structured environment using a non-linear � lter based on a genetic algorithm as an emerging optimization method tosearch for optimal positions. The proposed algorithm is based upon an iterativeEKF which utilizes matches between observed geometric beacons and a genericmap of beacon locations and the detection of arti� cial landmarks, to correct theposition and orientation of the vehicle.

No exhaustive map of the environment is provided to the mobile robot. Thismeans that the robot only knows a generic description of the kinds of entitiesin the environment: natural landmarks such as rooms, walls, doors and arti� ciallandmarks. The advantages of this approach are obvious: the effort of detailed mapconstruction is avoided and the environment can be changed as long as the genericrepresentation is still valid. The map maintenance would be very dif� cult if the mapincluded small objects such as tables, chairs and boxes.

2. LOCALIZATION SYSTEM

In this section, the sensors used and the algorithms developed to detect therepresentative landmarks of the environment are presented.

A peripheral ring of 24 ultrasonic sensors and a camera with a motorized zoom ona pan– tilt platform are used to obtain the information required for the localizationprocess in motion.

The ultrasounds make the most of the indoor semi-structured environment. In thecase described in this article, this means there are many free walls (e.g. corridors) inthe environment [12]. However, there are cases when this is not true, either becausethere are many objects in the environment (chairs, tables, people) that hide the wall

Figure 1. Map of the environment.

or because the walls are further than the ultrasound range (Fig. 1). In this case thevisual system still works. There are arti� cial landmarks (solid circles placed on thewalls) located in the environment.

Using the algorithm described in this article, the robot can still � nd its location.The two systems differ in the execution time because of practical limitations . Whileultrasound is used continuously because of the usual existence of walls in this typeof environment, the visual system is only used when there is a landmark in its visualrange.

2.1. Ultrasonic sensor localization

Ultrasonic time-of-� ight ranging is currently the most common technique employedon indoor mobile robotics systems. There are several reasons for its popularity:robustness, range accuracy, easy of use and low cost. As proposed in [6, 7], anymapped objects in the environment can be used as geometric beacons. Thesegeometric beacons need to be detectable by the sensors; also, the sensors must bemodeled, which means that is possible to predict sensor readings from a map of theenvironment (Fig. 1). Under these assumptions, the location of the vehicle can beupdated by matching sensor observations against model predictions.

2.1.1. The map of the environment. The mobile robot makes use of two coor-dinate systems. The � rst is the global coordinate system, in which the model isde� ned. The second is the odometry-based local coordinate system, in which mea-surements are made. Clearly, both systems must be related by some coordinatetransform 3 such that:

x D 3 Ox; (1)

where x is the robot’s position in the global coordinate system and Ox is the robot’sposition in the local coordinate system. The same transform is used for an observedbeacon.

The map of the environment is based on rooms and segments. Segments of theenvironment (walls, doors, etc.) are lines in 2D, and are de� ned by the parametervector ps D .Os; Ds; µs/. Where Os is origin of the local coordinate system,Ds is the length of the segment and µs is the angle with respect to the globalcoordinate system. A set of segments de� nes a speci� c room. Arti� cial landmarksare positioned in a relative form with respect to the rooms and corridors of theenvironment.

2.1.2. Probabilisti c modeling of ultrasonic sensor event. The principal stages inthe processing of an ultrasonic sensor event are as follows.

(i) Sensor preprocessing. This converts the sensor output to an estimate ofapparent range and its expected uncertainty, using sensor calibration and noisemodels. The odometry measurement is used to provide a prediction of thevehicle’s location at which these sonar range readings were acquired.

A detail a priori model of the sensor event is built using the current worldmodel (Fig. 2), vehicle state estimate and sensor model. This process isindependent of the sensor observation:

De D 1.cos µs C sin µs tan µs/

¢Xr ¡ Xs

.Ys ¡ Yr/ tan µs ¡ sin.µ a C µr ¡ µs/; (2)

De is the estimated distance between the sonar and the segment respects thelocal coordinate system. Where .Xr; Yr; µr/ is the position and heading of thevehicle and .Xs; Ys; µs/ is the position and heading of the observed segment, inboth cases with respect to the global coordinate system and µ a the sensor anglerefer to the local coordinate system.

(ii) Data fusion. This compares the observed ultrasonic sensor data with the model-based predictions to produce a probabilisti c state update, using the Mahalanobisdistance.

Figure 2. Probabilistic modeling of ultrasonic sensor event.

2.2. Visual localization

The localization system is based on the detection of arti� cial landmarks (solidcircles placed on the walls). The image of the landmark provides informationregarding the robot’s position and heading direction.

2.2.1. Arti� cial landmark detection. Normalized greyscale correlation is awidely used method for pattern matching applications . The correlation operationis a linear operator and can be seen as a form of convolution where the patternmatching model is analogous to the convolution kernel:

r.x; y/ D I .x; y/ ± M.x; y/ DiDNX

iD0

jDNX

jD0

M¤.i; j/I .x C i; y C j /: (3)

In other words, for each result the N pixels of the model M.x; y/ are multiplied bythe N underlying image pixels I .x; y/ and these products are summed.

When the correlation function is evaluated at every pixel in the target image, thelocations where the result is largest are those where the surrounding image is mostsimilar to the model. The search algorithm then has to locate these peaks in thecorrelation result and return their positions.

Unfortunately, ordinary correlation has the undesirable property that the resultincreases if the image simply gets brighter. In fact, the function will reach amaximum when the image is uniformly white, even though at this point it no longerlooks like the model. The solution is to use a more complex, normalized version of

the correlation function:

r.x; y/ DP

i

Pj .M.x C i; y C j/ ¡ SM/.I .i; j/ ¡ NI /

qPi

Pj .M.x C i; y C j/ ¡ SM/2

Pi

Pj .I .i; j / ¡ NI /2

: (4)

This expression has the desirable property that the result is unaffected by linearchanges in the image or model pixel values. It reaches its maximum value of 1where the image and model match exactly, and gives 0 where the model and imageare uncorrelated.

The result is converted to a percentage, where 100% represents a perfect match:

score D max.r; 0/2 ¤ 100%: (5)

Note that some of the terms in the normalized correlation function depend only onthe model, and hence can be evaluated once and for all when the model is de� ned.

2.2.2. Correlation threshold. To separate the results (match scores) of thenormalized correlation, an acceptance threshold is used. If the correspondencebetween the image and the model (also referred to as the match score) is less thanthis level, they are not considered as a match. The acceptance threshold dependson the noise and distortion of the image; however, poor quality images increase thechance of false matches and the search time (Fig. 3).

Figure 3. Landmark and match score.

The certainty threshold is the correlation value above which the algorithm canassume that is has found a match without continuing to search the rest of the imagefor a better one.

2.2.3. Determination of pan and tilt angles. The landmark selection processdepends on the current value of uncertainty in the vehicle position and the map ofthe environment. Initially, it is supposed that the landmark appears in the center ofthe image. The pan and tilt visual angles of the camera with respect to the landmarkare:

’pan D arctandpx.Cx ¡ uc/

f;

’tilt D arctandpy.vc ¡ Cy/

f;

(6)

where f is the focal distance, .uc; vc/ the landmark center in the image, .Cx ; Cy/ theimage center and dpx; dpy the CCD cell size. To obtain accurate results, a calibrationstep is necessary to determine intrinsic and extrinsic camera parameters [13, 14].

The image segmentation proved to be suf� ciently robust with respect to thefollowing parameters:

² Environment lighting conditions .² Camera to landmark distance.² Different landmark perspective views.

3. POSE ESTIMATION

An EKF is used to correct the position and orientation of the vehicle [7], using theerror between the observed and estimated robot– landmark angles, and the distancesbetween the robot and the segments of the environment (walls, doors, etc.). TheKalman � lter is a linear, discrete-time system. The covariance matrix PkC1jk is aconditional error covariance matrix associated with the state estimate. The � lter hasthe same structure as a class of deterministic estimators.

The Kalman � lter equations de� ne the evolution of the Gaussian conditionalprobability density of the state. The EKF was obtained using a Taylor seriesexpansion up to � rst-order or second-order terms. This � lter introduces errors inthe equations where the higher-order terms are neglected.

There are several ways of compensating for these errors:

(i) Compensation of errors in the state prediction. The equation of the stateprediction describes how the position of the vehicle changes from the previousstate, x.k/ D [xk; yk ; µk]T, at time tk to x.k C 1/ in tkC1 due to a control actionu.k/ and the presence of a white noise v.k/, normal random variable of averagezero and matrix of covariances Q.k/:

x.k C 1/ D f .x.k/; u.k// C v.k/ v.k/ ¼ N.0; Q.k//; (7)

Q¤.k/ > Q.k/: (8)

The control input u.k/ D u.T .k/, 121.k/, 122.k//, is a rotation 121.k/,followed by a translation of distance T .k/ and a new rotation 122.k/.

The function of state transition is described as:

f .x.k/; u.k// D

2

4x.k/ C T .k/ cos.µ .k/ C 121.k//

y.k/ ¡ T .k/ sin.µ.k/ C 121.k//

2.k/ C 121.k/ C 122.k/

3

5 : (9)

(ii) Multiplication of the state covariance by a scalar Á > 1 at every samplingtime:

P .k C 1/ D rf P .k/rf T C Q.k/; (10)

P ¤.k C 1/ D ÁP .k C 1/: (11)

Furthermore, Kalman � lter systems are effective at tracking moving parametersso long as their hypotheses are met, but they are extremely susceptible to outliersand systematic model deviations because the Kalman � lter update rule dependscritically on the assumed error distributions . The Kalman � lter does not correctlyhandle non-linear or non-Gaussian motion and measurement models, and cannotdeal with multi-modal densities as encountered during global localization. Thisproblem is clearly present when the mobile robot tries to localize its initial positionin a global environment model. There exist a high number of possible symmetriesin the environment, especially when the information provided by sensors is notsuf� cient or the a priori environment model does not reasonably match the real one.In this situation the probability distribution function is located around the differentacceptable positions and cannot be considered uni-modal. To avoid this problemdifferent solutions have been proposed, based on iterated procedures, Monte Carlomethods [15].

3.1. The iterated EKF

A modi� ed state updating approach can be obtained by an iterative procedure. Themeasurement prediction is:

Oz.k C 1jk/ D h¡k; Ox.k C 1jk/

¢; (12)

where h is a non-linear function of the state and Oz represents the observations ofthe sensorial system. The measurement function is affected by a gaussian noise ofaverage zero and covariance R.k/.

One approach to alleviate the additional errors due to the measurement non-linearity is to compute the updated state not as an approximate conditional mean,but as a maximum a posteriori estimate. An approximate estimate can be obtained

by an iteration that amounts to relinearization of the measurement equation. Theconditional probability density function of x.k C 1/ given z.k C 1/ is:

p.x.k C 1jZ.k C 1// D p.x.k C 1/jz.k C 1/; Z.k//; (13)

where:

Z.k/ D fz.j /; j D 1; : : : ; kg: (14)

Maximizing this expression with respect to x.k C 1/ is equivalent to minimizingthe innovation error:

F [x.k C 1/] D 1

2

©z.k C 1/ ¡ h[k C 1; x.k C 1/]

ªTR.k C 1/¡1

£©z.k C 1/ ¡ h[k C 1; x.k C 1/]

ªC 1

2[x.k C 1/ (15)

¡ Ox.k C 1jk/]TP .k C 1jk/¡1[x.k C 1/ ¡ Ox.k C 1jk/]:

The iterative minimization of this expression is usually done by a Newton–Raphsonalgorithm, expanding it in a Taylor series up to second order about the ith iteratedvalue of the estimate of x.k C 1/.

F D F i C F iT

x .x ¡ xi/ C 1

2.x ¡ xi/F i

xx.x ¡ xi/ C ¢ ¢ ¢ : (16)

This article presents a non-linear evolutionary � lter based on a genetic algorithmto generate an optimal position for the mobile robot, minimizing the above expres-sion. Genetic algorithms are different from more normal optimization and searchprocedures in four ways:

(i) They work with a coding of the parameter set.

(ii) They search from a population of points instead of a single point.

(iii) They use objective function information, not derivates.

(iv) They use probabilistic operation rules, not deterministic rules.

The basic idea of this � ltering technique is to combine the EKF approach, whichhas good properties due to the close form of the algorithm, and their recursiveform of integrating the information with a simulation-based � ltering technique.The technique presented in this article uses the EKF to obtain a seed, whichis used to estimate a neighborhood where the true value of the state is located.The most accurate solution is searched within the estimated neighborhood . Thesearch of this solution is done stochastically using a genetic technique, whichhas the advantage of being a non gradient-based optimization method (the geneticoptimization techniques constitute a probabilisti c search method which imitate thenatural selection process based on genetic laws).

3.2. Genetic algorithms

The genetic algorithm is a searching process based on the laws of natural selectionand genetics [16]. Genetic algorithms are typically black-box methods that use� tness information exclusively, i.e. they do not require gradient information or otherinternal knowledge of the problem.

Genetic algorithms presume that a potential solution of a problem is an individualthat can be represented by a set of parameters. These parameters, regarded as thegenes of a chromosome, can be structured by a string of values in binary form. Apositive value, known as a � tness value, is used to re� ect the degree of goodnessof the chromosome, which is generally correlated with the objective function of theproblem.

Usually, a simple genetic algorithm (Fig. 4) consists of three operations: selection,genetic operation and replacement. The three primary genetic operators focused onby most researchers are selection or reproduction, crossover and mutation.

Using these components, a simple genetic algorithm can be summarized asfollows:

(i) Initialization. Create an initial random population of chromosomes andevaluate each chromosome.

(ii) Selection. Select two parent chromosomes from the current population. Achromosome with high � tness is more likely to be selected.

(iii) Crossover. Generate two offspring from the two parents chromosomes byexchanging bit strings.

Figure 4. Genetic algorithm cycle.

(iv) Mutation. Apply a random mutation to each offspring.

(v) Repeat Steps (ii)– (iv) until the number of offspring in the new population isthe same as the number of chromosomes in the old population .

(vi) Evaluate each offspring and go back to Step (ii).

The cycle of evolution is repeated until a desired termination criterion is reached.This criterion can be set by the number of evolution cycles, or the amount ofvariation of individuals between different generations, or a pre-de� ned value of� tness. The best chromosome generated during the search is the � nal result of thegenetic algorithm.

The objective function is the main source to provide the evolution mechanismof the status of each chromosome. This is an important link between the geneticalgorithm and the system. Parent selection emulates the survival of the � ttestmechanism in nature.

It is expected that a � tter chromosome receives a higher number of offspring andthus has a higher chance of surviving in the subsequent generation.

4. NON-LINEAR EVOLUTIONARY FILTER DESIGN

Genetic algorithms have proven to be effective in a number of robotics applications ;a genetic approach to mobile robot motion with a distance-safety criterion ispresented in [17]. A genetic algorithm is proposed to search for near-optimal pathsin a workspace represented as a grid. Hein and Meystel [18] have developed agenetic algorithm that � nds admissible control trajectories that tend to minimizethe vehicle’s transit time through the obstacle � eld. Other applications includethe planning of control and trajectories for automated delivery vehicles, and theoptimization of control for racing vehicles. A route construction heuristic for avehicle routing problem is described in [19]. The genetic algorithm � nds goodparameter setting in the route construction phase. Kang et al. [20] present agenetic algorithm for global path planning to a goal for a mobile robot in an knownenvironment.

4.1. Mobile robot architecture

The vehicle architecture is based on a hybrid architecture (AFREB) approach witha wide range of reactive control methodologies [21]. The AFREB architecture(Fig. 5) consists of the following modules: fusion supervisor, behavior primitivesand executor. The function of the fusion supervisor module is the calculation ofthe weight of each primitive behavior. The function of the executor module is thecalculation of the actual robot commands based on the primitive behavior weights.

The standard inputs to the executor are speed v and curvature !. A primitivebehavior can be characterized by a temporal sequence of appropriate values for v

and ! which cause the robot to exhibit the pre-speci� ed primitive behavior. Thus,

Figure 5. Mobile robot architecture.

the output of a primitive behavior c.k/ is de� ned as the vector c.k/ D .v; !/, wherethe variable k denotes the kth cycle of the robot controller.

The purpose of the path planner is to create a path, as a set of points, from thecurrent mobile robot’s position to a destination, based on the knowledge of theenvironment and the user’s mission. Then, this whole path is sent to the robot’spilot level through a system supervisor that autonomously executes the path, whilerobust obstacle avoidance is guaranteed.

The localization process is based on two modules:

(i) Perception planner. Given a possible plan, it makes a preselection of landmarksand segments of the environment in each area where the robot will move about.A plan is a function that speci� es, for each position on the global coordinatesystem, the landmark or segments the robot should sense when it has thatposition:

a D PLAN.x/: (17)

(ii) Localizer. The localization supervisor selects on-line which landmarks have toactually be used and when, based on the preselection of landmarks a and on thebehavior of the vehicle. An EKF is used to correct the position and orientationof the vehicle from the difference found between the observed distances andangles to each landmark and the estimated ones. Subsequently, the methodbased on genetic algorithms performs a search in the new uncertainty area tooptimize the position estimation.

The localization plan can be executed in two different ways according to theincrease in uncertainty of the robot’s position. Experimentally, two limits havebeen established for the position uncertainty. The robot intends to perform thelocalization process in motion, according to the landmarks selected in each area,when the � rst limit has been reached. If the system were not able to identify thelandmarks, the uncertainty of the position would increase and reach the second limit.In this case, the vehicle would be forced to stop, to allow an easier acquisition andidenti� cation of the landmark. When the uncertainty decreases, the vehicle wouldrestart, beginning the process again in a cyclic manner. Given the localization inan instant and knowing the information of the odometry in the subsequent instant,what should be perceived is compared with what the sensor captures; by comparingboth items of information, a new position and orientation of the robot is determined.The algorithm consists of the following steps: prediction of the position, predictionof measurements, observation, correspondence and estimation.

These two levels are necessary because of the reactive characteristics of thearchitecture. The path produced by the path planner must be considered just asa suggestion to the pilot. A consequence of the reactive behavior of the pilot isthat the executed path is usually different from the planned path. This is especiallysigni� cant in a real scenario where unexpected obstacles appear in the planned path.

An obstacle avoidance behavior is provided to prevent contact with unknownobjects. A peripheral ring of 24 ultrasonic sensors is located around the robotperimeter that provides range information indicating the proximity of objects inthe vehicle environment.

4.2. The genetic structure

To solve the mobile robot localization problem by a genetic algorithm, a codingscheme is needed to encode the parameters of the problem into genetic strings [22].Here, robot position is coded as a string by their Cartesian coordinates as:

f.x1; y1; µ1/; .x2; y2; µ2/; :::::.xn; yn; µn/g; (18)

with all the values stored in decimal form. A set of valid random positions isgenerated as the initial generation around the EKF solution. In order to solvethe problem, a chromosome is 11 bits in length and populations have a six of 250elements. Chromosomes are sequences of � oats rather than sequences of bits (whereeach � oat represents a particular position, Fig. 6).

Figure 6. Genetic structure.

After a new population of strings has been generated, we should compute the� tness of each position. De� ning proper � tness functions is vital in geneticalgorithm applications because it affects the optimization performance.

4.3. De� nition of � tness

To optimize a structure using genetic algorithms, one must be able to assign somemeasure of quality to each structure in the search space. The � tness function isresponsible for this task. In genetic algorithms, the � tness function determines theproductivity of individuals in a population . In general, a � tness function can bedescribed as:

u.x/ D g.f .x//; (19)

where f is the objective function and u.x/ is a non-negative number. The functiong is often a linear transform, such as:

u.x/ D af .x/ C b; (20)

where a is positive when maximizing f and negative when minimizing f , and b isused to ensure a non-negative � tness.

Hence, it is essential to de� ne a � tness function, which characterizes the problemin a perfect way.

The generalised objective function can be de� ned as follows:

Landmarks:

gkl D

1

.µ k¡el¡pan ¡ µ ob

pan/2 C .µ k¡el¡tilt ¡ µ ob

tilt/2; (21)

where gkl is the cost of the position corresponding to the � rst string in the kth

generation, µk¡el¡pan, µk¡e

l¡tilt are the estimated angles between the robot and the landmarkof the � rst position in the population in the kth generation, respectively:

µ k¡el¡pan D arcsin

³ya ¡ pyp

.xa ¡ px/2 C .ya ¡ py/2

´; (22)

µk¡el¡tilt D arctan

³pz ¡ zap

.xa ¡ px/2 C .ya ¡ py/2

´; (23)

where .px ; py ; pz/ is the position of the landmark and .xa ; ya; za/ the position ofthe pan– tilt platform.

Segments of the environment:

gkl D 1

PniD0.Dk¡e

l¡s ¡ Dobs /2

; (24)

where gkl is the cost of the position corresponding to the � rst string in the kth

generation, and Dk¡el¡s (2) is the estimated distance between the sonar and the

segment of the � rst position in the population in the kth generation, respectively.

Figure 7. Example of � tness evolution.

4.4. Generational replacement

If the size of the population is n, then n parents of the mth generation are selectedand n offspring are created. Then, the next generation is composed of the n

best chromosomes among the pool of 2n chromosomes. Due this conservativereplacement scheme, the best chromosome at generation m C 1 is always betterthan or equal to the best chromosome at generation m.

This procedure is repeated for a � xed number of generations. The best chromo-some generated during the search is the � nal result of the genetic search. Throughthis process, it is expected that an initial population of randomly generated chro-mosomes with low � tness values will improve as parents are replaced by betteroffspring. Figure 7 shows a detailed map of the maximum peaks in a search cycle.

In this case the ultrasonic sensor system was employed (eight sensors) to detectsegments of the environment in a corridor, where the top � gure represents thethree-dimensional view and the bottom � gure the top view. In Fig. 8, the differentgenerations around the global maximum are shown. Finally, the best chromosomeof the last generation coincides with the existing global maximum.

5. EXPERIMENTAL RESULTS

All experiments were conducted on a B21-RWI mobile vehicle (Fig. 8), equippedwith a CCD camera and a motorized zoom lens (Sony, Ernitec 8–48 mm), mountedon a motorized Helpmate pan– tilt platform. The platform is able to perform tiltmovements in a range of 0–90± and pan movements in a range of 0–180± (twodifferent motors performs the movements), with an image acquisition board (MatroxImage-LC) and a peripheral ring of 24 Polaroid ultrasonic sensors.

Figure 8. B21 mobile robots.

These ultrasonic sensors present some drawbacks. They have limited angularresolution. The maximum range to be expected in an indoor environment is about10 m, with a mean error of 5 cm/m (calibration results) and a detected target canreside anywhere inside a conical region with a half angle of approximately §15±.

To test the localization method, the mobile robot moved along different paths.In all these applications the present approach has been shown to be both ef� cientand robust, running comfortably in real time. In order to test the technique, the

Figure 9. Example 1 of a travel path (room).

real position of the robot was obtained using a Geodimeter 600 (theodolite) , withaccuracy of distance measurement §5 mm. The position of the robot was calculatedevery 200 m during navigation (odometry) and the average velocity of the robot wasaround 40 cm/s in all cases.

Real situations are shown in the three following � gures: in Fig. 9, the mobile robotestimates its current position by an algorithm with a population size of 250 elementsand 10 generations. The localization points are marked with a discontinuous circle.The mean absolute error accumulated in the � nal position is about 3 cm in positionand 0.5± in orientation using the arti� cial vision system. In Fig. 10, the mobile robotestimates its current position by an algorithm with a population size of 250 elements

Figure 10. Example 2 of a travel path (corridor).

and 10 generations. The � nal error is about 4 cm in position and 0.5± in orientation.In Fig. 11, the mobile robot estimates its current position by an algorithm with apopulation size of 250 elements and � ve generations. The localization points aremarked with a discontinuous circle. The � nal error is about 5 cm in position and 1±

in orientation. In both cases the peripheral ring of 24 ultrasonic sensors is used. Thecomputational time for the algorithm with a population size of 250 elements is 0.6 sfor 10 generations and 0.3 s for � ve generations on a Pentium processor at 600 MHz.The method presented in this article converges quickly in a few generations sincethe search is performed in a restricted zone.

Figure 11. Example 3 of a travel path (corridor).

Tables 1 and 2 show the list of errors at each localization point between the EKFsolution, genetic algorithm solution and the real position in examples 2 and 3. Thenumber of the selected ultrasonic sensors at each localization point is also given.The odometry measurement is used to provide a prediction of the vehicle’s locationat which these ultrasonic sensors were acquired.

In both cases, the � nal result depends on the quality of the measure of thesensor (with the increase of the number of sensors employed, the errors due tofalse punctual measures diminish) and the number of generations used in the searchalgorithm.

The experiments were performed in a 12 £ 7 m2 room, in which seven landmarkshad been placed, and in a 20 m corridor with four landmarks.

The localization system based on genetic algorithms accomplishes the searchprocess within the uncertainty area provided by the EKF in the same localizationcycle. The method based on genetic algorithms provides an optimum solution thatminimizes the errors introduced by the EKF in the equations where the higher orderterms are neglected in the linearization process.

Through these experiments, one can see that a very high precision of a few cen-timetres and degrees can be obtained as long as suf� cient numbers of observationsare made. Positioning errors have been calculated as the difference between theodometry position improved by the localization process and the real position ob-tained by the Geodimeter 600 (theodolite) at each localization point.

Table 1.List of errors at each localization point (path 2)

250 elements £ Sensors EKF error Genetic algorithm error10 generations (cm) (cm)

Point 1 6 5.7 3.9Point 2 5 6.9 4.6Point 3 4 7.6 4.8Point 4 5 6.1 4.4

Table 2.List of errors at each localization point (path 3).

250 elements £ Sensors EKF error Genetic algorithm error10 generations (cm) (cm)

Point 1 6 4.9 3.9Point 2 5 6.7 5.5Point 3 5 5.6 4.8Point 4 4 6.2 5.6Point 5 4 7.8 5.7Point 6 5 6.3 5.4

6. CONCLUSIONS

A localization system for autonomous mobile robot navigation in an indoor semi-structured environment has been described. It is based on the modeling of theultrasonic sensor sensing, visual detecting process and genetic iterative re� nementmethod. The proposed algorithm is based upon an iterative EKF, which utilizesmatches between observed geometric beacons and an a priori map of beaconlocations and the detection of arti� cial landmarks to correct the position andorientation of the vehicle. This method is applicable to mobile robot localization andproved to achieve solutions with computational times less than those search methodswhich provide precision of the same order. Experiments using a real mobile robotin a dynamic environment (laboratories and corridors in the University Carlos IIIof Madrid) have show that this technique can result in signi� cantly more accurateposition estimation. Experimental results show that the proposed localizationsystem achieves a precision of a few centimetres and degrees, depending on thetype of the used sensors (ultrasonic sensors, computer vision).

The proposed method presents an improvement of the order of 15–20% over theEKF solution when dealing with the same localization problem.

Acknowledgements

The authors gratefully acknowledge the funds provided by the Spanish Governmentthrough the CICYT project TAP99-214.

REFERENCES

1. T. D’Orazio, F. P. Lovergine, M. Ianigro, E. Stella and A. Distante, Mobile robot positiondetermination using visual landmarks, IEEE Trans. IE-41 (6), 654–662 (1994).

2. M. Ricotti and A. Liotta, Real time landmarks detection for the mobile robot PARIDE, in: Proc.SPIE, Vol. 2423, pp. 60–71 (1995).

3. M. Han and S. Rhee, Navigation control for a mobile robot, J. Robotics Syst. 11 (3), 169–179(1994).

4. E. Byler, W. Chun, W. Hoff and D. Layne, Autonomous hazardous waste drum inspectionvehicle, IEEE Robotics Automat. Mag. 2 (1), 6–17 (1995).

5. A. Escalera, L. Moreno, M. A. Salichs and J. M. Armingol, Continuous mobile robot localizationby using structured light and a geometric map, Int. J. Syst. Sci. 27 (8), 771– 782 (1996).

6. B. Triggs, Model-based sonar localization formobile robots, Robotics Autonomous Syst. 12,173–186 (1994).

7. J. J. Leonard and H. F. Durrant-White, Directed Sonar Sensing for Mobile Robot Navigation.Kluwer, Dordrecht (1992).

8. M. Betke and L. Gurvits, Mobile robot localization using landmarks, IEEE Trans. RA-13 (2),251–263 (1997).

9. Ph. Bonnifait and G. Garcia, A multisensor localization algorithm for mobile robots and its real-time experimental validation, in: Proc. IEEE Int. Conf. on Robotics and Automation, Vol. 2,pp. 1395–1400 (1996).

10. B. Schiele and J. L. Crowley, A comparison of position estimation techniques using occupancygrids, Robotics Autonomous Syst. 12, 163–171 (1994).

11. H. P. Moravec and A. Elfes, High resolution maps from angle sonar, in: Proc. IEEE Int. Conf.on Robotics and Automation, pp. 116–121 (1985).

12. G. Wichet von, Mobile robot localization using a seft-organized visual environment representa-tion, Robotics Autonomous Syst. 25, 185– 194 (1998).

13. R. Y. Tsai, An ef� cient and accurate camera calibration technique for 3D machine vision, in:Proc. Comp. Vis. Pattern Recog., pp. 364– 374 (1986).

14. J. M. Lavest, G. Rives and M. Dhome, Three-dimensional reconstruction by zooming, IEEETrans. RA-9 (2), 196–207 (1993).

15. F. Dellaert, D. Fox, W. Burgard and S. Thrun, Monte Carlo localization for mobile robots, in:Proc. IEEE Int. Conf. on Robotics and Automation, pp. 1322–1328 (1999).

16. D. E. Goldberg, Genetic Algorithm in Search, Optimization, and Machine Learning. Addison,Wesley, Reading, MA (1989).

17. M. Chen and A. Zalzala, Safety considerations in the optimization of paths for mobile robotsusing genetic algorithms, in: Proc. 1st Int. Conf. on Genetic Algorithms in Engineering Systems:Innovations and Applications, pp. 299–304 (1995).

18. C. Hein and A. Meystel, A genetic technique for robotic trajectory planning, Telemat. Informat.11, 351–364 (1994).

19. J. Potvin, C. Duhamel and F. Guertin, A genetic algorithm for vehicle routing with backhauling,Appl. Intelligence 6, 345–355 (1996).

20. D. Kang, H. Hashimoto and F. Harashima, Path generation for mobile robot navigation usinggenetic algorithm, in: Proc. IEEE IECON: 1st Int. Conf. Industrial Electronics, Control andInstrumentation, Vol. 1, pp. 167–172 (1995).

21. L. Moreno, M. A. Salichs, D. Gachet and J. Pimentel, Neural networks for mobile robotpiloting control, in: Applications of Arti� cial Neural Networks in Robotic Control, pp. 137–161.Prentice-Hall, Englewood Cliffs, NJ (1995).

22. S. Garrido, L. Moreno and C. Balaguer, State estimation for nonlinear systems using restrictedgenetic optimization, in: Proc. 11th Int. Conf. on Industrial and Engineering Applications ofArti� cial Intelligence and Expert Systems, Vol. 1, pp. 758–767 (1998).

ABOUT THE AUTHORS

José María Armingol is Associate Professor of industrial engineering in theDivision of Systems Engineering and Automation at Universidad Carlos III deMadrid. He received the Degree in automation and electronics engineering fromthe Universidad Politécnica de Madrid (Spain) in 1992 and the PhD degree inindustrial engineering from the Universidad Carlos III de Madrid (Spain) in1997. His research interests are in mobile robotics, image processing and patternrecognition for intelligent autonomous vehicles.

Arturo de la Escalera graduated from the Universidad Politecnica de Madrid(Madrid, Spain) in Automation and Electronics Engineering in 1989, where healso obtained his PhD degree in Robotics in 1995. In 1993 he joined the Depart-ment of Robotics and Automation of Universidad Carlos III de Madrid, (Madrid,Spain) where he is an Associate Professor from 1995. His current research in-terests includes Advanced Robotics and Intelligent Transportation Systems withspecial emphasis on vision sensor systems and image data processing methods forenvironment perception and real-time pattern recognition.

Luis E. Moreno received the degree in Automation and Electronics Engineeringin 1984 and the PhD degree in 1988 from the Universidad Politécnica de Madrid,Madrid, Spain. From 1988 to 1994, he was an associate professor at theUniversidad Politécnica de Madrid. In 1994 he joined the Division of SystemsEngineering and Automation, Universidad Carlos III de Madrid, Madrid, Spain,where he has been involved in several mobile robotics projects. He is the authorof more than 80 contributions to international conferences, journals, and books.His research interests are in the areas of mobile robotics, mobile manipulators, 2Dand 3D environment modelling, intelligent control, and mobile robot localizationproblems.

Miguel A. Salichs received the Electrical Engineering and PhD degrees fromPolitechnical University of Madrid in 1978 and 1982, respectively. He is currentlya Full Professor of Systems Engineering and Automation at the University CarlosIII of Madrid. He is Chairman of the Technical Committee on IntelligentAutonomous Vehicles of the International Federation on Automatic Control(IFAC). He has published more than 80 papers on robotics and automation. Hisprimary research interests are mobile robotics, intelligent autonomous systemsand service and personal robots.