GPS-Wegpunktnavigation für einen Außenroboter

Grad sind die falsche Einheit für einen Roboter

Ein GPS-Modul liefert Breiten- und Längengrade in Grad. Jeder eigene Regler will Meter. Umrechnen ist der erste Job – und leichter, als das Internet suggeriert.

Über die wenigen hundert Meter, die ein Hobbyrover abdeckt, ist die Erde flach genug, dass eine plattkartenförmige Näherung exakt bis deutlich ins Empfängerrauschen hinein reicht:

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

Einen Ursprung lat₀, lon₀ einmal wählen – den Missionsstart –, und jeder Wegpunkt wird ein schlichtes (Ost, Nord)-Paar in Metern, zu dem Pure Pursuit lenken kann, ohne zu wissen, was ein Grad ist.

Die Haversine-Formel ist korrekter und man braucht sie nicht. Sie zählt über hunderte Kilometer; bei 500 m stimmen beide auf wenige Millimeter überein – drei Größenordnungen unter deinem Fehler.

Die Zahl zum Merken: Ein Breitengrad sind 111,3 km – fünf Nachkommastellen sind also 1,1 m und sechs 11 cm. Wegpunkte sechsstellig speichern. Alles darüber speichert Rauschen – und ein float auf AVR trägt nur etwa sieben signifikante Stellen: genug für 19.076123 und nicht mehr: weshalb man in Meter relativ zum Ursprung umrechnet statt auf Rohgraden zu rechnen.

Drei Dinge, die am Fix falsch sind

Er ist spät. Ein typisches NEO-6M meldet mit 1 Hz. Bei 1,5 m/s fährt der Roboter 1,5 m zwischen Fixes – und der Fix, den er bekommt, beschreibt, wo er war, als die Lösung gerechnet wurde, nicht wo er jetzt ist.

Er ist verrauscht. Consumer-GPS landet im Freien bei freiem Himmel etwa halb der Zeit innerhalb 2,5 m der Wahrheit. Unter Bäumen, neben einem Gebäude oder bei schlechter Satellitengeometrie weit schlechter – und der Fehler ist kein erkennbarer Aussetzer, sondern eine plausibel falsche Antwort.

Er hat keinen Kurs. Ein GPS-Fix ist eine Position. Er kann nicht sagen, wohin der Roboter zeigt – denn ein stehender Roboter nach Norden und derselbe nach Süden erzeugen identische Fixes. Fahrt-über-Grund leitet sich aus aufeinanderfolgenden Positionen ab und heißt nur bei wirklicher Fahrt etwas – unter etwa 1 m/s ist es meist Rauschen.

Was das mit einem Pfadfolger tut

Denselben Pure-Pursuit-Regler zweimal denselben Kurs geben lassen – einmal mit perfekter Odometrie, einmal mit 1-Hz-Fix bei ±1,5 m, zwischen Updates gehalten:

Beachte, was nicht geschieht: Der Roboter verirrt sich nie. Er bleibt grob auf dem Kurs und erreicht das Ende. Nur-GPS-Navigation funktioniert – sie ist nur enorm schlechter, als das Datenblatt aussieht, und die Kosten landen meist auf dem Aktor.

Die drei Lösungen, nach Wert geordnet

1 – Fusionieren, nicht folgen. Radodometrie und eine Kursquelle mit Schleifenrate integrieren und jeden GPS-Fix nutzen, um diese Schätzung zu korrigieren statt zu ersetzen. Selbst eine grobe Komplementärmischung verwandelt das Ergebnis:

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

Das ist ein Komplementärfilter, das denselben Job tut wie für Neigung: ein Signal, das driftet, aber glatt ist, korrigiert von einem, das verrauscht, aber begrenzt ist.

2 – Den Kurs woandersher holen. Ein Magnetometer liefert absoluten Kurs im Stand – und braucht Hart- und Weicheisenkalibrierung plus Abstand zu deinen Motoren. Ein Gyro liefert hervorragenden Kurzzeitkurs und driftet. Zusammen sind sie das, was jeder echte Rover nutzt; GPS-Fahrt-über-Grund korrigiert das Paar dann, wenn es schnell genug fährt, um ihm zu trauen.

3 – Erst die Rate heben, dann die Genauigkeit. Ein 10-Hz-Modul behebt die Veraltung, die das meiste Weben verursacht. Es ist meist das bessere erste Upgrade, als Zentimetergenauigkeit zu jagen – und weit billiger.

Das Modul auslesen

GPS-Module sprechen NMEA 0183 über eine serielle Schnittstelle mit 9600 Baud: kommagetrennte Sätze, einer pro Zeile. Zwei davon will man.

• $GPRMC – Zeit, Gültigkeit, Breite, Länge, Geschwindigkeit über Grund, Kurs über Grund.
• $GPGGA – Fixqualität, genutzte Satelliten und HDOP, die horizontale Präzisionsverwässerung.

Den Fix parsen, aber auf Qualität filtern. Zwei Regeln, die die meisten Außendesaster verhindern:

• Einen Fix mit Status V (void) oder Fixqualität 0 verwerfen. Er enthält weiter plausibel aussehende Zahlen.
• HDOP über etwa 2,5 verwerfen. HDOP beschreibt Satellitengeometrie – und schlechte Geometrie erzeugt eine selbstbewusste Antwort, die Dutzende Meter danebenliegt.

Eine Parsingbibliothek statt selbstgeschriebenem verwenden – aber genug von den Sätzen lesen, um zu wissen, was man wegwirft. Außerdem: Ein Kaltstart dauert 30 Sekunden bis mehrere Minuten ohne Almanach. Einen Roboter, der noch keinen Fix hat, nicht als Roboter am Ursprung behandeln. Warten, mit sichtbarer Anzeige, dass man wartet.

Erst einzäunen, dann testen

Ein Außenroboter, der sich danebenbenimmt, geht. Vor dem ersten Feldtest eine harte Grenze in Software hinzufügen: eine Maximalentfernung vom Startpunkt, und Stopp bei Überschreitung.

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

Zehn Zeilen – und der Unterschied zwischen Fehler und Roboter im Kanal. Mit Missions-Timeout und manuellem Notausschalter paaren, den man erreicht – ein Bumper auf dem Oberdeck funktioniert und braucht keine Funkverbindung.

Wenn es schiefgeht

Denselben Kurs mit beiden Positionsquellen im Pfadfolgesimulator fahren, dann den GPS-Wegpunktrover bauen – wo das Modul selbst und die Fusion oben das ganze Projekt sind.