Fuzzy Adaptive Particle Filter for Localization of a Mobile Robot ...

Fuzzy Adaptive Particle Filter for Localization of a Mobile Robot Young-Joong Kim, Chan-Hee Won, Jung-Min Pak, and Myo-Taeg Lim Department of Electrical Engineering, Korea University, 1, 5-ka, Anam-dong, Sungbuk-ku, Seoul 136-701, Korea {kyjoong,pucapuca,destin11,mlim}@korea.ac.kr http://cml.korea.ac.kr

Abstract. Localization is one of the important topics in robotics and it is essential to execute a mission. Most problems in the class of localization are due to uncertainties in the modeling and sensors. Therefore, various filters are developed to estimate the states in noisy information. Recently, particle filter is issued widely because it can be applied to a nonlinear model and a non-Gaussian noise. In this paper a fuzzy adaptive particle filter is proposed, whose basic idea is to generate samples at the high-likelihood using a fuzzy logic approach. The method brings out the improvement of an accuracy of estimation. In addition, this paper presents the localization method for a mobile robot with ultrasonic beacon systems. For comparison purposes, we test a conventional particle filter method and our proposed method. Experimental results show that the proposed method has better localization performance. Keywords: Localization, Mobile robot, Particle filter, Fuzzy logic.

1

Introduction

Localization of a mobile robot is to determine its position and heading with respect to known locations in the environment. It is one of the most important issues for mobile robot researches since it is essential to a mobile robot for long term reliable operations. The most common and basic method of performing localization is through dead-reckoning [1]. This approach integrates the velocity history of the robot over time to determine the change in position from the starting location. However dead-reckoning methods are prone to errors that grow without bound over time because effects of wheel slippage and an incomplete sensor reading cause the mobile robot’s intrinsic uncertainties. Other localization systems use beacons placed at known position in the environment [2]. They use ultrasonic pulses to determine a distance between robot and beacons, and estimate a position of a mobile robot. Although the methods have large noise randomly, they have advantage that error is not accumulated. Another method of localization is a laser system [3]. It is very precise, but considerably much more expensive than the ultrasonic sensor. B. Apolloni et al. (Eds.): KES 2007/ WIRN 2007, Part III, LNAI 4694, pp. 41–48, 2007. c Springer-Verlag Berlin Heidelberg 2007

42

Y.-J. Kim et al.

Kalman filter (KF) is used to eliminate system noise and measurement noise. KF is an estimator for linear-quadratic problem, which is the problem of estimating the instantaneous state of a linear dynamic system perturbed by white noise [4]. The resulting estimator is statistically optimal with respect to any quadratic function of estimation error. Many dynamic systems and sensor are not absolutely linear. In general, an extended Kalman filter (EKF) is used in nonlinear system. The main feature of EKF is that it linearizes the nonlinear functions in the state dynamic and measurement models. KF assumes that noise is Gaussian distribution. However, real data can be very complex, typically involving elements of non-Gaussianity, high dimensionality and nonlinearity. It should be noted that these preclude analytic solutions. The problem of KF was resolved by Sequential Monte Carlo (SMC) methods. The SMC methods are a set of simulation-based methods which provide a convenient and attractive approach to compute the posterior distributions. Particle filter (PF) is one of SMC methods. They perform sequential Monte Carlo estimation based on samples with associated weight representing probability densities. These methods have the great advantage of nonlinearity or non-gaussianity on the model and in addition they also have good convergence properties. However, the time complexity of PF is linear in the number of samples needed for the estimation. Therefore, several attempts have been done to increase the efficiency of particle filters by adapting the number of samples with respect to the underlying state uncertainty. The method is called an adaptive particle filter (APF) that is useful for reduction of operation time [5]. In this paper, we propose the fuzzy adaptive particle filter (FAPF) by using the well-known fuzzy logic approach [6,7], in order to lower computation costs and high accuracy of estimations. In addition, it is applied to localization of a mobile robot in real time. The contents of this paper are as follows. In section 2, we demonstrate existing PF. Section 3 describes the localization algorithm using the proposed FAPF. The experimental results are described in section 4. Finally, section 5 gives our concluding remarks.

2

Particle Filter

Particle filter represents the required posterior density function p(x0:k |y1:k ) by a set of random samples with associated weights. These samples are called “particles”. The probability assigned to each particle is proportional to the weight s [5,8]. In order to present the details of the algorithm, let {xi0:k , wki }N i=1 denote samples that characterize the posterior probability density distribution, p(x0:k |y1:k ). {xi0:k , i = 1, · · · , N } is a set of particles with their associated weights {wki , i = 1, · · · , N }. The weights are normalized that is i wki = 1. N is the number of samples used in the approximation. The posterior probability density distributions can be approximated as N p(x0:k |y1:k ) ≈ wki δ(x0:k − xi0:k ) (1) 1

where δ(·) is the Dirac delta function.

Fuzzy Adaptive Particle Filter for Localization

43

The normalized importance weights wki are chosen using the principle of importance sampling. If samples xi0:k were drawn from an importance density q(x0:k |y1:k ) by the principle of importance sampling, wki is given by wi wki = Nk i 1 wk

(2)

where wki ∝

p(x0:k |z0:k ) . q(x0:k |z0:k )

(3)

By Bayesian theorem [5], the posterior density function is expressed as follow: p(x0:k |y1:k ) ∝ p(yk |xk )p(xk |xk−1 )p(x0:k−1 |y1:k−1 )

(4)

If the importance density is chosen to factorize such that q(x0:k |y1:k ) ≡ q(xk |x0:k−1 , yk )q(x0:k−1 |y1:k−1 ),

(5)

then one can obtain samples xio:k ∼ q(x0:k |y1:k ) by augmenting each of the existing samples xio:k−1 ∼ q(x0:k−1 |y1:k−1 ) with the new state xik ∼ q(xk |x0:k−1 , y0:k ). By substituting (4) and (5) into (3), the weight update equation can then be shown to be wki ∝

i ) p(yk |xik )p(xik |xik−1 )p(xi0:k−1 |y1:k−1 i i i i q(xk |x0:k−1 , y1:k )q(x0:k−1 |y1:k−1 )

i = wk−1

p(zk |xik )p(xik |xik−1 ) q(xik |xi0:k−1 , y1:k ).

(6)

The choice of proposal distribution q(xik |xi0:k−1 , y1:k ) is one of the most critical design issues for successful particle filter. Two of those critical reasons are as follows: samples are drawn from the proposal distribution, and the proposal distribution is used to evaluate important weights. Accordingly, the support of proposal distribution must include the support of true posterior distribution. In addition, it must include most recent observations. The following equation is chosen to minimize variance of noise. q(xk |x0:k−1 , y0:k ) = p(xk |x0:k−1 , y0:k )

(7)

Although it does not include most recent observation (because it is easy to implement) the most common choice is the prior distribution as follows: q(xk |x0:k−1 , y0:k ) = p(xk |xk−1 )

(8)

By substituting (7) and (8) into (6), the weight update equation is simplified as follow: i wki ∝ wk−1

p(zk |xik )p(xik |xik−1 ) i p(zk |xik ). = wk−1 q(xik |xi0:k−1 , y0:k )

(9)

44

Y.-J. Kim et al.

A particle filter described above is called the Sequential Importance Sampling (SIS). SIS has a problem that is the discrete random measure degenerate quickly. In practical terms this means that after a certain number of recursive steps, most particles will have negligible. Degeneracy can be reduced by using resampling step [9]. Resampling is a scheme to eliminate particles small weights and to concentrate and replicate on particles with large weights. It involves a mapping of random measure with uniform weights.

3

Fuzzy Adaptive Particle Filter

The number of samples is adapted by state uncertainty in APF. A sum of weights is a measure of uncertainty in likelihood-based adaptation. For the case class of reduction of samples, they are generated until the sum of non-normalized likelihood exceeds a pre-specified threshold. If all sampled particles have low likelihood scores, a sum total can not exceed a threshold. When samples are increasing, they are generated randomly. New samples have low weights because of a large state uncertainty. Therefore, many new samples are generated for unsatisfying a condition. This is not an efficient method. Therefore we propose the fuzzy adaptive particle filter for effective sampling by taking advantage of current observation and using the well-known fuzzy logic approach. The basic idea essentially calculates the gradient information from the fuzzy logic approach using the probability density function of observation and generates new samples toward the high likelihood region, along the gradient-descent direction. In this paper, we propose a simple gradient information of probability density function as follows: Δq = q(xk ) − q(xk−1 ).

(10)

Now a new sample xi+N is proposed using fuzzy logic approach and the heuristic k formula is as follows: xi+N = xik + γS, k

(11)

where S is a step size parameter, N is the number of sample of particle filter at time k. γ is the output variable of the following fuzzy IF-THEN rules: Rule(1) : IF α1 is S and α2 is N, T HEN γ is S Rule(2) : IF α1 is S and α2 is Z, T HEN γ is S Rule(3) : IF α1 is S and α2 is P, T HEN γ is M Rule(4) : IF α1 is M and α2 is N, T HEN γ is S Rule(5) : IF α1 is M and α2 is Z, T HEN γ is M (6)

Rule : IF α1 is M and α2 is P, T HEN γ is L Rule(7) : IF α1 is L and α2 is N, T HEN γ is M Rule(8) : IF α1 is L and α2 is Z, T HEN γ is L Rule(9) : IF α1 is L and α2 is P, T HEN γ is L

(12)


45

where α1 is an input variable as the value of a proposal distribution q(xk ), α2 is an input variable as the gradient information Δq, the linguistic variables S, M , L, N , Z, and P mean “small”, “medium”, “large”, “negative”, “zero”, and “positive”, respectively. Moreover, each proposed membership function is presented in Fig. 1.

Fig. 1. (i) The value of a proposal distribution, α1 , as a linguistic variable that can take fuzzy sets “small”, “medium”, and “large” as q(xk ) values in the left plot. (ii) The proposed gradient information, α2 , as a linguistic variable that can take fuzzy sets “negative”, “zero”, and “positive” as Δq values in the center plot. (iii) The output of fuzzy rules, γ, as a linguistic variable that can take fuzzy sets as [0, 1] values in the right plot.

Specifically, in a ultrasonic beacon system, localization of a mobile robot is realized as the proposed fuzzy adaptive particle filter adopted by each sample sketched in Algorithm 1. Algorithm 1: Localization using the fuzzy adaptive particle filter Step 1. Initialization - Set up the number of sample as a maximum value. - Randomly generate an initial pose of a mobile robot. Step 2. Prediction - Each pose is passed through the system model to obtain samples from the prior. Step 3. Update - Evaluate the likelihood of each prior sample and compare sum of likelihood to threshold. - If a sum total is smaller than threshold, generate new samples using (11). Step 4. Resampling - Multiply/Suppress samples with high/low importance weights. - Obtain N random samples. Step 5. Final - Increase time k and iterate from Step 2 to Step 4.

46

4

Y.-J. Kim et al.

Experimental Evaluation of the Fuzzy Adaptive Particle Filter

The operation time of PF is important for localization in real time. It is related with the number of samples closely. First, we made an experiment on localization of mobile robot with PF with fixed samples so as to know relation between samples and operation time in ultrasonic beacon system. We set up maximum samples of FAPF using the results. Then it is carried out an experiment on localization with FAPF. Finally, experimental results prove an efficiency of new algorithm. In this experiment, a mobile robot travel with a constant speed 30cm/s, angle variation Δθ = 5◦ . It is assumed that process and measurement noise are gaussian for a test convenience. Each covariance of noise is set up through test as follows: ⎤ ⎡ 2 5 0 0 Pwk = ⎣ 052 0 0 ⎦ , pvk = 22 . (13) π 0 0 180 The listener is mounted above the center of the mobile robots. Beacons are attached on the ceiling. The number of beacons is adjusted to size of location space. An interval of beacons is limited because listener must communicate beacons. The distance is affected from performance of sensor. We set it 300cm. We have the comparative test on localization using PF and the proposed FAPF. Experimental results are showed in Fig. 2 and 3. Fig. 2 show the samples

Fig. 2. (i) The left plot is the samples generated using PF. (ii) The right plot is the samples generated using FAPF.

generated, where ◦ represent the true motion of a mobile robot. The left and right ∗ represent the samples generated using PF and FAPF, respectively. Fig. 3 show the results of localization. In the left plot, real line is the trajectory of the true mobile robot, dotted line is the trajectory of estimation using PF, and dashed line is the trajectory of estimation using FAPF. In the right plot, dotted line is

800

40

600

30

400 TRUE PF FAPF

200 100

200

300 400 [cm]

500

600

RMSE [cm]

[cm]


47

PF FAPF

20 10 0 0

20 40 Time [sec]

60

Fig. 3. (i) The left plot is the trajectories of a mobile robot. (ii) The right plot is RMSE between the true mobile robot and estimations.

the Root Mean Square Error (RMSE) of estimation using PF, and dashed line is the RMSE of estimation using FAPF. We can confirm that localization using the proposed FAPF with likelihood gradient is more accurate than PF. The RMSE of PF is 8.7067cm and RMSE of FAPF is 6.0269cm. Results of FAPF is more accurate than PF with 5000 samples. Therefore, we can say that the proposed method can reduce wasted samples and it provide the better performance.

5

Conclusion

In this paper, we presented the fuzzy adaptive particle filter using well-known fuzzy logic approach. The algorithm generates new samples at high likelihood region as uncertainty increases. Based on the proposed method, it is more accurate and lower computational cost. The method allows seriously reducing the number of required samples. The approach has been implemented and evaluated on localization of mobile robot in ultrasonic beacon system. Results performed with our algorithm have demonstrated its robustness and the ability of generating exact estimation through comparing the particle filter. Acknowledgement. The authors would like to acknowledge Ministry of Construction and Transportation, Korea. This work is supported by 2006 High-Tech Fusion Construction Technology Development Program [06 High-Tech Fusion D01].

References 1. Borenstein, J., Everett, R., Feng, L., Whehe, D.: Mobile robot positioning: Sensors and techniques. J. Robot. Syst. 14, 231–249 (1997) 2. Kleeman, L.: Ultrasonic autonomous robot localization system. In: IEEE International conference Intelligent Robots and Systems, Tsukuba, JAPAN, pp. 212–219. IEEE, Los Alamitos (1989)

48

Y.-J. Kim et al.

3. Diosi, A., Taylor, G., Kleeman, L.: Interactive SLAM using Laser and Advanced Sonar. In: ICRA, Proceedings of the 2005 IEEE International Conference, pp. 1103– 1108. IEEE Computer Society Press, Los Alamitos (2005) 4. Grewal, M.S., Andrews, A.P.: Theory and Practice Using MATLAB. WieleyInterscience, Canada (2001) 5. Doucet, A., de Freitas, N., Gordon, N.: Sequential Monte Carlo Methods in Practice. Springer, Heidelberg (2001) 6. Sugeno, M., Nishida, M.: Fuzzy control of model car. Fuzzy Sets and Systems, 103– 113 (1985) 7. Wang, L.X.: A Course in Fuzzy Systems and Control. Prentice-Hall, Englewood Cliffs (1997) 8. Ristic, B., Arulampalam, S., Gordon, N.: Beyond the kalman filter. Artech House Publishers, Boston (2004) 9. Fox, E., Burgard, W., Dellaert, F., Thrun, S.: Monte Carlo Localization: Efficient position estimation for mobile robots. In: Proc. Of the National Conference on Artifical Intelligence (1999)