Robots Autónomos. Depto. CCIA. 2. Bayesian Estimation and sensor models. Domingo Gallardo

Transcription

1 Robots Autónomos 2. Bayesian Estimation and sensor models Domingo Gallardo Depto. CCIA

2 References Recursive State Estimation: Thrun, chapter 2 Sensor models and robot perception: Thrun, chapter 6 Depto DCCIA - Robots Autónomos

3 Contents Probability theory and robotics Sensor models Dynamic state estimation Bayesian filter Depto DCCIA - Robots Autónomos

4 Autonomous Robotics We are interested in controlling autonomous mobile robots that continuously sense and manoeuvre in a semistructured environment with a defined goal. Interesting Robot guides Robot transporters Cars Vacuum cleaners Not interesting Manipulators Humanoids AGVs Game avatars Doctorado Depto. CCIA - Robots Autónomos 3/64

5 Uncertainty in Robotics Sensors obtain imprecise readings of the environment. From them we want to infer (estimate) the most probable state of the world. We have to model error and imprecision. Doctorado Depto. CCIA - Robots Autónomos 4/64

6 Probability theory and Robotics Random variables and probability functions to model readings. Variance and covariance to model error. Bayes theorem to infer unknown data. Expectations and Bayesian Filters to integrate the readings. Doctorado Depto. CCIA - Robots Autónomos 17/64

7 Remembering basic concepts Axioms of probability Random variables Probability Density Functions Joint distributions Bayes rule Expectation, variance, covariance and correlation Doctorado Depto. CCIA - Robots Autónomos 18/64

8 Axioms of probability Let A, B,... be propositions about the outcomes of a given experiment. The number P (A) represents the probability that the proposition A is true after performing the experiment. It must satisfy the following three axioms: P (A) > 0 P (True) =1 If A and B are exclusive (there are no outcome in which both are true) then P (A B) =P (A)+P (B) Doctorado Depto. CCIA - Robots Autónomos 19/64

9 Frequencies It s usual to interpret probabilities as relative frequencies We count the number of times that A is true and the number of executions of the experiment. Let s suppose that our robot tries to enter in the room 30 times: A (Open) = (25) B (Someone) = (12) C (Chair) = (2) P (A) = =0.83, P(B) =12 30 =0.4, P(C) = 2 30 =0.07 Doctorado Depto. CCIA - Robots Autónomos 21/64

10 Discrete Random Variables X denotes a random variable X can take on a contable number of values in {x 1,x 2,...,x n } P (X = x i ) or P (x i ) is the probability that an outcome of the experiment produces the value x i if: p(x i ) 0 i p(x i )=1 Doctorado Depto. CCIA - Robots Autónomos 23/64

11 Continuous Random Variables X takes on values in the continuum p(x = x) or p(x) is a probability density function or PDF iff x p(x) 0 p(x) dx =1 Doctorado Depto. CCIA - Robots Autónomos 24/64

12 PDF and cumulative function The probability of a event gives an outcome between values a and b is P (a <X b) = b a p(x)dx The cumulative distribution function is: P (X x) = x p(x) dx Relation between the PDF and the cumulative distribution: p(x) = lim h 0 P (x h 2 <X x + h 2 ) h = P (X x) x Doctorado Depto. CCIA - Robots Autónomos 25/64

13 Understanding p(x) What s the gut-feel meaning of p(x)? If then p(a) p(b) = α when a value x is sampled from the distribution, you are α times as likely to find that x is very close to a than that x is very close to b. Doctorado Depto. CCIA - Robots Autónomos 26/64

14 Some Probability Density Functions Uniform: p(x) = 0 if x<a 1 b a if a<x<b 0 if x>b Triangular: p(x) = 2(x a) (b a)(c a) 2(b x) (b a)(b c) if a x c if c<x b Doctorado Depto. CCIA - Robots Autónomos 27/64

15 Exponential pdf p(x; λ) = λe λx x 0, 0 x<0 Doctorado Depto. CCIA - Robots Autónomos 28/64

16 Normal (Gaussian) pdf p(x; µ, σ 2 )=N(µ, σ 2 ) = (2πσ 2 ) 1 2 exp 1 2 µ = mean, σ 2 = variance (x µ) 2 σ 2 Doctorado Depto. CCIA - Robots Autónomos 29/64

17 Joint distribution The joint distribution of two random variables X and Y is given by: p(x, y) =P (X = x and Y = y) Conditional probability: the probality of X = x given that Y = y is denoted by p(x y). p(x y) = p(x, y) p(y) If X and Y are independents then p(x, y) =p(x)p(y) p(x y) =p(x) Doctorado Depto. CCIA - Robots Autónomos 30/64

18 Multivariate Normal distributions If x is a multi-dimensional vector (column vector): x =(x 1,...,x n ) T the density function of the multivariate Normal distribution is: p(x; µ, Σ) = det(2πσ) 1 2 exp{ 1 2 (x µ)t σ 1 (x µ)} Means and covariances: µ =(µ 1,...,µ n ) T Σ = σ σ 1n σ n1... σn 2 Doctorado Depto. CCIA - Robots Autónomos 32/64

19 Expectations µ = E[X] is the expected value of random variable X E[X] = x= xp(x) dx The average value we d see if we took a very large number of random samples of X The first moment of the shape formed by the axes and the curve The best value to choose if you must guess an unknown value drawn from the distribution and you ll be fined the square of your error Doctorado Depto. CCIA - Robots Autónomos 42/64

20 Expectation of a function E[f(X)] is the expected value of f(x) where x is drawn from X s distribution E[f(X)] = x= f(x) p(x) dx The average value we d see if we took a very large number of random samples of f(x) Note that in general E[f(X)] = f(e[x]) If f(x) is a linear function: E[aX + b] = ae[x]+ b Doctorado Depto. CCIA - Robots Autónomos 43/64

21 Variance The variance of a random variable gives an indication of how the values of a distribution are separated from the mean. We ll use it as an indication of the quality of an estimation. Var[X] =E[(X µ) 2 ]= = x= (x µ) 2 p(x) dx Var[X] =E[X 2 ] (E[X])] 2 The standard deviation σ x is defined as Var[X] Doctorado Depto. CCIA - Robots Autónomos 44/64

22 Covariance and correlation (1) The covariance Cov[X, Y ] of two random variables X and Y is defined as Cov[X, Y ] = E[(X µ x )(Y µ y )] = E[XY ] E[X]E[Y ] The correlation coefficient ρ xy is defined by the ratio: ρ xy = Cov[X, Y ] σ x σ y Doctorado Depto. CCIA - Robots Autónomos 45/64

23 Covariance and correlation (2) Some properties: ρ xy 1 Cov[X, Y ] σ x σ y Two variables are called uncorrelated if their covariance is 0, which is equivalent to: Cov[X, Y ]=0 ρ xy =0 E[XY ]=E[X]E[Y ] Independence: if X and Y are independent then they are uncorrelated. The reverse is not in general true. However, if X and Y are normal random variables and their correlation coefficient is 0, then they are independent. Doctorado Depto. CCIA - Robots Autónomos 46/64

24 Covariance matrix Given n random variables {X 1,...,X n } their covariance matrix is defined by: being Σ = C C 1n C n1... C nn C ij = Cov[X i,x j ] Doctorado Depto. CCIA - Robots Autónomos 47/64

25 Example: measurement errors We measure an object of length η with n instruments of varying accuracies. The results of the measurements are n random variables x i = η + ν i E[ν i ]=0 Var[ν i ]=E[ν 2 i ]=σ 2 i where ν i are the measurement errors which we assume independent with zero mean. Our problem is to determine the unbiased, minimum variance, linear estimation of η. We wish to find n constants α i such that the sum ˆη = α 1 x α n x n is a random variable with mean E[ˆη] =α 1 E[x 1 ]+ + α n E[x n ]=η and its variance V = α 2 1σ α 2 nσ 2 n is minimum, subject to the constraint: α α n =1 Doctorado Depto. CCIA - Robots Autónomos 48/64

26 Solution measurement errors To solve this problem, we note that V = α 2 1σ α 2 nσ 2 n λ(α 1...α n 1) for any λ (Lagrange multiplier). Hence V is minimum if δv =2α i σi 2 =0 α i = λ δα i 2σi 2 Inserting into the constraint and solving for λ, we obtain λ 2 = V = 1 1/σ /σ2 n Hence ˆη = x i/σ x n /σ 2 n 1/σ /σ2 n Doctorado Depto. CCIA - Robots Autónomos 49/64

27 Algebraic manipulation Based on three rules: the product rule, the sum rule and the Bayes theorem Doctorado Depto. CCIA - Robots Autónomos 36/64

28 Product rule Also called the chain rule, it s derived from the definition of conditional probability p(x, y H) = p(x y, H) p(y H) = p(y x, H) p(x H) For generality we include the value H which represents all other knwon or assumed quantities. Doctorado Depto. CCIA - Robots Autónomos 37/64

29 Sum rule Also called the total probability theorem, the sum rule follows from the definition of marginalisation p(x H) = = p(x, y H) dy p(x y, H) p(y H) dy Doctorado Depto. CCIA - Robots Autónomos 38/64

30 Bayes rule The Bayes theorem is derived from the product rule p(x y, H) = = p(y x, H) p(x H) p(y H) p(y x, H) p(x H) p(y x, H) p(x H) dx The denominator is a normalizing constant that does not depend on x and such that p(x y, H) integrates to one. We can then can formulate the rule in this simpler way: p(x y, H) =η p(y x, H) p(x H) Doctorado Depto. CCIA - Robots Autónomos 39/64

31 Using the Bayes rule P (h D) = P (D h)p (h) P (D) h is the ignored variable (hypothesis) that we want to infer D is the data that we know P (h) is the a priori probability of h P (D h) is the likelihood of obtaining the data D if the hypothesis h is satisfied P (h D) is the a posteriori probability of h Doctorado Depto. CCIA - Robots Autónomos 40/64

32 Maximum a posteriori P (h D) = P (D h)p (h) P (D) Generally want the most probable hypothesis given the training data Maximum a posteriori hypothesis h MAP : h MAP = arg max h H = arg max h H = arg max h H P (h D) P (D h)p (h) P (D) P (D h)p (h) Doctorado Depto. CCIA - Robots Autónomos 41/64

33 Sensors for Mobile Robots Contact sensors: Bumpers Internal sensors Accelerometers (spring-mounted masses) Gyroscopes (spinning mass, laser light) Compasses, inclinometers (earth magnetic field, gravity) Proximity sensors Sonar (time of flight) Radar (phase and frequency) Laser range-finders (triangulation, tof, phase) Infrared (intensity) Visual sensors: Cameras Satellite-based sensors: GPS Depto DCCIA - Robots Autónomos

34 Proximity sensors The central task is to determine P(z x), i.e., the probability of a measurement z given that the robot is at position x. This is called the likelihood of the measurement Depto DCCIA - Robots Autónomos

35 Beam-based sensor model Scan z consists of K measurements. Individual measurements are independent given the robot position. Depto DCCIA - Robots Autónomos

36 Beam-based sensor model Depto DCCIA - Robots Autónomos

37 Typical errors of range measurements Depto DCCIA - Robots Autónomos

38 Proximity measurement Measurement can be caused by a known obstacle. cross-talk. an unexpected obstacle (people, furniture, ). missing all obstacles (total reflection, glass, ). Noise is due to uncertainty in measuring distance to known obstacle. in position of known obstacles. in position of additional obstacles. whether obstacle is missed. Depto DCCIA - Robots Autónomos

39 Beam-based proximity model Measurement noise Unexpected obstacles 0 z exp z max 0 z exp z max Depto DCCIA - Robots Autónomos

40 Beam-based proximity model Depto DCCIA - Robots Autónomos

41 Resulting mixture density How can we determine the model parameters? Depto DCCIA - Robots Autónomos

42 Raw sensor data Measured distances for expected distance of 300 cm. Sonar Laser Depto DCCIA - Robots Autónomos

43 Approximation Maximize log likelihood of the data Search space of n-1 parameters. Hill climbing Gradient descent Genetic algorithms Deterministically compute the n-th parameter to satisfy normalization constraint. Depto DCCIA - Robots Autónomos

44 Approximation results Laser Sonar 300cm 400cm Depto DCCIA - Robots Autónomos

45 Example z P(z x,m) Depto DCCIA - Robots Autónomos

46 Landmarks Active beacons (e.g., radio, GPS) Passive (e.g., visual, retro-reflective) Standard approach is triangulation Sensor provides distance, or bearing, or distance and bearing. Depto DCCIA - Robots Autónomos

47 Distance and bearing Depto DCCIA - Robots Autónomos

48 Computing the likelihood of a landmark measurement Depto DCCIA - Robots Autónomos

49 Data asociation problem Assume that landmarks cannot be uniquely identified Data association problem: the observed features must be matched with the previous registered landmarks Methods based on likelihood of features readings given the previous landmarks Depto DCCIA - Robots Autónomos

50 Summary of sensors models Explicitly modeling uncertainty in sensing is key to robustness. In many cases, good models can be found by the following approach: 1. Determine parametric model of noise free measurement. 2. Analyze sources of noise. 3. Add adequate noise to parameters (eventually mix in densities for noise). 4. Learn (and verify) parameters by fitting model to data. 5. Likelihood of measurement is given by probabilistically comparing the actual with the expected measurement. This holds for motion models as well. It is extremely important to be aware of the underlying assumptions! Depto DCCIA - Robots Autónomos

51 Bayesian Filters One of the most important techniques in probabilistic robotics is the use of bayesian filters to: Estimate the state of the robot after integrate the likelihood of the sensor perceptions and update it with the robot action model Doctorado Depto. CCIA - Robots Autónomos 14/64

52 An example: Markov Localization (1) (a) bel(x) x (b) p(z x) x bel(x) x (c) bel(x) x (d) Doctorado Depto. CCIA - Robots Autónomos 15/64

53 An example: Markov Localization (2) (d) p(z x) x bel(x) x (e) bel(x) x Doctorado Depto. CCIA - Robots Autónomos 16/64

54 Dynamic State Estimation Introduction State estimation with a single observation Application: laser scan likelihood Probabilistic State Dynamics Integrating everything: the Bayes Filter Doctorado Depto. CCIA - Robots Autónomos 50/64

55 Introduction The problem is to track the sequence of states {x 1, x 2,...,x t } of a dynamical system. The states can not be measured directly, but by means of observations. We obtain a set of t observations {z 1, z 2,...,z t } observations corresponding to the t states. The initial state has a distribution p(x 1 ). Examples: Robot Localization Missile Guidance Tracking of objects in video sequences Doctorado Depto. CCIA - Robots Autónomos 51/64

56 State Estimation with Single Observation Given the observation z = {z 1,z 2,...,z n } We want to obtain a posterior estimation (named belief) of the current state x given that observation Bel(x) p(x z 1,...,z n ) Doctorado Depto. CCIA - Robots Autónomos 52/64

57 Example z 1 = texture descriptor z 2 = number of edges... x {(door-open, dist), (door-closed, dist), (wall, dist)} Doctorado Depto. CCIA - Robots Autónomos 53/64

58 State estimation with single observation p(x z 1,...,z n )= p(z n x,z 1,...,z n 1 )p(x z 1,...,z n 1 ) p(z 1,...,z n ) Independence assumption: z i {z 1,...,z i 1,z i+1,...,z n } Doctorado Depto. CCIA - Robots Autónomos 54/64

59 State estimation with single observation p(x z 1,...,z n ) = p(z n x,z 1,...,z n 1 )p(x z 1,...,z n 1 ) p(z 1,...,z n ) = ηp(z n x,z 1,...,z n 1 )p(x z 1,...,z n 1 ) = η 1...n p(z i x)p(x) i n Doctorado Depto. CCIA - Robots Autónomos 55/64

60 A harder version of the problem z P(z x,m) Given an occupancy grid m and a laser reading z Compute the likelihood of having obtained the reading from the position x Doctorado Depto. CCIA - Robots Autónomos 11/64

61 Laser Scan Likelihood [S. Thrun, Artificial Intelligence, 2001] Doctorado Depto. CCIA - Robots Autónomos 56/64

62 Laser Scan Likelihood Doctorado Depto. CCIA - Robots Autónomos 57/64

63 Probabilistic State Dynamics The current state dependend on the previous state: p(x t x t 1 ) And only on it (Markov assumption): p(x t x t 1, x t 2,...,x 1 )=p(x t x t 1 ) Example: p(x t x t 1 ) e 1 2 (x t x t 1 1) 2 [M. Isard, 1998] Doctorado Depto. CCIA - Robots Autónomos 58/64

64 Modeling Actions The actions u change the world state according to a distribution p(x t x t 1, u), being x t 1 the state of the world before executing the action u that leads to the state x t Robot motion model: Doctorado Depto. CCIA - Robots Autónomos 59/64

65 Integrating everything: Bayes Filter We obtain a recursive formulation Bel(x t ) = ηp(z t x t )p(x t u t 1,...,z 1 ) Total prob = ηp(z t x t ) p(x t x t 1 u t 1,...,z 1 )p(x t 1 u t 1,...,z 1 )dx x t 1 Markov = ηp(z t x t ) p(x t x t 1, u t 1 )p(x t 1 u t 1,...,z 1 )dx t 1 x t 1 = p(z t x t ) p(x t x t 1, u t 1 )Bel(x t 1 )dx t 1 x t 1 Doctorado Depto. CCIA - Robots Autónomos 60/64

66 The Bayes Filter Given: A robot action u t A sensor reading z t A a-priori robot state belief estimation bel(x t 1 )=P(x t 1 ) Computes: The a-posteriori robot state belief estimate bel(x t )=P (x t x t 1,u t,z t ) Doctorado Depto. CCIA - Robots Autónomos 61/64

67 The Bayes Filter Algorithm Bel(x t )=p(z t x t ) x t 1 p(x t x t 1, u t 1 )Bel(x t 1 )dx t 1 Algorithm Bayes Filter (bel(x t 1 ),z t,u t ): Estimate the new state from the previous and the action 1. For all x t X: 2. bel (x i )= bel(x t 1 )P (x t u t,x t 1 ) Incorporate the observations in the belief 3. bel(x t )=ηp(z t x t )bel (x t ) 4. return bel(x t ) Doctorado Depto. CCIA - Robots Autónomos 62/64

68 Bayes Filter Techniques Bel(x t )=p(z t x t ) x t 1 p(x t x t 1, u t 1 )Bel(x t 1 )dx t 1 Kalman Filter Particle Filter Hidden Markov Models Dynamic Bayes Networks Partially Observable Markov Decision Process Doctorado Depto. CCIA - Robots Autónomos 63/64