Claudio Melchiorri
Dipartimento di Ingegneria dell’Energia Elettrica e dell’Informazione (DEI) Universit`a di Bologna
email:[email protected]
1 Dynamic models Introduction
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 define 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= Z ¨ qdt, q= Z ˙ 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 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 aseries of connected rigid bodiesis made.
Two problems may be defined for the study of the dynamic model:
Direct dynamic model: computation of the time evolution of ¨q(t) (and then ofq(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-effector, and the initial conditionsq(t =t0),q˙(t=t0).
τ(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-effector 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 synthesisof suitablecontrol algorithms
• analysis of the structural propertiesof the manipulator since the design phase. Two approaches for the definition of the dynamic model:
EULER-LAGRANGEapproach.
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-EULERapproach.
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.
Generalized variables, orLagrange 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 q1,q2, . . .qn.
From physics, we know that it is possible to define: 1 TheKinetic Energyfunction K(q,q)˙ 2 ThePotential Energyfunction P(q)
and therefore theLagrangian function L(q,q˙) =K(q,q˙)−P(q)
TheEuler-Lagrangeequations are
ψi = d dt ∂L ∂qi˙ − ∂L ∂qi i = 1, . . . ,n
beingψi thenon-conservative(external or dissipative) generalized forces performing work onqi. In robotics, we consider:
τi joint actuator torque
JTF
ci term due to external (contact) forces
diiq˙i joint friction torque
Since the potential energy does not depend on the velocity, the Euler-Lagrange equations can be rewritten as
ψi= d dt ∂K ∂qi˙ −∂K ∂qi + ∂P ∂qi i= 1, . . . ,n (1)
This formulation is more convenient since in robotics it is possible to compute quite easily the termsK andP from the geometric properties of the manipulator. Then, by applying (1), the dynamic model is obtained.
Note that K = n X i=1 Ki P= n X i=1 Pi
Computation of the Kinetic Energy. For a rigid bodyB: Themasscan be computed by
m= Z
B
ρ(x,y,z)dx dy dz
whereρ(x,y,z) is the mass density (assumed constant): ρ(x,y,z) =ρ. Thecenter of mass(CoM) is
pC = 1 m Z B p(x,y,z)ρdx dy dz= 1 m Z B p(x,y,z)dm Thekinetic energy results as
K = 1 2 Z B vT(x,y,z)v(x,y,z)ρdx dy dz = 1 2 Z B vT(x,y,z)v(x,y,z)dm
Let assume thatvC andω, i.e. the trans-lational and rotational velocities of the center of mass, are known with respect to an inertial frameF0.
The velocity of a generic pointp′ of the body is
v=vC+ω×(p′−pC) =vC +ω×r (2)
The velocity expressed in a frameF’ fixed to the rigid body may be computed by introducing the rotation matrixRbetweeenF’ andF0
RTv=RT(vC+ω×r) =RTvC+ (RTω)×(RTr) Therefore
v′ =v′
C+ω′×r′
Since the productω×rin (2) can be expressed asS(ω)r, we have: K = 1 2 Z B vT(x,y,z)v(x,y,z)dm = 1 2 Z B (vC+Sr)T(vC+Sr) dm = 1 2 Z B vCTvC dm+ 1 2 Z B rTSTSr dm+ Z B vTCSr dm = 1 2 Z B vCTvC dm+ 1 2 Z B rTSTSr dm
As a matter of fact, from the definition of CoM (R
Brdm= R B(pC−p)dm= 0): Z B vT CSr dm=vTCS Z B r dm= 0
In conclusion K = 1 2 Z B vT CvC dm+ 1 2 Z B rTSTSr dm
The first term depends on the linear velocityvC of the center of mass 1 2 Z B vT CvC dm= 1 2m v T CvC
For the second term, considering thataTb=Tr(a bT) and the particular structure of matrixS, we have
1 2 Z B rTSTSr dm = 1 2 Z B Tr(Sr rTST) dm= 1 2Tr(S Z B rrT dmST) = 1 2Tr(S J S T) =1 2ω
TIω I:body inertia matrix Also this term depends on the velocity of the center of mass (in this caseω).
MatrixJ,Euler matrix, and matrixI,body inertia matrix, are symmetric, and have the following general expressions:
J = R r2 x dm R rxrydm Rrxrzdm R rxrydm Rry2dm R ryrzdm R rxrzdm R ryrzdm R rz2dm I = R (ry2+rz2)dm −R rxrydm −R rxrzdm −R rxrydm R (rx2+rz2)dm −R ryrzdm −R rxrzdm −R ryrzdm R (rx2+ry2)dm
The elements of both matricesJed Idepend on vectorr, i.e. on the position of the generic point of the i-th link with respect to its center of mass, defined in the base frame.
Since the position of the i-th link depends on the configuration of the
Homogeneous bodies of massm, with axes of symmetry.
• Parallelepipedwith sidesa(length/height), b,c(base)
I= Ixx Iyy Izz = 1 12m(b2+c2) 1 12m(a2+c2) 1 12m(a2+b2)
• Empty cylinderwith lengthh, and external/internal radiia,b I= 1 2m(a2+b2) 1 12m 3(a2+b2) +h2 Izz , Izz = Iyy I′ zz = Izz+m h 2 2
single axis translationtheorem
x0 y0 z0 a b c x0 y0 z0 z′ 0 a b h h/2 Steiner theorem:
changes of body inertia due
to translationpof the frame
of computation:
I=Ic+m(pTp E3×3−ppT)
Ic body inertia matrix wrt the
center of mass
E3×3 (3×3) identity matrix
In conclusion, the kinetic energy of a rigid body is defined as (Konig Theorem) K =1 2mv T CvC + 1 2ω TIω (3)
Both terms depend only on the velocity of the rigid body.
The first term, being related to the magnitude of a vector (kvCk=vTCvC), is invariant with
respect to the reference frame used to express the velocity:
vT
C vC = (RvC)T(RvC) =vTC(RTR)vC, ∀R
This property holds also for the second term: the productωTIω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 RT
Then: ωTIω=ω′TI′ω′= (Rω)T(RIRT)(Rω) =ωT(RTR)I(RTR)ω.
Therefore, being (3) invariant with respect to the reference frame,F can be chosen in order to
Since the kinetic energyKi of the generic i-th link is invariant with respect to the reference frame (used to expressvC,ω,I), it is convenient to chose a frame Fi fixed to the link itself, with origin in the center of mass.
In this mannermatrixIis constantand simple to be computed on the basis of the geometric properties of the link.
On the other hand, normally the rotational velocityωis defined in the base frame F0, and therefore it is needed to transform it according toRTω, beingR the rotation matrix betweenFi andF0 (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 massmi;
the inertia matrixIi, computed wrt a frame Fi fixed to the center of mass in which it has a constant expression ˜Ii;
the linear velocityvCi of the center of mass, and the rotational velocityωi of the link (both expressed inF0);
the rotation matrixRi between the frame fixed to the link andF0.
The kinetic energyKi of the i-th link has the form: Ki= 1 2miv T CivCi+ 1 2ω T i Ri˜IiRTi ωi
It is now necessary to compute the linear and rotational velocities (vCi andωi) as functions of the Lagrangian coordinates (i.e. the joint variablesq).
The end-effector velocity may be computed as a function of the joint velocities ˙q1, . . . ,q˙n
through the Jacobian matrixJ. The same methodology can be used to compute the
velocity of a generic point of the manipulator, and in particular the velocityvCi = ˙pCi of
the center of masspCi, that results function of the joint velocities ˙q1, . . . ,q˙i only:
˙ pCi = Jiv1q˙1+Jiv2q˙2+. . .+Jiviq˙i = Jivq˙ ωi = Jiω1q˙1+Jiω2q˙2+. . .+Jiωiq˙i =Jiωq˙ where Jiv = Ji v1 . . . Jivi 0 . . . 0 Jiω = Ji ω1 . . . Jiωi 0 . . . 0 with Ji vj Ji ωj = zj−1× (pCi−pj−1) zj−1 rotational joint Ji vj Ji ωj = zj−1 0 linear joint
beingpj−1the position of the origin of the frame associated to the j-th link.
In conclusion, for andof manipulator we have: K = 1 2 n X i=1 mivT CivCi+ 1 2 n X i=1 ωT i Ri˜IiRTi ω = 1 2q˙ T n X i=1 h miJi T v (q)Jiv(q) +Ji Tω (q)Ri˜IiR T i Jiω(q) i ˙ q = 1 2q˙ TM(q) ˙q = 1 2 n X i=1 n X j=1 Mij(q) ˙qiq˙j
whereM(q) is an×n, symmetric and positive definite matrix, function of the manipulator configurationq.
Computation of the Potential Energy. For rigid bodies, the only potential energy taken into account in the dynamics is due to the gravitational fieldg. For the generic i-th link
Pi= Z Li gTpdm=gT Z Li pdm=gTpCimi
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 X i=1 gTp Cimi
In case of flexible link, one should consider also terms due to elastic forces.
K ePare computed (oncemi and ˜Iare known) with a procedure similar to the one
adopted for the forward kinematic model:
• K→matricesJi eR
i, • P →pCi position of the centers of mass
OnceK andP 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 ∂qk k= 1, . . . ,n The Lagrangian function is
L=K−P=1 2 n X i=1 n X j=1 Mijq˙iq˙j− n X i=1 gTpCimi Then ∂L ∂q˙k = ∂K ∂q˙k = n X j=1 Mkjq˙j and d dt ∂L ∂q˙k = n X j=1 Mkj¨qj+ n X j=1 d Mkj dt q˙j= n X j=1 Mkjq¨j+ n X i=1 n X j=1 ∂Mkj ∂qi ˙ qiq˙j Moreover ∂L ∂q = 1 2 n X n X∂Mij ∂q q˙iq˙j− ∂P ∂q
The Lagrangian equations have the following formulation n X j=1 Mkj¨qj+ n X i=1 n X j=1 ∂M kj ∂qi −1 2 ∂Mij ∂qk ˙ qiq˙j+ ∂P ∂qk =ψk k= 1, . . . ,n
By defining the termhkji(q) as hkji(q) = ∂Mkj(q) ∂qi −1 2 ∂Mij(q) ∂qk andgk(q) as gk(q) = ∂P(q) ∂qk the following equations are finally obtained
n X j=1 Mkj(q)¨qj+ n X i=1 n X j=1 hkji(q) ˙qiq˙j+gk(q) =ψk k = 1, . . . ,n
The elements Mkj(q), hijk(q), gk(q) are function of the joint position only, and
therefore their computation is relatively simple once the manipulator’s configuration is known. They have the following physical meaning:
For the acceleration terms:
Mkk is themoment of inertiaabout the k-th joint axis, in a given configuration and
considering blocked all the other joints
Mkj is theinertia coupling, accounting the effect of acceleration of jointjon jointk
For the quadratic velocity terms:
hkjjq˙2j represents thecentrifugal effectinduced on jointk by the velocity of jointj
(notice thathkkk= ∂∂Mqkk k = 0)
hkjiq˙iq˙j represents theCoriolis effectinduced on jointkby the velocities of jointsi
andj
For the configuration-dependent terms:
gkrepresents thetorquegenerated on jointk by the gravity force acting on the
Recalling that the non-conservative forcesψk are in general composed by
τk joint actuator torque
JTF
c
k external (contact) forces
dkk q˙k joint friction torque
the Lagrangian equations n X j=1 Mkj(q)¨qj+ n X i=1 n X j=1 hkji(q) ˙qiq˙j+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 thedynamic model of the manipulator.
The product C(q,q˙) ˙q=Pni=1Pnj=1hkji(q) ˙qiqj˙ is a (n×1) vectorvwhose elements are quadratic functions of the joint velocities ˙qj.
The k-th elementvk of this vector is:
vk = n X
j=1
Ckjq˙j
where the elementsCkj are computed as
Ckj = n X i=1 cijkqi˙ with cijk = 1 2 ∂Mkj ∂qi + ∂Mki ∂qj − ∂Mij ∂qk Christoffel Symbols (4)
The elements of matrixC(q,q˙) are computed as follows. From n X i=1 n X j=1 hkjiq˙iq˙j= n X i=1 n X j=1 ∂Mkj ∂qi −1 2 ∂Mij ∂qk ˙ qiq˙j
by exchanging the sum (i,j) and exploiting the symmetry one obtains
n X i=1 n X j=1 ∂Mkj ∂qi = 1 2 n X i=1 n X j=1 ∂Mkj ∂qi +∂Mki ∂qj and then n X i=1 n X j=1 ∂M kj ∂qi −1 2 ∂Mij ∂qk = n X i=1 n X j=1 1 2 ∂M kj ∂qi +∂Mki ∂qj −∂Mij ∂qk = n X i=1 n X j=1 cijk wherecijk= 12 h∂M kj ∂qi + ∂Mki ∂qj − ∂Mij ∂qk i
are the so-calledChristoffel Symbols.
Since matrixM(q) is symmetric, for a givenk then cijk=cjik.
The elements of matrixC(q,q˙)are then computed as
[C(q,q˙)]k,j=
n X
i=1
cijkq˙i (5)
This is not the only possible expression for matrixC(q,q˙). In general, any matrix such that n X j=1 cijqj˙ = n X j=1 n X k=1 hijkqk˙ qj˙
can be considered. The choice (4) is preferred since in this case the following property is verified.
Property. MatrixN(q,q˙), defined as
N(q,q˙) = ˙M(q,q˙)−2C(q,q˙) (6) in which the elements ofC(q,q˙) are defined as
cijk = 1 2 ∂Mkj ∂qi + ∂Mki ∂qj − ∂Mij ∂qk [C(q,q˙)]k,j= n X i=1 cijkq˙i
In fact, by considering the generic elementnkj, one obtains nkj = d Mkj dt −2[C]kj = n X i=1 ∂Mkj ∂qi −(∂Mkj ∂qi +∂Mki ∂qj −∂Mij ∂qk ) ˙ qi = n X i=1 ∂Mij ∂qk −∂Mki ∂qj ˙ qi
from which it follows (if indicesk andj are exchanged, because of the symmetry ofM(q)) thatnkj =−njk.
Since matrixNis skew-symmetrix, then
xT N(q,q˙)x= 0, ∀x
The condition
xT N(q,q˙)x= 0, ∀x
holds sinceN(q,q˙) is skew-symmetric, due to theparticularchoice of the elements of
matrixC(q,q˙). On the other hand, the condition
˙
qT N(q,q˙) ˙q= 0
holds forany choice of matrixC(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 M¨q=−Cq˙−Dq˙−g(q) +τ−JTF):
1 2 d dt ˙ qTMq˙=1 2q˙ TM˙q˙+ ˙qTM¨q=1 2q˙ TMq˙+ ˙qT(−Cq˙−Dq˙−g(q) +τ−JTF)
Then 1 2q˙ TMq˙ + ˙qT(−Cq˙ −Dq˙−g(q) +τ−JTF) = ˙qT(τ−Dq˙ −g(q)−JTF) from which 1 2q˙ TMq˙ = ˙qTCq˙ =⇒ q˙T( ˙M−2C) ˙q= 0
This equation holds∀q˙ and without any assumption on matrixC( ˙q,q) (it holds also ifCis not based on the Cristoffel 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 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 the dynamic model derived on the basis of the Euler-Lagrangian formulation.
Dynamic model of a pendulum (one dof manipulator).
Consider
θjoint variable,
τ joint torque,
mmass,
Ldistance between center of mass and joint,
d viscous friction coefficient,
I inertia seen at the rotation axis.
Kinetic energy: K = 12Iθ˙2
Potenzial energy: P=mgL(1−cosθ) Lagrangian functionL: L= 12Iθ˙2−mgL(1−cosθ)
Lagrangian function: L= 1 2Iθ˙ 2−mgL(1−cosθ) from which ∂L ∂θ˙ =I ˙ θ, d dt ∂L ∂θ˙ =I ¨ θ, ∂L ∂θ =−mgLsinθ
The generalized Lagrangian force in this case must account for the torque applied to 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θ˙+mgLsinθ=τ
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 through
the CoM and parallel toz;
ai i-th link length;
aCi distance between jointi and the CoM
of the i-th link;
τi torque on joint i;
g gravity force alongy;
Pi, Ki 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 energyK, P)
b) exploiting the particular structure of a manipulator (Jacobian, ...).
We chose as generalized coordinates the joint variablesq1=θ1; q2=θ2. The
kinetic and potential energiesKi andPi are:
link 1: K1= 1 2m1a 2 C1θ˙1 2 +1 2˜I1θ˙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˙ T C2p˙C2+1 2˜I2( ˙θ1+ ˙θ2) 2, P2=m2g(a1S1+aC2S12) where ˙ pT C2p˙C2=a21θ˙1 2 +a2C2( ˙θ1+ ˙θ2)2+ 2a1aC2C2( ˙θ21+ ˙θ1θ˙2)
Therefore, L=K1+K2−P1−P2 and τ1 = [m1a2C1+ ˜I1+m2(a21+a2C2+ 2a1aC2C2) + ˜I2]¨θ1+ +[m2(aC22+a1aC2C2) + ˜I2]¨θ2−m2a1aC2S2(2 ˙θ1θ˙2+ ˙θ22) + +m1gaC1C1+m2g(a1C1+aC2C12) τ2 = [m2(aC22+a1aC2C2) + ˜I2] ¨θ1+ (m2a2C2+ ˜I2)¨θ2+ m2a1aC2S2θ˙21+m2gaC2C12
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:
J1v = −aC1S1 0 aC1C1 0 0 0 J2v = −a1S1−aC2S12 −aC2S12 a1C1+aC2C12 aC2C12 0 0 and J1ω= 0 0 0 0 1 0 J2ω= 0 0 0 0 1 1
In this particular case, the frames associated to link 1 and 2 havezaxes parallel to the same axis ofF0, and therefore it is not necessary to consider the rotation
The kinetic energy is computed as K =1 2q˙ Thm 1J1v TJ1v +m2Jv2TJ2v+J1ωT˜I1J 1 ω+J 2T ω ˜I2J 2 ω i ˙ q being J1T ω ˜I1J 1 ω+J 1T ω ˜I2J 2 ω= ˜ I1+ ˜I2 ˜I2 ˜ I2 ˜I2
The elements of the inertia matrixM(q) are
M11 = m1a2C1+m2(a21+a2C2+ 2a1aC2C2) + ˜I1+ ˜I2
M12 = m2(aC22+a1aC2C2) + ˜I2
M22 = m2a2C2+ ˜I2
From M11 = m1aC21+m2(a21+a2C2+ 2a1aC2C2) + ˜I1+ ˜I2
M12 = m2(a2C2+a1aC2C2) + ˜I2
M22 = m2aC22+ ˜I2
The Christoffel symbols cijk=12 h∂M kj ∂qi + ∂Mki ∂qj − ∂Mij ∂qk i 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 =⇒MatrixC(q,q˙) is C(q,q˙) = hθ˙2 h( ˙θ1+ ˙θ2) −hθ˙1 0
MatrixN(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: 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
Summarizing, fromM(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 [m1aC21+m2(a21+a2C2+ 2a1aC2C2) + ˜I1+ ˜I2]¨θ1+ [m2(aC22+a1aC2C2) + ˜I2]¨θ2 −m2a1aC2S2θ˙22−2m2a1aC2S2θ˙1θ˙2 +(m1aC1+m2a1)gC1+m2gaC2C12 = τ1 [m2(a2C2+a1aC2C2) + ˜I2]¨θ1+ [m2aC22+ ˜I2]¨θ2 +m2a1aC2S2θ˙21 +m2gaC2C12 = τ2 =⇒ Same result!
The Euler-Lagrange dynamic model is characterized by some structural properties, concerning in particular:
1 The inertia matrixM(q); 2 The vectorc(q,q˙) =C(q,q˙)q; 3 The vectorsg(q) and Dq˙;
1. Properties of matrix M(q)
1 M(q)∈IRn×n is symmetric, positive definite and depends on the manipulator
configurationq
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 alsoM−1(q) is bounded 1 µ2 I≤M−1 (q)≤ 1 µ1 I
4 in case of revolute joints, thenµ1, µ2are constant (not function ofq) since the
elements ofM(q) are functions ofsin(qi) orcos(qi)
5 in case of prismatic joints,µ1, µ2may result scalar functions ofq
6 sinceM(q) is bounded, then
M1≤ ||M(q)|| ≤M2
for some properly defined norm (1, 2,p,∞)
2. Properties of vector c(q,q˙) =C(q,q˙) ˙q 1 C(q,q˙) ˙qis a quadratic function of ˙q
2 the generic k-th element of vectorc(q,q˙) =C(q,q˙) ˙qcan also be witten as ck(q,q˙) = q˙TSk(q) ˙q Sk(q) = 1 2 ∂mk ∂q + ∂mk ∂q T −∂M ∂qk ! mk= k-th col. ofM 3 it results that ||C(q,q˙) ˙q|| ≤vb||q˙||2
4 in case of rotative joints, thenvb is constant (not function ofq) since we have only
transcendental functions (sin(qi) orcos(qi))
5 in case of prismatic joints, thenvb may result a scalar function ofq
6 for any choice ofC(q,q˙), then matrixN(q,q˙) = ˙M(q,q˙)−2C(q,q˙) verifies:
˙
qTN(q,q˙) ˙q=0
7 with a proper choice of the elements ofC(q,q˙) (Christoffel symbols), matrix
N(q,q˙) = ˙M(q,q˙)−2C(q,q˙) is skew-symmetric, i.e.
3. Properties of vectors g(q)and Dq˙
1 the friction termDq˙ is bounded:
||Dq˙|| ≤dmax||q˙||
2 the gravity termg(q) is bounded
||g(q)|| ≤gb(q)
3 in case of revolute joints,gb is constant (does not depend onq) sinceqi depends
on sinusoidal functions (sin(qi) orcos(qi))
4 in case of prismatic joints, thengbmay result function ofq.
4. Linearity properties (in the geometrical/mechanical parameters)
The dynamic model of a manipulator:
in general is anon linearfunction ofq,q,˙ ¨qand with dynamic coupling effects
among the joints,
is alinearfunction 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)α=τ
Properties of the dynamic model of a 2 dof manipulator.
Neglecting friction effects we have:
M(q) = m1a2C1+m2(a12+a2C2+ 2a1aC2C2) + ˜I1+ ˜I2 m2(a2C2+a1aC2C2) + ˜I2 m2(aC22+a1aC2C2) + ˜I2 m2a2C2+ ˜I2 C(q,q˙) ˙q=−m2a1aC2S2 2 ˙θ1θ˙2+ ˙θ22 −θ˙2 1 , g(q) = (m1aC1+m2a1)gC1+m2gaC2C12 m2gaC2C12
Consider (for the sake of simplicity) the 1-norm|| · ||1, andθ1, θ2∈[−π/2, π/2].
1. Bounds on the inertia matrix.
The scalar quantitiesµ1, µ2can be defined as the minimum and maximum
eigenvalues (λmin, λmax) ofM(q), ∀q. Computationally, it is easier to define the scalarsM1,M2.
The 1–norm ofM(q) is alway defined on the basis of the first column:
||M(q)||1=|m1a2C1+m2(a21+a2C2+ 2a1aC2C2) +˜I1+˜I2|+|m2(a2C2+a1aC2C2) +˜I2|
which is bounded by
M1 = m1a2C1+m2(a12+ 2a2C2) + ˜I1+ 2˜I2
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 MoreoverN(q,q˙) = ˙M(q,q˙)−2C(q,q˙), (h=−m2a1aC2S2): 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
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 thereforeai,aCi may vary in time), thenvb,gbare functions ofq.
Plots of the eigenvalues ofM(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 lmin = 6.2399 lmax = 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 onq2! These results have been obtained
with:
m1=m2= 50kg; I1=I2= 10; a1=a2= 1; ac1=ac2= 0.5
One obtains
M1= 117,5, M2= 192.5 e kMk1= 192.5, kMk2= 161.26, kMk∞= 192.5
3. Linearity of the dynamic model Y(q,q˙,¨q)α=τ [m1aC21+m2(a21+a2C2+ 2a1aC2C2) + ˜I1+ ˜I2]¨θ1+ [m2(aC22+a1a2C2C2) + ˜I2]¨θ2 −m2a1aC2S2θ˙22−2m2a1aC2S2θ˙1θ˙2 +(m1aC1+m2a1)gC1+m2gaC2C12 = τ1 [m2(a2C2+a1aC2C2) + ˜I2]¨θ1+ [m2aC22+ ˜I2]¨θ2 +m2a1aC2S2θ˙21 +m2gaC2C12 = τ2
By inspection, one can define the parameters vector α= [α1, α2, α3, α4, α5]T with
Then Y(q,q,˙ ¨q) = y11 y12 y13 y14 y15 0 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θ˙12+gC12 y25 = θ¨1+ ¨θ2
The elementsyij depend onq,q˙,¨q,g and ona1.
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α → YT 0τ0=YT0Y0α → α= (YT 0Y0)−1Y0Tτ0=Y0+τ0 Being Y0+= (YT
0Y0)−1YT0 the (left)pseudo-inverseofY0.
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+Dq˙+g(q) =τ+JT(q)Fa
The elements of the inertia matrixM(q) are M11 = m1a2C1+m2(a21+a2C2+ 2a1aC2C2) + ˜I1+ ˜I2 M12 = m2(aC22+a1aC2C2) + ˜I2 M22 = m2a2C2+ ˜I2 MatrixC(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
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);
%%%%% 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
% 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];
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 Posizioni q1, q2 (dash) 0 1 2 3 4 5 6 7 8 9 10 −4 −2 0 2 4 Velocita‘ dq1, dq2 (dash) 0 1 2 3 4 5 6 7 8 9 10 −15 −10 −5 0 5 10 15 Accelerazioni ddq1, ddq2 (dash) 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 Velocita‘ dq1, dq2 (dash) 0 1 2 3 4 5 6 7 8 9 10 −3 −2 −1 0 1 Posizioni q1, q2 (dash) 0 1 2 3 4 5 6 7 8 9 10 −15 −10 −5 0 5 10 15 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)
