Robotic A
-
Upload
roberto-carlos-guevara-calume -
Category
Documents
-
view
107 -
download
1
Transcript of 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
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
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
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
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°
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
MODELO GEOMETRICO
figura 3
figura 4
Luego
𝑃𝑥1 = 𝑙1𝑐𝑜𝑠 𝜃1
𝑃𝑥2 = (𝑙2 + 𝑠) cos(𝜃1 + 𝜃2)
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
𝑃𝑥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
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
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
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
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 =
cos(𝜃1) −𝑠𝑒𝑛(𝜃1) 0 𝑙1cos(𝜃1)𝑠𝑒𝑛 (𝜃1) cos(𝜃1) 0 𝑙1sen(𝜃1)
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 ]
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
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
cos(𝜃2) −𝑠𝑒𝑛(𝜃2) 0 𝑙2cos(𝜃2)𝑠𝑒𝑛 (𝜃2) cos(𝜃2) 0 𝑙2sen(𝜃2)
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
𝑇
=
cos(𝜃1) −𝑠𝑒𝑛(𝜃1) 0 𝑙1cos(𝜃1)𝑠𝑒𝑛 (𝜃1) cos(𝜃1) 0 𝑙1sen(𝜃1)
0 0 1 00 0 0 1
.
cos(𝜃2) −𝑠𝑒𝑛(𝜃2) 0 𝑙2cos(𝜃2)𝑠𝑒𝑛 (𝜃2) cos(𝜃2) 0 𝑙2sen(𝜃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
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
>> 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 ]
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 =
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
[ 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
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
= 𝑃𝑥
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)
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
𝜕
𝜕𝑠𝑃𝑦 =
𝜕
𝜕𝑠(𝑙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
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
𝑙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
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
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
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
𝑇 = 𝐴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 + 𝑇𝑎𝑇𝑏
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
(𝑙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 ≤ 𝑡 ≤ 𝑇
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
𝑠 =𝑑
𝑑𝑡 𝑠 =
𝑑(𝑣𝑐)
𝑑𝑡= 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
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
𝑚 =𝑉𝑐 − 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
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
𝑆 𝑡 =𝑑𝑠(𝑡)
𝑑𝑡=
𝑑
𝑑𝑡 𝑡
𝑇 𝑑𝑡 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
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
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 = cos−1 𝑃𝑥
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 = cos−1 𝑃𝑥
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
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
𝜃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
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
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
Newton euler (Paso 2)
𝑅01 =
cos(𝜃1) −𝑠𝑒𝑛(𝜃1) 0𝑠𝑒𝑛 (𝜃1) cos(𝜃1) 0
0 0 1
𝑅12 =
cos(𝜃2) −𝑠𝑒𝑛(𝜃2) 0𝑠𝑒𝑛 (𝜃2) cos(𝜃2) 0
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 =
cos(𝜃1) 𝑠𝑒𝑛(𝜃1) 0−𝑠𝑒𝑛 (𝜃1) cos(𝜃1) 0
0 0 1
𝑅21 =
cos(𝜃2) 𝑠𝑒𝑛(𝜃2) 0−𝑠𝑒𝑛 (𝜃2) cos(𝜃2) 0
0 0 1
𝑅32 =
1 0 00 1 00 0 1
Newton euler (Paso 3)
Se establecen las condiciones iniciales para una base sin movimiento
𝑤00 =
00 0 𝑤 0
0 = 00 0
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
𝑣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
Newton euler (Paso 4)
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)
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
𝑤11 = 𝑅1
0 𝑤0 + 𝑧0𝜃1 0
𝑤11 =
cos(𝜃1) 𝑠𝑒𝑛(𝜃1) 0−𝑠𝑒𝑛 (𝜃1) cos(𝜃1) 0
0 0 1
000 +
001 𝜃1
= 00𝜃1
Para la segunda rotacion(𝜃2):
𝑤22 = 𝑅2
1 𝑤1 + 𝑧0𝜃2 1
𝑤22 =
cos(𝜃1) 𝑠𝑒𝑛(𝜃1) 0−𝑠𝑒𝑛 (𝜃1) cos(𝜃1) 0
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
Newton euler (Paso 5)
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 =
cos(𝜃1) 𝑠𝑒𝑛(𝜃1) 0−𝑠𝑒𝑛 (𝜃1) cos(𝜃1) 0
0 0 1
000 +
001 𝜃1
+ 000 ∗
000 𝜃1
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
𝑤 11 =
cos(𝜃1) 𝑠𝑒𝑛(𝜃1) 0−𝑠𝑒𝑛 (𝜃1) cos(𝜃1) 0
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 =
cos(𝜃2) 𝑠𝑒𝑛(𝜃2) 0−𝑠𝑒𝑛 (𝜃2) cos(𝜃2) 0
0 0 1
00𝜃1 +
001 𝜃1
+ 00𝜃1 ∗
001 𝜃2
𝑤 22 =
cos(𝜃1) 𝑠𝑒𝑛(𝜃1) 0−𝑠𝑒𝑛 (𝜃1) cos(𝜃1) 0
0 0 1
00
𝜃1 + 𝜃2
+
000 =
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
Newton euler (Paso 6)
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) 𝑠𝑖 𝑒𝑠 𝑑𝑒 𝑡𝑟𝑎𝑛𝑠𝑙𝑎𝑐𝑖𝑜𝑛
Para la primera rotación
𝑉 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 +
cos(𝜃1) 𝑠𝑒𝑛(𝜃1) 0−𝑠𝑒𝑛 (𝜃1) cos(𝜃1) 0
0 0 1
00𝑔
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
𝑉 11 =
0−𝑙1𝜃1
0 +
00𝜃1 x
0−𝑙1𝜃1
0 +
00𝑔
𝑉 11 =
𝑙1𝜃1 2
−𝑙1𝜃1
𝑔
Para la segunda rotación
𝑉 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 +
cos(𝜃2) 𝑠𝑒𝑛(𝜃2) 0−𝑠𝑒𝑛 (𝜃2) cos(𝜃2) 0
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
𝑔
Para la translación
𝑉 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
+ 𝑙1 (cos 𝜃 22 − 𝑠𝑒𝑛𝜃2𝜃1)
𝑙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
+ 𝑙1 (cos 𝜃 22 − 𝑠𝑒𝑛𝜃2𝜃1)
𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1
2+ cos𝜃2𝜃1
𝑠 + 𝑔
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
Newton euler (Paso 7)
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
+ 𝑙1 (cos 𝜃 22 − 𝑠𝑒𝑛𝜃2𝜃1)
𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1
2+ cos𝜃2𝜃1
𝑔
𝑎22 =
𝑙2 𝜃1 + 𝜃2
2
+ 𝑙1 (cos 𝜃 22 − 𝑠𝑒𝑛𝜃2𝜃1)
𝑙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
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
𝑎33 =
00
𝜃1 + 𝜃2
x
000 x
00
𝜃1 + 𝜃2
x
00
𝜃1 + 𝜃2
x
000
+
𝑙2 𝜃1 + 𝜃2
2
+ 𝑙1 (cos 𝜃 22 − 𝑠𝑒𝑛𝜃2𝜃1)
𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1
2+ cos𝜃2𝜃1
𝑠 + 𝑔
𝑎33 =
𝑙2 𝜃1 + 𝜃2
2
+ 𝑙1 (cos 𝜃 22 − 𝑠𝑒𝑛𝜃2𝜃1)
𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1
2+ cos𝜃2𝜃1
𝑠 + 𝑔
Newton euler (Paso 8)
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
+ 𝑙1 (cos 𝜃 22 − 𝑠𝑒𝑛𝜃2𝜃1)
𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1
2+ cos𝜃2𝜃1
𝑠 + 𝑔
𝐹33 = 𝑚3
𝑙2 𝜃1 + 𝜃2
2
+ 𝑙1 (cos 𝜃 22 − 𝑠𝑒𝑛𝜃2𝜃1)
𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1
2+ cos𝜃2𝜃1
𝑠 + 𝑔
Luego 𝐹22
𝐹22 = 𝑅2
3 𝐹33 + 𝑚2 𝑎2
2
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
𝐹22 =
1 0 00 1 00 0 1
𝑚3
𝑙2 𝜃1 + 𝜃2
2
+ 𝑙1 (cos 𝜃 22 − 𝑠𝑒𝑛𝜃2𝜃1)
𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1
2+ cos𝜃2𝜃1
𝑠 + 𝑔
+ 𝑚2
𝑙2 𝜃1 + 𝜃2
2
+ 𝑙1 (cos 𝜃 22 − 𝑠𝑒𝑛𝜃2𝜃1)
𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1
2+ cos𝜃2𝜃1
𝑔
𝐹22 = 𝑚3
𝑙2 𝜃1 + 𝜃2
2
+ 𝑙1 (cos 𝜃 22 − 𝑠𝑒𝑛𝜃2𝜃1)
𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1
2+ cos𝜃2𝜃1
𝑠 + 𝑔
+ 𝑚2
𝑙2 𝜃1 + 𝜃2
2
+ 𝑙1 (cos 𝜃 22 − 𝑠𝑒𝑛𝜃2𝜃1)
𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1
2+ cos𝜃2𝜃1
𝑔
Luego 𝐹11
𝐹11 = 𝑅1
2 𝐹22 + 𝑚1 𝑎1
1
𝐹11 =
cos(𝜃2) −𝑠𝑒𝑛(𝜃2) 0𝑠𝑒𝑛 (𝜃2) cos(𝜃2) 0
0 0 1
𝑚3
𝑙2 𝜃1 + 𝜃2
2
+ 𝑙1 (cos 𝜃 22 − 𝑠𝑒𝑛𝜃2𝜃1)
𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1
2+ cos𝜃2𝜃1
𝑠 + 𝑔
+ 𝑚2
𝑙2 𝜃1 + 𝜃2
2
+ 𝑙1 (cos 𝜃 22 − 𝑠𝑒𝑛𝜃2𝜃1)
𝑙2(𝜃 1 + 𝜃 2) − 𝑙1(sen𝜃2𝜃 12𝑙1𝜃1
2+ cos𝜃2𝜃1
𝑔
+ 𝑚1 𝑙1𝜃2
2
−𝑙1𝜃1 2
𝑔
Newton euler (Paso 9)
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
Mauricio Alberto Correa Villa
Roberto Carlos Guevara Calume
𝑛3 =3 000
000 +
000 +
𝑠00 +
000 x m3
𝑙2 𝜃1 + 𝜃2
2
+ 𝑙1 (cos 𝜃 22 − 𝑠𝑒𝑛𝜃2𝜃1)
𝑙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
Newton euler (Paso 10)
Obtenemos la fuerza o par aplicadoa la articulación i:
𝜏𝑖 = 𝑛𝑖 𝑖
𝑇 𝑅𝑖 𝑖−1 𝑧0 (1)
𝐹𝑖 𝑖𝑇 𝑅𝑖 𝑖−1 𝑧0 2
(1) 𝑠𝑖 𝑒𝑠 𝑟𝑜𝑡𝑎𝑐𝑖𝑜𝑛
(2) 𝑠𝑖 𝑒𝑠 𝑑𝑒 𝑡𝑟𝑎𝑛𝑠𝑙𝑎𝑐𝑖𝑜𝑛
Para la primera rotación
𝜏3 = 𝑛33𝑇 𝑅2
1 𝑧0
Para la segunda rotación
𝜏2 = 𝑛22𝑇 𝑅2
1 𝑧0
Para la translación
𝜏1 = 𝐹𝑖 𝑖𝑇 𝑅1
𝑖0 𝑧0