Estudio sobre la evasion de obstáculos por parte de drones de escala reducida

Estudio sobre la evasion de obst´aculos por parte de drones de escala reducida Memoria Autor: Felipe Gallardo J´auregui Director: Albert Masip-Alvarez 19/06/2024 Resumen El contenido de este documento es una tesis de grado que contiene un estudio de evasi´on de obst´aculos por parte de drones de escala reducida, utilizando t´ecnicas de procesado de imagen a trav´es de ROS usando el entorno de Matlab. Se incorpora y profundiza en el uso de temporizadores y maquinas de control de estados a trav´es del desarrollo de diferentes aplicaciones que convergen en la evasi´on de un obst´aculo por parte del dron. En los siguientes cap´ıtulos se encuentra una introducci´on con los objetivos, el alcance, los requisitos y plan del proyecto. Un cap´ıtulo de desarrollo que explica los elementos necesarios con los que se trabaj´o y los conceptos te´oricos necesarios para entender el contenido que sigue. Seguido por la metodolog´ıa y resultados de los maquinas de estado desarrolladas, primero un algoritmo de seguimiento de l´ınea alrededor de un circuito en el suelo y finalmente para el algoritmo desarrollado para la evasion de un obst´aculo predeterminado. Finalmente se presentan y discuten los resultados, junto con las conclusiones finales del trabajo. Abstract The content of this document is a degree thesis that contains a study about obstacle evasion done by a reduced scale drone using image processing techniques through ROS using the Matlab environment. It incorporates and deepens in the use of timer and control state machines through the development of different applications that converge in the obstacle evasion of a drone. In the upcoming chapters you will find an introduction with the objectives, reach, requirements and planning of the project. A development chapter that explains the necessary elements that will be worked upon and the required theoretical concepts to understand the upcoming content. Followed by the methodology and results used in the developed state machines, first for a patch tracking algorithm around a set circuit on the floor and lastly for the algorithm developed for the evasion of a predermined obstacle. Finally the results are presented and discussed, with the final conclusions to the completed work. Resum El contingut d’aquest document es una tesis de grau que cont´e un estudi sobre l’evasi´o d’obstacles per part de drons d’escala redu¨ıda, fent servir t´ecniques de processat d’imatge amb ROS utilitzant l’entorn de Matlab. S’incorpora i s’aprofundeix en l’´us de temporitzadors i m´aquines de control d’estat mitjan¸cant el devolupament de diferents aplicacions que convergeixen en l’evasi´o d’un obstacle per part del dron. En els cap´ıtols seg¨uents trobareu una introducci´o amb els objectius, l’abast, els requeriments i el pla del projecte. Un cap´ıtol de desenvolupament que explica els elements necessaris amb qu`e es va treballar i els conceptes te`orics necessaris per entendre el contingut que segueix. Seguit per la metodologia i resultats de les maquines d’estat desenvolupades, primer un algorisme de seguiment de l´ınia al voltant d’un circuit a terra i finalment per a l’algorisme desenvolupat per a l’evasi´o d’un obstacle predeterminat. Finalment es presenten i es discuteixen els resultats, juntament amb les conclusions finals del treball. ´ Indice general 1. Introducci´on 5 1.1. Objetivos ..................................... 5 1.2. Alcance ...................................... 5 1.3. Requisitos..................................... 6 1.4. Justificaci´on.................................... 6 1.5. Planificaci´on ................................... 7 2. Antecedentes y Estado del Arte 8 2.1. Antecedentes ................................... 8 2.2. EstadodelArte.................................. 10 2.2.1. Tecnologias de deteccion . . . . . . . . . . . . . . . . . . . . . . . . . 10 3. Desarrollo 11 3.1. Elementosnecesarios............................... 11 3.1.1. DJITelloDron.............................. 11 3.1.2. OrdenadorRemoto............................ 12 3.1.3. ROS2Dashing .............................. 13 3.1.4. Temporizadores.............................. 14 3.1.5. M´aquinas de Estado . . . . . . . . . . . . . . . . . . . . . . . . . . . 17 4. Circuito sobre linea roja 18 4.1. PlandeVuelo................................... 18 4.1.1. Diagrama de estado . . . . . . . . . . . . . . . . . . . . . . . . . . . 19 4.2. Metodolog´ıa.................................... 20 4.2.1. Conteodecruces............................. 22 4.2.2. Paradaencruces............................. 23 4.2.3. Seguimiento completo de trayectoria . . . . . . . . . . . . . . . . . . 24 4.3. Resultados..................................... 26 4.3.1. Seguimiento de Linea Parando en Cruces . . . . . . . . . . . . . . . 26 4.3.2. Circuito sobre la l´ınea roja . . . . . . . . . . . . . . . . . . . . . . . 31 5. Evasi´on de obst´aculo 37 5.1. PlandeVuelo................................... 37 5.1.1. Diagrama de Estado . . . . . . . . . . . . . . . . . . . . . . . . . . . 38 5.2. Metodolog´ıa.................................... 39 5.2.1. C´alculo de la Distancia . . . . . . . . . . . . . . . . . . . . . . . . . 39 5.2.2. Elecci´on del Obst´aculo . . . . . . . . . . . . . . . . . . . . . . . . . . 43 5.2.3. C´alculo de distancias . . . . . . . . . . . . . . . . . . . . . . . . . . . 44 5.2.4. Evasi´on del obst´aculo . . . . . . . . . . . . . . . . . . . . . . . . . . 47 5.2.5. M´aquina de Estado . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48 1 Felipe Gallardo GRETA 5.3. Resultados..................................... 51 6. Resumen de los Resultados 57 6.1. Resumen del presupuesto . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57 6.2. Conclusiones ................................... 58 6.3. Trabajofuturo .................................. 59 A. C´odigo del seguimiento de l´ınea con parada en cruces 60 A.1. MainFileTimerCreation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60 A.2.InitialiseTimer .................................. 60 A.3.TimerCallback .................................. 62 A.4.StopTimer..................................... 65 A.5.CalculateDistances................................ 66 A.6.CalculateGamma................................. 68 B. C´odigo del seguimiento de cuadrado en el suelo 69 B.1.TimerCallback .................................. 69 C. C´odigo de la evasion de obst´aculos 74 C.1.TimerCallback .................................. 74 C.2.CleanImages ................................... 80 C.3.CalculateLenghts................................. 80 C.4.CalculateDistances................................ 81 2 ´ Indice de figuras 1.1. Diagrama de Gantt con la planificaci´on . . . . . . . . . . . . . . . . . . . . 7 2.1. DinamicasdeunUAV .............................. 9 3.1. Elementosdecontrol............................... 11 3.2. Dron DJI Tello utilizado en el proyecto . . . . . . . . . . . . . . . . . . . . . 12 3.3. Arquitectura de software del ordenador remoto . . . . . . . . . . . . . . . . 13 3.4. Iniciaci´on de ROS en shell ............................ 14 3.5. Cronograma de un temporizador . . . . . . . . . . . . . . . . . . . . . . . . 15 3.6. Maquina de estado que detecta la secuencia 1011 . . . . . . . . . . . . . . . 17 4.1. Trayectoria a seguir por la aeronave . . . . . . . . . . . . . . . . . . . . . . 18 4.2. Diagrama de estado de la trayectoria de seguimiento de linea roja . . . . . . 19 4.3. Medidas para determinar dyθ......................... 21 4.4. Imagen original y binarizada en blanco y negro . . . . . . . . . . . . . . . . 21 4.5. Imagenes erosionadas para obtener componentes vertical y horizontal . . . . 21 4.6. Imagenes cambio de estado al detectar el primer cruce . . . . . . . . . . . . 29 4.7. Imagenes de centrado en el primer cruce . . . . . . . . . . . . . . . . . . . . 30 4.8. Alejamiento del cruce y retorno al estado de AVANCE S1.......... 31 4.9. Im´agenes del primer giro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34 4.10. Centrado sobre segundo cruce . . . . . . . . . . . . . . . . . . . . . . . . . . 34 4.11. Giro en el segundo cruce . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35 4.12. Giro en el tercer cruce . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36 5.1. Diagrama de estado de la trayectoria de evasi´on de un obst´aculo . . . . . . 39 5.2. Fotograf´ıas capturadas con el dron de un obst´aculo a diferentes distancias . 40 5.3. Procesamiento de imagen del obst´aculo a 220 cms . . . . . . . . . . . . . . 41 5.4. Relacion mm/pix en funci´on de la distancia . . . . . . . . . . . . . . . . . . 43 5.5. Obst´aculo utilizado finalmente . . . . . . . . . . . . . . . . . . . . . . . . . 44 5.6. Tratamiento de imagen previo al c´alculo de distancias . . . . . . . . . . . . 45 5.7. Obst´aculo a diferentes distancias . . . . . . . . . . . . . . . . . . . . . . . . 49 5.8. Imagen capturada en la primera iteraci´on . . . . . . . . . . . . . . . . . . . 52 5.9. Estado S1 de AVANCE hacia el obst´aculo . . . . . . . . . . . . . . . . . . . 53 5.10. Estado S2 de PARADA Y CENTRADO sobre el obst´aculo . . . . . . . . . 54 5.11. Estado S3 de EVASION al obst´aculo . . . . . . . . . . . . . . . . . . . . . . 55 5.12. Estado S4 de ESCAPE del obst´aculo . . . . . . . . . . . . . . . . . . . . . . 56 3 ´ Indice de cuadros 4.1. Estados presentes en la maquina . . . . . . . . . . . . . . . . . . . . . . . . 19 4.2. Datos de navegaci´on de parada en cruces . . . . . . . . . . . . . . . . . . . . 28 4.3. Datos de navegaci´on de seguimiento de cuadril´atero en linea roja . . . . . . 33 5.1. Estados presentes en la maquina de estado . . . . . . . . . . . . . . . . . . . 38 5.2. Datos de medici´on del obst´aculo en diferentes distancias . . . . . . . . . . . 42 5.3. Datos de navegaci´on de evasi´on de obst´aculo . . . . . . . . . . . . . . . . . 52 4 Cap´ıtulo 1 Introducci´on 1.1. Objetivos El objetivo de este proyecto de final de grado es el de desarrollar una soluci´on que permita que un dron de escala reducida con c´amara fija frontal sea capaz de detectar y esquivar un obst´aculo cuya distancia es desconocida. Combinando la informaci´on proporcionada por la imagen a tiempo real que captura el dron y el conocimiento adquirido en la asignatura de Control aplicado sobre un UAV. En primer lugar se deber´an expandir los conocimientos previamente adquiridos con el desarrollo de una maquina de estado que sea capaz de seguir un camino determinado siguiendo una linea roja en el suelo, a modo de expansi´on del trabajo realizado con anterioridad en la asignatura. Por ´ultimo se desarrollara una maquina de estado que permita llevar a cabo los pasos necesarios para detectar y esquivar un obst´aculo determinado previamente. 1.2. Alcance Las siguientes tareas se encuentran dentro del alcance del proyecto Desarrollar una maquina de estado capaz de seguir una trayectoria determinada. Desarrollar una maquina de estado capaz de evadir un obst´aculo Hacer las pruebas de vuelo necesarias para garantizar el funcionamiento consistente de los algoritmos desarrollados Llevar a cabo una forma de esquivar el obst´aculo de forma ´optima Quedan fuera del alcance del proyecto La utilizaci´on de algoritmos de inteligencia artificial para la detecci´on de objetos La posible existencia de elementos ajenos al entorno de trabajo que puedan entorpecer el proceso 5 Felipe Gallardo GRETA Figura 3.2: Dron DJI Tello utilizado en el proyecto Sus especificaciones son: Aeronave •Peso: Aproximadamente 80g (incluyendo bater´ıa y palas) •Dimensiones: 98 x 92.5 x 41 mm (sin jaula protectora) •Palas: 3 pulgadas •Funciones incluidas: Range finder, Bar´ometro, LED, Vision System,2.4 GHz 802.11n Wi-Fi,720p Live View •Puerto: Carga Micro USB Rendimiento de Vuelo •M´axima Distancia de Vuelo: 100m •M´axima Velocidad: 8m/s •M´aximo tiempo de vuelo: 13 min •M´axima Altura de Vuelo: 30m Bateria •Bateria Removible: 1.1 Ah/3.8V C´amara •Foto: 5MP (2592 x 1936) •FOV (Field of View) 82.6º •Video: HD720P30 •Formato: JPG (foto); MP4 (Video) •Estabilizaci´on de imagen 3.1.2. Ordenador Remoto El ordenador remoto es el elemento principal que comanda todo el sistema. La arquitectura del sistema se muestra en la figura 3.3. Aqu´ı se encuentra el sistema Linux Ubuntu Studio 18.04 LTS como la base, cuya versi´on de software actual esta elegida para conectar 12 Felipe Gallardo GRETA espec´ıficamente con la versi´on de ROS utilizada. En este , ya que los drivers de ROS del Tello dron son compatibles con ROS2 Dashing, la versi´on Ubuntu asociada es la 18.04. Figura 3.3: Arquitectura de software del ordenador remoto Aunque el sistema Ubuntu ofrece una interfaz gr´afica b´asica, es importante familiarizarse con el entorno shell de consola, ya que se utiliza para enviar comandos para conectarse con el dron e interactuar con los publishers ysubscribers en caso de que sea necesario. A trav´es de shell se ejecuta tambi´en Matlab, que ser´a el programa que se utilizara para desarrollar las aplicaciones de control para el dron. La versi´on a utilizar de Matlab es la R2021b, que se ejecuta a trav´es de la ventana de comandos a trav´es del comando ./MATLABR2021b/bin/matlab. 3.1.3. ROS2 Dashing ROS son las siglas de Robotic Operative System, que pese a lo que su nombre indica, no es un sistema operativo, sino un intermediario que funciona como el centro neur´algico de control de un sistema rob´otico. A trav´es de una colecci´on de herramientas, librer´ıas, consigue simplificar la creaci´on de los complejos comportamientos del dron a trav´es de m´ultiples plataformas. [9] ROS funciona gracias a los espacios de trabajo o workspaces, un t´ermino que define el ´area del sistema donde se lleva a cabo el desarrollo, una idea que tambi´en est´a presente en ROS2. ROS2 es la evoluci´on directa de ROS1, teniendo en cuenta un acercamiento mas moderno que lo adapta para dar mayor soporte a aplicaciones en tiempo real. Al espacio de trabajo fundamental de ROS2 se le conoce como underlay y a los espacios de trabajo locales adicionales como overlays. En este caso concreto se utilizan dos espacios de trabajo simult´aneos, uno para los drivers del Tello Drone y otro para Matlab R2021b. Es posible combinar diferentes espacios de trabajo, haciendo el desarrollo en diferentes versiones de ROS2 o con diferentes paquetes mas sencilla. Tambi´en permite trabajar con diferentes distribuciones de ROS2, intercambiando entre ´estas a trav´es de shell. Los elementos que intervienen en el funcionamiento de ROS son los siguientes: Mensajes: La informaci´on compartida a trav´es de estos tiene una estructura definida, en muchos caso predefinida para la informaci´on mas t´ıpica como im´agenes, IMUS, velocidades, aceleraciones, posici´on o distintos tipos de operadores. Esta informaci´on es compartida entre nodos a trav´es de los t´opicos. 13 Felipe Gallardo GRETA Figura 3.4: Iniciaci´on de ROS en shell T´opicos: Son los canales donde se comparten los mensajes, cada uno con un nombre ´unico y que en general da una idea de la informaci´on que se publica. Puede enviar y recibir tantos mensajes como desee el usuario a trav´es de los diferentes nodos. Nodos: Son las unidades que suscriben a los distintos t´opicos para poder recibir mensajes con informaci´on o bien modificar y publicar nueva informaci´on. ROS se estructura en packages, cada uno de estos paquetes contiene uno o varios nodos, con la idea de resolver problemas espec´ıficos. La mayor´ıa de los packages b´asicos se incluyen inicialmente aunque tambi´en se ofrece la posibilidad de crear y/o combinar paquetes espec´ıficos para cada proyecto. 3.1.4. Temporizadores Con el objetivo de interactuar con el dron de forma peri´odica, se ha hecho uso de los objetos timer dentro de Matlab. Estos objetos se utilizan para ejecutar acciones programadas despu´es de un determinado intervalo de tiempo a trav´es de funciones callback. Es importante notar que Matlab no es una aplicaci´on multihilos, esto quiere decir que dos temporizadores no se pueden ejecutar en paralelo, lo que quiere decir que el temporizador con un tiempo mas largo bloquear´a la que tenga un tiempo mas corto. El objeto timer tiene propiedades que permiten especificar que comandos se ejecutan cuando empieza un temporizador, adem´as de eventos para iniciarse, detenerse o en caso de error. El cronograma de la figura 3.5 muestra como se ejecutan los diferentes eventos en el timer . 14 Felipe Gallardo GRETA Figura 3.5: Cronograma de un temporizador Para el desarrollo de las aplicaciones se han utilizado cuatro archivos dentro del timer. En primer lugar, un archivo principal que crea el objeto y define sus propiedades, realizando la inicializaci´on, el tipo de ejecuci´on, el periodo, la funci´on de parada y la informaci´on del usuario, que se comparte entre todas las funciones del timer. La funci´on de inicializaci´on contiene los comandos de ROS que establecen el marco de la comunicaci´on con el dron. El callback es donde se encuentran las acciones que se ejecutan de forma peri´odica y por ´ultimo la funci´on stop es la ´ultima en ejecutarse y contendr´a el cierre de las conexiones y aterrizaje de la aeronave. Para ejemplificar el funcionamiento de los temporizadores en Matlab se procede a analizar el codigo de un temporizador que se utiliza para conectar con el dron y hacer fotos con el sin necesidad de que el dron se eleve. Timer Creation Este es el archivo donde se define el temporizador. Propiedades tales como el modo de ejecuci´on, el periodo y las funciones a cargo de cada acci´on. 1clc 2t= timer ; 3t. StartFcn = @InitialiseTimer ; 4t. TimerFcn = @TimerCallback ; 5t. StopFcn = @StopTimer ; 6t. Period = input ( ’Period ( in seconds ) to take pictures ’); 7t.TasksToExecute = input(’Number of pictures to be taken ’); 8t. ExecutionMode = ’fixedRate ’; 9start ( t) ; % start the timer 10 wait (t); % blocks the command prompt until timer , t, stops running 11 stop (t); % stops the timer 12 delete (t); % deletes timer object Timer initialisation Este archivo contiene la creacion de variables globales necesarias para el desarrollo del programa, asi como la tarea de establecer las conexiones con los nodos como suscriptores opublishers en caso de que sea necesario. En este caso concreto, tal y como indican los comentarios del c´odigo, creamos una conexi´on con el nodo que publica las im´agenes de la 15 Felipe Gallardo GRETA c´amara. Los par´ametros de entrada son src yevent. Src es el objeto asociado al timer, cuya clase tiene una propiedad llamada UserData que permite atribuir cualquier valor que quieras y usarla de forma global en todas las funciones del temporizador. El par´ametro event es una estructura con campos de Type yData. 1function InitialiseTimer ( src , event ) 2disp(’Timer initialisation begins ... ’) 3% Nodos 4% Crea una conexion al nodo donde se publican las imagenes de la camara 5ImageNode = ros2node ("/ image_raw "); 6src . UserData = setfield ( src .UserData ,’ ImageNode ’, ImageNode ); 7% Suscriptores 8% Nos suscribimos a las actualizaciones de la camara 9ImageSubscriber = ros2subscriber (ImageNode ,"/ image_raw "); 10 src . UserData = setfield ( src .UserData ,’ ImageSubscriber ’, ImageSubscriber ); 11 disp(’ Timer initialisation ended ! ’) 12 % Wait for USER ACTION 13 uiwait(msgbox(’ Drone will take pictures . Press OK when READY ! ’,’Information ’,’warn ’)); 14 end Timer Callback El callback contiene los comandos que se ejecutan cada vez que empieza un nuevo periodo. Hay que ser cuidadoso para asegurarse que el tiempo de procesamiento es menor que el periodo escogido, para evitar un solapamiento de las acciones, para ello se puede medir el tiempo requerido con tic ytoc. En este caso concreto obtenemos los suscriptores que se hab´ıa declarado previamente en el Initialiser y recibimos la informaci´on de los mismos, en este caso la imagen de la c´amara, que luego se puede almacenar. 1function TimerCallback ( src , event ) 2RowNumber = src . TasksExecuted ; 3% Obtenemos los sucriptores 4ImageSubscriber = getfield ( src .UserData , ’ ImageSubscriber ’); 5% Se obtiene la foto recibiendo la informacion del nodo 6ImageMessage = receive ( ImageSubscriber ,1) ; 7% Se convierte el mensaje en una matrix de imagen 8ImageRGB = rosReadImage(ImageMessage); 9% Se muestra la imagen en pantalla 10 imshow ( ImageRGB ) 11 % La ultima imagen se guarda en el disco 12 if src . TasksExecuted == src . TasksToExecute 13 imwrite ( ImageRGB ,’TakenPicture.png’,’PNG ’); 14 end 15 end Stop Timer Este archivo contiene todas las desconexiones necesarias, guardado de informaci´on o limpieza de variables si es necesario. En este caso, ya que no se despega la aeronave, no es necesario enviarle una desconexi´on y aterrizaje, pero mas adelante si lo ser´a. 1function StopTimer ( src , event ) 2disp(’Timer stopped ! ’) 3end 16 Felipe Gallardo GRETA 3.1.5. M´aquinas de Estado Para poder desarrollar misiones de vuelo mas complejas, es necesaria la incorporaci´on de maquinas de estado como sistema de comportamiento. Tambi´en llamadas Finite-State Machines (FSMs), estos modelos poseen un estado de inicio, de fin y un numero finito de estados intermedios en un sistema que transiciona entre uno y otro en funci´on de unas condiciones concretas. El estado se define como la condici´on de un sistema que esta listo para ejecutar un cambio. En respuesta a ciertos par´ametros, la FSM puede transicionar de un estado a otro [1]. Existen dos tipos fundamentales de m´aquinas de estados: M´aquina de estado determinista: Contienen un orden fijo de eventos que transcurren de forma cronol´ogica, un ejemplo es un sem´aforo, que transiciona en orden de un color a otro, siempre uno detr´as de otro. M´aquina de estado no determinista: En ellas el orden de los eventos cambia cada vez que se ejecuta la m´aquina, es el ejemplo de un ascensor, donde el estado depende de un input que cambia en cada iteracion y no tiene un orden fijo. Diagramas de Estado Los diagramas de estado son la forma gr´afica de representar una m´aquina de estado que muestra la acci´on de entrada, de salida y los posibles estados del sistema, as´ı como las posibles interacciones entre ellos y lo que causa los cambios de uno a otro. En un diagrama de estado (fig.3.6), cada estado se representa como una caja circular con una etiqueta, mientras una flecha indica el cambio de estado. Figura 3.6: Maquina de estado que detecta la secuencia 1011 El diagrama de la figura 3.6 sirve para ejemplificar una m´aquina de estado no determinista que muestra un sistema de alarma que permite al usuario introducir d´ıgitos 0 o 1, si la combinaci´on introducida es la correcta, se desactiva la alarma. En funci´on del d´ıgito introducido y el orden que representa en la secuencia, el estado vuelve al inicial o al primero o bien continua al siguiente, y cuando la combinaci´on se introduce por completo, regresa al estado 1 pero con un valor de salida distinto que activa el desarme de la alarma. En el caso del dron, se estudiar´a un caso inicial a trav´es de una m´aquina de estado, de forma que sirva para familiarizarse con este nuevo concepto, en el cu´al se programe una misi´on de vuelo que recorra una trayectoria delimitada con cinta roja en el suelo del laboratorio. 17 Cap´ıtulo 4 Circuito sobre linea roja Para poder conseguir el objetivo establecido en el proyecto, era fundamental familiarizarse con el concepto previamente desarrollado de m´aquinas de estado, que no se hab´ıa abarcado con anterioridad. Para ello, y teniendo en cuenta el trabajo previamente desarrollado en la asignatura de control, se opt´o por desarrollar una m´aquina de estado que partiera de la base del trabajo desarrollado previamente en la asignatura de control UAV, con el seguimiento de una linea roja en el suelo, ampliando para que el dron sea capaz de seguir una ruta preestablecida con esta cinta. 4.1. Plan de Vuelo Se espera que el dron pueda realizar una misi´on de vuelo sobre unas marcas en el suelo realizadas con cinta m´etrica roja. La trayectoria del dron se controla siguiendo esta linea roja a trav´es de guiado visual. Cada vez que un cruce es detectado, la aeronave se centrara en el mismo para posteriormente realizar un giro de 90ºcontrarreloj antes de seguir la siguiente linea roja vertical, y as´ı sucesivamente hasta completar un cuadril´atero en el suelo. Figura 4.1: Trayectoria a seguir por la aeronave 18 Felipe Gallardo GRETA 4.1.1. Diagrama de estado En la siguiente tabla podemos encontrar la lista de estados presentes en el sistema: Identificador Estado Descripci´on S0 PARADA Dron suspendido en el aire, sin velocidades lineales o angulares S1 AVANCE Control de trayectoria para seguir la linea vertical mientras se avanza S2 ACERCAMIENTO Cruce detectado y centrado sobre el mismo S3 GIRO Giro de 90ºcontrarreloj por odometr´ıa S4 ESCAPE El dron avanza hasta que el cruce desaparece de la imagen Cuadro 4.1: Estados presentes en la maquina El estado inicial es el de PARADA S0. Es el estado justo despu´es del despegue y mantiene el dron en modo hover o de suspensi´on en el aire. De manera que los valores de velocidad tanto lineal como angular se mantengan en 0. La altura de vuelo viene determinada por defecto por el dron por defecto. El estado de PARADA tambi´en sera el estado final si no hay ninguna linea roja en la imagen. Figura 4.2: Diagrama de estado de la trayectoria de seguimiento de linea roja En el diagrama de la fig.4.2 podemos ver como se relacionan unos estados con otros. Al detectar una linea vertical se pasa al estado de AVANCE S1 y la aeronave avanza a velocidad constante manteni´endose centrada a lo largo de la linea vertical roja. Cuando se detecta un cruce en la imagen, el estado cambia a ACERCAMIENTO S2. Como su nombre indica, el dron se acerca al cruce reduciendo la velocidad y centrando el cruce 19 Felipe Gallardo GRETA en la imagen. Esta operaci´on de centrado tiene una tolerancia ya que resulta complicado mantener el dron completamente estable en esta posici´on. El control de trayectoria sigue presente en este estado para mantener la aeronave alineada con la vertical. Una vez el dron esta centrado, se procede a cambiar al estado de GIRO S3. Las velocidades lineales y angulares se establecen en 0 y el dron gira 90ºen sentido opuesto a las agujas del reloj utilizando tan solo odometr´ıa, es decir, sin hacer uso de las im´agenes de la c´amara. Por ´ultimo, y siempre y cuando la imagen siga capturando una linea roja vertical en el suelo, se pasa al estado de ESCAPE S4, que se encarga de mover el dron fuera del cruce, para posteriormente volver al estado de AVANCE S1, y as´ı sucesivamente hasta completar el tiempo de ejecuci´on completando idealmente un cuadril´atero en el suelo o hasta que por alg´un motivo la linea roja desaparezca de la visi´on del dron. 4.2. Metodolog´ıa Para realizar la aplicaci´on descrita, se parti´o de la base de lo desarrollado previamente en la asignatura de control aplicado, donde el objetivo final era desarrollar una misi´on de vuelo que consist´ıa en el seguimiento de trayectoria utilizando la visi´on del dron a trav´es de una cinta roja en el suelo. [10] Este c´odigo es la base para el estado de AVANCE S1. El funcionamiento del algoritmo presente se basa en el posicionamiento de la aeronave vertical a la linea roja definida en el suelo con cinta roja. Para ello, se utiliza un algoritmo basado en el c´alculo en tiempo real de la distancia d de la aeronave al centro de la l´ınea, as´ı como el ´angulo θque forma el eje x, que apunta a la cabeza del dron, con la vertical formada por la cinta. Con estas variables es que se calcula un valor de curvatura γque sirve para describir una curva dependiendo de cuan lejos y desorientado esta el dron de la l´ınea, tal que: γ=2 L[d·cos θ−pL2−d2·sin θ] De tal manera que la curvatura depende de los valores relativos de dyθ, siendo adem´as la inversa del radio de la curva que trazara el dron. γ=1 R N´otese que la f´ormula adem´as contiene la variable L, este par´ametro debe ser escogido por el usuario teniendo en cuenta que su valor debe ser coherente en orden de magnitud y unidades en comparaci´on con d, siendo Lla estimaci´on de la distancia que le tomar´a a la aeronave a corregir su trayectoria. La obtenci´on de las variables dyγse encuentra detallado en [11], en grandes rasgos se trata de analizar la imagen obtenida por el dron en cada callback y realizar el procesado necesario para aislar la cinta roja de la imagen (fig. 4.4), separando la componente vertical de la misma a trav´es de erosionado de imagen (fig.4.5), de manera que se puede obtener la recta que genera esta linea al obtener los puntos superior e inferior de esta, pudiendo obtener el ´angulo de esta respecto de la vertical, as´ı como la distancia del centro de la misma respecto al centro de la imagen, tal como se puede ver en la figura 4.3. 20 Felipe Gallardo GRETA Figura 4.3: Medidas para determinar dyθ (a) Imagen Original (b) Imagen binarizada Figura 4.4: Imagen original y binarizada en blanco y negro (a) Imagen erosionada verticalmente (b) Imagen erosionada horizontalmente Figura 4.5: Imagenes erosionadas para obtener componentes vertical y horizontal 21 Felipe Gallardo GRETA Iteration Pitch Roll Yaw VxVyVzAltitud Estado Contador Cuadro 4.2: Datos de navegaci´on de parada en cruces En la tabla 4.2 se pueden ver como var´ıan los diferentes par´ametros del dron con cada iteraci´on, y teniendo en cuenta que el periodo establecido es de un segundo el cambio es cada periodo. Una de las cosas a destacar es que los valores de las velocidades no son precisos en cantidad, ya que no se guardan en el UserData en el callback, sin embargo, si podemos ver como var´ıan los ´angulos, y mas importante en este caso, como se producen los cambios de estado y de contador. Para corroborar, podemos ver la imagen capturada en las iteraciones en las que se producen los cambios de estado. 28 Felipe Gallardo GRETA (a) Imagen iteracion 8 (b) Imagen iteraci´on 9 (c) Imagen iteracion 24 (d) Imagen iteraci´on 25 (e) Imagen iteraci´on 39 (f) Imagen iteraci´on 40 (g) Imagen iteraci´on 54 (h) Imagen iteraci´on 55 Figura 4.6: Imagenes cambio de estado al detectar el primer cruce 29 Felipe Gallardo GRETA En las im´agenes de la figura 4.6 se puede ver el momento en el que la aeronave capta los cruces, en la iteraci´on 8 aparece por primera vez el cruce en la imagen, por lo que el estado en este momento es a´un de avance S1, al detectar el cruce aumenta el valor del contador en 1 y el estado cambia inmediatamente al estado 2 de centrado. Lo mismo ocurre en las iteraciones 24, 39 y 54, todos ellos se pueden comprobar en la tabla 4.2. (a) Imagen iteracion 10 (b) Imagen iteraci´on 11 (c) Imagen iteraci´on 12 (d) Imagen iteraci´on 13 (e) Imagen iteraci´on 14 (f) Imagen iteraci´on 15 (g) Imagen iteraci´on 16 Figura 4.7: Imagenes de centrado en el primer cruce En el conjunto de fotograf´ıas de la figura 4.7 se puede ver el proceso de centrado del dron en el primer cruce, en las primeras iteraciones se ve que la aeronave se aleja bastante del centro, esta situaci´on es normal si tenemos en cuenta que el dron viene con una velocidad constante que genera una inercia que hace que pase el cruce de largo, sin embargo esto no impide que haya tiempo suficiente para corregir, y a partir de la iteraci´on 12 podemos ver como retrocede hasta volver al centro otra vez. De igual manera, vemos como el dron empieza a uno de los lados y se va desplazando hacia el otro. Una vez que el dron esta centrado, lo que se ve en la iteraci´on 15, el estado cambia al de escape S3, momento en el que avanza alej´andose del cruce. 30 Felipe Gallardo GRETA (a) Imagen iteraci´on 19 (b) Imagen iteraci´on 20 (c) Imagen iteraci´on 21 Figura 4.8: Alejamiento del cruce y retorno al estado de AVANCE S1 En estas im´agenes se puede ver el momento en el que desaparece la linea horizontal, lo que corresponde con el paso del estado de ESCAPE al de AVANCE. 4.3.2. Circuito sobre la l´ınea roja Ya se ha visto como funciona la detecci´on de cruces y el centrado realizado sobre los mismos, el mismo principio se ha aplicado en la maquina de estado que efect´ua el giro que permite hacer el seguimiento del cuadril´atero marcado en el suelo con linea roja. Aun as´ı, se van a analizar tanto los datos de navegaci´on como las im´agenes obtenidas por el dron al efectuar el plan de vuelo con ´exito. Cabe se˜nalar que en ciertas ejecuciones se daban algunos problemas mas que en el seguimiento de la linea, una de las razones principales que se vieron es que al ser tramos mas cortos se produc´ıa que el paso entre el estado de ESCAPE S3 y el de AVANCE S1 se daba de forma que coincid´ıa con el siguiente cruce, lo que retrasaba el paso al estado de ACERCAMIENTO S2 y hacia que en ocasiones el dron se alejara demasiado del cruce. Sin embargo, el grado de ´exito fue satisfactorio y reproducible de forma consistente. A continuaci´on se proceder´a a analizar los resultados de una de las ejecuciones completas del circuito, cuya ejecuci´on completa consta de 90 iteraciones de 1 segundo de periodo, es decir, 1 minuto y 30 segundos de vuelo. Iteracion Pitch Roll Yaw VxVyVzAltitud Estado Contador 1 0 0 -30 0 0 0 1 0 0 2 0 0 -30 0 0 0 1 1 0 3 0 0 -34 0 0 0 1 1 0 4 0 0 -32 -1 0 0 1 1 0 5 0 0 -33 -1 0 0 1 2 1 6 1 0 -38 0 0 0 1 2 1 7 0 0 -34 0 0 0 1 2 1 8 0 0 -37 0 -1 0 1 2 1 9 0 -1 -36 0 0 0 1 2 1 10 0 0 -30 1 0 0 1 2 1 11 0 0 -34 0 0 0 1 2 1 12 1 0 -42 0 0 0 1 2 1 13 0 0 -40 0 0 0 1 2 1 14 0 0 -35 0 0 0 1 2 1 15 1 0 -30 0 0 0 1 2 1 16 1 0 -34 0 0 0 1 2 1 17 0 0 -34 1 0 0 1 2 1 18 0 0 -28 1 0 0 1 2 1 19 0 0 -31 0 0 0 1 2 1 Continua en la siguiente pagina 31 Felipe Gallardo GRETA Iteracion Pitch Roll Yaw VxVyVzAltitud Estado Contador 20 0 0 -38 0 0 0 1 2 1 21 0 0 -35 0 0 0 1 2 1 22 0 0 -32 0 0 0 1 2 1 23 0 0 -33 0 0 0 1 3 1 24 0 0 -43 0 0 0 1 3 1 25 0 0 -56 0 0 0 1 3 1 26 0 0 -67 0 0 0 1 3 1 27 0 0 -78 0 0 0 1 3 1 28 0 0 -90 0 0 0 1 3 1 29 0 0 -103 0 0 0 1 3 1 30 0 0 -115 0 0 0 1 3 1 31 0 0 -127 0 0 0 1 3 1 32 0 0 -130 0 0 0 1 4 1 33 0 0 -127 0 0 0 1 4 1 34 -1 0 -121 0 0 0 1 4 1 35 -1 0 -117 0 0 0 1 4 1 36 0 0 -119 0 1 0 1 4 1 37 0 0 -126 0 1 0 1 1 1 38 0 0 -123 0 1 0 1 1 1 39 0 0 -117 0 1 0 1 1 1 40 0 0 -108 0 1 0 1 1 1 41 0 0 -110 0 1 0 1 2 2 42 0 0 -105 0 1 0 1 2 2 43 0 0 -100 0 0 0 1 2 2 44 0 0 -104 0 0 0 1 2 2 45 0 0 -103 0 0 0 1 2 2 46 1 -1 -105 0 0 0 1 2 2 47 0 0 -110 0 0 0 1 2 2 48 0 0 -116 1 0 0 1 2 2 49 0 0 -123 0 0 0 1 3 2 50 0 0 -133 0 0 0 1 3 2 51 0 0 -145 0 0 0 1 3 2 52 0 0 -157 0 0 0 1 3 2 53 0 0 -169 0 0 0 1 3 2 54 0 0 178 0 0 0 1 3 2 55 0 0 165 0 0 0 1 3 2 56 0 0 152 0 0 0 1 3 2 57 0 0 148 0 0 0 1 4 2 58 -1 0 144 0 0 0 1 4 2 59 0 0 144 1 0 0 1 4 2 60 0 0 149 1 0 0 1 4 2 61 0 0 144 1 0 0 1 1 2 62 0 0 136 1 0 0 1 1 2 63 0 0 137 0 0 0 1 1 2 64 0 0 142 0 0 0 1 2 3 65 0 0 148 0 0 0 1 2 3 66 0 0 146 0 0 0 1 3 3 67 0 0 136 0 0 0 1 3 3 68 0 0 125 0 0 0 1 3 3 69 0 0 115 0 0 0 1 3 3 Continua en la siguiente pagina 32 Felipe Gallardo GRETA Iteracion Pitch Roll Yaw VxVyVzAltitud Estado Contador 70 0 0 94 0 0 0 1 3 3 71 0 -1 82 0 0 0 1 3 3 72 0 0 68 0 0 0 1 3 3 73 0 0 54 0 0 0 1 3 3 74 0 0 52 0 0 0 1 4 3 75 0 0 56 0 0 0 1 4 3 76 0 0 53 0 -1 0 1 4 3 77 -1 0 56 0 0 0 1 4 3 78 0 0 53 0 -1 0 1 4 3 79 0 0 55 0 -1 0 1 1 3 80 0 0 59 0 -1 0 1 1 3 81 0 0 57 0 -1 0 1 1 3 82 0 0 51 0 -1 0 1 1 3 83 0 0 50 0 0 0 1 2 4 84 0 0 56 0 0 0 1 2 4 85 0 0 61 0 -1 0 1 2 4 86 1 -1 59 0 0 0 1 3 4 87 0 0 48 0 0 0 1 3 4 88 0 0 36 0 0 0 1 3 4 89 0 0 23 0 0 0 1 3 4 90 0 0 21 0 0 0 1 3 4 Cuadro 4.3: Datos de navegaci´on de seguimiento de cuadril´atero en linea roja De manera similar a la tabla anterior, en esta (4.3) podemos ver como cambian los par´ametros de la aeronave, as´ı como las definidas en la m´aquina de estado. Adem´as de los cambios de estado, una variable a destacar aqu´ı es la del ´angulo de gui˜nada, que cambia significativamente al realizar los giros, y cuyos valores se utilizan para realizar el giro por odometria. Una cosa a destacar es que el ´angulo de gui˜nada no oscila entre 0 y 360 grados, sino que pasa de -180 a 180 grados, lo que en principio puede dificultar el comparar los ´angulos a la hora de realizar el giro. Por esta raz´on se ha utilizado la funci´on angdiff que devuelve la diferencia de ´angulos en radianes con valores entre −πyπy no da problemas al trabajar con ´angulos negativos. 33 Felipe Gallardo GRETA (a) Imagen iteraci´on 22 (b) Imagen iteraci´on 25 (c) Imagen iteracion 28 (d) Imagen iteraci´on 31 Figura 4.9: Im´agenes del primer giro En las fotograf´ıas de la figura 4.9 se puede ver como el dron realiza el primer giro, en la iteraci´on 22 se da por completado el centrado y comienza por tanto el giro a velocidad constante, el ´angulo de inicio de gui˜nada es de -31 grados y al final del giro es de -127, por lo que la diferencia absoluta supera los 90 grados, dando pie al comienzo del estado de ESCAPE S4. (a) Imagen iteraci´on 40 (b) Imagen iteraci´on 42 (c) Imagen iteraci´on 43 (d) Imagen iteraci´on 45 (e) Imagen iteraci´on 47 (f) Imagen iteraci´on 48 Figura 4.10: Centrado sobre segundo cruce 34 Felipe Gallardo GRETA El centrado sobre el segundo cruce que se puede ver en las fotograf´ıas de la figura 4.10 presenta ciertas complicaciones, se puede ver como las im´agenes de la iteraci´on 40 y 42 presentan cierta distorsi´on, ocasionada por un fallo de conexi´on moment´aneo con el dron, esto provoca que los c´alculos de la distancia no sean precisos y la aeronave se aleje en sentido contrario, lo que se puede ver reflejado en la iteraci´on 43. Por suerte se corrige a tiempo lo que permite acabar centrando la aeronave y pasando al estado de giro en la iteraci´on 48. (a) Imagen iteraci´on 49 (b) Imagen iteraci´on 51 (c) Imagen iteraci´on 53 (d) Imagen iteraci´on 55 (e) Imagen iteraci´on 57 (f) Imagen iteraci´on 58 Figura 4.11: Giro en el segundo cruce Este giro se ejecuta de forma similar al primero, empezando en la iteraci´on 49 (4.11a) con un ´angulo de -123 grados y terminando en la 56 (4.11f) con un ´angulo de 152 grados. Pese al cambio de signo de los ´angulos, el programa no tiene problemas a la hora de calcular la diferencia. 35 Felipe Gallardo GRETA (a) Imagen iteracion 66 (b) Imagen iteraci´on 68 (c) Imagen iteracion 71 (d) Imagen iteraci´on 73 Figura 4.12: Giro en el tercer cruce Por ´ultimo se realiza un tercer giro entre las iteraciones 66 y 73 (fig. 4.12), como se ve representado en las fotograf´ıas, para despu´es proceder a un ultimo avance hasta el cuarto cruce, cuyo giro no se completa porque se acaba el tiempo de ejecuci´on. 36 Cap´ıtulo 5 Evasi´on de obst´aculo Una vez desarrollada la m´aquina de estado que realiza el recorrido de seguimiento de la l´ınea roja con los giros, se procedi´o a abordar el problema principal del proyecto, la evasi´on del obst´aculo. Para este problema se plantean diversas dificultades que no exist´ıan previamente, se debe tener en cuenta que debido a que la aeronave solo posee una c´amara, ya no puede estar enfocando al suelo y el guiado por visi´on de ´este ya no puede ser de ayuda, por tanto, sera el obst´aculo en si mismo el que ha de servir de gu´ıa para el vuelo. Tambi´en se ha de tener en consideraci´on que el vuelo no ha de ser a altitud constante, ya que esta se debe ajustar a la altura a la que este el obst´aculo, que al servir de gu´ıa ha de estar siempre en la medida de lo posible en el centro de la imagen. Por otra parte, se debe encontrar una manera para mantener el obst´aculo como objeto de acercamiento, evitando la posible distracci´on ocasionada por elementos ajenos presentes en el laboratorio que pudiesen captar la atenci´on del dron. Con estas consideraciones en mente es que se desarroll´o la soluci´on al problema, aplicando diferentes acercamientos hasta encontrar la soluci´on definitiva. 5.1. Plan de Vuelo Se espera que el dron pueda realizar una misi´on de vuelo consistente en la detecci´on, acercamiento y evasi´on de un obst´aculo debidamente colocado en el laboratorio a trav´es de guiado visual y odometria. El dron debe ser capaz de detectar el obst´aculo a una distancia prudencial, centrarse para mantener el contacto visual mientras se va acercando a velocidad constante hasta alcanzar una distancia m´ınima , momento en el cual se ajustar´a el centrado antes de comenzar la maniobra de evasi´on lateral del objeto. Una vez completada la maniobra, el dron realizara las acciones necesarias para volver a ubicarse en la direcci´on original y seguir con su trayectoria original. 37 Felipe Gallardo GRETA Figura 5.5: Obst´aculo utilizado finalmente De manera que el obst´aculo es tal cual el que aparece en la figura 5.5, una caja de 40 cms de ancho y 32 cms de alto en todas sus caras. El procesado de imagen realizado sobre este obst´aculo es similar al realizado al seguir la trayectoria sobre la cinta roja, ya que sera necesario calcular la distancia a cada una de las l´ıneas para poder realizar el centrado del dron tanto en el estado 3 como en el de avance, adem´as de estas distancias se tienen que calcular en cada iteraci´on la longitud de las lineas horizontal y vertical en p´ıxeles para poder calcular a que distancia esta el dron del obst´aculo. Se realizaron pruebas adicionales utilizando este obst´aculo para comprobar que la conversi´on de p´ıxel a mm es la misma, los resultados fueron consistentes y adem´as esta prueba sirvi´o para desarrollar el algoritmo que calcula las longitudes de las lineas, cuya obtenci´on es diferente al de las ´areas calculadas previamente. Sin embargo, no se considera necesario compartir las fotograf´ıas ni los datos obtenidos ya que el resultado no varia. 5.2.3. C´alculo de distancias Como se viene mencionando, es necesario para el desarrollo de la misi´on estimar a que distancia se encuentra el dron en todo momento del obst´aculo. Para ello es necesario en primer lugar conocer que longitud en p´ıxeles tienen las lineas sobre el objeto. Lo que se hizo fue procesar la imagen transformandola en capas de crominancia [11], de la misma manera que se hizo con las l´ıneas rojas del suelo, pero esta vez seleccionando la segunda capa de crominancia azul. Aunque no hay tanta interferencia en la imagen con otros elementos, si que aparecen algunos y para poder eliminarlos en este caso se ha optado por erosionar y posteriormente dilatar la imagen. 44 Felipe Gallardo GRETA (a) Obst´aculo a 240 cms de distancia (b) Imagen del obst´aculo binarizada (c) Imagen erosionada verticalmente (d) Imagen dilatada verticalmente (e) Imagen erosionada horizontalmente (f) Imagen dilatada horizontalmente Figura 5.6: Tratamiento de imagen previo al c´alculo de distancias En las im´agenes de la figura 5.6 se puede ver el procesado que se realiza a la imagen previo a calcular las longitudes de las lineas, en primer lugar tenemos la imagen sin alterar, donde si nos fijamos bien ya podemos ver que aparecen elementos que incluyen algo de azul, elementos que est´an presentes en la segunda imagen binarizada despu´es de aislar las capas de crominancia azul de la imagen original. Para deshacernos de estos elementos se aplica erosi´on, esto hace que a trav´es de un elemento estructural se haga un barrido que elimine p´ıxeles que no est´an completamente rodeados por otros p´ıxeles blancos del tama˜no del elemento estructural escogido, esto sin embargo ocasiona que en los v´ertices de las l´ıneas tambi´en desaparezcan elementos y para solucionar este hecho se aplica la dilataci´on con el mismo elemento estructural, lo que 45 Felipe Gallardo GRETA nos deja con la imagen final tanto vertical como horizontal con las lineas en su distancia original. 86 function [ VerticalImage , HorizontalImage ] = CleanImages ( ImageRGB ) 87 88 % CleanImages Devuelve dos imagenes limpias , horizontal y vertical de una imagen RGB 89 % Binarizamos la imagen despues de convertirla a capas de Crominancia analizando la capa azul , y despues usando diferentes elementos estructurales para las lineas vertical y horizontal para devolver unas imagenes erosionadas y dilatadas que eliminan cualquier rastro de fuera del obstaculo 90 91 ImageCR = rgb2ycbcr ( ImageRGB ); 92 ImageBW = imbinarize ( ImageCR (: ,: ,2) ,.56) ; 93 SEV = strel ( ’line ’ ,40 ,90) ; 94 SEH = strel ( ’line ’ ,40,0); 95 ImageEV = imerode ( ImageBW , SEV ); 96 VerticalImage = imdilate ( ImageEV , SEV ); 97 ImageEH = imerode ( ImageBW , SEH ); 98 HorizontalImage = imdilate ( ImageEH , SEH ); 99 100 end Una vez obtenidas las im´agenes erosionadas y dilatadas se procede a calcular la distancia de las lineas, para ello se desarroll´o la siguiente funci´on: 101 function [ LenV , LenH ] = CalculateLengths ( ImageEV , ImageEH ) 102 %% Calculo distancia de izquierda a derecha 103 104 Row = []; 105 Column = 0; 106 LenH = 0; 107 108 while isempty ( Row ) && Column <960 109 Column = Column + 1; 110 Row = find ( ImageEH (: , Column )); 111 while ~ isempty ( Row ) && Column <960 112 Column = Column + 1; 113 Row = find ( ImageEH (: , Column )); 114 LenH = LenH + 1; 115 end 116 end 117 118 %% Calculo distancia de abajo a arriba 119 120 Column =[]; 121 Row =720; 122 LenV = 0; 123 124 while isempty ( Column ) && Row > 1 125 Row = Row - 1; 126 Column = find ( ImageEV ( Row ,:) ); 127 while ~ isempty ( Column ) && Row > 1 128 Row = Row - 1; 129 Column = find ( ImageEV (Row ,:) ); 130 LenV = LenV + 1; 131 end 132 end 133 end Lo que se hace en esta funci´on es simplemente contar los p´ıxeles de cada una de las lineas de izquierda a derecha y de abajo a arriba a trav´es de una variable que en realidad 46 Felipe Gallardo GRETA es un contador que a˜nade uno cada vez que se encuentran p´ıxeles blancos al recorrer la imagen. Estas longitudes nos van a permitir calcular la distancia al objeto de forma sencilla en cada callback, ya que sabemos su valor en mm, que es de 400 mm para la linea horizontal y de 320 mm para la vertical. Y sabiendo que: K1=1,07 ·X1+0,0113 = 400 LenH K2=1,037 ·X2+0,0133 = 320 LenV N´otese que estas ecuacion son ligeramente distintas a la presentada antes, dado que son las que se obtuvieron del an´alisis particular del obst´aculo con la cruz, adem´as de que estan separadas para la horizontal y la vertical. Esto nos da que la distancia es: X1 = (400 −0,0113 ·LenH) 1,07 ·LenH X2 = (320 −0,0133 ·LenV ) 1,037 ·LenV X=X1 + X2 2 5.2.4. Evasi´on del obst´aculo Para evadir el obst´aculo tambi´en se tuvieron en cuenta diferentes alternativas, todas ellas evadiendo eso si de forma lateral, ya que otras alternativas como pasar por encima carec´ıan de sentido debido a la falta de visi´on hacia abajo, adem´as de limitaciones de la aeronave a la hora de captar la altura, ya que posee un sensor que se activa por defecto y al detectar un objeto debajo aumenta la altitud pudiendo as´ı colisionar con el techo. Se tuvieron en consideraci´on diferentes opciones, una de ellas era realizar movimientos laterales sin giro con ayuda visual, o sea manteniendo el an´alisis de la imagen en todo momento, el problema de esto es que sin a˜nadir un giro es imposible establecer contacto visual con las caras laterales y pasar el objeto por odometria sin tener informaci´on de la posici´on del dron carec´ıa de sentido. Por lo tanto la existencia de giros es fundamental, sabiendo esto, se hicieron diferentes pruebas y finalmente se decidi´o resolver el problema realizando un giro completo al obst´aculo. Teniendo la distancia al objeto adem´as de su tama˜no, se puede atribuir un radio de giro como la suma de la distancia a la cara visible del obst´aculo mas la distancia al punto medio del objeto, y teniendo el radio podemos calcular la velocidad angular necesaria para realizar el giro, tal que: γ=1 R ωz=γ·vy 47 Felipe Gallardo GRETA Donde vyes la velocidad lineal escogida en el eje lateral, ya que el giro se realiza en esta direcci´on. 5.2.5. M´aquina de Estado Una vez definida en gran parte la metodolog´ıa de trabajo se pudo proceder a desarrollar la m´aquina de estado dentro del temporizador. En primer lugar el dron toma vuelo y se mantiene en estado de suspensi´on en el aire. Para evitar posibles errores la condici´on para el avance es detectar ambas lineas azules que marcan el obst´aculo. Esto se hace con los valores blocked devueltos por la funci´on que calcula las distancias a ambas l´ıneas, en caso de no encontrarlas. En caso de hallarse ambas l´ıneas la aeronave para al estado S1 de avance. 135 if State == 0 % HOVER . Drone mode de suspension en el aire. Las velocidades lineales y angulares son 0 136 if blocked1 && blocked2 % No hay objeto detectado 137 CommandMessage . angular .z = 0; 138 CommandMessage . linear .x = 0; 139 CommandMessage . linear .y = 0; 140 CommandMessage . linear .z = 0; 141 src . UserData . State = 0; 142 else 143 CommandMessage . angular .z = 0; 144 CommandMessage . linear .x = 0; 145 CommandMessage . linear .y = 0; 146 CommandMessage . linear .z = 0; 147 src . UserData . State = 1; 148 end En el estado de avance la aeronave se mueve a velocidad lineal constante en el eje x, a la vez que realiza un control de trayectoria centr´andose al obst´aculo. Para ello se utilizo un algoritmo similar al utilizado para el centrado en los cruces del seguimiento de trayectoria anterior, con la diferencia de que al tratarse de un objeto que esta al frente y no abajo, las correcciones se realizan con velocidades en los ejes yyz, tal que: 149 if dH <=0 && dV <=0 150 if dH > -0.3 151 if dV > -0.2 152 CommandMessage . linear .y = double (0) ; 153 CommandMessage . linear .z = double (0) ; 154 src . UserData . State = 1; 155 else 156 CommandMessage . linear .y = double (- VlinealN ); 157 CommandMessage . linear .z = double (0) ; 158 src . UserData . State = 1; 159 end 160 else 161 if dV > -0.2 162 CommandMessage . linear .y = double (0) ; 163 CommandMessage . linear .z = double ( VlinealN ); 164 src . UserData . State = 1; 165 else 166 CommandMessage . linear .y = double (- VlinealN ); 167 CommandMessage . linear .z = double ( VlinealN ); 168 src . UserData . State = 1; 169 end 170 end Este trozo de c´odigo muestra las correcciones de la aeronave en el caso de que las 48 Felipe Gallardo GRETA distancias sean menores a 0. Tal y como est´an calculadas estas distancias, un valor negativo significa que el obst´aculo esta a la derecha del dron, para la distancia a la linea vertical, y por encima de este para la distancia a la horizontal. En el ejemplo de la figura 5.6, ambas distancias son negativas. Dado que el objetivo de este estado no es que la aeronave este completamente centrada con el obst´aculo, sino que se aproxime sin perderlo de vista, los m´argenes para los cuales se realizan correcciones en las velocidades son significativamente superiores a los que encontramos para el centrado sobre las cruces o el que se utiliza para el centrado una vez estamos mas cerca del obst´aculo. Siendo ´estos de 30 cm para la linea horizontal y 20 cms para la linea vertical. Para pasar al estado S2 de parada y centrado la condici´on es sencilla, la distancia al objeto Xdebe ser inferior a 1m. Se ha escogido 1 metro ya que a esta distancia se garantiza visibilidad mas que completa del obst´aculo, adem´as se hizo sabiendo que la distancia final al obst´aculo acaba siendo bastante inferior. Esto se debe a que el dron lleva una velocidad constante junto a una inercia que le hace seguir avanzando algo despu´es de detectar esta distancia m´ınima, pero el margen es suficientemente amplio para garantizar que el dron no se acabe acercando demasiado en la mayor´ıa de los casos. (a) Obst´aculo a 60 cms (b) Obst´aculo a 80 cms (c) Obst´aculo a 120 cms Figura 5.7: Obst´aculo a diferentes distancias 171 elseif X < 1 % Only when the distance to the obstacle is less than 1m 172 CommandMessage . angular .z = 0; 173 CommandMessage . linear .x = 0; 174 CommandMessage . linear .y = 0; 175 CommandMessage . linear .z = 0; 176 src . UserData . State = 2; El estado S2 de parada y centrado es muy similar al de centrado en los cruces y utiliza el mismo principio que en el acercamiento, eso si, se redujeron considerablemente los margenes para garantizar que la aeronave este realmente lo mas centrada posible, por lo que la condici´on para pasar al estado S3 de evasi´on es que ambas distancias sean inferiores a 5 cms. Una vez se cumple esta condici´on, se guardan los datos de ´angulo de gui˜nada inicial y distancia inicial al objeto, que se utilizan en el siguiente estado. 177 CommandMessage . linear .y = double (0) ; 178 CommandMessage . linear .z = double (0) ; 179 src . UserData . InitialDistance = X; % Se guarda la distancia al objeto 180 src . UserData . InitialYaw = Yaw ; % Se guarda el angulo de yaw 181 disp (src. UserData . InitialYaw ) 182 src . UserData . State = 3; 49 Felipe Gallardo GRETA El estado S3 es el de evasi´on del obst´aculo, en este se definen las condiciones necesarias para ejecutar un giro alrededor del obst´aculo de manera que se llegue al otro lado. El giro se establece definiendo el radio de giro en funci´on de la distancia al obst´aculo y del tama˜no del mismo y por odometria comparando el ´angulo de gui˜nada inicial con el de cada iteraci´on hasta que la diferencia sea de 180 grados, momento en el cual la aeronave estar´a al otro lado del objeto. Los ´angulos de gui˜nada que se reciben del dron tienen valores negativos y positivos, lo que dificulta su comparaci´on cuando se produce un cambio de signo. Para remediar este hecho se ha aplicado el siguiente fragmento de c´odigo. 183 if DataMessage . yaw >= 0 184 Yaw = DataMessage . yaw ; 185 else 186 Yaw = 360 + DataMessage . yaw ; 187 end Esto corrige los valores que se tornan negativos al superar los 180 grados y los pone todos entre 0 y 360. De esta manera se puede hacer la diferencia de los ´angulos tal que: 188 if abs ( angdiff ( deg2rad ( double ( Yaw )), deg2rad ( double ( src . UserData . InitialYaw )))) < 0.95* pi 189 CommandMessage . angular .z = double (- Wz ); 190 CommandMessage . linear .x = 0; 191 CommandMessage . linear .y = double ( VlinealN ); 192 CommandMessage . linear .z = 0; 193 src . UserData . State = 3; A diferencia del giro en cruces del cap´ıtulo anterior, aqu´ı el giro al ser de 180 grados y no de 90, lo que presenta una dificultad a˜nadida, y es que la funci´on que calcula la diferencia de ´angulos lo hace entre −πyπ, por lo que nunca ser´a mayor que esto y si la condici´on es que sea menor que πse tendr´ıa que dar el raro caso de que fuese exactamente πpara pasar al siguiente estado. Esto se puede solucionar de diferentes maneras, pero en este caso se opt´o por a˜nadir un peque˜no margen de entre un 5 y un 10 % a la diferencia de ´angulos. Aunque esta soluci´on puede parecer algo tosca, en realidad resulta beneficiosa en la pr´actica, ya que se vio que a˜nadir un margen que activara el cambio de estado algo antes resultaba en un mejor punto de detenci´on del dron al girar, ya que compensaba con la inercia que ´este lleva y el tiempo de respuesta. Por ´ultimo est´a el estado S4 de escape, cuya ´unica diferencia con estos estados anteriores es que en este caso se ejecuta un ultimo giro en el sitio a velocidad angular constante, con el objetivo de realinear el dron con la trayectoria original, ya que la posici´on final del giro es mirando al obst´aculo. 194 elseif State == 4 % ESCAPE . El dron gira para poder continuar con la trayectoria original 195 if abs ( angdiff ( deg2rad ( double ( Yaw )) ,deg2rad ( double ( src . UserData . InitialYaw )))) < 0.95* pi 196 CommandMessage . angular .z = double (0.5) ; 197 CommandMessage . linear .x = 0; 198 CommandMessage . linear .y = 0; 199 CommandMessage . linear .z = 0; 200 src . UserData . State = 4; 50 Felipe Gallardo GRETA 5.3. Resultados El mismo m´etodo utilizado para la maquina de estado del cap´ıtulo 3 es el que se utiliza para comprobar el funcionamiento de ´esta. Se hicieron m´ultiples ejecuciones de la misi´on desde diferentes distancias sobretodo y aunque de igual manera que antes, la reproducibilidad no es del 100 %, salvo errores concretos las ejecuciones fueron correctas. Algunos errores t´ıpicos ten´ıan que ver con la desconexi´on del dron, o no detenerse a tiempo al acercarse al obst´aculo. Algo que tambi´en se observo es que al no seguir una trayectoria lineal en el suelo, cualquier inclinaci´on en la direcci´on que tuviese el dron no se corregir´ıa sino que simplemente avanzar´ıa con un ´angulo respecto a la trayectoria. Los resultados de las ejecuciones mas exitosas se guardaron tanto en formato de v´ıdeo, como las fotos sacadas por el dron en su trayectoria y los datos de navegaci´on guardados al final de la ejecuci´on. A continuaci´on se van a compartir los datos de una de las ejecuciones exitosas de la evasi´on del obst´aculo. Iteracion Pitch Roll Yaw VxVyVzEstado 1 0 0 -25 0 0 0 0 2 0 0 -25 0 0 0 1 3 0 0 -25 0 0 0 1 4 0 0 -25 0 0 0 1 5 0 0 -25 0 0 0 1 6 0 0 -25 1 0 0 1 7 0 0 -25 1 0 0 1 8 0 0 -25 1 0 0 1 9 0 0 -25 1 0 0 1 10 0 0 -25 1 0 0 1 11 0 0 -25 1 0 0 1 12 0 1 -25 1 0 0 1 13 0 0 -25 2 0 0 1 14 0 0 -25 2 0 0 1 15 0 0 -25 1 0 0 1 16 3 0 -25 0 0 0 2 17 0 0 -25 0 0 0 2 18 0 0 -25 0 -1 0 2 19 0 1 -25 0 0 0 2 20 0 0 -25 0 0 0 2 21 0 0 -25 0 0 0 2 22 0 0 -25 0 0 0 2 23 0 0 -25 0 0 0 3 24 1 -1 -15 0 -1 0 3 25 0 0 -1 0 -2 0 3 26 0 0 12 0 -2 0 3 27 0 0 26 0 -2 0 3 28 0 0 30 0 -2 0 3 29 0 0 55 1 -1 0 3 30 0 0 70 2 -1 0 3 31 0 0 84 2 0 0 3 32 0 0 100 2 0 0 3 33 0 0 112 2 1 0 3 Continua en la siguiente pagina 51 Felipe Gallardo GRETA Iteracion Pitch Roll Yaw VxVyVzEstado 34 0 0 125 2 1 0 3 35 0 0 141 1 1 0 3 36 0 0 148 0 1 0 4 37 1 0 126 0 0 0 4 38 0 0 74 0 0 0 4 39 0 0 57 0 0 0 4 40 0 0 18 0 0 0 4 Cuadro 5.3: Datos de navegaci´on de evasi´on de obst´aculo La informaci´on mas relevante que podemos extraer de estos datos es como varia el ´angulo de gui˜nada (yaw) y los momentos en los que se producen los cambios de estado. El primer cambio de estado es inmediato, y se produce al detectar el obst´aculo en la distancia (5.8). Figura 5.8: Imagen capturada en la primera iteraci´on Desde este punto se procede a realizar el avance y acercamiento hasta la iteraci´on 15, en la que se detecta que se ha superado la distancia de 1 metro con el objeto. 52 Felipe Gallardo GRETA (a) Iteracion 4 (b) Iteraci´on 7 (c) Iteraci´on 9 (d) Iteraci´on 11 (e) Iteraci´on 13 (f) Iteraci´on 15 Figura 5.9: Estado S1 de AVANCE hacia el obst´aculo La figura 5.9 recoge algunas de las im´agenes capturadas durante el estado de avance del dron hacia el obst´aculo. En ellas se puede ver no tan solo como se dirige hacia el sino como se va centrando, es especialmente notable como aumenta su altura con cada periodo, ya que al inicio esta bastante por debajo, aunque tambi´en vemos como oscila de izquierda a derecha. En la ultima imagen es donde el algoritmo capta que la distancia al objeto es menor a 1 metro. Si aplicamos las funciones de c´alculo de lineas y posteriormente extrapolamos para calcular la distancia obtenemos que es exactamente de 0.98 m. El siguiente estado es el de parada y centrado del dron S2, lo que ocurre entre los segundos 16 y 22, teniendo en cuenta que el periodo de cada ejecuci´on es de 1 segundo. 53 Ap´endice A C´odigo del seguimiento de l´ınea con parada en cruces A.1. MainFileTimerCreation 1t= timer ; 2t. StartFcn = @InitialiseTimer ; 3t. TimerFcn = @TimerCallback ; 4t. StopFcn = @StopTimer ; 5t. Period = 1; % Periodo en segundos 6t.TasksToExecute = 60; % Numero de tareas a ejecutar 7t. ExecutionMode = ’fixedRate ’; 8start ( t) ; % start the timer 9wait (t); % blocks the command prompt until timer , t, stops running 10 stop (t); % stops the timer 11 delete (t); % deletes timer object 12 % clear all A.2. InitialiseTimer 1function InitialiseTimer ( src , event ) 2% limpiamos la ventana de comandos : 3clc 4% Cerramos ventanas y limpiamos variables 5close all 6% Mensaje para el usuario 7disp(’Timer initialisation begins ... ’) 8% Topicos 9% Topico sobre los comandos de velocidades 10 % ordenador remoto es publisher mientras que el dron es un subscriber 11 CommandNode = ros2node ("/ cmd_vel ") ; 12 % Topico sobre capturar imagenes del dron , 13 % ordenador remoto es subscriber mientras que el dron es un publisher : 14 ImageNode = ros2node ("/ image_raw "); 15 % Topico sobre informacion de navigacion y actitud del dron 16 % ordenador remoto es subscriber mientras que el dron es un publisher : 17 DataNode = ros2node ("/ flight_data "); 18 % Topico sobre acciones basicas del dron 19 % ordenador remoto es publisher mientras que el dron es un subscriber 20 ActionNode = ros2node ("/ tello_action "); 21 % Todo los topicos se guardan en UserData 22 src . UserData = setfield ( src .UserData ,’CommandNode’, CommandNode ); 23 src . UserData = setfield ( src .UserData ,’ ImageNode ’, ImageNode ); 24 src . UserData = setfield ( src .UserData ,’ DataNode ’, DataNode ); 60 Felipe Gallardo GRETA 25 src . UserData = setfield ( src .UserData ,’ actionNode ’, ActionNode ); 26 % Definicion de servicio y cliente 27 [ client , reqmsg ] = ros2svcclient ( ActionNode ,"/ tello_action " ," tello_msgs / TelloAction"); 28 % guardamos en UserData 29 src . UserData = setfield ( src .UserData ,’client’,client); 30 src . UserData = setfield ( src .UserData ,’reqmsg’,reqmsg); 31 % Topicos a los que se suscribe el ordenador remoto 32 ImageSubscriber = ros2subscriber (ImageNode ,"/ image_raw "); % Se suscribe a las imagenes de la camara 33 DataSubscriber = ros2subscriber ( DataNode ,"/ flight_data "); 34 % Guardamos en los datos de usuario 35 src . UserData = setfield ( src .UserData ,’ ImageSubscriber ’, ImageSubscriber ); 36 src . UserData = setfield ( src .UserData ,’DataSubscriber’,DataSubscriber); 37 % Get Flight Data message 38 DataMessage = receive ( DataSubscriber ,10) ; 39 % Take ( barometric ) drone altitude before the takeoff 40 Floor = DataMessage . baro ; 41 % guardamos en UserData 42 src . UserData . Floor = Floor ; 43 % Creamos variables de estado y contador 44 State = 0; 45 Contador = 0; 46 src . UserData . Contador = Contador ; 47 src . UserData . State = State ; 48 % Topico donde publica el ordenador 49 CommandPublisher = ros2publisher ( CommandNode ,"/ cmd_vel " ," geometry_msgs / Twist ") ; 50 % lo guardamos en UserData 51 src . UserData = setfield ( src .UserData ,’CommandPublisher’,CommandPublisher) ; 52 % Se crea una estructura compatible con mensajes ROS 2: 53 CommandMessage = ros2message ( CommandPublisher ); 54 % Establecemos todos los comandos a 0 antes de despegue 55 CommandMessage . angular .x = 0.0; 56 CommandMessage . angular .y = 0.0; 57 CommandMessage . angular .z = 0.0; 58 CommandMessage . linear .x = 0.0; 59 CommandMessage . linear .y = 0.0; 60 CommandMessage . linear .z = 0.0; 61 % se guarda en UserData 62 src . UserData = setfield ( src .UserData ,’CommandMessage’,CommandMessage); 63 % Creacion de la matriz de navegacion 64 NavigationData = zeros ( src . TasksToExecute ,9) ; 65 % guardamos la matriz en UserData 66 src . UserData = setfield ( src .UserData ,’NavigationData’,NavigationData); 67 % Comando para el despegue 68 reqmsg . cmd = ’takeoff’; 69 [response , status , statustext ] = call( client ,reqmsg ,’Timeout’,10); 70 % todas las velocidades a 0 71 send ( CommandPublisher , CommandMessage ); 72 disp(’ Timer initialisation ended ! ’); 73 % Espera para el usuario para pasar al siguiente callback 74 uiwait(msgbox(’Drone is TAKING OFF . Press OK when HOVERING to move !’,’ Information’,’warn ’)); 75 end 61 Felipe Gallardo GRETA A.3. TimerCallback 1function TimerCallback ( src , event ) 2% Se guarda la informacion de vuelo en la matriz 3RowNumber = src . TasksExecuted ; 4NavigationData = getfield ( src . UserData , ’NavigationData’); 5% Se obtienen publishers , subscribers y messages de UserData 6CommandPublisher = getfield ( src .UserData , ’CommandPublisher’); 7DataSubscriber = getfield ( src . UserData , ’DataSubscriber’); 8ImageSubscriber = getfield ( src .UserData , ’ ImageSubscriber ’); 9CommandMessage = getfield ( src . UserData , ’CommandMessage’); 10 % Se recibe flight data 11 DataMessage = receive ( DataSubscriber ,10) ; 12 % Obtencion de la altitud del barometro 13 Floor = src . UserData . Floor ; 14 % Get the state 15 State = src . UserData . State ; 16 Contador = src . UserData . Contador ; 17 % calculo de altitud del suelo 18 Altitude = DataMessage . baro - Floor ; 19 % Se guarda flight data a matriz 20 NavigationData ( RowNumber ,:) = [ DataMessage .pitch , DataMessage . roll ,... 21 DataMessage.yaw,DataMessage.vgx,DataMessage.vgy,DataMessage.vgz ,... 22 Altitude , State , Contador ]; 23 % Se guarda la matriz en UserData 24 src . UserData = setfield ( src .UserData ,’NavigationData’,NavigationData); 25 % Recibe imagenes del dron 26 ImageMessage = receive ( ImageSubscriber ,10) ; 27 % se convierte a Matlab 720*960*3 RGB matrix 28 ImageRGB = rosReadImage(ImageMessage); 29 NameOfTheFile = strcat ( ’Picture’, mat2str ( src . TasksExecuted ) ,’. jpg ’); 30 imwrite (ImageRGB , NameOfTheFile ) 31 % Llamada a la funcion de calculo de distancia a las lineas 32 [d1 ,d2 ,theta , blocked1 , blocked2 ]= CalculateDistances ( ImageRGB ); 33 % Las distancias se trasladan de mm a metros 34 d1 =(2.13* Altitude ^2 -1.18* Altitude +0.58) * d1 /1000; 35 d2 =(2.13* Altitude ^2 -1.18* Altitude +0.58) * d2 /1000; 36 % Se calcula la curvatura con las medidas relativas a la linea 37 [ curvature ] = CalculateGamma (d1 , theta ); 38 % Vx =0.2 m/s es normalizada 39 Vlineal =.10; 40 VlinealN=0.535*Vlineal+0.110; 41 % Se calcula la velocidad angular en el eje z a partir 42 % de la curvatura y la velocidad lineal en x, se normaliza 43 if curvature >=0 44 Wz =0.548* curvature * Vlineal +0.1455; 45 else 46 Wz =0.5464* curvature * Vlineal -0.1428; 47 end 48 % Se muestran distintos valores en pantalla 49 disp ([ ’d1 ’ ," ",’d2 ’ ," ",’ theta ’ ," ",’ curvature ’ ," ",’blocked’, " ",’ Contador’," ",’State ’ ]) 50 disp ([d1 ,d2 ,theta ,curvature , blocked1 , blocked2 ,src. UserData . Contador , src . UserData . State ]) 51 % Finalmente se evaluan los estados 52 if State == 0 % HOVER 53 if blocked1 && blocked2 54 CommandMessage . angular .z = 0; %-1; % gamma ; % yaw () 55 CommandMessage . linear .x = 0; % endavant / enrera 56 src . UserData . State = 0; 57 elseif blocked2 58 CommandMessage . angular .z = 0; %-1; % gamma ; % yaw () 59 CommandMessage . linear .x = 0; % endavant / enrera 62 Felipe Gallardo GRETA 60 src . UserData . State = 1; 61 else 62 CommandMessage . angular .z = 0; %-1; % gamma ; % yaw () 63 CommandMessage . linear .x = 0; % endavant / enrera 64 src . UserData . State = 2; 65 src . UserData . Contador = Contador + 1; 66 end 67 elseif State == 1 % AVANCE 68 if blocked1 && blocked2 69 CommandMessage . angular .z = 0; %-1; % gamma ; % yaw () 70 CommandMessage . linear .x = 0; % endavant / enrera 71 src . UserData . State = 0; 72 elseif blocked2 73 CommandMessage . angular .z = double ( Wz ); %-1; % gamma ; % yaw () 74 CommandMessage . linear .x = double ( VlinealN ); % endavant / enrera 75 src . UserData . State = 1; 76 else 77 CommandMessage . angular .z = double ( Wz ); %-1; % gamma ; % yaw () 78 CommandMessage . linear .x = double ( VlinealN ); % endavant / enrera 79 src . UserData . Contador = Contador + 1; 80 src . UserData . State = 2; 81 end 82 elseif State == 2 % PARADA 83 if blocked1 && blocked2 84 CommandMessage . angular .z = 0; %-1; % gamma ; % yaw () 85 CommandMessage . linear .x = 0; % endavant / enrera 86 src . UserData . State = 0; 87 elseif blocked2 88 CommandMessage . angular .z = double ( Wz ); %-1; % gamma ; % yaw () 89 CommandMessage . linear .x = double ( VlinealN ); % endavant / enrera 90 src . UserData . State = 1; 91 else 92 Vlineal =0; 93 VlinealN=0.535*Vlineal+0.110; 94 CommandMessage . angular .z = double ( Wz ); %-1; % gamma ; % yaw () 95 if d2 <=0 && d1 <=0 96 if d2 > -0.07 97 if d1 > -0.1 98 CommandMessage . linear .x = double (0) ; 99 CommandMessage . linear .y = double (0) ; 100 src . UserData . State = 3; 101 else 102 CommandMessage . linear .x = double (0) ; 103 CommandMessage . linear .y = double (- VlinealN ); 104 src . UserData . State = 2; 105 end 106 else 107 if d1 > -0.1 108 CommandMessage . linear .x = double ( VlinealN ); % endavant / enrera 109 CommandMessage . linear .y = double (0) ; 110 src . UserData . State = 2; 111 else 112 CommandMessage . linear .x = double ( VlinealN ); 113 CommandMessage . linear .y = double (- VlinealN ); 114 src . UserData . State = 2; 115 end 116 end 117 elseif d2 <=0 && d1 >0 118 if d2 > -0.07 119 if d1 <0.1 120 CommandMessage . linear .x = double (0) ; 121 CommandMessage . linear .y = double (0) ; 63 Felipe Gallardo GRETA 122 src . UserData . State = 3; 123 else 124 CommandMessage . linear .x = double (0) ; 125 CommandMessage . linear .y = double ( VlinealN ); 126 src . UserData . State = 2; 127 end 128 else 129 if d1 <0.1 130 CommandMessage . linear .x = double ( VlinealN ); % endavant / enrera 131 CommandMessage . linear .y = double (0) ; 132 src . UserData . State = 2; 133 else 134 CommandMessage . linear .x = double ( VlinealN ); 135 CommandMessage . linear .y = double ( VlinealN ); 136 src . UserData . State = 2; 137 end 138 end 139 elseif d2 >0 && d1 <=0 140 if d2 <0.07 141 if d1 > -0.1 142 CommandMessage . linear .x = double (0) ; 143 CommandMessage . linear .y = double (0) ; 144 src . UserData . State = 3; 145 else 146 CommandMessage . linear .x = double (0) ; 147 CommandMessage . linear .y = double (- VlinealN ); 148 src . UserData . State = 2; 149 end 150 else 151 if d1 > -0.1 152 CommandMessage . linear .x = double (- VlinealN ); % endavant / enrera 153 CommandMessage . linear .y = double (0) ; 154 src . UserData . State = 2; 155 else 156 CommandMessage . linear .x = double (- VlinealN ); 157 CommandMessage . linear .y = double (- VlinealN ); 158 src . UserData . State = 2; 159 end 160 end 161 elseif d2 >0 && d1 >0 162 if d2 <0.07 163 if d1 <0.1 164 CommandMessage . linear .x = double (0) ; 165 CommandMessage . linear .y = double (0) ; 166 src . UserData . State = 3; 167 else 168 CommandMessage . linear .x = double (0) ; 169 CommandMessage . linear .y = double ( VlinealN ); 170 src . UserData . State = 2; 171 end 172 else 173 if d1 <0.1 174 CommandMessage . linear .x = double (- VlinealN ); 175 CommandMessage . linear .y = double (0) ; 176 src . UserData . State = 2; 177 else 178 CommandMessage . linear .x = double (- VlinealN ); 179 CommandMessage . linear .y = double ( VlinealN ); 180 src . UserData . State = 2; 181 end 182 end 64 Felipe Gallardo GRETA 183 else 184 CommandMessage . linear .x = double (0) ; 185 CommandMessage . linear .y = double (0) ; 186 src . UserData . State = 0; 187 end 188 end 189 elseif State == 3 % ESCAPE 190 if blocked1 && blocked2 191 CommandMessage . angular .z = 0; %-1; % gamma ; % yaw () 192 CommandMessage . linear .x = 0; % endavant / enrera 193 src . UserData . State = 0; 194 elseif blocked2 195 CommandMessage . angular .z = double ( Wz ); %-1; % gamma ; % yaw () 196 CommandMessage . linear .x = double ( VlinealN ); % endavant / enrera 197 src . UserData . State = 1; 198 else 199 CommandMessage . angular .z = double ( Wz ); %-1; % gamma ; % yaw () 200 CommandMessage . linear .x = double ( VlinealN ); % endavant / enrera 201 src . UserData . State = 3; 202 end 203 end 204 send ( CommandPublisher , CommandMessage ); % se envian las velocidades 205 % Se guardan los datos de navegacion en un archivo al finalizar 206 if src . TasksExecuted == src . TasksToExecute 207 save(’NavigationDataRecord.mat’,’NavigationData’); 208 end 209 end A.4. StopTimer 1function StopTimer ( src , event ) 2client = getfield ( src . UserData , ’client’); 3reqmsg = getfield ( src . UserData , ’reqmsg’); 4% Orden de aterrizaje 5reqmsg . cmd = ’land ’; 6[ response2 , status2 , statustext2 ] = call ( client , reqmsg , ’Timeout’,10); 7end 65 Felipe Gallardo GRETA A.5. CalculateDistances 1function [d1 ,d2 , theta , Blocked1 , Blocked2 ]= CalculateDistances ( ImageRGB ) 2% Funcion que calcula las distancias a las lineas vertical 3% y horizontal del suelo 4ImageCR = rgb2ycbcr ( flipud ( ImageRGB )); 5ImageBW = imbinarize ( ImageCR (: ,: ,3) ,.54) ; 6SEV = strel ( ’line ’ ,100 ,90); %30 ,90 7SEH = strel ( ’line ’ ,150 ,0) ; %30, 0 8ImageEV = imerode ( ImageBW , SEV ); 9ImageEH = imerode ( ImageBW , SEH ); 10 subplot (1 ,2 ,1) , imshow ( ImageEV ) 11 subplot (1 ,2 ,2) , imshow ( ImageEH ) 12 P1aV=[]; 13 P2aV=[]; 14 P1bV=[]; 15 P2bV=[]; 16 P1aH=[]; 17 P2aH=[]; 18 P1bH=[]; 19 P2bh=[]; 20 % The translation matrix is: 21 T=[1 0 -480;0 1 360;0 0 1]; 22 23 if sum ( ImageEV (:) ) <=0.005*720*960 24 % no hay suficientes pixeles blancos en la imagen 25 % se bloquea los movimientos del dron 26 Blocked1 =1; 27 Distance = Inf ; 28 Theta = Inf ; 29 P1aV=[]; 30 P2aV=[]; 31 else 32 % Si hay suficientes pixeles blancos en la imagen 33 Blocked1 =0; 34 Column =[]; 35 Row =0; 36 while isempty ( Column ) || isnan ( Column ) && Row <=710 37 Row=Row +10; 38 Column = round ( mean ( find ( ImageEV ( Row ,:) )) ); 39 end 40 if ~ isempty ( Column ) 41 P1aV =[ Column - Row 1] ’; 42 else 43 P1aV=[]; 44 end 45 Column =[]; 46 Row =730; 47 while isempty ( Column ) || isnan ( Column ) && Row >10 48 Row=Row -10; 49 Column = round ( mean ( find ( ImageEV ( Row ,:) )) ); 50 end 51 if ~ isempty ( Column ) 52 P2aV =[ Column - Row 1] ’; 53 else 54 P2aV=[]; 55 end 56 end 57 % Points P1 ,P2 in the new reference system are: 58 if ~ isempty ( P1aV ) 59 P1bV =T* P1aV ; 60 end 61 if ~ isempty ( P2aV ) 62 P2bV =T* P2aV ; 66 Felipe Gallardo GRETA 63 end 64 if isempty ( P1bV ) || isempty ( P2bV ) 65 d1= Inf ; 66 theta = Inf ; 67 Blocked1 =1; 68 else 69 LV = cross ( P1bV , P2bV ); 70 d1= LV (3) / sqrt (LV (1) ^2+ LV (2) ^2) ; 71 theta = atan2d (-LV (2) ,LV (1) ); 72 Blocked1 =0; 73 end 74 if sum ( ImageEH (:) ) <=0.002*720*960 % size 720 p AR 4:3 ( less than 2 %) 75 % not enough white pixels 76 % calculations and drone movements are blocked 77 Blocked2 =1; 78 Distance = Inf ; 79 P1aH=[]; 80 P2aH=[]; 81 else 82 % when enough white pixels in the picture ... 83 Blocked2 =0; 84 Column=0; 85 Row =[]; 86 while isempty ( Row ) || isnan (Row ) && Column <=950 87 Column=Column+10; 88 Row = round ( mean ( find ( ImageEH (: , Column )))); 89 end 90 if ~ isempty ( Row ) 91 P1aH =[ Column - Row 1] ’; 92 else 93 P1aH=[]; 94 end 95 Row =[]; 96 Column =970; 97 while isempty ( Row ) || isnan (Row ) && Column >10 98 Column = Column -10; 99 Row = round ( mean ( find ( ImageEH (: , Column )))); 100 end 101 if ~ isempty ( Column ) 102 P2aH =[ Column - Row 1] ’; 103 else 104 P2aH=[]; 105 end 106 end 107 % Points P1 ,P2 in the new reference system are: 108 if ~ isempty ( P1aH ) 109 P1bH =T* P1aH ; 110 end 111 if ~ isempty ( P2aH ) 112 P2bH =T* P2aH ; 113 end 114 if isempty ( P1bH ) || isempty ( P2bH ) 115 d2= Inf ; 116 Blocked2 =1; 117 else 118 LH = cross ( P1bH , P2bH ); 119 d2= LH (3) / sqrt (LH (1) ^2+ LH (2) ^2) ; 120 Blocked2 =0; 121 end 67 Felipe Gallardo GRETA A.6. CalculateGamma 1function [ curvatura ] = CalculateGamma (d1 , theta ) 2% Calculate Gamma calcula el valor de la curvatura en funcion de las distancias relativas 3% y el angulo a la misma 4L =150; %estimacion de la distancia que le tomara al dron corregir la trayectoria 5if d1 == Inf || theta == Inf 6curvatura =0; 7else 8% d1= abs ( d1); 9% theta = abs ( theta ); 10 if abs(L)>abs (d1) 11 curvatura =(2/ L ^2) *( d1 * cosd ( theta ) -sqrt (L^2 - d1 ^2) * sind ( theta )); 12 else 13 curvatura =0; 14 end 15 end 16 curvatura = curvatura *1.0; 17 end 68 Ap´endice B C´odigo del seguimiento de cuadrado en el suelo Aqu´ı el ´unico c´odigo que cambia es el callback B.1. TimerCallback 1function TimerCallback ( src , event ) 2% Se guardan los datos de navegacion 3RowNumber = src . TasksExecuted ; 4NavigationData = getfield ( src . UserData , ’NavigationData’); 5% Se obtienen publishers , subscribers y messages de UserData 6CommandPublisher = getfield ( src .UserData , ’CommandPublisher’); 7DataSubscriber = getfield ( src . UserData , ’DataSubscriber’); 8ImageSubscriber = getfield ( src .UserData , ’ ImageSubscriber ’); 9CommandMessage = getfield ( src . UserData , ’CommandMessage’); 10 % Recibe flight data 11 DataMessage = receive ( DataSubscriber ,10) ; 12 % Se obtiene la altitud del barometro 13 Floor = src . UserData . Floor ; 14 % Se obtiene el estado y contador 15 State = src . UserData . State ; 16 Contador = src . UserData . Contador ; 17 % Se obtiene el angulo de yaw inicial para el giro 18 InitialYaw = src . UserData . InitialYaw ; 19 % y calculamos la altura del suelo 20 Altitude = DataMessage . baro - Floor ; 21 % Guardamos datos de vuelo en la matriz de navegacion 22 NavigationData ( RowNumber ,:) = [ DataMessage .pitch , DataMessage . roll ,... 23 DataMessage.yaw,DataMessage.vgx,DataMessage.vgy,DataMessage.vgz ,... 24 Altitude , State , Contador ]; 25 % Se guarda la matriz en UserData 26 src . UserData = setfield ( src .UserData ,’NavigationData’,NavigationData); 27 % Recibimos imagenes del dron 28 ImageMessage = receive ( ImageSubscriber ,10) ; 29 % y convertimos en Matlab 720*960*3 RGB matriz 30 ImageRGB = rosReadImage(ImageMessage); 31 NameOfTheFile = strcat ( ’Picture’, mat2str ( src . TasksExecuted ) ,’. jpg ’); 32 imwrite (ImageRGB , NameOfTheFile ) 33 % Llamada a funcion que calcula las distancias a las lineas del suelo 34 [d1 ,d2 ,theta , blocked1 , blocked2 ]= CalculateDistances ( ImageRGB ); 35 % Las distancias se transforman de pixeles a mm y a metros 36 d1 =(2.13* Altitude ^2 -1.18* Altitude +0.58) * d1 /1000; 69 Felipe Gallardo GRETA 98 CommandMessage . linear .y = double (0) ; 99 CommandMessage . linear .z = double (0) ; 100 src . UserData . State = 1; 101 else 102 CommandMessage . linear .y = double (- VlinealN ); 103 CommandMessage . linear .z = double (0) ; 104 src . UserData . State = 1; 105 end 106 else 107 if dV > -0.2 108 CommandMessage . linear .y = double (0) ; 109 CommandMessage . linear .z = double ( VlinealN ); 110 src . UserData . State = 1; 111 else 112 CommandMessage . linear .y = double (- VlinealN ); 113 CommandMessage . linear .z = double ( VlinealN ); 114 src . UserData . State = 1; 115 end 116 end 117 elseif dH <=0 && dV >0 118 if dH > -0.3 119 if dV <0.2 120 CommandMessage . linear .y = double (0) ; 121 CommandMessage . linear .z = double (0) ; 122 src . UserData . State = 1; 123 else 124 CommandMessage . linear .y = double ( VlinealN ); 125 CommandMessage . linear .z = double (0) ; 126 src . UserData . State = 1; 127 end 128 else 129 if dV <0.2 130 CommandMessage . linear .y = double (0) ; 131 CommandMessage . linear .z = double ( VlinealN ); 132 src . UserData . State = 1; 133 else 134 CommandMessage . linear .y = double ( VlinealN ); 135 CommandMessage . linear .z = double ( VlinealN ); 136 src . UserData . State = 1; 137 end 138 end 139 elseif dH >0 && dV <=0 140 if dH <0.3 141 if dV > -0.2 142 CommandMessage . linear .y = double (0) ; 143 CommandMessage . linear .z = double (0) ; 144 src . UserData . State = 1; 145 else 146 CommandMessage . linear .y = double (- VlinealN ); 147 CommandMessage . linear .z = double (0) ; 148 src . UserData . State = 1; 149 end 150 else 151 if dV > -0.2 152 CommandMessage . linear .y = double (0) ; 153 CommandMessage . linear .z = double (- VlinealN ); 154 src . UserData . State = 1; 155 else 156 CommandMessage . linear .y = double (- VlinealN ); 157 CommandMessage . linear .z = double (- VlinealN ); 158 src . UserData . State = 1; 159 end 160 end 76 Felipe Gallardo GRETA 161 elseif dH >0 && dV >0 162 if dH <0.3 163 if dV <0.2 164 CommandMessage . linear .y = double (0) ; 165 CommandMessage . linear .z = double (0) ; 166 src . UserData . State = 1; 167 else 168 CommandMessage . linear .y = double ( VlinealN ); 169 CommandMessage . linear .z = double (0) ; 170 src . UserData . State = 1; 171 end 172 else 173 if dV <0.2 174 CommandMessage . linear .y = double (0) ; 175 CommandMessage . linear .z = double (- VlinealN ); 176 src . UserData . State = 1; 177 else 178 CommandMessage . linear .y = double ( VlinealN ); 179 CommandMessage . linear .z = double (- VlinealN ); 180 src . UserData . State = 1; 181 end 182 end 183 else 184 CommandMessage . linear .x = double (0) ; 185 CommandMessage . linear .y = double (0) ; 186 src . UserData . State = 0; 187 end 188 end 189 elseif State == 2 % PARADA Y CENTRADO . Objeto a menos de un metro , centramos 190 if blocked1 && blocked2 191 CommandMessage . angular .z = 0; 192 CommandMessage . linear .x = 0; 193 CommandMessage . linear .y = 0; 194 CommandMessage . linear .z = 0; 195 src . UserData . State = 0; 196 else 197 Vlineal =0; 198 VlinealN=0.535*Vlineal+0.110; 199 CommandMessage . linear .x = double (0) ; 200 if dH <=0 && dV <=0 201 if dH > -0.05 202 if dV > -0.05 203 CommandMessage . linear .y = double (0) ; 204 CommandMessage . linear .z = double (0) ; 205 src . UserData . InitialDistance = X; 206 src . UserData . InitialYaw = Yaw ; 207 disp (src. UserData . InitialYaw ) 208 src . UserData . State = 3; 209 else 210 CommandMessage . linear .y = double (- VlinealN ); 211 CommandMessage . linear .z = double (0) ; 212 src . UserData . State = 2; 213 end 214 else 215 if dV > -0.05 216 CommandMessage . linear .y = double (0) ; 217 CommandMessage . linear .z = double ( VlinealN ); 218 src . UserData . State = 2; 219 else 220 CommandMessage . linear .y = double (- VlinealN ); 221 CommandMessage . linear .z = double ( VlinealN ); 222 src . UserData . State = 2; 77 Felipe Gallardo GRETA 223 end 224 end 225 elseif dH <=0 && dV >0 226 if dH > -0.05 227 if dV <0.05 228 CommandMessage . linear .y = double (0) ; 229 CommandMessage . linear .z = double (0) ; 230 src . UserData . InitialDistance = X; 231 src . UserData . InitialYaw = Yaw ; 232 disp (src. UserData . InitialYaw ) 233 src . UserData . State = 3; 234 else 235 CommandMessage . linear .y = double ( VlinealN ); 236 CommandMessage . linear .z = double (0) ; 237 src . UserData . State = 2; 238 end 239 else 240 if dV <0.05 241 CommandMessage . linear .y = double (0) ; 242 CommandMessage . linear .z = double ( VlinealN ); 243 src . UserData . State = 2; 244 else 245 CommandMessage . linear .y = double ( VlinealN ); 246 CommandMessage . linear .z = double ( VlinealN ); 247 src . UserData . State = 2; 248 end 249 end 250 elseif dH >0 && dV <=0 251 if dH <0.05 252 if dV > -0.05 253 CommandMessage . linear .y = double (0) ; 254 CommandMessage . linear .z = double (0) ; 255 src . UserData . State = 3; 256 src . UserData . InitialDistance = X; 257 src . UserData . InitialYaw = Yaw ; 258 disp (src. UserData . InitialYaw ) 259 else 260 CommandMessage . linear .y = double (- VlinealN ); 261 CommandMessage . linear .z = double (0) ; 262 src . UserData . State = 2; 263 end 264 else 265 if dV > -0.05 266 CommandMessage . linear .y = double (0) ; 267 CommandMessage . linear .z = double (- VlinealN ); 268 src . UserData . State = 2; 269 else 270 CommandMessage . linear .y = double (- VlinealN ); 271 CommandMessage . linear .z = double (- VlinealN ); 272 src . UserData . State = 2; 273 end 274 end 275 elseif dH >0 && dV >0 276 if dH <0.05 277 if dV <0.05 278 CommandMessage . linear .y = double (0) ; 279 CommandMessage . linear .z = double (0) ; 280 src . UserData . InitialDistance = X; 281 src . UserData . InitialYaw = Yaw ; 282 disp (src. UserData . InitialYaw ) 283 src . UserData . State = 3; 284 285 else 78 Felipe Gallardo GRETA 286 CommandMessage . linear .y = double ( VlinealN ); 287 CommandMessage . linear .z = double (0) ; 288 src . UserData . State = 2; 289 end 290 else 291 if dV <0.05 292 CommandMessage . linear .y = double (0) ; 293 CommandMessage . linear .z = double (- VlinealN ); 294 src . UserData . State = 2; 295 else 296 CommandMessage . linear .y = double ( VlinealN ); 297 CommandMessage . linear .z = double (- VlinealN ); 298 src . UserData . State = 2; 299 end 300 end 301 end 302 end 303 elseif State == 3 % EVASION . Evader el obstaculo por odometria 304 Vlineal =0.2; 305 VlinealN=0.535*Vlineal+0.110; 306 R = src . UserData . InitialDistance + 0.2; % 0.2 m es la distancia al centro de la caja 307 curvature = 1/R; 308 % Se calcula la velocidad angular 309 Wz =0.548* curvature * Vlineal +0.1455; 310 311 if abs ( angdiff ( deg2rad ( double ( Yaw )) ,deg2rad ( double ( src . UserData . InitialYaw )))) < 0.9* pi 312 CommandMessage . angular .z = double (- Wz ); 313 CommandMessage . linear .x = 0; 314 CommandMessage . linear .y = double ( VlinealN ); 315 CommandMessage . linear .z = 0; 316 src . UserData . State = 3; 317 else 318 CommandMessage . angular .z = 0; 319 CommandMessage . linear .x = 0; 320 CommandMessage . linear .y = 0; 321 CommandMessage . linear .z = 0; 322 src . UserData . InitialYaw = Yaw ; 323 disp (src. UserData . InitialYaw ) 324 src . UserData . State = 4; 325 end 326 elseif State == 4 % ESCAPE . El dron gira para volve a la trayectoria original 327 if abs ( angdiff ( deg2rad ( double ( Yaw )) ,deg2rad ( double ( src . UserData . InitialYaw )))) < 0.95* pi 328 CommandMessage . angular .z = double (0.5) ; 329 CommandMessage . linear .x = 0; 330 CommandMessage . linear .y = 0; 331 CommandMessage . linear .z = 0; 332 src . UserData . State = 4; 333 else 334 CommandMessage . angular .z = 0; 335 CommandMessage . linear .x = 0; 336 CommandMessage . linear .y = 0; 337 CommandMessage . linear .z = 0; 338 src . UserData . State = 0; 339 end 340 end 341 342 send ( CommandPublisher , CommandMessage ); 343 344 if src . TasksExecuted == src . TasksToExecute 79 Felipe Gallardo GRETA 345 save(’NavigationDataRecord.mat’,’NavigationData’); 346 end 347 end C.2. CleanImages 1function [ VerticalImage , HorizontalImage ] = CleanImages ( ImageRGB ) 2% CleanImages devuelve dos imagenes limpias , horizontal y vertical de una imagen RGB 3% Se binariza la imagen luego de convertirla en capas de crominancia 4% analizando la capa azul , y luego de usar diferentes elementos estructurales 5% para las lineas horizontal y vertical para devolver imagenes erosionadas y 6% dilatadas limpiando cualquier rastro fuera del obstaculo 7ImageCR = rgb2ycbcr ( ImageRGB ); 8ImageBW = imbinarize ( ImageCR (: ,: ,2) ,.56) ; 9SEV = strel ( ’line ’ ,40 ,90) ; 10 SEH = strel ( ’line ’ ,40,0); 11 ImageEV = imerode ( ImageBW , SEV ); 12 VerticalImage = imdilate ( ImageEV , SEV ); 13 ImageEH = imerode ( ImageBW , SEH ); 14 HorizontalImage = imdilate ( ImageEH , SEH ); 15 16 imshow ( ImageBW ) 17 end C.3. CalculateLenghts 1function [ LenV , LenH ] = CalculateLengths ( ImageEV , ImageEH ) 2%% Calculo distancia de izquierda a derecha 3 4Row = []; 5Column = 0; 6LenH = 0; 7 8while isempty ( Row ) && Column <960 9Column = Column + 1; 10 Row = find ( ImageEH (: , Column )); 11 while ~ isempty ( Row ) && Column <960 12 Column = Column + 1; 13 Row = find ( ImageEH (: , Column )); 14 LenH = LenH + 1; 15 end 16 end 17 18 19 %% Calculo distancia de abajo a arriba 20 21 Column =[]; 22 Row =720; 23 LenV = 0; 24 25 while isempty ( Column ) && Row > 1 26 Row = Row - 1; 27 Column = find ( ImageEV ( Row ,:) ); 28 while ~ isempty ( Column ) && Row > 1 29 Row = Row - 1; 30 Column = find ( ImageEV (Row ,:) ); 31 LenV = LenV + 1; 32 end 80 Felipe Gallardo GRETA 33 end 34 end C.4. CalculateDistances 1function [d1 ,d2 , Blocked1 , Blocked2 ]= CalculateDistances (ImageEV , ImageEH ) 2P1aV=[]; 3P2aV=[]; 4P1bV=[]; 5P2bV=[]; 6P1aH=[]; 7P2aH=[]; 8P1bH=[]; 9P2bh=[]; 10 % Matriz de traslacion 11 T=[1 0 -480;0 1 360;0 0 1]; 12 13 % Comienzo del calculo de la distancia a la linea vertical 14 15 if sum ( ImageEV (:) ) <=0.0001*720*960 16 % no hay suficientes pixeles blancos 17 % calculos y movimientos del dron bloqueados 18 Blocked1 =1; 19 Distance = Inf ; 20 21 P1aV=[]; 22 P2aV=[]; 23 else 24 % cuando hay suficientes pixeles blancos 25 Blocked1 =0; 26 Column =[]; 27 Row =0; 28 29 %Encontramos el primer punto de la recta vertical de la cruz 30 while isempty ( Column ) || isnan ( Column ) && Row <=710 31 Row=Row +10; 32 Column = round ( mean ( find ( ImageEV ( Row ,:) )) ); 33 end 34 if ~ isempty ( Column ) 35 P1aV =[ Column - Row 1] ’; 36 else 37 P1aV=[]; 38 end 39 40 %Encontramos el punto inferior de la recta vertical de la cruz 41 Column =[]; 42 Row =730; 43 while isempty ( Column ) || isnan ( Column ) && Row >10 44 Row=Row -10; 45 Column = round ( mean ( find ( ImageEV ( Row ,:) )) ); 46 end 47 if ~ isempty ( Column ) 48 P2aV =[ Column - Row 1] ’; 49 else 50 P2aV=[]; 51 end 52 end 53 % Points P1 ,P2 in the new reference system are: 54 % Transformamos los dos puntos a un sistema de referencia en el centro de 55 % la imagen 56 57 if ~ isempty ( P1aV ) 81 Felipe Gallardo GRETA 58 P1bV =T* P1aV ; 59 end 60 if ~ isempty ( P2aV ) 61 P2bV =T* P2aV ; 62 end 63 64 % Calculamos el valor de las distancia del " centro del drone " a la vertical 65 66 if isempty ( P1bV ) || isempty ( P2bV ) 67 d1= Inf ; 68 69 Blocked1 =1; 70 else 71 LV = cross ( P1bV , P2bV ); 72 d1= LV (3) / sqrt (LV (1) ^2+ LV (2) ^2) ; 73 % theta = atan2d (-LV (2) ,LV (1) ); 74 Blocked1 =0; 75 end 76 77 % Comienzo del calculo de la distancia a la linea horizontal de la cruz 78 79 if sum ( ImageEH (:) ) <=0.0001*720*960 % size 720 p AR 4:3 (less than 2 %) 80 % not enough white pixels 81 % calculations and drone movements are blocked 82 Blocked2 =1; 83 Distance = Inf ; 84 P1aH=[]; 85 P2aH=[]; 86 else 87 % when enough white pixels in the picture ... 88 Blocked2 =0; 89 90 % Encontramos el primer punto de la izquierda de la recta horizontal de la cruz 91 92 Column=0; 93 Row =[]; 94 while isempty ( Row ) || isnan (Row ) && Column <=950 95 Column=Column+10; 96 Row = round ( mean ( find ( ImageEH (: , Column )))); 97 end 98 if ~ isempty ( Row ) 99 P1aH =[ Column - Row 1] ’; 100 else 101 P1aH=[]; 102 end 103 104 % Encontramos el punto de la derecha de la recta horizontal de la cruz 105 106 Row =[]; 107 Column =970; 108 while isempty ( Row ) || isnan (Row ) && Column >10 109 Column = Column -10; 110 Row = round ( mean ( find ( ImageEH (: , Column )))); 111 end 112 if ~ isempty ( Column ) 113 P2aH =[ Column - Row 1] ’; 114 else 115 P2aH=[]; 116 end 117 end 118 119 % Points P1 ,P2 in the new reference system are: 82 Felipe Gallardo GRETA 120 % Transformamos los dos puntos a un sistema de referencia en el centro de 121 % la imagen 122 123 if ~ isempty ( P1aH ) 124 P1bH =T* P1aH ; 125 end 126 if ~ isempty ( P2aH ) 127 P2bH =T* P2aH ; 128 end 129 % Calculamos el valor de las distancia del " centro del drone " a la 130 % horizontal de la cruz 131 132 if isempty ( P1bH ) || isempty ( P2bH ) 133 d2= Inf ; 134 Blocked2 =1; 135 else 136 LH = cross ( P1bH , P2bH ); 137 d2= LH (3) / sqrt (LH (1) ^2+ LH (2) ^2) ; 138 Blocked2 =0; 139 end 83 Bibliograf´ıa [1] Albert Masip i Juli´an Cayero. “Flight Mission State Machines”. En: (2023). [2] Mart´ı Som´o Ribera. Estudi pel control indoor d’actitud d’un hexarotor. Trabajo final de grado. Universitat Polit`ecnica de Catalunya, 2021. [3] Stefania-Denisa Bocu. “Study on the manoeuvres of a small scale drone applied on the physic evaluation of physically disabled pediatric patients”. Trabajo final de grado. Universitat Polit`ecnica de Catalunya, 2022. [4] El Impacto de la Tecnolog´ıa anti colisi´on en drones.url:https://idc.apddrones. com/noticias/seguridad-en-drones-el-impacto-de-la-tecnologia-anticolision/. [5] Siji Chen et al. Learning Decentralized Flocking Controllers with Spatio-Temporal Graph Neural Network. 2023. arXiv: 2309.17437. [6] Adil Farooq et al. “Vision-Based Navigation for a Small-Sized Drone: A Simple AI Strategy at IMAV22”. En: 2024 IEEE International Conference on Cybernetics and Innovations (ICCI). 2024, p´ags. 1-5. doi:10.1109/ICCI60780.2024.10532299. [7] Nivitha Aswini y Uma V. “Obstacle avoidance and distance measurement for unmanned aerial vehicles using monocular vision”. En: International Journal of Electrical and Computer Engineering (IJECE) 9 (oct. de 2019), p´ag. 3504. doi:10.11591/ ijece.v9i5.pp3504-3511. [8] Xiao Q Dong T Zhang Y y Huang Y. “The Control Method of Autonomous Flight Avoidance Barriers of UAVs in Confined Environments”. En: Sensors (Basel) (2023). doi:10.3390/s23135896. [9] Albert Masip i Juli´an Cayero. “Setup”. En: (2022). [10] Albert Masip i Juli´an Cayero. “Basic Manouvers and Path Tracking”. En: (2022). [11] Albert Masip i Juli´an Cayero. “Image Processing”. En: (2022). [12] DJI tienda.url:https://store.dji.com/es/product/telloboostcombo? vid=45701. [13] ¿Cu´anto gana un Ingeniero Aeron´autico en 2021? ¿Y un T´ecnico? url:https: //www.cesurformacion.com/blog/cuanto-gana-un-ingeniero-aeronauticoen-2021-y-un-tecnico. 84