Control of industrial robots Robot dynamics - Intranet DEIBhome.deib.polimi.it/rocco/cir/Robot dynamics.pdf · Control of industrial robots –Robot dynamics – Paolo Rocco. Introduction

Control of industrial robots

Robot dynamics

Prof. Paolo Rocco ([email protected])Politecnico di MilanoDipartimento di Elettronica, Informazione e Bioingegneria

Control of industrial robots – Robot dynamics – Paolo Rocco

Introduction

With these slides we will derive the dynamic model of the manipulator

The dynamic model accounts for the relation between the sources of motion (forces and moments) and the resulting motion (positions and velocities)

u(t)

q(t)

Systematic methods exist to derive the dynamic model of the manipulators, which will be reviewed here, along with the main properties of such model


Kinetic energy

Consider a point with mass m, whose position is described by vector p with respect to a xyz frame.

We define kinetic energy of the point the quantity:

P

x

y

z

Opp TmT21

=

Consider now a rigid body, with mass m, volume V and density ρ. The kinetic energy is defined with the integral:

∫ ρ=V

T dVT pp 21

P

x

y

z

O

dV

Similarly, for a system of points:

∑=

=n

ii

TiimT

121 pp


Potential energy

A system of position forces (i.e. depending only on the positions of the points of application) is said to be conservative is the work made by each force does not depend on the trajectory followed by the point of application, but only on the initial and final positions.In this case the elementary work coincides with the differential, with changed sign, of a function called potential energy:

dUdW −=

An example of a system of conservative forces is the gravitational force.For a point mass we have the potential energy:

pgTmU 0−=

For a rigid body:

lT

V

T mdVU pgpg 00 −=ρ−= ∫

where g0 is the gravity acceleration vector.

where pl is the position of the center of mass.


Systems of rigid bodies

Let us consider a system of r rigid bodies (as for example, the links of a robot). If all these bodies are free to move in space, the motion of the system is, at each time instant, described by means of 6r coordinates x.

Suppose now that limitations exist in the motion of the bodies of the system (as for example the presence of a joint, which eliminates five out of the six relative degrees of freedom between two consecutive links).A constraint thus exists on the motion of the bodies, which we will express with the relation:

( ) 0=xh

Such a constraint is said to be holonomic (it depends only on position coordinates, not velocities) and stationary (it does not change with time).


Free coordinates

( ) 0=xh

If the constraints h are composed of s scalar equations and they are all continuously differentiable, it is possible, by means of the constraints, to eliminate s coordinates from the system equations.

The remaining n = 6r – s coordinates are called free, or Lagrangian, or natural, or generalized coordinates. n is the number of degrees of freedom of the mechanical system.

For example in a robot with 6 joints, out of the 36 original coordinates, 30 are eliminated by virtue of the constraints imposed by the 6 joints. The remaining 6 are the Lagrangian coordinates (typically the joint variables used in the kinematic model).


Lagrange’s equations

( ) ( ) ( )qqqqq UTL −= ,,

∑=

ξ=n

iiidqdW

1

Given a system of rigid bodies, whose positions and orientations can be expressed by means of n generalized coordinates qi, we define Lagrangian of the mechanical system the quantity:

where T and U are the kinetic and the potential energies, respectively. Let then ξi be the generalized forces associated with the generalized coordinates qi. The elementary work performed by the forces acting on the system can be expressed as:

It can be proven that the dynamics of the system of bodies is expressed by the following Lagrange’s equations:

niqL

qL

dtd

iii

,,1,

=ξ=∂∂

−∂∂


An example

Let us consider a system composed of a motor rigidly connected to a load, subjected to the gravitational force. Let: Im and Ii the moments of inertia of the

motor and the load with respect to the motor spinning axis

m the mass of the load l the distance of the center of mass of the

load from the axis of the motor.

Kinetic energy of the motor: 2

21

ϑ= mm IT

mg

ϑ

τ

l x

y

C

Kinetic energy of the load: 2

21ϑ= ITc


Gravitational potential energy:

Lagrangian: ( ) ϑ−ϑ+=−+= sin21 2 lmgIIUTTL mcm

Lagrange’s equations:

mg

ϑ

τ

l [ ] ϑ=

ϑϑ

−−=−= sinsincos

00 lll

mggmmU lT pg

( )( ) τ=ϑ+ϑ+⇒τ=ϑ∂∂

−ϑ∂∂ coslmgII

dtdLL

dtd

m

Then:

( ) τ=ϑ+ϑ+ coslmgII m This equation can be easily interpreted as the

equilibrium of moments around the rotation axis.

An example


Kinetic energy of a link

The contribution of kinetic energy of a single link can be computed with the following integral:

∫ ρ=iV

iT

ii dVT **

21 pp

generic point along the link*ip

Position of the center of mass: ∫ ρ=i

i Vi

il dV

m*1 pp

Velocity of the generic point: ( ) iiliili iirSprpp ωω +=×+= *

where: ( )

ωω−ω−ωωω−

=0

00

ixiy

ixiz

iyiz

iωS (skew-symmetric matrix)


Translational contribution:

iii

ii lT

liV

lT

l mdV pppp 21

21

=ρ∫Mutual contribution:

( ) ( ) ( ) 0* =ρ−=ρ ∫∫i

iii

i Vlii

Tl

Vii

Tl dVdV ppSprSp ωω

Rotational contribution:

( ) ( ) ( ) ( ) iiTii

Vii

TTi

Viii

TTi

ii

dVdV ωωωωωω IrSrSrSSr21

21

21

=

ρ=ρ ∫∫ ( ) ( ) iiii ωω rSrS −=

Note:

Kinetic energy of a link

Then: iiTil

Tlii ii

mT ωω Ipp21

21

+=

velocity of center of mass

angular velocity of the link

König Theorem


Inertia tensor

iTi

ii ωω R=

(symmetric matrix)

( )( )

( )

−−−

=

ρ+

ρ−ρ+

ρ−ρ−ρ+

=

∫∫∫∫∫∫

izz

iyziyy

ixzixyixx

iyix

iziyizix

izixiyixiziy

i

IIIIII

dVrr

dVrrdVrr

dVrrdVrrdVrr

***

**

*22

22

22

I

The inertia tensor, if expressed in the base frame, depends on the robot configuration. If the angular velocity is expressed with reference to a frame rigidly attached to the link (for example the DH frame):

the inertia tensor referred to this frame is a constant matrix. Moreover:Ti

iiii RIRI =

We define inertia tensor the matrix:


Sum of the contributions

Let us sum the translational and rotational contributions:

iTi

iii

Til

Tlii ii

mT ωω RIRpp21

21

+=

Linear velocity:

( ) ( ) ( )qJjjp iiii

lPi

lPi

lPl qq =++= 11

( ) ( ) ( )[ ]00 iii lPi

lP

lP jjJ 1=

Angular velocity:

( ) ( ) ( )qJjj iii lOi

lOi

lOi qq =++= 11ω ( ) ( ) ( )[ ]00 iii l

Oil

Ol

O jjJ 1=


Computation of the Jacobians

( ) ( ) ( )[ ]00 iii lPi

lP

lP jjJ 1=

( ) ( ) ( )[ ]00 iii lOi

lO

lO jjJ 1=

( )

( ) ( )

−×

=

−

−−

−

joint rotational

jointprismatic

1

11

1

j

jlj

j

lOj

lPj

i

i

i

zppz

z

jj 0

Columns of the Jacobian:


Inertia matrix

Substituting expressions for the linear and angular velocities:

( ) ( ) ( ) ( )qJRIRJqqJJq iiii lO

Ti

iii

TlO

TlP

TlP

Tii mT

21

21

+=

By summing the contributions of all the links we obtain the kinetic energy of the whole arm:

( ) ( )qqBqq Tn

i

n

jjiij qqbT

21

21

1 1

== ∑∑= =

where:

( ) ( ) ( ) ( ) ( )( )∑=

+=n

i

lO

Ti

iii

TlO

lP

TlPi

iiiim1

JRIRJJJqB

is the inertia matrix of the manipulator.

symmetric

positive definite

depends on q


Potential energy

The potential energy of a rigid link is related just to the gravitational force:

ilT

iV

iT

i mdVU pgpg 0*

0 −=ρ−= ∫where g0 is the gravity acceleration vector expressed in the base frame.

The potential energy of the whole manipulator is then the sum of the single contributions:

∑∑==

−==n

il

Ti

n

ii i

mUU1

01

pg


Equations of motion

The Lagrangian of the manipulator is:

( ) ( ) ( ) ( ) ( )∑∑∑== =

+=−=n

ili

Ti

n

i

n

jjiij mqqbUTL

10

1 121,, qpgqqqqqq

If we differentiate the Lagrangian:

Furthermore:

( )( ) ( )qqjgp

g i

n

j

lP

Tj

n

j i

ljTj

igm

qm

qU j

i=−=

∂

∂−=

∂∂ ∑∑

== 10

10

( ) ( ) ( )

( ) ( )∑ ∑∑

∑∑∑

= ==

===

∂

∂+=

=+=

=

∂∂

=

∂∂

n

jj

n

kk

k

ijn

jjij

n

jj

ijn

jjij

n

jjij

ii

qqq

bqb

qdt

dbqbqb

dtd

qT

dtd

qL

dtd

1 11

111

qq

qqq

( )∑∑= = ∂

∂=

∂∂ n

j

n

kjk

i

jk

iqq

qb

qT

1 121

q


From Lagrange’s equations we obtain:

where:

( ) ( ) ( ) nigqqhqb ii

n

j

n

kjkijk

n

jjij ,,1

1 11

=ξ=++∑∑∑= ==

qqq

i

jk

k

ijijk q

bqb

h∂

∂−

∂

∂=

21

Acceleration terms:

bii : inertia moment as “seen” from the axis of joint i

bij : effect of the acceleration of joint j on the joint i

Centrifugal and Coriolis terms:

hijjqj2 : centrifugal effect induced at joint i by the velocity of joint j

hijkqjqk : Coriolis effect induced at joint i by the vel. of joints j e k

.

. .

gravitational term, depends only on the joint positions

hiii = 0

Equations of motion


Non conservative forces

Besides the gravitational conservative forces, other forces act on the manipulator:

actuation torques

viscous friction torques

static friction torques

τ

qF v−

( )qqf ,s−

diagonal matrix of viscous friction coefficientsvF

( )qqf ,s function that models the static friction at the joint


Complete dynamic model

In vector form the dynamic model can be expressed as follows:

( ) ( ) ( ) ( ) τ=++++ qgqqfqFqqqCqqB ,, sv

where C is a suitable n×n matrix, whose elements satisfy the equation:

∑∑∑= ==

=n

j

n

kjkijk

n

jjij qqhqc

1 11

C is not symmetric in general


Computation of the elements of C

The choice of matrix C is not unique. One possible choice is the following one:

∑∑∑∑

∑∑∑∑∑

= == =

= == ==

∂

∂−

∂∂

+∂

∂=

=

∂

∂−

∂

∂==

n

j

n

kjk

i

jk

j

ikn

j

n

kjk

k

ij

n

j

n

kjk

i

jk

k

ijn

j

n

kjkijk

n

jjij

qqqb

qbqq

qb

qqqb

qb

qqhqc

1 11 1

1 11 11

21

21

21

The generic element of C is:

∑=

=n

kkijkij qcc

1

∂

∂−

∂∂

+∂

∂=

i

jk

j

ik

k

ijijk q

bqb

qb

c21 Christoffel symbols of the

first kindwhere:


Skew-symmetry of matrix B−2C

The previous choice of matrix C allows to prove an important property of the dynamic model of the manipulator. Matrix:

∑∑∑===

∂

∂−

∂∂

+=

∂

∂−

∂∂

+∂

∂=

n

kk

i

jk

j

ikij

n

kk

i

jk

j

ikn

kk

k

ijij q

qb

qbbq

qb

qbq

qb

c111 2

121

21

21

.

( ) ( ) ( )qqCqBqqN ,2, −=

is skew-symmetric:

( ) wwqqNw ∀= ,0, T

In fact:

∑=

∂∂

−∂

∂=−=

n

kk

j

ik

i

jkijijij q

qb

qb

cbn1

2 jiij nn −= (skew-symmetric)


Energy conservation

The equation:

( ) 0, =qqqNq T

(particular case of the previous one) is valid whatever the choice of matrix C is. From the energy conservation principle, the derivative of the kinetic energy equals the power generated by all the forces acting at the joint of the manipulator:

( )( ) ( ) ( )( )qgqqfqFqqqBq −−−= ,21

svTT

dtd

τ

Taking the derivative at the left hand side and using the equation of the model:

( )( ) ( ) ( ) ( ) ( )( )

( ) ( )( )qgqqfqFq

qqqCqBqqqBqqqBqqqBq

−−−+

−=+=

,

,221

21

21

svT

TTTT

dtd

τ from which the equation follows.


Linearity in the dynamical parameters

If we assume a simplified expression for the static friction function:

( ) ( )qFqqf sgn, ss =

π: vector of p constant parameters

Y: n×p matrix, function of joint positions, velocities and accelerations (regression matrix)

( )πτ qqqY ,,=

it is possible to prove that the dynamic model of the manipulator is linear with respect to a suitable set of dynamic parameters (masses, moments of inertia)We can then write:


Two-link Cartesian manipulator

Consider a two-link Cartesian manipulator, characterized by link masses m1 e m2.The vector of generalized coordinates is:

The Jacobians needed for the computation of the inertia matrix are:

=

2

1

dd

q

( ) ( )[ ] [ ]

( ) ( ) ( )[ ] [ ]

===

===

010010

010000

10

0

2

2

2

1

2

1

1

1

zzjjJ

zjJ

lP

lP

lP

lP

lP 00

while there are no contributions to the angular velocities.

x0

z0

z1

m1

m2

d1

d2


Computing the inertia matrix with the general formula, we obtain:

As B is constant, C=0, i.e. there are no centrifugal and Coriolis terms.Then, since:

the vector of the gravitational terms is:

−=

g00

0g

( ) ( ) ( ) ( )

+=+=

2

2121 0

02211

mmm

mm lP

TlP

lP

TlP JJJJB

( )

+=

021 gmm

g



If there are no friction torques and no forces at the end-effector:

( ) ( )222

121121

fdm

fgmmdmm

=

=+++

f1 e f2: forces which act along the generalized coordinates



Two-link planar manipulator

Let us consider a two-link planar manipulator: masses: m1 and m2 lengths: a1 and a2 distances of the centers of mass from the joint axes: l1

and l2 moments of inertia around axes passing through the

centers of mass and parallel to z0: I1 and I2

ϑϑ

=2

1q

m1, I1

m2, I2

a1

a2

ϑ1

ϑ2

l1

l2

x0

y0

The Jacobians needed for the computation of the inertia matrix depend on the following vectors:

=

=

++

=

=

0,

000

,0

,0

11

11

1012211

12211

11

11

21saca

ssacca

sc

ll pppp ll

ll

Generalized coordinates:

==

100

10 zz



Link 1

( ) ( )[ ] ( )[ ]

−=−×==

0000

11

11

00 111

1 cs

ll

Pl

P ll

00 ppzjJ

( ) ( )[ ] [ ]

===

010000

01

1

1 00 zjJ lO

lO

Link 2

( ) ( ) ( )[ ] ( ) ( )[ ]

+

−−−=−×−×==

0012212211

12212211

1100 2222

21

2 cccasssa

lll

Pl

Pl

P llll

ppzppzjjJ

( ) ( ) ( )[ ] [ ]

===

110000

102

2

2

1

2 zzjjJ lO

lO

lO


Taking into account that the angular velocity vectors ω1 and ω2 are aligned with z0it is not necessary to compute rotation matrices Ri , so that the computation of the inertia matrix gives:

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )( )

( )( )

222222

22212222112

222122

2121

21111

2221

12112121

2

22112211

Imb

Icambb

IcaamImb

bbbb

IImm lO

TlO

lO

TlO

lP

TlP

lP

TlP

+=

++==

+++++=

ϑ

ϑϑ=+++=

l

ll

lll

JJJJJJJJqB


depends on ϑ2

depends on ϑ2

constant!


From the inertia matrix it is simple to compute the Christoffel symbols:

021

0212121

21

021

2

22222

1

22221212

22122

11

1

21211

22121

22

2

12122

22122

11121112

1

11111

=∂∂

=

=∂∂

==

−==∂∂

−∂∂

=

=−=∂∂

−∂∂

=

=−=∂∂

==

=∂∂

=

qbc

qbcc

hsamqb

qbc

hsamqb

qbc

hsamqbcc

qbc

l

l

l 2212 samh l−=



The expression of matrix C is then:

( ) ( ) ( )

ϑϑ+ϑ−ϑ−

=

ϑ−ϑ+ϑϑ

=00

,12212

21221222212

1

212

samsamsam

hhh

lllqqC

We can verify that matrix N is skew-symmetric:

( ) ( ) ( ) ( )

( )qqN

qqCqBqqN

,02

20

02

02,2,

21

21

1

212

2

22

T

hhhh

hhh

hhh

−=

ϑ+ϑϑ−ϑ−

=

=

ϑ−ϑ+ϑϑ

−

ϑϑϑ

=−=

Furthermore, since g0 = [0 −g 0]T, the vector of the gravitational terms is:

( )

++=

1222

122211211

cgmcgmgcamm

lll

g



τi : torques applied at the joints

( )( ) ( )( )

( ) 1122211211

222212212212

2222122212221

22

2121

211

2

2

τ=+++

+ϑ−ϑϑ−

+ϑ+++ϑ+++++

cgmgcammsamsam

IcamIcaamIm

llll

lllll

( )( ) ( )21222

212212

2222212221

222

τ=+ϑ+

+ϑ++ϑ++

gcmsam

ImIcam

ll

lll

Without friction at the joints, the equations of motion are:



By simple inspection, we can obtain the dynamical parameters with respect to which the model is linear, i.e. those parameters for which we can write:

( )πτ qqqY ,,=

We have:

[ ]T54321 πππππ=π

22225

224

23

21112

111

l

l

l

l

mI

mm

mI

m

+=π

=π

=π

+=π

=π



2125

12212112124

2115

122221212122112114

1112113

112

111

22

ϑ+ϑ=

+ϑ+ϑ=

ϑ+ϑ=

+ϑ−ϑϑ−ϑ+ϑ=

+ϑ=

ϑ=

=

y

gcsacay

y

gcsasacacay

gcaay

y

gcy

( )

=

2524

1514131211

000,,

yyyyyyy

qqqY

The coefficients of Y depend on ϑ1, ϑ2, their first and second derivatives, g and a1.



Identification of dynamical parameters

The linearity of the dynamic model with respect to the dynamical parameters:

( )πτ qqqY ,,=

allows to setup a procedure for the experimental identification of the same parameters, which are usually unknown or uncertain.

Suitable motion trajectories must be executed, along which the joint positions q are recorded, the velocities q are measured or obtained by numerical differentiation, and the accelerations q are obtained with filtered (also non-causal) differentiation.Also the torques τ are measured, directly (with suitable sensors) or indirectly, from the measurements of currents in the motors.

Suppose to have the measurements (direct or indirect ones) of all the variables for the time instants t1, …, tN.

...


With N measurement sets:

( )

( )

( )

( )ππ

τ

ττ Y

Y

Y=

=

=

NN t

t

t

t

11

Solving with a least-squares technique:

( ) τπ TT YYY1−

= Left pseudo-inverse matrix of Y−

Only the elements of π for which the corresponding column is different from zero can be identified.

Some parameters are identifiable only in combination with other ones. The trajectories to be used must be sufficiently rich (good conditioning of matrix

YTY): they need to involve all components in the dynamic model− −

Identification of dynamical parameters


Newton-Euler formulation

An alternative way to formulate the dynamic model of the manipulator is the Newton-Euler method.It is based on balances of forces and moments acting on the single link, due to the interactions with the nearby links in the kinematic chain.

We obtain a system of equations that might be solved in a recursive way, propagating the velocities and accelerations from the base to the end effector, while the forces and moments in the opposite way:

Recursion makes Newton-Euler algorithm computationally efficient.

velo

cities

acce

lerati

ons

forc

esm

omen

ts


Definition of the parameters

Let us consider the generic link i of the kinematic chain:

mi and Ii mass and inertia tensor of the linkri-1,Ci vector from the origin of frame (i-1) to the center of mass Ci

ri,Ci vector from the origin of frame i to the center of mass Ci

ri-1, i vector from the origin of frame (i-1) to the origin of frame i

We define the following parameters:

This picture is taken from the textbook:

B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo: Robotics: Modelling, Planning and Control, 3rd Ed. Springer, 2009


Definition of the variables

pCi linear velocity of the center of mass Ci

pi linear velocity of the origin of frame iωi angular velocity of the linkpCi linear acceleration of the center of mass Ci

pi linear acceleration of the origin of frame iωi angular acceleration of the linkg0 gravity acceleration

.

..

.

.

..

fi force exerted by link i-1 on link i−fi+1 force exerted by link i+1 on link iµi moment exerted by link i-1 on link i with respect to the origin of frame i-1

−µi+1 moment exerted by link i+1 on link i with respect to the origin of frame i

All vectors are expressed in the base frame.



Newton’s equation (translational motion of the center of mass)

iCiiii mm pgff =+− + 01

Euler’s equation (rotational motion)

( ) ( )iiiiiiiCiiiCiii dtd

iiωωωωµµ IIIrfrf ×+==×−−×+ ++− ,11,1

Generalized force at joint i:

it can be proven gyroscopic effect

=τ−

−

joint rotationaljointprismatic

1

1

iTi

iTi

i zzf

µ


Accelerations of a link

Propagation of the velocities:

ϑ+=

−−

−


11

1

iii

ii zω

ωω

×+×++

=−−

−−−


,11

,111

iiii

iiiiiii

drp

rzppω

ω

Propagation of the accelerations:

×ϑ+ϑ+=

−−−−

−


1111

1

iiiiii

ii zz ωω

ωω

( )( )

××+×+××+×+×++

=−−−

−−−−−

joint rotationaljointprismatic 2

,1,11

,1,1111

iiiiiiii

iiiiiiiiiiiiii

ddrrp

rrzzppωωω

ωωωω

( ) mass of center,, iii CiiiCiiiC rrpp ××+×+= ωωω

Note: derivative of a vector ai attached to the moving frame i

( ) ( )

( )( ) ( ) ( ) ( )ttttS

tdtdt

dtd

iiiiii

iiii

aaR

aRa

×==

==

ωω


Recursive algorithm

A forward recursion of velocities and accelerations is made: initial conditions on ω0, p0-g0, ω0

computation of ωi, ωi, pi, pCi

.. .

. .. ..

A backward recursion of forces and moments is made: terminal conditions on fn+1 and µn+1 computations:

iCiii m pff += +1

( ) ( )iiiiiCiiiCiiiii iiωωωµµ IIrfrrf ×++×+++×−= ++− ,11,,1

The generalized force at joint i is computed:

+ϑ+++

=τ−

−


1

1

siiviiTi

siiviiTi

i fFfdF

zzf

µ

friction contributions

velo

cities

acce

lerati

ons

forc

esm

omen

ts


Local reference frames

Up to now we have supposed that all the vectors are referred to the base frame.

It is more convenient to express the vectors with respect to the current frame on link i. In this way, vectors ri-1,i e ri,Ci and the inertia tensor Ii are constant, which makes the algorithm computationally more effcient.

The equations are modified in some terms (we need to multiply the vectors by suitable rotation matrices) but nothing changes in the nature of the method.



Let us consider again a two-link planar manipulator with rotational joints, whose model has been already derived with the Euler-Lagrange method: masses: m1 and m2 lengths: a1 and a2 distances of the centers of mass from the joint

axes: l1 and l2 moments of inertia around axes passing through

the centers of mass and parallel to z0: I1 and I2

Initial conditions for the forward recursion of velocities and accelerations;

[ ] 0===− 00

00

00

00 ,00 ωω Tggp

m1, I1

m2, I2

a1

a2

ϑ1

ϑ2

l1

l2

x0

y0

Initial conditions for the backward recursion of forces and moments:

00 == 33

33 µ,f


Definition of vectors and matrices

Let us refer all the quantities to the current frame on the link. We derive these constant vectors:

The rotation matrices are:

=

−=

=

−=

00,

00,

00,

00

222,1

222,2

111,0

111,1 21

aaaa

CC rrrrll

=

−=

−=

100010001

,10000

,10000

2322

221211

1101 RRR cs

sccssc


Forward recursion: link 1

( )

ϑ=ϑ+=

1

01001

11 0

0

zR ωω T

( )

ϑ=×ϑ+ϑ+=

1

00101001

11 0

0

zzR ωωω T

( )

+ϑ+ϑ−

=××+×+=0

111

1211

11,0

11

11

11,0

110

01

11 gca

gsaT

rrpRp ωωω

( )

+ϑ+ϑ−

=××+×+=0

111

1211

1,1

11

11

1,1

11

11

1111

gcgs

CCC

ll

rrpp ωωω


( )

ϑ+ϑ=ϑ+=

21

0211

12

22 0

0

zR ωω T

( )

ϑ+ϑ=×ϑ+ϑ+=

21

011202

11

12

22 0

0

zzR ωωω T

( )( )

( )

+ϑ+ϑ+ϑ+ϑ+ϑ+ϑ−ϑ−ϑ

=××+×+=0

122121212121

122

2122121121

22,1

22

22

22,1

22

11

12

22 gcsaaca

gsacasaT

rrpRp ωωω

( )( )

( )

+ϑ+ϑ+ϑ+ϑ+ϑ+ϑ−ϑ−ϑ

=××+×+=0

122121212121

122

2122121121

2,2

22

22

2,2

22

22

22 22

gcsacagscasa

CCC

ll

rrpp ωωω

Forward recursion: link 2


( )( )( )

+ϑ+ϑ+ϑ+ϑ

+ϑ+ϑ−ϑ−ϑ

==+=0

1221212121212

122

21221211212

22

22

33

23

22 22

gcsacam

gscasam

mm CC

l

l

ppfRf

( ) ( )

( )( )

+ϑ+ϑ+ϑ+ϑ+∗∗

=

=×++×+++×−=

12222122121221221

2222

22

22

22

22

22

2,2

33

23

33

23

2,2

22,1

22

22 22

gcmsamcammI

CC

llll

ωωωµµ IIrfRRrrf

( )( ) ( ) 12222122122

22221221

2222

012

222

gcmsammIcamI

TT

lllll +ϑ+ϑ++ϑ++=

==τ

zRµ

Backward recursion: link 2

(identical to the equation obtained with Euler-Lagrange method)


( ) ( ) ( )( ) ( ) ( ) ( )

++ϑ+ϑ−ϑ+ϑ+ϑ+

++ϑ+ϑ−ϑ−ϑ−ϑ+ϑ−

=+=0

1212

212222122211211

1212

212222112

211121222

12

22

12

11 1

gcmmsmcmammgsmmcmammsm

m C

llllll

pfRf

( ) ( )

( ) ( )( )( ) ( )

+++ϑ+ϑ−ϑ+

+ϑ+ϑ+++ϑ+++∗∗

=

=×++×+++×−=

1222112112

212212212212

212222212212212

211

2121

11

11

11

11

11

1,1

22

12

22

12

1,1

11,0

11

11 11

gcmgcammsamsam

mcamIcammamI

CC

llll

llll

ωωωµµ IIrfRRrrf

( )( ) ( )( )( ) 122211211

222212212212

222122221221

22

2122

2111

001

111

2

2

gcmgcammsamsam

camIcaamImI

TT

llll

lllll

+++ϑ−ϑϑ−

ϑ+++ϑ+++++=

==τ

zRµ

Backward recursion: link 1

(identical to the equation obtained with Euler-Lagrange method)


Euler-Lagrange vs. Newton-Euler

Euler-Lagrange formulation

it is systematic and easy to understand it returns the equations of motion in an analytic and compact form, separating

the inertia matrix, the Coriolis and centrifugal terms, the gravitational terms. All these elements are useful for the design of a model based controller

it lends itself to the introduction into the model of more complex effects (like joint or link deformation)


it is a computationally efficient recursive method


Direct and inverse dynamics

Direct dynamicsFor given joint torques τ(t), determine the joint accelerations q(t) and, if initial positions q(t0) and velocities q(t0) are known, the positions q(t) and the velocities q(t).

( ) ( ) ( ) ( ) τ=++++ qqfqFqgqqqCqqB ,, sv

Inverse dynamicsFor given accelerations q(t), velocities q(t) and positions q(t) determine the joint torques τ(t) needed for motion generation.

...

.

...

Problem whose solution is useful in order to compute the simulation model of the robot manipulator

It can be solved both with Euler-Lagrange and with Newton-Euler approaches

Problem whose solution is useful for trajectory planning and model based control

It can be efficiently solved with the Newton-Euler formulation


Computation of direct and inverse dynamics

( ) ( ) τ=+ qqnqqB ,

As for the computation of the direct dynamics, let us rewrite the dynamic model of the manipulator in these terms:

( ) ( ) ( ) ( )qqfqFqgqqqCqqn ,,, sv +++=

where:

Computation of the inverse dynamics can be easily done both with the Euler-Lagrange method and with the Newton-Euler one.

We thus have to numerically integrate the explicit system of differential equations:

( ) ( )( )qqnqBq ,1 −= − τ

where all the elements needed to build the system are directly computed by the Euler-Lagrange method.


With the current values of q and q, a first iteration of the script is performed, setting q = 0. In this way the torques τ computed by the method directly return the vector n.

How to compute the direct dynamics with the Newton-Euler method?

Then we set g0 = 0 inside the script (in order to eliminate the gravitational effects) and q = 0 (in order to eliminate Coriolis, centrifugal and friction effects). niterations of the script are performed, with qi = 1 and qj = 0, j≠i. This way matrix B is formed column by column and all elements to form the system of equations are available.

.

...

..

..

Computation of direct and inverse dynamics

Newton-Euler script (Matlab, C, …):

( )qqq ,,ΝΕ=τ

Documents

Control of industrial robots Robot dynamics - Intranet DEIBhome.deib.polimi.it/rocco/cir/Robot dynamics.pdf · Control of industrial robots –Robot dynamics – Paolo Rocco. Introduction