FACOLTA DI INGEGNERIA` CORSO DI LAUREA ... - Roma Tor …control.disp.uniroma2.it/carnevale/archivio/Tesi/robertovollaro/... · UNIVERSITA DEGLI STUDI DI ROMA` TOR ... Il lavoro svolto

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

“Se volete un lieto fine,

questo dipende, naturalmente,

da dove interrompete la vostra storia”

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


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


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


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


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


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


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


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


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


• 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


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


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


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


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


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


ϑ.

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


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


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


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


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


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


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


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


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


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


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


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


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


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


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


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


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


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


Figura 3.18: stima dello stato per ǫ = 0.05

50


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


(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


(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


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


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


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


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


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


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


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


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;



while(USART_GetFlagStatus(USART1,USART_FLAG_TC)==RESET);

USART_SendData(USART1,data2);








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


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


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


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


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


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


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


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


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


// 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


∗ 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


// 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


//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


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


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


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


end

82

Appendice B

guida all’uso del STm32Vl

Discovery

83

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

Bibliografia

[1] Hassan K Khalil, “Non linear systems third edition”

[2] STm32Vl Discovery, “http://www.st.com/internet/evalboard/product/250863.jsp”

[3] ,STM32F100RB “http://www.st.com/internet/mcu/product/216844.jsp”

86

Documents

FACOLTA DI INGEGNERIA` CORSO DI LAUREA ... - Roma Tor …control.disp.uniroma2.it/carnevale/archivio/Tesi/robertovollaro/... · UNIVERSITA DEGLI STUDI DI ROMA` TOR ... Il lavoro svolto