CONTROLLO DI ROBOT INDUSTRIALI IL FILTRO DI KALMAN

CONTROLLO DI SISTEMI ROBOTICI Laurea Specialistica in Ingegneria Meccatronica CONTROLLO DI ROBOT INDUSTRIALI IL FILTRO DI KALMAN Ing. Tel. 0522 522235 e-mail: secchi.cristian@unimore.it http://www.dismi.unimo.it/members/csecchi Tecnologia e Incertezza In pratica, l uscita di un sistema è una misura, ovvero l uscita di uno o più sensori che è affetta da un rumore che può essere dovuto a vari fattori come disturbi, limiti costruttivi e quantizzazione dell informazione I sistemi di attuazione, specialmente quelli low-cost, possono essere inaccurati e questa inaccuratezza può essere descritta come un rumore sull ingresso I modelli sono per definizione inaccurati e la loro imperfezione può esssere modellata come un rumore di processo Tutti questi rumori danno luogo sia a un incertezza di misura che a un incertezza di processo. Raggiungibilità e Controllabilità-- 2 Pag. 1

Tecnologia e Incertezza 2 m u k y b Incertezza di processo: attriti statici, dissipazione della molla, quantizzazione controllo Incertezza di misura: quantizzazione della misura (es.: encoder), rumore Raggiungibilità e Controllabilità-- 3 Tecnologia e Incertezza L incertezza rende problematico il problema della stima dello stato in quanto il rumore sull uscita falsa l informazione sullo stato e il rumore sul processo falsa il modello utilizzato per l osservatore. Se il modello è abbastanza accurato e sensori e attuatori abbastanza affidabili, l incertezza può essere trascurabile e l osservatore dello stato può essere utilizzato senza problemi Se l entità delle incertezze diventa significativa (es.: per applicazioni low cost), è necessario considerare il rumore nel problema della stima dello stato. Infatti l errore di misura (es.: y Cxˆ ) viene integrato dal modello del sistema e, di conseguenza, i rumori vengono integrati e l incertezza sulla stima si accumula Raggiungibilità e Controllabilità-- 4 Pag. 2

Esempio Raggiungibilità e Controllabilità-- 5 Esempio Se all uscita e sul processo è sovrapposto un rumore di media 0.1 e varianza 0.5 y(t) La stima degrada! Raggiungibilità e Controllabilità-- 6 Pag. 3

Esempio: Localizzazione Modellando l incertezza e sfruttando informazioni note è possibile gestire l incertezza e ottenere informazioni molto sicure Raggiungibilità e Controllabilità-- 7 Il filtro di Kalman Sfrutta un modello dell incertezza di misura e di processo Consente di costruire un osservatore che fornisce la stima ottima dello stato bilanciando l informazione estratta dal modello e quella estratta dalla misura a seconda dell affidabilità delle informazioni E usato in moltissime discipline: controlli, robotica, economia, computer vision R.E. Kalman, A New Approach to Linear Filtering and Prediction Problems, Transactions of the ASME 82, 1960 Rudolph Emil Kalman ha ricevuto la National Medal of Science da Barack Obama nel 2010 Raggiungibilità e Controllabilità-- 8 Pag. 4

Il filtro di Kalman Il rumore sulla misura e l incertezza sul modello può essere modellato come un processo stocastico, ovvero come una variabile casuale caratterizzato da alcuni parametri (media, varianza, ) che possono cambiare nel tempo Il modo in cui vengono descritte le incertezze agenti sul sistema è cruciale per sviluppare gli strumenti per l osservazione dello stato. Il filtro di Kalman utilizza processi Gaussiani per modellare le incertezze agenti sul sistema. Questa descrizione è spesso (ma non sempre!) la migliore per molte applicazioni Raggiungibilità e Controllabilità-- 9 Cenni di Probabilità Sia S l insieme di tutti i risultati di un fenomeno aleatorio S={testa,croce} S={1,2,3,4,5,6} S={1,2, } Un evento A è un sottoinsieme dei possibili risultati (A S) La probabilità che si verifichi un evento A è data da: Pr( A ) = numero dei risultati in A Numero dei risultati possibili Raggiungibilità e Controllabilità-- 10 Pag. 5

Cenni di Probabilità Lanciando un dado non truccato con 6 facce, qual è la probabilità che esca 1 o 2? S = {1,2,3,4,5,6} A ={1,2} 2 Pr( A) = = 6 1 3 Raggiungibilità e Controllabilità-- 11 Assiomi della probabilità Siano A e B due eventi: 0 Pr( A) 1 Pr( S) = 1 Pr( not( A)) = 1 Pr( A) Pr( ) = 0 Pr( A B) = Pr( A) + Pr( B) Pr( A B) True A A B B Se A e B sono eventi disgiunti, allora Pr( A B) = Pr( A) + Pr( B) Raggiungibilità e Controllabilità-- 12 Pag. 6

Variabili Aleatorie Discrete Una variabile aleatoria (random variable) discreta è una variabile che assume un valore casuale in un insieme di valori ammissibili. Ciascun valore ammissibile è caratterizzato da una certa probabilità di essere assunto. Formalmente, se X è una variabile aleatoria: 1) X può assumere un valore finito (o contabile) di valori {x 1, x 2,, x n }. 2) P(X=x i ), or P(x i ), è la probabilità che la variabile aleatoria X assuma il valore x i 3) Esempio: l esperimento lancio di un dado può essere rappresentato come una variabile aleatoria discreta X che può assumere i valori {x 1, x 2,, x 6 } ={1,2,3,4,5,6} e P(x i )=1/6 per i=1,,6 Raggiungibilità e Controllabilità-- 13 Variabili Aleatorie Continue Una variabile aleatoria continua è una variabile che assume un valore casuale in un insieme continuo di valori ammissibili. Ciascun valore ammissibile è caratterizzato da una certa probabilità di essere assunto. Formalmente, se X è una variabile aleatoria continua: 1) X può assumere valori in un intervallo continuo [x 1,x 2 ]. 2) p(x=x), o p(x) (o f(x)), è la funzione di densità di probabilità che la variabile aleatoria X assuma il valore x p(x) Pr( x ( a, b)) = p( x) dx b a E più probabile essere vicini al centro della campana x Raggiungibilità e Controllabilità-- 14 Pag. 7

La somma delle probabilità è 1 Variabili Aleatorie Discrete P( x) = 1 x Variabili Aleatorie Continue p ( x) dx =1 I valori che può assumere una variabile aleatoria possono essere interpretati come eventi disgiunti La probabilità che una variabile assuma un valore oppure un altro è la somma delle probabilità dei valori presi in considerazione La probabilità che una variabile aleatoria assuma un qualsiasi valore possibile è 1 Raggiungibilità e Controllabilità-- 15 Probabilità Congiunta e Condizionata Dati due eventi A e B, la probabilità congiunta è la probabilità che i due eventi si verifichino contemporaneamente. Date due variabili aleatorie X e Y la probabilità congiunta che X assuma il valore x e Y il valore y si indica come P(X=x ey=y) = P(x,y) Se X e Y sono indipendenti, allora P(x,y) = P(x) P(y) I lanci di un dado sono indipendenti. La probabilità di ottenre due volte lo stesso valore è 1/6*1/6=1/36 P(x y) indica la probabilità che X=x quando Y=y. In altre parole è la probabilità di x dato y P(x y) = P(x,y) / P(y) P(x,y) = P(x y) P(y) Se X e Y sono indipendenti, allora P(x y) = P(x) Raggiungibilità e Controllabilità-- 16 Pag. 8

Esempio Consideriamo un mazzo di carte. Il fenomeno aleatorio estrazione della prossima carta può essere modellato come una variabile aleatoria. Due giocatori estraggono una carta a turno e le estrazioni sono modellate con due variabili aleatorie X e Y. La probabilità un giocatore estragga un asso dopo che l altro ha estratto un asso è data da: 4 3 P( X = 1 Y = 1) = 52 51 = 4 52 12 204 Raggiungibilità e Controllabilità-- 17 Valore atteso (Expected Value) Generalizza il concetto di valore medio in un fenomeno aleatorio. Se il fenomeno è modellato con una variabile aleatoria discreta X allora il valore atteso è: E[ X ] = n i= 1 x P( i x i E[X] rappresenta il valore mediamente assunto da X considerando la probabilità con la quale possono essere assunti i valori ammissibili ) Esempio: X può assumere valori {x 1,x 2 }={1,2} con P(x 1 )=0.1 P(x 2 ) =0.9. Il valore atteso per X non è la media dei valori ammissibili (=1.5) ma E[X]=0.1*1+0.9*2=1.9. E[X] è spostato verso 2 perché 2 ha una probabilità di verificarsi molto maggiore. E[X] è la media dei valori che si ottengono da X dopo un numero abbastanza grande di esperimenti Raggiungibilità e Controllabilità-- 18 Pag. 9

Valore Atteso (Expected Value) Nel caso di variabili aleatorie continue, la definizione di valore atteso è concettualmente la stessa. Se l intervallo su cui è definita la variabile aleatoria X è R e p(x) indica la funzione di densità di probabilità si ha che E [ X ] = xp( x) dx Raggiungibilità e Controllabilità-- 19 Varianza La varianza σ 2 di un segnale attorno al suo valore atteso è data da 2 σ = E[( X E( X )) ] =! 2 = E[(X! E[X]) 2 ] = (x! E[X]) 2 p(x)dx 2 " #!" n i= 1 ( x E[ X ]) P( 2 i x i La varianza rappresenta la dispersione di una variabile aleatoria attorno al suo valore atteso. Maggiore è la varianza, maggiore è la dispersione intorno al valore medio. ) Tempo discreto Tempo continuo E[X]=1 σ 2 =1.41 E[X]=1 σ 2 =0.32 Raggiungibilità e Controllabilità-- 20 Pag. 10

Variabili Incorrelate Due variabili aleatorie X e Y si dicono incorrelate, se E [ XY ] = E[ X ] E[ Y] Intuitivamente questo implica che non esiste nessuna correlazione tra due fenomeni aleatori e che la loro combinazione non introduce nessun termine aggiuntivo al loro valore atteso Due variabili indipendenti sono incorrelate ma non è sempre vero il contrario Raggiungibilità e Controllabilità-- 21 Variabili Aleatorie Vettoriali Un vettore aleatorio X =(x 1,,x n ) T è un vettore le cui componenti sono variabili aleatorie Il valore atteso di un vettore aleatorio X è dato da: E[ x1 ] E[ X ] = E[ x n ] La matrice di covarianza Σ di un vettore aleatorio X è data da T Σ = E[( X E[ X ])( X E[ X ]) ] E la generalizzazione della varianza! Raggiungibilità e Controllabilità-- 22 Pag. 11

Variabili Aleatorie Vettoriali Nel caso scalare, la definizione di matrice di covarianza coincide con quella di varianza I termini sulla diagonale sono le varianze delle componenti del vettore aleatorio X I termini fuori dalla diagonale della matrice di covarianza indicano la correlazione che c è tra le componenti della matrice di covarianza Se un vettore aleatorio X è costituito da variabili aleatorie incorrelate, la matrice di covarianza e diagonale Raggiungibilità e Controllabilità-- 23 Variabili Aleatorie Gaussiane Una variabile aleatoria continua si dice variabile gaussiana (o variabile normale o semplicemente gaussiana) se la sua densità di probabilità è una curva di Gauss del tipo f ( xµ ) 1 2 2σ ( x) = e σ 2π 2 f(x) Si dimostra che: E[X]=µ E[(X-µ) 2 ]=σ 2 Il valore atteso si dice anche valore medio o media della gaussiana Più la campana è stretta minore è la varianza Una variabile aleatoria Gaussiana è completamente descritta dal suo valore atteso e dalla sua varianza µ -σ σ x Raggiungibilità e Controllabilità-- 24 Pag. 12

Variabili Aleatorie Gaussiane Un vettore aleatorio si dice gaussiano (o normale) se la sua densità di probabilità è del tipo f ( x) = 1 e n (2π ) det( Σ) 1 T 1 ( xµ ) Σ 2 ( xµ ) E possibile mostrare che la probabilità che nel 95% dei casi il vettore si trova entro un elissoide di incertezza centrato in µ e descritto da x T Σx=1 µ Si dimostra che: E[X]=µ E[(X-µ) (X-µ) T ]=Σ Raggiungibilità e Controllabilità-- 25 Variabili Aleatorie Gaussiane e Robotica Le variabili Gaussiane sono semplici perché sono descritte solo da due parametri Consentono di modellare incertezze molto comuni nella robotica fissa e, soprattutto, mobile Dove sono? In questa posizione a meno di una certa incertezza Indicheremo l insieme delle variabili aleatorie Gaussiane di media µ e varianza σ 2 come N(µ, σ 2 ) e l insieme dei vettori aleatori di media µ e matrice di covarianza Σ come N(µ, Σ) Raggiungibilità e Controllabilità-- 26 Pag. 13

Il filtro di Kalman Illustreremo il filtro di Kalman per sistemi discreti Il filtro viene usato quando è necessario considerare rumore di processo e/o di misura e, quindi, l uscita discreta dei sensori Una sua versione estesa è molto utilizzata nell ambito della robotica mobile La versione continua del filtro di Kalman è analoga Raggiungibilità e Controllabilità-- 27 Filtro di Kalman Discreto Consente di stimare lo stato x di un sistema lineare discreto descritto che evolve dall istante k-1 all istante k secondo le seguenti equazioni x( = Ax( + Bu( + w( y( = Cx( + v( w( è un vettore aleatorio che rappresenta le incertezze di processo v( è un vettore aleatorio che rappresenta l incertezza sulla misura Raggiungibilità e Controllabilità-- 28 Pag. 14

Filtro di Kalman w( è un processo gaussian con media nulla e matrice di covarianza data, pari a Q( v( è un processo gaussiano con media nulla e matrice di covarianza data, pari a R( w( e v( sono variabili indipendenti à L incertezza sulla misura non dipende dall incertezza sul processo e viceversa. w( e v( codificano un puro rumore che contiene tutte le incertezze indeterministiche del sistema Le matrici Q( e R( si possono determinare facendo un modello statistico del processo o del sensore. Anche con stime iniziali grossolane, il filtro solitamente restituisce buoni risultati. Raggiungibilità e Controllabilità-- 29 Filtro di Kalman Lo stato x(0) è un vettore aleatorio gaussiano di media µ 0 e covarianza Σ 0. Lo stato x( e l uscita y( sono vettori aleatori gaussiani perché combinazione lineare di vettori aleatori gaussiani L idea alla base del filtro di Kalman è quella di sfruttare la conoscenza del modello e la descrizione probabilistica dell incertezza per decidere come combinare in maniera ottima l informazione del processo e l informazione dell incertezza L uscita del filtro è lo stato del filtro stesso, ovvero un vettore aleatorio con una certa media e una certa covarianza che rappresenta una stima statistica dello stato del sistema. Raggiungibilità e Controllabilità-- 30 Pag. 15

Filtro di Kalman - L algoritmo E ricorsivo Sono necessari solo lo stato stimato all istante precedente e la misura attuale per calcolare la stima dello stato attuale Consente di avere all istante k la stima dello stato x( e solitamente è tutto ciò che serve per pianificare il movimento di un robot o per controllare un sistema Lo stato del filtro è rappresentato da due variabili xˆ ( : Il valore atteso dello stato stimato Σ(: la matrice di covarianza di xˆ ( (una misura dell accuratezza della stima dello stato) Il filtro deve essere inizializzato e poi cicla tra due stadi: Predizione Aggiornamento Raggiungibilità e Controllabilità-- 31 Inizializzazione E necessario inizializzare lo stato del filtro con un vettore aleatorio gaussiano Come per gli osservatori di stato deterministici, quanto più vicino è lo stato iniziale del filtro allo stato reale, tanto più veloce sarà la convergenza del filtro allo stato del sistema Se si ha una stima approssimativa µ 0 dello stato iniziale del sistema, è bene inizializzare il filtro con tale stima e con una matrice di covarianza adeguata Se non si ha nessuna stima dello stato iniziale, si inizializzi il filtro con un valore casuale e una matrice di covarianza grande (quanto, dipende dall applicazione, in modo da segnalare al filtro che la stima attuale è assolutamente inaccurata). Raggiungibilità e Controllabilità-- 32 Pag. 16

L algoritmo (Notazione 1) Predizione Stato Predetto xˆ ( k = Axˆ( k 1 + Bu( Covarianza Predetta Aggiornamento Innovazione Covarianza Innovazione Guadagno di Kalman Aggiornamento dello stato Aggiornamento Covarianza Σ( k = AΣ( k 1 A T + Q( e( = y( Cxˆ( k S( = CΣ( k C T + R( T K( = Σ( k C S( 1 x ˆ( k = xˆ( k + K( e( Σ( k = ( I K( C) Σ( k Raggiungibilità e Controllabilità-- 33 L algoritmo (Notazione 2) Predizione Stato Predetto xˆ ( = Axˆ( + Bu( Covarianza Predetta Aggiornamento Innovazione Covarianza Innovazione Guadagno di Kalman Aggiornamento dello stato Aggiornamento Covarianza Σ( = AΣ( A T + Q( e( = y( Cxˆ( S( = CΣ( C T + R( T K( = Σ( C S( 1 x ˆ( = xˆ( + K( e( Σ ( = ( I K( C) Σ( Raggiungibilità e Controllabilità-- 34 Pag. 17

Il filtro di Kalman Predizione (1) Predizione dello stato xˆ ( = Axˆ( + Bu( (2) Predizione della covarianza Σ( = AΣ( A T + Q( Aggiornamento xˆ ( Σ( (1) Calcolo il guadagno di Kalman T T K( = Σ( C ( CΣ( C + R) (2) Aggiorno la stima con la misura x ˆ( = xˆ( + K( e( (3) Aggiorno la covariaza Σ ( = ( I K( C) Σ( 1 Raggiungibilità e Controllabilità-- 35 Esempio x( = 3 x( + 2 u( + w( A B y( = 4 x( + v( C Q = 0.1 R = 0.8 x( = 3 u( = 1 Σ( = 0.1 y( = 43.78 xˆ ( = Axˆ( + Bu( = 11 T Σ( = AΣ( A + Q( = 1 e( = y( Cxˆ( = 0.22 T S( = CΣ( C + R( = 16.8 K( = Σ( C T S( 1 = 0.23 xˆ ( = xˆ( + K( e( = 10.94 Σ( = ( I K( C) Σ( = 0.08 previsione la covarianza peggiora a causa dell incertezza di processo costruisco l innovazione usando l uscita L innovazione è molto incerta Il guadagno di Kalman è basso per non dare troppo peso all innovazione che introdurrebbe una grande incertezza La correzione allo stato predetto è minima La stima ottenuta ha poca incertezza grazie alla correzione tramite l innovazione Raggiungibilità e Controllabilità-- 36 Pag. 18

Illustrazione del funzionamento Un robot si muove lungo l asse orizzontale. Il robot conosce un modello di se stesso e riceve informazioni sulla sua posizione da un sensore (GPS, telecamera, ). Sia il modello che il sensore sono imprecisi e, pertanto, sia lo stato che la misura sono rappresentabili come variabili aleatorie gaussiane. Raggiungibilità e Controllabilità-- 37 Illustrazione del funzionamento La posizione del robot è una variabile aleatoria gaussiana. Il robot sa dove si trova (valor medio à picco della campana) con una certa imprecisione (varianza à ampiezza della campana) Il robot legge l uscita del sensore per sapere dove si trova e riceve una misura dal sensore che dice dove si trova con una certa imprecisione Il Filtro di Kalman fonde l informazione della posizione del robot con quella derivante dal sensore e ottiene una stima più precisa di dove si trova. Raggiungibilità e Controllabilità-- 38 Pag. 19

Illustrazione del funzionamento Se il robot si muove a destra, ottiene dal modello (predizione) una stima imprecisa della sua posizione a causa dell incertezza del modello Il robot può interrogare il sensore che trasmette una misura incerta (un altra gaussiana) della posizione del robot Integrando la stima ottenuta dal modello e la misura dello stato tramite il filtro di Kalman si ottiene una stima migliore della posizione del robot Raggiungibilità e Controllabilità-- 39 Commenti Il filtro di Kalman consente di trovare la stima ottima dello stato nel caso in cui le incertezze siano modellabili come gaussiane Aggiorna la stima facendo una media pesata sulle incertezze: Se la covarianza di processo è alta il guadagno di Kalman dà più peso all innovazione Se la covarianza di innovazione è alta, il guadagno dà più peso alla predizione Nel tempo continuo, il guadagno di Kalman si ricava in maniera analoga e può essere usato come guadagno in uno stimatore asintotico dello stato in catena chiusa per ottenere la stima ottima dello stato in caso di incertezze gaussiane sul processo e la misura Raggiungibilità e Controllabilità-- 40 Pag. 20

Esempio Si desidera stimare un valore costante (es.: una tensione) quando la sua misura è corrotta da un rumore con varianza 0.1 V Raggiungibilità e Controllabilità-- 41 Esempio E possibile modellare il processo come un sistema LTI con un incertezza x ( = Ax( + Bu( + w( x ( = x( + w( L uscita è caratterizzata da un incertezza: y ( = Cx( + w( y ( = x( + w( Lo stato non cambia e non c è nessun ingresso x( R A = 1 B = 0 Si misura l intero stato C =1 w( è una variabile gaussiana di media nulla e varianza 0.1 Raggiungibilità e Controllabilità-- 42 Pag. 21

Esempio Raggiungibilità e Controllabilità-- 43 Esempio Si consideri un robot olonomo completamente attuato e che si muove su un piano. y u x u y La posizione sul piano del robot è rilevata con un sensore GPS caratterizzato da una certa incertezza x Raggiungibilità e Controllabilità-- 44 Pag. 22

Esempio Il robot può essere modellato come un sistema LTI discreto, supponendo che la sua velocità non cambi significativamente tra due istanti di campionamento x( = x( + u( + w( y( = x( + v( x x = y w( N(0, Q( ) v( N(0, R( ) 0.05 Q( = Q = 0 2 0 R( = R = 0 2 0 0.05 u u = u Il modello è noto con una buona precisone La precisione del un GPS è di un paio di metri Raggiungibilità e Controllabilità-- 45 x y Osservazione Il robot parte da x=0 e y=1 ed è eccitato con un ingresso u x (=sin (0.01 u y (=cos(0.1] y ( y( e traiettoria reale L incertezza corrompe la misura e dall uscita non è possibile risalire accuratamente al movimento del robot. La precisione è limitata dall incertezza della misura. Raggiungibilità e Controllabilità-- 46 Pag. 23

Esempio Utilizzando il filtro di Kalman è possibile sfruttare la conoscenza del modello per costruire una stima dello stato più precisa di quella data dall uscita La stima è inizializzata a xˆ (0) = [ 0 1] xˆ ( 4 Σ(0) = 0 0 4 xˆ ( e traiettoria reale Raggiungibilità e Controllabilità-- 47 Esempio Il filtro parte da una stima molto incerta e man mano che il sistema evolve la stima diventa più accurata grazie alla fusione dei dati del modello con la misura. Raggiungibilità e Controllabilità-- 48 Pag. 24

Esempio Il sistema parte dallo stato iniziale [10;10] e si controlla il sistema con un feedback 0.5 0 u( = x( 0 0.5 per rendere il sistema controllato asintoticamente stabile e far convergere a 0 il movimento del sistema Raggiungibilità e Controllabilità-- 49 Esempio u ( = Ky( u ( = Kxˆ( Il rumore sull uscita peggiora notevolmente le prestazioni del controllore. Utilizzando il filtro di Kalman è possibile migliorare notevolmente le prestazioni del sistema controllato. Raggiungibilità e Controllabilità-- 50 Pag. 25

CONTROLLO DI SISTEMI ROBOTICI Laurea Specialistica in Ingegneria Meccatronica CONTROLLO DI ROBOT INDUSTRIALI IL FILTRO DI KALMAN Ing. Tel. 0522 522235 e-mail: secchi.cristian@unimore.it http://www.dismi.unimo.it/members/csecchi Pag. 26