Geregelte Raddrehzahlregelung mit Encodern

PWM ist eine Spannung, keine Drehzahl

analogWrite(motorPin, 180) verlangt keine Drehzahl. Es verlangt etwa 70 % Batteriespannung – und welche Drehzahl das erzeugt, hängt von Radlast, Getriebereibung, Ladezustand und Bürstentemperatur ab.

Darum fährt ein Roboter mit zwei identischen Motoren und identischem PWM einen Bogen. Sie waren nie identisch – ein Getriebe ist enger, ein Bürstensatz neuer, und bei derselben Spannung laufen sie 3–8 % auseinander. Über drei Meter ist das eine sichtbare Kurve.

An einem Differenzialroboter kann man es mit Trimmkonstante zukleistern. Auf einer Mecanumbasis nicht – denn die inverse Kinematik übergibt den vier Rädern wild verschiedene Sollwerte: Im durchgerechneten Beispiel dort bekommt ein Rad 0,020 m/s und ein anderes 0,580 m/s aus demselben Befehl – und diese Verhältnisse entscheiden, wohin der Roboter fährt. Ein 5 % langsames Rad macht den Roboter nicht etwas langsam; es lässt ihn woandershin fahren.

Die Schleife um die gemessene Drehzahl jedes Rads zu schließen behebt das – und es ist dieselbe Lösung für jeden Roboter mit mehr als zwei Rädern oder jedem Bedürfnis, irgendwo anzukommen.

Die wirklich vorhandene Drehzahl messen

Von Quadraturencoderzählwerten ausgehen. Die Umrechnung ist feste Geometrie:

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

Für einen 12-CPR-Motorencoder hinter 30:1-Getriebe auf 97-mm-Rädern: 12 × 30 × 4 = 1440 Zählwerte/Umdr, und 2π × 0,0485 / 1440 = 212 µm pro Zählwert.

Dann der Teil, den alle falsch machen:

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

Bei 100 Hz und 0,05 m/s ist das ein Zählwert pro Messwert. Dein Drehzahlsignal ist 0, 212, 0, 212 µm – eine Rechteckwelle mit ±100 % Welligkeit, und ein D-Anteil verstärkt sie zu einem summenden Motor. Drei Auswege – und meist will man den ersten:

1. Langsamer abtasten, als man regelt. Drehzahl über 20 ms (50 Hz) rechnen und die Schleife mit 50 Hz fahren. Mehr Zählwerte pro Fenster, weniger Quantisierung, keine Filterverzögerung.
2. Die Zeit zwischen Flanken messen statt Flanken in einer Zeit. Hervorragend bei niedriger Drehzahl, umständlich bei hoher – und braucht Interruptzeitstempel.
3. Die Drehzahl tieffiltern. Am billigsten – und kostet Phasenreserve: das, wovon man am wenigsten hat.

// 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 und die noInterrupts()-Klammer sind nicht optional: Eine 32-Bit-Lesung sind mehrere Befehle auf AVR – und ein dazwischen landender ISR übergibt eine Zahl, die nie wahr war.

Feedforward zuerst, PID danach

Der Instinkt greift zu PID. Erst die leichten 90 % tun.

Die Beharrungsdrehzahl eines DC-Motors ist fast linear in angelegter Spannung – kennt man also die gewünschte Drehzahl, kann man den Großteil des PWM berechnen:

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

• kV – die Steigung. Wie viel PWM eine m/s kauft.
• kS – der Achsenabschnitt. Das PWM, das gerade Haftreibungsüberwindung braucht, bevor sich das Rad überhaupt bewegt.

Beide in zehn Minuten messen:

1. Den Roboter aufbocken, damit die Räder frei drehen.
2. PWM langsam erhöhen, bis das Rad gerade zu drehen beginnt. Das ist kS, typisch 25–45 von 255.
3. PWM auf 100, 150, 200, 250 schreiten und bei jedem die Beharrungsdrehzahl aufzeichnen.
4. Eine Gerade fitten. Ihre Steigung ist kV – und sie sollte zurück auf grob dein kS extrapolieren.

Nun muss die Schleife nur den Fehler zwischen dieser Schätzung und Realität korrigieren – der klein ist: Die Verstärkungen können also klein sein, das Ganze also stabil und leise:

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

Ohne Feedforward muss der Integrator den gesamten Befehl bei jeder Zieländerung aus null aufbauen. Das ist langsam, schwingt über – und ist der Grund, warum so viele handabgestimmte Motorschleifen mit riesigem kI und einem aufbrausenden Roboter enden.

Der Regler

P und I. Nicht 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;
  }
};

Warum kein Differenzial: Der D-Anteil differenziert das Drehzahlsignal – das bereits eine Differenz von Encoderzählwerten ist. Man differenziert also doppelt – verstärkt also Quantisierungsrauschen doppelt –, und an einem Rad gibt es kaum etwas, das es dämpfen müsste. Eine Drehzahlschleife mit PI und gutem Feedforward ist die Standardantwort – und D hinzuzufügen ist eine der gängigeren Arten, eine arbeitende Schleife schlechter zu machen. (Eine Positionsschleife ist anders – siehe kaskadierte Regelung.)

Anti-Windup ist der Teil, den man nicht auslassen darf. Den Roboter gegen eine Wand halten bei angelegtem Drehzahlbefehl, und ein naiver Integrator rampt in wenigen Sekunden auf Vollskala. Loslassen und der Roboter schießt los. Bedingte Integration, ein if, behebt es.

Abstimmen, in der richtigen Reihenfolge

1. Feedforward allein. kP = kI = 0. 0,3 m/s befehlen und messen, was herauskommt. Innerhalb etwa 10 % heißt: kV stimmt. Das beheben, bevor man sonst etwas anfasst.
2. kP hoch, bis es summt, dann ein Drittel zurück. Man horcht auf ein Jaulen, das mit der Verstärkung erscheint und ohne sie verschwindet.
3. kI, bis der bleibende Fehler weg ist. Ein Rad mit korrektem Feedforward braucht sehr wenig. Bei kP/10 pro Sekunde anfangen und hochkriechen.
4. Unter Last prüfen, nicht aufgebockt. Der ganze Sinn ist das Rad, das um mehr gebeten wird.
5. Alle vier gemeinsam testen, eine Diagonale fahrend. Das ist der Befehl mit der größten Spreizung der Radziele – und dort zeigt sich ein unterabgestimmtes Rad zuerst.

Der PID-Simulator baut Intuition für Schritte 2 und 3 schneller auf als ein Roboter, und die Abstimmanleitung hat das allgemeine Verfahren.

Abtastrate

Die Schleifenrate muss deutlich über dem liegen, was sie regelt. Die Drehzahl eines kleinen DC-Getriebemotors pendelt sich mit Zeitkonstante grob 50–150 ms ein; zehn Messwerte in dieser Zeitkonstante sind die übliche Regel, also 50–200 Hz.

Auf festem Zeitplan fahren, nicht „so schnell wie loop() geht“ – denn dt steckt direkt in Drehzahlschätzung wie 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
}

Der vorzeichenbehaftete Vergleich ist dieselbe millis()-Überlauffalle in micros()-Form, wo sie nach 71 Minuten statt 49 Tagen beißt.

Was es bringt, vermessen

Derselbe Roboter, derselbe Befehl, derselbe Boden:

Diese letzte Zeile ist die unterschätzte. Eine geschlossene Schleife weiß, wenn sie versagt hat: Ein Rad, dessen Ausgang auf 255 gepinnt ist und weiter hinter dem Ziel liegt, meldet etwas Nützliches – und man kann das auf eine Telemetriezeile legen, den Befehl verweigern oder den ganzen Roboter verlangsamen, um die Verhältnisse ehrlich zu halten.

Fehlersuche

Weiter

Die Räder liefern nun die Drehzahlen, um die man sie bittet. Das Ganze dorthin zeigen lassen, wohin der Fahrer meint, mit feldorientierter Fahrt – und ihm weiche Ziele statt Stufen füttern mit Bewegungsprofilen. Dann beobachten, was eine Drehzahlschleife retten kann und was nicht, im Mecanum-Antriebssimulator – eine perfekt gehaltene Raddrehzahl heißt weiter nichts, wenn der Boden loslässt.