Sei sulla pagina 1di 6

Robotica Industriale M - Progetto di robotica mobile

Gruppo n.9

- Amadori Manuel

- Fantoni Maria Elena

- Furieri Luca

- Ghetti Fabio

Robot mobile Uniciclo ottenuto da un kit robotico LEGO Mindstorms NXT.

Linguaggio di programmazione Il codice è scritto utilizzandoleJOS NXJ, un linguaggio ad alto livello basato sul Java che usa un firmware ad-hoc sviluppato dal team leJOS per il modello NXT.

Obiettivo Il robot dovrà essere assemblato in modo da costituire un uniciclo e dovrà essere fornito dei sensori necessari al superamento dei task richiesti. In particolare, il veicolo mobile ha due compiti: il primo riguarda la risoluzione di un problema di “line following”, mentre il secondo un problema di “obstacleavoidance”. Nel primo task verrà impiegato un sensore di colore per riconoscere la pista (nera su sfondo bianco) e seguirla correttamente: alla partenza il robot verrà posizionato con orientamento casuale all’interno di un quadrato delimitato da una linea nera; dopo essere riuscito a uscire dal quadrato inserendosi correttamente sulla pista, il robot dovrà seguire il percorso nel minor tempo possibile. Alla fine del percorso sarà presente un ostacolo che bloccherà temporaneamente il robot e segnalerà l’inizio del secondo task. Il robotin seguito riceverà un segnale bluetooth trasmesso da una telecamera in grado di rilevare, tramite l’utilizzo di marker posti sopra di essi, la posizione corrente del veicolo, dell’obbiettivo da raggiungere e di alcuni ostacoli. Il robot dovrà essere in grado di seguire una traiettoria ottima in modo da raggiungere il punto richiesto (“goal”) evitando gli ostacoli nel minor tempo possibile.

Struttura del veicolo Per la realizzazione di un uniciclo si è adottato il modello cinematico equivalente di tipo Differential Drive. Sono state impiegate due ruote poste nella parte anteriore del veicolo, fisse e comandate indipendentemente da due motori; il robot è in una condizione di equilibrio stabile grazie ad un’ulteriore ruota centrata orientabile, più piccola e senza motore, posta nella parte posteriore del veicolo. I sensori richiesti sono due: il sensore di colore che permette il rilevamento della pista da seguire nel primo task, ed un sensore di tocco che rileva l’ostacolo che separa i task. Entrambi i sensori sono posizionati frontalmente e centrati rispetto al corpo centrale del brick. La superficie di contatto del sensore di tocco è stata ampliata per migliorarne la sensibilità, mentre il sensore di colore è stato posizionato il più vicino possibile al pavimento ed è stato parzialmente oscurato in modo da rendere la lettura del livello di luminosità più precisa. Il brick è stato posto

orizzontalmente per motivi di comodità nel collegamento dei cavi e per una maggiore stabilità della struttura meccanica del robot.

1° Task: Line Following Per ottenere le prestazioni ottimali, si è pensato di creare due diverse modalità di controllo e di taratura dei parametri in base alla configurazione del percorso. Distingueremo fra:

1)

uscita dal quadrato

2)

percorrenza della pista

Nella prima modalità, la velocità viene impostata in modo da permettere un veloce raggiungimento del bordo del quadrato. Una sequenza di selezioni logiche permette di rilevare i vari fronti di salita o discesa dovuti al cambio repentino della luminosità (cambio di colore del pavimento dovuto all’alternanza fra bianco e nero). La variabile fronte serve per tener memoria del bordo appena incontrato, in modo da far curvare il robot solo dopo aver incontrato il secondo fronte, quindi il secondo bordo della pista (quello esterno, su cui è richiesto che il robot si posizioni per percorrerla pista stessa). Si noti che molte prove sono state dedicate al rilevamento del corretto valore di riferimento da fornire al robot. Per un’azione rapida e precise nel seguire la pista è infatti necessario rielaborare i valori di luminosità misurati dal sensore relativamente un valore di riferimento, in modo da rilevare quando e in che misura il robot sta uscendo dalla pista o la pista sta cambiando configurazione. Per raggiungere un risultato soddisfacente si è dovuta usare una funzione apposita del sensore di colore che permette di rilevare un unico valore di luminosità (getNormalizedLightValue()) e calibrarlo più volte per essere certi di lavorare su valori stabili. Uscito dal quadrato la modalità di funzionamento del robot passa al codice Line Follow vero e proprio, gestito tramite controllo PID.

publicstaticvoid quadrato(){

if ((( ref- val )<-40) && ( fronte ==0)){

Motor.

A .setSpeed(250);

Motor.

B .setSpeed(250);

Motor.

A .forward();

Motor.

B .forward();

}

elseif ((( ref -val )>(40)) && fronte <2) {

fronte

=1;

Motor.

A .setSpeed(250);

Motor.

B .setSpeed(250);

Motor.

A .forward();

Motor.

B .forward();

}

elseif ((( ref -val )<-40) && ( fronte >=1))

{

Motor.

Motor. A .setSpeed(250); Motor. B .setSpeed(20); Motor. A .forward(); B .backward();

fronte

=2;

}

elseif (( ref -val )>40 && ( fronte ==2)) CONTROLMODE =3;

}

Quando vengono contemporaneamente rilevate l'elevata differenza tra l'errore misurato ed una soglia-errore predeterminata e, contemporaneamente, l'elevata differenza tra l'errore-integrale ed una predeterminata soglia-integrale, allora il robot percepisce la presenza di una curva e adatta i parametri del controllore di conseguenza. Quando invece la condizione non è soddisfatta, il robot percepisce un tratto rettilineo e sfrutta una taratura diversa. In particolare, nel tratto rettilineo la velocità delle ruote può essere mantenuta più alta mentre l’effetto di correzione è tenuto più basso, più “smooth”, in modo da contenere le oscillazioni. Si nota che i parametri di base del controllore PID (Kp, Ki e Kd) sono stati calcolati con le formule di Ziegler-Nichols e successivamente modificati per rispondere ad un andamento ottimale. Nel caso discreto infatti, le formule implementate devono tenere conto del fatto che l’integrale e la derivata vengono aggiornate soltanto una volta ogni ciclo di programma. Quindi si è stimato il tempo di ciclo di programma con un cronometro (su 20000 giri di programma) in modo da inserirlo correttamente nelle relazioni.

= 0.6 ∙

= 2 ∙ (

)

= ( )

8

Formule di Ziegler-Nichols

dove Kc è il guadagno critico corrispondente al periodo critico Pc nel quale si presentano oscillazioni sostenute.

Per evitare di sbandare e andare fuori pista le curve devono essere le più strette possibili. Per questo, dopo aver calcolato il valore corretto di velocità, la funzione AzionaMotori() si occupa di impostare tali velocità ai motori in modo differenziale se richiesto (forward/backward) in modo da far ruotare sul posto il robot.

 

Kp

Ki

Kd

Velocità

Rettilineo

91.8

1,9584

1075,78

450

gradi/sec

Curva

128.52

2,7418

1506,1

400

gradi/sec

L’azione integrale è limitata in modulo da un livello di saturazione impostato dopo alcune prove pratiche, in modo da evitare che la correzione applicata al moto sia eccessiva con conseguente fuoriuscita dalla pista (SAT=2800).

Il codice che si occupa della modalità Line Following è inserito in un ciclo 'while': appena il sensore di tocco viene premuto, ovvero si incontra la scatola che delimita la fine della pista, il controllo ritorna al programma principale che ferma i motori e mette il robot in attesa di un input da parte del sistema bluetooth.

publicstaticvoid AzionaMotori(){ if ( omega_A >= 0){

Motor. A

.setSpeed( omega_A );

Motor. A

.forward();

}

else {

omega_A = ( omega_A )*(-1);

Motor. A

.setSpeed(

omega_A );

Motor. A

.backward();

}

if ( omega_B >= 0){

Motor. B

Motor. B

.setSpeed( omega_B );

.forward();

}

else {

omega_B = ( omega_B )*(-1);

Motor. B

.setSpeed(

omega_B );

Motor. B

.backward();

}

}

2° Task: Obstacle Avoidance Nel secondo task si utilizza una comunicazione bluetooth tra il robot e un sistema di visione in grado di fornire la posizione del veicolo, di alcuni ostacoli e un traguardo. Compito del robot sarà quello di raggiungere il traguardo evitando gli ostacoli presenti sul suo cammino. Per fare questo si è pensato di pianificare la traiettoria basandosi sui campi potenziali. All’obbiettivo verrà associata una funzione, il potenziale attrattivo, che avrà il compito di attirare verso di sè il robot; agli ostacoli invece sarà associato un potenziale repulsivo, che ha il compito di attivarsi preventivamente in modo da evitare collisioni senza tuttavia alterare la traiettoria del robot nel caso non questo non si trovi in prossimità di un ostacolo.

̇() = −∇()

: () = () +

()

Il vettore q(t) delle configurazioni presenta tre informazioni: posizioni X, Y e orientamento θ rispetto al sistema di riferimento. Come potenziale attrattivo si sceglie il potenziale elastico. In questo modo, quando il robot è lontano dall’obbiettivo si muove velocemente, rallentando progressivamente mano a mano che si avvicina.

=

1

2

∆(, )

= ( − )

Il controllo del moto viene affrontato utilizzando il modello cinematico del robot: in particolare, per svincolarsi da problemi di spostamenti laterali, impossibili all’uniciclo, si definisce un punto b posto al di fuori dell’asse delle ruote, in grado idealmente di spostarsi ovunque istantaneamente. Sfruttando come detto la cinematica del veicolo, il robot sarà in grado di seguire la traiettoria definita dal punto b. Per un inseguimento più preciso poniamo il punto immaginario b ad una distanza di 10 cm.

Le

+

nuove coordinate del robot saranno: { = +

=

=

Grazie al modello cinematico siamo in grado di ricavare le velocità da applicare ai motori per seguire il campo potenziale.

( ) = (

cos

sin

sin

̇

cos ) ( ̇

1 1

) = (

2

) ( )

2

Dove d è l’interasse pari a 12,5 cm e r è il raggio delle ruote (27 mm). Proseguendo:

( ) = −

2 (

2

2

) (

cos

1

sin

sin

1

̇

cos ) (

̇

)

Applicando la funzione del potenziale attrattivo avremo due equazioni che imposteranno le velocità

dei motori in base alla distanza dal goal.

{

= − ((

2

) ( + ) + ( + 2 ) ( + ))

= − (( + ) ( + ) + (

2

2

) ( + ))

Per il potenziale repulsivo si segue lo stesso procedimento, applicato in questo caso alla funzione:

() =

1

2

(

1

1

()

) 2

() = (

1

1

() ) 2

()

Con questa scelta, la forza repulsiva cresce quando il robot si avvicina, mentre risulta nulla oltre a una certa soglia c da noi impostata. La d i (q) rappresenta la distanza fra il robot e l’ostacolo e viene calcolata come modulo della differenza dei due vettori posizione.

= √( ) 2 + ( ) 2

Si ricavano in tal modo le relazioni di ω R , ω L nel caso repulsivo. In questo modo ω R e ω L vengono assegnate senza però tenere conto dell'orientamento : l'algoritmo garantisce infatti il raggiungimento del goal senza che sia possibile impostare l'orientamento del robot nel momento in

cui lo raggiunge.

Conclusioni Il robot è in grado di uscire da un quadrato e seguire correttamente la linea nera su sfondo bianco (sfruttando due controllori PID in commutazione) finché un input non fa capire al robot che questa modalità è terminata e si passa al secondo task. Dal momento in cui finisce il line following, il robot rimane in attesa di un segnale dal sistema centrale: il codice si comporta come un handler che viene eseguito non appena viene rilevato un messaggio bluetooth (interrupt contenente i vettori posizione

di ogni marker). Il codice si occupa quindi di applicare sempre il potenziale attrattivo e di

aggiungere il contributo dei potenziali repulsivi quando il robot si ritrova ad essere entro una

distanza c da essi (per ogni ostacolo viene calcolata la distanza dal robot, il potenziale repulsivo relativo e le modifiche necessarie alle velocità delle ruote). Una volta trovato il potenziale totale risultante, le velocità della ruota destra e sinistra (ω R , ω L ) vengono applicate ai motori grazie alla funzione AzionaMotori().