Algoritmo de Planificación de trayectorias

Algoritmo A Star

El algoritmo A* o A Star permite diseñar una solución a problemas de entorno y planificación de trayectorias para sistemas robóticos basados en vehículos o robots guiados de forma autónoma, obteniendo el camino más corto, rápido y óptimo, desde el punto de partida hasta el punto destino. Presenta una diferencia respecto a otros algoritmos, como el Algoritmo de Dijkstra, ya que permite definir la trayectoria óptima sin necesidad de recorrer y evaluar todos los nodos de la pista, basándose en cálculos de distancias euclideanas, desde el punto de origen al punto actual y desde el punto actual al destino, y costos de movimientos entre un nodo y otro.

El desarrollo del algoritmo se basa en la definición de dos conjuntos de nodos pertenecientes a las divisiones de la pista, los todos los nodos sin evaluar, que son buenos candidatos o nodos adyacentes a alguno ya evaluado, (lista abierta) y los nodos evaluados (lista cerrada), así como la definición del nodo padre, que es el nodo del cual se genera otro nodo como posible candidato de la trayectoria. Además, el recorrido de los nodos está basado en los siguientes parámetros que se definen para cada uno de los nodos: [1]

• Parámetro G: Se define como el costo de ir desde el nodo padre al nodo actual, se basa en el cálculo de la distancia euclideana desde cada nodo a los nodos adyacentes. En este caso, se tomó el costo para los nodos cuyo movimiento consistía en dar un paso horizontal o vertical, como uno "1"; y para aquellos nodos en los que el movimiento consistía en un paso en diagonal, el nodo está dado por un factor de la raíz cuadrática de dos, "sqrt(2)".
• Parámetro H: Se define como la distancia euclideana mínima y óptima desde el nodo evaluado hasta el nodo objetivo, sin tomar en cuenta la ubicación o presencia de obstáculos en la pista.
• Parámetro F: Se define como la suma algebraica de los parámetros G y H.

Figura 1: Algoritmo A Star

Se parte de los parámetros de las posiciones de inicio (horizontal y vertical), las posiciones del objetivo (horizontal y vertical), las posiciones (horizontales y verticales) de los obstáculos, la resolución de las cuadrículas de la pista, y el radio asignado al robot.

El procedimiento de la función, comienza inicializando el nodo de partida y el nodo de llegada con los parámetros recibidos, y ajustando las posiciones de los obstáculos a la resolución del mapa para poder definir la región de la pista donde se ubican los mismos.

Luego, se procede a definir la forma de recorrido de las cuadrículas, que en este caso, se manejan como nodos del grafo de recorrido. Para cada nodo evaluado, se van a realizar comparaciones con los ocho nodos adyacentes, que son definidos con las posiciones en horizontal y vertical, respecto al nodo evaluado, y el costo del mismo.

Se procede a ejecutar los pasos del algoritmo, comenzando por definir las listas de los nodos evaluados (lista cerrada) y los nodos sin evaluar (lista abierta). Además, se agrega como primer elemento de la lista abierta el nodo de partida, que debe ser el primer elemento a ser evaluado.

De forma correspondiente al primer paso, se debe extraer el primer nodo de la lista abierta y definirlo como el nodo actual de evaluación. Así, se emplea una función que permite calcular la distancia euclidieana más cercana (distancia óptima) desde la coordenada del nodo actual hasta el nodo objetivo, de manera que se pueda asegurar que se recorren sólo los nodos que permiten llegar de la forma más rápida al objetivo sin tener que realizar el recorrido y evaluación de cada uno de los nodos de la pista.

Posteriormente, se procede a extraer el primer elemento de la lista abierta y guardarlo en la lista cerrada, así como identificar los nodos adyacentes al nodo actual como modelo de movimiento o recorrido de trayectorias, lo que corresponde al segundo paso.

Para el tercer paso, es necesario evaluar en cuál de los tres casos posibles para los nodos adyacentes se encuentra el algoritmo:

• Caso A: Se verifica que las posiciones del nodo actual correspondan a las posiciones del nodo objetivo; de ser así, se detiene el recorrido de la pista así como la construcción de la trayectoria. En este caso, la trayectoria final se obtiene recorriendo de forma inversa la lista de nodos padres desde el punto objetivo hasta el origen.
• Caso B: Se verifica que el nodo adyacente que está siendo evaluado no sea un obstáculo, así como se verifica la condición de colición, que consiste en calcular la distancia entre el nodo evaluado y el nodo adyacente, y constatar que sea mayor la relación entre el radio del robot y el tamaño de la cuadrícula considerada como nodo.
• Caso C: Se verifica que el nodo adyacente no haya sido evaluado previamente, es decir, que no pertenezca a la lista cerrada. De ser así, no se toma en consideración dicho nodo.
• Caso D: Se verifica que el nodo pertenezca a la lista abierta y se vuelven a calcular los valores de los parámetros F, G y H. Luego, se compara el costo G original del nodo con el costo G actual, si éste último es mejor se coloca ese nodo como el nodo padre del nodo extraído, de lo contrario, no se toma en cuenta el nodo.
• Caso E: Para el resto de los nodos adyacetnes, se establece como padre el nodo extraído y se recalculan los parámetros F, G y H, para luego ser añadidos a la lista abierta.

Por otra parte, se debe reordenar la lista abierta de forma ascendente en función del parámetro F de los nodos.

Este procedimiento se realiza hasta que el nodo actual de evaluación corresponda con el nodo meta, en ese caso, se reconstruye la trayectoria final, con una función que permite determinar la trayectoria final para el recorrido del robot, partiendo de las posiciones del nodo de destino, el conjunto de todos los nodos ya evaluados, el tamaño de la pista y de las cuadrículas de la misma, y finalmente retorna las listas de las posiciones en horizontal y las posiciones en vertical del recorrido obtenido.

Esta función consiste en extraer la información correspondiente de la posición y el costo más óptimo de los nodos evaluados, y almacenar dichas posiciones en las listas del recorrido final, para ser retornadas y enviadas como referencia al control del robot.

Implementación del algoritmo y otras consideraciones

Se define una clase que especifica las características de cada uno de los nodos de la trayectoria: posición horizontal del nodo (x), posición vertical del nodo (y), costo del nodo y un identificador o bandera del nodo. Es a partir de esta clase sobre la que se desarrollan los pasos y evaluaciones de cada uno de los nodos, para el recorrido del grafo.

El algoritmo A Star [2] recibe como parámetros las posiciones horizontal y vertical de inicio, las posiciones horizontal y vertical del objetivo, el tamaño definido para los nodos y el radio del robot. Así como al inicio se debe definir las dimensiones de la pista.

Inicialmente, se hace un request al servidor proporcionado por el preparador [3], el Ing. Said Alvarado, para obtener todas las pelotas dispuestas en la pista. A partir de ello, se pasa dicha lista de pelotas obtenidas en la variable response a una función que permite clasificar las pelotas como objetivos y obstáculos. Por otra parte, partiendo de las identificaciones de las pelotas azul y amarillo, cuyas posiciones eran próximas entre sí, se clasificó dicha ubicación como la posición de robot o punto de inicio del algoritmo A Star. Estas funciones son:

class cordinates:

def colorClassification(response):

En el caso de que se presentaran diversas pelotas objetivos en la pista, se implementó una función que permitiera determinar por medio del cálculo de la distancia euclideana entre la posición del robot y de cada una de las pelotas objetivo, cuál de estas se encontraba ubicada más cerca, para ser enviada al algoritmo A Star como punto de inicio.

def distanceStart_Target ( balls, mediaX, mediaY ):

    dst, position, distance = 0, 0, 0
    distance = math.sqrt( math.pow((mediaX - balls.x[0]), 2) + math.pow((mediaY - balls.y[0]), 2) )

    for i in range(len(balls.x)):

        dst = math.sqrt( math.pow((mediaX - balls.x[i]), 2) + math.pow((mediaY - balls.y[i]), 2) )
        
        if dst <= distance:
            distance = dst
            position = i

    return position
...
        # Goal position
        position = distanceStart_Target ( balls, sx, sy )
        gx = balls.x[position]
        gy = balls.y[position]

Por otra parte, para evitar colisiones entre el robot y los obstáculos, se definió el tamaño del robot como siete veces el tamaño de la cuadrícula, y a su vez, se implementó una función que permitiera aumentar el tamaño de los obstáculos y generar cuatro paredes alrededor del mismo, cuyas dimensiones serían de dos (2) veces el radio del obstáculo.

def squares(obstacles):

    ox, oy = [], []

    for k in range(len(obstacles.x)):
        
        dist = int(100*obstacles.cordinates[k])

        x1 = obstacles.x[k] - 2*dist
        x2 = obstacles.x[k] + 2*dist
        y1 = obstacles.y[k] - 2*dist
        y2 = obstacles.y[k] + 2*dist
        x = x2 - x1
        y = y2 - y1
        
        for i in range((int)(y)):
            ox.append(x1)
            oy.append(y1 + i)
        for i in range((int)(y) + 1):
            ox.append(x2)
            oy.append(y1 + i)
        for i in range((int)(x)):
            ox.append(x1 + i)
            oy.append(y1)
        for i in range((int)(x)):
            ox.append(x1 + i)
            oy.append(y2)

    return ox, oy
...
        # Tamaño del carrito
        robot_size = 7.0

Además, se diseñaron tres funciones en el caso de que se realizara la competencia; sin embargo, no fueron implementadas en las pruebas finales.

Una de ellas, semiSquares, estaba destinada a generar semicuadrados sobre cada objetivo, del mismo modo que se realizaron los cuadrados de los obstáculos, pero en este caso, se tenía la finalidad de implementar sólo tres paredes, dejando una disponible para que el robot llegara al objetivo con la orientación respectiva, para facilitar el recorrido de retorno a la meta. Es decir, dicha pared que no iba a ser implementada estaba planificada para permitir una trayectoria más directa hacia la meta evitando la posibilidad de perder a la pelota modificando la orientación del robot o realizando giros sobre su mismo eje.

def semiSquares(obstacles , balls):

Otra de las funciones, comparationColors, tenía como objetivo verificar la presencia de pelotas del mismo color de los identificadores del robot en alguna otra parte de la pista. En caso de que así fuese, se tomaba como el identificador del robot aquel par de pelotas azul-amarillo de distancia más cercana.

def comparationColors(balls , obstacles , car):

La tercera función, comparationLists, tenía como objetivo detectar cambios en las posiciones de las pelotas objetivo y de los obstáculos, para sólo en ese caso, detener el seguimiento de la trayectoria del robot y realizar un nuevo cálculo de la misma.

def comparationLists(balls, ballsAnt, obstacles, obstaclesAnt):

Referencias

[1] "Pathfinding: A* - lambda lab 85", lambda lab 85, 2018. [Online]. Available: https://www.lanshor.com/pathfinding-a-estrella/.

[2] A. Sakai, "PythonRobotics - PathPlanning", GitHub, 2018. [Online]. Available: https://github.com/AtsushiSakai/PythonRobotics/tree/master/PathPlanning/AStar.

[3] S. Alvarado, "EC3883-CollorBallTracker", GitHub, 2018. [Online]. Available: https://github.com/SaidAlvarado/EC3883-ColorBallTracker

Regresar a la página anterior