Robot Dynamics

Hesheng WangDept. of Automation

Shanghai Jiao Tong University

What is Robot Dynamics?

• Robot dynamics studies the relation between robot motion and forces and moments acting on the robot.

1

x

l1

l2

2

1

2

The velocity v can be determined from the cross product of and rp . Here rp is a vector from any point on the axis of rotation to P.

v = x rp = x r

The direction of v is determined by the right-hand rule.

Rotation about a Fixed Axis

The acceleration of P can also be defined by differentiating the velocity.

a = dv/dt = d/dt x rP + x drP/dt= x rP + x ( x rP)

It can be shown that this equation reduces to

a = a x r – w2r = at + an

Rotation about a Fixed Axis(continued)

The magnitude of the acceleration vector is a = (at)2 + (an)2

Normal/centripetal accelerationTangent accel

Rotation of a Vector• Consider rotation of a vector about a

axis.

Point P is rotating about axis u.r is the position vector of point P.

u

Pr

rotation of speed the:

uω :lcotiyangular ve The

rωrr

dtd

:ectorposition v of change of rate The

Rotation of a Frame

• Consider a frame B rotating about an unit vector u.

A frame reference the w.r.t.B frame of

axes theof vectorsldirectionaunit :,, BBB kji u

xByB

zB

Frame A

BBBBBB dtd

dtd

dtd kωkjωjiωi , ,

RωR

kωjωiωkjiR

:B frame ofmatrix rotation theof derivative The

BBBBBBdtd

General Motion• A general motion can be considered a

combination of a translation with a point and motion about the point.

A with slate that tranframeA :frame reference fixedA :

111 zyxAzyxO ooo

y1

x0

z0

y0

x1

z1B

A

O

rωVV AB

)( rωωrαaa AB

The velocity relation:

The acceleration relation:

Introduction to Dynamics

Newton’s Laws of Motion

onaccelerati the:forcenet the:

mass the:

aF

aFm

m

Fa

F’F

First Law: A particle originally at rest, or moving in a straight line at constant velocity, will remain in this state if the resultant force acting on the particle is zero.

Second Law: If the resultant force on the particle is not zero, the particle experiences an acceleration in the same direction as the resultant force. This acceleration has a magnitude proportional to the resultant force.

Third Law: Mutual forces of action and reaction between two particles are equal, opposite, and collinear.

ExampleFind the accelerations of the ball

and the wedge.

m1

m2

Frictionless surface

N

m1gm2gN

R

Solution:(1) Draw the free-body diagram of the particles(2) Apply Newton’s 2nd Law

For the ball

x

y

(2) cos(1) sin

11

1

gmNymNxm

b

b

For the wedge

(3) sin2 Nxm w

Example (continued)• Consider acceleration relationship between

the ball and the wedge

aw

ab

ab/w

wedge the.r.t. w ball theof velocity relative :

/

/

wb

wbwb

aaaa

Note that the relative velocity is along the surface of the wedgeFrom the diagram, we have

(5) sin(4) cos

/

/

wbb

Wbwb

ayaxx

From eqs. (1)-(5), we can solve the five unknowns, i.e. the acceleration

Linear Momentum• Linear momentum: product of mass and velocity:

– It is a vector, in the same direction as velocity

• Principle of Linear Momentum: The rate of change of the linear momentum of a particle is equal to the result force acting on the particle

VL m

LVaF dtdm

:Law 2nd sNewton' Fromm

m/sKg : UnitSI - V

mV

Angular Momentum• r: the position vector of a particle w.r.t. a

reference point O.• V: the velocity of the particle• m: the mass of the particle• The angular momentum of the particle

about reference O:

V

r

O

VrH mo

s

o

/mKg isunit Itsrule hand-right by the determined isdirection Its

. and both lar toperpendicu vector a is

2

rVH

Principle of Angular MomentumConsider a force F acting on the particle V

r

O

F

oo

o mm

MFrH

FrVrVrH

Principle of angular momentum: The rate of change of the angular momentum of particle about a fixed point O is equal to the resultant moment of forces acting on the particle about point O

Differentiating the angular momentum

Dynamics of a System of ParticlesConsider a system of n particles.

eij

f2

f1 fi

fj

m1

m2

mi

mj

eji

fi: the external force exerted on particle ieij: the internal force exerted on particle i

by particle jmi: mass of particle i.ri: position vector of particle i

iiii dtdmm rVL :momentumLinear

i

i

mmr

rc :mass ofcenter The Cci Mm VrL Total mass

Velocity of center of massThe linear momentum of a system of particles is

equal to the product of the total mass and the velocity of the center of mass

Dynamics of a System of ParticlesDifferentiating the linear momentum

ei

f2

f1 fi

fj

m1

m2

mi

mj

ej

i j

ijii

ii dtdm )(2

2

efrL

i

icC MM faVL ac: acceleration of center of mass

The rate of change the linear momentum of a system of particles is equal to the resultant of all EXTERNAL forces acting on the particles

i

ii j

ij fLe 0

The center of mass of the system moves as if all the forces and masses are concentrated at the center of mass

Equation of motion ofCenter of mass

Angular Momentum of a System of Particles

Similarly, by differentiating the angular momentum

ei

f2

f1 fi

fj

m1

m2

mi

mj

ej

O

i

Oiio mdtd MVrH

The resultant moment of all EXTERNAL forces acting on the system about O

The reference point must be a fixed point. However, the center of mass of the system can be the reference point even when it is moving

Example:Example: Calculate the angular acceleration of the massless link

m2

m1

l1

l2

O

(2) Consider the angular momentum about O m1g

Rx

Ry

m2g

SolutionConsider the two particles and the link as a system

coscos

00

)(00

dt

1122222

211 glmglmlmlm

doo MH

222

211

2211 cos)(lmlm

lmlm

(1) Analyze the external forces

Linear and Angular Momentums for Rigid Body

• Since a rigid body can be considered as a system with infinite number of particles, the linear momentum

CmVL Center of mass

CV

body theoflocity angular ve :C.about matrix tensor Inertia :

ωI

IωH C

•The angular momentum about the center of mass ω

Newton’s Equation and Euler’s Equation

• A general motion can be considered a combination of a translation with the center of mass and motion about the center of mass

C with slates that tranframeA :frame reference fixedA :

111 zyxCzyxO ooo

icm fFa

iiiAA

CCA n

dtd frIωωωIMH )(

The equation of translation:

The equation of motion about C:

y1

x0

z0

y0

x1

z1

C

O fi

f1ni

ri

Newton’s equation

Euler’s equation

• The angular velocity is with respect to the translating frame.

• The inertia tensor matrix is with respect to the translating frame, so it will change its value with rotation of the body.

• The force is the resultant of the EXTERNAL force

• The moment is the resultant moment of the EXTERNAL forces and moments.

Newton’s Equation and Euler’s Equation (Cont’)

Example• Derive the dynamic equation of the 2 DOF

manipulator. Here, the masses of links 1 and 2 are m1 and m2 respectively. Assume that the mass is uniformly distributed over the link.

2

Solution:

(1) Analyzing forces acting on the links

m2g

Rx

Ry

1

x0

l1

l2

2

1

2

1Rx

Ry

Nx

Nym1g

O

Example (continued)(2) Dynamics of link 1. As

link 1 is rotating about O,

1O oI M

(1) sincoscos2

111111

111 lRlRlgmI xyO

1Rx

Ry

Nx

Nym1g

1

Moment of inertia about O Resultant moment about O

Example(3) Dynamics of link 2. As link 2 is in a general plane motion

2

2

22222

2

CCC

CmMωIωωI

Fa

2

m2g

Rx

RyC2

BAcceleration of C2: BnCBtCBC //2

aaaa

1

...111

111

12

11

12

11

clsl

slcl

BtBnB

aaa

Bna

Bta

21

BnC /a

BtC /a

12122

212

/ )(2 s

clBnC a

1212

212

/ )(2 c

slBtC a

122

212

12212

12

1111

122

212

12212

12

1111

)(2

)(2

)(

)(2

)(2

)(2

slclscl

clslcslC

a

Newton’sequation: (3)

(2) 22

2

2

2gmRam

Ramyyc

xxc

Example (continued)Consider Euler’s equation. As the mass in uniformly distributed and the link is

symmetric, the inertia tensor matrix is diagonal.

122

122

2212

22

21212

22

22

00

00

000000

00

00

000000

cRlsRlII

I

II

I

yxzz

yyxx

zz

yyxx

(4) 22

)( 122

122

2212 cRlsRlI yxzz

From (2) and (3), we can solve Rx and Ry. Substituting Rx and Ry in eqs. (1) and (4) leads to the dynamic equation of the robot arm.

Formulation of Robot Dynamics

Recursive Newton-Euler FormulationWe consider manipulators with revolute joints only.

i

Joint i

Joint i-1

Link i-1 zizi-1

ai-1

1i xi-1

yi-1

Oi-1

xi

yi

si-1Ci: centerof mass

Oi

ri

Angular velocity relation between link i-1 and i:

1/1 iiii ωωω

(1) 1 iiii zωω

Differentiating (1) (2) 11 iiiiiii zωzωω

Relative angular velocity if link I to link i-1

Recursive Newton-Euler Equation (Cont’)

• Consider velocity and acceleration of Oi.

1-ii1/1-i1

i

O toO of velocity realtive :O of velocity:

O of velocity:

ii

ii

VVV

1/1 iiii VVV

As the relative motion of Oi w.r.t. Oi-1 is a motion about Oi-1,

111/ iiii sωV (3) 111 iiii sωVV

Accelerationat Oi: (4) )( 111111

1

iiiiii

RnRtiisωωsωa

aaaa

Acceleration at the center of mass: (5) )(

/

iiiiii

OCiC iii

rωωrωa

aaa

Forward Equations

From (1), (2), (4) and (5), we can recursively calculate the angular velocity and acceleration of the links, and the acceleration at the center of mass.

The initial conditions:

0 ,0 0 ,0

0,iWhen

0000

aVωω

i=0

Calculate i+1 from (1)

Calculate i+1 from (2).

Calculate ai+1 from (3)

Calculate aci+1 from (4)

i=n-1 ENDi=i+1No YES

(1) 1 iiii zωω

(2) 11 iiiiiii zωzωω

(3) )( 111111 iiiiiii sωωsωaa

(4) )( 11 iiiiiiCi rωωrωaa

Dynamic equation of robot link

Derive the dynamics by applying Newton-Euler equations to link i.

(1) Draw the free-body diagram of link i. Assume that the links are rigidly connected. Cut link i from the arm:

gmm

iiici i

00

1ffa

ni -fi+1

-ni+1

fi

si

ri

mig

Oi

Ci

Oi+1

fi: the force acting on link i by link i-1ni: the moment applied on link i by link i-1

Applying Newton’s Law

(7) 00

1

gmm

iicii i

faf

Dynamics of robot link (Cont’)Applying the Euler’s equation

11 )( iiiiiiiiiiii frsfrnnωIωωI

ni -fi+1

-ni+1

fi

si

ri

mig

Oi

Ci

Oi+1

(8) )(

11

iiiiiiiiiiii frsfrnωIωωIn

Eqs. (7) and (8) give the recursive backward equation for calculating the interaction force and momentRelation between ni and joint torque

iTii nz

as actuator produces torque about the joint axis only

Backward CalculationInitial conditions:

links ofnumber the:0 ,0 :link)last (for thek iWhen 1k1

kk nf

i=K

Calculate fi from (7)

Calculate ni from (8)

i=i-1

i=0 ENDYESNO

Lagrange Formulation of Robot Dynamics

• Lagrange formulation is an analytical method for deriving the robot dynamics. It is based on the energy and work principle

• Energy of Mechanical Systems– Kinetic energy: energy due to motion of a

particle or body– Potential energy: due to gravitational forces,

deformation of mechanical systems, etc.

Kinetic Energy• A particle (body) has kinetic energy when it moves.

Kinetic energy is always greater than zero

Iωω

rr

Tc

T

mV

dm

21

21

21K :body rigid aFor

2V

2

21K :particle aFor mV

2

21K :particles of system aFor iiVm

V

C

ω

Vc

I: inertia tensor

Potential Energy• We here consider the gravitational potential

energy only.

cmgz U:body rigid aFor

z

mgz U :particle aFor

igzim U:particles of system aFor

z: the height of the particle w.r.t. a reference level

The height of the center of mass

Potential energy is a value relative to the reference. It could be positive, zero and negative

WorkWhen a particle underwent a displacement r

under a constant force f, the work done by the force on the particle is

– Work is a scalar– It could be positive, zero and negative– SI unit: Nm

r

frf TW

Work done by time-varying forceConsider the work done by a

time-varying force on a particle that moved from one position to another.

f(t)

Position 1

Position 2

V2

V1

K 21

21

)(

21

22

2

1

2

1

2

1

mVmV

dmddtdm

dtW T

VVrV

rf

Work-energy principle: The work done by a force acting on particle is equal to the change of its kinetic energy

Conservative force and Non-Conservative force

• The force associated to the potential energy is called conservative force. A force that is not associated with the potential energy is called non-conservative force.

• The work done by conservative force (gravity force)

• The work done by non-conservative forces

UzzmgWg )( 12

mg z1

z2

)( UKUKWKWW

n

gn

The work done by non-conservative forces is equal to the change of total energy

Conservation of Energy• If no non-conservative force acting on a system

(a particle, or a system of particles, or a rigid body), does not do any work, the total energy of the system is conserved.

constantUK

Lagrange Equation• Generalized coordinates q: A set of parameters

for representing the configuration (position & orientation) of a system.– q must specify the configuration uniquely– Once the values of q are fixed, the system cannot

move.– The choice of q is not unique.

• Degree of freedom (DOF): The dimension of the generalized coordinates vector q is called degrees of freedom of the system

Examples

1

x0

l1

l2

2

1

2

21q DOF =2

(x,y)

yx

q DOF=4

Generalized ForceConsider the work done by non-conservative

forces under a differential displacement of the system

qqf

r

scoordinate dgeneralize theof change aldifferenti a todue ofpoint action

at thent displaceme aldifferenti :i

i

fi:W work aldifferenti The

iTiW rf

The generalized force F of the system is given by

vectorscoordinate dgeneralize: )()(

q q

F W

ExampleCalculate the generalized force of

the 2 DOF arm

1

x0

l1

l2

2

1

2f

T21,motion aldifferentiFor q

21 :sCoordinate dGeneralize q

The differential work

point end theofmotion aldifferenti the:2211

xxf

TW

qq )(Jx qfqqqqf

T

T

JJW))((

)( 21

2211

当前无法显示此图像。

Generalized Force:

Lagrange Equationq: the generalized coordinates of a systemK: Kinetic energy of the systemU: the potential energy of the systemF: the generalized forces of the systemDefine L=K-U: Called LagrangianThe dynamics of the system is given by

Fqq

LL

dtd

Derivation of Robot Dynamics using Lagrange Equation

1) Choose the generalized coordinates q (usually use the kinematics parameters defined by the D-H method)

2) Identify the non-conservative forces that are exerted at the system and do work

3) Calculate the kinetic energy K and the potential energy U, and then L=K-U

4) Calculate the partial derivatives5) Calculate the generalized force F.6) Apply the Lagrange equation.

Example 1Example: Denote the mass of link i by mi. The mass is

uniformly distributed over the link. Derive the dynamics of the 2 DOF arm.

1

x

l1

l2

2

1

2Solution

21 :scoordiante dGeneralize )1( q

21 , : workdo that forces veconservati-Non )2( (3) The kinetic energy:

211 2

1K O,about rotates 1link oI22

222 22 2

121K motion, plane generalin 2link cc IVm

c2

Centerof mass

y

)()(4

2

2211221

221

222

121

222

122

11

122

11

222

2

2

cllllyxV

slsly

clclxccc

c

cPosition ofthe Center Of mass

Example 1 (continued)The total kinetic energy

2

2 21 2 2 1 1 2 1 2 2 1 1 2

2 22 2 1 2

1 1( ) ( )2 2

1 1 ( )( )8 2

o

c

K K K I m l m l l c

m l I

The potential energy (assume y=0 is the reference

)2

(2 12

21121

112211 slslgmslgmgymgymU cc

(4) Calculate the partial derivatives

))(41()

21()( 21

2222122121

2120

12

cIlmcllmlmIL

))(41(

21

2122212212

22

cIlmcllmL

)2

(2 12

21121

11

1clclgmclgmL

)(21

2 2112212122

22

sllmclgmL

Example 1 (continued)5) The generalized forces:

2211 W

21 F

6) Apply the Lagrange equation: Fqq

LL

dtd

21

12222

12212

122211212212212

2

1

222222122

222

2212222222122

222

2121

5.05.05.0)5.0()5.0(25.05.025.0

5.025.025.0

gclmsllmgclmgclmmsllm

IlmcllmIlmcllmIlmcllmIlmlmI

cc

cco

Example 2Example 2: The moment of inertia of the first link is I1. The mass

of link 2 is m2. The mass is concentrated at the endpoint. An external force f acts at the endpoint. Derive the dynamics of the arm.

0003009020001

22

111

l

dai iiii

x0

y0z0

z1

x1

y1

z2

x2y2 z3

x3y3

Solution: (1) Generalized coordinates

Use the D-H method to assign frames and select the joint angles as the generalized coordinates

21qGeneralized coordinates:

f

l2

Example 2 (continued)(2) No-conservative forces that do work:

(3) Kinetic energy, potential energy and Lagrangian

2111 2

1 :1Link IK 2

22 21 :2Link VmK

To find the velocity V of the endpoint, we need to solve the forward kinematics

100001000000

1111

10 cs

scT

100000010000

22

22

21

cs

scT

100001000010

001 2

32

lT

f force external theand ,, :inputsjoint 21

Kinetic energy:

Example 2 (continued)

Forward kinematics:

10000000

2212121

12121

21

10

20

cscsscsssccc

TTT

10000 2222

2121212121212121

32

20

30

slcscslcsscscclssccc

TTT

22

22

21

22

22

2

22212212

lclVslcslccl

dtd

V

The potential energy (Assuming that U=0 when z0=0).

22211 sglmghmU

2221122

222

21

22

2221 2

1)(21 sglmghmlmclmIUKL

Lagrangian:

Example 2 (continued)(4) Calculate the partial derivatives

122

2221

1)(

clmIL

2222

2

lmL

01

L

2222

12222

22sin

21 cglmlmL

Example 2 (continued)(5) Generalized forces:

fqJFxf )( 21

2211TTW

x: the position of the endpoint

22212212

slcslccl

x

22

212212212212

22212212

0)(

clsslcclsclcsl

slcslccl

qqxqJ

(6) Applying the Lagrange equation leads to the dynamics:

fqJ )(

02sin5.02sin5.0

00

21

2222

12222

212222

2

122

22

2221

T

cglmlmlm

lmclmI

Structure of Robot Dynamics• The dynamics of the 2 DOF manipulator:

21

)(

122212221121

),(

212212

2212212

2

1

)(

222222122

222

2212222222122

222

2121

5.05.0)5.0(

5.0)5.0(

25.05.025.05.025.025.0

qqq

q

GC

H

cc

cco

gclmgclmgclmm

sllmsllm

IlmcllmIlmcllmIlmcllmIlmlmI

positionjoint on the depending r,manipulato theofmatrix Intertia :)(qH matrix symmetric )()( qq HHT

forces Coriolis and lcentrifuga the:),( qq Cjoint. a ofcity joint velo theof square on the depend that Terms :force Centrigual

joints. twoof citiesjoint velo theof

product on the depend that Terms:force Coriolis

forcegravity The :)(qG

Structure of Robot Dynamics

• The centrifugal and Coriolis term can be re-written as

2

1

),(

212212

212212

)(21

22212

2221222212

212212

2212212

0)5.0(5.0)5.0(5.00

05.05.0

21

5.0)5.0(),(

qqSqH

qqC

sllmsllm

sllmsllmsllm

sllmsllm

0),( ,any For ),(),(

i.e. matrix, symmetric-skew a is ),(

T

xqqSxx

qqSqqSqqS

T

Structure of Robot Dynamics

• In general, the dynamics of a robot manipulator has the following form:

τqGqqqSqHqqH )()),()(21()(

matrix. inertial definite-positive and Symmetric :)(qH

matrix symmetric-skew a :),( qqS

0energy kinetic theis )(21

qqHq T

nT R ,0),( xxqqSx

Inertial force Centrifugal and Coriolis forces Gravity Joint inputs

Linear Parameterization of Robot Dynamics

The dynamics of the 2 DOF arm:

21

122212221121

212212

2212212

2

1

222222122

222

2212222222122

222

2121

5.05.0)5.0(

5.0)5.0(

25.05.025.05.025.025.0

gclmgclmgclmm

sllmsllm

IlmcllmIlmcllmIlmcllmIlmlmI

cc

cco

225214

21232

2222

2222

21211

5.0)5.0(

5.025.0

25.0

lmlmm

llmIlm

IlmlmIc

co

DefineThe parameters depend on mass, length, moment of inertia. They are the physical parameters

21

54321

),,,(

122111221

12122122122100

)5.0(2)2(

β

qqqqY

gcθsθcθθgcgcθθθsθθcθθ

Parametervector

The result can be generalized to n DOF robot manipulator