Simulatore di path following: Pure Pursuit e Stanley

Controlli

Controllore alterna tra pure pursuit e Stanley. Anticipo è l’unico parametro di pure pursuit ed è disabilitato per Stanley, che non ne usa uno: è proprio questo il punto.

Velocità e anticipo interagiscono, ed è la parte con cui vale la pena giocare: imposta 1 m/s e trova l’anticipo migliore, poi imposta 3 m/s e trovalo di nuovo.

Posizione da sostituisce l’odometria perfetta con una correzione a 1 Hz con rumore su scala metrica, mantenuta tra un aggiornamento e l’altro. L’errore viene sempre valutato rispetto alla verità, quindi la penalità è reale e non nascosta.

Trascina un punto di passaggio, oppure premi 1–8 per selezionarlo e spostarlo con le frecce. L’intera corsa viene ricalcolata mentre lo trascini.

Teoria

Entrambi i controllori rispondono alla stessa domanda — dato dove mi trovo e dov’è il percorso, quale angolo di sterzo? — ma lo fanno in modi completamente diversi.

Pure pursuit è geometria. Trova il punto sul percorso a una distanza di anticipo L davanti e percorri l’arco circolare che lo attraversa:

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

dove y è lo scostamento laterale del punto obiettivo nel sistema del robot e b è il passo della ruota. Un solo parametro, ed è una distanza anziché un guadagno: quindi si trasferisce tra robot diversi.

Stanley è controllo. Prendi come riferimento l’asse anteriore e aggiungi due termini:

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

errore di orientamento più un arcotangente dell’errore trasversale al percorso. Dividere per la velocità permette a un solo guadagno di funzionare su un intervallo di velocità. k è un vero guadagno di controllo e non si trasferisce.

Il laboratorio aggiunge ciò che le derivazioni dei manuali omettono: 0.4 s di ritardo totale, composto da una risposta di sterzo del primo ordine di 0.25 s più 0.15 s di ritardo di rilevamento e calcolo. Senza di esso, pure pursuit migliora monotonamente quando l’anticipo si riduce e la domanda sulla regolazione scompare.

Algoritmo

A ogni passo, per entrambi i controllori:

1. Proietta sul percorso, cercando una finestra attorno alla lunghezza d’arco raggiunta l’ultima volta anziché tutto il percorso. Una ricerca globale abbina il segmento più vicino, quindi un tracciato che torna indietro teletrasporta l’avanzamento del robot verso un angolo superato un minuto prima.
2. Risolvi un pareggio a un vertice scegliendo il segmento uscente. Dopo un angolo, entrambi i segmenti adiacenti si proiettano nello stesso punto alla stessa distanza; mantenere quello entrante consegna al controllore un errore di orientamento e uno scostamento laterale entrambi quasi nulli mentre esce dritto dal percorso, in silenzio.
3. Calcola l’angolo di sterzo secondo la regola del controllore e limitane il valore al limite meccanico.
4. Applica il ritardo, poi integra il modello della bicicletta.
5. Valuta rispetto alla verità, mai rispetto alla stima.

Il passo 2 non è un’ottimizzazione. È la differenza tra Stanley funzionante e Stanley che esce con sicurezza dal tracciato esattamente nel 78% delle esecuzioni.

Codice sorgente

Pure pursuit, completo, nella forma che assume su un robot reale:

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
}

Le tre righe che contano sono il limite inferiore della schedulazione, la monotonia di lastS e il limite di ld. Tutto il resto è la formula.

Ipotesi e differenze hardware

Il laboratorio dà al robot una mappa perfetta del percorso, un passo della ruota perfettamente noto, nessuno slittamento delle ruote e un ritardo modellato in modo pulito come sistema del primo ordine. Un robot reale non ha nessuna di queste proprietà.

Lo slittamento delle ruote rende sbagliata la stima della posizione in un modo che nessun controllore può rilevare. Il gioco di inversione del riduttore aggiunge corsa morta a ogni inversione di sterzo, si comporta come ritardo aggiuntivo e spinge in alto l’anticipo ottimale. Inoltre la trazione differenziale non ha alcun angolo di sterzo: ha una velocità di rotazione comandata, quindi ω = v κ sostituisce atan e il passo della ruota conta solo per capire quanto strettamente può curvare.

Ciò che sopravvive a tutto questo è la forma: un ottimo interno, che si sposta verso destra con la velocità, e un errore in quel punto che cresce comunque con la velocità.

Schema elettrico

Il controllore è la parte economica. Ciò che deve fornire il resto del robot è la parte costosa.

Posizione. Encoder in quadratura su entrambe le ruote e odometria a trazione differenziale che li integra, alla frequenza del ciclo. In ambienti interni è sufficiente per qualche decina di metri.

Orientamento. Un giroscopio per il breve termine, un magnetometro come riferimento assoluto, o entrambi. L’errore di orientamento è il termine che entrambi i controllori pesano maggiormente, ed è quello che l’odometria perde per primo.

Una correzione assoluta, all’aperto. Un modulo GPS fuso nella stima anziché seguito direttamente: la differenza è 0.09 m di errore RMS rispetto a 1.25.

Per la teoria completa, vedi pure pursuit e errore trasversale al percorso; per costruirlo, il rover GPS con punti di passaggio parte all’interno con gli encoder e passa all’esterno solo quando il sistema di inseguimento funziona.