intro to robotics

Chapter 6 Manipulator dynamics6.1 Introduction6.2 Acceleration of a rigid body6.3 Mass distribution6.4 Newton-Euler’s equation6.5 Iterative Newton-Euler dynamics equation6.9 Lagrangian formulation of manipulator dynamics6.10 Inclusion of non-rigid body effect6.11 Dynamic simulation

1

Introduction• Our study of manipulators so far has focused on kinematic

considerations only. We have studied static positions, staticforces, and velocities; but we have never considered theforces required to cause motion.

• In this chapter, we consider the equation of motion for amanipulator – the way in which motion of the manipulatorarise from torques applied by the actuator or from externalforces applied to the manipulator.

• Dynamics of mechanisms is a field in which many books have been written. Indeed, one can spend years studying the field.

• Obviously, we cannot cover the material in the completeness itdeserves. However, certain formulations of the particular, methods which make use of the serial-chain nature of manipulators are natural candidates for our study. 2

Forward DynamicsProblemGiven: Joint torques and links

geometry, mass, inertia,friction

Compute: Angular acceleration,velocity and position

of the links, (solve differential equations)

SolutionDynamic Equations - Newton-Euler method or Lagrangian Dynamics )()()()( ΘΘΘΘΘΘΘτ &&&& ,FG,VM +++=

)]()()()[(1 ΘΘΘΘΘτΘΘ &&&& ,FG,VM −−−= −

∫=∫= dtdt ΘΘΘΘ &&&& 3

Inverse DynamicsProblemGiven: Angular acceleration,

velocity and angels of the links in addition to the links geometry, mass, inertia, friction

Compute: Joint torques

SolutionDynamic Equations - Newton-Euler method or Lagrangian Dynamics

)()()()( ΘΘΘΘΘΘΘτ &&&& ,FG,VM +++=4

Acceleration of a rigid body

• Definitions of Linear and Angular acceleration

5

Linear Acceleration• The velocity of a vector as seen from frame {A} when

the origins are coincident:

6

Linear acceleration• Acceleration of as viewed from {A} when origins of {A}

and {B} coincide:

7

Linear acceleration• Finally, to generalize to the case in

which the origins are not coincident, we add one term which gives the linear acceleration of the origin of {B}, resulting in the final general

formula:

• For revolute joint link length usually is constant

8

Angular Acceleration• When {B} is rotation relative to {A} with

{C} is rotation relative to {B} with

What is ?

• We will use this result to calculate the angular accelerationof the links of a manipulator, with respect to the base frame {0}, and ith link {i} and i+1th link {i+1}

9

Newton’s Law

v)( F mdtd

=

Linear momentum rate of change of the linearmomentum is equal to the applied force

maF =

10

Angular Momentum

take the moment respect to o

Fm ×=× pvp &

v)(p N mdtd

=×

angular momentum applied moment

vvvpv)(p mmmdtd

=×+×=× & vp &m×

Fm =v&

=0

N=

N=

11

Linear & Angular Momentum

v)(p N mdtd

=×

Angular momentum

rate of change of the angularmomentum is equal to the applied moment

v)( F mdtd

=

Linear momentum

rate of change of the linearmomentum is equal to the applied force

12

Mass Distribution• Motion of a rigid body: translation and rotation. For

rotation about a single axis, one can use moment of inertia.

• For a rigid body that is free to move in three dimensions, there are infinitely many possible rotation axes.

• In case, of rotation about an arbitrary axis, we need a complete way of characterizing the mass distribution of a rigid body.

• Inertia tensor is used as a generalization of the scalar moment of inertia of an object.

13

Mass Distribution•We shall define a set of quantities that give information

about the distribution of mass of a rigid body relative toa reference frame.

Inertia tensor can be defined relative to any frame, but, we will always consider the case of an inertia tensor defined fora frame attached to the rigid body. 14

Angular Momentum• Rigid Body -Rotational Motion

Angular Momentum

ωρφ ]pp[v∫−= dvˆˆ

Inertial Tensor: I

Iω=φ15

Inertia Tensor

dvIIV

ρ∫= ]pp-p)[(p T3

T

222pp p zyxzyx

T ++=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

dvIV

ρ∫ −= ppT

T3

T pp-p)(p)pp( Iˆˆ =−

16

Linear & Angular Momentum

?

Iωmv == φϕ MomentumAngularentumLinear Mom

NIωdtdF mv

dtd

== )( )(

ation Euler Equ EquationNewton

NF == φϕ &&

NII =×+ ωωω& 17

Inertia Tensor

18

Inertia Tensor• For any frame {A}, the inertia tensor is given by

Mass moments of inertia

Mass products of inertia

19

Example 6.1•Find the inertia tensor for the rectangular body of uniform density ρ with respect to the frame {A}.

20

Example 6.1• Permuting the terms, we can obtain

• We next compute the mass products of inertia:

21

Example 6.1• Permuting the terms, we can obtain

22

Parallel Axis Theorem•Parallel-axis theorem relates the inertia tensor in a frame withorigin at the center of mass to the inertia tensor with respect toanother reference frame. The theorem can be stated as:

{C}

23

Example

{C}

24

Inertia Tensor

25

Inertia Tensor

26

Additional Facts About Inertial Tensors1. If two axes of the reference frame from a plane of symmetry

for the mass distribution of the body, the products of inertia having as an index the coordinate that is normal to the plane of symmetry will be zero.

2. Moments of inertia must always be positive. Products of inertia may have either sign.

3. The sum of the three moments of inertia is invariant underorientation change in the reference frame.

4. The eigenvalues of an inertia tensor are the principal moments for the body. The associated eigenvectors are the principle axes. Inertia tensor is always diagonal in the principle axes coordinate system.

• Most manipulators have links whose geometry and composition are somewhat complex, it is difficult to calculate in practice. A pragmatic option is actually to measure rather than to calculate the moment of inertia of each link by using a measuring device (e.g., an inertia pendulum). 27

Dynamics - Newton-Euler EquationsEular Equation• For a rotating rigid body, the moment that causes an angular acceleration is given by Euler’s equation

• For our robot manipulators, whose link moment of inertia is constant, this equation simplifies to

• The second term on the right will be non-zero when the link’s angular velocity vector is not coincident with the link’s principle axis of inertia.

dtIdN )(c ω

=

ωωω IIN cc ×+= &

Dynamics - Newton-Euler Equations

cv&

ω&

• To solve the Newton and Euler equations, we’ll need todevelop mathematical terms for:

– The linear acceleration of the center of mass

– The angular acceleration

– The Inertia tensor (moment of inertia)

cvmF &=

ωωω IIN cc ×+= &

Ic

Iterative Newton-Euler Equations

Iterative Newton-Euler Equations• The Newton and Euler equations are re-written for the forces and moments at each link:

• Where { } is a frame which has its origin at the link’s center of mass and has the same orientation as the link frame {i}.

iC

Propagation of Acceleration - AngularMatrix form (Revolute Joint)

• Converting from matrix to vector form gives the angular acceleration vector

Vector form(Revolute Joint)

• If joint i+1 is prismatic, the angular terms are zero and the above equation simplifies to:

Matrix form (Prismatic Joint)

Propagation of Acceleration - Linear• From the general equation, we can also get the solution for the

acceleration of the center of mass for link i. Appropriate frame substitution and elimination of prismatic terms gives we find:

• Frame {Ci} attached to each link with its origin located at thecenter of mass of the link, and with the same orientation as the link frame {i}

Newton-Euler Algorithm

34

Iterative Newton-Euler Equations -Solution Procedure: (Revolute Joint)

• Step 1 - Calculate the link velocities and accelerationsiteratively from the robot’s base to the end effector

• Step 2 - Write the Newton and Euler equations for each link.

35

Iterative Newton-Euler Equations -Solution Procedure: (Revolute Joint)

• Step 3 - Use the forces and torques generated by interacting with the environment (that is, tools, work stations, parts etc.) in calculating the joint torques from the end effector to the robot’s base.

36⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡⋅=

100

ii

i fτor

Iterative Newton-Euler Equations -Solution Procedure

• Error Checking - Check the units of each term in the resulting equations

• Gravity Effect - The effect of gravity can be included bysetting . This is the equivalent to saying that the base of the robot is accelerating upward at 1 g.

The result of this accelerating is the same as accelerating all the links individually as gravity does

37

Lagrangian Formulation of Manipulator Dynamics• Newton-Euler formulation: “force balance” approach

• Largrangian formulation: an “energy-based” approach

• Of course, for the same manipulator, both will give the sameequations of motion.

• Our statement of Largrangian dynamics will be brief andsomewhat specialized to case of a serial chain mechanicalmanipulator with rigid links.

ii

ici

ii

ii

ici

ii

cii

iii

IIN

vmF

ωωω ×+=

=

&

&

38

Lagrangian Formulation of Manipulator Dynamics• In our notation, the Largarngian of a manipulator is

where

is the total kinetic energy of the manipulator,

is the total potential energy of the manipulator.

• Kinetic energy is positive and scalar value. The total energy of the manipulator is the sum of the energy in the individuallinks.

39

Kinetic Energy

Total Kinetic Energy40

Mass Matrix of the Manipulator

• The kinetic energy of a manipulator is given by

M(Θ) is the n×n mass matrix of the manipulator• The total kinetic energy must always be positive. So the

manipulator mass matrix must be a so-called positivedefinite matrix.

• Positive definite matrices are those with the propertythat their quadratic form is always a positive scalar.

)(ΘM

41

Kinetic Energy in Different Frames

0 0_

1 12 2

T i T itran i i i i i i iK m V V m V V= =

0 0 0 0 0 0Ti i T T i i T ii i i i i i i i i iiV R V V V V R R V V V= ⇒ = =

0 0 0_

1 12 2

i T i Trot i i Ci i i Ci iK I Iω ω ω ω= =

RIRIR iCiT

iCiii

ii00000 == ωω

TiCiiCii

Tii

ii

i RRIIRR 0000000 === ωωω

42

Potential Energy

refuiu

- is a constant chosen so that the minimum value of is zero.

i

ii

refii

refcT

ii

ughm

uPgmu

+=

+−=

00

43

Lagrange Equations

τ

• The equation of motion for the manipulator are then given by

is the n×1 vector of actuator torques

is the total kinetic energy of the manipulator,u(Θ) is the total potential energy of the manipulator

44

Lagrange Equations Derivation

The principle of virtual work

Let Us consider the D’Alembert’s Principle in the generalized Coordinates:

45


46


47


ki i

k k i i

rU UQ Qr q q

∑∂∂ ∂′= − + = −

∂ ∂ ∂

0 1,2,ii i i

d K K U Q i ndt q q q∂ ∂ ∂ ′− + − = =∂ ∂ ∂

K&

( ) 1,2,ii i

d K K U Q i ndt q q∂ ∂ − ′− = =∂ ∂

K&

L K U= −

1,2,ii i

d L L Q i ndt q q

∂ ∂ ′− = =∂ ∂

K&

iQ′ are the generalized force not derivable from a potential function.

In general, the generalized force are partially derivable from apotential function, then we have

48

Five Steps to Obtain Lagrangian Formulation of Manipulator Dynamics

1. Define a set of generalized coordinates for i=1,2,3…N.These coordinates can be chosen arbitrarily as long asthey provide a set of independent variables that mapthe system in a 1-to-1 manner. The usual variable setfor serial manipulators is:

2. Define a set of generalized velocities for i=1,2,3…N

3. Define a set of generalized forces (and moments) fori=1,2,3…N

49


• The generalized forces must satisfy

WhereδΘi --is a small change in the generalized coordinate and δW --is the work done corresponding to that small change.

50


4. Write the equations describing the kinetic and potentialenergies as functions of the generalized coordinates aswell as the resulting Lagrangian.

• In case, there is spring involving:

51

52

2

0

21

Δ)(

XKu

XKXXKF

i Δ=

=−=

2

0

21

)(

θΔ

θΔθθ

Ku

KKT

i =

=−=

Springs

constants Spring

:LawsPhysical KθTKXF ==


5. The equations of motion are given by

• or,

• or in the component form as

53

Let L denote the Lagrangian given by

Example 6.5 Lagrangian Formulation of a PR ManipulatorKnown parameters:

X1

, d2,

• Generalized Variables: θ1,d2,• Generalized Forces: τ1, f2

2z

54

Example 6.5 Lagrangian Formulation of a PR Manipulator

• By definition, the inertia tensors:

• Joint Velocities and kinetic energy

55


• Kinetic energyof link i=1 56


• Kinetic energyof link i=2

• Total kineticenergy

?

ii

iCiT

ii I ωω

?

ΘΘ MT

21

= 57


• Kinetic energyof link i


?

ΘΘ MT

21

= 58


• Potential energy of links

is the maximum extension of joint 2.

• Total potential energy

• For i=1

• For i=2

59


•The partial derivatives of the energy of the manipulator

60

2221122221

211

22221

2111

122221

211

1

2)(

)()()(

ddmdmIIlm

dmIIlmdtd

dtddmIIlmk

dtd

yyzz

yyzzyyzz

&&&&

&&

θθ

θθθ

++++=

+++++++=∂∂

22222

)()( dmdmdtd

dk

dtd &&& ==

∂∂

)()( 2)( 122112122122221

2111 θθθτ cosgdmlmddmdmIIlm yyzz ++++++= &&&&


• The compact matrix form is

⎥⎦

⎤⎢⎣

⎡=

2

1

d&&&&

&& θΘ

61



[ ]2121 dM TT &&&&& θΘΘΘ ==

⎥⎦

⎤⎢⎣

⎡ +++=

2

22221

211

00

mdmIIlmM yyzz

M=?

62

Kinetic Energy

Total Kinetic Energy 63

Kinetic Energy

• Kinetic EnergyQuadratic Form ofGeneralized Velocities

64

Kinetic Energy

65

Kinetic Energy

⎥⎦

⎤⎢⎣

⎡∂∂

∂∂

∂∂

= 00021

LLi

cicicivi q

pqp

qpJ

]000[ 2211 LL ii zzzJi

εεεω =

⎩⎨⎧

=prismatic if0revolute if0

iiZB i

i]000[J 210

iLL iBBB=ω

66

Kinetic Energy & Mass Matrix

67

Mass Matrix –geometric approach

direct differentiation of the vectors:

• In frarme {0}, thesematrix are

• This yields

s

68

Mass Matrix

Joint 1 is revolute and joint 2 is prismatic:

The matrices and are given by

Finally,

⎥⎦

⎤⎢⎣

⎡==

000

)()( 22

1212

2212

12

12

12

1 yyc

TTc

T IJRIRJJIJ ωωωω

θω ω&Ji

ci =

)0( 2 =θ&

?⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

010100001

21R

69

Mass Matrix

[ ] 21

211

11

111111 210

000

21

21 θ

θθ &

&

& lml

lmvvm cTc =

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

222

21

222

211222111 2

1)(21

21

21 dmdmlmvvmvvm c

Tcc

Tc

&& ++=+ θ

[ ] ⎥⎦

⎤⎢⎣

⎡⎥⎦

⎤⎢⎣

⎡ +=

2

1

2

222

211

21 00

21

dmdmlmd

&

&&& θ

θ

70

[ ] )(2100

21

21 22

1222

2

12

2122222 ddmd

dddmvvm c

Tc

&&

&

&

&& +=⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡= θ

θθ

Mass Matrix

[ ] 21

211

11

111111 210

000

21

21 θ

θθ &

&

& lml

lmvvm cTc =

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

222

21

222

211222111 2

1)(21

21

21 dmdmlmvvmvvm c

Tcc

Tc

&& ++=+ θ

[ ] ⎥⎦

⎤⎢⎣

⎡⎥⎦

⎤⎢⎣

⎡ +=

2

1

2

222

211

21 00

21

dmdmlmd

&

&&& θ

θ

71

[ ] )(2100

21

21 22

1222

2

12

2122222 ddmd

dddmvvm c

Tc

&&

&

&

&& +=⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡= θ

θθ

Mass Matrix

[ ] 211

11

1

1

111

11

11

210

0

000000

0021 θ

θθωω &

&

&zz

zz

yy

xx

cT I

II

II =

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

[ ] 2121

2

2

2

122

22

22

21

0

0

000000

0021 θθθωω &&&

yy

zz

yy

xx

cT I

II

II =

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

212

2112

22

22

21

11

11

1

21

21

21

21 θθωωωω &&

yyzzcT

cT IIII +=+

[ ] ⎥⎦

⎤⎢⎣

⎡⎥⎦

⎤⎢⎣

⎡ +=

2

12121 00

0d

IId yyzz

&

&&& θ

θ

72

Lagrangian Formulation –2R Robot Example

⎥⎦

⎤⎢⎣

⎡++

=12211

122112

0

slsLclcL

Pg

gg

⎥⎦

⎤⎢⎣

⎡=

11

111

0

sLcL

Pg

gg

73


and Let :1 Step 2211 θΘθΘ ==

⎥⎦

⎤⎢⎣

⎡=

11

111

0

sLcL

Pg

gg θ&& ⎥

⎦

⎤⎢⎣

⎡−=

11

111

0

cLsL

Pg

gg

and Let :2 Step 2211 θΘθΘ &&&& ==

ii uesforce/torq extenal Let :3 Step ττ =

Step 4:

• Kinetic Energy: iii

ciTi

ici

Tciii Ivvmk ωω

21

21

+=

21

21 :1iFor 2

112

12

111 θθ && ILmk g +==74


Geometric Approach---direct projection

⎥⎦

⎤⎢⎣

⎡++

=12211

122112

0

sLsLcLcL

Pg

gg

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

+=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

212

2 00

00

θθωω

&&

)(2)( 212

12212

212

22

121 θθθθθθ &&&&&& ++++= cLLLLvv ggci

Tci

• The derivative squared gives

)(21)](2)( [

21

:2iFor

221221

21221

221

22

21

2122 θθθθθθθθ &&&&&&&& ++++++=

=

IcLLLLmk gg

• To find the velocity of the center of mass of link2, first considerits position given by

75


1111 sgLmu g=

)( 1211122 sLsLgmu g+=

• Potential Energy: ∑=∑= ii mghuu

• For i=1

• For i=2

• Lagrangian:2121 uukkL −−+=

76


• step 5 Solving

• or,

• or in the component form as

77


)g(g

)2(

])([

])c2([

122112111

22212212

222212

22

122212

22121111

cLcLmcLm

sLLm

IcLLLm

ILLLLmILm

gg

g

gg

ggg

+++

+−

+++

+++++=

θθθ

θ

θτ

&&&

&&

&&

1222

222212

222

22

122212

222

g

][

])c([

cLm

sLLm

ILm

ILLLm

g

g

g

gg

+

+

++

++=

θ

θ

θτ

&

&&

&&

? 1θ&∂∂K

dtd

78

Inclusion of non-rigid body effects• It is important to realize that the dynamic equations we

have derived do not encompass all the effects acting on amanipulator. They include just those forces which arise fromrigid body mechanics. The most important source of forcesthat are not included is friction.

• All mechanisms are, of course, affected by frictional forces.In present day manipulators in which significant gearing istypical, the forces due to friction can actually be quite large--- perhaps equalling 25% of the torque required to move themanipulator in typical situations.

• In order to make dynamic equations reflect the reality of thephysical device, it is important to model (at leastapproximately) these forces of friction.

• For simplicity, two types of frictional model are given in thissection: viscous friction and Coulomb friction

79

Inclusion of non-rigid body effects

)(θτ &sgncfrication =

• Viscous friction: the torque due to friction is proportional to the velocity of joint motion.

Thus we have

where v is a viscous friction constant.• Coulomb friction: is constant except for a sign

dependence on the joint velocity:

where c is a Coulomb friction constant, and,

c1 is the static coefficient and larger that c2, the dynamics coefficient.

θτ &vfrication =

⎩⎨⎧

<−≥

=⎩⎨⎧

≠=

=0101

)( and 00

2

1

θθ

θθθ

&

&&

&

&sgn

cc

c

80

Inclusion of non-rigid body effects

θθτ && vsgncfrication += )(• A reasonable model is combining both:

- It turns out that in many manipulator joints, friction also displays a dependence on the joint position.

- A major cause of this effect might be gears which are not perfectly round—their eccentricity would cause friction to change according to joint position.

• So a fairly complex friction model would be have the form

• There are other effects which are also neglected in this model.• However, these effects are extremely difficult to model, such as

the bending effects of the non-rigid link, and are beyond thescope of this course.

Hence,

),( θθτ &ffrication =

),()()()( θθΘΘΘΘΘτ &&&& FG,VM +++=

81

Dynamic simulation

)].,()()()[(1 θθΘΘΘτΘΘ &&&& FG,VM −−−= −

• Simulate the motion of a manipulator requires solving the dynamic equation for acceleration:

• Several known numerical integration techniques can be used to integrate the acceleration to compute future positions and velocities.

Give initial conditions on the motion of the manipulator, usually in the form:

• Numerically integrate forward in time by step size Δt.

.0 ,(0) 0 == ΘΘΘ &

82

Dynamic simulation• Euler integration (the simplest integration scheme):

Starting with t=0, iteratively compute:

ending with for a given input function The selection of Δt :• It should be sufficiently small that breaking continuous time

into these small increments is a reasonable approximation.• It should be sufficiently large that an excessive amountof computer time is not required to compute a simulation.

83

intro to robotics

Documents