Gesloten-luswielsnelheidsregeling met encoders

PWM is een spanning, geen snelheid

analogWrite(motorPin, 180) vraagt niet om een snelheid. Het vraagt om ongeveer 70% van de batterijspanning, en welke snelheid dat oplevert hangt af van de belasting op het wiel, de wrijving in die overbrenging, de ladingstoestand en de temperatuur van de borstels.

Daarom rijdt een robot met twee identieke motoren en identieke PWM in een boog. Ze waren nooit identiek — de ene overbrenging zit strakker, de ene borstelset is nieuwer, en bij dezelfde spanning lopen ze 3–8% uit elkaar. Over drie meter is dat een zichtbare kromme.

Op een robot met differentiële aandrijving kun je dat verhelpen met een trimconstante. Op een mecanumbasis kun je dat niet, want de inverse kinematica geeft de vier wielen drastisch verschillende doelen — in het rekenvoorbeeld daar krijgt het ene wiel 0.020 m/s en het andere 0.580 m/s uit hetzelfde commando — en die verhoudingen bepalen in welke richting de robot rijdt. Een wiel dat 5% te langzaam is maakt de robot niet een beetje langzaam; hij maakt hem ergens anders.

De lus sluiten rond elke gemeten wielsnelheid lost dat op, en het is dezelfde oplossing voor elke robot met meer dan twee wielen of met enige behoefte om ergens aan te komen.

Meet de snelheid die je werkelijk hebt

Begin bij de kwadratuurencodertellingen. De omrekening is vaste geometrie:

counts per wheel revolution = CPR_motor × gear ratio × 4     (×4 for quadrature edges)
metres per count            = 2πr / counts per revolution

Voor een motorencoder van 12 CPR achter een reductie van 30:1 op wielen van 97 mm: 12 × 30 × 4 = 1440 tellingen/omw, en 2π × 0.0485 / 1440 = 212 µm per telling.

En dan het deel dat iedereen fout doet:

// DON'T: counts since last loop, divided by loop time.
float speed = (count - lastCount) * METRES_PER_COUNT / dt;

Bij 100 Hz en 0.05 m/s is dat één telling per monster. Je snelheidssignaal is 0, 212, 0, 212 µm — een blokgolf met ±100% rimpel, en een D-term versterkt dat tot een zoemende motor. Drie manieren eruit, en doorgaans wil je de eerste:

1. Bemonster langzamer dan je regelt. Bereken de snelheid over 20 ms (50 Hz) en voer de lus op 50 Hz uit. Meer tellingen per venster, minder kwantisatie en geen filtervertraging.
2. Meet de tijd tussen flanken in plaats van de flanken in een tijd. Uitstekend bij lage snelheid, onhandig bij hoge snelheid, en het vereist onderbrekingstijdstempels.
3. Laagdoorlaat de snelheid. Goedkoopst, en het kost je fasemarge — het ding waarvan je er het minste hebt.

// A 50 Hz velocity estimate, with the counter read atomically because an
// encoder ISR is writing it.
noInterrupts();
long count = encoderCount;
interrupts();

float dt = (now - lastTime) * 1e-6f;             // micros(), so no ms rounding
float v = (count - lastCount) * METRES_PER_COUNT / dt;
lastCount = count; lastTime = now;

volatile long encoderCount en de noInterrupts()-haakjes zijn niet optioneel: een 32-bits lezing is op een AVR meerdere instructies, en een onderbreking die er middenin terechtkomt levert je een getal dat nooit waar was.

Eerst voorafsturing, dan PID

Het instinct is naar PID te grijpen. Doe eerst de makkelijke 90%.

De stationaire snelheid van een DC-motor is bij benadering lineair in de aangelegde spanning, dus als je de gewenste snelheid kent kun je het meeste van de PWM berekenen:

pwm ≈ kS·sign(v) + kV·v

• kV — de helling. Hoeveel PWM één m/s oplevert.
• kS — het snijpunt met de verticale as. De PWM die nodig is om alleen maar de statische wrijving te doorbreken, voordat het wiel ook maar beweegt.

Meet beide in tien minuten:

1. Zet de robot op blokjes zodat de wielen vrij kunnen draaien.
2. Verhoog de PWM langzaam tot het wiel juist gaat draaien. Dat is kS, typisch 25–45 van de 255.
3. Stap de PWM naar 100, 150, 200, 250 en noteer bij elke stand de stationaire snelheid.
4. Fit er een rechte lijn door. De helling is kV, en die zou terug naar ongeveer je kS moeten extrapoleren.

De lus hoeft nu alleen nog de fout tussen die schatting en de werkelijkheid te corrigeren — en die is klein, dus de versterkingen kunnen klein zijn, dus het geheel is stabiel en stil:

float feedforward = kS * sign(target) + kV * target;
float output = feedforward + kP * error + kI * integral;

Zonder voorafsturing moet de integrator het hele commando elke keer dat het doel verandert vanaf nul opbouwen. Dat is traag, het schiet door, en het is de reden dat zo veel met de hand afgestelde motorlussen eindigen met een enorme kI en een robot die schokt.

De regelaar

P en I. Geen D.

struct WheelLoop {
  float kS, kV, kP, kI;
  float integral = 0, target = 0;

  int update(float measured, float dt) {
    float error = target - measured;
    float ff = kS * (target > 0 ? 1 : target < 0 ? -1 : 0) + kV * target;
    float unclamped = ff + kP * error + kI * (integral + error * dt);
    int   output    = (int)constrain(unclamped, -255, 255);

    // Conditional integration: only accumulate if the output is NOT saturated,
    // or if the error would wind the integrator back toward the middle.
    // Otherwise a stalled wheel builds a command it cannot use and then keeps
    // driving for a second after you let go of the stick.
    if (fabsf(unclamped) < 255.0f || (error * unclamped) < 0) integral += error * dt;

    return output;
  }
};

Waarom geen differentiaal: de D-term differentieert het snelheidssignaal, en dat is al een verschil van encodertellingen. Je differentieert twee keer, wat tweemaal kwantisatieruis versterkt, en op een wiel is er sowieso vrijwel niets om te dempen. Een snelheidslus met PI en goede voorafsturing is het standaardantwoord, en D toevoegen is een van de manieren waarop een werkende lus wordt verergerd. (Een positielus is iets anders — zie cascaded control.)

Anti-windup is het deel dat je niet kunt overslaan. Houd de robot tegen een muur terwijl je een snelheidscommando geeft, en een naïeve integrator loopt in een paar seconden op naar volle schaal. Laat los en de robot schiet weg. Voorwaardelijke integratie, één if, lost het op.

Afstellen, in de juiste volgorde

1. Alleen voorafsturing. kP = kI = 0. Geef 0.3 m/s op en meet wat je krijgt. Binnen ongeveer 10% betekent dat kV klopt. Repareer dat voordat je iets anders aanraakt.
2. kP omhoog tot het zoemt, dan een derde terug. Je luistert naar een gehuil dat met de versterking verschijnt en zonder verdwijnt.
3. kI tot de blijvende fout verdwenen is. Een wiel met juiste voorafsturing heeft er heel weinig nodig. Begin op kP/10 per seconde en kruip omhoog.
4. Controleer het onder belasting, niet op blokjes. Het hele punt is het wiel waarom wordt gevraagd.
5. Test alle vier samen, door diagonaal te rijden. Dat is het commando met de grootste spreiding in wielerdoelen, en daar laat een te laag afgesteld wiel zich als eerste zien.

De PID-simulator is een snellere manier om het inzicht voor stappen 2 en 3 op te bouwen dan een robot, en de afstelgids bevat de algemene procedure.

Bemonsteringsfrequentie

De frequentie van de lus moet ruim boven het ding liggen dat hij regelt. De snelheid van een kleine DC-reductiemotor bezinkt met een tijdconstante van ongeveer 50–150 ms; tien monsters binnen die tijdconstante is de gebruikelijke vuistregel, dus 50–200 Hz.

Voer het uit op een vast schema, niet “zo snel loop() gaat”, want dt komt rechtstreeks voor in zowel de snelheidschatting als de integrator:

const unsigned long PERIOD_US = 10000;   // 100 Hz
static unsigned long next = 0;

void loop() {
  unsigned long now = micros();
  if ((long)(now - next) < 0) return;    // signed compare survives rollover
  next += PERIOD_US;
  // ... read encoders, run four loops, write four PWMs
}

Die vergelijking met teken is dezelfde valkuil van de millis()-overflow in zijn micros()-vorm, waar hij na 71 minuten toeslaat in plaats van 49 dagen.

Wat het je oplevert, gemeten

Dezelfde robot, hetzelfde commando, dezelfde vloer:

Die laatste rij wordt onderschat. Een gesloten lus weet wanneer hij het heeft laten afweten: een wiel waarvan de uitvoer op 255 vastzit en nog steeds onder het doel zit meldt iets bruikbaars, en je kunt dat op een telemetrielijn zetten, het commando weigeren, of de hele robot vertragen om de verhoudingen in orde te houden.

Storingzoeklijst

Volgende

De wielen leveren nu de snelheden die hun wordt gevraagd. Richt het geheel op waar de bestuurder mee voorloopt met veldgeoriënteerde aandrijving, en voer het soepele doelen in plaats van stappen met bewegingsprofielen. Kijk daarna in de mecanum-aandrijfsimulator waar een snelheidslus je wel en niet van kan redden — een perfect gehouden wielsnelheid betekent nog steeds niets als de vloer het laat afweten.