Recursive Bayes Filtering Advanced AI Wolfram Burgard Tutorial

Recursive Bayes Filtering Advanced AI Wolfram Burgard

Tutorial Goal To familiarize you with probabilistic paradigm in robotics n Basic techniques • Advantages • Pitfalls and limitations n n Successful Applications Open research issues 2

Robotics Yesterday 3

Robotics Today 4

Robotics Tomorrow? 5

6

Tasks to be Solved by Robots § Planning § Perception § Modeling § Localization § Interaction § Acting § Manipulation § Cooperation §. . . 7

Current Trends in Robotics Robots are moving away from factory floors to n n n Entertainment, toys Personal services Medical, surgery Industrial automation (mining, harvesting, …) Hazardous environments (space, underwater) 8

Robo. Cup 9

Physical Agents are Inherently Uncertain n Uncertainty arises from four major factors: Environment stochastic, unpredictable n Robot stochastic n Sensor limited, noisy n Models inaccurate n 10

Example: Museum Tour-Guide Robots Rhino, 1997 Minerva, 1998 11

Rhino (Univ. Bonn + CMU, 1997) W. Burgard, A. B. Cremers, D. Fox, D. Hähnel, G. Lakemeyer, D. Schulz, W. Steiner, S. Thrun 12

Minerva (CMU + Univ. Bonn, 1998) Minerva S. Thrun, M. Beetz, M. Bennewitz, W. Burgard, A. B. Cremers, F. Dellaert, D. Fox, D. Hähnel, C. Rosenberg, N. Roy, J. Schulte, D. Schulz 13

Technical Challenges n Navigation n n n Environment crowded, unpredictable Environment unmodified “Invisible” hazards Walking speed or faster High failure costs Interaction n n Individuals and crowds Museum visitors’ first encounter Age 2 through 99 Spend less than 15 minutes 14

Nature of Sensor Data Odometry Data Range Data 15

Probabilistic Techniques for Physical Agents Key idea: Explicit representation of uncertainty using the calculus of probability theory Perception = state estimation Action = utility optimization 16

Advantages of Probabilistic Paradigm Can accommodate inaccurate models n Can accommodate imperfect sensors n Robust in real-world applications n Best known approach to many hard robotics problems n 17

Pitfalls Computationally demanding n False assumptions n Approximate n 18

Outline n n Introduction Probabilistic State Estimation Robot Localization Probabilistic Decision Making n n Planning Between MDPs and POMDPs Exploration Conclusions 19

Axioms of Probability Theory Pr(A) denotes probability that proposition A is true. n n n 20

A Closer Look at Axiom 3 B 21

Using the Axioms 22

Discrete Random Variables n X denotes a random variable. n X can take on a finite number of values in {x 1, x 2, …, xn}. n P(X=xi), or P(xi), is the probability that the random variable X takes on value xi. n P(. ) is called probability mass function. n E. g. 23

Continuous Random Variables n X takes on values in the continuum. n p(X=x), or p(x), is a probability density function. p(x) n E. g. x 24

Joint and Conditional Probability n P(X=x and Y=y) = P(x, y) n If X and Y are independent then P(x, y) = P(x) P(y) n P(x | y) is the probability of x given y P(x | y) = P(x, y) / P(y) P(x, y) = P(x | y) P(y) n If X and Y are independent then P(x | y) = P(x) 25

Law of Total Probability, Marginals Discrete case Continuous case 26

Bayes Formula 27

Normalization Algorithm: 28

Conditioning n Total probability: n Bayes rule and background knowledge: 29

Simple Example of State Estimation n n Suppose a robot obtains measurement z What is P(open|z)? 30

Causal vs. Diagnostic Reasoning P(open|z) is diagnostic. n P(z|open) is causal. n Often causal knowledge is easier to obtain. count frequencies! n Bayes rule allows us to use causal knowledge: n 31

Example n n P(z|open) = 0. 6 P(z| open) = 0. 3 P(open) = P( open) = 0. 5 • z raises the probability that the door is open. 32

Combining Evidence n Suppose our robot obtains another observation z 2. n How can we integrate this new information? n More generally, how can we estimate P(x| z 1. . . zn )? 33

Recursive Bayesian Updating Markov assumption: zn is independent of z 1, . . . , zn-1 if we know x. 34

Example: Second Measurement n P(z 2|open) = 0. 5 n P(open|z 1)=2/3 P(z 2| open) = 0. 6 • z 2 lowers the probability that the door is open. 35

A Typical Pitfall Two possible locations x 1 and x 2 n P(x 1)= 1 -P(x 2)= 0. 99 n P(z|x 2)=0. 09 P(z|x 1)=0. 07 n 36

Actions n Often the world is dynamic since actions carried out by the robot, n actions carried out by other agents, n or just the time passing by n change the world. n How can we incorporate such actions? 37

Typical Actions n n n The robot turns its wheels to move The robot uses its manipulator to grasp an object Plants grow over time… Actions are never carried out with absolute certainty. In contrast to measurements, actions generally increase the uncertainty. 38

Modeling Actions n To incorporate the outcome of an action u into the current “belief”, we use the conditional pdf P(x|u, x’) n This term specifies the pdf that executing u changes the state from x’ to x. 39

Example: Closing the door 40

State Transitions P(x|u, x’) for u = “close door”: If the door is open, the action “close door” succeeds in 90% of all cases. 41

Integrating the Outcome of Actions Continuous case: Discrete case: 42

Example: The Resulting Belief 43

Bayes Filters: Framework n Given: n Stream of observations z and action data u: n n Sensor model P(z|x). Action model P(x|u, x’). Prior probability of the system state P(x). Wanted: n Estimate of the state X of a dynamical system. n The posterior of the state is also called Belief: 44

Markov Assumption Zt-1 Xt-1 Zt+1 Zt ut-1 Xt ut Xt+1 Underlying Assumptions n Static world n Independent noise n Perfect model, no approximation errors 45

Bayes Filters z = observation u = action x = state Bayes Markov Total prob. Markov 46

Bayes Filter Algorithm 1. 2. Algorithm Bayes_filter( Bel(x), d ): h=0 3. 4. 5. 6. 7. 8. if d is a perceptual data item z then For all x do 9. else if d is an action data item u then For all x do 10. 11. For all x do 12. return Bel’(x) 47

Bayes Filters are Familiar! n n n Kalman filters Particle filters Hidden Markov models Dynamic Bayes networks Partially Observable Markov Decision Processes (POMDPs) 48

Application to Door State Estimation n n Estimate the opening angle of a door and the state of other dynamic objects using a laser-range finder from a moving mobile robot and based on Bayes filters. 49

Result 50

Lessons Learned n Bayes rule allows us to compute probabilities that are hard to assess otherwise. n Under the Markov assumption, recursive Bayesian updating can be used to efficiently combine evidence. n Bayes filters are a probabilistic tool for estimating the state of dynamic systems. 51

Tutorial Outline n n Introduction Probabilistic State Estimation Localization Probabilistic Decision Making n n Planning Between MDPs and POMDPs Exploration Conclusions 52

The Localization Problem “Using sensory information to locate the robot in its environment is the most fundamental problem to providing a mobile robot with autonomous capabilities. ” [Cox ’ 91] n Given n Wanted n n Map of the environment. Sequence of sensor measurements. Estimate of the robot’s position. Problem classes n n n Position tracking Global localization Kidnapped robot problem (recovery) 53

Representations for Bayesian Robot Localization Kalman filters (late-80 s? ) Discrete approaches (’ 95) • Topological representation (’ 95) • uncertainty handling (POMDPs) • occas. global localization, recovery • Grid-based, metric representation (’ 96) • global localization, recovery Particle filters (’ 99) • sample-based representation • global localization, recovery AI • Gaussians • approximately linear models • position tracking Robotics Multi-hypothesis (’ 00) • multiple Kalman filters • global localization, recovery 54

What is the Right Representation? n Kalman filters n Multi-hypothesis tracking n Grid-based representations n Topological approaches n Particle filters 57

Gaussians m Univariate -s s m Multivariate 58

Kalman Filters Estimate the state of processes that are governed by the following linear stochastic difference equation. The random variables vt and wt represent the process measurement noise and are assumed to be independent, white and with normal probability distributions. 59

Kalman Filters [Schiele et al. 94], [Weiß et al. 94], [Borenstein 96], [Gutmann et al. 96, 98], [Arras 98] 60

Kalman Filter Algorithm 1. Algorithm Kalman_filter( <m, S>, d ): 2. 3. 4. 5. If d is a perceptual data item z then 6. 7. 8. Else if d is an action data item u then 9. Return <m, S> 61

Non-linear Systems n Very strong assumptions: Linear state dynamics n Observations linear in state n n What can we do if system is not linear? Linearize it: EKF n Compute the Jacobians of the dynamics and observations at the current state. n Extended Kalman filter works surprisingly well even for highly non-linear systems. n 62

Kalman Filter-based Systems (1) n [Gutmann et al. 96, 98]: n Match LRF scans against map n Highly successful in Robo. Cup mid-size league Courtesy of S. Gutmann 63

Kalman Filter-based Systems (2) Courtesy of S. Gutmann 64

Kalman Filter-based Systems (3) n [Arras et al. 98]: n Laser range-finder and vision n High precision (<1 cm accuracy) Courtesy of K. Arras 65

Multihypothesis Tracking [Cox 92], [Jensfelt, Kristensen 99] 67

Localization With MHT n Belief is represented by multiple hypotheses n Each hypothesis is tracked by a Kalman filter n Additional problems: n Data association: Which observation corresponds to which hypothesis? n Hypothesis management: When to add / delete hypotheses? n Huge body of literature on target tracking, motion correspondence etc. See e. g. [Cox 93] 68

MHT: Implemented System (1) n [Jensfelt and Kristensen 99, 01] n Hypotheses are extracted from LRF scans n Each hypothesis has probability of being the correct one: n Hypothesis probability is computed using Bayes’ rule n Hypotheses with low probability are deleted n New candidates are extracted from LRF scans 69

MHT: Implemented System (2) Courtesy of P. Jensfelt and S. Kristensen 70

MHT: Implemented System (3) Example run # hypotheses P(Hbest) Map and trajectory Courtesy of P. Jensfelt and S. Kristensen Hypotheses vs. time 71

Piecewise Constant [Burgard et al. 96, 98], [Fox et al. 99], [Konolige et al. 99] 73

Piecewise Constant Representation 74

Grid-based Localization 75

Tree-based Representations (1) Idea: Represent density using a variant of Octrees 76

Xavier: Localization in a Topological Map [Simmons and Koenig 96] 79

Particle Filters § § Represent density by random samples § Monte Carlo filter, Survival of the fittest, Condensation, Bootstrap filter, Particle filter § § § Filtering: [Rubin, 88], [Gordon et al. , 93], [Kitagawa 96] Estimation of non-Gaussian, nonlinear processes Computer vision: [Isard and Blake 96, 98] Dynamic Bayesian Networks: [Kanazawa et al. , 95] 81

MCL: Global Localization 84

MCL: Sensor Update 85

MCL: Robot Motion 86

MCL: Sensor Update 87

MCL: Robot Motion 88

Particle Filter Algorithm 1. Algorithm particle_filter( St-1, ut-1 zt): 2. 3. For Generate new samples 4. Sample index j(i) from the discrete distribution given by wt-1 5. Sample from using and 6. Compute importance weight 7. Update normalization factor 8. Insert 9. For 10. Normalize weights 89

Resampling n Given: Set S of weighted samples. n Wanted : Random sample, where the probability of drawing xi is given by wi. n Typically done n times with replacement to generate new sample set S’. 91

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

Motion Model p(xt | at-1, xt-1) Model odometry error as Gaussian noise on a, b, and d 94

Motion Model p(xt | at-1, xt-1) Start 95

Model for Proximity Sensors The sensor is reflected either by a known or by an unknown obstacle: Laser sensor Sonar sensor 96

MCL: Global Localization (Sonar) [Fox et al. , 99] 116

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

Vision-based Localization P(s|x) s h(x) 121

MCL: Global Localization Using Vision 125

Localization for AIBO robots 127

Adaptive Sampling 128

KLD-sampling • Idea: • Assume we know the true belief. • Represent this belief as a multinomial distribution. • Determine number of samples such that we can guarantee that, with probability (1 - d), the KL-distance between the true posterior and the sample-based approximation is less than e. • Observation: • For fixed d and e, number of samples only depends on number k of bins with support: 129

MCL: Adaptive Sampling (Sonar) 130

Particle Filters for Robot Localization (Summary) n Approximate Bayes Estimation/Filtering n Full posterior estimation n Converges in O(1/ #samples) [Tanner’ 93] n Robust: multiple hypotheses with degree of belief n Efficient in low-dimensional spaces: focuses computation where needed n Any-time: by varying number of samples n Easy to implement 133

Localization Algorithms - Comparison Kalman filter Multihypothesis tracking Topological maps (fixed/variable) Sensors Gaussian Features Non-Gaussian Non. Gaussian Posterior Gaussian Multi-modal Piecewise constant Samples Efficiency (memory) ++ ++ ++ -/+ +/++ Efficiency (time) ++ ++ ++ o/+ +/++ Implementation + o + +/o ++ ++ ++ - +/++ ++ - + + ++ +/++ No Yes Yes Accuracy Robustness Global localization Grid-based Particle filter 134

Multi-robot Localization: Idea [Fox et al. 00] 135

Robot Detection Camera image Laser scan 136

Multi-robot Localization • Combined belief state has dimension 3 N complexity grows exponentially in number of robots • Factorial representation of the belief • Perform localization for each robot and use detections to constrain the beliefs 137

Density Trees Integration of robot detection requires a density 138

Example Run 139

Pitfall: The World is not Markov! Distance filters: [Fox et al 1998] 143

Condensation: Contour Tracking [M. Isard and A. Blake, 98] 145

Condensation: Mixed State Tracking [M. Isard and A. Blake] 146

Localization: Lessons Learned Probabilistic Localization = Bayes filters n Particle filters: Approximate posterior by random samples n Extensions: n Filter for dynamic environments n Safe avoidance of invisible hazards n People tracking n Recovery from total failures n Active Localization n Multi-robot localization n 147