Indice - Corso di Studi in Ingegneria dell`Automazione

Transcript

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
Generalità 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 velocità 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 Generalità 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 è 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 è 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 è
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 è 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 è quello che nasce dall’unione tra la robotica e l’automazione.
L’automazione è 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 è 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 unità di controllo elettronico che vengono collegate ad appositi banchi di prova che riproducono in modo più o meno completo, fisicamente
e/o in software, il prodotto finale a cui sono destinate le unità 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 è la realizzazione di un simulatore real-time di
manipolatori robotici con sensori interfacciati tramite segnali encoder, cioè di un
sistema che rappresenta una soluzione di tipo Hardware-in-the-loop (HIL). Scopo
di questo simulatore è consentire di anticipare le verifiche sulle funzionalità e prestazioni di schede di controllo assi, già nella fase di progettazione e prototipazione,
senza attendere la disponibilità 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,
poiché 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 più 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 unità (o sistemi) di calcolo (si veda figura 1):
1. Unità 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 (cioè la realtà è più lenta della realtà simulata).
2. Unità veloce, costituita da un microcontrollore con opportuno programma
di calcolo (Sistema Veloce in figura 1), che riceve i segnali di velocità su
ogni asse dall’unità lenta e sulla base di questi produce i segnali encoder
in fase e quadratura, riuscendo a garantire le necessarie frequenze, perché
l’onere di calcolo legato alle equazioni matematiche del modello del sistema
è demandato all’unità lenta.
La sincronia tra le due unità di calcolo sopra citate è 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 velocità scambiati tra
l’unità 1 e l’unità 2 coincida con la posizione simulata all’interno dell’unità 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 velocità degli organi in movimento. La necessità di
aggiungere un secondo sistema, il Sistema Veloce, nasce dall’incapacità, 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 funzionalità 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 funzionalità 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
funzionalità implementabile sulla base della misura dei segnali encoder (si pensi a
trasmissione remota di dati, verifica di proprietà di compatibilità della configurazione dal punto di vista della sicurezza, dell’interazione con altri dispositivi meccanici,
di valutazione di parametri di prestazione, etc). Le funzionalità 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 è 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 è 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’è un microcontrollore e
in che modo è 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 è 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 è
pertanto la presentazione di due metodi che consentono la derivazione delle equazioni del moto di un manipolatore nello spazio dei giunti. Il primo metodo è basato
sulla formulazione di Lagrange, concettualmente semplice e sistematica, che conduce alla derivazione del modello dinamico in forma chiusa. Il secondo metodo è
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, dà come risultato una formulazione algoritmica conveniente e compatta delle equazioni di moto del manipolatore. L’algoritmo viene espresso tramite
operazioni matriciali e ciò semplifica sia l’analisi che l’implementazione su calcolatore.
La derivazione delle equazioni dinamiche di un manipolatore a n gradi di libertà è
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, cioè 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 perché 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 velocità di ogni giunto. Nel seguito dopo aver proposto un richiamo teorico
per introdurre una formulazione algoritmica delle equazioni dinamiche, verrà 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
può essere verificata con semplici considerazioni geometriche di composizione di
vettori:
q0 = 0 R1 q1 + d
(1.1)
13
Figura 1.1: Rappresentazione di un punto in diversi sistemi di coordinate.
dove 0 R1 è 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 più 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 più 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 può 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 più generale per la trasformazione
omogenea è data dalla seguente formula:
ω 1 q1
R d
ω 0 q0
=
ω1
f s
ω0
(1.2)
dove R ∈ R3x3 è la matrice di rotazione, d ∈ R3 è il vettore di posizione, f ∈ R1x3 è il
vettore di trasformazione prospettica e s ∈ R è 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).
14
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, può 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 può 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 libertà. Dunque, per un manipolatore con N gradi di libertà ci
15
sono N coppie giunto-link con il link 0 (non considerato parte del robot) ancorato
a una base di appoggio dove di norma è stabilito un sistema di coordinate inerziale;
all’ultimo link è 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 è connesso al più a due altri link perché 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 avrà 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, è che essi mantengono una configurazione fissa tra i loro giunti che
può essere caratterizzata da due parametri ai , αi . Il parametro ai rappresenta la
distanza più breve misurata lungo la normale comune tra gli assi dei giunti (cioè
gli assi zi−1 e zi rispettivamente per i giunti i e i+1 ), e αi è 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.
16
Dato l’asse zi , la posizione relativa dei due link adiacenti (i−1) e i è data da di , che
è 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 è 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 può 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 sarà 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-
17
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’estremità dell’effettore, espresso nell’ultimo sistema di coordinate, può
essere trasformata nelle ’coordinate di base’ che costituiscono il sistema di riferimento inerziale di questo sistema dinamico.
Come già accennato, il robot è 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 libertà 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 può 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 ;
18
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)
Poichè la (1.9) è di valore generale, essa può 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;
19
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 libertà. 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)
è 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 è quello di determinare la relazione tra
velocità ai giunti e velocità lineare e angolare dell’organo terminale. In altri termini,
si vogliono esprimere come vettori liberi la velocità lineare d˙ e la velocità angolare
ω dell’organo terminale in funzione delle velocità delle variabili di giunto mediante
relazioni del tipo:
d˙ = Jp (q)q̇
(1.11)
ω = Jω (q)q̇
(1.12)
Nella (1.11) Jp è la matrice (3 × N ) relativa al contributo delle velocità dei giunti
alla velocità lineare d˙ dell’organo terminale, mentre nella (1.12) Jω è la matrice
(3 × N ) relativa al contributo delle velocità dei giunti alla velocità 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 ) è lo Jacobiano geometrico del manipolatore, che in generale risulta funzione delle variabili di giunto. Per effettuare il calcolo dello Jacobiano geometrico è
opportuno richiamare delle proprietà delle matrici di rotazione e alcuni importanti
risultati di cinematica dei corpi rigidi.
20
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 è quello di caratterizzare velocità lineare e angolare dell’organo
terminale, è 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
proprietà di ortogonalità di R si ha la relazione:
R(t)RT (t) = I
che, derivata rispetto al tempo, fornisce l’identità:
Ṙ(t)RT (t) + R(t)ṘT (t) = 0
Posto:
S(t) = Ṙ(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:
Ṙ(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:
ṗ(t) = Ṙ(t)p0
21
che per la (1.16) può scriversi:
ṗ(t) = S(t)R(t)p0
Se con il vettore ω(t) si indica la velocità angolare della terna R(t) all’istante di
tempo t rispetto alla terna di riferimento, dalla meccanica è noto che:
ṗ(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) è 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 può dimostrare che vale la
seguente relazione:
RS(ω)RT = S(Rω)
(1.18)
che tornerà 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)
è il vettore delle variabili di giunto. Poiché d˙0i =
cioè:
∂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



(1.20)
22
con i = 1 . . . N . Per il calcolo di Jωi si avrà 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̇)) = Ṙ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 velocità 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 perché
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 può 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:
23
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 può scrivere:
P = QP 0 ⇒ p = Rp0 + d
(1.23)
derivando la (1.23), si ottiene:
ṗ = Ṙp0 + d˙ = SRp0 + d˙ ⇒ ṗT = p0T RT S T + d˙T
da cui si può scrivere l’energia cinetica associata al corpo rigido come:
Z
Z
1
1
T
T = ρ ṗ ṗ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 può 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 può 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
(1.28)
24
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), cioè 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 può 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
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 può essere ottenuta sommando le energie
potenziali di tutti i link:
U=
N
X
Ui
(1.33)
i=1
dove g è la costante gravitazione diretta lungo l’asse z ed Mi è 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, velocità ed accelerazione per ogni dato punto della traiettoria, costituisce
un collo di bottiglia computazionale per un controllo real-time. Il problema è 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, è stato inizialmente proposto un modello dinamico semplificato che
ignora le forze centrifughe e quelle di Coriolis. Ciò riduce il tempo necessario al
calcolo dei momenti dei giunti a un valore accettabile. Le forze centrifughe e quelle
di Coriolis sono però significative per i momenti dei giunti se il manipolatore si
muove ad alta velocità. Ne consegue che questa dinamica semplificata del robot
limita la velocità di movimento di ciascun link e ciò non è 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
velocità, a causa degli eccessivi momenti correttivi richiesti.
Per superare questi inconvenienti e per trovare equazioni del moto più efficaci, molti ricercatori si sono rivolti alla seconda legge di Newton, e hanno sviluppato varie
26
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 più importante e significativa di questa formulazione risiede nel fatto
che il tempo di calcolo dei momenti applicati può essere notevolmente ridotto per
permettere un controllo in tempo reale del manipolatore robotico. La derivazione
è 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
estenderà 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 può 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
27
Figura 1.4: Sistema di coordinate rotazionali.
Si vuole ora valutare la derivata del punto r e, poiché i sistemi di coordinate
ruotano uno rispetto all’altro, la derivata rispetto al tempo di r(t) potrà 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) può essere espressa
come:
di
dj
dk
dr
= ẋi + ẏj + żk + x + y + z
dt
dt
dt
dt
= ẋi + ẏj + żk
(1.38)
e usando la (1.35), la derivata asteriscata di r(t) è:
d∗ i*
d∗ j*
d∗ k*
d∗ r
= ẋ∗ i* + ẏ ∗ j* + ż ∗ k* + x∗
+ y∗
+ z∗
dt
dt
dt
dt
∗
∗
∗
= ẋ i* + ẏ j* + ż k*
(1.39)
28
Facendo uso delle equazioni (1.35) e (1.39) la derivata rispetto al tempo di r(t) si
può esprimere come:
dr
di*
dj*
dk*
= ẋ∗ i* + ẏ ∗ j* + ż ∗ 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 difficoltà nel calcolare:
di*
,
dt
dj*
,
dt
dk*
dt
perché 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) può presentare
una variazione nel tempo non nulla solo se esso cambia direzione. Infatti a seguito di
una traslazione, cioè 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 proprietà (modulo, direzione e verso): pertanto la derivata temporale di
→
−
un vettore che trasla è nulla. Nel seguito viene considerato il caso di un vettore A
−
di modulo costante che ruota con velocità 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 , sicché la sua variazione infinitesima d A
nel tempo dt è data da un vettore di modulo dA = A · sinθ · ωdt, essendo θ l’angolo
→
−
→
−
−
tra →
ω ed A . Il vettore infinitesimo d A può scriversi pertanto nella forma:
→
−
→
−
−
dA = →
ω × A dt
da cui si ottiene:
→
−
→
−
dA
−
=→
ω ×A
dt
(1.41)
Tale risultato può anche applicarsi a tutti i versori, in particolare a quelli i*, j*, k*
−
di un sistema di assi cartesiani OX ∗ Y ∗ Z ∗ ruotante a velocità angolare →
ω , rispetto
ad un osservatore fisso, intorno ad un asse passante per O. In tal caso si ottengono
29
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), è 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
(1.43)
30
Quest’ultima è 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) è il ragionamento essenziale del teorema di Coriolis. Il primo termine
della parte destra dell’equazione è l’accelerazione relativa al sistema di coordinate
asteriscato. Il secondo termine è chiamato accelerazione di Coriolis. Il terzo termine è detto accelerazione centripeta (diretta verso il centro) di un punto rotante
intorno a un asse. Si può verificare che ω × (ω × r) punta direttamente verso l’asse
di rotazione ed è perpendicolare a esso. L’ultimo termine diventa zero per una
velocità 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 è un sistema inerziale. Una particella p di massa m è individuata
dai vettori r∗ ed r∗ rispetto alle origini dei suddetti sistemi di coordinate. L’origine
O∗ è individuata dal vettore h rispetto all’origine O. La relazione tra i vettori r ∗
ed r∗ è data da (vedi figura 1.6 ):
r = r∗ + h
(1.45)
31
Figura 1.6: Sistema di coordinate mobile.
Se il sistema di coordinate asteriscato O ∗ X ∗ Y ∗ Z ∗ è mobile (cioè 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 velocità della particella mobile p rispettivamente
in relazione al sistema di coordinate O ∗ X ∗ Y ∗ Z ∗ e OXY Z, e vh è la velocità 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 può 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 è:
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 è 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) può essere espressa come:
a(t) =
d ∗ r∗
dω
d2 h
d∗2 r∗
∗
∗
+
2ω
×
+
ω
×
(ω
×
r
)
+
×
r
+
dt2
dt
dt
dt2
(1.49)
32
§ 1.5 Cinematica dei link
Avvalendosi di questa introduzione ai sistemi di coordinate mobili, si può 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 è 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 è stabilito un sistema di coordinate ortonormale (xi−1 , yi−1 , zi−1 ).
Il sistema di coordinate (x0 , y0 , z0 ) è 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 è 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 ∗ è individuata da un vettore posizione pi−1 dall’origine O rispetto al
sistema di coordinate fondamentale.
Siano vi−1 e ωi−1 le velocità 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 velocità
0
angolari di O rispetto a (x0 , y0 , z0 ) e a (xi−1 , yi−1 , zi−1 ) rispettivamente. Allora
la velocità lineare vi e la velocità 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∗
(1.51)
33
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 ) è:
ω̇i∗ =
d∗ ωi∗
+ ωi−1 × ωi∗
dt
(1.54)
dunque, la (1.53) si può esprimere come:
ω̇i = ω̇i−1 +
d∗ ωi∗
+ ωi−1 × ωi∗
dt
(1.55)
34
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 è traslazionale nel sistema di coordinate (xi−1 , yi−1 , zi−1 ) esso si sposterà nella
direzione di zi−1 con una velocità del giunto q̇i relativa al link i − 1. Se il link
ruota nel sistema di coordinate (xi−1 , yi−1 , zi−1 ), esso avrà velocità angolare ωi∗ e
il movimento angolare del link i avverrà intorno all’asse zi−1 . Dunque:
zi−1 q̇i se il link i è rotazionale
∗
ωi =
0
se il link i è traslazionale
(1.56)
dove q̇i è il modulo della velocità 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 è rotazionale
=
0
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 è rotazionale
ωi =
ωi−1
ω̇i =
ω̇i−1 + zi−1 q̈i + ωi−1 × (zi−1 q̇i ) se il link i è rotazionale
ω̇i−1
(1.58)
(1.59)
Utilizzando la (1.41), la velocità lineare e l’accelerazione del link i relative a i − 1
risultano essere, rispettivamente:
∗
d∗ p∗i
ωi × p∗i se il link i è rotazionale
=
zi−1 q̇i
dt
d∗2 p∗i
=
dt2
d∗ ωi∗
dt
× p∗i + ωi∗ × (ωi∗ × p∗i ) se il link i è rotazionale
zi−1 q̈i
(1.60)
(1.61)
Dunque, avvalendosi delle equazioni (1.60) e (1.51), la velocità lineare del link i
rispetto al sistema di coordinate di riferimento è [dalla (1.50)]:
ωi × p∗i + vi−1
se il link i è rotazionale
vi =
zi−1 q̇i + ωi × p∗i + vi−1 se il link i è traslazionale
(1.62)
35
Facendo uso delle seguenti identità 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, è (dalla (1.52)):

 ω̇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
(1.65)
Si noti che ωi = ωi−1 se nella (1.62) il link i è traslazionale. Le equazioni (1.58),
(1.59), (1.62) e (1.65) descrivono l’informazione cinematica del link i che è 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 è valido in tutti gli istanti di tempo. È effettivamente una forma
leggermente modificata della seconda legge del moto di Newton, e può essere cosı̀
enunciato:
Per ogni corpo, la somma algebrica delle forze applicate esternamente e di quelle
che oppongono resistenza al moto in qualunque direzione è 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:
36
• 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
= velocità lineare del baricentro del link i;
• ā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 āi
dt
(1.66)
d(Ii ωi )
= Ii ω̇i + ωi × (Ii ωi )
dt
(1.67)
Fi =
e
Ni =
37
Figura 1.8: Forze e momenti sul link i.
dove, utilizzando le equazioni (1.62) e (1.65), la velocità lineare e l’accelerazione
del baricentro del link i sono, rispettivamente (in questo caso (xi , yi , zi ) è 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 gravità e dai link contigui, i − 1 e i + 1. Cioè:
Fi = fi − fi+1
(1.70)
38
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 ā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 è rotazionale, allora
esso ruoterà 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 è la somma delle proiezioni di ni sull’asse zi−1 e del momento di smorzamento viscoso in quel sistema
di coordinate. Però, se il giunto i è traslazionale, allora esso traslerà di q i unità
relativamente al sistema di coordinate (xi−1 , yi−1 , zi−1 ) lungo l’asse zi−1 . Perciò,
la forza τi applicata a quel giunto è 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 è:
T
ni zi−1 + bi q̇i se il link i è rotazionale
τi =
fTi zi−1 + bi q̇i se il link i è traslazionale
(1.75)
39
dove bi è il coefficiente di smorzamento viscoso per il giunto i nelle equazioni
precedenti. Se la base di appoggio è fissata saldamente alla piattaforma e il link 0
è stazionario, allora sarà ω0 = ω̇0 = 0, v0 = 0 e v̇0 (per includere la gravità) è:


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 velocità e l’accelerazione lineare e angolare di ogni link
si propagano dal sistema di riferimento fondamentale (coincidente con la base di
appoggio) all’estremità dell’effettore. Per le equazioni ricorsive inverse, i momenti
e le forze esercitati su ogni link sono calcolati in modo ricorsivo dall’estremità
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 può essere applicato
sequenzialmente al braccio del robot. La ricorsività diretta propaga l’informazione
cinematica: le velocità angolari, le accelerazioni angolari e le accelerazioni lineari
dal sistema di riferimento fondamentale (sistema inerziale) all’estremità dell’effettore. La ricorsività inversa trasmette invece le forze esercitate su ciascun link
dall’estremità 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 )
40
Equazioni dirette: i = 1, 2, ..., n
ωi =
ωi−1 + zi−1 q̇i se il link i è rotazionale
ωi−1
ω̇i =
ω̇i−1 + zi−1 q̈i + ωi−1 × (zi−1 q̇i ) se il link i è rotazionale
ω̇i−1

 ω̇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
ai = ω̇i × s̄i + ωi × (ωi × s̄i ) + v̇i
Equazioni inverse: i = n, n − 1, ..., 1
.
Fi = mi ā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 è rotazionale
fTi zi−1 + bi q̇i se il link i è traslazionale
Dove bi è 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
(perché sia compresa anche la gravità), 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 velocità, accelerazioni,
matrici inerziali, posizione del baricentro di ogni link, e i momenti/forze ai sistemi
di coordinate di ogni link. In virtù della natura della formulazione e del meto-
41
do per calcolare i momenti dei giunti, i calcoli si semplificano notevolmente. La
conseguenza più importante di questa modificazione è che il tempo di calcolo dei
momenti applicati è linearmente proporzionale al numero dei giunti del braccio
ed è indipendente dalla sua configurazione. Ciò 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 è
la sottomatrice 3 × 3 superiore sinistra della matrice di trasformazione omogenea
i−1
Ai . É 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 , ā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 ā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 è rotazionale
i
R0 ωi = i i−1 i−1 0 i−1
Ri−1 ( R0 ωi−1 )
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 è rotazionale
i
Ri−1 (i−1 R0 ω̇i−1 )
(1.81)
42
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 )



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 )]
(1.82)
R0 ā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 ā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 è rotazionale
T
(i R0 fi ) (i Ri−1 z0 ) + bi q̇i se il link i è traslazionale
(1.89)
dove z0 = (0, 0, 1)T , i R0 s̄i è la posizione del baricentro del link i in relazione al
proprio sistema di coordinate xi , yi , zi , e i R0 p∗i è la posizione del sistema xi , yi , zi
dall’origine di xi−1 , yi−1 , zi−1 rispetto al sistema di coordinate i−esimo ed è:


ai
i
R0 p∗i =  di sinαi 
(1.90)
di cosαi
e (i R0 Ii 0 Ri ) è 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.
43
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 è rotazionale
i
Ri−1 (i−1 R0 ωi−1 )
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 è rotazionale
Ri−1 (i−1 R0 ω̇i−1 )
 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 )



i
R0 v̇i =
i
R0 ā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 )]
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 è rotazionale
T
(i R0 fi ) (i Ri−1 z0 ) + bi q̇i se il link i è traslazionale
Dove z0 = (0, 0, 1)T e bi è 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
(perché sia compresa anche la gravità), dove |g| = 9.8062 m/s2 .
Tabella 1.3: Efficienti equazioni ricorsive di moto di Newton-Eulero.
44
1.5.3
Algoritmo di calcolo
Le equazioni di moto di Newton-Eulero rappresentano oggi il sistema di equazioni
più 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 libertà del braccio.
Dato che le equazioni di moto che sono state ottenute sono per natura ricorsive, è
consigliabile considerare un approccio algoritmico per il calcolo dei momenti/forze
applicati ai giunti per tutti gli attuatori dei giunti. Un siffatto algoritmo è 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 ā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.
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’è 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:
• è 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);
• è efficace se si vogliono considerare effetti meccanici più complessi quali ad
esempio le deformazioni elastiche dei bracci.
La formulazione di Newton-Eulero presenta il seguente vantaggio fondamentale:
• è intrinsecamente un metodo ricorsivo che risulta efficiente da un punto
di vista computazionale e particolarmente adatto ad essere implementato
all’interno di un calcolatore.
46
Nello studio della dinamica di un manipolatore è 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 velocità 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 velocità 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 è 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 velocità 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 velocità q̇(tk ), si calcola l’accelerazione q̈(tk ) con la (1.91). Utilizzando poi una regola di integrazione
numerica con passo di integrazione ∆t, è possibile calcolare la velocità 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, è possibile calcolare la dinamica diretta con un metodo più 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
47
generalizzata di inerzia B(q) può 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.
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 è 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 controllabilità) 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, è 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 è 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 Generalità 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
Generalità sul Robotics Toolbox
Il RT è un toolbox per la simulazione e l’analisi dei risultati sperimentali effettuati sui robot reali. L’autore è 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 già
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 è 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
da qui è possibile scaricare la release più 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
possibilità del toolbox può 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 è 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 è 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 è necessario creare gli oggetti link con il comando
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 è 0 se il giunto è rotazionale oppure 1 se è 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 sarà 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)
Infine per visualizzare le informazioni relative al link, si può 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 è 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)
Figura 2.2: Notazione usata nel Robotics Toolbox per il calcolo della dinamica
inversa.
Se l’asse i + 1 è 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)
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 è rotazionale
T
Qi =
 if
(i Ri+1 z 0 ) se il link i + 1 è traslazionale
i
i
(2.12)
(2.13)
(2.14)
dove:
• i = indice dei link, che può variare tra 1 ed n;
• Ji = è il momento di inerzia del link i rispetto al suo baricentro;
• si = è il vettore che individua la posizione del baricentro del link i, rispetto
al sistema di riferimenti i;
• ω i = velocità angolare del link i;
• ω̇ i = accelerazione angolare del link i;
• v i = velocità lineare del sistema di riferimento i;
• v̇ i = accelerazione lineare del sistema di riferimento i;
• v̄ i = velocità 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;
•
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 è 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 (perché sia compresa anche la gravità), dove |g| = 9.8062 m/s2 .
§ 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 è 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 è 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);
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;
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.
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
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:
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. Dopodiché
chiudere la finestra di dialogo REAL-TIME S-Function Builder;
12. Selezionare Simulation −→ Configuration Parameters nel menù dello schema Simulink (vedi § A.4.1);
13. Nella finestra Configuration Parameters (figura A.9) nel menù 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 menù 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 è memorizzato lo schema Simulink su cui stiamo lavorando;
§ 2.4 Congruenza dei modelli del robot
15. Dal menù principale dello schema Simulink selezionare Tools −→ Real-Time
Workshop −→ Build Model;
16. Nel menù di simulazione selezionare External;
17. Impostare il tempo di simulazione desiderato;
18. Connettere l’hardware esterno necessario alla simulazione;
19. Dal menù 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
validità della procedura di calcolo automatico della dinamica del manipolatore.
La verifica è 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 è 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;
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 validità del modello real-time, infatti a parità di segnali
di coppia forniti in input, i due modelli, producono in uscita gli stessi segnali
di posizione angolare dei giunti del robot. Ciò può essere facilmente verificato
osservando le figure 2.7 e 2.8 che riportano i grafici forniti dalle simulazioni riguardo
ai suddetti segnali.
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.
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 3
Generazione e allineamento dei
segnali encoder
3.1
Introduzione
Il simulatore real-time proposto in questa tesi è costituito, come già anticipato nel
capitolo introduttivo, da due unità di calcolo (o sistemi) unite tra loro in modo
complementare. In particolare, l’unità 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 più lenti ed insufficienti per generare contemporaneamente anche i segnali
encoder. L’unità veloce (Sistema Veloce in figura 1 a pagina 8) invece riceve i segnali analogici corrispondenti alle velocità di giunto del robot, prodotte dall’unità
lenta, e genera sulla base di questi segnali i segnali encoder in fase e quadratura.
Quest’ultima unità può essere più veloce perché non soggetta ai calcoli onerosi
legati alla simulazione del modello matematico. La problematica fondamentale
dietro a questa soluzione con doppia unità è che risulta necessario allineare l’informazione di posizione fornita dall’unità veloce e l’informazione di posizione presente
nell’unità lenta, quella cioè che risulta dalla simulazione delle equazioni matematiche. Infatti la trasmissione dei segnali di velocità dall’unità lenta a quella veloce è
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 velocità nella corrispondente posizione (che è il suo
integrale), dunque una soluzione che non prevede una correzione efficace di questi
errori non può risultare affidabile in quanto la simulazione non corrisponde più alle
equazioni matematiche che descrivono il moto del sistema. Allo scopo di allineare
le suddette informazioni di posizione, è 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’unità veloce corrisponde esattamente all’informazione di posizione presente
all’interno dell’unità 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’unità 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 è costituito da un disco ottico che ruota solidamente con
il rotore di cui si vuole misurare la velocità (vedi figura 3.1). Sul disco è 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, è 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 velocità 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 è pari a 2π/n; pertanto l’errore massimo di
misura di posizione che si può avere contando il numero dei fronti transitati durante
l’intervallo di campionamento risulta pari a 2π/n e, di conseguenza, l’ampiezza
69
massima dell’errore sovrapposto alla singola misura di velocità è pari a:
eωM ax =
2π
nT
Si può osservare che è 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 bontà della misura in quanto questa
risulta proporzionale al numero di fronti contati.
Tuttavia impiegando il sistema di misura descritto non è possibile apprezzare il
verso di rotazione dell’asse; inoltre, eventuali oscillazioni attorno ad una posizione di equilibrio potrebbero fornire una errata misura della velocità. Quando sia
necessario rilevare anche il senso di rotazione, oppure avere una accurata misura
di velocità anche per piccoli valori di questa, si può 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, è 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 velocità è pari a:
eωM ax =
2π
2π
π
=
=
np T
4nT
2nT
Occorre comunque nella manipolazione dei due segnali, porre attenzione affinché
non succeda che eventuali oscillazioni attorno ad una posizione di equilibrio provochino una errata misura della velocità. Infine è disponibile anche un ulteriore
70
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.
71
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, cioè usato per generare a
partire da un segnale analogico di velocità 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 è 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
72
4. Si introduce un guadagno Kout che sarà 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 θ sarà aggiornata secondo l’equazione:
θ = θ + uKout ∆t
quindi la posizione angolare attuale è pari alla somma della posizione angolare
precedente e dell’incremento di posizione avvenuto nell’intervallo di tempo
∆t, dato dal prodotto della velocità 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
73
θ. Se il canale A ha valore alto (cioè pari ad 1) si pone yf pari ad 1, altrimenti
yf sarà 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 (cioè pari ad 1) si pone
yq pari ad 1, altrimenti yq sarà 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.
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 è riassunta
dalla seguente espressione:
Vout (t) = v(t) + b(t)qd (t)
v(t) = KP e(t) + xc (t)
e(t) = q(t) − θ(t)
ẋc (t) = KI e(t)
ḃ(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 velocità generate
dal Simulatore Modello (si veda figura 4.1), il vettore θ(t) è la posizione ricostruita
dal conteggio dei segnali encoder prodotti dal Sistema Veloce (si veda figura 4.1),
75
Ka , KI e KP sono costanti positive, il vettore e(t) è 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) è la tensione fornita tramite
i convertitori D/A al Sistema Veloce (si veda la connessione ”Segnali di velocità
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.
76
La soluzione di controllo di tipo adattativo è 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 velocità 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) = ā Vout (t) + d¯
dove ā > 0 (abar > 0) e d¯ (dbar) sono costanti incognite dipendenti dall’hardware,
valgono le seguenti proprietà:
R +∞
1. e(t) ∈ L2 , cioè 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
ā
xc (t) + d¯ +
(āb(t) − 1)2 > 0
KI
āKa
(3.1)
si dimostra per sostituzione che:
V̇ (t) = −2ā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) è 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, poiché V (t) è limitata, lo sono di conseguenza dalla (3.1) anche e(t), x c (t)
e b(t). Dunque se qd (t) è limitata, per sostituzione, saranno limitate
v(t) = KP e(t) + xc (t)
θ̇(t) = d¯ + v(t) + b(t)qd (t)ā
77
quindi sarà limitata anche
ė(t) = q̇d (t) − θ̇(t)
e similmente
V̈ (t) = −4āKP e(t)ė(t)
Da ciò, 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.
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 validità 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) è 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;
79
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, cioè 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 ;
80
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 .
81
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 più significativo dal punto di vista computazionale e allo stesso tempo preliminare per
i successivi test di verifica dell’intero sistema HIL, è 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 velocità
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 validità dell’algoritmo
di generazione dei segnali encoder e l’effettiva efficienza della legge di controllo
adattativa. Ciò può essere facilmente verificato osservando le figure 3.15 e 3.16 che
riportano i grafici forniti dalle simulazioni riguardo ai suddetti segnali.
82
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.
83
joint 4 [rad]
joint 3 [rad]
joint 2 [rad]
joint 1 [rad]
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).
84
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).
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 è molto importante perché costituisce una verifica
diretta della validità 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 velocità 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 è 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 è disponibile in Appendice B;
• 1 microcontrollore MICROCHIP dsPIC30F4013. La scheda tecnica di questo
microcontrollore è 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
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 nonché eliminazione degli errori tramite chiaro collegamento e
chiara siglatura di moduli e poli; 5) maggiore disponibilità 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 è 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
latore robotico che si interfaccia con l’ambiente esterno (cioè 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 velocità dei
giunti del robot . Questo sistema può tuttavia produrre ed inviare all’hardware di controllo questi segnali analogici di velocità solo a bassa
frequenza (segnali lenti) e non è 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’incapacità del primo sistema, in quanto è in grado di produrre
i segnali elettrici in fase e quadratura di tipo encoder ad alta frequenza
sulla base dell’ingresso analogico rappresentante le velocità 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
velocità dei giunti del robot.
89
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 velocità 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 è dettagliatamente descritta nel capitolo 3.1
(§3.3).
5. Realizzazione di uno schema di connessione ottimale dell’hardware impiegato
per garantire prestazioni e qualità 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.
90
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 è 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 velocità
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.
91
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 velocità 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 velocità 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 velocità.
Per ogni segnale analogico di velocità 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 è 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.
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, è 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 è
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 velocità 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 velocità 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 può
essere direttamente implementata se non si determina una stima della velocità dei
giunti del robot. Per risolvere questo problema è 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 velocità a partire dall’informazione di posizione ricostruita dal conteggio dei segnali encoder prodotti
dal Sistema Veloce di figura 4.1. Il filtro dinamico è dettagliatamente descritto nel
paragrafo successivo (§4.4).
4.4
Filtro dinamico per la stima della velocità
dei giunti del robot
Non disponendo di una misura diretta della velocità dei giunti del robot, è necessario trarne una stima in base alla misura di posizione dei giunti del manipolatore,
93
§ 4.4 Filtro dinamico per la stima della velocità 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 velocità è 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 velocità, 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 velocità, y è 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 libertà del robot)
si trovino nella metà 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 è discreto e la misura dell’encoder y(t) è solo disponibile negli
istanti t = kT (T = tc = tempo di campionamento) è 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) +
94
Le funzioni cosı̀ ottenute sono quindi una stima dinamica delle grandezze di posizione e velocità 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.
95
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).
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, cioè
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 è 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;
97
4. Due blocchi Scope, qdsim e qsim, che consentono di visualizzare in tempo reale
l’andamento dei segnali di velocità 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) è invece
costituito da:
1. Un blocco Subsystem, TRAJECTORY DEMAND, che fornisce gli andamenti
desiderati di posizione, velocità 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 velocità dei giunti del robot a partire dall’informazione di
posizione (degli stessi giunti) ricostruita come nel punto 2;
98
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, velocità ed accelerazione per i giunti del robot, l’azione di
99
controllo di tipo feedforward; cioè 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 velocità 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 già
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 già 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];
matrice
matrice
matrice
matrice
COG
COG
COG
COG
link
link
link
link
1
2
3
4
%matrice d’inerzia link 1
100
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;
eps2 = 0.3;
Hp2 = 5;
Hv2 = 6;
eps3 = 0.3;
101
Hp3 = 5;
Hv3 = 6;
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 semplicità non è stata eseguita la ricerca della posizione di Home ma è 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-
102
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, cioè il tempo
simulato corrisponde all’effettiva durata temporale della prova sperimentale. Dalla
figura 4.14 si può 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, cioè l’andamento temporale resta di tipo sinusoidale con un ampiezza pari a
0.04 rad. Le cause principali a cui può 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 velocità ai giunti del robot e
il medesimo modello real-time implementato all’interno del corrispondente blocco
103
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
104
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 può essere valutato osservando i grafici forniti dalle
prove simulative e riportati nelle figure 4.16 e 4.17.
105
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).
106
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).
107
Capitolo 5
Conclusioni
In questo lavoro di tesi è 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 (cioè 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 velocità 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 velocità 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 qualità 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
funzionalità e prestazioni di schede di controllo assi (o hardware di controllo equivalente), già nella fase di progettazione e prototipazione, senza attendere la disponibilità 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, poiché 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 flessibilità 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 sarà 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 consentirà
di valutare le prestazioni della scheda e la validità 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 sarà quello
di implentare all’interno del banco prova HIL le funzionalità 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 proprietà di compatibilità 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, cioè per connettere il computer (su cui è
installato il simulatore) con il microcontrollore e con la scheda che fornisce l’azione
di controllo. In particolare verrà spiegato come installare ed usare le funzionalità
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 è 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 capacità 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.
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) è 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.
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,
cioè 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 è sufficiente digitare, nella finestra di comando
(Command Window ) del MATLAB, il comando rtwinlib. Il risultato è quello che
si vede nella figura A.3, dove vengono mostrati tutti i blocchi disponibili:
Figura A.3: Libreria Real Time Windows Target.
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.
116
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 può 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 menù di installazione della scheda (figura A.4).
Per terminare l’installazione cliccare Apply e poi Ok.
117
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 è 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 è
necessario:
1. Selezionare Simulation −→ Conf iguration P arameters nel menù dello
schema Simulink;
Figura A.8: Snapshot del menù Simulation.
118
2. Nella finestra Configuration Parameters (figura A.9) nel menù 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 menù 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.
119
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 menù 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 menù 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 menù 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 è memorizzato lo schema Simulink su cui si sta lavorando;
7. Dal menù principale dello schema Simulink selezionare Tools −→ Real-Time
Workshop −→ Build Model ;
8. Nel menù 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 menù principale dello schema Simulink selezionare Simulation −→ Connect To Target;
120
Figura A.10: Snapshot del menù dello schema Simulink che consente di selezionare
la modalità 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.
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.
122
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).
123
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).
124
Pin
1
3
5
7
9
11
13
15
17
19
21
23
25
J5
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
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.
125
Appendice B
Il microcontrollore
B.1
Generalità sul microcontrollore
Un microcontrollore o microcontroller, detto anche computer single chip è 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 più generale. I microcontroller sono
la forma più diffusa e più invisibile di computer. Comprendono la CPU, un certo quantitativo di memoria RAM e memoria ROM (può 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 è l’utilizzo dell’interrupt. Un Interrupt è una segnalazione che viene fatta al PIC da una delle sue
periferiche o dal mondo esterno, ed avvisa il dispositivo che si è 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 è l’evento che avverte la sua presenza, nel secondo, invece, è il sistema
che richiede ciclicamente se l’evento è presente. Quando si verifica un Interrupt
126
§ B.1 Generalità sul microcontrollore
il PIC interrompe immediatamente la normale esecuzione del programma e salta
alla locazione di memoria a partire dalla quale è 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 riprenderà
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 è 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).
128
§ B.2 dsPIC30F4013
Tabella B.1:
dsPIC30F4013.
Descrizione funzioni PIN del microcontrollore Microchip
129
§ B.2 dsPIC30F4013
Tabella B.2:
dsPIC30F4013.
Descrizione funzioni PIN del microcontrollore Microchip
.
130
§ B.3 MPLAB
B.3
MPLAB
MPLAB è 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) è 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. È un tool
ideale per i progettisti di controlli che cercano un’alternativa di basso costo agli
emulatori incircuit costosi. La programmazione del PIC è possibile sia in modalità basso che in modalità 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ı̀ più veloce, è riservato ad una utenza specializzata. Affinché il
dispositivo possa essere sfruttato da una gamma di tecnici più 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 più 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) è 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 è possibile con verifica).
Queste operazioni sono facilitate dalla visione di opportuni LED diagnostici (Power, Busy, Error) e dalla possibilità 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 è installato il dsPIC30F4013.
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 è 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;
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.
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.
.
.
135
§ B.4 dsPICDEM 2
Tabella B.4: Configurazione dei Jumper della scheda dsPICDEM 2, richiesta per
il montaggio del microcontrollore dsPIC30F4013.
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 menù: Debugger −→ SelectT ool −→ M P LABICD2;
7. Selezionare dal menù: Conf igure −→ SelectDevice;
137
8. Nella finestra Select Device (figura B.7), selezionare dai menù Device e Device
Family il modello di microcontrollore da programmare. In questo caso:
Device: dsPIC30F4013
Device Family: 30xxxx
Figura B.7: Finestra SelectDevice.
138
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;
139
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;
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;
}
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);
giunto 2:
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);
144
§ C.2 INTx IO pins.c
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);
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);
giunto 6:
• Chiusura funzione INTx IO Init:
}
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
146
• 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
• 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;
147
• 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
148
4.98 Volt e codifica questo intervallo con 12 bit, cioè 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 è allocata l’ISR (Interrupt Service Routine)
IFS0bits.ADIF=0;
• Chiusura funzione void attribute (( interrupt )) ADCInterrupt(void) :
}
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;
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;
• 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)
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
if (theta2 > 2.0*R)
{
}
if (theta2 < 0.0)
{
}
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
if (theta3 > 2.0*R)
{
}
if (theta3 < 0.0)
{
}
if (theta3 <= R)
{
//yf[0]=1.0;
LATDbits.LATD0 = 0;
PORTDbits.RD0 = 1;
}
else
{
//yf[0]=0.0;
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
if (theta4 > 2.0*R)
{
}
if (theta4 < 0.0)
{
}
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;
154
§ C.4 Timers.c
PORTFbits.RF1 = 0;
}
else
{
//yq[0]=1.0;
LATFbits.LATF1 = 0;
PORTFbits.RF1 = 1;
}
//codice per qp5
if (theta5 > 2.0*R)
{
}
if (theta5 < 0.0)
{
}
if (theta5 <= R)
{
//yf[0]=1.0;
LATCbits.LATC13 = 0;
PORTCbits.RC13 = 1;
}
else
{
//yf[0]=0.0;
PORTCbits.RC13 = 0;
}
if ( ( (R/2.0) < theta5) && ( theta5 < ((3.0/2.0)*R)) )
{
//yq[0]=0.0;
PORTCbits.RC14 = 0;
}
else
{
//yq[0]=1.0;
PORTCbits.RC14 = 1;
}
155
§ C.4 Timers.c
//codice per qp6
if (theta6 > 2.0*R)
{
}
if (theta6 < 0.0)
{
}
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
156
§ C.4 Timers.c
schema a blocchi simulink. L’offset viene usato poiché 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:
}
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 velocità 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;
}
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));
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;
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];
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];
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;
nq=NQ;
/* for each point in the input trajectory
*/
for (p=0; p<nq; p++)
{
/*
*/
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:
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];
}
{
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);
}
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:
167
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;
{
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)
{
168
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;
169
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;
170
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;
nq=NQ;
/*
for each point in the input trajectory
*/
for (p=0; p<nq; p++) {
/*
*/
for (j = 0; j < NJOINTS; j++)
{
Link
*l = &robot.links[j];
171
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;
{
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);
}
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 è scelta come predefinita. Il
173
§ E.1 link
modello dinamico del manipolatore può essere anche inizializzato usando il
comando L = link(dyn row), dove dyn row è 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,
è 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.
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.
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 gravità
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.
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.
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.
178
§ E.5 robot/plot
E.5
robot/plot
• Tipologia: plot e animazione grafica 3D del movimento del robot.
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 è 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.
179
§ E.6 rne
E.6
rne
• Tipologia: Calcolo della dinamica inversa e diretta del manipolatore.
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), velocità
(qd) ed accelerazione (qdd) di giunto, consentendo quindi il calcolo della
dinamica inversa del manipolatore. L’algoritmo di calcolo usato è 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 gravità. Fext consente
invece di definire l’azione di forze/momenti agenti sull’effettore del robot. In
particolare fext è 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 può
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
180
§ E.6 rne
formulazione di Newton-Eulero (si veda il capitolo 1, §1.5.3) è 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) è 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) è 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 è dovuta ad
una versione C-MEX (matlab executable file) della funzione rne che risulta
essere fino a 500 volte più 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 poiché 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, perché 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.
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
più 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 già esistente:
clear L
2. Si caricano i parametri della tabella di D-H tramite l’oggetto link:
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
link([-pi/2 .0203
0
.15005 0], ’standard’);
link([pi/2 0 0
.4318
link([-pi/2 0 0
0
link([0 0
0
0
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
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:
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 è 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.
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 velocità 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 è 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 VELOCITÀ 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 velocità 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, Università degli studi di
Roma “Tor Vergata”.
[20] A. Tornambè, 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

Indice - Corso di Studi in Ingegneria dell`Automazione

Transcript

Documenti analoghi

Invasione robot al Museo della figurina – Corriere di Bologna, 29/03

"L`intelligenza artificiale" di Irene Perfetti

Io Robot - DivxMania

CS SRF ONO arte

L`Herakles a Vito Teti - Confraternita del SS. Crocifisso

RC 4000 Dati tecnici RC 4000 Robot per le pulizie domestiche

L`ITIS “GEROLAMO CARDANO GEROLAMO CARDANO” di Pavia

Arrivano i robot chirurghi e con un po` d`anima Justin prepar

RC 3000 - Kärcher Center PAT

Opzioni del software