Sistema de seguimiento de objetos usando OpenCv, ArUco y Filtro de Kalman extendido

Equation Chapter 1 Section 1 Proyecto Fin de Grado Grado en Ingeniería Electrónia, Robótica y Mecatrónica Sistema de seguimiento de objetos usando OpenCv, ArUco y Filtro de Kalman extendido Autor: Rafael Jiménez Bravo Tutores: José Ramiro Martínez de Dios Alejandro Suárez Fernández-Miranda Dep. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2018 iii Proyecto Fin de Grado Grado en Ingeniería Electrónia, Robótica y Mecatrónica Sistema de seguimiento de objetos usando OpenCv, ArUco y Filtro de Kalman extendido Autor: Rafael Jiménez Bravo Tutores: José Ramiro Martínez de Dios Alejandro Suárez Fernández-Miranda Dpto Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2018 v Proyecto Fin de Carrera: Sistema de seguimiento de objetos usando OpenCv, ArUco y Filtro de Kalman extendido Autor: Rafael Jiménez Bravo Tutores: José Ramiro Martínez de Dios Alejandro Suárez Fernández-Miranda 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 El secretario del Tribunal vii A mi familia A mi pareja A mis compañeros ix Agradecimientos Quisiera expresar mi mas sentido agradecimiento a todas aquellas personas que me han ayudado a alcanzar el objetivo. En primer lugar, a mi familia, que me ha ofrecido su apoyo incondicional desde el primer momento, y sin cuyo esfuerzo no podría haber llegado tan lejos. A mis amigos, por esos despropósitos capaces de relajar los momentos de mayor presión. A mi pareja, sostén fundamental, por soportarme en mis peores momentos y aguantarme en los mejores. A mi tutor, por la paciencia que ha demostrado conmigo. A mis profesores, por compartir sus conocimientos. Por último, agradecer los buenos momentos compartidos con los distintos compañeros de clase con los que he convivido durante este largo camino, con los cuales, hasta en los momentos más dificiles sacábamos hueco para echarnos unas risas. Rafael Jiménez Bravo Sevilla, 2018 ÍNDICE DE TABLAS Tabla 6-1: Notación 62 Tabla 7-1. Medidas estadísticas experimento I 72 Tabla 7-2. Medidas estadísticas experimento II 74 Tabla 7-3. Medidas estadísticas experimento III 76 xvii ÍNDICE DE FIGURAS Figura 1-1. Logitech C525 23 Figura 1-2. Logitech C920 23 Figura 1-3. Marcador ArUco 23 Figura 1-4. Idea fundamental implementada 24 En la Figura 1-5 se puede observar un prototipo del sistema ideado: 24 Figura 1-6. Prototipo del equipo empleado 24 Figura 2-1. Conceptos generales imagen 26 Figura 2-2. Diversas radiaciones que componen la luz blanca 27 Figura 2-3. Modelos aditivos 27 Figura 2-4. Modelos sustractivos 27 Figura 2-5. Modelos normalizados para la representación del color 27 Figura 2-6. Ejemplo histograma 28 Figura 2-7. Logo OpenCV 29 Figura 2-8. Algoritmo CAMShift 30 Figura 2-9. Diagrama de bloques del algoritmo CAMShift 31 Figura 2-10. Ejemplo marcador estandar 32 Figura 2-11. Ejemplo marcador cerrado 32 Figura 2-12. Fases ejecutadas para detectar los marcadores de ArUco 33 Figura 2-13. Imágenes tomadas para la calibración 34 Figura 2-14. Estimación de la posición de una cámara 34 Figura 2-15. Estimación de la posición en un mapa 34 Figura 2-16. Ogre ArUco 35 Figura 2-17. Fases de predicción y corrección 37 Figura 3-1. Secuencia de bloques 40 Figura 4-1. Esquema Tracking 44 Figura 4-2. Background detection 45 Figura 4-3. Selección objetivo 46 Figura 4-4. Resultado background-detection al mover un brazo 46 Figura 4-5. Seguimiento objetivo 47 Figura 4-6. Esquema de bloques del algoritmo de tracking 49 Figura 4-7. Ejemplo redetección. Cuadrante resultante: 1-3-3 50 Figura 5-1. Ejemplo de distorsión 53 Figura 5-2. Imágenes tomadas para la calibración 54 Figura 5-3. Archivo .yml obtenido tras la calibración 54 Figura 5-4. Problema de ambigüedad en la estimación de posición. 55 Figura 5-5. Transformada T coordenadas homogeneas 55 Figura 5-6. Sistema de referencia generado en el marcador 56 Figura 5-7. Definición punto v 57 Figura 5-8. Definición de los vectores 𝑣𝑝 y 𝑣𝑟 57 Figura 5-9. Definición vector 𝑣𝑟𝑟 58 Figura 5-10. Definición vector u 58 Figura 6-1. Estimación cooperativa de posición 62 Figura 6-2. Modelo pinhole 63 Figura 6-3. Modelo pinhole 2D 63 Figura 7-1. Representación 3D para el primer caso de estimación de la posición del objetivo en condiciones estáticas 71 Figura 7-2. Diferentes vistas procedentes de la figura 7-1 71 Figura 7-3. Representación de los valores estimados para cada eje pertenecientes al primer experimento 72 Figura 7-4. Representación 3D para el segundo caso de estimación de la posición del objetivo en condiciones estáticas 73 Figura 7-5. Representación de los valores estimados para cada eje pertenecientes al segundo experimento 74 Figura 7-6. Representación 3D para el tercer caso de estimación de la posición del objetivo en condiciones estáticas 75 Figura 7-7. Representación de los valores estimados para cada eje pertenecientes al tercer experimento 76 Figura 7-8. Representación de los valores estimados para cada eje para el experimento en el que la posición del objetivo oscila en la dirección del eje X 77 Figura 7-9. Representación de los valores estimados para cada eje para el experimento en el que la posición del objetivo oscila en la dirección del eje Y 78 Figura 7-10. Representación 3D para el caso en el que el móvil realiza un movimiento helicoidal 79 Figura 7-11. Vista de pájaro movimiento helicoidal 79 Figura 7-12. Representación de los valores estimados para cada eje en el experimento en el que el móvil realiza un movimiento helicoidal 80 xix Notación RGB Red, Green, Blue (Rojo, Verde ,Azul) CMY Cyan, Magenta, Yellow (Cian, Magenta y Amarillo) HSV Hue, Saturation, Value (Matiz, Saturación, Valor) 𝑚𝑖𝑗 Momento de orden i,j 𝑋𝑐 Componente x del centro de gravedad del objetivo 𝑌𝑐 Componente y del centro de gravedad del objetivo Ɲ(µ,𝜎) Normal de media µ y varianza 𝜎 sin Función seno cos Función coseno 𝑹 Matriz de rotación de Euler × Producto vectorial {𝐸} Sistema de referencia de la Tierra. 𝒓𝑻 𝑬 Posición del objetivo respecto al sistema de referencia {E} 𝒓 𝑬𝑪𝑨𝑴𝒋 Posición de la cámara respeto al sistema de referencia {E} 𝒓𝑻 𝑪𝑨𝑴𝒋 Posición del objetivo respecto del sistema de referencia de la cámara 𝑹𝑪𝑨𝑴 𝑬 Rotación del sistema de referencia de la cámara respecto al principal 𝒓𝑻,𝒊 𝑪𝑨𝑴𝒋 Componente i-ésima de la posición del objetivo respecto del sistema de referencia de la cámara 1 INTRODUCCIÓN a evolución de la electrónica y las comunicaciones en las últimas décadas ha permitido que las tecnologías de la información se instauren en diferentes ámbitos de la vida cotidiana. En esta línea, una parte importante de los esfuerzos de los investigadores en los últimos años se ha centrado en la creación de “entornos inteligentes” en los que los usuarios pueden interaccionar de manera natural con los diferentes sistemas y servicios computacionales que les facilitan la realización de sus tareas diarias. Las aplicaciones de los espacios inteligentes son numerosas y se encuentran en auge en la actualidad, abarcando diferentes ámbitos tales como la robótica de servicios, tareas de vigilancia automática, ayudas a personas con discapacidad o los servicios domóticos. En este trabajo, se considera un espacio inteligente a un área física dotada de cámaras, ubicadas en posiciones fijas, que son controladas por un sistema de supervisión dotado de capacidad de análisis y toma de decisiones. Una de las tareas esenciales en los espacios inteligentes, cuando la actuación se realiza mediante robots, es la localización de los mismos. Existen diferentes alternativas que permiten la localización de los robots móviles usando cámaras externas. Las propuestas más importantes pueden dividirse en dos grupos dependiendo del conocimiento a priori de los robots que requieren. El primer grupo incluye las técnicas que precisan de un gran conocimiento previo de los robots, y usan marcas artificiales a bordo de los mismos . Por otro lado, el segundo grupo en el que se encuadra este trabajo incluye las alternativas que emplean únicamente la apariencia natural de los robots y la geometría de las cámaras para el posicionamiento. 1.1. Motivación y objetivo Actualmente es posible encontrar una amplia variedad de aplicaciones de vehículos multirotor, típicamente quadrotors o hexarotors. Este sistema de vehículo aéreo no tripulado (UAVS / RPAS / VANT) ofrece un gran número de posibilidades en muchos sectores, tanto comerciales como en materia de seguridad. Si bien desde hace algunas décadas las aeronaves no tripuladas han sido motivo de interés, en particular en el ámbito militar, no ha sido hasta los últimos años que han pasado de sistemas experimentales a equipos aptos para su uso profesional. Su actual capacidad de desarrollar misiones reales se ha visto difundida no solo en los ámbitos restringidos de los investigadores, fabricantes o usuarios afines a esta tecnología, sino que también ha sido dada a conocer, por diferentes medios , a la opinión pública general, que comienza a conocer su existencia L Los científicos estudian el mundo tal como es; los ingenieros crean el mundo que nunca ha sido - Theodore Von Karman- Introducción 22 y utilidad. El uso de multirotors ha supuesto una gran mejora en muchos puestos de trabajo, ya que se han cambiado las formas de realizar tareas que eran complicadas o de difícil acceso. Una excelente herramienta de trabajo que supone una revolución tecnológica y que avanza a pasos agigantados para ofrecer nuevas prestaciones. Para conseguir el correcto funcionamiento de estos en las diversas tareas a las que se deseen destinar, es necesario llevar a cabo multitud de experimentos y pruebas. Para ello es necesario contar con un sistema de seguimiento que permita conocer la posición de dicho dispositivo en cada instante de tiempo. En muchas aplicaciones es necesario conocer la posición del dron en el espacio para poder controlarlo, por ejemplo, en las maniobras de despegue y aterrizaje. El uso de sensores GPS está restringido principalmente a exteriores, en zonas de cobertura, y además ,el error de posicionamiento puede oscilar algunos metros. Por ello, resulta interesante el desarrollo de un sistema de posicionamiento para drones ad-hoc, que se pueda desplegar rápidamente para dar servicio de posicionamiento en un área determinada. Por último, el precio de estos sistemas no es asequibles para cualquier empresa o particular. Por lo tanto, la intención de este proyecto es desarrollar un sistema de bajo coste, que nos permita conocer la posición de un objeto móvil en cada instante de tiempo, de forma que asegure su correcto funcionamiento ante cambios de luminosidad en el entorno. 1.2. Estructura de este documento El resto del documento se organiza de la siguiente forma. Constará de siete capítulos independientemente de la introducción. En el primero de ellos, denominado Métodos para la estimación de posición basada en visión, se describen los fundamentos teóricos necesarios para el desarrollo de este sistema. Se detallarán conceptos generales sobre el tratamiento de imágenes. También, se presentará el filtro de Kalman extendido (EKF) y su aplicación a la estimación de posición basada en visión. En este capítulo, también se describirá el algoritmo primitivo proporionado por la librería OpenCV a partir del cual se ha diseñado el descrito en este documento. Por último, se detallarán las librerías empleadas. Tras esta descripción de conceptos básicos del capítulo 2, se expondrán los modulos de los que consta el sistema diseñado. Se especificará el funcionamiento de cada uno de los módulos y las distintas transiciones existentes entre ellos. En el capítulo 4, se desarrolla la explicación de como se ha resuelto los problemas de detección, seguimiento y redetección del objetivo tras pérdida. Tras detallar los distintos módulos del algoritmo de seguimiento, se definirá la solución adoptada para establecer un sistema de referencia global, y conocer así, la posición de cada cámara respecto a este. En el sexto capítulo, se explicará cómo se obtiene la posición del objetivo respecto al sistema de referencia global, a partir de la posición de las cámaras y la localización del objetivo determinada por el algoritmo de tracking en cada una de ellas. En el penúltimo capítulo, se muestran los resultados obtenidos en los diversos experimentos realizados, presentando las conclusiones en el capítulo 8, en el que también se recogen los posibles trabajos futuros. 1.3. Equipo Para poder conocer la posición del objetivo, es necesario contar con un equipo específico que permita desarrollar cada uno de los bloques de los que consta el sistema. Los elementos de los que consta éste son los siguientes: 1.1.1 Cámara Logitech HD Webcam C525 Se trata de una pequeña cámara con una resolución HD 720p, con enfoque automático de gama alta. Se ha empleado este módelo por tratarse de una cámara con una buena relación calidad-precio.A pesar de que se podría haber elegido otros modelos con una mayor resolución, que permitiesen obtener una mayor precisión y reducir las pérdidas del objetivo en condiciones de baja luminosidad, se ha decidido utilizar esta cámara porque sus características permiten realizar el seguimiento del objetivo en la mayoría de los casos. 1.1.2 Cámara Logitech HD Webcam C920 Se trata de un modelo más avanzado que el anterior. Está cámara puede alcanzar una resolución de hasta 1080p. Además, cuenta con enfoque automático al igual que el modelo anterior, una característica fundamental para el seguimiento de un objetivo móvil. Esta cámara tiene un precio superior, no obstante, es necesario contar con una cámara que proporcione imágenes con una mayor calidad para evitar la pérdida del objetivo en condiciones pésimas de luminosidad. 1.1.3 Marcador ArUco Dicho elemento permite conocer la orientación y posición relativa de cada una de las cámaras respecto a este. La librería ArUco permite interpretar este tag como un sistema de referencia, de forma que calibrando cada una de las cámaras es posible conocer la posición de cada de ellas. Existen numerosos tipos de marcadores, cada uno de ellos pertenecen a un diccionario. ArUco proporciona su propio diccionario. El diseño de un diccionario es importante ya que la idea es que sus marcadores deben de ser tan diferentes como sea posible. El diccionario aconsejado a usar por ArUco es ARUCO_MIP_36h12. Por lo tanto, el primer paso es descargar dicho diccionario e imprimir un marcador en papel. Ilustración 1 Ilustración 2 Figura 1-1. Logitech C525 Figura 1-3. Marcador ArUco Figura 1-2. Logitech C920 Introducción 24 1.1.4 Prototipo del Sistema El sistema ideado para conocer la posición de un objetivo se basa en realizar el seguimiento de este en cada una de las cámaras. El módulo de seguimiento será el encargado de proporcionar el pixel en el que se encuentra el objetivo para cada cámara. Posteriormente, conociendo la posición y orientación de cada una de ellas gracias al marcador de ArUco, será posible estimar la posición del móvil aplicando el filtro de Kalman Extendido. Estos pasos se ejecutan siguiendo el orden que aparece en la siguiente figura. Figura 1-4. Idea fundamental implementada Es posible observar como el módulo de posicionamiento solo se ejecuta al comienzo, por lo que las cámaras no podrán desplazarse de la posición inicial. Tras finalizar este proceso, se ejecutan de forma cíclica los bloques que implementan el seguimiento y el filtro de Kalman Extendido para cada una de las cámaras tal y como se puede apreciar en la Figura 1-4. En la Figura 1-5 se puede observar un prototipo del sistema ideado: Figura 1-6. Prototipo del equipo empleado Por último, resaltar que el marcardor de ArUco se debe de encontrar dentro del rango de visión de ambás cámaras. Métodos para la estimación de posición basada en visión 32 2.4 ArUco Aruco es una la librería de código abierto basada en OpenCV que permite detectar marcadores cuadrados de referencia en imágenes. Además, si la cámara esta calibrada es posible detectar la posición de la cámara respecto al marcador. La librería ha sido desarrollada empleando el lenguaje C++ . Existen multitud de tipos de marcadores, cada uno de ellos pertenecen a un diccionario. ArUco proporciona su propio diccionario optimizado. El diccionario aconsejado para estimar la posición de las cámaras por ArUco es ARUCO_MIP_36h12. 2.4.1 Marcadores Cada marcador esta delimitado por un borde exterior de color negro y una región interior a este que codifica un patrón binario. Este patrón binario es único e identifica cada marcador. Dependiendo del diccionario al que pertenezca dicho marcador, encontraremos marcadores con mas o menos bits. Cuantos más bits tenga el marcador menor será la posibilidad de confusión . Sin embargo, un mayor numero de bits significa disponer de una mayor resolución para una correcta detección. Los marcadores se pueden usar como puntos de referencia 3D para estimación de posición de la cámara. Denotamos como s el tamaño del marcador una vez que está impreso en un trozo de papel. La siguiente imagen muestra el sistema de coordenadas empleado. Figura 2-10. Ejemplo marcador estandar Existe un tipo de marcadores denominados enclosed markers, que permiten una localización de las esquinas de los marcadores más precisa. Esto permite una estimación de la posición más precisa. No obstante, la detección de este tipo de marcadores es ligeramente más compleja que en los marcadores simples. Figura 2-11. Ejemplo marcador cerrado 2.4.2 Proceso de detección Es importante conocer el procedimiento que sigue ArUco para detectar estos marcadores. Este consta de una serie de pasos que se explican a continuación: • En primer lugar, se aplica un umbral adaptativo hasta obtener los bordes del marcador, tal y como se puede observar en la imagen número 1 de la ilustración 14. • Tras encontrar los bordes se buscan los contornos del marcador. Una vez encontrados, no solo se detectan los contornos del marcador, sino que se detectan gran cantidad de bordes no deseados. El resto de los pasos se llevan a cabo con el objetivo de eliminar aquellos contornos no deseados. • El primer paso para acabar con los bordes no deseados es eliminar los bordes con un pequeño numero de puntos. El resultado se puede observar en la imagen número 2 de la ilustración 14. • A continuación, se lleva a cabo una aproximación poligonal de los contornos encontrados y se mantiene aquellos con cuatro esquinas. Se enumeran las esquinas en sentido contrario a las agujas del reloj. El resultado se puede observar en la imagen número 3 de la ilustración 14. • Se eliminan los rectángulos demasiados cercanos. El resultado se puede observar en la imagen número 4 de la ilustración 14. • Identificador del marcador: - Se elimina la perspectiva de proyección para obtener una vista frontal del área rectangular usando una homografía. - Se emplea el método de Otsu para calcular el umbral óptimo que separá el marcador del resto. Los algoritmos de Otsu asumen una distribución bimodal y encuentra el umbral que maximiza la varianza extra-clase manteniendo una baja varianza dentro de la clase. - Identificación del código interno. - Para los marcadores válidos, refina las esquinas mediante la interpolación de subpíxeles. Figura 2-12. Fases ejecutadas para detectar los marcadores de ArUco Métodos para la estimación de posición basada en visión 34 2.4.3 Aplicaciones Las distintas tareas que se pueden llevar a cabo con esta librería son las siguientes: • Calibración de cámaras a partir de una batería de imágenes tomadas sobre un tablero de calibración. Figura 2-13. Imágenes tomadas para la calibración • Estimación de la posición de una cámara respecto a un marcador. Para llevar a cabo esta tarea es necesario haber calibrado la cámara previamente. Figura 2-14. Estimación de la posición de una cámara • Estimación de la posición en un mapa a partir de la visión obtenida por la cámara de cada marcador. Figura 2-15. Estimación de la posición en un mapa • Aplicaciones de realidad aumentada Figura 2-16. Ogre ArUco Métodos para la estimación de posición basada en visión 36 2.5 Filtro de Kalman Extendido 2.5.1 Introducción El filtro de Kalman, aparecido a principios de la década de los sesenta, es de una importancia comparable a los trabajos realizados por Nyquist y Bode en la década de los veinte y los de Wiener en los años treinta. El filtro de Kalman permite estimar en tiempo real el vector de estado de un sistema dinámico lineal a partir de medidas ruidosas indirectas que de él se van tomando. Estas estimaciones en tiempo real del estado del sistema son valiosas en sí mismas cuando el sistema opera en lazo abierto; pero considerando la posibilidad de operación en lazo cerrado, las mismas pueden ser utilizadas para sintetizar una acción de control adecuada que lleve el sistema al estado deseado. Esta última posibilidad es la que motiva nuestro interés en el filtro. En el contexto de la teoría de control lineal, el filtro de Kalman ocupa un lugar central como observador de estado óptimo y en este contexto está bien entendida su utilización. No obstante, cada vez con más frecuencia, se manejan modelos no-lineales en ámbitos de aplicación tan diversos como la ingeniería, biología, ecología y economía. El uso de estos modelos más complejos se ha visto estimulado por el desarrollo tecnológico de la computación, que mediante el cálculo numérico ha facilitado el análisis de los mismos y la implementación de métodos de control también no-lineales. Resulta entonces natural buscar algún método de estimación de estado en tiempo real, que pueda aplicarse a sistemas no-lineales. Es donde aparece el filtro de Kalman extendido. Destacar, que para implementarlo se ha acudido al tutorial desarrollado por Gabriel A. Terejanu. 2.5.2 Notación y modelo Un sistema dinámico puede ser descrito por el modelo espacio estado de la siguiente manera: 𝑥𝑘=𝑓(𝑥𝑘−1)+𝑤𝑘−1, 𝑤𝑘−1 ~ Ɲ(0,𝑄𝑡) ( 2-2) 𝑧𝑘=ℎ(𝑥𝑘)+𝑣𝑘, 𝑣𝑘 ~ Ɲ(0,𝑅𝑡) ( 2-3) Las ecuaciones anteriores representan un sistema dinámico, donde: • La función f es un operador de transición que mapea el espacio de estado dentro del mismo espacio de estado. • La función h es un operador que mapea el espacio de estado dentro del espacio de observaciones. • 𝑥𝑘 denota al vector de estados desconocidos en un tiempo k. • 𝑤𝑘 es un error aleatorio de estimación del estado (incertidumbre en el modelo). • 𝑧𝑘 es el vector de observaciones. • 𝑣𝑘 es un error aleatorio de observación (incertidumbre en la medida). 2.5.3 Descripción del método El estado inicial, 𝑥0, es un vector aleatorio de media conocida µ0=𝐸[𝑥0] y covarianza 𝑃0= 𝐸[(𝑥0−µ0)(𝑥0−µ0)𝑇] El filtro de Kalman extendido (EKF) resuelve el problema de la estimación del estado 𝑥𝑘 generado por un sistema no lineal, utilizando la expansión de la serie de Taylor que aproxima las ecuaciones no lineales de estado y de observación, sobre el valor actual estimado del estado (𝑥𝑘 ); igualmente, proporciona una estimación de la varianza mínima del estado basado en la información estadística sobre el modelo. Se supone que el vector de ruidos es un proceso Gaussiano de media cero y matrices de varianza covarianzas dadas por: (𝑤𝑘𝑤𝑘𝑇)=𝑄𝑡 , 𝐸(𝑣𝑘𝑣𝑘𝑇)=𝑅𝑡 ( 2-4) 𝐸(𝑤𝑘)=0 , 𝐸(𝑣𝑘)=0 ( 2-5) 𝐸(𝑤𝑘𝑤𝐽𝑇)=0 , 𝐸(𝑣𝑘𝑣𝐽𝑇)=0,𝑝𝑎𝑟𝑎 𝑘 ≠𝑗 ( 2-6) 𝐸(𝑤𝑘𝑣𝑗𝑇)=0, para todo 𝑘 y 𝑗 ( 2-7) El proceso iterativo del filtro de Kalman se descompone en dos etapas: una primera fase de predicción del estado actual a partir del estado anterior y las ecuaciones dinámicas y una segunda fase de corrección de la predicción usando la observación del estado actual. Figura 2-17. Fases de predicción y corrección La fase de predicción lleva a cabo los siguientes pasos: • Predice el vector de estados (𝑥𝑘𝑓) a partir del vector de estados del instante anterior. 𝑥𝑘𝑓≈𝑓(𝑥𝑘−1 𝑎) ( 2-8) • Predice la matriz de covarianza del error (𝑃𝑘𝑓). Dicha matriz inicialmente se puede definir como la matriz identidad. 𝑃𝑘𝑓=𝐽𝑓(𝑥𝑘−1 𝑎)·𝑃𝑘−1 ·𝐽𝑓𝑇(𝑥𝑘−1 𝑎)+𝑄𝑘−1 ( 2-9) siendo 𝐽𝑓 el jacobiano de la función f, definido como: 𝐽𝑓= ( 𝑑𝑓1 𝑑𝑥1𝑑𝑓1 𝑑𝑥2⋯𝑑𝑓1 𝑑𝑥𝑛 ⋮ ⋱ ⋮ 𝑑𝑓𝑛 𝑑𝑥1𝑑𝑓𝑛 𝑑𝑥2⋯𝑑𝑓𝑛 𝑑𝑥𝑛 ) y 𝑄𝑘−1 una matriz de covarianza que modela el ruido asociado al modelo del sistema. Métodos para la estimación de posición basada en visión 38 Una vez llevada a cabo la primera fase, se procede a corregir el vector de estados y la matriz de covarianza predichos. La etapa de corrección consiste en los pasos: • La corrección comienza con el calculo de la ganancia, 𝐾𝑘 . 𝐾𝑘=𝑃𝑘𝑓·𝐽ℎ𝑇(𝑥𝑘𝑓)·(𝐽ℎ(𝑥𝑘𝑓)·𝑃𝑘𝑓·𝐽ℎ𝑇(𝑥𝑘𝑓)+𝑅𝑘)−1 ( 2-10) siendo 𝐽ℎ el jacobiano de la función h, definido como: 𝐽𝑓=(𝑑ℎ1 𝑑𝑥1𝑑ℎ1 𝑑𝑥2⋯𝑑ℎ1 𝑑𝑥𝑛 ⋮ ⋱ ⋮ 𝑑ℎ𝑛 𝑑𝑥1𝑑ℎ𝑛 𝑑𝑥2⋯𝑑ℎ𝑛 𝑑𝑥𝑛) y 𝑅𝑘 una matriz de covarianza que modela la incertidumbre asociada a las medidas. • Se calcula el vector de estados corregido, 𝑋𝑘𝑎 . 𝑋𝑘𝑎=𝑋𝑘𝑓+ 𝐾𝑘·( 𝑍𝑘−ℎ(𝑥𝐾𝑓) ) ( 2-11) • Por último, se corrige la matriz de covarianza del error, 𝑃𝑘 , predicha en la primera fase. 𝑃𝑘 =(𝐼 −𝐾𝑘·𝐽ℎ )·𝑃𝑘𝑓 ( 2-12) Descripción modular del sistema 40 3 DESCRIPCIÓN MODULAR DEL SISTEMA n este capítulo se describirá a alto nivel como se ha resuelto el problema propuesto. Se presentará cada uno de los módulos de los que consta el sistema, se hará una pequeña descripción de cada uno y se definirán las distintas transiciones entre todos ellos. En los capítulos siguientes se describirá con mas detalle cada uno de ellos. Tal y como se ha descrito anteriormente, el objetivo del proyecto es desarrollar un sistema que permita, tras haber seleccionado un objetivo, conocer la posición de este. Para ello se emplearán 2 cámaras que permitirán estimar la profundidad. Trabajar con dos cámaras hace que el esquema diseñado se complique, ya que hay que contemplar multitud de casos que aparecen cuando se trabajan con dos cámaras en tiempo real. En primer lugar, es necesario conocer como se ha de lanzar el programa encargado de estimar la posición del objeto seleccionado. El comando encargado de lanzar el programa es el siguiente: ./EstimationPose live:1 -c <ruta archivo calibración cámara 1> live:2 -c <ruta archivo calibración cámara 2> -s <tamaño marcador aruco en metros> Tras lanzarlo aparecerán una serie de ventanas, que se irán abriendo y cerrando en función del bloque que se este ejecutando. En todo momento, el terminal mostrará toda la información relevante en el proceso. El sistema constará de 5 bloques principales: configuración, inicialización, tracking, redetección y estimación cooperativa de posición. A continuación, se profundizará en cada uno de ellos. En el siguiente diagrama se puede apreciar el orden de ejecución, así como los parámetros o ficheros necesarios en cada bloque. Figura 3-1. Secuencia de bloques E 3.1 Configuración Permite conocer la posición de cada una de las cámaras respecto al marcador de ArUco. Este bloque únicamente se ejecuta al principio del programa, por lo que si se modifica la posición de las cámaras habría que ejecutarlo de nuevo. El final de este modulo lo marcará la pulsación de la tecla g, dando lugar a la ejecución del módulo de inicialización. Para llevar a cabo este proceso de configuración, es necesario haber calibrado cada una de las cámaras con antelación. Concretamente, se deben de calibrar empleando el método facilitado por esta librería, la cual proporciona un fichero de extensión .YML donde se almacenan los resultados obtenidos. En la ejecución del programa se deberá de añadir como argumento la ruta a cada uno de los archivos de calibración. Durante el tiempo en el que se ejectura este bloque se mostrarán 4 ventanas. Dos de ellas mostrarán las imágenes capturadas por cada una de las cámaras en cada iteración, apreciando el sistema de coordenadas generado en el marcador de ArUco. Las dos restantes mostrarán los contornos percibidos por cada cámara tras haber ejecutado todos los pasos descritos en la sección 2.4. Al pulsar la tecla ‘g’ , se cerrán estas ventanas y aparecerán otras cuatro, dos de ellas mostrarán de nuevo las imágenes capturadas por cada una de las cámaras, mientras que las otras dos mostrarán cuanto se ha modificado la imagen actual respecto a la capturada en la iteración anterior. 3.2 Inicialización Tras conocer la posición de cada cámara, el siguiente paso es seleccionar el objetivo en cada una de ellas. Para ello será necesario marcar una región en cada una de las ventanas denominadas ‘Tracking’ correspondientes a cada cámara. Se ha de tener en cuenta que la selección de las dos regiones no es simultanea. Durante el tiempo que transcurre desde que se selecciona una región en una ventana, hasta que se selecciona la correspondiente en la otra, pueden ocurrir diversos sucesos que se han de considerar. Por ejemplo, es posible que durante este tiempo el objetivo seleccionado se pierda , por lo que habría que activar la etapa de redetección para esta ventana, mientras que la segunda se encontraría aun en dicha etapa de inicialización. Otro caso posible, es que, tras seleccionar el objetivo en una de ellas, se active el modulo de tracking, mientras que la segunda aun este esperando la selección correspondiente. Por lo tanto, es posible que cada cámara se encuentre en una determinada fase del sistema. 3.3 Tracking El modulo encargado de realizar el seguimiento del objetivo es el módulo de tracking. Este constará de dos etapas, las cuales se explicarán con mayor profundidad en el siguiente capítulo. Como resumen, destacar que este bloque permite conocer la posición del objetivo combinando una versión avanzada del algoritmo CAMShift proporcionada por OpenCV y un algoritmo de extracción de fondo, desarrollado para eliminar aquellas regiones estáticas de la imagen para mejorar los resultados en exteriores. Tal y como ya se ha descrito en el apartado anterior, es posible que una cámara se encuentre ejecutando la primera fase del tracking, mientras que la otra ejecuta la segunda fase de este, o cualquier otro de los módulos( redetección o inicialización). 3.4 Redetección Es posible que el objetivo seleccionado salga del rango de visión de la cámara o que las condiciones lumínicas en la zona en la que se encuentra sean tan deficientes que no permitan su apreciación, en estos casos, el módulo de tracking dará paso al modulo de redetección, encargado de encontrar de nuevo el objetivo perdido. Aunque el objetivo solo se haya perdido en una cámara, este bloque no empleará la información proporcionada por la otra. El algoritmo desarrollado se basa en momentos estadísticos y en una división en rejillas de la imagen obtenida. Además, emplea el algoritmo de extracción de fondo comentado en el apartado anterior. Tras localizar el objetivo en la cámara, de nuevo se activará el módulo de tracking. Este bloque se describirá con mas detalles en el siguiente capítulo. Selección y detección, seguimiento y redetección del objetivo 48 48 procesada. Así, el valor de cada píxel de esta imagen identificará la probabilidad de que dicho píxel en la imagen procesada pertenezca al objeto. calcBackProject (hue channel, number of images, dimension, histogram, output matrix, range) Tras haber calculado la retroproyección se ejecuta la función encargada de desarrollar el algoritmo CAMShift. trackBox = CamShift (back-projection, tracking Window, stop criterion) Por último, se modifica la posición de la región del interés, asignándole el valor proporcionado por la función anterior y se dibuja la elipse en la posición calculada. Esta fase solo se ejecutará una vez tras haber seleccionado la ventana de búsqueda. 3.2.1. Fase seguimiento dinámico Tras haber cumplido la etapa de inicicialización del bloque de seguimiento, se desarrolla la segunda fase de este, donde se lleva a cabo el proceso de comparación del histograma de la imagen actual con el histórico de histogramas, y la apilación de este en función de la diferencia respecto a ellos. Esta fase denominada seguimiento dinámico se desarrollará cíclicamente en cada imagen tomada por la cámara, siempre y cuando el objetivo se encuentre dentro del rango de visión. El principio de funcionamiento de esta fase se basa en el calculo del histograma de la región en la que se encuentra el objetivo, calculada en el instante anterior. Para ello se hace uso de la máscara de movimiento generada en la primera fase, para descriminar aquellos pixeles en los que no haya movimiento. Tras calcular el histograma se lleva a cabo una comparación de este con cada componente que conforma el array en el que se encuentran almacenados los diferentes histogramas con los que se ha trabajado lo largo de la prueba. Se almacenará el valor más pequeño obtenido. En función de este, la acción ha realizar será una u otra: - Si es inferior al umbralApilacion no se incluye en el histórico de histogramas. - Si se encuentra comprendido entre los límites umbralApilacion y umbralPerdida se incluye en el histórico. - Si es superior al umbralPerdida se activa la fase de redetección. A continuación, se va a explicar con mayor detalle el procedimiento seguido: En primer lugar, se calcula la mascará que permitirá descriminar aquellos píxeles en los que no se han producido movimiento y aquellos en los que el valor de los canales Value y Saturation no se encuentran dentro de los límites establecidos(background detection). Tras esto, se calcula el histograma de la región de búsqueda actual( función calcHist() ) y se normaliza (normalize() ). El siguiente paso consiste en la inicialización de un array de diferencias en el que se va a a almacenar las diferencias entre la componente correspondiente del histórico de histogramas y el histograma actual. Tras haber inicializado dicho array, se recorre el histórico de histogramas, se comparan y se almacenan en el array de diferencias. Tras finalizar este proceso almacenamos la posición y el valor de la componente más pequeña. La comparación se lleva a cabo empleando la función de OpenCv compareHist(), que recibe como parámetros los dos histogramas a comparar y el método de computar la comparación: correlación, chi-cuadrado, intersección y distancia de Bhattacharyya. El método empleado ha sido este último. El siguiente paso es determinar que acción se va a desarrollar, tal y como se ha explicado anteriormente. En el caso de que sea inferior al líminte umbralApilacion se calculará la retroproyección de dicho histograma y se ejecutará la función encargada de desarrollar el algoritmo CAMShift. Si la mínima diferencia almacenada se encuentra entre ambos umbrales, la primera acción a realizar es comprobar si el número de histogramas apilados supera a la longitud del array. Esto es fundamental para evitar un error por desbordamiento de buffer 3 . En el caso de que el número de histogramas almacenados iguale al tamaño de este, se realiza un proceso de eliminación. El criterio tomado ha sido el de eliminación de las componentes más antiguas. Concretamente se elimina el primer 25 % de la dimensión máxima. Esto se lleva a cabo empleando la función histProm.erase(),la cual recibe el rango de componentes que se desean eliminar. histProm.erase(Componente inicial, Componente final) Tras la eliminación, desplaza el resto de las componentes a ocupar las primeras posiciones. El histograma actual se apila y se calcula la retroproyección de este. Finalmente, se ejecuta el algoritmo CAMShift y se dibuja la elipse en la región obtenida. Por último, existe la posibilidad de que el objeto se haya perdido. Esto será cuando la diferencia supere el umbral umbralPerdida. En este caso, no se ejecutará ningúna función, simplemente se modificará el valor del estado para que en la siguiente iteración comience el proceso de búsqueda. Este desarrollo se puede observar en la siguiente ilustración: Figura 4-6. Esquema de bloques del algoritmo de tracking 3 Un desbordamiento de búfer (del inglés buffer overflow o buffer overrun) es un error de software que se produce cuando un programa no controla adecuadamente la cantidad de datos que se copian sobre un área de memoria reservada a tal efecto (buffer): Si dicha cantidad es superior a la capacidad preasignada, los bytes sobrantes se almacenan en zonas de memoria adyacentes, sobrescribiendo su contenido original, que probablemente pertenecían a datos o código almacenados en memoria. Selección y detección, seguimiento y redetección del objetivo 50 50 4.3 Redetección En este apartado se desarrolla la explicación teórica relacionada con la detección del objetivo cuando se haya perdido, debido a una oclusión o salida del rango de visión de la cámara. Como ya se ha explicado en la sección anterior, esta fase se activa tras llevar a cabo el cálculo de la mínima diferencia entre el histograma actual y el histórico de histogramas. Es implementado mediante la función redeteccionObjeto(). La idea principal desarrollada en esta consiste en una división de la imagen en regiones, calculando el momento de orden cero de cada una de ellas. Se discriminan todas aquellas regiones con un momento de orden cero nulo, y el resto se compara con un momento de orden cero de referencia. Si la diferencia respecto a este se encuentra dentro de un cierto umbral se almacenará dicha región. A continuación, se repite este proceso subdividiendo la región anterior en otras más pequeñas de igual tamaño. Este proceso se repetirá 3 veces y cada una de las regiones se subdivide en otras 4 de igual tamaño. Respecto al momento de orden cero de referencia, decir que este viene dado por el momento de orden cero calculado en la región de interés, en la última iteración en la que el objetivo se encontraba dentro del rango de visión. Esto nos permite descartar aquellos objetos que entren dentro del campo de visión con un aspecto similar al del objetivo, pero de diferente tamaño. En la siguiente imagen es posible observar como se realiza la división de la imagen en rejillas para detectar el objeto perdido. Figura 4-7. Ejemplo redetección. Cuadrante resultante: 1-3-3 Por otro lado, el trabajar con 2 cámaras implica que la pérdida del objetivo se puede producir en una o en ambas cámaras, de forma que habría que contemplar multitud de posibilidades. En primer lugar, es posible que tras seleccionar el objetivo únicamente en una de las cámaras, este se pierda, activándose el proceso de redetección unicamente en dicha cámara, mientras que la otra se encuentra esperando la selección de un objetivo. Tambien, es posible que el objetivo se pierda en una de las cámaras mientras que en la otra se encuentra en la etapa de tracking, ya sea en la fase de inicializición de tracking o en la etapa cíclica de tracking. Todos estos casos se encuentran recogidos en la función redeteccionObjeto(). Estas posibilidades se han de tener en cuenta a la hora de calcular la estimación de la posición aplicando el filtro de Kalman. El EKF podría seguir integrando medidas de una única cámara, pero se sabe que el error de estimación crecerá con el tiempo debido a que el sensor de imagen sólo proporciona una medida 2D y se pierde la información de profundidad (caso estimación monocular). Por lo tanto, se ha decidido que se ejecute el módulo de estimación unicamente cuando el objetivo se encuentra identificado por ambas cámaras. Posicionamiento de las cámaras mediante ArUco 52 52 5 POSICIONAMIENTO DE LAS CÁMARAS MEDIANTE ARUCO n este apartado se va a desarrollar la explicación teórica vinculada al procedimiento empleado para conocer la posición y orientación de cada una de las cámaras. Este proceso es llevado a cabo en el bloque de configuración. Como ya se describió en el capítulo 3, únicamente se ejecuta al comienzo del programa. Permite ubicar las cámaras donde se deseen, siempre y cuando, el marcador de ArUco se ubique dentro del campo de visión de cada una de ellas. Tras haberlas situado, se ha de pulsar la tecla ‘g’ para seleccionar el objetivo deseado en cada una de las cámaras e iniciar la estimación de posición del objetivo. Es imprescindible, una correcta calibración de cada una de las cámaras para conseguir conocer con exactitud la posición y orientación de cada una de ellas. Tanto la calibración como la estimación de la posición y orientación de cada cámara se han conseguido haciendo uso de la librería ArUco. 5.1 Calibración 4.1.1 Concepto La calibración de una cámara es el proceso mediante el cual se obtienen los parámetros fundamentales de una cámara. Estos parámetros permiten determinar donde se proyecta un punto 3D del espacio en el sensor de la cámara. Los parámetros de esta se clasifican en intrínsecos y extrínsecos. Lo primeros, son aquellos que describen el funcionamiento de una cámara, mientras que los segundos, definen la posición y orientación del cuadro de referencia de la cámara respecto al mundo real, es decir, dan la orientación externa de la cámara. Los parámetros intrínsecos son: • 𝑓𝑥 𝑦 𝑓𝑦: Distancia focal de la lente de la cámara en ambos ejes. Normalmente expresados en píxeles. • 𝐶𝑥 𝑦 𝐶𝑦: Centro óptico del sensor. Expresado en píxeles. • 𝑘1,𝑘2,𝑝1,𝑝2,𝑘3: Coeficientes de distorsión. E En una cámara ideal, un punto (X, Y, Z) en el espacio se proyectaría en el píxel determinado por las siguientes ecuaciones: 𝑥=−𝑋∙𝑓𝑍+𝐶 𝑥 ( 5-1) 𝑦=−𝑌∙𝑓𝑍+𝐶 𝑦 ( 5-2) Sin embargo, las lentes de las cámaras distorsionan la escena, haciendo que la distancia de los diferentes puntos respecto al centro de la imagen varie. Por lo tanto, si se desea conocer con exactitud la proyección de los diferentes puntos en la imagen se debe de considerar estos coeficientes de distorsión. La distorsión se puede descomponer en dos componentes, una componente radial y otra tangencial, ambas dependientes del ángulo y de la distancia respecto al centro de la imagen. Todo esto se contempla en los coeficientes de distorsión calculados en la calibración (𝑘1,𝑘2,𝑝1, 𝑝2,𝑘3). Si se desea conocer la proyección de un punto referido a un sistema de referencia arbitrario es necesario hacer uso de los parámetros extrínsecos. Los parámetros extrínsecos son básicamente las rotaciones 3D (𝑅𝑣𝑒𝑐={𝑅𝑥, 𝑅𝑦, 𝑅𝑧}) y las translaciones (𝑇𝑣𝑒𝑐={𝑇𝑥, 𝑇𝑦, 𝑇𝑧}) necesarias para traducir el sistema de referencia de la cámara al arbitrario. También se pueden tener en cuenta cambios de escala y cambios de perspectiva. Una forma de llevar a cabo este cambio de sistema de referencia es haciendo uso de las coordenadas homogéneas. 4.1.1 Procedimiento ArUco permite llevar a cabo este proceso de calibración. En primer lugar, es necesario imprimir un tablero de calibración proporcionada por esta librería. Este tablero es el que se puede observar en la siguiente figura: Ilustración 8. Tablero de calibración Una vez impreso el tablero, es necesario medir con la mayor precisión posible la dimensión de los marcadores. Respecto a la unidad de medida, no es un dato relevante, no importa si la medida esta en centímetros, metros o milímetros. No obstante, el resultado final lo obtendremos en la unidad de medida en la que hayamos tomado esta. Una vez impreso el tablero y anotada la dimensión de los marcadores, se procede a tomar fotos de este desde Ilustración 7 Figura 5-1. Ejemplo de distorsión Posicionamiento de las cámaras mediante ArUco 54 54 diferentes localizaciones y perspectivas. Se tomará al menos 15 imágenes y se tratará de que se observen en todas ella el tablero completo. A continuación, se pueden apreciar algunos ejemplos: Figura 5-2. Imágenes tomadas para la calibración Tras haber tomado las fotos del tablero, se ejecutará el programa proporcionado por la librería denominado aruco_calibration_fromimages que se puede encontrar en la carpeta utils_calibration. Para ejecutar este programa se debe ejecutar el siguiente comando en el terminal de Ubuntu: Aruco_calibration_fromimages mycalibrationfile.yml pathToDirWithImage -size 0.03 El parámetro mycalibrationfile.yml es la salida del proceso. PathToDirWithImage es el directorio en el que se encuentran las diferentes imágenes tomadas para la calibración. El último parámetro es el tamaño de los marcadores del tablero. El fichero obtenido tiene el siguiente aspecto: Figura 5-3. Archivo .yml obtenido tras la calibración En la ilustración anterior es posible apreciar que mediante esta función proporcionada por ArUco es posible determinar los parámetros intrínsecos de la cámara(distancia focal en los ejes X e Y, y las coordenadas del centro óptico), al igual que los coeficientes de distorsión. 5.2 Posicionamiento Una vez calibrada la imagen se puede proceder a conocer la posición de la cámara respecto a un tag de ArUco. La detección de las cuatro esquinas de un marcador permite aplicar estimadores de pose planar. No obstante, la estimación de la pose que usa solo 4 puntos coplanarios está sujeta a ambigüedad. Como se muestra en la siguiente figura, un marcador podría proyectarse en los mismos píxeles en dos ubicaciones de cámara diferentes. En general, la ambigüedad puede resolverse si la cámara está cerca del marcador. Sin embargo, a medida que el marcador se hace pequeño, aumentan los errores en la estimación de la posición. Figura 5-4. Problema de ambigüedad en la estimación de posición. Cuando se conocen los parámetros intrínsecos de la cámara y la dimensión del marcador, es posible determinar la posición relativa del marcador respecto de la cámara. Este algoritmo permite obtener cuanto debe de girar y transladarse el sistema de referencia del marcador para hacerlo coincidente con los ejes de la cámara. Esta transformación viene dada por los vectores Tvec y Rvec. El primero de ellos es un vector de 3 componentes en las que se almacena la translación sobre cada uno de los ejes correspondientes. El segundo es otro vector de 3 componentes que define el giro al que hay que someter al sistema de referencia del marcador para hacerlo paralelo con respecto al de la cámara. Esta rotación viene definida por el método conocido como Rodrigues’ rotation, muy empleado en la librería OpenCv. Para conocer la posición de un punto respecto al sistema de referencia de la cámara será necesario llevar a cabo una transformación que permita expresar el punto referido al sistema de referencia del marcador en el sistema de referencia de la cámara. Para ello, se hará uso de las coordendas homogéneas. Figura 5-5. Transformada T coordenadas homogeneas Posicionamiento de las cámaras mediante ArUco 56 56 A continuación, es posible observar el sistema de referencia generado gracias a la librería ArUco y el código desarrollado: Figura 5-6. Sistema de referencia generado en el marcador 5.2.1 Rodrigues’ rotation En muchas areas de la robótica, la vision artificial es necesaria para rotar puntos u objetos unos determinados ángulos sobre sus ejes correspondientes. Una forma de hacer esto es usando la formula conocida como Rodrigues’ rotation. 𝑅=𝐼+[𝑛]𝑥sin𝛼+[𝑛]𝑥2(1 −cos𝛼) ( 5-3) Donde [𝑛]𝑥 es una inclinación simétrica del vector normalizado del eje de rotación y 𝛼 es el ´ngulo de rotación en radianes. El resultado (R) es una matriz de rotación 3x3. La forma más básica de representar rotaciones es mediante los ángulos de Euler, donde culquier rotación en el espacio es representada mediante tres rotaciones 2D en los planos xy, yz y xz. 𝑨= (𝑐𝑜𝑠 𝜑 𝑠𝑖𝑛𝜑 0 −𝑠𝑖𝑛 𝜑 𝑐𝑜𝑠𝜑 0 0 0 1) 𝑩= (1 0 0 0 𝑐𝑜𝑠µ 𝑠𝑖𝑛µ 0 −𝑠𝑖𝑛 µ 𝑐𝑜𝑠µ) 𝑪= (𝑐𝑜𝑠𝜃 0 −𝑠𝑖𝑛𝜃 0 1 0 𝑠𝑖𝑛𝜃 0 𝑐𝑜𝑠𝜃) La matriz de rotación, por tanto, es el resultado de multiplicar estas tres matrices. 𝑹=𝑨𝑩𝑪 ( 5-4) Es evidente que el resultado de la matriz de rotación depende del orden en el que se lleven a cabo las rotaciones, debido a que el producto de matrices no es conmutativo. Los ángulos de rotación de Euler son un método simple de entender y visualizar, pero no es práctico en una gran cantidad de aplicaciones. Rodrigues’ rotation supone una alternativa que soluciona estos problemas. A continuación, se desarrollará brevemente su fundamento teórico. Se parte de un punto V, el cual se desea rotar un Angulo α, alrededor de un vector normalizado n . Para hacer esto se define un vector v, que parte desde el origen y llega al punto V, y un plano en el origen y perpendicular al eje de rotación n. Figura 5-7. Definición punto v El Segundo paso es proyectar el vector v en el eje de rotación y en el plano definido en el origen. El vector proyectado sobre el eje de rotación n se denomina 𝑣𝑝 y el vector proyectado sobre el plano se denomina 𝑣𝑟 . 𝑣𝑝=(𝑛·𝑣)𝑛 ( 5-5) 𝑣𝑟=𝑣−𝑣𝑝 ( 5-6) Figura 5-8. Definición de los vectores 𝑣𝑝 y 𝑣𝑟 Una vez construidos los vectores anteriores, se define uno nuevo denominado w, perpendicular al plano conformado por los vectores v y 𝑣𝑝 . 𝑤=𝑛 𝑥 𝑣 ( 5-7) Se define un vector 𝑣𝑟𝑟 , generado al hacer girar un cierto ángulo α el vector 𝑣𝑟 en el plano conformado por los vectores 𝑣𝑟 y 𝑤. 𝑣𝑟𝑟=𝑣𝑟cosα+𝑤sinα ( 5-8) Estimación coopertiva de posición 64 64 6.2 EKF aplicado a la estimación cooperativa de posición En el capítulo 2, se ha desarrollado el Filtro de Kalman Extendido de forma teórica para un caso genérico. En este caso, se profundizará en él, pero orientado a resolver el problema de la estimación cooperativa de posición. Partiendo de la base teórica proporcionada en el capítulo dos, el filtro de Kalman extendido consta de una fase de inicialización, que se ejecuta únicamente la primera vez, una fase de predicción y otra de corrección, siendo estas dos últimas las que se ejecutan en el resto de las iteraciones. Cómo ya se comentó en el capítulo 2, las ecuaciones dinámicas que modelan el sistema son las siguientes: 𝑋𝑘=𝑓(𝑋𝑘−1)+𝑊𝑘−1 ( 6-3) 𝑍𝑘=ℎ(𝑋𝑘)+𝑉𝑘 ( 6-4) Podemos identificar los siguientes vectores en las ecuaciones anteriores: • 𝑋𝑘: Vector de estado, definido por la posición y velocidad del objetivo. 𝑋𝑘= ( 𝑋𝑌𝑍 𝑉𝑋 𝑉𝑦 𝑉𝑧 ) • 𝑊𝑘−1: Vector que modeliza el ruido asociado al modelo del sistema. Se trata de un vector aleatorio gaussiano blanco de media cero y matriz de covarianza 𝑄𝑘. 𝑊𝑘−1= ( 𝜎𝑥 𝜎𝑦 𝜎𝑧 𝜎𝑉𝑥 𝜎𝑉𝑦 𝜎𝑉𝑧 ) • 𝑍𝑘 : Vector de observación, definido por la posición del objeto proporcionada por el algoritmo de tracking. 𝑍𝑘=(𝐶𝑥 𝐶𝑦) • 𝑉𝑘: Vector aleatorio que modeliza la incertidumbre asociada a la medida. Se trata de un vector aleatorio gaussiano blanco de media cero y matriz de covarianza 𝑅𝑘. 𝑉𝑘=(𝜎𝐶𝑥 𝜎𝐶𝑦) Para implementar las distintas fases del Filtro de Kalman extendido se ha desarrollado la función updateEstimation , que recibe los parámetros: • Posición de la cámara respecto al tag de ArUco (dada por ArUco). • Rotación de la cámara respecto al tag de ArUco (dada por ArUco). • Posición del objetivo obtenida por la cámara (Se obtiene una medida en pixeles que se debe pasar a cm utilizando los datos de la cámara). • ID de la cámara. A continuación, se procederá a describir como se ha implementado el EKF para nuestro sistema de seguimiento. Se describirá cada una de las fases y los valores tomados para cada una de las variables existentes. Fase inicialización En primer lugar, es necesarior inicializar el vector de estado inicial, 𝑋0𝑎. Este debe ser un vector aleatorio de media µ0 y covarianza 𝑃0. En este caso, el vector de estados inicial se ha inicializado con los siguientes valores: 𝑋0𝑎= ( 00 0.5 0.1 0.1 0.1 ) Además, también es necesario inicializar la matriz de covarianza del error en el instante anterior ( 𝑃𝑘−1) . Se le ha asignado la matriz identidad: 𝑃𝑘−1= ( 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 ) Fase de predicción Los pasos que seguir dentro de dicha función serán los siguientes: En primer lugar, se calcula el vector de estados predicho: 𝑋𝑓𝑜𝑟𝑒𝑐𝑎𝑠𝑡,𝑘= ( 𝑋𝑌𝑍 𝑉𝑋,𝑘 𝑉𝑦,𝑘 𝑉𝑧,𝑘 ) = ( 𝑋𝑘−1+𝛥𝑡·𝑉𝑥 𝑌𝑘−1+𝛥𝑡·𝑉𝑦 𝑍𝑘−1+𝛥𝑡·𝑉𝑧 𝑉𝑥,𝑘−1 𝑉𝑦,𝑘−1 𝑉𝑧,𝑘−1 ) Se va a suponer que 𝛥𝑡 es pequeño y , que, por lo tanto , se puede utilizar un modelo del sistema lineal. La velocidad se considera constante en los distintos ejes. Para calcular el incremento de tiempo, cada vez que se acceda a la función, se almacenará el instante de tiempo en el que se ejecuta, de forma que se almacenará en la variable t_anterior el instante de tiempo en el que se ejecutó por última vez el EKF. De esta forma es posible conocer el incremento de tiempo entre cada iteración. Estimación coopertiva de posición 66 66 En segundo lugar, se calcula la matriz de covarianza del error producido en la predicción: 𝑃𝑓,𝑘=𝐽𝑓·𝑃𝑘−1·𝐽𝑓𝑇 +𝑄𝑘−1 ( 6-5) Siendo: • 𝐽𝑓= ( 1 0 0 𝛥𝑡 0 0 0 1 0 0 𝛥𝑡 0 0 0 1 0 0 𝛥𝑡 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 ) , ya que 𝐽𝑓(𝑖,𝑗)=𝑑𝑓(𝑖) 𝑑𝑞𝑖 • 𝑄𝑘−1= ( 𝜎𝑥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.0012 • 𝑃𝑘−1: matriz de covarianza del error calculada en la iteración anterior, o en el caso de ser la primera iteración, matriz identidad de dimensión 6x6. Fase de corrección Tras haber calculado el vector de estados predichos, es necesario someterlo a un proceso de corrección a partir del vector de observación obtenido. Esta etapa de corrección comienza calculando el vector de observación en dicha iteración. Como se ha comentado con anterioridad, este se encuentra definido por la posición del objetivo proporcionada por el algoritmo de tracking, no obstante, este proporciona una posición en píxeles, por lo que el primer paso es construir transformar dicha medida en metros. Tras haber construido el vector de observación 𝑍𝑘 ,se calcula la ganancia de corrección 𝐾𝑘 mediante la siguiente expresión: 𝐾𝑘=𝑃𝑘𝑓·𝐽ℎ𝑇(𝑥𝑘𝑓)·(𝐽ℎ(𝑥𝑘𝑓)·𝑃𝑘𝑓·𝐽ℎ𝑇(𝑥𝑘𝑓)+𝑅𝑘)−1 ( 6-6) Siendo: • 𝑃𝑘𝑓: matriz de covarianza del error producido en la predicción. Calculado en la fase anterior. • 𝑅𝑘: matriz de covarianza que modela la incertidumbre asociada a las medidas. El valor asignado es constante. 𝑅𝑘=(0.0120 0 0.012) • 𝐽ℎ : matriz jacobiana de la ecuación de observación. Cada componente viene definida por la siguiente expresión: 𝐽ℎ(𝑖,𝑗)=𝑑ℎ(𝑖) 𝑑𝑞𝑖 ( 6-7) Es evidente que, para poder calcular la ganancia de corrección, 𝐾𝑘, es necesario calcular con anterioridad la matriz 𝐽ℎ. El calculo de esta no es trivial, por lo que se va a desarrollar el procedimiento realizado para calcularla. En primer lugar, se debe de conocer como se define la función h (operador que mapea el espacio de estado dentro del espacio de observaciones). Esta viene dada por el modelo pinhole de la cámara: 𝑍𝑘=ℎ(𝑋𝑘)=(𝐶𝑥 𝐶𝑦)= ( −𝑓∙ 𝑋𝑇 𝐶𝐴𝑀𝑗𝑍𝑇 𝐶𝐴𝑀𝑗 −𝑓∙ 𝑌𝑇 𝐶𝐴𝑀𝑗𝑍𝑇 𝐶𝐴𝑀𝑗 ) ( 6-8) Respecto a la etapa de predicción, se conoce la posición estimada del objetivo, pero con respecto al sistema de referencia proporcionado por el marcador de ArUco, que es el que se ha considerado como sistema de referencia principal. En este caso, es necesario conocer la posición del objetivo respecto al sistema de referencia de la cámara. Para conseguir este cambio en el sistema de referencia se han empleado las coordenadas homogéneas (explicadas en el capítulo 5): ( 𝑋𝑇 𝐶𝐴𝑀𝑗𝑌𝑇 𝐶𝐴𝑀𝑗𝑍𝑇 𝐶𝐴𝑀𝑗 1 ) =𝑇·(𝑋𝑌𝑍1),𝑠𝑖𝑒𝑛𝑑𝑜 𝑇=(𝑅3𝑥3 𝑝3𝑥1 𝑓1𝑥3 𝑤1𝑥1) ( 6-9) En nuestro caso, la matriz de transformación T tendrá el siguiente aspecto: 𝑇=(𝑅11 𝑅12 𝑅13 𝑡1 𝑅21 𝑅22 𝑅23 𝑡2 𝑅31 𝑅32 𝑅33 𝑡3 0 0 0 1) Por lo que, la ecuación de coordenadas homogéneas queda: (𝑋𝑇 𝐶𝐴𝑀𝑗𝑌𝑇 𝐶𝐴𝑀𝑗𝑍𝑇 𝐶𝐴𝑀𝑗 )=(𝑅11·𝑋+𝑅12·𝑌+𝑅13·𝑍+𝑡1 𝑅21·𝑋+𝑅22·𝑌+𝑅23·𝑍+𝑡2 𝑅31·𝑋+𝑅32·𝑌+𝑅33·𝑍+𝑡3) ( 6-10) Sustituyendo los valores anteriores en la ecuación (6-8) : 𝑍𝑘=ℎ(𝑋𝑘)=(𝐶𝑥 𝐶𝑦)= ( −𝑓∙𝑅11·𝑋+𝑅12·𝑌+𝑅13·𝑍+𝑡1 𝑅31·𝑋+𝑅32·𝑌+𝑅33·𝑍+𝑡3 −𝑓∙𝑅21·𝑋+𝑅22·𝑌+𝑅23·𝑍+𝑡2 𝑅31·𝑋+𝑅32·𝑌+𝑅33·𝑍+𝑡3 ) ( 6-11) Estimación coopertiva de posición 68 68 Una vez expresado el operador que mapea el espacio de estado dentro del espacio de observaciones en función de las componentes del vector de estados, se procede a calcular la matriz 𝐽ℎ Por lo tanto, la matriz de inercias Jh, se define como: 𝐽ℎ=−𝑓(𝐽ℎ11 𝐽ℎ12 𝐽ℎ13 0 0 0 𝐽ℎ21 𝐽ℎ22 𝐽ℎ23 0 0 0) ( 6-12) Donde: • 𝐽ℎ11=𝑌(𝑅11𝑅32−𝑅31𝑅12)+𝑍(𝑅11𝑅33−𝑅31𝑅13)+𝑅11𝑡3−𝑅31𝑡1 (𝑅31𝑋+𝑅32𝑌+𝑅33𝑍+𝑡3)2 • 𝐽ℎ12=𝑋(𝑅12𝑅31−𝑅32𝑅11)+𝑍(𝑅12𝑅33−𝑅32𝑅13)+𝑅12𝑡3−𝑅32𝑡1 (𝑅31𝑋+𝑅32𝑌+𝑅33𝑍+𝑡3)2 • 𝐽ℎ13=𝑋(𝑅13𝑅31−𝑅33𝑅11)+𝑌(𝑅13𝑅32−𝑅33𝑅12)+𝑅13𝑡3−𝑅33𝑡1 (𝑅31𝑋+𝑅32𝑌+𝑅33𝑍+𝑡3)2 • 𝐽ℎ21=𝑌(𝑅21𝑅32−𝑅31𝑅22)+𝑍(𝑅21𝑅33−𝑅31𝑅23)+𝑅21𝑡3−𝑅31𝑡2 (𝑅31𝑋+𝑅32𝑌+𝑅33𝑍+𝑡3)2 • 𝐽ℎ22=𝑋(𝑅22𝑅31−𝑅32𝑅21)+𝑍(𝑅22𝑅33−𝑅32𝑅23)+𝑅22𝑡3−𝑅32𝑡2 (𝑅31𝑋+𝑅32𝑌+𝑅33𝑍+𝑡3)2 • 𝐽ℎ23=𝑋(𝑅23𝑅31−𝑅33𝑅21)+𝑌(𝑅23𝑅32−𝑅33𝑅22)+𝑅23𝑡3−𝑅33𝑡2 (𝑅31𝑋+𝑅32𝑌+𝑅33𝑍+𝑡3)2 Tras haber calculado la matriz jacobiana del espacio de observación 𝐽ℎ y, posteriormente, la ganancia de corrección 𝐾𝑘 , el siguiente paso es calcular el vector de estado corregido 𝑋𝑘𝑎 empleando la siguiente ecuación: 𝑋𝑘𝑎=𝑋𝑘𝑓+ 𝐾𝑘·( 𝑍𝑘−ℎ(𝑥𝐾𝑓) ) ( 6-13) En la ecuación anterior, es posible identificar como se le aplica una ganancia 𝐾𝑘 al error producido entre la posición del objetivo proporcionada por el algoritmo de tracking y el punto en el que debería de estar a partir del vector de estados predicho en la etapa anterior. Por último, se actualiza la matriz de covarianza del error producido en la predicción 𝑃𝑘 : 𝑃𝑘 =(𝐼 −𝐾𝑘·𝐽ℎ )·𝑃𝑘𝑓 ( 6-14) Experimentos y resultados 70 70 7 EXPERIMENTOS Y RESULTADOS N este capítulo se mostrarán los resultados obtenidos en los diversos experimentos realizados. Se han llevado a cabo cuatro tipos de experimentos diferentes. El primero de ellos se basa en estimar la posición de un objetivo en una posición aproximadamente fija, utilizando diferentes matrices de covarianza (𝑅𝑘 , 𝑄𝑘). El siguiente experimento, consiste en estimar la posición del objetivo realizando este un movimiento oscilatorio en la dirección del eje X, respecto al sistema de referencia global dado por el marcador de ArUco. Similar al anterior es el tercer experimento realizado, consistente en estimar la posición del objetivo realizando este un movimiento oscilatorio, pero en la dirección del eje Y. El último experimento consiste en la estimación de la posición del objetivo cuando este realiza de forma aproximada una trayectoria circular. 7.1 Objetivo fijo Tal y como se ha descrito en la introducción del capítulo, en primer lugar, se mostrará la información obtenida estimando la posición de un objetivo que permanece ,de forma aproximada, estático en un cierto punto. El experimento se ha llevado a cabo realizando el seguimiento de un quadrotor en el aire, por lo que la posición de este sufrirá cierta oscilación. Las siguientes estimaciones, se han realizado utilizando un tiempo de captura de unos 60 segundos aproximadamente. De aquí se van a obtener algunas medidas estadísticas como el valor medio, varianza, y máxima desviación. 7.1.1 Experimento I En este primer caso, en el que se estima la posición estática del objetivo se han empleado las siguientes matrices de covarianza: 𝑅𝑘=(120 0 12) 𝑄𝑘= ( 0.120 0 0 0 0 0 0.120 0 0 0 0 0 0.120 0 0 0 0 0 0.120 0 0 0 0 0 0.120 0 0 0 0 0 0.12 ) Los resultdos obtenidos para estos valores han sido: E Figura 7-1. Representación 3D para el primer caso de estimación de la posición del objetivo en condiciones estáticas Figura 7-2. Diferentes vistas procedentes de la figura 7-1 Experimentos y resultados 72 72 Figura 7-3. Representación de los valores estimados para cada eje pertenecientes al primer experimento Para este experimento se han obtenido los siguientes parámetros estadísticos: Observando la tabla 7-1 , se puede comprobar como se obtiene una desviación máxima respecto de la media de unos 2 centímetros aproximadamente en cada eje. La varianza calculada para cada uno de los ejes, se encuentra comprendidoa entre 1 cm2 (ejes X e Y) y 6 cm2 (eje Z). Parámetro estadístico X Y Z Media (𝒎) 0.0367 0.085 0.2889 Varianza (𝒎𝟐) 1.37·10−4 1.07·10−4 6.108·10−5 Máxima desviación (𝒎) 0.0254 0.0251 0.0242 Tabla 7-1. Medidas estadísticas experimento I 7.1.2 Experimento II En este primer caso, en el que se estima la posición estática del objetivo se han empleado las siguientes matrices de covarianza: 𝑅𝑘=(0.120 0 0.12) 𝑄𝑘= ( 0.120 0 0 0 0 0 0.120 0 0 0 0 0 0.120 0 0 0 0 0 0.120 0 0 0 0 0 0.120 0 0 0 0 0 0.12 ) Los resultdos obtenidos para estos valores han sido: Figura 7-4. Representación 3D para el segundo caso de estimación de la posición del objetivo en condiciones estáticas Experimentos y resultados 80 80 Figura 7-12. Representación de los valores estimados para cada eje en el experimento en el que el móvil realiza un movimiento helicoidal 8 CONCLUSIONES Y TRABAJOS FUTUROS n la última década, el número de aplicaciones vinculadas a la visión artificial ha incrementado notoriamente, apareciendo en campos donde no había aparecido con anterioridad, como en el sector del automovilismo y en el comercial. Por esta razón, es valorable tener un nivel importante de conocimientos en esta área. Tras llevar a cabo este proyecto, destacar lo sencillo que puede resultar iniciarse en este mundo de visión artificial mediante la librería OpenCV, sin embargo, esta sencillez inicial se transforma en algo complejo cuando se desea trabajar en entornos reales, donde la luminosidad varia continuamente y el tiempo de procesamiento no debe de superar ciertos valores. Tras los experimentos llevados a cabo, se puede llegar a la conclusión de que el sistema diseñado proporciona una estimación de la posición de gran calidad, con un error absoluto máximo de 5 cm. A pesar de estos resultados, el sistema diseñado acepta una gran variedad de mejoras que supondría una mejora de resultados y disminuiría las restricciones apreciables en el sistema. En primer lugar, en el sistema diseñado las diversas cámaras no cuentan con un sistema Pitch & Roll, que permitan modificar el ángulo de cada una de ellas. Esta modificación permitiría variar la orientación de las cámaras, siendo posible conocer la posición y orientación sin la necesidad de que el marcador de ArUco se encuentre dentro del rango de visión de cada cámara. Esta modificación, sería posible llevarla a cabo tras conocer la posición y orientación de cada una respecto al marcador de ArUco inicialmente. La nueva matriz de transformación de coordenadas homogéneas se modificaría en función de la matriz inicial , calculada observando el marcador(en la primera iteración sería necesario que el marcador se encontrase dentro del rango de visión de cada cámara), y en función de la rotación llevada a cabo en los distintos ejes de la cámara. De esta forma sería posible conocer la posición y orientación de cada una de ellas sin observar el marcador. Otra posible mejora por destacar sería la implementación de este sistema en Android, de forma que se pudiese trabajar con las cámaras de cada dispositivo móvil. Esto supondría una mayor comodidad para el usuario que no tendría que portar las cámaras continuamente. Por último, dicho sistema permite realizar el seguimiento de un solo objetivo. Sería posible mejorar el sistema de forma que se pudiese conocer la posición de varios objetos de forma simultanea. Esto se podría implementar desarrollando un entorno multihilo. E Conclusiones y trabajos futuros 82 82 REFERENCIAS [1] Leonhard Euler, "Problema algebraicum ob affectiones prorsus singulares memorabile", Commentatio 407 Indicis Enestoemiani, Novi Comm. Acad. Sci. Petropolitanae 15 (1770), 75–106. [2] Gabriel A. Terejanu," Extended Kalman Filter Tutorial" [3} Leandro M. Di Matteo, Juan Carlos Gómez, Claudio Verrastro, "Algoritmo de seguimiento de objetos basado en visión asistida por computador en tiempo real utilizando CAMShift e histogramas" [4] Weisstein, Eric W. "Rodrigues' Rotation Formula". Available: http://electroncastle.com/wp/?p=39 [5] Alejandro Pascual,"EKF y UKF:dos extensiones del filtro de Kalman para sistemas no lineales". Available: https://iie.fing.edu.uy/ense/asign/tes/monografias/anteriores/alejandro_pascual/TES2004_pascual.pdf [6] Rafael Muñoz Salinas, " ArUco: An efficient library for detection of planar markers and camera pose estimation". Available: https://docs.google.com/document/d/1QU9KoBtjSM2kF6ITOjQ76xqL7H0TEtXriJX5kwi9Kgc/edit #heading=h.1z8vm961dhos [7] M. R. Arahal, Apuntes de Sistemas de Percepción. [8] << OpenCV documentation >> Available: https://docs.opencv.org/2.4/index.html [9] «MATLAB - El lenguaje del cálculo técnico,» [En línea]. Available: https://es.mathworks.com/products/matlab.html [10] << Rodrigues' rotation >> Available: http://electroncastle.com/wp/?p=39 [11] Alejandro Suarez, Anibal Ollero, Guillermo Heredia, "Cooperative Virtual Sensor for Fault Detection and Identification in Multi-UAV Applications" [12] Alejandro Suarez, Anibal Ollero, Guillermo Heredia, "Cooperative sensor fault recovery in multiUAV systems"