Proceedings of the 2010 IEEE International Conference on Information and Automation June 20 - 23, Harbin, China
Monte Carlo Localization for Mobile Robot Using Adaptive Particle Merging and Splitting Technique Tiancheng Li, Shudong Sun and Jun Duan Department of Mechatronic Northwestern Polytechnical University Xi’an, Shanxi Province, China
[email protected] Abstract - Monte Carlo localization (MCL) is a success application of particle filter (PF) to mobile robot localization. In this paper, an adaptive approach of MCL to increase the efficiency of filtering by adapting the sample size during the estimation process is described. The adaptive approach adopts an approximation technique of particle merging and splitting (PM&S) according to the spatial similarity of particles. In which, particles are merged by their weight based on the discrete partition of the running space of mobile robot. Using the PM&S technique, a Merge Monte Carlo localization (Merge–MCL) method is detailed. Simulation results illustrate that the approach is efficient.
depends on the number of particles used for estimation. Unfortunately, most existing approaches to MCL use a fixed number of particles during the entire state estimation process. This can be highly inefficient, since the complexity of the probability densities can vary drastically over time [4]. Several attempts have been made to make more effective use of the available samples, thereby allowing sample sets of reasonable size. One of the most elegant methods for adapting the number of particles is KLD-sampling approach developed by Fox, et al. The key idea of KLD-sampling method is to determine the number of samples based on statistical bounds on the sample-based approximation quality [4]. Based on the KLD-sampling method, Kwok represent posteriors as mixtures of sample sets, focuses computational resources (samples) on valuable sensor information [5]. Soto presents a revised bound for KLD-sampling based on the variance of importance sampling [6]. Further on, Liu presents an adaptive dynamic clustered particle filter based on particle cluster [7]. However, the problem with KLD-sampling is the derivation of the bound of sample size using the empirical distribution, which has the implicit assumption that the samples come from the true posterior distribution. This is not the case for particle filters where the samples come from an importance function. There are also some other improved approaches to decrease the number of particles needed to increase the efficiency of filtering [8]. Liu proposed the concept of characteristic particle in solving the multi-robot localization problem, which reduces the computing time greatly [9]. In our previous work [10], a grid set based particle merging resampling method is proposed, which can efficiently adapts the sample size according to the spatial distribution of particles. The work presented in this paper is an extension of our previous work as it proposes another implement of particle merging and further adds a particle splitting method. All of these works are focused on the rational distribution of spatial particles, which is a meaningful work for robot localization. Aimed at improving the computational efficiency of MCL, a particle merging and splitting technique (PM&S) is detailed in the paper. The technique adapts the number of particles according to not only weights of particles but also their spatial similarity. Executing this technique before the weights computing step to reduce the number of particles for updating, an adaptive approach of Monte Carlo localization, named Merge–MCL, is proposed. Simulation results illustrate that the approach is efficient.
Index Terms - Monte Carlo localization. Particle filter. Merging. Splitting.
I. INTRODUCTION As a key problem in the field of mobile robot, localization is the optimal estimation of the location and orientation of a mobile robot using a sequence of noisy measurements made on the environment [1]. Mobile robot localization is ideally suited for the Bayesian approach [2], which attempts to construct the posterior probability density function of the state based on all available information. In principle, an optimal estimate of the state may be obtained from the posterior probability density function. However, we need to deal with infinite-dimensional integral operator, even sometimes there may be no analytical solution in nonlinear and non-Gaussian system, and such is the mobile robot localization problem. In fact, the posterior distribution only admits an analytical expression for few special models, including linear Gaussian state-space models (Kalman filter) and finite state space hidden Markov models (HMM filters) [3]. Particle filter utilizes a large number of random samples (also called particles) to represent the posterior probability distributions. The samples are propagated over time using a combination of sequential importance sampling and resampling steps. These methods are very flexible and can be easily applied to nonlinear and non-Gaussian dynamic models [3]. Monte Carlo localization (MCL) is a success application of particle filter to mobile robot localization, which represents the required localization by a set of random particles with associated weights and computes estimate based on these particles and weights [1]. It is due to this representation that MCL combine efficiency with the ability to represent a wide range of probability densities. The complexity of MCL
978-1-4244-5704-5/10/$26.00 ©2010 IEEE
1913
The remainder of this paper is organized as follows: In section II, we detail the adaptive particle merging and splitting technique (PM&S), which the core contribution of this paper. Section III introduces its application to mobile robot localization, i.e., Merge-MCL. Simulation results are presented in Section IV before we conclude in Section V. II. ADAPTIVE PARTICLE MERGING AND SPLITTING TECHNIQUE In the framework of MCL, The posterior probability of particle denotes the possibility that the robot is located at a certain position [11]. If particles are close to each other, the differences of their state estimation will be very small. Considering the spatial rationality of particles, we assume that they are in the same position when the distance between these particles is less than the sensor reading error. Based on this assumption, we adopt a technique of adaptive particle merging and splitting (PM&S) to adapt the number of particles, in which particles will be merged or splitted by their weights and spatial distribution.
Fig. 1 Particle merging based on grid cells.
Since some particles may have relatively large weights, the problem of weight degeneracy may occur in the process of particle merging. To avoid that, we need to control the increasing of the weights of particles. One of the solutions is density-based clustering [12]. Particles are merged adaptively according to their distribution density, which is ideally agreed with the spatial rationality of particles. On the other hand, particles can be merged in variable precision grids [13]. In which, the size of grid cells can be decided adaptively according to the density of particles. In our approach, a particle splitting method is used to control the increasing of the weights of particles, especially the merge particle with large weight, which is much more flexible and can be more easily applied than the clustering algorithms and variable precision grids approach.
A. Discrete Grid Cells Usually, the robot localization system can be assumed as following: 1) Robot runs on a two-dimensional platform, which can be described in grid cells. 2) Robot is equipped with sensors, such as optical encoder, camera or ranger, to get odometry readings and real-time measurements from the environment. Besides, the sensors can provide almost accurate heading direction. Based on these assumptions, we decompose the state space of robot localization into discrete grid cells. The size of grid cells is defined as L =e/2 2 (1) Where L is the length of the cell, e denotes the maximum error of sensor reading. The PM&S technique is implemented based on this division of grid cells.
C. Particle Splitting In our approach, we split the particle with a larger weight than a set threshold by ° xki ,t = xtk k k i i ( xt , wt ) ( xk ,t , wk ,t ) i = 1, 2,...nk : ® i . (3) k °¯ wk ,t = wt / nk k k Where (x t, w t) represents the merge particle with a larger weight than a predetermined value and nk means that particle has been divided or splitted by nk times equally. The process of particle splitting can be described as shown in Fig. 2. Particle splitting in (3) divide merge particles equally without changing their position. So it will not influence the distribution of particles. Actually, this effect of particle splitting is a little like resampling.
B. particle merging Suppose the whole state space is partitioned into K grid cells. The number of particles in kth grid cell is denoted as Nk, k=1, 2, … , Kt. Bk, t denotes the weight of kth grid cell Ck, t at time t, which can be considered as the probability that the kth grid cell Ck, t contains the actual robot position. Then the kth i i grid cell can be described as (Ck, t, Bk, t)={(x k, t, w k, t)| i=1,2,…,Nk}. Based on our previous assumption, particles in kth grid cell can be merged by k Nk i i ° xt = ¦ xk ,t wk ,t / Bk ,t ° i =1 . (2) (Ck ,t , Bk ,t ) ( xtk , wtk ) : ® Nk ° wk = wi = B k ,t k ,t °¯ t ¦ i =1 k k k Where (x t, w t) represents the merge particle of kth grid cell, x t k is the location of kth merge particle and w t is its weight. It’s obvious that the number of particle will be great reduced by using (2). Replacing the particles before merged, these novel merge particles associated with weights will be used to represent the posterior probability. The process of particle merging can be described as shown in Fig. 1, in which the sizes of the circles (particles) represent their weights.
Fig. 2 Particle splitting based on grid cells.
D. Posterior Probability Distribution Let xt. denotes the robot’s state at time instant t. zt is the perceptual data (observation) at time t, and ut is the odometry data (control measurement) between time t-1 and t.
1914
Additionally, define Z0:t={z0, z1, …, zt}the set of all observations up to time t and U0:t-1={ u0, u1, …, ut-1}the history of control inputs. Then the posterior probability distribution will be p( xt U 0:t −1 , Z 0:t ) = ¦ wti δ ( xt − xti ) . (4)
Different from [10], we execute the particle merging and particle splitting operation separately between the two steps, named merge Monte Carlo localization (Merge-MCL). The scheme can be detailed in Algorithm 1.
What do we care most about PM&S is its influence on the posterior probability distribution of particles. For that, there is an important theorem as following. Theorem 1. PM&S will not change the mean of the distribution of particles, but reduce the variance. Proof. The mean of particles before PM&S is
Algorithm 3: Merge-MCL Input: St −1 = {( xti−1 , wti−1 )}iN=t1−1 , particle set ut-1, control measurement zt, observation L, grid size ߙ, the threshold value of weight Output: St = {( xtj , wtj )}Nj =t 1 , the new particle set
i
K
K
Nk
e = ¦ Ck ,t Bk ,t = ¦¦ xki ,t wki ,t , k =1
(5)
k =1 i =1
and the variance of particles is Nk
K
δ = ¦¦ ( xki ,t − e)2 wki ,t .
(6)
Procedure: St = {} , St = {} for p = 1: Nt-1 do draw xˆt ~ p( xt xt −1 , ut −1 )
k =1 i =1
If particles are merged and splitted by (2) and (3), the mean of particles will not change, since K
e′ = ¦ xtk wtk k =1 K
.
Nk
i = round(x/L) j = round(y/L) xˆtp = ( xˆtp , i, j)
(7)
= ¦¦ xki ,t wki ,t = e k =1 i =1
The variance of particles can be derived by following K
K
Nk
k =1
i =1
δ ′ = ¦ ( xtk − e′) 2 wtk = ¦ (¦ xki ,t wki ,t / Bki ,t − e) 2 Bki ,t k =1 K
Nk
Nk
k =1
i =1
i =1
= ¦ [(¦ xki ,t wki ,t / Bki ,t ) 2 − 2e(¦ xki ,t wki ,t / Bki ,t ) + e 2 ]Bki ,t . (8) K
Nk
≤ ¦¦ ( xki ,t − e)2 wki ,t = δ k =1 j =1
(Ci, j, Bi, j) = (Ci, j, Bi, j) {xˆtp ǡ wtp } end for for all (Ci, j, Bi, j) do tk ) via (2) (Ci , j , Bi , j ) ( xtk , w St = St * ( xtk , w tk ) end for for all ( xti , w ti ) ∈ St do update w ti ∝ p ( zt xti ) × w ti
Cauchy-Schwarz inequality has been used in (8). It is only when grids are divided so small that there is no more than one particle in each grid that the equal of the inequality appears. Theorem 1 illustrates that the distribution of particles after PM&S is an unbiased consistent estimation with less variance of that before PM&S. Thus, Merge particles are a good and efficient estimation for filtering. It is obvious that the PM&S approach can efficiently reduce the number of particles, which agrees with the spatial rationality of particles. Furthermore, the PM&S method will not discard any particle even with small weight, which can avoid the problem of sample impoverishment.
tk >ߙ if w tk ) ( xki ,t , wki ,t ) i = 1, 2,...nk via (3) ( xtk , w
St = St * {( xki ,t , wki ,t )}i else St = St * ( xtk , w tk ) end if end for St : ( xtj , wtj ) j = 1, 2,..., N t for all ( xtj , wtj ) ∈ St do
III. MERGE MONTE CARLO LOCALIZATION In typical Bayesian estimation framework, there are two basic stages: prediction and updating. For mobile robot Monte Carlo localization, they are the movement model and the perceptual model. Bayesian filters assume that the environment is Markov [1], and then the two models can be described by p( xt z t −1 , ut −1 ) = p( xt xt −1 , ut −1 ) × p( xt −1 zt −1 , ut − 2 ) , (9) and p( xt z t , ut −1 ) = p( zt xt ) p ( xt zt −1 , ut −1 ) / p( zt zt −1 , ut −1 ) . (10)
1915
normalize wtj = wtj [¦ wtj ]−1 end for
IV. SIMULATIONS RESULTS A. PM&S There are 1000 raw particles as shown in Fig. 3. The distribution of particles is shown in the 2-dimensional space (the scatter graph) and separately on the x-dimension and y-
dimension (two statistical graphs). There is also a threedimensional view in which the vertical axis represents the weight of particles. The weights of raw particles before PM&S are equally are set to 1. After PM&S, there are only 701 particles as shown in Fig. 4 and their weights are unequal as shown in the three-dimensional view. Comparison results of distributions of particles before and after PM&S are given in Table I. The results are consisted with our theorem 1, which illustrates that our PM&S approach can retain the overall permanence of particles without losing of variety.
Where et and vt are zero mean Gaussian random variables with variance 10 and 1. There are 100 initial particles at first. The true state xt is one-dimension, so our PM&S will be implemented in one-dimension grid cells. We separately choose L=0.1 and L=0.2 and use root mean square error (RMSE) to evaluate the estimate accuracy 1 NT (13) RMSE( x t ) = [ ¦ ( xt − x t )2 ]1/ 2 . NT t =1 Where NT is the number of iterations (time steps) and we choose NT=100 in our instance. Fig. 5 and Table II show the simulation results and Fig. 6 shows the volatility of sample size in filtering. From table I, we can see the number of particles is fixed in basic particle filter, but adaptively reduces according to the size of grid cells in Merge-PF. Therefore, the computational efficiency of filtering is improved in Merge-PF. However, the estimate accuracy is a little reduced because of the approximation error. But the estimation error increased by PM&S will never bigger than the approximation error in our approach. So, with a good choose of the size of grid (L), the PM&S can ensure the accuracy of estimation.
Fig. 3 Distribution of raw particles before merged.
Fig. 5 Simulation results of particle filters.
Fig. 4 Distribution of particles after merged with L=1. TABLE I COMPARISON OF DISTRIBUTIONS OF PARTICLES BEFORE AND AFTER PM&S Mean Variance Distribution of Number of Particles Particle x y x y Before PM&S 1000 55.965 50.804 10.651 10.015 After PM&S 701 55.965 50.804 10.648 10.012
B. Merge Particle Filter In order to evaluate the efficiency of the PM&S technique, we design a merge particle filter (Merge-PF) using adaptive particle merging and splitting technique. For a comparison with basic SIR particle filter based on a standard PC with a 2.0 GHz processor, we use the following typical prediction model xt = xt −1 / 2 + 25 xt −1 / (1 + xt2−1 ) + 8 cos(1.2t ) + et , (11) and updating model ot = xt2 / 20 + vt . (12)
Fig. 6 Number of particles in filtering. TABLE Ċ PARTICLE FILTER PERFORMANCE REFERENCE TABLE Number of RMSE Running time/s Filters particles SIR-PF 100 5.0254 1.554 Merge-PF 0.1 73.48 5.2248 1.335 Merge-PF 0.2 56.69 5.7173 0.846
C. Indoor Mobile Robot Localization
1916
In our third set of simulations, we evaluated Merge-MCL in an indoor simulation environment as shown in Fig. 7, a simplified model of the 3rd floor of Innovation Centre of Northwestern Polytechnical University (NPU), which is also used in [10]. Fig. 7 also shows the robot route. In our simulation, the robot collects data using a laser ranger. The measurement error and the movement error are mixture-Gauss set to be 40cm/3m with probability 0.95. In addition, we decompress the environment into grids with the size of L=3 pixel (approximately 9.4cm in the actual environment). In our instance, we evaluated Merge-MCL by comparing it with the fixed-sampling MCL and KLD-sampling MCL in 2 [4]. For KLD-sampling, the bin size is chose 25ൈ25cm ൈ5ι, and the error bound is set 0.05.There are 1000 initial random particles for these three algorithms, and our robot has realized localization 19 times. Specially, the robot is terribly disturbed in the corridor between the 7th and 8th iteration. Fig. 8 shows that, the number of particles in the basic fixed-sampling MCL is fixed, but reduced to about 200 in the KLD-sampling MCL and about 65-110 in our approach of Merge-MCL. Compared with the other two algorithms, the efficiency of Merge-MCL is the best. Further on, our approach recover most quickly from “interfered”, while uses lest iterations and particles. Fig. 9-10 show the distribution of merge particles in the localization process of our Merge-MCL. As is shown, particles are sparsely distributed in grids. That is because our PM&S operation is implemented based on discrete state space division. The discrete points represent particles, the rectangle frame “•” represents the real location of robot and the cross “+” is the centre of particles (points), as the estimate of the robot location in our instance.
Fig. 9 Merge particles in the localization process (Step5).
Fig. 10 Merge particles in the localization process (Step19).
V. CONCLUSION In this paper, we presented an adaptive particle merging and splitting technique to adapt the sample size of particle filters, according to not only the weights of particles but also their spatial similarity. Using the PM&S technique, a MergePF method for mobile robot localization is detailed. As the core contribution of our approach, the PM&S try to increase the computational efficiency of filtering without seriously reducing the estimation accuracy. Simulation results illustrate that PM&S can adapt the sample size effectively and the Merge-MCL approach is efficient. As we have conjectured, the spatial distribution of particles is an important reference to develop novel sampling and resampling methodologies for particle filters, which is meaningful for robot localization, and also is many other applications of particle filters. Further study on more efficient implement of particle merging and space sampling and its theoretical analysis is still needed. Furthermore, our approach can be combined with many other methods for improving the performance of particle filters, such as KLD-sampling, auxiliary particle filter.
Fig. 7 Path of robot in the simulation.
ACKNOWLEDGMENT This research is partly supported by graduate starting seed fund of Northwest Polytechnical University (Z200922). Tiancheng would like to thank Qian Jia.
Fig. 8 Number of particles in the localization process.
1917
Proceedings of the 2007 IEEE International Conference on Intelligent Robots and Systems, 2007, pp. 1059-1064. [8] H. Q. Min, H. Chen, R. H. Luo, “Active particle in MCL: an evolutionary view”, In Proceedings of the IEEE International Conference on Information and Automation, 2009, pp.1087~1092. [9] J. C. Liu, K. Yuan, Q. M. Yang, “Monte Carlo Multi-Robot Localization Based on Grid Cells and Characteristic particle”, In International Conference on Advanced Intelligent Mechatronics, 2005, pp. 510-515. [10]T. C. Li, S. D. Sun, S. B. Si, J. Q. Wang, “Particle Merging Resampling Based Monte Carlo Localization for Mobile Robot”, Robot, in press. (in Chinese) [11]A. Milstein, J. Z. Sanchez, E. Williamson, “Robust global localization using clustered particle filtering”, In Proceedings of National Conference on Artificial Intelligence, 2002, pp. 581-586. [12]J. G. Sun, J. Liu, L. Y. Zhao, “Clustering Algorithms Research”, Journal of Software, vol. 19, no. 1, pp. 48-61, 2008.(in Chinese) [13]T. C. Li, S. D. Sun, Y. G, “Localization of Mobile Robot Using Discrete Space Particle Filter”, Chinese Journal of Mechanical Engineering, in press. (in Chinese)
REFERENCES [1] S. Thrun, D. Fox, W. Burgard, F. Dellaert, “Robust Monte Carlo localization for mobile robots”, Artificial Intelligence, vol. 128, no. 1-2, pp. 99-141, 2001. [2] Y. Bar-Shalom, X. R. Li, T. Kirubarajan, Estimation with applications to tracking and navigation: theory, algorithms, and software, New York: John Wiley & Sons, 2001. [3] D. Crisan, A. Doucet, “A survey of convergence results on particle filtering methods for practitioners”, IEEE Trans. Signal Processing, Vol. 50, no. 3, pp. 736-746, 2002. [4] D. Fox, “Adapting the sample size in particle filters through KLDsampling”, The International Journal of Robotics Research, vol. 22, no. 12, pp. 985-1003, 2003. [5] C. Kwok, D. Fox, Meil, “Real-time particle filters”, In Proc. IEEE, Vol. 92, no. 3, 2004, pp. 469-484. [6] A. Soto, “Self adaptive Particle Filter”, In Proceedings of International Joint Conferences on Artificial Intelligence, 2005, pp.1398-1406. [7] Z. B. Liu, Z. Y. Shi, M. G. Zhao, W. L. Xu, “Mobile robots global localization using adaptive dynamic clustered particle filters”, In
1918
