Buclele de Control#

Buclele de control sunt funcții folosite pentru a opera sisteme de transmisii (precum șasiuri sau glisiere liniare) într-o manieră rapidă și controlată. Nu numai buclele de control îți permit să rulezi mecanisme rapid fără frica de a pierde controlul, însă în multe cazuri, prezervă fiabilitatea mecanismelor prin reducerea schimbărilor rapide în tensiunea aplicată motorului.

Ce este Eroarea?#

Primul lucru care trebuie definit când lucrezi cu buclele de control este conceptul erorii.

Error is defined as the difference between where you are and where you want to be. For instance, say you tell your drivetrain to drive at 30 inches per second, but in actuality, at a time, the drivetrain is driving at 28 inches per second. Since \(30-28=2\), the error of the drivetrain’s speed at this time \(T\) is 2 inches per second. In other words, at a time \(t=T\), \(e(t)=2\).

PID#

Un controller PID (sau controller cu Derivată Integrală Proporțională) este o buclă de control care se folosește de eroare pentru a controla sistemul. PID este o formă de buclă de control bazată pe feedback, sau o bucla de control închisă. Aceasta înseamnă ca datele în legatură cu variabila pe care o controlezi sunt necesare pentru ca bucla de control să corecteze acea variabilă. În acest caz, informații despre eroarea sistemului sunt necesare pentru a controla sistemul cu un controller PID.

Calculele Opționale#

Următoarea ecuație reprezintă definiția matematică riguroasă a ieșirii unui controller PID \(f\) la orice timp dat \(t\):

\[f(t) = K_p e(t) + K_i \int_o^t e(t) \mathrm{d}t + K_d \frac{\mathrm{d}e(t)}{\mathrm{d}t}\]

unde \(K_p\), \(K_i\), and \(K_d\) sunt constante iar \(e(t)\), menționat anterior, este eroarea sistemului.

If you have no experience with calculus, don’t worry; while PID is fundamentally rooted in calculus, you do not need any calculus experience to be able to understand it, only basic algebra. However, you are still urged to read the rest of the section regardless of calculus experience, as the formula alone doesn’t tell you why it works.

Simplificarea Formulei PID#

Aici este o variantă simplificată a formulei PID: \(f(t)=K_p P(t)+K_i I(t)+K_d D(t)\)

Tot ce am făcut este să luăm formula completă și să înlocuim partea de termeni cu funcții: \(P(t)\), \(I(t)\), and \(D(t)\).

Termenul Proporțional#

The first component of the function, \(K_p P(t)\), is by far the most simple and easy to understand, as \(P(t) = e(t)\). For the sake of example, let’s pretend that \(K_i=0\) and \(K_d=0\) (a PID controller with only a proportional constant is known as a P controller). How will the system behave? Well, if the error is large, the output will be large. Likewise, if the error is small, the output will be small. Also, ideally, given enough time, the system always approaches its destination, assuming \(K_p\) is of the correct sign.

Say we apply this to a drivetrain. You want to drive a distance \(D\), and you decide to set your motor powers using a P controller to accomplish this. In this case, your error is how far away the robot is from the desired location. As you start to drive forward, your error is large, so you drive forward quickly, which is desirable. After all, you aren’t concerned with overshooting the target yet if you are far away from it.

But as the robot’s distance to the target approaches 0, you will start to slow down, gaining more control over the robot. Once the error is zero, ideally, the robot will stop, and you have reached your destination. If you happen to overshoot, the error will become negative, and the robot will backtrack, repeating the process.

Termenul Derivat#

Acest termen, \(K_d D(t)\), este intenționat să amelioreze rata de schimbare a erorii. În alte cuvinte, încearcă să țină eroarea constantă. Cum se face acest lucru?

Păi, pentru cei care au experiență în analiza matematică, \(D(t)=\frac{de(t)}{dt}\). Pentru cei fără experiență în analiza matematică, aceasta reprezintă cât de rapid se schimbă eroarea. Grafic, \(D(t)\) este pur și simplu panta de eroare la orice timp dat \(t\).

Panta poate fi calculată prin observarea erorii prin iteratii succesive ale buclei de control. O iterație are loc la un timp dat \(t_n\) cu o eroare de \(e(t_n)\). La urmatoarea iterație, timpul este \(t_{n+1}\), cu o eroare de \(e(t_{n+1})\). Astfel, pentru a găsi \(D(t)\), pur și simplu găsește panta de \(e(t)\) cu aceste două puncte.

Termenul Integral#

Admittedly, the integral term is the least important term for FTC® PID control loops. With a properly tuned \(K_p\) and \(K_d\), you often can just set \(K_i\) to 0 and call it a day.

Totuși, poate fi folositoare în unele cazuri. Precum termenul derivat, termenul integral intenționează să corecteze depășirea. Dacă sistemul crede că și-a atins destinația, se va opri, și când, ca fapt divers, eroarea nu este înca 0. Poate că motorul nu are suficientă putere distribuită pentru a se mișca. Păi, cu suficient timp, termenul integral va mări output-ul (în acest caz, puterea motorului), pentru a se apropia de destinație.

Pentru a explica acest lucru fără analiză matematică, termenul integral este în esență suma erorilor într-un interval de timp dat. Pentru a face acest lucru, eroarea din fiecare buclă iterată este adaugată la o variabila (în acest caz, \(I(t)\)).

However, summing error this way has an unfortunate side effect: the longer the loop takes to complete one iteration, the more slowly this sum increases, which is obviously not desirable, as we don’t want lag to affect how the robot moves. To compensate for this, before the error is added to \(I(t)\), it is multiplied by how long the previous loop took to-complete, or \(t_{n+1}-t_n\), preventing lag from making the system sum more slowly.

So say the robot stops short of the target. The P and D combination aren’t strong enough to move it forward to the destination. You can either tune \(K_p\) and \(K_d\) to compensate (this is recommended), or you can add the integral term to increase output (this works too, but requires more attention and tuning to achieve the same result).

Pseudocod PID#

while True:
   current_time = get_current_time()
   current_error = desire_position-current_position

   p = k_p * current_error

   i += k_i * (current_error * (current_time - previous_time))

   if i > max_i:
       i = max_i
   elif i < -max_i:
       i = -max_i

   D = k_d * (current_error - previous_error) / (current_time - previous_time)

   output = p + i + d

   previous_error = current_error
   previous_time = current_time

Reglarea unei Bucle PID#

Cel mai important lucru de știut atunci când reglezi o buclă PID este cum fiecare termen afectează output-ul. Acest lucru îți permite să vezi care valori trebuie ajustate.

De exemplu, dacă ținta nu este atinsă dar în schimb punctul de referință începe să oscileze în jurul țintei, înseamnă ca valoarea D nu afectează suficient algoritmul. Dacă ținta este atinsă, însă foarte încet, înseamnă că valoarea P nu afectează suficient algoritmul sau D este prea mare.

Pe scurt, variabila P aduce eroarea spre zero, variabila I menține starea de echilibru al erorii, iar varibila D ameliorează efectele variabilei P, cu cât se apropie eroarea de zero, cu cât P devine mai mare, ceea ce previne depașirea țintei.

Cea mai comună metođă de a regla un controller PID este următoarea:

  1. Setează I și D la zero

  2. Crește valoarea P până când apar oscilații în jurul țintei

  3. Crește valoarea D până când robotul nu mai depășește ținta

  4. Dacă nu există o stare de echilibru, crește valoarea I până când se corectează

Un lucru important de reținut este că multe sisteme nu au nevoie de I și D. În general, sistemele fără prea multă frecare nu au nevoie de un termen I, însă vor avea nevoie de mai mult control D. Sistemele cu multă frecare, pe de altă parte, în general nu au nevoie de control D deoarece frecarea facilitează decelerarea însă au nevoie de control I deoarece frecarea previne sistemul din a-și atinge ținta.

Pentru mai multe detalii, apasă aici

Controllerul PID Integrat#

Pentru situațiile în care o persoană are nevoie să controleze viteza sau poziția unui singur motor, controllerul PID integrat poate fi folosit. PID poate fi activat prin schimbarea modului de rulare în RUN_USING_ENCODER.

Sugestie

Mulți nu înțeleg rolul RUN_USING_ENCODER, mulți pot înțelege că această funcție este necesară pentru ca encoderele să funcționeze, însă acest lucru nu este adevărat. În schimb, RUN_USING_ENCODER activează feedback-ul vitezei folosind encoderul. Dacă folosești un controller PID extern precum cel pe care îl implementezi, în general, este recomandat să folosești RUN_WITHOUT_ENCODER.

Pentru mai multe informații în legătură cu controllerul PID integrat, apasă aici

Depanarea Controllerului PID Integrat#

Problema

Soluția

Motorul merge la viteză maximă indiferent de viteza de referință

De cele mai multe ori acest lucru se întâmplă când au loc două probleme:

#1: Encoderul nu este conectat corespunzător.

Diagnoză: Înregistrează poziția encoderului în telemetrie, dacă poziția oscilează între 0-1 înseamnă că ai cablul corect și este conectat corespunzător.

#2: Motorul tău merge în direcția gresită.

Diagnoză: Înregistrează viteza ta în telemetrie, dacă ai o referință pozitivă a vitezei iar output-ul este negativ sau vice versa înseamnă că motorul tău este conectat invers.

Motorul nu atinge viteza maximă cu .setPower

Folosește metoda .setVelocity din clasa DcMotorEx sau RUN_WITHOUT_ENCODER cu un controller PID extern.

Rata de Numerizare a unui Controller PID#

Pentru echipele care vor cea mai bună performanță din controllerul lor PID, este esențial să iei în considerare rata de numerizare a controllerului. Rata de numerizare este atunci când controllerul îți actualizează output-ul având date noi despre senzori. Cu cât frecvența de numerizare este mai mare, cu cât controlul este mai stabil și permite folosirea unor coeficienți PID mai semnificativi pentru a reduce timpul de corectare. Vezi acest video pentru a vedea cum rata de numerizare afectează stabilitatea într-un exemplu practic cu un motor. Controllerul PID integrat este blocat la frecventa de actualizare de 20Hz (rata de numerizare de 50ms). Multe echipe FTC de top își optimizează buclele roboților lor pentru a rula la timpi de la până la 80Hz, obținând un control mult mai stabil cu un PID extern.

Controlul Feedforward#

Feedforward control is a method of what is known as „open-loop” control. This is the opposite of closed-loop control and the primary difference is that feedforward does not actively use sensors to control the system. Instead it „predicts” the desired input based on a model.

Typically feedforward is used to control either rates of change or combat known disturbances from your system.

Feedforward is very powerful because it is immune to noise or other sensor errors. This is because it is not actively measuring the system, but instead predicting the desired input. However, this also means that it is not very good at correcting for errors. This is why it is often used in conjunction with a closed-loop controller such as PID.

Kv Ka Feedforward Model#

The most common feedforward and the one used by libraries such as road-runner is the Kv-Ka feedforward model:

\[f(t) = K_v \cdot \text{Velocity} + K_a \cdot \text{Acceleration}\]

Where \(K_v\) is the velocity gain, \(K_a\) is the acceleration gain, and \(f(t)\) is the feedforward output sent to your motors.

These gains can be estimated by giving the controller a series of ramp inputs (such as those computed with a motion profile), measuring the output, and then changing these gains till the robot matches the desired motion.

Notă

The gains will change based on the robot’s mass, friction, and other factors. It is recommended to re-estimate these gains every time you make a significant change to your robot.

Kv Ka Feedforward Pseudocode#

while True:
     targetVelocity = getTargetVelocity(time)
     targetAcceleration = getTargetAcceleration(time)
     output = targetVelocity * Kv + targetAcceleration * Ka;

Static Friction Feedforward#

In every system there is bound to be some amount of static Friction. This means that the robot mechanism will not move until a certain amount of power is applied. This can be modeled by adding a constant feedforward term in the direction you want to move.

while True:
     error = desire_position - current_position;
     sign = signum(error) # sign of error, will be -1, 0, or 1
     output = sign * staticFriction + PID(error); # PID Controller + Friction Feedforward

Profilarea Mișcării#

Sfat

Motion profiles are not a specific type of control loop, but rather a technique that works well in combination with other control loops such as PID and feedforward.

Profilarea mișcării este o tehnică popularizată în FRC® care începe să își găsească folosul în FTC. Un profil al mișcării este o funcție folosită pentru a schimba viteza unei transmisii într-o manieră controlată și consistentă prin schimbarea vitezei treptat decât instantaneu.

Let’s illustrate this with an example: say you want your drivetrain, which is initially unmoving, to drive forward at full speed. Ordinarily, you would set all drivetrain motors to full power in the code. However, this can be problematic because even though you tell the motors to move at full speed instantaneously, the drivetrain takes time to get to full speed. This can lead to uncontrolled movements which have the potential to make autonomous less consistent and, perhaps more importantly, damage mechanisms.

Profilarea mișcării încearcă să rezolve această problemă.

Avantaje#

  • Mișcări mai controlate și previzibile

  • Reduce schimbarea rapidă a tensiunii aplicate motorului

Dezavantaje#

  • Poate fi mai lent

Există două tipuri de profilare a mișcării: profilare Trapezoidală și profilare S-Curve. Profilarea trapezoidală accerelează sistemul la o frecvență constantă, iar profilarea S-Curve presupune smucituri (jerk, sau schimbările în accerelația vitezei) care sunt constante. Astfel, profilarea S-Curve nu este optimă pentru a controla traiectorii 2D (precum condusul) și există pentru a reduce alunecarea (care de obicei apare doar când conduci în FTC), profilarea trapezoidală este recomandată pentru majoritatea aplicațiilor FTC.

Profilarea trapezoidală îți are numele din forma graficului velocității în timp:

Poziția în timp, velocitatea în timp, și accerelația în timp vizualizat pentru profilarea trapezoidală

These are the „magic functions” for velocity and acceleration over time alluded to in the feedforward section.#

Aici este niște pseudocod pentru un profil trapezoidal:

while True:
   current_velocity = get_current_velocity()
   current_time = get_current_time()

   direction_multiplier = 1

   if position_error < 0:
       direction_multiplier = -1

   # if maximum speed has not been reached
   if MAXIMUM_SPEED > abs(current_velocity):
       output_velocity = current_velocity + direction_multiplier * MAX_ACCELERATION * (current_time - previous_time)
       output_acceleration = MAX_ACCELERATION

   #if maximum speed has been reached, stay there for now
   else:
       outputVelocity = MAXIMUM_SPEED
       outputAcceleration = 0

   #if we are close enough to the object to begin slowing down
   if position_error <= (output_velocity * output_velocity) / (2 * MAX_ACCELERATION)):
       output_velocity = current_velocity - direction_multiplier * MAX_ACCELERATION * (current_time - previous_time)
       output_acceleration = -MAX_ACCELERATION

   previous_time = current_time

The results of the above pseudocode are then used in a feedforward and / or PID loop to control the position of the system in a smooth and predictable way.

Un exemplu mai avansat al matematicii din generarea profilării mișcării folosite în librăria Road Runner poate fi găsit în documentul Jupyter.