I ntroduction to Mobile Robotics Probabilistic Motion Models - PowerPoint PPT Presentation

I ntroduction to Mobile Robotics Probabilistic Motion Models Wolfram Burgard 1

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

Dynam ic Bayesian Netw ork for Controls, States, and Sensations 3

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

Coordinate System s  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 three- dimensional (x,y, θ ). 5

Typical Motion Models  In practice, one often finds two types of motion models:  Odom etry-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

Exam ple W heel 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: http: / / www.active-robots.com/ 7

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

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

Odom etry Model θ θ • Robot moves from to . x , y , x ' , y ' , ' = δ δ δ • Odometry information . u , , rot 1 rot 2 trans δ = − + − 2 2 ( x ' x ) ( y ' y ) trans δ = − − − θ atan2 ( y ' y , x ' x ) rot 1 δ = θ − θ − δ δ ' rot 2 rot 1 rot 2 θ x ' , y ' , ' δ θ x , y , δ trans rot 1

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

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

Typical Distributions for Probabilistic Motion Models Normal distribution Triangular distribution  > σ 2 0 if | x | 6 2  1 x 1 − ε σ = ε =  σ 2 ( x ) 2 σ − ( x ) e 2 6 | x | 2 σ 2 πσ  2 2  σ 2 6

Calculating the Probability Density ( zero-centered)  For a normal distribution query point 1. Algorithm prob_ norm al_ distribution ( a,b ): std. deviation 2. return  For a triangular distribution 1. Algorithm prob_ triangular_ distribution ( a,b ): 2. return 14

Calculating the Posterior Given x, x ’ , and Odom etry odometry hypotheses Algorithm m otion_ m odel_ odom etry( x, x ’ ,u) 1. δ = − + − 2 2 2. ( x ' x ) ( y ' y ) trans δ = − − − θ odometry params ( u ) atan2 ( y ' y , x ' x ) 3. rot 1 δ = θ − θ − δ ' 4. rot 2 rot 1 δ ˆ = − + − 2 2 ( x ' x ) ( y ' y ) 5. trans δ ˆ = − − − θ values of interest ( x , x ’ ) atan2 ( y ' y , x ' x ) 6. rot 1 δ ˆ = θ − θ − δ ˆ ' 7. rot 2 rot 1 = δ − δ ˆ α δ + α δ p prob ( , | | ) 8. 1 rot 1 rot 1 1 rot 1 2 trans ˆ = δ − δ α δ + α δ + δ p prob ( , (| | | |)) 9. 2 trans trans 3 trans 4 rot1 rot2 = δ − δ ˆ α δ + α δ p prob ( , | | ) 10. 3 rot 2 rot 2 1 rot 2 2 trans return p 1 · p 2 · p 3 11. 15

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

Sam ple-Based Density Representation

Sam ple-Based Density Representation 18

How to Sam ple from a Norm al Distribution?  Sampling from a normal distribution 1. Algorithm sam ple_ norm al_ distribution ( b ): 2. return 19

Norm ally Distributed Sam ples 10 6 samples 20

How to Sam ple from Norm al or Triangular Distributions?  Sampling from a normal distribution 1. Algorithm sam ple_ norm al_ distribution ( b ): 2. return  Sampling from a triangular distribution 1. Algorithm sam ple_ triangular_ distribution ( b ): 2. return 21

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

How to Obtain Sam ples from Arbitrary Functions? 23

Rejection Sam pling  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 f(x ’ ) c c ’ OK f(x) x ’ x 24

Rejection Sam pling  Sampling from arbitrary distributions 1. Algorithm sam ple_ distribution ( f,b ): 2. repeat 3. 4. 5. until ( ) 6. return 25

Exam ple  Sampling from 26

Sam ple Odom etry Motion Model 1. Algorithm sam ple_ m otion_ m odel (u, x): = δ δ δ = θ u , , , x x , y , rot 1 rot 2 trans δ ˆ = δ + α δ + α δ 1. sample( | | ) rot 1 rot 1 1 rot 1 2 trans δ ˆ = δ + α δ + α δ + δ sample( (| | | |)) 2. trans trans 3 trans 4 rot 1 rot 2 δ ˆ = δ + α δ + α δ 3. sample( | | ) rot 2 rot 2 1 rot 2 2 trans = + δ ˆ θ + δ ˆ x ' x cos( ) 4. trans rot 1 = + δ ˆ θ + δ ˆ y ' y sin( ) 5. sam ple_ norm al_ distribution trans rot 1 θ = θ + δ ˆ + δ ˆ 6. ' rot 1 rot 2 θ x ' , y ' , ' 7. Return

Exam ples ( Odom etry-Based)

Sam pling from Our Motion Model Start

Velocity-Based Model θ -90 30

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

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

Motion I ncluding 3 rd Param eter Term to account for the final rotation 33

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

Equation for the Velocity Model some constant Center of circle: 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

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

Equation for the Velocity Model and 37

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

Posterior Probability for Velocity Model 39

Sam pling from Velocity Model 40

Exam ples ( Velocity-Based)

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

Sum m ary  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