Humanoid Robotics. Monte Carlo Localization. Maren Bennewitz

Transcription

1 Humanoid Robotics Monte Carlo Localization Maren Bennewitz 1

2 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

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

4 Markov Assumption actions state of a dynamical system observations 4

5 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

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

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

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

9 Recursive Bayes Filter 2 Bayes rule 9

10 Recursive Bayes Filter 3 independence assumption 10

11 Recursive Bayes Filter 4 Law of total probability 11

12 Recursive Bayes Filter 5 Markov assumption 12

13 Recursive Bayes Filter 6 independence assumption 13

14 Recursive Bayes Filter 7 recursive term 14

15 Recursive Bayes Filter 7 motion model 15

16 Recursive Bayes Filter 7 observation model 16

17 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

18 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

19 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

20 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

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

22 Particle Filter Monte Carlo Localization 22

23 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

24 Motivation Goal: Approach for dealing with arbitrary distributions 24

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

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

27 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

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 [The weight has be chosen in exactly that way if the samples are drawn from the motion model] 28

29 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

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

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

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

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

34 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

35 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

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

37 initialization Courtesy: Thrun, Burgard, Fox 37

38 observation Courtesy: Thrun, Burgard, Fox 38

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

40 observation Courtesy: Thrun, Burgard, Fox 40

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

42 motion update Courtesy: Thrun, Burgard, Fox 42

43 observation Courtesy: Thrun, Burgard, Fox 43

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

45 motion update Courtesy: Thrun, Burgard, Fox 45

46 observation Courtesy: Thrun, Burgard, Fox 46

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

48 motion update Courtesy: Thrun, Burgard, Fox 48

49 observation Courtesy: Thrun, Burgard, Fox 49

50 50

51 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

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

53 6D Localization for Humanoid Robots 53

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

55 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

56 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

57 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

58 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

59 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

60 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

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

62 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

63 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

64 Sampling from the Motion Model We need to draw samples from a Gaussian Gaussian 65

65 How to Sample from a Gaussian Drawing from a 1D Gaussian can be done in closed form Example with 10 6 samples 66

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

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

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

69 Motion Model uncalibrated motion model: ground truth odometry (uncalibrated) Resulting particle distribution (2000 particles) Nao walking straight for 40cm 70

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

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

72 Observation Model Range data: Raycasting or endpoint model in 3D map 73

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

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

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

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

77

78 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

79 Trajectory and Error Over Time 80

80 Trajectory and Error Over Time 81

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

82 Literature Chapter 3, Humanoid Robot Navigation in Complex Indoor Environments, Armin Hornung, PhD thesis, Univ. Freiburg,

83 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

84 Acknowledgment Previous versions of parts of the slides have been created by Wolfram Burgard, Armin Hornung, and Cyrill Stachniss 87