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

1

Humanoid Robotics Monte Carlo Localization

Maren Bennewitz

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

3

Basis Probability Rules (2)

Law of total probability: continuous case discrete case

4

Markov Assumption

state of a dynamical system

• bservations

actions

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

6

Motivation

§ Estimate the pose of a robot in a given

model of the environment

§ Based on its sensor data and odometry

measurements

7

State Estimation

§ Estimate the state given observations

and odometry measurements/actions

§ Goal: Calculate the distribution

8

Recursive Bayes Filter 1

Definition of the belief all data up to time t

9

Recursive Bayes Filter 2

Bayes’ rule

10

Recursive Bayes Filter 3

independence assumption

11

Recursive Bayes Filter 4

Law of total probability

12

Recursive Bayes Filter 5

Markov assumption

13

Recursive Bayes Filter 6

independence assumption

14

Recursive Bayes Filter 7

recursive term

15

Recursive Bayes Filter 7

motion model

16

Recursive Bayes Filter 7

• bservation model

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

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”

19

§ 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

Simplest Ray-Cast Model

measured distance

• bject in map

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

21

Summary

§ Bayes filter is a framework for state

estimation

§ The motion and observation model are the

central components of the Bayes filter

22

Particle Filter Monte Carlo Localization

23

Particle Filter

§ One implementation of the recursive Bayes

filter

§ Non-parametric framework (not only

Gaussian)

§ Arbitrary models can be used as motion and

• bservation models

24

Motivation

§ Goal: Approach for dealing with arbitrary

distributions

25

Key Idea: Samples

§ Use multiple (weighted) samples to

represent arbitrary distributions

samples

26

Particle Set

§ Set of weighted samples

state hypothesis importance weight

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

• bservation 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

[The weight has be chosen in exactly that way if the samples are drawn from the motion model]

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)

30

MCL – Correction Step

Image Courtesy: S. Thrun, W. Burgard, D. Fox

31

MCL – Prediction Step

Image Courtesy: S. Thrun, W. Burgard, D. Fox

32

MCL – Correction Step

Image Courtesy: S. Thrun, W. Burgard, D. Fox

33

MCL – Prediction Step

Image Courtesy: S. Thrun, W. Burgard, D. Fox

34

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

35

w2 w3 w1 wn Wn-1

Resampling

w2 w3 w1 wn Wn-1

§ 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”

step size = 1/J initial value between 0 and 1/J “roulette wheel” Assumption: normalized weights

36

Low Variance Resampling

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

37

initialization

Courtesy: Thrun, Burgard, Fox

38

• bservation

Courtesy: Thrun, Burgard, Fox

39

weight update, resampling, motion update

Courtesy: Thrun, Burgard, Fox

40

• bservation

Courtesy: Thrun, Burgard, Fox

41

weight update, resampling

Courtesy: Thrun, Burgard, Fox

42

motion update

Courtesy: Thrun, Burgard, Fox

43

• bservation

Courtesy: Thrun, Burgard, Fox

44

weight update, resampling

Courtesy: Thrun, Burgard, Fox

45

motion update

Courtesy: Thrun, Burgard, Fox

46

• bservation

Courtesy: Thrun, Burgard, Fox

47

weight update, resampling

Courtesy: Thrun, Burgard, Fox

48

motion update

Courtesy: Thrun, Burgard, Fox

49

• bservation

Courtesy: Thrun, Burgard, Fox

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)

52

Literature

§ Book: Probabilistic Robotics,

• S. Thrun, W. Burgard, and D. Fox

53

6D Localization for Humanoid Robots

54

Localization for Humanoids

§ 3D environments require a 6D pose

estimate

height yaw, pitch, roll 2D position estimate the 6D torso pose

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

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

57

Kinematic Walking Odometry

Frfoot Ftorso Fodom

fixed frame frame of the current stance foot frame of the torso, transform can be computed with FK over the right leg

§ Keep track of the transform to the current

stance foot, starting with the identity

58

Kinematic Walking Odometry

Frfoot Ftorso Fodom Frfoot Ftorso Fodom

§ Both feet remain on the ground, compute

the transform to the frame of the left foot with FK

Frfoot Flfoot Ftorso Fodom

59

Kinematic Walking Odometry

Frfoot Ftorso Fodom Frfoot Ftorso Fodom Frfoot Flfoot Ftorso Fodom

Flfoot Ftorso Fodom

§ The left leg becomes the stance leg and is

the new reference to compute the transform to the torso frame

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

61

Odometry Estimate

§ Odometry estimate from two consecutive

torso poses

ut

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

• ver time

§ The particle filter has to account for

that noise within the motion model

63

Motion Model

§ Noise modelled as a Gaussian with

systematic drift on the 2D plane

§ Prediction step samples a new pose

according to

§ learned with least squares optimization

using ground truth data (as in Ch. 2)

covariance matrix calibration matrix motion composition

65

Sampling from the Motion Model

§ We need to draw samples from a

Gaussian

Gaussian

66

How to Sample from a Gaussian

§ Drawing from a 1D Gaussian can be done in

closed form

Example with 106 samples

67

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

68

Sampling from the Odometry

§ We know how to sample from a Gaussian § We sample an own motion for each particle § We then incorporate the sampled

• dometry into the pose of particle i

for each particle same distribution for each step

69

Example for Sampled Motions in the 2D Plane

large angular error, small translational error small angular error, larger translational error robot poses according to estimated odometry samples drawn from Gaussian in motion model

70

Motion Model

• dometry (uncalibrated)

ground truth

uncalibrated motion model:

§ Resulting particle distribution (2000 particles) § Nao walking straight for 40cm

71

Motion Model

• dometry (uncalibrated)

ground truth

• dometry (uncalibrated)

ground truth

uncalibrated motion model: calibrated motion model: properly captures drift, closer to the ground truth

72

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)

73

Observation Model

Range data: Raycasting or endpoint model in 3D map

74

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

• bject in map

75

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

76

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

77

Likelihoods of Measurements

§ Gaussian distribution

standard deviation corresponding to noise characteristics

• f joint encoders

and IMU computed from the height difference to the closest ground level in the map

79

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

80

Trajectory and Error Over Time

81

Trajectory and Error Over Time

84

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

85

Literature

§ Chapter 3, Humanoid Robot Navigation in

Complex Indoor Environments,

Armin Hornung, PhD thesis, Univ. Freiburg, 2014

86

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

87

Acknowledgment

§ Previous versions of parts of the slides have

been created by Wolfram Burgard, Armin Hornung, and Cyrill Stachniss