probabilistic self-localization and mapping - an asynchronous multirate approach

IEEE Robotics & Automation MagazineJUNE 2008 1070-9932/08/$25.002008 IEEE 77

AnAsynchronousMultirate Approach

BY LEOPOLDO ARMESTO, GIANLUCA IPPOLITI,

SAURO LONGHI, AND JOSEP TORNERO

One of the most common techniques for state esti-mation of nonlinear discrete-time dynamic sys-tems is the extended Kalman filter (EKF) [1] or,more recently, unscented Kalman filter (UKF)[2]. Kalman filter gives a robust, optimal, and

recursive state estimation to fuse redundant sensor informa-tion. However, both approaches assume that the probabilitydistribution function (PDF) is Gaussian, which is not true formost signals found in practice. Other recent filtering methodsare particles filters (PFs) [3], [4] in which the main advantage isthat the PDF can be accurately approximated with a largenumber of particles. The most common approach of PF is the

sampling importance resampling [5], [6] that provides a weight(importance factor) to each particle. The resampling stepselects those particles with higher weights and removes thoseparticles with lower weights. Another well-known approach isthe Rao-Blackwellized PF [7], which uses a PF for some varia-bles of the state and a Kalman filter for other variables.

In robotics, these estimation methods are commonly usedto determine the robot pose with respect to its environment(map); see, for instance, [1], [8][12]. The problem of deter-mining the robot pose is commonly known as self-localization.The map can be assumed to be known or unknown. In theformer, the EKF, UKF, and also the Monte Carlo localization(MCL) methods [11], [12] have been widely used. In the latter,the SLAM (simultaneous localization and map building)Digital Object Identifier 10.1109/M-RA.2007.907355

ARTVILLE

problem arises, which consists of estimating the robot pose andmap simultaneously. Classical approaches to solve this problemare based on EKF by joining the robot state and the map [1],[13], [14]. The problem of this approach is that the dimensionof the covariance grows to (N 3) 3 (N 3), with N beingthe number of map features.

Recently, a solution of the SLAM problem with Rao-Blackwellized PFs has been given, which is known as Fast-SLAM (Factored Solution to SLAM) [15]. The key ideabehind the FastSLAM is that the problem can be divided intoN 1 separated problems: one for estimating the robot posewith a PF and, therefore, with low state dimension, andN lowdimension separate problems for updating the map features.

One of the main contributions of this article is related to themultirate asynchronous filtering approach for the SLAMproblembased on PFs. Previous multirate filter contributions are mainlyfor linear systems. In [16] and [17], a Kalman filter is applied forlinear quadratic regulator (LQG) control, while in [18] a Kalmanfilter is developed using lifting techniques [19]. The problem ofmultirate filtering arises from the fact that sensors and actuators ofrobots are sampled at different sampling rates due to technologicallimitations, communication channels, processing time, etc.

In this article, significant improvements for robot pose esti-mation are obtained when introducing multirate techniques toFastSLAM. In particular, it is shown that multirate fusion aims toprovidemore accurate results in loop-closing problems in SLAM(localization andmap building problemswith closed paths).

Additionally, in this article a pose estimation algorithmbased on least squares (LS) fitting of line features is proposed.Since the complexity of LS fitting is linear to the number offeatures, this implies a low computational cost than other tech-niques. Therefore, methods based on PFs such as MCL andFastSLAM that require a large number of particles may benefitfrom this fact. In particular, this provides an accurate approxi-mation of the posterior PDF for FastSLAM 2.0 [20].

Moreover, we developed an asynchronous filtering methodto deal with measurements of sensors at different samplingrates. One of the key ideas of the method is to use an asynchro-nous hold to extrapolate inputs of the system. Another keypoint is the asynchronous execution of prediction and updatesteps in the filtering method, which aims to maintain a goodsystem performance. The prediction step is executed within atleast a prespecified sampling period (generally at a fast samplingrate to reduce discretization errors), and the update step is exe-cuted only when measurements are asynchronously received.

Experimental tests have been performed on a poweredwheelchair equipped with a fiber-optic gyroscope (FOG), alaser scanner, and two optical encoders connected to the axesof the driving wheels. These applications are of interest in theemerging area of assistance technologies where poweredwheelchairs can be used to strengthen the residual abilities ofusers with motor disabilities [21][23].

The proposed approach results in a computationally effi-cient solution to the localization problem and may really repre-sent a basic step towards the proper design of a navigationsystem aimed at enhancing the efficiency and the security ofcommercial powered wheelchairs.

Object DetectionIn this section, a method for object detection is described,based on laser ranger measurements. This method is specifi-cally recommended for indoor applications, since it assumescertain geometric constraints about the environment.

Firstly, we define an object as a set of points {xi,yi}, repre-senting a given landmark of the environment (walls, shelves,columns, corners, persons, etc.). In particular, for localizationpurposes, the map contains lines representing walls, shelves,and doors, etc. Moving objects such as persons are notincluded in the map, since it is assumed to be static.

Segmentation and Single Line FittingLaser points are segmented using an adaptive breakpointdetector [24] and the well-known split and merge algorithm[25]. Initially, a standard LS fitting is used for each segmentedline to estimate distance (q) and angle (j) parameters, wherethe well-known solution is

q^ x cos u^ y sin u^, u^ 12arctan

2r2xyr2y r2x

!,

where x, y, r2x, r2y , and r

2xy are means and covariances of the

dataset.

Multiple Line FittingSince our object detection algorithm is designed for structuredindoor environments, it is reasonable to make some assump-tions about the properties of the environment. In particular,walls are usually perpendicular or parallel to each other. Withthat in mind, we present a method for fitting lines by consider-ing geometric constraints of the environment.

The goal of this method is the minimization of the sum ofsquared distances for all lines (global multiple line fitting). Thisbasic idea allows us to improve typically noisy estimations oflines with few points. In general, the method may not onlyimprove parameter estimation but also data association andmap building. In addition to this, corners can easily be esti-mated as intersection points of contiguous lines.

By performing a previous single estimation (standard LSfitting), we can select those lines whose angle difference isclosed to jk j0 {0, 90, 180, 270} with respect to areference line. The idea is to force the estimation to generatejk j0 {0, 90, 180, 270}. The reference line shouldbe taken to be the most reliable one; i.e., the largest line or theline with the greatest number of points. Obviously, themethod is not limited to rectangular angles and any anglemight be forced. If a line does not satisfy the constrained con-dition, it is not forced and the single line fitting estimation isused instead.

The performance index for minimizing is as follows:

EML 12XNlk1

XNl;ki1

(qk xi,k cosuk yi,k sinuk)2

constrained to jk j0 jc,k, where jc,k is the constrainedangle with respect to the reference line (for convenience the

IEEE Robotics & Automation Magazine78 JUNE 2008

first line). Nl is the number of lines and Nl,k the number ofpoints for each segmented line.

The distances of lines can be obtained as follows:

@EML@qk

0) q^k xk cos u^k yk sin u^k.

Replacing these variables and angle constraints into theoriginal performance index and performing some standardalgebraic manipulations, we get to the optimal estimate of j0:

@EML@u0

~B sin (2u^0) ~A cos (2u^0) 0) tan (2u^0) ~A~B,

with

~A XNl,kk0

2r2xy,k cos (2uc,k) (r2y,k r2x,k) sin (2uc,k) and

~B XNl,kk0

2r2xy,k sin (2uc,k) (r2y,k r2x,k) cos (2uc,k).

It is interesting to remark that data statistics, that is, x, y, r2x,r2xy, and r

2y , were initially computed for single fitting and,

therefore, they are reused for saving computational resources.

Algorithm 1 describes the implementation of the proposedmethod. The input of the algorithm is raw polar data of thelaser ranger (rt, at), while the output of the algorithm is thevector of detected lines zt.

A comparison between the single and multiple line fittingmethods is shown in Figure 1. The multiple line fittingmethod gives a better estimation in the sense that lines are cor-rectly estimated and the corners are better defined. Therefore,this will improve global estimation and map building, withoutadditional efforts, since the cost of this multiple line fitting isalways of the orderO(Nl) as in the single-line fitting.

LS Pose EstimationIn this section, we present a simple but effective method ofestimating the pose of a robot based on line features. Themethod assumes that a data preprocess has been previouslydone so that detected line features of the environment havebeen obtained zit hqid,uidi. It also assumes that the map,containing line features m jt hq jm,u jmi, is known or it isbeing estimated.

The main issue of this method is that it estimates the robotpose in the LS sense, and it can be applied for global localizationunder the assumption of known data association. Since this is nota realistic approach in most real applications, the method requiresa previous association between the detected and map features,which is given by the hypothesis H, a list that relates eachdetected i feature with its corresponding feature on the mapj H(i). If a feature is not associated, thenwe setH(i) 0.

Based on the well-known line model,

qid qH(i)m x cosuH(i)m y sinuH(i)m q,i and (1)uid uH(i)m h u,i, (2)

where p [x y]T is the Cartesian robot position and h the ori-entation. eq,i and ej,i are distance and angle errors between the

Algorithm 1: Multiple Line Fitting

1 MultipleLineFitting(rt, at)2 remove those measurements from rt and at higher than

a maximum distance;3 compute break points based on discontinuities using

(rt, at);4 compute Cartesian points (xt, yt) from (rt, at);5 segment Cartesian points with Split & Merge algorithm;6 estimate lines zt with LS fitting, one for each seg-

mented set of points (single fitting);7 retrieve u^0 from the largest line of zt;8 ~A 0; ~B 0;9 for i 1 to length(zt) do10 retrieve huî,r2x,i,r2xy,i,r2y;ii from zit;11 if uî u^0 0 then u^c,i u^0;12 else if uî u^0 p2 then u^c,i u^0 p2;13 else if uî u^0 p then u^c,i u^0 p;14 else if uî u^0 3p2 then u^c,i u^0 3p2 ;15 else u^c,i uî;16 ~A ~A 2r2xy,i cos (2u^c,i) (r2y,i r2x,i) sin (2u^c,i);17 ~B ~B (r2y,i r2x,i) cos (2u^c,i) 2r2xy,i sin (2u^c,i);18 end19 u^c,0 12 arctan ( ~A~B );20 for i 1 to length(zt) do21 retrieve hqî, xi, yii from zit;22 uî u^c,i u^c,0;23 qî x cos uî y sin uî;24 replace hqî uîi of zit;25 end26 return Zt;

1 2 3

(a) (b)

4 52

1

0

1

2

3

4

5

6

X[m]

Y[m]

IncorrectAssociation

IncorrectCorner

1 2 3 4 52

1

0

1

2

3

4

5

6

X[m]

Y[m]

CorrectAssociation

CorrectCorner

Figure 1. Comparative between (a) single and (b)multiple line fit.

IEEE Robotics & Automation MagazineJUNE 2008 79

detected lines and predicted ones. Error covariances areassumed to be Rq r2qI and Ru r2uI, respectively.

The pose estimation problem can be easily separatedbecause (1) is affected only by the Cartesian pose, while (2) isaffected only by the orientation. In addition to this, we caneasily see that both equations are linear with respect to the posevariables, and they can be rewritten in matrix form as

qe X p q,ue 1 . . . 1Th u,

where

qe q1d qH(1)m , . . . , qNld qH(Nl)mh iT

,

X cosu

H(1)m sinu

H(1)m

..

. ...

cosuH(Nl)m sinuH(Nl)m

2664

3775,

ue u1d uH(1)m , . . . , uNld uH(Nl)mh iT

, and

q q,1, . . . , q,Nl T

, u u,1, . . . , u,Nl T

:

Therefore, the LS fitting provides the following estimationsfor the Cartesian pose and orientation:

p^ x^y^

(XTX)1 XTqe, h^ ue,

and the covariance estimation error is as follows:

Rxy r2q(XTX)1, Rh r2uNl

:

Matrix XTX is singular when all lines considered have thesame orientation. However, because the method estimates theCartesian positions separately from the orientation, and insuch situations, a valid estimation of the orientation can stillbe provided.

Algorithm 2 describes the implementation of the meth-od, where the following considerations must be taken intoaccount:

XTX A CC B

, XTqe DE

,

with

A XNli1

cos2 uH(i)m , B XNli1

sin2 uH(i)m ,

C XNli1

cosuH(i)m sinuH(i)m ,

D XNli1

(qid qHi)m cosuH(i)m , and

E XNli1

(qid qHi)m sinuH(i)m .

A similar approach is followed by Araujo and Aldon in[26], where the main difference is given in the definition ofthe performance index. In [26], the proposed index is basedon the minimization of Cartesian projections betweendetected lines and map lines with respect to robot pose. Thisrepresentation introduces the advantage of including pointsand lines under the same performance index, but the analyticsolution to this problem leads to solve a fourth order polyno-mial equation derived from a trigonometric rotation equa-tion. In addition, it presents four possible solutions (real and/or complex), and therefore, a study on the Jacobian is requiredto discard local maxima.

To analyze the performance of the proposedmethod, a simu-lation, using a mobile robot moving through a regular environ-ment with walls and corridors, has been considered. Obviously,the line detection method proposed in the previous section hasbeen taken into account for laser-ranger measurements, where anoise has been added to them to simulate more realistic data.Figure 2 shows the pose estimation error between ground truthand estimated pose with LS and Araujos methods. Crosses

Algorithm 2: Line-Based Pose Estimation

1 PoseEst(zt, mt, Ht, xt1)2 A B C D E je Nl 0;3 for i 0 to length(Ht) do4 if Ht(i)[0 then5 retrieve hq jm,u jmi from mHt(i)t ;6 retrieve hqid,uidi from zit;7 A A cos2 u jm;8 B B sin2 u jm;9 C C sinu jm cosu jm;10 D D (qid q jm) cosu jm;11 E E (qid q jm) sinu jm;12 ue ue uid u jm;13 Nl Nl 1;14 end15 end16 retrieve hxt, yt, hti from xt1;17 if j > 0 and A B C2 6 0 then18 xt CEBDABC2 ;19 yt CDAEABC2 ;20 Rxy r2q

A CC B

1;

21 else xt xt1; yt yt1; Rxy 0;22 if j > 0 then ht ueNl; Rh

r2uNl;

23 else ht ht1; Rh 0;24 xt [xt yt ht]T;25 Rxt Rxy 00T Rh

;

26 return xt and Rxt ;


depicted on Figure 2(a) and (b) represent iterations whereXTXbecomes singular and, therefore, they should not be considered.It can be appreciated that Araujos method presents more thanthree times higher cumulative computational time (Tc) than thatof LS pose estimation. In fact, the mean computational times ofLS and Araujos methods are 4.92 104 and 18 104 ms,respectively. This aspect is particularly relevant, since each parti-cle of the FastSLAM algorithm will correct its pose using thismethod (see the next section for more details).

Moreover, consider the mean estimation error (MSE) ofCartesian positions and orientations:

MSExy 1hXhk0

(xk x^k)2 (yk y^k)2, and

MSEh 1hXhk0

(hk h^k)2,

where h is the number of iterations of the simulation. Theperformance in terms of MSEx and MSEh of LS and Araujosmethods is shown in Table 1, where Nl is the minimum num-ber of lines required to produce a valid estimation. Araujosmethod gives more accurate results than those of LS in thecases where more than two lines have been detected (Nl 3 orNl 4). However, it is clear that the accuracy of both themethods is good enough for many practical situations.

Asynchronous Multirate TechniquesApplied to SLAM Problem

Probabilistic Robot Localization and Map BuildingThe key idea of PFs is to represent the posterior distribution ofa signal xt by a set of random samples (particles) drawn fromthis posterior. Such a set of samples is an approximation of thereal distribution and, in general, PFs can represent muchbroader spaces of distributions, rather than Gaussian distribu-tions such as EKF or UKF.

We denote the set of particles describing the posteriordistribution as

X t fx1t ,x2t , . . . , xM t g,

where M is the number of particles and each particle xkt isdrawn from the posterior distribution

xkt p(xtjz1:t,u1:t), (3)

where z1:t represents the set of whole measurements and u1:t theset of whole inputs. The approximation is valid if the number ofsamples is large enough, generally M 100. Similar to mostpopular filters, the PF uses a recursive estimation for approxi-mating the posterior distribution. Therefore, (3) simplifies to

xkt p(xtjxt1,zt,ut),

0 10 20 30 400.05

0

0.05

X [m

.]

0 10 20 30 400.05

0

0.05

Y [m

.]

0 10 20 30 400.05

0

0.05

[rd

.]

0 10 20 30 400

0.5

1

1.5

T c

Time (s)(a) (b)

0 10 20 30 400.05

0

0.05

X [m

.]

0 10 20 30 400.05

0

0.05

Y [m

.]

0 10 20 30 400.05

0

0.05

[rd

.]

0 10 20 30 400

0.5

1

1.5

T c

Time (s)

Figure 2. Pose estimation error and computational time with (a) LS pose estimation and (b) Araujos methods.


where only inputs and measurements at the present timeinstant are used. The main problem is that the posterior distri-bution is not known, and therefore a proposal distributionhas to be used based on the importance factor. For each parti-cle, a weight obtained from wkt p(ztjxkt ) is computed,where p(ztjxt) is the PDF of the sensor model. The set ofweighted particles represents (an approximation) of the poste-rior. This method is known as sequential importance resam-pling, where a resampling step and normalization must bedone to avoid the algorithm degeneracy [6], [27].

FastSLAM is the PF approach to SLAM. Unfortunately PFin SLAM is also affected by the curse of dimensionality, with expo-nential growth (M 3 N). For that reason, Rao-BlackwellizedPF [28] is applied, where the pose of the robot is estimated witha standard PF and the map is estimated by an EKF. In Fast-SLAM, each particle contains an estimated robot pose xkt and aset of Kalman filters with lkt and covariance Rkt , one for eachfeature in the map. Thus, the problem is decomposed in theN 1 problem: 1 for estimating the robot pose and N for esti-mating the features in the map. A key advantage of this solutionis that maps between particles are not correlated, and thereforeerrors in the map are filtered through the resampling process,where only the best particles (with best maps) will survive.

The FastSLAM described so far is known as FastSLAM 1.0[15], while in FastSLAM 2.0 [20] the proposal distributiontakes the measurements zt into account when sampling thepose xkt based on an EKF approach. This modification followsa similar approach to that of [29] and [30] for PF.

In mobile robots, inputs and outputs have different sam-pling rates. For instance, odometry sensors, such as encoders,gyros, and accelerometers, are typically sampled faster than

external sensors such as lasers [31], sonars, or vision systems[32], [33]. This is a problem that arises form the inherenttechnological limitations of sensors, communication channels,and processing cost, etc., which can not be neglected. A typicalsolution to overcome this problem is to reduce the overall sam-pling period to the slowest one. However, it is well known thatthis approach may decrease the overall system performance. Inaddition to this, discretization errors will also increase withhigher sampling periods. Therefore, faster dynamics requirefast sampling rates.

In this article, we describe an asynchronous filteringmethod to deal with measurements of sensors at differentsampling rates. One of the key ideas of the method is to usean asynchronous hold to extrapolate the inputs of the system.Another key point is the asynchronous execution of predic-tion and update steps in the filtering method, which main-tains a good system performance. The prediction step isexecuted within at least a prespecified sampling period (gen-erally at a fast sampling rate to reduce discretization errors),and the update step is executed only when measurements areasynchronously received.

Asynchronous HoldsA multirate hold is a hybrid device for generating, from asequence of inputs sampled at a slow sampling rate, a continu-ous signal that may be discretized at a high sampling rate. Themathematical background of multirate high-order holds (MR-HOH) and samplers is described in [34][36]. Initially, multi-rate holds were introduced as a generalizations of classical holdssuch as zero-, first-, and second-order holds (ZOH, FOH, andSOH). Later on, in [37], a wide variety of holds were obtainedfrom general primitive functions. The idea behind generalprimitive functions is to generate an extrapolated continuoussignal based on input samples futj ,utj1 , . . . ,utjng uniformlydistributed at a low frequency (dual-rate sampling) [37].

In this article, we extend the formulation for the asynchro-nous case, where input samplings are not uniformly distributed:

u^t Xnl0

f l(t,utjl ,tjl), (4)

where utj denotes an input that has been sampled at time instanttj being tjn . . . tj t. The primitive function f l(t) gener-ates the continuous signal u^t, which can be computed at anydesired time instant. Thus, the asynchronous hold is in chargeof generating a continuized signal regardless of when theinputs were sampled. Asynchronous holds may be used in twodifferent situations: for estimating signals in between samples aswell as for overcoming the data-missing problem.

Algorithm 3 implements the asynchronous hold of order n,based on a general primitive function. To implement the asyn-chronous hold, a shift register U j futj , . . . ,utjng is requiredto log the signal. Table 2 summarizes some primitive functionsthat can be used in asynchronous holds. In spline holds, a set ofcoefficients cj;l are obtained when a spline curve is adjusted tothe previous inputs. In Taylor holds, input derivatives areobtained using the backward approximation.

Table 1. MSE performance.

MSE Method Nl 2 Nl 3 Nl 4MSEx [m

2] LS 1.8103 2103 2.2103

Araujo 27.85 7.42105 6.26105

MSEh [rd2] LS 1.75105 1.93105 1.53105

Araujo 0.32 7.7106 4.74106

Algorithm 3: Asynchronous Hold

1 Asynchronous-Hold(ut,U j, t, j)2 If ut is updated then3 shift out utjn and shift in ut from U j;4 u^t ut;5 j t;6 else7 u^t 0;8 for l 0 to n do9 retrieve utjl from U j;10 u^t u^t f l(t,utjl , tjl);11 end12 end13 return u^t,U j and j;


An experiment has been performed on a mobile robotmoving in a real environment, where signal profiles of linearand angular velocities are shown in Figure 3. This figure alsoshows the MSE of the extrapolated signals when using a, MR-FOH (first order Lagrange hold), MR-SOBH (second-orderBezier hold), MR-SOTH (second-order Taylor hold) andMR-SOSH (second-order spline hold)(first order cases of spline, Bezier, and Tay-lor functions are the same as that of theFOH.). The sampling period of signals isTs 100 ms, i is the number of iterationsthat signals are extrapolated, and n is theorder of the hold. Obviously, stability can-not be guaranteed for any arbitrary extrap-olation time interval, since MSE increasesexponentially with i. However, it can beseen that for all cases with i 5 the resultsof MR-FOH, MR-SOBH, MR-SOTH,and MR-SOSH significantly improve theresults of the naiveMR-ZOH solution.

Multirate Asynchronous FastSLAMThe proposed multirate asynchronousFastSLAM (MR-FastSLAM) has been

implemented in the two different versions FastSLAM 1.0 andFastSLAM 2.0, described in Algorithm 4, where M is thenumber of particles. In FastSLAM 1.0, pseudo-code lines 11and 12 are not used. The pose estimation method used in line2 is the one described in the previous section. This modifiesthe original contribution of FastSLAM [20] in the sense that

0 100 200 300 400 5000.3

0.2

0.1

0

0.1

0.2

0.3

Time (s)

2 3 4 5 60

0.5

1

1.5

2 105

Iterations (i)

MSE

v

MRZOHMRFOH

MR2nd Bezier

MR2nd Taylor

MR2nd Spline

2 3 4 5 60

0.5

1

1.5

2 105

Iterations (i)

MSE

MRZOHMRFOH

MR2nd Bezier

MR2nd Taylor

MR2nd Spline

Linear Velocity

Angular Velocity

Figure 3. Input profile and MSE for different holds.

Table 2. Primitive functions.

Type Function

Interpolation

(Lagrange)u^t

Xnl0

Ynq0q6l

t tjqtjl tjq

utjl

264

375

Interpolation

(Spline) u^t Xnl0

cj,l(t tjl)l with fcjn1,0, . . . , cj,0, . . . , cjn1,m, . . . , cj,mg

spline(Utj )Approximation

(Bezier) u^t Xnl0

n!

l!(n l)! 1t tj

tj tjn

nl t tjtj tjn

lutjl

" #

Approximation

(Taylor) u^t Xnl0

t tjll!

ultjwith u

ltjul1tj

ul1tj1tj tj1


an LS approach is used instead of an EKF approach. The mainadvantage of this new approach is the reduced computationalcost of LS.

The steps are similar to the conventional FastSLAM algo-rithms, where differences lie in the inclusion of asynchronousholds and asynchronous prediction and update steps. In thatsense, asynchronism is due to a time-varying execution timeperiod t.

u If the time Ts has elapsed since the last execution(t 1) period of the algorithm and no measurementhas been received yet, then only the prediction step isexecuted with t (t 1) Ts to maintain a good sys-tem performance with low discretization error.

u Whenever a measurement is received, the predictionand update steps are executed with t (t 1) tz,

with tz being the elapsed time between the last execu-tion and the measurement sampling. Note that theprediction step must necessarily be performed tocoherently fuse measurements and states at the sametime instant.

Figure 4 shows an example of a tasks chronogram for theproposed algorithm. In this case, four different tasks have beenconsidered: encoder, laser, prediction, and update, each ofthem running at different frequencies. The encoder task is exe-cuted approximately at Tinc 100 ms, each time an encodermeasurement is received, while Laser Task is executed atTlas 400 ms. The laser task has been subdivided into moresubtasks to indicate when each of the detected lines have beenprocessed. In Figure 4, several lines are typically processed for aspecific laser scan. Prediction and update tasks are executedaccording to the conditions mentioned previously. In thissense, the update step is not performed until the processing ofthe laser scan has been completed. In Figure 4, it can also beseen that the prediction task is regularly executed with asampling period of Ts 100 ms. However, whenever a lasermeasurement is received, the prediction task is executed again,immediately followed by the update task. In addition to this, itis interesting to observe the effect of the asynchronous hold.Despite the asynchronous encoder samplings, the predictionstep, which uses these measurements, is executed at its ownsampling frequency (also asynchronous).

Experimental ResultsExperimental tests have been performed in an indoor environ-ment, on the Explorer powered wheelchair developed by thecompany TGR (Italy). This vehicle has two driving wheels. Theodometric system consists of two incremental encoders con-nected to independent passive wheels aligned with the axes ofthe driving wheels. An FOGHITACHI module, HOFG-1, hasalso been used to accurately measure the angular velocity. Thelaser scanner measurements have been acquired by the SICKLMS200 installed on the vehicle. A resolution of 0.5 and aspectrum of 180 have been chosen to have a number ofmeasurements that could simultaneously guarantee good map

0 0.2 0.4 0.6 0.8

Enco

der

0 0.2 0.4 0.6 0.8

Lase

r

0 0.2 0.4 0.6 0.8Pre

dict

ion

0 0.2 0.4 0.6 0.8

Upda

te

Time (s)

Figure 4. Task chronogram example.

Algorithm 4: Multirate FastSLAM 2.0

1 MR-FastSLAM 2.0(X t1,ut, zt,U j, t, j)2 X t X t zst ;;3 u^t,U j, j Asynchronous-Hold(ut,U j, t, j);4 u^t,U t MR-Hold(ut,U t, t);5 for k 1 to M do6 retrieve hxkt1,mkt1,wkt1i from X t1;7 xkt MotionModel(u^t, xkt1, t);8 If zt has been sampled then9 z^t MeasurementModel(mt1, xkt );10 Ht,wkt DataAssociation(zt, z^t);11 x^kt ,Rkxt PoseEst(m

kt1,Ht, zt, xkt );

12 sample xkt N (x^kt ,Rkxt );13 for i 1 to length(zt) do14 If Ht(i) 0 then

// New feature15 ~mkt InvMeasModel(zit, xkt );16 add ~mkt to m

kt ;

17 else// Update feature

18 mt EKF(mt1, zit, xkt );19 end20 end21 remove unstable features of mkt ;22 else wkt wkt1; mkt mkt1;23 add hxkt ,mkt ,wkt i to X t;24 end25 if zt has been sampled then26 normalize wkt of X t;27 X t Resampler( X t);28 else X t X t;29 return X t,U t and j;

The Kalman filter gives a robust,

optimal, and recursive state

estimation to fuse redundant

sensor information.


building and real-time implementation. The TGR Explorerpowered wheelchair with the data acquisition system is shown inFigure 5.

Different tests have been performed at the Dipartimento diIngegneria Informatica, Gestionale e dellAutomazione (DIIGA).Department for analyzing the performance of the proposedMR-FastSLAM algorithm. In this section, two significant tests areintroduced and discussed: a loop-shape and an L-shape experi-ment. Figure 6 shows the DIIGA Department taken from acomputer aided design (CAD) plan as well as pose estimations ofboth experiments acquired with classical MCL and assuming thatthe map is known. The purpose of the first experiment is toinvestigate whether the multirate sampling aims to solve theloop-closing problem. The starting and ending pose of thisexperiment is x 38:4 m, y 21:8 m, and h p rd. In thesecond experiment of an L-shaped corridor, doors 1 and 2 wereclosed. Themain difficulty lies on the U-turn done in the middleof the experiment, where the robot has the greatest difficulty inestimating the orientation due to the lack of visible walls and therate of turn. The starting and ending position of this experimentis x 23:5 m, y 45 m, and h 0 rd.

Figure 7(a) and (d) shows the results obtained when using thesingle-rate FastSLAM 1.0 for both experiments (running at thelaser sampling frequency), in which the orientation estimationclearly fails when turning. In this case, the number of samples usedfor the FastSLAMestimation is 100,which is probably not enough,and therefore the results may also be improved by increasing thenumber of particles, which is associated with a higher computa-tional cost.On the other hand, Figure 7(b) and (e) shows the resultsfor the multirate FastSLAM 1.0, where it can be seen that it closesthe loop and also improves the results of the the corridor experi-ment but still has some difficulties in the estimation of theorientation.

In addition to this, the map building and also pose estima-tion robustness can be significantly improved when using theMR-FastSLAM 2.0 instead of MR-FastSLAM 1.0. Figure7(c) and (f ) shows the estimation results for the MR-FastSLAM 2.0 case, where the map estimation is much moreaccurate (corners with forms of 90). In this sense, as seen in

Figure 8, MR-FastSLAM 2.0 uses less number of features,which is also an indicator of its good accuracy, decreasing thecomputational cost, but without a detriment on pose and mapestimation. The same performance has been also obtained inthe other developed experiments that are characterized bysimple paths along the corridors and laboratories of the DIIGADepartment. Moreover, no specific assumptions have beenconsidered on the structure and on the parameters of the con-sidered mobile base. Therefore, the same performance of theproposed MR-FastSLAM algorithm can be obtained on dif-ferent mobile bases with different kinematic models andparameters.

ConclusionsIn this article, a new approach for multirate fusion, probabilis-tic self-localization, and map building for mobile robots hasbeen developed. To overcome the filtering problem ofdynamic systems with inputs and outputs sampled at a differentrates, asynchronous multirate holds have been used. In addi-tion to this, an asynchronous fusion method (with asynchro-nous prediction-update steps) is also proposed. The predictionstep is executed at least within a maximum period, whichensures a low discretization error and stability, while an updatestep fuses measurements as they are received. Although in this

Figure 5. TGR explorer with data acquisition system for FOGsensor, incremental encoders, and laser scanner.

0 10 20 30 40 500

10

20

30

40

50

60

X [m]

Y [m

]

Door #2

Door #1

Shelves

Start #1

Start #2

Figure 6. CAD plane with loop-shape (dashed line) and L-shape(continuous line) experiments pose estimations with MCL.

The goal of this method is the

minimization of the sum of squared

distances for all lines.


article the multirate asynchronous structure has been appliedto PFs in FastSLAM, it is clear that it can be used with manyother filters such as EKF, UKF, or discrete-Bayes filters, sincethey also have the prediction-update steps.

Multirate asynchronous holds are hybrid systems that gener-ate continuous signals from discrete sequences of inputs that

may be uniformly distributed in time or not. The key idea is touse interpolation and approximation functions, or in generalany primitive function, to extrapolate continuous signals. Thus,multirate holds act as interfaces for signals with different sam-pling rates, providing signals properly adapted to the requiredsampling period (discretization of the continuous signal). In thisarticle, it has been shown that general multirate holds reduceextrapolation errors in comparison to the naive solution ofkeeping the last updated value (ZOH). This is specially interest-ing in data-missing problems such as communication failures.

These ideas have been successfully applied to the multiratefusion of laser ranger and odometry measurements, by consid-ering each sensor at its own sampling frequency rate. In thissense, the laser ranger is sampled at a slow sampling rate, whilethe odometry is sampled several times faster. These ideas canbe also extended to other sensor fusion applications, such asvision and inertial fusion. In fact, in [31] and [33], preliminaryideas of multirate (synchronous) EKF and UKF were success-fully applied to the mobile robot localization problem (laserand encoder fusion) and three-dimensional tracking problem(vision and inertial fusion).

0 10 20 30 40 501015202530354045505560

X [m](a)

Y [m

]

0 10 20 30 40 501015202530354045505560

X [m](b)

Y [m

]

0 10 20 30 40 501015202530354045505560

X [m](c)

Y [m

]20 30 40 50

0

10

20

30

40

50

60

X [m](d)

Y [m

]

20 30 40 500

10

20

30

40

50

60

X [m](e)

Y [m

]

20 30 40 500

10

20

30

40

50

60

X [m](f)

Y [m

]

Figure 7. Experiment results: (a) loop experiment with FastSLAM 1.0 at single-rate; (b) loop experiment with FastSLAM 1.0 atmultirate; (c) loop experiment with FastSLAM 2.0 at multirate; (d) corridor experiment FastSLAM 1.0 at single-rate; (e) corridorexperiment with FastSLAM 1.0 at multirate; (f) corridor experiment with FastSLAM 2.0 at multirate.

0 100 200 300 400 500 6000

50

100

150

200

Time (s)

Num

. of F

eatu

res

MRFastSLAM 2.0

MRFastSLAM 1.0

Figure 8. The number of estimated features for MR-FastSLAM1.0 and 2.0.


This article shows that a significant improvement can beobtained on the localization and map-building problem whenthe proposed multirate filtering approach is applied. The resultsshow that multirate fusion improves the estimation with respectto the single-rate one for the same number of particles, since themultirate filter has lower discretization errors. Alternatively, simi-lar errors are obtained with less number of particles. Therefore,reducing the number of particles reduces the computational cost.

This article also provides an LS pose-estimation methodbased on the detected lines in indoor environments. It has beenshown that the method is robust and accurate enough with alower computational cost than that of the Araujos method[24]. The LS method can successfully be applied to the Fast-SLAM 2.0 as demonstrated through experimental results.

Finally, this article describes a novel object detection methodthat is mainly based on multiple line fitting of landmarks (walls)with regular constrained angles. This method is particularlyindicated for indoor structured environments and represents animprovement with respect to the standard LS line fitting methodwithout significantly incrementing the computational cost.

To conclude,methods for object detection, LS pose-estimation,and asynchronous multirate filtering are combined to produce arobust and efficient overall method for mobile robot localization.These methods have been validated with experimental real data ina moving mobile robot in an unknown environment for solvingthe SLAMproblem.

AcknowledgmentsThis work has been supported by the Spanish Government(MCyT) research project BIA2005-09377-C03-02 andby the Italian Government (MIUR) research project PRIN2005097207.

Keywordsmultirate fusion, probabilistic, localization, mapping,FastSLAM.

References[1] M. Dissanayake, P. Newman, S. Clark, H. Durrant-Whyte, and M. Csorba,

A solution to the simultaneous localization and map building (SLAM) prob-lem, IEEE Trans. Robot. Automat., vol. 17, no. 3, pp. 229241, 2001.

[2] S. Julier and J. Uhlmann, Reduced sigma points filters for the propaga-tion of means and covariances through nonlinear transformations, inProc. American Control Conf., 2002, vol. 2, pp. 887892.

[3] N. Gordon, D. Salmond, and A. Smith, Novel aproach to nonlinear/non-gaussian bayesian state estimation, IEE Proc. F, vol. 140, no. 2,pp. 107113, 1993.

[4] A. Doucet, N. Gordon, and V. Krishnamurthy, Particle filters for stateestimation of jump markov linear systems, IEEE Trans. Signal Processing,vol. 49, no. 3, pp. 613624, 2001.

[5] A. Smith and E. Gelfand, Bayesian statistics without tears: A sampling-resampling perspective, Am. Stat., no. 2, pp. 8488, 1992.

[6] J. Carpenter, P. Clifford, and P. Fernhead, An improved particle filter fornon-linear problems, Dept. Math., Imperial College, Tech. Rep., 1997.

[7] A. Doucet, N. de Freitas, K. Murphy, and S. Russell, Rao-Blackwellisedparticle filtering for dynamic bayesian networks, presented at the Uncer-tainty in Artificial Intelligence 2000.

[8] P. Jensfelt and H. Christensen, Pose tracking using laser scanning andminimalistic environmental models, IEEE Trans. Robot. Automat.,vol. 17, no. 2, pp. 138147, 2001.

[9] G. Zunino and H. Christensen, Simultaneous localization and mappingin domestic environments, presented at the Int. Conf. Multisensor Fusionand Integration For Intelligent Systems, 2001.

[10] J. Castellanos, J. Neira, and J. Tardos, Multisensor fusion for simulta-neous localization and map building, IEEE Trans. Robot. Automat.,vol. 17, no. 6, pp. 908914, 2001.

[11] S. Thrun, D. Fox, and W. Burgard, A probabilistic approach to concur-rent mapping and localization, Mach. Learn. Auton. Robots, vol. 31,pp. 2953, 1998.

[12] F. Dellaert, D. Fox, W. Burgard, and S. Thrun, Monte carlo localiza-tion for mobile robots, in Proc. Int. Conf. Robotics and Automation, 1999,pp. 13221328.

[13] A. Bonci, G. Ippoliti, L. Jetto, T. Leo, and S. Longhi, Methods andalgorithms for sensor data fusion aimed at improving the autonomy of amobile robot, in Advances in Control of Articulated and Mobile Robots.Berlin, Germany: Springer, 2004, vol. 10 pp. 191222.

[14] A. Bonci, G. Ippoliti, A. L. Manna, S. Longhi, and L. Sartini, Sonarand video data fusion for robot localization and environment featureestimation, presented at the 44th IEEE Conference on Decision and Con-trol and European Control Conference, Dec. 2005.

[15] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, Slam: A fac-tored solution to the simultaneous localization and mapping problem,presented at the Proc. AAAI National Conf., 2002.

[16] P. Colaneri and G. de Nicolao, Multirate LQG control of continuous-time stochastic systems, Automatica, vol. 31, pp. 591596, 1995.

[17] J. Tornero, R. Piza, P. Albertos, and J. Salt, Multirate LQG con-troller applied to self-location and path tracking in mobile robots, inProc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems, 2001,pp. 625630.

[18] D. Lee and M. Tomizuka, Multirate optimal state estimation with sen-sor fusion, in Proc. American Control Conf., 2003, pp. 28872892.

[19] P. Khargonekar, K. Poolla, and A. Tannenbaum, Robust control oflinear time-invariant plants using periodic compensation, IEEE Trans.Automat. Contr., vol. AC-30, pp. 10881985, 1985.

[20] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, Fastslam 2.0:An improved particle filtering algorithm for simultaneous localizationand mapping that probably converges, presented at the Int. Joint Conf.Artificial Intelligence 2003.

[21] G. Bourhis, O. Horn, O. Habert, and A. Pruski, An autonomousvehicle for people with motor disabilities, IEEE Robot. Automat. Mag.,vol. 7, no. 1, pp. 2028, 2001.

[22] S. Fioretti, T. Leo, and S. Longhi, A navigation system for increasingthe autonomy and the security of powered wheelchairs, IEEE Trans.Rehab. Eng., vol. 8, no. 4, pp. 490498, 2000.

[23] E. Prassler, J. Scholz, and P. Fiorini, A robotic wheelchair forcrowded public environments, IEEE Robot. Automat. Mag., vol. 7,no. 1, pp. 3845, 2001.

[24] G. Araujo and M. Aldon, Line extraction in 2D range images formobile robotics, J. Intell. Robot. Sys., vol. 40, no. 3, pp. 267297, 2004.

[25] R. Duda, P. Hart, and D. Stork, Pattern Classification, 2nd ed. NewYork: John Wiley, 2001.

[26] G. Araujo and M. Aldon, Optimal mobile robot pose estimation usinggeometrical maps, IEEE Trans. Robot. Automat., vol. 18, no. 1, pp. 8794,2002.

[27] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics. Cambridge,MA: MIT Press, 2005.

[28] A. Doucet, On sequential simulation-based methods for bayesian fil-tering, CUED/F-INFENG/TR.310, Dept. of Eng. Univ. Cambridge,UK Tech. Rep., 1998.

We developed an asynchronous

filtering method to deal with

measurements of sensors at

different sampling rates.


[29] A. Doucet, J. de Freitas, and N. Gordon, Sequential Monte Carlo Meth-ods In Practice. Berlin, Germany, Springer, 2001.

[30] R. van der Merwe, A. Doucet, N. de Freitas, and E. Wan, Theunscented particle filter, CUED/F-INFENG/TR 380, Eng. Dept.Cambridge Univ. UK Tech. Rep., 2000.

[31] L. Armesto and J. Tornero, Slam based on kalman filter for multiratefusion of laser and encoder measurements, in Proc. IEEE Int. Conf.Intelligent Robots and Systems, 2004, pp. 18601865.

[32] A. Huster and S. Rock, Relative position sensing by fusing monocu-lar vision and inertial rate sensors, in Proc. Int. Conf. on AdvancedRobotics, 2003, pp. 15621567.

[33] L. Armesto, S. Chroust, M. Vincze, and J. Tornero, Multi-rate fusionwith vision and inertial sensors, in Proc. Int. Conf. on Robotics and Auto-mation, 2004, pp. 193199.

[34] J. Tornero, Y. Gu, and M. Tomizuka, Analysis of multi-rate discreteequivalent of continuous controller, in Proc. American Control Conf.,1999, pp. 27592763.

[35] J. Tornero and M. Tomizuka, Dual-rate high order hold equivalentcontrollers, in Proc. American Control Conf., 2000, pp. 175179.

[36] , Modeling, analysis and design tools for dual-rate systems, inAmerican Control Conference, 2002, pp. 41164121.

[37] L. Armesto and J. Tornero, Dual-rate high order holds based on prim-itive functions, in Proc. American Control Conf., 2003, pp. 11401145.

Leopoldo Armesto received the B.Sc. degree in electronicengineering, the M.Sc. degree in control systems engineer-ing, and the Ph.D. degree in automation and industrialcomputer science from the Technical University of Valencia,Spain, in 1998, 2001, and 2005, respectively. He held a Ph.D.fellowship for three years at the Department of Systems Engi-neering and Control at the same University, where he is anassistant professor since 2004. He is currently a member ofthe Robotics and Automation Research Group and also ofthe Design and Manufacture Institute of the TechnicalUniversity of Valencia. He was a visiting researcher at theAutomation and Control Institute (ACIN) Vienna Universityof Technology in 2003 and Dipartimento di Ingegneria Infor-matica, Gestionale e dellAutomazione (DIGA) UniversitaPolitecnica delle Marche, in 2005. He was awarded secondbest M.Sc. Spanish National Prize and Best Technical Univer-sity of Valencia Ph.D. prize.

Gianluca Ippoliti received the doctoral degree in elec-tronic engineering in 1996 from the Universita Politecnicadelle Marche (formerly University of Ancona), Italy. From1997 to 1998, he was with ISERM Unite 103, Montpellier,France, and then with the University of Montpellier I in theframework of the European research projects CAMARNand MOBINET. In 2002, he received a Ph.D. degree inintelligent artificial systems from the Universita Politecnicadelle Marche, and from 2002 to 2005, he was a postdoctoralfellow at the Dipartimento di Ingegneria Infomatica, Gestio-nale e dellAutomazione, at the same University. SinceMarch 2005, he is an assistant professor at the UniversitaPolitecnica delle Marche. His main research interests includeswitched systems and supervisory control, neural network-based system identification and control, marine vehicle con-trol, and mobile robot localization and control.

Sauro Longhi received the doctoral degree in electronicengineering in 1979 from the University of Ancona, Italy,

and postgraduate diploma in automatic control in 1985 fromthe University of Rome La Sapienza, Italy. From 1980 to1981, he held a fellowship at the University of Ancona.From 1981 to 1983, he was with the Telettra S.p.A., Chieti,Italy. Since 1983, he has been at the Dipartimento di Elet-tronica e Automatica of the University of Ancona, nowDipartimento di Ingegneria Informatica, Gestionale e del-lAutomazione, of the Universita Politecnica delle Marche,Ancona. Currently, he holds the position of full professor incontrol systems technologies and of coordinator of the Ph.Dcourse in intelligent artificial systems at the Universita Poli-tecnica delle Marche, Ancona. His research interests includemodeling, identification and control of linear systems, con-trol of mobile base robots and underwater vehicles, servicerobots for assistive applications, Web technology in processcontrol and remote control laboratories, power managementin hybrid cars, and cooperative control of autonomousagents. In these fields, he has published more then 180 articlesin international journals and conferences. His research hasbeen supported by the Italian Ministry of Education andUniversity, the Italian National Research Council, the ItalianSpace Agency, and the European Union. He is a member ofthe Technical Committee on Marine Systems in IFAC. Hewas the NOC chair of the IFAC Conference on ControlApplications in Marine Systems, Ancona, Italy, July 2004, andthe IPC chair of the IFAC Conference on Control Applica-tions in Marine Systems, CAMS 2007, Bol, Croatia, Septem-ber 2007. Since April 2007, he is the general administrator ofthe academic spin-off IDEA.

Josep Tornero received the M.S. degree in systems andcontrol from the University of Manchester, Institute ofScience and Technology, in 1982, and the Ph.D. degree inelectrical engineering from the Technical University ofValencia, Spain, in 1985. He is currently a professor at theDepartment of Systems Engineering and Control, Headof the Robotics and Automation Research Group, andalso the director of the Design and Manufacture Instituteof the Technical University of Valencia. He has been a vis-iting professor at the CIRSSE (NASA Center for Intelli-gent Robotics Systems for Space Exploration); theRensselaer Polytechnic Institute at Troy (New York); andat the Department of Mechanical Engineering at theUniversity of California (Berkeley). He is particularlyinterested in modeling, control, and simulation of auto-guided vehicles and robot arms; modeling, analysis, andcontrol of multirate sampled data systems; and collisiondetection or avoidance and automatic trajectory generation.He has participated in many European research projects suchas ESPRIT, BRITE, EUREKA, and STRIDE, and in edu-cational projects such as ERAMUS, INTERCAMPUS,ALPHAS, and TEMPUS.

Address for Correspondence: Sauro Longhi, Dipartimento diIngegneria Informatica, Gestionale e dell Automazione,Universita Politecnica delle Marche via Brecce Bianche,60131 Ancona, Italy. E-mail: [email protected].


/ColorImageDict > /JPEG2000ColorACSImageDict > /JPEG2000ColorImageDict > /AntiAliasGrayImages false /CropGrayImages true /GrayImageMinResolution 150 /GrayImageMinResolutionPolicy /OK /DownsampleGrayImages true /GrayImageDownsampleType /Bicubic /GrayImageResolution 300 /GrayImageDepth -1 /GrayImageMinDownsampleDepth 2 /GrayImageDownsampleThreshold 2.00333 /EncodeGrayImages true /GrayImageFilter /DCTEncode /AutoFilterGrayImages false /GrayImageAutoFilterStrategy /JPEG /GrayACSImageDict > /GrayImageDict > /JPEG2000GrayACSImageDict > /JPEG2000GrayImageDict > /AntiAliasMonoImages false /CropMonoImages true /MonoImageMinResolution 1200 /MonoImageMinResolutionPolicy /OK /DownsampleMonoImages true /MonoImageDownsampleType /Bicubic /MonoImageResolution 600 /MonoImageDepth -1 /MonoImageDownsampleThreshold 1.00167 /EncodeMonoImages true /MonoImageFilter /CCITTFaxEncode /MonoImageDict > /AllowPSXObjects false /CheckCompliance [ /None ] /PDFX1aCheck false /PDFX3Check false /PDFXCompliantPDFOnly false /PDFXNoTrimBoxError true /PDFXTrimBoxToMediaBoxOffset [ 0.00000 0.00000 0.00000 0.00000 ] /PDFXSetBleedBoxToMediaBox true /PDFXBleedBoxToTrimBoxOffset [ 0.00000 0.00000 0.00000 0.00000 ] /PDFXOutputIntentProfile (None) /PDFXOutputConditionIdentifier () /PDFXOutputCondition () /PDFXRegistryName () /PDFXTrapped /False

/SyntheticBoldness 1.000000 /Description >>> setdistillerparams> setpagedevice

probabilistic self-localization and mapping - an asynchronous multirate approach

Documents