Desarrollo e implementación de un algoritmo de seguimiento de trayectorias 5D para aeronaves no tripuladas

Proyecto Fin de Carrera Ingeniería de Telecomunicación Formato de Publicación de la Escuela Técnica Superior de Ingeniería Autor: F. Javier Payán Somet Tutor: Juan José Murillo Fuentes Dep. Teoría de la Señal y Comunicaciones Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2013 Trabajo Fin de Grado Grado en Ingeniería Electrónica, Robótica y Mecatrónica Desarrollo e implementación de un algoritmo de seguimiento de trayectorias 5D para aeronaves no tripuladas Autor: Ángel Corrales Cuevas Tutores: Jesús Iván Maza Alcañiz Pedro Jesús Sánchez Cuevas Dpto. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2022 Trabajo Fin de Grado Grado en Ingeniería Electrónica, Robótica y Mecatrónica Desarrollo e implementación de un algoritmo de seguimiento de trayectorias 5D para aeronaves no tripuladas Autor: Ángel Corrales Cuevas Tutores: Jesús Iván Maza Alcañiz Profesor Titular Pedro Jesús Sánchez Cuevas Doctor Dpto. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2022 Trabajo Fin de Grado: Desarrollo e implementación de un algoritmo de seguimiento de trayectorias 5D para aeronaves no tripuladas Autor: Ángel Corrales Cuevas Tutores: Jesús Iván Maza Alcañiz Pedro Jesús Sánchez Cuevas El tribunal nombrado para juzgar el trabajo arriba indicado, compuesto por los siguientes profesores: Presidente: Vocal/es: Secretario: acuerdan otorgarle la calificación de: El Secretario del Tribunal Fecha: Agradecimientos Me gustaría agradecer a todas las personas que me han acompañado en esta etapa tan importante. Agradecer en primer lugar a Pedro J. Sánchez, por hacerme sentir como en casa desde que llegué a CATEC y todo lo que he aprendido durante mis prácticas. Sin él no hubiera sido posible este proyecto. Gracias a todos los docentes que han participado en mi formación durante estos años en la ETSI. Gracias a mis amigos, a los de siempre y a todos los que me ha dejado esta etapa académica que han hecho el camino más divertido y gratificante. Finalmente, doy las gracias a mis padres, que han sido quienes me han dado todo en esta vida; a mi hermano, por estar ahí siempre que lo necesito; a mi pareja, por su apoyo incondicional y tener que aguantarme estos años; a ellos y al resto de mi familia, por el cariño que siempre me han dado. Ángel Corrales Cuevas Algodonales, 2022 I Resumen La popularidad de los UASs ha experimentado un gran crecimiento en los últimos años y se espera que su uso siga aumentando en las venideras décadas. Por ello, se prevé una mayor congestión del espacio aéreo. En este contexto, son necesarios mecanismos que garanticen la seguridad de las operaciones y permitan maximizar la ocupación del espacio aéreo. Para poder afrontar esta gestión del tráfico aéreo y garantizar la seguridad, la Unión Europea está desarrollando una solución denominada U-Space, que se encargará de asuntos como planificación de vuelo, resolución de conflictos y seguimiento entre otros. En este contexto, se definen las operaciones basadas en trayectorias 4D (4D-TBO), las cuales añaden la dimensión temporal a cada posición espacial. De esta forma, se define en el plan de vuelo en que punto exacto y en que momento va a estar un UAS. En este proyecto se han dado motivos por los cuales se considera oportuno extender estas operaciones en una dimensión más añadiendo el yaw del UAS, 5D-TBO. A partir de ello, se ha desarrollado un seguidor de trayectorias 5D con el cual poder seguir un plan de vuelo 5D predefinido. Este seguidor de trayectorias 5D está pensado para funcionar con cualquier tipo de aeronave, independientemente de su configuración de rotores o ala. Además, puede funcionar con trayectorias que presenten velocidades cambiantes durante su ejecución. Se ha seguido una metodología que busca priorizar la seguridad de las plataformas y usuarios. Por ello, primeramente se han realizado las simulaciones oportunas hasta considerar que era seguro realizar vuelos reales. Se ha llevado a cabo la validación del trabajo realizado haciendo vuelo en interiores en testbed, haciendo uso de la plataforma Crazyflie 2.1. En estos vuelos y en las simulaciones anteriores, se ha podido validar el trabajo realizado. Además, se han realizado simulaciones en otras plataformas, con las que validar la versatilidad del seguidor. III XÍndice Seguidor de trayectorias 22 Cómo afrontar trayectorias 4D inviables 23 3.2 Seguidor de trayectorias 5D 25 3.2.1 Planteamiento del problema 25 3.2.2 Seguidor 5D-TBO para UAS 26 3.2.3 Seguidor de yaw 26 Look-Ahead adaptativo 26 Servicios de ROS 27 Como afrontar trayectorias 5D inviables 27 4 Resultados 31 4.1 Estructura y comentarios 31 4.1.1 Comentarios de simulación 31 4.1.2 Comentarios de vuelo 31 Problemas con VICON 31 Problemas con Crazyflie Server / Backend 32 4.1.3 Trayectorias y experimentos 32 Trayectoria 1 32 Trayectoria 2 33 Trayectoria 3 33 Trayectoria 4 33 Trayectoria 5 34 Trayectoria 6 34 4.2 Resultados obtenidos 35 4.2.1 Trayectoria 1. Línea recta 35 Velocidad constante 0.5m/s35 Velocidad constante 1m/s37 Velocidad cambiante 0.25m/s→1m/s→0.5m/s→0.33m/s39 4.2.2 Trayectoria 2. Círculo achatado. 42 Velocidad constante 0.5m/s42 Velocidad constante 1m/s44 Velocidad cambiante 0.33m/s→1m/s→0.5m/s→0.25m/s46 4.2.3 Trayectoria 3. Forma de ocho 48 Velocidad constante 0.33m/s48 Velocidad constante 0.5m/s50 Velocidad constante 0.2m/s52 4.2.4 Trayectoria 4. Hipódromo + ocho 54 Velocidad constante 0.5m/s54 Velocidad constante 0.25m/s56 Velocidad constante 0.25m/s→0.5m/s58 4.2.5 Trayectoria 5. Espiral 61 Velocidad constante 0.2m/s61 Velocidad constante 0.2m/s(modificada) 63 4.3 Simulaciones adicionales 66 4.3.1 DJI F-450 66 Velocidad constante 0.4m/s66 velocidad cambiante de 0.5m/s→0.3m/s→0.75m/s68 4.3.2 DJI F-550 70 Índice XI Velocidad constante 0.4m/s70 velocidad cambiante de 0.5m/s→0.3m/s→0.75m/s72 4.3.3 DJI Matrice 600 74 Velocidad constante 0.4m/s74 velocidad cambiante de 0.5m/s→0.3m/s→0.75m/s76 5 Conclusiones y líneas futuras 79 5.1 Conclusiones generales 79 5.2 Trabajos futuros 80 Apéndice A Modelos URDF y SDF 81 Apéndice B Código Follower 5D 89 Apéndice C Código misión en testbed 95 Índice de Figuras 97 Índice de Códigos 101 Bibliografía 103 1 Introducción El uso de los sistemas aéreos no tripulados -en inglés Unmanned Aerial Systems, UAS- ha ganado un gran interés en las últimas décadas, tanto para aplicaciones civiles como industriales (ver Fig. 1.1). Las aplicaciones destinadas a los UAS son muy diversas, como pueden ser vigilancia [ 1 ], [ 2 ], [ 3 ], seguimiento de incendios forestales [ 4 ], [ 5 ], [ 6 ], transporte [ 7 ], e inspección de infraestructuras como puentes [8], [9] o refinerías [10], [11]. Figura 1.1 Aplicaciones de los UAS. Debido a este rápido crecimiento del uso de UAS, se prevé un tráfico aéreo mucho más denso en el llamado Very Low Level (VLL) Airspace [ 12 ] en los próximos años. La co-existencia de vehículos aéreos tripulados y vehículos aéreos no tripulados -en inglés Unmanned Aerial Vehicles, UAVs- en el espacio aéreo es uno de los mayores retos de la aviación para los años venideros y requerirá una correcta gestión del tráfico de los UAS, tanto antes del vuelo como durante, con el objetivo de garantizar los niveles de seguridad requeridos y la mayor eficiencia posible. 1 2Capítulo 1. Introducción 1.1 Motivación Para enfrentarse a este escenario de creciente uso de UAS y aumento del tráfico aéreo, las diferentes organizaciones nacionales e internacionales relacionadas con la gestión del espacio y tráfico aéreo están elaborando nuevas regulaciones y reorganizando el espacio aéreo con el objetivo de integrar los UAS en el espacio aéreo civil. 1.1.1 U-Space U-Space [ 13 ] (Fig. 1.2) es la solución para la gestión del tráfico aéreo de UAS en Europa, desarrollada por SESAR [ 14 ] (Single European Sky ATM Research), con el objetivo de garantizar la seguridad y acceso eficiente de una gran variedad de UAS al espacio aéreo civil, a través de una serie de servicios relacionados con la planificación de vuelo, seguimiento y gestión de conflictos entre otros. Otro concepto similar, el UTM [ 15 ] (UAS Traffic Management), está siendo desarrollado por la NASA (National Aeronautics and Space Administration) en Estados Unidos. Figura 1.2 SESAR U-Space logo. El desarrollo del U-Space se basa en los siguientes principios fundamentales [16]: •Garantizar la seguridad de todos los usuarios del espacio aéreo que operen en el framework del U-Space así como las personas en tierra. • Proporcionar un sistema escalable, flexible y adaptativo que pueda responder a los cambios de demanda, volumen, tecnología, modelos de negocio y aplicaciones, a la vez que se gestionan las interacciones con la aviación tripulada. • Permitir operaciones con una alta densidad de UASs bajo la supervisión de operadores en tierra. •Garantizar un acceso equitativo y justo al espacio aéreo para todos los usuarios. • Reducir los costes de implementación y operación manteniendo, dentro de lo posible, los servicios e infraestructuras aeronáuticas existentes como puede ser el GNSS así como los de otros sectores como los servicios de comunicación móvil. La implementación del U-Space será progresiva e irá ligada al desarrollo tecnológico que permitirá disponer de grupos de servicios mayores (ver Fig. 1.3). Con los años, los servicios del U-Space evolucionarán a la vez que lo hagan los niveles de autonomía de los UAS así como las formas de interaccionar con el entorno. • U1, Servicios Fundamentales. Provén de registro digital, identificación digital y establecimiento de perímetros virtuales. • U2, Servicios Iniciales. Incluyen la gestión de las operaciones de UAS y pueden incluir planes de vuelo, aprobaciones de vuelo, seguimiento, información dinámica del espacio aéreo y procedimientos de interacción con el control del tráfico aéreo. • U3, Servicios Avanzados. Permiten operaciones más complejas en áreas más densas y puede incluir capacidades de gestión y asistencia para la detección de conflictos. Incluirá la capacidad de detectar y esquivar de forma autónoma -en inglés detect and avoid, DAA- lo que permitirá el aumento de operaciones en todos los entornos. 1.1 Motivación 3 • U4, Todos los Servicios. Ofrecerán interfaces integradas con la aviación tripulada y estarán basados en un muy alto nivel de automatización, conectividad y digitalización tanto para los UAS como para los sistemas del U-Space. Figura 1.3 Despliegue progresivo del U-Space. 1.1.2 Operaciones basadas en trayectorias de 4 dimensiones (4D-TBO) En relación al U-Space, un concepto importante cuyo desarrollo está siendo estudiado por SESAR (y NextGen en Estados Unidos) [ 17 ] es las operaciones basadas en trayectorias de 4 dimensiones -en inglés 4D-trajectory based operation, 4D-TBO- las cuales incluyen la cuarta dimensión del tiempo en la definición del plan de vuelo junto a las dimensiones espaciales de la trayectoria. El sistema aéreo no solo debe seguir una trayectoria predefinida, si no que también tiene que cumplir con un horario, por lo que cualquier retraso o adelanto en el horario será considerado una desviación respecto a la trayectoria deseada. Inicialmente estaba orientado para la aviación comercial tripulada [ 18 ], pero también ha sido considerado en el framework del U-Space. Incluir la cuarta dimensión temporal en los planes de vuelo optimiza la ocupación del espacio aéreo, lo cual está fuertemente relacionado con los servicios de resolución de conflictos de U-Space. Figura 1.4 Vuelos de prueba tripulados 4D-TBO. 4Capítulo 1. Introducción SESAR llevó a cabo un periodo de vuelos de prueba implementando las 4D-TBO en aviación comercial tripulada [ 19 ]. Se tenía en cuenta las tres dimensiones espaciales y el tiempo en los planes de vuelo. En esta prueba, las operaciones de las distintas aeronaves se establecieron con mucha anticipación, asignando una secuencia de puntos 4D para cada una de ellas, existiendo un punto de convergencia para todas lo cual suele ser crítico en zonas con el espacio aéreo congestionado (ver Fig. 1.4). Después de la coordinación entre los sistemas en tierra y las distintas aeronaves, cada aeronave tenía una ventana de tiempo asignada para su llegada al punto crítico común. Los ejercicios de validación y los vuelos de prueba demostraron grandes reducciones de hasta el -100% de tiempo de espera medio, -6.34% en distancia recorrida por vuelo , -68% en el número de potenciales conflictos y -11.42% en la media de consumo de combustible por vuelo. En la prácica, las 4D-TBO deben definirse como una lista de waypoints compuesto por la posición deseada (tres dimensiones espaciales) más el tiempo deseado de llegada a cada punto (dimensión adicional). Como se indicó previamente, esta definición de las 4D-TBO las convierte en esenciales para el contexto de los servicios del U-Space, sobre todo para los servicios de monitorización y resolución de conflictos. También serán esenciales para permitir optimizar el uso del espacio aéreo, ya que un plan de vuelo no tendrá por qué bloquear el volumen 3D completo durante la ejecución de su operación. De este modo, las 4D-TBO aumentarán la previsibilidad del tráfico aéreo, aumentarán la capacidad del mismo y mejorarán en general la seguridad del tráfico aéreo al existir un menor número de conflictos. Sin embargo, este concepto implica que los sistemas aéreos deben seguir sus planes de vuelo de forma precisa, tanto en posición como en horario previsto. Por ello, un control de seguimiento de trayectorias 4D preciso y fiable es vital para el desarrollo de las 4D-TBO en el contexto del U-Space, permitiendo la integración de un creciente número de UASs en el espacio aéreo civil. Un requisito común para estas aplicaciones es un seguidor de trayectorias autónomo que sea preciso, robusto y eficiente. El problema del seguimiento de trayectorias en los UASs está bien estudiado en la literatura y hay diferentes métodos basados en control o geometría. Algunos de los métodos geométricos son pure pursuit [ 20 ], carrot chasing[ 21 ], line-of-sight (LOS) [ 22 ] y campo de vectores [23]. 1.1.3 Importancia del Yaw Las operaciones basadas en trayectorias 4D (4D-TBO), previamente descritas, definen una trayectoria teniendo en cuenta las coordenadas espaciales (x, y, z) y el momento temporal asociado a cada una de ellas. Sin embargo, no tienen en consideración otros grados de libertad que podría presentar el UAS para la realización de la trayectoria, como pueden ser el Roll, Pitch y Yaw (ver Fig. 1.5). En este punto se van a dar una serie de motivos por los cuales puede ser interesante tener también en cuenta el Yaw en los planes de vuelo de las distintas operaciones. Figura 1.5 Roll, Pitch y Yaw en UASs de ala fija y multirrotores. 1.1 Motivación 5 Los sistemas robóticos aéreos han sido ampliamente estudiados por la comunidad científica en la última década. En la actualidad, las habilidades de un UAS van desde operaciones completamente autónomas tanto en exterior como interior [ 24 ], [ 25 ], haciendo uso de sensores y planificación de movimiento [26], hasta escaneo y mapeo del entorno [27] así como búsqueda y vigilancia [28]. En varias de las aplicaciones mencionadas y en muchas otras más, es de gran importancia tener control sobre el campo de visión -en inglés, field of viewde la cámara o las cámaras del UAS (ejemplo en Fig. 1.6). Es necesario mantener dentro del campo de visión la zona que se desea vigilar, el terreno que se quiere mapear o el evento que se quiere filmar. En el caso de que la cámara o las cámaras se encuentren fijas o con gimbals de movilidad limitada, es necesario rotar el UAS para realizar la operación. Es por ello por lo que poder definir el Yaw de la aeronave en el plan de vuelo de la misma puede resultar interesante de cara a obtener los mejores resultados. Figura 1.6 Área de visión del sensor de un UAS en función de la altura de vuelo halt , la elevación del gímbal θg , el campo de visión horizontal ηh y el campo de visión vertical ηv . Extraido de [28]. El vuelo coordinado de múltiples UASs de forma autónoma está en la actualidad siendo estudiado por distintos grupos de investigación [ 29 ], [ 3 ]. Las operaciones y tareas desempeñadas por grupos de múltiples UASs implican un elevado riesgo de colisión entre los UASs, por lo que es necesario un sistema que garantice la seguridad e la operación. Algunas soluciones como las planteadas por [ 30 ] necesitan un mecanismo que permita una comunicación estable e ininterrumpida entre los UASs. Habrá situaciones en las que esta comunicación no pueda garantizarse y en el contexto actual de aumento de uso de los UAS, también será necesario evitar colisiones entre UASs que no pertenezcan a la misma operación, así como con el entorno y como obstáculos diversos. Es por ello por lo que puede llegar a ser necesario dotar al UAS de un sistema de detección de obstáculos y evitación de colisiones como se describe en [ 31 ]. Estos sistemas suelen funcionar gracias a la información recibida de sensores ubicados en el UAS como pueden ser sensores ultrasonidos [ 32 ], sensores láser o cámaras. No siempre resulta viable colocar estos sensores de forma que abarquen todas las posibles direcciones del UAS, ya sea por motivos de costes, reducción de la autonomía 6Capítulo 1. Introducción por gasto energético o aumento de peso del aeronave. Por este motivo resulta de gran importancia hacer avanzar la aeronave de forma que estos sensores abarquen la zona a la que va a dirigirse y su entorno cercano. Para conseguirlo, se precisa actuar sobre el yaw del UAS para mantener los sensores y el campo de recepción de los mismos en la dirección de avance del UAS. Por otro lado, la maniobrabilidad de la aeronave resulta un factor a tener en cuenta. Como es bien conocido, los multi rotores tienen una gran maniobrabilidad con muy pocas limitaciones. Pueden desplazarse de forma ágil. Sin embargo, pese a la amplia popularidad de los multi rotores en los últimos años, el abanico de UAS es muy amplio y diverso (ver Fig. 1.7). Figura 1.7 Diferentes tipos de UAS en función de la posición de sus rotores y configuraciones de ala que presenten. En función de la aplicación específica que se busque satisfacer, puede que sea necesario emplear otro tipo de UAS distinto del multi rotor como pueden ser los ala fija o los ala fija híbridos o VTOL -en inglés, ertical Take-Off and Landing- en cuyo caso su maniobrabilidad es más reducida y depende de otros factores como el radio de giro de la aeronave. En este tipo de UAS, es muy necesario definir y controlar el yaw de la aeronave puesto que están diseñados especificamente para avanzar de una forma concreta. 1.2 Objetivos El objetivo de este trabajo es obtener un seguidor de trayectorias 5D el cual siga planes de vuelo compuestos por las componentes descritas anteriormente en las 4D-TBO además del yaw de la aeronave, las cuales serán llamadas a partir de ahora trayectorias 5D. Este seguidor podrá adaptarse a todas las plataformas, independientemente de su configuración de ala o rotores, lo que permitirá el poder desarrollar algoritmos se puedan adaptar a todas ellas permitiendo una fácil abstracción en la etapa de desarrollo de los mismos. El seguidor deberá recibir un plan de vuelo definido en un a etapa de planificación de la misión de más alto nivel, el cual contendrá una serie de waypoints compuesto por las 5 dimensiones descritas previamente y comandará las velocidades necesarias en el UAS para seguir dicha trayectoria. En este trabajo se asumirán una serie de prerrequisitos: • Los waypoints que componen la trayectoria del plan de vuelo serán lo suficientemente densos (poco separados en tiempo y distancia) para que la trayectoria sea realizable. 1.3 Estructura del documento 7 • Las trayectorias se asumen realizables por el UAS al que vayan destinadas, se deberá de encargar de comprobar la viabilidad de las mismas la etapa de generación de trayectorias (ajena a este trabajo). • Pese a estar destinado a todo tipo de UAS, debido a las limitaciones de las instalaciones y temporales, solo se realizarán pruebas de validación en multi rotores. 1.3 Estructura del documento En el presente apartado se presentará la estructura que seguirá este documento, indicando una breve descripción de cada una de las partes. En el capitulo 1 se han introducido las motivaciones y objetivos del proyecto así como algunos conceptos clave como el U-Space o las 4D-TBO. En el capitulo 2 se describirá la metodología empleada en el desarrollo del proyecto, la cual seguirá como principal objetivo el minimizar los posibles riesgos. Se describirá el entorno de simulación y de vuelo así como la capa de abstracción desarrollada por el GRVC [ 33 ] la cual resultará de gran ayuda. En el capitulo 3 se presentará el algoritmo de partida, el cual se trata de un seguidor de trayectorias 4D. Se describirán todas las modificaciones realizadas hasta llegar a obtener el resultado final. En el capitulo 4 se expondrán los resultados obtenidos tanto en simulación como en vuelo real, permitiendo la validación del proyecto. En el capitulo 5 se presentarán lineas de mejora sobre el trabajo realizado y trabajos futuros. 14 Capítulo 2. Metodología 2.3.2 Arquitectura de UAL UAL está desarrollado sobre ROS [ 44 ], por lo que contiene las librerías y herramientas necesarias para ayudar al desarrollo de software para aplicaciones robóticas. Proporciona la abstracción de hardware, drivers de sensores, librerías dedicadas, herramientas de visualización, comunicación por paso de mensajes, gestión de paquetes, y mucho más. La principal ventaja de utilizar ROS es lo extendido que está su uso en la comunidad y la facilidad con la que se pueden comunicar los diferentes procesos y máquinas. Figura 2.7 Esquema de las diferentes capas para implementar el back-end en UAL. Extraido de [41]. Cada autopilot es particular y presenta diferencias con el resto, en sus protocolos de comunicación, servicios disponibles y demás elementos. El framework de trabajo propuesto por UAL consiste en tres capas básicas (ver Fig. 2.7). La capa más interna, también llamada derived back-end, se encarga de gestionar dicho autopiloto. La capa intermedia o back-end establece la interfaz común, igualando los distintos autopilotos al estandar de UAL. Por último, la capa exterior es UAL en sí misma. Por lo tanto, las capas de UAL y el back-end, son comunes; por otro lado, la capa derived back-end es especifica de cada aotopilot o protocolo. Por ejemplo, cualquier autopilot que utilice MAVLink [45] como protocolo de comunicaciones (PX4 o ArduPilot) puede utilizar el back-end MAVROS. El back-end MAVROS se comunica vía MAVROS con el autopilot y administra los problemas específicos del mismo como el cambio de modo (manual/auto) o las referencias en posición (ver Fig. 2.9). UAL permite dos interfaces distintas (ver Fig. 2.8) accesibles por los usuarios externos: Figura 2.8 Doble interfaz de UAL con el usuario, como clase o como llamadas se servicios de ROS. Extraido de [41]. 2.3 UAL - UAV Abstraction Layer 15 • Clases: el desarrollador puede crear objetos de la clase UAL y acceder a sus datos y funciones mediante la interfaz de clase, directamente llamando a sus métodos o funciones. • Servicios: al mismo tiempo, dentro de la instanciación de la clase y en hilos separados, UAL puede estar continuamente publicando datos y respondiendo a llamadas de servicios como cualquier otro nodo dentro de ROS. La interfaz de clase en un middleware [ 46 ] independiente, está siempre disponible y no introduce retrasos. Por otro lado, la interfaz de servidor depende de si un middleware (ROS) está disponible, lo que puede introducir retrasos en el sistema. Además, solo un proceso puede tener una (y solo una) interfaz de clase para un cierto UAS, lo cual puede ser un inconveniente para trabajar con múltiples UASs. Sin embargo, a la interfaz de servidor se puede acceder desde cualquier dispositivo de la red. Figura 2.9 Esquema con los back-ends disponibles para UAL. Extraido de [41]. 2.3.3 Simulación e integración con Gazebo Además de abstraer al usuario del autopilot, UAL proporciona herramientas que ayudan a probar los algoritmos desarrollados en simulación. En particular, UAL está completamente integrado con el simulador Gazebo[ 47 ] (ver Fig. 2.10). Este simulador permite crear escenarios y prototipos de robots, los cuales se presentarán en el siguiente punto, y está integrado con ROS. Aunque no vaya a ser objeto de este proyecto, destacar que UAL también está integrado con Unreal Engine [ 48 ] y el plugin para simulación de UAVs AirSim [ 49 ]. A continuación se detallará la integración con Gazebo, utilizada en el proyecto. Figura 2.10 Logo de Gazebo. Existen dos posibilidades para realizar simulaciones de UAL con Gazebo: una simulación ligera y la simulación software-in-the-loop (SITL) [ 50 ] de PX4. La primera utiliza un back-end ligero que proporciona un modelo simple del UAS, sin tener en cuenta dinámicas, representando el UAS en Gazebo. Para este proyecto se descartará este tipo de simulaciones puesto que se busca el obtener los resultados lo más fieles posibles a la realidad. La segunda opción está basada en el firmware de PX4 [ 51 ], el cual es un autopiloto de código abierto. Además de las funcionalidades típicas de un autopiloto, el firmware de PX4 incluye un entorno de simulación SITL basado en Gazebo y RotorS [ 52 ]. Este SITL tiene varios plugins de Gazebo que simulan sensores como IMU o GPS y dinámicas como las velocidades y fuerzas de los rotores del UAS. UAL ofrece la posibilidad de usar las simulaciones SITL con el PX4 SITL utilizando el back-end de MAVROS. Esta funcionalidad permite al usuario ejecutar simulaciones en el mismo software que en la plataforma real de vuelo, consiguiento imitar los comportamientos de bajo nivel del autopilot de una manera muy real. 16 Capítulo 2. Metodología Por todo ello, la capa de abstracción UAL es un elemento esencial en el desarrollo de este proyecto, ya que permite desarrollar el algoritmos buscado de una forma mucho más sencilla y dotándonos de unas simulaciones muy cercanas a la realidad. 2.4 Entorno de simulación Este entorno de simulación está desarrollado para utilizarse en ROS [ 53 ], [ 44 ] junto al simulador Gazebo [ 47 ]. Está compuesto por el ecosistema de UAL y su integración con el simulador Gazebo (la variante SITL de PX4), así como de los distintos modelos que se han desarrollado para este proyecto y se detallan a continuación. Tal y como se indicó anteriormente, todas las pruebas preliminares se realizarán en simulación con el fin de minimizar los posibles riesgos. Para llevarlas a cabo, se ha elaborado un modelo que sea lo más fielmente parecido al testbed donde tendrán lugar los vuelos reales. Además del testeb, son necesarios los modelos para simulación de los distintos UAS que se deseen simular, por lo que incluye algunos de los modelos de multi rotores más utilizados en CATEC [34]. 2.4.1 Testbed A partir del aspecto, dimesiones y forma del testbed de CATEC, se ha creado en Blender [ 54 ] un modelo 3D (Fig. 2.11) bastante parecido a la realidad (Fig. 2.2). No obstante, se ha priorizado que el modelo sea ligero y la simulación pueda mantener un factor de tiempo real lo más cercano a 1 en todo momento antes que tener un modelo muy detallado y pesado. Figura 2.11 Testbed en Gazebo. Para poder utilizar el modelo de Testbed en Gazebo, se han generado archivos .dae con el objeto 3D y se ha definido el modelo en formato SDF [ 55 ]. Este archivo SDF contiene las propiedades físicas o links del objeto, en este caso son del tipo "collision". Consultar apéndice A.1. 2.4 Entorno de simulación 17 2.4.2 Crazyflie 2.1 Se ha realizado un modelo del Crazyflie 2.1 para las simulaciones en Gazebo. Para crear el modelo es necesario tener los archivos en extensión .dae del modelo 3D tanto de las hélices (cw.dae / ccw.dae) como del cuerpo del UAV (multirotor.dae) por separado (para poder tener movimiento relativo de las hélices) y una serie de archivos .xacro (XML Macros) [ 56 ] donde se definen las características de la aeronave. Gracias al paquete URDF (Unified Robot Description Format) [ 57 ], [ 58 ], el cual es un parser en C++ con el cual se puede expresar el modelo de un robot en formato XML. Figura 2.12 Peso exacto del Crazyflie 2.1 y las hélices utilizadas. Para elaborar este modelo (Fig. 2.13) se han tenido en cuenta las características físicas de los Crazyflies disponibles en CATEC (ver Fig. 2.12). Además, se le ha añadido una IMU genérica y las macros para el correcto funcionamiento de Mavlink [ 45 ]. Debido al reducido tamaño de las hélices y su composición de plástico flexible, resulta muy complejo hacer un buen modelo de la dinámica del yaw. Por falta de datos, se han ajustado parámetro como la "motor constant" o "rotor drag coefficient" mediante prueba y error, hasta conseguir un modelo estable. Por ello, se prevé que el modelo diste del comportamiento real. Consultar apéndices A.2, A.3. Figura 2.13 Crazyflie 2.1 en Gazebo. 2.4.3 Otros UAVs Además del modelo descrito previamente para el Crazyflie 2.1, se han desarrollado modelos siguiendo la misma metodología para varios de los UASs con los que se cuenta en CATEC. La idea de esto es poder disponer de un entorno de simulación completo con el modelo del banco de pruebas de interiores y los distintos modelos de aeronaves de los que se vaya disponiendo. Esta herramienta puede llegar a ser muy útil y polivalente para futuros proyectos y trabajos que impliquen simulaciones. 18 Capítulo 2. Metodología DJI FlameWheel 450 (F-450) Se trata de un cuadricóptero o quadrotor diseñado para uso recreativo y como plataforma de investigación (Fig 2.14). Puede funcionar con sistemas de autopilot de DJI WKM o NAZA. Pesa 282g, la distancia entre motores es de 450mm y un peso máximo de despegue de hasta 1600g [ 59 ]. Figura 2.14 DJI F-450 con y sin tren de aterrizaje en la realidad y en Gazebo. DJI FlameWheel 550 (F-550) Se trata de un hexacóptero desarrollado por DJI (Fig. 2.15) con características muy similares al anterior F-450. Cuenta con un mayor peso de 478g, una envergadura de 550mm y un peso máximo de despegue 2400g [60]. Figura 2.15 DJI F-550 en la realidad y en Gazebo. 2.4 Entorno de simulación 19 DJI Matrice 600 El matrice 600 (Fig. 2.16) es un UAV profesional de DJI con un peso de 9.6kg, una distancia entre ejes diagonales de 1133 mm y peso máximo de despegue de 15.1kg [61]. Figura 2.16 DJI Matrice 600 en la realidad y en Gazebo. CATEC 750 Se trata de un modelo de fabricación propia de CATEC utilizado en distintos proyectos como en la prueba piloto del 5G en UAS realizada por Vodafone (ver Fig. 2.17). Tiene una envergadura de 1.85m y un peso máximo de despegue de 25kg. [62]. Figura 2.17 CATEC 750 en la realidad y en Gazebo. Destacar que en la actualidad el CATEC 750 cuenta con unos rotores coaxiales, por lo que cuenta con el doble de hélices. 3 Algoritmo Tal y como se ha planteado con anterioridad, el objetivo de este proyecto es desarrollar un seguidor de trayectorias 5D. Este seguidor, además de poder afrontar las 4D-TBO en el contexto del U-Space, podrá controlar el yaw del UAS. Para desarrollar el algoritmo en cuestión, se toma como punto de partida el seguidor de trayectorias 4D presentado en [ 63 ] por Héctor Pérez-Leon, José Joaquín Acevedo, Iván Maza y Anibal Ollero. Además de implementar el seguimiento de yaw para el UAS, se va a adoptar un criterio que permita hacer el algoritmo más independiente haciendo que el look −ahead del controlador sea calculado automáticamente. También se realizarán las adaptaciones necesarias en el código para que este funcione medainte llamadas a servicios de ROS. 3.1 Seguidor de trayectorias 4D 3.1.1 Planteamiento del problema El seguidor de trayectorias 4D propuesto afronta el problema del seguimiento de trayectorias desde un control de velocidad del UAS. Un UAS Q , cuya posición está definida por p(t)∈R3 en cualquier instante t , tiene que seguir una trayectoria 4D Γ de una longitud L . La trayectoria 4D se define por dos funciones: • Una curva en el espacio γ(λ)∈R3 , con λ∈[0,L] que define la posición deseada del UAS en el espacio. • Una funcion de tiempos τ(λ)∈R , con λ∈[0,L] , que define los tiempos deseados de llegada asociados a cada punto espacial. Asumiendo que Q el holonómico y controlado en velocidad, siendo su velocidad definida como v(t) en cualquier instante t . El movimiento del UAS es controlado por comandos de velocidad, de forma que dp(t)) dt =v(t) . Por otro lado, v(t) está limitada por vmax , por lo cual |v(t)|≤vmax en cualquier instante t. Durante el vuelo, en cualquier momento t , la distancia respecto a la trayectoria λp se define como λp=argminλ∈[0,L]|p(t)−γ(λ)|(3.1) El objetivo es implementar un sistema de control que genere comandos de velocidad para seguir la trayectoria, minimizando la distancia mínima normal entre la actual trayectoria realizada por Q y la trayectoria deseada, la cual viene dada por J1=1 TZT 0p(t)−γ(λp(t))dt,(3.2) 21 22 Capítulo 3. Algoritmo y haciendo mínima la distancia entre el tiempo actual t y el tiempo deseado dado por Γ(λp(t)) , la cual se define como J2= 1 TZT 0 (Γ(λp(t))−t)dt ,(3.3) donde Tes el tiempo necesario para completar la larea. 3.1.2 Seguidor 4D-TBO para UAS El generador de trayectorias y el seguidor de trayectorias son dos componentes de este framework. La forma estándar de usarlo es interactuando directamente con el seguidor de trayectorias, pero el usuario puede interaccionar con el generador si es necesario (ver Fig. 3.1). Figura 3.1 Arquitectura del framework 4D Follower. Extraido de [63]. Generador de trayectorias El generador de trayectorias tiene el objetivo de generar una trayectoria Γ a partir de la lista de waypoints 4D recibida. La trayectoria generada es una lista de waypoints 3D mucho más densa, la cual puede ser aproximada a la curva continua γ(λ) descrita anteriormente. También genera una lista interpolada de los tiempos iniciales τ(λ) de la misma dimensión que los waypoints 3D más densos. Ambas listas interpoladas son necesarias en este framework de seguidor de trayectorias 4D. Seguidor de trayectorias Figura 3.2 Vista cenital del seguidor de trayectorias basado en el algoritmo carrot chasing sin tener en cuenta el error en orientación. Extraido de [63]. 3.1 Seguidor de trayectorias 4D 23 El seguidor de trayectorias recibe la trayctoria deseada Γ definida como una lista de waypoints 4D. También debe recibir como parámetro la distancia loo-ahead d y la velocidad máxima vmax . Ajustar estos parámetros adecuadamente es la clave para obtener buenos resultados, dependiendo de la trayectoria 4D deseada. Una lista de waypoints densa en necesaria para el método de seguimiento de trayectorias de forma eficiente. Por ello, se utiliza el generador de trayectorias presentado con anterioridad para obtener una curva discreta γ(λ) a partir de la lista de waypoints original W . El seguidor recibe en todo momento la posición del UAS p(t) y el tiempo actual t para generar comandos de velocidad v(t) , basandose en el método que se va a describir a continuación. El método implementado en este seguidor de trayectorias está basado en el algoritmo carrot chasing (ver Fig. 3.2). Primero se obtiene el argumento λp usando la expresión 3.1, el cual minimiza la distancia entre la posición del UAS y la trayectoria. La distancia look ahead d es constante y conocida y la posición objetivo se obtiene como pt(t) = γ(λp(t)+d).(3.4) Para arreglar el error en tiempo, la velocidad de crucero en el instante tse calcula como vc=d τ(λp(t)+d)−t(3.5) El último paso es calcular la velocidad a comandar, a partir de la velocidad de crucero de la siguiente forma v(t) = vc pt(t)−p(t) |pt(t)−p(t)|(3.6) con la cual se puede alcanzar el punto objetivo. Cómo afrontar trayectorias 4D inviables Cabe la posibilidad de que el usuario establezca una lista de waypoints inviable ante las capacidades del UAS (para alcanzarlos a tiempo sea necesario superar la velocidad máxima del UAS), donde algunos waypoints wi son alcanzables pero otros no. En este caso, este framework se encargará de corregir la lista inicial de waypoints y reemplazará los inalcanzables con tiempos asumibles de forma que el UAS pueda realizar la trayectoria. La diferencia de tiempo entre afrontar un segmento a la máxima velocidad del UAS vmax y a la velocidad indicada inicialmente en los waypoints vdesired será ∆T=ωi+1−ωi vdesired −ωi+1−ωi vmax .(3.7) Cada segmento entre dos waypoints si se clasifica en tres opciones diferentes: inviable si ∆T es negativo, modificable si ∆T es positivo y no modificable si ∆T es igual a cero. Los segmentos no modificables son en los cuales el UAS va a la máxima velocidad. Los segmentos inviables necesitan ser ampliados en tiempo ya que el UAS no tiene tiempo suficiente para ejecutarlos sin sobrepasar la velocidad máxima. Los segmentos modificables Sson en los cuales el UAS podría aumentar su velocidad, por lo que se podría reducir su tiempo si fuera necesario. Para corregir una lista de tiempos inviables, se comienza ampliando los segmentos inviables hasta que son no modificables y se va almacenando cuánto se han ampliado. Después, la lista estará compuesta por segmentos modificables y no modificables únicamente. Cada segmento modificable puede recibir una cantidad de tiempo que lo convierta en no modificable, si el tiempo que se le añade es mayor que esa cantidad, los otros segmentos modificables deben tomar el tiempo restante si es posible. 4 Resultados 4.1 Estructura y comentarios Primero se realizarán una serie de apreciaciones sobre las simulaciones y vuelos que se van a detallar a continuación. Estos comentarios están principalmente relacionados con las distintas problemáticas que se han encontrado a lo largo del proyecto y cómo se han afrontado dichos problemas. Finalmente, se detallarán las trayectorias que han sido llevadas a la práctica en la sección posterior. Para cada una de las distintas trayectorias se aplicarán variaciones de velocidades. 4.1.1 Comentarios de simulación Las simulaciones se han llevado a cabo en el entorno descrito en los apartados 2.3, 2.4, haciendo uso del modelo del Crazyflie 2.1 descrito en 2.4.2. En las simulaciones, la posición del UAS viene determinada por un GPS simulado por UAL. Este sistema de posicionamiento es mucho menos preciso que VICON y su tiempo de respuesta es más lento. Por lo que pueden presentarse mayores errores de posición en simulación que en vuelo real según la trayectoria. Sobre todo en trayectorias que contengan cambios de direcciones y sean de pequeñas dimensiones. 4.1.2 Comentarios de vuelo Tras verificar, mediante simulaciones, que se podía pasar a la fase de vuelo real sin poner en peligro la plataforma se empezaron a detectar una serie de problemas. Dichos problemas se detallan a continuación, así como las soluciones adoptadas para algunos de ellos. Problemas con VICON El sistema de posicionamiento VICON disponible en el testbed descrito en 2.1.1 es el encargado de calcular en tiempo real la posición y la orientación del Crazyflie. Para poder conocer la posición del mismo, basta con que el UAV cuente con un marcador retrorreflectivo. Sin embargo, para poder estimar la orientación, es necesario que cuente con al menos dos de estos marcadores y no presenten simetrías. Debido a las reducidas dimensiones del Crazyflie, este solo puede disponer de dos de estos marcadores. Esto supone un problema cuando alguno de los marcadores queda fuera del rango de visión de varias de las cámaras, ya que el sistema termina perdiéndolo y es incapaz de calcular un yaw correcto. Este problema se agudiza en las partes más exteriores del testbed, próximas a las paredes y al techo. Mientras el UAS se desplaza realizando la trayectoria, el marcador colocado en uno de los brazos se pierde (el propio cuerpo del UAS "tapa" el marcador) constantemente por el sistemas arrojando orientaciones erróneas. Esto se agrava si el UAS realiza rotaciones sobre si mismo y el control de yaw está activo, lo que hace que el crazyflie intente corregir errores de orientación inexistentes de forma brusca. 31 32 Capítulo 4. Resultados Inicialmente, se optó por realizar los experimentos sin realizar el control de yaw, dejando de comandar la velocidad angular ωz . Después, cuando se obtuvieron buenos resultados de forma segura sin tener en cuenta el yaw, se adaptaron algunos de los experimentos para mantener la trayectoria del UAS en la zona del testbed donde había más probabilidad de que las dos balizas fueran localizadas exitosamente (centro). Con esto, se consiguieron obtener experimentos exitosos con un buen control de yaw. Problemas con Crazyflie Server / Backend Para realizar los vuelos en el testbed se hizo uso de un paquete de ROS desarrollado con anterioridad en CATEC que disponía de la integración del Crazyflie Server [ 64 ], la obtención de la posición mediante VICON, las comunicaciones mediante la Crazyradio PA 2.2.2 y demás elementos necesarios para su funcionamiento. Uno de los grandes inconvenientes detectados a la hora de comenzar con los vuelos fueron las problemáticas al utilizar los servicios del Crazyflie Driver. Presentaba grandes problemas a la hora de conmutar entre utilizar comandos de velocidad a servicios. Por ejemplo, si el UAV realizaba una trayectoria 5D mediante comandos de velocidades lineales y angulares, al completarla, resultaba imposible poder hacer llamadas a servicios del Crazyflie Driver (como podría ser el servicio de crazyflie_driver::Land). Por otro lado, si se hacía una llamada a un servicio del Crazyflie Driver y durante su ejecución se perdía la comunicación con la Crazyradio PA en algún instante, esta no volvía a retomarse y el UAV quedaba ejecutando el servicio de forma constante. Por ejemplo, si se hacía una llamada a crazyflie_driver::Takeoff que durase unos cinco segundos en completarse y en algún instante durante esos segundos se perdía la comunicación, el crazyflie quedaba ascendiendo de forma continuada hasta impactar con el techo del testbed. Debido a la saturación de redes en la banda de 2.4GHz en las instalaciones por demás puntos de acceso WiFi, estas incidencias eran habituales. Ante la falta de tiempo para poder afrontar estos problemas, ya que la fase de vuelos en testbed comenzó a falta de menos de una semana de finalizar la beca en CATEC, no se pudo profundizar en las causas de estos sucesos. Puesto que el objetivo final era poder validar el funcionamiento del seguidor de trayectorias 5D desarrollado, se optó por comandar los despegues y aterrizajes del UAV mediante comandos de velocidad, como puede verse en C.1, con un simple control proporcional. De esta forma se pudieron ejecutar las trayectorias de forma segura y sin incidencias y se validó el trabajo realizado. 4.1.3 Trayectorias y experimentos Para poder validar el funcionamiento del proyecto, se han preparado una serie de trayectorias 5D, las cuales van a ser ejecutadas previamente en simulación y posteriormente en vuelo real. En este apartado se van a detallar las características y geometría de las mismas. Destacar que en todas ellas, el criterio tomado para el yaw será orientar el UAS con la dirección de avance. No se aplicará el control de Yaw en los vuelos en testbed en todas las trayectorias que se describen a continuación a causa de la problemática del posicionamiento descrita con anterioridad. Para cada trayectoria, se indicará en cual de los experimentos se activará el seguimiento del yaw para el UAS. Trayectoria 1 Trayectoria en línea recta de 6m de longitud y 2.5m de altura (ver Fig. 4.1). Se realizarán tres experimentos haciendo uso de ella: •velocidad constante de 0.5m/s •velocidad constante de 1m/s •velocidad cambiante de 0.25m/s→1m/s→0.5m/s→0.33m/s.Sí yaw 4.1 Estructura y comentarios 33 Trayectoria 2 Se trata de una trayectoria con forma circular achatada, realizada a una altura constante de 2.5m (ver Fig. 4.1). Se realizarán tres experimentos haciendo uso de ella: •velocidad constante de 0.5m/s •velocidad constante de 1m/s •velocidad cambiante de 0.33m/s→1m/s→0.5m/s→0.25m/s Figura 4.1 Trayectorias 1 y 2. Trayectoria 3 Trayectoria en forma de ocho a 1.5m de altura (ver Fig. 4.2). Se realizarán tres experimentos: •velocidad constante de 0.33m/s •velocidad constante de 0.5m/s •velocidad constante de 0.2m/s.Sí yaw Trayectoria 4 Trayectoria en forma de ocho + hipódromo a 2.5m de altura (ver Fig. 4.2). Se realizarán tres experimentos haciendo uso de ella: •velocidad constante de 0.5m/s •velocidad constante de 0.25m/s.Sí yaw •velocidad cambiante de 0.25m/s→0.5m/s Figura 4.2 Trayectorias 3 y 4. 34 Capítulo 4. Resultados Trayectoria 5 Trayectoria en forma de espiral de 1m de radio ascendente desde una altura de 1.5m hasta 3.4m (ver Fig. 4.3), a una velocidad constante de 0.33m/s . En este caso en particular, las diferencias entre los dos experimentos presentados será el criterio de orientación del yaw: •UAS orientado hacia dirección de avance. Sí yaw •UAS alternando orientación entre dirección de avance y contra dirección de avance. Sí yaw Trayectoria 6 Esta trayectoria consiste en un doble hipódromo enlazado a una altura de 2m (ver Fig. 4.3). Se utilizará unicamente en las simulaciones adicionales y se realizarán dos experimentos: •velocidad constante de 0.4m/s •velocidad cambiante de 0.5m/s→0.33m/s→0.75m/s Figura 4.3 Trayectorias 5 y 6. 4.2 Resultados obtenidos 35 4.2 Resultados obtenidos A continuación se detallan los resultados obtenidos realizando las distintas trayectorias con el Crazyflie 2.1 tanto en simulación como en testbed. 4.2.1 Trayectoria 1. Línea recta Velocidad constante 0.5m/s Trayectoria en línea recta a una velocidad media, el control de yaw está desactivado en el testbed por lo que ωz=0 . Puede observarse en Fig. 4.4 como el despegue y aterrizaje del vuelo en testbed ser realizó comandando velocidades en las posiciones de comienzo y fin de la trayectoria, mientras que en simulación se hizo uso de los servicios de UAL para ir al punto de comienzo. Esto se repetirá en todos los experimentos futuros. Figura 4.4 Trayectoria 1 en simulación y en testbed a 0.5m/s. A simple vista, observando las trayectorias descritas, puede verse que tanto en simulación como en vuelo se presentaron resultados similares pudiéndose completar la trayectoria de forma correcta. En la Fig. 4.5 puede verse una captura de la estela descrita por el crazyflie en vuelo dentro del testbed. Figura 4.5 Trayectoria 1 descrita por el Crazyflie en testbed a 0.5m/s. 36 Capítulo 4. Resultados En la Fig. 4.6 se representan las referencias en posición y ángulo de yaw asi como las trayectorias descritas tanto en simulación como en testbed. En la Fig. 4.7 pueden verse las distintas gráficas de los errores en posición de este experimento. Puede apreciarse como el seguimiento en ambos casos es bueno, el comportamiento de ambos es bastante similar, con magnitudes de errores similares. Figura 4.6 Posición y referencia en el tiempo para la Trayectoria 1 a 0.5m/s. La trayectoria avanza únicamente en dirección x por lo que el mayor error se concentra aquí, de unos 10 centímetros como máximo en testbed y unos 18cm en simulación. Observando el error en x puede verse la mayor lentitud de la simulación a causa del sistema GPS. Los errores en yaw son prácticamente nulos, notándose cierto ruido a causa de los errores de VICON en el yaw. Figura 4.7 Errores de posición y yaw en el tiempo para la Trayectoria 1 a 0.5m/s. 4.2 Resultados obtenidos 37 Los comandos de velocidad aplicados en la Fig. 4.8 en ambos casos son muy parecidos, puede notarse la fase de aceleración y desaceleración en x . La velocidad angular ωz=0 en testbed permanece contante a cero en todo momento puesto que no estaba activado el control de yaw en este experimento. Figura 4.8 Velocidades comandadas en el tiempo para la Trayectoria 1 a 0.5m/s. Velocidad constante 1m/s En Fig. 4.9 se muestra la trayectoria en línea recta a una velocidad alta, el control de yaw está desactivado en el testbed por lo que ωz=0en todo momento. Figura 4.9 Trayectoria 1 en simulación y en testbed a 1m/s. 38 Capítulo 4. Resultados En las gráficas de Fig. 4.10 se puede ver como pese a duplicar la velocidad, el seguimiento sigue siendo bueno en ambas situaciones, algo mejor en el testbed por el mejor posicionamiento. Figura 4.10 Posición y referencia en el tiempo para la Trayectoria 1 a 1m/s. En la Fig. 4.11 podemos ver errores algo mayores en x en la fase de aceleración que en el experimento anterior, concretamente en el testbed al duplicar la velocidad en x se ha duplicado su error máximo al comienzo. Tras finalizar la fase de aceleración inicial el error en xy queda prácticamente nulo. En el resto de componentes el error es muy similar, sin apreciarse grandes cambios más allá de un leve aumento del error en y. Figura 4.11 Errores de posición y yaw en el tiempo para la Trayectoria 1 a 1m/s. 4.2 Resultados obtenidos 39 En la Fig. 4.12 también podemos apreciar como la fase de aceleración es más brusca y se estabiliza en la velocidad deseada de 1m/s. Figura 4.12 Velocidades comandadas en el tiempo para la Trayectoria 1 a 1m/s. Velocidad cambiante 0.25m/s→1m/s→0.5m/s→0.33m/s Trayectoria en línea recta a una velocidad variable (Fig. 4.13), el control de yaw está activado por lo que ωz se calculará automáticamente. En este experimento se pretende probar el correcto funcionamiento ante trayectorias de con velocidades variables, de forma que el look −ahead sea auto calculado. Además, se quiere observar el comportamiento del control de yaw ante los falsos errores de orientación que arroja VICON. Figura 4.13 Trayectoria 1 en simulación y en testbed a 0.25m/s→1m/s→0.5m/s→0.33m/s. 46 Capítulo 4. Resultados Velocidad cambiante 0.33m/s→1m/s→0.5m/s→0.25m/s Por último, se quiere probar el funcionamiento del seguidor ante esta trayectoria con velocidades cambiantes, de forma que se calcule el look −ahead . Las trayectorias descritas en Fig. 4.26 se puede ver que son similares a las anteriores. Figura 4.26 Trayectoria 2 en simulación y en testbed a 0.33m/s→1m/s→0.5m/s→0.25m/s. En la Fig. 4.27 se puede comprobar que el seguimiento en posición es muy preciso en testbed, no obstante también realiza un seguimiento correcto en simulación. Figura 4.27 Posición y referencia en el tiempo para la Trayectoria 2 a 0.33m/s→1m/s→0.5m/s→ 0.25m/s. 4.2 Resultados obtenidos 47 En las gráficas de Fig. 4.28 se aprecia que el control en posición con la velocidad cambiantes funciona adecuadamente en el testbed, no presentando errores superiores a los 10cm en posición. Respecto al control de yaw en simulación, el error de orientación no supera los 15º. Figura 4.28 Errores de posición y yaw en el tiempo para la Trayectoria 2 a 0.33m/s→1m/s→ 0.5m/s→0.25m/s. En la Fig. 4.29 vemos los comandos de velocidad con cambios suaves en los instantes de cambio de la velocidad de referencia. Las formas descritas tanto en simulación como en testbed son similares. Figura 4.29 Velocidades comandadas en el tiempo para la Trayectoria 2 a 0.33m/s→1m/s→ 0.5m/s→0.25m/s. 48 Capítulo 4. Resultados 4.2.3 Trayectoria 3. Forma de ocho Esta trayectoria, a diferencia de la anterior, se ha centrado en una zona desplazada del centrol del testbed donde el posicionamiento VICON parece funcionar algo mejor para poder hacer un test con seguimiento de yaw. Velocidad constante 0.33m/s Trayectoria en forma de ocho realizada a una velocidad baja (Fig. 4.30), el control de yaw está desactivado en el testbed por lo que ωz=0 en todo momento. Podemos ver como realiza un mejor seguimiento en posición en el testbed que en la simulación a causa del GPS. Figura 4.30 Trayectoria 3 en simulación y en testbed a 0.33m/s. El seguimiento de la trayectoria es bueno en ambos casos, como se aprecia en la Fig. 4.31. Figura 4.31 Posición y referencia en el tiempo para la Trayectoria 2 a 0.33m/s. 4.2 Resultados obtenidos 49 En la Fig. 4.32 vemos un menor error en xy en el testbed, sin llegar a ser superior de 20cm en la simulación. Esto se debe a que se trata de una trayectoria con giros en todo momento y reducidas dimensiones, por lo que el posicionamiento por GPS no responde a tiempo. Igualmente, pese a todos los giros que realiza en tan poco espacio, el error de yaw en simulación no supera los 15º. Figura 4.32 Errores de posición y yaw en el tiempo para la Trayectoria 2 a 0.33m/s. En la Fig. 4.33 vemos como los comandos de velocidad son suaves en todo momento sin llegar a saturar en ningún punto y presentan geometrías similares en simulación y en testbed. El control de yaw en simulación mantiene constante la velocidad angular en los momentos en los que se realizan giros a velocidad y radio de giro constantes. Figura 4.33 Velocidades comandadas en el tiempo para la Trayectoria 2 a 0.33m/s. 50 Capítulo 4. Resultados Velocidad constante 0.5m/s Trayectoria en forma de ocho realizada a una velocidad media, el control de yaw está desactivado en el testbed por lo que ωz=0 en todo momento. En la trayectoria de la figura 4.34 se observa Figura 4.34 Trayectoria 3 en simulación y en testbed a 0.5m/s. como, al aumentar la velocidad, la trayectoria descrita en el testbed se realiza por la parte interna de la referencia. Al tener mayor velocidad que en la Fig. 4.30 el look −ahead es mayor y "recorta" la ruta. Figura 4.35 Posición y referencia en el tiempo para la Trayectoria 3 a 0.5m/s. 4.2 Resultados obtenidos 51 Figura 4.36 Errores de posición y yaw en el tiempo para la Trayectoria 3 a 0.5m/s. Puede verse en las Fig. 4.36 y Fig. 4.37 que al aumentar la velocidad el comportamiento empeora en el testbed. Esto se debe a que el look −ahead aumenta y el control intenta alcanzar puntos más lejanos, pero al tratarse de una trayectoria muy reducida y en giro constante, el resultado es que se acorta la trayectoria y no se sigue igual de bien. Figura 4.37 Velocidades comandadas en el tiempo para la Trayectoria 3 a 0.5m/s. 52 Capítulo 4. Resultados Velocidad constante 0.2m/s Trayectoria en forma de ocho realizada a una velocidad baja (Fig. 4.38), el control de yaw está activado en el testbed por lo que ωz se calculará en todo momento. El resultado en posición es muy similar al primer experimento de esta trayectoria. Figura 4.38 Trayectoria 3 en simulación y en testbed a 0.2m/s. En la Fig. 4.39 puede observarse la estela de la trayectoria descrita por el crazyflie en el testbed, tanto desde una esquina del testbed como desde abajo. Figura 4.39 Trayectoria 3 en testbed a 0.2m/s. 4.2 Resultados obtenidos 53 Figura 4.40 Posicion y referencia en el tiempo para la Trayectoria 3 a 0.2m/s. En las Fig. 4.40 y Fig. 4.41 podemos ver, en línea con los resultados expuesto anteriormente, que el vuelo en testbed obtiene mejor resultado en xy , con errores inferiores a los 5cm, mientras que en simulación sobrepasan los 20cm en algún instante. No obstante, en simulación las correcciones de yaw son más suaves y no presentan el ruido introducido por VICON. A pesar de ello el error máximo en yaw en el testbed se mantiene sobre los 20ºy realiza en general un buen seguimiento. Figura 4.41 Errores de posición y yaw en el tiempo para la Trayectoria 3 a 0.2m/s. 54 Capítulo 4. Resultados Los comandos de velocidades lineales en Fig. 4.42 son suaves y similares. En cambio, se vuelve a repetir que las velocidades angulares comandadas en testbed saturan y oscilan bruscamente. Figura 4.42 Velocidades comandadas en el tiempo para la Trayectoria 3 a 0.2m/s. 4.2.4 Trayectoria 4. Hipódromo + ocho Se pretende extender la complejidad de los experimentos de la trayectoria anterior añadiéndole a la trayectoria una zona con forma de hipódromo que abarque más área del testbed. Velocidad constante 0.5m/s Trayectoria en forma de ocho + hipódromo realizada a una velocidad media (Fig. 4.43), el control de yaw está desactivado en el testbed por lo que ωz=0en todo momento. Figura 4.43 Trayectoria 4 en simulación y en testbed a 0.5m/s. 4.2 Resultados obtenidos 55 Por lo general, puede verse en la Fig. 4.44 como el seguimiento de posición en simulación y testbed es bueno en ambos casos. Las desviaciones que se aprecian en el seguimiento de yaw, se deben a que los águnlos están representados entre [-180,180), realmente se está efectuando el seguimiento de forma correcta. Figura 4.44 Posición y referencia en el tiempo para la Trayectoria 4 a 0.5m/s. En las gráficas de la Fig. 4.45 se observan zonas en las que se alcanza un error algo superior en la simulación. El error en yaw en simulación alcanza picos de unos 20ºcuando se producen giros más agudos, pero en general realiza un buen seguimiento. Figura 4.45 Errores de posición y yaw en el tiempo para la Trayectoria 4 a 0.5m/s. 62 Capítulo 4. Resultados En la Fig. 4.57, seguimiento de la trayectoria 5D se ve que es correcto en la simulación. Por otro lado, en el testbed, se puede apreciar como hay zonas en las que se hace un correcto seguimiento y otras en las que el seguimiento no funciona por la problemática de VICON. Igualmente, estos datos se reflejan en los errores mostrados en Fig. 4.58. Figura 4.58 Errores de posición y yaw en el tiempo para la Trayectoria 5 a 0.2m/s. Respecto a los comandos de velocidad en Fig. 4.59, son los esperados en simulación. En el vuelo en testbed, se aprecia como se aplican grandes picos de velocidad para corregir los errores de posicionamiento derivados del problema de posicionamiento del yaw lo antes posible. Figura 4.59 Velocidades comandadas en el tiempo para la Trayectoria 5 a 0.2m/s. 4.2 Resultados obtenidos 63 Velocidad constante 0.2m/s(modificada) Para intentar realizar la trayectoria anterior sin la incidencia de VICON, se ha modificado el yaw durante la ejecución. En este nuevo experimento (Fig. 4.60), cuando el crazyflie se encuentre en la mitad derecha del testbed, ajustará el yaw de forma que el UAS esté orientado hacia la dirección de avance; cuando se encuentre en la mitad derecha, el UAS ajustará el yaw para orientarse hacia atrás. De esta forma, se consigue que el marcador del brazo quede siempre en orientaciones mas favorables para ser detectado. Figura 4.60 Trayectoria 5 (modificada) en simulación y en testbed a 0.2m/s. En la Fig. 4.61 puede verse la estela de la trayectoria descrita por el crazyflie en el testbed para este experimento. Se incluye una imagen con una toma desde la esquina superior del testbed y otra desde el suelo, algo deformada por ser tomada con una cámara con gran angular. Figura 4.61 Trayectoria 5 (modificada) en testbed a 0.2m/s. 64 Capítulo 4. Resultados Ahora podemos apreciar como no llega a perderse el marcador de posicionamiento de VICON en ningún instante. De un instante a otro se introduce un giro de yaw de 180º, de ahí el error en yaw que se observa en Fig. 4.63, el cual es corregido suavemente hasta alcanzar la nueva orientación deseada. En las gráficas de Fig. 4.62 se ve como el tiempo que tarda en adaptarse al la nueva orientación en simulación es más rápido, no obstante, se hace de forma suave en ambos. Las velociades comandadas en Fig. 4.64 son las esperadas. Figura 4.62 Posición y referencia en el tiempo para la Trayectoria 5 a 0.2m/s. Figura 4.63 Errores de posición y yaw en el tiempo para la Trayectoria 5 a 0.2m/s. 4.2 Resultados obtenidos 65 Figura 4.64 Velocidades comandadas en el tiempo para la Trayectoria 5 a 0.2m/s. 66 Capítulo 4. Resultados 4.3 Simulaciones adicionales Para validar el correcto funcionamiento del resto de modelos de UAS descritos en Fig. 2.4.3, así como la versatilidad del seguidor 5D (objeto de este trabajo) ante otras plataformas, se van a realizar algunas simulaciones adicionales. En estas simulaciones se utilizará la trayectoria 6. 4.3.1 DJI F-450 En la captura de Fig. 4.65 puede verse una simulación en gazebo y la trayectoria descrita en rviz por el UAS DJI F-450. Figura 4.65 Simulación en Gazebo y Rviz con F-450. Velocidad constante 0.4m/s Figura 4.66 F-450: Trayectoria 6 en simulación a 0.2m/s. 4.3 Simulaciones adicionales 67 En la Fig. 4.66 puede verse la trayectoria descrita respecto a la referencia. En las gráficas de Fig. 4.67, Fig. 4.68 y Fig. 4.69 se muestran los resultados de la simulación. Figura 4.67 F-450: Posición y referencia en el tiempo para la Trayectoria 6 a 0.2m/s. Figura 4.68 F-450: Errores de posición y yaw en el tiempo para la Trayectoria 6 a 0.2m/s. Figura 4.69 F-450: Velocidades comandadas en el tiempo para la Trayectoria 6 a 0.2m/s. 68 Capítulo 4. Resultados velocidad cambiante de 0.5m/s→0.3m/s→0.75m/s Figura 4.70 F-450: Trayectoria 6 en simulación a 0.5m/s→0.3m/s→0.75m/s. En la Fig. 4.70 puede verse la trayectoria descrita respecto a la referencia. En las gráficas de Fig. 4.71, Fig. 4.72 y Fig. 4.73 se muestran los resultados de la simulación. Figura 4.71 F-450: Posición y referencia en el tiempo para la Trayectoria 6 a 0.5m/s→0.3m/s→ 0.75m/s. 4.3 Simulaciones adicionales 69 Figura 4.72 F-450: Errores de posición y yaw en el tiempo para la Trayectoria 6 0.5m/s→0.3m/s→ 0.75m/s. Figura 4.73 F-450: Velocidades comandadas en el tiempo para la Trayectoria 6 a 0.5m/s→ 0.3m/s→0.75m/s. 70 Capítulo 4. Resultados 4.3.2 DJI F-550 En la captura de Fig. 4.74 puede verse una simulación en gazebo y la trayectoria descrita en rviz por el UAS DJI F-550. Figura 4.74 Simulación en Gazebo y Rviz con F-550. Velocidad constante 0.4m/s Figura 4.75 F-550: Trayectoria 6 en simulación a 0.2m/s. En la Fig. 4.75 puede verse la trayectoria descrita respecto a la referencia. En las gráficas de Fig. 4.76, Fig. 4.77 y Fig. 4.78 se muestran los resultados de la simulación. 4.3 Simulaciones adicionales 71 Figura 4.76 F-550: Posición y referencia en el tiempo para la Trayectoria 6 a 0.2m/s. Figura 4.77 F-550: Errores de posición y yaw en el tiempo para la Trayectoria 6 0.2m/s. Figura 4.78 F-550: Velocidades comandadas en el tiempo para la Trayectoria 6 a 0.2m/s. 5 Conclusiones y líneas futuras En este último capítulo se expondrán las conclusiones generales que se pueden extraer de este proyecto, así como las líneas de trabajo futuras para la mejora del mismo. 5.1 Conclusiones generales El uso de los UASs va a continuar incrementándose en las próximas décadas, siendo la gestión del VLL Airspace mucho más compleja. En este contexto el concepto de la 4D-TBO es esencial para incrementar la seguridad y maximizar el espacio aéreo compartido. En este proyecto se ha querido ir más allá en está visión y ampliar una dimensión más añadiendo el yaw del UASs dada su importancia. Para que los distintos UASs puedan ejecutar con éxito estas 5D-TBO, es esencial disponer de un sistema de seguimiento de trayectorias 5D preciso y fiable. La principal aportación del proyecto ha sido desarrollar un seguidor de trayectorias 5D, con el objetivo de que sea apto para cualquier tipo de UAS, independientemente del tipo de configuración de rotores o ala que presente. Además, se ha pretendido que sea lo más autónomo posible, eliminando la necesidad de que el usuario tenga que asignar parámetros de control como la distancia look −ahead de forma obligatoria. El seguidor ha sido probado y validado tanto en simulación como en vuelo real, siguiendo una metodología de trabajo que buscaba priorizar la seguridad de la plataforma. Para ello ha resultado de gran ayuda la capa de abstracción UAL, la cual ha simplificado notablemente el desarrollo y ha ofrecido simulaciones muy fieles a la realidad. Estas simulaciones han dejado a su paso otra aportación derivada del proyecto, como son los distintos modelos del testbed y de los distintos UASs mencionados. Durante la etapa de validación en testbed, se han conseguido identificar los problemas ocasionados por VICON, adaptando los experimentos ante las circunstancias para poder realizarlos dentro de las condiciones dadas. Sin duda ha sido uno de los mayores inconvenientes durante los vuelos. Por otro lado, se ha podido comprobar que el modelo de la dinámica del yaw del Crazyflie 2.1 es difícil de modelar a causa de las pequeñas dimensiones de sus hélices y la flexibilidad de las mismas, sumado a las reducidas dimensiones y masas del mismo. Esto ha hecho que nos encontráramos ante comportamientos de yaw muy distinto entre las simulaciones y los vuelos en testbed. No obstante, se ha comprobado que el seguimiento de yaw ha funcionado muy bien en simulación. En el caso de vuelos reales en testbed, cuando las condiciones han permitido activar el control de yaw de forma segura, los errores de yaw se han mantenido normalmente por debajo de los 20º, lo cual es un valor admisible teniendo en cuenta el ruido y error introducidos por la falta de precisión en obtener la orientación del crazyflie. El hecho de haber estado actuando sobre el yaw del UAS no ha repercutido negativamente sobre el seguimiento en posición xyz de la trayectoria, puesto que ha seguido siendo muy preciso, siéndolo más aún si cabe en el testbed. Esta mayor precisión se 79 80 Capítulo 5. Conclusiones y líneas futuras debe a que VICON ofrece una posición más fiable y prácticamente en tiempo real, ha diferencia del sistema GPS utilizado en las simulaciones. 5.2 Trabajos futuros A lo largo del proyecto han surgido una serie de inconvenientes, los cuales no han sido atajados de raíz por el escaso tiempo disponible cuando se inició la fase de vuelos en testbed; por no ser esenciales para el desarrollo y validación del seguidor 5D, se adaptaron soluciones más rápidas. Uno de estos problemas ha sido la imposibilidad de poder utilizar los servicios del CrazyflieDriver de forma segura. Ya que no se pueden emplear los las llamadas a servicios del CrazyflieDriver, sería de gran utilidad desarrollar un back-end que desempeñe los servicios esenciales de un UAS (despegue, aterrizaje, ir a un punto, etc) a partir de comandos de velocidad. De esta forma, quedaría una interfaz mucho más cómoda para que el usuario realice los distintos experimentos. Por otro lado, sería interesante profundizar más en el modelo del crazyflie 2.1, en concreto realizar un ajuste más preciso de ciertos valores como la motor_constant yrotor_drag_coe f ficient. Una de las mayores tareas pendiente de este proyecto, la cual no pudo realizarse por finalización del periodo de prácticas en CATEC, es el probar el seguidor 5D en otras plataformas. Sería interesante hacer pruebas de vuelo similares a las realizadas con otros UASs de mayor tamaño como pueden ser los F-450 o F-550 de DJI. En estos UASs sería posible colocar un mayor número de balizas de posicionamiento para el sistema VICON sin ningún problema de limitación de espacio y se podría comprobar mejor el correcto funcionamiento de todo el sistema sin la presencia de problemas a la hora de obtener la orientación del UAS. Además de validar el seguidor 5D en otros quad rotor, resultaría de gran interés poder hacer pruebas en plataformas con otro tipo de configuración como pueden ser UAS de ala fija. El seguidor fue desarrollado con el objetivo de ser apto para cualquier tipo de UAS por lo que esta validación resultaría de gran valor. Apéndice A Modelos URDF y SDF Código A.1 Modelo SDF del Testbed. <?xml version="1.0" ?> <sdf version="1.5"> <model name="testbed"> <static>true</static> <link name="testbed"> <pose>0 0 0 0 0 0</pose> <collision name="collision"> <geometry> <mesh> <uri>model://testbed/meshes/testbed.dae</uri> <scale>1 1 1</scale> </mesh> </geometry> </collision> <visual name="visual"> <geometry> <mesh> <uri>model://testbed/meshes/testbed.dae</uri> <scale>1 1 1</scale> </mesh> </geometry> </visual> </link> </model> </sdf> 81 82 Appendix A. Modelos URDF y SDF Código A.2 Modelo URDF de la geometría del Crazyflie 2.1. <?xml version="1.0"?> <robot name="cf" xmlns:xacro="http://ros.org/wiki/xacro"> <!-- Properties --> <xacro:property name="model_name" value="cf" /> <xacro:property name="description_package" value=" usa_gazebo_simulation" /> <xacro:property name="namespace" value="" /> <xacro:property name="rotor_velocity_slowdown_sim" value="10" /> <xacro:property name="mesh_file" value="multirotor.dae" /> <xacro:property name="mesh_scale" value="1 1 -1"/> <xacro:property name="mesh_scale_prop" value="1 1 1"/> <xacro:property name="mass" value="0.0316" /> <!-- [kg] --> <xacro:property name="body_width" value="0.092" /> <!-- [m] --> <xacro:property name="body_height" value="0.029" /> <!-- [m] --> <xacro:property name="mass_rotor" value="0.0003" /> <!-- [kg] --> <xacro:property name="rotor_offset_top" value="0.0" /> <!-- [m] --> <xacro:property name="radius_rotor" value="0.022" /> <!-- [m] --> <xacro:property name="motor_constant" value="5.0e-07" /> <!-- [kg.m/s ^2] --> <xacro:property name="moment_constant" value="0.016" /> <!-- [m] --> <xacro:property name="time_constant_up" value="0.0125" /> <!-- [s] --> <xacro:property name="time_constant_down" value="0.025" /> <!-- [s]--> <xacro:property name="max_rot_velocity" value="1100" /><!--[rad/s]--> <xacro:property name="sin30" value="0.5" /> <xacro:property name="cos30" value="0.866025403784" /> <xacro:property name="sqrt2" value="1.4142135623730951" /> <xacro:property name="rotor_drag_coefficient" value="8.06428e-06" /> <xacro:property name="rolling_moment_coefficient" value="0.000001" /> <xacro:property name="color" value="$(arg visual_material)" /> <xacro:property name="arm_length" value="0.046" /> <!-- [m] --> <xacro:property name="arm_height" value="0.025" /> <!-- [m] --> <!-- Inertia constants {--> <xacro:property name="inertia_body_radius" value="0.20" /> <!-- [m]--> <xacro:property name="inertia_body_height" value="0.05" /> <!-- [m]--> <!--}--> <!-- Property Blocks --> <xacro:property name="body_inertia"> <inertia ixx="${mass * (3 * inertia_body_radius * inertia_body_radius + inertia_body_height * inertia_body_height) / 12}" iyy="${mass * (3 * inertia_body_radius * inertia_body_radius + inertia_body_height * inertia_body_height) / 12}" izz="${mass * inertia_body_radius * inertia_body_radius / 2}" ixy="0" ixz="0" iyz="0"/> 83 </xacro:property> <!-- inertia of a single rotor, assuming it is a cuboid. Height=3mm, width=15mm --> <xacro:property name="rotor_inertia"> <inertia ixx="${1/12 * mass_rotor * (0.015 * 0.015 + 0.003 * 0.003) * rotor_velocity_slowdown_sim}" iyy="${1/12 * mass_rotor * (4 * radius_rotor * radius_rotor + 0.003 * 0.003) * rotor_velocity_slowdown_sim}" izz="${1/12 * mass_rotor * (4 * radius_rotor * radius_rotor + 0.015 * 0.015) * rotor_velocity_slowdown_sim}" ixy="0.0" ixz="0.0" iyz="0.0" /> <!-- [kg.m^2] [kg.m^2] [kg.m^2] [kg. m^2] [kg.m^2] [kg.m^2] --> </xacro:property> <!-- Included URDF Files --> <xacro:include filename="$(find robots_description)/urdf/ multirotor_base.xacro" /> <xacro:include filename="$(find robots_description)/urdf/ component_snippets.xacro" /> <!-- Instantiate multirotor_base_macro once --> <xacro:multirotor_base_macro model_name="${model_name}" description_package="${description_package}" robot_namespace="${namespace}" mass="${mass}" body_width="${body_width}" body_height="${body_height}" mesh_file="${mesh_file}" mesh_scale="${mesh_scale}" color="DarkGrey" > <xacro:insert_block name="body_inertia" /> </xacro:multirotor_base_macro> <!-- Instantiate rotors --> <xacro:vertical_rotor robot_namespace="${namespace}" description_package="${description_package}" suffix="front_right" direction="ccw" motor_constant="${motor_constant}" moment_constant="${moment_constant}" parent="base_link" mass_rotor="${mass_rotor}" radius_rotor="${radius_rotor}" time_constant_up="${time_constant_up}" time_constant_down="${time_constant_down}" 84 Appendix A. Modelos URDF y SDF max_rot_velocity="${max_rot_velocity}" motor_number="0" rotor_drag_coefficient="${rotor_drag_coefficient}" rolling_moment_coefficient="${rolling_moment_coefficient}" mesh="prop" mesh_scale="${mesh_scale_prop}" color="Red"> <origin xyz="0.0315 -0.0315 -0.0035" rpy="0 0 0" /> <xacro:insert_block name="rotor_inertia" /> </xacro:vertical_rotor> <xacro:vertical_rotor robot_namespace="${namespace}" description_package="${description_package}" suffix="back_left" direction="ccw" motor_constant="${motor_constant}" moment_constant="${moment_constant}" parent="base_link" mass_rotor="${mass_rotor}" radius_rotor="${radius_rotor}" time_constant_up="${time_constant_up}" time_constant_down="${time_constant_down}" max_rot_velocity="${max_rot_velocity}" motor_number="1" rotor_drag_coefficient="${rotor_drag_coefficient}" rolling_moment_coefficient="${rolling_moment_coefficient}" mesh="prop" mesh_scale="${mesh_scale_prop}" color="White"> <origin xyz="-0.0315 0.0315 -0.0035" rpy="0 0 0" /> <xacro:insert_block name="rotor_inertia" /> </xacro:vertical_rotor> <xacro:vertical_rotor robot_namespace="${namespace}" description_package="${description_package}" suffix="fron_left" direction="cw" motor_constant="${motor_constant}" moment_constant="${moment_constant}" parent="base_link" mass_rotor="${mass_rotor}" radius_rotor="${radius_rotor}" time_constant_up="${time_constant_up}" time_constant_down="${time_constant_down}" max_rot_velocity="${max_rot_velocity}" motor_number="2" rotor_drag_coefficient="${rotor_drag_coefficient}" rolling_moment_coefficient="${rolling_moment_coefficient}" mesh="prop" 85 mesh_scale="${mesh_scale_prop}" color="Red"> <origin xyz="0.0315 0.0315 -0.0035" rpy="0 0 0" /> <xacro:insert_block name="rotor_inertia" /> </xacro:vertical_rotor> <xacro:vertical_rotor robot_namespace="${namespace}" description_package="${description_package}" suffix="back_right" direction="cw" motor_constant="${motor_constant}" moment_constant="${moment_constant}" parent="base_link" mass_rotor="${mass_rotor}" radius_rotor="${radius_rotor}" time_constant_up="${time_constant_up}" time_constant_down="${time_constant_down}" max_rot_velocity="${max_rot_velocity}" motor_number="3" rotor_drag_coefficient="${rotor_drag_coefficient}" rolling_moment_coefficient="${rolling_moment_coefficient}" mesh="prop" mesh_scale="${mesh_scale_prop}" color="White"> <origin xyz="-0.0315 -0.0315 -0.0035" rpy="0 0 0" /> <xacro:insert_block name="rotor_inertia" /> </xacro:vertical_rotor> <gazebo> <plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so"> <alwaysOn>true</alwaysOn> <updateRate>50.0</updateRate> <bodyName>base_link</bodyName> <topicName>ground_truth/state</topicName> <gaussianNoise>0.01</gaussianNoise> <frameName>world</frameName> <xyzOffsets>0 0 0</xyzOffsets> <rpyOffsets>0 0 0</rpyOffsets> </plugin> </gazebo> </robot> 86 Appendix A. Modelos URDF y SDF Código A.3 Modelo URDF del Crazyflie 2.1. <?xml version="1.0"?> <robot name="cf" xmlns:xacro="http://ros.org/wiki/xacro"> <!-- Properties that can be assigned at build time as arguments. Is there a reason not to make all properties arguments? --> <xacro:arg name=’mavlink_addr’ default=’INADDR_ANY’ /> <xacro:arg name=’mavlink_tcp_port’ default=’4560’ /> <xacro:arg name=’mavlink_udp_port’ default=’14560’ /> <xacro:arg name=’visual_material’ default=’DarkGrey’ /> <xacro:arg name=’enable_mavlink_interface’ default=’true’ /> <xacro:arg name=’enable_gps_plugin’ default=’true’ /> <xacro:arg name=’gps_noise’ default=’true’ /> <xacro:arg name=’enable_ground_truth’ default=’false’ /> <xacro:arg name=’enable_logging’ default=’false’ /> <!-- macros for gazebo plugins, sensors --> <xacro:include filename="$(find robots_description)/urdf/ component_snippets.xacro" /> <!-- Instantiate cf "mechanics" --> <xacro:include filename="$(find usa_gazebo_simulation)/models/cf/ geometry.xacro" /> <xacro:if value="$(arg enable_gps_plugin)"> <!-- Instantiate gps plugin. --> <xacro:gps_plugin_macro namespace="${namespace}" gps_noise="$(arg gps_noise)" > </xacro:gps_plugin_macro> </xacro:if> <xacro:if value="$(arg enable_mavlink_interface)"> <!-- Instantiate mavlink telemetry interface. --> <xacro:mavlink_interface_macro namespace="${namespace}" imu_sub_topic="/imu" mavlink_addr="$(arg mavlink_addr)" mavlink_tcp_port="$(arg mavlink_tcp_port)" mavlink_udp_port="$(arg mavlink_udp_port)" > </xacro:mavlink_interface_macro> </xacro:if> <!-- Mount an ADIS16448 IMU. --> <xacro:imu_plugin_macro namespace="${namespace}" 87 imu_suffix="" parent_link="base_link" imu_topic="/imu" mass_imu_sensor="0.015" gyroscope_noise_density="0.00018665" gyroscopoe_random_walk="0.000038785" gyroscope_bias_correlation_time="1000.0" gyroscope_turn_on_bias_sigma="0.0087" accelerometer_noise_density="0.00186" accelerometer_random_walk="0.006" accelerometer_bias_correlation_time="300.0" accelerometer_turn_on_bias_sigma="0.1960" > <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" /> <origin xyz="0 0 0" rpy="0 0 0" /> </xacro:imu_plugin_macro> <xacro:if value="$(arg enable_ground_truth)"> <!-- Mount an IMU providing ground truth.--> <xacro:imu_plugin_macro namespace="${namespace}" imu_suffix="gt" parent_link="base_link" imu_topic="ground_truth/imu" mass_imu_sensor="0.00001" gyroscope_noise_density="0.0" gyroscopoe_random_walk="0.0" gyroscope_bias_correlation_time="1000.0" gyroscope_turn_on_bias_sigma="0.0" accelerometer_noise_density="0.0" accelerometer_random_walk="0.0" accelerometer_bias_correlation_time="300.0" accelerometer_turn_on_bias_sigma="0.0"> <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" /> <origin xyz="0 0 0" rpy="0 0 0" /> </xacro:imu_plugin_macro> <!-- Mount a generic odometry sensor providing ground truth. --> <xacro:odometry_plugin_macro namespace="${namespace}/ground_truth" odometry_sensor_suffix="gt" parent_link="base_link" pose_topic="pose" pose_with_covariance_topic="pose_with_covariance" position_topic="position" transform_topic="transform" odometry_topic="odometry" parent_frame_id="world" 94 Apéndice B. Código Follower 5D double distance = (target_p - _current_point).norm(); double angle1 = (target_o - _current_orientation); if (abs(angle1) > 3.141592865358979) { if(angle1 > 0) { angle = target_o - _current_orientation - twoPi; } else { angle = target_o - _current_orientation + twoPi; } } else { angle=angle1; } double dist_target_projection = (target_p - projection_p).norm(); double mod_vel, ang_vel; unit_vec = (target_p - _current_point) / distance; unit_vec = unit_vec / unit_vec.norm(); mod_vel = (dist_target_projection) / ((generated_times_[ _pos_look_ahead] - actual_time_)); ang_vel = (angle) / ((generated_times_[_pos_look_ahead] - actual_time_)); if (ang_vel > 3) ang_vel = 3; if (ang_vel < -3) ang_vel = -3; if (mod_vel > max_vel_) mod_vel = max_vel_; if (mod_vel < 0) mod_vel = max_vel_; out_vel.twist.linear.x = unit_vec(0) * mod_vel; out_vel.twist.linear.y = unit_vec(1) * mod_vel; out_vel.twist.linear.z = unit_vec(2) * mod_vel; out_vel.twist.angular.z = ang_vel; out_vel.header.frame_id = target_path_.header.frame_id; return out_vel; } Apéndice C Código misión en testbed Código C.1 Despegue y aterrizaje mediante comandos de velocidad. switch (estado){ case 1: if (cur_pose_.pose.position.z<1.49){ output_vel.vel.x = 0.0; output_vel.vel.y = 0.0; output_vel.yawRate = 0.0; output_vel.vel.z = (1.5-cur_pose_.pose.position.z)*0.5; if (output_vel.vel.z<0.1) output_vel.vel.z=0.1; if (output_vel.vel.z>0.5) output_vel.vel.z=0.5; pub_set_velocity_.publish(output_vel); }else{ ROS_INFO("ESTOY ARRIBA"); output_vel.vel.z = 0.0; pub_set_velocity_.publish(output_vel); ROS_INFO("VUELO AUTOMATICO"); estado = 2;} break; case 2: if (client_execute_trajectory_.call(execute_trajectory)) { ROS_INFO("ATERRIZAJE"); estado = 3; } break; case 3: if (cur_pose_.pose.position.z>0.1){ output_vel.vel.x = 0.0; output_vel.vel.y = 0.0; output_vel.yawRate = 0.0; output_vel.vel.z = (cur_pose_.pose.position.z)*0.5; 95 96 Apéndice C. Código misión en testbed if (output_vel.vel.z>-0.1) output_vel.vel.z=-0.1; if (output_vel.vel.z<-0.5) output_vel.vel.z=-0.5; pub_set_velocity_.publish(output_vel); }else{ ROS_INFO("ESTOY EN TIERRA"); output_vel.vel.z = 0.0; pub_set_velocity_.publish(output_vel); ROS_INFO("FIN"); estado = 4;} break; case 4: break; } Índice de Figuras 1.1 Aplicaciones de los UAS 1 1.2 SESAR U-Space logo 2 1.3 Despliegue progresivo del U-Space 3 1.4 Vuelos de prueba tripulados 4D-TBO 3 1.5 Roll, Pitch y Yaw en UASs de ala fija y multirrotores 4 1.6 Área de visión del sensor de un UAS en función de la altura de vuelo halt , la elevación del gímbal θg, el campo de visión horizontal ηhy el campo de visión vertical ηv. Extraido de [28] 5 1.7 Diferentes tipos de UAS en función de la posición de sus rotores y configuraciones de ala que presenten 6 2.1 Metodología empleada en el proyecto 9 2.2 Testbed de CATEC 10 2.3 Camaras VICON T-40 en el testbed y marcadores retrorreflectores 10 2.4 Crazyflie 2.1 y batería LiPo 11 2.5 Arquitectura del Crazyflie 2.1 12 2.6 Crazyradio PA 13 2.7 Esquema de las diferentes capas para implementar el back-end en UAL. Extraido de [41] 14 2.8 Doble interfaz de UAL con el usuario, como clase o como llamadas se servicios de ROS. Extraido de [41] 14 2.9 Esquema con los back-ends disponibles para UAL. Extraido de [41] 15 2.10 Logo de Gazebo 15 2.11 Testbed en Gazebo 16 2.12 Peso exacto del Crazyflie 2.1 y las hélices utilizadas 17 2.13 Crazyflie 2.1 en Gazebo 17 2.14 DJI F-450 con y sin tren de aterrizaje en la realidad y en Gazebo 18 2.15 DJI F-550 en la realidad y en Gazebo 18 2.16 DJI Matrice 600 en la realidad y en Gazebo 19 2.17 CATEC 750 en la realidad y en Gazebo 19 3.1 Arquitectura del framework 4D Follower. Extraido de [63] 22 3.2 Vista cenital del seguidor de trayectorias basado en el algoritmo carrot chasing sin tener en cuenta el error en orientación. Extraido de [63] 22 3.3 Arquitectura del framework 5D Follower 26 3.4 Comportamiento del seguimiento en función de la distancia look-ahead 27 4.1 Trayectorias 1 y 2 33 4.2 Trayectorias 3 y 4 33 97 98 Índice de Figuras 4.3 Trayectorias 5 y 6 34 4.4 Trayectoria 1 en simulación y en testbed a 0.5m/s35 4.5 Trayectoria 1 descrita por el Crazyflie en testbed a 0.5m/s35 4.6 Posición y referencia en el tiempo para la Trayectoria 1 a 0.5m/s36 4.7 Errores de posición y yaw en el tiempo para la Trayectoria 1 a 0.5m/s36 4.8 Velocidades comandadas en el tiempo para la Trayectoria 1 a 0.5m/s37 4.9 Trayectoria 1 en simulación y en testbed a 1m/s37 4.10 Posición y referencia en el tiempo para la Trayectoria 1 a 1m/s38 4.11 Errores de posición y yaw en el tiempo para la Trayectoria 1 a 1m/s38 4.12 Velocidades comandadas en el tiempo para la Trayectoria 1 a 1m/s39 4.13 Trayectoria 1 en simulación y en testbed a 0.25m/s→1m/s→0.5m/s→0.33m/s39 4.14 Posición y referencia en el tiempo para la Trayectoria 1 a 0.25m/s→1m/s→0.5m/s→ 0.33m/s40 4.15 Errores de posición y yaw en el tiempo para la Trayectoria 1 a 0.25m/s→1m/s→ 0.5m/s→0.33m/s40 4.16 Velocidades comandadas en el tiempo para la Trayectoria 1 a 0.25m/s→1m/s→ 0.5m/s→0.33m/s41 4.17 Trayectoria 2 en simulación y en testbed a 0.5m/s42 4.18 Trayectoria 2 descrita por el Crazyflie en testbed a 0.5m/s42 4.19 Posición y referencia en el tiempo para la Trayectoria 2 a 0.5m/s43 4.20 Errores de posición y yaw en el tiempo para la Trayectoria 2 a 0.5m/s43 4.21 Velocidades comandadas en el tiempo para la Trayectoria 2 a 0.5m/s44 4.22 Trayectoria 2 en simulación y en testbed a 1m/s44 4.23 Posición y referencia en el tiempo para la Trayectoria 2 a 1m/s45 4.24 Errores de posición y yaw en el tiempo para la Trayectoria 2 a 1m/s45 4.25 Velocidades comandadas en el tiempo para la Trayectoria 2 a 1m/s45 4.26 Trayectoria 2 en simulación y en testbed a 0.33m/s→1m/s→0.5m/s→0.25m/s46 4.27 Posición y referencia en el tiempo para la Trayectoria 2 a 0.33m/s→1m/s→0.5m/s→ 0.25m/s46 4.28 Errores de posición y yaw en el tiempo para la Trayectoria 2 a 0.33m/s→1m/s→ 0.5m/s→0.25m/s47 4.29 Velocidades comandadas en el tiempo para la Trayectoria 2 a 0.33m/s→1m/s→ 0.5m/s→0.25m/s47 4.30 Trayectoria 3 en simulación y en testbed a 0.33m/s48 4.31 Posición y referencia en el tiempo para la Trayectoria 2 a 0.33m/s48 4.32 Errores de posición y yaw en el tiempo para la Trayectoria 2 a 0.33m/s49 4.33 Velocidades comandadas en el tiempo para la Trayectoria 2 a 0.33m/s49 4.34 Trayectoria 3 en simulación y en testbed a 0.5m/s50 4.35 Posición y referencia en el tiempo para la Trayectoria 3 a 0.5m/s50 4.36 Errores de posición y yaw en el tiempo para la Trayectoria 3 a 0.5m/s51 4.37 Velocidades comandadas en el tiempo para la Trayectoria 3 a 0.5m/s51 4.38 Trayectoria 3 en simulación y en testbed a 0.2m/s52 4.39 Trayectoria 3 en testbed a 0.2m/s52 4.40 Posicion y referencia en el tiempo para la Trayectoria 3 a 0.2m/s53 4.41 Errores de posición y yaw en el tiempo para la Trayectoria 3 a 0.2m/s53 4.42 Velocidades comandadas en el tiempo para la Trayectoria 3 a 0.2m/s54 4.43 Trayectoria 4 en simulación y en testbed a 0.5m/s54 4.44 Posición y referencia en el tiempo para la Trayectoria 4 a 0.5m/s55 4.45 Errores de posición y yaw en el tiempo para la Trayectoria 4 a 0.5m/s55 4.46 Velocidades comandadas en el tiempo para la Trayectoria 4 a 0.5m/s56 Índice de Figuras 99 4.47 Trayectoria 4 en simulación y en testbed a 0.25m/s56 4.48 Posición y referencia en el tiempo para la Trayectoria 4 a 0.25m/s57 4.49 Errores de posición y yaw en el tiempo para la Trayectoria 4 a 0.25m/s57 4.50 Velocidades comandadas en el tiempo para la Trayectoria 4 a 0.25m/s58 4.51 Trayectoria 4 en testbed a 0.25m/s→0.5m/s58 4.52 Trayectoria 4 en simulación y en testbed a 0.25m/s→0.5m/s59 4.53 Posición y referencia en el tiempo para la Trayectoria 4 a 0.25m/s→0.5m/s59 4.54 Errores de posición y yaw en el tiempo para la Trayectoria 4 a 0.25m/s→0.5m/s60 4.55 Velocidades comandadas en el tiempo para la Trayectoria 4 a 0.25m/s→0.5m/s60 4.56 Trayectoria 5 en simulación y en testbed a 0.2m/s61 4.57 Posición y referencia en el tiempo para la Trayectoria 5 a 0.2m/s61 4.58 Errores de posición y yaw en el tiempo para la Trayectoria 5 a 0.2m/s62 4.59 Velocidades comandadas en el tiempo para la Trayectoria 5 a 0.2m/s62 4.60 Trayectoria 5 (modificada) en simulación y en testbed a 0.2m/s63 4.61 Trayectoria 5 (modificada) en testbed a 0.2m/s63 4.62 Posición y referencia en el tiempo para la Trayectoria 5 a 0.2m/s64 4.63 Errores de posición y yaw en el tiempo para la Trayectoria 5 a 0.2m/s64 4.64 Velocidades comandadas en el tiempo para la Trayectoria 5 a 0.2m/s65 4.65 Simulación en Gazebo y Rviz con F-450 66 4.66 F-450: Trayectoria 6 en simulación a 0.2m/s66 4.67 F-450: Posición y referencia en el tiempo para la Trayectoria 6 a 0.2m/s67 4.68 F-450: Errores de posición y yaw en el tiempo para la Trayectoria 6 a 0.2m/s67 4.69 F-450: Velocidades comandadas en el tiempo para la Trayectoria 6 a 0.2m/s67 4.70 F-450: Trayectoria 6 en simulación a 0.5m/s→0.3m/s→0.75m/s68 4.71 F-450: Posición y referencia en el tiempo para la Trayectoria 6 a 0.5m/s→0.3m/s→0.75m/s68 4.72 F-450: Errores de posición y yaw en el tiempo para la Trayectoria 6 0.5m/s→0.3m/s→ 0.75m/s69 4.73 F-450: Velocidades comandadas en el tiempo para la Trayectoria 6 a 0.5m/s→0.3m/s→ 0.75m/s69 4.74 Simulación en Gazebo y Rviz con F-550 70 4.75 F-550: Trayectoria 6 en simulación a 0.2m/s70 4.76 F-550: Posición y referencia en el tiempo para la Trayectoria 6 a 0.2m/s71 4.77 F-550: Errores de posición y yaw en el tiempo para la Trayectoria 6 0.2m/s71 4.78 F-550: Velocidades comandadas en el tiempo para la Trayectoria 6 a 0.2m/s71 4.79 F-550: Trayectoria 6 en simulación a 0.5m/s→0.3m/s→0.75m/s72 4.80 F-550: Posición y referencia en el tiempo para la Trayectoria 6 a 0.5m/s→0.3m/s→0.75m/s72 4.81 F-550: Errores de posición y yaw en el tiempo para la Trayectoria 6 0.5m/s→0.3m/s→ 0.75m/s73 4.82 F-550: Velocidades comandadas en el tiempo para la Trayectoria 6 a 0.5m/s→0.3m/s→ 0.75m/s73 4.83 Simulación en Gazebo y Rviz con M600 74 4.84 M600: Trayectoria 6 en simulación a 0.2m/s74 4.85 M600: Posición y referencia en el tiempo para la Trayectoria 6 a 0.2m/s75 4.86 M600: Errores de posición y yaw en el tiempo para la Trayectoria 6 0.2m/s75 4.87 M600: Velocidades comandadas en el tiempo para la Trayectoria 6 a 0.2m/s75 4.88 M600: Trayectoria 6 en simulación a 0.5m/s→0.3m/s→0.75m/s76 4.89 M600: Posición y referencia en el tiempo para la Trayectoria 6 a 0.5m/s→0.3m/s→0.75m/s76 4.90 M600: Errores de posición y yaw en el tiempo para la Trayectoria 6 0.5m/s→0.3m/s→ 0.75m/s77 100 Índice de Figuras 4.91 M600: Velocidades comandadas en el tiempo para la Trayectoria 6 a 0.5m/s→0.3m/s→ 0.75m/s77 Índice de Códigos A.1 Modelo SDF del Testbed 81 A.2 Modelo URDF de la geometría del Crazyflie 2.1 82 A.3 Modelo URDF del Crazyflie 2.1 86 B.1 Callback servicio prepare_trajectory 89 B.2 Función csvToPath 89 B.3 Función construcPath 90 B.4 Callback servicio execute_trajectory 91 B.5 Función getVelocity 91 B.6 Función calculateVelocity 93 C.1 Despegue y aterrizaje mediante comandos de velocidad 95 101 Bibliografía [1] Jose J Acevedo, Begona C Arrue, Ivan Maza, and Anibal Ollero. A decentralized algorithm for area surveillance missions using a team of aerial robots with different sensing capabilities. In 2014 IEEE International Conference on Robotics and Automation (ICRA), pages 4735–4740. IEEE, 2014. [2] Nicola Basilico and Stefano Carpin. Deploying teams of heterogeneous uavs in cooperative two-level surveillance missions. In 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 610–615. IEEE, 2015. [3] Fotios Balampanis, Iván Maza, and Aníbal Ollero. Coastal areas division and coverage with multiple uavs for remote sensing. Sensors, 17(4):808, 2017. [4] Luis Merino, Fernando Caballero, J Ramiro Martínez-de Dios, Iván Maza, and Aníbal Ollero. An unmanned aircraft system for automatic forest fire monitoring and measurement. Journal of Intelligent & Robotic Systems, 65(1):533–548, 2012. [5] Huy Xuan Pham, Hung Manh La, David Feil-Seifer, and Matthew C Deans. A distributed control framework of multiple unmanned aerial vehicles for dynamic wildfire tracking. IEEE Transactions on Systems, Man, and Cybernetics: Systems, 50(4):1537–1548, 2018. [6] Chi Yuan, Zhixiang Liu, and Youmin Zhang. Fire detection using infrared images for uav-based forest fire surveillance. In 2017 International Conference on Unmanned Aircraft Systems (ICUAS), pages 567–572. IEEE, 2017. [7] Konstantin Kondak, Markus Bernard, Fernando Caballero, Ivan Maza, and Anibal Ollero. Cooperative autonomous helicopters for load transportation and environment perception. In Advances in Robotics Research, pages 299–310. Springer, 2009. [8] Pedro J Sanchez-Cuevas, Pablo Ramon-Soria, Begoña Arrue, Anibal Ollero, and Guillermo Heredia. Robotic system for inspection by contact of bridge beams using uavs. Sensors, 19(2):305, 2019. [9] Antidio Viguria, Rafael Caballero, Ángel Petrus, Francisco Javier Pérez-Grau, and Miguel Ángel Trujillo. Aerial robotic system for complete bridge inspections. In International Road Federation World Meeting & Exhibition, pages 171–186. Springer, 2022. [10] Roberto C Giacchetta, José Brizuela, Jorge Fernández Cruza, and Francisco Alarcón Romero. Hyfliers: drones inteligentes para automatizar la inspección por ultrasonido de infraestructuras de petróleo y gas. 103