Relative-Absolute Map Filter for Simultaneous Localization and Mapping Shu Yun Chung

Han Pang Huang

Department of Mechanical Engineering National Taiwan University Taipei, Taiwan [email protected]

Department of Mechanical Engineering National Taiwan University Taipei, Taiwan [email protected]

AbstractIn this paper, a new algorithm, Relative-Absolute Map Filter (RAMF), is proposed to solve the simultaneous localization and mapping problem. Compared with FastSLAM, which adopts many absolute maps to describe the relationship between features, RAMF utilizes only one relative map instead. By fusing the information of relative map and absolute map, RAMF can create a more accurate map. Moreover, the embedded particle filter in RAMF can handle robot localization. Simulation results show that RAMF has better performance than FastSLAM and UKF SLAM in the noisy robot motion. Index Terms - SLAM, relative map filter (RMF), absolute map filter (AMF), Particle Filter (PF).

I.

INTRODUCTION

In the last two decades, Simultaneous Localization and Mapping (SLAM) becomes one of the most popular topics in the mobile robot. SLAM requires a mobile robot to explore the environment and localize its position by on-board sensors. The principal difficulties of SLAM come from two parts. One is a sequence of noisy landmark measurements obtained from a moving robot, the other is the uncertainty of robot motion. Both of them cause a deformed map and blurred localization. That is why it is always required to handle the localization and mapping “simultaneously.” Many approaches have been proposed in different views and structures for a SLAM problem. Among those methods, the stochastic map approach is widely adopted. It presents the states of the robot and landmarks in statistics and uses Kalman Filter for recursive estimation. According to the characteristics of estimated states, the stochastic map approach can be roughly classified into two types: absolute map filter (AMF) and relative map filter (RMF). Although most researches were focused on AMF and RMF in the past, they are limited to specific environment conditions. In this paper, we combine the concept of AMF and RMF to form a new approach called RAMF (Relative-Absolute Map Filter). FastSLAM adopts many absolute maps to describe the relationship between features, while the proposed RAMF utilizes only one relative map instead. From the simulation results, it will show that RAMF has the characteristics of higher accuracy

and robust mapping. To make the idea of RAMF clearly, the comparison between AMF and RMF will be discussed in section II. The structure of RAMF will be presented in section III, and its details will be given in section IV. The simulation results and comparison with UKF and FastSLAM will be shown in section V. Finally, conclusions are given in section VI. II.

COMPARISON BETWEEN AMF AND RMF

The AMF was first presented by Smith [15]. This paper adopted EKF (Extended Kalman Filter) to incrementally estimate the posterior distribution over the robot pose along with the positions of landmarks. In the last decades, this approach gets widely accepted in the field of mobile robots. Until now, EKF-based approach is still the dominant method for the SLAM problem. But EKF-based approaches suffer from two important limitations, quadratic complexity and sensitivity to failures in data association. The quadratic complexity stems from the update stage in the EKF. It required O( K 2 ) updated computation even if just a single landmark is observed. It limits the number of landmarks that can be handled by EKF. Moreover, EKF does not have the ability to present robust data association. One incorrect association can result in map divergence. To overcome those two limitations, lots of papers were proposed in different architectures and modifications. [1][5]. One of the most famous approaches to deal with these two disadvantages is the FastSLAM, which was proposed by Montemerlo and Thrun in 2003 [9][10]. FastSLAM adopts the Rao-Blackwellized particle filter as the principal architecture [11]. It decomposes the SLAM problem into a robot localization problem and a collection of landmark estimation problem. The experimental results in [9][10] showed that FastSLAM outperformed the traditional EKF both in computation and data association. Compared with AMF, RMF estimates the relation between the landmarks and presents the landmarks in relative state (relative distance, relative angle), as shown in Fig. 1. Recently, a lot of researches have been carried out on studying the relative map approach. Csorba [2] was the first one to introduce the concept of relative map. He used Kalman Filter to estimate the invariant features of the environment. Since the features are invariant to the robot

pose, RMF can completely decouple the robot pose error from the map accuracy. It is also the most famous advantages of RMF. However, RMF in [2] induced an inconsistent relative map because the correlations between the relative features were ignored. In order to solve the problem, Newman [12] proposed Geometric Projection Filter (GPF) to create a consistent relative map by solving a set of linear constraints. A nonlinear version of GPF was developed by Dean [3]. Data association is another difficulty in the SLAM problem. Although the RMF can produce a high accuracy map without considering the robot motion, it is hard to handle the data association in the relative map. In general, RMF will consider the invariant features, such as distances and angles, in the matching step. Since angles cost more computation for recognition, most people choose to match the relative distances only [12]. However, it is not robust enough, especially in the environment with similar geometric characteristics. In [14], a triangle area matching method was proposed to increase the robustness of data association in RMF. But it still can not perform well for a similar geometric environment. In addition, the relative map still needs to be transformed into the absolute map to represent the global map. However, the transformation errors and inaccuracies depend upon the sequences of the relative transforms [13]. One possible solution to handle all the problems in AMF and RMF is to combine both of them, and gets the information of relative map and absolute map simultaneously. To achieve this goal, the RAMF is proposed in this paper. It manages and fuses the information of both relative map and absolute map. In this way, the RMF and AMF can share the useful information and update the deficient information. III.

pose and absolute map can be available. The detail for UGPF will be discussed in the next section. In RMF, Kalman Filter is adopted for the estimation of relative states. To speed up the calculation, we only choose distances as relative states. Furthermore, the data association of the relative map can be easily handled by the information fused in the RAMF. The details will be listed in the next section. Finally, the fusion of AMF and RMF will be completed by RAMF. The UKF is used to combine the information and produces a new absolute map. B.

Basic Idea in RAMF In general, a robot can extracts two different kinds of information by its external perception. One is absolute information, the other is relative information (Fig. 3(a)). By the intrinsic limitations of sensors, the relative information is sometimes derived from the absolute information. EKF-based SLAM adopts absolute information to estimate the posterior over the robot pose and map (Fig. 3(b)). However, it suffers from two well-known problems, quadratic complexity and sensitivity to failures in data association. f1 Absolute landmarks

fr(1,2) f2

robot Fig. 1 Relationship between absolute landmarks and relative states.

robot pose, global absolute map

RELATIVE-ABSOLUTE MAP FILTER (RAMF)

Structure of RAMF The RAMF, which is like a bridge, connects the RMF and the AMF. It manages the information and gets the higher accuracy by fusing RMF with AMF. After fusion, AMF can obtain the information of relative states while RMF gets the data association. RAMF uses the information to predict and estimate the states of the robot and landmarks in the next step. The architecture of RAMF is shown in Fig. 2. RAMF integrates AMF with RMF. The AMF provides the information of the robot location and absolute map, while the RMF gives the information of the relative map. In addition, an Unscented Gaussian Particle Filter (UGPF) is built in the AMF for the robust localization and absolute map reconstruction in this paper. UGPF, which consists of GPF (Gaussian Particle Filter) and UKF, is a modification of Rao-Blackwellization Particle Filter (RBPF). By combining Particle filter and UKF, the more reliable robot

Relative state (distance)

A.

RAMF UKF Fuse absolute map and relative map Invariant feature

Robot localization absolute map

Data association

Relative map

AMF

RMF

UGPF Kalman Filter

GPF Absolute map

Robot localization

Relative map

UKF Fig. 2 Architecture of RAMF

In the FastSLAM, the Bao-Blackwellization Particle Filter is used to improve those deficits in EKF. Every particle indicates one possible robot pose and is utilized to describe one absolute map (Fig. 3(c)). Thus, there are many absolute maps to be created in the FastSLAM. In other words, FastSLAM achieves robustness by fusing the maps. A common way to provide high accuracy in FastSLAM is to increase the number of particles at the expense of increasing computation time. Comparison with FastSLAM, RAMF only requires one relative map and one absolute map to describe the relationship between landmarks (Fig. 3(d)). No matter how many possible robot poses are introduced, there always exists one relative map. The robot pose is completely unrelated to the relative map. Besides, the relative map can enhance data association, and RAMF still keeps the absolute information to create one absolute map. By combination of relative map and absolute map, RAMF can handle the SLAM problem efficiently. IV.

RELATIVE MAP FILTER (RMF) AND ABSOLUTE MAP FILTER (AMF) IN RAMF

A.

Unscented Gaussian Particle Filter (UGPF) Montemerlo [9] demonstrated that a good absolute map can be reconstructed if we can fetch good estimation for the robot pose. In other words, the core to reconstruct a reliable absolute map is the robot localization problem. To realize this idea, Montemerlo proposed the FastSLAM algorithm. It turns out to be an AMF. FastSLAM is based on Rao-Blackwellization Particle Filter (RBPF) which uses the particle filter to estimate robot pose and EKF for the landmarks. Since it is assumed that all the landmarks are conditionally independent by given robot path, the computation for update can be constant. The experiment results [9][10] showed that FastSLAM performed well in the situations in which EKF inevitably diverges. In order to achieve robust localization and mapping in the absolute map, we adopt an absolute map filter, Unscented Gaussian Particle Filter (UGPF). Similar to the RBPF, UGPF partitions the SLAM problem into robot localization and landmark estimation. Robot localization is handled by Gaussian Particle Filter (GPF) which was proposed in [6]. UKF is adopted for landmarks estimation (mapping). In comparison with RBPF, which uses many particles to describe several absolute maps, UGPF uses GPF to estimate the mean and covariance of robot pose, and applies this information for the landmark estimation. We can also say that UGPF only keeps one absolute map. Thus, UGPF costs less computation time than RBPF. The decomposition of UGPF follows from the chain rule of probability: p( x kR , f ka | z1:k , u k ) = p( x kR | z1:k , u k ) p( f ka | x kR , z1:k , u k ) 1442443 14442444 3 (1) robot path posterior

where

x kR

landmark posterior

: the robot pose at time step k

f ka : the states of landmarks in the absolute map

z1:k : a sequence of measurement u k : control input at time step k B.

Gaussian Particle Filter (GPF) GPF is based on the particle filtering concept and it approximates the posterior distributions by Gaussian. Thus, it is easier to incorporate GPF with other Kalman filter based methods. Moreover, GPF still retains the multiple hypotheses property of particle filter which can create robust localization. In Kalman Filter based methods, it requires that the noises in the process and measurement model are additive and Gaussian. These assumptions can be relaxed for GPF which can be applied for non-additive and non-Gaussian noise. Besides, Kotecha[6] also showed that GPF provides the MMSE estimate asymptotically during the measurement update and can fetch higher order moments information when particle number is large. The procedures of GPF that we adopted is summarized in the TABLE I. The detail of GPF can be referred to [6].

Relative information

Absolute information

(a)

(b)

(c)

(d) Fig. 3 Diagrams for different SLAM algorithm.(a) the perception of robot.(b) EKF-based SLAM. (c) FastSLAM. (d) RAMF SLAM

TABLE I The Procedures of GPF GPF – Prediction Process 1) Retain weighted particles obtained from last measurement update process.

{x

(i ) k −1

, ω~k( i−)1

}

N

(2)

i =1

2) For i = 1,….,N, sample particles from prediction model.

{x }

(i ) N k i =1

~ p ( x k | x k −1 = x k(i−)1 )

(3)

3) Calculate the mean and covariance N

xˆ k− = ∑ ω~k( i−)1 x k( i ) i =1 N

Pˆk− = ∑ ω~k( i−)1 ( x k(i ) − xˆ k− )( x k(i ) − xˆ k− ) T

[

f r = f r (1, 2) f r (1,3) L f r (i , j ) L f r ( N −1, N )

]T

(8)

where f r (i, j ) is the relative distance between landmark i and landmark j. Since all the landmarks in this paper are assumed static, the relative distances are invariant all the time. f r (i , j ) (k ) = f r (i , j ) (k − 1) = f r (i , j ) (9)

Since both of prediction and measurement processes are linear, a standard Kalman Filter can be applied to update relative map. It should be noted that all the relative states (distances) in the relative map are independent. The covariance matrix Pr will be diagonal. In other words, it is only required to update the relative states with current observation. The computation of RMF for each update step in KF is also constant.

i =1

Pr (k | k ) =

GPF – Measurement Update Process 1) Sample from the importance function

{x }

(i ) N k i =1

~ π ( x k | z1:k )

0 0 ⎡ Pr (1, 2 ) (k | k ) ⎢ 0 0 Pr ( 2,3) (k | k ) ⎢ ⎢ 0 0 Pr (i , j ) (k | k ) ⎢ M M M ⎣

(4)

2) Update the respective weights by

ω k( i ) ∝

p ( z k | x k(i ) ) N ( x k = x k( i ) ; xˆ k− , Pˆk− ) π ( x k(i ) | z1:k )

(5)

N

∑ω j =1

( j) k

(6)

(7)

1)

4) Estimate the mean and covariance by N

xˆ k+ = ∑ ω~k(i ) x ki i =1 N

Pˆk+ = ∑ ω~k(i ) ( x k(i ) − xˆ k+ )( x k( i ) − xˆ k+ ) T

D.

In this paper, we adopted UKF to generate the importance function. By GPF, the robot pose posterior can be obtained and it is used for update the absolute map with the UKF. Then, we can get the absolute position f a and covariance Pa of the landmark. It is worth noting that Pa is a diagonal matrix since all the landmarks are assumed conditionally independent in the absolute map. That is to say, it is only required to update the landmark with current observation. By this reason, the computation to update the absolute map is constant. The detail of updating the absolute map with UKF is ignored in this paper. Relative Map Filter Since the measurement model is linear in the RMF, we estimate the relative distances between landmarks with Kalman Filter. The relative state vector f r can be reconstructed by the following form.

Search the correspondence of absolute map f a and relative map f r .

[

f a , k |k −1 = f a , k −1 = f a (1) f a ( 2) L f a (i ) f a ( j )

i =1

C.

(10)

Map Fusion in RAMF In the RAMF, the main purpose is to fuse the information from RMF and AMF. RMF provides the relative distance between landmarks and AMF gives the absolute landmark position. To combine the information, UKF is adopted again to fuse the maps. The fusion procedure is listed below.

3) Normalize the weights as

ω~k(i ) = ω k(i )

L⎤ L⎥⎥ L⎥ ⎥ O⎦

[

f r , k = f r (1, 2) f r (1,3) L f r (i , j ) 2)

]T

]T

(12)

Calculate sigma points Fk −1 = [ f a , k −1 f a , k −1 ± (n R + λ ) Pa , k −1 ]

3)

(11)

(13)

Prediction stage Fk |k −1 = Fk −1 , f a , k |k −1 =

2 na

∑W

i

i =0

Pa , k |k −1 =

2 na

∑W

i

(c)

( m)

Z k |k −1 = h ( Fa , k |k −1 , Pr , k ) z k |k −1 =

Fi ,k |k −1 ,

2 na

∑W

i

(m)

(14)

Z i ,k |k −1

(15)

i =0

[ Fi ,k |k −1 − f a ,k |k −1 ][ Fi , k |k −1 − f a ,k |k −1 ]T

(16)

i =0

PZ k Z k =

2 na

∑W i =0

i

(c)

[ Z i ,k |k −1 − z k |k −1 ][ Z i ,k |k −1 − z k |k −1 ]T

(17)

PFk Z k =

2 na

∑W

i

(c)

[ Fi ,k |k −1 − f a , k |k −1 ][ Z i ,k |k −1 − z k |k −1 ]T

(18)

i =0

where h (⋅) : the transformation from absolute states to relative states. W ( c ) , W ( m ) : weight of sigma points[8] 4)

Update stage f a , k = f a , k |k −1 + K k ( f r , k − Z k |k −1 ) Pa , k = Pa , k |k −1 + Kk =

K k PZ Z K kT k k

PFk Z k PZ−k1Z k

(19) (20) (21)

In the fusion procedure, the relative map f r imposes constraints on the absolute map f a . UKF will fuse both information and produce a new absolute map. Moreover, to avoid the error measurement effect, which may lead the large oscillation of gain K , an appropriate threshold of K is required. Basically, RAMF is a kind of AMF since it produces an absolute map at the end. But it also concludes information of the relative map. That is why we called it “Relative-Absolute Map Filter.” Because of the characteristics of the absolute map, all the absolute position of landmarks in the Gaussian model can be reconstructed by mean and covariance. In other words, there is no inconsistent problem in RAMF. The inconsistency can be covered by the covariance of the absolute position of landmarks. It is another advantage of RAMF.

E.

The Correlation in RAMF In general KF based method, it assumes that the process noise and measurement noise are decoupled. To avoid the correlation between absolute map and relative map, the absolute landmarks and relative features (distance) can be extracted from different laser scan images. Moreover, it should be noticed that all the landmarks in the absolute map will still be correlated after map fusion. Thus the procedure of map fusion should be executed at appropriate instant. It can also fuse the partial map to provide proper data association while exploring the environment, but the original absolute map is still retained. Finally, it fuses the whole map after detecting the loop.

V.

SIMULATION RESULTS

In this section, we compare the algorithms, UKF, FastSLAM, and RAMF in different motion noises ν . The array-type landmarks were placed on the simulation environment. The gaps between the landmarks are 20 meters. The environment adopted in our simulations is shown in Fig. 6. The simulated external sensor is a laser range finder. The scanning range of the robot is equal to 25 meters with 1 degree resolution. The measurement model is also introduced by the noise ω . The standard deviations of measurement noise are 0.2 meters in distance and 1 degree in orientation. The Maximum Likelihood is adopted for data association in the simulation. v ~ N ( 0, Pv ) , ω ~ N ( 0, Pω )

⎡(0.2) 2 ⎡δ 2 0 ⎤ 0 ⎤ Pv = ⎢ d , Pω = ⎢ ⎥ 2⎥ (1) 2 ⎦⎥ ⎣ 0 δθ ⎦ ⎣⎢ 0 The robot is required to move circularly in the environment. The robot moves 5 meters and rotates 5 degrees in each motion step. The motion noises are also modeled by white Gaussian noise. To show the robustness of RAMF, we introduce different motion noises in two simulations. The simulation results were also compared with UKF and FastSLAM. The robot trajectory is marked with the symbol • and the landmark location estimation is presented by symbol +, while for the ground truth the symbol o is adopted. SLAM results without any noise are shown in Fig. 7.The 200 particles are utilized both in the FastSLAM and RAMF. In the Simulation I, the normal motion noise is introduced to the SLAM model. Both noises are assumed as white Gaussian noise. The standard derivations of motion noises are 0.5m in distance and 2 degrees in orientation. The simulation result is shown in Fig. 8. In the Simulation II, we increase the motion noise to δ d = 1 m and δ θ = 10 o . The simulation result is shown in Fig. 9. From the simulation results, we can see that RAMF has better performance than UKF and FastSLAM in the noisy robot motion. In the Simulation II, the excellent where

dˆ3

dˆ 2

dˆ 2

dˆ1 Fig. 4 The inconsistency of the relative map estimates.[13]

Fig. 5 In RAMF, the covariance covers the inconsistent problem.

Fig. 6 Virtual environment

Fig. 7 ground truth

-results still can be available in RAMF even though the motion noise is larger than robot motion step. VI.

CONCLUSIONS

In this paper, a new algorithm, Relative-Absolute Map Filter (RAMF), is proposed for the SLAM problem. By combination of RMF and AMF, RAMF can still perform well in the noisy robot motion. To achieve efficient and robust localization, Unscented Gaussian Particle Filter (UGPF) is also adopted in this paper. The simulation results show that RAMF is more robust than the UKF and FastSLAM in the noisy robot motion.

. (a)

(b)

REFERENCES [1] T. Bailey,” Mobile Robot Localization and Mapping in Extensive Outdoor Environments,” PhD thesis, University of Sydney, 2002. [2] M. Csorba, ”Simultaneous Localisation and Map Building,” Doctoral Dissertation, Department of Engineering Science, University of Oxford, 1997. [3] M. C. Deans, M. Hebert, “ Invariant Filtering for Simultaneous Localization and Mapping, “ International Conference on Robotics and Automation, vol.2, pp.1042-1047, 2000. [4] J. Guivant and E. Nebot.,”Optimization of the Simultaneous Localization and Map Building Algorithm for Real Time Implementation,” IEEE Transactions on Robotics and Automation, Vol.17, pp.242–257, 2001. [5] J. Knight, A. Davison, and I. Reid.,”Constant Time SLAM Using Postponement,” In Proceedings of IEEE International Conference on Intelligent Robots and Systems (IROS), Vol.1, pp.405-413, 2001. [6] Jayesh H. Kotecha and Petar M. Djuric´,”Gaussian Particle Filtering,” IEEE Transactions on Signal Processing, vol.51, 2003. [7] A. Martinelli, N. Tomatis, R.Siegwart,” Open Challenges in SLAM: an Optimal Solution Based on Shift and Rotation Invariants,” International Conference on Robotics and Automation, vol.2, pp.1327-1332, 2004. [8] R. Merwe, A. Doucet, N. Freitas, E. Wan,” The Unscented Particle Filter,” Tech report CUED/F-INFENG/TR-380, Cambridge University Engineering Department, Aug. 2000. [9] M. Montemerlo, ”FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem With Unknown Data Association,” Doctoral Dissertation, school of Computer Science, Carnegie Mellon University, 2003. [10] M. Montemerlo, S. Thrun,” Simultaneous Localization and Mapping with Unknown Data Association Using FastSLAM,” IEEE International Conference on Robotics and Automation, vol.2, pp.1985-1991, 2003. [11] K. Murphy and S. Russell,” Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks,” Sequential Monte Carlo Methods in Practice, chapter 24, pp. 499-515, Springer-Verlag, 2001. [12] P. M. Newman, “On the Structure and Solution of the Simultaneous Localisation and Map Building Problem,” Doctoral Dissertation, Australian Centre for Field Robotics, University of Sydney, 1999. [13] V. Nguyen, A. Martinelli, R. Siegwart, “Handling the Inconsistency of Relative Map Filter,” IEEE International Conference on Robotics and Automation, pp. 649-654, 2005. [14] C. Paradalier, S. Sekhavat,” Simultaneous Localization and Mapping Using the Geometric Projection Filter and Correspondence Graph Matching,” Advanced Robotics, vol. 17, pp.675-690, 2003. [15] R. C. Smith, M. Self, and P. Cheeseman, “On the Representation of Spatial Uncertainty,” Int. J. Robotics Research, vol. 5, pp.56-68, 1987.

(c)

(d) Fig. 8 Simulation I. o the standard derivation of motion noise: δ d = 0.5m δ θ = 2 . (a) robot path without any correction. (b) UKF. (c) FastSLAM. (d) RAMF

(a)

(b)

(d)

(c) Fig. 9 Simulation II.

the standard derivation of motion noise: δ d = 1 m δ θ = 10 . (a) robot path without any correction. (b) UKF. (c) FastSLAM. (d) RAMF o