Odometrie met differentiële aandrijving uit wielencoders

Een robot met differentiële aandrijving heeft twee onafhankelijk aangedreven wielen met een vaste spoorbreedte ertussen. Beweegen beide wielen dezelfde afstand, dan rijdt de robot recht. Beweegt er eentje verder, dan volgt het chassis een boog. Odometrie integreert die kleine bewegingen over de tijd om de pose te schatten: horizontale positie x, verticale positie y en richting θ.

De schatting is lokaal en cumulatief. Ze begint bij een gekozen pose en kan zichzelf op basis van alleen wielencoders niet corrigeren. Kleine fouten in wieldiameter, spoorbreedte, timing en slip tellen zich op, dus het doel is een bruikbare schatting op korte termijn met gemeten grenzen — geen perfecte werkelijkheid.

Eén coördinatenconventie vastleggen

Kies een referentiesysteem voordat je vergelijkingen schrijft. Een gangbare tweedimensionale conventie is:

• x wijst vooruit vanaf de startpose.
• y wijst naar links.
• positieve θ draait tegen de klok in.
• de afstand van linker- en rechterwiel is positief als de robot vooruit rijdt.

Bewaar hoeken intern in radialen. Converteer alleen naar graden voor weergave. Normaliseer de richting als begrensde waarden het debuggen helpen, maar sinus en cosinus werken ook met onbegrensde hoeken.

Laat dL en dR de linker- en rechterwielafstand zijn sinds de vorige update, en b de effectieve afstand tussen de raaklijnen van de wielen.

Omzetten van tellingen naar meters

Alles hierboven veronderstelt dat je dL en dR al in meters hebt. Encoders leveren geen meters; ze leveren tellingen. Eén omrekening zit ertussen, en een fout daarin schaalt elk getal dat de rest van deze pagina berekent.

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

edgesDecoded is alleen 4 als je beide flanken van beide kanalen decodeert — de volledige kwadratuur. Het decoderen van alleen de oplopende flank van één kanaal geeft 1, en gooit bovendien de richtingsinformatie weg, zodat de robot achteruitrijden niet van vooruitrijden kan onderscheiden.

De onderste regel is de verrassing. Een gaafrond schijfje dat met een draadje aan het wiel is vastgezet is goedkoop, maar 10 mm kwantisatie betekent dat een verplaatsing van 200 mm tot ongeveer 5% bekend is, en geen hoeveelheid kalibratie haalt informatie terug die de sensor nooit heeft opgeslagen. De tutorial over kwadratuurencoders behandelt de decoderkant volledig.

Resolutie is een ondergrens voor de precisie, geen belofte van nauwkeurigheid. Een telling van 73 um betekent niets als de wieldiameter 2% fout is.

Voorwaartse en hoekbeweging berekenen

Het midden van de as beweegt met het gemiddelde van de wielafstanden:

dCenter = (dR + dL) / 2

De richting verandert met hun verschil:

dTheta = (dR - dL) / b

Gelijke afstanden maken dTheta = 0. Gelijke en tegengestelde afstanden maken dCenter = 0, dus de robot draait ter plaatse om het midden van de as.

Bij kleine updates is de verplaatsing toepassen op de gemiddelde richting nauwkeurig en vermijd je een apart geval voor de rechte lijn:

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

Deze integratie via het midden is eenvoudig genoeg voor een embedded regelaar en gedraagt zich goed als de lus vaak draait. Een exacte boogintegratie is nuttig bij grote stappen, maar repareert geen verkeerde wielmetingen.

Een herhaalbare update implementeren

Bewaar de ruwe tellertotalen en reken hun verschillen om naar meters. Reset de hardwaretellers niet in elke lus; momentopnames maken doorlopen en diagnostiek makkelijker.

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

Roep dit aan nadat je beide encoders atomair hebt gelezen. De momentopnames van links en rechts moeten vrijwel hetzelfde moment voorstellen; een lang gat ertussen creëert schijnbare besturing tijdens snelle beweging.

Als een ander subsysteem snelheid nodig heeft, deel dan de stappen door de gemeten verstreken tijd. De integratie van de pose zelf gebruikt afstand, dus jitter in de lustijd verandert de schatting van de afgelegde afstand niet rechtstreeks.

Beide encoders lezen zonder tellingen te verliezen

De functie hierboven is puur rekenwerk. Hem correct voeden is waar embedded odometrie in de praktijk stukgaat, en drie details dragen vrijwel al het risico.

Maak van beide tellers op hetzelfde moment een momentopname. De interrupt-handlers tellen door terwijl je lus ze leest. Eerst links lezen, wat werk doen, dan rechts lezen, schrijft de beweging die tussentijds plaatsvond toe aan maar één wiel — en de formules lezen dat getrouw als een bocht.

Reset de hardwaretellers nooit. Vrijlopende totalen plus een onthouden vorige waarde geven je het verschil, en laten het ruwe telwerk beschikbaar voor diagnostiek.

Bereken het verschil in unsigned rekenkunde. Overloop van een ondertekend getal is ongedefinieerd gedrag in C++; de cast hieronder is wél gedefinieerd, kost niets, en blijft correct over de grens heen.

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

Bij 73 um per telling loopt een 32-bits teller met teken na ongeveer 157 km om, dus op een bureaurobot is dat doorlopen puur theoretisch. Het kost twee casts om het toch goed te doen, en hetzelfde patroon is verplicht op een 16-bits teller, die na 2,4 m doorloopt.

Eén bocht doorrekenen

Stel dat het linkerwiel 0,20 m beweegt, het rechter 0,30 m, en de spoorbreedte 0,25 m is:

• dCenter = 0.25 m
• dTheta = (0.30 - 0.20) / 0.25 = 0.4 rad
• uitgaande van richting nul is de gemiddelde richting 0.2 rad
• dx ≈ 0.25 cos(0.2) = 0.245 m
• dy ≈ 0.25 sin(0.2) = 0.050 m

De robot eindigt net links van zijn oorspronkelijke vooras en ongeveer 22,9° gedraaid. Zo’n handberekening nakijken vangt teken- en eenheidsfouten voordat de code de hardware bereikt.

De twee dominante parameters kalibreren

De effectieve wieldiameter bepaalt de afstandsschaal. Rij een lange, gemeten rechte lijn, vergelijk de gemelde met de werkelijke afstand, en vermenigvuldig de diameterschaling met actual / reported. Kalibreer elke zijde apart als de robot bocht ondanks gelijke commando’s.

De effectieve spoorbreedte bepaalt de draaischaal. Commandeer meerdere volledige draaiingen ter plaatse, meet de werkelijke richtingsverandering met een externe referentie, en pas de spoorbreedte aan. Meerdere draaiingen vergroten de fout en maken meten makkelijker. Bandenschuif betekent dat de effectieve waarde kan afwijken van de met de liniaal gemeten wielafstand.

Houd beide parameters instelbaar. Ondergrond, bandcompressie, lading en slijtage kunnen ze veranderen; hardwaredellen hebben kalibratieknoppen nodig.

Weten hoe snel de fout groeit

Kalibratie verwijdert de fout niet, hij verkleint de coëfficiënt. Weten hoe groot het overblijfsel is, vertelt je hoe lang je de schatting kunt vertrouwen voordat iets externs hem moet corrigeren.

Het vierkantsgetal komt van het integreren van een vierkant van 1 m met vier draaiingen van 90 deg ter plaatse, en verder niets fout dan de spoorbreedte. Al het andere is exact, en toch eindigt de robot 8,7 cm van zijn startpunt.

Lees de laatste regel naast de eerste twee. De integratierekenkunde is nooit je dominante fout. Ze ligt drie ordegrootten onder de kalibratietermen en vier onder slip, en daarom is het verhogen van de looptempo om de precisie na te jagen de verkeerde optimalisatie — en daarom telt de pagina over wielslip zwaarder voor je eindpose dan alles op deze pagina.

Merk ook op dat systematische en willekeurige fouten zich volstrekt anders gedragen. Systematische schuif telt lineair op en valt de moeite uit te kalibreren. Willekeurige schuif groeit met de wortel van het aantal draaiingen, dus 50 draaiingen kost ongeveer 6 deg in plaats van 45 — irritant, maar met geen enkele parametertuning te repareren.

Kies een updatefrequentie

De boog tussen twee metingen wordt benaderd door een rechte koorde op de gemiddelde richting, dus de fout hangt af van hoeveel de richting per stap verandert. Voor een stap van dTheta radalen is de koorde ongeveer dTheta^2 / 24 te kort:

Kies 50 Hz en denk er niet meer over na. Besteed de bespaarde aandacht aan het synchroniseren van de twee encoderleesacties, wat hoe dan ook belangrijker is dan de frequentie zelf: een verschil van 1 ms tussen de momentopname van links en rechts terwijl de wielen 0,5 m/s verschillen, verzint elke lus 0,5 mm aan wielverschil uit het niets.

Een gyroscoop toevoegen zonder de frames door elkaar te halen

Een gyroscoop geeft vaak een betere korte-termijnaandering van de richting dan het verschil tussen de wielencoders. Je kunt de gyrorichting gebruiken voor theta en toch de gemiddelde wielafstand voor de verplaatsing. Pas de verplaatsing toe op het gemiddelde van de oude en de nieuwe gyrorichting.

Daarmee wordt de positie niet absoluut. Gyroscopen hebben bias en wielafstand blijft slippen. Een globale sensor — herkenningspunten uit een camera, lidar-lokalisatie, of een andere externe referentie — is nodig om de opgebouwde pose te corrigeren.

In ROS-terminologie hoort een continue lokale schatting vanzelfsprekend in het odom-frame. Een lokalisatiesysteem mag map → odom bijwerken zonder de lokaal gladde transform odom → base_link te laten verspringen.

Meet de drift in plaats van hem te verbergen

Voer drie herhaalbare tests uit:

1. Rij vooruit en terug naar het begin.
2. Draai meerdere rondjes en keer terug naar de oorspronkelijke richting.
3. Rij een groot vierkant en vergelijk de eindpose met de startmarkering.

Log de verschillen van de encoders, de berekende pose, de accuspanning en de ondergrond. Herhaal elke test in beide richtingen. Een consistente schaalfout wijst op kalibratie; een fout die met de richting verandert wijst op verdraaispeling of asymmetrie; een fout die met de ondergrond verandert wijst op slip.

Storingzoeken

Bekijk de gevolgen voor het sturen in de Line Follower Simulator en vergelijk lokale bewegingsschattingen met de kaartbegrippen in SLAM fundamentals. Voor de taak die het meest rechtstreeks van deze schatting afhangt — evenwijdige rijen over een hele vloer rijden — zie padplanning voor volledige dekking en de coverage simulator.