INSTITUTO TECNOLÓGICO Y DE ESTUDIOS SUPERIORES DE MONTERREY CAMPUS MONTERREY PROGRAMA DE GRADUADOS EN MECATRÓNICA Y TECNOLOGÍAS DE LA INFORMACIÓN

REDISEÑO MECATRÓNICO Y CONTROL DIFUSO DE UN ROBOT TIPO DELTA

TESIS PRESENTADA COMO REQUISITO PARCIAL PARA OBTENER EL GRADO ACADÉMICO DE: MAESTRO EN CIENCIAS CON ESPECIALIDAD EN AUTOMATIZACIÓN POR: GABRIEL ANTONIO ROSALES MARTÍNEZ

MONTERREY , N.L.

MAYO 2010

INSTITUTO TECNOLÓGICO DE ESTUDIOS SUPERIORES DE MONTERREY DIVISIÓN DE MECATRÓNICA Y TECNOLOGÍAS INFORMACIÓN PROGRAMA DE GRA DUADOS EN MECATRÓNICA Y TECNOLOGÍAS DE INFORMACIÓN Los miembros del comité de tesis recomendamos que la presente tesis del Ing. Gabriel Antonio Rosales Martínez sea aceptada como requisito parcial para obtener el grado académico de: Maestro en Ciencias con Especialidad en Automatización Comité de tesis: ______________________________ Dr. Antonio Favela Contreras Asesor ______________________________ Dr. José de Jesús Rodríguez Ortiz Sinodal ______________________________ M.C. Manuel G. Cabrera López Sinodal _________________________________________ Dr. Gerardo Antonio Castañón Ávila Director de las Maestrías de Electrónica y Automatización de DMTI

Mayo 2010

DISEÑO MECATRONICO Y CONTROL DIFUSO DE UN ROBOT TIPO DELTA

POR: GABRIEL ANTONIO ROSALES MARTÍNEZ

TESIS

PRESENTADA COMO REQUISITO PARCIAL PARA OBTENER EL GRADO ACADÉMICO DE: MAESTRO EN CIENCIAS CON ESPECIALIDAD EN AUTOMATIZACIÓN

INSTITUTO TECNOLÓGICO Y DE ESTUDIOS SUPERIORES DE MONTERREY

Mayo 2010

DEDICATORIA A mis padres por haberme apoyado incondicionalmente en mis decisiones. A mis hermanos, Gualberto y Griselda quienes siempre estuvieron al pendiente de mí a pesar de la distancia que nos separa.

VII

AGRADECIMIENTOS A mi asesor, el Dr. Antonio Favela quien me apoyó en todo momento para sacar adelante mi trabajo de tesis. Al Dr. José de Jesús Rodríguez y al M.C. Manuel Cabrera por su interés y apoyo como sinodales en la presentación de este proyecto. A mi amiga y compañera de carrera y maestría Carolina Atilano por su valiosa amistad a lo largo de mi estancia en Monterrey. A mis compañeros de maestría: Eduardo Rivera, Jesús Sánchez, Jorge Claros, Ernesto Pérez, Luis Sandoval, Andrés Rosado, Enrique Jiménez, José Luis Turrubiates, Aldo Cedillo, Claudia Escalera, Raymundo Magaña por su amistad incondicional, en especial a Angelo Raimondi quien estuvo a lo largo de mi estancia en la

maestría dándome

sugerencias para mejorar día a día. A Rodrigo Vargas por su esfuerzo y dedicación dirigido al desarrollo del prototipo.

IX

Resumen En este trabajo de tesis presentado como requisito parcial para la obtención del grado de Maestro en Ciencias con Especialidad en Automatización, se presenta el desarrollo e implementación de un control difuso aplicado en un Robot Manipulador Delta. El control difuso está basado en reglas lingüísticas las cuales pretenden emular el conocimiento humano en un proceso. Debido a lo anterior, este esquema es ampliamente utilizado tanto en procesos lineales como no lineales. El Robot manipulador tipo Delta pertenece a los robots paralelos o de cadena cinemática cerrada, los cuales tienen la característica de alcanzar altas aceleraciones, sin embargo poseen una modelación cinemática complicada la cual se debe analizar para llevar a cabo cualquier esquema de control. Debido a esta desventaja se diseñó, construyó e implementó un novedoso sistema de sensado el cual permite calcular la posición del efector final sin la necesidad de realizar la modelación cinemática directa. Se realizó el rediseño mecatrónico de un prototipo de Robot Delta propiedad del Departamento de Mecatrónica y Automatización del Tecnológico de Monterrey con el fin de aplicar un controlador difuso y el sistema de sensado descrito anteriormente.

XI

Índice general Resumen...................................................................................................................... XI 1.

2.

Introducción .............................................................................................................. 1 1.1

Definición del problema ................................................................................... 2

1.2

Objetivos de la tesis .......................................................................................... 3

1.3

Metodología ...................................................................................................... 3

1.4

Alcances ............................................................................................................ 4

1.5

Estructura .......................................................................................................... 4

Marco Teórico........................................................................................................... 5 2.1.

Introducción ...................................................................................................... 6

2.2.

Robots Paralelos ............................................................................................... 7

2.3.

Robot Manipulador Delta ............................................................................... 10

2.3.1. 2.4.

3.

Aplicaciones y Patentes .......................................................................... 11

Cinemática ...................................................................................................... 14

2.4.1.

Trilateración ............................................................................................ 15

2.4.2.

Trilateración como principio de funcionamiento de un GPS ................. 15

2.4.3.

El método de trilateración aplicado a robótica ....................................... 18

2.4.4.

Cinemática inversa .................................................................................. 22

2.5.

Espacio de Trabajo.......................................................................................... 24

2.6.

Resumen.......................................................................................................... 24

Desarrollo de un Manipulador Delta ...................................................................... 25 3.1.

Introducción .................................................................................................... 26

3.2.

Diseño Mecánico ............................................................................................ 27

3.2.1.

Diseño original del prototipo .................................................................. 28

3.2.2.

Rediseño final del prototipo .................................................................... 30

3.3.

Diseño Electrónico .......................................................................................... 33

3.4.

Comunicación ................................................................................................. 35

3.4.1.

Interfaz Humano-Máquina ...................................................................... 35 XIII

3.4.2. 3.5. 4.

4.1.

Introducción .................................................................................................... 42

4.2.

Control Difuso ................................................................................................. 42

4.2.1.

Fuzificación ............................................................................................. 43

4.2.2.

Mecanismo de inferencia ........................................................................ 45

4.2.3.

Defuzificación ......................................................................................... 46

4.2.4.

Método de Centroide ............................................................................... 46

Implementación y sintonización del control difuso ........................................ 48

4.3.1.

Fuzificación ............................................................................................. 48

4.3.2.

Inferencia ................................................................................................. 49

4.3.3.

Defuzificación ......................................................................................... 50

4.4.

Resumen .......................................................................................................... 51

Pruebas y Análisis de Resultados ............................................................................ 53 5.1.

Introducción .................................................................................................... 54

5.2.

Prueba I ........................................................................................................... 54

5.2.1.

Prueba I.A................................................................................................ 54

5.2.2.

Prueba I.B ................................................................................................ 58

5.3.

Prueba II .......................................................................................................... 61

5.3.1.

Prueba II.A .............................................................................................. 61

5.3.2.

Prueba II.B .............................................................................................. 64

5.4.

Prueba III ......................................................................................................... 68

5.4.1.

Prueba III.A ............................................................................................. 68

5.4.2.

Prueba III.B ............................................................................................. 71

5.5. 6.

Resumen .......................................................................................................... 39

Diseño e Implementación de un controlador Difuso en un Manipulador Delta...... 41

4.3.

5.

Microcontrolador ..................................................................................... 38

Análisis de Resultados .................................................................................... 74

Conclusiones y Trabajos Futuros ........................................................................... 75 6.1.

Conclusiones ................................................................................................... 76

6.2.

Trabajos futuros............................................................................................... 77

Bibliografía...................................................................................................................... 79 Apéndice A ...................................................................................................................... 81 Apéndice B ...................................................................................................................... 88

XV

Lista de figuras Figura 2-1 – Estructura cinemática de un robot a) serial b) paralela ................... 8 Figura 2-2 – Robot Delta, U.S. Patent number 4,976,582 [14] .......................... 11 Figura 2-3 – Modelo de Robot delta IRB 340 de ABB para operaciones de “pick-and-place” ................................................................................................. 12 Figura 2-4 – Robots Hitachi Seiki con cnfiguración delta para apliaciones de a) “pick-and-place” y .............................................................................................. 12 Figura 2-5 – U.S. Pantent No. US2006/0182602 a) Robot Delta b) Vista de la articulación c) Tracción de reacción libre ........................................................... 13 Figura 2-6 – U.S. Patent No. 6,516,681 .............................................................. 14 Figura 2-7 – Esfera con radio r1 formada por un satélite .................................... 16 Figura 2-8 – Intersección de dos esferas ............................................................. 17 Figura 2-9 – Intersección de tres esferas ............................................................ 17 Figura 2-10 – Sensores montados en la base fija del robot delta ........................ 18 Figura 2-111 – Sistema coordenado formado por la geometría de los sensores. 19 Figura 2-12 – Número de articulación en el Robot Delta ................................... 20 Figura 2-13 - Geometría para el metodo de trilateración.................................... 20 Figura 2-14 - Descripción de la geometría del brazo i ...................................... 22 Figura 2-15 – Espacio de trabajo para el prototipo de Robot Delta ................... 24 Figura 3-1 – Prototipo desarrollado en el Departamento de Mecatrónica y Automatización del Instituto Tecnológico y de Estudios Superiores de Monterrey campus Monterrey ............................................................................ 26 Figura 3-2 – Estructura Inicial del prototipo ...................................................... 28 Figura 3-3 - Vista de a) mecanismo de tracción y b) Union entre antebrazo y brazo.................................................................................................................... 29 Figura 3-4 – Base movil del manipulador .......................................................... 30 Figura 3-5 – Ajustes mecánicos implementados en el prototipo ........................ 31 Figura 3-6 – Vista de los sensores de desplazamiento instalados en la base fija del manipulador .................................................................................................. 32 Figura 3-7 – Estructura final del prototipo ......................................................... 32 Figura 3-8 - Esquemático de la conexión electrónica del microcontrolador ...... 33

XVII

Figura 3-9 – Esquemático electrónico para el protocolo de comunicación RS232 ............................................................................................................................. 34 Figura 3-10 – Esquematico del encapsulado GENCODRIVER 6320 ................ 35 Figura 3-11 – IHM diseñada para el control del manipulador ............................ 36 Figura 3-12 – Detalle de la IHM ......................................................................... 37 Figura 3-13 – Pseudocódigo implementado en LabView® ................................ 38 Figura 3-14 – Esquemático general de la electrónica y comunicación del manipulador Delta ............................................................................................... 39 Figura 4-1 – Diagrama de bloques de un esquema de control difuso ................. 42 Figura 4-2 – Estructura del Controlador Difuso ................................................. 43 Figura 4-3 - Defuzificación por centroide .......................................................... 47 Figura 4-4 – Términos lingüisticos activos a) de entrada y b) de salida para una entrada igual a diez ............................................................................................. 47 Figura 4-5v– Área formada con los términos lingüisticos activos de salida ...... 48 Figura 4-6 - Lazo de control implementado en el prototipo Delta ..................... 48 Figura 4-7 – Conjuntos difusos para el error en la posición angular .................. 49 Figura 4-8 - Conjuntos difusos de salida ............................................................ 50 Figura 5-1 -Respuesta de las articulaciones del robot para la prueba I.A de (xo, yo, zo) = (0, 0, -500) a (xf, yf, zf) = (0, 0, -300) con respecto al punto P sin carga. ................................................................................................................... 55 Figura 5-2 - Respuesta espacial del robot para la prueba I.A de (xo, yo, zo) = (0, 0, -500) a (xf, yf, zf) = (0, 0, -300) con respecto al punto P sin carga. .............. 57 Figura 5-3 - Respuesta de las articulaciones del robot para la prueba I.B de (xo, yo, zo) = (0, 0, -500) a (xf, yf, zf) = (0, 0, -300) con respecto al punto P sin carga. ................................................................................................................... 59 Figura 5-4 - Respuesta espacial del robot para la prueba I.B con cambio de posición de (xo, yo, zo) = (0, 0, -500) a (xf, yf, zf) = (0, 0, -300) con respecto al punto P con carga de 0.20 kilogramos. .............................................................. 60 Figura 5-5 - Respuesta de las articulaciones del robot para la prueba II.A con cambio de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P sin carga. ................................................................................................ 62 Figura 5-6 - Respuesta espacial del robot para la prueba II.A con para cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P sin carga. ................................................................................................ 64 Figura 5-7 - Respuesta articular para la prueba II.B con cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P con carga de 0.20 kilogramos .................................................................................... 65

Figura 5-8 - Respuesta espacial del robot para la prueba II.B con para cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P con carga de 0.20 kilogramos. ............................................................... 67 Figura 5-9 - Respuesta articular para la prueba III.A con cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P sin carga .............................................................................................................. 69 Figura 5-10 - Respuesta espacial del robot para la prueba III.A con cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P sin carga. ............................................................................................... 70 Figura 5-11 - Respuesta articular para la prueba III.B con cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (-100, 0, -450) con respecto al punto P con carga de 0.20 kilogramos. ............................................................................ 72 Figura 5-12 - Respuesta espacial del robot para la prueba III.B con cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (-100, 0, -450) con carga ... 73

XIX

Lista de tablas Tabla 2-1 – Ventajas y desventajas de los robots paralelos con respecto a robots seriales ................................................................................................................ 10 Tabla 4-1 – Funciones de membresía más comunes para la fuzificación ........... 44 Tabla 4-2 - Tabla de implicaciones difusas para dos variables y una salida con cinco conjuntos difusos. PG: positivo grande; PP: positivo pequeño; C: cero; NP negativo pequeño; NG: negativo grande. ........................................................... 46 Tabla 4-3 - Tabla de implicaciones difusas para el controlador ......................... 50 Tabla 5-1 - Índices de desempeño para la prueba I.A con cambio de posición de (xo, yo, zo) = (0, 0, -500) a (xf, yf, zf) = (0, 0, -300) con respecto al punto P sin carga. ................................................................................................................... 56 Tabla 5-3 - Índices de desempeño para la prueba I.B con cambio de posición de (xo, yo, zo) = (0, 0, -500) a (xf, yf, zf) = (0, 0, -300) con respecto al punto P con carga de 0.20 kilogramos. ................................................................................... 59 Tabla 5-4 - Índices de desempeño para la prueba I.B con cambio de posición de (xo, yo, zo) = (0, 0, -500) a (xf, yf, zf) = (0, 0, -300) con respecto al punto P con carga de 0.20 kilogramos. ................................................................................... 60 Tabla 5-5 - Índices de desempeño para la prueba II.B con cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P sin carga. ................................................................................................................... 63 Tabla 5-6 – Índices de desempeño para cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P sin carga. ................ 64 Tabla 5-7 - Índices de desempeño para cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P con carga de 0.20 kilogramos .......................................................................................................... 66 Tabla 5-8 -Índices de desempeño para cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P con carga de 0.20 kilogramos. ......................................................................................................... 67 Tabla 5-9 - Índices de desempeño para cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P sin carga ........... 69 Tabla 5-10 - Índices de desempeño para cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P sin carga. ............. 70 Tabla 5-11 - Índices de desempeño para cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (-100, 0, -450) con carga de 0.20 kilogramos. ................ 72 Tabla 5-12 - Índices de desempeño para cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (-100, 0, -450) con carga. ................................................ 73

XXI

1.

Introducción

Actualmente el uso de robots en la industria se ha hecho indispensable para la producción diaria de muchas de las empresas manufactureras en todo el mundo. Debido a esta creciente necesidad se ha visto un gran avance científico, teórico y práctico en el área de la robótica. Dos grandes familias de robots son fácilmente identificables: robots seriales (de cadena cinemática abierta) y robots paralelos (de cadena cinemática cerrada). Estos últimos no tienen extremos libres y cada uno de los extremos de sus brazos forma una cadena cerrada. A diferencia de los robots paralelos, los robots seriales suelen tener un extremo libre para manipulación de objetos mientras el otro extremo está unido a la base del robot [9]. Debido a que su configuración reduce significativamente las masas en movimiento, por ende son capaces de alcanzar grandes aceleraciones, los robots de cadena cinemática cerrada han sido objeto de intensos estudios científicos recientes [20]. El Robot Manipulador Delta pertenece a la familia de robots paralelos y es uno de los robots más simples de esta categoría ya que, mecánicamente hablando, es capaz de alcanzar una alta aceleración al coste de tener un espacio de trabajo más limitado y difícil de 1

Capítulo 1: Introducción modelar [3]. Para este tipo de manipulador se han desarrollado esquemas de convencionales, incluso aplicando algunas estrategias de control avanzado dentro del control convencional, sin embargo, expertos concuerdan que es necesario implementar técnicas de control que excluyan el control clásico, es decir, esquemas predictivo-adaptivo, difuso, sistemas expertos o deslizante. El control difuso es una metodología que permite expresar las reglas de operación de un sistema en términos lingüísticos en lugar de ecuaciones matemáticas. Es un esquema tal que permite emular la experiencia de operadores y expertos en el proceso, lo cual permite tener un buen desempeño en zonas altamente no lineales. En [3] y [11] se presenta una modelación cinemática y dinámica para este tipo de robot manipulador y se propone una estrategia de control supervisorio en el que los parámetros de un control clásico PID son modificados por una ganancia a partir del error y el cambio de éste. En este trabajo de tesis se retoma el uso de este manipulador, ya que se cuenta con un prototipo físico desarrollado en el departamento de Mecatrónica y Automatización del Instituto Tecnológico y de Estudios Superiores de Monterrey campus Monterrey. En él se implementan los modelos cinemáticos inversos y se propone una novedosa forma de calcular la posición del efector final en el espacio; además se diseña e implementa un esquema de control difuso para controlar la posición angular de cada articulación y por ende, su posición espacial.

1.1 Definición del problema Se identifica un área de oportunidad en el control de posición en un manipulador paralelo tipo Delta, ya que se han desarrollado técnicas convencionales así también como una mezcla entre control clásico y control avanzado, sin embargo una técnica enteramente difusa no se ha visto aplicado en este tipo de manipuladores. Se elige este tipo de manipulador ya que proporciona las ventajas de la familia de los manipuladores de estructura cinemática cerrada siendo la plataforma más sencilla de implementar. 2

1.2 Objetivos de la tesis El objetivo de la tesis es desarrollar una estrategia de control avanzado basado en el esquema de control difuso para el control de la posición de un robot manipulador tipo delta con actuadores rotacionales. Con este objetivo se pretende obtener una buena regulación de las variables de operación que intervienen en el proceso. Se entiende como buena regulación lograr obtener el conjunto de valores de manipulación para que la posición del robot alcance la referencia deseada en un mínimo de tiempo sin que llegue a presentarse el sobretiro en su desempeño. Con el fin de lograr el objetivo general de la tesis se proponen los siguientes objetivos específicos. -

Proponer modificaciones físicas para el prototipo con el fin de mejorar su diseño mecánico, electrónico, sensorial y de comunicación.

-

Obtener un modelo matemático que sustituya al análisis cinemático directo.

-

Diseñar e implementar un esquema de control Supervisorio difuso a través de un software especializado.

1.3 Metodología Para alcanzar los objetivos mencionados anteriormente se siguió la siguiente metodología: 

Analizar y validar los modelos matemáticos que gobiernan al manipulador.



Determinar los parámetros geométricos y físicos para el cálculo de dichas ecuaciones.



Elaborar e implementar un sistema de sensado el cual facilite la modelación cinemática.



Desarrollar los modelos cinemáticos en el software de programación gráfica de National Instruments LabView®.

3

Capítulo 1: Introducción 

Diseñar y sintonizar el control difuso según la dinámica de la planta.



Agregar el algoritmo de control a la plataforma de programación gráfica LabView®.



Desarrollar e implementar una interfaz electrónica para la comunicación entre los actuadores y el software LabView®.



Realizar pruebas y analizar resultados.



Elaborar conclusiones en base a lo obtenido en el punto anterior.

1.4 Alcances El alcance de la presente tesis abarcará la modelación del manipulador Delta y el diseño e implementación de un control difuso sobre este. Así también como la implementación de un sistema de sensado para determinar la posición del efector final en el espacio.

1.5 Estructura La estructura general de este documento es como sigue: En el capítulo dos se describe el estado del arte de los manipuladores paralelos tipo Delta. También se describe y analiza la modelación cinemática del mismo. En el capítulo tres se describe el prototipo del manipulador Delta donde se ha implementado la estrategia de control. En el capítulo cuatro se muestra la etapa de diseño e implementación del control difuso para el manipulador Delta. Los resultados y análisis de las pruebas realizadas se presentan en el capítulo cinco. En el capítulo seis se finaliza con las conclusiones y trabajos futuros.

4

2.

Marco Teórico

En este capítulo se introducen conceptos utilizados en la robótica paralela. En la primera parte se definen y clasifican los robots paralelos. Sucesivamente se describen aplicaciones y patentes referentes al manipulador tipo Delta. En la parte central del capítulo se muestra la forma de calcular la posición del efector final mediante el método de trilateración sustituyendo por completo el análisis de la cinemática directa. Finalmente se describe el análisis de la cinemática inversa mediante el método geométrico y se muestra el espacio de trabajo del Robot Delta. 5

Capítulo 2: Marco Teórico

2.1. Introducción Existen ciertas dificultades a la hora de establecer una definición formal de lo que es un robot industrial. La primera de ellas surge de la diferencia conceptual entre el mercado japonés y el euro-americano de lo que es un robot y lo que es un manipulador. Así, mientras que para los japoneses un robot industrial es cualquier dispositivo mecánico dotado de articulaciones móviles destinado a la manipulación, el mercado occidental es más restrictivo, exigiendo una mayor complejidad, sobre todo en lo relativo al control. En segundo lugar, y centrándose ya en el concepto occidental, aunque existe una idea común acerca de lo que es un robot industrial, no es fácil ponerse de acuerdo a la hora de establecer una definición formal [8]. Además, la evolución de la robótica ha ido obligando a diferentes actualizaciones de su definición. La definición más comúnmente aceptada posiblemente sea la de la Asociación de Industrias Robóticas (RIA), según la cual: “Un robot industrial es un manipulador multifuncional reprogramable, capaz de mover materias, piezas, herramientas, o dispositivos especiales, según trayectorias variables, programadas para realizar tareas diversas.”´[18] Esta definición, ligeramente modificada, ha sido adoptada por la Organización Internacional de Estándares (ISO) que define al robot industrial como: “Manipulador multifuncional reprogramable con varios grados de libertad, capaz de manipular materias, piezas, herramientas o dispositivos especiales según trayectorias variables programadas para realizar tareas diversas.”[12] Mientras que la Federación Internacional de Robótica (IFR) distingue entre robot industrial de manipulación y otros robots: “Por robot industrial de manipulación se entiende una máquina de manipulación automática, reprogramable y multifuncional con tres o más ejes que pueden posicionar y orientar materias, piezas, herramientas o dispositivos especiales para la ejecución de trabajos diversos en las diferentes etapas de la producción industrial, ya sea en una posición fija o en movimiento.”[17] 6

En esta definición se debe entender que la reprogramabilidad y la multifunción se consiguen sin modificaciones físicas del robot. Común en todas las definiciones anteriores es la aceptación del robot industrial como un brazo mecánico con capacidad de manipulación y que incorpora un control más o menos complejo. Un sistema robotizado, en cambio, es un concepto más amplio. Engloba todos aquellos dispositivos que realizan tareas de forma automática en sustitución de un ser humano y que pueden incorporar o no a uno o varios robots, siendo esto último lo más frecuente. La AFRI (Asociación Francesa de Robótica Industrial) propone la siguiente clasificación basadas en “tipos” de robots [4]: -

Manipulador con control manual o telemando.

-

Manipulador automático con ciclos pre-ajustados; regulación mediante fines de carrera o topes; control por PLC; accionamiento neumático, eléctrico o hidráulico.

-

Robot programable con trayectoria punto a punto. Carece de conocimiento sobre su entorno.

-

Robot programable con trayectoria continúa “particular” (rectas, espirales, círculos). Carece de conocimiento sobre su entorno.

-

Robot programable con trayectoria continua “general” (múltiples trayectorias programables). Carece de conocimiento sobre su entorno.

2.2. Robots Paralelos Las primeras máquinas que se diseñaron con arquitecturas paralelas fueron para realizar pruebas neumáticas y simuladores de vuelo como la estructura de Stewart [2]. En los últimos años, los robots con estructura paralela empezaron a ser objeto de interés no solo a nivel industrial sino también a nivel científico. 7

Capítulo 2: Marco Teórico En la actualidad existen varios criterios para la clasificación de robots. Como se mencionó anteriormente, un criterio de clasificación puede ser su nivel de “inteligencia”. Otro criterio de clasificación puede ser según su estructura cinemática; y se pueden clasificar en robots seriales, paralelos o híbridos. Un robot es serial cuando su estructura cinemática es una cadena en lazo abierto (Figura 2.1a); es paralelo si tiene una o varias cadenas en lazo cerrado (Figura 2.1b), y; es híbrido cuando contiene tanto cadenas en lazo cerrado como cadenas en lazo abierto.

a)

b)

Figura 2-1 – Estructura cinemática de un robot a) serial b) paralela

La gran mayoría de los robots comerciales utilizados en la industria son de estructura serial, sin embargo, la aplicación de robots paralelos ha ganado terreno debido a las ventajas que estos presentan sobre los seriales. Sin embargo, al comparar robots con estructuras paralelas contra robots con estructuras seriales, la discusión se centra en características como la relación carga-masa del robot, precisión, espacio alcanzable, repetibilidad, entre otras. A continuación se enlistan los principales elementos de comparación: 

Relación carga-masa del robot. En las estructuras seriales el efector final, junto con el objeto a manipular, se encuentran en el extremo de toda cadena. Como consecuencia, cada actuador necesita la suficiente potencia, no solo para cargar el objeto, sino los eslabones y actuadores que se encuentran en medio. Lo que resulta 8

en una baja relación carga-masa del robot. En cambio, en las estructuras paralelas, la carga del objeto a manipular esta soportada directamente por los actuadores; además las articulaciones se pueden colocar cerca de la base fija. Por lo tanto, los eslabones entre la base fija y la base móvil pueden ser más ligeros y la relación carga-masa es mucho más grande, generalmente en un factor de al mendos de 10 [16]. 

Movimiento espacial. Dentro de los robots paralelos, las arquitecturas ocasionan que haya ciertas regiones dentro de su espacio de trabajo que sean imposibles de alcanzar. Estas regiones se deben a la presencia de singularidades dentro de su espacio de trabajo.



Espacio Alcanzable. Esta característica se podría considerar una desventaja que tiene la estructura paralela, ya que el espacio alcanzable es relativamente pequeño si se le compara con el de las estructuras seriales. En estructuras paralelas este se determina con la intersección de los espacios de trabajo de todas las cadenas cinemáticas que conforman dicha estructura.



Precisión y Repetibilidad. Los robots seriales van acumulando el error de una articulación a otra, de igual forma la fricción y la flexibilidad también se comportan de manera serial afectando al error global del robot. En cambio, los robots paralelos no presentan esta situación puesto que el error en vez de acumularse se divide debido a que las articulaciones de sus brazos no dependen de las demás.



Análisis Cinemático. Para los robots paralelos es más sencillo el análisis cinemático inverso que el directo, a diferencia de los robots paralelos donde existe una mayor simplicidad al hacer el análisis cinemático directo.

Por lo que, en general, se puede establecer una tabla comparativa de las ventajas y desventajas de un robot paralelo con respecto a un robot serial tal y como la muestra la Tabla 2.1

9

Capítulo 2: Marco Teórico Ventajas

Desventajas

Arquitectura más rígida

Espacio de trabajo reducido

Mayor precisión y repetibilidad

Cinemática compleja

Altas aceleraciones

Arquitectura mecánica compleja

Mayor relación carga-masa

Configuraciones singulares

Mejores características dinámicas Tabla 2-1 – Ventajas y desventajas de los robots paralelos con respecto a robots seriales

2.3. Robot Manipulador Delta Uno de los robots paralelos más simples es el robot tipo delta por tener tres grados de libertad y una estructura de tres brazos que pueden ser articulados o de movimiento lineal. Este tipo de manipulador fue diseñado y patentado por R. Clavel en 1991, con la idea de tener un robot capaz de alcanzar grandes aceleraciones para la manipulación de objetos. El diseño original (ver Figura 2.2) está constituido por tres cadenas cinemáticas paralelas unidas a una base móvil, cada una estas cadenas son dirigidas por tres actuadores rotacionales que se encuentran acoplados a una base fija del robot. El movimiento de la base móvil se logra por la combinación del movimiento de los brazos y transmitidos a la base móvil por los antebrazos a través de dos rótulas. La combinación de las contribuciones de las tres cadenas cinemáticas y de los antebrazos provoca que la base móvil siempre se mantenga paralela a la base fija. Idealmente su espacio alcanzable se encuentra en la intersección de las tres esferas provocadas por los tres brazos. El Robot Delta, al pertenecer a la familia de estructuras paralelas cómprate la característica de tener mayor precisión y rigidez que los robots seriales. De igual manera, debido a las pocas masas en movimiento, su estructura paralela le permite obtener altas aceleraciones por lo que se utiliza principalmente en aplicaciones donde se requiere manipular objetos con precisión y velocidad.

10

Figura 2-2 – Robot Delta, U.S. Patent number 4,976,582 [14]

2.3.1. Aplicaciones y Patentes El robot Delta fue introducido al mercado por Demaurex, cuyo objetivo era el comercializar el robot para la industria del empaquetamiento; y por ABB bajo el nombre de IRB 340 FlexPicker.

Aplicaciones Industriales Debido a que el robot Delta fue diseñado para aplicaciones de manipulación de objetos el área fuerte de este tipo de robots son las operaciones llamadas “fast pick-and-place”. ABB ha diseñado robots delta para incorporarse a las industrias farmacéuticas, alimenticias y electrónicas. El principal modelo comercial es el IRB 340 felxpicker (Figura 2-3). También existen aplicaciones para las operaciones de maquinado, un ejemplo es el robot desarrollado por el grupo Krause and Mauser. Ellos desarrollaron el Quickstep, una fresadora CNC con tres grados de libertad. Adicionalmente, la compañía Hitahi Seiki manufactura robots Delta de pequeñas dimensiones para trabajo de empaquetamiento y perforado.

11

Capítulo 2: Marco Teórico

Figura 2-3 – Modelo de Robot delta IRB 340 de ABB para operaciones de “pick-and-place”

a)

b)

Figura 2-4 – Robots Hitachi Seiki con cnfiguración delta para apliaciones de a) “pick-and-place” y b) fresadora CNC

12

Patentes Sucesoras al invento registrado por Clavel, diversas patentes han surgido proponiendo alguna mejora al manipulador Delta, con respecto a máquinas herramientas con tecnología de manipuladores paralelos, sobre la modificación de articulaciones o mecanismos, entre otras. Una de las patentes más recientes e innovadoras es la registrada por Persson [16], la cual propone un sistema de tracción de reacción libre (Figura 2-5). Es decir, no presenta reacciones en los engranes de la transmisión permitiendo así una optimización del robot en su rigidez, características de control, requerimiento espacial, velocidad y precisión en el posicionamiento. Otra de las patentes menciona la modificación de la estructura Delta al agregarle un cuarto brazo (Figura 2-6), es decir, que cada brazo este a 90̊ con respecto al brazo más cercano, esta adición permite aumentar el reducido espacio de trabajo del robot Delta así como aumentar la rigidez estructural y posicionar la base móvil con rigidez y precisión.

a)

b)

c)

Figura 2-5 – U.S. Pantent No. US2006/0182602 a) Robot Delta b) Vista de la articulación c) Tracción de reacción libre

13

Capítulo 2: Marco Teórico

Figura 2-6 – U.S. Patent No. 6,516,681

2.4. Cinemática La cinemática del robot estudia el movimiento del mismo con respecto a un sistema de referencia. Se interesa principalmente por la descripción analítica del movimiento espacial del robot como una función del tiempo, y en particular por las relaciones entre la posición y la orientación del efector final con los valores que toman las coordenadas de sus articulaciones. Se conocen dos planteamientos de modelos cinemáticas: modelo cinemático directo y modelo cinemático inverso. El objetivo del análisis de la cinemática directa es el encontrar el punto en el espacio del efector final en función de los ángulos de cada articulación. Mientras que el modelo cinemática inverso consiste en determinar la posición que deben tener las articulaciones para que el efector final llegue a una posición conocida. En los robots paralelos, a diferencia de los robots seriales, el análisis de la cinemática directa llega a ser complejo, no así para la inversa. Es por esta razón que se emplea un método alterno para encontrar el punto deseado respecto a un sistema coordenado de referencia x, y, z denominado trilateración.

14

2.4.1. Trilateración La trilateración es el principio base para la localización en el sistema de posicionamiento global (GPS) ya que aunque la tierra contiene 24 satélites, solamente se necesitan 3 para poder encontrar un objeto y un cuarto para solucionar algún error en el tiempo de localización [13].

Por tal motivo resulta muy importante primeramente explicar el

funcionamiento de la trilateración en un sistema GPS para que sirva como antecedente y una forma de comprender más fácilmente el método antes de explicarlo más a detalle.

2.4.2. Trilateración como principio de funcionamiento de un GPS El sistema GPS está formado por 24 satélites y cinco estaciones terrestres además del receptor del usuario. Estos satélites, a partir de la información incluida en ellos y la que reciben de las estaciones, generan una señal que transmiten a los receptores. Una vez que los receptores reciben esta señal, calculan la posición. Debido a que las señales de radio son transmitidas por un grupo de satélites artificiales cuyas orbitas se conocen con precisión, es posible calcular la distancia del transmisor al receptor. Si medimos las distancias de al menos tres diferentes satélites a un punto sobre la tierra, es posible determinar la posición de dicho punto por trilateración. Para llevar a cabo el proceso de trilateración, el receptor GPS calcula la distancia hasta el satélite midiendo el tiempo que tarda la señal en llegar hasta él. Para ello, el GPS necesita un sistema muy preciso para medir el tiempo. Además, es preciso conocer la posición exacta del satélite. Finalmente, la señal recibida debe corregirse para eliminar los retardos ocasionados. Una vez que el receptor GPS recibe la posición de al menos cuatro satélites y conoce su distancia hasta cada uno de ellos, puede determinar su posición superponiendo las esferas imaginarias que generan. Como se mencionó anteriormente tres satélites podrían parecer suficientes para resolver la posición que se está buscando, ya que el espacio tiene tres dimensiones y una posición en la superficie de la Tierra se puede suponer. Sin embargo, incluso un error de reloj muy pequeño, multiplicado por la velocidad de la luz (la velocidad con que se propagan las 15

Capítulo 2: Marco Teórico señales satelitales), resulta en un error de posición de gran tamaño. Por lo tanto los receptores utilizan cuatro o más satélites para resolver la ubicación del receptor y el tiempo. A pesar de utilizar cuatro satélites para obtener una operación normal, se pueden utilizar menos si se aplican en casos especiales. Si una variable ya es conocida, un receptor puede determinar su posición con sólo tres satélites. (Por ejemplo, un barco o avión ya tiene la variable de elevación conocida). Suponiendo que un receptor en la Tierra capta la señal de un primer satélite determinando la distancia entre ambos. Esto solamente indicaría que el receptor puede ser ubicado en un punto cualquiera dentro de la superficie de una esfera de un radio r1:

Figura 2-7 – Esfera con radio r1 formada por un satélite

Si se mide la distancia de un segundo satélite al mismo receptor se generará una superficie esférica de radio R2, que al intersecarse con la primera esfera se formará un círculo en cuyo perímetro pudiera ser ubicado el punto a medir.

16

Figura 2-8 – Intersección de dos esferas

Si se agrega una tercera medición, la intersección de la nueva esfera con las dos anteriores se reduce a dos puntos sobre el perímetro del círculo descrito.

Figura 2-9 – Intersección de tres esferas

Para automóviles y otros vehículos cercanos a la Tierra, la posición correcta del receptor GPS es la intersección más cercana a la superficie de la Tierra. Para los vehículos de espacio, la intersección más lejana de la Tierra puede ser la correcta. La posición correcta 17

Capítulo 2: Marco Teórico para el receptor GPS es también la intersección cercana a la superficie de la esfera que corresponde al cuarto satélite. Para resolver un problema de trilateración tridimensional es necesario tomar las fórmulas de 3 esferas y proceder a igualarlas. Para poder realizar esto es necesario tener tres limitaciones referentes a los centros de las esferas: -

Todas deben de estar en el plano z=0.

-

Una esfera debe estar colocada en el origen.

-

La tercera esfera debe estar sobre el eje x.

Es posible, sin embargo, trasladar cualquier conjunto de tres puntos para cumplir con estas limitaciones, encontrar el punto solución, y luego invertir la traslación para encontrar el punto solución en el sistema de coordenadas original.

2.4.3. El método de trilateración aplicado a robótica Debido a que el método de trilateración necesita tres puntos conocidos para determinar un punto deseado en el espacio, se diseñaron, construyeron e implementaron tres sensores de desplazamiento los cuales calculan la distancia de cada uno de los tres radios de las esferas imaginarias y se montaron en la base fija del prototipo físico del manipulador paralelo tipo delta formando un triangulo equilátero de 163 mm de lado (Figura 2-10).

Figura 2-10 – Sensores montados en la base fija del robot delta

18

Dichos sensores poseen un hilo de nylon los cuales son pivoteados para forman la geometría triangular representada en la Figura 2-11 como los puntos P1, P2 y P3 estableciendo un sistema coordenado con la misma referencia que tiene el punto OEF de la plataforma móvil. Es decir se establece una referencia absoluta siendo el punto P el origen de esta referencia (Figura 2-13). De esta forma, sin importar la posición en la que se encuentre el punto OEF, se podrá calcular su posición mediante la metodología de trilateración explicada anteriormente.

Figura 2-111 – Sistema coordenado formado por la geometría de los sensores

Cada uno de los puntos pivoteados, P1, P2 y P3, fueron físicamente alineados con cada una de las articulaciones, es decir, el punto P1 con la articulación 1, el punto P2 con la articulación 2 y el punto P3 con la articulación 3 respectivamente (Figura 2-12) .Esto se hizo con el fin de referenciar la articulación 1 con el eje de las y.

19

Capítulo 2: Marco Teórico

Figura 2-12 - Número de articulación en el Robot Delta

Figura 2-13 - Geometría para el metodo de trilateración

Al estar los radios de las esferas en el mismo plano z sus ecuaciones quedarían como r

2 1

= x + (y - y ) + z

r = (x 2

2

r

2 3

2

2

(2.1)

2

1

+ (y - y )

2

x)

2

2

2

= (x - x )

2

3

+ (y - y )

2

3

20

+ z

2

+z

2

(2.2) (2.3)

Al restar 2.3 de 2.2

(2.4)

Al reducir términos y estableciendo la igualdad y = y se despeja para x 2

3

(2.5)

Al restar 2.3 de 2.1

(2.6)

Reduciendo términos y despejando para y

(2.7)

Al despejar z de 2.1

(2.8)

Para la ecuación 2.8 se tendrían dos valores z+ y z para la intersección de las tres esferas en -

el eje z, sin embargo, de la forma que está establecido el eje coordenado podemos descartar automáticamente el valor positivo ya que físicamente este valor es inalcanzable por la base móvil del manipulador prototipo Delta, por lo que el valor correcto siempre será el negativo para la coordenada z del punto O . E F

21

Capítulo 2: Marco Teórico

2.4.4. Cinemática inversa Existen varias metodologías para la cinemática inversa, Newton-Rhampson, método geométrico, etc. Debido a la facilidad del análisis en el método geométrico, en donde se utilizan teoremas trigonométricos y proyecciones sobre planos, se utilizará para dar solución al problema de calcular el ángulo en el brazo i (i = 1, 2, 3) a partir del conocimiento del punto OEF. Primeramente, el punto OEF se traslada hacia el punto Ci del brazo i con la siguiente matriz

(2.9)

a)

Vista frontal

b) Vista lateral

Figura 2-14 - Descripción de la geometría del brazo i

Siendo C el punto relativo a el eje coordenado x , y , z , b' la longitud del brazo b i

i

proyectado en el plano x z . Y P O i;

i

E F

i

i

el vector de la posición del punto O relativo al eje EF

coordenado xi, zi.

22

Debido a que el cálculo de la proyección b' es necesaria para el método geométrico, éste se calcula siendo su valor:

(2.10)

Los lados a y b' del brazo i forman un triangulo siendo el segmento A C el vector que va I

I

de A al punto C . Este vector forma un ángulo con este segmento A B llamado 0 , y la i

i

I

I

2i

horizontal forma un ángulo 9 con el vector A C por lo que: 3i

I

I

(2.11)

Al resolver para 9 se usa la tangente inversa siendo: 3i

(2.12)

Aplicando la ley de cosenos se resuelve para encontrar 9

2i

(2.13)

23

Capítulo 2: Marco Teórico

2.5. Espacio de Trabajo

El cálculo del espacio de trabajo del robot Delta es equivalente a la intersección de los volúmenes que forma cada articulación, de esta forma, los puntos alcanzables por cada brazo describen una esfera de radio el antebrazo del robot (parámetro a), siendo el codo del robot su centro(punto B). De esta manera, el espacio alcanzable por el efector final es la intersección de los tres volúmenes en forma de toroide que se forman en cada brazo.

a) Vista Isométrica

b) Vista Superior

Figura 2-15 – Espacio de trabajo para el prototipo de Robot Delta

2.6. Resumen En el presente capítulo se presentó el estado del arte del tipo de robot paralelo utilizado en este trabajo de tesis: El Robot Delta. Se definió tanto el método alterno utilizado para determinar la posición del efector final respecto a un sistema coordenado absoluto denominado trilateración como también el sistema de sensado el cual es una implementación de vanguardia en este tipo de manipuladores. Es decir, la presencia de este tipo de aplicación en el mercado es inexistente. Así también se presentó el análisis de la cinemática inversa mediante el método geométrico el cual es sencillo de aplicar debido a que es demostrada trigonometría simple. 24

3.

Desarrollo de un Manipulador Delta

En este capítulo se hace una descripción del prototipo desarrollado en el Departamento de Mecatrónica y Automatización del Instituto Tecnológico y de Estudios Superiores de Monterrey campus Monterrey y que sirve para la implementación de un control difuso expuesto en el capítulo cuatro. Al principio se presenta el diseño mecánico, seguido del diseño electrónico y por último el sistema de comunicación utilizado. Al final de la sección de comunicación se describe la interfaz humano-máquina y el algoritmo implementado para el control del manipulador.

25

Capítulo 3: Desarrollo de un Manipulador Delta

3.1. Introducción Como parte del trabajo de investigación de esta tesis, se ha continuado con la coordinación del proyecto de construcción de un prototipo del Robot Delta con actuadores rotacionales. Este proyecto se ha venido desarrollando en el Departamento de Mecatrónica y Automatización del Instituto Tecnológico y de Estudios Superiores de Monterrey campus Monterrey. El desarrollo permitirá la implementación y evaluación de esquemas de control como lo son el control adaptable, predictivo y experto; control difuso; control con antealimentación, entre otros. Además de contar con otras aéreas de desarrollo en robótica como lo puede ser la implementación de sistemas de visión, planeación de trayectorias, etc.

Figura 3-1 – Prototipo desarrollado en el Departamento de Mecatrónica y Automatización del Instituto Tecnológico y de Estudios Superiores de Monterrey campus Monterrey

26

El diseño de implementación se ha dividido en tres áreas:

 Diseño Mecánico.- Todo aquello que esté relacionado con la estructura mecánica; la selección de los actuadores, sistemas de transmisión y demás componentes como la construcción de las articulaciones, sensores que tendrá el robot manipulador delta.  Diseño Electrónico.- Todo aquello que esté relacionado con la circuitería electrónica que permita el movimiento de los actuadores, comunicación entre dispositivos para el control del manipulador y demás componentes que permita el buen funcionamiento del robot.  Comunicación.- Todo aquello relacionado con los protocolos de comunicación entre la computadora y el microcontrolador. Además está relacionado con la programación de las funciones de la interfaz humano-maquina y del microcontrolador. Al término de esta tesis, se ha rediseñado la estructura mecánica que logre el movimiento del robot así también como su sensado; se ha implementado un circuito electrónico que permite la comunicación entre la computadora y el microcontrolador; se ha realizado una interfaz humano-máquina para operar el manipulador paralelo tipo delta y se ha realizado un esquema de control difuso para cada una de las tres articulaciones.

3.2. Diseño Mecánico Debido a que este trabajo es una continuación de [3] y [11] se partió de la estructura mecánica usada en ambos trabajos mostrada en la Figura 3-2; en ésta se muestra la base fija y móvil del robot, los tres brazos de la configuración Delta y los tres motores acoplados a un sistema de reducción siendo una banda la forma de acoplamiento. Esta estructura tiene los componentes mecánicos básicos para la configuración Delta, pero debido a las condiciones adversas que ha tenido a lo largo del tiempo sus articulaciones han sufrido desajustes dando origen a “juegos” mecánicos. Estos “juegos” son perjudiciales para el control por lo que en la segunda parte de esta sección se amplían los detalles de las 27

Capítulo 3: Desarrollo de un Manipulador Delta implementaciones mecánicas realizadas en el prototipo para disminuir estas fallas en los diversos acoplamientos.

a)Vista lateral

b) Vista superior

Figura 3-2 – Estructura Inicial del prototipo

3.2.1. Diseño original del prototipo En esta sección se describirá la estructura mecánica de la que se partió para este trabajo de tesis. Así mismo se mostrarán las características más importantes de cada uno de los elementos que la componen. La estructura que sostiene a la base fija del prototipo es un perfil PTR cuadrado de acero de 50 mm y tiene una altura de 1200 mm (Figura 3.2). La base fija que sostiene los motores los cuales forman un triángulo equilátero de 320 mm de lado. Estos se encuentran a una distancia de 150 mm con respecto al centro de la misma.

28

a)

b)

Figura 3-3 - Vista de a) mecanismo de tracción y b) Union entre antebrazo y brazo

Los brazos y antebrazos están hechos con barras tubulares de aluminio de 12.7mm (1 2’’) y 9.525mm (3 8’’) de diámetro. La longitud de total de cada brazo es de 200 mm, mientras que la longitud total de cada antebrazo es de 400 mm. La unión entre estos dos elementos se realiza mediante dos rotulas de 9.525 mm (3 8’’) con rosca fina de 24 hilos. En uno de los extremos de los antebrazos se encuentra la base móvil donde se acopla el efector final del robot manipulador Delta. La distancia entre el centro y esta es de 25 mm. En la Figura 3.4 se muestra tanto la base móvil, su unión con el antebrazo y el efector final. Los motores de corriente directa utilizados para mover cada una de las articulaciones del robot delta utilizan 24 VCD de alimentación, soportan una corriente máxima de 2 A y una potencia promedio de 22 W. Su velocidad máxima es de 3200 rpm. Además, estos motores, se encuentran acoplados a los brazos del robot por un tren de reducción 200:1 y una banda que transmite el movimiento a las articulaciones (ver Figura 3.3a).

29

Capítulo 3: Desarrollo de un Manipulador Delta

a) Vista lateral

b) Vista superior

Figura 3-4 – Base movil del manipulador

3.2.2. Rediseño final del prototipo Si no se tiene un buen diseño mecánico de cualquier proceso físico, el desarrollo de casi cualquier tipo de control resultaría infructuoso debido a que la planta en sí sería inestable. Es por esto que, antes de intentar la sintonía de cualquier tipo de control, se realizaron los ajustes mecánicos pertinentes para reducir en gran parte, los desajustes mecánicos presentes del prototipo original. Se colocaron soportes de aluminio para cada uno de los elementos de transmisión de 3 mm de espesor los cuales eliminan prácticamente el desajuste mecánico existente con la base fija (Figura 3-5a). Para la unión del brazo con el mecanismo de transmisión de movimiento se agregaron dos arandelas de acero de 2.5 mm de espesor los cuales redujeron considerablemente el “juego” pero sin eliminarlo por completo (Figura 3-5b).

30

a) Soporte de transmisión

b) Arandelas en el brazo

Figura 3-5 – Ajustes mecánicos implementados en el prototipo

Se han colocado tres sensores de desplazamiento con el fin de llevar a cabo la metodología de trilateración explicada en el capítulo dos de este trabajo de tesis (Figura 3-6). Estos tres sensores han sido construidos en una estructura de acrílico y tienen un hilo de nylon el cual se conecta en el efector final y su longitud varía según sea el desplazamiento del mismo. Este movimiento se transmite a un potenciómetro de precisión de diez vueltas de 10 kΩ que entrega un voltaje proporcional a la distancia que la base móvil recorre. Se han montado sobre la base fija formando un triángulo equilátero como se muestra en la Figuras 3.5 y el hilo de cada sensor se pivotea en dos lugares fijos, tal como se mostró en el capítulo dos, de la estructura ampliando este triángulo a uno de 163 mm de lado con el fin de lograr una mejor sensibilidad al momento de aplicar el método de trilateración.

31

Capítulo 3: Desarrollo de un Manipulador Delta

a) Vista superior

b) Vista lateral

Figura 3-6 – Vista de los sensores de desplazamiento instalados en la base fija del manipulador

Figura 3-7 – Estructura final del prototipo

Como se puede apreciar en la Figura 3-7 en la parte superior de la base fija se colocó una cubierta de acrílico con el fin de no dejar expuestos los elementos electrónicos y sensores que se montaron en el prototipo del robot Delta. 32

3.3. Diseño Electrónico Respecto al diseño electrónico, se identifican dos elementos principales: -

Microcontrolador ATmega8 de Atmel

-

3 circuitos de potencia GENCODRIVE 6320

Figura 3-8 - Esquemático de la conexión electrónica del microcontrolador

Cada motor se acciona a través de una interfaz de potencia encapsulado GENCODRIVE 6320 el cual es básicamente un puente H de 5 A y 55VCD diseñado para aplicaciones de control de movimiento en motores de corriente directa. Este puente H recibe un tren de pulsos de 5 VCD para transmitir de igual forma un ancho de pulso modulable a 24 VCD. Además es utilizado un microcontrolador ATmega8 de Atmel donde se encuentra el programa responsable de la comunicación entre la interfaz de potencia del robot y la Interfaz Humano-Máquina (IHM) desarrollado en el software de programación grafica LabView® de National Instruments. También se lleva a cabo una conversión de datos para las señales que salgan del microcontrolador puedan ser interpretados por el circuito de 33

Capítulo 3: Desarrollo de un Manipulador Delta potencia. Adicionalmente, los tres sensores de posición son conectados a los pines de conversión analógica-digital del microcontrolador (ver Figura 3.6), no sin antes pasar por una etapa de filtrado que consta de un circuito pasivo RC. Como se explicó anteriormente, el circuito de potencia recibe un tren de pulsos para mandar una señal de potencia hacia los motores, este tren de pulsos es enviado por el microcontrolador pasando por resistencias de 100 Ohms tal como se muestra en la Figura 3-6. Como son tres los motores que se deben mover en el Robot Delta, solo las resistencias que están conectadas a los pines 12, 14 y 16 del microcontrolador son las que llevan esos trenes de pulsos. Las resistencias que se encuentran en los pines 11, 13 y 15 del microcontrolador son las que determinan si el motor se moverá en sentido a favor o en contra de las manecillas del reloj. Debido a que se ha desarrollado una interfaz en una computadora se utiliza el protocolo de comunicación RS232 para interactuar entre el microcontrolador y un ordenador por lo que se incluye un buffer para este proceso, el integrado max232 (Figura 3-7).

Figura 3-9 – Esquemático electrónico para el protocolo de comunicación RS232

34

Figura 3-10 – Esquematico del encapsulado GENCODRIVER 6320

En la Figura 3-10 se muesra el esquematico del encapsulado GENCODRIVER 6320 usado para convertir el tren de pulsos enviado por el microcontrolado la señal recibida del microcontrolador en una señal de ancho modulable de potencia hacia cada motor de cada articulación.

3.4. Comunicación Esta área es la que lleva a cabo el intercambio de datos y señales entre el circuito de potencia, los sensores, el microcontrolador y la computadora a través de una Interfaz Humano-Máquina (IHM). En este sistema el microcontrolador actúa como un convertidor y adaptador de señales, y es en la interfaz hecha en la plataforma LabView ® donde se realiza el control general del robot.

3.4.1. Interfaz Humano-Máquina Para la interaccion con el robot manipulador tipo delta se diseñó una interfaz sobre la plataforma LabView®. Esta provee al usuario la capacidad de mover el robot, observar las trayectorias y proporcionar la sintonía del control difuso aplicado a cada una de las articulaciones del robot. 35

Capítulo 3: Desarrollo de un Manipulador Delta

Figura 3-11 – IHM diseñada para el control del manipulador

La interfaz cuenta con un periodo de muestreo ajustable. En cada intervalo se realiza el control de las tres posiciones deseadas (x, y, z) mediante lógica difusa y se mandan los valores de la manipulación al microcontrolador y éste a su vez a cada uno de los circuitos de potencia de cada brazo. Por otra parte se reciben los paquetes del microcontrolador con los valores de los tres sensores y en la programación de la interfaz se calculan las tanto las posicones cartesianas (x, y, z) como las posiciones angulares (θ11, θ12, θ13) de cada una de las articulaciones y cada medición se muestra en la parte inferior de la interfaz (ver Figura 3.7a). En la parte inferior izquierda se encuentra el interruptor principal para la conexión del microcontrolador con la IHM mediante el protocolo de comunicación RS232 y se muestra el estado de esta conexión. Antes de presionar el botón se tuvo que haber configurado el número de puerto en el cual está conectado físicamente el microcontrolador (ver Figura 3.7c). En la parte inferior central se encuentra la posición espacial de referencia que puede ser cambiada en cualquier momento ya que, como se mencionó anteriormente, el esquema de control difuso estará calculando los valores de manipulación de cada una de las articulaciones para llegar a la posición espacial deseada. La posición actual del punto OEF se gráfica en la parte superior para los ejes x, y, z. 36

Por otro lado, con el fin de dar de alta la sintonía del control difuso, es necesario seleccionar el archivo con extensión *.fc previamente creado para cada articulación (ver Figura 3.7b). El procedimiento para crear un archivo *.fc se incluye en el apéndice A.

a)

b)

c) Figura 3-12 – Detalle de la IHM

En la sección de programación del software LabView®, se incluyeron los algoritmos para la implementación del esquema de control, la metodología de trilateración, el análisis de la cinemática inversa y comunicación con el microcontrolador. En la Figura 3-12 se muestra el pseudocódigo utilizado para realizar el control de la posición del efector final en el manipulador Delta.

37

Capítulo 3: Desarrollo de un Manipulador Delta

Figura 3-13 – Pseudocódigo implementado en LabView®

3.4.2. Microcontrolador El microcontrolador que se seleccionó para la adquisición y procesamiento de datos fue el ATmega8 de Atmel el cual incorpora seis canales de conversión analógica digital (ADC), así como puertos de comunicaciones SPI (Serial Peripheral Interface) y UART (Universal Asynchronous Receiver-Transmiter). Las funciones que lleva a cabo el microcontrolador son las siguientes: 

Transmisión a la IHM. Enviar los valores actuales de cada uno de los sensores para realizar la metodología de trilateración. 38



Recepción de Manipulación. Recibir los valores numéricos de las manipulaciones calculadas por el control difuso de cada uno de las tres articulaciones



Conversión de datos. Convertir los valores numéricos de las manipulaciones a señales digitales para los circuitos de potencia de cada una de las articulaciones.



Lectura de Sensores. Mediante tres canales de conversión analógica digital se realiza la lectura actual de cada sensor de desplazamiento para poder transmitir estos datos a la IHM.

Figura 3-14 – Esquemático general de la electrónica y comunicación del manipulador Delta

3.5. Resumen En este capítulo se ha descrito de forma general el desarrollo del prototipo en el cual se implementó un esquema de control difuso. Se muestra la forma en que se instalaron físicamente los sensores para la metodología de trilateración, esto es muy importante debido a que si se incurre en algún error de posicionamiento o imprecisión física se tendrían que reestructurar las ecuaciones que dan origen a la misma. Así también se describe el algoritmo del programa implementado en LabView® y los componentes electrónicos usados para el control manipulador Delta basado en lógica difusa.

39

Capítulo 3: Desarrollo de un Manipulador Delta

40

4.

Diseño e Implementación de un controlador Difuso en un Manipulador Delta

En el presente capítulo se da la introducción a control difuso y las bases teóricas que lo fundamentan así como el esquema de control difuso implementado en el robot manipulador Delta

41

Capítulo 4: Diseño e Implementación de un Controlador Difuso en un Manipulador Delta

4.1. Introducción

Con el fin de controlar la posición del efector final del manipulador se ha diseñado una estrategia de control inteligente conocida como control difuso. Dicha estrategia fue desarrollada en la década de los 60’s por Lofti Zadeh [19] con el fin de emular el conocimiento humano sobre un proceso a partir de una teoría denominada teoría de conjuntos difusos. En este capítulo se expondrá la estrategia de control y su estructuración. Se hará una introducción a los principales conceptos del control difuso y a la lógica detrás de este. Se analizarán los pasos para diseñar un controlador difuso.

4.2. Control Difuso

El Control difuso provee una metodología formal para representar, manipular y aplicar el conocimiento heurístico (tanteo ó reglas) de un ser humano acerca de cómo controlar un sistema. Es decir, las variables de entradas y de salidas se procesan cualitativamente en vez de hacerlas cuantitativamente, por ejemplo, normal, frío, muy frío, extremadamente frío. De esta forma, este tipo de control utiliza un conjunto de términos lingüísticas como acciones de control sobre el proceso que satisfagan las necesidades y requerimientos del sistema.

Figura 4-1 – Diagrama de bloques de un esquema de control difuso

42

Para poder desarrollar un controlador difuso primero se toma como base el modelo FKBC (Fuzzy Knowledge Base Control) que se divide en tres etapas: La fuzificación, el mecanismo de inferencia y la defuzificación (Figura 4-2). Primero se plantean los antecedentes, es decir traducir las variables cuantificables en elementos cualitativos. A esto se le conoce con el nombre de fuzificación. Después, el mecanismo de inferencia obtiene conclusiones o consecuentes a partir de los antecedentes (variables fuzificadas). Como estas variables fuzificadas todavía no son cuantificables, es necesario pasarlas a través de un proceso denominado defuzificación para poder utilizarlas como una variable de control.

Figura 4-2 – Estructura del Controlador Difuso

4.2.1. Fuzificación En este proceso es necesario definir los conjuntos difusos que serán empleados, de forma que puedan ser catalogadas las variables de entrada. Así también, se definen las funciones de membresía para cada una de estas variables con el fin de determinar el grado de pertenencia dentro de estas funciones. 43

Capítulo 4: Diseño e Implementación de un Controlador Difuso en un Manipulador Delta Una de las funciones más utilizadas para el diseño del proceso de fuzificación es la trapezoidal, ya que tiene la ventaja de poder configurarse para construir otras funciones de membresía mostradas en la tabla 4.1. Función de Membresía

Parámetros

L

α=β

Γ

γ= δ

Triangular

β=γ

Trapezoidal

α≠β≠γ≠δ

Tabla 4-1 – Funciones de membresía más comunes para la fuzificación

44

4.2.2. Mecanismo de inferencia Una vez realizada la fuzificación se está en condiciones de evaluar los antecedentes de las reglas obteniendo el grado de verdad o “peso” para cada una de ellas simulando el proceso de decisión humana, al que se le denomina consecuente. Al utilizar este procedimiento se llega a la inferencia de resultados. Si se tiene una regla como la siguiente: SI la temperatura es baja ENTONCES aumente el ciclo de actividad del calefactor. Se asigna directamente como peso, el grado de pertenencia del valor leído de temperatura a la etiqueta lingüística baja. Por lo que es la finalidad del mecanismo de inferencia el formar expresiones o reglas base de la siguiente forma: SI [antecedente 1] y.. y [antecedente n] ENTONCES [consecuente 1] y.. y [consecuente n]

Con el fin de obtener los consecuentes, se utilizan operadores e implicaciones difusas que toman en cuenta los grados de pertenencia de las variables de entrada: complemento, unión e intersección. Cuando a dos variables difusas se les aplica una operación de unión (que en lógica boolena es equivalente a una operación OR) el resultado se obtiene tomando el valor más grande entre las variables de entrada. Para el caso de la intersección (equivalente a la operación AND) el valor resultante de la operación corresponde al mínimo valor de alguna de las entradas. En la operación complemento (equivale a una operación NOT), se toma el valor que complementa a uno. Zadeh define estos operadores de la siguiente forma: 





AND 𝜇𝐴

𝐵

𝑢 = 𝑚á𝑥 𝜇𝐴 𝑢 , 𝜇𝐵 𝑢

(4.1)

𝜇𝐴

𝐵

𝑢 = 𝑚í𝑛 𝜇𝐴 𝑢 , 𝜇𝐵 𝑢

(4.2)

OR

NOT 𝜇𝐴′ 𝑢 = 1 − 𝜇𝐴 𝑢

45

(4.3)

Capítulo 4: Diseño e Implementación de un Controlador Difuso en un Manipulador Delta La forma en que se relacionan antecedentes y consecuentes es por medio de implicaciones difusas; la más utilizada es la implicación de Mamdani que se muestra a continuación. 

Implicación de Mamdani 𝜇𝑅𝑚 𝑥, 𝑦 = 𝑚í𝑛 𝜇𝐴 𝑥 , 𝜇𝐵 𝑦

(4.4)

x1/x2

NG

NP

C

PP

PG

PG

C

PP

PG

PG

PG

PP

NP

C

PP

PG

PG

C

NG

NP

C

PP

PG

NP

NG

NG

NP

C

PP

NG

NG

NG

NG

NP

C

Tabla 4-2 - Tabla de implicaciones difusas para dos variables y una salida con cinco conjuntos difusos. PG: positivo grande; PP: positivo pequeño; C: cero; NP negativo pequeño; NG: negativo grande.

4.2.3. Defuzificación Una vez formadas las reglas base y definidas las funciones de membresía para los consecuentes, estos deben ser traducidos a variables cuantitativos para poder utilizarlas como acciones de control. Los más comúnmente usados son: Singeltons, Centroide, Promedio del centro de área, entre otros. Se describirá a continuación el método de defuzificación por Centroide, por ser el método a emplear en ésta tesis.

4.2.4. Método de Centroide Este método consiste en determinar el centro de gravedad de todas las áreas generadas por el mecanismo de inferencia difusa en las funciones de membresía de salida, este valor numérico del centroide es la manipulación hacia el proceso a controlar y es representado matemáticamente de la siguiente manera: 46

𝑧∗ =

𝜇𝑐 𝑧 𝑧𝑑𝑧 𝜇𝑐 𝑧 𝑑𝑧

(4.5)

Figura 4-3 - Defuzificación por centroide

Con el fin de explicar mejor este método se da el siguiente ejemplo. Tomándose como base los conjuntos difusos mostrados en la Figura 4-4 y 4-5. Se asigna un valor a la variable de entrada (Error) de diez, activando las áreas de pertenencia de cada término tal como se muestra en la Figura 4-5a y mediante las el mecanismo de inferencia mostrado en la sección 4.3.2 se activan los términos de salida correspondientes y dan como origen a las áreas sombreadas mostrada en la Figura 4-4. Aplicando la ecuación 4.5, se obtiene el valor de 211.

a)

b)

Figura 4-4 – Términos lingüisticos activos a) de entrada y b) de salida para una entrada igual a diez

47

Capítulo 4: Diseño e Implementación de un Controlador Difuso en un Manipulador Delta

Figura 4-5v– Área formada con los términos lingüisticos activos de salida

4.3. Implementación y sintonización del control difuso

Debido a la no linealidad del prototipo, se ha implementado un esquema de control difuso, que en conjunto con el método de trilateración y el modelo cinemático inverso forman el lazo de control que se muestra en la Figura 4-5.

Figura 4-6 - Lazo de control implementado en el prototipo Delta

Con el fin de sintonizar el controlador difuso es necesario entender el comportamiento del manipulador en lazo cerrado para conocer los efectos del mismo ante cambios de referencias “grandes” y “pequeñas”.

4.3.1. Fuzificación Las funciones de membresía se muestran en la figura. La variable de entrada que se maneja es el error de la posición angular con tres conjuntos difusos y sus respectivas funciones de membresía. Estas funciones son: Positivo, Cero y Negativo.

48

1 0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0 -90 -80 -70 -60 -50 -40 -30 -20 -10 0 10 20 30 40 50 60 70 80 90 Grados Negativo

Cero

Positivo

Figura 4-7 – Conjuntos difusos para el error en la posición angular

Se aprecia que el término lingüístico Cero de la Figura 4-7 es una función del tipo trapezoidal en donde β = -2 grados y γ = 2 grados, es decir, existe una banda muerta de 4 grados en la cual el controlador considera que el error generado por la posición angular de cada una de los brazos con respecto a la horizontal es igual a cero, por lo tanto el controlador no realizará ningún cambio en la manipulación; esto puede dar origen a un error de estado estable en la respuesta articular de ± 2 grados. Sin embargo, el diseño de estos términos lingüísticos es razonable debido a que, experimentalmente, solo se obtuvo una desviación espacial máxima de 10 mm, es decir, en x, y, z. Esta banda muerta se diseña a partir del ruido electrónico introducido por el sistema de sensado.

4.3.2. Inferencia El controlador difuso debe manipular la referencia para guiar al sistema a la posición angular deseada por lo que las reglas base para el mecanismo de inferencia serian las siguientes de acuerdo a la tabla 4.3 1. Si e es Negativo entonces μ0 49

Capítulo 4: Diseño e Implementación de un Controlador Difuso en un Manipulador Delta

4.3.3. Defuzificación Para la defuzificación se utilizará el método del centroide el cual modifica la acción de control según el cambio del centro de masa del área bajo la curva formada por el mecanismo de inferencia difusa; por lo que se deben de establecer los conjuntos difusos para la salida, que en este caso será el número que tendrá el tren de pulsos enviado al circuito de potencia.

Error

N

C

P

Salida

N

C

P

Tabla 4-3 - Tabla de implicaciones difusas para el controlador

1 0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0 -6000

-4000

-2000

0

2000

4000

6000

Número de Pulsos Negativo

Cero

Positivo

Figura 4-8 - Conjuntos difusos de salida

En la Figura 4-8 se pueden apreciar los conjuntos difusos de salida, sin embargo con el fin de apreciar mejor el término lingüístico Cero, se han reducido los límites mostrados en el eje de las x siendo en realidad los valores máximos y mínimos de este eje 11,200 y -11,200

50

4.4. Resumen A lo largo del presente capítulo se ha dado un repaso a los conceptos básicos del control difuso y se ha presentado el diseño del esquema control implementado para un robot manipulador tipo delta. Se menciona la causa por la cual puede haber un error de estado estable en cualquiera de las respuestas articulares, dando origen a un error en estado estable espacial, es decir en cualquiera de las coordenadas x, y, z. En el capítulo siguiente se documentan las pruebas realizadas en el prototipo con el fin de justificar este tipo de implementación.

51

52

5. Pruebas y Análisis de Resultados

En este capítulo se presentan las pruebas realizadas en el manipulador Delta con un esquema de control basado en lógica difusa. Se presentan los comportamientos de las articulaciones al aplicar un cambio en la referencia espacial. Se exponen las gráficas obtenidas y tablas con indicadores de desempeño en cada caso.

53

Capítulo 5: Pruebas y Análisis de Resultados

5.1. Introducción Con el fin de determinar el desempeño de un control difuso en el prototipo manipulador tipo delta descrito en el capítulo 3 se realizaron pruebas de cambio de posición espacial las cuales se expondrán en la siguiente sección. Prueba I: Cambio de referencia en posición de (xo, yo, zo) = (0, 0, -500) a (xf, yf, zf) = (0, 0, -300) con respecto al punto P sin carga y con carga de 0.20 kilogramos. Prueba II: Cambio de referencia en posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P sin carga y con carga de 0.20 kilogramos. Prueba III: Cambio de referencia en posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (-100,0, 450) con respecto al punto P sin carga y con carga de 0.20 kilogramos. En todas las pruebas se tomó como tiempo de muestreo 40 ms. Esto es importante ya que la escala del eje de las abscisas esta en intervalos de muestreo. Como criterio de evaluación se utilizan los indicadores de desempeño: el tiempo de estabilización y el sobretiro que la variable tiene sobre la referencia.

5.2. Prueba I 5.2.1. Prueba I.A Las condiciones de la prueba fueron las siguientes: Cambio de referencia en posición espacial de de (xo, yo, zo) = (0, 0, -500) a (xf, yf, zf) = (0, 0, -300) con respecto al punto P sin carga. Los resultados de esta prueba se pueden observar en las Figuras 5-1 y 5-2 y los indicadores de desempeño en las tablas 5-1 y 5-2.

54

a)

b)

c) Figura 5-1 -Respuesta de las articulaciones del robot para la prueba I.A de (xo, yo, zo) = (0, 0, -500) a (xf, yf, zf) = (0, 0, -300) con respecto al punto P sin carga.

55

Capítulo 5: Pruebas y Análisis de Resultados

Articulación

T. Establecimiento

Sobretiro

1

1.52 seg.

0.0%

2

1.52 seg

0.0%

3

1.52 seg.

0.0%

Tabla 5-1 - Índices de desempeño para la prueba I.A con cambio de posición de (xo, yo, zo) = (0, 0, -500) a (xf, yf, zf) = (0, 0, -300) con respecto al punto P sin carga.

a)

b)

56

c) Figura 5-2 - Respuesta espacial del robot para la prueba I.A de (xo, yo, zo) = (0, 0, -500) a (xf, yf, zf) = (0, 0, -300) con respecto al punto P sin carga.

Eje

T. Establecimiento

Sobretiro

X

1.52 seg.

0.0%

Y

1.52 seg

0.0%

Z

1.52 seg.

0.0%

Tabla 5-2 - Índices de desempeño para la prueba I.B con cambio de posición de (xo, yo, zo) = (0, 0, 500) a (xf, yf, zf) = (0, 0, -300) con respecto al punto P sin carga.

57

Capítulo 5: Pruebas y Análisis de Resultados

5.2.2. Prueba I.B Las condiciones de la prueba fueron las siguientes: Cambio de referencia en posición espacial de de (xo, yo, zo) = (0, 0, -500) a (xf, yf, zf) = (0, 0, -300) con una carga de 0.20 kilogramos. Los resultados de esta prueba se pueden observar en las Figuras 5-3 y 5-4 y los indicadores de desempeño en las tablas 5-3 y 5-4.

a)

b)

58

c) Figura 5-3 - Respuesta de las articulaciones del robot para la prueba I.B de (xo, yo, zo) = (0, 0, -500) a (xf, yf, zf) = (0, 0, -300) con respecto al punto P sin carga.

Articulación

T. Establecimiento

Sobretiro

1

1.6 seg.

0.0%

2

1.6 seg

0.0%

3

1.6 seg.

0.0%

Tabla 5-3 - Índices de desempeño para la prueba I.B con cambio de posición de (xo, yo, zo) = (0, 0, -500) a (xf, yf, zf) = (0, 0, -300) con respecto al punto P con carga de 0.20 kilogramos.

. a)

59

Capítulo 5: Pruebas y Análisis de Resultados

b)

c) Figura 5-4 - Respuesta espacial del robot para la prueba I.B con cambio de posición de (xo, yo, zo) = (0, 0, -500) a (xf, yf, zf) = (0, 0, -300) con respecto al punto P con carga de 0.20 kilogramos.

Eje

T. Establecimiento

Sobretiro

X

0.8 seg.

0.0%

Y

1.2 seg

0.0%

Z

1.6 seg.

0.0%

Tabla 5-4 - Índices de desempeño para la prueba I.B con cambio de posición de (xo, yo, zo) = (0, 0, -500) a (xf, yf, zf) = (0, 0, -300) con respecto al punto P con carga de 0.20 kilogramos.

60

Comentarios Prueba I En las figuras 5-1, 5-3, 5-2 y 5-4 se grafican las respuestas articulares de los brazos del manipulador, asi como la respuesta en la posición espacial resultante al aplicar la prueba. En las tablas 5-1, 5-3, 5-2 y 5-4 se exponen los índices de desempeño: el tiempo de establecimiento y sobretiro de las respuestas del esquema de control probado tanto para las articulaciones como para las coordenadas x, y, z. La prueba I es una prueba que demanda el movimiento de los tres brazos con un movimiento coordinado, esto es debido a que la posición inicial y final se encuentran sobre el eje Z (de arriba a hacia abajo). Debido a la estructura simétrica del prototipo Delta, las tres articulaciones se mueven de la misma forma. Como puede observarse en esta prueba no se presenta sobretiro en ninguna de las pruebas, sin embargo en la Figura 5.1.a se presenta un error de estado estable de dos grados, esto se debe debido a la banda muerta de la función de membresía del error de dos grados por lo que el controlador no manda manipulación la articulación uno y por ende se ve reflejado en la posición espacial en y.

5.3. Prueba II 5.3.1. Prueba II.A Las condiciones de la prueba fueron las siguientes: Cambio de referencia en posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P sin carga. Los resultados de esta prueba se pueden observar en las Figuras 5-5 y 5-6 y los indicadores de desempeño en las tablas 5-5 y 5-6.

61

Capítulo 5: Pruebas y Análisis de Resultados

a)

b)

c) Figura 5-5 - Respuesta de las articulaciones del robot para la prueba II.A con cambio de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P sin carga.

62

Articulación

T. Establecimiento

Sobretiro

1

0.8 seg.

0.0%

2

0.6 seg.

0.0%

3

1.6 seg.

0.0%

Tabla 5-5 - Índices de desempeño para la prueba II.A con cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P sin carga.

a)

b)

63

Capítulo 5: Pruebas y Análisis de Resultados

c) Figura 5-6 - Respuesta espacial del robot para la prueba II.A con para cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P sin carga.

Eje

T. Establecimiento

Sobretiro

X

0 seg.

0.0%

Y

0.8 seg

0.0%

Z

0 seg.

0.0%

Tabla 5-6 – Índices de desempeño para cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P sin carga.

5.3.2. Prueba II.B Las condiciones de la prueba fueron las siguientes: Cambio de referencia en posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P con carga de 0.20 kilogramos. Los resultados de esta prueba se pueden observar en las Figuras 5-7 y 5-8 y los indicadores de desempeño en las tablas 5-7 y 5-8.

64

a)

b)

c) Figura 5-7 - Respuesta articular para la prueba II.B con cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P con carga de 0.20 kilogramos

65

Capítulo 5: Pruebas y Análisis de Resultados Articulación

T. Establecimiento

Sobretiro

1

0.8 seg.

0.0%

2

0.6 seg.

0.0%

3

0.6 seg.

0.0%

Tabla 5-7 - Índices de desempeño para cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P con carga de 0.20 kilogramos

a)

b)

66

c) Figura 5-8 - Respuesta espacial del robot para la prueba II.B con para cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P con carga de 0.20 kilogramos.

Eje

T. Establecimiento

Sobretiro

X

0 seg.

0.0%

Y

0.8 seg

0.0%

Z

0 seg.

0.0%

Tabla 5-8 -Índices de desempeño para cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P con carga de 0.20 kilogramos.

Comentarios Prueba II En las figuras 5-5, 5-7, 5-6 y 5-8 se grafican las respuestas de la articulares de los brazos del manipulador asi como la respuesta en la posición espacial resultante al aplicar la prueba. En las tablas 5-1, 5-3, 5-2 y 5-4 se exponen los índices de desempeño: el tiempo de establecimiento y sobretiro de las respuestas del esquema de control probado tanto para las articulaciones como para las coordenadas x, y, z. Estas pruebas fueron realizadas con el fin de realizar un movimiento en el eje y, el cual está establecido por el método de trilateración visto en el capítulo dos de este trabajo de tesis. Adicionalmente, tiene como objetivo el observar que, efectivamente, el eje de las y está 67

Capítulo 5: Pruebas y Análisis de Resultados alineado con el brazo de la articulación 1 debido a como lo muestra la Figura 5-7b y 5-7c, el desplazamiento que se tiene de la articulación 2 y 3 son similares y la que tiene el cambio más significativo es la articulación 1 (ver Figura 5-7a). Esta prueba por lo tanto valida la aplicación del método.

5.4. Prueba III 5.4.1. Prueba III.A Las condiciones de prueba fueron las siguientes: Cambio de referencia en posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (-100,0, 450) sin carga. Los resultados de esta prueba se pueden observar en las Figuras 5-9 y 5-10 y los indicadores de desempeño en las tablas 5-9 y 5-10.

a)

b) 68

c) Figura 5-9 - Respuesta articular para la prueba III.A con cambio de posición de (xo, yo, zo) = (0, 0, 450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P sin carga

Articulación

T. Establecimiento

Sobretiro

1

0.5 seg.

0.0%

2

0.6 seg.

0.0%

3

1 seg.

0.0%

Tabla 5-9 - Índices de desempeño para cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P sin carga

a)

69

Capítulo 5: Pruebas y Análisis de Resultados

b)

c) Figura 5-10 - Respuesta espacial del robot para la prueba III.A con cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P sin carga.

Eje

T. Establecimiento

Sobretiro

X

1 seg.

0.0%

Y

1 seg

0.0%

Z

1 seg.

0.0%

Tabla 5-10 - Índices de desempeño para cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (0, 100, -450) con respecto al punto P sin carga.

70

5.4.2. Prueba III.B Las condiciones de prueba fueron las siguientes: Cambio de referencia en posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (-100,0, 450) con carga de 0.20 kilogramos. Los resultados de esta prueba se pueden observar en las Figuras 5-11 y 5-12 y los indicadores de desempeño en las tablas 5-11 y 5-12.

a)

b)

71

Capítulo 5: Pruebas y Análisis de Resultados

c) Figura 5-11 - Respuesta articular para la prueba III.B con cambio de posición de (xo, yo, zo) = (0, 0, 450) a (xf, yf, zf) = (-100, 0, -450) con respecto al punto P con carga de 0.20 kilogramos.

Articulación

T. Establecimiento

Sobretiro

1

1 seg.

0.0%

2

1 seg.

0.0%

3

1 seg.

0.0%

Tabla 5-11 - Índices de desempeño para cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (100, 0, -450) con carga de 0.20 kilogramos.

a)

72

b)

c) Figura 5-12 - Respuesta espacial del robot para la prueba III.B con cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (-100, 0, -450) con carga

Eje

T. Establecimiento

Sobretiro

X

1 seg.

0.0%

Y

1 seg

0.0%

Z

0.8 seg.

0.0%

Tabla 5-12 - Índices de desempeño para cambio de posición de (xo, yo, zo) = (0, 0, -450) a (xf, yf, zf) = (100, 0, -450) con carga.

73

Capítulo 5: Pruebas y Análisis de Resultados Comentarios prueba III En las figuras 5-9, 5-11, 5-10 y 5-12 se grafican las respuestas de la articulares de los brazos del manipulador asi como la respuesta en la posición espacial resultante al aplicar la prueba. En las tablas 5-9, 5-11, 5-10 y 5-12 se exponen los índices de desempeño: el tiempo de establecimiento y sobretiro de las respuestas del esquema de control probado tanto para las articulaciones como para las coordenadas x, y, z. Esta prueba realiza un cambio en el eje de las x con el fin de probar que el valor de esta coordenada efectivamente se controla con las articulaciones dos y tres del robot según la ecuación 2.5, ya que al observar las Figuras 5-9 y 5-11 se aprecia que el cambio de referencia mas significativo lo poseen estas dos articulaciones.

5.5. Análisis de Resultados En general, se observa que, en cada una de las pruebas realizadas se ha logrado un control satisfactorio de las posiciones tanto espaciales como angulares del prototipo Delta. Tanto en las pruebas I.B, III.A y III.B se nota una variación en las coordenadas donde no se modificó su referencia (Figuras 5.4a, 5.4b, 5.10b, 5.11c, 5.12b, 5.12c), esto se debe a que el movimiento que realiza de un punto a otro no siguen una trayectoria lineal, sino que solo se garantiza que se llegará a estabilizar en el punto final deseado. Otra cuestión importante a considerar es el hecho de la banda muerta en el diseño del control difuso, ya que si la(s) articulación(es) llegara(n) a un rango de dos grados ya sean positivos o negativos para llegar a la referencia, el mismo controlador dejaría de mandar manipulación debido a esta banda muerta, produciéndose un error de estado estable en la posición final de diez milímetros.

74

6. Conclusiones y Trabajos Futuros

En el presente capítulo se realizan las conclusiones finales a las que se llegaron con el trabajo de tesis presentado en capítulos anteriores. Así también se proponen las líneas de investigación que parten de este trabajo.

75

Capítulo 6: Conclusiones y Trabajo a Futuro

6.1. Conclusiones Con el desarrollo de este trabajo de tesis se realizaron aportes tanto tecnológicos como científicos, los cuales se presentan en este capítulo. A pesar de ser la continuación de un trabajo presentado en [3] y [11], se realizaron avances significativos en cuanto a la modelación cinemática, el esquema de control utilizado y la interfaz humano-máquina.

El rediseño mecánico del prototipo Delta fue esencial para eliminar los desajustes mecánicos que presentaba la estructura del manipulador. En cuanto a la electrónica, fue necesario implementar nuevos dispositivos y circuitos integrados para la comunicación y transmisión de señales de la computadora al Robot Delta ya que el prototipo original no contaba con ningún tipo de componentes electrónicos.

Tal como se mencionó en el capítulo dos de este documento se desarrolló un sistema de sensado, el cual ha sido probado ampliamente y es actualmente utilizado en el sistema de posicionamiento global (GPS), con el fin de eliminar una complicación en los manipuladores paralelos que es la del análisis de la cinemática directa; quitando así una desventaja para el uso de esta clasificación de brazos robóticos. Cabe mencionar que la aplicación de esta metodología en el área de robótica no ha sido desarrollada a nivel comercial y tampoco existen en el mercado modelos de manipuladores tipo Delta con este nivel de implementación.

También se realizó un análisis para la cinemática inversa el cual se implementó con éxito al programarlo en la plataforma de programación gráfica LabView®. Los resultados de las pruebas con estos modelos fueron satisfactorios tal como se muestran en cada una de las pruebas expuestas en el capítulo cinco.

Se ha desarrollado e implementado sobre un proceso físico real no lineal como lo es un robot manipulador paralelo tipo delta un esquema de control difuso para el control de posición. Cabe mencionar el hecho de que la estructura física del robot ayuda al controlador mismo; ya que, al eliminar casi por completo las fuerzas inerciales, el diseño de los 76

conjuntos difusos tanto para la entrada como para la salida y las reglas base del controlador difuso es relativamente sencillo.

Se ha programado el algoritmo de control, la metodología de trilateración y el análisis de la cinemática inversa en el diagrama de bloques del software LabView® de National Instruments; que a su vez proporciona un entrono gráfico amigable para el usuario, ya que cuenta con gráficas, botones e indicadores que son intuitivamente sencillos de manejar, dejando al microcontrolador solamente como un tipo de “traductor” para los datos y señales que van del robot a la computadora y viceversa. Es muy importante señalar que el algoritmo del control difuso fue implementado en LabView® mediante el Fuzzy Logic Controller Design incluido en el Fuzzy Logic Toolkit® propiedad de National Instruments. Este punto es explicado en el apéndice de este trabajo.

6.2. Trabajos futuros. Como consecuencia de esta tesis se proponen las siguientes lineas de investigación: 

Estudiar el espacio de trabajo dinámico del robot a diferentes alturas con el fin de aprovechar al máximo los movimientos del manipulador dentro de los límites del espacio de trabajo.



Desarrollar un generador de trayectorias el cual pueda realizar interpolaciones lineales, circulares, entre otras para asegurar el movimiento del efector final de un punto a otro.



Diseñar e implementar otras estrategias de control avanzado para procesos no lineales como lo son: el control deslizante, adaptativo, predictivo, experto, entre otros.



Sustituir el sistema de sensado de trilateración por un sensor wireless, como lo puede ser un acelerómetro, para monitorear la posición en el espacio sin tener que realizar el analisis de cinemática directa.

77

Capítulo 6: Conclusiones y Trabajo a Futuro 

Maquinar y probar varios juegos de brazos y antebrazos de diferentes longitudes para observar las relaciones existentes entre el esquema de control y el espacio alcanzable.

78

Bibliografía [1] J.O. Roa, A.R. Jiménez, F. Seco, C. Prieto, J.L. Ealo, F. Ramos, J. Guevara. Distribución de balizas para sistemas de localización fundamentados en Trilateración Esférica e Hiperbólica. Instituto de Automática Industrial. La Poveda, Madrid 2007.

[2] S.B. Niku. Introduction to Robotics: Analysis, Systems, Applications. Prentice Hall, New Jersey, 2001.

[3] Gilberto Reynoso Meza. Modelación de un Robot Manipulador tipo Delta y Diseño de un Control Supervisorio Difuso. Tesis de Maestría. ITESM Campus Monterrey, Dic. 2005

[4] Lung-Wen Tsai. Robot Analysis. Wiley-Interscience. 1999 [5] Juan Briones León. Diseño, Análisis y Construcción de un Robot Paralelo Traslacional. Tesis de Maestría. Instituto Politécnico Nacional. Mayo 2009 [6] Manases Jiménez, Canseco, Cortés. Diseño y Control de un Robot Paralelo. 6to Congreso Nacional de Mecatrónica. Instituto Tecnológico de San Luis Potosí. Noviembre 2007 [7] Karol Miller. Maximization of Workspace Volume of 3-DOF Spatial Parallel Manipulators. Journal of Mechanical Design. Vol. 124/347 Junio 2002 [8] Sebastien Briot, Ilian A. Bonev. Are Parallel Robots More Accurate Than Serial Robots?. École de technologie supérieure (ETS). Agosto 2007 [9] Michael Stock, Karol Miller. Optimal Kinematic Design of Spatial Parallel Manipulators: Application to Linear Delta Robot. Journal of Mechanical Design. Vol. 125. Junio 203 [10] Oscar Vivas Albán. Control Predictivo Funcional de un Robot Paralelo. Revista Colombiana de Tecnologías de Avanzada. Vol. 1 Noviembre 2007 79

Bibliografía [11] Pedro Armando Juárez Pérez. Implantación de un Control Supervisorio Difuso para el control de posición de un Robot Manipulador Delta. Tesis de Maestría. ITESM Campus Monterrey, Dic. 2006 [12] Robots for Industrial Enviroments. International Organization for Standarization estraido en Mayo de 2010 de http://www.iso.org/ [13] Thomas, Federico. Revisiting Trilateration for Robot Localization. IEEE TRANSACTIONS ON ROBOTICS, Department of Electronic & Telecommunication Engineering is unique in Sri Lanka. VOL. 21, NO. 1, Febrero 2005. [14] Trilateration Communication Protocol. University of Wyoming. Extraído en Marzo de 2010 desde www.cs.uwyo.edu/~wspears/MaxelbotXcommunicationXprotocol.doc

[15] Reymond Clavel. Device for the movement and positioning of an element in space. U.S. Patent 4,976,582. Diciembre 1990 [16] W. Khali, Dombre. Modeling, Identification & Control of Robots. Kogan Page Science, 2004 [17] Industrial Robots. International Federation of Robotics. Extraido en Mayo de 2010 desde http://www.ifr.org/industrial-robots/ [18] Desarrollo de la Robótica. Universidad de Guadalajara. Extraído en Mayo de 2010 desde http://proton.ucting.udg.mx [19] Passino, Yurkovich. Fuzzy Control. Prentice-Hall. 1998

[20] S. Negri, G. Di Bernado, I. Fassi, L. Molinari Tosatti, G. Bianchi, C.R. Boër. Kinematic Analysis of Parallel Manipulators. En Parallel Kinematic Machines: Theortical Aspects and Industrial Requirements, Springer, 1999

80

Apéndice A

Fuzzy Logic Toolkit

En este apéndice se muestra la forma de programar el Fuzzy Logic Toolkit de National Instruments utilizado para la programación del algoritmo de control difuso en este trabajo.

81

Apéndice A

Programación del controlador difuso usando Fuzzy Logic Toolkit® Para la aplicación de la teoría del control difuso en el software LabView® se utilizó el toolkit de lógica difusa el cual carga un archivo con extensión *.fc el cual debe ser creado con anterioridad y que contiene tanto las funciones de membresía de entrada – salida como las reglas IF-THEN. Se empieza por ejecutar el programa LabView® y se selecciona la ruta Tools>>Control Design and Simulation>>Fuzzy Logic Controller Design (ver Figura A.1) para abrir la pantalla de interface principal que se muestra en la Figura A.2.

Figura A.1 Ruta para abrir la herramienta para la programación del control difuso

82

Figura A.2 – Pantalla principal del toolkit de lógica Fuzzy Para editar las funciones de membresía se selecciona el menú Edit>>Set-Editor, cuyo panel frontal se muestra en la figura 3.

Figura A.3 – Panel frontal del editor de funciones de membresía

83

Apéndice A La configuración inicial de un nuevo proyecto siempre empieza de la misma forma: -

Dos entradas lingüísticas normalizadas. Los rangos de ambas variables van de -1 a 1.

-

Cada variable lingüística de entrada está compuesta de tres términos lingüísticos, para la entrada uno: NE1 (negativo), ZE1 (cero), PE1 (positivo) y para la entrada dos: NE2 (negativo), ZE2 (cero), PE2 (positivo).

-

Una variable lingüística de salida la cual tiene tres términos lingüísticos Neo (negativo), ZEo (cero) y PEo (positivo). El rango de la variable va de -1 a 1.

Es posible modificar los términos lingüísticos cambiando los valores de los límites que se encuentran en la parte inferior del panel frontal. También se pueden introducir nuevas variables o nuevos términos a las variables existentes mediante el selector define tal como se muestra en la Figura 4.

Figura A.4 – Opciones del menú define Es muy importante tener en cuenta que el VI que invoca las funciones de membresía dentro del diagrama de bloques de LabView® y éste soporta un máximo de cuarto variables lingüísticas de entrada. También es posible editar el rango de cada variable lingüística así como el nombre que el usuario desee para cada variable con el selector specify.

Figura A.5 – Opciones del menú specify

84

Una vez terminadas las funciones de membresía, se presiona el botón Quit y automáticamente el programa pasa a la pantalla principal. Ahora se deben editar las reglasbase siguiendo la ruta de Edit>>Rulebase-Editor.

Figura A.6 – Pantalla para la edición de reglas base del controlador difuso. Dentro de esta pantalla también se puede seleccionar el método de defuzificación deseado. Una vez que se hayan establecido las reglas, se presiona el botón Quit para regresar a la pantalla principal y guardar el controlador. Se sigue la ruta File>>Save As para salvar el archivo en la ruta deseada y el toolkit le asignará la extensión *.fc el cual se utilizará para “cargar” el controlador difuso dentro del diagrama de bloques en LabView. Dentro del diagrama de bloques de LabView, se sigue la ruta Addons>>PID Control>> Fuzzy Logic para obtener los dos sub-VI’s que se utilizan con el fin de agregar el controlador fuzzy a nuestro programa. Se selecciona el sub-VI Load Fuzzy Controller.vi y se coloca fuera de todos los ciclos del programa. Acto seguido se selecciona el sub-VI Fuzzy Controller.vi y se coloca en el lugar deseado dentro del diagrama de bloques. 85

Apéndice A Las terminales del primer sub-VI son como se muestra en la figura 7 y las del segundo son como se muestran en la figura 8.

Figura A.7 – Entradas y salidas del sub-VI

Figura A.8 – Entradas y salidas del sub-VI

Las conexiones entre ambos sub-VI’s deben ser como siguen: Load Fuzzy Controller.vi

Fuzzy Controller.vi

Controller out

Controller data

input name 1

name 1

min 1

N/C

input name 2

name 2

min 2

N/C

input name 3

name 3

min 3

N/C

input name 4

name 4

min 4

N/C

Tabla A.1 – Conexiones entre terminales

86

La terminal llamada analog output del sub-VI Fuzzy Controller.vi es la manipulación correspondiente a la defuzificación según las funciones de membresía y las reglas base del controlador difuso. Al momento de ejecutar el programa principal se abrirá un cuadro de diálogo que pedirá un archivo con extensión .fc correspondiente a un controlador difuso. Se selecciona el archivo creado con anterioridad y el algoritmo programado en LabView seguirá las reglas base y funciones de membresía que contenga este archivo .fc o en la terminal File Path del Load Fuzzy Controller.vi se coloca la ruta del archivo .fc del controlador difuso. Otra forma de “cargar” el controlador difuso es conectando en la terminal File Path la ruta que tiene el archivo .fc creado.

87

Apéndice B

Programación del Microcontrolador

En este apéndice se documenta el código del microcontrolador el cual es el responsable de realizar la función de intermediario entre el programa desarrollado en LabView y el Robot Delta

88

Apéndice B

Programación del microcontrolador Para la programación del microcontrolador se utilizo la plataforma basada en Basic llamada Bascom. Para poder empezar a programar en esta plataforma es necesario seguir los siguientes pasos: 1. Abrir el programa Bascom-AVR instalado en su computadora. 2. En el menú File se selecciona new para crear un documento nuevo. 3. Se abrirá una ventana que es donde teclearemos nuestro código como lo muestra la Figura B.1

Figura B.1 Ventana de Bascom-AVR para escribir el código de programación

Enseguida se muestra el código programado en el microcontrolador que adecua las señales para la interfaz de potencia, así también lee los valores de cada uno de los tres sensores para la metodología de trilateración $regfile = "m8def.dat" $lib "mcsbyte.lbx" $crystal = 11059200 $baud = 9600

En las primeras dos líneas se incluyen las librerías para manejo de datos y funciones necesarias para el compilador. En las líneas tres y cuatro se especifican la velocidad del cristal externo y el baud rate de comunicación.

Dim A As Byte Dim Inter1 As Integer

89

Apéndice B Dim Inter2 As Integer Dim Inter3 As Integer Dim En1 As Integer Dim En2 As Integer Dim En3 As Integer Dim F1 As Integer Dim F2 As Integer Dim F3 As Integer Dim Cont As Byte Dim U1 As Word At &HB5 Dim U1h As Byte At &HB6 Overlay Dim U1l As Byte At &HB5 Overlay Dim U2 As Word At &HB7 Dim U2h As Byte At &HB8 Overlay Dim U2l As Byte At &HB7 Overlay Dim U3 As Word At &HB3 Dim U3h As Byte At &HB4 Overlay Dim U3l As Byte At &HB3 Overlay

Dim Resultx As Word , Resulty As Word , Resultz As Word

'----------------------------------------- Port Config 'Motor 1 Config Portd.6 = Output Config Portd.5 = Output 'Motor 2 Config Portb.0 = Output Config Portd.7 = Output 'Motor 3 Config Portb.1 = Output Config Portb.2 = Output

Configuración de los pines que indicaran la velocidad y dirección al motor del brazo 1

Configuración de los pines que indicaran la velocidad y dirección al motor del brazo 2

Configuración de los pines que indicaran la velocidad y dirección al motor del brazo 3

Config Adc = Single , Prescaler = Auto , Reference = Internal Start Adc U1 = 0 U2 = 0

90

Inicialización del ADC con referencia de voltaje de 2.56 V

Apéndice B U3 = 0 En1 = 0 En2 = 0 En3 = 0 F1 = 0 F2 = 0 F3 = 0 A=0 Cont = 0 Do Resultx = Getadc(5) Resulty = Getadc(4) Resultz = Getadc(3)

Muestreo de los voltajes causados por el desplazamiento de los sensores instalados para el método de trilateración.

'Inicializa la cuenta según el numero de motores prendidos If En1 = 1 And En2 = 0 And En3 = 0 Or En1 = 0 And En2 = 1 And En3 = 0 Or En1 = 0 And En2 = 0 And En3 = 1 Then Inter1 = 31 Inter2 = 31 Inter3 = 31 Else If En1 = 0 And En2 = 1 And En3 = 1 Or En1 = 1 And En2 = 0 And En3 = 1 Or En1 = 1 And En2 = 1 And En3 = 0 Then Inter1 = 15 Inter2 = 15 Inter3 = 15 Else Inter1 = 7 Inter2 = 7 Inter3 = 7 End If End If

If U1 > 0 Then ' Se prende el motor 1 If En1 = 1 Then Set Portd.6 Do Inter1 = Inter1 - 1

91

Apéndice B Loop Until Inter1 = 0 ' Reset Portd.6 If En2 = 0 And En3 = 0 Then Inter1 = 31 End If If En3 = 1 And En2 = 0 Then Inter1 = 15 End If If En3 = 0 And En2 = 1 Then Inter1 = 15 End If If En2 = 1 And En3 = 1 Then Inter1 = 7 End If Do Inter1 = Inter1 - 1 Loop Until Inter1 = 0 U1 = U1 - 1 End If Else En1 = 0 F1 = 0 End If If U2 > 0 Then ' Se prende el motor 2 If En2 = 1 Then Set Portb.0 Do Inter2 = Inter2 - 1 Loop Until Inter2 = 0 ' Reset Portb.0 If En1 = 0 And En3 = 0 Then Inter2 = 31 End If If En3 = 1 And En1 = 0 Then

92

Apéndice B Inter2 = 15 End If If En3 = 0 And En1 = 1 Then Inter2 = 15 End If If En1 = 1 And En3 = 1 Then Inter2 = 7 End If Do Inter2 = Inter2 - 1 Loop Until Inter2 = 0 U2 = U2 - 1 End If Else En2 = 0 F2 = 0 End If If U3 > 0 Then ' Se prende el motor 3 If En3 = 1 Then Set Portb.1 Do Inter3 = Inter3 - 1 Loop Until Inter3 = 0 ' Reset Portb.1 If En1 = 0 And En2 = 0 Then Inter3 = 31 End If If En1 = 1 And En2 = 0 Then Inter3 = 15 End If If En1 = 0 And En2 = 1 Then Inter3 = 15 End If

93

Apéndice B If En2 = 1 And En1 = 1 Then Inter3 = 7 End If Do Inter3 = Inter3 - 1 Loop Until Inter3 = 0 U3 = U3 - 1 End If Else En3 = 0 F3 = 0 End If

Loop Handle_serial: A = Udr If A = 1 And Cont = 0 Then Printbin Resultx End If If A = 2 And Cont = 0 Then Printbin Resulty End If If A = 3 And Cont = 0 Then Printbin Resultz End If

Select Case Cont Case 0: Select Case A Case 0: Set Portd.5 Set Portd.7 Set Portb.2 Cont = Cont + 1 Case 4:

94

Apéndice B Set Portd.5 Set Portd.7 Reset Portb.2 Cont = Cont + 1 Case 8: Set Portd.5 Reset Portd.7 Set Portb.2 Cont = Cont + 1 Case 12: Set Portd.5 Reset Portd.7 Reset Portb.2 Cont = Cont + 1 Case 16: Reset Portd.5 Set Portd.7 Set Portb.2 Cont = Cont + 1 Case 20: Reset Portd.5 Set Portd.7 Reset Portb.2 Cont = Cont + 1 Case 24: Reset Portd.5 Reset Portd.7 Set Portb.2 Cont = Cont + 1 Case 28: Reset Portd.5 Reset Portd.7 Reset Portb.2 Cont = Cont + 1 End Select Case 1: U1l = A Cont = Cont + 1 Case 2: U1h = A En1 = 1 Cont = Cont + 1 Case 3: U2l = A Cont = Cont + 1 Case 4:

95

Apéndice B U2h = A En2 = 1 Cont = Cont + 1 Case 5: U3l = A Cont = Cont + 1 Case 6: U3h = A En3 = 1 Cont = 0 End Select Return Return End

96