Pure pursuit: een pad volgen met één knop

De sport die niemand leert

Een planner geeft je een pad. A* geeft je een lijst rastercellen; een kartering geeft je een lijst waypoints; een missie geeft je een lijn op een kaart. Geen van die dingen zegt tegen de wielen wat ze moeten doen.

In dat gat woont pure pursuit, en het is één alinea geometrie:

Zoek het punt op het pad op een vaste afstand voor je. Rijd de cirkelboog die er doorheen gaat. Herhaal.

Er is geen foutsignaal, geen integraalterm, geen gainschema. Er is één getal—de vooruitkijkafstand—en dat bepaalt alles.

De geometrie

Zet het doelpunt in het eigen stelsel van de robot: x vooruit, y naar links, op afstand L_d. Een cirkelboog vanaf de huidige positie van de robot door dat punt heeft de kromming

κ = 2y / L_d²

die de moeite waard is om twee keer te lezen, want alles volgt eruit. De laterale offset y staat in de teller, dus de boog buigt sterker naarmate het doel verder van de as ligt. De vooruitkijk staat gekwadrateerd in de noemer, dus verdubbelen ervan brengt de kromming tot een kwart terug.

Omgezet in een straal, en in een stuurhoek voor een voertuig met wielbasis b:

R = L_d² / 2y            δ = atan(2 b sin α / L_d)

Bij differentiaalaandrijving is er geen stuurhoek—geef je rechtstreeks een draaisnelheid op, ω = v κ. Dezelfde kromming, een andere actuator.

De ene knop, en waarom hij zichzelf tegenwerkt

Korte vooruitkijk. Het doelpunt is dichtbij, dus een kleine laterale offset geeft een grote kromming. De robot volgt het pad strak—en reageert hevig op elke wiebel, ook op de wiebels die zijn eigen stuurcommando net veroorzaakte.

Lange vooruitkijk. Het doelpunt is ver, de kromming is zacht, het sturen is vloeiend—en bij een bocht is het doelpunt de bocht al voorbij terwijl de robot dat niet is, dus de robot snijdt de bocht af.

Die twee trekken elkaar in tegengestelde richting en er zit niets in het algoritme om ze in evenwicht te brengen. Jij kiest een getal.

De valkuil in elke schoolboekafleiding

Schrijf pure pursuit op papier, voor een robot die direct stuurt, en korter is altijd beter. De fout daalt monotoon naarmate de vooruitkijk kleiner wordt. Er is geen optimum, en de U-vorm hierboven bestaat niet.

Echte robots hebben naloop. Sturen kost tijd om de opgedragen hoek te bereiken; de pose waarop je handelt is tegen de tijd dat de wielen reageren al enkele tientallen milliseconden oud. De simulator modelleert een stuurtijdconstante van 0,25 s en 0,15 s aan detectie en berekening—0,4 s totale naloop, wat voor een hobby-rover heel gewoon is.

Die naloop is de hele reden dat het optimum bestaat. Een korte vooruitkijk geeft een correctie op, de correctie komt te laat, de robot is de fout al voorbij, en de volgende opdracht corrigeert hard de andere kant op. Een regelaar die sneller is dan zijn eigen actuator oscilleert. Dat is hetzelfde feit dat de differentiatieterm van een PID bepaalt en hetzelfde dat achter de remafstand zit: latentie is geen detail, het is datgene wat de afstelling bepaalt.

En hoe kies je hem dan?

De gemeten optima, en wat ze zijn in eenheden die je kunt overnemen:

Twee eerlijke lezingen van die tabel.

De vooruitkijk groeit met de snelheid, maar niet evenredig. Het gebruikelijke advies “maak er een vast aantal seconden reistijd van” klopt bij benadering maar niet—de tijd daalt over dit bereik van 0,90 s naar 0,47 s.

Hij zit op één tot twee keer de afstand die je aflegt tijdens je eigen regelnaloop. Dat is de nuttige regel, want hij is meetbaar: bepaal je naloop, vermenigvuldig met je snelheid en begin daar. Op deze robot loopt v × lag van 0,4 m naar 1,2 m en liggen de optima tussen 0,9 m en 1,4 m.

In de praktijk lever je dat af als een klein lineair schema en klem je het:

float lookahead = constrain(K_LOOK * speed + L_MIN, L_MIN, L_MAX);

L_MIN is belangrijker dan K_LOOK. Bij stilstand wil het schema een vooruitkijk van nul, en dat is een deling door nul in vermomming.

Implementeren

// Pure pursuit over a waypoint list. Returns the curvature to command.
float pursue(float x, float y, float heading, float lookahead) {
  // 1. Where am I on the path? Search only near the arc length I last reached —
  //    a global search latches onto whichever segment happens to be nearest, and a
  //    path that doubles back will teleport your progress to a corner you passed
  //    twenty seconds ago.
  float s = projectOntoPath(x, y, lastS);
  lastS = max(lastS, s);

  // 2. The goal point, one lookahead further along.
  Point goal = pointAtArcLength(lastS + lookahead);

  // 3. Into the robot frame, then straight into the curvature formula.
  float dx = goal.x - x, dy = goal.y - y;
  float lateral = -sin(heading) * dx + cos(heading) * dy;
  float ld = max(0.3f, hypotf(dx, dy));
  return 2.0f * lateral / (ld * ld);
}

Drie dingen in twaalf regels zijn meer waard dan de rest:

• lastS is toestand, en hij loopt alleen vooruit. Zonder hem strandt elke regelaar op een pad dat zichzelf kruist.
• Klem ld. Naarmate het doelpunt dichterbij komt, gaat de kromming naar oneindig.
• Het doelpunt ligt op het pad, niet op het dichtstbijzijnde waypoint. Op waypoints mikken laat de robot heen en weer zigzaggen; mikken op een punt op het lijnstuk is wat de beweging vloeiend maakt.

Wat het wel en niet doet

Hij blijft niet staan om zich naar het pad te draaien. Pure pursuit heeft geen begrip van stilstaan—de kromming is een vorm, en een vorm heeft voorwaartse beweging nodig. Start de robot ongeveer uitgelijnd, of rij eerst een korte openingsboog.

Hij heeft een minimale draaistraal en het pad houdt daar misschien geen rekening mee. Is een bocht scherper dan b / tan(δ_max), dan kan de robot hem niet volgen, hoe je ook afstelt. Verzacht het pad of rem af voor de bocht—en dat is waar een bewegingsprofiel voor is.

Hij snijdt altijd bochten af, en dat is bewust. In de gemeten rit op 2 m/s reed hij 0,8% minder ver dan het opgedragen pad. Is dat onacceptabel, dan wil je een andere regelaar—Stanley stuurt in plaats daarvan op cross-trackfout en slingert ruim uit in plaats van af te snijden.

Hij repareert geen slechte positiebepaling. Al het voorgaande neemt aan dat de robot weet waar hij is. Geef hem een GPS-fix van 1 Hz en de RMS-fout gaat van 0,09 m naar 1,25 m—zie GPS-waypointnavigatie.

Als het misgaat

Rij beide regelaars over dezelfde baan in de padvolgsimulator—sleep de vooruitkijk en de hele rit tekent zich opnieuw. Bouw daarna de GPS-waypointrover, waar de positiebepaling het lastige deel wordt.