Alessandro
YASBR – Yet Another Self-Balancing Robot
Tutorials -
Domenica 28 Novembre 2010 07:21
Scritto da Alessandro

Ormai realizzare un self-balancing robot è una attività alla portata di qualunque hobbista che si cimenta con la robotica su Arduino.

Cercando su google self-balancing robot escono una caterva di realizzazioni, che utilizzano i sensori più diversi e le tecniche di filtraggio e di controllo più diverse. Quindi realizzare un self balancing robot è ormai un’attività standard per qualunque hobbista, come realizzare un rover che se ne va in giro per la casa.

In particolare il livello di semplicità che è stato raggiunto da Gbm nel suo tutorial sul self-balancing robot per principianti è inarrivabile. Una manciata di componenti e di linee di codice che permettono di ottenere un risultato notevole, come è quello  di avere un robottino che rimane in equilibrio, era una cosa impensabile finché lui non ha provato e ci è riuscito.

Mi sono posto il problema di fare qualcosa che fosse il più semplice possibile sulla parte costruttiva e possibilemente che fosse semplice anche nella parte sw. Pertanto mi sono dedicato ad un self-balancing robot che utilizzi un solo sensore di distanza (un semplice sensore IR Sharp) e nient’altro. Nessun giroscopio, né accelerometro, né altri sensori. Tutto auto-costruito, nello spirito del DYI puro.

Ecco l’elenco dei componenti utilizzati per realizzare il robottino:

-        Una lastra di plexiglass presa da Leroy Marlin per il telaio

-        Due ruote per carrelli prese sempre da Leroy Marlin

-        Due servomotori standard modificati

-        Due Elle per servomotori di Gioblu

-        Un sensore IR sharp 10-80 cm

-        Un Arduino 2009 di Gioblu

-        Due pacchi batterie da 4 AA per motori e Arduino di Gioblu

-        Un po’ di pezzi di lamiera e una manciata di viti

In totale siamo abbondantemente sotto i 100 euro, quindi un buon risultato per quanto riguarda il costo.

Durante le prime prove ho avuto una serie di problemi con le ruote del carrello, erano troppo ovalizzate, e le ho sostituite con un paio di ruote che avevo nella cassettina degli attrezzi (sono le route di un Boe-Bot!).

Ecco la fase di costruzione del telaio, a tre piani, trasparente e di plexiglass. In particolare il taglio del telaio è stato fatto con un semplice cutter, successivamente i buchi con un trapanino tipo Dremel.

Il telaio con in evidenza i tagli da fare:

 

I piani del telaio tagliati e bucati:

 

Per collegare i servi ho usato le insostituibili Elle di Gioblu e per collegare i vari piani ho usato delle lastrine bucate di metallo. Arduino e batterie sono fissate con nastro adesivo a velcro.

Una fase di costruzione del telaio:

 

 

Ed ecco il robottino completo e montato. Si noti il sensore IR Sharp che è posizionato sul braccetto che sporge lateralmente. Ho voluto un robottino a tre piani per poter studiare l’impatto della distribuzione dei pesi, dati principalmente dai due pacchi batteria, sui quali in realtà non ho fatto ancora molte prove. Sull’Arduino 2009 ho posizionato uno shield con una breadboard su cui ho già appoggiato un accelerometro a tre assi. Per ora è scollegato e quindi non utilizzato, l’intenzione è chiaramente quello di usarlo nel prossimo futuro.

Foto robottino 1:

 

 

Foto robottino 2:

 

Foto robottino 3:

 

Veniamo ora al funzionamento del self-balancing. L’algoritmo utilizza un controllo PI, cioè un PID in cui il termine derivativo è uguale a zero. Il segnale proveniente dal sensore IR viene filtrato in modo molto semplice. Vengono letti  10 valori, poi vengono ordinati, eliminate le ‘code’, cioè i 2 valori più piccoli e più grandi, e sui restanti 6 valori viene fatta la media. Quindi nessun utilizzo di filtri di Kalman o simili. Infatti il segnale del sensore IR è sufficientemente ‘pulito’ per poter fare una semplice media. Si noti che il sensore ha range 10-80 cm e poiché ha una funzione di trasferimento tensione-distanza che è simile ad una iperbole nel range di funzionamento, ho cercato di posizionarlo nel punto di massima sensibilità, in modo da ottenere la massima variazione di tensione in uscita in rapporto alla distanza percorsa.

Per l’algoritmo PID ho attinto a piene mani dai listati di Gbm, qui riportati. Inizialmente ho sperato di poter usare un semplice controllo proporzionale, cioè con Kp diverso da zero e Ki, Kd uguali a zero. Ho passato qualche ora a fare prove, ma non sono riuscito mai a farlo stare in piedi, giusto per confermare la teoria che dice che con solo termine proporzionale si ottengono al massimo oscillazioni.

Introducendo poi il termine integrale nel controllo (cioè Ki diverso da zero) ho ottenuto un beneficio immediato e con qualche prova sono riuscito a far stare in equilibrio il robottino. Ecco un paio di video del robottino:

VIDEO 1

Qui qualche tentativo di introdurre un disturbo (con una spinta) e la reiezione del disturbo. Come si vede la situazione potrebbe essere migliore, perché dopo un po’ va in risonanza, però alla fine si riprende e rimane in piedi. Tutto ciò è dovuto alla mancanza del termine derivativo che ha la funzione di limitare le oscillazioni.

VIDEO 2

Questo è il semplice listato sull’Arduino, completamente commentato:

// ************************************************************************************

// INITIAL VARIABLES - VARIABILI INIZIALI

// *************************************************************************************

#include

Servo sservo;

Servo dservo;

int value0=0;

float H; // balance IR sensor - valore dell'IR all'equilibrio

int analogInput0 = 0;

float distance=0;



//PID variables - variabili PID

float error=0;

float previousError=0;

float time=0;

float P = 0;  // proportional control - controllo proporzionale

float I = 0;  // integral control - controllo integrale

float D = 0;  // derivative control - controllo derivativo



// PID gain - guadagni dei coefficienti PID

float kP = 45;  // proportional gain - guadagno proporzionale

float kI = 30;  // integral gain - guadagno integrale

float kD = 0;   // derivative gain - guadagno derivativo

int t;





//***********************************************************************************

// SETUP() - INITIAL OPERATION - OPERAZIONI INIZIALI

//***********************************************************************************

void setup() {

Serial.begin(9600); // Sets the baud rate to 9600 - imposta il baud rate a 9600

pinMode(analogInput0, INPUT);   // set the IR Sharp sensor as input - imposta il sensore Sharp come input

sservo.attach(9);  // attaches the servo on pin 9 to the servo object - LEFT - servo sinistro sul pin 9

dservo.attach(10);  // attaches the servo on pin 10 to the servo object - RIGHT - servo destro sul pin 10





// read IR initial value (balance value) - leggi il valore iniziale dell'IR (valore di equilibrio)

delay(300);

readIR();

delay(50);

readIR();

delay(50);



// distance at balance (in volts) - distanza in equilibrio (in volt)

H=distance;

}





// *******************************************************************************

// LOOP() - MAIN CYCLE - CICLO PRINCIPALE

// *******************************************************************************

void loop()

{



calcError(); //calculate vertical position error - calcola l'errore rispetto alla verticale



// loop cycle time calculation - calcolo tempo del ciclo loop

float previousTime = time;

time = millis();

float interval = time - previousTime;



// calculate PID coefficient - calcola i coefficienti del PID

P = error * kP / 1000;

I = I + (P * interval) * kI / 10000;

if (I > 20) I=20; //limit the integral - limita l'integrale

if (I  90) PID = 90; //value PID between -90 and 90

if(PID = -1 && PID < 0) PID = 0; //taglia le micro-correzioni



// move the servos - muovi i servomotori

sservo.write(90 - PID);

dservo.write(90 + PID);

previousError=error;



}



// *********************************************************************************

// readIR() - Read IR sensor - Leggi il valore del sensore IR

// ********************************************************************************

void readIR() {

int impulses=0;

int i, n;

int val[10];

int j, app;

// read n=10 values from IR sharp sensor

n=9;



// read n=10 values from IR sharp sensor - leggi n=10 valore del sensore IR sharp

for (i=0; i app); j--)

val[j+1] = val[j];

val[j] = app;

}



impulses=0;

// consider only 6 values (discard left and right tails) - considera solo 6 valori (elimina le code sinistra e a destra)

for(i=2; i<=n-2; i++)

{

impulses  =  impulses+val[i];

}

distance=impulses/(n+1-4); // calculate 6 values average - calcola la media dei 6 valori

}





// *******************************************************************************

// calcError() - calculate balance error (in volts) - calcola l'errore rispetto all'equilibrio (in volt)

// *******************************************************************************

void calcError()

{

readIR();

error = distance - H;

}

 

 

Qualche considerazione: il robottino è estremamente sensibile alla modifica dei parametri del controllo PI. Infatti, cambiando anche del 10% i valori dei coefficienti Kp e Ki il robottino non sta più in equilibrio. Con il senno di poi posso dire di aver avuto fortuna nel trovare una coppia di parametri che permettono di rimanere in equilibrio. Spostando i pesi, ad esempio le batterie da un piano all’altro, non sono riuscito a trovare dei parametri Kp e Ki adeguati per l’equilibrio. Mentre cercavo i valori corretti di Kp e Ki mi sono sentito perfettamente in sintonia con Gbm quando parlava del tempo trascorso nel cercare i valori corretti. Un lavoro di pazienza!

L’utilizzo di un solo sensore penso sia una situazione quasi ‘limite’ per poter ottenere l’equilibrio, pertanto se volete realizzare un self-balancing usate due o più sensori! Altrimenti dovete armarvi di tanta tanta pazienza nel cercare i parametri del controllo corretti.

Ora rimane da esplorare qualche nuova possibilità:

-        Vedere cosa cambia al cambiare dei pesi e quindi del baricentro del robottino (è meglio un baricentro basso o un baricentro altro?)

-        Usare un filtro di Kalman per filtrare i dati del sensore IR e vedere se la scelta dei parametri del PID diventa meno critica

-        Introdurre anche il termine derivativo Kd

-        Cominciare ad introdurre un nuovo sensore, l’accelerometro. E vedere cosa succede in termini di stabilità e facilità di controllo

 

Gioblu Robotics © 2010 - 2012 · Sitemap · privacy

gioscarab@gmail.com

Gioblu BOTServer è online dal 10 Aprile 2010 - 319.232 Visite - 1.027.175 Pagine visualizzate - 182.309 Visitatori unici - 536 utenti attivi