GPS-waypointnavigatie voor een buitenrobot

Graden zijn de verkeerde eenheid voor een robot

Een GPS-module geeft je breedtegraad en lengtegraad in graden. Elke regelaar die je bezit wil meters. Omrekenen is de eerste klus, en dat valt mee met wat het internet suggereert.

Over de paar honderd meter die een hobbyrover aflegt, is de aarde zo vlak dat een equirectangulaire benadering exact is tot ver binnen de ruis van de ontvanger zelf:

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

Kies één keer een oorsprong lat₀, lon₀ — het begin van de missie — en elk waypoint wordt een simpel paar (east, north) in meters waarop pure pursuit kan sturen zonder te hoeven weten wat een graad is.

De haversine-formule is nauwkeuriger en je hebt haar niet nodig. Ze telt over honderden kilometers; op 500 m zijn de twee het eens tot op enkele millimeters, drie ordegrootten onder je fout.

Het getal om te onthouden: één graad breedtegraad is 111,3 km, dus vijf decimalen is 1,1 m en zes is 11 cm. Bewaar waypoints met zes. Alles daarboven is ruis opslaan, en een float op een AVR heeft ongeveer zeven significante cijfers — genoeg voor 19.076123 en niet voor meer, en daarom reken je om naar meters ten opzichte van een oorsprong in plaats van rekenkunde te doen op ruwe graden.

Drie dingen die aan een fix fout zijn

Hij is laat. Een typische NEO-6M meldt op 1 Hz. Bij 1,5 m/s legt de robot 1,5 m af tussen twee fixes, en de fix die hij krijgt beschrijft waar hij was toen de oplossing werd berekend, niet waar hij nu is.

Hij is ruisachtig. Consumenten-GPS komt de helft van de tijd binnen ongeveer 2,5 m van de waarheid, in het open, met heldere hemel. Onder bomen, naast een gebouw, of bij slechte satellietgeometrie is het veel erger — en de storing is geen uitval die je kunt detecteren, maar een ogenschijnlijk plausibel verkeerd antwoord.

Hij heeft geen koers. Een GPS-fix is een positie. Hij kan je niet vertellen welke kant de robot op wijst, want een stilstaande robot die naar het noorden wijst en dezelfde robot die naar het zuiden wijst leveren identieke fixes op. Koers over de grond wordt afgeleid uit opeenvolgende posities en is alleen betekenisvol als er werkelijk gereden wordt — onder ongeveer 1 m/s is hij grotendeels ruis.

Wat dat met een padvolger doet

Geef dezelfde pure-pursuit-regelaar dezelfde baan twee keer, een keer met perfecte odometrie en een keer met een 1 Hz-fix op ±1,5 m die tussen de updates wordt vastgehouden:

Let op wat er niet gebeurt: de robot komt nooit op de verkeerde plek. Hij blijft ongeveer op de baan en bereikt het einde. Navigatie op alleen GPS werkt — hij is alleen enorm veel slechter dan hij op het datasheet lijkt, en de kosten komen grotendeels op de actuator terecht.

De drie oplossingen, op volgorde van waarde

1 — Fusieer, volg niet. Integreer wielodometrie en een koersbron op je looptempo, en gebruik elke GPS-fix om die schatting te corrigeren in plaats van hem te vervangen. Zelfs een grove complementaire menging transformeert het resultaat:

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

Dat is een complementair filter dat hetzelfde werk doet als voor de tilt: een signaal dat drijft maar glad is, gecorrigeerd door een signaal dat ruisachtig maar begrensd is.

2 — Haal de koers ergens anders vandaan. Een magnetometer geeft absolute koers in stilstand, en vraagt hard- en softijzerkalibratie plus afstand tot je motoren. Een gyroscoop geeft uitstekende koers op korte termijn en drijft. Samen zijn ze wat elke echte rover gebruikt; de GPS-koers over de grond corrigeert dat paar dan zodra je snel genoeg rijdt om hem te vertrouwen.

3 — Verhoog de frequentie voordat je de nauwkeurigheid verhoogt. Een 10 Hz-module haalt de veroudering weg die het meeste van het heen-en-weer zwaaien veroorzaakt. Het is meestal een betere eerste upgrade dan het najagen van nauwkeurigheid op centimeters, en het is veel goedkoper.

De module uitlezen

GPS-modules spreken NMEA 0183 over een seriële poort op 9600 baud: door komma’s gescheiden zinnen, één per regel. Je wilt er twee.

• $GPRMC — tijd, geldigheid, breedtegraad, lengtegraad, snelheid over de grond, koers over de grond.
• $GPGGA — fixkwaliteit, gebruikte satellieten, en HDOP, de horizontale precisieverslechtering.

Parseer de fix, maar houd de kwaliteit aan de poort. Twee regels die de meeste rampen buiten voorkomen:

• Verwerp een fix met status V (void), of met een fixkwaliteit van 0. Hij bevat nog steeds ogenschijnlijk plausibele getallen.
• Verwerp een HDOP boven ongeveer 2,5. HDOP beschrijft de satellietgeometrie, en een slechte geometrie levert een zelfverzekerd antwoord op dat tientallen meters ernaast zit.

Gebruik een parsebibliotheek in plaats van je eigen te schrijven — maar lees genoeg van de zinnen om te weten wat je weggooit. En nog iets: een koude start duurt 30 seconden tot enkele minuten zonder almanac. Behandel een robot die nog geen fix heeft niet als een robot die op de oorsprong staat. Wacht, met een zichtbare indicatie dat je wacht.

Zet er een geofence omheen voor je het test

Een robot voor buiten die zich misdraagt vertrekt. Voeg vóór de eerste veldtest een harde grens in de software toe: een maximale afstand vanaf het startpunt, en een stop zodra die wordt overschreden.

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

Tien regels, en dat is het verschil tussen een bug en een robot in een gracht. Combineer dat met een missietimeout en een handmatige onderbreker die je kunt bereiken — een bump switch op het bovenste dek werkt en heeft geen radioverbinding nodig.

Wanneer het misgaat

Rij dezelfde baan met beide positiebronnen in de path following simulator, en bouw daarna de GPS waypoint rover — waarin de module zelf en de fusie hierboven het hele project vormen.