Kaskadierte PID-Regelung für Balance-Roboter

Einführung

Du hast einen vertrauenswürdigen Neigewinkel. Nun musst du den Roboter darauf stehen halten – und irgendwann dafür sorgen, dass er an einer Stelle bleibt, statt technisch aufrecht durch den Raum zu driften.

Das sind zwei verschiedene Aufgaben, und der Fehler, den fast jeder zuerst macht, ist, sie mit einer Schleife lösen zu wollen. Diese Anleitung baut sie als zwei verschachtelte – so werden echte Balance-Roboter geregelt.

Warum eine Schleife nicht reicht

Was du willst, ist ein Roboter, der bleibt, wo du ihn hinstellst. Was du kommandieren kannst, ist Motordrehmoment. Dazwischen sitzt eine unbequeme Tatsache: Die einzige Art, die Position des Roboters zu ändern, ist, erst seine Neigung zu ändern.

Versuche, die Position direkt zu regeln – fahre die Motoren proportional zum Positionsfehler –, schießen die Räder unterm Schwerpunkt hervor. Der Körper rotiert entgegengesetzt, und der Roboter fällt sofort um, nachdem er sich auf dem Weg nach unten in die falsche Richtung bewegt hat. Das ist kein Abstimmproblem; das Vorzeichen der Anfangsantwort ist echt verkehrt, eine Eigenschaft namens Nichtminimalphasigkeit.

Der Ausweg: die zwei Größen mit verschiedenen Geschwindigkeiten regeln, in Kaskade.

Die Struktur

   position error → [ OUTER PID ] → tilt target
                                        ↓
   tilt error     → [ INNER PID ] → motor command → robot
                                        ↑
                                   measured tilt

Die innere Schleife hat eine Aufgabe: Halten, welchen Neigewinkel man ihr sagt. Sie läuft schnell – 200 Hz oder mehr –, weil sie gegen die Schwerkraft kämpft, und die Schwerkraft wartet nicht.

Die äußere Schleife hat die andere Aufgabe: den Roboter an eine Position bringen. Sie kann die Motoren nicht anfassen, arbeitet also, indem sie die innere Schleife um eine kleine Neigung bittet. Hinter dem Ziel fordert sie Vorwärtsneigung an; die innere Schleife treibt die Räder, um diese Neigung zu halten; eine Vorwärtsneigung zu halten heißt kontinuierlich vorwärts beschleunigen. Sie läuft vielleicht zehnmal langsamer als die innere Schleife.

Diese Ratentrennung ist das ganze Design. Ist die äußere Schleife irgendwo nahe der Geschwindigkeit der inneren, bekämpfen sie sich – die äußere ändert das Ziel schneller, als die innere folgen kann, und der Roboter schwingt sich in den Boden.

Die innere Schleife

float angleError = angle - tiltTarget;      // measured MINUS target
angleIntegral   = constrain(angleIntegral + angleError * dt, -3, 3);

float command = Kp * angleError
              + Ki * angleIntegral
              + Kd * gyroRate;

command = constrain(command, -255, 255);

Drei Details darin zählen mehr als die Verstärkungen.

Der Fehler ist gemessen minus Ziel, umgekehrt zum Lehrbuch Sollwert − Messwert. Um einen vorwärts fallenden Roboter zu fangen, fährst du die Räder vorwärts, unter die fallende Masse – so wie du deine Hand unter einen Besen bewegst. Eine positive Neigung muss also ein positives Motorkommando erzeugen. Vertausche das, treibt der Regler den Roboter beim Einschalten augenblicklich in den Boden – ein markanter, heftiger Ausfall, der fast immer dieses Vorzeichen ist.

Der D-Anteil nutzt direkt die Gyrorate, keine Differenz aufeinanderfolgender Winkel. Das Gyroskop misst Winkelrate als natives Ausgangssignal. Ein verrauschtes Winkelsignal zu differenzieren verstärkt genau das Rauschen, das du am wenigsten verstärkt willst, und es ist der Grund, warum viele erste Versuche eine verdächtig niedrige D-Verstärkung brauchen, um stabil zu bleiben.

Das Integral ist begrenzt. Während eines Sturzes sättigen die Motoren und der Fehler wächst weiter, ein unbegrenztes Integral läuft also zu einer Riesenzahl auf, die dann auf dem Rückweg die Erholung bekämpft. Begrenze es und höre ganz auf zu akkumulieren, solange der Ausgang gesättigt ist.

Die äußere Schleife

if (++slowTick >= 10) {                     // ten times slower
  slowTick = 0;
  float positionError = targetPosition - wheelPosition;
  tiltTarget = constrain(KpPos * positionError - KdPos * wheelVelocity,
                         -0.12, 0.12);      // radians, about ±7°
}

wheelPosition kommt aus der Summe deiner Encoder-Zählwerte. Ohne Encoder kannst du das Motorkommando als groben Ersatz integrieren, aber die Schleife wird spürbar schlechter.

Die Begrenzung ist nicht optional. Eine äußere Schleife, die 30° fordern darf, übergibt der inneren ein Ziel, das sie physisch nicht halten kann – der Roboter verpflichtet sich zu einer Neigung, aus der er sich nicht erholen kann, und fällt bei perfektem Befehlsgehorsam. Wenige Grad reichen locker; 7° Neigung beschleunigen einen Roboter zügig.

Beachte, dass der Geschwindigkeitsterm subtrahiert wird. Er dämpft die Annäherung, damit der Roboter nicht übers Ziel hinausschießt und zurückkommen muss.

Abstimmen in der Reihenfolge, die funktioniert

Stimme erst die innere Schleife allein ab. Setze KpPos und KdPos auf null, damit die äußere Schleife träge ist und tiltTarget auf null bleibt. Der Roboter balanciert auf der Stelle und driftet langsam – das ist vorerst erwartet und in Ordnung.

1. Ki und Kd auf null. Kp erhöhen, bis der Roboter scharf auf einen Schubs reagiert. Zu niedrig, sackt er kampflos zu Boden; zu hoch, brummt er.
2. Kd erhöhen, bis die Schwingung ausdämpft. Das ist der Anteil, der aus einem Roboter, der sich selbst umvibriert, einen macht, der sich setzt. Im Balance-Simulator kannst du D an einem laufenden Roboter auf null setzen und die Schwingungen zählen, bevor er umfällt.
3. Ki zuletzt erhöhen, und sanft. Es entfernt die langsame Neigung aus IMU-Montagetrimm. Zu viel, entwickelt der Roboter ein langsames, wachsendes Pendeln, während das Integral über- und zurückschwingt.

Schalte erst dann die äußere Schleife zu und starte KpPos bei etwa einem Zehntel dessen, was vernünftig wirkt. Seine Wirkung ist indirekt – du achtest darauf, dass der Roboter aufhört zu driften, nicht auf knackige Reaktion.

Ausfallarten und was sie bedeuten

Motoren jenseits der Rettung abschalten

if (fabs(angle) > FALL_ANGLE) {             // about 35 degrees
  driveMotors(0, 0);
  angleIntegral = 0;
  return;
}

Jenseits von rund 35° übersteigt die zum Zurückkommen unter den Schwerpunkt nötige Horizontalbeschleunigung das, was die Räder liefern können, bevor der Körper den Boden erreicht. Der Regler weiß das nicht und stemmt sich mit vollem Drehmoment bis nach unten, was Getrieben zusetzt und den Ausfall lauter macht als nötig.

Abschalten setzt auch das Integral zurück, den Roboter wieder aufzuheben gibt dir also eine saubere Schleife statt einer mit aufgewundener Korrektur vom Sturz.

Wohin als Nächstes

Die Verstärkungen wechselwirken hier stärker als in einem Einzelschleifen-PID, weil die Strecke instabil ist – es gibt kein „ungefähr richtig“, das nur schlecht leistet. Er steht oder nicht.

Das macht einen Simulator ungewöhnlich nützlich: Du kannst D an einem gerade balancierenden Roboter auf null setzen, den exakten Ausfall beobachten und rückgängig machen, ohne etwas zu reparieren. Probiere die Presets im Balance-Simulator, dann baue das Echte im Balance-Roboter-Projekt.