Ensayo de técnicas de localización y seguimiento visual para vehículos aéreos no tripulados

Equation Chapter 1 Section 1 Trabajo Fin de Grado Ingeniería Electrónica, Robótica y Mecatrónica Ensayo de técnicas de localización y seguimiento visual para vehículos aéreos no tripulados Dpto. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Universidad de Sevilla niv Autor: Iván de la Fuente Trinidad Tutor: Carlos Vivas Venegas Sevilla, 2021 ii iii Trabajo Fin de Grado Ingeniería Electrónica, Robótica y Mecatrónica Ensayo de técnicas de localización y seguimiento visual para vehículos aéreos no tripulados Autor: Iván de la Fuente Trinidad Tutor: Carlos Vivas Venegas Profesor titular Dpto. de Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2021 iv Trabajo Fin de Grado: Ensayo de técnicas de localización y seguimiento visual para vehículos aéreos no tripulados Autor: Iván de la Fuente Trinidad Tutor: Carlos Vivas Venegas El tribunal nombrado para juzgar el Proyecto arriba indicado, compuesto por los siguientes miembros: Presidente: Vocales: Secretario: Acuerdan otorgarle la calificación de: Sevilla, 2021 El Secretario del Tribunal v vi A mi familia A mis abuelos A mis amigos vii viii Agradecimientos La universidad ha sido una etapa que me ha marcado en todos los sentidos. Quería agradecerle en primer lugar a mi familia por dármelo todo y más para conseguir mis objetivos. Por apoyarme con mis decisiones, aconsejarme y estar en los momentos más difíciles. Sin ellos no hubiera sido posible. Son lo más importante. A mis compañeros, en especial a Paula, Juan y Ana, por todos esos momentos dentro y fuera de la universidad. Gracias por ayudarme siempre y hacer esta etapa inolvidable. A todos mis amigos, por estar ahí siempre. A Carlos Vivas por ofrecerme trabajar en este proyecto. A todos mis profesores por acompañarme en este largo trayecto de aprendizaje. Iván de la Fuente Trinidad Sevilla, 2021 ix xvi ÍNDICE DE TABLAS Tabla 2.1 Métodos para la detección de objetivos. ................................................................................. 5 Tabla 5.1 Nodos. ................................................................................................................................. 25 Tabla 5.2 Topics y nodos que los utilizan............................................................................................. 26 xvii ÍNDICE DE FIGURAS Figura 1.1 UAV comercial. ................................................................................................................... 1 Figura 1.2 Diseño 3D final del dron. ...................................................................................................... 2 Figura 2.1 Detección de un peatón. ........................................................................................................ 4 Figura 3.1 Logotipo de Gazebo. ............................................................................................................ 6 Figura 3.2 Logotipo de ROS. ................................................................................................................. 7 Figura 3.3 Logotipo de ROS Melodic. ................................................................................................... 7 Figura 3.4 Representación de la ecuación de ROS. ................................................................................ 7 Figura 3.5 Imagen explicativa del intercambio de mensajes entre dos nodos. ......................................... 8 Figura 3.6 Simulación del "mundo" en Gazebo. ..................................................................................... 9 Figura 4.1 Keypoints detectados por algoritmo SIFT en dos imágenes. ................................................ 10 Figura 4.2 Correspondencia de los keypoints en ambas imágenes. ........................................................ 11 Figura 4.3 Objetivo identificado en la imagen de la cámara. ................................................................ 13 Figura 4.4 Coordenadas del centro del objetivo publicándose en el topic /target_cam_pose. ................ 13 Figura 4.5 Actor en una simulación de Gazebo. ................................................................................... 14 Figura 4.6 Trayectoria seguida por el actor en la simulación. ............................................................... 14 Figura 4.7 Detección del modelo de pickup. ........................................................................................ 16 Figura 4.8 Detección del modelo de person_walking. .......................................................................... 16 Figura 5.1 Nomenclatura para los sólidos del modelo del dron. ............................................................ 20 Figura 5.2 Sistemas de referencia principales en la posición de reposo. ................................................ 20 Figura 5.3 Ángulos de rotación de la cámara y coordenadas articulares del gimbal. ............................. 20 Figura 5.4 Sistema de referencia de la cámara según el paper. ............................................................. 21 Figura 5.5 Esquema de bloques en Simulink. ....................................................................................... 23 Figura 5.6 Detalle del interior del subsistema. ..................................................................................... 23 Figura 5.7 Seguimiento de referencias en velocidad angular del gimbal. .............................................. 24 Figura 5.8 Esquema de la conexión entre nodos obtenido mediante rqt_graph. .................................... 27 Figura 6.1 Seguimiento de las velocidades angulares de referencia para el gimbal (prueba 1). .............. 28 Figura 6.2 Error respecto al centro de la cámara (prueba 1). ................................................................. 29 Figura 6.3 Seguimiento de las velocidades angulares de referencia para el gimbal (prueba 2). .............. 29 Figura 6.4 Error respecto al centro de la cámara (prueba 2). ................................................................. 30 Figura 6.5 Seguimiento de las velocidades angulares de referencia para el gimbal (prueba 3). .............. 31 xviii Figura 6.6 Detalle de la figura anterior (prueba 3)................................................................................ 31 Figura 6.7 Error respecto al centro de la cámara (prueba 3). ................................................................. 32 Figura 6.8 Error en la estimación del centro del objetivo. .................................................................... 33 Figura 6.9 Seguimiento de las velocidades angulares de referencia para el gimbal (prueba 4)............... 33 Figura 6.10 Error respecto al centro de la cámara (prueba 4). ............................................................... 34 Figura 6.11 Seguimiento de las velocidades angulares de referencia para el gimbal (prueba 5). ............ 35 Figura 6.12 Error respecto al centro de la cámara (prueba 5). ............................................................... 35 Figura 6.13 Seguimiento de las velocidades angulares de referencia para el gimbal (prueba 6). ............ 36 Figura 6.14 Error respecto al centro de la cámara (prueba 6). ............................................................... 36 xix Notación ´ Operador de producto vectorial · Operador de producto escalar S Función que transforma vector en matriz antisimétrica S-1 Función que transforma matriz antisimétrica en vector Ȧ Derivada con respecto al tiempo de la matriz A A' Traspuesta de la matriz/vector A || a || Norma del vector a A-1 Matriz inversa de A Índice de Figuras 20 20 Figura 1.1 UAV comercial. 1 INTRODUCCIÓN 1.1. Motivación Un vehículo aéreo no tripulado (VANT), UAV (del inglés unmanned aerial vehicle), más apropiadamente RPAS (del inglés Remotely Piloted Aircraft System), comúnmente conocido como dron, hace referencia a una aeronave que vuela sin tripulación, la cual ejerce su función remotamente. Un VANT es un vehículo sin tripulación, reutilizable, capaz de mantener de manera autónoma un nivel de vuelo controlado y sostenido, y propulsado por un motor de explosión, eléctrico o de reacción. El diseño de los VANT tiene una amplia variedad de formas, tamaños, configuraciones y características. Actualmente los UAV tienen numerosas aplicaciones en el ámbito civil y comercial (su uso se ha extendido en los últimos años), aunque en sus inicios eran utilizados principalmente con fines militares (tareas de reconocimiento, vigilancia y adquisición de objetivos). Las principales ventajas de los vehículos aéreos no tripulados son las siguientes: - No se necesita un piloto a bordo, lo que evita los posibles daños que pudiera sufrir y permite que las dimensiones del dron sean mucho más pequeñas. - Uso en zonas aéreas peligrosas o de difícil acceso para los humanos. - En el caso de los drones comerciales, su manejo no requiere de una alta cualificación. En este trabajo, el dron utilizado es el descrito en el trabajo previo. Se trata de un quadrotor del que cuelga un gimbal de tres grados de libertad y en cuyo extremo se encuentra la cámara, cuya orientación queremos controlar para el seguimiento (tracking) del objetivo (target) deseado. Introducción 2 2 Figura 1.2 Diseño 3D final del dron. 1.2. Objetivos Como se ha explicado en el resumen, el objetivo principal de este trabajo es el ensayo bajo simulación del sistema descrito para rastrear de forma autónoma un objetivo en movimiento. Para ello se hará uso del dron mostrado anteriormente, cuyo gimbal será controlado para conseguir el seguimiento de los objetivos con la cámara. En un primer paso habrá que adquirir toda la información del proyecto del que partimos para ser capaces de hacer funcionar todo lo resuelto en el trabajo anterior. Tras identificar y comprender el funcionamiento de todos los componentes del sistema completo se pasa resolver el problema de este trabajo. En primer lugar, hay que buscar algún algoritmo que extraiga los puntos característicos de una imagen para identificar con la cámara el objeto al que se quiere seguir. Una vez se resuelva lo anterior, se pasa a encontrar una estrategia de control del gimbal para conseguir el seguimiento visual del objetivo. 1.3. Estructura del trabajo Este trabajo se divide en los capítulos siguientes, además de la introducción: • Capítulo 2: en el segundo capítulo se aborda el estado del arte del seguimiento de objetivos para ver cuáles son los métodos más empleados en este ámbito. • Capítulo 3: este capítulo introduce y explica las herramientas que se van a usar en el proyecto, que son: - Gazebo. - ROS. • Capítulo 4: en el cuarto capítulo se expone el algoritmo SIFT, escogido para la detección del objetivo del cual se quiere realizar el seguimiento. • Capítulo 5: en el quinto capítulo se desarrolla la ley de control del gimbal necesaria para llevar a cabo el tracking de nuestro objetivo. • Capítulo 6: en este capítulo se muestran los resultados de las distintas pruebas realizadas sobre la simulación de Gazebo. • Capítulo 7: en el último capítulo se acaba con las conclusiones del proyecto y los posibles trabajos futuros. 2 2 ESTADO DEL ARTE 2.1. Introducción En los últimos tiempos, se ha producido una explosión en el uso de la tecnología de seguimiento de objetos (object tracking) en aplicaciones no militares. Los algoritmos de seguimiento de objetos se han convertido en una parte esencial de nuestra vida cotidiana. Por ejemplo, la navegación basada en el GPS es una herramienta cotidiana de la humanidad. En esta aplicación, un grupo de satélites artificiales en el espacio exterior continuamente localiza los vehículos que conducen las personas y los algoritmos de seguimiento de objetos en el GPS realizan la auto-localización y nos permiten disfrutar de una serie de servicios basados en la localización, como la búsqueda de lugares de interés y la planificación de rutas. Del mismo modo, el seguimiento de objetos se utiliza en una gran variedad de contextos, como la vigilancia del espacio aéreo, el seguimiento de vehículos, el seguimiento de submarinos y ballenas y la vigilancia inteligente por vídeo. También se utilizan en la navegación de robots autónomos mediante láseres, cámaras estereoscópicas y otros sensores de proximidad. El seguimiento de objetos/objetivos se refiere al problema de utilizar las mediciones de los sensores para determinar la ubicación, la trayectoria y las características de los objetos de interés. Un sensor puede ser cualquier dispositivo de medición, como un radar, un sonar, un ladar, una cámara, un sensor de infrarrojos, un micrófono, un ultrasonido o cualquier otro sensor que pueda utilizarse para recoger información sobre objetos en el entorno. Los objetivos típicos del seguimiento de objetos son la determinación del número de objetos, su identidad y sus estados, como la posición, la velocidad y el tiempo de permanencia. El seguimiento de objetos es una tarea importante dentro del campo de la visión por ordenador. La proliferación de ordenadores de gran potencia, la disponibilidad de cámaras de vídeo de alta calidad y bajo coste, y la creciente necesidad de automatizar el análisis de vídeo han generado un gran interés en los algoritmos de seguimiento de objetos. Hay tres pasos clave en el análisis de vídeo: la detección de objetos en movimiento interesantes, el seguimiento de dichos objetos de un fotograma a otro y el análisis del comportamiento de los objetos para predecir su trayectoria. Por tanto, el uso del seguimiento de objetos es pertinente en las tareas de: - Reconocimiento basado en el movimiento. - Vigilancia automatizada. - Indexación de vídeos. - Interacción persona-ordenador. - Seguimiento del tráfico. - Navegación de vehículos. Estado del arte 4 4 Figura 2.1 Detección de un peatón. El seguimiento de objetos puede ser bastante complejo debido a: - Pérdida de información causada por la proyección del mundo 3D en una imagen 2D, - Ruido en las imágenes. - El movimiento complejo de los objetos. - La naturaleza no rígida o articulada de los objetos. - Las oclusiones parciales y totales de los objetos, las formas complejas de los objetos. - Cambios en la iluminación de la escena y requisitos de procesamiento en tiempo real. Se puede simplificar el seguimiento imponiendo restricciones al movimiento y/o a la apariencia de los objetos. Por ejemplo, casi todos los algoritmos de seguimiento suponen que el movimiento del objeto es suave y sin cambios bruscos. Además, se puede restringir el movimiento del objeto para que sea de velocidad constante o de aceleración constante basándose en información a priori. El conocimiento previo sobre el número y el tamaño de los objetos, o el aspecto y la forma del objeto, también puede utilizarse para simplificar el problema. 2.2. Detección de objetos Todos los métodos de seguimiento requieren un mecanismo de detección de objetos, ya sea en cada fotograma o cuando el objeto aparece por primera vez en el vídeo. En la Tabla 2.1 se muestran varios métodos habituales de detección de objetos. Aunque la detección de objetos en sí misma es muy amplia, aquí se resumen los métodos más populares en el contexto del seguimiento de objetos para completar la información: 4 5 5 Ensayo de técnicas de localización y seguimiento visual para vehículos aéreos no tripulados Tabla 2.1 Métodos para la detección de objetivos. Categorías Métodos Point detectors Moravec’s detector [Moravec 1979], Harris detector [Harris and Stephens 1988], Scale Invariant Feature Transform [Lowe 2004], Affine Invariant Point Detector [Mikolajczyk and Schmid 2002]. Segmentation Mean-shift [Comaniciu and Meer 1999], Graph-cut [Shi and Malik 2000], Active contours [Caselles et al. 1995]. Background Modeling Mixture of Gaussians[Stauffer and Grimson 2000], Eigenbackground[Oliver et al. 2000], Wall flower [Toyama et al. 1999], Dynamic texture background [Monnet et al. 2003]. Supervised Classifiers Support Vector Machines [Papageorgiou et al. 1998], Neural Networks [Rowley et al. 1998], Adaptive Boosting [Viola et al. 2003]. Detección del objetivo 12 12 En su interior encontramos dos carpetas: - launch: contiene el archivo image_pose_estimation.launch encargado de lanzar el nodo correspondiente a este paquete. Se especifica la ruta de la carpeta con la imagen del objeto a detectar en el campo image_path de dicho archivo. - src: aquí disponemos de los dos scripts en Python que se ocupan de detectar el objeto deseado y localizar su centro en la imagen de la cámara. Estos archivos son image_pose_estimation_script.py y image_processing.py. image_pose_estimation_script.py Este código se encarga de recoger los parámetros introducidos en el launcher (image_pose_estimation.launch) tales como el nombre de la cámara, el path de la imagen con el objeto a identificar y otros valores que se han dejado por defecto. Desde este código se llama al script de image_processing.py, que devuelve una "clase" (class) de la cual nos interesan las variables de las coordenadas del centro del objetivo en la cámara y una variable flag que se explica a continuación. Este script ha sido modificado para que se publique la posición del objetivo en la cámara a través de un topic (/target_cam_pose). Solo se publica a través de él cuando el valor de la variable flag es True. image_processing.py En este script se implementa el algoritmo SIFT con la ayuda de las funciones de OpenCV SIFT_create(), que construye un objeto SIFT y sift.detectAndCompute() que obtiene los keypoints y los descriptores en un solo paso. También se hace uso de una función ya creada en el código encargada de realizar el matching de los puntos clave de la imagen del objetivo con el frame de la cámara en cada instante. Se introdujo una variable flag cuya función es la de variable bandera, su valor por defecto es False, pero cuando el objetivo es identificado, es decir, cuando se supera un umbral (establecido en el parámetro min_match_count del archivo .launch) del número de correspondencias de keypoints de la imagen del objetivo con la imagen de la cámara, la variable cambia a True. Problemas del paquete Al tratar de compilar el proyecto (al hacer catkin_make en el workspace) aparecen una serie de errores tales como: - fatal error: opencv2/nonfree/features2d.hpp: No such file or directory Esto se debe a que no tenemos la librería nonfree de OpenCV, en la que se encuentran los algoritmos de SIFT y SURF, que se sabe que están patentados. Una solución para obtener dicha librería es ejecutando las siguientes instrucciones en la línea de comandos: ivan@ivan~ZenBook:~$ sudo add-apt-repository --yes ppa:jeff250/opencv ivan@ivan~ZenBook:~$ sudo apt-get update ivan@ivan~ZenBook:~$ sudo apt-get install libopencv-dev ivan@ivan~ZenBook:~$ sudo apt-get install libopencv-nonfree-dev 13 13 Ensayo de técnicas de localización y seguimiento visual para vehículos aéreos no tripulados Figura 4.3 Objetivo identificado en la imagen de la cámara. Figura 4.4 Coordenadas del centro del objetivo publicándose en el topic /target_cam_pose. 4.1.3 Lanzamiento del nodo detector de objetivo Para hacer uso de este paquete, una vez abierta la simulación (launcher del paquete dron_gazebo) y aplicado el control (launcher del paquete ctrl_desacoplado), se escribe lo siguiente en una ventana de comandos: 4.2. Detección de un objetivo estático En este apartado se muestra un ejemplo del funcionamiento del paquete al lanzar el nodo, tomando una de las señales de tráfico que se encuentran en el "mundo" de Gazebo como objetivo a identificar. Vemos cómo el algoritmo detecta correctamente el objeto (dibujando un cuadrado azul a su alrededor) y publica a través del topic /target_cam_pose las coordenadas del centro del objetivo. 4.3. Detección de objetivo en movimiento Tras comprobar el buen funcionamiento del algoritmo con un objetivo estático, ahora hay que comprobar la detección de un objeto que esté en movimiento. ivan@ivan~ZenBook:~$ roslaunch image_pose_estimation image_pose_estimation.launch Detección del objetivo 14 14 Figura 4.5 Actor en una simulación de Gazebo. Figura 4.6 Trayectoria seguida por el actor en la simulación. 4.3.1 Actor Para ello, se ha introducido en la simulación un “actor”. En Gazebo, un actor es un modelo animado. Los actores son como los modelos, pero con la diferencia que a los actores no se les aplica ninguna fuerza (como fuerzas de gravedad o de contacto). Un actor también puede describir una trayectoria definida directamente en SDF (dentro del archivo dron.world), por lo que es bastante sencillo de utilizar. La trayectoria consiste en especificar una serie de posiciones que el actor tiene que alcanzar en tiempos determinados. Gazebo se encarga de interpolar el movimiento entre ellos para que la animación sea fluida. La trayectoria elegida en nuestro caso ha sido una línea recta, de manera que nuestro actor cruza la carretera de un lado hacia el otro en un bucle. Para ello, en el archivo dron.world se incluye el actor, especificando su apariencia (skin), la animación a realizar (animation) y la trayectoria deseada (trajectory). La apariencia y la animación elegida es la misma en ambos casos, walk.dae (ya instalado con Gazebo). El código correspondiente se encuentra en el Apéndice A. 15 15 Ensayo de técnicas de localización y seguimiento visual para vehículos aéreos no tripulados Modificaciones Como el objetivo en este caso está en movimiento, se ha decidido aportar varias imágenes del objetivo para ser detectado. Al igual que en la detección de un objetivo estático, en el launcher del paquete se especifica la ruta de la carpeta que incluye las imágenes del objetivo. Esto se ha visto necesario ya que el actor no simplemente cambia su posición, sino que al estar animado mueve brazos y piernas y cambia su orientación. Para ello, en el script image_processing.py se aplica el algoritmo SIFT y se realiza el matching de la imagen de la cámara con todas las imágenes que haya en la carpeta proporcionada, de manera que, si no hay correspondencia con una de ellas, se pueda comprobar la correspondencia con otra imagen distinta. Problemas Tras realizar numerosas pruebas se comprobó que el algoritmo SIFT no detectaba del todo bien a dicho actor, ya que el personaje no tiene muchas peculiaridades para ser detectado correctamente. Esto hacía necesario bajar el umbral de correspondencia de los keypoints, lo que llevaba a la detección errónea del objetivo. 4.3.2 Modelos de Gazebo Aunque el uso de un actor para tener un objeto en movimiento a priori era la mejor opción, hubo que buscar otras alternativas, por lo que se decidió utilizar un modelo de Gazebo con características más notables. El primer paso es introducir el modelo deseado en el archivo dron.world para cargarlo con la simulación. En dicho archivo le damos su posición inicial en el "mundo". Para hacer que nuestro modelo estuviera en movimiento se programó un script sencillo (ver en Apéndice A) donde se obtiene la posición actual del modelo mediante la llamada al servicio GetModelState, y tras ello publicar a través del topic /gazebo/set_model_state la nueva posición que queremos que adquiera el modelo. Al servicio GetModelState es necesario pasarle dos parámetros: el nombre del modelo de Gazebo a utilizar (model_name) y el nombre de la entidad relativa (relative_entity_name). El primero es directamente el nombre del modelo que aparece en Gazebo, y el segundo es el nombre que se obtiene del campo body_names que proporciona el servicio GetModelProperties (a este servicio solamente es neserario pasarle el nombre del modelo), que suele ser "link". La trayectoria seguida por el modelo elegido es una línea recta que atraviesa la carretera, de la misma manera que lo hacía el actor. Para ello se incrementa la coordenada Y del modelo poco a poco hasta alcanzar un límite, donde se detiene por unos segundos. Tras ello, empieza a decrementar su valor hasta llegar al otro límite establecido, donde se vuelve a detener por unos instantes. Así simulamos un bucle en el que el modelo va hacia adelante y hacia atrás. El modelo elegido para realizar las pruebas de seguimiento ha sido el de una camioneta (pickup), ya que, como se puede ver en la imagen mostrada a continuación, el número de matches con este modelo es bastante alto, lo que hace que el algoritmo tenga menos error y detecte correctamente la camioneta. Detección del objetivo 16 16 Figura 4.7 Detección del modelo de pickup. Figura 4.8 Detección del modelo de person_walking. También se ha probado con otros modelos como el de person_walking: Como se observa en este caso, el número de matches es mucho más bajo, por lo que hay mucho más error que con el modelo de la camioneta. 4.3.3 Lanzamiento del nodo para el movimiento de la camioneta Al ejecutar esta instrucción por línea de comandos la camioneta empezará a moverse: ivan@ivan~ZenBook:~$ python ~/catkin_ws/src/dron_gazebo/scripts/mueve_pickup.py 5 SEGUIMIENTO DEL OBJETIVO Este capítulo está dedicado a la parte del control del gimbal para realizar la tarea del seguimiento del objetivo identificado anteriormente por el algoritmo SIFT. En un primer paso se probó un control básico, proporcional al error de las coordenadas del objeto con el centro de la cámara, para comprobar que todo funcionaba correctamente. Finalmente, para el control del gimbal se ha tomado el paper "Gimbal Control for Vision-based Target Tracking" ([2]) como referencia para resolver el problema de tracking. 5.1. Ley de control del gimbal Este documento soluciona el problema del control de un gimbal de tres ejes que lleva una cámara, de la cual se utilizan las imágenes para realimentar el control. La finalidad es la de mantener el objetivo de interés en el centro de la imagen de la cámara. Se procede a la explicación de la solución propuesta por este paper. En primer lugar, antes de comenzar con la explicación hay que decir que la matriz que genera el operador S (expresión 2 del documento) es errónea: 𝑆(𝑎)= ' 0 𝑎)−𝑎+ −𝑎)0 𝑎, 𝑎+−𝑎,0 - (5.1) Como bien se dice en la línea debajo de dicha expresión en el paper, 𝑆(𝑎)𝑏/=𝑎/ ´ //𝑏 , siendo ´ el producto cruzado (o producto vectorial). Se comprobó que el producto escalar 𝑆(𝑎)·𝑏 no daba el mismo resultado que el producto cruzado de a y b, siendo 𝑆(𝑎)𝑏/=−/(𝑎/ ´ //𝑏) . 𝑆(𝑎)𝑏/= ' 𝑎)·𝑏+−𝑎+·𝑏) −𝑎)·𝑏,+𝑎,·𝑏) 𝑎+·𝑏1−𝑎,·𝑏+ - /𝑎 ´ 𝑏/= ' −𝑎)·𝑏++𝑎+·𝑏) 𝑎)·𝑏,−𝑎,·𝑏) −𝑎+·𝑏1+𝑎,·𝑏+ - (5.2) Esto se ha corroborado con una búsqueda en Internet. En la teoría helicoidal, un uso de este operador, representado por el acento circunflejo (â en lugar de S(a)), es el de representar el producto vectorial. Se concluye que la forma correcta de dicha matriz expresada en (5.1) es: 𝑆(𝑎)= ' 0 −𝑎)𝑎+ 𝑎)0 −𝑎, −𝑎+𝑎,0 - (5.3) El modelo del Gimbal describe cómo varía la matriz de rotación (RC) que me lleva de la cámara a los ejes mundo para una velocidad angular del gimbal (omega) determinada. 𝑅/3𝐶/=𝑅𝐶/𝑆(𝜔) (5.4 ) El control se plantea fijando el valor que debería tener esta omega en cada instante de tiempo para conseguir el objetivo propuesto, que RC converja a RC* (la orientación deseada). 𝜔/=/−𝑘𝑆7,(𝑅8−/𝑅8′/)/ (5.5) Seguimiento del objetivo 18 18 Esta expresión da como resultado la omega de control, suponiendo que el error Re en orientación entre la cámara y el objetivo es conocido. La ecuación (5.5) hay que interpretarla del siguiente modo. Cuando aparece el operador 𝑆7, significa que se trata de la operación inversa a la expresada en (5.3): Es decir, Re-Re' (Re menos su traspuesta) es una matriz antisimétrica que siempre tendrá la misma estructura que la matriz (5.3). Por lo tanto, 𝑆7,(𝑅8−𝑅8′)=[𝑒1//𝑒2//𝑒3]′ , es decir, un vector columna. Luego según (5.5), 𝜔/=/−𝑘𝑆7,(𝑅8−/𝑅8′/) , donde e1, e2, e3 son conocidos si se tiene Re, y k es en principio cualquier valor positivo. Cuanto mayor sea k más agresivo será el control. Lo que sigue es como calcular Re, que es el Lemma 1. La expresión (16) del documento dice cómo sacar Re como función de q y a: 𝑅8= ? −@ ( A ) BC ||@ ( A ) BC|| @(A)C ||@(A)C|| A ||A|| E (5.6) Esta expresión también ha sido corregida, ya que el denominador de la primera columna tiene una errata. La fórmula que aparece en el documento tiene como denominador ||𝑆(𝑞)𝑎|| , y como se dice después que todas las columnas de Re deben tener norma 1, se ha deducido que faltaba un cuadrado al operador S. De esta ecuación podemos decir lo siguiente: - q es el vector que une el origen del marco del target {T} con el origen de la cámara {C}. Es la dirección en que queremos que mire la cámara. El problema es que q no es conocida porque no tenemos las coordenadas 3D del target (pT), solo conocemos su proyección en el marco de la cámara, que es y. Como se dice en el apartado B, el eje de la cámara y el target están alineados (nuestro objetivo), cuando el eje Z de la cámara (en nuestro caso es el eje X), que se supone que apunta según el eje óptico de ésta, y q están alineados. - a es la medida de la aceleración que mide el Gimbal. Supone que hay poca aceleración en el movimiento y que la acelaración que mide la IMU del Gimbal aproximadamente se debe solo a la gravedad. Suponiendo que a se obtiene de la IMU, solo queda calcular q, que sale de la expresión (17) del paper. Esta expresión depende solo de y (posición del target en la imagen de la cámara). A ||A||=GHI ? J , E ||GHI ? J , E || (5.6) Lo que se obtiene en esta ecuación es q/||q||, pero eso no es problema porque las columnas de Re tienen norma 1. Solo es necesario conocer su dirección, no su magnitud. Por tanto, el valor obtenido en (5.6) será el utilizado como q en el cálculo de Re. El vector y se obtiene del cálculo del centro del objetivo en la cámara, realizado por el algoritmo SIFT. No hay más que suscribirse al topic /target_cam_pose para obtener su valor. A es la matriz de parámetros intrínsecos de la cámara. Esto se han obtenido fácilmente viendo su valor en el parámetro K del topic /dron/camara/camera_info: 𝐴= ' 315.8158 0265.5 0315.8158 150.5 0 0 1 - (5.7) Con esto ya se tiene todo lo necesario para obtener el valor de q, el cual introducimos en el cálculo de Re. No había ninguna IMU asociada al gimbal de la que pudieramos extraer los datos del acelerómetro para el vector a, por lo que hubo que buscar algún plugin para obtener este dato. 19 19 Ensayo de técnicas de localización y seguimiento visual para vehículos aéreos no tripulados En la página web de Gazebo se encontró un plugin que simula un sensor IMU llamado GazeboRosImu. Este plugin publica la información a través de mensajes de tipo sensor_msgs/Imu. La estructura de este mensaje incluye un campo llamado linear_acceleration, que proporciona la aceleración lineal en los tres ejes. Pero resulta que este sensor no tiene en cuenta la fuerza gravitatoria, por lo que esta es omitida. Como solución se encontró un plugin que sí incluía dicha fuerza, como en las IMUs reales. Se trata de un plugin perteneciente al paquete hector_gazebo_plugins, llamado de la misma forma (GazeboRosImu). Dicho plugin también utiliza mensajes del tipo sensor_msgs/Imu. Para hacer uso del paquete hector_gazebo_plugins primero hay que instalar las dependencias: Después pasamos a clonar el paquete en el workspace del proyecto: Finalmente, para incluir este sensor en el modelo del dron es necesario incluir lo siguiente en el archivo urdf del paquete dron_description: Código 5.1 Plugin libhector_gazebo_ros_imu.so. ivan@ivan~ZenBook:~$ sudo apt-get install ros-melodic-hector-gazebo-plugins ivan@ivan~ZenBook:~$ cd catkin_ws/src ivan@ivan~ZenBook:~$ git clone https://gitlab.iri.upc.edu/labrobotica/ros/sensors/imu/iri_imu_gazebo.git <gazebo> <plugin name="accel_imu_sim" filename="libhector_gazebo_ros_imu.so"> <updateRate>500.0</updateRate> <bodyName>Gimbal2</bodyName> <frameId>world</frameId> <topicName>/dron/imu_gimbal</topicName> <accelOffset>0 0 0</accelOffset> <accelDrift>0 0 0</accelDrift> <accelGaussianNoise>0 0 0</accelGaussianNoise> <rateOffset>0 0 0</rateOffset> <rateDrift>0 0 0</rateDrift> <rateGaussianNoise>0 0 0</rateGaussianNoise> <yawOffset>0</yawOffset> <yawDrift>0</yawDrift> <yawGaussianNoise>0</yawGaussianNoise> </plugin> </gazebo> Figura 9.1 Nomenclatura para los sólidos del modelo del dron.<gazebo> <plugin name="accel_imu_sim" filename="libhector_gazebo_ros_imu.so"> <updateRate>500.0</updateRate> <bodyName>Gimbal2</bodyName> <frameId>world</frameId> Seguimiento del objetivo 20 20 Figura 5.1 Nomenclatura para los sólidos del modelo del dron. Figura 5.2 Sistemas de referencia principales en la posición de reposo. Figura 5.3 Ángulos de rotación de la cámara y coordenadas articulares del gimbal. Gracias a este plugin ya disponemos de las medidas de a del gimbal (el plugin se asocia al "Gimbal2"), que son publicadas continuamente a través del topic /dron/imu_gimbal. Ahora se dispone de todos los datos para hallar la matriz Re. Restando dicha matriz a su traspuesta se obtiene otra matriz de la forma: 𝑅8−𝑅8′= ' 0 −𝑒)𝑒+ 𝑒)0 −𝑒, −𝑒+𝑒,0 - (5.8) Por tanto, como se explicó antes, 𝑆7,(𝑅8−𝑅8′)=[𝑒1//𝑒2//𝑒3]′ . Finalmente, solo quedaría ajustar la constante k para completar la ley de control (5.5). Faltan por añadir unas modificaciones a realizar a la hora de implementar el control, ya que los ejes que se utilizan en el documento no coinciden con los ejes de nuestro dron: 21 21 Ensayo de técnicas de localización y seguimiento visual para vehículos aéreos no tripulados Para la obtención del vector de aceleraciones del gimbal, habría que pasar las medidas que nos proporciona la IMU según nuestro sistema de coordenadas al sistema de referencia según el paper. Por lo que: 𝑎= ' 𝑎𝑐𝑐𝑒𝑙_𝑧 𝑎𝑐𝑐𝑒𝑙_𝑦 −𝑎𝑐𝑐𝑒𝑙_𝑥 - (5.9) calculo_wref.py Todos estos cálculos han sido plasmados en un nuevo script en Python, llamado calculo_wref.py (el código completo se encuentra en el Apéndice A). Este script crea un nodo más del proyecto, y se encuentra dentro del paquete ctrl_desacoplado, en el interior de una carpeta llamada scripts. En su interior se realiza la suscripción a dos topics para realizar los cálculos: - /dron/imu_gimbal: para obtener las aceleraciones de la IMU del gimbal, necesarias para disponer del vector a. - /target_cam_pose: del cual se adquieren las coordenadas del centro del objetivo en la imagen de la cámara y así formar el vector y. Tras ello, se ha creado una función que realiza todos los cálculos para finalmente obtener la ley de control del gimbal, es decir, las velocidades angulares de referencia. El lenguaje utilizado ha sido Python, ya que las distintas operaciones con matrices, como la inversión, la trasposición y la multiplicación, se realizan de manera muy sencilla con la librería Numpy, con la ayuda de las funciones numpy.linalg.inv(), numpy.transpose() y numpy.dot() respectivamente. Además, esta librería permite también obtener de manera rápida cálculos como la norma de un vector (numpy.norm()). Para la publicación de las velocidades angulares de referencia (wref), se ha creado un mensaje con un único campo en su interior, un vector flotante de tres componentes (float64[3]). Este mensaje ha recibido el nombre de calc_wref_msg.msg. Figura 5.4 Sistema de referencia de la cámara según el paper. zC xC yC Figura 6.1 Seguimiento de las velocidades angulares de referencia para el gimbal (prueba 1). 6 ANÁLISIS DE RESULTADOS En este capítulo se muestran los resultados de varias pruebas realizadas siguiendo un objetivo con la cámara del dron en distintas situaciones. Las simulaciones en las que se basan las distintas pruebas han sido grabadas y se pueden ver a través de este enlace de GitHub: https://github.com/ivanf8/target-tracking/blob/main/README.md. Haciendo click en las imágenes que aparecen, se abre el enlace del vídeo correspondiente en YouTube. 6.1. "Seguimiento" de un objetivo estático Se ha realizado esta prueba con dos objetivos distintos, una señal de tráfico y una ambulancia. Señal de tráfico Antes de apuntar al centro del objetivo, la señal se encuentra en la parte superior derecha de la imagen de la cámara. 29 29 Ensayo de técnicas de localización y seguimiento visual para vehículos aéreos no tripulados Figura 6.2 Error respecto al centro de la cámara (prueba 1). Figura 6.3 Seguimiento de las velocidades angulares de referencia para el gimbal (prueba 2). Ambulancia Al principio situamos la cámara de tal manera que la ambulancia quede en la parte inferior izquierda de la imagen. Análisis de resultados 30 30 Figura 6.4 Error respecto al centro de la cámara (prueba 2). Se puede observar cómo en el caso de los objetivos estáticos se consigue centrarlos en la cámara con éxito. El seguimiento de las referencias cumple con los tiempos de subida especificados. Uno de los problemas con los que nos podemos encontrar en estos casos es que el objetivo elegido tenga pocos puntos característicos o que alguno de sus puntos característicos coincida con algún otro punto de la escena, lo que a veces hace que varíen las coordenadas de las esquinas que encuadran al objetivo y el centro del objeto cambia bruscamente por un instante. También aparecen problemas de detección y errores cuando el objetivo está muy alejado de la cámara, lo que dificulta el matching de los keypoints. Por tanto, se consiguen los mejores resultados cuando: - El aspecto del objetivo tiene muchas peculiaridades, lo cual aumenta el número de keypoints. - La cámara está cerca del objetivo en cuestión, facilitando la correspondencia de los puntos clave. 31 31 Ensayo de técnicas de localización y seguimiento visual para vehículos aéreos no tripulados Figura 6.5 Seguimiento de las velocidades angulares de referencia para el gimbal (prueba 3). 6.2. Seguimiento de un objetivo en movimiento Velocidad del objetivo: 2 m/s Figura 6.6 Detalle de la figura anterior (prueba 3). Análisis de resultados 32 32 Figura 6.7 Error respecto al centro de la cámara (prueba 3). En este experimento el objetivo utilizado ha sido el modelo de la camioneta, que se encuentra en la parte inferior derecha de la imagen en un principio. 1. Al principio de la prueba se centra el objetivo en la cámara. 2. El objetivo empieza a moverse hacia la izquierda y se detiene unos segundos. 3. El objetivo se desplaza hacia el otro extremo de la carretera y se vuelve a detener. 4. Por último, regresa al extremo izquierdo y se detiene. Como se comentó en el capítulo 2, no se predice la trayectoria del objetivo, por lo que cuando este está en movimiento nunca va a estar en el centro de la cámara, pero si se va a seguir su trayectoria. Los intervalos en los que los errores de las coordenadas son nulos corresponden a los instantes en los que la camioneta se encuentra parada en uno de los extremos. También se observa cómo sobre el segundo 29 de la simulación se produce un pico en el error. Se observa perfectamente en la Figura 6.8. En ese breve instante se detectan erróneamente las esquinas que rodean al objetivo, por lo que el centro de la cámara varía. Esto hace que aumente el error, lo que provoca a su vez un pico en las velocidades angulares de referencia. 33 33 Ensayo de técnicas de localización y seguimiento visual para vehículos aéreos no tripulados Velocidad del objetivo: 15 m/s Si aumentamos la velocidad, la cámara continúa siguiendo al objetivo, aunque cuanto mayor sea dicha velocidad, mayor es la dificultad para conseguirlo. Se ha realizado una prueba con la camioneta a una velocidad de 15 m/s, donde se puede observar que la alta velocidad provoca que el algoritmo de detección tenga fallos instantáneos que perjudican al control. En esta figura se pueden observar los numerosos fallos en la detección. El objetivo en un momento dado llega a desaparecer por completo de la imagen, pero debido a que se detiene en un cierto punto la cámara es capaz de seguirlo. Figura 6.9 Seguimiento de las velocidades angulares de referencia para el gimbal (prueba 4). Figura 6.8 Error en la estimación del centro del objetivo. Análisis de resultados 34 34 Todos los picos que aparecen en esta figura se corresponden con los errores en la detección de la camioneta, provocando pequeños giros bruscos de la cámara. Velocidad del objetivo: 30 m/s Con esta prueba se buscaba ver la velocidad del objetivo que provocaba el fallo total del seguimiento para este tipo de experimento. En las figuras mostradas en la página siguiente se observan los abundantes picos resultantes del error del algoritmo de detección, llegando en el instante final al fallo, perdiéndose el objetivo fuera de la imagen de la cámara. Figura 6.10 Error respecto al centro de la cámara (prueba 4). 35 35 Ensayo de técnicas de localización y seguimiento visual para vehículos aéreos no tripulados Figura 6.11 Seguimiento de las velocidades angulares de referencia para el gimbal (prueba 5). Figura 27.12 Error respecto al centro de la cámara (prueba 5).Figura 28.11 Seguimiento de las velocidades angulares de referencia para el gimbal (prueba 5). Figura 6.12 Error respecto al centro de la cámara (prueba 5). Análisis de resultados 36 36 Figura 6.13 Seguimiento de las velocidades angulares de referencia para el gimbal (prueba 6). Figura 6.14 Error respecto al centro de la cámara (prueba 6). 6.3. Seguimiento de un objetivo estático con dron en movimiento 37 37 Ensayo de técnicas de localización y seguimiento visual para vehículos aéreos no tripulados En esta prueba el dron se encuentra en movimiento siguiendo una cierta trayectoria cambiante. Como el movimiento del dron es lento debido a sus limitaciones, el seguimiento se cumple sin ningún problema, aunque el objetivo no aparece del todo centrado en la cámara debido al los movimientos realizados por el cuerpo del dron. Apéndice A: códigos de ROS 44 44 move_msg.pose.position.z = 0 move_msg.pose.orientation.x = resp_coordinates.pose.orientation.x move_msg.pose.orientation.y = resp_coordinates.pose.orientation.y move_msg.pose.orientation.z = resp_coordinates.pose.orientation.z move_msg.pose.orientation.w = resp_coordinates.pose.orientation.w ########################################################### pub.publish(move_msg) rate.sleep() except rospy.ServiceException as e: rospy.loginfo("Get Model State service call failed: {0}".format(e)) if __name__ == '__main__': try: show_gazebo_models() except rospy.ROSInterruptException: pass 45 45 Ensayo de técnicas de localización y seguimiento visual para vehículos aéreos no tripulados Referencias 46 46 REFERENCIAS [1] JÁÑEZ VAZ, Isidro Marcelo, 2020. Modelado y Simulación de un Quadrotor con Gimbal mediante Gazebo y ROS. Trabajo Fin de Grado Inédito. Universidad de Sevilla, Sevilla. [2] CUNHA, Rita, et al. Gimbal control for vision-based target tracking. En Proceedings of the European Conference on Signal Processing. 2019. [3] BORREGO PRADO, Fernando Manuel, 2020. Modelado y Control de un Quadrotor perior de Ingeniería con Gimbal mediante la perspectiva Fly-The-Camera. Trabajo Fin de Grado Inédito. Universidad de Sevilla, Sevilla. [4] Colaboradores de Wikipedia. Vehículo aéreo no tripulado. Wikipedia, La enciclopedia libre [en línea]. 2021 [fecha de consulta: 13 de septiembre del 2021]. Disponible en <https://es.wikipedia.org/w/index.php?title=Veh%C3%ADculo_a%C3%A9reo_no_tripulado&oldid =137413486>. [5] ALEGRE GUTIÉRREZ, Enrique, et al. SIFT (Scale Invariant Feature Transform). [6] Tutorial: Beginner: Intro. Gazebo [en línea]. [sin fecha] [fecha de consulta: 7 de agosto de 2021]. Disponible en: http://gazebosim.org/tutorials?cat=guided_b&tut=guided_b1. [7] About ROS. ROS.org [en línea]. [sin fecha] [fecha de consulta: 7 de agosto de 2021]. [sin fecha] Disponible en: https://www.ros.org/about-ros/. [8] HESS, Rob. SIFT Features. OpenSIFT: An Open-Source SIFT Library [en línea]. [sin fecha] [fecha de consulta: 10 de agosto de 2021]. Disponible en: https://robwhess.github.io/opensift/. [9] Introduction to SIFT (Scale-Invariant Feature Transform). OpenCV [en línea]. [sin fecha] [fecha de consulta: 10 de agosto de 2021]. Disponible en: https://docs.opencv.org/4.5.1/da/df5/tutorial_py_sift_intro.html. [10] YILMAZ, Alper; JAVED, Omar; SHAH, Mubarak. Object tracking: A survey. Acm computing surveys (CSUR), 2006, vol. 38, no 4, p. 13-es. [11] Nodos, Topics y Mensajes. ROS TUTORIAL [en línea]. [sin fecha] [fecha de consulta: 13 de agosto de 2021]. Disponible en: http://rostutorial.com/4-nodos-topics-y-mensajes-turtlesim/. [12] PETROVICHEVA, Anna. Multiple Object Tracking in Realtime. OpenCV [en línea]. 27 de octubre de 2020 [fecha de consulta: 16 de agosto de 2021]. Disponible en: https://opencv.org/multipleobject-tracking-in-realtime/. [13] CHALLA, Subhash, et al. Fundamentals of object tracking. Cambridge University Press, 2011. 47 47 Ensayo de técnicas de localización y seguimiento visual para vehículos aéreos no tripulados [14] TYAGI, Deepanshu. Introduction to SIFT. Medium [en línea]. 16 de marzo de 2019 [fecha de consulta: 14 de agosto de 2021]. Disponible en: https://medium.com/data-breach/introduction-to- sift-scale-invariant-feature-transform-65d7f3a72d40. [15] How to Move a Gazebo Model from Terminal. VarHowto [en línea]. 15 de mayo de 2020 [fecha de consulta: 23 de agosto de 2021]. Disponible en: https://varhowto.com/how-to-move-a-gazebo- model-from-command-line-ros/. [19] HERRERO, Fernando. iri_imu_gazebo. GitLab [en línea]. 11 de febrero de 2021 [fecha de consulta: 15 de agosto de 2021]. Disponible en: https://gitlab.iri.upc.edu/labrobotica/ros/sensors/imu/iri_imu_gazebo/-/tree/master. [20] Tutorial: Make an animated model (actor). Gazebo [en línea]. [sin fecha] [fecha de consulta: 20 de agosto de 2021]. Disponible en: http://gazebosim.org/tutorials?tut=actor&cat=build_robot. [21] DMITRY, Devitt. image_pose_estimation. GitHub [en línea]. 21 de marzo de 2019 [fecha de consulta: 9 de junio de 2021]. Disponible en: https://github.com/GigaFlopsis/image_pose_estimation. [22] LENTIN, Aleena. What is ROS? Robocademy [en línea]. 1 de julio de 2020 [fecha de consulta: 11 de agosto de 2021]. Disponible en: https://robocademy.com/2020/07/01/what-is-ros/. Glosario 48 48 GLOSARIO UAV: Unmanned Aerial Vehicle 4 VANT: Vehículo Aéreo No Tripulado 4 RPAS: Remotely Piloted Aircraft System 4 ROS: Robot Operating System 4 IMU: Inertial Measurement Unit 4 SIFT: Scale Invariant Feature Transform 4 PID: Proporcional Integral Derivativo 4 PI: Proporcional Integral 4 GPS: Global Positioning System 4