FIR 05 Dynamics

Dynamic model of robot manipulators
Claudio Melchiorri
Dipartimento di Ingegneria dellEnergia Elettrica e dellInformazione (DEI)
Universit`a di Bologna
email: claudio.melchiorri@unibo.it
C. Melchiorri (DEI) Dynamic Model 1 / 65
Summary
1
Dynamic models
Introduction
Euler-Lagrange model
Dynamic models Introduction
Dynamic model of manipulators
Robot Dynamics = Study of the relation between the applied forces/torques
and the resulting motion of an industrial manipulator.
Similarly to kinematics, also for the dynamics it is possible to dene two models:
Direct model: once the forces/torques applied to the joints, as well as the
joint positions and velocities are known, compute the joint accelerations
q = f (q, q, )
and then
q =
_
qdt, q =
_
qdt
Inverse model: once the joint accelerations, velocities and positions are
known, compute the corresponding forces/torques
= f
1
( q, q, q) = g( q, q, q)
Normally, a manipulator is composed by an open kinematic chain, and its dynamic
model is aected by several drawbacks:
low rigidity (elasticity in the structure and in the joints)
potentially unknown parameters (dimensions, inertia, mass,. . . )
dynamic coupling among links.
Other non linear eects are usually introduced by the actuation system:
friction
dead zones
. . .
In any case, in the derivation of the dynamic model, an ideal case of a series of
connected rigid bodies is made.
Two problems may be dened for the study of the dynamic model:
Direct dynamic model: computation of the time evolution of q(t) (and
then of q(t), q(t)), given the vector of generalized forces (torques and/or
forces) (t) applied to the joints and, in case, the external forces applied to
the end-eector, and the initial conditions q(t = t
0
), q(t = t
0
).
(t) = q(t) ( q(t), q(t) )
Inverse dynamic problem: computation of the vector (t) necessary to
obtain a desired trajectory q(t), q(t), q(t), once the forces applied of the
end-eector are known.
q(t), q(t), q(t) = (t)
There are several reasons for studying the dynamics of a manipulator:
simulation: test desired motions without resorting to real experimentation
analysis and synthesis of suitable control algorithms
analysis of the structural properties of the manipulator since the design phase.
Two approaches for the denition of the dynamic model:
EULER-LAGRANGE approach.
First approach to be developed. The dynamic model obtained in this manner is simpler and
more intuitive, and also more suitable to understand the eects of changes in the
mechanical parameters. The links are considered altogether, and the model is obtained
analytically. Drawbacks: the model is obtained starting from the kinetic and potential
energies (non intuitive); the model is not computationally ecient.
NEWTON-EULER approach.
Based on a computationally ecient recursive technique that exploits the serial structure of
an industrial manipulator. On the other hand, the mathematical model is not expressed in
closed form.
Obviously, the two techniques are equivalent (provide the same results).
Dynamic models Euler-Lagrange model
Generalized variables, or Lagrange coordinates:
Independent variables used to describe the position of rigid bodies in the space.
For the same physical system, more choices for the Lagrangian coordinates are
usually possible.
In robotics = joint variables q
1
, q
2
, . . . q
n
.
From physics, we know that it is possible to dene:
1
The Kinetic Energy function K(q, q)
2
The Potential Energy function P(q)
and therefore the Lagrangian function L(q, q) = K(q, q) P(q)
The Euler-Lagrange equations are
i
=
d
dt
_
L
q
i
_
L
q
i
i = 1, . . . , n
being
i
the non-conservative (external or dissipative) generalized forces
performing work on q
i
. In robotics, we consider:
i
joint actuator torque
_
J
T
F
c
_
i
term due to external (contact) forces
d
ii
q
i
joint friction torque
Therefore:
i
=
i
+
_
J
T
F
c
i
d
ii
q
i
.
Since the potential energy does not depend on the velocity, the Euler-Lagrange
equations can be rewritten as
i
=
d
dt
_
K
q
i
_
K
q
i
+
P
q
i
i = 1, . . . , n (1)
This formulation is more convenient since in robotics it is possible to compute
quite easily the terms K and P from the geometric properties of the manipulator.
Then, by applying (1), the dynamic model is obtained.
Note that
K =
n
i =1
K
i
P =
n
i =1
P
i
Computation of the Kinetic Energy. For a rigid body B:
The mass can be computed by
m =
_
B
(x, y, z) dx dy dz
where (x, y, z) is the mass density (assumed constant): (x, y, z) = .
The center of mass (CoM) is
p
C
=
1
m
_
B
p(x, y, z) dx dy dz =
1
m
_
B
p(x, y, z) dm
The kinetic energy results as
K =
1
2
_
B
v
T
(x, y, z)v(x, y, z) dx dy dz
=
1
2
_
B
v
T
(x, y, z)v(x, y, z) dm
Let assume that v
C
and , i.e. the trans-
lational and rotational velocities of the
center of mass, are known with respect
to an inertial frame F
0
.
The velocity of a generic point p
of the body is
v = v
C
+ (p
p
C
) = v
C
+ r (2)
The velocity expressed in a frame F xed to the rigid body may be computed by
introducing the rotation matrix R betweeen F and F
0
R
T
v = R
T
(v
C
+ r) = R
T
v
C
+ (R
T
) (R
T
r)
Therefore
v
= v
C
+

Since the product r in (2) can be expressed as S() r, we have:
K =
1
2
_
B
v
T
(x, y, z)v(x, y, z) dm
=
1
2
_
B
(v
C
+ Sr)
T
(v
C
+ Sr) dm
=
1
2
_
B
v
T
C
v
C
dm +
1
2
_
B
r
T
S
T
Sr dm +
_
B
v
T
C
Sr dm
=
1
2
_
B
v
T
C
v
C
dm +
1
2
_
B
r
T
S
T
Sr dm
As a matter of fact, from the denition of CoM (
_
B
r dm =
_
B
(p
C
p)dm = 0):
_
B
v
T
C
Sr dm = v
T
C
S
_
B
r dm = 0
In conclusion
K =
1
2
_
B
v
T
C
v
C
dm +
1
2
_
B
r
T
S
T
Sr dm
The rst term depends on the linear velocity v
C
of the center of mass
1
2
_
B
v
T
C
v
C
dm =
1
2
m v
T
C
v
C
For the second term, considering that a
T
b = Tr (a b
T
) and the particular
structure of matrix S, we have
1
2
_
B
r
T
S
T
Sr dm =
1
2
_
B
Tr (Sr r
T
S
T
) dm =
1
2
Tr (S
_
B
rr
T
dm S
T
)
=
1
2
Tr (S J S
T
) =
1
2
T
I I : body inertia matrix
Also this term depends on the velocity of the center of mass (in this case ).
Matrix J, Euler matrix, and matrix I, body inertia matrix, are symmetric, and have
the following general expressions:
J =
_
_
_
r
2
x
dm
_
r
x
r
y
dm
_
r
x
r
z
dm
_
r
x
r
y
dm
_
r
2
y
dm
_
r
y
r
z
dm
_
r
x
r
z
dm
_
r
y
r
z
dm
_
r
2
z
dm
_
_
I =
_
_
_
(r
2
y
+ r
2
z
) dm
_
r
x
r
y
dm
_
r
x
r
z
dm
_
r
x
r
y
dm
_
(r
2
x
+ r
2
z
) dm
_
r
y
r
z
dm
_
r
x
r
z
dm
_
r
y
r
z
dm
_
(r
2
x
+ r
2
y
) dm
_
_
The elements of both matrices J ed I depend on vector r, i.e. on the position of
the generic point of the i-th link with respect to its center of mass, dened in the
base frame.
Since the position of the i-th link depends on the conguration of the
manipulator, matrices J and I are in general functions of the joint variables q!
Examples of body inertia matrices
Homogeneous bodies of mass m, with axes of symmetry.
Parallelepiped with sides a (length/height), b, c (base)
I =
I
xx
I
yy
I
zz
1
12
m(b
2
+ c
2
)
1
12
m(a
2
+ c
2
)
1
12
m(a
2
+ b
2
)
Empty cylinder with length h, and external/internal radii a, b

I =
1
2
m(a
2
+ b
2
)
1
12
m
3(a
2
+ b
2
) + h
2
I
zz
,
I
zz
= I
yy
I
zz
= I
zz
+ m
h
2
2
single axis translation theorem
x
0
y
0
z
0
a
b
c
x
0
y
0
z
0
z
0
a
b
h
h/2
Steiner theorem:
changes of body inertia due
to translation p of the frame
of computation:
I = I
c
+m(p
T
p E
33
pp
T
)
I
c
body inertia matrix wrt the
center of mass
E
33
(3 3) identity matrix
In conclusion, the kinetic energy of a rigid body is dened as (Konig Theorem)
K =
1
2
m v
T
C
v
C
+
1
2
T
I (3)
Both terms depend only on the velocity of the rigid body.
The rst term, being related to the magnitude of a vector (v
C
= v
T
C
v
C
), is invariant with
respect to the reference frame used to express the velocity:
v
T
C
v
C
= (Rv
C
)
T
(Rv
C
) = v
T
C
(R
T
R)v
C
, R
This property holds also for the second term: the product
T
I is invariant with respect to the
reference frame. As a matter of fact, the body inertia matrix is transformed according to the
following relation:
I
= R I R
T
Then:
T
I =
T
I
= (R )
T
(RIR
T
)(R) =
T
(R
T
R)I(R
T
R).
Therefore, being (3) invariant with respect to the reference frame, F can be chosen in order to
simplify the computations required for the evaluation of K.
Since the kinetic energy K
i
of the generic i-th link is invariant with respect to the
reference frame (used to express v
C
, , I), it is convenient to chose a frame
F
i
xed to the link itself, with origin in the center of mass.
In this manner matrix I is constant and simple to be computed on the basis of the
geometric properties of the link.
On the other hand, normally the rotational velocity is dened in the base frame
F
0
, and therefore it is needed to transform it according to R
T
, being R the
rotation matrix between F
i
and F
0
(known from the kinematic model of the
manipulator).
In conclusion, the kinetic energy of a manipulator can be determined when, for
each link, the following quantities are known:
the link mass m
i
;
the inertia matrix I
i
, computed wrt a frame F
i
xed to the center of mass in
which it has a constant expression

I
i
;
the linear velocity v
Ci
of the center of mass, and the rotational velocity
i
of
the link (both expressed in F
0
);
the rotation matrix R
i
between the frame xed to the link and F
0
.
The kinetic energy K
i
of the i-th link has the form:
K
i
=
1
2
m
i
v
T
Ci
v
Ci
+
1
2
T
i
R
i
I
i
R
T
i

i
It is now necessary to compute the linear and rotational velocities (v
Ci
and
i
) as
functions of the Lagrangian coordinates (i.e. the joint variables q).
The end-eector velocity may be computed as a function of the joint velocities q
1
, . . . , q
n
through the Jacobian matrix J. The same methodology can be used to compute the
velocity of a generic point of the manipulator, and in particular the velocity v
Ci
= p
Ci
of
the center of mass p
Ci
, that results function of the joint velocities q
1
, . . . , q
i
only:
p
Ci
= J
i
v1
q
1
+ J
i
v2
q
2
+ . . . +J
i
vi
q
i
= J
i
v
q
i
= J
i
1
q
1
+J
i
2
q
2
+ . . . + J
i
i
q
i
= J
i
q
where
J
i
v
=
_
J
i
v1
. . . J
i
vi
0 . . . 0
_
J
i
=
_
J
i
1
. . . J
i
i
0 . . . 0
_
with
_
J
i
vj
J
i
j
_
=
_
z
j 1
(p
Ci
p
j 1
)
z
j 1
_
rotational joint
_
J
i
vj
J
i
j
_
=
_
z
j 1
0
_
linear joint
being p
j 1
the position of the origin of the frame associated to the j-th link.
In conclusion, for a n dof manipulator we have:
K =
1
2
n
i =1
m
i
v
T
Ci
v
Ci
+
1
2
n
i =1
T
i
R
i
I
i
R
T
i

=
1
2
q
T
n
i =1
_
m
i
J
i T
v
(q)J
i
v
(q) + J
i T
(q)R
i
I
i
R
T
i
J
i
(q)
_
q
=
1
2
q
T
M(q) q
=
1
2
n
i =1
n
j=1
M
ij
(q) q
i
q
j
where M(q) is a n n, symmetric and positive denite matrix, function of the
manipulator conguration q.
Matrix M(q) is called the Inertia Matrix of the manipulator.
Computation of the Potential Energy. For rigid bodies, the only potential
energy taken into account in the dynamics is due to the gravitational eld g. For
the generic i-th link
P
i
=
_
L
i
g
T
p dm = g
T
_
L
i
p dm = g
T
p
Ci
m
i
The potential energy does not depend on the joint velocities q, and may be
expressed as a function of the position of the centers of mass. For the whole
manipulator:
P =
n
i =1
g
T
p
Ci
m
i
In case of exible link, one should consider also terms due to elastic forces.
K e P are computed (once m
i
and

I are known) with a procedure similar to the one
adopted for the forward kinematic model:
K matrices J
i
e R
i
, P p
Ci
position of the centers of mass
Once K and P are known, it is possible to compute the dynamic model of the
manipulator. The dynamics is expressed by
k
=
d
dt
_
L
q
k
_
L
q
k
k = 1, . . . , n
The Lagrangian function is
L = K P =
1
2
n
i =1
n
j =1
M
ij
q
i
q
j

n
i =1
g
T
p
Ci
m
i
Then
L
q
k
=
K
q
k
=
n
j =1
M
kj
q
j
and
d
dt
L
q
k
=
n
j =1
M
kj
q
j
+
n
j =1
d M
kj
dt
q
j
=
n
j =1
M
kj
q
j
+
n
i =1
n
j =1
M
kj
q
i
q
i
q
j
Moreover
L
q
k
=
1
2
n
i =1
n
j =1
M
ij
q
k
q
i
q
j

P
q
k
The Lagrangian equations have the following formulation
n
j =1
M
kj
q
j
+
n
i =1
n
j =1
_
M
kj
q
i
1
2
M
ij
q
k
_
q
i
q
j
+
P
q
k
=
k
k = 1, . . . , n
By dening the term h
kji
(q) as
h
kji
(q) =
M
kj
(q)
q
i
1
2
M
ij
(q)
q
k
and g
k
(q) as
g
k
(q) =
P(q)
q
k
the following equations are nally obtained
n
j=1
M
kj
(q) q
j
+
n
i =1
n
j=1
h
kji
(q) q
i
q
j
+ g
k
(q) =
k
k = 1, . . . , n
The elements M
kj
(q), h
ijk
(q), g
k
(q) are function of the joint position only, and
therefore their computation is relatively simple once the manipulators conguration is
known. They have the following physical meaning:
For the acceleration terms:
M
kk
is the moment of inertia about the k-th joint axis, in a given conguration and
considering blocked all the other joints
M
kj
is the inertia coupling, accounting the eect of acceleration of joint j on joint k
For the quadratic velocity terms:
h
kjj
q
2
j
represents the centrifugal eect induced on joint k by the velocity of joint j
(notice that h
kkk
=
M
kk
q
k
= 0)
h
kji
q
i
q
j
represents the Coriolis eect induced on joint k by the velocities of joints i
and j
For the conguration-dependent terms:
g
k
represents the torque generated on joint k by the gravity force acting on the
manipulator in the current conguration.
Recalling that the non-conservative forces
k
are in general composed by
k
joint actuator torque
_
J
T
F
c
_
k
external (contact) forces
d
kk
q
k
joint friction torque
the Lagrangian equations
n
j=1
M
kj
(q) q
j
+
n
i =1
n
j=1
h
kji
(q) q
i
q
j
+ g
k
(q) =
k
k = 1, . . . , n
can be written in matrix form as
M(q) q + C(q, q) q + D q + g(q) = + J
T
(q)F
c
This matrix equation is known as the dynamic model of the manipulator.
Euler-Lagrange model - Some considerations
The product C(q, q) q =
n
i =1
n
j=1
h
kji
(q) q
i
q
j
is a (n 1) vector v whose
elements are quadratic functions of the joint velocities q
j
.
The k-th element v
k
of this vector is:
v
k
=
n
j=1
C
kj
q
j
where the elements C
kj
are computed as
C
kj
=
n
i =1
c
ijk
q
i
with
c
ijk
=
1
2
_
M
kj
q
i
+
M
ki
q
j
M
ij
q
k
_
Christoel Symbols (4)
The elements of matrix C(q, q) are computed as follows. From
n
i =1
n
j =1
h
kji
q
i
q
j
=
n
i =1
n
j =1
_
M
kj
q
i
1
2
M
ij
q
k
_
q
i
q
j
by exchanging the sum (i , j ) and exploiting the symmetry one obtains
n
i =1
n
j =1
M
kj
q
i
=
1
2
n
i =1
n
j =1
_
M
kj
q
i
+
M
ki
q
j
_
and then
n
i =1
n
j =1
_
M
kj
q
i
1
2
M
ij
q
k
_
=
n
i =1
n
j =1
1
2
_
M
kj
q
i
+
M
ki
q
j
M
ij
q
k
_
=
n
i =1
n
j =1
c
ijk
where c
ijk
=
1
2
_
M
kj
q
i
+
M
ki
q
j
M
ij
q
k
_
are the so-called Christoel Symbols.
Since matrix M(q) is symmetric, for a given k then c
ijk
= c
jik
.
The elements of matrix C(q, q)are then computed as
[C(q, q)]
k,j
=
n
i =1
c
ijk
q
i
(5)
This is not the only possible expression for matrix C(q, q). In general, any matrix
such that
n
j=1
c
ij
q
j
=
n
j=1
n
k=1
h
ijk
q
k
q
j
can be considered. The choice (4) is preferred since in this case the following
property is veried.
Property. Matrix N(q, q), dened as
N(q, q) =

M(q, q) 2C(q, q) (6)
in which the elements of C(q, q) are dened as
c
ijk
=
1
2
_
M
kj
q
i
+
M
ki
q
j
M
ij
q
k
_
[C(q, q)]
k,j
=
n
i =1
c
ijk
q
i
results skew-symmetric, i.e. n
kj
= n
jk
, n
kk
= 0.
In fact, by considering the generic element n
kj
, one obtains
n
kj
=
d M
kj
dt
2[C]
kj
=
n
i =1
_
M
kj
q
i
(
M
kj
q
i
+
M
ki
q
j
M
ij
q
k
)
_
q
i
=
n
i =1
_
M
ij
q
k
M
ki
q
j
_
q
i
from which it follows (if indices k and j are exchanged, because of the symmetry
of M(q)) that n
kj
= n
jk
.
Since matrix N is skew-symmetrix, then
x
T
N(q, q) x = 0, x
The condition
x
T
N(q, q) x = 0, x
holds since N(q, q) is skew-symmetric, due to the particular choice of the elements of
matrix C(q, q). On the other hand, the condition
q
T
N(q, q) q = 0
holds for any choice of matrix C(q, q) (from the energy conservation principle).
The evolution over time of the kinetic energy K must be equal to the work generated by
the forces acting at joints:
d K
dt
=
1
2
d
dt
_
q
T
M q
_
= q
T
( D q g(q) J
T
F)
The rst element is (from the dynamic model M q = C q D q g(q) + J
T
F):
1
2
d
dt
_
q
T
M q
_
=
1
2
q
T
M q + q
T
M q =
1
2
q
T
M q + q
T
(C q D q g(q) + J
T
F)
Then
1
2
q
T
M q + q
T
(C q D q g(q) + J
T
F) = q
T
( D q g(q) J
T
F)
from which
1
2
q
T
M q = q
T
C q = q
T
(

M2C) q = 0
This equation holds q and without any assumption on matrix C( q, q) (it holds
also if C is not based on the Cristoel symbols).
In deriving the dynamic model, the actuation system has not been taken into
account. This is normally composed by:
motors
reduction gears
trasmission system.
The actuation system has several eects on the dynamics:
if motors are installed on the links, then masses and inertia are changed
it introduces its own dynamics (electromechanical, pneumatic, hydraulic, ...)
that may be non negligible (e.g. in case of lightweight manipulators)
it introduces additional nonlinear eects such as backslash, friction, elasticity,
...
Notice that these eects could be considered by introducing suitable terms in the
dynamic model derived on the basis of the Euler-Lagrangian formulation.
Example - 1
Dynamic model of a pendulum (one dof manipulator).
Consider
joint variable,
joint torque,
m mass,
L distance between center of mass and joint,
d viscous friction coecient,
I inertia seen at the rotation axis.
Kinetic energy: K =
1
2
I

2
Potenzial energy: P = mgL(1 cos )
Lagrangian function L: L =
1
2
I

2
mgL(1 cos )
Example - 1
Lagrangian function: L =
1
2
I

2
mgL(1 cos )
from which
L
= I

,
d
dt
L
= I

,
L
= mgL sin
The generalized Lagrangian force in this case must account for the torque applied
to the joint and for the friction eect:
= d

From the general expression

=
d
dt
_
L
we have the following second order dierential equation

I

+ d

+ mgL sin =
Example - 2
Dynamic model of a 2 dof manipulator. Consider:
i
i-th joint variable;
m
i
i-th link mass ;
I
i
i-th link inertia, about an axis through
the CoM and parallel to z;
a
i
i-th link length;
a
Ci
distance between joint i and the CoM
of the i-th link;
i
torque on joint i;
g gravity force along y;
P
i
, K
i
potential and kinetic energy of the
i-th link.
The dynamic equations will be obtained in two manners:
a) with the classic approach, deriving the Lagrangian function (based on the kinetic
and potential energy K, P)
b) exploiting the particular structure of a manipulator (Jacobian, ...).
Example - 2 (classic approach)
We chose as generalized coordinates the joint variables q
1
=
1
; q
2
=
2
. The
kinetic and potential energies K
i
and P
i
are:
link 1:
K
1
=
1
2
m
1
a
2
C1

1
2
+
1
2
I
1

1
2
, P
1
= m
1
ga
C1
S
1
link 2: in this case, the position and velocity of the CoM are
_
_
_
p
C2x
= a
1
C
1
+ a
C2
C
12
p
C2y
= a
1
S
1
+ a
C2
S
12
_
_
_
p
C2x
= a
1
S
1

1
a
C2
S
12
(
1
+

2
)
p
C2y
= a
1
C
1

1
+ a
C2
C
12
(
1
+

2
)
then
K
2
=
1
2
m
2
p
T
C2
p
C2
+
1
2
I
2
(
1
+

2
)
2
, P
2
= m
2
g(a
1
S
1
+ a
C2
S
12
)
where
p
T
C2
p
C2
= a
2
1

1
2
+ a
2
C2
(

1
+

2
)
2
+ 2a
1
a
C2
C
2
(
2
1
+

2
)
Example - 2 (classic approach)
Therefore, L = K
1
+ K
2
P
1
P
2
and
1
= [m
1
a
2
C1
+
I
1
+ m
2
(a
2
1
+ a
2
C2
+ 2a
1
a
C2
C
2
) +
I
2
]
1
+
+[m
2
(a
2
C2
+ a
1
a
C2
C
2
) +
I
2
]
2
m
2
a
1
a
C2
S
2
(2
2
+

2
2
) +
+m
1
ga
C1
C
1
+ m
2
g(a
1
C
1
+ a
C2
C
12
)
2
= [m
2
(a
2
C2
+ a
1
a
C2
C
2
) +
I
2
]
1
+ (m
2
a
2
C2
+
I
2
)
2
+
m
2
a
1
a
C2
S
2

2
1
+ m
2
ga
C2
C
12
Example - 2 (robotics approach)
The structural properties of the manipulator are exploited for the computation of
the kinematic and potential energies. For the computation of the velocities of the
CoM, one obtains:
J
1
v
=
_
_
a
C1
S
1
0
a
C1
C
1
0
0 0
_
_
J
2
v
=
_
_
a
1
S
1
a
C2
S
12
a
C2
S
12
a
1
C
1
+ a
C2
C
12
a
C2
C
12
0 0
_
_
and
J
1
=
_
_
0 0
0 0
1 0
_
_
J
2
=
_
_
0 0
0 0
1 1
_
_
In this particular case, the frames associated to link 1 and 2 have z axes parallel to
the same axis of F
0
, and therefore it is not necessary to consider the rotation
matrices R
1
, R
2
( =
z
).
The kinetic energy is computed as
K =
1
2
q
T
_
m
1
J
1 T
v
J
1
v
+ m
2
J
2 T
v
J
2
v
+ J
1 T
I
1
J
1
+ J
2 T
I
2
J
2
_
q
being
J
1 T
I
1
J
1
+ J
1 T
I
2
J
2
=
_

I
1
+
I
2

I
2
I
2

I
2
_
The elements of the inertia matrix M(q) are
M
11
= m
1
a
2
C1
+ m
2
(a
2
1
+ a
2
C2
+ 2a
1
a
C2
C
2
) +
I
1
+
I
2
M
12
= m
2
(a
2
C2
+ a
1
a
C2
C
2
) +
I
2
M
22
= m
2
a
2
C2
+
I
2
From M
11
= m
1
a
2
C1
+ m
2
(a
2
1
+ a
2
C2
+ 2a
1
a
C2
C
2
) +
I
1
+
I
2
M
12
= m
2
(a
2
C2
+ a
1
a
C2
C
2
) +
I
2
M
22
= m
2
a
2
C2
+
I
2
The Christoel symbols c
ijk
=
1
2
_
M
kj
q
i
+
M
ki
q
j
M
ij
q
k
_
are
c
111
=
1
2
M
11
q
1
= 0
c
121
= c
211
=
1
2
M
11
q
2
= m
2
a
1
a
C2
S
2
= h
c
221
=
M
12
q
2
1
2
M
22
q
1
= h
c
112
=
M
21
q
1
1
2
M
11
q
2
= h
c
122
= c
212
=
M
22
q
1
= 0
c
222
=
M
22
q
2
= 0
= Matrix C(q, q) is
C(q, q) =
_
h

2
h(
1
+

2
)
h

1
0
_
Matrix N(q) is
N(q) =

M(q) 2C(q, q)
=
_
2h

2
h

2
h

2
0
_
2
_
h

2
h(

1
+

2
)
h

1
0
_
=
_
0 2h
1
h

2
2h

1
+ h

2
0
_
As expected, it results skew-symmetric.
For the potential energy, we have:
P
1
= m
1
ga
C1
S
1
P
2
= m
2
g(a
1
S
1
+ a
C2
S
12
)
Then
P = P
1
+ P
2
= (m
1
a
C1
+ m
2
a
1
)gS
1
+ m
2
ga
C2
S
12
g
1
=
P
1
= (m
1
a
C1
+ m
2
a
1
)gC
1
+ m
2
ga
C2
C
12
g
2
=
P
2
= m
2
ga
C2
C
12
Summarizing, from M(q) q + C(q, q) q + D q + g(q) = we have
M
11
1
+ M
12
2
+ c
121

2
+ c
211

1
+ c
221

2
2
+ g
1
=
1
M
21
1
+ M
22
2
+ c
112

2
1
+ g
2
=
2
or
[m
1
a
2
C1
+ m
2
(a
2
1
+ a
2
C2
+ 2a
1
a
C2
C
2
) +
I
1
+
I
2
]
1
+ [m
2
(a
2
C2
+ a
1
a
C2
C
2
) +
I
2
]
2
m
2
a
1
a
C2
S
2

2
2
2m
2
a
1
a
C2
S
2

2
+(m
1
a
C1
+ m
2
a
1
)gC
1
+ m
2
ga
C2
C
12
=
1
[m
2
(a
2
C2
+ a
1
a
C2
C
2
) +
I
2
]
1
+ [m
2
a
2
C2
+
I
2
]
2
+m
2
a
1
a
C2
S
2

2
1
+m
2
ga
C2
C
12
=
2
= Same result!
Properties of the Euler-Lagrangian dynamic model
The Euler-Lagrange dynamic model is characterized by some structural properties,
concerning in particular:
1
The inertia matrix M(q);
2
The vector c(q, q) = C(q, q)q;
3
The vectors g(q) and D q;
4
Linearity with respect to the geometric/mechanical parameters.
1. Properties of matrix M(q)
1
M(q) IR
nn
is symmetric, positive denite and depends on the manipulator
conguration q
2
M(q) is upper and lower bounded:
1
I M(q)
2
I
that is
x
T
(M(q)
1
I)x 0 x
T
(
2
I M(q))x 0
3
also M
1
(q) is bounded
1
2
I M
1
(q)
1
1
I
4
in case of revolute joints, then
1
,
2
are constant (not function of q) since the
elements of M(q) are functions of sin(q
i
) or cos(q
i
)
5
in case of prismatic joints,
1
,
2
may result scalar functions of q
6
since M(q) is bounded, then
M
1
||M(q)|| M
2
for some properly dened norm (1, 2, p, )
2. Properties of vector c(q, q) = C(q, q) q
1
C(q, q) q is a quadratic function of q
2
the generic k-th element of vector c(q, q) = C(q, q) q can also be witten as
c
k
(q, q) = q
T
S
k
(q) q
S
k
(q) =
1
2
_
m
k
q
+
_
m
k
q
_
T
M
q
k
_
m
k
= k-th col. of M
3
it results that
||C(q, q) q|| v
b
|| q||
2
4
in case of rotative joints, then v
b
is constant (not function of q) since we have only
transcendental functions (sin(q
i
) or cos(q
i
))
5
in case of prismatic joints, then v
b
may result a scalar function of q
6
for any choice of C(q, q), then matrix N(q, q) =

M(q, q) 2C(q, q) veries:
q
T
N(q, q) q = 0
7
with a proper choice of the elements of C(q, q) (Christoel symbols), matrix
N(q, q) =

M(q, q) 2C(q, q) is skew-symmetric, i.e.
x
T
N(q, q)x = 0 x
3. Properties of vectors g(q) and D q
1
the friction term D q is bounded:
||D q|| d
max
|| q||
2
the gravity term g(q) is bounded
||g(q)|| g
b
(q)
3
in case of revolute joints, g
b
is constant (does not depend on q) since q
i
depends
on sinusoidal functions (sin(q
i
) or cos(q
i
))
4
in case of prismatic joints, then g
b
may result function of q.
4. Linearity properties (in the geometrical/mechanical parameters)
The dynamic model of a manipulator:
in general is a non linear function of q, q, q and with dynamic coupling eects
among the joints,
is a linear function of the geometrical/mechanical parameters of the links (i.e.
masses, inertia, friction, ...)
M(q) q + C(q, q) q + D q +g(q) =
Y(q, q, q) =
Example
Properties of the dynamic model of a
2 dof manipulator.
Neglecting friction eects we have:
M(q) =
_
m
1
a
2
C1
+ m
2
(a
2
1
+ a
2
C2
+ 2a
1
a
C2
C
2
) +
I
1
+
I
2
m
2
(a
2
C2
+ a
1
a
C2
C
2
) +
I
2
m
2
(a
2
C2
+ a
1
a
C2
C
2
) +
I
2
m
2
a
2
C2
+
I
2
_
C(q, q) q = m
2
a
1
a
C2
S
2
_
2
2
+

2
2
2
1
_
, g(q) =
_
(m
1
a
C1
+ m
2
a
1
)gC
1
+ m
2
ga
C2
C
12
m
2
ga
C2
C
12
_
Consider (for the sake of simplicity) the 1-norm || ||
1
, and
1
,
2
[/2, /2].
Example
1. Bounds on the inertia matrix.
The scalar quantities
1
,
2
can be dened as the minimum and maximum
eigenvalues (
min
,
max
) of M(q), q. Computationally, it is easier to dene the
scalars M
1
, M
2
.
The 1norm of M(q) is alway dened on the basis of the rst column:
||M(q)||
1
= |m
1
a
2
C1
+m
2
(a
2
1
+a
2
C2
+2a
1
a
C2
C
2
) +
I
1
+
I
2
| +|m
2
(a
2
C2
+a
1
a
C2
C
2
) +
I
2
|
which is bounded by
M
1
= m
1
a
2
C1
+ m
2
(a
2
1
+ 2a
2
C2
) +
I
1
+ 2
I
2
M
2
= m
1
a
2
C1
+ m
2
(a
2
1
+ 2a
2
C2
+ 3a
1
a
C2
) +
I
1
+ 2
I
2
Example
2. Bounds on vector C(q, q) q
It results that
||C(q, q) q||
1
= |m
2
a
1
a
C2
S
2
(2
2
+

2
2
)| +|m
2
a
1
a
C2
S
2

2
1
|
m
2
a
1
a
C2
|2
2
+

2
2
+

2
1
|
m
2
a
1
a
C2
(|
1
| +|
2
|)
2
= v
b
|| q||
2
Moreover N(q, q) =

M(q, q) 2C(q, q), (h = m
2
a
1
a
C2
S
2
):
N(q) =

M(q) 2C(q, q)
=
_
2h

2
h

2
h

2
0
_
2
_
h

2
h(
1
+

2
)
h

1
0
_
=
_
0 2h

1
h

2
2h

1
+ h

2
0
_
Example
3. Bounds on the gravity vector g(q)
||g(q)||
1
= |(m
1
a
C1
+ m
2
a
1
)gC
1
+ m
2
ga
C2
C
12
| +|m
2
ga
C2
C
12
|
(m
1
a
C1
+ m
2
a
1
)g + 2m
2
ga
C2
= g
b
Notice that if one of the joints is prismatic (and therefore a
i
, a
Ci
may vary in
time), then v
b
, g
b
are functions of q.
Example
Plots of the eigenvalues of M(q) for the 2 dof planar manipulator
0
50
100
150
200
250
300
350
0
100
200
300
400
0
20
40
60
80
100
120
140
160
180
l
m
in = 6.2399
l
m
ax = 161.2601
q1 [deg]
Andamento autovalori minimo e massimo di M(q)
q2 [deg]
min
= 6.2399
max
= 161.2601
In this case, the eigenvalues depend only on q
2
! These results have been obtained
with:
m
1
= m
2
= 50kg; I
1
= I
2
= 10; a
1
= a
2
= 1; a
c1
= a
c2
= 0.5
One obtains
M
1
= 117, 5, M
2
= 192.5 e M
1
= 192.5, M
2
= 161.26, M
= 192.5
Example
3. Linearity of the dynamic model Y(q, q, q) =
[m
1
a
2
C1
+ m
2
(a
2
1
+ a
2
C2
+ 2a
1
a
C2
C
2
) +
I
1
+
I
2
]
1
+ [m
2
(a
2
C2
+ a
1
a
2
C2
C
2
) +
I
2
]
2
m
2
a
1
a
C2
S
2

2
2
2m
2
a
1
a
C2
S
2

2
+(m
1
a
C1
+ m
2
a
1
)gC
1
+ m
2
ga
C2
C
12
=
1
[m
2
(a
2
C2
+ a
1
a
C2
C
2
) +
I
2
]
1
+ [m
2
a
2
C2
+
I
2
]
2
+m
2
a
1
a
C2
S
2

2
1
+m
2
ga
C2
C
12
=
2
By inspection, one can dene the parameters vector
= [
1
,
2
,
3
,
4
,
5
]
T
with
1
= m
1
a
C1
,
2
= m
1
a
2
C1
+
I
1
,
3
= m
2
,
4
= m
2
a
C2
,
5
= m
2
a
2
C2
+
I
2
Example
Then
Y(q, q, q) =
_
y
11
y
12
y
13
y
14
y
15
0 0 0 y
24
y
25
_
with
y
11
= gC
1
y
12
=

1
y
13
= a
2
1
1
+ a
1
gC
1
y
14
= 2a
1
C
2
1
+ a
1
C
2
2
2a
1
S
2

2
a
1
S
2

2
1
+ gC
12
y
15
=

1
+

2
y
24
= a
1
C
2
1
+ a
1
S
2

2
1
+ gC
12
y
25
=

1
+

2
The elements y
ij
depend on q, q, q, g and on a
1
.
Example
Identication of the dynamic parameters
From:
Y(q, q, q) =
by measuring along a proper trajectory the quantities q, q, q, one obtains:

0
= Y
0
Y
T
0

0
= Y
T
0
Y
0
= (Y
T
0
Y
0
)
1
Y
T
0

0
= Y
+
0
0
Being Y
+
0
= (Y
T
0
Y
0
)
1
Y
T
0
the (left) pseudo-inverse of Y
0
.
Note that some of the parameters may result non identiable!
A Simulink dynamic model
Demux
.
1/s
q2
1/s
dq2
Demux
Demux2
Mux
Mux2 ddq
dq
Mux
Mux
Mux
Mux3
1/s
q1
1/s
dq1
MATLAB
Function
dir2yn
Clock
t
Tempo
Coppie
Disturbo
q
Posizioni
dq
Velocita`
ddq
Accelerazioni
tau
Tau
Mux
Mux1
q
tau
sc_d2dof.m
Inizializzazione con
esed2dof.m
The model takes into account the dynamics of a planar 2 dof arm, with gravity (arm in a
vertical plane). The dynamic model, described by the Matlab block dir2dyn.m
is obtained with the Euler-Lagrange approach:
M(q) q + C(q, q) q + D q + g(q) = +J
T
(q)F
a
Dynamic model in Simulink
The elements of the inertia matrix M(q) are
M
11
= m
1
a
2
C1
+ m
2
(a
2
1
+ a
2
C2
+ 2a
1
a
C2
C
2
) +
I
1
+
I
2
M
12
= m
2
(a
2
C2
+ a
1
a
C2
C
2
) +
I
2
M
22
= m
2
a
2
C2
+
I
2
Matrix C(q, q) is
C(q, q) =
_
h

2
h(

1
+

2
)
h

1
0
_
h = m
2
a
1
a
C2
S
2
The terms in the gravity vector are:
g
1
= (m
1
a
C1
+ m
2
a
1
)gC
1
+ m
2
ga
C2
C
12
g
2
= m
2
ga
C2
C
12
function ddq = dir2dyn(u);
% function ddq = dir2dyn(q,dq,tau,tauint);
% Dynamic model of a planar 2 dof manipulator
% Input:
% dq -->> joint velocity vector
% q -->> joint position vector
% tau -->> joint torques
% tauint -->> interaction forces
% Output:
% ddq -->> joint acceleration vector
global I1z I2z m1 m2 L1 L2 Lg1 Lg2 g D
q1 = u(1); q2 = u(2);
dq = u(3:4); dq1 = u(3); dq2 = u(4);
tau = u(5:6); tauint = u(7:8);
% Kinematic functions
s1 = sin(q1); s2 = sin(q2); s12 = sin(q1+q2);
c1 = cos(q1); c2 = cos(q2); c12 = cos(q1+q2);
dq12 = dq1 + dq2;
%%%%% Elements of the Inertia Matrix M
M11 = I1z+I2z+Lg1^2*m1+m2*(L1^2+Lg2^2+2*L1*Lg2*c2);
M12 = I2z+m2*(Lg2^2+L1*Lg2*c2);
M22 = I2z+Lg2^2*m2;
M = [M11, M12; M12, M22];
%%%%% Coriolis and centrifugal elements
C11 = -(L1*dq2*s2*(Lg2*m2)); C12 = -(L1*dq12*s2*(Lg2*m2));
C21 = m2*L1*Lg2*s2*dq1; C22 = 0;
C = [C11 C12; C21 C22];
%%%%% Gravity :
g1 = m1*Lg1*c1+m2*(Lg2*c12 + L1*c1); g2 = m2*Lg2*c12;
G = g*[g1; g2];
%%%%% Computation of acceleration
ddq = inv(M) * (tau + tauint - C*dq - D*dq - G);
% Simulation of the dynamic model of a planar 2 dof manipulator
% Simulink scheme: sc_d2dof.m
% Definition and initialization of global variables
% Run the simulation RK45
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% Robots Coefficients %%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear I1z I2z m1 m2 L1 L2 Lg1 Lg2 g D
global I1z I2z m1 m2 L1 L2 Lg1 Lg2 g D
I1z = 10; I2z = 10;
m1 = 50.0; m2 = 50.0;
L1 = 1; L2 = 1;
Lg1=L1/2; Lg2=L2/2;
g = 9.81;
D = 0.0 * eye(2); % friction
% D = diag([30,30]); % friction
% D = diag([80,80]); % friction
COPPIA = 0; % joint torques
DISTURBO = 0; % joint torques due to interaction with environment
%%%% Iniziatialization and run
TI = 0; TF = 10;
ERR = 1e-3;
TMIN = 0.002; TMAX = 10*TMIN;
OPTIONS = [ERR,TMIN,TMAX,0,0,2];
X0 = [0 0 0 0];
[ti,xi,yi]=rk45(sc_d2dof,TF,X0,OPTIONS);
D = diag[0, 0];
0 1 2 3 4 5 6 7 8 9 10
4
3
2
1
0
1
Posizioni q1, q2 (dash)
0 1 2 3 4 5 6 7 8 9 10
10
5
0
5
10
Velocita dq1, dq2 (dash)
0 1 2 3 4 5 6 7 8 9 10
40
20
0
20
40
Accelerazioni ddq1, ddq2 (dash)
0 1 2 3 4 5 6 7 8 9 10
1
0.5
0
0.5
1
Coppie tau1, tau2 (dash)
D = diag[30, 30];
0 1 2 3 4 5 6 7 8 9 10
3
2
1
0
1
0 1 2 3 4 5 6 7 8 9 10
4
2
0
2
4
0 1 2 3 4 5 6 7 8 9 10
15
10
5
0
5
10
15
0 1 2 3 4 5 6 7 8 9 10
1
0.5
0
0.5
1
D = diag[80, 80];
0 1 2 3 4 5 6 7 8 9 10
3
2
1
0
1
2
0 1 2 3 4 5 6 7 8 9 10
3
2
1
0
1
0 1 2 3 4 5 6 7 8 9 10
15
10
5
0
5
10
15
0 1 2 3 4 5 6 7 8 9 10
1
0.5
0
0.5
1

FIR 05 Dynamics

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

FIR 05 Dynamics

Uploaded by

Copyright:

Available Formats

Dynamic model of robot manipulators

C. Melchiorri (DEI) Dynamic Model 11 / 65

Empty cylinder with length h, and external/internal radii a, b

From the general expression

we have the following second order dierential equation

You might also like