Planification de trajectoire A* sur grille d'occupation

Un planificateur robotique répond à une autre question qu’un contrôleur d’évitement d’obstacles. Le planificateur demande : « Quelles cases libres relient le départ au but au moindre coût ? » Le contrôleur demande : « Quel mouvement est sûr maintenant ? » A* est un algorithme de recherche de graphe qui combine le coût déjà parcouru avec une estimation du coût restant, ce qui en fait un premier planificateur pratique pour une grille d’occupation 2D.

L’itinéraire n’est sûr que comme la grille et l’empreinte du robot fournies. Une ligne mathématiquement la plus courte par centres de cases libres peut quand même racler un mur si les obstacles ne sont pas gonflés.

Transformer la carte en graphe

Une grille d’occupation découpe l’espace en cases égales. Chaque case peut être libre, occupée ou inconnue. Pour un planificateur de base, chaque case franchissable est un nœud de graphe relié à ses voisines.

En mouvement 4 directions, les voisins sont nord, est, sud et ouest, chacun de coût 1. En mouvement 8 directions, ajoutez les diagonales de coût √2. Multiplier plus tard par la résolution de carte convertit la distance grille en mètres.

Décidez du traitement des cases inconnues. Un robot d’intérieur prudent peut les considérer bloquées ; un robot d’exploration peut les autoriser avec un surcoût. Faites-en un paramètre de politique, pas une comparaison accidentelle à la mauvaise valeur numérique.

Comprendre les trois scores de A*

Pour chaque nœud n, A* suit :

• g(n) : coût connu du départ à n ;
• h(n) : estimation heuristique de n au but ;
• f(n) = g(n) + h(n) : coût total estimé de l’itinéraire via n.

L’ensemble ouvert contient les nœuds découverts à explorer. À chaque étape, retirez le nœud de plus petit f. Si c’est le but, reconstruisez l’itinéraire en remontant les liens parents vers le départ. Sinon, relâchez chaque voisin valide : si le nouveau g est moins cher que son coût enregistré, mettez à jour son coût et son parent.

L’heuristique guide la recherche sans changer le coût déjà payé. En mouvement 4 directions à coût uniforme, la distance de Manhattan convient :

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

Pour un mouvement 8 directions à coût diagonal √2, utilisez la distance octile. La distance euclidienne reste aussi inférieure ou égale au vrai coût en espace ouvert. Surestimer peut accélérer la recherche mais sacrifie la garantie d’itinéraire optimal.

Implémenter la recherche avec une file de priorité

La bibliothèque standard Python suffit pour une implémentation compacte de référence :

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

Au lieu d’éditer une entrée existante du tas, poussez la valeur améliorée et écartez les entrées périmées à l’extraction. Cela garde l’implémentation petite et correcte. Sur un contrôleur contraint en mémoire, une grille de taille fixe et des tableaux indexés évitent les cartes dynamiques.

Empêcher la coupe des coins en diagonale

En mouvement 8 directions, un voisin diagonal doit être rejeté quand l’une des cases cardinales adjacentes est occupée. Sinon le planificateur peut passer entre deux obstacles qui se touchent en coin — un mouvement impossible pour un robot de largeur non nulle.

Pour un déplacement de (x, y) vers (x+1, y+1), vérifiez que (x+1, y) et (x, y+1) sont toutes deux franchissables. Appliquez ensuite le gonflement d’empreinte comme couche de sécurité séparée.

Gonfler les obstacles pour le vrai robot

Planifier le centre du robot dans des cases occupées brutes traite le robot comme un point. Gonflez chaque obstacle d’au moins le rayon inscrit du robot plus une marge de sécurité. De façon équivalente, dilatez les obstacles et planifiez un centre ponctuel dans l’espace libre restant.

Un gonflement strict marque bloquées les cases proches. Un champ de coûts gradué ajoute un coût plus grand près des murs, encourageant le dégagement sans déclarer impossible chaque case proche. Le rayon choisi doit inclure le porte-à-faux, l’incertitude de localisation et l’erreur de suivi.

La résolution de carte change le nombre de cases : inflationCells = ceil(inflationMetres / resolution). Testez explicitement la conversion. Un rayon de 0,20 m fait quatre cases à 0,05 m/case, pas 0,20 case.

Ajouter des coûts de traversée utiles

A* uniforme rend le chemin de grille le plus court. Les robots veulent souvent plutôt le chemin de coût minimal. Ajoutez un coût pour les cases près des obstacles, la marche arrière, les virages serrés ou les terrains rugueux. Gardez tous les coûts d’arêtes non négatifs, et gardez l’heuristique au plus égale au coût restant sous ce modèle si l’optimalité compte.

N’ajoutez pas de pénalité de virage sans inclure l’orientation dans l’état. Si arriver à la même case face à l’est et au nord produit des coûts futurs différents, (x, y) seul ne décrit plus le nœud ; utilisez (x, y, heading) ou acceptez que la pénalité ne soit qu’une préférence de post-traitement.

Valider le planificateur systématiquement

Démarrez avec de minuscules cartes à réponse évidente :

1. Grille vide : le chemin doit approcher l’optimum en ligne droite.
2. Un mur avec un passage : l’itinéraire doit passer par le passage.
3. But bloqué : rendre aucun chemin sans boucler.
4. Départ égal au but : rendre la seule case de départ.
5. Obstacles se touchant en diagonale : aucune coupe de coin.
6. Couloir étroit plus petit que l’empreinte gonflée : aucun chemin.

Pour chaque chemin rendu, vérifiez qu’il démarre et finit correctement, que chaque pas est adjacent, qu’aucune case n’est bloquée, et que le coût total égale la somme des coûts de pas. Visualisez les cases explorées ainsi que l’itinéraire final ; une exploration excessive révèle souvent une mauvaise heuristique ou un écart d’unités.

Diagnostiquer les pannes courantes du planificateur

• L’itinéraire traverse un mur : indexation d’occupation, ordre lignes/colonnes, ou validation diagonale faux.
• L’itinéraire racle les coins : obstacles non gonflés ou coupe de coins en diagonale autorisée.
• A rend un itinéraire plus long que prévu :* unités d’heuristique ou de coûts de pas en désaccord, ou coûts périmés acceptés.
• La recherche consomme trop de mémoire : résolution de grille inutilement fine ou entrées de file dupliquées jamais écartées.
• Le robot ne peut pas suivre un chemin valide : le plan ignore empreinte, rayon de braquage, limites du contrôleur ou erreur de localisation.
• Le chemin scintille à chaque mise à jour capteur : la planification globale redémarre pour de petits changements sans hystérésis de coût ni politique de replanification.

Le simulateur de résolution de labyrinthe montre une méthode de champ de distances voisine pour un petit labyrinthe inconnu. Le simulateur d’évitement d’obstacles montre pourquoi une trajectoire globale exige toujours un contrôleur local de sécurité. Et quand la tâche est de visiter partout plutôt qu’un but, rien de cela ne s’applique — la planification de couverture n’a ni case but ni heuristique, et se mesure en distance par mètre carré. Dans tous les cas le plan doit encore être roulé, et c’est un problème séparé avec son propre contrôleur — voyez la poursuite pure.