Pure Pursuit: seguire un percorso con un solo parametro

Il gradino che nessuno insegna

Un pianificatore fornisce un percorso. A* fornisce un elenco di celle della griglia; un rilievo fornisce un elenco di punti di passaggio; una missione fornisce una linea sulla mappa. Nessuno di questi dice alle ruote che cosa fare.

In quello scarto vive pure pursuit, racchiuso in un paragrafo di geometria:

Trova il punto del percorso a una distanza fissa davanti a te. Percorri l’arco circolare che passa per quel punto. Ripeti.

Non c’è un segnale d’errore, né un termine integrale, né una tabella dei guadagni. C’è un solo numero, la distanza di anticipo, e decide tutto.

La geometria

Metti il punto obiettivo nel sistema di riferimento del robot: x in avanti, y verso sinistra, alla distanza L_d. Un arco circolare dalla posizione corrente del robot passando per quel punto ha curvatura

κ = 2y / L_d²

Vale la pena leggerla due volte, perché tutto discende da qui. Lo scostamento laterale y è al numeratore, quindi l’arco curva tanto più quanto più il bersaglio si discosta dall’asse. L’anticipo compare al quadrato al denominatore, quindi raddoppiarlo riduce la curvatura a un quarto.

Espressa come raggio e come angolo di sterzo per un veicolo con interasse b:

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

Per una trazione differenziale non esiste un angolo di sterzo: comandi direttamente una velocità angolare, ω = v κ. Stessa curvatura, attuatore diverso.

L’unico parametro e il motivo per cui si contraddice

Anticipo breve. Il punto obiettivo è vicino, quindi un piccolo scostamento laterale produce una curvatura elevata. Il robot segue strettamente il percorso e reagisce violentemente a ogni oscillazione, comprese quelle appena provocate dal suo stesso sterzo.

Anticipo lungo. Il punto obiettivo è lontano, la curvatura è lieve e lo sterzo è fluido; ma in un angolo il punto obiettivo ha già aggirato la curva mentre il robot no, quindi il robot taglia l’angolo.

Queste due esigenze tirano in direzioni opposte e nell’algoritmo non c’è nulla che le equilibri. Scegli un numero.

La trappola in ogni derivazione dei libri di testo

Scrivi pure pursuit su carta, con un robot che sterza all’istante, e più breve è sempre meglio. L’errore diminuisce monotonicamente all’accorciare l’anticipo. Non esiste un optimum e la forma a U precedente non esiste.

I robot reali hanno ritardo. Ci vuole tempo perché lo sterzo raggiunga l’angolo comandato; quando le ruote rispondono, la posa su cui agisci è già vecchia di poche decine di millisecondi. Il simulatore modella una costante di tempo di 0.25 s per lo sterzo e 0.15 s di ritardo di sensazione e calcolo, cioè 0.4 s di ritardo totale, un valore ordinario per un rover hobbistico.

Quel ritardo è l’intero motivo per cui esiste un optimum. Un anticipo breve comanda una correzione, la correzione arriva in ritardo, il robot ha già superato l’errore e il comando successivo sovracorrega nella direzione opposta. Un controllore più veloce del proprio attuatore oscilla. È lo stesso fatto che determina il guadagno derivativo PID e quello alla base della distanza di frenatura: la latenza non è un dettaglio, è ciò che determina la regolazione.

Come scegliere il valore?

Gli optimum misurati, espressi in unità trasferibili:

Due letture oneste di questa tabella.

L’anticipo cresce con la velocità, ma non in proporzione. Il consiglio comune «impostalo a un numero fisso di secondi di percorrenza» è vicino ma non corretto: il tempo scende da 0.90 s a 0.47 s in questo intervallo.

Varia una o due volte la distanza percorsa durante il tuo stesso ritardo di controllo. Questa è la regola utile, perché è misurabile: determina il ritardo, moltiplicalo per la velocità e parti da lì. Su questo robot v × lag va da 0.4 m a 1.2 m, mentre gli optimum vanno da 0.9 m a 1.4 m.

In pratica, implementalo come un piccolo schema lineare con limiti:

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

L_MIN conta più di K_LOOK. Da fermo, lo schema richiede un anticipo nullo, che è una divisione per zero travestita.

Implementazione

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

Tre elementi in dodici righe valgono più di tutto il resto:

• lastS è uno stato e avanza soltanto in avanti. Senza di esso, qualsiasi percorso che si attraversa lascerebbe il controllore bloccato.
• Limita ld. Quando il punto obiettivo si avvicina, la curvatura tende all’infinito.
• Il punto obiettivo è sul percorso, non è il punto di passaggio più vicino. Puntare ai punti di passaggio fa oscillare il robot fra essi; puntare a un punto del segmento rende fluido il movimento.

Che cosa fa e che cosa non fa

Non resta fermo e gira per dare al percorso la faccia. Pure Pursuit non ha il concetto di fermarsi: la curvatura è una forma e una forma richiede movimento in avanti. Avvia il robot allineato in modo approssimativo, oppure percorri prima un breve arco iniziale.

Ha un raggio di sterzata minimo e il percorso potrebbe non rispettarlo. Se un angolo è più acuto di b / tan(δ_max), il robot non può seguirlo, per quanto tu regoli. Addolcisci il percorso oppure rallenta per l’angolo: è a questo che serve un profilo di movimento.

Taglia sempre gli angoli, per progetto. Nell’esecuzione misurata a 2 m/s ha percorso lo 0.8% in meno del percorso comandato. Se non è accettabile, ti serve un altro controllore: Stanley sterza in base all’errore laterale e allarga la curva invece di tagliarla.

Non può correggere una stima di posizione errata. Tutto quanto precede presuppone che il robot sappia dove si trova. Forniscigli una stima GPS a 1 Hz e l’errore quadratico medio passa da 0.09 m a 1.25 m: vedi Navigazione GPS per punti di passaggio.

Quando qualcosa va storto

Esegui entrambi i controllori sullo stesso tracciato nel simulatore di inseguimento del percorso: trascina l’anticipo e l’intera esecuzione viene ricalcolata. Poi costruisci il rover con navigazione GPS per punti di passaggio, dove la stima di posizione diventa la parte difficile.