Diseño de un control de posición X-Y de un quadrotor basado en un algoritmo de flujo óptico

Curso Académico: TRABAJO FIN DE GRADO EN INGENIERÍA EN TECNOLOGÍAS INDUSTRIALES DISEÑO DE UN CONTROL DE POSICIÓN X-Y DE UN QUADROTOR BASADO EN UN ALGORITMO DE FLUJO ÓPTICO AUTOR: TUTOR: FRANCISCO MIGUEL AROCA JIMÉNEZ PEDRO GARCÍA GIL 2014-15 Índice de documentos  Memoria  Presupuesto  Anexo de programación Índice de la memoria Glosario de ilustraciones ..................................................................................................................... 1 1. Introducción .................................................................................................................................... 4 1.1. Conceptos generales ............................................................................................................... 4 1.2. Modelo teórico ........................................................................................................................ 5 1.3. Motivación y justificación ........................................................................................................ 7 1.4. Antecedentes y soluciones previas ......................................................................................... 8 1.5. Objetivos del trabajo ............................................................................................................... 8 1.6. Normativa ................................................................................................................................ 9 2. Desarrollo del trabajo ...................................................................................................................... 9 2.1. Planteamiento general ............................................................................................................ 9 2.2. Fundamentos de la visión por computador .......................................................................... 10 2.3. Selección del material (hardware y software) ...................................................................... 14 2.4. Desarrollo del algoritmo de medición y control .................................................................... 18 2.4.1. Algoritmos de medición ................................................................................................ 18 Algoritmo general ...................................................................................................................... 19 Algoritmo específico (círculos) .................................................................................................. 27 Algoritmo específico (color) ...................................................................................................... 30 2.4.2. Tratamiento de la medida ............................................................................................. 31 2.4.3. Desarrollo del algoritmo de control .............................................................................. 36 3. Resultados y conclusiones ............................................................................................................. 39 Bibliografía......................................................................................................................................... 40 Índice del presupuesto Unidades de obra .............................................................................................................................. 42 Presupuesto final ............................................................................................................................... 43 Índice del anexo Código del algoritmo general de medición ....................................................................................... 45 Código del algoritmo específico de medición (algoritmo de círculos) .............................................. 48 Código del algoritmo específico de medición (algoritmo de color) .................................................. 49 Modificaciones para adaptar la medida ............................................................................................ 50 Filtrado .............................................................................................................................................. 50 Control ............................................................................................................................................... 51 1 Glosario de ilustraciones Se cita aquí el origen de las imágenes tomadas de fuentes externas. Ilustración 1. Dron MQ-9 para uso militar .............................................................................................. 4 http://upload.wikimedia.org/wikipedia/commons/thumb/b/b0/MQ9_Reaper_in_flight_%282007%29.jpg/250px-MQ-9_Reaper_in_flight_%282007%29.jpg Ilustración 2. Dron de 8 hélices para uso civil ......................................................................................... 4 http://www.atalayar.com/content/argelia-tendr%C3%A1-su-primer-drone-supers%C3%B3nico-en2016 Ilustración 3. Dron casero propulsado con MCIA único unido a un sistema de correas......................... 5 http://diydrones.com/profiles/blogs/gas-powered-quadcopter-second-pixhawk-based-finalist-forthe-hack Ilustración 4. Quadrotor objeto del trabajo ............................................................................................ 5 Ilustración 5. Angulos de orientación (φ, 𝜃, ψ) ....................................................................................... 6 Ilustración 6. Coordenadas globales (x,y,z) ............................................................................................. 6 Ilustración 7. 1er giro (yaw)..................................................................................................................... 6 Ilustración 8. 2ndo giro (roll) ................................................................................................................... 6 Ilustración 9. 3r giro (pitch) ..................................................................................................................... 6 Ilustración 10. Medida de flujo óptico considerando un pixel .............................................................. 12 Ilustración 11. Medida de flujo óptico considerando una ventana de 2x2 píxeles ............................... 12 Ilustración 12. Ejemplo de flujo óptico denso ....................................................................................... 13 http://www.hizook.com/files/users/7/OpticalFlow_Street.jpg Ilustración 13. Ejemplo de flujo óptico disperso ................................................................................... 13 http://i.ytimg.com/vi/OIm6VXa0vZ0/hqdefault.jpg Ilustración 14. BeagleBone Black .......................................................................................................... 14 http://beagleboard.org/black Ilustración 15. IGEPv2 ............................................................................................................................ 14 https://www.isee.biz/products/igep-processor-boards/igepv2-dm3730 Ilustración 16. Eye Toy Namtai .............................................................................................................. 16 http://blog.us.playstation.com/2010/11/03/eyetoy-innovation-and-beyond/ Ilustración 17. PlayStation Eye .............................................................................................................. 16 http://www.amazon.com/PlayStation-Eye-3/dp/B000VTQ3LU Ilustración 18. Cámara PS eye sin carcasa ............................................................................................. 18 Ilustración 19. Detalle del montaje de la cámara (líneas rojas = ejes X-Y quadrotor) .......................... 18 Ilustración 20. Diagrama de flujo del algoritmo general inicial ............................................................ 20 Ilustración 21. Detalle del diagrama de flujo (selección de puntos) ..................................................... 21 Ilustración 22. Gráfica de comparación de algoritmos de selección de puntos ................................... 22 Ilustración 23. Selección de puntos a intervalos regulares ................................................................... 22 Ilustración 24. Selección de puntos con GoodFeatures ........................................................................ 22 2 Ilustración 25. Selección de puntos con FAST ....................................................................................... 22 Ilustración 26. Selección de puntos con ORB ........................................................................................ 22 Ilustración 27. Detalle del diagrama de flujo (seguimiento de puntos) ................................................ 23 Ilustración 28. Flujo óptico con tamaño de ventana 10x10 .................................................................. 24 Ilustración 29. Flujo óptico con tamaño de ventana 3x3 ...................................................................... 24 Ilustración 30. Detalle del diagrama de flujo (cálculo desplazamiento) ............................................... 24 Ilustración 31. Diagrama de flujo del algoritmo general modificado .................................................... 25 Ilustración 32. Gráfica comparativa entre algoritmos de medida ........................................................ 26 Ilustración 33. Gráfica de trayectoria ejemplo ...................................................................................... 26 Ilustración 34. Diagrama de flujo del algoritmo específico ................................................................... 27 Ilustración 35. Procedimiento ejecutado por el algoritmo específico .................................................. 28 Ilustración 36. Resultado del algoritmo utilizando 2 círculos ............................................................... 29 Ilustración 37. Gráfica resultado experimento círculos ........................................................................ 29 Ilustración 38. Imagen original .............................................................................................................. 30 Ilustración 39. Imagen tras comparación y tratamiento ....................................................................... 30 Ilustración 40. Resultado (punto rojo = centro) .................................................................................... 30 Ilustración 41. Modelo teórico de la cámara ........................................................................................ 31 Ilustración 42. Imagen de ejemplo para la calibración ......................................................................... 31 Ilustración 43. Triángulo extraído del modelo ...................................................................................... 32 Ilustración 44. Validación del modelo con trayectoria ejemplo ........................................................... 33 Ilustración 45. Composición de velocidades en X ................................................................................. 33 Ilustración 46. Gráfica de la posición X-Y (sin filtrar) ............................................................................ 35 Ilustración 47. Gráfica de velocidad filtrada en X (𝛼=0.05) .............................................................. 35 Ilustración 48. Diagrama del filtro de Kalman ....................................................................................... 35 http://www.scielo.cl/fbpe/img/ingeniare/v17n3/fig11-2.GIF Ilustración 49. Diagrama de flujo del control de orientación ............................................................... 36 Ilustración 50. Diagrama de flujo del control de la posición en X ......................................................... 36 Ilustración 51. Resultados de la acción proporcional en vuelo ............................................................. 37 Ilustración 52. Filtrado de la aceleración .............................................................................................. 38 Ilustración 53. Retardo en la medida de visión ..................................................................................... 38 Ilustración 54. Medida final de velocidad ............................................................................................. 39 3 2015 Memoria 4 1. Introducción 1.1. Conceptos generales Los vehículos aéreos no tripulados (VANT o UAV en inglés) también llamados drones, constituyen un grupo muy amplio de aeronaves de tipologías muy distintas, desde configuraciones similares a los aviones clásicos, hasta otras más próximas al sistema de funcionamiento de los helicópteros, con varias hélices que proporcionan la sustentación necesaria. Ilustración 1. Dron MQ-9 para uso militar Ilustración 2. Dron de 8 hélices para uso civil Dentro de este último grupo, también podemos encontrar diferencias principalmente en el número de hélices y en el sistema de propulsión. Así, un mayor número de hélices proporciona un mayor control de la orientación a costa de una ley de control más compleja debido al aumento de los grados de libertad. Por otra parte, en cuanto a los sistemas de propulsión, principalmente se utilizan 2: motores eléctricos y motores de combustión interna. El sistema más popular es el basado en motores eléctricos debido a la facilidad de regulación, poco peso y versatilidad aunque tiene inconvenientes como las limitaciones de potencia y autonomía debido a la baja potencia específica de los motores y la baja energía específica de las baterías que los alimentan. Estos problemas no aparecen con los motores de combustión, su mayor potencia permite elevar mayor peso y la densidad energética del combustible ofrece una autonomía superior a las baterías, aun así este tipo de motores son de difícil regulación e introducen vibraciones indeseables en el sistema que pueden interferir en los sensores. 5 Ilustración 3. Dron casero propulsado con MCIA único unido a un sistema de correas En nuestro caso, el quadrotor sobre el que se ha trabajado se trata de un dron con 4 hélices configuradas en forma de cruz y movidas por pequeños motores eléctricos. Esta es una de las configuraciones más populares por su versatilidad y sencillez de control. Ilustración 4. Quadrotor objeto del trabajo 1.2. Modelo teórico Una vez definido el marco de trabajo, pasamos a introducir brevemente las variables y grados de libertad del quadrotor y el modelo dinámico del mismo. Para plantear un sistema de control hacen falta variables que controlar. Las variables habituales utilizadas en este ámbito son los 3 ángulos de orientación (pitch, roll, yaw) relativos a un sistema de referencia ligado al quadrotor y las 3 coordenadas cartesianas globales del quadrotor (x, y, z). 6 Ilustración 5. Angulos de orientación (φ, 𝜃, ψ) Ilustración 6. Coordenadas globales (x,y,z) La posición y orientación del quadrotor se obtienen trasladando el origen de coordenadas del sistema global al centro del quadrotor, posteriormente efectuando el giro correspondiente a la medida yaw sobre el eje local Z, rotando después en torno al eje X del sistema de referencia resultante el ángulo roll, y finalmente, rotando de nuevo el ángulo pitch respecto al eje Y de forma análoga al giro anterior. Ilustración 7. 1er giro (yaw) Ilustración 8. 2ndo giro (roll) Ilustración 9. 3r giro (pitch) 7 Pasamos ahora a exponer el modelo dinámico del quadrotor [1], éste deriva de aplicar las ecuaciones de Euler-Lagrange a nuestro sistema incluyendo además la simplificación de que los ángulos pitch y roll son pequeños. El resultado es el siguiente sistema de ecuaciones: Donde 𝑚 es la masa del quadrotor, 𝑢 es el empuje total de los motores, los parámetros 𝑘𝜓, 𝑘𝜃 y 𝑘𝜑 son constantes que engloban los momentos de inercia del quadrotor respecto a su centro, la relación entre voltaje aplicado al motor y velocidad de giro del mismo y la ley de las hélices que relaciona su velocidad de giro con la sustentación que proporcionan. Y las variables 𝑉𝑥 son los voltajes aplicados a cada motor que constituirán la acción de control. 1.3. Motivación y justificación La razón de ser de este trabajo, además de la finalización de los estudios de grado, es la resolución de los problemas que acarrea no controlar la posición del quadrotor que van desde la deriva excesiva y posible pérdida del aparato, hasta choques con obstáculos o personas que pueden ocasionar graves daños tanto al quadrotor cómo a aquello con lo que choca. Procedemos ahora a analizar el modelo matemático para comprender la problemática del control en el plano X-Y. Tomemos como punto de partida la configuración inicial del quadrotor en el laboratorio, éste tiene implementado ya, gracias a trabajos anteriores de otros alumnos y personal investigador, un control de las variables de orientación (pitch, roll y yaw) y de la altura. Así pues para un vuelo “hover” (estático a una altura determinada) no hay más que ajustar las referencias de pitch y roll a 0º, consiguiendo así que el quadrotor permanezca paralelo al suelo y poner la referencia de la altura a un valor determinado constante 𝑧0. Suponiendo que el control funciona correctamente y mantiene los ángulos de orientación en su referencia, analicemos que ocurre con las variables 𝑥 e 𝑦 de nuestro modelo teórico: 𝑚𝑥󰇘=−𝑢sin𝜃 𝑚𝑦󰇘=𝑢cos𝜃sin𝜑 𝜃=0 𝜑=0 → 𝑥󰇘=0 𝑦󰇘=0 → 𝑥󰇗=𝑣𝑥=𝐶𝑥 𝑦󰇗=𝑣𝑦=𝐶𝑦 Ecuación 2. Problema de la deriva Como podemos observar, de la ecuación 2 se extrae que el quadrotor se moverá con velocidad constante (no necesariamente nula) en el plano X-Y de forma análoga al movimiento que tendría un objeto en una pista de hielo. Este movimiento se comprueba de forma experimental al poner el quadrotor en vuelo, se observa como parece que se desplace en un plano imaginario paralelo al suelo y establecido a la altura de la referencia 𝑧0. 𝑚𝑥󰇘=−𝑢sin𝜃 𝑚𝑦󰇘=𝑢cos𝜃sin𝜑 𝑚𝑧󰇘=𝑢cos𝜃cos𝜑−𝑚𝑔 𝜓󰇘=𝑢𝜓=𝑘𝜓(𝑉𝑓−𝑉𝑙+𝑉𝑏+𝑉𝑟) 𝜃󰇘=𝑢𝜃=𝑘𝜃(𝑉𝑏−𝑉𝑓) 𝜑󰇘 =𝑢𝜑=𝑘𝜑(𝑉𝑙−𝑉𝑟) Ecuación 1. Modelo matemático simplificado del quadrotor 14 busca una serie de puntos e intenta rastrearlos en la imagen siguiente y que sería utilizado como herramienta de “benchmarking” para ayudar a elegir el material a utilizar (hardware y software). 2.3. Selección del material (hardware y software) En este punto se empezó con la selección del software base para el algoritmo de flujo óptico, de entre las diversas librerías de visión artificial disponibles se eligió OpenCV por diversas razones: se trata de software libre, multiplataforma con una gran comunidad de usuarios y una amplia documentación sobre su uso e instalación. Además, versiones anteriores de estas librerías habían sido utilizadas ya por otros alumnos e investigadores en otros proyectos similares con buenos resultados. Los programas diseñados en el apartado anterior fueron compilados con estas librerías. Más adelante, en el punto 2.4 se profundizará en el contenido y funciones de estas librerías. Pasemos ahora a la selección de la plataforma hardware a utilizar, el conjunto placa-cámara, hablamos de estos dos elementos como conjunto porque, pese a tratarse de partes físicamente independientes, es imprescindible que sean compatibles para el correcto funcionamiento del sistema. Ilustración 14. BeagleBone Black Ilustración 15. IGEPv2 Empezamos este proceso de selección comparando dos placas disponibles en el laboratorio, la IGEPv2 (actualmente en uso en el quadrotor) y la Beaglebone Black (BBB en adelante). Las especificaciones técnicas de ambas placas son similares tal y como exponemos en la siguiente tabla: IGEPv2 BBB Procesador DM3730 ARM Cortex A8 1Ghz AM335x 1GHz ARM® CortexA8 Memoria RAM 512 MB 512MB WiFi Incluido en la placa Con adaptador USB WiFi Conexiones disponibles 1 x USB 2.0 1 x Ethernet 3 x UART (2 x RS232 y 1 x RS485) 3 x SPI 2 x I2C 1 x USB 2.0 1 x Ethernet 1 x UART Precio 179 € 51.99 € Tabla 1. Comparación de las especificaciones de las placas 15 La IGEPv2, al estar ya instalada en el quadrotor, estaba ya casi preparada para ejecutar nuestro programa de prueba, a falta de la instalación de las librerías OpenCV, que fueron compiladas en cruzado desde un ordenador de sobremesa para la arquitectura de la placa (ARM) dejando así la IGEPv2 lista para su uso. La BBB, por otra parte, estaba prácticamente recién salida de fábrica, con lo cual fue necesario un proceso de preparación del entorno de programación instalando un sistema operativo basado en Linux (Debian) y aplicando un parche (Xenomai) al kernel de Linux que permite la ejecución de programas en “tiempo real”, las tareas ejecutadas en tiempo real tienen máxima prioridad dentro del sistema, se les puede asignar una prioridad incluso mayor a la de las propias tareas del sistema operativo evitando así cualquier “cuelgue” del programa de vuelo que provocaría la caída del quadrotor. El sistema operativo seleccionado para la BBB ha sido el disponible en la siguiente página web bajo licencia libre: http://www.machinekit.net/deb/rootfs/wheezy/BBB-eMMC-flasher-debian-7.4-machinekit-armhf2014-05-19-4gb.img.xz Se ha elegido este sistema concreto por estar diseñado para el manejo de máquinas de fabricación por control numérico, las necesidades de este campo son similares a las nuestras y esto se ve en el software incluido en este sistema operativo dedicado que ya contiene el kernel con Xenomai y las librerías de OpenCV. Una vez preparados los respectivos entornos de programación, se procede a ejecutar el programa de prueba que habíamos preparado en el apartado anterior. El experimento se ejecuta en las siguientes condiciones:  Cámara inmóvil y entorno invariante.  El programa de la cámara es el único programa en ejecución en la placa.  Los resultados son impresos a fichero para no comprometer la velocidad del programa. En las condiciones expuestas los resultados esperados son: una medida de desplazamiento nula y un tiempo de ejecución solo dependiente de las características de la placa y del sistema operativo. Los resultados obtenidos son los siguientes: IGEPv2 BBB Desplazamiento máximo (pix) 0.01 0.01 Tiempo de ejecución (mín – med – máx) (ms) 30 – 50 – 120 50 – 80 – 200 Tabla 2. Comparación del rendimiento del programa de prueba en las placas Pasemos ahora al análisis de los resultados, a primera vista, sorprende que el desplazamiento máximo de píxeles sea un número decimal y no entero, este resultado se explica por la naturaleza del programa utilizado, que, tras obtener el valor individual de flujo óptico para cada píxel, pasa a calcular la media aritmética de todos estos valores que es lo que consideramos como flujo óptico total de la imagen. 16 Dicho esto, cabe remarcar que un “error” de 0.01 píxeles es un buen resultado que podemos asociar a pequeños cambios de iluminación o incluso al ruido presente en la imagen. Otra conclusión importante que extraemos de este resultado es que la precisión de la medida no depende de la placa utilizada sino más bien de la cámara en uso. Por otra parte sí que encontramos diferencias en el tiempo de ejecución de la tarea, pese a que estamos trabajando en un entorno de tiempo real (Xenomai) el programa compilado no hace uso de estas funciones, lo que hace que pueda ser pospuesta su ejecución en favor de tareas internas del sistema, es por eso que no obtenemos un tiempo de ejecución fijo. Cabe mencionar que tras detectar este problema se intentó implementar el mismo programa como tarea de tiempo real pero sin éxito, al parecer las librerías de OpenCV no son compatibles con las tareas de Xenomai. El problema expuesto no compromete la validez del experimento ya que se realizó del mismo modo en ambas placas, en los resultados se observa que la placa BBB tiene un tiempo de ejecución ligeramente más alto, creemos que el origen de esta diferencia es la interfaz de usuario del sistema operativo de la BBB, en la IGEPv2 no se ejecuta ningún entorno gráfico lo cual evita cargar innecesariamente el procesador. Pese a que se podría haber desactivado la interfaz gráfica de la BBB para mejorar la velocidad de procesamiento, se encontró otro problema que llevó a tomar la IGEPv2 como placa de trabajo: los puertos USB. Para el funcionamiento autónomo del quadrotor es necesaria tanto la conexión WiFi para comunicarse con el PC en tierra como la cámara para hacer el control por flujo óptico. En el caso de la IGEPv2 esto no es un problema porque el WiFi está integrado en la propia placa y tiene un puerto USB para conectar la cámara, pero la BBB tiene un solo puerto USB al que se deberían conectar tanto la cámara como el dispositivo de WiFi, se probó un ampliador de puertos USB pero daba problemas con la alimentación así que se apartó por el momento la idea de utilizar la BBB y se tomó como placa de trabajo la IGEPv2 que además estaba ya instalada en el quadrotor y tenía los puertos configurados para las conexiones del resto de sensores. Pasemos ahora a la selección de la cámara, las opciones consideradas son las 2 siguientes: Eye Toy Namtai y PlayStation Eye, ambas son cámaras diseñadas con fines de entretenimiento en juegos basados en la detección del movimiento del jugador, se ajustan pues a nuestras necesidades de detección de movimiento. Ilustración 16. Eye Toy Namtai Ilustración 17. PlayStation Eye Las especificaciones técnicas de estas cámaras no se han encontrado y se ha procedido a su determinación empírica, en concreto nos interesan dos parámetros: la velocidad de captura o FPS, y 17 la resolución, que debe ser lo suficientemente alta como para detectar movimiento, pero sin llegar a valores demasiado altos porque una alta resolución de imagen aumenta exponencialmente el tiempo de cálculo del flujo óptico. Para poder utilizar las cámaras es necesario instalar primero los drivers correctos, así pues se buscó el driver correspondiente a ambas cámaras y se comprobó si estaba instalado en la IGEPv2, con resultado satisfactorio. Una vez comprobado esto, se pasó a la extracción de parámetros característicos de las cámaras, con este fin se compiló un pequeño programa que toma fotos de forma cíclica a la máxima velocidad posible y comprueba el tamaño de las imágenes. Los resultados se expresan en la tabla siguiente: Eye Toy Namtai PS eye Velocidad (FPS) 60 180 ó 70 Resolución (pix x pix) 320 x 240 (0.0768 Mpx) 320 x 240 (0.0768 Mpx) ó 640 x 480 (0.3 Mpx) Tabla 3. Características de las cámaras Como podemos ver los FPS son mayores en el PS eye, sobre todo a baja resolución, esta diferencia se debe principalmente al driver instalado, modificado por la comunidad de usuarios de Linux para aumentar al máximo posible la velocidad de muestreo de la cámara. La resolución de 320x240 píxel hemos comprobado en el laboratorio que es más que suficiente para nuestro propósito de detección de movimiento, además con esta resolución obtenemos un tiempo de cálculo mucho menor (4 veces menor) que con la resolución de 640x480 píxel, en el caso de querer utilizar la cámara para otros propósitos como grabación de vídeo o mapeado mediante imagen utilizaríamos la máxima resolución pero en este caso es prioritaria la velocidad de cálculo. Con los datos que tenemos concluimos que la cámara PS eye se ajusta mejor a nuestros propósitos y es por eso que la elegimos como cámara a utilizar en el trabajo. Con el objetivo de reducir peso y mejorar la integración física de la cámara en el conjunto del quadrotor, le quitamos todos los elementos de protección innecesarios e instalamos la cámara en la parte inferior, tal y como se muestra en las siguientes imágenes: 18 Ilustración 18. Cámara PS eye sin carcasa Ilustración 19. Detalle del montaje de la cámara (líneas rojas = ejes X-Y quadrotor) 2.4. Desarrollo del algoritmo de medición y control El desarrollo del algoritmo de control se puede dividir en tres etapas:  Medición  Tratamiento de la información (filtrado)  Control Procedemos a explicar el desarrollo de cada una de ellas. 2.4.1. Algoritmos de medición Antes de explicar el desarrollo y funcionamiento de los algoritmos de medición cabe mencionar que se han desarrollado paralelamente dos algoritmos: un primer algoritmo lo más general posible que pudiese funcionar en cualquier entorno, y otro más específico para entornos visuales semicontrolados. Estos dos algoritmos serán explicados de forma independiente puesto que son muy diferentes tanto en contenido como en base teórica, coincidiendo solo en el resultado que ofrecen, una medida de posición. Se desarrolló también una variante del algoritmo de círculos para detectar un color determinado, aunque el funcionamiento es muy similar. Por otra parte, debemos notar que, para una mejor comprensión de los procesos que se ejecutan en el algoritmo, en este documento hemos incluido diversas imágenes mostradas por pantalla pero que en la ejecución final del programa no se muestran para no ralentizar la ejecución del programa. Además, no hemos de olvidar la naturaleza indirecta de esta medida de posición, nuestro sensor es una cámara de la cual obtenemos imágenes y no es hasta después de un procesamiento interno de estas imágenes que se obtiene la medida de posición, esto hace que los periodos de muestreo obtenidos al aplicar estos métodos sean bastante superiores a los obtenidos con sensores directos como la IMU. 19 Por último, decir que se ha intentado en la medida de lo posible que este apartado sea lo más claro posible reduciendo al mínimo necesario la inclusión de fragmentos de código. Pasemos sin más demora al desarrollo del primero de los algoritmos: Algoritmo general Este programa parte de las siguientes suposiciones:  No tenemos ninguna información previa del entorno visual en el que se mueve el quadrotor  La diferencia de posición entre dos frames es pequeña (esto se puede conseguir con una baja velocidad de movimiento del quadrotor o con una alta frecuencia de muestreo de la cámara).  En la imagen hay elementos “característicos” de fácil seguimiento (un elemento característico es cualquier parte de la imagen que se diferencie del resto, por ejemplo los bordes de objetos, más adelante se aportarán ejemplos visuales de estos elementos) El proceso de medición de la posición se divide en varios subprocesos encadenados que aquí exponemos en el mismo orden en el que se ejecutan en el programa diseñado inicialmente, incluimos en este punto un diagrama de flujo con la intención de proporcionar una visión global del programa como conjunto (recuperaremos esta figura con fin de clarificar los sub-procesos y modificaciones sucesivas): 20 INICIO TOMA IMAGEN BÚSQUEDA DE PTS1 EN IMG2 IMG1 PTS1 TOMA IMAGEN SELECCIÓN DE PUNTOS IMG1 IMG2 PTS1 IMG2 PTS2 DIFERENCIA DE PUNTOS PTS1 PTS2 DESP ¿EN EJECUCIÓN? ACTUALIZACIÓN IMG1 = IMG2 IMG2 IMG1 FIN SÍ NO FUNCIÓN VARIABLE DECISIÓN FLUJO DE PROCESOS FLUJO DE DATOS LEYENDA Ilustración 20. Diagrama de flujo del algoritmo general inicial 21 1. Selección de puntos Considerando que tenemos la cámara y su driver correspondiente correctamente configurados, el primer paso de nuestro algoritmo es la selección de puntos. A grandes rasgos la función de selección de puntos debe tomar como entrada una imagen y devolver una serie de puntos de interés seleccionados en esa imagen: Ilustración 21. Detalle del diagrama de flujo (selección de puntos) Descartamos directamente la opción de seleccionar manualmente los puntos por la lentitud del proceso y porque el objetivo es automatizar el algoritmo. Pasamos pues a considerar las diferentes opciones que nos ofrece OpenCV para la selección de puntos: En primer lugar se probó a seleccionar puntos a intervalos regulares de píxeles de la imagen, a modo de cuadrícula, éste método más tarde probaría ser el más rápido (ya que no requiere operaciones internas de selección) pero también el más impreciso ya que los puntos seleccionados no tenían ninguna garantía de ser óptimos para el seguimiento. Posteriormente se probaron tres funciones internas de OpenCV diseñadas específicamente para la selección de puntos: GoodFeaturesToTrack, FAST y ORB. Los fundamentos de estas tres funciones son similares, tratan de buscar, mediante un proceso iterativo, los puntos que más contrastan con su entorno, asignando a cada punto una puntuación, luego son seleccionados y almacenados los puntos con mayor puntuación. La diferencia entre ellos está en los métodos matemáticos internos que ejecuta cada función que hacen variar los resultados notablemente. Para elegir uno de los cuatro algoritmos propuestos, tomamos como variables importantes las siguientes: el tiempo que consume la función y los puntos que no se han encontrado (puntos “perdidos”) entre dos frames. Esta última variable se puede considerar un indicador de la “calidad” de los puntos encontrados, si los puntos seleccionados son buenos candidatos para el seguimiento, deberían ser fácilmente encontrados en el frame siguiente, del mismo modo, si los puntos elegidos no son los adecuados, éstos no se encontrarán. El experimento de selección se realizó primero con la cámara en reposo y luego con un ligero movimiento hacia un lado desplazando la imagen unos 100 píxel en horizontal en un período de aproximadamente 10 segundos, es decir, a una velocidad de más o menos 10 píxel/segundo que no debería ser problema para unas funciones que se ejecutan en el orden de los milisegundos. Como condición extra se limitó la cantidad de puntos a encontrar a 50 puntos, una cantidad suficiente para efectuar el cálculo de posición sin comprometer la velocidad del procesador. Los resultados de la comparación se muestran en el gráfico siguiente: PTS1 SELECCIÓN DE PUNTOS IMG1 22 Ilustración 22. Gráfica de comparación de algoritmos de selección de puntos Ilustración 23. Selección de puntos a intervalos regulares Ilustración 24. Selección de puntos con GoodFeatures Ilustración 25. Selección de puntos con FAST Ilustración 26. Selección de puntos con ORB IDEAL Intervalos Regulares GoodFeatures ToTrack FAST ORB 0 5 10 15 20 25 30 35 0100 200 300 400 Puntos "perdidos" Tiempo (ms) 23 En la gráfica anterior (Ilustración 19) podemos ver que de los algoritmos expuestos, el que más se aproxima al algoritmo ideal es el ORB que ofrece la mejor relación calidad/tiempo. Es por eso que incluimos esta función en nuestro programa para la selección de puntos. Cabe mencionar que cada una de las funciones anteriores admite una serie de parámetros relativos al proceso de selección de puntos que también fueron muestreados para obtener la máxima eficiencia y que aquí sólo se muestran los mejores resultados de cada función. 2. Seguimiento de puntos Una vez elegidos los puntos a seguir, pasamos a buscarlos en una nueva imagen para calcular el desplazamiento que han sufrido. La función que buscamos debe funcionar del modo siguiente: Ilustración 27. Detalle del diagrama de flujo (seguimiento de puntos) Tomando como argumentos la imagen siguiente y los puntos encontrados en la imagen anterior, debe determinar la nueva posición de los puntos. Consultando la documentación online de OpenCV la opción sugerida más popular es el algoritmo de flujo óptico desarrollado por Lucas-Kanade basado en pirámides (aproximaciones sucesivas reduciendo la resolución de la imagen hasta llegar a la imagen original), implementado en la función “calcOpticalFlowPyrLK”. Analicemos ahora el funcionamiento de este algoritmo, como argumentos admite varios parámetros pero los más importantes son:  Tamaño de la ventana de búsqueda: determina el ancho y alto de la ventana alrededor del punto original en la cual se va a buscar el punto desplazado, un tamaño grande mejora la precisión pero puede comprometer la velocidad de cálculo. Tomamos como solución de compromiso un tamaño de 15x15 píxel.  Número de pirámides: cantidad de veces que se reduce la resolución de la imagen para los sucesivos cálculos del flujo óptico. Un valor de 3 mejora el rendimiento del algoritmo respecto a no aplicar reducción, valores superiores no mejoran la eficiencia e incluso llegan a reducirla, es por esto que asignamos 3 niveles de pirámides a nuestro algoritmo.  Criterios de terminación: determinan cuando la función debe parar de buscar el flujo óptico para un punto determinado, en nuestro caso hemos dejado los valores por defecto porque funcionan correctamente, estos hacen que el algoritmo pare si el error de la solución (diferencia entre una iteración y la anterior) es menor a 0.3 píxel o si se han hecho más de 20 iteraciones. De estos tres argumentos el más notable a nivel de rendimiento es el tamaño de la ventana, mostramos aquí dos ejemplos de cálculo del flujo óptico tomando tamaños de ventana diferentes, el flujo óptico se calcula entre la imagen mostrada en los ejemplos de selección de puntos y otra versión de esa misma imagen, desplazada un poco hacia la izquierda y hacia arriba usando un programa de edición de imágenes: BÚSQUEDA DE PTS1 EN IMG2 PTS1 IMG2 PTS2 30 Algoritmo específico (color) Se ha diseñado también una tercera opción de medida similar al algoritmo de los círculos que elimina algunos de los problemas de éste a costa de un ligero aumento del tiempo de computación por utilizar la imagen en color en lugar de en escala de grises. Este algoritmo funciona exactamente igual que el anterior pero en lugar de buscar un círculo, busca algún elemento de un color determinado, lo aísla y calcula su centro geométrico que se toma como medida de posición. La búsqueda de color se realiza haciendo una simple comparación de los valores de los tres canales de color (HSV) asignando un 1 a un píxel si sus colores se encuentran dentro del rango deseado, y un 0 si no se cumple esta condición. Después a la imagen binaria obtenida, se le hace un tratamiento para eliminar el posible ruido y se le calculan los momentos de la imagen con los cuales se obtiene el centro del objeto de color. El proceso se puede ver en las siguientes imágenes (se ha elegido el color verde para la búsqueda): Ilustración 38. Imagen original Ilustración 39. Imagen tras comparación y tratamiento Ilustración 40. Resultado (punto rojo = centro) 31 2.4.2. Tratamiento de la medida La medida obtenida con los algoritmos anteriores no se puede utilizar para hacer el control de forma directa ya que presenta ruido, interferencias provenientes de otras variables, y no está en las unidades adecuadas. Estos factores justifican la necesidad de un tratamiento de la medida. En primer lugar, nuestra medida se encuentra en píxel, lo que buscamos es una medida del desplazamiento en metros (o centímetros), con este fin es necesario calibrar la cámara obteniendo la correspondencia entre distancias en la imagen y distancias reales. Desgraciadamente la calibración implementada en OpenCV solo permite obtener esta correspondencia para una posición estática de la cámara, las condiciones de posición y orientación de nuestra cámara están ligadas a las del quadrotor y por lo tanto son muy variables, por lo tanto la calibración de OpenCV resulta inviable así que pasamos a hacer una calibración sencilla utilizando un modelo de cámara básico. Para calibrar la cámara tomamos el modelo “pin-hole” que considera la cámara como un punto del espacio con un ángulo de visión constante, con este ángulo y la altura de la cámara como únicos parámetros, podemos (aplicando algunas simplificaciones) calcular el factor de conversión de medidas imagen-realidad. Procedemos a exponer el experimento de calibración: Ilustración 41. Modelo teórico de la cámara Ilustración 42. Imagen de ejemplo para la calibración La simplificación que consideramos para esta calibración es que la cámara está situada de forma que el plano de la imagen (ver Ilustración 41) es paralelo al suelo, esto hace que las medidas tomadas sean directamente proporcionales con las medidas reales. La simplificación es razonable siempre que los ángulos de rotación sean pequeños, esta hipótesis se ha considerado ya en la deducción del modelo teórico y se ha comprobado que es correcta. La ejecución del experimento consiste en tomar una imagen de un objeto de longitud conocida, a una distancia también conocida y cumpliendo la simplificación expuesta anteriormente. Consideramos el triángulo naranja del modelo de la Ilustración 41, que ampliamos aquí: 32 Ilustración 43. Triángulo extraído del modelo Mediante trigonometría simple, establecemos una relación entre las 3 variables importantes: tan𝐹𝑂𝑉 2=𝑥 2∗𝑧 Ecuación 6. Ecuación modelo 1 Sabiendo además que la medida 𝑥 está representada en la imagen por 320 píxel podemos establecer la relación metros/píxel mediante la siguiente ecuación: 𝑚 𝑝í𝑥=2∗tan𝐹𝑂𝑉 2 320 ∗𝑧 Ecuación 7. Ecuación modelo 2 Queda así establecida la relación entre metros reales y píxeles de imagen que solo depende de la altura 𝑧, por último hemos de determinar el ángulo de visión (FOV) que es una característica de la cámara. Para esto tomamos varias imágenes en las condiciones especificadas y calculamos el valor: 𝐹𝑂𝑉=2∗arctan( 𝑥 2∗𝑧)≈62.5 º Ecuación 8. Ecuación modelo 3 Hecho esto tenemos un modelo sencillo y con la precisión suficiente para aplicarlo al quadrotor. A modo de ejemplo y validación mostramos aquí el resultado de aplicar el modelo calculado al ejemplo de trayectoria mostrado en la Ilustración 33, las características físicas del experimento fueron registradas en el momento de su ejecución con objeto de aplicar más tarde el modelo expuesto: 𝑭𝑶𝑽 𝟐 𝒛 (𝒂𝒍𝒕𝒖𝒓𝒂) 𝒙 𝟐 (𝒎𝒆𝒅𝒊𝒅𝒂 𝒄𝒐𝒏𝒐𝒄𝒊𝒅𝒂) 33 Ilustración 44. Validación del modelo con trayectoria ejemplo En rojo aparece la trayectoria ideal y en azul la trayectoria medida y escalada, al ser el experimento realizado a una altura determinada, el modelo únicamente realiza un escalado de los ejes para ajustarlos a las dimensiones reales. Durante la validación del modelo previamente expuesto, se detectó otro problema que afectaba a la medida: la influencia de la velocidad angular. La cámara se puede considerar como un observador ligado al sistema físico del quadrotor, por lo tanto mide la velocidad relativa del suelo respecto a sí misma, esta medida es una medida compuesta, incluye tanto la velocidad real en x o y del quadrotor, como la velocidad correspondiente al giro. Ilustración 45. Composición de velocidades en X Ecuación 9. Composición de velocidades en X Para evitar este error simplemente corregimos la medida del flujo óptico con las medidas de velocidad angular y altura provenientes de otros sensores (IMU y ultrasonidos o barómetro). En un primer 𝐹𝑂𝑥=−(𝑣𝑥+𝜔𝑦∗𝑧) 34 momento se intentó corregir con la medida de velocidad angular que proporciona la IMU, el resultado no fue bueno y por eso se pasó a utilizar la medida de orientación (ángulo) corrigiendo sus cambios. La extensión de esta corrección a puntos que no se encuentren inmediatamente debajo del quadrotor es directa ya que, para un giro Wy, todos los puntos que se encuentren en el plano del suelo tendrán una componente de velocidad en X del mismo módulo, y esta componente es lo que medimos con FO. Con las correcciones aplicadas podríamos, con el algoritmo general, obtener la posición del quadrotor respecto a un origen arbitrario (punto de arranque del programa) suponiendo un ángulo yaw constante mediante la ecuación de la posición expuesta anteriormente (Ecuación 4). Esto no tiene por qué ser así ya que el ángulo yaw puede variar, así pues modificamos la ecuación de la posición para adaptarla a un ángulo yaw variable del siguiente modo: 𝑃𝑂𝑆𝑁=∑𝐹𝑂(𝑐𝑜𝑟𝑟)𝑖 𝑁 𝑖=1 +(𝑥0 𝑦0)𝜓 𝑣𝑎𝑟𝑖𝑎𝑏𝑙𝑒 → 𝑃𝑂𝑆′𝑁=∑( cos𝜓𝑖sin𝜓𝑖 −sin𝜓𝑖cos𝜓𝑖)∗𝐹𝑂(𝑐𝑜𝑟𝑟)𝑖 𝑁 𝑖=1 +(𝑥0 𝑦0) Ecuación 10. Ecuación modificada de la posición (yaw variable) En esta ecuación simplemente se ha aplicado una rotación del sistema de referencia para que el resultado sea siempre la posición en coordenadas globales. Una vez aplicadas las correcciones descritas, podemos afirmar que la medida que obtenemos es una medida de posición absoluta en x-y del quadrotor. Ahora bien, como toda medida, presenta ruido, al cual además le hemos añadido el ruido presente en las medidas de altura y orientación al usar sus valores para las correcciones anteriores. Es por esto que la medida requiere un filtrado previo a su uso, con este fin implementamos un filtro paso bajo para intentar rechazar el ruido mediante la siguiente ecuación: 𝑥𝑘=𝑧𝑘∗𝛼+𝑥𝑘−1∗(1−𝛼) Ecuación 11. Filtro paso bajo Donde 𝑥𝑘 es la estimación de la posición en el instante k, 𝑧𝑘 es la medida de posición del sensor y 𝛼 es el parámetro del filtro, que puede variar entre 0 y 1, valores cercanos al 0 implican un filtrado muy fuerte, ignorando prácticamente al sensor, y valores cercanos al 1 anulan el filtrado dejando sin modificar la medida proveniente del sensor. Mostramos aquí medidas de posición y velocidad obtenidas mediante este filtrado, las medidas están en píxeles para tratar de forma independiente al filtrado y a las correcciones anteriores: 35 Ilustración 46. Gráfica de la posición X-Y (sin filtrar) Ilustración 47. Gráfica de velocidad filtrada en X (𝛼=0.05) Hemos adaptado en nuestro programa también un filtro de Kalman diseñado en trabajos anteriores por los compañeros del laboratorio. El fundamento de este algoritmo se basa en generar un modelo de nuestro sistema que utilizamos para predecir la siguiente medida y, una vez tomada, la medida real se utiliza para corregir el modelo generado. Ilustración 48. Diagrama del filtro de Kalman En la imagen aparece el funcionamiento del filtro, los parámetros que interesan son las entradas y salidas. La entrada 𝑧𝑘 es la medida proveniente del sensor, y la salida 𝑥𝑘 es la estimación del estado de la variable intentando minimizar el error de medida. Las entradas 𝑄𝑘 y 𝑅𝑘 son las matrices de covarianzas de los ruidos presentes en el proceso y en el sensor respectivamente, y determinan el peso que se le dará a las diferentes variables y mediciones en el modelo creado, a mayor covarianza, menor peso. -50 0 50 100 150 200 -300 -200 -100 0 Pos Y (píx) Pos X (píx) -100 -80 -60 -40 -20 0 20 40 60 80 -40 60 160 260 Velocidad X (pix/s) Iteraciones 36 2.4.3. Desarrollo del algoritmo de control Una vez tomada la medida de posición, se pasa al desarrollo del control, para poder controlar la posición, acudimos al modelo teórico (Ecuación 1) y observamos que no tenemos un control directo de la posición (o de la aceleración) sino que hemos de diseñarlo pasando por el control de orientación. Explicaremos el algoritmo de control para el eje X y el ángulo pitch pero la extensión al eje Y y el ángulo roll sería inmediata (si consideramos ángulos pequeños). Partimos del control de la orientación, que tiene la siguiente forma: Ilustración 49. Diagrama de flujo del control de orientación Donde ref 𝜃 es la referencia de pitch, 𝑢𝜃 es la acción aplicada (tal y como aparece en el modelo teórico) y 𝜃󰇘 es la respuesta dinámica del sistema, a la cual añadimos posibles perturbaciones no presentes en el modelo teórico (viento, choques,…) y que luego es captada (tras un doble integrador) por el sensor y retroalimentada al sistema de control. El control que proponemos para la posición X es el siguiente: Ilustración 50. Diagrama de flujo del control de la posición en X Como se observa en el diagrama, se mantiene el control de orientación original al cual se superpone un control de posición a través de la variable ref 𝜃𝑥 que hace la función de “actuación” indirecta sobre el sistema quadrotor. En las pruebas realizadas con el control de posición, los resultados obtenidos no fueron buenos, el control era inestable, y el quadrotor se movía de forma errática, reaccionando a las medidas de posición pero sin lograr controlar estas variables, se llegó a la conclusión de que un control de posición actuaba con demasiado retardo. También se observó que el quadrotor empezaba a tener sustentación suficiente para volar con los motores a un 75% de su potencia máxima, esto hacía que el control no fuese fiable porque se llegaba 𝑃𝐼𝐷𝜃 𝑠𝑖𝑠𝑡𝑒𝑚𝑎 𝑞𝑢𝑎𝑑𝑟𝑜𝑡𝑜𝑟 𝑠𝑒𝑛𝑠𝑜𝑟 (𝐼𝑀𝑈) 1 𝑠2 ref 𝜃 pert 𝜃󰇘 𝜃 𝜃󰇘 𝑢𝜃 𝜃󰇘 − + 𝑃𝐼𝐷𝜃 𝑠𝑖𝑠𝑡𝑒𝑚𝑎 𝑞𝑢𝑎𝑑𝑟𝑜𝑡𝑜𝑟 𝑠𝑒𝑛𝑠𝑜𝑟 (𝐼𝑀𝑈) 1 𝑠2 𝑠𝑒𝑛𝑠𝑜𝑟 (𝐶𝑎𝑚) 1 𝑠2 𝑃𝐼𝐷𝑥 ref 𝜃𝑥 pert 𝜃󰇘 𝜃 𝜃󰇘 𝑢𝜃 𝜃󰇘 − 𝑥󰇘 𝑥 ref 𝜃 ref 𝑥 − + + + pert 𝑥󰇘 𝑥󰇘 37 al límite de potencia al sumar la acción de control, así que se cambiaron los motores por otros que permitían sustentar el quadrotor a sólo un 60% de potencia, dando margen suficiente para la acción de control. En base a los resultados del control de posición, se decidió pasar a un control de velocidad utilizando como medida el flujo óptico dividido entre el periodo entre imágenes. Se ha fusionado la medida de velocidad proveniente de la cámara con la velocidad integrada de la aceleración de la IMU en un filtro de Kalman. Mostramos aquí una medida tomada en vuelo con el algoritmo de control de la velocidad, en azul se muestra la velocidad en el eje X, y en rojo, la referencia de pitch enviada al control de orientación: Ilustración 51. Resultados de la acción proporcional en vuelo En el vuelo mostrado en la gráfica sólo se aplicó un control proporcional para comprobar el correcto comportamiento y respuesta a la medida. Los resultados son satisfactorios ya que se consigue mantener la velocidad en X del quadrotor en un rango de ±0.3 m/s. La acción derivativa se implementó más tarde pues la medida de aceleración de la IMU presentaba mucho ruido, mostramos aquí el resultado del filtro paso bajo aplicado a la aceleración: 38 Ilustración 52. Filtrado de la aceleración Aun así, tras el filtrado, la acción derivativa desestabilizaba el quadrotor y se intentó implementar un control PI, con antiwindup, con la finalidad de evitar la deriva a velocidades lentas. Que tampoco funcionó por la misma razón, creaba inestabilidad en el quadrotor. Se observó durante estas pruebas también un grave retardo (ver gráfica inferior) en la medida obtenida de la cámara a intervalos regulares (partes planas de la gráfica azul). Esto dificultaba gravemente el control por dejar de tener medida por periodos de 2-3 segundos, lo cual distorsionaba mucho el modelo elaborado por el filtro de Kalman (gráfica roja). Una vez solucionado este cuello de botella relacionado con la escritura en ficheros se procedió a volver a intentar realizar el control. Ilustración 53. Retardo en la medida de visión Los resultados del control aplicado finalmente se muestran en el apartado siguiente. 39 3. Resultados y conclusiones Los resultados obtenidos son los siguientes: Ilustración 54. Medida final de velocidad La gráfica es el resultado de un control de velocidad proporcional pero sin retardo en la medida de visión, las acciones proporcional y derivativa no se pudieron probar por falta de tiempo. Como se ve, el sistema tiende a la velocidad 0 aunque con oscilaciones por la simplicidad del control proporcional. Los experimentos verifican que el sistema funciona aceptablemente en interiores y que podría ser aplicado como mecanismo de emergencia (en caso de fallo de la señal gps) en exteriores para evitar la pérdida del quadrotor Tras la realización de este trabajo, concluimos que la visión artificial es una herramienta potente y polivalente tal y como hemos visto puesto que permite calcular posiciones, velocidades e incluso la orientación del quadrotor. Pero esta polivalencia tiene un precio, que pagamos en las dificultades de obtener una medida precisa y aislada, debido a los parámetros de resolución, velocidad y ruido de la cámara y a las múltiples interacciones con otras variables causadas por la naturaleza del sistema de visión empleado. Mencionamos aquí algunas de las potenciales mejoras que se podrían aplicar a este sistema, las razones de que no se hayan aplicado en la ejecución del trabajo son principalmente la limitación de recursos y tiempo:  Inclusión de una segunda cámara (estereovisión) para dejar de depender de sensores externos.  Utilizar una cámara (o más) externa al quadrotor, eliminando así el carácter relativo de la medida.  Para el sistema con la cámara integrada en el quadrotor, añadir un sistema de estabilización dinámica para evitar interacciones con la velocidad angular. 46 gettimeofday(&tve, NULL); //calculamos la diferencia de tiempos entre timersub(&tve, &tvs, &tvd); //capturas, esta parte se puede ejecutar de gettimeofday(&tvs, NULL); //forma cíclica periodo=((float)tvd.tv_sec+1e-6*(float)tvd.tv_usec); cvtColor(imagen2,imagen2,CV_BGR2GRAY);//pasamos la imagen a escala de grises //dentro de este “if” se seleccionan los puntos de interés, “reseteador” es //una variable que se utiliza para decidir si se deben buscar puntos o no if (reseteador==1){ //reseteador = 1 indica que se deben buscar puntos corners1.clear(); //se limpian los puntos anteriores //se buscan los puntos y se convierten a un fomato adecuado para su //uso (la función ORB devuelve más datos de los necesarios) OrbFeatureDetector det(100,1.2f,8,31,0,2,0,31); det.detect(imagen1,keypoints) for (i=0;i<int(keypoints.size());i++){ corners1.push_back(keypoints[i].pt); } } if (corners1.size()>0){ reseteador = 0; //si se han encontrado puntos (el vector corners1 no //está vacío), se indica mediante reseteador que no //hace falta buscar puntos //Función de cálculo del flujo óptico calcOpticalFlowPyrLK(imagen1,imagen2, //imágenes previa y actual corners1,corners2, //vectores de puntos previos y actuales, //corners2 es la salida de esta función status,err, //status indica los puntos encontrados //err indica el error de posición Size(15,15),3, //Size(x,y) indica el tamaño de la ventana //de búsqueda y el 3 es el número de pirámides //criterios de terminación: 20 iteraciones o error inferior a 0.3 TermCriteria(TermCriteria::COUNT+TermCriteria::EPS,20,0.3)); ofx=0; //inicializamos variables de desplazamiento a 0 47 ofy=0; nb_depl=0; //este bucle calcula la media de los desplazamientos de los píxel for(i=0;i<int(corners1.size());i++) { if(status[i]!=0) { ofx = ofx + (corners2[i].x-corners1[i].x); ofy = ofy + (corners2[i].y-corners1[i].y); nb_depl++; } } //aquí se comprueba si no se han perdido todo los puntos if (nb_depl>0){ ofx=ofx/nb_depl; ofy=ofy/nb_depl; } //calculamos la velocidad (derivada de la posición) filtrada para //la acción derivativa del control velx = (1-0.05)*velx + 0.05*(ofx/periodo); vely = (1-0.05)*vely + 0.05*(ofy/periodo); //condición de reset, que se pierda más de un 75% de los puntos if(float(nb_depl)/float(corners1.size())<0.25) reseteador=1; //actualización de imágenes y vectores imagen2.copyTo(imagen1); std::swap(corners2, corners1); //acumulación del desplazamiento para obtener posición suma[0] += ofx; suma[1] += ofy; //impresión a fichero 48 fflush(fichflujo); fprintf(fichflujo,"%.6f\t %+.5f\t%+.5f\n",periodo,suma[0],suma[1]); } else imagen2.copyTo(imagen1); //en caso que no se encuentren puntos, //simplemente se actualiza la imagen } Código del algoritmo específico de medición (algoritmo de círculos) Este código está basado en el código de ejemplo presente en la página web: http://docs.opencv.org/doc/tutorials/imgproc/imgtrans/hough_circle/hough_circle.html Inicialización: VideoCapture cam(0); cam.set(CV_CAP_PROP_FRAME_HEIGHT,320); cam.set(CV_CAP_PROP_FRAME_WIDTH,240); Bucle principal: while(1){ //toma de imagen y tratamiento de la misma (escala de grises y desenfoque) cam >> src; cvtColor( src, src_gray, CV_BGR2GRAY ); GaussianBlur( src_gray, src_gray, Size(9, 9), 2, 2 ); //aplicación de la transformada de Hough para encontrar círculos HoughCircles( src_gray, //imagen circles, //vector de centros y radios CV_HOUGH_GRADIENT, //método utilizado para la detección 1, //proporción inversa de resolución src_gray.rows/8, //distancia mínima entre centros 200, //margen para detección de círculos 100, //margen para el filtro canny (detección de bordes) 0,0); //radios minimo y máximo de los círculos //adaptación de los datos al formato de uso if(int(circles.size())>0){ for(i=0;i<int(circles.size());i++){ Point center(cvRound(circles[i][0]), cvRound(circles[i][1])); centros[i]=center; } //si se encuentran 2 círculos, calcular la inclinación y el centro 49 if (circles.size()==2){ float yaw; yaw=atan(float(centros[1].y-centros[0].y)/float(centros[1].xcentros[0].x)); fflush(fichflujo); fprintf(fichflujo,"%f %f %f\n", (centros[1].x+centros[0].x)*0.5-160, -(centros[1].y+centros[0].y)*0.5+120, yaw*180.0/PI); } //si se encuentra solo un círculo, calcular la posición de este if (circles.size()==1){ fflush(fichflujo); fprintf(fichflujo,"%d %d\n",centros[0].x-160,-centros[0].y+120); } } Estos algoritmos han sido implementados como threads que se lanzan desde un objeto dedicado para adecuarse a la estructura del programa principal del quadrotor. Código del algoritmo específico de medición (algoritmo de color) Este código está basado en el código de ejemplo presente en la página web: http://opencv-srf.blogspot.com.es/2010/09/object-detection-using-color-seperation.html Por ser muy similar al algoritmo de círculos se explicarán solo aquellas funciones que no hayan sido explicadas ya. Inicialización: VideoCapture cam(0); cam.set(CV_CAP_PROP_FRAME_HEIGHT,320); cam.set(CV_CAP_PROP_FRAME_WIDTH,240); Bucle principal: while(1){ cam >> src; gettimeofday(&tve, NULL); timersub(&tve, &tvs, &tvd); gettimeofday(&tvs, NULL); mediaper=((float)tvd.tv_sec+1e-6*(float)tvd.tv_usec); cvtColor(src, src, COLOR_BGR2HSV); //pasamos la imagen a HSV inRange(src, //imagen origen Scalar(iLowH, iLowS, iLowV), //límite inferior del rango 50 Scalar(iHighH, iHighS, iHighV), //límite superior del rango src); //imagen destino (binaria) //tratamiento de la imagen (eliminar ruido) erode(src, src, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) ); dilate( src, src, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) ); dilate( src, src, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) ); erode(src, src, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) ); //cálculo de momentos Moments oMoments = moments(src); double dM01 = oMoments.m01; double dM10 = oMoments.m10; double dArea = oMoments.m00; //si se encuentra algún elemento de área suficiente, calcular su centro if (dArea > 100) { posX = -(dM10 / dArea)+160; posY = (dM01 / dArea)-120; } } Estos algoritmos han sido implementados como threads que se lanzan desde un objeto dedicado para adecuarse a la estructura del programa principal del quadrotor. Modificaciones para adaptar la medida //adaptación de píxel a metros (coefficient = 0.00379259) MeasureInMetres = MeasureInPixels*coefficient*altura; //corrección para rechazar medidas falsas por giros Measure = Measure – sin(Angle)*altura; Filtrado //implementación del filtro paso bajo MeasureFiltered = alfa*Measure + (1-alfa)*PreviousMeasure; No se incluye el código del filtro de Kalman porque ha sido desarrollado principalmente por Alberto Castillo, compañero del laboratorio, y no procede anexarlo aquí. Solo mencionar que este filtro ejecuta una fusión de datos combinando las medidas de la cámara y la IMU, consiguiendo así una medida más fiable y precisa. 51 Control El control PID, ya sea de posición o velocidad se ha implementado del siguiente modo: accPx = kPx * (refx – x); //proporcional x accDx = kDx * (-dx); //derivativa x accIx = accIx + kIx * (refx – x); //integral x refPitchX = accP + accD + accI; //acción total x accPy = kPy * (refy – y); //proporcional y accDy = kDy * (-dy); //derivativa y accIy = accIy + kIy * (refy – y); //integral y refRollY = accPy + accDy + accIy; //acción total y refPitchX = saturacion(refPitchX,-5,5); //saturamos las acciones tanto en refRollY = saturacion(refRollY,-5,5); //x como en y para evitar peligro