robotic a

MAESTRÍA EN AUTOMATIZACIÓN Y CONTROL INDUSTRIAL

MECATRONICA

Docente : Julio Casas

Mauricio Alberto Correa Villa

Roberto Carlos Guevara Calume

ITM 2008

MEDELLIN



Ejercicio robótica

Para el robot RRT de la figura 1 hallar la cinemática directa, el área de trabajo, planificación de

trayectoria en línea horizontal de 25 cm y la dinámica dado que

0 ≤ 𝜃1 ≤ 270°

0 ≤ 𝜃2 ≤ 180°

0 ≤ 𝑠 ≤ 50𝑐𝑚

𝑙1 = 𝑙2 = 25𝑐𝑚

𝐹 = 25𝐾𝑖𝑙𝑜𝑠

Figura 1



Definición del área de trabajo

figura 2

la variación de los angulos 𝜃1 , 𝜃2 𝑦 𝑆 describe un area de recorrido máximo mostrado en la

regiaon sonbreada con verde se nota un sector no cubierto en blancoen el trecer cuadrante

debido a que 𝜃1 esta solo en el ran go de 0a 270°



MODELO GEOMETRICO

figura 3

figura 4

Luego

𝑃𝑥1 = 𝑙1𝑐𝑜𝑠 𝜃1

𝑃𝑥2 = (𝑙2 + 𝑠) cos(𝜃1 + 𝜃2)



𝑃𝑥2 = 𝑃𝑥1 + 𝑃𝑥2

𝑃𝑥 = 𝑙1𝑐𝑜𝑠 𝜃1 + (𝑙2 + 𝑠) cos(𝜃1 + 𝜃2)

𝑃𝑦1 = 𝑙1𝑠𝑒𝑛 𝜃1

𝑃𝑦2 = (𝑙2 + 𝑠) sen(𝜃1 + 𝜃2)

Luego

𝑃𝑦 = 𝑃𝑦1 + 𝑃𝑦2

𝑃𝑦 = 𝑙1𝑠𝑒𝑛 𝜃1 + (𝑙2 + 𝑠) sen(𝜃1 + 𝜃2)

Luego para el punto 𝑃 = (𝑃𝑥, 𝑃𝑦, 𝑃𝑧) la solución corresponde a:

𝑃𝑥 = 𝑙1𝑐𝑜𝑠 𝜃1 + (𝑙2 + 𝑠) cos(𝜃1 + 𝜃2)

𝑃𝑦 = 𝑙1𝑠𝑒𝑛 𝜃1 + (𝑙2 + 𝑠) sen(𝜃1 + 𝜃2)𝑃𝑧 = 0

Algoritmo de denavit-hertenberg

En primer lugar se localizan los sistemas de referencia de cada uno de los ejes de las articulaciones

del robot figura .

Se determinan losm parámetros de D-H

Denavit-hertenberg D-H

Rz θ

Tz d

Tx a

Rx

1 𝜃1 0 𝑙1 0

2 𝜃2 𝑙2 0

3 0 0 S 0



El paso de {S0} a {S1} se consigue mediante un giro de θ1 en torno al eje Z y una translación 𝑙1 a lo

largo del nuevo eje X:

𝐴01 = 𝑅𝑂𝑇𝑧 𝜃1 .𝑇( 𝑙1 , 0,0)

El paso de {S1} a {S2} se consigue mediante un giro de θ2 en torno al eje Z y un desplazamiento de 𝑙2

a lo largo del nuevo eje X:

𝐴21 = 𝑅𝑂𝑇𝑧 𝜃2 .𝑇( 𝑙2 , 0,0)

El paso de {S2} a {S3} se consigue mediante un desplazamiento S en le plano X,Y:

𝐴32 = 𝑇( 𝑆, 0,0)

𝐴10 =

cos(𝜃1) −𝑠𝑒𝑛(𝜃1) 0 0𝑠𝑒𝑛 (𝜃1) cos(𝜃1) 0 0

0 0 1 00 0 0 1

1 0 0 𝑙10 1 0 00 0 1 00 0 0 1

Usando mat lab

>> syms t1

>> syms l1

>> A1=[cos(t1), -sin(t1),0,0; sin(t1), cos(t1),0,0; 0,0,1,0; 0,0,0,1 ]

A1 =

[ cos(t1), -sin(t1), 0, 0]

[ sin(t1), cos(t1), 0, 0]

[ 0, 0, 1, 0]

[ 0, 0, 0, 1]

>> b=[1 0 0 l1; 0 1 0 0; 0 0 1 0; 0 0 0 1]

b =

[ 1, 0, 0, l1]

[ 0, 1, 0, 0]

[ 0, 0, 1, 0]

[ 0, 0, 0, 1]

A1*b

Resultado en matlab



ans =

[ cos(t1), -sin(t1), 0, cos(t1)*l1]

[ sin(t1), cos(t1), 0, sin(t1)*l1]

[ 0, 0, 1, 0]

[ 0, 0, 0, 0]

En limpio

𝐴10 =

cos(𝜃1) −𝑠𝑒𝑛(𝜃1) 0 𝑙1cos(𝜃1)𝑠𝑒𝑛 (𝜃1) cos(𝜃1) 0 𝑙1sen(𝜃1)

0 0 1 00 0 0 1

𝐴10 =


0 0 1 00 0 0 1

Para 𝐴21

𝐴2 =1

cos(𝜃2) −𝑠𝑒𝑛(𝜃2) 0 0𝑠𝑒𝑛 (𝜃2) cos(𝜃2) 0 0

0 0 1 00 0 0 1

1 0 0 𝑙2

0 1 0 00 0 1 00 0 0 1

Usando mat lab

>> b=[1 0 0 l2; 0 1 0 0; 0 0 1 0; 0 0 0 1]

b =

[ 1, 0, 0, l2]

[ 0, 1, 0, 0]

[ 0, 0, 1, 0]

[ 0, 0, 0, 1]




A2 =

[ cos(t2), -sin(t2), 0, 0]

[ sin(t2), cos(t2), 0, 0]

[ 0, 0, 1, 0]

[ 0, 0, 0, 1]

>> A2*b

ans =

[ cos(t2), -sin(t2), 0, l2*cos(t2)]

[ sin(t2), cos(t2), 0, l2*sin(t2)]

[ 0, 0, 1, 0]

[ 0, 0, 0, 1]

En limpio

𝐴2 =1


0 0 1 00 0 0 1

𝐴23 =

1 0 0 𝑆0 1 0 00 0 1 00 0 0 1

Con lo que :

𝑇 = 𝐴01 𝐴1

2 𝐴23 Lo que corresponde a la localización del sistema final con respecto al sistema de

referencia

𝑇

=


0 0 1 00 0 0 1

.


0 0 1 00 0 0 1

.

1 0 0 𝑆0 1 0 00 0 1 00 0 0 1

Usando mat lab nuevamente

>> syms t1

>> syms l1

>> syms t2



>> syms l2

>> syms S


A1 =

[ cos(t1), -sin(t1), 0, 0]

[ sin(t1), cos(t1), 0, 0]

[ 0, 0, 1, 0]

[ 0, 0, 0, 1]

A2 =[cos(t2), -sin(t2),0,l2*cos(t2); sin(t2), cos(t2),0,l2*sin(t2); 0,0,1,0; 0,0,0,1 ]

[ cos(t2), -sin(t2), 0, l2*cos(t2)]

[ sin(t2), cos(t2), 0, l2*sin(t2)]

[ 0, 0, 1, 0]

[ 0, 0, 0, 1]

>> A3=[1 0 0 S;0 1 0 0; 0 0 1 0;0 0 0 1]

A3 =

[ 1, 0, 0, S]

[ 0, 1, 0, 0]

[ 0, 0, 1, 0]

[ 0, 0, 0, 1]

R=A1.*A2

R =



[ cos(t1)*cos(t2), sin(t1)*sin(t2), 0, cos(t1)*l1*l2*cos(t2)]

[ sin(t1)*sin(t2), cos(t1)*cos(t2), 0, sin(t1)*l1*l2*sin(t2)]

[ 0, 0, 1, 0]

[ 0, 0, 0, 1]

>> T=A1*A2*A3

T =

[ cos(t1)*cos(t2)-sin(t1)*sin(t2), -cos(t1)*sin(t2)-sin(t1)*cos(t2), 0, (cos(t1)*cos(t2)-

sin(t1)*sin(t2))*S+cos(t1)*l2*cos(t2)-sin(t1)*l2*sin(t2)+cos(t1)*l1]

[sin(t1)*cos(t2)+cos(t1)*sin(t2), cos(t1)*cos(t2)-sin(t1)*sin(t2), 0,

(sin(t1)*cos(t2)+cos(t1)*sin(t2))*S+sin(t1)*l2*cos(t2)+cos(t1)*l2*sin(t2)+sin(t1)*l1]

[0, 0, 1, 0]

En limpio

𝑇

=

cos 𝜃1 cos 𝜃2 − 𝑠𝑒𝑛 𝜃1 𝑠𝑒𝑛 𝜃2 −𝑐𝑜𝑠 𝜃1 𝑠𝑒𝑛 𝜃2 − 𝑠𝑒𝑛(𝜃1)𝑐𝑜𝑠(𝜃2) 0 (𝑙2 + s)(cos 𝜃1 cos 𝜃2 − 𝑠𝑒𝑛 𝜃1 𝑠𝑒𝑛 𝜃2 + (𝑙1𝑐𝑜𝑠(𝜃1)

𝑠𝑒𝑛 (𝜃1) cos 𝜃2 + 𝑐𝑜𝑠 𝜃1 𝑠𝑒𝑛 𝜃2 −𝑠𝑒𝑛 𝜃1 𝑠𝑒𝑛 𝜃2 + cos 𝜃1 cos 𝜃2 0 (𝑙2 + s)(cos 𝜃2 sen 𝜃1 + 𝑠𝑒𝑛 2 𝑐𝑜𝑠 𝜃1 + (𝑙1𝑠𝑒𝑛(𝜃1)0 0 1 00 0 0 1

Sabemos que el seno y el coseno dela suma de los angulos cumplen con las identidades

𝑠𝑒𝑛 𝑎 + 𝑏 = 𝑠𝑒𝑛 𝑎 cos 𝑏 + cos 𝑎 𝑠𝑒𝑛 𝑏 𝑖𝑑𝑒𝑛𝑡𝑖𝑑𝑎𝑑 (1)

𝑐𝑜𝑠 𝑎 + 𝑏 = 𝑐𝑜𝑠 𝑎 cos 𝑏 − sen 𝑎 𝑠𝑒𝑛 𝑏 𝑖𝑑𝑒𝑛𝑡𝑖𝑑𝑎𝑑 (2)

Para nuestro caso tomamos posiciones de matriz T=(1,4) Y (2,4)

De la posición (1,4):

= (𝑙2 + s) cos(𝜃1 cos 𝜃2 − 𝑠𝑒𝑛(𝜃1)𝑠𝑒𝑛(𝜃1) + (𝑙2 cos 𝜃1 )

= (𝑙2 + s) cos(𝜃1 + 𝜃2) + −𝑠𝑒𝑛(𝜃1)(𝑙2 cos 𝜃1 )

= (𝑙1 cos(𝜃1) + (𝑙2 + 𝑠) cos 𝜃1 + 𝜃2



= 𝑃𝑥

Lo que se obtuvo por método geométrico

De la posición (2,4)

= (𝑙2 + s) cos(𝜃2 sen 𝜃1 + 𝑠𝑒𝑛(𝜃2)𝑐𝑜𝑠(𝜃1) + (𝑙1 cos 𝜃2 )

= (𝑙2 + s) sen 𝜃1+𝜃2 + (𝑙1 sen 𝜃1 ) 𝑎𝑝𝑙𝑖𝑐𝑎𝑛𝑑𝑜 𝑖𝑑𝑒𝑛𝑡𝑖𝑑𝑎𝑑 (1)

= (𝑙2 + sen 𝜃1 ) + (𝑙2 + s) sen 𝜃1+𝜃2 = 𝑃𝑦

Lo mismo que se obtuvo por el método geométrico

Pagina 4

Matriz jacobiana

𝑥 𝑦 = 𝐽

𝜕

𝜕𝜃1𝑃𝑥

𝜕

𝜕𝜃2𝑃𝑥

𝜕

𝜕𝑠𝑃𝑥

𝜕

𝜕𝜃1𝑃𝑦

𝜕

𝜕𝜃2𝑃𝑦

𝜕

𝜕𝑠𝑃𝑦

𝜕

𝜕𝜃1𝑃𝑥 =

𝜕

𝜕𝜃1(𝑙1𝑐𝑜𝑠 𝜃1 + (𝑙2 + 𝑠) cos(𝜃1 + 𝜃2)) = −𝑙1𝑠𝑒𝑛(𝜃1) − (𝑙2 + 𝑠)𝑠𝑒𝑛(𝜃1 + 𝜃2)

𝜕

𝜕𝜃2𝑃𝑥 =

𝜕

𝜕2(𝑙1𝑐𝑜𝑠 𝜃1 + (𝑙2 + 𝑠) cos(𝜃1 + 𝜃2)) =

𝜕

𝜕𝑠𝑃𝑥 =

𝜕

𝜕𝑠(𝑙1𝑐𝑜𝑠 𝜃1 + (𝑙2 + 𝑠) cos(𝜃1 + 𝜃2)) = 𝑐𝑜𝑠(𝜃1 + 𝜃2)

𝜕

𝜕𝜃1𝑃𝑦 =

𝜕

𝜕𝜃1(𝑙1𝑠𝑒𝑛 𝜃1 + (𝑙2 + 𝑠) sen(𝜃1 + 𝜃2)) = 𝑙1𝑐𝑜𝑠(𝜃1) + (𝑙2 + 𝑠)𝑐𝑜𝑠(𝜃1 + 𝜃2)

𝜕

𝜕𝜃2𝑃𝑦 =

𝜕

𝜕𝜃2(𝑙1𝑠𝑒𝑛 𝜃1 + (𝑙2 + 𝑠) sen(𝜃1 + 𝜃2)) = (𝑙2 + 𝑠)𝑐𝑜𝑠(𝜃1 + 𝜃2)



𝜕

𝜕𝑠𝑃𝑦 =

𝜕

𝜕𝑠(𝑙1𝑠𝑒𝑛 𝜃1 + (𝑙2 + 𝑠) sen(𝜃1 + 𝜃2)) = 𝑠𝑒𝑛(𝜃1 + 𝜃2)

𝐽 = −𝑙1𝑠𝑒𝑛(𝜃1) − (𝑙2 + 𝑠)𝑠𝑒𝑛(𝜃1 + 𝜃2) −(𝑙2 + 𝑠)𝑠𝑒𝑛(𝜃1 + 𝜃2) 𝑐𝑜𝑠(𝜃1 + 𝜃2)𝑙1𝑐𝑜𝑠(𝜃1) + (𝑙2 + 𝑠)𝑐𝑜𝑠(𝜃1 + 𝜃2) (𝑙2 + 𝑠)𝑐𝑜𝑠(𝜃1 + 𝜃2) 𝑠𝑒𝑛(𝜃1 + 𝜃2)

J no es cuadrada por lo tanto no tiene determinante .

Singularidad : punto limite de espacio de trabajo

Si det[J]=0 despejaríamos las condiciones que la hacen cero(0)

Hacemos jacobiano 2x2 por ser Planar Hallamos determinante

(−𝑙1 sen 𝜃1 − (𝑙2 + 𝑠)𝑠𝑒𝑛(𝜃1 + 𝜃2) (𝑙2 + 𝑠)𝑐𝑜𝑠(𝜃1 + 𝜃2)

− (𝑙1 cos 𝜃1 + (𝑙2 + 𝑠)𝑐𝑜𝑠(𝜃1 + 𝜃2) −(𝑙2 + 𝑠)𝑠𝑒𝑛(𝜃1 + 𝜃2)

= −𝑙1 sen 𝜃1 (𝑙2 + 𝑠)𝑐𝑜𝑠(𝜃1 + 𝜃2)−(𝑙2 + 𝑠)2𝑠𝑒𝑛(𝜃1 + 𝜃2) 𝑐𝑜𝑠(𝜃1 + 𝜃2)

+ (𝑙1 cos 𝜃1 + (𝑙2 + 𝑠)𝑠𝑒𝑛(𝜃1 + 𝜃2) +(𝑙2 + 𝑠)2𝑠𝑒𝑛(𝜃1 + 𝜃2)𝑐𝑜𝑠(𝜃1 + 𝜃2)

= 𝑙1(𝑙2 + 𝑠) −𝑠𝑒𝑛(𝜃1)𝑐𝑜𝑠(𝜃1 + 𝜃2) + 𝑐𝑜𝑠(𝜃1) 𝑠𝑒𝑛(𝜃1 + 𝜃2)

= 𝑙1(𝑙2 + 𝑠) −𝑠𝑒𝑛 𝜃1 𝑐𝑜𝑠(𝜃1 cos 𝜃2 − 𝑠𝑒𝑛 𝜃1 sen 𝜃2 )

+ 𝑐𝑜𝑠(𝜃1 𝑠𝑒𝑛(𝜃1 cos 𝜃2 + cos 𝜃1 𝑠𝑒𝑛 𝜃2 )

= 𝑙1(𝑙2 + 𝑠) −𝑠𝑒𝑛 𝜃1 𝑐𝑜𝑠(𝜃2 + 𝑠𝑒𝑛2(𝜃1)sen 𝜃2 ) + cos 𝜃1 𝑠𝑒𝑛 𝜃1 cos 𝜃2

+ cos2 𝜃1 𝑠𝑒𝑛(𝜃2)

= 𝑙1(𝑙2 + 𝑠) 𝑠𝑒𝑛2(𝜃1)sen 𝜃2 + cos2 𝜃1 𝑠𝑒𝑛(𝜃2)

= 𝑙1(𝑙2 + 𝑠) 𝑠𝑒𝑛 𝜃2 (𝑠𝑒𝑛2 𝜃1 + cos2 𝜃1 )

= 𝑙1(𝑙2 + 𝑠) 𝑠𝑒𝑛 𝜃2

Hacemos el determinante igual 0



𝑙1(𝑙2 + 𝑠) 𝑠𝑒𝑛 𝜃2 = 0

𝜃2 = 0, 𝜋

El valor absoluto del determinante en f para un valor de 𝜃2nos da el factor en el cual a junción f

expande o contrae su volumen(area en este caso) el valor para el punto limite del espacio de

trabajoestatra dado cuando :

𝜃2 = 0, 𝜋 𝑦 𝑠 = 50𝑐𝑚

Cinemática inversa

De la solución geométrica

𝑃𝑥 = 𝑙1 cos 𝜃1 + (𝑙2 + 𝑠)cos(𝜃1 + 𝜃1)

𝑃𝑦 = 𝑙1 sen 𝜃1 + (𝑙2 + 𝑠)sen(𝜃1 + 𝜃1)

𝑃𝑧 = 0

𝑃𝑥2+𝑃𝑦

2 = 𝑙12 cos2 𝜃1 + 2(𝑙1 cos 𝜃1 (𝑙2 + 𝑠)cos(𝜃1 + 𝜃2) + (𝑙2 + 𝑠)2 + cos2(𝜃1 + 𝜃2)

+ 𝑙12 sen2 𝜃1 + 2 (𝑙1 sen 𝜃1 (𝑙2 + 𝑠)𝑠𝑒𝑛(𝜃1 + 𝜃2) + (𝑙2 + 𝑠)2 + 𝑠𝑒𝑛2(𝜃1 + 𝜃2)

= 𝑙12 (cos2 𝜃1 + sen2 𝜃1 ) + (𝑙2 + 𝑠)2 cos2(𝜃1 + 𝜃2 + 𝑠𝑒𝑛2(𝜃1 + 𝜃2))

+ 2𝑙1 (𝑙2 + 𝑠)(cos 𝜃1 cos(𝜃1 + 𝜃2) + (sen 𝜃1 sen(𝜃1 + 𝜃2))

= 𝑙12 + (𝑙2 + 𝑠)2 + 2𝑙1(𝑙2 + 𝑠) (𝑙2 + 𝑠)(cos 𝜃1 cos(𝜃1 + 𝜃2) + (sen 𝜃1 sen(𝜃1 + 𝜃2))

= 𝑙12 + (𝑙2 + 𝑠)2 + 2𝑙1(𝑙2 + 𝑠)(cos 𝜃2 (cos2 𝜃1 + 𝑠𝑒𝑛2(𝜃1))

= 𝑙12 + (𝑙2 + 𝑠)2 + 2𝑙1(𝑙2 + 𝑠) cos2 𝜃1 cos 𝜃2 − cos 𝜃1 sen 𝜃1 𝑠𝑒𝑛(𝜃2

+ 𝑠𝑒𝑛 𝜃1 𝑠𝑒𝑛 𝜃1 cos 𝜃1 + cos 𝜃2 + 𝑠𝑒𝑛2 𝜃1

= 𝑙12 + (𝑙2 + 𝑠)2 + 2𝑙1(𝑙2 + 𝑠) (cos 𝜃2 (cos2 𝜃2 +𝑠𝑒𝑛2 𝜃1 )

𝑃𝑥2+𝑃𝑦

2 = 𝑙12(𝑙2 + 𝑠)2 + (2𝑙1( 𝑙2 + 𝑠) cos(𝜃2)

Despejando



cos 𝜃2 =𝑃𝑥

2 + 𝑃𝑦2 − 𝑙1

2(𝑙2 + 𝑠)2

(2𝑙1( 𝑙2 + 𝑠)

𝜃2 = cos−1 𝑃𝑥

2 + 𝑃𝑦2 − 𝑙1

2(𝑙2 + 𝑠)2

(2𝑙1( 𝑙2 + 𝑠)

Para obtener 𝜃1 hacemos

𝑇 = 𝐴01 𝐴1

2 = 𝑛 𝑜 𝑎 𝑝0 0 0 1

Pre multiplicando por la inversa de 𝐴01 se tiene

𝐴12 = 𝐴0

1 −1

𝑛 𝑜 𝑎 𝑝0 0 0 1

(𝐴10)−1 = (𝐴0

1 )

cos(𝜃1) 𝑠𝑒𝑛(𝜃1) 0 −𝑙1𝑠𝑒𝑛 (𝜃1) cos(𝜃1) 0 0

0 0 1 00 0 0 1

Con lo que:

𝐴01 . 𝑇 = 𝐴1

2

Una matriz homogénea sirve para transformar un vector expresado en coordenadas homogéneas

con respecto a un sistema O’UVW, asu expresión en las corfenadasde referencia OXYZ. También se

puede utilizar para rotar y girar un vector referido a un sistema de referencia fijo, para expreasar

la orientación y la posisicon de un sistema O’UVW con respecto a otro fijo OXYZ

La matriz T de transformación se escriba de esta forma.

𝑇 =

𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑃𝑥𝑛𝑦 𝑜𝑦 𝑎𝑦 𝑃𝑦𝑛𝑧 𝑜𝑧 𝑎𝑧 𝑃𝑧0 0 0 1

= 𝑛 𝑜 𝑎 𝑝0 0 0 1

En nuestro caso



𝑇 = 𝐴10 𝐴2

1 = 𝑛 𝑜 𝑎 𝑝0 0 0 1

𝐴21 = (𝐴1

0)−1 𝑛 𝑜 𝑎 𝑝0 0 0 1

(𝐴10)−1 = (𝐴0

1 )

cos(𝜃1) 𝑠𝑒𝑛(𝜃1) 0 −𝑙1𝑠𝑒𝑛 (𝜃1) cos(𝜃1) 0 0

0 0 1 00 0 0 1

Con lo que :

𝐴01

−1 .𝑇

= 𝐴12

cos1𝑛𝑥 + 𝑆1𝑛𝑦 cos1𝑜𝑥 + 𝑆1𝑜𝑦 cos1𝑎𝑥 + 𝑠𝑒𝑛1𝑎𝑦 cos1𝑝𝑥 + 𝑠𝑒𝑛1𝑝𝑦 − 𝑙1− sen1𝑛𝑥 + 𝑐𝑜𝑠1𝑛𝑦 sen1𝑜𝑥 + 𝑐𝑜𝑠1𝑜𝑦 sen1𝑎𝑥 + 𝑐𝑜𝑠1𝑎𝑦 −sen1𝑝𝑥 + 𝑐𝑜𝑠1𝑝𝑦

𝑛𝑧 𝑜𝑧 𝑎𝑧 𝑝𝑧

0 0 0 1

=

cos(θ2) −𝑠𝑒𝑛(𝜃2) 0 (𝑙2 + 𝑠)cos(𝜃2) 𝑠𝑒𝑛(𝜃2) cos(θ2) 0 (𝑙2 + 𝑠)𝑠𝑒𝑛(𝜃2)

0 0 1 00 0 0 1

𝑐1𝑝𝑥 + 𝑠1𝑝𝑦 − 𝑙1 = (𝑙2 + 𝑠)𝑐2

−s1𝑝𝑥 + 𝑐1𝑝𝑦 = (𝑙2 + 𝑠)𝑠2

Luego

(𝑙2 + 𝑠)

(𝑙2 + 𝑠)𝑐2 + 𝑙1=

𝑐1𝑝𝑥 − 𝑠1𝑝𝑥

𝑐1𝑝𝑥 + 𝑠1𝑝𝑦

Dividiendo todo por 𝑐1𝑝𝑥

(𝑙2 + 𝑠)

(𝑙2 + 𝑠)𝑐2 + 𝑙1=

𝑝𝑦

𝑝𝑥− 𝑇1

1 + 𝑇1

𝑝𝑦

𝑝𝑥

Teniendo en cuenta

𝑇 𝑎 − 𝑏 =𝑇𝑎 − 𝑇𝑏

1 + 𝑇𝑎𝑇𝑏



(𝑙2𝑠2)

(𝑙2𝑠2 + 𝑙1)= 𝑇 𝑎𝑟𝑐𝑡𝑔

𝑝𝑦

𝑝𝑥− 𝑞1

De donde

𝑎𝑟𝑐𝑡𝑔 (𝑙2𝑠2)

(𝑙2𝑐2 + 𝑙1)= 𝑎𝑟𝑐𝑡𝑔

𝑝𝑦

𝑝𝑥− 𝑞1

luego

𝑞1 = 𝑎𝑟𝑐𝑡𝑔 𝑝𝑦

𝑝𝑥− 𝑎𝑟𝑐𝑡𝑔

(𝑙2𝑠2)

(𝑙2𝑐2 + 𝑙1)

Por lo que l modelo cinemático es :

𝑞1 = 𝑎𝑟𝑐𝑡𝑔 𝑝𝑦

𝑝𝑥− 𝑎𝑟𝑐𝑡𝑔

(𝑙2𝑠2)

(𝑙2𝑐2 + 𝑙1)

Trayectorias

Trayectoria en línea recta velocidad uniforme vc

𝑣 = 𝑠 = 𝑣𝑐 𝑐𝑜𝑛𝑠𝑑𝑡𝑎𝑛𝑡𝑒

𝑠 = 𝑠 = 𝑣𝑐 𝑑𝑡 = 𝑣𝑐𝑡

𝑚 =𝑉𝑐 − 0

𝑡𝑎 − 0 𝑒𝑐 𝑙𝑖𝑛𝑒𝑎 𝑞𝑢𝑒 𝑝𝑎𝑠𝑎 𝑝𝑜𝑟 0,0

𝑦 = 𝑚𝑥 normalizada vc=1

𝑠 =𝑡

𝑇 0 ≤ 𝑡 ≤ 𝑇



𝑠 =𝑑

𝑑𝑡 𝑠 =

𝑑(𝑣𝑐)

𝑑𝑡= 0

b) trayectoria en línea recta, velocidad variable por tramos

𝑣 = 𝑠 𝑣𝑒𝑙𝑜𝑐𝑖𝑑𝑎𝑑

tramo 1: aceleración de o a vc durante ta

tramo 2: velocidad constante vc entre ta y T-td

tramo 3: desaceleración de Vc a 0 durante td

Trayectoria para una línea recta

Tramo 1 aceleración

𝑚 =𝑉𝑐 − 0

𝑡𝑎 − 0=

𝑉𝑐

𝑡𝑎

𝑠 =𝑣𝑐

𝑡𝑎𝑡

𝑦 = 𝑚𝑥 𝑒𝑐𝑢𝑎𝑐𝑖𝑜𝑛 𝑝𝑎𝑠𝑎 𝑝𝑜𝑟 𝑒𝑙 𝑜𝑟𝑖𝑔𝑒𝑛

Tramo 2 velocidad constante vc

𝑣 = 𝑘𝑡𝑒 = 𝑣𝑐

𝑠 = 𝑣𝑐

Tramo 3 desaceleración



𝑚 =𝑉𝑐 − 0

(𝑇 − 𝑡𝑑) − 𝑇= −

𝑉𝑐

𝑡𝑑

𝑦 = 𝑚 𝑥 − 𝑥1 + 𝑦1 𝑒𝑐 𝑝𝑢𝑛𝑡𝑜 𝑝𝑒𝑛𝑑𝑖𝑒𝑛𝑡𝑒

𝑣 = −𝑣𝑐

𝑡𝑑 𝑡 − 𝑇 − 𝑡𝑑 + 𝑣𝑐

𝑠 = 𝑣 = 𝑣𝑐 −𝑣𝑐

𝑡𝑑 𝑡 − 𝑇 − 𝑡𝑑

Despejamos

𝑠 = 𝑠

Tramo 1

𝑠 𝑡 =𝑣𝑐

𝑡𝑎.𝑡2

𝑡𝑎 0 ≤ 𝑡 ≤ 𝑡𝑎

Tramo 2

𝑠 𝑡 = 𝑣𝑐 𝑡 −𝑣𝑐

2𝑡𝑎. 𝑡𝑎 ≤ 𝑡 ≤ 𝑇 − 𝑡𝑑

Tramo 3

𝑠 𝑡 = 𝑣𝑐 𝑡 −𝑣𝑐

𝑡𝑑 𝑡2

2− 𝑇 − 𝑡𝑑 𝑡 −

𝑣𝑐 𝑡𝑎

2 −

𝑣𝑐 ( 𝑇 − 𝑡𝑑)2

2𝑡𝑑, 𝑇 − 𝑡𝑑 ≤ 𝑡 ≤ 𝑇

𝑣𝑐 =2

2𝑇 − 𝑡𝑎 − 𝑡𝑑

Trayectoria para una line a recta horizontal

Sea

𝑤𝑖 = 5500

𝑝𝑜𝑠𝑖𝑐𝑖𝑜𝑛 𝑖𝑛𝑖𝑐𝑖𝑎𝑙

𝑤𝑓 = 8000

𝑝𝑜𝑠𝑖𝑐𝑖𝑜𝑛 𝐹𝑖𝑛𝑎𝑙

Si 𝑙1 = 𝑙225cm

a) Calculo del movimiento con velocidad uniforme



𝑆 𝑡 =𝑑𝑠(𝑡)

𝑑𝑡=

𝑑

𝑑𝑡 𝑡

𝑇 𝑑𝑡 con t=10 seg

𝑠 𝑡 =𝑡

10

𝑤 𝑡 = 𝑤𝑖

.

. 1 − 𝑠(𝑡) + 𝑠(𝑡)

𝑤𝑓

.

.

𝑤 𝑡 = 5500

1 −𝑡

10 +

𝑡

10 8000

Punto inicial

𝑡 = 0 , 𝑥: 𝑤 𝑡 = 0 = 𝑤𝑥(0) = 55 1 −0

10 +

0

10 80 = 55 = 𝑝𝑥

𝑦: 𝑤 𝑡 = 0 = 𝑤𝑦 0 = 0 1 −0

10 +

0

10 0 = 0 = 𝑝𝑦

𝑧: 𝑤 𝑡 = 0 = 𝑤𝑧(0) = 0 1 −0

10 +

0

10 0 = 0 = 𝑝𝑧

Punto final

𝑡 = 10 , 𝑥: 𝑤 𝑡 = 10 = 𝑤𝑥(10) = 55 1 −10

10 +

10

10 80 = 80 = 𝑝𝑥

𝑦: 𝑤 𝑡 = 10 = 𝑤𝑦(10) = 0 1 −10

10 +

0

10 0 = 0 = 𝑝𝑦

𝑧: 𝑤 𝑡 = 10 = 𝑤𝑧(10) = 0 1 −10

10 +

0

10 0 = 0 = 𝑝𝑧

Trayectoria del Px durante 10 segundos de recorrido

t 0 1 2 3 4 5 6 7 8 9 10

px 55 57.5 60 62.5 65 67 70 72.5 75 77.5 80

py 0 0 0 0 0 0 0 0 0 0 0

𝑃𝑥 = 55 −55𝑡

10+

𝑡

1080 𝑝𝑥 = 55 +

25𝑡

10

Se usa Mat lab para calcular px para en la tabla desde 0 a 10

𝑤 𝑡 = 1 𝑝𝑥 = 𝑝𝑥 = 55 +25(1)

10= 57.5

Usando el siguente programa



for t=0:10 px=(55-(55*(t/10))+(t/10)*80); t,px

end

t = 0, x = 55

t = 1, px =57.5000

t = 2, px = 60

t = 3, px = 62.5000

t = 4, px = 65

t = 5, px = 67.5000

t = 6, px = 70

t = 7, px = 72.5000

t =8, px = 75

t =9, px = 77.5000

t =10, px = 80

Trayectoria de las articulaciones


2 + 𝑃𝑦2 − 𝑙1

2 − (𝑙2 + 𝑠)2

(2𝑙1( 𝑙2 + 𝑠)

𝜃1 = 𝑡𝑎𝑛−1 𝑝𝑦

𝑝𝑥 − 𝑡𝑎𝑛−1

(𝑙2 + 𝑠)𝑠𝑒𝑛(𝜃2)

(𝑙2 + 𝑠)𝑐𝑜𝑠 𝜃2 + 𝑙1

Hallamos 𝜃2 con 𝑝𝑥 = 55, 𝑝𝑦 = 0, 𝑙1 = 𝑙2 = 25, 𝑠 = 5


2 + 0 − 252 − (25 + 5)2

(2(25)( 25 + 5)

Usamos matlab

> px=55

>> py=0

>> l1=25

>> l2=25

>> t2=acos((px^2+py-l1^2-(l2+s)^2)/(2*l1*(l2+s)))

t2 = 0

Hallamos 𝜃1



𝜃1 = 𝑡𝑎𝑛−1 𝑝𝑦

𝑝𝑥 − 𝑡𝑎𝑛−1

(𝑙2 + 𝑠)𝑠𝑒𝑛(𝜃2)

(𝑙2 + 𝑠)𝑐𝑜𝑠 𝜃2 + 𝑙1

La trayectoria de la artuiculaciones l1=25, con θ1 = 0° y l2=25 con θ2 =0°

En matlab

s=5 l1=25 l2=25

t2=acos((px^2+py-l1^2-(l2+s)^2)/(2*l1*(l2+s))) t1=atan(px/py)-atan(((l2+s)*sin(t2))/((l2+s)*cos(t2)+l1))

si θ1 = 0°, θ2 = 0°, 5 ≤ s ≤ 30 el desplazamiento se realiza horizontalmente

t 0 1 2 3 4 5 6 7 8 9 10

θ1 0 0 0 0 0 0 0 0 0 0 0

θ2 0 0 0 0 0 0 0 0 0 0 0

Px 55 57.5 60 62.5 65 67 70 72.5 75 77.5 80

s 5 7.5 10 12.5 15 17.5 20 22.5 25 27.5 30

Trayectoria de las coordenadas

Trayectorias delas articulaciones

DINAMICA



Newton euler (Paso 1)

D-H

θi di ai i

1 Θ1 0 L1 0

2 Θ2 0 L2 0

3 Θ3 0 L3 0


𝑅01 =

cos(𝜃1) −𝑠𝑒𝑛(𝜃1) 0𝑠𝑒𝑛 (𝜃1) cos(𝜃1) 0

0 0 1

𝑅12 =


0 0 1

𝑅23 =

1 0 00 1 00 0 1

Hallamos las transpuestas

𝑅𝑖−1𝑖 =

cos(𝜃1) 𝑠𝑒𝑛(𝜃1) 0−𝑠𝑒𝑛 (𝜃1) cos(𝜃1) 0

0 0 1

𝑅10 =


0 0 1

𝑅21 =


0 0 1

𝑅32 =

1 0 00 1 00 0 1


Se establecen las condiciones iniciales para una base sin movimiento

𝑤00 =

00 0 𝑤 0

0 = 00 0



𝑣00 =

00 0 𝑣 0

0 = 00 𝑔

𝑧0 = 00 1

Las coordenadas del sistema en el origen respecto al sistema S i-1

𝑃𝑖 𝑖 =

𝑎𝑖

𝑑𝑖 𝑠𝑒𝑛 𝑑𝑖

𝑑𝑖 𝑐𝑜𝑠 𝑑𝑖

𝑃11 =

𝑙100 𝑃2

2 = 𝑙2

00 𝑃3

3 = 𝑠00

Coordenadas de centros de masas i respecto del sistemas Si

𝑠𝑖 𝑖 = 000 𝑠1

1 = 000 𝑠2

2 = 000 𝑠3

3 = 000

𝐼𝑖 𝑖Define la matriz intercepta del eslabón i respecto a su centro de masa

𝐼11 =

0 0 00 0 00 0 0

𝐼22 =

0 0 00 0 00 0 0

𝐼33 =

0 0 00 0 00 0 0


Se obtienen las velocidades angulares del sistema { Si }:

𝑤𝑖 𝑖 = 𝑅𝑖 𝑖−1 𝑤𝑖−1 + 𝑧0𝑞𝑖

𝑖−1 (1)

𝑅𝑖 𝑖−1 𝑤𝑖−1 𝑖−1 (2)

(1) 𝑠𝑖 𝑒𝑙𝑒𝑠𝑙𝑎𝑏𝑜𝑛 𝑒𝑠 𝑑𝑒 𝑟𝑜𝑡𝑎𝑐𝑖𝑜𝑛

(2) 𝑠𝑖 𝑒𝑙𝑒𝑠𝑙𝑎𝑏𝑜𝑛 𝑒𝑠 𝑑𝑒 𝑡𝑟𝑎𝑛𝑠𝑙𝑎𝑐𝑖𝑜𝑛

Para la primera rotación (𝜃1)



𝑤11 = 𝑅1

0 𝑤0 + 𝑧0𝜃1 0

𝑤11 =


0 0 1

000 +

001 𝜃1

= 00𝜃1

Para la segunda rotacion(𝜃2):

𝑤22 = 𝑅2

1 𝑤1 + 𝑧0𝜃2 1

𝑤22 =


0 0 1

000 +

001 𝜃1

=

00

𝜃1 + 𝜃2

Para la translación

𝑤33 = 𝑅3

2 . 𝑤22

𝑤33 =

1 0 00 1 00 0 1

00

𝜃1 + 𝜃2

=

00

𝜃1 + 𝜃2


Obtenemos la aceleración angular del sistema

𝑤 𝑖𝑖 =

𝑅𝑖 𝑖−1 𝑤 𝑖−1 + 𝑧0𝑞𝑖 𝑖−1 + 𝑤𝑖−1 + 𝑧0𝑞𝑖

𝑖−1 (1)

𝑅𝑖 𝑖−1 𝑤 𝑖−1 𝑖−1 (2)

(1) 𝑠𝑖 𝑒𝑙𝑒𝑠𝑙𝑎𝑏𝑜𝑛 𝑒𝑠 𝑑𝑒 𝑟𝑜𝑡𝑎𝑐𝑖𝑜𝑛

(2) 𝑠𝑖 𝑒𝑙𝑒𝑠𝑙𝑎𝑏𝑜𝑛 𝑒𝑠 𝑑𝑒 𝑡𝑟𝑎𝑛𝑠𝑙𝑎𝑐𝑖𝑜𝑛

Para la primera rotación

𝑤 11 = 𝑅1

0 𝑤 0 + 𝑧0𝑞𝜃1𝑖 0 + 𝑤0 + 𝑧0𝜃1

0

𝑤 11 =


0 0 1

000 +

001 𝜃1

+ 000 ∗

000 𝜃1



𝑤 11 =


0 0 1

00𝜃1 +

000 =

00𝜃1

Para la segunda rotación

𝑤 22 = 𝑅2

1 𝑤 1 + 𝑧0𝜃2 1 + 𝑤1 + 𝑧0𝜃2

1

𝑤 22 =


0 0 1

00𝜃1 +

001 𝜃1

+ 00𝜃1 ∗

001 𝜃2

𝑤 22 =


0 0 1

00

𝜃1 + 𝜃2

+

000 =

00

𝜃1 + 𝜃2


𝑤 33 = 𝑅3

2 𝑤 22

𝑤 33 =

1 0 00 1 00 0 1

00

𝜃1 + 𝜃2

=

00

𝜃1 + 𝜃2


Obtenemos la aceleración lineal del sistema

𝑉𝑖𝑖

𝑤 𝑖𝑖 x 𝑖𝑝𝑖 + 𝑤𝑖 𝑖 x( 𝑤𝑖 𝑝𝑖

𝑖 ) +𝑖 𝑅𝑖 𝑖−1 𝑉 𝑖−1𝑖−1 (1)

𝑅𝑖 𝑖−1 𝑧0+𝑞𝑖 + 𝑉 𝑖−1𝑖−1 + 𝑤1

1x 𝑖𝑝𝑖 + 2 𝑤𝑖 𝑖x 𝑅𝑖 𝑖−1 𝑧0 + 𝑞𝑖 + 𝑤𝑖 𝑖x( 𝑤𝑖 𝑖x 𝑖𝑝𝑖 2

(1) 𝑠𝑖 𝑒𝑠 𝑟𝑜𝑡𝑎𝑐𝑖𝑜𝑛

(2) 𝑠𝑖 𝑒𝑠 𝑑𝑒 𝑡𝑟𝑎𝑛𝑠𝑙𝑎𝑐𝑖𝑜𝑛


𝑉 11 = 𝑤 1

1 x 1𝑝1 + 𝑤11 x( 𝑤1 𝑝1

1 ) +1 𝑅10 𝑉 0

0

𝑉 11 =

0−𝑙1𝜃1

0 x

𝑙100 +

00𝜃1 x

00𝜃1 x

𝑙101 +


0 0 1

00𝑔



𝑉 11 =

0−𝑙1𝜃1

0 +

00𝜃1 x

0−𝑙1𝜃1

0 +

00𝑔

𝑉 11 =

𝑙1𝜃1 2

−𝑙1𝜃1

𝑔


𝑉 22 = 𝑤 2

2 x 2𝑝2 + 𝑤22 x( 𝑤2 𝑝2

2 ) +2 𝑅21 𝑉 1

1

𝑉 22 =

00

𝜃1 + 𝜃 2

x 𝑙2

00 +

00

𝜃1 + 𝜃2

x

00

𝜃1 + 𝜃2

x

𝑙2

00 +


0 0 1

𝑙1𝜃1

2

−𝑙1𝜃1

𝑔

=

𝑙2 𝜃1 + 𝜃2

2

+ 𝑙1 (cos 𝜃 22 − 𝑠𝑒𝑛𝜃2𝜃1)

𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1

2+ cos𝜃2𝜃1

𝑔


𝑉 33 = 𝑅3

2 𝑧0+𝑞3 + 𝑉 22 + 𝑤2

2x 2𝑝2 + 2 𝑤22x 𝑅3

2 𝑧0 + 𝑞3 + 𝑤33x( 𝑤3

3x 3𝑝3

𝑉 33 =

1 0 00 1 00 0 1

001 𝑆 +

𝑙2 𝜃1 + 𝜃2

2


𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1

2+ cos𝜃2𝜃1

𝑠 + 𝑔

+ 00

𝜃1 + 𝜃2

x

𝑠00

+ 2

00

𝜃1 + 𝜃2

x

1 0 00 1 00 0 1

001 𝑆 +

00

𝜃1 + 𝜃2

x

00

𝜃1 + 𝜃2

x

𝑠00

𝑉 33 =

𝑙2 𝜃1 + 𝜃2

2


𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1

2+ cos𝜃2𝜃1

𝑠 + 𝑔




Obtenemos la aceleración lineal del centro de gravedad del eslabon 1:

𝑎𝑖 𝑖 = 𝑤𝑖 𝑖 x 𝑠𝑖 + 𝑤𝑖 x 𝑖𝑖 𝑤𝑖 𝑖 x 𝑠𝑖 𝑖 + 𝑣𝑖 𝑖

Luego para 𝑎11

𝑎11 = 𝑤1

1 x 𝑠1 + 𝑤1 x 11 𝑤11 x 𝑠1

1 + 𝑣11

𝑎11 =

00𝜃1 x

000 +

00𝜃1 x

00

𝜃1 + 𝜃2

x

00

𝜃1 + 𝜃2

x

000 +

𝑙1𝜃2 2

−𝑙1𝜃1 2

𝑔

𝑎11 =

𝑙1𝜃2 2

−𝑙1𝜃1 2

𝑔

Luego para 𝑎22

𝑎22 = 𝑤2

2 x 𝑠2 + 𝑤2 x 22 𝑤22 x 𝑠2

2 + 𝑣22

𝑎22 =

00

𝜃1 + 𝜃2

xx

00

𝜃1 + 𝜃2

x

00

𝜃1 + 𝜃2

x

000 +

𝑙2 𝜃1 + 𝜃2

2


𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1

2+ cos𝜃2𝜃1

𝑔

𝑎22 =

𝑙2 𝜃1 + 𝜃2

2


𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1

2+ cos𝜃2𝜃1

𝑔

Luego para 𝑎33

𝑎33 = 𝑤3

3 x 𝑠3 + 𝑤3 x 33 𝑤33 x 𝑠3

3 + 𝑣33



𝑎33 =

00

𝜃1 + 𝜃2

x

000 x

00

𝜃1 + 𝜃2

x

00

𝜃1 + 𝜃2

x

000

+

𝑙2 𝜃1 + 𝜃2

2


𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1

2+ cos𝜃2𝜃1

𝑠 + 𝑔

𝑎33 =

𝑙2 𝜃1 + 𝜃2

2


𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1

2+ cos𝜃2𝜃1

𝑠 + 𝑔


Obtenemos la fuerz ejercida sobre el eslabon i

𝐹𝑖 𝑖 = 𝑅𝑖 𝑖+1 𝐹𝑖+1𝑖+1 + 𝑚𝑖 𝑎𝑖 𝑖

Condiciones iníciales

𝐹44 = 0

Luego 𝐹33

𝐹33 = 𝑅3

4 𝐹44 + 𝑚3 𝑎3

3

𝐹33 =

1 0 00 1 00 0 1

000 + 𝑚3

𝑙2 𝜃1 + 𝜃2

2


𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1

2+ cos𝜃2𝜃1

𝑠 + 𝑔

𝐹33 = 𝑚3

𝑙2 𝜃1 + 𝜃2

2


𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1

2+ cos𝜃2𝜃1

𝑠 + 𝑔

Luego 𝐹22

𝐹22 = 𝑅2

3 𝐹33 + 𝑚2 𝑎2

2



𝐹22 =

1 0 00 1 00 0 1

𝑚3

𝑙2 𝜃1 + 𝜃2

2


𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1

2+ cos𝜃2𝜃1

𝑠 + 𝑔

+ 𝑚2

𝑙2 𝜃1 + 𝜃2

2


𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1

2+ cos𝜃2𝜃1

𝑔

𝐹22 = 𝑚3

𝑙2 𝜃1 + 𝜃2

2


𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1

2+ cos𝜃2𝜃1

𝑠 + 𝑔

+ 𝑚2

𝑙2 𝜃1 + 𝜃2

2


𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1

2+ cos𝜃2𝜃1

𝑔

Luego 𝐹11

𝐹11 = 𝑅1

2 𝐹22 + 𝑚1 𝑎1

1

𝐹11 =


0 0 1

𝑚3

𝑙2 𝜃1 + 𝜃2

2


𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1

2+ cos𝜃2𝜃1

𝑠 + 𝑔

+ 𝑚2

𝑙2 𝜃1 + 𝜃2

2


𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1

2+ cos𝜃2𝜃1

𝑔

+ 𝑚1 𝑙1𝜃2

2

−𝑙1𝜃1 2

𝑔


Obtenemos el par ejercido sobre el eslabon i:

𝑛𝑖 =𝑖 𝑅𝑖 𝑖+1 𝑛𝑖+1𝑖+1 + 𝑅𝑖+1

𝑖 𝑝𝑖 𝑖 x 𝐹𝑖+1𝑖+1 + 𝑝𝑖 𝑖 𝑠𝑖 𝑖 x 𝑚𝑖 𝑎𝑖 𝑖 + 𝐼𝑖 𝑖 𝑤𝑖 𝑖

+ 𝑤𝑖 𝑖 x 𝐼𝑖 𝑖 𝑤𝑖 𝑖

Luego para 𝑛3 =3

𝑛3 =3 𝑅34 𝑛4

4 + 𝑅43 𝑝3

3 x 𝐹44 + 𝑝3

3 𝑠33 x 𝑚3 𝑎3

3 + 𝐼33 𝑤3

3 + 𝑤33 x 𝐼3

3 𝑤33



𝑛3 =3 000

000 +

000 +

𝑠00 +

000 x m3

𝑙2 𝜃1 + 𝜃2

2


𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1

2+ cos𝜃2𝜃1

𝑠 + 𝑔

+ 0 0 00 0 00 0 0

+ 00

𝜃 1 + 𝜃 2

+ 00

𝜃 1 + 𝜃 2

x 0 0 00 0 00 0 0

00

𝜃 1 + 𝜃 2

𝑛2 =2 𝑅23 𝑛3

3 + 𝑅32 𝑝2

2 x 𝐹33 + 𝑝2

2 𝑠23 x 𝑚3 𝑎3

3 + 𝐼33 𝑤3

3 + 𝑤33 x 𝐼3

3 𝑤33

𝑛1 =1 𝑅12 𝑛2

2 + 𝑅21 𝑝1

1 x 𝐹22 + 𝑝1

1 𝑠12 x 𝑚2 𝑎2

2 + 𝐼22 𝑤2

2 + 𝑤22 x 𝐼2

2 𝑤22


Obtenemos la fuerza o par aplicadoa la articulación i:

𝜏𝑖 = 𝑛𝑖 𝑖

𝑇 𝑅𝑖 𝑖−1 𝑧0 (1)

𝐹𝑖 𝑖𝑇 𝑅𝑖 𝑖−1 𝑧0 2

(1) 𝑠𝑖 𝑒𝑠 𝑟𝑜𝑡𝑎𝑐𝑖𝑜𝑛

(2) 𝑠𝑖 𝑒𝑠 𝑑𝑒 𝑡𝑟𝑎𝑛𝑠𝑙𝑎𝑐𝑖𝑜𝑛


𝜏3 = 𝑛33𝑇 𝑅2

1 𝑧0


𝜏2 = 𝑛22𝑇 𝑅2

1 𝑧0


𝜏1 = 𝐹𝑖 𝑖𝑇 𝑅1

𝑖0 𝑧0

robotic a

Documents