L AB N OTES : O DOMETRY , ROS R EFERENCE F RAMES I NSTRUCTOR : G - - PowerPoint PPT Presentation

16-311-Q INTRODUCTION TO ROBOTICS

LAB NOTES:

ODOMETRY, ROS REFERENCE FRAMES

INSTRUCTOR: GIANNI A. DI CARO

2

A T Y P I C A L R E S U LT F R O M O D O M E T RY

This the path the robot has estimated using

• dometry measures

3

D E A D ( D E D U C E D ) R E C K O N I N G

Deduced reckoning: The process of (incrementally, at discrete time steps) determining its own position based on the knowledge of some reference point (a fix) and the knowledge or the estimate of the velocities (speeds and headings) actuated

• ver time. Data related to exogenous and endogenous disturbances can be included.

Time-integration of velocity vectors estimated through on-board data

In absence of an external infrastructure (e.g., GPS + Filters + Cameras) able to track the pose of the robot, numerical integration can be used, based on the kinematic model of the robot and on the knowledge of the issued velocity commands, [v(t) ω(t)]T ➔ Incrementally build the state using on-board information

• Odometry is “basically” another way to say the same

thing, that has different roots and etymology …

• In the animal world, this is called path integration

4

E R R O R A C C U M U L AT I O N I N D E A D R E C K O N I N G

• The uncertainty of dead reckoning / odometry increases over time and maybe
• ver distance ➔ A new fix is intermittently needed to determine a more

reliable position from which a new dead reckoning process (i.e., integration) can be restarted

• In navigation, from where the

terms comes from, lighthouses and/or celestial observations were used to get a new fix

True Estimated by DR

Δξ0 = 0 Δξ1 Δξ2 Δξ3

Fix

5

D I S C R E T E - T I M E F O R M U L AT I O N O F K I N E M AT I C E Q U AT I O N S

=     xk+1 yk+1 θk+1     =     xk + vk∆tk cos θk yk + vk∆tk sin θk θk + ωk∆tk    

Exact numeric integration

Euler numeric integration

2 6 6 4 xk+1 yk+1 θk+1 3 7 7 5 = 2 6 6 6 6 4 xk + ∆Sk cos

• θk + ∆θk

2

• yk + ∆Sk sin
• θk + ∆θk

2

• θk + ∆θk

3 7 7 7 7 5

Runge- Kutta numeric integration

ξk+1 =     xk+1 yk+1 θk+1     =          xk + vk ωk (sin θk+1 − sin θk) yk − vk ωk (cos θk+1 − cos θk) θk + ωk∆tk         

6

K I N E M AT I C E Q U AT I O N S I N D I S C R E T E - T I M E F O R M U L AT I O N

=     xk+1 yk+1 θk+1     =     xk + vk∆tk cos θk yk + vk∆tk sin θk θk + ωk∆tk    

Exact numeric integration

Euler numeric integration

2 6 6 4 xk+1 yk+1 θk+1 3 7 7 5 = 2 6 6 6 6 4 xk + ∆Sk cos

• θk + ∆θk

2

• yk + ∆Sk sin
• θk + ∆θk

2

• θk + ∆θk

3 7 7 7 7 5

Runge- Kutta numeric integration

ξk+1 =     xk+1 yk+1 θk+1     =          xk + vk ωk (sin θk+1 − sin θk) yk − vk ωk (cos θk+1 − cos θk) θk + ωk∆tk         

• Instead, it is common practice to measure the effect of the actual inputs in terms of

traveled distance ΔSk = vk Δtk and heading change Δ𝜾k = ω kΔtk during the integration interval Δtk, with vk / ω k = ΔSk / Δ𝜾k

• ΔSk and Δ𝜾k can be reconstructed from proprioceptive sensors
• In the case of a differential robot: ∆Sk = r

2

• ∆R + ∆L
• ∆✓k = r

2`

• ∆R − ∆L
• ΔΦR and ΔΦL are the rotations of the right/left wheels, e.g. measured by wheel encoders
• In practice, due to the non ideality of any actuations system, the command

inputs vk and ωk and not used in the numerical integration equations

7

M O T O R E N C O D E R S

8

W H E E L E N C O D E R S

9

W H E E L E N C O D E R S

Typical resolutions: 64 – 2048 increments per revolution; for higher resolution: interpolation

10

M E A S U R E S F R O M W H E E L E N C O D E R S

11

I N W H I C H F R A M E P O S E S / O D O M E T RY M E A S U R E S A R E R E P R E S E N T E D I N R O S ?

earth

12

M A P S … .

Cartesian, 2D 3D earth coordinates Geographical map, landmarks T

• plogical map

Metric map Sensor map

13

B A S I C R O S F R A M E S ( H T T P : / / W W W. R O S . O R G / R E P S / R E P - 0 1 0 5 . H T M L )

• base_link is rigidly attached to the mobile robot base. It can be

attached to the base in any arbitrary position or orientation

• odom is a world-fixed frame. The pose of a mobile platform in the
• dom frame can drift over time, without any bounds. This drift makes

the odom frame useless as a long-term global reference.

• The pose of a robot in the odom frame is guaranteed to be continuous,

meaning that the pose of a mobile platform in the odom frame always evolves in a smooth way, without discrete jumps.

• In a typical setup the odom frame is computed based on an odometry

source, such as wheel odometry, visual odometry or an IMU

• map is a world fixed frame, with its Z-axis pointing upwards. The pose of a mobile platform, relative

to the map frame, should not significantly drift over time. The map frame is not continuous, meaning the pose of a mobile platform in the map frame can change in discrete jumps at any time.

• In a typical setup, a localization component constantly re-computes the robot pose in the map

frame based on sensor observations, therefore eliminating drift, but causing discrete jumps when new sensor information arrives. The map frame is useful as a long-term global reference, but discrete jumps in position estimators make it a poor reference frame for local sensing and acting.

• earth is the origin of ECEF. This frame is designed to allow the interaction of multiple robots in different

map frames. If the application only needs one map the earth coordinate frame is not expected to be present.

14

T R A N S F O R M AT I O N B E T W E E N F R A M E S

map

• dom

base_link

pose data twist data*

ekf_localization_node

ekf_localization_node

* and IMU data

TF ROS package

15

W E W I L L N E E D S E N S O R F U S I O N + E R R O R M O D E L

ξk+1 = 2 6 6 4 xk+1 yk+1 θk+1 3 7 7 5 = 2 6 6 6 6 4 xk + ∆Sk cos

• θk + ∆θk

2

• yk + ∆Sk sin
• θk + ∆θk

2

• θk + ∆θk

3 7 7 7 7 5

Assume that the odometry model is perfect, based on measured distance 𝛦S, and heading 𝛦𝜾 Odometry measurements are noisy! ➔ Random noise is added to 𝛦S and 𝛦𝜾 ξk+1 =     xk yk θk     +      (∆Sk + νs

k) cos(θk + ∆θk 2 + νθ k)

(∆Sk + νs

k) sin(θk + ∆θk 2 + νθ k)

∆θk + νθ

k

     In absence of specific information, the odometry noise can be modeled as Gaussian white noise and the two noise components are assumed to be uncorrelated: νk = ⇥ νs

k

νθ

k

⇤T ∼ N(0, Vk), Vk = " σ2

ks

σ2

kθ

# Or, more general, non-parametric models need to be adopted …