Instituto Tecnolgico de La Laguna

Robtica

Tarea #6

Obtencin de las trayectorias articulares del Stabli 170XL y

graficacin de su movimiento
Nombre: Osvaldo Muoz Muoz
Nmero de Control: 11130605
Dr. Jos Alfonso Pamanes Garca
Torren, Coah. A 21 de Noviembre de 2014

TAREA #6
Obtenga las trayectorias articulares (grficas ; i = 1, 2, 3, 4, 5 y 6) del
manipulador Stabli 170XL, de la tarea 5, que le permitan al manipulador aplicar
silicn en la pieza mostrada en la figura 1, siguiendo las rutas indicadas con las
orientaciones tambin especificadas, en tiempo T 1=T2=5seg. Con los valores
obtenidos de las variables articulares, anime el modelo del robot de la tarea 5 y
verifique que el rgano terminal describa las rutas deseadas. Proponga
emplazamientos de la pieza y del manipulador con respecto a la estacin de
trabajo de tal manera que las variables articulares se mantengan alejados de sus
valores lmite.
Funcin cicloidal para la graficacin:
() =

1
2

sin (
)
2

Esquema cinemtico del robot Stabli 170XL con marcos ortonormales asignados
10

85

Z0,1

105
X4,5,6

X3

X0,1

Z2

X2

Z3

*Unidades en cm
cmcmcm
Z4,6

Z5

Pieza (las lneas color naranja son las trayectorias deseadas):

Parmetros de Denavit-Hartenberg modificados:

dj=distancia de Zj-1 a Zj en direccin a Xj-1

rj= distancia de Xj-1 a Xj en direccin de Zj
j=ngulo de Zj-1 a Zj visto en Xj-1
j = angulo de Xj-1 a Xj visto en Zj

1 0
0
2 90 d2
3 0 d3
4 90 0
5 -90 0
6 90 0

1 0
2 0
3 0
4 r4
5 0
6 0

Matriz de transformacin homognea general:

1

= [

0

]

1

Matrices elementales obtenidas con los parmetros de Denavit-Hartenberg

modificados:
1
1
0
1 = [
0
0

1
1
0
0

0
0
1
0

3
3
2
3 = [
0
0

3
3
0
0

0 3
0 0
] ;
1 0
0 1

5
0
4
5 = [
5
0

5
0
5
0

0
0
] ;
0
1

0
1
0
0

2
0
1
2 = [
2
0

0
0
] ;
0
1

2
0
2
0

0 2
1 0
] ;
0
0
0
1

4
0
3
4 = [
4
0

4
0
4
0

0
0
1 4
]
0
0
0
1

6
0
5
6 = [
6
0

6
0
6
0

0
1
0
0

0
0
]
0
1

Ecuaciones del modelo directo de posicin obtenidas con la multiplicacin de

matrices elementales (elementos de la cuarta columna de la matrz de 0 a 6:
06 = 1 (23 4 + 2 3 + 2 )
06 = 1 (23 4 + 2 3 + 2 )
06 = 2 3 23 4

06 = 1 (10523 + 852 + 10)

06 = 1 (10523 + 852 + 10)
06 = 852 10523

Frmulas para los ngulos de Euler determinados por las frmulas de la matriz
rotacional para la orientacin del marco h con respecto al marco 0:
= 2(21 , 11 )

= 2({6 [5 (1 23 4 1 4 ) 1 23 5 ] 6 (1 4 + 1 23 4 )}, {6 [5 (1 23 4 + 1 4 ) 1 23 5 ] + 6 (1 4 1 23 4 )})

11
= 2 (31 ,
)

= 2({6 (23 4 5 + 23 5 ) 23 4 6 },

{6 [5 (1 23 4 +1 4 )1 23 5 ]+6 (1 4 1 23 4 )}

= 2 (32 , 33 )
= 2({6 (23 4 5 + 23 5 ) 23 4 6 }, {23 4 5 23 5 })
Con las ecuaciones de movimiento y los ngulos de Euler se procede a realizar el
clculo del modelo inverso de posicin para la graficacin de las rutas calculando
las variables articulares para las posiciones y orientaciones del marco h con
respecto a un marco p (marco de la pieza) deseadas. El clculo es muy complejo
y para la simplificacin del mismo nos auxiliamos del software Symoro y obtuvimos
lo siguiente:
Geometric parameters

ant mu

1
2
3
4
5
6

0
1
2
3
4
5

1
1
1
1
1
1

sigma gamma b

0
0
0
0
0
0

0
0
0
0
0
0

alpha d

0 0
0
0 Pi/2 d2
0 0
d3
0 Pi/2 0
0 -Pi/2 0
0 Pi/2 0

t1 0
t2 0
t3 0
t4 r4
t5 0
t6 0

The given situation is supposed as follows :

0T6 =

sx nx ax Px
sy ny ay Py
sz nz az Pz

Equation type 2
Py*Cos(t1) - Px*Sin(t1) = 0
Solution
t1 = ATAN2(-Py,-Px )
or t1 = ATAN2(Py,Px )

theta r

Equation type 6
X=d2 - Px*Cos(t1) - Py*Sin(t1)
-(r4*Sin(t3)) = d3 + X*Cos(t2) - Pz*Sin(t2)
-(r4*Cos(t3)) = Pz*Cos(t2) + X*Sin(t2)
Solution
e= 1 or e = -1
B1 = -2*d3*Pz
B2 = 2*d3*X
B3 = -d3**2 - Pz**2 + r4**2 - X**2
SQ = (B1*B3 + B2*Sqrt(B1**2 + B2**2 - B3**2)*e)/(B1**2 + B2**2)
CQ = (B2*B3 - B1*Sqrt(B1**2 + B2**2 - B3**2)*e)/(B1**2 + B2**2)
t2 = ATAN2( SQ , CQ )
t3 = ATAN2(-((d3 + X*Cos(t2) - Pz*Sin(t2))/r4) , -((Pz*Cos(t2) + X*Sin(t2))/r4) )
Remark : SQ = Sin(t2)
CQ = Cos(t2)
Equation type 2
X=-(ay*Cos(t1)) + ax*Sin(t1)
Y=-(ax*Cos(t1)*Cos(t2 + t3)) - ay*Cos(t2 + t3)*Sin(t1) - az*Sin(t2 + t3)
X*Cos(t4) + Y*Sin(t4) = 0
Solution
t4 = ATAN2(-X,Y )
or t4 = ATAN2(X,-Y )
Equation type 3
Y=-(Cos(t4)*(ax*Cos(t1)*Cos(t2 + t3) + ay*Cos(t2 + t3)*Sin(t1) + az*Sin(t2 + t3))) (-(ay*Cos(t1)) + ax*Sin(t1))*Sin(t4)
Y1=az*Cos(t2 + t3) - ax*Cos(t1)*Sin(t2 + t3) - ay*Sin(t1)*Sin(t2 + t3)
-Sin(t5) = Y
-Cos(t5) = Y1
Solution
t5 = ATAN2(-Y , -Y1 )
Equation type 3
Y=-(Cos(t4)*(-(sy*Cos(t1)) + sx*Sin(t1))) + (sx*Cos(t1)*Cos(t2 + t3) + sy*Cos(t2 +
t3)*Sin(t1) + sz*Sin(t2 + t3))*Sin(t4)
Y1=-(Cos(t4)*(-(ny*Cos(t1)) + nx*Sin(t1))) + (nx*Cos(t1)*Cos(t2 + t3) + ny*Cos(t2 +
t3)*Sin(t1) + nz*Sin(t2 + t3))*Sin(t4)

-Sin(t6) = Y
-Cos(t6) = Y1
Solution
t6 = ATAN2(-Y , -Y1 )
Con las frmulas para el clculo de las variables articulares para el modelo inverso
de posicin del Stabli 170XL, procedemos con el diseo del programa
considerando:
Ruta 1:
xp

rp
yp

Coordenadas de Oh en el marco p en la ruta1:

= ngulo de rp
= radio de la trayectoria circular
=
=
=
Ecuaciones para el clculo de xpj, ypj y zpj
= cos()
= sin()
= 0
= + ()
= 2
= 2
Orientacin del marco h con respecto al marco p:
= = = 0 ()

Ruta 2:
L

zp

yp
Coordenadas de Oh en el marco p:
= 15 ()
= 0 ()
= ()
= 100
Orientacin del marco h con respecto al marco p:
= 90 ; = 0 ; = 90

()

Matriz para la ubicacin y orientacin del marco de la pieza con respecto al marco
de la estacin de trabajo:
0
0

= [
1
0

0 1
1 0
]
0 0
0 0
1

Para la graficacin de ambas rutas, se debe definir la matriz TphD:

= [
0

Donde:
, , , , , , , & determinan la orientacin del marco h con respecto
al marco de la pieza.
stas orientaciones se obtienen con las matrices de rotaciones de los ngulos de
Euler:

= [
0

0
0] ;
1

=[ 0

0
1 0];
0

1
= [0
0

0
]

Siendo:

Por lo tanto:

= [

+
+

+
+ ]

Y por consiguiente:
= 11
= 21
= 31
= 12
= 22
= 32
= 13
= 23
= 33
, , son las coordenadas de Oh en el marco de la pieza en cada trayectoria,
es decir:
=
=
=
Tambin se necesita la matriz U0 para la transformacin de los puntos de la pinza
del marco h al marco 0:
0 = 6
Donde:

Y
1
0
6
= [
0
0

0
1
0
0

0 0
0 0
]
1 6
0 1

= 6 1

TRAYECTORIAS ARTICULARES DE CADA ELEMENTO ROTACIONAL DEL

STABLI 170XL
Con las ecuaciones y matrices anteriores, se obtuvieron las siguientes trayectorias
de cada variable articular para la graficacin de las 2 rutas de la tarea:

LMITES ARTICULARES DEL STABLI 170XL

RESULTADOS OBTENIDOS CON EL PROGRAMA REALIZADO EN MATLAB

Posicin inicial

Fin de la ruta 1

Posicionamiento para el cambio de orientacin de h con respecto a p de la

ruta 2 (no se aprecia claramente)

Orientacin de h con respecto a p para la ruta 2

Posicionamiento para la ruta 2 (no se aprecia claramente)

Final de la ruta 2