dynamic model of robot manipulators - lar-deis home page · dynamic models introduction...
TRANSCRIPT
Dynamic model of robot manipulators
Claudio Melchiorri
Dipartimento di Ingegneria dell’Energia Elettrica e dell’Informazione (DEI)
Universita di Bologna
email: [email protected]
C. Melchiorri (DEI) Dynamic Model 1 / 65
Summary
1 Dynamic modelsIntroductionEuler-Lagrange model
C. Melchiorri (DEI) Dynamic Model 2 / 65
Dynamic models Introduction
Dynamic model of manipulators
Robot Dynamics =⇒ Study of the relation between the applied forces/torquesand the resulting motion of an industrial manipulator.
Similarly to kinematics, also for the dynamics it is possible to define two “models”:
Direct model: once the forces/torques applied to the joints, as well as thejoint positions and velocities are known, compute the joint accelerations
q = f (q, q, τ)
and then
q =
∫
q dt, q =
∫
q dt
Inverse model: once the joint accelerations, velocities and positions areknown, compute the corresponding forces/torques
τ = f −1(q, q, q) = g(q, q, q)
C. Melchiorri (DEI) Dynamic Model 3 / 65
Dynamic models Introduction
Dynamic model of manipulators
Normally, a manipulator is composed by an open kinematic chain, and its dynamicmodel is affected 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 effects 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 ofconnected rigid bodies is made.
C. Melchiorri (DEI) Dynamic Model 4 / 65
Dynamic models Introduction
Dynamic model of manipulators
Two problems may be defined for the study of the dynamic model:
Direct dynamic model: computation of the time evolution of q(t) (andthen of q(t), q(t)), given the vector of generalized forces (torques and/orforces) τ (t) applied to the joints and, in case, the external forces applied tothe end-effector, and the initial conditions q(t = t0), q(t = t0).
τ (t) =⇒ q(t) ( q(t), q(t) )
Inverse dynamic problem: computation of the vector τ (t) necessary toobtain a desired trajectory q(t), q(t), q(t), once the forces applied of theend-effector are known.
q(t), q(t), q(t) =⇒ τ (t)
C. Melchiorri (DEI) Dynamic Model 5 / 65
Dynamic models Introduction
Dynamic model of manipulators
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 definition 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 effects 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 efficient.
NEWTON-EULER approach.Based on a computationally efficient 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).
C. Melchiorri (DEI) Dynamic Model 6 / 65
Dynamic models Euler-Lagrange model
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 areusually possible.
In robotics =⇒ joint variables q1, q2, . . . qn.
C. Melchiorri (DEI) Dynamic Model 7 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model
From physics, we know that it is possible to define:
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
∂qi
)
−∂L
∂qii = 1, . . . , n
being ψi the non-conservative (external or dissipative) generalized forcesperforming work on qi . In robotics, we consider:
τi joint actuator torque[
JTFc
]
iterm due to external (contact) forces
dii qi joint friction torque
Therefore: ψi = τi +[
JTFc
]
i− dii qi .
C. Melchiorri (DEI) Dynamic Model 8 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model
Since the potential energy does not depend on the velocity, the Euler-Lagrangeequations can be rewritten as
ψi =d
dt
(
∂K
∂qi
)
−∂K
∂qi+∂P
∂qii = 1, . . . , n (1)
This formulation is more convenient since in robotics it is possible to computequite 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
Ki P =
n∑
i=1
Pi
C. Melchiorri (DEI) Dynamic Model 9 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model
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
pC =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
vT (x , y , z)v(x , y , z)ρ dx dy dz
=1
2
∫
B
vT (x , y , z)v(x , y , z) dm
C. Melchiorri (DEI) Dynamic Model 10 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model
Let assume that vC and ω, i.e. the trans-lational and rotational velocities of thecenter of mass, are known with respectto an inertial frame F0 .
The velocity of a generic point p′ of the body is
v = vC + ω × (p′ − pC ) = vC + ω × r (2)
The velocity expressed in a frame F ’ fixed to the rigid body may be computed byintroducing the rotation matrix R betweeen F ’ and F0
RT v = RT (vC + ω × r) = RT vC + (RTω)× (RT r)
Thereforev′ = v′C + ω′ × r′
C. Melchiorri (DEI) Dynamic Model 11 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model
Since the product ω × r in (2) can be expressed as S(ω) r, we have:
K =1
2
∫
B
vT (x , y , z)v(x , y , z) dm
=1
2
∫
B
(vC + Sr)T (vC + Sr) dm
=1
2
∫
B
vTC vC dm +1
2
∫
B
rTSTSr dm +
∫
B
vTC Sr dm
=1
2
∫
B
vTC vC dm +1
2
∫
B
rTSTSr dm
As a matter of fact, from the definition of CoM (∫
Br dm =
∫
B(pC − p)dm = 0):
∫
B
vTC Sr dm = vTC S
∫
B
r dm = 0
C. Melchiorri (DEI) Dynamic Model 12 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model
In conclusion
K =1
2
∫
B
vTC vC dm +1
2
∫
B
rTSTSr dm
The first term depends on the linear velocity vC of the center of mass
1
2
∫
B
vTC vC dm =1
2m vTC vC
For the second term, considering that aTb = Tr(a bT ) and the particularstructure of matrix S, we have
1
2
∫
B
rTSTSr dm =1
2
∫
B
Tr(Sr rTST ) dm =1
2Tr(S
∫
B
rrT dm ST )
=1
2Tr(S J ST ) =
1
2ω
T I ω I : body inertia matrix
Also this term depends on the velocity of the center of mass (in this case ω).
C. Melchiorri (DEI) Dynamic Model 13 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model
Matrix J, Euler matrix, and matrix I, body inertia matrix, are symmetric, and havethe following general expressions:
J =
∫
r2x dm∫
rx ry dm∫
rx rz dm∫
rx ry dm∫
r2y dm∫
ry rz dm∫
rx rz dm∫
ry rz dm∫
r2z dm
I =
∫
(r2y + r2z ) dm −∫
rx ry dm −∫
rx rz dm
−∫
rx ry dm∫
(r2x + r2z ) dm −∫
ry rz dm
−∫
rx rz dm −∫
ry rz dm∫
(r2x + r2y ) dm
The elements of both matrices J ed I depend on vector r, i.e. on the position ofthe generic point of the i-th link with respect to its center of mass, defined in thebase frame.
Since the position of the i-th link depends on the configuration of themanipulator, matrices J and I are in general functions of the joint variables q!
C. Melchiorri (DEI) Dynamic Model 14 / 65
Dynamic models Euler-Lagrange model
Examples of body inertia matrices
Homogeneous bodies of mass m, with axes of symmetry.
• Parallelepiped with sides a (length/height), b, c (base)
I =
IxxIyy
Izz
=
112m(b2 + c2)
112m(a2 + c2)
112m(a2 + b2)
• Empty cylinder with length h, and external/internal radii a, b
I =
12m(a2 + b2)
112m
(
3(a2 + b2) + h2)
Izz
,
Izz = Iyy
I ′zz = Izz +m(
h2
)2
single axis translation theorem
x0
y0
z0
a
b
c
x0
y0
z0z ′0
a
b
h
h/2
Steiner theorem:changes of body inertia dueto translation p of the frameof computation:
I = Ic +m(pT p E3×3 − ppT )
Ic body inertia matrix wrt thecenter of massE3×3 (3× 3) identity matrix
C. Melchiorri (DEI) Dynamic Model 15 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model
In conclusion, the kinetic energy of a rigid body is defined as (Konig Theorem)
K =1
2m vTC vC +
1
2ωT Iω (3)
Both terms depend only on the velocity of the rigid body.
The first term, being related to the magnitude of a vector (‖vC ‖ = vTCvC ), is invariant with
respect to the reference frame used to express the velocity:
vTC vC = (RvC )T (RvC ) = vTC (RTR)vC , ∀ 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 thefollowing relation:
I′ = R I RT
Then: ωT Iω = ω
′T I′ω′ = (R ω)T (RIRT )(Rω) = ωT (RTR)I(RTR)ω.
Therefore, being (3) invariant with respect to the reference frame, F can be chosen in order tosimplify the computations required for the evaluation of K .
C. Melchiorri (DEI) Dynamic Model 16 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model
Since the kinetic energy Ki of the generic i-th link is invariant with respect to thereference frame (used to express vC ,ω, I), it is convenient to chose a frameFi fixed 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 thegeometric properties of the link.
On the other hand, normally the rotational velocity ω is defined in the base frameF0 , and therefore it is needed to transform it according to RT
ω, being R therotation matrix between Fi and F0 (known from the kinematic model of themanipulator).
C. Melchiorri (DEI) Dynamic Model 17 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model
In conclusion, the kinetic energy of a manipulator can be determined when, foreach link, the following quantities are known:
the link mass mi ;
the inertia matrix Ii , computed wrt a frame Fi fixed to the center of mass inwhich it has a constant expression Ii ;
the linear velocity vCi of the center of mass, and the rotational velocity ωi ofthe link (both expressed in F0 );
the rotation matrix Ri between the frame fixed to the link and F0 .
The kinetic energy Ki of the i-th link has the form:
Ki =1
2miv
TCivCi +
1
2ωTi Ri IiR
Ti ωi
It is now necessary to compute the linear and rotational velocities (vCi and ωi ) asfunctions of the Lagrangian coordinates (i.e. the joint variables q).
C. Melchiorri (DEI) Dynamic Model 18 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model
The end-effector velocity may be computed as a function of the joint velocities q1, . . . , qnthrough the Jacobian matrix J. The same methodology can be used to compute thevelocity of a generic point of the manipulator, and in particular the velocity vCi = pCi ofthe center of mass pCi , that results function of the joint velocities q1, . . . , qi only:
pCi = Jiv1q1 + Jiv2q2 + . . .+ Jivi qi = Jiv q
ωi = Jiω1q1 + Jiω2q2 + . . .+ Jiωi qi = Jiωq
where
Jiv =[
Jiv1 . . . Jivi 0 . . . 0]
Jiω =[
Jiω1 . . . Jiωi 0 . . . 0]
with[
JivjJiωj
]
=
[
zj−1 × (pCi − pj−1)zj−1
]
rotational joint[
JivjJiωj
]
=
[
zj−1
0
]
linear joint
being pj−1 the position of the origin of the frame associated to the j-th link.C. Melchiorri (DEI) Dynamic Model 19 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model
In conclusion, for a n dof manipulator we have:
K =1
2
n∑
i=1
mivTCivCi +
1
2
n∑
i=1
ωTi Ri IiR
Ti ω
=1
2qT
n∑
i=1
[
miJi Tv (q)Jiv (q) + Ji T
ω(q)Ri IiR
Ti J
iω(q)
]
q
=1
2qTM(q)q
=1
2
n∑
i=1
n∑
j=1
Mij(q)qi qj
where M(q) is a n × n, symmetric and positive definite matrix, function of themanipulator configuration q.
Matrix M(q) is called the Inertia Matrix of the manipulator.
C. Melchiorri (DEI) Dynamic Model 20 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model
Computation of the Potential Energy. For rigid bodies, the only potentialenergy taken into account in the dynamics is due to the gravitational field g. Forthe generic i-th link
Pi =
∫
Li
gTp dm = gT
∫
Li
p dm = gTpCimi
The potential energy does not depend on the joint velocities q, and may beexpressed as a function of the position of the centers of mass. For the wholemanipulator:
P =
n∑
i=1
gTpCimi
In case of flexible link, one should consider also terms due to elastic forces.
K e P are computed (once mi and I are known) with a procedure similar to the oneadopted for the forward kinematic model:
• K → matrices Ji e Ri , • P → pCi position of the centers of mass
C. Melchiorri (DEI) Dynamic Model 21 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model
Once K and P are known, it is possible to compute the dynamic model of themanipulator. The dynamics is expressed by
ψk =d
dt
(
∂L
∂qk
)
−∂L
∂qkk = 1, . . . , n
The Lagrangian function is
L = K − P =1
2
n∑
i=1
n∑
j=1
Mij qi qj −n∑
i=1
gTpCimi
Then∂L
∂qk=∂K
∂qk=
n∑
j=1
Mkj qj
and
d
dt
∂L
∂qk=
n∑
j=1
Mkj qj +n∑
j=1
d Mkj
dtqj =
n∑
j=1
Mkj qj +n∑
i=1
n∑
j=1
∂Mkj
∂qiqi qj
Moreover∂L
∂qk=
1
2
n∑
i=1
n∑
j=1
∂Mij
∂qkqi qj −
∂P
∂qkC. Melchiorri (DEI) Dynamic Model 22 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model
The Lagrangian equations have the following formulation
n∑
j=1
Mkj qj +n∑
i=1
n∑
j=1
[
∂Mkj
∂qi−
1
2
∂Mij
∂qk
]
qi qj +∂P
∂qk= ψk k = 1, . . . , n
By defining the term hkji (q) as
hkji (q) =∂Mkj(q)
∂qi−
1
2
∂Mij (q)
∂qk
and gk(q) as
gk(q) =∂P(q)
∂qk
the following equations are finally obtained
n∑
j=1
Mkj(q)qj +n
∑
i=1
n∑
j=1
hkji (q)qi qj + gk(q) = ψk k = 1, . . . , n
C. Melchiorri (DEI) Dynamic Model 23 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model
The elements Mkj (q), hijk(q), gk (q) are function of the joint position only, andtherefore their computation is relatively simple once the manipulator’s configuration isknown. They have the following physical meaning:
For the acceleration terms:
Mkk is the moment of inertia about the k-th joint axis, in a given configuration andconsidering blocked all the other joints
Mkj is the inertia coupling, accounting the effect of acceleration of joint j on joint k
For the quadratic velocity terms:
hkjj q2j represents the centrifugal effect induced on joint k by the velocity of joint j
(notice that hkkk = ∂Mkk
∂qk= 0)
hkji qi qj represents the Coriolis effect induced on joint k by the velocities of joints iand j
For the configuration-dependent terms:
gk represents the torque generated on joint k by the gravity force acting on themanipulator in the current configuration.
C. Melchiorri (DEI) Dynamic Model 24 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model
Recalling that the non-conservative forces ψk are in general composed by
τk joint actuator torque[
JTFc
]
kexternal (contact) forces
dkk qk joint friction torque
the Lagrangian equations
n∑
j=1
Mkj(q)qj +
n∑
i=1
n∑
j=1
hkji (q)qi qj + gk(q) = ψk k = 1, . . . , n
can be written in matrix form as
M(q)q+ C(q, q)q+Dq+ g(q) = τ + JT (q)Fc
This matrix equation is known as the dynamic model of the manipulator.
C. Melchiorri (DEI) Dynamic Model 25 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model - Some considerations
The product C(q, q)q =∑n
i=1
∑n
j=1 hkji (q)qi qj is a (n × 1) vector v whoseelements are quadratic functions of the joint velocities qj .
The k-th element vk of this vector is:
vk =n
∑
j=1
Ckj qj
where the elements Ckj are computed as
Ckj =n
∑
i=1
cijk qi
with
cijk =1
2
[
∂Mkj
∂qi+∂Mki
∂qj−∂Mij
∂qk
]
Christoffel Symbols (4)
C. Melchiorri (DEI) Dynamic Model 26 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model - Some considerations
The elements of matrix C(q, q) are computed as follows. From
n∑
i=1
n∑
j=1
hkji qi qj =
n∑
i=1
n∑
j=1
[
∂Mkj
∂qi−
1
2
∂Mij
∂qk
]
qi qj
by exchanging the sum (i , j) and exploiting the symmetry one obtains
n∑
i=1
n∑
j=1
∂Mkj
∂qi=
1
2
n∑
i=1
n∑
j=1
[
∂Mkj
∂qi+∂Mki
∂qj
]
and thenn∑
i=1
n∑
j=1
[
∂Mkj
∂qi−
1
2
∂Mij
∂qk
]
=n∑
i=1
n∑
j=1
1
2
[
∂Mkj
∂qi+∂Mki
∂qj−∂Mij
∂qk
]
=n∑
i=1
n∑
j=1
cijk
where cijk = 12
[
∂Mkj
∂qi+ ∂Mki
∂qj−
∂Mij
∂qk
]
are the so-called Christoffel Symbols.
Since matrix M(q) is symmetric, for a given k then cijk = cjik .The elements of matrix C(q, q)are then computed as
[C(q, q)]k,j
=
n∑
i=1
cijk qi (5)
C. Melchiorri (DEI) Dynamic Model 27 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model - Some considerations
This is not the only possible expression for matrix C(q, q). In general, any matrixsuch that
n∑
j=1
cij qj =
n∑
j=1
n∑
k=1
hijk qk qj
can be considered. The choice (4) is preferred since in this case the followingproperty is verified.
Property. Matrix N(q, q), defined as
N(q, q) = M(q, q)− 2C(q, q) (6)
in which the elements of C(q, q) are defined as
cijk =1
2
[
∂Mkj
∂qi+∂Mki
∂qj−∂Mij
∂qk
]
[C(q, q)]k,j =n
∑
i=1
cijk qi
results skew-symmetric, i.e. nkj = −njk , nkk = 0.
C. Melchiorri (DEI) Dynamic Model 28 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model - Some considerations
In fact, by considering the generic element nkj , one obtains
nkj =d Mkj
dt− 2[C]kj
=
n∑
i=1
[
∂Mkj
∂qi− (
∂Mkj
∂qi+∂Mki
∂qj−∂Mij
∂qk)
]
qi
=
n∑
i=1
[
∂Mij
∂qk−∂Mki
∂qj
]
qi
from which it follows (if indices k and j are exchanged, because of the symmetryof M(q)) that nkj = −njk .
Since matrix N is skew-symmetrix, then
xT N(q, q) x = 0, ∀x
C. Melchiorri (DEI) Dynamic Model 29 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model - Some considerations
The conditionxT N(q, q) x = 0, ∀x
holds since N(q, q) is skew-symmetric, due to the particular choice of the elements ofmatrix C(q, q). On the other hand, the condition
qT 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
(
qTMq)
= qT (τ −Dq− g(q)− JTF)
The first element is (from the dynamic model Mq = −Cq−Dq − g(q) + τ − JTF):
1
2
d
dt
(
qTMq)
=1
2qTMq+ qTMq =
1
2qTMq+ qT (−Cq−Dq− g(q) + τ − JTF)
C. Melchiorri (DEI) Dynamic Model 30 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model - Some considerations
Then
1
2qTMq+ qT (−Cq −Dq− g(q) + τ − JTF) = qT (τ −Dq− g(q)− JTF)
from which
1
2qTMq = qTCq =⇒ qT (M− 2C)q = 0
This equation holds ∀q and without any assumption on matrix C(q, q) (it holdsalso if C is not based on the Cristoffel symbols).
C. Melchiorri (DEI) Dynamic Model 31 / 65
Dynamic models Euler-Lagrange model
Euler-Lagrange model - Some considerations
In deriving the dynamic model, the actuation system has not been taken intoaccount. This is normally composed by:
motors
reduction gears
trasmission system.
The actuation system has several effects 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 effects such as backslash, friction, elasticity,...
Notice that these effects could be considered by introducing suitable terms in thedynamic model derived on the basis of the Euler-Lagrangian formulation.
C. Melchiorri (DEI) Dynamic Model 32 / 65
Dynamic models Euler-Lagrange model
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 coefficient,
I inertia seen at the rotation axis.
Kinetic energy: K = 12 I θ
2
Potenzial energy: P = mgL(1− cos θ)
Lagrangian function L: L = 12 I θ
2 −mgL(1− cos θ)
C. Melchiorri (DEI) Dynamic Model 33 / 65
Dynamic models Euler-Lagrange model
Example - 1
Lagrangian function: L = 12 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 appliedto the joint and for the friction effect:
ψ = τ − d θ
From the general expression
ψ =d
dt
(
∂L
∂θ
)
−∂L
∂θ
we have the following second order differential equation
I θ + d θ +mgL sin θ = τ
C. Melchiorri (DEI) Dynamic Model 34 / 65
Dynamic models Euler-Lagrange model
Example - 2
Dynamic model of a 2 dof manipulator. Consider:
θi i-th joint variable;
mi i-th link mass ;
Ii i-th link inertia, about an axis throughthe CoM and parallel to z;
ai i-th link length;
aCi distance between joint i and the CoMof the i-th link;
τi torque on joint i;
g gravity force along y;
Pi , Ki potential and kinetic energy of thei-th link.
The dynamic equations will be obtained in two manners:
a) with the “classic” approach, deriving the Lagrangian function (based on the kineticand potential energy K , P)
b) exploiting the particular structure of a manipulator (Jacobian, ...).
C. Melchiorri (DEI) Dynamic Model 35 / 65
Dynamic models Euler-Lagrange model
Example - 2 (classic approach)
We chose as generalized coordinates the joint variables q1 = θ1; q2 = θ2. Thekinetic and potential energies Ki and Pi are:
link 1:
K1 =1
2m1a
2C1θ1
2+
1
2I1θ1
2, P1 = m1gaC1S1
link 2: in this case, the position and velocity of the CoM are
pC2x = a1C1 + aC2C12
pC2y = a1S1 + aC2S12
pC2x = −a1S1θ1 − aC2S12(θ1 + θ2)
pC2y = a1C1θ1 + aC2C12(θ1 + θ2)
then
K2 =1
2m2p
TC2pC2 +
1
2I2(θ1 + θ2)
2, P2 = m2g(a1S1 + aC2S12)
wherepTC2pC2 = a
21 θ1
2+ a
2C2(θ1 + θ2)
2 + 2a1aC2C2(θ21 + θ1θ2)
C. Melchiorri (DEI) Dynamic Model 36 / 65
Dynamic models Euler-Lagrange model
Example - 2 (classic approach)
Therefore, L = K1 + K2 − P1 − P2 and
τ1 = [m1a2C1 + I1 +m2(a
21 + a2C2 + 2a1aC2C2) + I2]θ1 +
+[m2(a2C2 + a1aC2C2) + I2]θ2 −m2a1aC2S2(2θ1θ2 + θ22) +
+m1gaC1C1 +m2g(a1C1 + aC2C12)
τ2 = [m2(a2C2 + a1aC2C2) + I2]θ1 + (m2a
2C2 + I2)θ2 +
m2a1aC2S2θ21 +m2gaC2C12
C. Melchiorri (DEI) Dynamic Model 37 / 65
Dynamic models Euler-Lagrange model
Example - 2 (‘robotics’ approach)
The structural properties of the manipulator are exploited for the computation ofthe kinematic and potential energies. For the computation of the velocities of theCoM, one obtains:
J1v =
−aC1S1 0aC1C1 0
0 0
J2v =
−a1S1 − aC2S12 −aC2S12a1C1 + aC2C12 aC2C12
0 0
and
J1ω=
0 00 01 0
J2ω=
0 00 01 1
In this particular case, the frames associated to link 1 and 2 have z axes parallel tothe same axis of F0 , and therefore it is not necessary to consider the rotationmatrices R1,R2 (ω = ωz).
C. Melchiorri (DEI) Dynamic Model 38 / 65
Dynamic models Euler-Lagrange model
Example - 2 (‘robotics’ approach)
The kinetic energy is computed as
K =1
2qT
[
m1J1 Tv J1v +m2J
2 Tv J2v + J1 T
ωI1J
1ω+ J2 T
ωI2J
2ω
]
q
being
J1 Tω
I1J1ω+ J1 T
ωI2J
2ω=
[
I1 + I2 I2I2 I2
]
The elements of the inertia matrix M(q) are
M11 = m1a2C1 +m2(a
21 + a2C2 + 2a1aC2C2) + I1 + I2
M12 = m2(a2C2 + a1aC2C2) + I2
M22 = m2a2C2 + I2
C. Melchiorri (DEI) Dynamic Model 39 / 65
Dynamic models Euler-Lagrange model
Example - 2 (‘robotics’ approach)
From M11 = m1a2C1 +m2(a
21 + a
2C2 + 2a1aC2C2) + I1 + I2
M12 = m2(a2C2 + a1aC2C2) + I2
M22 = m2a2C2 + I2
The Christoffel symbols cijk = 12
[
∂Mkj
∂qi+ ∂Mki
∂qj−
∂Mij
∂qk
]
are
c111 =1
2
∂M11
∂q1= 0
c121 = c211 =1
2
∂M11
∂q2= −m2a1aC2S2 = h
c221 =∂M12
∂q2−
1
2
∂M22
∂q1= h
c112 =∂M21
∂q1−
1
2
∂M11
∂q2= −h
c122 = c212 =∂M22
∂q1= 0
c222 =∂M22
∂q2= 0
=⇒ Matrix C(q, q) is
C(q, q) =
[
hθ2 h(θ1 + θ2)
−hθ1 0
]
C. Melchiorri (DEI) Dynamic Model 40 / 65
Dynamic models Euler-Lagrange model
Example - 2 (‘robotics’ approach)
Matrix N(q) is
N(q) = M(q)− 2C(q, q)
=
[
2hθ2 hθ2hθ2 0
]
− 2
[
hθ2 h(θ1 + θ2)
−hθ1 0
]
=
[
0 −2hθ1 − hθ22hθ1 + hθ2 0
]
As expected, it results skew-symmetric.
C. Melchiorri (DEI) Dynamic Model 41 / 65
Dynamic models Euler-Lagrange model
Example - 2 (‘robotics’ approach)
For the potential energy, we have:
P1 = m1gaC1S1
P2 = m2g(a1S1 + aC2S12)
Then
P = P1 + P2 = (m1aC1 +m2a1)gS1 +m2gaC2S12
g1 =∂P
∂θ1= (m1aC1 +m2a1)gC1 +m2gaC2C12
g2 =∂P
∂θ2= m2gaC2C12
C. Melchiorri (DEI) Dynamic Model 42 / 65
Dynamic models Euler-Lagrange model
Example - 2 (‘robotics’ approach)
Summarizing, from M(q)q+ C(q, q)q+Dq+ g(q) = τ we have
M11θ1 +M12θ2 + c121θ1θ2 + c211θ2θ1 + c221θ22 + g1 = τ1
M21θ1 +M22θ2 + c112θ21 + g2 = τ2
or
[m1a2C1 +m2(a
21 + a2C2 + 2a1aC2C2) + I1 + I2]θ1 + [m2(a
2C2 + a1aC2C2) + I2]θ2
−m2a1aC2S2θ22 − 2m2a1aC2S2θ1θ2
+(m1aC1 +m2a1)gC1 +m2gaC2C12 = τ1
[m2(a2C2 + a1aC2C2) + I2]θ1 + [m2a
2C2 + I2]θ2
+m2a1aC2S2θ21
+m2gaC2C12 = τ2
=⇒ Same result!
C. Melchiorri (DEI) Dynamic Model 43 / 65
Dynamic models Euler-Lagrange model
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.
C. Melchiorri (DEI) Dynamic Model 44 / 65
Dynamic models Euler-Lagrange model
Properties of the Euler-Lagrangian dynamic model
1. Properties of matrix M(q)
1 M(q) ∈ IRn×n is symmetric, positive definite and depends on the manipulator
configuration q
2 M(q) is upper and lower bounded:
µ1I ≤ M(q) ≤ µ2I
that is
xT (M(q)− µ1I)x ≥ 0 xT (µ2I−M(q))x ≥ 0
3 also M−1(q) is bounded1
µ2I ≤ M−1(q) ≤
1
µ1I
4 in case of revolute joints, then µ1, µ2 are constant (not function of q) since theelements of M(q) are functions of sin(qi ) or cos(qi )
5 in case of prismatic joints, µ1, µ2 may result scalar functions of q
6 since M(q) is bounded, then
M1 ≤ ||M(q)|| ≤ M2
for some properly defined norm (1, 2, p,∞)
C. Melchiorri (DEI) Dynamic Model 45 / 65
Dynamic models Euler-Lagrange model
Properties of the Euler-Lagrangian dynamic model
2. Properties of vector c(q, q) = C(q, q)q1 C(q, q)q is a quadratic function of q2 the generic k-th element of vector c(q, q) = C(q, q)q can also be witten as
ck(q, q) = qTSk(q)q
Sk(q) =1
2
(
∂mk
∂q+
(
∂mk
∂q
)T
−∂M
∂qk
)
mk = k-th col. of M
3 it results that||C(q, q)q|| ≤ vb||q||
2
4 in case of rotative joints, then vb is constant (not function of q) since we have onlytranscendental functions (sin(qi ) or cos(qi))
5 in case of prismatic joints, then vb may result a scalar function of q6 for any choice of C(q, q), then matrix N(q, q) = M(q, q)− 2C(q, q) verifies:
qTN(q, q)q = 0
7 with a proper choice of the elements of C(q, q) (Christoffel symbols), matrix
N(q, q) = M(q, q)− 2C(q, q) is skew-symmetric, i.e.
xTN(q, q)x = 0 ∀x
C. Melchiorri (DEI) Dynamic Model 46 / 65
Dynamic models Euler-Lagrange model
Properties of the Euler-Lagrangian dynamic model
3. Properties of vectors g(q) and Dq
1 the friction term Dq is bounded:
||Dq|| ≤ dmax ||q||
2 the gravity term g(q) is bounded
||g(q)|| ≤ gb(q)
3 in case of revolute joints, gb is constant (does not depend on q) since qi dependson sinusoidal functions (sin(qi) or cos(qi ))
4 in case of prismatic joints, then gb may result function of q.
C. Melchiorri (DEI) Dynamic Model 47 / 65
Dynamic models Euler-Lagrange model
Properties of the Euler-Lagrangian dynamic model
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 effectsamong 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+Dq + g(q) = τ
Y(q, q, q)α = τ
C. Melchiorri (DEI) Dynamic Model 48 / 65
Dynamic models Euler-Lagrange model
Example
Properties of the dynamic model of a2 dof manipulator.
Neglecting friction effects we have:
M(q) =
[
m1a2C1 +m2(a
21 + a2C2 + 2a1aC2C2) + I1 + I2 m2(a
2C2 + a1aC2C2) + I2
m2(a2C2 + a1aC2C2) + I2 m2a
2C2 + I2
]
C(q, q)q = −m2a1aC2S2
[
2θ1θ2 + θ22−θ21
]
, g(q)=
[
(m1aC1 +m2a1)gC1 +m2gaC2C12
m2gaC2C12
]
Consider (for the sake of simplicity) the 1-norm || · ||1, and θ1, θ2 ∈ [−π/2, π/2].
C. Melchiorri (DEI) Dynamic Model 49 / 65
Dynamic models Euler-Lagrange model
Example
1. Bounds on the inertia matrix.
The scalar quantities µ1, µ2 can be defined as the minimum and maximumeigenvalues (λmin, λmax) of M(q), ∀q. Computationally, it is easier to define thescalars M1,M2.
The 1–norm of M(q) is alway defined on the basis of the first column:
||M(q)||1 = |m1a2C1+m2(a
21+a2C2+2a1aC2C2)+ I1+ I2|+ |m2(a
2C2+a1aC2C2)+ I2|
which is bounded by
M1 = m1a2C1 +m2(a
21 + 2a2C2) + I1 + 2I2
M2 = m1a2C1 +m2(a
21 + 2a2C2 + 3a1aC2) + I1 + 2I2
C. Melchiorri (DEI) Dynamic Model 50 / 65
Dynamic models Euler-Lagrange model
Example
2. Bounds on vector C(q, q)q
It results that
||C(q, q)q||1 = |m2a1aC2S2(2θ1θ2 + θ22)|+ |m2a1aC2S2θ21 |
≤ m2a1aC2|2θ1θ2 + θ22 + θ21 |
≤ m2a1aC2(|θ1|+ |θ2|)2
= vb||q||2
Moreover N(q, q) = M(q, q)− 2C(q, q), (h = −m2a1aC2S2):
N(q) = M(q)− 2C(q, q)
=
[
2hθ2 hθ2hθ2 0
]
− 2
[
hθ2 h(θ1 + θ2)
−hθ1 0
]
=
[
0 −2hθ1 − hθ22hθ1 + hθ2 0
]
C. Melchiorri (DEI) Dynamic Model 51 / 65
Dynamic models Euler-Lagrange model
Example
3. Bounds on the gravity vector g(q)
||g(q)||1 = |(m1aC1 +m2a1)gC1 +m2gaC2C12|+ |m2gaC2C12|
≤ (m1aC1 +m2a1)g + 2m2gaC2
= gb
Notice that if one of the joints is prismatic (and therefore ai , aCi may vary intime), then vb, gb are functions of q.
C. Melchiorri (DEI) Dynamic Model 52 / 65
Dynamic models Euler-Lagrange model
Example
Plots of the eigenvalues of M(q) for the 2 dof planar manipulator
050
100150
200250
300350
0
100
200
300
4000
20
40
60
80
100
120
140
160
180
lm
in = 6.2399
lm
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 q2! These results have been obtainedwith:
m1 = m2 = 50kg ; I1 = I2 = 10; a1 = a2 = 1; ac1 = ac2 = 0.5
One obtains
M1 = 117, 5, M2 = 192.5 e ‖M‖1 = 192.5, ‖M‖2 = 161.26, ‖M‖∞ = 192.5C. Melchiorri (DEI) Dynamic Model 53 / 65
Dynamic models Euler-Lagrange model
Example
3. Linearity of the dynamic model Y(q, q, q)α = τ
[m1a2C1 +m2(a
21 + a2C2 + 2a1aC2C2) + I1 + I2]θ1 + [m2(a
2C2 + a1a
2C2C2) + I2]θ2
−m2a1aC2S2θ22 − 2m2a1aC2S2θ1θ2
+(m1aC1 +m2a1)gC1 +m2gaC2C12 = τ1
[m2(a2C2 + a1aC2C2) + I2]θ1 + [m2a
2C2 + I2]θ2
+m2a1aC2S2θ21
+m2gaC2C12 = τ2
By inspection, one can define the parameters vector
α = [α1, α2, α3, α4, α5]T
with
α1 = m1aC1, α2 = m1a2C1 + I1, α3 = m2, α4 = m2aC2, α5 = m2a
2C2 + I2
C. Melchiorri (DEI) Dynamic Model 54 / 65
Dynamic models Euler-Lagrange model
Example
Then
Y(q, q, q) =
[
y11 y12 y13 y14 y150 0 0 y24 y25
]
with
y11 = gC1
y12 = θ1
y13 = a21θ1 + a1gC1
y14 = 2a1C2θ1 + a1C2θ2 − 2a1S2θ1θ2 − a1S2θ21 + gC12
y15 = θ1 + θ2
y24 = a1C2θ1 + a1S2θ21 + gC12
y25 = θ1 + θ2
The elements yij depend on q, q, q, g and on a1.
C. Melchiorri (DEI) Dynamic Model 55 / 65
Dynamic models Euler-Lagrange model
Example
Identification of the dynamic parameters
From:Y(q, q, q)α = τ
by measuring along a proper trajectory the quantities q, q, q, τ one obtains:
→ τ 0 = Y0α
→ YT0 τ 0 = YT
0 Y0α
→ α = (YT0 Y0)
−1YT0 τ 0 = Y+
0 τ 0
Being Y+0 = (YT
0 Y0)−1YT
0 the (left) pseudo-inverse of Y0.
Note that some of the parameters may result non identifiable!
C. Melchiorri (DEI) Dynamic Model 56 / 65
Dynamic models Euler-Lagrange model
A Simulink dynamic model
Demux
.
1/sq2
1/sdq2
Demux
Demux2
Mux
Mux2 ddq
dq
Mux
Mux
Mux
Mux3
1/sq1
1/sdq1
MATLABFunctiondir2yn
Clockt
Tempo
Coppie
Disturbo
qPosizioni
dqVelocita`
ddqAccelerazioni
tauTau
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 avertical 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+Dq+ g(q) = τ + JT (q)Fa
C. Melchiorri (DEI) Dynamic Model 57 / 65
Dynamic models Euler-Lagrange model
Dynamic model in Simulink
The elements of the inertia matrix M(q) are
M11 = m1a2C1 +m2(a
21 + a2C2 + 2a1aC2C2) + I1 + I2
M12 = m2(a2C2 + a1aC2C2) + I2
M22 = m2a2C2 + I2
Matrix C(q, q) is
C(q, q) =
[
hθ2 h(θ1 + θ2)
−hθ1 0
]
h = −m2a1aC2S2
The terms in the gravity vector are:
g1 = (m1aC1 +m2a1)gC1 +m2gaC2C12
g2 = m2gaC2C12
C. Melchiorri (DEI) Dynamic Model 58 / 65
Dynamic models Euler-Lagrange model
Dynamic model in Simulink
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;C. Melchiorri (DEI) Dynamic Model 59 / 65
Dynamic models Euler-Lagrange model
Dynamic model in Simulink
%%%%% 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);
C. Melchiorri (DEI) Dynamic Model 60 / 65
Dynamic models Euler-Lagrange model
Dynamic model in Simulink
% 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]); % frictionC. Melchiorri (DEI) Dynamic Model 61 / 65
Dynamic models Euler-Lagrange model
Dynamic model in Simulink
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);
C. Melchiorri (DEI) Dynamic Model 62 / 65
Dynamic models Euler-Lagrange model
Dynamic model in Simulink
D = diag[0, 0];
0 1 2 3 4 5 6 7 8 9 10−4
−3
−2
−1
0
1Posizioni q1, q2 (dash)
0 1 2 3 4 5 6 7 8 9 10−10
−5
0
5
10Velocita‘ dq1, dq2 (dash)
0 1 2 3 4 5 6 7 8 9 10−40
−20
0
20
40Accelerazioni ddq1, ddq2 (dash)
0 1 2 3 4 5 6 7 8 9 10−1
−0.5
0
0.5
1Coppie tau1, tau2 (dash)
C. Melchiorri (DEI) Dynamic Model 63 / 65
Dynamic models Euler-Lagrange model
Dynamic model in Simulink
D = diag[30, 30];
0 1 2 3 4 5 6 7 8 9 10−3
−2
−1
0
1Posizioni q1, q2 (dash)
0 1 2 3 4 5 6 7 8 9 10−4
−2
0
2
4Velocita‘ dq1, dq2 (dash)
0 1 2 3 4 5 6 7 8 9 10−15
−10
−5
0
5
10
15Accelerazioni ddq1, ddq2 (dash)
0 1 2 3 4 5 6 7 8 9 10−1
−0.5
0
0.5
1Coppie tau1, tau2 (dash)
C. Melchiorri (DEI) Dynamic Model 64 / 65
Dynamic models Euler-Lagrange model
Dynamic model in Simulink
D = diag[80, 80];
0 1 2 3 4 5 6 7 8 9 10−3
−2
−1
0
1
2Velocita‘ dq1, dq2 (dash)
0 1 2 3 4 5 6 7 8 9 10−3
−2
−1
0
1Posizioni q1, q2 (dash)
0 1 2 3 4 5 6 7 8 9 10−15
−10
−5
0
5
10
15Accelerazioni ddq1, ddq2 (dash)
0 1 2 3 4 5 6 7 8 9 10−1
−0.5
0
0.5
1Coppie tau1, tau2 (dash)
C. Melchiorri (DEI) Dynamic Model 65 / 65