Indice - Corso di Studi in Ingegneria dell`Automazione
Transcript
Indice - Corso di Studi in Ingegneria dell`Automazione
Indice Introduzione 5 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 11 1.1 Introduzione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11 1.2 Formulazione di Eulero-Lagrange . . . . . . . . . . . . . . . . . . . 12 1.3 Cinematica e dinamica di un manipolatore . . . . . . . . . . . . . . 13 1.3.1 Coordinate omogenee . . . . . . . . . . . . . . . . . . . . . . 13 1.3.2 Link, giunti e loro parametri . . . . . . . . . . . . . . . . . . 15 1.3.3 Rappresentazione di Denavit-Hartenberg . . . . . . . . . . . 17 1.3.4 Trasformazione omogenea associata ad un link . . . . . . . . 18 1.3.5 Jacobiano geometrico . . . . . . . . . . . . . . . . . . . . . . 20 1.3.6 Derivata di una matrice di rotazione . . . . . . . . . . . . . 21 1.3.7 Calcolo dello Jacobiano geometrico . . . . . . . . . . . . . . 22 1.3.8 Energia cinetica di un corpo rigido tridimensionale che si muove nello spazio . . . . . . . . . . . . . . . . . . . . . . . 23 Energia potenziale di un manipolatore . . . . . . . . . . . . 25 Formulazione di Newton-Eulero . . . . . . . . . . . . . . . . . . . . 26 1.4.1 Sistemi di coordinate rotanti . . . . . . . . . . . . . . . . . . 27 1.4.2 Sistemi di coordinate mobili . . . . . . . . . . . . . . . . . . 31 Cinematica dei link . . . . . . . . . . . . . . . . . . . . . . . . . . . 33 1.5.1 Equazioni ricorsive del moto dei manipolatori . . . . . . . . 36 1.5.2 Equazioni di moto di un link intorno al suo sistema di coor- 1.3.9 1.4 1.5 dinate . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1 40 INDICE 1.5.3 1.6 Algoritmo di calcolo . . . . . . . . . . . . . . . . . . . . . . 45 Dinamica diretta e dinamica inversa . . . . . . . . . . . . . . . . . . 46 2 Modelli e simulazione real-time e non real-time della dinamica di un robot 49 2.1 Introduzione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49 2.2 Generalità sul Robotics Toolbox . . . . . . . . . . . . . . . . . . . . 50 2.2.1 Note sulle convenzioni cinematiche . . . . . . . . . . . . . . 51 2.2.2 Definizione di un robot . . . . . . . . . . . . . . . . . . . . . 51 2.2.3 Notazione e formulazione ricorsiva di Newton-Eulero adottata nel “Robotics Toolbox” . . . . . . . . . . . . . . . . . . . 2.3 2.4 53 Simulazione real-time e procedura per il calcolo automatico della dinamica del manipolatore . . . . . . . . . . . . . . . . . . . . . . . 57 Congruenza dei modelli del robot . . . . . . . . . . . . . . . . . . . 63 3 Generazione e allineamento dei segnali encoder 67 3.1 Introduzione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67 3.2 Generazione dei segnali encoder . . . . . . . . . . . . . . . . . . . . 69 3.2.1 Funzionamento di un encoder incrementale . . . . . . . . . . 69 3.2.2 Algoritmo per la generazione dei segnali encoder . . . . . . . 72 3.3 Legge di controllo adattativa . . . . . . . . . . . . . . . . . . . . . . 75 3.4 Test di verifica per l’algoritmo di generazione segnali encoder e la legge di controllo adattativa . . . . . . . . . . . . . . . . . . . . . . 4 Il simulatore real-time 79 86 4.1 Introduzione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86 4.2 Descrizione del prototipo del sistema HIL . . . . . . . . . . . . . . . 86 4.3 Descrizione del sistema di controllo . . . . . . . . . . . . . . . . . . 93 4.4 Filtro dinamico per la stima della velocità dei giunti del robot . . . 93 4.5 Prove sperimentali . . . . . . . . . . . . . . . . . . . . . . . . . . . 97 4.5.1 INDICE Parametri di simulazione . . . . . . . . . . . . . . . . . . . . 100 2 INDICE 4.5.2 Grafici delle prove simulative . . . . . . . . . . . . . . . . . 102 5 Conclusioni 108 A La scheda Sensoray 626 111 A.1 Caratteristiche tecniche . . . . . . . . . . . . . . . . . . . . . . . . . 112 A.2 Installazione scheda Sensoray 626 . . . . . . . . . . . . . . . . . . . 114 A.3 Matlab e Sensoray 626 . . . . . . . . . . . . . . . . . . . . . . . . . 115 A.3.1 Libreria Real Time Windows Target . . . . . . . . . . . . . 115 A.4 Installazione di una scheda di I/O . . . . . . . . . . . . . . . . . . . 116 A.4.1 Creazione di un’ applicazione Simulink Real-Time . . . . . . 118 A.5 Tabelle I/O-pin Sensoray 626 . . . . . . . . . . . . . . . . . . . . . 122 B Il microcontrollore B.1 Generalità sul microcontrollore 126 . . . . . . . . . . . . . . . . . . . . 126 B.2 dsPIC30F4013 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 128 B.3 MPLAB . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131 B.4 dsPICDEM 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133 B.5 Step programmazione PIC . . . . . . . . . . . . . . . . . . . . . . . 137 C Codice C30 per l’algoritmo di generazione dei segnali encoder 141 C.1 Main.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142 C.2 INTx IO pins.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 144 C.3 A to D Converter.c . . . . . . . . . . . . . . . . . . . . . . . . . . 146 C.4 Timers.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 150 D Codice C per la definizione del modello real-time 158 D.1 RTrneSCARA.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 158 D.2 RTrneSCARAff.c . . . . . . . . . . . . . . . . . . . . . . . . . . . . 167 E Librerie ed oggetti del Robotics Toolbox 173 E.1 link . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 173 INDICE 3 INDICE E.2 robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 175 E.3 dh . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 177 E.4 dyn . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 178 E.5 robot/plot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 179 E.6 rne . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 180 E.7 Puma 560 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 182 F Collegamenti dei componenti hardware del sistema HIL 186 Bibliografia 189 INDICE 4 Introduzione La robotica è una scienza che, studiando i comportamenti degli esseri intelligenti, cerca di sviluppare delle metodologie che permettano ad una macchina (robot), dotata di opportuni dispositivi atti a percepire l’ambiente circostante ed interagire con esso quali sensori e attuatori, di eseguire dei compiti specifici. La parola robotica proviene dal ceco robota, dove ha il significato di “lavoro pesante” o “lavoro forzato”. Questo termine è stato introdotto dallo scrittore ceco Karel Capek, nel 1920 nel suo racconto R.U.R. (Rossum’s Universal Robots). Il termine inglese derivato robotics, secondo l’Oxford English Dictionary, compare per la prima volta in un racconto di fantascienza dello scrittore Isaac Asimov intitolato Bugiardo! (Liar!, 1941) sempre ad Asimov si deve anche l’invenzione delle famose Tre Leggi della Robotica enunciate interamente nel racconto Circolo vizioso (Runaround, 1942); entrambi i racconti fanno parte dell’antologia Io, Robot. La robotica è una disciplina relativamente nuova, che affonda le sue radici nell’antico desiderio dell’uomo di costruire strumenti che possano liberarlo da compiti troppo faticosi, noiosi o pericolosi. Anche se la robotica è una branca dell’ingegneria in essa confluiscono gli studi di molte discipline sia di natura umanistica come biologia, fisiologia, linguistica e psicologia che scientifica quali automazione, elettronica, fisica, informatica, matematica e meccanica. Il settore di maggior interesse nell’ambito di questo lavoro di tesi è quello che nasce dall’unione tra la robotica e l’automazione. L’automazione è un complesso di tecniche volte a sostituire l’intervento umano, o a migliorarne l’efficienza, nell’esercizio di dispositivi o impianti. Un importante settore della scienza dell’automazione è attualmente costituito dalle cosiddette soluzioni Hardware-in-the-loop. Il termine Hardware-in-the-loop (HIL) indica le tecniche di 5 Introduzione verifica (o testing) delle unità di controllo elettronico che vengono collegate ad appositi banchi di prova che riproducono in modo più o meno completo, fisicamente e/o in software, il prodotto finale a cui sono destinate le unità da testare. I banchi di prova sono costituiti dai seguenti componenti: • sensori : dispositivi che emulano la misura delle grandezze fisiche in gioco; • attuatori : dispositivi che eseguono l’azione di controllo su comando di una centralina o di un processore installato su un calcolatore; • cablaggi elettrici ed elettronici e relative connessioni : l’impianto elettrico e la rete che interconnette tra loro la centralina (il processore) con i sensori e gli attuatori; • modelli : programmi informatici che emulano quanto non fisicamente parte del banco ingannando i dispositivi di controllo effettivamente collegati. L’oggetto di questo lavoro di tesi è la realizzazione di un simulatore real-time di manipolatori robotici con sensori interfacciati tramite segnali encoder, cioè di un sistema che rappresenta una soluzione di tipo Hardware-in-the-loop (HIL). Scopo di questo simulatore è consentire di anticipare le verifiche sulle funzionalità e prestazioni di schede di controllo assi, già nella fase di progettazione e prototipazione, senza attendere la disponibilità del prodotto finale a cui sono destinate (es. robot o macchine utensili a controllo numerico). Infatti, i componenti hardware installati rispondono ai segnali simulati come se stessero operando in un ambiente reale, poiché non sono in grado di distinguere segnali provenienti da un ambiente fisico da quelli prodotti da modelli software. In questo modo il metodo HIL permette di riprodurre le condizioni operative più diverse ed osservare allo stesso tempo il comportamento del sistema controllato e delle schede controllo assi. La realizzazione di questo sistema Hardware-in-the-loop (HIL) a basso costo, si ottiene combinando in modo complementare due unità (o sistemi) di calcolo (si veda figura 1): 1. Unità lenta (Sistema Lento in figura 1), costituita da un PC con software real-time, che effettua la simulazione in tempo reale delle equazioni matemaIntroduzione 6 Introduzione tiche che descrivono il sistema: a questo scopo si utilizzano tecniche ad alte prestazioni basate sulla formulazione Newton-Eulero e ci si avvale di soluzioni presenti in letteratura in base alle quali i tempi di calcolo diventano minori del tempo reale simulato (cioè la realtà è più lenta della realtà simulata). 2. Unità veloce, costituita da un microcontrollore con opportuno programma di calcolo (Sistema Veloce in figura 1), che riceve i segnali di velocità su ogni asse dall’unità lenta e sulla base di questi produce i segnali encoder in fase e quadratura, riuscendo a garantire le necessarie frequenze, perché l’onere di calcolo legato alle equazioni matematiche del modello del sistema è demandato all’unità lenta. La sincronia tra le due unità di calcolo sopra citate è garantita da una legge di controllo adattativa (si veda il blocco Legge Adattativa in figura 1) che garantisce che l’informazione sulla posizione ricostruita dai segnali di velocità scambiati tra l’unità 1 e l’unità 2 coincida con la posizione simulata all’interno dell’unità lenta. Il sistema HIL, cosı̀ concepito, permette di simulare il modello virtuale di un manipolatore robotico, o meglio di robot inteso come catena cinematica aperta provvisto o meno di un effettore capace di manipolare oggetti esterni. Per caratterizzare il movimento degli elementi che compongono il modello virtuale, il blocco Simulatore Modello in figura 1 produce dei segnali analogici che corrispondono alla posizione e alla corrispondente velocità degli organi in movimento. La necessità di aggiungere un secondo sistema, il Sistema Veloce, nasce dall’incapacità, del primo sistema (oberato dall’onere del calcolo del modello matematico), di generare direttamente i segnali digitali (ad alta frequenza) encoder in fase e quadratura che descrivono il suddetto movimento delle parti che compongono il modello virtuale. I due blocchi in basso in figura 1 non sono essenziali per il funzionamento del sistema HIL ma possono essere aggiunti, in un secondo momento, per aumentare le funzionalità del sistema stesso. Essi illustrano come i segnali encoder simulati possano essere inviati ad un grande numero di dispositivi ausiliari che si comportino come “sniffers” della configurazione simulata nel sistema lento e che forniscano Introduzione 7 Introduzione Figura 1: Schema a blocchi del sistema HIL e logica di funzionamento. delle funzionalità aggiuntive. In particolare, il blocco Video consente all’utente di visualizzare a schermo e in tempo reale una animazione del movimento eseguito dal modello simulato. Il blocco Monitoraggio Robot rappresenta una qualsiasi funzionalità implementabile sulla base della misura dei segnali encoder (si pensi a trasmissione remota di dati, verifica di proprietà di compatibilità della configurazione dal punto di vista della sicurezza, dell’interazione con altri dispositivi meccanici, di valutazione di parametri di prestazione, etc). Le funzionalità aggiuntive fornite dai blocchi Video e Monitoraggio Robot non sono state ancora implementate al- Introduzione 8 Introduzione l’interno del prototipo di simulatore HIL in quanto non risultano necessarie ai fini della validazione e della valutazione delle prestazioni della soluzione HIL proposta. Gli argomenti trattati nella tesi sono organizzati in quattro capitoli principali e l’appendice. Nel primo capitolo viene richiamata la teoria della cinematica e dinamica di un manipolatore. Vengono inoltre riportate le formulazioni che consentono il calcolo delle equazioni dinamiche di un manipolatore robotico, la formulazione di Eulero-Lagrange e quella di Newton-Eulero che è stata adottata per simulare la dinamica del modello virtuale del robot all’interno del sistema HIL. Nel secondo capitolo si introduce prima brevemente il Robotics Toolbox, il toolbox che consente la simulazione non real-time dei modelli virtuali dei manipolatori robotici definiti a partire dalla corrispondente tabella di Denavit-Hartenberg. Successivamente si descrive la procedura di calcolo automatica necessaria per definire un modello virtuale di manipolatore robotico che sia adatto per una simulazione in tempo reale. Il secondo capitolo si chiude quindi con il confronto eseguito per garantire la congruenza dei suddetti modelli, il modello non real-time e quello real-time. Il terzo capitolo è dedicato oltre alla generazione dei segnali encoder, alla spiegazione e ai test di verifica della legge di controllo adattativa. Nel quarto capitolo si descrive dettagliatamente il prototipo di simulatore HIL realizzato e le prove sperimentali che hanno consentito di verificarne l’efficienza e le prestazioni. L’appendice, infine, contiene tutte le informazioni tecniche necessarie per la realizzazione pratica del prototipo di simulatore HIL proposto. In particolare sono presenti le informazioni relative all’installazione e al funzionamento delle schede, Sensoray 626, di acquisizione e generazione (I/O) dei segnali analogici e digitali (A/D), dotate di contatori encoder, che sono state usate per interfacciare i Computer (PC) con l’ambiente esterno. Viene spiegato che cos’è un microcontrollore e in che modo è possibile programmarlo con il codice C30 necessario per implementare all’interno del microcontrollore l’algoritmo di generazione dei segnali encoder descritto nel capitolo 3 di questo documento. Vengono inoltre riportate le librerie C scritte ed usate per definire i blocchi simulink C-MEX S-Function, indispensabili per definire e simulare in tempo reale il modello virtuale del manipolatore Introduzione 9 Introduzione robotico scelto. L’appendice si chiude con la descrizione delle principali librerie definite all’interno Robotics Toolbox e con la sezione dedicata alla descrizione delle connessioni elettriche che consentono di collegare fisicamente tra loro i dispositivi hardware che compongono il sistema HIL. Introduzione 10 Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 1.1 Introduzione La dinamica di un robot tratta le formulazioni matematiche delle equazioni del moto del manipolatore. Le equazioni dinamiche del moto di un manipolatore costituiscono un sistema di equazioni matematiche che ne descrivono il comportamento dinamico. Tali equazioni sono utili per la simulazione al calcolatore del movimento del braccio e per formulare le equazioni per il controllo dello stesso. Lo scopo del controllo di un manipolatore guidato da un calcolatore è far sı̀ che la sua risposta dinamica soddisfi le prestazioni ipotizzate in fase di progetto. In generale, le prestazioni di un manipolatore dipendono direttamente dall’efficienza dell’algoritmo di controllo e del modello dinamico del manipolatore. Il modello dinamico di un robot si ottiene da note leggi fisiche. Questo conduce allo sviluppo delle equazioni dinamiche del movimento per i vari giunti articolati del manipolatore in relazione ai parametri geometrici e inerziali specifici dei link. Obiettivo di questo capitolo è pertanto la presentazione di due metodi che consentono la derivazione delle equazioni del moto di un manipolatore nello spazio dei giunti. Il primo metodo è basato sulla formulazione di Lagrange, concettualmente semplice e sistematica, che conduce alla derivazione del modello dinamico in forma chiusa. Il secondo metodo è basato sulla formulazione di Newton-Eulero che consente di dedurre un modello di tipo ricorsivo che risulta efficiente da un punto di vista computazionale in quanto 11 § 1.2 Formulazione di Eulero-Lagrange sfrutta la natura seriale a catena aperta tipica del manipolatore. 1.2 Formulazione di Eulero-Lagrange Grazie all’applicazione diretta della formulazione di Eulero-Lagrange per sistemi non conservativi si possono esprimere convenientemente le equazioni di moto generali di un manipolatore. L’applicazione diretta della formulazione dinamica di Lagrange, insieme alla rappresentazione delle coordinate dei link di DenavitHartenberg, dà come risultato una formulazione algoritmica conveniente e compatta delle equazioni di moto del manipolatore. L’algoritmo viene espresso tramite operazioni matriciali e ciò semplifica sia l’analisi che l’implementazione su calcolatore. La derivazione delle equazioni dinamiche di un manipolatore a n gradi di libertà è basata sulla comprensione delle seguenti nozioni: • La matrice di trasformazione 4x4 in coordinate omogenee,i−1 Qi ,che descrive la relazione spaziale tra i sistemi di coordinate dei link i -esimo e (i−1)-esimo. Essa mette in relazione un punto stabilito sul link i, espresso in coordinate omogenee rispetto al sistema di coordinate i -esimo, col sistema di coordinate (i − 1)-esimo. • L’equazione di Eulero-Lagrange: d ∂L ∂L − = τi dt ∂ q˙i ∂qi i = 1, 2, ..., n dove: L = funzione lagrangiana = energia cinetica T - energia potenziale U T = energia cinetica totale del manipolatore U = energia potenziale totale del manipolatore qi = coordinate generalizzate del braccio q˙i = derivata prima rispetto al tempo delle coordinate generalizzate, qi τi = forza generalizzata (o momento) applicata al sistema al giunto i per muovere il link i Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 12 § 1.3 Cinematica e dinamica di un manipolatore Dalla precedente equazione di Eulero-Lagrange, per prima cosa viene richiesto di scegliere un insieme di coordinate generalizzate atte a descrivere il sistema, cioè un insieme di coordinate che rappresenti la configurazione di un sistema (posizione e orientamento) rispetto a un sistema di coordinate di riferimento. Per descrivere un manipolatore si possono trovare diversi insiemi di coordinate generalizzate. Comunque, dal momento che le posizioni angolari dei giunti sono rapidamente ottenibili perché possono essere misurate con potenziometri o encoder o altri sensori, esse hanno una ovvia corrispondenza con le coordinate generalizzate. Questo, in effetti, corrisponde alle coordinate generalizzate con la variabile del giunto definita in ognuna delle matrici di trasformazione 4x4 delle coordinate dei link. Cosı̀, nel caso di giunto rotazionale, qi ≡ θi , rotazione del giunto; per un giunto prismatico, qi ≡ di , distanza percorsa dal giunto. Infine, la formulazione di Eulero-Lagrange richiede la conoscenza dell’energia potenziale e dell’energia cinetica del sistema fisico, che a sua volta richiede la cognizione della velocità di ogni giunto. Nel seguito dopo aver proposto un richiamo teorico per introdurre una formulazione algoritmica delle equazioni dinamiche, verrà spiegato come ricavare le suddette grandezze e illustrata per ciscuna, la procedura che ne permette il calcolo automatico al computer. 1.3 1.3.1 Cinematica e dinamica di un manipolatore Coordinate omogenee Con riferimento alla figura 1.1, dato un punto P nello spazio e dati due sistemi di riferimento (Oxyz )0 e (Oxyz )1 , se le coordinate del punto o1 rispetto al sistema di riferimento (Oxyz )0 sono rappresentate dal vettore d, e se q0 e q1 denotano le coordinate del punto P nei due sistemi di riferimento, allora la seguente relazione può essere verificata con semplici considerazioni geometriche di composizione di vettori: q0 = 0 R1 q1 + d Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore (1.1) 13 § 1.3 Cinematica e dinamica di un manipolatore Figura 1.1: Rappresentazione di un punto in diversi sistemi di coordinate. dove 0 R1 è la matrice di rotazione (nella spiegazione si suppone nota la forma delle matrici 3x3 di rotazione) del sistema di riferimento (Oxyz )1 rispetto al sistema di riferimento (Oxyz )0 . Per rappresentare in maniera più compatta il generico spostamento rigido (1.1), invece delle coordinate cartesiane [ x y z ]T si utilizzano le coordinate omogenee: [ ωx ωy ωz ω ]T , costituite non più da tre ma da quattro componenti, di cui la quarta costituisce il fattore di scala per cui le componenti x, y, z sono moltiplicate e può essere utilizzata per risalire alle coordinate cartesiane. Sulla base delle coordinate omogenee si definiscono le trasformazioni omogenee, rappresentate da matrici appartenenti allo spazio R4x4 che consentono la rappresentazione compatta non solo di rotazioni (come avveniva per le matrici nella spazio R3x3 ) ma anche di traslazioni. La forma più generale per la trasformazione omogenea è data dalla seguente formula: ω 1 q1 R d ω 0 q0 = ω1 f s ω0 (1.2) dove R ∈ R3x3 è la matrice di rotazione, d ∈ R3 è il vettore di posizione, f ∈ R1x3 è il vettore di trasformazione prospettica e s ∈ R è il fattore di scala. Nelle applicazioni robotiche (nel nostro caso), si considera sempre f = [ 0 0 0 ] e s = 1, quindi le coordinate omogenee associate ad un punto di coordinate cartesiane [ x y z ]T saranno sempre definite come [ x y z 1 ]T (il fattore di scala e il vettore di trasformazione prospettica sono usati, ad esempio, in applicazioni di computer grafica). Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 14 § 1.3 Cinematica e dinamica di un manipolatore Sulla base della struttura delle trasformazioni omogenee descritta dall’equazione (1.2), si possono scrivere le seguenti matrici di rotazioni cos(θ) − sin(θ) 0 0 sin(θ) cos(θ) 0 0 Qz,θ := 0 0 1 0 0 0 0 1 1 0 0 0 0 cos(θ) − sin(θ) 0 Qx,θ := 0 sin(θ) cos(θ) 0 0 0 0 1 cos(θ) 0 sin(θ) 0 0 1 0 0 Qy,θ := − sin(θ) 0 cos(θ) 0 0 0 0 1 e la matrice di traslazione omogenea fondamentale 1 0 0 dx 0 1 0 dy Qtr,d := 0 0 1 dz 0 0 0 1 omogenee fondamentali : (1.3) (1.4) (1.5) (1.6) dove d = [ dx dy dz ]T costituisce un generico spostamento traslatorio. Il vantaggio delle coordinate e trasformazioni omogenee sta nel fatto che l’equazione (1.1), caratterizzante un generico spostamento rigido, può essere descritta in forma matriciale come segue, tramite la trasformazione omogenea 0 Q1 : 0 0 R d q0 q R q + d 1 1 1 1 0 P0 = = Q1 P 1 = = 0 0 0 1 1 1 1 (1.7) e, analogamente a quanto accadeva per le rotazioni (descritte da matrici in R3x3 ), gli spostamenti rigidi si possono comporre, consentendo la rappresentazione compatta di catene di sistemi di riferimento in relazione l’uno con l’altro. In particolare, se P0 = 0 Q1 P1 e P1 = 1 Q2 P2 , allora si può sostituire e ottenere: P 0 = 0 Q1 P 1 = 0 Q1 1 Q2 P 2 = 0 Q2 P 2 1.3.2 Link, giunti e loro parametri Un manipolatore meccanico consiste in una sequenza di segmenti rigidi chiamati link, connessi da giunti prismatici o di rotazione. Ogni coppia giunto-link, rappresenta un grado di libertà. Dunque, per un manipolatore con N gradi di libertà ci Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 15 § 1.3 Cinematica e dinamica di un manipolatore sono N coppie giunto-link con il link 0 (non considerato parte del robot) ancorato a una base di appoggio dove di norma è stabilito un sistema di coordinate inerziale; all’ultimo link è collegato un utensile. I giunti e i link sono numerati in successione partendo dalla base; cosı̀ il giunto 1 costituisce il punto di connessione fra il link 1 e la base di supporto. Ogni link è connesso al più a due altri link perché non si formino catene chiuse. In generale, due link sono collegati fra loro con un giunto di prima specie, e di questi, solo i giunti rotazionali e prismatici sono comuni nei manipolatori. I primi sono giunti associati ad uno spostamento angolare dei link adiacenti, mentre i secondi sono associati ad uno spostamento lineare dei link adiacenti. L’asse di un giunto si definisce come l’asse di rotazione o di traslazione individuato dal movimento del giunto. Questo asse avrà due normali, una per ogni link. Si faccia riferimento alla figura 1.2. Il significato dei link, considerati in un’ot- Figura 1.2: Sistemi di coordinate dei link e loro parametri. tica cinematica, è che essi mantengono una configurazione fissa tra i loro giunti che può essere caratterizzata da due parametri ai , αi . Il parametro ai rappresenta la distanza più breve misurata lungo la normale comune tra gli assi dei giunti (cioè gli assi zi−1 e zi rispettivamente per i giunti i e i+1 ), e αi è l’angolo compreso tra gli assi dei giunti su un piano perpendicolare ad ai . Cosı̀ ai e αi possono essere chiamati, rispettivamente, lunghezza e angolo di twist del link i. Essi sono associati alla struttura del link. Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 16 § 1.3 Cinematica e dinamica di un manipolatore Dato l’asse zi , la posizione relativa dei due link adiacenti (i−1) e i è data da di , che è la distanza misurata lungo l’asse del giunto fra la normale proveniente dall’asse del giunto i e la normale che va al giunto i + 1 (ovvero, il segmento ai ). L’angolo θi del giunto è l’angolo formato dalle 2 normali, misurato su un piano ortogonale all’asse del giunto. Dunque, di e θi possono essere chiamati rispettivamente distanza e angolo di giunto, fra due link adiacenti. Essi sono associati alla posizione relativa tra i due link. Riassumendo a ciascuna coppia giunto/link di un manipolatore sono associati quattro parametri: ai , αi , di , θi . Una volta stabilita una convenzione di segno per ognuno di essi, questi costituiscono un insieme sufficiente a determinare completamente la configurazione cinematica di ciascun anello della catena articolata del braccio. In base alla caratterizzazione precedente, dato un manipolatore, sulla base dei quattro parametri ai , αi , di , θi , si può compilare una tabella: dove ogni link (giunto) Link/giunto 1 2 . . . N αi α1 α2 . . . αN ai a1 a2 . . . aN θi θ1 θ2 . . . θN di d1 d2 . . . dN Tabella 1.1: Tabella dei parametri per un robot generico. corrisponde ad una riga su cui sono presenti i parametri caratteristici del link e del giunto i. Questa tabella sarà utile per il calcolo della cinematica diretta del robot, ovvero della funzione che trasforma le coordinate del sistema di effettore (o della mano) nel sistema di coordinate di base. 1.3.3 Rappresentazione di Denavit-Hartenberg Per descrivere le relazioni di rotazione e traslazione tra link adiacenti, Denavit e Hartenberg hanno proposto un metodo matriciale per stabilire sistematicamente un sistema di coordinate per ogni link di una catena articolata. La rappresen- Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 17 § 1.3 Cinematica e dinamica di un manipolatore tazione di Denavit-Hartenberg (D-H) consiste in una matrice di trasformazione omogenea i−1 Qi ∈ R4x4 che rappresenta il sistema di coordinate del link i rispetto al riferimento del link precedente (i − 1). Pertanto, attraverso trasformazioni sequenziali, l’estremità dell’effettore, espresso nell’ultimo sistema di coordinate, può essere trasformata nelle ’coordinate di base’ che costituiscono il sistema di riferimento inerziale di questo sistema dinamico. Come già accennato, il robot è individuato da un sistema di riferimento inerziale (Oxyz )0 , detto di base, da un sistema di riferimento di effettore (Oxyz )e (anche detto “della mano”) e dai sistemi di riferimento intermedi, ognuno assocciato ad un link (e ad un giunto). Per ognuno di questi N sistemi di riferimento (dove N sono i gradi di libertà della catena cinematica), la procedura di D-H, fornisce delle regole di tracciamento (che in questo contesto non verranno illustrate) in termini di: 1. posizione dell’asse zi 2. posizione del centro oi 3. posizione dell’asse xi e stabilisce una convenzione per la determinazione dei quattro parametri ai , αi , di , θi , di ciascun giunto, necessari a compilare la tabella (vedi tabella 1.1) di DenavitHartenberg. 1.3.4 Trasformazione omogenea associata ad un link Stabilito il sistema di coordinate di D-H per ogni link, si può sviluppare facilmente una matrice di trasformazione omogenea che metta in relazione il sistema di coordinate i -esimo col sistema di coordinate (i − 1)-esimo. Per ottenere la suddetta matrice, bisogna prima sovrapporre il sistema di riferimento (Oxyz )i al sistema di riferimento (Oxyz )i−1 , poi effettuare le seguenti operazioni elementari per riportare (Oxyz )i nella sua posizione originaria: 1. traslare lungo l’asse zi−1 di una lunghezza pari a di per portare l’origine oi sul punto di intersezione tra il segmento ai e l’asse zi−1 ; Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 18 § 1.3 Cinematica e dinamica di un manipolatore 2. ruotare intorno all’asse zi−1 di un angolo θi per allineare l’asse xi con il segmento ai ; 3. traslare lungo l’asse xi di una lunghezza pari ad ai per portare oi nella posizione originaria; 4. ruotare intorno all’asse xi di un angolo pari ad αi per portare l’asse zi nella posizione originaria. Le prime due trasformazioni sono riferite al sistema di riferimento (i − 1) e corrispondono dunque alla premoltiplicazione per le matrici Qtr,[0,0,di ] e Qz,θi , rispettivamente. Le ultime due trasformazioni, sono invece riferite al sistema di riferimento i e corrispondono alla postmoltiplicazione per le matrici Qtr,[ai ,0,0] e Qx,αi , rispettivamente.In sintesi la trasformazione i−1 Qi si costruisce secondo la seguente sequenza: i−1 Qi = Qz,θi Qtr,[0,0,di ] Qtr,[αi ,0,0] Qx,αi (1.8) cos(θi ) −cos(αi )sin(θi ) sin(αi )sin(θi ) ai cos(θi ) sin(θi ) cos(αi )cos(θi ) −sin(αi )cos(θi ) ai sin(θi ) i−1 Qi = 0 sin(αi ) cos(αi ) di 0 0 0 1 (1.9) Poichè la (1.9) è di valore generale, essa può essere applicata iterativamente sostituendo i dati della tabella 1.1 per ottenere in maniera immediata la matrice 0 Qe (e ≡ N ) della cinematica diretta. Dunque, il calcolo della cinematica diretta, si scompone in una procedura costituita dai seguenti passi: 1. Si analizza la struttura del manipolatore posizionando tutti i sistemi di riferimento secondo la procedura di Denavit-Hartenberg; 2. In base alla posizione dei sistemi di riferimento, si ricavano i parametri di D-H, compilando la tabella 1.1; 3. Si sostituiscono i valori della riga i -esima della tabella nella corrispettiva matrice di trasformazione i−1 Qi e si semplificano le moltiplicazioni; Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 19 § 1.3 Cinematica e dinamica di un manipolatore 4. Si moltiplicano tutte le matrici ricavate al punto precedente per ricavare la matrice finale: P 0 = 0 Qe P e = 0 Q1 1 Q2 · · · 1.3.5 N −1 Qe P e Jacobiano geometrico Si consideri un manipolatore ad N gradi di libertà. Sia nota l’equazione cinematica diretta nella forma: 0 in cui q = q1 . . . q N QN (q) = T 0 RN (q) d0N (q) 0T 1 (1.10) è il vettore delle variabili di giunto. Al variare di q variano sia la posizione che l’orientamento dell’organo terminale del manipolatore. Obiettivo della cinematica differenziale è quello di determinare la relazione tra velocità ai giunti e velocità lineare e angolare dell’organo terminale. In altri termini, si vogliono esprimere come vettori liberi la velocità lineare d˙ e la velocità angolare ω dell’organo terminale in funzione delle velocità delle variabili di giunto mediante relazioni del tipo: d˙ = Jp (q)q̇ (1.11) ω = Jω (q)q̇ (1.12) Nella (1.11) Jp è la matrice (3 × N ) relativa al contributo delle velocità dei giunti alla velocità lineare d˙ dell’organo terminale, mentre nella (1.12) Jω è la matrice (3 × N ) relativa al contributo delle velocità dei giunti alla velocità angolare ω dell’organo terminale. In forma compatta, i legami (1.11),(1.12) possono essere scritti come v= d˙ ω = J(q)q̇ (1.13) che rappresenta l’equazione cinematica differenziale del manipolatore. La matrice J (6 × N ) è lo Jacobiano geometrico del manipolatore, che in generale risulta funzione delle variabili di giunto. Per effettuare il calcolo dello Jacobiano geometrico è opportuno richiamare delle proprietà delle matrici di rotazione e alcuni importanti risultati di cinematica dei corpi rigidi. Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 20 § 1.3 Cinematica e dinamica di un manipolatore 1.3.6 Derivata di una matrice di rotazione L’equazione cinematica diretta di un manipolatore (1.10) descrive, se (i = N ), la posizione e l’orientamento dell’organo terminale, in funzione delle variabili di giunto, in termini di un vettore di posizione d0N e di una matrice di rotazione 0 RN . Se l’obiettivo è quello di caratterizzare velocità lineare e angolare dell’organo terminale, è opportuno analizzare in primo luogo la derivata di una matrice di rotazione rispetto al tempo. Si supponga che la matrice di rotazione vari nel tempo, ovvero R = R(t). Dalla proprietà di ortogonalità di R si ha la relazione: R(t)RT (t) = I che, derivata rispetto al tempo, fornisce l’identità: Ṙ(t)RT (t) + R(t)ṘT (t) = 0 Posto: S(t) = Ṙ(t)RT (t) (1.14) la matrice (3 × 3) S risulta essere anti-simmetrica in quanto: S(t) + S T (t) = 0 (1.15) Moltiplicando da destra ambo i membri della (1.14) per R(t), si ottiene: Ṙ(t) = S(t)R(t) (1.16) che consente di esprimere la derivata temporale di R(t) in funzione della matrice stessa. La relazione (1.16), che lega la matrice di rotazione R alla sua derivata attraverso l’operatore anti-simmetrico S, ammette una interessante interpretazione fisica. Si considerino un vettore costante p0 ed il vettore p(t) = R(t)p0 . La derivata temporale di p(t) risulta: ṗ(t) = Ṙ(t)p0 Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 21 § 1.3 Cinematica e dinamica di un manipolatore che per la (1.16) può scriversi: ṗ(t) = S(t)R(t)p0 Se con il vettore ω(t) si indica la velocità angolare della terna R(t) all’istante di tempo t rispetto alla terna di riferimento, dalla meccanica è noto che: ṗ(t) = ω(t) × R(t)p0 Pertanto l’operatore matriciale S(t) descrive il prodotto vettoriale tra il vettore ω e il vettore R(t)p0 . La matrice S(t) è tale che i suoi elementi simmetrici rispetto alla diagonale princiT nella forma: pale rappresentano le componenti del vettore ω(t) = ωx ωy ωz 0 −ωz ωy 0 −ωx (1.17) S = ωz −ωy ωx 0 che giustifica la notazione S(t) = S(ω(t)). Inoltre, se R rappresenta una matrice di rotazione, si può dimostrare che vale la seguente relazione: RS(ω)RT = S(Rω) (1.18) che tornerà utile in seguito. 1.3.7 Calcolo dello Jacobiano geometrico Nota l’equazione cinematica diretta fino al giunto i ≤ N nella r1,1 (q) r1,2 (q) r1,3 (q) 0 r2,1 (q) r2,2 (q) r2,3 (q) Ri (q) d0i (q) 0 Qi (q) = = r3,1 (q) r3,2 (q) r3,3 (q) 0T 1 0 0 0 in cui q = ∂d0i (q) ∂q q1 . . . q i · q̇, allora Jpi (q) = T ∂d0i (q) ∂q Jpi (q) = forma: dx (q) dy (q) dz (q) 1 (1.19) è il vettore delle variabili di giunto. Poiché d˙0i = cioè: ∂dx (q) ∂q1 ∂dy (q) ∂q1 ∂dx (q) ∂q1 ∂dx (q) ∂q2 ∂dy (q) ∂q2 ∂dz (q) ∂q2 . . . . . . . . . ∂dx (q) ∂qi ∂dy (q) ∂qi ∂dz (q) ∂qi Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore (1.20) 22 § 1.3 Cinematica e dinamica di un manipolatore con i = 1 . . . N . Per il calcolo di Jωi si avrà dalla (1.14): ∂r1,1 (q) (q) q̇ ∂r1,2 q̇ ∂q ∂q ∂r2,1 (q) ∂r2,2 (q) 0 T 0 0 Si (ωi (q, q̇)) = Ṙi (q, q̇) Ri (q) = ∂q q̇ q̇ ∂q ∂r3,1 (q) ∂r3,2 (q) q̇ q̇ ∂q ∂q ∂r1,3 (q) q̇ ∂q ∂r2,3 (q) q̇ ∂q ∂r3,3 (q) q̇ ∂q dove: " N # X ∂rmn (q) ∂rmn (q) q̇ = q˙k , ∂q ∂q k k=1 0 T Ri (q) (1.21) m = 1, ..., 3 n = 1, ..., 3 Quindi, calcolata la matrice 0 Si (ωi (q, q̇)), dalla (1.17) si ottiene il vettore ωi (t) = T ω x i ω yi ω z i della velocità angolare della terna relativa al link i all’istante di tempo t. Quindi possiamo ora calcolare Jωi con: ∂ωx (q̇) ∂ωx (q̇) ∂ωxi (q̇) i i . . . ∂ q̇1 ∂ q̇2 ∂ q̇i ∂ωyi (q̇) ∂ωyi (q̇) ∂ωyi (q̇) Jωi (q) = ∂ q̇1 . . . , ∂ q̇2 ∂ q̇i ∂ωzi (q̇) ∂ωzi (q̇) ∂ωzi (q̇) . . . ∂ q̇1 ∂ q̇2 ∂ q̇i i = 1, ..., N (1.22) nella quale si dimostra che la dipendenza da q̇ scompare a valle delle derivate perché dalla (1.21) si osserva che q̇ compare linearmente in 0 Si (ωi (q, q̇)). 1.3.8 Energia cinetica di un corpo rigido tridimensionale che si muove nello spazio Figura 1.3: Posizione e orientamento di un corpo rigido. Si dimostra come può essere calcolata l’energia cinetica di un corpo rigido tridimensionale (es. link di un robot) che si muove nello spazio. Con riferimento alla figura 1.3, definiamo: Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 23 § 1.3 Cinematica e dinamica di un manipolatore 1. il sistema di riferimento inerziale (Oxyz); 2. il sistema di riferimento (Oxyz)0 solidale al corpo, ed avente l’origine O 0 che coincide con il baricentro G del corpo medesimo. Identifichiamo con P = x y z T = p 1 T le coordinate omogenee del T = puntoP rispetto al sistema di riferimento inerziale, e con P 0 = x0 y 0 z 0 1 0 T p 1 le coordinate omogenee dello stesso punto rispetto al sistema di riferimeto (Oxyz)0 . Nota l’equazione cinematica diretta nella forma: R d Q= 0 1 per la (1.7) si può scrivere: P = QP 0 ⇒ p = Rp0 + d (1.23) derivando la (1.23), si ottiene: ṗ = Ṙp0 + d˙ = SRp0 + d˙ ⇒ ṗT = p0T RT S T + d˙T da cui si può scrivere l’energia cinetica associata al corpo rigido come: Z Z 1 1 T T = ρ ṗ ṗdV = ρ p0T RT S T SRp0 + d˙T d + 2d˙T SRp0 dV 2 V 2 V (1.24) (1.25) la (1.25) si può scomporre nella somma di tre contributi: Z Z 1 1 1 0T T T 0 p R S SRp dV, T2 = ρ T1 = ρ d˙T ddV, T3 = ρd˙T d + 2d˙T SRp0 dV 2 V 2 V 2 (1.26) i quali possono essere semplificati come segue: 1. si può dimostrare, che se O 0 ≡ G, allora T3 = 0; 2. 1 T2 = ρ 2 Z V 1 M d˙T d dV = R 2 V dv Z V 1 d˙T d dV = M d˙T d 2 (1.27) per la cinematica differenziale, d˙ = Jp q̇ e quindi: 1 1 T2 = q̇ T (M JpT Jp )q̇ = q̇ T Bt q̇ 2 2 Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore (1.28) 24 § 1.3 Cinematica e dinamica di un manipolatore 3. 1 T1 = ρ 2 Z p0T RT S T SRp0 dV (1.29) V p0T RT S T SRp0 = p0T RT S T ISRp0 = p0T RT S T RRT SRp0 per la (1.18), cioè RT S(ω)R = S(RT ω) = S(ω 0 ), si ha: p0T RT S T RRT SRp0 = p0T S T (RT ω)S(RT ω)p0 p0T S T (ω 0 )S(ω 0 )p0 = traccia(S(ω 0 )p0 p0T S T (ω) quindi: 1 T1 = ρtraccia(S(ω 0 ) 2 Z p0 p0T dV S T (ω 0 ) V posto: J= Z p0 p0T dV S T (ω 0 )) V si può scrivere: 1 1 T1 = ρtraccia(S(ω 0 )JS T (ω 0 )) = ω 0T Iω 0 2 2 I si chiama matrice di inerzia e dipende univocamente da J secondo la relazione: Jxx Jxy Jxz J = ∗ Jyy Jyz , ∗ ∗ Jzz Jyy + Jzz −Jxy −Jxz ∗ Jxx + Jzz −Jyz (1.30) I= ∗ ∗ Jxx + Jyy Infine per la (1.12), ω = Jω q̇ e si ottiene: 1 1 1 T1 = ω T RIRT ω = q̇ T [JωT RIRT Jω ]q̇ = q̇ T Bω q̇ 2 2 2 1.3.9 (1.31) Energia potenziale di un manipolatore Sia U l’energia potenziale di un braccio e sia Ui l’energia potenziale di ogni suo link. Con riferimento all’equazione cinematica R1,1 (q) R1,2 (q) R2,1 (q) R2,2 (q) 0 Qi (q) = R3,1 (q) R3,2 (q) 0 0 diretta nella forma: R1,3 (q) dx (q) R2,3 (q) dy (q) R3,3 (q) dz (q) 0 1 Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 25 § 1.4 Formulazione di Newton-Eulero si ha che: Ui = −Mi gdz (q) i = 1, 2, ..., N (1.32) L’energia potenziale totale del braccio può essere ottenuta sommando le energie potenziali di tutti i link: U= N X Ui (1.33) i=1 dove g è la costante gravitazione diretta lungo l’asse z ed Mi è la massa del link i-esimo. 1.4 Formulazione di Newton-Eulero La formulazione di Eulero-Lagrange, vista nei paragrafi precedenti, consente di ricavare un sistema di equazioni differenziali non lineari del secondo ordine, che descrivono il comportamento dinamico del robot. Tuttavia l’uso di queste equazioni per calcolare in tempo reale i momenti nominali dei giunti in funzione della posizione, velocità ed accelerazione per ogni dato punto della traiettoria, costituisce un collo di bottiglia computazionale per un controllo real-time. Il problema è dovuto principalmente alla inefficienza delle equazioni del moto di Eulero-Lagrange, che utilizzano matrici di trasformazione omogenee 4 × 4. Per effettuare il controllo in tempo reale, è stato inizialmente proposto un modello dinamico semplificato che ignora le forze centrifughe e quelle di Coriolis. Ciò riduce il tempo necessario al calcolo dei momenti dei giunti a un valore accettabile. Le forze centrifughe e quelle di Coriolis sono però significative per i momenti dei giunti se il manipolatore si muove ad alta velocità. Ne consegue che questa dinamica semplificata del robot limita la velocità di movimento di ciascun link e ciò non è desiderabile nel tipico ambiente manifatturiero. Inoltre, gli errori nei momenti dei giunti, risultanti dalla mancata conoscenza delle forze di Coriolis e di quelle centrifughe, non possono essere corretti con un controllo retroazionato quando il braccio si muove ad alta velocità, a causa degli eccessivi momenti correttivi richiesti. Per superare questi inconvenienti e per trovare equazioni del moto più efficaci, molti ricercatori si sono rivolti alla seconda legge di Newton, e hanno sviluppato varie Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 26 § 1.4 Formulazione di Newton-Eulero configurazioni di equazioni di moto di Newton-Eulero per una catena cinematica aperta (Armstrong [11], Orin et al. [17], Luch et al.[16], Walker e Orin [21]). Applicando questa formulazione al braccio di un robot si ottiene un sistema di equazioni ricorsive dirette e inverse con termini di prodotto vettotiale ”disordinati”. La caratteristica più importante e significativa di questa formulazione risiede nel fatto che il tempo di calcolo dei momenti applicati può essere notevolmente ridotto per permettere un controllo in tempo reale del manipolatore robotico. La derivazione è basata sul principio di d’Alambert e su di un sistema di equazioni matematiche che descrivono la relazione cinematica del movimento dei link rispetto al sistema di coordinate fondamentale. Per comprendere la formulazione di Newton-Eulero, nei prossimi paragrafi, saranno richiamati alcuni concetti dei sistemi di coordinate traslanti e rotanti. 1.4.1 Sistemi di coordinate rotanti Verranno ora sviluppate le relazioni matematiche che intercorrono tra un sistema di coordinate rotazionale e un sistema inerziale fisso di coordinate, e in seguito si estenderà il concetto fino a includere una trattazione delle relazioni tra un sistema di coordinate mobile (rotazionale e traslazionale) e un sistema inerziale. La figura 1.4, mostra due sistemi di coordinate destrorsi: un sistema di coordinate inerziale (sistema non asteriscato: OXY Z ), e un sistema di coordinate rotante (sistema asteriscato: OX ∗ Y ∗ Z ∗ ) le cui origini coincidono nel punto O, e gli assi OX ∗ , OY ∗ , OZ ∗ ruotano rispettivamente rispetto agli assi OX, OY , OZ. Siano (i,j,k) e (i*,j*,k*) i loro rispettivi versori lungo gli assi principali. Un punto r in quiete nel sistema asteriscato può essere espresso in relazione alle sue componenti in entrambi i sistemi di assi: r = xi + yj + zk (1.34) r = x∗ i* + y ∗ j* + z ∗ k* (1.35) o Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 27 § 1.4 Formulazione di Newton-Eulero Figura 1.4: Sistema di coordinate rotazionali. Si vuole ora valutare la derivata del punto r e, poiché i sistemi di coordinate ruotano uno rispetto all’altro, la derivata rispetto al tempo di r(t) potrà essere calcolata in relazione ai due differenti sistemi di coordinate. Queste due derivate vengono distinte con le seguenti notazioni: i) ii) d( ) = Derivata rispetto al tempo nel sistema di coordinate dt inerziale ( sistema non asteriscato e f isso ) (1.36) d∗ ( ) = Derivata rispetto al tempo nel sistema di coordinate dt rotante ( sistema asteriscato ) (1.37) Quindi usando la (1.34), la derivata rispetto al tempo di r(t) può essere espressa come: di dj dk dr = ẋi + ẏj + żk + x + y + z dt dt dt dt = ẋi + ẏj + żk (1.38) e usando la (1.35), la derivata asteriscata di r(t) è: d∗ i* d∗ j* d∗ k* d∗ r = ẋ∗ i* + ẏ ∗ j* + ż ∗ k* + x∗ + y∗ + z∗ dt dt dt dt ∗ ∗ ∗ = ẋ i* + ẏ j* + ż k* (1.39) Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 28 § 1.4 Formulazione di Newton-Eulero Facendo uso delle equazioni (1.35) e (1.39) la derivata rispetto al tempo di r(t) si può esprimere come: dr di* dj* dk* = ẋ∗ i* + ẏ ∗ j* + ż ∗ k* + x∗ + y∗ + z∗ dt dt dt dt di* dj* dk* d∗ r + x∗ + y∗ + z∗ = dt dt dt dt (1.40) Valutando questa derivata, si incontrano difficoltà nel calcolare: di* , dt dj* , dt dk* dt perché i versori, i*, j* e k* ruotano rispetto ai versori i, j e k. Per risolvere questo problema viene calcolata la derivata temporale di un vettore di modulo costante. Un vettore di modulo costante (e quindi in particolare un versore) può presentare una variazione nel tempo non nulla solo se esso cambia direzione. Infatti a seguito di una traslazione, cioè di un trasporto caratterizzato dall’invarianza delle componenti del vettore e quindi anche della sua direzione, non si ha alcuna variazione di nessuna delle sue proprietà (modulo, direzione e verso): pertanto la derivata temporale di → − un vettore che trasla è nulla. Nel seguito viene considerato il caso di un vettore A − di modulo costante che ruota con velocità angolare → ω di direzione e verso costante 0 intorno ad una direzione fissa definita da un asse aa (vedi figura 1.5). Il vettore → − 0 descrive un cono di vertice O ed asse aa , sicché la sua variazione infinitesima d A nel tempo dt è data da un vettore di modulo dA = A · sinθ · ωdt, essendo θ l’angolo → − → − − tra → ω ed A . Il vettore infinitesimo d A può scriversi pertanto nella forma: → − → − − dA = → ω × A dt da cui si ottiene: → − → − dA − =→ ω ×A dt (1.41) Tale risultato può anche applicarsi a tutti i versori, in particolare a quelli i*, j*, k* − di un sistema di assi cartesiani OX ∗ Y ∗ Z ∗ ruotante a velocità angolare → ω , rispetto ad un osservatore fisso, intorno ad un asse passante per O. In tal caso si ottengono Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 29 § 1.4 Formulazione di Newton-Eulero Figura 1.5: Derivata temporale di un vettore ruotante. le seguenti relazioni: di* = ω × i*, dt dj* = ω × j*, dt dk* = ω × k* dt (1.42) note come formule di Poisson. Si osservi che dalle (1.42) si ha, per successiva − derivazione rispetto al tempo, nel caso che → ω = costante: d2 i* d d di* di* = = (ω × i*) = ω × = ω × (ω × i*) dt2 dt dt dt dt d2 j* = ω × (ω × j*) dt2 d2 k* = ω × (ω × k*) dt2 dalle quali, insieme alle (1.42), è possibile ottenere le formule di ricorsione per le derivate temporali di ordine superiore di versori. A questo punto basta applicare le (1.42) nella (1.40) per ottenere: dr d∗ r = + x∗ (ω × i*) + y ∗ (ω × j*) + z ∗ (ω × k*) dt dt d∗ r +ω×r = dt Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore (1.43) 30 § 1.4 Formulazione di Newton-Eulero Quest’ultima è l’equazione fondamentale che stabilisce la relazione tra le derivate rispetto al tempo per sistemi di coordinate rotanti. Prendendo le derivate dei membri sinistro e destro della (1.43) e applicando le (1.42) ancora a r e d∗ r/dt, si ottiene la derivata seconda rispetto al tempo del vettore r(t): d2 r dr dω d d∗ r + ω × = + ×r dt2 dt dt dt dt ∗ d∗ r dω dr d∗2 r +ω× +ω×r + ×r = 2 +ω× dt dt dt dt d∗ r dω d∗2 r + ω × (ω × r) + ×r = 2 + 2ω × dt dt dt (1.44) La (1.44) è il ragionamento essenziale del teorema di Coriolis. Il primo termine della parte destra dell’equazione è l’accelerazione relativa al sistema di coordinate asteriscato. Il secondo termine è chiamato accelerazione di Coriolis. Il terzo termine è detto accelerazione centripeta (diretta verso il centro) di un punto rotante intorno a un asse. Si può verificare che ω × (ω × r) punta direttamente verso l’asse di rotazione ed è perpendicolare a esso. L’ultimo termine diventa zero per una velocità di rotazione costante intorno a un asse fisso. 1.4.2 Sistemi di coordinate mobili Viene ulteriormente esteso il concetto dei precedenti sistemi di coordinate rotanti per comprendere il movimento di traslazione del sistema di coordinate asteriscato rispetto a quello fisso non asteriscato. Dalla figura 1.6, il sistema di coordinate asteriscato O ∗ X ∗ Y ∗ Z ∗ ruota e trasla rispetto al sistema di coordinate non asteriscato OXY Z che è un sistema inerziale. Una particella p di massa m è individuata dai vettori r∗ ed r∗ rispetto alle origini dei suddetti sistemi di coordinate. L’origine O∗ è individuata dal vettore h rispetto all’origine O. La relazione tra i vettori r ∗ ed r∗ è data da (vedi figura 1.6 ): r = r∗ + h Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore (1.45) 31 § 1.4 Formulazione di Newton-Eulero Figura 1.6: Sistema di coordinate mobile. Se il sistema di coordinate asteriscato O ∗ X ∗ Y ∗ Z ∗ è mobile (cioè ruota e trasla) rispetto al sistema di coordinate inerziale non asteriscato OXY Z, allora: v(t) , dr∗ dh dr = + = v ∗ + vh dt dt dt (1.46) dove v∗ e v rappresentano le velocità della particella mobile p rispettivamente in relazione al sistema di coordinate O ∗ X ∗ Y ∗ Z ∗ e OXY Z, e vh è la velocità del sistema di coordinate asteriscato O ∗ X ∗ Y ∗ Z ∗ in relazione al sistema di coordinate non asteriscato OXY Z. Usando la (1.43), la (1.46) si può esprimere come: v(t) = dr∗ dh d∗ r∗ dh + = + ω × r∗ + dt dt dt dt (1.47) In modo simile, l’accelerazione della particella p rispetto al sistema di coordinate non asteriscato è: a(t) = d(vt) d 2 r∗ d 2 h = 2 + 2 = a∗ + ah dt dt dt (1.48) dove a∗ e a rappresentano le accelerazioni della particella mobile p relative rispettivamente ai sistemi di coordinate O ∗ X ∗ Y ∗ Z ∗ e OXY Z, e ah è l’accelerazione del sistema di coordinate asteriscato O ∗ X ∗ Y ∗ Z ∗ relativa al sistema di coordinate non asteriscato OXY Z. Utilizzando la (1.44), la (1.47) può essere espressa come: a(t) = d ∗ r∗ dω d2 h d∗2 r∗ ∗ ∗ + 2ω × + ω × (ω × r ) + × r + dt2 dt dt dt2 Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore (1.49) 32 § 1.5 Cinematica dei link Avvalendosi di questa introduzione ai sistemi di coordinate mobili, si può ora applicare tale concetto ai sistemi di coordinate dei link, che sono stati stabiliti per il braccio del robot, per ricavare le informazioni cinematiche dei link, e in seguito applicare il principio di d’Alambert a questi sistemi di coordinate traslanti e/o rotanti per ricavare le equazioni di moto del braccio. 1.5 Cinematica dei link L’obiettivo di questo paragrafo è di derivare un sistema di equazioni matematiche che, basandosi sui sistemi di coordinate mobili descritti nel paragrafo precedente, fornisca la relazione cinematica dei link traslanti e rotanti del braccio rispetto al sistema di coordinate fondamentale. Facendo riferimento alla figura 1.7, si ricorda che sul giunto i è stabilito un sistema di coordinate ortonormale (xi−1 , yi−1 , zi−1 ). Il sistema di coordinate (x0 , y0 , z0 ) è allora il sistema di coordinate fondamentale (applicato alla base), mentre i sistemi di coordinate (xi−1 , yi−1 , zi−1 ) e (xi , yi , zi ) 0 sono applicati rispettivamente al link i−1 con origine O ∗ , e al link i con origine O . 0 L’origine O è individuata da un vettore posizione pi rispetto all’origine O e da un vettore posizione p∗i dall’origine O ∗ rispetto al sistema di coordinate fondamentale. L’origine O ∗ è individuata da un vettore posizione pi−1 dall’origine O rispetto al sistema di coordinate fondamentale. Siano vi−1 e ωi−1 le velocità lineari e angolari del sistema di coordinate (xi−1 , yi−1 , zi−1 ) rispetto al sistema di coordinate fondamentale (x0 , y0 , z0 ). Siano ωi e ωi∗ le velocità 0 angolari di O rispetto a (x0 , y0 , z0 ) e a (xi−1 , yi−1 , zi−1 ) rispettivamente. Allora la velocità lineare vi e la velocità angolare ωi del sistema di coordinate (xi , yi , zi ) relative al sistema di coordinate fondamentale sono rispettivamente, (dalla (1.47)): vi = d∗ p∗i + ωi−1 × p∗i + vi−1 dt (1.50) e ωi = ωi−1 + ωi∗ Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore (1.51) 33 § 1.5 Cinematica dei link 0 Figura 1.7: Relazione tra i sistemi O, O ∗ e O . dove d∗ ( )/dt indica la derivata temporale rispetto al sistema di coordinate mobile (xi−1 , yi−1 , zi−1 ). L’accelerazione lineare v̇i e l’accelerazione angolare ω̇i del sistema di coordinate (xi , yi , zi ) rispetto al sistema di coordinate fondamentale sono (dalla (1.49)), rispettivamente: d∗ p∗i d∗2 p∗i ∗ + ω̇ × p + 2ω × i−1 i−1 i dt2 dt ∗ + ωi−1 × (ωi−1 × pi ) + v̇i−1 v̇i = (1.52) e ω̇i = ω̇i−1 + ω̇i∗ (1.53) quindi, dalla (1.43), l’accelerazione angolare del sistema di coordinate (x i , yi , zi ) rispetto a (xi−1 , yi−1 , zi−1 ) è: ω̇i∗ = d∗ ωi∗ + ωi−1 × ωi∗ dt (1.54) dunque, la (1.53) si può esprimere come: ω̇i = ω̇i−1 + d∗ ωi∗ + ωi−1 × ωi∗ dt Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore (1.55) 34 § 1.5 Cinematica dei link Richiamando la definizione dei parametri dei link e dei giunti e il procedimento per stabilire i sistemi di coordinate dei link del braccio, si applicano i sistemi di coordinate (xi−1 , yi−1 , zi−1 ) e (xi , yi , zi ) rispettivamente ai link i − 1 e i. Se il link i è traslazionale nel sistema di coordinate (xi−1 , yi−1 , zi−1 ) esso si sposterà nella direzione di zi−1 con una velocità del giunto q̇i relativa al link i − 1. Se il link ruota nel sistema di coordinate (xi−1 , yi−1 , zi−1 ), esso avrà velocità angolare ωi∗ e il movimento angolare del link i avverrà intorno all’asse zi−1 . Dunque: zi−1 q̇i se il link i è rotazionale ∗ ωi = 0 se il link i è traslazionale (1.56) dove q̇i è il modulo della velocità angolare del giunto i rispetto al sistema di coordinate (xi−1 , yi−1 , zi−1 ). In modo simile: d∗ ω ∗ zi−1 q̈i se il link i è rotazionale = 0 se il link i è traslazionale dt (1.57) Avvalendosi delle equazioni (1.56) e (1.57), le equazioni (1.51) e (1.55) possono essere espresse, rispettivamente: ωi−1 + zi−1 q̇i se il link i è rotazionale ωi = ωi−1 se il link i è traslazionale ω̇i = ω̇i−1 + zi−1 q̈i + ωi−1 × (zi−1 q̇i ) se il link i è rotazionale ω̇i−1 se il link i è traslazionale (1.58) (1.59) Utilizzando la (1.41), la velocità lineare e l’accelerazione del link i relative a i − 1 risultano essere, rispettivamente: ∗ d∗ p∗i ωi × p∗i se il link i è rotazionale = zi−1 q̇i se il link i è traslazionale dt d∗2 p∗i = dt2 d∗ ωi∗ dt × p∗i + ωi∗ × (ωi∗ × p∗i ) se il link i è rotazionale zi−1 q̈i se il link i è traslazionale (1.60) (1.61) Dunque, avvalendosi delle equazioni (1.60) e (1.51), la velocità lineare del link i rispetto al sistema di coordinate di riferimento è [dalla (1.50)]: ωi × p∗i + vi−1 se il link i è rotazionale vi = zi−1 q̇i + ωi × p∗i + vi−1 se il link i è traslazionale Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore (1.62) 35 § 1.5 Cinematica dei link Facendo uso delle seguenti identità dei prodotti vettoriali: (a × b) × c = b(a · c) − a(b · c) (1.63) a × (b × c) = b(a · c) − c(a · b) (1.64) e delle equazioni (1.56) a (1.61), l’accelerazione del link i, rispetto al sistema di riferimento, è (dalla (1.52)): se il link i è rotazionale ω̇i × p∗i + ωi × (ωi × p∗i ) + v̇i−1 zi−1 q̈i + ω̇i × p∗i + 2ωi × (zi−1 q̇i )+ v̇i = se il link i è traslazionale +ωi × (ωi × p∗i ) + v̇i−1 (1.65) Si noti che ωi = ωi−1 se nella (1.62) il link i è traslazionale. Le equazioni (1.58), (1.59), (1.62) e (1.65) descrivono l’informazione cinematica del link i che è utile per ricavare le equazioni di moto del braccio. 1.5.1 Equazioni ricorsive del moto dei manipolatori Partendo dalla conoscenza della cinematica di ciascun giunto, si vuole descrivere il moto dei link del braccio applicando il principio di d’Alambert a ogni giunto. Tale principio applica le condizioni di equilibrio statico ai problemi della cinematica considerando sia le forze di attuazione applicate esternamente, sia le forze di reazione degli elementi meccanici che si oppongono al movimento. Il principio di d’Alambert è valido in tutti gli istanti di tempo. È effettivamente una forma leggermente modificata della seconda legge del moto di Newton, e può essere cosı̀ enunciato: Per ogni corpo, la somma algebrica delle forze applicate esternamente e di quelle che oppongono resistenza al moto in qualunque direzione è zero. Si considera un link i, come mostrato nella figura 1.8, e si ammette che l’origine 0 O sia situata in corrispondenza del suo baricentro. Allora, mettendo in relazione le variabili definite dalla figura 1.7 con le variabili definite della figura 1.8 , le restanti variabili indefinite, espresse rispetto al sistema fondamentale di riferimento (x0 , y0 , z0 ) sono: Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 36 § 1.5 Cinematica dei link • mi = massa complessiva del link i; • r̄i = posizione del baricentro del link i dall’origine del sistema di riferimento applicato alla base; • s̄i = posizione del baricentro del link i dall’origine del sistema di coordinate (xi , yi , zi ); • p∗i = origine del sistema di coordinate i−esimo rispetto al sistema di coordinate (i − 1)−esimo; • v̄i = dr̄i dt = velocità lineare del baricentro del link i; • āi = dv̄i dt = accelerazione lineare del baricentro del link i; • Fi = risultante delle forze esterne applicate al link i nel baricentro; • Ni = momento esterno risultante applicato al link i nel baricentro; • Ii = matrice di inerzia del link i intorno al suo baricentro relativamente al sistema di coordinate (x0 , y0 , z0 ); • fi = forza esercitata sul link i dal link i − 1 nel sistema di coordinate (xi−1 , yi−1 , zi−1 ) per reggere il link i e i link successivi; • ni = momento esercitato sul link i dal link i − 1 sul sistema di coordinate (xi−1 , yi−1 , zi−1 ); Allora, tralasciando gli effetti di smorzamento viscoso di tutti i giunti, e applicando il principio di d’Alambert a ogni link, si ha: d(mi v̄i ) = mi āi dt (1.66) d(Ii ωi ) = Ii ω̇i + ωi × (Ii ωi ) dt (1.67) Fi = e Ni = Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 37 § 1.5 Cinematica dei link Figura 1.8: Forze e momenti sul link i. dove, utilizzando le equazioni (1.62) e (1.65), la velocità lineare e l’accelerazione del baricentro del link i sono, rispettivamente (in questo caso (xi , yi , zi ) è il sistema di coordinate rotante e traslante): vi = ωi × s̄i + vi (1.68) ai = ω̇i × s̄i + ωi × (ωi × s̄i ) + v̇i (1.69) e quindi dalla figura 1.8, e osservando tutte le forze e i momenti agenti sul link i, la risultante delle forze esterne Fi e il momento Ni sono quelli esercitati sul link i dalla gravità e dai link contigui, i − 1 e i + 1. Cioè: Fi = fi − fi+1 Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore (1.70) 38 § 1.5 Cinematica dei link e Ni = ni − ni+1 + (pi−1 − r̄i ) × fi − (pi − r̄i ) × fi+1 (1.71) = ni − ni+1 + (pi−1 − r̄i ) × Fi − (p∗i × fi+1 ) (1.72) Dunque, le equazioni precedenti possono essere riscritte in forma di equazioni ricorsive sapendo che: r̄i − pi−1 = p∗i + s̄i si ottiene: fi = Fi + fi+1 = mi āi + fi+1 (1.73) ni = ni+1 + p∗i × fi+1 + (p∗i + s̄i ) × Fi + Ni (1.74) e Le equazioni precedenti sono ricorsive e possono essere usate per trovare le forze e i momenti (fi , ni ) relativi ai link per i = 1, 2, ..., n in un manipolatore a n link, tenendo presente che fn+1 e nn+1 sono, rispettivamente, le forze e i momenti esercitati dalla mano del manipolatore su un oggetto esterno. La relazione cinematica tra i link contigui e l’assegnazione dei sistemi di coordinate (vista nei paragrafi precedenti) dimostra che se il giunto i è rotazionale, allora esso ruoterà effettivamente di qi radianti nel sistema di coordinate (xi−1 , yi−1 , zi−1 ) intorno all’asse zi−1 . Cosı̀, il momento applicato al giunto i è la somma delle proiezioni di ni sull’asse zi−1 e del momento di smorzamento viscoso in quel sistema di coordinate. Però, se il giunto i è traslazionale, allora esso traslerà di q i unità relativamente al sistema di coordinate (xi−1 , yi−1 , zi−1 ) lungo l’asse zi−1 . Perciò, la forza τi applicata a quel giunto è la somma delle proiezioni di fi sull’asse zi−1 e della forza dello smorzamento viscoso in quel sistema. Quindi, la forza/momento applicata al giunto i è: T ni zi−1 + bi q̇i se il link i è rotazionale τi = fTi zi−1 + bi q̇i se il link i è traslazionale Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore (1.75) 39 § 1.5 Cinematica dei link dove bi è il coefficiente di smorzamento viscoso per il giunto i nelle equazioni precedenti. Se la base di appoggio è fissata saldamente alla piattaforma e il link 0 è stazionario, allora sarà ω0 = ω̇0 = 0, v0 = 0 e v̇0 (per includere la gravità) è: 0 v̇0 = g = 0 dove |g| = 9.8062 m/s2 (1.76) gz Riassumendo, le equazioni di moto di Newton-Eulero consistono in un sistema di equazioni ricorsive dirette e inverse. Si tratta delle equazioni (1.58), (1.59), (1.65), (1.59), e di quelle da (1.73) a (1.75) e sono tutte elencate nella Tabella 1.2. Per le equazioni ricorsive dirette, la velocità e l’accelerazione lineare e angolare di ogni link si propagano dal sistema di riferimento fondamentale (coincidente con la base di appoggio) all’estremità dell’effettore. Per le equazioni ricorsive inverse, i momenti e le forze esercitati su ogni link sono calcolati in modo ricorsivo dall’estremità dell’effettore al sistema di riferimento fondamentale. Dunque, le equazioni dirette propagano informazioni cinematiche di ciascun link dal sistema fondamentale alla mano, mentre le equazioni inverse calcolano i momenti/forze necessari per ogni giunto dalla mano al sistema di riferimento fondamentale. . 1.5.2 Equazioni di moto di un link intorno al suo sistema di coordinate Le precedenti equazioni di moto del braccio indicano che le equazioni dinamiche risultanti N − E, escludendo la frizione degli ingranaggi, costituiscono un sistema di equazioni ricorsive compatte dirette e inverse. Questo sistema può essere applicato sequenzialmente al braccio del robot. La ricorsività diretta propaga l’informazione cinematica: le velocità angolari, le accelerazioni angolari e le accelerazioni lineari dal sistema di riferimento fondamentale (sistema inerziale) all’estremità dell’effettore. La ricorsività inversa trasmette invece le forze esercitate su ciascun link dall’estremità dell’effettore al sistema di riferimento fondamentale, e i momenti applicati ai giunti sono calcolati in base a queste forze. Un ovvio inconveniente delle equazioni di moto ricorsive sopra descritte risiede nel fatto che tutte le matrici inerziali Ii e i parametri geometrici fisici (r̄i , s̄i , pi−1 ), p∗i ) Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 40 § 1.5 Cinematica dei link Equazioni dirette: i = 1, 2, ..., n ωi = ωi−1 + zi−1 q̇i se il link i è rotazionale ωi−1 se il link i è traslazionale ω̇i = ω̇i−1 + zi−1 q̈i + ωi−1 × (zi−1 q̇i ) se il link i è rotazionale ω̇i−1 se il link i è traslazionale se il link i è rotazionale ω̇i × p∗i + ωi × (ωi × p∗i ) + v̇i−1 zi−1 q̈i + ω̇i × p∗i + 2ωi × (zi−1 q̇i )+ v̇i = +ωi × (ωi × p∗i ) + v̇i−1 se il link i è traslazionale ai = ω̇i × s̄i + ωi × (ωi × s̄i ) + v̇i Equazioni inverse: i = n, n − 1, ..., 1 . Fi = mi āi Ni = Ii ω̇i + ωi × (Ii ωi ) fi = Fi + fi+1 ni = ni+1 + p∗i × fi+1 + (p∗i + s̄i ) × Fi + Ni τi = nTi zi−1 + bi q̇i se il link i è rotazionale fTi zi−1 + bi q̇i se il link i è traslazionale Dove bi è il coefficiente di smorzamento viscoso per il giunto i. Le condizioni iniziali “consuete” sono: ω0 = ω̇0 = v0 = 0 e v̇0 = (0, 0, gz )T (perché sia compresa anche la gravità), dove |g| = 9.8062 m/s2 . Tabella 1.2: Equazioni di moto ricorsive di Newton-Eulero. sono riferiti al sistema di coordinate fondamentale. Ne consegue che essi variano conseguentemente al movimento del robot. Luh et al. [16] hanno perfezionato le precedenti equazioni di moto di N − E riferendo tutte le velocità, accelerazioni, matrici inerziali, posizione del baricentro di ogni link, e i momenti/forze ai sistemi di coordinate di ogni link. In virtù della natura della formulazione e del meto- Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 41 § 1.5 Cinematica dei link do per calcolare i momenti dei giunti, i calcoli si semplificano notevolmente. La conseguenza più importante di questa modificazione è che il tempo di calcolo dei momenti applicati è linearmente proporzionale al numero dei giunti del braccio ed è indipendente dalla sua configurazione. Ciò permette l’implementazione di un controllo in tempo reale, per il braccio, nello spazio delle variabili dei giunti. Sia i−1 Ri una matrice di rotazione 3 × 3 che trasforma ogni vettore riferito al sistema di coordinate xi , yi , zi nel sistema di coordinate xi−1 , yi−1 , zi−1 . Tale matrice è la sottomatrice 3 × 3 superiore sinistra della matrice di trasformazione omogenea i−1 Ai . É stato precedentemente dimostrato che: i Ri−1 = dove i−1 Ri −1 = i−1 Ri T cosθi −cosαi sinθi sinαi sinθi i−1 Ri = cosθi cosαi cosθi −sinαi cosθi 0 sinαi cosαi e i−1 Ri −1 cosθi cosθi 0 = −cosαi sinθi cosαi cosθi sinαi sinαi sinθi −sinαi cosθi cosαi (1.77) (1.78) (1.79) Invece di calcolare ωi , ω̇i , v̇i , āi , p∗i , s̄i , Fi , Ni , fi , ni e τi , che si riferiscono al sistema di coordinate fondamentale, si calcolano i R0 ωi , i R0 ω̇i , i R0 v̇i , i R0 āi , i R0 p∗i , i R0 s̄i , i R0 Fi , i R0 Ni , i R0 fi , i R0 ni e i R0 τi , che si riferiscono al sistema di coordinate del rispettivo link xi , yi , zi . Dunque, le equazioni (1.58), (1.59), (1.65), (1.69), (1.66), (1.67), (1.73), (1.74) e (1.75) diventano, rispettivamente: i R (i−1 R ω + z0 q̇i ) se il link i è rotazionale i R0 ωi = i i−1 i−1 0 i−1 Ri−1 ( R0 ωi−1 ) se il link i è traslazionale i R0 ω̇i = (1.80) i Ri−1 [i−1 R0 ω̇i−1 + z0 q̈i + (i−1 R0 ωi−1 ) × z0 q̇i ] se il link i è rotazionale i Ri−1 (i−1 R0 ω̇i−1 ) se il link i è traslazionale (1.81) Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 42 § 1.5 Cinematica dei link i R0 v̇i = i i i e i ( R0 ω̇i ) × (i R0 p∗i ) + (i R0 ωi ) × [(i R0 ωi ) × (i R0 p∗i )] +i Ri−1 (i−1 R0 v̇i−1 ) se il link i è rotazionale i Ri−1 (z0 q̈i +i−1 R0 v̇i−1 ) + (i R0 ω̇i ) × (i R0 p∗i ) +2 (i R0 ω̇i ) × (i Ri−1 z0 q̇i ) se il link i è traslazionale + (i R0 ωi ) × [(i R0 ωi ) × (i R0 p∗i )] (1.82) R0 āi = i R0 ω̇i × i R0 s̄i + i R0 ωi × i R0 ωi × i R0 s̄i +i R0 v̇i (1.84) R0 Ii 0 Ri (1.85) R0 Fi = mi i R0 āi R0 ω̇i + i R0 ωi × i R0 Ii 0 Ri i R0 ωi i R0 fi = i Ri+1 i+1 R0 fi+1 +i R0 Fi i R0 ni = i Ri+1 i+1 R0 ni+1 + i+1 R0 p∗i × i+1 R0 fi+1 + i R0 p∗i + i R0 s̄i × i R0 Fi + i R0 Ni R 0 Ni = τi = i ( i (1.83) (1.86) (1.87) (1.88) T (i R0 ni ) (i Ri−1 z0 ) + bi q̇i se il link i è rotazionale T (i R0 fi ) (i Ri−1 z0 ) + bi q̇i se il link i è traslazionale (1.89) dove z0 = (0, 0, 1)T , i R0 s̄i è la posizione del baricentro del link i in relazione al proprio sistema di coordinate xi , yi , zi , e i R0 p∗i è la posizione del sistema xi , yi , zi dall’origine di xi−1 , yi−1 , zi−1 rispetto al sistema di coordinate i−esimo ed è: ai i R0 p∗i = di sinαi (1.90) di cosαi e (i R0 Ii 0 Ri ) è la matrice di inerzia del link i intorno al suo baricentro riferita al sistema di coordinate del link xi , yi , zi . Quindi, riassumendo, le equazioni di moto di Newton-Eulero efficienti sono costituite da un sistema di equazioni ricorsive dirette e inverse con la dinamica e la cinematica di ogni link riferite al proprio sistema di coordinate. Nella Tabella 1.3 sono elencate le equazioni ricorsive. Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 43 § 1.5 Cinematica dei link Equazioni dirette: i = 1, 2, ..., n i i R0 ω i = i Ri−1 (i−1 R0 ωi−1 + z0 q̇i ) se il link i è rotazionale i Ri−1 (i−1 R0 ωi−1 ) se il link i è traslazionale R0 ω̇i = i i Ri−1 [i−1 R0 ω̇i−1 + z0 q̈i + (i−1 R0 ωi−1 ) × z0 q̇i ] se il link i è rotazionale Ri−1 (i−1 R0 ω̇i−1 ) se il link i è traslazionale i ( R0 ω̇i ) × (i R0 p∗i ) + (i R0 ωi ) × [(i R0 ωi ) × (i R0 p∗i )] +i Ri−1 (i−1 R0 v̇i−1 ) se il link i è rotazionale i R0 v̇i = i R0 āi = (i R0 ω̇i ) × (i R0 s̄i ) + (i R0 ωi ) × [(i R0 ωi ) × (i R0 s̄i )] +i R0 v̇i i Ri−1 (z0 q̈i +i−1 R0 v̇i−1 ) + (i R0 ω̇i ) × (i R0 p∗i ) +2 (i R0 ω̇i ) × (i Ri−1 z0 q̇i ) + (i R0 ωi ) × [(i R0 ωi ) × (i R0 p∗i )] se il link i è traslazionale Equazioni inverse: i = n, n − 1, ..., 1 i R0 fi = i Ri+1 (i+1 R0 fi+1 ) +i R0 Fi i R0 ni = i Ri+1 [i+1 R0 ni+1 + (i+1 R0 p∗i ) × (i+1 R0 fi+1 )] + (i R0 p∗i + i R0 s̄i ) × (i R0 Fi ) + i R0 Ni τi = ( T (i R0 ni ) (i Ri−1 z0 ) + bi q̇i se il link i è rotazionale T (i R0 fi ) (i Ri−1 z0 ) + bi q̇i se il link i è traslazionale Dove z0 = (0, 0, 1)T e bi è il coefficiente di smorzamento viscoso per il giunto i. Le condizioni iniziali “consuete” sono: ω0 = ω̇0 = v0 = 0 e v̇0 = (0, 0, gz )T (perché sia compresa anche la gravità), dove |g| = 9.8062 m/s2 . Tabella 1.3: Efficienti equazioni ricorsive di moto di Newton-Eulero. Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 44 § 1.5 Cinematica dei link 1.5.3 Algoritmo di calcolo Le equazioni di moto di Newton-Eulero rappresentano oggi il sistema di equazioni più efficiente che possa essere implementato su un calcolatore. Infatti le operazioni matematiche complessive (moltiplicazioni e addizioni) sono proporzionali a n, che rappresenta il numero di gradi di libertà del braccio. Dato che le equazioni di moto che sono state ottenute sono per natura ricorsive, è consigliabile considerare un approccio algoritmico per il calcolo dei momenti/forze applicati ai giunti per tutti gli attuatori dei giunti. Un siffatto algoritmo è descritto qui di seguito. Algoritmo: Dato un manipolatore a n link, questo procedimento di calcolo ricava il momento/forza nominale applicato a ogni giunto per tutti gli attuatori dei giunti. I calcoli sono basati sulle equazioni esposte nella Tabella 1.3. Condizioni iniziali: • n = numero dei link (n giunti); • ω0 = ω̇0 = v0 = 0 e v̇0 = (0, 0, gz )T dove |g| = 9.8062 m/s2 ; • Le variabili dei giunti sono: qi , q̇i , q̈i per i = 1, 2, ..., n; • Le variabili dei link sono: i, Fi , fi , ni , τi . Iterazioni dirette: NE1. [Inizializza il contatore delle iterazioni]. Pone i ←− 1; NE2. [Iterazione diretta per l’informazione cinematica]. Calcola i R0 ωi , i R0 ω̇i , i R0 v̇i , i R0 āi utilizzando le equazioni della Tabella 1.3; NE3. [Verifica i = n?]. Se i = n, va al passo NE4.; altrimenti pone i ←− i + 1 e ritorna a NE2. Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 45 § 1.6 Dinamica diretta e dinamica inversa Iterazioni inverse: NE4. [Assegna un valore a fn+1 e nn+1 ]. Pone fn+1 e nn+1 pari alla forza e al momento richiesti, rispettivamente, per portare il carico. Se non c’è carico, li pone a zero; NE5. [Calcola la forza/momento del giunto]. Calcola i R0 Fi , i R0 Ni , i R0 fi , i R0 ni e τi , dati fn+1 e nn+1 ; NE6. [Iterazione inversa]. Se i = n, termina; altrimenti pone i ←− i − 1 e va al passo NE5. 1.6 Dinamica diretta e dinamica inversa La formulazione di Lagrange e la formulazione di Newton-Eulero consentono di calcolare le relazioni esistenti tra le coppie di attuazione ai giunti (ed eventualmente le forze all’organo terminale) e il moto della struttura. Un confronto tra le due formulazione mette in evidenza quanto segue. La formulazione di Lagrange presenta i seguenti vantaggi: • è sistematica e di facile comprensione; • fornisce le equazioni di moto del robot in una forma analitica compatta che evidenzia la matrice generalizzata di inerzia B(q), la matrice a fattore delle forze centrifughe e di Coriolis C(q, q̇) e il vettore delle forze gravitazionali g(q) (utile ai fini delle sintesi del controllo); • è efficace se si vogliono considerare effetti meccanici più complessi quali ad esempio le deformazioni elastiche dei bracci. La formulazione di Newton-Eulero presenta il seguente vantaggio fondamentale: • è intrinsecamente un metodo ricorsivo che risulta efficiente da un punto di vista computazionale e particolarmente adatto ad essere implementato all’interno di un calcolatore. Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 46 § 1.6 Dinamica diretta e dinamica inversa Nello studio della dinamica di un manipolatore è significativo individuare la soluzione dei due tipi di problemi che si pongono in relazione al calcolo della dinamica diretta e della dinamica inversa. Il problema della dinamica diretta consiste nel determinare, per t > t0 , le accelerazioni risultanti ai giunti (q̈(t)) (e quindi per successiva integrazione q̇(t) e q(t)), quando siano assegnate le coppie ai giunti τ (t) (ed eventualmente le forze all’organo terminale h(t)) e siano note le posizioni q(t0 ) e le velocità q̇(t0 ) ai giunti (stato iniziale del sistema). Il problema della dinamica inversa consiste nel determinare le coppie ai giunti τ (t) necessarie alla generazione del movimento specificato assegnando le accelerazioni q̈(t), le velocità q̇(t) e le posizioni q(t) dei giunti (note le eventuali forze all’organo terminale h(t)). La soluzione del problema della dinamica diretta, indispensabile per la fase di simulazione del manipolatore, si ottiene calcolando le accelerazioni ai giunti q̈(t) mediante la seguente espressione (all’interno della quale è stata omessa la dipendenza dalla variabile temporale t): −1 q̈ = B (q) τ − τ 0 (1.91) dove 0 τ (q, q̇) = C(q, q̇)q̇ + R(q)q̇ + g(q) + J T (q)h (1.92) denota il contributo delle coppie dipendenti da posizioni e velocità dei giunti. Pertanto, ai fini della simulazione numerica del moto del manipolatore, una volta noto lo stato all’istante tk in termini della posizione q(tk ) e della velocità q̇(tk ), si calcola l’accelerazione q̈(tk ) con la (1.91). Utilizzando poi una regola di integrazione numerica con passo di integrazione ∆t, è possibile calcolare la velocità q̇(tk+1 ) e la posizione q(tk+1 ) all’istante tk+1 = tk + ∆t. Se le equazioni del moto sono ottenute mediante la formulazione di Newton-Eulero, è possibile calcolare la dinamica diretta con un metodo più efficiente da un punto di vista computazionale. Infatti, note 0 q e q̇, le coppie τ (q, q̇) nella (1.92) possono essere calcolate come le coppie fornite dall’algoritmo riportato nel §1.5.3 per q̈ = 0. Inoltre la colonna bi della matrice Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 47 § 1.6 Dinamica diretta e dinamica inversa generalizzata di inerzia B(q) può essere calcolata come il vettore delle coppie che si ottiene con il sopracitato algoritmo ponendo g0 = 0, q̇ = 0, q̈i = 1 e q̈j = 0 per i 6= j; iterando tale procedimento per i = 1, ..., n (n = numero giunti del robot) si costruisce la matrice B(q). Pertanto, avendo calcolato i valori correnti di B(q) e 0 τ (q, q̇) ed essendo noto τ , si possono integrare le equazioni (1.91) come illustrato sopra. Capitolo 1 Calcolo delle equazioni dinamiche del moto di un manipolatore 48 Capitolo 2 Modelli e simulazione real-time e non real-time della dinamica di un robot 2.1 Introduzione Nell’uso scientifico e tecnico, un modello è una rappresentazione di un oggetto o di un fenomeno, che corrisponde alla cosa modellata per il fatto di riprodurne (evidentemente alla luce di precisi riscontri fattuali ottenuti a partire da un metodo che sia garanzia di controllabilità) alcune caratteristiche o comportamenti fondamentali; in modo tale che questi aspetti possano essere mostrati, studiati, conosciuti laddove l’oggetto modellato non sia direttamente accessibile. La costituzione di un modello scientifico o tecnico, per quanto possa essere genericamente orientata o guidata in partenza da una teoria metafisica, è sempre il risultato di un contesto della prova rigoroso, predisposto in modo tale da non essere minimamente influenzato dalle aspettative e dall’interpretazione soggettiva dell’osservatore (si dice che l’osservazione e l’esperienza scientifiche, su cui si fonda la formulazione di modelli teorici validi, sono invarianti rispetto all’osservatore). In questa sezione vengono fornite le informazioni relative agli elementi necessari ed essenziali per definire e analizzare in simulazione il comportamento di un particolare modello meccanico. Il modello in questione è quello del manipolatore robotico SCARA implementato all’interno del simulatore real-time (Simulatore Modello in figura 1) per effettuare i test di validazione dell’intero sistema Hardware-in-the-loop (HIL). La congruen49 § 2.2 Generalità sul Robotics Toolbox za del modello real-time del manipolatore SCARA, definito mediante le librerie C real-time riportate in Appendice D e simulato con l’ausilio dei toolbox Matlab: “Simulink”, “Real Time Workshop” e “Real Time Windows Target”, viene verificata per confronto con il corrispondente modello non real-time definito con il “Robotics Toolbox” (RT). Il RT viene introdotto in questa sezione ma le sue principali funzioni sono disponibili e dettagliatamente descritte in Appendice E. 2.2 Generalità sul Robotics Toolbox Il RT è un toolbox per la simulazione e l’analisi dei risultati sperimentali effettuati sui robot reali. L’autore è il Prof. Peter I. Corke, CSIRO, Manufacturing Science and Technology Pinjarra Hills, Australia. Il RT rende disponibile un set di funzioni molto utili in robotica per lo studio della cinematica e della dinamica di robot e per la generazione di traiettorie. Per fornire le equazioni dinamiche di moto dei manipolatori il software impiega un algoritmo di calcolo efficiente basato sulla teoria di “Newton-Eulero”, descritta nel capitolo 1. All’interno del “Robotics Toolbox” tutti i parametri sono incapsulati in Matlab Objects. Gli oggetti Robot possono essere creati per ogni manipolatore a catena cinematica aperta. Sono già inclusi i modelli di alcuni robot particolarmente noti e/o diffusi, come il Puma 560, il manipolatore di Stanford o il manipolatore antropomorfo + polso sferico. Il toolbox fornisce anche funzioni per la manipolazione di dati come: • vettori; • trasformazioni omogenee; • quaternioni unitari. elementi essenziali per rappresentare posizione ed orientamento in 3D. Il Robotics Toolbox è liberamente disponibile all’indirizzo web: http : //www.petercorke.com/Robotics%20T oolbox.html Capitolo 2 Modelli e simulazione real-time e non real-time della dinamica di un robot50 § 2.2 Generalità sul Robotics Toolbox da qui è possibile scaricare la release più recente, bug-fixes ed eventuali aggiornamenti. Il manuale contenente un tutorial e la descrizione di ogni funzione messa a disposizione viene fornito nel file robot.pdf, mentre una demo che descrive le possibilità del toolbox può essere invocata con la funzione rtdemo. 2.2.1 Note sulle convenzioni cinematiche Esistono due diverse formulazioni della rappresentazione di Denavit-Hartenberg (D-H) per la rappresentazione di catene cinematiche: • la rappresentazione classica (vedi §1.3), dall’articolo del 1955 di DenavitHartenberg [14]; • la forma modificata, introdotta da Craig [13]. Entrambe le notazioni rappresentano un giunto tramite 2 translazioni (parametri a e d) e 2 rotazioni (parametri α e θ), ma le matrici omogenee corrispondenti sono diverse. L’ultima versione del toolbox supporta pienamente sia la convenzione classica che quella modicata. In questo lavoro di tesi si è scelto di adottatare la notazione classica proposta da D-H. 2.2.2 Definizione di un robot In RT, tutte le informazioni relative ad un manipolatore vengono passate ad un’unica funzione, la funzione robot(), che provvede alla costruzione di un oggetto Matlab (classe), che è proprio l’oggetto-robot e che le contiene interamente . Tra queste informazioni si trovano anzitutto quelle che servono a descrivere la catena cinematica del robot e che vengono usate, ad esempio, per il calcolo della cinematica diretta. Con riferimento alla figura 2.1, vengono di seguito descritti gli step necessari ed essenziali per definire un nuovo robot, in particolare viene considerato il manipolatore planare a 2-gdl. Se si assumono pari ad 1 le lunghezze dei link, i parametri D-H sono dati da: 1. Prima di tutto è necessario creare gli oggetti link con il comando Capitolo 2 Modelli e simulazione real-time e non real-time della dinamica di un robot51 § 2.2 Generalità sul Robotics Toolbox Figura 2.1: Manipolatore planare a 2-gdl. Link/giunto αi 1 0 2 0 ai 1 1 θi θ1 θ2 di 0 0 Tabella 2.1: Tabella dei parametri di D-H per il manipolatore planare 2-gdl. link([alpha A theta D sigma]) dove sigma è 0 se il giunto è rotazionale oppure 1 se è prismatico. >> L1 >> L2 L1 = link([0 1 0 0 0]) = 0.0 1.0 0.0 0.0 R L2 = link([0 1 0 0 0]) = 0.0 1.0 0.0 0.0 R 2. Gli oggetti-link vengono passati alla funzione robot() che crea l’oggetto-robot che sarà l’argomento fondamentale della maggior parte delle funzioni del toolbox. Si scrive: >> R = robot({L1, L2}) R = noname (2 axis, RR) grav = [0.00 0.00 9.81] standard D&H parameters alpha A theta D R/P 0.0 1.0 0.0 0.0 R (std) 0.0 1.0 0.0 0.0 R (std) Capitolo 2 Modelli e simulazione real-time e non real-time della dinamica di un robot52 § 2.2 Generalità sul Robotics Toolbox Infine per visualizzare le informazioni relative al link, si può usare il comando showlink(link): >> showlink(L1) alpha = 0 A = 1 theta = 0 D = 0 sigma = 0 mdh = 0 offset = 0 m = r = I = Jm = G = B = 0 Tc = 0 0 qlim = 2.2.3 Notazione e formulazione ricorsiva di Newton-Eulero adottata nel “Robotics Toolbox” Con riferimento alla formulazione ricorsiva di Newton-Eulero, ampiamente descritta nel capitolo 1 (§1.4), viene di seguito riportata la notazione adottata all’interno del Robotics Toolbox per descrivere la suddetta formulazione. Ricorsione diretta: 1 ≤ i ≤ n Se l’asse i + 1 è rotazionale: i+1 i+1 ω i+1 = i+1 Ri ω̇ i+1 =i+1 Ri i n ω i + z 0 q̇ i+1 i (2.1) ω̇ i + z 0 q̈ i+1 +i ω i × z 0 q̇ i+1 i+1 v i+1 =i+1 ω i+1 ×i+1 p∗i+1 +i+1 Rii v i i+1 v̇ i+1 =i+1 ω̇ i+1 ×i+1 p∗i+1 +i+1 ω i+1 × o n i+1 i+1 ∗ ω i+1 × pi+1 +i+1 Rii v̇ i o (2.2) (2.3) (2.4) Capitolo 2 Modelli e simulazione real-time e non real-time della dinamica di un robot53 § 2.2 Generalità sul Robotics Toolbox Figura 2.2: Notazione usata nel Robotics Toolbox per il calcolo della dinamica inversa. Se l’asse i + 1 è traslazionale: i+1 ω i+1 =i+1 Rii ω i (2.5) (2.6) ω̇ i+1 =i+1 Rii ω̇ i i+1 v i+1 =i+1 Ri z 0 q̇ i+1 +i v i +i+1 ω i+1 ×i+1 p∗i+1 (2.7) i+1 i i+1 i+1 i+1 ∗ i+1 i+1 Ri z 0 q̇ i+1 v̇ i+1 = Ri z 0 q̈ i+1 + v̇ i + ω̇ i+1 × pi+1 + 2 ω i+1 × +i+1 ω i+1 × i+1 ω i+1 ×i+1 p∗i+1 (2.8) i˙ v̄ i =i ω̇ i × si +i ω i × i ω i × si +i v̇ i (2.9) i+1 i i F i = mii v̄˙ i N i = Jii ω̇ i +i ω i × Jii ω i (2.10) (2.11) Capitolo 2 Modelli e simulazione real-time e non real-time della dinamica di un robot54 § 2.2 Generalità sul Robotics Toolbox Ricorsione inversa: n ≥ i ≥ 1 i f i =i Ri+1 i+1 f i+1 + F i o n i+1 i ∗ i i+1 i i i ∗ ni = Ri+1 ni+1 + Ri+1 pi × f i+1 + pi + si ×i F i +i N i (i ni )T (i Ri+1 z 0 ) se il link i + 1 è rotazionale T Qi = if (i Ri+1 z 0 ) se il link i + 1 è traslazionale i i (2.12) (2.13) (2.14) dove: • i = indice dei link, che può variare tra 1 ed n; • Ji = è il momento di inerzia del link i rispetto al suo baricentro; • si = è il vettore che individua la posizione del baricentro del link i, rispetto al sistema di riferimenti i; • ω i = velocità angolare del link i; • ω̇ i = accelerazione angolare del link i; • v i = velocità lineare del sistema di riferimento i; • v̇ i = accelerazione lineare del sistema di riferimento i; • v̄ i = velocità lineare del baricentro del link i; • v̄˙ i = accelerazione lineare del baricentro del link i; • ni = momento esercitato sul link i dal link i − 1; • f i = forza esercitata sul link i dal link i − 1; • N i = momento totale applicato nel baricentro del link i; • F i = forza totale applicata nel baricentro del link i; • Qi = forza o coppia applicata dall’attuatore sul giunto i; Capitolo 2 Modelli e simulazione real-time e non real-time della dinamica di un robot55 § 2.2 Generalità sul Robotics Toolbox • i−1 Ri = matrice (3x3) ortonormale di rotazione, che definisce l’orientamento del sistema di riferimento i rispetto al sistema di rifermento i − 1: i ∗ pi cosθi −cosαi sinθi sinαi sinθi i−1 Ri = cosθi cosαi cosθi −sinαi cosθi 0 sinαi cosαi −1 T i Ri−1 = i−1 Ri = i−1 Ri (2.15) (2.16) indica le coordinate, misurate dall’origine del sistema di riferimento i − 1, dell’origine del sistema di riferimento i, espresse rispetto al sistema di riferimento i−esimo. ai i ∗ pi = di sinαi di cosαi (2.17) z 0 è un vettore di modulo unitario (versore), avente la stessa direzione dell’asse Z, z 0 = [0 0 1]. Le condizioni iniziali ”consuete” sono: ω0 = ω̇0 = v0 = 0 e v̇0 = −g = −(0, 0, gz )T (perché sia compresa anche la gravità), dove |g| = 9.8062 m/s2 . Capitolo 2 Modelli e simulazione real-time e non real-time della dinamica di un robot56 § 2.3 Simulazione real-time e procedura per il calcolo automatico della dinamica del manipolatore 2.3 Simulazione real-time e procedura per il calcolo automatico della dinamica del manipolatore In questo paragrafo viene descritta la procedura per il calcolo automatico della dinamica del manipolatore, implementata con il software di simulazione Matlab, prodotto dalla MathWorks Inc. La procedura è basata sulla formulazione di Newton-Eulero introdotta nel capitolo 1, e fornisce un algoritmo di calcolo ricorsivo molto efficiente da un punto di vista computazionale. Il codice programma necessario per implementare il suddetto algoritmo viene automaticamente generato dal file.m, writeRTrne.m, che riceve in input la variabile oggetto robot (vedi § 2.2.2 ), definita dall’utente, e restituisce il file C, RTrne.c, specifico per il calcolo della dinamica del robot richiesto. Vengono di seguito descritti, uno alla volta e con il corrispondente codice, gli step della procedura di calcolo. Il robot oggetto della descrizione è il manipolatore SCARA. Procedura: 1. Si analizza la struttura del robot e si posizionano tutti i sistemi di riferimento secondo la procedura di Denavit-Hartenberg (D-H) (vedi figura 2.3); 2. In base alla posizione dei sistemi di riferimento, si ricavano i parametri di D-H e si compila la tabella corrispondente (vedi Tabella 2.2); 3. Si calcolano le coordinate del baricentro del link i, Gi = [gxi , gyi , gzi ], rispetto all’origine del sistema di riferimento solidale al link i. Le suddette coordinate vanno a completare le informazioni fornite dalla tabella di D-H (vedi Tabella 2.2); Capitolo 2 Modelli e simulazione real-time e non real-time della dinamica di un robot57 § 2.3 Simulazione real-time e procedura per il calcolo automatico della dinamica del manipolatore Figura 2.3: Il manipolatore SCARA con i sistemi di riferimento. Link/giunto 1 2 3 4 αi 0 +π 0 0 ai a1 a2 0 0 θi θ1 θ2 0 θ4 di d1 0 d3 0 gx i gx 1 gx2 gx 3 gx4 gy i gy 1 gy 2 gy 3 gy 4 gz i gz 1 gz 2 gz 3 gz 4 Tabella 2.2: Tabella dei parametri di D-H per il manipolatore SCARA. 4. Si individuano per ciascun link del robot i seguenti dati: • massa del link; • matrice simmetrica di inerzia del link; • l’inerzia (Jm) e il rapporto di riduzione (G) del motore di attuazione del giunto; Capitolo 2 Modelli e simulazione real-time e non real-time della dinamica di un robot58 § 2.3 Simulazione real-time e procedura per il calcolo automatico della dinamica del manipolatore • i valori di attrito viscoso e di Coulomb (valori riferiti al motore di attuazione del giunto); 5. Si definisce l’oggetto Matlab robot (vedi § 2.2.2 ) in cui vengono memorizzati, i parametri della tabella di D-H e tutti i dati precedentemente rilevati; 6. Si apre il file.m, writeRTrne.m, e si scrive all’interno di questo file il nome del file e dell’oggetto Matlab robot creati nel punto precedente; %%%%%%%%%%%%%%%%%%% robot SCARA %%%%%%%%%%%%%%%%%%%%%%% robot_scara %carica nel workspace il robot SCARA robots=scara; %copia nell’oggetto robots il robot SCARA fd = fopen(’RTrneSCARA.c’,’w’); %apre in scrittura il %file RTrneSCARA.c 7. Si manda in esecuzione il file writeRTrne.m, per generare automaticamente il file C, RTrneSCARA.c, specifico per il calcolo della dinamica del robot richiesto; 8. Si carica lo schema a blocchi simulink RobotRTsimulator (vedi figura 4.4), e facendo doppio click sul blocco ROBOT REAL-TIME, si accede allo schema subsystem mostrato in figura 2.4: Figura 2.4: Schema subsystem contenuto nel blocco ROBOT REAL-TIME. Capitolo 2 Modelli e simulazione real-time e non real-time della dinamica di un robot59 § 2.3 Simulazione real-time e procedura per il calcolo automatico della dinamica del manipolatore 9. Facendo doppio click sul blocco S-Function Builder (vedi figura 2.4) si apre la finestra di dialogo REAL-TIME S-Function Builder (vedi figura 2.5); Figura 2.5: Finestra di dialogo REAL-TIME S-Function Builder. 10. Vengono caricati all’interno dei campi della finestra di dialogo REAL-TIME Capitolo 2 Modelli e simulazione real-time e non real-time della dinamica di un robot60 § 2.3 Simulazione real-time e procedura per il calcolo automatico della dinamica del manipolatore S-Function Builder i file C ed i parametri di simulazioni necessari per consentire il calcolo della dinamica del manipolatore robotico scelto: • S-function name: robotdynSCARA; • Libraries: – Library/Object/Source files: RTrneSCARA.c ne.c vmath.c – Includes: #include <stdio.h> #include <stdlib.h> #include <string.h> #include <math.h> #include <frne.h> #include <vmath.h> – External function declarations: extern void RTrne(real T *u0, real T *u1, real T *u2, real T *y0); • Data Properties: – Input ports: Port name: u0, Dimension: 1-D, rows: 4, Complexity: real, Frame: off Port name: u1, Dimension: 1-D, rows: 4, Complexity: real, Frame: off Port name: u2, Dimension: 1-D, rows: 4, Complexity: real, Frame: off – Output ports: Capitolo 2 Modelli e simulazione real-time e non real-time della dinamica di un robot61 § 2.3 Simulazione real-time e procedura per il calcolo automatico della dinamica del manipolatore Port name: y0, Dimension: 1-D, rows: 41, Complexity: real, Frame: off • Outputs: RTrne(u0, u1, u2, y0); • Build Info: Spuntare la casella Generate wrapper TLC; 11. Cliccare la casella Build della finestra di dialogo REAL-TIME S-Function Builder per compilare il codice C e generare il blocco real-time obotdynSCARA che consente di simulare la dinamica del robot cosı̀ definito. Dopodiché chiudere la finestra di dialogo REAL-TIME S-Function Builder; 12. Selezionare Simulation −→ Configuration Parameters nel menù dello schema Simulink (vedi § A.4.1); 13. Nella finestra Configuration Parameters (figura A.9) nel menù Select selezionare Solver. Impostare in Solver options: • Type: Fixed-step; • Solver: ode1 (Euler); • Fixed-step size: tempo di campionamento desiderato (es. 0.01 s); • Periodic sample time constraint: Unconstrained. Sempre nel menù Select della finestra Configuration Parameters, selezionare Real-Time Workshop. Impostare in Target selection: • System target file: • Language: rtwin.tlc; C. Al termine cliccare su Apply e poi Ok; 14. Verificare che la directory di lavoro del Matlab sia la stessa in cui è memorizzato lo schema Simulink su cui stiamo lavorando; Capitolo 2 Modelli e simulazione real-time e non real-time della dinamica di un robot62 § 2.4 Congruenza dei modelli del robot 15. Dal menù principale dello schema Simulink selezionare Tools −→ Real-Time Workshop −→ Build Model; 16. Nel menù di simulazione selezionare External; 17. Impostare il tempo di simulazione desiderato; 18. Connettere l’hardware esterno necessario alla simulazione; 19. Dal menù principale dello schema Simulink selezionare Simulation −→ Connect To Target; 20. Premere il tasto Play per avviare la simulazione. 2.4 Congruenza dei modelli del robot I test di congruenza dei modelli del robot sono stati eseguiti per confermare la validità della procedura di calcolo automatico della dinamica del manipolatore. La verifica è stata effettuata per confronto tra i modelli real-time e non real-time dei manipolatori robotici scelti (manipolatori: PUMA 560, SCARA). La figura 2.6 mostra lo schema a blocchi che consente di eseguire il matching dei due robot. Lo schema è costituito da: 1. Un blocco subsystem TORQUE che genera le coppie applicate ai giunti del robot; 2. Un blocco subsystem ROBOT REAL-TIME che fornisce il modello real-time del manipolatore robotico in esame. 3. Un blocco subsystem ROBOT NON REAL-TIME che fornisce il modello non real-time del manipolatore robotico in esame; 4. Due blocchi To Workspace che memorizzano i segnali di posizione dei giunti del robot; Capitolo 2 Modelli e simulazione real-time e non real-time della dinamica di un robot63 § 2.4 Congruenza dei modelli del robot Figura 2.6: Schema a blocchi per il matching dei modelli del robot. 5. Un blocco Clock che restituisce e mostra a video il valore corrente del tempo di simulazione; 6. Un blocco Scope che registra l’andamento del segnale di errore tra i segnali di posizione dei giunti del robot prodotti dal modello real-time e non real-time. Dalle simulazioni risulta la validità del modello real-time, infatti a parità di segnali di coppia forniti in input, i due modelli, producono in uscita gli stessi segnali di posizione angolare dei giunti del robot. Ciò può essere facilmente verificato osservando le figure 2.7 e 2.8 che riportano i grafici forniti dalle simulazioni riguardo ai suddetti segnali. Capitolo 2 Modelli e simulazione real-time e non real-time della dinamica di un robot64 § 2.4 Congruenza dei modelli del robot Robot SCARA joint 1 [rad] 2 1 0 Real−Time Normal 0 5 10 15 20 25 30 0 5 10 15 20 25 30 0 5 10 15 20 25 30 0 5 10 15 time [sec] 20 25 30 joint 2 [rad] 2 1 0 joint 3 [m] 0.4 0.2 0 joint 4 [rad] 1 0 −1 Figura 2.7: Grafici matching modelli robot SCARA. Capitolo 2 Modelli e simulazione real-time e non real-time della dinamica di un robot65 § 2.4 Congruenza dei modelli del robot 0.5 0 Real−Time Normal 0 5 10 15 20 0 5 10 15 20 0 5 10 15 20 0 5 10 15 20 0 5 10 15 20 0 5 10 time [sec] 15 20 2 1 0 0 −1 −2 2 0 −2 2 0 −2 joint 6 [rad] joint 5 [rad] joint 4 [rad] joint 3 [rad] joint 2 [rad] joint 1 [rad] Robot Puma 560 1 2 1 0 Figura 2.8: Grafici matching modelli robot Puma 560. Capitolo 2 Modelli e simulazione real-time e non real-time della dinamica di un robot66 Capitolo 3 Generazione e allineamento dei segnali encoder 3.1 Introduzione Il simulatore real-time proposto in questa tesi è costituito, come già anticipato nel capitolo introduttivo, da due unità di calcolo (o sistemi) unite tra loro in modo complementare. In particolare, l’unità lenta (Sistema Veloce in figura 1 a pagina 8) si occupa della simulazione delle equazioni matematiche (anche molto complesse e che derivano dalla formulazione di Newton-Eulero, si veda il §1.4) con tempi di calcolo più lenti ed insufficienti per generare contemporaneamente anche i segnali encoder. L’unità veloce (Sistema Veloce in figura 1 a pagina 8) invece riceve i segnali analogici corrispondenti alle velocità di giunto del robot, prodotte dall’unità lenta, e genera sulla base di questi segnali i segnali encoder in fase e quadratura. Quest’ultima unità può essere più veloce perché non soggetta ai calcoli onerosi legati alla simulazione del modello matematico. La problematica fondamentale dietro a questa soluzione con doppia unità è che risulta necessario allineare l’informazione di posizione fornita dall’unità veloce e l’informazione di posizione presente nell’unità lenta, quella cioè che risulta dalla simulazione delle equazioni matematiche. Infatti la trasmissione dei segnali di velocità dall’unità lenta a quella veloce è soggetta a rumore proveniente da 1) interferenze sulla linea di trasmissione, 2) incertezze sul valore di tensione corrispondente allo zero (questo risulta in un offset), 3) incertezze sul guadagno del convertitore D/A nel quale avviene la trasmissione. 67 § 3.1 Introduzione Questi disturbi di varia natura purtroppo si accumulano quando attraverso i circuiti encoder si trasforma la velocità nella corrispondente posizione (che è il suo integrale), dunque una soluzione che non prevede una correzione efficace di questi errori non può risultare affidabile in quanto la simulazione non corrisponde più alle equazioni matematiche che descrivono il moto del sistema. Allo scopo di allineare le suddette informazioni di posizione, è stata progettata e verificata una legge di controllo adattativa, la quale garantisce che anche in presenza dei tre tipi di disturbo sopra citati, l’informazione di posizione ricostruita dai segnali encoder prodotti dall’unità veloce corrisponde esattamente all’informazione di posizione presente all’interno dell’unità lenta, e ricavata sulla base delle equazioni matematiche che descrivono il manipolatore robotico che si sta simulando. In questo capitolo dopo aver spiegato come funziona un encoder ottico e cosa sono i segnali encoder in fase e quadratura, viene illustrato e commentato l’algoritmo di generazione dei segnali encoder implementato all’interno dell’unità veloce del simulatore real-time. Il capitolo si chiude con la spiegazione della legge di controllo adattativa e con i test simulativi eseguiti per verificare il corretto funzionamento e le prestazioni del suddetto algoritmo di generazione dei segnali encoder e della sopra citata legge adattativa. Capitolo 3 Generazione e allineamento dei segnali encoder 68 § 3.2 Generazione dei segnali encoder 3.2 3.2.1 Generazione dei segnali encoder Funzionamento di un encoder incrementale Un encoder incrementale è costituito da un disco ottico che ruota solidamente con il rotore di cui si vuole misurare la velocità (vedi figura 3.1). Sul disco è disposta una serie di riferimenti (fori o tacche), posti su una circonferenza ed equamente intervallati. I fori vengono illuminati da un fascio di luce generata da un LED, posto su un lato del disco, mentre sull’altro lato, è collocato un fotorivelatore (fotodiodo o fototransistor) che ha il compito di fornire un segnale quando viene irradiato dal fascio di luce emesso dal LED. Quando il disco ruota, il trasduttore genera un Figura 3.1: Struttura di un encoder incrementale. segnale a due livelli (in genere ad onda quadra); conteggiando il numero di fronti di salita o di discesa di questo segnale in un prefissato intervallo di tempo (di durata T) si ottiene un numero proporzionale alla velocità di rotazione dell’asse. Indicato con n il numero di riferimenti posti lungo la circonferenza, l’ampiezza del settore angolare percorso nell’intervallo di tempo intercorrente tra il passaggio di un fronte di salita (o di discesa) ed il successivo è pari a 2π/n; pertanto l’errore massimo di misura di posizione che si può avere contando il numero dei fronti transitati durante l’intervallo di campionamento risulta pari a 2π/n e, di conseguenza, l’ampiezza Capitolo 3 Generazione e allineamento dei segnali encoder 69 § 3.2 Generazione dei segnali encoder massima dell’errore sovrapposto alla singola misura di velocità è pari a: eωM ax = 2π nT Si può osservare che è possibile dimezzare l’ampiezza del rumore di misura contando sia i fronti di salita che quelli di discesa del segnale fornito dal trasduttore; inoltre, le eventuali inesattezze nel posizionamento dei riferimenti lungo la circonferenza non alterano in maniera significativa la bontà della misura in quanto questa risulta proporzionale al numero di fronti contati. Tuttavia impiegando il sistema di misura descritto non è possibile apprezzare il verso di rotazione dell’asse; inoltre, eventuali oscillazioni attorno ad una posizione di equilibrio potrebbero fornire una errata misura della velocità. Quando sia necessario rilevare anche il senso di rotazione, oppure avere una accurata misura di velocità anche per piccoli valori di questa, si può eseguire una doppia misura impiegando due LED, due rivelatori e due serie concentriche di riferimenti; questi ultimi devono essere disposti in modo che i riferimenti di una serie siano sfasati, rispetto a quelli dell’altra serie, di una distanza angolare pari ad un quarto di quella relativa a due riferimenti della stessa serie. In tal modo, quando il disco ruota, i due rivelatori generano due forme d’onda, usualmente chiamate canale A e canale B, sfasate tra loro di 90◦ (vedi figura 3.2). A seconda di quale dei due canali fornisca il segnale in anticipo, è possibile stabilire se la rotazione sia oraria o antioraria. L’impiego di due canali presenta anche il vantaggio che, se si contano tutti i fronti di salita e di discesa relativi ad entrambi i canali, si ottiene un numero di fronti al giro np pari a quattro volte il numero n dei riferimenti posti lungo una circonferenza. Di conseguenza, l’ampiezza massima dell’errore sovrapposto alla singola misura di velocità è pari a: eωM ax = 2π 2π π = = np T 4nT 2nT Occorre comunque nella manipolazione dei due segnali, porre attenzione affinché non succeda che eventuali oscillazioni attorno ad una posizione di equilibrio provochino una errata misura della velocità. Infine è disponibile anche un ulteriore Capitolo 3 Generazione e allineamento dei segnali encoder 70 § 3.2 Generazione dei segnali encoder Figura 3.2: Rappresentazione canali A, B e Z. segnale chiamato canale Z o zero, che fornisce una posizione assoluta di zero dell’albero encoder (vedi figura 3.2). Questo segnale si presenta sotto forma di un impulso quadrato con fasatura e larghezza centrata sul canale A. Capitolo 3 Generazione e allineamento dei segnali encoder 71 § 3.2 Generazione dei segnali encoder 3.2.2 Algoritmo per la generazione dei segnali encoder Con riferimento alla figura 3.3, viene ora presentato l’algoritmo adottato per simulare il funzionamento di un encoder incrementale, cioè usato per generare a partire da un segnale analogico di velocità angolare, i due segnali digitali in fase e quadratura (segnali a due livelli, 0 o 1), che coincidono con i canali A e B forniti dall’ encoder reale. Il codice C30 che implementa l’algoritmo all’interno del microcontrollore Microchip dsPIC30F4013 è riportato e commentato in Appendice C. Algoritmo: Figura 3.3: Rappresentazione canali A, B per confronto e generazione segnali encoder. 1. Si sceglie il numero (N) di settori angolari (parti o riferimenti) in cui si vuole suddividere l’angolo giro (2π = 360◦ ); 2. Si pone pari ad R l’ampiezza del settore angolare (ampiezza della zona trasparente o opaca); R= 2π N 3. Si pone ∆t pari al tempo di campionamento tc scelto per le simulazioni realtime; ∆t = tc Capitolo 3 Generazione e allineamento dei segnali encoder 72 § 3.2 Generazione dei segnali encoder 4. Si introduce un guadagno Kout che sarà necessario successivamente per ricostruire correttamente l’ampiezza del segnale analogico acquisito dall’esterno; 5. Si pone u pari all’ampiezza del segnale analogico in ingresso; 6. Si sceglie θ come variabile statica per memorizzare l’informazione angolare di posizione via via ”ricostruita”; 7. Si sceglie yf (fase) come variabile binaria per rappresentare il canale A; 8. Si sceglie yq (quadratura) come variabile binaria per rappresentare il canale B; 9. La variabile θ sarà aggiornata secondo l’equazione: θ = θ + uKout ∆t quindi la posizione angolare attuale è pari alla somma della posizione angolare precedente e dell’incremento di posizione avvenuto nell’intervallo di tempo ∆t, dato dal prodotto della velocità angolare u per il tempo di campionamento ∆t; 10. A questo punto si fa in modo che il valore θ calcolato sia sempre compreso nell’intervallo chiuso [0, 2R], dato che i due canali A e B sono periodici con periodo pari a 2R; if(θ > 2.0 ∗ R) θ = θ − 2.0 ∗ R if(θ < 0.0) θ = θ + 2.0 ∗ R 11. Con riferimento alla figura 3.3, si confronta il valore θ ottenuto precedentemente con il valore assunto dal canale A in funzione del valore della variabile Capitolo 3 Generazione e allineamento dei segnali encoder 73 § 3.2 Generazione dei segnali encoder θ. Se il canale A ha valore alto (cioè pari ad 1) si pone yf pari ad 1, altrimenti yf sarà pari a 0: if(θ <= R) yf = 1.0 else yf = 0.0 12. Allo stesso modo con riferimento alla figura 3.3, si confronta il valore θ ottenuto precedentemente con il valore assunto dal canale B in funzione del valore della variabile θ. Se il canale B ha valore alto (cioè pari ad 1) si pone yq pari ad 1, altrimenti yq sarà pari a 0: if(R/2.0 < θ && θ < (3.0/2.0)R) yq = 0.0 else yq = 1.0 I due segnali yf ed yq cosı̀ generati possono essere inviati ad un contatore encoder. Il numero totale (nAB ) di fronti di salita e di discesa contati su entrambi i canali (A e B) moltiplicato per R/2 fornisce il valore della posizione angolare corrente rilevato dal contatore encoder. Capitolo 3 Generazione e allineamento dei segnali encoder 74 § 3.3 Legge di controllo adattativa 3.3 Legge di controllo adattativa In questo paragrafo viene descritta e dimostrata l’efficacia della legge di controllo adattativa. All’interno delle figure, per semplificare la notazione, non viene indicata la dipendenza dei simboli dalla variabile temporale t. Con riferimento alle figure 3.4, 3.5 e 3.6, la legge di controllo adattativa è riassunta dalla seguente espressione: Vout (t) = v(t) + b(t)qd (t) v(t) = KP e(t) + xc (t) e(t) = q(t) − θ(t) ẋc (t) = KI e(t) ḃ(t) = Ka e(t)qd (t) Figura 3.4: Schema a blocchi della legge di controllo adattativa. dove i vettori q(t) e qd (t) sono rispettivamente la posizione e la velocità generate dal Simulatore Modello (si veda figura 4.1), il vettore θ(t) è la posizione ricostruita dal conteggio dei segnali encoder prodotti dal Sistema Veloce (si veda figura 4.1), Capitolo 3 Generazione e allineamento dei segnali encoder 75 § 3.3 Legge di controllo adattativa Ka , KI e KP sono costanti positive, il vettore e(t) è l’errore di posizione all’istante di tempo t, dato dalla differenza tra q(t) e θ(t), i vettori b(t), x c (t) e v(t) sono variabili interne alla legge adattativa e il vettore Vout (t) è la tensione fornita tramite i convertitori D/A al Sistema Veloce (si veda la connessione ”Segnali di velocità corretti” in figura 4.1). Figura 3.5: Particolare del blocco PI di figura 3.4 e 3.7. Figura 3.6: Particolare del Blocco Adattativo di figura 3.4. Figura 3.7: Struttura dello schema a blocchi per la validazione della legge di controllo adattativa. Capitolo 3 Generazione e allineamento dei segnali encoder 76 § 3.3 Legge di controllo adattativa La soluzione di controllo di tipo adattativo è stata ideata per garantire che l’informazione di posizione θ(t) ricostruita sulla base del conteggio dei segnali encoder prodotti dal microcontrollore coincida con la posizione q(t) prodotta dal simulatore del modello (presente nel Sistema Lento) nonostante l’errore introdotto dall’incertezza sul guadagno e sull’offset intrinseci nei convertitori D/A che inviano i segnali di velocità corretti Vout (t) al microcontrollore. Vale il seguente risultato formale che dimostra l’efficacia della legge adattativa ideata: Teorema: Approssimando la catena convertitore D/A-Microcontrollore-Interfaccia encoder con il blocco blu in figura 3.7, e corrispondente all’equazione vettoriale: θ̇(t) = ā Vout (t) + d¯ dove ā > 0 (abar > 0) e d¯ (dbar) sono costanti incognite dipendenti dall’hardware, valgono le seguenti proprietà: R +∞ 1. e(t) ∈ L2 , cioè 0 |e(t)|2 dt < +∞ 2. se ∃ M > 0 : |qd (t)| < M ∀t, allora lim e(t) = 0 t−→+∞ Dimostrazione: Usando la funzione di Lyapunov: V (t) = e2 (t) + 2 1 ā xc (t) + d¯ + (āb(t) − 1)2 > 0 KI āKa (3.1) si dimostra per sostituzione che: V̇ (t) = −2āKP |e(t)|2 ≤ 0 (3.2) Integrando ambo i membri della (3.2) tra t = 0 e t = +∞ e usando il fatto che la V (t) è limitata per ogni t (infatti dalle (3.1) e (3.2) si ha V (t) > 0 e V̇ (t) ≤ 0), si ottiene il punto 1. Infine, poiché V (t) è limitata, lo sono di conseguenza dalla (3.1) anche e(t), x c (t) e b(t). Dunque se qd (t) è limitata, per sostituzione, saranno limitate v(t) = KP e(t) + xc (t) θ̇(t) = d¯ + v(t) + b(t)qd (t)ā Capitolo 3 Generazione e allineamento dei segnali encoder 77 § 3.3 Legge di controllo adattativa quindi sarà limitata anche ė(t) = q̇d (t) − θ̇(t) e similmente V̈ (t) = −4āKP e(t)ė(t) Da ciò, usando il Lemma di Barbalat, si dimostra che lim V (t) = 0 t−→+∞ e dunque lim e(t) = 0 t−→+∞ come indicato al punto 2. Capitolo 3 Generazione e allineamento dei segnali encoder 78 § 3.4 Test di verifica per l’algoritmo di generazione segnali encoder e la legge di controllo adattativa 3.4 Test di verifica per l’algoritmo di generazione segnali encoder e la legge di controllo adattativa Le figure 3.8 e 3.13 mostrano gli schemi a blocchi con cui sono stati eseguiti i test di verifica che hanno consentito di valutare la validità dell’algoritmo di generazione dei segnali in fase e quadratura di tipo encoder, e l’efficienza dell’azione di controllo prodotta dalla legge adattativa. Lo schema in figura 3.8 (TEST 1) è costituito da: Figura 3.8: Schema a blocchi per i test di verifica dell’algoritmo di generazione segnali encoder e della legge di controllo adattativa. 1. Un blocco Analog Input che consente di importare, all’interno dello schema di simulazione, il segnale analogico esterno, prodotto dal generatore di forme d’onda ed inviato alla scheda di A&D I/O Sensoray 626; Capitolo 3 Generazione e allineamento dei segnali encoder 79 § 3.4 Test di verifica per l’algoritmo di generazione segnali encoder e la legge di controllo adattativa 2. Un blocco Scope, Input, che permette di visualizzare in tempo reale l’andamento del segnale analogico fornito dall’esterno; 3. Quattro blocchi Subsystem, ADAPTIVE LAW, che racchiudono lo schema di controllo adattativo e i blocchi di interfaccia real-time che consentono di comunicare con la scheda di A&D I/O Sensoray 626, cioè di ricevere ed inviare i segnali elettrici da e verso l’ambiente esterno (vedi figure 3.9, 3.10, 3.11 e 3.12). Figura 3.9: Particolare blocco Subsystem, ADAPTIVE LAW, di figura 3.8. Avviata la simulazione il sistema esegue ciclicamente e con tempo di campionamento, tc = 0.01 sec, le seguenti operazioni: 1. Lo schema di simulazione riceve tramite le schede di I/O, il segnale analogico di riferimento; 2. Il segnale analogico di riferimento, qd, viene inviato insieme al suo integrale, q, all’interno dei blocchi ADAPTIVE LAW ; Capitolo 3 Generazione e allineamento dei segnali encoder 80 § 3.4 Test di verifica per l’algoritmo di generazione segnali encoder e la legge di controllo adattativa Figura 3.10: Particolare blocco Subsystem, TO MICROCONTROLLER, di figura 3.9. Figura 3.11: Particolare blocco Subsystem, FROM MICROCONTROLLER, di figura 3.9. Figura 3.12: Particolare blocco Subsystem, ADAPTIVE CONTROL, di figura 3.9. 3. All’interno dei blocchi ADAPTIVE LAW, i blocchi Subsystem, TO MICROCONTROLLER, comunicano con la scheda di I/O che invia al microcontrollore i segnali analogici qdi (in questo caso i = 1, ..., 4); 4. Il microcontrollore (connesso alla scheda di I/O ad esempio tramite le morsettiere a vite con connettore per cavo flat) riceve i segnali analogici qdi . Capitolo 3 Generazione e allineamento dei segnali encoder 81 § 3.4 Test di verifica per l’algoritmo di generazione segnali encoder e la legge di controllo adattativa Per ogni segnale analogico qdi ricevuto, il microcontrollore produce secondo l’algoritmo di generazione dei segnali encoder , i segnali elettrici in fase e quadratura di tipo encoder; 5. I segnali elettrici in fase e quadratura di tipo encoder vengono inviati ai contatori encoder delle schede di I/O Sensoray 626; 6. Viene calcolato l’errore di posizione ei (t) = qi − θi relativo ad ogni blocco ADAPTIVE LAW e con l’azione di controllo prodotta dalla legge adattativa si garantisce che l’andamento temporale del segnale θi ricostruito sulla base del conteggio dei segnali encoder prodotti dal microcontrollore coincida in ogni istante con l’andamento temporale del segnale analogico esterno prodotto dal generatore di forme d’onda. Nello schema in figura 3.13 per eseguire un test (TEST 2) che risultasse più significativo dal punto di vista computazionale e allo stesso tempo preliminare per i successivi test di verifica dell’intero sistema HIL, è stato inserito anche il blocco Subsystem che implementa la dinamica del manipolatore. Durante la simulazione il sistema esegue ciclicamente le stesse operazioni del caso precedente con l’unica differenza che, in questo caso, i segnali qdi e qi rappresentano i segnali di velocità e posizione dei giunti del robot prodotti in real-time dal blocco che implementa la dinamica del manipolatore e che riceve in input i segnali di coppia che vengono applicati ai giunti del robot. Dalle simulazioni risulta la validità dell’algoritmo di generazione dei segnali encoder e l’effettiva efficienza della legge di controllo adattativa. Ciò può essere facilmente verificato osservando le figure 3.15 e 3.16 che riportano i grafici forniti dalle simulazioni riguardo ai suddetti segnali. Capitolo 3 Generazione e allineamento dei segnali encoder 82 § 3.4 Test di verifica per l’algoritmo di generazione segnali encoder e la legge di controllo adattativa Figura 3.13: Schema a blocchi con dinamica robot per i test di verifica dell’algoritmo di generazione segnali encoder e della legge di controllo adattativa. Figura 3.14: Particolare blocco Subsystem, ADAPTIVE LAW, di figura 3.13. Capitolo 3 Generazione e allineamento dei segnali encoder 83 joint 4 [rad] joint 3 [rad] joint 2 [rad] joint 1 [rad] § 3.4 Test di verifica per l’algoritmo di generazione segnali encoder e la legge di controllo adattativa 0 −1 −2 0 5 10 15 20 25 30 0 5 10 15 20 25 30 0 5 10 15 20 25 30 0 5 10 15 time [sec] 20 25 30 0 −1 −2 0 −1 −2 0 −1 −2 Figura 3.15: TEST 1: Andamento temporale del segnale di input (segnale in blu) e dei corrispondenti segnali di posizione ricostruiti dal conteggio dei fronti di salita e di discesa dei segnali encoder in fase e quadratura (segnali in rosso). Capitolo 3 Generazione e allineamento dei segnali encoder 84 § 3.4 Test di verifica per l’algoritmo di generazione segnali encoder e la legge di controllo adattativa joint 1 [rad] 2 1 0 −1 0 5 10 15 20 25 30 0 5 10 15 20 25 30 0 5 10 15 20 25 30 0 5 10 15 time [sec] 20 25 30 joint 2 [rad] 2 1 0 −1 joint 3 [m] 0.5 0 −0.5 joint 4 [rad] 0.5 0 −0.5 Figura 3.16: TEST 2: Andamento temporale delle posizioni angolari ai giunti prodotte dal Simulatore (segnali in blu) e delle posizioni angolari ai giunti ricostruite dal conteggio dei fronti di salita e di discesa dei segnali encoder in fase e quadratura (segnali in rosso). Capitolo 3 Generazione e allineamento dei segnali encoder 85 Capitolo 4 Il simulatore real-time 4.1 Introduzione In questo capitolo viene descritto in modo dettagliato il simulatore real-time e vengono riportati i risultati forniti dai test simulativi. Lo studio dei grafici forniti dagli schemi di simulazione è molto importante perché costituisce una verifica diretta della validità e delle prestazioni dell’intero sistema Hardware-in-the-loop (HIL) ideato e realizzato. In particolare per prima cosa viene descritto il simulatore real-time sotto il suo duplice aspetto, hardware e software. Segue la descrizione dell’hardware di controllo impiegato (vedi blocco Hardware di Controllo Assi e/o Numerico in figura 4.1 e in figura 1 a pagina 8) e del filtro dinamico necessario per fornire al controllore, oltre all’informazione di posizione dei giunti del robot, anche una stima della loro velocità a partire dalla stessa informazione di posizione dei giunti prodotta dal simulatore. Seguono infine le prove sperimentali con cui sono state valutate le prestazioni dell’intero sistema HIL. 4.2 Descrizione del prototipo del sistema HIL Il prototipo di simulatore real-time realizzato è costituito dai seguenti componenti: Componenti hardware: • 1 Personal Computer dotato di processore AMD Sempron 3000+ 1.80 GHz, 1 GB di RAM ; 86 § 4.2 Descrizione del prototipo del sistema HIL Figura 4.1: Schema a blocchi del prototipo di sistema HIL e logica di funzionamento. • 2 schede PCI di I/O Sensoray modello 626, dotate di ingressi e uscite analogiche/digitali e di contatori di tipo encoder. La scheda tecnica e la descrizione della procedura di installazione e del funzionamento della scheda Sensoray 626 sono disponibili in Appendice A; • 1 scheda dsPICDEM2 di programmazione e prototipazione per microcontrollori dsPIC30Fxxx prodotta dalla MICROCHIP. La documentazione della scheda dsPICDEM2 è disponibile in Appendice B; • 1 microcontrollore MICROCHIP dsPIC30F4013. La scheda tecnica di questo microcontrollore è disponibile in Appendice B; • Interfacce di cablaggio Phoenix Contact VARIOFACE con connettore per cavo flat 26, 40 e 50 poli (si veda figura 4.2). I moduli VARIOFACE soCapitolo 4 Il simulatore real-time 87 § 4.2 Descrizione del prototipo del sistema HIL no dispositivi di interfaccia standardizzati che semplificano e ottimizzano la progettazione, il montaggio e la messa in servizio su tutto il cablaggio dell’impianto. I vantaggi per l’utente sono: 1) ottimo adattamento dell’interfaccia alla varie esigenze di cablaggio e collegamento; 2) facile e rapido montaggio su tutte le guide di supporto EN; 3) facile progettazione; 4) rapida messa in funzione nonché eliminazione degli errori tramite chiaro collegamento e chiara siglatura di moduli e poli; 5) maggiore disponibilità di spazio grazie alle caratteristiche dei moduli compatti. Figura 4.2: Interfaccia di cablaggio Phoenix Contact VARIOFACE. Componenti software: • Sistema operativo Microsoft Windows XP; • Mathworks Matlab e toolbox Matlab: ”Simulink”, ”Real Time Workshop” e ”Real Time Windows Target”; • Microchip MPLAB e Microchip C30. La realizzazione del prototipo è caratterizzata dalle seguenti fasi essenziali: 1. L’interconnessione di due sistemi per creare un simulatore HIL a basso costo capace di riprodurre in tempo reale il comportamento dinamico di un manipo- Capitolo 4 Il simulatore real-time 88 § 4.2 Descrizione del prototipo del sistema HIL latore robotico che si interfaccia con l’ambiente esterno (cioè con l’hardware di controllo) tramite segnali encoder: • Il sistema lento costituito da un Personal Computer (PC), software di simulazione e schede PCI per acquisizione e generazione di segnali analogici e digitali, dotate di contatori di tipo encoder. Questo sistema simula il comportamento dinamico del robot virtuale (valutandone il modello matematico) e produce, dal calcolo della dinamica diretta effettuato in base ai valori dei segnali analogici di coppia/forza che riceve dall’azione di controllo esterna (questi segnali di coppia/forza sono prodotti dall’hardware di controllo), i segnali analogici di posizione e velocità dei giunti del robot . Questo sistema può tuttavia produrre ed inviare all’hardware di controllo questi segnali analogici di velocità solo a bassa frequenza (segnali lenti) e non è in grado di produrre direttamente i segnali elettrici in fase e quadratura di tipo encoder corrispondenti al movimento simulato del robot, in quanto le frequenze richieste da questi ultimi sono troppo alte rispetto alle frequenze imposte dal calcolo in real-time della dinamica del robot; • Il sistema veloce costituito da un microcontrollore e dalle morsettiere industriali che consentono il collegamento con il primo sistema e con l’hardware di controllo esterno. Questo secondo sistema consente di sopperire all’incapacità del primo sistema, in quanto è in grado di produrre i segnali elettrici in fase e quadratura di tipo encoder ad alta frequenza sulla base dell’ingresso analogico rappresentante le velocità dei giunti e proveniente dal primo sistema tramite le schede di acquisizione. 2. Scrittura di codice simulativo in linguaggio C, disponibile in Appendice D, che consente di realizzare i blocchi Simulink C-MEX S-Function, adatti al funzionamento real-time, che implementano nel calcolatore la dinamica del manipolatore robotico scelto, calcolando in real-time i segnali di posizione e velocità dei giunti del robot. Capitolo 4 Il simulatore real-time 89 § 4.2 Descrizione del prototipo del sistema HIL 3. Scrittura dell’algoritmo e del codice in linguaggio Microchip C30, disponibile in Appendice C, che consente di programmare il microcontrollore Microchip dsPIC30F4013 per produrre i segnali elettrici in fase e quadratura di tipo encoder sulla base dell’ingresso analogico rappresentante la velocità ai giunti e proveniente dal PC. 4. Realizzazione di una legge di controllo adattativa che garantisce che l’informazione di posizione ricostruita sulla base del conteggio dei segnali encoder in fase e quadratura coincida con la posizione prodotta dal simulatore real-time. La legge di controllo adattativa è dettagliatamente descritta nel capitolo 3.1 (§3.3). 5. Realizzazione di uno schema di connessione ottimale dell’hardware impiegato per garantire prestazioni e qualità dei segnali trasmessi; 6. Realizzazione degli schemi a blocchi Simulink per la fase di progettazione e prototipazione. Figura 4.3: Struttura del prototipo del sistema HIL. Capitolo 4 Il simulatore real-time 90 § 4.2 Descrizione del prototipo del sistema HIL Figura 4.4: Schema a blocchi Simulink del simulatore nel Sistema Lento. Per quanto riguarda la dinamica dei robot, le equazioni dinamiche del moto del manipolatore vengono ricavate sulla base della formulazione di Newton-Eulero, che consente di dedurre un modello di tipo ricorsivo molto efficiente da un punto di vista computazionale. Tale modello è particolarmente adatto ad essere implementato al calcolatore, in quanto sfrutta la natura seriale a catena aperta tipica del manipolatore. La piattaforma software di simulazione real-time gestisce la compilazione real-time compliant del codice C, scritto per implementare il suddetto algoritmo di Newton-Eulero, e fornisce i driver di comunicazione tra le schede PCI di I/O A/D, consentendo di inviare verso l’esterno i segnali analogici di velocità ai giunti. Durante il normale funzionamento (vedi figura 4.3), dopo aver caricato i parametri del robot desiderato, ed avviato la simulazione con tempo di campionamento (tc) fissato dall’utente, il simulatore real-time genera degli opportuni segnali digitali che determinano i fine corsa dei motori dei giunti del robot e la posizione di home, e quindi di reset del conteggio, di ciascun contatore encoder. Raggiunta la posizione di home il sistema HIL esegue ciclicamente e con lo stesso tempo di campionamento (tc) fissato dall’utente le seguenti operazioni: 1. Il simulatore real-time riceve tramite le schede di I/O A/D, i segnali analogici dei riferimenti di coppia per i giunti del robot, prodotti dall’hardware di controllo. Capitolo 4 Il simulatore real-time 91 § 4.2 Descrizione del prototipo del sistema HIL 2. Il simulatore real-time calcola, usando l’algoritmo basato sulla formulazione di Newton-Eulero, la dinamica diretta del manipolatore e aggiorna i segnali di posizione e velocità ai giunti del robot, quelli di home e di fine corsa (figura 4.4). 3. La schede di I/O A/D inviano al microcontrollore i segnali analogici rappresentanti la velocità ai giunti appena calcolata (punto 2). 4. Il microcontrollore (connesso alla scheda di I/O A/D tramite le morsettiere a vite con connettore per cavo flat) riceve i segnali analogici di velocità. Per ogni segnale analogico di velocità ricevuto, il microcontrollore produce secondo l’algoritmo di generazione dei segnali encoder (vedi sotto), i segnali elettrici in fase e quadratura di tipo encoder. 5. I segnali elettrici in fase e quadratura di tipo encoder vengono inviati allo stesso tempo sia ai contatori encoder della scheda (o hardware equivalente di controllo assi), che ai contatori encoder delle schede di I/O A/D montate sul computer su cui è installato il simulatore. 6. I contatori encoder delle suddette schede di I/O A/D e di controllo assi (punto 5), ricostruiscono l’informazione di posizione corrente dei giunti del robot, contando i fronti di salita e di discesa dei corrispondenti segnali encoder in fase e quadratura prodotti dal microcontrollore. 7. Viene calcolato l’errore di posizione ai giunti per ciascun giunto del robot, dato dalla differenza tra la posizione prodotta dal simulatore real-time all’interno del PC e la posizione ricostruita sulla base del conteggio dei segnali encoder prodotti dal microcontrollore e con l’azione di controllo prodotta dalla legge adattativa (vedi sotto) si garantisce che le due informazioni di posizione, dopo un breve transitorio, coincidano in ogni istante successivo. Capitolo 4 Il simulatore real-time 92 § 4.3 Descrizione del sistema di controllo 4.3 Descrizione del sistema di controllo Il controllore (blocco Hardware di Controllo Assi e/o Numerico in figura 4.1), impiegato all’interno del prototipo del sistema HIL, è realizzato mediante un Computer (PC) dotato di: 1) sistema operativo Microsoft Windows XP; 2) software di simulazione Mathworks Matlab con toolbox “Simulink”, “Real Time Workshop” e “Real Time Windows Target”; 3) scheda di interfaccia con l’ambiente esterno Sensoray 626. La legge di controllo implementata all’interno di questo sistema riproduce l’azione di controllo che verrebbe fornita da un tipico regolatore proporzionale-integrale-derivativo (PID). La legge di controllo in questione è riassunta dalla seguente espressione: u = η(q, q̇, t) = KP (qd (t) − q(t)) + KI Z t (qd (τ ) − q(τ )) dτ + KD (q̇d (t) − q̇(t)) 0 (4.1) dove i vettori q(t) e q̇(t) sono rispettivamente la posizione e la velocità dei giunti del robot generate dal blocco Simulatore Modello (si veda figura 4.1), qd (t) e q̇d (t) sono rispettivamente i vettori che denotano la posizione e la velocità dei giunti corrispondenti alla traiettoria desiderata che si vuole far seguire al manipolatore robotico. In presenza dei soli sensori encoder, la legge di controllo (4.1) non può essere direttamente implementata se non si determina una stima della velocità dei giunti del robot. Per risolvere questo problema è stato implementato all’interno dello schema simulink che permette di simulare in tempo reale l’azione di controllo, un filtro dinamico che consente di ottenere una stima della velocità a partire dall’informazione di posizione ricostruita dal conteggio dei segnali encoder prodotti dal Sistema Veloce di figura 4.1. Il filtro dinamico è dettagliatamente descritto nel paragrafo successivo (§4.4). 4.4 Filtro dinamico per la stima della velocità dei giunti del robot Non disponendo di una misura diretta della velocità dei giunti del robot, è necessario trarne una stima in base alla misura di posizione dei giunti del manipolatore, Capitolo 4 Il simulatore real-time 93 § 4.4 Filtro dinamico per la stima della velocità dei giunti del robot fornita dal conteggio dei fronti di salita e discesa dei segnali in fase e quadratura di tipo encoder. La stima delle suddette velocità è necessaria per implementare correttamente la legge di controllo PID prodotta dall’hardware di controllo all’interno del simulatore HIL. Si definisce uno stimatore delle variabili di posizione, x1 = q, e velocità, x2 = q̇. Tale stimatore fornisce una stima delle variabili x1 e x2 appena definite, la quale permette di implementare la legge di controllo PID, almeno a livello approssimato. Le equazioni dello stimatore sono: 1 x̂˙ 1 = x̂2 + Hp (y − x̂1 ) 1 x̂˙ 2 = 2 Hv (y − x̂1 ) (4.2) dove x̂1 , x̂2 denotano, rispettivamente, la stima dinamica delle coordinate di posizione e di velocità, y è il valore di posizione misurato all’istante attuale (ovvero il segnale proveniente dall’encoder). Le matrici di costanti Hp = diag(hpi ) e Hv = diag(hvi ) devono essere scelte in modo che le soluzioni del polinomio caratteristico pi (λ) = 2 + λhpi + hvi , i = 1, ..., N (N = numero gradi di libertà del robot) si trovino nella metà sinistra del piano complesso, e un piccolo valore che rappresenta un parametro di progetto. Il sistema (4.2) costituisce un filtro dinamico che permette di risolvere il suddetto problema. Dato che il processo real-time del sistema sperimentale è discreto e la misura dell’encoder y(t) è solo disponibile negli istanti t = kT (T = tc = tempo di campionamento) è necessario discretizzare il filtro (4.2) in base alla seguente approssimazione: x̂1 (t + T ) ≈ x̂1 (t) + T x̂˙ 1 (t) (4.3) x̂2 (t + T ) ≈ x̂2 (t) + T x̂˙ 2 (t) da cui si ottiene il seguente filtro discretizzato: x̂1 (t + T ) ≈ x̂1 (t) + T x̂2 (t) + T Hp (y(t + T ) − x̂1 (t)) (4.4) T Hv (y(t + T ) − x̂1 (t)) 2 x̂1 (0) = 0, x̂2 (0) = 0 x̂2 (t + T ) ≈ x̂2 (t) + Capitolo 4 Il simulatore real-time 94 § 4.4 Filtro dinamico per la stima della velocità dei giunti del robot Le funzioni cosı̀ ottenute sono quindi una stima dinamica delle grandezze di posizione e velocità dei giunti del robot. La figura 4.5 mostra lo schema di simulazione impiegato per testare l’efficienza del filtro dinamico. Le figure 4.7 e 4.8 mostrano invece i risultati forniti dai test simulativi. Figura 4.5: Schema di simulazione impiegato per testare l’efficienza del filtro dinamico. Figura 4.6: Particolare del blocco FILTRO DINAMICO di figura 4.5. Capitolo 4 Il simulatore real-time 95 § 4.4 Filtro dinamico per la stima della velocità dei giunti del robot INPUT 2 x1(t) 1 0 −1 −2 0 2 4 0 2 4 6 8 10 6 8 10 0.8 0.6 x2(t) 0.4 0.2 0 −0.2 time [sec] Figura 4.7: Segnali di ingresso forniti in input al filtro dinamico. STIMA 2 ^ x1(t) 1 0 −1 −2 0 2 4 0 2 4 6 8 10 6 8 10 0.8 ^ x2(t) 0.6 0.4 0.2 0 −0.2 time [sec] Figura 4.8: Stima dei segnali di input (vedi figura 4.7) prodotta dal filtro dinamico ( = 0.1, Hp = 5, Hv = 6). Capitolo 4 Il simulatore real-time 96 § 4.5 Prove sperimentali 4.5 Prove sperimentali In questo paragrafo vengono descritti gli schemi di simulazione e i corrispondenti risultati forniti dai test di verifica eseguiti per testare il funzionamento e le prestazioni dell’intero sistema HIL. Le figure 4.9 e 4.10 mostrano rispettivamente: • Lo schema a blocchi che consente di simulare il processo da controllare, cioè il manipolatore robotico; • Lo schema a blocchi che genera l’azione di controllo. Figura 4.9: Schema a blocchi del processo controllato. Lo schema a blocchi con cui si riproduce il processo controllato è costituito da: 1. Un blocco Subsystem INPUT TORQUE che consente di interfacciare lo schema di simulazione con l’ambiente esterno e ricevere i segnali analogici di coppia, generati dall’ hardware di controllo, che vengono applicati ai giunti del robot; 2. Un blocco Subsystem ROBOT REAL-TIME che consente di simulare la dinamica del manipolatore robotico scelto; 3. Un blocco Subsystem ADAPTIVE LAW, vedi §3.4; Capitolo 4 Il simulatore real-time 97 § 4.5 Prove sperimentali 4. Due blocchi Scope, qdsim e qsim, che consentono di visualizzare in tempo reale l’andamento dei segnali di velocità e posizione dei giunti del robot prodotti dal simulatore; Figura 4.10: Schema a blocchi del controllore. Lo schema a blocchi che genera l’azione di controllo (vedi figura 4.10) è invece costituito da: 1. Un blocco Subsystem, TRAJECTORY DEMAND, che fornisce gli andamenti desiderati di posizione, velocità ed accelerazione per i giunti del robot sotto controllo; 2. Un blocco Subsystem, Encoder Input, all’interno del quale viene ricostruita, dal conteggio dei fronti di salita e di discesa dei segnali encoder in fase e quadratura prodotti dal microcontrollore, l’informazione corrente di posizione dei giunti del robot; 3. Un blocco Subsystem, DYNAMIC FILTER, (vedi appendice 4.4) che produce una stima della velocità dei giunti del robot a partire dall’informazione di posizione (degli stessi giunti) ricostruita come nel punto 2; Capitolo 4 Il simulatore real-time 98 § 4.5 Prove sperimentali 4. Un blocco Subsystem, PID CONTROLLER, che implementa l’azione di controllo proporzionale, integrale e derivativa; Figura 4.11: Particolare del blocco PID Controller di figura 4.10. Figura 4.12: Particolare del blocco PID Controller 1 di figura 4.11. 5. Un blocco Subsystem, ROBOT RT torque feedforward che produce, in base alla conoscenza della dinamica del manipolatore e degli andamenti desiderati di posizione, velocità ed accelerazione per i giunti del robot, l’azione di Capitolo 4 Il simulatore real-time 99 § 4.5 Prove sperimentali controllo di tipo feedforward; cioè il blocco calcola la dinamica inversa del robot; 6. Un blocco Subsystem, TO SIMULATOR che invia al simulatore i segnali di coppia prodotti dall’azione di controllo combinata PID+feedforward; 7. Due blocchi Scope, qdˆ e TORQUE, che consentono di visualizzare in tempo reale, rispettivamente, l’andamento della stima dei segnali di velocità e le coppie, prodotte dall’azione di controllo, che vengono inviate al simulatore; Lo schema di lavoro ed il funzionamento dell’intero sistema HIL sono stati già descritti in precedenza, quindi si rimanda il lettore al §4.2, e si chiude questa sezione riportando i parametri di simulazione ed illustrando i grafici forniti dalle prove simulative. 4.5.1 Parametri di simulazione • Parametri cinematici e dinamici del modello virtuale del manipolatore robotico SCARA (file Matlab robot scara.m): clear L % Cancella qualsiasi oggetto link L già esistente %L(i) = link([alpha A theta D R=0/P=1], ’standard’); L{1} = link([0, 0.4, 0, 0, 0], ’standard’); % riga 1 L{2} = link([pi, 0.3, 0, 0, 0], ’standard’);% riga 2 L{3} = link([0, 0, 0, 0, 1], ’standard’); % riga 3 L{4} = link([0, 0, 0, 0, 0], ’standard’); % riga 4 L{1}.m L{2}.m L{3}.m L{4}.m = = = = 15; % 8; % 6; % 0.5;% L{1}.r L{2}.r L{3}.r L{4}.r = = = = [ [ [ [ massa massa massa massa del del del del link link link link 0 0 0.25 ]; % -0.2 0 0]; % -0.15 0 0]; % 0 0 -0.25];% D-H D-H D-H D-H 1 2 3 4 offset offset offset offset %L{i} = [Ixx,Iyy,Izz,Ixy,Iyz,Ixz]; L{1}.I = [0.2, 0.2, 0.04, 0, 0, 0]; L{2}.I = [0.1, 0.02, 0.2, 0, 0, 0]; L{3}.I = [0.05, 0.01, 0.05, 0, 0, 0]; Capitolo 4 Il simulatore real-time matrice matrice matrice matrice COG COG COG COG link link link link 1 2 3 4 %matrice d’inerzia link 1 %matrice d’inerzia link 2 %matrice d’inerzia link 3 100 § 4.5 Prove sperimentali L{4}.I = [0.01, 0.01, 0.0005, 0, 0, 0];%matrice d’inerzia link 4 % Inerzia (Jm) e rapporto di riduzione (G) del motore di % attuazione di ciascun giunto del manipolatore L{1}.Jm = 200e-6;% L{2}.Jm = 200e-6;% L{3}.Jm = 200e-6;% L{4}.Jm = 33e-6; % L{1}.G L{2}.G L{3}.G L{4}.G = = = = -62.6111;% 107.815; % -53.7063;% 76.0364; % % valori dell’attrito viscoso riferiti al motore % di attuazione del giunto L{1}.B = 1.48e-3; L{2}.B = .817e-3; L{3}.B = 1.38e-3; L{4}.B = 71.2e-6; % % some useful poses % qz = [0 0 0 0]; % zero angles, L shaped pose scara = robot(L); clear L scara.name = ’SCARA’; scara.manuf = ’’; • Parametri controllore e coefficienti filtro dinamico: clear all % cancella tutte le variabili in memoria close all % chiude tutte le finestre attive clc % pulisce lo schermo tsim = 100; % imposta il tempo di simulazione [secondi] tc = 0.01; % imposta il tempo di campionamento [secondi] %parametri per stima qp giunto 1 eps1 = 0.3; Hp1 = 5; Hv1 = 6; %parametri per stima qp giunto 2 eps2 = 0.3; Hp2 = 5; Hv2 = 6; %parametri per stima qp giunto 3 eps3 = 0.3; Capitolo 4 Il simulatore real-time 101 § 4.5 Prove sperimentali Hp3 = 5; Hv3 = 6; %parametri per stima qp giunto 4 eps4 = 0.3; Hp4 = 5; Hv4 = 6; load rif % carica i valori della traiettoria desiderata %PID Ki = Kp = Kd = giunto 1 2.4; 12; 15; %PID Ki = Kp = Kd = giunto 2 2; 11; 13; %PID Ki = Kp = Kd = giunto 3 1; 5; 9; %PID Ki = Kp = Kd = giunto 4 0.5; 0.6; 0.9; 4.5.2 Grafici delle prove simulative In queste prove simulative per semplicità non è stata eseguita la ricerca della posizione di Home ma è stato solo inviato, all’istante iniziale di simulazione, un segnale di reset ai contatori encoder delle schede Sensoray 626 per consentire di allineare a zero l’informazione iniziale di posizione su tutti i contatori. Durante il tempo di simulazione la legge adattativa ha quindi garantito che l’informazione di posizione simulata ai giunti del manipolatore e quella ricostruita sulla base del conteggio dei segnali encoder in fase e quadratura fossero (dopo un breve transitorio) sempre allineate, mentre l’azione di controllo fornita dal regolatore PID ha consentito al manipolatore di inseguire la traiettoria richiesta. La figura 4.13 mostra l’an- Capitolo 4 Il simulatore real-time 102 § 4.5 Prove sperimentali damento temporale delle posizioni angolari desiderate ai giunti del manipolatore robotico SCARA. In particolare al robot viene richiesto di partire dalla configurazione iniziale q(t0 = 0) = [0, 0, 0, 0]T e raggiungere in un tempo pari a tf = 70 sec, la posizione finale q(tf ) = [pi/2, pi/2, 0.5, 0]. La figura 4.14 mostra invece l’andamento temporale delle posizioni angolari ai giunti del robot fornito dalle prove simulative che come si ricorda sono state eseguite in tempo reale, cioè il tempo simulato corrisponde all’effettiva durata temporale della prova sperimentale. Dalla figura 4.14 si può valutare come il robot, dopo aver raggiunto nei primi 30 secondi di simulazione la configurazione iniziale richiesta, insegua effettivamente la traiettoria desiderata. L’unico inconveniente si rileva sul giunto numero 2 e 4, dato che a regime si evidenzia un’oscillazione intorno al valore di posizione angolare richiesto, cioè l’andamento temporale resta di tipo sinusoidale con un ampiezza pari a 0.04 rad. Le cause principali a cui può essere attribuito il suddetto inconveniente sono senz’altro: 1. la precisione BLU (basic length unit) raggiunta nella generazione dei segnali encoder in fase e quadratura che produce un errore di quantizzazione non eliminabile in simulazione: BLU = R 2π 1 π = = = 0.0314 rad. 2 N 2 N 2. ritardi intrinseci delle connessioni dei componenti hardware del sistema HIL. Si tratta di ritardi molto piccoli ma aventi valori comparabili con il tempo di campionamento tc fissato per le prove sperimentali. Per confermare che il problema riscontrato al giunto 2 e 4 possa essere effettivamente dovuto alle sopracitate cause sono state eseguite delle ulteriori prove simulative servendosi dello schema a blocchi simulink riportato in figura 4.15. In questo schema di simulazione sono state ricreate le condizioni reali in cui opera il prototipo HIL realizzato. In particolare sono stati inseriti lo stesso regolatore usato per le prove sperimentali, il filtro dinamico di stima della velocità ai giunti del robot e il medesimo modello real-time implementato all’interno del corrispondente blocco Capitolo 4 Il simulatore real-time 103 § 4.5 Prove sperimentali joint 1 [rad] 2 1 0 0 10 20 30 40 50 60 70 0 10 20 30 40 50 60 70 0 10 20 30 40 50 60 70 0 10 20 30 40 time [sec] 50 60 70 joint 2 [rad] 2 1 0 joint 3 [m] 1 0.5 0 joint 4 [rad] 1 0 −1 Figura 4.13: Andamento temporale delle posizioni angolari desiderate ai giunti del manipolatore robotico SCARA. Simulatore Modello in figura 1 a pagina 8. All’interno dello schema di simulazione di figura 4.15 i blocchi Quantizer e Integer Delay consentono di modellare rispettivamente l’errore di quantizzazione introdotto dal microcontrollore e i ritardi intrinseci dei collegamenti dei componenti hardware del sistema HIL. Le simulazioni hanno confermato che in assenza di errore di quantizzazione e di ritardi il robot insegue senza problemi la traiettoria richiesta. Al contrario in presenza di questi Capitolo 4 Il simulatore real-time 104 § 4.5 Prove sperimentali joint 1 [rad] 2 1 0 −1 0 10 20 30 40 50 60 70 0 10 20 30 40 50 60 70 0 10 20 30 40 50 60 70 0 10 20 30 40 time [sec] 50 60 70 joint 2 [rad] 2 1 0 −1 joint 3 [m] 1 0 joint 4 [rad] −1 1 0 −1 Figura 4.14: Andamento temporale delle posizioni angolari ai giunti del manipolatore robotico SCARA (rosso = traiettoria desiderata). fenomeni le simulazioni evidenziano la presenza del problema riscontrato nelle prove sperimentali. Quanto detto può essere valutato osservando i grafici forniti dalle prove simulative e riportati nelle figure 4.16 e 4.17. Capitolo 4 Il simulatore real-time 105 § 4.5 Prove sperimentali joint 3 [m] joint 2 [rad] joint 1 [rad] Figura 4.15: Schema a blocchi simulink che riproduce le condizioni reali in cui opera il prototipo di sistema HIL. 2 1 0 10 20 30 40 50 60 70 0 10 20 30 40 50 60 70 0 10 20 30 40 50 60 70 0 10 20 30 40 time [sec] 50 60 70 1 0 1 0 −1 joint 4 [rad] 0 2 1 0 −1 Figura 4.16: Andamento temporale delle posizioni angolari ai giunti del manipolatore robotico SCARA fornite dallo schema a blocchi di figura 4.15 in assenza dei blocchi Quantizer e Integer Delay (rosso = traiettoria desiderata). Capitolo 4 Il simulatore real-time 106 § 4.5 Prove sperimentali joint 1 [rad] 2 1 0 0 10 20 30 40 50 60 70 0 10 20 30 40 50 60 70 0 10 20 30 40 50 60 70 0 10 20 30 40 time [sec] 50 60 70 joint 2 [rad] 2 1 0 joint 3 [m] 1 0 joint 4 [rad] −1 1 0 −1 Figura 4.17: Andamento temporale delle posizioni angolari ai giunti del manipolatore robotico SCARA fornite dallo schema a blocchi di figura 4.15 in presenza dei blocchi Quantizer e Integer Delay (Quantization interval = BLU = 0.03141, Delay = 0.08 sec, rosso = traiettoria desiderata). Capitolo 4 Il simulatore real-time 107 Capitolo 5 Conclusioni In questo lavoro di tesi è stato realizzato un simulatore real-time di manipolatori robotici con sensori interfacciati tramite segnali encoder, un sistema che rappresenta una soluzione di tipo Hardware-in-the-loop (HIL). Gli elementi essenziali che caratterizzano questa soluzione HIL sono: 1. L’interconnessione di due sistemi, uno lento ed uno veloce, per creare un simulatore HIL a basso costo capace di riprodurre in tempo reale il comportamento dinamico di un manipolatore robotico che si interfaccia con l’ambiente esterno (cioè con l’hardware di controllo) tramite segnali encoder; 2. Il codice programma scritto in linguaggio C che consente di realizzare i blocchi Simulink C-MEX S-Function, adatti al funzionamento real-time, che implementano nel calcolatore la dinamica del manipolatore robotico scelto, calcolando in real-time i segnali di posizione e velocità dei giunti del robot; 3. L’algoritmo e il relativo codice in linguaggio Microchip C30 che consente di programmare il microcontrollore Microchip dsPIC30F4013 per produrre i segnali elettrici in fase e quadratura di tipo encoder sulla base dell’ingresso analogico rappresentante la velocità ai giunti e proveniente dal PC; 4. La legge di controllo adattativa che garantisce in ogni istante di simulazione che l’informazione di posizione ricostruita sulla base del conteggio dei segnali encoder in fase e quadratura coincida con la posizione prodotta dal simulatore real-time; 108 5. Lo schema di connessione ottimale dell’hardware impiegato per garantire prestazioni e qualità dei segnali trasmessi; 6. Gli schemi a blocchi Simulink impiegati per la fase di progettazione e prototipazione. Questo simulatore fornisce all’utente il vantaggio di anticipare le verifiche sulle funzionalità e prestazioni di schede di controllo assi (o hardware di controllo equivalente), già nella fase di progettazione e prototipazione, senza attendere la disponibilità del prodotto finale a cui sono destinate (es. robot o macchine utensili a controllo numerico). Infatti, i componenti hardware installati rispondono ai segnali simulati come se stessero operando in un ambiente reale, poiché non sono in grado di distinguere segnali provenienti da un ambiente fisico da quelli prodotti da modelli software. Gli ulteriori vantaggi che derivano da questa soluzione HIL sono 1) di risparmio economico, in quanto i sistemi qui simulati sono molto costosi; 2) di sicurezza, in quanto i sistemi simulati possono a volte essere pericolosi per l’uomo in caso di malfunzionamento durante la fase di sviluppo del software per le sopraccitate schede controllo assi; 3) di flessibilità ed efficienza, in quanto il sistema sviluppato permette di effettuare svariate prove su sistemi di tipo diverso semplicemente aggiornando il software piuttosto che dovendo trasportare, installare e collegare dispositivi meccanici a volte pesanti e ingombranti. Uno degli obiettivi futuri sarà quello di impiegare all’interno del sistema HIL, come hardware di controllo assi e/o numerico (si veda la figura 1 a pagina 8), la scheda per il controllo assi GALIL DMC1860. In questo modo il sistema HIL consentirà di valutare le prestazioni della scheda e la validità e l’efficienza di un nuovo driver di comunicazione realizzato per installare la scheda all’interno di un computer (PC) dotato di sistema operativo LINUX. Un ulteriore sviluppo futuro sarà quello di implentare all’interno del banco prova HIL le funzionalità aggiuntive prodotte dai blocchi indicati in figura 1 a pagina 8 come “Video” e “Monitoraggio Robot”, che consentiranno all’utente di 1) Visualizzare a schermo e in tempo reale una animazione del movimento eseguito dal modello simulato; 2) trasmissione remota Capitolo 5 Conclusioni 109 di dati; 3) verifica di proprietà di compatibilità della configurazione dal punto di vista della sicurezza, dell’interazione con altri dispositivi meccanici, di valutazione di parametri di prestazione, etc. Capitolo 5 Conclusioni 110 Appendice A La scheda Sensoray 626 Questa sezione costituisce una breve guida introduttiva all’installazione e all’uso della scheda PCI multifunzione Sensoray 626, impiegata per interfacciare il simulatore real-time con l’ambiente esterno, cioè per connettere il computer (su cui è installato il simulatore) con il microcontrollore e con la scheda che fornisce l’azione di controllo. In particolare verrà spiegato come installare ed usare le funzionalità della scheda con i toolbox Simulink, Real Time Workshop e Real Time Windows Target del MATLAB (R 2006b). Verranno inoltre presentati i blocchi Simulink Real-Time che consentono alla scheda Sensoray di acquisire e restituire i segnali analogici e digitali. Infine per quanto riguarda una spiegazione dettagliata delle caratteristiche tecniche della Sensoray 626, si rimanda il lettore al manuale d’uso della scheda, disponibile on-line all’indirizzo internet www.sensoray.com. 111 § A.1 Caratteristiche tecniche A.1 Caratteristiche tecniche Figura A.1: Scheda Sensoray 626. La scheda Sensoray modello 626 è una scheda PCI multifunzione che consente l’acquisizione e la generazione di segnali analogici e digitali. La scheda include: • 48 canali per ingressi/uscite digitali (connettori di tipo industriale, 2 cavi flat da 50 poli); • 20 di questi canali digitali sono provvisti di rilevazione di fronte (salita o discesa) e capacità di interrupt; • 6 ingressi encoder (connettori encoder di tipo industriale, 2 cavi flat da 26 poli); • Watchdog timer, con vari periodi di reset disponibili, per il reset del bus PCI; • Controllo della carica della batteria Ni-Cad di backup; • 16 ingressi analogici differenziali (14 bit di risoluzione per canale); • 4 uscite analogiche (13 bit di risoluzione per canale); Appendice A La scheda Sensoray 626 112 § A.1 Caratteristiche tecniche • Connettore degli ingressi/uscite analogici di tipo industriale (cavo flat da 50 poli). Figura A.2: Scheda Sensoray 626 Board Layout. Nelle tabelle A.1, A.2, A.3 e A.5, inserite nel §A.5 di questa sezione, sono riportate le piedinature dei connettori flat della scheda Sensoray 626 con la corrispondente funzione svolta da ciascun pin del connettore considerato. Appendice A La scheda Sensoray 626 113 § A.2 Installazione scheda Sensoray 626 A.2 Installazione scheda Sensoray 626 Per installare la scheda Sensoray 626 su di un computer provvisto di sistema operativo Microsoft Windows (98-ME-2000-XP) è necessario: 1. Copiare il file S626.DLL nella directory W indows \ System; 2. Copiare il file SXDRV 98.SY S nella directory W indows\System32\drivers (nel caso in cui si stia installando una seconda scheda Sensoray sullo stesso computer, verificare che nel file system ci sia una sola voce relativa al file SXDRV 98.SY S); 3. Spegnere il computer; 4. Installare (alloggiare) la scheda Sensoray 626 nell’apposito slot PCI; 5. Accendere il computer ed avviare Windows; 6. All’avvio di Windows attendere che si apra la finestra di dialogo: Rilevato nuovo hardware (altrimenti selezionare Installazione nuovo hardware). Inserire quindi il percorso specifico (installazione per utenti esperti) dove individuare il file SX.INF ; 7. Riavviare il computer. I file a cui si fa riferimento nella procedura di installazione sono disponibili on-line all’indirizzo web www.sensoray.com, nella cartella sdk626. Appendice A La scheda Sensoray 626 114 § A.3 Matlab e Sensoray 626 A.3 Matlab e Sensoray 626 In questa sezione verranno introdotti gli schemi Simulink ed i blocchi Real Time Windows Target che consentono alla scheda di interfacciarsi con l’ambiente esterno, cioè di acquisire e generare dei segnali analogici e digitali. A.3.1 Libreria Real Time Windows Target I blocchi real-time di I/O sono contenuti all’interno della libreria Real Time Windows Target. Per visualizzarla è sufficiente digitare, nella finestra di comando (Command Window ) del MATLAB, il comando rtwinlib. Il risultato è quello che si vede nella figura A.3, dove vengono mostrati tutti i blocchi disponibili: Figura A.3: Libreria Real Time Windows Target. Appendice A La scheda Sensoray 626 115 § A.4 Installazione di una scheda di I/O A.4 Installazione di una scheda di I/O Per installare e configurare una scheda di I/O all’interno di uno schema a blocchi Simulink sono necessari i seguenti step: 1. Installare i driver della scheda forniti dal produttore per il sistema operativo in uso sul computer; 2. Creare un nuovo modello Simulink. Selezionare e trascinare il blocco realtime di interesse, dalla libreria rtwinlib; 3. Fare doppio click sul blocco real-time per aprire la finestra di dialogo (figura A.4) che consente di installare la scheda di I/O e di configurarla per la particolare funzione real-time richiesta (I/O analogico, I/O digitale, contatore encoder, ecc); Figura A.4: Finestra di dialogo per l’installazione e la configurazione della scheda I/O. Appendice A La scheda Sensoray 626 116 § A.4 Installazione di una scheda di I/O 4. Cliccare su Install new board e selezionare la scheda di I/O desiderata, nel nostro caso Sensoray −→ M odel626. Compare una finestra (figura A.5) in cui inserire l’indirizzo della scheda PCI (digitare valori tra 1-9, o tra af, oppure tra A-F). Si può anche scegliere Auto detect, nel caso in cui sul computer sia montata una sola scheda di I/O. Infine cliccare test; Figura A.5: Finestra di dialogo per la configurazione della scheda I/O Sensoray 626. 5. Se la scheda viene individuata correttamente dal Real Time Windows Target compare la finestra mostrata in figura A.6; Figura A.6: Finestra di dialogo che conferma la connessione e l’installazione della scheda I/O con il Real Time Windows Target. 6. Cliccare su Ok per tornare al menù di installazione della scheda (figura A.4). Per terminare l’installazione cliccare Apply e poi Ok. Appendice A La scheda Sensoray 626 117 § A.4 Installazione di una scheda di I/O A.4.1 Creazione di un’ applicazione Simulink Real-Time Figura A.7: Scheda Simulink per l’acquisizione di un segnale analogico. Con riferimento alla figura A.7 si analizza lo schema ed i parametri di configurazione, che consentono di creare un modello Simulink Real-Time, per l’acquisizione di un segnale analogico esterno. Lo schema a blocchi di figura A.7 è costituito da: • Un blocco Analog Input, disponibile nella libreria Real Time Windows Target; • Un blocco Scope, disponibile nella libreria Sink del Simulink. Per poter acquisire e memorizzare un segnale analogico con il suddetto schema è necessario: 1. Selezionare Simulation −→ Conf iguration P arameters nel menù dello schema Simulink; Figura A.8: Snapshot del menù Simulation. Appendice A La scheda Sensoray 626 118 § A.4 Installazione di una scheda di I/O 2. Nella finestra Configuration Parameters (figura A.9) nel menù Select selezionare Solver. Impostare in Solver options: • Type: Fixed-step; • Solver: ode1 (Euler); • Fixed-step size: tempo di campionamento desiderato (es. 0.01 s); • Periodic sample time constraint: Unconstrained. Sempre nel menù Select della finestra Configuration Parameters, selezionare Real-Time Workshop. Impostare in Target selection: • System target file: rtwin.tlc; • Language: C. Al termine cliccare su Apply e poi Ok ; Figura A.9: Finestra Configuration Parameters. Appendice A La scheda Sensoray 626 119 § A.4 Installazione di una scheda di I/O 3. Fare doppio click sul blocco Analog Input per aprire la finestra di configurazione del blocco di I/O (figura A.4). Selezionare la scheda di acquisizione desiderata. Nel menù Parameters impostare: • tc: pari al valore scelto per Fixed-step size; • Input channels: il numero corrispondente al canale di acquisizione scelto (es. scheda Sensoray, Input channels: 1, pin 3 e 4 Tabella A.1) • Input range: -10 to 10 V, oppure -5 to 5 V; • Block output signal: Volts. 4. Fare doppio click sul blocco Scope. Selezionare dal menù la voce Parameters. Nella finestra Parameters cliccare su Data history. Togliere il segno di spunta da Limit data points to last:. Attivare la casella Save data to workspace e scegliere un nome ed un formato per i dati che dovranno essere salvati. 5. Dal menù principale dello schema Simulink selezionare Tools −→ External Mode Control Panel.... Cliccare sulla casella Signal & Triggering.... Digitare nel campo Duration il numero di campioni, compatibili con il tempo di simulazione (tsim) e di campionamento (tc) scelti, che verranno memorizzati dal blocco Scope; 6. Verificare che la directory di lavoro del Matlab sia la stessa in cui è memorizzato lo schema Simulink su cui si sta lavorando; 7. Dal menù principale dello schema Simulink selezionare Tools −→ Real-Time Workshop −→ Build Model ; 8. Nel menù di simulazione selezionare External (vedi figura A.10); 9. Impostare il tempo di simulazione desiderato; 10. Connettere l’hardware esterno con la scheda di I/O; 11. Dal menù principale dello schema Simulink selezionare Simulation −→ Connect To Target; Appendice A La scheda Sensoray 626 120 § A.4 Installazione di una scheda di I/O Figura A.10: Snapshot del menù dello schema Simulink che consente di selezionare la modalità di simulazione. 12. Premere il tasto Play per avviare la simulazione ed acquisire il segnale analogico. La figura A.11 mostra il risultato dell’acquisizione di un onda sinusoidale generata da un generatore di forme d’onda e avente: • Ampiezza: A = 4 Vp-p; • Frequenza: 1 Hz. 2.5 2 1.5 1 0.5 A [V] 0 −0.5 −1 −1.5 −2 −2.5 0 2 4 6 8 10 t [s] Figura A.11: Finestra che mostra il risultato dell’acquisizione (real-time) di un segnale analogico. Appendice A La scheda Sensoray 626 121 § A.5 Tabelle I/O-pin Sensoray 626 A.5 Tabelle I/O-pin Sensoray 626 J1 50 pin IDC ribbon cable connector Analog inputs & outputs Pin Funzione Funzione MATLAB Pin Funzione 1 Shield 2 Ground 3 -AD0 -Analog Input1 4 +AD0 5 -AD1 -Analog Input2 6 +AD1 7 -AD2 -Analog Input3 8 +AD2 9 -AD3 -Analog Input4 10 +AD3 11 -AD4 -Analog Input5 12 +AD4 13 -AD5 -Analog Input6 14 +AD5 15 -AD6 -Analog Input7 16 +AD6 17 -AD7 -Analog Input8 18 +AD7 19 Ground Ground 20 Ground 21 -AD8 -Analog Input9 22 +AD8 23 -AD9 -Analog Input10 24 +AD9 25 -AD10 -Analog Input11 26 +AD10 27 -AD11 -Analog Input12 28 +AD11 29 -AD12 -Analog Input13 30 +AD12 31 -AD13 -Analog Input14 32 +AD13 33 -AD14 -Analog Input15 34 +AD14 35 -AD15 -Analog Input16 36 +AD815 37 Ground Ground 38 Ground 39 Ground Ground 40 Ground 41 Sense1 42 DAC0 43 Sense2 44 DAC1 45 Sense3 46 DAC2 47 Sense4 48 DAC3 49 Ground Ground 50 Shield Funzione MATLAB +Analog Input1 +Analog Input2 +Analog Input3 +Analog Input4 +Analog Input5 +Analog Input6 +Analog Input7 +Analog Input8 Ground +Analog Input9 +Analog Input10 +Analog Input11 +Analog Input12 +Analog Input13 +Analog Input14 +Analog Input15 +Analog Input16 Ground Ground Analog Output1 Analog Output2 Analog Output3 Analog Output4 Tabella A.1: Connettore J1 per gli ingressi e le uscite analogiche. Appendice A La scheda Sensoray 626 122 § A.5 Tabelle I/O-pin Sensoray 626 J2 50 pin IDC ribbon connector Digital I/O 1-24 Pin Funzione Funzione MATLAB 1 DIO23 DIO24 3 DIO22 DIO23 5 DIO21 DIO22 7 DIO20 DIO21 9 DIO19 DIO20 11 DIO18 DIO19 13 DIO17 DIO18 15 DIO16 DIO17 17 DIO15 DIO16 19 DIO14 DIO15 21 DIO13 DIO14 23 DIO12 DIO13 25 DIO11 DIO12 27 DIO10 DIO11 29 DIO9 DIO10 31 DIO8 DIO9 33 DIO7 DIO8 35 DIO6 DIO7 37 DIO5 DIO6 39 DIO4 DIO5 41 DIO3 DIO4 43 DIO2 DIO3 45 DIO1 DIO2 47 DIO0 DIO1 49 5V 5V tutti i pin pari Ground Ground Tabella A.2: Connettore digitale J2 (DIOn = Digital I/O numero). Appendice A La scheda Sensoray 626 123 § A.5 Tabelle I/O-pin Sensoray 626 J3 50 pin IDC ribbon connector Digital I/O 25-48 Pin Funzione Funzione MATLAB 1 DIO47 DIO48 3 DIO46 DIO47 5 DIO45 DIO46 7 DIO44 DIO45 9 DIO43 DIO44 11 DIO42 DIO43 13 DIO41 DIO42 15 DIO40 DIO41 17 DIO39 DIO40 19 DIO38 DIO39 21 DIO37 DIO38 23 DIO36 DIO37 25 DIO35 DIO36 27 DIO34 DIO35 29 DIO33 DIO34 31 DIO32 DIO33 33 DIO31 DIO32 35 DIO30 DIO31 37 DIO29 DIO30 39 DIO28 DIO29 41 DIO27 DIO28 43 DIO26 DIO27 45 DIO25 DIO26 47 DIO24 DIO25 49 5V 5V tutti i pin pari Ground Ground Tabella A.3: Connettore digitale J3 (DIOn = Digital I/O numero). Appendice A La scheda Sensoray 626 124 § A.5 Tabelle I/O-pin Sensoray 626 Pin 1 3 5 7 9 11 13 15 17 19 21 23 25 J5 26 pin IDC ribbon cable connector Encoder 1A-3A Funzione Pin Funzione Encoder (1A) A2 Encoder (1A) GND 4 Encoder (1A) Encoder (1A) B+ 6 5V Encoder (1A) I8 Encoder (1A) GND 10 Encoder (2A) Encoder (2A) A+ 12 5V Encoder (2A) B14 Encoder (2A) GND 16 Encoder (2A) Encoder (2A) I+ 18 5V Encoder (3A) A20 Encoder (3A) GND 22 Encoder (3A) Encoder (3A) B+ 24 5V Encoder (3A) I26 Encoder (3A) A+ BI+ AB+ IA+ BI+ Tabella A.4: Connettore Encoder J5. Pin 1 3 5 7 9 11 13 15 17 19 21 23 25 J4 26 pin IDC ribbon cable connector Encoder 1B-3B Funzione Pin Funzione Encoder (1B) A2 Encoder (1B) GND 4 Encoder (1B) Encoder (1B) B+ 6 5V Encoder (1B) I8 Encoder (1B) GND 10 Encoder (2B) Encoder (2B) A+ 12 5V Encoder (2B) B14 Encoder (2B) GND 16 Encoder (2B) Encoder (2B) I+ 18 5V Encoder (3B) A20 Encoder (3B) GND 22 Encoder (3B) Encoder (3B) B+ 24 5V Encoder (3B) I26 Encoder (3B) A+ BI+ AB+ IA+ BI+ Tabella A.5: Connettore Encoder J4. Appendice A La scheda Sensoray 626 125 Appendice B Il microcontrollore B.1 Generalità sul microcontrollore Un microcontrollore o microcontroller, detto anche computer single chip è un sistema a microprocessore completo, integrato in un solo chip, progettato per ottenere la massima autosufficienza funzionale ed ottimizzare il rapporto prezzo-prestazioni per una specifica applicazione, a differenza, ad esempio, dei microprocessori impiegati nei personal computer, adatti per un uso più generale. I microcontroller sono la forma più diffusa e più invisibile di computer. Comprendono la CPU, un certo quantitativo di memoria RAM e memoria ROM (può essere PROM, EPROM, EEPROM o FlashROM) e una serie di interfacce di I/O (input/output) standard, fra cui molto spesso bus (I 2 C,SP I,CAN ,LIN ). Le periferiche integrate sono la vera forza di questi dispositivi: si possono avere convertitori ADC e convertitori DAC multicanale, timer/counters, USART, numerose porte esterne bidirezionali bufferizzate, comparatori, PWM. Infine una caratteristica di alte prestazioni del microcontrollore è l’utilizzo dell’interrupt. Un Interrupt è una segnalazione che viene fatta al PIC da una delle sue periferiche o dal mondo esterno, ed avvisa il dispositivo che si è verificato un determinato evento. La tecnica interrupt aumenta le prestazioni rispetto alla tecnica di polling. Entrambe eseguono una subroutine solo se si verifica un evento ma nel primo caso è l’evento che avverte la sua presenza, nel secondo, invece, è il sistema che richiede ciclicamente se l’evento è presente. Quando si verifica un Interrupt 126 § B.1 Generalità sul microcontrollore il PIC interrompe immediatamente la normale esecuzione del programma e salta alla locazione di memoria a partire dalla quale è allocata l’ISR (Interrupt Service Routine). A seconda di quale sia l’evento verificatosi, il programma in esecuzione deve svolgere la particolare funzione richiesta, al termine della quale riprenderà la normale esecuzione del programma registrato in memoria. In questa Appendice viene spiegato come programmare un microcontrollore MICROCHIP della famiglia dsPIC30F. Il microcontrollore usato in questa tesi è il dsPIC30F4013, installato sulla demoboard dsP ICDEM T M 2 (vedi figura B.1). In Appendice C viene invece descritto il listato C30 del programma, sviluppato con il pacchetto MICROCHIP MPLAB, che consente al PIC di simulare il comportamento di un encoder ottico. Figura B.1: Scheda dsP ICDEM T M 2. Appendice B Il microcontrollore 127 § B.2 dsPIC30F4013 B.2 dsPIC30F4013 Di seguito vengono riportate le tabelle (B.1 e B.2) che descrivono in dettaglio la funzione di ciascun pin del microcontrollore dsPIC30F4013 mostrato nelle figure B.2 e B.3. . Figura B.2: Microcontrollore MICROCHIP dsPIC30F4013. Figura B.3: Piedinatura del dsPIC30F4013 (40-PIN PDIP dsPIC30F4013). Appendice B Il microcontrollore 128 § B.2 dsPIC30F4013 Tabella B.1: dsPIC30F4013. Descrizione funzioni PIN del microcontrollore Microchip Appendice B Il microcontrollore 129 § B.2 dsPIC30F4013 Tabella B.2: dsPIC30F4013. Descrizione funzioni PIN del microcontrollore Microchip . Appendice B Il microcontrollore 130 § B.3 MPLAB B.3 MPLAB MPLAB è un pacchetto software e hardware per la programmazione e la simuc FLASH e dei dsP IC T M . Il pacchetto lazione dei microcontrollori di tipo P IC c IDE (Integrated Development Environment) è un’intersoftware, detto M P LAB faccia per l’ambiente di sviluppo di applicazioni integrate di tipo grafica e consta essenzialmente di un editor, un project manager e un assemblatore. È un tool ideale per i progettisti di controlli che cercano un’alternativa di basso costo agli emulatori incircuit costosi. La programmazione del PIC è possibile sia in modalità basso che in modalità alto livello, usando rispettivamente il linguaggio ASM (Assembly Code ASM30) o il C (C18 o C30). Purtroppo l’Assembly, nonostante ottimizzi l’utilizzazione del dispositivo per mezzo di un linguaggio molto essenziale, rendendolo cosı̀ più veloce, è riservato ad una utenza specializzata. Affinché il dispositivo possa essere sfruttato da una gamma di tecnici più ampia la Microchip mette a disposizione per i microcontrollori della serie dsPIC30xxx un compilatore altamente ottimizzato (MPLAB C30) per un linguaggio di programmazione di medio livello più portabile e semplice, di tipo C-Like: il C30. MPLAB funge quindi da dispositivo di interfacciamento/traduzione intelligente fra PC e PIC permettendo al tecnico l’esamine del microcontrollore, e consentendo l’osservazione in tempo reale delle variabili e dei registri, usando opportune finestre di vigilanza. c ICD 2 (In-Circuit Debugger 2) è un dispoIl pacchetto hardware, detto M P LAB sitivo che possiede un interfaccia RS-232 collegabile al PC (tramite porta USB), indispensabile per la scrittura, la lettura e la cancellazione del programma dalla memoria del microcontroller (quest’ultima operazione è possibile con verifica). Queste operazioni sono facilitate dalla visione di opportuni LED diagnostici (Power, Busy, Error) e dalla possibilità di poter eseguire il Debug in background in tempo reale. Ovviamente il dispositivo, compatibile con l’ambiente di sviluppo c IDE di cui sopra, deve essere collegato alla demoboard specifica grafico M P LAB per il microcontrollore scelto, nel nostro caso la dsP ICDEM T M 2 (figura B.1) su cui è installato il dsPIC30F4013. Appendice B Il microcontrollore 131 § B.3 MPLAB Figura B.4: Una snapshot del workspace del MPLAB dopo la connessione con l’hardware di programmazione. c ICD 2. Figura B.5: M P LAB Appendice B Il microcontrollore 132 § B.4 dsPICDEM 2 B.4 dsPICDEM 2 La dsP ICDEM T M 2 Development Board è il tool di sviluppo e valutazione che consente al progettista di creare applicazioni embedded basate su microcontrollori Microchip dsPIC30F DSC (Digital Signal Controllers). La scheda supporta i dispositivi ”dsPIC30F General Purpose and Sensor family” con 18, 28 e 40-pin (socket U1C1, U1B1 e U1A1) e ”dsPIC30F Motor Control family” con 28 o 40-pin (socket U2B1 e U2A1). La dsPICDEM 2 include: un oscillatore a cristallo per ogni set di socket, una porta ICD per la connessione con l’MPLAB ICD 2 In-Circuit Debugger, porte CAN ed UART, indicatori a LED, interruttori a pulsante, un potenziometro, un sensore di temperatura e un piccolo schermo LCD. Per completare la descrizione della scheda vengono di seguito riportate le caratteristiche tecniche, la tabella dei componenti hardware (con riferimento alla figura B.6) , ed infine la tabella per la configurazione dei Jumper della scheda dsPICDEM 2, richiesta per il montaggio del microcontrollore dsPIC30F4013. Caratteristiche tecniche: • Sockets per dispositivi con 18, 28 e 40-pin PDIP e SPDIP; • Alimentazione 9V DC con indicatore LED power on; • Regolatore di tensione 9V DC −→ 5V per VDD e AVDD ; • Connettore per MPLAB ICD 2 In-Circuit Debugger o MPLAB ICE 4000 In-Circuit Emulator; • Interfaccia RS-232; • Interfaccia CAN (Controller Area Network interface); • Potenziometro e Sensore di temperatura per simulare ingressi A/D; • Interruttori a pulsante ed indicatori a LED per simulare ingressi e uscite digitali; Appendice B Il microcontrollore 133 § B.4 dsPICDEM 2 • Display LCD 2x16 caratteri ASCII con interfaccia SP I T M ; • Area (2x20) per installazione di hardware di prototipazione aggiuntivo (es. socket per cavo flat 40 poli). Figura B.6: Dettaglio componenti scheda dsP ICDEM T M 2. Appendice B Il microcontrollore 134 § B.4 dsPICDEM 2 No. 1 2 3 4 5 6 7 8 9 13 14 Componente Hardware Porta CAN Ricetrasmettitore CAN CAN Header Ingresso Alimentazione Esterna Power ON LED Jumpers Alimentazione Regolatore di Tensione UART 1 Header Interruttore DIP dispositivi General Purpose Alternate UART 1 header Sockets Microcontrollori dsPIC30F Oscillatore a cristallo per dispositivi General Purpose Interruttore a pulsante reset UART 2 header 15 16 17 18 LED di uscita LED Header Interruttore a pulsante S5 e S6 Header interruttore S5 10 11 12 No. Componente Hardware 19 Header interruttore S6 20 Potenziometro Analogico 21 Header Potenziometro Analogico 22 Display grafico LCD 23 Controllore SP I T M per LCD 24 SP I T M Programming Header 25 LCD Controller Header 26 Header Sensore Temperatura 27 Sensore Temperatura 28 29 30 31 32 33 34 35 Headers Connessioni Esterne Oscillatore a cristallo per dispositivi controllo motori Interruttore DIP dispositivi Motor Control Microcontrollore dsPIC30F4011 Selettore di programmazione interruttore DIP Connettore ICD Ricetrasmettitore UART Porta Seriale RS-232 Tabella B.3: Componenti hardware dsP ICDEM T M 2. . . Appendice B Il microcontrollore 135 § B.4 dsPICDEM 2 Tabella B.4: Configurazione dei Jumper della scheda dsPICDEM 2, richiesta per il montaggio del microcontrollore dsPIC30F4013. Appendice B Il microcontrollore 136 § B.5 Step programmazione PIC B.5 Step programmazione PIC 1. Installare MPLAB IDE; c ICD 2; 2. Installare il driver per l’ M P LAB 3. Installare MPLAB C30; c ICD 2 4. Collegare l’hardware esterno al computer, in questo caso l’M P LAB e la dsP ICDEM T M 2; 5. Aprire MPLAB IDE; 6. Selezionare dal menù: Debugger −→ SelectT ool −→ M P LABICD2; 7. Selezionare dal menù: Conf igure −→ SelectDevice; Appendice B Il microcontrollore 137 § B.5 Step programmazione PIC 8. Nella finestra Select Device (figura B.7), selezionare dai menù Device e Device Family il modello di microcontrollore da programmare. In questo caso: Device: dsPIC30F4013 Device Family: 30xxxx Figura B.7: Finestra SelectDevice. Appendice B Il microcontrollore 138 § B.5 Step programmazione PIC 9. Cliccare sull’icona Reset and Connect to ICD; 10. Cliccare sull’icona Open Project, per aprire il nuovo progetto (vedi paragrafo per creazione nuovo progetto); 11. Compilare il progetto cliccando sull’icona Build All ; 12. Cliccare sull’icona Make; Appendice B Il microcontrollore 139 § B.5 Step programmazione PIC 13. Cancellare (ripulire), se necessario, la memoria del PIC, cliccando sull’icona Erase target device; 14. Programmare il PIC cliccando sull’icona Program target device; Appendice B Il microcontrollore 140 Appendice C Codice C30 per l’algoritmo di generazione dei segnali encoder In questo paragrafo viene spiegato in dettaglio il codice, scritto in linguaggio C30, che consente di implementare l’algoritmo di generazione segnali encoder (vedi §3.2.2) sul microcontrollore Microchip dsPIC30F4013. Il progetto MPLAB, encoder.mcp, creato per programmare il microcontrollore include: Source Files: • Main.c • INTx IO pins.c • A to D Converter.c • Timers.c Header Files: • p30f4013.h Library Files: • libpic30-coff.a Linker Script: • p30f4013.gld Segue la descrizione del codice C30 dei suddetti files. 141 § C.1 Main.c C.1 Main.c • #include <p30fxxxx.h>: Generico header file per i dispositivi della famiglia dsPIC30F; • FOSC(CSW FSCM OFF XT PLL8): Fissa il tempo di ciclo di ciascuna & istruzione del codice del programma. In particolare si ha: TCY = Instruction Cycle T ime FCY = Internal Instruction Cycle Clock FOSC = System Clock Source Source Oscillator F requency ∗ P LL M ultipler FCY = F OSC/4 = ∗4 P rogrammable P ost Scalar 7.3728 M Hz · 8 ' 14.74 M Hz FCY = 4 1 1 = ' 67 ns TCY = F CY 14.74 M Hz • FWDT(WDT OFF): Disabilita il Watch-Dog Timer; • FBORPOR(MCLR EN & PWRT OFF): Abilita il pin MCLR reset e disabilita il power-up timers; • FGS(CODE PROT OFF): Disabilita il Code Protection; • Dichiarazione delle funzioni esterne che dovranno essere linkate durante la compilazione del progetto: extern ADC Init(void); extern INTx Init(void); extern Timer1 Init(void); • Dichiarazione della funzione globale main: int main(void); • Inizio funzione main.c: int main(void) { Appendice C Codice C30 per l’algoritmo di generazione dei segnali encoder 142 § C.1 Main.c • ADPCFG=0xFFFF: Dopo il reset pone a zero tutti i pin del registro ADPCFG (A/D Port Configuration Register); • Inizializzazione funzioni esterne: ADC Init(void); INTx Init(void); Timer1 Init(void); • Loop per l’esecuzione del codice dell’algoritmo di generazione dei segnali encoder: while(1) { } • Chiusura funzione main.c: return 0; } Appendice C Codice C30 per l’algoritmo di generazione dei segnali encoder 143 § C.2 INTx IO pins.c C.2 INTx IO pins.c • #include <p30fxxxx.h>: Generico header file per i dispositivi della famiglia dsPIC30F; • Dichiarazione della funzione globale INTx IO Init: void INTx IO Init(void); • Inizio funzione INTx IO Init: void INTx IO Init(void); { • Uscite digitali per i segnali elettrici in fase e quadratura di tipo encoder del giunto 1: LATBbits.LATB0 = 0; Clear Latch bit del pin 0 della porta B (RB0); TRISBbits.TRISB0 = 0; Setta come uscita il pin 0 della porta B (RB0); LATBbits.LATB1 = 0; Clear Latch bit del pin 1 della porta B (RB1); TRISBbits.TRISB1 = 0; Setta come uscita il pin 1 della porta B (RB1); • Uscite digitali per i segnali elettrici in fase e quadratura di tipo encoder del giunto 2: LATBbits.LATB4 = 0; Clear Latch bit del pin 4 della porta B (RB4); TRISBbits.TRISB4 = 0; Setta come uscita il pin 4 della porta B (RB4); LATBbits.LATB5 = 0; Clear Latch bit del pin 5 della porta B (RB5); TRISBbits.TRISB5 = 0; Setta come uscita il pin 5 della porta B (RB5); • Uscite digitali per i segnali elettrici in fase e quadratura di tipo encoder del giunto 3: LATDbits.LATD0 = 0; Clear Latch bit del pin 0 della porta D (RD0); TRISDbits.TRISD0 = 0; Setta come uscita il pin 0 della porta D (RD0); LATDbits.LATD1 = 0; Clear Latch bit del pin 1 della porta D (RD1); TRISDbits.TRISD1 = 0; Setta come uscita il pin 1 della porta D (RD1); Appendice C Codice C30 per l’algoritmo di generazione dei segnali encoder 144 § C.2 INTx IO pins.c • Uscite digitali per i segnali elettrici in fase e quadratura di tipo encoder del giunto 4: LATFbits.LATF0 = 0; Clear Latch bit del pin 0 della porta F (RF0); TRISFbits.TRISF0 = 0; Setta come uscita il pin 0 della porta F (RF0); LATFbits.LATF1 = 0; Clear Latch bit del pin 1 della porta F (RF1); TRISFbits.TRISF1 = 0; Setta come uscita il pin 1 della porta F (RF1); • Uscite digitali per i segnali elettrici in fase e quadratura di tipo encoder del giunto 5: LATCbits.LATC13 = 0; Clear Latch bit del pin 13 della porta C (RC13); TRISCbits.TRISC13 = 0; Setta come uscita il pin 13 della porta C (RC13); LATCbits.LATC14 = 0; Clear Latch bit del pin 14 della porta C (RC14); TRISCbits.TRISC14 = 0; Setta come uscita il pin 14 della porta C (RC14); • Uscite digitali per i segnali elettrici in fase e quadratura di tipo encoder del giunto 6: LATFbits.LATF4 = 0; Clear Latch bit del pin 4 della porta F (RF4); TRISFbits.TRISF4 = 0; Setta come uscita il pin 4 della porta F (RF4); LATFbits.LATF5 = 0; Clear Latch bit del pin 5 della porta F (RF5); TRISFbits.TRISF5 = 0; Setta come uscita il pin 5 della porta F (RF5); • Chiusura funzione INTx IO Init: } Appendice C Codice C30 per l’algoritmo di generazione dei segnali encoder 145 § C.3 A to D Converter.c C.3 A to D Converter.c • Direttive di pre-processore: #include <p30xxxx.h> #include "system.h" #include <stdio.h> • Dichiarazione funzioni e variabili globali: void ADC Init(void); void attribute float qp1 = 0.0; float qp2 = 0.0; float qp3 = 0.0; float qp4 = 0.0; float qp5 = 0.0; float qp6 = 0.0; (( interrupt )) ADCInterrupt(void); • Inizio funzione ADC Init(): void ADC Init(void) { • Configurazione pin del registro ADCON1 per avere le funzioni di A/D Automatic Sampling, ed A/D Auto-Convert: ADCON1bits.SSRC = 7; ADCON1bits.ASAM = 1; tutti gli altri pin del registro ADCON1 vengono lasciati nello stato di default; • Configurazione pin del registro ADCON2: ADCON2bits.SMPI = 5; esegue la funzione di interrupt sul canale di input richiesto solo dopo che 2 campioni del segnale in ingresso, vengono memorizzati nel buffer dello stesso canale ADCON1bits.CSCNA = 1; abilita la funzione di Channel scanning tutti gli altri pin del registro ADCON2 vengono lasciati nello stato di default; Appendice C Codice C30 per l’algoritmo di generazione dei segnali encoder 146 § C.3 A to D Converter.c • Configurazione pin del registro ADCON3: Viene impostato un tempo di acquisizione (Tacq) pari a 31 Tad, dove: Tad = A/D conversion clock time Viene poi impostato: Tad = 20.5Tcy Tcy = instruction cycle time Dato che ogni conversione richiede 14·Tad (= Tconv) periodi, il tempo totale di campionamento (Tsamp), dato dalla somma del tempo di acquisizione e del tempo di conversione, risulta pari a: Tsamp = Tacq + Tconv = (31 + 14) · Tad = 45 · Tad periodi Tsamp = 45 · 20.5 · Tcy = 922.5 · Tcy = 922.5 · 67 ns ' 62 microsecondi 1 1 Fsamp = = ' 16 KHz Tsamp 62 ADCON3bits.SAMC = 31; imposta Tacq = 31 · Tad ADCON3bits.ADCS = 40; imposta Tad = 20.5 · Tcy tutti gli altri pin del registro ADCON3 vengono lasciati nello stato di default; • Configurazione pin del registro ADCHS: ADCHS = 0x0000; imposta il registro come ”don’t care”; • Configurazione pin del registro ADCSSL: ADCSSL = 0x1D0C; imposta i canali su cui viene eseguito lo scan, in particolare sono i canali: AN2, AN3, AN8, AN10, AN11, AN12; • Configurazione pin del registro ADPCFG. Vengono impostati come analogici i (suddetti) canali di ingresso: AN2, AN3, AN8, AN10, AN11, AN12: ADPCFGbits.PCFG2 = 0; Appendice C Codice C30 per l’algoritmo di generazione dei segnali encoder 147 § C.3 A to D Converter.c ADPCFGbits.PCFG3 = 0; ADPCFGbits.PCFG8 = 0; ADPCFGbits.PCFG10 = 0; ADPCFGbits.PCFG11 = 0; ADPCFGbits.PCFG12 = 0; • Clear A/D interrupt flag bit: IFS0bits.ADIF=0; • Set A/D interrupt enable bit: IEC0bits.ADIE=0; • Accensione del convertitore A/D. Questo comando viene eseguito al termine della configurazioni di tutti i registri del convertitore A/D: ADCON1bits.ADON=1; • Chiusura funzione ADC Init(): } • Inizio funzione void attribute (( interrupt )) ADCInterrupt(void) : void attribute (( interrupt )) ADCInterrupt(void); { • Vengono copiati nelle variabili qpi i valori, precedentemente memorizzati nei registri 12 bit dei buffer ADCBUFn (n = numero canale) dei canali di input: qp1 = (float) (ADCBUF0) ∗ (4.98/4096.0)); qp2 = (float) (ADCBUF1) ∗ (4.98/4096.0)); qp3 = (float) (ADCBUF2) ∗ (4.98/4096.0)); qp4 = (float) (ADCBUF3) ∗ (4.98/4096.0)); qp5 = (float) (ADCBUF4) ∗ (4.98/4096.0)); qp6 = (float) (ADCBUF5) ∗ (4.98/4096.0)); La costante: 4.98 4096.0 consente di convertire in Volt i valori memorizzati nei registri 12 bit dei buffer ADCBUFn. Il convertitore A/D infatti rileva solo i valori compresi tra 0 e Appendice C Codice C30 per l’algoritmo di generazione dei segnali encoder 148 § C.3 A to D Converter.c 4.98 Volt e codifica questo intervallo con 12 bit, cioè lo divide in 4096.0 valori, ad ognuno dei quali corrisponde una stringa binaria di 12 bit (vedi figura C.1); Figura C.1: Rappresentazione grafica della funzione di trasferimento ideale del convertitore A/D. • Clear A/D Interrupt flag bit oppure salta alla locazione di memoria a partire dalla quale è allocata l’ISR (Interrupt Service Routine) IFS0bits.ADIF=0; • Chiusura funzione void attribute (( interrupt )) ADCInterrupt(void) : } Appendice C Codice C30 per l’algoritmo di generazione dei segnali encoder 149 § C.4 Timers.c C.4 Timers.c In questo file viene scritto il codice necessario per implementare l’algoritmo di generazione dei segnali in fase e quadratura di tipo encoder descritto nel §3.2.2. • Direttive di pre-processore: #include <p30xxxx.h> #include "system.h" #include <stdio.h> • Dichiarazioni per il link delle variabili esterne: extern float qp1; extern float qp2; extern float qp3; extern float qp4; extern float qp5; extern float qp6; • Dichiarazione funzioni e variabili globali: void Timer1 Init(void); void attribute (( interrupt )) T1Interrupt(void); • Inizio funzione T imer1 Init: void Timer1 Init(void) { Questa funzione imposta il valore massimo di conteggio per il contatore Timer 1 e abilita l’ISR (Interrupt Service Routine) Timer 1; • T1CON=0x0020; • PR1 = 0xFFFF; Imposta il periodo massimo di conteggio; • IFS0bits.T1IF = 0; Clear Timer1 Interrupt Flag; • IEC0bits.T1IE = 1; Abilita la Timer1 Interrupt Service Routine; Appendice C Codice C30 per l’algoritmo di generazione dei segnali encoder 150 § C.4 Timers.c • T1CONbits.TON = 1; Start Timer 1 • Chiusura funzione T imer1 Init: } • Inizio della Timer 1 Interrupt Service Routine: void attribute (( interrupt )) T1Interrupt(void); { • Dichiarazione variabili statiche θ: static float theta1 = 0.0; static float theta2 = 0.0; static float theta3 = 0.0; static float theta4 = 0.0; static float theta5 = 0.0; static float theta6 = 0.0; • Dichiarazione e inizializzazione costanti: float Kout, delta T, pi, N, R; delta T= 0.0044; Kout = 5.0; N = 100.0; pi = 3.1415; R = (2.0*pi)/N; • Codice dell’algoritmo di generazione dei segnali in fase e quadratura di tipo encoder: //codice per qp1 theta1 = theta1 + (qp1-2.5)*Kout*delta_T; if (theta1 > 2.0*R) Appendice C Codice C30 per l’algoritmo di generazione dei segnali encoder 151 § C.4 Timers.c { theta1 = theta1 - 2.0*R; } if (theta1 < 0.0) { theta1 = theta1 + 2.0*R; } if (theta1 <= R) { //yf[0]=1.0; LATBbits.LATB0 = 0; PORTBbits.RB0 = 1; } else { //yf[0]=0.0; LATBbits.LATB0 = 0; PORTBbits.RB0 = 0; } if ( ( (R/2.0) < theta1) && ( theta1 < ((3.0/2.0)*R)) ) { //yq[0]=0.0; LATBbits.LATB1 = 0; PORTBbits.RB1 = 0; } else { //yq[0]=1.0; LATBbits.LATB1 = 0; PORTBbits.RB1 = 1; } //codice per qp2 theta2 = theta2 + (qp2-2.5)*Kout*delta_T; if (theta2 > 2.0*R) { theta2 = theta2 - 2.0*R; } if (theta2 < 0.0) { theta2 = theta2 + 2.0*R; } Appendice C Codice C30 per l’algoritmo di generazione dei segnali encoder 152 § C.4 Timers.c if (theta2 <= R) { //yf[0]=1.0; LATBbits.LATB4 = 0; PORTBbits.RB4 = 1; } else { //yf[0]=0.0; LATBbits.LATB4 = 0; PORTBbits.RB4 = 0; } if ( ( (R/2.0) < theta2) && ( theta2 < ((3.0/2.0)*R)) ) { //yq[0]=0.0; LATBbits.LATB5 = 0; PORTBbits.RB5 = 0; } else { //yq[0]=1.0; LATBbits.LATB5 = 0; PORTBbits.RB5 = 1; } //codice per qp3 theta3 = theta3 + (qp3-2.5)*Kout*delta_T; if (theta3 > 2.0*R) { theta3 = theta3 - 2.0*R; } if (theta3 < 0.0) { theta3 = theta3 + 2.0*R; } if (theta3 <= R) { //yf[0]=1.0; LATDbits.LATD0 = 0; PORTDbits.RD0 = 1; } else { //yf[0]=0.0; Appendice C Codice C30 per l’algoritmo di generazione dei segnali encoder 153 § C.4 Timers.c LATDbits.LATD0 = 0; PORTDbits.RD0 = 0; } if ( ( (R/2.0) < theta3) && ( theta3 < ((3.0/2.0)*R)) ) { //yq[0]=0.0; LATDbits.LATD1 = 0; PORTDbits.RD1 = 0; } else { //yq[0]=1.0; LATDbits.LATD1 = 0; PORTDbits.RD1 = 1; } //codice per qp4 theta4 = theta4 + (qp4-2.5)*Kout*delta_T; if (theta4 > 2.0*R) { theta4 = theta4 - 2.0*R; } if (theta4 < 0.0) { theta4 = theta4 + 2.0*R; } if (theta4 <= R) { //yf[0]=1.0; LATFbits.LATF0 = 0; PORTFbits.RF0 = 1; } else { //yf[0]=0.0; LATFbits.LATF0 = 0; PORTFbits.RF0 = 0; } if ( ( (R/2.0) < theta4) && ( theta4 < ((3.0/2.0)*R)) ) { //yq[0]=0.0; LATFbits.LATF1 = 0; Appendice C Codice C30 per l’algoritmo di generazione dei segnali encoder 154 § C.4 Timers.c PORTFbits.RF1 = 0; } else { //yq[0]=1.0; LATFbits.LATF1 = 0; PORTFbits.RF1 = 1; } //codice per qp5 theta5 = theta5 + (qp5-2.5)*Kout*delta_T; if (theta5 > 2.0*R) { theta5 = theta5 - 2.0*R; } if (theta5 < 0.0) { theta5 = theta5 + 2.0*R; } if (theta5 <= R) { //yf[0]=1.0; LATCbits.LATC13 = 0; PORTCbits.RC13 = 1; } else { //yf[0]=0.0; LATCbits.LATC13 = 0; PORTCbits.RC13 = 0; } if ( ( (R/2.0) < theta5) && ( theta5 < ((3.0/2.0)*R)) ) { //yq[0]=0.0; LATCbits.LATC14 = 0; PORTCbits.RC14 = 0; } else { //yq[0]=1.0; LATCbits.LATC14 = 0; PORTCbits.RC14 = 1; } Appendice C Codice C30 per l’algoritmo di generazione dei segnali encoder 155 § C.4 Timers.c //codice per qp6 theta6 = theta6 + (qp6-2.5)*Kout*delta_T; if (theta6 > 2.0*R) { theta6 = theta6 - 2.0*R; } if (theta6 < 0.0) { theta6 = theta6 + 2.0*R; } if (theta6 <= R) { //yf[0]=1.0; LATFbits.LATF4 = 0; PORTFbits.RF4 = 1; } else { //yf[0]=0.0; LATFbits.LATF4 = 0; PORTFbits.RF4 = 0; } if ( ( (R/2.0) < theta6) && ( theta6 < ((3.0/2.0)*R)) ) { //yq[0]=0.0; LATFbits.LATF5 = 0; PORTFbits.RF5 = 0; } else { //yq[0]=1.0; LATFbits.LATF5 = 0; PORTFbits.RF5 = 1; } IFS0bits.T1IF = 0; //Clear Timer1 Interrupt Flag Nella riga di codice: theta = theta + (qp-2.5)*Kout*delta T; viene sottratto il valore costante 2.5 per eliminare l’offset introdotto nello Appendice C Codice C30 per l’algoritmo di generazione dei segnali encoder 156 § C.4 Timers.c schema a blocchi simulink. L’offset viene usato poiché il convertitore A/D rileva solo valori di ingresso compresi tra 0 e 5 Volt. Quindi aggiungendo un piccolo offset a qp nello schema simulink si fa in modo che anche i segnali negativi vengano rilevati dal convertitore A/D. • Chiusura della Timer 1 Interrupt Service Routine: } Appendice C Codice C30 per l’algoritmo di generazione dei segnali encoder 157 Appendice D Codice C per la definizione del modello real-time In questa sezione viene descritto il codice C dei file, RTrneSCARA.c e RTrneSCARAff.c, che vengono automaticamente generati dai corrispondenti file.m, per consentire, rispettivamente, il calcolo della dinamica diretta e inversa del manipolatore robotico scelto. I file C sono stati appositamente ideati per integrare le librerie del Robotics Toolbox (RT) (ne.c, vmath.c e rot mat) non adatte per un funzionamento di tipo real-time. Caricando questi file C insieme alle librerie del RT all’interno dei blocchi Simulink C-MEX S-Function, si ottiene il blocco Simulink real-time che calcolando in tempo reale i segnali di posizione e velocità dei giunti del robot, implementa nel calcolatore la dinamica del manipolatore robotico scelto. Segue la descrizione del codice dei suddetti file. D.1 RTrneSCARA.c • Inclusione librerie per i file di tipo MEX (Matlab Executable): #ifdef MATLAB_MEX_FILE #include "tmwtypes.h" #else #include "rtwtypes.h" #endif • Inclusione librerie esterne: 158 § D.1 RTrneSCARA.c #include #include #include #include #include #include <stdio.h> <stdlib.h> <string.h> <math.h> <frne.h> <vmath.h> • Definizione delle macro, NQ e NJOINTS : #define NJOINTS 4 #define NQ 1 • Dichiarazione funzioni esterne: void newton_euler(Robot *, double *, double *, double *, double *, int); void rot_mat(Link *, double, double, DHType); • Inizio codice funzione RTrne per il calcolo della dinamica diretta del manipolatore: \renewcommand{\baselinestretch}{0.9} void RTrne(real_T *A1_IN, real_T *A2_IN, real_T *A3_IN, real_T *y) { • Dichiarazione variabili e puntatori: double *I, *Tc; double *fext=NULL; Vect gravity, *rbar; Robot robot; Link *l; double *q, *qd, *qdd; double *q_B, *qd_B, *qdd_B; double *tau_B, *tau; int k, kk, k1, k2, k3, pB, jB, p, j, nq; • Allocazione dinamica della memoria ed inizializzazione dei suddetti puntatori: Appendice D Codice C per la definizione del modello real-time 159 § D.1 RTrneSCARA.c y[0]=NJOINTS; for (k1=0;k1<NJOINTS*NJOINTS;k1++) { y[k1+1]=0; } for (k2=0;k2<NQ*NJOINTS;k2++) { y[k2+1+NJOINTS*NJOINTS]=0; } for (k3=0;k3<NJOINTS;k3++) { y[k3+1+NJOINTS*NJOINTS+NQ*NJOINTS]=0; } q=A1_IN; qd=A2_IN; qdd = malloc(NJOINTS*sizeof(double)); if (qdd==NULL) { printf("Errore malloc per qdd \n"); return; } memset(qdd,0,NJOINTS*sizeof(double)); q_B=malloc(NJOINTS*NJOINTS*sizeof(double)); if (q_B==NULL) { printf("Errore malloc per q_B \n"); return; } memset(q_B,0,NJOINTS*NJOINTS*sizeof(double)); qd_B=malloc(NJOINTS*NJOINTS*sizeof(double)); if (qd_B==NULL) { printf("Errore malloc per qd_B \n"); return; } memset(qd_B,0,NJOINTS*NJOINTS*sizeof(double)); qdd_B=malloc(NJOINTS*NJOINTS*sizeof(double)); if (qdd_B==NULL) { printf("Errore malloc per qdd_B \n"); return; } Appendice D Codice C per la definizione del modello real-time 160 § D.1 RTrneSCARA.c memset(qdd_B,0,NJOINTS*NJOINTS*sizeof(double)); for (k=0;k<NJOINTS;k++) { qdd_B[k*(1+NJOINTS)]=1; for (kk=0;kk<NJOINTS;kk++) { q_B[k*NJOINTS+kk]=A1_IN[kk]; } } tau_B=malloc(NJOINTS*NJOINTS*sizeof(double)); if (tau_B==NULL) { printf("Errore malloc per tau_B \n"); return; } memset(tau_B,0,NJOINTS*NJOINTS*sizeof(double)); tau=malloc(NQ*NJOINTS*sizeof(double)); if (tau==NULL) { printf("Errore malloc per tau \n"); return; } memset(tau,0,NQ*NJOINTS*sizeof(double)); • Definizione del robot: robot.njoints=NJOINTS; robot.dhtype=0; rbar=malloc(NJOINTS*sizeof(Vect)); if (rbar==NULL) { printf("Errore malloc per rbar \n"); return; } I=malloc(NJOINTS*9*sizeof(double)); if (I==NULL) { printf("Errore malloc per I \n"); return; } Tc=malloc(NJOINTS*2*sizeof(double)); Appendice D Codice C per la definizione del modello real-time 161 § D.1 RTrneSCARA.c if (Tc==NULL) { printf("Errore malloc per Tc \n"); return; } rbar[0].x=0.000000; rbar[0].y=0.000000; rbar[0].z=0.250000; rbar[1].x=-0.200000; rbar[1].y=0.000000; rbar[1].z=0.000000; rbar[2].x=-0.150000; rbar[2].y=0.000000; rbar[2].z=0.000000; rbar[3].x=0.000000; rbar[3].y=0.000000; rbar[3].z=-0.250000; I[0]=0.200000; I[1]=0.000000; I[2]=0.000000; I[3]=0.000000; I[4]=0.200000; I[5]=0.000000; I[6]=0.000000; I[7]=0.000000; I[8]=0.040000; I[9]=0.100000; I[10]=0.000000; I[11]=0.000000; I[12]=0.000000; I[13]=0.020000; I[14]=0.000000; I[15]=0.000000; I[16]=0.000000; I[17]=0.200000; I[18]=0.050000; I[19]=0.000000; I[20]=0.000000; I[21]=0.000000; I[22]=0.010000; I[23]=0.000000; I[24]=0.000000; I[25]=0.000000; I[26]=0.050000; I[27]=0.010000; I[28]=0.000000; I[29]=0.000000; I[30]=0.000000; I[31]=0.010000; Appendice D Codice C per la definizione del modello real-time 162 § D.1 RTrneSCARA.c I[32]=0.000000; I[33]=0.000000; I[34]=0.000000; I[35]=0.000500; Tc[0]=0.000000; Tc[1]=0.000000; Tc[2]=0.000000; Tc[3]=0.000000; Tc[4]=0.000000; Tc[5]=0.000000; Tc[6]=0.000000; Tc[7]=0.000000; robot.links=malloc(NJOINTS*sizeof(Link)); if (robot.links==NULL) { printf("Errore malloc per robot.links \n"); return; } memset(robot.links,0,NJOINTS*sizeof(Link)); l=&robot.links[0]; l->alpha=0.000000; l->A=0.400000; l->theta=0.000000; l->D=0.000000; l->sigma=0; l->offset=0.000000; l->m=15.000000; l->rbar=&rbar[0]; l->I=&I[9*0]; l->Jm=0.000200; l->G=-62.611100; l->B=0.001480; l->Tc=&Tc[2*0]; l=&robot.links[1]; l->alpha=3.141593; l->A=0.300000; l->theta=0.000000; l->D=0.000000; l->sigma=0; l->offset=0.000000; l->m=8.000000; l->rbar=&rbar[1]; l->I=&I[9*1]; l->Jm=0.000200; l->G=107.815000; l->B=0.000817; l->Tc=&Tc[2*1]; Appendice D Codice C per la definizione del modello real-time 163 § D.1 RTrneSCARA.c l=&robot.links[2]; l->alpha=0.000000; l->A=0.000000; l->theta=0.000000; l->D=0.000000; l->sigma=1; l->offset=0.000000; l->m=6.000000; l->rbar=&rbar[2]; l->I=&I[9*2]; l->Jm=0.000200; l->G=-53.706300; l->B=0.001380; l->Tc=&Tc[2*2]; l=&robot.links[3]; l->alpha=0.000000; l->A=0.000000; l->theta=0.000000; l->D=0.000000; l->sigma=0; l->offset=0.000000; l->m=0.500000; l->rbar=&rbar[3]; l->I=&I[9*3]; l->Jm=0.000033; l->G=76.036400; l->B=0.000071; l->Tc=&Tc[2*3]; • Calcolo della dinamica diretta del manipolatore: #define MEL(x,R,C) (x[(R)+(C)*nq]) gravity.x=0.000000; gravity.y=0.000000; gravity.z=0.000000; robot.gravity=&gravity; nq=NJOINTS; /*CALCOLO LA MATRICE DI INERZIA B */ /* for each point in the input trajectory */ for (pB=0; pB<nq; pB++) { /* * update all position dependent variables */ for (jB = 0; jB < NJOINTS; jB++) { Link *l = &robot.links[jB]; Appendice D Codice C per la definizione del modello real-time 164 § D.1 RTrneSCARA.c switch (l->sigma) { case REVOLUTE: rot_mat(l, q_B[(pB*nq)+jB]+l->offset, l->D, robot.dhtype); break; case PRISMATIC: rot_mat(l, l->theta, q_B[(pB*nq)+jB]+l->offset,robot.dhtype); break; } } newton_euler(&robot, &tau_B[pB], &qd_B[pB], &qdd_B[pB], fext, nq); } /*CALCOLO DEL VETTORE tau*/ gravity.x=0.000000; gravity.y=0.000000; gravity.z=9.810000; robot.gravity=&gravity; nq=NQ; /* for each point in the input trajectory */ for (p=0; p<nq; p++) { /* * update all position dependent variables */ for (j = 0; j < NJOINTS; j++) { Link *l = &robot.links[j]; switch (l->sigma) { case REVOLUTE: rot_mat(l, MEL(q,p,j)+l->offset, l->D, robot.dhtype); break; case PRISMATIC: rot_mat(l, l->theta, MEL(q,p,j)+l->offset, robot.dhtype); break; } } newton_euler(&robot, &tau[p], &qd[p], &qdd[p], fext, nq); } • Scrittura delle uscite: Appendice D Codice C per la definizione del modello real-time 165 § D.1 RTrneSCARA.c y[0]=NJOINTS; for (k1=0;k1<NJOINTS*NJOINTS;k1++) { y[k1+1]=tau_B[k1]; } for (k2=0;k2<NQ*NJOINTS;k2++) { y[k2+1+NJOINTS*NJOINTS]=tau[k2]; } for (k3=0;k3<NJOINTS;k3++) { y[k3+1+NJOINTS*NJOINTS+NQ*NJOINTS]=A3_IN[k3]; } • Deallocazione della memoria dinamica e termine della funzione RTrneSCARA.c: free(qdd); free(q_B); free(qd_B); free(qdd_B); free(tau_B); free(tau); free(rbar); free(I); free(Tc); free(robot.links); } Appendice D Codice C per la definizione del modello real-time 166 § D.2 RTrneSCARAff.c D.2 RTrneSCARAff.c • Inclusione librerie per i file di tipo MEX (Matlab Executable): #ifdef MATLAB_MEX_FILE #include "tmwtypes.h" #else #include "rtwtypes.h" #endif • Inclusione librerie esterne: #include #include #include #include #include #include <stdio.h> <stdlib.h> <string.h> <math.h> <frne.h> <vmath.h> • Definizione delle macro, NQ e NJOINTS : #define NJOINTS 4 #define NQ 1 • Dichiarazione funzioni esterne: void newton_euler(Robot *, double *, double *, double *, double *, int); void rot_mat(Link *, double, double, DHType); • Inizio codice funzione RTrne per il calcolo della dinamica inversa del manipolatore: void RTrne(real_T *A1_IN, real_T *A2_IN, real_T *A3_IN, real_T *y) { • Dichiarazione variabili e puntatori: Appendice D Codice C per la definizione del modello real-time 167 § D.2 RTrneSCARAff.c double *I, *Tc; double *fext=NULL; Vect gravity, *rbar; Robot robot; Link *l; double *q, *qd, *qdd; double *tau; int k, kk, k1, k2, p, j, nq; • Allocazione dinamica della memoria ed inizializzazione dei suddetti puntatori: y[0]=NJOINTS; for (k1=0;k1<NJOINTS;k1++) { y[k1+1]=0; } q=A1_IN; qd=A2_IN; qdd=A3_IN; tau=malloc(NQ*NJOINTS*sizeof(double)); if (tau==NULL) { printf("Errore malloc per tau \n"); return; } memset(tau,0,NQ*NJOINTS*sizeof(double)); • Definizione del robot: robot.njoints=NJOINTS; robot.dhtype=0; rbar=malloc(NJOINTS*sizeof(Vect)); if (rbar==NULL) { printf("Errore malloc per rbar \n"); return; } I=malloc(NJOINTS*9*sizeof(double)); if (I==NULL) { Appendice D Codice C per la definizione del modello real-time 168 § D.2 RTrneSCARAff.c printf("Errore malloc per I \n"); return; } Tc=malloc(NJOINTS*2*sizeof(double)); if (Tc==NULL) { printf("Errore malloc per Tc \n"); return; } rbar[0].x=0.000000; rbar[0].y=0.000000; rbar[0].z=0.250000; rbar[1].x=-0.200000; rbar[1].y=0.000000; rbar[1].z=0.000000; rbar[2].x=-0.150000; rbar[2].y=0.000000; rbar[2].z=0.000000; rbar[3].x=0.000000; rbar[3].y=0.000000; rbar[3].z=-0.250000; I[0]=0.200000; I[1]=0.000000; I[2]=0.000000; I[3]=0.000000; I[4]=0.200000; I[5]=0.000000; I[6]=0.000000; I[7]=0.000000; I[8]=0.040000; I[9]=0.100000; I[10]=0.000000; I[11]=0.000000; I[12]=0.000000; I[13]=0.020000; I[14]=0.000000; I[15]=0.000000; I[16]=0.000000; I[17]=0.200000; I[18]=0.050000; I[19]=0.000000; I[20]=0.000000; I[21]=0.000000; I[22]=0.010000; I[23]=0.000000; I[24]=0.000000; I[25]=0.000000; I[26]=0.050000; I[27]=0.010000; I[28]=0.000000; Appendice D Codice C per la definizione del modello real-time 169 § D.2 RTrneSCARAff.c I[29]=0.000000; I[30]=0.000000; I[31]=0.010000; I[32]=0.000000; I[33]=0.000000; I[34]=0.000000; I[35]=0.000500; Tc[0]=0.000000; Tc[1]=0.000000; Tc[2]=0.000000; Tc[3]=0.000000; Tc[4]=0.000000; Tc[5]=0.000000; Tc[6]=0.000000; Tc[7]=0.000000; robot.links=malloc(NJOINTS*sizeof(Link)); if (robot.links==NULL) { printf("Errore malloc per robot.links \n"); return; } memset(robot.links,0,NJOINTS*sizeof(Link)); l=&robot.links[0]; l->alpha=0.000000; l->A=0.400000; l->theta=0.000000; l->D=0.000000; l->sigma=0; l->offset=0.000000; l->m=15.000000; l->rbar=&rbar[0]; l->I=&I[9*0]; l->Jm=0.000200; l->G=-62.611100; l->B=0.001480; l->Tc=&Tc[2*0]; l=&robot.links[1]; l->alpha=3.141593; l->A=0.300000; l->theta=0.000000; l->D=0.000000; l->sigma=0; l->offset=0.000000; l->m=8.000000; l->rbar=&rbar[1]; l->I=&I[9*1]; l->Jm=0.000200; l->G=107.815000; l->B=0.000817; Appendice D Codice C per la definizione del modello real-time 170 § D.2 RTrneSCARAff.c l->Tc=&Tc[2*1]; l=&robot.links[2]; l->alpha=0.000000; l->A=0.000000; l->theta=0.000000; l->D=0.000000; l->sigma=1; l->offset=0.000000; l->m=6.000000; l->rbar=&rbar[2]; l->I=&I[9*2]; l->Jm=0.000200; l->G=-53.706300; l->B=0.001380; l->Tc=&Tc[2*2]; l=&robot.links[3]; l->alpha=0.000000; l->A=0.000000; l->theta=0.000000; l->D=0.000000; l->sigma=0; l->offset=0.000000; l->m=0.500000; l->rbar=&rbar[3]; l->I=&I[9*3]; l->Jm=0.000033; l->G=76.036400; l->B=0.000071; l->Tc=&Tc[2*3]; • Calcolo della dinamica inversa del manipolatore: #define MEL(x,R,C) (x[(R)+(C)*nq]) gravity.x=0.000000; gravity.y=0.000000; gravity.z=9.810000; robot.gravity=&gravity; nq=NQ; /* for each point in the input trajectory */ for (p=0; p<nq; p++) { /* * update all position dependent variables */ for (j = 0; j < NJOINTS; j++) { Link *l = &robot.links[j]; Appendice D Codice C per la definizione del modello real-time 171 § D.2 RTrneSCARAff.c switch (l->sigma) { case REVOLUTE: rot_mat(l, MEL(q,p,j)+l->offset, l->D, robot.dhtype); break; case PRISMATIC: rot_mat(l, l->theta, MEL(q,p,j)+l->offset, robot.dhtype); break; } } newton_euler(&robot, &tau[p], &qd[p], &qdd[p], fext, nq); } • Scrittura delle uscite: y[0]=NJOINTS; for (k1=0;k1<NJOINTS;k1++) { y[k1+1]=tau[k1]; } • Deallocazione della memoria dinamica e termine della funzione RTrneSCARAff.c: free(tau); free(rbar); free(I); free(Tc); free(robot.links); } Appendice D Codice C per la definizione del modello real-time 172 Appendice E Librerie ed oggetti del Robotics Toolbox In questa sezione verranno introdotti i principali oggetti gestiti dal RT e le librerie per il calcolo della dinamica di un manipolatore. E.1 link • Tipologia: oggetto link. • Sintassi comando: L = link L = link([alpha, a, theta, d]) L = link([alpha, a, theta, d, sigma]) L = link(dyn_row) A = link(q) showlink(L) • Descrizione: La funzione link() genera l’oggetto link. L’oggetto contiene le informazioni sui parametri cinematici e dinamici del robot, ma anche altri dati relativi ad esempio ai parametri inerziali del motore di attuazione del giunto. La prima forma della sintassi di comando ritorna l’oggetto predefinito, mentre la seconda e la terza forma inizializzano il modello cinematico basato sulla rappresentazione di Denavit-Hartenberg. Nelle suddette chiamate a funzione la rappresentazione di D-H classica è scelta come predefinita. Il 173 § E.1 link modello dinamico del manipolatore può essere anche inizializzato usando il comando L = link(dyn row), dove dyn row è una matrice n x 20 (n = numero assi del manipolatore) avente per righe le righe della “legacy dyn matrix” (matrice DYN). La penultima forma della sintassi di comando consente invece di ottenere la matrice di trasformazione, relativa al link considerato, per un dato vettore q delle coordinate di giunto. L’ultimo comando showlink(L) mostra a schermo tutte le informazioni relative al link L richiesto dall’utente. Method link.alpha link.A link.theta link.D link.sigma link.RP link.mdh link.I link.I Operations r+a r+a r+a r+a r+a r r+a r a link.m link.r link.G link.Jm link.B link.Tc link.Tc r+a r+a r+a r+a r+a r a link.dh link.dyn link.qlim link.offset r+a r+a r+a r+a Returns angolo α distanza a angolo θ distanza D tipo di giunto, 0 se rotazionale, 1 se prismatico tipo di giunto, ’R’ se rotazionale, ’P’ se prismatico convenzione di D-H, 0 se standard, 1 se modificata matrice 3 x 3 simmetrica di inerzia assegnazione della matrice di inerzia da una matrice 3 x 3, o dal vettore [Ixx Iyy Izz Ixy Iyz Ixz ] massa del link vettore 3 x 1 del link COG gear ratio inerzia del motore attrito viscoso attrito di Coulomb, vettore 1 x 2 [τ + τ − ] attrito di Coulomb, scalare per attrito simmetrico, è un vettore 1 x 2 in caso di attrito asimmetrico righe della matrice di D-H righe della matrice DYN valori limite per le variabili di giunto, vettore 1 x 2 offset delle coordinate giunto (vedere oggetto robot) Tabella E.1: Parametri dell’oggetto link (r = lettura, a = scrittura). I valori di default previsti per l’oggetto link sono: metodo di D-H classico (standard), attrito, massa ed inerzia pari a zero. Tutti i parametri dell’oggetto link sono elencati e descritti in tabella E.1. Appendice E Librerie ed oggetti del Robotics Toolbox 174 § E.2 robot E.2 robot • Tipologia: oggetto robot. • Sintassi comando: r r r r r = = = = = robot robot(rr) link(link ...) robot(DH ...) robot(DYN ...) • Descrizione: La funzione robot() genera l’oggetto robot. La prima forma della sintassi di comando ritorna l’oggetto robot di default, la seconda forma, invece, crea un nuovo oggetto robot con gli stessi valori dell’oggetto robot (rr) fornito come argomento della funzione. Il comando r = robot(link...) genera il robot tramite le informazioni relative agli oggetti link argomenti della funzione. Gli ultimi due comandi creano l’oggetto robot rispettivamente, dalla matrice di Denavit-Hartenberg (D-H) e dalla matrice n x 20 (n = numero assi del manipolatore) indicata nel RT come matrice DYN. Tutti i parametri dell’oggetto robot sono elencati e descritti nella tabella E.2. I valori di default previsti per i suddetti parametri sono elencati in tabella E.3. Appendice E Librerie ed oggetti del Robotics Toolbox 175 § E.2 robot Method robot.n robot.link link.name robot.manuf robot.comment robot.gravity robot.mdh robot.base robot.tool robot.dh robot.dyn robot.q robot.qlim robot.islimit robot.offset robot.lineopt robot.shadowopt Operations r r+a r+a r+a r+a r r r+a r+a r r r+a r+a r r+a r+a r+a Returns numero di giunti del robot array di oggetti link nome robot produttore del robot commenti generali sul robot vettore forza di gravità convenzione di D-H, 0 se standard, 1 se modificata matrice di Denavit-Hartenberg legacy DYN matrix coordinate dei giunti valori limite per le variabili di giunto, vettore 1 x 2 offset coordinate di giunto stile linea per la rappresentazione grafica del link stile linea shadow links Tabella E.2: Parametri dell’oggetto robot (r = lettura, a = scrittura). Parametro robot.name robot.manuf link.comment robot.gravity robot.offset robot.base robot.tool robot.lineopt robot.shadowopt Valore di default ’noname’ ” ” [0, 0, 9.81]m/s2 ones(n,1) eye(4,4) eye(4,4) ’Color’, ’black’, ’Linewidht’, 4 ’Color’, ’black’, ’Linewidht’, 1 Tabella E.3: Valori di default dei parametri dell’oggetto robot. Appendice E Librerie ed oggetti del Robotics Toolbox 176 § E.3 dh E.3 dh • Tipologia: rappresentazione matriciale della cinematica del manipolatore. • Descrizione: La matrice dh descrive la cinematica del manipolatore usando la rappresentazione classica proposta da Denavit-Hartenberg. Ogni riga della matrice corrisponde ad un link del robot, mentre i valori nelle colonne sono assegnati in accordo con la tabella E.4. Colonna 1 2 3 4 5 Simbolo αi Ai θi Di σi Descrizione angolo di twist del link i lunghezza di twist del link i angolo di giunto distanza di giunto tipo di giunto; 0 se rotazionale, 1 se prismatico Tabella E.4: Elementi della matrice di Denavit-Hartenberg (D-H). Se i valori della quinta colonna non vengono specificati, la funzione assume di default che i giunti siano tutti rotazionali. Per un manipolatore a n-assi, dh corrisponde a una matrice n x 4 o n x 5. Appendice E Librerie ed oggetti del Robotics Toolbox 177 § E.4 dyn E.4 dyn • Tipologia: rappresentazione matriciale della cinematica e dinamica del manipolatore. • Descrizione: La matrice dyn descrive la cinematica e la dinamica di un manipolatore usando la rappresentazione classica proposta da Denavit-Hartenberg. Ogni riga della matrice corrisponde ad un link del robot, mentre i valori nelle colonne sono assegnati in accordo con la tabella E.5. Colonna 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 Simbolo αi Ai θi Di σi mass rx ry rz Ixx Iyy Izz Ixy Iyz Ixz Jm G B Tc+ Tc- Descrizione angolo di twist del link i lunghezza di twist del link i angolo di giunto distanza di giunto tipo di giunto; 0 se rotazionale, 1 se prismatico massa del link i coordinata x del COG del link i coordinata y del COG del link i coordinata z del COG del link i elementi matrice di inerzia inerzia del motore di attuazione del giunto rapporto di riduzione del motore attrito viscoso, riferito al motore attrito di Coulomb (rotazione positiva) attrito di Coulomb (rotazione negativa) Tabella E.5: Elementi della matrice dyn. Per un manipolatore a n-assi, dyn corrisponde a una matrice n x 20. Appendice E Librerie ed oggetti del Robotics Toolbox 178 § E.5 robot/plot E.5 robot/plot • Tipologia: plot e animazione grafica 3D del movimento del robot. • Sintassi comando: plot(robot, q) plot(robot, q, arguments...) • Descrizione: La funzione plot (fornita nel RT) consente di visualizzare a schermo il robot nella configurazione data dal vettore q in input, oppure se q è una matrice che descrive la traiettoria del manipolatore nello spazio dei giunti, la funzione mostra a video l’animazione grafica in 3D della traiettoria percorsa dal manipolatore. Figura E.1: Plot del manipolatore Puma 560. Appendice E Librerie ed oggetti del Robotics Toolbox 179 § E.6 rne E.6 rne • Tipologia: Calcolo della dinamica inversa e diretta del manipolatore. • Sintassi comando: tau = rne(robot, q, qd, qdd) tau = rne(robot, [q, qd, qdd]) tau = rne(robot, q, qd, qdd, grav) tau = rne(robot, [q, qd, qdd], grav) tau = rne(robot, q, qd, qdd, grav, fext) tau = rne(robot, [q, qd, qdd], grav, fext) • Descrizione: La funzione rne (recursive Newton-Euler) restituisce il vettore delle coppie di giunto come funzione dei vettori di posizione (q), velocità (qd) ed accelerazione (qdd) di giunto, consentendo quindi il calcolo della dinamica inversa del manipolatore. L’algoritmo di calcolo usato è basato sulla formulazione ricorsiva di Newton-Eulero, descritta nel capitolo 1. La funzione rne ritorna un vettore (tau) se q, qd e qdd sono anch’essi vettori, restituisce invece una matrice (tau) se q, qd e qdd sono matrici. In quest’ultimo caso, i valori sulla riga i-esima della matrice tau, sono le coppie di giunto calcolate in corrispondenza ai valori della riga i-esima delle matrici q, qd e qdd. Grav definisce la direzione del vettore di accelezione di gravità. Fext consente invece di definire l’azione di forze/momenti agenti sull’effettore del robot. In particolare fext è un vettore 1 × 6: fext = [Fx Fy Fz Mx My Mz] in cui tutti gli elementi sono espressi rispetto al sistema di riferimento solidale con l’effettore del manipolatore. Il calcolo delle coppie di giunto può essere eseguito senza trascurare i contributi relativi all’inerzia del motore di attuazione e agli attriti di giunto, che possono essere indicati dall’utente come parametri nella matrice dyn. Se le equazioni del moto sono ottenute con la Appendice E Librerie ed oggetti del Robotics Toolbox 180 § E.6 rne formulazione di Newton-Eulero (si veda il capitolo 1, §1.5.3) è possibile calcolare la dinamica diretta del manipolatore con un metodo che risulta molto efficiente dal punto di vista computazionale: −1 0 q̈ = B (q) τ − τ (q, _ q) (E.1) dove B(q) è la matrice generalizzata d’inerzia che viene restituita dalla funzione rne se invocata con la seguente sintassi: n = robot.n B = rne(robot, ones(n, 1) ∗ q0 , zeros(n, n), eye(n), [0, 0, 0]) 0 τ (q, _ q) è il vettore delle coppie di giunto restituito da: 0 τ (q, _ q) = rne(robot, q0 , qd0 , zeros1, n) Il calcolo dei vettori qd e q si ottiene semplicemente integrando le equazioni fornite dalla (E.1). L’efficienza di calcolo del suddetto metodo è dovuta ad una versione C-MEX (matlab executable file) della funzione rne che risulta essere fino a 500 volte più rapida della corrispondente versione M file appena descritta. La dinamica diretta, infatti, richiede ad ogni passo di integrazione di valutare la matrice generalizzata d’inerzia B(q). Questo calcolo risulta molto complesso poiché il computer deve valutare n+1 la funzione rne (n = numero di giunti del robot). Dunque nel calcolo della dinamica diretta la versione .m della funzione rne costituisce il cosiddetto collo di bottiglia computazionale, bottleneck, in quanto richiede tempi di simulazioni troppo lunghi e non compatibili con la dinamica dei modelli simulati. La funzione C-MEX rne, invece, una volta compilata risolve questo inconveniente, perché riesce ad ogni passo di integrazione e con tempi di campionamento dell’ordine del centesimo di secondo a: 1) accedere ai valori correnti q, qd, τ ed ai valori dei parametri cinematici ed inerziali del robot (memorizzati nell’apposito matlab robot object); 2) calcolare la dinamica diretta del manipolatore robotico scelto; 3) rilasciare i valori delle variabili e dei parametri usati per il calcolo. Appendice E Librerie ed oggetti del Robotics Toolbox 181 § E.7 Puma 560 E.7 Puma 560 Con riferimento alla figura E.2 e alla tabella E.6 si analizza in dettaglio il codice MATLAB, scritto nel file puma560.m del Robotics Toolbox, che consente di definire il manipolatore Puma 560 prodotto dalla Unimation. Al fine di rendere più comprensibile la lettura del codice, gli step di definizione del robot, vengono descritti uno alla volta, con il corrispondente codice: Figura E.2: Robot Puma 560 con sistemi di riferimento per la definizione dei parametri di Denavit-Hartenberg (figura con angoli di giunto tutti pari a zero). 1. Si cancella l’oggetto link L se già esistente: clear L 2. Si caricano i parametri della tabella di D-H tramite l’oggetto link: Appendice E Librerie ed oggetti del Robotics Toolbox 182 § E.7 Puma 560 Link/giunto 1 2 3 4 5 6 αi ai π/2 0 0 0.4318 −π/2 0.0203 π/2 0 −π/2 0 0 0 θi θ1 θ2 θ3 θ4 θ5 θ6 di 0 0 0.15005 0.4318 0 0 Tabella E.6: Tabella dei parametri di D-H per il robot Puma 560. L{1} L{2} L{3} L{4} L{5} L{6} = = = = = = link([ pi/2 0 0 0 0], ’standard’); link([ 0 .4318 0 0 0], ’standard’); link([-pi/2 .0203 0 .15005 0], ’standard’); link([pi/2 0 0 .4318 0], ’standard’); link([-pi/2 0 0 0 0], ’standard’); link([0 0 0 0 0], ’standard’); 3. Si definisce la massa di ogni link del robot: L{1}.m L{2}.m L{3}.m L{4}.m L{5}.m L{6}.m = = = = = = 0; 17.4; 4.8; 0.82; 0.34; .09; 4. Si definiscono per i = 1...n (n = numero di assi del robot) l’offset del baricentro del link i rispetto all’origine del sistema di riferimento solidale al giunto i: L{1}.r L{2}.r L{3}.r L{4}.r L{5}.r L{6}.r = = = = = = [ [ [ [ [ [ 0 0 0 ]; -.3638 .006 .2275]; -.0203 -.0141 .070]; 0 .019 0]; 0 0 0]; 0 0 .032]; 5. Si definisce la matrice simmetrica di inerzia corrispondente a ciascun link: L{1}.I = [ 0 0.35 0 0 Appendice E Librerie ed oggetti del Robotics Toolbox 0 0]; 183 § E.7 Puma 560 L{2}.I L{3}.I L{4}.I L{5}.I L{6}.I = = = = = [ [ [ [ [ .13 .524 .539 0 .066 .086 .0125 0 1.8e-3 1.3e-3 1.8e-3 .3e-3 .4e-3 .3e-3 .15e-3 .15e-3 .04e-3 0 0]; 0 0]; 0 0 0]; 0 0 0]; 0 0 0]; 6. Viene definita l’inerzia (Jm ) e il rapporto di riduzione (G) del motore di attuazione di ciascun giunto del robot: L{1}.Jm L{2}.Jm L{3}.Jm L{4}.Jm L{5}.Jm L{6}.Jm L{1}.G L{2}.G L{3}.G L{4}.G L{5}.G L{6}.G = 200e-6; = 200e-6; = 200e-6; = 33e-6; = 33e-6; = 33e-6; = = = = = = -62.6111; 107.815; -53.7063; 76.0364; 71.923; 76.686; 7. Vengono definiti i coefficienti di attrito viscoso e di Coulomb (valori riferiti ai motori di attuazione di ciascun giunto): \% viscous L{1}.B = L{2}.B = L{3}.B = L{4}.B = L{5}.B = L{6}.B = friction (motor referenced) 1.48e-3; .817e-3; 1.38e-3; 71.2e-6; 82.6e-6; 36.7e-6; \% Coulomb friction L{1}.Tc = [ .395 L{2}.Tc = [ .126 L{3}.Tc = [ .132 L{4}.Tc = [ 11.2e-3 L{5}.Tc = [ 9.26e-3 L{6}.Tc = [ 3.96e-3 (motor referenced) -.435]; -.071]; -.105]; -16.9e-3]; -14.5e-3]; -10.5e-3]; 8. Vengono definite alcune configurazioni spaziali caratteristiche per il robot: Appendice E Librerie ed oggetti del Robotics Toolbox 184 § E.7 Puma 560 qz = [0 0 0 0 0 0]; \% zero angles, L shaped pose qr = [0 pi/2 -pi/2 0 0 0]; \% ready pose, arm up qstretch = [0 0 -pi/2 0 0 0]; 9. Si definisce infine l’oggetto matlab robot, con il nome p560, che contiene tutte le informazioni registrate nell’oggetto link L: p560 = robot(L, ’Puma 560’, ’Unimation’, ’params of 8/95’); clear L p560.name = ’Puma 560’; p560.manuf = ’Unimation’; A questo punto è sufficiente digitare puma560 nella Command Window del MATLAB per caricare l’oggetto definito nel file puma560.m (vedi figura E.3). Figura E.3: Snapshot del caricamento dell’oggetto robot Puma 560. Appendice E Librerie ed oggetti del Robotics Toolbox 185 Appendice F Collegamenti dei componenti hardware del sistema HIL In questa Appendice vengono riportati lo schema (vedi figura F.1) e le tabelle di connessione delle interfacce Phoenix Contact VARIOFACE con i dispositivi hardware che compongono il sistema HIL. Le tabelle riportano in alto il nome del tipo di segnali di cui caratterizzano le connessioni elettriche: 1) segnali analogici di comando; 2) segnali di velocità corretti; 3) segnali di home e fine corsa; 4) segnali encoder in fase e quadratura. All’interno di ciascuna tabella ogni riga rappresenta una connessione elettrica e specifica la corrispondente funzione Matlab real-time svolta. In figura F.1 è stato anche specificato se le morsettiere sono collegate con il Sistema Lento (S.L.) (vedi figura 1), con l’Hardware di Controllo Assi e/o Numerico (H.C.) (vedi figura 1) o con entrambi (S.L./H.C.). SEGNALI DI HOME E FINE CORSA Matlab DIO PIN VARIOFACE E PIN VARIOFACE B PIN VARIOFACE C DIO 24 1 8 DIO 23 3 17 DIO 22 5 26 DIO 21 7 8 GROUND tutti i pin pari di VARIOFACE E Tabella F.1: Tabella dei collegamenti per i segnali di Home e fine corsa (DIOn = Digital I/O numero). 186 SEGNALI ANALOGICI DI COMANDO Matlab DAC PIN VARIOFACE D PIN VARIOFACE A DAC 1 42 6 DAC 2 44 8 DAC 3 46 10 DAC 4 48 12 GROUND 40 5,7,9,11 Tabella F.2: Tabella dei collegamenti per i segnali analogici di comando (DACn = Digital Analog Converter numero). SEGNALI ANALOGICI DI VELOCITÀ CORRETTI Matlab DAC PIN VARIOFACE A PIN VARIOFACE F DAC 1 42 4 DAC 2 44 5 DAC 3 46 10 DAC 4 48 37 GROUND 40 14 Tabella F.3: Tabella dei collegamenti per i segnali analogici di velocità corretti (DACn = Digital Analog Converter numero). SEGNALI ENCODER IN FASE E QUADRATURA PIN VARIOFACE F PIN VARIOFACE B e C (B/C) Matlab Encoder FASE QUAD. FASE QUAD. 1 2 3 2(B) 5(B) 2 6 7 11(B) 14(B) 3 34 33 20(B) 23(B) 4 30 29 2(C) 5(C) GROUND 40 3(B),9(B),21(B),3(C) Tabella F.4: Tabella dei collegamenti per i segnali encoder in fase e quadratura. Appendice F Collegamenti dei componenti hardware del sistema HIL 187 Figura F.1: Schema di collegamento delle interfacce VARIOFACE con i blocchi Sistema Lento, Sistema Veloce e Hardware di Controllo Assi e/o Numerico (si veda figura 1). Appendice F Collegamenti dei componenti hardware del sistema HIL 188 Bibliografia c [1] “dsPIC30F Family Reference Manual”. 2006 Microchip Technology Inc. [2] “dsPICDEM T M 2 Development Board User’s Guide”. c 2005 Microchip Technology Inc. [3] “Instruction manual Sensoray Model 626 PCI Multifunction I/O Board, c Revision F January 28, 2004”. Copyright SENSORAY. c [4] “MPLABr C30 C Compiler User’s Guide”. 2005 Microchip Technology Inc. c [5] “MPLABr IDE User’s Guide”. 2006 Microchip Technology Inc. c [6] “Real-Time Windows Target User’s Guide”. COPYRIGHT 1999-2004 by The MathWorks, Inc. c [7] “Real-Time Workshop User’s Guide”. COPYRIGHT 1994-2007 by The MathWorks, Inc. c [8] “Target Language Compiler Reference Guide”. COPYRIGHT 1999-2002 by The MathWorks, Inc. c [9] “Using Simulink”. COPYRIGHT 1990-2007 by The MathWorks, Inc. c [10] “Writing S-Functions”. COPYRIGHT 1999-2007 by The MathWorks, Inc. [11] W.M. Armstrong [1979]. “Recursive Solution to the Equations of Motions of an N-link Manipulator”. Proc. 5th World Congr., Theory of Machines, Mechanisms, vol. 2, pp. 1343-1346. 189 BIBLIOGRAFIA [12] P.I. Corke. A robotics toolbox for MATLAB. IEEE Robotics and Automation Magazine, 3(1):24–32, March 1996. [13] J.J. Craig. “Introduction to Robotics”. Addison Wesley, 1986. [14] J. Denavit and R.S. Hartenberg [1955]. “A Kinematic Notation for Lower-Pair Mechanisms Based on Matrices”. J. App. Mech., vol. 77, pp. 215-221. [15] King-Sun Fu, Rafael C. Gonzalez, and C.S. George Lee. “Robotica”. Copyright c 1989 McGraw-Hill Libri Italia srl. [16] J.Y.S. Luh, M.W. Walker, and R.P. Paul [1980]. “On-Line Computational Scheme for Mechanical Manipulators”. Trans. ASME, J. Dynamic Systems, Measurements and Control, vol. 120, pp. 69-76. [17] D.E. Orin, R.B. McGhee, M. Vukobratovic, and G. Hartoch [1979]. “Kinematic and Kinetic Analysis of Open-Chain Linkages Utilizing Newton-Euler Methods”. Math. Biosci., vol. 43, pp. 107-130. [18] L. Sciavicco and B. Siciliano. “Robotica Industriale”. c Copyright 2000 McGraw-Hill Libri Italia srl. [19] R. De Simone. “Azionamento di motori in corrente continua tramite microcontrollori”. Tesi di laurea Ingegneria Automazione, Università degli studi di Roma “Tor Vergata”. [20] A. Tornambè, S. Nicosia, and P. Valigi. “Experimental results in state estimation of industrial robots”. Procedings of the 29th IEEE conference on decision and control, 1990. [21] M.W. Walker and D.E. Orin [1982]. “Efficient Dynamic Computer Simulation of Robotic Mechanisms”. Trans. ASME, J. Systems, Measurements and Control, vol. 104, pp. 205-211. BIBLIOGRAFIA 190