Introduction to Mobile Robotics Bayes Filter Particle Filter and Monte Carlo Localization

Transcription

1 Introduction to Mobile Robotics Bayes Filter Particle Filter and Monte Carlo Localization Wolfram Burgard, Cyrill Stachniss, Maren Bennewitz, Kai Arras 1

2 Motivation Recall: Discrete filter Discretize the continuous state space High memory complexity Fixed resolution (does not adapt to the belief) Particle filters are a way to efficiently represent non-gaussian distribution Basic principle Set of state hypotheses ( particles ) Survival-of-the-fittest 2

3 Sample-based Localization (sonar)

4 Mathematical Description Set of weighted samples State hypothesis Importance weight The samples represent the posterior 4

5 Function Approximation Particle sets can be used to approximate functions The more particles fall into an interval, the higher the probability of that interval How to draw samples form a function/distribution? 5

6 Rejection Sampling Let us assume that f(x)<1 for all x Sample x from a uniform distribution Sample c from [0,1] if f(x) > c keep the sample otherwise reject the sampe c x f(x) x f (x ) c OK 6

7 Importance Sampling Principle We can even use a different distribution g to generate samples from f By introducing an importance weight w, we can account for the differences between g and f w = f / g f is often called target g is often called proposal Pre-condition: f(x)>0 à g(x)>0 7

8 Importance Sampling with Resampling: Landmark Detection Example

9 Distributions

10 Distributions Wanted: samples distributed according to p (x z 1, z 2, z 3 ) 10

11 This is Easy! We can draw samples from p(x z l ) by adding noise to the detection parameters.

12 Importance Sampling ),...,, ( ) ( ) ( ),...,, ( : Target distribution f n k k n z z z p x p x z p z z z x p = ) ( ) ( ) ( ) ( Sampling distribution g : l l l z p x p x z p z x p = ),...,, ( ) ( ) ( ) ( ),...,, ( w : Importance weights n l k k l l n z z z p x z p z p z x p z z z x p g f = =

13 Importance Sampling with Resampling Weighted samples After resampling

14 Particle Filters

15 Sensor Information: Importance Sampling Bel( x) w α p( z x) Bel α p( z x) Bel Bel ( x) ( x) ( x) = α p( z x)

16 Robot Motion Bel (, ') ') x x) p( x u x Bel( x d ' The image cannot be displayed. Your computer may not have enough memory to open the image, or the image may have been corrupted. Restart your computer, and then open the file again. If the red x still appears, you may have to delete the image and then insert it again.

17 Sensor Information: Importance Sampling Bel( x) w α p( z x) Bel α p( z x) Bel Bel ( x) ( x) ( x) = α p( z x)

18 Robot Motion Bel ( x) p( x u, x') Bel( x') d x'

19 Particle Filter Algorithm Sample the next generation for particles using the proposal distribution Compute the importance weights : weight = target distribution / proposal distribution Resampling: Replace unlikely samples by more likely ones [Derivation of the MCL equations on the blackboard] 19

20 Particle Filter Algorithm 1. Algorithm particle_filter( S t-1, u t-1 z t ): 2. S t 3. For i =1 n Generate new samples 4. Sample index j(i) from the discrete distribution given by w t-1 i j ( i) 5. Sample from x p x x, uusing ) and i i 6. w = p( z x ) Compute importance weight i 7. η = η + w t Update normalization factor i i 8. S = S { < x, w > } Insert 9. For =, η = 0 t t t i =1 n t t t t t ( t t 1 t 1 xt u 1 t 1 i i 10. w = w /η Normalize weights t t 20

21 draw x i t 1 from Bel(x t 1 ) draw x i t from p(x t xi t 1,u t 1 ) Importance factor for x i t : ) ( ) ( ), ( ) ( ), ( ) ( distribution proposal target distribution t t t t t t t t t t t t i t x z p x Bel u x x p x Bel u x x p x z p w = = η ) ( ), ( ) ( ) ( = t t t t t t t t dx x Bel u x x p x z p x Bel η Particle Filter Algorithm

22 Resampling Given: Set S of weighted samples. Wanted : Random sample, where the probability of drawing x i is given by w i. Typically done n times with replacement to generate new sample set S.

23 Resampling W n-1 w n w 1 w 2 W n-1 w n w 1 w 2 w 3 w 3 Roulette wheel Binary search, n log n Stochastic universal sampling Systematic resampling Linear time complexity Easy to implement, low variance

24 Resampling Algorithm 1. Algorithm systematic_resampling(s,n): 1 2. S ' =, c1 = w 3. For i = 2 n Generate cdf i 4. c i = ci 1 + w 1 5. u ~ U]0, n ], i 1 Initialize threshold 1 = 6. For j =1 n Draw samples 7. While ( u j > c i ) Skip until next threshold reached 8. i = i { i 1 S' = S' < x, n > } Insert 10. u = u 1 n Increment threshold j+ 1 j Return S Also called stochastic universal sampling

25 Mobile Robot Localization Each particle is a potential pose of the robot Proposal distribution is the motion model of the robot (prediction step) The observation model is used to compute the importance weight (correction step) [For details, see PDF file on the lecture web page] 25

26 Motion Model Reminder Start

27 Proximity Sensor Model Reminder Laser sensor Sonar sensor

28 28

29 29

30 30

31 31

32 32

33 33

34 34

35 35

36 36

37 37

38 38

39 39

40 40

41 41

42 42

43 43

44 44

45 45

46 Sample-based Localization (sonar) 46

47 Initial Distribution 47

48 After Incorporating Ten Ultrasound Scans 48

49 After Incorporating 65 Ultrasound Scans 49

50 Estimated Path 50

51 Localization for AIBO robots 51

52 Using Ceiling Maps for Localization [Dellaert et al. 99]

53 Vision-based Localization z P(z x) h(x)

54 Under a Light Measurement z: P(z x):

55 Next to a Light Measurement z: P(z x):

56 Elsewhere Measurement z: P(z x):

57 Global Localization Using Vision

58 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)? 58

59 Approaches Randomly insert samples (the robot can be teleported at any point in time). Insert random samples proportional to the average likelihood of the particles (the robot has been teleported with higher probability when the likelihood of its observations drops). 59

60 Summary Particle Filters Particle filters are an implementation of recursive Bayesian filtering They represent the posterior by a set of weighted samples They can model non-gaussian distributions Proposal to draw new samples Weight to account for the differences between the proposal and the target Monte Carlo filter, Survival of the fittest, Condensation, Bootstrap filter 60

61 Summary PF Localization 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. 61