Upload
ngothuy
View
213
Download
0
Embed Size (px)
Citation preview
Università degli Studi di Bologna
FACOLTÀ DI I NGEGNERIACorso di Laurea in Ingegneria Informatica
ROBOTICA INDUSTRIALE
MODELLAZIONE E CONTROLLO TRAMITE
VISUAL SERVOING PER MANI ROBOTICHE
ARTICOLATE
Tesi di Laurea di:
GIANLUCA PALLI
Relatore:
CHIAR .MO PROF. I NG. CLAUDIO M ELCHIORRI
Correlatori:
CHIAR .MO PROF. I NG. ALBERTO TONIELLIDOTT. I NG. L UIGI BIAGIOTTI
Sessione Estiva
Anno Accademico 2002/2003
Parole chiave:Mani robotiche articolateControllo d’impedenzaVisual servoingReal Time LinuxMotori Lineari
L.A.R., Laboratorio di Automazione e Robotica.D.E.I.S., Dipartimento di Elettronica, Informatica e Sistemistica.Università di Bologna.Le simulazioni sono effettuate in MATLAB e SIMULINK , MATLAB e SIMULINK sonomarchi registrati di THE MATHWORKS, INC.La tesi è scritta in LATEX 2ε, la stampa è in POSTSCRIPT.
Ringraziamenti
Bologna, 27 Giugno 2003
Innanzi tutto vorrei ringraziare i miei genitori per tutto quello che hano fattoper me in questi anni e per l’appoggio e la tranquillità che mihanno donato.
Vorrei ringraziare il Prof. Ing. Claudio Melchiorri per avermi dato la possibiltàdi svolgere la tesi al L.A.R. e per la fiducia concessami.
Un doveroso ringraziamento al Dott. Ing. Luigi Biagiotti per il costante aiu-to, al Dott. Ing. Alessandro Macchelli e al Dott. Ing. Marco Guidetti per ladisponibiltà.
Un grazie particolare anche a tutti i ragazzi del L.A.R. per il magnifico periodotrascorso insieme.
Grazie a tuttiGianluca
Indice
Introduzione 1
1 Descrizione del sistema 31.1 Introduzione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31.2 Il dito robotico articolato . . . . . . . . . . . . . . . . . . . . . . . 31.3 Visione artificiale . . . . . . . . . . . . . . . . . . . . . . . . . . . 41.4 La scheda QUANSER MULTI-Q PCI . . . . . . . . . . . . . . . . 51.5 Motori lineari . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61.6 I PC utilizzati . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81.7 Il collegamento via ethernet . . . . . . . . . . . . . . . . . . . . . . 9
2 Il dito robotico articolato 112.1 Il nuovo prototipo di dito robotico . . . . . . . . . . . . . . . . . .122.2 Cinematica del dito . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.2.1 Cinematica diretta . . . . . . . . . . . . . . . . . . . . . . 152.2.2 Cinematica inversa . . . . . . . . . . . . . . . . . . . . . . 18
2.3 Algoritmo numerico per l’inversione della cinematica .. . . . . . . 202.3.1 Risultati sperimentali . . . . . . . . . . . . . . . . . . . . . 22
2.4 Strategie di controllo per l’interazione con l’ambiente . . . . . . . . 262.5 Controllo d’impedenza . . . . . . . . . . . . . . . . . . . . . . . . 272.6 Implementazione del controllo d’impedenza . . . . . . . . . .. . . 30
2.6.1 Dalle coppie di giunto alla tensione dei tendini . . . . .. . 322.7 L’algoritmo di controllo . . . . . . . . . . . . . . . . . . . . . . . . 34
2.7.1 Modello dinamico del dito . . . . . . . . . . . . . . . . . . 342.7.2 Risultati delle simulazioni . . . . . . . . . . . . . . . . . . 36
2.8 Strutture software per il controllo del dito robotico . .. . . . . . . 392.8.1 La comunicazione fra client e server . . . . . . . . . . . . . 40
2.9 Conclusioni . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
3 Controllo real time 433.1 I sistemi real time: generalità . . . . . . . . . . . . . . . . . . . . .43
viii INDICE
3.2 Real Time Linux . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
3.2.1 Il kernel di Linux . . . . . . . . . . . . . . . . . . . . . . . 44
3.2.2 Realizzazione del real time in RTLinux . . . . . . . . . . . 46
3.2.3 Caratteristiche di RTLinux . . . . . . . . . . . . . . . . . . 48
3.2.4 La programmazione con RTLinux . . . . . . . . . . . . . . 49
3.3 Controllo del dito artificiale con RTLinux . . . . . . . . . . . .. . 50
3.3.1 Strutture software . . . . . . . . . . . . . . . . . . . . . . . 50
3.3.2 Il modulo di controllo mqpcicontrol.o . . . . . . . . . . . . 53
3.4 L’algoritmo di controllo . . . . . . . . . . . . . . . . . . . . . . . . 60
3.5 Conclusioni . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
4 Visione artificiale 65
4.1 Il programma greydetect . . . . . . . . . . . . . . . . . . . . . . . 67
4.2 Acquisizione dell’immagine . . . . . . . . . . . . . . . . . . . . . 71
4.3 Correzione della distorsione radiale . . . . . . . . . . . . . . .. . 72
4.3.1 La distorsione radiale . . . . . . . . . . . . . . . . . . . . . 73
4.3.2 Calcolo dei coefficienti di correzione . . . . . . . . . . . . 74
4.3.3 Correzione on-line delle immagini . . . . . . . . . . . . . . 76
4.3.4 Risultati sperimentali . . . . . . . . . . . . . . . . . . . . . 78
4.4 Definizione delle caratteristiche cinematiche . . . . . . .. . . . . . 79
4.4.1 Inseguimento dei punti . . . . . . . . . . . . . . . . . . . . 80
4.4.2 Definizione dei sistemi di riferimento . . . . . . . . . . . . 81
4.4.3 Procedura di ricerca dei punti . . . . . . . . . . . . . . . . 83
4.5 Trasmissione dei dati al server . . . . . . . . . . . . . . . . . . . . 88
4.6 Conclusioni . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89
5 Risultati sperimentali 91
5.1 Come sono state ottenute le misure . . . . . . . . . . . . . . . . . . 92
5.2 Stima degli angoli di giunto . . . . . . . . . . . . . . . . . . . . . . 92
5.2.1 Modello statico e dinamico dei tendini . . . . . . . . . . . . 93
5.2.2 Inizializzazione del valore degli encoder . . . . . . . . .. . 96
5.3 Controllo d’impedenza tramite la sola stima degli angoli di giunto . 97
5.4 Controllo d’impedenza tramite immagini . . . . . . . . . . . . .. . 99
5.4.1 Rallentamento della dinamica dei motori lineari . . . .. . . 99
5.4.2 Prestazioni del controllo tramite immagini . . . . . . . .. . 100
5.5 Utilizzo del sistema di visione artificiale per la taratura dei sensori . 101
5.6 Conclusioni . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102
Indice ix
A Manuale d’uso del programa greydetect 105A.1 Introduzione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105A.2 Parametri da linea di comando . . . . . . . . . . . . . . . . . . . . 106A.3 Uso del programma greydetect . . . . . . . . . . . . . . . . . . . . 107A.4 Comandi da tastiera . . . . . . . . . . . . . . . . . . . . . . . . . . 108A.5 Procedura di definizione dei sistemi di riferimento . . . .. . . . . . 109A.6 Procedura di calibrazione automatica della telecamera. . . . . . . . 109
B I moduli del kernel di Linux 111B.1 Introduzione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111B.2 Moduli: funzionamento . . . . . . . . . . . . . . . . . . . . . . . . 111B.3 Caricamento automatico di un modulo . . . . . . . . . . . . . . . . 113
Bibliografia 116
Elenco delle tabelle
1.1 Parametri caratteristici del motore lineare PS01-23Sx80 con unatensione di alimentazione di 48V. . . . . . . . . . . . . . . . . . . . 8
2.1 Parametri di Denavit-Hartemberg del dito robotico. . . .. . . . . . 142.2 Parametri del modello dinamico del dito robotico utilizzati nelle
simulazioni. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
3.1 Header file per la programmazione della scheda MULTI-Q PCI. . . 54
5.1 Parametri del modello dinamico del collegamento fra slider e link. . 96
Elenco delle figure
1.1 Fotografia del dito. . . . . . . . . . . . . . . . . . . . . . . . . . . 41.2 Campo magnetico nei motori rotativi (a), svolgimento planare del
campo (b) e campo magnetico nei motori lineari (c). . . . . . . . .. 61.3 Schema del motore lineare LinMot P01-23x80. . . . . . . . . . .. 71.4 Schema a blocchi del controllore di forza. . . . . . . . . . . . .. . 71.5 Schema di connessione dei due PC tramite la rete ethernet. . . . . . 9
2.1 Disegno del dito. . . . . . . . . . . . . . . . . . . . . . . . . . . . 122.2 Schema di connessione dei tendini. . . . . . . . . . . . . . . . . . .132.3 Definizione dei sistemi di riferimento e dei parametri del moto delle
falangi. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142.4 Particolare del comportamento del tendine nel passaggio all’esterno
della cerniera. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 152.5 Particolare della cerniera con passagio interno del tendine. . . . . . 162.6 Particolare della cerniera con passagio esterno del tendine. . . . . . 172.7 Grafico dell’accoppiamento fra gli angoliθ2 e θ3. . . . . . . . . . . 192.8 Inversione della cinematica: schema del dito e relativasimbologia. . 202.9 Schema in retroazione dell’algoritmo di inversione cinematica. . . . 212.10 Schema Simulink dell’algoritmo di inversione della cinematica. . . 222.11 Grafici del setpoint, delle traiettorie nello spazio dilavoro ed errore
di inseguimento con e senza azione in avanti. . . . . . . . . . . . . 242.12 Traiettorie nello spazio di giunto. . . . . . . . . . . . . . . . .. . . 242.13 Grafici del setpoint, delle traiettorie nello spazio dilavoro ed errore
di inseguimento con e senza azione in avanti. . . . . . . . . . . . . 252.14 Traiettorie nello spazio di giunto. . . . . . . . . . . . . . . . .. . . 252.15 Descrizione dell’interazione robot/ambiente tramite una rete elettrica. 272.16 Schema di controllo completo del dito robotico. . . . . . .. . . . . 312.17 Schema Simulink del modello dinamico del dito. . . . . . . .. . . 352.18 Schema Simulink del sistema completo. . . . . . . . . . . . . . .. 362.19 Risultati della simulazione durante il moto libero. . .. . . . . . . . 372.20 Risultati della simulazione durante l’interazione con un ostacolo. . . 38
xiv Elenco delle figure
3.1 Dettaglio del kernel di Linux. . . . . . . . . . . . . . . . . . . . . . 453.2 Dettaglio del kernel di RTLinux. . . . . . . . . . . . . . . . . . . . 473.3 Struttura generale della separazione dei compiti nellaprogramma-
zione real time. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
4.1 Schema di comunicazione fra applicazione utente e driver del fra-megrabber a) con le libbgrab e b) senza le libbgrab. . . . . . . . .. 66
4.2 Visuale della telecamera. . . . . . . . . . . . . . . . . . . . . . . . 684.3 Interfaccia grafica del programma greydetect. . . . . . . . .. . . . 684.4 Immagine della telecamera in cui è visibile il righello virtuale. . . . 694.5 Schema logico dell’architettura del programma greydetect. . . . . . 704.6 Schema implementativo dell’algoritmo di correzzione dell’immagine. 734.7 Variabili e sistemi di riferimento utilizzati per indicare la posizione
dei punti nell’immagine distorta (a) e nell’immagine corretta (b). . . 734.8 Griglia di calibrazione. . . . . . . . . . . . . . . . . . . . . . . . . 754.9 Schema in retroazione per il calcolo dei coefficienti delpolinomio. . 764.10 Griglia di calibrazione ripresa dalla telecamera senza correzione (a
sinistra) e con correzione (a destra). . . . . . . . . . . . . . . . . . 784.11 Errore di visualizzazione dei punti della griglia di calibrazione con
e senza correzione della distorsione radiale. . . . . . . . . . . .. . 784.12 Posizionamento dei sistemi di riferimento. . . . . . . . . .. . . . . 844.13 Sequenza di immagini del dito in movimento. . . . . . . . . . .. . 854.14 Parametri per la localizzazione dei punti. . . . . . . . . . .. . . . . 87
5.1 Fotografia del dito e degli attuatori. . . . . . . . . . . . . . . . .. . 915.2 Allungamento dei tendini in funzione delle forze applicate. . . . . . 935.3 Schema di collegamento fra slider e link del dito. . . . . . .. . . . 945.4 Risposta in frequenza del sistema di trasmissione del moto fra slider
e link. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 955.5 Grafici dell’interazione fra dito robotico e ostacolo. .. . . . . . . . 985.6 Grafici delle forze applicate ai motori e delle forze nello spazio di
lavoro. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 985.7 Dati del controllo di impededenza tramite immagini. . . .. . . . . 1015.8 Dati del controllo di impededenza tramite immagini con esempio di
setpoint fuori dallo spazio di lavoro. . . . . . . . . . . . . . . . . . 102
Introduzione
La presente tesi è stata sviluppata presso il L.A.R., Laboratorio di Automazio-ne e Robotica, della facoltà di Ingegneria dell’Universitàdi Bologna. Lo scopo diquesta tesi è definire un modello cinematico e realizzare un controllo d’impeden-za tramite telecamera per un dito robotico articolato realizzato dal DIEM, Dipar-timento di Ingegneria delle Costruzioni Meccaniche, Nucleari, Aeronautiche e diMetallurgia.
Per lo sviluppo del software si è preso spunto da alcuni lavori precedentementesviluppati presso il L.A.R. riguardo la visione artificialee il controllo di motori inambiente RTLinux.
La scelta di utilizzare la telecamera come unico sistema di acquisizione richiedel’elaborazione di immagini ad elevata risoluzione per ridurre il più possibile gli erro-ri di misura. Questo comporta un carico computazione molto elevato per il sistemadelegato al controllo della telecamera. Per questo e per altri motivi che varranno ap-profonditi in questa trattazione si è scelto di implementare la parte di controllo e laparte di acquisizione su due PC separati realizzando così unsistema distribuito. Lacomunicazione fra i due PC avviene tramite la LAN presente inlaboratorio. Questascelta comporta un inevitabile degrado delle prestazioni dinamiche del controllo afavore di un netto miglioramento di quelle statiche.
L’acquisizione e l’elaborazione delle immagini è effettuata in ambiente Linuxda un PC IBM-compatibile con a bordo una scheda PINNACLE STUDIO PCTVper la cattura del segnale video proveniente dalla telecamera.
Il controllo è stato realizzato in ambiente RTLinux su di un normale PC IBM-compatibile con a bordo una scheda QUANSER MULTI-Q PCI che permette l’in-terfacciamento dei dispositivi esterni tramite ingressi encoder ed input/output (I/O)analogici e digitali.
Per quanto riguarda la parte di attuazione si sono utilizzati motori lineari prodottidalla LinMot. Questi rappresentano una soluzione molto interessante ed innovativasia in questo specifico caso che in molti casi di interesse industriale, grazie alla lorosemplicità di controllo, alle loro caratteristiche dinamiche e alla ottime prestazionenell’utilizzo come attuatori di forza. Gli esperimenti effettuati hanno però messo
2 Introduzione
in mostra come la forza massima attuabile da questi dispositivi non sia sufficientea realizzare una buona interazione con l’ambiente esterno.Si è quindi valutatal’opportunità di sostituire questi dispositivi con altri più efficienti nelle successivefasi di sviluppo del progetto.
Tutto il codice è stato realizzato sfruttando una programmazione a pseudo-oggetti in quanto, utilizzando il linguaggio C che è lo standard di programmazionenon solo in ambiente Linux, non si ha a disposizione un vero e proprio supportoalla programmazione ad oggetti. Questa scelta è stata fattaper rendere il softwarefacilmente espandibile, anche in vista di futuri sviluppi del progetto.
In questa tesi si è cercato di giustificare le scelte fatte durante lo sviluppo tramiteaspetti teorici o tramite risultati sperimentali.
La struttura di questa tesi è la seguente:
• Nel Capitolo 1 si è voluto fornire una descrizione del sistema nel suo com-plesso per fornire al lettore un quadro generale del lavoro svolto.
• NelCapitolo 2è stato descritto il dito robotico articolato e sono state illustratele sue caratteristiche cinematiche. Si è poi introdotto il controllo d’impedenzaapplicato al dito robotico.
• Nel Capitolo 3 è stato illustrato il concetto di real time in teoria per poidescrivere come questo sia stato sfruttato per realizzare il controllo del ditorobotico.
• Nel Capitolo 4 è descritto il sistema di visione artificiale realizzato pereffet-tuare misure sul dito robotico.
• Nel Capitolo 5 sono riportati i risultati sperimentali delle prove effettuate pervalutare il corretto funzionamento del sistema oggetto di questa tesi.
Infine in Appendice Aè riportato un breve manuale per l’utilizzo del program-ma di acquisizione ed elaborazione delle immagini realizzato mentre inAppendiceB viene illustrato il concetto di modulo del kernel e l’utilizzo dei moduli in ambienteLinux.
Capitolo 1
Descrizione del sistema
1.1 Introduzione
Il lavoro svolto in questa tesi fa parte di un progetto volto alla realizzazione diun nuovo modello di mano robotica. Altri aspetti di questo progetto sono stati [5] evengono tuttora sviluppati parallelamente al LAR nell’ambito di altre tesi.
In questo capitolo vengono sinteticamente descritte le principali componenti delsistema in modo da poter avere una visione generale del lavoro svolto.
1.2 Il dito robotico articolato
La struttura che si deve controllare è un dito robotico articolato progettato e rea-lizzato dal DIEM [15]. L’approccio utilizzato nel progettare questa struttura puòessere considerato alternativo all’approccio “tradizionale”, e prende in considera-zione strutture flessibili invece che rigide. In passato la flessibilità delle strutturemeccaniche era considerata un difetto da evitare ed eliminare, mentre in questoprogetto viene vista come una proprietà da introdurre e controllare per dotare lastruttura meccanica delle proprietà desiderate.
Un motivo per cui è stato scelto questo approccio per realizzare questo nuovomodello di dito robotico risiede nella necessità di semplificare la struttura meccani-ca del dito stesso. Precedenti progetti [6, 4, 21, 1, 7, 2] sviluppati sia al LAR chein altri centri di ricerca hanno portato a realizzare mani robotiche meccanicamentemolto complesse, molto costose e scarsamente affidabili.
Il dito robotico utilizzato in questa tesi, visibile in figura 1.1, è composto daquattro parti rigide (le tre falangi più una parte da collegare al palmo della ma-no robotica) collegate in catena da elementi flessibili. L’intera struttura è ricavata
4 Descrizione del sistema
sagomando opportunamente un unico pezzo di materiale plastico. In questo caso,sono state utilizzate, per le prove sperimentali, dita realizzate in Teflon e altre realiz-zate in Delrin. I tendini sono in entrambi i casi realizzati in Delrin inquanto questomateriale presenta migliori caratteristiche meccaniche rispetto al Teflon.
A differenza di altri prototipi realizzati in precedenza sfruttando il concetto distruttura flessibile, il modello di dito robotico articolato utilizzato in questa ricercapresenta un terzo tendine collegato alla terza falange, oltre ai due delegati al movi-mento delle prime due falangi, che serve per estendere il dito robotico permettendoagli altri tendini di lavorare sempre in trazione. Questo, oltre a migliorare la mecca-nica del sistema, permette di diminuire la sezione dei tendini. Inoltre il movimentodelle terza falange è accoppiato a quello della seconda tramite un tendine interno.Si ottiene quindi una struttura con due gradi di libertà.
Per permettere il corretto funzionamento del sistema di visione artificiale, sonostate applicati sul dito dei cerchietti di carta di colore nero. Sotto il dito è statomesso un foglio di carta bianco per migliorare il contrasto fra gli elementi visibili (icerchietti neri) e il resto dell’ambiente inquadrato dallatelecamera.
Figura 1.1: Fotografia del dito.
1.3 Visione artificiale
Il sistema di visione artificiale è composto da una telecamera PULNiX modello
1.4. La scheda QUANSER MULTI-Q PCI 5
TM-6CN in bianco e nero con uscita analogica in formato S-Video e da una schedaPINNACLE STUDIO PCTV per l’acquisizione video.
Per effettuare misure di precisione e per ottenere un buon controllo in retroazio-ne, la risoluzione delle immagini acquisite e il numero di campioni (frame) al secon-do dovrebbero essere il più elevati possibile. I limiti imposti dall’hardware e fattorilegati al carico computazionale portano a scegliere una risoluzione di 640x480 pixele un frame rate di 25 fps1. Questi parametri comportano una buona precisione dimisura ma scarse prestazioni dinamiche del controllo.
Per migliorare ulteriormente la precisione di misura, si è implementato un al-goritmo per la compensazione della distorsione radiale introdotta dall’ottica dellatelecamera. Inoltre la procedura di calibrazione permettedi comparare distanze no-te con le misure effettuate dalla telecamera in modo da avereuna corrispondenzafra le dimensioni delle immagini e le dimensioni reali deglioggetti inquadrati.
Il driver della scheda di acquisizione video utilizzato è quello presente nel kernel-2.4.18 di Linux. Questo driver manca di caratteristiche real time, cosa che impe-disce di associare i dati acquisiti ad un preciso instante temporale. Inoltre la tra-smissione dei dati tramite LAN introduce un ulteriore ritardo fra l’acquisizione el’attuazione. Per questi e per altri fattori che verranno approfonditi nel resto del-la trattazione si è dovuta implementare una strategia di controllo che sacrifichi leprestazioni dinamiche per mantenere stabile il sistema.
1.4 La scheda QUANSER MULTI-Q PCI
La MULTI-Q PCI prodotta dalla QUANSER è una scheda di acquisizione nonprovvista di DSP che incorpora i componenti necessari sia alla conversione analo-gica/digitale (A/D) che alla conversione digitale/analogica (D/A). Questa scheda hale seguenti caratteristiche:
• 16 ingressi analogici: 8 di questi sono differenziali mentre i restanti 8 possonoessere single ended o differenziali. Per quanto riguarda i range delle tensioniin ingresso si può scegliere±5V oppure±10V;
• 4 uscite analogiche con escursioni±10V;
• 6 ingressi encoders;
• 48 canali digitali di I/O. Ogni canale è programmabile via software comeingresso o uscita.
1fps: frame per second
6 Descrizione del sistema
Per quanto riguarda la conversione A/D, la risoluzione è di 14 bit, mentre nellaconversione D/A è di 13 bit. Per il conteggio degli impulsi inviati dagli encoderssono presenti dei contatori a 24 bit. La scheda MULTI-Q PCI vainserita in uno slotPCI del PC ed è collegata tramite 5 cavi flat ad un’altra schedadenominata MULTI-Q Terminal Board: quest’ultima ospita vari connettori corrispondenti a tutte le portedi ingresso e di uscita della scheda MULTI-Q PCI. La MULTI-Q Termonal Boardha quindi il compito di raccogliere i segnali provenienti dai vari dispositivi esternie inviarli alla scheda interna al PC per l’acquisizione verae propria.
1.5 Motori lineari
I motori lineari rappresentano una soluzione molto interessante per applicazionidove siano richieste elevate prestazioni dinamiche.
Figura 1.2: Campo magnetico nei motori rotativi (a), svolgimento planare delcampo (b) e campo magnetico nei motori lineari (c).
In questi motori il movimento lineare è generato direttamente dal campo elettro-magnetico senza l’intervento di alcuna parte meccanica. Come nei motori sincronirotativi, vi sono dei magneti permanenti nella parte mobilee degli avvolgimentinella parte fissa. In figura 1.2 viene evidenziata la disposizione del campo nei mo-tori rotativi, il suo sviluppo planare e la successiva disposizione con direzione delcampo parallela all’asse dello slider nei motori lineari.
Nei motori lineari l’avvolgimento statorico è a due fasi e non a tre come neimotori sincroni rotativi.
Il modello di motore lineare utilizzato nelle prove è del tipo PS01-23Sx80 in-sieme al servo-controller E210 e alla power supply unit S01-48/300, tutti prodottidella LinMot. Integrati nel corpo dello statore vi sono un sensore di posizione chesimula un encoder incrementale lineare e un sensore di temperatura.
Il servo-controller è un’unità digitale programmabile viaporta seriale da un nor-male PC-IBM compatibile tramite il programma LinCommander. Sul modello E210sono presenti due canali completamente indipendenti a ciascuno dei quali può esse-re collegato un motore. Il servo-controller si interfacciacon il sistema di controllotramite un connettore a 25 pin su cui sono presenti, per ogni canale:
1.5. Motori lineari 7
Avvolgimenti statorici(due fasi)
Encoder
Elettronica di controllo
Slider
Magneti permanenti
Figura 1.3: Schema del motore lineare LinMot P01-23x80.
• un ingresso analogico differenziale a±10V;
• l’uscita dell’encoder (i segnali dell’encoder rispettanolo standard RS422/5V);
• due input digitali per l’abilitazione del motore e per la limitazione della cor-rente di uscita;
• due output digitali per i segnali di allarme (AMPLIFIER WARNING e AM-PLIFIER FAULT).
Ciascun canale presenta due diversi modi di funzionamento:come controlloredi forza o come controllore di velocità. Durante il lavoro svolto si è utilizzata esclu-sivamente la modalità di controllo in forza. Inoltre fra i parametri programmabilipiù importanti vi sono la risoluzione dell’encoder che può variare da 1 a 50µm ela costante che lega la tensione in ingresso al servo-controller (±10V) e la correnteche ncircola sul motore (max 3A). La costante che lega la forza alla corrente sulmotore è data dal costruttore ed è pari a 11 N/A.
++
Ingresso AnalogicoGuadagno
Offset
Saturazione corrente
10VCorrente richiesta
Corrente d’uscita
Figura 1.4: Schema a blocchi del controllore di forza.
Il limite alla velocità di spostamento dello slider misurabile è dato dalla bandapassante dell’encoder che è di 3MHz [14]. Questo fattore nonpone alcuna limita-
8 Descrizione del sistema
zione all’applicazione oggetto di questo studio in quanto non sono rischieste veloci-tà degli attuatori particolarmente elevate. Inoltre, nel controllo tramite telecamera,gli attuatori sono stati rallentati via software per migliorare la stabilità del sistema.
Dalle prove sperimentali effettuate su questi dispositivie dai risultati di altreesperienze [5] si è notato come, entro i limiti delle nostre applicazioni, il model-lo statico di figura 1.4, ricavato dal manuale del costruttore, sia sufficentementeaccurato.
I dati caratteristici del motore lineare, ricavati dal datasheet del costruttore, sonoriportati nella tabella 1.1.
Massima forza attuabile di picco 33NMassima forza attuabile continua 9NMassima accelarazione 280m/s2
Massima velocità 2.4m/s
Tabella 1.1: Parametri caratteristici del motore lineare PS01-23Sx80 con unatensione di alimentazione di 48V.
La massima forza di picco può essere attuata per pochi secondi dopo i quali ilservo-controller toglie l’alimentazione ai motori per evitare danni da surriscalda-mento. Come verrà meglio esposto in seguito, ciò ha rappresentato un grosso limiteall’applicazione di questi motori al controllo d’impedenza.
1.6 I PC utilizzati
Per lo svolgimento del lavoro oggetto di questa tesi sono stati utilizzati due PCcon architettura Intel i386.
Per l’elaborazione e l’acquisizione delle immagini è statoutilizzato un PC conprocessore Intel PentiumIII a 850 MHz con 256 MB di RAM. Il sistema operativoinstallato è Linux, più precisamente la distribuzione è la RedHat 8.0. La versionedel kernel presente in questa distribuzione è la 2.4.18. E’ stata scelta questa di-stribuzione perchè era la più aggiornata, a livello software, al momento in cui siè iniziato a sviluppare il codice per l’elaborazione e l’acquisizione delle immaginidella telecamera. Nonostante la relativa potenza di questamacchina il carico delprocessore è risultato, durante le prove effettutate, sempre superiore al 60%.
Per il controllo real-time è stato utilizzato un PC con processore Intel PentiumIIIa 450 MHz con 128 MB di RAM. Il sistema operativo installato è RTLinux, piùprecisamente la distribuzione RedHat 6.2 con kernel 2.2.16con applicata la patchRTLinux versione 2.2. La scelta della distribuzione di Linux da installare in questa
1.7. Il collegamento via ethernet 9
macchina è stata dettata dal fatto che le librerie per la programmazione della schedaQUANSER MULTI-Q PCI, di cui non sono disponibili i codici sorgente, sono statecompilate per questa distribuzione e il fornitore della scheda stessa indica questaconfigurazione di sistema come l’unica garantita per il corretto funzionamento dellelibrerie.
Il carico computazionale di questo PC non è risultato essereparticolarmenteelevato. Dalle misure effettuate risulta che l’esecuzionedel task real-time per ilcontrollo del sistema richiede un tempo pari a circa il 10% del periodo di campio-namento che è stato scelto pari a 1ms.
1.7 Il collegamento via ethernet
Come già precedentemente accennato, il controllo in real-time e l’acquisizione-elaborazione delle immagini sono stati effettuati su due macchine distinte, collegatefra loro tramite la LAN presente il laboratorio.
Quello che si è realizzato è di fatto un sistema con architettura client-server: ilPC che esegue il controllo real-time funge da server per le applicazioni (i client) cherichiedono di muovere il dito rimanendo in attesa di connessioni da parte dei client.Per ovvi motivi il server accetta una solo client connesso e processa le richieste diconnessione di eventuali altri client solo dopo che il client corrente ha chiuso laconnessione.
������������������������
������������������������
������������������
������������������ LAN
Lato serverGestione delle
connessioni da partedei client.
Controllo real-time.
Lato clientAcquisizione ed
elaborazione delleimmagini.
Interazione conl’utente.
Figura 1.5: Schema di connessione dei due PC tramite la rete ethernet.
Volendo realizzare un sistema modulare, sarebbe eventualmente possibile gesti-re la comunicazione con più client se a questi fossero assegnati compiti diversi comead esempio nel caso in cui un client sia delegato alla gestione della comunicazionecon i dispositivi di acquisizione (in questo caso la telecamera), un altro client si
10 Descrizione del sistema
occupi alla gestione della richieste dell’utente e eventualmente un terzo comunichial server eventuali cambiamenti da effettuare nei parametri del loop di controllo. Ilclient realizzato in questa tesi, ovvero il programma che sioccupa dell’acquisioneed elaborazione delle immagini e dell’interazione con l’utente, racchiude in se tuttele funzionalità sopra descritte.
Il sistema server è inoltre stato predisposto per accettareconnessioni anche daparte di una normale console a caratteri (tipo telnet) tramite la quale poter passareal sistema di controllo del dito le coordinate del punto che si desidera raggiungere eil tempo di percorrenza della traiettoria congiungente il punto di partenza e il puntodi arrivo.
Come già precedentemente detto, la comunicazione via rete introduce un note-vole degrado delle prestazioni dinamiche del controllo. Per questo motivo si puòpensare, come eventuale sviluppo futuro del progetto, di integrare controllo real-time, acquisizione ed elaborazione delle immagini su di un unica macchina, possi-bilmente dotando pure l’algoritmo di visione artificiale dicaratteristiche real-time,mantenendo la struttura client-server per quanto riguardal’interazione con l’utentee la programmazione del loop di controllo. Si tenga anche presente che, pur mante-nendo questa struttura, nulla vieta di far “girare” sia il server che gli eventuali clientsulla stessa macchina.
Capitolo 2
Il dito robotico articolato
Questa tesi è parte di un progetto di ricerca più ampio volto alla costruzionedi un nuovo prototipo di mano robotica. Molti progetti di questo tipo sono giàstati sviluppati in diversi ambiti sia industriali che universitari [6, 4, 21, 1, 7, 2].La denominazione di “mano” nasce dal fatto che in questi dispositivi si cerca diriprodurre le funzionalità e spesso anche la struttura della mano umana.
Il fattore che maggiormente limita la diffusione di questi dispositivi al di fuoridei laboratori di ricerca è la loro complessità meccanica, elettronica e di controllodovuta all’elevato numero di gradi di libertà, alla molteplicità di apparati sensorialie di attuatori necessari per ottenere buone capacità di manipolazione suprattutto nelcaso di interazione con oggetti o ambienti sconosciuti.
Questa ricerca parte da considerazioni sull’adeguatezza di approcci meccanici“tradizionali” al progetto di mani robotiche che, al giornod’oggi, mostrano pesantilimitazioni a causa dell’elevata complessità, degli elevati costi e della scarsa affida-bilità. Inoltre, per ridurre la complessità del progetto finale, si è adottato un sistemadi sviluppo “integrato” in cui la parte meccanica, elettronica e di controllo vengonosviluppate parallelamente: in questo modo i vari aspetti del progetto tengono contodi esigenze derivanti da discipline diverse evitando problemi di complessità causatidall’incompatibilità delle scelte che i singoli aspetti altrimenti comporterebbero.
Un approccio di tipo modulare inoltre semplifica notevolmente il progetto finaledividendo il problema in sottoproblemi più semplici. Per questo motivo nel progettodi un nuovo modello di mano robotica si parte prendendo in considerazione quelloche è forse il componente più importante per dotare la mano dicapacità di manipo-lazione avanzate: il singolo dito robotico articolato. Come mostrato in precedentiricerche [3], anche il modello umano di controllo della manoe degli altri arti è ba-sato su un controllo gerarchico di tipo modulare. Ricalcando quindi quello che èil modello biologico, verranno studiate soluzioni per il controllo del dito robotico.Queste soluzioni verranno sfruttate per la costruzione della nuova mano robotica.
12 Il dito robotico articolato
2.1 Il nuovo prototipo di dito robotico
Una struttura meccanica ispirata alla struttura della manoumana ha portato alladefinizione e al progetto di un nuovo prototipo di dito robotico articolato basato sudi una struttura che comprenda elementi flessibili in materiale plastico. L’interesseriguardo le strutture flessibili e le loro proprietà è notevolmente cresciuto negli ul-timi anni e applicazioni di queste strutture si possono ritrovare in diversi campi, fracui anche la robotica [13, 10].
Il prototipo di dito robotico articolato utilizzato in questa tesi è realizzato in ununico pezzo composto da parti rigide (le falangi) e da parti flessibili (legamenti etendini): questa struttura semplifica notevolmente le operazioni di costuzione e diassemblaggio delle parti, riduce i costi e aumenta l’affidabilità.
Figura 2.1: Disegno del dito.
Diverse soluzioni morfologiche sono state definite [15, 16]e i diversi progettisono stati implementati e testati sperimentalmente. In questa tesi l’attenzione èstata concentrata su di un prototipo in PTFE costituito da tre falangi unite da giuntiflessibili che fungono da coppie rotoidali con assi di rotazione paralleli. Il disegnodel dito comprensivo di quote è visibile in figura 2.1.
Il movimento della struttura viene effettuato tramite tre tendini: il primo tendi-
2.2. Cinematica del dito 13
ne, denominato h1 in figura 2.2, mouve la prima falange mentreh2 mouve le ultimedue. Il terzo tendine, h3, funge da antagonista al movimentodei due precedenti pergarantire che questi lavorino sempre in trazione. Un quartotendine, h4 non acces-sibile dall’esterno, serve ad accoppiare il movimento della terza falange a quellodella seconda.
L’introduzione del tendine antagonista, non presente nei precedenti prototipi,ha portato notevoli vantaggi in quanto, soprattuto in fase di estensione del dito, itendini h1 e h2 dovrebbero altrimenti lavorare in compressione. Questo permette direalizzare tendini più sottili, eventualmente filiformi, di modificare la giunzione fratendine e falange e di avere tratti di percorso non guidati.
h1 h2 h3
h4
Figura 2.2: Schema di connessione dei tendini.
2.2 Cinematica del dito
Nella definizione della cinematica del dito articolato si sono fatte alcune ipotesisemplificative per ottenere un modello cinematico che fosseil più semplice possi-bile. In particolare si sono considerate ideali le cerniereche uniscono fra di lorofalangi contigue trattandole a tutti gli effetti come giunti rotoidali e si è trascuratala flessibilità e l’elasticità dei tendini e delle cerniere.
Per definire la cinematica del dito articolato si sono utilizzate le convenzioni
14 Il dito robotico articolato
θ1
θ2θ3
x0
y0
x1
y1
x2
y2x3
y3
Figura 2.3: Definizione dei sistemi di riferimento e dei parametri del moto dellefalangi.
di Denavit-Hartemberg [22]. In figura 2.3 sono illustrati i sistemi di riferimentoutilizzati su ciascuna falange (link) e i parametri (angoli) necessari per descrivereil moto del sistema. I parametri della cinematica sono riportati sinteticamente nellatabella 2.1. Si noti che, per come è costruito il sistema, gliangoli fra link contiguirimangono compresi fra 0◦ e 90◦.
d θ a αL1 0 θ1 l1 0L2 0 θ2 l2 0L3 0 θ3 l3 0
Tabella 2.1: Parametri di Denavit-Hartemberg del dito robotico.
Nella figura 2.2 si possono individuare due differenti percorsi dei tendini nelpassaggio da una falange all’altra: un percorso prevede il passaggio del tendinenella parte interna del dito, come riportato in dettaglio infigura 2.5, mentre l’al-tro prevede che il tendine passi nella parte esterna del ditodescrivendo un arco dicirconferenza, come illustrato in figura 2.6.
Nel caso in cui il tendine passa all’interno della cerniera si ipotizza per sempli-cità che questo formi un triangolo con vertice opposto al tendine coincidente con lacerniera fra le due falangi. Ciò è ovviamente una semplificazione perchè il tendinetende a curvarsi nel punto di uscita dal tratto guidato, il punto di appoggio del tendi-ne non è fisso ma dipende dall’angolo fra le falangi a causa della curvatura presente
2.2. Cinematica del dito 15
nella guida. Inoltre la curavatura del tendine dipende, ovviamente, dalla tensioneapplicata al tendine stesso.
Nel caso invece in cui il tendine passa all’esterno della cerniera, l’ipotesi sem-plificativa consiste nell’assumere che il tendine rimanga parallelo alla guida in cuiscorre il tendine stesso all’interno del link nel punto di ingresso e di uscita dallaguida stessa e che nel tratto intermedio fra i due link descriva un arco di circonfe-renza che ha come centro il centro della cerniera che unisce le due falangi. In realtàil tendine tende a stirarsi all’aumentare della forza di trazione ad esso applicata au-mentando così il raggio di curvatura del tendine stesso. In figura 2.4 è illustrato ilcomportamento reale del tendine nel passaggio all’esternodella cerniera sotto l’a-zione di una tensione F in confronto con il comportamento teorico ipotizzato perricavare il modello che esprime la relazione fra posizione dei tendini e angoli digiunto.
F
F
Guide del tendine
Percorso teorico del tendine
Percorso reale del tendine
Figura 2.4: Particolare del comportamento del tendine nel passaggio all’esternodella cerniera.
2.2.1 Cinematica diretta
Il modello cinematico diretto del dito articolato può essere ricavato conside-rando che la relazione che lega gli angoli di giunto alla posizione della punta del
16 Il dito robotico articolato
dito nello spazio di lavoro si può rappresentare con il modello cinematico di unmanipolatore planare formato da tre link con movimenti non accoppiati.
In accordo con la tabella 2.1 è possibile definire anche la matrice di trasforma-zione omogenea che porta dal sistema di riferimento dell’end-effector (la punta deldito) al sistema di riferimento base (il sistema di riferimento del link 0):
0H3 = 0H11H2
2H3 =
=
C123 −S123 0 l3C123+ l2C12+ l1C1
S123 C123 0 l3S123+ l2S12+ l1S1
0 0 1 00 0 0 1
(2.1)
dove
S1 = sin(θ1), S12 = sin(θ1+θ2), S123 = sin(θ1+θ2+θ3)
C1 = cos(θ1), C12 = cos(θ1+θ2), C123 = cos(θ1+θ2 +θ3)
θi
h j0−h j
h j0
r ji
d ji
θi0−θi
θi0
Figura 2.5: Particolare della cerniera con passagio interno del tendine.
Indicando conp il vettore posizione della punta del dito nello spazio di lavoro(il pianoxy) si ha:
2.2. Cinematica del dito 17
p =
[
xy
]
=
[
l3cos(θ1+θ2 +θ3)+ l2cos(θ1+θ2)+ l1cos(θ1)
l3sin(θ1+θ2 +θ3)+ l2sin(θ1+θ2)+ l1sin(θ1)
]
(2.2)
dove conl i si indica la lunghezza dell’i-esimo link. Il modello cinematico 2.2 puòessere rappresentato in forma compatta secondo la consuetasimbologia utilizzatain robotica ottenendo il modello:
p =
[
l3C123+ l2C12+ l1C1
l3S123+ l2S12+ l1S1
]
(2.3)
Il modello cinematico ottenuto può essere rappresentato come una funzione diq = [θ1 θ2 θ3]
T del tipo:
p = f (q) (2.4)
θi
r ji
Figura 2.6: Particolare della cerniera con passagio esterno del tendine.
Per ottenere un modello cinematico completo è ora necessario ricavare le rela-zioni che legano la posizione dei tendinihi agli angoli di giuntoθi . Dalle figure 2.5e 2.6 si possono facilmente ricavare le relazioni:
18 Il dito robotico articolato
h1 = h10−√
r211+d2
11−2r11d11cos(θ10−θ1)
h2 = h20−√
r222+d2
22−2r22d22cos(θ20−θ2)− r21θ1
h3 = h30− r31θ1− r32θ2− r33θ3
h4 = h40−√
r243+d2
43−2r43d43cos(θ30−θ3)− r42θ2 = const
(2.5)
dovehi0, θi0, r i j e di j sono parametri costruttivi propri della struttura in esame. Sinoti che l’ultima delle 2.5 rappresenta il termine d’accoppiamento fra le ultime duefalangi dovuto al tendine internoh4. Invertendo le 2.5 si ottengono le equazioni:
θ1 = θ10−arccos(
(h10−h1)2−d2
11−r211
−2d11r11
)
θ2 = θ20−arccos(
(h20−h2−r21θ1)2−d2
22−r222
−2d22r22
)
θ3 = θ30−arccos(
(h40−r42θ2)2−d2
43−r243
−2d43r43
)
θ3 = h30−h3−r31θ1−r32θ2r33
(2.6)
Si noti cheθ3 può essere ricavato tramite due differenti relazioni: questo fattopuò rappresentare un vantaggio in fase realizzativa permettendo di risalire all’ac-coppiamento fra il moto delle due ultime falangi in due differenti modi.
2.2.2 Cinematica inversa
Il modello della cinematica inversa verrà espresso come:
q = g(p) (2.7)
Invertire il modello cinematico diretto espresso dalle 2.2considerando le dueultime falangi non accoppiate risulta molto semplice. Tuttavia, se si considera illegame fraθ2 e θ3 espresso dall’ultima delle 2.5, il modello risultante non èpiùinvertibile a causa dalla non linearità dell’equazione cheesprime questo legame.Per questo motivo si è implementato un algoritmo numerico inretroazione per ilcalcolo della cinematica inversa del dito robotico. In questo algoritmo si è utilizzatoil modello cinematico inverso del dito ottenuto considerando un accoppiamento ditipo lineare fraθ2 eθ3 come azione in avanti per ridurre il tempo di convergenza. Inquesto modo l’algoritmo in retroazione si occuperà solamente di correggere l’erroredovuto alla non linearità dell’accoppiamento fra i movimenti degli ultimi due giunti.
Questo modello cinematico inverso approssimato si può ottenere considerandoθ2 = θ3 e si può esprimere come:
2.2. Cinematica del dito 19
0 0.5 1 1.50
0.5
1
1.5
Accoppiamento fra gli angoli q2 e q3
Angolo [rad]
Ang
olo
[rad
]
approssimatoreale
Figura 2.7: Grafico dell’accoppiamento fra gli angoliθ2 e θ3.
q = g′(p) (2.8)
Con riferimento alla figura 2.8, attraverso semplici considerazioni geometrichesi possono ricavare le seguenti equazioni, utili per ricavare il modello cinematicoinverso approssimato:
l =√
p2x + p2
y (2.9)
l2 = l21 + l2
23+2l1l23cosθ23 (2.10)
l223 = l2
2 + l23 +2l2l3cosθ3 (2.11)
l23cosθ23 = l2cosθ2+ l3cos(θ2+θ3) (2.12)
l cosθ′1 = l1cosθ1+ l23cosθ23 (2.13)
Sostituendo la 2.11 e la 2.12 nella 2.10 e considerando l’ipotesi inizialeθ2 = θ3,dopo alcuni semplici passaggi si ottiene:
cosθ2 =14
l2l1l3
{
−(l1+ l3)+
√
(l1+ l3)2−4
l1l3l22
[
(l1− l3)2+ l2
2 − l2]
}
(2.14)
da cui, dato che la struttura meccanica del dito robotico imponeπ/2 ≥ θi ≥ 0, è
20 Il dito robotico articolato
θ1
θ2
θ3
θ23
θ′1
l23l
l1
l2
l3
x
y
px
py
Figura 2.8: Inversione della cinematica: schema del dito e relativa simbologia.
possibile ricavare il valore diθ2 e quindi diθ3. Dalla 2.13 si ottiene poi:
θ1 = arctan
(
py
px
)
−arccos
[
l1+ l2cosθ2+ l3cos(2θ2)
l
]
(2.15)
che insieme alla 2.14 rappresenta il modello cinematico inverso approssimato deldito robotico.
2.3 Algoritmo numerico per l’inversione della cine-matica
L’algoritmo di inversione della cinematica è stato realizzato utilizzando la tec-nica dello Jacobiano trasposto. La dimostrazione dell’asintotica stabilità e dellaconvergenza a zero dell’errore di inseguimento del setpoint di questo algoritmo èreperibile in [17]. L’algoritmo sviluppato permette di ricavare gli angoli di giunto apartire dalle coordinate della punta del dito rispetto al sistema di riferimento base,ovvero quello del link zero. L’algoritmo non prende in considerazione le compo-nenti di orientamento del sistema di riferimento della punta del dito. Questo perchè,siccome la struttura del dito gode di due soli gradi di libertà, si sono scelti come pa-
2.3. Algoritmo numerico per l’inversione della cinematica 21
rametri del moto nello spazio di lavoro1 le coordinate cartesianex e y dell’originedel sistema di riferimento del’end-effector (la punta del dito).
Lo schema a blocchi dell’algoritmo per l’inversione della cinematica è illustratoin figura 2.9.
+++
−
pd
p
K
f(q)
g′(p)
∫
JT(q)q q
Figura 2.9: Schema in retroazione dell’algoritmo di inversione cinematica.
Per realizzare l’algoritmo numerico di inversione della cinematica è quindi ne-cessario disporre dello Jacobiano del dito articolato:
J(q) =
[
l1S1+ l2S12+ l3S123 l2S12+ l3S123 l3S123
l1C1+ l2C12+ l3C123 l2C12+ l3C123 l3C123
]
(2.16)
Lo Jacobiano espresso in 2.16 si ottiene direttamente a partire dalle 2.3 e nontiene conto dell’accoppiamento presente fra gli angoliθ2 e θ3. Per poter utilizzarelo Jacobiano nell’algoritmo per l’inversione cinematica ènecessario introdurre talelegame che porta ad uno Jacobiano ridotto di dimensione 2×2:
Jr (q) =
[
l1S1+ l2S12+ l3S123 l2S12+ l3S123[1+J23(θ2)]
l1C1+ l2C12+ l3C123 l2C12+ l3C123[1+J23(θ2)]
]
(2.17)
dove:
J23(θ2) =(h40− r42θ2) r42
d43r43
√
1−[
(h40−r42θ2)2−d2
43−r243
2d43r43
]2(2.18)
L’equazione 2.18 esprime la relazione fra le velocità angolari θ2 e θ3 a causa
1Più di di spazio di lavoro sarebbe corretto parlare di piano di lavoro visto che la traiettoriadell’origine del sistema di riferimento della punta del dito giace sul pianoxy.
22 Il dito robotico articolato
dell’accoppiamento fra le ultime due falangi. Tale relazione può essere esplicitatacome:
θ3 = J23θ2 (2.19)
per cui dalla 2.16 e dalla 2.19 è immediato ricavare la 2.17.La velocità di convergenza dell’algoritmo è determinato dalla matriceK sim-
metrica definita positiva di dimensione 2×2. I valori da assegnare a questa matricesono stati determinati in base a prove sperimentali. Valoritroppo bassi rendonolenta la convergenza dell’algoritmo mentre valori troppo alti lo rendono instabile.
2.3.1 Risultati sperimentali
Per verificare il funzionamento dell’algoritmo numerico diinversione della ci-nematica si è implementato l’algoritmo in Simulink e si sonoeffettuate diversesimulazioni. Lo schema Simulink utilizzato è visibile in figura 2.10.
q
pdot
qdot
traspose jacobianq
p
forwinvkinq
e
qp
direct kinematics
MATLABFunction
aproximated inverse kinematics
Out1
Setpoint
In Out
Gain Matrix
qdot q
2−D integrator
Figura 2.10: Schema Simulink dell’algoritmo di inversionedella cinematica.
Inizialmente si è cercato di far convergere l’algoritmo il più velocemente possi-bile, mantenendo comunque la stabilità, senza l’ausilio dell’azione in avanti tramiteun opportuna scelta dei valori dei coefficienti della matrice K . Dalle prove effet-tuate si è notato che l’algoritmo converge più lentamente per configurazioni finalidegli angoli di giunto che tendono a zero. Questo fatto è giustificabile analizzandoquali siano le configurazioni singolari del dito robotico. Le configurazioni singolarisono date dai valori degli angoli di giunto per le quali il determinate dello Jacobianorisulta nullo. Il determinante della 2.17 è:
2.3. Algoritmo numerico per l’inversione della cinematica 23
det(Jr(q)) = l1l2S2+ l1l3S23[1+J23(θ2)]+ l2l3S3J23(θ2) (2.20)
che risulta essere nullo perθ2 = 0. Si noti che questa condizione implica ancheθ3 = 0.
Nelle prime simulazioni si è verificato il comportamento dell’algoritmo in ri-sposta ad un setpoint nello spazio di lavoro che varia a gradino per evidenziare iltempo di risposta e la convergenza asintotica a zero dell’errore di inseguimento delsetpoint. Con variazioni del setpoint così brusche si sono evitate configurazioni de-gli angoli di giunto vicine alla singolarità per evitare l’instaurarsi di oscillazioni cheaumentano notevolmente il tempo di convergenza dell’algoritmo.
Per garantire che il setpoint rimanga all’interno dello spazio di lavoro raggiun-gile la traiettorie vengono generate nello spazio di giuntoe poi riportate nello spaziodi lavoro tramite la 2.3.
Nei grafici di figura 2.11 sono visibili gli andamenti delle coordinate della puntadel dito e dell’errore di inseguimento del setpoint con e senza azione in avanti. Sipuò notare come l’inserimento dell’azione in avanti porti notevoli vantaggi in ter-mini di riduzione dell’errore durante i transitori con conseguente beneficio ancheper quanto riguarda il tempo di assestamento dell’algoritmo. In particolare si puònotare che senza azione in avanti vi è anche un ritardo pari adun periodo di cam-pionamento fra variazione del setpoint e variazione dell’uscita del sistema mentrel’introduzione dell’azione in avanti elimina questo ritardo. L’ errore a regime risul-ta in entrambi i casi nullo per la presenza dell’integratoreall’interno del sistema.Nei grafici di figura 2.12 sono visibili le traiettorie nello spazio di giunto: in questigrafici risulta particolarmente evidente come la risposta del sistema senza azionein avanti sia assimilabile a quella di un sistema discreto del primo ordine con gra-do relativo uguale ad uno. Con l’introduzione dell’azione in avanti il transitoriosi riduce alla correzione dell’errore dovuto alla differenza fra la cinematica inversaapprossimata e quella reale.
In seguito si è verificata la risposta del sistema a setpoint con variazioni con-tinue. Anche in questo caso, come visibile dai grafici e in particolare dal graficodell’errore, l’introduzione dell’azione in avanti porta un notevole beneficio in ter-mini di precisione e di velocità di risposta dell’algoritmo. Da notare che, nel casodi variazioni continue del setpoint, possono essere raggiunte anche configurazioniche prevedono angoli di giunto uguali a zero. Si può notare però dai grafici chequando l’angoloθ2 è prossimo a zero si instaurano delle leggere oscillazioni suivalori degli angoli che deteriorano leggermente le prestazioni del sistema. Questeoscillazioni sono da imputare al fatto che il contributo dell’azione in avanti, quandogli angoli di giunto e in particolareθ2 tendono a zero, tende a far diventare nega-tivi gli angoli di giunto, cosa che è ben visibile nei grafici di figura 2.14. Questo
24 Il dito robotico articolato
0 10 20 30 40 50 60 70 80 90 100−0.02
0
0.02
0.04
0.06
0.08Setpoint
Tempo [sample]
Posiz
ione
[m]
xy
0 10 20 30 40 50 60 70 80 90 100−0.02
0
0.02
0.04
0.06
0.08Setpoint
Tempo [sample]
Posiz
ione
[m]
xy
0 10 20 30 40 50 60 70 80 90 100−0.02
0
0.02
0.04
0.06
0.08
0.1Traiettorie con azione in avanti
Tempo [sample]
Posiz
ione
[m]
xy
0 10 20 30 40 50 60 70 80 90 100−0.02
0
0.02
0.04
0.06
0.08
0.1Traiettorie senza azione in avanti
Tempo [sample]
Posiz
ione
[m]
xy
0 10 20 30 40 50 60 70 80 90 100−0.06
−0.04
−0.02
0
0.02
0.04
0.06
Errore con azione in avanti
Tempo [sample]
Erro
re n
ello
spa
zio d
i lavo
ro [m
]ExEy
0 10 20 30 40 50 60 70 80 90 100−0.06
−0.04
−0.02
0
0.02
0.04
0.06
Errore senza azione in avanti
Tempo [sample]
Erro
re n
ello
spa
zio d
i lavo
ro [m
]
ExEy
Figura 2.11: Grafici del setpoint, delle traiettorie nello spazio di lavoro ed errore diinseguimento con e senza azione in avanti.
0 10 20 30 40 50 60 70 80 90 1000
0.2
0.4
0.6
0.8
1
1.2
1.4Angoli di giunto con azione in avanti
Tempo [sample]
Ango
li di
giu
nto
[rad]
q1q2
0 10 20 30 40 50 60 70 80 90 1000
0.2
0.4
0.6
0.8
1
1.2
1.4Angoli di giunto senza azione in avanti
Tempo [sample]
Ango
li di
giu
nto
[rad]
q1q2
0 10 20 30 40 50 60 70 80 90 1000
0.2
0.4
0.6
0.8
1
1.2
1.4Setpoint nello spazio di giunto
Tempo [sample]
Ango
li di
giu
nto
[rad]
q1q2
Figura 2.12: Traiettorie nello spazio di giunto.
2.3. Algoritmo numerico per l’inversione della cinematica 25
0 20 40 60 80 1000
0.02
0.04
0.06
0.08
0.1
0.12Setpoint
Tempo [sample]
Posiz
ione [
m]
xy
0 20 40 60 80 1000
0.02
0.04
0.06
0.08
0.1
0.12Setpoint
Tempo [sample]
Posiz
ione [
m]
xy
0 20 40 60 80 1000
0.02
0.04
0.06
0.08
0.1
0.12Traiettorie con azione in avanti
Tempo [sample]
Posiz
ione [
m]
xy
0 20 40 60 80 1000
0.02
0.04
0.06
0.08
0.1
0.12Traiettorie senza azione in avanti
Tempo [sample]
Posiz
ione [
m]
xy
0 20 40 60 80 100−0.06
−0.04
−0.02
0
0.02
0.04
0.06
Errore con azione in avanti
Tempo [sample]
Erro
re ne
llo sp
azio
di lav
oro [
m]
ExEy
0 20 40 60 80 100−0.06
−0.04
−0.02
0
0.02
0.04
0.06
Errore senza azione in avanti
Tempo [sample]
Erro
re ne
llo sp
azio
di lav
oro [
m]
ExEy
Figura 2.13: Grafici del setpoint, delle traiettorie nello spazio di lavoro ed errore diinseguimento con e senza azione in avanti.
0 10 20 30 40 50 60 70 80 90 100−0.2
0
0.2
0.4
0.6
0.8
1Angoli di giunto con azione in avanti
Tempo [sample]
Ang
oli d
i giu
nto
[rad]
q1q2
0 10 20 30 40 50 60 70 80 90 100−0.2
0
0.2
0.4
0.6
0.8
1Angoli di giunto senza azione in avanti
Tempo [sample]
Ang
oli d
i giu
nto
[rad]
q1q2
0 10 20 30 40 50 60 70 80 90 100−0.2
0
0.2
0.4
0.6
0.8
1Setpoint nello spazio di giunto
Tempo [sample]
Ang
oli d
i giu
nto
[rad]
q1q2
Figura 2.14: Traiettorie nello spazio di giunto.
26 Il dito robotico articolato
fatto introduce incertezza nella ricerca della soluzione corretta che prevede angolidi giunto maggiori o uguali a zero. Simulazioni effettuate con variazioni più lentedel setpoint hanno però messo in evidenza che si tratta di un fenomeno transitoriodi breve durata e che le oscillazioni diminuiscono di ampiezza al diminuire dellavelocità di variazione del setpoint. Per evitare che gli angoli di giunto diventino ne-gativi si è introdotta una saturazione che ne limiti il valore all’interno dell’intervallo[0,π/2] ma questo provoca problemi di convergenza dell’integrale con conseguenteaumento dell’errore di inseguimento per cui questa soluzione è stata scartata.
2.4 Strategie di controllo per l’interazione con l’am-biente
La manipolazione robotica consiste nell’interazione del robot con l’ambiente ocon oggetti su cui si devono compiere determinate operazioni. Per poter eseguireoperazioni di manipolazione non è possibile ignorare la possibilità di incontrareostacoli inprevisti o di avere a che fare con oggetti di cui non è nota a priori laforma e la rigidezza. In questi casi le strategie di controllo che possono essereapplicate sono:
• Controllo di forza;
• Controllo ibrido forza/posizione, dove il controllo di forza e di posizione sonoutilizzati per controllare il comportamento del sistema lungo diverse direzionia seconda del compito da eseguire;
• Controllo d’impedenza.
Un controllo di forza sarebbe inadeguato nel caso in cui non si ha contatto conl’ambiente esterno perchè nello spazio libero non è possibile applicare forze diverseda zero. Inoltre la manipolazione richiede, oltre alla possibilità di applicare forze, lapossibilità di guidare il manipolatore lungo determinate traiettorie per posizionare ivari elementi nella maniera più idonea per poter eseguire ilcompito prefissato.
Il controllo ibrido non è adatto per la manipolazione perchèspesso è necessariopoter applicare forze ed eseguire spostamenti lungo la stessa direzione tenendo an-che conto delle dinamiche presenti nella transizione da condizioni di moto libero acondizioni di contatto e viceversa.
Recenti ricerche nel campo della manipolazione robotica hanno dimostrato co-me il controllo d’impedenza si presti particolarmente beneper gestire l’interazionefra il sistema meccanico oggetto del controllo e l’ambienteesterno. In particolare
2.5. Controllo d’impedenza 27
il controllo d’impedenza risulta particolarmente robustonei confronti di incertez-ze sul modello cinematico del robot e nei confronti delle dinamiche di contattorobot-ambiente che nella maggior parte dei casi sono sconosciute o comunque nonmodellate [3].
2.5 Controllo d’impedenza
L’idea alla base del controllo d’impedenza è piuttosto semplice ed ha un pre-ciso significato fisico. Con questo controllo non si cerca di inseguire traiettorie dimovimento o di forza ma si definisce una relazione dinamica tra velocità e forze.Quando un robot interagisce con l’ambiente, fra questi due sistemi avviene unoscambio di energia. Per analogia al caso delle reti elettriche, si può rappresentarequesto scambio di energia con il circuito di figura 2.15 in cuiè presente un genera-tore di corrente, il setpoint, e due elementi passivi, un’impedenza, che rappresentail robot, e un’ammettenza, che rappresenta l’ambiente esterno: la tensione ai capidegli elementi passivi rappresenta la forza che si scambiano il robot e l’ambientedurante l’interazione mentre la corrente rappresenta la velocità del robot.
x0
xF
Z Y
Robot Ambiente
Figura 2.15: Descrizione dell’interazione robot/ambiente tramite una rete elettrica.
Ciò che si vuole ottenere è che il robot abbia delle caratteristiche di impedenzamodulabili a seconda del tipo di interazione con l’ambienteesterno che si desideraeffettuare. Sfruttando le trasformate di Laplace, con riferimento al circuito di figura2.15, la relazione di impedenza che si vuole il robot abbia sipuò rappresentarecome:
F(s)sx(s)
= Z(s) (2.21)
28 Il dito robotico articolato
oppure:
F(s)x(s)
= sZ(s) (2.22)
dove conF(s) si indica la trasformata di Laplace del vettore delle forze econx(s)la traformata della posizione nello spazio di lavoro, mentre conZ(s) si indica latrasformata dell’impedenza del robot. Si noti che al tendere all’infinito dell’impe-denza si realizza di fatto un controllo di pura forza, mentre, all’opposto, facendotendere a zero l’impedenza si realizza un controllo di posizione. E’ quindi possibileconsiderare il controllo di posizione e di forza come casi particolari del control-lo d’impedenza. Inoltre, mantenendo disaccoppiata l’impedenza lungo le direzionidello spazio di lavoro, è possibile realizzare un controlloibrido di forza/posizioneconfigurando anche queste tipologia di controllo come caso particolare del controllod’impedenza.
Tipicamente si desidera ottenere per il robot un modello lineare di impedenzadel tipo:
Z(s) = sMd +Bd +Kd
s(2.23)
dove le matriciMd,Bd e Kd, diagonali definite positive, sono rispettivamente lematrici delle masse, degli smorzamenti e di rigidezza del sistema. Dalle 2.22 e2.23, tornando nel dominio dei tempi, si ha quindi che:
Mdx+Bdx+Kdx = F (2.24)
Quest’ultima equazione rappresenta il modello d’impedenza che si desidera im-porre al robot. In particolare, si noti che modificando opportunamente le matriciMd e Kd si può cambiare il comportamento del sistema: analogamentea quantoavviene nel corpo umano, il sistema è reso rigido e veloce durante i movimenti incui si richiede precisione e non si prevede di interagire conl’ambiente mentre vie-ne reso lento e cedevole durante l’avvicinamento e il contatto. La scelta di matricidiagonali ci permette di selezionare:
• alti valori di Md,i e piccoli valori di Kd,i lungo le direzioni dello spaziooperativo dove si prevedono contatti;
• alti valori di Kd, j e piccoli valori diMd, j nelle direzioni libere dello spaziooperativo.
Nel caso in cui l’ambiente sia rigido occorre scegliere bassi valori di Kd perlimitare le oscillazioni dovute al contatto. La matriceBd rappresenta il termine che
2.5. Controllo d’impedenza 29
dissipa l’energia del sistema per cui agendo su questa matrice si possono modificarei transitori.
Questo tipo di controllore è usualmente progettato in due fasi distinte:
• Linearizzazione del modello del robot;
• Imposizione del modello desiderato di impedenza.
Con riferimento al modello dinamico lagrangiano di un robotgenerico e trascu-rando gli attriti
M(θ)θ+C(θ,θ)θ+g(θ) = τ+JT(θ)Fa (2.25)
doveM(θ) è la matrice d’inerzia del robot,C(θ,θ) è la matrice dei termini centri-fughi e di Coriolis,g(θ) tiene conto degli effetti della forza di gravità,τ è il vettoredelle coppie applicate ai giunti del robot,J(θ) è lo Jacobiano del sistema eFa è ilvettore delle forze esterne.
Riportando il modello 2.25 nello spazio operativo si ha:
M(x)x+ C(x,x)x+ g(x) = F+Fa (2.26)
dove
M = (JM−1JT)−1 = J−TMJ−1
C = M(JM−1CJ−1− JJ−1) = J−TCJ−1− M JJ−1
g = MJM −1g = J−Tg
Imponendo la legge di controllo
F = [M(x)y+ C(x,x)x+ g(x)−Fa] (2.27)
si ottiene il modello lineare
x = y (2.28)
Si noti che per ottenere la legge di controllo 2.27 è necessario avere a dispo-sizione la misura delle forze esterne applicate al robot. Questo implica una ade-guata sensorialità del sistema che deve essere in grado di effettuare tale misura consufficiente precisione e prontezza.
Una volta linearizzato il modello del robot è possibile imporre l’impedenzadesiderata scegliendo una opportuna legge di controllo.
30 Il dito robotico articolato
Scegliendo:
y = xd +M−1d [Bd(xd− x)+Kd(xd−x)+Fa] (2.29)
e definendo la funzione errore come
e= xd−x (2.30)
sostituendo le 2.29 e 2.30 nella 2.28 si ottiene il seguente modello dinamico basatosull’errore:
Mde+Bde+Kde+Fa = 0 (2.31)
2.6 Implementazione del controllo d’impedenza
Nel precedente paragrafo si sono illustrate le procedure direalizzazione di unalgoritmo che realizzi il controllo d’impedenza di un robotgenerico. Tuttavia, nelcaso specifico trattato in questa tesi, sono necessarie alcune modifiche a quantoprecedentemente detto.
Innanzi tutto, nel progetto del controllo, si è trascurata la dinamica del dito percui nel modello dinamico 2.25 le matriciM(θ) e C(θ,θ) risultano trascurabili. Ladinamica del dito non è stata considerata perchè i link del dito, essendo costruitiin materiale plastico, risultano molto leggeri e le loro ridotte dimensioni rendonotrascurabili i momenti d’inerzia.
Nel setup utilizzato durante le prove, il dito è stato disposto in modo tale daeffettuare movimenti orizzontali rendendo ininfluente l’azione delle forze gravità eannullando cosi il contributo dellag(θ) nella 2.25.
Rispetto al modello dinamico di un robot generico è necessario considerare l’ef-fetto dell’elasticità delle cerniere realizzate in materiale flessibile che realizzano difatto una molla circolare. L’azione di queste cerniere è stata modellata approssi-mativamente con una coppia proporzionale alla differenza fra l’angolo di giunto el’angolo di riposo della cerniera2.
Un altro motivo per cui si è trascurata la dinamica del dito è che si vuole ottenereun algoritmo di controllo efficiente soprattutto in condizioni statiche. Inoltre, ilsistema di acquisizione utilizzato per la realizzazione pratica del sistema, descrittonel capitolo 4, presenta pessime performance dinamiche a causa del valore del framerate della telecamera che risulta troppo basso.
2Per angolo di riposo delle cerniere si intende l’angolo di giunto presente fra link consecutiviquando sul dito non agiscono forze esterne. Tali angoli sonodovuti a come vengono costruite lecerniere.
2.6. Implementazione del controllo d’impedenza 31
Il modello statico del dito risulta quindi:
Keθ = τ+JT(θ)Fa (2.32)
conKe costante elastica della molla equivalente alla cerniera eτ vettore delle coppieapplicate ai giunti dal controllo. In questo modello non compare alcun termine chetenga conto dell’attrito statico. Questo perchè nei giuntidel dito non vi sono partia contatto in movimento reciproco essendo le cerniere realizzate tramite elementiflessibili. Imponendo:
τ = τq+Keθ (2.33)
conτq vettore delle coppie dovute al controllo d’impedenza, si ottiene
τq+JT(θ)Fa = 0 (2.34)
che esprime l’equilibrio fra le forze esterne applicate alla punta del dito e le coppieapplicate ai giunti dal controllore.
Dato che:
τq = JT(θ)F (2.35)
il modello 2.34 può essere riportato nello spazio di lavoro:
F+Fa = 0 (2.36)
che esprime l’equilibrio delle forze nello spazio di lavoro.
−
−
+
+
+
+
+
−Kd
Ke
Bd
FingerJ(θ)T
J(θ)
f(θ)p
p
p0
p0
θ
θ
τq
Figura 2.16: Schema di controllo completo del dito robotico.
Il modello d’impedenza che si desidera imporre viene semplificato rispetto al
32 Il dito robotico articolato
modello 2.24 in ragione del fatto che si trascurano i terminidi inerzia e che lavelocità di riferimento viene considerata nulla:
F = Kd(p−p0)+Bdp (2.37)
doveF è il vettore delle forze nello spazio di lavoro,Kd è la matrice di rigidezzae Bd è la matrice di smorzamento, entrambe di dimensione 2× 2, p è il vettoreposizione nello spazio di lavoro,p0 è il setpoint ep è la velocità dell’end-effector.Dalla 2.36 e 2.37 e considerando le ipotesi precedentementefatte sul setpoint siottiene un modello dinamico basato sull’errore del tipo 2.31.
2.6.1 Dalle coppie di giunto alla tensione dei tendini
Una volta note le coppieτq si possono ricavare le forze da applicare ai tendini.Differenziando le 2.5 si ottiene:
h1 = r11d11sin(θ10−θ1)√r211+d2
11−2r11d11cos(θ10−θ1)θ1
h2 = r22d22sin(θ20−θ2)√r222+d2
22−2r22d22cos(θ20−θ2)θ2− r21θ1
h3 = −r31θ1− r32θ2− r33θ3
h4 =r43d43sin(θ30−θ3)√
r243+d2
43−2r43d43cos(θ30−θ3)θ3− r42θ2 = 0
(2.38)
Dall’ultima delle 2.38 si ricava il legame fraθ2 e θ3 dovuto all’accoppiamentofra i il movimento dei due giunti. Le 2.38 si possono in fine esprimere come:
h = P(q)q (2.39)
doveh è il vettore delle velocità dei tendini eP(q) è lo Jacobiano della 2.7. Dalla2.39 si rivaca quindi che:
τq = PT(q)Fh (2.40)
in cui Fh è il vettore delle forze applicate ai tendini. Invertendo la2.40 si ottieneinfine:
Fh = P+(q)τq (2.41)
doveP+ è la pseudoinversa diPT . Per garantire che i tendini lavorino sempre intrazione si può modificareFh sommandovi una componente che non modifichi lecoppie applicate ai giunti:
F′h = P+(q)τq+FhN (2.42)
2.6. Implementazione del controllo d’impedenza 33
conFhN ∈ ker(PT(q)). Ovviamente laFhN deve essere la più piccola possibile chegarantisce la positività delleF
′h per evitare di sollecitare troppo i tendini.
Questo sistema non permette però di tenere in considerazione le coppie applicateal terzo giunto a causa dell’accoppiamento presente fra il secondo e il terzo giuntodovuto al quarto tendine. Un metodo alternativo per ricavare le forze da applicareai tendini si può ricavare studiando l’equilibrio delle forze e delle coppie applicatead ogni giunto:
τ1 = f1(θ1)Fh1− r21Fh2− r31Fh3
τ2 = f2(θ2)Fh2− r32Fh3− r42Fh4
τ3 = f4(θ3)Fh4− r33Fh3
(2.43)
dove
fi(θ j) =r i j di j sin(θ j0−θ j)
√
r2i j +d2
i j −2r i j di j cos(θ j0−θ j)
sono i termini diP(q) dovuti al passaggio interno alla cerniera del tendine mentre leτi , componenti del vettoreτq, sono le coppie applicate all’i-esimo giunto. Nelle 2.43si sono trascurati gli attriti che si oppongono allo scorrimento dei tendini all’internodelle guide in cui sono alloggiati perchè il loro valore risulta molto basso grazieal basso coefficiente di attrito del materiale con cui sono realizzati i tendini e lastruttura del dito, nonchè alla possibilità di lubrificare itendini stessi per ridurreulteriormente l’attrito.
Ricavate leτq dalla 2.35 e fissando arbitrariamente il valore diFh4 = ε, dall’ul-tima delle 2.43 si può ricavare il valore diFh3:
Fh3 =f4(θ3)ε− τ3
r33(2.44)
Dalle seconda e dalla prima delle 2.43 si possono in seguito ricavare rispettiva-mente il valore diFh2 e di Fh1.
Fh2 =r42ε+ r32Fh3 + τ2
f2(θ2)(2.45)
Fh1 =r21Fh2+ r31Fh3 + τ1
f1(θ1)(2.46)
A questo punto è necessario verificare che:
Fh3 > 0
Fh2 > 0
34 Il dito robotico articolato
Fh1 > 0
Se queste condizioni non sono verificate bisogna sommare aFh una componenteFhN ∈ ker(PT(q)). In particolare dati i valori delle forze da applicare ai tendini conτq = 0 eFh3 = 1
Fh30 =f4(θ3)
r33(2.47)
Fh20 =r42+ r32Fh30
f2(θ2)(2.48)
Fh10 =r21Fh20+ r31Fh30
f1(θ1)(2.49)
è possibile determinare il minimo valore diFhN che rende positivaF′h
m=
∣
∣
∣
∣
min
{
Fh1
Fh10,
Fh2
Fh20,
Fh3
Fh30
}∣
∣
∣
∣
(2.50)
F′h4 = Fh4(1+m) (2.51)
DataF′h4 si possono calcolare tramite le 2.47, 2.48 e 2.49 i valori diF
′h1,F
′h2 e
F′h3 così da ottenere il vettoreF
′h.
2.7 L’algoritmo di controllo
Nella descrizione del controllo d’impedenza implementato, i termini dinamicidel modello del dito sono stati trascurati perchè sufficentemente piccoli. Le di-mensioni ridotte del dito robotico e la leggerezza del materiale plastico con cuiè costruito rendono appunto trascurabile la dinamica del dito stesso. Volendo peròverificare le caratteristiche dello schema di controllo tramite simulazioni con oppor-tuni strumenti di calcolo, è necessario costruire un modello dinamico del dito che,rispetto al modello 2.32, approssimi meglio il reale comportamento della struttura.
2.7.1 Modello dinamico del dito
Il modello dinamico del dito utilizzato nelle simulazioni si basa su di un modellodinamico del tipo
M θ+Dθ+Keθ = τ+JT(θ)Fa (2.52)
2.7. L’algoritmo di controllo 35
che rispetto al modello 2.32 presenta il contributo dei termini d’inerzia e di attritoviscoso dovuti rispettivamente alle matriciM eD.
M ii 0.02 KgDii 0.0025 Nsm−1
Keii 0.015 Nmrad−1
Tabella 2.2: Parametri del modello dinamico del dito robotico utilizzati nellesimulazioni.
Le matrici M e D sono state considerate diagonali in modo tale da trascuraregli accoppiamenti dei termini inerziali e d’attrito fra i vari giunti per semplificare lasuccessiva realizzazione del modello in ambiente Simulink. Si è poi fatto uso delle2.43 per ricavare le coppie sui giunti a partire dalle forze applicate ai tendini e dalcontributo delle forze esterne. I valori da assegnare alle matrici M , D eKe sono statiscelti sulla base di valutazioni approssimative sulle dimensioni, sulle caratteristichecostruttive e sulla tipologia del materiale con cui è stato realizzato il dito.
4
pdot
3
qdot
2
p
1
q
Scope5
Scope4
Scope3
Scope2
Scope1
Scope
qdot
q
pdot
Jacobiano
Tau3_ext
F3
q3
F4
tau
Equilibrio giunto 3
F3
Tau2_ext
F4
q2
F2
tau
Equilibrio giunto 2
F3
Tau1_ext
F2
q1
F1
tau
Equilibrio giunto 1
Tau3
q3
q3dot
Dinamica giunto 3
Tau2
q2
q2dot
Dinamica giunto 2
Tau1
q1
q1dot
Dinamica giunto 1
Demux
Demux
MATLABFunction
Cinematica diretta
q3
q2
q3dot
q2dot
F4
Accoppiamento
2
Tau_ext
1
Fh
Figura 2.17: Schema Simulink del modello dinamico del dito.
Per quanto riguarda il termine di accoppiamento, si è modellato il tendine inter-no tramite un sistema molla-smorzatore dalla rigidezza molto elevata e con smorza-mento sufficiente ad evitare l’instaurarsi di fastidiose vibrazioni nel sistema. Questaassunzione è giustificata dal fatto che, nel ricavare l’ultima delle 2.5, si è conside-rato un tendine ideale, ovvero inestensibile, mentre per effettuare la simulazione siprende in considerazione la reale elasticità del tendine.
36 Il dito robotico articolato
2.7.2 Risultati delle simulazioni
Obbiettivo delle simulazioni è verificare la stabilità e le prestazioni statiche e di-namiche del controllo d’impedenza realizzato sul dito robotico sia durante il motolibero che durante il contatto con eventuali ostacoli. Per questo motivo, nello sche-ma Simulink completo del sistema utilizzato per le simulazioni riportato in figura2.18, oltre al modello del dito di figura 2.17 e ai blocchi necessari a realizzare ilcontrollore, sono stati inseriti dei blocchi che simulano l’azione sul dito di un osta-colo, tipicamente un muro disposto parallelamente all’assex o y, o di un disturbo dicoppia applicato ai giunti dall’esterno.
Out1
Wall coordinates
F
q
Tau
Trasposed Jacobian
F
q
Tau
Trasposed Jacobian
Tau
q
Fh
F4
Tau2Fh
p0
p0dot
Setpoint
Scope9Scope8
Scope6
Scope5
Scope4Scope3 Scope2
Scope1
Scope
q tau
Ke compensation
Fh
Tau_ext
q
p
qdot
pdot
Finger Model
Tau_ext
External torque
MATLABFunction
Contact
p
p0
pdot
p0dot
F
Cartesianimpedance control
Figura 2.18: Schema Simulink del sistema completo.
I valori da assegnare alle matriciKd e Bd sono stati ricavati sperimentalmenteprocedendo per passi: partendo con smorzamento nullo e rigidezza unitaria, si èaumentata quest’ultima fino ad ottenere una risposta sufficientemente rapida ed unridotto errore di inseguimento a regime aumentando di voltain volta anche lo smor-zamento per evitare l’instaurarsi di brusche oscillazionidell’azione di controllo. Inparticolare si è notato che, come già previsto per il comportamento del controllod’impedenza, con elevati valori di rigidezza si ha un errorea regime piccolo, unarisposta rapida ma una reazione agli ostacoli brusca e oscillatoria con valori delleforze in gioco molto elevati mentre con bassi valori di rigidezza il sistema e piùlento ma le reazioni nei confronti degli ostacoli sono più dolci e limitate. Questofatto ci permette quindi di modulare il comportamento del sistema rendendolo più
2.7. L’algoritmo di controllo 37
0 2 4 6 8 100.04
0.06
0.08
0.1Setpoint nello spazio di lavoro
p0 [m
]xy
0 2 4 6 8 100.04
0.06
0.08
0.1Traiettoria nello spazio di lavoro
p [m
]
xy
0 2 4 6 8 10−0.4
−0.2
0
0.2
0.4Forza applicata nello spazio di lavoro
F [N
]
FxFy
0 2 4 6 8 100
0.2
0.4
0.6
0.8Traiettoria nello spazio di giunto
tempo [s]
q [r
ad]
q1q2q3
0 2 4 6 8 10−0.01
−0.005
0
0.005
0.01Coppie applicate ai giunti
tempo [s]
tauq
[Nm
]
tq1tq2tq3
0 2 4 6 8 100
5
10Forze applicate ai tendini
Fh
[N]
Fh1Fh2Fh3
Figura 2.19: Risultati della simulazione durante il moto libero.
rapido durante i movimenti liberi e più dolce durante l’interazione con l’ambienteesterno.
Nei grafici di figura 2.19 vengono riportati gli andamenti deiparametri signi-ficativi del sistema durante il moto libero, ovvero senza interazione con l’ambien-te esterno. Queste prove sono state effettuate conKdii = 40 [N/m] e conBdii =
1 [Ns/m]. Dai grafici si può notare che la risposta risulta molto veloce ma brusca,con picchi molto elevati nelle forze applicate ai tendini durante i transitori. Si notiche comunque le forze applicate ai tendini risultano, durante il funzionamento aregime, entro i limiti ammissibili per i motori lineari scelti come attuatori e comun-que sempre maggiori di zero in modo da far lavorare i tendini sempre in trazione.Le coppie risultanti applicate ai giunti a regime risultanopiccole perchè risentonosoltanto della compensazione delle coppie elastiche dellecerniere. Il loro valore diregime è quindi funzione solo dello scostamento della posizione attuale rispetto allaposizione di riposo del dito.
Le simulazioni successive si sono concentrate sulla verifica del comportamentodel sistema durante l’interazione con l’ambiente. Queste prove sono state effettuate
38 Il dito robotico articolato
conKdii = 4 [N/m] e conBdii = 0.1 [Ns/m]. Dai grafici di figura 2.20 si può notarecome, rispetto al caso precendente, ora la risposta del sistema sia più lenta e dolce,a causa della minor rigidezza imposta dal controllo. La presenza di un’ostacolo,ovvero un “muro”, parallelo all’assex posto ay = 0.05 [m] impedisce al dito ro-botico di inseguire correttamente il setpoint di posizionema permette di valutarela possibilità di applicare forze in una determinata direzione, come evidenziato dalgrafico delle forze applicate nello spazio di lavoro. Nel grafico della traiettoria nellospazio operativo si può notare uno scivolamento della puntadel dito lungo l’ostaco-lo che evidenzia l’efficacia del controllo d’impedenza nell’utilizzo come controllodi posizione nelle direzioni lungo le quali non si ha interazione con l’ambiente. Unfatto rilevante messo in mostra con questa simulazione è l’impossibilità di applicareforze nello spazio di lavoro tali da assicurare una buona capacità di manipolazioneal sistema rimanendo nei limiti imposti dalle caratteristiche degli attuatori. Questofatto ci porterà, in fase di realizzazione pratica del sistema, a rivalutare il tipo diattuatori da utilizzare.
0 2 4 6 8 100.04
0.06
0.08
0.1Setpoint nello spazio di lavoro
p0 [m
]
xy
0 2 4 6 8 100.04
0.06
0.08
0.1Traiettoria nello spazio di lavoro
p [m
]
xy
0 2 4 6 8 10−0.02
0
0.02
0.04
0.06Forza applicata nello spazio di lavoro
F [N
]
FxFy
0 2 4 6 8 100
0.2
0.4
0.6
0.8Traiettoria nello spazio di giunto
tempo [s]
q [r
ad]
q1q2q3
0 2 4 6 8 10−0.01
−0.005
0
0.005
0.01Coppie applicate ai giunti
tempo [s]
tauq
[Nm
]
tq1tq2tq3
0 2 4 6 8 100
5
10Forze applicate ai tendini
Fh
[N]
Fh1Fh2Fh3
Figura 2.20: Risultati della simulazione durante l’interazione con un ostacolo.
2.8. Strutture software per il controllo del dito robotico 39
2.8 Strutture software per il controllo del dito robo-tico
Per realizzare il controllo del dito robotico è stato necessario scrivere in lin-guaggio C una serie di funzioni utili a definire la cinematicadel sistema. Inoltre èstata definita una struttura software che contiene le principali proprietà cinematichee geometriche del dito robotico.
/* finger data structure */
typedef struct {
int finger_num; // finger identification number
double l1,l2,l3; // link length
double h10,h20,h30,h40;
double q10,q20,q30,q33;
double q1init,q2init,q3init;
double r11,d11;
double r21,r22,d22;
double r31,r32,r33;
double r42,r43,d43;
double s1,s12,s123;
double c1,c12,c123;
double ke; // elastic joints constant
} finger_t;
I campi di questa struttura contengono i dati sulla geometria del dito roboticonecessarie per il calcolo della cinematica diretta e inversa. La terminologia utilizza-ta per indicare i vari campi di questa struttura è la stessa utilizzata nelle equazionidefinite nel paragrafo 2.2.1. I campiq1init , q2init , q3init indicano gli angolidi riposo delle cerniere.
Questo pseudo-oggetto permette di gestire diverse istanzedello stesso tipo. Invista di sviluppi futuri in cui si prevede la costruzione di una mano completa, èquindi possibile gestire in maniera flessibile dita di dimensione e con caratteristi-che cinematiche diverse. Anche se non previsto dalla attuale struttura, con piccolemodifiche è possibile anche gestire, in maniera trasparente, dita con modelli cine-matici diversi. Questo permetterebbe di costruire una manocon il pollice diversodalle altre dita, come daltronde già ipotizzato, e di poter controllare il sistema conmodifiche minime al software di controllo. Le funzioni utilizzate per difinire la di-namica del dito prevedono, fra i loro argomenti, un puntatore ad una struttura di tipofinger_t in modo da effettuare i calcoli sulla base delle specifiche caratteristichedel dito la cui struttura è stata passata come argomento.
40 Il dito robotico articolato
Fra le funzioni implementate vi è anche l’algoritmo numerico per l’inversionedella cinematica illustrato nel paragrafo 2.3.
2.8.1 La comunicazione fra client e server
Come descritto nel paragrafo 1.7, nella pratica il sistema di controllo è statorealizzato connettendo via ethernet due PC uno dei quali effettua il controllo realtime mentre l’altro si occupa dell’acquisizione e dell’elaborazione delle immagini.In particolare, il client deve trasmettere al server le informazioni ricavate dalle im-magini, come ad esempio la posizione dei vari link del dito e gli angoli di giunto.Per fare ciò è stata creata una opportuna struttura dati: l’insieme delle informazio-ni contenute in questa struttura rappresenta il “messaggio” che viene trasmetto dalclient al server dopo l’elaborazione di ciascun frame acquisito dalla telecamera.
/* input message */
typedef struct {
int command; // command identification number
work_coord setpoint; // setpoint
work_coord p; // end effector position
joint_ang q; // joint angles vector
work_force F; // impedance control applied force
int time; // time counter
} message_t;
Il campo command di questa struttura identifica il tipo di comando che che ilclient invia al server. I comandi disponibili sono:
• ComandoRESET: serve per fermare l’esecuzione di un eventuale comandoprecedente e per porre il sistema di controllo in condizionedi funzionamentoautonomo3;
• ComandoSETPOINT: indica al sistema di controllo che di interpretare il mes-saggio come un setpoint di posizione da attuare in modalità autonoma. Questocomando viene eseguito per esempio quando ci si connette al server tramiteun terminale a caratteri (tipo telnet) e si inviano le coordinate relative al puntoche si vuole far raggiungere alla punta del dito;
3Per funzionamento autonomo si intende la modalità in cui il sistema di controllo real time fun-ziona solo sulla base dei dati ricavati dagli encoder dei motori lineari. Questo aspetto verrà megliochiarito nel capitolo 5
2.9. Conclusioni 41
• ComandoSTART: viene utilizzato per indicare al sistema di controllo che sidesidera attuare una traiettoria i cui dati sono stati passati tramite le appositearee di memoria condivisa (vedi paragrafo 3.3.1);
• ComandoPLOT: indica al sistema di registrazione dei dati di iniziare unanuova sessione di registrazione;
• ComandoSTOPPLOT: frema il sistema di registrazione dati;
• ComandoCONNENCTED: informa il sistema di controllo che il programma diacquisizione dati tramite telecamera è collegato;
• ComandoNOTCONNECTED: indica che il programma di aquisizione dati è of-fline.
Gli altri campi della strutturamessage_t vengono utilizzati dall’algoritmo diacquisizione ed elaborazione delle immagini per al sistemadi controllo i dati sullostato del dito robotico e le richieste dell’utente:
• il camposetpoint contiene le coordinate del punto che si desidera raggiun-gere;
• il campop contiene le coordinate attuali della punta del dito rilevate dall’al-goritmo di visione artificiale;
• nel campoq sono contenuti gli angoli di giunto misurati dall’algoritmo divisione;
• F è il vettore delle forze nello spazio di lavoro da applicare per realizzare ilcontrollo d’impedenza secondo i dati rilevati dalla telecamera;
• il campo time indicata al sistema di controllo il tempo di esecuzione dellatraiettoria.
2.9 Conclusioni
In questo capitolo sono state studiate, a livello teorico, le proprietà cinemati-che e dinamiche del dito robotico realizzato dal DIEM. Sono state inoltre valutatele possibili strategie di controllo per questa struttura e si è discusso in particola-re la realizzazione del controllo d’impedenza. La parte finale di questo capitolointroduce i principali elementi software, utilizzati per l’implementazione praticadell’algoritmo di controllo, necessari a gestire il dito robotico.
Capitolo 3
Controllo real time
3.1 I sistemi real time: generalità
Lo svolgimento di alcuni compiti necessita un’elevata precisione nella tempo-rizzazione delle azioni da compiere. Nell’ambito dell’automazione molte sono leapplicazioni con questa caratteristica. Un tipico esempioè quello dell’esecuzione diun algoritmo di controllo di natura discreta in cui stabilità e precisione del periododi campionamento sono fondamentali.
I sistemi operativi non real time1 non sono in grado di soddisfare tale vincolopoiché sono progettati con il criterio generale di garantire buone prestazioni medienella maggior parte dei casi di comune impiego. Il problema èche il comporta-mento di questi sistemi nei casi peggiori risulta molto sfavorevole e soprattutto nonpredicibile. Questo risulta in contrasto con le specifiche richieste in precedenza.
Nei sistemi operativi real time, più che a massimizzare le prestazioni generali,si pone la massima attenzione alla corretta gestione delle priorità e delle temporiz-zazioni.
Le applicazioni real time possono essere distinte in due grandi famiglie:
• Sistemisoft real time, in cui il software deve essere in grado di rispettare letemporizzazioni nella maggior parte dei casi.
• Sistemihard real time, in cui il software deve essere in grado di garantire inogni caso tempi di latenza minimi per processi a più alta priorità.
1Come ad esempio sistemi operativi UNIX-like, fra cui anche Linux, e i vari sistemi operativi dicasa Microsoft.
44 Controllo real time
In altre parole non è possibile basarsi sulla media delle performance per valutareun sistema hard real time.
Queste riflessioni iniziali portano a definire quali sono le caratteristiche che unsitema real time deve possedere:
• Comportamento deterministico.Il sistema deve essere in grado di mettere inesecuzione un task RT2 in modo riproducibile e deterministico, cioè il tempomassimo di esecuzione deve essere noto con certezza.
• Prontezza.I task RT devono avere latenza minima, cioè l’esecuzione deltaskdeve iniziare il più rapidamente possibile.
• Robustezza.Il sistema deve funzionare sempre: un blocco della macchinanon deve avvenire per ovvie ragioni di sicurezza.
Esistono, attualmente, diversi sistemi operativi real time fra cui è doveroso citareQNX e Real Time Linux.
3.2 Real Time Linux
I sistemi real time Linux sfruttano una variante del kernel ufficiale di Linux. Neesistono essenzialmente due: RTLinux e RTAI3; entrambe si comportano in modomolto simile e sfruttano le stesse idee di base.
Per motivi legati alla compatibilità delle librerie della scheda MULTI-Q PCIdisponibili al momento, si è deciso di usare RTLinux per lo sviluppo delle applica-zioni legate a questa tesi.
3.2.1 Il kernel di Linux
Il kernel di Linux non nasce per applicazioni real time. La struttura interna èa divisione di tempo ed è studiata par ottimizzare le prestazioni medie del sistema,quindi anche le sue caratteristiche sono in contraddizionecon quelle che sono leesigenze di un kernel real time.
Si può notare che la sincronizzazione a “grana grossa”, utilizzata dai sistemiLinux, implica che vi siano lunghi intervalli di tempo in cuiun task ha accessoesclusivo ad alcuni dati. Questo inevitabilmente ritarderà l’esecuzione di un pro-cesso RT che richieda l’accesso agli stessi dati. D’altra parte una sincronizzazione
2RT: Real Time.3RTAI: Real Time Application Interface.
3.2. Real Time Linux 45
a “grana fine” porta allo spreco di molte risorse per gestire il cambio di contesto, acausa di continui passaggi da un task all’altro. Linux utilizza quindi una sincroniz-zazione a grana grossa per ottimizzare le prestazioni medie. Da non trascurare poiil fatto che Linux concede prima o poi unatime sliceal processo con priorità piùbassa anche nel caso in cui ci sia un processo più importante pronto ad andare inesecuzione. In un sistema real time un processo con prioritàmaggiore non dovrebbemai attendere a causa di un task con priorità minore mentre, in un sistema generico,come Linux, vale la regole che: “tutti i processi fanno progressi almeno ogni tanto”.Inoltre per ottenere buone prestazioni medie Linux riordina le richieste di accessoall’hardware da parte dei vari processi al fine di renderne più efficiente l’uso dellamacchina e tenta di riunire varie operazioni insieme per sfruttare meglio l’hardwarestesso. Si noti che Linux fa attendere i processi ad alta priorità finchè processi apriorità minore non abbiano rilasciato le risorse necessarie. Linux, inoltre, ha lacaratteristica di avere porzioni di codice critico eseguito con interrupt disabilitati.
01101011...
01101011...
01101011...
User processes
Linux KernelDevice driver
System libraries
I/O Hardware interrupts
Hardware
Figura 3.1: Dettaglio del kernel di Linux.
Disabilitare gli interrupt è il modo più semplice per risolvere problemi di sin-cronizzazione tra processi e di regolamentazione di accesso ad aree dati comuni.Questa soluzione compromette la possibilità di rispondereimmediatamente a ri-chieste che vengono da eventi esterni, cosa inammissibile per sistemi real time chedevono gestire con prontezza segnali provenienti dal mondoesterno che risultano,per definizione, asincroni.
Molto importanti sono anche le considerazioni su come vienetrattata la variabiletemporale dai vari sistemi. Linux offre la possibilità di utilizzare una serie di timer
46 Controllo real time
hardware presenti sulle schede dei PC IBM-compatibili. La risoluzione dei timer èbassa e la lora accuratezza inadeguata per utilizzarli in applicazioni hard real time.
Da queste considerazioni si evince che Linux, senza opportune modifiche al ker-nel, è adatto a gestire applicazioni soft real time, ma non è sufficiente per controllareapplicazioni di tipo hard real time.
3.2.2 Realizzazione del real time in RTLinux
In precedenza si è messo in evidenza come siano diverse le finalità e quindile specifiche per la realizzazione di un sistema hard real time rispetto ad uno ditipo standard. Il tentativo di soddisfarle entrambe in un unico sistema porterebbea prestazioni non ottimali in nessuna delle due tipologie diutilizzo. Un’idea disoluzione è quella di scindere il sistema operativo generico da quello real time inmodo che ognuno possa essere ottimizzato in modo indipendente.
Questo può essere realizzato sfruttando il concetto di RTHAL4 che inseriscestrato di astrazione tra il sistema operativo generico e l’hardware di gestione del-l’interrupt. Così il kernel del sistema operativo a divisione di tempo funziona comeun normale processo, eseguito da un piccolo sistema operativo real time. In praticail sistema operativo a divisione di tempo è un processo in background del siste-ma operativo real time e viene eseguito solo quando quest’ultimo non ha processida eseguire. Il sistema operativo a divisione di tempo non può mai bloccare gliinterrupt o impedire di essere posto in attesa.
In pratica questa astrazione viene realizzata creando una copia della tabella deidescrittori degli interrupts, intrappolando così tutte lefunzioni che li gestiscono,creando una nuova tabella con i desrittori delle funzioni real time e prendendo ilcontrollo del timer di sistema. Quando viene inviato all’hardware il comando didisabilitazione degli interrupt il sistema operativo realtime intercetta la richiesta,la registra e la ritorna. Così al sistema operativo a divisione di tempo non è maiconsentito di disabilitare veramente gli interrupt hardware: non importa in che sta-to si trovi, non potrà mai inserire ritardi nella gestione degli interrupt da parte delsistema operativo real time. Quando arriva una richiesta diinterrupt dall’hardwarequesta viene intercettata dal sistema real time che decide cosa farne. Una soluzionebasata su queste caratteristiche permette di scindere i meccanismi del kernel gene-rico da quelli del kernel real time, in modo che entrambi possano essere ottimizzatiindipendentemente secondo le rispettive finalità e facendosi che il kernel real timepossa essere mantenuto semplice e compatto.
L’inserimento dell’RTHAL implica inoltre che i processi che necessitano di ca-ratteristiche real time debbano condividere con il kernel RT lo spazio di indirizza-
4RTHAL: Real Time Hardware Abstraction Layer.
3.2. Real Time Linux 47
01101011...
01101011...
01101011...
Linux is executed in background
User processes
Linux KernelDevice driver
System libraries
I/O
I/O
Hardware interrupts
Software interrupts
RT-Scheduler
Real Time tasks
Hardware
RTLinux
Directhardware
access
Figura 3.2: Dettaglio del kernel di RTLinux.
mento. Infatti i processi standard “vedono” un sistema operativo standard mentresolo a livello di kernel sono disponibili le primitive per lagestione del sistema realtime. Tutto ciò è ottenibile in modo semplice ed elegante sfruttando i moduli5 delkernel di Linux. Un approccio di questo tipo ha il grande vantaggio che il cambiodi contesto fra i vari processi è molto rapido.
Questa è la soluzione adottata da RTLinux per realizzare il real time, quindi Li-nux viene considerato come un processo mandato in esecuzione quando RTLinuxnon è impegnato in nessuna delle operazioni real time. RTLinux è progettato inmodo che il suo kernel RT non debba mai essere lasciato in attesa della liberazionedi risorse da parte di Linux. Una delle direttive principalinella progettazione diRTLinux è: quanto più è demandato a Linux, e quanto meno è lasciato al kernelRT, tanto è meglio. Linux ha il compito di gestire le procedure di inizializzazionedel sistema, delle periferiche, del caricamento dei moduli(anche di quelli real time)e di tutte quelle operazioni non strettamente real time. Il compito del kernel RT èsolo quello di fornire ai programmi real time accesso diretto all’hardware, in mo-do che possano avere latenza minima e il massimo potere computazionale quandonecessario. Per sfruttare realmente i vantaggi introdottiè necessario che i proces-si operanti nei due kernel possano essere messi in condizione di comunicare e di
5Un modulo del kernel è un file oggetto che può essere “linkato”dinamicamente al kernel stes-so, con esso condivide lo spazio di indirizzamento, può quindi utilizzare funzioni del kernel edaggiungerne delle nuove.
48 Controllo real time
scambiare dati frakernel spacee user space6. All’interno di RTLinux sono statiquindi implementati i classici meccanismi di IPC7.
3.2.3 Caratteristiche di RTLinux
RTLinux è una versione di Linux che fornisce capacità hard real time. Si trattadi un piccolo kernel in grado di mandare in esecuzione processi con temporizzazio-ni estremamente precise. Nei casi peggiori, vale a dire quando il sistema è occupato,i ritardi di temporizzazione sono al limite di ciò che l’hardware è in grado di offrire.La sua grande utilità è data dalla capacità di estendere l’ambiente di programmazio-ne standard Linux ai problemi real time. Il software real time può comunicare con inormali processi di Linux tramite i classici strumenti di IPC quali FIFO, SHAREDMEMORY e SEGNALI.
Dal punto di vista dei programmatori, RTLinux aggiunge un processo specialereal time a Linux e consente di inserire thread real time e handler di segnale all’in-terno di questo processo. RTLinux consente la creazione di applicazioni real timein un ambiente di programmazione standard con accesso a tutti gli strumenti di svi-luppo e ai servizi tipici di Linux. Per i tecnici che progettano sistemi di controllo,RTLinux è un’alternativa ai sistemi operativi real time dedicati e ai processori disegnali digitali (DSP) che, fino a poco tempo fa, erano l’unica possibilità per losviluppo di applicazioni real time. L’approccio al controllo con RTLinux offre unelevato livello di flessibilità rispetto a questi ultimi. Con RTLinux gli sviluppatoripossono utilizzare i compilatori standard gcc, effettuareil debug dei thread di RTLi-nux con gdb e creare software real time che utilizza programmi eseguibili su linuxcome linguaggi di scripting, strumenti di rete, database etc..
In conclusione si può dire che i compromessi di progettazione che rendono Li-nux un sistema operativo generico così potente, lo rendono meno idoneo alle ap-plicazioni hard e soft real time. Dividendo i processi real time da quelli non realtime, RTLinux è in grado di mettere in esecuzione ogni tipologia di processo nel-l’ambiente più opportuno. Da una parte questa soluzione offre prestazioni eleva-te e un ambiente strettamente deterministico per le applicazioni real time mentre,
6Per user space si intende lo spazio di indirizzamento dove vengono eseguite le applicazioniutente, esso è distinto e separato dal kernel space che è lo spazio di indirizzamento dove vieneeseguito il kernel e tutti i task real time. A questi due spazidi indirizzamento corrispondono duediversi livelli di libertà, detti “supervisor mode” per il kernel space e “user mode” per lo user space,che stabiliscono se un’applicazione è abilitata o meno ad eseguire determinate operazioni. Benchèl’architettura Intel i386 metta a disposizione ben 4 livelli di libertà, solo due di questi vengonoutilizzati nei sistemi UNIX[23].
7IPC: Inter Process Comunication, comunicazione inter processo.
3.2. Real Time Linux 49
dall’altra, offre un ambiente di programmazione ricco, grazie soprattuto alla vastadisponibilità strumenti di sviluppo e alla potenza di connessione in rete di Linux.
Inoltre, tutte le migliorie apportate a Linux dalla sua enorme comunità di svi-luppo sono immediatamente disponibili agli utenti di RTLinux.
3.2.4 La programmazione con RTLinux
In linea con i principi costitutivi di RTLinux le applicazioni real time devonoessere progettate in maniera da scindere i compiti da eseguire in due. Da una partequelli con esigenze di temporizzazione stringente sarannoeseguiti da processi chegirano nel kernel real time, mentre dall’altra quelli senzaquest’esigenza specificasaranno eseguiti da processi di Linux. Con questo approcciosi mantiene la caratte-ristica di ottimizzazione nell’utilizzo dei due sistemi, infatti ogni applicazione girain un ambiente ottimizzato per le sue necessità.
Si può prendere in esempio un sistema di controllo di un motore elettrico. L’e-secuzione dell’algoritmo di controllo necessita di assoluta precisione e stabilità delperiodo di campionamento e deve quindi essere implementatoin un processo realtime. Altri servizi di contorno, quali ad esempio la generazione di traiettorie, chenon richiedono vincoli temporali così stringenti saranno eseguiti da processi nonreal time.
utentemodulo
segnalishared memory
fifo
sensori
attuatorireal−timemodulo
gestisce sistemi e processi che necessitano di vincoli
stringenti
gestisce sistemi e processi che non necessitano di vincoli
temporali stringenti
parametriinserimento
salvataggio dati
Figura 3.3: Struttura generale della separazione dei compiti nella programmazionereal time.
L’interazione fra i processi eseguiti nei due ambienti può essere realizzata con imeccanismi di comunicazione che RTLinux mette a disposizione:
• FIFO: un buffer di caratteri, gestito come se fosse un file di caratteri dove illato real time scrive i dati e il lato Linux li legge o viceversa. La politica dilettura è quella first in first out quindi viene mantenuto l’ordine nell’invio deidati. Può essere gestito in due modi: a polling, oppure è possibile associareuna funzione handler alla FIFO che viene eseguita quando avvengono degliaccessi in lettura o scrittura sul buffer.
50 Controllo real time
• SHARED MEMORY: è un’area di memoria allocata nello spazio di kernel econdivisa fra i task real time e i processi Linux. Non è implementata nessunapolitica di gestione degli accessi che è tutta a carico del programmatore. Èadatta per lo scambio di grosse quantità di dati.
Come Linux anche RTLinux è un sistema modulare: è quindi possibile aggiun-gere qualsiasi capacità real time si desideri creando un modulo opportuno e cari-candolo nel kernel. Come in Linux un modulo real time è caricato nel kernel conil comando insmod e nel codice sono presenti le due direttiveinit_module() ecleanup_module() . Un modulo real time è quindi formalmente del tutto similead un normale modulo di Linux solo che al suo interno si utilizzano delle parti-colari funzioni che si occupano della creazione e gestione dei processi real time.Alcuni moduli base tra cui quelli che implementano la gestione dello schedulingdei processi real time e delle fifo sono distribuiti assieme al kernel di RTLinux.
3.3 Controllo del dito artificiale con RTLinux
Il primo problema che ci si è trovati ad affrontare dal punto di vista del controlloreal time è stato quello di implementare il controllore dei motori lineari. Per fareciò si è preso spunto da un lavoro precedentemente svolto [9]in cui si è fatto uso diMatlab, Simulink e del Real Time Workshop per la generazionedell’algoritmo dicontrollo e del relativo codice.
In questa tesi si è invece scelto di scrivere direttamente inlinguaggio C tutto ilcodice del controllore real time sfruttando l’API8 della scheda MULTI-Q PCI [20]e di RTLinux [8], andando a realizzare di fatto un device driver che implemental’algoritmo di controllo e i meccanismi di comunicazione con le applicazioni uten-te. Questa scelta è dettata dalla volontà di realizzare un software efficiente tramitel’uso dei soli strumenti messi a disposizione dal sistema base. Inoltre avendo pienaconoscenza del software si può rendere questo più flessibilee quindi più facilmenteadattabile a diverse tipologie di dispositivi.
3.3.1 Strutture software
Le principali strutture utilizzate nel modulo di controllosono dichiarate nel fi-le mqpcicontrol.h insieme i prototipi delle funzioni utilizzate per manipolare talistrutture.
8API: Application Programming Interface.
3.3. Controllo del dito artificiale con RTLinux 51
/* amplifier alarm control structure */
typedef struct {
MQPCI* Card; /* card structure pointer */
uint16_T fault_mask; /* AMP-FAULT bit mask */
uint16_T warn_mask; /* AMP-WARN bit mask */
int state; /* amplifier state */
} amplifier_t;
Questa struttura non è di utilizzo generale ma è strettamente legata all’uso delservo-controller LinMot E210 e viene utilizzata per testare lo stato dei segnalidi allarme e per salvarne lo stato. La funzione che inizializza la struttura è laamp_init() mentre la funzioneamp_signal() esegue il test dello stato dei se-gnali per poi salvarlo nel campostate della struttura. Nel caso si verifichi unasituazione di allarme, la funzioneemergency_stop() ferma i motori e disabilita iservo-controller.
Ad ogni motore lineare utilizzato viene associata una struttura del tipo:
/* linear motor structure */
typedef struct {
MQPCI* Card; /* card structure pointer */
int motornum; /* motor DAC channel */
double kpos; /* encoder output to position (mm) costant */
double kf2v; /* force to amplifier input voltage costant */
} lin_motor_t;
In questo modo è possibile, all’interno della stessa applicazione, utilizzare piùmotori anche con risoluzioni9 e guadagni diversi. È inoltre possibile cambiare ilcanale a cui ciascun motore è collegato semplicemente assegnando il nuovo canaleall’interno della struttura senza dover cambiare il resto del codice.
/* digital first order filter stucture */
typedef struct {
double a0,a1; /* denominator coefficient */
double b0,b1; /* numerator coefficient */
double old_input;
double old_output;
} d_filter_t;
9Questa flessibilità è stata utilizzata per correggere un malfunzionamento della scheda MULTI-QPCI, che presenta i segni dell’encoder del banco 1 invertitirispetto agli altri banchi, semplicementeaggiungendo al campokpos un segno negativo.
52 Controllo real time
Con questa struttura si implementa un filtro digitale del primo ordine. Diver-si filtri di questo tipo sono stati utilizzati nell’algoritmo di controllo per filtra-re opportunamente determinati segnali come ad esempio l’azione di controllo inmodo da evitare vibrazioni e bruschi colpi alla struttura meccanica. La funzio-ne init_filter() inizializza la struttura mentre la funzionefilter_output()
che richiede come ingresso, oltre al filtro utilizzato, il valore attuale del segnale dafiltrare, restituisce in uscita il valore del segnale filtrato.
/* generic PID controller */
typedef struct {
double kp,kv,ki; /* pid controller parameter */
double integral; /* intregral action state */
double maxint; /* max integral absolute value */
double oldinput; /* position state (for derivative control ) */
} PID_t;
Questa struttura implementa un controllore PID. Non viene utilizzata nell’algo-ritmo di controllo ma serve per poter controllare in posizione i motori lineari.
/* controller structure */
typedef struct {
int ctrlnum; /* controller indentification number */
lin_motor_t *ctrlmotor;
d_filter_t *filter;
int forceflag; /* mode control flag position=0 force=1 */
double thresold;
double position;
double output;
PID_t* PID;
shm_tr* tr; /* trajectory data */
unsigned long tr_index; /* trajectory data index */
} controller_t;
I dati relativi al controllo di ogni motore sono contenuti all’interno di questastruttura. La struttura può essere inizializzata tramite la funzioneinit_controller() .A ciascun controllore è associato un motore, un filtro per filtrare l’azione di control-lo, eventualmente un PID nel caso il motore sia controllato in posizione e un bufferche contiene i dati relativi alla traiettoria da inseguire.Questo controllore ha dueprincipali modalità di funzionamento: come controllore diforza o come controlloredi posizione. Per cambiare la modalità di funzionamento basta settare opportuna-mente la variabileforceflag oppure utilizzare le funzioniforce_controller()
3.3. Controllo del dito artificiale con RTLinux 53
e position_controller() . Per leggere la posizione degli attuatori si fa uso dellafunzionectrl_read_pos() mentre per inseguire o attuare un setpoint(a secondadella modalità di funzionamento) si utilizza la funzionectrl_setpoint_track() .
3.3.2 Il modulo di controllo mqpcicontrol.o
Questo modulo rappresenta il cuore del sistema di controlloreal time. Il suocompito principale è quello di eseguire l’algoritmo di controllo discreto con periododi campionamento fisso di 1ms.
L’algoritmo di controllo viene eseguito da un thread con massima priorità cheviene risvegliato dallo scheduler real time all’inizio di ogni periodo di campiona-mento. Dopo aver eseguito tutte le operazioni questo threadsi pone nuovamente inattesa di essere risvegliato dallo scheduler.
Su questo modulo sono state inoltre implementate alcune funzionalità aggiunti-ve di fondamentale importanza:
• FIFO per i comandi: tramite questa FIFO è possibile inviare comandi aldriver. A seconda del tipo di comando possono essere inviatianche i datinecessari al comando (vedi paragrafo 2.8.1.
• Thread per la registrazione dei dati: questo processo viene eseguito pe-riodicamente (con frequenza minore o uguale di quella del thread di control-lo e con priorità minore) e si occupa di campionare i principali dati relativiall’algoritmo di controllo.
• FIFO per i dati : tramite questa FIFO il driver comunica la disponibilità dinuovi dati per la registrazione o il plottaggio.
Per realizzare tutto ciò, il modulo di controllo, al suo caricamento quindi nellafunzione init_module() , deve effettuare una serie di operazioni prima di potereseguire il vero e proprio algoritmo di controllo. Queste operazioni comprendono:
1. Inizializzazione della scheda MULTI-Q PCI;
2. Inizializzazione delle strutture software;
3. Allocazione dei buffer per lo scambio dei dati;
4. Creazione delle FIFO e dei relativi handler;
5. Generazione dei thread di controllo e di registrazione dati.
Per comprendere meglio i meccanismi di funzionamento di questo modulo,entriamo nel dettaglio di come siano state realizzate le diverse funzionalità.
54 Controllo real time
Inizializzazione della scheda MULTI-Q PCI
Le funzioni per programmare e per dialogare con la scheda MULTI-Q PCI sonocontenute nelle libreria libquanser.a che implementa l’API della scheda. Gli headerdi queste funzioni sono contenuti nei seguenti file:
mqpci.h Funzioni per l’inizializzazione e il controllo delle schedamqpciad.h Funzioni per leggere gli ADCmqpcida.h Funzioni per scrivere sui DACmqpcidi.h Funzioni per gestire gli input digitalimqpcido.h Funzioni per gestire gli output digitalimqpcien.h Funzioni per leggere gli encodermqpciext.h Funzioni di controllo aggiuntivemqpcitb.h Funzioni di gestione dei timer hardwaremqpciwd.h Funzioni di programmazione del watchdog timer
Tabella 3.1: Header file per la programmazione della scheda MULTI-Q PCI.
La prima cosa da fare per inizializzare la scheda è utilizzare la funzioneMQPCI_Initialize() per settare i parametri della strutturaCard0 che contienetutte le informazioni relative alla scheda MULTI-Q PCI. Dopo aver affettuato questaoperazione è possibile comandare i vari dispositivi di I/O presenti sulla scheda.
...
#include “mqpci.h”
#include “mqpcida.h”
#include “mqpcido.h”
...
MQPCI Card0;
...
int init_module()
{
...
/* initializing MULTI-Q PCI card */
if (MQPCI_Initialize(0,&Card0)) rtl_printf("Card initi alized!\n");
else {
rtl_printf("Card0 could not be allocated!\n");
return -1;
3.3. Controllo del dito artificiale con RTLinux 55
}
/* reset all output channel*/
MQPCI_dacOutput(&Card0,0,MQPCI_dacVoltageToOutput(0 .0));
MQPCI_dacOutput(&Card0,1,MQPCI_dacVoltageToOutput(0 .0));
MQPCI_dacOutput(&Card0,2,MQPCI_dacVoltageToOutput(0 .0));
MQPCI_dacOutput(&Card0,3,MQPCI_dacVoltageToOutput(0 .0));
/* configuring digital output and input channel */
MQPCI_DIOWriteBank(Card,DIG_IO_BANK,CHAN_CONF);
...
/* activating motor */
MQPCI_DIOWriteBank(&Card0,DIG_IO_BANK,MOTOR0_ON|MOT OR1_ON|MOTOR2_ON);
...
}
Dopo aver posto i canali di uscita a zero viene effettuata la configurazione deicanali digitali, procedura necessaria per indicare alla scheda quali di questi cana-li vengono utilizzati come input e quali come output. Una volta completate tuttequeste procedure è possibile inviare ai servo-controller il segnale di abilitazione deimotori. È necessario porre a zero le uscita dei DAC prima di abilitare i motori,oltre che per una questione di sicurezza, perchè ciò è espressamente richiesto daiservo-controller: infatti se questi ultimi vengono abilitati mentre in ingresso vi èuna tensione diversa da zero, viene generato un segnale di AMP-FAULT e il servosi blocca fino a quando non viene tolto il segnale di abilitazione.
Inizializzazione delle strutture software
In questa parte vengono inizializzate tutte le strutture che i due thread che ver-ranno creati in seguito devono utilizzare. La più importante è senza dubbio la strut-tura oggetto del controllo, ovvero la strutturafinger di tipo finger_t che descrivele caratteristiche geometriche del dito artificiale e la suaconfigurazione istantanea.Questa struttura è descritta in dettaglio nel paragrafo 2.8. In questo caso, dovendocontrollare un solo dito, è presente una sola struttura di questo tipo ma è suffi-ciente dichiarare più strutture e inizializzarle con i parametri di ciascun dito percontrollarne più di una.
int init_module()
56 Controllo real time
{
...
Init_Finger(&finger);
...
amp_init(&lifier,&Card0,fault_mask,warn_mask);
/* initializing motor structure */
for(i=0;i<MOTOR_NUM;i++){
init_motor(&Card0,&motor[i],KPOS,KF2V,i);
init_filter(&filter[i],AA0,AA1,BB0,BB1);
init_controller(&controller[i],&motor[i], \\
&filter[i],&PID[i],THRESOLD,i);
force_controller(&controller[i]);
}
...
}
Allocazione dei buffer per lo scambio dei dati
Per monitorare l’algoritmo di controllo è necessario scambiare con le applica-zioni a livello utente i valori di diverse variabili utilizzate nel codice. Le FIFO nonsono adatte per scambi continui di grandi quantità di dati percui per questo compitosi è deciso di ricorrere all’uso dellashared memory. Tramite questo meccanismoè possibile allocare all’interno dei moduli real time aree di memorie condivise frakernel spaceeuser spaceutilizzabili per lo scambio di dati.
Per avere un meccanismo efficiente di salvataggio dei dati siè scelto di adottarela tecnica del doppio buffer. Con questo sistema vengono allocate due aree di me-moria delle stesse dimensioni e mentre una viene utilizzataper salvare i dati, l’altraviene utilizzata dalle applicazioni che ne richiedono l’utilizzo per le successive ela-borazioni (plottagio, salvataggio su disco, statistiche ecc...). Quando una area dimemoria è stata riempita viene eseguito lo scambio fra i due buffer e quello cheprima veniva utilizzato dalle applicazioni esterne ora viene utilizzato per la scritturae viceversa.
Un ulteriore buffer di memoria viene allocato per passare all’algoritmo di con-trollo i dati relativi alla traiettoria da inseguire. L’applicazione utente, dopo averriempito questo buffer con i dati relativi alla traiettoria, invia, tramite la FIFO co-
3.3. Controllo del dito artificiale con RTLinux 57
mandi FIFO_IN (vedi paragrafo successivo), il comando che abilità l’iseguimen-to della traiettoria desiderata. Per maggiori dettagli suicomandi che è possibilepassare al modulo real time si veda il paragrafo 2.8.1.
int init_module() { ...
if((shm_data1 = (shm_data *) mbuff_alloc ("data1", \\
sizeof (shm_data)))==NULL){
rtl_printf("Shared memory allocation fail!");
return -1;
}
if((shm_data2 = (shm_data *) mbuff_alloc ("data2", \\
sizeof (shm_data)))==NULL){
rtl_printf("Shared memory allocation fail!");
return -1;
}
bufnum=1;
if((tr = (shm_tr *) mbuff_alloc ("traiettoria", \\
sizeof (shm_tr)))==NULL){
rtl_printf("Shared memory allocation fail!");
return -1;
}
...
}
Creazione delle FIFO e dei relativi handler
Per lo scambio di piccole quantità di dati, come ad esempio segnali o comandi,con frequenze non troppo elevate, le FIFO mettono a disposizione un meccanismoefficiente. Particolarmente interessante è poi la possibilità da parte del task realtime di associare una funzione ad una FIFO che viene eseguitaogni qual volta siadisponibile in essa un nuovo dato. Si noti che l’immissione di comandi sulla FIFO èun’operazione asincrona perchè legata alle richieste dell’utente e delle applicazioniche dialogano con il modulo. È quindi possibile delegare ad un unica funzione larisposta ai comandi che vengono inviati al modulo real time:questa funzione sidovrà solo preoccupare di identificare il comando e di cambiare conseguentemente
58 Controllo real time
lo stato del sistema. Per maggiori dettagli sui comandi che èpossibile passare almodulo real time si veda il paragrafo 2.8.1.
int in_handler(unsigned int fifo)
{
rtf_get(FIFO_IN,&message,sizeof(message));
switch(message.command){
...
}
}
int init_module()
{
...
rtf_destroy(FIFO_OUT);
rtf_destroy(FIFO_IN);
if(rtf_create(FIFO_OUT,160)<0) {
rtl_printf("Unalbe to create fifo_out\n");
return -1;
}
if(rtf_create(FIFO_IN,160)<0) {
rtl_printf("Unalbe to create fifo_in\n");
return -1;
}
/* creating FIFO_IN handler */
rtf_create_handler(FIFO_IN,&in_handler);
...
}
La funzione che gestisce laFIFO_IN deve quindi, come prima operazione, leg-gere la strutturamessage per poi eseguire unaswitch() sulla base del valore delcampocommandper identificare il comando ed eseguire le necesarie operazioni.
La FIFO_OUT viene utilizzata per segnalere all’applicazione che ne fa richie-sta la disponibilità di un buffer di memoria condivisa con nuovi dati passandoall’applicazione l’indice del buffer.
3.3. Controllo del dito artificiale con RTLinux 59
Generazione dei thread di controllo e di registrazione dati
Questa è l’ultima fase dell’inizializzazione del modulo dicontrollo. In questafase vengono avviati due thread: uno è il vero e proprio algoritmo di controllo men-tre il secondo si occupa di campionare i dati e di passarli alle applicazioni esternetramite la memoria condivisa. Si noti che il thread che esegue l’algoritmo di con-trollo, per ovvi motivi, viene eseguito con priorità maggiore rispetto al thread diregistrazione dei dati.
int init_module()
{
...
/* set main loop scheduling parameter */
pthread_attr_init(&main_attr);
p.sched_priority=1;
pthread_attr_setschedparam(&main_attr,&main_param);
/* create main loop thread */
pthread_create(&controltask,&main_attr,mainloop,(vo id *)1);
/* main thread use FPU */
pthread_setfp_np(controltask,1);
/* set main thread period */
pthread_make_periodic_np(controltask,gethrtime(),SA MPLETIME);
/* set data task scheduling parameter */
pthread_attr_init(&data_attr);
p2.sched_priority=0;
pthread_attr_setschedparam(&data_attr,&data_param);
/* create data thread */
pthread_create(&datatask,&data_attr,fifoloop,(void * )1);
/* data thread use FPU */
pthread_setfp_np(datatask,1);
/* set data thread period */
pthread_make_periodic_np(datatask,gethrtime(),DATAT IME);
...
}
60 Controllo real time
3.4 L’algoritmo di controllo
Entriamo ora nel dettaglio dell’implementazione softwaredell’algoritmo di con-trollo real time.
La prima istruzione che si incontra nel ciclo principale è lapthread_wait_np() .Questa istruzione è di fondamentale importanza perchè è quella che pone il th-read in attesa finchè lo scheduler, all’inizio di un nuovo periodo di campionamento,non risveglia il processo. È tramite questo meccanismo che èpossibile eseguireperiodicamente e con tempistiche precise l’algoritmo di controllo.
/* main control thread */
void *mainloop(void *t)
{
int i=0;
int state=0;
int start=0;
while(1){
/* waiting for periodic execution */
pthread_wait_np();
La sequenza di operazioni effettuate dall’algoritmo ad ogni ciclo può essereriassunta in:
1. Verifica dello stato del sistema;
2. Acquisizione dei dati;
3. Calcolo dell’azione di controllo;
4. Attuazzione del controllo;
nel seguito descritte in dettaglio.
Verifica dello stato del sistema
In questa fase vengono testati i segnali provenienti dai servo-controller e se so-no presenti segnali di AMP-FAULT i servo vengono disattivati e la procedura dicontrollo termina.
3.4. L’algoritmo di controllo 61
...
/* check amplifier state */
state=amp_signal(&lifier);
if(state==2){
rtl_printf("Amplifier fault!!!\n");
emergency_stop(&Card0);
fault=1;
return NULL;
}
Acquisizione dei dati
L’algoritmo di controllo vero e proprio inizia qui in quantole operazioni effet-tuate precedentemente sono operazioni necessarie a sincronizzare il software e atestare eventuali allarmi. Quindi, per monitorare il tempodi esecuzione dell’algo-ritmo, si pone in uscita al DAC inutilizzato della scheda MULTI-Q PCI un segnaleche è possibile visualizzare tramite un’oscilloscopio.
La parte di acquisizione dati vera e propria consiste nel leggere al posizionedegli slider dei motori lineari da cui è possibile stimare gli angoli dei giunti deldito. A seconda poi che il modulo di controllo si connesso o meno all’applicazioneche gestisce lo scambio dei dati per il controllo si aggiornano gli angoli di giunto ela forza del controllo di impedenza. Si noti che nel caso in cui non siano disponibilii dati provenienti dalla telecamera il controllo d’impedenza viene disabilitato.
if((state==0 || state==1) && start){
/* control procedure start signal for execution time monito ring */
MQPCI_dacOutput(&Card0,3,MQPCI_dacVoltageToOutput(1 ));
/* read linear motor slider position */
h.h1=(double)ctrl_read_pos(&controller[0])*(double) KMM2M;
h.h2=(double)ctrl_read_pos(&controller[1])*(double) KMM2M;
h.h3=(double)ctrl_read_pos(&controller[2])*(double) KMM2M;
...
/* joint angle estimator */
tend2ang(&finger,&tendq,&htemp);
if(connected){
q.q1=camq.q1;
62 Controllo real time
q.q2=camq.q2;
q.q3=camq.q3;
F.Fx=camF.Fx;
F.Fy=camF.Fy;
} else {
q.q1=tendq.q1;
q.q2=tendq.q2;
q.q3=tendq.q3;
F.Fx=0.0;
F.Fy=0.0;
}
Calcolo dell’azione di controllo
Come prima cosa vengono calcolati i parametri (seni e coseni) relativi agli ango-li di giunto del dito che vengono salvati nella strutturafinger . Questa operazioneviene effettuata per non ripetere diverse volte nelle successive funzioni questi calco-li che sono i più computazionalmente onerosi. Successivamente vengono calcolatele coppie da applicare ai giunti, vengono compensate le coppi dovute all’elasticitàdelle cerniere e la proceduratau2Fh() calcola le tensioni da applicare ai tendini.In seguito, per rallentare il sistema che sarebbe altrimenti instabile a causa dellabassa frequenza di aggiornamento delle immagini e del ritardo di trasmissione deidati, viene corretta la tensione applicata ai tendini in base alla loro velocità di spo-stamento. L’azione di controllo viene infine filtrata per evitare brusche variazioni el’innescarsi di vibrazioni nella struttura meccanica e soprattutto nei tendini.
...
init_sin_cos(&finger,&q);
trasp_jacob_qx(&finger,&jacobian,&tau,&q,&F);
ke_compensation(&finger,&comp_tau,&q);
tau_sum(&tau,&tau,&comp_tau);
tau2Fh(&finger,&Fh,&q,&tau);
Fh.Fh1-=KFV*hdot.h1;
Fh.Fh2-=KFV*hdot.h2;
Fh.Fh3-=KFV*hdot.h3;
Fh.Fh1=filter_output(&Fh1filter,Fh.Fh1);
Fh.Fh2=filter_output(&Fh2filter,Fh.Fh2);
3.5. Conclusioni 63
Fh.Fh3=filter_output(&Fh3filter,Fh.Fh3);
Attuazione del controllo
A questo punto vengono attuati i valori del controllo calcolati precedentementedopo di che viene resettato il segnale per il calcolo del tempo di esecuzione. Dopoaver effettuato queste operazioni il ciclo inizia da capo rimettendosi in attesa.
...
ctrl_setpoint_track(&controller[0],Fh.Fh1);
ctrl_setpoint_track(&controller[1],Fh.Fh2);
ctrl_setpoint_track(&controller[2],Fh.Fh3);
/* execution terminated signal*/
MQPCI_dacOutput(&Card0,3,MQPCI_dacVoltageToOutput(0 ));
}
}
return NULL;
}
3.5 Conclusioni
RTLinux rappresenta una soluzione interessante a livello industriale e a bassocosto per lo sviluppo di applicazioni hard real time. Tramite l’uso di una schedadi I/O, congiuntamente a RTLinux, si può inoltre realizzareun sistema di control-lo in retroazione in maniera piuttosto semplice e rapida. Inquesto capitolo si èdescritto, a livello software, come la combinazione RTLinux + scheda di I/O siastata utilizzata per il controllo del dito robotico articolato focalizzando l’attenzionesulla realizzazione del ciclo di controllo real time. Il software realizzato è stato re-so modulare in modo da poter essere utilizzato in futuro anche con più di un ditocontemporaneamente.
Capitolo 4
Visione artificiale
Il sistema di visione artificiale realizzato in questa tesi èstato utilizzato per effet-tuare misure sul dito robotico descritto nel capitolo 2. Unodei motivi per cui è statautilizzata una telecamera come sistema di misura risiede nella completa mancanzasul dito di sensori tramite i quali poter rilevare le grandezze necessarie al controllodel dito robotico. Vi sono poi una serie di fattori che introducono incertezza nelmodello cinematico del dito:
• essendo i giunti del dito realizzati con elementi flessibili, non è noto concertezza il centro di istantanea rotazione dei vari link durante il moto men-tre, nel ricavare il modello cinematico del dito, si è assunto che i giunti sicomportassero come cerniere ideali;
• i tendini sono elementi elastici. La loro lunghezza cambia quindi in funzionedella forza ad essi applicata mentre in precedenza si è assunto che i tendinifossero inestensibili;
• nel ricavare il legame fra spostamento dei tendini e angoli di giunto espressodalle 2.5 si è ipotizzato che:
1. il contatto fra tendine e guida nel punto d’appoggio sia puntiforme e chela curvatura dei tendini avvenga con raggio nullo nel caso del passaggiointerno del tendine;
2. il tendine rimanga parallelo alla guida in cui scorre il tendine stessoall’interno del link nel punto di ingresso e di uscita dalla guida stessa eche nel tratto intermedio fra i due link descriva un arco di circonferenzache ha come centro il centro della cerniera che unisce le due falangi;
mentre in realtà si ha che:
66 Visione artificiale
1. il punto di appoggio del tendine non è né puntiforme, né fisso ma di-pende dall’angolo fra le falangi e la curavatura del tendinedipende,ovviamente, dalla tensione applicata al tendine stesso;
2. il tendine tende a stirarsi all’aumentare della forza di trazione ad essoapplicata aumentando così il raggio di curvatura del tendine stesso.
L’uso della telecamera permette di risolvere in maniera concettualmente sempli-ce i problemi legati all’incertezza del modello cinematicodel dito robotico andandoa “vedere” in che posizione si trovano i vari link e in particolare la punta del ditorobotico. Tramite telecamera e anche possibile effettuareprocedure di identifica-zione dei parametri del dito non noti in modo da poter creare un nuovo modellocinematico più idoneo ad essere utilizzato al lato pratico rispetto al modello teorico.
A differenza di precedenti casi in cui si sono realizzati algoritmi di visione ar-tificiale [11], in questa tesi le caratteristiche più importanti che si richiede abbiaquesto algoritmo sono l’elevata risoluzione e la precisione di misura.
Applicazionein user space
Applicazionein user space
Driverdel framegrabberin kernel space
Driverdel framegrabberin kernel space
Libbgrag threadin user space
a) b)
Comunicazione utilizzando le libbgrab Comunicazione senza le libbgrab
Trasferimento dati
Figura 4.1: Schema di comunicazione fra applicazione utente e driver delframegrabber a) con le libbgrab e b) senza le libbgrab.
Nelle realizzazioni precedenti si è fatto uso della libreria libbgrab che fornisceun’interfaccia semplificata per il dialogo con il driver della scheda di acquisizionevideo e si occupa di creare un thread per gestire lo scambio dei dati fra il driver
4.1. Il programma greydetect 67
a livello di kernel e l’applicazione a livello utente. Queste librerie creano quindiuno strato di separazione fra il driver e l’applicazione. Dal punto di vista del pro-grammatore questo può rappresentare un vantaggio, grazie alla maggior semplicitàdi stesura del software, ma dal punto di vista del carico computazionale è senz’altrouno svantaggio perchè si introduce uno strato aggiuntivo diseparazione fra devicedriver e applicazione a livello utente che implica che i datiacquisiti devono transita-re prima per il thread creato dalle libbgrab per poi essere trasferiti all’applicazioneche ne fa richiesta.
Per questo motivo si è scelto di evitare di usare le libbgrab per dialogare conil driver della scheda di acquisizione video e si è scelto di scrivere direttamente ilcodice necessario allo scambio dei dati relativi all’immagine acquisita in modo daottimizzare le risorse necessarie. Questo approccio a portato ad una significativadiminuzione del carico computazionale giustificando la scelta fatta.
4.1 Il programma greydetect
Il programma greydetect si occupa dell’acquisizione, dell’elaborazione e dellavisualizzazione dell’immagine; inoltre gestisce l’interazione con l’utente e l’inte-faccia grafica, ed infine si occupa della comunicazione via ethernet con il modulodi controllo real time.
L’immagine rispresa dalla telecamera viene mostrata a video sovrapponendo adessa alcune informazioni utili all’utente. Tramite la finestra mostrata in figura 4.2 èpossibile svolgere diverse operazioni:
• è possibile spostare con il mouse le patch1 in modo da posizionarle sui cer-chietti qualora non fossero nella giusta posizione;
• è possibile selezionare il punto dove spostare la punta del dito;
• è possibile effettuare misure tramite un righello virtualeche può essere spo-stato semplicemente trascinandolo con il mouse2.
All’immagine ripresa dalla telecamera, vengono sovrapposte, prima che l’im-magine venga visualizzata a video, alcune informazioni grafiche visibili nelle figure4.2 e 4.4:
1Per patch si intende il riquadro che contorna ciascun cerchietto nero presente sul dito.2Il righello virtuale non è visibile in figura 4.2 perchè può essere escluso per avere una visuale
più pulita. In figura 4.4 è visibile il righello virtuale ma sono state tolte le informazioni grafiche suisistemi di riferimento.
68 Visione artificiale
Figura 4.2: Visuale della telecamera.
Figura 4.3: Interfaccia grafica del programma greydetect.
4.1. Il programma greydetect 69
Figura 4.4: Immagine della telecamera in cui è visibile il righello virtuale.
• Vengono evidenziati i vertici delle patch in modo da renderle visibili all’uten-te e vengono numerate in modo da poterle disporre nell’ordine corretto;
• Si disegnano i sistemi di riferimento dei vari link. Le coordinate dell’originedell’ultimo sistema di riferimento, ovvero la punta del dito, vengono proiet-tate sugli assi del sistema di riferimento base, quello del link zero, e vienestampato a video il loro valore in mm. Queste informazioni possono essereinserite o escluse dall’utente;
• In alto a sinistra vengono stampati, in gradi, i valori degliangoli di giunto peravere un riscontro immediato del loro valore;
• Viene disegnato il righello virtuale graduato in mm. Il righello virtuale puòessere inserito o escluso dall’utente.
Nella figura 4.3 è invece visibile l’interfaccia grafica del programma tramite laquale si possono aggiustare i parametri di contrasto e luminosità del framegrab-ber, si può selezionare il livello di saturazione, si possono settare i parametri delcontrollo d’impedenza ed accedere ad alcune funzioni di utilità generale. È inoltrepossibile stabilire la connessione con il modulo di controllo e monitorare eventualimessaggi tramite un buffer di testo.
70 Visione artificiale
Per la visualizzazione delle immagini a schermo sono state utilizzate le librerieXLib3 mentre l’interfaccia grafica è stata sviluppata utilizzando le librerie GTK4.Per disegnare l’interfaccia grafica si è fatto ricorso a Glade5.
greydetectapplication
Xserver
Network
Framegrabberdriver
Comunicationthread
GUIthread
Mainthread
Figura 4.5: Schema logico dell’architettura del programmagreydetect.
A livello software, il programma greydetect è composto da tre thread:
• un thread (main thread) implementa il vero e proprio algoritmo di visione ar-tificiale e si occupa di gestire anche l’interazione dell’utente con l’immaginedella telecamera;
• un thread (GUI thread) gestisce l’interfaccia grafica;
• un terzo thread (Comunication thread) viene attivato per lacomunicazionevia ethernet con il modulo di controllo.
Tralasciando di approfondire la trattazione sullo sviluppo dell’interfaccia graficae della comunicazione, rimandando ad altri testi tali approfondimenti [12, 18], siesamina nel dettaglio il thread che implementa l’algoritmodi visione artificiale.Questo thread esegue, ogni volta che il driver del framegrabber mette a disposizioneuna nuova immagine, una sequenza di operazioni fra le quali le principali sono:
1. Acquisizione dell’immagine: vengono copiati i dati messi a disposizione daldriver del framegrabber;
3Le librerie XLib sono un componente fondamentale dell’Xserver, il motore grafico dei sistemioperativi UNIX-like. Le applicazioni che fanno uso della grafica (i client) si connettono all’Xserverper avere accesso alle risorse grafiche.
4GTK: Gimp ToolKit. Si tratta di una libreria di componenti per la creazione di interfacciegrafiche molto diffusa in ambiente Linux e disponibile ancheper Microsoft Windows.
5Glade GTK+ User Interface Builder è un’applicazione per facilitare la creazione di interfaccegrafiche.
4.2. Acquisizione dell’immagine 71
2. Correzione della distrorsione radiale introdotta dallatelecamera;
3. Definizione delle caratteristiche cinematiche del dito robotico attraverso leinformazioni ricavate dall’immagine;
4. Trasmissione dei dati al server;
5. Sovrapposizione all’immagine delle informazioni grafiche necessarie;
6. Aggiornamento del framebuffer video;
7. Esecuzione dei comandi utente;
8. Attesa di una nuova immagine.
Le principali operazioni effettuate dall’algoritmo di visione artificiale verrannodescritte in dettaglio nei prossimi capitoli.
4.2 Acquisizione dell’immagine
Come già precedentemente detto, si è scelto di comunicare direttamente conil driver del framegrabber per poter scambiare più velocemente i dati relativi al-l’immagine. In questo modo è anche possibile sincronizzarel’algoritmo di visioneartificiale con il driver in modo che una nuova elaborazione inizi non appena èdisponibile una nuova immagine.
/* Sync old images in the sequence to complete grabbing */
ioctl (frame_device, VIDIOCSYNC, &videommap[memmapcoun t].frame);
if (ioctl (frame_device, VIDIOCMCAPTURE, \\
&videommap[memmapcount]) == -1) {
perror ("VIDIOCMCAPTURE");
return;
}
memmapcount=(memmapcount+1)%2;
La prima istruzione di questa sequenza pone il thread in attesa che il driver delframegrabber invii il segnale di sincronismo che indica la presenza di una nuovaimmagine. Lo scambio delle informazioni avviene tramite l’area di memoria condi-visavideommap . Per assicurare che non avvengano conflitti fra scritture daparte deldriver e lettura da parte del thread di elaborazione dell’immagine viene adottata latecnica del doppio buffer. Ogni volta che si rende disponibile una nuova immagine,
72 Visione artificiale
il buffer contenente l’immagine viene elaborato, mentre sull’altro buffer, il driverinizia a scrivere i dati relativi ad una nuova acquisizione.
Prima di proseguire nella descrizione del funzionamento dell’algoritmo di vi-sione artificiale è neccessario fare alcune precisazioni per quanto riguarda i formatidelle immagini: la telecamera utilizzata è in banco e nero e l’immagine acquisitàdal driver risulta essere in toni di grigio6. Per visualizzare l’immagine a video ènecessario adottare la stessa codifica utilizzata dall’Xserver, che in questo caso è lacodifica RGB5657. Per convertire l’immagine da un formato all’altro si è quindi do-vuto far ricorso ad una lookup table in cui si stabilisce la corrispondenza fra i pixelnei due diversi formati. Il formato dell’immagine su cui si effettua l’elaborazione èinvece lo stesso utilizzato dal driver: non è quindi necessaria alcuna conversione diformato.
Ogni volta che il framegrabber mette a disposizione una nuova immagine nevengono fatte due copie: una copia viene fatta nel framebuffer per la visualizzazionea video e una copia in bianco e nero viene salvata per le successive elaborazioni.
4.3 Correzione della distorsione radiale
Un aspetto fondamentale nell’algoritmo di visione artificiale implementato èla correzione della distorsione radiale introdotta dalla telecamera. Per corregge-re l’immagine si ricorre a due lookup table tramite le quali èpossibile ricostruireun’immagine non distorta a partire da quella distorta.
Dal punto di vista implementativo, la procedura di correzione della distorsio-ne radiale può essere divisa in due parti, come evidenziato in figura 4.6: la primaparte, a sinistra nell’immagine, viene eseguita una sola volta quando si effettua lacalibrazione della telecamera e viene eseguita off-line, ovvero al di fuori del cicloprincipale del programma, perchè la sua esecuzione richiede molto tempo (diversiminuti sulla macchina utilizzata per l’elaborazione dell’immagine) mentre la secon-da parte, a destra nell’immagine, viene eseguita ogni voltache si ha a disposizioneuna nuova immagine.
Si può quindi distinguere una procedura di calibrazione off-line della telecamerae una di correzione on-line dell’immagine.
6In questo formato ogni pixel è codificato con 8 bit che indicano l’intensità del bianco (0=nero,255=bianco).
7Nella codifica RGB565 ogni pixel dell’immagine è rappresentato da 16 bit, i primi 5 MSBcodificano l’intensità del rosso (Red), i successivi 6 l’intensità del verde (Green) e gli ultimi 5 LSBl’intensità del blu (Blue).
4.3. Correzione della distorsione radiale 73
Griglia dicalibrazione
Matrici dicalibrazione
Sequenzavideo
Correzione on-linedella sequenza videotramite look-up table
Sequenzavideo corretta
Calcolo off-linedei parametri di correzione
A. Estrazione dei punti dallagriglia di calibrazioneB. Calcolo dell’errore divisualizzazioneC. Calcolo dei parametri dicorrezioneD. Costruzione delle matricidi correzione
Figura 4.6: Schema implementativo dell’algoritmo di correzzione dell’immagine.
4.3.1 La distorsione radiale
In figura 4.7 viene evidenziato, tramite alcune file di punti,l’effetto della distor-sione radiale rispetto a quello che dovrebbe essere l’immagine corretta.
(u′c,v
′c)
~P′(x
′i ,y
′i) θ′
y′
x′
(a)
~P(xi ,yi)
θ
y
x(uc,vc)
(b)
Figura 4.7: Variabili e sistemi di riferimento utilizzati per indicare la posizione deipunti nell’immagine distorta (a) e nell’immagine corretta(b).
L’obbiettivo da raggiungere è quello di trovare un metodo per ricostruire l’im-magine corretta a partire da quella distorta. Inoltre, per effettuare misure affidabi-li, è necessario conoscere la corrispondenza fra le dimensioni visualizzate con latelecamera e le dimensioni reali degli oggetti visualizzati.
Si assume che i centri delle due immagini coincidano, per cui(u′c,v
′c)≡ (uc,vc).
74 Visione artificiale
Le coordinate(x′,y
′) indicano la posizione di un generico pixel dell’immagi-
ne distorta mentre(x,y) indicano la posizione di un generico pixel nell’immaginecorretta. I centri dei cerchietti neri nell’immagine distorta e nell’immagine cor-retta vengono indicati rispettivamente con(x
′ci,y
′ci) e (xci,yci). Per questo proble-
ma sono particolarmente interessanti le coordinate polaripiane del generico pi-xel nell’immagine distorta(ρ′
,θ′) e dell’immagine corretta è(ρ,θ). Si può quindi
scrivere:
ρ′=√
(x′ −u′c)
2+(y′ −v′c)
2, θ′= arctan
(
y′ −v
′c
x′ −u′c
)
(4.1)
ρ =√
(x−uc)2+(y−vc)2, θ = arctan
(
y−vc
x−uc
)
(4.2)
Secondo il modello classico di distorsione radiale [19], sipossono ricavare lerelazioni che legano le coordinate polari dell’immagine distorta e quelle dell’imma-gine corretta come:
ρ =M
∑m=1
αmρ′m, θ = θ
′(4.3)
dove conM si indica il grado del polinomio utilizzato nel modello di distorsioneradiale e conαm l’m-esimo coefficiente del polinomio.
Conoscendo i coefficienti del polinomio è quindi possibile ricostruire le coordi-nate cartesiane dei pixel dell’immegine corretta partendoda quelle dell’immaginedistorta.
x = uc +ρcosθ′, y = vc +ρsinθ′ (4.4)
4.3.2 Calcolo dei coefficienti di correzione
Per quanto riguarda la procedura di calibrazione, questa sibasa essenzialmentesul confronto di due immagini: una griglia di punti costruita ad hoc (vedi figura4.8) quindi dalle caratteristiche geometriche note viene stampata su di un foglio esuccessivamente inquadrata dalla telacamera che ne acquisisce un frame (immaginedistorta) che viene poi confrontato con la griglia stessa cosi come è stata stampata(immagine non distorta).
L’algoritmo di visione artificiale è in grado di riconoscerei punti della griglia e
4.3. Correzione della distorsione radiale 75
Figura 4.8: Griglia di calibrazione.
quindi è possibile estrarre dall’immagine distorta le coordinate(x′ci,y
′ci) dei centri
dei punti e quindi ancheρ′ci.
Si definisce la distanza di un punto dal centro dell’immaginedopo che è stataapplicata una qualche correzione come
ρ′′(∆) =
M
∑m=1
αm(∆)ρ′m (4.5)
dove conαk(∆) si indicanoM coefficienti arbitrari opportunamente scelti.A questo punto si può definire l’errore di visualizzazione introdotto dalla distor-
sione radiale come
E(∆) =1N
N
∑i=1
∣
∣
∣ρci−ρ
′′ci(∆)
∣
∣
∣=
1N
N
∑i=1
∣
∣
∣
∣
∣
ρci −M
∑m=1
αm(∆)ρ′mci
∣
∣
∣
∣
∣
(4.6)
dove conN si indica il numero di punti presente nella griglia di calibrazione. Ora ilproblema consiste nel trovare un algoritmo per il calcolo dei coefficientiαm(∆) cheminimizzano l’errore di visualizzazione.
In figura 4.9 viene schematizzata una procedura iterativa che calcola l’erroreE(∆) per ogni nuovo set di coefficienti. I coefficienti vengono aggiornati secondola legge
αm(∆+1) = αm(∆)+ambE(∆)1
(
∂E(∆)∂αm
) (4.7)
dovea e b definiscono la velocità di convergenza dell’algoritmo e∂E(∆)∂αm
e il gra-
76 Visione artificiale
−
+ρci
ρ′ci(∆)
ρ′ci(∆+1)αm(∆+1)E(∆)
Hold
Calcolodell’errore di
visualizzazione
Calcolo deicoefficienti
Correzionedell’immagine
Immagineacquisita dalla
telecamera
Figura 4.9: Schema in retroazione per il calcolo dei coefficienti del polinomio.
diente dell’errore dato da:
∂E(∆)
∂αm= −
1N
N
∑i=1
(
ρ′mci sign(ρci−αmρ
′mci ))
(4.8)
4.3.3 Correzione on-line delle immagini
A questo punto è noto come ottenere i coefficienti del polinomio di correzione.Ciò non è però sufficiente per il nostro scopo. Come già precedente mente accen-nato, per motivi legati principalmente alla complessità computazionale, la correzio-ne delle immagini che via via vengono acquisite viene fatta tramite due tabelle olook-up table. Il motivo per cui sono necessarie due tabellerisiede nel fatto che ènecessario mappare i punti dell’immagine corretta nei punti dell’immagine distorta,mappatura che può essere espressa nel seguente modo:
x′= Fx(x,y), y
′= Fy(x,y) (4.9)
Tramite queste relazioni è possibile ottenere le coordinate dei pixel dell’imma-gine distorta per ogni punto dell’immagine corretta. Le duetabelle sono quindicalcolate tramite le espressioni:
4.3. Correzione della distorsione radiale 77
Fx(x,y) =
(
K
∑k=1
βkρk
)
x−uc
ρ(4.10)
Fy(x,y) =
(
K
∑k=1
βkρk
)
y−vc
ρ(4.11)
(4.12)
dove i coefficientiβk rispettano la relazione
ρ′= β1ρ+β2ρ2+ . . .+βKρK (4.13)
che rappresenta la mappatura inversa della 4.3. Per ricavare i coefficientiβk siutilizza un’approssimazione ai minimi quadrati. Sostituendo la 4.3 nella 4.13 siottiene la relazione:
ρ′= β1
[
M
∑m=1
αmρ′m
]
+ . . .+βK
[
M
∑m=1
αmρ′m
]K
(4.14)
Un sistema di equazioni ai minimi quadrati si può ottenere sottraendo il membrodi sinistra da quello di destra, quadrando la differenza e integrando sull’intervallo divalori di ρ′
. Derivando rispetto aβi e ponendo le equazioni ottenute uguali a zero,si ottiene il sistema:
∫
ρ′[∑M
m=1αmρ′m]dρ′
...∫
ρ′[∑M
m=1αmρ′m]Kdρ′
=
∫
[∑Mm=1αmρ′m]2dρ′
. . .∫
[∑Mm=1 αmρ′m]K+1dρ′
.... . .
...∫
[∑Mm=1αmρ′m]K+1dρ′
. . .∫
[∑Mm=1 αmρ′m]2Kdρ′
β1...
βK
(4.15)
I limiti di integrazione sono(0,ρ′max) doveρ′
max è la massima distanza di unpunto dal centro dell’immagine. Gli integrali possono essere risolti per via nu-merica utilizzando il metodo dell’integrale trapezoidaledividendo l’intervallo diintegrazione in un numero sufficientemente elevato di partiuguali.
78 Visione artificiale
4.3.4 Risultati sperimentali
Come indicato in [24] per la 4.3 si è scelto un polinomio di quinto grado. Inoltreper omogeneità si sceglieM = K.
Figura 4.10: Griglia di calibrazione ripresa dalla telecamera senza correzione (asinistra) e con correzione (a destra).
0 5 10 15 20 25 30 35 40 45 50−0.5
0
0.5
1
1.5
2
2.5Distorsione radiale
raggio [mm]
erro
re [m
m]
senza correzionecon correzione
Figura 4.11: Errore di visualizzazione dei punti della griglia di calibrazione con esenza correzione della distorsione radiale.
Per l’integrazione numerica delle 4.15 si sono utilizzate le librerie GSL8 utiliz-zando l’algoritmo di Gauss-Kronrod non adattativo. Con questa soluzione si sono
8GSL: GNU Scientific Library
4.4. Definizione delle caratteristiche cinematiche 79
ottentute ottime prestazioni in termini di tempi di calcoloche, per completare laprocedura di calibrazione della telecamera, rimangono comunque nell’ordine delminuto sulla macchina utilizzata per l’elaborazione dell’immagine. Anche se iltempo di esecuzione di questa procedura può sembrare elevato, questo non rappre-senta un problema perchè questi calcoli vengono effettuatioff-line e non all’internodel ciclo principale dell’algoritmo di visione artificiale.
Il grafico di figura 4.11 mostra l’effetto dell’algoritmo di correzione della di-storsione radiale: è evidente come l’errore di visualizzazione risulti limitato nellafascia±0.5mmnella zona più distante dal centro dell’immagine, riducendo l’errorea circa il 20% di quello presente nel caso senza correzione della distorsione.
4.4 Definizione delle caratteristiche cinematiche
Scopo principale dell’algoritmo di visione artificiale è individuare la posizionedei link del dito tramite la misurazione della posizione deicerchietti neri posti suilink stessi.
Come primo problema da affrontare si ha quindi quello di riconoscere e insegui-re i punti desiderati (i cerchietti neri). Per fare ciò nel modo più semplice possibilesi sono appunto utilizzati dei cerchietti neri posti su di una superficie bianca in mo-do che l’elevato contrasto di questi elementi faciliti la loro individuazione da partedell’algoritmo di visione artificiale. La posizione dei cerchietti sui link del dito ro-botico può essere scelta arbitrariamente evitando però di posizionarle troppo vicineper evitare che più di un cerchietto venga “coperto” dalla stessa patch. Per patch siintendono le porzioni di immagine nelle quali si restringe la ricerca dei cerchietti ne-ri: l’algoritmo di visione artificiale verifica, su ogni nuova immagine, se all’internodi ciascuna patch sia visibile o meno uno dei cerchietti neriposti sul dito.
La ricerca dei punti viene effettuata su di una immagine che contiene pixel chepossono essere solo o bianchi o neri. Questa immagine viene ottenuta dall’im-magine corretta in toni di grigio settando un opportuno livello di saturazione: sela “luminosità” di un pixel supera il livello di soglia, il corrispondente pixel nellanuova immagine è bianco mentre in caso contrario è nero.
Nella situazione ideale soli i cerchietti neri dovrebbero essere visibili nell’im-magine da elaborare. In realtà sono quasi sempre presenti altre zone nere a causa diombre, di insufficiente o mal distribuita illuminazione e a causa della presenza di al-tre zone scure nell’immagine. Per limitare il più possibiletale “distrurbo”, oltre allasaturazione è possibile settare opportunamente il valore della luminosità e del con-trasto dell’immagine proveniente dalla telecamera. Aggiustando questi parametri inbase alle condizioni operative è possibile limitare gli effetti indesiderati dovuti alla
80 Visione artificiale
non pulizia dell’immagine. Per verificare gli effetti delleimpostazione effettuate èstata predisposta nell’interfaccia grafica del programma greydetect una opportunaarea, in alto a destra in figura 4.3, dove è possibile visualizzare l’immagine saturata,utilizzata per la ricerca dei punti, ricavata dall’immagine della telecamera, visibilein figura 4.2.
4.4.1 Inseguimento dei punti
Per ridurre ulteriormente la complessità computazione si sono definite un numero diaree sensibili di dimensione prefissata, i quadrati attornoai cerchietti neri in figura4.2 detti patch, pari al numero dei punti che si devono inseguire, in questo casootto, e si è ristretta la ricerca dei punti sono all’interno di queste aree. In ogni nuovaimmagine viene calcolato il “baricentro del nero” all’interno di ogni patch e il centrodella patch viene poi aggiornato con le coordinate del corrispondente baricentro. Inquesto modo, dimensionando opportunamente le patch in modoche esse possanocontenere i cerchietti con un opportuno margine, dipendente dalla velocità massimacon cui si suppone si spostino i cerchietti neri posti sul dito, e supponendo chei cerchietti non possano uscire dalla patch da un frame al successivo, è possibilemantenere centrate le patch sui cerchietti neri semplicemente aggiornando ad ogniframe il valore delle coordinate del centro della patch con il valore delle coordinatedel baricentro del cerchietto stesso.
Per memorizzare le informazioni relative a ciascuna patch,è stata predispostauna opportuna struttura dati:
/* Areas to test for difference changes */
typedef struct {
int patchnum; // patch identification number
int xmin, ymin; // top-left corner coordinates
int xmax, ymax; // bottom-right corner coordinates
float x,y; // center of the patch
Gan_Vector position; // center vector
float massa; // weight of the patch
float iarea; // Inverse area of patch for calculations
float mean; // Mean of Y values in patch
float stddev; // Standard deviation of Y values in patch
int updated; // update flag
int targeted; // target flag
} tpatch;
Fra le informazioni più importanti contenute in questa struttura vi sono, oltreall’identificativo della patch, le coordinate del centro della patch sia in termini di
4.4. Definizione delle caratteristiche cinematiche 81
pixel (coordinate video) che in termini di distanze dagli assi del sistema di riferi-mento base (vettore9 position ). Nella struttura sono presenti anche alcuni flag permemorizzare lo stato di ciascuna patch. Alcune informazioni contenute in questastruttura sono ridondanti ma vengono mantenute per facilitare l’elaborazione deidati di ciascuna patch da parte del codice.
4.4.2 Definizione dei sistemi di riferimento
Per poter localizzare i sistemi di riferimento sui link, ad ognuno di essi vengo-no associate due patch: in base alle informazioni relative alla posizione delle patchè poi possibile risalire alle caratteristiche del sistema di riferimento del link corri-spondente. Il motivo per cui sono state assegnate due patch ad ogni link risiede nelfatto che conoscere le coordinate di due punti di un corpo rigido piano rappresenta ilminimo delle informazioni necessarie a ricostruire lo stato di moto piano del corporigido stesso.
In una apposita struttura dati vengono quindi memorizzate tutte le informazioniutili alla definizione delle caratteristiche di ciascun link:
/* Link parameter data structure */
typedef struct { // definition of link n
int linknum; // link identification number
int defined; // 1=link n is defined, 0=not defined
Gan_Vector origin; // link n axis origin
Gan_Vector first_point; // first patch center
Gan_Vector second_point; // second patch center
Gan_Vector assex; // x axis vector
Gan_Vector assey; // y axis vector
double lx; // x axis lenght
double dist1; // distance from point 1 to origin
double dist2; // distance from point 2 to origin
double teta12; // angle between points vectors
double angle; // displacement angle
double orientation; // link n orientation
Gan_Matrix trasformn; // from link n-1 to link n
Gan_Matrix trasform0; // from link 0 to link n
} tlink;
Nei vettori first_point e second_point sono memorizzate le posizioni deidue cerchietti neri di ciascun link rispetto al sistema di riferimento del link stes-
9Per la creazione e la manipolazione di vettori e matrici si è fatto uso delle Gandalf Library.
82 Visione artificiale
so. Essendo sia i cerchietti neri che il sistema di riferimento del link solidali conlink stesso, le posizioni dei cerchietti rispetto al sistema di riferimento del link noncambiano durante il moto del dito robotico. La posizione di ogni punto “sensibile”(i cerchietti) è nota se il cerchietto stesso è stato “agganciato” dalla relativa patch:sono quindi note le sue coordinate rispetto al sitema di riferimento dell’immagine.L’orientamento del sistema di riferimento di ciascun link può essere ricavato me-morizzando l’angoloφn fra l’asse x del sistema di riferimento e la retta passante peri centri dei due cerchietti neri posti sul link. Questa informazione è memorizzata nelcampoorientation della strutturatlink . I campi della strutturafirst_point ,second_point , orientation , dist1 , dist2 e teta12 (il siginificato e l’utilità diquesti ultimi tre campi verrà spiegato nel paragrafo 4.4.3)che non variano duranteil moto del dito, vengono inizializzati tramite una apposita procedura, descritta inA.5, con cui l’utente può definire l’origine e l’orientamento dei sistemi di riferi-mento di ciascun link. Tramite questa procedura (vedi la guida in appendice A perle modalità di attivazione) l’utente seleziona due punti per ciascun link semplice-mente selezionandoli con il mouse sull’immagine della telecamera: il primo puntoè l’origine del sistema di riferimento del link e il secondo definisce l’orientamentodell’asse x del sistema di riferimento stesso.
Una volta definiti i parametri necessari e dopo averli salvati nei rispettivi campidella strutturatlink , la ricostruzione della posizione del sistema di riferimentodell’n-esimo link viene effettuata seguendo i seguenti passi:
1. Viene definita la retta orientata passante per i centri delle due patch con orien-tamento dalla prima (la più distante dall’origine del sistema di riferimento dellink) alla seconda (la più vicina all’origine). I dati relativi alla retta sono con-tenuti nella strutturatline fra cui vi è l’angoloψn che la retta forma rispettoal sistema di riferimento dell’immagine;
2. Si calcola l’orientamento del sistema di riferimento dell’n-esimo link rispettoal sistema di riferimento dell’immagine:
θ0n = ψn−φn
doveθ0n è l’angolo che l’asse x del sistema di riferimento dell’n-esimo linkforma con l’asse x del sistema di riferimento dell’immagine;
3. Si calcola la matrice di trasformazione che porta dal sistema di riferimentodell’n-esimo link al sistema di riferimento dell’immagine:
oRn =
cosθ0n sinθ0n 0sinθ0n −cosθ0n 0
0 0 1
4.4. Definizione delle caratteristiche cinematiche 83
Si noti che questa matrice di trasformazione è data dalla composizione diuna matrice di rotazione e di una riflessione dell’asse y perchè il sistema diriferimento dell’immagine non è un sistema di riferimanto destro a causa delfatto che l’asse y del sistema di riferimento dell’immaginerisulta invertito;
4. Il vettorenpn0 che esprime la posizione del centro della prima patch nel si-stema di riferimetno del link corrispondente viene riportato nel sistema diriferimento dell’immagine:
opn0 = oRnnpn0
5. L’origine del sistema di riferimento del link rispetto alsistema di riferimetnodell’immagine si può ora ricavare come:
opon = opon0− opn0
con opon0 posizione della prima patch associata all’n-esimo link rispetto alsistema di riferimento dell’immagine.
6. Si definisce la matrice di trasformazione omogenea che porta dal sistema diriferimento del link al sistema di riferimento dell’immagine:
oHn =
cosθ0n sinθ0n 0 oponx
sinθ0n −cosθ0n 0 opony
0 0 1 00 0 0 1
In figura 4.12 viene riportato un esempio di quanto precedentemente riportato.Una volta definita la posizione e l’orientamento dei sistemidi riferimento di ciascunlink si hanno a disposizione tutte le informazioni per ricostruire la posizione di ognipunto della struttura. Si noti che la sequenza di operazionisopra riportate deveessere effettuata per ciascun link e una volta per ogni nuovoframe, il che comportaun notevole carico computazionale.
4.4.3 Procedura di ricerca dei punti
In realtà le ipotesi a cui si accennava in precedenza sul fatto che una volta chele patch sono state posizionate sui cerchietti neri posti sul dito, queste rimanganocentrate su di essi senza possibilità che i cerchietti escano dalla patch non sono ve-rificate. Le dimensioni delle patch sono state scelte in modotale da poter contenere
84 Visione artificiale
x
y
po2
po20
po21
φ2
x0
y0
x1
y1
x2
y2
x3
y3
p00
p01
p10
p11
p20
p21
p30p31
Figura 4.12: Posizionamento dei sistemi di riferimento.
e “inseguire” i cerchietti in caso di movimenti non troppo rapidi. Aumentare le di-mensioni delle patch migliorerebbe la possibilità di inseguire i cerchietti durante ilmovimento del dito ma, oltre ad un aumento del carico computazionale dovuto al-l’aumento dei pixel dell’immagine da controllare, aumentrebbe la minima distanzafra i cerchietti neri, fattore che, date le ridotte dimensioni del dito, costituisce unagrossa limitazione.
Se il movimento dei cerchietti è sufficientemente rapido, questi possono usciredalla patch con conseguente perdita delle informazioni sulla loro posizione. Inoltre,a causa del fatto che la telecamera acquisisce l’immagine intempo finito attraversouna scansione sequenziale dei pixel che costituiscono il CCD10, se il cerchietto èin movimento questo verra deformato è l’intensità del colore tenderà a diminuire inmaniera proporzionale alla sua velocità di spostamento. Unesempio di sequenza diimmagini del dito in movimento in cui sono evidenti gli effetti dello spostamentodei cerchietti sulla qualità dell’immagine sono visibili in figura 4.13.
La perdita delle informazioni relative anche ad un solo cerchietto rende impos-sibile effettuare il controllo del sistema. Per questo motivo è necessario predisporrel’algoritmo di visione di una procedura di ricerca dei punti, qualora questi venganopersi. Questa operazione verrà effettuata su ogni nuova immagine subito dopo l’ac-
10CCD: Charge-Coupled Device, dispositivo ad accoppiamentodi carica, è un trasduttorecomposto una matrice di elementi fotosensibili.
4.4. Definizione delle caratteristiche cinematiche 85
Figura 4.13: Sequenza di immagini del dito in movimento.
quisizione e la correzione dell’immagine in modo che ogni successiva elaborazionedell’immagine venga effettuata avendo a disposizione informazioni sulla posizionedei link corrette. Volendo ottimizzare le risorse, nella struttura datitpatch , il flagtargeted indica se la patch è ancora “agganciata” al corrispondente cerchietto ose lo ha perso. Questa informazione può essere sfruttata perapplicare la proceduradi ricerca dei punti solo alle patch che effettivamente hanno perso il loro cerchietto.Questa ottimizzazione non è stata implementata perchè si è verificato che questaprocedura non influenza significativamente il carico computazionale complessivo.
Per rendere la ricerca dei cerchietti il più veloce possibile è necessario fare al-cune ipotesi sul comportamento del sistema oggetto della misura. Si prenderannoin considerazione le informazioni relative alla geometriae alla cinematica del ditoe si cercherà di posizionare i cerchietti in maniera da essere a conoscenza, anchese non necessariamente con precisione, della loro posizione sul dito. Si supponeinizialmente che la base del dito, ovvero il link zero, rimanga fermo rispetto allatelecamera, ipotesi che risulta sempre verificata perchè sisuppone che la teleca-mera e il dito siano rigidamente collegati al medesimo pianodi lavoro. Quindi,una volta che siano state “fissate” le patch sui corrispodenti cerchietti posizionatisulla base, saranno note le informazioni necessarie per determinare la posizione e
86 Visione artificiale
l’orientamento del sistema di riferimento base.
Nella strutturatlink sono memorizzate nei campidist1 e dist2 le distanzedelle due patch associate al link successivo. Inoltre nel campo teta12 viene me-morizzato l’angolo fra i vettori che identificano i centri delle due patch nel sistemadi riferimento del link corrente. Per le caratteristiche costruttive della struttura, gliangoliθi si mantengono nell’intervallo[0,π/2].
Partendo dal sistema di riferimento del link zero, che risulta sempre definitoper quanto detto sopra, e in particolare dalle coordinate della sua origine, è possi-bile posizionare correttamente la patch relativa al primo cerchietto del sistema diriferimento successivo. Generallizando la procedura si può scrivere:
opo(n+1)0 =o pon+
[
cos(θn0 +α) −sin(θn0+α)
sin(θn0+α) cos(θn0+α)
][
dn0
0
]
doveopo(n+1)0 e opon sono rispettivamente la posizione della prima patch as-sociata all’(n+1)-esimo link e l’origine dell’n-esimo link nel sistema di riferimentodell’immagine,θn0 è l’angolo fra l’asse x del sistema di riferimento dell’n-esimolink e l’asse x del sistema di riferimento dell’immagine,dn0 è la distanza della pri-ma patch associata all’(n+1)-esimo link dall’origine del sistema di riferimento dellink n e α è un angolo che viene fatto variare all’interno del range[0,π/2]. In parti-colareα viene incrementato, in maniera discreta e partendo da zero,di 10◦ ad ogniinterazione fino a raggiungere i 90◦. Per ogni valore dell’angoloα viene verificatose, una volta posizionata la patch, questa non si trovi, almeno in parte, sopra al cer-chietto sulla quale si vuole centrare la patch stessa. Questa operazione viene fattatramite la normale procedura di inseguimento dei punti: questa procedura assicu-ra anche che la patch venga centrata sul cerchietto. Ovviamente questa proceduraviene effettuata partendo dal link zero e applicata ricorsivamente fino al linkN−1,doveN indica il numero dei link della struttura.
Per posizionare correttamente la seconda patch dell’(n+1)-esimo link, una voltaposizionata la prima viene definita la rettar passante per l’origine dell’n-esimo linke per il centro della prima patch. A questo punto la posizionedella seconda patch èdefinita come:
opo(n+1)1 =o pon+
[
cosβ −sinβsinβ cosβ
][
dn1
0
]
con
β = ψn +θn12
doveopo(n+1)1 è la posizione della seconda patch associata all’(n+1)-esimo linknel sistema di riferimento dell’immagine,ψn è l’angolo fra la rettar e l’asse x del
4.4. Definizione delle caratteristiche cinematiche 87
sistema di riferimento dell’immagine,dn1 è la distanza della seconda patch associataall’(n+1)-esimo link dall’origine del sistema di riferimento del linkn.
θn12
xn
yn
xn+1
xo
yo
yn+1
dn0
dn1
po(n+1)0
po(n+1)1
opo(n+1)0opo(n+1)1
opon
Figura 4.14: Parametri per la localizzazione dei punti.
Una volta che le patch siano state correttamente posizionate sui cerchietti è pos-sibile posizionare correttamente i sistemi di riferimentosui link tramite la proceduradescritta in 4.4.2.
I dati contenuti nelle strutturetpatch e tlink possono essere salvati per essereimmediatamente resi disponibili al nuovo riavvio del programma greydetect. Inquesto modo, supponendo che la telecamera rimanga fissa rispetto al primo linkdel dito (che è fissato alla base a cui è fissata anche la telecamera) non è necessarioriposizionare le patch in quanto la posizione delle prime due è nota e di conseguenzasi possono posizionare correttamente anche le successive.Per lo stesso motivo,qualora le patch perdessero il “bersaglio”, è sufficiente posizionare le prime duepatch, quelle relative al link zero, affinchè la procedura diricerca dei punti localizzianche le successive sulla base delle informazioni sulla geometria del dito contenutenelle apposite strutture.
88 Visione artificiale
4.5 Trasmissione dei dati al server
Dopo che è stata completata l’elaborazione dell’immagine esiano state rese di-sponibili tutte le informazioni necessarie al sistema di controllo, la strutturamessage
di tipo message_t , descritta nel paragrafo 2.8.1, viene aggiornata con le informa-zioni da trasmettere al server per effettuare il controllo.
...
\* forward message data to communication thread *\
if(connected && alldefined) client_send_message(sockfd ,&message);
...
I flag connected ealldefined indicano rispettivamente se il collegamento fraclient a server è attivo e se tutte la patch sono agganciate sul rispettivo cerchietto.Se il collegamento fra client e server non è attivo è ovvio chenon è possibile tra-smettere i dati al server. Se l’utente cerca di inviare comandi al server senza averprima aver attivato il collegamento, un messaggio indica all’utente che il collega-mento non è attivo. Il collegamento viene stabilito dall’utente tramite un’appositopulsante disponibile sull’interfaccia grafica del programma greydetect. Tramite lostesso pulsante è anche possibile interrompere il collegamento se questo è già atti-vo. Non è stata predisposta alcuna procedura di connessioneautomatica perchè, puressendo concettualmente semplice da implementare, esula dagli obbiettivi di questaricerca. Il protocollo di comunicazione fra il client greydetect e il server, a livellodell’applicazione, prevede una fase iniziale in cui i due processi si scambiano mes-saggi utili al reciproco riconoscimento, per evitare di connettersi erroneamente adeventuali altri servizi di rete in funzione sulla macchina che funge da server. Questaprocedura è utile anche a gestire la comunicazioni con client diversi ed eterogeneicome comportamento, come nel caso del client telnet a cui si èaccennato nel pa-ragrafo 2.8.1. Dopo che il collegamento è stato stabilito e idue processi si sono“riconosciuti”, la comunicazione fra il client greydetected il server avviene a sensounico, in quanto il client inizia ad inviare, senza richiedere nessuna risposta, i da-ti contenuti nella strutturamessage . La chiusura del collegamento avviene a latoclient semplicemente chiudendo ilsocket11 di comunicazione con il server.
Il flag alldefined indica se tutte le patch sono correttamente posizionate sulrispettivo cerchietto. Questo flag è stato predisposto perchè la procedura di ricerca
11Un socket è un particolare file utilizzato per trasmettere dati da un’applicazione ad un’altra.Esistono differenti famiglie di socket fra cui le più importanti sono quelli di tipoAF_UNIX utilizzateper implementare i meccanismi di IPC e quelli di tipoAF_INET utilizzati per le connessioni via rete.
4.6. Conclusioni 89
automatica dei punti, descritta nel paragrafo 4.4.3, non assicura che sia possibile inogni caso posizionare correttamente le patch. Infatti, nelcaso in cui i movimenti deldito siano molto rapidi, risulta impossibile per il sistemadi acquisizione visualizzarecorrettamente l’immagine, si veda ad esempio la sequenza diimmagini di figura4.13. Se non tutte le patch sono posizionate correttamente,per evitare di inviare alserver informazioni errate, viene saltata la procedura di invio dei messaggi.
4.6 Conclusioni
In questo capitolo si è descritto il funzionamento delle componenti fondamentalidel programma greydetect, tralasciando di descrivere quelle parti che, pur facendoparte del programma stesso, esulano dallo scopo di questa trattazione. Il program-ma greydetect è stato scritto per uno scopo ben specifico: ricostruire i parametricinematici del dito robotico articolato, visibile in figura1.1, tramite l’uso di unatelecamera. Si è cercato comunque di realizare un software flessibile e facilmen-te adattabile all’utilizzo con strutture diverse da quelledel dito robotico oggetto diquesta tesi. Si è cercato anche di rendere questo software sufficientemente sempliceed intuitivo da utilizzare tramite l’uso delle interfacce grafiche di figura 4.2 e 4.3.
Le prestazioni di questo sistema di visione artificiale sonostate limitate, in pri-mo luogo, dalle caratteristiche dell’hardware utilizzato. La scheda di acquisizionevideo e la telecamera utilizzata sono componenti normalmente reperibili in com-mercio a basso costo e il PC stesso dove si è fatta girare l’applicazione non è certoquanto di meglio disponibile sul mercato in fatto di potenzadi calcolo. Gran partedel lavoro di sviluppo di questo software è stato rivolto verso l’ottimizzazione dellerisorse e la ricerca di opportune soluzioni al fine di ridurreil carico computazio-nale che risulta comunque piuttosto elevato: in media oltreil 60% del carico delprocessore sul PC utilizzato è dovuto al programma greydetect.
Capitolo 5
Risultati sperimentali
Per verificare il buon funzionamento delle varie parti che compongono il sistemadi controllo del dito robotico articolato, si sono realizzati diversi esperimenti.
In figura 5.1 è riportato una foto dell’attrezzatura utilizzata durante gli esperi-menti.
Figura 5.1: Fotografia del dito e degli attuatori.
92 Risultati sperimentali
5.1 Come sono state ottenute le misure
Nelle prove effettuate si assunto che i motori lineari fossero degli attuatori diforza ideali e che il loro modello fosse effettivamente quello di figura 1.4, ovveroche la forza che i motori attuano sia sempre quella effettivamente richiesta [5].
Il tempo di campionamento dell’algoritmo di controllo è stato scelto inizial-mente pari a 1 ms. Si è poi verificato che il tempo di esecuzionedell’algoritmodi controllo fosse sufficientemente breve in modo da non introdurre un ritardo ec-cessivo fra l’acquisizione dei segnali e l’attuazione. Sulla macchina su cui è statoimplementato l’algoritmo di controllo il tempo di esecuzione dell’algoritmo risultasempre compreso fra i 100 e i 120µs. Il tempo di esecuzione dell’algoritmo è statomisurato visualizzando su un’oscilloscopio un segnale digitale settato al valore altoall’inizio del ciclo real time che implementa il controllo eresettato al termine delciclo stesso.
La procedura real time di acquisizione dei dati del controllore è stata eseguitacon un tempo di campionamento di 10 ms.
E’ importante evedenziare che, non avendo a disposizione nessun sensore perpoter effettuare misure di coppia sui giunti del dito robotico e quindi non potendorealizzare alcun controllo su queste coppie, le forze da applicare ai tendini sono statericavate dalle coppie da applicare ai giunti direttamente tramite le equazioni 2.43 ele forze da applicare ai motori sono state ricavate sommandoalle forze da applicareai tendini un valore fisso pari a 1.5 N per vincere gli attriti di strisciamento deglislider e dei tendini.
5.2 Stima degli angoli di giunto
I primi test effettuati sono stati volti a valutare il corretto funzionamento delcontrollore real time descritto nel capitolo 3. In questi esperimenti non si è utilizzatoil sistema di visione artificiale quindi gli unici sensori presenti nel sistema, tramite iquali ottenere informazioni sulla configurazione del dito robotico, sono gli encoderpresenti nei motori lineari utilizzati come attuatori. In questi esperimenti si è quindicercato di ricostruire gli angoli di giunto del dito tramitela misura della posizionedegli slider dei motori lineari facendo uso delle equazioni2.6. La stima degli angoliè stata resa il più semplice possibile per poter essere effettuata all’interno dellaprocedura real time senza penalizzare le prestazioni complessive del sistema. Unavolta noti gli angoli di giunto, si è verificato l’efficacia del controllo d’impedenzarealizzato.
5.2. Stima degli angoli di giunto 93
5.2.1 Modello statico e dinamico dei tendini
Durante le prove effettuate si è notato che stimare il valoredegli angoli di giuntosemplicemente tramite la misura della posizione degli slider dei motori lineari senzaprendere in considerazione l’elasticità dei tendini e quindi la dipendenza della lorolunghezza dalla forza ad essi applicata comporta errori di stima molto elevati, tali darendere inutilizzabile la stima stessa. Per questo motivo si sono affettuante alcunemisure di elasticità sui tendini stessi. Le misure sono state effettuate bloccando ildito nella configurazione in cui gli angoli di giunto sono tutti nulli e applicando aitendini delle forze variabili e misurando la posizione degli slider dei motori linearitramite gli encoder.
0 2 4 6 8 10 12 14 16 18 200
2
4
6
8
10
12
Time (s)
App
lied
forc
e (N
)
0 2 4 6 8 10 12 14 16 18 200.2
0.4
0.6
0.8
1
Time (s)
Ten
don
leng
th (
mm
)
Figura 5.2: Allungamento dei tendini in funzione delle forze applicate.
Le prove sono state effettuate applicando ai tendini una forza sinusoidale al-la frequenza di 1 Hz con ampiezza 5 N e offset di 6.5 N. La forza risulta quindivariabile fra 1.5 N e 11.5 N in modo da far lavorare i tendini sempre in trazione.
I grafici di figura 5.2 sono relativi ad un tendine lungo 160 mm.Da questigrafici si nota che il comportamento dei tendini, per le bassefrequenze a cui so-no state effettuate queste prove ed in particolare in condizioni statiche, può esse-re modellato, con buona approssimazione, come una molla concostante elastica
94 Risultati sperimentali
pari a 16.67 Nmm−1. L’allungamento specifico dei tendini risulta quindi essere3.75×10−4 N−1.
Dalle misure effettuate emerge che il comportamento dei tretendini è piuttostoomogeneo. Quindi per ogni tendine si è definita una costante in base all’elasti-cità rilevata dai grafici di figura 5.2 e alla lunghezza del tendine che moltiplicataper la forza applicata permette di ottenere l’allungamentodel tendine stesso. Nel-l’algoritmo di controllo, dopo aver acquisito i valori degli encoder dei motori, si èquindi provveduto a sottrarre dalla misura effettuata il valore dell’allungamento deitendini.
...
/* encoder data acquisition */
h.h1=(double)ctrl_read_pos(&controller[0])*(double) KMM2M;
h.h2=(double)ctrl_read_pos(&controller[1])*(double) KMM2M;
h.h3=(double)ctrl_read_pos(&controller[2])*(double) KMM2M;
/* subtract tendons elongation */
h.h1-=K1*(filter_output(&Fh1filter2,Fh.Fh1)-4.0);
h.h2-=K2*(filter_output(&Fh2filter2,Fh.Fh2)-4.0);
h.h3-=K3*(filter_output(&Fh3filter2,Fh.Fh3)-8.0);
...
/*joint angle estimator*/
tend2ang(&finger,&tendq,&htemp);
...
Nella strutturaFh sono memorizzati i valori delle forze applicate ai tendini du-rante il ciclo precedente, ovvero quelle attuate al momentodell’acquisizione degliencoder. Si noti che il valore della forza applicata a ciascun tendine viene filtratatramite la funzionefilter_output() per evitare che si instaurino vibrazioni suitendini.
LinkTendine
Slider
Ml Ms
Bt
Kt
σl σs
Figura 5.3: Schema di collegamento fra slider e link del dito.
5.2. Stima degli angoli di giunto 95
In figura 5.3 è mostrato lo schema concettuale del collegamento fra slider deimotori lineari e link del dito tramite i tendini. I tendini sono modellati tramite unamolla dalla rigidezzaKt piuttosto elevata, come evidenziato dai grafici di figura 5.2,e da uno smorzatore con costante di smorzamentoBt molto bassa. La rigidezzadel tendine è stata misurata sperimentalmente come riportato all’inizio di questasezione.
Nel movimento dello slider è presente attrito dovuto allo strisciamento fra sta-tore e slider del motore lineare. Allo stesso modo è presenteun’attrito dovuto alloscorrimento del tendine all’interno delle guide presenti nel dito robotico. Questiattriti sono modellati tramite le costantiσs e σl di valore piuttosto piccolo e indivi-duate nel contatto dello slider e del link con la superficie d’appoggio nel modello difigura 5.3.
Bode Diagram
Frequency (rad/sec)
Pha
se (
deg)
Mag
nitu
de (
dB)
101
102
103
0
45
90
135
180
To:
y1
−100
−50
0
50From: u1
To:
y1
Figura 5.4: Risposta in frequenza del sistema di trasmissione del moto fra slider elink.
Nelle prove effettuate si è notato che, sfruttando a pieno ladinamica dei motorilineari, si instauravano vibrazioni piuttosto forti nel tendine che tendevano a rendereinstabile il sistema. La spiegazione di questo fenomeno risulta evidente prendendoin considerazione i grafici di figura 5.4 in cui è riportata la risposta in frequenza delsistema rappresentato in figura 5.3 in cui si sono presi come input la posizione delloslider e come output la posizione del link. I parametri con i quali sono stati ottenutii grafici di figura 5.4 sono riportati nella tabella 5.1.
96 Risultati sperimentali
Ms 0.165 kgMl 0.01 kgKt 16.67 Nmm−1
Bt 1 Nsm−1
σs trascurabileσl trascurabile
Tabella 5.1: Parametri del modello dinamico del collegamento fra slider e link.
Dai grafici di figura 5.4 si nota come, se si supera la frequenzadi risonanza delsistema che è circa di 30 rad s−1, il sistema subisce un’inversione di fase positivache causa l’instabilità.
Per evitare l’instaurarsi di vibrazioni nei tendini è quindi necessario filtrare l’a-zione di controllo ad una frequenza sufficientemente bassa.Durante gli esperimentisono stati applicati filtri con frequenza di taglio pari a 10 rad s−1. Con questi filtrinon si sono verificati problemi di instabilità.
5.2.2 Inizializzazione del valore degli encoder
Per inizializzare gli encoder, al valore restituito dalla funzionefilter_output()
viene sottratto un offset. Questo offset è dovuto alla procedura di inizializzazionenecessaria per settare gli encoder dei motori lineari al valore corretto quando siavvia l’algoritmo di controllo.
...
/* initilization procedure*/
if(!initialized && !start && i<1/(SAMPLETIME*T)){
ctrl_setpoint_track(&controller[0],4.0);
ctrl_setpoint_track(&controller[1],4.0);
ctrl_setpoint_track(&controller[2],8.0);
MQPCI_encSetCount1A(&Card0,0);
MQPCI_encSetCount2A(&Card0,0);
MQPCI_encSetCount3A(&Card0,0);
i++;
if(i>=1/(SAMPLETIME*T)){
i=0;
initialized=1;
5.3. Controllo d’impedenza tramite la sola stima degli angoli di giunto 97
}
}
...
Queste porzione di codice viene eseguita per un intervallo di 1s all’avvio dell’al-goritmo di controllo. Le forze applicate ai tendini sono tali da portare il dito nellaconfigurazione con gli angoli di giunto uguali a zero ed assicurare che i tendini la-vorino in trazione e siano ben tesi. In questa configurazionegli encoder vengonoresettati in modo da far corrispondere ad angoli nulli valori degli encoder nulli ameno dell’allungamento dei tendini.
5.3 Controllo d’impedenza tramite la sola stima degliangoli di giunto
Nelle prime prove effettuate ci si è concentrati maggiormente sulla verifica del-l’efficienza e della stabilità del controllo d’impedenza implementato. In questi espe-rimenti non si è posta particolare attenzione alla valutazione degli errori di posizio-namento della punta del dito robotico. Gli esperimenti effettuati sono stati svolti fa-cendo interagire il dito con diversi ostacoli. In particolare in figura 5.5 sono visibilii grafici di un’esperimento effettuato con un ostacolo che impedisce il movimentodel dito lungo l’asse x del sistema di riferimento base, ovvero quello del link zero. Idati di questi grafici sono stati ottenuti con un rigidezza del controllo d’impedenzapari a 100 Nm−1 lungo entrambi gli assi.
Dopo che si è stabilito il contatto fra dito ed ostacolo, la forza applicata daldito può essere variata semplicemente facendo variare il setpoint lungo la direzionedell’ostacolo. Si noti che, nonostante l’ostacolo, la posizione della punta del ditocambia al variare della forza applicata. Questo è dovuto al fatto che il contattofra dito e ostacolo non avviene nel punto esatto in cui è localizzata l’origine delsistema di riferimento dell’ultimo link a cui si riferiscono le coordinate visualizzateni grafici. Variando il setpoint la configurazione del dito cambia e la punta deldito routa sull’ostacolo facendo cosi variare la posizionedell’origine del sistema diriferimento della punta del dito.
Dai grafici è facile notare che il controllo risulta stabile durante l’interazione enel passaggio da condizione di moto libero a condizione di contatto.
Nei grafici di figura 5.6 sono visibili le forze applicate dai motori per avere leforze nello spazio di lavoro necessarie a realizzare il controllo d’impedenza. Questeforze risultano sempre positive in modo da far lavorare i tendini sempre in tensione.Si noti in particolare che la forza applicata al terzo tendine risulta piuttosto piccola,
98 Risultati sperimentali
0 5 10 15 200
0.02
0.04
0.06
0.08
0.1Position (m)
Time (s)
x
PositionSetpoint
0 5 10 15 200
0.02
0.04
0.06
0.08
0.1
Time (s)
y
PositionSetpoint
0 5 10 15 20−6
−4
−2
0
2Force (N)
Time (s)
Fx
0 5 10 15 20−3
−2
−1
0
1
2
Time (s)
Fy
Figura 5.5: Grafici dell’interazione fra dito robotico e ostacolo.
0 2 4 6 8 10 12 14 16 18 200
5
10
15
20
25
30
35Motor forces
Time (s)
For
ce (
N)
Fh1Fh2Fh3
0 2 4 6 8 10 12 14 16 18 20−6
−4
−2
0
2Workspace forces
Time (s)
For
ce (
N)
FxFy
Figura 5.6: Grafici delle forze applicate ai motori e delle forze nello spazio dilavoro.
5.4. Controllo d’impedenza tramite immagini 99
a parte brevi e limitati transitori, costante. Si può quindipensare di sostituire ilmotore del terzo tendine con un elemento passivo (es. una molla). Per quantoriguarda le forze applicate dagli altri due motori si nota che durante l’interazionecon l’ostacolo queste superano ampiamente la massima forzacontinua applicabiledai motori lineari utilizzati nonostante la forza applicata nello spazio di lavoro siadi pochi Newton. Per questo motivo si è pensato di sostituirei motori lineari conaltri dispositivi in grado di applicare forze continue più elevate per migliorare leprestazioni ottenibili dal controllo durante l’interazione con l’ambiente.
5.4 Controllo d’impedenza tramite immagini
Dopo aver verificato la stabilità del controllore d’impedenza, si è cercato dimigliorare le prestazioni in termini di errore di posizionamento della punta del ditotramite l’utilizzo del sistema di visione artificiale.
Il valore degli angoli di giunto stimati tramite la misura della posizione deglislider dei motori lineari viene ora corretto con il valore trasmesso dall’algoritmodi visione artificiale. La trasmissione dei dati avviene, come già precedentementedetto, tramite la LAN presente in laboratorio. Questa modalità di trasmissione in-troduce un ritardo fra l’istante in cui vengono effettuate le misure e l’istante in cui idati sono disponibili all’algoritmo di controllo. Inoltrela frequenza con cui il siste-ma di visione artificiale mette a disposizione nuovi dati all’algoritmo di controllo èlimitata dal numero di immagini al secondo acquisite tramite la telecamera, nume-ro che è limitato, dall’hardware utilizzato, a 25 fps. Le prestazioni dinamiche delsistema completo sono, a causa di questi fattori, piuttostoscarse rispetto a quelleottenibili con la sola stima degli angoli di giunto effettuata tramite la misura dellaposizione degli slider dei motori lineari ma le prestazionistatiche risultano notevol-mente migliorate grazie alla misura degli angoli di giunto effettuata dal sistema divisione artificiale.
5.4.1 Rallentamento della dinamica dei motori lineari
Come già accennato in precedenza, fra i motivi che hanno portato all’uso dellatelecamera vi è la necessità di correggere gli errori dovutialla non idealità del mo-dello cinematico e la totale assenza di sensori sul dito robotico. Ciò che interessanon sono quindi le prestazioni dinamiche ma è importante ottenere misure precisein condizioni quasi statiche.
Per mantenere stabile il controllore d’impedenza tramite immagini è stato quindinecessario rallentare sensibilmente gli attuatori.
100 Risultati sperimentali
...
/* calculating sliders speed */
hdot.h1=(h.h1-hold.h1)/(SAMPLETIME*T);
hdot.h2=(h.h2-hold.h2)/(SAMPLETIME*T);
hdot.h3=(h.h3-hold.h3)/(SAMPLETIME*T);
/* sliders speed saturation */
if(fabs(hdot.h1)>0.1) hdot.h1=0.1*fabs(hdot.h1)/hdot .h1;
if(fabs(hdot.h2)>0.1) hdot.h2=0.1*fabs(hdot.h2)/hdot .h2;
if(fabs(hdot.h3)>0.1) hdot.h3=0.1*fabs(hdot.h3)/hdot .h3;
...
/* damping factor */
Fh.Fh1-=KFV*hdot.h1;
Fh.Fh2-=KFV*hdot.h2;
Fh.Fh3-=KFV*hdot.h3;
...
Tramite l’introduzione di questa parte di codice nel ciclo principale del control-lore real time, viene applicata agli slider una forza proporzionale alla velocità dispostamento degli slider stessi che si oppone al loro movimento introducendo cosìuno smorzamento il cui valore può essere modulato tramite lacostanteKFV.
I test effettuati hanno messo in evidenza che, grazie all’introduzione dello smor-zamento nel movimento degli slider, il controllo d’impedenza tramite immaginirimane stabile con valori di rigidezza del controllo che arrivano a 600 Nm−1 garan-tendo così un errore di posizionamento molto basso.
5.4.2 Prestazioni del controllo tramite immagini
I grafici di figura 5.7 riportano i dati relativi al controllo d’impedenza tramiteimmagini utilizzato come controllo di posizione della punta del dito, ovvero con unvalore di rigidezza di 400 Nm−1 quindi piuttosto elevato.
In queste condizioni non è possibile testare il comportamento del controllo d’im-pedenza durante l’interazione con l’ambiente perchè valori di rigidezza così elevatiimplicano valori delle forze applicate dai motori lineari ai tendini che superano ilmassimo continuo ammissibile per questi motori. L’azionamento che pilota i motoriinterrompe l’alimentazione ai motori stessi se la forza richiesta supera il massimovalore continuo per un tempo prolungato per evitare l’eccessivo surriscaldamen-to dei motori. La durata dell’intervallo durante il quale i motori possono attuareuna forza superiore al massimo continuo ammissibile dipende dalle condizoni di
5.5. Utilizzo del sistema di visione artificiale per la taratura dei sensori 101
raffreddamento dei motori stessi. Nelle condizioni degli esperimenti svolti questointervallo è nell’ordine dei 10 s.
Dai grafici di figura 5.7 si nota come l’errore di posizionamento sia piuttostobasso, dell’ordine del millimetro, anche nell’inseguimento di una traiettoria, ovverodurante gli spostamenti lineari da un punto ad un’altro.
Nei grafici di figura 5.8 è visibile il comportamento del sistema nel caso in cuiviene dato un setpoint fuori dallo spazio di lavoro. Si può notare che in questo casola forza applicata nello spazio di lavoro risulta puttosto elevata rispetto al caso incui il setpoint giace all’interno dello spazio di lavoro e laforza applicata dai motoriarriva al massimo attuabile per il motore 3 mentre va a zero per il motore 1.
Figura 5.7: Dati del controllo di impededenza tramite immagini.
5.5 Utilizzo del sistema di visione artificiale per lataratura dei sensori
In questa tesi gli unici sensori che sono stati presi in considerazione sono la
102 Risultati sperimentali
Figura 5.8: Dati del controllo di impededenza tramite immagini con esempio disetpoint fuori dallo spazio di lavoro.
telecamera e gli encoder presenti sui motori lineari. Tuttavia diversi sensori dautilizzare per la costruzione del nuovo modello di mano robotica sono stati testatiin precedenti ricerche [5] e sono attualmente in fase di studio al LAR.
Il sistema di visione può essere un ottimo strumento per la taratura di sensori perla misura degli angoli di giunto grazie alla qualità delle misure ottenibili con questosistema. Confrontando quindi le misure degli angoli di giunto effettuate tramitei sensori e le informazioni ricavate dalla telecamera si puòottenere una taraturaprecisa ed affidabile dei sensori stessi.
5.6 Conclusioni
I dati riportati in questo capitolo mettono in evidenza le caratteristiche del con-trollo d’impedenza applicato al dito robotico articolato.Nelle prove effettuate sisono raggiunte buone prestazioni di stabilità del sistema sia senza che con l’uso delsistema di visione artificile. La precisione di posizionamento della punta del dito
5.6. Conclusioni 103
da parte del controllo risulta molto migliorata grazie all’uso del sistema di visioneartificiale perchè, con questo sistema, vengono risolti i problemi di non idealità delmodello cinematico del dito robotico. Le prestazioni stesse del sistema di visioneartificiale risultano molto buone in condizioni di movimenti del dito sufficientemen-te lenti, raggiungiendo una risoluzione di misura di 0.125 mm ed un errore massimoai bordi dell’immagine di 0.5 mm.
La possibilità di interagire con l’ambiente esterno è fortemente limitata dallamassima forza attuabile dai motori lineari utilizzati durante le prove. Per questomotivo si è pensato di sostituire questi motori con altri in grado di attuare forze piùelevate nei prossimi test da effettuare.
Appendice A
Manuale d’uso del programagreydetect
A.1 Introduzione
Questo programma è utilizzato per ricavare gli angoli di giunto del dito roboticoarticolato tramite le informazioni ricavate dalle immagini riprese dalla telecamera.Gli angoli di giunto vengono ricavati posizionando in maniera opportuna i sistemidi riferimento sui link del dito robotico tramite le informazioni memorizzate dalprogramma e la posizione dei cerchietti neri posti sui link del dito robotico rilevatadalle immagini.
L’input video può essere a colori o in banco e nero. Il grabbing dell’immagineviene fatto in scala di grigi con una risoluzione di 8bit.
L’immagine a video è impostata per essere visualizzata utilizzando Xwindowcon una profondità di colore di 16bit.
Il programma è provvisto di un’interfaccia grafica tramite la quale è possibilesettare i parametri per l’acquisizione dell’immagine quali il contrasto, la luminositàe il livello di saturazione. Tramite l’interfaccia grafica èanche possibile salvare leimpostazioni del programma, salvare le immagini catturatedalla telecamera e vi-sualizzare l’immagine utilizzata per l’elaborazione per verificare l’adeguatezza dellivello di saturazione applicato. Inoltre è possibile gestire e monitorare il collega-mento via ethernet con il server che si occupa di comunicare con l’algoritmo dicontrollo del dito robotico articolato.
106 Manuale d’uso del programa greydetect
A.2 Parametri da linea di comando
Al programma greydetect è possibile passare alcuni parametri da linea di co-mando per modificare la modalità di visualizzazione e di acquisizione delle imma-gini della telecamera.
Opzioni video
-width # Larghezza dell’immagine acquisita in pixel (default 640)-height # Altezza dell’immagine acuisita in pixel (default 480)-channel # Numero del canale del framegrabber da cui acquisire
l’immagine (default 1)-videodev device Device video associato al framegrabber
(default /dev/video)-ntsc Usa lo standard NTSC per l’acquisizione video
(default usa lo standard PAL)-secam Usa lo standard SECAM per l’acquisizione video
(default usa lo standard PAL)
Opzioni di rete
Questi parametri possono essere settati anche attraverso l’interfaccia grafica delprogramma greydetect.
-host ip Indirizzo IP o nome del server a cui collegarsi-port # Porta su cui è in ascolto il server (dafault 1430)
Opzioni generali
-config configfile File di configurazione dei parametri inizialidel sistema di visione artificiale (default ./greydetect.cnf)
-linkconf linkconffile File di configurazione dei parametri inizialidei link (default ./linksetting.cnf)
-nox Esclude la visualizzazione delle immagini della telacamera
A.3. Uso del programma greydetect 107
A.3 Uso del programma greydetect
1. Avviare il programma1 tramite un terminale selezionando la risoluzione desi-derata, il canale di input e la modalità video se diverse da quelle di default.
2. Aggiustare il contrasto e la luminosità dell’immagine tramite i corrispondenticursori sull’interfaccia grafica o tramite i rispettivi comandi da tastiera2 inmodo da ottenere una immagine nitida e chiara.
3. Verificare tramite l’infaccia grafica che il livello di saturazione dell’immagi-ne sia adeguato, ovvero che siano ben visibili e distinguibili i cerchietti neriapplicati sul dito robotico. Eventualmente aggiustare la saturazione tramite ilcursore presente sull’interfaccia grafica.
4. Posizionare le patch trascinandole con il mouse sui rispettivi cerchietti. Sesono gia disponibili le informazioni necessarie per ricostruire i sistemi di ri-ferimento dei link del dito robotico, dopo aver posizionatocorrettamente leprime due patch, il sistema localizza automaticamente la posizione delle pat-ch successive e visualizza i sistemi di riferimento. Se le informazioni per laricostruzione dei sistemi di riferimento non sono disponibili o si vuole mo-dificare queste informazioni, avviare la procedura di definizione dei sistemidi riferimento3 premendo il tasto “p”. Per ulteriori informazioni su questaprocedura si veda il paragrafo A.5.
5. Salvare i dati relativi alle impostazioni attuali per renderle disponibili persuccessivi utilizzi.
6. A questo punto è possibile collegarsi al server per inviare i dati ricavati dall’e-laborazione dell’immagine. Impostare sull’interfaccia grafica l’indirizzo o ilnome dell’host a cui ci si vuole collegare e la porta su cui il server è in attesadi connessione e stabilire il collegamento tramite l’apposito tasto.
7. Le coordinate nello spazio di lavoro del punto in cui si vuole posizionare lapunta del dito robotico, la rigidezza del controllo d’impedenza e il tempo di
1L’Xserver deve essere attivo con una profondità di colore di16bit.2Per utilizzare i comandi da tastiera la finestra che visualizza le immagini della telecamera deve
essere attiva.3Per poter avviare la procedura di definizione dei sistemi di riferimento, tutte le patch devono
essere correttamente posizionate sui rispettivi cerchietti. Se si cerca di avviare questa proceduraquando le patch non sono correttamente posizionate viene visualizzato un messaggio d’errore e laprocedura non parte.
108 Manuale d’uso del programa greydetect
percorrenza della traiettoria4 possono essere impostate tramite l’interfacciagrafica su cui è anche presente il tasto per l’invio dei comandi utente al siste-ma di controllo. I punti in cui posizionare la punta del dito possono essereselezionati anche tramite il mouse entrando in modalità inserimento premen-do sulla tastiera il tasto “q” e cliccando con il mouse sull’immagine dellatelecamera nel punto desiderato. La punta del dito verrà quindi guidata lungoun segmento dal punto di partenza al punto selezionato secondo i parametri dirigidezza e di tempo di esecuzione della traiettoria impostati sull’interfacciagrafica.
8. In ogni momento è possibile salvare l’immagine ripresa dalla telecamera tra-mite l’apposito tasto sull’interfaccia grafica o premendo da tastiera il tasto“t”.
9. Per uscire dal programma greydetect cliccare sull’apposito tasto posto sul-l’interfaccia grafica.
A.4 Comandi da tastiera
Tasto Comandot Salva l’immagine ripresa dalla telecamera nel file ./capture#.png
dove # è un numero progressivo che viene incrementatoautomaticamente dal programma ad ogni salvataggio.
r Esclude la visualizzazione del righello virtuale.l Esclude la visualizzazione dei sistemi di riferimento.u Attiva la procedura di definizione dei sistemi di riferimento.q Entra in modalità inserimento punti.
Una successiva pressione esce da questa modalità.p Attiva la procedura di calibrazione automatica della telecamera.y Crea matrici di calibrazione vuote.c Diminuisce il contrasto.v Aumenta il contrasto.b Diminuisce la luminosità.n Aumenta la luminosità.s Salva le impostazioni attuali.d Recupera le impostazioni di default per i sistemi di riferimento dei link.
4La punta del dito viene guidata dal punto di partenza al successivo punto impostato dall’utentelungo il segmento che unisce i due punti.
A.5. Procedura di definizione dei sistemi di riferimento 109
A.5 Procedura di definizione dei sistemi di riferimen-to
La procedura di definizione dei sistemi di riferimento si attiva premendo datastiera il tasto “u”. Per poter attivare questa procedura tutte la patch devono esserecorrettamente posizionate sui rispettivi cerchietti altrimenti viene visualizzato unmessaggio d’errore e la procedura non parte.
All’attivazione della procedura, viene visualizzato un fermo immagine di ciòche la telecamera sta inquadrando. Cliccando con il mouse suquesta immagine siselezionano quindi, in sequenza partendo dal primo fino all’ultimo link, il punto incui si vuole posizionare l’origine del sistema di riferimento del link e un secondopunto per orientare l’asse x del sistema di riferimento. Unavolta selezionati tutti ipunti (due per ciascun link) la procedura termina e ritorna alla modalità di funziona-mento normale ricostruendo ora i sistemi di riferimento delvari link sulla base dellenuove informazioni appena impostate. Le nuove impostazioni vengono salvate au-tomaticamente all’uscita dalla procedura di definizione dei sistemi di riferimento.Le impostazioni di default dei sistemi di riferimento, basate sulle informazioni sullageometria del dito robotico sulla base delle quali si calcola anche la cinematica deldito stesso, possono essere recuperate premendo il tasto “d”.
A.6 Procedura di calibrazione automatica della tele-camera
Premendo il tasto “p” si attiva la procedura automatica di calibrazione dellatelecamera per la correzione della distorsione radiale. Per poter eseguire questaprocedura, la griglia di calibrazione deve essere inquadrata dalla telecamera e tut-ti i cerchietti neri presenti sulla griglia devono essere correttamente riconosciutidall’algoritmo di visione artificiale. Inoltre la griglia deve essere allineata con l’in-quadratura: è possibile verificare l’allineamento della griglia tramite l’utilizzo delrighello virtuale.
La procedura di calibrazione verifica che tutti i cerchiettineri della griglia sianocorrettamente visualizzati e in caso contrario termina conun messaggio d’erroreche informa che la griglia di calibrazione non è corretta riprendendo poi il norma-le funzionamento del programma. In questo caso è necessarioagire su contrasto,luminosità e saturazione per visualizzare correttamente la griglia di calibrazione eriavviare la procedura.
110 Manuale d’uso del programa greydetect
Una volta effettuata la procedura di calibrazione, le matrici di calibrazione ven-gono salvate in appositi file per essere ricaricate nei successivi riavvii del program-ma graydetect.
Appendice B
I moduli del kernel di Linux
B.1 Introduzione
Il kernel di un sistema operativo provvede alla gestione e alcorretto funzio-namento del computer. Tutto questo lavoro viene implementato via software dalkernel, il quale viene caricato in memoria e messo in esecuzione al momento delboot della macchina. I kernel moderni, per non essere di dimensioni sproposita-te visto la grande varietà di hardware sul mercato, sono modulari; in questo modosi ha un kernel base di dimensioni ridotte, al quale vengono aggiunte funzionalitàcaricando in esso i moduli quando servono.
Un modulo è, quindi, un programma oggetto1 che viene inserito nel nucleoquando una certa funzionalità è richiesta. Il fatto che un modulo viene linkato alkernel ci permette di intuire che esso andrà ad operare nelkernel space2. Nel mo-mento in cui viene caricato nel nucleo mette a disposizione le sue risorse sia ad altreparti del kernel che ai processi utente che ne richiedano i servigi.
B.2 Moduli: funzionamento
Può essere utile analizzare cosa succede quando un modulo viene “linkato”, acausa di una richiesta esplicita da parte dell’utente (si vedrà in seguito come questaprocedura può essere automatizzata), per comprenderne meglio il funzionamento.
1Un programma oggetto ha questo nome perché, pur essendo codice eseguibile, e quindi unprogramma, deve ancora subire delle modifiche affinché possaessere eseguito dal computer.
2Il kernel spaceè in contrapposizione aluser spacenel quale operano le normali applicazioniQuesta divisione serve per evitare accessi non autorizzatialle risorse del sistema da parte di processiutente.
112 I moduli del kernel di Linux
Viene di seguito riportato il codice di un ipotetico modulo al quale si farà riferimentoper spiegarne il funzionamento3.
#define __KERNEL__
#define MODULE
#define EXPORT_SYMTAB
#include <linux/module.h>
#include <linux/kernel.h>
#include <asm/io.h>
#include <asm/page.h>
#include <linux/ioport.h>
int sum(int a, int b)
{
return a+b;
}
EXPORT_SYMBOL(sum)
int init_module(void)
{
if (check_region(0x200,0xf)) {
printk("Area già riservata\n");
return ENODEV;
}
request_region(0x200,0xf,"example_module");
return 0;
}
void cleanup_module(void)
{
release_region(0x200,0xf);
printk("Modulo rimosso\n");
}
Compilato il sorgente è possibile caricarlo nel kernel in modo esplicito digitan-do al prompt il seguente comando:[root@lar22 misc]$ insmod ./example_module.o
Il comandoinsmod cerca di caricare il modulo nel nucleo risolvendo tutti queisim-boli che si trovono nella tabella dei simboli del nucleo ed utilizzati dal kernel stesso,quindi esegue la funzioneinit_module() . Questa funzione viene richiamata tutte
3Si ipotizzi che il file oggetto ottenuto dalla compilazione di questo sorgente si chiamiexample_module.o .
B.3. Caricamento automatico di un modulo 113
le volte che il modulo viene caricato nel kernel. In essa vengono quindi inizializ-zate tutte quelle strutture dati che il modulo andrà poi ad utilizzare. Nell’esempioriportato questa funzione controlla se una determinata area di memoria è libera e incaso affermativo la riserva in modo che nessun altro modulo possa utilizzarla.
In modo del tutto analogo per rimuovere un modulo dalla memoria si deve digi-tare il comando:[root@lar22 misc]$ rmmod ./example_module.o
In questo caso viene seguita la funzionecleanup_module() che ha il compito dirilasciare le risorse utilizzate dal modulo ripulendo la memoria della strutture defi-nite in init_module() . Questo compito è essenziale perché se non venisse fattole risorse non sarebbero disponibili per altri moduli, anche se queste non sono piùutilizzate.
Il sorgente preso in esame è abbastanza semplice. Si noti cheè definita unafunzionesum() , la quale esegue la somma di due numeri, che non è utilizzata dalmodulo preso ad esempio. In realtà tale funzione è messa a disposizione per altrimoduli che volessero utilizzarne i servigi. Si noti che conEXPORT_SYMBOL(sum)siafferma l’intenzione di rendere disponibile ad altri moduli tale funzione. È possi-bile allora introdurre il concetto dimodule stacking: può accadere che un modulonecessiti di una funzione implementata da un altro modulo e quindi prima di poterlocaricare si ha la necessità di caricarne un altro. I moduli vengono allora “impilati”uno sull’altro come avviene appunto nello stack da cui il nome di module stacking.Un modulo che intenda utilizzare una funzione definita in un altro modulo devechiedere esplicitamente che venga caricato in memoria se non è già presente, e perfare tutto ciò basta aggiungere nella funzioneinit_module() la seguente porzionedi codice:
int res;
if ((res=request_module(example_module.o))<0){
printk("Non è possibile caricare il modulo richiesto\n");
return res;
}
Dove request_module() è una speciale funzione che fa si che il kernel si in-carichi di caricare tutti i moduli che occorrono al modulo richiesto e poi il modulostesso.
B.3 Caricamento automatico di un modulo
Si è accennato in precedenza che un modulo può venire caricato nel kernel anche
114 I moduli del kernel di Linux
senza una richiesta esplicita da parte di un utente, in modo del tutto automatico.Prima di continuare è opportuno fare qualche precisazione.
In Linux è presente una directory che contiene dei file speciali, ogni file presentein /dev rappresenta in realtà un dispositivo installato (o installabile) nel sistema,questo permette di vedere un dispositivo come se fosse un normale file. Ogni filespeciale ha associato unmajor numbere un minor numberche rappresentano ilcodice di identificazione di un determinato dispositivo. È possibile creare un filespeciale digitando il seguente comando:[root@lar22 dev]$ mknod -m 666 /dev/example_device c 200 0
Questo comando crea un file speciale di nomeexample_deviceil cui major numberè 200 e ilminor numberè pari a 04.
Per ottenere il caricamento automatico del modulo si devonoaggiungere partidi codice al modulo stesso. Si consideri nuovamente l’esempio precedente e siaggiunga al codice le seguenti parti:
...
#define MOD_NAME "example_module.o"
#define MOD_MAJOR 200
int example_device_open(struct inode *inode, struct file *file)
{
return 0;
}
int example_device_release(struct inode *inode, struct f ile *file)
{
return 0;
}
struct file_operations example_device_fops = {
open:example_device_open,
release:examples_device_release,
};
...
Inoltre si aggiunga dentro le funzioniinit_module() e cleanup_module() iseguenti pezzi di codice:
int init_module(void)
{
int res;
4Per ulteriori dettagli su mknod si faccia riferimento alle man pages.
B.3. Caricamento automatico di un modulo 115
...
if((res=register_chrdev(MOD_MAJOR,MOD_NAME,&example _device_fops))<0){
printk("Il major number %d non è libero\n",MOD_MAJOR);
return res;
}
...
}
void cleanup_module(void)
{
...
unregister_chrdev(MOD_MAJOR,MOD_NAME);
...
}
In questo semplice esempio compaiono due funzioniexample_device_open()
eexample_device_release() che non fanno nulla, in realtà in generale hanno unruolo molto importante e vengono eseguite tutte le volte cheun programma neluserspaceesegue unaopen() sul nodo che rappresenta il dispositivo con ilmajor num-bercoincidente alMOD_MAJOR. Tutte queste informazioni sono definite nella struttu-ra example_device_fops che contiene i puntatori a tutte le funzioni che operanosul nostro dispositivo e che corrispondono ad altrettante chiamate di sistema. Si notiinoltre la funzioneregister_chrdev() che serve per “registrare” il dispositivo al-l’interno del nucleo legandolo una volta per tutte al suomajor number. La funzionecomplementare viene invocata all’atto della rimozione delmodulo dal nucleo, perrendere nuovamente disponibile la risorsa per altri dispositivi.
Resta ora da precisare il modo per collegare un determinato modulo ad un pre-cisomajor number. È necessario aggiungere una riga nel file di configurazione deimoduli5 che crei questo legame. Ritornando all’esempio utilizzatofino ad ora, bastaaggiungere:
alias char-major-200 example_module.o
Fatto ciò il tentativo di apertura del file/dev/example_device avrà come effettoil caricamento del modulo nel kernel.
Si noti che il modulo rimarrà in “linkato” al kernel fin tanto che non avverrà unrimozione esplicita con l’opportuno comando, non vi è un metodo automatico dirimozione dei moduli dalkernel space.
5Nella distribuzione RedHat questo file è/etc/modules.conf
Bibliografia
[1] R.O. Ambrose, H. Aldridge, R.S. Askew, R.R. Burridge, W.Bluethmann,M. Diftler, C. Lovchik, D. Magruder, and F. Rehnmark. Robonaut: Nasa’sspace humanoid.IEEE Transactions on robotics and automation, 15(4), 2000.
[2] G.A. Bekey, R. Tomovic, and I. Zeljkovic. Control architecture for the belgra-de/usc hand. In T. Iberall S.T. Venkataraman, editor,Dexterous Robot Hands.Springer-Verlag, 1990.
[3] Luigi Biagiotti. Advanced robotic hands: design and control aspect. Tesi diDottorato, Università degli studi di Bologna, Facoltà di Ingegneria, 2003.
[4] J. Butterfass, M. Grebenstein, H. Liu, and G. Hirzinger.Dlr-hand ii: Nextgeneration of a dextrous robot hand. InProc. IEEE International Conferenceon Robotics and Automation, ICRA01, Seoul, Korea, 2001.
[5] Alessandro Casella. Caratterizzazione di sensori di forza per applicazio-ni robotiche. Tesi di Laurea, Università degli studi di Bologna, Facoltà diIngegneria, LAR, 2003.
[6] C.Melchiorri and G.Vassura. Mechanical and control features of the ub handversion ii. In Proc. IEEE/RSJ Int.Conf.on Intelligent Robots and Systems,IROS’92, 1992.
[7] S.C. Jacobsen et al. Design of the utah/mit dexterous hand. In Proc. IEEEInternational Conference on Robotics and Automation, ICRA86, 1986.
[8] FSM Labs, Inc.Getting Started with RTLinux, April 20 2001.
[9] Simone Gabrieli. Controllo di azionamenti elettrici inambiente rtlinux. Tesidi laurea, Università degli studi di Bologna, Facoltà di Ingegneria, LAR, 2002.
[10] J. M. Goldfarb and N. Celanovic. A flexure-based gripperfor small-scalemanipulation.Robotica, 17, 1999.
118 Bibliografia
[11] Marco Guidetti. Integrazione di visione artificiale e controllo real time per unrobot industriale. Tesi di Laurea, Università degli studi di Bologna, Facoltà diIngegneria, LAR, 2002.
[12] Brian Beej Hall. Beej’s guide to network programming. Revision version2.3.1, October 8 2001.
[13] L.L. Howell. Compliant mechanisms. John Wiley & Sons, 2001.
[14] LinMot. LinMot Tecnical Product Information, 05/2002 edition.
[15] F. Lotti and G. Vassura. A novel approach to mechanical design of articulatedfingers for robotic hands. InProc. IEEE/RSJ Int. Conf. on Intelligent Robotsand Systems ’02, IROS 02, 2002.
[16] F. Lotti and G. Vassura. Sviluppo di soluzioni innovative per la strutturameccanica di dita articolate per mani robotiche. InAssociazione Italiana perl’Analisi delle Sollecitazioni (AIAS) XXXI Convegno Nazionale, 2002.
[17] Prof. Claudio Melchiorri. Corso di robotica industriale. Università degli studidi Bologna, Facoltà di Ingegneria, A.A. 2001-2002. Dispense delle lezioni.
[18] Havoc Pennington.GTK+/Gnome Sviluppo di applicazioni. APOGEO, 2000.
[19] Janez Peres and Stanislav Kovacic. An alternative model of radial distortion inwide-angle lenses. Faculty of Electrical Engineering, Universitu of Ljubljana.
[20] Quanser.MultiQ PCI Data Acquisition System Programming Guide.
[21] S. Schulz, C. Pylatiuk, and G. Bretthauer. A new ultralight anthropomorphichand. InProc. IEEE International Conference on Robotics and Automation,ICRA01, Seoul, Korea, 2001.
[22] L. Sciavicco and B. Siciliano.Modelling and Control of Robot Manipulators.McGraw-Hill, 1996.
[23] Peter Jay Slazman and Ori Pomerantz. The linux kernel module programmingguide. Version 2.1, March 25 2003.
[24] Chao Zhang, James P.Helferty, Geoffrey McLennan, and William E.Higgins.Nonlinear distorsion correction in endoscopic video image. Penn Sta-te University, Departement of Electrical Engineering, University Park, PA16802.