A Hybrid Framework for Mobile Robot Localization ... · A Hybrid Framework for Mobile Robot...

Autonomous Robots 15, 169–191, 2003c© 2003 Kluwer Academic Publishers. Manufactured in The Netherlands.

A Hybrid Framework for Mobile Robot Localization:Formulation Using Switching State-Space Models

HARIS BALTZAKIS AND PANOS TRAHANIASInstitute of Computer Science, Foundation for Research and Technology—Hellas (FORTH), P.O. Box 1385,

Heraklion, 711 10 Crete, GreeceDepartment of Computer Science, University of Crete, P.O. Box 1470, Heraklion, 714 09 Crete, Greece

[email protected]

[email protected]

Abstract. In this paper we address one of the most important issues for autonomous mobile robots, namely theirability to localize themselves safely and reliably within their environments. We propose a probabilistic frame-work for modelling the robot’s state and sensory information based on a Switching State-Space Model. Theproposed framework generalizes two of the most successful probabilistic model families currently used for thispurpose: the Kalman filter Linear models and the Hidden Markov Models. The proposed model combines the ad-vantages of both models, relaxing at the same time inherent assumptions made individually in each of these existingmodels.

Keywords: localization, Kalman filters, Hidden Markov Models, Switching State-Space Models, hybrid models

1. Introduction

Building autonomous mobile robots is one of theprimary goals of robotics and artificial intelligenceresearch. In order to carry out complex navigationaltasks, an autonomous robotic agent must be able toprovide answers to the “Where am I?” question (Levittand Lawton, 1990), that is, to localize itself within itsenvironment.

To reduce the inherent complexity associated withthis problem, adoption of appropriate geometric con-strains in combination with effective modeling ofrelated information is required. The achieved abstrac-tion, not only makes robotic problems computation-ally feasible, but also provides robustness in the pres-ence of noise, or other, often unpredictable, factors.Successful probabilistic models proposed for this pur-pose in the past, generally fall into two major cate-gories: Hidden Markov Models (HMM) (Cassandra etal., 1996; Dellaert et al., 1999; Thrun et al., 2000) andKalman filters (Arras et al., 2000; Castellanos et al.,1998; Lu and Milios, 1994).

In Gutmann et al. (1998) a comparison of these twomajor classes of approaches has been performed. Theresults indicate the superiority of the Kalman filter ap-proaches with respect to computational efficiency, scal-ability, and accuracy. On the other hand, HMM-basedapproaches were proved to be more robust in the pres-ence of noise and/or unreliable odometry information.This comparative work also encourages work in hy-brid approaches, noting that combination of these tech-niques could lead to localization methods that inheritadvantages from both.

In this paper we propose a probabilistic frameworkfor modelling the robot’s state and sensory information,based on Switching State-Space Models (Ghahramaniand Hinton, 1996). The proposed approach combinesadvantages from both models mentioned above, relax-ing at the same time inherent assumptions made indi-vidually in each of these existing models. A central con-cept in our framework is to let HMM models handle thequalitative aspects of the problem, i.e., perform coarselocalization, and Kalman filters the metric aspects, thatis, elaborate on the previous result and provide accurate

170 Baltzakis and Trahanias

localization. Discrete HMM update equations are usedin order to update the probabilities assigned to a fixed,small number of discrete states, while Kalman filterbased trackers, operating within each discrete state, areused in order to provide accurate metric information.

High-level features, consisting of sequences of linesegments and corner points, extracted robustly fromlaser range data, are used to facilitate the implementa-tion of the model, while, a fast dynamic programmingalgorithm is used in order to produce and score matchesbetween them and an a-priori map. Experimental re-sults have shown the applicability of the algorithm forindoor navigation tasks where the global localizationcapabilities of the HMM approaches and efficiency andaccuracy of the Kalman filter based approaches are re-quired at the same time.

2. Background

In probabilistic terms, localization is the process of de-termining the likelihood of finding the robot’s state xt

at time instant t , 1 ≤ t ≤ T , given a sequence of ob-servations {y}T

1 = y1, . . . , yT , that is determining theprobability P(xt | {y}T

1 ). State vector xt is usually com-posed of the 2D position and orientation of the robotwith respect to some arbitrary 2D coordinate system,while, the vector of observations yt expresses sensorand odometry readings.

In practice, it is too difficult to determine the jointeffect of all observations up to time T . A common as-sumption made in order to overcome this difficulty isthat the hidden state vectors obey the Markov inde-pendence assumption (often referred also as the staticworld assumption), so that the joint probability for thesequences of states xt can be factored as

P({x}T

1 , {y}T1

)

= P(x1)P(y1 | x1)T∏

t=2

P(xt | xt−1)P(yt | xt ) (1)

The conditional independence specified by Eq. (1) canbe expressed graphically in the form of the indepen-dence graph depicted in Fig. 1. In this figure, horizontalarrows symbolize the transition functions P(xt | xt−1),while, vertical arrows symbolize the output (emission)functions P(yt | xt ). Both transition and emission func-tions can, in general, be arbitrary functions. The distri-butions of the state and the observation variables canbe arbitrary as well.

Figure 1. A network-like graph specifying conditional indepen-dence relations for the state-space model of Eq. (1).

For localizing the mobile agent at time t , observa-tions {y}t

1 up to time t are available and the relatedproblem (often referred as the Inference problem) canbe formulated as:

Localization Problem: Compute the probability thatthe robot is at pose z at time t given all observations upto time t , i.e., compute p(xt = z | {y}t

1).In order to compute the probabilities p(xt = z |

{y}t1), Eq. (1) should be integrated over all possible

states, which results in the recursive formula

P(xt

∣∣ {y}t1

) = β ·∫

P(xt | xt−1, yt )

× P(xt−1

∣∣ {y}t−11

)dxt−1 (2)

where β is a scaling factor ensuring that P(xt | {y}t1)

sums up to unity over all possible states. It is obviousfrom Eq. (2) that all information about the past his-tory of the robot can be represented by the distributionP(xt−1 | {y}t−1

1 ).The formulation stated above dictates that practically

any probabilistic localization algorithm must addressthe following points (Gutmann et al., 1998):

1. How is the prior distribution P(xt−1 | {y}t−11 ) rep-

resented?2. How is the posterior distribution P(xt | {y}t

1) cal-culated?

Depending on the method employed to tackle the firstpoint, two major classes of approaches exist, leadingto different treatments for the second point.

2.1. The Discrete Case

When the state variables xt become discrete, the inte-grals in Eq. (2) become sums and the prior probabilitiescan be computed and stored explicitly. For this purpose,grid maps are usually employed to cover the spaceof all possible states and keep probabilities for eachindividual element. This leads to the Hidden Markov

A Hybrid Framework for Mobile Robot Localization 171

Model (HMM), a completely general model in terms ofwhat the transition and the emission probabilities canbe. Different variants of this scheme for localizationand map building have been developed utilizing eitherfine-grained (Thrun et al., 2000) or coarser topologicalgrids (Cassandra et al., 1996; Nourbakhsh et al., 1995;Simmons and Koenig, 1995) of the robot’s state space.Such approaches are characterized by the followingfeatures (Gutmann et al., 1998; Scott et al., 2000):

• Ability (to a certain extent) to localize the robot evenwhen its initial pose is unknown.

• Ability to represent ambiguities.• Computational time scales heavily with the number

of discrete states (dimensionality of the grid, size ofthe cells, size of the map),

• Metric localization accuracy (wherever metric infor-mation is calculated) is limited by the size and thenumber of the cells.

One more characteristic, specific to fine-grained gridapproaches, is the capability of utilizing row sensordata for probability updates, whereas in topological ap-proaches higher level features need to be extracted forthis purpose due to the lack of accurate metric robotpose information.

To combine computational efficiency and accuracyat the same time, Burgard et al. (1998) has proposed adynamic scheme for selectively updating only the beliefstates of the most likely parts of a fine-grained grid. Amore recent approach proposed by Dellaert et al. (1999)and Thrun et al. (2001) is based on the representationof the belief state of the robot via a set of samples,namely Monte Carlo localization. Here, computationaltimes and accuracy are independent of the size of themap and the number of grid cells, but still depend on thenumber of samples utilized for representing the beliefof the robot.

2.2. The Continuous Case

When the state variables xt are continuous the probabil-ities must be calculated analytically and stored implic-itly. This leads to restrictions on the kinds of transitionand emission densities that can be handled efficiently,i.e., in computational time linear to the measurementsequence length. The restrictions stem from the re-quirement that multiplications with the transition andemission densities should not make the distributionsmore complex, but only change their parameters. The

only family of distributions that is closed to multipli-cation is the exponential family. Fortunately, the expo-nential family of distributions is quite large includingthe Gamma, Poison, Beta, and, most importantly, theGaussian distribution. By choosing the distributions tobe Gaussians, which is a reasonable choice given noa-priori information, Eq. (2) leads to a set of linearequations (Minka, 1999; Murphy, 1998):

xt = At xt−1 + wt (3)

wt ∼ N (0, �t ) (4)

yt = Ct xt−1 + vt (5)

vt ∼ N (0, �t ) (6)

In such models, often called Linear Dynamical Sys-tems (LDSs), the Kalman filter update equations(Bar-Shalom and Li, 1993; Maybeck, 1979) are usedin order to propagate recursively the state probabil-ity distribution forward in time. It can be proven thatthey give rise to optimal solutions with respect to vir-tually any relevant criterion (Maybeck, 1979). If thetransition and/or the observation functions are not lin-ear (common case in robot localization), a linearizationprocedure is usually applied, resulting to the so-calledExtended Kalman Filter (EKF).

Kalman filters and their extensions have beenused by many researchers in robot navigation tasks(Arras et al., 2000; Castellanos et al., 1998; Lu andMilios, 1994) and are characterized by the followingproperties:

• Requirement of known initial state of the robot.• Perform very accurately if the inputs are precise.• Inability to recover from catastrophic failures caused

by erroneous matches or incorrect error models.• Inability to track multiple hypotheses about the lo-

cation of the robot.• Computational efficiency.

2.3. Comparison and Hybrid Approaches

In Gutmann et al. (1998) a comparison of the two ma-jor classes of approaches presented above has beenperformed. The results indicate the superiority of theKalman filter approaches with respect to computationalefficiency, scalability, and accuracy. On the other hand,HMM-based localization approaches were proved to bemore robust in the presence of noise and/or unreliableodometry information. In Scott et al. (2000), a simi-lar comparison between three representative mapping


algorithms has been performed. The results confirmedthe above stated observations.

Very recently, Thiebaux and Lamb (2000) andDuckett and Nehmzow (2001) have proposed methodsthat combine elements from both model families pre-sented above. In Thiebaux and Lamb (2000) a methodis proposed for localization in “network-like” environ-ments. Kalman filtering is used for position tracking inone-dimensional straight line segments (such as roadsor sewer system pipes) and Discrete HMM is used inorder to describe transition from one line segment tothe other (e.g., when the robot/vehicle reaches a cross-road). In Duckett and Nehmzow (2001) a localizationscheme is proposed for indoor robots based on a verycoarse grid. Discrete Markovian dynamics are used inorder to describe the robot transition from one cell tothe other. However, within each grid cell, robot positionis computed by matching sensor information to a-priorilearned histograms, while robot orientation is alwayskept constant by the use of a compass.

Combinations of discrete and continuous modelscan also be found in multiple hypothesis trackingapproaches. In Jensfelt and Kristensen (2001) andRoumeliotis and Bekey (2000), multiple Kalman track-ers are assumed, each of which is assigned a probabilitythat evolves according to discrete dynamics. In theseapproaches, Kalman trackers are dynamically gener-ated to accommodate hypotheses about the solution ofthe data association problem (Cox and Leonard, 1994).

3. The Switching State-Space Approach

As outlined in the previous section, Kalman filter andDiscrete HMM-based approaches have complementaryadvantages. Kalman filtering is efficient and accurate,but confined to small Gaussian sensor perturbations,while discrete Markov modeling is more robust to noiseand high levels of uncertainty. In this paper we intro-duce a hybrid model that merges ideas underlying onboth the Kalman filter and the HMM approaches. Dis-crete HMM update equations are used in order to up-date the probabilities assigned to a small number ofdiscrete states, while Kalman filter based trackers, op-erating within each discrete state, are used in order toprovide accurate metric information.

A central concept in our model is to let the DiscreteMarkov Models handle the qualitative aspects of theproblem, i.e., perform coarse localization, and Kalmanfilters the metric aspects, that is, elaborate on the pre-vious discrete result and provide accurate localization.

Figure 2. A switching state-space model.

This is achieved by partitioning the robot’s state-spaceinto a fixed, small number of discrete states and assign-ing dynamically created hypotheses about the robot’sposition to each of them. Discrete Markov model up-date equations are utilized in order to update the stateprobabilities, while, for updating hypotheses, standardextended Kalman trackers are used. We show how linesegments and corner points extracted from laser rangedata can be utilized in order to dynamically generateand Kalman-update trackers, and how global localiza-tion is achieved by effectively computing the probabil-ities of the discrete states.

An abstract graphical representation of the proposedmodel is depicted in Fig. 2. In such models, known asSwitching State-Space Models (SSSM) (Ghahramaniand Hinton, 1996), the probabilistic relation betweenthe observations {y}T

1 is modeled by the utilizationof M real-valued state vectors, x (m)

t , mε{1, 2, . . . , M}and one discrete state vector St . Each real-valued statevector evolves to linear Gaussian dynamics, while,the discrete state, St , called “the switch”, is modeledas a multinomial variable that can take on M valuesStε{s1, . . . , sM} and transitions according to DiscreteMarkovian Dynamics. The function of the switch St isto choose the proper real-valued state vector that mostlikely represents the correct position of the robot withinits environment. Such models have been successfullyemployed in econometrics, signal processing and otherfields and efficient algorithms for solving the inferenceand learning tasks in such networks have been proposed(Ghahramani and Hinton, 1996).

Application of the proposed model to the localizationtask requires that the robot’s state-space is partitionedinto a fixed number of partitions; this is accomplishedin this work according to corner point visibility infor-mation. The belief of the robot regarding the partition


Figure 3. Operation of the hybrid localization algorithm.

it may be in, constitutes the switch St of Fig. 2 andevolves according to discrete Markovian Dynamics.Since in the employed formulation state selection isperformed via the switch, the term “switcher states”is used hereafter to denote them. Multiple Kalmantrackers are assigned to switcher states that are re-sponsible for dynamically generating and handlingthem.

Figure 3 gives an overview of the operation of thecomplete localization algorithm utilizing the switch-ing state-space model. Initially (Step A), line segmentsextracted out of raw sensor data are used in order to up-date all Kalman trackers. In Steps B and C, new track-ers are initiated and handled as needed by consideringcorner points (i.e., intersections of adjacent line seg-ments), while, in Step D, switcher states are assignednew probabilities by considering discrete Markoviandynamics.

To successfully apply the proposed model to the lo-calization task, appropriate modelling of all param-eters involved is very significant. In the next sec-tion we describe the modeling of the robot state andsensory information. Further, higher order modelingof sensor information in order to extract a powerfulset of features consisting of ordered sets of line seg-ments and corner points is described in Section 3.2. InSections 3.3 and 3.4 we give the HMM and the Kalmanfilter state-update equations, while, related, implemen-tation specific details are given in Section 3.5. InSection 3.6, appropriate mapping and discrete state par-titioning strategies are discussed. Finally, Section 3.7presents a comparison of existing multiple hypothesistracking approaches with our approach.

3.1. State and Sensor Modeling

3.1.1. Robot State Model. Robot’s state at time t ismodelled as a Gaussian distribution xt ∼ N (µxt , �xt ),where µxt = (xt , yt , θt )T is the mean value of robot’sposition and orientation, and �xt , the associated 3 × 3covariance matrix.

3.1.2. Odometry Model. A robot action αt at time t ,as measured by the odometry encoders of the robot,can also be modeled as a Gaussian distribution of atranslation dt followed by a rotation ft . That is, αt ∼N (( ft , dt )T , �at ).

Odometry errors �at are computed as a function ofthree constants kr (range error), kt (turn error) and kd

(drift error) that can be experimentally calculated.

�at =[

kr dt 0

0 kt ft + kddt

](7)

3.1.3. Transition Model. According to odometrymeasurements, αt , the robot’s state xt can be updatedusing the formulas

µxt+1 = Exp(F

(µxt , αt

)) =

xt − dt sin(θt )

yt + dt cos(θt )

θt + dt

(8)

�xt+1 = ∇Fx�xt ∇F Tx + ∇Fα�αt ∇F T

α (9)

where Exp is the expectation operator, F the transitionfunction and ∇Fx and ∇Fα denote the Jacobians of Fwith respect to µxt and αt .

3.1.4. Sensor Model. A range measurement r at angleφ can be transformed into Cartesian coordinates by afunction R

[x, y]T = Exp(R(r, φ))

= [r cos(φ), r sin(φ)]T (10)

The rangefinder error in polar coordinates is obtainedas

�polar =[

kφφ 0

0 kρ0 + kρ1r

](11)

where kφ , kρ0 , kρ1 are parameters related to the rangefinder angular error, range error independent of dis-tance, and distance-dependent range error, respectively.


The transformation of �polar to Cartesian coordinatesis

�p = ∇ R�polar∇ RT (12)

where ∇ R is the Jacobian of R with respect to (φ, r ).

3.2. Feature Extraction

Line segments and corner points extracted out of laserrange measurements constitute the feature set currentlyutilized in the proposed framework. Line segments areextracted directly from range data and constitute thefeature set utilized by the Kalman trackers of the pro-posed SSSM. Corner points are computed as the in-tersection points of subsequent line segments and areemployed by the HMM (switch in the SSSM) in thestate estimation process.

3.2.1. Line Segments. For line segment extraction,a three-stage algorithm has been implemented. Rangemeasurements are initially grouped to clusters of con-nected points according to their Sphere-of-Influencegraph (Duda and Hart, 1973). Clusters are then further-segmented to points belonging to the same line segmentby utilization of the well-known Iterative-End-Point-Fit (IEPF) algorithm (Michael and Quint, 1994). IEPFrecursively splits a set of points until a distance relatedcriterion is satisfied.

After range points have been clustered into groupsof collinear points, a recursive Kalman-filter-basedmethod is used in order to “best-fit” lines to them, min-imizing the squared radial distance error from the po-sition of the range measuring device to each point. Theexact process is as follows.

Line l = N ((l f , ld )T , �l) is initially defined by itsextreme points p1 and pn . Recursively, each other pointpt , (1 < t < n), is used in order to update the lineestimate l. A prediction of point pt ∼ N ((xt , yt )T , �pt )is computed as the trace (x−

t , y−t ) on l of the line passing

through the sensor and pt . Defining Tr as the functionthat computes the trace, yields:

(x−t , y−

t )T = Exp(Tr(l f , ld ))

=

ld tan(u)

tan(u) cos(l f ) − sin(l f )

−ld

tan(u) cos(l f ) − sin(l f )

(13)

where u = tan−1(yt/xt ). The EKF update equationscan be written as

(l f , ld )′T = (l f , ld )T +K ((xt , yt )T −(x−

t , y−t )T ) (14)

�′l =�l − K∇Tr−

l �l (15)

where ∇Tr−l is the Jacobian of Tr with respect to l,

obtained by linearizing about the prediction point, andK , the Kalman gain given by

K = �l∇Tr−l

T (∇Tr−l �l∇Tr−

lT + �pt

)−1(16)

3.2.2. Corner Points. Corner points are computed atthe intersection points of directly adjacent line seg-ments. A corner point is defined as c ∼ N ((cx , cy)T ,

�c) where (cx , cy)T = I ( f1, d1, f2, d2) is the intersect-ion of lines l1 ∼ N (( f1, d1)T , �l1 ) and l2 ∼ N (( f2,

d2)T , �l2 ), and �c is the related covariance matrix,computed as

�c = J1�l1 J T1 + J2�l2 J T

2 (17)

Where J1, J2 are the jacobians of I with respect to( f1, d1)T and ( f2, d2)T , respectively.

3.3. Discrete Model

Discrete probabilistic approaches, as the ones de-scribed in Section 2.1, have been widely used formobile robot localization. In their most generalform, robot’s state-space is discretized in a three-dimensional, fine-grained histogram, representing thebelief of the robot about its state. Each histogram cellcorresponds to a single (xt , yt , θt ) triplet and its valuerepresents the probability, according to robot’s belief,of the robot being at this state. For updating the his-togram values, Eq. (2) is rewritten as

P(sk

t

) = β · P(

ft

∣∣ skt

) ∑i

P(si

t−1

)P

(sk

t

∣∣ sit−1, αt

)

(18)

where P( ft | skt ) is the probability to observe feature

set ft given that the robot is at cell skt , P(sk

t | sit−1, αt ) is

the probability of jumping from cell sit−1 to cell sk

t giventhe odometry measurements αt , and β is a normalizingfactor ensuring that all probabilities sum up to one. Thesummation index i spans over all cells in the histogram.


Despite the obvious advantages stemming from itsinherent simplicity, this model suffers from computa-tional inefficiency. Depending on the dimensions ofthe environment, the number of the cells to be updatedoften becomes very large, and may result in unafford-able computational times for real time applications. Toovercome this limitation, various methods have beenproposed for selectively updating only portions of thehistogram (Burgard et al., 1998; Fox et al., 1999), de-parting however from the generality and the simplicityof the original method.

In the proposed framework, it is desirable to partitionthe robot’s state-space into a relatively small number ofswitcher states; such states should convey coarse local-ization information. For meaningfully partitioning therobot’s state space into switcher states, various strate-gies can be adopted. Any such successful partitioningstrategy should have the following characteristics:

• Produce partitions dense enough so that localizationambiguities are minimized.

• Produce partitions wide enough so that the totalnumber of partitions is not prohibitive for real timelocalization.

• Ability for automatic application, without human su-pervision, independently of the map size and com-plexity.

• Facilitate the derivation of a function/algorithmfor determining the partition, given the robot’sworkspace and the features used to obtain thispartition.

3.3.1. State Partitioning. In this paper we employ amethod for partitioning the robot’s state-space accord-ing to structural information contained in a map of theenvironment. The features used by the method are cor-ner points, i.e., intersection points of consecutive linesegments in the map. The switcher states strictly cor-respond to the corner points. Thus, the total number ofpartitions equals the number of corner points of the mapand partitioning is achieved via a “visibility” function,that is by computing the corner point most likely to bevisible from each possible state of the robot.

Figure 4(b) illustrates specific instances of the par-titioning for a simple artificial map shown in Fig. 4(a);the map extents in the x, y axes are from −8 to +8. Two“2D slices” of the robot’s 3D state-space (x, y, θ ) cor-responding to θ ∼ 180◦ and θ ∼ 270◦ are depicted onthe right part of Fig. 4(b). Different discrete states areshown in different colors, while corresponding corner

points are marked with circles painted with the samecolor.

For implementing the “visibility” function, the view-ing angles (VA) ai

1, ai2 of the visible parts of the pair

of line segments that define a corner point ci are con-sidered. The visibility of the corner point is defined asthe minimum of the two VAs corresponding to the twoline segments that constitute the corner point. That is,the index imax of corner point most likely to be visibleis given as

imax = arg maxi

(min

(ai

1, ai2

))(19)

The employment of the specific visibility function isjustified in our case by the use of a laser range finderfor providing input data for feature extraction. Laserrange finders have a constant angle resolution, hence,the total number of range samples corresponding toeach line segment, is proportional to the VA of thatline segment. The greater the VA of a line segment,the larger the amount of range information utilized bythe feature extraction algorithm and, therefore, themore accurate the parameter extraction process for thisline segment.

Figure 5 demonstrates the process of calculating thevisibility function for three corner points, ca , cb and cc.Assuming the robot in the shown configuration, thefunction’s value is greater for ca compared to cc, sinceaa

2 (=min(aa1, aa

2)) is greater than ac1 (=min(ac

1, ac2)).

Corner point cb is not visible at all. Therefore, theswitcher state at the current configuration is the onecorresponding to corner point ca .

It is evident that discrete partitioning of the robot’sstate-space, like the one explained above, combinedwith the discrete Markovian dynamics of Eq. (18),provides a simple, yet powerful, framework for rep-resenting the robot’s uncertainty. The produced statescorrespond strictly to the features (corner points) usedfor their definition and, moreover, convey structuralinformation. Hence, they can be straightforwardly andunambiguously extracted (as outlined above) and alsoprovide coarse localization information.

The number of discrete states (partitions), althoughstill scaling with the size of environment, is by far muchlower than that of grid based representations, permit-ting real-time probability updates. Another character-istic of the proposed partitioning method is that thenumber of partitions scales in proportion to the com-plexity of the environment (the more complex the en-vironment, the larger the number of corner points, the


Figure 4. State-space partitioning.

larger the number of partitions). It should be noted atthis point that even in the more complex and large envi-ronments that we experimented in (see later Section 4),the number of discrete states was always small (belowthirty), thus facilitating real-time computations.

Figure 5. Determining the visibility of corner points.

3.3.2. Metric Information. The previous describedpartitioning of the robot’s state space, although beingappropriate for providing coarse localization informa-tion, cannot provide accurate (metric) localization es-timates (determination of the exact robot’s pose). Thisis tackled in our model by appropriate utilization ofa Kalman filtering based methodology. Being able toaccurately track the robot’s state, Kalman filter basedtrackers assigned to the discrete partitions are utilized inorder to provide metric information, not easily achiev-able by any discrete HMM based approach.

This combination of discrete HMM dynamicswith continuous Kalman filter dynamics constitutesa switching state-space model as the one depicted inFig. 2. Partitions of the robot’s state-space constitutethe switcher’s set of possible discrete states, whileextended Kalman trackers, continuously tracking therobot’s state, constitute the set of linear dynamicalsystems attached to the corresponding switcher states.Switcher states are assigned probabilities that evolveaccording to Eq. (18), while metric information withineach switcher state is computed by the Kalman filter


state-update equations given in the next section. TheKalman tracker pointed out by the most probable state,signifies the most likely state of the robot.

Implementation details concerning both the Kalmantracking methodology and the computation of the prob-abilities involved in Eq. (18) are given in the next twosections.

3.4. Kalman Filtering

As mentioned in the previous section, Kalman track-ers assigned to switcher states are utilized in order toprovide accurate metric information about the robot’sstate. The features used for this task are line seg-ments that are extracted by the procedure described inSection 3.2. These features are matched to an a-prioriknown set of line segments (map). An EKF is then used,that employs sequentially each matched pair, in orderto track the robots state over time.

The transition model of the Kalman filter, that is, thefunction used to project state estimates forward in time(prediction step) is given according to Eqs. (8) and (9)as

µx−t+1

= F(µxt , αt

)(20)

�x−t+1

= ∇Fx�xt ∇F Tx + ∇Fα�αt ∇F T

α (21)

Using this predicted state, a known line segment l ispredicted as

l−t+1 = H(µx−

t+1

)(22)

Where H is the function used to convert line segmentparameters to robot frame coordinates and is given by

H = (θt − l f , ld − xt cos(l f ) − yt sin(l f ))T (23)

The difference between the predicted line segment l−t+1and the measured line segment lt+1 is the measurementresidual (Kalman Innovation) and can be written as

rt+1 = lt+1 − l−t+1 (24)

�rt+1 = ∇ Hx−t+1

�x−t+1

∇ H Tx−

t+1+ �lt+1 (25)

where �lt +1 is the measured line segment covarianceand ∇ Hx−

t+1is the Jacobian of H , obtained by lineariz-

ing about the state prediction x−t+1.

The Kalman gain is computed as

Kt+1 = �x−t+1

∇ H Tx−

t+1�−1

rt+1(26)

and, finally, the update to the state prediction is:

µxt+1 = µx−t+1

+ Kt+1rt+1 (27)

�xt+1 = �x−t+1

− Kt+1�rt+1�x−t+1

(28)

3.4.1. Feature Matching. The success of any Kalmanfiltering method for localization tasks relies heavily onthe correct matching of features. Various methods forthis purpose have been proposed in the past (Gutmannet al., 2001; Lu and Milios, 1994).

In this paper we propose a fast and accurate methodbased on a dynamic programming string-search al-gorithm. The motivation for the introduction of thisalgorithm was the quest for a fast feature matchingalgorithm that exploits information contained in boththe features themselves and their spatial ordering aswell. Other contemporary algorithms rely on exhaus-tive feature comparisons in order to consider the spa-tial ordering of features, with an obvious effect on theircomputational performance.

The proposed algorithm originates from a class ofdynamic programming algorithms (Benson, 1997) verywidely used for matching, aligning and measuring thedegree of structural similarity between biological se-quences. Given a pair of sequences, the dynamic pro-gramming algorithm performs a global alignment ofthe second sequence on the first, finding all solutionsthat produce a maximum score according to a scor-ing function. As an example, consider the charactersequences: ABBCDEFG and BCEAA. The algorithmattempts to align BCEAA sequence on ABBCDEFG,wrapping around the edges of the latter if necessary.The result depends on the scoring function assumed.A typical scoring function, as the one used in our im-plementation (see later in this section), produces thefollowing two, equally scored, alignments:

ABBCDEFG ABBCDEFG

| | | |BC E AA

BC E AA

Measured line segments and map line segmentscomprise the pair of sequences to be matched by thedynamic programming algorithm. The measured line


segment sequence is comprised of all line segments ex-tracted by the feature extraction algorithm. For gener-ating the map line segment sequence, a fast ray tracingalgorithm seeks visible line segments at the predictedstate given by Eq. (20). In order to compensate withpossible errors in the predicted state, causing visibleline segments to be missing from the predicted mapline segment sequence, non visible line segments thatare directly connected to visible ones through cornerpoints are inserted to the predicted map line segmentsequence as well.

For the purpose of this work, a scoring functionhas been chosen that equally rewards matches withinthe measured and the map line segment sequences ifthe measurement residual of Eq. (24) is bellow alarge threshold (experimentally obtained). Gaps inboth the measured and the map sequences are allowedby the scoring function, although the former are slightlypenalized. This means that no penalty is applied whena line segment within the map sequence is not matchedwith any measured line segment. On the contrary mea-sured line segments that are not matched with any seg-ments on the predicted map sequence will result in apenalization. The latter is marginal so that it will notaffect drastically the total score, but still will force an-other alignment, if such one exists, to become morepreferable.

Evidently, the dynamic programming matchingmethod exploits information contained in the spatialordering of the features, offering, at the same time,computational efficiency.

3.5. Switching Operation and Implementation Issues

The Kalman filtering method discussed in the previoussection is unable to perform global localization. Evenwith correct feature matches available, the unimodalityof Gaussian distributions, used to express the belief ofthe robot about its state, confines the robot to a sin-gle hypothesis about its state. An obvious case wherethe Kalman filter fails to achieve global localization isrobot “kidnapping” (Fox et al., 1999).

In this work, multiple Kalman trackers, assignedto multiple hypotheses about the robot’s state are as-sumed. More specifically, Kalman trackers are assignedto switcher states that are responsible for dynamicallygenerating and handling them. For generating trackers,extracted corner points are used, as described later inthis section.

A fundamental theoretical issue with the hybridmodel described above is that the number of Kalmantrackers grows with time. To understand this, supposethat initially we have M Kalman trackers, one for eachswitcher state. Assuming that each switcher state cor-responds to a data association ambiguity per iteration(a measured corner point that has to be matched), eachof the M initial Kalman trackers will be propagatedthrough M different equations (one for each data asso-ciation ambiguity, that is one for each switcher state).After t iterations there will be in total Mt Kalman track-ers, one for each possible path through discrete states.

One approach in reducing the number of Kalmantrackers comes from the observation that most of thesepaths are very improbable and can safely be pruned.Keeping the most probable paths and pruning the restresembles the multiple hypothesis tracking approach(Jensfelt and Kristensen, 2001; Roumeliotis and Bekey,2000) and is referred in the literature as selection ap-proximation (Bar-Shalom and Li, 1993).

Another approach stems from the observation thatmany paths result in trackers that are very close to eachother and can safely be merged together, this being themost common case for paths that are very similar intheir recent history. In our approach we merge all pathsthat differ in their history one step ago (this is referredin SSSM terminology as collapsing approximation oforder one (Bar-Shalom and Li, 1993)) and is equivalentto keeping only one Kalman tracker per switcher state.

The implied assumption in keeping only one Kalmantracker per discrete state is that, given the discrete state(i.e., given the partition of the state-space that the robotis currently in) and a number of Kalman trackers withinthat state, it is relatively easy to decide witch Kalmantracker is the correct one. This statement is equivalent tothe assumption that the discrete partitions of the robot’sstate-space are simple and small enough so that no dataassociation problems exist within them. In practice, theobservations of the robot at time instant t would onlymatch with the predicted ones at the position of at mostone Kalman tracker per switcher state. This means thatall but at most one Kalman tracker per switcher stateshould have very high covariances and/or very highinnovations.

Each switcher state is responsible for managingKalman trackers within itself, according to some prede-fined strategy. As outlined above, the strategy adoptedfor this purpose, consists of merging (as an averageweighted by their covariances) all Kalman trackers thatare close to each other and pruning all Kalman trackers,


except the most probable, within each discrete state(the one with the lowest covariance and/or the lowestinnovation).

Although only one Kalman tracker is eventually keptin each discrete state, propagating this tracker throughthe equations corresponding to all discrete states (thatis, equations corresponding to all data association am-biguities) can be time consuming. Instead, in this workKalman trackers are propagated only according to lo-cal data association solutions. Moreover, line segmentsinstead of corner points are used for this purpose, re-sulting in simpler Kalman update equations and reduc-ing the risk of no feature (corner point) being visiblefor a long period of time. Kalman trackers not beingcreated due to using only local data association solu-tions may affect the algorithm’s ability to cope with the“kidnapped robot” problem. To compensate for that, aprocedure for creating trackers from scratch is embod-ied into our framework.

For generating Kalman trackers, measured cornerpoints (corner points extracted by the feature extrac-tion algorithm, as described in Section 3.2), are uti-lized. Since only one measured corner point is neededin order to create hypotheses, at time instant t themost confident (dominant) measured corner point ispicked out of the set of measured features. The con-fidence of a corner point is defined as the joint like-lihood of its parameters (x-y coordinates and orienta-tions of the adjacent line segments) at the mode of theirjoint distribution. Switcher states utilize the dominantcorner point in order to generate hypotheses (Kalmantrackers) depending on weather their correspondingmap corner point matches with the measured cornerpoint.

New Kalman trackers are created and initialized atthe position tc given by the solution to the problem offinding the state of the robot that produces a predic-tion about the measured corner point that best matchesthe map corner point. (x, y) coordinates of both cornerpoints, as well as orientations l1

f , l2f of their adjacent

pairs of line segments are utilized for this purpose. Thisyields a four-equations-with-three-unknowns problemsolved in a weighted least squares sense as:

tc = (∇CT �c∇C)−1∇CT �−1c c (29)

where c = (x, y, l1f , l2

f )T , the observed quadruple cor-responding to corner point at (x, y), �c the relatedcovariance matrix, and ∇C the Jacobian of the ob-servation function, i.e., the function mapping map

corner points to predicted corner points, given the state(c = C(h, s)).

After, new Kalman trackers are generated, validatedand updated, probabilities are assigned to switcherstates. This is done by utilizing Eq. (18).

For computing P( ft | skt ) and P(sk

t | sit−1, αt ),

needed in Eq. (18), the position of the tracker t kt of

switcher state skt is considered. This is based on the

assumption that the belief of the robot about its state attime instant t , given that the robot is within the discretepartition sk , is appropriately represented by the gaus-sian distribution of the one and only Kalman trackerwithin sk .

P( ft | skt ), that is the probability to observe ft , given

the discrete state skt , is computed for each switcher state

as the score returned by the dynamic programming al-gorithm at the point of its tracker t k

t , normalized bythe number of visible line segments at this point. Thisheuristic gives a fast and precise way of determiningthe similarity of what is actually seen from a robot’sstate with what should have been seen. Alternatively,in cases that the measured line segments are detectedto be of low confidence, at the cost of a slight com-putational overhead, row range data can be utilized inorder to compute P( ft | sk

t ). This is achieved by pro-jecting them on the map and determining the degree ofoverlapping.

On the other hand, P(skt | si

t−1, αt ), that is the prob-ability of switching from discrete state si to discretestate sk , is computed by a fast sampling algorithm asthe value of t i

t−1 displaced by odometry readings αt ,integrated over sk

t .Evidently the procedure of generating new Kalman

trackers need not be repeated with every iteration ofthe Kalman update procedure. Moreover, probabilityassignment to switcher states can also be performed ina lower frequency. Depending on the needs of the appli-cation, the complexity of the environment and the avail-able computing power, the algorithm can ultimately betuned to operate at any intermediate point in betweena pure Kalman tracker and an HMM-based localizer.

3.6. Map Generation and Robot’sState-Space Partitioning

For utilizing the proposed localization frameworkin real environments, appropriate maps of the envi-ronment should be available. For this purpose, anEM-based feature mapping algorithm (Baltzakis andTrahanias, 2002) has been employed. This algorithm


is capable of mapping line segments and corner points,while correctly closing loops during the mapping pro-cess, thus ensuring the topological consistency of theproduced maps.

Moreover, for the dynamic programming featurematching algorithm to work effectively and producecorrect matches, ordering information for the featuresis computed and stored during the mapping processas well. That is, mapped features are organized inchains of interconnected features (e.g., chains of linesegments and corner points connected to each other inalternate fashion, chains of line segments connectedthrough noisy, but opaque to the range finder, areas,etc.). These chains are used in order to correctly expandthe sequence of the visible map line segments given bythe ray-tracing algorithm at the predicted state of therobot and form a correct map line segment sequence tobe utilized by the feature matching algorithm.

Finally, switcher states are given by the corner-basedpartitioning strategy discussed in Section 3.3. More-over, to further expedite online computations, visibilityinformation for map corner points is computed off-lineand stored in a look-up table encoding all possible po-sitions of the robot to corresponding switcher states.

3.7. Comparison with Existing Approaches

Combinations of discrete and continuous models canalso be found in multiple hypothesis tracking ap-proaches. Multiple hypothesis tracking was firstly em-ployed in mobile robot localization in Cox and Leonard(1994), in an attempt to solve the data association prob-lem. In this work, odometry was assumed perfect, con-fining thus the direct applicability of the method toreal world problems. Moreover, probabilities assignedto trackers were only updated whenever new featureswere detected.

The multiple hypothesis tracking concept was devel-oped further in more recent work by Roumeliotis andBekey (2000) and Jensfelt and Kristensen (2001). Here,the assumption of perfect odometry was relaxed byutilizing probabilistic odometry models while proba-bilities assigned to trackers were updated continuouslyduring the operation of the algorithm, regardless of thefrequency that new features were detected.

In Roumeliotis and Bekey (2000), detected featuresgenerate hypotheses which are displaced according toodometry information using a single Kalman filter.Bayessian hypothesis testing is applied every time a

new feature is detected in order to combine the exist-ing displaced hypotheses with the ones imposed by thefeature. In this work, only features capable of creatingnew hypotheses where assumed and between the detec-tion of two such features, Kalman tracking is done onlythrough the project-forward Kalman recursion equa-tions (Eqs. (8) and (9)), that is, only according to odom-etry. Although this allows fast computations throughthe use of only a single update formula for all Kalmantrackers in between detection of features, it encapsu-lates the risk for the robot to get completely lost dueto accumulated odometry errors, if features are not de-tected for a long period of time.

In Jensfelt and Kristensen (2001), features are di-vided to creative and supportive ones. Creative fea-tures are used, as in Roumeliotis and Bekey (2000),in order to create new trackers. Supportive features (asuperset of features including creative ones) are usedin order to assist trackers not to get lost in betweenthe detection of two creative features. The concept ofdividing the features into creative and supportive onesbears similarities with the utilization of two types offeatures (line segments and corner points) in our frame-work. However, since corner points are extracted from(consecutive) line segments, no need for separate, linesegment and corner point, update equations exist inour case. Noting that two successive Kalman updates,through the use of two consecutive line segments, areequivalent with one update through the use of the cor-responding corner point, only line segments are usedfor the Kalman update phase in our framework. Thisupdating requires much less computational resourcescompared to the corner point-based updating, due tothe lower dimensions of the matrices involved in theKalman update equations.

Another innovation in Jensfelt and Kristensen (2001)was the introduction of the term “null hypothesis”, rep-resenting the probability that none of the hypotheses ata certain time instant is correct. This permits the cor-rect application of Bayessian estimation formulas byletting the closed-world assumption hold true (that is,the belief of the robot about its state sums up to oneinside the robot’s area of operation, or, in other words,the robot is always inside the map and this is correctlyrepresented in its belief).

A major drawback in multiple hypothesis trackingapproaches is that the number of Kalman trackers andthus the number of discrete probabilities to be han-dled can grow very large as new features get detected.By eliminating and merging hypotheses though proper


thresholding, their number can remain tractable but stillunfixed, thus imposing various implementation consid-erations.

Most of the differences between the approach de-scribed in this paper and the approaches presented inJensfelt and Kristensen (2001) and Roumeliotis andBekey (2000) stem from the fact that in the switch-ing state-space approach, the discrete states are definedspatially as partitions of the robot’s state space. Thispermits the discrete states to convey coarse localiza-tion information and also to keep a fixed number ofsuch states, thus enabling computational times to re-main fixed for a given map. The spatial definition ofthe discrete states also permits the easier and intuitivelymore plausible computation of the various probabilitiesinvolved in the model. Another advantage comparedto multiple hypothesis tracking approaches is that theclosed-world assumption inherently holds true at alltimes, as long as the robot stays within the map. Thatis, the probabilities of the discrete states always sumup to one, regardless of any errors in the belief of therobot about its state.

As a concrete example of the above, consider thecase that the robot travels along the borderline of twopartitions of the state space. Although probability mayflow from one state to the other and vice-versa and therobot might not be able to stably determine in whichpartition it may be, the total probability mass of thesetwo partitions would be constant along time. Moreover,when the robot exits the borderline and enters any ofthe two states, all the probability mass will follow it tothe correct partition. In a multiple hypothesis trackinganalogue, if two hypotheses are close to each other,probability would flow both ways and there is no wayto assure that the total probability of the two hypotheseswould be the same as if only one hypothesis existed.

The above argumentation consents encouragingly onthe applicability of the proposed framework to mo-bile robot localization. The powerful SSSM model fur-nishes it with the appropriate mathematical handles to

Table 1. Different error configurations for robot odometry and laser range finder.

Odometry Laser range finder

Conf√

kr (m/m)√

kt (deg/deg)√

kd (deg/m) Resol. (deg/meas.)√

kφ (deg)√

kρ0 (m) kρ1 (m/m) Quant. (m)

A 0.02 0.02 1 0.5 0.0 0.01 0.01 0.00

B 0.05 0.10 3 1.0 0.2 0.05 0.01 0.05

C 0.07 0.20 5 1.0 0.3 0.07 0.02 0.05

D 0.10 0.40 10 1.0 0.5 0.10 0.03 0.05

Figure 6. Artificial environments used for testing the performanceof the system. (a) Environment A. (b) Environment B.

cope with the intricacies of the localization task. More-over, it gives rise to efficient implementations that fa-cilitate real-time operation without hazarding perfor-mance.

4. Experimental Results

The probabilistic framework proposed in this paper hasbeen implemented and assessed on a robotic platformof our laboratory, namely an iRobot-B21r, equippedwith a SICK-PLS laser range finder. The range finderis capable of scanning 180 degrees of the environment,with an angle resolution of one measurement per degreeand a range measuring accuracy of 5 cm. All processingwas performed on an average PC workstation (1.2 GHzCPU, 512 MB RAM). Extensive tests have been per-formed with real and simulated data. In all cases theproposed framework was verified to operate very ac-curately. Sample results from these experiments arepresented in the sequel that illustrate its performance.

4.1. Artificial Data

For demonstrating the performance of the proposedframework against ground truth, a home-built simula-tor allowing various environment and robotic hardwareconfigurations was utilized. Figure 6 depicts two differ-ent environments utilized in the artificial experiments


presented in this paper. In addition to the two differ-ent environments, four different configurations, corre-sponding to increasing odometry and range finding er-ror levels, were used. All parameters involved in theseconfigurations are summarized in Table 1. Their phys-ical meaning is that described in Section 3.1. Of allfour configurations, configuration B corresponds to er-

Figure 7. Global localization in a simulated environment. 95% uncertainty ellipses are magnified by 5. (a) Initial estimate, (b, c, d) estimatesafter 1, 4, 8 iterations.

ror levels comparable to that of a real mobile robotoperating indoors.

Figure 7 illustrates the results of the algorithm, at-tempting to perform global localization in the simulatedenvironment of Fig. 6(a). Simulated odometry errors aswell as measuring errors have been set to those definedby error configuration B (Table 1).


Corner points, corresponding to switcher states, aremarked with colored circles, while their probabilitiesare shown next to them. Kalman trackers are shown asrobots colored and labelled according to the switcherstate they belong to. That is, tracker ti is the trackercorresponding to switcher state si . In cases where in-stantly more than one trackers are assigned to the sameswitcher state, trackers are marked with an extra index

Figure 8. Global localization for the “kidnapped robot” case; 95% uncertainty ellipses are magnified by 5. (a) Robot is “kidnapped” to a newstate, (b, c, d) estimates after 1, 3, 6 iterations.

number next to the state number (e.g., ti−0, ti−1, . . .).Ellipses centered at each tracker’s position depict 95%isoprobability contours, magnified, for displaying pur-poses, by a factor of 5.

Initially, the correct state of the robot is that oft4 (marked with the black “x”). The algorithm startsin Fig. 7(a) by generating five trackers initially po-sitioned according to the measured dominant corner


point (which is the corner point of s4 in this case).All six state probabilities are initially equal. It takeshowever only one iteration for the algorithm to esti-mate its correct state (s4, c.f. Fig. 7(b)) and completelyeliminate two of the five hypotheses by zeroing theircorresponding state probabilities. After four iterations,and while the robot is moving, the new state is s5 andthe robot is already quite confident about its location(0.6 state probability). After eight iterations is becomescompletely confident about its correct state, which isstill s5.

The effectiveness of the algorithm in performingglobal localization even when the robot is “kidnapped”is demonstrated in Fig. 8. The robot is artificially movedfrom the actual position that it had correctly estimatedin Fig. 7(d) to a new position. As can be verified, it takesonly a few iterations for the robot to reset the proba-bilities of all states and to regain confidence about itsnew position.

In order to obtain quantitative results with groundtruth available, a number of experiments were con-ducted with our simulator. Table 2 summarizes the re-sults obtained during eight simulated sessions of theproposed framework corresponding to both maps ofFig. 6 for all error configurations of Table 1. For ob-taining the results the simulated robot was set to fol-low random paths for approximately one hour for eachenvironment/configuration combination, while it wasperiodically kidnapped to random positions (approx-imately one kidnap per 10 seconds). Both the local-izer’s accuracy (position and orientation error) whenthe robot was not kidnapped, and the kidnapped robotrecovery time (the time before the localizer would man-age to re-localize the robot to its correct position aftera kidnap) were measured. In all cases, ground truthinformation was available by the simulator. As can be

Table 2. Localization accuracy and performance in the “kidnapped robot” case.

Environment A Environment B

Configuration A B C D A B C D

Iters 48013 48649 42365 47362 38307 50692 34301 36953

Sec/Iter(sec) 0.075 0.074 0.085 0.076 0.094 0.071 0.105 0.103

AVS (m/sec) 0.683 0.643 0.606 0.594 0.664 0.665 0.579 0.604

AVPE (m) 0.006 0.019 0.022 0.031 0.014 0.015 0.018 0.026

AVOE (deg) 0.211 0.062 0.809 1.009 0.444 0.445 0.479 0.653

No. of kidnaps 358 340 365 360 375 357 363 377

KRT (sec) 0.445 0.615 0.796 0.865 1.061 0.928 2.482 3.134

AVS: Average speed, AVPE: Average position error, AVOE: Average orientation error, KRT: Kidnapped robot recovery time.

observed from the results in Table 2, the localization ac-curacy is very high (very small position/orientation er-rors) and the kidnap recovery times are very favorable.

4.2. Real Data

Figure 9 demonstrates the operation of the algorithm ina complex indoor structure of approximately 2500 m2.An a-priori map has been used for this purpose. Al-though the structure favors ambiguities about the robotposition, the algorithm successfully determines the cor-rect robot position after only a few iterations (trackert1−0 in Fig. 9(a) and (b), tracker t1−1 in Fig. 9(c). By thetime it reaches its position in Fig. 9(d), it has alreadyresolved all ambiguities and is confident enough aboutits correct state (tracker t3 in Fig. 9(d)). The probabil-ities assigned to each state are shown on the bottomof each image and the Kalman tracker closest to thecorrect robot position is marked with a tick (�).

In Fig. 10 the performance of the algorithm whenfacing the “kidnapped robot” problem is once againdemonstrated. The algorithm successfully resets allprobabilities and correctly identifies the robot’s newposition within a couple of iterations.

In order to provide quantitative results on the localiz-ers accuracy when operating in a real environment, fourdistinct robot locations were manually marked on thefloor of our institute’s main entrance hall (Fig. 11). Therobot was manually driven to visit these marked loca-tions several times each, in random order and throughrandom paths. Each time the robot reached a mark,it was manually adjusted as accurately as possible tothe marked location and the localizer’s report about therobot position and orientation was stored. Table 3 givesthe number of times each of the marked locations were


Figure 9. Global localization in a real environment; 95% uncertainty ellipses are magnified by 5.(Continued on next page.)


Figure 9. (Continued).


Figure 10. Global localization for the “kidnapped robot” case in a real environment; 95% uncertainty ellipses are magnified by 5.(Continued on next page.)


Figure 10. (Continued).


Figure 11. Top view of the environment used for testing thelocalization accuracy of the proposed approach (approximately1200 m2).

Figure 12. Performance of the proposed framework when facing the “kidnapped robot” problem. (a) Top view of the environment used for theexperiments (approximately 1000 m2) and (b) Robot kidnaps arranged according to the distance travelled before the correct state of the robotwas recovered.

Table 3. Localization accuracy in a real environment.

Mark A Mark B Mark C Mark D

Number of visits 6 9 11 8

Std dev x (m) 0.032 0.008 0.012 0.010

y (m) 0.016 0.005 0.006 0.006

f (deg) 2.421 0.552 1.019 0.983

visited and the standard deviations (as a measure of ac-curacy) in the localizer’s reports obtained during eachof these visits. As can easily be observed, the localiza-tion measurements were very consistent, taking alsointo consideration that the accuracy of the manual pro-cedure used to position the robot within each markedlocation was limited.


Table 4. Performance for the kidnapped robot problem.

Number of kidnaps 29

Average number of iterations for recover 20.13793103

Average distance per iter (m) 0.086677985

Average distance for recover (m) 1.745515281

Figure 12 demonstrates the performance of the pro-posed framework for mobile robot localization whenfacing the “kidnapped robot” problem. During this ex-periment, the robot was manually driven within theenvironment of Fig. 12(a). Periodically, the localizerwas halt and the robot was kidnapped to a random newposition. Table 4 summarizes the average number ofiterations and distance that the robot needed to travelin order for the localizer to recover to the correct posi-tion. The diagram in Fig. 12(b) depicts the distributionof the kidnapped robot cases according to this distancemetric. As can be easily seen, in the vast majority ofthe kidnapped robot cases, the robot could recover itscorrect position in less than one meter.

It should be noted at this point, that all real experi-ments were conducted during normal working hours ofour institute. This means that the localizer was forced tocope with a considerable amount of unavoidable humantraffic, rendering, up to certain degree, a dynamicallychanging environment.

5. Conclusions and Future Work

In this paper we have proposed a probabilistic frame-work for modelling problems related to the localizationtask for mobile robots. The proposed approach, basedon an underlying Switching State-Space model, gener-alizes two of the most successful probabilistic modelfamilies used for this purpose, namely the Kalman filterlinear models and HMMs. Sequences of line segmentsand corner points, extracted from laser range data, incombination with a fast dynamic programming algo-rithm, able to produce and score matches among them,have been used in order to facilitate the implementationof the proposed framework.

Experimental results confirm that the resulting hy-brid model combines the advantages of both modelsmentioned above, that is the computational efficiency,scalability, and accuracy of Kalman filters with the ro-bustness and global localization capabilities of discreteapproaches.

Evidently, the proposed framework possesses char-acteristics that constitute it suitable for mobile robotlocalization. The SSSM model that constitutes its heartendows it with accurate performance and capabilitiesto handle noisy data and other unpredictable situations(e.g., robot kidnapping). The above are achieved withlimited execution times, facilitating real-time operationwith minimal computational resources.

It is in our intension to further investigate in theproposed framework the utilization of other sources ofsensory data, such as vision, in order to exploit sensorinformation not included in 2D range scans. The map-ping algorithm should also be appropriately enhancedin order to accommodate such sensory information inthe generated workspace representation.

Acknowledgment

This work was partially supported by EuropeanCommission Contract No IST-2000-29456 (WebFAIRhttp://www.ics.forth.gr/webfair) under the ISTProgramme.

References

Arras, K.O., Tomatis, N., and Siegwart, R. 2000. Multisensor on-the-fly localization using laser and vision. In Proc. IEEE/RSJ Intler-national Conference Intelligent Robotics and Systems.

Baltzakis, H. and Trahanias, P. 2002. An iterative approach for build-ing feature maps in cyclic environments, Accepted for presenta-tion, IROS 2002. In IEEE/RSJ Intl. Conf. on Intelligent Robotsand Systems.

Bar-Shalom, Y. and Li, X.R. 1993. Estimation and Tracking: Princi-ples, Techniques, and Software, Artech House Inc., Boston, MA,USA.

Benson, G. 1997. Sequence alignment with tandem duplication. InAnnual International Conference on Computational Molecular Bi-ology (Santa Fe, NM), ACM Press, pp. 27–36.

Burgard, W., Derr, A., Fox, D., and Cremers, A.B. 1998. Integratingglobal position estimation and position tracking for mobile robots:The Dynamic Markov Localization approach. In Proc. IEEE/RSJIntlernational Conference Intelligent Robotics and Systems.

Cassandra, A.R., Kaelbling, L.P., and Kurien, J.A. 1996. Acting un-der uncertainty: Discrete bayesian models for mobile robot nav-igation. In Proc. IEEE/RSJ Intlernational Conference IntelligentRobotics and Systems.

Castellanos, J.A., Martınez, J.M., Neira, J., and Tardos, J.D. 1998.Simultaneous map building and localization for mobile robots: Amultisensor fusion approach. In Proc. International Conferenceon Robotics and Automation, pp. 1244–1249.

Cox, I.J. and Leonard, J.J. 1994. Modeling a dynamic environ-ment using a multiple hypothesis approach. Artificial Inteligence,66(1):311–344.


Dellaert, F., Fox, D., Burgard, W., and Thrun, F. 1999. Monte Carlolocalization for mobile robots. In Proc. International Conferenceon Robotics and Automation.

Duckett, T. and Nehmzow, U. 2001. Mobile robot self-localisationusing occupancy histograms and a mixture of gaussian loca-tion hypotheses. Robotics and Autonomous Systems Journal,34(2/3):119–130.

Duda, R.O. and Hart, P.E. 1973. Pattern Classification and SceneAnalysis, Wiley-Interscience, New York.

Fox, D., Burgard, W., and Thrun, S. 1999. Markov localization formobile robots in dynamic environments. Journal of Artificial In-telligence Research, 11:391–427.

Ghahramani, Z. and Hinton, G. 1996. Switching state-space models.Technical Report CRG-TR-96-3, Department of Computer Sci-ence, University of Toronto.

Gutmann, J.-S., Burgard, W., Fox, D., and Konolige, K. 1998.An experimental comparison of localization methods. In Proc.IEEE/RSJ Intlernational Conference Intelligent Robotics and Sys-tems.

Gutmann, J.-S., Weigel, T., and Nebel, B. 2001. A fast, accurate,and robust self-localization in polygonal environments using laser-range-finders. Advanced Robotics, 14(8):651–668.

Jensfelt, P. and Kristensen, S. 2001. Active global localisation fora mobile robot using multiple hypothesis tracking. IEEE Trans.Robotics and Autom, 17(5):748–760.

Levitt, T. and Lawton, D. 1990. Qualitative navigation for mobilerobots. Artificial Inteligence, 44:305–360.

Lu, F. and Milios, E.E. 1994. Robot pose estimation in unknownenvironments by matching 2d rangescans. CVPR, 94:935–938.

Maybeck, P. 1979. Stochastic Models, Estimation ond Control,Academic, New York, Vol. 1.

Michael, T.S. and Quint, T. 1994. Sphere of influence graphs ingeneral metric spaces. Mathematical and Computer Modelling,29:45–53.

Minka, T.P. 1999. From hidden markov models to linear dynamicalsystems. Technical Report TR-531, Vision and Modeling Group,MIT Media Lab.

Murphy, K. 1998. Learning switching kalman filter models. Techni-cal Report 98-10, Compaq Cambridge Research Lab.

Nourbakhsh, I., Powers, R., and Birchfield, S. 1995. DERVISH—Anoffice-navigating robot. AI Magazine, 16(2):53–60.

Roumeliotis, S.I. and Bekey, G.A. 2000. Bayesian estimation andkalman filtering: A unified framework for mobile robot localiza-tion. In Proc. International Conference on Robotics and Automa-tion, San Francisco, California, pp. 2985–2992.

Scott, A., Parker, L.E., and Touzet, C. 2000. Quantitative and qual-itative comparison of three laser-range mapping algorithms usingtwo types of laser scanner data. In IEEE Systems, Man, and Cy-bernetics Conference.

Simmons, R. and Koenig, S. 1995. Probabilistic robot navigation inpartially observable environments. In Proc. of the InternationalJoint Conference on Artificial Intelligence, pp. 1080–1087.

Thiebaux, S. and Lamb, P. 2000. Combining kalman filtering andmarkov localization in network-like environments. In Pacific RimInternational Conference on Artificial Intelligence, pp. 756–766.

Thrun, S., Beetz, M., Bennewitz, M., Burgard, W., Creemers, A.B.,Dellaert, F., Fox, D., Hahnel, D., Rosenberg, C., Roy, N., Schulte,

J., and Schulz, D. 2000. Probabilistic algorithms and the interac-tive museum tour-guide robot minerva. International Journal ofRobotics Research, 19(11):972–999.

Thrun, S., Fox, D., Burgard, W., and Dellaert, F. 2001. Robustmonte carlo localization for mobile robots. Artificial Inteligence,128(1/2):99–141.

Haris Baltzakis was Born in Heraklion, Crete, Greece in 1973. He re-ceived the B.Eng. degree in electrical engineering from DemoctritusUniversity of Thrace, Xanthi, Greece, in 1997, and the M.Sc. degreein computer science from University of Crete, Heraklion, Greece in1999. He is currently a Ph.D. student at the Department of ComputerScience, University of Crete, Greece. His research interests includecomputer vision and autonomous robot navigation.

Panos Trahanias is a Professor with the Department of ComputerScience, University of Crete, Greece and the Institute of Com-puter Science, Foundation for Research and Technology—Hellas(FORTH), Greece. He received his Ph.D. in Computer Sciencefrom the National Technical University of Athens, Greece, in 1988.He has been a Research Associate at the Institute of Informaticsand Telecommunications, National Centre for Scientific Research“Demokritos”, Athens, Greece. From 1991 to 1993 he was with theDepartment of Electrical and Computer Engineering, University ofToronto, Canada, as a Research Associate. He has participated inmany RTD programs in image analysis at the University of Torontoand has been a consultant to SPAR Aerospace Ltd., Toronto. Since1993 he is with the University of Crete and FORTH; currently, he isthe supervisor of the Computational Vision & Robotics Laboratoryat FORTH where he is engaged in research and RTD programs invision-based robot navigation, telematics applications and virtual &augmented environments. Prof. Trahanias has published over 75 pa-pers in the above areas and has co-ordinated and participated in manyfunded projects.

A Hybrid Framework for Mobile Robot Localization ... · A Hybrid Framework for Mobile Robot...

Documents

Transcript of A Hybrid Framework for Mobile Robot Localization ... · A Hybrid Framework for Mobile Robot...