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

16-311-Q I NTRODUCTION TO R OBOTICS L AB N OTES : O DOMETRY , ROS R EFERENCE F RAMES I NSTRUCTOR : G IANNI A. D I C ARO

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 odometry measures 2

D E A D ( D E D U C E D ) R E C K O N I N G 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 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 over time. Data related to exogenous and endogenous disturbances can be included. Time-integration of velocity vectors estimated through on-board data • Odometry is “basically” another way to say the same thing, that has di ff erent roots and etymology … • In the animal world, this is called path integration 3

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 over 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 Δξ 2 Δξ 0 = 0 Δξ 1 True Δξ 3 Fix Estimated by DR 4

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   x k + v k (sin θ k +1 − sin θ k )   ω k   x k +1 Exact       y k − v k ξ k +1 =  = y k +1   numeric (cos θ k +1 − cos θ k )      ω k   θ k +1 integration     θ k + ω k ∆ t k 2 3     θ k + ∆ θ k � � Runge- x k + v k ∆ t k cos θ k x k + ∆ S k cos x k +1 2 3 Euler x k +1 2 6 7 Kutta     y k + v k ∆ t k sin θ k numeric 6 7 6 7 =  = y k +1 y k +1 θ k + ∆ θ k 5 = � � y k + ∆ S k sin     6 7 6 7 numeric 2    6 7 4 integration θ k +1 θ k + ω k ∆ t k θ k +1 4 5 integration θ k + ∆ θ k 5

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   x k + v k (sin θ k +1 − sin θ k )   ω k   x k +1 Exact       y k − v k ξ k +1 =  = y k +1   numeric (cos θ k +1 − cos θ k )      ω k   θ k +1 integration     θ k + ω k ∆ t k 2 3     θ k + ∆ θ k � � Runge- x k + v k ∆ t k cos θ k x k + ∆ S k cos x k +1 2 3 Euler x k +1 2 6 7 Kutta     y k + v k ∆ t k sin θ k numeric 6 7 6 7 =  = y k +1 y k +1 θ k + ∆ θ k 5 = � � y k + ∆ S k sin     6 7 6 7 numeric 2    6 7 4 integration θ k +1 θ k + ω k ∆ t k θ k +1 4 5 integration θ k + ∆ θ k • In practice, due to the non ideality of any actuations system, the command inputs v k and ω k and not used in the numerical integration equations • Instead, it is common practice to measure the e ff ect of the actual inputs in terms of traveled distance Δ S k = v k Δ t k and heading change Δ 𝜾 k = ω k Δ t k during the integration interval Δ t k , with v k / ω k = Δ S k / Δ 𝜾 k • Δ S k and Δ 𝜾 k can be reconstructed from proprioceptive sensors • In the case of a di ff erential robot: ∆ S k = r ∆ ✓ k = r � � � � ∆ � R + ∆ � L ∆ � R − ∆ � L 2 2 ` ΔΦ R and ΔΦ L are the rotations of the right/left wheels, e.g. measured by wheel encoders 6

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

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

W H E E L E N C O D E R S Typical resolutions: 64 – 2048 increments per revolution; for higher resolution: interpolation 9

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 10

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 11

M A P S … . Geographical map, landmarks Cartesian, 2D 3D earth coordinates T oplogical map Metric map Sensor map 12

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 odom 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 di ff erent map frames. If the application only needs one map the earth coordinate frame is not expected to be present. 13

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 TF ROS package map ekf_localization_node odom pose data ekf_localization_node base_link twist data* * and IMU data 14

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 2 3 θ k + ∆ θ k � � x k + ∆ S k cos Assume that the odometry model is 2 3 x k +1 2 6 7 perfect, based on measured 6 7 6 7 ξ k +1 = y k +1 θ k + ∆ θ k 5 = � � y k + ∆ S k sin 6 7 6 7 2 6 7 4 distance 𝛦 S, and heading 𝛦𝜾 4 5 θ k +1 θ k + ∆ θ k Odometry measurements are noisy! ➔ Random noise is added to 𝛦 S and 𝛦𝜾     k ) cos( θ k + ∆ θ k ( ∆ S k + ν s 2 + ν θ x k k )     k ) sin( θ k + ∆ θ k ξ k +1 =  + ( ∆ S k + ν s 2 + ν θ y k   k )        ∆ θ k + ν θ θ k k In absence of specific information, the odometry noise can be modeled as Gaussian white noise and the two Or, more general, non-parametric noise components are assumed to be uncorrelated : models need to be adopted … " # σ 2 0 ⇤ T ∼ N (0 , V k ) , ks ⇥ ν s ν θ ν k = V k = k k σ 2 0 k θ 15