A*-padplanning op een bezettingsraster

Een robotplanner beantwoordt een andere vraag dan een regelaar voor obstakelvermijding. De planner vraagt: “Welke vrije cellen verbinden het startpunt met het doel tegen de laagste kosten?” De regelaar vraagt: “Welke beweging is op dit moment veilig?” A* is een graafzoekalgoritme dat de al afgelegde kosten combineert met een schatting van de resterende kosten, wat het tot een praktische eerste planner voor een tweedimensionaal bezettingsraster maakt.

De route is zo veilig als het raster en de robotvoetafdruk die je hem meegeeft. De wiskundig kortste lijn door de middens van vrije cellen kan alsnog langs een muur schuren als obstakels niet zijn opgeblazen.

Maak van de kaart een graaf

Een bezettingsraster verdeelt de ruimte in gelijke cellen. Elke cel kan vrij, bezet of onbekend zijn. Voor een eenvoudige planner is elke begaanbare cel een graafknoop die verbonden is met zijn buren.

Bij vierrichtingsbeweging zijn de buren noord, oost, zuid en west, elk met kosten 1. Bij achtrichtingsbeweging komen er diagonalen bij met kosten √2. Vermenigvuldigen met de rasterresolutie zet later een rasterafstand om in meters.

Bepaal hoe je onbekende cellen behandelt. Een voorzichtige binnenrobot kan ze als geblokkeerd beschouwen; een verkenningsrobot kan ze met extra kosten toestaan. Maak dat een expliciete beleidsinvoer en geen toevallige vergelijking tegen de verkeerde numerieke waarde.

Begrijp de drie A*-scores

Voor elke knoop n houdt A* bij:

• g(n): bekende kosten van het startpunt naar n;
• h(n): heuristische schatting van n naar het doel;
• f(n) = g(n) + h(n): geschatte totale routekosten via n.

De open set bevat ontdekte knopen die uitgebreid mogen worden. Haal bij elke stap de knoop met de kleinste f eruit. Is het het doel, dan herstel je de route door ouderlinks terug te volgen naar het startpunt. Anders ontspan je elke geldige buur: is de nieuwe g goedkoper dan de vastgelegde kosten, dan werk je de kosten en de ouder bij.

De heuristiek stuurt de zoekopdracht zonder de al betaalde kosten te veranderen. Bij vierrichtingsbeweging en uniforme kosten is de Manhattanafstand geschikt:

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

Bij achtrichtingsbeweging met diagonale kosten √2 gebruik je de octiele afstand. De euclidische afstand blijft in open terrein eveneens onder of gelijk aan de werkelijke padkosten. Overschatten maakt de zoekopdracht sneller maar kost de garantie op een optimale route.

Implementeer de zoekopdracht met een prioriteitswachtrij

De standaardbibliotheek van Python volstaat voor een compacte referentie-implementatie:

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

In plaats van te proberen een bestaande heap-entry te bewerken, duw je de verbeterde waarde en gooi je verouderde entries weg wanneer je ze eruit haalt. Dat houdt de implementatie klein en correct. Op een controller met weinig geheugen vermijden een raster van vaste grootte en geïndexeerde arrays dynamische maps.

Voorkom hoeksnijden bij diagonale stappen

Bij achtrichtingsbeweging moet een diagonale buur worden geweigerd wanneer een van de aangrenzende cardinale cellen bezet is. Anders kan de planner tussen twee obstakels door die elkaar in een hoek raken — een onmogelijke beweging voor een robot met een breedte die niet nul is.

Controleer voor een zet van (x, y) naar (x+1, y+1) dat (x+1, y) en (x, y+1) beide begaanbaar zijn. Pas daarna de opblazing van de voetafdruk toe als een aparte veiligheidslaag.

Blaas obstakels op voor de echte robot

Het robotmiddelpunt door ruwe bezette cellen plannen behandelt de robot als een punt. Blaas elk obstakel op met minstens de ingeschreven straal van de robot plus een veiligheidsmarge. Equivalent: vergroot de obstakels en plan een puntvormig middelpunt door de overgebleven vrije ruimte.

Een harde opblazingsstraal markeert nabije cellen als geblokkeerd. Een genuanceerd kostveld voegt hogere kosten toe nabij muren, wat extra ruimte stimuleert zonder elke nabije cel onmogelijk te verklaren. De gekozen straal moet overstekende carrosserie, localisatieonzekerheid en volgfout meenemen.

De rasterresolutie verandert het aantal cellen: inflationCells = ceil(inflationMetres / resolution). Test die omrekening expliciet. Een straal van 0,20 m is vier cellen bij 0,05 m per cel, niet 0,20 cellen.

Voeg nuttige doorkruiskosten toe

Uniforme A* geeft het kortste pad in het raster terug. Robots hebben vaak het pad met laagste kosten nodig. Voeg kosten toe voor cellen nabij obstakels, achteruit rijden, scherpe bochten of oneffen terrein. Houd alle kantenkosten niet-negatief, en houd de heuristiek niet groter dan de resterende kosten in dat model als optimaliteit belangrijk is.

Voeg geen boetepenalty toe zonder de oriëntatie in de toestand op te nemen. Als aankomen in dezelfde cel met het gezicht naar oosten of naar noorden verschillende toekomstige kosten oplevert, beschrijft (x, y) alleen de knoop niet meer; gebruik (x, y, heading) of accepteer dat de penalty alleen een voorkeur in nabewerking is.

Valideer de planner systematisch

Begin met kleine kaarten waarvan het antwoord voor de hand ligt:

1. Leeg raster: het pad moet de rechte-lijnsoptimum benaderen.
2. Eén muur met één opening: de route moet door die opening.
3. Geblokkeerd doel: geef geen pad terug zonder te blijven draaien.
4. Start gelijk aan doel: geef de enkele startcel terug.
5. Diagonaal raakende obstakels: geen hoeksnijden.
6. Smalle gang kleiner dan de opgeblazen voetafdruk: geen pad.

Controleer voor elk teruggegeven pad dat het correct begint en eindigt, dat elke stap naast elkaar ligt, dat geen cel geblokkeerd is en dat de totale kosten gelijk zijn aan de som van de stapkosten. Visualiseer zowel de onderzochte cellen als de uiteindelijke route; overmatige uitbreiding onthult vaak een slechte heuristiek of een eenhedenfout.

Diagnoseer veelvoorkomende plannerfouten

• De route loopt door een muur: de indexering van de bezetting, de rij-/kolomvolgorde of de diagonale validatie klopt niet.
• De route snijdt hoeken: obstakels zijn niet opgeblazen of diagonaal hoeksnijden is toegestaan.
• A geeft een langere route terug dan verwacht:* de eenheden van de heuristiek of van de stapkosten botsen, of verouderde kosten worden geaccepteerd.
• De zoekopdracht gebruikt te veel geheugen: de rasterresolutie is onnodig fijn of dubbele wachtrijeintries worden nooit weggegooid.
• De robot kan een geldig pad niet volgen: het plan negeert voetafdruk, draaistraal, regelgrenzen of localisatiefout.
• Het pad flikkert na elke sensorupdate: de globale planning wordt voor kleine veranderingen hervat zonder kostenhysterese of herplaningsbeleid.

De Maze Solver Simulator laat een verwant afstandsveldmethodetje zien voor een klein onbekend labyrint. De Obstacle Avoidance Simulator laat zien waarom een globaal pad nog steeds een lokale veiligheidsregelaar nodig heeft. En als de taak is om overal te komen in plaats van bij één doel, geldt hier niets van — dekkende padplanning heeft geen doelcel en geen heuristiek, en wordt in plaats daarvan gemeten in afstand per vierkante meter. Hoe dan ook moet het plan nog worden gereden, en dat is een apart probleem met een eigen regelaar — zie pure pursuit.