Self Balancing Robot - parte 6 - quella magic box chiamata PID

Croce e delizia di ogni progetto di self balancing è il PID.

Il PID è acronimo di Proporzionale/Integrale/Derivativo, ed è un sistema che prendendo come parametro di ingresso un valore di errore, restituisce un valore di uscita che dovrebbe servire a riportare il sistema controllato a un valore di errore pari a zero.

L'argomento è decisamente interessante e anche la sua trattazione matematica, ma se anche voi come me volete capire e fare senza avere una laurea in matematica, allora seguite i miei ragionamenti che forse saranno bestemmie per i colti matematici, ma vi assicuro che permetteranno di capire e di far funzionare le cose, la prova è che io sono riuscito a bilanciare il robot senza nessuna simulazione o calcoli tavolino.

La cosa interessante è che una volta capito cos'è un pid vi renderete conto che tantissime cose che vi circondano sono gestite con un pid, che può essere di volta in volta meccanico, elettronico o software. Vi faccio un esempio: il cruise control dell'automobile, il termostato elettronico della caldaia di casa, ecc.

Vediamo di capire il funzionamento del PID.

Immaginiamo un serbatoio di acqua che deve essere mantenuto il più possibile a livello costante. Un omino guarda ogni 10 minuti nel serbatoio e vigila su di esso. Quando il serbatoio aumenta di livello e supera il livello prestabilito, l'omino deve svuotarlo fino al raggiungimento del livello, se invece il livello cala perchè l'acqua viene usata da qualcuno, allora l'omino deve accendere una pompa e ripristinare il livello attingendo dal ruscello affianco.

L'omino ragiona in base al livello dell'acqua in quel momento e il livello ottimale. La differenza costituisce il nostro valore di ERRORE. 

Se l'acqua sale, l'errore sarà positivo, se l'acqua scende l'errore sarà negativo.

Immaginiamo ora alcune dinamiche:

piove molto e l'acqua sale. L'omino si rende conto del livello accresciuto e scarica l'acqua. Ma quando raggiunge il livello ottimale si accinge a chiudere lo scarico, ma nel farlo il livello ottimale viene raggiunto e sorpassato verso il basso a causa di un ritardo nel chiudere lo scarico. A questo punto l'omino apre la pompa per aggiungere altra acqua ma quando raggiunge il livello, nonostante chiuda la pompa nel più breve tempo possibile, il livello ottimale viene superato verso l'alto, seppur di poco. Di nuovo l'omino scarica ritrovandosi di nuovo sotto livello e poi carica fino ad ottenere un nuovo livello molto più vicino rispetto a prima al valore ottimale.

Questo effetto "a fisarmonica" è quanto di fatto avviene in un sistema PID dove viene utilizzata la sola componente proporzionale, cioè la P significa che per ogni valore di errore, viene generata una contromisura che è pari all'errore moltiplicato per un valore P. 

Quello che succede, rappresentato in un grafico, è che di fronte a un aumento improvviso del livello dell'acqua, il sistema risponderà non "a scatto" ma con una risposta che in funzione del diverso valore P tenderà a oscillare tanto più sarà elevato il valore di P, mentre con valori insufficienti di P la risposta sarà tanto più smorzata e lenta quanto il valore di P sarà piccolo.
Il seguente grafico tratto da PID without PHD rappresenta l'andamento della risposta del PID con diversi valori di P:

Se vi sembra difficile determinare il giusto valore di P, iniziate a rendervi conto che per ottenere un self balancing robot non basterà il termine P, ma dovrete lavorare sia con l'integrale che la derivata.

Parliamo di I: l'integrale. Ignorando la sua trattazione matematica, diciamo che rappresenta una sorta di storia del nostro errore. Possiamo pensare a un grafico che rappresenta l'andamento del nostro errore, e considerare che il nostro integrale sia legato al valore dell'area da esso rappresentato. Cioè, il nostro valore di integrale non dipende solo dal valore corrente, ma da tutti i valori assunti in precedenza, anzi di fatto è la sommatoria di tutti i valori precedenti.

Perchè vi serve l'integrale? vi faccio un esempio che tutti possono comprendere (cari fisici, cambiate blog prima di imprecare contro di me).

Se io vi passo in mano una scatola che pesa molto, da ferma, voi dovrete applicare una certa forza per sostenerla. Ma se questa scatola ve la lancio dal 3° piano voi dovrete applicare una forza molto maggiore per tenere la scatola. In un certo senso il parametro P lo posso applicare al peso della scatola per determina la forza da applicare. Più pesa, più forza serve, meno pesa, meno forza. Tutto chiaro. Ma funziona solo se la scatola è ferma.

Ma se la scatola la devo prendere al volo, ho bisogno di sapere quale forza applicare, e sarà tanto maggiore tanto la scatola ha preso velocità. In un certo senso, questo è rappresentato dall'integrale, che rappresenta quanto la "forza" della scatola in caduta si è incrementata fino a quel momento (ok, cari fisici, ora bestemmiate pure...).

Il valore di Derivata è difficile spiegarlo con un esempio: diciamo che rappresenta quanto velocemente  il nostro errore sta cambiando in quel momento. Capirete dopo, non tanto cosa rappresenta ma qual'è l'effetto.

Applichiamo questi concetti al robot.
Di base, l'errore è l'angolo di inclinazione rispetto alla posizione di errore, l'uscita del PID è il valore da applicare ai motori per contrastare la situazione di non equilibrio. Come concetto di base, più il robot si piega, più i motori devono girare veloci per contrastare la posizione di non equilibrio.

Quando il robot è in piedi, perfettamente in equilibrio, la differenza tra la sua posizione e l'angolo di equilibrio sarà pari a 0, e come tale i motori dovranno stare fermi.

Se incliniamo il robot di 1°, questo sarà il valore di errore che sarà passato al PID, attivando i motori con un valore calcolato dal PID.

Usando il solo parametro P, moltiplicheremo l'errore * P , ottenendo il valore da inviare ai motori.

Ipotizzando di avere la formula magica per ottenere un valore di P corretto, vi accorgerete da subito che il sistema funzionerà bene solo per piccoli errori, nell'ordine di pochissimi gradi, dopodichè il robot cadrà inevitabilmente a terra.

Qualsiasi tentativo di aumentare il valore di P non risolverà il problema, anzi, incapperete quasi subito in un fenomeno di oscillazione, per i motivi spiegati sopra legati al modo in cui l'omino osserva e applica l'azione di carico/scarico.

Ci sono due modi per evitare l'oscillazione: diminuire P fino a che l'oscillazione non si genera, oppure, aumentare il valore di D fino ad ottenere lo stesso risultato.

Bene, abbiamo capito a cosa serve D: serve per "smorzare" l'oscillazione, senza diminuire i valori di P. Cioè applicando il giusto valore di D, si può ottenere un valore di P abbastanza elevato da garantire una rapida risposta del sistema, cosa che invece un valore di P basso non garantirebbe. Osservate il grafico sopra per comprendere.

Proseguendo così però non arriverete a un valore infinito di P, ma arriverete a una situazione limite dove l'oscillazione oltre a non smorzarsi avverrà a una frequenza molto maggiore di quella innescata da P. Questo è un valore limite da non oltrepassare perchè non crea stabilità nel sistema.

Veniamo a I: questa è la vera magia del PID. Il valore I è quello che fa si che quando un robot sta cadendo, applica tanta più forza di reazione tanto più quanto è il tempo passato da quando il robot è in caduta. 

Proviamo a capire con un esempio: se un robot parte fermo da una situazione di non equilibrio di 30° è ben diverso se il robot sta cadendo da qualche decina di millisecondi e arriva in caduta a 30°. Nel secondo caso la forza di reazione dovrà essere maggiore. Quindi come nell'esempio precedente, capiamo che non è possibile applicare una forza proporzionale solo all'angolo di errore, ma dobbiamo tenere conto anche di cosa stava facendo il robot un attimo prima, due attimi prima, tre attimi prima, ecc., come per la scatola di prima lanciata dal poggiolo.
Il valore di D invece ci aiuta a contrastare la caduta, applicando una forza che è proporzionale a quanto velocemente il robot sta cadendo. Tuttavia, il suo reale effetto e scopo è principalmente quello di smorzare le oscillazioni.
Semplificando quindi le cose diciamo che P contrasta la caduta quando l'inclinazione è di pochi gradi, P + I contrastano la caduta quando l'inclinazione è sensibile, D contrasta le cadute rapide ma allo stesso tempo smorza l'effetto di oscillazione che P+I innescano.

Per determina la giusta combinazione di P, I e D, dobbiamo quindi SPERIMENTALMENTE determinare questi valori osservando il nostro robot:

- dobbiamo avere un valore di P che riesca a ripristinare l'equilibrio per piccole variazioni di errore (pochi gradi), ma non deve essere troppo elevato per non innescare auto-oscillazioni

- dobbiamo avere un valori di I che riesca a far riprendere l'equilibrio quando il robot affronta significative variazioni di inclinazione. In queste dinamiche il robot inclinandosi di molto tenderà a prendere velocità motore per via dell'effetto di P, a questo si dovrà sommare l'effetto di I per aumentare ancora la velocità dei motori e offrire una reazione maggiore all'effetto di caduta

- dobbiamo trovare un valore di D tale che si riesca a smorzare al meglio tutti i fenomeni di auto oscillazione che l'effetto sommato di P e I tendono a dare.

Detto questo vi assicuro che trovare la giusta combinazione di P I e D è un lavoro lungo, fatto di centinaia di prove. Nessun simulazione a tavolino vi darà delle risposte sensate: la combinazione di valori dipende da una infinità di variabili: il peso del robot, la distribuzione dei pesi, le caratteristiche dei motori e delle ruote, ecc.

Il codice nonostante la spiegazione all'acqua di rose è il seguente:

int updatePid(int targetPosition, int currentPosition)   {
  error = targetPosition - currentPosition; 
  pTerm = kP * error;
  integrated_error += error;                                       
  iTerm =constrain( kI * integrated_error, -GUARD_GAIN, GUARD_GAIN);
  dTerm = kD * (error - last_error);                            
  last_error = error;
  return -constrain(K*(pTerm + iTerm + dTerm), -255, 255);
}

dove targetPosition è il set point ovvero il valore desiderato (posizione di equilibrio, 0°), currentPosition è il valore corrente dell'errore.
Tutto qui? si non serve altro, sono meno di 10 righe di codice.

kP,kI e kD rappresentano le nostre costanti del sistema, ovvero i valori che dovrete trovare per stabilizzare il sistema. K è un fattore di moltiplicazione globale, che per semplicità vi consiglio di tenere a 1.

GUARD_GAIN è un valore di soglia del fattore integrale che serve a evitare il cossidetto effetto di saturazione, per semplicità vi consiglio di impostarlo a valore massimo dei vostri motori (255).

Esiste una libreria PID di arduino che fa lo stesso lavoro. Considerata la semplicità dell'algoritmo e il fatto che meno codice diamo in pasto ad arduino meno risorse dello stesso usiamo, ho preferito integrare queste poche righe direttamente nel mio codice.

Nel prossimo post: altri progetti Self Balancing e approfondimenti sul PID