Práctica 4 - Global Navigation

 

Introducción

En esta práctica programaremos un algoritmo de GPP (Gradient Path Planning) para hacer que el coche siga una ruta global de manera automática mediante el uso del mapa como único input y sin utilizar ningún tipo de sensor general.

Nota: esta página del blog ha sido actualizada para acoger los nuevos cambios realizados para la reentrega.

Implementación

Primera aproximación: creación del campo del gradiente.

Primero realizaremos un algoritmo a expansión del campo basado en BFS (Breadth Fisrt Search), al que luego añadiremos optimizaciones y detalles.

Para optimizarlo respecto a la anterior versión, que tardaba en ejecutarse mucho más, lo que haremos será expandir en un orden, que vendrá dado por la frontera, en la que se insertarán los valores de forma que queden ordenados de menor a mayor por su valor de campo, ya que antes de expandir cada nodo se le deberá asignar dicho valor.

Realizaremos esta parte al margen de Unibotics para ahorrar tiempo a la hora de ejecutar y hacer así mediciones de este tiempo de ejecución.

El primer resultado que obtenemos es el siguiente, donde podemos ver el campo expandido, que tarda 1.8s en el peor de los casos, ya que depende de la cantidad de puntos a expandir, que aumenta con la lejanía entre los puntos:

En la imagen podemos observar también, el punto inicial del coche, que será el punto final del campo, indicado con un círculo de color verde, y el punto de destino del coche, a donde se quiere llegar, que será el punto inicial del campo y está indicado con un círculo azul.

Ahora podremos añadir la repulsión, que se aplicará a todo el mapa, ya que no es tan costoso computacionalmente, ya que tardará solo 0.9s constantes independientemente de los puntos elegidos:

Este algoritmo detecta los bordes de las paredes y aplica un campo de la siguiente forma en todas las direcciones donde hay más pared (color negro):

Ahora, si nos fijamos en el punto inicial del coche (círculo verde), podemos ver que el campo se frena en seco, y eso nos puede perjudicar a la hora de maniobrar si lo necesitamos, ya que podríamos acabar fuera del campo y no poder planificar la ruta.

Es por eso que necesitamos añadir un radio de seguridad alrededor del coche donde el campo siga expandiéndose, lo que nos dará la seguridad necesaria para poder cumplir con el objetivo, pero aumentará el coste computacional y por tanto el tiempo.

Una primera aproximación para cumplir este objetivo es usar la fuerza bruta: una vez acabado el campo, expande X nodos adicionales. En nuestro caso con 3000 nodos más será suficiente, y obtendremos el siguiente resultado en 2.3s (0.5 segundos más, esto sin tener en cuenta que en Unibotics tardará bastante más):

Podemos comparar esta mejora con la imagen anterior (sin mejora) para ver como el campo se ha expandido por todos sus frentes de manera uniforme.

Esta mejora del algoritmo es suficiente, pero imprecisa y poco escalable, ya que cuanto más grande fuese el mapa más nodos adicionales se deberían expandir, por lo que podemos aproximarnos al objetivo con otra solución: una vez expandido el campo, creamos una nueva frontera en la que meteremos solo los nodos que hayan quedado en la frontera anterior cercanos al punto inicial del coche o final del campo, en un área de, por ejemplo, 21x21, y continuaremos expandiendo con esta nueva frontera, unos 300 nodos más (10 veces menos que en la anterior mejora). Esto tendrá el mismo efecto alrededor del punto inicial del coche, pero no hará falta expandir por otras zonas como en la anterior mejora:

Con esta última mejora obtenemos tan solo 0.1s más, con un total de 1.9s de expansión del campo y sumando la repulsión obtenemos un tiempo final de 2.8s en el peor de los casos, que en Unibotics se convertirán en 30s de máximo, lo cual no está nada mal.

Una vez hecho esto podemos también mostrar la ruta ideal (aunque el coche no deberá guiarse por eso, sino por los valores directos del campo, para poder improvisar cambios de ruta):

 

Segunda aproximación: navegación usando el campo generado.

Con el campo generado, podemos seguir los valores del campo con menor valor (color más rojizo) hasta el objetivo final.

Aquí podemos ver un ejemplo de que direcciones iría tomando el coche al principio con el punto de destino de las anteriores imágenes (arriba a la derecha), pintadas de color azul oscuro sobre los valores del campo:

 

También podemos ver representados el punto inicial del coche pintado en verde, los ejes X e Y del sistema de referencias de la imagen, que es el mismo que el del campo, pintados en rojo y cian respectivamente. Los valores 0.0 subrayados representan la presencia de un obstáculo o pared.

Antes de comenzar a navegar nos hará falta tener claros los siguientes sistemas de referencia:

· Sistema de referencia de la imagen o el campo, con el origen en el pixel superior izquierdo de la imagen y sus ejes X e Y positivos apuntando hacia el este y hacia el sur respectivamente.

· Sistema de referencia del mundo, con el origen en el (200, 200) con respecto al sistema de referencia de la imagen y con sus ejes X e Y positivos apuntando hacia el sur y hacia el este respectivamente (al revés que en el sistema de referencia de la imagen).

· Sistema de referencia del coche o sistema de referencia relativo. Este sistema de referencia es móvil ya que su origen es la posición del coche en cada momento y sus ejes X e Y positivos apuntando hacia el frente del coche y hacia su izquierda u oeste respectivamente, e independientemente del yaw del coche, ya que giran solidariamente con este para mantener siempre esta orientación relativa.

Esto se puede resumir en la siguiente imagen:

Sabiendo esto lo único que queda por hacer es pasar las coordenadas de las direcciones que se quieren tomar al sistema de referencias del robot o relativo, en coordenadas polares, y con esto, realizar un control de velocidades que permita conducir el coche hasta su destino.

Tras realizar esto y hacer algunas pruebas, ajustamos el modelo de repulsión para evitar que el coche se acercase tanto a las esquinas:

Aquí muestro algunos de los recorridos que se realizaron para comprobar la robustez del algoritmo tras este cambio, dándolo así por finalizado:

 

Nota: en el segundo vídeo se ve cómo existe un bug en las texturas del coche, que aparece verticalmente, pero es puramente visual, no afecta a la ejecución.