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 allinterno 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 linizio del secondo task. Il robotin seguito ricever un segnale bluetooth
trasmesso da una telecamera in grado di rilevare, tramite lutilizzo di marker posti sopra di essi, la
posizione corrente del veicolo, dellobbiettivo 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 unulteriore 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 lostacolo 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
allalternanza 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 unazione 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.A.setSpeed(250);
Motor.B.setSpeed(20);
Motor.A.forward();
Motor.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 leffetto 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 lintegrale 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 (

( )
) =

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
1,9584 1075,78 450 gradi/sec
Rettilineo 91.8
128.52 2,7418 1506,1 400 gradi/sec
Curva
Lazione 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.

publicstaticvoidAzionaMotori(){
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.setSpeed(omega_B);
Motor.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. Allobbiettivo 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 dallobbiettivo 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 alluniciclo, si definisce un punto b
posto al di fuori dellasse 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

( )=( 1
sin

sin

1
) ( ) = (2

cos

2 ) ( )

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

( ) = 2(

cos

2) ( 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
1
1 2
1
1 2
() = (
)
() = (
) ()
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 di(q) rappresenta la distanza fra il robot e lostacolo 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().