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 hypothesis importance 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 times ( =#particles). Repeat 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
Resampling roulette wheel Assumption: normalized weights initial value between 0 and 1/J step size = 1/J W n-1 w n w 1 w 2 W n-1 w n w 1 w 2 w 3 w 3 Draw randomly between 0 and 1 Binary search Repeat J times O(J log J) Systematic resampling Low variance O(J) Also called stochastic 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 F torso frame of the torso, transform can be computed 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 torso poses from two consecutive 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 matrix covariance matrix motion composition learned with least squares optimization using ground truth data (as in Ch. 2) 63
Sampling from the Motion Model We need to draw samples from a Gaussian Gaussian 65
How to Sample from a Gaussian Drawing from a 1D Gaussian can be done in closed form Example with 10 6 samples 66
How to Sample from a Gaussian Drawing from a 1D Gaussian can be done in closed form If we consider the individual dimensions of the odometry as independent, we can draw each dimension using a 1D Gaussian draw each of the dimensions independently 67
Sampling from the Odometry We know how to sample from a Gaussian We sample an own motion for each particle for each particle same distribution for each step We then incorporate the sampled odometry into the pose of particle i 68
Example for Sampled Motions in the 2D Plane samples drawn from Gaussian in motion model robot poses according to estimated odometry small angular error, larger translational error large angular error, small translational error 69
Motion Model uncalibrated motion model: ground truth odometry (uncalibrated) Resulting particle distribution (2000 particles) Nao walking straight for 40cm 70
Motion Model uncalibrated motion model: ground truth odometry (uncalibrated) calibrated motion model: ground truth odometry (uncalibrated) properly captures drift, closer to the ground truth 71
Observation Model Observation consists of independent range, height, and IMU (roll and pitch ) measurements is computed from the values of the joint encoders and are provided by the IMU (inertial measurement unit) 72
Observation Model Range data: Raycasting or endpoint model in 3D map 73
Recap: 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 74
Recap: 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 75
Observation Model Range data: Ray-casting or endpoint model in 3D map Torso height: Compare measured value from kinematics to predicted height (accord. to motion model) IMU data: Compare measured roll and pitch to the predicted angles Use individual Gaussians to evaluate the difference 76
Likelihoods of Measurements Gaussian distribution computed from the height difference to the closest ground level in the map standard deviation corresponding to noise characteristics of joint encoders and IMU 77
Localization Evaluation Trajectory of 5m, 10 runs each Ground truth from external motion capture system Raycasting results in significantly smaller error Calibrated motion model requires fewer particles 79
Trajectory and Error Over Time 80
Trajectory and Error Over Time 81
Summary Estimation of the 6D torso pose (globally and local tracking) in a given 3D model Odometry estimate from kinematic walking Sample an own motion for each particle from a Gaussian Use individual Gaussians in the observation model to evaluate the differences between measured and expected values Particle filter allows to locally track and globally estimate the robot s pose 84
Literature Chapter 3, Humanoid Robot Navigation in Complex Indoor Environments, Armin Hornung, PhD thesis, Univ. Freiburg, 2014 85
Next Lecture Inaugural lecture: Wed, May 20 at 5 p.m., lecture hall III on the university's main campus (talk in German, slides in English) No lecture on Thu, May 21 Lecture 6 of the course: Thu, June 11 86
Acknowledgment Previous versions of parts of the slides have been created by Wolfram Burgard, Armin Hornung, and Cyrill Stachniss 87