UNIVERSIDAD TECNOLÓGICA DE LA MIXTECA
INSTITUTO DE ELECTRÓNICA Y MECATRÓNICA
INGENIERÍA EN MECATRÓNICA
ROBÓTICA I.
EXAMEN FINAL
Generador de ángulos para el Robot PowerCube de 6 grados
de libertad.
Manual de usuario para el Software
PROFESOR:
Dr. Vitaliy Ribak.
PRESENTA:
Gómez Merlín Emanuel
Huajuapan de León, Oaxaca, México, 23 de Septiembre del 2015.
2
ÍNDICE.
Introducción . . . . . . . 3
Objetivo . . . . . . . 3
Desarrollo del algoritmo para la obtención de los ángulos . . . .3
Procedimiento de ejecución del Software . . . . . 8
Ingreso de datos en el Software . . . . . . 8
Ubicación de parámetros constantes en el código . . . . 9
Ejemplo . . . . . . . . . 10
3
INTRODUCCIÓN
En este manual de usuario se presenta la aplicación de los conocimientos de Cinemática
inversa, la construcción del esquema cinemático y construcción de trayectorias aplicados al
robot PowerCube de 6 grados de libertad con el fin de aplicar una metodología que nos
permita generar trayectorias por medio de la solución del problema de cinemática inversa.
OBJETIVO
Aplicar los conocimientos vistos en la materia ROBOTICA I que se adquirieron a lo largo
del curso de manera práctica a fin de comprender de mejor manera las técnicas y conceptos
que se aprendieron y aplicarlos directamente sobre el robot Powercube del laboratorio.
DESARROLLO DEL ALGORITMO PARA LA OBTENCIÓN DE LOS ÁNGULOS.
Antes de desarrollar el algoritmo pertinente, es necesario fijar los límites a los que se
restringirá el movimiento del brazo robótico, es decir, las dimensiones máximas y mínimas
en donde se efectuará el movimiento para el trazado de las letras, así como los sistemas de
coordenadas fijo y los sistemas de coordenadas de la mesa. Dichos parámetros se muestran
a continuación:
[ ] (1)
(2)
(3)
Dónde:
es la altura de la mesa y es la altura del lápiz.
Figura 1. Área de trabajo sobre el plano de la hoja con respecto al sistema de
coordenadas fijo.
4
Es importante mencionar que los 720mm define la distancia que existe entre el origen de
coordenadas fijo (origen del manipulador) hasta el origen de coordenadas de la mesa. De tal
modo que cada cuadrante de la mesa tiene las siguientes dimensiones 85mm sobre el eje X
por 221mm sobre el eje Y del manipulador.
Una vez que se tiene claro los sistemas de coordenadas con los que se estará trabajando se
procede a diseñar la figura que se desea trazar con el manipulador, en este caso se
generarán letras del alfabeto.
Para la realización de este trabajo se consideró un área de 40mm de largo x 40mm de alto
para delimitar el tamaño de la letra W y M, mientras que se tomó un área de 30mm de largo
x 40mm de alto para todas las demás letras tomando como origen la esquina inferior
izquierda. A continuación se muestran los puntos para generar la letra “M”.
Figura 2. Diseño de la letra “M” con 17 puntos.
5
Punto
[mm]
[mm]
Punto
[mm]
[mm]
1 0 0 10 25 10
2 0 10 11 30 20
3 0 20 12 35 30
4 0 30 13 40 40
5 0 40 14 40 30
6 5 30 15 40 20
7 10 20 16 40 10
8 15 10 17 40 0
9 20 0
Tabla 1.1 Coordenadas de cada uno de los puntos que conforman la letra “M”
respecto al sistema de coordenadas de la letra.
Punto
[mm]
[mm]
Punto
[mm]
[mm]
1 720 20
10 745 10
2 720 10
11 750 0
3 720 0
12 755 -10
4 720 -10
13 760 -20
5 720 -20
14 760 -10
6 725 -10 15 760 0
7 730 0 16 760 10
8 735 10 17 760 20
9 740 20
Tabla 1.1 Coordenadas de cada uno de los puntos que conforman la letra “M”
respecto al sistema de coordenadas de fijo.
6
De este modo es posible capturar infinidad de puntos dentro del plano cartesiano a fin de
generar un trazo más suave y uniforme de la figura deseada. Una vez que se tiene la
sucesión de puntos que debe visitar el efector final del manipulador se procede a obtener las
matrices con ayuda de las ecuaciones que se mostrarán a continuación.
Por cada coordenada en el plano (x,y) se generará una matriz de transformación homogénea
, de tal modo que si una letra como la “S” posee 32 puntos a lo largo de su trayectoria,
entonces se necesitara calcular 32 matrices de transformación homogénea, debido a que
para cada punto en el plano se necesitará que cada una de las articulaciones del robot
cambien de posición y esto generará nuevos ángulos.
A continuación se muestran las fórmulas para hallar las matrices de transformación
homogénea de cada uno.
√
(4)
√
(5)
(6)
Donde:
(7)
(8)
Donde es la distancia del origen de la hoja hacia el punto lateral donde se quiera
posicionar la letra.
(9)
Estos puntos obtenidos son las coordenadas del efector final con respecto al eje de
coordenadas fijo (cuarta columna de la matriz ). Ahora, a partir del ángulo se
podrán obtener los puntos X, Y y Z que estarán con respecto al eje de coordenadas rotado.
Los vectores de coordenadas
representados con respecto al sistema de
coordenadas básico se muestran a continuación.
[ ] [
] [ ] (10)
A continuación se muestra la matriz de rotación con respecto al eje ZOAR:
7
[
] (11)
La ecuación de rotación indica el giro con grados respecto al eje ZOAR.
Posteriormente se realizan las siguientes operaciones trigonométricas para hallar la
orientación con respecto al sistema de coordenadas fijo.
[
] [
]=[
] (12)
[
] [
] [
] (13)
[
] [
] [
] (14)
La matriz de transformación será de la siguiente manera:
[
] (15)
[
] (16)
Al obtener las matrices de cada punto los cuales generan la una trayectoria, en este
caso la letra, es posible obtener los ángulos ( ) que deberá tomar el sistema para
posicionarse en el punto indicado, esto mediante la aplicación de la cinemática inversa. A
fin de reducir tiempos en la inserción de datos al programa se desarrolló un programa para
la obtención de las matrices a partir de los puntos indicados en el diseño de cada una de las
letras y posteriormente, haciendo uso del programa que resuelve el problema de cinemática
inversa se obtienen los ángulos para cada grado de libertad del robot manipulador
PowerCube.
Una vez que encuentran los ángulos para cada punto se procede a ejecutar el código del
ANEXO A, en el cual se recibe la secuencia de ángulos para el movimiento del efector
final.
8
PROCEDIMIENTO DE EJECUCIÓN DEL SOFTWARE
El programa que se encarga de generar la matriz de transformación homogénea para cada
coordenada (x,y) fue creado en el lenguaje C dentro del entorno de programación de Dev-
Cpp 5.9.2. Para ejecutar el programa es necesario abrir el código con el compilador de Dev-
Cpp el cual lleva por nombre “MatrizOrientacion.cpp” que se encuentra en este mismo y
ahí mismo hacer unos ligeros cambios (véase “INGERESO DE DATOS EN EL SOFTWARE”) y presionar
la tecla F11.
De igual modo para generar el arreglo de ángulos de cada coordenada (x,y) se debe abrir el
código que lleva por nombre “cinematica inversa_FINAL.cpp” en el cual se deberá ingresar
en una parte del código la matriz de transformación homogénea a la que se desea hallar sus
ángulos.
INGRESO DE DATOS EN EL SOFTWARE
Primeramente se debe calcular la matriz de transformación homogénea para cada
coordenada (x,y) que conforma una letra esto se hace con ayuda del programa “Matriz
Orientacion.cpp”. En el cual se debe de modificar la línea de código #26, la cual contiene el
nombre del archivo que deberá leer el programa para ir extrayendo cada coordenada (x,y)
sin necesidad de ingresarlas manualmente.
Figura3.-Muestra la línea de código que deberá ser modificada
El archivo que se usó en este trabajo era un formato con extensión “.txt” y se bede guardar
dentro de la carpeta donde se encuentra el programa “MatrizOrientacion.cpp”. Ahí mismo
el programa arrojará un nuevo archivo con extensión “.txt” con el nombre “Genera0T6.txt”
(este nombre se lo puedes cambiar si deseas), este nuevo archivo contiene las matrices de
transformación homogénea que se usarán en el siguiente programa.
Una vez que se tiene el conjunto de matrices de transformación homogéneas 0T6 para cada
coordenada que conforma una letra, entonces se procede a ejecutar el segundo código,
llamado “cinematica inversa_FINAL.cpp”. Para realizar exitosamente la ejecución de dicho
programa se debe copiar los valores de cada matriz contenidas en el archivo
“Genera0T6.txt”.
9
Figura 4 Línea de código donde se ingresan los datos de la matriz .
Cabe mencionar que este procedimiento es tedioso, debido a que en el segundo programa
no se implementó de manera correcta la utilización de archivos para evitar estar ingresando
manualmente cada matriz 0T6 a causa de errores de programación que no se pudieron
corregir a tiempo. Este programa generará una o dos soluciones, dependiendo del tipo, de
tal modo que el usuario deberá elegir el arreglo de ángulos que permitan tener el brazo del
robot en una posición tal que no interfiera con el movimiento fluido del mismo sobre la
mesa.
UBICACIÓN DE PARÁMETROS CONSTANTES EN EL CÓDIGO
Después de agregar las librerías usadas en el código se establecieron los valores constantes
de los parámetros que tiene el PowerCube. A continuación se muestra la parte del código en
donde se consideran dichos valores.
Figura 5 Ubicación de los parámetros constantes en el código
10
Como se aprecia en la Figura4 la constante “dirZ” delimitará en todo momento la posición
del lápiz sobre el eje z contante (110 es la altura en z para que el lápiz toque el papel sobre
la mesa), “dirY” delimita que tan a la izquierda o derecha del centro de coordenadas del eje
rotado se empezará a escribir y la constante “pb” delimita el renglón donde se escribirá, es
decir, más arriba o más abajo dentro de los límites establecidos para la mesa.
EJEMPLO
Dada las coordenadas de un punto sobre un plano (x,y) para 0≤ x≤30mm y 0≤y≤40mm
generar los ángulos para cada uno de los 6 grados de libertad del robot PowerCube tales
que generen una trayectoria a fin de dibujar la letra “M”.
Figura 6 lista de coordenadas para generar la letra “M”.
RESPUESTA:
Se guardan estas coordenadas en un archivo, en este caso se llamará, “M.txt” y se guarda
dentro de la carpeta donde se ubica este mismo programa, posteriormente se modifica el
código del programa que lleva por nombre “MatrizOrientacion.cpp” para poder ingresar los
datos del archivo “M.txt” (véase el apartado “INGRESO DE DATOS EN EL SOFTWARE”)
11
este programa generará las matrices de cada punto, se almacenará en un archivo
llamado “Genera0T6.txt”dentro de la misma carpeta donde se encuentra este programa.
Figura 7 Modificación del primer programa para el ejemplo con la letra “M”.
Nota: El archivo“Genera0T6.txt” reescribirá su contenido en el caso de que este no se
cambie de nombre y se vulva a generar una nueva matriz para otra letra.
Figura 8 Pantalla emergente una vez que se generó las matrices 0T6.
12
Figura 9 Archivo “Genera0T6.txt” que contiene las matrices 0T6.
En la Figura 8 se aprecia que cada cuatro renglones representa una matriz de
transformación homogénea 0T6 para una coordenada que conforma la letra “M”.
Posteriormente se abre el código del segundo programa que lleva por nombre “cinematica
inversa_FINAL.cpp” y se ingresan los datos de la matriz a la cual se desea calcular su
13
arreglo de ángulos. Este programa arrojará en pantalla el arreglo de ángulos para la matriz
0T6 ingresada, de donde el usuario tomara la mejor de las dos soluciones y manualmente se
copiará dicho arreglo en otro archivo como se muestra a continuación.
Figura 10 arreglo de ángulos para cada grado de libertad del robot.
Este archivo es él se recibirá que recibirá el algoritmo para el movimiento asíncrono (véase
ANEXO A) el cual será el encargado de interpretar los ángulos de cada grado de libertad y
activar los motores para que dichas articulaciones comiencen a moverse y formen la letra
“M” deseada sobre el plano de la mesa.
14
ANEXO A
Angulos es una matriz que contiene la secuencia de ángulos que reciben las tetas para el
movimiento
Num_Ang indica el número de los puntos a visitar
Num_Theta indica el número de tetas del manipulador (número de grados de libertad)
DeltaT indica el tiempo delta ha usarse para los cálculos
Vel indica la velocidad usada por el manipulador
Acc la aceleración usada por el manipulador
Procedimiento (Angulos, Num_Ang, Num_Theta, DeltaT, Vel, Acc,)
Variables:
Entero i<-0, j<-0;
Real Tiempo1<-Vel/Acc;
Real Tiempo2;
Matriz Real Tiempos;
Matriz Real Deltas;
Arreglo Entero Pos_Matriz;
Arreglo Entero Tiempo_Act;
Lógico Bandera;
Para i<-0 Hasta Num_Thetha-1 Hacer
Deltas0 , i<-Angulos0 , i;
Fin Para
Para i<-1 Hasta Num_Ang-1 Hacer
Para j<-0 Hasta Num_Theta-1 Hacer
Deltasi-1 , j<-Deltasi-1 , j - Angulosi , j;
Fin Para
Fin Para
Para i<-0 Hasta Num_Ang-2 Hacer
Para j<-0 Hasta Num_Theta-1 Hacer
Tiempo2<-( Deltasi , j - Acc * Tiempo1 * Tiempo1 ) / Vel;
Tiemposi , j<-Tiempo2 + 2 * Tiempo1 – DeltaT;
Fin Para
Fin Para
Para i<-0 Hasta Num_Thetha-1 Hacer
Pos_Matrizi<-0;
Fin Para
Para i<-0 Hasta Num_Thetha-1 Hacer
15
Mover_Thetai<- Deltas0 , i;
Tiempo_Acti<- Tiempo actual;
Fin Para
Repetir
Bandera<- Falso;
Para i<-0 Hasta Num_Theta-1 Hacer
Si Tiempo actual – Tiempo_Acti TiemposPos_Matrizi , i
Si Pos_Matrizi < Num_Ang-2
Mover_Thetai<- DeltasPos_Matrizi+1 , i;
Pos_Matrizi<- Pos_Matrizi + 1;
Tiempo_Acti<- Tiempo actual;
Bandera<- Verdadero;
Fin Si
Fin Si
Fin Para
Hasta Que Bandera = Falso;
Fin Procedimiento