Motion Models (cont) 1 2/15/2012

Transcription

1 Motion Models (cont 1

2 Odometry Motion Model the key to computing p( xt ut, xt 1 for the odometry motion model is to remember that the robot has an internal estimate of its pose θ x t 1 x y θ θ true poses x t x y θ

3 Odometry Motion Model the key to computing p( xt ut, xt 1 for the odometry motion model is to remember that the robot has an internal estimate of its pose x' x x t y' x t 1 y θ ' θ ' θ θ robot s internal poses 3

4 Odometry Motion Model the control vector is made up of the three motions made by the robot u t rot use the robot s internal pose estimates to compute the + ( x' x ( y' y atan( y' y, x' θ x θ θ rot ' 4

5 Odometry Motion Model use the true poses to compute the ˆ ˆ rot 1 ( x' x + ( y' y atan( y' y, x' x θ ˆ θ ' θ ˆ rot rot 1 as with the velocity motion model, we have to solve the inverse kinematics problem here 5

6 Odometry Motion Model recall the model ˆ ε α ˆ ˆ 3 + α 4 ( rot 1 + ˆ ε α ˆ ˆ 1 rot 1 + α ˆ rot rot ε α ˆ + α ˆ rot ˆ 1 rot which makes it easy to compute the probabilities of observing the differences in the p ˆ ˆ prob(, ( ˆ α + α + ˆ rot p p prob( ˆ, ˆ α + α ˆ 1 prob( ˆ, ˆ α + α ˆ 3 rot rot 1 rot 6

7 Calculating the Posterior Given x, x, and u 7 1. Algorithm motion_model_odometry(x,x,u ( x' x + ( y' y atan( y' y, x' x θ rot θ ' θ ˆ ( x' x + ( y' y ˆ rot 1 atan( y' y, x' x θ ˆ ˆ rot θ ' θ rot 1 ˆ ˆ prob(, ˆ p1 α1 + α ˆ ˆ prob(, ( ˆ p α3 + α 4 + ˆ ˆ prob(, ˆ p α + α rot 3 rot rot 1 rot 11. return p 1 p p 3 odometry values (u values of interest (x,x ˆ

8 Recap 8 velocity motion model control variables were linear velocity, angular velocity about ICC, and final angular velocity about robot center c c y x θ y x x t 1 θ y x x t ω v γ

9 Recap odometric motion model control variables were derived from odometry initial rotation, lation, final rotation x t 1 x y θ rot x t x y θ 9

10 Recap for both models we assumed the control inputs u t were noisy the models were assumed to be zero-mean additive with a specified variance vˆ ˆ ω actual velocity v v + ω ω commanded velocity var( v var( ω α v 1 α v 3 + α ω + α ω 4 10

11 Recap for both models we assumed the control inputs u t were noisy the models were assumed to be zero-mean additive with a specified variance var( var( var( ˆ ˆ ˆ rot actual motion,, rot, rot commanded motion α α α ˆ ˆ ˆ + rot + α + α + α,, rot, 4 ˆ ( ˆ ˆ + ˆ rot 11

12 Recap for both models we studied how to derive given x t-1 u t x t current pose control input new pose p( xt ut, xt 1 find the probability density that the new pose is generated by the current pose and control input required inverting the motion model to compare the actual with the commanded control parameters 1

13 Recap for both models we studied how to sample from given x t-1 u t current pose control input p( xt ut, xt 1 generate a random new pose x t consistent with the motion model sampling from p( x is often easier than calculating t ut, xt 1 p( xt ut, xt 1 directly because only the forward kinematics are required 13

14 Recap see section 5.5 of the textbook for an extension of the motion models to include a map of the environment in particular notice the numerous assumptions and approximations that need to be made to make the computations tractable also, pay attention to the consequences of making such assumptions and approximations 14