Indice
I
Modellazione del sistema
6
1 Modello dinamico 6
1.1 Assunzioni iniziali . . . 6
1.2 Dinamica dei link . . . 9
1.3 Dinamica dei motori . . . 9
1.4 Sistema in forma di stato . . . 10
2 Proprietà strutturali 12 2.1 Stabilità . . . 12
2.2 Raggiungibilità . . . 13
2.3 Osservabilità . . . 13
3 Pianificazione ottima 15 3.1 Configurazione iniziale e finale . . . 15
3.2 Il problema di minimo vincolato . . . 16
3.2.1 Discretizzazione . . . 17
3.2.2 Tempo minimo . . . 18
3.2.3 Ingresso minimo . . . 18
3.2.4 Limiti alle uscite . . . 19
3.3 Algoritmo lsqlin . . . 20
II
Progetto del controllore
23
4 Controllore lineare 23 4.1 Dinamica del regolatore . . . 234.2 Modello equivalente . . . 25
4.3 Implementazione e risultati . . . 26
4.4 Considerazioni . . . 27
5 Controllore non lineare 28 5.1 Feedback linearization . . . 29
5.2 Analisi critica dei risultati . . . 31
6 Controllo model-free 33 6.1 Gli obiettivi del controllo . . . 33
6.2 Schema del controllore . . . 33
6.3 Le fasi del processo . . . 35
6.4 Simulazione sul sistema non lineare . . . 36
III
Validazione sperimentale
39
6.5 Robot e setup . . . 396.7 Controllo in Iterative Learning . . . 42
7 Conclusioni 47
7.1 Riepilogo del progetto . . . 47 7.2 Analisi critica dei risultati . . . 47
Prefazione
Fin dai primordi della robotica l’uomo ha cercato di riprodurre i movimenti umani e animali attraverso cinematismi più o meno complessi, inseguendo quella naturalezza ed armoniosità che solo un sistema biologico può possedere. In particolare, i movimenti che più hanno affascinato gli studiosi sono quelli di locomozione, la cui efficienza può essere frutto solo di un lungo processo evolutivo. Una delle caratteristiche principali che permette di eseguire una tale vastità di movimenti è la naturale cedevolezza dei sistemi biologici, che li rende flessibili e robusti alle imprevedibili dinamiche dell’ambiente. La presente tesi tratta la progettazione di un robot che possa riprodurre un movimento tipico dei primati: la brachiazione. Si tratta di una tecnica per spostarsi attraverso una serie di appigli (come i rami di un albero), che consiste nel dondolarsi tra un appiglio e il successivo sfruttando l’inerzia acquisita dall’oscillazione. Data la complessità e l’elevata dinamicità del moto di brachiazione, il tentativo di riprodurre tale evoluzione in un sistema robotico rappresenta una affascinante sfida per la comunità scientifica.
Abstract
Reproduction of bio-inspired movements has attracted the attention of the robotics community for decades. Across the years, several prototypes have been presented of robots performing brachiation: a locomotion technique used by primates in order to swing and travel across the branches of trees.
This thesis concerns the dynamics study of a brachiating soft robot, i.e. flexible components are introduced in its design with the purpose of improve energy efficiency and robustness. After the formulation of the dynamics, an optimized trajectory of brachiation will be obtained and given as reference of a control framework, which design combines a low-gain feedback action with an iterative learned feedforward action (Iterative Learning Control).
Sommario
Per realizzare il moto di brachiazione sarà necessario un design che garantisca la flessibilità tipica dei sistemi biologici ed uno schema di controllo robusto ed adattivo. Ci proponiamo quindi di studiare un cinematismo capace di eseguire l’evoluzione proposta dotandolo di elementi a cedevolezza aumentata, inseren-dolo quindi nella famiglia dei soft robot. Per prima cosa, verrà affrontato uno studio basato su un modello dinamico (model-based study) per poter ricavare una traiettoria di oscillazione che possa ottimizzare le prestazioni del robot. La traiettoria ottimizzata verrà poi testata su diversi modelli ideali a complessità crescente, fino ad arrivare ad un sistema robotico non ideale.
Per quanto riguarda il controllo, partiremo da un azione in anello aperto per studiare la realizzabilità della brachiazione sul nostro sistema, basata sulla tra-iettoria ottimizzata e su una tecnica di linearizzazione retroattiva (Feedback Linearization). Passeremo poi alla implementazione di uno schema di con-trollo più complesso e robusto, basato sulla tecnica "ILC" (Iterative Learning Control) che possa far fronte alle incertezze del modello e dell’ambiente.
Stato dell’arte
In passato sono stati proposti diversi prototipi di robot brachianti, tipicamente composti da due link e un motore centrale. In [1] è stato presentato un mo-dello relativamente semplice ma efficace, controllato prevalentemente in anello aperto a cui è stata aggiunta una componente di correzione della traiettoria in base alle evoluzioni precedenti. Uno schema diverso è stato adottato in [3], dove al posto di appigli su cui aggrapparsi è stato montato un cavo sospeso in modo da richiedere una ridotta precisione di posizionamento, per cui è stato sufficiente implementare uno schema in anello aperto per il controllo. In [2] è stato studiato un modello composto da un giunto flessibile a rigidezza variabile (VSA: variable stiffness actuator) controllato in modo passivo, massimizzando lo sfruttamento dell’inerzia di oscillazione. I robot brachianti sopra citati sono illustrati in figura 1.
Figura 1: Modelli di robot brachianti ([1] a sinistra e [3] a destra)
L’efficacia di adottare elementi cedevoli in un sistema robotico è stata di-mostrata e confermata in diversi studi svolti sui soft robot. In [5] è stata dimostrata l’ottimizzazione energetica, in quanto l’accumulo di energia elasti-ca nei componenti flessibili porta ad una riduzione delle forze e delle coppie necessarie per prestazioni cicliche. In [4] sono stati studiati gli effetti di impatti e forti variazioni di energia cinetica per dimostrare la robustezza e la capacità dei soft robot di assorbire energia ed eseguire task ad elevata dinamicità.
L’Iterative Learning Control (ILC) è stato studiato in [6], [7] e [8] prevalen-temente su attuatori rigidi. In questa trattazione useremo lo schema proposto in [9], dove il principio di apprendimento iterativo è stato combinato con il design tipico dei soft robot.
Parte I
Modellazione del sistema
Per studiare in modo completo la realizzabilità di un robot brachiante è neces-sario semplificare il problema ed analizzare l’evoluzione desiderata su sistemi via via più complessi. Per prima cosa quindi, verrà affrontato il problema su un modello dinamico del robot, per poi passare alla sperimentazione sul si-stema vero e proprio. La prima analisi ci permetterà di ricavare le traiettorie desiderate e simulare l’evoluzione su un modello virtuale. Attraverso opportu-ni schemi di controllo testeremo l’evoluzione su modelli sempre più complessi, fino ad analizzare il comportamento del soft robot assemblato.
Studieremo l’evoluzione e il controllo di tre modelli del robot:
• Modello linearizzato ideale, basato sulla linearizzazione del sistema dinamico rispetto alla configurazione di equilibrio.
• Modello non lineare ideale, ottenuto dalle equazioni della dinamica
non lineare quindi più simile al reale comportamento del robot.
• Modello reale non lineare non ideale, consiste nel robot vero e proprio, approssimato dai modelli precedenti.
In questa prima fase studieremo i primi due modelli basati sul comportamento ideale del robot (analisi model-based), mentre il sistema reale verrà studiato successivamente mediante validazione sperimentale.
1
Modello dinamico
Il robot è composto da tre link, connessi in serie tramite due giunti attuati e alle cui estremità sono montati i dispositivi di afferraggio. In figura 2 è mo-strato il modello schematico del sistema. Studieremo una singola oscillazione e supporremo che il robot possa ruotare senza attrito in corrispondenza di un’estremità, schematizzandolo come un pendolo composto bidimensionale.
I motori sono dotati di rigidezza modulabile in modo da poter conferire al cinematismo la cedevolezza tipica dei soft robot. Come illustrato in figura 3 ciascun motore può essere schematizzato mediante un rotore, soggetto all’a-zione del motore stesso, che agisce sul link (statore) attraverso un elemento elastico (modello SEA: Series Elastic Actuator). Nel capitolo . . . verrà illu-strato più nel dettaglio il reale funzionamento e le caratteristiche tecniche dei motori.
1.1
Assunzioni iniziali
Prima di passare allo studio dinamico del robot è opportuno prestare atten-zione ad alcune caratteristiche del cinematismo che complicano lo sviluppo del problema:
Figura 2: Modello schematico del robot
• Il sistema è sottoattuato, ovvero il numero di giunti motorizzati è infe-riore ai gradi di libertà del robot. In particolare la rotazione rispetto al fulcro di oscillazione rimane scoperta dal controllo, non è possibile quindi ricavare una combinazione delle coppie ai motori capace di mantenere il sistema in equilibrio (se non nella configurazione di riposo).
• Durante l’oscillazione il sistema complessivo cambia la sua inerzia, per-tanto studiando un sistema linearizzato attorno alla posizione di equi-librio ci si allontana tanto più dal sistema vero e proprio (non lineare) quanto più il braccio è lontano dalla posizione di equilibrio. Ci si aspetta quindi che il comportamento del sistema linearizzato differisca da quello reale.
Introduciamo inoltre alcune ipotesi semplificative che ci permetteranno di disaccoppiare il sistema dei link da quello dei motori e ricavare agevolmente le equazioni della dinamica [11]:
• Il centro di massa dei motori giace sull’asse di rotazione del rotore • Lo statore è rigidamente connesso con il link a monte del giunto mentre
il rotore è rigidamente connesso con il link a valle
• L’energia cinetica rotazionale del motore dipende solo dalla componen-te rotorica, la matrice d’inerzia dei motori è quindi diagonale definita positiva
Siamo quindi pronti per la formulazione della dinamica del sistema link e del sistema motori. La posizione angolare dei link, in relazione alla figura 2, è definita dal vettore
q = q1 q2 q3
mentre la posizione angolare dei motori, schematizzata in figura . . . , è data dal vettore θ = " θ2 θ3 #
Si noti che i due vettori hanno dimensione diversa, essendo il robot sottoattua-to. I pedici del vettore theta sono stati scelti per migliorare l’accoppiamento logico tra motori e giunti.
1.2
Dinamica dei link
Per poter usufruire delle ipotesi semplificative del paragrafo . . . e snellire così lo studio dinamico dei motori, verranno inclusi nel sistema link anche i dispositivi di afferraggio e la carcassa dei motori. Le equazioni della dinamica sono ricavate mediante le relazioni di Eulero Lagrange:
d dt ∂L ∂ ˙q − ∂L ∂q = Q
dove L è la funzione di Lagrange, T l’energia cinetica totale e U l’energia po-tenziale totale del sistema link. Il vettore Q indica le forze esterne generalizzate relative a q. Nel nostro caso le forze esterne che agiranno su ogni coordinata lagrangiana del sistema dei link è dato dalle azioni dei motori e dall’attrito dei giunti. Chiamati ki e bi rispettivamente le costanti elastiche dei motori (vedi
figura 3) e i coefficienti di attrito viscoso dei giunti si ha
Q = 0 k2(θ2− q2) k3(θ3− q3) + −b1q˙1 −b2q˙2 −b3q˙3 = kL(q, θ) + b( ˙q)
dove b ∈ R3 è il vettore delle forze di attrito viscoso, mentre il vettore
kL ∈ R3 rappresenta l’accoppiamento elastico tra link e motori (il pedice "L"
indica che tale vettore è definito rispetto al sistema dei link). Sostituendo i fattori nell’equazione di Eulero Lagrange (si omette la formulazione di T e U ), raccogliendo le forze gravitazionali e viscose in un vettore di coppie esterne τext
e raggruppando si ottiene l’equazione della dinamica dei link:
M (q)¨q + C(q, ˙q) ˙q = kL(q, θ) + τext(q, ˙q) (1)
dove M ∈ R3×3 rappresenta la matrice di inerzia, C ∈ R3×3 la matrice di
Coriolis, g ∈ R3 il vettore delle forze gravitazionali.
1.3
Dinamica dei motori
Procedendo in modo analogo, date le assunzioni semplificative nel paragrafo . . . si ottiene " JM 1 0 0 JM 2 # "¨ θ1 ¨ θ2 # = " τ2 τ3 # + " −k2(θ2− q2) −k3(θ3− q3) # (2)
Indicando con IM la matrice d’inerzia si ottiene, in forma compatta
IMθ = k¨ M(q, θ) + τM (3)
con IM ∈ R2×2matrice d’inerzia (diagonale contenente i momenti d’inerzia dei
vettore di accoppiamento elastico tra i due sistemi. É quindi evidente la neces-sità di distinguere i due vettori di accoppiamento kLe kM attraverso il pedice.
É importante sottolineare che i due sistemi sono sì disaccoppiati algebricamen-te grazie alle assunzioni fatalgebricamen-te nel paragrafo . . . ma legati dinamicamenalgebricamen-te dal vettore delle forze elastiche.
1.4
Sistema in forma di stato
Definiamo il vettore di stato del sistema link-motori come
x = q θ ˙ q ˙ θ ∈ R10×1 e quindi Dx = ˙ q ˙ θ ¨ q ¨ θ
e consideriamo come uscita da controllare
y =
"
β γ
#
e come ingresso il vettore
u =
"
τ1
τ2
#
Si rammenta che la coordinata α, non essendo direttamente controllabile dai motori, seguirà solo l’evoluzione libera del sistema ed è stata esclusa dal vettore
y. Trattandosi di un pendolo composto, scegliamo senza indugi l’equilibrio
attorno a cui linearizzeremo come
xeq = 0 .. . 0 ∈ R 10×1
Siamo in grado quindi di ricavare le matrici A e B della forma di stato, ottenute derivando il vettore Dx rispetto a x e u rispettivamente e sostituendo a queste il vettore x con xeq. Per semplicità grafica, non riportiamo qui di
seguito le matrici ottenute ma solo la loro dimensione:
Dx = Ax + Bu
con A ∈ Rn×n B ∈ Rn×m C = " 0 1 0 0 · · · 0 0 0 1 0 · · · 0 # ∈ Rl×n dove n = 10 e m = l = 2.
2
Proprietà strutturali
Prima di passare al progetto del controllore è necessario effettuare un’analisi del sistema dinamico in termini di stabilita, raggiungibilità ed osservabilità. Queste proprietà ci permetteranno di avanzare nel progetto utilizzando op-portuni strumenti della teoria dei controlli automatici, quali la pianificazione ottima vincolata e la regolazione dei sistemi.
2.1
Stabilità
Benché possa risultare poco interessante intraprendere un’analisi di stabilità per un semplice pendolo composto, verrà illustrato di seguito un procedimento molto usato nella teoria dei controlli per stimare la regione di asintotica
sta-bilità (RAS). In questa sede tale procedimento è stato utilizzato come verifica
di una corretta scrittura del sistema dinamico.
Per studiare la stabilità del sistema (non lineare) dalla generica espressio-ne Dx = f (x, t), utilizziamo il teorema di Lyapunov. Cerchiamo quindi una
candidata di Lyapunov, ovvero una funzione V ∈ C1 tale che la sua derivata
direzionale lungo f sia non positiva
LfV =
dV dxf ≤ 0
Essendo per sistemi LTI
dV dxf = dV dx dx dt = dV dt = ˙V
la condizione da soddisfare diventa ˙V ≤ 0.
Studiamo ora il sistema linearizzato ˙x = Ax e scegliamo come candidata di Lyapunov una funzione quadratica definita come
V = xTP x
con P ∈ Rn×n simmetrica definita positiva. Derivando otteniamo1 V =˙
−xTQx dove Q verifica l’equazione di Lyapunov
P A + APT = −Q ovvero −Q è la parte simmetrica di 2P A.
Per avere asintotica stabilità è necessario che ˙V < 0, ovvero che Q sia
defi-nita positiva. Per trovare la funzione di Lyapunov dobbiamo quindi prendere una matrice Q definita positiva arbitraria e tramite l’equazione di Lyapunov ricavare P e infine V = xTP x.
Una volta ricavata V , è possibile dare una stima cautelativa della RAS verificando che una certa curva di livello V (x) = k delimiti una regione in cui vale ˙V < 0, di conseguenza quella regione appartiene alla RAS e ne dà una
stima per difetto.
Per fare questo, calcoliamo per un certo k la derivata ˙V per un insieme di
punti appartenenti a V = k e verifichiamo che essa sia strettamente minore di zero. Ripetendo tale calcolo per k via via crescenti e trovato il valore massimo di k che verifica la condizione di assoluta stabilità, la RAS conterrà la regione interna a V = k.
Se k = ∞ la RAS coincide con lo spazio di stato e il sistema si dice
globalmente asintoticamente stabile (GAS).
In allegato [A1] è riportato uno script eseguito su Matlab che compie il calcolo sopra descritto.
Applicando questo algoritmo al braccio robotico otteniamo (come era da aspettarsi) che la funzione di Lyapunov risulta radialmente illimitata e che quindi la RAS coincide con l’intero spazio di stato e il sistema è globalmente asintoticamente stabile.
2.2
Raggiungibilità
Il sottospazio di raggiungibilità è l’insieme degli stati che possono essere rag-giunti dal sistema a partire da un certo stato iniziale x0 utilizzando una
opportuna combinazione degli ingressi.
Tale insieme è definito come R = Im(R) dove R è la matrice di
raggiungi-bilità data da
R =hB AB A2B · · · An−1Bi
La situazione più vantaggiosa per progettare una legge di controllo si ha quando tutto lo spazio di stato è raggiungibile dal sistema a partire da un generico stato iniziale. Tale condizione risulta verificata quando l’insieme dei punti raggiungibili coincide con lo spazio di stato, ovvero
dim(R) = rank(R) = n
e prende il nome di completa raggiungibilità.
Costruendo la matrice di raggiungibilità R per il robot in esame e valutato-ne il rango, si ha che il sistema è completamente raggiungibile. Esisterà quindi almeno una combinazione degli ingressi che permetterà al robot di raggiungere ad un certo istante t la postura e le velocità desiderate.
É importante sottolineare che la legge degli ingressi non è unica ed è stret-tamente legata all’istante t nel quale si desidera che il sistema raggiunga lo stato finale, oltre che a condizioni di ottimizzazione che verranno affrontate nei paragrafi successivi.
2.3
Osservabilità
Ai fini del progetto di un controllore in anello chiuso, che quindi prende in ingresso le uscite del sistema, è importante ricostruire l’evoluzione dello stato a partire dalla lettura delle uscite nell’arco di tempo considerato. Questa ricostruzione non è possibile se due stati calcolati allo stesso istante t sono
indistinguibili ovvero applicando la stessa legge di controllo a partire da essi
raggiungo il medesimo stato finale al tempo tf.
Si definisce insieme di indistinguibilità ¯O l’insieme di tutti gli stati indi-stinguibili e si ha ¯ O = ker(O) con O = C CA CA2 .. . CAn−1
dove O prende il nome di matrice di osservabilità. Per poter ricostruire lo stato a qualunque istante di tempo precedente alla lettura delle uscite è necessario che il sottospazio di indistinguibilità sia costituito dalla sola origine
ker(O) = 0
ovvero
rank(O) = n
tale condizione prende il nome di completa osservabilità.
Analogamente al caso di raggiungibilità, costruiamo la matrice di osser-vabilità del sistema e ne calcoliamo il rango. Essendo quest’ultimo ugua-le alla dimensione del sistema, concludiamo che il sistema è compugua-letamente osservabile.
3
Pianificazione ottima
Una volta ricavata la dinamica del sistema complessivo, passiamo a definire la traiettoria desiderata in modo da fornire un riferimento al controllore, il cui schema verrà definito nei prossimi capitoli.
La pianificazione ottima consente di ricavare tale traiettoria per via indi-retta, ovvero cercando prima la combinazione degli ingressi (coppie ai motori) che guidi il robot da una postura iniziale ad una finale. Una volta ricavati gli ingressi, attraverso le equazioni del sistema in forma di stato, si ottiene l’evo-luzione cinematica del robot, ovvero la traiettoria richiesta. Il set di ingressi, come dimostrato in [10], esiste e non è unico, essendo il sistema completamente raggiungibile. Abbiamo quindi a disposizione un insieme infinito di combina-zioni di ingressi che possa portare il robot dalla configurazione iniziale a quella finale. Questo ci permette di poter selezionare, tra gli infiniti set, quello che risponde ad una serie di vincoli progettuali che costituiscono i parametri di ottimizzazione.
L’algoritmo della pianificazione ottima ricava il set di ingressi imponendo che l’entità degli ingressi stessi (ovvero la sua norma) sia minima. Gli ulteriori vincoli che andremo ad imporre sono:
• Che il sistema compia l’evoluzione nel minor tempo possibile
• Che gli ingressi non superino un certo valore massimo, dato dalle speci-fiche del motore utilizzato
• Che le posizioni e le velocità angolari dei giunti siano limitate da oppor-tuni valori massimi
3.1
Configurazione iniziale e finale
Studiamo la configurazione che assume il sistema a inizio e fine evoluzione, ovvero quando entrambi i gripper stanno afferrando gli appigli. In riferimento a figura 4, si è scelto per il robot una postura simmetrica rispetto al link centrale, per cui sono definite le configurazioni iniziale e finale in funzione delle distanze di e df espresse dai vettori
Figura 4: Postura iniziale e finale del robot
Dalle posture ricaviamo gli stati iniziale e finale, imponendo che le velocità nelle due configurazioni scelte siano nulle e che le posizioni angolari dei motori siano coincidenti con quelle dei giunti.
3.2
Il problema di minimo vincolato
L’obiettivo della pianificazione ottima è quello di ottenere una possibile evo-luzione del robot in cui un certo costo, nel nostro caso le coppie erogate dai motori, siano minimizzate. Questo porta alla definizione di un problema di minimo vincolato, in cui si vuole trovare il valore minimo di una certa gran-dezza (gli ingressi) soggetta ad una serie di vincoli. Tali vincoli di fatto ci permettono di definire completamente il problema, in quanto potremo legare gli ingressi alla dinamica del sistema ed ottenere la traiettoria desiderata at-traverso specifici parametri progettuali. La definizione algebrica del problema di minimo vincolato e la sua implementazione su computer sono illustrati in [10] e in allegato ... .
La pianificazione ottima si basa quindi sul minimizzare gli ingressi (u(t)), vincolandoli attraverso le equazioni dinamiche del sistema linearizzato. I pa-rametri che consentono di definire completamente il problema sono:
• tf: tempo finale dell’evoluzione. É richiesto che il moto avvenga nel
minor tempo possibile, per cui anche tf deve essere minimo. Se però è
(relativamente) piccolo, ovvero se chiediamo al robot di eseguire l’evo-luzione in un arco di tempo molto ristretto, l’entità degli ingressi cresce sensibilmente. Scegliamo di definire il valore massimo delle coppie eroga-bili; il tempo di evoluzione minimo è quindi ottenibile per via iterativa, risolvendo il problema di minimo vincolato e aumentando tf fino a che
l’algoritmo non trova una soluzione.
• xmax: entità massima degli stati, ovvero massime ampiezze e velocità
dei giunti e degli attuatori. Questo parametro deve essere impostato compatibilmente con i vincoli fisici dei componenti del robot, che saranno introdotti nei capitoli successivi.
• umax: coppia massima (in modulo), dipendente dalle specifiche dei motori
utilizzati e ridotte attraverso un coefficiente di sicurezza.
Il problema di minimo vincolato consiste quindi nel ricavare
u : min u 2 soggetto a ˙x(t) = Ax(t) + Bu(t) x(0) = xi x(tf) = xf |x| < xmax |u| < umax
Data la diretta implementazione di questo procedimento su Matlab, è necessario passare dalla definizione del sistema in tempo continuo a tempo discreto.
3.2.1 Discretizzazione
Passiamo ora a discretizzare il sistema appena espresso. Avendo a disposizione dei motori che agiscono tramite una coppia costante a tratti o a gradini, un cri-terio efficace di discretizzazione è quello denominato Zero Order Hold, che for-nisce il valore esatto dello stato per ogni multiplo del tempo di campionamento
Ts. Le matrici discretizzate sono ricavabili dalla relazione ([10])
exp " A B 0 0 # Ts ! = " Atd Btd 0 I # . ottenendo Atd = eATs ; Btd = " Z Ts 0 eA(Ts−t)dt # B
Avendo a disposizione un motore capace di erogare una coppia a “gradini”, quindi un ingresso costante a tratti, un efficiente modello di discretizzazione è quello chiamato Zero Order Hold. Questo metodo fornisce il valore esatto dello stato per ogni multiplo del tempo di campionamento Ts, sfruttando la
soluzione dei sistemi lineari in tempo continuo2, ottenendo [10]:
exp " A B 0 0 # Ts ! = " Ad Bd 0 I #
Il sistema espresso in tempo discreto è quindi
xd(k + 1) = Adxd(k) + Bdu(k) (4)
2Si ha infatti Ad= eATs e Bd=RTs
0 e
Volendo ricavare lo stato dopo un certo numero p di passi, svolgendo dei semplici calcoli1, si ha x(p) = Apdx0+ Rp u(p − 1) u(p − 2) .. . u(0)
dove Rp è la matrice di raggiungibilità per p passi
Rp = h
B AB · · · Ap−1Bi
e l’ultimo vettore a destra che chiameremo up impila il valore degli ingressi nei
vari step da 0 a p − 1.
Poiché la postura finale non è di equilibrio (come anticipato nel paragrafo (1.2)), nulla possiamo dire sull’evoluzione del sistema per gli step successivi a
p.
3.2.2 Tempo minimo
Il numero di passi p rappresenta il tempo impiegato per portare il sistema da
x(0) a x(p) ed è quindi il parametro che risponde al primo vincolo imposto.
Bisogna notare però che al diminuire di p cresce l’entità degli ingressi che devono svolgere tale compito in un tempo così ridotto.
Il compromesso tra il tempo minimo richiesto e il “risparmio” degli ingressi costituisce il fulcro della pianificazione ottima. In questa sede fisseremo un va-lore massimo alle coppie dei motori e troveremo il tempo minimo che permette l’oscillazione del robot.
3.2.3 Ingresso minimo
Andiamo ora ad imporre la condizione di ingresso minimo, inteso come cop-pia ai motori. Tale problema può essere risolto attraverso i moltiplicatori di Lagrange che ci forniscono l’espressione di up di norma minima per p passi:
up = R†p(x0− Apx(p))
dove R†p è la pseudoinversa di R e vale1 R†
p = RT(RRT)
−1.
Oltre a cercare la legge che minimizza la norma degli ingressi dobbiamo provvedere a stabilire un valore massimo del modulo delle coppie in modo da definire anche il numero di passi necessario. Chiamiamo questo valore massimo
umax e rimandiamo a breve la sua determinazione numerica.
1Basta procedere con l’equazione (4) per step successivi, sostituendo x(1) = Adx 0+
Bdu(0) in x(2) = Adx(1) + Bdu(1) e così via
1L’espressione completa è R†
p = W−1RT(RW−1RT)−1 dove W è una funzione di peso che, essendo l’ingresso formato da grandezze fisiche omogenee, equivale all’identità.
3.2.4 Limiti alle uscite
Possiamo migliorare la pianificazione aggiungendo un ulteriore vincolo sul mas-simo angolo relativo accettabile tra i link del braccio. Impilati questi valori in un vettore ymax possiamo scrivere
−ymax(k) ≤ y(k) ≤ ymax(k)
per ogni generico istante k. Essendo y(k) = Cx(k) ricaviamo
y(k) = CAkx0+ CRku(k)
e quindi
CRku(k) ≤ ymax− CAkx0
CRku(k) ≥ −ymax− CAkx0
che impilati in un unica disequazione fornisce
" CRk −CRk # u(k) ≤ " ymax ymax # + " −CAk CAk # x0
La precedente espressione deve essere verificata per ogni k da 1 a p, si ha a che fare quindi con un sistema di p disequazioni matriciali. Facendo attenzione alla struttura degli ingressi impiliamo nuovamente le equazioni per ottenere un’espressione che racchiude il vincolo sugli ingressi:
" Hp −Hp # | {z } 2p·l×p·m u(p − 1) .. . u(0) | {z } p·m×1 ≤ ymax .. . ymax | {z } 2p·l×1 + " −Gp Gp # | {z } 2p·l×n x0 |{z} n×1 (5)
dove la matrici denominate con Hp e Gp hanno la seguente forma:
Hp = 0 · · · 0 CB .. . CB CAB 0 ... ... CB · · · CAp−2B CAp−1B Gp = CA .. . CAp−1
3.3
Algoritmo lsqlin
Implementiamo ora le relazioni ottenute su Matlab per calcolare la legge degli ingressi che permette al robot di effettuare il moto sopra pianificato. Ciò è possibile grazie alla funzione lsqlin, dedicata ai problemi dei minimi quadrati vincolati.
I problemi a cui lsqlin cerca la generica soluzione x a minima norma sono nella forma: x : min|Cx − d| tale che Aeqx = beq Adisx ≤ bdis lb ≤ x ≤ ub
Perciò gli input che dovremo dare sono C, d, Aeq, beq,Adis, bdis, lb, ub.
Siccome ci interessa calcolare la norma minima di u, si ricava facilmente:
C = I d = 0
L’equazione di uguaglianza a cui è soggetto u è la (4) qui riarrangiata:
Rpup = xf − Apx0
si ha quindi
Aeq= Rp
beq = xf − Apx0
La relazione di diseguaglianza è data dal vincolo sulle uscite, quindi dalla (5), ottenendo Adis= " Hp −Hp # bdis = ymax .. . ymax + " −Gp Gp # x0
I limiti inferiore e superiore a cui è soggetta u sono semplicemente
lb = −umax
Figura 5: Oscillazione ottenuta dalla pianificazione
É notare che tutti gli input sono definiti una volta stabilito il numero di passi, ovvero p. Se si vuole trovare p minimo, è necessario eseguire la funzione lsqlin per p via via crescenti finché non viene trovata una soluzione che coniughi tutti i vincoli sopra descritti.
Tale soluzione è stata trovata per i valori numerici riportati in tabella 1. Grandezza Simbolo Valore numerico Unità di misura
massa dei link mi 200 g
massa dei motori mM i 200 g
massa disp. afferraggio mC 300 g
lunghezza dei link li 200 mm
inerzia dei link Ji 0.001 kg · m2
inerzia dei motori JM i 0.001 kg · m2
costante elastica ki 10 N/rad
attrito viscoso bi 0.005 N · s/rad
tempo di campionamento T s 0.005 s
uscita massima ymax π/2 rad
coppia massima umax 1.5 N · m
numero di passi p 133 /
Tabella 1: Parametri della pianificazione ottima
In allegato [A2] è riportato il codice Matlab che esegue la pianificazione ottima vincolata, mentre in figura 5 sono riportati degli screenshot di una simulazione grafica dell’evoluzione dinamica pianificata.
In figura 6 e 7 sono mostrati gli andamenti dell’uscita (compreso l’angolo
0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 Time (s) -1 -0.75 -0.5 -0.25 0 0.25 0.5 0.75 1 (rad)
Optimal joint trajectories
qd(1) q
d(2)
qd(3)
Figura 6: Evoluzione degli angoli relativi ai link
0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 Time (s) -1 -0.75 -0.5 -0.25 0 0.25 0.5 0.75 1 (rad)
Optimazed motor trajectories
thetao(2) thetao(3)
Parte II
Progetto del controllore
4
Controllore lineare
Dai risultati della pianificazione ottima vincolata possiamo ricavare le traiet-torie percorse dai link durante l’oscillazione e utilizzarle come riferimento per la progettazione di un contollore.
Tale legge di controllo, a partire dalla lettura delle uscite, fornirà una se-rie di ingressi tali da permettere al sistema di inseguire il riferimento voluto. Partiamo quindi con il progetto di un controllore per il sistema lineare.
Sono stati applicati al sistema diverse architetture di controllore, basati sia sull’azione in avanti (feedforward) sia in retroazione (feedback), verrà quindi illustrato qui di seguito lo schema che ha prodotto i risultati più soddisfacenti.
Esso si basa sull’applicazione di tre strategie di controllo:
• Osservatore asintotico: si tratta di un sistema dinamico digitale, “clone” del sistema meccanico vero e propiro. L’osservatore prende gli stessi ingressi del sistema originale e produce una copia più o meno esatta dello stato. Ciò è garantito dalla completa raggiungibilità ed osservabilità del sistema.
• Retroazione degli stati: gli ingressi applicati al sistema sono espressi in funzione dello stato. Questo permette di modificare la dinamica del sistema complessivo (in termini di autovalori e quindi di funzione di trasferimento) in base alle specifiche desiderate.
• Iniezione delle uscite: è possibile modificare la struttura dell’osservatore aggiungendo una dipendenza sia dalle uscite del sistema originale che dalle uscite dell’osservatore. Ciò offre la possibilità di alterare arbitra-riamente la dinamica, e quindi la velocità, con cui viene stimato lo stato del sistema meccanico.
4.1
Dinamica del regolatore
Il sistema meccanico è definito in forma di stato come
Dx = Ax + Bu
y = Cx
La dinamica dell’osservatore, il cui stato è indicato con ¯x, con iniezione delle
uscite è [10]: D¯x = A¯x + Bu − L(y − ¯y) ¯ y = C ¯x
La matrice L permette di fissare la dinamica dell’errore commesso dall’osser-vatore definito come e = x − ¯x. Si ha infatti
De = (A + LC)e
Allocando opportunamente i poli di L quindi, definisco la velocità con cui l’errore tenderà a zero.
L’ingresso dipende dallo stato retroazionato
u = Kx + r
dove r è un opportuno riferimento. Combinata con la dinamica del sistema meccanico fornisce
Dx = (A + BK)x + Br
che permette di fissare dinamica arbitraria al sistema tramite la matrice K. É da notare che le matrici K e L definiscono il comportamento dinamico rispettivamente del sistema originale e dell’osservatore in modo indipendente, disaccoppiando di fatto le due strutture.
Figura 8: Schema di un osservatore con retroazione degli stati e iniezione delle uscite
Il sistema complessivo è schemattizzato in figura 8. Si noti che lo stato che viene retroazionato nell’ingresso è quello stimato dall’osservatore. Questo a causa del fatto che (nella maggior parte dei casi) si ha accesso solo alle com-ponenti misurabili dello stato, che di fatto costituiscono l’uscita del sistema. Per questa ragione un osservatore digitale è uno strumento molto efficace per il progetto di un controllo, permettendo di monitorare e correggere l’evoluzione dinamica del robot. Tale impianto prende il nome di regolatore.
Raggruppando quindi le relazioni ricavate sopra si ottiene Dx = Ax + BK ¯x + Br D¯x = (A + BK + LC)¯x + Br − LCx (6)
4.2
Modello equivalente
Costruiamo ora uno schema equivalente con sistema meccanico e regolato-re montati in regolato-retroazione per poterlo implementaregolato-re su Matlab e simularne l’evoluzione.
Figura 9: Sistema meccanico e regolatore montati in retroazione
Tale schema è riportato in figura 9. Indicando con il pedice "R" i vettori e le matrici del regolatore, le relazioni che caratterizzano il sistema complessivo sono: Dx = Ax + Bu y = Cx DxR= ARxR+ BRy yR= CRxR+ DRy u = r − yR da cui si ottiene Dx = Ax − BCRxR− BDRCx + Br DxR= ARxR+ BRCx (7)
Confrontando i sistemi (6) e (7) possiamo ricavare le matrici del sistema equivalente in retroazione, ottenendo
AR= A + BK + LC BR= −K CR= −L DR= 0
4.3
Implementazione e risultati
Utilizziamo infine i risultati ottenuti dalla pianificazione per ottimizzare i risul-tati. In particolare diamo come riferimento r la legge degli ingressi pianificati e forniamo in ingresso al regolatore la differenza tra l’uscita del sistema mecca-nico y e l’uscita ricavata dalla pianificazione (desiderata) yrif, quindi l’errore
commesso (figura 10).
Per ricavare la matrice K è stata utilizzata la tecnica di regolazione ai
qua-drati lineari o LQR. Essa calcola la matrice K soggetta a u = Kx e applicata
al sistema dinamico Dx = Ax + Bu in modo che minimizzi una funzione di costo definita come
J =
Z ∞
0
(xTQx + uTRu) dt
dove Q e R rappresentano le funzioni di costo rispettivamente dello stato e degli ingressi. In pratica prendendo Q maggiore in modulo di R si avrà un controllo più prestante ma con elevati ingressi e vice versa. I valori che hanno portato ai risultati migliori sono
Q = 1 0 · · · 0 0 . .. 1 ... .. . 103 . .. 0 0 · · · 0 103 | {z } n×n R = " 10−2 0 0 10−2 #
Lo script eseguito su Matlab che svolge questa funzione è riportato in allegato [A3]. É da sottolineare che il comando lqr considera K definita da u = −Kx per cui è stato invertito il segno.
Per quanto riguarda L si ricordi che essa definisce la dinamica dell’osserva-tore, il quale deve fornire una stima dello stato più velocemente dell’evoluzione dello stato stesso. Chiediamo dunque che i poli di A + LC siano più veloci (di valore assoluto maggiore) di quelli del sistema, ovvero di A + BK.
Per ricavare L è stato usato il comando place assegnando i poli3 di A + LC cinque volte più veloci di quelli relativi a A + BK.
In figura 10 è mostrato lo schema finale implementato su Matlab.
3La funzione place fornisce la generica matrice di retroazione K assegnando i poli di
A − BK per cui si è ricavato LT soggetto a AT − CTLT, si è poi calcolato il trasposto e invertito il segno.
Figura 10: Schema del sistema lineare controllato con regolatore
4.4
Considerazioni
Benchè i risultati siano molto soddisfacenti, va ricordato che l’uscita mostra solo gli angoli q2 e q3, ovvero gli angoli relativi tra i link, ma nulla ci dice
sull’angolo q1 tra il link 1 e l’appiglio (riferimento a fig 2).
Per verificare che anche q1 permetta al robot di effettuare l’evoluzione
de-siderata, è stato costruito un impianto del tutto analogo a quello illustrato in figura 10 ma con uscita
y0 = q1 q2 q3
Il vettore y0 è stato poi restituito al regolatore epurandolo della stessa q1
tramite una matrice di selezione. Grazie ad una simulazione visiva è stata confermata l’efficienza del controllore progettato.
5
Controllore non lineare
Applicando gli ingressi ottenuti dalla pianificazione al sistema non-lineare
idea-le espresso dalidea-le equazioni 1 si ha un comportamento molto differente
dall’e-voluzione del sistema linearizzato. In figura 11 sono illustrate le traiettorie dei giunti del sistema non-lineare confrontate con quelle del sistema lineariz-zato mentre in figura 12 sono mostrati alcuni frammenti di una simulazione effettuata sul sistema non-lineare.
0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 Time (s) -1 -0.75 -0.5 -0.25 0 0.25 0.5 0.75 1 (rad)
Non-linear model joint trajectories
qd(1) qd(2) qd(3) qN L(1) qN L(2) qN L(3)
Figura 11: Traiettorie dei giunti del sistema non lineare (tratto pieno) e pianificata (tratteggiato)
Figura 12: Time-frame della simulazione del sistema non-lineare al quale sono dati gli ingressi del sistema linearizzato
Questo è dovuto al fatto che, come già anticipato nel paragrafo (1.2), il si-stema linearizzato differisce in modo non trascurabile da quello reale. Una
pos-sibile scelta per colmare questo "gap" tra i due sistemi, consiste nel progettare un controllore non lineare.
5.1
Feedback linearization
É possibile ricavare una legge di controllo che sfrutti la dinamica stessa del sistema non lineare per generare un ingresso che linearizzi il sistema comples-sivo. Tale tecnica prende il nome di feedback linearization (FL). Per applicare questo metodo è necessario derivare l’uscita un numero λ di volte, finché nel-la sua espressione non compaia l’ingresso. Il valore di λ che verifica questa condizione prende il nome di grado relativo del sistema.
Per ottenere tale relazione tra ingresso e uscita risulta efficace scomporre il vettore q separando le coordinate attuabili (q2 e q3), che costituiscono l’uscita
y del sistema, da quelle fuori dalla portata degli ingressi (q1).
Si ha quindi q = " qn qa # ; qn= q1 qa= q2 q3
per cui vale
y = qa
Riprendiamo le equazioni del paragrafo 2.1, in particolare la (1) qui riportata
M (q)¨q + C(q, ˙q) ˙q = kL(q, θ) + τext
Ipotizziamo di poter controllare il cinematismo direttamente tramite il vet-tore θ. Scomponiamo quindi le matrici ed i vettori con lo stesso criterio con cui abbiamo scomposto q
" Mnn Mna Man Maa # " ¨ qn ¨ qa # + " Cnn Cna Can Caa # " ˙ qn ˙ qa # = " 0 k(θ − qa) # + " τext,n τext,a # ,
Ricaviamo ¨qn dalla prima equazione e sostituiamolo nella seconda, da cui si
ottiene θ = k−1h−MnaMnn−1Man+ M aa ¨ qa+ MnaMnn−1τext,n− τext,a i + qa,
Ricordando che l’uscita del sistema è
y = " q2 q3 # = qa
abbiamo ottenuto un’equazione in cui compaiono sia la derivata seconda del-l’uscita ¨y = ¨qa sia l’ingresso θ e che racchiude la dinamica del sistema.
Se definiamo l’ingresso θ nella forma θ = k−1h−MnaMnn−1Man+ M aa σ + MnaMnn−1τext,n− τext,a i + qa , (8)
si ha la cancellazione della non linearità dall’equazione (1) che diventa
¨
y = 1sigma
dove la funzione a rappresenta il nuovo controllo del sistema linearizzato il cui stato è x0 = " y ˙ y # .
Per definire σ è possibile sfruttare i risultati ottenuti dalla pianificazione per creare un controllo in anello chiuso che assegni dinamica arbitraria all’errore
e = qd− q
dove qd contiene le traiettorie pianificate e costituisce il riferimento.
Definiamo quindi σ come
σ = ¨qd+ p1( ˙qd− ˙q) + p0(qd− q)
che equivale a scrivere
¨
e + p1˙e + p0e = 0
Lo schema che simula l’evoluzione del sistema non lineare e del controllore feedback linearization è esposto in figura 13, dove la funzione "Feedlin" prende in ingresso lo stato x e l’uscita pianificata q2,P con le sue derivate ˙q2,P e ¨q2,P per
restituire le coordinate dei motori θ, che costituiscono l’ingresso per il sistema non lineare "SysNL".
Figura 13: Schema Simulink della feedback linearization
Utilizzando come valori dei coefficienti p1 = p2 = −350 sono stati ottenuti i
0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 Time (s) -1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 1 (rad)
Feedback linearized joint trajectories
q
F L(1)
qF L(2) qF L(3)
Figura 14: Traiettoria dei giunti feedback linearizzati confrontati con la traiettoria desiderata (tratteggiata).
0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 Time (s) -1.5 -1.25 -1 -0.75 -0.5 -0.25 0 0.25 0.5 0.75 1 1.25 1.5 (rad)
Feedback linearizing motor trajectories
thF L(2) thF L(3)
Figura 15: Traiettoria dei motori derivata dalla feedback linearizzazione.
lineare confrontata con quella pianificata e la legge degli ingressi che linearizza il sistema complessivo.
In figura 16 sono riportati dei frammenti di una simulazione grafica dei ri-sultati ottenuti dalla FL. Oltre all’evoluzione del sistema non lineare è mostrata (in trasparenza) l’oscillazione pianificata che rappresenta il riferimento.
5.2
Analisi critica dei risultati
Dalla figura 14 è evidente come il controllore progettato riesca efficientemente a stabilizzare l’errore relativo alle coordinate q2 e q3 ma sia limitato nel controllo
di q1. Questo perché, come già anticipato, essendo il sistema sottoattuato non
è possibile agire su una coordinata del sistema (q1appunto) che resterà sempre
scoperta dal controllo.
La Feedback Linearization, benché abbia efficacemente controllato i giunti attuati e realizzato (in simulazione) il moto desiderato, presenta due
inconve-Figura 16: Screenshot dell’evoluzione del sistema non lineare confrontata con l’evoluzione pianificata
nienti riguardo l’applicabilità su un soft robot reale: è richiesto un modello accurato del sistema dinamico, in quanto la legge di controllo ricavata (equa-zione 8) dipende dalle matrici della dinamica, inoltre i controlli in feedback tendono a irrigidire i sistemi dinamici, perdendo così la cedevolezza desiderata di un soft robot.
É quindi necessario introdurre una legge di controllo capace di inseguire la traiettoria desiderata adattandosi al sistema reale, considerando quindi
• incertezze del modello
• disturbi dell’ambiente come attriti o vibrazioni • variazioni del setup
6
Controllo model-free
Lo schema di controllo utilizzato in questo studio consiste nel far compiere al robot l’evoluzione per un certo numero di volte, in modo che il controllore possa confrontare la traiettoria misurata in tempo reale con quella pianificata. L’errore commesso diminuisce man mano che viene ripetuto il processo; il controllore infatti “impara” la dinamica del cinematismo i modo iterativo, da qui il nome Iterative Learning Control o ILC.
6.1
Gli obiettivi del controllo
Oltre all’inseguimento della traiettoria desiderata, ci proponiamo di progettare il controllore con i seguenti obiettivi:
1. Conservazione della cedevolezza: come già anticipato, l’oggetto del no-stro studio è un soft robot, la cui flessibilità è fondamentale per permet-tere un’evoluzione naturale e “morbida”. Il cinematismo quindi, non deve irrigidirsi a causa di un’azione troppo prestante da parte del controllore 2. Indipendenza dalla dinamica complessiva: ci proponiamo di agire su ciascun motore in modo indipendente, decentralizzando le equazioni e analizzando il problema dinamico “visto” da ogni giunto attuato
3. Robustezza alle incertezze dell’ambiente: questo comprende sia incer-tezze di misura delle grandezze fisiche sia possibili disturbi provenienti dall’ambiente esterno al robot
Decentralizzare il sistema significa riscrivere le equazioni della dinamica disaccoppiando le coordinate del robot; per fare questo è sufficiente ricava-re il valoricava-re dell’inerzia e dello smorzamento del sistema complessivo visti da ogni singolo giunto. Tali valori saranno ricavati in una fase preliminare alle iterazioni di learning che verrà illustrata nei paragrafi successivi.
6.2
Schema del controllore
Passiamo ora a definire le equazioni algebriche che costituiscono il controllore. In questa sede verranno descritte solo le relazioni fondamentali che realizzano il controllo iterativo, per uno studio più approfondito si rimanda a [9].
I motori che utilizzeremo (come illustrato più nel dettaglio in seguito) rice-vono come input la posizione angolare dei rotori, per cui la legge di controllo esprimerà l’andamento della coordinata theta in funzione del tempo. L’uscita sarà sempre espressa dal vettore y in quanto raccoglie le coordinate misurabili del robot.
Definito quindi l’errore come
e(t) = qd(t) − q(t) ,
θj(t) = θj−1(t) + Koff· ej−1(t) + Kon· ej(t) ,
rappresentata schematicamente in figura 17.
Figura 17: Iterative Learning Control framework
Analizziamo nel dettaglio gli elementi della equazione appena espressa: • θj rappresenta la traiettoria data in ingresso ai motori per la j-esima
iterazione
• θj−1 rappresenta la traiettoria utilizzata nella precedente iterazione, che
verrà quindi corretta ad ogni ciclo iterativo. Si noti che per eseguire la prima iterazione è necessario fornire una traiettoria di primo ten-tativo θ0. Un metodo per ricavarla è illustrato in [9] e sfrutta la
di-namica decentralizzata insieme alla traiettoria pianificata ottenuta in precedenza.
• Kon è la matrice dei guadagni “online”, che agisce sull’errore
commes-so all’iterazione corrente (al pascommes-so j) il cui contributo viene aggiornato durante l’evoluzione del robot, ovvero durante la fase di learning. Il vettore Kon· ej costituisce quindi il controllo in retroazione (feedback).
Affinché il controllo non irrigidisca il sistema compromettendone la ce-devolezza perseguita, è necessario che l’entità dei guadagni di feedback sia relativamente bassa.
• Koff è la matrice dei guadagni “offline”, che moltiplica l’errore commesso
durante l’iterazione precedente (al passo j − 1), il quale viene calcolato tra un’iterazione e la successiva. Per questo motivo, il vettore θj−1+K
off·
ej−1 rappresenta il controllo in anello aperto (feedforward) e costituisce
il fulcro del ILC in quanto la sua azione viene aggiornata in base alla conoscenza delle iterazioni precedenti, realizzando così l’apprendimento del controllo.
Dalla formulazione disaccoppiata del sistema è possibile esprimere le matrici
Kon= diag (Kon,i) con Kon,i= " Kpon 0 0 Kdon #
che raccoglie i contributi all’errore di posizione e di velocità. In [9] è illustrata un’opportuna formulazione delle matrici Kon e Koff che non alterano la
rigidez-za del sistema e forniscono robustezrigidez-za alle incertezze dell’ambiente, garantendo il raggiungimento degli obiettivi 1) e 3).
É evidente quindi dalla figura 17 come tale schema di controllo sfrutti la combinazione di anello aperto e anello chiuso per inseguire la traiettoria desiderata e perseguire gli obiettivi descritti nel paragrafo precedente.
6.3
Le fasi del processo
Il controllo iterativo descritto nel precedente paragrafo non è applicabile di per sé, ma va accompagnato da una serie di operazioni volte a ricavare gli input del controllore ed a configurare il robot per il ciclo iterativo. In questo paragrafo ci limiteremo ad una descrizione generale del processo, concentrando la nostra attenzione sulle grandezze algebriche che completano lo schema del controllo.
Le fasi e le sottofasi operative che costituiscono lo schema ILC sono:
1. Definizione dei parametri: oltre alla traiettoria di riferimento e le sue de-rivate è necessario fornire all’algoritmo alcune grandezze di input come la rigidezza dei motori (o una sua legge temporale), il tempo di campio-namento del controllo e altri parametri che verranno introdotti nelle fasi successive.
2. Inizializzazione: questa fase costituisce il primo azionamento del robot vero e proprio e ci permetterà di configurare correttamente il sistema nelle varie sottofasi del ciclo iterativo.
• Rilevazione della posizione di riposo: trattasi della postura che il robot assume a motori spenti. É necessario posizionare manual-mente il sistema in una configurazione comoda per l’accensione dei motori e per la manipolazione da parte del progettista, dopodiché viene letta la posizione angolare dei motori e viene registrata come posizione di riposo.
• Raggiungimento della posizione iniziale: vale a dire della configu-razione iniziale e ricavata dalla traiettoria di riferimento. A partire dalla posizione di riposo, il robot si posizionerà in tale configurazio-ne mediante una azioconfigurazio-ne integrale la cui entità può essere modulata tramite un parametro da definire nella fase 1. Tale azione integrale fornisce quindi la combinazione degli ingressi per portare il robot dalla configurazione di riposo a quella iniziale e verrà utilizzata ad
ogni iterazione del processo di apprendimento per far coincidere la posizione dei giunti con quella pianificata a inizio traiettoria, azze-rando così l’errore relativo. La stessa combinazione di ingressi verrà sommata all’azione di feedforward in modo da compensare l’effetto di forze.
• Calcolo della dinamica decentralizzata: mediante la sottofase pre-cedente, viene calcolata l’inerzia e lo smorzamento del sistema com-plessivo visti dal singolo giunto attuato. Questi parametri sono fondamentali poiché disaccoppiano il problema (obiettivo 2) e for-niscono i guadagni del controllore Kon, Koff e la traiettoria di primo
tentativo θ0.
3. Ciclo di apprendimento iterativo: questa è la fase principale, dove verran-no effettuate le iterazioni che permetteranverran-no al controllore di inseguire correttamente la traiettoria desiderata garantendo gli obiettivi sopra de-scritti. Ogni iterazione comprende tre sottofasi che vengono eseguite in modo ciclico.
• Preparazione alla iterazione: a partire dalla posizione di riposo i motori guideranno il robot nella posizione iniziale, grazie all’azione integrale sopra definita. La durata di questa sottofase viene de-finita nella fase 1 e deve essere sufficiente a garantire un corretto posizionamento, ovvero errore iniziale nullo.
• Iterazione: il robot “tenta” di eseguire l’evoluzione desiderata, at-traverso la combinazione di controllo in anello aperto e anello chiuso (vedi paragrafo . . . ). Viene misurata istante per istante la traietto-ria effettivamente seguita dal sistema, la quale verrà utilizzata per la iterazione successiva. La durata di questa sottofase coincide con la durata dell’evoluzione pianificata.
• Conclusione della iterazione: una volta terminata l’evoluzione il controllore mantiene il robot nella postura finale per un certo ar-co di tempo, anch’esso da definire nella fase 1. Al termine di tale intervallo i motori vengono spenti per poter riposizionare manual-mente il robot nella posizione di riposo e poter così riiniziare con l’iterazione successiva.
Il ciclo di apprendimento viene ripetuto per un numero arbitrario di ite-razioni. Rispettando tutte le fasi viste sopra si ha una progressiva riduzione dell’errore commesso ad ogni iterazione, segnale di una corretta convergenza del processo.
6.4
Simulazione sul sistema non lineare
Prima di applicare la tecnica ILC al robot reale è stato testato l’algoritmo di apprendimento sul sistema non lineare ideale per verificarne l’applicabilità. In figura 18 è mostrato l’andamento dell’errore medio commesso per iterazione
0 5 10 15 20 25 30 35 40 45 50 55 60 Iteration 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.6 (rad)
Average error over iterations
ea(2) ea(3)
Figura 18: Errore medio commesso in un set di 60 iterazioni
0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 Time (s) -1.5 -1.25 -1 -0.75 -0.5 -0.25 0 0.25 0.5 0.75 1 (rad)
ILC simulated joint trajectories
q d(1) qd(2) qd(3) qILC(1) qILC(2) q ILC(3)
Figura 19: Traiettorie dei giunti della simulazione ILC confrontate con quelle desiderate
in un set di 60 iterazioni. Si nota che il processo di apprendimento avviene correttamente in quanto l’errore decresce ma converge ad un valore minimo.
Le traiettorie dei giunti all’ultima iterazione confrontate con quelle deside-rate sono illustdeside-rate in figura 19, dove si nota la non perfetta convergenza tra le due evoluzioni. In figura 20 infine, sono riportati alcuni time frame della evoluzione relativa all’ultima iterazione.
Parte III
Validazione sperimentale
Dopo aver studiato, pianificato e controllato il modello del robot, è stato possi-bile realizzare un prototipo con cui sperimentare la dinamica progettata. Sono state affrontate due prove sperimentali: nella prima prova il robot è stato pilotato in anello aperto (open loop), attraverso una legge di ingressi basata sulla traiettoria pianificata, nella seconda è stato testato il controllo ILC attra-verso diverse serie di iterazioni. Nella parte finale saranno discussi i risultati ottenuti.
6.5
Robot e setup
Il robot è stato assemblato con
• elementi strutturali come distanziatori e connettori in PVC stampati in 3D
• due motori qbmove-maker pro (figura 21) di rigidezza variabile • due dispositivi soft-hand (figura 22) per l’afferraggio degli appigli In base alle proprietà fisiche e alle specifiche tecniche dei componenti scelti, le variabili del sistema sono state modificate, come mostrato in tabella 2.
Grandezza Simbolo Valore numerico Unità di misura
massa dei link mi 200 g
massa dei motori mM i 260 g
massa disp. afferraggio mC 300 g
lunghezza dei link li 200 mm
inerzia dei link Ji 0.001 kg · m2
inerzia dei motori JM i 0.010 kg · m2
costante elastica ki 10 N/rad
attrito viscoso bi 0.03 N · s/rad
Tabella 2: Parametri del modello usati per la prova sperimentale I motori sono costituiti da due alberi motorizzati connessi ad un terzo albero centrale, che rappresenta il rotore vero e proprio, tramite elementi elastici. Come illustrato in figura 21, agendo sulle coordinate dei due alberi laterali q1
e q2, è possibile pilotare il rotore principale di coordinata x e, assegnando valori
diversi a q1 e q2, determinare la rigidezza del motore. Sono inoltre pilotabili sia
in posizione che in corrente e sono dotati di sensori che monitorano la posizione dei tre alberi.
Il robot così assemblato è riportato in figura 23.
Per creare gli appigli dove il cinematismo possa oscillare sono stati montati dei profilati opportunamente distanziati, la struttura montata e pronta per la prova è riportata in figura 24.
Figura 21: Motore qbmove e proncipio di funzionamento
Grandezza Valore numerico Unità di misura
coppia nominale 1.2 N · m
coppia massima 1.5 N · m
velocità nominale 4 rad/s
rigidezza minima 0.5 N · m/rad
rigidezza massima 13 N · m/rad
Tabella 3: Specifiche qbmove
Figura 23: Robot assemblato con due soft hand dotate di guanto per aumentare la presa, due qbmove montati con flange a ’C’ e tre distanziatori in PVC
6.6
Controllo in anello aperto
Per pilotare la catena cinematica non è stato possibile applicare i risultati dalla feedback linearization in quanto tale metodo richiede la conoscenza dell’intero stato del sistema, mentre in questo caso è possibile misurare solo gli angoli di giunto, ovvero l’uscita. É stato scelto quindi di pianificare nuovamente l’oscillazione inserendo i nuovi valori numerici ed aggiungendo un vincolo sulle velocità compatibilmente con le specifiche di tabella 3 e di pilotare i giunti motorizzati in anello aperto, ovvero assegnando ai motori la legge temporale ottenuta dalla pianificazione. Nonostante un controllo in anello aperto abbia forti limiti rispetto ad un impianto in retroazione, tale scelta è stata fatta per studiare la realizzabilità di un moto così pianificato e fissare tutti quei parametri che accoppiano il sistema matematico con quello reale.
La prova ha portato buoni risultati permettendo al robot di afferrare l’ap-piglio e completando così il task preposto. In figura 25 sono mostrate le traiet-torie dei giunti misurate durante la prova, mentre in figura 26 sono mostrate le traiettorie dei motori che realizzano il moto. In figura 27 infine, sono mostrati frammenti di un filmato che riporta l’evoluzione del cinematismo durante la prova. 15 15.1 15.2 15.3 15.4 15.5 15.6 15.7 15.8 15.9 16 16.1 16.2 Time (s) -1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 (rad)
Feedforward control joint trajectories
qF F(2) qF F(3)
Figura 25: Traiettorie dei giunti controllati in anello aperto
6.7
Controllo in Iterative Learning
Dopo aver verificato la fattibilità del moto di brachiazione ed aver ricavato una traiettoria dei giunti che la realizza, testiamo il comportamento del robot sottoponendolo a dei cicli di apprendimento. Come già introdotto, la tecnica Iterative Learning Control prevede una serie di evoluzioni del sistema nelle quali ogni iterazione viene corretta dalla conoscenza della precedente e sfrut-tata per migliorare quella successiva. Sono state fatte diverse serie di iterazioni con lo scopo di riprodurre il più fedelmente possibile il sistema che ha realizzato la brachiazione. Per questo motivo, è stato dato come riferimento al codice di apprendimento la traiettoria dei giunti realizzata nella prova in anello aperto
15 15.1 15.2 15.3 15.4 15.5 15.6 15.7 15.8 15.9 16 16.1 16.2 Time (s) -1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 (rad)
Feedforward control motor trajectories
thetaF F(2) thetaF F(3)
Figura 26: Traiettorie dei motori in anello aperto
14.9 15 15.1 15.2 15.3 15.4 15.5 15.6 15.7 15.8 15.9 16 16.1 16.2 16.3 Time (s) 0.24 0.25 0.26 0.27 0.28 0.29 0.3 0.31 0.32 0.33 0.34 0.35
0.36 Feedforward control motors preset
kF F(1) kF F(2)
Figura 28: Profilo temporale di rigidezza ottenuto nella prova in anello aperto
(figura 25). Inoltre, grazie alla possibilità di variare la rigidezza degli attuatori (modello VSA: Variable Stiffness Actuator) è stato dato come input il profilo di rigidezza che si ha avuto durante la brachiazione, illustrato in figura 28.
La prova è stata realizzata mediante 50 iterazioni, nelle quali l’errore medio di inseguimento della traiettoria (si ricorda che e(t) = qd(t)−q(t)) decresce fino
a raggiungere un valore di convergenza, pari a circa 2,8 gradi. L’andamento dell’errore medio è illustrato in figura 29, mentre nelle figure 30 e 31 sono mostrate le traiettorie realizzate sui giunti e sui motori confrontate con quelle desiderate. 0 5 10 15 20 25 30 35 40 45 50 55 60 Iteration 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 (rad)
Average errors evolution
ea(1) ea(2)
Figura 29: Andamento dell’errore medio nel corso delle iterazioni
In figura ?? sono mostrati alcuni timeframe di un video riportante l’ultima iterazione, ovvero quella a errore medio minimo. Nonostante si abbia una buona convergenza delle traiettorie, tale convergenza non è perfetta (ovvero l’errore non tende a zero) per cui il moto di brachiazione non è realizzabile.
15 15.1 15.2 15.3 15.4 15.5 15.6 15.7 15.8 15.9 16 16.1 16.2 Time (seconds) -1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 (rad)
Trajectory tracking of iteration 1
qd(2) qd(3) qi1(2) qi1(3) 15 15.1 15.2 15.3 15.4 15.5 15.6 15.7 15.8 15.9 16 16.1 16.2 Time (s) -1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 (rad)
Trajectory tracking of iteration 15
qd(2) qd(3) q i15(2) qi15(3) 15 15.1 15.2 15.3 15.4 15.5 15.6 15.7 15.8 15.9 16 16.1 16.2 Time (s) -1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 (rad)
Trajectory tracking of iteration 60
qd(2) qd(3) qi60(2) qi60(3)
Figura 30: Traiettorie dei giunti confrontate con quelle desiderate per le iterazioni 1, 15 e 60
15 15.1 15.2 15.3 15.4 15.5 15.6 15.7 15.8 15.9 16 16.1 16.2 Time (s) -1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 (rad)
Motor trajectories of iteration
thetaF F(2) thetaF F(3) thetai1(2) thetai1(3) 15 15.1 15.2 15.3 15.4 15.5 15.6 15.7 15.8 15.9 16 16.1 16.2 Time (s) -1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 (rad)
Motor trajectories of iteration 15
thetaF F(2) thetaF F(3) theta i15(2) thetai15(3) 15 15.1 15.2 15.3 15.4 15.5 15.6 15.7 15.8 15.9 16 16.1 16.2 Time (s) -1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 (rad)
Motor trajectories of iteration 60
thetaF F(2) thetaF F(3) thetai60(2) thetai60(3)
Figura 31: Traiettorie dei motori confrontate con quelle desiderate per le iterazioni 1, 15 e 60
7
Conclusioni
7.1
Riepilogo del progetto
Per studiare il moto di brachiazione su un soft robot, sono stati definiti dei modelli matematici del sistema, dai quali è stata pianificata l’evoluzione de-siderata. Per controllare i vari modelli del sistema sono stati studiati diversi schemi di controllo: uno lineare (regolatore) uno non lineare (Feedback Linea-rization) e uno adattivo per superare la non idealità del sistema reale (Iterative Learning Control).
Sono stati eseguiti diversi test sperimentali di controllo sul robot brachian-te. In una prima fase è stato ottenuto il moto di brachiazione mediante un’azio-ne di controllo in aun’azio-nello aperto, basato sulla traiettoria pianificata e ottimizzato mediante una serie di regolazioni manuali agli ingressi e al setup.
Una volta ottenuta la traiettoria di brachiazione sul sistema reale, essa è stata utilizzata come riferimento per un’applicazione sperimentale di Iterative Learning Control, nella quale si è realizzato un soddisfacente inseguimento della traiettoria desiderata mediante una serie di iterazioni che hanno portato a convergenza l’errore commesso senza alterare la cedevolezza desiderata del soft robot.
7.2
Analisi critica dei risultati
Essendo il robot sottoatutato, non è stato possibile agire direttamente su una coordinata del sistema mediante un’azione di controllo. Mentre nel caso di anello aperto l’evoluzione è stata efficientemente compiuta grazie ad una serie di regolazioni trail and error, l’applicazione del metodo ILC non ha realizzato una convergenza delle traiettorie sufficiente a far compiere al sistema il moto di brachiazione.
