UNIVERSIDAD TECNOLGICA DE LA MIXTECA

INSTITUTO DE ELECTRNICA Y MECATRNICA

INGENIERA EN MECATRNICA

ROBTICA I.
PARCIAL II

REPORTE 1.
Manual de Algoritmo para Software de
Cinemtica Inversa.

PROFESOR:
Dr. Vitaliy Ribak.
PRESENTA:
Ventura Sigenza Yubesny Iriani

Huajuapan de Len, Oaxaca, Mxico, 02 de Septiembre del 2015.

NDICE.

Parmetros Cinemticos del Manipulador PowerCube

Desarrollo del Algoritmo tipo gra .

Procedimiento de ejecucin e ingreso de datos en el Software

Ubicacin de parmetros constantes en el cdigo

10

Ejemplo

11

PARMETROS CINEMTICOS DEL

MANIPULADOR POWERCUBE.
Tabla 1. Parmetros cinemticos.
Articulacin

[]

[]

a[mm]

d[mm]

1
2
3
4
5
6

0
90
0
-90
90
0

90
90
-90
0
90
0

109.7
0
0
174.2
0
0

0
0
334
0
0
229.3

Rango de
Articulacin []
-160 a +160
-105 a +91
-160 a +160
-115 a +115
-120 a +120
-1080 a +1080

MATRICES DE TRANSFORMACIN HOMOGNEAS

0
1 = [ 1
0
0
2

1
2 = [ 2
0
0
3

2
3 = [ 3
0
0

0
0
1
0

1
1
0
0
0
0
1
0

2
2
0
0

0 3
0
3
1
0
0
0

3
4
4 = [
0
0

4
4
0
0

4
5 = [ 5
0
0

0
0
1
0

6
6
5
6 = [
0
0

6
6
0
0

1 1
1 1
]
0
1

1
0
1
0 = [
1
0

0
0
]
0
1
0
0
]
3
1

0
0
1
0

0
1
0
0

1
0
]
0
1

2
0
2
1 = [
2
0

2
0
2
0

0
1
0
0

0
0
]
0
1

3
0
3
2 = [
3
0

3 0
0
3
0

4
3 = [ 4
0
0

0 4 4
0 4 4
]
1
0
0
1

5
5
0
0

1
0
1
0

0
0
]
0
1

5
0
5
4 = [
5
0

0
0
]
6
1

6
5 = [ 6
0
0
3

0
0
1 3
]
0
0
0
1

4
4
0
0

0
0
1
0

0
0
]
0
1

5
0
5
0

0
1
0
0

0
0
]
0
1

6
6
0
0

0
0
1
0

0
0
]
6
1

ALGORITMO TIPO GRA.

PASO 0. Establecer 3 = 0

PASO 1. Calcular 6
6 = 62 + 62

PASO 2. Calcular 1 , 1 , 1
1 =

6
= 1
6

1 =

6
= 1
6

1 = (1 ) arccos 1
Si 160 < 1 < 160 entonces ir al paso 3.
En caso opuesto dar el diagnstico
1 = [El valor 1 encontrado]. El nodo no es alcanzable.
Salir ya que para gra no hay siguiente nodo.
PASO 3. Calcular 4 , 4
6 6 6,13
4
[ 4 ] = [ 6 6 6,23 ]
4
6 6 6,33
Donde el valor del parmetro 6 es el parmetro del PowerCube y 6,3 , = 1,2,3 es el
valor del elemento k-simo de la tercera columna de la matriz 6 .
2

4 = (6 6 6,13 ) + (6 6 6,23 )
4 = 6 6 6,33

()

()

PASO 4. Calcular los valores de 3 , 3 , = 1,2

Para 4 0

() 2

()
()
3 =3 +

3 =

Donde:

14
4

= 1 + 2

42 +42 12 42 +32
24

= 12 32 + 2 ;

= 1 ;

Para 4 = 0
()

(2)

3 = 3

32 + 42 12 42
2(4 1 )

()

(1,2)

3 = 32 (3
()

()

PASO 5. Calcular los valores de 2 =

1 )
()

()

, 2 , 2 , = 1,2

()

()
2

{
= 1,2

1
()
= 3
= 2
3
()
2

()

0 ()
3

3
()

()

2 = ( 2 )( 2 )

i=1
(1)

Si -105 < 2 <91 Entonces

Ir al Paso 6
En caso opuesto dar el diagnstico
(1)

(1)

= 2
5

PASO 5.1
i=2
(2)

Si -105 < 2 <91 Entonces

Ir al Paso 6
En caso opuesto dar el diagnstico
(2)

=
(2)

[ 2 ].
SALIR
()

()

()

PASO 6: Calcular los valores de 4 , cos 4 , 4 .

()
4

9 .

(01 04 )2 3 2 4 2
()
=
= 4
23 4

()

{
(2)

Comentario: 4

4 = (

()

) 1 2 4

1,3() ,4

(1)

= 4

Donde
(01 04 )2 = (4 1 )2 + 04
()

()

4 = (

) 4

1,3() ,4

Donde
1

1,3() ,4

() 1
| () 03 1|
3
4 04() 1

()

Si -115 < 4 <115 Entonces

Ir al Paso 7
En caso opuesto dar el diagnstico

. . 9

(1)

=
(1)

[ 4 ].
Si i=1 entonces ir al paso 5.1
En caso opuesto dar el diagnostico La solucin no existe.
SALIR
()

()

()

PASO 7: Calcular los valores de 5 , cos 5 , 5 .

()
5

(03 06 )2 4 2 6 2
()
=
= 5
26 4

()

()

5 = (
) 1 2 5
()
3 ,4,6
{
()

()

5 = (

) 5

3() ,4,6

Donde
() 0 ()
3 1
0
4 1|
0
6 1

=|
4
3() ,4,6
6
()

()

(03 06 )() = (6 3 )2 + ( 06 03 )2
()

Si -120 < 5 <120 Entonces

Ir al Paso 8
En caso opuesto dar el diagnostico
(1)

=
(1)

[ 5 ].
Si i=1 entonces ir al paso 5.1
En caso opuesto dar el diagnstico La solucin no existe.
SALIR

()

()

()

PASO 8: Calcular los valores de 6 , cos 6 , 6

Calcular 56

()

()

()

= 1,2.

06
5

()

43

()

32

()

21

()

10

()

()

6 = 56 [1,2]
()

6 = 56 [1,1] = 6
6 = (6 ) arccos 6
PASO 9: Definir los valores de ngulo 1 , 2 , , 6 de las articulaciones en
representacin del sistema de control de manipulador Power Cube.
(1)

= 1

(2)

= 1

1
1

(1)

(2)

(1)

(2)

(1)

(2)

= 2

(1)

= 5

(2)

= 5

5
5

(1)

= 2

(1)

=0

(2)

=0

(2)

(1)

= 6

(2)

= 6

(1)

(2)

(2)

= 4

(2)

= 4

EJECUCIN E INGRESO DE DATOS EN EL SOFTWARE.

El programa fue realizado en el lenguaje C en la plataforma del programa Dev C++
para Windows, el usuario podr ejecutar el programa desde el programa Dev C++ o
ejecutando el archivo Cinemtica_Inversa_2do Parcial.exe, sin embargo no se podr
realizar cambio alguno del programa si as lo requiriera.
Para iniciar el programa, el usuario podr ejecutar simplemente el archivo .exe
mencionado con anterioridad. ste abrir la siguiente ventana:

Figura 1. Pantalla de inicio del programa.

Como se puede observar el usuario deber ingresar los datos de la matriz
manualmente. Iniciando con los valores de la primera fila.
Despus de ingresar todos los valores de la matrz 0 6 el programa preguntar si
desea verificar los valores de la matriz ingresada, se deber oprimir el nmero [1] si se desea
verificar los datos o oprimir [2] si no se desea observar la matriz ingresadas.

Figura 2. Opcin para la verificacin de la matriz de entrada.

Posterior a esta opcin, el programa realizar el algoritmo presentado con anterioridad

obteniendo los ngulos admisibles para la configuracin de las articulaciones del actuador.
As mismo realizar el proceso de cinemtica directa para obtener la matrz 0 6 y
de esta manera comparar los resultados de sta con la matriz de entrada, cabe mencionara
que sern resultados aproximados mas no exactos pues debido al manejo de la cantidad de
decimales, stos presentarn un margen de error.
Junto a esta matriz se mostrarn las otras seis para poder realizar los monogramas
manualmente si el usuario lo requiere.

UBICACIN DE PARMETROS CONSTANTES EN EL CDIGO.

Los parmetros constantes son los parmetros que se muestran en la tabla 1 y stos se
declaran de manera global al inicio del cdigo, despus de la declaracin de libreras. Dentro
de stos parmetros se encuentra tambin la inicializacin de PI [], ya que lo utilizaremos
para la transformacin de los ngulos a grados ya que el software trabajo en radianes.

Figura 3. Declaracin de constantes de acuerdo a los parmetros del manipulador

powerCube.
As mismo se declaran como constantes las matrices de transformacin homogneas
as como sus inversas en las cules se evaluarn los ngulos obtenidos.
Las matrices inversas se utilizarn en el paso 8_1 y 8_2 para la obtencin del ngulo
6 cuando i=1,2.

Figura 4. Declaracin de las matrices inversas para el paso 8.

10

Las matrices directas se utilizaron para aplicara el mtodo de cinemtica directa y as

obtener la matriz 0 6 con los ngulos obtenidos, esto ayudar a comprobar que el
procedimiento realizado por el software es correcto.

Figrua 6. Declaracin de las matrices directas para la obtencin de la matrz

6 .

EJEMPLO.
0
Dada la matrz
6 obtenga los ngulos admisibles para el manipulador
PowerCube. Tomando como constantes los valores dados en la tabla 1.

0
0.5
0 .866
0
6 = [
1
0
0
0

. 866
0.5
0
0

619.5134
357.6762
]
16.1384
1

Figura 7. Cargo de datos a la matriz. Iniciando por los datos de la fila 1.

11

Una vez cargados los datos aparece la opcin de verificar los datos ingresados a la
matriz, en este caso oprimimos si:

Figura 8. Opcin para verificacin de datos ingresados a la matriz.

Al oprimir enter la matriz ser mostrada, y con otro pulso el programa mostrar los
resultados obtenidos.

Figura 9. Matriz ingresada.

12

Los resultados se presentarn de manera ordenada, primero se muestran los ngulos

admisibles cuando i=1, si se llegara a presentar que un ngulo se sale del lmite establecido
(observar algoritmo) entonces stos no se obtienen a partir del punto donde se hall el
incumplimiento de parmetros y en la ventana del programa solo se mostrar una leyenda
recalcando el ngulo que se sali del rango.
Para este ejercicio se obtuvieron las dos respuestas:

Figura 10. Resultados de ngulos y matrices homogneas para i=1.

13

Figura 11. Resultado de ngulos y matrices homogneas cuando i=2.

14

Tabla 2. ngulos admisibles obtenidos con el software.

i=1

i=2

30

30

-25 = 25

30

90

-90

-65

60

15