16th Mediterranean Conference on Control and Automation Congress Centre, Ajaccio, France June 25-27, 2008

Navigation of an autonomous mobile robot using EKF-SLAM and FastSLAM J. Z. Sasiadek*, A. Monjazeb* and D. Necsulescu**

Abstract—This paper presents a navigation of an autonomous robot using Simultaneous Localization and Mapping (SLAM) in outdoor environments. SLAM is a method in which localization and mapping are done simultaneously in an unknown environment without an access to a priori map. This paper introduces a probabilistic approach to a SLAM problem under Gaussian and non-Gaussian conditions and offers alternative solutions. First, an Extended Kalman Filter algorithm for the SLAM problem under Gaussian condition will be shown. Also, an alternative way of dealing with SLAM problem with assumption of non-Gaussian and called FastSLAM will be analyzed. FastSLAM is an algorithm that using Rao-Blackwellised method for particle filtering, estimates the path of robot while the landmarks positions which are mutually independent and with no correlation, can be estimated by EKF. This is done using a factorization that fits very well into SLAM problem based on a Bayesian network. In this paper, a real outdoor autonomous robot is presented and several experiments have been performed based on both methods. The experimental results are discussed and compared. INTRODUCTION

S

imultaneous Localization and Mapping (SLAM) is a process by which a mobile robot, or an autonomous vehicle, at an unknown location and in an unknown environment, incrementally constructs a navigation map of its surroundings. At the same time, the robot simultaneously, and with the observations of features, uses this navigation map for localization. In other words, the autonomous vehicle tries to do both mapping and localization at the same time. This is due to the fact that the vehicle position has an error at every time step and the obtained map is highly correlated with the error of the vehicle. Since a real dynamic system is always subject to noise, a good understanding of noise representation is needed. Furthermore, most dynamic systems in the real world are non-linear which means that the mathematical models

* Author is with the Carleton University, Department of Mechanical and Aerospace Engineering, Ottawa, Ontario, K1S 5B6, Canada, e-mail: [email protected] ** Author is with the University of Ottawa, Department of Mechanical Engineering, Ottawa, Ontario, K1N 6H5, Canada, email: [email protected]

978-1-4244-2505-1/08/$20.00 ©2008 IEEE

describing the dynamic system and related observations can not be considered as a linear problem. Extended Kalman Filter (EKF) has been one of the most appropriate tools to deal with a non-linear dynamic system model subject to Gaussian noise [1]-[3]. The singlehypothesis data association and quadratic complexity due to the high dimensional Gaussian approximations for states of the robot and landmarks locations, makes the off-diagonal elements of the covariance matrix very large. This causes more complexity and cost increase of computation and, in most cases, diverges the filter. Moreover, when a large number of landmarks are present in the environment, the computation becomes almost impossible. As an alternative approach, there is another algorithm which uses the multihypothesis data association and logarithmic complexity instead of quadratic. This approach, known as FastSLAM utilizes Rao-Blackwellised particle filter to solve the SLAM problem efficiently. Using FastSLAM algorithm, the posterior estimation will be over the robot’s pose and landmarks locations. The FastSLAM algorithm has been implemented successfully over thousands of landmarks and compare to EKF-SLAM that can only handle a few hundreds of landmarks, it has appeared with considerable advantages. I. SLAM PROBLEM Simultaneous Localization and Mapping (SLAM) [1]-[7] describes a situation in which a mobile robot is placed in an unknown environment while there is no access to a priori map and only with sensing the environment and through some sequential controls, localize itself and at the same time incrementally builds a navigation map of its surroundings. The general SLAM problem can be described in form of a probability distribution. If there is an access to the initial R

state of the robot x 0 (state at k=0) and recorded landmark observations Zk and control inputs Uk up to and including time k, the joint posterior density of the landmark locations and the state of the autonomous robot can be described by the following probability distribution: R

R

P (x k , m | Z k , U k , x 0 )

(1) R

If the state of the motion of the robot, x k and also the true locations of the landmarks m are known, the observation model describes the probability of making an observation of all landmarks zk. Therefore, we can describe the sensing of 517

the robot in probabilistic form [5]. For simplicity, we will note the motion of the robot and map of landmarks at time step k as x k in form of following augmented set [12] and it will be denoted as state of the whole system: R

xk ={xk ,m}

(2)

Then the observation model can be described as follows: P (z k | x k )

× P (x k −1 | Z k −1 , Uk , x0) d x k −1

To further simplify equation (6), Markov assumption which states that based on information of the current state, the previous state of the system is independent of the current one, can be used. Markov assumption leaves all the previous control and observation behind and will make equation (6) simpler. Therefore equation (6) can be revised as

(3)

Bel–( x k ) =

An autonomous robot performs actions in the environment that changes the position of the robot. This action is due to a sequence of control actions. If we let uk from the set of control actions Uk = {u1 , u2 , … , uk}, be the action performed by the robot at time k, we can express the way the location of the robot changes probabilistically by a transition density as: P (x k | x k −1 , u k )

(5)

That is, with the consideration that the map of landmarks is R

m, the robot believes that it is at state x k at time k, given R

the initial state and all locations of the robot x 0 , and the set of control actions up to time k. This probability distribution has the highest possible probability at which the system can be. The goal of SLAM is to make this belief as close as possible to the real distribution of the system. There is only one peak out of this distribution at the true locations and it is zero everywhere else. This specification is the definition of a unimodal system.

Using total probability and the Markov localization [8-10], an efficient formula for the next step can be derived. Bel–( x k ) = P (x k | z 1 , u 0 , z 2 , u 1 , … , z k −1 , u k , x 0 )

∫

s

s

P (x k | x k −1 , Uk , x0) (7)

or, using the Markov assumption Bel+( x k )× P ( z k | Z k −1 , Uk) = P ( z k | x k ) × Bel–( x k )

(8)

II. FAST SLAM

This transition probability density gives the probability that if at time step k-1 the robot was at location xk-1, and performed a control action uk, then it ended up at location xk, at time step k. This transaction density is therefore motion model that describes how the control input uk changes the location of the robot. SLAM can be described as a Bayesian estimation problem. Estimation of location of the robot and also landmark locations can be described with the consideration of noise. With a probabilistic point of view we can say that the robot has a belief of what the position of landmarks and its pose is. At time step k, the robot is not considering one specific location but a number of locations for itself and the landmarks. Markov Localization is an iterative way of using the Bayesian Theorem [5] to update the available information each time new sensor reading is available. The Markov Localization framework combines the data from different sensors to form a combined belief regarding the location of a robot and landmarks. The belief can be described by a probability density over all locations xk ∈ S where S is the set of all locations of the robot and the landmarks. The belief then can be described as follows:

= P (x k | Z k −1 , Uk , x0) =

∫

× Bel+( x k −1 ) d x k −1

(4)

Bel ( x k ) = P (x k | X k −1 , U k , x 0 )

(6)

P (x k | x k −1 , Z k −1 , Uk , x 0 )

Extended Kalman Filter (EKF) estimates the posterior of state of the system based on equation (1). This means that the estimation is accomplished over landmark location and robot’s pose sequentially at every time step k. In FastSLAM algorithm [8]-[10], the posterior is estimated over the landmark location as well but instead of the single state of the robot at time step k, the path of it is estimated. In fact, FastSLAM is employing equation (9) instead of equation (1). R

R

P (X k , m | Z k , U k , x 0 , d k )

(9)

or simply R

P (X k | Z k , U k , x 0 , d k )

(10)

The difference between equations (1) and (9) is that in R

equation (1), only a single state x k is involved in the posterior estimation but in equation (9) the whole path of the R

robot X k is estimated. Landmark mj is observed at the first time step, mi is observed at the first time step, time step 2 and time step k and landmark mn is observed at time steps 2 and k. From this it is obvious that if the true path (trajectory) of the robot is known, landmarks mi, mj, and mn, are mutually independent. Equation (10) shows that with the notification of robot’s path, a land mark position is conditional to path of the robot and independent of the other landmarks. In other words there will be no correlation between any two landmarks of the map. This makes the SLAM problem low-dimensional because there will be M+1 filters, One particle filter for path of the robot and M EKFs for landmarks positions estimation according to the path of the robot. This factorization is exact and fits very well for any SLAM application [10]. As soon as the filter estimates path of the robot using particle filter, every landmark position is being tracked by EKF. The number of EKFs depends on the number of particles P. There will be M×P EKFs in total to estimate landmarks locations using path of the robot at this step. The superscript on the left hand side of

518

the state shows the number of the particle used to estimate the state of the robot in time step k. It appears that the state of the robot for a specific particle n can be expressed as follows: n

x kR = { n X kR , n μ k ,1 , n σ k ,1 , K , n μ k , M , n σ k , M }

(11)

The superscript on the left hand side of each term shows the involving of the nth particle at time step k for estimating M landmarks locations in the map using EKF and according to the estimated path of the robot using RBPF (Rao-Blackwell Particle Filter). In this paragraph, we will first consider mapping between observation and landmarks as known data. Known data association simply means that information of observation of each landmark is being incorporated to the map without being mixed with any other data observation of other landmarks. In other words, the data from each observation is absolutely known and related to only one landmark. The classical way to deal with the data association problem in SLAM is to choose d k ,i such that it maximizes the likelihood of sensor measurement z k given all available data [10].

) d k , j = argmax P (z k

| d k ,i ,

) d k −1 , X kR , Z k −1

yG β

, U k ) (12)

mi xr φ

α

mn xG Figure 1: The vehicle coordinate system and the position of ith landmark with respect to the robot coordinate system [12]

Unlike EKF-SLAM which is based on single-hypothesis data association, FastSLAM does not determine the data association once for each observation [10]. This property of FastSLAM is called multiple-hypothesis data association. Consequently, if a wrong observation of a landmark is not accurate enough, the data is simply ignored and is not fused to the map since the particle carrying it could not achieve enough weight in the resampling process and has already been ignored. FastSLAM has multiple data association property and only the most accurate data is being incorporated and fused to the map. The particles with the

most correct data association that have enough weight can pass the resampling process and the most accurate data association which are carried by those particles are filtered from the resampling process and considered to be fused to the map. Some interesting applications were presented in [11]. III. EXPERIMENTAL RESULTS For more clarification, there have been two different filters EKF and RBPF considered for the same trajectory and landmarks locations for a particular SLAM problem. In this situation the noise in which the system is subject to, is first assumed Gaussian white with zero-mean and independent. The result in Figure 2-a shows that the vehicle stays near the path with a highly acceptable estimation. The dotted line is the odometer information. Figure 2-b illustrates the same trajectory in which the noise is not Gaussian anymore and instead, a mixture of exponential and Gaussian has been assumed. This figure clearly shows how fragile the EKFSLAM can be due to non-Gaussian implication. It can be noticed that almost in the middle of the trajectory, EKF can no longer manage the non-Gaussian error of the system. The catastrophic result is obvious when the vehicle is approximately at the point (x=325, y=200) with respect to the reference coordinate system. The true path is shown by the dashed line. Notice that how much difference along both x and y-axes are made after the robot arrives to the end of the trajectory. Another reason of this failure might be due to the fact that EKF can not deal with the non-linearity of the observations. If for some reason the observation information is wrong, it will be embedded to the filter before fusing the data to the map and of course with no more considerations. Since EKF algorithm does not do any correction at this step, the wrong data will be one part of the map. If more wrong observations in the next steps happen, which is very much likely, the error accumulates and the EKF diverges. Figure 2-c indicates the same situation using RBPF. In this case, the non-Gaussian assumption is considered as well. Likewise, the filter performs a good estimation of the trajectory. This diagram is the result of 100 particles to observe a single landmark. For the robot position tracking, 50 particles are considered. This figure is a proof of RBPF success for a situation in which the system noise is not Gaussian. It is obvious that the data association is in no more concern. The result shows how well the RBPF deals with a situation in which EKF could not as indicated in figure 3-b. In Figures 3 and 4 the position and orientation estimation errors using both EKF with Gaussian and nonGaussian implication and RBPF with non-Gaussian implication for a non-linear system are compared. While the estimated position error in case of EKF-SLAM with nonGaussian assumptions increases more than 90 meters, the FastSLAM limits the position estimation error up to 1 meter and the average stays around 0.40 meters. This makes the result very much appropriate for the SLAM solution. Furthermore, while the average of orientation error in figure 4-b is around 0.025 radian, this average does not have a pick more than 0.015 radian at the beginning of the trajectory and

519

of particles receive non-negligible weights. Consequently there will be not many particles left to estimate the path at the next time step. Furthermore, as more landmarks are observed, many more particles are thrown out and the problem is more compounded. The subsequent observation makes the robot’s true position to lie within the overlaid probability, while the samples inside the ellipse will be duplicated multiple times by the re-sampling process. As the observations become more accurate, fewer unique samples will remain for the rest of process. Finally, sufficiently accurate observations will cause the particle filter to diverge. EKF-SLAM, Vehicle Estimated Path (Gausssian Implication) 500 450 400

350

y direction (m)

300

250 200

150 100 50

0

0

50

100

150

200

250 300 x direction (m)

350

400

450

500

400

450

500

400

450

500

(a) EKF-Slam With Non-Gaussian Implication 500

450 400

y direction (m)

350 300

250

200 150

100 True Path Dead Reckoning Estimated Path (PF)

50 0

0

50

100

150

200

250

300

350

x direction (m)

(b) Fast-SLAM with N-on-Gaussian Implication 500 450

400

350

y direction (m)

it goes down to the average of 0.005 radians for the rest of the trajectory and it gives the same promising result with the EKF-SLAM under Gaussian conditions. Result of FastSLAM performance versus EKF-SLAM in terms of increasing process control noise was investigated in this work. In this experiment, the root mean square position errors, for both filters, were analyzed. By increasing control noise the RMS position error of FastSLAM did not increase while this error increased substantially for EKF-SLAM and consequently caused the filter to diverge. The RMS error was computed for both filters over 10 different runs in the experiments with three different levels of odometric noise. Experiments showed that as number of incorporated particles increased, the RMS position error difference between EKF-SLAM under Gaussian condition and RBPF under non-Gaussian condition became less. It was obvious that with a number of 100 particles the difference between RMS position errors of both filters became reasonable. In other words, 100 particles suffice to make the accuracy of FastSLAM under non-Gaussian condition as much as EKFSLAM under Gaussian condition. The interesting fact was that FastSLAM with 100 particles required an order of magnitude fewer parameters than the EKF-SLAM in order to achieve this level of accuracy. This result suggests that only with a few hundred of particles in FastSLAM the level of accuracy is high enough and there is no need of relatively high number of particles involvement. Figures 4 and 5 show some part of an experiment in which a specific path was implemented with different observation noises for RBPF or FastSLAM. Number of particles in those simulations was considered 300. When the robotic vehicle was very noisy, as the measurement noise approached zero, the filter started diverging. This is an important disadvantage of a standard particle filter. If the sensor is very accurate which means its error is close to zero, and at the same time the robot’s motion is very noisy, the observation noise and the robot noise will not be good matches. The reason for this is that many particles will be thrown in the resampling step and there will be not enough particles left to be incorporated for the rest of the path estimation. Consequently, the filter diverges and the result will be catastrophic. This property that affects the performance of FastSLAM is called sample impoverishment. Another version of FastSLAM called FastSLAM 2.0 has been developed in [7] which addresses this defect and using more detailed resampling representation prevents the failure in this particular situation. In the following experiment the robot linear velocity noise has been 0.20, while the observation noise changes from 0 to 0.10. The Root Mean Square (RMS) position error and orientation error were plotted for the comparison on each different level of observation noise. As the level of observation noise became less, the Root Mean Square position error increased as well as the orientation error. It is important to mention that this situation happens only when the proposal and posterior distribution [7] are mismatched and this can be the result of a big difference between noises of robot’s motion and the sensor. The motion model spreads the particles out over a large space and only a small fraction

300

250

200

150 100 True Path Dead Reckoning Estimated Path

50

0

0

50

100

150

200

250 300 x direction (m)

350

(c) Figure 2: (a) Estimation of the trajectory using EKF with Gaussian implication. (b) Estimation of the trajectory using EKF with Non-Gaussian implication. (c) Estimation of the trajectory using RBPF with Non-Gaussian implication.

520

EKF-SLAM with Gaussian Implication

500

2.5

True Path Dead Reckoning Estimated Path (RBPF)

450 400

2

y direction (m)

Estimated position error (m)

350 1.5

1

300 250 200 150 100

0.5

50 0

0

500

1000

1500

2000

2500

3000

3500

4000

0

4500

0

50

100

150

200 250 300 x direction (m)

Time (s)

350

400

450

500

(a)

(a)

Position Error vs. Time

EKF-SLAM with Nom-Gaussian Implication

70

100 90

60 80

50 RMS Positon Error (m)

Estimated position error

70 60 50 40

30

40

30

20

20

10 10 0

0

500

1000

1500

2000

2500

3000

3500

4000

0

4500

0

500

1000

Time (s)

2500

3000

2500

3000

Orientation Error vs. Time

Fast-SLAM with Non-Gaussian Implication 1.4

0.7

1.2

0.6

1

0.5

Orientation Error (radians)

Position estimation error

2000

(b)

(b)

0.8

0.6

0.4

0.4

0.3

0.2

0.1

0.2

0

1500 Time (s)

0

500

1000

1500

2000 2500 Time (s)

3000

3500

4000

0

4500

0

500

1000

1500 Time (s)

2000

(c)

(c) Figure 3: (a) Position estimation error of the trajectory using EKF with Gaussian implication. (b) Position estimation error of the trajectory using EKF with Non-Gaussian implication. (c) Position estimation error of the trajectory using RBPF with Non-Gaussian implication.

Figure 4: Simulated path with 300 particles. The velocity noise is 0.20. The observation noise is 0.10. (a)-Path of the robot. (b)-Root Mean Square of the position error. (c)- Orientation error.

521

Carlo method and localization by implementation of RaoBlackwellised particle filtering (RBPF) was presented through some simulation results for the same scenario. In that algorithm, the noise did not have to be Gaussian. This alternative algorithm computes independent landmark estimates conditioned on each particle in a Dynamic Bayesian Network. Since landmarks in this network are mutually independent, there is no correlation between any pairs of landmarks in the map. Multiple hypothesis data association allows taking uncertainties into account. Referring to experimental results, FastSLAM is very suitable for non-linear motions and under non-Gaussian Conditions.

500 True Path Dead Reckoning Estimated Path (RBPF)

450 400

y direction (m)

350 300 250 200 150 100 50 0

0

50

100

150

200 250 300 x direction (m)

350

400

450

500

(a) Position Error vs. Time 100

ACKNOWLEDGMENT

90

RMS Positon Error (m)

80

The authors would like to acknowledge their use of the software code available for public from CNRS LASMEA at University Blaise Pascal. The software in form of MATLAB and C++ codes is available at http://www.lasmea.univbpclermont.fr

70 60 50 40 30 20

REFERENCES

10 0

0

500

1000

1500 Time (s)

2000

2500

3000

[1]

(b) Orientation Error vs. Time

[2]

0.7

Orientation Error (radians)

0.6

[3]

0.5

0.4

[4]

0.3

0.2

[5]

0.1

0

0

500

1000

1500 Time (s)

2000

2500

[6]

3000

(c) Figure 5: Simulated path with 300 particles. The velocity noise is 0.20. The observation noise is 0.075. (a)-Path of the robot. (b)-Root Mean Square of the position error. (c)- Orientation error.

IV.

CONCLUSIONS

[7] [8]

In this paper, an autonomous robot navigation problem base on Bayesian approach was presented. An alternative algorithm called FastSlam was presented; Implementation of a classical solution based on Extended Kalman Filter (EKF) was shown and discussed under Gaussian assumptions of such system. EKF is limited to environments having a few hundred landmarks since at each time step every two pairs of landmarks become correlated. Therefore, for environments with thousands of landmarks, such algorithm diverges due to high rigidity of the map. EKF does not have any mechanism to correct landmarks uncertainty due to its single hypothesis data association property. The shortcomings of EKF-SLAM algorithm for a specific case were shown and compared with FastSLAM algorithm. The alternative algorithm called FastSLAM based on Monte 522

[9]

[10]

[11]

[12]

J. Z. Sasiadek, P. Hartana, “Sensor Fusion for dead-reckoning mobile robot navigation”, Proceeding of the 6th IFAC Symposium on Robot Control, pp. 531-536, SYROCO, 2000. H. Durrant-Whyte, T. Bailey, “Simultaneous localization and mapping (SLAM): part I essential algorithms”, IEEE Robotics and Automation Mag. Vol 13, No. 2, pp. 99-108, June, 2006. H. Durrant-Whyte, T. Bailey, “Simultaneous localization and mapping (SLAM): part II state of the art”, IEEE Robotics and Automation Magazine. Vol 13, No. 3, pp. 108-117, Sept., 2006. S. Thrun, d. Fox, W. Burgard, “A probabilistic approach to concurrent mapping and localization for mobile robots”, Machine Learning 31, pp. 29-53 and Autonomous Robots 5, pp. 253-271, (joint issue), 1998. S. Thrun, “Probabilistic algorithms in robotics”, AI Magazine 21, 4, pp. 93-109, 2000.. M. W. M. G. Dissanayake, P. Newman, S. Clark, H. DurrantWhyte, M. Csorba, “A solution to the simultaneous localization and map Building (SLAM) problem”, IEEE Trans. on Robotics and Automation. Vol 17, No. 3, pp. 229-241, June, 2001. M. Montemerlo, S. Thrun, D. Koller, B. Wegbreit, “FastSLAM 2.0: An improved particle filtering algorithm for simultaneous localization and mapping that converges”, Proc. of IJCAI, 2003. S. Thrun, M. Montemerlo, D. Koller, B. Wegbreit, J. Nieto, E. Nebot, “FastSLAM: an efficient solution to the simultaneous localization and mapping problem with unknown data association”, Journal of Machine Learning Research, 2004. S. Thrun, D. Fox, W. Burgard, “A real-time algorithm for mobile robot mapping with applications to multi-robot and 3D mapping”, IEEE International Conference on Robotics and Automation, San Francisco, USA, April, 2000. M. Montemerlo, S. Thrun, D. Koller, B. Wegbreit, “FastSLAM: A factored solution to the simultaneous localization and mapping problem”, Proceedings of AAAI National Conference of Artificial Intelligence, 2002. J. Z. Sasiadek, Q. Wang, “Sensor fusion based on fuzzy Kalman filtering for autonomous robot vehicle”, Proc. of the IFAC Workshop on Mobile Robot Technology (J. Z. Sasiadek (Ed)), pp. 251-256, Pergamon, Oxford, 2001. J. E. Guivant, E. M. Nebot, S. Baiker, “Localization and map building using laser range sensors in outdoor applications”, Journal of Robotic Systems, 17(10): pp. 565-583, 2000.