Pure Pursuit: Pfadfolge mit einem einzigen Knopf

Die Sprosse, die niemand lehrt

Ein Planer gibt dir einen Pfad. A* gibt dir eine Liste von Rasterzellen; eine Vermessung gibt dir eine Wegpunktliste; eine Mission gibt dir eine Linie auf einer Karte. Keines davon sagt den Rädern, was zu tun ist.

Genau in dieser Lücke lebt Pure Pursuit, und es ist ein Absatz Geometrie:

Finde den Punkt auf dem Pfad in fester Distanz vor dir. Fahre den Kreisbogen, der durch ihn geht. Wiederholen.

Es gibt kein Fehlersignal, keinen Integralanteil, keinen Verstärkungsfahrplan. Es gibt eine Zahl — die Vorausschaudistanz — und sie entscheidet alles.

Die Geometrie

Lege den Zielpunkt ins robotereigene Koordinatensystem: x vorwärts, y nach links, in Distanz L_d. Ein Kreisbogen von der aktuellen Roboterposition durch diesen Punkt hat die Krümmung

κ = 2y / L_d²

die man zweimal lesen sollte, denn alles folgt daraus. Der Seitenversatz y steht im Zähler, also krümmt sich der Bogen umso stärker, je weiter der Zielpunkt seitlich liegt. Die Vorausschau steht quadriert im Nenner, also viertelt Verdoppeln die Krümmung.

Umgedreht in einen Radius und in einen Lenkwinkel für ein Fahrzeug mit Radstand b:

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

Für einen Differenzialantrieb gibt es keinen Lenkwinkel — du kommandierst direkt eine Drehrate, ω = v κ. Gleiche Krümmung, anderer Aktor.

Der eine Knopf, und warum er gegen sich selbst kämpft

Kurze Vorausschau. Der Zielpunkt ist nah, also erzeugt ein kleiner Seitenversatz eine große Krümmung. Der Roboter folgt dem Pfad eng — und reagiert heftig auf jedes Wackeln, auch auf das, das seine Lenkung gerade selbst verursacht hat.

Lange Vorausschau. Der Zielpunkt ist fern, die Krümmung sanft, die Lenkung ruhig — und in der Kurve hat der Zielpunkt die Biegung schon umrundet, der Roboter aber nicht, also schneidet der Roboter die Kurve.

Die ziehen in Gegenrichtungen, und der Algorithmus hat nichts, um sie auszubalancieren. Du wählst eine Zahl.

Die Falle in jeder Lehrbuchherleitung

Schreibe Pure Pursuit auf Papier, mit einem Roboter, der instantan lenkt, und kürzer ist immer besser. Der Fehler fällt monoton mit schrumpfender Vorausschau. Es gibt kein Optimum, und die obige U-Form existiert nicht.

Echte Roboter haben Verzögerung. Das Lenken braucht Zeit bis zum Sollwinkel; die Pose, auf die du reagierst, ist ein paar zehn Millisekunden alt, wenn die Räder antworten. Der Simulator modelliert 0,25 s Lenkzeitkonstante und 0,15 s Fühl- und Rechenverzug — 0,4 s Gesamtverzug, was für einen Hobbyrover unauffällig ist.

Dieser Verzug ist der ganze Grund, warum das Optimum existiert. Eine kurze Vorausschau kommandiert eine Korrektur, die Korrektur kommt spät, der Roboter ist schon am Fehler vorbei, und das nächste Kommando überkorrigiert zur anderen Seite. Ein Regler, der schneller ist als sein eigener Aktor, schwingt. Das ist dieselbe Tatsache, die die PID-D-Verstärkung regiert, und dieselbe hinter dem Bremsweg: Latenz ist kein Detail, sie setzt die Abstimmung.

Wie wählt man sie also?

Die gemessenen Optima, und was sie in übertragbaren Einheiten sind:

Zwei ehrliche Lesarten dieser Tabelle.

Die Vorausschau wächst mit der Geschwindigkeit, aber unterproportional. Der gängige Rat „nimm eine feste Anzahl Sekunden Fahrzeit“ ist nah dran, aber nicht richtig — die Zeit fällt über diesen Bereich von 0,90 s auf 0,47 s.

Sie liegt beim Ein- bis Zweifachen der Strecke, die du während deines eigenen Regelverzugs fährst. Das ist die brauchbare Regel, denn sie ist messbar: Finde deinen Verzug, multipliziere mit deiner Geschwindigkeit und starte dort. Auf diesem Roboter läuft v × Verzug von 0,4 m bis 1,2 m, die Optima liegen bei 0,9 m bis 1,4 m.

In der Praxis liefere sie als kleinen linearen Fahrplan mit Begrenzung aus:

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

L_MIN zählt mehr als K_LOOK. Im Stand will der Fahrplan null Vorausschau, und das ist eine Division durch null in Verkleidung.

Es implementieren

// 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);
}

Drei Dinge in zwölf Zeilen sind mehr wert als der Rest:

• lastS ist Zustand und bewegt sich nur vorwärts. Ohne ihn strandet jede sich selbst kreuzende Pfadform den Regler.
• Begrenze ld. Wenn der Zielpunkt nahekommt, geht die Krümmung gegen unendlich.
• Der Zielpunkt liegt auf dem Pfad, nicht auf dem nächsten Wegpunkt. Auf Wegpunkte zu zielen lässt den Roboter zwischen ihnen zickzack fahren; auf einen Punkt auf dem Segment zu zielen macht die Bewegung ruhig.

Was es tut und was nicht

Es bleibt nicht stehen und dreht sich zum Pfad. Pure Pursuit kennt kein Stillstehen — die Krümmung ist eine Form, und eine Form braucht Vorwärtsbewegung. Starte den Roboter grob ausgerichtet oder fahre erst einen kurzen Öffnungsbogen.

Es hat einen Mindestradius, und der Pfad respektiert ihn vielleicht nicht. Ist eine Kurve schärfer als b / tan(δ_max), kann der Roboter ihr nicht folgen, egal wie du abstimmst. Entweder den Pfad glätten oder für die Kurve langsamer werden — wofür ein Bewegungsprofil da ist.

Es schneidet immer Kurven, konstruktionsbedingt. In der Messfahrt bei 2 m/s fuhr es 0,8 % weniger als der Sollpfad. Ist das inakzeptabel, willst du einen anderen Regler — Stanley regelt auf Querfehler und holt weit aus statt zu schneiden.

Es kann keine schlechte Positionsschätzung fixen. Alles oben nimmt an, der Roboter wisse, wo er ist. Füttere es mit einem 1-Hz-GPS-Fix, und der RMS-Fehler geht von 0,09 m auf 1,25 m — siehe GPS-Wegpunktnavigation.

Wenn es schiefgeht

Fahre beide Regler über denselben Kurs im Pfadfolge-Simulator — ziehe an der Vorausschau, und die ganze Fahrt zeichnet neu. Baue dann den GPS-Wegpunkt-Rover, wo die Positionsschätzung der schwere Teil wird.