Robust Landmark Estimation for SLAM in Dynamic Outdoor Environment Atsushi SAKAI, Teppei SAITOH and Yoji KURODA Meiji University, Department of Mechanical Engineering, 1-1-1 Higashimita, Tama-ku, Kawasaki, Kanagawa, Japan Email: [email protected] Abstract— In this paper, we propose techniques which make SLAM accurate and practical in dynamic outdoor environment. In order to achieve the objective, stable feature detection by a laser range finder and efficient data management are introduced. The stable feature detection is used to select static and characteristic landmarks, it is possible to estimate every position of landmark accurately in the dynamic environment. The data management is introduced in order to decrease computational time and localization error of SLAM. Furthermore, for estimating landmark and robot’s position accurately, unscented transformation based sampling technique is also adopted. As a result, it is shown that the computing time and the maximum position error of SLAM decreases by 80% compared with typical landmark estimation, and position error of SLAM with unscented transformation becomes more accurate compared with FastSLAM2.0 in the dynamic outdoor environment. Index Terms— FastSLAM, Landmark estimation, Data association, Unscented Transformation.

I. INTRODUCTION N recent years, NASA’s Mars Exploration Rovers (MERs) carried out various scientific discoveries in the surface of the Mars [1]. In the future, robots will operate in various environments, not only other planets but also such as undersea, underground, and so on. An important problem at the time of a exploration by a robot is estimation of localization and map. In the environment (e.g. other planets,underground), it is difficult to acquire globally accurate position (e.g. GPS) and information of ambient surrounding. In order for a robot to operate autonomously in the environment, it is required that technologys of the estimation of robot’s position and map accurately. The Simultaneous Localization and Mapping, also known as SLAM, has gotten a lot of attention in the mobile robotics literature [2]. The problem of SLAM is building a map of an environment from landmark measurements obtained from a moving robot. But a robot motion involves error, the mapping problem must induce a robot localization problem. To solve this problems, various solutions to the SLAM problem have been studied, such as using the Extended Kalman Filter (EKF), the information filter, and the Graph SLAM [3][4][5]. In these algorithms, FastSLAM algorithm is one of the algorithms which succeed at doing accurate SLAM in outdoor environments[6]. The FastSLAM algorithm is a solution to stochastic SLAM that is based on a particle filter to approximate the ideal recursive the Bayesian filter. FastSLAM algorithm has two advantages: First, the timecomplexity of SLAM is small;hence the appelation ”Fast”.

I

Because FastSLAM is separated the mapping problem and the localization problem. Second, FastSLAM algorithm is robust to failure of data association. If observations are associated correctly in some particles and incorrectly the others, the incorrect particles will receive lower importance weight and being removed in future re-sampling step. Because of these advantages, FastSLAM made precise and robust SLAM possible. Besides, in order to do SLAM with more accurate, the new algorithm FastSLAM2.0 was proposed as developed alogorithm of FastSLAM [7]. In FastSLAM2.0, a proposal distribution relies not only on the motion estimation (as is the case in FastSLAM), but also on the most recent sensor measurement when proposing a new robot pose. The technique enable to estimate the true distribution. Alternatively, because the sampling accuracy is improved by this technique, the SLAM can be calculated with a few particles. The timecomplexity of SLAM is more small than using FastSLAM. FastSLAM2.0 can do SLAM with high accuracy and highspeed. However, using FastSLAM algorithm, it is difficult to excute SLAM with high accuracy and high speed in the environment where complicated like outdoor environment and have two or more moving objects. For example, failures of data association increase in outdoor environment compared with indoor environment. This is attributed to spurious landmark from sensor noise and dynamic objects. For this reason, error of localization and map increase, then the calculation of SLAM may diverge. Besides, it is known that calculation time of FastSLAM increase logarithmically observed landmarks increase [6]. The more a robot operate a long distance, the more the calculation cost of SLAM increase. In consequence, the SLAM can not be done on the fly. For these reason, with holding only static and characteristic landmarks on a map, failure of data association must be prevented and the calculation time must be reduced. Moreover, Localization and mapping are the same problems in SLAM. In order to create a more accurate map, we have to estimate robot’s position accurately. The particle filter in the FastSLAM relies on the importance sampling. Therefore, the FastSLAM requires the design of proposal distributions that can approximate the ture posterior reasonably well. FastSLAM2.0 algorithm samples using the formula of EKF so that particles are sampled with high accurately. Although, there are problems in it: The first is that, in EKF, a nonlinear model is approximated with Jacobian matrices.

If the nonlinearity of the model is high, the accuracy of approximation becomes bad. The second is that we have to develop the expression for Jacobian matrices of motion and observation models. However, this derivation is difficult when each model is complicated. Uhlmann et al proposed the Unscented filter as an alternative to the EKF for recursive state estimation [8]. This approach is used to avoid the analytical linerization based on Taylor series expansion of both the motion and the measurement model. Merwe et al introduced the Unscented Particle Filter (UPF) algorithm [9]. This algorithm uses unscented transformation for a proposal distribution in the sampling step of a particle filter. Because of using unscented transform, they enabled to do precise estimations in the nonlinear models. In this paper, we propose a set of methods of landmark estimation for high-speed and accurately SLAM on the basis of FastSLAM algorithm. To estimate a landmark accurately and to decrease the time-complexity of SLAM, stable feature detection and data management are important. We introduce that stable feature detection with a laser range finder and efficient data management. The feature detection and the data management involved three techniques respectively. Firstly, we propose following methods in the feature detection: 1) Removing dynamic objects and sensor noise with difference processing 2) Detecting feature points sparsely and evenly 3) Adjusting a threshold value of data association with a landmark density Taking in these techniques, we show that detecting feature points becomes stable, then these feature points make easy to do data association. Secondly, we also propose these methods in the data management: 1) Calculating landmark’s existence probability 2) Using the landmark exclusivity for data association 3) Predicting importance weights with observation range Because of these techniques, we show that the computational time and a position error are decreased and data association becomes robust. Furthermore, we take in a sampling method using unscented transformation. Because of introducing it and the landmark estimation we proposed, we show that the our SLAM is more accurate than FastSLAM2.0 algorithm. II. FAST SLAM A LGORITHM SLAM is the problem of determining robot poses xt and the position of all landmarks θ from measurements zt = z1 , · · · · · · , zt and robot’s controls ut = u1 , · · · · · · , ut . In probabilistic terms, this is expressed by the posterior p(xt , θ | zt , ut ), where we use the superscript to refer to a set of variables from time 1 to time t. If the correspondences of landmark each time are known, the SLAM posterior is : p(xt , θ | zt , ut , nt ) where nt is the correspondence variables.

(1)

FastSLAM algorithm divides the problem of localization and mapping, and calculates each probability. The posterior (1) be factored as follows: p(xt , θ |zt , ut , nt ) p(xt |zt , ut , nt )

=

N ∏

p(θk |xt , ut , zt , nt )(2)

k=1

where k is a number of each landmarks, N is a total of landmarks. The FastSLAM algorithm implements the robot’s position estimator p(xt |zt , ut , nt ) using a particle filter, and each landmark position estimators p(θk |xt , ut , zt , nt ) using low dimension Kalman filters. Dividing problems of localization and mapping, the FastSLAM algorithm makes decreasing the time-complexity possible. Furthermore, each particle in the particle filter has its own local map, FastSLAM is robust to a failure of data association. FastSLAM2.0 was proposed as development algorithm of FastSLAM. In FastSLAM2.0 algorithm, robot’s poses are sampled under a consideration of both the motion ut and the measurement zt . The i-th particle is sampled by a following probability expression: [i]

[i]

xt ∼ p(xt |xt , zt , ut , θ[i] )

(3)

[i]

where xt is the variable which is the i-th particle’s state, θ[k] is the variable that the i-th particle has the landmark map. The accuracy of localization improved by the use of this technique. Because of it, the number of particles could be held down to a small number, and the time-complexity of FastSLAM2.0 is small than FastSLAM. III. L ANDMARK ESTIMATION In landmark based SLAM which uses LRF, the feature detection and the data association are very important problems. In this paper, we propose a set of techniques to solve these problems effectively. A. Feature detection In FastSLAM, a feature point are served as a candidate of a landmark. A camera can detect feature points using a color distribution of a picture. However, a LRF can detect a feature point only using distance information between a robot and obstacles. This problem makes data association with LRF adversity. In this paper, we use the extremum of the observation value of LRF as feature points. This section proposes three techniques of the feature detection for doing it stably. 1) Removing dynamic objects and sensor noise with difference processing: For accurate SLAM, it is necessary to remove dynamic objects and sensor noise from a measurement data of LRF. If they are reflected into a map, the timecomplexity of SLAM and an error of map estimation would increase. We presents a technique of removing dynamic objects and sensor noise to piling up the LRF measurement data for two times. With a curent observation value and a previous, each observations on same angle are assciated. Then, a distance

between the couple of accosiated observations is calculated. If the distance longer than a threshold distance determined in advance, it is removed. Because of this method, dynamic objects and sensor noises are removed. 2) Detecting feature points sparsely and evenly: We use Maximum Likelihood Estimation (MLE) for data association [10]. MLE selects data associations by maximizing a importance weight of each landmarks. The importance weight is calculated from the Mahalanobis distance between a observation and a landmark, and the function of Gaussians distribution: 1 1 ˆj )) πj = |2πQj |− 2 exp(− (zt − zˆj )t Q−1 j (zt − z 2

(4)

where, the variable πj is importance weight, Qj is covariance of landmark position, zˆj is predicted observation value, and superscript j is a index of landmarks. Importance weights are calculated by (4) with respect to each observation. The landmark which has the most biggest importance weight in all landmarks is associated the observation. If all importance weights to all landmarks are below a threshold determined in advance, it is judged that the feature point is a new landmark. MLE associates individually all landmarks on each feature point. Therefore, it is one of the robust data association methods in the outdoor environment where a dynamic object exists. However, because data association of MLE uses Mahalanobis distance between each landmarks and a observation value, it tends to go wrong assciation where landmarks are dense. We propose that it makes a distance of feature points already detected into the conditions on the feature detection. If the distance between a observation and the most nearest feature is longer than the threshold value, the observation is not selected as the feature point. Put simply, in a adjacent place where a lot of feature are detected, it makes features detected sparsely. In a adjacent place where a few feature detected, it makes features detected thickly. Because of this, feature points are detected sparsely and evenly, a failure of data association decreases. 3) Adjusting a threshold value of data association with a landmark density: In data association using the distance between a observation and a landmark, it is necessary to adjust a threshold of data association with a landmark density. For example, when distance is long between one landmark and another landmark which is the nearest it, only the object exists around there. That is, the landmark should be associated even if some observation errors arise. On the contrary, in the place where landmarks exists closely, the failure of data association may be occured. In the situation like thies, the data association should be done when the association is really absolute. In this paper, we propose a method that a threshold value of data association is adjusted with the distance between two adjacent landmarks. The threshold value is calculated by following formula: Mth = Mc − K·dmin

(5)

where the variable Mc is basic threshold of importance weight, dmin is the distance between two adjacent landmarks, and K is a constant. The threshold value is adjusted with respect to each observations. B. Data Management In this paper, three techniques are used for efficient data association. 1) landmark’s existence probability: Failures of feature detection resulting from sensor noise or complicated environment becomes a problem in outdoor environment. Therefore, even if it is used methods of feature detection which described in the chapter 3-A, it could not prevent completely spurious features from geting mixed in the map. To solve the problem, we always have to execute the SLAM with removing spurious landmarks. Montemerlo et al calculated the probability of the landmark’s existence using available r of two values to all the landmarks that each particles hold [10]: [k]

Γ = log

p(r|zt , xt ) [k]

1 − p(r|zt , xt )

(6)

where the variable Γ the probability of the landmark’s existence, it is updated in each observation. When the probability becomes below a threshold, it judges that the landmark is a spurious, and removes it from a map. By this, only a true landmark could be held on the map, and the number of handing landmarks is decrease. As a result, the timecomplexity becomes small and failure of data association decreases. 2) Using the landmark exclusivity for data association: When features are detected sparsely with the technique mentioned in the chapter 3.A.2, it can be figured out that each feature points are individual objects. Therefore, several observations which observed in the same time should not be associated simultaneously with one landmark. In this paper, when some observations are identified as one landmark by MLE, the observation of the highest importance weight is only associated, and other observations are judged as a new landmarks. As a result, failure of data association could be prevented. 3) Predicting importance weights with observation range: In order to deal with many landmarks on the fly, the timecomplexity has to be reduced. However, using MLE for data association, importance weights are calculated to all landmarks in a map. The more the number of landmarks in a map increases, the more calculation increases. Especially, it is well known that the calculation time increases logarithmically in proportion to the number of observed landmarks on FastSLAM algorithm [6]. Hence, the calculation time of importance weight is needed to be kept down much further. We propose that the obsevation range which is determined preliminarily is used for decresing the computational time. Observable landmarks at the next step is presumued from the information of robot’s position and its observable range. And then, it is assumed that landmarks which are out of the range may not be observed, the landmark’s importance weights are

120

100

100

80

80

Y Displacement (m)

Y Displacement (m)

120

60 40 Ground Truth SLAM Landmark Estimated Landmark

20 0 −20 −20

0

20 40 60 80 X Displacement (m)

100

The particle filter in the FastSLAM relies on the importance sampling and, as a result, requires the design of proposal distributions that can approximate the true posterior reasonably well. In this paper, the sampling technique using the unscented transformation which Merwe et al proposed is taken in [8]. The unscented transformation is a method for calculating the statistics of a random variable which undergoes a nonlinear transformation. It builds on the principle that it is easier to approximate a probability distribution than an arbitrary nonlinear function. The procedure which is to estimate mean and covariance with unscented transformation is following: Firstly, weighted samples, called sigma-points, are deterministically chosen. Secondly, each sigma points is propagated through the nonlinear function. At the end, mean and covariance are estimated with these points. Equation (7) is the formula of choosing sigma-points, (8) is the formula of calculating weights to sigma-points.

(0) ωm

ωc(0) (i) ωm

0

20 40 60 80 X Displacement (m)

100

120

Simulation results of SLAM

IV. S AMPLING USING UNSCENTED TRANSFORMATION

x(i)

Ground Truth SLAM Landmark Estimated Landmark

(B) Proposed landmark estimation

not calculated. Thus, the futile calculation of the importance weights would not be excuted.

x

20

−20 −20

120

Fig. 1.

(i)

40

0

(A) Normal landmark estimation

x(0)

60

= x ˆ

√ = x ˆ + ( (N + λ)P ) i = 1, ....., N (7) √ = x ˆ − ( (N + λ)P ) i = N + 1, ....., 2N λ = n+λ λ + (1 − α + β) (8) = n+λ 1 = ωc(i) = i = 1, ....., 2N 2(n + λ)

where x ˆ is the mean of robot’s position on previous step, λ = α2 (n + κ) − n, n is state dimension, α, κ are scaling parameter of choosing sigma-points, and β is a parameter which minimizes the effects from high order terms.

Comparing with EKF, the estimation with the unscented transformation does not need to develop the expression for Jacobian matrixes of models. This is a big advantage when using a complicated model. Additionally, as proved in the previous researches, the estimation of the mean and covariance are accurate to the third order of the Taylor series expansion of any nonlinear function, whereas the EKF estimation has a first order [13]. V. S IMULATION R ESULTS SLAM simulations was performed, which used our landmark estimation . These simulations was done by MATLAB, the algorithm of SLAM was FastSLAM2.0 algorithm. MLE was adopted for data association. This simulator makes the noise of observation and input with respect to Gaussian distribution. Two kinds of simulations were done, which the one used the normal landmark estimation, the another took in our landmark estimation method. Same parameters(e.g. noise, robot’s speed, etc) were used in both simulation. Fig. 1 shows the results of these simulations. (A) is result of the simulation using the normal landmark estimation, (B) is result of simulation using our landmark estimation. These figures show estimated trajectory of SLAM, Ground truth, true landmark position, and landmark position. Fig. 2 shows the position error of these simulations as a function of time. The figure indicates that our method allows maximum error of position to decrease by 80%. Because our methods makes possible to hold only true landmarks so that spurious landmarks were removed. As a result, the true landmark is updated frequently, the position of the landmark is converged in the true position. Meanwhile, in normal estimation, because of not to remove spurious landmarks, error of the map increase as the failure of data association. Table I compares the number of landmarks at the end of the simulation and the computational time. These simulations was done by the system that the CPU is Intel Core2 Duo 2.33GHz, the RAM is 3.25GB. Our method removed the

Fig. 2.

Fig. 3.

Position Error

Infant

TABLE I C OMPUTATIONAL T IME

Normal Estimation Proposed Estimation

Number of Landmark 627 213

Computational Time 643 109

spurious landmark, so that the number of landmarks treated was reduced by more than 60%. As a result, the computational time was reduced by more than 80%. Fig. 4.

VI. R ESULTS OF SLAM

Experiment field

IN OUTDOOR ENVIRONMENT

We simulated feature detection and SLAM with a set of sensor data that were taken in real outdoor environment. The experimental field were Tsukuba central park, Ibaraki. There were a number of other team ’s robots and many passers in this driving course (Fig. 4). This experimental field had objects static and dynamic objects. Additionally, This environment had many objects that are complicated shapes (e.g. grass, shrubbery, bicycle, building). Infant, which is our system for research of autonomic mobile robot (Fig. 3) and Table II is it’s specification, was used for getting the sensor data. The experiments are performed with the LRF of TOPURG UTM-X001S made in Hokuyo denki co, the sensor samples data at 10 Hz, and the robot was traveling at about 0.5 m/s.

TABLE II S PECIFICATION OF I NFANT Parameters [Unit] Length [m] Width [m] Height [m] Tread [m] Wheel base [m] Climbable angle [m] Weight [m] Radius of wheel [m] Maximum speed [km/h] Range of continuous drive [km]

Value 0.9 0.6 1.1 0.5 0.45 4.9 60 0.125 7 20

A. Feature detection

B. Results of SLAM We simulated two SLAM algorithms, which the one was FastSLAM2.0 with our landmark estimation, the another was the one innovated the unscented transformation. These

15 Robot Position LRF Observations Feature points

10 Y Displacement (m)

Fig. 5 shows detecting feature points with our methods stated in chapter 3-A. Fig. 5 also shows robot’s position and LRF observations. This shows these features were detected sparsely. In this simulation, the threshold of the least distance between a observation and the most nearest feature were 10cm. As a result, feature points were detected with the more distance than 10cm each other. Moreover, Fig. 5 shows dynamic objects are not detected with the difference processing stated chapter in 3.A.1. These features were used for the our SLAM.

5 Dynamic Objects

0

−5

−10 −5

0

5 10 15 X Displacement (m)

Fig. 5.

Feature detection

20

25

80

80

Y Displacement (m)

60 40 20 0 −20 −40

Ground Truth Unscented SLAM Odometry Estimated Landmark

60 Y Displacement (m)

Ground Truth FastSLAM2.0 Odometry Estimated Landmark

40 20 0 −20

0

20

40

60 80 100 120 X Displacement (m)

140

160

(A) FastSLAM2.0 Fig. 6.

0

20

40

60 80 100 120 X Displacement (m)

140

160

(B) Unscented FastSLAM Results of SLAM in dynamic outdoor environment

SLAM simulations only used sensor data of wheel odometry and LRF observations. Fig. 6 is results of SLAM simulations. (A) is the result of SLAM with FastSLAM2.0, (B) is the result of our SLAM with the sampling method of unscented transformation. These figures show the trajectory of wheel odometry, SLAM, Ground truth, and the feature map created with SLAM. Both results shows that the estimation of robot’s position is more accurately than the trajectory of odometry. This is attributed to ourlandmark estimation methods, it enabled to construct map stably and accurately in dynamic environment. Finally, the result of SLAM using unscented transformation was compared with the result of SLAM using FastSLAM2.0. Fig. 6 shows that the SLAM using unscented transformation is more accurate about localization than FastSLAM2.0. Because the using unscented transformation is more accurate than the use of EKF in the aspect of approximation accuracy of nonlinear function. For this reason, it was shown that the accurately of localization was improved by introducing the unscented transformation. VII. C ONCLUSION In this paper, we proposed the landmark estimation for high-speed and accurate SLAM. In landmark estimation, three techniques were taken in respectively in the category of the feature detection and the data management. In the feature detection, removing dynamic objects and sensor noise with difference processing, detecting feature points sparsely and evenly, and adjusting a threshold value of data association with a landmark density, were introduced. By using these techniques, it made possible to detect feature point stably in outdoor environment, we showed that accurate SLAM became possible under the complicated and dynamic environment. In the data management, calculating landmark’s existence probability, using the landmark exclusivity for data association, and predicting a importance weight with observation range, are taken in. These techniques made it possible to decrease the time-complexity of SLAM. We

showed that our method allowed maximum error of position estimation and computational time to decrease by 80%. We also proposed the sampling method using unscented transformation. With this method and our landmark estimation, we showed the our SLAM was more accurate than FastSLAM2.0. R EFERENCES [1] Li, R., K. Di, L.H. Matthies, R.E. Arvidson, W.M. Folkner and B.A. Archinal.“ Rover Localization and Landing Site Mapping Technology for 2003 Mars Exploration Rover Mission. ” Journal of Photogrammetric Engineering and Remote Sensing, Vol.70(1): 77-90, 2003. [2] H. Durrant-Whyte and T. Bailey, “ Simultaneous Localisation and Mapping (SLAM): Part I The Essential Algorithms, ” Robotics and Automation Magazine, vol. 13, pp. 99?110, 2006. [3] G. Dissanayake, P. Newman, S. Clark, H.F. Durrant-Whyte,and M. Csorba. A solution to the simultaneous localisation and map building (SLAM) problem. IEEE Transactions of Robotics and Automation, 2001. [4] Y. Liu and S. Thrun. Results for outdoor-SLAM using sparse extended information filters. Submitted to ICRA-03. [5] S. Thrun and M. Montemerlo, “ The graphslam algorithm with applications to large-scale mapping of urban structures, ” IJRR, vol. 25. [6] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit. Fast-SLAM: A factored solution to the simultaneous localization andmapping problem. Proc. AAAI, 2002. [7] M. Montemerlo, S. Thrun D. Koller, and B. Wegbreit. FastSLAM 2.0: An improved particle filtering algorithm for simultaneous localization and mapping that provably converges. In Proc. of the Int. Conf. on Articial Intelligence (IJCAI), 2003. [8] S. J. Julier, J. K. Uhlmann, and H. F. Durrant-Whyte, “ A new approach for filtering nonlinear systems, ”in Proc. Amer. Contr. Conf., Seattle,WA, June 1995, pp. 1628?1632. [9] R. Merwe, A. Doucet, N. Freitas and E. Wan,“ The Unscented Particle Filter ”, Technical Report CUED/F-INFENG/TR 380, Cambridge University Engineering Department, 2000. [10] Avitzour, D. (1992) A maximum likelihood approach to data association.IEEE Transactions on Aerospace and Electronics Systems, 28, 2 (Apr. 1992), 560?565. [11] M.Montemerlo,S.Thrun,Simultaneous localization and mapping with unknown data association using FastSLAM.Proc. ICRA, 2003. [12] S. Thrun, W. Burgard, D. Fox, Probabilistic Robotics, MIT Press, 2005. [13] C.Kim, R.Sakthivel, and W.K.Chung, Unscented FastSLAM: A robust algorithm for the simultaneous localization and mapping problem, in Proc. IEEE Int. Conf. Robot. Autom., 2007, pp. 2439-2445.