UNIVERSIDAD TECNOLÓGICA DEL PERU

ASIGNATURA: CINEMATICA Y DINAMICA DE ROBOTS

SECCION: 15007

DOCENTE: Ing. MACHUCA MINES, José Ambrosio

INFORME: PRACTICA Y LABORATORIO CALIFICADO N°3

TEMA: DINAMICA DE UN ROBOT DE 4 GRADOS DE

LIBERTAD DEL TIPO RRPR”

ALUMNOS:

• AGUILAR HEREDIA, Emmanuel 1510135

• ARANCIBIA CARRASCO, Eduardo 0521191
• PILLHUAMAN ARAUCO, Gilbert 1524683
• SOLORZANO VILCA KENNY G. 1021030

Lima, 17 de noviembre del 2021

 i

Índice
1. INTRODUCCIÓN .................................................................................................... 1

2. DINÁMICA DEL ROBOT ....................................................................................... 2

2.1. Cinemática del Robot ........................................................................................ 2

2.2. Variables involucradas para la resolución ......................................................... 4

Centros de masa .......................................................................................................... 4

Tensores de Inercia ..................................................................................................... 6

Tensores de Inercia con respecto a la base de referencia x0 , y0 , z0 ........................... 7

Vectores de la posición del centro de masa de cada eslabón con respecto a x0 , y0 , z0

................................................................................................................................................. 9

Matriz Jacobiana ....................................................................................................... 10

Tensor inercial .......................................................................................................... 12

2.3. Energía cinética ............................................................................................... 15

2.4. Energía potencial ............................................................................................. 15

La energía potencial del robot está dada por: ............................................................... 15

2.5. Lagrange .......................................................................................................... 15

2.6. Dinámica del robot .......................................................................................... 16

Forma matricial ......................................................................................................... 25

Simulaciones en Matlab ............................................................................................ 27

Código fuente (MATLAB) ....................................................................................... 28

 ii

3. CONCLUSIONES Y OBSERVACIONES ............................................................ 35

 iii

Índice de Figuras

Figura 1. Dibujo del robot RRPR propuesto ....................................................................... 2

Figura 2. Representación del eslabón en dos cilindros solidos ........................................... 4

Figura 3. Representación de los eslabones 2 y 3 ................................................................ 5

Índice de Tablas
Tabla 1. Parámetros de D-H para el robot propuesto........................................................ 2
 1. INTRODUCCIÓN

La dinámica se ocupa de la relación entre las fuerzas que actúan sobre un cuerpo y el

movimiento que en él se origina. Por tanto, el modelo dinámico de un robot tiene por objetivo

conocer la relación entre el movimiento del robot y las fuerzas implicadas en el mismo. Esta

relación se obtiene mediante el denominado modelo dinámico, que establece la relación

matemática entre: La localización del robot definida por sus variables articulares o por las

coordenadas de localización de su extremo, y sus derivadas: velocidad y aceleración; las fuerzas y

pares aplicados en las articulaciones (o en el extremo del robot) y los parámetros dimensionales

del robot, como longitud, masas e inercias de sus elementos.

El problema de la obtención del modelo dinámico de un robot es, por tanto, uno de los

aspectos más complejos de la robótica, lo que ha llevado a ser obviado en numerosas ocasiones.

Es importante hacer notar que el modelo dinámico completo de un robot debe incluir no

sólo la dinámica de sus elementos (barras o eslabones) sino también la propia de sus

sistemas de transmisión, de los actuadores y sus equipos electrónicos de mando. Estos elementos

incorporan al modelo dinámico nuevas inercias, rozamientos, saturaciones de los circuitos

electrónicos, etc., aumentando aún más su complejidad.

 2

2. DINÁMICA DEL ROBOT

2.1. Cinemática del Robot

Para obtener la dinámica del robot es necesario tener en cuenta la cinemática del robot, el

robot propuesto anteriormente es un robot RRPR, el cual consiste en dos articulaciones

rotaciones, luego una prismática y finalmente una articulación rotacional, Figura 1.

Figura 1. Dibujo del robot RRPR propuesto

Y los parámetros de Denavith – Hartenberg son:

Tabla 1. Parámetros de D-H para el robot propuesto

i ai αi di θi
1 l2 90° l1 θ1
2 0 -90° 0 θ2
3 0 -90° d3 180°
4 l3 0 0 θ4
 3

Con los parámetros de D-H encontramos las ecuaciones matriciales de cada articulación:

C1 0 S1 l2C1  C2 0 − S2 0

S 0 −C1 l2 S1  S 0 C2 0
T 1
=  1 T 2
=  2
0
0 1 0 l1 
1
0 −1 0 0
   
0 0 0 1  0 0 0 1

1 0 0
0 C4 − S4 0 l4C4 
0 0 −1 0  S C4 0 l4 S4 
2T =
3  3T =
4  4
0 1 0 d3  0 0 1 0 
   
0 0 0 1 0 0 0 1 

Matriz de transformación de cada articulación con respecto al sistema de referencia:

C2C1 − S1 −C1S 2 l2C1 

C S C1 − S1S 2 l2 S1 
0T = 0T 1T =
2 1 2  2 1
 S2 0 C2 l1 
 
 0 0 0 1 

 −C2C1 C1S2 S1 l2C1 − d3C1S 2 

 −C S S S −C1 l2 S1 − d3 S1S 2 
T 3
= T 2
T 3
=  2 1 1 2
0 0 2
 − S2 −C2 0 l1 + d3C2 
 
 0 0 0 1 

Así pues, se puede calcular la matriz T que indica la localización del sistema final con

respecto al sistema de referencia de la base del robot.

−C24C1 S 24C1 S1 −C1 ( l4C24 − l2 + d 3 S 2 ) 

 
 −C24 S1 S 24 S1 −C1 − S1 ( l4C24 − l2 + d3 S 2 ) 
0T = 0T 1T 2T 3T =
4 1 2 3 4
 − S24 −C24 0 l1 − l4 S 24 + d3C2 
 
 0 0 0 1 

Donde la posición del robot es:

 −C1 ( l4C24 − l2 + d3 S2 ) 
 
P =  − S1 ( l4C24 − l2 + d3 S2 ) 
 l1 − l4 S24 + d3C2 
 4

Con lo que:

x = −C1 ( l4C24 − l2 + d 3 S 2 )
y = − S1 ( l4C24 − l2 + d3 S 2 )
z = l1 − l4 S24 + C2 d3

2.2. Variables involucradas para la resolución

Centros de masa

Los momentos de inercia de masas deben estar en el centro de masa de cada eslabón, por

lo que es importante saber el centro de inercia de cada eslabón que esta ubicado con su eje de

referencia, es decir ci debe estar en referencia del sistema xi , yi , zi .

Para los eslabones 2,3 y 4 las masas son uniformes con el sistema referencial, sin

embargo, el eslabón 1 no es uniforme, por lo que calcularemos su centro de masa. Para facilitar

el cálculo, sin dejar de considerar el radio del eslabón, simplificamos el eslabón con dos cilindros

de radio R1 ambos.
l2
x1

y1
c1

l1

Figura 2. Representación del eslabón en dos cilindros solidos

En la Figura 2 se puede observar las representaciones del centro de masa del eslabón.

Debido a que la distribución de la masa y volumen con uniformes, el centro de masa y el centroide

coinciden entre sí. Además, se puede encontrar el centroide de área ya que el eslabón esta

solamente en el plano XY. Las fórmulas correspondientes son:

 xi Ai  yi Ai
x= y=
 Ai  Ai
 5

Las áreas de ambas partes del eslabón son: A1 = 2 R1 ( l1 − R1 ) y A2 = 2 R1 ( l2 + R1 ) . Y los

l
centros de cada área con respecto al sistema de referencia 1 son: x1 = −l2 , y1 = − R1 − 1 ,
2

− ( l2 + R1 )
x2 = y y2 = 0 .
2

Reemplazando estos valores se tiene:

( l2 + R1 )
−l2 2 R1 ( l1 − R1 ) − 2 R1 ( l2 + R1 ) 2l2 ( l1 − R1 ) + ( l2 + R1 )
2
2 l2 2 + 2l1l2 + R12
x1 = =− =−
2 R1 ( l1 − R1 ) + 2 R1 ( l2 + R1 ) 2 ( l1 + l2 ) 2 ( l1 + l2 )

 l 
−  R1 + 1  2 R1 ( l1 − R1 )
y1 =  2
=−
( l12 − R12 )
2 R1 ( l1 − R1 ) + 2 R1 ( l2 + R1 ) 2 ( l1 + l2 )

Entonces se tienen los centros de masa:

 − ( l2 2 + 2l1l2 + R12 ) / 2 ( l1 + l2 )   0   0   −l4 / 2 

       
c1 =  R12 − l12 / 2 ( l1 + l2 )  , c2 =  0  , c3 = l3 / 2  , c4 =  0 
 0  l3 / 2   0   0 
 

d3
l3

l3

Figura 3. Representación de los eslabones 2 y 3

Los valores l3 y l3 corresponden a las longitudes de los eslabones 2 y 3, y se debe

recordar que d 3 es la posición de la articulación del sistema de referencia {2} al {3}.

 6

Tensores de Inercia

El tensor de inercia de un sólido está dado por la siguiente expresión:

 I xx I xy I xz 
 
I =  I yx I yy I yz 
 I zx I zy I zz 


Los tensores de inercial deben estar ubicados en el centro de masa de cada eslabón,

entonces encontraremos el tensor de inercia de cada eslabón.

Como se vio anteriormente en los centros de masa los eslabones 2, 3 y 4 son uniformes

así que calcularemos paso a paso los tensores de inercia del eslabón 1.

Considerando la Figura 2, los momentos de inercia son:

( l + R1 ) 
2

1
12
( 2
)
I xx ,1 = m1 3R12 + ( l1 − R1 ) + m1 ( x1 − l2 ) + m1R12 + m1 x1 − 2
2 1
2  2



(l + R )  1
2

1
2  2  12
(
I yy ,1 = m1R12 + m1  y1 − 1 1  + m1 3R12 + ( l2 + R1 ) + m1y 2
2
)

I zz ,1 =
1
12
( ) 1
(
m1 3R12 + ( l1 − R1 ) + m1 3R12 + ( l2 + R1 )
2

12
2
)
 ( l + R1 )  + m x − ( l2 + R1 )  y
I xy ,1 = I yx ,1 = m1 ( x1 − l2 )  y1 − 1  1 1  1
 2   2 

I yz ,1 = I zx,1 = 0

Donde m1 y m1 corresponden a las masas correspondientes de la separación del eslabón,

es decir m1 + m1 = m1 , para encontrar estas masas se puede relacionar con la longitud de estas de

m1 l1
la siguiente manera:  = k . Por lo que, m = km1 , usando esta relación se encuentran estas
m1 l2

m1 k
masas y son: m1 = y m1 = m1 .
k +1 k +1
 7

Entonces los tensores de inercias de los eslabones son:

 I xx ,1 − I xy ,1 0 
 
I1 =  − I yx ,1 I yy ,1 0 
 0 0 I zz ,1 

1 
12 m2 ( 6 R2 + l 3 )
2 2
0 0 
   I xx ,2 0 0 
 
 m2 ( 6 R2 + l 3 ) 0 = 0
1
I2 = 0 2 2
I yy ,2 0 
 12 
 2  0 0 I zz ,2 
 0 0 m2 R2 
 

1 
12 m3 ( 3R3 + l 3 )
2 2
0 0 
   I xx ,3 0 0 
I3 =  0
1
m3 R32 0 = 0 I yy ,3 0 

 2  
   0 0 I zz ,3 
m3 ( 3R3 + l 3 ) 
 1 2 2
0 0
 12 

1 2 
 2 m4 R4 0 0 
   I xx ,4 0 0 
I4 =  0
1
m4 ( 3R4 2 + l4 2 ) 0 = 0 I yy ,4 0 

 12  
   0 0 I zz ,4 
m4 ( 3R4 + l4 ) 
 0 1 2 2
0
 12 

Los tensores de inercia de los eslabones 3 y 4 corresponden a un cilindro solido de radios

R3 y R4. El tensor de inercia del eslabón dos corresponde a un cilindro con cascarón delgado de

Radio R2.

Tensores de Inercia con respecto a la base de referencia x0 , y0 , z0

 8

C1 0 S1   I xx ,1 − I xy ,1 0  C1 S1 0
 
D1 (q) = 0 R I1 1 R =  S1 0 −C1   − I xy ,1
1 0
I yy ,1 0   0 0 1 
 0 1 0   0 0 I zz ,1   S1 −C1 0 

 I xx ,1C12 + I zz ,1S12

(I xx ,1 − I zz ,1 ) C1S1 − I xy ,1C1 

D1 (q) = ( I xx ,1 − I zz ,1 ) C1S1 I zz ,1C12 + I xx ,1S12 − I xy ,1S1 
 
 − I xy ,1C1 − I xy ,1S1 I yy ,1 
 

C1C2 − S1 −C1S2   I xx,2 0 0   C1C2 S1C2 S2 

 
D2 (q) = 0 R I 2 2 R =  S1C2
2 0
C1 − S1S2   0 I yy ,2 0   − S1 C1 0  =
 S2 0 C2   0 0 I zz ,2   −C1S2 − S1S2 C2 

 ( I xx ,2C2 2 + I zz ,2 S 2 2 ) C12 + I yy ,2 S12


(I xx ,2 − I yy ,2 − ( I xx,2 − I zz ,2 ) S2 2 C1S1 ) (I xx ,2 − I zz,2 ) C1C2 S2 

(
 I − I −(I − I )S 2 C S
 xx ,2 yy ,2 xx ,2 zz ,2 2 ) 1 1 (I xx ,2 C2 2 + I zz ,2 S2 2 ) S12 + I yy ,2C12 (I xx ,2 − I zz ,2 ) 1 2 2
S C S 


 ( I xx,2 − I zz,2 ) C1C2 S2 (I xx ,2 − I zz ,2 ) S1C2 S2 I zz ,2 + ( I xx,2 − I zz ,2 ) S2 2


 −C1C2 C1S 22 S1   I xx ,3 0 0   −C1C2 − S1C2 −S2 

 
D3 (q) = 0 R I 3 3 R =  − S1C
3 0
S1S2 −C1   0 I yy ,3 0   C1S2 S1S 2 −C2 
 − S2 −C2 0   0 0 I zz ,3   S1 −C1 0 

 ( I xx ,3C2 2 + I yy ,3 S2 2 ) C12 + I zz ,3 S12


(I xx ,3 − I zz ,3 − ( I xx,3 − I yy ,3 ) S2 2 C1S1 ) (I xx,3 − I yy,3 ) C1C2 S2 

(
 xx ,3 zz ,3 ( xx,3 yy ,3 ) 2
 I −I − I −I S 2 C1S1) (I xx ,3 C2 2 + I yy ,3 S2 2 ) S12 + I zz ,3C12 (I xx,3 − I zz ,3 ) S1C2 S2


 2
 ( I xx,3 − I yy,3 ) C1C2 S2 (I xx ,2 − I zz ,2 ) S1C2 S2 I yy ,2 + ( I xx,3 − I yy ,3 ) S2 


 −C1C24 C1S24 S1   I xx ,4 0 0   −C1C24 − S1C24 − S 24 

 
D4 (q) = 0 R I 4 4 R =  − S1C24
4 0
S1S24 −C1   0 I yy ,4 0   C1S 24 S1S 24 −C24 
 − S24 −C24 0   0 0 I zz ,4   S1 −C1 0 

 ( I xx ,4C2 2 + I yy ,4 S2 2 ) C12 + I zz ,4 S12


(I xx ,4 − I zz ,4 − ( I xx,4 − I yy ,4 ) S2 2 C1S1 ) (I xx ,4− I yy,4 ) C1C2 S2 

(
 I −I − I −I
 xx ,4 zz ,4 ( xx,4 yy,4 ) S22 C1S1 ) (I xx ,4 C2 2 + I yy ,4 S2 2 ) S12 + I zz ,3C12 (I xx ,4 − I zz ,4 ) S1C2 S 2


 2
 ( I xx,4 − I yy,4 ) C1C2 S2 (I xx ,4 − I zz ,4 ) S1C2 S2 I yy ,4 + ( I xx,4 − I yy ,4 ) S2 

 9

Vectores de la posición del centro de masa de cada eslabón con respecto a x0 , y0 , z0

l2C1  C1 0 S1   − ( l2 + 2l1l2 + R1 ) / 2 ( l1 + l2 ) 

2 2

 
C1 (q) = P1 ( q ) + 0 R1c1 =  l2 S1  +  S1 0 −C1   R12 − l12 / 2 ( l1 + l2 ) 
 l1   0 1 0   0 
 

 C1 ( R12 + l2 2 ) 
− 
 2 ( l1 + l2 ) 
 2 
 S1 ( R1 + l2 ) 
2

C1 (q) =  −
2 ( l1 + l2 ) 
 
 R12 + l2 2 
 l1 + 
 2 ( l1 + l2 ) 
 

l2C1  C1C2 − S1 −C1S 2   0 

C 2 (q) = P2 ( q ) + 0 R c2 =  l2 S1  +  S1C2
2
C1 − S1S 2   0 
 l1   S 2 0 C2  l3 / 2 

 C1 ( 2l2 2 − l3S 2 ) 
 
 2 
 S ( 2l 2 − l S ) 
C 2 (q) =  
1 2 3 2

 2 
 l C 
 l1 + 3 2 
 2 
 

( l2 − d3 S2 ) C1   −C1C2 C1S22 S1   0 
 
C3 (q) = P3 ( q ) + 0 R c3 =  ( l2 − d3 S2 ) S1  +  − S1C
3
S1S2 −C1  l3 / 2
 l1 + d3C2   − S2 −C2 0   0 

 C1 ( 2l2 2 − 2d 3 S 2 + l3S 2 ) 
 
 2 
 S ( 2l 2 − 2d S − l S ) 
C3 (q) =  
1 2 3 2 3 2

 2 
 l  − C2 
 l1 + d3C2 + 3 
 2 
 
 10

 −C1 ( l4C24 − l2 + d3 S2 )   −C1C24 C1S 24 S1   −l4 / 2

 
C 4 (q) = P4 ( q ) + 0 R c4 =  − S1 ( l4C24 − l2 + d3 S2 )  +  − S1C24
4
S1S24 −C1   0 
 l1 − l4 S24 + d3C2   − S24 −C24 0   0 

 C1 ( l4C24 − 2l2 2 + 2d3 S 2 ) 

− 
 2 
 S ( l C − 2l 2 + 2d S ) 
C 4 (q) =  − 
1 4 24 2 3 2

 2 
 lS 
 l1 + d3C2 − 4 24 
 2 
 

Matriz Jacobiana

Usando las formulas propuestas encontramos la matriz Jacobiana de velocidades lineales

y rotacionales.

Para lineales:

 S1 ( R12 + l2 2 ) 
 0 0 0
 2 ( l1 + l2 ) 
 
 C ( q )   C1 ( R1 + l2 )
2 2

J v1 (q) =  1 0 0 0 = − 0 0 0
 1   2 ( l1 + l2 )

 0 0 0 0
 
 
 

 S1 ( 2l2 2 − l3S 2 ) l3C1C2 

− − 0 0
 2 2 
 C ( q ) C 2 ( q )   C1 ( 2l2 − l3S 2 )
2
l3S1C2

J v 2 (q) =  1 0 0 =  − 0 0
 1  2   2 2 
 l S 
 0 − 3 2 0 0
 2 
 

 C ( q ) C 2 ( q ) C3 ( q ) 
J v 3 (q) =  1 0
 1  2 d3 
 11

 S1 ( 2l2 2 − 2d3 S 2 − l3S 2 ) C1 ( 2d3C2 + l3C2 ) 

− − −C1S 2 0
 2 2 
 C ( 2l 2 − 2d S + l S ) S1 ( 2d3C2 + l3C2 ) 
= − − S1S 2 0
1 2 3 2 3 2

 2 2 
 l S 
 0 −d3 S2 + 3 2 C2 0
 2 
 

 C ( q ) C 2 ( q ) C3 ( q ) C 4 ( q ) 
J v 4 (q) =  1 
 1  2 d3  4 

 S1 ( l4C24 − 2l2 2 + 2d 3 S 2 ) C1 ( l4 S 24 − 2d3C2 ) l4 S 24C1 

 −C1S 2 
 2 2 2 
 C ( l C − 2l 2 + 2d S ) S1 ( l4 S 24 − 2d3C2 ) 
l4 S24 S1 
= − − S1S 2
1 4 24 2 3 2

 2 2 2 
 l4 S24 lS 
 0 −d3 S2 − C2 − 4 24 
 2 2 
 

Para rotacionales:

0 0 0 0 
J  (q) = 1z 0
1
0 0 0 = 0 0 0 0 
1 0 0 0 

0 − S1 0 0 
J  (q) = 1z 0  2 z1
2
0 0 = 0 C1 0 0 
1 0 0 0 

0 − S1 −C1S 2 0
J  (q) = 1z 0  2 z1 3z 2
3
0 = 0 C1 − S1S 2 0 
1 0 C2 0 

0 − S1 −C1S 2 S1 
J  (q) = 1z 0  2 z1 3z 2
4
 4 z 3  = 0 C1 − S1S2 −C1 
1 0 C2 0 

Entonces el Jacobino de cada eslabón se denomina:

 12

 J 1 (q)   J 2 (q)   J 3 (q)   J 4 (q) 

J 1 (q) =  v 1  J 2 (q) =  v 2  J 3 (q) =  v 3  J 4 (q) =  v 4 
 J  (q)  J  (q)  J  (q)  J  (q)

Tensor inercial

El tensor inercial se define como la matriz D(q), que describe el movimiento traslacional

y rotacional de cada eslabón del robot.

D1 ( q ) = ( J v1 ) m1J v1 + ( J 1 ) D1J 1
T T

 d111 0 0 0
 
0 0 0 0
D1 ( q ) = 
0 0 0 0
 
0 0 0 0

m1 ( R12 − l2 2 ) l1m1 ( 4 R12 + 2 R1l2 + l2 2 )

2
R12l2 m1
d =
1
+ +
4 ( l1 + l2 ) 12 ( l1 + l2 ) 2 ( l1 + l2 )
11 2

l1m1 ( R1 ( l1 + l2 ) − R12 + l12 + l1 ( l1 + l2 ) ) l1m1 ( 4 R12 + 2 R1l2 + l2 2 )

2

+ +
4 ( l1 + l2 ) 4 ( l1 + l2 )
3 3

D2 ( q ) = ( J v2 ) m2 J v2 + ( J 2 ) D2 J 2
T T

 d112 0 0 0
 
0 d 222 0 0
D2 ( q ) = 
0 0 0 0
 
0 0 0 0

m2 ( −3R2 2 S2 2 + 6 R2 2 + 6l2 2 − 6l2l32 S 2 )

d =
1
11
6

m2 ( 3R2 2 S + 2l32 )
d =
2
22
6

D3 ( q ) = ( J v3 ) m3 J v3 + ( J 3 ) D3 J 3
T T
 13

 d113 0 d133 0
 3 
0 d 0 0
D3 ( q ) =  3 22
 d31 0 d333 0
 
0 0 0 0

d =
3
(
m3 ( l32 − 3R32 )( 2S 2 2 − 1) + 9 R32 + l32 ) + m ( 2l
1 2 − 2d3 S2 + l3S 2 )
2

11
24 4

m3 ( 3R32 + l32 ) m1 ( 4d32 + l32 − 4d3l3)

2
3
d 22 = +
12 4

m3 R32
d333 = + m1
2

m3 R32C 2
d =
3
31
2

d133 = d313

D4 ( q ) = ( J v4 ) m4 J v4 + ( J 4 ) D4 J 4
T T

 d114 0 d134 0 
 4 4 
0 d d d 234 
D4 ( q ) =  4 22 23
 d31 d 4
d 4
d344 
 
32 33

 0 d 444 
4 4
d 42 d 43

m4C24 ( 3R4 2 + l4 2 ) + 6 R4 2 S24 2 + 3 ( l4C24 − 2l2 + 2d3 S2 )

2

d =
4
11
12

m4 ( 3R4 2 + 12d3 − 12S4 d3l4 + 4l4 2 )

d = 4
22
12

m4 ( 3R4 2 S4 2 + 3R4 2 − l4 2 S4 2 + l4 2 + 12 )
d =
4
33
12
 14

m4 ( 3R4 2 + 4l4 2 )
d = 4
44
12

d =
4
(
m4 l4 2C2 − ( 3R4 2 − l4 2 )( C1C4 2 − C1 − 2C4 S1S 4 ) + 9 R4 2C2 )
31
24

m4l4C4
d324 = −
2

m4 ( 3R4 2 + 4l4 2 − 6S4 d3l4 )

d =
4
42
12

d424 = d244 = d434 = d344

d324 = d234

Entonces el vector D(q), está dado por:

D ( q ) = D1 ( q ) + D2 ( q ) + D3 ( q ) + D4 ( q )
 d111 0 0 0   d112 0 0 0   d113 0 d133 0   d114 0 d134 0 
       
0 0 0 0  0 d 222 0 0  0 3
d 22 0 0  0 d 224 d 234 d 234 
D (q) =  + + +
0 0 0 0  0 0 0 0   d313 0 d333 0   d314 d324 d334 d 344 
       
0 0 0 0  0 0 0 0  0 0 0 0   0 d 424 d 434 d 444 

 d111 + d112 + d113 + d114 0 d 313 + d 314 0 

 
0 d +d +d +d
1 2 3 4
d 4
d 4
D (q) =  22 22 22 22 23 23 
 d31 + d314
3
d324 d + d + d 333 + d 334
1 2
d 4 
 4 
33 33 34

 0 d 424 d 434 d 44 + d 44 + d 44 + d 44 
1 2 3

 d11 0 d13 0 
 
0 d 22 d 23 d 24 
D (q) = 
 d31 d32 d33 d34 
 
 0 d 42 d 43 d 44 

Donde:

d42 = d24 = d43 = d34

d32 = d23
 15

2.3. Energía cinética

La energía cinética total del robot se describe como:

1 T
Ec ( q, q ) = q D(q )q
2

Entonces se resuelve:

 d11 0 d13 0   q1 
  
0 d 22 d 23 d 24   q2 
q4  
1
E ( q, q ) =  q1 q2 d3
2  d31 d32 d33 d34   d3 
  
 0 d 42 d 43 d 44   q4 

E c ( q, q ) =
1
2
(
d11q12 + d 22 q2 2 + d33d32 + d 44 q4 2 + 2d13d3q1 + 2d32 d3 ( q2 + q4 ) + 2d 24 q2 q4 )
2.4. Energía potencial

La energía potencial del robot está dada por:

4
E p ( q ) = -  g T mi Ci ( q )
i =1

E p ( q ) = g ( m1C1, z + m2C2, z + m3C3, z + m4C4, z )

  R 2 + l2 2   l C   l3 − C2   l4 S 24  
E p ( q ) = g  m1  l1 + 1  + m2  l1 + 3 2  + m3  l1 + d 3C2 + 2 + 4  1 + − 
 2  
 m l d C
 2 ( l1 + l2 ) 
3 2
   2    

2.5. Lagrange

La expresión de Lagrange es la siguiente:

L ( q, q ) = E c ( q, q ) − E p ( q )

L ( q, q ) =
1
2
( )
d11q12 + d 22 q2 2 + d 33 d 32 + d 44 q4 2 + 2d13 d 3 q1 + 2d 32 d 3 ( q2 + q4 ) + 2d 24 q2 q4 −

  R 2 + l2 2   l3C2   l3 − C2   l4 S 24  
g  m1  l1 + 1  + m  l +  + m3  l1 + d3C2 + 2  + m4  l1 + d3C2 − 2  
 2 ( l1 + l2 ) 
2 1
   2     
 16

2.6. Dinámica del robot

La ecuación para la dinámica del robot es la siguiente:

d  
L ( q, q ) - L ( q, q ) + bi ( qi ) =  i , para i= 1:4.
dt qi qi

Para i =1:

d  
L ( q, q ) - L ( q, q ) + b1 ( q1 ) =  1
dt q1 q1

Derivada parcial con respecto a la velocidad

  
q1
L ( q, q ) =
q1
( Ec ( q, q ) − E p ( q ) ) =
q1
Ec ( q, q )


L ( q, q ) = d11q1 + 2d13 d 3
q1

Derivada con respecto al tiempo

d 
dt q1
L ( q, q ) =
d
dt
( d
) d
d11 q1 + 2d13 d3 = d11 q1 + d11q1 + 2 d13 d3 + 2d13 d3
dt dt

d    
d11 = d11q1 + d11q2 + d11d 3 + d11q4
dt q1 q2 d3 q4


d11 = 0
q1

 m2 ( 6C1 S2 R2 2 − 4C2 S2 l32 + 6l2 l3C2 ) m1 ( 2d3 C2 − l3C2 )( 2l2 − 2d3 S2 + l3S2 )
d11 = R4 m4 C24 S 24 −
2
−
q2 6 2
m4 C24 S 24 ( 3R4 2 + l4 2 ) m4 S12 ( l4 C24 − 2l2 + 2d3 S 2 )( l4 S 24 − 2d3C2 ) m3 ( 6C2 S 2 R32 − 2l32 C2 S 2 )
− − −
6 2 6


d11 = m4 S 2 ( l4 C24 − 2l2 + 2d 3 S 2 ) − m1 S 2 ( ( 2l2 − 2d 3 S 2 + l3S 2 ) )
d3

 m4 C24 S24 ( 3R4 2 + l4 2 ) m4 l4 S24 S12 ( l4 C24 − 2l2 + 2d3 S2 )

d11 = R4 m4 C24 S24 −
2
−
q4 6 2
 17

d    
d13 = d13 q1 + d13 q2 + d13 d3 + d13 q4
dt q1 q2 d1 q4


d13 = 0
q1


d13 = − −
(
m3 S 2 R33 m4 l4 S 2 − ( 3R4 − l4 ) S 2,4 x 2 + 9 R4 S 2
2 2 2
)
; S 24/2 = sin(q2 + 2q4 )
q2 2 24


d13 = 0
d3

 m4 S2,4 x 2 ( 6 R4 2 − 2l4 2 )
d13 =
q4 24

Derivada parcial con respecto a la posición articular

   
q1
L ( q, q ) =
q1
( Ec ( q, q ) − E p ( q ) ) =
q1
Ec ( q, q ) −
q1
E p (q)


E c ( q, q )
q1
1        
=  d11 q12 + d 22 q2 2 + d33 d32 + d 44 q4 2 + 2 d13 d 3 q1 + 2 d 32 d 3 ( q2 + q4 ) + 2 d 24 q2 q4 
2  q1 q1 q1 q1 q1 q1 q1 

      
d11 = d 22 = d33 = d 44 = d13 = d 32 = d 24 = 0
q1 q1 q1 q1 q1 q1 q1


E c ( q, q ) = 0
q1

    R 2 + l2 2   l3C2   l3 − C2   l4 S 24  
Ep (q) = g  m1  l1 + 1  + m  l +  + m3  l1 + d3C2 + 2  + m4  l1 + d3 C2 − 2  
q1 q1   2 ( l1 + l2 ) 
2 1
 2     
 18


Ep (q) = 0
q1

Fuerza de fricción:

b1 ( q1 ) = sgn(q1 )b1s + b1v q1

La expresión dinámica para accionar la articulación 1 es:

        
 1 = d11 q1 + 2d13 d3 +  d11q2 + d11d 3 + d11q4  q1 + 2  d13 q2 + d13 q4  d 3 + b1 ( q1 )
 q2 d3 q4   q2 q4 

Para i =2:

d  
L ( q, q ) - L ( q, q ) + b2 ( q2 ) =  2
dt q2 q2

Derivada parcial con respecto a la velocidad

  
q2
L ( q, q ) =
q2
( Ec ( q, q ) − E p ( q ) ) =
q2
Ec ( q, q )


L ( q, q ) = d 22 q2 + 2d 32 d 3 + 2d 24 q4
q2

Derivada con respecto al tiempo

d 
dt q2
L ( q, q ) =
d
dt
( d
) d d
d 22 q2 + 2d32 d 3 + 2d 24 q4 = d 22 q2 + d 22 q2 + 2 d 32 d 3 + 2d 32 d 3 + 2 d 24 q4 + 2d 24 q4
dt dt dt

d    
d 22 = d 22 q1 + d 22 q2 + d 22 d3 + d 22 q4
dt q1 q2 d3 q4

 
d 22 = d 22 = 0
q1 q2

 1
d 22 = ( 2d3 − l3) m1 + m4 ( 24d3 − 12l4 S 4 )
d3 12
 19


d 22 = −d3l4 m4 C4
q4

d    
d 24 = d 24 q1 + d 24 q2 + d 24 d3 + d 24 q4
dt q1 q2 d3 q4

 
d 24 = d 24 = 0
q1 q2

 1
d 22 = − l4 m4 S4
d3 2

 1
d 22 = − d3 l4 m4 C4
q4 2

d    
d32 = d32 q1 + d32 q2 + d32 d3 + d32 q4
dt q1 q2 d3 q4

  
d32 = d32 = d32 = 0
q1 q2 d3

 1
d32 = l4 m4 S 4
q4 2

Derivada parcial con respecto a la posición articular

   
q2
L ( q, q ) =
q2
( E c ( q, q ) − E p ( q ) ) =
q2
Ec ( q, q ) −
q2
E p (q)


Ec ( q, q )
q2
1        
=  d11 q12 + d 22 q2 2 + d33 d32 + d 44 q4 2 + 2 d13 d 3 q1 + 2 d 32 d 3 ( q2 + q4 ) + 2 d 24 q2 q4 
2  q2 q2 q2 q2 q2 q2 q2 

     
d11 = d 22 = d33 = d 44 = d 32 = d 24 = 0
q2 q2 q2 q2 q2 q2
 20


d13 = − −
(
m3 S 2 R32 m4 l4 S 2 − ( 3R4 − l4 ) S 2,4 x 2 + 9 R4 S 2
2 2 2 2
)
q2 2 24

 
Ec ( q, q ) = d13 q1 d3
q2 q2

    R 2 + l2 2   l3C2   l3 − C2   l4 S 24 
Ep (q) = g  m1  l1 + 1  + m  l +  + m3  l1 + d 3C2 + 2  + m4  l1 + d 3C2 − 2  
q2 q1   2 ( l1 + l2 ) 
2 1
 2     

  l C   l S  1 
E p ( q ) = − g  m4  4 24 + d3 S 2  + m3  d 3 S 2 + 3 2  + 2 m2 l3S 2  ^
q2   2   2  

   l C   l S  1 
L ( q, q ) = d13 q1 d3 + g  m4  4 24 + d3 S 2  + m3  d 3 S 2 + 3 2  + m2 l3S 2 
q2 q2   2   2  2 

Fuerza de fricción:

b2 ( q2 ) = sgn(q2 )b2s + b2v q2

La expresión dinámica para accionar la articulación 2 es:

d  
2 = L ( q, q ) - L ( q, q ) + b2 ( q2 ) =
dt q2 q2
        
 d 22 d3 + d 22 q4  q1 + d 22 q1 + 2 d 24 d3 + 2d32 d3 + 2  d 24 + d 24  q4 + 2d 24 q4
 d3 q4  d3  d3 q4 
  l C   l S  1 
− d13 q1 d3 − g  m4  4 24 + d3 S 2  + m3  d3 S 2 + 3 2  + m2 l3S 2  + sgn(q2 )b2s + b2v q2
q2   2   2  2 

Para i =3:

d  
L ( q, q ) - L ( q, q ) + b3 ( d3 ) = f3
dt d3 d3

Derivada parcial con respecto a la velocidad

 21

  
d3
L ( q, q ) =
d3
( E c ( q, q ) − E p ( q ) ) =
d3
E c ( q, q )


L ( q, q ) = d33 d3 + 2d13 q1 + 2d32 ( q2 + q4 )
d3

Derivada con respecto al tiempo

d 
dt d3
L ( q, q ) =
d
dt
(
d33 d 3 + 2d13 q1 + 2d 32 ( q2 + q4 ) )
d d d
= d33 d3 + d33 d3 + 2 d13 q1 + 2d13 q + 2 d 32 ( q2 + q4 ) + 2d 32 ( q2 + q4 )
dt dt dt

d    
d33 = d33 q1 + d33 q2 + d33 d 3 + d 33 q4
dt q1 q2 d3 q4

  
d33 = d33 = d33 = 0
q1 q2 d3


d33 = − m4 ( 6 R4 C4 S 4 − 2l4 2 C4 S 4 )
1
q4 12

d    
d13 = d13 q1 + d13 q2 + d13 d3 + d13 q4
dt q1 q2 d3 q4

 
d13 = d13 = 0
q1 d3


q2 2
1
24
(
d13 = − m3 S2 R32 − m4 l4 2 S 2 − ( 3R4 2 − l4 2 ) S 2,4 x 2 + 9 R4 2 S2
1
)

m4 S 2,4 x 2 ( 6 R4 − 2l4 2 )
1
d13 =
q4 24

d
La expresión de d32 , ya se conoce.
dt

Derivada parcial con respecto a la posición articular

   
d3
L ( q, q ) =
d3
( Ec ( q, q ) − E p ( q ) ) =
d3
Ec ( q , q ) −
d3
Ep (q)
 22


Ec ( q, q )
d3
1        
=  d11 q12 + d 22 q2 2 + d33 d32 + d 44 q4 2 + 2 d13 d 3 q1 + 2 d 32 d 3 ( q2 + q4 ) + 2 d 24 q2 q4 
2  d3 d3 d3 d3 d3 d3 d3 

   
d33 = d 44 = d 24 = d13 = 0
d3 d3 d3 d3


d11 = m4 S 2 ( l4 C24 + 2l2 + 2d3 S 2 ) − m1 S 2 ( 2l2 − ( 2d3 − l3) S 2 )
d3

 m ( 24d3 − 12l4 S4 )
d 22 = 2d3 m1 − l3m1 + 4
d3 12

 1
d32 = − m4 l4 S 4
d3 2

 1    
Ec ( q, q ) =  d11 q12 + d 22 q2 2 + 2 d 32 d 3 ( q2 + q4 ) 
d3 2  d3 d3 d3 

    R12 + l2 2   l3C2   l3 − C2   l4 S 24 

Ep (q) = g  m1  l1 +  + m  l +  + m  l + d C +  + m4  l1 + d 3C2 − 2  
d3 d3   2 ( l1 + l2 ) 
2 1 3 1 3 2
 2   2   


E p ( q ) = g ( m3 + m4 ) C2
d3

 1    
L ( q, q ) =  d11 q12 + d 22 q2 2 + 2 d32 d3 ( q2 + q4 )  − g ( m3 + m4 ) C2
d3 2  d3 d3 d3 

Fuerza de fricción:

b3 ( d3 ) = sgn(d3 )b3s + b3v d3

La expresión dinámica para accionar la articulación 3 es:

 23

d  
f3 = L ( q, q ) - L ( q, q ) + b3 ( d3 )
dt d3 d3
     
= d33 q4 d3 + d33 d3 + 2  d13 q2 + d13 q4  q1 + 2d13 q + 2 d 32 ( q2 + q4 ) + 2d 32 ( q2 + q4 ) −
q4  q2 q4  q4
1     
  d11 q12 + d 22 q2 2 + 2 d32 d3 ( q2 + q4 )  − g ( m3 + m4 ) C2  + sgn(d 3 )b3s + b3v d3
 2  d3 d3 d3  
Para i =4:

d  
L ( q, q ) - L ( q, q ) + b4 ( q4 ) =  4
dt q4 q4

Derivada parcial con respecto a la velocidad

  
q4
L ( q, q ) =
q4
( Ec ( q, q ) − E p ( q ) ) =
q4
Ec ( q, q )


L ( q, q ) = d 44 q4 + 2d 32 d 3 + 2d 24 q2
q4

Derivada con respecto al tiempo

d 
dt q4
L ( q, q ) =
d
dt
( d
) d d
d 44 q4 + 2d32 d 3 + 2d 24 q2 = d 44 q4 + d 44 q4 + 2 d 32 d 3 + 2d 32 d 3 + 2 d 24 q2 + 2d 24 q2
dt dt dt

d    
d 44 = d 44 q1 + d 44 q2 + d 44 d3 + d 44 q4
dt q1 q2 d3 q4

   
d 44 = d 44 = d 44 = d 44 = 0
q1 q2 d3 q2

d d
Las expresiones de d32 y d 24 se conocen.
dt dt

d  d d
L ( q, q ) = d 44 q4 + 2 d 32 d 3 + 2d 32 d 3 + 2 d 24 q2 + 2d 24 q2
dt q4 dt dt

Derivada parcial con respecto a la posición articular

 24

   
q4
L ( q, q ) =
q4
( E c ( q, q ) − E p ( q ) ) =
q4
Ec ( q, q ) −
q4
E p (q)


Ec ( q, q )
q4
1        
=  d11 q12 + d 22 q2 2 + d33 d32 + d 44 q4 2 + 2 d13 d 3 q1 + 2 d 32 d 3 ( q2 + q4 ) + 2 d 24 q2 q4 
2  q4 q4 q4 q4 q4 q4 q4 


d 44 = 0
q4


d11 = R4 2 m4 C24 S 24 − m4 C24 S 24 ( 3R4 2 + l4 2 ) − m4 S 24 ( l4 C24 − 2l2 + 2d 3 S 2 )
1 1
q4 6 2


d 22 = −d3l4 m4 C4
q4


d33 = m4 ( 6 R4 2 C4 S4 − 2l4 2 C4 S4 )
1
q4 12


m4 S 2,4 x 2 ( 6 R4 2 − 2l4 2 )
1
d13 =
q4 24

 1
d32 = m4 l4 S4
q4 2

 1
d 24 = − m4 d3l4 C4
q4 2


E c ( q, q )
q4
1       
=  d11 q12 + d 22 q2 2 + d 33 d 32 + 2 d13 d 3 q1 + 2 d 32 d 3 ( q2 + q4 ) + 2 d 24 q2 q4 
2  q4 q4 q4 q4 q4 q4 

    R 2 + l2 2   l3C2   l3 − C2   l4 S 24 
Ep (q) = g  m1  l1 + 1  + m  l +  + m3  l1 + d 3C2 + 2  + m4  l1 + d 3C2 − 2  
q4 q4   2 ( l1 + l2 ) 
2 1
 2     

 1
E p ( q ) = − gl4 m4 C24
q4 2
 25


L ( q, q )
q4
1        1
=  d11 q12 + d 22 q2 2 + d33 d 32 + 2 d13 d 3 q1 + 2 d32 d 3 ( q2 + q4 ) + 2 d 24 q2 q4  + gl4 m4 C24
2  q4 q4 q4 q4 q4 q4  2

Fuerza de fricción:

b4 ( q4 ) = sgn(q4 )b4s + b4v q4

La expresión dinámica para accionar la articulación 4 es:

d  
4 = L ( q, q ) - L ( q, q ) + b4 ( q4 )
dt q4 q4
    
= d 44 q4 + 2 d 24 d3 + 2d32 d3 + 2  d 24 + d 24  q2 + 2d 24 q2
d3  d3 q4 
1       
−  d11 q12 + d 22 q2 2 + d 33 d 32 + 2 d13 d 3 q1 + 2 d 32 d 3 ( q2 + q4 ) + 2 d 24 q2 q4 
2  q4 q4 q4 q4 q4 q4 
1
− gl4 m4 C24 + sgn(q4 )b4s + b4v q4
2

Forma matricial

De las ecuaciones que gobiernan al sistema:

        
 1 = d11 q1 + 2d13 d3 +  d11q2 + d11d 3 + d11q4  q1 + 2  d13 q2 + d13 q4  d 3 + b1 ( q1 )
 q2 d3 q4   q2 q4 

        
2 =  d 22 d3 + d 22 q4  q1 + d 22 q1 + 2 d 24 d3 + 2d32 d3 + 2  d 24 + d 24  q4 + 2d 24 q4
 d3 q4  d3  d3 q4 
  l C   l S  1 
− d13 q1 d3 − g  m4  4 24 + d3 S 2  + m3  d3 S 2 + 3 2  + m2 l3S 2  + sgn(q2 )b2s + b2v q2
q2   2   2  2 

     
f3 = d33 q4 d3 + d33 d3 + 2  d13 q2 + d13 q4  q1 + 2d13 q + 2 d 32 ( q2 + q4 ) + 2d 32 ( q2 + q4 ) −
q4  q2 q4  q4
1     
  d11 q12 + d 22 q2 2 + 2 d32 d3 ( q2 + q4 )  − g ( m3 + m4 ) C2  + sgn(d3 )b3s + b3v d 3
 2  d3 d3 d3  
 26

    
 4 = d 44 q4 + 2 d 24 d3 + 2d32 d3 + 2  d 24 + d 24  q2 + 2d 24 q2
d3  d3 q4 
1       
−  d11 q12 + d 22 q2 2 + d33 d32 + 2 d13 d3 q1 + 2 d 32 d 3 ( q2 + q4 ) + 2 d 24 q2 q4 
2  q4 q4 q4 q4 q4 q4 
1
− gl4 m4 C24 + sgn(q4 )b4s + b4v q4
2

Se obtiene la forma matricial que corresponde a:

 1   d11 0 d13 0   q1   c1 ( q, q )   h1 ( q )   b1 ( q ) 
          
 2 =  0 d 22 d 23 d 24   q2  c2 ( q, q )   h2 ( q )  b2 ( q ) 
+ + +
 f3   d31 d32 d33 d34   d3   c3 ( q, q )   h3 ( q )  b3 ( q ) 
          
 4   0 d 42 d 43 d 44   q4  c4 ( q, q )   h4 ( q )  b4 ( q ) 

Que corresponde a la forma sintetizada matricial:

τ = D( q )q + c( q , q ) + h ( q ) + b ( q )

Donde:

        
c1 ( q, q ) =  d11 q2 + d11d 3 + d11q4  q1 + 2  d13 q2 + d13 q4  d 3
 q2 d3 q4   q2 q4 

         
c2 ( q, q ) =  d 22 d 3 + d 22 q4  q1 + 2 d 24 d 3 + 2  d 24 + d 24  q4 − d13 q1d 3
 d3 q4  d3  d3 q4  q2

     
c3 ( q, q ) = d33 q4 d3 + 2  d13 q2 + d13 q4  q1 + 2 d 32 ( q2 + q4 )
q4  q2 q4  q4
1    
−  d11 q12 + d 22 q2 2 + 2 d32 d3 ( q2 + q4 ) 
2  d3 d3 d3 


c4 ( q, q ) = +2 d 24 d3
d3

h1 ( q ) = 0

 l C   l S  1 
h2 ( q ) = − g  m4  4 24 + d3 S 2  + m3  d 3 S 2 + 3 2  + m2 l3S 2 
  2   2  2 
 27

h3 ( q ) = − g ( m3 + m4 ) C2

1
h4 ( q ) = − gl4 m4 C24
2

 b1 ( q )   sgn(q1 )b1s + b1v q1 

   
b2 ( q )  = sgn(q2 )b2 + b2v q2 
s

b3 ( q )  sgn(d3 )b3s + b3v d3 

   
b4 ( q )  sgn(q4 )b4 + b4v q4 
s

Simulaciones en Matlab

Con un movimiento del angulo theta1 de la forma theta1=2*sin(2*t)

 28

Con un movimiento del ángulo theta2 de la forma theta2=2*t

Con un movimiento del ángulo d3 de la forma d2=2*t

Código fuente (MATLAB)

clc
clear

%% DINAMICA DE UN ROBOT RRPR

syms theta1 theta2 d3 theta4 real
syms dtheta1 dtheta2 dd3 dtheta4 real
syms ddtheta1 ddtheta2 ddd3 ddtheta4 real
% Datos de entrada
t = 0:0.05:2;
 29

n= length(t);
tet1 = zeros(1,n);
tet2 = zeros(1,n);
d_3 = 2*t;
tet4 = zeros(1,n);

dtet1 = zeros(1,n);
dtet2 = zeros(1,n);
dd_3 = 2*ones(1,n);
dtet4 = zeros(1,n);

ddtet1 = zeros(1,n);
ddtet2 = zeros(1,n);
ddd_3 = zeros(1,n);
ddtet4 = zeros(1,n);

Q = [tet1;tet2 ;d_3 ;tet4];

Qd = [dtet1;dtet2 ;dd_3 ;dtet4];
Qdd = [ddtet1;ddtet2 ;ddd_3 ;ddtet4];

qd = [dtheta1 dtheta2 dd3 dtheta4]';

qdd = [ddtheta1 ddtheta2 ddd3 ddtheta4]';

%DATOS DEL ROBOT

g = 9.81;
l1 = 1;
l2 = 1.5;
l4 = 1;
l31= 0.3;
l32= 0.8;
bs = ones(4,1)*0.3;
bv = ones(4,1)*0.1;

R1=0.04;
R2=0.03;
R3=0.02;
R4=0.01; % RADIOS de cada eslabon
m1=5;
m2=2;
m3=2;
m4=1; % Masas
 30

L(1) = Link([0 l1 l2 pi/2 0 ]);

L(2) = Link([0 0 0 -pi/2 0 ]);
L(3) = Link([pi 0 0 -pi/2 1 ]);
L(4) = Link([0 0 l4 0 0 ]);

A01 = L(1).A(theta1);
A02 = A01*L(2).A(theta2);
A03 = A02*L(3).A(d3);
A04 = A03*L(4).A(theta4);

xm = (-(l2*(l1-R1)+(l2+R1)^2/2)/(l1+l2));

ym = (-((l1+R1)/2*(l1-R1))/(l1+l2));

c1 = [xm;ym;0];
c2 = [0;0;l31/2];
c3 = [0;l32/2;0];
c4 = [-l4/2;0;0];

k = l1/l2;
m11 = (m1/(k+1));
m12 = (k*m1/(k+1));

Ixx1 = 1/12*m11*(3*R1^2+(l1-R1)^2)+m11*(abs(c1(1))-l2)^2+...
1/2*m12*R1^2+m12*(abs(c1(1))-(l2+R1)/2)^2;

Iyy1 = 1/2*m11*R1^2+m11*(abs(c1(2))-(l1+R1)/2)^2 + ...

1/12*m12*(3*R1^2+(l2+R1)^2)+m12*(abs(c1(2)))^2;

Izz1 = 1/12*m11*(3*R1^2+(l1-R1)^2)+ 1/12*m12*(3*R1^2+(l2+R1)^2);

Ixy = m11*(abs(c1(1))-l2)*(abs(c1(2))-(l1+R1)/2)+
m12*(abs(c1(1))-(l2+R1)/2)*abs(c1(2));

% TENSORES DE INERCIA
 31

I1 = [Ixx1 -Ixy 0;...

-Ixy Iyy1 0;...
0 0 Izz1];

I2(1,1) = 1/12*m2*(6*R2^2+l31^2);
I2(2,2) = 1/12*m2*(6*R2^2+l31^2);
I2(3,3) = m2*R2^2;

I3(1,1) = 1/12*m3*(3*R3^2+l32^2);
I3(3,3) = 1/12*m3*(3*R3^2+l32^2);
I3(2,2) = 1/2*m3*R3^2;

I4(2,2) = 1/12*m4*(3*R4^2+l4^2);
I4(3,3) = 1/12*m4*(3*R4^2+l4^2);
I4(1,1) = 1/2*m4*R4^2;

% ROtaciones
R01 = tr2rt(A01);
R10 = (inv(R01));

R02 = tr2rt(A02);
R20 = (inv(R02));

R03 = tr2rt(A03);
R30 = (inv(R03));

R04 = tr2rt(A04);
R40 = (inv(R04));

% tensores de inercial referencial D

D1p = (R01*I1*R10);
D2p = simplify(R02*I2*R20);
D3p = simplify(R03*I3*R30);
D4p = simplify(R04*I4*R40);

% Posicion del centrode masa de cada eslabon

P1 = A01.t;
P2 = simplify(A02.t);
P3 = simplify(A03.t);
P4 = simplify(A04.t);

C1 = simplify(P1 + R01*c1);
 32

C2 = simplify(P2 + R02*c2);
C3 = simplify(P3 + R03*c3);
C4 = simplify(P4 + R04*c4);

% Jacobianos
Jv1=jacobian(C1,[theta1 theta2 d3 theta4]);
Jv2=jacobian(C2,[theta1 theta2 d3 theta4]);
Jv3=jacobian(C3,[theta1 theta2 d3 theta4]);
Jv4=jacobian(C4,[theta1 theta2 d3 theta4]);

z0 =[0;0;1];
z1 =A01.a;
z2 =A02.a;
z3 =A03.a;

Jw1 = [z0 zeros(3,3)];

Jw2 = [z0 z1 zeros(3,2)];
Jw3 = [z0 z1 z2 zeros(3,1)];
Jw4 = [z0 z1 z2 z3 ];

J1 = [Jv1;Jw1];
J2 = [Jv2;Jw2];
J3 = [Jv3;Jw3];
J4 = [Jv4;Jw4];

% TENSOR INERCIAL D
D1 = Jv1'*m1*Jv1+ Jw1'*D1p*Jw1;
D2 = Jv2'*m2*Jv2+ Jw2'*D2p*Jw2;
D3 = Jv3'*m1*Jv3+ Jw3'*D3p*Jw3;

D4 = (Jv4'*m4*Jv4+ Jw4'*D4p*Jw4);

D= (D1+D2+D3+D4);

%%

Ec =(1/2*(qd')*D*qd);
Ep = g*(m1*C1(3)+m2*C2(3)+m3*C3(3)+m4*C4(3));

L = Ec-Ep;

%% Articulacion 1
dqd1=diff(L,dtheta1);
 33

JJ1 = jacobian(dqd1,[theta1 theta2 d3 theta4]);

dtdq1 = JJ1*qdd;
dq1=diff(L,theta1);
b1q = sign(dtheta1)*bs(1)+bv(1)*dtheta1;

tor1 = dtdq1-dq1+b1q;
%% Articulacion 2
dqd2=diff(L,dtheta2);
JJ2 = jacobian(dqd2,[theta1 theta2 d3 theta4]);
dtdq2 = JJ2*qdd;
dq2=diff(L,theta2);
b2q = sign(dtheta2)*bs(2)+bv(2)*dtheta2;

tor2 = dtdq2-dq2+b2q;

%% Articulacion 3
dqd3=diff(L,dd3);
JJ3 = jacobian(dqd3,[theta1 theta2 d3 theta4]);
dtdq3 = JJ3*qdd;
dq3=diff(L,d3);
b3q = sign(dtheta2)*bs(3)+bv(3)*dd3;

f3 = dtdq3-dq3+b3q;
%% Articulacion 4
dqd4=diff(L,dtheta4);
JJ4 = jacobian(dqd4,[theta1 theta2 d3 theta4]);
dtdq4 = JJ4*qdd;
dq4=diff(L,theta4);
b4q = sign(dtheta4)*bs(4)+bv(4)*dtheta4;

tor4 = dtdq4-dq4+b4q;

%% EVALUACION
theta1=Q(1,:);
theta2=Q(2,:);
d3=Q(3,:);
theta4=Q(4,:);

dtheta1=Qd(1,:);
dtheta2=Qd(2,:);
dd3=Qd(3,:);
dtheta4=Qd(4,:);

ddtheta1=Qdd(1);
ddtheta2=Qdd(2);
ddd3=Qdd(3);
ddtheta4=Qdd(4);
 34

T1 = eval(tor1);
T2 = eval(tor2);
F3 = eval(f3);
T4 = eval(tor4);
%% Ploteos
plot(t,T1,'o','linewidth',2)
hold on
plot(t,T2,'x','linewidth',2)
plot(t,F3,':','linewidth',2)
plot(t,T4,'+','linewidth',2)
grid on

xlabel('Tiempo (s) ')

ylabel('Torques o fuerzas del sistema ')

title('Comportamiento de los torques y fuerzas ')

legend('Torque1','Torque2','Fuerza3','Torque4')
 35

3. CONCLUSIONES Y OBSERVACIONES

1. Es importante considerar la forma y geometría de cada eslabón del robot, ya que

varia mucho si el eslabón no es uniforme o no se encuentra en un eje de su sistema

de referencia como se vio en el eslabón 1. Por ser de forma tipo “L” cambió su

centro de masa y también su tensor de inercias, esto complicó el desarrollo ya que

existían más productos inerciales.

2. En el lagrangiano se necesita ser cuidadosos con la derivación, ya que siendo de

alta complejidad es mejor usar métodos de derivación de cálculo en varias

variables.

3. Hay que tener en cuenta que hay ecuaciones en las que no se ha despejado

completamente, debido al tamaño de las ecuaciones es preferible dejarlo como

variable, de todos modos, se conoce el valor de cada expresión

4. La dinámica del robot de 4 grados de libertad RRPR es de complejidad alta, por lo

que se observa el desarrollo de su problema es complejo y tedioso, por lo que es

importante considerar los elementos que se están utilizando.

5. La resolución del código se resolvió de manera simbólica, por lo que ocupara

mucha memoria en su procedimiento, dependiendo del computador que se tiene,

esto puede llevar mucho tiempo incluso horas. Para facilitar resultados concretos

se ha ejecutado con tres tipos entradas.

6. Como se pueden observar en las gráficas los torques varían dependiendo de las

entradas, de esta manera se puede analizar bien el comportamiento

36