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


ICC

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

?

wheels slip


wheels slip

Kinematics

δWj

WR

αδ

Wω

drj W

i

αrϕ=v

rR

yr

ωri

y

0R


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


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

ω


( )

Differential Drive (DD) Rover

Left wheel, active, non steering

Passive supportpp(castor, omniwheel)

Right wheel, active, non steering


Right wheel, active, non steering

Differential Drive Rover

jrj

L ft h lLeft wheel

2r

i

2r

ri

rk

2

Right wheel


Differential Drive KinematicsICC

( )tθ

ICC

( )

( )t( )tvr

R

( )tv ( )tv

( )tvBasilio Bona 11ROBOTICA 03CFIOR

( )rtv

0R

Differential Drive Kinematics

( )tv ( )( )tθICC

( )tv( )rtv

( )


0R


ICC

C Consider the generic time instant iC

vρ

vvi

ρ

rv



vrv

v

afterC

rR

iC

vv

r>v v

>v v

Turn left

Turn right

Rrv

r>v v Turn right

rR

before



C

tδθ

iC

t

θ



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


00


A

ikC

B

AB = tk

δθB

ks

ks

A sA 2k

δθ rks



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ρ

ω= =

−


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)


10 0 1

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


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+



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+



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.)


Odometry Errors

MapDesired path

Estimated pathbased only on odometry


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


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


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


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

Path Planning

BFixed

obstaclesMobile obstacles

Velocity constraints

Acceleration constraints

A


Path Planning

2k =

k N=

1k =

j

1k =

0j

0k =

0i0k0R

Which commands shall I give to the rover?


Path Planning

Final pose

B

Final pose

Path BPath A

Initial poseA


Motion Planning

Final pose

Move 3:rotationB

p

Move 2: go straight

Move 1: rotationA

Initial pose

A


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


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


35ROBOTICA 03CFIOR


36ROBOTICA 03CFIOR


37ROBOTICA 03CFIOR

Documents

Mobile & service robotics 02 - PoliTO