J. Cent. South Univ. (2014) 21: 2208−2215 DOI: 10.1007/s11771-014-2172-4

A novel robust approach for SLAM of mobile robot

MA Jia-chen(马家辰)1, 2, ZHANG Qi(张琦)1 , MA Li-yong(马立勇)1

1. School of Astronautics, Harbin Institute of Technology, Harbin 150001, China;

2. School of Informations and Electrical Engineering, Harbin Institute of Technology at Weihai, Weihai 264209, China

© Central South University Press and Springer-Verlag Berlin Heidelberg 2014

Abstract: The task of simultaneous localization and mapping (SLAM) is to build environmental map and locate the position of mobile robot at the same time. FastSLAM 2.0 is one of powerful techniques to solve the SLAM problem. However, there are two obvious limitations in FastSLAM 2.0, one is the linear approximations of nonlinear functions which would cause the filter inconsistent and the other is the “particle depletion” phenomenon. A kind of PSO & H∞-based FastSLAM 2.0 algorithm is proposed. For maintaining the estimation accuracy, H∞ filter is used instead of EKF for overcoming the inaccuracy caused by the linear approximations of nonlinear functions. The unreasonable proposal distribution of particle greatly influences the pose state estimation of robot. A new sampling strategy based on PSO (particle swarm optimization) is presented to solve the “particle depletion” phenomenon and improve the accuracy of pose state estimation. The proposed approach overcomes the obvious drawbacks of standard FastSLAM 2.0 algorithm and enhances the robustness and efficiency in the parts of consistency of filter and accuracy of state estimation in SLAM. Simulation results demonstrate the superiority of the proposed approach. Key words: mobile robot; simultaneous localization and mapping (SLAM); improved FastSLAM 2.0; H∞ filter; particle swarm optimization (PSO)

1 Introduction

The common formulation of the simultaneous localization and map (SLAM) building problem consists of building some kinds of environmental representations and simultaneously localizing the robot pose [1]. The representation of environmental map is one of the fundamental tasks of mobile robots because the localization of robot needs a consistent map and the robot requires the accurate estimation of its location for building a map. In the past years, lots of researchers were focused on the consistency [2], data association [3], and interval analysis [4], which all of these are essential in the SLAM problem.

As one of probability estimation theories, the extended Kalman filter (EKF) is one of most popular approaches for SLAM problem. The linear approximations of the motion and the measurement models are assumed in EKF-SLAM, which means that the Gaussian is represented by the probability density functions. Joint posterior probability density of robot poses and feature state are estimated at the same time as the effectiveness of SLAM problem. Due to the errors introduced by linearization for nonlinear function, inconsistent of solution of EKF-SLAM is proved in

Refs. [5−6]. Therefore, many recent researches have been focused on the consistency of EKF-SLAM [7]. Particle filter (PF) [8] has received much attention as an approach of SLAM problem recently. A set of weighted sample or called particles are used to approximate the posterior probability of robot pose, which is the basic principle of PF. Based on the formation of Rao-Blackwellized particle filter (RBPF), THRUN [9] had developed a new SLAM framework called FastSLAM 2.0 which contains EKF and PF. In FastSLAM 2.0 algorithm, the standard PF is used for estimating the robot pose and EKF is used for estimating the feature state of map in the environment. However, there are still two drawbacks which could not be neglected in the FastSLAM 2.0 framework [10]. One is the inaccurate approximation introduced in the step of the linearization for nonlinear function. The posterior of measurement state is assumed as the normalized form of two Gaussians. The posterior is approximated by a linear function in EKF. In real-world situation, there are errors and noises for the environmental measurement of sensors, of which distributions are not Guassians. So, the measurement state is not assumed as Gaussians. The estimated accuracy and the consistency of SLAM could be influenced because of the errors caused by assumption.

Foundation item: Project(ZR2011FM005) supported by the Natural Science Foundation of Shandong Province, China Received date: 2013−02−18; Accepted date: 2013−05−10 Corresponding author: ZHANG Qi, PhD; Tel: +86−13009726382; E-mail: hitzhangqi@126.com

J. Cent. South Univ. (2014) 21: 2208−2215

2209

As a powerful state estimation approach, H∞ filter has the characters to minimize the estimated errors [11]. It is not necessary that the statistical properties of the noises are known so long as the noises are energy bounded which means that the posterior of measurement state could not has to be assumed Guassians in SLAM framework. This work is an extension of our previous work. In Ref. [12], H∞ filter is introduced instead of EKF to get high estimate accuracy and the consistency.

The contribution of this work is that a new sampling strategy based on particle swarm optimization (PSO) [13] is proposed for avoiding the “particle depletion” phenomenon and maintaining the diversity of particles, which improves the accuracy of the pose estimation. Just as the discussion above, this work is an extension of our previous research. It is notable that H∞ filter is used for estimating the measurement state as the same as the previous research. 2 Background technologies 2.1 H∞ filter

The H∞ filter technique has been well developed over the past decades. The advantages of H∞ filter have two parts: one is the modality of the noise when variance is more loosened, and the other is when there are additional uncertainties in the system, the H∞ filter tends to be more robust and adaptive. Due to the constraint of length, only some necessary formulae about the H∞ filter are given and more details about H∞ filter can be found in Refs. [14−15]. It is assumed that the state model of random linear discrete time system in H∞ filter is described as

1k k k k k

k k k k

X X W

y H X V

(1)

Given the filter error as

0 1

ˆ

ˆ ( , , , )

k k k k

k k k

k k

z

z F y y y

e L X

z L X (2)

where kz is the estimation of zk given the value of observation (yk).

q nk

RL is given matrix. zk=LkXk is linear combination of the state Xk.

Theorem 1: Existence conditions of H∞ filtering Given γ>0, if k k is full rank ， then

kT F

<γ exists when k , thus

1 T 2 T 0k k k k k P H H L L (3) where Pk is satisfy with recursion Riccati equation

1 T T T1

1 T,

T T, 2

0

0

k k k k k k k k k k

ke k k k

k

ke k k k k

k

P P P H L

HR P

L

HR P H L

L

(4)

If Riccati equation exists, then ˆˆk k kz L X and ˆ

kX can be calculated by

1 1 1 1

1T T1 1 1 1 1 1

ˆ ˆ ˆ( )k k k k k k k k

k k k k k k

X X K y H X

K P H H HP (5)

2.2 Particle swarm optimization

Particle swarm optimization (PSO) algorithm uses the particles driven from natural swarms which communicate based on the evolutionary computations [16]. PSO has the ability to memorize the particle optimal position and the strategy which the particles could share the information from each other. PSO searches for the optimal solution of the problem by means of the individuals’ cooperation and competition. Each individual has its position

tiX and is assigned the

moving velocity .tiV In PSO, the simple strategy is used

to express the fittest position at each iteration time.

1 1 1 1 11 1 lbest 2 2 gbest

1 1

t t t t t ti i i i

t t ti i i

V V c r p X c r p X

X X V

(6) where ,1 ,2 , ,1 ,2 ,, , , , , , ,t t t t t t t t

i i i i d i i i i dX x x x V v v v are

the location and velocity of i-th individual, respectively; d is the dimension of search space; ω is the inertia weight operator; When ω is larger，PSO has the ability with strong global search，vice versa; 1 2, 0,1 ,r r are uniform random numbers; c1 and c2 are the acceleration of operators. The performance of the PSO algorithm depends on the choice of parameters in the algorithm, which consists of the size of population, the maximum speed of particle, and the other various operators. 3 Previous work

Most processes associated to the EKF SLAM have to be linear in the number of map features, which estimates the pose of vehicle and the map feature positions in an augmented state vector. Although EKF-based SLAM within the stochastic mapping framework gained wide popularity among the SLAM research community, there are still several shortcomings. Notable shortcomings are its susceptibility to data association errors, non-Gaussian noises and inconsistent treatment of nonlinearities. The optimality of EKF

J. Cent. South Univ. (2014) 21: 2208−2215

2210

depends on the modeling-formalization of the noise, which requires the formalization of noise has to be Gaussian. EKF will be no longer optimal when the noise statistics are unknown or un-Gaussian.

1 1 1

PF

, , , , , ,( ) ( )t t t t t t tt tz u n z u np s p s s

1

1H

( , , , )K

t t t tk

k

p s z u n

(7)

Due to the shortcomings of the EKF SLAM, the

characteristics which H∞ filter was suitable for any hypothetical noise statistical properties was considered, H∞ filter was substituted for EKF in Ref. [12]. In this section, the introduction about our previous work is just briefly reviewed. In Ref. [12], we presented the hybrid H∞ FastSLAM 2.0, in which 1( , , , )t t t t

kp s z u n is calculated by H∞ filter instead of EKF in the process of updating the map feature. 4 Proposed approach

In this section, a novel sampling strategy for the proposal distribution of particles to solve the “particle depletion” phenomenon is introduced. The PSO and genetic algorithm (GA) are combined to obtain the further improvement of the proposal distribution in performances. The FastSLAM 2.0 algorithm based on the PSO-sampling strategy and H∞ filter called PSO & H∞ -based FastSLAM 2.0 is proposed. 4.1 PSO-based sampling strategy

As discussed in literature, the “particle depletion” phenomenon means the variance of the particle weight increases following with the time. After more than once particle sampling, a few particles are with bigger weights and the rest are tiny, which will affect the particle distribution of particles. It will cause the situation that it costs lots of time to calculate the particles with tiny weights, so the consistency of robot pose estimation will be corrupted. To solve the “particle depletion” problem, typical method is to choose appropriate sampling approach and resampling strategy [17]. In our presented novel sampling strategy, the speed and position update formula of PSO is utilized to obtain the particle set of robot pose state. Then, the particle set will be in the optimized operation by using the crossover and mutation in GA. The generated new particle is used instead of the inferior particle.

In a general way, the concrete realization process is simple. 4.1.1 Particle sampling method

Given a set of weighted particles 1, ,

ti i N

S

1, , 1, ,

, ,i i

t t tii N i N

s w S

is to represent the posterior

probability distribution of robot location 1 1, ,( t t t

s up s

zt). tis and t

iw denote the particle and the corresponding

weight, respectively. In our work, the location and velocity function of PSO is utilized to update the particle. The location of i-th particle at the time t denoted

as tis with corresponding velocity t

iV is

11 1 pbest 2 2 gbest

t t t ti i i iV V c r S s c r S s (8)

1t t t

i i is s V (9) where 22 2 4 , 4. As discussed before, ω has the effect to constrict and limit the velocity. Α regulates the global search behavior of particles which is set as dynamically decremented value. Its range is suggested to be 0.4≤α≤0.6. The particle locations of local optimum and global optimum are respectively denoted Spbest and Sgbest, respectively. In PSO algorithm, c1 and c2 are the factors to balance the effect of local-knowledge and global-knowledge and are chosen to set to the value from 0 to 4. Here, in our approach, various combinations of c1 and c2 were tested and c1=c2=2 in the setting.

1 2, 0,1r r are uniform random numbers. 1tis is the

updated particle. Our algorithm samples are in proportion to the following importance factor:

1 1~ , , ,t t t t t ti iw p z s u z n (10)

The average value tw of particle weights is

1

1 Nt t

ii

w wN

(11)

4.1.2 Separation of particles

After all the particles are updated according to the

Eq. (15), the particle set 1 1

1, , 1, ,,t t t

i i ii N i NS s w

is

separated by two parts: 1, ,l l LS consisting of particles

whose weight value is bigger or equal to the mean and

1, ,m m MS consisting of particles whose weight value

is smaller than the mean, L+M=N. 4.1.3 Crossover and mutation operation

The basic idea of the GA is that according to Darwin’s “survival of the fittest” principle, the inter-subject processes the operation of crossover and mutation depending on the different individual fitnesses, which the new set of individuals are produced. In this work, the operation of crossover pc and mutation pm are drawn into dealing with 1,i i N

S for optimization.

After separating the particle set, two particles are randomly chosen from 1, ,l l L

S and 1, ,,m m M

S

which are denoted by Sl and Sm, respectively. Crossover process:

J. Cent. South Univ. (2014) 21: 2208−2215

2211

pc l mS S S (12) where Spc is the new generated particle from the crossover operation. α is the factor of crossover operation. The crossover operation involves the particle with which the weight is bigger than

tw and the particle with which the weight is smaller than

tw . The process above do echoes the purpose of crossover operation in GA.

Mutation process:

pm 1 lmt

wS S

w

(13)

where Spm is the new generated particle from the

mutation operation. 1 tlw w is the factor of mutation

operation. wl is the random weight of particle from

1, ,l l LS and tw is mean value of weights. The mutation

operation only involves the particle from 1, ,m m MS

with the purpose of improve the quality of particle from

1, ,.m m M

S

4.1.4 Producing new particle After that, Snew is the generated new particle.

new pc pm2S S S

(14)

According to Eqs. (11)−(13), Snew could be rewriten

as

new 12

ll m mt

wS S S S

w

12

ll mt

wS S

w

(15)

Note that ,t

lw w due to the particle with weight wl is from 1, ,

,l l LS there is

1 1 1 2 1t tl lw w w w (16)

If α and β are both positive numbers and satisfied

with the condition:

1

, 0,1

m ls s

(17)

Then, it is easy to show that Snew is satisfied with the

following the equation new l mS S S (18)

New particle Snew is used instead of the Sm from

1, ,m m MS and there will be a new particle set

1, ,{ }ti i NS . It is worth mentioning here that the new

particle Snew has the better quality than Sm, which does not lead to the “particle depletion” phenomenon. It’s all because that the generated new particle is from the two

different weighted particles and better than the one with the smaller weight. 4.1.5 Weight of new particle

The weight of generated new particle is adjusted by

new 12

ll mt

ww w w

w

(19)

Evidently though, new .m lw w w The initial values

of Spbest and Sgbest are defined beforehand. The particle Snew is as Spbest and the one with the biggest weight from

1, ,

ti i N

S

at the time t is as Sgbest. It suggests a strategy

based on the value of Neff, which is strongly related to the distribution of the particles.

eff 2

1

12N i

i

NNw

(20)

Neff is the decision condition of the resampling

strategy [18]. When the new particle set1, ,

{ }i N

tiS

is ready,

the weights of all particles in1, ,

{ }i N

tiS

are calculated

according to Eq. (20). If Eq. (20) is satisfied, the presented sampling strategy will be end. Otherwise, the new particle set 1, ,m m M

S

will be used into the Eq. (8)

until the Eq. (20) is satisfied. 4.2 PSO & H∞-based FastSLAM 2.0

The essential structure of PSO and H∞-based FastSLAM 2.0 is that the joint distribution at time t is

represented by the set1, , 1, , 1, ,

,{ , , ,( t

i N i N i N

t t t tzs w sp

},)tn where the observation map of each particle consists of independent Gaussian distributions. PF is used to perform the pose state of robot and H∞ filter is used to perform the observation map at each recursive estimation time step, which is as the same as Eq. (7). When the pose state of robot is known, each observed landmark is updated as the H∞ filter measurement. The general form of PSO& H∞-based FastSLAM 2.0 is as follows.

In the initialization, the number of particle and the weight accompanying each particle are set up first. At time t, it is assume that the joint state is represented

by 1, , 1, , 1, , 1, ,,, , , , { ,( )t

i N i N i N

Nt t t t t tt i Nzs w s n S sp

1, , }ti Nw is the particle set, which represents the pose

state of robot. With the different [ ] [ ]1~ ( |N N

t t tS p s s

, , )t t tu z n , the location and velocity function of PSO is

utilized to update the particle set 1, ,{ ,N t

t i NS s

1, , }.ti Nw

Two particles are chosen based on their weight compared with the mean of all particle weights. Snew from the chosen particles in the process of genetic

J. Cent. South Univ. (2014) 21: 2208−2215

2212

manipulation is the generated new particle, which is used instead of the poor particle. If necessary, the resampling procedure is performed until Eq. (18) is satisfied.

In data association procedure, when the observed environment characteristics are obtained at the time t+1, it will be compared with the one at the time t.

1, , 1 1ˆ ˆarg max , , ,t

i t i t i t tt t

nn p z n s z u

1, 1, 1ˆarg max , , , , dt t t

t i t i t i tt n t n np z n s p n s z

(21)

For each particle, the H∞ filter is performed for updating the observed landmarks which represents the environment map with the known robot pose. According to the associated information of the particles in the data

association procedure, the state of particle ,t ms , and

observed information zt−1, the estimated mean ,m

n t and

varianc ,m

n t of the posterior probability of environmental

characteristic ,( , , )t

t m t tnp s z n is obtained using the H∞

filter.

1

~ ; ,

( , , ) ( , , , )t t

mt n tt

m mt t t t tn t n t

N z g s

p s z n p z s z n

, 1 , 1

1 1

~ ; ,

( , , )t

m mn t n t n tt t t

m t tn t

N

p s z n

(22)

The l inear izat ion of , 1( , , , )t

t mt t tnp z s z n in

Eq. (22) is

, 1 , 1

ˆ

, , ,t t t

m mt nt

m mm m mn t t tn t n t

Z G

g s g s g s

, 1t t

mn n t (23)

Given ,k k k L I ,nt

kH G 1 , ,m

k n tX

1 , ,

t

mk n tP Eq. (1) is changed as

, , 1

, 1nt

m mn t n t k

mk n t k

W

y G V

(24)

According to , 1ˆ ,mt t n tz g s and

1 , ,mk n tX

there is

, 1 , 1

ˆ ˆˆ , mmt t n t k n tz g s X (25)

According to

1 , ,t

mk n t P ,k k k L I

,nt

k H G Eq. (4) is changed as

, 1

T, , 1 , 1

1, , 1

T, , 12

;

0

0

,

nt t t t

nt

t

nt

nt t

n t mmt ntt t n n tt t

m m mn t n t n t

me t n t

me t n t

t ns s

RR

G g s

I G

GR

GG

(26)

According to ,k I ,

ntk H G

1 , ,mk n tX

Eq. (5) is changed as

, , 1 , 1 , 1

1T T

, 1 , 1

ˆ ˆ ˆnt t t tt

n n nt tt t t

m m m mtn t n t n t n t

m mt n t n t

G

K

K G I G G (27)

4.3 Complexity of proposed approach

In the proposed approach, the procedure of computing the proposal distribution need to handle M particles and the procedure of updating the map (calculations of N landmarks) have the constant complexities. The computing the proposal distribution, calculating the important weights and updating the landmarks require the complexity of O(M). The procedure of resampling requires O(MN) time, which means the complexity of our method is the same as FastSLAM 2.0 algorithm. 5 Simulation results 5.1 Preliminary experiments

The wheelbase of robot is 0.8 m and is equipped with the SICK laser range finder with the 180° frontal field-of-view. The velocity of robot is vt=0.5 m/s and angular velocity is wt=30 (°)/s. There are both Gaussian noise covariance generated in the motion model and the measurement of robot. The motion noise of robot is assumed as {σv=0.05 m/s, σγ=2°}. The measurement noise is assumed as {σr=0.08 m/s, σθ=4°}. To simplify the situation, data association is assumed to be known. Four particles are used to evaluate the pose of robot. The results using the PSO and H∞-based FastSLAM 2.0 algorithm which outperforms the standard FASTSLAM 2.0 algorithm is shown in Fig. 1. In Fig. 1, the stars denote the position of landmarks in the environment. Four particles are used and the measurement noises are set largely {σr=0.08 m/s, σθ=4°}. There are errors between the real path and the estimated path due to the “particle depletion” phenomenon in FastSLAM 2.0 algorithm. However, the proposed algorithm avoids above phenomenon and ensures that the robot pose estimation is relatively accurate. The error does not accumulate in the feature state estimation, which

J. Cent. South Univ. (2014) 21: 2208−2215

2213

Fig. 1 Simulation results of 100 m×100 m environment with 60

landmarks

guarantees that the real path is closer to the estimated path in proposed algorithm than that in FastSLAM 2.0 algorithm. The estimation accuracy of robot is enhanced by using the proposed algorithm.

From Tables 1 and 2, the estimation accuracy of the proposed approach is better than that of FastSLAM 2.0, which the reason is that the more accurate hypothesis about the noise statistical properties using the proposed approach leads to better estimate results than the nonlinear noise for linearization. The values of mean square error (EMS) show that the performance of the presented sampling strategy affects the estimating accuracy. In Fig. 1, Tables 1 and 2, there are certain errors between the actual path and the estimate path by using FastSLAM 2.0, which is because that EKF is used to update environment characteristics in FastSLAM 2.0 algorithm. In the actual cases, statistical properties of the system model and the exact external disturbance signal often can not satisfy the requirements of the EKF, which leads to the estimated deviation of the environmental characteristics. The deviation will affect the estimated veracity of robot pose in the next moment.

Table 1 Results in 50 m×50 m environment

SLAM algorithm

(4 particles)

EMS/m Heading direction

errors/(°) Mean Variance

FastSLAM 2.0 4.621 0.457 −5.6374

Proposed approach 1.386 0.062 −2.5419

Table 2 Results in 100 m×100 m environment

SLAM algorithm

(4 particles)

EMS/m Heading direction

errors/(°) Mean Variance

FastSLAM 2.0 5.861 0.529 −7.2769

Proposed approach 2.132 0.097 −2.1386

5.2 Performance comparison between two algorithms In this section, two experiments are set to

demonstrate the effectiveness of the presented approach. The first experiment is designed to test the robustness of the measurement uncertainty. Five particles are used under the different measurement noise hypotheses in 30 m×60 m environment and 10 independent runs are operated for the estimated pose of robot. The different bars represent the mean and variance of MSE (mean square error) of the estimated pose of robot using the proposed approach and FastSLAM 2.0. The mean and variance of MSE of the estimated pose of robot are shown in Fig. 2. In Fig. 2, the setting of measurement noises of each bar is: (a) {σr=0.08 m/s, σθ=3°}; (b) {σr=0.15 m/s, σθ=5°}; (c) {σr=0.15 m/s, σθ=8°}; (d) {σr=0.3 m/s, σθ=10°}; (e) {σr=0.3 m/s, σθ=14°}. The data association is known in the loop closure. As shown as in Fig. 2, the mean of estimation errors of our presented approach and FastSLAM 2.0 are bigger with the increased setting of measurement noise, however, the increase in our presented approach is much more slow than that in FastSLAM 2.0 and the variance of MSE is also smaller than that in FastSLAM 2.0, which proves that our presented approach has higher robustness than FastSLAM 2.0.

Fig. 2 Mean and variance of MSE of estimated pose of robot

The second experiment is designed to test the

number of the needed particles in our presented approach for obtaining the same estimation accuracy as FastSLAM 2.0. The second experiment is developed in 30 m×60 m and 100 m×100 m environments and the traveled trajectory of robot are both more than 300 m.The number of needed particles are listed in Table 3. When the estimated error is almost the same, the number of needed particles in our presented approach is far less than those in FastSLAM 2.0. 5.3 Evaluation of consistency

In this section, the setting experiment is developed to demonstrate the consistency of the presented approach. To illustrate the consistency of SLAM algorithm, NEES

J. Cent. South Univ. (2014) 21: 2208−2215

2214

Table 3 Second experiment

No. Algorithm Needed number

of particles

Environment,

traveled trajectory

1 Proposed approach 5 30 m×60 m, 300 m

FastSLAM 2.0 35 30 m×60 m, 300 m

2 Proposed approach 10 100 m×100 m, 500 m

FastSLAM 2.0 50 100 m×100 m, 500 m

(normalized estimation error squared) is standardized as the condition. NEES is defined as

1ˆ ˆkk k k k k T x x P x x (28)

1

1 N

ii

k kN

(29)

where x(k) is the true pose of robot at time k; )(ˆ kx is the estimated pose of robot at time k. Pk denotes the covariance of estimated pose at time k. k is the mean of k . Due to space constraints, it suggests that the reader to absorb more details about NEES in Ref. [19].

The robot has 0.8 m wheelbase and the velocity is vt=0.5 m/s. The noise of the motion model of robot and the measurement model of sensors are {σv=0.1 m/s, σγ=3°} and {σr=0.05 m/s, σθ=1°}, respectively. The frequencies of control and sensor are 50 Hz and 10 Hz, respectively. The results of 50 times Monte Carlo test runs are shown in Fig. 3. The acceptance interval of k is [2.36 3.72] denoted as the dotted line in Fig. 3,

which is 95% probability concentration region of the average NEES. Figure 3 indicates that the consistency of our approach is better than that of FastSLAM 2.0. The H∞ filter is adapted to more extensive noise model than EKF and the more accurate estimate is without accumulating linearization error in our approach, which improve the consistency of SLAM. The sampling strategy is essential for estimating the accurate pose, however, the inaccurate pose estimation could induce the “particle depletion” phenomenon over time. In the proposed approach, the poor particle is replaced by the generated one which has better distribution. 6 Experimental results

The Intel Research Lab (IRL) dataset is used to compare the proposed approach with standard FastSLAM 2.0, which is shown in Fig. 4. The number of particles, execution time and maximum memory needed are compared between two SLAM approaches in IRL dataset, which is done using PC with Intel CoreTM i5 CPU and 2.26 GHz RAM. The results of comparison are listed in Table 4. In the experiment, the savings on computed proposal distribution so that it can be used for runtime are mainly caused by transforming an already several particles instead of computing it from scratch

Fig. 3 Consistency of proposed approach: (a) Result of

consistency in 30 m×60 m environment with 10 particles; (b)

Result of consistency in 100 m×100 m environment with 20

particles

Fig. 4 Intel research lab (IRL)

Table 4 Comparison of our approach and FastSLAM 2.0 for

IRL

Algorithm No.Number of particles

Execution time/min

Maximum memory/MB

Proposed approach

1 30 37 115

2 15 29 81

FastSLAM 2.0

1 60 89 850

2 35 46 510

J. Cent. South Univ. (2014) 21: 2208−2215

2215

each time. The memory savings are due to the new sampling strategy updates particles by using PSO. The memory usage and runtime of our approach grows comparably slow when increasing the number of particles.

The execution time is analyzed for validating the computational cost of two algorithms in different environments, as shown as Fig. 5. Five particles are used for this testing experiment. From Fig. 5, it can be seen that the computational cost of FastSLAM 2.0 is a little less than that of the proposed approach. It takes more time for particle sampling in the proposed approach.

Fig. 5 Computational cost 7 Conclusions

1) The proposed new sampling strategy updates particles by using the location and velocity function of PSO. After crossover and mutation operation about particles, a new generated particle is used to replace the poor one of two chosen particles. Due to that, the diversity of particles improving the accuracy of the pose estimation is kept, which maintains the number of particles and avoids the “particle depletion” phenomenon.

2) In the feature estimation step, H∞ filter is used for updating the mean and the covariance of the feature state, which avoids the errors of linearization for nonlinear function and the complexity of computing Jacobian. The error does not accumulate in the feature state estimation, which guarantees that most particles keep the more accurate information in pose state estimation. It is the reason that the consistent of algorithm is better than FastSLAM 2.0.

3) The new proposed approach is implemented and tested in a series of experiments, which contains the dataset and the real robot. The results of these experiments demonstrate that the proposed algorithm has the advantage of more accurate estimation and

outperforms FastSLAM 2.0. References [1] CHEN Bai-fan, CAI Zi-xing, HU De-wen. Approach of simultaneous

localization and mapping based on local maps for robot [J]. Journal of Central South University of Technology, 2006, 13(6): 713−716.

[2] LI Hui-ping, XU De-min, ZHANG Fu-bin, YAO Yao. Consistency analysis of EKF-based SLAM by measurement noise and observation times [J]. Acta Automatica Sinica, 2009, 35(9): 1177−1184.

[3] BOOIJ O, ZIVKOVIC Z, KRÖSE B. Efficient data association for view based SLAM using connected dominating sets [J]. Robotics and Autonomous Systems, 2009, 57(12): 1225−1234.

[4] NASSREDDINE G, ABDALLAH F, DENOUX T. State estimation using interval analysis and belief-function theory: Application to dynamic vehicle localization [J]. IEEE Transactions on Systems, Man, and Cybernetics, Part B: Cybernetics, 2010, 40(5): 1205− 1218.

[5] AUAT C F, STEINER G, PEREZ P G. Optimized EIF-SLAM algorithm for precision agriculture mapping based on stems detection [J]. Computers and Electronics in Agriculture, 2011, 78(2): 195−207.

[6] CHATTERJEE A, MATSUNO F. A Geese PSO tuned fuzzy supervisor for EKF based solutions of simultaneous localization and mapping (SLAM) problems in mobile robots [J]. Expert Systems with Applications, 2010, 37(8): 5542−5548.

[7] CASTELLANOS J A, MARTINEZ C R, TARDÓS J D. Robocentric map joining: Improving the consistency of EKF-SLAM [J]. Robotics and Autonomous Systems, 2007, 55 (1): 21−29.

[8] YU Ling-li, CAI Zi-xing, ZHOU Zhi, FENG Zhen-qiu. Fault detection and identification for dead reckoning system of mobile robot based on fuzzy logic particle filter [J]. Journal of Central South University, 2012, 19(5): 1249−1257.

[9] THRUN S. robotics and cognitive approaches to spatial mapping [M]. Berlin: Springer Berlin Heidelberg, 2008: 13−41.

[10] KIM C, SAKTHIVEL R, CHUNG W K. Unscented FastSLAM: A robust and efficient solution to the SLAM problem [J]. IEEE Transactions on Robotics, 2008, 24(4): 808−820.

[11] SHANG Lin, LIU Guo-hua, ZHANG Rui, LI Guo-tong. An information fusion algorithm for integrated autonomous orbit determination of navigation satellites [J]. Acta Astronautica, 2013, 85: 33−40.

[12] ZHANG, Qi, MA Jia-chen, LIU Qiang. Improved FastSLAM2.0 based on the H∞ filter for intelligent mobile robot [J]. Research Journal of Applied Sciences, Engineering and Technology, 2012, 16(4): 2748−2754.

[13] HU Guang-hao, MAO Zhi-zhong, HE Da-kuo. Multi-objective optimization for leaching process using improved two-stage guide PSO algorithm [J]. Journal of Central South University of Technology, 2011, 18(2): 1200−1210.

[14] CHANG Xiao-heng, YANG Guang-hong. Non-fragile fuzzy H∞ filter design for nonlinear continuous-time systems with stability constraints [J]. Signal Processing, 2012, 92(2): 575−586.

[15] SAHOO H K, DASH P K, RATH N P. Frequency estimation of distorted non-stationary signals using complex H∞ filter [J]. International Journal of Electronics and Communications, 2012, 66(4): 267−274.

[16] POLI R, KENNEDY J, BLACKWELL T. Swarm itelligence [M]. New York, USA: Springer US, 2007: 3−57.

[17] GRZONKA S, PLAGEMANN C, GRISETTI G, BURGARD W. Look-ahead proposals for robust grid-based slam with rao-blackwellized particle filters [J]. The International Journal of Robotics Research, 2009, 28(2): 191−200.

[18] DOUCET A, FREITAS N. Sequential monte-carlo methods in practice [M]. Cambridge, USA: MIT Press, 2001: 221−235.

[19] CASTELLANOS J A, MARTINEZ C R, TARDÓS J D. Robocentric map joining: Improving the consistency of EKF-SLAM [J]. Robotics and Autonomous Systems, 2007, 55(1): 21−29.

(Edited by DENG Lü-xiang)