Control servo-visual de una plataforma giroestabilizada

Equation Chapter 1 Section 1 Trabajo Fin de Grado Grado en Ingeniería de Tecnologías Industriales Control servo-visual de una plataforma giroestabilizada Dpto. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Autor: José Ignacio Murillo Álvarez Tutor: Daniel Rodríguez Ramírez Sevilla, 2021 iii Trabajo Fin de Grado Grado en Ingeniería de Tecnologías Industriales Control servo-visual de una plataforma giroestabilizada Autor: José Ignacio Murillo Álvarez Tutor: Daniel Rodríguez Ramírez Profesor titular Dpto. de Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2021 v Trabajo Fin de Grado: Control servo-visual de una plataforma giroestabilizada Autor: José Ignacio Murillo Álvarez Tutor: Daniel Rodríguez Ramírez 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 vii A mi familia A mis maestros ix Agradecimientos En primer lugar, agradecer a toda mi familia el esfuerzo que supone educar, formar y promover el estudio en una persona desde que es un niño hasta que se convierte en un adulto. Gracias a mis amigos, por permitirme desconectar de las duras horas de estudio como ellos mejor saben. A Rocío, por apoyarme y darme fuerzas durante todo el desarrollo del TFG, que no ha sido fácil para ninguno. A Juan, por el apoyo técnico en materia audiovisual. También agradecer a todos los profesores de la Escuela que me hayan hecho un poco mejor ingeniero en cada una de mis etapas, especialmente a Daniel, el tutor de este trabajo. Finalmente, agradecer a Hernán por toda la ayuda con el proyecto, así como por poder haber aprendido muchísimo de él durante todo el tiempo que hemos trabajado juntos. José Ignacio Murillo Álvarez Sevilla, 2021 ÍNDICE DE TABLAS TABLA 2–1. CARACTERÍSTICAS MECÁNICAS Y ELÉCTRICAS DE PIXY U...................................................................................... 12 TABLA 2–2. CARACTERÍSTICAS DE TRABAJO DE PIXY U ............................................................................................................. 13 TABLA 3–1. ESPECIFICACIONES CÁMARA SONY RX0 II .............................................................................................................. 18 TABLA 5–1. POSICIÓN RELATIVA DE LA CÁMARA EN LA TOMA DE IMÁGENES ...................................................................... 32 TABLA 5–2. RESULTADOS DE LA TRIANGULACIÓN (METROS) .................................................................................................. 33 TABLA 7–1. PARÁMETROS DE CONTROL. PRUEBA 1 ................................................................................................................. 47 xvii ÍNDICE DE FIGURAS FIGURA 2-1. GIRO POSITIVO DE ÁNGULO 𝜽 DE UN VECTOR 𝒗𝒐 [6].......................................................................................... 3 FIGURA 2-2. ROTACIONES CON ÁNGULOS DE EULER [41] .......................................................................................................... 4 FIGURA 2-3. ÁNGULOS DE EULER EN UN AVIÓN [42] ................................................................................................................. 4 FIGURA 2-4. INSCRIPCIÓN EN BROOM BRIDGE, DUBLÍN, EN REFERENCIA A LA FÓRMULA DE LOS CUATERNIOS ESCRITA POR HAMILTON .................................................................................................................................................................... 5 FIGURA 2-5. ROTACIÓN DE UN CUERPO EJE-ÁNGULO [44] ........................................................................................................ 7 FIGURA 2-6. ESQUEMA DEL MODELO GEOMÉTRICO DE LA PLATAFORMA CON ÁNGULOS DE EULER .................................. 8 FIGURA 2-7. ESQUEMA DEL MODELO GEOMÉTRICO DE LA PLATAFORMA CON CUATERNIOS .............................................. 9 FIGURA 2-8. APOLLO IMU [11] .................................................................................................................................................... 10 FIGURA 2-9. PIXY U ....................................................................................................................................................................... 12 FIGURA 3-1. MODELO DE LA CÁMARA PINHOLE [13] ............................................................................................................... 15 FIGURA 3-2. MODELO DE CÁMARA OSCURA O ESTENOPEICA [13] ......................................................................................... 16 FIGURA 3-3. MODELADO DE DISTORSIÓN DE LENTE EN COMPONENTE RADIAL Y TANGENCIAL [13] ................................. 17 FIGURA 3-4. CÁMARA SONY RX0 II [17] ...................................................................................................................................... 18 FIGURA 4-1. REPRESENTACIÓN DEL CUATERNIO 𝒒𝒕 ................................................................................................................. 20 FIGURA 4-2. ESQUEMA EN SIMULINK DE LA SIMULACIÓN DEL CONTROLADOR ................................................................... 21 FIGURA 4-3. OBTENCIÓN DEL CUATERNIO DESEADO EN LA SIMULACIÓN ............................................................................. 22 FIGURA 4-4. ESQUEMA PARA EL CÁLCULO DE LAS VELOCIDADES ANGULARES DESEADAS .................................................. 22 FIGURA 4-5. REPRESENTACIÓN DE 𝒒𝒕, 𝒒𝒆, 𝒒 EN LA SIMULACIÓN........................................................................................... 23 FIGURA 4-6. REPRESENTACIÓN DEL VECTOR DESEADO, VECTOR ACTUAL Y EL PRODUCTO VECTORIAL ............................. 24 FIGURA 4-7. REPRESENTACIÓN DEL ÁNGULO DE ERROR Y LA VELOCIDAD ANGULAR DESEADA ......................................... 24 FIGURA 5-1. ETAPAS DEL PROCESO DE ANÁLISIS DE IMÁGENES [47] ...................................................................................... 25 FIGURA 5-2. EJEMPLO DE RECONOCIMIENTO DE APRILTAGS EN UNA IMAGEN .................................................................... 27 FIGURA 5-3. TRIANGULACIÓN CON DOS IMÁGENES [16] ......................................................................................................... 28 FIGURA 5-4. PATRÓN USADO PARA LA CALIBRACIÓN. TABLERO DE AJEDREZ 10X7 .............................................................. 29 FIGURA 5-5. EJEMPLO DE LAS IMÁGENES TOMADAS PARA LA CALIBRACIÓN........................................................................ 30 FIGURA 5-6. EJE DE COORDENADAS DE LA CÁMARA Y DE LA IMAGEN ................................................................................... 31 FIGURA 5-7. IMÁGENES TOMADAS PARA LA TRIANGULACIÓN. RESPECTIVAMENTE, IMAGEN 1,2 Y 3 ............................... 32 FIGURA 5-8. IMÁGENES USADAS PARA EL CÁLCULO DE LA TRIANGULACIÓN ........................................................................ 33 FIGURA 5-9. REPRESENTACIÓN EN EL ESPACIO DEL RESULTADO DE LA TRIANGULACIÓN.................................................... 34 FIGURA 5-10. ITERACIÓN EN FILTRO DE KALMAN [50] ............................................................................................................. 36 FIGURA 6-1. ESTRUCTURA DE UN MENSAJE MAVLINK [32] ..................................................................................................... 38 FIGURA 6-2. COMUNICACIÓN ENTRE DOS NODOS, POR MENSAJES A TRAVÉS DE UN TÓPICO [51] .................................... 40 FIGURA 6-3. DIFERENCIA ENTRE COMUNICACIÓN POR TOPIC Y SERVICIOS [36] ................................................................... 41 FIGURA 6-4. ESQUEMA DEL PROGRAMA DE TRACKING ........................................................................................................... 41 FIGURA 6-5. RECTÁNGULO USADO REPRESENTADO EN EL PLANO IMAGEN.......................................................................... 43 FIGURA 6-6. INTERFAZ GRÁFICA PARA INTRODUCIR LOS PARÁMETROS DESEADOS ............................................................. 45 FIGURA 6-7. RQT_GRAPH DURANTE EL FUNCIONAMIENTO DEL PROGRAMA ....................................................................... 46 FIGURA 6-8. NODOS Y TÓPICOS DEL PROGRAMA COMPLETO ................................................................................................. 46 FIGURA 7-1. REPRESENTACIÓN GRÁFICA DE CUATERNIOS. PRUEBA 1 ................................................................................... 48 FIGURA 7-2. REPRESENTACIÓN GRÁFICA DEL VECTOR DE APUNTAMIENTO. PRUEBA 1 ....................................................... 48 FIGURA 7-3. REPRESENTACIÓN DE LAS VELOCIDADES ANGULARES. PRUEBA 1 ..................................................................... 49 FIGURA 7-4. REPRESENTACIÓN DE LA ORIENTACIÓN EXPRESADA EN ÁNGULOS DE EULER. PRUEBA 1 .............................. 49 FIGURA 7-5. REPRESENTACIÓN GRÁFICA DEL VECTOR DE POSICIÓN DEL OBJETIVO ESTIMADO. PRUEBA 1. ..................... 50 FIGURA 7-6. REPRESENTACIÓN GRÁFICA DEL VECTOR DE POSICIÓN. AMPLIADO. PRUEBA 1 .............................................. 51 FIGURA 7-7. REPRESENTACIÓN DE LA DISTANCIA EN PÍXELES AL CENTRO DEL OBJETO EN EL PLANO IMAGEN. PRUEBA 1 .............................................................................................................................................................................................. 51 FIGURA 7-8. TRAZA DE LA MATRIZ P. PRUEBA 1 ........................................................................................................................ 52 FIGURA 7-9. TRAZA MATRIZ P. AMPLIADO. PRUEBA 1.............................................................................................................. 52 FIGURA 7-10. TRAZA MATRIZ K. PRUEBA 1. ............................................................................................................................... 53 FIGURA 7-11. COMPONENTES DE LA MATRIZ A. PRUEBA 1 ..................................................................................................... 53 FIGURA 7-12. IMÁGENES DE LA PRUEBA .................................................................................................................................... 54 FIGURA 7-13. REPRESENTACIÓN GRÁFICA DE CUATERNIOS. PRUEBA 2 ................................................................................. 55 FIGURA 7-14. REPRESENTACIÓN GRÁFICA DEL VECTOR DE POSICIÓN. AMPLIADO. PRUEBA 1 ............................................ 55 FIGURA 7-15. REPRESENTACIÓN GRÁFICA DE CUATERNIOS. PRUEBA 3 ................................................................................. 56 FIGURA 7-16. REPRESENTACIÓN GRÁFICA DE ORIENTACIÓN EN ÁNGULOS DE EULER. PRUEBA 3 ...................................... 57 FIGURA 7-17. REPRESENTACIÓN DE CUATERNIOS. PRUEBA 4 ................................................................................................. 58 FIGURA 7-18. REPRESENTACIÓN DEL VECTOR DEL OBJETIVO RELATIVO A LA CÁMARA. PRUEBA 4 .................................... 59 FIGURA 7-19. REPRESENTACIÓN DE LA DISTANCIA EN PÍXELES AL CENTRO DEL OBJETO EN EL PLANO IMAGEN. PRUEBA 4 .............................................................................................................................................................................................. 59 xix Notación A* Conjugado sin Función seno cos ln Función coseno Función logaritmo natural e A 󰇍 󰇍 Número e Vector AT Vector o matriz traspuesta ℝ Números reales ‖A‖ Norma 2 tanh Tangente hiperbólica × Producto vectorial 1 1 INTRODUCCIÓN n los últimos años, el uso de los UAV (Unmaned Aerial Vehicle), también conocidos como drones, ha tenido un notable aumento, mostrándose muy útiles en diversas tareas. Entre ellas, la agricultura de precisión [1], en la cual se busca la toma de imágenes georeferenciadas para su posterior procesamiento y usarlas como herramienta para optimizar los cultivos. Por su naturaleza, los UAV son idóneos para este tipo de trabajos. Otra de las actividades más desarrolladas por estos vehículos aéreos es el seguimiento de objetivos móviles [2], donde se utilizan las ventajas de estos robots para seguir y detectar objetivos diferentes desde el aire. También es digno de mención el uso de los UAV para servicios de reparto [3]. Para desempeñar todas estas tareas de la forma óptima y sin inconvenientes, es necesario disponer de diferentes dispositivos que surtan al UAV de información obtenida de su entorno. Uno de los sensores más usados son las cámaras de vídeo, las cuales ayudan a los drones a conocer cómo se estructura su alrededor. Viendo las actividades que hemos presentado en el párrafo anterior, nos podemos hacer una idea de la importancia de que un UAV lleve una cámara, ya sea para obtener imágenes o para obtener un flujo de vídeo en directo que nos permita reconocer o detectar un objetivo que deseamos seguir. Debido a las perturbaciones que un dron soporta durante el vuelo, movimientos bruscos o vientos, la cámara acoplada al sistema debe estar integrada en una plataforma giroestabilizada (gimbal). Una plataforma giroestabilizada, o gimbal, es un dispositivo electromecánico cuyo objetivo es el mantener estable una cámara ante perturbaciones debidas a, por ejemplo, fricciones en los motores, aerodinámicas no balanceadas, fuerzas de torsión y otras. Las cámaras estabilizadas mediante gimbal se usan para diversos escenarios, tanto civiles como militares: fotografía aérea, vigilancia, seguimiento de misiles y navegación autónoma. Si la cámara no se estabiliza correctamente, es imposible capturar las imágenes de forma nítida en estas aplicaciones, lo cual es clave en su exitosa puesta en marcha [4]. 1.1 Objetivos El problema a resolver en este trabajo será el de un algoritmo de tracking mediante el uso de una plataforma gimbal de tres grados de libertad, en la cual estará acoplada una cámara de video. El algoritmo de control debe fijar el objetivo en el centro del plano de la imagen, de forma que facilite un seguimiento correcto de este, así como permita realizar diferentes procesos sobre la imagen deseada. En primer lugar, debemos acometer la detección del objetivo mediante el uso del flujo de vídeo obtenido mediante la cámara. Una vez reconocido, solo conoceremos su posición en el plano imagen, por lo que estimaremos la posición que tiene respecto a la plataforma, de forma que nos facilite el control de esta. Una vez tengamos esta posición, actuaremos sobre la plataforma giroestabilizada para conseguir centrar el objetivo en la imagen. 1.2 Metodología Para abordar los objetivos descritos anteriormente, necesitaremos diversas herramientas. En primer lugar, para resolver los problemas geométricos y cinemáticos propios del control del gimbal, utilizaremos herramientas como los Ángulos de Euler, y, especialmente, los Cuaternios. La detección del objetivo la llevaremos a cabo mediante la librería AprilTag, que nos permite usar algoritmos de detección sin tener que programarlos por nuestra propia mano, lo cual se escapa del alcance de este trabajo. Acometeremos la obtención de la posición del objetivo con técnicas de fotogrametría, como la triangulación. La comunicación entre los diferentes sistemas la implementaremos mediante ROS (Robot Operating System), un sistema operativo que ofrece un protocolo de comunicaciones y sincronización entre las diferentes partes de E Introducción 2 un sistema robótico. Para validar el cumplimiento de los objetivos, llevaremos a cabo algunas simulaciones mediante Matlab- Simulink y, fundamentalmente, realizaremos experimentos con la plataforma gimbal real que vamos a controlar. 1.3 Sumario En el Capítulo 2, trataremos de obtener un modelo geométrico de la plataforma que nos permita representar la orientación de esta, así como una herramienta para poder desarrollar el algoritmo de control, meta del trabajo. También presentaremos el modelo real de la plataforma que vamos a usar. A continuación, en el Capítulo 3, definiremos el modelo geométrico de la cámara que vamos a usar. Análogamente a lo que se hizo en el Capítulo 2, presentaremos el modelo real de este dispositivo. Continuaremos con el Capítulo 4, en el cual se presentará el algoritmo de control para la plataforma gimbal. Luego, en el Capítulo 5, seguiremos con la obtención de la posición del objetivo con respecto a la plataforma mediante técnicas de fotogrametría. Posteriormente, en el Capítulo 6, definiremos las comunicaciones y trataremos la integración de los diferentes dispositivos que hemos definido, la cámara, la plataforma y la CPU encargada de ejecutar los programas. Una vez hemos definido el sistema completo, mostraremos y analizaremos los resultados de los experimentos y pruebas llevados a cabo en el sistema real, todo ello en el Capítulo 7. Para terminar, en el Capítulo 8, finalizaremos con las conclusiones y los posibles trabajos futuros que derivan de este trabajo. 3 Figura 2-1. Giro positivo de ángulo 𝜽 de un vector 𝒗𝒐 [6] 2 MODELO DE LA PLATAFORMA ara comenzar, es necesario desarrollar y definir correctamente el modelo geométrico de la plataforma que vamos a usar en este trabajo, y que nos será necesario de cara a diseñar el algoritmo de control que tenemos como objetivo. 2.1 Herramientas matemáticas Vamos a presentar dos de las herramientas más usadas en este tipo de sistemas para representar giros y orientaciones. Los Ángulos de Euler y los Cuaternios. 2.1.1 Ángulos de Euler El Teorema de Rotación de Euler afirma que la rotación arbitraria de un sólido puede ser descrita por tres parámetros. Estos parámetros son los Ángulos de Euler [5]. Estas rotaciones pueden ser escritas en forma de matrices de rotación. Una matriz de rotación representa el giro de un ángulo θ contrario a las agujas del reloj de un vector v dado [6]. La matriz de rotación que representa este giro es: 𝑅θ =[cosθ sinθ −sinθ cosθ ] (2.1) Las matrices correspondientes a giros en tres dimensiones corresponden a las siguientes: 𝑅𝑥(𝛼)=[1 0 0 0 cos𝛼 sin𝛼 0 −sin𝛼 cos𝛼] (2.2) P Modelo de la plataforma 4 Figura 1. Figura 1. Figura 2-2. Rotaciones con Ángulos de Euler [41] Figura 2-3. Ángulos de Euler en un avión [42] 𝑅𝑦(𝛽)=[cos𝛽 0 −sin𝛽 0 1 0 sin𝛽 0 cos𝛽] (2.3) 𝑅𝑧(𝛾)=[cos𝛾 sin𝛾 0 −sin𝛾 cos𝛾 0 0 0 1] (2.4) Los Ángulos de Euler podemos dibujarlos de esta forma: Para entenderlos mejor, veamos un ejemplo de cómo se usarían estos parámetros para definir las rotaciones que tienen lugar en un avión: Usando matrices de rotación, podemos establecer las rotaciones de la figura como: 𝐷=𝑅𝑧(𝜙) 𝐶=𝑅𝑥(𝜃) 𝐵=𝑅𝑦(𝜓) (2.5) Donde 𝜙,𝜃, 𝜓 son los Ángulos de Euler (respectivamente, yaw, roll, pitch). 5 Control servo-visual de una plataforma giroestabilizada Figura 2-4. Inscripción en Broom Bridge, Dublín, en referencia a la fórmula de los cuaternios escrita por Hamilton La matriz que expresa la rotación total del sólido, siguiendo el orden de rotaciones de la figura anterior es: 𝐴=𝐵∗𝐶∗𝐷 (2.6) 2.1.2 Cuaternios En 1843, William Rowan Hamilton, creaba una versión en tres dimensiones de los números complejos, siendo llamados números hipercomplejos, escribiendo una famosa ecuación en una piedra del Broom Bridge, Dublín, en la que se basaría el algebra de los cuaternios [7]. 𝑖2=𝑗2=𝑘2=𝑖𝑗𝑘=−1 (2.7) Los cuaternios se pueden denotar como: 𝑞=𝑞𝑜+𝐪=𝑞𝑜+𝑖𝑞1+𝑗𝑞2+𝑘𝑞3 (2.8) Donde i, j, k forman la base ortonormal estándar en ℝ3. En este caso, 𝑞𝑜 es llamado la parte escalar del cuaternio, mientras que q es llamado la parte vectorial del cuaternio. Los escalares 𝑞𝑜,𝑞1,𝑞2,𝑞3 son llamados los componentes del cuaternio. 2.1.2.1 Conjugado del cuaternio El conjugado del cuaternio: 𝑞=𝑞𝑜+𝐪=𝑞𝑜+𝑖𝑞1+𝑗𝑞2+𝑘𝑞3 (2.9) Es denotado como 𝑞∗ , y obtenido como: 𝑞=𝑞𝑜−𝐪=𝑞𝑜−𝑖𝑞1−𝑗𝑞2−𝑘𝑞3 (2.10) Este conjugado cumple: Modelo de la plataforma 12 Figura 2-9. Pixy U 2.3 Descripción del sistema real La plataforma gimbal que se va a usar en este trabajo es la Pixy U de Gremsy [12]. Es un gimbal que soporta diferentes tipos de cámaras, compacto y ligero, idóneo para el tipo de actividades que se llevan a cabo con UAV, como la que se desarrolla en nuestro proyecto. Tabla 2–1. Características mecánicas y eléctricas de Pixy U Corriente de trabajo Corriente estática: 400mA (@12V) Corriente dinámica: 800mA (@12V) Corriente locked motor: Max 4.0A (@12V) Temperatura de operación 0º C ~ 50º C Peso 0.465 kg Dimensión 112(W) x 145(D) x 200(H) 13 Control servo-visual de una plataforma giroestabilizada Tabla 2–2. Características de trabajo de Pixy U Rango de vibración angular ± 0.02° Carga máxima 0.465 kg Máxima velocidad de rotación controlada Eje yaw: 100º/s Eje pitch: 100º/s Eje roll: 100º/s Rango mecánico Eje yaw: +330° a -330° Eje pitch: +135° a -45° Eje roll: +45° a -90° Rango controlado de rotación Eje yaw: +330° a -330° Eje pitch: +135° a -45° Eje roll: ± 45° 15 Figura 3-1. Modelo de la cámara pinhole [13] 3 MODELO DE LA CÁMARA ontinuando con el capítulo anterior, tras establecer un modelo geométrico de la plataforma con distintas herramientas definidas, en este capítulo vamos a introducir los diferentes modelos matemáticos con los que vamos a trabajar en la introducción en el sistema de la cámara de vídeo, que será la encargada de proporcionar esa información necesaria para que el algoritmo de control actúe de la forma requerida en cada momento. 3.1 Cámara Pinhole El modelo de cámara pinhole consiste en un centro óptico C en el cual convergen los rayos de la proyección, y un plano de imagen R en el cual la imagen es proyectada. El plano de imagen está ubicado a una distancia focal f del centro óptico y perpendicular al eje óptico [13]. El punto M en 3D se proyecta en la imagen como el punto m, que es definido por la intersección de la recta que une C y M con el plano de imagen R. La ecuación que define la relación entre estos dos puntos en coordenadas homogéneas es: 𝑍[𝑥𝑦1]=[𝑓 0 0 𝑓 0 0 0 0 0 0 1 0][𝑋𝑌𝑍1] (3.1) Donde 𝑋,𝑌,𝑍 son las coordenadas en el espacio del punto M; 𝑥,𝑦 son las coordenadas en el plano imagen del punto m y 𝑓 es la distancia focal. Otra forma muy común de representar este modelo de cámara es mediante el modelo geométrico de proyección de cámara oscura o estenopeica, en el cual el centro óptico está situado entre el punto M y el plano de imagen R. Este modelo está basado en la cámara oscura, un instrumeto óptico que consistía en una caja cerrada y un pequeño agujero en el que entraban los rayos de luz que proyectaban la imagen en la pared opuesta [14]. C Modelo de la cámara 16 Figura 3-2. Modelo de cámara oscura o estenopeica [13] En este modelo, es necesario cambiar en la ecuación matricial 𝑓 por −𝑓: 𝑍[𝑥𝑦1]=[−𝑓 0 0 −𝑓 0 0 0 0 0 0 1 0][𝑋𝑌𝑍1] (3.2) 3.2 Cámara con sensor CCD o CMOS Las cámaras reales actuales funcionan con elementos semiconductores fotosensibles que forman una matriz. En el caso del CCD, las cargas de las celdas de la matriz se convierten en un voltaje que entrega una señal analógica, que posteriormente es digitalizada por la cámara. En el sensor CMOS, cada celda es independiente, y la transformación analógico-digital tiene lugar en cada una de las celdas mediante un transistor [15]. A la hora de hablar de la formación geométrica de la imagen, tenemos que considerar cuatro aspectos que van a afectar a la misma cuando usamos una cámara de vídeo real [13]: • Cambio de escala: Para transformar las coordenadas que tenemos en una imagen, que están expresadas en píxeles (𝑢,𝑣), a coordenadas en milímetros (𝑥,𝑦), necesitamos un factor de escala. Además, debido a que los píxeles no son cuadrados, sino que son rectangulares, tendremos uno diferente para cada eje de la imagen. Estos factores se expresan como 𝛼𝑥 y 𝛼𝑦, expresados en píxel/mm. • Traslación del origen: Tomamos las variables 𝑢𝑜 y 𝑣𝑜 para definir el punto de origen de la imagen en el nuevo sistema de coordenadas. • Rotación de los ejes: Los ejes 𝑥,𝑦 y los ejes 𝑢,𝑣 no tienen la misma orientación. Existen dos métodos en la modelación de la cámara. Uno que supone que el ángulo entre los ejes es cero y que el ajuste debe hacerse en los ejes 𝑋,𝑌,𝑍 y otro que considera un ángulo θ entre los ejes. • Factor de torcimiento: Los ejes 𝑢,𝑣 pueden no ser ortogonales debido a que los píxeles en los arreglos CCD o CMOS no son completamente rectangulares. En este caso, se introduce un factor de torcimiento (skew) 𝑠. Normalmente, este parámetro suele ser cero en la mayoría de las cámaras. 17 Control servo-visual de una plataforma giroestabilizada Figura 3-3. Modelado de distorsión de lente en componente radial y tangencial [13] Finalmente, considerando que la orientación de los ejes 𝑢,𝑣 y 𝑥,𝑦 es la misma, la transformación de coordenadas está definida por: [𝑢𝑣1]=[𝛼𝑥𝑠 0 𝛼𝑦 0 0 𝑢𝑜 𝑣𝑜 1][𝑥𝑦1] (3.3) Donde la matriz definida por los parámetros que hemos listado anteriormente es conocida como la matriz de calibración de la cámara. También vamos a introducir los parámetros externos de la cámara, que no son más que la transformación entre el eje de coordenadas global y el eje de coordenadas de la cámara [16]: [𝑋𝑐𝑎𝑚 𝑌𝑐𝑎𝑚 𝑍𝑐𝑎𝑚 1]=[𝑅 𝑡 0𝑇1][𝑋𝑌𝑍1] (3.4) Donde R es una matriz de rotación 3x3 y t es un vector de translación 3x1. Finalmente, considerando las ecuaciones (3.2), (3.3) y (3.4), definimos la matriz de proyección desde el espacio en 3D hasta la imagen: [𝑢𝑣1]=𝑃[𝑋𝑌𝑍1] 𝑃=𝐾[𝑅|𝑡] (3.5) Otro de los errores que introducen las cámaras comerciales a la proyección de la imagen es la distorsión provocada por la lente. Esto hace que las líneas rectas en 3D ya no sean vistas como líneas rectas en la proyección, sino como líneas curvas. La distorsión es modelada generalmente como una componente radial y otra tangencial. La distorsión radial asume que un punto ideal (𝑥,𝑦) se proyecta en la imagen sobre la línea radial que une el punto con el centro de la imagen, afectando la distorsión mediante un cambio en la magnitud del radio. La distorsión tangencial asume que el cambio que existe tiene lugar en el ángulo de esa línea radial. Modelo de la cámara 18 Figura 3-4. Cámara Sony RX0 II [17] 3.3 Descripción del Sistema real La cámara que vamos a usar en este proyecto es la Sony RX0 II [17]. Tabla 3–1. Especificaciones cámara Sony RX0 II Tipo de sensor Sensor CMOS Exmor RS® tipo 1.0 (13,2 x 8,8 mm) Número de píxeles efectivos Aprox. 15,3 megapíxeles Tipo de lente Lente ZEISS® Tessar T* Distancia focal f = 7,9 mm Temperatura de funcionamiento 0º C. - +40 º C Tamaño 59 x 40,5 x 35 mm Peso Aprox. 132 g (batería y microSD incluidas) / aprox. 117 g (solo el cuerpo) Apertura F4.0 Ángulo de visión 84º 19 4 CONTROL DE LA PLATAFORMA l primer problema al que vamos a enfrentarnos en este trabajo es el de diseñar un algoritmo de control para la plataforma gimbal, en el que tendremos en cuenta que la información de la que dispondremos es la que la cámara nos proporciona mediante el flujo de vídeo. 4.1 Diseño del controlador Para este tipo de problemas hay diversas soluciones. Nosotros vamos a escoger una en la que supondremos que la posición del objetivo a seguir está definida (este problema será estimado en el Capítulo 5), ya que, además de que saber la posición del objetivo nos facilita el control de la plataforma, también es útil en general para el seguimiento de objetivos desde un UAV. En [9], se diseña un algoritmo basado en cuaternios para controlar la orientación de un quadrotor, un dron con cuatro rotores. Nosotros vamos a adaptarlo a nuestra plataforma. Definimos los siguientes parámetros: 𝑛 󰇍 𝑥=Vector normalizado de origen 𝑛 󰇍 𝑢=Vector normalizado de apuntamiento deseado 𝑞𝑑=Cuaternio que define la rotación más corta entre los vectores Recordando la ecuación (2.23), 𝑞𝑑 es definido como: 𝑞𝑑 =𝑒12𝜃𝑑𝑢 󰇍 󰇍 𝑑=cos(𝜃𝑑/2)+𝑢 󰇍 𝑑sin (𝜃𝑑/2) (4.1) Donde 𝜃𝑑 y 𝑢 󰇍 󰇍 󰇍 𝑑 son, respectivamente, el ángulo y el eje de la rotación más corta. El producto vectorial entre ambos vectores es definido como: 𝑛 󰇍 𝑢×𝑛 󰇍 𝑥=𝑢 󰇍 𝑑sin(𝜃𝑑) (4.2) Mientras que el producto escalar es: 𝑛 󰇍 𝑢∙𝑛 󰇍 𝑥=𝑐𝑜𝑠(𝜃𝑑) (4.3) Nos ayudaremos de algunas igualdades trigonométricas como: cos(𝜃𝑑/2)=±√1+𝑐𝑜𝑠(𝜃𝑑) 2 ; sin(𝜃𝑑/2)=±√1−𝑐𝑜𝑠(𝜃𝑑) 2 (4.4) Entonces, el cuaternio que alinea el vector 𝑛 󰇍 𝑥 con el vector 𝑛 󰇍 𝑢 se define como: 𝑞𝑡 =±√1+𝑛 󰇍 𝑢∙𝑛 󰇍 𝑥 2+𝑛 󰇍 𝑢×𝑛 󰇍 𝑥 ‖𝑛 󰇍 𝑢×𝑛 󰇍 𝑥‖√1−𝑛 󰇍 𝑢∙𝑛 󰇍 𝑥 2 (4.5) Se puede añadir una rotación adicional sobre el eje X (rotación en roll): E Control de la plataforma 20 Figura 4-1. Representación del cuaternio 𝒒𝒕 𝑞𝑑 =𝑞𝑡∘𝑞𝑧 (4.6) Si denotamos 𝑞 como el cuaternio que define la orientación de la plataforma, podemos considerar el cuaternio de error como: 𝑞𝑒 =𝑞𝑑∗∘𝑞 (4.7) Los ángulos de rotación de error se obtienen con las ecuaciones (4.24) y (4.25): 𝜃𝑒 =2ln (𝑞𝑒) (4.8) El objetivo del controlador es que 𝑞 → 𝑞𝑑 , lo que supone que 𝑞𝑒→(1,0,0,0), debido a que 𝑞 y 𝑞𝑑 son cuaternios unitarios, y su inversa es igual que su conjugado, y 𝑞−1𝑞=(1,0,0,0)=1+0𝑖+0𝑗+0𝑘=1. La velocidad angular deseada para conseguir la rotación deseada la obtendremos mediante el siguiente controlador: 𝑤𝑒 =−𝛾∗tanh(‖𝐾𝑝ln(𝑞𝑒)+𝐾𝑑∗𝑤‖ 𝛾)∗𝐾𝑝ln (𝑞𝑒)+𝐾𝑑∗𝑤 ‖𝐾𝑝ln (𝑞𝑒)+𝐾𝑑∗𝑤‖ (4.9) Siendo 𝐾𝑝 y 𝐾𝑑 constantes proporcionales y derivativas, 𝛾 un parámetro de saturación de la actuación y 𝑤 la velocidad angular actual de la plataforma. 21 Control servo-visual de una plataforma giroestabilizada Figura 4-2. Esquema en Simulink de la simulación del controlador 4.2 Simulación del controlador Para validar el funcionamiento del controlador vamos a realizar una simulación en el entorno de Matlab- Simulink [18]. Hemos dividido el entorno de la simulación en tres partes: • Parámetros de entrada (Azul): Aquí introducimos los vectores 𝑛 󰇍 𝑢 y 𝑛 󰇍 𝑥, así como las constantes de control 𝐾𝑝, 𝐾𝑑 y 𝛾. • Esquema de control (Verde): Es donde calculamos el cuaternio de referencia, así como realizamos el cálculo de las velocidades angulares deseadas. • Obtención de las gráficas (Rojo): Las variables se envían al Workspace de Matlab para procesar las gráficas deseadas. También se obtienen algunos resultados de interés, como el vector actual, que se obtiene mediante el giro del vector inicial según el cuaternio actual de la plataforma, o el producto vectorial del vector actual y el vector inicial. Estimación de la posición del objetivo 28 Figura 5-3. Triangulación con dos imágenes [16] 5.3 Calibración Antes de comenzar con los procedimientos de obtención de la posición del punto objetivo, debemos obtener la matriz de calibración de la cámara, vista en el Capítulo 3. Para ello, se ha de realizar un algoritmo de calibración. La matriz de calibración K proporciona la transformación entre un punto en la imagen y un rayo en el espacio euclídeo en 3 dimensiones [16]. Una cámara calibrada es como un sensor de dirección, capaz de medir la dirección de los rayos. Rescatando la ecuación 3.5, 𝑃=𝐾[𝑅|𝑡], la matriz P expresa la correspondencia entre el punto de la imagen (𝑥) y el punto en el espacio (𝑋), lo que se expresa como 𝑥=𝑃𝑋. El algoritmo de calibración tiene dos partes [16]: • Parte 1: Calculamos la matriz P a través de correspondencias entre un grupo de puntos • Parte 2: Descomponemos P en K, R y t a través de factorización QR Para realizar la calibración, tendremos que realizar varias fotografías de un patrón predefinido. Cada correspondencia 𝑥𝑖=𝑃𝑋𝑖 genera dos ecuaciones: 𝑢𝑖=𝑝11𝑋𝑖+𝑝12𝑌𝑖+𝑝13𝑍𝑖+𝑝14 𝑝31𝑋𝑖+𝑝32𝑌𝑖+𝑝33𝑍𝑖+𝑝34 𝑣𝑖=𝑝21𝑋𝑖+𝑝22𝑌𝑖+𝑝23𝑍𝑖+𝑝24 𝑝31𝑋𝑖+𝑝32𝑌𝑖+𝑝33𝑍𝑖+𝑝34 (5.3) Multiplicando, obtenemos las siguientes ecuaciones lineales: 𝑢𝑖(𝑝31𝑋𝑖+𝑝32𝑌𝑖+𝑝33𝑍𝑖+𝑝34)=𝑝11𝑋𝑖+𝑝12𝑌𝑖+𝑝13𝑍𝑖+𝑝14 𝑣𝑖(𝑝31𝑋𝑖+𝑝32𝑌𝑖+𝑝33𝑍𝑖+𝑝34)=𝑝21𝑋𝑖+𝑝22𝑌𝑖+𝑝23𝑍𝑖+𝑝24 (5.4) Que se pueden reorganizar como: (𝑋 𝑌 𝑍 0 0 0 1 0 0 0 𝑋 𝑌 0 0 −𝑢𝑋 𝑍 1 −𝑣𝑋 −𝑢𝑌 −𝑢𝑍 −𝑢 −𝑣𝑌 −𝑣𝑍 −𝑣)𝑝=0 (5.5) Con 𝑝=(𝑝11,𝑝12,𝑝13,𝑝14,𝑝21,𝑝22,𝑝23,𝑝24,𝑝31,𝑝32,𝑝33,𝑝34)𝑇 como un vector de dimension 12. Concatenamos las ecuaciones de 𝑛≥6 correspondencias para generar 2𝑛 ecuaciones, que pueden ser escritas como 𝐴𝑝=0, donde 𝐴 es una matriz 2𝑛 𝑥 12. 29 Control servo-visual de una plataforma giroestabilizada Figura 5-4. Patrón usado para la calibración. Tablero de ajedrez 10x7 En general, esto no tendrá una solución exacta, pero sí una solución lineal que minimiza |𝐴𝑝| sujeto a |𝑝|=1, obtenida del autovector con menor autovalor de 𝐴𝑇𝐴. Una vez obtenida 𝑃, 𝑀 es la primera submatriz 3x3, la cual es el producto (𝑀=𝐾𝑅) de una matriz triangular superior y una matriz de rotación. Se factoriza 𝑀 en 𝐾𝑅 usando factorización QR. Entonces, 𝑡=𝐾−1(𝑝14,𝑝24,𝑝34)𝑇 (5.6) Finalmente, obtenemos la ecuación de calibración de la cámara, que ya habíamos definido en la ecuación 3.3. 𝐾=[𝛼𝑥𝑠 0 𝛼𝑦 0 0 𝑢𝑜 𝑣𝑜 1] (5.7) Para resolver el problema de la calibración, vamos a ayudarnos de los algoritmos implementados en la librería de OpenCV [27], mediante el cual también estudiaremos la distorsión tangencial y radial de la cámara, conceptos introducidos en el Apartado 3.2. La distorsión tangencial se puede expresar como: 𝑢𝑑𝑖𝑠𝑡=𝑢+2𝑝1𝑢𝑣+𝑝2(𝑟2+2𝑢2) (5.8) 𝑣𝑑𝑖𝑠𝑡=𝑣+𝑝1(𝑟2+2𝑣2)+2𝑝2𝑢𝑣 Mientras que la distorsión radial quedaría como: 𝑢𝑑𝑖𝑠𝑡=𝑢(1+𝑘1𝑟2+𝑘2𝑟4+𝑘3𝑟6) (5.9) 𝑣𝑑𝑖𝑠𝑡=𝑣(1+𝑘1𝑟2+𝑘2𝑟4+𝑘3𝑟6) Siendo r la distancia euclídea entre el punto distorsionado de la imagen y el centro, y (𝑘1,𝑘2,𝑘3,𝑝1,𝑝2) los coeficientes de distorsión. Para el cálculo de la calibración, usaremos imágenes de un patrón predefinido, en este caso un tablero de ajedrez. En el tablero, encontraremos puntos cuya posición relativa sea conocida, como, por ejemplo, las esquinas de las casillas. Conocemos las coordenadas de estos puntos en coordenadas globales y en las coordenadas de la imagen, por lo que podemos resolver el problema de la calibración. Para obtener mejores resultados, es recomendable usar al menos 10 fotografías del patrón. Estimación de la posición del objetivo 30 Figura 5-5. Ejemplo de las imágenes tomadas para la calibración Usando el código proporcionado en [27], particularizado para nuestro patrón, obtenemos los siguientes resultados: 𝑀𝑎𝑡𝑟𝑖𝑧 𝑑𝑒 𝑐𝑎𝑙𝑖𝑏𝑟𝑎𝑐𝑖ó𝑛 (𝐾)=[1637.04 0 01638.64 0 0 949.36 532.63 1] (5.10) 𝐷𝑖𝑠𝑡=[0.017377 −0.365425 0.013127 −0.002851 0.802158] (5.11) 5.4 Obtención de la posición del objetivo. Primer método. El primer método que vamos a desarrollar es el de la triangulación, explicado teóricamente en el Apartado 5.2. Este método es el más preciso, pero a su vez es el más complicado de implementar, especialmente a nivel del hardware necesario, ya que necesitaríamos más de una cámara. También sería imprescindible conocer siempre la orientación y distancia relativas entre las dos cámaras en todo momento, lo que requeriría usar sistemas GPS, además de que necesitaríamos varios UAV, y para desarrollar este trabajo no vamos a poder disponer de toda esta arquitectura. Sin embargo, para comprobar la validez de esta solución de cara a futuros trabajos y proyectos en los que se disponga del material necesario, vamos a llevar a cabo un experimento sencillo, en el que tomaremos fotos del objetivo desde diferentes puntos en el espacio conocidos, con orientaciones de la cámara también definidas, para así realizar la triangulación entre las imágenes. Es evidente que esto es una limitación de cara a probar el algoritmo, ya que el mero hecho de ejecutar el programa en tiempo real incluirá ruidos de medida introducidos, por ejemplo, por el movimiento del objeto, o por fallos en la detección, que seguramente nos lleven a necesitar filtrar los resultados. 31 Control servo-visual de una plataforma giroestabilizada Figura 5-6. Eje de coordenadas de la cámara y de la imagen En nuestra simulación, vamos a suponer que disponemos de 3 cámaras, de forma que podamos hacer triangulaciones con cada par de dispositivos, de forma que se reduzca y compense el posible error de medición. Así se resuelve en [28], donde se realiza una triangulación con las medidas de sensores de radiofrecuencia portados por tres UAV. De esta triangulación por pares se obtienen 3 medidas de la posición, las cuales se usan para definir un triángulo en el espacio, cuyo centroide será tomado como la posición final del objetivo. Antes de continuar, vamos a definir el eje de coordenadas de la cámara: El eje z se sitúa respetando un sistema dextrógiro con 𝑥,𝑦. Los ejes 𝑥𝑝,𝑦𝑝 identifican las coordenadas del plano imagen, y se miden en píxeles. A su vez, 𝑥𝑜 e 𝑦𝑜 definen el centro de la imagen. La altura y anchura en píxeles están definidas por la resolución de la cámara. Recordamos que para computar la triangulación, necesitamos la matriz 𝑃 de la cámara. Esta matriz, como se introdujo en la ecuación 3.5, se define como P=𝐾[𝑅|𝑡], donde 𝐾 es la matriz de calibración de la cámara, obtenida en el Apartado 5.3, y [𝑅|𝑡], es la matriz conocida como matriz de parámetros extrínsecos, que determina la orientación y traslación de la cámara con respecto al origen. A continuación, vamos a definir en la siguiente tabla las traslaciones y rotaciones que hay entre las imágenes que hemos tomado para realizar la triangulación. Los ángulos están expresados en Ángulos de Euler (Roll, Pitch, Yaw). Estimación de la posición del objetivo 32 Tabla 5–1. Posición relativa de la cámara en la toma de imágenes Traslación posición 2-1 (metros) (-0.68, 0, 0.62) Rotación posición 2-1 (grados) (0, 90, 0) Traslación posición 3-1 (metros) (-0.50, 0, 0) Rotación posición 3-1 (grados) (0, 45, 0) Traslación posición 3-2 (metros) (0.62, 0, 0.18) Rotación posición 3-2 (grados) (0, -45, 0) Para calcular la matriz 𝑅 en cada una de las imágenes, nos basamos en las matrices de rotación para cada uno de los Ángulos de Euler, ecuaciones 2.2, 2.3, 2.4. Recordamos que la imagen 1 la hemos tomado como el origen de coordenadas. Las matrices de parámetros extrínsecos en cada una de las imágenes sería la siguiente: 𝐸𝑥𝑡𝑟𝑖𝑛𝑠𝑖𝑐1−0=[1 0 0 0 1 0 000010|0001] (5.12) 𝐸𝑥𝑡𝑟𝑖𝑛𝑠𝑖𝑐2−1=[cos(90) 0 𝑠𝑖𝑛(90) 0 1 0 −sin (90) 000cos (90) 0|−0.68 0 0.62 1] (5.13) 𝐸𝑥𝑡𝑟𝑖𝑛𝑠𝑖𝑐3−1=[cos(45) 0 𝑠𝑖𝑛(45) 0 1 0 −sin (45) 000cos (45) 0|−0.5 001] (5.14) 𝐸𝑥𝑡𝑟𝑖𝑛𝑠𝑖𝑐3−2=[cos(−45) 0 𝑠𝑖𝑛(−45) 0 1 0 −sin (−45) 000cos (−45) 0|0.62 0 0.18 1] (5.15) Con esto, ya tenemos todos los datos necesarios para realizar la triangulación. Para ello, nos vamos a ayudar de la función de OpenCV “triangulatePoints” [29]. Esta función utiliza internamente el método de triangulación lineal explicado en el Apartado 5.2. La función recibe como parámetros de entrada un array con los puntos del centro del objeto medidos en píxeles, en cada una de las dos imágenes tomadas, así como las matrices de proyección 𝑃 de cada imagen. La salida es, como es de esperar, un punto en tres dimensiones que define la posición del punto objetivo en el espacio. El código completo del programa se puede observar en el Anexo A. 33 Control servo-visual de una plataforma giroestabilizada Figura 5-8. Imágenes usadas para el cálculo de la triangulación En la siguiente tabla, mostramos los resultados obtenidos con cada una de las triangulaciones, así como la posición real del punto en tres dimensiones. Tabla 5–2. Resultados de la triangulación (metros) Punto 1-2 (0.059, -0.130, 0.643) Punto 1-3 (0.063, -0.114, 0.670) Punto 2-3 (0.098, -0.149, 0.667) Punto real (0.060, -0.110, 0.670) Punto calculado (centroide) (0.073, -0.131, 0.660) El error lo determinamos como el módulo del vector que une el punto calculado y el punto real, que se trata de 0.0193 metros, es decir, en torno a 2 centímetros, lo que supone un error relativo con respecto al módulo del vector de posición real del 2.85%. A continuación, mostramos el resultado representado en el espacio. Estimación de la posición del objetivo 34 Figura 5-9. Representación en el espacio del resultado de la triangulación 5.5 Obtención de la posición del objetivo. Segundo método. En este apartado, vamos a presentar un método para obtener la posición que sea implementable en tiempo real con el equipo disponible en este trabajo. Para ello, nos vamos a basar en el modelo Pinhole, introducido en el Capítulo 3. Considerando que R y t expresan rotación y traslación nulas: 𝑍[𝑢𝑣1]=[𝛼𝑥0 0 𝛼𝑦 0 0 𝑢𝑜 𝑣𝑜 1 0 0 0][𝑋𝑌𝑍1] (5.16) Esta expresión no es más que la ecuación 3.1, incluyendo la matirz de calibración. De aquí, inferimos que: 𝑋=𝑍∗(𝑢−𝑢𝑜) 𝛼𝑥 (5.17) 𝑌=𝑍∗(𝑣−𝑣𝑜) 𝛼𝑦 (5.18) 35 Control servo-visual de una plataforma giroestabilizada Comprobamos que lo necesario para obtener las posiciones 𝑋 e 𝑌 en el espacio del punto objetivo son los parámetros de calibración de la cámara (ya obtenidos) y la distancia 𝑍 hasta el objeto. Dado que este algoritmo está pensado para funcionar en un UAV a una altura determinada, rastreando objetivos a nivel del suelo, se puede considerar 𝑍 constante y conocida, en una simplificación realizada para poder implementar el algoritmo previamente sin necesidad de conseguir una triangulación que no necesitaría conocer la distancia al objetivo, sino que sería calculada. Entonces, tenemos una constante directamente proporcional a la distancia 𝑍 e inversamente proporcional al parámetro de distancia focal expresado en píxeles. 𝐷𝑖𝑠𝑡𝑎𝑛𝑐𝑒 𝑝𝑎𝑟𝑎𝑚𝑒𝑡𝑒𝑟 𝑋=𝑍∗0.0006108 (5.19) 𝐷𝑖𝑠𝑡𝑎𝑛𝑐𝑒 𝑝𝑎𝑟𝑎𝑚𝑒𝑡𝑒𝑟 𝑌=𝑍∗0.0006102 (5.20) Multiplicando este parámetro por el número de píxeles que distan del centro horizontal y verticalmente, obtenemos la posición 𝑋 e 𝑌 relativas a los ejes de la cámara. Recuperando la Figura 5-6, podemos obtener la ecuación según los ejes ilustrados en la imagen: 𝑥=𝑍∗0.0006108∗(𝑥𝑜−𝑥𝑝−𝑜𝑏𝑗) (5.21) 𝑦=𝑍∗0.0006102∗(𝑦𝑜−𝑦𝑝−𝑜𝑏𝑗) (5.22) Las ecuaciones 5.21 y 5.22 son en las que nos vamos a basar a la hora de crear nuestro programa. 5.5.1 Filtro de Kalman Adaptativo El Filtro de Kalman es ampliamente usado en visión artificial. La dificultad de aplicar este filtro a este tipo de sistemas son las no linealidades. La aproximación más común es la de linealizar el sistema. En [30], se propone un Filtro de Kalman Adaptativo (AKF) para un sistema de control servovisual mediante una cámara estereoscópica solidaria a un robot. Otras aproximaciones del Filtro de Kalman Adaptativo consisten en modificar los parámetros estadísticos del mismo, mientras que el analizado en este trabajo utiliza un modelo de estado del sistema que se modifica a lo largo del tiempo, es decir, un modelo adaptativo del sistema. Esto es especialmente útil en este tipo de situaciones, en las que el conocimiento del movimiento del objetivo en el espacio es nulo a priori. 5.5.1.1 Filtro de Kalman general Describimos un proceso lineal con la representación en espacio de estados. En primer lugar, tenemos la ecuación del proceso [30]: 𝑋𝑘+1=𝐴𝑋𝑘+𝐵𝑢𝑘+𝐷𝑤𝑘 (5.23) Donde 𝑋𝑘∈ℝ𝑛 es el vector de estados. El modelo del estado es caracterizado por la matriz de transición de estado 𝐴∈ℝ𝑛∗𝑛 , que expresa la evolución del sistema en el tiempo. 𝐵∈ℝ𝑛∗𝑟 es la matriz de entradas, 𝐷∈ ℝ𝑛∗𝑠 es la matriz de ruido, expresado como 𝑤𝑘∈ℝ𝑠. k denota el índice discreto de tiempo. En el proceso pueden aplicarse entradas 𝑢𝑘∈ℝ𝑟. La ecuación de medición es expresada como: 𝑌𝑘=𝐶𝑋𝑘+𝑣𝑘 (5.24) Donde 𝐶∈ℝ𝑝∗𝑛 es conocida como la matriz de observabilidad, 𝑣𝑘∈ℝ𝑝 es el ruido de medición y, finalmente, 𝑌𝑘∈ℝ𝑝 es la salida medible del sistema. Estimación de la posición del objetivo 36 Figura 5-10. Iteración en Filtro de Kalman [50] Los vectores de ruido 𝑤𝑘 y 𝑣𝑘 son estadísticamente independientes, lo que conlleva a que 𝐸[𝑤𝑖𝑣𝑗𝑇]=0 para todo 𝑖,𝑗, donde 𝐸[.] denota la esperanza matemática. También asumimos la incorrelación del vector de estados con el ruido, por lo que 𝐸[𝑥𝑖𝑣𝑗𝑇]=0 y 𝐸[𝑤𝑖𝑥𝑗𝑇]=0 para todo 𝑖,𝑗. Si el proceso no es lineal, el modelo debe ser linealizado en torno a su estimación actual. Esto se conoce como Filtro de Kalman Extendido (EKF). El Filtro de Kalman (KF) corrige iterativamente la ganancia de Kalman 𝐾∈ℝ𝑛∗𝑝 para hacer que la estimación del vector de estados tienda a una solución óptima. Esto se define con las siguientes ecuaciones: 𝑋𝑘+1|𝑘=𝐴𝑋𝑘|𝑘+𝐵𝑢𝑘 (5.25) 𝑃𝑘+1|𝑘=𝐴𝑃𝑘|𝑘𝐴𝑇+𝐷𝑄𝐷𝑇 (5.26) 𝐾𝑘+1=𝑃𝑘+1|𝑘𝐶𝑇(𝐶𝑃𝑘+1|𝑘𝐶𝑇+𝑅)−1 (5.27) 𝑋𝑘+1|𝑘+1=𝑋𝑘+1|𝑘+𝐾𝑘+1(𝑌𝑘+1−𝐶𝑋𝑘+1|𝑘) (5.28) 𝑃𝑘+1|𝑘+1=(𝐼−𝐾𝑘+1𝐶) 𝑃𝑘+1|𝑘 (5.29) El KF controla el modelo del estado calculado la matriz de ganancia variable en el tiempo 𝐾𝑘+1. La estimación es óptima si 𝑤𝑘 y 𝑣𝑘 son ruidos blancos de media cero con distribución Gaussiana. Estos son definidos respectivamente por sus matrices de covarianza 𝑄𝑘∈ℝ𝑠∗𝑠 y 𝑅𝑘∈ℝ𝑝∗𝑝 de forma que 𝐸[𝑤𝑘𝑤𝑖𝑇]= 𝑄𝑘𝛿𝑘𝑖 y 𝐸[𝑣𝑘𝑣𝑖𝑇]=𝑅𝑘𝛿𝑘𝑖, siendo 𝛿𝑘𝑖 el operador de Kronecker. Si estas afirmaciones se respetan, el filtro da una estimación óptima de las variables de estado. 37 Control servo-visual de una plataforma giroestabilizada 5.5.1.2 Implementación AKF El filtro adaptativo se va a conseguir mediante una matriz de transición 𝐴 dependiente del tiempo. Consideramos que las entradas vistas en la ecuación 5.25 son parte de las perturbaciones del sistema. Para calcular la matriz 𝐴, usaremos los dos últimos estados: 𝐴󰆹𝑘=𝑋𝑘𝑋+𝑘−1. La inversa 𝑋+𝑘−1 será calculada usando una aproximación mediante mínimos cuadrados. Esto nos proporciona, para una iteración k, la matriz de transición estimada: 𝐴󰆹𝑘=𝑋𝑘(𝑋𝑇𝑘−1𝑋𝑘−1)−1𝑋𝑇𝑘−1 (5.30) En cada iteración k, se calcula la matriz 𝐴󰆹𝑘, para luego continuar con las ecuaciones 5.25, 5.26, 5.27, 5.28, 5.29. En nuestro problema, hemos definido el vector de estados como: 𝑋𝑘=[𝑥𝑦𝑧] (5.31) Siendo 𝑥,𝑦,𝑧 las posiciones del objeto seguido en el espacio. La matriz de observabilidad 𝐶, como: 𝐶=[1 0 0 0 1 0 0 0 1] (5.32) Considerando que tenemos en cada iteración las mediciones de 𝑥,𝑦,𝑧. Las matrices 𝑄,𝑅: 𝑄=𝑅=[0.005 0 0 0 0.005 0 0 0 0.005] (5.33) Elegidas mediante experimentación. El AKF nos proporcionará una estimación óptima de la posición del objetivo, considerando un modelo en espacio de estados con una matriz de transición variable en el tiempo, según los estados anteriores. Los resultados del Filtro de Kalman Adaptativo podrán revisarse en el Capítulo 7, “Resultados”, donde se analizarán todos los resultados del sistema completo, incluido el comportamiento del AKF en el algoritmo de tracking. Integración del sistema 44 Si la última posición registrada del objetivo se encuentra dentro del rectángulo, podemos asumir que es una perturbación por la que se ha perdido la posición del objetivo, por lo que el vector de posición enviado será 0 en el eje x e y, ya que estamos enviando un vector relativo a la cámara (un vector 0 en esos ejes significará que no haya movimiento de la plataforma). Si la última posición, trasciende los bordes del rectángulo en el eje x (tamaño 1060px), el eje x del vector de posición enviado será calculado como si estuviera en el borde del rectángulo, es decir, será de 500 o -500 multiplicado por la constante de la ecuación 5.19. El eje y del vector de posición lo dejaremos tal cual se ha registrado en la última posición detectada. Como estamos enviando el vector relativo a la cámara, situar ese vector en el rectángulo supondrá un movimiento suave hacia la última posición registrada, por donde tiene gran probabilidad de encontrarse el objetivo, ya que ha salido del plano imagen por esa dirección. Si la última posición, trasciende los bordes del rectángulo en el eje y (tamaño 600px), el eje x del vector de posición enviado será calculado como si estuviera en el borde del rectángulo, es decir, será de 300 o -300 multiplicado por la constante de la ecuación 5.20. El eje x del vector de posición lo dejaremos tal cual se ha registrado en la última posición detectada. Si ambos ejes sobrepasan el rectángulo, el vector enviado será el de la esquina correspondiente. Si el gimbal supera unos ángulos umbrales en la búsqueda del objetivo, se devuelve a su estado inicial. El nodo publica en los siguientes tópicos: • /vector_goals: En este tópico publica un mensaje de ROS de tipo geometry_msgs/Vector3Stamped, que contiene el vector del objetivo con respecto a la cámara. • /detección: En este tópico publica un mensaje de ROS de tipo std_msgs/Bool, que contiene una variable booleana que indica si se está produciendo o no la detección del objetivo. El nodo se subscribe a los siguientes tópicos: • /ros_gremsy/angulos_actual: Recibe un mensaje de ROS de tipo geometry_msgs/Vector3Stamped, que contiene los valores de los Ángulos de Euler roll, pitch, yaw en coordenadas globales en los dos primeros y locales en el último. • /sincro: Recibe un mensaje de ROS de tipo std_msgs/Bool, que contiene una variable booleana que sirve para sincronizar las gráficas de los datos de interés de los diferentes nodos a la frecuencia requerida. • /UAV_1/usb_cam/Image_raw: Recibe un mensaje de ROS de tipo sensor_msgs/Image, que contiene una imagen capturada por la cámara. El código completo de este nodo, escrito en Python, se encuentra en el Anexo C. 6.3.4 Nodo Grabación Este nodo se usa para capturar el vídeo grabado desde la cámara. El nodo se subscribe a los siguientes tópicos: • /UAV_1/usb_cam/Image_raw: Recibe un mensaje de ROS de tipo sensor_msgs/Image, que contiene una imagen capturada por la cámara. El código completo de este nodo, escrito en Python, se encuentra en el Anexo D. 45 Control servo-visual de una plataforma giroestabilizada Figura 6-6. Interfaz gráfica para introducir los parámetros deseados 6.3.5 Nodo Sincronización Este nodo se usa para obtener en el mismo instante de tiempo las gráficas con los datos que nos interesan de cara a evaluar el programa. El nodo publica en los siguientes tópicos: • /sincro: Envía un mensaje de ROS de tipo std_msgs/Bool, que contiene una variable booleana que sirve para sincronizar las gráficas de los datos de interés de los diferentes nodos a la frecuencia requerida. El código completo de este nodo, escrito en Python, se encuentra en el Anexo E. 6.3.5 Nodo Parámetros Este nodo es de utilidad para sintonizar los parámetros del algoritmo usado en el control en velocidad. El nodo publica en los siguientes tópicos: • /parámetros: Este tópico solo se usa para sintonizar el controlador. Envía un mensaje de ROS de tipo std_msgs/Float32MultiArray con los parámetros de control deseados. Los parámetros se introducen por pantalla, con esta interfaz: El código completo de este nodo, escrito en Python, se encuentra en el Anexo F. 6.3.6 Nodo usb_cam Este nodo [39] sirve para crear una interfaz con diferentes tipos de cámara. El nodo publica en los siguientes tópicos: • /UAV_1/usb_cam/Image_raw: Envía un mensaje de ROS de tipo sensor_msgs/Image, que contiene una imagen capturada por la cámara. Integración del sistema 46 Figura 6-7. rqt_graph durante el funcionamiento del programa Figura 6-8. Nodos y tópicos del programa completo 6.3.7 Esquema general Usando la herramienta rqt_graph [40], podemos obtener un esquema en tiempo real de los nodos y tópicos que están en funcionamiento: En este gráfico no estábamos usando el nodo Parámetros. A continuación, mostramos otro gráfico que tal vez contenga de forma menos liosa todos los nodos y tópicos: 47 7 RESULTADOS En este capítulo, analizaremos los resultados obtenidos en varios tipos diferentes de pruebas. En primer lugar, veremos si el sistema se comporta correctamente en un entorno en el que el objetivo nunca sale del campo de visión. Una vez comprobado el funcionamiento en el supuesto más sencillo, probaremos la funcionalidad en la que la plataforma intenta perseguir al objetivo que sale del campo de visión. Luego, nos cercionaremos de que el sistema funciona correctamente cuando movemos el UAV en el que se encuentra la plataforma, para asegurarnos de que es viable la aplicación en vuelo del algoritmo. Además, supondremos en todas las pruebas una distancia al objetivo de 1 metro, comprobando de esta forma que el algoritmo es robusto frente a distancias diferentes de la real. 7.1 Prueba 1. Objetivo siempre en el rango de visión Los parámetros de control usados en esta prueba los tenemos en la siguiente tabla: Tabla 7–1. Parámetros de control. Prueba 1 Kp (yaw) 8 Kp (roll) 0.0001 Kp (pitch) 8 Kd (yaw) 0.0005 Kd (roll) 0.0005 Kd (pitch) 0.0005 Gamma (yaw) 25 Gamma (roll) 15 Gamma (pitch) 15 Al poder desacoplar en controlador en 3 ejes, podemos usar diferentes parámetros para cada uno, ya que cada uno de los ejes tiene sus propias inercias. A continuación, analizaremos gráficamente los datos obtenidos directamente de la prueba. Como se ha comentado en la introducción, en esta primera prueba el objetivo nunca saldrá del campo de visión, por lo que se comprobará el funcionamiento correcto del algoritmo de seguimiento sin búsqueda de objetivo. En primer lugar, mostraremos los datos relativos al control de la plataforma, es decir, todos aquellos que se obtienen del nodo control_gremsy. Comenzaremos con los datos de los cuaternios utilizados: Resultados 48 Figura 7-1. Representación gráfica de cuaternios. Prueba 1 Figura 7-2. Representación gráfica del vector de apuntamiento. Prueba 1 Podemos observar que la orientación de la plataforma expresada en cuaternio 𝑞, sigue perfectamente la orientación de referencia 𝑞𝑟. El cuaternio de error 𝑞𝑒 tiende a cero en cada uno de los movimientos realizados por el objetivo, por lo que se puede considerar que el desempeño del controlador es acorde a lo deseado. Ahora veremos cómo evoluciona el vector de apuntamiento de la cámara: En esta gráfica, vemos igualmente cómo el vector de apuntamiento de la cámara sigue en la prueba al vector de apuntamiento deseado, mostrando algo más de error que en el caso de los cuaternios, pero manteniéndose estable. 49 Control servo-visual de una plataforma giroestabilizada Figura 7-3. Representación de las velocidades angulares. Prueba 1 Figura 7-4. Representación de la orientación expresada en Ángulos de Euler. Prueba 1 Continuamos comprobando las velocidades angulares comandadas (no olvidemos que la referencia que enviamos al gimbal son las velocidades deseadas), las medidas, y las filtradas: Observamos que las velocidades medidas son consecuentes con las velocidades que estamos enviando al controlador del gimbal. El filtro paso bajo funciona bien, anulando las componentes de alta frecuencia y suavizando la señal sin introducir un retraso reseñable. Por último, mostraremos gráficamente la orientación en la otra representación que hemos analizado en este trabajo, los Ángulos de Euler. Los ángulos que se muestran están expresados en coordenadas globales, excepto el yaw, que es expresado con respecto al vehículo. Resultados 50 Figura 7-5. Representación gráfica del vector de posición del objetivo estimado. Prueba 1. Una vez hemos terminado con las gráficas del nodo de control, pasaremos a analizar los datos del nodo Procesamiento. En primer lugar, analizaremos los valores del vector de posición estimado del objetivo relativo a la cámara. Recordamos que este vector de posición estimado es el que se envía al nodo control_gremsy para realizar el algoritmo de control. Como suponemos que la distancia es constante e igual a 1, la Z del vector que enviamos es siempre 1. Sin embargo, para los ejes X e Y, se envía la corrección del vector realizada con el Filtro de Kalman Adaptativo. Se puede observar que el vector tiende a cero cada vez que se mueve el objetivo, por lo que indica que se está centrando en el plano imagen adecuadamente. Vamos a ver una imagen ampliada de las componentes X e Y en diferentes instantes de tiempo, en los que se produce un cambio rápido en la posición del objetivo, para comprobar los valores proporcionados por el Filtro de Kalman. 51 Control servo-visual de una plataforma giroestabilizada Figura 7-6. Representación gráfica del vector de posición. Ampliado. Prueba 1 Figura 7-7. Representación de la distancia en píxeles al centro del objeto en el plano imagen. Prueba 1 Se puede ver que la predicción del Filtro de Kalman Adaptativo (color azul) tiende a aumentar conforme el movimiento del objetivo se ve reflejado en la gráfica, ya que los valores de la matriz de transición son calculados mediante los estados anteriores. La corrección (color rojo) pondera entre la medición (color negro) y la predicción, y es el valor que usamos para realizar el control. Para apoyar a la figura del vector de posición relativo, mostraremos cómo evoluciona la distancia en píxeles del objetivo detectado al centro de la imagen, lo que confirmará que se produce un centrado del objeto en el plano. Resultados 52 Figura 7-8. Traza de la matriz P. Prueba 1 Figura 7-9. Traza matriz P. Ampliado. Prueba 1 Observamos que se confirma la tendencia hacia el centro de la imagen de la detección del objetivo. Ahora, mostramos gráficamente cómo evoluciona el valor de la traza matriz P de covarianza del algoritmo del Filtro de Kalman Adaptativo. La matriz evoluciona hasta un valor bajo, que concretamente es algo menor a 0.01. 53 Control servo-visual de una plataforma giroestabilizada Figura 7-10. Traza matriz K. Prueba 1. Figura 7-11. Componentes de la matriz A. Prueba 1 Siguiendo con el análisis de las matrices del filtro, vemos cómo varía la traza de la matriz de ganancia K. Esta ganancia de Kalman va variando en el tiempo, de forma que pondera más o menos la diferencia entre la medición y la estimación, pero en general oscila sobre el cero, por lo que da la mayoría del peso a la estimación, es decir, a la predicción obtenida por la matriz de transición adaptativa. Para concluir con el análisis del comportamiento del Filtro de Kalman Adaptativo, mostraremos una figura en la que se observa cómo evolucionan las componentes de la matriz A, la matriz de transición, a lo largo de la prueba. Resultados 60 Igualmente, la distancia en píxeles del objetivo al centro de la imagen también tiene a cero durante toda la simulación, por lo que damos por todo un éxito el nuevo algoritmo de control en posición. Con esto, hemos terminado de exponer las pruebas realizadas en este trabajo. 61 8 CONCLUSIÓN Y TRABAJOS FUTUROS Terminamos este proyecto cumpliendo los objetivos que establecimos en la introducción. Tenemos un algoritmo completamente capaz de realizar la función de seguimiento de objetivo, centrado a este en el plano de la imagen. Además, los beneficios de haber usado un sistema operativo como ROS nos permitirán usar este programa en el futuro para múltiples sistemas compatibles con este entorno. Los objetivos que seguirían la línea temporal de este trabajo serían varios. Primeramente, habría que desarrollar un algoritmo de detección en función de qué tipos de objetivos queremos seguir. Esta es una tarea que bien podría ser desarrollada en otro Trabajo de Fin de Grado. Seguidamente, nos faltaría comprobar el funcionamiento de un sistema de estimación de la posición como el de la triangulación, el cual nos permitiría saber con exactitud la situación del objetivo si tenemos varias cámaras. Esto sería muy útil para diferentes casos de uso. Finalmente, hubiera sido ideal probar este algoritmo en un vuelo real, pero no ha sido posible. 62 REFERENCIAS [1] D. Z. Rabanal Carretero, «Integración de un sistema UAV con control autónomo en un equipo aéreo para agricultura de precisión,» Pontificia Universidad Católica del Perú, 2011. [2] W. A. Richard y R. T. Rysdyk, «UAV Coordination for Autonomous Target Tracking,» University of Washington, 2006. [3] S. Jung y H. Kim, «Analysis of Amazon Prime Air UAV Delivery Service,» Journal of Knowledge Information Technology and Systems, vol. 12, nº 2, pp. 253-266, 2017. [4] R. Rajesh y P. Kavitha, «Camera Gimbal Stabilization Using Conventional PID Controller And Evolutionary Algorithms,» de IEEE International Conference on Computer Communication and Control, IC4 2015, Indore, 2015. [5] E. W. Weisstein, «"Euler's Rotation Theorem." From MathWorld--A Wolfram Web Resource.,» [En línea]. Available: https://mathworld.wolfram.com/RotationMatrix.html. [6] E. W. Weisstein, «"Rotation Matrix." From MathWorld--A Wolfram Web Resource.,» [En línea]. Available: https://mathworld.wolfram.com/EulersRotationTheorem.html. [7] J. B. Kuipers, «Quaternions and Rotation Sequences,» de Quaternions and Rotation Sequences: A Primer with Applications to Orbits, Aerospace and Virtual Reality, Princeton University Press, 1999, pp. 127- 142. [8] A. L. Schwab, «Quaternions, Finite Rotation and Euler Parameters,» Laboratory for Engineering Mechanics, Delft University of Technology, 2002. [9] H. Abaunza , «Robust Tracking of Dynamic Targets with Aerial Vehicles Using Quaternion-based Techniques,» Université de Technologie de Compiègne, 2019. [10] L. Perumal, «Quaternion and Its Application in Rotation Using Sets of Regions,» de International Journal of Engineering and Technology Innovation, 35-52, 1(1), 2011. [11] «Apollo11Space,» [En línea]. Available: https://apollo11space.com/apollo-and-gimbal-lock/. [12] «https://gremsy.com/pixy-u-spec,» [En línea]. [13] D. Mery, Visión por Computador, Santiago de Chile: Universidad Católica de Chile, 2004. [14] «https://es.wikipedia.org/wiki/C%C3%A1mara_oscura,» [En línea]. [15] «https://www.xatakafoto.com/camaras/sensores-con-tecnologia-ccd-vs-cmos,» [En línea]. [16] R. Hartley y A. Zisserman, Multiple View Geometry, 2000. [17] «https://www.sony.es/electronics/camaras-compactas-cyber-shot/dsc- 63 Control servo-visual de una plataforma giroestabilizada rx0m2/specifications#specifications,» [En línea]. [18] «Simulink. Simulación y diseño basado en modelos.,» [En línea]. Available: https://es.mathworks.com/products/simulink.html?s_tid=srchtitle. [19] A. I. Barranco Gutiérrez, «Visión esteroscópica por computadora,» Instituto Politécnico Nacional. Centro de Investigación en Ciencia Aplicada y Tecnología Avanzada, 2007. [20] «OpenCV,» [En línea]. Available: https://opencv.org/about/. [21] «Documentos OpenCV,» [En línea]. Available: https://docs.opencv.org/4.5.2/d1/dfb/intro.html. [22] «AprilTag,» [En línea]. Available: https://april.eecs.umich.edu/software/apriltag. [23] X. Liang, G. Chen, S. Zhao y Y. Xiu, «Moving target tracking method for unmanned aerial vehicle/unmanned ground vehicle heterogeneous system based on AprilTags,» Measurement and Control (United Kingdom), 2020. [24] B. Winkler, «Apriltag,» [En línea]. Available: https://pypi.org/project/apriltag/. [25] «Visión estereoscópica,» [En línea]. Available: https://es.wikipedia.org/wiki/Visi%C3%B3n_estereosc%C3%B3pica. [26] R. I. Hartley y P. Sturm, «Triangulation,» Computer Vision and Image Understanding, vol. 68, nº 2, pp. 146-157, 1997. [27] «Camera Calibration-OpenCV,» [En línea]. Available: https://docs.opencv.org/master/dc/dbb/tutorial_py_calibration.html. [28] G. York y D. Pack, «Comparative study on time-varying target localization methods using multiple unmanned aerial vehicles: Kalman estimation and triangulation techniques,» IEEE Networking, Sensing and Control, ICNSC2005 - Proceedings, pp. 305-310, 2005. [29] «OpenCV-Triangulation,» [En línea]. Available: https://docs.opencv.org/3.4/d0/dbd/group__triangulation.html. [30] P. Wira y J. P. Urban, «A New Adaptive Kalman Filter Applied To Visual Servoing Tasks,» de Knowledge-Based Intelligent Engineering Systems and Allied Technologies, Brighton, UK, 2000. [31] L. Meier, «https://mavlink.io/en/,» [En línea]. [32] A. Koubaa, A. Allouch, M. Alajlan, Y. Javed, A. Belghith y M. Khalgui, «Micro Air Vehicle Link (MAVlink) in a Nutshell: A Survey,» IEEE Access, pp. 87658-87680, 2019. [33] Gremsy. [En línea]. Available: https://github.com/Gremsy/gSDK. [34] «Robot Operating System,» [En línea]. Available: http://wiki.ros.org/ROS/Introduction. [35] «Robot Operating System,» [En línea]. Available: https://www.ros.org/. [36] «Robot Operating System,» [En línea]. Available: http://wiki.ros.org/ROS/Concepts. Referencias 64 [37] «Wikipedia,» [En línea]. Available: https://es.wikipedia.org/wiki/Peer-to-peer. [38] Flova y gChuNguyen. [En línea]. Available: https://github.com/Flova/ros_gremsy. [39] «Robot Operating System,» [En línea]. Available: http://wiki.ros.org/usb_cam. [40] «Robot Operating System,» [En línea]. Available: http://wiki.ros.org/rqt_graph. [41] E. W. Weisstein, « "Euler Angles." From MathWorld--A Wolfram Web Resource.,» [En línea]. Available: https://mathworld.wolfram.com/EulerAngles.html. [42] Auawise, «An image showing all three axes. Wikipedia.,» 2008. [En línea]. Available: https://commons.wikimedia.org/wiki/File:Yaw_Axis.svg. [43] Cone83, «Inscription on Broom Bridge,» 2017. [En línea]. Available: commons.wikipedia.org. [44] D. Malan, «Representation of how the axis and angle rotation representation can be visualized,» 2004. [En línea]. Available: https://commons.wikimedia.org/wiki/File:Euler_AxisAngle.png. [45] Autor, «Este es el ejemplo de una cita,» Tesis Doctoral, vol. 2, nº 13, 2012. [46] O. Autor, «Otra cita distinta,» revista, p. 12, 2001. [47] D. Mery y F. Pedreschi, «Segmentation of colour food images using a robust algorithm,» Journal of Food Engineering, 2004. [48] [En línea]. Available: https://opencv.org/resources/media-kit/. [49] «OpenCV-Triangulation,» [En línea]. Available: https://docs.opencv.org/3.4/d0/dbd/group__triangulation.html. [50] [En línea]. Available: https://www.laboratoriogluon.com/filtro-de-kalman-deduccion-ejemplos/. [51] «Rostutorial,» [En línea]. Available: http://rostutorial.com/4-nodos-topics-y-mensajes-turtlesim/. 65 ANEXOS Anexo A: Triangulación #!/usr/bin/env python3 # -*- coding: utf-8 -*- """ Created on Fri May 14 12:01:23 2021 @author: nacho """ import cv2 import numpy as np import apriltag """Matriz calculada en la calibración""" mtx=np.array([[1.63704531e+03, 0, 9.49359532e+02], [0, 1.63864093e+03, 5.32636161e+02], [0, 0, 1]]) dist=np.array([0.01737745, -0.36542463, 0.01312702, -0.00285169, 0.80215804]) """Leemos imagen 1""" image1 = cv2.imread('im0.png') gray1 = cv2.cvtColor(image1, cv2.COLOR_BGR2GRAY) detector1 = apriltag.Detector() results1 = detector1.detect(gray1) for r in results1: (cX1, cY1) = (int(r.center[0]), int(r.center[1])) x1=np.array([float(cX1), float(cY1)]) cv2.circle(image1, (cX1, cY1), 5, (0, 0, 255), -1) cv2.namedWindow('im1', cv2.WINDOW_NORMAL) cv2.resizeWindow('im1', 800, 800) cv2.imshow('im1', image1) cv2.imwrite('imagen1tri.png', image1) cv2.waitKey(0) """Creamos matriz mejorada""" h, w = image1.shape[:2] newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w,h), 1, (w,h)) intrinsic=np.array(newcameramtx) """Parametros imagen 1""" rot1=np.identity(3) trans1=np.array([[0, 0, 0]]) extrinsic1=np.concatenate((rot1, trans1.T), axis=1) Anexos 66 mat1=np.matmul(intrinsic,extrinsic1) """Leemos imagen 2""" image2 = cv2.imread('im1.png') gray2 = cv2.cvtColor(image2, cv2.COLOR_BGR2GRAY) detector2 = apriltag.Detector() results2 = detector2.detect(gray2) for r in results2: (cX2, cY2) = (int(r.center[0]), int(r.center[1])) x2=np.array([float(cX2), float(cY2)]) cv2.circle(image2, (cX2, cY2), 5, (0, 0, 255), -1) cv2.namedWindow('im2', cv2.WINDOW_NORMAL) cv2.resizeWindow('im2', 800, 800) cv2.imshow('im2', image2) cv2.imwrite('imagen2tri.png', image2) cv2.waitKey(0) """Parametros imagen 2""" roll2=0 pitch2=3.14159*90/180 yaw2=0 rotx2=np.array([[1, 0, 0], [0, np.cos(roll2), - np.sin(roll2)], [0, np.sin(roll2), np.cos(roll2)]]) roty2=np.array([[np.cos(pitch2), 0, np.sin(pitch2)], [0, 1, 0], [- np.sin(pitch2), 0, np.cos(pitch2)]]) rotz2=np.array([[np.cos(yaw2), - np.sin(yaw2), 0], [np.sin(yaw2), np.cos(yaw2), 0], [0, 0, 1]]) rot2=np.matmul(rotz2,roty2) rot2=np.matmul(rot2,rotx2) trans2=np.array([[-0.68, 0, 0.62]]) extrinsic2=np.concatenate((rot2, trans2.T), axis=1) mat2=np.matmul(intrinsic,extrinsic2) """Leemos imagen 3""" image3 = cv2.imread('im2.png') gray3 = cv2.cvtColor(image3, cv2.COLOR_BGR2GRAY) detector3 = apriltag.Detector() results3 = detector3.detect(gray3) for r in results3: (cX3, cY3) = (int(r.center[0]), int(r.center[1])) x3=np.array([float(cX3), float(cY3)]) cv2.circle(image3, (cX3, cY3), 5, (0, 0, 255), -1) cv2.namedWindow('im3', cv2.WINDOW_NORMAL) cv2.resizeWindow('im3', 800, 800) cv2.imshow('im3', image3) cv2.imwrite('imagen3tri.png', image3) cv2.waitKey(0) 67 Control servo-visual de una plataforma giroestabilizada """Parametros imagen 3""" roll31=0 pitch31=3.14159*45/180 yaw31=0 roll32=0 pitch32=-3.14159*45/180 yaw32=0 rotx31=np.array([[1, 0, 0], [0, np.cos(roll31), - np.sin(roll31)], [0, np.sin(roll31), np.cos(roll31)]]) roty31=np.array([[np.cos(pitch31), 0, np.sin(pitch31)], [0, 1, 0], [- np.sin(pitch31), 0, np.cos(pitch31)]]) rotz31=np.array([[np.cos(yaw31), - np.sin(yaw31), 0], [np.sin(yaw31), np.cos(yaw31), 0], [0, 0, 1]]) rot31=np.matmul(rotz31,roty31) rot31=np.matmul(rot31,rotx31) trans31=np.array([[-0.5, 0, 0]]) extrinsic31=np.concatenate((rot31, trans31.T), axis=1) mat31=np.matmul(intrinsic,extrinsic31) rotx32=np.array([[1, 0, 0], [0, np.cos(roll32), - np.sin(roll32)], [0, np.sin(roll32), np.cos(roll32)]]) roty32=np.array([[np.cos(pitch32), 0, np.sin(pitch32)], [0, 1, 0], [- np.sin(pitch32), 0, np.cos(pitch32)]]) rotz32=np.array([[np.cos(yaw32), - np.sin(yaw32), 0], [np.sin(yaw32), np.cos(yaw32), 0], [0, 0, 1]]) rot32=np.matmul(rotz32,roty32) rot32=np.matmul(rot32,rotx32) trans32=np.array([[0.62, 0, 0.18]]) extrinsic32=np.concatenate((rot32, trans32.T), axis=1) mat32=np.matmul(intrinsic,extrinsic32) """Primer punto""" p1 = cv2.triangulatePoints(mat1, mat2, x1.T, x2.T) # Transformamos a coordenadas no homogeneas p1/=p1[3] """Segundo punto""" p2 = cv2.triangulatePoints(mat1, mat31, x1.T, x3.T) p2/=p2[3] """Tercer punto""" p3 = cv2.triangulatePoints(mat1, mat32, x2.T, x3.T) p3/=p3[3] # Transformamos punto al sistema origen p3 = np.matmul(extrinsic2,p3) print('Punto 1 calculado', p1.T) Anexos 68 print('Punto 2 calculado', p2.T) print('Punto 3 calculado', p3.T) """Calcular centroide del triángulo""" xc1=float((p1[0]+p2[0])/2) yc1=float((p1[1]+p2[1])/2) zc1=float((p1[2]+p2[2])/2) xc2=float((p1[0]+p3[0])/2) yc2=float((p1[1]+p3[1])/2) zc2=float((p1[2]+p3[2])/2) v10=float(xc1-p3[0]) v11=float(yc1-p3[1]) v12=float(zc1-p3[2]) v20=float(xc2-p2[0]) v21=float(yc2-p2[1]) v22=float(zc2-p2[2]) A=np.array([[1, 0, 0, -v10, 0], [0, 1, 0, -v11, 0], [0, 0, 1, - v12, 0], [1, 0, 0, 0, -v20], [0, 1, 0, 0, -v21]]) B=np.array([[xc1], [yc1], [zc1], [xc2], [yc2]]) x=np.linalg.solve(A,B) p=x[0:3] print('Projected point from openCV:', p.T) 69 Control servo-visual de una plataforma giroestabilizada Anexo B: Nodo Control_gremsy #include "ros/ros.h" #include <std_msgs/Bool.h> #include <std_msgs/Float32.h> #include <std_msgs/Float32MultiArray.h> #include <geometry_msgs/Quaternion.h> #include <geometry_msgs/Vector3Stamped.h> #include <sstream> #include <sensor_msgs/Imu.h> #include <unistd.h> #include <cmath> #include <iostream> #include <stdlib.h> #include <fstream> #include <ctime> using namespace std; #define DEG_TO_RAD (M_PI / 180.0) #define RAD_TO_DEG (180.0 / M_PI) //Variables globales ofstream archivo; ros:: Publisher control_goal_pub; //Variables a graficar float desired_vector[3]; float current_vector[3]={0,0,0}; float qr[4]; float qe[4]; float omega_real[3]={0,0,0}; //Declaracion angulos en grados float pitch_deg; float yaw_deg; float roll_deg; float omega_e[3]; //Guardar valores subscritos float qros[4]={1,0,0,0}; int16_t omega_act[3]={0,0,0}; int16_t omega_ant[3]={0,0,0}; int16_t omega_raw[3]={0,0,0}; float vector_goals[3]={0,0,0}; float pitch_act; float yaw_act; float roll_act; Anexos 76 Kd2 = message_parametros.data[3]; gamma1 = message_parametros.data[4]; gamma2 = message_parametros.data[5]; } void getSincroCallback(std_msgs::Bool message_sincro) { archivo << qr[0] << endl; archivo << qr[1] << endl; archivo << qr[2] << endl; archivo << qr[3] << endl; archivo << qe[0] << endl; archivo << qe[1] << endl; archivo << qe[2] << endl; archivo << qe[3] << endl; archivo << qros[0] << endl; archivo << qros[1] << endl; archivo << qros[2] << endl; archivo << qros[3] << endl; archivo << omega_e[0] << endl; archivo << omega_e[1] << endl; archivo << omega_e[2] << endl; archivo << pitch_deg << endl; archivo << roll_deg << endl; archivo << yaw_deg << endl; archivo << omega_act[0] << endl; archivo << omega_act[1] << endl; archivo << omega_act[2] << endl; archivo << desired_vector[0] << endl; archivo << desired_vector[1] << endl; archivo << desired_vector[2] << endl; archivo << current_vector[0] << endl; archivo << current_vector[1] << endl; archivo << current_vector[2] << endl; archivo << omega_real[0] << endl; archivo << omega_real[1] << endl; archivo << omega_real[2] << endl; } int main(int argc, char **argv) { sleep(1); archivo.open("/home/xavier1/Documentos/datoscontrol.txt",ios::out); if(archivo.fail()){ cout<<"No se pudo abrir el archivo"; exit(1); } ros::init(argc, argv, "control_gremsy"); ros::NodeHandle pnh; 77 Control servo-visual de una plataforma giroestabilizada ros::NodeHandle nh; ros:: Subscriber control_imu_sub = pnh.subscribe("ros_gremsy/imu/data", 1, getIm uCallback); ros:: Subscriber control_orientation_sub = pnh.subscribe("ros_gremsy/mount_orien tation_local_yaw", 1, getOrientationCallback); ros:: Subscriber control_vector_sub = pnh.subscribe("vector_goals", 1, getGoalsC allback); ros:: Subscriber deteccion_sub = pnh.subscribe("deteccion", 1, getDetCallback); ros:: Subscriber angles_sub = pnh.subscribe("ros_gremsy/angulos_actual", 1, getA nglesCallback); ros:: Subscriber param_sub = pnh.subscribe("parametros",1, getParametrosCallback ); ros:: Subscriber sincro_sub = pnh.subscribe("sincro",1, getSincroCallback); control_goal_pub = pnh.advertise <geometry_msgs::Vector3Stamped>("ros_gremsy/goa ls", 10); ros::Timer timer = nh.createTimer(ros::Duration(1 / 50), publishStateTimerCallback); ros::spin(); archivo.close(); return 0; Anexos 78 Anexo C: Nodo Procesamiento #!/usr/bin/env python3 # -*- coding: utf-8 -*- """ Created on Wed May 26 09:32:04 2021 CODIGO PARA PROBAR PYTHON Y OPENCV EN ROS @author: nacho """ #Python libraries import sys, time import numpy as np import apriltag #Open CV import cv2 #ROS libraries import rospy #ROS messages from gimbalpack.msg import Vector from geometry_msgs.msg import Vector3Stamped from std_msgs.msg import Bool from sensor_msgs.msg import Image #VARIABLES GLOBALES k=0 img_counter=0 det=1 distancia=1 GSD=0.00061*distancia X0=1920/2 Y0=1080/2 cX=X0-1 cY=Y0-1 x_hat=np.matrix([0.01, -0.01, 1]) P_ant=0.5*np.identity(3) x_hat=x_hat.T x_ant=x_hat y_ant=x_ant x_vector=[] P_vector=[] x_corr=x_hat P_corr=P_ant K=x_hat y=y_ant x_hat_vector=[] x_med=[] 79 Control servo-visual de una plataforma giroestabilizada K_vector=[] x_vector=[] P_vector=[] A_vector=[] translation_matrix=[] search=0 detector = apriltag.Detector() ultrefx=0 ultrefy=0 A_search=[] flag_busqueda=0 A=[] def talker(goal,det): pub = rospy.Publisher("vector_goals", Vector3Stamped, queue_size=10) pub2 = rospy.Publisher("deteccion", Bool, queue_size=10) rospy.loginfo('publicando') rospy.loginfo(goal) pub.publish(goal) pub2.publish(det) def callback(imagen): global detector global flag_busqueda global ultrefx global ultrefy global search global A_search global x_hat global x_ant global y_ant global y global P_corr global x_corr global x_hat global K global A global P_ant VERBOSE=False goal=Vector3Stamped() dtype = np.dtype("uint8") dtype = dtype.newbyteorder('>' if imagen.is_bigendian else '<') imagencv = np.ndarray(shape=(imagen.height, imagen.width, 3), dtype=dtype, buffer=imagen.data) imagencv = cv2.cvtColor(imagencv, cv2.COLOR_RGB2BGR) gray = cv2.cvtColor(imagencv, cv2.COLOR_BGR2GRAY) results = detector.detect(gray) Anexos 80 # Bucle con los resultados de apriltags if not results: print("No hay resultado") xb=ultrefx yb=ultrefy print('ultrefx:') print(ultrefx) print('ultrefy:') print(ultrefy) if (search==1): flag_busqueda=1 det=1 if(xb>530): goal.vector.x=530*GSD if(yb>300): goal.vector.y=300*GSD if(xb<-530): goal.vector.x=-530*GSD if(yb<-300): goal.vector.y=-300*GSD if(xb<530.0 and xb>-530.0 and yb<300.0 and yb>- 300.0): #Si estamos dentro del cuadrado, voy a asumir que es una perturbacion, m e quedo como estoy goal.vector.x=float(0) goal.vector.y=float(0) goal.vector.z=float(1) print('Buscando...') talker(goal,det) else: det=0 goal.vector.x=float(0) goal.vector.y=float(0) goal.vector.z=float(1) print('NO BUSCANDO') y=([0, 0, 1]) talker(goal,det) else: for r in results: (cX, cY) = (int(r.center[0]), int(r.center[1])) 81 Control servo-visual de una plataforma giroestabilizada cv2.circle(imagencv, (cX, cY), 5, (0, 0, 255), -1) det=1 X=(cX-X0)*GSD Y=(cY-Y0)*GSD Z=distancia ultrefx=X/GSD ultrefy=Y/GSD y = np.matrix([X, Y, Z]) y = y.T if (flag_busqueda==1): y_ant=y x_ant=y flag_busqueda=0 A = y_ant * y_ant.T P, D, Q = np.linalg.svd(A, full_matrices=False) A = P @ np.diag(D) @ Q A = (y.T * A).T A = A * y_ant.T Q=np.matrix([[0.005, 0, 0], [0, 0.005, 0], [0, 0, 0.005]]) R=Q x_hat = A * x_ant P = (A * P_ant * A.T) + Q K = P * ((P+R)**-1) x_corr = x_hat + K*(y-x_hat) P_corr = (np.identity(3)-K) * P x_ant=x_corr y_ant=y P_ant=P_corr goal.vector.x=float(x_corr[0]) goal.vector.y=float(x_corr[1]) goal.vector.z=float(1) Anexos 82 talker(goal,det) search=1 def listener(): rospy.init_node('Procesamiento') rospy.Subscriber("/UAV_1/usb_cam/image_raw", Image, callback) rospy.Subscriber("ros_gremsy/angulos_actual", Vector3Stamped, callback2) rospy.Subscriber("sincro", Bool, callback3) rospy.spin() with open('/home/nacho/pruebas/outfile1.txt','wb') as f: for line in x_vector: np.savetxt(f, line, fmt='%.2f') with open('/home/nacho/pruebas/outfile2.txt','wb') as f: for line in P_vector: np.savetxt(f, line, fmt='%.5f') with open('/home/nacho/pruebas/outfile3.txt','wb') as f: for line in x_hat_vector: np.savetxt(f, line, fmt='%.2f') with open('/home/nacho/pruebas/outfile4.txt','wb') as f: for line in K_vector: np.savetxt(f, line, fmt='%.2f') with open('/home/nacho/pruebas/outfile5.txt','wb') as f: for line in x_med: np.savetxt(f, line, fmt='%.2f') with open('/home/nacho/pruebas/outfile6.txt','wb') as f: for line in A_vector: np.savetxt(f, line, fmt='%.2f') def callback2(mensaje): global search if ((mensaje.vector.y*180/3.1415)>55.0 or (mensaje.vector.y*180/3.1415)<- 55.0 or (mensaje.vector.z*180/3.1415)>100.0 or (mensaje.vector.z*180/3.1415)<- 100.0): search=0 print(mensaje.vector.y*180/3.1415) print(mensaje.vector.z*180/3.1415) print(search) 83 Control servo-visual de una plataforma giroestabilizada def callback3(mensaje): global x_hat global x_ant global y_ant global P_ant global x_vector global P_vector global x_hat_vector global K_vector global K global x_corr global y global P_corr global A global A_vector x_vector.append(x_corr) P_vector.append(P_corr) x_hat_vector.append(x_hat) K_vector.append(K) x_med.append(y) A_vector.append(A) rospy.loginfo('graficando') if __name__ == '__main__': listener() Anexos 84 Anexo D: Nodo Grabación #!/usr/bin/env python3 # -*- coding: utf-8 -*- """ Created on Wed May 26 09:50:04 2021 PRUEBA PYTHON OPEN CV Y ROS @author: nacho """ #Python libraries import sys, time import numpy as np import apriltag #Open CV import cv2 #ROS libraries import roslib import rospy #ROS messages from gimbalpack.msg import Vector from geometry_msgs.msg import Vector3 from std_msgs.msg import Float32MultiArray from sensor_msgs.msg import Image salida = cv2.VideoWriter('/home/nacho/pruebas/VideoSalida.avi',cv2.VideoWriter_f ourcc(*'XVID'),7.0,(1920,1080)) detector = apriltag.Detector() def callback(imagen): dtype = np.dtype("uint8") dtype = dtype.newbyteorder('>' if imagen.is_bigendian else '<') imagencv = np.ndarray(shape=(imagen.height, imagen.width, 3), dtype=dtype, buffer=imagen.data) print('recibiendo imagen') imagencv = cv2.cvtColor(imagencv, cv2.COLOR_RGB2BGR) gray = cv2.cvtColor(imagencv, cv2.COLOR_BGR2GRAY) results = detector.detect(gray) for r in results: (cX, cY) = (int(r.center[0]), int(r.center[1])) cv2.circle(imagencv, (cX, cY), 5, (0, 0, 255), -1) salida.write(imagencv) def listener(): rospy.init_node('Grabacion') 85 Control servo-visual de una plataforma giroestabilizada rospy.Subscriber("/UAV_1/usb_cam/image_raw", Image, callback) rospy.spin() if __name__ == '__main__': listener() Anexos 92 //Obtener current vector quat_current_vector[0]=(qros[0]*quat_origin_vector[0])- (qros[1]*quat_origin_vector[1])-(qros[2]*quat_origin_vector[2])- (qros[3]*quat_origin_vector[3]); quat_current_vector[1]=(qros[0]*quat_origin_vector[1])+(qros[2]*quat_origin_vect or[3])+(qros[1]*quat_origin_vector[0])-(qros[3]*quat_origin_vector[2]); quat_current_vector[2]=(qros[0]*quat_origin_vector[2])+(qros[2]*quat_origin_vect or[0])+(qros[3]*quat_origin_vector[1])-(qros[1]*quat_origin_vector[3]); quat_current_vector[3]=(qros[0]*quat_origin_vector[3])+(qros[1]*quat_origin_vect or[2])-(qros[2]*quat_origin_vector[1])+(qros[3]*quat_origin_vector[0]); quat_current_vector[0]=(quat_origin_vector[0]*qros[0])-(quat_origin_vector[1]*- qros[1])-(quat_origin_vector[2]*-qros[2])-(quat_origin_vector[3]*-qros[3]); quat_current_vector[1]=(quat_origin_vector[0]*-qros[1])+(quat_origin_vector[2]*- qros[3])+(quat_origin_vector[1]*qros[0])-(quat_origin_vector[3]*-qros[2]); quat_current_vector[2]=(quat_origin_vector[0]*- qros[2])+(quat_origin_vector[2]*qros[0])+(quat_origin_vector[3]*-qros[1])- (quat_origin_vector[1]*-qros[3]); quat_current_vector[3]=(quat_origin_vector[0]*-qros[3])+(quat_origin_vector[1]*- qros[2])-(quat_origin_vector[2]*-qros[1])+(quat_origin_vector[3]*qros[0]); current_vector[0]=quat_current_vector[1]; current_vector[1]=quat_current_vector[2]; current_vector[2]=quat_current_vector[3]; //Obtenemos qreferencia if (det==1) { qe[0]=(qros[0]*qt[0])-(qros[1]*qt[1])-(qros[2]*qt[2])-(qros[3]*qt[3]); qe[1]=(qros[0]*qt[1])+(qros[2]*qt[3])+(qros[1]*qt[0])-(qros[3]*qt[2]); qe[2]=(qros[0]*qt[2])+(qros[2]*qt[0])+(qros[3]*qt[1])-(qros[1]*qt[3]); qe[3]=(qros[0]*qt[3])+(qros[1]*qt[2])-(qros[2]*qt[1])+(qros[3]*qt[0]); Acos = acos(qe[0]); enorm = sqrt(qe[1]*qe[1] + qe[2]*qe[2] + qe[3]*qe[3]); //Multiplicamos el vector componente por componente if (enorm > 0.0001){ Prod = Acos*qe[1]*2.0/enorm; theta_e[0] = Prod; Prod = Acos*qe[2]*2.0/enorm; theta_e[1] = Prod; 93 Control servo-visual de una plataforma giroestabilizada Prod = Acos*qe[3]*2.0/enorm; theta_e[2] = Prod; } else{ theta_e[0] = 0; theta_e[1] = 0; theta_e[2] = 0; } setpoint_pitch = theta_e[1]; setpoint_roll = 0; setpoint_yaw = theta_e[2]; } else { setpoint_pitch = 0; setpoint_roll = 0; setpoint_yaw = 0; } //Establezco y publico goals goals.vector.x = setpoint_roll; goals.vector.y = setpoint_pitch; goals.vector.z = setpoint_yaw; control_goal_pub.publish(goals); //Salida por pantalla t++; if(t%60000==0){ printf("Ejecutando...\n"); } } void changeDet(const ros::TimerEvent &event) { det=0; printf("salta timer"); } void getAnglesCallback(geometry_msgs::Vector3Stamped message_angles) { roll_act = message_angles.vector.x; pitch_act = message_angles.vector.y; yaw_act = message_angles.vector.z; } Anexos 94 void getGoalsCallback(geometry_msgs::Vector3Stamped message_goals) { vector_goals[0] = message_goals.vector.x; vector_goals[1] = message_goals.vector.y; vector_goals[2] = message_goals.vector.z; det=1; puntero_timer->stop(); puntero_timer->start(); } void getImuCallback(sensor_msgs::Imu message_imu) { omega_real[0] = message_imu.angular_velocity.y; omega_real[1] = -message_imu.angular_velocity.x; omega_real[2] = message_imu.angular_velocity.z; } void getOrientationCallback(geometry_msgs::Quaternion message_orientation) { qros[0] = message_orientation.w; qros[1] = message_orientation.x; qros[2] = -message_orientation.y; qros[3] = message_orientation.z; } void getSincroCallback(std_msgs::Bool message_sincro) { archivo << qr[0] << endl; archivo << qr[1] << endl; archivo << qr[2] << endl; archivo << qr[3] << endl; archivo << qe[0] << endl; archivo << qe[1] << endl; archivo << qe[2] << endl; archivo << qe[3] << endl; archivo << qros[0] << endl; archivo << qros[1] << endl; archivo << qros[2] << endl; archivo << qros[3] << endl; archivo << theta_e[0] << endl; archivo << theta_e[1] << endl; archivo << theta_e[2] << endl; archivo << desired_vector[0] << endl; archivo << desired_vector[1] << endl; archivo << desired_vector[2] << endl; archivo << current_vector[0] << endl; archivo << current_vector[1] << endl; 95 Control servo-visual de una plataforma giroestabilizada archivo << current_vector[2] << endl; archivo << omega_real[0] << endl; archivo << omega_real[1] << endl; archivo << omega_real[2] << endl; archivo << pitch_act << endl; archivo << roll_act << endl; archivo << yaw_act << endl; printf("comprobando rate"); } int main(int argc, char **argv) { archivo.open("/home/xavier1/Documentos/datoscontrol.txt",ios::out); if(archivo.fail()){ cout<<"No se pudo abrir el archivo"; exit(1); } ros::init(argc, argv, "control_gremsy"); ros::NodeHandle pnh; ros::NodeHandle nh; ros:: Subscriber control_orientation_sub = pnh.subscribe("ros_gremsy/mount_orien tation_local_yaw", 1, getOrientationCallback); ros:: Subscriber control_vector_sub = pnh.subscribe("vector_goals", 1, getGoalsC allback); ros:: Subscriber angles_sub = pnh.subscribe("ros_gremsy/angulos_actual", 1, getA nglesCallback); ros:: Subscriber sincro_sub = pnh.subscribe("sincro",1, getSincroCallback); control_goal_pub = pnh.advertise <geometry_msgs::Vector3Stamped>("ros_gremsy/goa ls", 10); ros::Timer timer = nh.createTimer(ros::Duration(1 / 100), publishStateTimerCallback); ros::Timer timer_det = nh.createTimer(ros::Duration(0.5), changeDet); puntero_timer = &timer_det; ros::spin(); archivo.close(); return 0; }