02.1 - Schemi di controllo centralizzato

Controllo PD + compensazione di gravit`
a
Controllo a dinamica inversa
Schemi di controllo centralizzato
Claudio Melchiorri
Dipartimento di Ingegneria dell’Energia Elettrica e dell’Informazione (DEI)
Universit`
a di Bologna
email: claudio.melchiorri@unibo.it
Claudio Melchiorri
Schemi di controllo centralizzato
1 / 15
Controllo PD + compensazione di gravit`
a
Controllo a dinamica inversa
Sommario
1
Controllo PD + compensazione di gravit`
a
2
Controllo a dinamica inversa
Claudio Melchiorri
Schemi di controllo centralizzato
2 / 15
Controllo PD + compensazione di gravit`
a
Controllo a dinamica inversa
Controllo PD + compensazione di gravit`
a
Si vuole determinare un controllore che assicuri una globale asintotica stabilit`
a
al sistema dinamico non lineare descritto da
˙ q˙ + Dq˙ + g(q) = u
M(q)¨
q + C(q, q)
Sia data una configurazione di equilibrio descritta da qd . Si definisca
˜ = qd − q
q
e si consideri il sistema dinamico con stato definito da
˜
q
x=
q˙
Per la determinazione del controllore, si utilizza il metodo diretto di Lyapunov.
Claudio Melchiorri
Schemi di controllo centralizzato
3 / 15
Controllo PD + compensazione di gravit`
a
Controllo a dinamica inversa
Controllo PD + compensazione di gravit`
a
Si considera la funzione candidata di Lyapunov
1 T
1 T
˜ KP q
˜>0
q˙ M(q)q˙ + q
2
2
dove KP `e una matrice quadrata definita positiva.
˙ =
V (q, q)
˙ q
˜ 6= 0
∀q,
˙ `e composta di due termini:
La funzione V (q, q)
1/2 q˙ T M(q)q˙
che rappresenta l’energia cinetica del sistema;
˜T KP q
˜
1/2 q
che pu`
o essere considerato come energia elastica immagazzinata da un
sistema di molle con rigidezza equivalente KP realizzato dagli anelli di
controllo.
Claudio Melchiorri
Schemi di controllo centralizzato
4 / 15
Controllo PD + compensazione di gravit`
a
Controllo a dinamica inversa
Controllo PD + compensazione di gravit`
a
˜˙ = −q):
˙
Derivando V rispetto al tempo (q
1
T
˙ q˙ − q˙ T KP q
˜
V˙ = q˙ M¨
q + q˙ T M
2
ma
M¨
q = u − Cq˙ − Dq˙ − g
per cui
V˙
=
=
=
Per`
o:
1 T ˙
˜
q˙ Mq˙ − q˙ T KP q
2
1
˙ q˙ − q˙ T KP q
˜
q˙ T (u − Cq˙ − Dq˙ − g) + q˙ T M
2
1 T ˙
˙ q˙ − q˙ T Dq˙ + q˙ T [u − g(q) − KP q
˜]
q˙ [M(q) − 2C(q, q)]
2
q˙ T M¨
q+
˙
˙ q˙ = 0
q˙ T [M(q)
− 2C(q, q)]
per la scelta fatta di C;
−q˙ T Dq˙ `e definito negativo;
scegliendo
˜
u = g(q) + KP q
ci si assicura che V˙ sia semidefinita negativa, poich`e
V˙ = 0
Claudio Melchiorri
q˙ = 0,
∀˜
q
Schemi di controllo centralizzato
5 / 15
Controllo PD + compensazione di gravit`
a
Controllo a dinamica inversa
Controllo PD + compensazione di gravit`
a
In realt`
a, si ottiene questo risultato anche con la scelta:
˜ − KD q˙
u = g(q) + KP q
essendo KD una matrice definita positiva, in quanto si ha
V˙ = −q˙ T (D + KD )q˙
e quindi un miglioramento in termini di rapidit`
a di convergenza delle traiettorie
a zero.
Il termine KD q˙ equivale all’introduzione di un’azione derivativa nell’anello di
controllo.
Claudio Melchiorri
Schemi di controllo centralizzato
6 / 15
Controllo PD + compensazione di gravit`
a
Controllo a dinamica inversa
Controllo PD + compensazione di gravit`
a
Osservazioni:
KD ✛
qd
˜
q
✲ ❧✲ KP
- ✻
+
u
❄
✲ ❧✲ ❧ ✲
q˙
Manipol.
- ✻
✲
✲
q
g(˙) ✛
Legge di controllo non lineare nei termini gravitazionali e con azione
lineare di tipo PD: il sistema risulta asintoticamente globalmente stabile
per qualsiasi scelta di KP , KD definite positive
L’azione derivativa `e essenziale in sistemi con bassi coefficienti di attrito F,
ad esempio Direct Drive controllati in corrente: il basso smorzamento
elettrico viene aumentato dall’azione di controllo;
Claudio Melchiorri
Schemi di controllo centralizzato
7 / 15
Controllo PD + compensazione di gravit`
a
Controllo a dinamica inversa
Controllo PD + compensazione di gravit`
a
V decresce fintanto che q˙ 6= 0. Si pu`
o vedere che si raggiunge uno stato
di equilibrio individuato dalla evoluzione di
˙ q˙ + Dq˙ + g(q) = g(q) + KP q
˜ − KD q˙
M(q)¨
q + C(q, q)
¨ = 0) si ha
All’equilibrio (q˙ = q
˜=0
KP q
e quindi
q = qd
il calcolo di g(q) deve essere perfetto, altrimenti la metodologia presentata
non consente di trarre nessuna conclusione sulla stabilit`
a (robustezza del
controllore).
Claudio Melchiorri
Schemi di controllo centralizzato
8 / 15
Controllo PD + compensazione di gravit`
a
Controllo a dinamica inversa
Controllo a dinamica inversa
Sistema MIMO non lineare descritto da
˙ q˙ + Dq˙ + g(q) = u
M(q)¨
q + C(q, q)
o, in breve
˙ =u
M(q)¨
q + n(q, q)
Si vuole determinare il controllo u in modo da avere un sistema MIMO lineare.
Si ottiene questo risultato (linearizzazione globale) mediante una retroazione
non lineare dello stato.
Si pu`
o dimostrare che tale controllo esiste in quanto:
il modello `e lineare nel controllo u;
la matrice M(q) `e invertibile per qualsiasi configurazione del manipolatore.
Si sceglie il controllo u dipendente dallo stato del manipolatore:
˙
u = M(q)y + n(q, q)
Claudio Melchiorri
Schemi di controllo centralizzato
9 / 15
Controllo PD + compensazione di gravit`
a
Controllo a dinamica inversa
Controllo a dinamica inversa
Con il controllo u
˙
u = M(q)y + n(q, q)
si ottiene
M¨
q + n = My + n
e quindi
¨=y
q
dove y `e il nuovo ingresso del sistema.
y
❄
u
✲ M(q) ✲ ❤ ✲
✻
q
Manip.
✛
s✲
✲
s
❍
❍
✟
✟
y
q˙
✲ R
q˙
✲ R
˙ ✛
n(q, q)
Claudio Melchiorri
Schemi di controllo centralizzato
10 / 15
q
✲
Controllo PD + compensazione di gravit`
a
Controllo a dinamica inversa
Controllo a dinamica inversa
Controllo a dinamica inversa: si deve calcolare la dinamica inversa del
manipolatore.
Complessivamente il sistema risulta lineare e disaccoppiato rispetto a y: yi
influenza solamente qi (yi = q¨i )
Si deve ora determinare una legge di controllo y che stabilizzi il sistema.
Scegliendo
y = −KP q − KD q˙ + r
¨ = y si ottiene
da q
¨ + KD q˙ + KP q = r
q
che, se le matrici KP , KD sono definite positive, risulta asintoticamente stabile.
Claudio Melchiorri
Schemi di controllo centralizzato
11 / 15
Controllo PD + compensazione di gravit`
a
Controllo a dinamica inversa
Controllo a dinamica inversa
Se le matrici KP , KD vengono scelte diagonali, e definite da
KP = diag {ωni2 }
KD = diag {2δi ωni }
si ottiene una dinamica per l’i-esima componente specificata dalla pulsazione
naturale ωni e dal coefficiente di smorzamento δi .
Per ottenere l’inseguimento di una data traiettoria qD (t), a questo punto si pu`
o
imporre
¨d + KD q˙ d + KP qd
r=q
e quindi complessivamente si ottiene la dinamica dell’errore di inseguimento:
¨
˜ + KD q
˜˙ + KP q
˜=0
q
˜(0) 6= 0, q
˜˙ (0) 6= 0, e converge a zero con dinamica
L’errore `e presente solo se q
dipendente da KP , KD .
Claudio Melchiorri
Schemi di controllo centralizzato
12 / 15
Controllo PD + compensazione di gravit`
a
Controllo a dinamica inversa
Controllo a dinamica inversa
¨d
q
q˙ d ✲ ❤
˜˙
q
✲
KD
✻
qd
˜
q
✲ ❤ ✲
KP
✻
❅
❄
u
❄y ✲
❘ ❤
❅
M(q) ✲ ❤ ✲
✒
✻
q
Manip.
✛
˙ ✛
n(q, q)
s✲
✲
s
q˙
Si ha complessivamente un sistema con due anelli di controllo.
Il primo `e basato su una retroazione non lineare dello stato e fornisce un
modello lineare e disaccoppiato tra le variabili y e q (doppio integratore);
il secondo `e lineare e stabilizza il sistema complessivo, ed il suo progetto `e
facilitato in quanto la dinamica che deve stabilizzare risulta lineare e
stazionaria;
`e richiesto il calcolo di tutti i termini del modello dinamico
˙ D, g(q)) che deve essere svolto in tempo reale poich`e si ha
(M(q), C(q, q),
un controllo con retroazione dello stato;
Claudio Melchiorri
Schemi di controllo centralizzato
13 / 15
Controllo PD + compensazione di gravit`
a
Controllo a dinamica inversa
Controllo a dinamica inversa
Ci sono problemi di carattere realizzativo:
conoscenza esatta del modello del manipolatore (carichi, dinamiche non
modellate, approssimazione nei parametri meccanici e geometrici, . . . );
calcolo in linea dei vari contributi dinamici;
Si considerano solo i termini principali, e le compensazioni risultano imperfette.
Necessit`
a di tecniche di controllo che compensano modelli non precisi:
Controllo robusto (sliding mode, ...);
Controllo adattativo.
Claudio Melchiorri
Schemi di controllo centralizzato
14 / 15
Controllo PD + compensazione di gravit`
a
Controllo a dinamica inversa
Controllo a dinamica inversa - Esempio
p
√
Sistema non lineare:
(2 + sin q)¨
q + q˙ 3 1 − 0.5 cos q + 1 + q 2 = u
Traiettoria desiderata: lineare con raccordi parabolici.
kp = 100,
kd = 14
Posizione
Velocita‘
100
20
80
15
60
10
40
5
20
0
0
1
2
3
5
6
7
8
9
10
0
0
1
2
3
Errore di Posizione
-3
2
4
x 10
4
5
6
7
8
9
10
7
8
9
10
Errore di Velocita‘
0.02
1
0
0
-1
-0.02
-2
-3
0
1
2
3
4
5
6
7
8
9
10
6
7
8
9
10
7
8
9
10
-0.04
0
1
2
3
4
5
6
Accelerazione
5
0
-5
0
1
2
3
4
5
Errore di Accelerazione
0.6
0.4
0.2
0
-0.2
0
1
2
3
4
5
6
Claudio Melchiorri
Schemi di controllo centralizzato
15 / 15