A*-Pfadplanung auf einem Belegungsraster

Ein Roboterplaner beantwortet eine andere Frage als eine Hindernisvermeidungs-Regelung. Der Planer fragt: „Welche freien Zellen verbinden Start und Ziel zu niedrigsten Kosten?“ Die Regelung fragt: „Welche Bewegung ist gerade jetzt sicher?“ A* ist ein Graphsuch-Algorithmus, der bereits gefahrene Kosten mit einer Schätzung der Restkosten verbindet – ein praktischer erster Planer für ein zweidimensionales Belegungsraster.

Die Route ist nur so sicher wie Raster und Robotergrundriss, die man ihm gibt. Eine mathematisch kürzeste Linie durch freie Zellmitten kann trotzdem eine Wand streifen, wenn Hindernisse nicht aufgeweitet sind.

Aus der Karte einen Graphen machen

Ein Belegungsraster teilt den Raum in gleiche Zellen. Jede Zelle kann frei, belegt oder unbekannt sein. Für einen einfachen Planer ist jede befahrbare Zelle ein Graphknoten, verbunden mit seinen Nachbarn.

Bei vier Richtungen sind Nachbarn Nord, Ost, Süd und West, je mit Kosten 1. Bei acht Richtungen kommen Diagonalen mit Kosten √2 hinzu. Multiplikation mit der Kartenauflösung rechnet Rasterdistanz später in Meter um.

Entscheide, wie unbekannte Zellen behandelt werden. Ein vorsichtiger Innenraumroboter betrachtet sie als blockiert; ein Erkundungsroboter erlaubt sie mit Extrakosten. Mache das zu einer Policy-Eingabe, nicht zu einem versehentlichen Vergleich gegen einen falschen Zahlenwert.

Die drei A*-Werte verstehen

Für jeden Knoten n verfolgt A*:

• g(n): bekannte Kosten vom Start bis n;
• h(n): Heuristik-Schätzung von n bis zum Ziel;
• f(n) = g(n) + h(n): geschätzte Gesamtroutenkosten über n.

Die offene Menge hält entdeckte Knoten, die noch expandiert werden können. Entferne jeweils den Knoten mit kleinstem f. Ist es das Ziel, rekonstruiere die Route, indem du den Elternzeigern zurück zum Start folgst. Sonst entspanne jeden gültigen Nachbarn: Ist das neue g billiger als seine gespeicherten Kosten, aktualisiere Kosten und Elternteil.

Die Heuristik lenkt die Suche, ohne die bereits gezahlten Kosten zu ändern. Bei vier Richtungen und einheitlichen Kosten passt die Manhattan-Distanz:

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

Für acht Richtungen mit Diagonalkosten √2 nutze die Oktildistanz. Auch die euklidische Distanz bleibt im freien Raum unter oder gleich den echten Pfadkosten. Überschätzen kann die Suche beschleunigen, opfert aber die Optimalitätsgarantie.

Die Suche mit Prioritätswarteschlange implementieren

Die Python-Standardbibliothek reicht für eine kompakte Referenzimplementierung:

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

Statt einen vorhandenen Heap-Eintrag zu ändern, schiebe den verbesserten Wert hinein und verwerfe alte Einträge beim Entnehmen. Das hält die Implementierung klein und korrekt. Auf einem speicherknappen Controller vermeiden ein fest dimensioniertes Raster und indexierte Arrays dynamische Karten.

Diagonales Eckenschneiden verhindern

Bei acht Richtungen sollte ein diagonaler Nachbar verworfen werden, wenn eine der beiden angrenzenden Kardinalzellen belegt ist. Sonst kann der Planer zwischen zwei Hindernissen hindurch, die sich an einer Ecke berühren – eine unmögliche Bewegung für einen Roboter mit Breite größer null.

Für eine Bewegung von (x, y) nach (x+1, y+1) prüfe, dass (x+1, y) und (x, y+1) beide befahrbar sind. Wende danach die Grundriss-Aufweitung als separate Sicherheitsschicht an.

Hindernisse für den echten Roboter aufweiten

Den Robotermittelpunkt durch rohe belegte Zellen zu planen behandelt den Roboter wie einen Punkt. Weite jedes Hindernis mindestens um den einbeschriebenen Roboterradius plus Sicherheitsmarge auf. Äquivalent: Erweitere Hindernisse und plane einen Punktmittelpunkt durch den übrigen freien Raum.

Eine harte Aufweitung markiert nahe Zellen als blockiert. Ein abgestuftes Kostenfeld addiert größere Kosten nahe Wänden, fördert Abstand, ohne jede nahe Zelle für unmöglich zu erklären. Der gewählte Radius muss Überhang, Lokalisierungsunsicherheit und Folgefehler enthalten.

Die Kartenauflösung ändert die Zellenzahl: inflationCells = ceil(inflationMetres / resolution). Teste die Umrechnung explizit. Ein Radius von 0,20 m sind vier Zellen bei 0,05 m/Zelle, nicht 0,20 Zellen.

Nützliche Fahrkosten hinzufügen

Einheitliches A* liefert den kürzesten Rasterpfad. Roboter brauchen oft stattdessen den kostengünstigsten Pfad. Addiere Kosten für Zellen nahe Hindernissen, Rückwärtsfahrt, scharfe Kurven oder raues Gelände. Halte alle Kantenkosten nichtnegativ und die Heuristik nicht größer als die Restkosten unter diesem Modell, wenn Optimalität zählt.

Füge keine Kurvenstrafe hinzu, ohne die Orientierung in den Zustand aufzunehmen. Führt das Ankommen in derselben Zelle nach Osten bzw. Norden blickend zu verschiedenen Folgekosten, beschreibt (x, y) allein den Knoten nicht mehr; nutze (x, y, heading) oder akzeptiere, dass die Strafe nur eine Nachbearbeitungs-Präferenz ist.

Den Planer systematisch validieren

Beginne mit winzigen Karten, deren Antwort offensichtlich ist:

1. Leeres Raster: Der Pfad sollte sich dem geradlinigen Optimum nähern.
2. Eine Wand mit einer Lücke: Die Route muss durch die Lücke führen.
3. Blockiertes Ziel: Keinen Pfad zurückgeben, ohne zu schleifen.
4. Start gleich Ziel: Die einzelne Startzelle zurückgeben.
5. Diagonal berührende Hindernisse: Kein Eckenschneiden.
6. Enger Korridor kleiner als der aufgeweitete Grundriss: Kein Pfad.

Prüfe für jeden zurückgegebenen Pfad, dass er korrekt beginnt und endet, jeder Schritt benachbart ist, keine Zelle blockiert ist und die Gesamtkosten der Summe der Schrittkosten entsprechen. Visualisiere erkundete Zellen wie die finale Route; exzessive Expansion verrät oft eine schlechte Heuristik oder Einheitenfehler.

Typische Planerfehler diagnostizieren

• Die Route quert eine Wand: Belegungsindexierung, Zeilen-/Spaltenreihenfolge oder Diagonalprüfung ist falsch.
• Die Route schneidet Ecken: Hindernisse wurden nicht aufgeweitet oder diagonales Eckenschneiden ist erlaubt.
• A* liefert eine längere Route als erwartet: Heuristik- oder Schrittkosten-Einheiten stimmen nicht überein, oder alte Kosten werden akzeptiert.
• Die Suche verbraucht zu viel Speicher: Rasterauflösung ist unnötig fein oder doppelte Warteschlangeneinträge werden nie verworfen.
• Der Roboter kann einem gültigen Pfad nicht folgen: Der Plan ignoriert Grundriss, Wenderadius, Reglergrenzen oder Lokalisierungsfehler.
• Der Pfad flackert nach jeder Sensoraktualisierung: Die globale Planung wird für kleine Änderungen ohne Kosten-Hysterese oder Neuplanungs-Policy neu gestartet.

Der Labyrinthlöser-Simulator zeigt ein verwandtes Distanzfeld-Verfahren für ein kleines unbekanntes Labyrinth. Der Hindernisvermeidungs-Simulator zeigt, warum ein globaler Pfad trotzdem eine lokale Sicherheitsregelung braucht. Und wenn die Aufgabe lautet, überall hinzufahren statt zu einem Ziel, gilt nichts davon – Abdeckungs-Pfadplanung hat keine Zielzelle und keine Heuristik und wird in Distanz pro Quadratmeter gemessen. So oder so muss der Plan noch gefahren werden, und das ist ein eigenes Problem mit eigenem Regler – siehe Pure Pursuit.