An Evaluation of the Sequential Monte Carlo Technique for ...
An Evaluation of the Sequential Monte Carlo Technique for Simultaneous Localisation and Map-building David C.K. Yuen and Bruce A. MacDonald Department of Electrical and Electronic Engineering, University of Auckland, Private Bag 92019, Auckland, New Zealand. [email protected], [email protected] Abstract Simultaneous Localisation and Map-building (SLAM) can be considered as a combined state and parameter estimation problem. Instead of using Extended Kalman Filtering, the more flexible Sequential Monte Carlo method is considered. Multiple Generic Particle Filters are initialised to estimate the robot and obstacle positions concurrently. Simulation results based on a simple robot environment, which represents obstacles by line segments, indicate the feasibility of the proposed method.
1 Introduction Simultaneous Localisation And Map-building (SLAM), also known as Concurrent Mapping and Localisation (CML), was introduced originally by Motarlier and Chatila [11] and Smith et.al. [15] to estimate both the robot and obstacle positions at the same time. There is a strong coupling between these two estimation tasks: without a reliable map, the robot cannot determine its position precisely; without a good robot position estimate, it is difficult to place the detected obstacle onto a map. This is the major difficulty in SLAM. Bayes Theorem [13] is the foundation of estimation theory. It states the relationship between the hidden states (robot positions) and the observations in form of conditional probabilities. Many estimation algorithms, including Kalman filtering [16] and the Sequential Monte Carlo (SMC) method [1], are based on Bayes Theorem. For SLAM applications, Extended Kalman filtering (EKF) is the prevalent solution because of its near real time performance. In addition to ranging sensors, EKF has also been demonstrated to work with active stereo vision [4, 3] or with multiple sensor types [2]. The convergence property and steady state behaviour of EKF based SLAM have been established by Dissanayake et.al. [5]. While the storage requirement of EKF is O(N 2 )1 , with N being the number of obstacle edges in the map, Guivant and Nebot [8] showed an optimal solution with O(Na2 ) 1 The computation complexity is O(N 3 ) due to the matrix inversion step [14].
by considering only the obstacle edges on the sub-map (Na ). However, EKF works well only if the system under examination can be represented by the first order Taylor approximation. Like the original Kalman filter, it also expects noise sources to be Gaussian. Due to sensor characteristics and partial obstacle occlusion, the sensory data perceived by the robot can be highly nonlinear. The limitations of EKF can thus become quite apparent. The Sequential Monte Carlo (SMC) method [1], also known as particle filtering, is a recursive Bayesian filter implemented by Monte Carlo simulation. It is easy to construct and is not restricted by the linear Gaussian state-space assumption. It provides an attractive simulation-based approach to compute the posterior distribution. Fox et.al. [6] introduced a family of SMC based algorithms, called Monte Carlo Localisation (MCL) methods. They are developed to solve many different forms of localisation problems, including position tracking, global localisation and even the difficult kidnapped robot problem [7]. Using a Rao-Blackwellised particle filter, Murphy [12] solved a simple form of SLAM problem with a solution domain limited to a small 10 × 10 grid world. This paper outlines an SMC SLAM architecture that initialises multiple particle filters for discrete time, continuous state–space systems to estimate the robot and obstacle positions simultaneously. A simple line obstacle model is introduced for the SLAM application. Preliminary simulation results are provided to evaluate the feasibility of the method.
2 SMC SLAM 2.1 Introduction to SMC A brief overview of SMC and its application to localisation is provided here. Please refer to Fox et.al. [7] and Arulampalam et.al. [1] for more detailed analysis. In many situations, raw sensor data do not give the most relevant information to the user. For example, while it is useful to obtain the robot position during navigation, the most common ranging sensor only gives obstacle disp. 1
tance. The estimation process is intended to retrieve the value of a desired hidden state variable x ∈ Rnx , from the available observation data z ∈ Rnz , where nx and nz are the dimensionality of the hidden state and measurement data respectively. A fixed sampling interval is assumed. The subscript represents the sampling time, e.g. x0 is the initial state and zk is the measurement taken at time k. As mentioned in the last section, Bayes Theorem is the foundation of estimation theory. It updates the posterior probability density function (pdf) of the hidden state2 p(xk+1 |zk+1 ) from the previous estimates using the provided measurement data. When expressed in terms of the posterior distribution, Bayes Theorem takes the form of: p(xk+1 |zk+1 ) = p(xk )
p(zk+1 |xk+1 ) p(zk+1 |xik )p(xik )dxik
(1)
In general the state variable represents all the information that it is desired to estimate. The full state in mobile robot SLAM application is an estimate of the robot’s environment, represented as a map of the obstacle positions, plus an estimate of the robot position, all combined in one single state vector. The general robot position and orientation can be expressed by the 3-D Cartesian coordinates and the roll, yaw, and pitch angles. In this study, we consider only the 2-D coordinates on a flat plane, plus the heading direction, which is the yaw angle, for the robot position. The measurement data set z is divided into the perceptual data y such as ultrasonic range measurement and odometry/controller input u. In our notation, the latest perceptual and odometry data at time k + 1 are yk+1 and uk respectively. In much robotics and AI literature, the posterior is often called the Belief, Bel(xk+1 ) = p(xk+1 |yk+1 , uk ). Bayes Theorem can be re-expressed in terms of Bel(xk ) as shown in Equation 2, where η represents a normalisation constant. Please refer to [7] for the derivation of this expression. Bel(xk+1 ) = η p(yk+1 |xk+1 ) p(xk+1 |xk , uk )Bel(xk ) dxk (2) Equation 2 can be realised with various types of particle filters. A filter contains a set of P particles. In addition to the estimated state x, each particle also has an importance factor f . The particle set Ψ is defined as: Ψ = {xi , f i }i=1:P.
(3)
During initialisation, the particles are assigned to random positions in the work space. The importance factors are initialised to P1 . 2 To be more exact, the posterior distribution should be defined as p(x0:k+1 |z0:k+1 ), where the subscript 0 : k + 1 refers to all the samples from start to time k. The expression is simplified after applying the Markov assumption, which states that zk is conditionally independent with the previous measurements.
A simple implementation such as Algorithm 1 is vulnerable to degeneracy, which happens when the importance factors for all but one of the particles become negligible. Resampling is usually required, which may in turn lead to sample impoverishment after successive iterations. Various improved resampling schedules and particle filter architectures are thus introduced to cope with degeneracy and other associated problems. Despite the apparent differences, all of these filters share the same core structure as illustrated in Algorithm 1. Algorithm 1 Realisation of Bayes Theorem for each particle in Ψ do 1. Importance sampling 2. State estimation update 3. Importance factor update end for
Algorithm 1 effectively evaluates the Belief expression (Equation 2) from right to left. At time k + 1, a particle i is first sampled from Ψ. The chance of being selected is proportional to the importance factor fk . Then, the hidden state estimation is updated by sampling the transition probability p(xk+1 |xk , uk ). Finally, the likelihood of the particular observation p(yk+1 |xk+1 ) is evaluated. The new importance factor fk+1 is obtained by scaling the previous factor fk by p(yk+1 |xk+1 ). The procedure is repeated sequentially for each particle in each round of filter update. 2.2 State versus parameter estimation In SLAM, there is a subtle difference between the estimation of robot and obstacle position. While the robot position is supposed to be time-varying, the obstacles should remain at the same place. Therefore, the update of robot position is a typical state estimation problem whereas the update of obstacle position should be considered as parameter estimation. SMC is often implemented for state estimation. Combined state and parameter estimation has received relatively little attention. Since most of the original SMC methods assume the estimates are timevarying, using them for parameter estimation may result in over-dispersion of the solution. Liu and West [9] combine both the state and parameter estimates into the same particle. A shrinking kernel is then applied so that an increasing attenuation factor is imposed onto the possible further change to the parameter estimates. For systems with a small number of parameters, it is reasonable to augment the estimated state variable directly with the additional parameters. However, for any sizable map, the number of obstacle positions may be very large and the parameter portion may increase the dimensionality of the state vector by orders of magnitude over the dimensionality of the robot position. The high dimensionality of the resulting particles means that many more individual particles must be included in each filter. Moreover, many obstacles are outside the detection p. 2
range from the initial robot position. Once the updating has been started, it is not clear what importance factors should be assigned when adding extra hidden states. 2.3 The Multiple Filter Extension to SLAM This paper proposes that in applications such as robot localisation, a novel extension is made by splitting the full state estimation vector x into smaller components and developing a separate estimator for each component. This is similar to the idea of using a multiple model bootstrap filter for fast manoeuvring target tracking [10]. However, each of the filters in [10] estimates the same state while our filters estimate different components of the full state. In our case, it is useful to first separate the state into two components, x = (s, ϕ), where s is an estimate of the time–varying portion, in this case the robot position, and ϕ is an estimate of the time–invariant portion (the parameters) in this case the map of obstacles. Secondly, it is sensible to further separate the estimate of parameters into each parameter, in this case dividing the obstacle map state component ϕ into a separate estimate ϕj=1:L for each of the L obstacles. The intention is that a sensible subdivision of the estimation problem will lead to computational advantages as a result of the degree of independence between the components. Separate particle filters are thus introduced for the position estimation of the robot and for each of the obstacles. The filter that estimates the robot position is denoted as the state filter Ψ0 and the filters assigned for the estimation of the L obstacle positions are known as parameter filters Ψj=1:L . The presence of dynamic objects has not been explicitly considered. The dependence of each particle filter on the estimated positions of other objects is accounted for by randomly selecting a single particle from each filter, as needed. Algorithm 2 outlines the initialisation and updating procedures for our SMC SLAM approach. The presence of a system model sk+1 = Fsys (sk , uk ) and perceptual model yk+1 = Fperceptual (xk+1 ) is assumed. The perceptual model relies on a map of the environment as well as the robot position; the map is assumed to be empty for SLAM initially. Detected obstacles are gradually added to the map as the SLAM operation proceeds. The state filter and the individual parameter filters are closely related. For example, the computation of the measurement likelihood p(yk |xk ) would require the current robot and obstacle positions. Neither the robot nor the obstacle positions can be assumed as completely reliable at any time for SLAM, and so a probabilistic sampling approached is adopted. At the start, a particle filter Ψ0 is initialised to track the robot position. Since SLAM makes no assumption to any external reference frame, rather than spreading uniformly across the whole work space, it is reasonable to assign the particles randomly around an arbitrary starting point.
No obstacle filter is assigned. The new obstacle detection and filter update algorithms are then repeated at each time step. Considering the (k + 1)-th iteration, the first step is to find the mean obby examining each of the paramestacle positions ϕ¯j=1:L k ter filters. The map associated with the perceptual model is updated with each ϕ¯jk . The mean robot position s¯k is evaluated in a similar manner. The projected robot posi¯k and uk to the systion sprj k+1 is obtained by substituting s tem model Fsys (sk , uk ), which is in turn being substituted into the perceptual model Fperceptual (xk+1 ) and yields the prj projected measurements yk+1 . Significant difference between the actual and projected measurements is a good indicator for the discovery of new obstacle. The approximated obstacle position can be found as the mean robot position and actual measurements are available. The approximated obstacle position is then applied to initialise a new parameter filter. The internal map is basically empty during the early phase of the operation. Any obstacle being detected would result in large difference between the actual and projected measurements. This triggers the initialisation of the parameter filters. The approximate obstacle position will then be added as a temporary object to the map at the start of next iteration. When the same obstacle is being observed again, the measurement difference should be substantially smaller and thus avoid the creation of duplicate obstacle filters. The next step is to update all the filters. When updating the Ψj filter, a random particle is selected from each of the other filters, Ψi=0:L,i=j , and each random particle’s obstacle position estimate is added into the map as a temporary object. The robot position is taken from the state filter particle just drawn. This provides the necessary coupling between these dependent variables and is an important part of the algorithm. Once an obstacle under observation leaves the detection zone of the robot, the estimation may drift away from the correct value due to the lack of further observations. To prevent this problem, any parameter filter Ψj with a mean obstacle position ϕ¯jk that is outside the detection limit of the mean robot position s¯k+1 will be suspended from updating temporarily. Once it comes within range again it will be reactivated; hopefully before a large discrepancy with projected values causes a new filter to be created as a duplicate of the same obstacle. The state filter and all the valid parameter filters are updated one after another one. The order of updating is unimportant, which allows for parallel implementation in the future. The obstacle filters are examined and considered for removal after each complete round of filter update. This procedure not only reduces computation, but also stabilises the estimation. The parameter filter removal rules are described in more detail in the next section. Basically, a parameter filter will be deleted without further p. 3
w
consideration if its creation is likely to be triggered accidentally by noise. On the other hand, the mean obstacle position ϕ¯jk will be extracted and stored permanently on the map if the progress of the estimation is considered as satisfactory. The associated parameter filter is deleted afterwards.
φ
v
Figure 1: The convention for the coordinate system
Algorithm 2 SMC SLAM Initialise state filter Ψ0 for robot position L = 0 {L is the total number of new obstacles} repeat Update perceptual map by ϕ ¯j=1:L k prj prj Calculate s¯k , sk+1 and yk+1 prj yk+1
Compare yk+1 with if new obstacle is identified then L= L+1 Initialise new parameter filter ΨL end if Update all the filters Ψi=0:L for all Ψi=1:L do Check for possible removal of Ψi end for
until the last time step
Generic Particle Filter (GPF) [1] is selected for the implementation of both state and parameter filters. Its update procedure is shown in Algorithm 3. GPF shares the basic particle filter structure shown in Algorithm 1, but the Importance Sampling is replaced by a systematic Resampling stage. The Resampling stage follows the algorithm described by Arulampalam [1]. It takes a new set of samples in accordance to the cumulative distribution function of the importance factors. Particles are resampled only if the effective particle number Peff falls below a threshold of PT . In this study, PT is set to P/5, where P is the total number of particles in a particular filter. The selection of GPF is due to its simplicity and can be substituted with more advanced particle filters, such as Auxiliary Particle Filter. In order to prevent sample impoverishment, a small amount of Gaussian noise is added during the update of the state estimates. Algorithm 3 Update of Generic Particle Filter i i P [{xik , fki }P i=1 ] =GPF[{xk−1 , fk−1 }i=1 , uk , yk ] for i = 1 : P do State estimation update Importance factor update end for i Calculate: fsum = P i=1 fk for i = 1 : P do Normalise: fki = fki /fsum end for Calculate: Peff = P 1(f i )2
i=1
boundaries are approximated by line segments, of which the start and end points are stored as parameters. The simulated robot has 2 degrees-of-freedom, (move forward/backward and “turn-on-the-spot”). The robot position s = [v, w, φ] can be represented by the 2-D coordinates v, w and the heading direction φ. The convention is shown in Figure 1. The system model Fsys (sk , uk ) consists of the following kinematic equations: v(k + 1) = v(k) + u1 (k) cos φ(k)
(4)
w(k + 1) = w(k) + u1 (k) sin φ(k) φ(k + 1) = φ(k) + u2 (k)
(5) (6)
u1 and u2 are the control inputs for the linear and angular motions respectively. The state variables are corrupted by independent zero mean Gaussian noise streams. The robot is equipped with a ring of 24 ultrasonic-like range finders. The perceptual model Fperceptual (xk+1 ) adopted is quite simple. It relies on the use of a map. In addition to already mapped objects, the obstacle position still under estimation should also be included. A sample is randomly drawn from each parameter filter and added to the map as temporarily obstacles. The perceptual model returns 24 readings, which represents the minimum obstacle distance from the current robot position sk+1 in each of the 24 directions. Since the sensors have only a limited range, the maximum allowable reading is the same as the detection limit. It is taken as 4 m in this study. The measurement of obstacle distance is affected by a mixture of zero mean Gaussian G(σ) and Laplace L(λ) noise components. The former models the normal sensor noise whereas the latter represents the more catastrophic events such as multiple reflection or cross talk between the sensors. The Laplace distribution, with a pdf of λ1 exp −|x/λ|, is also known as the two-sided exponential distribution. It has long tails on both sides. For the mixed noise source M = aG(σ) + bL(λ), the 4 parameters a, σ, b, λ are selected as 0.05, 1.00, 0.05 and 1.00 respectively.
k
if Peff < PT then i i P [{xik , fki }P i=1 ] =Resample[{xk , fk }i=1 ] end if
2.4 Implementation details This section covers the system dependent implementation details. To evaluate the SMC SLAM approach, a simple simulated environment is adopted. The obstacle
When updating the importance factor, it is necessary to find the measurement likelihood p(yk+1 |xk+1 ). Since the prj projected measurement yk+1 can be calculated from xk+1 using the perceptual model, the measurement likelihood prj is equivalent to p(yk+1 |yk+1 ). Suppose the robot position estimate sk+1 is accurate and the robot environment has been thoroughly mapped, the error distribution of individual sensor reading should follow the mixed noise p. 4
1
1111111111111 0000000000000 0000000000000 1111111111111 0000000000000 1111111111111 0000000000000 1111111111111 0000000000000 1111111111111 0000000000000 1111111111111 0000000000000 1111111111111 2 0000000000000 1111111111111
1m
actual measurements predicted by perceptual model
Figure 2: New obstacle detection. If there exists consecutive mismatched measurements (2nd dotted circle), the chance of detecting a genuine obstacle is very high. On the other hand, an odd measurement (1st dotted circle) may just be caused by noise.
111111111111 000000000000 000000000000 111111111111 000000000000 111111111111 000000000000 111111111111 000000000000 111111111111 New Line 000000000000 111111111111 2 000000000000 111111111111 Existing Line
Start point End point
Figure 4: The actual and estimated robot trajectories. The track which joined together with a thin line is the actual trajectory, with the other one being the estimated trajectory.
1 ure 3).
Figure 3: Merge similar lines. The robot is moved from Point 1 to 2. It detects a new line segment at Point 2. In fact, this line should be considered as the extension of an existing line. It is because the 2 lines have almost the same slope and their end points are close.
model M described in the last paragraph. For multiple sensor readings, it is possible to derive a pdf for the sum of squared errors S = M21 + M22 + . . . + M224 for these independent variables. The S distribution would be identical to Chi-squared if the error source contains only the Gaussian component. The probability density function of S is generated by drawing a large number of samples with a distribution of M before calculating the sum squared. A few obstacle filter management rules are established to trigger the addition and removal of the parameter filters. In this work, the obstacle edges are modelled as straight lines. A new parameter filter will be initialised if the differences between the actual and projected measurements prj ) are sufficiently large for at least three consec(yk+1 , yk+1 utive readings (see Figure 2). A line is fit through these measurement points using the least square method. The start and end measurement points are projected onto the best fit line to determine the end points for the new line segment. These end points functions like the foci of an ellipse. Particles are generated at random position near them. Suppose the robot is moving along a long corridor, it will initialise new parameter filters once the measurement difference is getting large. Multiple short line segments, instead of a pair of continuous parallel lines, will be extracted. It is undesired to generate unnecessary filters due to computation requirements. Also, existing filters need more samples to train. Therefore, a new line will be combined to an existing one if they share the same end point and with more or less the same slope (see Fig-
If a genuine obstacle is detected, the particle distribution of the filter will become more compact after a few iterations and this can be judged from the standard deviation. The mean obstacle position ϕ¯jk will be stored permanently on the map before the filter removal. Random error sometimes triggers the construction of a parameter filter. Some of the particles may contain parameters that represents longer line segments, while the others represent the shorter ones. Since the obstacle does not exist, the importance factor of the particles that represents shorter line segments will increase. After a few iterations, the length of the mean line segment (ϕ¯jk ), will become very short. These filters can then be identified and removed.
3 Results and Discussions A simple simulation environment with mainly polygonal obstacles has been established. The system and perceptual model and the associated error models are explained with the parameters documented in the last section. The map attached to the robot perceptual model is empty initially. The number of particles assigned for the state and each of the parameter filters are 2000 and 200 respectively. The algorithm is implemented in C++ with extensive use of STL. The average computation time per iteration is 5.0 seconds in a single desktop PC. A few relative simple code optimisations can be carried out and should speed up the computation to near real-time. At the start, the robot has no idea about its actual position and environment. It assumes that its initial position is at (0.0, 0.0) facing 0.0o . To compare the actual and estimated robot trajectories, the estimated trajectory is translated and rotated so that the initial estimate overlaps with the actual initial position. The trajectories are p. 5
Positioning error (m) 1.5
Acknowledgements odometry only estimated trajectory
1.0
This work was supported in part by the Foundation for Research, Science and Technology, New Zealand, with a Top Achiever Doctoral Scholarship.
0.5 0.0
15
30
45
60
time step
Figure 5: The absolute position error for the estimated trajectories.
Figure 6: The line obstacles detected in the simulated environment.
shown in Figure 4. It is clear that the localisation algorithm can successfully track the robot position. Figure 5 shows the changes in absolute position error. Localisation error will continue to accumulate if the robot relies only on the odometry. The map-building process is not as successful when compared with the localisation module. Many duplicated obstacles (Figure 6) are marked at the incorrect position especially during the later parts of the localisation. The robot sometimes fails to recognise that a certain obstacles have already been detected after moved to a new position. New parameter filters are created to estimate these obstacles again. Due to localisation error, the duplicated obstacles will appear with an offset from the originals. A more realistic polygonal obstacle model should be introduced.
4 Conclusion In this paper, an innovative SMC SLAM architecture has been evaluated. The SLAM is regarded as a combined state and parameter estimation problem. Multiple Generic Particle Filters are initialised to estimate the robot and obstacle positions simultaneously. While system complexity and implementation details affect the performance of these algorithms, the preliminary results suggest the feasibility of the SMC SLAM idea.
References [1] M.S. Arulampalam, S. Maskell, N. Gordon, and T. Clapp. A tutorial on particle filters for online nonlinear/non-gaussian bayesian tracking. IEEE Transactions on Signal Processing, 50(2), 2002. [2] J.A. Castellanos, J. Neira, and J.D. Tardos. Multisensor fusion for simultaneous localization and map building. IEEE Transactions on Robotics and Automation, 17(6):908– 914, 2001. [3] A. Davison and N. Kita. 3d simultaneous localisation and map building using active vision for a robot moving on undulating terrain. In CVPR, 2001. [4] A.J. Davison and D.W. Murray. Simultaneous localization and map-building using active vision. IEEE Transactions on PAMI, 24(7):865–880, 2002. [5] M.W.M.G. Dissanayake, P. Newman, S. Clark, and M. Csorba H.F. Durrant-Whyte. A solution to the simultaneous localization and map building (slam) problem. IEEE Transactions on Robotics and Automation, 17(3):229–241, 2001. [6] D. Fox, W. Burgard, F. Dellaert, and S. Thrun. Monte carlo localization: Efficient position estimation for mobile robots. In Proceedings of the National Conference on Artifical Intelligence. AAAI, 1999. [7] D. Fox, S. Thrun, W. Burgard, and F. Dellaert. Sequential Monte Carlo Methods in Practice, chapter 19. Springer, 2000. [8] J.E. Guivant and E.M. Nebot. Optimization of the simultaneous localization and map-building algorithm for realtime implementation. IEEE Transactions on Robotics and Automation, 17(3):242–257, 2001. [9] J. Liu and M. West. Sequential Monte Carlo Methods in Practice, chapter 10. Springer, 2000. [10] S. McGinnity and G.W. Irwin. Sequential Monte Carlo Methods in Practice, chapter 23. Springer, 2000. [11] P. Motarlier and R. Chatila. Stochastic multi-sensory data fusion for mobile robot location and environmental modeling. In Fifth Symp. Robot. Res., pages 85–94, Tokyo, 1989. [12] K.P. Murphy. Bayesian map learning in dynamic environments. In Advances in Neural Information Processing System, volume 12, pages 1015–1021. MIT Press, 2000. [13] A. Papoulis. Probability, Random Variables, and Stochastic Processes. McGraw-Hill, New York, 2 edition, 1984. [14] William H. Press, editor. Numerical recipes in C : the art of scientific computing. Cambridge University Press, 2 edition, 1992. [15] M. Self R. Smith and P. Cheeseman. Estimating uncertain spatial relationships in robotics. In Uncertainty in Artificial Intelligence, volume 2, pages 435–461, New York, 1988. Elsevier Science. [16] S. J. Russell. Artificial intelligence : a modern approach. Prentice Hall, 1995.
p. 6
1 Introduction Simultaneous Localisation And Map-building (SLAM), also known as Concurrent Mapping and Localisation (CML), was introduced originally by Motarlier and Chatila [11] and Smith et.al. [15] to estimate both the robot and obstacle positions at the same time. There is a strong coupling between these two estimation tasks: without a reliable map, the robot cannot determine its position precisely; without a good robot position estimate, it is difficult to place the detected obstacle onto a map. This is the major difficulty in SLAM. Bayes Theorem [13] is the foundation of estimation theory. It states the relationship between the hidden states (robot positions) and the observations in form of conditional probabilities. Many estimation algorithms, including Kalman filtering [16] and the Sequential Monte Carlo (SMC) method [1], are based on Bayes Theorem. For SLAM applications, Extended Kalman filtering (EKF) is the prevalent solution because of its near real time performance. In addition to ranging sensors, EKF has also been demonstrated to work with active stereo vision [4, 3] or with multiple sensor types [2]. The convergence property and steady state behaviour of EKF based SLAM have been established by Dissanayake et.al. [5]. While the storage requirement of EKF is O(N 2 )1 , with N being the number of obstacle edges in the map, Guivant and Nebot [8] showed an optimal solution with O(Na2 ) 1 The computation complexity is O(N 3 ) due to the matrix inversion step [14].
by considering only the obstacle edges on the sub-map (Na ). However, EKF works well only if the system under examination can be represented by the first order Taylor approximation. Like the original Kalman filter, it also expects noise sources to be Gaussian. Due to sensor characteristics and partial obstacle occlusion, the sensory data perceived by the robot can be highly nonlinear. The limitations of EKF can thus become quite apparent. The Sequential Monte Carlo (SMC) method [1], also known as particle filtering, is a recursive Bayesian filter implemented by Monte Carlo simulation. It is easy to construct and is not restricted by the linear Gaussian state-space assumption. It provides an attractive simulation-based approach to compute the posterior distribution. Fox et.al. [6] introduced a family of SMC based algorithms, called Monte Carlo Localisation (MCL) methods. They are developed to solve many different forms of localisation problems, including position tracking, global localisation and even the difficult kidnapped robot problem [7]. Using a Rao-Blackwellised particle filter, Murphy [12] solved a simple form of SLAM problem with a solution domain limited to a small 10 × 10 grid world. This paper outlines an SMC SLAM architecture that initialises multiple particle filters for discrete time, continuous state–space systems to estimate the robot and obstacle positions simultaneously. A simple line obstacle model is introduced for the SLAM application. Preliminary simulation results are provided to evaluate the feasibility of the method.
2 SMC SLAM 2.1 Introduction to SMC A brief overview of SMC and its application to localisation is provided here. Please refer to Fox et.al. [7] and Arulampalam et.al. [1] for more detailed analysis. In many situations, raw sensor data do not give the most relevant information to the user. For example, while it is useful to obtain the robot position during navigation, the most common ranging sensor only gives obstacle disp. 1
tance. The estimation process is intended to retrieve the value of a desired hidden state variable x ∈ Rnx , from the available observation data z ∈ Rnz , where nx and nz are the dimensionality of the hidden state and measurement data respectively. A fixed sampling interval is assumed. The subscript represents the sampling time, e.g. x0 is the initial state and zk is the measurement taken at time k. As mentioned in the last section, Bayes Theorem is the foundation of estimation theory. It updates the posterior probability density function (pdf) of the hidden state2 p(xk+1 |zk+1 ) from the previous estimates using the provided measurement data. When expressed in terms of the posterior distribution, Bayes Theorem takes the form of: p(xk+1 |zk+1 ) = p(xk )
p(zk+1 |xk+1 ) p(zk+1 |xik )p(xik )dxik
(1)
In general the state variable represents all the information that it is desired to estimate. The full state in mobile robot SLAM application is an estimate of the robot’s environment, represented as a map of the obstacle positions, plus an estimate of the robot position, all combined in one single state vector. The general robot position and orientation can be expressed by the 3-D Cartesian coordinates and the roll, yaw, and pitch angles. In this study, we consider only the 2-D coordinates on a flat plane, plus the heading direction, which is the yaw angle, for the robot position. The measurement data set z is divided into the perceptual data y such as ultrasonic range measurement and odometry/controller input u. In our notation, the latest perceptual and odometry data at time k + 1 are yk+1 and uk respectively. In much robotics and AI literature, the posterior is often called the Belief, Bel(xk+1 ) = p(xk+1 |yk+1 , uk ). Bayes Theorem can be re-expressed in terms of Bel(xk ) as shown in Equation 2, where η represents a normalisation constant. Please refer to [7] for the derivation of this expression. Bel(xk+1 ) = η p(yk+1 |xk+1 ) p(xk+1 |xk , uk )Bel(xk ) dxk (2) Equation 2 can be realised with various types of particle filters. A filter contains a set of P particles. In addition to the estimated state x, each particle also has an importance factor f . The particle set Ψ is defined as: Ψ = {xi , f i }i=1:P.
(3)
During initialisation, the particles are assigned to random positions in the work space. The importance factors are initialised to P1 . 2 To be more exact, the posterior distribution should be defined as p(x0:k+1 |z0:k+1 ), where the subscript 0 : k + 1 refers to all the samples from start to time k. The expression is simplified after applying the Markov assumption, which states that zk is conditionally independent with the previous measurements.
A simple implementation such as Algorithm 1 is vulnerable to degeneracy, which happens when the importance factors for all but one of the particles become negligible. Resampling is usually required, which may in turn lead to sample impoverishment after successive iterations. Various improved resampling schedules and particle filter architectures are thus introduced to cope with degeneracy and other associated problems. Despite the apparent differences, all of these filters share the same core structure as illustrated in Algorithm 1. Algorithm 1 Realisation of Bayes Theorem for each particle in Ψ do 1. Importance sampling 2. State estimation update 3. Importance factor update end for
Algorithm 1 effectively evaluates the Belief expression (Equation 2) from right to left. At time k + 1, a particle i is first sampled from Ψ. The chance of being selected is proportional to the importance factor fk . Then, the hidden state estimation is updated by sampling the transition probability p(xk+1 |xk , uk ). Finally, the likelihood of the particular observation p(yk+1 |xk+1 ) is evaluated. The new importance factor fk+1 is obtained by scaling the previous factor fk by p(yk+1 |xk+1 ). The procedure is repeated sequentially for each particle in each round of filter update. 2.2 State versus parameter estimation In SLAM, there is a subtle difference between the estimation of robot and obstacle position. While the robot position is supposed to be time-varying, the obstacles should remain at the same place. Therefore, the update of robot position is a typical state estimation problem whereas the update of obstacle position should be considered as parameter estimation. SMC is often implemented for state estimation. Combined state and parameter estimation has received relatively little attention. Since most of the original SMC methods assume the estimates are timevarying, using them for parameter estimation may result in over-dispersion of the solution. Liu and West [9] combine both the state and parameter estimates into the same particle. A shrinking kernel is then applied so that an increasing attenuation factor is imposed onto the possible further change to the parameter estimates. For systems with a small number of parameters, it is reasonable to augment the estimated state variable directly with the additional parameters. However, for any sizable map, the number of obstacle positions may be very large and the parameter portion may increase the dimensionality of the state vector by orders of magnitude over the dimensionality of the robot position. The high dimensionality of the resulting particles means that many more individual particles must be included in each filter. Moreover, many obstacles are outside the detection p. 2
range from the initial robot position. Once the updating has been started, it is not clear what importance factors should be assigned when adding extra hidden states. 2.3 The Multiple Filter Extension to SLAM This paper proposes that in applications such as robot localisation, a novel extension is made by splitting the full state estimation vector x into smaller components and developing a separate estimator for each component. This is similar to the idea of using a multiple model bootstrap filter for fast manoeuvring target tracking [10]. However, each of the filters in [10] estimates the same state while our filters estimate different components of the full state. In our case, it is useful to first separate the state into two components, x = (s, ϕ), where s is an estimate of the time–varying portion, in this case the robot position, and ϕ is an estimate of the time–invariant portion (the parameters) in this case the map of obstacles. Secondly, it is sensible to further separate the estimate of parameters into each parameter, in this case dividing the obstacle map state component ϕ into a separate estimate ϕj=1:L for each of the L obstacles. The intention is that a sensible subdivision of the estimation problem will lead to computational advantages as a result of the degree of independence between the components. Separate particle filters are thus introduced for the position estimation of the robot and for each of the obstacles. The filter that estimates the robot position is denoted as the state filter Ψ0 and the filters assigned for the estimation of the L obstacle positions are known as parameter filters Ψj=1:L . The presence of dynamic objects has not been explicitly considered. The dependence of each particle filter on the estimated positions of other objects is accounted for by randomly selecting a single particle from each filter, as needed. Algorithm 2 outlines the initialisation and updating procedures for our SMC SLAM approach. The presence of a system model sk+1 = Fsys (sk , uk ) and perceptual model yk+1 = Fperceptual (xk+1 ) is assumed. The perceptual model relies on a map of the environment as well as the robot position; the map is assumed to be empty for SLAM initially. Detected obstacles are gradually added to the map as the SLAM operation proceeds. The state filter and the individual parameter filters are closely related. For example, the computation of the measurement likelihood p(yk |xk ) would require the current robot and obstacle positions. Neither the robot nor the obstacle positions can be assumed as completely reliable at any time for SLAM, and so a probabilistic sampling approached is adopted. At the start, a particle filter Ψ0 is initialised to track the robot position. Since SLAM makes no assumption to any external reference frame, rather than spreading uniformly across the whole work space, it is reasonable to assign the particles randomly around an arbitrary starting point.
No obstacle filter is assigned. The new obstacle detection and filter update algorithms are then repeated at each time step. Considering the (k + 1)-th iteration, the first step is to find the mean obby examining each of the paramestacle positions ϕ¯j=1:L k ter filters. The map associated with the perceptual model is updated with each ϕ¯jk . The mean robot position s¯k is evaluated in a similar manner. The projected robot posi¯k and uk to the systion sprj k+1 is obtained by substituting s tem model Fsys (sk , uk ), which is in turn being substituted into the perceptual model Fperceptual (xk+1 ) and yields the prj projected measurements yk+1 . Significant difference between the actual and projected measurements is a good indicator for the discovery of new obstacle. The approximated obstacle position can be found as the mean robot position and actual measurements are available. The approximated obstacle position is then applied to initialise a new parameter filter. The internal map is basically empty during the early phase of the operation. Any obstacle being detected would result in large difference between the actual and projected measurements. This triggers the initialisation of the parameter filters. The approximate obstacle position will then be added as a temporary object to the map at the start of next iteration. When the same obstacle is being observed again, the measurement difference should be substantially smaller and thus avoid the creation of duplicate obstacle filters. The next step is to update all the filters. When updating the Ψj filter, a random particle is selected from each of the other filters, Ψi=0:L,i=j , and each random particle’s obstacle position estimate is added into the map as a temporary object. The robot position is taken from the state filter particle just drawn. This provides the necessary coupling between these dependent variables and is an important part of the algorithm. Once an obstacle under observation leaves the detection zone of the robot, the estimation may drift away from the correct value due to the lack of further observations. To prevent this problem, any parameter filter Ψj with a mean obstacle position ϕ¯jk that is outside the detection limit of the mean robot position s¯k+1 will be suspended from updating temporarily. Once it comes within range again it will be reactivated; hopefully before a large discrepancy with projected values causes a new filter to be created as a duplicate of the same obstacle. The state filter and all the valid parameter filters are updated one after another one. The order of updating is unimportant, which allows for parallel implementation in the future. The obstacle filters are examined and considered for removal after each complete round of filter update. This procedure not only reduces computation, but also stabilises the estimation. The parameter filter removal rules are described in more detail in the next section. Basically, a parameter filter will be deleted without further p. 3
w
consideration if its creation is likely to be triggered accidentally by noise. On the other hand, the mean obstacle position ϕ¯jk will be extracted and stored permanently on the map if the progress of the estimation is considered as satisfactory. The associated parameter filter is deleted afterwards.
φ
v
Figure 1: The convention for the coordinate system
Algorithm 2 SMC SLAM Initialise state filter Ψ0 for robot position L = 0 {L is the total number of new obstacles} repeat Update perceptual map by ϕ ¯j=1:L k prj prj Calculate s¯k , sk+1 and yk+1 prj yk+1
Compare yk+1 with if new obstacle is identified then L= L+1 Initialise new parameter filter ΨL end if Update all the filters Ψi=0:L for all Ψi=1:L do Check for possible removal of Ψi end for
until the last time step
Generic Particle Filter (GPF) [1] is selected for the implementation of both state and parameter filters. Its update procedure is shown in Algorithm 3. GPF shares the basic particle filter structure shown in Algorithm 1, but the Importance Sampling is replaced by a systematic Resampling stage. The Resampling stage follows the algorithm described by Arulampalam [1]. It takes a new set of samples in accordance to the cumulative distribution function of the importance factors. Particles are resampled only if the effective particle number Peff falls below a threshold of PT . In this study, PT is set to P/5, where P is the total number of particles in a particular filter. The selection of GPF is due to its simplicity and can be substituted with more advanced particle filters, such as Auxiliary Particle Filter. In order to prevent sample impoverishment, a small amount of Gaussian noise is added during the update of the state estimates. Algorithm 3 Update of Generic Particle Filter i i P [{xik , fki }P i=1 ] =GPF[{xk−1 , fk−1 }i=1 , uk , yk ] for i = 1 : P do State estimation update Importance factor update end for i Calculate: fsum = P i=1 fk for i = 1 : P do Normalise: fki = fki /fsum end for Calculate: Peff = P 1(f i )2
i=1
boundaries are approximated by line segments, of which the start and end points are stored as parameters. The simulated robot has 2 degrees-of-freedom, (move forward/backward and “turn-on-the-spot”). The robot position s = [v, w, φ] can be represented by the 2-D coordinates v, w and the heading direction φ. The convention is shown in Figure 1. The system model Fsys (sk , uk ) consists of the following kinematic equations: v(k + 1) = v(k) + u1 (k) cos φ(k)
(4)
w(k + 1) = w(k) + u1 (k) sin φ(k) φ(k + 1) = φ(k) + u2 (k)
(5) (6)
u1 and u2 are the control inputs for the linear and angular motions respectively. The state variables are corrupted by independent zero mean Gaussian noise streams. The robot is equipped with a ring of 24 ultrasonic-like range finders. The perceptual model Fperceptual (xk+1 ) adopted is quite simple. It relies on the use of a map. In addition to already mapped objects, the obstacle position still under estimation should also be included. A sample is randomly drawn from each parameter filter and added to the map as temporarily obstacles. The perceptual model returns 24 readings, which represents the minimum obstacle distance from the current robot position sk+1 in each of the 24 directions. Since the sensors have only a limited range, the maximum allowable reading is the same as the detection limit. It is taken as 4 m in this study. The measurement of obstacle distance is affected by a mixture of zero mean Gaussian G(σ) and Laplace L(λ) noise components. The former models the normal sensor noise whereas the latter represents the more catastrophic events such as multiple reflection or cross talk between the sensors. The Laplace distribution, with a pdf of λ1 exp −|x/λ|, is also known as the two-sided exponential distribution. It has long tails on both sides. For the mixed noise source M = aG(σ) + bL(λ), the 4 parameters a, σ, b, λ are selected as 0.05, 1.00, 0.05 and 1.00 respectively.
k
if Peff < PT then i i P [{xik , fki }P i=1 ] =Resample[{xk , fk }i=1 ] end if
2.4 Implementation details This section covers the system dependent implementation details. To evaluate the SMC SLAM approach, a simple simulated environment is adopted. The obstacle
When updating the importance factor, it is necessary to find the measurement likelihood p(yk+1 |xk+1 ). Since the prj projected measurement yk+1 can be calculated from xk+1 using the perceptual model, the measurement likelihood prj is equivalent to p(yk+1 |yk+1 ). Suppose the robot position estimate sk+1 is accurate and the robot environment has been thoroughly mapped, the error distribution of individual sensor reading should follow the mixed noise p. 4
1
1111111111111 0000000000000 0000000000000 1111111111111 0000000000000 1111111111111 0000000000000 1111111111111 0000000000000 1111111111111 0000000000000 1111111111111 0000000000000 1111111111111 2 0000000000000 1111111111111
1m
actual measurements predicted by perceptual model
Figure 2: New obstacle detection. If there exists consecutive mismatched measurements (2nd dotted circle), the chance of detecting a genuine obstacle is very high. On the other hand, an odd measurement (1st dotted circle) may just be caused by noise.
111111111111 000000000000 000000000000 111111111111 000000000000 111111111111 000000000000 111111111111 000000000000 111111111111 New Line 000000000000 111111111111 2 000000000000 111111111111 Existing Line
Start point End point
Figure 4: The actual and estimated robot trajectories. The track which joined together with a thin line is the actual trajectory, with the other one being the estimated trajectory.
1 ure 3).
Figure 3: Merge similar lines. The robot is moved from Point 1 to 2. It detects a new line segment at Point 2. In fact, this line should be considered as the extension of an existing line. It is because the 2 lines have almost the same slope and their end points are close.
model M described in the last paragraph. For multiple sensor readings, it is possible to derive a pdf for the sum of squared errors S = M21 + M22 + . . . + M224 for these independent variables. The S distribution would be identical to Chi-squared if the error source contains only the Gaussian component. The probability density function of S is generated by drawing a large number of samples with a distribution of M before calculating the sum squared. A few obstacle filter management rules are established to trigger the addition and removal of the parameter filters. In this work, the obstacle edges are modelled as straight lines. A new parameter filter will be initialised if the differences between the actual and projected measurements prj ) are sufficiently large for at least three consec(yk+1 , yk+1 utive readings (see Figure 2). A line is fit through these measurement points using the least square method. The start and end measurement points are projected onto the best fit line to determine the end points for the new line segment. These end points functions like the foci of an ellipse. Particles are generated at random position near them. Suppose the robot is moving along a long corridor, it will initialise new parameter filters once the measurement difference is getting large. Multiple short line segments, instead of a pair of continuous parallel lines, will be extracted. It is undesired to generate unnecessary filters due to computation requirements. Also, existing filters need more samples to train. Therefore, a new line will be combined to an existing one if they share the same end point and with more or less the same slope (see Fig-
If a genuine obstacle is detected, the particle distribution of the filter will become more compact after a few iterations and this can be judged from the standard deviation. The mean obstacle position ϕ¯jk will be stored permanently on the map before the filter removal. Random error sometimes triggers the construction of a parameter filter. Some of the particles may contain parameters that represents longer line segments, while the others represent the shorter ones. Since the obstacle does not exist, the importance factor of the particles that represents shorter line segments will increase. After a few iterations, the length of the mean line segment (ϕ¯jk ), will become very short. These filters can then be identified and removed.
3 Results and Discussions A simple simulation environment with mainly polygonal obstacles has been established. The system and perceptual model and the associated error models are explained with the parameters documented in the last section. The map attached to the robot perceptual model is empty initially. The number of particles assigned for the state and each of the parameter filters are 2000 and 200 respectively. The algorithm is implemented in C++ with extensive use of STL. The average computation time per iteration is 5.0 seconds in a single desktop PC. A few relative simple code optimisations can be carried out and should speed up the computation to near real-time. At the start, the robot has no idea about its actual position and environment. It assumes that its initial position is at (0.0, 0.0) facing 0.0o . To compare the actual and estimated robot trajectories, the estimated trajectory is translated and rotated so that the initial estimate overlaps with the actual initial position. The trajectories are p. 5
Positioning error (m) 1.5
Acknowledgements odometry only estimated trajectory
1.0
This work was supported in part by the Foundation for Research, Science and Technology, New Zealand, with a Top Achiever Doctoral Scholarship.
0.5 0.0
15
30
45
60
time step
Figure 5: The absolute position error for the estimated trajectories.
Figure 6: The line obstacles detected in the simulated environment.
shown in Figure 4. It is clear that the localisation algorithm can successfully track the robot position. Figure 5 shows the changes in absolute position error. Localisation error will continue to accumulate if the robot relies only on the odometry. The map-building process is not as successful when compared with the localisation module. Many duplicated obstacles (Figure 6) are marked at the incorrect position especially during the later parts of the localisation. The robot sometimes fails to recognise that a certain obstacles have already been detected after moved to a new position. New parameter filters are created to estimate these obstacles again. Due to localisation error, the duplicated obstacles will appear with an offset from the originals. A more realistic polygonal obstacle model should be introduced.
4 Conclusion In this paper, an innovative SMC SLAM architecture has been evaluated. The SLAM is regarded as a combined state and parameter estimation problem. Multiple Generic Particle Filters are initialised to estimate the robot and obstacle positions simultaneously. While system complexity and implementation details affect the performance of these algorithms, the preliminary results suggest the feasibility of the SMC SLAM idea.
References [1] M.S. Arulampalam, S. Maskell, N. Gordon, and T. Clapp. A tutorial on particle filters for online nonlinear/non-gaussian bayesian tracking. IEEE Transactions on Signal Processing, 50(2), 2002. [2] J.A. Castellanos, J. Neira, and J.D. Tardos. Multisensor fusion for simultaneous localization and map building. IEEE Transactions on Robotics and Automation, 17(6):908– 914, 2001. [3] A. Davison and N. Kita. 3d simultaneous localisation and map building using active vision for a robot moving on undulating terrain. In CVPR, 2001. [4] A.J. Davison and D.W. Murray. Simultaneous localization and map-building using active vision. IEEE Transactions on PAMI, 24(7):865–880, 2002. [5] M.W.M.G. Dissanayake, P. Newman, S. Clark, and M. Csorba H.F. Durrant-Whyte. A solution to the simultaneous localization and map building (slam) problem. IEEE Transactions on Robotics and Automation, 17(3):229–241, 2001. [6] D. Fox, W. Burgard, F. Dellaert, and S. Thrun. Monte carlo localization: Efficient position estimation for mobile robots. In Proceedings of the National Conference on Artifical Intelligence. AAAI, 1999. [7] D. Fox, S. Thrun, W. Burgard, and F. Dellaert. Sequential Monte Carlo Methods in Practice, chapter 19. Springer, 2000. [8] J.E. Guivant and E.M. Nebot. Optimization of the simultaneous localization and map-building algorithm for realtime implementation. IEEE Transactions on Robotics and Automation, 17(3):242–257, 2001. [9] J. Liu and M. West. Sequential Monte Carlo Methods in Practice, chapter 10. Springer, 2000. [10] S. McGinnity and G.W. Irwin. Sequential Monte Carlo Methods in Practice, chapter 23. Springer, 2000. [11] P. Motarlier and R. Chatila. Stochastic multi-sensory data fusion for mobile robot location and environmental modeling. In Fifth Symp. Robot. Res., pages 85–94, Tokyo, 1989. [12] K.P. Murphy. Bayesian map learning in dynamic environments. In Advances in Neural Information Processing System, volume 12, pages 1015–1021. MIT Press, 2000. [13] A. Papoulis. Probability, Random Variables, and Stochastic Processes. McGraw-Hill, New York, 2 edition, 1984. [14] William H. Press, editor. Numerical recipes in C : the art of scientific computing. Cambridge University Press, 2 edition, 1992. [15] M. Self R. Smith and P. Cheeseman. Estimating uncertain spatial relationships in robotics. In Uncertainty in Artificial Intelligence, volume 2, pages 435–461, New York, 1988. Elsevier Science. [16] S. J. Russell. Artificial intelligence : a modern approach. Prentice Hall, 1995.
p. 6
