Pages: [1]   Go Down
Author Topic: AutoBalance - Controllo PID  (Read 2652 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Full Member
***
Karma: 0
Posts: 103
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Salve a tutti smiley-grin
Mi sto cimentando nella costruzione di un AutoBalance, uno di quei sistemi con due ruote che riescono a tenersi in equilibrio da soli.

Per ora, il mio unico obbiettivo è mantenere l'equilibrio su una superficie piana.
Quindi niente salite, discese, forze esterne, o controllo del movimento.

La parte meccanica, in pratica è finita.
Purtroppo non ho una foto a portata di mano, quindi vi metto un rendering per darvi un'idea di come è fatto:

http://oi47.tinypic.com/2le303k.jpg

Il "robot" è composto da un Arduino UNO, due sensori SHARP analogici ( GP2Y0A21YK0F ), due motoriduttori ( SBGM9) ed un driver motore fatto da me ( se volete poi allego anche lo schema elettrico di questo).
Il primo piano per ora è vuoto, in seguito ci andranno le batterie. (Per ora faccio le prove con dei fili volanti)

--

Allora, il problema è, ovviamente, che non si tiene in equilibrio  smiley-mr-green

La parte della gestione del PID l'ho copiata da un esempio trovato online (per la precisione quì: http://schianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf), il resto l'ho scritto io..

Non sò se il problema sia dovuto ad un problema software, al fatto che non sono capace di gestire un PID oppure (la cosa che temo di più) che i due motoriduttori non sono adatti, e che dovrò cambiarli con dei servo 360° (cosa che vorrei, ovviamente, evitare...)

Continuo nel post successivo che non fà postare più 9500 caratteri  smiley-red
Logged

Offline Offline
Full Member
***
Karma: 0
Posts: 103
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Vi allego lo Sketch: (L'ho diviso in schede per comodità)

Main Program:
Code:
// Definizione dei pin utilizzati dai sensori
#define sensore1 A0                           // Definisce il pin del sensore1
#define sensore2 A1                           // Definisce il pin del sensore2

// Definizione dei pin utilizzati dai motori
#define motore1 3                             // Definisce il pin PWM a cui il motore1 è collegato
#define motore2 5                             // Definisce il pin PWM a cui il motore2 è collegato

// Variabile per le distanze anteriore e posteriore
int distanzas1 = 0;                           // Valore letto dal sensore1
int distanzas2 = 0;                           // Valore letto dal sensore2

// Contatori d'appoggio
int C;                                        // Contatore d'appoggio
int J;                                        // Contatore d'appoggio

// Variabili per il filtraggio delle misure
int arrays1[10];                              // Array di appoggio per la rilevazione della distanza1
int arrays2[10];                              // Array di appoggio per la rilevazione della distanza2
int n = 20;                                   // Numero di volte che si deve effettuare la misura, per il calcolo della media
int somma = 0;                                // Variabile di appoggio per il calcolo della media
int appoggio;                                 // Variabie di appoggio per l'ordinamento degli array

// Variabili per il calcolo dell'orientamento
int orientamento = 0;                         // Inclinazione dell'AutoBalance, in funzione delle distanze rilevate
int previousorientamento = 0;

// Variabili temporali, per il calcolo del PID
float time = 0;                               // Tempo auttuale, dall'accensione del dispositivo
float previousTime = 0;                       // Tempo del precedente ciclo
float interval = 0;                           // Tempo impiegato per l'esecuzione di un ciclo di programma

// Variabili PID
float P = 0;                                  // Variabile PROPORZIONALE  del PID
float I = 0;                                  // Variabile INTEGRATIVA del PID
float D = 0;                                  // Variabile DERIVATIVA del PID
float PID = 0;                                // Parametro PID

// Guadagni dei coefficenti PID
float kP = 12;                                // Variabile PID PROPORZIONALE
float kI = 430;                               // Variabile PID INTEGRATIVA
float kD = 20;                                // Variabile PID DERIVATRIVA

void setup()
{
    Serial.begin( 9600 );                     // Avvia la comunicazzione seriale
    pinMode( motore1, OUTPUT );               // Imposta il pin del motore1 come uscita
    pinMode( motore2, OUTPUT );               // Imposta il pin del motore2 come uscita
}

void loop()                                   //  In Debug
{  
  controllomotori( getPID() );
  
  Serial.print( "Distanza S1 =  " );    
  Serial.print(  analogRead( sensore1 ));                      
  Serial.print( "  DS1 Filtrata =  " );    
  Serial.print( distanzas1 );  
  Serial.print( "  Distanza S2 =  " );    
  Serial.print(  analogRead( sensore2 ));                      
  Serial.print( "  DS2 Filtrata =  " );    
  Serial.print( distanzas2 );
  Serial.print( "  Orientamento =  " );    
  Serial.print( orientamento );
  Serial.print( "  P =  " );    
  Serial.print( P );
  Serial.print( "  I =  " );    
  Serial.print( I );
  Serial.print( "  D =  " );    
  Serial.print( D );
  Serial.print( "  PID =  " );    
  Serial.print( PID );
  Serial.print( "  To Motori =  " );
  Serial.println( ( 255 / 2 ) + PID );
  
  previousorientamento = orientamento;         // Orientamento al ciclo precedente  
}


Acquisizione Dati:
Code:
int getdistanzas1()                              // Algoritmo per l'acquisizione della distanza s1
{
  for ( C = 1; C <= n; C++ )                     // Legge 20 valori dal sensore1, e li trascrive in un array
  {
    arrays1[C] = analogRead( sensore1 );
  }  
    
  for ( C = 1; C <= n; C++ )                     // Ordina l'array in modo crescente
  {
    for ( J = (C + 1); J <= n; J++ )
    {
      if ( arrays1[C] > arrays1[J] )
      {
        appoggio = arrays1[C];
       arrays1[C] = arrays1[J];
arrays1[J] = appoggio;
      }
    }
  }
  
  somma = 0;
  
  for ( C = 6; C <= ( n - 5 ); C++ )             // Non considera gli estremi
  {
    somma = somma + arrays1[C];
  }
  
  distanzas1 = somma / ( n - 10 );               // Effettua la media dei valori
  
  return( distanzas1 );                          // Ritorna il valore distanzas2 oppurtunamente filtrato
}  




int getdistanzas2()                              // Algoritmo per l'acquisizione della distanza s2
{
  for ( C = 1; C <= n; C++ )                     // Legge 20 valori dal sensore2, e li trascrive in un array
  {
    arrays2[C] = analogRead( sensore2 );
  }  
    
  for ( C = 1; C <= n; C++ )                     // Ordina l'array in modo crescente
  {
    for ( J = (C + 1); J <= n; J++ )
    {
      if ( arrays2[C] > arrays2[J] )
      {
        appoggio = arrays2[C];
       arrays2[C] = arrays2[J];
arrays2[J] = appoggio;
      }
    }
  }
  
  somma = 0;
  
  for ( C = 6; C <= ( n - 5 ); C++ )             // Non considera gli estremi
  {
    somma = somma + arrays2[C];
  }
  
  distanzas2 = somma / ( n - 10 );               // Effettua la media dei valori
  
  return( distanzas2 );                          // Ritorna il valore distanzas2 oppurtunamente filtrato
}


Calcolo PID:
Code:
int getPID()                                                      // Algoritmo per il calcolo del PID
{
  getorientation();                                               // Richiama la funzione per ottenere l'orientamento
  
  previousTime = time;                                            // Salva il tempo al ciclo precedente
  time = millis();
  interval = time - previousTime;                           // Calcola il tempo impiegato per eseguire un ciclo
  
  P = orientamento * kP;                                          // Calcola la variabile proporzionale
  I = I + interval * kI * P;                                      // Calcola la variabile integrativa
  D = kD * ( orientamento - previousorientamento ) / interval;    // Calcola la variabile derivativa
  
  PID = P + I + D;                                                // Calcola il PID
  
  if( PID >= 255/2 ) PID = 255/2;                                 // Se il valore supera 255/2, il valore è impostato a 255/2  
  if( PID <= -255/2 ) PID = -255/2;                               // Se il valore è minore di -255/2, il valore è impostato a -255/2  
  
  if(PID <= 1 && PID > 0) PID = 0;                                // Approssima il PID a 0
  if(PID >= -1 && PID < 0) PID = 0;                               // Approssima il PID a 0
  
  return( PID );                                                  // Ritorna il valore del PID
}

void getorientation()                                             // Algoritmo per il calcolo dell'orientamento
{
  orientamento = getdistanzas1() - getdistanzas2();               // Differenza fra le distanze rilevate dai due sensori
}



Controllo motori:
Code:
void controllomotori ( int PID )                      // Algoritmo per il controlla i due motori DC
{
  analogWrite( motore1, ( 255 / 2 ) + PID );          // Controlla il motore1 in base al PID
  analogWrite( motore2, ( 255 / 2 ) + PID );          // Controlla il motore2 in base al PID
}


Quello di cui sono abbastanza sicuro è che la parte di acquisizione dati funziona "bene".
Non avevo idea di come farla, cosi ho deciso di prendere n valori, metterli in un tabella, ordinarla, togliere gli estremi e fare la media con i restanti.
Non ho idea se questo sia un metodo efficace oppure no, ma sembra fare il suo dovere...

Il problema è il resto  smiley-sweat

Togliendo la parte integrativa dal PID da accenni di funzionamento, ma non riesce assolutamente a tenersi in equilibrio...

Qualcuno ha qualche idea / consiglio su quello che posso fare?  smiley-sweat

L'obiettivo è portarlo come progetto d'esame  smiley
Grazie a tutti in anticipo.
« Last Edit: January 16, 2013, 05:16:11 pm by Mosc » Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 23
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Magari fai un video, io oggi ho sperimentato proprio l'algoritmo PID e ho ottenuto questo: https://www.youtube.com/watch?v=Jo9mfy4mhxg
Ancora un po' da affinare ma cominciamo ad esserci.

Ho notato questo:

La prima equazione agisce scandendo il ritmo del pendolo, quindi non stabilizza niente, imposta solo un valore di partenza in rapporto al grado di inclinazione. (Io l'ho immaginato come una molla)

La seconda equazione invece tende a tardare il cambio di stato, frenando l'effetto pendolo innescato dalla prima. (io l'ho immaginato come un freno idraulico, tipo ammortizzatore)

La terza invece agisce su variazioni repentine di angolazione, se aumenti troppo diventa troppo nervoso, ma per me serve molto perché stabilizza in seguito a urti, come nel video quando lo sbilancio volontariamente.

Io ho fatto così: prima ho settato un valore di KP per il quale il pendolo non fosse troppo violento, poi ho trovato un valore di KI che mi frenasse il giusto il pendolo anche se per tornare in bolla impiegava tempo. Infine settando KD si è ravvivato il tutto rispondendo abbastanza velocemente.

Se ho detto fesserie o imprecisioni o avete consigli sul mio video sono lieto di ascoltarli, spero comunque di essere stato d'aiuto.
Logged

Offline Offline
Full Member
***
Karma: 0
Posts: 103
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Grazie mille per la risposta!  smiley-grin

Se ti posso chiedere una cosa, le formule che hai usato per il calcolo del PID, sono uguali a quelle che ho usato io?
E come invii il valore del PID ai motori? sempre nello stesso modo?

Ok!
Come rimonto il tutto farò un video!  smiley

E grazie per i consigli su come regolare i coefficienti!
Non sapevo proprio come fare, come posso rifaccio qualche prova, e provo a regolarli imitando il tuo metodo  smiley-mr-green


edit. Mi interessa particolarmente il calcolo della I (del pid)  smiley-grin
La I assume valori troppo grandi, penso che ci sia qualche errore nel suo calcolo..

Per fargli assumere un valore nell'ordine delle centinaia ho dovuto mettere un kI = 0.00005;  o.o'

edit.2 Pensandoci, una cosa che mi darebbe un grande aiuto sarebbe conoscere quale ordine di grandezza assumo P, I e D durante il funzionamento smiley-grin (P suppongo 10 o 10², gli altri due?)
Ed un'ultima cosa, il valore di I và limitato in qualche modo? O deve essere libero di salire o scendere di qualsiasi valore? (in alcuni esempi trovati online ho visto che veniva limitato)
« Last Edit: January 16, 2013, 05:43:03 pm by Mosc » Logged

Offline Offline
Full Member
***
Karma: 0
Posts: 103
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Vi tengo aggiornati smiley-grin

Ho così modificato il programma:


Main program:
Code:
// Definizione dei pin utilizzati dai sensori
#define sensore1 A0                           // Definisce il pin del sensore1
#define sensore2 A1                           // Definisce il pin del sensore2

// Definizione dei pin utilizzati dai motori
#define motore1 3                             // Definisce il pin PWM a cui il motore1 è collegato
#define motore2 5                             // Definisce il pin PWM a cui il motore2 è collegato

// Variabili per il calcolo dell'orientamento
float orientamento = 0;                         // Inclinazione dell'AutoBalance, in funzione delle distanze rilevate
float previousorientamento = 0;

// Variabili temporali, per il calcolo del PID
long int time = 0;                               // Tempo auttuale, dall'accensione del dispositivo
long int previousTime = 0;                       // Tempo del precedente ciclo
int interval = 0;                           // Tempo impiegato per l'esecuzione di un ciclo di programma

// Variabili PID
float P = 0;                                  // Variabile PROPORZIONALE  del PID
float I = 0;                                  // Variabile INTEGRATIVA del PID
float D = 0;                                  // Variabile DERIVATIVA del PID
float PID = 0;                                // Parametro PID

// Guadagni dei coefficenti PID
float kP = 5;                                // Variabile PID PROPORZIONALE
float kI = 18;                         // Variabile PID INTEGRATIVA
float kD = 25;                                // Variabile PID DERIVATRIVA

// Gestione PWM
float pwm = 0;

void setup()
{
//Serial.begin( 9600 );                     // Avvia la comunicazzione seriale
    pinMode( motore1, OUTPUT );               // Imposta il pin del motore1 come uscita
    pinMode( motore2, OUTPUT );               // Imposta il pin del motore2 come uscita
}

void loop()                                   //
{  
  controllomotori( getPID() );
/*
  Serial.print( "  Orientamento =  " );    
  Serial.print( orientamento );
  Serial.print( "  Intervallo =  " );    
  Serial.print( interval );
  Serial.print( "  P =  " );    
  Serial.print( P );
  Serial.print( "  I =  " );    
  Serial.print( I );
  Serial.print( "  D =  " );    
  Serial.print( D );
  Serial.print( "  PID =  " );    
  Serial.print( PID );
  Serial.print( "  PWM =  " );
  Serial.println( pwm );
*/
  previousorientamento = orientamento;         // Orientamento al ciclo precedente  
}

Acquisizione dati:
Code:
float getdistanza( int sensore )
{
  int n = 5;                                     // Numero di volte che si deve effettuare la misura, per il calcolo della media
  int arrays1[n];                                // Array di appoggio per la rilevazione della distanza1
  int somma = 0;                                 // Variabile di appoggio per il calcolo della media
  int minimo = 9999;
  int massimo = 0;
  int C;                                        

  for ( C = 0; C < n; C++ )
  {  
    arrays1[C] = analogRead( sensore );
  
    if ( arrays1[C] < minimo )
        minimo = arrays1[C];
      
    if ( arrays1[C] > massimo )
        massimo = arrays1[C];
 
    somma = somma + arrays1[C];
  }
          
  return (somma - minimo - massimo) / ( n - 2 );
}


Calcolo PID:
Code:
float getPID()                                                      // Algoritmo per il calcolo del PID
{
  getorientation();                                               // Richiama la funzione per ottenere l'orientamento
  
  previousTime = time;                                            // Salva il tempo al ciclo precedente
  time = millis();
  interval = time - previousTime;                                 // Calcola il tempo impiegato per eseguire un ciclo
  
  P = orientamento * kP;                                          // Calcola la variabile proporzionale
  I = I + P * interval * kI / 10000 ;                                      // Calcola la variabile integrativa
  D = kD * ( orientamento - previousorientamento ) / interval;    // Calcola la variabile derivativa
  
 // if (I > 100) I=100;
 // if (I < 100) I=100;

  PID = P + I + D;                                                // Calcola il PID
  
  if( PID > 127 ) PID = 127;                                 // Se il valore supera 255/2, il valore è impostato a 255/2  
  if( PID < -127 ) PID = -127;                               // Se il valore è minore di -255/2, il valore è impostato a -255/2  
  
  //if(PID <= 1 && PID > 0) PID = 0;                                // Approssima il PID a 0
  //if(PID >= -1 && PID < 0) PID = 0;                               // Approssima il PID a 0
  
  return( PID );                                                  // Ritorna il valore del PID
}

void getorientation()                                             // Algoritmo per il calcolo dell'orientamento
{
  orientamento = getdistanza( sensore1 ) - getdistanza( sensore2 );               // Differenza fra le distanze rilevate dai due sensori
}


Controllo motori:
Code:
void controllomotori ( float PID )                      // Algoritmo per il controllo dei due motori DC
{
  pwm = 127 + PID;
 /*
  if ( pwm > 127 && pwm < 180 )
     pwm = 180;
    
  if ( pwm < 127 && pwm > 74 )
    pwm = 74;
 */
  analogWrite( motore1, pwm );          // Controlla il motore1 in base al PID
  analogWrite( motore2, pwm );          // Controlla il motore2 in base al PID
}


Non sono ancora riuscito a bilanciare perfettamente il PID, ma ora riesce a tenersi in equlibrio per 15/20 secondi.
Il problema principale erano i print a schermo, che prendevano troppo tempo, tolti quelli e ottimizzato un pò il programma la situazione è migliorata molto  smiley-grin

Inoltre ho notato che distanziando maggiormente i sensori dal corpo del robot funziona meglio (anche se non ho ben capito perchè).

Inoltre ho provato a togliere la "zona morta" in cui il valore del PWM è così basso che i motori non si muovono, forzando questi valori al minimo valore del PWM per cui le ruote muovono (molto lentamente).
Devo ancora capire se serve questa parte o riesco a farla tramite PID  smiley
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 1
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

se vuoi continuare ad usare la parte integrale devi inserire un antiwindup altrimenti ti satura velocemente l'output, quello che ti consiglio io e di creare una variabile "Antiwindup" poi inserire all'interno della funzione
...
Code:
P = orientamento * kP;                                          // Calcola la variabile proporzionale
  I = I + (interval * kI * P)- Antiwindup ;                                      // Calcola la variabile integrativa sottraendo l'eccesso della saturazione in modo da desaturare velocemente
  D = kD * ( orientamento - previousorientamento ) / interval;    // Calcola la variabile derivativa
  
  PID = P + I + D;                                                // Calcola il PID
  Antiwindup= PID; //somma l'uscita del PID non clampata

  if( PID >= 255/2 ) PID = 255/2;                                 // Se il valore supera 255/2, il valore è impostato a 255/2  
  if( PID <= -255/2 ) PID = -255/2;                               // Se il valore è minore di -255/2, il valore è impostato a -255/2  
  
  if(PID <= 1 && PID > 0) PID = 0;                                // Approssima il PID a 0
  if(PID >= -1 && PID < 0) PID = 0;                               // Approssima il PID a 0
Antiwindup -=PID;  //Sottrae l'uscita del PID dopo il clamping
  return( PID );                    
« Last Edit: May 15, 2013, 04:30:30 am by leo72 » Logged

Cagliari, Italy
Offline Offline
Tesla Member
***
Karma: 112
Posts: 7077
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Se vuoi puoi anche allegare il file dello sketch tramite il pulsante "Additional Options..." nell'editor.
Logged

Code fast. Code easy. Codebender --> http://codebender.cc/?referrer=PaoloP

Offline Offline
Newbie
*
Karma: 0
Posts: 6
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Salve a tutti, io sto provando a fare anche un AutoBalance Robot ma la violenza con cui oscilla lo rende instabile e cade subito, potete spiegarmi come calcolare le variabili kI, kP, kD? grazie !
Logged

ivrea (to)
Offline Offline
Faraday Member
**
Karma: 85
Posts: 4790
miaaao ^-^
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Per @PV97
ti invitiamo a presentarti qui: http://forum.arduino.cc/index.php?topic=113640.0
e a leggere il regolamento: http://forum.arduino.cc/index.php?topic=149082.0
- qui una serie di schede by xxxPighi per i collegamenti elettronici vari: http://forum.arduino.cc/index.php?topic=146152.0
- qui le pinout delle varie schede by xxxPighi: http://forum.arduino.cc/index.php?topic=151646.0
- qui una serie di link utili: http://forum.arduino.cc/index.php?topic=126861.0
Logged

my name is IGOR, not AIGOR

Offline Offline
Newbie
*
Karma: 0
Posts: 6
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

okk  nid69ita mi sono presentato ora dovrei mettere lì il mio progetto ??  smiley
Logged

ivrea (to)
Offline Offline
Faraday Member
**
Karma: 85
Posts: 4790
miaaao ^-^
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

okk  nid69ita mi sono presentato ora dovrei mettere lì il mio progetto ??  smiley
Direi di no. Però se nessuno ti risponde (tra qualche giorno, non avere fretta) allora ti consiglio di aprite un tuo thread sotto sezione software e magari citare questo thread (basta che copi e incolli il link del tuo post in questo thread)
Logged

my name is IGOR, not AIGOR

Offline Offline
Newbie
*
Karma: 0
Posts: 6
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

ahh va bene grazie delle delucidazioni  smiley
Logged

Pages: [1]   Go Up
Jump to: