AUTONOMOUS SYSTEMS. PROBABILISTIC LOCALIZATION Monte Carlo Localization

Transcription

1 AUTONOMOUS SYSTEMS PROBABILISTIC LOCALIZATION Monte Carlo Localization Maria Isabel Ribeiro Pedro Lima With revisions introduced by Rodrigo Ventura Instituto Superior Técnico/Instituto de Sistemas e Robótica October 2008 Course Handouts All rights reserved

2 References S. Thrun, W.Burgard, D. Fox, Probabilistic Robotics, MIT Press, 2005 Monte Carlo Localization (Chap. 8) Particle Filters (Chap. 4) F. Dellaert, D. Fox, W. Burgard, S. Thrun, Monte-Carlo Localization for Mobile Robots, Proc. of IEEE Conf. on Robotics and Automation, ICRA1999, May (available at the course web page) Ioannis M. Rekleitis, A Particle Filter Tutorial for Mobile Robot Localization, Technical Report TR-CIM,04-02, McGill University, Canada,

3 Markov Localization prediction phase uses motion model update phase uses measurement model bel(x t ) = p(x t Z t ) bel(x t -1) = p(x t-1 Z t-1 ) bel(x t ) = p(x t Z t ) bel(xt ) = p(x t Z t-1 t,u ) EKF Localization when all densities are represented by unimodal Gaussians 3

4 Markov Localization with General PDFs prior (could be the a posteriori distribution from last iteration) measurement model a posteriori distribution prediction measurement model a posteriori distribution prediction 4

5 Markov Localization with Unimodal Gaussians EKF prior (could be the a posteriori distribution from last iteration) bel(x) prediction measurement model a posteriori distribution prediction bel(x)... 5

6 Monte Carlo Localization (MCL) MCL solves a global localization problem Differences to the unimodal Gaussian technique Can process raw sensor measurements. There is no need to extract features from sensor values; Non parametric. Not bound to a unimodal distribution as the EKF localizer; Can solve global localization and, in some instances, kidnapped robot problems. MCL uses particle filters to estimate posteriors over robot poses 6

7 Particle Filters Represent the posterior belief bel(x t ) by random samples Estimation of non-gaussian, nonlinear processes Instead of representing a pdf by a parametric form, particle filters represent a pdf by a set of samples drawn from this distribution Sample based representation also have the ability to model nonlinear transformations of random variables 7

8 Particle Filters The samples of a posterior distribution: χ t := { x [1] [ t, x 2] [M] t,..., x } t Set of samples at time t x [m] t, m = 1,2,..., M Sample m, at time t concrete instantiation of the state at time t A sample is a hypothesis as to what the true world state may be at time t For the Robot Localization problem in a 2D environment, each sample represents the robot state (position + orientation) bel(x t -1 ) bel(x t ) c t-1 c t Markov Localization Monte Carlo Localization 8

9 Monte Carlo Localization The robot ignores its pose (p(x o ) is uniform) The initial global uncertainty is achieved through a set of pose samples drawn at random and uniformly over the entire pose space. M particles (samples + importance weights); uniform importance M -1 assigned to each particle Note that the particles are not equally spaced in the pose space, but result from a uniform distribution a) bel(x) x 9

10 Monte Carlo Localization The robot senses one door, according to a measurement model given in terms of a likelihood p(z x) The MCL assigns importance factors (weight of each particle) to each particle as shown in bel(x) This set of particles is identical to the one in Fig. a). The only thing modified by the measurement update are the importance weights b) p(z x) x bel(x) x 10

11 Monte Carlo Localization b) bel(x) x c) The particle set is resample (uniform importance weights) and The robot moves bel(x) x A new particle set with uniform importance weights, but with an increased number of particles near the three likely places 11

12 Monte Carlo Localization c) bel(x) x d) The new measurement assigns non-uniform importance weights to the particle set p(z x) x bel(x) x

13 Monte Carlo Localization At this point most of the cumulative mass is centered on the second door, which is also the most likely location d) bel(x) x Further motion leads to another resampling step and a step in which a new particle set is generated according to the motion model e) bel(x) x The particle sets approximate the correct posterior pdf

14 Monte Carlo Localization Initialization From motion model From measurement model Prediction + weighting Re-sampling Prediction + weighting Source: Raja Chatila 14

15 Monte Carlo Localization c t-1 resampling or Importance sampling c t [m] w t importance factor of the particle with < x [i] t, w [i] t > ℵ, i =1,..., m [m] x t c t 15

16 Monte Carlo Localization Represents bel(x t-1 ) by M random samples χ t 1 = { < x [m] t 1 > } 1 m M Samples are updated in two stages: Prediction: according to motion model [m x ] [m t p(x ] [m t x ] t 1,u t ) Correction/Update: Weighing samples according to likelihood of observations w p( z [ m] [ m] t t t x χ t = < x t [m],w t [m ] > ) These particles are distributed according to bel x ) { } The set of particles (weighted samples) represents (in 1 m M ( t approximation) the posterior pdf bel(x t ) 16

17 MCL - Resampling c t { [ m] [ m] < x } t, wt > m M = 1 The set of particles (weighted samples) represents (in approximation) the posterior pdf bel(x t ) Set of M samples [m] x t distributed according to resampling [m] w t Another set of M samples [m] x t distributed according to bel( x t ) bel( x t ) = hp( z t x [ m] t ) bel( x t ) and with associated weights and with equal weights Resampling algorithm The resampling algorithm draws with replacement M samples from the temporary set c t The probability of drawing each sample is given by its importance weight Refocuses the particle set to regions in state space with high posterior probability 17

18 From Probabilistic Robotics S. Thrun, W. Burgard, D.Fox 18

19 From Probabilistic Robotics S. Thrun, W. Burgard, D.Fox 19

20 From Probabilistic Robotics S. Thrun, W. Burgard, D.Fox 2

21 From Probabilistic Robotics S. Thrun, W. Burgard, D.Fox 2

22 From Probabilistic Robotics S. Thrun, W. Burgard, D.Fox 2

23 From Probabilistic Robotics S. Thrun, W. Burgard, D.Fox 23

24 From Probabilistic Robotics S. Thrun, W. Burgard, D.Fox 24

25 From Probabilistic Robotics S. Thrun, W. Burgard, D.Fox 25

26 From Probabilistic Robotics S. Thrun, W. Burgard, D.Fox 2

27 From Probabilistic Robotics S. Thrun, W. Burgard, D.Fox 27

28 From Probabilistic Robotics S. Thrun, W. Burgard, D.Fox 28

29 From Probabilistic Robotics S. Thrun, W. Burgard, D.Fox 29

30 From Probabilistic Robotics S. Thrun, W. Burgard, D.Fox 30

31 From Probabilistic Robotics S. Thrun, W. Burgard, D.Fox 3

32 From Probabilistic Robotics S. Thrun, W. Burgard, D.Fox 3

33 From Probabilistic Robotics S. Thrun, W. Burgard, D.Fox 33

34 From Probabilistic Robotics S. Thrun, W. Burgard, D.Fox 3

35 MCL: Example From Probabilistic Robotics S. Thrun, W. Burgard, D.Fox 35

36 Limitations and Its Overcome Limitations The approach described so far is able to track the pose of a mobile robot and to globally localize the robot. How can we deal with localization errors (i.e., the kidnapped robot problem)? Approaches Randomly insert samples (the robot can be teleported at any point in time). Insert random samples inversely proportional to the average likelihood of the particles (the robot has been teleported with higher probability when the likelihood of its observations drops). 36

37 Importance Sampling with Resampling: Landmark Detection Example 37

38 Importance Sampling with Resampling Weighted samples After resampling 38

39 MCL: Example I From Probabilistic Robotics S. Thrun, W. Burgard, D.Fox 39

40 MCL: Example II Localization in soccer field coordinate x of nearest model line to line point j coordinate x of line point j in the frame of robot pose hypothesis represented by particle m f [m] lpj = ( ) [m] [m] x model,lpj x lpj y model,lpj y lpj D [m] = 1 #lp lp j [m] f lpj 2 Distance matrix: height map represents squared distance of each position on the field to the next field marking line. w [m] = α 1 D [m] From Heinemann, P., Haase, J., & Zell, A. (2006). A Combined Monte-Carlo Localization and Tracking Algorithm for RoboCup. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (2006) (pp ). 40

41 MCL: Example II (cont.) From Heinemann, P., Haase, J., & Zell, A. (2006). A Combined Monte-Carlo Localization and Tracking Algorithm for RoboCup. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (2006) (pp ). 41

42 Particle filter: Summary Particle filters are an implementation of recursive Bayesian filtering They represent the posterior by a set of weighted samples. In the context of localization, the particles are propagated according to the motion model. They are then weighted according to the likelihood of the observations. In a re-sampling step, new particles are drawn with a probability proportional to the likelihood of the observation. 42