Cascaderegeling met PID voor een zelfbalancerende robot

Inleiding

Je hebt een betrouwbare kanthoek. Nu moet je de robot rechtop houden — en hem daarna, uiteindelijk, op één plek laten blijven in plaats van technisch gezien rechtop door de kamer te drijven.

Dat zijn twee verschillende taken, en de fout die vrijwel iedereen eerst maakt is proberen ze met één lus te doen. Deze tutorial bouwt ze als twee geneste lussen, en zo worden echte balancerende roboten geregeld.

Waarom één lus niet kan werken

Wat je wilt is een robot die blijft waar je hem neerzet. Wat je kunt aansturen is motorkoppel. Daartussen zit een ongemakkelijk feit: de enige manier om de positie van de robot te veranderen is eerst zijn kanthoek te veranderen.

Probeer de positie rechtstreeks te regelen — drijf de motoren evenredig met de positiefout — en de wielen schieten onder het zwaartepunt uit. Het lichaam draait de andere kant op en de robot valt meteen om, terwijl hij op weg naar beneden de verkeerde kant op beweegt. Dit is geen afstelprobleem; het teken van de initiële respons is werkelijk omgekeerd, een eigenschap die niet-minimale fase heet.

De uitweg is de twee grootheden met verschillende snelheden te regelen, in een cascade.

De structuur

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

De binnenlus doet één ding: vasthouden op de kanthoek die hem wordt opgedragen. Hij draait snel — 200 Hz of meer — omdat hij tegen de zwaartekracht vecht, en de zwaartekracht wacht niet.

De buitenlus doet het andere werk: de robot op een positie brengen. Hij kan de motoren niet aanraken en werkt daarom door de binnenlus om een kleine kanteling te vragen. Achter de plek waar hij wil zijn vraagt hij om een voorwaartse kanteling; de binnenlus drijft de wielen om die kanteling te handhaven; een voorwaartse kanteling handhaven betekent continu vooruit accelereren. Hij draait misschien tien keer zo langzaam als de binnenlus.

Die snelheidsscheiding is het hele ontwerp. Zit de buitenlus ook maar in de buurt van de snelheid van de binnenlus, dan vechten ze — de buitenlus verandert de setpoint sneller dan de binnenlus hem kan volgen, en de robot slingert zichzelf op de grond.

De binnenlus

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

Drie details daarin zijn belangrijker dan de versterkingen.

De fout is gemeten min doel, het omgekeerde van de standaard setpoint − meting. Om een voorover vallende robot op te vangen stuur je de wielen vooruit, onder de vallende massa, net zoals je je hand onder een bezempaard schuift. Een positieve kanteling moet dus een positief motorcommando opleveren. Draai je dit om, dan rijdt de regelaar de robot het moment dat je hem aanzet de vloer in — een herkenbare, heftige fout die vrijwel altijd dit teken is.

De differentiële term gebruikt de gyrosnelheid rechtstreeks, niet het verschil van opeenvolgende hoeken. De gyroscoop meet de hoeksnelheid als eigen uitvoer. Differentiëren van een ruisachtig hoeksignaal versterkt precies de ruis die je het minst versterkt wilt hebben, en dat is de reden dat veel eerste pogingen een verdacht lage D-versterking nodig hebben om stabiel te blijven.

De integraal is begrensd. Tijdens een val satureert de motor en blijft de fout groeien, dus een onbegrensde integraal loopt op tot een enorm getal dat vervolgens tijdens het herstel tegen de regelaar vecht. Begrens hem en stop helemaal met accumuleren zolang de uitgang gesatureerd is.

De buitenlus

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 komt uit het sommeren van je encoder-tellingen. Heb je geen encoders, dan kun je het motorcommando integreren als grove benadering, maar de lus wordt merkbaar slechter.

De begrenzing is niet optioneel. Een buitenlus die 30° mag eisen geeft de binnenlus een doel dat hij fysiek niet kan vasthouden — de robot committeert aan een kanteling waaruit hij zich niet kan herstellen en valt terwijl hij de opdracht perfect uitvoert. Een paar graden is ruim voldoende; een kanteling van 7° laat een robot flink accelereren.

Merk op dat de snelheidsterm wordt afgetrokken. Hij dempt de nadering zodat de robot niet voorbij zijn doel zeilt en terug moet komen.

Afstellen, in de volgorde die werkt

Stel eerst alleen de binnenlus af. Zet KpPos en KdPos op nul zodat de buitenlus niets doet en tiltTarget op nul blijft. De robot balanceert op zijn plaats en drijft langzaam — dat is te verwachten en voorlopig prima.

1. Ki en Kd op nul. Verhoog Kp tot de robot scherp op een duwtje reageert. Te laag en hij zakt naar de vloer zonder echt te vechten; te hoog en hij zoemt.
2. Verhoog Kd tot het oscilleren uitdempt. Dit is de term die een robot die zichzelf trillend omver slaat verandert in een robot die tot rust komt. In de balancesimulator kun je bij een draaiende robot D op nul zetten en de slingerbewegingen tellen voordat hij omgaat.
3. Verhoog Ki als laatste, en voorzichtig. Hij haalt de langzame kanteling weg die door de aftrimming van de IMU-montering ontstaat. Te veel en de robot ontwikkelt een trage, groeiende wiebeling terwijl de integraal doorschiet en weer terugveert.

Schakel pas daarna de buitenlus in en begin KpPos rond een tiende van wat redelijk lijkt. Het effect is indirect — je kijkt erop dat de robot stopt met drijven, niet op een scherp reageren.

Foutmodi en wat ze betekenen

Motoren afsnijden voorbij het herstel

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

Voorbij ongeveer 35° is de horizontale versnelling die nodig is om weer onder het zwaartepunt te komen groter dan wat de wielen kunnen leveren voordat het lichaam de vloer bereikt. De regelaar weet dat niet en zal tot de grond toe op volle koppel blijven zwoegen, wat zwaar is voor de overbrenging en de fout luider maakt dan nodig.

Afsnijden reset ook de integraal, dus de robot weer oppakken geeft je een schone lus in plaats van een die nog een opgewonden correctie uit de val meedraagt.

Waar je heen gaat

De versterkingen werken hier sterker op elkaar in dan in een PID met één lus, omdat het proces instabiel is — er is geen “ongeveer goed” dat alleen maar slecht presteert. Hij staat op, of hij doet het niet.

Daarmee is een simulator ongewoon nuttig: je kunt bij een robot die op dit moment balanceert D op nul zetten, de exacte fout gadeslaan en die ongedaan maken zonder iets te repareren. Probeer de presets in de balancesimulator en bouw daarna het echte werk in het project zelfbalancerende robot.