FACOLTA DI INGEGNERIA` CORSO DI LAUREA ... - Roma Tor...
Transcript of FACOLTA DI INGEGNERIA` CORSO DI LAUREA ... - Roma Tor...
UNIVERSITA DEGLI STUDI DI ROMA
TOR VERGATA
FACOLTA DI INGEGNERIA
CORSO DI LAUREA IN INGEGNERIA
DELL’AUTOMAZIONE
A.A. 2010/2011
Tesi di Laurea
Realizzazione e controllo di un dispositivo di contenimento per
atteggiamenti posturali involontari
RELATORE CANDIDATO
Ing. Daniele Carnevale Roberto Vollaro
Indice
Ringraziamenti 1
Introduzione 3
1 Introduzione al problema 5
1.1 Idea generale . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.2 STm32 VL Discovery . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2 Passivita e sistemi Passivi 10
2.1 Passivita per funzioni memoryless . . . . . . . . . . . . . . . . . . . . 10
2.2 Passivita in un sistema dinamico . . . . . . . . . . . . . . . . . . . . 12
2.3 Passivita e stabilita . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.4 Controllo basato sulla passivita . . . . . . . . . . . . . . . . . . . . . 18
3 Equazioni del modello e sintesi del sistema di controllo 23
3.1 Equazioni del modello . . . . . . . . . . . . . . . . . . . . . . . . . . 23
3.2 Sintesi del controllo di posizione . . . . . . . . . . . . . . . . . . . . . 28
3.3 Osservatori ad alto guadagno . . . . . . . . . . . . . . . . . . . . . . 33
3.4 Sintesi del controllo di coppia . . . . . . . . . . . . . . . . . . . . . . 38
3.5 Risultati simulativi . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
3.5.1 Simulazione 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
INDICE I
INDICE
3.5.2 Simulazione 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
4 Realizzazione del prototipo e dati sperimentali 52
4.1 Realizzazione del prototipo . . . . . . . . . . . . . . . . . . . . . . . . 52
4.2 Gestione dei sensori . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
4.3 IL software utilizzato . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
4.4 Risultati sperimentali . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
Appendice A
Codice creato 72
Appendice B
guida all’uso del STm32Vl Disocvery 83
Elenco delle figure 84
Bibliografia 85
INDICE II
Ringraziamenti
Come tipico del mio modo di essere, mi accingo a scrivere i ringraziamenti a pochi
minuti dalla messa in stampa, cercando di concentrare in pochi minuti tutti i mo-
menti e le persone importanti incontrate in questi in questi 5 anni. Il mio primo
ringraziamento va all’ing Carnevale (o meglio Daniele) per avermi seguito con una
disponibilita fuori dal comune nel completamento di questo lavoro finale. Inoltre devo
sicuramente ringraziare Paolo Mondini di Ergonixart per avermi affidato il lavoro e
per essere stato sempre pronto a rimediare alla mia insana dote distruttiva, e Aurora
Caputi per la sua disponibilita. E’ d’obbligo ringraziare anche Paolo Testa, che e sta-
to indispensabile nella prima fase di lavoro quando mi sono dovuto cimentare con un
incomprensibile e complicata scheda di controllo, che ‘ “misteriosamente e inavverti-
tamente” ha lasciato questo mondo1.
Per il supporto morale meritano i miei ringraziamenti Ale (catania), che per una
ritrovata vena NERD mi ha accompagnato nelle lunghe serate in laboratorio, e Ale
(spizzi) che si e rivelato una musa ispiratrice per ogni problema.
Non so se questa inclinazione alle materie scientifiche sia dovuta ai momenti passa-
ti al liceo, quando iniziavo la giornata con seminari sulla meccanica quantistica (del
grande Gino che non poteva mancare nei miei ringraziamenti), quando nelle ore di
matematica mi chiudevo nell’armadio diffondendo allarmi bomba, o piu semplicemen-
1Attenzione: e dimostrato che collegamenti casuali su schede di discreto valore non sonoraccomandabili
Introduzione 1
Introduzione
te ad una totale negazione per le scienze umanistiche, fatto sta che da solo non sarei
partito e da solo non sarei arrivato fin qui.
Ringrazio Mammarella e Pimpi per il supporto economico e morale fornitomi e i miei
molteplici fratelli di cui non posso ricordare tutti i nomi.
Andrea, Fonzi, Andrea, Libero per le risate e le cazzate fatte insieme,
I “ragazzi del laboratorio” per aver reso molto piu divertente questo triste covo di
ingegneri.
Per ultime, ma solo per dedicargli lo spazio che meritano, ringrazio:
Laura incredibile compagna di studi di questi anni, per avermi aiutato, supportato e
sOpportato tutto il giorno tutti i giorni in questi ultimi due anni. Sei membro ufficiale
del circolo dell’amicizia.
Flavia (meglio nota come cucciolina) per aver assecondato le miei pazzie durante le
fasi finali, e per essere sempre presente. Sei il mio pensiero felice.
Grazie a tuti per essere parte della mia vita e per essere la conferma che nella vita
non contano solo i numeri.
Introduzione 2
Introduzione
Il problema affrontato in questo lavoro di tesi riguarda la realizzazione e controllo
di un dispositivo di contenimento per atteggiamenti posturali involontari. Esistono
diverse patologia nell’ambito dei disturbi del movimento, come ad esempio la distonia,
in cui chi ne e affetto tende ad assumere posizioni innaturali e le mantiene per tempi
prolungati, riuscendo a modificarle molto lentamente, oppure in altri casi il soggetto
esercita involontariamente una elevata forza muscolare che rischia di compromettere
l’integrita della struttura muscolo-scheletrica. L’idea di base e quella di realizzare
un supporto mobile motorizzato su cui vincolare il paziente, che assecondi lo scatto
muscolare cercando di contenerne la forza e riportarlo gradualmente in una posizione
corretta. Il lavoro svolto e una caratterizzazione di un progetto piu ampio sviluppato
da ErgonixArt societa specializzata nell’offrire servizi a tutti coloro che operano nel
campo dell’ergonomia, con particolare riguardo al settore della disabilita fisica e sen-
soriale. Nel dettaglio si e lavorato alla realizzazione e controllo di un supporto per una
gamba che permetta l’eventuale movimento dell’arto contenendo la forza sviluppata
e la conseguente assistenza motorizzata per riportare la gamba in posizione corretta.
Dal punto di vista ingegneristico la modellazione e il controllo sono basati sulla teoria
dei sistemi passivi e degli osservatori alto guadagno, mentre per l’implementazione
software e stata usata la scheda di controllo STm32 VL Discovery prodotta dalla ST
microelettronics che sfrutta le potenzialita di un processore ARM Cortex-M3. La teo-
Introduzione 3
Introduzione
ria dei sistemi passivi rientra nella sfera del controllo non lineare. Si basa sull’energia
accumulata e dissipata dai sistemi dinamici che sotto opportune condizioni possono
essere definiti passivi. Un sistema passivo gode di determinate proprieta di stabilita
che possono essere sfruttate per l’analisi o la sintesi del controllo. Inizialmente verra
presentato il problema e la scheda STm32 VL con le sue principali funzionalita. Segue
un capitolo in cui viene spiegata la teoria alla base del controllo utilizzato. Nel terzo
sono descritte la modellazione matematica del prototipo, la sintesi del controllo e la
presentazione dei dati simulativi. Il capitolo conclusivo descrive la realizzazione del
prototipo con i risultati sperimentali ottenuti.
Introduzione 4
Capitolo 1
Introduzione al problema e
descrizione della STm32 VL
discovery
In questo capitolo vengono introdotte le specifiche del prototipo edei principali componenti usati. Viene inoltre data una descrizionedella scheda di controllo utilizzata.
1.1 Idea generale
Per identificare le specifiche del prototipo occorre in prima analisi capire la proble-
matica per cui il dispositivo e progettato. L’idea e quella di contenere e correggere
atteggiamenti posturali involontari dovuti a patologie dei disturbi del movimento. I
pazienti affetti da questo tipo di patologie non hanno il pieno controllo dell’attivita
muscolare e sono soggetti a spasmi, talvolta anche molto violenti, che inducono una
postura involontaria. Tale postura risulta spesso non reversibile o reversibile dopo
lunghi periodi di tempo. Un dispositivo di contenimento che sia di ausilio a questi
pazienti, deve concedere il movimento degli arti, esercitando pero una forza contenuta
e riportare grazie ad un sistema motorizzato il paziente in una posizione corretta. Il
lavoro presentato si focalizza sulla realizzazione di un supporto per una gamba che
5
Cap. 1 Introduzione al problema §1.2 STm32 VL Discovery
risulta parte di un progetto piu ampio che prevede una struttura di supporto per
l’intero corpo.
Il dispositivo consiste in un alloggiamento, il cui moto viene controllato da un motore
elettrico, su cui viene vincolata la gamba. Sulla base di una misura di posizione,
il motore riporta la gamba del paziente in una posizione desiderata mentre cerca di
limitarne gli sforzi muscolari entro determinati limiti, assecondandone per quanto
possibile il movimento. Come strumento di misura per la posizione vine usato un
potenziometro rotante. Per la misura di forza viene invece usata una cella di carico,
attiva sia in compressione che in trazione, che permette di misurare la forza applicata
sul supporto mobile. Per l’implementazione software e la gestione delle letture dei se-
gnali e utilizzata la scheda di controllo STm32 VL discovery basata su un processore
ARM cortex-M3 a 32-bit. Nella sezione seguente si andra a presentare la scheda con
le sue funzionalita e le principali periferiche utilizzate.
1.2 STm32 VL Discovery
La scheda Stm32 VL discovery e basata sul microcontrollore STM32F100RBT6B (un
ARM cortex-M3 a 32-bit), ed e provvista di una memoria flash da 128 KB, 8 KB
di ram e 64 pin multifunzionali. Presenta gia incorporato l’ST-link che permette di
programmare la scheda dal computer tramite cavo USB. Tale dispositivo puo essere
usato anche per la compilazione di processori esterni alla scheda, rimuovendo i due
jumper CN3 e collegando la scheda esterna al connettore CN2. Naturalmente i pro-
cessori supportati sono solo gli ST. Sicuramente tra i principali vantaggi della scheda
ci sono le numerose periferiche di cui il processore e dotato, la potenza di calcolo (fino
a 24Mhz) e il costo davvero contenuto del dispositivo.
Per quel che riguarda l’ambiente di sviluppo la ST fornisce tre piattaforme: Atollic
6
Cap. 1 Introduzione al problema §1.2 STm32 VL Discovery
Figura 1.1: STm32 VL Discovery
true studio (basato su eclipse), IAR Embedded Workbench e MDK-ARM. Nel lavoro
svolto e stata utilizzata la piattaforma Atollic TrueSTUDIO for STMicroelectronics
STM32 Lite v2.3.0.
La scheda presenta una serie di librerie appositamente concepite per l’utilizzo delle
periferiche. Gli header file e le funzion usate sono ben commentate e permettono un
semplice apprendimento per la programmazione. Nel progetto sviluppato, le principali
periferiche utilizzate sono:
• GPIO
• ADC
• TIMER
• USART
• DMA
7
Cap. 1 Introduzione al problema §1.2 STm32 VL Discovery
GPIO: La periferica GPIO e una delle piu importanti periferiche del micro in
quanto consente di gestire gli input/output della scheda e impostare i diversi pin per
interfacciare i segnali con le funzioni specifiche di ogni periferica del micro. Dei 64 pin
della scheda in questione, 51 sono relativi alla GPIO e risultano divisi su 4 periferiche
(GPIOA, GPIOB, GPIOC, GPIOD) ognuna delle quali controlla 16 pin (solo 3 pin
per GPIOD). Ogni pin puo essere usato per alcune funzioni specifiche.
ADC: La periferica ADC svolge la funzione di conversione di segnali analogici
in segnali digitali interpretabili dal micro. Tale periferica deve essere usata per la
lettura di segnali in ingresso. La scheda in questione presenta una sola periferica
ADC con 16 canali. Il valore di riferimento per l’ADC deve essere compreso tra 2.4V
e 3.6V. La scala di divisione dell’ADC e basata sul voltaggio di riferimento. In questa
scheda tale valore viene automaticamente preso dall’alimentazione della scheda. I dati
letti dall’ADC vengono memorizzati in un registro a 16 bit, che essendo unico, rende
necessario l’uso del DMA per la lettura di piu di un segnale. Se abilitata, ogni volta
che una lettura e conclusa, viene inviata una richiesta al DMA che copia il dato del
registro in un banco di memoria specificato dall’utente.
TIMER: La scheda presenta 16 timer che possono essere utilizzati per le piu
svariate funzioni: temporizzatori, generatori di PWM, generatori di interrupt e molto
altro. La struttura generale e quella di un registro contatore aggiornato con un certa
frequenza (periodo del timer) sulla base del clock interno a 24 MHz. Viene fissato
dall’utente un valore massimo del contatore, raggiunto il quale il timer si azzera
generando un evento che puo essere utilizzato per generare interrupt. Ogni timer
presenta 4 canali di uscita che sulla base di un registro denominato output compare
register, possono determinare un segnale di uscita basato sul confronto tra il valore
del timer e un valore stabilito dall’utente specifico per il singolo canale. Con questa
8
Cap. 1 Introduzione al problema §1.2 STm32 VL Discovery
modalita e ad esempio possibile generare segnali PWM.
DMA: Il DMA (Direct Memory Access) e un meccanismo che permette ad alcune
periferiche di accedere direttamente alla memoria di sistema per scambiarsi dati, op-
pure leggere o scrivere, senza chiamare in causa il processore. Nel progetto sviluppato
l’uso del DMA e risultato indispensabile per la lettura da parte dell’ADC di piu di un
segnale e per un corretto protocollo di ricezione con la seriale. Ad esempio nel caso
dell’ADC se c’e la necessita di leggere piu di un segnale esterno, abilitando la richiesta
al DMA, ogni volta che una lettura e completata, il DMA si occupa di trasferire il
dato letto dall’unico registro dati presente per l’ADC in una variabile specificata dal-
l’utente. Senza l’ausilio di tale periferica risulterebbe impossibile accedere alle singole
letture che verrebbero sovrascritte dalle successive (il tempo di lettura dell’ADC e di
circa 15µsec).
USART: La USART o Universal Synchronous-Asynchronous Receiver-Transmitter
(ricevitore-trasmettitore sincorno-asincrono universale) e un dispositivo hardware di
uso generale o dedicato. Converte flussi di bit di dati da un formato parallelo a
un formato seriale sincrono/asincrono o viceversa. Tale periferica e utilizzata per la
comunicazione seriale con altri dispositivi o con il PC. Nel progetto e stato imple-
mentato un protocollo seriale tra microcontrollore e Pc per memorizzare i dati letti
dalla scheda (utilizzando Processing) e per gestire in tempo reale il controllo tramite
Matlab.
In appendice e presente una piccola guida introduttiva all’uso della scheda, con i pas-
si principali di configurazione, una breve descrizione delle periferiche e delle relative
funzioni con degli esempi esplicativi.
9
Capitolo 2
Passivita e sistemi Passivi
In questo capitolo vengono presentati gli aspetti teorici su cui il la-voro e basato. Viene data una definizione di passivita per funzioninon lineari “memoryless” per poi estendere il concetto ai sistemi di-namici. Verrano infine presentati alcuni teoremi che caratterizzanoi sistemi passivi con un’introduzione al controllo basato su passivita.
2.1 Passivita per funzioni memoryless
La passivita rappresenta uno strumento generale molto elegante e potente per l’analisi
di sistemi dinamici, lineari e non lineari, e per il progetto di sistemi di controllo. Nello
studio delle proprieta legate alla passivita, ci si basa su di una descrizione ingresso/u-
scita del sistema e su considerazioni energetiche. L’idea principale alla base di questi
studi e che molti sistemi fisici hanno determinate caratteristiche comuni I/O legate
alla conservazione, al trasporto e alla dissipazione dell’energia. Tali caratteristiche
permettono una generalizzazione e lo sviluppo di strumenti di analisi e sintesi molto
potenti. Per capire e definire il concetto di passivita e utile partire con un esempio
che caratterizza la passivita per funzioni memoryless.
Definizione 2.1.1. Le nonlinearita memoryless sono il piu comune elemento non
lineare. Dato un segnale in ingresso x(t), l’uscita di una funzione non lineare memo-
10
Cap. 2 Passivita e sistemi Passivi §2.1 Passivita per funzioni memoryless
ryless e data da:
y(t) = f(x(t))
dove f(·) e una funzione che mappa istantaneamente i valori di x(t) nel relativo
codominio y(t).
Iniziamo con il considerare un semplice circuito elettrico composto da una resi-
stenza R sottoposta ad un voltaggio V e attraversata da una corrente I. Considerando
questo semplice schema come un sistema privo di memoria, prendiamo la differenza
di potenziale V come ingresso e la corrente I come uscita.
V=u
I=y
R
Figura 2.1: analogia elettrica
La resistenza e passiva se la potenza che scorre nel sistema e sempre positiva o
nulla, ovvero se il prodotto uy ≥ 0 per ogni coppia di punti (u, y).
Basandoci su questo concetto possiamo estendere l’idea di passivita ad una generica
funzione y = h(t, u) (anche tempo variante), a prescindere dal suo significato fisico.
Definendo il prodotto uy come l’energia che scorre nel sistema diremo che il sistema
e passivo se uy ≥ 0 per ogni u. Da un punto di vista geometrico la curva sara
sempre contenuta nel primo e terzo quadrante e possiamo dire che h ∈ [0,∞] dove
zero e infinito sono le pendenze dei confini della regione contenuta nel I-III quadrante.
Analogamente nel caso vettoriale si ha:
y = h(t, y) con h : [0,∞)×Rp → Rp
uTy ≥ 0 con uTy =∑p
i=1 uiyi
11
Cap. 2 Passivita e sistemi Passivi §2.2 Passivita in un sistema dinamico
Inoltre se per una particolare funzione h() vale che uTy ≥ uTϕ(u) per una generica
ϕ(u), se uTϕ(u) > 0 ogni u 6= 0, allora il sistema descritto dalla funzione h() e detto
ad input strettamente passivo nel senso che uTy = 0 solo se u = 0.
Analogamente si parlera di sistema ad output strettamente passivo se uTy ≥ yT δ(y)
per una generica δ(y) con yT δ(y) > 0 ogni y 6= 0. In questo caso uTy = 0 solo se
y = 0. Possiamo riassumere quanto detto nella seguente definizione:
Definizione 2.1.2. Il sistema y = h(t, u) e:
• passivo se uTy ≥ 0
• senza perdita (lossless)se uTy = 0
• input strettamente passivo (input strictly passive)se uTy ≥ uTϕ(u) con uTϕ(u) >
0 ∀u 6= 0
• output strettamente passivo (output strictly passive) se uTy ≥ yT δ(y) con yT δ(y) >
0 ∀y 6= 0
2.2 Passivita in un sistema dinamico
Consideriamo il sistema dinamico rappresentato dalle equazioni:
x = f(x, u)y = h(x, u)
(2.2.1)
con f() : Rn × Rp → Rn localmente di Lipschitz f(0,0)=0,
h():Rn × Rp → Rp e h(0,0)=0.
Per definire il concetto di passivita per un sistema dinamico, consideriamo anche in
questo caso uno schema elettrico composto da induttanze condensatori e resistenze.
Come si evince dalla figura 2.2 vengono prese come variabili di stato la corrente
che scorre nell’induttore e la differenza di potenziale ai capi del condensatore, come
12
Cap. 2 Passivita e sistemi Passivi §2.2 Passivita in un sistema dinamico
Figura 2.2: circuito RLC di esempio
ingresso il voltaggio fornito dal generatore e come uscita la corrente fornita dal gene-
ratore. Le equazioni del circuito possono facilmente essere calcolate sulla base delle
leggi fisiche che regolano il funzionamento dei componenti, con la sola differenza che
le resistenze vengono considerata non lineari, quindi al posto della legge di Ohm vale
che IR = hR(VR) o VR = hR(IR). Possiamo quindi andare a scrivere:
Lx1 = u− h2(x1)− x2Cx2 = x1 − h3(x2)y = x1 + h1(u)
Come nel caso del semplice resistore la potenza del sistema sara data dal prodotto
uy, ma in questo circuito oltre all’energia dissipata c’e quella immagazzinata dal con-
densatore e dall’induttore. Il sistema in questione sara dunque passivo se l’energia
dissipata dalla rete in un certo periodo risultera maggiore o uguale dell’energia im-
magazzinata nello stesso periodo. In termini matematici possiamo definire l’energia
accumulata dalla rete come la somma dell’energia immagazzinata nel condensatore
(12CV 2) e quella accumulata dall’induttore (1
2LI2). Riportando tali espressioni nelle
variabili di stato definiamo l’energia accumulata come:
V (x) =1
2Lx21 +
1
2Cx22 (2.2.2)
13
Cap. 2 Passivita e sistemi Passivi §2.2 Passivita in un sistema dinamico
La relazione energetica affinche il sistema si possa definire passivo e:
∫ t
0
u(τ)y(τ)δτ ≥ V (x(t))− V (x(0)) (2.2.3)
Derivando ambo i membri possiamo esprimere la condizione di passivita come:
u(t)y(t) ≥ V (x(t), u(t)) (2.2.4)
Dal calcolo della derivata della funzione V si ottiene la relazione seguente:
uy = V + uh1(u) + x1h2(x1) + x2h3(x2) (2.2.5)
Dalla relazione ottenuta si puo osservare che se le funzioni h1,h2,h3 sono passive
(secondo la definizione data nella sezione precedente), allora uy ≥ V e il sistema
e passivo. Inoltre sempre sulla base della (2.2.5) possiamo distinguere dei casi che ci
portano ad estendere le definizioni di passivita date per le funzioni memoryless anche
ai sistemi dinamici.
1. Se h1 = h2 = h3 = 0 allora uy = V : non c’e dissipazione di energia, il sistema e
senza perdita (lossless).
2. h2 e h3 passivi (∈ [0,∞]):
uy ≥ V + uh1(u)
se uh1(u) > 0 ∀u 6= 0 il sistema risulta ad output strettamente passivo (output
strictly pssive). Infatti a prescindere dall’andamento di h2 e h3 se u 6= 0 l’energia
assorbita nell’intervallo [0,t] risultera sempre maggiore dell’incremento di energia
immagazzinata.
3. h1 = 0 e h3 ∈ [0,∞]. In questo caso risulta y = x1 e vale quindi che:
uy ≥ V + yh2(y) (2.2.6)
14
Cap. 2 Passivita e sistemi Passivi §2.2 Passivita in un sistema dinamico
se yh2(y) > 0 ∀y 6= 0 il sistema risulta ad input strettamente passivo (input
strictly passive). Infatti a prescindere dall’andamento di h3 se y 6= 0 l’energia
assorbita nell’intervallo [0,t] risultera sempre maggiore dell’incremento di energia
immagazzinata.
4. se h1 ∈ [0,∞] allora:
uy ≥ V + x1h2(x1) + x2h3(x2) (2.2.7)
se h1 e h2 ∈ (0,∞) allora x1h2(x1)+x2h3(x2) e una funzione definita positiva di x.
In questo caso si dice che il sistema e a stato strettamente passivo o strettamente
passivo (strictly passive) poiche l’energia assorbita nell’intervallo [0,t] risultera
sempre maggiore dell’incremento di energia immagazzinata a meno che lo stato
x non sia identicamente nullo.
Dall’esempio mostrato possiamo generalizzare quanto detto con la seguente defi-
nizione:
Definizione 2.2.1. Il sistema
x = f(x, u)y = h(x, u)
(2.2.8)
e passivo se esiste una funzione di storage V (x) semidefinita positiva, continua e
differenziabile tale che:
uTy ≥ V =δV
δxf(x, u), ∀(x, u) (2.2.9)
In particolare possiamo dire che:
• se uTy = V il sistema e senza perdita
• se uy ≥ V + uTϕ(u) con uTϕ(u) > 0 ∀u 6= 0, il sistema e ad input strettamente
passivo
15
Cap. 2 Passivita e sistemi Passivi §2.3 Passivita e stabilita
• se uy ≥ V + yTρ(y) con yTρ(y) > 0 ∀y 6= 0, il sistema e ad output strettamente
passivo
• se uy ≥ V +ψ(x) per una funzione ψ() definita positiva, il sistema e strettamente
passivo
2.3 Passivita e stabilita
In base alla definizione di passivita data e alle sue diverse caratterizzazioni e inte-
ressata notare le relazioni che legano la passivita di un sistema alla sua stabilita. In
prima analisi e utile accennare la condizione di stabilita secondo Lyapunov:
Dato un sistema del tipo
x = f(x) (2.3.1)
con x = 0 un punto di equilibrio sia V(x) una funzione definita positiva, continua e
differenziabile tale che:
V =δV
δxf(x) (2.3.2)
se V ≤ 0 in un intorno di 0 allora il punto di equilibrio e stabile. Se inoltre V < 0 in un
intorno di 0 allora l’equilibrio e asintoticamente stabile.1 Basandosi sulla condizione
sufficiente di stabilita di un punto di equilibrio secondo Lyapunov e possibile andare
a presentare i seguenti lemmi che mostrano l’implicazione della stabilita per sistemi
passivi. Si considera il sistema descritto dalla 2.2.8:
Lemma 2.3.1. Se il sistema descritto nella (2.2.8) e passivo, con una funzione di
storage V(x) definita positiva, allora l’origine di x = f(x, 0) e stabile.
1Il teorema di La-Salle in alcuni casi permette di determinare l’attrattivita di un punto anche perV ≤ 0
16
Cap. 2 Passivita e sistemi Passivi §2.3 Passivita e stabilita
Dimostrazione:
Per la passivita si ha uTy ≥ δVδxf(x, u), che per u=0 risulta essere δV
δxf(x, 0) ≤ 0.
Ovvero stabile per il teorema di Lyapunov, considerando la funzione di storage V(x)
come funzione candidata di Lyapunov.
Lemma 2.3.2. Se il sistema descritto nella (2.2.8) e strettamente passivo, allora
l’origine di x = f(x, 0) e asintoticamente stabile. Inoltre se la V(x) e radialmente
illimitata l’origine e globalmente asintoticamente stabile.
Dimostrazione:
E’ possibile dimostrare che se un sistema e strettamente passivo la funzione di storage
V(x) e definita positiva. Su questa base e facile mostrare:
uTy ≥ δVδxf(x, u) + ψ(x) per u=0 ⇒ δV
δxf(x, 0) ≤ −ψ(x) (2.3.3)
Poiche per quanto detto sui sistemi strettamente passivi ψ() e definita positiva V
risulta definita negativa, con la relativa implicazione che l’origine e asintoticamente
stabile.
Prima di enunciare il prossimo lemma e utile definire un concetto di osservabilita per
sistemi non lineari.
Definizione 2.3.1. Il sistema descritto dala (2.2.8) e detto stato-zero osservabile
(detectabile) se l’unica soluzione di x = f(x, 0) tale che h(x, 0) = 0 e x(t) ≡ 0.
Lemma 2.3.3. Se il sistema descritto nella (2.2.8) e output strictly passive e stato-
zero osservabile, allora l’origine di x = f(x, 0) e asintoticamente stabile. Inoltre se la
V(x) e radialmente illimitata l’origine e globalmente asintoticamente stabile.
Dimostrazione:
Anche in questo caso e possibile dimostrare che se un sistema gode delle proprieta
17
Cap. 2 Passivita e sistemi Passivi §2.4 Controllo basato sulla passivita
descritte nella tesi del lemma, la funzione di storage V(x) e definita positiva. Su
questa base e facile mostrare che come nel lemma precedente vale:
uTy ≥ δVδxf(x, u) + yTρ(y) per u=0 ⇒ δV
δxf(x, 0) ≤ −yTρ(y) (2.3.4)
La V risulta uguale a zero solo per y(t) = 0 (output strictly passive) e per la proprieta
di osservabilita appena definita y(t) = 0 solo se x(t) = 0, cio equivale a dire che la V
e definita negativa. Da questa constatazione deriva la stabilita asintotica dell’origine.
2.4 Controllo basato sulla passivita
Sulla base dei lemmi introdotti si va a presentare un teorema che definisce le condizioni
e la tipologia di un controllo in retroazione dall’uscita che stabilizza asintoticamente
l’origine di un sistema passivo che gode di determinate caratteristiche.
Teorema 2.4.1. Da un sistema del tipo
x = f(x, u)y = h(x)
dove f(·, ·) e localmente di Lipschitz in (x,u), h(·) e continua in x per ogni x ∈ Rn e
u ∈ Rm e con f(0,0)=0, h(0)=0;
se tale sistema e:
• passivo con una funzione V(x) radialmente illimitata e definita positiva
• stato-zero osservabile
Allora l’origine x=0 puo essere stabilizzata globalmente e asintoticamente da u =
−φ(y), dove φ(·) e una funzione localmente di Lipschitz tale che φ(0) = 0 e yTφ(y) > 0
∀y 6= 0.
18
Cap. 2 Passivita e sistemi Passivi §2.4 Controllo basato sulla passivita
Dimostrazione:
Per quanto visto nel 2.3.1 in accordo con la prima ipotesi del teorema il punto di
equilibrio del sistema e stabile. Se si considera la funzione di accumulo V(x) del
sistema come funzione candidata di Lyapunov in accordo alla definizione di sistema
passivo si ha:V = δV
δxf(x, u) ≤ yTu
scegliendo u = −φ(y)
δVδxf(x,−φ(y)) ≤ −yTφ(y) ≤ 0
V ≤ 0
(2.4.1)
La V risulta quindi semidefinita negativa, ma per le proprieta della funzione φ(),
V = 0 solo se y=0.
y(t) = 0 ⇒ u(t) = 0 (poiche u = −φ(y)) ⇒ x(t) = 0
Per l’ipotesi di stato-zero osservabile si ha che V (x) = 0 solo se x = 0, il che implica
che l’origine globalmente asintoticamente stabile.
L’idea del teorema si basa sul fatto che la V(x) rappresenta l’energia del sistema,
e un sistema passivo ha un origine stabile. Il ruolo della funzione φ e quello di
introdurre un termine che dissipa l’energia del sistema quando x(t) 6= 0. Tale teorema
risulta molto utile per sistemi che possono essere resi passivi attraverso un feedback
(dall’uscita y) oppure attraverso la scelta dell’output. Partendo da un sistema che
non ha un uscita definita, posso sceglierne una per la quale siano verificate le ipotesi
di passivita e utilizzarla in retroazione per stabilizzare il sistema.
Pur potendo scegliere l’uscita e possibile applicare questo ragionamento solo a sistemi
per cui l’origine rappresenta un punto di equilibrio per la catena diretta. A questo
19
Cap. 2 Passivita e sistemi Passivi §2.4 Controllo basato sulla passivita
proposito e possibile utilizzare un feedback che renda passivo un sistema estendendo
cosı la teoria ad un insieme piu vasto di sistemi.
Dato un sistema del tipo:
x = f(x) + g(x)u (2.4.2)
se esiste un controllo in feedback
u = α(x) + β(x)v (2.4.3)
e una funzione di uscita h(x) tale che il sistema
x = f(x) + g(x)α(x) + g(x)β(x)vy = h(x)
(2.4.4)
sia passivo rispetto ad y e v e stato-zero osservabile allora si puo stabilizzare global-
mente asintoticamente l’origine con un controllo in retroazione dall’uscita v = −φ(y).
A titolo di esempio si puo considerare il controllo di posizione di un robot composto
da m-link. Oltre a mostrare l’uso del feedback per rendere passivo il sistema e la
scelta dell’uscita per il controllo, questo esempio e anche un caso che generalizza il
prototipo realizzato. Le equazioni del modello di un robot composto da m-link sono:
M(q)q + C(q, q)q +Dq + g(q) = u,
q ∈ Rm,u ∈ Rm,
(2.4.5)
dove:
• M(q) =MT (q) > 0 ∀q matrice di inerzia
• D = DT termini di attrito lineare.
• C termini dovuti alla forza di Coriolis e centrifuga dove C e tale che M − 2C =
−(M − 2C)T (matrice antisimmetrica)
20
Cap. 2 Passivita e sistemi Passivi §2.4 Controllo basato sulla passivita
• g(q) termine dovuti all’azione di gravita.
Si vuole progettare una legge di controllo che faccia tendere asintoticamente q a qr.
Definiamo l’errore di inseguimento e = q − qr. Sotto l’ipotesi che qr = cost vale che
e = q e e = q. Possiamo quindi riscrivere il modello come:
M(q)e + C(q, q)e +De+ g(q) = u (2.4.6)
Per l’inseguimento di posizione si deve naturalmente avere e = 0, e = 0. Tale punto
di lavoro non risulta pero un punto di equilibrio per il sistema non controllato in
catena diretta. Come osservato e pero possibile rendere il sistema compatibile con le
specifiche di passivita studiate attraverso un feedback e un eventuale scelta dell’uscita.
Se si considera:u = g(q)−Kpe+ v
Kp > 0, Kp = KTp
(2.4.7)
otteniamo il sistema:
M(q)e + C(q, q)e +De+Kpe = v (2.4.8)
Ora e possibile andare a studiare la passivita del sistema. Scegliamo una funzione V
del tipo:
V =1
2eTM(q)e +
1
2eTKpe (2.4.9)
La funzione e definita positiva. La sua derivata risulta essere:
V = eTM(q)e + 12eT Me + eTKpe
= 12eT (M − 2C)e− eTDe− eTKpe + eTv + eTKpe
(2.4.10)
Per le proprieta evidenziate delle matrici: il termine eT (M − 2C)e risulta nullo, i
termini con Kp si annullano (Kp = KTp ) e il prodotto −eTDe e minore o uguale di
zero (D > 0). Sulla base di quanto detto vale che:
V ≤ eTv (2.4.11)
21
Cap. 2 Passivita e sistemi Passivi §2.4 Controllo basato sulla passivita
Scegliendo quindi y = e o piu precisamente y = q, il sistema risulta passivo. Per
quanto riguarda la detectabilita notiamo che y(t) = 0 solo se e(t) = 0. Quindi per
ingresso nullo si ha:
M(q)e +Kpe = 0 (2.4.12)
Una derivata prima identicamente nulla comporta che anche la derivata secondo
sia nulla: ¨e(t) = 0. Poiche Kp ha rango pieno si deve necessariamente avere e(t) = 0.
y(t) = 0 ↔ e(t) = 0 → e(t) = 0 → Kpe(t) = 0 → e(t) = 0 (2.4.13)
L’ipotesi di stato-zero osservabile e quindi verificata. Essendo soddisfatte le ipotesi
del teorema posso stabilizzare globalmente con un controllo v = −φ(y) = −φ(e), con
φ(0) = 0 e yTφ(y) ≥ 0, ∀y 6= 0.
22
Capitolo 3
Equazioni del modello e sintesi del
sistema di controllo
In questa capitolo vengo modellate le equazioni del prototipo e sullabase del modello ottenuto viene sintetizzata la legge di controllo.Al fine di rispettare le specifiche imposte, il controllo si componedi due parti: un controllo di posizione e un controllo di coppia.Poiche il controllo di posizione necessita la conoscenza di tutto lostato del sistema, si ricorre all’uso di un osservatore. A questoscopo verra introdotto l’osservatore ad alto guadagno, una tipologiadi osservatore utilizzabile nei sistemi non lineari.
3.1 Equazioni del modello
Per la progettazione di una legge di controllo che svolga con precisione il compito da
eseguire, e indispensabile conoscere il modello dell’impianto. Sulla base delle equazioni
fisiche che governano la dinamica del prototipo e possibile caratterizzare un modello
affidabile su cui progettare una legge di controllo. Il prototipo realizzato deve svolgere
la funzione di un sostegno per una gamba, assecondando eventuali spasmi per poi
tornare in una posizione desiderata. Poiche tale prototipo si riferisce ad un elemento
di un dispositivo piu complesso, in fase di progettazione si e cercato di mantenere la
cinematica il piu possibile fedele al modello originale. In figura 3.1 e rappresentata la
progettazione virtuale del prototipo.
23
Cap. 3 Equazioni del modello e sintesi del sistema di controllo §3.1 Equazioni del modello
Figura 3.1: Progettazione virtuale del prototipo
L’alloggiamento per la gamba consiste in un parallelepipedo vincolato nella parte
superiore da un perno, attorno al quale e libero di ruotare. Il motore trasmette il
movimento tramite una cremagliera. Tale supporto e collegato al sostegno attraverso
una cella di carico che oltre a trasferire il movimento del motore al sostegno, misura
la forza esercitata su di esso. In figura, sull’estremita superiore del supporto, sono
visibili gli strumenti di misura che verrano definiti con precisione nel prossimo capitolo.
Per quanto riguarda la dinamica, dopo aver definito le equazioni del link mobile, e
possibile studiare le relazioni tra link e motore riducendo il modello a due ruote dentate
di diversa dimensione che ingranano tra loro.
Come schematizzato in figura 3.2(a), la dinamica del link puo essere descritta come
un solido libero di girare attorno ad un perno, soggetto alla forza di gravita ed una
coppia T2 relativa alla coppia del motore trasmessa dagli ingranaggi e ad un eventuale
spasmo del paziente. Definendo θ l’angolo formato dal link con una parallela al piano
24
Cap. 3 Equazioni del modello e sintesi del sistema di controllo §3.1 Equazioni del modello
x
y
0
0
ϑ
L
mg
T2
T2
Tm
ω
ω
m
2
a) b)
r2rm
Figura 3.2: Schema del prototipo
di lavoro, possiamo descrivere la sua dinamica come:
θ(Iz +M
(
L
2
)2
) +L
2mgcos(θ) + C2θ = T2; (3.1.1)
Il termine di inerzia che moltiplica θ e dato dal teorema di Huygens-Steiner poiche
il solido ruota attorno ad un asse che non passa per il baricentro. Il termine C2
rappresenta il coefficiente di attrito che agisce sul link. Per quanto riguarda il motore
la dinamica puo essere considerata come:
θmIm = Tm − C1θm − Cr (3.1.2)
dove Im rappresenta l’inerzia dell’asse del motore, C1 il coefficiente di attrito e Cr la
coppia resistente. Per ottenere un modello completo occorre determinare le relazioni
tra motore e link. Possiamo considerare i due elementi come due ruote dentate che
ingranano. Su questa base la coppia resistente che agisce sul motore corrispondera
alla coppia T2 generata sulla seconda ruota moltiplicata per un coefficiente dovuto
al rapporto di trasmissione introdotto dalle ruote. In figura 3.2(b) e schematizzata
l’interazione tra il motore e il link. Come mostrato in figura 3.3 la forza sviluppata
dal motore nel punto di contatto tra le ruote risulta:
25
Cap. 3 Equazioni del modello e sintesi del sistema di controllo §3.1 Equazioni del modello
Fp
r r1 2
T1 T2
Figura 3.3: Trasferimento di coppia
Fp =T1r1
(3.1.3)
considerato che la forza Fp e uguale per entrambe le ruote e che la coppia ha verso
opposto, si ottiene:
Fp =T2r2
= −T1r1
⇒ T2r1 = −T1r2 (3.1.4)
ovvero l’inverso della piu nota relazione che mette in relazione le velocita angolari:
ω1r1 = −ω2r2 (3.1.5)
Per quanto mostrato, partendo dall’equazione della dinamica del motore e conside-
rando la relazione tra Cr e T2 si ha:
θmIm = Tm − C1θ − Cr
Cr = −T2rmr2
(3.1.6)
per la (3.1.1) vale:
T2 = θ(Iz +M
(
L
2
)2
) +L
2mgcos(θ) + C2θ
da cui segue:
θmIm = Tm − C1θ +rmr2
(θ(Iz +M
(
L
2
)2
) +L
2mgcos(θ) + C2θ) (3.1.7)
Dalla (3.1.5) si esprimono θm e θm in funzione di θ e θ:
θm = − r2rmθ
θm = − r2rmθ
(3.1.8)
26
Cap. 3 Equazioni del modello e sintesi del sistema di controllo §3.1 Equazioni del modello
La (3.1.9) diventa:
−θ(Imr2rm
+ I2rmr2
) = Tm + C1θr2rm
+ (L
2mgcos(θ) + C2θ)
rmr2
(3.1.9)
con I2 = Iz +M(L2)2.
moltiplicando ambo i membri per r2rm
e definendo il rapporto di riduzione r2rm
= n si
ottiene:
θ(Imn2 + I2) = −θ(C1n
2 + C2)−L
2mgcos(θ)− Tmn (3.1.10)
Tale equazione descrive la dinamica del prototipo realizzato e potra essere usata per
la sintesi del controllo.
Come prima verifica sulla validita del modello, implementando le equazioni in am-
biente simulink, puo essere eseguito un semplice studio sulla velocita del link. Dal
datasheet del motore risulta che, con la riduzione equipaggiata, la velocita di regime e
pari a 60 rpm, (2πrad/sec) mentre la coppia generata e pari a 20.3 Kgcm (1.9894Nm).
Sulla base di questi dati e possibile dare una stima del coefficiente di attrito C1 del mo-
tore. Dall’equazione (3.1.2) che descrive la dinamica del motore, a velocita constante
e supponendo nulla la coppia resistente si ha:
Tm = C1˙θm ⇒ C1 =
Tm˙θm
⇒ C1 = 0.3166 (3.1.11)
Poiche la velocita angolare del link e legata a quella del motore attraverso la relazione
(3.1.5), simulando le equazioni della dinamica del link imponendo una valore di coppia
in ingresso Tm = 1.9894Nm, considerando nullo l’attrito e supponendo che il link sia
libero di ruotare a 360 gradi, la velocita del link deve risultare pari a θ = −θmr2rm
=
−0.6702rad/sec. In figura 3.4 e mostrata la velocita angolare del link simulata con
i valori citati. In blu e rappresentata la velocita del link, mentre in brosso quella
calcolata (0.6702rad/sec). Come si nota la velocita del link oscilla attorno ad un
27
Cap. 3 Equazioni del modello e sintesi del sistema di controllo§3.2 Sintesi del controllo di posizione
valore medio che risulta pari alla velocita calcolata. Questo e dovuto alla componente
della forza di gravita che periodicamente risulta opposta o concorde al movimento.
0 5 10 15 20 25 30 35−0.7
−0.6
−0.5
−0.4
−0.3
−0.2
−0.1
0
tempo
velo
cità
ang
olar
e lin
k
velocità angolare simulatavelocità angolare calcolata
Figura 3.4: confronto tra velocita simulata e velocita calcolata
3.2 Sintesi del controllo di posizione
Sulla base del modello ottenuto si puo procedere con la sintesi del controllo. Poiche
il controllo dovra garantire l’asservimento di posizione compatibilmente con i vincoli
di coppia massima, sara progettato in due fasi differenti. Come descritto nel capitolo
precedente, per la sintesi del controllo di posizione, si procedera con uno studio basato
sulla passivita. Ricordando il teorema 2.4.1, se si dimostra che il sistema e passivo
con una V(x) definita positiva e radialmente illimitata, e stato zero osservabile, allora
esiste un controllo in retroazione dall’uscita che stabilizza asintoticamente il sistema.
Volendo ottenere un controllo di posizione, se si considera la dinamica di errore, si
osserva che l’origine non e punto di equilibrio per la catena diretta. Per applicare
lo studio di passivita occorre quindi realizzare un feedback del tipo u = α(x) +
β(x)v in modo da ottenere un sistema che verifichi le condizioni di passivita per
28
Cap. 3 Equazioni del modello e sintesi del sistema di controllo§3.2 Sintesi del controllo di posizione
l’ingresso v e un uscita y. Per un’analisi che tenga conto di tutti gli aspetti non
lineari, si introduce nel modello una funzione h(θ) che ha il ruolo di una piu precisa
caratterizzazione dell’attrito. La funzione di attrito inizialmente ha un andamento
non lineare. Superato l’attrito di primo distacco, tende ad evolvere linearmente con la
velocita (figura 3.5). Poiche l’andamento lineare e descritto nel modello dai coefficienti
ϑ.
att
rito
andamento lineare
andamento lineare
Figura 3.5: Andamento dell’attrito al variare della velocita angolare
di attrito C1 e C2 moltiplicati per la velocita, l’idea e quella di aggiungere una funzione
h(θ) che approssimi l’andamento non lineare. In figura 3.6 e mostrata la funzione
h(θ) scelta. A differenza del caso reale, si considera che la funzione sia nulla per valori
nulli di velocita. Tale approssimazione e giustificata da una rapida crescita iniziale
della funzione. Su questa ipotesi la forza di attrito totale sara data dalla somma di
(C1n2 + C2)θ + h(θ), dove
h(0) = 0
θh(θ) > 0, ∀θ 6= 0
Sulla base delle osservazioni fatte il modello risulta:
θM + θD + h(θ) +L
2mgcos(θ) = u (3.2.1)
29
Cap. 3 Equazioni del modello e sintesi del sistema di controllo§3.2 Sintesi del controllo di posizione
ϑ.
h(ϑ).
Figura 3.6: fuzione h(θ)
dove:M = (Imn
2 + I2)D = (C1n
2 + C2)u = −nTm
Definendo l’errore di posizione e = θ−θrif dove θrif e la posizione desiderata supposta
costante nel tempo, si puo definire la dinamica di errore come:
eM + eD + h(e) +L
2mgcos(θ) = u (3.2.2)
poiche vale la relazione:
e = θ
e = θ
Risulta evidente che il punto (e(t) = 0, e(t) = 0) non e un punto di equilibrio per
il sistema descritto dalla (3.2.2). Introduciamo a questo scopo un primo controllo
feedback del tipo:
u =L
2mgcos(θ)− φ(e) + v (3.2.3)
che introduce una compensazione dell’effetto di gravita e una termine φ(·) funzione
dell’errore, dove φ(e) verifica le ipotesi:
φ(0) = 0eφ(e) > 0 ∀e 6= 0
30
Cap. 3 Equazioni del modello e sintesi del sistema di controllo§3.2 Sintesi del controllo di posizione
Sulla base del feedback descritto la (3.2.2) diventa:
eM + eD + h(e) + φ(e) = v (3.2.4)
Sul modello ottenuto e ora possibile verificare la passivita del sistema, rispetto al-
l’ingresso v e ad una uscita y ancora da definire. A tale scopo occorre scegliere una
funzione V (e, e) definita positiva, che risulti una funzione di immagazzinamento per
il sistema descritto dalla (3.2.4).
Per le proprieta della funzione φ(e) scegliendo:
V (e, e) =1
2Me2 +
∫ e
0
φ(τ)δτ (3.2.5)
ho una funzione definita positiva (V (e, e) = 0 solo se (e, e) = (0, 0)), radialmente
illimitata, continua e differenziabile, che soddisfa le ipotesi per essere una candidata
funzione di immagazzinamento per il sistema. Si procede allo studio della derivata.
V =Mee+ φ(e)e (3.2.6)
sostituendo Me dalla (3.2.4):
V = −De2 − eh(e)− φ(e)e+ ev + φ(e)e= −De2 − eh(e) + ev
(3.2.7)
ricordando che:D = costante D > 0
eh(e) > 0, ∀e 6= 0⇓
−De− eh(e) < 0, ∀e 6= 0
si puo concludere che:
V = −De2 − eh(e) + ev < ev (3.2.8)
Scegliendo come uscita y = e il sistema e strictly passive:
yv ≥ V
31
Cap. 3 Equazioni del modello e sintesi del sistema di controllo§3.2 Sintesi del controllo di posizione
in seguito a questa maggioranza stretta.
Per verificare l’osservabilita del sistema si valuta il sistema per y(t) ≡ 0 e v(t) = 0:
y(t) = 0 ↔ e(t) = 0
Poiche l’espressione vale per ogni t,
e(t) ≡ 0 ⇒ e(t) ≡ 0
Infine per com’e stata definita φ():
φ(e(t)) ≡ 0 ⇒ e(t) ≡ 0
In relazione ad un ingresso nullo, l’unica soluzione per la quale l’uscita risulta nulla
e e(t) ≡ 0. La proprieta di stato-zero osservabilita risulta verificata per il sistema
in esame. Le ipotesi del teorema 2.4.1 risulta entrambe verificate, si puo dunque
affermare che per il sistema(3.2.4), un controllore in retroazione dall’uscita y = e = θ
del tipo:v = −ρ(e)
ρ(0) = 0eρ(e) > 0 ∀e 6= 0
stabilizza globalmente, asintoticamente il sistema. La forma completa del controllore
u e quindi:
u = mgcos(θ)− φ(e)− ρ(θ) (3.2.9)
da cui si ricava che la coppia del motore in ingresso al sistema e pari a:
u = −nTm
Tm = − [mgcos(θ)−φ(e)−ρ(θ)]n
(3.2.10)
Se nel caso specifico si definisce:
φ(e) = Kpe
ρ(θ) = Kdθ(3.2.11)
32
Cap. 3 Equazioni del modello e sintesi del sistema di controllo§3.3 Osservatori ad alto guadagno
si ottiene un classico controllore proporzionale derivativo.
Riassumendo, e interessante osservare che il controllo e stato sintetizzato sulla base
della teoria della passivita partendo da un sistema che non risultava passivo. Grazie
all’utilizzo di un feedback passivizzante e alla scelta dell’uscita e stato possibile esten-
dere tale teoria ad una gamma di sistemi piu ampia. Un’altro vantaggio del controllo
basato su passivita e la sua generalita. A differenza di una feedback linearization che
prevede la conoscenza esatta del modello non lineare, il controllo basato su passivita
permette di trovare un controllore stabilizzante anche con informazioni parziali sulle
funzioni non lineari che compongono il sistema. Inoltre la grande liberta di scelta
delle funzioni controllanti permette di limitare gli sforzi del controllo e di rispetta-
re eventuali limitazioni sull’ingresso. L’unico vantaggio che la feedback linearization
presenta rispetto al controllo basato su passivita e la possibilita di assegnare arbi-
trariamente gli autovalori, e stabilire di conseguenza la velocita di convergenza del
sistema controllato.
3.3 Osservatori ad alto guadagno
Nella sintesi del controllo descritta, la stabilizzazione avviene sulla base della misura
dell’errore di posizione e sulla misura di velocita angolare. Come descritto nella
presentazione del prototipo, i sensori equipaggiati permettono di misura posizione e
forza, ma non la velocita angolare. Per la ricostruzione dell’intero stato del sistema
e necessario ricorrere all’uso di un osservatore. A tale scopo e stato implementato
un osservatore ad alto guadagno che permette di stimare lo stato di alcuni sistemi
non lineari anche se si conosce parzialmente o per nulla la componente non lineare.
33
Cap. 3 Equazioni del modello e sintesi del sistema di controllo§3.3 Osservatori ad alto guadagno
Supponiamo che un sistema non lineare possa essere ricondotto ad una forma del tipo:
x = Ax+ g(y, u)y = Cx
(3.3.1)
con (A,C) osservabile. La particolarita di questa formulazione e che la non linearita
e funzione solo dell’uscita e dell’ingresso quindi puo essere calcolata. Progettando un
osservatore del tipo:
˙x = Ax+ g(y, u) +H(y − Cx) (3.3.2)
e facile verificare che l’errore di stima x = x− x descritto da:
˙x = Ax−HCx = (A−HC)x (3.3.3)
converge asintoticamente a zero scegliendo H in modo che (A−HC) sia di Hurwitz.
Questo approccio risulta valido se si ha una conoscenza esatta della funzione g(u, y).
Gli errori di modellazione e le incertezze si riflettono direttamente sulla stima dello
stato. Supponendo di conoscere un modello nominale di g(u, y) definito g0(u, y), la
(3.3.3) diventa:
˙x = (A−HC)x+ g(u, y)− g0(u, y) (3.3.4)
Anche se e (A-HC) e di Hurwitz non e detto che compensi la perturbazione introdotta
da g − g0. Per risolvere tale problema e possibile progettare uno speciale design per
il guadagno dell’osservatore che lo renda robusto alle incertezze di modellazione di
dinamiche non lineari.
Si considera come caso generale un sistema non lineare del secondo ordine:
x1 = x2x2 = φ(x, u)y = x1
(3.3.5)
sia u = γ(x) con x = [x1, x2]T , un controllo in retroazione dallo stato che stabilizza
l’origine x=0 del sistema a ciclo chiuso:
x1 = x2x2 = φ(x, γ(x))
34
Cap. 3 Equazioni del modello e sintesi del sistema di controllo§3.3 Osservatori ad alto guadagno
Per ricostruire lo stato progettiamo l’osservatore:
˙x1 = x2 + h1(y − x1)˙x2 = φ0(x, γ(x)) + h2(y − x1)
(3.3.6)
dove φ0(x, u) descrive un modello nominale di φ(x, u).
La dinamica dell’errore di stima x risulta:
˙x2 = x2 − h1x1˙x2 = δ(x, x)− h2x1
(3.3.7)
dove δ(x, x) = φ(x, γ(x))− φ0(x, γ(x)).
La matrice dinamica dell’osservatore:
Ao =
[
−h1 1−h2 0
]
risulta di Hurwitz per ogni h1, h2 > 0. La presenza della perturbazione δ(x, x) impone
una progettazione del guadagno H = [h1, h2]T che rigetti l’effetto di δ sull’errore di
stima x. Se si considera la relazione tra x e δ per ogni δ, la funzione di trasferimento
da δ a x e:
Go(s) =1
s2 + h1s+ h2
[
1s+ h1
]
(3.3.8)
Per rendere nullo l’effetto di δ su x, Go(s) dovrebbe essere nulla. Come si osserva
questo non e possibile. L’idea e quella di scegliere h1 e h2 in modo da rendere piccolo
supω∈R |G0(jω)|. A tale scopo si definisce un guadagno H tale che h2 >> h1 >> 1.
In particolare scegliendo:
h1 =α1
ǫ, h1 =
α2
ǫ2(3.3.9)
con α1, α2 > 0, e ǫ << 1 la G0(s) puo essere riscritta come:
Go(s) =ǫ
ǫ2s2 + α1ǫs + α2
[
ǫǫs + α1
]
(3.3.10)
per la quale vale che limǫ→0G0(s) = 0.
Osservando le equazioni nel dominio del tempo, si puo riscrivere l’equazione (3.3.7)
35
Cap. 3 Equazioni del modello e sintesi del sistema di controllo§3.3 Osservatori ad alto guadagno
sulla base del guadagno H definito:
˙x1 = x2 −α1
ǫx1
˙x2 = δ − α2
ǫ2x1
(3.3.11)
operando un cambio di variabili:
η1 = x1
ǫ
η2 = x2
si ottiene
ǫη1 = −α1η1 + η2ǫη2 = −α2η1 + ǫδ
(3.3.12)
Sulla base del nuovo errore di stima introdotto, risulta evidente come ǫ → 0 riduce
l’effetto del termine δ. Tuttavia anche se per ǫ piccolo l’errore di stima η converge
piu rapidamente di x, va notato che
η1 =x1 − x1
ǫ
Nell’istante di stima iniziale t=0, si ha
x1(0) 6= x1(0)
ed η(0) sara dell’ordine di (1ǫ). La risposta transitoria potrebbe quindi contenere un
termine che tende ad essere impulsivo per ǫ→ 0. Tale andamento noto come fenomeno
di picco, e tipico degli osservatori ad alto guadagno e in alcuni casi puo portare
l’evoluzione della dinamica al di fuori della regione di stabilita. Tale fenomeno puo
essere contrastato con opportune scelte delle funzioni di controllo, o semplicemente
introducendo delle saturazioni.
Tornando a considerare il modello del prototipo trovato, e possibile ricorrere ad un
osservatore ad alto guadagno che ricostruisca lo stato, e garantisca che incertezze
di modellazione del sistema, dovute ad esempio alla presenza di attriti difficili da
36
Cap. 3 Equazioni del modello e sintesi del sistema di controllo§3.3 Osservatori ad alto guadagno
definire, non influiscano sulla stima. Riprendendo l’equazione del modello espressa
nella (3.2.1)
θM + θD + h(θ) +L
2mgcos(θ) = −nTm
dove:
M = (Imn2 + I2)
D = (C1n2 + C2)
si porta sotto forma di sistema del secondo ordine:
θ =−θD−h(θ)−L
2mgcos(θ)−nTm
M
x1 = x2x2 = − D
Mx2 − γ(x1, x2)−
nMTm
y = x1
(3.3.13)
con Tm l’ingresso del sistema e[
x1x2
]
=
[
θ
θ
]
γ(x1, x2) =h(x2) +
L2mgcos(x1)
M
L’osservatore viene definito come
˙x1 = x2 + h1(y − x1)˙x2 = − D
Mx2 −
nMTm + γ0(x1, x2) + h2(y − x1)
(3.3.14)
Definito x = x− x l’errore di stima, si ha:
˙x1 = −h1x1 + x2˙x2 = −h2x1 −
DMx2 + δ(x)
(3.3.15)
dove δ(x) = γ(x1, x2)− γ0(x1, x2). Sulla base della teoria studiata scegliendo h1 e h2
come:
h1 =α1
ǫ, h1 =
α2
ǫ2
con α1, α2 > 0, e ǫ << 1
La matrice[
−h1 1−h2 − D
M
]
37
Cap. 3 Equazioni del modello e sintesi del sistema di controllo§3.4 Sintesi del controllo di coppia
e di Hurwtiz, e la scelta di un’ alto guadagno H = [h1, h2]T garantisce la robustezza
della stima a perturbazioni dovute al termine δ. L’osservatore progettato e pari a:
˙x = Ax+
[
0− n
M
]
Tm −HCx+Hy +
[
01
]
γ0(x1, x2) (3.3.16)
= (A−HC)x+
[
0 h1− n
Mh2
] [
Tmy
]
+
[
01
]
γ0(x1, x2) (3.3.17)
= Aox+Bo
[
Tmy
]
+
[
01
]
γ0(x1, x2) (3.3.18)
con
Ao =
[
−α1
ǫ1
−α2
ǫ2− D
M
]
Bo =
[
0 α1
ǫ
− nM
α2
ǫ2
]
Tm =−1
n[mgcos(x1)− φ(x1 − θrif)− ρ(x2)]
[
x1x2
]
=
[
θ˙θ
]
3.4 Sintesi del controllo di coppia
Dopo aver progettato il controllo di posizione con relativo osservatore, resta da sod-
disfare l’ultima specifica del prototipo: il controllo di coppia. Come gia detto il
dispositivo progettato oltre a mantenere il paziente in una posizione desiderata, deve
permettere e assecondare un eventuale spasmo. Visti i rapporti di riduzione presenti
tra motore e link e la potenza sviluppata dal motore stesso, con il solo controllo di
posizione il paziente sarebbe impossibilitato a muoversi. Dal punto di vista medico,
impedire uno spasmo puo avere conseguenze negative per il paziente dal punto di vista
osseo e muscolare. Risulta quindi indispensabile un controllo che in presenza di coppie
elevate interrompa il controllo di posizione per assecondare un eventuale spasmo. Per
permettere questo tipo di comportamento si possono sfruttare i valori di forza agenti
38
Cap. 3 Equazioni del modello e sintesi del sistema di controllo§3.4 Sintesi del controllo di coppia
sul link misurati dalla cella di carico. La cella di carico e stata posizionata tra il link
e la ruota che trasferisce il movimento del motore(figura 3.7) in modo da poter misu-
rare i valori di coppia scambiati. Finche il paziente non oppone resistenza al moto del
link, i valori di forza misurati saranno contenuti. Quando invece il motore impedisce
il movimento del link la cella di carico inizia a deformarsi dando una stima della forza
sviluppata dal paziente. Se tali valori risultano al di fuori di valori limite prefissati,
il controllo deve interrompere il controllo di posizione assecondando il paziente fino a
riportare i valori di forza nell’intervallo di sicurezza. Dal punto di vista del controllo,
Figura 3.7: Dettaglio dei sensori di misura
l’idea e quella di ricavare dalla forza lineare misurata dalla cella, la coppia esercitata
sulla ruota solidale al link; nel caso in cui tale valore superi un valore massimo prede-
finito, comandare il motore affinche generi una coppia di compensazione che riporti i
valori di forza al di sotto del limite.
In riferimento alla schematizzazione in figura 3.8, si puo osservare che in relazione
ad una forza F misurata dalla cella, essendo L la distanza tra il punto di applicazione
39
Cap. 3 Equazioni del modello e sintesi del sistema di controllo§3.4 Sintesi del controllo di coppia
t
Tm
ω
ω
m
2
r2
rm
L
Fmisurata
Figura 3.8: schematizzazione del funzionamento della cella di carico
della forza e il polo di rotazione, la coppia esercitata nel giunto del link e pari a
τ = FL (3.4.1)
Definendo TMax il massimo valore di coppia desiderato, se τ > TMax dovra essere
generata una coppia proporzionale alla differenza τ − TMax che riporti τ nei valo-
ri limite. Poiche la coppia misurata puo assumere sia valori positivi che negativi,
possiamo calcolare la compensazione come:
uattenuatrice = Kc(|τ | − TMax)sign(τ) (3.4.2)
con Kc > 0.
E’ importante notare che la compensazione di coppia deve avere stesso segno della
coppia misurata, in quanto bisogna permettere il movimento del link in modo da far
diminuire la forza applicata.
Ricordando la relazione tra coppia del motore e coppia del link espressa nella (3.1.4)
si calcola la coppia che il motore deve erogare:
Tm = −1
nuattenuatrice = −
1
n[Kc(|τ | − TMax)sign(τ)] (3.4.3)
Dal punto di vista teorico, non e stato dimostrato che l’applicazione di tale controllo
garantisca il vincolo di non superare un determinato valore massimo. Interpretando
40
Cap. 3 Equazioni del modello e sintesi del sistema di controllo§3.4 Sintesi del controllo di coppia
tale espressione come un controllo si dovrebbe studiare la stabilita del sistema a ciclo
chiuso. Possiamo pero interpretare tale espressione come un nuovo riferimento di
posizione imposto dal movimento del paziente, evitando il problema dello studio di
stabilita. Su questa base appunto e garantita la convergenza a regime ma non e
possibile definire cosa accade nel transitorio.
L’ultimo aspetto da considerare e la modalita di commutazione dei controlli. Il metodo
sviluppato per risolvere il problema, consiste nel definire una funzione ψ(τ) che pesa
i due controlli, mappando i valori di coppia misurati in un intervallo [0,1]. Quando
le misurazioni della cella di carico indicano un superamento della coppia massima,
il controllo di posizione viene annullato, dando operativita al controllo in coppia.
Operativamente la funzione ψ(·) e stata realizzata mediante la tangente iperbolica,
che viene spesso usata come funzione di saturazione o come funzione sigmoidale.
Introducendo un parametro di offset (γ2) che regola la dead-zone centrale, e uno che
modella la velocita di crescita tra [0,1] (γ1), e stato possibile creare una funzione ψ(τ)
che permette di aver una rapida commutazione dei controlli pur mantenendo proprieta
di continuita e differenziabilita.
ψ(τ) =1
2tanh(γ1(|τ | − γ2)) + 1) (3.4.4)
Sulla base della funzione ψ(τ) creata, e possibile considerare un supervisor con-
troller permette la commutazione dei controllori del sottosistema :
Tm = upassivo(1− ψ(τ)) + uattenuatrice(ψ(τ)) (3.4.5)
41
Cap. 3 Equazioni del modello e sintesi del sistema di controllo §3.5 Risultati simulativi
−3 −2 −1 0 1 2 30
0.2
0.4
0.6
0.8
1
τ
ψ(τ
)
ψ(τ)
Figura 3.9: funzione di commutazione ψ(τ)
ricordando le espressioni di upassivo e uattenuatrice:
upassivo =−1
n[mgcos(x1)− φ(x1 − θrif)− ρ(x2)] (3.4.6)
uattenuatrice =−1
n[Kc(|τ | − TMax)sign(τ)] (3.4.7)
3.5 Risultati simulativi
Completata la sintesi del controllo, vengono presentati alcuni risultati simulativi ot-
tenuti in ambiente simulink. Sula base del modello ricavato nella sezione 3.1 viene
provata la legge di controllo progettata per verificare che tutte le specifiche iniziali
siano rispettate. In figura 3.10 e presentato il modello simulink che implementa il
modello e il controllo progettato.
Come definito nelle sezioni precedenti troviamo:
• MODELLO
θ(Imn2 + I2) = −θ(C1n
2 + C2)−L
2mgcos(θ)− Tmn
42
Cap. 3 Equazioni del modello e sintesi del sistema di controllo §3.5 Risultati simulativi
Figura 3.10: schema simulink del controllo
dove n rappresenta il rapporto di riduzione tra link e motore, C1,C2 coefficienti di
attrito proporzionali ala velocita, e Tm e la coppia formita dal motore che rappresenta
l’ingresso controllante del sistema. La saturazione all’uscita del controllo e stata
inserita per rispettare le limitazioni sulla coppia sviluppata imposte da specifiche
fisiche del motore.
• OSSERVATORE
˙x =Aox+Bo
[
Tmy
]
+
[
01
]
γ0(x1, x2)
[
x1x2
]
=
[
θ˙θ
]
Ao =
[
−α1
ǫ1
−α2
ǫ2− D
M
]
Bo =
[
0 α1
ǫ
− nM
α2
ǫ2
]
γ0(x1, x2) =L2mgcos(x1)
M
43
Cap. 3 Equazioni del modello e sintesi del sistema di controllo §3.5 Risultati simulativi
dove D = (C1n2 +C2) e M = (Imn
2 + I2). Il guadagno dell’osservatore e stato scelto
ponendo α1 = α2 e ǫ = 0.05.
• CONTROLLO
Per la scelta del controllo sono state scelte le funzioni φ(e) e ρ(θ) come coefficienti
Kp e Kd in modo da implementare un controllo PD basato su osservatore che viene
spesso usato per il controllo dei link dei robot.
Tm = upassivo = −1
n[mgcos(x1)−Kp(x1 − θrif )− kdx2]
Verrno inotlre treste le performance del supervisor contoller che tiene conto anche
delle misura di forza applicata al link:
Tm =upassivo(1− ψ(τ)) + uattenuatrice(ψ(τ))
uattenuatrice =−1
n[Kc(|τ | − TMax)sign(τ)]
Per quanto riguarda la scelta dei parametri di controllo, le simulazioni presentate sono
basate sulla seguente scelta di valori:
• Kp=15;
• Kd=1.5;
• Kc=12;
Contrariamente a quanto avviene nella classica progettazione del controllo, in cui si
tarano accuratamente i coefficienti per ottenere il meglio in termini di performance, in
questo caso si e prestata particolare attenzione a non creare dinamiche troppo rapide
che possano disturbare il paziente. Le alte performance del motore permetterebbe-
ro la convergenza alla posizione desiderata in pochissimi secondi. Nell’ottica di un
44
Cap. 3 Equazioni del modello e sintesi del sistema di controllo §3.5 Risultati simulativi
paziente vincolato al sostegno, una risposta troppo pronta del motore non presenta
affatto un vantaggio, ma potrebbe risultare addirittura dannoso; mentre rapida deve
essere l’azione del sistema di controllo per limitare le forze di contatto tra link ed
arto del paziente. Per la scelta dei coefficienti si e puntato piu sulle performance del
controllo in coppia che su quella del controllore di posizione. Mentre la risposta ad
eventuali spasmi deve essere molto pronta, il raggiungimento della posizione desidera-
ta deve essere conseguita con delicatezza. Tale aspetto risulta ancora piu importante
se si pensa al fatto che i paziente affetti da questi disturbi manifestano posture rigide,
ovvero statiche ma in contrazione muscolare, e un azione del motore in questo senso
potrebbe recare danno.
3.5.1 Simulazione 1
Nella prima simulazione effettuata viene presentato il solo controllo in coppia. La
misura della cella simulata, viene mantenuta a zero in modo che il controllo in coppia
venga inibito dalla funzione ψ. In figura 3.11 sono mostrati gli andamenti di velocita
e posizione. Il controllo porta il link in posizione desiderata e come si voleva i tempi
e le coppie in gioco rientrano in valori normali. L’andamento del controllo del motore
e rappresentato in figura 3.12.
Per quanto riguarda l’osservatore, si nota che ricostruisce lo stato in tempo molto
ridotto (3.13). Non si presentano particolari effetti dovuto al fenomeno di picco che
per il caso in esame risulta contenuto.
45
Cap. 3 Equazioni del modello e sintesi del sistema di controllo §3.5 Risultati simulativi
0 5 10 15−0.65
−0.6
−0.55
−0.5
−0.45
−0.4
−0.35
−0.3
sec
θ
θθ
rif
0 5 10 15−0.14
−0.12
−0.1
−0.08
−0.06
−0.04
−0.02
0
sec
velo
cità
ang
olar
e
velcoità angolare
Figura 3.11: andamento di θ e θ
3.5.2 Simulazione 2
Introducendo un’ipotetica misura della cella, si osserva il funzionamento combinato
dei due controlli. Sulla base della prima simulazione presentata, viene supposto una
contrazione del paziente che esercita una coppia sul link che ha una andamento pre-
sentato in figura 3.14. Poiche il valore di coppia massima TMax = 1Nm e la coppia
misurata raggiunge un intensita di 2Nm, il controllo di coppia deve agire per ripor-
tare i valori nei limiti. L’andamento del controllo e mostrato in figura 3.15. Come ci
si aspetta il controllo porta il link in posizione desiderata sulla base del controllo di
posizione, quando il valore misurato dall cella sale oltre valori limite, il controllo in
coppia impone una coppia di compensazione che favorisce il movimento del link. In
figura 3.16 e mostrato un dettaglio del controllo in relazione alla coppia misurata.
La coppia generata dal motore risulta proporzionale e di verso opposto alla coppia
misurata. L’effetto sul link e quindi un movimento concorde alla coppia misurata
che permette l’evoluzione di un possibile spasmo del paziente. Tale fenomeno risulta
evidente se si osserva la figura 3.17 che riporta l’andamento di velocita e posizione
46
Cap. 3 Equazioni del modello e sintesi del sistema di controllo §3.5 Risultati simulativi
0 5 10 15−0.1
−0.05
0
0.05
0.1
0.15
0.2
0.25
0.3
0.35
sec
cont
rollo
controllo
Figura 3.12: andamento del controllo Tm
del link. Raggiunta la posizione di riferimento, dopo circa 20 secondi dall’inizio della
simulazione, in corrispondenza della misura della cella di carico, la coppia fornita dal
motore permette il movimento del link nel verso concorde alla coppia misurata, per
poi riportarlo in posizione desiderata una volta che la forza misurata rientra nei valori
limite. In ultima analisi e interessante valutare le prestazioni dell’osservatore. In
figura 3.18 e riportando l’andamento della stima fornita dall’osservatore nel caso in
cui si suppone di conoscere parzialmente l’effetto della gravita (come per esempio per
incertezze sulla conoscenza della massa), e degli attriti in gioco. Il valore di ǫ e pari a
0.05. Come si nota, mentre la stima di posizione resta molto accurata, la stima della
velocita risente della perturbazione. Come studiato in precedenza, l’errore di stima
e introdotto dall’errore δ dovuto al fatto che la dinamica non lineare reale differisce
da quella nominale. Per migliorare le prestazioni occorre modificare il guadagno H
in modo che compensi tale perturbazione. Simulando lo stesso scenario ma con una
scelta di ǫ = 0.005 si ottiene l’andamento descritto in figura 3.19(a). L’azione del
guadagno riesce a compensare la perturbazione migliorando l’andamento della stima.
47
Cap. 3 Equazioni del modello e sintesi del sistema di controllo §3.5 Risultati simulativi
0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5−0.8
−0.6
−0.4
−0.2
0
0.2
sec
θ
θθ
stimata
0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5−3.5
−3
−2.5
−2
−1.5
−1
−0.5
0
sec
velo
cità
ang
olar
e
velcoità angolarevelocita stimata
Figura 3.13: Istante iniziale per la stima dello stato
18 19 20 21 22 23 24 25−0.5
0
0.5
1
1.5
2
sec
mis
uraz
ione
cel
la (
N−
m)
misurazione cella
Figura 3.14: andamento della coppia esercitata sul paziente
L’aumento del guadagno ha pero un’effetto sul fenomeno di picco. Il termine ǫ molto
piccolo accentua il comportamento impulsivo della stima negli istanti iniziali. Come
si osserva in figura 3.19(b), che riporta l’andamento della stima negli istanti iniziali,
la velocita presenta un brusco picco iniziale che la porta intorno ad un valore di -50.
In questo caso specifico tale fenomeno non influenza significativamente il sistema in
quanto il valore del controllo resta entro i limiti fisici imposti dal motore che ha il
comportamento di un filtro passa basso.
48
Cap. 3 Equazioni del modello e sintesi del sistema di controllo §3.5 Risultati simulativi
0 5 10 15 20 25 30 35 40 45 50−1.4
−1.2
−1
−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
sec
cont
rollo
controllo
Figura 3.15: andamento del controllo Tm
18 19 20 21 22 23 24 25−1.5
−1
−0.5
0
0.5
sec
cont
rollo
controllo
18 19 20 21 22 23 24 25−1
−0.5
0
0.5
1
1.5
2
2.5
sec
mis
uraz
ione
cel
la
misurazione cella
Figura 3.16: particolare del controllo in relazione alla coppia misurata
49
Cap. 3 Equazioni del modello e sintesi del sistema di controllo §3.5 Risultati simulativi
0 5 10 15 20 25 30 35 40 45 50−0.7
−0.6
−0.5
−0.4
−0.3
−0.2
sec
θ
θθ
rif
0 5 10 15 20 25 30 35 40 45 50−0.2
−0.1
0
0.1
0.2
0.3
0.4
0.5
sec
velo
cità
ang
olar
e
velcoità angolare
Figura 3.17: Posizione e velocita del link con commutazione del controllo.
0 5 10 15 20 25 30 35 40 45 50−0.7
−0.6
−0.5
−0.4
−0.3
−0.2
−0.1
0
sec
θ
θθ
stimata
0 5 10 15 20 25 30 35 40 45 50−0.5
0
0.5
sec
velo
cità
ang
olar
e
velcoità angolarevelocita stimata
Figura 3.18: stima dello stato per ǫ = 0.05
50
Cap. 3 Equazioni del modello e sintesi del sistema di controllo §3.5 Risultati simulativi
0 5 10 15 20 25 30 35 40 45 50−0.7
−0.6
−0.5
−0.4
−0.3
−0.2
−0.1
0
sec
θ
θθ
stimata
0 5 10 15 20 25 30 35 40 45 50−0.5
0
0.5
sec
velo
cità
ang
olar
e
velcoità angolarevelocita stimata
(a) stima dello stato per ǫ = 0.005e
0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5−0.7
−0.6
−0.5
−0.4
−0.3
−0.2
−0.1
0
sec
θ
θθ
stimata
0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5−50
−40
−30
−20
−10
0
sec
velo
cità
ang
olar
e
velcoità angolarevelocita stimata
(b) dettaglio degli istanti iniziali di stima
Figura 3.19: Stima dello stato con ǫ = 0.005
51
Capitolo 4
Realizzazione del prototipo e dati
sperimentali
Quest’ultimo capitolo presenta la realizzazione del prototipo sullabase del progetto sviluppato e la relativa implementazione del con-trollo. Dopo una descrizione delle principali componenti hardware esoftware del prototipo: motore, sensori di misura, driver del motore,scheda di controllo; sono presentati i dati sperimentali ottenuti.
4.1 Realizzazione del prototipo
Come descritto nel capitolo 1, il lavoro svolto e una particolarizzazione di un progetto
piu ampio che prevede un dispositivo contenitivo per l’intero corpo. La progettazione
meccanica del prototipo e stata quindi concepita in modo da rispettare il piu fedel-
mente possibile le caratteristiche cinematiche del modello originale. Il link mobile, che
funge da supporto per la gamba del paziente, e costituito da due elementi paralleli tra
loro, vincolati nella parte superiore da un perno che ne permette la rotazione. Attorno
allo stesso asse ruota una semicirconferenza dentata posizionata tra i due elementi di
sostegno, che ha il ruolo di trasferire il movimento dal motore al link. La connessione
tra ruota e link avviene tramite l’inserimento di una cella di carico che permette di
misurare la forza sviluppata sul link e di conseguenza, ricavare la coppia applicata. La
52
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.1 Realizzazione del prototipo
Figura 4.1: Progettazione del prototipo
particolare posizione e il funzionamento della cella sia in trazione che in compressione,
permette di valutare l’entita della forza sviluppata nel caso il movimento incontri la
resistenza del paziente. Il motore che trasferisce il movimento al link e un motore DC
12 volt con una potenza di circa 35W. Per ridurre la velocita e aumentare la coppia e
stato montato un riduttore 60 : 1 che permette un funzionamento a regime di 60rpm
e 20.3Kgcm ≈ 1.98Nm. Per quanto riguarda la sensoristica, il dispositivo e equipag-
giato con un potenziometro che permette di misurare l’angolo formato dal link con
un parallela al piano di lavoro, e una cella di carico per le misure di forza applicate al
link. Infine per trasferire il movimento dal motore al link, e stata calettata sull’albero
del motore una ruota dentata che ingrana con una cremagliera flessibile fissata sulla
semicirconferenza solidale al link (figura 4.2). Per quanto riguarda la gestione via
software e stata utilizzata la scheda di controllo STM32 Vl Discovery della STmicroe-
53
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.2 Gestione dei sensori
Figura 4.2: Dettaglio della modalita di trasferimento del movimento
lettronics. La scheda permette di interfacciare il dispositivo con il Matlab, che sulla
base dei dati forniti calcola la legge di controllo con la quale la scheda pilota il driver
motore.
4.2 Gestione dei sensori
Sensore di posizione
Sulla base del sistema di contrllo progettato, occorre misurare solo la posizione del
link e la forza ad esso applicata. Per la misura di posizione e stato utilizzato un poten-
ziometro rotante. Per una corretto riferimento di posizione la parte piu delicata e la
scelta del posizionamento del sensore. Dopo diversi approcci e stato scelto di fissare il
potenziometro solidale al link con l’asse della manopola parallelo all’asse di rotazione
del link. Si e scelto di fissare una ruota dentata lungo l’asse di rotazione del link, che
ingrana con un’altra ruota fissata sul potenziometro. Nel momento in cui il link inizia
a muoversi la ruota sul perno e fissa e il movimento del potenziometro attorno ad essa
54
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.2 Gestione dei sensori
genera la rotazione della manopola (figura 4.3). Una volta fissato il potenziometro sul
Figura 4.3: Dettaglio del potenziometro usato come misura di posizione
dispositivo, si procede con la taratura. I valori di tensione forniti dal potenziometro
vengono letti dalla periferica ADC della scheda che li converte in un valore a 12 bit
compreso tra 0 e 4095. E’ indispensabile conoscere la relazione che sussiste tra anglo
misurato e valore letto dall’ADC. Per calcolare tale legame, sulla base di misurazioni
sperimentali si e usato il metodo di interpolazione lineare fra diverse coppie di valori e
applicando una media, e stata ricavata la funzione di conversione finale. La taratura
e stata effettuata su 5 valori campione presi nell’arco [0,-90] gradi.
Graficando le coppie di punti ottenuti si nota che il legame puo essere giustamente ap-
Conversione ADC Angolo2830 −10
2950 −23
3170 −42
3400 −62
3680 −86
prossimato come lineare. In figura 4.4 sono rappresentati i punti campione considerati
e in verde la retta di conversione trovata.
Angolo = −0.0863 ∗ ADC + 231.6178 (4.2.1)
55
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.2 Gestione dei sensori
2500 3000 3500 4000
−90
−80
−70
−60
−50
−40
−30
−20
−10
0
10
20
Figura 4.4: Curva di taratura ottenuta per il potenziometro
Sensore di forza
Per la misura della forza applicata al link e stata utilizzata una cella di carico con 20
Kg di portata funzionante sia in trazione che in compressione. La cella di carico si
base su un ponte di Wheatstone composto da quattro estensimetri. All’interno di una
struttura in alluminio sono incollati i quattro estensimetri che sulla base della defor-
mazione della cella, dovuta alla forza applicata, restituiscono un valore di tensione.
La cella e posizionata tra il link e la semicirconferenza dentata, e trasferendo il movi-
Figura 4.5: cella di carico utilizzata
mento dalla ruota al link misura le forza agenti su di esso in entrambi le direzioni. Il
voltaggio restituito dalla cella e molto basso (pochi mV), quindi e indispensabile l’uso
di un amplificatore che renda il segnale leggibile dall’ADC della scheda di controllo.
Inoltre lavorando sia in trazione che in compressione i valori di voltaggio sono sia
positivi che negativi. L’ADC non ha la possibilita di discriminare i due casi, quindi
56
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.2 Gestione dei sensori
si e utilizzato un circuito sommatore che introduce un voltaggio di offset che sposta
lo zero della cella ad un valore di circa 1.65V che corrisponde ad un valore di lettura
medio per l’ADC.
L’amplificatore costruito e un semplice circuito che sfrutta un’amplificatore opera-
zionale in configurazione non invertente e un set di resistenze che ne definiscono il
guadagno. Per le caratteristiche della tensione in uscita dalla cella e stato scelto un
guadagno dell’amplificatore di circa 150. La tensione amplificata della cella viene poi
mandata in ingresso ad un circuito sommatore che aggiunge una tensione di offset di
circa 1.4 Volt in modo da far corrispondere lo zero della cella ad un valore di 1.65
volt.
-
+
R R12
=1KΩ =1KΩ
Vamp VADC
Voffset
R3=100Ω
R4=100ΩVampVADC = +Voffset
-
+
R R12
=1KΩ =147KΩ
Vcella
Vamp
Vcella
Vamp = (1+R \ R )1 2
Amplificatore non invertente Circuito sommatore
Figura 4.6: Schemi dei circuiti realizzati
Andando a testare l’amplificatore leggendo i valori dall’ADC della scheda, si e
notata un’eccessiva variazione dei valori ADC in riferimento ad una singola misura.
Tali variazioni sono dovute ad un consistente disturbo del segnale. L’introduzione
dell’amplificatore con un alto guadagno, oltre ad amplificare il segnale desiderato,
amplifica la componente di rumore che disturba le letture ADC. Per risolvere tale
problema e stato progettato un filtro passa basso. Sulla base delle misurazioni effet-
tuate, riportando i valori in matlab e stato possibile valutare la componente armonica
del disturbo e implementare un apposito filtro. La prima configurazione usata pre-
57
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.2 Gestione dei sensori
vede una resistenza da 1MΩ e un condensatore da 10 µF . Essendo interessati solo
alla componente continua del segnale, in prima analisi e stato scelto un filtro con un
frequenza di taglio molto bassa. In figura 4.7 e mostrato il confronto tra segnale origi-
nale e segnale filtrato. Il fatto che il segnale filtrato sia leggermente inferiore al valore
medio del segnale originale e dovuto alla caduta di tensione imposta dalla resistenza.
Il segnale ottenuto con il filtro implementato risulta soddisfacente dal punto di vista
0 50 100 150 200 250 300 350 4001400
1600
1800
2000
2200
2400
2600
2800
valo
re A
DC
uscita non filtratauscita filtrata
Figura 4.7: Confronto tra segnale filtrato e segnale originale
dell’attenuazione dei disturbi ma introduce un ritardo troppo elevato. La scelta del
condensatore introduce un eccessiva componente di ritardo che non e accettabile per
le misurazioni da eseguire. In figura 4.8 e presentato in verde l’andamento dei valori
letti dall’ADC in corrispondenza di un peso di 5Kg sulla cella. Come si nota il va-
lore di regime viene raggiunto dopo circa 350 campioni ricevuti corrispondenti ad un
tempo di circa 3,5 secondi. Per compensare tale ritardo e stato ridimensionato il con-
densatore scegliendone uno da 100nF . Tale scelta e risultata un buon compromesso
tra attenuazione dei disturbi e ritardo introdotto. Progettato il filtro e stato possibile
procedere alla taratura della cella. Usando pesi di 5Kg e 10Kg sia in trazione che
58
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.2 Gestione dei sensori
in compressione, sono stati memorizzati gli andamenti dei valori letti dall’ADC. In
figura 4.9 sono graficati i dati ottenuti.
0 100 200 300 400 500 6001950
2000
2050
2100
2150
2200
2250
2300
tempo u=100ms
valo
re A
DC
uscita cella 5 kg
Figura 4.8: Ritardo introdotto dal filtro
0 20 40 60 80 100 120 140 160 180 200500
1000
1500
2000
2500
3000
3500
4000
tempo u=100ms
valo
re A
DC
uscita cella 0 kguscita cella 5 kg compressioneuscita cella 10 kg compressioneuscita cella 5 kg trazioneuscita cella 10 kg trazione
Figura 4.9: Confronto tra segnale filtrato e segnale originale
Basandosi sulle medie dei valori misurati per ogni pesata, sono state individuate
le corrispondenze tra peso applicato e valore letto dall’ADC. Supponendo positive le
misure in trazione e negative quelle in compressione si ha:
59
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.3 IL software utilizzato
Conversione ADC Angolo3521 10 Kg2908 5 Kg2186 0 Kg1432 -5 Kg713 -10 Kg
Graficando i dati calcolati si osserva il comportamento sufficientemente lineare
della cella (figura 4.10). Come nel caso del potenziometro, con l’interpolazione lineare
500 1000 1500 2000 2500 3000 3500 4000−15
−10
−5
0
5
10
15
valore ADC
For
za (
Kg)
Figura 4.10: Relazione tra pesi e valori ADC letti
tra i punti trovati e stata definita la funzione di conversione:
Forza = 0.0068 ∗ ADC − 14.8653 (4.2.2)
Moltiplicando il valore di forza misurato per la distanza tra cella di carico e asse di
rotazione, si calcola la coppia generata dalla forza sul giunto del link.
4.3 IL software utilizzato
Per la gestione delle misure e il pilotaggio del driver del motore sulla base della legge
di controllo calcolata, viene utilizzata la scheda di controllo STm32VL. Il codice e
60
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.3 IL software utilizzato
stato scritto in C utilizzando l’ambiente di sviluppo Atollic true stuido for STM32. Il
ciclo principale su cui si basa il programma esegue le seguenti funzioni:
• lettura valori dell’ADC
• invio dei dati al matlab tramite seriale
• attesa per ricevere la legge
• pilotaggio via seriale del driver motore in base alla legge di controllo calcolata
Uso dei Timer
Il primo aspetto fondamentale per il corretto funzionamento del controllo e quello
di un tempo ciclo costante e ben definito. Per rispettare questa specifica e stata
utilizzata la periferica TIMER della scheda. La periferica in questione permette di
definire dei timer sulla base di un counter register che viene aggiornato ogni ciclo di
clock. Sulla base di due valori impostati dall’utente (periodo e prescaler) e possibile
definire un timer con un definito periodo. Nello sviluppo del progetto poiche non e
necessaria un alta frequenza di lavoro, si e scelto un periodo di 10ms pari ad una
frequenza di 100hz. Per assicurarsi che il ciclo venga iterato esattamente ogni 10ms
si e ricorso al’uso degli interrupt. Impostando specifiche funzioni e possibile generare
un interrupt ogni volta che il timer arriva alla fine di un periodo. Alla fine del ciclo
viene aggiornato un flag che indica il completamento con successo delle operazioni da
svolgere, e si attende l’eventuale completamento del tempo ciclo scelto. Infine e stata
associata all’interrupt generato alla scadenza del timer, una funzione che verifica lo
stato del flag di completamento. Se il flag risulta aggiornato il ciclo riprende, in caso
contrario il ciclo non ha terminato le operazioni nel tempo prestabilito e il programma
viene interrotto segnalando il problema all’utente con il lampeggio di due led.
61
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.3 IL software utilizzato
Uso dell’ADC
Per quanto riguarda la lettura dei segnali, si utilizza la periferica ADC. Tale periferica
sulla base di un voltaggio interno di riferimento misura le tensioni in uscita dai sensori
memorizzandoli come valori digitali a 12 bit. Nella programmazione dell’ADC, poiche
si ha piu di un segnale di ingresso, e stato necessario ricorrere all’uso del DMA (direct
memory access). La periferica ADC presenta un solo registro di memorizzazione dati.
Visto l’elevata velocita di conversione analogico digitale (pochi µsec), non e possibile
copiare via software il valore dell’ADC in un variabile di appoggio prima che il suc-
cessivo valore venga memorizzato. L’idea implementata e stata quella di utilizzare il
DMA per trasferire il dato dal registro dati ADC ad un indirizzo definito dall’utente.
E’ stato quindi programmato il DMA per spostare i dati memorizzati dall’ADC in un
vettore di appoggio da cui risultano consultabili per i diversi scopi.
Comunicazione seriale
L’ultima parte degna di nota nel software creato e il protocollo di comunicazione seria-
le. La periferica USART che gestisce la comunicazione seriale in ingresso e uscita dalla
scheda, lavora su pacchetti di informazione da 8 bit che rende necessario lo sviluppo
di protocolli di invio e ricezione di dati superiore a 8 bit. La modalita di trasmissione
dei dati si basa sulla scomposizione di un dato in singoli byte gestibili dalla USART.
Nel protocollo definito si e scelto di considerare tutti i valori come numeri a 32 bit
in modo da creare un protocollo generale applicabile a dati di ogni dimensione. Ogni
volta che un dato deve essere inviato via seriale viene chiamata un apposita funzione
che si occupa di scomporre il valore passato in 4 byte, e ad inviarli singolarmente
utilizzando le specifiche funzioni definite per la USART (applicabili solo a valori ad 8
bit).
Come si osserva nella funzione riportata, il dato viene inizialmente moltiplicato per
62
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.3 IL software utilizzato
1000 per ottenere una precisione alla terza cifra decimale (poi viene fatto un cast ad
intero e tale informazione verrebbe persa), in seguito utilizzando le operazioni di shift
di bit e l’operazione di AND sugli ultimi 8 bit di ogni dato vengono identificati i 4
byte da inviare.
void inviaDato(float data)
int data1= data*1000;
int data2=data1&0xFF;
int data3=(data1 >>8)&0xFF;
int data4=(data1 >>16)&0xFF;
int data5=(data1 >>24)&0xFF;
while(USART_GetFlagStatus(USART1,USART_FLAG_TC)==RESET);
USART_SendData(USART1,data2);
while(USART_GetFlagStatus(USART1,USART_FLAG_TC)==RESET);
USART_SendData(USART1,data3);
while(USART_GetFlagStatus(USART1,USART_FLAG_TC)==RESET);
USART_SendData(USART1,data4);
while(USART_GetFlagStatus(USART1,USART_FLAG_TC)==RESET);
USART_SendData(USART1,data5);
while(USART_GetFlagStatus(USART1,USART_FLAG_TC)==RESET);
Per quanto riguarda la ricezione bisogna eseguire un’operazione inversa a quella ap-
plicata ai dati inviati. Per prima cosa si programma la fonte di trasmissione dei dati
con un protocollo di invio uguale a quello creato per la scheda. In questo modo il
pin di ricezione della seriale riceve 4 byte distinti. Come nel caso dell’ADC, anche
qui il registro di memorizzazione dati della USART e singolo ed e necessario ricorrere
al DMA. Poiche generalmente non e possibile sapere quando un dato e stato ricevu-
to, il DMA e stato programmato in modo da generare un interrupt ogni volta che
il trasferimento dei 4 byte che compongono un dato viene completato. La funzione
associata all’interrupt del DMA, sposta i valori che il DMA ha memorizzato in un
indirizzo definito dall’utente, in una seconda variabile di appoggio in modo da rende-
re possibile una nuova ricezione dati. Inoltre viene aggiornato un flag che indica la
ricezione di un dato. Nel ciclo principale viene effettuato un controllo in polling del
flag in questione e in caso di ricezione, il dato viene ricostruito sommando tra loro i
63
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.4 Risultati sperimentali
byte opportunamente modificati con l’operazione di shift dei bit. E’ stato anche im-
plementato un metodo di identificazione della sequenza di arrivo dei dati, applicabile
nel caso di ricezione di una specifica successione di dati dagli utilizzi differenti. Nella
comunicazione con il Matlab, poiche e noto il momento in cui il dato viene inviato, si
aspetta la ricezione direttamente all’interno del ciclo principale.
4.4 Risultati sperimentali
Si vanno ora a mostrare i risultati ottenuti applicando la legge di controllo sul proto-
tipo realizzato. Per le simulazioni e stata usato il microcontrollore per interfacciare i
segnali di misura e il driver del motore con il matlab. Sulla base delle misure ricevute
dalla scheda, il matlab implementa il sistema di controllo progettato e calcola la legge
con cui il micro controllera il driver motore. Per comunicare con il driver motore si usa
un protocollo seriale. I valori del controllo vengono mappati in un intervallo [0,127],
interpretato dal driver come percentuale di alimentazione da erogare al motore. Il
protocollo di comunicazione inizia con un richiesta del mtalab al microcontrollore,
che inizia la trasmissione dei segnali di misura e attende l’elaborazione della legge
di controllo. I valori ricevuti dal matlab vengono filtrati con un filtro discreto che
attenua eventuali disturbi e incertezze di misura, e utilizzati per il calcolo della legge
di controllo. In figura 4.11 e riportato il prototipo.
Da dati sperimentali si e osservato che e conveniente cambiare i valori dei guadagni
per migliorare le prestazioni del prototipo. Per le simualazioni presentate si e utilizzata
la seguente scelta dei guadagni:
• Kp=20.5;
• Kd=1.5;
64
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.4 Risultati sperimentali
Figura 4.11: Immagine del prototipo realizzato
• Kc=8;
Per la comunicazione con il driver motore e stato necessario filtrare i valori di
comunicazione seriale. Inviando un comando corrispondente alla velocita del motore
che risulta troppo elevato rispetto al precedente il driver va in blocco. Nelle prime
prove simulative e stato constatato che il motore interrompe il suo funzionamento
in corrispondenza di variazioni troppo rapide imposte dalla legge di controllo. Il
problema e stato risolto filtrando i valori relativi al comando di velocita interpretati
dal driver e inibendo l’invio della legge di controllo al motore per le prime iterazioni,
in cui l’errore di stima iniziale e elevato. Nella prima simulazione si considera solo
l’asservimento di posizione. In figura 4.12 sono riportati gli andamenti di posizione
e velocita stimate del link. La posizione del link tende a quella di riferimento e la
65
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.4 Risultati sperimentali
raggiunge in poche iterazioni. Per quanto riguarda la velocita si nota il picco iniziale
dovuto all’errore di stima dell’osservatore alto guadagno. Il controllo e riportato in
figura 4.13.
0 50 100 150 200 250−2
−1.5
−1
−0.5
0
iterazioni
posi
zion
e an
gola
re s
timat
a
Poszione angolare stiamtaposizione di riferimento
0 50 100 150 200 250−15
−10
−5
0
5
iterazioni
velo
cità
ang
olar
e st
imat
a
Velocità angolare stimata
Figura 4.12: Posizione e velocita stimate (solo asservimento di posizione)
0 50 100 150 200 250−1.5
−1
−0.5
0
0.5
1
1.5
2
iterazioni
cont
rollo
Controllo
Figura 4.13: Andamento del controllo (solo asservimento di posizione)
Introducendo la misura di forza data dalla cella di carico e il termine non lineare
φ si osserva il funzionamento della commutazione dei controlli. In figura 4.14 sono
mostrate velocita e posizione stimate del link. Come si osserva, il link inizialmente si
66
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.4 Risultati sperimentali
stabilizza sul valore di riferimento. Dopo circa 150 iterazioni la posizione angolare au-
menta, poi decresce infine si stabilizza sul valore di riferimento. Tale comportamento
e dovuto all’introduzione della misura di forza. In figura 4.15 e riportato l’andamento
dei valori letti dalla cella di carico. Si nota come in corrispondenza delle variazioni di
posizione il valore di forza supera il valore limite imposto (forza di 1 Kg). Mettendo
0 50 100 150 200 250 300 350 400 450 500−1.4
−1.2
−1
−0.8
−0.6
−0.4
−0.2
0
iterazioni
posi
zion
e an
gola
re s
timat
a
Poszione angolare stiamtaposizione di riferimento
0 50 100 150 200 250 300 350 400 450 500−8
−6
−4
−2
0
2
iterazioni
velo
cità
ang
olar
e st
imat
a
Velocità angolare stimata
Figura 4.14: Posizione e velocita stimate
0 50 100 150 200 250 300 350 400 450 500−4
−3
−2
−1
0
1
2
3
4
iterazioni
mis
ura
di fo
rza
(rad
)
Forza misurata
Figura 4.15: Andamento della msiura di forza)
a confronto la posizione e la forza misurata, si nota come la posizione aumenta nella
67
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.4 Risultati sperimentali
stessa direzione della forza misurata in quanto il controllo impone una coppia che
induca un movimento concorde alla coppia agente sul link.
0 50 100 150 200 250 300 350 400 450 500−1.3
−1.2
−1.1
−1
−0.9
−0.8
−0.7
−0.6
−0.5
−0.4
−0.3
iterazioni
mis
ura
di p
osiz
ione
(ra
d)
Posizione misurata
(a) Posizione misurata
0 50 100 150 200 250 300 350 400 450 500−4
−3
−2
−1
0
1
2
3
4
iterazioni
mis
ura
di fo
rza
(rad
)
Forza misurata
(b) Forza misurata
Figura 4.16: Confronto tra posizione misurata e forza misurata
Il controllo segue l’andamento presentato in figura 4.17.
Dai risultati ottenuti si puo concludere che il controllo progettato soddisfa tutte le
specifiche imposte. L’andamento del sistema nel caso reale e comparabile con quello
simulato; e stato solo necessario introdurre dei filtri sulle misure e variare leggermente
68
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.4 Risultati sperimentali
0 50 100 150 200 250 300 350 400 450 500−2
−1.5
−1
−0.5
0
0.5
1
1.5
2co
ntro
llo
Controllo
Figura 4.17: Controllo
i coefficienti di guadagno.
69
Conclusioni
Il lavoro svolto ha portato alla realizzazione e al controllo di un link mobile atto alla
correzione e al contenimento di atteggiamenti posturali involontari. La teoria della
passivita studiata si e dimostrata uno strumento molto efficacie ed elegante per lo
studio della stabilita e la sintesi del controllo. La possibilita di introdurre un feed-
back passivation e di scegliere un’uscita in relazione alla quale verificare le ipotesi
di passivita, permette di estendere questa teoria ad una classe di sistemi molto piu
ampia. Inoltre rispetto ad approccio basato su feedback linearization, che prevede la
conoscenza esatta del modello non lineare, il controllo basato su passivita permette
di trovare un controllore stabilizzante anche con informazioni parziali sulle funzioni
non lineari che compongono il sistema. La necessita di conoscere la velocita di ro-
tazione del link, ha richiesto l’uso di un osservatore che ricostruisca lo stato. A tale
scopo e stato implementato un osservatore ad alto guadagno. Progettato il controllo
di posizione, validato dalla teoria studiata, e stato scelto di introdurre un termine non
lineare che sotto opportune condizioni, affidi la gestione del motore ad un controllo
di attenuazione della forza. L’introduzione di tale termine non invalida quanto detto
sulla passivita in quanto non introduce variazioni di segno nel controllo. Il controllo
di attenuazione ideato, sulla base di una misura di forza fornita dalla cella di ca-
rico, permette di contenere la forza esercitata su link. I dati sperimentali ottenuti
soddisfano le specifiche imposte, le performance del controllo nel caso reale risultano
70
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.4 Risultati sperimentali
comparabili al caso simulativo. Possibili sviluppi futuri nell’ambito teorico potrebbero
concentrarsi su una dimostrazione formale della stabilita del ciclo chiuso comprensivo
dell’osservatore e del termine non lineare φ. Dal punto di vista pratico si potrebbe
estendere il prototipo sviluppato ad un sostegno che sia di ausilio all’intero corpo.
71
Appendice A
Codice creato
Codice C per STm32
Listing 4.1: Codice C for STm32 VL Discovery
/∗ In c l ude s ∗/#include <s tdde f . h>#include ”stm32f10x .h”#include ”STM32vldiscovery .h”// v a r i a b i l i g l o b a l ichar end=1;char start=1;char scadenza=1;char RicComplete =0;char address , checksum , ID_Data , ID ;uint8_t byteRic [ 4 ] , byte [ 4 ] ;
// f unz i on i p r i va t e
void delay ( int n ) ;void inviaDato ( float data ) ;void riceviAck ( void ) ;void motorMove ( char , char ) ;
// ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ MAIN ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗int main ( void )
uint16_t LettureADC [ 2 ] ;
float num , req , valore1 , valore2 ;float m_angolo=−0.0863 ,q_angolo =231.6178 , m_cella= 0.0068 , q_cella=−14.7756;
int periodo , DutyCicle , i , cella=0;char command , speed , ricevi ;
ADC_InitTypeDef ADC_Struct ;GPIO_InitTypeDef GPIO_Struct ;TIM_TimeBaseInitTypeDef TIMER_Struct ;TIM_OCInitTypeDef TIMEROC_Struct ;USART_InitTypeDef Usart_Struct ;
72
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.4 Risultati sperimentali
USART_ClockInitTypeDef UsartClock_Struct ;DMA_InitTypeDef DMA_Struct ;NVIC_InitTypeDef NVIC_Struct ;
// Ab i l i t o i l c l ock per l a GPIOA,GPIOC, ADC1 e TIM2, USART1 e USART2RCC_APB2PeriphClockCmd ( RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOC | ←
RCC_APB2Periph_ADC1 | RCC_APB2Periph_USART1 , ENABLE ) ;RCC_APB1PeriphClockCmd ( RCC_APB1Periph_TIM2 | RCC_APB1Periph_USART2 , ENABLE ) ;RCC_AHBPeriphClockCmd ( RCC_AHBPeriph_DMA1 , ENABLE ) ; // a b i l i t o c l okc per DMA
// i n i z i a l i z z o i l edSTM32vldiscovery_LEDInit ( LED3 ) ;
STM32vldiscovery_LEDInit ( LED4 ) ;
GPIO_StructInit(&GPIO_Struct ) ; // r e s e t t o l a s t r u t tu r a
// i n i z i a l i z z o i l pin PC0 PC2 in modal i t a ana l og i ca per l ’ADCGPIO_Struct . GPIO_Pin=GPIO_Pin_0 | GPIO_Pin_2 ;GPIO_Struct . GPIO_Mode=GPIO_Mode_AIN ;GPIO_Init ( GPIOC ,&GPIO_Struct ) ;
GPIO_StructInit(&GPIO_Struct ) ; // r e s e t t o l a s t r u t tu r a
// PA2 USART2 TX, PA9 USART1 TXGPIO_Struct . GPIO_Pin=GPIO_Pin_2 | GPIO_Pin_9 ;
GPIO_Struct . GPIO_Mode=GPIO_Mode_AF_PP ;GPIO_Struct . GPIO_Speed= GPIO_Speed_50MHz ;GPIO_Init ( GPIOA ,&GPIO_Struct ) ;
// PA0 in i n g r e s s o per l e g g e r e i l bottone , PA10 USART1 RXGPIO_StructInit(&GPIO_Struct ) ;GPIO_Struct . GPIO_Pin=GPIO_Pin_0 | GPIO_Pin_10 ; ;GPIO_Struct . GPIO_Mode= GPIO_Mode_IN_FLOATING ;GPIO_Init ( GPIOA , &GPIO_Struct ) ;
/∗ ∗∗∗∗∗∗∗∗DMA∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗∗∗ I n i z i a l i z z o i l DMa per l ’ADC e l a USART 1∗ i n r i c e z i o n e∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/
/∗DMA per ADC ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/DMA_Struct . DMA_PeripheralBaseAddr=(uint32_t ) 0x4001244C ; //ADC1 BASE; // i n d i r i z z o ←
d e l l a p e r i f e r i c aDMA_Struct . DMA_MemoryBaseAddr= ( uint32_t ) &LettureADC ; // i n i d i r i z z o d i ←
memorizzazione ;DMA_Struct . DMA_DIR=DMA_DIR_PeripheralSRC ; // d i r e z i o n e de l t r a s f e r imen to ( c o s i i l ←
DMA l egge da l l a p e r i f e r i c a )DMA_Struct . DMA_BufferSize=2; // dimensione de l bu f f e r . uguale a l numero d i c an a l i ←
d e l l ’ adcDMA_Struct . DMA_PeripheralInc=DMA_PeripheralInc_Disable ; //non s i incrementa l ’←
i n i d i r i z z o d e l l a p e r i f e r i c aDMA_Struct . DMA_MemoryInc=DMA_MemoryInc_Enable ; // v i ene incrementato l ’ i n d i r i z z o d i←
memoriaDMA_Struct . DMA_PeripheralDataSize=DMA_PeripheralDataSize_HalfWord ; // dimensione de l ←
dato da t r a s f e r i r e in memoriaDMA_Struct . DMA_MemoryDataSize=DMA_MemoryDataSize_HalfWord ; // dimensione de l dato da←
t r a s f e r i r e da l l a p e r i f e r i c aDMA_Struct . DMA_Mode=DMA_Mode_Circular ; //modal i ta c i r c o l a r e . i l numero d i da t i da ←
t r a s f e r i r e v i ene r i s p r i s t i n a t o ogni vo l ta che s i a r r i va a zeroDMA_Struct . DMA_Priority=DMA_Priority_High ; // p r i o r i t a de l cana l e ( in questo caso ne←
useremo so l o uno )
73
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.4 Risultati sperimentali
DMA_Struct . DMA_M2M=DMA_M2M_Disable ; // d i s a b i l i t o l a funz i one d i t r a s f e r ime tno ←memory to memory
//Assegno l ’ i n i z i a l i z z a i o n e a l cana l e 1 de l DMA1 c i o e que l l o d e l l ’ADCDMA_Init ( DMA1_Channel1 ,&DMA_Struct ) ;// Ab i l i t o i l cana l e 1 de l DMADMA_Cmd ( DMA1_Channel1 , ENABLE ) ;
/∗DMA per USART RX ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/
DMA_Struct . DMA_PeripheralBaseAddr=(uint32_t ) USART1+0x04 ; // i n d i r i z z o de l USART1 ←data r e g i s t e r
DMA_Struct . DMA_MemoryBaseAddr= ( uint32_t ) &byteRic ; // i n i d i r i z z o d i ←memorizzazione ;
DMA_Struct . DMA_DIR=DMA_DIR_PeripheralSRC ; // d i r e z i o n e de l t r a s f e r imen to ( c o s i i l ←DMA l egge da l l a p e r i f e r i c a )
DMA_Struct . DMA_BufferSize=4; // dimensione de l bu f f e r .DMA_Struct . DMA_PeripheralInc=DMA_PeripheralInc_Disable ; //non s i incrementa l ’←
i n i d i r i z z o d e l l a p e r i f e r i c aDMA_Struct . DMA_MemoryInc=DMA_MemoryInc_Enable ; // v i ene incrementato l ’ i n d i r i z z o d i←
memoriaDMA_Struct . DMA_PeripheralDataSize=DMA_PeripheralDataSize_Byte ; // dimensione de l ←
dato da t r a s f e r i r e in memoriaDMA_Struct . DMA_MemoryDataSize=DMA_MemoryDataSize_Byte ; // dimensione de l dato da ←
t r a s f e r i r e da l l a p e r i f e r i c aDMA_Struct . DMA_Mode=DMA_Mode_Circular ; //modal i ta c i r c o l a r e . i l numero d i da t i da ←
t r a s f e r i r e v i ene// r i s p r i s t i n a t o ogni vo l ta che s i a r r i va a ←
zeroDMA_Struct . DMA_Priority=DMA_Priority_High ; // p r i o r i t a de l cana l eDMA_Struct . DMA_M2M=DMA_M2M_Disable ; // d i s a b i l i t o l a funz i one d i t r a s f e r ime tno ←
memory to memory
//Assegno l ’ i n i z i a l i z z a i o n e a l cana l e 5 de l DMA1 c i o e a que l l o d e l l ’ u sar t1 RXDMA_Init ( DMA1_Channel5 ,&DMA_Struct ) ;
// a b i l i t o l ’ i n t e r r up t generato quando i l t r a s f e r ime tno e completatoDMA_ITConfig ( DMA1_Channel5 , DMA_IT_TC , ENABLE ) ;// Ab i l i t o i l cana l e 5 de l DMADMA_Cmd ( DMA1_Channel5 , ENABLE ) ;
/∗ con f i gu r a z i one i n t e r r up t per USART RX∗/NVIC_Struct . NVIC_IRQChannel=DMA1_Channel5_IRQn ;NVIC_Struct . NVIC_IRQChannelCmd=ENABLE ;NVIC_Struct . NVIC_IRQChannelPreemptionPriority=0;NVIC_Struct . NVIC_IRQChannelSubPriority=1;NVIC_Init (&NVIC_Struct ) ;
/∗∗∗∗∗∗USART∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/
// ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗USART 1 per comunicare con pc// d e f i n i s c o l e p r op r i e t a d e l l a USARTUsart_Struct . USART_BaudRate=115200;Usart_Struct . USART_Parity=USART_Parity_No ;Usart_Struct . USART_StopBits=USART_StopBits_1 ;Usart_Struct . USART_WordLength=USART_WordLength_8b ;Usart_Struct . USART_Mode=USART_Mode_Tx | USART_Mode_Rx ;Usart_Struct . USART_HardwareFlowControl=USART_HardwareFlowControl_None ;// I n i z i a l i z z o l e p r op r i e t a d e l l a USARTUSART_Init ( USART1 , &Usart_Struct ) ;
74
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.4 Risultati sperimentali
// imposto i l c l ock d e l l a usar t ( non n e c e s s a r i o )UsartClock_Struct . USART_Clock=USART_Clock_Disable ;UsartClock_Struct . USART_CPHA=USART_CPHA_1Edge ;UsartClock_Struct . USART_CPOL=USART_CPOL_Low ;UsartClock_Struct . USART_LastBit=USART_LastBit_Disable ;USART_ClockInit ( USART1 ,&UsartClock_Struct ) ;
// a b i l i t o l a r i c h i e s t a d i DMA quando v i ene r i c evu to un datoUSART_DMACmd ( USART1 , USART_DMAReq_Rx , ENABLE ) ;// a b i l i t o l a usar tUSART_Cmd ( USART1 , ENABLE ) ;
// ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗USART2 per comunicare con saber tooth
// d e f i n i s c o l e p r op r i e t a d e l l a USARTUsart_Struct . USART_BaudRate=19200;Usart_Struct . USART_Parity=USART_Parity_No ;Usart_Struct . USART_StopBits=USART_StopBits_1 ;Usart_Struct . USART_WordLength=USART_WordLength_8b ;Usart_Struct . USART_Mode=USART_Mode_Tx ;Usart_Struct . USART_HardwareFlowControl=USART_HardwareFlowControl_None ;// I n i z i a l i z z o l e p r op r i e t a d e l l a USARTUSART_Init ( USART2 , &Usart_Struct ) ;
// imposto i l c l ock d e l l a usar t ( non n e c e s s a r i o )UsartClock_Struct . USART_Clock=USART_Clock_Disable ;UsartClock_Struct . USART_CPHA=USART_CPHA_1Edge ;UsartClock_Struct . USART_CPOL=USART_CPOL_Low ;UsartClock_Struct . USART_LastBit=USART_LastBit_Disable ;USART_ClockInit ( USART2 ,&UsartClock_Struct ) ;
// a b i l i t o l a usar tUSART_Cmd ( USART2 , ENABLE ) ;
// ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗PWM∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/
/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗+∗ tempo=(per i odo∗ p r e s c a l e r ) /24Mhz∗ ESEMPI :∗ 5ms−−−> per i odo=120∗ p r e s c a l e r =1000∗∗ 1ms−−−> per i odo=12000∗ p r e s c a l e r=2∗∗ 1s−−−> per i odo=0x2710∗ p r e s c a l e r=0x960∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/
TIMER_Struct . TIM_CounterMode=TIM_CounterMode_Up ;TIMER_Struct . TIM_ClockDivision=TIM_CKD_DIV1 ;periodo=500;TIMER_Struct . TIM_Period =2000; // I l t imer conta da 0 a 499TIMER_Struct . TIM_Prescaler=120−1; // d i v i s i o n e de c l ock i n t e r no Ottengo un s egn l e a←
2Khz
TIM_TimeBaseInit ( TIM2 , &TIMER_Struct ) ;TIM_ITConfig ( TIM2 , TIM_IT_Update , ENABLE ) ; // a b i l i t o l a IRQ per i l t imer2
/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗INTERRUT PER TIMER2∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ Viene generato un i n t e r r up t ogni vo l ta che i l t imer termina i l c i c l o .
75
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.4 Risultati sperimentali
∗ se non s i e con l cuso i l loop∗ i l programma vi ene i n t e r r o t t o e s i accende i l l ed verde∗/
NVIC_Struct . NVIC_IRQChannel=TIM2_IRQn ;NVIC_Struct . NVIC_IRQChannelCmd=ENABLE ;NVIC_Struct . NVIC_IRQChannelPreemptionPriority=0;NVIC_Struct . NVIC_IRQChannelSubPriority=2;NVIC_Init (&NVIC_Struct ) ;
/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/
// ∗∗∗∗∗∗∗∗∗∗∗∗∗ADC∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗
ADC_DeInit ( ADC1 ) ;// Imposto l e c a r a t t e r i s t i c h e Base de l ADCADC_Struct . ADC_Mode=ADC_Mode_Independent ;ADC_Struct . ADC_ContinuousConvMode=DISABLE ;ADC_Struct . ADC_ScanConvMode=ENABLE ;ADC_Struct . ADC_ExternalTrigConv=ADC_ExternalTrigConv_None ;ADC_Struct . ADC_DataAlign=ADC_DataAlign_Right ;ADC_Struct . ADC_NbrOfChannel=2;
// i n i z i a l i z z o l a p e r i f a e r i c aADC_Init ( ADC1 ,&ADC_Struct ) ;// a b i l i t o l a DMA reques t s u l l ADCADC_DMACmd ( ADC1 , ENABLE ) ;// a b i l i t o l a p e r i f e r i c aADC_Cmd ( ADC1 , ENABLE ) ;
/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/
// c a l i b r a z i o n e d e l l a p e r i f e r i c aADC_ResetCalibration ( ADC1 ) ;while ( ADC_GetResetCalibrationStatus ( ADC1 ) ) ;ADC_StartCalibration ( ADC1 ) ;while ( ADC_GetCalibrationStatus ( ADC1 ) ) ;
// imposto i l cana l eADC_RegularChannelConfig ( ADC1 , ADC_Channel_10 , 1 , ADC_SampleTime_55Cycles5 ) ;ADC_RegularChannelConfig ( ADC1 , ADC_Channel_12 , 2 , ADC_SampleTime_55Cycles5 ) ;
// ∗∗∗∗∗∗∗∗∗∗∗∗CICLO∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗
address=130;command=0;speed=0;scadenza=1;
/∗ ATTESA Di i n z i o comunicazione ∗/while ( start ) STM32vldiscovery_LEDOn ( LED3 ) ;riceviAck ( ) ;// ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗//
76
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.4 Risultati sperimentali
// I n i z i a l i z z o comunicazione con SabertoothSTM32vldiscovery_LEDOff ( LED3 ) ;
while ( USART_GetFlagStatus ( USART2 , USART_FLAG_TC )==RESET ) ;USART_SendData ( USART2 , 0 xAA ) ;while ( USART_GetFlagStatus ( USART2 , USART_FLAG_TC )==RESET ) ;
/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/// avvio i l t imer
TIM_Cmd ( TIM2 , ENABLE ) ;
while (1)
ADC_SoftwareStartConvCmd ( ADC1 , ENABLE ) ;while ( DMA_GetFlagStatus ( DMA1_FLAG_TC1 )==RESET ) ;
valore1=LettureADC [ 0 ] ∗ m_angolo+q_angolo ;valore2=LettureADC [ 1 ] ∗ m_cella+q_cella ;inviaDato ( valore1 ) ;inviaDato ( valore2 ) ;
// I da t i de l comando motore venogno i nv i a to come un neumero con segno .// i l segno d e f i n i s c e i l veso d i r o ta z i one
while ( RicComplete==0) ;if ( RicComplete==1)num=(( byte [ 3 ] << 24)+(byte [ 2 ] << 16)+(byte [ 1 ] << 8)+(byte [ 0 ] ) ) /1000 . 0 ;
if ( num>=0)command=0;
speed=num ;
elsecommand=1;speed=−1∗num ;
motorMove ( command , speed ) ;RicComplete =0;STM32vldiscovery_LEDOn ( LED4 ) ;
scadenza=1;while ( end ) ;end=1;// scadenza=0; //Ponendo scdenza a 0 a l l a f i e n de l c i c l o s i avvia i l c o n t r l l o s u l ←
tempo d i c i c l o// se i l c i c l o non f i n i s c e l e ope r a z i on i ne l tempo s t a b i l i t o i l prgrmma s i b l occa//e i due l ed lampeggiano
return (0) ; // System w i l l implode
/∗ ∗∗∗∗∗∗∗∗∗∗∗∗ FUNZIONI ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/
void delay ( int n )
for ( ; n != 0 ; n−−) ;
77
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.4 Risultati sperimentali
//Funzione d i In t e r r up t generato quando i l t imer 2 f i n i s c c e d i contar evoid TIM2_IRQHandler ( void ) if ( scadenza==0)
while (1) STM32vldiscovery_LEDOn ( LED3 ) ;
STM32vldiscovery_LEDOff ( LED4 ) ;delay (0 x5FFFF ) ;
STM32vldiscovery_LEDOff ( LED3 ) ;STM32vldiscovery_LEDOn ( LED4 ) ;delay (0 x5FFFF ) ;
end=0;// pu l i s c o i l f l a g d e l l ’ i n t t eup t a l t i rm en t i a l prossimo evento non ver r a chiamata l a←
f unz i oneTIM_ClearITPendingBit ( TIM2 , TIM_IT_Update ) ;
// Pul i s co i l f l a g che i nd i ca che i l t imer ha f i n i t o d i contar eTIM_ClearFlag ( TIM2 , TIM_FLAG_Update ) ;
//Funzione d i i n t e r r up t generato quando i l DMA t r a s f e r i s c e 4 byte dal r e g i s t r o DR ←d e l l a usar t
// a l l ’ i n d i r i z z o d i memorizzazione ( byteRic )void DMA1_Channel5_IRQHandler ( void )
//PROTOCLLO PER INVIO DI DATI DIFFERENTI/∗ i n t k ;
ID Data=ID Data+1;i f ( ID Data==3)
ID Data=1;
i f ( RicComplete==0)f o r ( k=0;k<4;k++)
byte [ k]=byteRic [ k ] ;
ID=ID Data ;RicComplete=1;
∗/
int k ;if ( RicComplete==0)
for ( k=0;k<4;k++)byte [ k ]=byteRic [ k ] ;
RicComplete =1;
// pu l i s c o i l f l a g d e l l ’ i n t t eup t a l t r im en t i a l prossimo evento non ver r a chiamata ←l a funz i one
DMA_ClearITPendingBit ( DMA1_IT_TC5 ) ;DMA_ClearFlag ( DMA1_FLAG_TC5 ) ;
void inviaDato ( float data ) int data1= data ∗1000;int data2=data1&0xFF ;int data3=(data1 >>8)&0xFF ;int data4=(data1 >>16)&0xFF ;
78
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.4 Risultati sperimentali
int data5=(data1 >>24)&0xFF ;while ( USART_GetFlagStatus ( USART1 , USART_FLAG_TC )==RESET ) ;USART_SendData ( USART1 , data2 ) ;while ( USART_GetFlagStatus ( USART1 , USART_FLAG_TC )==RESET ) ;USART_SendData ( USART1 , data3 ) ;while ( USART_GetFlagStatus ( USART1 , USART_FLAG_TC )==RESET ) ;USART_SendData ( USART1 , data4 ) ;while ( USART_GetFlagStatus ( USART1 , USART_FLAG_TC )==RESET ) ;USART_SendData ( USART1 , data5 ) ;while ( USART_GetFlagStatus ( USART1 , USART_FLAG_TC )==RESET ) ;
void riceviAck ( void )if ( RicComplete==1)
int ack=((byte [ 3 ] << 24)+(byte [ 2 ] << 16)+(byte [ 1 ] << 8)+(byte [ 0 ] ) ) /1000 . 0 ;
if ( ack==0xFAFA ) start=0;
RicComplete =0;
void motorMove ( char command , char speed )
checksum=(( address+command+speed )&0x7F ) ;
while ( USART_GetFlagStatus ( USART2 , USART_FLAG_TC )==RESET ) ;USART_SendData ( USART2 , address ) ;while ( USART_GetFlagStatus ( USART2 , USART_FLAG_TC )==RESET ) ;USART_SendData ( USART2 , command ) ;while ( USART_GetFlagStatus ( USART2 , USART_FLAG_TC )==RESET ) ;USART_SendData ( USART2 , speed ) ;while ( USART_GetFlagStatus ( USART2 , USART_FLAG_TC )==RESET ) ;USART_SendData ( USART2 , checksum ) ;while ( USART_GetFlagStatus ( USART2 , USART_FLAG_TC )==RESET ) ;
79
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.4 Risultati sperimentali
Codice Matlab
%% PARAMETRI INIZIALI
dati ;controllo =0;x_meno =[ 0 ; 0 ] ;
T0=0.1;filter0=tf ( 1 , [ T0 , 1 ] ) ;T1=0.05;filter1=tf ( 1 , [ T1 , 1 ] ) ;T2=0.1;filter2=tf ( 1 , [ T2 , 1 ] ) ;%bode ( filter )
filterD0=c2d ( filter0 , 0 . 0 1 ) ;filterD1=c2d ( filter1 , 0 . 0 1 ) ;filterD2=c2d ( filter2 , 0 . 0 1 ) ;[ NUM , DEN ] = tfdata ( filterD0 , ’v ’ ) ;a0=DEN (2) ;b0=NUM (2) ;[ NUM , DEN ] = tfdata ( filterD1 , ’v ’ ) ;a1=DEN (2) ;b1=NUM (2) ;[ NUM , DEN ] = tfdata ( filterD2 , ’v ’ ) ;a2=DEN (2) ;b2=NUM (2) ;%% CONFIGURAZIONE PORTA SERIALE
s=serial ( ’COM10’ ) ;set (s , ’BaudRate ’ , 115200) ;set (s , ’DataBits ’ , 8) ;set (s , ’StopBits ’ , 1) ;set (s , ’Parity ’ , ’none ’ ) ;
fopen ( s ) ; %Abilito connession
inviaDato (64250 , s ) ;%letture=(fread (s , 1 , ’ int ’ ) ) /1000;
for i=1:500
letture=(fread (s , 2 , ’ int ’ ) ) /1000;
th_misurata=letture (1) ∗radianti ;forza=letture (2) ;misura2 (i )=forza ;misura1 (i )=th_misurata ;if ( i==1)misuraF1 ( i )=th_misurata ;misuraF2 ( i )=forza ;else
misuraF1_MENO=misuraF1 (i−1) ;misura1_MENO=misura1 (i−1) ;misuraF1 ( i )=−a0∗misuraF1_MENO+b0∗misura1_MENO ;misuraF2_MENO=misuraF2 (i−1) ;misura2_MENO=misura2 (i−1) ;misuraF2 ( i )=−a1∗misuraF2_MENO+b1∗misura2_MENO ;end
pos (i )=x_meno (1) ;
80
Cap. 4 Realizzazione del prototipo e dati sperimentali §4.4 Risultati sperimentali
vel (i )=x_meno (2) ;pos_stimata=pos (i ) ;vel_stimata=vel (i ) ;u=[controllo ; misuraF1 (i ) ] ;%% OSSERVATORE
x_piu=Ao_d∗x_meno+Bo_d∗u ;y=Co_d∗x_meno ;
th_hat=y (1) ;thd_hat=y (2) ;
%% CONTROLLO
u_passivo=Kd∗thd_hat+Kp ∗( th_hat−th_rif ) ;u_coppia=Kc ∗( tau_max−abs ( misuraF2 (i ) ) ) ∗sign ( misuraF2 ( i ) ) ;phi=(tanh ( pend ∗( abs ( misuraF2 ( i ) )−shift ) )+1) /2 ;controllo=u_passivo ∗(1/ n )∗(1−phi )+u_coppia ∗phi ;if ( abs ( controllo )>Cm )
controllo=Cm∗sign ( controllo ) ;end
speed=127/Cm ;controlloMotore (i )=speed ∗controllo ;if ( i==1)controlF ( i )=controlloMotore ;else
controlF_MENO=controlF (i−1) ;control_MENO=controlloMotore (i−1) ;controlF ( i )=−a2∗controlF_MENO+b2∗control_MENO ;end
if ( abs ( ceil ( controlF ) )<=10)controlloMotore=ceil ( controlloMotore ∗3) ;end
if (i<30)inviaDato (0 , s ) ;else
inviaDato ( ceil ( controlF (i ) ) , s ) ;end
errore (i )=th_hat−th_rif ;
x_meno=x_piu ;
end
inviaDato (0 , s ) ;inviaDato (0 , s ) ;inviaDato (0 , s ) ;inviaDato (0 , s ) ;%chiudo connnessione e elimmino la seriale
fclose (s ) ;delete (s ) ;clear s ;
function inviaDato ( dato , s )intDato=cast ( dato ∗1000 , ’ int32 ’ ) ;byte=typecast ( intDato , ’ uint8 ’ ) ;fwrite (s , byte (1) ) ;fwrite (s , byte (2) ) ;fwrite (s , byte (3) ) ;fwrite (s , byte (4) ) ;
81
Elenco delle figure
1.1 STm32 VL Discovery . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.1 analogia elettrica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.2 circuito RLC di esempio . . . . . . . . . . . . . . . . . . . . . . . . . 13
3.1 Progettazione virtuale del prototipo . . . . . . . . . . . . . . . . . . . 24
3.2 Schema del prototipo . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
3.3 Trasferimento di coppia . . . . . . . . . . . . . . . . . . . . . . . . . . 26
3.4 confronto tra velocita simulata e velocita calcolata . . . . . . . . . . . 28
3.5 Andamento dell’attrito al variare della velocita angolare . . . . . . . . 29
3.6 fuzione h(θ) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
3.7 Dettaglio dei sensori di misura . . . . . . . . . . . . . . . . . . . . . . 39
3.8 schematizzazione del funzionamento della cella di carico . . . . . . . . 40
3.9 funzione di commutazione ψ(τ) . . . . . . . . . . . . . . . . . . . . . 42
3.10 schema simulink del controllo . . . . . . . . . . . . . . . . . . . . . . 43
3.11 andamento di θ e θ . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
3.12 andamento del controllo Tm . . . . . . . . . . . . . . . . . . . . . . . 47
3.13 Istante iniziale per la stima dello stato . . . . . . . . . . . . . . . . . 48
3.14 andamento della coppia esercitata sul paziente . . . . . . . . . . . . . 48
3.15 andamento del controllo Tm . . . . . . . . . . . . . . . . . . . . . . . 49
84
ELENCO DELLE FIGURE ELENCO DELLE FIGURE
3.16 particolare del controllo in relazione alla coppia misurata . . . . . . . 49
3.17 Posizione e velocita del link con commutazione del controllo. . . . . . 50
3.18 stima dello stato per ǫ = 0.05 . . . . . . . . . . . . . . . . . . . . . . 50
3.19 Stima dello stato con ǫ = 0.005 . . . . . . . . . . . . . . . . . . . . . 51
4.1 Progettazione del prototipo . . . . . . . . . . . . . . . . . . . . . . . 53
4.2 Dettaglio della modalita di trasferimento del movimento . . . . . . . 54
4.3 Dettaglio del potenziometro usato come misura di posizione . . . . . 55
4.4 Curva di taratura ottenuta per il potenziometro . . . . . . . . . . . . 56
4.5 cella di carico utilizzata . . . . . . . . . . . . . . . . . . . . . . . . . . 56
4.6 Schemi dei circuiti realizzati . . . . . . . . . . . . . . . . . . . . . . . 57
4.7 Confronto tra segnale filtrato e segnale originale . . . . . . . . . . . . 58
4.8 Ritardo introdotto dal filtro . . . . . . . . . . . . . . . . . . . . . . . 59
4.9 Confronto tra segnale filtrato e segnale originale . . . . . . . . . . . . 59
4.10 Relazione tra pesi e valori ADC letti . . . . . . . . . . . . . . . . . . 60
4.11 Immagine del prototipo realizzato . . . . . . . . . . . . . . . . . . . . 65
4.12 Posizione e velocita stimate (solo asservimento di posizione) . . . . . 66
4.13 Andamento del controllo (solo asservimento di posizione) . . . . . . . 66
4.14 Posizione e velocita stimate . . . . . . . . . . . . . . . . . . . . . . . 67
4.15 Andamento della msiura di forza) . . . . . . . . . . . . . . . . . . . . 67
4.16 Confronto tra posizione misurata e forza misurata . . . . . . . . . . . 68
4.17 Controllo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
85