Mobile & service robotics 02 - PoliTO

Mobile & Service RoboticsMobile & Service Robotics

KinematicsKinematicsKinematicsKinematicsBasilio Bona ROBOTICA 03CFIOR 1

Kinematics

Basilio Bona 2ROBOTICA 03CFIOR

Kinematics

the steering angle         is the angle between the velocity vector and the steered wheel direction 

( )tβ

is equivalent to the angle between the normal to the velocity vector and the steered wheel rotation axis we indicate with robot pose the position and the orientation (with respect to some give inertial axis)

( )( )t x y θ=pT

( )

0

( )

0

0

c sθ θ⎛ ⎞− ⎟⎜ ⎟⎜ ⎟⎜ ⎟R0 0

0 0 1m

s cθ θ⎜ ⎟= ⎜ ⎟⎜ ⎟⎜ ⎟⎜ ⎟⎟⎜⎝ ⎠

R

⎝ ⎠

Basilio Bona ROBOTICA 03CFIOR 3

Instantaneous Curvature Centre (ICC)

If an ICC exists, the wheel motion occurs without slippage

ICCICC

Basilio Bona 4ROBOTICA 03CFIOR

ICC

If an ICC does not exist, the wheel motion occurs with slippage

?

wheels slip

Basilio Bona 5ROBOTICA 03CFIOR

wheels slip

Kinematics

δWj

WR

αδ

Wω

drj W

i

αrϕ=v

rR

yr

ωri

y

0R

Basilio Bona 6ROBOTICA 03CFIOR

x

Kinematics – Fixed wheel

Th ki ti d lThe kinematic model represents a rover frame with a generic 

,α δ

( )i 0d ddTg

active non‐steering wheel, located at a given position with

( )cos sin 0d dα α=d

given constantsgiven position, with a local orientation

given constants

The kinematic equations describe the relations and

( )0x yv v=v

TLinear velocity of the wheel at the contact point with the planethe relations and 

constraints between the wheel angular l it d th

( )0W Wx Wyω ω=ω

T

p

Angular velocity of the wheel

velocity and the angular velocity of the frame

( )

( )0 0 ω=ωT

Angular velocity of the robot ( )r rzg y

Basilio Bona 7ROBOTICA 03CFIOR

Kinematic constraints

Constraints:Hypothesis

θ=ω

Constraints:To avoid slippage the tangential velocity at the wheel contact point, due to the rover rotation around its

b l h d l f hr

W

θ

ϕ=

ω

ωcenter, must be equal to the advance velocity of the wheel

rϕ=v

In the wheel reference frame we have

( )T

T( )( )

T

T

0 0 1

0 1 0

rθ

ϕ

=

=

ω

ω ( )( )

T

0 1 0

1 0 0

W

r

ϕ

ϕ

=

=v

ω

Basilio Bona 8ROBOTICA 03CFIOR

( )

Differential Drive (DD) Rover

Left wheel, active, non steering

Passive supportpp(castor, omniwheel)

Right wheel, active, non steering

Basilio Bona 9ROBOTICA 03CFIOR

Right wheel, active, non steering

Differential Drive Rover

jrj

L ft h lLeft wheel

2r

i

2r

ri

rk

2

Right wheel

Basilio Bona 10ROBOTICA 03CFIOR

Differential Drive KinematicsICC

( )tθ

ICC

( )

( )t( )tvr

R

( )tv ( )tv

( )tvBasilio Bona 11ROBOTICA 03CFIOR

( )rtv

0R

Differential Drive Kinematics

( )tv ( )( )tθICC

( )tv( )rtv

( )

Basilio Bona 12ROBOTICA 03CFIOR

0R

Differential Drive Kinematics

ICC

C Consider the generic time instant iC

vρ

vvi

ρ

rv

Basilio Bona 13ROBOTICA 03CFIOR

Differential Drive Kinematics

vrv

v

afterC

rR

iC

vv

r>v v

>v v

Turn left

Turn right

Rrv

r>v v Turn right

rR

before

Basilio Bona 14ROBOTICA 03CFIOR

Differential Drive Kinematics

C

tδθ

iC

t

θ

Basilio Bona 15ROBOTICA 03CFIOR

Differential Drive Kinematics

kδθ

ikC

kθ

kδθ

ik rkρ− j k

1k+tk

0j ik

c

ik kρ j k

θ

0

ik rkρ j k

0i

0rkt

0R

Basilio Bona 16ROBOTICA 03CFIOR

00

Differential Drive Kinematics

A

ikC

B

AB = tk

δθB

ks

ks

A sA 2k

δθ rks

Basilio Bona 17ROBOTICA 03CFIOR

Differential Drive Kinematics

ICCsin

cosxk k k k

k

C x

C y

ρ θρ θ

⎡ ⎤ ⎡ ⎤−⎢ ⎥ ⎢ ⎥= =⎢ ⎥ ⎢ ⎥+⎢ ⎥ ⎢ ⎥cosyk k k kC y ρ θ+⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦

;rk k rk kv v s s

ω δθ− −

( / 2)

( / )k k rk

vω ρ + =;rk k rk k

k k

rk kv v

v

ω δθ= =

+= ( / 2)

k k kvω ρ − =2

( )k

k rk k

v

v v vρ

=

+= =

2 ( )kk rk k

v vρ

ω= =

−

Basilio Bona 18ROBOTICA 03CFIOR

Differential Drive Kinematicst t

0 0

( ) cos ( ) ( ) d ; ( ) sin ( ) ( ) d ;t t

x t v y t vθ τ τ τ θ τ τ τ= =∫ ∫0 0

( ) ( ) dt

tθ ω τ τ= ∫Approximation when sampling period is small

0∫

1

ddk k k k k

Tt

ω θ ω θ θ δθ+= ⇒ −dt

1cos( ) sin( ) 0

k k k k xk xkx x C Cδθ δθ+⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤− −⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥

1sin( ) cos( ) 0

0 0 1k k k k yk yky y C Cδθ δθθ θ δθ

+⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥= − +⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦

(A)

Basilio Bona 19ROBOTICA 03CFIOR

10 0 1

k k kθ θ δθ+⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦

Differential Drive Kinematics

EULER APPROXIMATION

1cos

k k k kx x v T θ+ = +

1

1

sink k k k

k k k

y y v T

T

θθ θ ω

+

+

= += +

1k k k+

Basilio Bona 20ROBOTICA 03CFIOR

Differential Drive Kinematics

RUNGE‐KUTTA APPROXIMATION

1

1cos

k k k k kx x v T Tθ ω+

⎛ ⎞⎟⎜= + + ⎟⎜ ⎟⎜1 21

sin

k k k k k

y y v T Tθ ω

+ + + ⎟⎜ ⎟⎝ ⎠⎛ ⎞⎟⎜= + + ⎟⎜1

1

sin2k k k k k

k k k

y y v T T

T

θ ω

θ θ ω

+

+

= + + ⎟⎜ ⎟⎜ ⎟⎝ ⎠= +

1k k k+

Basilio Bona 21ROBOTICA 03CFIOR

Differential Drive Kinematics

EXACT INTEGRATION

( )1 1sin sink

k k k kx x

ωθ θ+ += + −( )

( )

1 1

1 1cos cos

k k k kk

kk k k k

v

y yω

θ θ

+ +

+ += − − (B)( )1 1

1

k k k kk

k k k

y yvTθ θ ω

+ +

+ = +

(A) and (B) are the sameBasilio Bona 22ROBOTICA 03CFIOR

(A) and (B) are the same

Odometry

Odometry is the estimation of the successive robot poses based on the wheel motion. Wheel angles are measured and used for pose computation

Odometric errors increase with the distance covered, and are due to many causes, e.g.

Imperfect knowledge of the wheels geometryk i i h id l b hUnknown contact points: in the ideal DD robot there are two 

geometric contact points between wheels and ground. In real robots the wheels are several centimeters wide and the actual contact points are undefinedSlippage of the wheel wrt the terrain

Errors can be compensated sensing the environment around the robot and comparing it with known data (maps, etc.)

Basilio Bona 23ROBOTICA 03CFIOR

Odometry Errors

MapDesired path

Estimated pathbased only on odometry

Basilio Bona 24ROBOTICA 03CFIOR

Map

Other important problems that must be addressed

1. Path planning: the definition of an “optimal” geometric path in the environmentpath in the environment

2. Mapping: how to build environment maps (geometrical vs semantic))

3. Localization: where am I?

4. Simultaneous Localization and Mapping (SLAM): build a map and at the same time localize the robot in the map

Basilio Bona 25ROBOTICA 03CFIOR

map and at the same time localize the robot in the map while moving

Path Planning

Path planning: it computes which route to take, from the initial to the final pose,  based on the current internal representation of the terrain and considering a “cost function” (minimum time, minimum energy, maximum comfort, etc)Complete path generation: this path is created as a sequence of successive moves from the initial poseMotion planning: is the execution of this theoretical route, by translating the plan from the internal representation to the physical movement of the wheels Next move selection: at each step, the algorithm must decide which way to move next. An efficient dynamic implementation is required

Basilio Bona 26ROBOTICA 03CFIOR

Path Planning

NavigationWhere am I going? Mission planningWhat’s the best way there? What s the best way there? Path planningWhere have I been? Map

Mi iC t ive

makingWhere am I? Localization

MissionPlanner

Carto-grapher

elib

erat

ide

BehaviorsBehaviors

BehaviorsB h i ti

ve

How am I going to getthere?

Behaviors

reac

t

Basilio Bona 27ROBOTICA 03CFIOR

Introduction to AI Robotics (MIT Press), copyright Robin Murphy 2000

Path Planning

BFixed

obstaclesMobile obstacles

Velocity constraints

Acceleration constraints

A

Basilio Bona 28ROBOTICA 03CFIOR

Path Planning

2k =

k N=

1k =

j

1k =

0j

0k =

0i0k0R

Which commands shall I give to the rover?

Basilio Bona 29ROBOTICA 03CFIOR

Path Planning

Final pose

B

Final pose

Path BPath A

Initial poseA

Basilio Bona 30ROBOTICA 03CFIOR

Motion Planning

Final pose

Move 3:rotationB

p

Move 2: go straight

Move 1: rotationA

Initial pose

A

Basilio Bona 31ROBOTICA 03CFIOR

Non‐holonomic constraints

Non‐holonomic constraints limit the possible incremental movements in the configuration space of the robot

Robots with differential drive move on a circular trajectory j yand cannot move sideways

O i h l b t idOmni‐wheel robots can move sideways

Basilio Bona 32ROBOTICA 03CFIOR

Holonomic vs. Non‐Holonomic

Non‐holonomic constraints reduce the control space withNon holonomic constraints reduce the control space with respect to the current configuration (e.g., moving sideways is impossible)p )Holonomic constraints reduce the configuration space

Basilio Bona ROBOTICA 03CFIOR 33

Academic year 2010/11 by Prof. Alessandro De Luca

34ROBOTICA 03CFIOR

Academic year 2010/11 by Prof. Alessandro De Luca

35ROBOTICA 03CFIOR

Academic year 2010/11 by Prof. Alessandro De Luca

36ROBOTICA 03CFIOR

Academic year 2010/11 by Prof. Alessandro De Luca

37ROBOTICA 03CFIOR