Advanced Robotics 24 (2010) 585604

brill.nl/ar

Full paper Analysis of Rank-Based Resampling Based on Particle Diversity in the RaoBlackwellized Particle Filter for Simultaneous Localization and Mapping
Nosan Kwak a, , Kazuhito Yokoi a and Beom-Hee Lee b
Humanoid Research Group/Joint Robotics Laboratory UMI3218/CRT, National Institute of Advanced Industrial Science and Technology, Central 2, AIST, Umezono 1-1-1, Tsukuba, Ibaraki 305-8568, Japan School of Electrical Engineering and Computer Science, Seoul National University, Gwanak 599, Gwanak-gu, Seoul 151-742 South Korea Received 6 March 2009; accepted 10 June 2009
a

Abstract In order to solve the simultaneous localization and mapping (SLAM) problem of mobile robots, the Rao Blackwellized particle lter (RBPF) has been intensively employed. However, it suffers from particle depletion problem, i.e., the number of distinct particles becomes smaller during the SLAM process. As a result, the particles optimistically estimate the SLAM posterior, meaning that particles tend to underestimate their own uncertainty and the lter quickly becomes inconsistent. The main reason of loss of particle diversity is the resampling process of RBPF-SLAM. Standard resampling algorithms for RBPF-SLAM cannot preserve particle diversity due to the behavior of their removing and replicating particles. Thus, we propose rank-based resampling (RBR), which assigns selection probabilities to resample particles based on the rankings of particles. In addition, we provide an extensive analysis on the performance of RBR, including scheduling of resampling. Through the simulation results, we show that the estimation capability of RBPF-SLAM by RBR outperforms that by standard resampling algorithms. More importantly, RBR preserves particle diversity much longer, so it can prevent a certain particle from dominating the particle set and reduce the estimation errors. In addition, through consistency tests, it is shown that RBPF-SLAM by the standard resampling algorithms is optimistically inconsistent, but RBPF-SLAM by RBR is so pessimistically inconsistent that it gives a chance to reduce the estimation errors. Koninklijke Brill NV, Leiden and The Robotics Society of Japan, 2010 Keywords SLAM, particle diversity, resampling, ranking, consistency

To whom correspondence should be addressed. E-mail: [email protected]

DOI:10.1163/016918610X487126

Koninklijke Brill NV, Leiden and The Robotics Society of Japan, 2010

586

N. Kwak et al. / Advanced Robotics 24 (2010) 585604

1. Introduction The RaoBlackwellized particle lter (RBPF) was introduced about a decade ago as an effective means to solve the simultaneous localization and mapping (SLAM) problem by Murphy [1] and Doucet et al. [2]. This solution to SLAM is RBPFSLAM, which is also known as FastSLAM [3]. The SLAM posterior is normally the joint estimation of a robots path and a map. However, RBPF-SLAM factors the joint estimation using RaoBlackwellization, which factors a state into a sampled part (path) and an analytical part (map). In RBPF-SLAM, the robots path is estimated by a particle lter and the map by low-dimensional extended Kalman lters (EKFs). RBPF-SLAM has two major advantages compared to other approaches. (i) By factoring the SLAM posteriors, RBPF-SLAM has linear time complexity. (ii) Unlike EKF-SLAM, RBPF-SLAM allows each particle to perform its own data association, which implements multi-hypothesis data association [3]. The ability to simultaneously pursues multi-hypothesis data association makes RBPF-SLAM more robust to data association problems than algorithms based on incremental maximum likelihood data association such as EKF-SLAM. This special characteristic is dependent on particle diversity. The bigger the number of distinct particles, the more chance to close a loop because new observations can affect the locations of the landmark [4]. Thus, it is crucial for RBPF-SLAM to maintain particle diversity as long as possible. However, the innate disadvantage of RBPF-SLAM is that past pose estimation errors of a robot are not forgotten, which means that they are recorded in the feature estimates. Whenever the resampling process is conducted, the entire robot paths and feature estimates of rejected particles are lost forever. As a result, the number of particles representing past paths and feature estimates severely decreases. This is called the particle depletion problem [5]. In other words, as the particle set loses its diversity, it becomes over-condent, which means it tends to underestimate its own uncertainty. According to the work by Bailey et al. [4], the particle diversity is drastically attenuated when a robot closes a large traverse loop. Another critical issue in RBPF-SLAM is the consistency, which is the ability of the lter to accurately estimate uncertainty. A lter can be inconsistent in either an optimistic or a pessimistic way. According to Ref. [6], a lter is optimistic or over-condent if there is signicant bias in the estimates, the errors are too large compared to the lter-calculated covariance or the covariance is too small. On the other hand, a lter is pessimistic or conservative if the covariance is too large. It has been shown in simulation works [4, 7] that the current RBPF-SLAM algorithm is inconsistent in an optimistic way and Stachniss et al. [8] showed in their experiments that RBPF-SLAM is consistent only in a local area (or in the short term). According to Ref. [4], RBPF-SLAM in its current form cannot produce consistent estimates in the long term, although it stays reasonably consistent for a few tens of seconds after starting. Beevers et al. [7] improved the consistency by applying block proposal distribution in the sampling process which exploits future information. Their

N. Kwak et al. / Advanced Robotics 24 (2010) 585604

587

modied strategies, however, cannot guarantee a consistent lter in the long term. The consistency of a lter is closely related to particle depletion, which is directly related to the resampling process in RBPF-SLAM. Thus, we extensively conducted analysis on standard resampling algorithms to investigate the relationship between particle diversity and resampling algorithms in the previous work [9]. According to our results, all resampling algorithms cannot preserve particle diversity. The existing results show that the loss of particle diversity causes critical problems such as poor data association and inconsistent estimates. Even though RBPFSLAM, thanks to accurate sensors, is applicable to practical problems, it is desirable to guarantee its performance over the long term. One-particle RBPF-SLAM sometimes shows results as good as one with 100 particles. This can be interpreted that the single particle estimates the SLAM posterior with the help of accurate sensors. However, if the only particle has large estimation errors, then there is no way to compensate for the errors. Besides, according to our previous work [10], performance by taking the mean of particles is better than that by the most weighted particle. Thus, the only way for consistent estimates over the long term is to keep particle diversity as long as possible. To preserve particle diversity, in this work, we propose rank-based resampling (RBR), which is an indirect resampling algorithm since it uses the ranking of a particle for resampling. Actually, we briey introduced the RBR in our previous work [9], but we underestimated its effectiveness for keeping particle diversity. In this work, we thoroughly analyze RBR in terms of particle diversity and consistency of RBPF-SLAM, and emphasize its capabilities. For the organization of this work, a brief introduction to RBPF-SLAM including its particle diversity and consistency will be presented in Section 2. RBR will be described in Section 3, and its performance will be investigated in Section 4 with estimation errors, particle diversity and consistency of RBPF-SLAM. Finally, concluding remarks on the capability of RBR will be given in Section 5. 2. RBPF-SLAM The structure of SLAM enables particle lters to be applicable since the SLAM problem is characterized by a conditional independence between any two disjoint sets of landmarks in the map, given the robots pose [11]. It means if the robots true path was given, locations of all landmarks would be estimated independently. This special particle lter is known as RBPF. In this section, the algorithm of SLAM using RBPF (RBPF-SLAM) is briey introduced in terms of particle diversity and consistency of RBPF-SLAM. 2.1. RBPF-SLAM Algorithm RBPF-SLAM enables us to factor the SLAM posterior into a product of simpler terms. The key mathematical insight of RBPF-SLAM pertains to the fact that the

588

N. Kwak et al. / Advanced Robotics 24 (2010) 585604

full SLAM posterior can be factored as [11]:

Nf

p(x1:t , M|z1:t , u1:t , c1:t ) = p(x1:t |z1:t , u1:t , c1:t )

n=1

p(mn |x1:t , z1:t , c1:t ),

(1)

where x1:t is the robot path up to time t, M (mn is nth landmark and there are Nf landmarks) is the map, and z1:t , u1:t and c1:t are the measurements, controls and correspondences up to time t, respectively. RBPF-SLAM uses a particle lter to estimate the robots pose and EKFs to estimate the robots map. More specically, the mapping problem can be factored into separate low-dimensional EKFs using the conditional independence among landmarks [3]. A particle at time t, Yt[k] , is denoted by:
[k] Yt[k] = x1:t , [k] , 1,t [k] [k] 1,t , . . . , Nf ,t , [k] Nf ,t

(2)

[k] where the [k] indicates the index of the particle and x1:t is the robot path estimate [k] of the kth particle at time t. [k] and n,t are mean and covariance of the Gaussian n,t distribution, representing the nth feature location relative to the kth particle, respectively. Altogether, these elements form the kth particle, Yt[k] , and there are a total of Np particles and Nf features in a particle set. The RBPF-SLAM algorithm consists of four steps as follows [11]: [k] (i) Sampling. xt[k] p(xt |xt1 , z1:t , u1:t , c1:t ). i (ii) Measurement update. For each observed feature, zt , identify the corresponi , and incorporate the measurement zi into the dence j for the measurement zt t [k] corresponding EKF by updating the mean [k] and covariance j,t . j,t

(iii) Importance weight. Calculate the importance weight w[k] for the new particle. (iv) Resampling. Sample Np particles with replacement, where each particle is sampled with a probability proportional to w[k] . 2.2. Resampling Algorithms In common particle ltering, resampling is used to reduce the particle degeneracy, which occurs because particles or samples have negligible weights over time. Through resampling (removing particles with low weights and replicating more particles in more probable regions), a particle set can better reect the true posterior of SLAM. However, resampling makes particles with high weights be selected more and more often. As a result, after a few iterations of the algorithm, the particles with high weights dominate the particle set. Thus, a particle cannot perform its own function because it is a copy of the dominant particle at a certain point. The most commonly used resampling algorithms in SLAM are different variants of stratied sampling such as residual resampling (RR) and systematic resampling (SR). SR is the most commonly used since it is the fastest resampling algorithm

N. Kwak et al. / Advanced Robotics 24 (2010) 585604 Table 1. RSR algorithm Algorithm RSR(w, Nin , Nout ) Input: A set of normalized weights w, and number of inputs Nin and outputs Nout Output: A set of numbers to replicate each particle, NR Generate a random number, U0 U([0, N1 ]) out for i = 1 to Nin [i] NR = [(w[i] Ui1 ) Nout ] + 1 [i] Ui = Ui1 + NR /Nout w[i] end

589

for computer simulations. Boli et al. [12] proposed the residual systematic rec sampling (RSR), which produces an identical resampling result as SR with fewer operations and less memory access. In the previous work [9], we conrmed that RSR for RBPF-SLAM shows the best performance among the variants of stratied resampling approaches. Thus, we will compare our resampling algorithm with RSR. The algorithm of RSR is presented in Table 1 where RSR draws the rst uniform [i] [i] random number, U0 = U0 , and updates it by Ui = Ui1 + NR /Nout wn . The output NR is an array of indices, which means how many times each particle is replicated for the next particle set. In the RSR algorithm, the updated uniform random number is formed in a different fashion, compared to the standard SR. That is, it requires only one iteration loop. In addition, in RSR, resampling is performed in xed time, whereas in SR, it is not performed in xed time because the number of replicated particles is random, which makes an unspecied number of operations. 2.3. Particle Diversity In the resampling step, particles are resampled based on their importance weights, which are computed by the ratio of the target or posterior distribution and the proposal distribution for sampling as [11]: wt[k] = = target distribtuion proposal distribution
[k] [k] p(x1:t1 |u1:t1 , z1:t1 , c1:t1 )p(xt[k] |x1:t1 , u1:t , z1:t , c1:t ) [k] p(x1:t |u1:t , z1:t , c1:t )

(3)

[k] where it is assumed that paths in x1:t1 have been generated according to the tar[k] get distribution one step earlier, p(x1:t1 |u1:t1 , z1:t1 , c1:t1 ). Note that the most recent measurement zt is used to construct the proposal distribution from which particles are sampled. If the sensor is very accurate relative to the motion model, the target distribution will be sharply peaked relative to a at proposal distribution.

590

N. Kwak et al. / Advanced Robotics 24 (2010) 585604

Figure 1. Number of distinct particles over time.

After resampling, a small percentage of particles are assigned non-negligible importance weights, causing signicant duplication of a few dominant particles. Once the particles are removed in the set, particle diversity cannot be recovered because particles share the robot path and feature estimates at some point. This is the particle depletion problem. Over time, particle depletion could result in particles drifting away from the true state [13]. A measure for the rate of loss of particle diversity is obtained by recording the number of distinct particles having different estimates for a landmark in the set. Once a landmark goes out of the robots sight, resampling causes some particles to be rejected and others to be replicated. At rst, all of the particles are distinct, which means they have different feature estimates about a landmark. As time passes, only particles with high weights survive and particles with low weights disappear together with their feature estimates. Thus, the number of distinct estimates of the landmark becomes smaller. The number of distinct particles is counted after every resampling process and its transition (the result is obtained from simulations in the environment of Fig. 6 with the condition in Section 4.1.) in the case of using RSR is shown in Fig. 1. Soon after closing a loop, the ratio of distinct particles becomes smaller than 3% of the initial distinct particles. As is seen by this example, particle depletion often occurs and due to this particle depletion, RBPF-SLAM might produce very inaccurate estimates. 2.4. Consistency of RBPF-SLAM The 2 distribution is often used to check state estimators for consistency, i.e., whether their actual errors are consistent with the variances calculated by the estimator [6]. For the RBPF-SLAM algorithm, to measure if a lter is consistent, one would compare its estimate with the probability density function (PDF), obtained from an ideal Bayesian lter. The PDF is, however, not available for the RBPF-

N. Kwak et al. / Advanced Robotics 24 (2010) 585604

591

SLAM algorithm. Instead, the true pose of the robot can be known in simulations, but not in real experiments. With this information, the normalized estimation error squared (NEES) dened in (4) can be used to investigate the consistency of a lter [4, 6, 7]. NEES is dened as: t = (xt xt )T Pt1 (xt xt ), (4) where {xt , Pt } are the estimated mean and covariance of particles at time t. A mea sure of lter consistency is obtained by examination of the average NEES over N Monte-Carlo runs of the lter. Under the assumptions that the lter is consistent and is approximately linear Gaussian, t is 2 distributed with dim(xt ) degrees of freedom. The consistency of RBPF-SLAM is evaluated by conducting several Monte-Carlo runs and computing the average NEES. Given N runs, the average NEES is obtained as: t = 1 N
N

ti .
i=1

(5)

Given the hypothesis of a consistent linear Gaussian lter, N t has a 2 density with N dim(xt ) degrees of freedom. Thus, in case of three-dimensional robot pose, with N = 50, the 95% probability concentration region for t is bounded by the in terval [2.36, 3.72] [6]. If t rises signicantly higher than the upper bound, the ler is optimistic or over-condent. If it tends below the lower bound, the lter is pessimistic or conservative. The average NEES of the current RBPF-SLAM framework presented in Fig. 2 shows that the lter is not consistent. More precisely, at rst, the lter is pessimistic, but after about 3000 time steps, it suddenly becomes optimistic.

Figure 2. Average NEES of the standard RBPF-SLAM algorithm over 50 Monte-Carlo runs. Two horizontal red lines indicate the upper and the lower bounds of 2 , which are obtained with the assumption that the lter is consistent.

592

N. Kwak et al. / Advanced Robotics 24 (2010) 585604

3. RBR 3.1. Ranking for Selection Direct resampling with importance weights causes loss of particle diversity and, as a result, RBPF-SLAM becomes inconsistent in an optimistic way. If the optimistic estimation becomes erroneous, there would be no way to correct the error in RBPF-SLAM. Thus, for any particle lters including RBPF, preserving particle diversity is of paramount importance since each distinct particle represents a different hypothesis of the SLAM posterior. In other words, as the number of distinct particles becomes smaller, several probable regions of the posterior cannot be estimated because the particles that are assigned in the regions have been removed in the resampling process. The diversity issue also occurs in a genetic algorithm [14], which is a search technique used in computing to nd exact or approximate solutions to optimization and search problems. In a genetic algorithm, each gene that represents a solution to a problem is reproduced using its own tness. The mechanism of the reproduction process is very similar to the resampling process in particle ltering. Most of the schemes in genetic algorithms cannot overcome premature convergence or diversity better than the rank-based reproduction [15]. A ranking is used as a transformation function that assigns a new value to a gene based on its tness. By using not a tness but a ranking, it is possible to slow down the premature convergence. Furthermore, it is possible to control the speed of the convergence with varying the ranking function. To take advantage of the rank-based reproduction scheme, it is modied in this work and used as RBR. In addition, we thoroughly analyze RBR in terms of particle diversity and consistency of RBPF-SLAM. 3.2. RBR for RBPF-SLAM In order to employ the rank-based reproduction scheme in the RBPF-SLAM framework, RBR is described in this section. RBR consists of two parts. The rst part is assigning a selection probability of a particle using a ranking function. The ranking can be easily obtained by sorting the particles by the magnitude of their importance weights. The second part is standard resampling with the selection probability of each particle. This ranking approach seems to discard information of importance weights, but it actually discards the information about the magnitude of importance weights and assigns relative magnitude instead. Therefore, RBR can be called an indirect resampling algorithm. In the current RBPF-SLAM framework, the importance weight is the only measure to evaluate the performance of a particle. However, when an accurate sensor such as a laser range nder is used, the differences of importance weights between the most weighted particle and the others having slight pose differences are so large that only the most weighted particle dominates the particle set. As a result, several particles are suddenly rejected in the particle set. With RBR, these ill-balanced performance measures are linearly re-assigned using the ranking function.

N. Kwak et al. / Advanced Robotics 24 (2010) 585604

593

With the selection probabilities of all the particles, the RBR performs the standard resampling, RSR. In this work, a linear ranking function is used to assign the selection probability of a particle. When the ranking function is linear, the mean of the selection probabilities will correspond to the median rank in the particle set [15]. One can think of a non-linear ranking function, but it will shift the mean toward the top of the set. This is not desirable to preserve particle diversity as long as possible because of larger differences of selection probabilities than in the linear ranking function. Thus, the non-linear ranking function is not considered in this work. The slopes of linear functions are adjusted to control the selection pressure, which is the ratio of the best particles selection probability over the average selection probability of all particles in the set. The following linear equation is used as [k] the ranking function for the selection probability of the kth particle, ps :
[k] ps =

1 (rank(k) 1) , max (max min ) Np Np 1

(6)

where Np is the number of particles, max /Np is the maximum selection probability of the highest weight and min /Np is the minimum selection probability of the lowest weight. The particle at the rst ranking gets the highest selection probability, whereas the particle at the last ranking gets the lowest selection probability. When the number of particles is xed, min = 2 max 0 should be satised, and max usually has a value between 1 and 2 [16]. Depending on max , the selection probability varies. The larger max , which means the selection pressure is getting larger, the larger the differences between selection probabilities. The relation between selection probabilities and rankings of particles with varying max is plotted in Fig. 3 in the case of six particles in the set. This ranking approach is inserted into the RBR

Figure 3. Selection probabilities over rankings of particles with varying max .

594

N. Kwak et al. / Advanced Robotics 24 (2010) 585604 Table 2. RBR algorithm for resampling in RBPF-SLAM Algorithm RBR (w, Nin , Nout ) Input: A set of normalized weights w, and Nin and Nout Output: A set of numbers to replicate each particle, NR 1: 2: 3: 4: 5: 5: 6: 7: Set a value between [1,2] to max . min 2 max [wsorted , Isorted ] Sort w in a descending order for i = 1 to Nin k Isorted (i) /*i is the ranking.*/ [k] ps (max (max min ) (i 1)/(Nin ))/Nin . end for NR Call RSR( ps , Nin , Nout ).

algorithm, shown in Table 2, where Isorted stores indices of particles in a descending order, i.e., the rst element of Isorted has the highest ranking. 3.3. A Biased Resampler It is worth noting that RBR has a big difference from the standard resampling algorithms such as RSR since RBR uses the indirect information of importance weights. Particles drawn from RBR construct a different distribution from the true posterior due to the indirect usage of the importance weights. In this sense, adding new random particles also distorts the particle distribution. This kind of resampler is called a biased resampler which violates the unbiasedness or proper weighting condition, dened as follows. A random variable X drawn from a proposal distribution q is said to be properly weighted [17] by a weighting function w(X) with respect to the target distribution if for any integrable function h: Eq {h(X)w(X)} = E {h(X)}. A set of random samples and weights with respect to if:
Np

(7)

(x [k] , w[k] )

is said to be properly weighted

lim

Np [k] [k] k=1 h(x )w Np [k] k=1 w

= E {h(X)}.

(8)

The effect of RBR seems like that of regularization, which attempts to create a more diverse posterior density approximation by relocating the particles to a more continuous distribution [13]. However, RBR does not draw a new particle. Instead, it selects particles taking into account the indirect information of the posterior, the ranking. Figure 4 shows the normalized importance weights of all the particles in case of the RSR. In Fig. 4, few particles have very high weights, whereas most of the particles have negligible weights even though the weights are normalized. Therefore, after RSR, only the particles with high weights survive and are replicated

N. Kwak et al. / Advanced Robotics 24 (2010) 585604

595

Figure 4. Importance weights of all the particles just before the loop-closure when applying RSR. Table 3. Number of replicas of the dominant particles Particle index No. replicas 1 29 2 1 3 5 21 1 46 2 49 9 83 1 87 1 88 1 96 42 100 8

as shown in Table 3. As shown in the above example, peaked weight distribution severely damages particle diversity and particle depletion often occurs in RBPFSLAM since the measurement of an accurate sensor is used to build the sampling distribution (proposal distribution). It is known that the standard resampling satisfying the proper weighting condition cannot resolve the particle depletion problem [4, 9]. According to our previous works [10], in the current RBPF-SLAM framework, keeping particle diversity is very important because all the particles drawn from the proposal distribution are valuable. When particle diversity is preserved, we showed that mean particle data gives the better estimation results. It is worth testing how RBPF is biased using RBR instead of the unbiased RSR, in the perspective of unbiasedness of an estimator. Generally, means of particle poses and variances are unbiased and asymptotically unbiased, respectively [6]. The unbiasedness of an estimator is dened as: E[x] = 0, (9) where x is the estimation error. A simulation is conducted for the bias test and the result is provided in Fig. 5, where means of particle paths and features are presented with the true path and landmarks. According to the simulation result, the

596

N. Kwak et al. / Advanced Robotics 24 (2010) 585604

Figure 5. Path mean and feature mean of all the particles for checking the bias of RBR.

particle mean estimates the path and the landmarks correctly. The estimation performance of RBR is usually better than that of RSR thanks to the particle diversity. In this sense, RBR can be a solution to keep particle diversity even though it does not satisfy the proper weighting condition. In addition, results in Refs [18, 19] indicate that the proper weighting condition is unnecessary to obtain convergence results [5]. In this paper, strategies that reallocate particles such as articial evolution [20] are not considered since the particle lter is used for SLAM, which has to deal with robot pose and the map at the same time. Perturbation to the particles cannot inuence the map data that each particle stores. 4. Simulation Results In this work, we only conducted simulations of RBPF-SLAM with a mobile robot since comparing the estimation error of the lters requires the true values of pose and features. In the simulation, it was assumed that a mobile robot detects point features using a laser range scanner that produces the range and the bearing to a feature. Also, it was assumed that data association between measurements and features is known in order to effectively investigate the performance of the lter. The simulation works were focused on the consistency and particle diversity of RBPF-SLAM. For this purpose, NEES, particle diversity and r.m.s. estimation errors, including scheduling of RBR and ranking functions, were analyzed in this work.

N. Kwak et al. / Advanced Robotics 24 (2010) 585604

597

Figure 6. Sparse environment; 54 landmarks. The point C is the only loop closing point.

4.1. Simulation Set-up RBPF-SLAM simulations were performed on a sparse environment shown in Fig. 6 (and Fig. 9). Simulations were run with 100 particles. In Fig. 6, there is only one outer loop-closure, the point C. In every simulation, the mobile robot closed the large loop twice. Note that the resampling process is not conducted at every iteration of RBPF-SLAM. It is conducted only when the effective sample size [21] falls below a threshold to keep particle diversity as long as possible. We conducted simulations to compare the performance of several thresholds. In addition, we also conducted simulations with varying max in (6). The weights of all the particles are initialized with the same weight after every resampling. The motion noise and the observation noise of the robot were set to (0.3 m/s, 3 /s) and (0.1 m, 1 ), respectively. Control and observation times were set to 25 and 200 ms, respectively. Every result in the following simulations was obtained by averaging over 50 Monte-Carlo runs since results of RBPF-SLAM are different at each run. 4.2. Estimation Errors In order to compare the localization and mapping performance of RBR, we measured estimation results with varying max of (6). Estimation errors with different max in the environment of Fig. 6 are summarized in Table 4, where r.m.s. position and orientation of the robot pose, and feature errors are denoted RMSEP , RMSEO

598

N. Kwak et al. / Advanced Robotics 24 (2010) 585604 Table 4. Summary of estimation errors with different max max 1.1 1.3 2.0 RMSEP (m) 0.1239 0.1267 0.1637 RMSEO (rad) 0.0554 0.0543 0.0491 RMSEF (m) 0.1239 0.1201 0.1529

Table 5. Comparison of estimation errors between RSR and RBR in the environment of Fig. 6 Resampling RSR RBR Remarks RMSEP (m) 0.3099 0.1267 59% RMSEO (rad) 0.0468 0.0543 16% RMSEF (m) 0.3590 0.1201 66%

and RMSEF , respectively. The estimation errors over 50 Monte-Carlo runs were collected after the robot closed the large loop twice. These results were obtained by taking the mean of all the particles since we conrmed that the mean of particles produces the best results of RBPF-SLAM [10]. According to the results of Table 4, the case of max = 1.3 showed the least errors overall even though its position error was slightly larger than the case of max = 1.1. From now on every result for RBR was from the simulations with max = 1.3. In order to compare the estimation performance of RBR with that of RSR, simulation results are summarized in Table 5. Note that again, these results are obtained by taking the mean of all the particles since, for instance, the error variance of feature errors are much smaller (0.0033 m) when taking the mean of particles than when taking the most weighted particle (0.0381 m). In Table 5, estimation errors of RBPF-SLAM by RBR were much reduced compared to those of RBPF-SLAM by RSR. The bigger error in the orientation by 16% (=0.4286 ) is small compared to the improvements in the position and the feature errors. These estimation improvements come from particle diversity. The estimation improvements in RMSEP and RMSEF were about 59 and 66%, respectively. In addition, the standard deviation of the estimation errors over 50 runs were much reduced. The standard deviations by RBR were 0.0537 (RMSEP ) and 0.0574 m (RMSEF ) while those by RSR were 0.1049 (RMSEP ) and 0.1774 m (RMSEF ). 4.3. Particle Diversity Comparison of the loss of particle diversity between RBPF-SLAM by RSR and RBPF-SLAM by RBR is shown in Fig. 7, where the time step is presented when the robot closed the large loop. The rate of the loss of particle diversity by RBR is

N. Kwak et al. / Advanced Robotics 24 (2010) 585604

599

Figure 7. Particle diversity of RBPF-SLAM by RBR decreases almost linearly, which is signicantly different from the result of RBPF-SLAM by RSR. In both resampling approaches, no resampling occurred after the rst loop-closure, which is presented as the time step.

Table 6. Comparison of estimation errors varying a threshold for resampling Threshold (%) 25 50 75 RMSEP (m) 0.1267 0.1312 0.1513 RMSEO (rad) 0.0543 0.0537 0.0534 RMSEF (m) 0.1201 0.1324 0.1563

almost linear, whereas that by RSR is exponential. After the loop-closure, the number of distinct particles by RBR was more than 50% of the particle size. The reason why the graphs keep the constant value after the loop-closure is because no resampling was conducted after the rst loop-closure. We conrmed that resampling after the large loop-closure is not effective for RBPF-SLAM performance. Even though the loss of particle diversity by RBR cannot be prevented, RBR makes more than half of the particle size survive after the robot closes the large loop. Related to particle diversity, we also conducted simulations for the time to instigate the RBR. The RBR was conducted when the effective sample size falls below the thresholds as shown in Table 6, where estimation errors are presented. A threshold of 25%, for instance, means that the RBR was conducted whenever the ratio of the effective sample size falls below 25% of the particle size. According to the results in Table 6, the case of the 25% threshold showed the most accurate results overall. Also, note that the lower the threshold, the less computational cost, since the lower threshold means that the resampling occurs less often.

600

N. Kwak et al. / Advanced Robotics 24 (2010) 585604

Figure 8. Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR. Two horizontal red lines indicate the upper and the lower bounds of 2 , which are obtained with the assumption that the lter is consistent. Both approaches show that they are not consistent, but that by RBR is inconsistent in a pessimistic way and that by RSR is inconsistent in an optimistic way.

4.4. Consistency We measured the average NEES over 50 Monte-Carlo runs to test whether RBPFSLAM by RBR is consistent over the long term and compared the results with the average NEES of RBPF-SLAM by RSR, which are shown in Fig. 8. According to the average NEES of RBPF-SLAM by RBR, although it is not consistent (the NEES is not always inside the two bounds; red lines), RBR produces a very different graph from that of RSR. More specically, RBPF-SLAM by RSR is inconsistent in an optimistic way, meaning that the estimated uncertainty is smaller than the true uncertainty. On the other hand, RBPF-SLAM by RBR is inconsistent in a pessimistic way, meaning that the uncertainty of particles is larger than the true uncertainty. Several SLAM literature [4, 7] only report inconsistency of RBPF-SLAM as optimistic estimates. No instance of pessimistic estimates of RBPF-SLAM has been reported. In our previous work [10], we conrmed that after several loop-closures, one can obtain an accurate map and path by taking the mean of particles when the particle diversity is preserved even though RBPF-SLAM is pessimistically inconsistent. When RBPF-SLAM is optimistically inconsistent, however, there is no way to induce the better map and path than those of the most weighted particle since the uncertainty of particles is too small to keep the particle diversity. 4.5. Analysis in a Large Environment We also analyzed the performance of RBR in a large environment; 240 m 240 m, as shown in Fig. 9. The resulting data were also obtained by averaging over 50 Monte-Carlo runs. In Table 7, the resulting data such as estimation errors and number of distinct particles are compared with those of RSR. RBR produced about 50%

N. Kwak et al. / Advanced Robotics 24 (2010) 585604

601

Figure 9. Large sparse environment; 35 landmarks, 240 m 240 m. Table 7. Summary of simulation results in the large environment Resampling RSR RBR Remarks RMSEP (m) 4.4857 1.9060 57% RMSEO (rad) 0.0696 0.0873 25% RMSEF (m) 4.4575 1.9306 57% No. distinct particles (%) 2.9 24.3 8.4%

less r.m.s. errors and about 8 times higher particle diversity than RSR, and RMSEO slightly increased. The degradation in RMSEO by 25% (=1.0115 ) is small compared to the improvement in RMSEP and RMSEF . In addition, the average NEES graphs of both RBR and RSR are presented in Fig. 10 to examine the consistency of RBPF-SLAM. The average NEES of RBPF-SLAM by RBR is similar to that in the smaller environment of Fig. 8. The RBPF-SLAM by RBR is pessimistically inconsistent during the SLAM process, but its SLAM estimation is much better than that of when RSR is used for the resampling process for RBPF-SLAM. 5. Conclusions RBPF has been employed in several robotic problems such as SLAM, thanks to the robust data association and the lower computational complexity. However, it suffers from the particle depletion problem, i.e., the number of distinct particles becomes smaller over the RBPF-SLAM process. As a result, the particle set to estimate the

602

N. Kwak et al. / Advanced Robotics 24 (2010) 585604

Figure 10. Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR. Also, in this environment, that by RBR is inconsistent in a pessimistic way and that by RSR is inconsistent in an optimistic way.

SLAM posterior becomes over-condent, which means it tends to underestimate its own uncertainty. Even though RBPF-SLAM, thanks to accurate sensors, is applicable to practical problems, it is desirable to guarantee its performance as long as possible. However, the standard resampling algorithms of RBPF-SLAM cannot preserve particle diversity after a large loop-closure. Thus, we analyzed on RBR, which assigns selection probabilities to resample particles based on the rankings of importance weights. The estimation capability of RBPF-SLAM by RBR outperformed that by RSR, which is the commonly used resampling algorithm. More specically, RBR preserves particle diversity much longer than RSR, so it can prevent some particles from dominating the particle set. Through our results, we conrmed that when particle diversity is preserved, the estimation performance is almost always better than the particle depletion case. In addition, through the consistency test, although RBPF-SLAM by RBR is not consistent, the average NEES of RBPF-SLAM by RBR is bounded during the RBPF-SLAM process, whereas that of RBPF-SLAM by RSR quickly diverges just after the loop-closure. Consequently, RBPF-SLAM by RBR can preserve particle diversity much longer than usual while reducing the estimation errors. Although RBR cannot guarantee the consistency of RBPF-SLAM, it can make the lter pessimistically inconsistent, which prevents the lter from diverging. Acknowledgements This work has been supported by a JSPS Postdoctoral Fellowship for Foreign Researchers (20-08610), and the Brain Korea 21 Project, Korea Science and Engineer-

N. Kwak et al. / Advanced Robotics 24 (2010) 585604

603

ing Foundation (KOSEF) NRL Program grant funded by the Korean government (MEST) (R0A-2008-000-20004-0), and the Growth Engine Technology Development Program funded by the Ministry of Knowledge Economy. References
1. K. Murphy, Bayesian Map Learning in Dynamic Environments (Advances in Neural Information Processing Systems). MIT Press, Cambridge, MA (1999). 2. A. Doucet, N. de Freitas, K. Murphy and S. Russell, RaoBlackwellized particle ltering for dynamic Bayesian networks, in: Proc. Conf. on Uncertainty in Articial Intelligence, Stanford, CA, pp. 176183 (2000). 3. M. Montemerlo, FastSLAM: a factored solution to the simultaneous localization and mapping problem with unknown data association, PhD Dissertation, Carnegie Mellon University (2003). 4. T. Bailey, J. Nieto and E. Nebot, Consistency of the FastSLAM algorithm, in: Proc. IEEE Int. Conf. on Robotics and Automation, Orlando, FL, pp. 424429 (2006). 5. R. Merwe, A. Doucet, N. de Freitas and E. Wan, The unscented particle lter, Technical Report CUED/F INFENG/TR380, Cambridge University Engineering Department (2000). 6. Y. Bar-Shalom, X. R. Li and T. Kirubarajan, Estimation with Applications to Tracking and Navigation. Wiley, New York, NY (2001). 7. K. R. Beevers and W. H. Huang, Fixed-lag sampling strategies for particle ltering SLAM, in: Proc. IEEE Int. Conf. on Robotics and Automation, Rome, pp. 24332438 (2007). 8. C. Stachniss, D. Hahnel, W. Burgard and G. Grisetti, On actively closing loops in grid-based FastSLAM, Adv. Robotics 19, 10591080 (2005). 9. N. Kwak, G. W. Kim and B. H. Lee, A new compensation technique based on analysis of resampling process in FastSLAM, Robotica 26, 205217 (2008). 10. N. Kwak, B. H. Lee and K. Yokoi, Result representation of RaoBlackwellized particle lter for Mobile Robot SLAM, J. Korea Robotics Soc. 3, 308314 (2008) (in Korean). 11. S. Thrun, W. Burgard and D. Fox, Probabilistic Robotics. MIT Press, Cambridge, MA (2005). 12. M. Boli , P. M. Djuric and S. J. Hong, Resampling algorithms for particle lters: a computational c complexity perspective, Eurasip J. Appl. Signal Process. 15, 22672277 (2004). 13. A. D. Anderson, Recovering sample diversity in RaoBlackwellized particle lters for simultaneous localization and mapping, Master Thesis, MIT, Cambridge, MA (2006). 14. D. E. Goldberg, Genetic Algorithms in Search, Optimization and Machine Learning. Kluwer, Boston, MA (1989). 15. D. Whitley, The GENITOR algorithm and selection pressure: why rank-based allocation of reproductive trials is best, in: Proc. 3rd Int. Conf. on Genetic Algorithms, Fairfax, pp. 116121 (1989). 16. K.-K. Jin, Genetic Algorithms and Their Applications. Kyo Woo Sa, Seoul (2002) (in Korean). 17. J. S. Liu and R. Chen, Sequential Monte-Carlo methods for dynamic systems, J. Am. Stat. Assoc. 93, 10321044 (1998). 18. G. Kitagawa, Monte-Carlo lter and smoother for non-Gaussian nonlinear state space models, J. Computat. Graph. Stat. 5, 125 (1996). 19. D. Crisan, P. Moral and T. Lyons, Discrete ltering using branching and interacting particle systems, Markov Process Related Fields 5, 293318 (1999). 20. A. Doucet, N. de Freitas and N. Gordon, Sequetial Monte-Carlo Methods in Practice. Springer, Berlin (2000). 21. J. S. Liu and R. Chen, Blind deconvolution via sequential imputations, J. Am. Stat. Ass. 90, 567576 (1995).

604

N. Kwak et al. / Advanced Robotics 24 (2010) 585604

About the Authors

Nosan Kwak received the PhD degree (MS and PhD integrated course) in Electrical Engineering and Computer Science from Seoul National University, South Korea, in 2008, and the BE in Mechanical Engineering from Ajou University, South Korea, in 2001. He is currently a Japan Society for the Promotion of Science (JSPS) Postdoctoral Fellow at Humanoid Research Group, National Institute of Advanced Industrial Science and Technology (AIST), at Tsukuba, Japan. He was a postdoctoral researcher at AIST until September 2008. He also belongs to the Joint Robotics Laboratory, where he researches Visual SLAM for a humanoid robots using particle lters. His major research elds are SLAM, particle ltering, game theory and altruistic behaviors. Overall, his goal in robotics is to develop pragmatic robots in order to use them in daily life. Kazuhito Yokoi received his BE degree in Mechanical Engineering from Nagoya Institute of Technology, in 1984, and the ME and PhD degrees in Mechanical Engineering Science from Tokyo Institute of Technology, in 1986 and 1994, respectively. In 1986, he joined the Mechanical Engineering Laboratory, Ministry of International Trade and Industry. He was the Co-director of the IS/AIST-CNRS Joint Robotics Laboratory. He is currently the Deputy Director of the Intelligent Systems Research Institute, National Institute of Advanced Industrial Science and Technology at Tsukuba, and is the Group Leader of the Humanoid Research Group. He is also an Adjunctive Professor of the Cooperative Graduate School at University, Tsukuba. From November 1994 to October 1995, he was a Visiting Scholar at the Robotics Laboratory, Computer Science Department, Stanford University. His research interests include humanoids, humancentered robotics and intelligent robot systems. He is a Member of the IEEE Robotics and Automation Society. Beom-Hee Lee received the BS and MS degrees in Electronics Engineering from Seoul National University in 1978 and 1980, respectively, and the PhD degree in Computer Information, and Control Engineering from the University of Michigan, Ann Arbor, MI, USA, in 1985. Since then, he was associated with the School of Electrical Engineering at Purdue University as an Assistant Professor, until 1987. He is now with the School of Electrical Engineering and Computer Science as a Professor, and with the Ofce of Information Systems as Dean at Seoul National University. His major research interests include motion planning and control of robot manipulators, multi-robot operation, sensor fusion applications, and factory automation. He has been the President of the Korea Robotics Society since December 2008.