trabajo fin de master extensión de las capacidades de...

Máster en Ingeniería Informática

Universidad Politécnica de Madrid

Escuela Técnica Superior deIngenieros Informáticos

TRABAJO FIN DE MASTER

Extensión de las capacidades de operación de robotsaéreos en el entorno Aerostack

Autor: Rafael Artiñano Muñoz

Director: Martín Molina González

MADRID, JUNIO DE 2019

ÍNDICE

1. Introducción 1

1.1. Objetivos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

2. Descripción del problema 3

2.1. Robótica Aérea . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.2. ROS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.3. Aerostack . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2.4. Comportamientos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2.4.1. Motion-planning en Visual Servoing . . . . . . . . . . . . . . . . 7

2.4.2. Motion-planning . . . . . . . . . . . . . . . . . . . . . . . . . . 11

3. Diseño de la solucion 14

3.1. Auto-localización . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

3.1.1. Comportamiento "Self Localize by odometry" . . . . . . . . . . . 15

3.2. Control de movimiento sobre mapas geométricos . . . . . . . . . . . . . 18

3.2.1. Comportamiento "Generate path in geometric map" . . . . . . . . 18

3.2.2. Comportamiento "Follow path in geometric map" . . . . . . . . . 22

3.3. Seguimiento de imagenes . . . . . . . . . . . . . . . . . . . . . . . . . . 28

3.3.1. Comportamiento "Select object image" . . . . . . . . . . . . . . 28

3.3.2. Comportamiento "Follow object image" . . . . . . . . . . . . . . 31

3.4. Interacción con el operador . . . . . . . . . . . . . . . . . . . . . . . . . 35

3.4.1. Comportamiento "Request operator confirm recognition" . . . . . 35

4. Validación y pruebas 39

4.1. Entornos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

4.1.1. Simulador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

4.1.2. ARDrone . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42

4.2. Pruebas realizadas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

4.2.1. Auto-localización . . . . . . . . . . . . . . . . . . . . . . . . . . 43

I

4.2.2. Control de movimiento sobre mapas geométricos . . . . . . . . . 44

4.2.3. Seguimiento de imágenes . . . . . . . . . . . . . . . . . . . . . 44

4.2.4. Interacción con operador . . . . . . . . . . . . . . . . . . . . . . 45

5. Conclusiones 47

6. Referencias 48

II

ÍNDICE DE FIGURAS

1. Capas de Aerostack . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

2. Diagrama de la capa reactiva . . . . . . . . . . . . . . . . . . . . . . . . 6

3. Diagrama UML de behavior generico . . . . . . . . . . . . . . . . . . . 15

4. Diagrama del comportamiento Self Localize By odometry . . . . . . . . 16

5. Diagrama del comportamiento Generate Path in Geometric Map . . . . . 19

6. Diagrama del comportamiento Follow Path in Geometric Map . . . . . . 23

7. Diagrama del comportamiento Select Object Image . . . . . . . . . . . . 29

8. Diagrama del comportamiento Follow Object Image . . . . . . . . . . . . 32

9. Diagrama del comportamiento Request Operator Confirm Recognition . . 36

10. Interfaz del CLI . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

11. Captura de RVIZ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42

12. Foto del ARDrone . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

ÍNDICE DE TABLAS

ÍNDICE DE TABLAS1

Algorithm:ownSetUp . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

Algorithm:ownStart . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17



Algorithm:ownRun . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20




Algorithm:OwnCheckActivationsConditions . . . . . . . . . . . . . . . . . . . 26

Algorithm:societyPoseCallback . . . . . . . . . . . . . . . . . . . . . . . . . . 26

Algorithm:obstaclesCallback . . . . . . . . . . . . . . . . . . . . . . . . . . . 26


III



Algorithm:ownCheckActivationConditions . . . . . . . . . . . . . . . . . . . 30

Algorithm:boundingBoxCallback . . . . . . . . . . . . . . . . . . . . . . . . . 31








Algorithm:ownStop . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

2. tabla de pruebas de comportamientos de Auto-localización . . . . . . . . 43

3. tabla de pruebas de comportamientos de control de movimiento en mapas

geométricos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

4. tabla de pruebas de comportamientos de seguimiento de imágenes . . . . 45

5. tabla de pruebas de comportamientos de interacción con el operador . . . 45

IV

Resumen

Este documento describe los estudios realizados sobre las distintas técni-

cas de planificación de movimiento, interacción con el usuario y lectura de la

información de los sensores para la creación de comportamientos que ayudan

a la mejora en estos aspectos. Se hablará del diseño, implementación y las

distintas pruebas realizadas sobre simuladores y drones reales.

Palabras clave: Aerostack, Comportamientos.

Abstract

This document describes the studies over the different tecniques of plan-

ification of movement, interaction with the user and information obtained

from the drone sensors for the development of behaviors that helps with the

improvement in those aspects. This document will describe the design, im-

plementation and the tests performed with simulators and real drones.

Keywords: Aerostack, Comportamientos.

V

1. INTRODUCCIÓN

En la sociedad cada vez más informatizada, cada vez más se busca que se hagan las cosas

de la manera más automática posibles. Por ello aumenta el auge de todo aquello que tenga

cierto automatismo. En la actualidad es cada vez más común encontrarse algún elemento

que tenga un algoritmo de inteligencia artificial con el fin de ayudarnos en labores con las

que tenemos problemas debido a la complejidad de hacerlo por nosotros mismos o del vo-

lumen que se tendría que manejar. En este caso profundizaremos en vehículos autónomos

y dentro de estos los aéreos.

En la Universidad Politécnica de Madrid se desarrolló y actualmente se mantiene accesi-

ble un entorno software de uso libre, denominado Aerostack, que tiene como fin ayudar al

desarrollo de robots aéreos autónomos. El presente trabajo ha tenido como fin extender las

capacidades que ofrece dicho entorno para ciertos comportamientos autónomos de robots

aéreos.

Para ello este trabajo se centra en esa vertiente, donde se intenta aumentar el número

y complejidad de las acciones automáticas que realizar, permitiendo al usuario mayo-

res posibilidades a la hora de realizar tareas complejas, permitiendo distintos modos de

realización o por distintos métodos para adaptarse a toda situación.

Este se divide en 3 grandes partes:

En la primera permitiremos al usuario, planificar rutas usando los datos obtenidos de los

sensores y establecer rutas geométricas, intentando alcanzar el objetivo de la manera más

eficiente posible. Para ello planificaremos las rutas de forma eficiente y comprobaremos

que la ruta a seguir, es válida, ya que es necesario saber si la trayectoria generada sigue

siendo o no valida mientras esta siga en curso.

En la segunda se expondrá sobre un poco de interacción hombre-máquina, para pedir al

usuario confirmación en casos donde el nivel de incertidumbre sea alto, o donde el usuario

requiera confirmar una acción/observación.

En la tercera y última se hablará de auto localización y las funciones que permiten al

1

dron saber su posición, esta se sabe que contiene un cierto error, así que se pueden llevar

a cabo otras técnicas para calcular la posición actual y hacer un control del error tanto

en componentes aleatorias como en elementos de cálculo no calibrados. En este caso

usaremos el altímetro y acelerómetro del vehículo aéreo y la cámara para realizar el ajuste

de la posición captada con la cámara, para realizar un cálculo más robusto, aunque no libre

de errores.

1.1. Objetivos

Los objetivos del presente trabajo son los siguientes:

1. Análisis de capacidades de los robots aéreos: Con este objetivo se intenta conseguir una

visión del modo de funcionamiento de este tipo de robots.

2. Diseño de nuevos algoritmos para la mejora o ampliación de capacidades actuales:

Se buscan nuevas soluciones que ayuden a la mejora y ampliación de comportamiento

existentes que se veian faltos de cierta funcionalidad o que se quería extender.

3. Diseño de soluciones integradas al código ya desarrollado: Se intenta que la nueva

solución no sea disruptiva con el sistema actual.

4. Programación de las soluciones: Se quiere que las soluciones planteadas tengan un

resultado verificable para asegurarnos que aquello que planteamos pueda ser evaluado.

5. Evaluación y pruebas del funcionamiento de las soluciones: Se busca comprobar que los

algoritmos son correctos así como el código desarrollados también es correcto. También

verificamos que estos sistemas se comportan como deben en sistemas reales.

2

2. DESCRIPCIÓN DEL PROBLEMA

Para poder entender la solución planteada, hablaremos de la robotica aerea e introducire-

mos las herramientas y los distintos elementos dentro de estas.

2.1. Robótica Aérea

La robótica es un area extensa donde existen muchos campos, desde robots manejados de

forma remota, a robots manejados de forma autónoma. A su vez en paralelo, estos pueden

ser de distinto ámbito, desde robots de tierra, voladores, submarinos, que se desplazan por

la superficie del agua, incluso mixtos.

En este trabajo nos centramos en la robotica aérea y para ser más concreto en la autonoma.

El objetivo de esta parte de la robotica es que el vehículo aereo, sea capaz de realizar sus

acciones de manera autónoma dada solo la misión a realizar. Donde este tome decisiones

o incluso notifique al usuario observaciones o datos utiles para el mismo. También estos

datos pueden ser utiles para el dron u otros drones, por lo tanto es interesante la inclusión

de un sistema de comunicación para con otros drones o con usuarios.

Por tanto en este tipo de sistemas, por el momento se intenta que el dron sea capaz de

recibir una acción a realizar, donde puede ser desde muy generica a muy concreta y sea

capaz de llevarla a cabo.

Para este trabajo se han realizado comportamientos que serán integrados en una herra-

mienta llamada Aerostack.

Para poder introducir Aerostack primero debemos introducir ROS.

2.2. ROS

ROS es un middleware orientado a facilitar programación de modulos mediante una ar-

quitectura editor/subscriptor[1].

3

Este middleware cuenta con distintos componentes:

-topics: canales de comunicación de mensajes, los modulos se suscriben o publican en

estos para ayudar en la comunicación.

-modulos: son los codigos a ejecutar, estos ejecutan la funcionalidad y se comunican por

topics con otros modulos. Estos a su vez pueden ofrecer o consumir servicios proporcio-

nados por otros modulos.

-servicios: estos ofrecen una funcionalidad a otros modulos que reciben una entrada y

proporcionan una respuesta. Este tipo de elemento es util si se quiere utilizar una parte

concreta de un modulo sin que este interaccione de forma directa a traves del hilo princi-

pal.

Los lenguajes que ROS soporta son C++ y python aunque por rendimiento los modulos

se han implementado en C++.

Todos ellos se combinan facilitando la labor de comunicación entre procesos. Los cuales

son usados en la implementación de los distintos comportamientos a presentar y parte

integral del sistema que intentamos mejorar: Aerostack.

2.3. Aerostack

Aerostack es un sistema desarrollado en la universidad Politécnica de Madrid, para ser

más concretos en el grupo cvar, que permite la actuación autónoma de drones[2].

Este sistema permite realizar misiones para los drones que posteriormente llevarán a cabo.

Aerostack cuenta con distintas capas1 entre las cuales podemos distinguir:

1.Capa Reactiva: capa encargada de la recogida de datos, y de la activación de los distintos

actuadores hardware.

2.Capa ejecutiva: capa encargada de las acciones realizadas en la capa reactiva, esta capa

es de mayor nivel por lo tanto cuenta con más información y puede realizar operaciones

más complejas que requieran varios componentes.

4

3.Capa deliberativa: capa encargada de acciones de mayor nivel que la anterior, en esta

capa ya se realiza un plan y se manda a la capa ejecutiva las acciones para llevarlo a cabo.

4.Capa reflectiva: capa encargada de observar el estado del dron e indicar a la capa de-

liberativa este y ver si es necesario realizar alguna modificación al plan actual. Si lo

pusieramos en terminos humanos este sería el sistema nervioso o receptores de estimu-

los(positivos y negativos).

5. Capa social: Capa que permite la interacción del dron. Esta se realiza con el usuario o

con otro dron, con el fin de obtener información que le ayude a completar la misión.

Figura 1: Capas de Aerostack

Dentro de la capa ejecutiva2, se puede encontrar el execution engine. Este modulo se

encarga de dos tareas fundamentales:

- La gestion de los comportamientos que puede llevar a cabo el dron. Estos comporta-

mientos componen la funcionalidad básica del dron, pero permiten realizar acciones más

complejas al combinarse en misiones. Estos se coordinan y ejecutan en el behavior ma-

nagement system.

- La gestión de creencias y distintos datos inferidos u observados por el dron.Estos compo-

nen su base de hechos, que son los hechos o resultados observados a los que denominamos

5

creencias. Estas se actualizan, añaden o borran en el belief managment system.

Figura 2: Diagrama de la capa reactiva

2.4. Comportamientos

En esta sección se indican los tipos de comportamientos que se plantean incluir en Aeros-

tack y que permiten incrementar las capacidades de funcionamiento autónomo en robots

aéreos. Los comportamientos son:

1. Comportamiento encargado de habilitar los datos de los sensores de odometria a los

demás modulos. Este comportamiento complementa a los demás comportamientos que

hacen uso de los datos de posición y velocidad.

2. Comportamiento que se encarga de la generación de trayectorias del dron. Recibe el

punto al que se quiere llegar y genera una trayectoria para alcanzarlo.

3. Comportamiento que toma una trayectoria y sigue el camino que dicta dicha trayectoria.

Esta trayectoria puede estar generada de multiples maneras, la recibe en forma de puntos

y las conecta de modo que se genere un camino.

4. Comportamiento que permite al usuario seleccionar una imagen y guardarla para su

6

posterior analisis o uso.

5. Comportamiento que toma una imagen y realiza un seguimiento de esta. Hace uso de

un planificador de movimientos para hacer su seguimiento.

6. Comportamiento que permite la interacción con el usuario. Este se encarga de realizar

preguntas al mismo con el fin de obtener información o permitir la confirmación de un

reconocimiento que se considera dudoso.

En las siguientes secciones se describe la forma en que se llevan a cabo los comporta-

mientos. Por su mayor complejidad, la descripción se centrará en métodos utilizados en

comportamientos relacionados con la planificación de movimiento: (1) seguimiento de

imágenes (visual-servoing) y (2) generación de trayectorias (motion planning).

2.4.1. Motion-planning en Visual Servoing

El método de visual servoing es el encargado de realizar el seguimiento de la velocidad y/o

aceleración necesaria para conseguir un posicionamiento del objeto, posición o imagen a

seguir[3].

Estos sistemas parten de minimizar el error1 que es la diferencia entre el punto deseado y

el punto actual del dron. También se tienen encuenta los parametros de la camara: como

el distancia focal, coordenadas del punto de referencia y ratio de las dimensiones de la

camara.

e(t) = s(m(t), a)− s∗ (1)

También se parte de que la variación de las posiciones de referencia depende de la ve-

locidad de dron linear en los 3 ejes y angular en los mismos generando vectores de 6

componentes. También se usa un jacoviano de transformación temporal, que proporciona

7

la variación temporal para indicar el tiempo de cada paso.

s = Le ∗ vc (2)

Se observa que la variación del error es equivalente a la variación de la referencia.

e = s′(m′(t), a′)− s ∗ −s(m(t), a) + s∗ (3)

Como se puede ver en 3 el punto destino es fijo por lo que no aplica para la variación del

error, lo que hace que la variación del error sea la variación de las referencias.

Esta es la consideración general una vez en este punto se puede considerar que la derivada

del error sigue un decrecimiento exponencial(esta puede ser cualquier tipo de derivada,

es según diseño). Como ejemplo ponemos la derivada del decrecimiento exponencial.

∂e

∂t= −λ ∗ e (4)

Esto es debido a que la aplicación al ser negativa la hara menor y al ser una relación

del valor actual hace que el decrecimiento sea mayor al principio y va decreciendo la

reducción a medida que disminuye el error.

Debido a que la utilidad principal de este tipo de modelos es la de la generación de un

controlador de velocidades. Se considerarán las ecuaciones anteriores y el ejemplo de la

reducción exponencial para la generación de un modelo. Este modelo esta reflejado en la

ecuación . En esta se puede ver que se usa la variación del error estimada de forma ex-

ponencial, usando el error actual, para conseguir las velocidades necesarias para alcanzar

dicha reducción.

vc = −λ ∗ L+e ∗ e (5)

8

Una de las partes importantes el

Le (6)

el cual es como ya se mencionó antes el jacoviano de transformación temporal, este es

muy importante. Este parametro es dependiente de la interacción actual de la referencia

actual, lo que hace que sea necesario su calculo. Para esto se realizarán estimaciones. En

la ecuación anterior vemos que aparece

L+e (7)

esta hace referencia a la pseudo inversa Moore-Penrose. El objetivo es conseguir una

inversa en todos los casos independientemente de que no pueda existir, obteniendo una

aproximación lo suficientemente acertada.

La matriz de interacción se puede aproximar facilmente consiguiendo la matriz de varia-

ción de posición, considerando fijos los parametros de la camara. Para ello se realiza la

proyección de los puntos y se obtienen sus variaciones 3D mediante la siguiente ecuación

9 . Los signos negativos se refieren a que consideran que la variación en los puntos del

espacio es por el movimiento de la cámara(si mueves la camara a la derecha, la escena se

mueve hacia la izquierda). Una vez realizado esto dado que podemos extraer la matriz

Le (8)

usando extrayendo las velocidades de la ecuación obtenida de sustituir 9 sobre las ecua-

ciones de la proyección al plano de la cámara.

X = −vc − wc ×X (9)

Para obtener la matriz completa, dado que se requieren 6 grados de libertad, dado que

como mínimo se necesitan 6 factores, se emplean como minimo 3 puntos para determinar

la imagen.

9

Una vez descritos los distintos componentes, dentro del visual servoing génerico se en-

cuentran 2 tipos el IBVS y el PBVS, con objetivo fijo o variable. Dada la similaridad del

objetivo fijo y variable(se requiere introducir la variación del error existente entre el anti-

guo objetivo el actual), vamos a explicar sobre este. Dado que el comportamiento Follow

Object Image usa IBVS nos vamos a centrar en este.

El IBVS hace uso de vectores de parametros de 3 puntos. Estos puntos estan descritos en

coordenadas 2D en el plano de la camara. Para la estimación de

L+e (10)

se siguen multiples estrategias, las mas usadas son:

1. El empleo de

Le = Lx (11)

para todos los puntos. Para esto se requiere la obtención de una aproximación de cada

profundidad valor profundidad(Z). Este tiene gran coste y su variación se comporta nor-

malmente de forma constante, ya que al avanzar los puntos no tiene demasiada variación

de velocidad con respecto al punto actual.

2. El empleo de

Le∗ = Lx∗ (12)

solo para el punto final. No se requiere nada más que el calculo para cada una de las

metas. Su inicio es muy rápido. Esto se debe a que las velocidades a inducir pueden ser

muy distintas y tienden a mantenerse.

2. El empleo de

L+e = (

Le

2+

Le∗2

)+

(13)

se usan todos los puntos como en el primer caso. Se requiere el calculo exhaustivo al igual

que en el primero, pero se mejora su fase inicial al usar parte de la mejora que ofrece el

segundo metodo. Pero ofrece la linealidad y control de las velocidades que dan los puntos

10

actuales que pueden proporcionar una aproximación menos abrupta.

2.4.2. Motion-planning

Para el motion planning son las tecnicas que permiten al dron realizar un movimiento se

forma precisa y sin colisionar con ningún obstaculo. Existen muchas tecnicas para esto,

pero nos vamos a centrar en 2 principales:

1. El potencial field.

Esta tecnica de evasión de obstaculos consiste en la generación de vectores usando fuer-

zas de atracción generadas por los puntos destino(suele haber solo uno) y las fuerzas de

repulsión generadas por los obstaculos.

Estas fuerzas se suman para obtener la fuerza total que determinará el movimiento.

Ftot = Fatt + Frep (14)

Para esto se generan dos ecuaciones para las dos fuerzas necesarias.

Freq = katt + ntarget (15)

Donde n es el vector unitario entre el punto actual y el objetivo, de modo que seguiremos

esa dirección. K es la constante de atracción.

En cuanto a las ecuaciones de repulsión se plantea la clasica:

⎧⎨⎩Krep ∗

∑j(

1d(pj ,qi)

− 1d0) ∗ nj d(pj, qi) < d0

0 otherwise(16)

En esta K representa la constante de repulsión, p es la posición del obstaculo, y q la del

punto. n representa el vector unitario entre nuestra posición y el obstaculo. Como se puede

observar este siempre retorna valores positivos o 0. Es por ello que el vector unitario o

11

la contante de repulsión deben llevar el signo negativo que compense la desviación en

sentido opuesto sobre la posición q.

Existe otra forma planteada que es la que emplea aceleraciones. Esta no requiere de un

cambio de signo al contrario que la anterior.

⎧⎨⎩Krep ∗

∑j(

a∗qi2ad(pj ,qi)−q2i

) ∗ nj ∗ nqi qi > 0

0 otherwise(17)

En esta aparece a que es la aceleración máxima, q que es la velocidad actual y nqi que es

el vector unitario de la velocidad del dron. En esta se observa que en el denominador se

obtiene la velocidad inicial al cuadrado con signo negativo(La ecuación es la de la velo-

cidad en Movimiento rectilineo uniformemente acelerado). Esto determina que cuando la

distancia y aceleración dejan de superar a la velocidad por poco el el valor es minimo. Lo

que hace que el valor negativo sea maximo(Esta en el denominador). esto se ve potencia-

do si la aceleración y velocidad son altas. Aunque el nivel de rechazo puede decaer mucho

si el vector velocidad y el vector de posición y obstaculo no tienen un angulo donde se

pueda producir incidencia(el producto escalar de ambos depende del coseno del angulo,

donde si coinciden es máximo). A medida que decrece la distancia desde ese punto em-

pieza cada vez más a estabilizarse sin dejar de ser negativo, aunque el vector entre punto

y obstaculo puede llegar a división por 0 lo que puede provocar fallos si se acerca mucho.

2. Proabalistic Road:

Esta tecnica consiste en la generación de puntos aleatorios y conexión de unos a los otros

formando un grafo.

Esta tecnica permite observar si la conexión es posible al comprobar si el camino colisiona

con un obstaculo. Esto produce que sea bastante más lento que el potentialField, pero

resuelve otros problemas.

Los caminos se suelen generar entre puntos que esten proximos, para ello se recurre al

metodo del vecino más próximo, estos se obtienen en base a la distancia hasta llegar a un

12

k preestablecido.

Una vez generado se recorre el grafo de forma optima mediante metodos como el algorit-

mo de Dijkstra o de Bellman-Ford. Con ello se consigue la trayectoria a seguir.

Este metodo es mas simple que el anterior, y resuelve problemas de imposibilidad de

movimiento en algunos casos debido a compensaciones de fuerzas. Hay problemas en el

Potential Field sobre el aumento de fuerza repulsiva a medida que nos acercamos al obje-

tivo. Esto se suele compensar añadiendo a la ecuación la relación entre el punto y la meta,

de modo que si nos acercamos disminuya el valor del vector, consiguiendo compesar esa

falla. También el hecho de que si estamos muy lejos esta no deba atraer con tanta fuerza y

pueda suprimir a las fuerzas de repulsión para ello se suele poner un limite de modo que

haga menor la atracción pero existente, esta se ira modificando de modo que se adapte a la

posición con respecto a la meta actual(también la de repulsión habria aumentado debido a

la corrección anterior, lo que hace a el limite una medida para evitar que se sobrepase por

mucho la fuerza de repulsión si la distancia al obstaculo es corta, provocando choques).

13

3. DISEÑO DE LA SOLUCION

Los comportamientos a desarrollar, se construyen sobre módulos de ROS comunicados

entre sí mediante topics, y servicios que permitirán la activación y parada de los mismos.

Esto permite la coordinación de los distintos comportamientos.

También emplea el uso de servicios que permiten la comunicación con el belief manag-

ment system, que permite conocimiento de “creencias” o información capturada, que per-

mite saber el estado actual y si es viable la activación o continuación del comportamiento.

Cada comportamiento es de dos tipos:

1.Goal based: Tiene una condición de terminación, así que termina sin que el usuario

tenga que preocuparse por su finalización.

2.Recursive: No tienen condición de terminación, es el usuario en encargado de terminar

con ellos una vez hayan cumplido con su función. Algunos de estos están diseñados para

coexistir con otros comportamientos manteniéndose en segundo plano.

Todos independientemente de su tipo tienen unas funciones predeterminadas:

1.OwnStart: Es la función que realiza todas las acciones que luego serán monitorizadas.

2.OwnSetUp: Es la función que prepara todos los topics y servicios necesarios y prepara

al comportamiento para la realización de su funcionalidad.

3.OwnRun: Es la función que monitoriza si el comportamiento cumple o no su objetivo.

En aquellos que no necesitan esa comprobación(Recursive) no suele ser usada, aunque

pueden servir para monitorizar que no se producen errores.

4.OwnCheckActivationsConditions: Esta función es la encargada de comprobar que se

cumplen las condiciones de activación necesarias.

Todos los behaviors heredan del behavior process3, cada comportamiento puede realizar

acciones de diversa indole, a continuación se les presentaran aquellos desarrollados.

14

Figura 3: Diagrama UML de behavior generico

3.1. Auto-localización

Este tipo de comportamientos permite obtener la posición actual del dron. Existen varias

formas de obtener la posición actual del dron.

3.1.1. Comportamiento "Self Localize by odometry"

Este comportamiento proporciona la posición del dron obtenida a través de sus sensores.

Estos proporcionan la X, Y y Z que representan sus coordenadas absolutas en el mapa

generado internamente por el dron. Estas coordenadas están en función del punto inicial

del que inicio el dron.

También permite la obtención de las velocidades del dron en cada uno de los ejes, así

como sus rotaciones en ángulos de Euler (yaw, pitch, roll).

La funcionalidad principal de este comportamiento es la activación de un modulo. Este

modulo es el "Self localization selector" el cual tiene 3 modos: 1. modo usando odometria:

este el el modo que se activa. 2. modo usando visual markers: este modo es para el uso

de marcadores visuales para obtener la posición. 3. modo ekfsensor fusion: realiza una

mezcla de los valores obtenidos de los distintos sensores para la localización.

15

Es el módulo que realiza la lectura de los datos del controlador del sensor y realiza en

calculo, en base a la posición actual y la posición anterior realizando, recibida por el

sensor. Esto permite al restar la actual a la anterior y dividirla por el periodo de obtención

de datos calcular la velocidad: v= x’ que dado que estamos en un sistema discreto 18

donde (t-t1) es mínimo).

x′ .=

(x(t)− x(t− 1))

(t− t1)(18)

A continuación se muestra el diagrama del comportamiento:

Figura 4: Diagrama del comportamiento Self Localize By odometry

En este diagrama se puede observar que se usa un servicio(circulo), que conecta el com-

portamiento con el Belief Manager System, este hace uso del topic consult_belief, que se

describirá más adelante.

También se observar que usa otro servicio para comunicarse con el Self Localization

Selector y ponerlo en modo odometria, lo que proporciona por sus topics estimated_pose

16

y estimated_speed la posición y velocidad obtenida por los sensores del dron. Este hace

uso del change_self_localization_mode_to_odometry.

En cuanto la implementación usado se va a ver la logica de las distintas funciones.

Algorithm 1: ownSetUp()

1

2 droneid ->String

3 droneNamespace -> String

4 directory -> String

5 consult_belief -> String

6 change_self_localization_mode_to_odometry -> String

Se puede observar en el pseudocódigo presentado anteriormente que el ownSetUp es el

metodo encargado de cargar los topics de los ficheros de configuración.

Algorithm 2: ownStart()

1

2 consult_belief -> Service consult_belief

3 change_self_localization_mode_to_odometry -> Service

change_self_localization_mode_to_odometry

4

5 call Service_change_self_localization_mode_to_odometry(void message)

Como se muestra en el pseudocódigo presentado anteriormente en el ownStart se crean

los servicios que usarán los topics como medio para realizar la petición al modulo que

la publica. También se realiza una llamada al servicio de self_localization_mode para

ponerlo en modo odometria. Para ello basta con mandar un mensaje vacio a ese servicio.

El metodo ownRun esta vacio y el metodo OwnCheckActivationsConditions no realiza

ninguna comprobación en este caso. Por tanto no hay algoritmo que presentar como pseu-

docódigo.

Los topics utilizados son:

1. change_self_localization_mode_to_odometry: Permite cambiar el modo del Self loca-

lization Selector a modo odometria.

17

2. consult_belief: Permite obtener el resultado de la creencia solicitada(si existe).

3.2. Control de movimiento sobre mapas geométricos

Estos comportamientos tienen como objetivo la planificación y ejecución del desplaza-

miento de un dron. Esta evita obstáculos. En este caso es una planificación sobre un mapa

geométrico.

3.2.1. Comportamiento "Generate path in geometric map"

Este comportamiento es goal based. Este comportamiento es el encargado de generar una

trayectoria y añadirla a la base de datos de creencias del dron. Esta trayectoria generada

no tiene por qué ser definitiva, ya que esta puede no ser válida en el momento de la

ejecución del movimiento, por lo que es común llamarle varias veces durante la ejecución

del movimiento.

Este comportamiento activa procesos(módulos) que permiten el cálculo de trayectorias.

Estas se realizan usando las posiciones de los distintos obstáculos y generando un camino

mínimo dada una nube de puntos generada aleatoriamente sobre el mapa. Estos se conec-

tan en forma de grafo y se busca la distancia mínima entre ello (siempre que no colisionen

con los obstáculos, aunque esto se refleja en el grafo). Esta se rectifica para hacer que el

camino sea más suave evitando todo lo posible que el camino sea muy irregular. También

existe la posibilidad de obtener el camino mínimo usando mapeo de campo potencial que

ordena en base a distancias a los distintos puntos de origen a los posibles destinos.


18

Figura 5: Diagrama del comportamiento Generate Path in Geometric Map

Este diagrama muestra la conexión con el Belief Manager System, al igual que en el caso

anterior hace uso del consult_belief, pero esta vez añade creencias a la base de hechos

para ello usa el add_belief.

También se conecta con el trajectory planner, de donde solicta la trayectoria dado un punto

destino. Este punto destino se pasa a traves del droneMissionPoint y se recibe por medio

de droneTrajectoryAbsRefGenerated(Trajectory Ref) la trayectoria generada.



1

2 def ownSetUp()

3

4 droneid -> String

5 droneNamespace -> String

19

6 directory -> String


8 droneTrajectoryAbsRefGenerated -> String

9 add_belief -> String

10 droneMissionPoint -> String

11

12 end ownSetUp

Se puede observar en el pseudocódigo presentado anteriormente que el ownSetUp es el

metodo encargado de cargar los topics de los ficheros de configuración.


1

2 consult_belief -> Service consult_belief

3 subscriber droneTrajectoryAbsRefGenerated -> Subscribe

droneTrajectoryAbsRefGenerated

4 add_belief -> Service add_belief

5 droneMissionPoint -> Publisher droneMissionPoint

6 load coordinates(point)

7

8 Publish coordinates in Pub_droneMissionPoint

Como se muestra en el pseudocódigo presentado anteriormente en el ownStart se crean

los servicios que usarán los topics como medio para realizar la petición al modulo que

la publica. En este también se muestra que se usan los argumentos recibidos por yaml,

para enviarselos al trajectory planner a traves de el topic droneMissionPoint publicando

en este.

Algorithm 5: ownRun()

1

2

3 if camino generado then

4 return GOAL_ARCHIEVED

5 end if

6

7 if timer finished then

8 return timeout

9 end if

20

En el pseudo código previamente presentado podemos ver el metodo ownRun este es el

que se dedica a comprobar que alguno de los estados de terminación se cumple y devuelve

la causa de la finalización del comportamiento.

El metodo OwnCheckActivationsConditions no realiza ninguna comprobación por tan-

to no se presenta. Pero si se presenta el metodo callback de la trayectoria debido a su

relevancia

1 def TrajectoryCallback()

2

3

4 llamada a Service_add_belief con la trayectoria de parametro.

5 camino generado -> true

6

7 end TrajectoryCallback

Como se puede observar en este callback se añade la trayectoria recibida a la base de

hechos del dron. También se establece la condición de terminación para indicar que la

generación de trayectorias ha finalizado con éxito.


1. add_belief: Permite añadir una creencia a la base de hechos del dron. Esta se usa para

añadir la trayectoria a seguir una vez obtenida.

2. consult_belief: Permite obtener una creencia dado el nombre de la tupla. Esta se usa

para obtener parametros del dron contenidos en la base de hechos del mismo.

3. droneMissionPoint: Permite indicar al trajectory planner el punto destino al que quere-

mos llegar. Este generará una trajectoria para alcanzar ese punto.

4. droneTrajectoryAbsRefGenerated: Este topic contiene las trajectorias generadas por el

trajectory planner. Estas se añadirán a la base de hechos del dron.

21

3.2.2. Comportamiento "Follow path in geometric map"

Este comportamiento es goal based. Es el encargado de tomar una trayectoria y seguirla

usando el trajectoryController. Mientras el trajectoryController se mantiene llevando a

siguiendo los puntos de la trayectoria, el comportamiento observa los obstáculos para

comprobar que el movimiento es seguro y no lleva a una colisión.

Este comportamiento activa los procesos(módulos) que permiten la ejecución del movi-

miento, siguiendo los puntos enviados al controlador de trayectoria. Hace comprobacio-

nes de las situaciones de cada obstáculo. Esta la realiza el society pose, que es el encargado

de observar cambios en los objetos y notificarlos.

La trayectoria se recibe a través de los parámetros, esto se debe a que de esta manera se

comporta de forma universal y es capaz de seguir trayectorias generadas con múltiples

técnicas ( potentialField, probabilistic road, Ocupancy Grid. . . ).


22

Figura 6: Diagrama del comportamiento Follow Path in Geometric Map

El diagrama muestra que como en casos anteriores se conecta con el Belief Manager

System a traves del consult_belief. Por otro lado usa el Self Localization Selector del que

obtiene la posición del dron por medio del estimated_pose y la velocidad y angulos de

rotación usando estimated_speed y rotation_angles respectivamente.

Este se comunica con el DroneCommand para colocar los controladores a MOVE usando

el topic command/high_level. También se conecta con el trajectory controller para realizar

el control de la trayectoria que se le pasa por medio del droneTrajectoryAbsRefGenerated

la trajectoria que se seguirá. A parte al trayectory controller se le pasa el yaw actual usando

el droneControllerYawRefCommand que usará como yaw del que partir para establecer

el camino. Por otro lado el trayectory controller consta de 3 modos: trajectory, speed y

point. Se requiere poner el trajectory controller en modo trajectory para poder indicarle

que la entrada la interprete de ese modo.

23



1

2 estimated_pose -> String

3

4 estimated_speed -> String

5

6 command/high_level -> String

7


9


11

12 rotation_angles -> String

13

14 societyPose -> String

15

16 obstacles -> String

17

18 droneTrajectoryAbsRefCommand -> String

19

20 droneTrajectoryController/setControlMode -> String

21

22 droneControllerYawRefCommand -> String

En el pseudocódigo presentado anteriormente podemos observar que al igual que en casos

anteriores la función ownSetUp permite cargar los valores de los topics para luego ser

empleados en ownStart.


1

2

3 String rotation_angles -> Subscriber rotation_angles

4

5 String droneTrajectoryAbsRefCommand-> Publisher droneTrajectoryAbsRefCommand

6

7 String estimated_pose -> Subscriber estimated_pose

8

9 String estimated_speed -> Subscriber estimated_speed

10

11 String command/high_level -> Publisher command/high_level

24

12

13 String consult_belief -> Service consult_belief

14

15 String add_belief -> Service add_belief

16


18

19 String droneControllerYawRefCommand -> Publisher droneControllerYawRefCommand

20

21 String obstacles -> Subscriber obstacles

22

23 String societyPose -> Subscriber societyPose

24

25 Load trajectory

26

27 publish trajectory in droneTrajectoryAbsRefCommand

28

29 publish yaw in droneControllerYawRefCommand

30

31 publish MOVE in command/high_level

En ownStart apreciamos como creamos los servicios, suscriptores y publishers para el

envio o recepción de valores. En este caso cargamos la trayectoria de un fichero yaml

que se enviará al trajectoryController por medio del droneTrajectoryAbsRefCommand,

también usamos el command/high_level para poner el DroneCommand en modo MOVE.

A parte el trajectory controller requiere el yaw inicial que se pasará por medio del topic

droneControllerYawRefCommand.


1

2

3 if pos_final == estimated_pose then


5 end if

6 if timer_is_finished then

7 return TIMEOUT

8 end if

9 if fail then

10 return FAIL

11 end if

25

En el fragmento anterior se describe el metodo ownRun donde vemos las distintas posi-

bilidades que permitirían terminar al comportamiento. Estas requieren alcanzar el punto

destino, que termine el timer o que ocurra alguna situación que requiera replanificar(fail),

estas se verán en el societyPoseCallback y el obstaclesCallback

Algorithm 9: OwnCheckActivationsConditions()

1

2

3 call Service consult_belief with battery(self,low)->query1

4

5 if query1 then

6

7 return tuple(error, "battery low")

8

9 end if

10

11 call Service consult_belief with LANDED->query2

12

13 if query2 then

14

15 return tuple(error, "drone landed")

16

17 end if

18

19 return tuple(ok, "")

En el metodo ownCheckActivationConditions podemos observar que para realizar el com-

portamiento se require un buen nivel de bateria, para evitar que el dron se desconecte en

mitad del comportamiento. También que el dron este en el aire ya que el dron no se des-

plaza una vez aterrizado.

Algorithm 10: societyPoseCallback()

1

2

3 if estimated_pose - drone_poses < 1 then

4 fail=true

5 end if

Algorithm 11: obstaclesCallback()

26

1 fail=true

Para los dos callback anteriores, se observa que mientra que el que controla la posición

de los drones se espera a replanificar una vez estemos a una distancia corta de otro dron,

cuando se produce un cambio en los obstaculos se replanifica directamente. Esto se debe

a que suponemos que los drones conoces nuestra posición ya que se conectan a nosotros,

mientras que los obstaculos pueden ser de distinto tipo, produciendo situaciones donde

podemos a colisionar si no controlamos si la ruta es libre de obstaculos.


1. consult_belief: Permite obtener una creencia a la base de hechos del dron.

2. estimated_pose: Permite obtener la posición actual del dron. Permitirá saber la posición

actual y si ha alcanzado su destino. También se usa para comprobar que no hay colisiones

con otros drones.

3. command/high_level: Permite cambiar el modo del DroneCommand para poner el dron

en modo MOVE, lo que inciará permitirá el movimiento del dron.

4. rotation_angles: Permite obtener los angulos de rotación del dron. Se pueden utilizar

para obtener la orientación del dron.

5. estimated_speed: Permite obtener la velocidad del dron. Esto permite saber si nos pa-

samos al alcanzar el objetivo o no. Esto ocurre cuando la velocidad en destino es mayor

de las esperada.

6.droneControllerYawRefCommand: Permite indicar al trajectory controller el angulo

yaw del dron. Esto permite al modulo tener un angulo inicial sobre el que realizar los

movimientos necesarios para moverse por los puntos de la trayectoria.

7.droneTrajectoryController/setControlMode: Permite poner el trajectory controller en

modo trajectoria y que por tanto use puntos como referencia.

8.societyPose: Permite obtener la posición de los drones que se encuentran en comunica-

27

ción con este. Se usa para evitar colisiones ya que al estar comunicados pueden conocer

la trayectoria de nuestro dron y evitar interrupciones. Unicamente se pide replanificar

cuando se corre riesgo de choque.

9. obstacles: Permite obtener los obstaculos del mapa. Este se actualiza cuando cambian.

En ese caso se pide replanificar, debido a que no se tiene ningún control sobre estos, ni

los obstaculos tienen por que tener consciencia del dron(al ser más probable encontrarnos

en una situación desfavorable se replanifica para evitar riesgos).

10. droneTrajectoryAbsRefGenerated: Permite transmitir la trajectoria al trajectory con-

troller. Este la usará para seguir los puntos que componen la trayectoria hasta llegar al

destino.

3.3. Seguimiento de imagenes

Esta familia de comportamientos tiene como objetivo la selección y seguimiento de un

objeto capturado en una imagen.

3.3.1. Comportamiento "Select object image"

Este comportamiento es el encargado de seleccionar una imagen. El propósito principal es

el guardar una imagen para su posterior uso ya sea seguirla, como tratarla de otros modos.

Este comportamiento activa los procesos relativos al OpenTLD que permite la selección

de imágenes y creación del boundary box alrededor el objeto que marca los limtes de

cercanía que se pueden mantener en torno a este.

Esta imagen con todos sus parámetros relativos a la boundary box y al elemento selec-

cionado por al gui del OpenTLD en un rosbag. Este rosbag es un fichero con todos los

valores de los parámetros seleccionados en este caso la boundary box y la imagen a seguir.

Este comportamiento recibe el nombre del fichero donde se quiere guardar la imagen. No

es necesario especificarle que es .bag. Este comportamiento es goal based.

28


Figura 7: Diagrama del comportamiento Select Object Image

En el diagrama anterior se puede ver como hace uso del consult_belief para comunicarse

con el Belief Manager System. También se puede observar como esta el add_belief esto

se debe a que como mejora se podría incluir la imagen X con su fichero en la base de

hechos, por tanto no se hace uso actual de este.

Se conecta con el OpenTLD al que no envía ningún tipo de información ya que es

el usuario el que hace directamente la interacción con este. Pero recibe por medio de

tld_bb la imagen con su bounding box y se recibe si la imagen esta visible por meido del

is_object_on_frame.



1

2 tld_bb -> String

3 consult_belief-> String

4 is_object_on_frame -> String

29


1

2 String tld_bb -> Subscriber tld_bb

3 String consult_belief-> Service consult_belief

4 String is_object_on_frame -> Subscriber is_object_on_frame

Como se obseva el ownSetUp carga los topics y el ownRun crea los suscriptores y servi-

cios necesarios.


1

2 if image_recieved then


4 end if


6 return TIMEOUT

7 end if

En el ownRun se observa que el comportamiento termina si recibe una imagen o si termina

el tiempo del timer.

Algorithm 15: ownCheckActivationConditions()

1


3

4 if query1 then

5


7

8 end if

9


11

12 if query2 then

13


15

16 end if

17


30

El ownCheckActivationConditions no permite la acción del comportamiento si la bateria

es baja y si esta aterrizado. Esto último se debe a que la imagen se toma para ser seguida

posteriormente y ese seguimiento requiere que el dron este en el aire. Por tanto facilita al

dron el reconocimiento de la imagen buscada en la imagen capturada con la camara en

ese momento.

Algorithm 16: boundingBoxCallback()

1

2 write in rosbag image

3

4 image_recieved -> true

Este callback recibe la imagen y la guarda en un rosbag para su posterior uso y/o analisis.

Los topics utilizados son: 1.tld_bb: Permite al usuario recibir la imagen seleccionada por

medio del openTLD. Esta viene con su bounding box.

2.is_object_on_frame: Permite al usuario conocer si en ese momento se observa la ima-

gen.

3. consult_belief: Permite consultar la base de creencias del dron para conocer su situación

actual.

3.3.2. Comportamiento "Follow object image"

Es un comportamiento goal based. Este comportamiento es el encargado de seguir una

imagen descrita en un fichero rosbag. Este sigue el objeto durante un tiempo pasado como

argumento al comportamiento.

Este comportamiento activa el proceso del IBVSController, este controlador es el que

mantendrá el objeto a una posición adecuada para no sobrepasar los límites de la boundary

box pero tampoco alejarse del objeto.

31

Este comportamiento recibe como parámetro el nombre del fichero de la imagen a seguir,

y lee del fichero rosbag. Una vez terminado el tiempo especificado este termina y deja

de seguir al objeto. El tiempo especificado es opcional, por defecto está colocado a 120

segundos.


Figura 8: Diagrama del comportamiento Follow Object Image

Este diagrama muestra que al igual que en casos anteriores hacemos uso del consult_belief

para acceder a la base de hechos del Belief Manager System. También se conecta con el

DroneCommand, esto es para cambiar el modo del dron a MOVE de modo que se pue-

da realizar el movimiento, para seguir la imagen recibida como arguemento. Por último

se comunica con el IBVSController para enviar la imagen recibida y que comience el

seguimiento. El topic empleado es el tld_gui_bb.


32


1 estimated_pose -> String

2

3 estimated_speed -> String

4

5 command/high_level -> String

6


8


10

11 rotation_angles -> String

12

13 tld_gui_bb -> String


1 String activate_behavior -> Service activate_behavior

2

3 String consult_incompatible_behaviors-> Service consult_incompatible_behaviors

4

5 String estimated_pose -> Subscriber estimated_pose

6

7 String estimated_speed -> Subscriber estimated_speed

8

9 String command/high_level -> Publisher command/high_level

10


12


14


16

17 String tld_gui_bb -> Service tld_gui_bb

18

19 Load image

20

21 call Service tld_gui_bb with image

22

23 publish MOVE in Publisher command/high_level

Al igual que en casos anteriores ownSetUp y ownRun cargan los topics y crean los publis-

33

hers, servicios y suscriptores. A parte de esto en el ownStart Se carga la imagen pasada

como argumento. Esto permite el uso de distintas imagenes, tomadas en distintos tiempos.

Esto es para no limitar a un tipo de fichero o de nombre.

También se coloca a MOVE el DroneCommand para permitir el movimiento del dron.



2


4

5 end if

El ownRun solo termina cuando se alcanza el timeout y devuelve GOAL_ARCHIEVED

ya que es el objetivo de este comportamiento, seguir la imagen durante un tiempo prees-

tablecido.



2

3 if query1 then

4


6

7 end if

8


10

11 if query2 then

12


14

15 end if

16


Al igual que en casos anteriores se requiere buen nivel de bateria y que el dron este

volando.

34

Los topics utilizados son: 1. tld_gui_bb: Permite enviar al IBVSController la imagen a

seguir y su bounding box.

2. command/high_level: Permite poner al dron en modo MOVE, esto permite iniciar el

movimiento del dron.

3.4. Interacción con el operador

Son comportamientos encargados de recibir información del usuario. Pueden realizar pre-

guntas de distinto tipo una vez se alcanza una condición definida.

3.4.1. Comportamiento "Request operator confirm recognition"

Este comportamiento pregunta al usuario y da como opciones distintas preguntas especi-

ficadas como parámetro en la activación del mismo. Es un comportamiento goal based.

En contraposición a otros comportamientos este para la activación de todos los comporta-

mientos de movimiento en la activación y al acabar los reactiva. Durante su ejecución este

permite la llamada cualquier comportamiento usando la interfaz(GUI) y lo puede realizar

usando las teclas especificadas en este. Esto es para permitir al usuario responder con

mayor nivel de seguridad la pregunta planteada.


35

Figura 9: Diagrama del comportamiento Request Operator Confirm Recognition

El diagrama anterior muestra que se conecta al behavior coordinator. Esto se debe a que

debe solicitar la activación de comportamientos. Estos son los comportamientos interru-

pidos y el keep hovering para interrumpir los comportamientos de movimiento durante la

ejecución del comportamiento. El topic empleado para esto es activate_behavior. Para po-

der activar los comportamientos interrumpidos se obtiene usando el consult_incompatible_behaviors

la lista de los behavior que se interrumpiran al ser incompatibles con el keep hovering.

Por otro lado se consultan creencias(consult_belief) como en los comportamientos an-

teriores, pero además se añaden y se borran. Esto se hace a través del add_belief y el

remove_belief. Esto es debido a que se deben añadir las creencias proporcionadas por el

usuario y borrar aquellas que el usuario no considere precisas o correctas.


36


1

2

3 activate_behavior -> String

4

5 consult_incompatible_behaviors -> String

6

7 remove_belief -> String

8


10



1

2

3 String activate_behavior -> Service activate_behavior

4

5

6

7 String consult_incompatible_behaviors-> Service consult_incompatible_behaviors

8

9

10


12

13

14

15 String remove_belief -> Service remove_belief

16

17

18


20

21

22

23 Load question

El ownSetUp y ownStart se comportan de un modo bastante standard cargando los topics

y creando los servicios a emplear.

37


1

2

3 if not first steps finished then

4

5 call Service consult_incompatible_behaviors for behavior KEEP_HOVERING ->

Result Keep_hovering_incompatibilites

6

7 call activate_behavior with HOVER

8

9 first steps finished <- true

10

11 end if

12

13 if not finished then

14

15 activate ui -> answer

16

17 call Service consult_belief with question -> query

18

19 if not query and answer confirm then

20

21 call Service add belief with tuple(question,answer)

22

23 end if

24

25 elif query and and answer confirm and query[1]!=answer then

26

27 call Service remove_belief with tuple(question,query[1])

28

29 call Service add belief with tuple(question,answer)

30

31 end if

32

33 elif query then

34

35 call Service remove_belief with tuple(question,query[1])

36

37 endif

38

39 end if

El ownRun es el encargado de activar el behavior KEEP_HOVERING y preguntar por los

38

activos para guardarlos. Una vez guardados y activado el KEEP_HOVERING, se le pasara

el control a la interfaz grafica de donde se obtendrá el resultado, esto lleva a eliminar o

añadir según se necesite de la base de hechos.

Algorithm 24: ownStop()

1

2

3 call activate_behavior with Keep_hovering_incompatibilites

Una vez realizado el stop, se reactivan los comportamientos interrumpidos.

Los topics utilizados son: 1. activate_behavior: Permite solicitar al behavior coordinator la

activación de un comportamiento. Este parará todos los comportamientos incompatibles.

Esto permite iniciar el keep Hovering en activación lo que hace que el dron se detenga y

una vez se realiza la función de este comportamiento se reactivan aquellos parados en la

activación del keep hovering. Esto se hace guardando todos los comportamientos activos

incompatibles con el keep hovering.

2. consult_incompatible_behaviors: Permite solicitar al behavior coordinator todos los

behaviors activos incompatibles con respecto a uno dado.

3. add_belief: Permite añadir una creencia a la base de hechos. Se usa para añadir la

respuesta del usuario a las creencias del dron.

4. consult_belief: Permite consultar a la base de hechos del dron sobre el estado del dron.

5. remove_belief: Permite eliminar una creencia de la base de hechos, esta se usa si el

usuario considera que aquello preguntado no es cierto.

4. VALIDACIÓN Y PRUEBAS

Para la validación de los distintos comportamientos, se han realizado pruebas de distinta

indole, estas las especificaremos por cada grupo de comportamientos.

39

Estos comportamientos tienen pruebas individuales y pruebas de integración tanto con

comportamientos que hacen uso de sus salidas, como de comportamientos que pueden

realizar su acción de manera concurrente. Se espera que se detecte si es posible la activa-

ción dada la situación actual y si debe parar un comportamiento en ejecución o no debido

a incompatibilidades entre ellos.

Anteriormente a esto describiremos los entornos donde se han llevado las pruebas simu-

lador y ARDrone.

4.1. Entornos

4.1.1. Simulador

Este entorno se usa como primera fase de las pruebas. Existen dos componentes dentro

de este el CLI y RVIZ.

El CLI10 es una interfaz que muestra las distintas variables que se llevan a cabo por el

dron, en función de las acciones a realizar en la misión y del mapa que tiene el dron,

esta tiene una versión grafica donde se pueden ver todas las variables y el movimiento del

dron de acorde a las variables recogidas, como la posición actual del dron, la velocidad,

los angulos de rotación...

Esta se actualiza de acorde a los datos recibidos de los topics de localización del dron.

Estos se pueden activar y desactivar usando los comportamientos de auto-localización,

que permite el uso del "Self localization selector" en distintos modos, según el comporta-

miento usado para su activación.

40

Figura 10: Interfaz del CLI

RVIZ11 es un simulador 3D que muestra el recorrido del dron y muestra un escenario

usando la información de posicion del dron y de los obstaculos. Este se usa para visualizar

el movimiento del dron a lo largo del mapa y su posición respecto a los obstaculos.

Esto sirve para ver visualmente si realiza lo esperado, aunque no asegura su funciona-

miento en el dron real.

41

Figura 11: Captura de RVIZ

4.1.2. ARDrone

El ARDrone12 es un dron de pruebas donde se realizan vuelos reales. Este entorno es

para pruebas con condiciones reales, a las que preceden pruebas en el simulador. En al-

gunos casos este es el primer medio de pruebas debido a imposibilidad de simular ciertos

dispositivos.

Este entorno también hace uso de la interfaz grafica(GUI) para la carga de misiones y el

control por teclado del operario.

42

Figura 12: Foto del ARDrone

4.2. Pruebas realizadas

4.2.1. Auto-localización

Para este grupo de comportamientos realizamos las pruebas descritas en 2.

En esta se describen las pruebas, con el comportamiento a probar en cada una de ellas de

los comportamientos de este tipo. Podemos observar que se realizaron pruebas en conjun-

ción con otro tipo de comportamientos para verificar que permitian una integración con

los demás comportamientos, tanto nuevos como exitentes. En este caso no se especifica

en la tabla el comportamiento por que solo describe las realizadas en "Self Localize By

odometry".

Prueba Descripción resultado

Prueba1 comprobación de la activación de los modulos correcto

Prueba2 Comprobación de las condiciones de activación correcto

Prueba3 Comprobación de desactivación del modulo al terminar correcto

Prueba4 Prueba con comportamiento GO TO POINT correcto

Tabla 2: tabla de pruebas de comportamientos de Auto-localización

43

Se puede observar que la cantidad de pruebas no fue extensa ya que su proposito es muy

concreto que es recibir la posición cuando este se activa y comprobar que el proceso se

desactiva bien ya que no tienen condición de terminación los comportamientos de este

tipo presentados.

4.2.2. Control de movimiento sobre mapas geométricos

Para este grupo de comportamientos realizamos las pruebas descritas en 3.

En esta se describen las pruebas, en esta se describen pruebas para varios comportamien-

tos, asi que indicamos la prueba y el comportamiento al que se refiere. Se realizaron como

en el caso anterior pruebas en conjunción a otros comportamientos. En este caso era ne-

cesario para la generación de trayectorias y para seguimiento de las mismas.

Prueba Comportamiento Descripción resultado

Prueba1 Generate path in geometric map comprobación de la activación de los modulos correcto

Prueba2 Generate path in geometric map Comprobación de generación de trajectorias error: se recibio trayectoria incompleta

Prueba3 Generate path in geometric map Comprobación de generación de trajectorias error: se recibían trayectorias antiguas

Prueba4 Generate path in geometric map Comprobación de generación de trajectorias correcto

Prueba5 Follow path in geometric map Comprobación de seguimiento de trajectorias correcto

Prueba6 Follow path in geometric map Comprobación de seguimiento de trajectorias con obstaculos variables correcto: para al no poder seguir y lo notifica

Tabla 3: tabla de pruebas de comportamientos de control de movimiento en mapas geo-

métricos

Las pruebas llevadas a cabo fueron principalmente en el simulador, aunque también se

probó en el dron real. El funcionamiento dadas las pruebas realizadas parecen indicar que

funciona correctamente.

4.2.3. Seguimiento de imágenes

Las pruebas realizadas se pueden ver en 4.

Las pruebas realizadas en los comportamientos de este tipo, se realizaron directamente en

el dron real, ya que no se disponía de modo de recolección de imágenes ni movimiento

de la cámara de acorde, para la captura y seguimiento de las imágenes.

Se puede observar que el tipo de pruebas realizadas son muy similares. Esto es debido a

que se tratan de unos comportamientos que dependen mucho de los parámetros escogidos,

44

Prueba Comportamiento Descripción resultado

Prueba1 Select object image Comprobación de salida del gui del openTLD correcto

Prueba2 Select object image Comprobación de recepción de la imagen correcto

Prueba3 Select object image Comprobación de no recepción por el IBVSController error: Se recibía la imagen en activación

Prueba4 Select object image Comprobación de no recepción de la imagen por el IBVSController correcto

Prueba5 Select object image Guardado de la imagen correcto

Prueba6 Follow object image Comprobación de seguimiento la imagen error: necesarias las bounding box

Prueba7 Follow object image Comprobación seguimiento de la imagen correcto

Prueba8 Follow object image Comprobación del timeout correcto

Tabla 4: tabla de pruebas de comportamientos de seguimiento de imágenes

ya que estos pueden provocar que la imagen tenga todos los puntos correctamente, la

sigue y si se mantiene a la distancia correcta en cada caso, cambiando el objeto a seguir e

interpretando la información de la bounding box.

4.2.4. Interacción con operador

Las pruebas realizadas se pueden ver en 5.

Este grupo de comportamientos solo consta de un comportamiento a probar por tanto se

omite el campo comportamiento. Este es en todas las filas el “Request operator confirm

recognition”.

Prueba Descripción resultado

Prueba1 comprobación muestra de la interfaz de selección correcto

Prueba2 Comprobación de muestra de la pregunta y opciones correcto

Prueba3 Comprobación de los botones generados correcto

Prueba4 Prueba de recepción de la respuesta correcto

Prueba5 Prueba de parada de los comportamientos correcto

Prueba6 Prueba de control del operario correcto

Prueba7 Prueba de reactivación de comportamientos correcto

Tabla 5: tabla de pruebas de comportamientos de interacción con el operador

Las pruebas realizadas se realizaron principalmente en el simulador, aunque también se

comprobó su funcionamiento en el dron real. Para este comportamiento se realizaron prue-

bas en conjunción con los comportamientos GO TO POINT, KEEP MOVING y KEEP

HOVERING

En todas las pruebas se usó la GUI, ya que esta tiene la función de conectarse con el dron

45

real para el manejo manual por parte del operario.

En las pruebas realizadas podemos ver que principalmente se hicieron pruebas de parada

y continuación y que al reactivar los comportamientos se retornaba de forma correcta al

estado anterior de movimiento.

46

5. CONCLUSIONES

Para concluir revisaremos los objetivos y como se ha conseguido completarlos:

1. Análisis de capacidades de los robots aéreos: Se ha hecho una revisión de los com-

portamientos y de las capacidades existentes para extenderlo. Esto ha sido trabajo de

investigación de los comportamientos y los elementos con los que estos interactúan.

2. Diseño de nuevos algoritmos para la mejora o ampliación de capacidades actuales: Se

ha realizado un diseño de mejora de algunos comportamientos existentes y se ha realizado

otros que permitan una mejora en las capacidades. Esta proporciona mayor flexibilidad

para la incorporación de nuevos comportamientos.

3. Diseño de soluciones integradas al código ya desarrollado: Se ha usado código ya

existente y se han generados nuevos comportamientos a partir de estos. Se han tenido

que realizar modificaciones y adaptación de algunos módulos relacionados con ellos para

conseguir un funcionamiento adecuado.

4. Programación de las soluciones: Se han desarrollado los modulos adecuados y se ha co-

dificado de acorde a las soluciones previamente diseñadas. Este se ha programado usando

ROS como middleware y C++ como lenguaje. Con ello se han generado 6 comportamien-

tos de los cuales cuatro suponen una mejora sobre los existentes anteriormente:

- Antiguos: behavior go to point; Nuevos: behavior generate path in geometric map y

behavior follow path in geometric Path.

- Antiguos: behavior follow object image; Nuevos: behavior select object image y beha-

vior follow object image.

Y dos nuevos: behavior self localize by odometry y behavior request operator confirm

recognition.

5. Evaluación y pruebas del funcionamiento de las soluciones: Se ha evaluado los nuevos

módulos producidos y se han puesto a prueba en situaciones reales. Las pruebas realizadas

se han podido observar en apartados anteriores y se han logrado que los comportamientos

47

implementados realicen su función de forma correcta.

A partir de aquí se plantean nuevos comportamientos, nuevas posibles formas de genera-

ción de trayectorias o incluso inclusión de funcionalidad nueva en alguno de los compor-

tamientos existentes para aumentar su funcionalidad u optimizar la misma.

6. REFERENCIAS

[1] O. S. R. Foundation, ROS tutorials. dirección: http://wiki.ros.org/ROS/

Tutorials.

[2] J. Sanchez-Lopez, M. Molina, H. Bavle, C. Sampedro, R. A. S. Fernandez, J.Pestana

y P. Campoy, «multi-layered component-based approach for the development of ae-

rial robotic systems: the aerostack framework», journal of Intelligent and Robotic

Systems, págs. 1-27, 2007.

[3] B. Siciliano y O. Khatib, Springer Handbook of Robotics. Berlin,Heidelberg: Sprin-

ger, 2016.

48

trabajo fin de master extensión de las capacidades de...

Documents