1

Robot Motion and Perception(ch. 5, 6)

• These two chapters discuss how one obtains the motion model and measurement model mentioned before.

• They are needed for implementing Bayes filter algorithm,

• And the Bayes filter algorithm is the mother of all other algorithms in the book….

2

Robot Motion and Perception(ch. 5, 6)

•What’s your thought on motion model and measurement model?

p(x’|x, u), p(z|x)

•How would you get them? Supplied by somebody else?

3

Robot Motion and Perception(ch. 5, 6)

•Kinematics is the calculus describing the effect of control actions on the configuration of a robot.

•The configuration of a robot….• Euler Angle, roll, pitch, yaw,…• http://www.nasm.si.edu/exhibitions/gal109/NEWHTF/HTF541B.HTM

•See figure 5.1.

•The pose of a robot (location+bearing).

4

Probabilistic Kinematics

Key question:

We may know where our robot is supposed to be, but in reality it might be somewhere else…

VR(t)

VL (t)

starting position

supposed final pose

x

y

lots of possibilities for the actual final pose

What should we do?

5

running around in squares

• Create a program that will run your robot in a square (~2m to a side), pausing after each side before turning and proceeding.

1

2

3

4

• For 10 runs, collect both the odometric estimates of where the robot thinks it is and where the robot actually is after each side. • You should end up with two sets of 30 angle measurements and 40 length measurements: one set from odometry and one from “ground-truth.”• Find the mean and the standard deviation of the differences between odometry and ground truth for the angles and for the lengths – this is the robot’s motion uncertainty model.

start and “end”

This will provide a probabilistic kinematic model.

MODEL the error in order to reason about it!

6SA-1SA-1

Probabilistic Robotics

Probabilistic Motion Models

7

Robot Motion

• Robot motion is inherently uncertain.

• How can we model this uncertainty?

8

Dynamic Bayesian Network for Controls, States, and Sensations

9

Probabilistic Motion Models

•To implement the Bayes Filter, we need the transition model p(x | x’, u).

•The term p(x | x’, u) specifies a posterior probability, that action u carries the robot from x’ to x.

• In this section we will specify, how p(x | x’, u) can be modeled based on the motion equations.

10

Coordinate Systems

• In general the configuration of a robot can be described by six parameters.

• Three-dimensional cartesian coordinates plus three Euler angles pitch, roll, and tilt.

• Throughout this section, we consider robots operating on a planar surface.

• The state space of such systems is three-dimensional (x,y,).

11

Typical Motion Models

• In practice, one often finds two types of motion models:• Odometry-based

• Velocity-based (dead reckoning)

• Odometry-based models are used when systems are equipped with wheel encoders.

• Velocity-based models have to be applied when no wheel encoders are given.

• They calculate the new pose based on the velocities and the time elapsed.

12

Typical Motion Models

• In practice, Odometry models tend to be more accurate than Velocity models.

•Odometry-based models are suitable for estimation, but not for planning.

•Velocity-based models are used for probabilistic motion planning.

13

Reasons for Motion Errors

bump

ideal case different wheeldiameters

carpetand many more …

14

Odometry Model—Set up

tx

1tx•At time t, the correct pose of the robot is modeled by the random variable .

•The robot needs to estimate this pose.

•In the time interval (t-1, t], the robot advances from a pose to pose .

•The odometry reports back a related advance from to .

•The bar here indicates that there are measurements.

tx

Tt yxx ),,(1

Tt yxx )',','(1

15

Odometry Model—Set up

•The key insight here is to use the difference between and as a good estimator for the difference of the true pose and .

•The motion is given by

1tx tx1tx tx

tu

t

tt x

xu 1

•The motion is transformed into a sequence of three steps: a rotation, a straight line motion (translation), and another rotation.

•See Figure 5.7

tu

transrotrott

tt x

xu ,, 21

1

16

Odometry Model

22 )'()'( yyxxtrans

)','(atan21 xxyyrot

12 ' rotrot

• Robot moves from to .

• Odometry information .

,, yx ',',' yx

transrotrotu ,, 21

trans1rot

2rot

,, yx

',',' yx

17

The atan2 Function

• Extends the inverse tangent and correctly copes with the signs of x and y.

18

Noise Model for Odometry

•The measured motion is given by the true motion corrupted with noise.

||||11 211

ˆtransrotrotrot

||||22 221

ˆtransrotrotrot

|||| 2143

ˆrotrottranstranstrans

19

Typical Distributions for Probabilistic Motion Models

2

2

22

1

22

1)(

x

ex

2

2

2

6

||6

6|x|if0)(2

xx

Normal distribution Triangular distribution

20

Calculating the Probability (zero-centered)

• For a normal distribution

• For a triangular distribution

1. Algorithm prob_normal_distribution(a,b):

2. return

1. Algorithm prob_triangular_distribution(a,b):

2. return

21

Calculating the Posterior Given x, x’, and u

22 )'()'( yyxxtrans )','(atan21 xxyyrot

12 ' rotrot 22 )'()'(ˆ yyxxtrans )','(atan2ˆ

1 xxyyrot

12ˆ'ˆrotrot

)ˆ|ˆ|,ˆ(prob trans21rot11rot1rot1 p|))ˆ||ˆ(|ˆ,ˆ(prob rot2rot14trans3transtrans2 p

)ˆ|ˆ|,ˆ(prob trans22rot12rot2rot3 p

1. Algorithm motion_model_odometry(x,x’,u)

2.

3.

4.

5.

6.

7.

8.

9.

10.

11. return p1 · p2 · p3

odometry values (u)

values of interest (x,x’)

22

Application

• Repeated application of the sensor model for short movements.

• Typical banana-shaped distributions obtained for 2d-projection of 3d posterior.

• See Figure 5.8.

x’u

p(x|u,x’)

u

x’

23

Sample-based Density Representation

24

Sample-based Density Representation

25

How to Sample from Normal or Triangular Distributions?

• Sampling from a normal distribution

• Sampling from a triangular distribution

1. Algorithm sample_normal_distribution(b):

2. return

1. Algorithm sample_triangular_distribution(b):

2. return

26

Normally Distributed Samples

106 samples

27

For Triangular Distribution

103 samples 104 samples

106 samples105 samples

28

Sample Odometry Motion Model

1. Algorithm sample_motion_model(u, x):

1.

2.

3.

4.

5.

6.

7. Return

)||sample(ˆ21111 transrotrotrot

|))||(|sample(ˆ2143 rotrottranstranstrans

)||sample(ˆ22122 transrotrotrot

)ˆcos(ˆ' 1rottransxx )ˆsin(ˆ' 1rottransyy

21ˆˆ' rotrot

',',' yx

,,,,, 21 yxxu transrotrot

sample_normal_distribution

29

Sampling from Our Motion Model

Start

30

Examples (Odometry-Based)

31

Velocity-Based Model

32

Equation for the Velocity Model

Center of circle:

with

33

Posterior Probability for Velocity Model

34

Sampling from Velocity Model

35

Examples (velocity based)

36

Map-Consistent Motion Model

)',|( xuxp ),',|( mxuxp

)',|()|(),',|( xuxpmxpmxuxp Approximation:

37

Summary

• We discussed motion models for odometry-based and velocity-based systems

• We discussed ways to calculate the posterior probability p(x| x’, u).

• We also described how to sample from p(x| x’, u).

• Typically the calculations are done in fixed time intervals t.

• In practice, the parameters of the models have to be learned.

• We also discussed an extended motion model that takes the map into account.