Objetivo

El objetivo de esta práctica es implementar la lógica de un algoritmo de navegación para una aspiradora autónoma haciendo uso de la ubicación del robot. El robot está equipado con un mapa y conoce su ubicación actual en él. El objetivo principal será cubrir la mayor superficie de una casa utilizando el algoritmo programado.

Representación del mapa Grid

Se nos proporciona una imagen que contiene el mapa del mundo, que erosionándola haremos que los obstáculos y las paredes se ensanchen, así evitando que el robot llegue a pegarse demasiado a estos.

Con nuestro mapa ya erosionado, lo que haremos sera celdillas de 28x28, ni tan grande para que falten zonas por recorrer, ni tan pequeño para que se recorra varias veces la misma zona. Una vez hechas las líneas horizontales y verticales, he decidido que si en la celda hay un 0,001% de negro, ya toda la celda es negra.

Ahora para definir que celdas ya fueron recorridas y cuales son puntos de retorno he usado los siguientes colores:

Los puntos de retorno son todos los puntos alrededor de la ubicación del robot que estén disponibles, es decir, que se puedan visitar y no sean obstáculos, sin contar las diagonales. Estos puntos servirán para que una vez que el robot no tenga ninguna dirección disponible alrededor suya, vaya al punto de retorno mas cercano donde volver a empezar con el algoritmo BSA.

Transformada de gazebo al grid

El punto que se obtiene de gazebo no concuerda con la imagen del mapa, para ello tenemos que hacer una transformación de los puntos mediante una ecuación de ajuste lineal. Para ello me ayude de Geogebra, donde usando varios puntos del mapa, como esquinas y zonas que puedas ser problemáticas, obtuve las ecuaciones para encontrar la x y la y correspondiente al mapa Grid.

X_grid = -3.55*x_gazebo + 20.05
Y_grid = 3.48*y_gazebo + 14.72

Algortimo de cobertura

El algoritmo usado para recorrer el mapa es el algoritmo de BSA, que está basado en prioridades haciendo espirales. En este caso la prioridad está definida principalmente hacia el norte, sino está disponible será hacia el este, sino hacia el sur y por último al oeste. Al final no termina haciendo espirales como tal, sino por cómo está definido las prioridades termina yendo de arriba a abajo como se muestra en la siguiente imagen, donde lo verde es la ruta para ir al punto de retorno y lo rosado la ruta que sigue el algoritmo:

De esta manera me he asegurado de limpiar zonas completas antes de abandonarlas. Mi forma de ejecutar el algoritmo es ir calculando casilla por casilla viendo hacia que dirección ir, ya que si el mapa fuera dinámico, que los obstáculos cambien por ejemplo, podría evitarlos, para ello también tendría que usar el láser para modificar el mapa y no pasar por esa zona. Si tuviera una ruta ya definida y se me genera un obstáculo al frente tendría que recalcular la ruta para evitarlo.

Una vez llegado a una casilla donde ya no puedo avanzar tendría que ir al punto más cercano que he guardado de los puntos de backtracking (puntos de retorno). Todos estos puntos los voy almacenando en una priority queue, donde la prioridad es la distancia al robot. Al momento de necesitar un punto, actualizo la queue para recalcular las distancias a esos puntos y así obtener el punto más cercano y voy hacia ese punto.

Para llegar al punto de retorno más cercano he utilizado el algoritmo de descenso por gradiente, donde me cree mi mapa de 36x36 donde iré asignando un coste ascendente desde el goal (punto de retorno) hasta que llegue a la posición del robot de esta forma:

Como se observa, la ola se va expandiendo sumando de uno en uno hasta que se llega la posición del robot. El robot para ir por esa ruta irá buscando la posición con el menor coste sin incluir el cero, entonces irá hacia el norte y luego hacia el oeste hasta tener que ir al sur, llegando al punto de retorno y volviendo al algoritmo de BSA.

Movimiento del robot

Para ir de una casilla a otra yo me base en la orientación del robot, siempre tengo en cuenta si apunta hacia el norte, sur, este o el oeste.

directions_angles = {
    'north': -math.pi / 2,  # Norte
    'east': math.pi ,             # Este
    'south': math.pi / 2, # Sur
    'west': 0       # Oeste
}

Cuando me tengo que mover, por ejemplo al norte, primero compruebo que el robot mire hacia el norte, teniendo un margen de error de un grado (la orientación que obtengo de gazebo lo cambio a grados para comprenderlo mejor). Una vez ya este orientado hacia donde tengo que ir, avanzo hasta que llegue a la siguiente coordenada, que sería la siguiente casilla. El robot siempre intentará seguir una línea recta para no perderse.

Para que el mundo de gazebo y mi mapa concordaran en el sistema de coordenadas tuve que hacer la siguiente transformación, donde angle serían las direcciones dadas anteriormente:

angle = -1*(math.degrees(directions_angles[direction])-90)
current_yaw = -1*(math.degrees(HAL.getPose3d().yaw)-90)

Los ángulos vienen dados desde el 0 al 180 y desde -180 al 0 y para que vaya desde 0 al 360 cuando se da un ángulo negativo se resta a 360 para completar la circunferencia para una mejor compresión sobre las orientaciones.

if angle < 0:
        angle = 360 - abs(angle)
    if current_yaw < 0:
        current_yaw = 360 - abs(current_yaw)

Para el giro se uso un controlador KP sencillo que se multiplica por la diferencia de los ángulos. Esta diferencia llega a funcionar por si sola si la diferencia no es mayor a 180 o el camino mas corto de giro no pasa de 0 a 360 ya que siempre girará desde 0 al 360 o al revés pero nunca ira entre ellos, por ejemplo, el robot tiene orientación 10º y la baliza 315º, lo mas lógico es girar hacia la derecha 55º pero si calculamos con la fórmula anterior daría 305º girando hacia la izquierda. Para solucionar esto y que pueda pasar desde 0 a 360 se vió si esta diferencia era mayor a 180º, colocando el signo contrario, quedando el calculo de la velocidad de giro de la siguiente manera:

dif = current_yaw-angle
        if dif > 180 or dif < -180:
            turn = KP_*(360 - abs(dif))
            if dif > 0:
                turn = -turn
        else:
            turn = KP_*(dif)

Teniendo controlado el giro solo me queda comprobar mi ubicación mientras va avanzando. Una vez llega el robot a la nueva casilla, se para, mira hacia que casilla va a ir a continuacion y la colorea de verde, mira el resto de casillas y si estan disponibles la pinta de morado claro y avanza otra vez a la nueva casilla.

Para finalizar, dejo un vídeo con todo el recorrido hecho. Probando diferentes parámetros de PID y de velocidades, como máximo obtuve 28 minutos y como mínimo obtuve 17 minutos, que es el que se muestra en el siguiente vídeo:

prueba_completa_17.webm