Pianificazione del percorso con A* su una griglia di occupazione

Un pianificatore per robot risponde a una domanda diversa rispetto a un controllore di evitamento degli ostacoli. Il pianificatore chiede: «Quali celle libere collegano la partenza all’obiettivo con il costo più basso?» Il controllore chiede: «Quale movimento è sicuro adesso?» A* è un algoritmo di ricerca su grafi che combina il costo già percorso con una stima del costo rimanente, rendendolo un primo pianificatore pratico per una griglia di occupazione bidimensionale.

Il percorso è sicuro solo quanto la griglia e l’ingombro del robot forniti. Una retta matematicamente più breve attraverso i centri delle celle libere può comunque sfiorare un muro se gli ostacoli non vengono gonfiati.

Trasforma la mappa in un grafo

Una griglia di occupazione divide lo spazio in celle uguali. Ogni cella può essere libera, occupata o sconosciuta. Per un pianificatore di base, ogni cella attraversabile è un nodo del grafo collegato ai suoi vicini.

Con movimento a quattro vie, i vicini sono nord, est, sud e ovest, ciascuno con costo 1. Con movimento a otto vie, aggiungi le diagonali con costo √2. Moltiplicando in seguito per la risoluzione della mappa, la distanza sulla griglia si converte in metri.

Decidi come trattare le celle sconosciute. Un robot prudente per ambienti interni può considerarle bloccate; un robot di esplorazione può consentirle con un costo aggiuntivo. Rendi questa scelta un criterio esplicito, non un confronto accidentale con il valore numerico sbagliato.

Comprendi i tre punteggi di A*

Per ogni nodo n, A* tiene traccia di:

• g(n): costo noto dalla partenza a n;
• h(n): stima euristica da n all’obiettivo;
• f(n) = g(n) + h(n): costo totale stimato del percorso che passa per n.

L’insieme aperto contiene i nodi scoperti che possono essere espansi. A ogni passo, rimuovi il nodo con il valore f più piccolo. Se è l’obiettivo, ricostruisci il percorso seguendo i collegamenti verso il padre fino alla partenza. Altrimenti, aggiorna ogni vicino valido: se il nuovo g è più basso del costo registrato, aggiornane il costo e il padre.

L’euristica guida la ricerca senza modificare il costo già sostenuto. Con movimento a quattro vie e costo uniforme, la distanza di Manhattan è adatta:

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

Per il movimento a otto vie con costo diagonale √2, usa la distanza ottile. Anche la distanza euclidea resta inferiore o uguale al costo reale del percorso in uno spazio aperto. Una sovrastima può rendere la ricerca più veloce, ma sacrifica la garanzia del percorso ottimale.

Implementa la ricerca con una coda a priorità

La libreria standard di Python è sufficiente per un’implementazione di riferimento compatta:

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))

Invece di tentare di modificare una voce già presente nell’heap, inserisci il valore migliore e scarta le voci obsolete quando le estraggi. Questo mantiene l’implementazione piccola e corretta. Su un controllore con memoria limitata, una griglia a dimensione fissa e vettori indicizzati evitano mappe dinamiche.

Impedisci il taglio diagonale degli angoli

Con movimento a otto vie, un vicino diagonale deve essere rifiutato quando una delle due celle cardinali adiacenti è occupata. Altrimenti il pianificatore può passare tra due ostacoli che si toccano in un angolo—un movimento impossibile per un robot con larghezza diversa da zero.

Per uno spostamento da (x, y) a (x+1, y+1), verifica che (x+1, y) e (x, y+1) siano entrambe attraversabili. Poi applica il gonfiamento dell’ingombro come un livello di sicurezza separato.

Gonfia gli ostacoli per il robot reale

Pianificare il centro del robot attraverso celle occupate grezze tratta il robot come un punto. Gonfia ogni ostacolo di almeno il raggio inscritto del robot più un margine di sicurezza. In modo equivalente, espandi gli ostacoli e pianifica il punto centrale nello spazio libero rimanente.

Un raggio di gonfiamento rigido marca come bloccate le celle vicine. Un campo di costo graduale aggiunge un costo maggiore vicino ai muri, incentivando la distanza senza dichiarare impossibili ogni cella vicina. Il raggio scelto deve includere la sporgenza del corpo, l’incertezza di localizzazione e l’errore di tracciamento.

La risoluzione della mappa cambia il numero di celle: inflationCells = ceil(inflationMetres / resolution). Verifica esplicitamente la conversione. Un raggio di 0.20 m equivale a quattro celle con 0.05 m/cella, non a 0.20 celle.

Aggiungi costi di percorso utili

A* uniforme restituisce il percorso più breve sulla griglia. I robot spesso hanno invece bisogno del percorso di costo più basso. Aggiungi costo per le celle vicine agli ostacoli, per la retromarcia, per le svolte brusche o per il terreno irregolare. Mantieni tutti i costi degli archi non negativi e, se l’ottimalità conta, mantieni l’euristica non superiore al costo rimanente secondo quel modello.

Non aggiungere una penalità di svolta senza includere l’orientamento nello stato. Se arrivare alla stessa cella rivolti a est o a nord produce un costo futuro diverso, (x, y) da solo non descrive più il nodo; usa (x, y, heading) oppure accetta che la penalità sia soltanto una preferenza di post-elaborazione.

Valida il pianificatore sistematicamente

Comincia con mappe minuscole la cui risposta è ovvia:

1. Griglia vuota: il percorso deve avvicinarsi all’ottimo in linea retta.
2. Un muro con un’apertura: il percorso deve attraversare l’apertura.
3. Obiettivo bloccato: restituisci un percorso assente senza entrare in un ciclo.
4. Partenza uguale a obiettivo: restituisci la singola cella di partenza.
5. Ostacoli che si toccano diagonalmente: nessun taglio degli angoli.
6. Corridoio più stretto dell’ingombro gonfiato: nessun percorso.

Per ogni percorso restituito, verifica con assert che inizi e finisca correttamente, che ogni passo sia adiacente, che nessuna cella sia bloccata e che il costo totale sia uguale alla somma dei costi dei passi. Visualizza anche le celle esplorate oltre al percorso finale; un’eccessiva espansione rivela spesso un’euristica sbagliata o un disallineamento delle unità.

Diagnosi i problemi comuni del pianificatore

• Il percorso attraversa un muro: l’indicizzazione dell’occupazione, l’ordine righe/colonne o la validazione diagonale sono sbagliati.
• Il percorso taglia gli angoli: gli ostacoli non sono stati gonfiati oppure è consentito il taglio diagonale degli angoli.
• A restituisce un percorso più lungo del previsto:* le unità dell’euristica o del costo del passo non coincidono, oppure vengono accettati costi obsoleti.
• La ricerca consuma troppa memoria: la risoluzione della griglia è inutilmente fine oppure le voci duplicate nella coda non vengono mai scartate.
• Il robot non riesce a seguire un percorso valido: il piano ignora l’ingombro, il raggio di sterzata, i limiti del controllore o l’errore di localizzazione.
• Il percorso lampeggia dopo ogni aggiornamento dei sensori: la pianificazione globale viene riavviata per piccole variazioni senza isteresi del costo o una policy di ripianificazione.

Il Simulatore di risolutore di labirinto dimostra un metodo correlato basato su un campo di distanza per un piccolo labirinto sconosciuto. L’Simulatore di evitamento degli ostacoli mostra perché un percorso globale ha ancora bisogno di un controllore locale di sicurezza. E quando il compito è visitare ovunque invece di raggiungere un solo obiettivo, niente di tutto questo si applica—la pianificazione del percorso di copertura non ha una cella obiettivo né un’euristica e viene misurata in distanza per metro quadro. In ogni caso il piano deve comunque essere guidato e questo è un problema separato, con un controllore tutto suo—vedi pure pursuit.