Planificación de rutas A* sobre una rejilla de ocupación

Un planificador de robot responde una pregunta distinta que un controlador de evitación de obstáculos. El planificador pregunta: «¿qué celdas libres conectan el inicio con la meta al menor coste?». El controlador pregunta: «¿qué movimiento es seguro ahora mismo?». A* es un algoritmo de búsqueda en grafos que combina el coste ya recorrido con una estimación del coste restante, lo que lo hace un primer planificador práctico para una rejilla de ocupación bidimensional.

La ruta solo es tan segura como la rejilla y la huella del robot que se le dan. Una línea matemáticamente más corta por centros de celdas libres puede rozar una pared si los obstáculos no están inflados.

Convierte el mapa en un grafo

Una rejilla de ocupación divide el espacio en celdas iguales. Cada celda puede estar libre, ocupada o desconocida. Para un planificador básico, cada celda transitable es un nodo del grafo conectado a sus vecinos.

Con movimiento en cuatro direcciones, los vecinos son norte, este, sur y oeste, cada uno con coste 1. Con movimiento en ocho direcciones, añade diagonales con coste √2. Multiplicar después por la resolución del mapa convierte la distancia en rejilla a metros.

Decide cómo tratar las celdas desconocidas. Un robot precavido de interior puede considerarlas bloqueadas; un robot de exploración puede permitirlas con coste extra. Haz que eso sea una entrada de política, no una comparación accidental con un valor numérico equivocado.

Entiende las tres puntuaciones de A*

Para cada nodo n, A* lleva:

• g(n): coste conocido desde el inicio hasta n;
• h(n): estimación heurística desde n hasta la meta;
• f(n) = g(n) + h(n): coste total estimado de la ruta por n.

El conjunto abierto contiene nodos descubiertos que pueden expandirse. En cada paso, retira el nodo con el f más pequeño. Si es la meta, reconstruye la ruta siguiendo los enlaces padre de vuelta al inicio. Si no, relaja cada vecino válido: si el nuevo g es más barato que su coste registrado, actualiza su coste y su padre.

La heurística guía la búsqueda sin cambiar el coste ya pagado. Con movimiento en cuatro direcciones y coste uniforme, la distancia Manhattan es adecuada:

h = |goalX - x| + |goalY - y|

Para movimiento en ocho direcciones con coste diagonal √2, usa distancia octil. La distancia euclídea también se mantiene por debajo o igual al coste real de la ruta en espacio abierto. Sobreestimar puede acelerar la búsqueda pero sacrifica la garantía de ruta óptima.

Implementa la búsqueda con una cola de prioridad

La biblioteca estándar de Python basta para una implementación compacta de referencia:

from heapq import heappop, heappush

def astar(start, goal, neighbors, heuristic):
    frontier = [(heuristic(start, goal), 0, start)]
    came_from = {start: None}
    cost = {start: 0}

    while frontier:
        _, current_cost, current = heappop(frontier)
        if current_cost != cost.get(current):
            continue  # stale queue entry
        if current == goal:
            break

        for next_cell, step_cost in neighbors(current):
            new_cost = current_cost + step_cost
            if new_cost < cost.get(next_cell, float("inf")):
                cost[next_cell] = new_cost
                came_from[next_cell] = current
                heappush(frontier, (
                    new_cost + heuristic(next_cell, goal),
                    new_cost,
                    next_cell,
                ))

    if goal not in came_from:
        return None

    path, cell = [], goal
    while cell is not None:
        path.append(cell)
        cell = came_from[cell]
    return list(reversed(path))

En lugar de intentar editar una entrada existente del montículo, inserta el valor mejorado y descarta las entradas obsoletas al extraerlas. Esto mantiene la implementación pequeña y correcta. En un controlador con memoria limitada, una rejilla de tamaño fijo y arrays indexados evitan mapas dinámicos.

Evita cortar esquinas en diagonal

Con movimiento en ocho direcciones, un vecino diagonal debería rechazarse cuando cualquiera de las celdas cardinales adyacentes esté ocupada. Si no, el planificador puede pasar entre dos obstáculos que se tocan en una esquina —un movimiento imposible para un robot con anchura no nula.

Para un movimiento de (x, y) a (x+1, y+1), verifica que (x+1, y) y (x, y+1) sean ambas transitables. Luego aplica el inflado de huella como una capa de seguridad separada.

Infla los obstáculos para el robot real

Planificar el centro del robot a través de celdas ocupadas sin tratar lo trata como un punto. Infla cada obstáculo al menos con el radio inscrito del robot más un margen de seguridad. De forma equivalente, expande los obstáculos y planifica un centro puntual por el espacio libre restante.

Un radio de inflado duro marca como bloqueadas las celdas cercanas. Un campo de coste graduado añade mayor coste cerca de las paredes, fomentando holgura sin declarar imposible cada celda cercana. El radio elegido debe incluir el voladizo del cuerpo, la incertidumbre de localización y el error de seguimiento.

La resolución del mapa cambia el número de celdas: inflationCells = ceil(inflationMetres / resolution). Prueba la conversión explícitamente. Un radio de 0,20 m son cuatro celdas a 0,05 m/celda, no 0,20 celdas.

Añade costes de recorrido útiles

A* uniforme devuelve la ruta más corta en rejilla. Los robots a menudo necesitan en cambio la ruta de menor coste. Añade coste por celdas cerca de obstáculos, movimiento marcha atrás, giros bruscos o terreno áspero. Mantén todos los costes de arista no negativos, y mantén la heurística no mayor que el coste restante bajo ese modelo si la optimalidad importa.

No añadas una penalización de giro sin incluir la orientación en el estado. Si llegar a la misma celda mirando al este y al norte produce distinto coste futuro, (x, y) solo ya no describe el nodo; usa (x, y, heading) o acepta que la penalización es solo una preferencia de posprocesado.

Valida el planificador de forma sistemática

Empieza con mapas diminutos cuya respuesta sea obvia:

1. Rejilla vacía: la ruta debería acercarse al óptimo en línea recta.
2. Una pared con un hueco: la ruta debe pasar por el hueco.
3. Meta bloqueada: devuelve sin ruta sin entrar en bucle.
4. Inicio igual a meta: devuelve la única celda de inicio.
5. Obstáculos que se tocan en diagonal: sin cortar esquinas.
6. Pasillo estrecho menor que la huella inflada: sin ruta.

Para cada ruta devuelta, afirma que empieza y termina correctamente, que cada paso es adyacente, que ninguna celda está bloqueada y que el coste total iguala la suma de costes de paso. Visualiza las celdas exploradas además de la ruta final; una expansión excesiva suele revelar una mala heurística o una discordancia de unidades.

Diagnostica los fallos comunes del planificador

• La ruta cruza una pared: el indexado de ocupación, el orden fila/columna o la validación diagonal están mal.
• La ruta roza esquinas: los obstáculos no se inflaron o se permite cortar esquinas en diagonal.
• A devuelve una ruta más larga de lo esperado:* la heurística o las unidades de coste de paso no concuerdan, o se aceptan costes obsoletos.
• La búsqueda consume demasiada memoria: la resolución de la rejilla es innecesariamente fina o las entradas duplicadas de la cola nunca se descartan.
• El robot no puede seguir una ruta válida: el plan ignora huella, radio de giro, límites del controlador o error de localización.
• La ruta parpadea tras cada actualización del sensor: la planificación global se reinicia por cambios pequeños sin histéresis de coste ni política de replanificación.

El simulador de laberintos muestra un método de campo de distancias relacionado para un laberinto pequeño desconocido. El simulador de evitación de obstáculos muestra por qué una ruta global aún necesita un controlador local de seguridad. Y cuando la tarea es visitar todas partes en lugar de una meta, nada de esto se aplica —la planificación de cobertura no tiene celda de meta ni heurística, y se mide en distancia por metro cuadrado. En cualquier caso el plan aún hay que conducirlo, y eso es un problema aparte con su propio controlador —ver pure pursuit.