Diseño y control de un robot manipulador

i Equation Chapter 1 Section 1 Dpto. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2013 Proyecto Fin de Grado Grado en Ingeniería Electrónica, Robótica y Mecatrónica. Diseño y Control de un Robot Manipulador Autor: Jesús Tormo Barbero Tutor: José Ángel Acosta Rodríguez ii iii Proyecto Fin de Grado Grado en Ingeniería Electrónica, Robótica y Mecatrónica. Diseño y Control de un Robot Manipulador Autor: Jesús Tormo Barbero Tutor: José Ángel Acosta Rodríguez Profesor titular Dpto. de Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2018 iv v Proyecto Fin de Grado: Diseño y Control de un Robot Manipulador Autor: Jesús Tormo Barbero Tutor: José Ángel Acosta Rodríguez El tribunal nombrado para juzgar el Proyecto arriba indicado, compuesto por los siguientes miembros: Presidente: Vocales: Secretario: Acuerdan otorgarle la calificación de: Sevilla, 2018 vi El Secretario del Tribunal vii A quienes me empujaron a comenzar el camino; al que conmigo lo reccorió y al que apareció en él. A quien seguirá en el que está por venir. A todos, ¡Gracias! viii ix Agradecimientos Quiero aprovechar la ocasión para mostrar mi gratitud a todo aquel que ha estado conmigo en todo este tiempo y dándome cada uno una pizca de ellos mismos, ya que al final, acabamos siendo un poco de cada persona que comparte el tiempo con nosotros. Así, quiero agradecer a mi tutor Jose Ángel Acosta todo el apoyo que me ha dado, la implicación que ha tenido desde primera hora conmigo y con el trabajo, por dejar que volara mi imaginación y permitir desarrollar las ideas que se me ocurrían y ayudarme a que se lograran. Muchas gracias por todo, hay pocos como tu. No se puede dejar atrás a Carlos Rodríguez, que se ha implicado en mi proyecto como si fuera suyo, ofreciéndome multitud de consejos e ideas que han permitido que el proyecto concluyera como es debido. Por supuesto, a las personas que más quiero, a mi padre y a mi madre, al día y a la noche, al que es todo razón y a la que es todo corazón, aunque nadie tiene un corazón más grande que mi padre, ni más sabiduría que mi madre. Gracias a mi padre, por cada consejo, por cada palabra de apoyo y el quitarme peso de presión de encima, por ayudarme con el proyecto en todo momento, por el apoyo proporcionado y por su paciencia/exigencia. Gracias a mi madre, sin tus ánimos y tu risa no habría conseguido nada, por permitirme tenerle todo patas arriba, por los abrazos y las cruces. Gracias a los dos por permitirme vivir esta experiencia. Soy como soy gracias a vosotros, nunca podré agradeceros todo lo que habéis hecho por mi. A mi hermana Cristi, por su autenticidad y su fuerza, por ser siempre un pilar en mi vida, por tener una persona en quien confiar y a la que acudir cuando todo parece ir mal, por todas las cosas que hace una hermana grande a lo largo de su vida por su hermanito pequeño. No sabes cuanto te quiero. A toda mi familia, en especial a mi abuelita, que me ha acogido durante todo el proyecto en su casa dándome la merienda. A mis tios y primos, por darme todo su apoyo y motivación, no hace falta que os diga lo importantes que sois todos para mi y cuanto os quiero. Gracias. No puede faltar mis palabras hacia mi familia elegida, cada una de las personas que me llevan aguantando desde que puse un pie en la calle, con los que he crecido y avanzado, con los que he compartido tantos momentos buenos y no tan buenos, aquellos que se que puedo contar en cualquier momento… Hay personas que dicen poder contar con una mano a sus amigos de verdad, yo tengo la suerte de no compartir esa misma opinión. ¡¡Va por vosotros, Escort!! Y por supuesto a mi banda, por proporcionarme una cama día si y día también, por haber vivido esta aventura juntos, y acabar consiguiendo el objetivo sudando sangre, pero siempre juntos y apoyándonos, y lo más importante, creando un lazo entre nosotros que no se perderá. Ha merecido la pena estos años en la escuela solo por haberos conocido. Y sí, tampoco me olvido de ti, gracias por todo este tiempo guapa, nunca cambies ni dejes de ser tu misma. Te quiero enana. Y para terminar, gracias a todos y cada uno de las personas con las que he convivido día a día en la escuela, en especial, a aquellos profesores que de verdad han disfrutado enseñando, vosotros marcáis la diferencia. No tengo forma de agradeceros todo lo que habéis hecho y hacéis por mi, espero poder hacerlo algún día. Os quiero. Jesús Tormo Barbero Sevilla, 2018 xvi 4.3 Simulaciones Robot Industrial 6 GDL: 41 4.3.1 Comprobación Cinemática: 41 4.3.2 Simulación del Control: 42 5 Experimentación y Análisis de Resultados. 47 5.1 Introducción. 47 5.2 Configuración de Visión Artificial para Robot Compliant. 48 5.2.1 Función de Visión y Método Empleado. 48 5.2.2 Pseudo código Implementado en la RaspBerry Pi. 49 5.3 Programación Arbotix – M. 50 5.3.1 Pseudo Código implemrntado en Arbotix – M. 50 5.4 Estructura del Banco de Pruebas. 52 5.5 Experimento. 53 5.5.1 Objetivo del Experimento: 54 5.5.2 Resultados de la Experimentación. 55 6 Conclusiones. 58 7 Anexos: 60 7.1 Anexo I. Listado de components del Kit Bioloid Premium: 60 7.2 Anexo II. Conexión de Arbotix-M para la programación: 62 7.3 Anexo III. Configuración Software Slic3r para Impresión: 64 7.4 Anexo IV. Manual de Preparación de RaspBerry Pi. 67 7.4.1 Instalación de Sistema Operativo Raspbian Stretch. 67 I. Instalación de Software: 67 II. Formateo de tarjeta SD: 67 III. Descargar y montar Sistema operativo: 67 IV. Crear un archivo sin extensión llamado SSH: 67 7.4.2 Conexión Remota. 68 7.4.3 OpenCV y Librerías. 68 7.5 Anexo V: Código Control Brazo Robot Industrial Arduino: 71 7.6 Anexo VI: Código Python Para Reconocimiento de Ángulos: 87 7.7 Anexo VII: Código Python Calibración de Colores 96 Bibliografía. 98 xvii ÍNDICE DE TABLAS Tabla 2-1. Tabla de Control Zona EEPROM 10 Tabla 2-2. Tabla de Control Zona RAM 10 Tabla 3-1. Parámetros Denavit y Hatemberg Robot Industrial 26 Tabla 3-2. Longitudes del Robot 26 Tabla 5-1. Tabla de Centros 48 xviii ÍNDICE DE FIGURAS Figura 2-1. Kit Bioloid Premium 5 Figura 2-2. CM-530 6 Figura 2-3. RoboPlus Task 7 Figura 2-4. RoboPlusManager 7 Figura 2-5. RoboPlus Motion 8 Figura 2-6. DYNAMIXEL AX-12A 9 Figura 2-7. Explicación Registro “Goal Position” 11 Figura 2-8. Piezas de Ensamble Motores 12 Figura 2-9. Arbotix - M 13 Figura 2-10. RaspBerry Pi 3B 14 Figura 2-11. Impresora Geeetech Prusa i2 Pro - W 15 Figura 3-1. Robot Industrial 6 GDL 17 Figura 3-2. Robot Compliant 17 Figura 3-3. Esquema Robot Antropomórfico 18 Figura 3-4. Elabón 0. Pieza Inferior 19 Figura 3-5. Eslabón 0. Pieza superior 19 Figura 3-6. Eslabón 1. Cara Superior 19 Figura 3-7. Eslabón 1. Cara Inferior 19 Figura 3-8. Eslabón 2. Pieza Superior e Inferior 20 Figura 3-9. Eslabón 2. Piezas Laterales 20 Figura 3-10. Eslabón 2. Pieza Central 20 Figura 3-11. Eslabón 3. Unión Brazo – Muñeca 21 Figura 3-12. Pinza. 21 Figura 3-13. Montaje Robot Industrial Paso 1 23 Figura 3-14. Montaje Robot Industrial Paso 2 23 Figura 3-15. Montaje Robot Industrial Paso 3 23 Figura 3-16. Montaje Robot Industrial Paso 4 23 Figura 3-17. Montaje Robot Industrial Paso 5 23 Figura 3-18. Montaje Robot Industrial Paso 6 23 Figura 3-19. Montaje Robot Industrial Paso 7 23 Figura 3-20. Montaje Robot Industrial Paso 8 23 Figura 3-21. Montaje Robot Industrial Paso 9 23 Figura 3-22. Montaje Robot Industrial Paso 10 24 Figura 3-23. Montaje Robot Industrial Paso 11 24 Figura 3-24. Montaje Robot Industrial Paso 12 24 xix Figura 3-25. Montaje Robot Industrial Paso 13 24 Figura 3-26. Montaje Robot Industrial Paso 14 24 Figura 3-27. Montaje Robot Industrial Paso 15 24 Figura 3-28. Montaje Robot Industrial Paso 16 24 Figura 3-29. Montaje Robot Industrial Paso 17 24 Figura 3-30. Montaje Robot Industrial Paso 18 24 Figura 3-31. Ejes de Coordenadas Robot Industrial 25 Figura 3-32. Esquema Brazo Robot 27 Figura 3-33. Codo Abajo y Codo Arriba. 28 Figura 3-34. Muñeca del Robot. 29 Figura 3-35. Esquema Jacobiano. 30 Figura 3-36. Robot Compliant 31 Figura 3-37. Base de Motores 32 Figura 4-1. Simulación Robot Industrial con ToolBox de Peter Corke. 41 Figura 4-2. Trayectoria Robot Industrial. 42 Figura 4-3. Trayectoria Articular 43 Figura 5-1. Experimento 47 Figura 5-2. Brazo Robot Compliant 48 Figura 5-3. Estructura del Banco de Pruebas 52 Figura 5-4. Posición Inicial 53 Figura 5-5. Posición Final 1 53 Figura 5-6. Posición Final 2 54 Figura 5-7. Posición final 3 54 Figura 5-8. Coordenadas Articulares Robot 6 gdl 55 Figura 5-9. Errores Robot 6 gdl 55 Figura 5-10. Posiciones Articulares Robot Compliant 56 Figura 7-1. Componentes Kit Bioloid Premium 61 Figura 7-2. Conexión Arbotix con Programador FTDI-USB 62 Figura 7-3. Conexión Completa Arbotix - M 63 Figura 7-4. Jumper VIN 63 Figura 7-5. Configuración Slicer. Líneas y Perímetros 64 Figura 7-6. Configuración Slicer. Relleno 64 Figura 7-7. Configuración Slicer. Velocidades 65 Figura 7-8. Configración Slicer. Soportes 65 Figura 7-9. Configuración Slicer. Filamento 66 Figura 7-10. Aplicación ABS Juice 66 xx ÍNDICE DE ECUACIONES Ecuación 3-1. Matriz de Transformación Homogénea. 26 Ecuación 3-2. Matriz de Tranformación T. 27 Ecuación 3-3. Cinemática Inversa. Articulación 1. 27 Ecuación 3-4. Cinemática Inversa. Articulación 3. 28 Ecuación 3-5. Cinemática Inversa. Articulación 2. 28 Ecuación 3-6. Matrices de Rotación 29 Ecuación 3-7. Matriz de Rotación Muñeca. 29 Ecuación 3-8. Coordenadas Articulares Muñeca 29 Ecuación 3-9. Matriz Jacobiana. 30 Ecuación 3-10. Cinemática Directa del Robot Compliant 34 Ecuación 3-11. Velocidad Final del Efector 34 Ecuación 3-12. Ángulo Gamma 35 Ecuación 3-13. Momentos Geométricos 36 Ecuación 3-14. Centro de Gravedad 36 Ecuación 4-1. Datos de Control 39 Ecuación 4-2. Ecuación de error en Velocidad 39 Ecuación 4-5. Jacobiano Inverso sin Configuraciones Singulares. 39 Ecuación 4-6. Controlador PI en el tiempo 40 Ecuación 4-7. Control PI Discreto 40 Ecuación 4-8. Error Cuadrático Medio. 40 Ecuación 4-9. Ecuación de Control Final. 40 xxi xxii 1 1 INTRODUCCIÓN. lo largo de este primer capitulo se van a desarrollar diferentes aspectos que se consideran importantes para la comprensión del trabajo que nos ocupa. Se considera de vital importancia justificar la realización de este proyecto ya que no solo aborda aspectos teóricos sobre los robots, sino que también dispone de un fuerte componente de desarrollo práctico, llevando a cabo el prototipado de dos brazos robóticos. 1.1 Justificación del Proyecto. Cuando se intenta llevar una idea a la realidad, surgen aspectos que no habían sido tenidos en cuenta, de ahí la importancia de que existan prototipos en los cuales se comprueben los contratiempos que puedan surgir, así como solucionar los problemas en versiones de pruebas que ahorran dinero y esfuerzo para la versión final. En este proyecto se ha realizado un prototipo funcional de un sistema en el cuál dos brazos robóticos interactuarán entre ellos. Hay que destacar que se pretende probar el funcionamiento de un nuevo tipo de control para brazos robóticos industriales y compliant. Este nuevo tipo de robot se caracteriza por tener uniones entre los grados de libertad que son flexibles y disponen de amortiguación, posibilitando una amplia cantidad de nuevas funcionalidades y horizontes en el mundo de la robótica. A “Si no puedes volar entonces corre, si no puedes correr entonces camina, si no puedes caminar entonces arrástrate, pero sea lo que sea que hagas, sigue moviéndote hacia adelante.” - Martin Luthr King, Jr. - Introducción. 2 2 1.2 Objetivos: En un principio, el objetivo de este Trabajo Fin de Grado consistía en diseñar un sistema robótico onteniendo el máximo partido posible al Kit Bioloid Premium de la empresa ROBOTIS. Tras realizar un trabajo de investigación sobre los componentes de kit (servomotores, controlador y piezas), se llega a la conclusión de que los actuadores que ofrece el kit son capaces de ofrecer una funcionalidad muy amplia, pero quedan limitados debido al resto de componentes que los acompañan. Llegados a esta conclusión, se decide diseñar y fabricar nuevas piezas que complementan al kit, así como un nuevo controlador más potente que evita los cuellos de botella que tiene el del kit. Tras la adquisición de los nuevos elementos que compondrán el proyecto y conscientes del potencial y el rendimiento que se le podía sacar a los actuadores, los objetivos se amplían, adoptando unas metas un poco más ambiciosas. 1.2.1 Objetivo General. El objetivo principal del Proyecto es realizar un Sistema robótico de pruebas con dos brazos manipuladores y realimentación visual con el fin de experimentar y probar los nuevos controles de robots, en particular, los brazos flexibles. 1.2.2 Objetivos Específicos. • Diseño, construcción y programación de un brazo robótico industrial de seis grados de libertad que sirva para probar el sistema de control que, posteriormente, se empleará en el robot compliant más complejo. • Diseño y construcción de un brazo robótico compliant con articulaciones flexibles y sirva como banco de pruebas de controladores de este tipo. • Realización de un sistema de vision artificial, capaz de complementar las medidas necesarias para el controlador que se deasea implementar en el brazo robótico compliant. 1.3 Estructura de la memoria: El documento que nos ocupa está organizado de forma secuencial, abordando los apartados de la misma manera que fueron surgiendo a medida que se desarrollaba el proyecto. Seguirá el siguiente orden: - Hardware → Se presentarán los elementos usados durante el proyecto, como son los controladores, los actuadores y herramientas de fabricación de piezas. - Sistema → Se divide en tres subapartados en los cuales se explicarán los conceptos, duseño y fabricación de cada uno de los tres objetivos específicos. - Control y Simulación → Definición de controles usados para los sistemas robóticos, así como las simulaciones. - Experimentación y Análisis de Resultados → Se expondrán como se han preparado los experimentos realizados, los pseudocódigos implementados y los resultados obtenidos. - Conclusiones → Se valorará el trabajo realizado una vez concluido el proyecto. - Anexos → Se podrá encontrar en este apartado toda la información considerada de interés, así como los códigos implementados al completo. 3 - Hardware. 10 10 ❖ ÁREA EEPROM: Tabla 2-1. Tabla de Control Zona EEPROM ❖ ÁREA RAM: Tabla 2-2. Tabla de Control Zona RAM 11 Una peculiaridad de los actuadores es que tienen dos modos de funcionamiento, el modo servo motor y el llamado “modo rueda” o de giro continuo. ❖ Modo Rueda o Giro Continuo: Cuando se usa los motores AS-12A en este modo, los registros de posición carecen de utilidad, y solo se usa el de “Goal Speed”. En este registro se podrá escribir un valor entre 0 y 2047 (0xFFF). Cada unidad es aproximadamente un 0.1% del par máximo que puede ejercer el motor. Si se usa un valor comprendido entre 0 y 1023, girará en sentido antihorario, siendo el valor 0 la parada. Si usamos el rango de valores de 1024 a 2047, girará en sentido horario. Esto se traduce a que el último bit se convierte en el que especifica la dirección de giro. Ejemplo 2-1. Modo Rueda: si se escribe el valor 512 en el registro de “Goal Speed”, el motor girará a un 50% del par máximo. ❖ Modo Servo Motor: para el uso de este modo, usaremos dos registros, “Goal Position” y “Goal Speed”. En esta ocasión, “Goal Speed” solo tomará valores comprendidos entre 0 y 1023. Cada unidad tiene un valor aproximado de 0.111 rpm, por lo que, en este caso, en dicho registro si controla la velocidad (no como en el caso anterior, que puede llevar a confusión). Por otro lado, tenemos el registro de “Goal Position”. Como se puede observar en la figura 2-7, existe una zona muerta de 60 grados, que es inaccesible para el motor (debido a su encoder). Los 300 grados restantes se codifican con un rango de valores de 0 a 1023. Figura 2-7. Explicación Registro “Goal Position” Por tanto, si se usa este modo se deberá especificar la posición de destino deseada, así como a la velocidad a la que irá a dicha posición. Ejemplo 2-2. Modo Servo Motor: Si queremos que el motor se posicione a 150º, se deberá escribir el valor 512 en el registro “Goal Position”. Si queremos que se mueva a 33.3 rpm, se pondrá el valor 300 en el registro “Goal Speed”. La conclusión que se puede sacar acerca de estos actuadores es su versatilidad y la gran cantidad de información que se puede obtener de ellos. Como aspecto negativo, el modo de giro continuo tiene limitaciones, ya que no se puede controlar la velocidad sino el par, lo que genera una problemática si se quiere hacer un control en velocidad. 4 4 Se ha conseguido implementar un control en velocidad haciendo uso del modo Servo Motor. Se expondrá en el capítulo 5 apartado 3. Hardware. 12 12 2.2.3 Piezas de Ensamble de Motores (Robotis). Como se puede ver en el Anexo I, el kit dispone de una gran variedad de piezas que permiten diseñar múltiples eslabones para la conexión física de los motores. Sin embargo, son dos de estas piezas las principales para el diseño. La figura 2-8 es un extracto del datasheet de los AX-12A, donde se hace referencia a las dos piezas de ensamble principales, así como las diferentes formas de unirlas a los motores. Figura 2-8. Piezas de Ensamble Motores 13 2.3 Procesadores. 2.3.1 Arbotix-M (Robocontroller). Es un controlador compatible con Arduino, sompartiendo su lenguaje de programación, librerías y su IDE. Se beneficia de una comunidad de librerias de Código abierto y ejemplos. Desde su lanzamiento en 2010, se ha convertido en el favorito para el control de los actuadores de Dynamixel, siendo usado tanto para la enseñanza como para la investigación de alto nivel. Figura 2-9. Arbotix - M El Arbotix (Figura 3-1) cuenta con: 5 - Microcontrolador AVR 16MHz (ATMEGA644p). - Dos tomas de corriente continua para alimentación. - 3 puertos Dynamixel (TTL). - Regulador de 5 V. Puede trabajar con entradas de entre 7 y 30 V, y pudede suministrar 1.5A a 5V. - Puerto Serie / Programación FTDI. - Puerto de programación ISP. - 20 puertos de E/S digitales (3 pines por entrada: Signal – VCC - Ground), de los cuales 4 están habilitados para PWM. - 8 puertos de entradas analógicas. Se ha decidido usar este controlador por tener un procesador bastante más potente que el de la controladora CM-530, además de una mayor accesibiliadd y versatilidad con los puertos que posee. Sin embargo, la mayor ventaja que dispone es la posibilidad de programarlo a través de la IDE de Arduino, así como haciendo uso de su lenguaje de programación. Esto se debe a que la casa TrossenRobotis ha diseñado unas bibliotecas específicas para este microcontrolador que permite adentrarnos en las entrañas del Dynamixel, proporcionando la posibilidad de explotar todo su potencial. 5 Para más información sobre el Arbotix-M, visitar el siguiente enlace (pagina oficial): http://learn.trossenrobotics.com/arbotix/arbotixgetting-started/38-arbotix-m-hardware-overview#&panel1-1 Hardware. 14 14 2.3.2 RaspBerry Pi 3 B: Para la realización del sistema de visión artificial se ha usado una RaspBerry Pi 3B. Estos dispositivos son ordenadores de bajo costo y gran potencia. Son usadas tanto para investigación como para aprendizaje, teniendo un amplio abanico de posibilidades de programación. Las características de las RaspBerry son las siguientes: • Procesador: o Chipset Broadcom BCM2387. o 1,2 GHz de cuatro núcleos ARM Cortex-A53 • GPU o Dual Core VideoCore IV ® Multimedia Co-procesador. Proporciona Open GL ES 2.0, OpenVG acelerado por hardware, y 1080p30 H.264 de alto perfil de decodificación. o Capaz de 1 Gpixel / s, 1.5Gtexel / s o 24 GFLOPs con el filtrado de texturas y la infraestructura DMA • RAM: 1GB LPDDR2. • Conectividad o Ethernet socket Ethernet 10/100 BaseT o 802.11 b / g / n LAN inalámbrica y Bluetooth 4.1 (Classic Bluetooth y LE) o Salida de vídeo ▪ HDMI rev 1.3 y 1.4 ▪ RCA compuesto (PAL y NTSC) o Salida de audio ▪ jack de 3,5 mm de salida de audio, HDMI ▪ USB 4 x Conector USB 2.0 o Conector GPIO ▪ 40-clavijas de 2,54 mm (100 milésimas de pulgada) de expansión: 2x20 tira ▪ Proporcionar 27 pines GPIO, así como 3,3 V, +5 V y GND líneas de suministro o Conector de la cámara de 15 pines cámara MIPI interfaz en serie (CSI-2) o Pantalla de visualización Conector de la interfaz de serie (DSI) Conector de 15 vías plana flex cable con dos carriles de datos y un carril de reloj o Ranura de tarjeta de memoria Empuje / tire Micro SDIO Figura 2-10. RaspBerry Pi 3B 15 2.4 Impresora 3D. Uno de los objetivos principales del proyecto es el diseño y la fabricación de los dos brazos de robots. Para ello se ha usado el software de diseño Catia, así como una impresora 3D para la impresion de las piezas. La impresora que se ha usado para la fabricación de las piezas es la Geeetech Prusa i3 Pro-W 6 . Es una impresora de bajo coste, la cual hay que montar y equilibrar antes de usarla. Sus características principales son: Figura 2-11. Impresora Geeetech Prusa i2 Pro - W - Tecnología: FFF / FDM - Tamaño de impresión. 200 x 200 x 180 cm - Resolución de capa: 0,01 – 0,03 mm - Precisión: 0,01 – 0,03 mm - Diámetro de la boquilla: 0,03mm - Tipo de filamento: ABS/PLA/Flexible PLA/NYLON/WOOD. - Diámetro del filamento: 1,75 mm - Temperatura cama caliente: 110º max. - Temperatura Extrusor: 240º max. - Dimensiones: 45 x 44 x 45 cm - Compatibilidad: Win/Mac/Linux - Software: descarga desde Geeetech, Easyprint 3D. - Formato de los archivos: STL / G-code - Panel LCD - Fuente de alimentación: entrada 110-220V / salida DC12V/ 15A - Conectividad: USB / Tarjeta SD / Wi-fi (módulo aparte) - Estructura Física: Rep-rap - Cuerpo de madera de 6 mm, color negro. - Plataforma de construcción: Vidrio + Calefacción. - Sistema de control: GT2560 de autodesarrollo, compatible con la función de nivelación automática. - Motores: Angulo 1, 8º / con 1/6 de micro-stepping. 6 Se podrá ver el manual de impresión en el Anexo III. Hardware. 16 16 17 3 SISTEMAS. ontinuaremos con la explicación teórica de nuestro sistema, el cuál estará compuesto por los dos brazos robóticos. El capítulo se comenzará con una presentación de ambos robots. Tras esto, se dispondrá de una información detallada de las principales características de cada uno de ellos expuestas en dos subapartados diferentes. Cada uno de estos subapartados dispondrá de un bloque de diseño y otro de estudio cinemático. Si se desea adquirir más información sobre su construcción o el método usado para la impresión, el lector podrá obtenerla en los anexos de la memoria. 3.1 Introducción. Como se expuso al inicio del documento, los objetivos principales del proyecto giran alrededor del diseño y creación de dos brazos robóticos, los cuales compondrán el sistema. Para ello se creó primero un modelo de robot industrial de 6 grados de libertad, sobre el cuál se han realalizado los experimentos de control. Por tanto, este brazo debía de ser suficientemente robusto para la experimentación. El robot Compliant debe de tener articulaciones pasivas flexibles, pero que fueran capaces de sobortar la experimentación. El resultado del diseño y contrucción de ambos brazos se muestran en las siguientes imágenes: C Figura 3-1. Robot Compliant Figura 3-2. Robot Industrial 6 GDL “Corta tu propia leña y te calentará dos veces” - Henry Ford - Sistemas. 18 18 3.2 Brazo Robótico Industrial 6 Grados De Libertad. Se comenzará con el barzo robótico industrial de seis grados de libertad. En un principio, fue el objetivo principal del proyecto, y en el cuál se invirtió buena parte del tiempo para su diseño, realizándose diferentes modelos, todos ellos haciendo uso únicamente de las piezas del kit. Tras haber probado diferentes prototipos, se comprueba que, si se desea hacer un brazo robótico industrial de seis grados de libertad, las dimensiones que alcanza el robot son demasiado grandes y, por tanto, las articulaciones inferiores sufren debido a la gran cantidad de esfuerzo que deben soportar, impidiendo que realicen su cometido. Este problema se soluciona poniendo dos motores en paralelo en las articulaciones que necesitan un mayor par motor para mover el conjunto, lo que implica rediseñar el robot al completo, y buscar nuevas piezas que permitan ensamblar ambos actuadores en paralelo. Tomada la decisión de buscar piezas extras al kit, se procede al diseño de las piezas y su posterior impresión 3D. Finalizada la impresión de todas las partes del robot, se procede al montaje y estudio matemático, y tras esto, a la decisión del tipo de control que se realizará, sus simulaciones y su posterior programación en el controlador Arbotix-M. 3.2.1 Diseño del Robot. Finalmente, se decide realizar un robot industrial antropomórfico (también llamado angular), que implica que todas las articulaciones son rotativas, condición de diseño evidente debido a la naturalez de los servomotores que se usarán como actuadores. Gracias a los seis grados de libertad que poseerá el robot, se podrá situar la herramienta en el punto del espacio deseado (px, py, pz) y en la orientación adecuado (roll, pich y yaw) adecuados para la manipulacción correcta del objetivo. Para determinar la orientación de las articulaciones se requiere que pueda realizar un desacople cinemático entre el brazo y la muñeca, lo que simplificará el estudio del robot. Para ello, la muñeca deberá ser esférica. A continuación, se expondrán las imágenes realizadas en Catia para la impresión 3D de cada una de las piezas del robot. En las figuras se puede ver marcado en naranja el contorno de la pieza. Todas las piezas están diseñadas sobre una plataforma ovalada o circular, que tiene como cometido mejorar la impresión 3D y evitar probemas de Wraping. Figura 3-3. Esquema Robot Antropomórfico 19 I. Esalbón 0: Base. Es la estructura sobre la cuál se apoyará el eslabón 1. Está compuesta por dos piezas octogonales. La inferior tiene un hueco circular, en el que estará el primer motor. Se une a la segunda parte por 8 tornillos con dos tuercas, que servirán para nivelar ambos módulos y queden a la altura necesaria. La parte superior de la estructura tiene un canal circular, en el cuál se añadiran rodamientos para que gire el eslabón 1. Figura 3-4. Elabón 0. Pieza Inferior Figura 3-5. Eslabón 0. Pieza superior II. Esalbón 1: Plataforma Circular. Unión de las articulaciónes 1ª y 2ª. El eslabón 1 está compuesto de una pieza circular que se posará encima de la plataforma de rodamientos sobre la cuál girará. Es la unión de la primera y la segunda articulación. Cuenta con 8 orificios para atornillar los tres motores que conecta, uno para la primera articulación, y dos de la segunda. Cuenta con un hueco por el que pasará el bus de comunicación de los actuadores. Figura 3-6. Eslabón 1. Cara Superior Figura 3-7. Eslabón 1. Cara Inferior Sistemas. 26 26 Una vez definidos los ejes, construimos la tabla de parámetros de Denavit y Hartenberg. Se ha añadido una columna denominada offset 7 , que se corresponde con un giro en la articulación para que coincida con la posición inicial del robot real. Articulación θ (rad) d (m) a (m) Α (rad) Offset (rad) Q1 𝜃1 L1 0 π/2 π Q2 𝜃2 0 L2 0 π/2 Q3 𝜃3 0 0 π/2 π/2 Q4 𝜃4 L3 0 - π/2 0 Q5 𝜃5 0 0 π/2 0 Q6 𝜃6 L4 0 0 0 Tabla 3-1. Parámetros Denavit y Hatemberg Robot Industrial Para el caso que nos ocupa, las longitudes tienen las siguientes medidas: L1 (m) L2 (m) L3 (m) L4 (m) 0.096 0.170 0.132 0.150 Tabla 3-2. Longitudes del Robot Una vez están definidos todos los parámetros del brazo robótico, se deben crear las matrices de transformación homogéneas, que nos permitirán relacionar cada articulación con la siguiente, y tienen la siguiente estructura: 𝐴𝑖𝑖−1= (cos𝜃𝑖−cos𝛼𝑖 ∙ sin𝜃𝑖 sin𝛼𝑖 ∙ sin𝜃𝑖𝑎𝑖 ∙ cos𝜃𝑖 sin𝜃𝑖 cos𝛼𝑖 ∙ cos𝜃𝑖− sin𝛼𝑖 ∙ cos𝜃𝑖𝑎𝑖 ∙ sin𝜃𝑖 00sin𝛼𝑖 0cos𝛼𝑖 𝑑𝑖 0 1 ) Ecuación 3-1. Matriz de Transformación Homogénea. Para cada una de las filas de la Tabla 3-1, se debe crear una matriz de transformación. Si se multiplican todas las matrices en el orden correcto, como muestra la Ecuación 3-2, se obtendrá la matriz T. 7 Este dato de offset es encesario para ejecutar el archivo de Matlab CinematicaTool, el cuál hace uso del ToolBox de Peter Corke, que ha sido implementado para la simulación del brazo robótico. 27 𝑇=𝐴10 ∙ 𝐴21 ∙ 𝐴32 ∙ 𝐴43 ∙ 𝐴54 ∙ 𝐴65= 𝐴60= (𝑛𝑥𝑜𝑥𝑎𝑥𝑝𝑥 𝑛𝑦𝑜𝑦𝑎𝑦𝑝𝑦 𝑛𝑧 0𝑜𝑧 0𝑎𝑧𝑝𝑧 0 1) 𝑐𝑜𝑛 𝑎=𝑛 × 𝑜 Ecuación 3-2. Matriz de Tranformación T. Como resultado de la matriz T, se tienen los puntos 𝑝𝑥,𝑝𝑦 𝑦 𝑝𝑧 que corresponden con las coordenadas cartesianas del extremo del robot, así como los vectores n, o y a que nos forman un sistema de ejes cartesianos, indicando la orientación. II. Cinemática Inversa. Como se explica al comienzo de este capitulo, se diseñó un robot antropomórfico al cuál se le puede aplicar el procedimiento de desacople cinemático, que será de utilidad en este apartado. Esto significa que se puede tratar de manera independiente los problemas de posicionamiento y de orientación del extremo. Si se considera únicamente los tres primeros grados de libertad, observamos que tienen una estructura planar, es decir, que quedan contenidos en un mismo plano. Gracias a esto, usando métodos geométricos podemos obtener la posición del extremo del robot. Haciendo uso de los tres últimos grados de libertad, es posible averiguar la orientación del extremo sin necesidad de saber en que posición se encuentran el resto de las articulaciones. i. Resolucio n del problema de Posicionamiento del Extremo del Robot por Me todos Geome tricos: Identificado el primer problema, se tienen como datos los puntos cartesianos del extremo del robot, denominados 𝑝𝑥,𝑝𝑦 𝑦 𝑝𝑧. Debido a la naturaleza planar del brazo, queda definido el valor de la primera articulación. 𝑞1= tan−1(𝑝𝑥 𝑝𝑦) Ecuación 3-3. Cinemática Inversa. Articulación 1. Considerando los eslabones 2 y 3, y usando el teorema del coseno, obtendremos las siguientes ecuaciones. 𝑟2=𝑝𝑥2+ 𝑝𝑦2 𝑟2+ 𝑝𝑧2= 𝐿2 2+ 𝐿3 2+ 2𝐿2∙ 𝐿3 ∙cos𝑞3 cos𝑞3= 𝑝𝑥2+ 𝑝𝑦2+ 𝑝𝑧2− 𝐿2 2− 𝐿3 2 2𝐿2 ∙ 𝐿3 Figura 3-32. Esquema Brazo Robot Sistemas. 28 28 Por motivos de eficiencia a la hora de la computación y la programación del algoritmo, es mejor usar la expresión de la arcotangente en lugar del arcoseno, obteniendo la ecuación 3-4. 𝑞3= tan−1(±√1−𝑐𝑜𝑠2𝑞3 2cos𝑞3) 𝑐𝑜𝑛cos𝑞3= 𝑝𝑥2+ 𝑝𝑦2+ 𝑝𝑧2− 𝐿2 2− 𝐿3 2 2𝐿2 ∙ 𝐿3 Ecuación 3-4. Cinemática Inversa. Articulación 3. Llegados al cálculo de la articulación 2, se consideran dos posibles soluciones diferentes, comúnmente conocidas como Codo Abajo y Codo Arriba. Figura 3-33. Codo Abajo y Codo Arriba. El cálculo de la articulación 2 se hace mediante las variables A y B: 𝑞2=𝐵−𝐴 𝑐𝑜𝑛 𝐵=tan−1(𝑝𝑧 𝑟)= tan−1(𝑝𝑧 ±√𝑝𝑥2+𝑝𝑦2) 𝑦 𝐴= 𝑎𝑟𝑐𝑜𝑡𝑎𝑛( 𝐿3∙sin𝑞3 𝐿+ 𝐿3∙cos𝑞3) Sustituyendo, obtenemos la ecuación 3-5 que nos definirá el valor de la articulación 2. 𝑞2= tan−1(𝑝𝑧 ±√𝑝𝑥2+𝑝𝑦2) − 𝑎𝑟𝑐𝑜𝑡𝑎𝑛( 𝐿3∙sin𝑞3 𝐿+ 𝐿3∙cos𝑞3) Ecuación 3-5. Cinemática Inversa. Articulación 2. Una vez obtenidas las ecuaciones 4-3, 4-4 y 4-5, se ha conseguido resolver el problema cinemático inverso para las tres primeras articulaciones que fijan la posición del extremo del robot en el espacio de trabajo. 29 ii. Resolucio n del problema de Orientacio n del Extremo del Robot: Se comienza distinguiendo dos puntos diferentes, el conocido como “centro de la muñeca” (𝑃𝑚), que corresponde con el origen del Sistema de referencia de la articulación 5, y el final del robot, que se corresponde con el origen del Sistema de la articulación 6 (𝑃𝑟), y corresponde con los anteriormente conocidos puntos 𝑝𝑥,𝑝𝑦 𝑦 𝑝𝑧. 𝑝𝑟 =[𝑝𝑥,𝑝𝑦 ,𝑝𝑧]′ Como se observa en la figura, los ejes coordenados en Z de las articulaciones 5 y 6 deben coincidir, y están separadas una distancia igual a 𝐿4, por lo que obtenemos que: 𝑝𝑚= 𝑝𝑟− 𝐿4∙ 𝑧6 𝑐𝑜𝑛 𝑧6=[𝑎𝑥,𝑎𝑦 ,𝑎𝑧]𝑇 Una vez que se obtiene el punto 𝑝𝑚, se debe averiguar los valores de las articulaciones 4, 5 y 6 que determinarán la orientación deseada del efector del robot. Para ello se volverá a usar las matrices de transformación. Partiendo de la matriz 𝑅60 (submatriz de rotación de 𝑇60) se obtiene: 𝑅60=[ 𝑛 𝑜 𝑎] = 𝑅30∙𝑅63 𝑐𝑜𝑛 𝑅30= 𝐴10∙ 𝐴21 ∙ 𝐴32 Ecuación 3-6. Matrices de Rotación Si despejamos la matriz 𝑅63 de la ecuación 3-6, se obtiene una matriz que solo dependerá de los valores específicos de los tres últimos grados de libertad. En la siguiente ecuación se expondrá cuál es el resultado simbólico de realizar dicha ecuación. 𝑅6𝑖,𝑗 3= [cos𝑞4cos𝑞5cos𝑞6−sin𝑞4sin𝑞6−cos𝑞4cos𝑞5sin𝑞6−sin𝑞4cos𝑞6 cos𝑞4sin𝑞5 sin𝑞4cos𝑞5cos𝑞6+cos𝑞4sin𝑞6−sin𝑞4cos𝑞5sin𝑞6+cos𝑞4cos𝑞6−sin𝑞4cos𝑞5 −sin𝑞5cos𝑞6sin𝑞5sin𝑞6cos𝑞5] Ecuación 3-7. Matriz de Rotación Muñeca. De las relaciones expresadas en esta matriz, podemos despejar los valores de 𝑞4,𝑞5 𝑦 𝑞6, obteniendo las siguientes expresiones: 8 𝑞4= arcsin(𝑅62,3 3 𝑅63,3 3) 𝑞5= 𝑎𝑟𝑐𝑜𝑠 (𝑅63,3 3) 𝑞6= 𝑎𝑟𝑐𝑜𝑡𝑎𝑛(−𝑅63,2 3 𝑅63,1 3) Ecuación 3-8. Coordenadas Articulares Muñeca 8 Como en el caso de la cinemática directa para la articulación 3, se realizarán los cambios pertinentes para usar la función arcotangente, debido a su naturaleza más robusta a la hora de la computación. Figura 3-34. Muñeca del Robot. Sistemas. 30 30 3.2.4 Generador de Trayectorias – Jacobiano. Existe la posibilidad de establecer relación entre las deribadas de las posiciones de cada uno de los grados de libertad del robot. Esto permitirá conocer qué velocidad debe establecerse en cada instante de tiempo en cada articulación, para que el extremo del robot consiga llegar a la posición adecuada. Esta relación se obtiene a través de la denominada matriz Jacobiana. La matriz Jacobiana directa permite conocer la velocidad del extremo del robot haciendo uso de las velocidades de cada grado de libertad. Como en el caso de la cinemática, este método tiene su inverso, que proporcionará unas velocidades determinadas para el extremo del robot. Figura 3-35. Esquema Jacobiano. Si se ha resuelto el problema cinemático, es fácil obtener el jacobiano, realizando las derivadas como se puede observar la la ecuación 3-9. [ 𝑥󰇗𝑦󰇗𝑧󰇗𝛼󰇗𝛽󰇗𝛾󰇗 ] =𝐽 ∙ [ 𝑞1󰇗𝑞2󰇗𝑞3󰇗𝑞4󰇗𝑞5󰇗𝑞6󰇗 ] 𝑐𝑜𝑛 𝐽= [ 𝛿𝑓𝑥 𝛿𝑞1⋯𝛿𝑓𝑥 𝛿𝑞6 ⋮ ⋱ ⋮ 𝛿𝑓𝛾 𝛿𝑞1⋯𝛿𝑓𝛾 𝛿𝑞6 ] Ecuación 3-9. Matriz Jacobiana. Debido a que el valor numérico de cada elemento de la matriz será diferente en cada instante según la posición en la que se encuentre, el valor de la jacobiana será diferente para cada uno de los puntos del espacio de trabajo. Existen problemas de singularidades de un robot, y estas surgen cuando el jacobiano se hace nulo, lo que implica que un pequeño incremento de las coordenadas cartesianas provocaría un incremento infinito en las coordenadas articulares. Esto provocaría que los actuadores del robot intentasen dar unas velocidades imposibles de alcanzar por el extremo del robot, así como perjudicial para el sistema robótico al completo. Para evitar estas singularidades, se realizará un proceso matemático que incluirá un termino externo variable, que impedirá que la matriz jacobiana se anule y provoque estos errores, pero se explicará en el siguiente apartado del capitulo. 31 3.3 Brazo de Robot Compliant. Este brazo robótico es muy diferente a los que se acostumbran a ver debido a que no se ha buscado una configuración común en la industria, sin embargo, su propósito es poder probar un nuevo tipo de control que permite incluir articulaciones flexibles. Este tipo de robot está pensado para ir en drones y poder realizar tareas aéreas. Debido a la naturaleza en la que estará funcionando, las amortiguaciones flexibles permitirán ejercer presión de manera controlada, así como en caso de que se produzca un choque entre el brazo de robot y algún animal u objeto que esté en el espacio de trabajo del robot, la amortiguación pueda absorber parte del impacto y, en medida de lo posible, no desestabilice al dron. Estas articulaciones flexibles se consideran grados de libertad, pero no se puede ejercer una actuación sobre ellos, pero si se debe conseguir información a cerca de su posicionamiento. Por este motivo, se ha necesitado realizar un sistema de visión artificial que permita reconocer el ángulo que existe entre las amortiguaciones. Para simplificar el problema, se ha realizado un robot planar, teniendo así una altura de la pinza constante, y sus desplazamientos se limitan a dos dimensiones. Figura 3-36. Robot Compliant 3.3.1 Diseño de Piezas: El diseño de este brazo de robot será simple, ya que todas las piezas serán iguales y se busca únicamente la conexión entre los motores, todos ellos en la misma posición. La dificultad que se encuentra es la poca superficie que tienen los actuadores para conectarse entre ellos, lo que implicó crear una base cuya función únicamente es aumentar la zona de unión y disponer de espacio suficiente para poder hacer un sistema que permita la amortiguación. Una vez se dispuso de espacio suficiente, se pudo realziar la articulación. Como en las piezas anteriores, los diseños están realizados sobre una base en forma de elipse, que servirá para mejorar la calidad de la impresión y evitar problemas de Wrapping. Sistemas. 32 32 I. Base de motores: La base es rectangular, y tiene unas dimensiones de 60 x 40 mm. Dispone de cuatro orificios en el centro, que se usarán para unirlo a las piezas de ensamble con los motores, así como de otros cuatro agujeros colocados en las esquinas, que permitirán unir la base al resto de la articulación. II. Unión flexible: Esta union cuenta con dos partes, cada una correspondiente a uno de los motores. La del primer motor (el que tiene la union en la parte móvil), cuenta con dos semicirculos colocados en la parte superior e inferior, mientras que la pieza de unión del segundo actuador cuenta con una sola circunferencia central. Esto permitirá unir ambas piezas como si de un “sandwich” se tratase, y tras esto colocar un pasador que los una. Para la amortiguación, ambas estructuras cuentan con cuatro agujeros en los cuales irán cuatro muelles que servirán como amortiguadores, así como otros cuatro en agujeros en las esquinas que permiten unirlo con la base. Figura 3-37. Base de Motores Figura 3-38. Pieza Unión Flexible 1 Figura 3-39. Pieza Unión Flexible 2 33 3.3.2 Construcción Brazo Compliant. A continuación, se realizará una breve explicación de como se deben de montar las articulaciones flexibles. En función de la longitud que se desee para el robot, se pueden poner más o menos uniones. En este caso, se ha decidido dejar tres articulaciones que unen cuatro actuadores. 1. El primer paso será unir dos bases a los dos extremos de los robots. Para ello se usará una unión fija al motor y otra móvil. Se cogerán con cuatro tornillos penetrantes para que no choquen con las piezas de unión. 2. Conectar a cada una de las bases la unión correspondiente usando los cuatro agujeros de las esquinas. 9 3. Colocar los cuatro muelles en los orificios de una de las partes. Hacer que coincidan con las de la otra pieza y unir ambas dos y alinear los agujeros de sus circunferencias para poder pasar un tornillo dajarlas fijas. 4. Una vez ensamblada las partes, se une a los motores. Repetir el proceso cuantas veces se necesite. Figura 3-40. Montaje Robot Compliant Paso 1 Figura 3-41. Montaje Robot Compliant Paso 2 Figura 3-42. Montaje Robot Compliant Paso 3 Figura 3-43. Montaje Robot Compliant Paso 4 9 Se han usado en este caso tornillos más grandes de los necesario, pero solo para hacer las fotos de la memoria. Estos tornillos se sustituyen por otros de menor longitud. Sistemas. 34 34 3.3.3 Cinemática del Robot Compliant: Para el estudio de la cinemática de este robot se usará el esquema de la figura 3-44, en la cual se pueden observar los grados de libertad 𝜃 𝜖 ℝ4, que se corresponderán con las articulaciones modificables, es decir, con los motores del robot, así como las articulaciones pasivas, denominadas 𝛿 𝜖 ℝ3. Figura 3-44. Esquema de los Grados de Libertad del Robot Compliant Se puede expresar la posición y orientación del efector mediante la siguiente expresión: 𝑥= 𝐿1 (100)+ 𝐿2 (cos(𝜃1)+cos(𝜃1+ 𝛿1+ 𝜃2)+cos(𝜃1+𝛿1+ 𝜃2+𝛿2+𝜃3) sin(𝜃1)+cos(𝜃1+ 𝛿1+ 𝜃2)+sin(𝜃1+𝛿1+ 𝜃2+𝛿2+𝜃3) 0) + 𝐿3(cos(𝜃1+ 𝛿1)+cos(𝜃1+𝛿1+𝜃2+𝛿2)+cos(𝜃1+𝛿1+𝜃2+𝛿2+𝜃3+𝛿3) sin(𝜃1+ 𝛿1)+cos(𝜃1+𝛿1+𝜃2+𝛿2)+sin(𝜃1+𝛿1+𝜃2+𝛿2+𝜃3+𝛿3) 0) + 𝐿4(cos(𝜃1+𝛿1+𝜃2+𝛿2+𝜃3+𝛿3+ 𝜃4) sin(𝜃1+𝛿1+𝜃2+𝛿2+𝜃3+𝛿3+ 𝜃4) 0)+ ( 00 𝜃1+𝛿1+𝜃2+𝛿2+𝜃3+𝛿3+ 𝜃4) Ecuación 3-10. Cinemática Directa del Robot Compliant Con este cálculo, se puede expresar la velocidad del efector usando el jacobiano como 𝑣= 𝑥󰇗= 𝐽𝜃𝜃󰇗+ 𝐽𝛿𝛿󰇗 Ecuación 3-11. Velocidad Final del Efector Con los jacobianos definidos como 𝐽𝜃 𝜖 ℝ3 𝑥 4=:𝛿𝑥 𝛿𝜃 y 𝐽𝛿 𝜖 ℝ3 𝑥 3=:𝛿𝑥 𝛿𝛿 . 35 3.4 Sistema de Visión. 3.4.1 Base Matemática para la Obtención de los Ángulos. El método empleado para la adquisición de los ángulos se basa en la triangulación. Para ello se unirán los centros de dos motores consecutivos junto con el centro que corresponde a la articulación que los une (Figura 6-2), por lo que se tendrán tres puntos que nos permitirán definir un triángulo. Figura 3-45. Triangulación Motor - Articulación - Motor El ángulo que se necesita para el cálculo del efector es el correspondiente a la articulación flexible. Por tanto, una vez obtenido el triángulo, se calculará la distancia que separa a cada uno de los vertices y así poder usar el teorema del coseno: 𝑐2=𝑎2+𝑏2+2𝑎𝑏∗cos𝛾 Como se disponen de las longitudes de los lados del triángulo, se puede despejar el ángulo 𝛾 y así obtener la medida necesaria. Por tanto, se puede obtener directamente la medida del ángulo necesario, obteniendo la ecuación 3-12. 𝛾=cos−1(𝑎2+𝑏2−𝑐2 2𝑎𝑏 ) Ecuación 3-12. Ángulo Gamma Figura 3-46. Teorema Del Coseno Control y Simulación. 42 42 4.3.2 Simulación del Control: Se dispone de un archivo matlab denominado ControlPIKOvariable.m en el cuál se ha implementado el control que se va a realizar. El experiment concreto cuenta con un número de 500 muestras (cada una correspondiente a 0.1 segundos). El experimento constará de ir de un punto a otro y observar como irán modificando las velocidades y posiciones del robot hasta llegar a su destino, comprobando que el error en regimen permanente sea nulo. Se legará al punto → x = 0.065 (m) y = 0.284 (m) z = 0.132 (m) Con la orientación → alpha = 0 (rad) betta = 0.698 (rad) gamma = 0 (rad) Se partirá con los siguientes valores de coordenadas articulares: 𝑞1=10º 𝑞2=30º 𝑞3=30º 𝑞4=45º 𝑞5=30º 𝑞6=60º Una vez ejecutado el script, se dispone de otro llamado plotstrayectorias.m que al ejecutarlo nos mostrará las graficas con los resultados obtenidos. Podemos observarlas en las siguientes figuras: Figura 4-2. Trayectoria Robot Industrial. 43 I. Gráficas de Posiciones Articulares: Figura 4-3. Trayectoria Articular Se puede observar como cada uno de los grados de libertad consigue llegar a la posición adecuada para que el extremo esté con las condiciones deseadas de posición y orientación. Control y Simulación. 44 44 II. Gráficas de Velocidades Articulares: Figura 4-4. Velocidad Articular En esta ocasión se ven las velocidades que han debido aplicarse a cada una de las articulaciones para cumplir el objetivo. 45 III. Gráficas de Errores: Figura 4-5. Errores Posición y Orientación Se finaliza el experimento con las gráficas de los errores en posición y orientación, los cuales van disminuyendo conforme el control va avanzando y consigue llegar al destino. Vemos como el error es aproximadamente nulo. Control y Simulación. 46 46 47 5 EXPERIMENTACIÓN Y ANÁLISIS DE RESULTADOS. legados a este punto en el que se tienene los dos brazos de robots contruidos y listos para su uso, hay que centrarse en la configuración experimental. Recordamos al lector que el objetivo de este proyecto se centra en poder sentar las bases para poder probar el control del robot compliant. Por tanto, a lo largo de este capítulo, podrá comprobarse como se ha preparado el experimento, así como la programación realizada tanto en la RaspBerry Pi como en el Arbotix – M. 5.1 Introducción. El experimento que se desea realizar trata de hacer que el robot industrial sea capaz de coger un objeto, como puede ser una pelota, de un punto prefijado del espacio de trabajo del robot. Una vez recogido, el brazo lo colocará cerca del robot Compliant. Cuando el robot Compliant reciba el estímulo de que el objeto está dispuesto en su podición, comenzará su movimiento. El robot compliant comenará en una posición incial recogida cerca de la zona interior de la region de manipulabilidad. Cuando se le indique, el robot se desplegará y comenzará su interactuación con el robot industrial. El Compliant tratará de ejercer presión sobre la pelota que estará sujetando el robot industrial, lo que provocará la deflexión de las articulaciones flexibles, lo que implicará que el control tenga que recalcular las actuaciones de los motores para que siga ejerciendo una presión sobre el objetivo. Se puede ver un esquema del experimento en la siguiente figura: Figura 5-1. Experimento L “El paso más importante que puede dar una persona será siempre el siguiente” - Brandom Sanderson - Experimentación y Análisis de Resultados. 48 48 5.2 Configuración de Visión Artificial para Robot Compliant. Como se ha podido observar, uno de los principales “inconvenientes” que posee el robot Compliant es que tiene articulaciones pasivas, en las cuales no tenemos actuadores, por lo que tampoco se tiene forma de recabar de manera directa información de ellas. Surge la necesidad de diseñar un sistema que sea compatible con resto del conjunto y que realice la función de extraer la información necesaria de los ángulos que forman las articulaciones flexibles. Para ello se recurre a la RaspBerry Pi y a OpenCV, que permitirán realizar un programa en Python capaz de realizar dicho cometido. A continuación, se expondrá el procedimiento seguido para cumplir dicho objetivo. 5.2.1 Función de Visión y Método Empleado. La función principal de usar la RaspBerry es poder implementar un sistema de visión artificial que sirva de apoyo al brazo robótico compliant, realizando la medición de los ángulos que forman las articulaciones flexibles. Para ello se usará una webcam que capturará videos en tiempo real y realizará una serie de procesos que permitan obtener los centros de giro tanto de los motores como de las articulaciones los cuales estarán pintados de diferentes colores, lo que permitirá asociar cada color con una de las articulaciones o motores, permitiendo así realizar los cálculos necesarios para la obtención de los ángulos, además de facilitar en gran medida el código que procesará las imágenes de video. En la siguiente figura se observa el robot completo con los centros pintados cada uno de un color. Figura 5-2. Brazo Robot Compliant Centro Número 1 Motor Cyan Centro Número 2 Articulación flexible Morado Centro Número 3 Motor Verde Centro Número 4 Articulación flexible Amarillo Centro Número 5 Motor Rojo Centro Número 6 Articulación flexible Azul Centro Número 6 Motor Crema Tabla 5-1. Tabla de Centros 49 5.2.2 Pseudo código Implementado en la RaspBerry Pi. Se disponen de dos archivos.py que serán los encargados de obtener la medida de los ángulos aplicando el método descrito en el punto anterior. El primer ejecutable denominado Frames creará una imagen de video en la cuál se obtendrá las imágenes capturadas por la webcam, junto con unas barras de desplazamiento que permitirán ajustar los parámetros de la imagen en HSV para poder aislar el color que se desee. En el otro ejecutable se deberán insertar los valores obtenidos de asilar los colores con la imagen anterior. Una vez hecho esto, cuando se ejecture imprimirá por pantalla los datos de los ángulos medidos, así como dos imágenes de video en las cuales se podrán ver los centros y demás datos de interés. o Cabeceras: - Bibliotecas. - Variables usadas. Es necesario ajustar en este punto los márgenes para el reconocimiento de imagen. Para ello hay que ejecutar antes un ejecutable que permitirá realizar este proceso. o Comenzar a grabar la imagen. o Convertirla a HSV. o While (1): - Espera 5 segundos para que se estabilice la imagen. - Filtrar el ruido con operaciones morfológicas. - Encontrar momento central para cada máscara → Área de cada máscara. - Si Área de máscara > NumPixeles → Modificar centro del color adecuado. - Calcular distancias entre los centros adecuados: ▪ Triángulo 1 → Centros 1, 2 y 3 → Calcular Gamma1 ▪ Triángulo 2 → Centros 3, 4 y 5 → Calcular Gamma2 ▪ Triángulo 3 → Centros 5, 6 y 7 → Calcular Gamma3 - Imprimir por pantalla imágenes con los resultados. - Imprimir por puerto serie los resultados o Fin While (1). Experimentación y Análisis de Resultados. 50 50 5.3 Programación Arbotix – M. Como ya se ha comentado con anteriorirdad, se desea realizar un control en velocidad para los brazos de robots, pero los motores no tienen la posibilidad de controlarlos de esta manera, ya que el modo “rueda” o de giro continuo que posee modifica el torque de los motores, no la velocidad. Por consiguiente, para alcanzar el objetivo del control en velocidad se ha tenido que usar el modo servo motor, en el cuál si se puede especificar la velocidad a la que se dese ir en cada grado de libertad, aunque para ello, se ha tenido que calcular previamente una posición de destino. Para conseguir realizar esto, se ha buscado experimentalmente un tiempo adecuado durante el cuál se mantendrá la velocidad constante. En el caso del robot industrial, esta velocidad ha sido 3 milisegundos. De esta manera, sabiendo la posición actual, la velocidad definida y el tiempo durante el que se mantendrá, se puede calculary la velocidad de destino. A continuación se expondrá un pseudocódigo en el que se podrá ver el proceso seguido para implementar el control en el microcontrolador. 10 5.3.1 Pseudo Código implemrntado en Arbotix – M. o Cabeceras: - Bibliotecas. - Variables usadas. o Void Setup: - Reserva de memoria para puerto serie. - Activación puerto serie. - Habilitación de torque de motores. o Void Loop: - Reset Flags. - Leer posiciones. - Calcular posición y orientación actual. - Elige punto de destino. - Cálculo del error 10 En el Anexo del documento se podrá ver el código implementado. 51 - Hacer: ▪ Calcular Jacobiano. ▪ Calcular Dumpping. ▪ Calcular Pseudo Iinversa Jacobiano con Dumping ▪ Calcular velocidad de actuación. ▪ Calcular posición de destino. ▪ Calcular actuación de los motores. ▪ Leer posiciones. ▪ Calcular posición actual. ▪ Calcular error. ▪ Si iteración < NumIteraciones → iteración ++ ▪ Si no → OverFlow (No converge la posición); Imprime por pantalla error. - Mientras que Flag = 0. - Siguiente punto de destino. o Fin: Conclusiones. 58 58 6 CONCLUSIONES. Tras la realizalización de los experimentos y comprobar su funcionamiento, se han llegado a las siguientes concluisiones. o El objetivo general de crear un sistema robótico de pruebas con dos brazos de robots robustos capaces de experimentar nuevos controladores se ha cumplido de manera muy satisfactoria. o Para el objetivo específico de diseñar, construir y programar un brazo robótico industrial de 6 grados de libertad se ha desempeñado de forma exitosa, ya que se ha conseguido un prototipo perfectamente funcional, robusto y al cuál se le han programado el control deseado, pudiendo comprobar con los experimentos su perfecto funcionamiento. o Se ha conseguido realizar el diseño y construcción de un robot flexible con el cuál poder realizar los experimentos deseados para la implementación de nuevos controladores, por lo que el objetivo se ha realizado correctamente. o El sistema de visión ha sido probado y se comprueba que la RaspBerry Pi carece de suficiente potencia computacional para poder servir de apoyo para el robot compliant. Por tanto, si se desea realizar el control de robots flexibles, se deberá sustituir la la RaspBerry Pi por un hardware de mayor potencia. “La verdadera felicidad radica en la finalización del trabajo utilizando tu propio cerebro y tus habilidades” - Soichiro Honda - 59 Anexos: 60 60 7 ANEXOS: 7.1 Anexo I. Listado de components del Kit Bioloid Premium: 61 Figura 7-1. Componentes Kit Bioloid Premium Anexos: 62 62 7.2 Anexo II. Conexión de Arbotix-M para la programación: I. Descargar e Instalar Arduino IDE: Es necesario descargar el software ARDUINO 1.0.6, ya que las bibliotecas están preparadas para este software. - Pagina de descarga: https://www.arduino.cc/en/Main/OldSoftwareReleases II. Instalar los Controladores FTDI.: Esto permitirá usar el cable FTDI-USB usado para la programación y la comunicación serie. - Página de descarga: http://www.ftdichip.com/Drivers/VCP.htm - Guía de instalación de FTDI: http://www.ftdichip.com/Support/Documents/InstallGuides.htm III. Instalar el hardware Arbotix-M y los archivos de la Biblioteca: Necesitaremos descargar las biblitoecas. Será un archivo comprimido con 3 carpetas, (hardware, bibliotecas y ArbotiX Sketches). Será necesario copiar las tres en la carpeta de usuario ‘Arduino’, (no es la ubicación del Arduino IDE). Según su Sistema operative, la carpeta puede estar en: Windows XP → Mis documentos \ Arduino \ Windows Vista o posteriors → Documentos \ Arduino \ Mac / Linux → ~ / Documentos / Arduino / IMPORTANTE: Si ya dispone de estas carpetas, copie el contenido de dentro de cada carpeta en su análoga, sin sobreescribir las ya existentes. - Enlace de descarga de bibliotecas: https://github.com/trossenrobotics/arbotix/archive/master.zip IV. Conectar el Arbotix Robocontroller a su computadora y a los Actuadores. La orientadocion del cable FTDI es muy importante, por lo que se debe tener especial precaución al conectarlo. Se deberá conectar el pin correspondiente al cable negro con el pin de la placa con la marca ‘BLK’. Figura 7-2. Conexión Arbotix con Programador FTDI-USB 63 Figura 7-4. Jumper VIN Por otro lado, se deberá puentear con el “Jumper” la alimentación, conectando el pin marcado con ‘VIN’ y el pin central (que se trata de la referencia), y, por consiguiente, dejando libre el marcado con ‘USB’. Esto permitirá conectar la Fuente de tensión al controlador. Figura 7-3. Conexión Completa Arbotix - M Por último, se debe conectar el bus TTL del primero motor de la cadena al controlador. Una vez terminados los preparativos, puede empezar a programar el Arbotix hacienda uso de la IDE Arduino. Esto se verá en detalle en el capítulo X. Para comenzar con la programación del Arbotix, se deben de seguir los siguientes pasos: 11 11 Manual de inicio rápido: http://learn.trossenrobotics.com/arbotix/7-arbotix-quick-start-guide Anexos: 64 64 7.3 Anexo III. Configuración Software Slic3r para Impresión: A continuación, se desarrollan los pasos seguidos para la impresión de cada una de las piezas, previamente diseñadas en Catia I. Diseño de Piezas: Todas las piezas se han diseñado sobre una base ovalada. Esto se debe ha que se han intentado minimizar los problemas de “Wrapping”, que se acentúan con las esquinas y los tramos rectos. Esta base ovalada suele tener un grosor de entre 0.6 y 1 mm. II. Crear G-CODE: Una vez realizado el diseño de la pieza, hay que usar un Slicer o “rebanador” para crear un código con extensión GCODE. Este software tiene como función generar el código necesario para la impresión de la pieza. En este apartado se tendrá que especificar a la máquina todas las configuraciones que son necesarias para que la estructura cumpla sus cometidos de rigidez y precisión. Seguidamente, se mostrará la configuración elegida para la impresión de nuestras piezas. Para ello se usó el softwre Repetier-Host GEEEtech. - Líneas y Perímetros: Layer height: Con este parámetro se controla la altura de capa. Esta es la distancia que el extrusor subirá con respecto a la base cada vez que acabe una capa. First Layer Height: Este parámetro es igual al anterior con la única diferencia de que afectará solo a la primera capa de impresión. Perimeters (mínimum): Como su nombre indica, este valor permite modificar el número de perímetros o paredes que tendrá la pieza, de esta forma podremos tener piezas con paredes más gruesas o más finas. Spiral vase: Algo muy útil si queremos hacer piezas sin relleno, únicamente con el grosor de un perímetro. Aunque para el caso que nos ocupa es contraproducente. Solid Layers: Hace referencia al número de capas totalmente sólidas que se realizarán en la pieza - Relleno: Fill density: Este parámetro controla directamente la densidad que tendrá nuestra pieza. Fill pattern: Aquí le diremos la forma con la que rellenaremos la pieza. Top/bottom fill pattern: Controla el patrón de relleno de las capas iniciales y finales, este se dejará en rectilíneo para conseguir los mejores acabados. Reducing printing time: Estos dos parámetros permiten realizar rellenos más rápidos pudiendo alternar entre capas sólidas y huecas, algo que no es recomendable, mantendremos este valor en 1 y desactivaremos la función “Only infill where needed” (Solo rellenar cuando sea necesario). Advanced: Encontraremos parámetros más avanzados Figura 7-5. Configuración Slicer. Líneas y Perímetros Figura 7-6. Configuración Slicer. Relleno 65 como “Solid Infill Every” que nos permite crear una capa totalmente sólida cada cierto número de capas. “Fill Angle” es el ángulo con el que se realizará el relleno, dejaremos un valor de 45°. El resto de los parámetros se dejan tal y como vienen predefinidos. - Velocidades: Perimeters: Las tres primeras casillas hacen referencia a la velocidad con la que se imprimirán los perímetros. Small perimeters: Permite ajustar la velocidad de los perímetros pequeños donde necesitaremos una menor velocidad. External perimeters: Decide la velocidad del perímetro exterior de nuestra pieza, el que veremos, es conveniente que lo realicemos con una menor velocidad a la general. Infill: Aquí controlaremos la velocidad de impresión del relleno de la pieza. Solid infill: Hace referencia a la velocidad de impresión de las capas con un relleno del 100%, excluyendo a la primera y última capa. Top solid infill: Permite ajustar la velocidad de la última capa de impresión Support material / Support material interface: Velocidad con la que se imprimirán los soportes de la pieza en el caso de los hayamos activado. Bridges: Esta es la velocidad con la que se realizarán los puentes, es decir, cuando el extrusor tenga que estirar el filamento de un punto a otro en el aire sin ningún soporte debajo. Al no tener ningún punto de soporte el filamento fundido colgará y se deformará. Travel: Este parámetro controla la velocidad con la que se mueve el extrusor de un punto a otro cuando no tiene que extruir plástico, por lo tanto, puede subirse. First layer speed: Permite decidir la velocidad con la que imprimiremos la primera capa, es la que estará en contacto con la superficie de impresión y deberemos hacerla despacio para que la pieza se enganche bien y no tengamos ningún problema con plásticos como ABS que tiende a deformarse. - Soportes: Generate support material: Esta casilla es la que se activa para que directamente Slic3r genere soportes en nuestras piezas, este proceso lo hará de forma automática a partir de los valores se configuren. Overhang threshold: Permite marcar en grados la inclinación máxima que la impresora puede imprimir sin realizar soportes. Enforce support for the first: Este parámetro nos permite forzar la creación de soportes en las primeras capas independientemente de la inclinación que tengan. Raft layers : El raft es una cama sobre la que se imprimirá la pieza, actúa como soporte, el valor que pongamos configurará el número de capas que generará de este tipo. Normalmente se utiliza en piezas que tengan dificultades para adherirse a la superficie de impresión o bien piezas donde la base no es plana. En este caso, se diseñaron las piezas con este soporte lo suficientemente grande como para luego poder juntar el ABS Juice. Patter: Aquí se define el patrón con el que se generarán los soportes. Normalmente se utiliza el patrón rectilíneo ya que es más fácil de retirar. Pattern spacing: Define la separación del patrón de soporte. Pattern angle: Es el ángulo con el que se imprimirá el soporte. Figura 7-7. Configuración Slicer. Velocidades Figura 7-8. Configración Slicer. Soportes Anexos: 66 66 - Filamento: Diameter: Se usa este parámetro para configurar el diámetro de nuestro filamento. Extrusion multiplier: Es la relación entre el engranaje pequeño y el engranaje grande del extrusor. Recomendamos no modificar este valor ya que este parámetro lo define Slic3r a través del firmware de la impresora. Dejaremos como valor 1. Temperature Extruder: Temperatura del extrusor. Se puede poner temperaturas diferentes para la primera y las siguientes. Temperature Bed: La temperatura de la base caliente también varía en función del material a imprimir. III. Calibración de Impresora: Una vez tenemos el archivo GCODE, se debe configurar la impresora para la realización de la pieza. Lo primero que se debe hacer es ejecutar el comando de Auto-Home. Tras esto, hay que calibrar la cama con el extrusor. Para ello, se irá deslizando hasta ponerlo en las cuatro esquinas de la cama y el centro. Se deberá pasar un pedazo de papel entre el extrusor y la cama, notándose una resistencia (no debe de entrar de manera holgada). Cuando se note en las cuatro esquinas y el centro aproximadamente la misma resistencia al pasar el papel, estará bien equlibrada y a la altura correcta. Una vez calibrada, se le echa laca de fijación en abundancia en el cristal térmico. IV. Impresión: Se vuelve a hacer el Auto-Home, se introduce la tarjeta SD con el archivo GCODE y se ejecuta la intrucción para comenzar a imprimir. Primero se calentará la cama y una vez llegue a la temperatura adecuada, lo hará el extrusor. Cuando todas las temperaturas sean las adecuadas comenzará automáticamente la impresión. V. Aplicación de ABS Juice: Este paso es de vital importancia para evitar el “Wraping 12 ”. Una vez la pieza haya imprimido el primer milímetro de la pieza, habrá terminado de hacer la base. Será el momento de pausar la impresión. Cuando haya parado, se aplicará el ABS Juice, una mezcla de ABS (40%) y acetona (60%). Esto creará una sustancia pegajosa que se extenderá en el contorno de la base de la pieza, haciendo que se pegue al cristal. Una vez echada, reanudar la impresión. VI. Despegue de Pieza y Acabado Final: Cuando la impresión termine, dejar enfriar la pieza. Cuando lo haya hecho, con un cuter despegar la pieza del cristal y recortar el sobrante de la base. Se lijan los contornos para conseguir un buen acabado. 12 Wrapping: El ABS se imprime a unos 260 ºC. Al enfriarse rapidamenteal ser extruido, este se deforma y se contrao un poco. Cuando se realizan objetos con grandes superficies, los filamentos intermedios tienden a tirar de las esquinas, provocando que la pieza se despegue del cristal templado. Esto hace que la pieza se deforme y pierda su funcionalidad. Es uno de los principales problemas y requirió de bastante tiempo el poder solucionarlo. Figura 7-10. Aplicación ABS Juice Figura 7-9. Configuración Slicer. Filamento 67 7.4 Anexo IV. Manual de Preparación de RaspBerry Pi. Para poder implementar el procedimiento de visión artificial, primero hay que configurar el software de la RaspBerry. Para ello habrá que instalar el sistema operativo. Una vez realizado, se deberá realizar una conexión remota por SSH. Para concluir la configuración, se debe instalar las librerías Open CV que permite realizar el procesamiento de imagen. 7.4.1 Instalación de Sistema Operativo Raspbian Stretch. I. Instalación de Software: Para la configuración de la Raspberry necesitaremos los siguientes programas software instalados en el PC: - Un programa para formatear tarjetas SD (SD Card Formater). - Software para montar imágenes en tarjetas SD (Etcher). - Cliente SSH para conectar a servidores remotos y ejecutar comandos (Putty). - Cliente VCN para conectar a servidores remotos y tener visión de la pantalla (VCN Viewer). - Software para escanear direcciones IP (Wirelles Netwoek Watcher). II. Formateo de tarjeta SD: a. Insertar la tarjeta microSD en el ordenador. b. Hacer clic en el icono del programa SD Card Formate. c. Saldrá una pantalla como la que tenemos en la imagen. Se debe seleccionar el puerto en el cual tenemos insertado la tarjeta y clicamos en Format. d. Aparecerá un cuadro de diálogo en el cual pregunta si estamos seguros de que se desea realizar el formateo de la tarjeta. Se deberá clicar en Si. e. Comenzará el formateo de la tarjeta. Una vez terminado, aparecerá una nueva ventana indicando el resumen del proceso. Para finalizar, pulsar Aceptar. III. Descargar y montar Sistema operativo: a. Descargar el sistema operativo RASPBIAN STRETCH CON ESCRITORIO. Para ello solo tenemos que ir al siguiente enlace y descargarlo, haciendo clic en Descargar ZIP. b. ENLACE: https://www.raspberrypi.org/downloads/raspbian/ c. Hacer clic en el icono de Etcher. Aparecerá una ventana como la que se observa en la Imagen. d. Seleccionar el archivo raspbian-stretch y pulsar en Abrir. e. Para comenzar el montaje de la imagen, hacer clic en Flash! f. Una vez concluido, pulsar en Flash Another para terminar. IV. Crear un archivo sin extensión llamado SSH: Tras la instalación del sistema operativo, habrá que ir a la root donde se ha instalado el sistema operativo en la tarjeta SD. Una vez dentro, se deberá crear un archivo sin extensión y que no contenga ninguna información, que se llamará SSH. Para ello se debe abrir el bloc de notas y guardar un archivo en blanco en el directorio anteriormente indicado. Una vez hecho esto, habrá que dirigirse al archivo creado, hacer clic con el botón derecho y renombrar el archivo, borrando la extensión del final del nombre y pulsar aceptar. Aparecerá una ventana que dirá si estamos seguro de querer quitar la extensión al archivo, la cual se debe volver a aceptar. Una vez realizado esto, extraer con seguridad la tarjeta SD del ordenador. Anexos: 74 74 int velmax[6][1] = { { 300 },{ 800 },{ 800 },{ 500 },{ 500 },{ 500 } }; // Velocidad de saturacion de los motores. //----------------------------------------------------- VARIABLES EN FORMA DE MATRIZ PARA JACOBIANO :--------------------------------------- -----------------------// // Matriz Jacobiana. float J[6][6] = { { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 } }; // Jacobiana traspuesta. float Jt[6][6] = { { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 } }; // Jacobiana por Jacobiana traspuesta float JJ[6][6] = { { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 } }; // Matriz identidad con parametro K float IK[6][6] = { { 0.01,0,0,0,0,0 }, { 0,0.01,0,0,0,0 }, { 0,0,0.01,0,0,0 }, { 0,0,0,0.01,0,0 }, { 0,0,0,0,0.01,0 }, { 0,0,0,0,0,0.01 } }; // Matriz JJ multiplicada por matriz IK float JJJ[6][6] = { { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 } }; // Inversa de la matris JJ float JJinv[6][6] = { { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 } }; // Jacobiano Cruz Final. float Jc[6][6] = { { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, 75 { 0,0,0,0,0,0 } }; float Jcn[6][6] = { { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 }, { 0,0,0,0,0,0 } }; // the setup function runs once when you press reset or power the board void setup() { pinMode(0, OUTPUT); //INICIALIZACION DE VARIABLES: cadena.reserve(200); cadenaleida.reserve(200); esperaleer = 0; // HABILITANDO COMUNICACION SERIE: // Serial.begin(9600); delay(1000); Serial.println("###########################"); Serial.println("COMUNICACION SERIE HABILITADA."); Serial.println("###########################"); Serial.println("###########################"); Serial.println("Activamos el torque de todos los motores"); Serial.println("###########################"); dxlTorqueOnAll(); // Funcion para activar todos los torques delay(1000); } // the loop function runs over and over again until power down or reset void loop() { FLAG = 0; posicion=0; orientacion=0; iteracion=0; LeePosiciones(); CalculaInversa(); EligePosicion(); CalculaInversa(); CalculaError(); do { CalculaJacobiano(); CalculaIK(); CalculaJC(); CalculaVelDestino(); CalculaPosDestino(); CalculaActuacionMotores(); Actua(); LeePosiciones(); CalculaInversa(); CalculaError(); if (iteracion<300) {iteracion++;} else { FLAG=1; Anexos: 76 76 Serial.println("OverFLow"); } } while (FLAG == 0); if (punto<4) { punto++; cont_total++; delay(500); } else if (cont_total<40){punto=1;} else { punto=0; cont_total=0; } } // FUNCION PARA LEER POSICIONES DE LOS MOTORES. void LeePosiciones() { q1m = dxlGetPosition(2); if (q1m<1) { q1m = dxlGetPosition(2); } q1 = (q1m - 512)*0.0051; q2m = dxlGetPosition(4); if (q2m<1) { q2m = dxlGetPosition(4); } q22m = dxlGetPosition(6); if (q22m<1) { q22m = dxlGetPosition(6); } q2 = (q2m - 512)*0.0051; q3m = dxlGetPosition(8); if (q3m<1) { q3m = dxlGetPosition(8); } q33m = dxlGetPosition(10); if (q33m<1) { q33m = dxlGetPosition(10); } q3 = (q3m - 512)*0.0051; q4m = dxlGetPosition(12); if (q4m<1) { q4m = dxlGetPosition(12); } q4 = (q4m - 512)*0.0051; q5m = dxlGetPosition(14); if (q5m<1) 77 { q5m = dxlGetPosition(14); } q5 = -(q5m - 512)*0.0051; q6m = dxlGetPosition(18); if (q6m<1) { q6m = dxlGetPosition(18); } q6 = (q6m - 512)*0.0051; } // CALCULO DE X, Y, Z, A, B y G ACTUAL E IMPRIME - void CalculaInversa() { xact = 0.085*sin(q1 + q2) - 0.085*sin(q1 - 1.0*q2) + 0.066*cos(q1 - 1.0*q2)*sin(q3) - 0.066*sin(q1 - 1.0*q2)*cos(q3) + 0.075*sin(q1 + q2 + q3)*cos(q5) + 0.075*sin(q2 - 1.0*q1 + q3)*cos(q5) + 0.066*cos(q1 + q2)*sin(q3) + 0.066*sin(q1 + q2)*cos(q3) - 0.15*sin(q1)*sin(q4)*sin(q5) + 0.075*cos(q1 + q2 + q3)*cos(q4)*sin(q5) + 0.075*cos(q2 - 1.0*q1 + q3)*cos(q4)*sin(q5); yact = 0.085*cos(q1 - 1.0*q2) - 0.085*cos(q1 + q2) + 0.066*sin(q1 - 1.0*q2)*sin(q3) - 0.075*cos(q1 + q2 + q3)*cos(q5) + 0.075*cos(q2 - 1.0*q1 + q3)*cos(q5) - 0.066*cos(q1 + q2)*cos(q3) + 0.066*cos(q1 - 1.0*q2)*cos(q3) + 0.066*sin(q1 + q2)*sin(q3) + 0.075*sin(q1 + q2 + q3)*cos(q4)*sin(q5) - 0.075*sin(q2 - 1.0*q1 + q3)*cos(q4)*sin(q5) + 0.15*cos(q1)*sin(q4)*sin(q5); zact = 0.13*cos(q2 + q3) + 0.17*cos(q2) + cos(q5)*(0.15*cos(q2 + q3)) - 1.0*sin(q5)*(+0.15*sin(q2 + q3)*cos(q4)) + 0.096; aact = atan((sin(q4)*sin(q5)) / (cos(q4)*sin(q5))); bact = atan(sqrt((sin(q4)*sin(q5))*(sin(q4)*sin(q5)) + (cos(q4)*sin(q5)) * (cos(q4)*sin(q5))) / (cos(q5))); gact = atan((1.0*sin(q5)*sin(q6)) / (1.0*cos(q6)*sin(q5))); } // ELECCION DE POSICIONES DE DESTINO (PUESTO SERIE) void EligePosicion() { if (punto==0) { Xdes= 0.1702; Ydes= 0.0183; Zdes= 0.3948; Ades= -0.0612; Bdes= 0.7293; Gdes= -1.4841; Serial.println("###########################"); Serial.println("- Introduce algo para continuar: "); Serial.println("---------------------------"); Leer(); Continuar = cadenaleida.toInt(); cadenaleida = ""; esperaleer = 0; Serial.print("Has pulsado continuar: "); Serial.println(Continuar); Serial.println(""); } if (punto==1) Anexos: 78 78 { Xdes= 0.1702; Ydes= 0.0183; Zdes= 0.3948; Ades= -0.0612; Bdes= 0.7293; Gdes= -1.4841; } else if (punto==2) { Xdes= 0.2729; Ydes= 0.0787; Zdes= 0.2367; Ades= 0.4998; Bdes= 0.6987; Gdes= -0.0306; } else if (punto==3) { Xdes= 0.2505; Ydes= 0.0744; Zdes= 0.0948; Ades= -0.0612; Bdes= 0.7293; Gdes= -1.4841; } } // CALCULO DE ERRORES. void CalculaError() { error[0][0] = (Xdes - xact)/(1+(20*((Xdes - xact)*(Xdes - xact)))); error[1][0] = (Ydes - yact)/(1+(20*((Ydes - yact)*(Ydes - yact)))); error[2][0] = (Zdes - zact)/(1+(100*((Zdes - zact)*(Zdes - zact)))); error[3][0] = (Ades - aact)/(1+(10*((Ades - aact)*(Ades - aact)))); error[4][0] = (Bdes - bact)/(1+(10*((Bdes - bact)*(Bdes - bact)))); error[5][0] = (Gdes - gact)/(1+(10*((Gdes - gact)*(Gdes - gact)))); errorabs[0][0] = abs(Xdes - xact); errorabs[1][0] = abs(Ydes - yact); errorabs[2][0] = abs(Zdes - zact); errorabs[3][0] = abs(Ades - aact); errorabs[4][0] = abs(Bdes - bact); errorabs[5][0] = abs(Gdes - gact); errorkp[0][0] = error[0][0] * kpx; errorkp[1][0] = error[1][0] * kpy; errorkp[2][0] = error[2][0] * kpz; errorkp[3][0] = error[3][0] * kpa; errorkp[4][0] = error[4][0] * kpb; errorkp[5][0] = error[5][0] * kpg; Sxant = Sx; Sx = Sxant + (error[0][0] * t); if (Sx > sat) { Sx = sat; } if (Sx < -sat) { Sx = -sat; } Syant = Sy; Sy = Syant + (error[1][0] * t); if (Sy > sat) { Sy = sat; } if (Sy < -sat) { Sy = -sat; } Szant = Sz; 79 Sz = Szant + (error[2][0] * t); if (Sz > sat) { Sz = sat; } if (Sz < -sat) { Sz = -sat; } Saant = Sa; Sa = Saant + (error[3][0] * t); if (Sa > sat) { Sa = sat; } if (Sa < -sat) { Sa = -sat; } Sbant = Sb; Sb = Sbant + (error[4][0] * t); if (Sb > sat) { Sb = sat; } if (Sb < -sat) { Sb = -sat; } Sgant = Sg; Sg = Sgant + (error[5][0] * t); if (Sg > sat) { Sg = sat; } if (Sg < -sat) { Sg = -sat; } errorki[0][0] = Sx * kix; errorki[1][0] = Sy * kiy; errorki[2][0] = Sz * kiz; errorki[3][0] = Sa * kia; errorki[4][0] = Sb * kib; errorki[5][0] = Sg * kig; errorkpi[0][0]=errorkp[0][0]+errorki[5][0]; errorkpi[1][0]=errorkp[1][0]+errorki[5][0]; errorkpi[2][0]=errorkp[2][0]+errorki[5][0]; errorkpi[3][0]=errorkp[3][0]+errorki[5][0]; errorkpi[4][0]=errorkp[4][0]+errorki[5][0]; errorkpi[5][0]=errorkp[5][0]+errorki[5][0]; errorposicion = ((errorabs[0][0] + errorabs[1][0] + errorabs[2][0]) / 3); errororientacion = ((errorabs[3][0] + errorabs[4][0] + errorabs[5][0]) / 3); if (errorposicion<0.01) { posicion = 1; } else { posicion = 0; } if (errororientacion<0.1) { orientacion = 1; } else { orientacion = 0; } if ((posicion==1) && (orientacion==1)) { FLAG=1; Serial.println("Goal achieved"); } else { FLAG=0;} } // CALCULA JACOBIANO . void CalculaJacobiano() { J[0][0] = 0.15*cos(q4)*sin(q1)*sin(q2)*sin(q3)*sin(q5) - 0.13*cos(q2)*sin(q1)*sin(q3) - 0.13*cos(q3)*sin(q1)*sin(q2) - 0.15*cos(q1)*sin(q4)*sin(q5) - 0.15*cos(q2)*cos(q5)*sin(q1)*sin(q3) - 0.15*cos(q3)*cos(q5)*sin(q1)*sin(q2) - 0.15*cos(q2)*cos(q3)*cos(q4)*sin(q1)*sin(q5) - 0.17*sin(q1)*sin(q2); J[0][1] = -0.002*cos(q1)*(66.0*sin(q2)*sin(q3) - 66.0*cos(q2)*cos(q3) - 85.0*cos(q2) - 75.0*cos(q2)*cos(q3)*cos(q5) + 75.0*cos(q5)*sin(q2)*sin(q3) + 75.0*cos(q2)*cos(q4)*sin(q3)*sin(q5) + 75.0*cos(q3)*cos(q4)*sin(q2)*sin(q5)); J[0][2] = -0.006*cos(q1)*(22.0*sin(q2)*sin(q3) - 22.0*cos(q2)*cos(q3) - 25.0*cos(q2)*cos(q3)*cos(q5) + 25.0*cos(q5)*sin(q2)*sin(q3) + 25.0*cos(q2)*cos(q4)*sin(q3)*sin(q5) + 25.0*cos(q3)*cos(q4)*sin(q2)*sin(q5)); J[0][3] = -0.15*sin(q5)*(cos(q4)*sin(q1) + cos(q1)*cos(q2)*cos(q3)*sin(q4) - 1.0*cos(q1)*sin(q2)*sin(q3)*sin(q4)); Anexos: 80 80 J[0][4] = 0.15*cos(q1)*cos(q2)*cos(q3)*cos(q4)*cos(q5) - 0.15*cos(q1)*cos(q2)*sin(q3)*sin(q5) - 0.15*cos(q1)*cos(q3)*sin(q2)*sin(q5) - 0.15*cos(q5)*sin(q1)*sin(q4) - 0.15*cos(q1)*cos(q4)*cos(q5)*sin(q2)*sin(q3); J[0][5] = 0; J[1][0] = 0.17*cos(q1)*sin(q2) - 0.15*sin(q1)*sin(q4)*sin(q5) + 0.13*cos(q1)*cos(q2)*sin(q3) + 0.13*cos(q1)*cos(q3)*sin(q2) + 0.15*cos(q1)*cos(q2)*cos(q5)*sin(q3) + 0.15*cos(q1)*cos(q3)*cos(q5)*sin(q2) + 0.15*cos(q1)*cos(q2)*cos(q3)*cos(q4)*sin(q5) - 0.15*cos(q1)*cos(q4)*sin(q2)*sin(q3)*sin(q5); J[1][1] = -0.002*sin(q1)*(66.0*sin(q2)*sin(q3) - 66.0*cos(q2)*cos(q3) - 85.0*cos(q2) - 75.0*cos(q2)*cos(q3)*cos(q5) + 75.0*cos(q5)*sin(q2)*sin(q3) + 75.0*cos(q2)*cos(q4)*sin(q3)*sin(q5) + 75.0*cos(q3)*cos(q4)*sin(q2)*sin(q5)); J[1][2] = -0.006*sin(q1)*(22.0*sin(q2)*sin(q3) - 22.0*cos(q2)*cos(q3) - 25.0*cos(q2)*cos(q3)*cos(q5) + 25.0*cos(q5)*sin(q2)*sin(q3) + 25.0*cos(q2)*cos(q4)*sin(q3)*sin(q5) + 25.0*cos(q3)*cos(q4)*sin(q2)*sin(q5)); J[1][3] = 0.15*sin(q5)*(cos(q1)*cos(q4) - 1.0*cos(q2)*cos(q3)*sin(q1)*sin(q4) + sin(q1)*sin(q2)*sin(q3)*sin(q4)); J[1][4] = 0.15*cos(q1)*cos(q5)*sin(q4) - 0.15*cos(q2)*sin(q1)*sin(q3)*sin(q5) - 0.15*cos(q3)*sin(q1)*sin(q2)*sin(q5) + 0.15*cos(q2)*cos(q3)*cos(q4)*cos(q5)*sin(q1) - 0.15*cos(q4)*cos(q5)*sin(q1)*sin(q2)*sin(q3); J[1][5] = 0; J[2][0] = 0; J[2][1] = -0.13*sin(q2 + q3) - 0.17*sin(q2) - 0.15*sin(q2 + q3)*cos(q5) - 0.15*cos(q2 + q3)*cos(q4)*sin(q5); J[2][2] = -0.13*sin(q2 + q3) - 0.15*sin(q2 + q3)*cos(q5) - 0.15*cos(q2 + q3)*cos(q4)*sin(q5); J[2][3] = 0.15*sin(q2 + q3)*sin(q4)*sin(q5); J[2][4] = -0.15*cos(q2 + q3)*sin(q5) - 0.15*sin(q2 + q3)*cos(q4)*cos(q5); J[2][5] = 0; J[3][0] = 0; J[3][1] = 0; J[3][2] = 0; J[3][3] = 1; J[3][4] = 0; J[3][5] = 0; J[4][0] = 0; J[4][1] = 0; J[4][2] = 0; J[4][3] = 0; J[4][4] = sin(q5) / sqrt(sin(q5)*sin(q5)); J[4][5] = 0; J[5][0] = 0; J[5][1] = 0; J[5][2] = 0; J[5][3] = 0; J[5][4] = 0; J[5][5] = 1; } // CALCULA JACOBIANO CRUZ. void CalculaJC() { ////////////////////////// CALCULO DE LA MATRIZ TRASPUESTA ///////////////////////// for (i = 0; i<6; i++) { for (j = 0; j<6; j++) { Jt[j][i] = J[i][j]; } } 81 ///////////////////////////// CALCULO DE LA MATRIZ J*J' //////////////////////////// Matrix.Multiply((float*)J, (float*)Jt, 6, 6, 6, (float*)JJ); ////////////////////////// CALCULO DE LA MATRIZ +I*Kp ////////////////////////////// Matrix.Add((float *)JJ, (float *)IK, 6, 6, (float *)JJJ); ////////////////////////// CALCULO DE LA MATRIZ INVERSA ////////////////////////////// // Primero es necesario copiarla para no reemplazarla for (i = 0; i<6; i++) { for (j = 0; j<6; j++) { JJinv[i][j] = JJJ[i][j]; } } Matrix.Invert((float*)JJinv, 6); ////////////////////////// CALCULO DE LA MATRIZ JCruz //////////////////////////////// Matrix.Multiply((float*)Jt, (float*)JJinv, 6, 6, 6, (float*)Jc); ///////////////////////////// CAMBIAMOS EL SIGNO A J cruz /////////////////////////// for (i = 0; i < 6; i++) { for (j = 0; j < 6; j++) { Jcn[i][j] = (-1)*Jc[i][j]; } } } // MULTIPLICA JC POR ERRORKP Y CALCULA VEL EN RAD/S, RPM Y COORD MOTOR. void CalculaVelDestino() { ///////////////////////////// CALCULO DE LAs VELOCIDADES //////////////////////////// Matrix.Multiply((float*)Jc, (float*)errorkpi, 6, 6, 1, (float*)qd); for (i = 0; i<6; i++) { if (qd[i][0] >= 0) { qds[i][0] = 1; } // Signo positivo. if (qd[i][0]<0) { qds[i][0] = -1; } // Signo negativo. } //////////////////////// CALCULO DE LAs VELOCIDADES EN RPM ////////////////////////// for (i = 0; i < 6; i++) { qdr[i][0] = abs(qd[i][0] * 9.5493); } Anexos: 82 82 ///////////////////// CALCULO DE LAs VELOCIDADES EN MOTORES ///////////////////////////// for (i = 0; i < 6; i++) { qdm[i][0] = qdr[i][0] * 9; if (qdm[i][0] >= velmax[i][0]) { qdm[i][0] = velmax[i][0]; } // Podemos poner una variable de saturacion mejor. } /////////////////// CALCULO DE LAs VELOCIDADES EN RPM FINALES ////////////////////////// for (i = 0; i < 6; i++) { qdr[i][0] = qdm[i][0] * 0.1111; } ////////////////// CALCULO DE LAs VELOCIDADES FINALES EN RAD/S /////////////////////// for (i = 0; i < 6; i++) { qdf[i][0] = qdr[i][0] * 0.1047; } } // CON LA VELOCIDAD ANTERIOR CALCULA LA POS DE DESTINO. void CalculaPosDestino() { ///////////////////////// CALCULO DE LAS POSICIONES DESTINO ///////////////////////// incq1 = qds[0][0] * qdf[0][0] * t; incq2 = qds[1][0] * qdf[1][0] * t; incq3 = qds[2][0] * qdf[2][0] * t; incq4 = qds[3][0] * qdf[3][0] * t; incq5 = qds[4][0] * qdf[4][0] * t; incq6 = qds[5][0] * qdf[5][0] * t; q1des = q1 + (2* incq1); q2des = q2 + (2* incq2); q3des = q3 + (2* incq3); q4des = q4 + (2* incq4); q5des = q5 + (2* incq5); q6des = q6 + (2* incq6); q1mdes = q1des * 196.0784 + 512; q2mdes = q2des * 196.0784 + 512; q3mdes = q3des * 196.0784 + 512; q4mdes = q4des * 196.0784 + 512; q5mdes = ((-1)*q5des*196.0784) + 512; q6mdes = q6des * 196.0784 + 512; } // FUNCION PARA COLOCAR LOS VALORES CORRECTOS EN LOS VECTORES QUE SE USAN PARA ACTUAL EN LOS MOTORES. void CalculaActuacionMotores() { ////////////////////////////////////////////////////////////////////////////////////// //////////// ///////////////////////////// ACTUACION SOBRE LOS MOTORES /////////////////////////// ////////////////////////////////////////////////////////////////////////////////////// //////////// 83 //------------ ARTICULACION 1 ( 1 MOTOR ) --------------------------// vectpos[0][1] = q1mdes; vectvel[0][1] = qdm[0][0] //------------ ARTICULACION 2 ( 2 MOTORES ) ----------------------------// vectvel[1][1] = qdm[1][0]; vectvel[2][1] = qdm[1][0]; if (q2mdes<q2m) { vectpos[1][1] = q2mdes; vectpos[2][1] = q22m + (q2m - q2mdes); } if (q2mdes>q2m) { vectpos[1][1] = q2mdes; vectpos[2][1] = q22m - (q2mdes - q2m); } //------------ ARTICULACION 3 ( 2 MOTORES ) -------------------------// vectvel[3][1] = qdm[2][0]; vectvel[4][1] = qdm[2][0]; if (q3mdes<q3m) { vectpos[3][1] = q3mdes; vectpos[4][1] = q33m + (q3m - q3mdes); } if (q3mdes>q3m) { vectpos[3][1] = q3mdes; vectpos[4][1] = q33m - (q3mdes - q3m); } //------------ ARTICULACION 4 ( 1 MOTOR ) ---------------------------// vectpos[5][1] = q4mdes; vectvel[5][1] = qdm[3][0]; //------------ ARTICULACION 5 ( 1 MOTOR ) --------------------------// vectpos[6][1] = q5mdes; vectvel[6][1] = qdm[4][0]; //------------ ARTICULACION 6 ( 1 MOTOR ) ---------------------------// vectpos[7][1] = q6mdes; vectvel[7][1] = qdm[5][0]; } // FUNCION QUE MANDA LA SEÑAL A LOS MOTORES. void Actua() { ////////////////////////// VELOCIDAD DESEADA A LOS MOTORES /////////////////////////// dxlSyncWrite(vectvel, 8, AX_GOAL_SPEED_L, 2); Anexos: 90 90 y2= int( (y2a+y2)/2 ) # Ajustamos centro para color 3 if ( abs(x3a-x3)>5 ): x3=x3a else: x3= int( (x3a+x3)/2 ) if ( abs(y3a-y3)>5 ): y3=y3a else: y3= int( (y3a+y3)/2 ) # Ajustamos centro para color 4 if ( abs(x4a-x4)>5 ): x4=x4a else: x4= int( (x4a+x4)/2 ) if ( abs(y4a-y4)>5 ): y4=y4a else: y4= int( (y4a+y4)/2 ) # Ajustamos centro para color 5 if ( abs(x5a-x5)>5 ): x5=x5a else: x5= int( (x5a+x5)/2 ) if ( abs(y5a-y5)>5 ): y5=y5a else: y5= int( (y5a+y5)/2 ) # Ajustamos centro para color 6 if ( abs(x6a-x6)>5 ): x6=x6a else: x6= int( (x6a+x6)/2 ) if ( abs(y6a-y6)>5 ): 91 y6=y6a else: y6= int( (y6a+y6)/2 ) # Ajustamos centro para color 7 if ( abs(x7a-x7)>5 ): x7=x7a else: x7= int( (x7a+x7)/2 ) if ( abs(y7a-y7)>5 ): y7=y7a else: y7= int( (y7a+y7)/2 ) ## CALCULAMOS EL TRIANGULO 1 FORMADO POR 1,2 Y 3: distancia21=math.sqrt(((x2-x1)**2)+((y2-y1)**2)) if (distancia21<=0): distancia21=0.001 print ("distancia21 = ", distancia21) distancia31=math.sqrt(((x3-x1)**2)+((y3-y1)**2)) if (distancia31<=0): distancia31=0.001 print ("distancia31 = ", distancia31) distancia32=math.sqrt(((x3-x2)**2)+((y3-y2)**2)) if (distancia32<=0): distancia32=0.001 print ("distancia32 = ", distancia32) #Calculamos los angulos: gamma1=math.acos(((distancia21**2)+(distancia32**2)- (distanci31**2))/(2*distancia21*distancia32)) ## CALCULAMOS EL TRIANGULO 1 FORMADO POR 3,4 Y 5: distancia43=math.sqrt(((x4-x3)**2)+((y4-y3)**2)) if (distancia43<=0): distancia43=0.001 print ("distancia43 = ", distancia43) distancia54=math.sqrt(((x5-x4)**2)+((y5-y4)**2)) if (distancia54<=0): distancia54=0.001 print ("distancia54 = ", distancia54) distancia53=math.sqrt(((x5-x3)**2)+((y5-y3)**2)) if (distancia53<=0): distancia53=0.001 print ("distancia53 = ", distancia53) #Calculamos los angulos: gamma2=math.acos(((distancia43**2)+(distancia54**2)- (distanci53**2))/(2*distancia43*distancia54)) ## CALCULAMOS EL TRIANGULO 1 FORMADO POR 5,6 Y 7: Anexos: 92 92 distancia65=math.sqrt(((x6-x5)**2)+((y6-y5)**2)) if (distancia65<=0): distancia65=0.001 print ("distancia65 = ", distancia65) distancia76=math.sqrt(((x7-x6)**2)+((y7-y6)**2)) if (distancia76<=0): distancia76=0.001 print ("distancia76 = ", distancia76) distancia75=math.sqrt(((x7-x5)**2)+((y7-y5)**2)) if (distancia75<=0): distancia75=0.001 print ("distancia75 = ", distancia75) #Calculamos los angulos: gamma3=math.acos(((distancia65**2)+(distancia76**2)- (distanci75**2))/(2*distancia65*distancia76)) #Mostramos sus coordenadas por pantalla print ("x1 = ", x1) print ("y1 = ", y1) print ("x2 = ", x2) print ("y2 = ", y2) print ("x3 = ", x3) print ("y3 = ", y3) print ("x4 = ", x4) print ("y4 = ", y4) print ("x5 = ", x5) print ("y5 = ", y5) print ("x6 = ", x6) print ("y6 = ", y6) print ("x7 = ", x7) print ("y7 = ", y7) print ("gamma1 = ", gamma1) print ("gamma2 = ", gamma2) print ("gamma3 = ", gamma3) #Dibujamos una marca en el centro del objeto cv2.circle(imagen, (x1, y1), 2,(204,204,0), -1) cv2.circle(Result, (x1, y1), 2,(204,204,0), -1) cv2.circle(imagen, (x2, y2), 2,(255,0,127), -1) cv2.circle(Result, (x2, y2), 2,(255,0,127), -1) cv2.circle(imagen, (x3, y3), 2,(0,255,0), -1) cv2.circle(Result, (x3, y3), 2,(0,255,0), -1) cv2.circle(imagen, (x4, y4), 2,(0,255,255), -1) cv2.circle(Result, (x4, y4), 2,(0,255,255), -1) cv2.circle(imagen, (x5, y5), 2,(0,0,255), -1) cv2.circle(Result, (x5, y5), 2,(0,0,255), -1) cv2.circle(imagen, (x6, y6), 2,(255,0,0), -1) cv2.circle(Result, (x6, y6), 2,(255,0,0), -1) cv2.circle(imagen, (x7, y7), 2,(0,128,255), -1) cv2.circle(Result, (x7, y7), 2,(0,128,255), -1) # Dibujamos las rectas que unen los centros: cv2.line(imagen, (x3, y3), (x1, y1), (255,0,255),1) cv2.line(Result, (x3, y3), (x1, y1), (255,0,255),1) cv2.line(imagen, (x2, y2), (x1, y1), (255,0,255),1) cv2.line(Result, (x2, y2), (x1, y1), (255,0,255),1) cv2.line(imagen, (x3, y3), (x2, y2), (255,0,255),1) 93 cv2.line(Result, (x3, y3), (x2, y2), (255,0,255),1) cv2.line(imagen, (xM1, yM1), (x2, y2), (255,255,255),1) cv2.line(Result, (xM1, yM1), (x2, y2), (255,255,255),1) cv2.line(imagen, (x5, y5), (x3, y3), (255,255,255),1) cv2.line(Result, (x5, y5), (x3, y3), (255,255,255),1) cv2.line(imagen, (x4, y4), (x3, y3), (255,255,255),1) cv2.line(Result, (x4, y4), (x3, y3), (255,255,255),1) cv2.line(imagen, (x5, y5), (x4, y4), (255,255,255),1) cv2.line(Result, (x5, y5), (x4, y4), (255,255,255),1) cv2.line(imagen, (xM2, yM2), (x4, y4), (255,0,255),1) cv2.line(Result, (xM2, yM2), (x4, y4), (255,0,255),1) cv2.line(imagen, (x7, y7), (x5, y5), (255,0,255),1) cv2.line(Result, (x7, y7), (x5, y5), (255,0,255),1) cv2.line(imagen, (x6, y6), (x5, y5), (255,0,255),1) cv2.line(Result, (x6, y6), (x5, y5), (255,0,255),1) cv2.line(imagen, (x7, y7), (x6, y6), (255,0,255),1) cv2.line(Result, (x7, y7), (x6, y6), (255,0,255),1) cv2.line(imagen, (xM3, yM3), (x6, y6), (255,255,255),1) cv2.line(Result, (xM3, yM3), (x6, y6), (255,255,255),1) # Escribimos en el video los centros: cv2.putText(imagen, str(x1), (x1, y1), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (204,204,0),1,cv2.LINE_AA) cv2.putText(imagen, str(y1), (x1+50, y1), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (204,204,0),1,cv2.LINE_AA) cv2.putText(imagen, str(x2), (x2, y2), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,0,127),1,cv2.LINE_AA) cv2.putText(imagen, str(y2), (x2+50, y2), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,0,127),1,cv2.LINE_AA) cv2.putText(imagen, str(x3), (x3, y3), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,255,0),1,cv2.LINE_AA) cv2.putText(imagen, str(y3), (x3+50, y3), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,255,0),1,cv2.LINE_AA) cv2.putText(imagen, str(x4), (x4, y4), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,0),1,cv2.LINE_AA) cv2.putText(imagen, str(y4), (x4+50, y4), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,0),1,cv2.LINE_AA) cv2.putText(imagen, str(x5), (x5, y5), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,0,255),1,cv2.LINE_AA) cv2.putText(imagen, str(y5), (x5+50, y5), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,0,255),1,cv2.LINE_AA) cv2.putText(imagen, str(x6), (x6, y6), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,0,0),1,cv2.LINE_AA) cv2.putText(imagen, str(y6), (x6+50, y6), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,0,0),1,cv2.LINE_AA) cv2.putText(imagen, str(x7), (x7, y7), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,128,255),1,cv2.LINE_AA) cv2.putText(imagen, str(y7), (x7+50, y7), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,128,255),1,cv2.LINE_AA) cv2.putText(imagen, str(xM1), (xM1, yM1), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,0,255),1,cv2.LINE_AA) cv2.putText(imagen, str(yM1), (xM1+50, yM1), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,0,255),1,cv2.LINE_AA) cv2.putText(imagen, str(xM2), (xM2, yM2), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,0,255),1,cv2.LINE_AA) cv2.putText(imagen, str(yM2), (xM2+50, yM2), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,0,255),1,cv2.LINE_AA) Anexos: 94 94 cv2.putText(imagen, str(xM3), (xM3, yM3), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,0,255),1,cv2.LINE_AA) cv2.putText(imagen, str(yM3), (xM3+50, yM3), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,0,255),1,cv2.LINE_AA) cv2.putText(Result, str(x1), (x1, y1), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (204,204,0),1,cv2.LINE_AA) cv2.putText(Result, str(y1), (x1+50, y1), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (204,204,0),1,cv2.LINE_AA) cv2.putText(Result, str(x2), (x2, y2), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,0,127),1,cv2.LINE_AA) cv2.putText(Result, str(y2), (x2+50, y2), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,0,127),1,cv2.LINE_AA) cv2.putText(Result, str(x3), (x3, y3), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,255,0),1,cv2.LINE_AA) cv2.putText(Result, str(y3), (x3+50, y3), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,255,0),1,cv2.LINE_AA) cv2.putText(Result, str(x4), (x4, y4), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,0),1,cv2.LINE_AA) cv2.putText(Result, str(y4), (x4+50, y4), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,0),1,cv2.LINE_AA) cv2.putText(Result, str(x5), (x5, y5), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,0,255),1,cv2.LINE_AA) cv2.putText(Result, str(y5), (x5+50, y5), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,0,255),1,cv2.LINE_AA) cv2.putText(Result, str(x6), (x6, y6), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,0,0),1,cv2.LINE_AA) cv2.putText(Result, str(y6), (x6+50, y6), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,0,0),1,cv2.LINE_AA) cv2.putText(Result, str(x7), (x7, y7), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,128,255),1,cv2.LINE_AA) cv2.putText(Result, str(y7), (x7+50, y7), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,128,255),1,cv2.LINE_AA) cv2.putText(Result, str(xM1), (xM1, yM1), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,0,255),1,cv2.LINE_AA) cv2.putText(Result, str(yM1), (xM1+50, yM1), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,0,255),1,cv2.LINE_AA) cv2.putText(Result, str(xM2), (xM2, yM2), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,0,255),1,cv2.LINE_AA) cv2.putText(Result, str(yM2), (xM2+50, yM2), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,0,255),1,cv2.LINE_AA) cv2.putText(Result, str(xM3), (xM3, yM3), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,0,255),1,cv2.LINE_AA) cv2.putText(Result, str(yM3), (xM3+50, yM3), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,0,255),1,cv2.LINE_AA) cv2.putText(Datos, "Angulo 1", (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (204,204,0),1,cv2.LINE_AA) cv2.putText(Datos, str(gamma1), (230, 50), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (204,204,0),1,cv2.LINE_AA) cv2.putText(Datos, "Angulo 2", (50, 80), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255,0,127),1,cv2.LINE_AA) cv2.putText(Datos, str(gamma2), (230, 80), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255,0,127),1,cv2.LINE_AA) 95 cv2.putText(Datos, "Angulo 3", (50, 110), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,255,0),1,cv2.LINE_AA) cv2.putText(Datos, str(gamma3), (230, 110), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,255,0),1,cv2.LINE_AA) #Mostramos la imagen original con la marca del centro y #la mascara cv2.imshow('Camara', imagen) cv2.imshow('Result', Result) cv2.imshow('Datos', Datos) tecla = cv2.waitKey(5) & 0xFF if tecla == 27: break cv2.destroyAllWindows() Anexos: 96 96 7.7 Anexo VII: Código Python Calibración de Colores 14 import serial import cv2 import numpy as np cap = cv2.VideoCapture(0) def nothing(x): pass #Creamos una ventana llamada 'image' en la que habra todos los sliders cv2.namedWindow('image') cv2.createTrackbar('Hue Minimo','image',0,255,nothing) cv2.createTrackbar('Hue Maximo','image',0,255,nothing) cv2.createTrackbar('Saturation Minimo','image',0,255,nothing) cv2.createTrackbar('Saturation Maximo','image',0,255,nothing) cv2.createTrackbar('Value Minimo','image',0,255,nothing) cv2.createTrackbar('Value Maximo','image',0,255,nothing) while(1): _,frame = cap.read() #Leer un frame hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) #Convertirlo a espacio de color HSV #Los valores maximo y minimo de H,S y V se guardan en funcion de la posicion de los sliders hMin = cv2.getTrackbarPos('Hue Minimo','image') hMax = cv2.getTrackbarPos('Hue Maximo','image') sMin = cv2.getTrackbarPos('Saturation Minimo','image') sMax = cv2.getTrackbarPos('Saturation Maximo','image') vMin = cv2.getTrackbarPos('Value Minimo','image') vMax = cv2.getTrackbarPos('Value Maximo','image') #Se crea un array con las posiciones minimas y maximas lower=np.array([hMin,sMin,vMin]) upper=np.array([hMax,sMax,vMax]) #Crear una mascara con solo los pixeles dentro del rango de verdes mask = cv2.inRange(hsv, lower, upper) #Mostrar los resultados y salir cv2.imshow('camara',frame) cv2.imshow('mask',mask) k = cv2.waitKey(5) & 0xFF if k == 27: break cv2.destroyAllWindows() 14 Código obtenido de: https://robologs.net/2014/07/02/deteccion-de-colores-con-opencv-y-python/ 97 Bibliografía. 98 98 BIBLIOGRAFÍA. Libros de Robótica: [1] Barrientos, Antonio, Fundamentos de Robótica, 2ª edición, 2007. Enlaces de Arobotix – M: [2] https://learn.trossenrobotics.com/arbotix/7-arbotix-quick-start-guide [3] http://codigoelectronica.com/blog/como-instalar-arduino-en-raspberry-pi [4]https://www.ftdichip.com/Support/Documents/AppNotes/AN_220_FTDI_Drivers_Installation_Guide_for_ Linux.pdf Enlaces de RaspBerry Pi y OpenCV: [5] https://www.pyimagesearch.com/ [6] https://www.pyimagesearch.com/2017/09/04/raspbian-stretch-install-opencv-3-python-on-your-raspberry- pi/ [7] https://robologs.net/2014/07/02/deteccion-de-colores-con-opencv-y-python/ Enlaces de Dynamixel AX-12A: [8] http://emanual.robotis.com/docs/en/dxl/ax/ax-12a/ [9] http://forums.trossenrobotics.com/showthread.php?7272-Precision-of-AX-12A Impresión 3D: [10] http://soloelectronicos.com/2017/10/03/primeros-pasos-con-una-impresora-3d-prusa-i3-pro/ [11] http://imprimalia3d.com/recursosimpresion3d/gu-configuraci-n-par-metros-slic3r [12] https://www.spainlabs.com/foros/ 99