3.2 Controllo in posizione
3.3.2 Controllo in ammettenza con
Tunnel-Force-Assistance
Il principio che sta alla base di ogni controllo in ammettenza, ovvero la regolazione della risposta del robot agli input ricevuti dall’esterno, può essere impiegato, in antitesi con il caso precedente, anche per la costruzione di logiche assistive. Per i casi neurologici di interesse, i desiderata sono:
• Deve essere possibile definire un task obbiettivo, personalizzato sulle esigenze del paziente, costituito da una qualsiasi rotazione nello spazio. • Si vuole che il robot assista il paziente durante il compito assegnato,
seguendo il principio dell’AAN (1.3).
• L’assistenza deve poter essere regolata sia nel tipo che nell’intensità. • E’ necessario che la volontà del paziente sia l’origine del movimento; la
libertà di movimento deve essere completa, ma il risultato deve rimanere vicino al task assegnato entro dei limiti prefissati, così da ristabilire la connessione tra intenzione e movimento corretto.
Tali necessità generano una serie di problematiche che hanno già trovato soluzio- ne in molti casi ormai parte della letteratura. La maggioranza di questi, però, riguardano robot traslanti. I dispositivi presentati in 1.2 fanno parte di questa categoria e sono esempio di successo in tali propositi. La ricerca di un algoritmo che risponda a tutte queste esigenze per il caso di un robot puramente rotativo, costituisce la principale sfida di innovazione di questo lavoro di tesi.
Descriveremo ora nel dettaglio tutti i passaggi logici e analitici che hanno portato alla soluzione proposta, partendo ancora una volta dai desiderata dell’applicazione.
Definizione del task
Sono stati già presentati in 3.2.2 gli strumenti necessari a definire una task rotativo interpolato. Tali strumenti generici vengono impiegati nel medesimo modo nel controllo in posizione e in questo algoritmo in ammettenza. Il task viene assemblato a partire da alcune pose di riferimento che vengono successivamente interpolate per creare una rotazione continua.
3.3. Controlli in ammettenza 75
Libertà di movimento
Per garantire che il paziente possa muoversi come desidera, viene mantenuto come base l’algoritmo di free-admittance presentato nella sezione precedente. Rimangono valide tutte le considerazioni fatte riguardo la regolazione della resistenza che il paziente avverte sotto il piede, nonchè tutti gli accorgimenti legati alla sicurezza. La presenza del task aggiungerà un contributo all’azione del controllo in ammettenza libero. Da qui in avanti, ogni passaggio descritto presuppone l’esistenza del controllo di ammettenza libero come background.
Aderenza al task: la Tunnel-Force-Assistance
Immaginiamo un robot il cui end-effector abbia la possibilità di traslare nello spazio con un certo numero di gdl. Definiti gli opportuni riferimenti, un task assegnato a tale robot corrisponde ad una traiettoria nello spazio carte- siano. Come spiegato in [13][31], è possibile definire un tunnel virtuale intorno a tale traiettoria. La posizione istantanea dell’end-effector rispetto ai limiti di tale regione virtuale, costituisce un’importante risorsa per la definizione di logiche assistive. In particolare è possibile progettare un controllore che, in base alla posizione rispetto ai limiti del tunnel, generi un campo di forze dalle caratteristiche definibili a piacere, in grado di contrastare l’allontanamento dalla traiettoria. Si è deciso di sfruttare questo strumento come base di partenza per la logica assistiva; nel trasferire questo concetto su PkAnkle, o qualsiasi altro robot puramente rotativo, il primo grande scoglio da affrontare riguarda il fatto che l’end-effector rimane in ogni istante fisso nello spazio, variando unicamente il proprio orientamento, di conseguenza ciò che nel caso traslante definiamo come traiettoria, non trova una corrispondenza diretta con la nostra applicazione. Si potrebbe pensare di costruire logiche del tutto analoghe a quelle dei casi traslanti, adottando come riferimento un punto della piattaforma che non coincida con il centro di rotazione. Tale punto, solidale al corpo rigi- do piattaforma e traslante nello spazio, consente di recuperare il concetto di traiettoria. Si è deciso di scartare questa ipotesi perchè legare l’intera logica assistiva ad una grandezza geometrica specifica del nostro robot, scelta in modo del tutto casuale, rappresenta una perdita di generalità troppo marcata per i nostri intenti. Diversamente, una logica costruita partendo dal concetto generico
3.3. Controlli in ammettenza 76
di giunto sferico, consente di arrivare ad una soluzione molto più generale e sopratutto spendibile su altre applicazioni.
Nel seguito spiegheremo nel dettaglio la soluzione adottata, facendo alcuni richiami ai fondamentali dell’algebra dei quaternioni.
Come già detto in 3.2.1, l’orientamento del robot è identificabile con più di una convenzione. Le variabili indipendenti scelte corrispondono ai tre angoli del cardano in tf , ma l’orientamento della piattaforma è esprimibile anche attraverso la convenzione asse angolo: questa prevede di definire un versore e uno scalare, per un totale di 4 variabili caratteristiche. Il versore rappresenta l’asse e il verso di rotazione necessari a riportare il sistema alla condizione di partenza, lo scalare identifica l’entità della rotazione necessaria.
I quaternioni ricombinano le stesse informazioni, direzione, verso e angolo, per un totale di ancora 4 variabili, in un unico vettore di quattro elementi. Rispetto agli angoli di Eulero, queste due rappresentazioni permettono di eliminare il problema delle singolarità. Inoltre, soprattutto per i quaternioni, una grande varietà di strumenti algebrici permette una più agevole interpolazione e combinazione delle rotazioni.
Mettiamo a confronto le tre rappresentazioni per un caso molto semplice, in modo da chiarire la differenza. Prendiamo una rotazione di ϑ attorno all’asse
Y di un frame destrorso qualsiasi. Andiamo a definire con le tre convenzioni,
l’orientamento del frame ruotato rispetto a quello di partenza:
Rϑ= cos ϑ 0 sin ϑ 0 1 0 − sin ϑ 0 cos ϑ matrice di rotazione iy= 0 1 0 , ϑ, R(ϑ, iy) convenzione asse-angolo
3.3. Controlli in ammettenza 77 q = qv − qr
con qv = iysin(ϑ/2) e qr = cos(ϑ/2) (3.67)
q = qx qy qz − qr quaternione (3.68)
Ciascun quaternione è quindi composto da due parti: una vettoriale ( qv
) di tre elementi, e una scalare ( qr ), comunemente detta parte reale del
quaternione. Osservando la (3.67), si intuisce che la parte vettoriale è legata contemporaneamente sia all’asse di rotazione equivalente che all’angolo. Il vettore qvè il risultato del prodotto tra il versore iye il sin ϑ/2; di conseguenza
si tratta di un vettore che giace sulla direzione definita dal versore, di norma non unitaria, bensì pari proprio a sin ϑ/2. Se avessimo scelto una qualsiasi altra rotazione, la parte vettoriale del quaternione avrebbe sempre corrisposto al versore derivante dalla convenzione asse angolo, moltiplicato per il seno di metà dell’angolo di rotazione. E’ chiaro quindi che la lunghezza di qv è dipendente
unicamente dall’entità della rotazione, comunque sia orientato l’asse equivalente. Andiamo a vedere come varia la lunghezza di qv in funzione di ϑ:
ϑ = 0 → kqvk = sin ϑ/2 = 0 qr = cos ϑ/2 = 1 → q = 0 0 0 1 ϑ = π → kqvk = sin ϑ/2 = 1 qr = cos ϑ/2 = 0 → q = i − 0 (3.69)
Dato che la stessa rotazione è esprimibile con asse e angolo positivo, o con angolo negativo e asse inverso, fissiamo come convenzione di prendere sempre angoli positivi. Qualsiasi posa può quindi essere descritta mediante una rotazione di angolo compreso tra [0; π]. Osservando i casi espressi in (3.69), si intuisce che
3.3. Controlli in ammettenza 78
la massima lunghezza di qv è pari a 1, ovvero per rotazioni di 180o questo si
trasforma in un versore. Per rotazioni inferiori la norma risulta <1, fino ad annullarsi per rotazioni nulle, dando origine al quaternione degenere unitario.
Queste considarazioni hanno portato ad un’importante astrazione, che risul- ta essere alla base del controllo innovativo: fissato un sistema di riferimento, che nel nostro caso coincide con il ground frame, qualsiasi orientamento del robot è riconducibile ad un quaternione, la cui parte vettoriale rappresenta l’asse e l’angolo attorno al quale dobbiamo ruotare per tornare al ground frame. Tale vettore è orientato nello spazio come l’asse di rotazione equivalente, e la sua norma è sempre ≤1. Tale vettore risulta quindi sempre racchiuso in una sfera di raggio unitario, la cui origine rappresenta le rotazioni degeneri, e la cui superficie rappresenta il luogo delle rotazioni di 180o attorno a qualsiasi asse.
Chiameremo tale volume sferico Spazio delle rotazioni.
Ora definiamo un task rotativo qualsiasi. Campionando tale task otteniamo un certo numero di pose intermedie, ad ognuna delle quali corrisponde un vettore nello spazio delle rotazioni: se ora colleghiamo le posizioni individuate da tali vettori nello spazio delle rotazioni, si ottiene una astrazione del task molto simile visivamente al concetto di traiettoria. Le curve che vengono disegnate sono sempre inscritte nella sfera unitaria, in particolare si ottengono curve chiuse quando durante il task partiamo e arriviamo con la stessa posa, passando da pose sempre diverse. Se invece durante la rotazione eseguiamo un’andata e un ritorno passando per le medesime pose, si ottiene una curva aperta. Stesso vale per le rotazioni semplici da una Posa A ad una Posa B, analoghe ad una sola andata. Ogni volta che la direzione dell’asse equivalente rimane fissa, la curva degenera in un tratto rettilineo. Se tutto il task avviene attorno al medesimo asse di rotazione, ciò che si ottiene è un segmento, come ad esempio in Fig.3.22b, dove l’intera rotazione è eseguita unicamente attorno all’asse Y0. Negli altri esempi in Fig.3.23,3.24 si può notare il risultato di due diverse combinazioni di pitch e yaw, la prima che produce una traiettoria aperta curva, la seconda corrispondente ad una curva chiusa. Una volta definita tale pseudo-traiettoria, abbiamo distinto due modalità di controllo, entrambe di carattere assistivo, ma basate su due concetti di assistenza diversi.
3.3. Controlli in ammettenza 79 0 1 2 3 4 5 6 7 8 9 10 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 0.8
task visto sugli angoli di cardano
t [s] [rad] roll pitch yaw (a) X0 Y0 Z0 (b) −1 −0.5 0 0.5 1 −1 −0.5 0 0.5 1 −1 −0.5 0 0.5 1 (c)
3.3. Controlli in ammettenza 80 0 1 2 3 4 5 6 7 8 9 10 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 0.8
task visto sugli angoli di cardano
t [s] [rad] roll pitch yaw (a) −0.1−0.05 0 0.05 −0.3 −0.2 −0.1 0 0.1 0.2 0.3 −0.15 −0.1 −0.05 0 0.05 0.1 Y0 Z 0 X 0 (b) −1 −0.5 0 0.5 1 −1 −0.5 0 0.5 1 −1 −0.5 0 0.5 1 (c)
3.3. Controlli in ammettenza 81 0 1 2 3 4 5 6 7 8 9 10 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 0.8
task visto sugli angoli di cardano
t [s] [rad] roll pitch yaw (a) −0.05 0 0.05 −0.2 0 0.2 −0.15 −0.1 −0.05 0 0.05 0.1 0.15 X 0 Z 0 Y 0 (b) −1 −0.5 0 0.5 1 −1 −0.5 0 0.5 1 −1 −0.5 0 0.5 1 (c)
3.3. Controlli in ammettenza 82
Tunnel force tempo dipendente Quando programmiamo il task obbiet-
tivo, definiamo anche la durata di ciascuna sezione tra le pose intermedie da interpolare, di conseguenza definiamo velocità e accelerazioni di ciascun tratto. Una volta interpolata la traiettoria e definito un istante di inizio dell’esercizio, al procedere del tempo estraiamo dal polinomio interpolante la posa di riferimento desiderata per la piattaforma. Al paziente è richiesto non solo di muoversi seguendo la traiettoria pianificata, ma di farlo anche con una legge di moto il più possibile vicina a quella definita.
Il compito affidato al controllore sarà di controllare istante per istante l’o- rientamento del robot, dipendente dall’azione del soggetto e dal controllo in ammettenza libero, confrontare tale posa con il riferimento attuale, ed esercitare sulla piattaforma una Restoring Force proporzionale all’errore di orientamento complessivo.
La posa target è costituita da un vettore nello spazio delle rotazioni, che indi- vidua un punto della traiettoria, la posa attuale è invece un vettore orientato in modo qualsiasi. La distanza tra i punti individuati da tali vettori costi- tuisce l’errore di orientamento. Questo può essere visto attraverso due componenti: una tangenziale alla pseudo traiettoria, l’altra ortogonale. Per poter definire tali componenti occorre prima ricavare la direzione tangenziale alla traiettoria nel punto corrispondente al riferimento attuale. Il modo più
ti ti+1 ti+n Ground frame ttan t* tan
Figura 3.25: Direzione puntualmente tangenziale alla traiettoria
3.3. Controlli in ammettenza 83
consecutivi della traiettoria ed effettuare la differenza vettoriale tra i vettori po- sizione. Come indicato in Fig.3.25, la procedura è efficiente se la discretizzazione della traiettoria è piuttosto fitta. Nel nostro caso operiamo con un orizzonte temporale di aggiornamento del riferimento molto breve, quindi la traiettoria è stata discretizzata con molti punti e l’approssimazione risulta efficacie. Il vettore in rosso in Fig.3.25, rappresenta la direzione tangenziale all’istante i, per ragioni algebrici è conveniente trasformarlo in un versore normalizzandolo. In figura è indicato come ttanil vettore differenza, e con t∗tanil versore normalizzato.
Definiamo con a il vettore che corrisponde alla posa attuale, con t il target attuale e con δ la loro differenza vettoriale. Ricavato il vettore δ possiamo
t a
δ
δ
˔
δ//
Figura 3.26: Differenza vettoriale tra posa attuale e target
proiettarlo sulla direzione tangenziale. Il modulo della proiezione si ricava dal prodotto scalare con t∗tan, il vettore δ// si ricava moltiplicando il modulo ancora
per t∗tan.
δ = a − t δ// = δ · t∗tan = kδk kt
∗
tank cos α = kδk cos α = δ
Tt∗
tan
3.3. Controlli in ammettenza 84
Una volta noto δ// possiamo ricavare δ⊥ nel modo seguente
δ⊥ = δ − δ// (3.71)
δ⊥ = kδ⊥k
Le due grandezze ricavate sono la base del controllo assistivo. Impostando due tolleranze che chiameremo tolltan e tollort andiamo a definire i limiti dell’errore
entro i quali il controllo non dovrà esercitare alcuna forza di richiamo verso il target, secondo il principio dell’AAN espresso in 1.3.
toll = sinϑlim
2 (3.72)
Il valore dei limiti di tolleranza è calcolato a partire da uno scostamento angolare che viene poi tradotto nella lunghezza di un vettore nello spazio delle rotazioni, utilizzando i concetti espressi in (3.67). La grandezza δ⊥ è rappresentativa dello scostamento dalla traiettoria desiderata, δ// rappresenta invece l’anticipo o il
ritardo rispetto al riferimento temporale, ovvero l’aderenza ai profili di velocità e accelerazione decisi con la legge di moto. Sono stati creati due controllori che si occupano dei due errori separatamente; i due contributi sono poi riuniti prima di generare il comando ai motori. I valori delle due tolleranze vanno a definire la forma del volume di tolleranza attorno alla posa target: due valori identici creano una sfera, due valori diversi un ellissoide. Definito l’errore di
Figura 3.27: Alcuni esempi di volume di tolleranza al variare dei parametri
3.3. Controlli in ammettenza 85
valori dei due δ, restituisca i comandi in posizione per i motori. Ciò che deside- riamo è che una volta usciti dal volume di tolleranza, il sistema reagisca con una certa ammettenza, cercando di riportare il sistema entro i limiti imposti. Per farlo immaginiamo che le lunghezze dei vettori δ⊥ e δ// siano equivalenti
a due spostamenti imposti su altrettanti sistemi massa-molla-smorzatore del secondo ordine. Il tipo di risposta di tali sistemi allo squilibrio è ciò che vogliamo ottenere con il controllo in ammettenza. Per chiarire tale procedura descriviamo con un esempio le azioni del controllore in un caso molto semplice, ancora una volta il puro pitch, ipotizzando per questo primo caso, che il soggetto rimanga passivo ovvero non eserciti spinta.
Z0
X0 Y
0
Figura 3.28: Traiettoria Pseudo-traiettoria: pseudo traiettoria
corrispondente ad una Ldm cicloidale analoga a 3.22a. Ruotando attorno ad un asse fisso alli- neato con l’asse Y0, la traiettoria risulta essere un segmento allineato con tale asse (Fig.3.28). Gli zeri della ldm coincidono con i passag- gi dall’origine, mentre la parte positiva della Ldm corrisponde alla parte di segmento sul semiasse positivo di Y0. Z0 X0 Y 0 t a = 0 Utan = 0
Figura 3.29: Fase iniziale, orientamento attuale in tolleranza Fase iniziale: al procedere del
tempo estraiamo dalla traiettoria le pose di riferimento. In figura il vetto- re t corrisponde al riferimento attuale. Attorno al target è disegnato il volu- me di tolleranza, in questo caso sferico avendo scelto un unico valore limite. La posa attuale è rappresentata da a, coincidente col vettore nullo dato che ancora non è avenuta alcuna rotazione.
3.3. Controlli in ammettenza 86
Utan = δ//− tolltan se δ// ≥ tolltan
Utan = 0 se δ//< tolltan
(3.73)
δ// è calcolato come nell’eq.(3.70). In questo caso semplificato, δ// corrisponde
al modulo dell’intero δ. Essendo la differenza a − t inferiore alla tolleranza,
Utan rimane nullo e il controllo non esercita alcuna azione.
Z0 X0 Y 0 t a = 0 Utan δ = a – t δ=δ//
Figura 3.30: Fase iniziale, orientamento attuale fuori tolleranza
mtan
rtan
ktan
Utan,U˙tan ,U¨tan
Figura 3.31: Sistema smorzato del secon- do ordine
Inizio del movimento: man ma-
no che i riferimenti procedono, rima- nendo inizialmente fermi, cresce l’erro- re di orientamento, fino a che δ// non
supera il valore di tolleranza tangen- ziale. Il vettore Utan in figura rappre-
senta la parte di errore tangenziale, con segno, eccedente il limite. Questa situazione corrisponde al caso di ade- renza perfetta alla traiettoria (δ⊥= 0)
ma ritardo sulla Ldm. Lo scalare Utan
e le sue derivante entrano nel sistema del secondo ordine in Fig.3.31, rap- presentato traslazionale per comodità ma a tutti gli effetti rotativo poichè ciò che vogliamo estrarre è una coppia. Una volta ricavato tale valore di cop- pia dalla dinamica di 3.31, vengono seguiti gli stessi passaggi visti in 3.3.1: da una coppia viene ricavata una ve- locità, da questa un delta angolo. Il
sistema del secondo ordine che realizza la coppia, segue il riferimento temporale in ogni istante. Dell’errore tangenziale non conosciamo solo l’entità ma anche la direzione essendo noto il vettore δ//. Da questo otteniamo un versore. Siamo
così in possesso dell’angolo, generato con il concetto di ammettenza, e dell’asse che costituiscono il contributo alla rotazione dovuto alla coppia di richiamo.
3.3. Controlli in ammettenza 87
E’ importante notare che il contributo dato dal tunnel e quello del controllo in ammettenza libero si sommano: più l’individuo spinge per allontanarsi dalla traiettoria, più la restoring force aumenta, rimanendo in equilibrio. Cessata la spinta, il sistema si riporta entro la tolleranza, comportandosi come una sistema smorzato del secondo ordine. La percezione del paziente sarà quindi quella di potersi muovere liberamente in un ambiente viscoso, ma uscito dall’area di tolleranza, la difficoltà di rotazione aumenta.
Ovviamente le caratteristiche del volume di tolleranza e i parametri del sistema smorzato sono completamente modificabili, in modo da ottenere la risposta desiderata e il giusto grado di assistenza.
Z0 X0 Y0 t a Utan δ= a - t δ// δ˔ Uort
Figura 3.32: Orientamento che genera er- rori in entrambe le direzioni Caso con squilibrio generico:
Vediamo un caso semplice per capi- re cosa accade se il paziente applica una spinta qualsiasi. La spinta ge- nera l’orientamento a; questo orien- tamento genera un errore rispetto al riferimento, sia in direzione tangen- ziale che ortogonale. In Fig.3.32 il volume di tolleranza è stato rappre- sentato sull’origine poichè visivamen- te e analiticamente più corretto, è comunque possibile adottare la vi-
sualizzazione più intuitiva in cui δ e il volume hanno origine in t. Se l’errore è sufficiente a superare la soglia di tolleranza, le due componenti eccedenti (Utan e Uort) caricano altrettanti sistemi di richiamo come in Fig.3.31.
Le coppie generate da tali sistemi generano un contributo in ammettenza che contrasta quello del controllore senza vincoli di traiettoria, col risultato di una maggiore resistenza alla rotazione avvertita sotto il piede. Cessata la spinta il sistema è richiamato all’interno del volume di tolleranza con la dinamica desiderata.
3.3. Controlli in ammettenza 88 Z0 X0 Y0 gtan gort g
Figura 3.33: Vettori che riassumono l’a- zione dei controllori
Il vettore g in Fig.3.33, rappre- senta il risultato finale della logica di controllo, composto dai contributi del controllore tangenziale e di quello or- togonale. Occorre ora tradurre tale vettore in un comando ai motori. Per farlo sfruttiamo ancora le convenzio- ni con le quali abbiamo costruito lo spazio delle rotazioni: g rappresenta l’asse e l’angolo del quale dobbiamo ruotare per realizzare il contributo in
ammettenza dovuto al tunnel. Costruiamo una matrice di rotazione a partire da queste due informazioni, ricorrendo all’eq.(3.1). Questa viene tradotta in tre contributi ai rispettivi angoli del cardano in tf. In questo modo abbiamo ottenuto un output della Tunnel-Force-Assistance, che nella forma è del tutto analogo a quello del controllo in ammettenza libero. I due sono antagonisti e perciò il contributo del tunnel viene sottratto a quello libero.
g → axis, angle
Rmm(axis, angle) come in eq.(3.1)
Rcardan = RtfRmmRTtf con Rtf presa da (3.6)
∆ψ = arctan −Rcardan(2,3) Rcardan(3,3) ∆ϑ = arcsin Rcardan(1, 3) ∆φ = arctan −Rcardan(1,2) Rcardan(1,1)
Quest’ultimo passaggio è stato implementato nel simulatore, mentre nel software reale l’interazione dei due controlli in ammettenza avviene a livello delle coppie. I passaggi sono i medesimi, cambia unicamente il punto in cui i contributi si riuniscono. Tale differenza è frutto dell’ottimizzazione delle prestazioni del software di controllo.
3.3. Controlli in ammettenza 89 Cella Filtro gravità FREE ADMITTANCE CONTROL Sensore virtuale Posa attuale robot + SAFE POSITION La nuova Posa sarebbe sicura? Robot Cinematica inversa Si, consenti movimento No ΔS La Posa attuale è