PID regelaar in het echt

Voor dit project ga ik verder werken aan een project wat ik ooit ben begonnen maar waar ik destijds nog lang niet goed genoeg voor kon coderen. Dit is namelijk het maken van een PID regelaar in het echt en hiervoor ook een digitale twin maken. Dit ga ik doen omdat ik wil kijken of ik in de afgelopen paar jaar beter ben geworden in zowel programmeren wiskunde en simuleren.

Wat is een PID-regelaar

Stel je voor je hebt een boot en je wilt deze draaien dat gaat niet zo makkelijk als bij een auto waar je gewoon kan stoppen met sturen zodra je klaar bent met draaien. Een boot die drijft door met de snelheid waar die mee aan het draaien is dit noem je oversteer. Als je de boot op de goede koers wilt houden moet je voordat je daar bent al stoppen met sturen dit doet een PID regelaar voor systemen. Dit gebruik je bijvoorbeeld bij het verwarmen van een ketel.

De grafiek hierboven is wat je zou krijgen bij het sturen van een boot zonder dat je eerder stopt met sturen waardoor je telkens een overshoot krijgt en het enorm lang duurt voordat je op de goede koers beland.

Een PID regelaar werkt door de error (gewenste waarde - huidige waarde) te vermenigvuldigen met van te voren bepaalde waardes en hieruit een waarde te krijgen die teruggestopt wordt in het systeem

Zoals hierboven te zien is heb je een P een I en een D deze waardes helpen allemaal op een andere manier om zo weinig mogelijk overshoot te krijgen en voor elk systeem zijn deze waardes anders.

De P staat voor proportioneel deze is het simpelst het kijkt simpelweg naar het verschil tussen de gewenste waarde en de huidige waarde en geeft hiermee een waarde. Dat betekend dat als je alleen een P zou gebruiken je pas zou stoppen met sturen als je op de gewenste waarde bent aangekomen.

de I staat voor integraal deze neem alle error over een tijd van t mee en zorgt ervoor dat je geen blijvende offset hebt van je gewenste waarde. Dit wordt dus gebruikt om kleine aanpassingen te maken zodat je de laatste stapjes kan halen

De D staat voor derivaat en wordt gebruikt om te toekomst te voorspellen van wat het systeem gaat doen door de laatste en huidige error te bekijken en te zien of dit verschil verandert.
Dus als de error net 5 was en nu 4 dan weet de D dat we elke stap 1 dichterbij de gewenste waarde komen en over 4 stappen er zijn dus misschien minder snel moeten bewegen.

Zoals je ziet zijn PID-regelaars enorm lastig om te gebruiken en al helemaal in echte situaties.

Mijn testopstelling

Ik heb gekozen voor een simpele opstelling namelijk de bal op een plank die doormiddel van een PID-regelaar naar een gekozen punt op de plank kan verplaatsen.

De opstelling bestaat uit een servo die de plank omhoog en omlaag kan bewegen zodat de bal er overheen kan rollen en een afstand sensor die gebruikt wordt als input voor de PID-regelaar. Als bal is er gekozen voor een pingpongbal omdat deze makkelijk beweegt en niet te zwaar is waardoor er geen snelle bewegingen nodig zijn om hem te stoppen. Ik heb de testopstelling compleet zelf gemaakt in fusion en geprint de baan is 430mm lang en de servo kan deze 50mm van hoogte veranderen op een afstand van 270mm. Dit zorgt ervoor dat de baan een hoek van 5,29 graden kan hebben naar boven en beneden. Om dit allemaal te besturen maak ik gebruik van een simpele Arduino code waarin de PID-regelaar is verwerkt.

#include <VL53L0X.h> 
#include <Wire.h> 
#include <Servo.h> 

// Pin definitie 
const int servoPin = 9; 

// PID regelaar variabelen 
double dt, laatsteTijd; 
float setpoint = 150; // Gewenste afstand in mm 
float input, vorigeInput, output, vorigeOutput, stuuractie; 
float kp = 0.75, ki = 0.1, kd = 0.1; 
float error, vorigeError = 0; 
float integral = 0; 
float derivative; 
float errorP, errorI, errorD; 

// Servo object 
Servo myServo; 

// VL53L0X sensor object 
VL53L0X sensor; 

void setup() { 
  Serial.begin(9600); 
  Wire.begin(); 

  // Initaliseer de VL53L0X sensor 
  sensor.init(); 
  sensor.setTimeout(500); 
  sensor.setSignalRateLimit(0.1); 
  sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodPreRange, 18); 
  sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodFinalRange, 14); 
  sensor.setMeasurementTimingBudget(33000); 

  myServo.attach(servoPin); 
} 

void loop() { 

  double now = millis(); 
  dt = (now - laatsteTijd)/1000.00; 
  last_time = now; 
  // Meet de afstand 
  input = sensor.readRangeSingleMillimeters() - 50; 


  // Bereken de fout 
  error = setpoint - input; 

  // PID berekeningen 
  integral = integral + error * dt; 
  derivative = (error - vorigeError) / dt; 
  vorigeError = error; 
  // PID output berekenen en begrenzen 
  vorigeOutput = output; 
  errorP = kp * error; 
  errorI = ki * integral; 
  errorD = kd * derivative; 
  output = errorP + errorI + errorD; 

  // Beperk de output 
  stuuractie = output; 
  stuuractie = constrain(stuuractie, -90, 90); 

  // Stuur de servo aan 
  myServo.write(stuuractie + 90); 

  // Print de data uit om over te zetten naar een grafiek 
  Serial.print(" "); 
  Serial.print(now); 
  Serial.print(","); 
  Serial.print(input); 
  // Serial.print(" ErrorP: "); Serial.print(errorP); 
  // Serial.print(" ErrorI: "); Serial.print(errorI); 
  // Serial.print(" ErrorD: "); Serial.print(errorD); 
  // Serial.print(" Output: "); Serial.println(output); 

  // Wacht voor de volgende meting 
  delay(dt); 
}

Theorie en digital twin

Een digitaltwin is vaak een model van de echte opstelling in een simulatieprogramma die rekening houdt met alle krachten die ook in het echt inspelen op het model zoals rolweerstand luchtweerstand en de bewegingstijd van de servo. Maar hoe maak je nou zo’n digitaltwin? dat is best complex want hoe dingen in het echt gewoon werken (zoals zwaartekracht en het rollen van een bal) moet je dit in de digitaltwin allemaal zelf met wiskundige berekeningen erin zetten.
Om dat te kunnen doen moet ik eerst weten welke krachten er allemaal op mijn systeem invloed hebben en deze omzetten naar wiskundige formules. Dus is het tijd voor natuurkunde.

Hier is een versimpelde versie van alle krachten die op mijn bal spelen. Als eerste speelt zwaartekracht een belangrijke rol:

$$F𝑧 = 𝑀𝐵𝑎𝑙 ∗ g$$

Hierin is Mbal de massa van de bal (2,7g en g het gravitatieconstante (9,81)
Fzx is de kracht waarmee de pingpongbal accelereert op de helling deze wordt als volgt berekend:

$$Fzx = 𝐹z ∗ sina$$

waarin a de hoek van de baan is in radialen.

om de normaalkracht te berekenen wordt er gebruikt gemaakt van de volgende berekening:

$$Fn =𝐹z ∗ cosa$$

De laatste kracht die invloed heeft op het model is de rolweerstand van de pingpongbal.

$$Frw =𝐶𝑟 ∗ 𝐹n$$

Hierbij is Cr de rolweerstandcoëfficiënt van de pingpongbal die ligt tussen de 0.001 en 0.002 afhankelijk van het materiaal van de baan. Voor de simulatie is er een waarde van 0.0013 gekozen door uitbundig testen.

De resulterende kracht van de pingpongbal kan je dan als volgt berekenen.

$$F𝑟e𝑠 = 𝐹zx −𝐹rw$$

Omdat de pingpongbal rolt en niet schuift moet er om de acceleratie van de pingpongbal te berekenen ook de hoekversnelling uitgerekend worden ook moet er gekeken worden naar de massatraagheid van de bal dit is de kracht die een rond object vertoont bij het starten met bewegen. Dit kan berekend worden met de volgende formules:

$$a = M/I$$

waar M het krachtmoment in Nm is en I het massatraagheidsmoment en a de hoekversnelling in rad/s².

Om het krachtmoment te berekenen wordt de volgende formule gebruikt:

$$M - Fres * r$$

waarin r de radius van de bal is.
Om het massatraagheidsmoment te berekenen is de formule voor een holle bal gebruikt:

$$I = 2/3 * m * r^2$$

Wat voor de pingpongbal het volgende is:

$$I = 2/3 * 0,0027 * 0,02^2 = 7,2 *10^{-7} *m^2$$

Als je dit allemaal samenvoegt krijg je het model van de krachten die op de bal spelen dit is als volgt:

$$Fres = Mbal * g * sin a - Cr * Mbal * g * cos a$$

Als laatste moet er nog rekening worden gehouden met de bewegingssnelheid van de servo (in het echt kan deze niet onmiddellijk van punt naar punt springen) van 0,17 seconden per 60 graden. dat betekend dat het bewegen van de servo een 1ste orde systeem is.

Om dit allemaal in een simulatieprogramma te implementeren ga ik gebruik maken van 20-sim een programma om wiskundige simulaties te maken het werkt doormiddel van het slepen van blokjes Om te zorgen dat het werkt moeten er een paar extra dingen worden toegevoegd zoals de maximale bewegingshoek van de servo en de uiteindes van de baan zodat als de bal daar tegen aankomt de snelheid automatisch naar 0 verandert.

Het bovenstaande is de complete simulatie in 20-sim dit heeft me ongeveer 1.5 week gekost om alle waardes goed te krijgen en edge-cases te verhelpen. Het bestaat uit verschillende blokken om alles overzichtelijk te houden. Een van de pluspunten van de digitaltwin is dat ik zo veel sneller kan testen met nieuwe PID waardes voor het echte systeem.

Testen

Om een goede PID regelaar te maken is er geen formule je moet testen simpelweg waardes proberen tot je een goed werkend systeem krijgt. Dit kan je het beste doen door te beginnen met alleen een P daarna een PD en daarna een compleet PID systeem. Ook gebruik ik het testen om te kijken of mijn digitale twin overeenkomt met het echte systeem.

Het is belangrijk om eerste de twee systemen te valideren door te testen zonder regeling dit betekend eigenlijk gewoon de bal van de helling te laten rollen en te kijken hoelang het duurt voordat die beneden is.

Bij de fysieke opstelling deed de bal er 1,23 seconde over en de digitaltwin deed er 1,25 seconde over.

Testen met alleen P
Er is gekeken naar het systeem bij een waarde van 0,5 en 1 dit is gedaan omdat bij deze waardes de verschillen erg groot zijn en dat maakt het makkelijker om te kijken of de digitaltwin en fysieke opstelling beter overeenkomen.

Hierboven is de fysieke opstelling en de digitaltwin te zien bij een P van 0,5 en hier komen ze deels overheen ook kan je zien dat het systeem zichzelf al best goed regelt en bijna tot het setpoint komt wel met veel overshoot en een lange tijd.

Hierboven is te zien hoe het systeem reageert bij een P van 1 en je kan zien dat die niet in de buurt komt van het gewenste punt en blijft oscilleren.

Testen met PD
Om het systeem te verbeteren ga ik een D-term toevoegen die zorg ervoor dat de bal minder ver doorschiet en dus sneller bij het gewenste punt komt. Na veel proberen zijn de beste waardes hiervoor een P van 0,75 en een D van 0,1

Hier is te zien dat de twee systemen erg goed overeenkomen alleen dat het fysieke systeem nog een offset heeft van ongeveer 20-30mm gelukkig kan dat opgelost worden door de I-term later toe te voegen. Voor nu is er wel te zien dat er minder “overshoot” is en het systeem sneller stopt met oscilleren.

Testen met PID
Om het gehele systeem te testen wordt er nu een I-term toegevoegd om deze zo goed mogelijk te krijgen heb ik op zowel het fysieke systeem als de digitale 100en verschillende waardes geprobeerd om een zo’n goed mogelijk systeem te krijgen.

Zoals je hier kan zien door het toevoegen van een I word in het fysieke systeem de offset weggehaald en blijft de bal op het correcte punt liggen dit komt ook overheen met wat er te zien is in de digitaltwin.

Fysieke opstelling

Hoe ziet dat er nou uit als de fysieke opstelling aan het werk is dat laat ik in de onderstaande video zien.