Simulatie: zelfbalancerende robot met PID op een slinger

Bediening

Starten, Pauzeren en Resetten laten de lus draaien. Duwen levert een impuls zoals een vinger dat zou doen, en Lus aan/uit schakelt beide regelaars uit zodat je het kale proces kunt zien vallen.

De meest directe bediening is de robot zelf: sleep de romp naar een willekeurige hoek en laat los. In welke stand je hem achterlaat, daarin neemt de regelaar het over — er is geen herinschakelstap, en de versterkingschuifregelaars werken onmiddellijk op de draaiende simulatie. Zet de differentiaalversterking op nul terwijl hij aan het balanceren is en hij begint in diezelfde seconde te slingeren.

Het menu Afstellen laadt vier voorinstellingen: één die werkt, en drie die op de drie manieren falen waarop echte balancers falen.

Focus het canvas en gebruik spatie, S, L, R en F voor dezelfde bediening vanaf het toetsenbord.

Theorie

Een robot op twee wielen overeind houden betekent een omgekeerde slinger overeind houden. Het zwaartepunt zit boven de as, dus verticaal is een evenwicht in dezelfde zin als een potlood dat op zijn punt balanceert: wiskundig reëel, fysiek hopeloos zonder constante correctie. Kantel hem met een willekeurige hoek en de zwaartekracht levert een koppel dat hem verder kantelt. Hoe verder hij gaat, hoe sneller het gaat.

De robot heeft precies één manier om dat koppel te bestrijden — hij kan de as verplaatsen. De wielen vooruit versnellen duwt de basis terug onder het zwaartepunt, waardoor de romp terug naar verticaal draait. Daarom voelt de correctie eerst omgekeerd: om voorover vallen te stoppen moet je vooruit rijden, op dezelfde manier als je je hand beweegt onder een bezem die je in balans houdt in plaats van ervandaan.

Dat geeft de robot een harde grens. Voorbij ongeveer 35° is de benodigde horizontale versnelling groter dan de wielen kunnen leveren voordat de romp de vloer raakt, en geen enkele versterking helpt. De stippellijnen in de grafiek markeren die grens.

Algoritme

De regelaar is gekoppeld — twee PID-lussen, de een voedt de ander.

De binnenste lus draait snel en houdt een kantelhoek vast. Hij vergelijkt de gemeten hoek met een doel en beveelt motorkoppel:

error   = measured_angle − target_angle
motor   = Kp·error + Ki·∫error + Kd·d(measured)/dt

Let op dat de fout gemeten min doel is, niet het klassieke doel min gemeten. Een voorwaartse val opvangen betekent de wielen vooruit rijden, dus een positieve kanteling moet een positieve motorsturing opleveren. Dit teken is de meest voorkomende reden dat een eerste balancerende robot zich de seconde dat hij aangaat op de grond stort.

De buitenste lus draait langzamer en houdt een positie vast. Hij kan de motoren niet aansturen — die zijn al bezet — dus stuurt hij in plaats daarvan een kanteldoel voor de binnenste lus:

target_angle = Kp_pos·(desired_position − position) − Kd_pos·velocity

Achter de plek waar hij wil zijn vraagt hij om een kleine voorwaartse helling; de binnenste lus rijdt dan de wielen aan om die helling vast te houden, en de robot rijdt vooruit. De helling wordt bewust begrensd tot een paar graden. Een buitenste lus die een grote hoek mag eisen geeft de binnenste lus een doel dat hij niet kan vasthouden, en beide lussen verliezen tegelijk.

De integratorterm is hier belangrijker dan in de meeste lussen, omdat hij het verschil opvangt tussen waar de IMU denkt dat verticaal ligt en waar verticaal werkelijk is. Hij is ook de reden dat de integrator moet stoppen met accumuleren terwijl de motoren verzadigd zijn — anders loopt hij op tijdens een val die hij geen macht heeft te voorkomen, en vecht hij daarna tegen het herstel.

Broncode

// Inner loop: tilt → motors. Runs every 5 ms off a timer, not in loop().
float angleError = angle - tiltTarget;          // measured minus target
angleIntegral   = constrain(angleIntegral + angleError * dt, -3, 3);
float command   = Kp * angleError
                + Ki * angleIntegral
                + Kd * gyroRate;                // rate straight from the gyro

command = constrain(command, -255, 255);
if (fabs(angle) > FALL_ANGLE) command = 0;      // past saving; cut the motors
driveMotors(command, command);

// Outer loop: position → tilt target. Runs ten times slower, and may only
// ever ask for a small lean.
if (++slowTick >= 10) {
  slowTick = 0;
  float positionError = 0 - wheelPosition;
  tiltTarget = constrain(KpPos * positionError - KdPos * wheelVelocity,
                         -0.12, 0.12);          // radians, about ±7°
}

Twee details neem je rechtstreeks over uit deze simulator. Neem de differentiaalterm van de gyroscoopsnelheid in plaats van de hoek te differentiëren: de gyroscoop meet die snelheid rechtstreeks en zonder de ruis die differentiëren versterkt. En schakel de motoren af voorbij de herstelhoek in plaats van de regelaar te laten blijven trekken — dat spaart de versnellingsbakken en maakt de storing duidelijk in plaats van heftig.

Hardware

De sensor is een MPU6050, die zowel een versneller als een gyroscoop bevat, omdat geen van beide op zichzelf bruikbaar is. De versneller weet waar omlaag is maar wordt overspoeld door de beweging van de robot zelf; de gyroscoop volgt rotatie zuiver maar loopt binnen seconden weg. Een complementair filter voegt ze samen tot een hoek die zowel stabiel als snel is.

Motoren moeten laag genoeg verkleed zijn voor koppel en snel genoeg om een val op te vangen — N20-kastmotoren rond 200 rpm zijn een gangbare keuze, aangestuurd door een TB6612FNG, waarvan de lage spanningsval telt wanneer de lus duizenden keren per seconde om kleine correcties vraagt.

Bouw hem echt in het project zelfbalancerende robot, of begin met de tutorial kanteling lezen als de IMU nieuw voor je is.

Aannames en hardwareverschillen

Het model is een tweedimensionale omgekeerde slinger met één kantelas, starre koppels, geen wielslip en een IMU die de werkelijke hoek en snelheid meldt met alleen de drift en ruis die je zelf instelt. Een echte balancerende robot voelt motorkoppel dat meebeweegt met de accuspanning, encoderkwantisatie, IMU-montageoffset en temperatuurdrift, en hij kan ook opzij vallen — een vrijheidsgraad die dit lab volledig weglat. De gekoppelde PID-structuur en de differentiaalterm op de gyroscoopsnelheid die hier getoond worden gaan rechtstreeks over, maar de montage-trim, de filterafstelling en de grenshoek voor vallen moeten op het fysieke chassis opnieuw worden gevonden.

Schakelschema

Twee bedradingsregels bepalen of deze robot werkt.

De motoraccu bereikt de VM-pin van de besturing en niets anders. Motorstroom mag nooit door de Arduino lopen, en alle massa’s — bord, besturing, IMU, accuminus — moeten met elkaar verbonden zijn, anders hebben de regelsignalen geen gemeenschappelijke referentie en is de storing stil in plaats van duidelijk.

De encoder krijgt voeding uit de logica, niet uit de motorrail. Een encoder op de motorvoeding spannen vernietigt hem of vult de tellingen met schakelruis. Zet encoderkanaal A ook op een pin die interrupts kan verwerken; hem uit loop() pollen kost tellingen, en verloren tellingen betekenen dat de buitenste lus langzaam kwijtraakt waar de robot is.

Monteer de IMU dicht bij de as. Hoog monteren vermenigvuldigt de lineaire versnelling die hij bij elke correctie ziet, wat het fusieprobleem moeilijker maakt zonder enig voordeel.