E190Q Lecture 10 Autonomous Robot Navigation

Transcription

1 E190Q Lecture 10 Autonomous Robot Navigation Instructor: Chris Clark Semester: Spring Figures courtesy of Siegwart & Nourbakhsh

2 Kilobots 2

3 Control Structures Planning Based Control Prior Knowledge Operator Commands Localization Cognition Perception Motion Control 3

4 Particle Filter Localization: Outline 1. Particle Filters 1. What are particles? 2. Algorithm Overview 3. Algorithm Example 4. Using the particles 2. PFL Application Example 4

5 What is a particle? Like Markov localization, PFs represent the belief state with a set of discrete possible states, and assigning a probability of being in each of the possible states. 5 Unlike Markov localization, the set of possible states are not constructed by discretizing the configuration space, they are a randomly generated set of particles.

6 What is a particle? A particle is an individual state estimate. A particle is defined by its: 1. State values that determine its location in the configuration space, e.g. x = [ x y θ ] 2. A probability that indicates it s likelihood. 6

7 What is a particle? Particle filters use many particles to for representing the belief state. 7

8 What is a particle? Example: A Particle filter uses 3 particles to represent the position of a (white) robot in a square room. If the robot has a perfect compass, each particle is described as: x [1] = [ x 1 y 1 ] x [2] = [ x 2 y 2 ] x [3] = [ x 3 y 3 ] x [1] x [2] x [3] x 0 8

9 What is a particle? Example: Each of the particles x [1], x [2], x [3] also have associated weights w [1], w [2], w [3]. In the example below, x [2] should have the highest weight if the filter is working. x [1] x [3] x [2] x 0 9

10 What is a particle? The user can choose how many particles to use: More particles ensures a higher likelihood of converging to the correct belief state Fewer particles may be necessary to ensure realtime implementation 10

11 Particle Filter Localization: Outline 1. Particle Filters 1. What are particles? 2. Algorithm Overview 3. Algorithm Example 4. Using the particles 2. PFL Application Example 11

12 Markov Localization Particle Filter Algorithm (Initialize at t =0): Randomly draw N states in the work space and add them to the set X 0. X 0 ={x 0 [1], x 0 [2],, x 0 [N] } Iterate on these N states over time (see next slide). 12

13 Markov Localization Particle Filter Algorithm (Loop over time step t ): 1. For i = 1 N 2. Pick x [i] t-1 from X t-1 3. Draw x [i] t with probability P( x [i] t x [i] t-1, o t ) 4. Calculate w [i] t = P( z t x [i] t ) Prediction 5. Add x t [i] to X t Predict 6. For j = 1 N 7. Draw x t [j] from X t Predict with probability w t [j] Correction Add x t [j] to X t

14 Particle Filter Localization: Outline 1. Particle Filters 1. What are particles? 2. Algorithm Overview 3. Algorithm Example 4. Using the particles 2. PFL Application Example 14

15 Particle Filter Example Provided is an example where a robot (depicted below), starts at some unknown location in the bounded workspace. x 0 15

16 Particle Filter Example At time step t 0 : We randomly pick N=3 states represented as X 0 ={x [1] 0, x [2] 0, x [3] 0 } For simplicity, assume known heading x 0 [1] x 0 [3] x 0 [2] x 0 16

17 Particle Filter Example The next few slides provide an example of one iteration of the algorithm, given X 0. This iteration is for time step t 1. The inputs are the measurement z 1, odometry o 1 x 0 [1] x 0 [3] x 0 [2] x 0 17

18 Particle Filter Example For Time step t 1 : Randomly generate new states by propagating previous states X 0 with o 1 X 1 Predict ={x 1 [1], x 1 [2], x 1 [3] } x 1 [1] x 1 [3] x 1 [2] x 1 18

19 Particle Filter Example For Time step t 1 : To get new states, use the motion model from lecture 3 to randomly generate new state x [i] 1. Recall that given some Δs r and Δs l we can calculate the robot state in global coordinates: 19

20 Particle Filter Example 20 For Time step t 1 : If you add some random errors ε r and ε l to Δs r and Δs l, you can generate a new random state that follows the probability distribution dictated by the motion model. So, in the prediction step of the PF, the i th particle can be randomly propagated forward using measured odometry o 1 = [ Δs r Δs l ] according to: Δs r [i] = Δs r + rand( norm, 0, σ s ) Δs l [i] = Δs l + rand( norm, 0, σ s )

21 Particle Filter Example For Time step t 1 : For example: x 0 [i] o 1 x 1 [i] 21

22 Particle Filter Example Example Prediction Steps 22 Yiannis, McGill University, PF Tutorial

23 Particle Filter Example Example Prediction Steps 23 Yiannis, McGill University, PF Tutorial

24 Particle Filter Example For Time step t 1 : We get a new measurement z 1, e.g. a forward facing range measurement. x 1 [1] x 1 [3] x 1 [2] x 1 24 z 1

25 Particle Filter Example For Time step t 1 : Using the measurement z 1, and expected measurements µ 1 [i], calculate the weights w [i] = P( z 1 x 1 [i] ) for each state. x 1 [1], w 1 [1] x 1 [3], w 1 [3] µ 1 [1] µ 1 [3] x 1 [2], w 1 [2] x 1 25 µ 1 [2] z 1

26 Particle Filter Example For Time step t 1 : To calculate P( z 1 x 1 [i] ) we use the sensor probability distribution of a single Gaussian of mean µ 1 [i] that is the expected range for the particle The Gaussian variance is obtained from experiment. P(z 1 x 1 [i] ) 26 z 1 µ 1 [i]

27 Particle Filter Example For Time step t 1 : Resample from the temporary state distribution based on the weights w 1 [2] > w 1 [1] > w 1 [3] X 1 ={x 1 [2], x 1 [2], x 1 [1] } x 1 [1] x 1 [2] x 1 27

28 Particle Filter Example For Time step t 1 : How do we resample? Exact Method Approximate Method Others 28

29 Particle Filter Example An Exact Method w tot = Σ j w j for i=1..n r = rand( uniform )*w tot j =1 w sum = w 1 while (w sum < r) j =j+1 29 x i =x j Predict w sum = w sum + w j

30 Particle Filter Example An Approximate Method w tot = max j w j for i =1..N w i = w i / w tot if w i <0.25 else if w i <0.50 else if w i <0.75 else if w i <1.00 add 1 copy of x i Predict to X TEMP add 2 copies of x i Predict to X TEMP add 3 copies of x i Predict to X TEMP add 4 copies of x i Predict to X TEMP 30

31 Particle Filter Example An Approximate Method (cont ) for i =1..N r = (int) rand( uniform )*size(x TEMP ) x i = x r TEMP 31

32 Particle Filter Example NOTE: We should only resample when we get NEW measurements. 32

33 Particle Filter Example For Time step t 2 : Iterate on previous steps to update state belief at time step t 2 given (X 1, o 2, z 2 ). 33

34 Particle Filter Localization: Outline 1. Particle Filters 1. What are particles? 2. Algorithm Overview 3. Algorithm Example 4. Using the particles 2. PFL Application Example 34

35 Additional Notes How do we use the belief? To control the robot, we often distill the belief into a lower dimension representation. Examples: x 1 = Σ i w 1 [i] x 1 [i] Σ i w 1 [i] x 1 = { x 1 [i] w 1 [i] > w 1 [j] j i } 35

36 Additional Notes How do we use the belief? Sometimes we have several clusters Lets introduce a new algorithm 36

37 Additional Notes K-means Clustering Given: Find: A set of N data points X = { x [1], x [2],.. x [N] } The number of clusters k N The k hyperplanes which best divide the data points into k clusters 37

38 Additional Notes Subtractive Clustering Given: Find: A set of N data points X = { x [1], x [2],.. x [N] } Neighborhood Radius r A The k data points which best divide the data points into k clusters c 2 c 1 38

39 Additional Notes Subtractive Clustering Algorithm (initialization) // Calculate Potential Values P i for i = 1..N P i = Σ j exp( - x [i] - x 1 [j] 2 / (0.5 r A ) 2 ) // Define first centroid center c 1 c 1 = { x 1 [m] P m > P j j m } PotVal(c 1 ) = P m 39 Chen, Qin, Jia Weighted Mean Subtractive Clustering Algorithm (2008)

40 Additional Notes Subtractive Clustering Algorithm (iterations) k =1 while (! stoppingcriteria) // Update Potential Values for i = 1..N P i = P i PotVal(c k ) exp( - x [i] c k 2 / (0.75 r A ) 2 ) 40 // Calculate k th centroid c k = { x 1 [m] P m > P j j m } PotVal(c k ) = P m k=k+1

41 Additional Notes Subtractive Clustering Algorithm (iterations) The stoppingcriteria can take on many forms: max i ( P i ) < threshold 41

42 Additional Notes Subtractive Clustering Algorithm Example for N =7 c 2 c 1 42

43 Particle Filter Localization: Outline 1. Particle Filters 1. What are particles? 2. Algorithm Overview 3. Algorithm Example 4. Using the particles 2. PFL Application Example 43

44 Markov Localization 44 Courtesy of S. Thrun

45 Markov Localization 45 Courtesy of S. Thrun

46 46 Markov Localization

47 Markov Localization 47