2017 2nd International Conference on Computer Science and Technology (CST 2017) ISBN: 978-1-60595-461-5
Mobile Robot Localization Using an Evolutionary Particle Filter
Yu-Ting SUN
1and Xiao-Lin LUAN
2,a*1 Institute of Oceanographic Instrumentation, Shandong Academy of Sciences,
NO.37 Miaoling Road, Qingdao, China
2 Shandong Foreign Trade Vocational College, Qingdao, China
*Corresponding author
Keywords: Robot localization, Particle filter, Sample impoverishment, Evolutionary computation
Abstract. This paper presents an evolutionary particle filter method for mobile robot localization. Particle filter suffers from the impoverishment problem and needs a large number of particles to represent the system posterior probability density function (PDF). In order to improve the performance of PF, the Selection, Crossover and Mutation operations in evolutionary computation are introduced into PF to optimize the samples and make them move towards regions with large value of PDF, as well as improve the diversity of samples and reduce sample impoverishment phenomenon. Simulation results show that, when compared with PF, the evolutionary particle filter algorithm needs fewer particles and is more precise and robust for mobile robot localization.
Introduction
Another problem is sample impoverishment. Due to the limited number of samples in the practical application, the number of different samples in a certain number of cycles will be reduced sharply, which result in a serious dilution of the sample support set and cause the degradation of samples [4].Now researchers have made some improvements to the ordinary PF algorithm, such as resampling mobile algorithm [5], simulated annealing particle filter algorithm[6] and KLD adaptive sampling algorithm [7].
This paper proposed an evolutionary particle filter localization method. The evolution mechanism is introduced into the PF algorithm, the samples are optimized in genetic algorithm selection, crossover and mutation procedures, which makes the sample distribution moves backward to the posterior density area with larger values. All these improvements will get a better expression of the state of the robot, so as to effectively reduce the number of required samples. At the same time, it will improve the diversity of samples and reduce the sample dilution. The algorithm is implemented to the position tracking and global localization of mobile robot. The simulation results show the effectiveness of the algorithm.
Robot Localization
The localization of mobile robot can be described as: the current state of the system is estimated according to the initial state probability distributionp(x0) and the observed data Z { t|t 0,1, ,t}
t
=
= z in the dynamic system composed of the robot and the environment.
T t t t t =(x ,y ,θ )
x represents the robot pose (position and orientation). From the perspective of Statistics, the estimation of xtis a Bayesian filtering problem, which can
be realized by estimating the posterior density distribution ( | t)
t Z
p x .
According to the Markov hypothesis and Bayesian rule, we can solve the problem in 2 steps:
(1) Prediction stage: the state of the system at the next moment is predicted by the motion model, the probability density ( | t−1)
t Z
px of the system is calculated by formula
(1): 1 1 1 1 1
1) ( | , ) ( | )
|
( − =
− − − t− t−t t
t t t
t Z p p Z d
p x x x u x x (1)
) , |
( t t−1 t−1
p x x u is the motion model of the system (state transition prior density).
(2) Update stage: the state of the system is updated by the observation model, and the posterior probability density ( | t)
t Z
p x of the system is calculated by formula
) | ( ) | ( ) | ( ) | ( 1 1 − − = t t t t t t t t Z p Z p p Z p z x x z
x (2)
) | ( t t
p z x is the observation model (observed density). ( | t−1) t Z
p z is constant. Under
the condition that the current state is known, the past measurement is independent with the current measurement.
The above two improved steps are given to solve the robot localization problem. However, in order to obtain the analytic representation of the posterior density distribution ( | t)
t Z
px of the system, the integral in the formula (1) must be calculated,
situation, an approximate method can be used to solve the problem. PF algorithm provides an effective method to solve this kind of problem [8].
Particle Filter Localization Algorithm
The PF algorithm is defined as the Monte Carlo method (Monte Carlo methods) by Arulampalam et al, which can be used in any state space model, which generalizes the traditional EKF positioning method.
PF is a variant of Bayesian filtering algorithm; the main idea is to use weights of discrete samplesSt ={(xtj,wtj)|j=1,,N} to express the posterior probability density
) |
( t
t Z
p x :
) ( )
| (
1
j t t N
i j t t
t Z w x x
p =
−= δ
x (3)
) (j t
x is a sample of the robot's position xt in the t moment, and the corresponding
weight (j)
t
w represents the probability x(tj) in the t moment t.
The goal of the PF positioning algorithm is to compute the sample set St for
approximate posterior density distribution ( | t) t Z
p x at each time step t. The algorithm is
divided into two stages:
(1) Prediction stage: the T-1 time sample set and the motion modelp(xt|xt−1,ut−1) are used to calculate the prediction sample set '
t
S of the t moment, that is, for each
sample j t−1
x of St−1 , the new sample tj
'
x is obtained from the motion model
) , |
( j−1 t−1 t t
p x x u , and the sample setS' { 'j|j 1, N} t
t = x = is set up.
(2) Update stage: using the observationzt of t time, the weight of each sample
) | ( 'j
t t j
t p
w = z x is calculated, and resampling S' {( ' ,wtj)|j 1, N}
j t
t = x = is performed according to the weight j
t
w . Finally, a set of samples with approximate posterior
probability density ( | t) t Z
px can be obtained by using the weighted regression method.
The principle of resampling is to try to collect samples with high weight values. In the stage of recursive call prediction and update, the robot can continuously update the sample setSt with weight, and use theSt estimate the pose of itself.
Compared with other methods, the PF algorithm has the following advantages: 1) can adapt to any perception model, dynamic model and noise distribution; 2) is a general probability approximation operator, which reduces the restrictive assumption on the shape of a posteriori probability; 3) according to the posterior probability sampling, the computing resources are concentrated in the most relevant areas, and the efficiency is improved; 4) the number of samples can be controlled in real time on the line, which can adapt to different available computing resources, and is easy to be implemented.
However, the disadvantages are obvious: if the sample concentration is small, and the sample does not include the correct position of the sample, the robot can’t locate accurately. In addition, the sample impoverishment phenomenon will cause the loss of sample diversity, leading to the degradation of PF algorithm[9].
Evolutionary Particle Filter Localization Algorithm
solution, 2) contains the evaluation and selection process for possible solutions. The difference between the two methods is that the state transition in different ways: genetic algorithm evolution through crossover and mutation in the genetic evolution simulation of biological thought; and the PF algorithm in the sample according to the motion model of state transition.
In this paper, the evolutionary mechanism is introduced into the PF algorithm, and the selection, crossover and mutation operation of the genetic algorithm are used to optimize the samples to improve the diversity of the samples and improve the performance of the PF positioning algorithm[12], [13].
Evolutionary Selection
The results show that the efficiency of the proportional selection strategy is not high in the current PF algorithm [11]. Due to the weight of the scale, it is prone to bias. Therefore, in this paper, Truncation selection is proposed to extract samples, so as to maintain the diversity of samples. We must select parameters based on the following criteria:
(1) Loss of diversity: percentage of selected samples not selected.
(2) Selection of the variance: select the expected variance of the sample distribution Truncation selection strategy can be described as: when the weight of the sample falls at a threshold in the range of T, the sample will be evenly selected. The loss of diversity is defined as:
T
Ld = 1 − (4)
In this paper, T=0.9 is chosen to guarantee the diversity loss.
The other requirement of designing truncation selection strategy is to guarantee the convergence of the selected samples. The choice of variance is precisely the degree of convergence:
) ( 1 n n c
s I I f
V = − − (5)
Where In is the selection intensity,
2 2
2 1 fc
n e
T I
− =
π (6) The fc can be obtained by the following formula:
df f
f
c
c e
T 2
2
2 1 − ∞
=π (7)
Crossover and Mutation
In this paper, the crossover and mutation operation is used to optimize the sample, which makes the sample move to the region with larger value of the posterior density distribution, so as to better express the posterior density distribution of the system.
Two samples( , ti),( , tj)
j t i
t x
x ω ω were randomly selected from the sample set, and were crossed according to the formula (7).
− + =
− + =
i t j
t jn t
j t i
t in t
x x
x
x x
x
) 1 (
) 1 (
ξ ξ
ξ
Which ξ∈[0 1]is a uniformly distributed random number. Then according to (8) to generate the new sample variation. For the sample i
t
x , a new sample in t
x is generated by variation:
2
σ γ × +
= i
t in
t x
x (9)
where γ is a random number of Gauss distribution, its mean value is 0, the covariance is σ2. In addition, in order to control the crossover and mutation operation,
the crossover probabilitypc =0.9 and mutation probabilitypm =0.1.
Evolutionary Particle Filter Algorithm
Evolutionary selection, crossover and mutation operations are introduced into PF to form an evolutionary particle filter algorithm. The algorithm is described as follows:
(1) Initialization: at t=0, a sample {x0j| j=1,,N} is sampled from the prior distributionp(x0), and let t=1;
(2) Sampling stage: at the t time, the sample j t
'
x is collected from the motion model )
, |
( j−1 t−1 t t
p x x u , and the sample setS' { 'j|j 1, N} t
t = x = is set up. Then, according to the observation of zt at the t time, the weight of each sample is calculated;
(3) Phylogenetic scale
a) Truncate selection: select threshold T and arrange all samples in ascending order by weight. N samples were randomly selected from the samples {[(1−T)N],,N} in the range of sorting, and the sample set Mt was obtained;
b) Crossover and mutation: a random sample is drawn from the sample setMt, and
the crossover and mutation operations are performed according to the formula (7) and (8);
c) Calculate the weight: calculate the weight of each sample according to the calculation of the likelihood in the set of c
t
M , ( t| tj)
j
t p
w = z x and weight parameters are
normalized, a new weighted sample set St ={(xtj,wtj)|j=1,,N} is got.
(4) Resampling: {xtj|j=1,,N} is obtained in the filtering stage according to the weighted distribution of the resampling procedure, which is a sample of the posterior density distribution ( | t)
t Z
px ; the current state of the robot is estimated;
(5) T=t+1 then go to step (2) and start a new loop;
The parameters of the algorithm include sample size N, truncation threshold T, crossover probability pc and mutation probability pm. Although the application of
evolutionary strategy to make the calculation of each sample required increased, but the sample diversity must be set to maintain to reduce the impact of sample dilution, ensure the best samples are retained, and reduce the number of samples required, to overcome the traditional PF algorithm uses large sample set guarantee algorithm to estimate the effect of defects.
Simulation
Robot Motion Model and Observation Model
Motion modelp(xt+1|xt,ut) is used to predict the change of robot motion state with time. When a control of forward velocity and angular velocity utis applied to the robot,
the state transfer equation proposed by Dissanayake is used to predict the motion of the robot [15]: ) ( ) ( ) 1 ( ) ) ( ) ( sin( ) ( ) 1 ( ) ) ( ) ( cos( ) ( ) 1 ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( + Δ + = + + Δ + Δ + = + + Δ + Δ + = + φ γ φ φ γ φ γ φ w T t t t w T t t T v t y t y w T t t T v t x t x i i y i i r i r x i i r i r (10)
The (x ()(t),yr(i)(t))
i
r and ( (), ())
) ( ) ( t y t
x ri
i
r are the position and azimuth of the robot at
the t moment, and v is the speed of the robot γ is the angular velocity, wxyφ∈N(0,σxyφ)
) ( ) (i t
φ is the noise term ,which is used to describe the unknown characteristics of wheel slippage.
The robot uses its own sensors to detect the road signs to get the distance and azimuth. The observation model can be expressed as:
) ( ) ) ( sin ) ( ) ( cos ) ( arctan( )) ( cos ) ( ( )) ( sin ) ( ( ) ( ) ( ) ( 2 2 − + − − − + − − + − = = k k A k x x t A t y y t A t y y t A t x x t t r t Z r l r l r l r l θ θ θ θ θ α (11)
Among them r(t) and α(t)are the distance and azimuth of the landmark to the sonar
sensor. (xl,yl) is the location of the road sign. A indicates the location of the sonar sensor mounted on the robot to the center of the robot (xr(t),yr(t),θ(t))[15].
Simulation Results and Analysis
The trajectory of the robot is shown in figure 1. The evolutionary particle filter (EPF) algorithm and PF algorithm are applied to the robot localization process, and the average error of localization is shown in figure 2. In order to eliminate the influence of other factors, 100 experiments were carried out for each case.
Figure 1. Robot trajectory.
[image:7.612.155.447.74.361.2]In order to further verify the performance of the algorithm, the evolutionary particle filter algorithm is applied to the global localization of the robot. The initial state of the robot is evenly distributed, and the number of samples is 10000. Positioning results and errors are shown in Figure 3, Figure 4.
[image:8.612.164.439.141.383.2]Figure 3. Global localization results
Figure 4. Global positioning error.
Compared with the PF algorithm, by introducing the evolution strategy, improve the diversity of samples, reduces the effect of sample dilution; the samples were optimized, the sample in the posterior region of mobile large density value, so the evolution of particle filter samples can better express the system posterior density, reduce the number of the required samples, improve the positioning accuracy and robustness. In spite of the introduction of evolutionary operations, the amount of computation required for each sample in the algorithm is increased, but the total amount of computation will be reduced due to the reduction of the number of samples.
Conclusion
In practical applications, the PF algorithm is easy to be affected by the dilution of samples, and a large number of samples are needed to obtain a better positioning effect, resulting in an increase in the complexity of the algorithm. Aiming at the problems of PF algorithm, this paper introduces the selection, crossover and mutation operation in evolutionary computation into PF algorithm to improve the performance of the algorithm. The evolutionary particle filter algorithm is applied to robot pose tracking and global localization. The simulation results show that the application of evolutionary strategy not only improves the diversity of samples, reduces the effect of sample dilution, and realizes the optimization of sample, reduce the number of required samples, the evolutionary particle filter algorithm has higher positioning accuracy and robustness. The further research work is to compare the performance of the proposed particle filter algorithm with the resampling algorithm and the simulated annealing particle filter algorithm.
Reference
[1] S. Thrun, D. Fox, W. Burgard, and F. Dellaert. “Robust Monte Carlo localization for mobile robots,” Artif. Intell., vol. 128, no. 1–2, pp. 99–141, 2001.
[2] F. Dellaert, D. Fox, W. Burgard, and S. Thrun. “Monte Carlo localization for mobile robots,” in IEEE International Conference on Robotics and Automation, 1999. Proceedings, 1999, pp. 1322–1328 vol.2.
[3] H. Van Trees and K. Bell. A Tutorial on Particle Filters for Online Nonlinear/NonGaussian Bayesian Tracking. Wiley-IEEE Press, 2007.
[4] R. Havangi. “Robust evolutionary particle filter.,” Isa Trans., vol. 57, p. 179, 2015. [5] W. R. Gilks and C. Berzuini. “Following a moving target-Monte Carlo inference for dynamic Bayesian models,” J. R. Stat. Soc., vol. 63, no. 1, pp. 127–146, 2001. [6] T. Clapp. “Statistical methods for the processing of communications data,” Univ. Cambridge, 2001.
[9] A. Zhu and S. X. Yang. “A Survey On Intelligent Interaction And Cooperative Control Of Multi-Robot Systems,” in Control and Automation (ICCA), 2010 8th IEEE International Conference on, 2010, pp. 1812–1817.
[10] M. Murata, H. Nagano, and K. Kashino. “Monte Carlo filter particle filter,” in Control Conference, 2015, pp. 2836–2841.
[11] T. Higuchi. “Monte carlo filter using the genetic algorithm operators,” J. Stat. Comput. Simul., vol. 59, no. 59, pp. 1–23, 1997.
[12] N. M. Kwok, G. Fang, and W. Zhou. “Evolutionary particle filter: Re-sampling from the genetic algorithm perspective,” in Ieee/rsj International Conference on Intelligent Robots and Systems, 2005, pp. 2935–2940.
[13] F. Herrera, M. Lozano, and J. L. Verdegay. “Tackling real-coded genetic algorithms: operators and tools for the behavioural analysis,” in Arti Intelligence Reviews, 1998.
[14] M. W. M. G. Dissanayake, P. Newman, S. Clark, H. F. Durrant-Whyte, and M. Csorba. “A solution to the simultaneous localization and map building (SLAM) problem,” Robot. Autom. IEEE Trans., vol. 17, no. 3, pp. 229–241, 2001.
