Universit`
a degli Studi di Napoli Federico II
` DI INGEGNERIA
FACOLTA
Corso di Laurea in Ingegneria dell’Automazione
Elaborato Tecnico, Controllo dei Robot
Analisi e Controllo di un
manipolatore S.C.A.R.A.
Candidati
Relatore
Angelo Salvati
Ch.mo Prof. Bruno Siciliano
Matricola M58/01
Angelo Tufano
Matricola M58/23
Anno Accademico 2011-2012
Indice
1 Analisi Cinematica
3
2 Pianificazione della Traiettoria
7
2.1
2.2
Algoritmi di Inversione Cinematica . . . . . . . . . . .
12
2.1.1
CLIK del I ordine con inversa dello Jacobiano .
13
2.1.2
CLIK del I ordine con trasposta dello Jacobiano
16
2.1.3
CLIK del II ordine con inversa dello Jacobiano .
18
Ridondanza . . . . . . . . . . . . . . . . . . . . . . . .
22
3 Dinamica
3.1
27
Parametrizzazione del modello . . . . . . . . . . . . . .
4 Controllo del Moto
29
35
4.1
Controllo a Dinamica Inversa . . . . . . . . . . . . . .
35
4.2
Controllo Robusto
. . . . . . . . . . . . . . . . . . . .
36
4.3
Controllo Adattativo . . . . . . . . . . . . . . . . . . .
46
5 Controllo di Forza
51
5.1
Controllo di Impedenza . . . . . . . . . . . . . . . . . .
52
5.2
Controllo di Forza . . . . . . . . . . . . . . . . . . . . .
57
II
INDICE
5.2.1
5.2.2
5.2.3
Anello interno di posizione . . . . . . . . . . . .
Anello interno di velocitá . . . . . . . . . . . . .
Controllo Parallelo Forza/Posizione . . . . . . .
Bibliografia
57
62
66
71
© 2013 by A.Salvati, A.Tufano
Introduzione
Il robot SCARA é un manipolatore di semplice struttura, molto diffuso nelle operazioni di montaggio. I suoi assi dei giunti sono verticali. La
sigla SCARA deriva dalle parole inglesi Selective Compliance Assembly Robot Arm (robot di montaggio a cedevolezza selettiva). Il robot
é a cedevolezza selettiva perché la pinza, se forzata, si puó muovere
leggermente nel piano orizzontale ma non in quello verticale. Questo
fattore dipende dalle caratteristiche strutturali del robot e dall’elasticitá torsionale degli alberi di trasmissione nonché dei riduttori di
velocitá. Un certo grado di cedevolezza é appositamente inserito nel
robot per compensare automaticamente alcuni errori di posizionamento del robot stesso. Il robot SCARA da un punto di vista costruttivo
é formato da due membri rigidi (detti link ) collegati tra di loro da
accoppiamenti rotoidali per effettuare i movimenti nel piano x-y e da
un terzo elemento collegato mediante accoppiamento prismatico per
effettuare i movimenti verticali. Un ulteriore accoppiamento rotoidale
é talvolta presente per consentire una rotazione della pinza attorno al
suo asse, che altrimenti durante il moto assumerebbe un orientamento
dipendente dalla posizione dei primi due membri.
2
INDICE
© 2013 by A.Salvati, A.Tufano
CAPITOLO
1
Analisi Cinematica
Lo SCARA é un manipolatore a 4 gradi di libertá, con tre giunti rotoidali ed uno prismatico (n = 4). Per la determinazione della cinematica diretta sono state posizionate le terne solidali ai bracci secondo la
convenzione di Denavit-Hartenberg come in Figura1.1:
Figura 1.1: Terne secondo D-H
4
1. Analisi Cinematica
Specifichiamo i parametri di D-H in tabella:
Braccio ai
αi
di
θi
1
0.5 0
1
θ1
2
0.5 0
0
θ2
3
0
0
d3
0
4
0
0
0
θ4
Le matrici di trasformazione omogenea relative ad ogni braccio sono:


c −s1
 1

s1
c1
A01 (q1 ) = 

0
0

0
0
0 0.5c1 

0 0.5s1 


1
1 

0
1
c −s2
 2

s2
c2
A12 (q2 ) = 

0
0

0
0
0 0.5c2 

0 0.5s2 


1
0 

0
1


1


0
A23 (q3 ) = 

0

0

0
1
0
0


0 0

0 0


1 d3 

0 1
c −s4
 4

s4
c4
A34 (q4 ) = 

0
0

0
0

0
0
1
0
0

0


0

1
con q=[q1 q2 q3 q4 ]T = [θ1 θ2 d3 θ4 ]T vettore delle variabili di giunto.
La matrice di trasformazione da braccio 4 a terna utensile é data da:

0


1
A4e = 

0

0

1 0 0

0 0 0


0 −1 0

0 0 1
© 2013 by A.Salvati, A.Tufano
5
1. Analisi Cinematica
La trasformazione di coordinate complessiva che esprime posizione ed
orientamento della terna utensile rispetto alla terna 0 é data dalla
matrice:


−s
c
0
a
c
+
a
c
124
124
2
12
1
1





c124 s124 0 a2 s12 + a1 s1 
0


Te (q) = 

 0

0
−1
d
0 + d3


0
0
0
1
Le variabili di spazio operativo sono quattro (m = 4), tre di posizione
e una di orientamento (intorno a z):

Lo Jacobiano geometrico é:

pe
x= 
φ


z0 × (pe − p0 ) z1 × (pe − p1 ) z2 z3 × (pe − p3 )
J(q) = 
z0
z1
0
z3

−a2 s12 − a1 s1 −a2 s12


 a2 c12 + a1 c1
a2 c12



0
0
=


0
0



0
0

1
1
0
0
1
0
0
0

0

0


0


0


0

1
Eliminando le righe nulle corrispondenti a rotazioni intorno agli assi x0
e y0 , che risultano impossibili per uno SCARA, si ottiene lo Jacobiano
in forma ridotta:

−a2 s12 − a1 s1 −a2 s12


 a2 c12 + a1 c1
a2 c12
J(q) = 


0
0

1
1
0
0
1
0
© 2013 by A.Salvati, A.Tufano

0

0


0

1
6
1. Analisi Cinematica
L’orientamento dell’organo terminale dipende solo dai giunti rotoidali, il giunto prismatico non apporta alcun contributo. Poiché in
questo caso i gradi di libertá possono imporre rotazioni all’organo terminale solo intorno ad un asse fisso nello spazio (z0 ), allora lo Jacobiano
analitico é equivalente allo Jacobiano geometrico (J = JA ).
Si noti che solo i primi due giunti determinano le coordinate x e y
(come un manipolatore planare a due bracci), il terzo giunto prismatico la sola coordinata z e l’ultimo giunto l’orientamento (considerando θ4 = φ − θ1 − θ2 ). Quindi c’é un quasi totale disaccoppiamento
cinematico.
© 2013 by A.Salvati, A.Tufano
CAPITOLO
2
Pianificazione della Traiettoria
Al nostro manipolatore abbiamo voluto far eseguire una traiettoria
che ci restituisse il logo della casa automobilistica tedesca Mercedes.
8
2. Pianificazione della Traiettoria
Scelti i seguenti punti:
p1 = [−0.2; 0.6; 0.6]
p2 = [−0.5; 0.4; 0.2]
p3 = [−0.56; 0.1; 0.2]
p4 = [−0.57; 0.04; 0.2]
p5 = [−0.62; 0; 0.2]
p6 = [−0.845; −0.2; 0.2]
p7 = [−0.56; −0.1; 0.2]
p8 = [−0.5; −0.09; 0.2]
p9 = [−0.44; −0.1; 0.2]
p10 = [−0.15; −0.2; 0.2]
p11 = [−0.38; 0; 0.2]
p12 = [−0.43; 0.04; 0.2]
p13 = [−0.44; 0.1; 0.2]
p14 = [−0.5; 0.4; 0.2]
p15 = [−0.2; 0.6; 0.6]
ove si é supposto che il manipolatore partisse da una configurazione
home definita da p1 e vi ritornasse (p15 ) una volta ultimata la traiettoria
desiderata. Inoltre, per migliorare l’esecuzione, i punti p4 , p8 , p12 sono
stati scelti come punti di via e realizzati sovrapponendo le traiettorie
corrispondenti per un ∆t = 1/3 del tempo di percorrenza del primo
segmento. Tramite apposite funzioni Matlab, costruite per mezzo di
interpolazioni successive avendo impostato la velocitá di crociera1 come
|q −q |
q˙c = 34 f T i , abbiamo unito i punti nello spazio operativo con tratti
rettilinei o circolari laddove necessario.
1
T é il tempo di percorrenza di ogni singolo tratto
© 2013 by A.Salvati, A.Tufano
2. Pianificazione della Traiettoria
9
Il risultato é il seguente:
Figura 2.1: Percorso nello spazio X-Y
Figura 2.2: Percorso nello spazio operativo
É possibile osservare lo spazio di lavoro del manipolatore tramite le
circonferenze di colore verde.
© 2013 by A.Salvati, A.Tufano
© 2013 by A.Salvati, A.Tufano
2. Pianificazione della Traiettoria
10
Figura 2.3: Tutte le traiettorie sono state generate assegnando una velocitá media di percorrenza di 0.16m/s con
un tempo di sosta di 0.2s alla fine di ogni tratto. Le traiettorie desiderate all’organo terminale risultano a tipico
profilo trapezoidale di velocitá, e di conseguenza si ottengono brusche variazioni di accelerazione con tipico profilo
rettangolare.
2. Pianificazione della Traiettoria
11
Per quanto concerne l’orientamento del manipolatore, é stata considerata una sua variazione di π durante l’esecuzione della movimentazione:
Figura 2.4: Traiettoria Orientamento Φ
© 2013 by A.Salvati, A.Tufano
12
2.1
2. Pianificazione della Traiettoria
Algoritmi di Inversione Cinematica
Prima di invertire l’intera traiettoria é necessario ricavare i valori iniziali per gli algoritmi di inversione cosí da garantire errore di inseguimento
nullo. Riportando i risultati solamente per il CLIK del primo ordine
con inversa dello Jacobiano, abbiamo:
q0 =

1
π
4
π
2








 ; qp0


−0.1


π
12

 
0
=
 
 
0
 ;
 
0
 
0

500 0
0
0 



 0
500 0
0 

KI = 


 0

0
500
0


0
0
0 200
Otteniamo, tramite il CLICK di Figura(2.7) il seguente risultato:
Figura 2.5: Condizioni iniziali
© 2013 by A.Salvati, A.Tufano
2. Pianificazione della Traiettoria
2.1.1
13
CLIK del I ordine con inversa dello Jacobiano
Per risolvere il problema cinematico diretto si é implementato il seguente schema in MATLAB:
Figura 2.6: Schema a blocchi di base
Dove k(·) indica la funzione cinematica diretta, K é una matrice definita positiva, diagonale scelta in modo da rendere asintoticamente
stabile il sistema che rappresenta la dinamica dell’errore nello spazio
operativo. Il tutto é stato implementato in ambiente simulink:
Figura 2.7: Schema Simulink per CLICK del I ordine con inversa dello
Jacobiano
ove si é scelta


700 0
0
0 



 0
700 0
0 

KI = 


 0

0
700
0


0
0
0 400
© 2013 by A.Salvati, A.Tufano
14
2. Pianificazione della Traiettoria
Ottenendo:
Figura 2.8: Traiettorie CLIK del I ordine con inversa dello Jacobiano
Figura 2.9: Errore di Posizione CLIK del I ordine con inversa dello
Jacobiano
© 2013 by A.Salvati, A.Tufano
2. Pianificazione della Traiettoria
15
A differenza di ció che si sarebbe ottenuto con uno schema di controllo
in anello aperto dove l’errore si sarebbe assestato ad un valore costante
in conseguenza al fenomeno di deriva della soluzione, qui invece la
norma dell’errore in posizione risulta essere molto ridotta e converge a
zero a regime, grazie alla caratteristica in anello chiuso dello schema.
© 2013 by A.Salvati, A.Tufano
16
2.1.2
2. Pianificazione della Traiettoria
CLIK del I ordine con trasposta dello Jacobiano
Allo stesso modo, abbiamo:
Figura 2.10: Schema a blocchi di base
ove si utilizza la trasposta dello jacobiano; in simulink:
Figura 2.11: Schema Simulink per CLICK del I ordine con trasposta
dello Jacobiano
ove si é scelta


500 0
0
0 



 0
500 0
0 


KT = 

 0

0
500
0


0
0
0 250
© 2013 by A.Salvati, A.Tufano
2. Pianificazione della Traiettoria
17
Ottenendo:
Figura 2.12:
Jacobiano
Traiettorie CLIK del I ordine con trasposta dello
Figura 2.13: Errore di Posizione CLIK del I ordine con trasposta dello
Jacobiano
© 2013 by A.Salvati, A.Tufano
18
2. Pianificazione della Traiettoria
Il vantaggio di tale algoritmo risiede nella sua semplicitá computazionale poiché evita il calcolo oneroso dell’inversa dello Jacobiano; lo
svantaggio di questo algoritmo peró risiede in un aumento dell’errore
, il quale resta comunque ridotto ed accettabile. Essendo xd variabile nel tempo non é possibile ottenere l’asintotica stabilitá del sistema
errore, il quale per’øsará limitato superiormente in norma e sará tanto
piú contenuto quanto piú elevata é la matrice dei guadagni K. Il limite superiore alla norma di K é imposto dal periodo di campionamento
utilizzato.
2.1.3
CLIK del II ordine con inversa dello Jacobiano
Ai fini del controllo, puó essere insufficiente conoscere la velocitá e la
posizione dei giunti istante per istante.
Figura 2.14: Schema a blocchi di base
Implementando tale schema é possibile ottenere l’inversione cinematica
della traiettoria di moto specificata in termini di posizione, velocitá e
accelerazione. KP e KD sono due matrici definite positive (solitamente
diagonali) che consentono di rendere il sistema lineare di errore equivalente asintoticamente stabile e di specificarne la velocitá di convergenza
© 2013 by A.Salvati, A.Tufano
19
2. Pianificazione della Traiettoria
a zero. Scelti i seguenti guadagni:




400 0
0
0 
100 0
0
0






 0
 0
400 0
0 
100 0
0




KP = 
 ; KD = 

 0

 0

0
400
0
0
100
0




0
0
0 100
0
0
0 20
tramite lo schema simulink
Figura 2.15: Schema Simulink per CLICK del II ordine con inversa
dello Jacobiano
© 2013 by A.Salvati, A.Tufano
20
2. Pianificazione della Traiettoria
abbiamo ottenuto i seguenti risultati:
Figura 2.16: Traiettorie CLIK del II ordine con inversa dello Jacobiano
© 2013 by A.Salvati, A.Tufano
2. Pianificazione della Traiettoria
21
(a) Errore di Posizione
di Velocitá
© 2013(b)byErrore
A.Salvati,
A.Tufano
Figura 2.17: Errori con CLIK del II ordine con inversa dello Jacobiano
22
2.2
2. Pianificazione della Traiettoria
Ridondanza
Si analizza, adesso, il caso in cui una delle variabili di spazio operativo
viene rilassata in maniera tale da conferire al manipolatore ridondanza
funzionale. Tale ridondanza viene risolta in fase di inversione cinematica con l’utilizzo di un algoritmo del primo ordine con pseudo-inversa
dello Jacobiano.
Figura 2.18: Schema a blocchi con pseudoinversa
q˙ = J† ve + (I4 − J† J)(
∂w(q) T
)
∂q
Dove il proiettore nel nullo (I4 − J† J) consente di generare moti interni
alla struttura lasciando inalterata la posa dell’end-effector mentre la
funzione w(q) ci consente di massimizzare un vincolo secondario. Nel
nostro caso abbiamo scelto la seguente funzione obiettivo:
w(q) =
�
det(J(q)JT (q))
che si annulla in corrispondenza di una configurazione singolare, per
cui massimizzando tale indice si utilizza la ridondanza per allontanarsi
© 2013 by A.Salvati, A.Tufano
23
2. Pianificazione della Traiettoria
dalle singolaritá. Per completezza riportiamo:

0


0
(I4 − J† J) = 

0

0
0
0
0
0
0
0
0
0



0
0





a1 a2 cos ϑ2 
0
∂w(q)
;


=


∂q


0
0



1
0
I casi analizzati riguardano il rilassamento, rispettivamente, della variabile di orientamento Φ e della variabile di posizione x. Nel caso in
cui si trascura l’orientamento lo Jacobiano risulta essere:
J(O)


−a2 s12 − a1 s1 −a2 s12 0 0



=  a2 c12 + a1 c1
a2 c12 0 0


0
0
1 0
Viceversa se é la coordinata x ad essere trascurata lo Jacobiano diventa:
J(x)


a c + a1 c1 a2 c12 0 0
 2 12


=

0
0
1
0


1
1
0 1
Di seguito riportiamo i risultati ottenuti.
© 2013 by A.Salvati, A.Tufano
24
2. Pianificazione della Traiettoria
Figura 2.19: Traiettorie
Figura 2.20: Confronto Angolo t2
© 2013 by A.Salvati, A.Tufano
2. Pianificazione della Traiettoria
25
Figura 2.21: Misura di manipolabilitá
Come ci si aspettava, essendo il compito di posizionamento disaccoppiato da quello di orientamento, non si ottiene alcun miglioramento
della destrezza del manipolatore nel caso in cui si rilassi il vincolo di
orientamento. Viceversa nel secondo caso si ottiene un sostanziale
miglioramento della manipolabilitá, sebbene il compito fittizio considerato sia di scarso interesse pratico.
L’errore di inseguimento non é riportato in quanto risulta praticamente
invariato rispetto all’andamento della soluzione non vincolata, proprio
perché il termine aggiuntivo di velocitá viene proiettato nel nullo dello
Jacobiano, cosí da non alterare le prestazioni del compito posizionale
specificato all’organo terminale.
© 2013 by A.Salvati, A.Tufano
26
2. Pianificazione della Traiettoria
© 2013 by A.Salvati, A.Tufano
CAPITOLO
3
Dinamica
La deduzione del modello dinamico di un manipolatore gioca un ruolo di estrema importanza in relazione a problemi di simulazione del
moto, di analisi di strutture di manipolazione e di determinazione di
algoritmi di controllo. La simulazione del moto di un manipolatore
consente di provare strategie di controllo e tecniche di pianificazione di
traiettoria senza la necessitá di riferirsi a una struttura di manipolazione fisicamente disponibile. La conoscenza di forze e coppie richieste
per l’esecuzione di movimenti tipici fornisce informazioni utili al progetto di giunti e trasmissioni e alla scelta degli attuatori. L’obiettivo
della dinamica é pertanto la derivazione delle equazioni del moto di
un manipolatore nello spazio dei giunti. Abbiamo calcolato il modello
dinamico nello spazio dei giunti tramite la formulazione di Lagrange: essendo semplice e sistematica, consente di giungere al modello
dinamico in forma chiusa ed é basata sul calcolo dell’energia.
Il modello dinamico nello spazio dei giunti risulta essere il seguente:
B(q)¨
q + C(q, q)
˙ q˙ + Fv q˙ + g(q) = τ − JT (q)he
28
3. Dinamica
Ove B(q) é la matrice di inerzia:

b (q ) b12 (q2 )
b22
B(q) =
b32
b41
b42
 11 2

b21 (q2 )


 b31

con:
b13
b23
b33
b43

b14 

b24 


b34 

b44
b11 = Il1 + Il2 + Il3 + Il4 + Im1 kr21 + Im2 + Im3 + Im4 + a21 ml1
+ (a21 + a22 )(ml2 + ml3 + ml4 ) + (l1 − 2a1 )ml1 l1 + (l2 − 2a2 )ml2 l2
+ 2a1 c2 [a2 (ml2 + ml3 + ml4 ) − ml2 l2 ]
b22 = Il2 + Il3 + Il4 + Im2 kr22 + Im3 + Im4 + a22 (ml2 + ml3 + ml4 )
+ (l2 − 2a2 )ml2 l2
b33 = Im3 kr23 + ml3 + ml4
b44 = Il4 + Im4 kr24
b12 = b21 = Il2 + Il3 + Il4 + Im2 kr2 + Im3 + Im4 + a22 (ml2 + ml3 + ml4 )
+ (l2 − 2a2 )ml2 l2 + a1 c2 [a2 (ml2 + ml3 + ml4 ) − ml2 l2 ]
b13 = b23 = b31 = b32 = Im3 kr3
b14 = b24 = b41 = b42 = Il4 + Im4 kr4
b34 = b43 = 0
La matrice C(q, q)
˙ contiene i contributi delle forze centrifughe e delle
forze di Coriolis, con la seguente struttura:

c (q , q˙ ) c12 (q2 , q˙1 , q˙2 )
 11 2 2

c21 (q2 , q˙1 )
0
C(q, q)
˙ =


0
0

0
0
© 2013 by A.Salvati, A.Tufano
0
0
0
0

0

0


0

0
29
3. Dinamica
gli elementi di tale matrice, ricavati utilizzando i simboli di Christoffel,
risultano avere le seguenti espressioni:
c11 = [−a1 a2 (ml2 + ml3 + ml4 ) + a1 (ml2 l2 )]s2 q˙2
c12 = [−a1 a2 (ml2 + ml3 + ml4 ) + a1 (ml2 l2 )]s2 (q˙1 + q˙2 )
c21 = [a1 a2 (ml2 + ml3 + ml4 ) − a1 (ml2 l2 )]s2 q˙1
Fv é la matrice diagonale dei coefficienti di attrito viscoso:


F k2
0
0
0 
 m1 r1


 0
Fm2 kr22
0
0 


Fv = 

2
 0

0
F
k
0
m3 r3


0
0
0
Fm4 kr24
Infine, il termine gravitazionale é:
g = [0 0 g(ml3 + ml4 ) 0]T
3.1
Parametrizzazione del modello
Dovendo realizzare un controllo adattativo, risulta utile considerare il
moto libero ponendo he = 0; il modello dinamico diventa:
B(q)¨
q + C(q, q)
˙ q˙ + Fv q˙ + g(q) = τ
che corrisponde al seguente set di equazioni:
b11 q¨r1 + b12 q¨r2 + b13 q¨r3 + b14 q¨r4 + c11 q˙r1 + c12 q˙r1 + Fm1 kr21 q˙r1 = τ1
b21 q¨r1 + b22 q¨r2 + b23 q¨r3 + b24 q¨r4 + c21 q˙r1 + Fm2 kr22 q˙r2 = τ2
b31 q¨r1 + b32 q¨r2 + b33 q¨r3 + Fm3 kr23 q˙r3 + g(ml3 + ml4 ) = τ3
b41 q¨r1 + b42 q¨r2 + b44 q¨r4 + Fm4 kr24 q˙r4 = τ4
A questo punto possiamo individuare il vettore π composto da 14 parametri (o 13, se vogliamo isolare ml4 relativo al carico in punta al
© 2013 by A.Salvati, A.Tufano
30
3. Dinamica
manipolatore), i cui elementi sono:
π1 = Il1 + Im1 kr21 + a21 ml1 + (l1 − 2a1 )ml1 l1
π2 = Fm1
π3 = Il2 + Il3
π4 = Im2
π 5 = m l 2 + m l 3 + m l4
π6 = ml2 l2
π7 = Fm2
π8 = Im3
π9 = Fm3
π10 = Il4
π11 = Im4
π12 = Fm4
π13 = ml3
π14 = ml4
Come ben noto dalla teoria, possiamo riscrivere le equazioni in forma
compatta tramite la matrice dei regressori Y le cui dimensioni sono
(4 × 14):
Y(q, q,
˙ q˙ r , q
¨r )π = τ
© 2013 by A.Salvati, A.Tufano
3. Dinamica
31
gli elementi di Y sono riportati di seguito:
y1,1 = q¨r1
y1,2 = kr22 q˙r1
y1,3 = q¨r1 + q¨r2
y1,4 = q¨r1 + kr22 q˙r2
y1,5 = (a21 + a22 + 2a1 a2 c2 )¨
qr1 + (a22 + 2a1 a2 c2 )¨
qr2 − a1 a2 s2 [q˙2 q˙r1 + (q˙1 + q˙2 )q˙r2 ]
y1,6 = [(l2 − 2a2 − 2a1 c2 )¨
qr1 + (l2 − 2a2 − a1 c2 )¨
qr2 + a1 s2 [q˙2 q˙r1 + (q˙1 + q˙2 )q˙r2 ]]
y1,7 = 0
y1,8 = q¨r1 + q¨r2 + kr3 q¨r3
y1,9 = 0
y1,10 = q¨r1 + q¨r2 + q¨r4
y1,11 = q¨r1 + q¨r2 + kr4 q¨r4
y1,12 = y1,13 = y1,14 = 0
y2,1 = y2,2 = 0
y2,3 = y1,3
y2,4 = kr2 y1,4
y2,5 = (a22 + 2a1 a2 c2 )¨
qr1 + a22 q¨r2 + (a1 a2 s2 q˙1 )q˙r1
y2,6 = (l2 − 2a2 − a1 c2 )¨
qr1 + (l2 − 2a2 )¨
qr2 − (a1 s2 q˙1 )q˙r1
y2,7 = kr22 q˙r2
y2,8 = y1,8
y2,9 = 0
y2,10 = y1,10
y2,11 = y1,11
y2,12 = y2,13 = y2,14 = 0
© 2013 by A.Salvati, A.Tufano
32
3. Dinamica
y3,1 = y3,2 = y3,3 = y3,4 = y3,5 = y3,6 = y3,7 = 0
y3,8 = kr3 y1,8
y3,9 = kr23 q˙r3
y3,10 = y3,11 = y3,12 = 0
y3,13 = y3,14 = q¨r3 + g
y4,1 = y4,2 = y4,3 = y4,4 = y4,5 = y4,6 = y4,7 = y4,8 = y4,9 = 0
y4,10 = y1,10
y4,11 = kr4 y1,11
y4,12 = kr24 q˙r4
y4,13 = y4,14 = 0
Come detto precedentemente, la ml4 rappresenta la massa del carico
in punta, quindi in sede di controllo é da considerarsi nulla in quanto
ignota e il suo vero valore verrá utilizzato solo nella dinamica diretta
per la simulazione. La ml3 é invece la somma delle masse dei bracci 3
e 4 (il polso). Inoltre il momento di inerzia del braccio 3 lungo z, Il3 ,
é trascurabile rispetto a quello del polso ed é quindi stato considerato
nullo. Tramite il seguente modello Simulink
Figura 3.1: Schema Simulink dinamica inversa
é possibile risolvere il problema dinamico inverso che consiste nel calco© 2013 by A.Salvati, A.Tufano
3. Dinamica
33
lo delle coppie ai giunti τ (t) necessarie alla generazione del movimento
specificato assegnando le accelerazioni q¨(t), le velocitá q(t)
˙
e le posizioni q(t) dei giunti note le eventuali forze all’organo terminale he (t).
Figura 3.2: Coppie e Forze ai giunti
Il manipolatore Scara analizzato in questo elaborato é azionato da
motori rotanti, calettati direttamente sui giunti 1 e 2 senza organi di
riduzione , mentre vengono utilizzati riduttori per i giunti 3 e 4. Di
conseguenza i problemi di attrito, elasticitá e giochi sono eliminati per
i primi due giunti, diventando peró significativi i pesi delle non linearitá e dell’accoppiamento tra i giunti; d’altro canto invece la presenza di
riduttori per gli ultimi due giunti tenderá a linearizzare la dinamica del
sistema e a disaccoppiare i giunti per via della riduzione degli effetti
della non linearitá e dell’accoppiamento sugli attuatori stessi.
© 2013 by A.Salvati, A.Tufano
34
3. Dinamica
© 2013 by A.Salvati, A.Tufano
CAPITOLO
4
Controllo del Moto
4.1
Controllo a Dinamica Inversa
Tale strategia di controllo si basa sulla proprietá del modello dinamico
di avere una struttura lineare nel controllo u e di avere una matrice
B(q) invertibile per qualsiasi configurazione del robot; ció rende possibile una linearizzazione globale del sistema tramite una retroazione
non lineare dello stato.
Figura 4.1: Schema di principio
36
4. Controllo del Moto
É possibile individuare due anelli di controllo indipendenti nello schema: l’anello interno rende disponibile un legame ingesso-uscita lineare e
disaccoppiato mentre l’anello esterno si occupa di stabilizzare il sistema
lineare e stazionario complessivo rendendo molto semplice il progetto
del controllore. Questa tecnica di controllo basandosi su cancellazione
di contributi presenta problemi di sensibilitá e scarsa robustezza poiché
le compensazioni non si riescono a effettuare perfettamente a causa del
modello dinamico del manipolatore, che é sempre noto con un certo
grado d’indeterminazione, e numerosi altri fattori d’incertezza; inoltre
impone vincoli sull’architettura hardware/software dell’unitá di governo. Per ovviare a tali problematiche si modifica lo schema di Figura4.1
inserendo un’azione che conferisce robustezza al controllo (Figura4.2).
4.2
Controllo Robusto
Figura 4.2: Schema di principio
Implementato in ambiente simulink1 come in Figura4.3.
1
Tutti gli schemi di controllo sono stati realizzati con un periodo di campionamento di 1ms mediante l’interposizione di elementi a ritenuta di ordine zero (ZOH)
nelle linee di misura e di attuazione
© 2013 by A.Salvati, A.Tufano
4. Controllo del Moto
© 2013 by A.Salvati, A.Tufano
Figura 4.3: Controllo Robusto in Simulink
37
38
4. Controllo del Moto
Il blocco Compensazione non lineare e Disaccoppiamento realizza la
compensazione ed il disaccoppiamento tipici di un controllo a dinamica
inversa, sulla base del modello dinamico fornito, che puó risultare piú
o meno approssimato. Si é scelto il modello dinamico approssimato:
u = By + g
con B matrice costante e diagonale relativa alle inerzie medie agli assi
di ogni giunto con elementi
b11 = Il1 + Il2 + Il4 + Im1 kr21 + Im2 + Im3 + Im4 + a21 ml1
+ (a21 + a22 )(ml2 + ml3 ) + (l1 − 2a1 )ml1 l1 + (l2 − 2a2 )ml2 l2
b22 = Il2 + Il4 + Im2 kr22 + Im3 + Im4 + a22 (ml2 + ml3 )
+ (l2 − 2a2 )ml2 l2
b33 = Im3 kr23 + ml3
b44 = Il4 + Im4 kr24


0 



 0 


g=

gml3 


0
In particolare la massa di carico in punta al manipolatore ml4 é stata
posta uguale a 0 in quanto sconosciuta.
Il blocco Controllo Robusto realizza la legge di controllo lineare stabilizzante e quella a cui é demandata la robustezza del controllo
y=q
¨d + KD q
˜˙ + KP q
˜+w
con
dove

 ρ
z se �z� ≥ ε
w =  �z�
ρ
z se �z� < ε
ε
© 2013 by A.Salvati, A.Tufano
4. Controllo del Moto
39
• z = DT Qξ con ξ = [˜
qT q
˜˙ T ]T ;
• w rappresenta il contributo di robustezza che contrasta l’errore
di modellazione con cui sono calcolati gli elementi non lineari;
• ρ é uno scalare positivo tanto maggiore quanto piú é grande
l’incertezza;
• ε rappresenta l’ampiezza della fascia all’interno della quale pendola l’errore.
Scegliendo i seguenti parametri:
KD = diag([10 10 5 10])
KP = diag([100 100 50 100])
D = [zeros(4; 4); eye(4; 4)]
Q = [diag([10 10 10 10]) diag([10 10 10 10]); 10KP KD ]
ρ = 10
ε=1
si nota nelle fig.4.4,4.5,4.6, che gli errori sono abbastanza contenuti
e la variabile di controllo non presenta oscillazioni di rilievo in virtú
della scelta piuttosto ampia del boundary layer ε. Come ci si poteva aspettare l’errore sulle velocitá é meno contenuto, avendo pesato
maggiormente l’errore in posizione.
© 2013 by A.Salvati, A.Tufano
40
4. Controllo del Moto
Figura 4.4: Errore di posizione (ρ = 10, ε = 1)
Figura 4.5: Errore di velocitá (ρ = 10, ε = 1)
© 2013 by A.Salvati, A.Tufano
4. Controllo del Moto
41
Figura 4.6: Coppie (ρ = 10, ε = 1)
Scegliendo ε = 0.1 e lasciando inalterato ρ = 10, si nota un leggero
miglioramento degli errori di posizione e velocitá:
Figura 4.7: Errore di posizione (ρ = 10, ε = 0.1)
© 2013 by A.Salvati, A.Tufano
42
4. Controllo del Moto
((a))
,
((b))
Figura 4.8: Errore di velocitá e coppie (ρ = 10, ε = 0.1)
© 2013 by A.Salvati, A.Tufano
4. Controllo del Moto
43
Se a questo punto pensiamo di porre ρ = 10 e ε = 0.01, si puó notare nelle fig.4.9,4.10, che diminuendo il boundary layer si desta il fenomeno del chattering. Le componenti in alta frequenza dell’azione di
controllo causerebbero delle sollecitazioni dannose per il manipolatore.
Figura 4.9: Errore di posizione (ρ = 10, ε = 0.01)
© 2013 by A.Salvati, A.Tufano
44
4. Controllo del Moto
((a))
,
((b))
Figura 4.10: Errore di velocitá e coppie (ρ = 10, ε = 0.01)
© 2013 by A.Salvati, A.Tufano
4. Controllo del Moto
45
Per ottenere l’andamento desiderato basta fare una media delle
traiettorie affette dal fenomeno di chattering.
Si é appena visto quindi che la scelta di un valore di soglia piccolo per
ε comporta delle componenti in alta frequenza nelle coppie dei giunti,
a vantaggio di un errore di inseguimento molto ridotto; d’altro canto,
aumentando il valore della soglia ε, la coppia assume un andamento
piú regolare a discapito di un aumento dell’errore di inseguimento.
© 2013 by A.Salvati, A.Tufano
46
4.3
4. Controllo del Moto
Controllo Adattativo
Il controllo adattativo sfrutta la proprietá di linearitá nei parametri del
modello dinamico per definire una legge di controllo robusta a fronte
di incertezze sui parametri stessi.
Figura 4.11: Schema di principio
La legge di controllo u é scelta pari a:
ˆ qr + C(q,
ˆ
ˆ V q˙ r + g
u = B(q)¨
q)
˙ q˙ r + F
ˆ(q) + KD σ = Y(q, q,
˙ q˙ r , q
¨r )ˆ
π + KD σ
con
q˙ r = q˙d + Λ˜
q
σ = q˙r − q˙ = q˜˙ + Λ˜
q
Abbiamo quindi impostato la seguente legge dinamica di adattamento
dei parametri:
T
π
ˆ˙ = K−1
˙ q˙ r , q
¨r )σ
π Y (q, q,
si ottiene la convergenza a zero del sistema di errore equivalente e la
limitatezza di π
ˆ asintoticamente se risulta:
Y(q, q,
˙ q˙ r , q
¨r )(ˆ
π − π) = 0
ció non vuol dire che π
ˆ tenda a π; la convergenza dei parametri al valore
vero dipende dalla struttura di Y e quindi dalle leggi del moto da cui
dipende. Questo tipo di controllo non serve a determinare il vettore
© 2013 by A.Salvati, A.Tufano
4. Controllo del Moto
47
esatto dei parametri ma a trovare una legge di controllo efficiente.
Lo schema Simulink che realizza la legge di controllo generale é in
Figura4.12.
I sottosistemi Y e Y � sigma calcolano, rispettivamente, il regressore ed
il blocco YT (q, q,
˙ q˙ r , q
¨r )σ. L’integratore discreto é stato utilizzato per
ricavare i parametri correnti dalla legge di aggiornamento.
Si é supposto di conoscere tutti i parametri con certezza, eccetto la
massa del carico in punta, la quale oscilla intorno al suo valore effettivo.
Le matrici caratteristiche del controllo in tal caso sono state scelte
come:
Λ = diag[5 5 5 5]
KD = diag[200 200 500 200] ∗ 10
Kπ = eye(14, 14)
Kπ (14, 14) = 0.01
Di seguito si riportano lo schema in ambiente Simulink e i risultati
ottenuti.
© 2013 by A.Salvati, A.Tufano
© 2013 by A.Salvati, A.Tufano
4. Controllo del Moto
48
Figura 4.12: Schema Simulink per il Controllo Adattativo
4. Controllo del Moto
Figura 4.13: Controllo Adattativo: Errore di posizione
Figura 4.14: Controllo Adattativo: Coppie
© 2013 by A.Salvati, A.Tufano
49
50
4. Controllo del Moto
Figura 4.15: Controllo Adattativo: Stima del Carico
© 2013 by A.Salvati, A.Tufano
CAPITOLO
5
Controllo di Forza
Il piú delle volte il manipolatore non é libero di muoversi liberamente
nello spazio ma deve tenere conto di alcuni vincoli naturali, imposti
dalla geometria del compito, e vincoli artificiali, imposti dal controllista. Questo tipo di problematiche di controllo vengono affrontate
con schemi di controllo nello spazio operativo e comporta un carico
computazionale piú gravoso per l’unitá di governo; la grandezza fondamentale da controllare é la forza d’interazione tra l’organo terminale
e l’ambiente. Oltre al modello cinematico e dinamico del manipolatore é necessario avere informazioni dettagliate anche sull’ambiente, non
sempre possibile.
Per la nostra analisi si suppone che il manipolatore entri in contatto
con un piano orizzontale elasticamente cedevole posto ad una quota
z = 0.6m e caratterizzato da una rigidezza pari a 5 · 103 N/m.
52
5.1
5. Controllo di Forza
Controllo di Impedenza
Lo schema di principio
Figura 5.1: Schema di principio
é stato implementato in Simulink come in Figura5.2.
Lo schema presenta i blocchi relativi alla dinamica del manipolatore
ed alla parte di compensazione non lineare e disaccoppiamento che
presentano in ingresso anche l’informazione relativa alla forza applicata
all’organo terminale, necessaria sia come informazione nel modello sia
per utilizzare la seguente compensazione
u = B(q)y + n(q, q)
˙ + JT (q)he
© 2013 by A.Salvati, A.Tufano
5. Controllo di Forza
© 2013 by A.Salvati, A.Tufano
53
Figura 5.2: Schema Simulink Controllo di Impedenza
54
5. Controllo di Forza
La forza dell’organo terminale é generata nel sottosistema forza
ambiente-manipolatore sulla base della posizione dell’end effector. Il
controllo di impedenza é realizzato nel blocco Controllo di Impedenza
grazie a una Matlab function che realizza la seguente legge di controllo
˙
y = J−1 (q)M−1
¨d + KD x
˜˙ + KP x
˜ − Md J(q,
q)
˙ q˙ − he )
d (Md x
da cui si ottiene
Md x
¨d + KD x
˜˙ + KP x
˜ = he
Le variabili desiderate sono ovviamente quelle nello spazio operativo e
l’errore é ricavato sottraendo a queste le variabili effettive nello spazio
operativo ricavate dalla cinematica diretta.
É interessante notare che:
• tale strategia di controllo realizza un controllo indiretto in forza,
cioé la forza viene modulata tramite un controllo di moto;
• l’ambiente é stato modellato con una legge di tipo elastica del
tipo fe = K(xe − xr ).
Le matrici caratteristiche del controllo, che definiscono l’impedenza
attiva conferita al manipolatore, sono state scelte come segue
Md = diag[1 1 1 1]
KD = diag[70 70 70 70]
KP = diag[40 40 40 40]
e abbiamo ottenuto i risultati di fig.5.3,5.4,5.5,5.6: l’errore di posizione
risulta limitato finché il manipolatore non impatta sul piano orizzontale, a questo punto affonda leggermente nel piano sviluppando una
forza massima di circa 16N , per poi continuare a seguire la traiettoria nelle altre direzioni. Si é quindi conferito al manipolatore un
comportamento cedevole nei confronti dell’ambiente.
© 2013 by A.Salvati, A.Tufano
5. Controllo di Forza
Figura 5.3: Controllo di Impedenza : Errore di posizione
Figura 5.4: Controllo di Impedenza : Forza all’organo terminale
© 2013 by A.Salvati, A.Tufano
55
56
5. Controllo di Forza
Figura 5.5: Controllo di Impedenza : Coppie
Figura 5.6: Controllo di Impedenza :Traiettorie
© 2013 by A.Salvati, A.Tufano
5. Controllo di Forza
5.2
57
Controllo di Forza
I controlli di forza presentati di seguito hanno un riferimento che, insieme al relativo controllo di forza, viene attivato solo al primo contatto
con il piano cedevole. Prima di tale contatto la traiettoria viene seguita
con un modello a dinamica inversa nello spazio operativo.
5.2.1
Anello interno di posizione
Figura 5.7: Schema di principio
implementato in Simulink come in Figura5.8.
L’azione di controllo assume la seguente forma
˙
y = J−1 (q)M−1
˙ e + KP (xF − xe ) − Md J(q,
q)
˙ q)
˙
d (−KD x
Il riferimento di posizione xF é calcolato a partire dall’errore di forza
mediante un controllore PI, soltanto al primo contatto del manipolatore con il piano cedevole; la sequenza di blocchi, nella quale compare
anche un relé, realizzano proprio un rilevatore del primo fronte di salita della forza misurata. Dal momento del contatto fino alla fine della
traiettoria si passa dal controllo a dinamica inversa ad un controllo di
forza con anello interno di posizione.
© 2013 by A.Salvati, A.Tufano
© 2013 by A.Salvati, A.Tufano
5. Controllo di Forza
58
Figura 5.8: Schema Simulink anello interno di posizione
5. Controllo di Forza
59
Le matrici caratteristiche del controllo a dinamica inversa sono:
KD = diag[150 150 150 150]
KP = diag[400 400 400 400]
Quando poi il manipolatore entra in contatto con il piano le matrici
sono modificate:
KD = diag[100 100 100 100]
KP = diag[200 200 0.1 200]
I guadagni del PI sono stati fissati pari a 2 per il proporzionale e 4 per
l’azione integrale.
Dai risultati delle simulazioni,é possibile notare che il manipolatore
una volta entrato in contatto col piano realizza la forza d’interazione
desiderata di 50N , mentre lungo le direzioni non interessate al contatto
é nulla.
© 2013 by A.Salvati, A.Tufano
60
5. Controllo di Forza
Figura 5.9: Controllo Forza (a.i.p.): Errore di posizione
Figura 5.10: Controllo Forza (a.i.p.): Forza all’organo terminale
© 2013 by A.Salvati, A.Tufano
5. Controllo di Forza
Figura 5.11: Controllo Forza (a.i.p.): Coppie
Figura 5.12: Controllo Forza (a.i.p.): Traiettorie
© 2013 by A.Salvati, A.Tufano
61
62
5. Controllo di Forza
5.2.2
Anello interno di velocitá
Figura 5.13: Schema di principio
implementato in Simulink come in Figura5.14.
L’azione di controllo diventa
˙
y = J−1 (q)M−1
˙ e + KP xF − Md J(q,
q)
˙ q)
˙
d (−KD x
Anche in questo caso un rilevatore di fronte di salita switcha tra controllo a dinamica inversa e controllo di forza non appena l’end-effector
entra in contatto col piano elasticamente cedevole. Da notare che:
• in questo caso viene eliminata l’azione integrale sull’errore di
forza;
• Le matrici sono le stesse del caso precedente.
Di seguito i risultati delle simulazioni.
© 2013 by A.Salvati, A.Tufano
5. Controllo di Forza
© 2013 by A.Salvati, A.Tufano
Figura 5.14: Schema Simulink anello interno di velocitá
63
64
5. Controllo di Forza
Figura 5.15: Controllo Forza (a.i.v.): Errore di Posizione
Figura 5.16: Controllo Forza (a.i.v.): Forza all’organo terminale
© 2013 by A.Salvati, A.Tufano
5. Controllo di Forza
Figura 5.17: Controllo Forza (a.i.v.): Coppie
Figura 5.18: Controllo Forza (a.i.v.): Traiettorie
© 2013 by A.Salvati, A.Tufano
65
66
5. Controllo di Forza
5.2.3
Controllo Parallelo Forza/Posizione
Figura 5.19: Schema di principio
implementato in Simulink come in Figura5.20.
La legge di controllo implementata é
˙
y = J−1 (q)M−1
˙ e + KP (˜
x + xF ) − Md J(q,
q)
˙ q)
˙
d (−KD x
con x
˜ = xd − xe .
Ancora una volta il controllo interviene appena l’end-effector entra in
contatto col piano cedevole; per realizzare l’inseguimento di traiettoria
nelle direzioni in cui il moto é libero viene aggiunto un termine di
feedforward sulla velocitá desiderata. I guadagni sono impostati come
nei casi precedenti.
© 2013 by A.Salvati, A.Tufano
5. Controllo di Forza
© 2013 by A.Salvati, A.Tufano
Figura 5.20: Schema Simulink controllo parallelo
67
68
5. Controllo di Forza
Figura 5.21: Controllo Forza Parallelo: Errore di Posizione
Figura 5.22: Controllo Forza Parallelo: Forza all’organo terminale
© 2013 by A.Salvati, A.Tufano
5. Controllo di Forza
Figura 5.23: Controllo Forza Parallelo: Coppie
Figura 5.24: Controllo Forza Parallelo: Traiettorie
© 2013 by A.Salvati, A.Tufano
69
70
5. Controllo di Forza
Dalle simulazioni si puó vedere che il manipolatore una volta entrato in contatto col piano segue la traiettoria desiderata nelle direzioni libere (Figura5.21), mentre la forza all’organo terminale subisce delle oscillazioni nell’intorno del valore desiderato ad ogni impatto
(Figura5.22).
© 2013 by A.Salvati, A.Tufano
Bibliografia
[1] Siciliano B., Sciavicco L., Villani L., Oriolo G., Robotica Modellistica, Pianificazione e Controllo, Terza Edizione, 2008,
McGraw-Hill.

TEAM ICT PEGASO - App Unipegaso

Circ. n. 60.A- Ricevimento docenti 14-15

COMITATO DIRETTIVO

Manipolatori uterini KARL STORZ

5.89. Urto contro una sfera ⋆⋆⋆