Humanoid Robotics Monte Carlo Localization Maren Bennewitz 1 - PowerPoint PPT Presentation

Humanoid Robotics Monte Carlo Localization Maren Bennewitz 1

Basis Probability Rules (1) If x and y are independent: Bayes’ rule: Often written as: The denominator is a normalizing constant that ensures that the posterior of the left hand side adds up to 1 over all possible values of x . In case the background knowledge, Bayes' rule turns into 2

Basis Probability Rules (2) Law of total probability: continuous case discrete case 3

Markov Assumption actions state of a dynamical system observations 4

Motivation § To perform useful service, a robot needs to know its pose in its environment § Motion commands are only executed inaccurately § Here: Estimate the global pose of a robot in a given model of the environment § Based on its sensor data and odometry measurements § Sensor data: Range image with depth measurements or laser range readings 5

Motivation § Estimate the pose of a robot in a given model of the environment § Based on its sensor data and odometry measurements 6

State Estimation § Estimate the state given observations and odometry measurements/actions § Goal: Calculate the distribution 7

Recursive Bayes Filter 1 Definition of the belief all data up to time t 8

Recursive Bayes Filter 2 Bayes’ rule 9

Recursive Bayes Filter 3 independence assumption 10

Recursive Bayes Filter 4 Law of total probability 11

Recursive Bayes Filter 5 Markov assumption 12

Recursive Bayes Filter 6 independence assumption 13

Recursive Bayes Filter 7 recursive term 14

Recursive Bayes Filter 7 motion model 15

Recursive Bayes Filter 7 observation model 16

Probabilistic Motion Model § Robots execute motion commands only inaccurately § The motion model specifies the probability that action u carries the robot from to : § Defined individually for each type or robot 17

Model for Range Readings § Sensor data consists of measurements § The individual measurements are independent given the robot’s pose § “How well can the distance measurements be explained given the pose and the map” 18

Simplest Ray-Cast Model § Ray-cast models consider the first obstacle along the ray § Compare the actually measured distance with the expected distance § Use a Gaussian to evaluate the difference measured distance object in map 19

Beam-Endpoint Model § Evaluate the distance of the hypothetical beam end point to the closest obstacle in the map with a Gaussian Courtesy: Thrun, Burgard, Fox 20

Summary § Bayes filter is a framework for state estimation § The motion and observation model are the central components of the Bayes filter 21

Particle Filter Monte Carlo Localization 22

Particle Filter § One implementation of the recursive Bayes filter § Non-parametric framework (not only Gaussian) § Arbitrary models can be used as motion and observation models 23

Motivation § Goal: Approach for dealing with arbitrary distributions 24

Key Idea: Samples § Use multiple (weighted) samples to represent arbitrary distributions samples 25

Particle Set § Set of weighted samples state importance hypothesis weight 26

Particle Filter § Recursive Bayes filter § Non-parametric approach § The set of weighted particles approximates the belief about the robot’s pose § Two main steps to update the belief § Prediction: Draw samples from the motion model (propagate forward) § Correction: Weight samples based on the observation model 27

Monte Carlo Localization § Each particle is a pose hypothesis § Prediction : For each particle sample a new pose from the the motion model § Correction : Weight samples according to the observation model [The weight has be chosen in exactly that way if the samples are drawn from the motion model] 28

Monte Carlo Localization § Each particle is a pose hypothesis § Prediction : For each particle sample a new pose from the the motion model § Correction : Weight samples according to the observation model § Resampling : Draw sample with probability and repeat times ( =#particles) 29

MCL – Correction Step Image Courtesy: S. Thrun, W. Burgard, D. Fox 30

MCL – Prediction Step Image Courtesy: S. Thrun, W. Burgard, D. Fox 31

MCL – Correction Step Image Courtesy: S. Thrun, W. Burgard, D. Fox 32

MCL – Prediction Step Image Courtesy: S. Thrun, W. Burgard, D. Fox 33

Resampling § Draw sample with probability . Repeat times ( =#particles) § Informally: “ Replace unlikely samples by more likely ones ” § Survival-of-the-fittest principle § “Trick” to avoid that many samples cover unlikely states § Needed since we have a limited number of samples 34

Assumption: normalized weights Resampling initial value between 0 and 1/J step size = 1/J “roulette wheel” w 1 w n w 1 w n w 2 w 2 W n-1 W n-1 w 3 w 3 § Systematic resampling § Draw randomly § Low variance between 0 and 1 § Binary search § O( J ) § Repeat J times § Also called “stochastic § O( J log J ) universal resampling” 35

Low Variance Resampling initialization cumulative sum of weights J = #particles step size = 1/J decide whether or not to take particle i 36

initialization Courtesy: Thrun, Burgard, Fox 37

observation Courtesy: Thrun, Burgard, Fox 38

weight update, resampling, motion update Courtesy: Thrun, Burgard, Fox 39

observation Courtesy: Thrun, Burgard, Fox 40

weight update, resampling Courtesy: Thrun, Burgard, Fox 41

motion update Courtesy: Thrun, Burgard, Fox 42

observation Courtesy: Thrun, Burgard, Fox 43

weight update, resampling Courtesy: Thrun, Burgard, Fox 44

motion update Courtesy: Thrun, Burgard, Fox 45

observation Courtesy: Thrun, Burgard, Fox 46

weight update, resampling Courtesy: Thrun, Burgard, Fox 47

motion update Courtesy: Thrun, Burgard, Fox 48

observation Courtesy: Thrun, Burgard, Fox 49

50

Summary – Particle Filters § Particle filters are non-parametric, recursive Bayes filters § The belief about the state is represented by a set of weighted samples § The motion model is used to draw the samples for the next time step § The weights of the particles are computed based on the observation model § Resampling is carried out based on weights § Called: Monte-Carlo localization (MCL) 51

Literature § Book: Probabilistic Robotics, S. Thrun, W. Burgard, and D. Fox 52

6D Localization for Humanoid Robots 53

Localization for Humanoids § 3D environments require a 6D pose estimate 2D position height yaw, pitch, roll estimate the 6D torso pose 54

Localization for Humanoids § Recursively estimate the distribution about the robot‘s pose using MCL § Probability distribution represented by pose hypotheses (particles) § Needed: Motion model and observation model 55

Motion Model § The odometry estimate corresponds to the incremental motion of the torso § is computed from forward kinematics (FK) from the current stance foot while walking 56

Kinematic Walking Odometry § Keep track of the transform to the current stance foot, starting with the identity frame of the torso, transform can be computed F torso with FK over the right leg F odom F rfoot fixed frame frame of the current stance foot 57

Kinematic Walking Odometry § Both feet remain on the ground, compute the transform to the frame of the left foot with FK F torso F torso F torso F odom F odom F odom F rfoot F rfoot F rfoot F lfoot 58

Kinematic Walking Odometry § The left leg becomes the stance leg and is the new reference to compute the transform to the torso frame F torso F torso F torso F torso F odom F odom F odom F odom F rfoot F rfoot F rfoot F lfoot F lfoot 59

Kinematic Walking Odometry § Using FK, the poses of all joints and sensors can be computed relative to the stance foot at each time step § The transform from the fixed world frame to the stance foot is updated whenever the swing foot becomes the new stance foot § Concatenate the accumulated transform to the previous stance foot and the relative transform to the new stance foot 60

Odometry Estimate § Odometry estimate from two consecutive torso poses u t 61

Odometry Estimate § The incremental motion of the torso is computed from kinematics of the legs § Typically error sources: Slippage on the ground and backlash in the joints § Accordingly, only noisy odometry estimates while walking, drift accumulates over time § The particle filter has to account for that noise within the motion model 62

Motion Model § Noise modelled as a Gaussian with systematic drift on the 2D plane § Prediction step samples a new pose according to calibration covariance matrix matrix motion composition § learned with least squares optimization using ground truth data (as in Ch. 2) 63