L ECTURE 8: K INEMATICS E QUATIONS O DOMETRY , D EAD R ECKONING I - - PowerPoint PPT Presentation

16-311-Q INTRODUCTION TO ROBOTICS

LECTURE 8:

KINEMATICS EQUATIONS ODOMETRY, DEAD RECKONING

INSTRUCTOR: GIANNI A. DI CARO

2

F O R WA R D K I N E M AT I C S E Q U AT I O N S

To obtain future poses over time-extended intervals, it is necessary to provide initial conditions, specify geometry parameters, assign the linear and angular velocity profiles v(t) and ω(t), and integrate over time (which might not be obvious/easy) In the specific case of a two-wheeled differential robot, v(t) and ω(t) at the reference point P on the chassis are functions of the Left and Right speeds issued to the Left and Right wheel, respectively:

!P (t) = r ˙ 'R − r ˙ 'L 2` = vR(t) − vL(t) 2` vP (t) = r ˙ ϕR + r ˙ ϕL 2 = vR(t) + vL(t) 2

Function of the ICR Function of the issued velocities

W 2 6 6 6 4 x(t + δt) y(t + δt) θ(t + δt) 3 7 7 7 5 = 2 6 6 6 4 x(t) + R(t) ⇣ sin

• θ(t) + ωδt
• − sin(θ(t))

⌘ y(t) − R(t) ⇣ cos

• θ(t) + ωδt
• − cos(θ(t))

⌘ θ(t) + ωδt 3 7 7 7 5 = 2 6 6 6 4 x(t) + R(t) ⇣ sin

• θ(t) + ∆θ(t + δt)
• − sin(θ(t))

⌘ y(t) − R(t) ⇣ cos

• θ(t) + ∆θ(t + δt)
• − cos(θ(t))

⌘ θ(t) + ∆θ(t + δt) 3 7 7 7 5 = 2 6 6 6 6 6 6 6 4 x(t) + v(t) ω(t) ⇣ sin(θ(t) + ∆θ(t + δt)) − sin(θ(t)) ⌘ y(t) − v(t) ω(t) ⇣ cos(θ(t) + ∆θ(t + δt)) − cos(θ(t)) ⌘ θ(t) + ω(t)δt 3 7 7 7 7 7 7 7 5

3

F O R WA R D K I N E M AT I C S F O R D I F F E R E N T I A L D R I V E

VR(t) VL(t)

l

ICR(t)

ω(t)

V(t)

P

XR YR XW YW 𝜾

l

x y R

       ˙ x ˙ y ˙ θ       

R

=       

r ˙ 'R(t) + r ˙ 'L(t) 2 r ˙ 'R(t) – r ˙ 'L(t) 2`

       =       

vR(t) + vL(t) 2 vR(t) – vL(t) 2`

      

In the robot frame, given the spinning velocity controls for wheels

       ˙ x ˙ y ˙ θ       

R

=        v(t) ω(t)       

Two control inputs, linear and angular velocity In the world reference frame, given the local inputs v(t), ω(t)        ˙ x ˙ y ˙ θ       

W

=        cos(θ(t)) – sin(θ(t)) sin(θ(t)) cos(θ(t)) 1               v(t) ω(t)       

R

˙ x = v(t) cos(θ(t)) ˙ y = v(t) sin(θ(t)) ˙ θ = ω(t)

4

A G E N E R A L M O D E L : T H E U N I C Y C L E M O D E L

VR(t) VL(t)

l

ICR(t)

ω(t) V(t)

P XR YR XW YW

𝜾

l

x y

R

˙ x = v(t) cos(θ(t)) ˙ y = v(t) sin(θ(t)) ˙ θ = ω(t)

˙ x = vR(t) + vL(t) 2 cos(✓(t)) ˙ y = vR(t) + vL(t) 2 sin(✓(t)) ˙ ✓ = vR(t) – vL(t) 2` ˙ x = r ˙ 'R(t) + r ˙ 'L(t) 2 cos(✓(t)) ˙ y = r ˙ 'R(t) + r ˙ 'L(t) 2 sin(✓(t)) ˙ ✓ = r ˙ 'R(t) – r ˙ 'L(t) 2`

Abstract model!

X

V(t) 𝜾 ω(t) x y

Unicyle model (non holonomic)

5

F U L L F O R WA R D K I N E M AT I C S : T I M E T R A J E C T O RY

For a generic robot, given v(t), ω(t) as local inputs, the velocity of pose change in the world reference frame:

For a 2-wheeled differential robot

V(t) 𝜾 ω(t) x y

˙ x = v(t) cos(θ(t)) ˙ y = v(t) sin(θ(t)) ˙ θ = ω(t)

If the time-profiles of the velocities are known, the equations can be integrated over time to predict the time trajectory:

6

C O M P U TAT I O N O F R AT E O F C H A N G E I N T H E P O S E V E C T O R

A robot is positioned at an angle of 60 degrees with respect to the global reference frame and has wheels with a radius of 1 cm. The wheels are 2 cm from the center of the chassis. If the speeds of wheels 1 and 2, are 4 cm/s and 2 cm/s, respectively, what is the robot velocity with respect to the global reference frame?

7

F O R WA R D K I N E M AT I C S : E A S Y ( B U T U S E F U L ) C A S E S

Equal (constant) forward speed for both wheels The robot moves along a straight trajectory vL = vR = v x(t) = x0 + vt cos(θ) y(t) = y0 + vt sin(θ) θ(t) = θ0

Also for interval-wise changes in the common velocity

Equal but opposite wheel speeds vL = −vR x(t) = x0 y(t) = y0 ✓(t) = ✓0 + 2vt 2` The robot rotates in place Constant (different) speeds for both wheels

The robot moves along a circular trajectory

• f constant radius R

R = `vR + vL vR − vL vL(t) = vL, vR(t) = vR, vL 6= vR x(t) = x0 + ` 2 vR + vL vR vL sin t

`(vR vL)

• y(t) = y0 `

2 vR + vL vR vL cos t

`(vR vL)

• ✓(t) = ✓0 + t

`

• vR vL

8

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

R(t) = R1(t) = L tan(γ(t)), ω(t) = v(t) R(t)

The max value of 𝛅 limits maneuverability: parking problem, complex inverse kinematics

˙ x = v(t) cos(θ(t)) ˙ y = v(t) sin(θ(t)) ˙ θ = ω(t)

˙ x = v(t) cos(θ(t)) ˙ y = v(t) sin(θ(t)) ˙ θ = v(t) L tan(γ(t))

9

A C K E R M A N N S T E E R I N G

R2(t) > R1(t)

The front wheel must follow a longer path, and therefore must rotate faster than the rear

• wheel. With two front wheels a differential

gear is necessary to implement this difference

L R 𝓂 𝓂 R(t) − ` = L tan(↵l(t)) R(t) + ` = L tan(↵r(t)) Once set the steering for the left wheel, the right wheel is constrained by rolling motion to steer a specific angle which is coherent with the vehicle’s ICR

10

F O R WA R D V S . I N V E R S E K I N E M AT I C S

Posture prediction: Forward Kinematics

Pose

Robot Robot

Controls (v,𝞉)

?

Robot Posture regulation: Inverse Kinematics R

• b
• t

?

Controls

Goal pose Feasible? Path(s)

Path following (geometry) Robot Robot R

• b
• t

Robot Controls ?

Feasible? Path(s,t)

Robot Robot R

• b
• t

Robot Trajectory following (kinematics, time) Controls ?

Feasible?

Path Planning Trajectory Planning Control

11

I N V E R S E K I N E M AT I C S ( F O R D I F F E R E N T I A L R O B O T S )

vL(t) = vL, vR(t) = vR, vL 6= vR x(t) = x0 + ` 2 vR + vL vR vL sin t

`(vR vL)

• y(t) = y0 `

2 vR + vL vR vL cos t

`(vR vL)

• ✓(t) = ✓0 + t

`

• vR vL
• Given an initial and a goal pose, what are the velocity profiles to provide

to the wheels to achieve the desired pose transition?

{W} ? In the general case, a very hard problem, the presence of the non-holonomic sliding constraints makes computations difficult The same final pose(t) can be achieved in many/infinite ways Given a time t and a goal pose, the equations solve for vL and vR but do not provide an independent control for 𝜄 (𝜺m =2) Goal Different radii, and/or multiple iterations over the same circular path to meet time requirements Start

12

S I M P L I F I E D I N V E R S E K I N E M AT I C S A P P R O A C H

In general, a forward solution is already quite complex, a direct approach to inversion would be problematic for general relations between the two control velocities

Solve the problem by decomposing the trajectory in primitive motion segments (very easy in open space):

• Straight lines
• Segments of a circle or rotation in place

Easier but not easy: a lot of issues to guarantee smoothness (and other quality constraints) and to deal with robot and environments’ constraints Easy forward kinematics cases

Move straight Rotate in place

vL = −vR x(t) = x0 y(t) = y0 ✓(t) = ✓0 + 2vt 2` vL = vR = v x(t) = x0 + vt cos(θ) y(t) = y0 + vt sin(θ) θ(t) = θ0

Other (better) ways to do it, later on ….

13

C O M P U T I N G T H E S TAT E / P O S E

What is robot’s pose in {W} after moving at a velocity (v,𝞉), for 1 minute? {W} Target trajectory Actual trajectory

Δξ(t) ?

ξ(t) = [x(t) y(t) 𝜄(t)] ξd(t) = [xd(t) yd(t) 𝜄d(t)]

Where am I? / What is my pose? With respect to an initial reference point, a coordinate system, a map ….

wind

Δξ

14

W H AT I S T H E T Y P E O F A P O S E ?

Pose / Position should be thought in a quite general sense. The required form of a position depends on the type of the reference system that is of interest / available

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

• plogical map

Metric map Sensor map

15

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

16

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

17

I N T R I N S I C E R R O R S I N L O C A L I Z AT I O N

Small/Large discrepancies between issued commands and actual motion, due to friction, imprecision, approximations, … computations and real world intrinsically bring errors!

True Estimated by DR

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

Fix

• 1. Robot started in the initial configuration
• 2. Moved N steps in direction x, and then M steps in direction y …
• 3. In general, will the new position be determined with high accuracy?

18

E R R O R P R O PA G AT I O N E F F E C T S

• Let’s assume (as it is commonly done) that the errors are independent, with

zero mean, and stationary variance: ∀iE[✏x

i ] = E[✏y i ] = 0

∀iE[(✏x

i − E[✏x i ])(✏x i − E[✏x i ])] = 2

∀iE[(✏y

i − E[✏y i ])(✏y i − E[✏y i ])] = 2

∀i6=jE[(✏x

i − E[✏x i ])(✏y i − E[✏y i ])] = 2

• At each time step i the controller tells the robot to move by some amount

(∆xi, ∆yi), but the robot actually moves by (∆xi + ✏x

i , ∆yi + ✏y i ), where ✏x i

and ✏y

i are small random errors.

• After N steps, starting from (x0, y0), the robot is expected to be in

(xN, yN) = (x0, y0) + X (∆xi, ∆yi)

19

E R R O R P R O PA G AT I O N E F F E C T S

The expected position is at the commanded location (no error bias)

E[(xN, yN)] = E[(x0, y0) + P(∆xi + ✏x

i , ∆yi + ✏y i )]

= (x0, y0) + (E[P(∆xi + ✏x

i )], E[P(∆yi + ✏y i )])

= (x0, y0) + (P ∆xi + P E[✏x

i ], P ∆yi + P E[✏y i ])

= (x0, y0) + P(∆xi, ∆yi).

20

E R R O R P R O PA G AT I O N E F F E C T S

What is the uncertainty on the final position?

Σ = σxx σxy σyx σyy

• σxx = E[(x − E[x])2] . . . = Nσ2

σyy = E[(y − E[y])2] . . . = Nσ2

Σ = N " σ2 σ2 #

Errors in x and y grow unbounded for N → ∞

σxy = σyx = 0

21

E R R O R S O U R C E S I N O D O M E T RY / D E A D R E C K O N I N G

Odometry drift (error growth) in pose estimate is due to:

• Numerical integration errors (approximations, floating point rounding,….)
• Error readings from the wheel encoders
• Wheel slippage (friction issues, uneven ground, …)
• Inaccurate measurement or calibration of wheel and chassis parameters, that

are reflected in inaccuracies in the results from the kinematic model

• Rotations usually determine more errors than translations (more slippage)
• Systematic / Deterministic errors can be corrected by a calibration process
• Random / Environment-related errors have to be explicitly modeled, but will

inevitably determine uncertain pose estimates

• The additional use of inertial measures (heading, acceleration) can greatly improve

accuracy of the whole dead reckoning process

22

T Y P I C A L I S S U E S R E L AT E D T O W H E E L S

Using wheels’ measures to compute odometry is not perfect at all, it comes with a number of uncertainties!

23

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

24

I N T E R E S T I N G C A S E S F O R O D O M E T RY E R R O R S

Cumulative error

• ver a squared path

Different errors summing up Different errors canceling with each other

25

F R O M C O N T I N U O U S T I M E T O D I S C R E T E T I M E

˙ x = v(t) cos(θ(t)) ˙ y = v(t) sin(θ(t)) ˙ θ = ω(t)

Dynamics of the robot system

6 4 7 5 6 4 = 2 6 6 6 6 6 6 6 4 x(t) + v(t) ω(t) ⇣ sin(θ(t) + ∆θ(t + δt)) − sin(θ(t)) ⌘ y(t) − v(t) ω(t) ⇣ cos(θ(t) + ∆θ(t + δt)) − cos(θ(t)) ⌘ θ(t) + ω(t)δt 3 7 7 7 7 7 7 7 5

W    x(t + δt) y(t + δt) θ(t + δt)   

Motion from geometric considerations

• Transform the continuous kinematic equations into a

discrete-time system

• Numeric integration is therefore a viable option can be also done online
• Useful to numerically predict future poses
• Useful to keep updating current pose / state: Where am I? (In the

environment, on the trajectory …)

26

A S S U M P T I O N S F O R T H E N U M E R I C A L I N T E G R AT I O N

• Time is discretized in short intervals of length 𝛦t
• When used online, a numeric integration is performed at each time step
• During a time interval [tk,tk+1], the velocity inputs vk and ωk are

assumed to be constant and the robot moves along a circle of radius vk / ωk centered at the ICR(t), or moves along a segment if ωk = 0

• At step k, robot's pose ξk and its velocities are assumed to be known and

are used to compute ξk+1 by integration of the kinematic model over 𝛦tk = [tk,tk+1]

Three main approaches

Euler Runge-Kutta Exact

27

E U L E R A P P R O A C H : I N I T I A L O R I E N TAT I O N

Problem: During the interval, the orientation changes because of the issued velocity commands, but this is not taken into account in the calculation of sin and cos, since 𝜾k is kept as at the beginning of the interval

˙ x = v(t) cos(θ(t)) ˙ y = v(t) sin(θ(t)) ˙ θ = ω(t)

xk+1 and yk+1 are approximate, θk+1 is exact

˙ x = v(t) cos(θ(t)) ˙ x ≈ x(t + ∆t) – x(t) ∆t x(t + ∆t) ≈ x(t) + ∆t v(t) cos(θ(t)) = x(t) + ∆t ˙ x

         xk+1 = xk + ∆tkvk cos(θk) yk+1 = yk + ∆tkvk sin(θk) θk+1 = θk + ∆tkωk

E.g., for x component

28

E U L E R A P P R O A C H : I N I T I A L O R I E N TAT I O N

˙ x = v(t) cos(θ(t)) ˙ y = v(t) sin(θ(t)) ˙ θ = ω(t)

Euler method: first term of Taylor series, 1st order approximation

x(t0 + ∆t) = x(t0) + ∆t ˙ x(t0) + (∆t)2 2! ¨ x(t0) + (∆t)3 3! ... x (t0) + . . . ˜ x(t0 + ∆t) = x(t0) + ∆t ˙ x(t0)

t0 + ∆t t0 x(t0) x(t0 + ∆t) ˜ x(t0 + ∆t) ∆t x

˙ x = v(t) cos(θ(t)) ˙ x ≈ x(t + ∆t) – x(t) ∆t x(t + ∆t) ≈ x(t) + ∆t v(t) cos(θ(t)) = x(t) + ∆t ˙ x

29

R U N G E - K U T TA A P P R O A C H : AV E R A G E O R I E N TAT I O N

t0 + ∆t t0 x(t0) x(t0 + ∆t) ∆t x

EM, slope at t0 EM slope at

t0 + ∆t

Slope at t0 + ∆t 2 t0 + ∆t 2 ˜ x(t0 + ∆t 2 )

• Find slope s1 (1st order approx) in t0
• Find slope s2 (1st order approx) in t0 + 𝛦t
• Approximate the value in t0 + 𝛦t with the

average slope between t0 and t0 + 𝛦t → 2nd order approx, Taylor in t0 + 𝛦t/2

˜ x(t0 + ∆t) = x(t0) + ∆ts1 + s2 2 = x(t0) + ∆t ˙ x(t0) + ˙ x(t0 + ∆t) 2 !

30

R U N G E - K U T TA A P P R O A C H : AV E R A G E O R I E N TAT I O N

• The average orientation over the integration interval is considered, therefore

the xk+1 and yk+1 values are better than in the Euler case, when only the initial

• rientation is taken, but still they are approximations, while 𝜄k+1 is exact
• The value of 𝛦𝜄k can be computed directly from the issued velocity commands

(v, ω), or can be estimated from measures (from wheels’ encoders)

• In the right set of equations, 𝛦Sk is used instead of vk𝛦tk, which are equivalent,

but 𝛦Sk can be more direct to compute with wheels

         xk+1 = xk + vk∆tk cos(θk + ∆θk

2 )

yk+1 = yk + vk∆tk sin(θk + ∆θk

2 )

θk+1 = θk + ωk∆tk

         xk+1 = xk + ∆Sk cos(θk + ∆θk

2 )

yk+1 = yk + ∆Sk sin(θk + ∆θk

2 )

θk+1 = θk + ∆θk

Geometric derivation of the Runge-Kutta equations → …

31

R U N G E - K U T TA : G E O M E T R I C D E R I VAT I O N

• Travel length: At time-step k, if ∆tk sufficiently small,

the robot can be seen as traveling on a circular arc, cen- tered at the ICR, for a distance ∆Sk that can be approxi- mated with the length of the corresponding cord: ∆Sk ≈ ∆dk. Since ∆Sk represents the distance traveled in ∆tk moving at speed vk: vk∆tk = ∆Sk ≈ ∆dk

32

R U N G E - K U T TA : G E O M E T R I C D E R I VAT I O N

⇣ ⌘ The relations for ∆xk and ∆yk result from trigonometry,

• bserving that:

\ P – ICR – P0 = ∆θ ⇒ \ ICR – P – P0 = 90 – ∆θ/2 ⇒ \ P0 – P – O0 = 90 – (90 – ∆θ/2) = ∆θ/2

• Orientation change: With the cord approximation

(i.e, the robot is moving along the cord), the change in the orientation angle becomes equal to ∆θk 2 , and the change in x and y: ∆xk = ∆dk cos ⇣ θk + ∆θk 2 ⌘ ∆yk = ∆dk sin ⇣ θk + ∆θk 2 ⌘

33

E X A C T A P P R O A C H : VA R I AT I O N O F T H E O R I E N TAT I O N

ω = 0 is a condition that must be monitored, also when ω is close to 0 some practical precaution is necessary in the code

Discretization of the kinematic equations previously found using the ICR:

xk+1 = xk + vk ωk (sin θk+1 − sin θk) yk+1 = yk − vk ωk (cos θk+1 − cos θk) θk+1 = θk + ωk∆tk

6 4 7 5 6 4 = 2 6 6 6 6 6 6 6 4 x(t) + v(t) ω(t) ⇣ sin(θ(t) + ∆θ(t + δt)) − sin(θ(t)) ⌘ y(t) − v(t) ω(t) ⇣ cos(θ(t) + ∆θ(t + δt)) − cos(θ(t)) ⌘ θ(t) + ω(t)δt 3 7 7 7 7 7 7 7 5

W    x(t + δt) y(t + δt) θ(t + δt)   

34

C O M PA R I S O N A M O N G T H E T H R E E M E T H O D S