Probabilistic Models of Sensing and Movement
This project explores complex behaviors in robotics through probabilistic models of sensing and movement. We delve into sensor interpretation challenges and their implications for goal-directed motion and reactive behaviors. Key concepts include particle filter implementation, Markov localization, and various filtering techniques like Kalman filters and discrete Bayes filters. We focus on improving localization accuracy, addressing issues like the kidnapped robot problem, and integrating random sampling methods for better state estimation. This comprehensive examination will provide insights into efficient robot navigation strategies.
Probabilistic Models of Sensing and Movement
E N D
Presentation Transcript
Probabilistic Models of Sensing and Movement • Move to probability models of sensing and movement • Project 2 is about complex behavior using sensing • Sensor interpretation is difficult – simple interpretation in this section • Artifacts [goal-directed motion] and reactive behaviors • Lectures • Probabilistic sensor models • Probabilistic representation of uncertain movement • Particle filter implementation • Project • PF for motion model • Markov localization with PF • Stretch – feature-based localization Slides thanks to Steffen Gutmann CS225B Kurt Konolige
Markov Localization • Bayes filter • Motion: • Sense: • Implementations • Discrete filter • Particle filter • Kalman filter • Multi-Hypotheses tracking (MHT) • … CS225B Kurt Konolige
Discrete Filter • Piecewise constant • Histogram • Probability grid CS225B Kurt Konolige
Discrete Bayes Filter Algorithm • Algorithm Discrete_Bayes_filter( Bel(x),d ): • h=0 • Ifd is a perceptual data item z then • For all x do • For all x do • Else ifd is an action data item uthen • For all x do • ReturnBel’(x) CS225B Kurt Konolige
Piecewise Constant Representation CS225B Kurt Konolige
Grid-based Localization CS225B Kurt Konolige
Xavier: Localization in a Topological Map [Courtesy of Reid Simmons] CS225B Kurt Konolige
Sample-based Localization (Sonar) CS225B Kurt Konolige
Particle Filters • Represent belief by random samples + weight state + weight • Estimation of non-Gaussian, nonlinear processes • Monte Carlo filter, Survival of the fittest, Condensation, Bootstrap filter, Particle filter • Filtering: [Rubin, 88], [Gordon et al., 93], [Kitagawa 96] • Computer vision: [Isard and Blake 96, 98] • Dynamic Bayesian Networks: [Kanazawa et al., 95] CS225B Kurt Konolige
Robot Motion Sample: CS225B Kurt Konolige
Motion Model in Particle Filter Start CS225B Kurt Konolige
Sensor Information: Importance Sampling CS225B Kurt Konolige
Importance Sampling Weight samples: w = f / g CS225B Kurt Konolige
Importance Sampling with Resampling Draw sample i with probability Weighted samples After resampling CS225B Kurt Konolige
draw xit-1from Bel(xt-1) draw xitfrom p(xt | xit-1,ut) Importance factor for xit: Particle Filter Algorithm CS225B Kurt Konolige
Particle Filter Algorithm • Algorithm particle_filter(Xt-1, ut, zt): • for Generate new samples • Insert into temporary set • endfor • forResample • draw i with probability • Insert into final set • endfor • return Xt CS225B Kurt Konolige
Resampling • Given: Set X of weighted samples. • Wanted : Random sample, where the probability of drawing xi is given by wi. • Typically done n times with replacement to generate new sample set X’. CS225B Kurt Konolige
w1 wn w2 Wn-1 w1 wn w2 Wn-1 w3 w3 Resampling • Stochastic universal sampling • Systematic resampling • Linear time complexity • Easy to implement, low variance • Roulette wheel • Binary search, n log n CS225B Kurt Konolige
Resampling Algorithm • Algorithm systematic_resampling(): • Initialize threshold • for • Increment threshold • while Skip until next threshold • endwhile • Insert • endfor • returnXt CS225B Kurt Konolige
Berkeley Street Localization [Freuh] CS225B Kurt Konolige
Limitations • The approach described so far is able • to globally localize the robot, and • to track the pose of a mobile robot • How can we deal with localization errors such as • the kidnapped robot problem, or • recovery from localization failures? CS225B Kurt Konolige
Approaches • Random samples: • During re-sampling, insert a few random samples (the robot can be teleported at any point in time) • Insert random samples inverse proportional to the average likelihood of the particles (the robot has been teleported with higher probability when the likelihood of its observations drops). • Samples drawn from observations: p(x|z) • Inverse proportional to average likelihood of particles (Sensor Resetting Localization) [Lenser, Veloso, 2000] • Constant number but weight each sample by motion model (Mixture MCL) [Thrun et al., 2000] • Proportional to noise in observations estimated over time (Adaptive MCL) [Fox 2002] CS225B Kurt Konolige
Particle Filter with Random Samples • Algorithm particle_filter_random(Xt-1, ut, zt): • for Generate new samples • Insert into temporary set • endfor • forResample • with probability pranddo • Insert random pose • else • draw i with probability • Insert sample • endwith • endfor • return Xt CS225B Kurt Konolige
Recovery from Failure failure CS225B Kurt Konolige
Random SamplesVision-Based Localization 936 Images, invariant features for matching images Trajectory of the robot: CS225B Kurt Konolige
Kidnapping the Robot CS225B Kurt Konolige
Adaptive Monte Carlo Localization • Algorithm Augmented_MCL(Xt-1, ut, zt): • static wslow, wfast • forGenerate new samples • Insert into temporary set • endfor • forResample • with probability max{0, 1– wfast / wslow} do • Insert random pose • else • draw i with probability • Insert sample • endwith • endfor • return Xt CS225B Kurt Konolige
