Controllo di traiettoria

Transcript

1 Corso di Robotica 2 Controllo di traiettoria Prof. Alessandro De Luca A. De Luca

2 Controllo a dinamica inversa dato il modello dinamico del robot B(q) q + n(q, q ) = u c(q, q ) + g(q) + modelli di attrito ed una traiettoria desiderata differenziabile due volte (t) " (t), (t) in condizioni nominali, applicando la coppia (feedforward) u d = B( ) + n(, ) si riproduce esattamente la traiettoria desiderata A. De Luca 2

3 In pratica... si hanno spesso condizioni non ideali stato iniziale non agganciato a quello della traiettoria (t) non accurata conoscenza dei parametri dinamici del robot (masse, inerzie, posizione baricentri) presenza di fenomeni non modellati (attriti complessi, elasticità, ) valore del carico trasportato non noto disturbi sugli attuatori, errori di troncamento, A. De Luca 3

4 In condizioni perturbate u d * = B * ( ) + n * (, ) con B *, n * stime dei termini nel modello dinamico N.B. u d * può essere tutto calcolato fuori linea per irrobustire lo schema di controllo lo si trasforma in uno a retroazione vari schemi implementativi a seconda della ripartizione del carico computazionale fra FUORI LINEA ( IN LINEA ( anello aperto) anello chiuso) strategia a due passi: 1. compensazione (feedforward)/cancellazione (feedback) delle non linearità 2. sintesi di un controllo lineare stabilizzante A. De Luca 4

5 Possibili schemi di asservimento 1. compensazione a dinamica inversa + PD costante u = u d * + K P " q ( ) + K D ( q d " q ) 2. compensazione a dinamica inversa + PD variabile u = u d * + B * ( ) K P " q ( ) + K D ( q d " q ) [ ] stabilizzazione solo locale dell errore di traiettoria e(t)= (t)-q(t) 3. feedback linearizzazione + PD costante = COMPUTED TORQUE u = B * (q) + K P " q ( ) + K D ( q d " q ) [ ] + n * (q, 4. feedback linearizzazione + PID costante [ ] + n * (q, u = B * (q) + K P " q ( ) + K D ( q d " q ) + K I # ( " q)dt il più robusto, ma anche il più complesso da implementare in tempo reale A. De Luca 5 q ) q )

6 Feedback linearizzazione (FL) + K D + a + u B * (q) + K P q B"1 (q) d _ + ROBOT (o suo MODELLO DINAMICO) q " + _ n(q, q ) q " q matrici K P, K D simmetriche definite positive K D n * (q, q ) in condizioni ideali (B * =B, n * =n) stabilizzazione lineare globale A. De Luca 6 K P B(q) q + n(q, q ) = u = B(q)a + n(q, q ) dinamica del robot controllo non lineare a = + K D ( q d " q ) + K P " q ( ) q = a sistema lineare e disaccoppiato

7 Interpretazione nel dominio lineare + K D + K P + - a = q q " " q diagonali, > 0 K D K P il robot sotto controllo FL ha un comportamento dinamico invariante, lineare e disaccoppiato in tutto lo spazio di stato (" q, q ) linearità i transitori di errore e i = i -q i 0 esponenzialmente (scelta di K Di, K Pi ) disaccoppiamento massa unitaria (m=1) nello spazio dei giunti!! ogni coordinata q i evolve in modo indipendente dalle altre, forzata da a i A. De Luca 7

8 Commenti traiettorie generate a partire da p d (t), p d (0), p d (0) cartesiane (t) q d (0) (0) q d (t) " " (t) ( ) (0) = f "1 p d (0) q d (0) = J "1 ( (0)) p d (0) [ ] (t) = J "1 (q) p d (t) " J (q) q simulazione del controllo FL. (t), (t),.. (t) parametri stimati ˆ " controllo FL robot parametri veri " q q ˆ " = " N.B. non ha molto senso simulare solo il caso ideale ( ), perché gli andamenti sono identici al caso lineare di doppi integratori stabilizzati A. De Luca 8

9 Altre considerazioni scelta degli elementi in diagonale di K P, K D (e K I ) in base al transitorio desiderato e ai vincoli di saturazione dei motori e(t)= (t)-q(t) e(0) identificazione parametrica da effettuarsi preliminarmente, sfruttando la linearità nei coefficienti dinamici del modello dei robot scelta del passo di campionamento smorzamento critico compromesso fra tempo di calcolo e accuratezza: T c 20 ms la linearizzazione esatta via feedback è una tecnica generale di controllo nonlineare estendibile al caso di robot con giunti elastici altre applicazioni: assetto satelliti, motori ad induzione, volo elicottero A. De Luca 9 t

10 Un altro esempio di FL in robotica modello dinamico di robot con giunti elastici θ = posizione dei motori (a valle delle riduzioni) 2N coordinate q = posizione dei bracci generalizzate M = matrice diagonale (>0) delle inerzie dei motori (bilanciati) K = matrice diagonale (>0) delle rigidezze dei giunti 4N variabili di stato esiste una legge di controllo che linearizza via feedback? u = "(#,q, #, q ) + $(#,q, #, q )a SI, e fornisce B(q) q + c(q, q ) +g(q) +K(q "#) = 0 M # + K(# " q) = u d 4 q i dt 4 = a i i =1,...,N hint: differenziare nel tempo la (1) fino a far comparire l accelerazione dei motori; sostiture la (2) e scegliere u per cancellare le nonlinearità A. De Luca 10 (1) (2) lineare e disaccoppiato (stabilizzato poi da un PDDD con guadagni opportuni)

11 Un altro controllore di traiettoria u = B(q) + S(q, q ) + g(q) + F V + K P e + K D e fattorizzazione. tale da avere B-2S antisimmetrica matrici simmetriche e definite positive non opera una cancellazione completa delle non linearità si valutano sulla traiettoria desiderata tutte le q(t) e sue derivate che compaiono linearmente nel modello non induce un comportamento lineare e disaccoppiato all errore di traiettoria e(t)= (t)-q(t) ad anello chiuso. stabilizza però globalmente (e,e) a zero si presta più agevolmente ad una versione adattativa analisi della stabilità asintotica con Lyapunov e LaSalle A. De Luca 11

12 Analisi di stabilità asintotica B(q) q + S(q, q ) q + g(q) + F V q = u legge di controllo u = B(q) + S(q, q ) + g(q) + F V + K P e + K D e candidata di Lyapunov e sua derivata V = 1 2 e T B(q) e e T K P e " 0 dal modello + controllo (sistema ad anello chiuso) modello dinamico robot B(q) e = "S( q, q ) e "K P e " (K D + F V ) e sostituendo e sfruttando la proprietà di antisimmetria analisi di LaSalle V = " e T ( K D + F V ) e # 0 B(q) e = "K P e " e = 0 # e = 0 V = 1 e 2 T B (q) e + e T B(q) e + e T K P e V = 0 " e = 0 ( e, e ) = 0,0 ( ) massimo insieme. invariante in V=0 A. De Luca 12

13 Regolazione come caso particolare cosa succede alle leggi di controllo per asservimento di traiettoria quando è costante? si semplificano? feedback linearizzazione nessuna particolare semplificazione però si mantiene la stabilizzazione esponenziale (oltre che il disaccoppiamento dei transitori) controllore globale alternativo [ ( ) "K D q ] + c(q, u = B(q) K P " q u = K P ( " q) "K D q + g(q) q ) + g(q) si ritrova il caso del PD + cancellazione della gravità A. De Luca 13