T reba ll F ina l de G rau
AUTOMÀTICA
Desarrollo e implementación de varios algoritmos de exploración de entornos
desconocidos
ALBERTO MARÍN VALLS
Tutor
Javier Antich Alberto Ortiz
Escola Politècnica Superior
Universitat de les Illes Balears
Índice general i
Índice de figuras iii
Índice de tablas v
Resumen vii
1 Introducción 1
1.1 Robots móviles autónomos . . . 1
1.2 Tarea de exploración de un entorno . . . 1
1.3 Objetivos . . . 3
1.4 Estructura del documento . . . 3
2 Algoritmo de exploración Random 5 2.1 Herramientas de ROS utilizadas . . . 6
2.1.1 Comunicaciones básicas en ROS . . . 6
2.1.2 MORSE . . . 7
2.1.3 Move Base . . . 8
2.1.4 Gmapping . . . 8
2.2 El algoritmo de exploración Random . . . 9
2.2.1 Integración del algoritmo en ROS . . . 9
2.2.2 Funcionamiento del algoritmo . . . 9
2.2.3 Problemas y soluciones ante un mensaje de aborto . . . 12
2.2.4 Ejemplos de ejecución . . . 18
3 Algoritmo de exploración MCDM 23 3.1 Conceptos teóricos . . . 23
3.2 Integración del algoritmo en ROS . . . 28
3.3 Funcionamiento del algoritmo . . . 28
3.3.1 El mapa topológico . . . 30
3.3.2 Procedimiento para el cálculo de los puntos objetivo . . . 33
3.4 Problemas y soluciones . . . 37
3.5 Ejemplos de ejecución con diversos pesos de criterios . . . 38
3.6 Ejemplos de ejecución en diversos entornos . . . 44
4 Estudio comparativo 47
1.1 Mapa de entorno desconocido a medio explorar . . . 1
1.2 Área a explorar donde no se puede completar al 90 % . . . 2
2.1 Área de búsqueda de puntos inexplorados del entorno . . . 5
2.2 Mensajes intercambiados entre los nodosMove Base,Gmapping,MORSEy Random Exploration. . . 6
2.3 Ejemplo de dos nodos comunicándose a través de un topic . . . 7
2.4 El robot PR2 en una simulación . . . 7
2.5 Dos vistas diferentes de un mismo mapa . . . 9
2.6 Evaluación de las condiciones para ser un punto objetivo . . . 11
2.7 Punto objetivo dentro de una columna . . . 13
2.8 Problema y solución a la asignación de puntos objetivo en el exterior del entorno . . . 14
2.9 Punto objetivo muy cercano a un obstáculo . . . 15
2.10 Punto objetivo situado a un radio inferior al radio máximo de la base del robot 16 2.11 Aplicación del algoritmo de puntos inalcanzables sin inflado de obstáculos 17 2.12 Aplicación del algoritmo de puntos inalcanzables en un mapa con los obs- táculos inflados . . . 18
2.13 Exploración de una habitación vacía mediante el algoritmo Random . . . . 20
2.14 Exploración de un entorno con habitaciones mediante el algoritmo Random (primera parte) . . . 21
2.15 Exploración de un entorno con habitaciones mediante el algoritmo Random (segunda parte) . . . 22
3.1 Criterios . . . 24
3.2 Comunicaciones de ROS con el algoritmo de exploración MCDM . . . 28
3.3 Casos considerados en la creación del mapa topológico . . . 31
3.4 Ejemplo mapa topológico . . . 33
3.5 Selección de celdas candidatas . . . 34
3.6 Visibilidad de nodos desde posiciones inaccesibles . . . 37
3.7 Problema de visibilidad entre nodos y candidatos con mapa inflado . . . . 38
3.8 Entorno con dos habitaciones y una puerta que las comunica . . . 39
3.9 Exploración de una habitación vacía mediante el algoritmo de exploración MCDM . . . 44
3.10 Mapa topológico resultante de la exploración de una habitación vacía me- diante el algoritmo de exploración MCDM . . . 45
5.1 Problemas encontrados al implementar el algoritmo Random . . . 56 5.2 Entornos donde se han realizado las simulaciones . . . 58
2.1 Parámetros utilizados durante las simulaciones . . . 18
3.1 Configuración de pesos dando más importancia a la nueva información que al coste del viaje . . . 26
3.2 Utilidades calculadas para cada criterio . . . 26
3.3 Configuración de pesos incluyendo los subconjuntos . . . 27
3.4 Utilidades globales calculadas con laIntegral Choquet . . . 27
3.5 Configuración de pesos del artículo . . . 39
3.6 Configuración de pesos equivalente a una media ponderada . . . 40
3.7 Resultados obtenidos con la configuración de pesos del artículo . . . 40
3.8 Resultados obtenidos con la configuración de pesos equivalente a una media ponderada . . . 41
3.9 Configuración de pesos 3 . . . 42
3.10 Resultados obtenidos con la configuración de pesos 3 . . . 42
3.11 Configuración de pesos 4 . . . 42
3.12 Resultados obtenidos con la configuración de pesos 4 . . . 43
3.13 Configuración de pesos 5 . . . 43
3.14 Resultados obtenidos con la configuración de pesos 5 . . . 43
4.1 Resultados en una habitación vacía . . . 48
4.2 Resultados en un entorno con varias habitaciones . . . 50
4.3 Resultados en un entorno con varias habitaciones con puertas entreabiertas 51 4.4 Resultados en el laberinto . . . 52
5.1 Funciones implementadas para realizar el algoritmo MCDM . . . 58
5.2 Resultados obtenidos con los dos algoritmos y en los cuatro entornos defini- dos en la figura 5.2 . . . 58
La exploración y la generación de mapas, dentro de la robótica móvil, es de las tareas más importantes, con múltiples aplicaciones, como la exploración de lugares peligrosos o la búsqueda y rescate de personas. Es por ello que es interesante conseguir que un ro- bot pueda explorar toda una zona específica en el menor tiempo posible y recorriendo la mínima distancia.
En este trabajo se han implementado dos algoritmos de exploración claramente diferenciados, mientras el primero destaca por su simplicidad, el segundo requiere de un estudio previo del entorno antes de desplazar el robot. Se realizará una comparativa de la eficiencia de los dos algoritmos a la hora de explorar. Para ello se han creado cuatro entornos diferentes para poder observar y analizar el comportamiento de los algoritmos en cada uno de ellos.
Palabras clave: algoritmos, exploración, entornos desconocidos, robótica, ROS.
C
APÍTU1
I NTRODUCCIÓN
1.1 Robots móviles autónomos
Un robot móvil autónomo es un vehículo cuyos objetivos incluyen, entre otros, des- plazarse de forma autónoma en un entorno ya sea desconocido o conocido, tomando las decisiones oportunas basadas en observaciones tomadas en ese entorno. Estas observaciones son obtenidas por el robot a través de unos sensores que lleva acoplados por los cuales obtiene información del lugar donde opera.
1.2 Tarea de exploración de un entorno
El presente trabajo se va a centrar en la tarea de exploración de un entorno. La ex- ploración es la tarea de guiar un vehículo cubriendo toda la zona a explorar con sus sensores, de forma que obtenga información del entorno donde se encuentra, la posi-
Figura 1.1: Mapa de entorno desconocido a medio explorar
un entorno:
• Para realizar la exploración se define un área a explorar, en donde el robot hará los movimientos pertinentes para descubrir el entorno. Previamente, se le indica al robot que finalice la exploración cuando haya descubierto un cierto porcentaje de todo el área a explorar, el cual puede rondar el 90 %. Pero hay que tener en cuenta que no siempre todo el área es accesible para el robot. Como se puede observar en la figura 1.2, el entorno que puede explorar el robot son las dos habitaciones y el pasillo que las comunica, toda la zona externa nunca podrá ser descubierta, por tanto en este caso no podría completarse el 90 % de la exploración del rectángulo indicado.
Figura 1.2: Área a explorar donde no se puede completar la exploración al 90 %
• El robot realiza la exploración desplazándose hasta unos puntos objetivo, de manera que por el camino vaya recogiendo información del entorno para la construcción del mapa. La elección de estos puntos objetivo se programa previa- mente para que según donde se encuentre el robot y la información obtenida del entorno, se le indique que se desplace hasta un punto que convenga. En ocasio- nes los puntos objetivo se seleccionan de manera aleatoria, pero en realidad hay que tener en cuenta que hay ciertas zonas del mapa a las que el robot no puede acceder, como puede ser cualquier lugar de color gris de la figura 1.2. Si al robot se le indica que vaya hasta cualquier punto inaccesible para él, éste empieza a
realizar unos comportamientos extraños porque no sabe como llegar al lugar. Por ello hay que evitar este tipo de situaciones.
La exploración en robótica móvil es una tarea con una gran cantidad de aplicaciones.
Entre muchas otras:
• Realizar mapeados de lugares desconocidos con una gran precisión y de lugares de difícil acceso para el ser humano.
• La exploración de terrenos para la detección de minas. Es muy importante evitar riesgos sobre las personas que utilizan equipos de detección manuales con la utilización de robots autónomos que puedan localizar las minas.
• La exploración de entornos también se podría aplicar a un robot aspiradora, para que de forma eficiente recorra toda una casa limpiándola. Pero equipar un robot con todos los elementos necesarios para realizar dicha tarea es muy caro, por ello los robots aspiradora que hay en el mercado optan por otros sistemas más asequibles.
1.3 Objetivos
El objetivo de este trabajo es la implementación de dos algoritmos de exploración, de forma que un robot terrestre de forma autónoma genere un mapa del entorno. Se llevará a cabo una comparativa de la eficiencia de cada algoritmo a la hora de construir un mapa completo para identificar cuál de los dos actúa de la mejor forma.
El nivel de complejidad de los algoritmos en bastante diferente, mientras uno desta- ca por su simplicidad, el otro tiene una mayor carga computacional. Dependiendo del entorno donde se apliquen cada uno de los algoritmos, variará el comportamiento de éstos. Para poder realizar la comparativa se definirán un conjunto de entornos variando su complicación y así poder determinar en qué situaciones el comportamiento de los algoritmos es mejor.
Todo lo previamente descrito se realizará sobre el sistema operativo ROS (Robot Operating System). ROS es una plataforma de desarrollo abierto que provee toda una serie de librerías y servicios que ayudan significativamente a la creación de software para robótica. Tiene un diseño modular, de modo que los usuarios pueden utilizar solo lo que necesiten de ROS sin tener que configurarlo completamente. Esta modularidad permite seleccionar qué partes ya diseñadas se aprovecharán para usarlas en el trabajo y de esta manera poder centrarse simplemente en la implementación de los algoritmos.
1.4 Estructura del documento
Este documento se estructura en 5 capítulos. A continuación se muestra una breve descripción de cada no de ellos:
• Capítulo 1: Introducción, presenta una introducción al trabajo.
C
APÍTU2
A LGORITMO DE EXPLORACIÓN R ANDOM
El primer algoritmo de exploración que se decide implementar es el Random. Al ser un algoritmo sencillo nos permite centrarnos en entender y configurar las comunicaciones de todo el entorno de ROS, además de visualizar la forma de actuar del robot en ciertos casos en los que su comportamiento no fuera el deseado y poderlo depurar.
Este método de exploración consiste en que el robot explore puntos inexplorados del mapa que son seleccionados de forma aleatoria dentro de un círculo de radioR centrado en la posición actual del robot, tal y como se muestra en la figura 2.1a. En el caso de que no se encuentre ningún punto inexplorado en la distancia indicada, se duplicará el radio del mencionado círculo (ver figura 2.1b). Dicho radio seguirá creciendo en incrementos deRunidades mientras la anterior circunstancia se siga dando.
(a) Radio de búsqueda inicial (b) Aumento del radio de búsqueda Figura 2.1: Área de búsqueda de puntos inexplorados del entorno
interactuando con dichos paquetes.
Figura 2.2: Mensajes intercambiados entre los nodosMove Base,Gmapping,MORSEy Random Exploration
2.1.1 Comunicaciones básicas en ROS
Las comunicaciones en ROS están compuestas de los siguientes elementos:
• Nodos: los nodos son programas ejecutables que ejecuta el computador para cumplir con una función. Cada nodo que se ejecuta tiene un nombre único que lo identifica. Para el funcionamiento de esta simulación se utilizan varios nodos que se explicarán más adelante. Un beneficio de utilizar nodos es que hace al conjunto del sistema más robusto ante fallos; es decir, si un nodo falla, el resto de nodos del sistema puede seguir funcionando.
• Mensajes: un mensaje es una estructura de datos que utilizan los nodos para comunicarse entre ellos. Son soportados tanto los tipos primitivos (booleano, entero, etc) como los tipos compuestos (por ejemplo, arrays).
• Topic: es el mecanismo mediante el cual los nodos intercambian mensajes. Los mensajes se enrutan mediante un publicador y un subscriptor. Un nodo publica un mensaje por untopicdeterminado. Si hay un nodo que necesita un tipo de dato en concreto, debe suscribirse altopicen cuestión para recibirlo. Puede haber
varios publicadores y subscriptores simultáneos en cadatopic. El nombre del topicse puede utilizar para identificar el contenido del mensaje.
• Master: es el nodo principal de ROS, encargado de hacer las conexiones entre los nodos que requieran comunicarse.
A modo de ejemplo, el gráfico de la figura 2.3 muestra un pequeño sistema com- puesto por dos nodos,teleop_turtleyturtlesim, los cuales son representados mediante una elipse. Éstos se comunican a través de untopicllamadoturtle1/command_velocity por el cual se envían los mensajes. La flecha indica queteleop_turtlees el nodo que envía mensajes (el nodo publicador) por dichotopicyturtlesimes el nodo que se ha suscrito altopicpara recibirlos.
Figura 2.3: Ejemplo de dos nodos comunicándose a través de un topic
2.1.2 MORSE
MORSE (Modular OpenRobots Simulation Engine) es un simulador genérico para robó- tica académica. Permite la simulación de uno o varios robots móviles en entornos 3D complejos y realistas. Las escenas (entendiendoescenacomo el conjunto del entorno más el/los robot/s) se describen mediante scripts escritos en lenguaje Python.
MORSE cuenta con un gran conjunto de sensores, como pueden ser escáners lásers o cámaras de vídeo, además de actuadores y robots completos ya creados.
(a) El robot PR2 (b) Rango de detección del escáner láser Hokuyo
Figura 2.4: El robot PR2 en una simulación
Uno de los robots que viene completamente definido en MORSE es el robot PR2 (ver figura 2.4a). Éste ha sido el robot que se ha utilizado en este trabajo. Como se puede observar en la figura 2.4b, el robot PR2 dispone de un sensor láser Hokuyo que le permite detectar la presencia de obstáculos. Este láser cubre una distancia de hasta 30
que esté subscrito a los siguientes topics para su correcto funcionamiento:
• Goal: Es el punto objetivo a alcanzar por el robot. Estetopiclo genera el nodo creado por nosotros, denominadoRandom Exploration.
• Map: El nodoGmappinggenera un mapa del entorno y lo publica en estetopic.
El nodoMove Baseutiliza este mapa para planificar un camino hasta el punto objetivo.
• Odom: Este es untopicpublicado por MORSE. Indica en qué posición se encuen- tra el robot en todo momento. Esta información es necesaria para situar al robot en el mapa.
Además, el nodoMove Basepublica, a través deltopic cmd_vel, la velocidad a la que se deben mover las ruedas del robot con el objetivo de seguir el camino que se ha planificado. El simulador MORSE consulta estetopicy se encarga de mover el robot a la velocidad indicada.
Por último, el nodoMove Baseprovee información importante sobre el estado de la acción que está actualmente ejecutando (topic status). Más concretamente, indica si se ha llegado al punto objetivo de manera satisfactoria o, en caso contrario, si se ha tenido que abortar debido a que el robot ha encontrado algún impedimento para alcanzar dicho punto.
2.1.4 Gmapping
El paqueteGmappingse encarga de crear un mapa del entorno basándose en: un mapa local de ocupación a partir de la información proporcionada por el láser (topic Base scan); la información sobre la posición actual del robot (topic Odom). Un mapa no es más que una matriz en donde cada celda tendrá un valor decimal que determina su estado (ver figura 2.5a). En esta figura, las celdas con valor de 100 representan un obstáculo, un 0 es un espacio libre y, finalmente, un -1 representa el lugar por dónde el láser no ha hecho todavía ningún barrido, con lo cual es una zona inexplorada. Esta información, procesada con un pequeño programa en Python, se puede transformar en una imagen (ver figura 2.5b).
El mapa generado, como se ha comentado anteriormente, es utilizado por el nodo Move Base, al igual que por el nodoRandom Exploration.
(a) Matriz (b) Imagen equivalente
Figura 2.5: Dos vistas diferentes de un mismo mapa
2.2 El algoritmo de exploración Random
2.2.1 Integración del algoritmo en ROS
Se ha creado un nuevo paquete en ROS denominadoRandom Exploration. En este paquete se ha implementado el algoritmo de exploración Random. Este algoritmo es el encargado de indicarle al robot a qué puntos tiene que moverse para realizar una exploración completa del entorno. Se utiliza eltopic Goalpara publicar el punto del entorno al que al robot debe ir actualmente.
El nodoRandom Explorationrequiere información del mapa y de la odometría del robot para saber en qué lugar del entorno se encuentra el vehículo y decidir cuál debe ser el próximo punto objetivo. Es más, el nodoRandom Explorationnecesita también conocer el estado de la acción llevado a cabo por el nodoMove Base.
En la figura 2.2 se puede observar cómo se intercambia información entre los dis- tintos nodos que son necesarios para realizar la simulación.
2.2.2 Funcionamiento del algoritmo
En este apartado se va a explicar el funcionamiento del algoritmo de exploración Random que se ha implementado. Aunque en un principio parece que es un algorit- mo simple, se han encontrado numerosos problemas a la hora de programarlo y de comprender el comportamiento del robot en ciertas situaciones que más adelante se detallarán.
En primer lugar, como se puede ver en las líneas 3−5 del algoritmo 1, antes de seleccionar a qué punto del mapa hay que mandar al robot, se tiene que hacer una lectura de lostopics MapyOdomcon el objetivo de obtener una copia del mapa del entorno junto a la posición en la que se encuentra el robot en ese momento. Hasta que no se ha recibido esta información, se mantiene a la espera de obtenerla.
En las líneas 6−11, se calcula el porcentaje de mapa que ya ha sido explorado mediante la función calcularPorcentajeExplorado(). El valor devuelto por esta función se compara con un porcentaje, cuyo nombre esporcentajeAExplorar. Este porcentaje
10: finMision=verdadero
11: fin si
12: sise produce abortoentonces
13: sino hay obstáculo cercanoentonces
14: actualizarMapaInflado();
15: expandirPosicionesInalcanzables();
16: fin si
17: fin si
18: fin mientras
constituye unos de los parámetros del algoritmo Random. Si el valor devuelto por la función calcularPorcentajeExplorado() es mayor queporcentajeAExplorar, se da por terminada la exploración; en caso contrario, se llama la función RandomPoint(). En esta última función se selecciona un punto objetivo de manera aleatoria dentro de un radio Rexpcentrado en la posición actual del robot. Para que un punto del entorno/celda del mapa pueda convertirse en punto objetivo debe cumplir las siguientes condiciones:
C1. La celda ha de estar a una distancia igual o inferior aRexp con respecto a la posición actual del robot.
C2. El estado de la celda debe serinexplorada. Además, al menos una de sus 8 celdas vecinas debe tener el estado delibre de obstáculos.
C3. El obstáculo más próximo a la celda debe encontrarse a una distancia superior a los 0,334 metros. Este valor proviene del hecho de que la base del robot PR2 es una plataforma cuadrada de tamaño 0,668 metros por cada lado.
C4. La celda y la posición actual del robot deben encontrarse, como mínimo, a una distancia igual a 0,334 metros.
C5 Y, finalmente, la celda no ha de estar en la lista de posiciones a las que el robot no puede acceder. En esta lista se recogen los puntos del entorno que son inac- cesibles para el robot, bien sea por la existencia de espacios cerrados o por la existencia de espacios con entradas de tamaño inferior a las dimensiones del robot.
Estas condiciones son explicadas con más detalle en la sección 2.2.3.
En la figura 2.6 se muestra un ejemplo de una celda que no cumple todas las con- diciones anteriores y otra que sí las cumple. En estas imágenes el punto de color azul representa la posición actual del robot y el centro de la pequeña circunferencia roja es la celda considerada. En la figura 2.6a, la celda cumple todas las condiciones con la excepción de laC3 (la zona marcada en color verde incluye, de forma aproximada, a todas aquellas celdas que tienen un obstáculo a una distancia menor o igual a 0,334 metros; por tanto, es evidente que la celda incumple la condiciónC3). En cambio, en la figura 2.6b, la celda sí que cumple cada una de las cinco condiciones establecidas.
(a) Incumplimiento de la con- dición C3
(b) Cumplimiento de todas las condiciones
Figura 2.6: Evaluación de las condiciones para ser un punto objetivo
Una vez hecha la selección de todas las celdas que cumplen las condiciones ante- riormente indicadas, se escoge una aleatoriamente. En el caso en el que no se haya encontrado ninguna celda que cumpla dichas condiciones, el radioRexpse incrementa respecto del valor inicialRexpi ni y se vuelve a buscar celdas. Y así sucesivamente hasta que el valor del radioRexpsea igual de grande que la distancia de la diagonal del mapa, momento en el que se finaliza la exploración al no haber ninguna celda que cumpla las condiciones. En el caso de encontrar nuevas celdas, el valor deRexpse restablece en Ri niexpnuevamente para la siguiente búsqueda. Dicho procedimiento queda resumido en forma de pseudocódigo en el algoritmo 2.
Una vez se ha seleccionado el punto objetivo, se envían las coordenadas X e Y de éste al nodoMove Basemediante eltopiccorrespondiente para que el robot proceda a desplazarse hasta esa posición.
El programa se mantiene a la espera hasta recibir por parte del nodoMove Base la confirmación de que el robot ha alcanzado satisfactoriamente el punto objetivo o, alternativamente, la confirmación de que el robot no ha podido alcanzarlo en forma de un mensaje de aborto.
En el primer caso, el programa vuelve a seleccionar un nuevo punto objetivo de acuerdo con los criterios antes mencionados. De esta manera, se repite todo el proceso
10: puntoCalculado=verdadero;
11: Rexp=Rexpi ni;
12: sino
13: Rexp=Rexp+Ri niexp;
14: si(Rexp>diagonalMapa)entonces
15: finMision=verdadero;
16: fin si
17: fin si
18: fin mientras
hasta que el entorno quede explorado en un porcentaje mayor aporcentajeAExplorar.
Por otro lado, si se produce un aborto, hay que identificar cuál ha sido la causa y cómo hay que tratarlo. En la sección 2.2.3 se explica toda la causística vinculada con este error.
2.2.3 Problemas y soluciones ante un mensaje de aborto
Durante la implementación del algoritmo se han encontrando varias dificultades que se han ido subsanando añadiendo las condiciones definidas en la anterior sección para conseguir que el comportamiento del robot durante la exploración fuese adecuado.
Para la mejor comprensión de esta sección imaginemos que en un principio solo se ha implementado la condiciónC1.
2.2.3.1 Punto objetivo dentro de un espacio cerrado
La situación más frecuentemente encontrada durante la implementación es el hecho de que el robot no pueda llegar al punto objetivo que se le indica porque es inaccesible para él. En un principio, el punto objetivo que se le manda al robot es una celda inex- plorada y elegida aleatoriamente, con lo cual se da el caso en muchas ocasiones que se le indica como objetivo una posición que se encuentra justo dentro de un obstáculo cerrado. Un ejemplo de ello puede ser una posición en el interior de una columna tal y como muestra la figura 2.7a. El color negro corresponde al obstáculo, el blanco a la zona libre de obstáculos y el gris a la zona inexplorada, mientras que el color rojo indica
el punto objetivo marcado. El robot estaría situado en el exterior del recinto cerrado.
Previamente al momento de la figura 2.7a, toda esa zona es desconocida. El robot explora todos los alrededores para intentar acceder al objetivo marcado, pero cuando detecta que el destino es inaccesible para él porque esta en un espacio cerrado (2.7a) se queda parado y realiza dos vueltas sobre si mismo. Con ello ejecuta un nuevo escaneo de la zona limpiando las posibles lecturas erróneas del sensor láser que indiquen un obstáculo que no existe y que haya quedado plasmado en el mapa. Tras ello, si sigue sin encontrar un camino válido, genera un aborto y se queda parado a la espera de recibir un nuevo objetivo. Este proceso en el que realiza un segundo escaneo puede tardar unos 15 segundos, por lo que es interesante evitar que se generen abortos a menudo porque es una gran cantidad de tiempo que se pierde.
Lógicamente, todas las zonas interiores de un obstáculo, como pueden ser las de una columna, son zona desconocida y un punto potencial a ser punto objetivo para el robot. Por esto, se siguen seleccionando como puntos objetivo puntos interiores a la columna, de manera que se van generando más abortos.
(a) Antes de aplicar el algoritmo (b) Después de aplicar el algoritmo
Figura 2.7: Punto objetivo dentro de una columna
Para evitar este hecho, se ha creado el algoritmo de expansión de puntos inalcanza- bles. Este algoritmo funciona de la siguiente manera, en el momento en que se genera el aborto, a partir de la celda que era el destino del robot, se marcan los puntos que rodean a ésta como posiciones inalcanzables, con la condición de que las celdas no correspondan a un obstáculo, y, a su vez, se comprueba a partir de cada una de las celdas que rodean la celda inicial si las celdas que la rodean no son obstáculos para marcarlas como puntos inalcanzables, y así sucesivamente. Es decir, es un algoritmo que a partir de un punto inicial, se expande en todas direcciones hasta que encuentra un obstáculo. En la figura 2.7b se muestra el resultado final de aplicar el algoritmo después de generarse el aborto sobre el objetivo (color rojo) y así tener un registro de las posiciones inalcanzables para el robot (color verde).
De esta manera, cuando se eligen los posibles objetivos a los cuales enviar al robot a explorar, se comprueba que no correspondan con ninguna posición del registro de posiciones inalcanzables (condiciónC5), evitando así un comportamiento no deseado del robot e impedir una gran pérdida de tiempo.
(a) Punto objetivo en el exte- rior del entorno
(b) Punto objetivo inexplorado con celda vecina libre de obstáculos
Figura 2.8: Problema y solución a la asignación de puntos objetivo en el exterior del entorno
La solución que se ha aplicado es la de exigir que la celda candidata a explorar sea inexplorada y que al menos uno de sus ocho celdas vecinas esté libre de obstáculos (ver figura 2.8b), que es la condiciónC2indicada anteriormente. Con ello se consigue que los puntos que pueden ser elegidos de manera aleatoria estén en una zona ya parcialmente explorada y en la frontera entre la parte todavía oculta del mapa y la ya conocida. Y además, evita que se escoja como candidato un punto en el exterior del entorno.
2.2.3.3 Punto objetivo muy cercano a un obstáculo
Otro problema que se ha tratado es el hecho de que cuando se le indica un objetivo muy cercano a un obstáculo, al robot le es imposible llegar a él porque no hay espacio suficiente para que pueda colocarse en esa posición (ver figura 2.9).
El nodoMove Basea partir del mapa que recibe, genera un nuevo mapa llamado costmap. Éste indica las posiciones a las cuales no puede acercarse el robot. Básicamen-
Figura 2.9: Punto objetivo muy cercano a un obstáculo
te realiza un inflado de un radio R sobre las celdas que corresponden a un obstáculo.
El problema es que no se ha podido tener acceso alcostmap, por lo que se ha op- tado por añadir una nueva condición para la selección de candidatos a ser objetivos, la condiciónC3, en la que se comprueba que no haya ninguna celda correspondiente a un obstáculo en el área formado por un radio igual al radio del robot desde la celda candidata. De esta forma, se evita marcar como punto objetivo una posición inaccesible para el robot debido a la cercanía de un obstáculo.
Pero también se puede dar el caso en el que el objetivo seleccionado, en un princi- pio, no tiene ningún obstáculo más cercano al radio del robot porque todavía no ha sido descubierto, pero en realidad sí que está. Si sucede tal cosa, el robot en el momen- to que descubre que hay un obstáculo muy cercano a la posición del punto objetivo, genera un aborto. Pero como se ha comentado anteriormente, el algoritmo de puntos inalcanzables se aplica en el instante en el que se produce el aborto. Para controlar que no se aplique dicho algoritmo en este tipo de situaciones en las que no es necesario aplicarlo, se comprueba en el momento de la interrupción, ya con el mapa actualizado, si hay un obstáculo en las cercanías del punto objetivo.
2.2.3.4 Objetivo muy cercano a la posición del robot
Otra situación que se ha detectado se produce cuando se le asigna al robot como punto objetivo una posición muy cercana a la que se encuentra. Si el punto objetivo esta a menos de 0.334 metros de la posición del robot (ver figura 2.10), nunca va a poder descubrir con su sensor láser qué contiene esa zona, ya que la base del robot tiene de radio esa misma medida y el sensor láser está situado en uno de los lados de la base.
Figura 2.10: Punto objetivo situado a un radio inferior al radio máximo de la base del robot
En muchas ocasiones lo que ha ocurrido es que los alrededores del robot ya estaban explorados, menos una pequeña zona justo en el área que ocupaba la base del robot;
entonces siempre se asignaba como objetivo un punto de esa zona de forma infinita, ya que nunca lo podía explorar. Por ello se ha añadido la condiciónC4, la cual obliga a que un punto objetivo esté situado a una distancia mayor a 0.334 metros de la posición del robot.
2.2.3.5 Objetivo en lugar inaccesible
También hay ciertas ocasiones en las que se generan objetivos en lugares inaccesibles para el robot. Por ejemplo, en la figura 2.11a se puede observar un mapa generado justo cuando el robot ha llegado al punto objetivo indicado con un punto rojo. En la parte inferior se puede contemplar una habitación con una puerta entreabierta, lo que posibilita que se generen puntos candidatos dentro de ella al haber celdas inexploradas con vecinos libres de obstáculos y sin obstáculos cercanos. Pero el vehículo no puede acceder a ese lugar al ser muy estrecho su acceso.
En estas situaciones, el robot intenta acceder al objetivo por todos los lugares po- sibles, y va descubriendo gran parte del entorno buscando algún camino posible que comunique hasta el objetivo. Cuando el robot tiene la información necesaria del en- torno para decir que no puede acceder a la zona deseada se genera un aborto. El mapa obtenido justo en el momento de la interrupción se muestra en la figura 2.11b. Co- mo se puede comprobar no hay camino posible para acceder a la habitación. En este preciso instante se debe aplicar el algoritmo de puntos inalcanzables, pero surge un nuevo problema. En este caso, el objetivo no está del todo cerrado entre paredes, hay pequeños huecos como puede ser la puerta entreabierta. Entonces a la hora de aplicar el algoritmo (ver figura 2.11c) se extienden los puntos inalcanzables por todo el mapa, y en la siguiente selección de un punto objetivo, se encuentra que no hay ninguno disponible, al estar todos en la lista de puntos inaccesibles.
(a) Mapa en el objetivo previo al aborto (b) Mapa en el aborto
(c) Después de aplicar el algoritmo
Figura 2.11: Aplicación del algoritmo de puntos inalcanzables sin inflado de obstáculos
Para subsanar este inconveniente, se ha optado por crear una copia del mapa in- flando los obstáculos un radio igual al radio de la base del robot. De esta manera las zonas inaccesibles quedan encerradas por obstáculos y al aplicar el algoritmo no se expanden por todo el mapa los puntos inalcanzables.
El procedimiento es el siguiente. En el momento en que se genera el aborto se recibe la última actualización del mapa (figura 2.12a), se crea una copia de éste y se procesa. Se realiza una búsqueda de todas las celdas que corresponden a un obstáculo del mapa inicial, y en todas estas mismas posiciones se crea una circunferencia de obstáculos en la copia del mapa.
Sobre este mapa inflado (figura 2.12b) se aplica el algoritmo de puntos inalcan- zables. Como se puede observar en la figura 2.12c las posiciones inaccesibles no se extienden por todo el mapa gracias al inflado de los obstáculos, sino que se expanden únicamente por las zonas a las que el robot no puede llegar.
(a) Mapa normal (b) Mapa inflado
(c) Mapa inflado aplicando los puntos inalcanzables
Figura 2.12: Aplicación del algoritmo de puntos inalcanzables en un mapa con los obstáculos inflados
2.2.4 Ejemplos de ejecución
Se ha testeado el funcionamiento del algoritmo de exploración Random en dos entor- nos diferentes. En la tabla 2.1 se puede ver los valores de los parámetros que se han utilizado para realizar las simulaciones.
Parámetro Valor Rexpi ni 5 m porcentajeAExplorar 90 %
Tabla 2.1: Parámetros utilizados durante las simulaciones
El primer entorno a explorar es una habitación cuadrada totalmente vacía. En la figura 2.13 se recoge cómo se va actualizando el mapa a medida que el robot recorre el entorno. Debido a la sencillez de éste, el robot no tiene grandes problemas en explorarlo completamente, ya que, a parte de las paredes que delimitan la habitación, el robot no se encuentra con ningún otro obstáculo.
Se puede observar como en cada punto objetivo el robot gana mucha información del entorno. Es por ello que en este ejemplo el robot consigue explorar gran parte del entorno visitando sólo 5 puntos objetivo (ver figura 2.13e). Tras el quinto punto objetivo, al robot todavía le queda por descubrir la esquina superior derecha de la habitación.
Esa esquina es explorada cuando el robot se desplaza al sexto punto objetivo. En ese momento, mas del 90 % del entorno ha sido ya explorado y, por tanto, la exploración acaba.
A modo de resumen, en este primer entorno, para realizar una exploración de más del 90 %, se han tenido que generar seis puntos objetivo. El robot ha recorrido una distancia total de 23,61 metros y ha tardado 54 segundos en recorrer esta distancia.
El segundo entorno consiste en un conjunto de habitaciones conectadas por dos pasillos. Este es un entorno sumamente más complejo que el anterior que nos permitirá evaluar la eficiencia del algoritmo de exploración Random.
En las figuras 2.14 y 2.15 se observa, paso a paso, la ejecución del algoritmo en el entorno indicado. Como se puede ver, en la primera parte de la exploración, el robot descubre rápidamente gran parte del entorno.
Pero el mayor problema que tiene este algoritmo se evidencia en la segunda se- cuencia de imágenes (ver figura 2.15). Cuando el robot se encuentra en la parte inferior del pasillo (punto rojo de la figura 2.15a), junto a la entrada de una habitación todavía sin explorar completamente (de aquí en adelante, haremos referencia a esta habita- ción como A), se le asigna como punto objetivo la habitación que se encuentra en la esquina superior derecha de la imagen (ver figura 2.15b; de aquí en adelante, haremos referencia a esta habitación como B). Para poder llegar a esa habitación (B), el robot tiene que realizar un gran recorrido, si lo comparamos con el que tendría que hacer para acabar de explorar la habitación A. Además, después de explorar la habitación B, el robot deberá recorrer de nuevo el pasillo para poder explorar la habitación, dando así por concluida la exploración. Este comportamiento es consecuencia de la aleatoriedad que existe a la hora de elegir un punto objetivo del entorno.
A modo de resumen, en este segundo entorno, el robot ha tenido que visitar 17 puntos objetivo. Además, ha recorrido un total de 95,21 metros, tardando 220 segundos en completar la exploración del entorno. El mapa resultante de esta ejecución es el que se muestra en la figura 2.15c.
(a) Mapa con el robot situa- do en suposición inicial
(b) Punto objetivo 2
(c) Punto objetivo 3 (d) Punto objetivo 4
(e) Punto objetivo 5 (f ) Último punto objetivo (6)
Figura 2.13: Exploración de una habitación vacía mediante el algoritmo Random
(a) Mapa con el robot situa- do en su posición inicial
(b) Punto objetivo 6
(c) Punto objetivo 8 (d) Punto objetivo 11
Figura 2.14: Exploración de un entorno con habitaciones mediante el algoritmo Ran- dom (primera parte)
(a) Punto objetivo 14 (b) Punto objetivo 15
(c) Último punto objetivo (17)
Figura 2.15: Exploración de un entorno con habitaciones mediante el algoritmo Ran- dom (segunda parte)
C
APÍTU3
A LGORITMO DE EXPLORACIÓN MCDM
En este capítulo se van a explicar los conceptos teóricos y la implementación del algoritmo de exploración Multi-Criteria Decision Making (MCDM). Este método de exploración es bastante más complejo que el algoritmo de exploración Random, ya que se hace un estudio previo a la elección del punto objetivo a explorar.
Dicho punto será seleccionado teniendo en cuenta una serie de criterios que serán analizados para que el entorno se explore de una forma completa de la manera más eficiente posible, intentando reducir el tiempo de exploración y la distancia recorrida por el robot. El hecho de que sea un algoritmo más complejo implica que tiene una mayor carga computacional que se traduce en una pequeña espera para que el robot seleccione el siguiente punto del entorno a visitar.
3.1 Conceptos teóricos
En primer lugar se va a hacer una explicación de la parte teórica de este algoritmo, dando a conocer cuáles son los criterios a tener en cuenta a la hora de seleccionar un punto objetivo entre una serie de candidatos y cómo realizar dicha selección.
La localización de los candidatos proviene de una lista de frontera en donde quedan registradas todas las posiciones que están en la frontera entre la parte inexplorada del mapa, y la parte explorada y libre de obstáculos. Por cada localización de un candidato p, se realiza una evaluación a partir de cuatro criterios. Para explicar dichos criterios se va a utilizar el entorno que se muestra en la figura 3.1. Es una pequeña habitación rectangular, donde la región de color gris representa el área inexplorada por el robot, mientras que la región blanca es la zona libre de obstáculos. Las líneas negras que forman un rectángulo son los obstáculos detectados por el robot. Por otro lado, el punto azul representa la posición actual del robot; en cuanto al de color rojo es el punto
(a) iArea (b) iSeg
(c) Overlap (d) Mapa topológico para el cálculo del coste
Figura 3.1: Criterios
• El primero de ellos esiArea(p). Es el tamaño del área desconocida desde el punto pen un radio R que corresponde al rango de detección del sensor láser del robot.
Este criterio nos indica la cantidad de nueva información que se puede ganar viajando a ese lugar. En la figura 3.1a se puede ver un ejemplo de la ganancia potencial de información (región de color verde) desde el punto candidatop.
• Otro de los criterios es el llamadoiSeg(p). Es calculado como la longitud de la frontera entre la parte conocida y libre de obstáculos del entorno, y la parte desconocida, visible en un radio R desde el puntop(representada a través de la línea verde en la figura 3.1b). O lo que es lo mismo, el número de posiciones que están registradas en la lista de fronteras en el área de radio R centrada sobre el puntop.
• Overlap,o(p), es el tercero de los criterios a tener en cuenta. Se define como la longitud de la frontera entre la parte conocida y no libre de obstáculos del entorno, y la parte desconocida, visible desde el puntopen un radio R (ver figura 3.1c).
• El último de los criterios es el coste del viaje, llamado de forma abreviadac(p).
Indica la distancia que ha de recorrer el robot desde su posición actual hasta el puntop. Para poder calcular dicha distancia, durante la exploración se construye un mapa topológico del entorno (ver figura 3.1d). Ese mapa registra los puntos del entorno que el robot ya ha visitado y la conectividad que existe entre estos puntos.
Con esta información se puede llegar a estimar cuál es la distancia mínima que separa al robot de un punto candidatopcualquiera. En el ejemplo de la figura 3.1d, dado ese mapa topológico, el camino más corto entre la posición del robot y el puntopes el indicado en verde.
Dado un criterioiy un punto candidatop, se calcula un valor de utilidadui(p) en el intervalo [0,1], con el fin de evaluar en una escala común la calidad de todos los puntos candidatos de acuerdo con cada criterio. La utilidad está definida por la normalización de todos los candidatos en la etapa de exploración actual. Por ejemplo, realizando los cálculos sobreiArea(p), siendoLel conjunto de puntos candidatos actuales, la utilidad ui Ar ea(p) conp∈Lse extrae de la siguiente función:
uiArea(p)=(iArea(p)−m´ın
q∈L iArea(q))/(m´ax
r∈L iArea(r)−m´ın
q∈L iArea(q)) (3.1) Las funcionesmínymáxrepresentan el valor mínimo y máximo de todo el conjunto de valores deiArea.
El objetivo es que cuanto mayor sea el valor de la utilidad, mejor sea la satisfacción del criterio. En otros criterios, como el coste del viajec(p), se deben penalizar los valores altos. Para ello, su utilidad se calcula con la siguiente función:
uc(p)=1−(c(p)−m´ın
q∈L c(q))/(m´ax
q∈L c(q)−m´ın
q∈L c(q)) (3.2)
Para la selección del punto objetivo, se calcula la utilidad global de cada candidato.
La función para el cálculo de la utilidad global debe tener en cuenta el grado de impor- tancia que se le da a cada uno de los criterios.
SeaNun conjunto dencriterios. Cada elemento j∈N denota un criterio simple, como puede ser eloverlap. Por otro lado,Les el conjunto de los actuales candidatos.
Por cada parp∈Lyj∈N se calcula un valor de utilidaduj(p).
Antes de entrar en la explicación de lo que nos propone el algoritmo MCDM para calcular la utilidad global, veremos primero cómo se ha hecho habitualmente este cálculo. SuponiendoN={i Ar ea,i Seg,c}, una forma sencilla de calcular la utilidad glo- bal consiste en realizar un promedio en base al establecimiento de un peso individual a cada criterio que refleje su importancia relativa. Si se quiere, por ejemplo, valorar más la nueva información que se pueda ganar viajando a un candidato, restando importancia
al coste del viaje
Imaginemos ahora tres puntos candidatos,L={p1,p2,p3}, y que las utilidades cal- culadas para cada criterio son las mostradas en la tabla 3.2.
Candidato iArea iSeg c Promedio p1 0,90 0,90 0,05 0,73 p2 0,70 0,70 0,60 0,68 p3 0,05 0,10 0,80 0,22
Tabla 3.2: Utilidades calculadas para cada criterio
Dados estos números, el candidato seleccionado seríap1al tener una mayor uti- lidad global. Pero si nos fijamos en los valores de utilidad de cada criterio, se puede observar que a pesar de satisfacer en gran medida los dos criterios más valorados (iArea eiSeg), en lo que se refiere al coste del viaje deja mucho que desear, cuando en realidad no se le está dando tan poca importancia. Por lo tanto, al ser elegidop1, el robot tendría que hacer un largo recorrido hasta dicho punto, algo que se quiere evitar.
MCDM da la posibilidad de establecer pesos en diferentes configuraciones de crite- rios, de manera que se pueda evitar el problema del ejemplo anterior. Este problema proviene del hecho de que cuando se hace un promedio se asume la independencia mutua entre los criterios, cosa que no siempre es cierta. Con MCDM se puede estable- cer el grado de redundancia entre criterios (como la que hay entreiAreaeiSeg) y el nivel de sinergia.
Para ello, MCDM aplica una técnica de agregación llamadaIntegral Choquet fuzzy.
Dentro de esta técnica, a la funciónµ: 2N →[0, 1] se le llama capacidad o medida
"fuzzy"del conjunto de criteriosN cuando satisface las siguientes tres propiedades:
• µ(;)=0
• µ(N)=1
• SiA⊂B⊂N entoncesµ(A)≤µ(B)
µ(A) representa el peso del subconjunto de criteriosA. De esta manera, como se ha dicho anteriormente, se asignan pesos a las combinaciones de criterios, indicando qué relación tienen entre ellos. Dado dos criterios,c1yc2, y sus pesosµ(c1) yµ(c2):
• Dos criterios son redundantes siµ({c1,c2})<µ(c1)+µ(c2)
• Dos criterios son sinérgicos siµ({c1,c2})>µ(c1)+µ(c2)
La utilidad globalu(p) de un candidatopse calcula a partir de laIntegral Choquet con respecto a la medida "fuzzy"µ:
u(p)=
n
X
j=1
(u(j)(p)−u(j−1)(p))µ({A(j)}), (3.3) donde(j)se refiere al índice después de haber realizado una permutación enN cambiando el orden tal queu(1)(p)≤u(2)(p)≤...≤u(n−1)(p)≤u(n)(p), yu(0):=0, yA(j)
es el subconjunto de los criterios dados porA(j)={u(j)(p)≤ui(p)≤u(n)(p)} una vez hecho la permutación.
Recuperando el ejemplo anterior, para calcular la utilidad global con laIntegral Choquet, además de definir los pesos individuales, también hay que dar un peso a todos los subconjuntos de criterios posibles (ver tabla 3.3).
Criterio Peso µ({i Ar ea}) 0,40 µ({i Seg}) 0,40 µ({c}) 0,20 µ({i Ar ea,i Seg}) 0,50 µ({i Ar ea,c}) 0,80 µ({i Seg,c}) 0,80 µ({i Ar ea,i Seg,c}) 1
Tabla 3.3: Configuración de pesos incluyendo los subconjuntos
Dados estos pesos, se establece una relación de sinergia entre los criteriosiAreayc, eiSegyc, mientras que entreiAreaeiSegse define una relación de redundancia. Si se aplica la función para el cálculo de la utilidad global, utilizando las mismas utilidades individuales del ejemplo anterior, los resultados son los mostrados en la tabla 3.4.
Candidato iArea iSeg c Integral Choquet
p1 0,90 0,90 0,05 0,48
p2 0,70 0,70 0,60 0,65
p3 0,05 0,10 0,80 0,23
Tabla 3.4: Utilidades globales calculadas con laIntegral Choquet
que se encuentra del robot.
3.2 Integración del algoritmo en ROS
Se ha creado un paquete llamado MCDM, en el cual se encuentra el algoritmo de exploración MCDM. Como se puede obvervar en la figura 3.2, las comunicaciones en este caso son exactamente las mismas que con el algoritmo de exploración Random. El algoritmo MCDM necesita la información del mapa ofrecida por el nodoGmapping, además de la odometría del robot que provee el simulador MORSE, de manera que se pueda analizar en qué situación se encuentra el robot en el entorno para así poder definir un punto objetivo adecuado. Una vez seleccionado el punto objetivo, el nodo Move Basese encarga de mover el robot hasta dicho punto.
Figura 3.2: Comunicaciones de ROS con el algoritmo de exploración MCDM
3.3 Funcionamiento del algoritmo
En este apartado se explica el funcionamiento del algoritmo de exploración MCDM que se ha implementado. En este caso, el algoritmo es más complejo y tiene una gran carga computacional debido a la gran cantidad de información que maneja.
La estructura de la función principal es bastante parecida a la del algoritmo Ran- dom. La única diferencia es que ahora mientras el robot está en movimiento se debe actualizar un mapa topológico. En primer lugar, como se puede observar en el pseu- docódigo del algoritmo 3, se realiza la lectura de los pesos de cada uno de los criterios establecidos para poder calcular los puntos objetivo. Además, es necesario hacer una lectura inicial de lostopicsde la odometría del robot y del primer mapa del entorno.
Una vez recogidos estos primeros datos, se procede a calcular el porcentaje de entorno que se ha explorado. En el caso de que se haya explorado más del 90 % del mapa se da por concluida la exploración. En caso contrario, se procede a calcular el próximo punto objetivo a alcanzar por el robot.
Algoritmo 3Main()
1: finMision=falso;
2: lecturaPesos();
3: mientrasmapa y odometría no leídoshacer
4: Leer mapa y odometría;
5: fin mientras
6: mientrasfinMision=falso hacer
7: si(calcularPorcentajeExplorado() <porcentajeAExplorar)entonces
8: MCDMPoint();
9: Enviar al robot el punto objetivo elegido;
10: mientrasel robot no ha llegado al punto objetivoyno se produce abortohacer
11: Leer mapa y odometría;
12: actualizarMapaTopologico();
13: fin mientras
14: sino
15: finMision=verdadero;
16: fin si
17: si(se produce abortoyno hay obstáculo cercano)entonces
18: inflarMapa();
19: expandirPosicionesInalcanzables();
20: fin si
21: fin mientras
Una vez elegida la posición más adecuada para explorar, se le transmite al robot dicho punto objetivo. Mientras el robot se desplaza, hasta que no llegue a su destino o hasta que no se produzca un aborto por no poder alcanzarlo, se irá actualizando un mapa topológico que registra el camino recorrido por el robot. Este mapa se utiliza para poder saber el coste del viaje/camino hasta un determinado punto objetivo candidato.
Esta información se necesita a la hora de calcular la utilidad global de cada candidato.
Si el robot llega de manera satisfactoria hasta su destino, se vuelve a calcular un nuevo punto objetivo. Si durante dicho cálculo, no se encuentra a ningún candidato a ser punto objetivo, entonces se da por finalizada la exploración.
Si, por el contrario, se produce un aborto mientras el robot navega hasta su destino,
3.3.1 El mapa topológico
El mapa topológico es donde se registra el recorrido que ha hecho el robot alrededor del entorno. Hacer este seguimiento nos ofrece una información muy importante a la hora de estimar cuál es la distancia que separa el robot de un determinado punto objetivo candidato. Conocer esa distancia es imprescindible para poder aplicar el criterio del coste del viaje.
Un mapa topológico esta formado pornnodos que representan lugares del entorno en los que el robot ha estado. Por otro lado, las aristas que unen dichos nodos (ni,nj) indican caminos libres de obstáculos por los que el robot puede circular sin peligro de colisión.
Para contruir el mapa topológico se tienen, esencialmente, en cuenta los cuatro casos que se ilustran de manera gráfica en la figura 3.3. En dicha figura se utiliza la siguiente anotación:n0representa el nodo más cercano visible desde la posición actual del robot;des la distancia que les separa;dt hes una distancia límite; la posición en la que se encuentra el robot se denominaxt;xt−1se refiere a la anterior posición del robot registrada;nt−1es el último nodo que ha sido creado.
El algoritmo aplicado es el siguiente:
• Cuando el nodo más cercano y visiblen0es el último nodo creado anteriormente nt−1y se encuentra a una distancia mayor adt h, se crea un nodonten la misma posición del robot en la que está (ver figura 3.3a). Es el caso más típico. Se produce, por ejemplo, cuando el robot se mueve en línea recta.
• El segundo caso es el que se puede ver en la figura 3.3b, el cual ocurre cuando desde la posición actual del robot no hay ningún nodo visiblen0. En esta ocasión, se crea un nuevo nodont enxt, pero la arista es de tipo poligonal (nt,nt−1) con un punto intermedio en la anterior posición del robot registradaxt−1.
Figura 3.3: Casos considerados en la creación del mapa topológico (figura extraída del artículo [1])
En el algoritmo 4 se muestra en forma de pseudocódigo la implementación de estos cuatro casos. En este algoritmo se emplean dos listas: la primera llamadaNodos en la cual se guarda en qué posición se encuentra cada uno de los nodos creados;
y la segunda llamadaadjacency_list. Esta última lista contiene las aristas del mapa topológico. De cada arista se guarda qué nodos están involucrados en la unión y el coste que hay entre ellos, que no es más que la distancia que los separa.
Algoritmo 4Función actualizarMapaTopologico()
1: /* Todos los nodos que son creados se introducen en la lista Nodos */
2: /* Al añadir una arista se esta introduciendo la información en la lista adjacency_list
*/
3: nodoMasCercano(xt)→(n0,d);
4: sin0no existeod>dt hentonces
5: Añadir nuevo nodonnenxt;
6: Hacernn→nt;
7: sin0existeyn06=nt−1yarista entre (n0,nt−1) no existeentonces
8: Añadir arista recta (n0,nt−1);
9: fin si
10: si[(n0existeyn0=nt−1)oesVisible(nt−1,xt)]entonces
11: Añadir arista recta (nt−1,nt);
12: sino
13: Añadir arista poligonal (nt−1,nt) con punto medio enxt−1;
14: fin si
15: sino
16: Hacern0→nt;
17: sin06=nt−1yarista entre (n0,nt−1) no existeentonces
18: siesVisible(nt−1,xt)entonces
19: Añadir arista recta (n0,nt−1);
20: sino
21: Añadir arista poligonal (n0,nt−1) con punto medio enxt;
22: fin si
23: fin si
24: fin si
Como se puede comprobar, se utilizan dos funciones muy importantes, como son las denodoMasCercano(), en el que dado la posición en la que se encuentra el robot,
nos indica, de toda la lista de nodos que han sido creados, cuál es el más cercano sin que haya ningún obstáculo en el camino. Se utiliza una función,esVisible(), de tipo booleano, que indica, dado dos nodos, si son visibles entre si, es decir, no se encuentra ningún obstáculo entre ellos. Para esta función se utiliza el algoritmo de Bresenham.
El algoritmo de Bresenham [2] es utilizado para trazar líneas rectas píxel a píxel en un entorno discreto. En nuestro caso es muy útil para realizar un recorrido por todas las celdas que conectan dos nodos del mapa topológico o un nodo con la posición del robot, de forma que se puede verificar celda a celda si se encuentra un obstáculo en medio y así confirmar si hay visibilidad entre las dos posiciones.
En la figura 3.4 se muestra un ejemplo del mapa topológico que se ha creado du- rante la exploración de un entorno. Como se puede observar, el mapa topológico se aplica en el mapa del entorno con los obstáculos inflados. Esto es así porque este mapa nos indica de mejor forma los lugares por dónde el robot puede circular, de manera que las distancias entre nodos cercanos a obstáculos son más fieles a la realidad.
Figura 3.4: Ejemplo de mapa topológico
3.3.2 Procedimiento para el cálculo de los puntos objetivo
Para realizar el cálculo de los puntos objetivo mediante el algoritmo MCDM es necesa- rio tener un registro de todos los puntos de frontera entre la parte explorada y la parte desconocida del mapa. Las posiciones almacenadas son aquellas que son celdas explo- radas y libres de obstáculos, y que tienen al menos una celda vecina inexplorada. Esta lista de posiciones, llamadalistaSegFronteras, está agrupada en diferentes segmentos de frontera, donde cada segmento se refiere a todas aquellas celdas consideradas de
(a) Mapa (b) Celdas de frontera (c) Candidatos seleccionados
Figura 3.5: Selección de celdas candidatas
El siguiente paso sería hacer el estudio de qué punto de la listalistaSegFronterases el más adecuado para realizar una exploración eficiente. Dentro delistaSegFronteras hay una gran cantidad de celdas almacenadas, y a medida que el robot se desplace un poco más por el entorno el número puede ser mucho mayor. Esto significa que habría que analizar miles de celdas calculando sus criterios y utilidades, aumentando muchísi- mo la carga computacional. Además, no tiene mucho sentido analizar celdas que están una al lado de la otra, ya que el resultado de la utilidad calculada sería prácticamente el mismo.
Para evitar dicha redundancia, se realiza una selección de algunas celdas que se encuentran enlistaSegFronteras, las cuales deben tener una cierta separación entre ellas. Para ello, a la hora de escoger qué celda es adecuada para ser candidata a punto objetivo, se comprueba que dicha celda no esté a una distancia menor a un radioRc and de una celda ya seleccionada como candidata.
Además de dicha condición, el candidato debe cumplir las siguientes condiciones:
C1 No debe haber ningún obstáculo a menos de 0,334 metros de la celda candidata, que es el radio de la base del robot. De esta forma, se evita un futuro aborto por parte del robot por no poder acceder al punto objetivo.
C2 La celda candidata debe estar a una distancia mayor a 0,334 metros con respecto de la posición actual del robot.
C3 No debe encontrarse en la lista de puntos inalcanzables. Esta lista contiene aquellos lugares del entorno a los que el robot no puede acceder.
C4 En el mapa topológico debe existir al menos un nodo que sea visible desde la celda candidata. Aquí es importante puntualizar que la comprobación de que haya o no visibilidad entre un nodo y una celda se hace desde el mapa sin inflar (es decir, desde el mapa donde los obstáculos no han sido inflados de acuerdo con el tamaño de la base del robot). Más adelante, se justifica porqué no se utiliza el mapa inflado.
En el algoritmo 5 se resume en forma de pseudocódigo el procedimiento que se aplica.
Algoritmo 5Función seleccionCandidatos()
1: Candidatos=;;
2: candidatoDescartado=falso;
3: paratoda posiciónf perteneciente alistaSegFronterashacer
4: paratoda posiciónaperteneciente aCandidatoshacer
5: sidistancia entref ya<Rc andentonces
6: candidatoDescartado=verdadero;
7: fin si
8: fin para
9: sicandidatoDescartado=falso entonces
10: sicumple condiciones C1, C2, C3, C4entonces
11: insertarf enCandidatos;
12: fin si
13: fin si
14: candidatoDescartado=falso;
15: fin para
Una vez escogidos todos los puntos candidatos, se procede a puntuarlos a través de los criteriosiArea,iSeg,overlapyc. Para cada candidato se obtiene una puntuación para cada criterio siguiendo el algoritmo 6.
Por cada celda escogida como candidata, en un área de radio igual al rango de alcance del láser,Rl aser, se comprueba el contenido de cada celda, de manera que se realiza un recuento de los tres tipos de celda que definen tres de los cuatro criterios esta- blecidos: celdas inexploradas para el criterioiArea; celdas que equivalen a un obstáculo para el criteriooverlap; y, por último, celdas de frontera para el criterioiSeg(siendo una celda de frontera una celda libre de obstáculos con, al menos, una vecina inexplorada).
Para el cuarto criterio que falta, el coste del viaje (c), se utiliza el mapa topológico creado. Con la información que se recopila en las listasNodosyadjacency_list, equiva- lentes a todos los nodos y aristas que configuran el mapa topológico, se calcula cuál es la distancia mínima que separa la posición actual del robot con la de cada uno de los candidatos. Para ello se utiliza el algoritmo de Dijkstra.
El algoritmo de Dijkstra [3], también llamado del camino mínimo, permite determi- nar cuál es el camino más corto entre un vértice inicial y uno final. Más formalmente, la idea de este algoritmo consiste en encontrar el camino más corto entre cada par de
Algoritmo 6Función calcularCriterios()
1: paratoda posiciónpperteneciente aCandidatoshacer
2: celdasInexploradas= 0;
3: celdasObstaculo= 0;
4: celdasFrontera= 0;
5: paratoda celdaadentro del rangoRl asercon centro enphacer
6: siaes celda inexploradaentonces
7: celdasInexploradas++;
8: fin si
9: siaes celda obstáculoentonces
10: celdasObstaculo++;
11: fin si
12: siaes celda libre de obstáculos con un vecino sin explorarentonces
13: celdasFrontera++;
14: fin si
15: fin para
16: InsertarceldasInexploradasenvalores_iArea;
17: InsertarceldasFronteraenvalores_iSeg;
18: InsertarceldasObstaculoenvalores_overlap;
19: nodoInicial= nodoMasCercano(posRobot);
20: nodoFinal= nodoMasCercano(p);
21: DijkstraComputePaths(nodoInicial,adjacency_list,min_distance);
22: distanciaTotal= distNodos(nodoInicial,posRobot) + distNodos(nodoFinal,p) + min_distance[nodoFinal];
23: InsertardistanciaTotalenvalores_c;
24: fin para
Antes de aplicar la funciónDijkstraComputePaths(), se identifica el nodo más cer- cano y visible a la posición actual del robot (posRobot) y al punto candidatop. Esos nodos representarán el nodo inicial y final, respectivamente. Después, se ejecuta la función y se procede a calcular la distancia total del recorrido. La distancia entre los nodos inicial y final se obtiene a partir de la listamin_distanceindicándole cuál es el nodo final. Pero a esta distancia hay que sumarle las que hay entre la posición actual del robot y el nodo inicial, y la posición del candidato y el nodo final, que son calculadas como la distancia euclídea entre dichos puntos.
