Práctica 1 - Localized Vacuum Cleaner

Introducción

En esta práctica se quiere recorrer la casa entera con la aspiradora localizada de forma óptima, y por tanto inteligente, por este motivo programaremos un algoritmo de BSA (Backtracking Search Optimization Algorithm) para recorrer todo el área, pero primero debemos separar el problema en 3 grandes pasos:

1. Buscar una relación ente ambos sistemas de referencia (mundo y mapa).
2. Planificación de rutas óptimas, en nuestro caso mediante el algoritmo de BSA.
3. Navegación por las rutas anteriormente creadas.

Estos dos últimos pasos se pueden ir realizando de manera paralela, aunque habrá que determinar si se consume demasiado tiempo para comprobar la siguiente celda cada vez que se llegue a la celda destino como para no poder realizarse dentro del bucle principal para no ralentizar las iteraciones y asegurar así una buena frecuencia de ejecución. En este caso habrá que realizar todo el cómputo previamente, lo que consumirá un tiempo inicial relativamente cuantioso, pero las iteraciones del bucle principal serán fluidas.

También deberemos explorar nuestras posibilidades antes de empezar, viendo qué nos ofrece la interfaz del HAL y qué no:

Información de los sensores:

• Posición respecto al mundo.

• Información del bumper (True o False, en caso de True podemos ver de que lado se ha chocado: 0, 1 o 2).

• Mapa en formato imagen (2 colores: espacio libre o espacio ocupado) .
  

• Láser de 180º.
  

Posibles acciones:

• Control de velocidades lineal y angular (V y W).

• Mostrar imagen multicolor (4 colores: 0 - gris, 1 - verde, 2 - amarillo, 3 - rojo).

Implementación

Relación ente los sistemas de referencia del mundo y del mapa.

Primero de todo vemos que podemos usar una función que nos devuelve la posición del robot en el sistema de referencia del mundo (absoluto), pero no sabemos ninguna otra información que nos pueda relacionar el sistema de referencia del mapa (relativo) con el del mundo, por lo que tendremos que idear alguna estrategia para obtener una relación entre varios puntos.

Cogeremos varios puntos del mundo que sean esquinas de la casa y situaremos al robot en ellas, pudiendo reconocerlas fácilmente después en el mapa para establecer una relación. Se deben coger varias esquinas para disminuir el error.

Una vez tenemos las posiciones de las esquinas del mundo, haremos un detector de esquinas, en mi caso en local, habiendo descargado la imagen del mapa previamente. En este detector seleccionaremos las esquinas que coincidan con las elegidas en el mundo e imprimiremos sus coordenadas en pixeles en el sistema de referencia relativo.

 

Esquinas detectadas (círculos grises) y esquinas elegidas (círculos negros)

Hecho esto podemos establecer una relación entre las coordenadas en píxeles (sistema de referencia del mapa) y las coordenadas correspondientes en metros (sistema de referencia del mundo), por lo que podemos aplicar un modelo de regresión lineal para cada par de coordenadas de los ejes de abscisas de ambos sistemas de referencia y lo mismo para los ejes de ordenadas, obteniendo de esta manera las ecuaciones que relacionan ambos sistemas:

Regresión lineal de los ejes de abscisas y ecuación de relación entre sistemas.

 

Donde la "x" representa la coordenada x en el sistema de referencia del mundo y la "y" representa la coordenada x en el sistema de referencia del mapa.

 

Regresión lineal de los ejes de ordenadas y ecuación de relación entre sistemas.

 

Donde la "x" representa la coordenada y en el sistema de referencia del mundo y la "y" representa la coordenada y en el sistema de referencia del mapa.

Como podemos observar, la pendiente de la primera gráfica, correspondiente al eje de abscisas, es negativa. Esto nos indica que los ejes X de los sistemas de referencia apuntan en direcciones opuestas, como es el caso, ya que el de gazebo apunta hacia el oeste y el de la imagen hacia el este.

Con el eje Y solo podemos ver una relación de proporción, y no vemos este fenómeno debido a que en este caso ambos ejes apuntan hacia el sur, por eso la pendiente es positiva.

Hecho esto ahora probaremos la función world2map() creada a partir de estas ecuaciones para comprobar su funcionamiento, moveremos al robot a ciertas posiciones de la casa en el mundo y miraremos su posición, que transformaremos con esta función en la posición del mapa:

Posiciones de spawn, origen de coordenadas del mundo y otras 3 (Círculos negros).

Despejando de las anteriores ecuaciones podemos realizar la transformación en el sentido inverso, y obtener así la posición en el mundo, desde un pixel en el mapa, función a la que llamamos map2world():

Posiciones de la esquina superior izquierda del mapa transformada.

Planificación de rutas óptimas mediante el algoritmo de BSA.

Antes de comenzar con el algoritmo debemos preparar el terreno: ensancharemos los obstáculos del mapa para poder considerar al robot un punto, dividiremos el mapa en celdillas y discretizaremos las mismas para definir que casillas podrán ser practicables y cuales no.

Para el ensanchamiento de paredes haremos algo parecido a lo que hicimos con la práctica del curso pasado de GPP, pero ya que tenemos un detector de esquinas lo modificaremos también para detectar paredes. Para las esquinas no necesitaremos complicarnos, ya que con rellenar la diagonal será suficiente para detectar si la casilla esta ocupada o no, por lo que solo nos preocuparemos de las direcciones de expansión desde cada píxel.

El resultado quedará de la siguiente manera:

Ensanchamiento de obstáculos (bordes grises).

 

Ahora realizaremos el grid para poder observar con que color se discretizará cada celda de manera general, para poder escoger una anchura de expansión de obstáculos y un tamaño de celda óptimo para cubrir el mayor espacio posible sin acercarse demasiado a las paredes. El resultado será el siguiente:

 

Grid sobre la imagen de ensanchamiento de obstáculos.

Hecho esto podemos discretizar a gusto, obteniendo el siguiente resultado:

Celdas discretizadas.

 

A la vez que se discretizan las celdas, se crea una matriz del tamaño de las celdas con los valores de estos colores, pudiendo guardarse o copiarse en Unibotics posteriormente de manera cómoda:

Matriz de colores de las celdas discretizadas.

Ahora comenzaremos con el algoritmo de BSA, para lo que primero realizaremos las espirales sin preocuparnos de las rutas entre celdas lejanas: de momento haremos que se teletransporte, para mayor comodidad mientras hacemos el algoritmo.

 

Hemos implementado esto con una máquina de estados finita de la siguiente forma:

 

• El estado "Search Wall" lo hemos implementado simplemente moviendose hacia adelante hasta toparse con la pared, como podemos ve en el vídeo anterior.

• El estado "Follow Spiral" va siguiendo la pared siempre por la derecha, es decir en sentido horario y teniendo en cuenta las casillas por las que pasa como si fuesen pared, lo que le da el efecto de rellenado al algoritmo.

• El estado de "Search Dirty" se compone de dos algortimos, el de la búsqueda de la casilla sucia mas cercana, y el de la creación de una ruta óptima para llegar a ella.  La forma de buscar las casillas aún no limpiadas es a siguiente: expansión de celdillas en forma de rombo o flor, es decir, 4 diagonales desde los distintos puntos Norte, Este, Sur y Oeste a una distancia D (radio) que vamos incrementando. Llamamos frontera a cada cuarteto de diagonales, y en cada expansión buscamos en la misma la casilla limpia más cercana, usando como referencia la distancia euclídea.

          Cuando ya tenemos la casilla a la que queremos llegar obtenemos la ruta óptima gracias al

          algoritmo de BFS (Breadth-First Search), que es óptimo cuando todos los costes son 1, como es

          el caso.

 Obtenemos el siguiente resultado:

 Implementado en Unibotics queda de la siguiente manera, y tarda menos de un segundo en ejecutar, aunque en este video hemos dejado 0.1s de pausa entre las actualizaciones de cada celda para visualizarlo mejor:

 

Como se puede ver en la siguiente leyenda, las celdas grises indican un hueco que no ha sido limpiado (o sucio), las amarillas indican el lugar de los obstáculos como las sillas, mesas o las propias paredes, las celdas verdes indican la trayectoria que se deberá seguir, y las casillas rojas, como podemos ver en la ejecución en el siguiente vídeo, indican la siguiente casilla a la que se debe llegar en el momento actual:

Una vez hechos los algoritmos y obtenidas las relaciones entre sistemas de coordenadas solo queda aplicar todo esto junto a un sistema de movimiento que se oriente en la dirección que queremos y avance a la siguiente casilla, para conseguir recorrer la casa entera de manera localizada, como podemos ver en el siguiente video, en el que se muestra el resultado final tras algunas correcciones en el mapa para una mejor precisión:

 

Nota: las dos primeras celdas no se han coloreado de rojo ya que cuando el algoritmo comienza, estas ya no forman parte de las casillas futuras debido a que el robot ya ha avanzado.