3.Dynamics

Lecture 6

Manipulator Dynamics

Introduction

• Dynamics concerns the motion of bodies

• Includes• Kinematics – study of motion without reference to

the forces that cause it

• Kinetics – relates these forces to the motion

• Dynamic behavior of a robot = time rate of

change of the arm configuration in relation

to the torques exerted by the actuators

Directions of study

• Forward dynamics• Given vector of joint torques -> find the resultant

manipulator motion

• Inverse dynamics• Given a vector of manipulator positions, velocities

and accelerations -> find the required vector of

joint torques

Torques

• Control problem: the actuator has to balance

torques from 4 different sources:

– Dynamic torques (arise from motion)» Inertial (prop. to joint acceleration, according to Newton’s

law)

» centripetal (prop. to square of joint velocity, directed toward

the centre of circular motion)

» Coriolis (vortical forces, interaction of two rotating links)

– Static torques (arise from friction)

– Gravity torques (due to gravity)

– External torques (exerted on the end-effector, arise

from the task)


• Mathematical equations describing the

dynamic behavior of the manipulator

– For computer simulation

– Design of suitable controller

– Evaluation of robot structure

– Joint torques Robot motion, i.e.

acceleration, velocity, position


• Lagrange-Euler Formulation

– Lagrange function is defined

• K: Total kinetic energy of robot

• P: Total potential energy of robot

• : Joint variable of i-th joint

• : first time derivative of

• : Generalized force (torque) at i-th joint

iq

PKL

i

i i

d L L

dt q q

iq iq

i

Center of mass

• Center of mass is the point on a body that moves

in the same way that a single particle subject to

the same external forces will move

– with:

» rcm the location of the center of mass;

» M the total mass of the object;

» r is the location of the reference frame;

» dm is the differential element of mass, at point r

1

1 1 i

cm i ir r dm m rM M

Inertia

• The tendency of a body to remain in a state of rest or uniform motion. – Inertial Frame = the frame in which this state is

measured (i.e. a frame at rest or moving with constant velocity)

– Non-inertial Frame = a frame that is accelerating with respect to a inertial frame.

Moment of inertia

• Moment of inertia is the rotational inertia of a body w.r.t. an axis of rotation. Moment of inertia depend on the axis and the on the manner in which the mass is distributed. Is the sum of the product between mass of particles and the square of their distance to the axis of rotation

• where mi is the mass of particle i, and ri is the distance to the particle i.

• For a rigid body with a continuous distribution of mass, summation becomes integral over the whole body:

• Ex: Inertia of a cylinder, length l, mass M: • Consider the cylinder made up of an infinite number of cylinders

thickness dr, at radius r, of mass dm, and volume dV

2

1

i

i iI m r

2I r dm

4 4 2 2

2 1 2 12 3

2

2 24 2

dm dV l rdr

r r r rI r dm l r dr l M

Inertia tensor

• a rigid body, free to move in space, has an infinite number of possible rotation axis. The inertia tensor is the analog, for general rotations of the moment of inertia for rotation about a single axis.

• For a rigid body rotating about a fixed point, which is not the center of the mass, the inertia tensor I:

dmyxyzdmxzdm

yzdmdmzxxydm

xzdmxydmdmzy

I

)(

)(

)(

22

22

22

xx xy xz

yx yy yz

zx zy yz

I I I

I I I I

I I I

2 2 2 2

2 2

xxI y z dm y z dV

y z dx dy dz

the moment of inertia w.r.t. the x axis of

the Cartesian frame

xy yxI I x y dm

the product of inertia w.r.t the x axis


• Kinetic energy

– Single particle:

– Rigid body in 3-D space with linear velocity (V) , and

angular velocity ( ) about the center of mass

– I : Inertia Tensor:

• Diagonal terms: moments of inertia

• Off-diagonal terms: products of inertia

2

2

1mvk

TT IVmVk2

1

2

1

dmyxyzdmxzdm

yzdmdmzxxydm

xzdmxydmdmzy

I

)(

)(

)(

22

22

22

dmzyI xx )( 22

dmxyI xy )(

Velocity of a link

i

irix

iy

iz

0x

0z

0y

ir0

1

i

i

i

i

iz

y

x

r

i

i

i

i

i

i

ii rTTTrTr )( 1

2

1

1

000

A point fixed in link i and expressed w.r.t. the i-th frame

Same point w.r.t the base frame

Velocity of a link

i

i

i

j

j

j

ii

i

ii

i

i

i

i

i

i

i

i

i

i

i

i

i

i

i

ii

i

rqq

TrTrTTT

rTTTrTTT

rTTTdt

dr

dt

dVV

)(

)(

1

001

2

1

1

0

1

2

1

1

01

2

1

1

0

1

2

1

1

000

0i

irVelocity of point expressed w.r.t. i-th frame is zero

i

ir

Velocity of point expressed w.r.t. base frame is:i

ir

Velocity of a link

1000

01

iii

iiiiiii

iiiiiii

i

idCS

SaCSCCS

CaSSSCC

T

1000

0000

1 iiiiiii

iiiiiii

i

i

iCaSSSCC

SaCSCCS

q

T

1000

0

0000

0000

0001

0010

11

iii

iiiiiii

iiiiiii

i

ii

i

i

i

dCS

SaCSCCS

CaSSSCC

TQq

T

• Rotary joints, iiq

0000

0000

0001

0010

iQ

Velocity of a link

1000

01

iii

iiiiiii

iiiiiii

i

idCS

SaCSCCS

CaSSSCC

T

0000

1000

0000

0000

1

i

i

i

q

Ti

ii

i

i

i TQq

T1

1

• Prismatic joint, ii dq

0000

1000

0000

0000

iQ

Velocity of a link

ijfor

ijforTTQTTT

q

Ti

i

j

jj

j

j

j

i

0

11

1

2

2

1

1

00

ijfor

ijforTQT

q

TU

i

jj

j

j

i

ij0

1

1

00

The effect of the motion of joint j on all the points on link i

i

j

i

ijij

i

i

i

j

j

j

ii

i

i

i

ii

i rqUrqq

TrTTT

dt

dr

dt

dVV

11

01

2

1

1

000 )()()(

Kinetic energy of link i

i

p

rp

T

ir

Ti

i

i

iip

i

r

i

p

rp

T

ir

Ti

i

i

iip

i

r

i

p

i

r

Ti

irir

i

ipip

T

iiiiii

qqUdmrrUTr

dmqqUrrUTr

dmrqUrqUTr

dmVVtracedmzyxdK

1 1

1 1

1 1

222

)(2

1

2

1

)(2

1

)(2

1)(

2

1

n

i

iiaATr1

)(

• Kinetic energy of a particle with differential mass dm in link i

Kinetic energy of link ii

p

rp

T

ir

Ti

i

i

iip

i

r

ii qqUdmrrUTrdKK1 1

)(2

1

iiiiiii

ii

zzyyxx

yzxz

iiyz

zzyyxx

xy

iixzxy

zzyyxx

iii

iiiiii

iiiiii

iiiiii

Ti

i

i

ii

mzmymxm

zmIII

II

ymIIII

I

xmIIIII

dmdmzdmydmx

dmzdmzdmzydmzx

dmydmzydmydmyx

dmxdmzxdmyxdmx

dmrrJ

2

2

2

2

2

2

1

i

i

i

i

iz

y

x

r

Center of mass

dmxm

x i

i

i

1

Pseudo-inertia

matrix of link i


n

i

i

p

i

r

rp

T

iriip

i

p

rp

T

ir

Ti

i

i

iip

i

r

n

i

n

i

i

qqUJUTr

qqUdmrrUTrKK

1 1 1

1 111

)(2

1

)(2

1

iJ

iq

• Total kinetic energy of a robot arm

: Pseudo-inertia matrix of link i, dependent on the mass

distribution of link i and are expressed w.r.t. the i-th frame,

Need to be computed once for evaluating the kinetic energy

Scalar quantity, function of and ,iqni ,2,1


)( 00

i

i

i

i

i

ii rTgmrgmP

n

i

n

i

i

i

i

ii rTgmPP1 1

0 ])([

)0,,,( zyx gggg

• Potential energy of link i

: gravity row vector

expressed in base frame2sec/8.9 mg

ir0

i

ir : Center of mass

w.r.t. i-th frame

: Center of mass

w.r.t. base frame

• Potential energy of a robot arm

g

Function of iq


• Lagrangian functionn

i

i

j

i

k

n

i

i

i

i

ikj

T

ikiij rTgmqqUJUTrPKL1 1 1 1

0 )()(2

1

j

jji

n

ij

j

n

ij

j

k

j

m

mk

T

jij

m

jkn

ij

j

k

k

T

jijjk

ii

i

rUgm

qqUJq

UTrqUJUTr

q

L

q

L

dt

d

1 11

)()(

)(


kiorji

kjiTQTQT

jkiTQTQT

Uq

Ui

jj

j

kk

k

i

kk

k

jj

j

ijk

k

ij

0

1

1

1

1

0

1

1

1

1

0

The interaction effects of the motion of joint j and joint k

on all the points on link i

ijfor

ijforTQT

q

TU

i

jj

j

j

i

ij0

1

1

00

The effect of the motion of joint j on all the points on link i


im

n

k

n

m

kikm

n

k

kiki CqqhqD 1 11

n

kij

T

jijjkik UJUTrD),max(

)(

n

mkij

T

jijjkmikm UJUTrh),,max(

)(

• Dynamics Model

j

jji

n

ij

ji rUgmC


)(),()( qCqqhqqD

nnn

n

DD

DD

D

1

111

nh

h

qqh 1

),(

• Dynamics Model of n-link Arm

nC

C

qC 1

)(

The Acceleration-related Inertia

matrix term, Symmetric

The Coriolis and Centrifugal terms

The Gravity terms

n

1

Driving torque

applied on each link

Dynamic model of an n-link arm

2

g mA q B q q C q D q Q q Q

With:inertial termA q

Coriolis termB q q 2 centrifugal termC q

viscuous friction termD q

gravity termgQ q

( )activetorque motorsmQ

Nonlinear – variable coefficients in equ

Multivariable – number of inputs is

equal to number of joint variables

Coupled – outputs depend not only on

the corresponding input

Dynamic model

1

2

x qx = =

x q

2

g mA q B q q C q Q Q

2xqx = =

q q

2


2-1 -1

g mq -A B q q C q Q A Q -1

mq L x A Q

1 2

-1

2 m

x = x

x q L x A Q

2 n

m-1

x 0x = + Q

L x A

x = A x + B x u

The dynamic model form

Define the state variable

Another form

2


2

gB q q C q Q h(q)

mQ A q h(q)

h is the coupling vector

Example

X0

Y0

X1Y1

1

L

m

1

0

01

1

l

r

Example: One joint arm with point mass (m)

concentrated at the end of the arm, link length is l , find the dynamic model of the

robot using L-E method.

Set up coordinate frame as in the figure

]0,0,8.9,0[g

1

1

11

11

1

1

1

0

1

0

1000

0100

00

00

rCS

SC

rTr

Example

X0

Y0

X1Y1

1

L

m

0

0

1

1

1

1

1

01

1

1

1

0

1

01

Cl

Sl

rTQrTdt

drV

1

1

11

11

1

1

1

0

1

0

1000

0100

00

00

rCS

SC

rTr

Example

1

1 2

1 1 1

10 0

02

0

l S

l CK Tr l S l C dm

dmVVTrdKT

)(2

111

2 2 2

1 1 1

2 2 2

21 1 1

1

( ) 0 0

( ) 0 01

2 0 0 0 0

0 0 0 0

l S l S C

l S C l CTr m

2

1

222

1

22

1

2

2

1])()([

2

1 mlmClSl

Kinetic energy

Example

)( 1

1

0 rTmgP

1

0

0

1000

0100

00

00

008.9011

11 l

CS

SC

m

• Potential energy

18.9 Slm

1

2

1

2 8.92

1SlmmlPKL

11

2

11

2

11

1

8.98.9)(

)(

ClmmlClmmldt

d

LL

dt

d

• Lagrange function

• Equation of Motion

Example: Puma 560

• Derive dynamic equations for the first 4 links of

PUMA 560 robot

Example: Puma 560

Joint i i i ai(mm) di(mm)

1 1 -90 0 0

2 2 0 431.8 -149.09

3 3 90 -20.32 0

4 4 -90 0 433.07

5 5 90 0 0

6 6 0 0 56.25

• Get robot link parameters

• Set up D-H Coordinate frame

• Get transformation matricesi

iT 1

• Get D, H, C terms

Example: Puma 560

)()()( 31331212211111111

TTTUJUTrUJUTrUJUTrD

)()( 31332212222112

TTUJUTrUJUTrDD

)( 313333113

TUJUTrDD

• Get D, H, C terms n

kij

T

jijjkik UJUTrD),max(

)(3,2,1;3 in

)()( 323322222222

TTUJUTrUJUTrD

)( 323333223

TUJUTrDD

)( 3333333

TUJUTrD

Example: Puma 560

)()()( 313311212211111111111

TTTUJUTrUJUTrUJUTrh

n

mkij

T

jijjkmikm UJUTrh),,max(

)(

nh

h

qqh 1

),(

• Get D, H, C terms n

k

n

m

mkikmi qqhh1 1

)()( 313312212212112

TTUJUTrUJUTrh

)( 313323123

TUJUTrh

)()( 313321212221121

TTUJUTrUJUTrh

)()( 313322212222122

TTUJUTrUJUTrh……

2

3133231321313132123

2

2122211213111321112

2

11111

qhqqhqqhqqh

qhqqhqqhqqhqhh

)( 313313113

TUJUTrh

Example: Puma 560

2

0

2

1211 )( TQU

kiorji

kjiTQTQT

jkiTQTQT

Uq

Ui

jj

k

jk

k

i

kk

k

jj

j

ijk

k

ij

0

1

1

1

1

0

1

1

1

1

0

3

0

2

1311 )( TQU


1

0

2

1111 )( TQU

2

1

2

2

1

0222 )( TQTU

3

12

1

01321312 TQTQUU

3

23

2

01313 TQTQU

2

12

1

01221212 TQTQUU

3

1

2

2

1

0322 )( TQTU

3

23

2

12

1

0332323 TQTQTUU 3

23

2

01331 TQTQU 3

223

2

0333 TQQTU

Example: Puma 560

j

jji

n

ij

ji rUgmC

3

3313

2

2212

1

11111 rgUmrgUmrgUmC

3

3323

2

22222 rgUmrgUmC


3

33333 rgUmC

Documents

3.Dynamics