UNIVERSITÀ DEGLI STUDI DI ROMA
TOR VERGATA
FACOLTÀ DI INGEGNERIA
CORSO DI LAUREA IN INGEGNERIA INFORMATICA
A.A. 2011/2012
Tesi di Laurea
CONTROLLO DI UN ROBOT
MEDIANTE SISTEMA DI ACQUISIZIONE AD
INFRAROSSI
RELATORE
CANDIDATO
Daniele Carnevale
Giuseppe Ferrò
Alla mia famiglia.
Indice
Ringraziamenti
1
Introduzione
2
1 Descrizione degli Strumenti Utilizzati
4
1.1
Controller della Wii . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4
1.2
Posizionamento dei Led . . . . . . . . . . . . . . . . . . . . . . . . . .
5
1.3
Scorbot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6
2 Struttura Stereoscopica
10
2.1
Stereocalibrazione . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
11
2.2
Ordinamento dei Led . . . . . . . . . . . . . . . . . . . . . . . . . . .
12
2.3
Stereotriangolazione
. . . . . . . . . . . . . . . . . . . . . . . . . . .
18
2.4
Filtro di Kalman . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
19
3 Controllo del manipolatore
25
3.1
Connessione LAN . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
25
3.2
Controllo dello Scorbot . . . . . . . . . . . . . . . . . . . . . . . . . .
27
3.3
Interfacciamento con Processing . . . . . . . . . . . . . . . . . . . . .
30
4 Conclusioni e sviluppi futuri
INDICE
34
I
INDICE
Appendice A - Prodotti matriciali per il filtro di Kalman
36
Elenco delle figure
41
INDICE
II
Ringraziamenti
Vorrei innanzitutto ringraziare l’Ing. Daniele Carnevale per la disponibilità e la prontezza nel consigliare, correggere e risolvere ogni problema.
Ringrazio inoltre tutti gli amici che rendono divertenti anche i momenti peggiori e
sentito grazie va a Paolo di Gregorio per avermi consigliato e aiutato all’inizio della
tesi e per avermi fornito con molta pazienza di tutto il software necessario.
Un ringraziamento particolare va inoltre alla mia famiglia che mi sostiene sempre.
Introduzione
1
Introduzione
Fin dai tempi più remoti l’uomo ha costruito macchine via via sempre più sofisticate
che gli hanno permesso di affrontare la vita più facilmente. Recentemente l’esplosione
dell’informatica ha reso possibile esplorare e studiare campi che soltanto cinquant’anni
fa sembravano irraggiungibili. Lo sviluppo dei circuiti integrati e della microelettronica ha permesso la costruzione di sistemi di calcolo sempre più veloci e performanti che
sul piano della velocità computazionale hanno ormai superato di gran lunga le capacità del cervello umano. Le nuove tecnologie hanno inoltre aperto la strada al mondo
della robotica e dell’automazione. La robotica viene infatti utilizzata nei più svariati campi: medico, aereospaziale, industriale, ecc. L’automazione nelle industrie ha
permesso maggiore velocità di produzione e riduzione della manodopera, la robotica
nella medicina rende possibili operazioni di precisione microscopica che trascendono
la capacità umana e, laddove per un uomo diventa impossibile o estremamente pericoloso operare, le macchine si rendono particolarmente utili.
In questo lavoro è stato utilizzato un manipolatore a cinque gradi di libertà (scorbot)
ed è stato controllato per replicare in realtime i movimenti del mio braccio. Per far
questo sono stati applicati sul braccio quattro led infrarossi: uno sulla spalla, uno sul
gomito, uno sul polso e uno sulla punta del dito; i led vengono poi rilevati e triangolati
da una struttura stereoscopica e successivamente vengono calcolati gli angoli di giunto
necessari al controllo del manipolatore. E’ stato quindi necessario:
Introduzione
2
Introduzione
• Rilevare i led tramite due controller della wii (console di gioco della Nintendo)
• Implementare un algoritmo di mappatura dei led tra articolazione del braccio
umano e giunto robotico
• Triangolare i led in un sistema di riferimento 3D inerziale
• Calcolare gli angoli di giunto
• Creare una connessione lan con il computer in grado di controllare il manipolatore
• Trasformare gli angoli di giunto in encoder per il controllo dello scorbot
La tesi è strutturata nel seguente modo:
1. Nel primo capitolo viene data una descrizione piuttosto tecnica degli strumenti
che sono stati utilizzati. Sono quindi descritti il funzionamento e le prestazioni
dei controller della wii e dello scorbot e definito il corretto posizionamento dei
led sul braccio per ottimizzare l’algoritmo di mappatura.
2. Nel secondo capitolo viene analizzato dettagliatamente il lavoro svolto sulla
struttura stereoscopica: il posizionamento dei controller della wii, la stereocalibrazione, l’algoritmo di riconoscimento dei led, il filtro di kalman necessario a
ridurre errori di misura, la triangolazione dei led in un sistema di riferimento
3D inerziale e infine il calcolo degli angoli di giunto.
3. Nel terzo capitolo viene spiegato come è stata realizzata la connessione LAN
per trasferire gli angoli di giunto al pc collegato allo scorbot e le accortezze
necessarie al corretto posizionamento del manipolatore.
Introduzione
3
Capitolo 1
Descrizione degli Strumenti
Utilizzati
In questa sezione verrà descritta l’attrezzatura che è stata utilizzata
nel lavoro svolto:
• due controller della wii
• quattro led infrarossi da 5 mm
• un portatile dotato di programma WMGUI e librerie
necessarie all’interfacciamento con i controller wii.
• un pc del laboratorio di robotica e il relativo Scorbot ad esso
collegato.
• una penna bluetooth usb e un cavo ethernet
1.1
Controller della Wii
Un controller della wii è molto simile ad un telecomando e al suo interno contiene
sensori ottici e accelerometri che lo rendono in grado di rilevare movimenti. E’ inoltre
dotato di un sensore camera a infrarossi in grado di individuare fino a un massimo di
quattro led in movimento con una risoluzione di 128x96 pixel che diventano 1024x768
sfruttando l’analisi subpixel 8x. Rispetto alle normali telecamere infrarossi che raggiungono un tempo di refresh intorno ai 30 Hz, il controller wii arriva fino a 100 Hz
4
Cap. 1 Descrizione degli Strumenti Utilizzati
§1.2 Posizionamento dei Led
offendo un tempo di risposta molto più veloce. Il campo visivo è di 41◦ sul piano
orizzontale e 31◦ per quello verticale. Nel lavoro svolto sono stati utilizzati due controller wii collegati via bluetooth ad un portatile con sistema operativo linux. Per
interfacciare i controller con il programma sul pc è stato quindi necessario utilizzare
una pennetta usb bluetooth e installare su linux le librerie necessarie al funzionamento
della struttura stereoscopica.
Figura 1.1: Un controller wii.
1.2
Posizionamento dei Led
Come già descritto nel paragrafo precedente un controller wii può rilevare fino a un
massimo di 4 led. Nella fattispecie sono stati utilizzati 4 led IR da 5 mm alimentati
ad una tensione di circa 3 Volt. Perchè fosse possibile calcolare gli angoli di giunto da
passare allo scorbot, i led sono stati posizionati in prossimità delle giunture principali
del braccio quindi su spalla, gomito, polso e punta dell’indice. Purtroppo, anche a
causa dei movimenti del braccio, molto spesso alcuni led non venivano rilevati dalle
telecamere anche se lievemente inclinati. Poichè per ottenere la completa configurazione del braccio si ha bisogno di tutti e quattro i led, il fatto che anche per piccole
5
Cap. 1 Descrizione degli Strumenti Utilizzati
§1.3 Scorbot
inclinazioni questi non venissero rilevati ha creato uno spinoso problema. Si è cercato
di superarlo utilizzando dei marker illuminati da una lampada di led ma i controller
non sono riusciti a rilevarli forse perchè la luce riflessa era troppo debole per i loro
sensori IR. Nel lavoro svolto ci si è dovuto accontentare degli inaffidabili led, ma nulla
vieta l’utilizzazione altri metodi, magari più efficaci, per permettere ai controller wii
di ricevere al meglio le posizioni di spalla, gomito, polso e mano.
Figura 1.2: Collegamento in parallelo dei led.
1.3
Scorbot
Lo SCORBOT-ER è un robot a 5 gradi di libertà fornito di una pinza motorizzata
come strumento di presa. I cinque giunti che lo compongono sono messi in movimento
da motori in corrente continua dotati di rapporti di riduzione. Con riferimento alla
figura 1.3 i movimenti di BASE, SHOULDER ed ELBOW sono gestiti da singoli
motori, viceversa i movimenti di PITCH e ROLL sono gestiti da due motori accoppiati
che realizzano il movimento di puro ROLL se girano in fase, di puro PITCH se girano
in controfase (figura 1.4).
L’informazione relativa alla posizione angolare del giunto è ottenuta tramite encoders
di tipo incrementale montati in asse con l’albero di ciascun motore. Su ogni link c’è
un microinterruttore che si attiva quando l’angolo del giunto è in una posizione nota;
questo serve allo scorbot per trovare la posizione di HOME in modo che possa azzerare
6
Cap. 1 Descrizione degli Strumenti Utilizzati
§1.3 Scorbot
Figura 1.3:
Figura 1.4:
i contatori degli encoders in una configurazione fissa, prima che venga effettivamente
utilizzato. Lo Scorbot è collegato alla propria unità di potenza che a sua volta è
collegata al computer. L’unità di potenza si occupa di ricevere segnali dal computer
e controllare di conseguenza i motori sui giunti.
Sullo scorbot utilizzato è implementato un sistema di controllo dei motori DC di
tipo proporzionale. In sostanza viene quindi applicata una tensione proporzionale
all’errore ossia alla differenza tra la posizione voluta e la posizione corrente (misurata
in encoders). In figura 1.5 viene rappresentato lo schema a blocchi del sistema di
controllo in retroazione dove con P si intende la funzione di trasferimento del motore,
con v la posizione angolare desiderata e con y la posizione angolare corrente. K è la
costante che rappresenta il controllore proporzionale.
La funzione di trasferimento di un motore DC è del tipo:
θ(s) =
s(τm τa
s2
Km K a
V (s)
+ (τm + τa )s + Ke + 1)
(1.3.1)
7
Cap. 1 Descrizione degli Strumenti Utilizzati
§1.3 Scorbot
Figura 1.5:
dove, definendo con F la costante d’attrito e con J il momento d’inerzia, si ha Km =
e τm =
J
F
1
F
e, in riferimento alla figura 1.6 (che rappresenta lo schema elettrico di un
motore DC), τa =
La
Ra
e Ka =
1
.
Ra
Ke viene detta costante elettrica ed è propria
di ogni motore. Dato un processo di questo tipo e tenendo conto del fatto che τa è
trascurabile rispetto a τm , un controllore proporzionale con una costante K non troppo
elevata garantisce poca o nulla sovraelongazione e buona velocità di convergenza.
Figura 1.6:
8
Cap. 1 Descrizione degli Strumenti Utilizzati
§1.3 Scorbot
Figura 1.7: Un esemplare di Scorbot.
9
Capitolo 2
Struttura Stereoscopica
In questo capitolo verrà descritto il funzionamento del vicon a infrarossi e il procedimento che permette di rilevare, mappare e filtrare
le posizioni dei led posizionati sul braccio umano.
Per permettere al programma WMGUI precedentemente descritto di riconoscere, ordinare e calcolare le coordinate nello spazio dei 4 led, bisogna dapprima effettuare
due operazioni: stereocalibrazione e stereotriangolazione. Tramite la calibrazione è
possibile reperire le informazioni necessarie per effettuare successivamente la triangolazione dei led posizionandoli all’interno dello spazio. Infatti, una sola telecamera non
è in grado di misurare la profondità spaziale, ma se accoppiata ad un altra è possibile
ottenere le informazioni necessarie per calcolare univocamente le coordinate dei led
in un sistema di riferimento inerziale tridimensionale. In particolare ogni telecamera
vede i led su uno spazio bidimensionale e non riesce a percepirne la profondità senza l’ausilio di una seconda telecamera che, posizionata diversamente e con un piano
immagine proprio, vede i led da un altro punto di vista. Intersecando le informazioni
percepite dai piani immagini di entrambe le telecamere è possibile quindi calcolare la
profondità di ogni led.
10
Cap. 2 Struttura Stereoscopica
§2.1 Stereocalibrazione
Un sistema di visione stereoscopica è caratterizzato da parametri intrinseci ed estrinseci e solamente conoscendo tali parametri è possibile ricavare la posizione dei led in
un sistema di riferimento inerziale.
parametri intrinseci: sono quei parametri che trasformano le coordinate spaziale
nelle coordinate sul piano immagine di ogni telecamera. Solitamente tali parametri sono noti a priori e dipendono dagli strumenti utilizzati: distanza focale,
distorsione delle lenti, ecc.
parametri estrinseci: sono quei parametri che devono essere calcolati tramite la
stereocalibrazione. Comprendono le coordinate delle telecamere in un sistema
di riferimento noto.
Durante la stereocalibrazione vengono determinati tutti questi parametri ed è quindi
possibile procedere con la stereotriangolazione.
2.1
Stereocalibrazione
La stereocalibrazione è stata effettuata posizionando i led sui vertici di un quadrato
di lato 180 cm. Le telecamere infrarossi, ossia i controller wii, sono stati posizionati
ad una distanza tra loro di circa 50cm. Le posizioni relative delle telecamere sono
state decise prima della calibrazione e non possono essere cambiate successivamente
a meno di ricalibrare il sistema (altrimenti vengono a cambiare i parametri estrinseci
rendendo la stereotriangolazione errata). Tramite il programma WMGUI sono state
registrate in un file di testo circa 50 posizioni diverse dei 4 led nel piano immagine
di entrambe le telecamere. Per realizzare al meglio la calibrazione è stato necessario
registrare le posizioni dei led a diverse distanze dalle telecamere e in diversi orientamenti facendo attenzione a non perdere l’etichettatura dei led. Infatti se qualche led
11
Cap. 2 Struttura Stereoscopica
§2.2 Ordinamento dei Led
non viene visto dalla telecamera anche per piccoli intervalli temporali, il programma
li riordina in modo sbagliato e questo rende sia le informazioni della calibrazione, sia
successivamente le posizioni da cui (tramite triangolazione) si calcolano le coordinate
spaziali, errate.
Una volta registrate in due file (uno per ogni telecamera) un numero sufficiente di
posizioni diverse dei 4 led, tramite il camera calibration toolbox in matlab è stato
possibile creare il file definitivo di stereocalibrazione. Questo file verrà poi utilizzato
nel programma (grazie all’invocazione di funzioni matlab) per effettuare la stereotriangolazione dei led e ricavarne le coordinate x, y, z in millimetri in un sistema di
riferimento inerziale.
Figura 2.1: Risultato della calibrazione.
2.2
Ordinamento dei Led
Una volta effettuata la stereocalibrazione si hanno tutte le informazioni necessarie
per il calcolo delle coordinate spaziali dei led ed è quindi possibile procedere con la
stereotriangolazione.
Per effettuare la triangolazione è necessario che il programma sia in grado di riconoscere quale sia il led relativo a spalla, gomito, polso e dito su entrambi i piani
12
Cap. 2 Struttura Stereoscopica
§2.2 Ordinamento dei Led
immagine. Per far questo è stato realizzato un apposito algoritmo di mappatura dei
led capace di riordinarli in modo corretto a patto che si conosca a priori soltanto la
posizione del led relativo alla spalla.
In particolare è necessario che inizialmente il braccio sia in una posizione tale che il
led sulla spalla si trovi a sinistra (vista dalle telecamere) rispetto agli altri led. Tenendo conto di questa accortezza, che rappresenta anche una naturale posizione di
home, ad ogni iterazione il led relativo alla spalla viene riconosciuto come il led che
ha coordinate bidimensionali più vicine alle coordinate precedenti della spalla.
In questo modo l’algoritmo, utilizzando particolari proprietà naturali di ogni braccio
umano, è in grado di riconoscere in modo corretto le coordinate di gomito polso e
mano. In particolare definendo come avambraccio la distanza spalla-gomito, come
braccio la distanza gomito-polso e come mano la distanza polso-dito (l’ultimo led viene posizionato sulla punta dell’indice), se i led sono posizionati in modo corretto il
braccio è più lungo sia dell’avambraccio sia della mano.
Di seguito sono elencati in dettaglio i passi dell’algoritmo
1. Per ogni telecamera il programma rileva le posizioni sul piano immagine dei 4
led e ne registra le coordinate (x, y) in due array (uno per ogni telecamera)
di 4 elementi ciascuno. L’algoritmo si applica separatamente per i due array,
ordinando dapprima i led nell’array relativo alle posizioni registrate dalla prima telecamera e successivamente viene utilizzato sull’array relativo alla seconda
telecamera. La funzione riceve due array, uno relativo alle coordinate dell’iterazione corrente, uno relativo alle coordinate della precedente iterazione, questo
per far si che si possa riconoscere la posizione della spalla come quella più vicina
alla spalla dell’iterazione precedente.
13
§2.2 Ordinamento dei Led
Cap. 2 Struttura Stereoscopica
2. La funzione riceve le coordinate appena rilevate e le coordinate relative alla
precedente iterazione. Le coordinate precedenti sono già ordinate quindi la
spalla si trova nella prima posizione dell’array; l’array relativo alle posizioni
appena rilevate viene scandito trovando la posizione più vicina (come distanza
euclidea) alla spalla della precedente iterazione, ottenendo le coordinate della
nuova spalla.
3. A questo punto vengono calcolate le 6 distanze tra i 4 punti rilevati e vengono
messe in un array. Ogni elemento dell’array è una struttura che registra il valore
della distanza e i punti estremali del segmento. L’array delle distanze viene poi
ordinato dalla distanza più breve a quella più lunga.
Figura 2.2:
4. Viene scandito l’array delle distanze e separate quelle che hanno un punto estremale uguale alla spalla. Le distanze passano cosı̀ da sei a tre e le tre che hanno
come estremo la spalla diventano i candidati avambracci. A questo punto si parte dal candidato avambraccio più corto e si esaminano le tre distanze rimaste
alla ricerca di un candidato braccio che sia più lungo dell’avambraccio e abbia un
punto estremale uguale a un punto estremale dell’avambraccio (il gomito). Se
la ricerca va a buon fine si ottiengono un candidato avambraccio e un candidato
braccio che rispettano le proporzioni giuste di un arto umano. Rimane quindi
solamente la mano da scegliere tra due distanze: una (gomito-dito) maggiore
14
§2.2 Ordinamento dei Led
Cap. 2 Struttura Stereoscopica
Figura 2.3:
del braccio e una (polso-dito) minore del braccio. Poichè la mano è più corta
del braccio la scelta è ovvia e quindi viene scelta la distanza polso-dito.
Figura 2.4:
Se ad esempio scelto un candidato avambraccio non si riesce a trovare un candidato braccio idoneo, la ricerca continua passando al candidato avambraccio
successivo procedendo in modo ricorsivo finchè non vengono trovate 3 distanze
collegate tra loro che rispettano le giuste proporzioni di avambraccio, braccio e
mano.
5. Trovata la giusta configurazione la funzione riconosce correttamente le posizioni
di gomito polso e mano dai punti estremali delle distanze e restituisce l’array
perfettamente riordinato.
Il codice che implementa il riconoscimento dei led è il seguente:
int riconosciLed(struct coord* coordinate, struct coord* oldcoords,int flag){
struct dista distanza[6];
int i=0,j=0,b=0;
while(i<4){j=i+1;
while(j<4){
distanza[b].dist=calcolaDistanza(coordinate[i],coordinate[j]);
distanza[b].point0=i;
distanza[b].point1=j;
15
§2.2 Ordinamento dei Led
Cap. 2 Struttura Stereoscopica
Figura 2.5:
b++;
j++;}
i++;}
return ordinaPunti(distanza,coordinate,oldcoords,flag);}
Questa è la funzione che viene chiamata ad ogni iterazione dal ciclo principale del
processo per riconoscere i led. Come si nota dal codice viene invocata la funzione
’CalcolaDistanza’ che calcola la distanza euclidea tra due led e la salva in un array:
double
double
double
double
double
return
calcolaDistanza(struct coord coord1, struct coord coord2){
x1=(double)coord1.x;
x2=(double)coord2.x;
y1=(double)coord1.y;
y2=(double)coord2.y;
sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1));}
La funzione che svolge la maggior parte del lavoro è la funzione ’ordinaPunti’ che
prende in ingresso l’array delle distanze tra i led e l’array delle coordinate misurate,
il quale verrà poi restituito correttamente ordinato:
int ordinaPunti(struct dista* distanze, struct coord* coordinate,struct coord* oldcoords, int flag){
struct coord coordtemp[4];
int i=0,x=0,puntopartenza=0,c=0,match=0,cont=0,cont2=0,indexspalla=0,avambracci[6];
double min, candidato;
coordtemp[0]=coordinate[0];
if(flag==0){
while(i<4){
if(coordinate[i].x<coordtemp[0].x){
coordtemp[0]=coordinate[i];
indexspalla=i;}
i++;}}
else{
min=calcolaDistanza(oldcoords[0],coordinate[0]);
while(i<4){
candidato=calcolaDistanza(oldcoords[0],coordinate[i]);
if(candidato<min){
coordtemp[0]=coordinate[i];
min=candidato;
indexspalla=i;}
i++;}}
i=0;
ordinadistanze(distanze);
while(i<6){
16
Cap. 2 Struttura Stereoscopica
§2.2 Ordinamento dei Led
if(distanze[i].point0==indexspalla||distanze[i].point1==indexspalla){
avambracci[x]=i;
x++;}
i++;}
while(cont<x&&match==0){
i=0;
while(i<6&&match==0){
if(i!=avambracci[0]&&i!=avambracci[1]&&i!=avambracci[2]&&
distanze[i].dist>distanze[avambracci[cont]].dist&&
puntiok(distanze[i],distanze[avambracci[cont]])==1){
cont2=0;
while(cont2<6&&match==0){
if(cont2!=i&&cont2!=avambracci[0]&&cont2!=avambracci[1]&&
cont2!=avambracci[2]&&distanze[cont2].dist<distanze[i].dist&&
puntiok(distanze[i],distanze[cont2])==1){
if(distanze[avambracci[cont]].point0!=distanze[i].point0&&
distanze[avambracci[cont]].point0!=distanze[i].point1)
coordtemp[1]=coordinate[distanze[avambracci[cont]].point1];
else
coordtemp[1]=coordinate[distanze[avambracci[cont]].point0];
if(distanze[cont2].point0!=distanze[i].point0&&
distanze[cont2].point0!=distanze[i].point1){
coordtemp[3]=coordinate[distanze[cont2].point0];
coordtemp[2]=coordinate[distanze[cont2].point1];}
else{
coordtemp[3]=coordinate[distanze[cont2].point1];
coordtemp[2]=coordinate[distanze[cont2].point0];}
if(linearcontrol(coordtemp)==1){
while(c<4){
coordinate[c]=coordtemp[c];
c++;
}match=1;
}
}
cont2++;}
}
i++;}
cont++;}
return match;
}
Come si può notare dal codice inizialmente viene invocata la funzione ’ordinadistanze’
la quale si occupa di ordinare le distanze precedentemente calcolate in ordine crescente:
void ordinadistanze(struct dista* distanze){
int i=0,j=0;
struct dista temp;
while(i<6){
j=i+1;
while(j<6){
if(distanze[j].dist<distanze[i].dist){
temp=distanze[i];
distanze[i]=distanze[j];
distanze[j]=temp;}
j++;}
i++;}
}
Successivamente vengono chiamate anche altre funzioni: alcune necessarie alla mappatura dei led altre per evitare una serie di altri errori. In particolare la funzione
17
Cap. 2 Struttura Stereoscopica
§2.3 Stereotriangolazione
’puntiok’ prende in ingresso due distanze e controlla che abbiano un estremo in comune: questo serve per scegliere candidati bracci fissato un avambraccio e candidate
mani fissato un braccio. Infine la funzione ’linearcontrol’ fa un controllo sulle coordinate riordinate che ha successo solamente se i 4 led sono in diverse posizioni. Questo
perchè alcune particolari configurazioni del braccio porterebbero ad un errato riconoscimento dei led, ma fortunatamente spesso l’errore è rilevabile perchè in tal caso un
led viene contato due volte (ad esempio se lo stesso led viene riconosciuto sia come
spalla sia come dito).
int puntiok(struct dista distanza1, struct dista distanza2){
if(distanza1.point0!=distanza2.point0&&distanza1.point0!=distanza2.point1){
if(distanza1.point1!=distanza2.point0&&distanza1.point1!=distanza2.point1)
return 0;
else return 1;}
else return 1;}
int linearcontrol(struct coord* coordinate){
int i=0,j=0;
while(i<4){
j=i+1;
while(j<4){
if(coordinate[i].x==coordinate[j].x&&coordinate[i].y==coordinate[j].y)
return 0;
j++;}
i++;}
return 1;}
2.3
Stereotriangolazione
Dopo aver mappato i led sul piano immagine delle telecamere, tramite la stereotriangolazione è stato possibile ottenerne le coordinate in un sistema di riferimento
tridimensionale inerziale. Questo è stato possibile grazie all’interfacciamento di alcune funzioni matlab utilizzate nel codice C tramite l’ inclusione della libreria ’engine.h’. Di seguito viene riportato il codice che effettua la stereotriangolazione e salva
le coordinate x, y, z in millimetri dei quattro led in un array di tipo double.
mxArray *pts1, *pts2;
pts1 = mxCreateDoubleMatrix(2, count, mxREAL);
pts2 = mxCreateDoubleMatrix(2, count, mxREAL);
double pts1_pr[2 * count];
double pts2_pr[2 * count];
int j = 0;
for(int i = 0; i < 4; i++) {
if(coords[i].x != -1 || coords[i].y != -1 || coords2[i].x != -1 || coords2[i].y != -1) {
pts1_pr[j] = coords[i].x;
18
§2.4 Filtro di Kalman
Cap. 2 Struttura Stereoscopica
pts1_pr[j+1] = coords[i].y;
pts2_pr[j] = coords2[i].x;
pts2_pr[j+1] = coords2[i].y;
j += 2;
}
}
memcpy(mxGetPr(pts1), pts1_pr, 2 * count * sizeof(double));
memcpy(mxGetPr(pts2), pts2_pr, 2 * count * sizeof(double));
mxArray *XL = NULL;
mxArray *XR = NULL;
mlfStereo_triangulation(2, &XL, &XR,pts1,pts2,om,T,fc_left,cc_left,kc_left,
alpha_c_left,fc_right,cc_right,kc_right,alpha_c_right);
double *XL_pr = mxGetPr(XL);
2.4
Filtro di Kalman
Il passo successivo alla stereotriangolazione dei led è stato quello di implementare il
filtro di Kalman, un filtro molto potente in grado di stimare lo stato in caso di rumori
gaussiani a media nulla.
Per implementare il filtro di Kalman si ha bisogno di conoscere la dinamica del processo e le matrici di covarianze dei rumori sia nello stato che nell’uscita. Il filtro
agisce poi come un algoritmo iterativo a cui viene dato in pasto lo stato stimato e le
misure e volta per volta calcola la stima ottimale dello stato (secondo un indice di
costo quadratico) aggiornando le proprie variabili.
Per prima cosa è stato necessario definire la dinamica del processo considerato. Trattandosi di un immagine campionata dalle telecamere, il sistema considerato è di tipo
discreto; inoltre è stata supposta l’assenza di un ingresso forzante u(k). Definendo
quindi con x(k) la posizione del led al tempo k (tramite le coordinate spaziali x1 , x2
e x3 ) e con x(k+1) la posizione del led al tempo di campionamento immediatamente
successivo, è possibile descrivere il passaggio dallo stato x(k) allo stato x(k+1) tramite
la seguente equazione:
x(k + 1) = x(k) + v(k)T
(2.4.1)
19
§2.4 Filtro di Kalman
Cap. 2 Struttura Stereoscopica
dove v(k) è il vettore di velocità del led al passo k e T è il tempo di campionamento.
v(k) è quindi pari a:
v(k) =
x(k) − x(k − 1)
T
(2.4.2)
Sostitutuendo la 2.4.2 nella 2.4.1 si ottiene:
x(k + 1) = 2x(k) − x(k − 1)
(2.4.3)
Tale modello ha dato nella pratica risultati più che accettabili ma è stato necessario
considerare lo stato precedente oltre a quello corrente. Il sistema si può ricondurre
alla forma classica di un sistema lineare stazionario:
x(k + 1) = Ax(k) + Bu(k)
(2.4.4)
y(k) = Cx(k) + Du(k)
(2.4.5)
Definendo x(k), A e C nel seguente modo (B e D sono irrilevanti essendo u(k) nullo):




A:



2
0
0
1
0
0
0
2
0
0
1
0


0 −1 0
0
x1 (k)
 x2 (k)


0 0 −1 0 


1 0 0 0 0 0

2 0
0 −1 
 C:  0 1 0 0 0 0  x(k):  x3 (k)
 x1 (k − 1)
0 0
0
0 


0 0 1 0 0 0

 x2 (k − 1)
0 0
0
0
1 0
0
0
x3 (k − 1)








Ora che è stato definito il modello del sistema dinamico considerato è possibile
descrivere il funzionamento del filtro di Kalman.
• Vengono definite le matrici di covarianza dei rumori gaussiani a media nulla. Si
definisce con Qk (semidefinita positiva) la matrice di covarianza dei rumori che
agiscono sullo stato e con Rk (definita positiva) quella dei rumori che agiscono
sull’uscita.
20
§2.4 Filtro di Kalman
Cap. 2 Struttura Stereoscopica




Qk : 



10 0 0 0 0 0
0 10 0 0 0 0
0 0 10 0 0 0
0 0 0 10 0 0
0 0 0 0 10 0
0 0 0 0 0 10





1
0
0

 Rk :  0 1 0 


0 0 1

• Si ricava uno stato stimato grazie alla conoscenza del sistema dinamico e delle
misure. Definendo con x(k − 1)+ lo stato precedente, lo stato stimato x(k)− si
ottiene quindi in tal modo:
x(k)− = Ax(k − 1)+
(2.4.6)
• Viene aggiornata la covarianza dell’errore a priori. Definendo infatti con e−
k
l’errore di stima a priori e con ek l’errore di stima a posteriori definiti dalle
seguenti relazioni:
−
e−
k = x(k) − x(k)
(2.4.7)
+
e+
k = x(k) − x(k)
(2.4.8)
si possono definire la covarianza dell’errore a priori Pk− e la covarianza dell’errore
a posteriori Pk . In questo passo dell’algoritmo viene calcolata Pk− che si ricava
dalla seguente espressione:
Pk− = APk−1 A0 + Qk−1
(2.4.9)
In questa espressione Qk−1 rappresenta la matrice di covarianza dell’errore sullo
stato. Nel lavoro svolto sia Qk sia Rk sono state considerate costanti ma in
generale, come mostrato nella precedente espressione, potrebbero variare nel
tempo.
21
§2.4 Filtro di Kalman
Cap. 2 Struttura Stereoscopica
• Viene calcolato il guadagno di Kalman Kk , determinato in modo da rendere
minima la covarianza dell’errore a posteriori Pk .
Kk = Pk− C 0 (CPk− C 0 + Rk )−1
(2.4.10)
• Si può ricavare il valore dello stato ottimo x(k)+ dalla seguente espressione che
utilizza il guadagno di Kalman Kk , lo stato stimato x(k)− e l’uscita misurata
y(k):
x(k)+ = x(k)− + Kk (y(k) − Cx(k)− )
(2.4.11)
• Viene infine calcolato il valore della covarianza a posteriori (che verrà utilizzata
nella prossima iterazione come covarianza a priori) tramite l’espressione:
Pk = (I − Kk C)Pk−
(2.4.12)
Nel lavoro svolto il filtro di Kalman è stato applicato in real time sulle coordinate 3D
dei led a valle della stereotriangolazione. Di seguito viene mostrato il codice C che
implementa il filtro.
void kalman3D(double* misurate, double* previste){
double t1[6][6],A3DT[6][6],t2[6][6],Pk3Dstimato[6][6],
t3[3][6],C3DT[6][3],t4[3][3],t5[3][3],t6[6][3],Kk3D[6][3];
double mul=0,corrtemp=0;
int x=0,e=0,d=0,c=0;
Prodotto6x6(6,A3D,Pk3D,t1);
Transpose3D(A3D,A3DT);
Prodotto6x6(6,t1,A3DT,t2);
Somma3D(t2,Qk3D,Pk3Dstimato,1);
Prodotto6x6(3,C3D,Pk3Dstimato,t3);
Transpose13D(C3D,C3DT);
Prodotto6x3(3,t3,C3DT,t4);
Somma13D(t4,Rk3D,t5);
if(Inverse3D(t5,t4)!=-1){
Prodotto3x3(6,C3DT,t4,t6);
Prodotto6x3(6,Pk3Dstimato,t6,Kk3D);
while(x<4){
e=0;
while(e<3){
d=0;
mul=0;
while(d<3){
mul+=Kk3D[e][d]*(misurate[d+x*3]-previste[d+x*3]);
d++;}
misurate[c]=previste[c]+mul;
e++;
22
Cap. 2 Struttura Stereoscopica
§2.4 Filtro di Kalman
c++;}
while(e<6){
d=0;
corrtemp=0;
while(d<3){
corrtemp+=Kk3D[e][d]*(misurate[d+x*3]-previste[d+x*3]);
d++;}
correnti[c-3]=correnti[c-3]+corrtemp;
e++;
c++;}
x++;
c=x*3;}
Prodotto3x6(6,Kk3D,C3D,t1);
Somma3D(I3D,t1,t2,0);
Prodotto6x6(6,t2,Pk3Dstimato,Pk3D);
}
}
Come si nota dal codice, la funzione che implementa il filtro di Kalman prende in
ingresso le coordinate stimate dei led calcolate utilizzando la conoscenza del sistema
dinamico e le coordinate effettivamente misurate. Successivamente tramite funzioni
che implementano i prodotti matriciali necessari a implementare l’algoritmo (riportate
in Appendice A), restituisce l’array delle coordinate filtrate. Le coordinate stimate dei
led vengono calcolate tramite la funzione:
void CalcolaStimate3D(double* stima){
int a=0;
while(a<12){
stima[a]=2*correnti[a]-precedenti[a];
a++;}
}
Per far vedere l’effetto del filtro sulle misure delle coordinate dei led viene riportato
un esempio in cui si possono vedere le coordinate (in millimetri) non filtrate e le coordinate filtrate con Kalman:
Coordinate misurate ma non ancora filtrate:








−264.392151
−192.378594
10.087395
54.803910
Led 1:  −84.615574  Led 2:  −24.525432  Led 3:  −158.891130  Led 4:  −317.333983 
1340.932703
1335.249945
1347.946567
1370.781757
Dopo aver applicato il filtro diventano:








−273.260679
−198.870609
10.425758
56.642202







−87.453841
−25.352748
−164.220828
−327.978343 
Led 1:
Led 2:
Led 3:
Led 4:
1385.911721
1380.308093
1393.160850
1416.762004
23
§2.4 Filtro di Kalman
Cap. 2 Struttura Stereoscopica
Una volta applicato il filtro sulle coordinate dei led è stato necessario ricavare gli
angoli tra i giunti che verranno poi passati allo scorbot. La funzione riportata di seguito utilizza la funzione atan2 per calcolare: angolo di imbardata, angolo tra spalla
e gomito, angolo tra gomito e polso e infine angolo tra polso e dito (pitch). Successivamente gli angoli vengono convertiti da radianti a gradi e sono pronti per essere
trasferiti al programma di controllo dello scorbot. In figura 2.6 si ha un esempio di
come viene calcolato un angolo di giunto a partire dalle coordinate dei led.
void calcolaAngoli(double* punti, double* radianti){
radianti[0]=atan2(punti[5]-punti[2],punti[3]-punti[0]);
radianti[1]=atan2(punti[4]-punti[1],sqrt((punti[5]-punti[2])*(punti[5]-punti[2])+
(punti[3]-punti[0])*(punti[3]-punti[0])));
radianti[2]=atan2(punti[7]-punti[4],sign(punti[6]-punti[3])*
sqrt((punti[8]-punti[5])*(punti[8]-punti[5])+(punti[6]-punti[3])*(punti[6]-punti[3])));
radianti[3]=atan2(punti[10]-punti[7],sign(punti[9]-punti[6])*
sqrt((punti[11]-punti[8])*(punti[11]-punti[8])+(punti[9]-punti[6])*(punti[9]-punti[6])));
Figura 2.6:
θ(gomito/polso) = atan2(Y polso − Y gomito,
q
(Xgomito − Xpolso)2 + (Zgomito − Zpolso)2 )
(2.4.13)
24
Capitolo 3
Controllo del manipolatore
In questo capitolo verrà descritto il procedimento che permette, a
partire dagli angoli calcolati dai led, di posizionare lo scorbot nel
modo corretto. Inoltre si spiegherà in dettaglio come è stata realizzata la connessione lan per il controllo in remoto e come avviene lo
scambio di dati
3.1
Connessione LAN
Per trasferire i dati dal computer collegato al sistema stereoscopico al computer collegato allo scorbot è stata utilizzata una connessione LAN. Questo tipo di soluzione
per lo scambio di dati tra host si è rivelato incredibilmente performante ed efficace: i
ritardi sono risultati praticamente nulli rendendo la manipolazione del robot in realtime veloce e fluida. Inoltre grazie a questo tipo di collegamento si ha la possibilità
di controllare lo scorbot in remoto anche a grandi distanze, anche se questo potrebbe
ovviamente portare a ritardi magari non più cosı̀ trascurabili come nel caso sperimentato (i due computer erano nella stessa stanza). E’ stata utilizzata una connessione di
tipo client-server utilizzando TCP come protocollo al livello di trasporto: il computer
collegato allo scorbot viene utilizzato come server e può essere lasciato perennemente acceso, mentre il computer collegato alla struttura stereoscopica svolge il ruolo di
25
Cap. 3 Controllo del manipolatore
§3.1 Connessione LAN
client. Il programma di controllo dello scorbot una volta avviato si mette in attesa
che un client si colleghi. Quando questo avviene viene creata una socket sulla quale
può avvenire lo scambio di dati. Quindi ad ogni iterazione il client deve: rilevare
i led, mapparli, triangolarli, filtrarne le misure, calcolare gli angoli e infine inviarli
sulla socket appena creata. Nel frattempo sul server parte il loop principale del programma, che riceve di volta in volta un array di 4 angoli dal client e li imposta allo
scorbot come angoli di giunto. In questo modo lo scorbot si posiziona con gli angoli di
giunto uguali agli angoli formati dai led, con l’effetto di riprodurre il movimento del
braccio umano. Una volta che un client si disconnette, il server è pronto ad accettare
nuove connessioni e si mette quindi in attesa. Di seguito viene riportato il codice che
implementa connessione e scambio di dati sia sul client sia sul server.
Lato Server
La seguente funzione fa in modo che il processo server si metta in attesa di connessioni sulla porta 4444. Se un client si collega, il server crea una socket attraverso
la quale avviene lo scambio dati.
if (ROBOTPowerON())
{
struct sockaddr_in server;
sock=socket(AF_INET,SOCK_STREAM,0);
server.sin_family=AF_INET;
server.sin_port=htons(4444);
server.sin_addr.s_addr=INADDR_ANY;
bind(sock,(struct sockaddr*)&server,sizeof(server));
listen(sock,23);
while(1){
if((idsock=accept(sock,NULL,NULL))!=-1)
DemoLoop(idsock); //loop principale del programma
else{
erase();
mvprintw(7,22,"Problemi nella Connessione");
refresh();
getch();}
}
}
Viene poi utilizzata la chiamata read nel loop principale per leggere ad ogni iterazione
dalla socket l’array di angoli. La funzione read ritorna il numero dei byte letti e se
ritorna 0 vuol dire che il client ha chiuso la connessione. Se accade questo anche il
server chiude la connessione ed esce dal loop principale per mettersi in attesa di altre
connessioni.
26
Cap. 3 Controllo del manipolatore
§3.2 Controllo dello Scorbot
if((letti=recv(idsock,angoli,sizeof(angoli),0))>0){
angoli[1]=-angoli[1];
angoli[2]=-angoli[2];
angoli[3]=-angoli[3];
aggiornaMovimento(angoli);}
else{
close(idsock);
break;}
Lato Client
La funzione riportata di seguito permette al processo client di collegarsi al processo server utilizzando la porta 4444. In questo caso per effettuare la connessione è
necessario specificare l’indirizzo IP del server che viene passato come parametro.
int connetti(char* ip){
struct sockaddr_in client;
int lunghezza=sizeof(client);
idsocks[0]=socket(AF_INET,SOCK_STREAM,0);
client.sin_family=AF_INET;
client.sin_port=htons(4444);
inet_aton(ip,&(client.sin_addr));
if(connect(idsocks[0],(struct sockaddr*)&client,sizeof(client))==-1)
return -1;
return 0;
}
Ad ogni iterazione, inoltre, il client invia un array di quattro angoli scrivendoli sulla
socket utilizzando la funzione write:
write(idsocks[0],angoli,sizeof(angoli));
Infine, quando viene chiuso il programma viene anche chiusa la connessione utilizzando la funzione close sul descrittore della socket.
3.2
Controllo dello Scorbot
Il programma di controllo dello Scorbot è caratterizzato da un loop principale che
richiama le funzioni necessarie al movimento del manipolatore. All’interno di questo
loop è stata inserita la funzione read descritta nel paragrafo precedente in grado di
ricevere ad ogni iterazione l’array con gli angoli di giunto.
27
Cap. 3 Controllo del manipolatore
§3.2 Controllo dello Scorbot
Per costruzione, lo scorbot ha un motore per ogni giunto, fatta eccezione per i movimenti di pitch e roll attuati tramite due motori accoppiati che possono girare nello
stesso verso o in versi opposti. Sull’asse di ogni motore sono montati encoders ottici
che misurano la posizione angolare dei giunti e, per configurare la posizione del manipolatore, il programma di controllo dello Scorbot ha bisogno di ricevere in ingresso
per ogni motore la posizione desiderata in encoders. E’ quindi necessario convertire in
passi encoders gli angoli di giunto ricevuti per posizionare in modo corretto il robot.
Inoltre, prima di poter effettivamente utilizzare il manipolatore, è necessario azzerare
gli encoders in una posizione nota, viene quindi effettuata l’operazione di HOME che
pone il robot in una posizione specifica e il valore degli encoders in quella posizione
viene impostato a zero. La posizione di HOME dello scorbot non coincide con la
posizione in cui ogni angolo di giunto è uguale a zero, per cui ogni volta che viene
fornito un valore in encoder che corrisponde all’angolo di giunto, a questo deve essere
sommato il valore costante in encoder che corrisponde all’angolo del giunto rispetto
al riferimento inerziale chiamato HOME (Un esempio in figura 3.1).
Accedendo alla struttura ROBOTMotor definita nell’header file ‘scorb.h’ è possibile ricavare la posizione in encoder relativi ad ogni motore scrivendo RobotMotor[i].encoder dove i è un indice (che va da 0 a 4) che identifica il motore. In questo
modo, dopo aver effettuato la HOME e posizionato il robot nella posizione con angoli
di giunto uguali a zero, sono stati ricavati gli encoders realtivi agli angoli di home,
che devono essere poi sommati agli encoders relativi agli angoli di giunto letti dalla
socket.
Per trasformare gli angoli di giunto ricevuti in passi encoder è stato fatto ruotare ogni
giunto di un angolo noto α (in questo caso 180◦ ) e dopo aver letto i passi encoder
relativi a tale angolo è possibile ricavare facilmente i passi encoder relativi a qualsiasi
28
§3.2 Controllo dello Scorbot
Cap. 3 Controllo del manipolatore
Figura 3.1:
angolo θ:
Encoders(θ) = Encoders(α)
θ
α
(3.2.1)
Per ogni giunto è quindi necessario:
• Trasformare l’angolo ricevuto in passi encoder utilizzando l’espressione 3.2.1
• Sommare ai passi encoder calcolati i valori costanti di home del giunto ottenendo
la posizione in encoder assoluta (rispetto alla posizione di HOME).
• Impostare gli angoli di giunto dello scorbot utilizzando ‘ROBOTDefCtrlPosReq[i]=(encoder assoluti)’ per ogni i che va da 0 a 4, dove encoder assoluti
rappresenta la posizione in encoder rispetto alla home dell’angolo ricevuto. I
giunti si pongono cosi nella posizione desiderata.
29
Cap. 3 Controllo del manipolatore
§3.3 Interfacciamento con Processing
Viene riportata la funzione che svolge i suddetti compiti ed il cui ingresso ad
ogni iterazione è l’array di angoli letto dalla socket.
aggiornaMovimento(double* ang_DH_ins)
{
int base=-ang_DH_ins[0];
int shoulder=ang_DH_ins[1];
int elbow=-(ang_DH_ins[2]);
int pitch=-(ang_DH_ins[3])-90;
ROBOTDefCtrlPosReq[0]=base/gain[0]+ENC1;
ROBOTDefCtrlPosReq[1]=(shoulder/gain[1])+ENC2;
ROBOTDefCtrlPosReq[2]=(elbow/gain[2])+ENC3;
ROBOTDefCtrlPosReq[3]=(pitch/gain[3])+ENC4;
ROBOTDefCtrlPosReq[4]=(-1)*(pitch/gain[3])+ENC5;
}
3.3
Interfacciamento con Processing
Per visualizzare sul client i movimenti del robot o per simulare il funzionamento del
programma in modo virtuale senza effettivamente utilizzare il manipolatore vero e
proprio, il programma di rilevamento dei led wmgui è stato interfacciato ad un programma appositamente creato e scritto in Processing.
Processing è un linguaggio di programmazione basato su Java, che consente di sviluppare diverse applicazioni come giochi, animazioni e contenuti interattivi. Da Java
eredita completamente la sintassi, i comandi e il paradigma di programmazione orientata agli oggetti, ma in più mette a disposizione numerose funzioni ad alto livello per
gestire facilmente l’aspetto grafico e multimediale. È distribuito sotto licenza Open
Source, ed è supportato dai sistemi operativi GNU/Linux, Mac OS X e Windows.
E’ stato quindi utilizzato un programma in Processing raffigurante uno Scorbot virtuale ed è stato interfacciato a wmgui per ricevere gli angoli e posizionarsi nello stesso
modo del braccio umano, proprio come se fosse il manipolatore vero. In figura 3.2 è
riportata una foto dello scorbot virtuale in Processing.
Per far comunicare Processing con wmgui è stata utilizzata una connessione LAN,
30
Cap. 3 Controllo del manipolatore
§3.3 Interfacciamento con Processing
con l’unica differenza che la connessione avviene in locale utilizzando l’indirizzo di
loopback 127.0.0.1. Il ruolo di client viene svolto dal programma in processing mentre
il processo wmgui viene utilizzato come server. Wmgui, ossia il programma collegato
alla struttura stereoscopica, si ritrova cosı̀ ad essere sia client nel caso si debba controllare in remoto uno scorbot vero, sia server se deve accettare una connessione dallo
scorbot virtuale in Processing. Una volta lanciato, wmgui si collega automaticamente
all’indirizzo IP del computer remoto collegato allo scorbot che viene passato come
parametro (se non viene passato alcun parametro si suppone che l’utente non voglia
collegarsi allo scorbot in remoto) e successivamente chiede all’utente se deve mettersi
in attesa di altre connessioni. In caso di risposta positiva wmgui si mette in attesa e
basta lanciare lo scorbot virtuale in Processing, il quale si collega a wmgui generando
cosı̀ una ulteriore socket sulla quale può avvenire lo scambio di dati. In questo modo
wmgui una volta che ha calcolato gli angoli di giunto può inviarli sia sulla socket di
comunicazione con lo scorbot vero e proprio che sulla socket connessa allo scorbot
in Processing. Inoltre il programma in Processing può collegarsi a wmgui anche da
remoto e quindi si possono visualizzare i movimenti dello scorbot virtuale anche su un
altro computer a grande distanza: basta cambiare l’indirizzo IP, che in questo caso è
l’indirizzo locale, con l’IP della macchina sulla quale gira wmgui.
Processing è composto da due funzioni fondamentali: la funzione setup() nella quale vengono inserite le impostazioni iniziali e la funzione draw() che consiste in un
loop che disegna di volta in volta gli oggetti grafici. Per far sı̀ che il programma in
processing effettui la connessione basta scrivere all’interno della funzione setup():
myClient=new Client(this,"127.0.0.1",4444);
dove viene specificato l’indirizzo IP della macchina alla quale ci si vuole collegare e il
numero di porta. Nella funzione draw() invece, il programma riceve di volta in volta
31
§3.3 Interfacciamento con Processing
Cap. 3 Controllo del manipolatore
Figura 3.2:
gli angoli da wmgui grazie a queste istruzioni:
while(i<4){
if(myClient.available()>0){
angolint[i]=myClient.read();
angolint[i]=myClient.read()*256+angolint[i];
data=myClient.read();
data=myClient.read();}
angoli[i]=(angolint[i]/max)*2*PI;
if(angoli[i]>PI)
angoli[i]=angoli[i]-2*PI;
i++;}
Come si può notare dal codice la funzione myClient.read() legge un byte per volta
dalla socket. Si è quindi scelto di far inviare da wmgui quattro interi che rappresentano
i quattro angoli di giunto in gradi. Dato che ogni intero è formato da quattro byte
vengono letti quattro byte per ogni angolo. Inoltre per evitare problemi wmgui invia
soltanto angoli positivi sostituendo ad un eventuale angolo negativo θ l’angolo 2π +θ e
Processing utilizza per ogni angolo solo i primi due byte dei quattro letti (un angolo è
32
Cap. 3 Controllo del manipolatore
§3.3 Interfacciamento con Processing
al massimo 360, ampiamente all’interno dell’intervallo di rappresentazione di 2 byte).
Inoltre se Processing legge un angolo θ maggiore di π lo converte, come è giusto che
sia, nell’angolo negativo corrispondente ossia θ − 2π. Una volta ricevuti gli angoli
di giunto questi vengono utilizzati per disegnare lo scorbot virtuale che quindi si
posiziona nello stesso modo del braccio umano.
33
Capitolo 4
Conclusioni e sviluppi futuri
Il principale obiettivo di questo lavoro era quello di sviluppare un sistema in grado di
far replicare in realtime e con velocità tollerabili i movimenti di un braccio umano ad
un manipolatore robotico. L’obiettivo è stato raggiunto con risultati molto soddisfacenti, l’utilizzo della connessione LAN permette inoltre una manipolazione in remoto
che può risultare molto utile. Il lavoro svolto può essere applicato in molti settori:
• Situazioni in cui bisogna manipolare sostanze pericolose, utilizzando il manipolatore con l’immediatezza e la velocità con cui si utilizza il proprio braccio.
• In campo medico può essere utilizzato da pazienti con arti amputati come arto
sostitutivo. Ovviamente per far questo il sistema sviluppato in questo lavoro
deve essere appositamente modificato.
• Può essere utilizzato per effettuare lavori manuali a grandi distanze grazie alla
connessione LAN.
Il sistema sviluppato presenta tuttavia qualche problema, primo fra tutti il sistema
stereoscopico che non rileva i led IR in modo soddisfacente. Bastano infatti anche
piccole inclinazioni del led rispetto ai piani immagini delle telecamere perchè questo
non venga più rilevato. Si è provato ad utilizzare dei marker riflettenti illuminati da
34
Cap. 4 Conclusioni e sviluppi futuri
luce infrarossa, ma i controller non riuscivano a rilevarli. Una valida alternativa a tale
sistema stereoscopico consiste nell’utilizzo della kinnect, inoltre già fornita di librerie
necessarie al riconoscimento delle articolazioni e delle varie parti e movimenti del
corpo umano senza che ci sia bisogno di indossare led o altro. L’utilizzo di un miglior
sistema di visione stereoscopica può essere quindi oggetto di un lavoro successivo in
modo da aumentare sensibilmente le prestazioni della manipolazione robotica.
35
Appendice A
Funzioni che implementano i prodotti matriciali per l’implementazione del filtro di
Kalman:
double I3D[6][6]={
{1,0,0,0,0,0},
{0,1,0,0,0,0},
{0,0,1,0,0,0},
{0,0,0,1,0,0},
{0,0,0,0,1,0},
{0,0,0,0,0,1}};
double A3D[6][6]={
{2,0,0,-1,0,0},
{0,2,0,0,-1,0},
{0,0,2,0,0,-1},
{1,0,0,0,0,0},
{0,1,0,0,0,0},
{0,0,1,0,0,0}};
double C3D[3][6]={
{1,0,0,0,0,0},
{0,1,0,0,0,0},
{0,0,1,0,0,0}};
double Pk3D[6][6]={
{1,0,0,0,0,0},
{0,1,0,0,0,0},
{0,0,1,0,0,0},
{0,0,0,1,0,0},
{0,0,0,0,1,0},
{0,0,0,0,0,1}};
double Qk3D[6][6]={
{10,0,0,0,0,0},
{0,10,0,0,0,0},
{0,0,10,0,0,0},
{0,0,0,10,0,0},
{0,0,0,0,10,0},
{0,0,0,0,0,10}};
double Rk3D[3][3]={
36
Cap. 4 Conclusioni e sviluppi futuri
{1,0,0},
{0,1,0},
{0,0,1}};
void Somma3D(double matrice1[][6],double matrice2[][6], double result[][6],int flag){
int i=0,j=0;
while(i<6){
j=0;
while(j<6){
if(flag==1)
result[i][j]=matrice1[i][j]+matrice2[i][j];
else
result[i][j]=matrice1[i][j]-matrice2[i][j];
j++;}
i++;}
}
void Somma13D(double matrix[][3], double matrix2[][3], double result[][3]){
int i=0,j=0;
while(i<3){
j=0;
while(j<3){
result[i][j]=matrix[i][j]+matrix2[i][j];
j++;}
i++;}
}
void Transpose3D(double matrix[][6], double trasposta[][6]){
int i=0,j=0;
while(i<6){
j=0;
while(j<6){
trasposta[j][i]=matrix[i][j];
j++;}
i++;}
}
void Transpose13D(double matrix[][6], double trasposta[][3]){
int i=0,j=0;
while(i<3){
j=0;
while(j<6){
trasposta[j][i]=matrix[i][j];
j++;}
i++;}
}
37
Cap. 4 Conclusioni e sviluppi futuri
void Prodotto6x6(int dimrighe, double matrix1[][6],double matrix2[][6],
double result[][6]){
int i=0,j=0,k=0;
double somma=0;
while(i<dimrighe){
j=0;
while(j<6){
k=0; somma=0;
while(k<6){
somma+=matrix1[i][k]*matrix2[k][j];
k++;}
result[i][j]=somma;
j++;}
i++;}
}
void Prodotto6x3(int dimrighe, double matrix1[][6],double matrix2[][3],
double result[][3]){
int i=0,j=0,k=0;
double somma=0;
while(i<dimrighe){
j=0;
while(j<3){
k=0; somma=0;
while(k<6){
somma+=matrix1[i][k]*matrix2[k][j];
k++;}
result[i][j]=somma;
j++;}
i++;}
}
void Prodotto3x3(int dimrighe, double matrix1[][3],double matrix2[][3],
double result[][3]){
int i=0,j=0,k=0;
double somma=0;
while(i<dimrighe){
j=0;
while(j<3){
k=0; somma=0;
while(k<3){
somma+=matrix1[i][k]*matrix2[k][j];
k++;}
result[i][j]=somma;
j++;}
i++;}
}
38
Cap. 4 Conclusioni e sviluppi futuri
void Prodotto3x6(int dimrighe, double matrix1[][3],double matrix2[][6],
double result[][6]){
int i=0,j=0,k=0;
double somma=0;
while(i<dimrighe){
j=0;
while(j<6){
k=0; somma=0;
while(k<3){
somma+=matrix1[i][k]*matrix2[k][j];
k++;}
result[i][j]=somma;
j++;}
i++;}
}
double Determinant(double matrix[][2]){
return matrix[0][0]*matrix[1][1]-matrix[0][1]*matrix[1][0];}
double Determinant3D(double matrix[][3]){
return matrix[0][0]*(matrix[1][1]*matrix[2][2]-matrix[1][2]*matrix[2][1])matrix[0][1]*(matrix[1][0]*matrix[2][2]-matrix[1][2]*matrix[2][0]+
matrix[0][2]*(matrix[1][0]*matrix[2][1]-matrix[1][1]*matrix[2][0]);}
int Inverse3D(double matrix[][3], double inversa[][3]){
double det;
double tem[2][2];
int i=0,j=0,q=0,w=0,h=0,g=0;
det=Determinant3D(matrix);
if(det!=0){
while(i<3){
j=0;
while(j<3){
q=0;
g=0;
while(q<3){
w=0;
if(q!=i){
h=0;
while(w<3){
if(w!=j){
tem[g][h]=matrix[q][w];
w++;
h++;}
else{ w++;}
}
g++;}
q++;}
39
Cap. 4 Conclusioni e sviluppi futuri
if((i+j)%2==0)
inversa[j][i]=Determinant(tem)/det;
else
inversa[j][i]=-Determinant(tem)/det;
j++;}
i++;}
return 1;}
else return -1;}
40
Elenco delle figure
1.1
1.2
1.3
1.4
1.5
1.6
1.7
Un controller wii. . . . . . . . . .
Collegamento in parallelo dei led.
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . .
Un esemplare di Scorbot. . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
5
6
7
7
8
8
9
2.1
2.2
2.3
2.4
2.5
2.6
Risultato della calibrazione.
. . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
12
14
15
15
16
24
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
29
32
3.1
3.2
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
41