Pfadfolge-Simulator: Pure Pursuit und Stanley

Steuerung

Regler schaltet zwischen Pure Pursuit und Stanley. Vorausschau ist Pure Pursuits einziger Knopf und für Stanley deaktiviert, das keine nutzt — das ist selbst der Punkt.

Tempo und Vorausschau interagieren — das Lohnende zum Spielen: Setze 1 m/s und finde die beste Vorausschau, dann 3 m/s und finde sie erneut.

Position aus tauscht perfekte Odometrie gegen einen 1-Hz-Fix mit metergroßem Rauschen, gehalten zwischen Updates. Fehler wird immer gegen die Wahrheit gewertet — die Strafe ist also echt statt versteckt.

Ziehe jeden Wegpunkt oder wähle mit 1–8 einen und bewege ihn mit den Pfeiltasten. Der ganze Lauf rechnet beim Ziehen neu.

Theorie

Beide Regler beantworten dieselbe Frage — wo bin ich, wo ist der Pfad, welcher Lenkwinkel? — und beantworten sie völlig verschieden.

Pure Pursuit ist Geometrie. Finde den Pfadpunkt eine Vorausschaudistanz L voraus und fahre den Kreisbogen hindurch:

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

mit y Querversatz des Zielpunkts im Roboterbezug und b Radstand. Ein Parameter, und es ist eine Distanz statt Verstärkung — überträgt sich also zwischen Robotern.

Stanley ist Regelung. Referenziere die Vorderachse und addiere zwei Terme:

δ = θ_e + atan(k · e / v)

Kursfehler plus Arkustangens der Querabweichung. Durch Tempo zu teilen lässt eine Verstärkung über einen Tempobereich arbeiten. k ist eine echte Regelverstärkung und überträgt sich nicht.

Das Labor fügt hinzu, was Lehrbuchherleitungen weglassen: 0,4 s Gesamtverzug als 0,25 s Lenkantwort erster Ordnung plus 0,15 s Erfassungs- und Rechenverzug. Ohne ihn wird Pure Pursuit mit schrumpfender Vorausschau monoton besser, und die ganze Abstimmfrage verschwindet.

Algorithmus

Jeder Schritt, für beide Regler:

1. Auf den Pfad projizieren, suchend in einem Fenster um die zuletzt erreichte Bogenlänge statt im ganzen Pfad. Eine globale Suche matcht das nächste Segment — ein Kurs, der sich zurückfaltet, teleportiert den Fortschritt zu einer Ecke von vor einer Minute.
2. Gleichstand am Scheitel zum ausgehenden Segment auflösen. Hinter einer Ecke projizieren beide Nachbarsegmente auf denselben Punkt in derselben Distanz; das eingehende zu behalten übergibt Kursfehler und Querversatz nahe null, während es geradeaus vom Pfad fährt, lautlos.
3. Lenkwinkel berechnen nach der eigenen Regel des Reglers und auf das mechanische Limit klemmen.
4. Verzug anwenden, dann Fahrradmodell integrieren.
5. Gegen die Wahrheit werten, nie die Schätzung.

Schritt 2 ist keine Optimierung. Er war der Unterschied zwischen Stanley-geht und Stanley-verlässt-selbstsicher bei exakt 78 % jedes Laufs den Kurs.

Quellcode

Pure Pursuit, komplett, in der Form auf einem echten Roboter:

float lastS = 0;   // arc length last reached — state, and it only moves forward

float pursue(float x, float y, float heading, float speed) {
  // The lookahead is scheduled on speed, with a hard floor: at a standstill a
  // proportional schedule asks for zero, which is a division by zero in disguise.
  float lookahead = constrain(0.7f * speed + 0.4f, 0.4f, 3.0f);

  float s = projectOntoPath(x, y, lastS);      // windowed around lastS
  lastS = max(lastS, s);
  Point goal = pointAtArcLength(lastS + lookahead);

  float dx = goal.x - x, dy = goal.y - y;
  float lateral = -sinf(heading) * dx + cosf(heading) * dy;
  float ld = max(0.3f, hypotf(dx, dy));        // clamp, or curvature blows up

  float curvature = 2.0f * lateral / (ld * ld);
  return curvature;                            // omega = v * curvature
}

Die drei Zeilen, die zählen, sind der Kennlinienboden, die lastS-Monotonie und die ld-Klemmung. Alles andere ist die Formel.

Annahmen und Unterschiede zur Hardware

Das Labor gibt dem Roboter eine perfekte Pfadkarte, perfekt bekannten Radstand, keinen Schlupf und Verzug als sauberes Modell erster Ordnung. Ein echter Roboter hat nichts davon.

Schlupf macht die Positionsschätzung falsch, wie kein Regler erkennen kann. Getriebespiel fügt Totweg an jeder Lenkumkehr hinzu — verhält sich wie Extraverzug und schiebt die optimale Vorausschau hoch. Und ein Differenzialantrieb hat gar keinen Lenkwinkel — er hat eine kommandierte Drehrate, ω = v κ ersetzt also das atan, und der Radstand zählt nur dafür, wie eng er drehen kann.

Was alles überlebt, ist die Form: ein inneres Optimum, wandernd rechts mit Tempo, und Fehler in diesem Optimum wachsend mit Tempo ohnehin.

Schaltplan

Der Regler ist der billige Teil. Was er vom Rest des Roboters braucht, ist der teure.

Position. Quadratur-Encoder an beiden Rädern und Differenzialantrieb-Odometrie, die sie integriert, in Schleifenrate. Drinnen genügt das für wenige dutzend Meter.

Kurs. Ein Gyroskop für kurz, ein Magnetometer für absolut, oder beides. Kursfehler ist der Term, den beide Regler am stärksten gewichten — und den Odometrie zuerst verliert.

Absolute Korrektur, draußen. Ein GPS-Modul, in die Schätzung fusioniert statt direkt verfolgt — der Unterschied ist 0,09 m RMS-Fehler gegen 1,25.

Für die Theorie in Gänze siehe Pure Pursuit und Querabweichung; zum Bauen startet der GPS-Wegpunkt-Rover drinnen auf Encodern und geht erst raus, wenn der Folger läuft.