2.

Modelos Cl
asicos
Dr. Martn Velasco Villa

Secci
on de Mecatr
onica
Marzo de 2015

Manipulador Cartesiano

Coordenadas generalizadas: q1 , q2
Fuerzas aplicadas: f1 , f2
Masas de los eslabones: m1 , m2
Energa cin
etica: k = 21 q T D (q ) q
Energa potencial: P = P (q )

Matriz de Inercia:
"
D (q ) =

i =1
I

mi JvTi

(q ) Jvi (q ) + JwTi

(q ) Ri (q ) Iii RiT

Como s
olo se tienen uniones prism
aticas: Jwi = 0

( q ) J wi ( q )

Par
ametros DH

Par
ametros D H
Eslab
on a1
i
di
1
0
90 d1
2
0
0
d2

i
0
0

Relaciones b
asicas para la obtenci
on del Jacobiano

Recu
erdese que
vn0 = Jv q,
wn0 = Jw q
donde el Jacobiano del manipulador se obtiene a partir de
 

 
Z0
Jv
Jv1
Jv2
=
J (q ) =
=
J w1 J w2
0
Jw

Z1
0

con Jv , Jw R 3n .
A partir de la cinem
atica del manipulador Jv y Jw se obtienen considerando:

Zi 1 (On Oi 1 )
Uniones Rotacionales
Jvi =
Zi 1
Uniones Prism
aticas
y

Jwi =

Zi 1
0

Uniones Rotacionales
Uniones Prism
aticas

donde,
Zi01 = Ri01 k,

k=

T

Relaciones b
asicas para la obtenci
on del Jacobiano
Recu
erdese que el Jacobiano del manipulador se encuentra a trav
es de las siguientes
relaciones,
Rji = Rii+1 . . . Rjj i
Oji = Oji1 + Rji1 Ojj 1 .
an dadas por,
Las transformaciones homog
eneas Ai , Tji est

Ai =

Rii 1
0

Oii 1
1


,

Tji

que se relacionan entre si mediante,

Tji = Ai +1 Ai +2 . . . Aj 1 Aj =

Rji
0

Oji
1


,

(i < j ) .

En el caso particular del manipulador prism

atico,
T10 = A1 , T20 = A1 A2
 0

 1
R1 O10
R2
A1 =
, A2 =
0
1
0
R20 = R10 R21 .

O21
1

Las matrices Ai tienen la forma general,

Ci Si Ci
S
Ci Ci
i
Ai =
0
Si
0
0

Si Si
Ci Si
Ci
0

ai C i
ai S i
.
di
1

A partir de los parametros D H se obtiene directamente,

1
0
0 0
1 0
0

0
1 0
, A2 = 0 1
A1 =
0 1 0 d1
0 0
0
0
0 1
0 0
con lo cual,

1
0

A1 A2 =
0
0

0
0
1
0

0
1
0
0

0
d2
.
d1
1

0
0
1
0

0
0
,
d2
1

De las matrices, A1 , A1 A2 se obtienen directamente las posiciones de los orgenes con

respecto al marco inercial,

0
0
0

0 , O1 =
0
d2 .
O0 =
, O2 =
0
d1
d1
El Jacobiano que relaciona la velocidad del efector final con las velocidades de las
articulaciones se obtiene entonces al considerar,


Z0 Z1
J (q ) =
,
0
0
donde

0
Z0 = 0 ,
1

Z1 =

R10 Z0

1
= 0
0

0
0
1

0
0
0
1 0 = 1 .
0
1
0

De los desarrollos anteriores,


J (q ) =

Jv
Jw


,

Jw = 0

0
Jv = 0
1

0
1 ,
0

esto es, la velocidad del efector final resulta,


0 0 
q 1
.
vn0 (t ) = 0 1
q 2
1 0
Observaci
on. El Jacobiano anterior se obtiene tambi
en f
acilmente al considerar
directamente la evoluci
on temporal del efector final dada por el punto Pn0 .


0
0 0 
0
P
q 1
n
Pn0 = d2 , P n (t ) =
,
q = 0 1
q 2
q
d1
1 0
donde q1 = d1 y q2 = d2 .

Modelo Din
amico
Para obtener el modelo din
amico del manipulador se tienen que considerar los
jacobianos relacionados con lo centros de masa de cada uno de los eslabones, esto es,
se requieren relaciones de la forma,
vc1 = Jvc1 q,

vc2 = Jvc2 q

donde en este caso,

Jvc1 =

Z0

0
= 0
1

0
0 ,
0

Jvc2 =

Z0

Z1

0
= 0
1

De lo anterior, la energa cin

etica se obtiene en la forma,
o
1 Tn
q
m1 JvTc Jvc1 + m2 JvTc Jvc2 q
1
2
2
 



1 T
1 0
1 0
= q
m1
+ m2
q
0 1
0 0
2


1
m1 + m2
0
= q T
q
0
m2
2

k=

1 T
q D q.

0
1 .
0

La energa potencial del i-

esimo eslab
on se obtiene como,
Pi = mi g T rci
I

mi : Masa del i-
esimo eslab
on.

gi : Vector de gravedad.

rci : Coordenadas del centro de masa.

Entonces,

P=

mi g T rci

i =1

P = m1

0

0 + m1 0
d1

0
d2
d1

= m1 gd1 + m2 gd1
= g (m1 + m2 ) q1 .
I

Como la matriz de inercia D es constante, los smbolos de Christoffel son cero.

Los componentes gk , generados por el vector de gravedad resultan

g1 =

P
= g (m1 + m2 ) ,
q1

g2 =

P
= 0.
q2

Finalmente, el modelo din

amico tiene la forma general
D (q ) q + C (q, q ) q + g (q ) =
Equivalentemente,


m1 + m2
0

0
m2


q +

g (m1 + m2 )
0

esto es,

(m1 + m2 ) q1 + g (m1 + m2 ) = f1
m2 q2 = f2 .

f1
f2

Manipulador Planar Rotacional

Considere un manipulador de la forma,

Par
ametros D H
Eslab
on
ai
i
di
1
a1
0
0
2
a2
0
0

i
1
2

Con lo cual el Jacobiano relacionado con el efector final se obtiene a partir de


 
 

Jv1
Jv2
Z0 (O2 O0 ) Z1 (O2 O1 )
Jv
=
=
J (q ) =
Jw1 Jw2
Z0
Z1
Jw

Recu
erdese que la velocidad angular del i-
esimo eslab
on wii 1 esta dada por la
rotaci
on del i-
esimo eslab
on, expresada relativa al marco Oi 1 xi 1 yi 1 zi 1 . En el
marco i 1 esta velocidad se expresa localmente como
wii 1 = q i Zii11 = q i k
donde k es el vector unitario k =
Por lo tanto, considerando que

Zi01 = Ri01 k,

T

k=

T

la velocidad angular total del efector final se obtiene como

n

wn0 =

i q i zi01 = 1 q 1 k + 2 q 2 R10 k + + n q n Rn01 k

i =1

con i = 1 si la uni
on es rotacional y i = 0 si es prism
atica.

Adem
as se tiene que,
Rji = Rii+j Rjj 1 ,

Oji = Oji1 + Rji1 Ojj 1 ,

(i < j )

donde las transformaciones homog

eneas Ai , Tji se obtienen como,

Ai =

Rii 1
0

Oii 1
1

Tji = Ai +1 Ai +2 . . . Aj =

Rji
0

Oji
1


,

(i < j )

En nuestro caso particular,

T10 = A1 ,

Ci
S

i
Ai =
0
0

Si Ci
Ci Ci
Si
0

T20 = A1 A2
Si Si
Ci Si
Ci
0

ai C i
ai S i
.
di
1

Entonces,

C1
S1
A1 =
0
0

C1
R10 = S1
0

S1
C1
0

a1 C1
 0

0
a1 S 1
= R1 O 1
= T10
0
0
1
1

a1 C1
0
0

a1 S1 ,
0 , O1 =
0
1

S2
C2
0
0

0
0
1
0

S1
C1
0
0

0
0
1
0

C2
S2

A2 =
0
0

C2
R21 = S2
0

S2
C2
0

a2 C2

 1
1
a2 S2
= R2 O 2
= T21

0
0
1
1

0
a2 C2
1
0 , O2 = a2 S2 .
0
1

Por lo tanto,


A1 A2 =

R10 R21

O10

+ R10 O21

C12
S12
=
0
0

S12
C12
0
0

0
0
1
0

a1 C1 + a2 C12
a1 S1 + a2 S12
= T20 .

0
1

Dado que,

J (q ) =

Z0 (O2 O0 )
Z0

Z1 (O2 O1 )
Z1


,

Entonces,

0
Z0 = 0 ,
1

0
O0 = 0 ,
0

Z1 =

R10 k

0
0
0 = 0
1
1

a1 C1 + a2 C12
, O2 = a1 S1 + a2 S12 .
0

R10

a1 C 1
O1 = a1 S1
0

N
otese tambi
en que de manera alterna,
O20 = O10 + R10 O21 ,
entonces,

C1 S1 0
a2 C2
a1 C1
C1
0 a2 S 2
O20 = a1 S1 + S1
0
0
1
0
0

a1 C1
a2 (C1 C2 S1 S2 )
= a1 S1 + a2 (S1 C2 + C1 S2 )
0
0

a1 C1 + a2 C12
= a1 S1 + a2 S12 .
0


 i

 0

 m1

j
0
m2

k
1
m3






=i 0

 m2




 0
1 


j
 m1
m3 

= m2 i + m1j + 0k



 0
1 

+
k
 m1
m3 


0 
m2 

De lo anterior,

0
Z0 (O2 O0 ) = 0
1

0
Z0 (O2 O1 ) = 0
1

0
= 0
1

a1 S1 a2 S12
a1 C1 + a2 C12
a1 S1 + a2 S12 = a1 C1 + a2 C12
0
0

a1 C1 + a2 C12 a1 C1
a1 S1 + a2 S12 a1 S1
0

a2 C12
a2 S12
a2 S12 = a2 C12 .
0
0

El Jacobiano toma entonces la forma = J (q ) q,

a1 S1 + a2 S12
a1 C1 + a2 C12

J (q ) =
0

a2 S12
a2 C12
0
0
0
1

vx
vy
vz
wx
wy
wz


q =

q 1
q 2

Ecuaciones din
amicas o de movimiento

li = a1
q1 = 1

l2 = a2
q2 = 2

Recu
erdese que las ecuaciones de movimiento se obtienen considerando las
energas cin
etica y potencial de cada eslab
on referenciadas a sus centros de
masa.Oc1 , Oc2 .

Entonces las velocidades lineales y angulares pueden expresarse en funci

on de
matrices Jacobianas apropiadas Jvi , Jwi , como,
vi = Jvi (q ) q,

wi = Jwi (q ) q.

Energa Cin
etica

La energa cin
etica tiene la forma,
#
"
o
n n
1
k = q T mi JvTi (q ) Jvi (q ) + JwTi (q ) Ri (q ) Ii RiT (q ) Jwi (q ) q
2
i =1

1 T
q D (q ) q.

Las matrices Jvi , Jwi para i = c1 , c2 se obtiene a partir de sus respectivos Jacobianos
dados por,




z0 (Oc1 O0 ) 0
z0 (Oc2 O0 ) z1 (Oc2 O1 )
Jc1 =
, Jc2 =
z0
0
z0
z1

Jc1 =

Jvc1
J wc 1


,

Jc2 =

Jvc2
Jwc2


.

Tomando en cuenta en principio los efectos traslacionales,




Jvc1 = z0 (Oc1 O0 ) 0 , Jvc2 = z0 (Oc2 O0 )

0 ,
O0 =
0

Oc1

l1 C 1

l1 S1 ,
O1 =
0

lc2 C2
= lc2 S2 .
0

lc1 C1

lc1 S1 ,
=
0

Oc12

z1 (Oc2 O1 )

Oc02 = O10 + R10 Oc12

Entonces,

Oc02

l1 C1
= l1 S1 +
0

l1 C1
= l1 S1 +
0

0
lc2 C2
0 lc2 S2
1
0

lc2 (C1 C2 S1 S2 )
lc2 (S1 C2 C1 S2 ) =
0

C1
S1
0

S1
C1
0

l1 C1 + lc2 C12
l1 S1 + lc2 S12 .
0

Por otra parte,

0
z0 = 0 ,
1

z1 =

R10 k

0
= 0
1

con lo cual,

z0 (Oc2

z0 (Oc1

z1 (Oc2

0
O0 ) = 0
1

0
O0 ) = 0
1

0
O1 ) = 0
1

l1 S1 lc2 S12
l1 C1 + lc2 C12
l1 S1 + lc S12 = l1 C1 + lc C12
2
2
0
0

lc1 C1
lc1 S1
lc S1 = lc C1
1
1
0
0

lc2 S12
lc2 C12
lc S12 = lc C12 .
2
2
0
0

De lo anterior se obtiene entonces,

lc1 S1 0

lc1 C1
0 ,
Jvc1 =
0
0

Jvc2

lc2 S12
lc2 C12 .
0

l1 S1 lc2 S12
= l1 C1 + lc2 C12
0

La energa cin
etica traslacional resulta,
kT =

o
1 Tn
q
m1 JvTc Jvc1 + m2 JvTc Jvc2 q
1
2
2

donde,
JvTc Jvc1 =

JvTc Jvc2 =
2

lc21 S12 + C12

0

0
0

l12 + lc22 + 2l1 lc2 C2

lc22 + l1 lc2 C2

lc21
0

0
0

lc22 + l1 lc2 C2
lc22

Por su parte, la energa cin

etica rotacional se obtiene a partir de
kRi =

o
1 Tn T
q
Jwi (q ) Ri (q ) Ii RiT (q ) Jwi (q ) q
2

donde,
J w1 =

R10

z0

J w2 =

como

z0

z1

0
0
0 = 0
1
1

z1 =

R10 k

con lo cual,

J w1

0
= 0
1

0
0 ,
0

Jw2

0
= 0
1

0
0 .
1

Por otra parte,

R10

C1
= S1
0

S1
C1
0

0
0 ,
1

R20

R10 R21

C12
= S12
0

S12
C12
0

0
0 .
1

Considerando elementos (eslabones) sim

etricos, los tensores de inercia resultan
matrices diagonales en la forma,

I2xx
0
0
I1xx
0
0
I2yy
0 .
I1yy
0 , I2 = 0
I1 = 0
0
0
I2zz
0
0
I1zz
Considerando que,

R1 I1 R1T

R2 I2 R2T

I1xx C12 + I1yy S12

I1xx C1 S1 I1yy C1 S1
0
2
2
= I1xx C1 S1 I1yy C1 S1
I1yy C1 + I1xx S1
0
0
0
Izz

2 +I
2
I2xx C12 S12 I2yy C12 S12
0
I2xx C12
2yy S12
2
2
= I2xx C12 S12 I2yy C12 S21
I2yy C12 + I2xx S12
0 .
0
0
Izz

Se obtiene entonces,
JwT1 R1 I1 R1T Jw1 =
JwT2 R2 I2 R2T Jw2 =

I1zz
0

0
0

I2zz
I2zz

I2zz
I2zz

1
0



1 0
= I1zz
0 0



1 1
.
= I2zz
1 1

Produciendo,
KR =

1 T
q
2


I1zz

0
0

+ I2zz

1
1

1
1


q,

con lo cual la energa cin

etica total K = 21 q T D (q ) q toma la forma,
K =

1 T
q
2


+I1zz


 2
l1 + lc22 + 2l1 lc2 C2
lc21 0
+ m2
lc22 + l1 lc2 C2
0
0



0
1 1
+ I2zz
q.

0
1 1


m1
1
0

lc22 + l1 lc2 C2
lc22

Por lo tanto,



m1 lc21 + m2 l12 + lc22 + 2l1 lc2 cos

q2 + I1zz + I2zz
D (q ) =
2
m2 lc2 + l1 lc2 cos q2 + I2zz


m2 lc22 + l1 lc2 cos q2 + I2zz
.
2
m2 lc2 + I2zz

La matriz de Coriolis y fuerzas centrifugas se obtiene a partir de los smbolos de

Christoffel en la forma,
(
)
dij
dki
1 dkj
Cijk =
+

.
2
qi
qj
qk
Por lo tanto,
1 d11
=0
2 q1

1 d12
= C211 =
2 q1

1 d12
d12
=
+
2 q2
q2

1 d21
d21
=
+
2 q1
q1

1 d22
= C212 =
2 q1

1 d22
d22
=
+
2 q2
q2

C111 =
C121
C221
C112
C122
C222




d11
d12
1 d11
1

=
= 2m2 l1 lc2 sin q2 = m2 l1 lc2 sin q2
q2
q1
2 q2
2

d22
d12

=
= m2 l1 lc2 sin q2
q1
q2

d11
1 d11

=
= m2 l1 lc2 sin q2
q2
2 q2



d21
d12
1 d12
d12
+

=0
q2
q2
2 q2
q2



d22
1 d22

=
= 0.
q2
2 q2

Considerando que,
n

Ckj =

Cijk q i ,

i =1

se obtiene,
2

C11 =

Ci11 q i

= C111 q 1 + C211 q 2 = m2 l1 lc2 sin q2 q 2

i =1
2

C22 =

Ci22 q i

= C122 q 1 + C222 q 2 = 0

i =1
2

C12 =

Ci21 q i

= C121 q 1 + C221 q 2 = m2 l1 lc2 sin q2 q 1 m2 l1 lc2 sin q2 q 2

i =1
2

C21 =

Ci12 q i

i =1

= C112 q 1 + C212 q 2 = m2 l1 lc2 sin q2 q 1 .

Produciendo,

C (q, q ) =

m2 l1 lc2 sin q2 q 1
m2 l1 lc2 sin q2 q 1

m2 l1 lc2 sin q2 q 1 m2 l1 lc2 sin q2 q 2

0

Considerando h (q2 ) = m2 l1 lc2 sin q2 la matriz anterior puede escribirse como,



hq 2 h (q 1 + q 2 )
C (q1 q ) =
.
hq 1
0

Finalmente, la energa potencial de cada eslab

on resulta,
Pi = mi g T Oci
esto es,

P=

Pi

i =1

mi g T Oci .

i =1

Entonces,

P1 = m1 g

P2 = m2 g

lc1 C1
lc S1 = m1 glc sin q1
1
1
0

lc1 C1 + lc2 C12


0 lc1 S1 + lc2 S12 = m2 g (l1 S1 + lc2 S12 ) ,
0
0

por lo tanto
P = m1 glc1 sin q1 + m2 g (l1 sin q1 + lc2 sin (q1 + q2 ))

= m1 glc1 S1 + m2 g (l1 S1 + lc2 S12 ) .

De lo anterior,
g1 =

P
= m1 glc1 C1 + m2 g (l1 C1 + lc2 C12 )
q1

= (m1 lc1 + m2 l1 ) g cos q1 + m2 glc2 cos (q1 + q2 )

g2 =

P
= m2 glc2 C12 = m2 lc2 g cos (q1 + q2 ) .
q2

Obteni
endose el modelo din
amico,

D (q ) q + C (q1 q ) q +

g1
g2

1
2


.

N
otese que



hq 2
hq 1

hq 1 q 2 hq 1 q 2 hq 22
hq 12

C (q1 q ) q =

h (q 1 + q 2 )
0

 

h (q 1 q 2 ) h (q 1 + q 2 ) q 2
q 1
=
2
q 2
hq 1

 
2hq 1 q 2 hq 22
=
.
hq 12