Differenzial-Odometrie aus Radencodern

Ein Differenzialroboter hat zwei einzeln angetriebene Räder im Spurabstand. Bewegen sich beide Räder gleich weit, fährt der Roboter geradeaus. Fährt eines weiter, folgt das Chassis einem Bogen. Odometrie integriert diese kleinen Bewegungen über Zeit zur Poseschätzung: Horizontalposition x, Vertikalposition y und Kurs θ.

Die Schätzung ist lokal und kumulativ. Sie startet von einer gewählten Pose und kann sich aus Radencodern allein nicht korrigieren. Kleine Raddurchmesser-, Spurweiten-, Timing- und Schlupffehler summieren sich – das Ziel ist also eine nützliche Kurzzeitschätzung mit vermessenen Grenzen, keine perfekte Ground Truth.

Eine Koordinatenkonvention festlegen

Einen Frame wählen, bevor man Gleichungen schreibt. Eine gängige zweidimensionale Konvention:

• x zeigt von der Startpose nach vorne.
• y zeigt nach links.
• Positives θ dreht gegen den Uhrzeigersinn.
• Linke und rechte Radstrecke sind positiv, wenn der Roboter vorwärts fährt.

Winkel intern in Radiant speichern. Nur zur Anzeige in Grad umrechnen. Kurs normieren, wenn begrenzte Werte beim Debuggen helfen – Sinus und Kosinus arbeiten auch mit unbegrenzten Winkeln.

dL und dR seien linke und rechte Radstrecke seit dem letzten Update, b der wirksame Abstand zwischen den Radaufstandslinien.

Aus Zählwerten Meter machen

Alles oben setzt dL und dR in Metern voraus. Encoder liefern keine Meter; sie liefern Zählwerte. Eine Umrechnung sitzt dazwischen – und falsch skaliert sie jede Zahl, die der Rest dieser Seite rechnet.

countsPerWheelTurn = PPR x edgesDecoded x gearRatio
metresPerCount     = (pi x wheelDiameter) / countsPerWheelTurn

edgesDecoded ist nur 4, wenn man beide Flanken beider Kanäle dekodiert – die volle Quadratur. Nur die steigende Flanke eines Kanals zu dekodieren ergibt 1 – und wirft außerdem die Richtungsinformation weg, sodass der Roboter Rückwärts nicht von Vorwärts unterscheiden kann.

Die unterste Zeile überrascht Leute. Eine ans Rad gekabelte Schlitzscheibe ist billig – aber 10 mm Quantisierung heißen: Eine 200-mm-Fahrt ist auf etwa 5 % bekannt, und keine Kalibrierung holt Information zurück, die der Sensor nie erfasste. Das Quadraturencoder-Tutorial deckt die Dekodierungsseite vollständig ab.

Auflösung ist eine Untergrenze der Präzision, kein Genauigkeitsversprechen. Ein 73-µm-Zählwert ist bedeutungslos, wenn der Raddurchmesser 2 % falsch ist.

Vorwärts- und Drehbewegung berechnen

Die Achsenmitte bewegt sich um die mittlere Radstrecke:

dCenter = (dR + dL) / 2

Der Kurs ändert sich nach ihrer Differenz:

dTheta = (dR - dL) / b

Gleiche Strecken ergeben dTheta = 0. Gleiche, gegensätzliche Strecken ergeben dCenter = 0 – der Roboter dreht also auf der Stelle um die Achsenmitte.

Für kleine Updates ist Translation am Mittenkurs genau und vermeidet einen Geradeaus-Sonderfall:

headingMid = theta + dTheta / 2
x = x + dCenter * cos(headingMid)
y = y + dCenter * sin(headingMid)
theta = theta + dTheta

Diese Mittenintegration ist einfach genug für einen eingebetteten Controller und benimmt sich gut, wenn die Schleife häufig läuft. Eine exakte Bogenintegration nützt bei großen Schritten – repariert aber keine falschen Radmessungen.

Ein wiederholbares Update implementieren

Rohe Encodertotale behalten und ihre Differenzen in Meter umrechnen. Hardwarezähler nicht jede Schleife zurücksetzen; Schnappschüsse erleichtern Überlauf und Diagnose.

struct Pose { float x, y, theta; };

void updateOdometry(Pose& pose, float dLeft, float dRight, float trackWidth) {
  const float dCenter = 0.5f * (dLeft + dRight);
  const float dTheta = (dRight - dLeft) / trackWidth;
  const float headingMid = pose.theta + 0.5f * dTheta;

  pose.x += dCenter * cosf(headingMid);
  pose.y += dCenter * sinf(headingMid);
  pose.theta += dTheta;
}

Nach atomarem Lesen beider Encoder aufrufen. Die Links- und Rechts-Schnappschüsse sollten fast denselben Augenblick darstellen; eine lange Lücke dazwischen erzeugt scheinbares Lenken bei schneller Bewegung.

Braucht ein anderes Subsystem Geschwindigkeit, die Inkremente durch die gemessene vergangene Zeit teilen. Die Posenintegration selbst nutzt Strecke – Schleifenzeitjitter ändert die geschätzte gefahrene Strecke also nicht direkt.

Beide Encoder lesen, ohne Zählwerte zu verlieren

Die Funktion oben ist reine Arithmetik. Sie korrekt zu füttern ist der Ort, wo eingebettete Odometrie wirklich bricht – und drei Details tragen fast das ganze Risiko.

Beide Zähler im selben Augenblick per Schnappschuss. Die Interrupthandler zählen weiter, während die Schleife liest. Links lesen, etwas arbeiten, dann rechts lesen schreibt Bewegung dazwischen nur einem Rad zu – was die Gleichungen treu als Drehung deuten.

Die Hardwarezähler nie zurücksetzen. Freilaufende Totale plus gemerkter Vorwert liefern das Delta – und lassen den Rohzählwert für Diagnose verfügbar.

Das Delta in vorzeichenloser Arithmetik rechnen. Vorzeichenüberlauf ist in C++ undefiniertes Verhalten; der Cast-Tanz unten ist definiert, kostet nichts und bleibt über den Umbruch korrekt.

volatile int32_t leftCount = 0, rightCount = 0;   // written only by the ISRs

void isrLeft()  { leftCount  += digitalRead(L_CHAN_B) ? +1 : -1; }
void isrRight() { rightCount += digitalRead(R_CHAN_B) ? +1 : -1; }

constexpr float METRES_PER_COUNT = (3.14159265f * 0.065f) / 2800.0f;   // 72.9 um
constexpr float TRACK_WIDTH      = 0.152f;                             // calibrated, not measured

void odometryTick(Pose& pose) {
  int32_t nowLeft, nowRight;
  noInterrupts();                      // one instant, both wheels
  nowLeft  = leftCount;
  nowRight = rightCount;
  interrupts();

  static int32_t prevLeft = 0, prevRight = 0;
  // Unsigned subtraction wraps in a defined way; the cast back recovers the
  // signed delta even across the 2^32 boundary.
  const int32_t dLeftCounts  = (int32_t)((uint32_t)nowLeft  - (uint32_t)prevLeft);
  const int32_t dRightCounts = (int32_t)((uint32_t)nowRight - (uint32_t)prevRight);
  prevLeft  = nowLeft;
  prevRight = nowRight;

  updateOdometry(pose,
                 dLeftCounts  * METRES_PER_COUNT,
                 dRightCounts * METRES_PER_COUNT,
                 TRACK_WIDTH);
}

Bei 73 µm pro Zählwert wickelt ein vorzeichenbehafteter 32-Bit-Zähler nach etwa 157 km – an einem Tischroboter also theoretisch. Zwei Casts kostet es, trotzdem richtig zu liegen – und dasselbe Muster ist an einem 16-Bit-Zähler Pflicht, der nach 2,4 m wickelt.

Eine Drehung durcharbeiten

Angenommen, das linke Rad fährt 0,20 m, das rechte 0,30 m, Spurweite 0,25 m:

• dCenter = 0,25 m
• dTheta = (0,30 − 0,20) / 0,25 = 0,4 rad
• Aus Kurs null ist Mittenkurs 0,2 rad
• dx ≈ 0,25 cos(0,2) = 0,245 m
• dy ≈ 0,25 sin(0,2) = 0,050 m

Der Roboter endet leicht links seiner ursprünglichen Vorwärtsachse und um etwa 22,9° gedreht. Eine Handrechnung wie diese fängt Vorzeichen- und Einheitenfehler, bevor der Code Hardware erreicht.

Die zwei dominanten Parameter kalibrieren

Der wirksame Raddurchmesser steuert den Streckenmaßstab. Eine lange vermessene Gerade fahren, gemeldete gegen wirkliche Strecke vergleichen und den Durchmessermaßstab mit wirklich / gemeldet multiplizieren. Jede Seite einzeln kalibrieren, wenn der Roboter trotz gleicher Befehle kurvt.

Die wirksame Spurweite steuert den Drehmaßstab. Mehrere volle Drehungen auf der Stelle befehlen, tatsächliche Kursänderung mit externer Referenz messen und Spurweite anpassen. Mehrere Drehungen vergrößern den Fehler und erleichtern Messen. Reifenwalken heißt: Der wirksame Wert kann vom linealvermessenem Radabstand abweichen.

Beide Parameter konfigurierbar halten. Untergrund, Reifenkompression, Zuladung und Verschleiß können sie ändern; Hardwaremodelle brauchen Stellschrauben.

Wissen, wie schnell der Fehler wächst

Kalibrierung beseitigt Fehler nicht, sie verkleinert den Koeffizienten. Die Größe des Rests zu kennen sagt, wie lange man der Schätzung trauen kann, bevor etwas Externes korrigieren muss.

Die Quadratzahl stammt aus Integration eines 1-m-Quadrats mit vier 90-Grad-Drehungen auf der Stelle und sonst nichts Falschem außer der Spurweite. Alles andere ist exakt – und der Roboter endet trotzdem 8,7 cm vom Start entfernt.

Die letzte Zeile gegen die ersten zwei lesen. Die Integrationsmathematik ist nie dein dominanter Fehler. Sie liegt drei Größenordnungen unter den Kalibriertermen und vier unter Schlupf – weshalb Schleifenrate zu erhöhen, um Präzision zu jagen, die falsche Optimierung ist, und weshalb die Radschlupfseite für deine Endpose mehr zählt als alles auf dieser Seite.

Außerdem verhalten sich systematische und zufällige Fehler völlig verschieden. Ein systematisches Walken summiert sich linear und lohnt Herauszukalibrieren. Ein zufälliges wächst mit der Wurzel der Drehzahl – 50 Drehungen kosten also etwa 6 Grad statt 45: lästig, aber durch kein Parameterdrehen behebbar.

Eine Aktualisierungsrate wählen

Der Bogen zwischen zwei Messwerten wird per gerader Sehne am Mittenkurs angenähert – der Fehler hängt also davon ab, wie stark sich der Kurs pro Schritt ändert. Für einen Schritt von dTheta Radiant ist die Sehne grob um dTheta² / 24 zu kurz:

50 Hz wählen und aufhören, darüber nachzudenken. Die gesparte Aufmerksamkeit in Synchronisieren beider Encoderlesungen stecken – das zählt bei jeder Rate mehr als die Rate selbst: 1 ms Versatz zwischen Links- und Rechts-Schnappschuss erfindet bei 0,5 m/s Raddifferenz aus dem Nichts 0,5 mm Raddifferenz, jede einzelne Schleife.

Ein Gyro hinzufügen, ohne die Frames zu verwirren

Ein Gyro liefert oft eine bessere kurzfristige Kursänderung als die Differenz zwischen Radencodern. Man kann Gyrokurs für theta nutzen und weiter mittlere Radstrecke für Translation. Translation am Mittel aus altem und neuem Gyrokurs anwenden.

Das macht Position nicht absolut. Gyros haben Bias und Radstrecke rutscht weiter. Ein globaler Sensor – Kameralandmarken, Lidar-Lokalisierung oder eine andere externe Referenz – muss die aufgelaufene Pose korrigieren.

In ROS-Terminologie gehört eine fortlaufende lokale Schätzung natürlicherweise in den odom-Frame. Ein Lokalisierungssystem darf map → odom aktualisieren, ohne den lokal glatten odom → base_link-Transform springen zu lassen.

Drift messen statt verstecken

Drei wiederholbare Tests fahren:

1. Vorwärts und rückwärts zum Start fahren.
2. Mehrere Drehungen drehen und zum Ausgangskurs zurückkehren.
3. Ein großes Quadrat fahren und Endpose mit Startmarkierung vergleichen.

Encoderdeltas, gerechnete Pose, Akkuspannung und Untergrund loggen. Jeden Test in beide Richtungen wiederholen. Konsistenter Maßstabsfehler deutet Kalibrierung an; richtungsabhängiger Fehler deutet Spiel oder Asymmetrie an; untergrundabhängiger Fehler deutet Schlupf an.

Fehlersuche

Die Lenkfolgen im Linienfolger-Simulator ausprobieren und lokale Bewegungsschätzungen mit den Kartierungskonzepten in SLAM-Grundlagen vergleichen. Für die Aufgabe, die am direktesten von dieser Schätzung abhängt – parallele Reihen über einen ganzen Boden fahren –, siehe Abdeckungsplanung und den Abdeckungssimulator.