Introduction to Mobile Robotics Probabilistic Motion Models

Transcription

1 Introduction to Mobile Robotics Probabilistic Motion Models Wolfram Burgard 1

2 Robot Motion Robot motion is inherently uncertain. How can we model this uncertainty? 2

3 Dynamic Bayesian Network for Controls, States, and Sensations 3

4 Probabilistic Motion Models To implement the Bayes Filter, we need the transition model. The term specifies a posterior probability, that action u t carries the robot from x t-1 to x t. In this section we will discuss, how can be modeled based on the motion equations and the uncertain outcome of the movements. 4

5 Coordinate Systems The configuration of a typical wheeled robot in 3D can be described by six parameters. This are the three-dimensional Cartesian coordinates plus the three Euler angles for roll, pitch, and yaw. For simplicity, throughout this section we consider robots operating on a planar surface. The state space of such systems is threedimensional (x,y,θ). 5

6 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. 6

7 Example Wheel Encoders These modules provide +5V output when they "see" white, and a 0V output when they "see" black. These disks are manufactured out of high quality laminated color plastic to offer a very crisp black to white transition. This enables a wheel encoder sensor to easily see the transitions. Source: 7

8 Dead Reckoning Derived from deduced reckoning. Mathematical procedure for determining the present location of a vehicle. Achieved by calculating the current pose of the vehicle based on its velocities and the time elapsed. Historically used to log the position of ships. [Image source: Wikipedia, LoKiLeCh] 8

9 Reasons for Motion Errors of Wheeled Robots ideal case different wheel diameters bump and many more carpet 9

10 Odometry Model Robot moves from x, y,θ to x', y', θ '. Odometry information u = δ δ, δ., rot1 rot 2 trans δ trans = 2 ( x' x) + ( y' y δ = atan2( y' y, x' ) θ δ rot1 x = θ θ δ rot 2 ' rot1 ) 2 δ rot 2 x', y', θ ' x, y,θ δ rot1 δ trans

11 The atan2 Function Extends the inverse tangent and correctly copes with the signs of x and y. 11

12 Noise Model for Odometry The measured motion is given by the true motion corrupted with noise. ˆ δ rot1 = δ rot1 + ε α 1 δ rot1 + α 2 ˆ ˆ = + δ rot 2 = δ rot 2 + ε α 1 δ rot 2 + α 2 δ trans δ trans δtrans ε α 3 δtrans + α 4 ( δ rot1 + δ rot 2 ) δ trans

13 Typical Distributions for Probabilistic Motion Models ) ( σ σ πσ ε x e x = > = x 0 if ) ( 2 σ σ σ ε σ x x Normal distribution Triangular distribution

14 Calculating the Probability Density (zero-centered) For a normal distribution query point 1. Algorithm prob_normal_distribution(a,b): 2. return std. deviation For a triangular distribution 1. Algorithm prob_triangular_distribution(a,b): 2. return 14

15 Calculating the Posterior Given x, x, and Odometry hypotheses odometry 1. Algorithm motion_model_odometry(x, x,u) δtrans = ( x' x) + ( y' y) δ rot1 = atan2( y' y, x' x) θ δ rot 2 = θ ' θ δ rot1 ˆ 2 2 δtrans = ( x' x) + ( y' y) ˆ δ rot 1 = atan2( y' y, x' x) θ ˆ δ ˆ rot 2 = θ ' θ δ rot 1 p ˆ 1 = prob( δ rot1 δ rot1, α1 δ rot1 + α 2δ trans) p = prob( δ ˆ trans δ trans, α3δ trans + α 4( δ rot1 + p = prob( δ ˆ δ, α δ + α ) 2 δ rot2 3 rot2 rot2 1 rot2 2δ trans 11. return p 1 p 2 p 3 odometry params (u) values of interest (x,x ) )) 15

16 Application Repeated application of the motion model for short movements. Typical banana-shaped distributions obtained for the 2d-projection of the 3d posterior. x u x u

17 Sample-Based Density Representation

18 Sample-Based Density Representation 18

19 How to Sample from a Normal Distribution? Sampling from a normal distribution 1. Algorithm sample_normal_distribution(b): 2. return 19

20 Normally Distributed Samples 10 6 samples 20

21 How to Sample from Normal or Triangular Distributions? Sampling from a normal distribution 1. Algorithm sample_normal_distribution(b): 2. return Sampling from a triangular distribution 1. Algorithm sample_triangular_distribution(b): 2. return 21

22 For Triangular Distribution 10 3 samples 10 4 samples 10 5 samples 10 6 samples 22

23 How to Obtain Samples from Arbitrary Functions? 23

24 Rejection Sampling Sampling from arbitrary distributions Sample x from a uniform distribution from [-b,b] Sample c from [0, max f] if f(x) > c keep the sample otherwise reject the sample c x f(x) x f(x ) c OK 24

25 Rejection Sampling Sampling from arbitrary distributions 1. Algorithm sample_distribution(f,b): 2. repeat until ( ) 6. return 25

26 Example Sampling from 26

27 Sample Odometry Motion Model 1. Algorithm sample_motion_model(u, x): u = δ, δ, rot 1 rot 2 δtrans, x = x, y, θ ˆ δ rot1 = δ rot1 + sample( α1 δ rot1 + α 2 δtrans ) ˆ δ trans = δtrans + sample( α δtrans + α 4 ( δ rot1 + δ ˆ δ = δ + sample( α δ + α δ ) 3 rot 2 rot 2 rot 2 1 rot 2 2 trans )) x y = x +δ ˆ cos( ˆ trans θ + δ y +δ ˆ sin( θ + ˆ δ ' rot1 ' = trans rot1 θ ' = θ + ˆ δ ˆ rot + δ 1 rot 2 ) ) sample_normal_distribution 7. Return x', y', θ '

28 Examples (Odometry-Based)

29 Sampling from Our Motion Model Start

30 Velocity-Based Model θ-90 30

31 Noise Model for the Velocity- Based Model The measured motion is given by the true motion corrupted with noise. vˆ ˆ = v + ε α v + α ω 1 2 ω = ω + ε α v + α ω 3 4 Discussion: What is the disadvantage of this noise model? 31

32 Noise Model for the Velocity- Based Model The ( vˆ, ˆ) ω -circle constrains the final orientation (2D manifold in a 3D space) Better approach: vˆ ˆ ˆ = v + ε α v + α ω 1 2 ω = ω + ε α v + α ω γ = ε α 5 v + α6 ω 3 4 Term to account for the final rotation 32

33 Motion Including 3 rd Parameter Term to account for the final rotation 33

34 Equation for the Velocity Model Center of circle: some constant (distance to ICC) (center of circle is orthogonal to the initial heading) 34

35 Equation for the Velocity Model Center of circle: some constant some constant (the center of the circle lies on a ray half way between x and x and is orthogonal to the line between x and x ) 35

36 Equation for the Velocity Model Center of circle: some constant Allows us to solve the equations to: 36

37 Equation for the Velocity Model and 37

38 Equation for the Velocity Model The parameters of the circle: allow for computing the velocities as 38

39 Posterior Probability for Velocity Model 39

40 Sampling from Velocity Model 40

41 Examples (Velocity-Based)

42 Map-Consistent Motion Model p( x' u, x) p( x' u, x, m) Approximation: p( x' u, x, m) = η p( x' m) p( x' u, x)

43 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 how to improve this motion model to take the map into account. 43