Dinámica de Robots IME8VC Unidad 2

Por Francisco Javier Ramírez Ruiz EA2021

G
Ii i

Ji i
J vi
i
J i i
K L = K −U
M
M i, j (i, j ) M
n
k *
p ci i − ésimo
k − ésimo

Qi i − ésimas

Q Q = Q1 , Q2 , , Qn 
T

qi
q =  q1 , q2 , , qn 
q T

U
V
W

L = K −U

d  L  L
 − = Qi i = 1, 2, ,n
dt  qi  qi

1
Dinámica de Robots IME8VC Unidad 2
Por Francisco Javier Ramírez Ruiz EA2021

f (x1 , x 2 , t) = 0

xi

q=  q1 , q2 , , qn 
T

2
Dinámica de Robots IME8VC Unidad 2
Por Francisco Javier Ramírez Ruiz EA2021

qi
Qi
QiT qi

i
1 T 1
Ki = v ci mi vci + iT I ii
2 2

I i = 0 Ri i I i ( 0 Ri )
T

x ci = J i q

v  J 
x ci =  ci  y J i =  vi 
 i   J i 

 j −1 *
 z j −1 x pci para una articulación rotacional
J =
j
vi

z j −1 para una articulación prismática

z para una articulación rotacional

J ji =  j −1
0 para una articulación prismática

j −1
p*ci
j −1 i

3
Dinámica de Robots IME8VC Unidad 2
Por Francisco Javier Ramírez Ruiz EA2021

K=
1 n T

2 i =1
( v ci mi v ci + iT I ii )

1 n 
=  ( J vi q ) mi ( J vi q ) + ( J vi q ) I i ( J vi q ) 
T T

2 i =1  
1 T n 
= q   ( J viT mi J vi + J Ti I i J i )  q
2  i =1 

nxn

M =  ( J viT mi J vi + J Ti I i J i )
n

i =1

1
K = qT Mq
2

i
i

i pci
T
-mi g p ci
n
U = − mi gT p ci
i =1

n
1 T
L= q Mq +  mi gT p ci
2 i =1

4
Dinámica de Robots IME8VC Unidad 2
Por Francisco Javier Ramírez Ruiz EA2021

qi , qi y t

M i, j ( i, j )
M
1 n m n
L= 
2 i =1 j =1
M i. j qi q j +  mi gT pci
i =1

qi
qi

L n
=  M ij q j
qi j =1

n M
d  L  n n
 dM ij  n n

  ij j  
= +  j  ij j 
= +
ij
 M q q M q qk q j
dt  q i  j =1 j =1  dt  j =1 j =1 k =1 q k

qi

L 1   n n  n T 
pcj 
=   jk j k  +  m j g 
M q q 
q i 2 q i  j =1 k =1  j =1  q i 

p cj qi i − ésima
J vj

L 1 n n M jk n
=  q j q k +  m j gT J vji
q i 2 j =1 k =1 q i j =1

M
j =1
ij q j + Vi + Gi = Qi para i = 1, 2, ,n

5
Dinámica de Robots IME8VC Unidad 2
Por Francisco Javier Ramírez Ruiz EA2021

n n
 M ij 1 M jk 
Vi =   −  q jqk
j =1 k =1  q k 2 q i 
n
Gi = − m j gT J vji
j =1


n
j =1
M ij q j

M q+V+G =Q

V =  V1 , , Vn  G = G1 , , Gn  Q = Q1 , ,Qn 

T T T

V
G

M
M

6
Dinámica de Robots IME8VC Unidad 2
Por Francisco Javier Ramírez Ruiz EA2021

Ejemplo: Calculo del Modelo Dinámico Manipulador de 2 GDL

Cinemática Directa DH

Eslabón i di ai i
1 1 0 l1 0
2 2 0 l2 0

Ci −C i Si S i Si ai Ci 

 S C i Ci − S i Ci ai Si 
i −1
Ai =  i
 0 S i C i di 
 
 0 0 0 1 

C1 − S1 0 l1C1  C2 − S2 0 l2C2 

S C1 
0 l1S1  1 S C2 0 l2 S2 
0
A1 =  1 ; A2 = 
2

0 0 1 0  0 0 1 0 
   
0 0 0 1  0 0 0 1 

7
Dinámica de Robots IME8VC Unidad 2
Por Francisco Javier Ramírez Ruiz EA2021

C1 − S1 0 l1C1  C2 −S2 0 l2C2  C12 − S12 0 l2C12 + l1C1 

S C1 0 l1S1   S2 C2 0 l2 S2   S12 C12 0 l2 S12 + l1S1 
0
A2 = 0 A1 1 A2 =  1 =
0 0 1 0  0 0 1 0   0 0 1 0 
    
0 0 0 1  0 0 0 1   0 0 0 1 

Matrices de Rotación

C1 − S1 0  C12 − S12 0

0
R1 =  S1 C1 0  0
R2 =  S12 C12 0 
 0 0 1   0 0 1 

a) Matriz de Inercia

m 2 2 
12 (b + c ) 0 0 
 
I=  0
m 2 2
(a + c ) 0 
 12 
 m 2 
 0 0 (a + b ) 
2

 12 

8
Dinámica de Robots IME8VC Unidad 2
Por Francisco Javier Ramírez Ruiz EA2021

b c,
a,

2
0 0 0 
I i = i i 0 1 0 
m a
i = 1, 2
i

12
0 0 1 

i − ésimo

I i = 0 Ri i I i ( 0 Ri )
T

i =1

0 0 0 
m1l12 
1
I1 =  0 1 0 
12
0 0 1 

I1 = 0 R1 1I1 ( 0 R1 )
T

   m1l12 S1 
 0 − 0 
0 0 0  12
− S1 − S1 0     C1 S1 0 
T
C1 0   C1
2
 m1l12C1 
I1 =  S1 0  0 0   S1 C1 0  0   − S1 C1 0 
ml
C1 11
= 0
 12  12
 0 1   2 0 1    0 0 1 
m1l1   m1l12  
0 0
0 
0 0 0
 12   12 
 m1l12 S12 m1l12 S1C1 
 − 0 
 12 12   S12 − S1C1 0 
 m l 2S C m1l12C12  1  
I1 =  − 1 1 1 1 0  = m1l12  − S1C1 C12 0
12 12 12
   0 0 1 
 m1l1 
2 
 0 0
 12 

i=2

0 0 0
m2l22 
2
I2 =  0 1 0 
12
0 0 1 

9
Dinámica de Robots IME8VC Unidad 2
Por Francisco Javier Ramírez Ruiz EA2021

I 2 = 0 R2 2 I 2 ( 0 R2 )
T

   m2l22 S12 
 0 − 0 
0 0 0  12
− S12 − S12    C12
T
C12 0   C12 0 S12 0
m2l22  m2l22C12 
I 2 =  S12 C12 0  0 0   S12 C12 0  = 0 0   − S12 C12 0 
 12  12
 0 1   2 1    0 1 
m2l2   m2l22  
0 0 0 0
0 
0 0 0
 12   12 
 m2l22 S122 m2l22 S12C12 
 − 0 
 12 12   S122 − S12C12 0
 m l 2S C m2l22C122  1  
I 2 =  − 2 2 12 12 0  = m2l22  − S12C12 C 2
12 0
12 12 12
   0 0 1 
 m2l2 
2 
 0 0
 12 

b) Matrices Jacobianas de los Eslabones

Vectores de posición del centro de masa de los eslabones 1 y 2 se determinan con respecto al
sistema de coordenadas de la articulación j − 1 al eslabón i , pero referidos a la base del robot:

 l1C1 
 2 
 
p c1 =
0 *  l1S1 
 2 
 0 
 
 

10
Dinámica de Robots IME8VC Unidad 2
Por Francisco Javier Ramírez Ruiz EA2021

 l2C12 
 2 
 
p c 2 =  2 12 
1 * lS
 2 
 0 
 
 

 l2C12 
 2 + l1C1 
 
pc 2 =
0 *  l2 S12
+ l 1 S1 
 2 
 0 
 
 

 j −1 *
 z x pci para una articulación rotacional
J vij =  j −1

z j −1 para una articulación prismática

z para una articulación rotacional

J ji =  j −1
0 para una articulación prismática

i j

i =1 j =1 j=2

 l1C1 
 
0   2  i j k
1 0 *  
J v1 = z0 x pc1 = 0  x  l1S1 
= 0 0 1
 2 
1    l1C1 l1S1
 0  2 2
0
 

11
Dinámica de Robots IME8VC Unidad 2
Por Francisco Javier Ramírez Ruiz EA2021

0 1 0 1 0 0
l1S1 l1C1
J = l1S1
1
v1 i − l1C1 j + l1C1 l1S1 k = − i+ j + 0k
0 0 2 2
2 2 2 2
 l1S1 
− 2 
 
J v1 =
1  l1C1 
 2 
 0 
 
 

 l1S1 
− 2 0
 
0 =  1 1 0
lC
J v1 =  J v11 
 2 
 0 0 

 

0
J 1 = z0 =  0 
1

1 

0 0 
J 1 =  J 1 1
0  = 0 0 
1 0 

i=2 j =1 j=2

 l2C12 
 2 + l1C1 
0    i j k
1 0 *  
J v 2 = z0 x pc 2 = 0  x  l2 S12 
+ l 1 S1 = 0 0 1
 2 
1    l2C12 l2 S12
 0  + l1C + l 1 S1 0
  2 2

0 1 0 1
l S  l C 
J = l2 S12
1
i − l2C12 j + 0k = −  2 12 + l 1 S1  i +  2 12 + l1C1  j + 0k
+ l 1 S1 0 + l1C1 0
v2
 2   2 
2 2

12
Dinámica de Robots IME8VC Unidad 2
Por Francisco Javier Ramírez Ruiz EA2021

 l2 S12 
 − 2 − l 1 S1 
 
Jv2 =
1  l2C12
+ l1C1 
 2 
 0 
 
 

 l1C12 
 
0   2  i j k 0 1 0 1
2 1 *  
J v 2 = z1 x pc 2 = 0  x  1 12  = 0
l S
0 1 = l 1 S12 i − l1C12 j + 0k
 2  0 0
1    l1C12 l 1 S12 2 2
 0  2 2
0
 
 l 1 S12 
− 2 
 
J v22 = −
l 1 S12 lC
i + 1 12 j + 0k =  l1C12 
2 2  2 
 0 
 
 

 l2 S12 l2 S12 
 − 2 − l 1 S1 −
2 
 
J v 2  =  2 12 + l 1 C1
lC l2C12 
J v 2 =  J v12 2
 2 2 
 0 0 

 

0 
1
J 2 = z0 = 0 
1 
0 
J 2 2 = z1 = 0 
1 

0 0 
J  2 =  J  2
1
J  2  = 0 0 
2

1 1 

13
Dinámica de Robots IME8VC Unidad 2
Por Francisco Javier Ramírez Ruiz EA2021

c) Matrices de Inercia del Manipulador

M =  ( J viT mi J vi + J Ti I i J i )
n

i =1

n Es el número de grados de libertad

n=2

M =  ( J viT mi J vi + J Ti I i J i )
2

i =1

M = J vT1m1J v1 + JT1I1J1 + J vT2m2 J v 2 + JT 2 I 2 J 2

 l1S1 
T
 l1S1   m1l12 S12 m1l12 S1C1 
−
− 2 − 2 
0 0 0 
12 12
 0 0    0 0 
T
  
 m l 2S C m1l12C12 
M = 1 1 0 m1  1 1
 0  + 0 0   − 1 1 1 1 0  0 0 
lC lC
 2   2  12 12
 0   0 1 0    1 0 
 0  0    m1l12  
 0 0
     12 
T
 l2 S12 l2 S12   l2 S12 l2 S12 
 − 2 − l 1 S1 −
2   − 2 − l 1 S1 −
2 
   
+  2 12 + l 1 C1
l2C12 
m 2  2 12 + l 1 C1
lC lC l2C12 
 2 2   2 2 
 0 0    0 0 
 
   
 m2l22 S122 m2l22 S12C12 
 − 0 
T
 12 12  0 0 
0 0 
 m2l22 S12C12 m2l22C122 
+ 0 0  − 0  0 0 
12 12
1 1    1 1 
 m2l22  
 0 0
 12 

14
Dinámica de Robots IME8VC Unidad 2
Por Francisco Javier Ramírez Ruiz EA2021

 m1l1S1 
− 2 0  
 l1S1 l1C1    0 0
− 0   m1l1C1  0 0 1   
M = 2 0 + 
 0 0 0  
2  0 0
  2  
0 
 0 0 
0 2

 0  m1l1 0
   12 

 m2l2 S12 l2 S12 

− − m2l 1 S1 −m2
 l2 S12 l2C12  2 2 
 − 2 − l 1 S1 + l 1 C1 0  

2 m2l2C12 m 2l2C12 
+ + m2l 1 C1
 − l2 S12 l2C12   2 2 
0  
 2 2  0 0
 
 
 
 0 0 
0 0 1  
+  0 0 
0 0 1  2
 m2l2 m2l22 
 12 12 

 m1l12 S12 m1l12C12  m l2 

+ 0  1 1
M = 4
0
4 +
   12 
 0 0   0 0 
 m2l22 S122 m2l1l2 S1S12 m2l1l2 S1S12 m l 2C 2 m l l C C m ll CC m2l22 S122 m2l1l2 S1S12 m2l 22C122 m2l 1l 2C1C12 
 + + + m2l12 S12 + 2 2 12 + 2 1 2 1 12 + 2 1 2 1 12 + m2l12C12 + + + 
4 2 2 4 2 2 4 2 4 2
+ 
 m2l22 S122 m2l1l2 S1S12 m2l22C122 m2l1l2C1C12 2 2 2 2
m2l2 S12 m2l2 C12 
 + + + + 
4 2 4 2 4 4
 m2l22 m2l22 
 
12 12 
+ 2
 m2l2 m2l2 
2

 12 12 

15
 Dinámica de Robots IME8VC Unidad 2
Por Francisco Javier Ramírez Ruiz EA2021

 m l2 m l2 m2l22 
 m1l12 2   11 + 2 2
M = 4
( S1 + C1 ) 0 +  12 12
2
12 

   m l2 m2l2 
2
 0 0   2 2

 12 12 
 m2l22 2 m2l22 2 

4
( S12 + C122 ) + m2l1l2 S1S12 + m2l12 ( S12 + C12 ) + m2l1l2C1C12 4
( S12 + C122 ) + 2 1 2 ( S1S12 + C1C12 ) 
m ll
2
+ 
 m2l22 2 2

 4
( S12 + C122 ) + m22l1l2 ( S1S12 + C1C12 ) m2l2 2
4
( S12 + C12 )
2


S12 + C12 = 1 sen 2 q1 + cos 2 q1 = 1

S12 = S1C2 + C1S 2

C12 = C1C2 − S1S 2

S1S12 + C1C12 = S1  S1C2 + C1S2  + C1 C1C2 − S1S2  = S12C2 + S1C1S2 + C12C2 − C1S1S 2
= C2 ( S12 + C12 ) = C2

 m1l12 m2l22 m2l22 

 m1l12 2  +

M= 4
( S1 + C12 ) 0   12
+  12 12 

   m l2 m2l22 
 0 0   2 2

 12 12 
 m2l22 2 m2l22 2 

4
( S12 + C122 ) + m2l1l2 S1S12 + m2l12 ( S12 + C12 ) + m2l1l2C1C12 4
( S12 + C122 ) + m22l1l2 ( S1S12 + C1C12 )
+ 
 m2l22 2 m2l22 2 
 4
( S12 + C122 ) + 2 1 2 ( S1S12 + C1C12 )
m ll
2 4
( S12 + C12 )
2

 m1l12 m2l22 m2l22   m2l22 m2l22 m2l1l2C2 
 +   + m2l1l2 ( S1S12 + C1C12 ) + m2l12 + 
3 12 12   4 4 2 
M=  +
 m2l22 m2l22   m2l22 m2l1l2C2 m2l22 
 +
12 12   4 2 4 
 m1l12 m2l22 m2l22   m2l22 m2l22 m2l1l2C2 
 +  + m2l1l2C2 + m2l12 +
3 12 12   4 4 2 

M = +
 m2l2 2
m2l22   m2l22 m2l1l2C2 m2l22

 +
12 12   4 2 4 

16
Dinámica de Robots IME8VC Unidad 2
Por Francisco Javier Ramírez Ruiz EA2021

1 1 3 1 4 1
+ = + = =
4 12 12 12 12 3

 m1l12 m2l22 m2l22 m2l1l2C2 

 + m l
2 1
2
+ + m2l1l2C2 + 
3 3 3 2 
M =
 m2l22 m2l1l2C2 m2l22 
 + 
3 2 3
 m1l12  2 l22   l1l2C2 l22  
 + m2 1l + l l C
1 2 2 +  m2 + 
3  3   2 3 
M =
  l l C l2  m2l22 
 m2  1 2 2 + 2  
  2 3 3 

C2 = cos ( q2 ) S2 = sen(q2 )

C1 = cos ( q1 ) S1 = sen(q1 )

d) Vector de Velocidad de Acoplamiento

M M 12 
M =  11
 M 21 M 22 

n n
 M ij 1 M jk 
Vi =   −  q jqk
j =1 k =1  q k 2 q i 

i =1
2 2
 M 1 j 1 M jk 
V1 =   −  q jqk
j =1 k =1  q k 2 q1 

 M 11 1 M 11   M 11 1 M 12   M 12 1 M 21   M 12 1 M 22 
V1 =  −  q1q1 +  −  q1q 2 +  −  q 2q1 +  −  q 2q 2
 q1 2 q1   q 2 2 q1   q1 2 q1   q 2 2 q1 
 M 11 1 M 11  2  M 11 1 M 12   M 12 1 M 21   M 12 1 M 22  2
V1 =  −  q1 +  −  q1q 2 +  −  q 2 q1 +  −  q2
 q1 2 q1   q 2 2 q1   q1 2 q1   q 2 2 q1 

i=2
2 2
 M 2 j 1 M jk 
V2 =   −  q jqk
j =1 k =1  q k 2 q 2 

17
Dinámica de Robots IME8VC Unidad 2
Por Francisco Javier Ramírez Ruiz EA2021

e) Vector Gravitacional
n
Gi = − m j gT J vji
j =1

i =1
2
G1 = − m j gT J vj1
j =1

i=2
2
G2 = − m j gT J vj2
j =1

f) Ecuaciones de Movimiento de Lagrange

 = M q+V+G

V =  V1 , , Vn  G = G1 , , Gn  Q = Q1 , ,Qn 

T T T

18