Navigazione GPS a punti di passaggio per un robot da esterno

I gradi sono l’unità sbagliata per un robot

Un modulo GPS fornisce latitudine e longitudine in gradi. Ogni controllore che possiedi vuole metri. La conversione è il primo compito ed è più facile di quanto suggerisca Internet.

Sui pochi centinaia di metri percorsi da un rover hobbistico, la Terra è sufficientemente piatta da rendere un’approssimazione equirettangolare accurata ben oltre il rumore del ricevitore stesso:

north (m) = (lat − lat₀) × 111 320
east  (m) = (lon − lon₀) × 111 320 × cos(lat₀)

Scegli una volta un’origine lat₀, lon₀, all’inizio della missione: ogni punto di passaggio diventerà una normale coppia (east, north) in metri che pure pursuit può seguire senza sapere che cos’è un grado.

La formula dell’haversine è più corretta, ma non ti serve. Diventa importante su centinaia di chilometri; a 500 m i due risultati coincidono entro pochi millimetri, tre ordini di grandezza sotto il tuo errore.

Il numero da ricordare: un grado di latitudine è 111.3 km, quindi cinque cifre decimali danno 1.1 m e sei danno 11 cm. Memorizza i punti di passaggio con sei cifre. Oltre questo stai memorizzando rumore e un float su un AVR contiene soltanto circa sette cifre significative: abbastanza per 19.076123, non per nient’altro. Per questo converti in metri rispetto a un’origine anziché fare aritmetica sui gradi grezzi.

Tre problemi di un fix

È in ritardo. Un tipico NEO-6M comunica a 1 Hz. A 1.5 m/s il robot percorre 1.5 m fra un fix e l’altro e il fix ricevuto descrive dove si trovava quando è stata calcolata la soluzione, non dove si trova adesso.

È rumoroso. Un GPS di consumo si trova entro circa 2.5 m dalla verità metà delle volte, all’aperto e con il cielo sereno. Sotto gli alberi, accanto a un edificio o con una geometria satellitare scarsa è molto peggio e il guasto non è una perdita di segnale rilevabile, ma una risposta sbagliata ma plausibile.

Non ha un orientamento. Un fix GPS è una posizione. Non può indicarti verso dove è rivolto il robot, perché un robot fermo rivolto a nord e lo stesso robot rivolto a sud producono fix identici. La direzione rispetto al suolo deriva da posizioni consecutive ed è significativa solo quando il robot si muove davvero: sotto circa 1 m/s è quasi tutto rumore.

Che cosa comporta per un robot seguilinea

Fornisci allo stesso controllore pure pursuit la stessa rotta due volte, una con odometria perfetta e una con un fix a 1 Hz a ±1.5 m mantenuto tra un aggiornamento e l’altro:

Nota che cosa non sta accadendo: il robot non si perde. Resta all’incirca sulla rotta e raggiunge la fine. La navigazione basata solo sul GPS funziona, ma è enormemente peggiore di quanto sembri sulla scheda tecnica e il costo ricade soprattutto sull’attuatore.

Le tre soluzioni, in ordine di valore

1 — Fusiona, non seguire. Integra l’odometria delle ruote e una fonte di orientamento alla frequenza del loop e usa ogni fix GPS per correggere quella stima anziché sostituirla. Anche una semplice miscela complementare grossolana trasforma il risultato:

// Between fixes: dead reckon. This runs at the loop rate.
x += cos(heading) * v * dt;
y += sin(heading) * v * dt;

// On a fix: pull the estimate toward it, gently. alpha near 0.1 trusts odometry
// over the second between fixes, which is exactly the right instinct.
if (newFix && fix.hdop < 2.5) {
  x += ALPHA * (fixX - x);
  y += ALPHA * (fixY - y);
}

È un filtro complementare che svolge lo stesso lavoro che svolge per l’inclinazione: un segnale che deriva ma è regolare, corretto da uno che è rumoroso ma limitato.

2 — Ricava l’orientamento da un’altra fonte. Un magnetometro fornisce un orientamento assoluto da fermo e richiede calibrazione del ferro dolce e duro, oltre a distanza dai motori. Un giroscopio fornisce un ottimo orientamento a breve termine ma deriva. Insieme sono ciò che usa ogni vero rover; la direzione GPS rispetto al suolo corregge poi la coppia quando il robot si muove abbastanza veloce da poterci fidare.

3 — Aumenta la frequenza prima di aumentare la precisione. Un modulo a 10 Hz elimina la obsolescenza responsabile di gran parte dell’ondeggiamento. Di solito è un primo aggiornamento migliore che inseguire una precisione centimetrica ed è molto più economico.

Lettura del modulo

I moduli GPS comunicano in NMEA 0183 su una porta seriale a 9600 baud: frasi separate da virgole, una per riga. Ti servono due di queste.

• $GPRMC — ora, validità, latitudine, longitudine, velocità rispetto al suolo e direzione rispetto al suolo.
• $GPGGA — qualità del fix, satelliti utilizzati e HDOP, la diluizione orizzontale della precisione.

Decodifica il fix, ma valuta la qualità. Due regole evitano la maggior parte dei disastri all’aperto:

• Rifiuta un fix con stato V (void) o qualità del fix pari a 0. Continuerà a contenere numeri dall’aspetto plausibile.
• Rifiuta un HDOP superiore a circa 2.5. L’HDOP descrive la geometria dei satelliti e una geometria errata produce una risposta convincente che può essere sbagliata di decine di metri.

Usa una libreria di parsing anziché scrivere la tua, ma leggi abbastanza delle frasi da sapere cosa stai scartando. Inoltre: un avvio a freddo richiede da 30 secondi a diversi minuti senza almanacco. Non trattare un robot che non ha ancora ottenuto un fix come un robot situato all’origine. Aspetta, mostrando in modo visibile che stai aspettando.

Imposta una geofence prima di provarlo

Un robot da esterno che si comporta male se ne va. Prima del primo test sul campo, aggiungi in software un limite rigido: una distanza massima dal punto di partenza e un arresto se viene superata.

if (hypotf(x, y) > FENCE_RADIUS) { stop(); return; }   // before anything else

Dieci righe, e sono la differenza tra un bug e un robot in un canale. Abbinala a un timeout di missione e a un’interruzione manuale che puoi raggiungere: un interruttore a leva sul piano superiore funziona e non richiede alcun collegamento radio.

Quando va storto

Percorri la stessa rotta con entrambe le fonti di posizione nel simulatore di inseguimento dei percorsi, poi costruisci il rover con navigazione GPS a punti di passaggio, dove il modulo stesso e la fusione descritta sopra costituiscono l’intero progetto.