CONTROLLO DI SISTEMI ROBOTICI Laurea Specialistica in Ingegneria Meccatronica CONTROLLO DI ROBOT INDUSTRIALI IL FILTRO DI KALMAN Ing. Cristian Secchi Tel. 0522 522235 e-mail: [email protected] 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. Cristian Secchi Cristian Secchi Raggiungibilità e Controllabilità-- 2 Pag. 1 Tecnologia e Incertezza x1 x2 m k u b y Incertezza di processo: attriti statici, dissipazione della molla, quantizzazione controllo Incertezza di misura: quantizzazione della misura (es.: encoder), rumore Cristian Secchi 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 Cristian Secchi Cristian Secchi Raggiungibilità e Controllabilità-- 4 Pag. 2 Esempio Raggiungibilità e Controllabilità-- 5 Cristian Secchi Esempio Se all’uscita e sul processo è sovrapposto un rumore di media 0.1 e varianza 0.5 y(t) La stima degrada! Cristian Secchi Cristian Secchi Raggiungibilità e Controllabilità-- 6 Pag. 3 Esempio: Localizzazione Modellando l’incertezza e sfruttando informazioni note è possibile gestire l’incertezza e ottenere informazioni molto sicure Cristian Secchi 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 Cristian Secchi Cristian Secchi 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 Cristian Secchi 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) = Cristian Secchi Cristian Secchi 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} Pr( A) = 2 1 = 6 3 Raggiungibilità e Controllabilità-- 11 Cristian Secchi Assiomi della probabilità Siano A e B due eventi: • 0 ≤ Pr( A) ≤ 1 • Pr( S ) = 1 • Pr( A ∨ B) = Pr( A) + Pr( B) − Pr( A ∧ B) True A Pr(not ( A)) = 1 − Pr( A) Pr(∅) = 0 A∧ B B Se A e B sono eventi disgiunti, allora Pr( A ∨ B) = Pr( A) + Pr( B) Cristian Secchi Cristian Secchi 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 {x1, x2, …, xn}. 2) P(X=xi), or P(xi), è la probabilità che la variabile aleatoria X assuma il valore xi 3) Esempio: l’esperimento lancio di un dado può essere rappresentato come una variabile aleatoria discreta X che può assumere i valori {x1, x2, …, x6} ={1,2,3,4,5,6} e P(xi)=1/6 per i=1,…,6 Raggiungibilità e Controllabilità-- 13 Cristian Secchi 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 [x1,x2]. 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 b Pr( x ∈ (a, b)) = ∫ p( x)dx p(x) a E’ più probabile essere vicini al centro della campana x Cristian Secchi Cristian Secchi 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 Cristian Secchi 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) Cristian Secchi Cristian Secchi 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 12 P( X = 1 | Y = 1) = 52 51 = 4 204 52 Raggiungibilità e Controllabilità-- 17 Cristian Secchi 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 è: n E[ X ] = ∑ xi P( xi ) i =1 • 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 {x1,x2}={1,2} con P(x1)=0.1 P(x2) =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 Cristian Secchi Cristian Secchi 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 Cristian Secchi Varianza • La varianza σ2 di un segnale attorno al suo valore atteso è data da 2 n 2 σ = E[( X − E ( X )) ] = ∑ ( xi − E[ X ])2 P( xi ) Tempo discreto i =1 " 2 ! = E[(X ! E[X]) ] = # (x ! E[X])2 p(x)dx • 2 !" La varianza rappresenta la dispersione di una variabile aleatoria attorno al suo valore atteso. Maggiore è la varianza, maggiore è la dispersione intorno al valore medio. Cristian Secchi Cristian Secchi Tempo continuo E[X]=1 E[X]=1 σ2 =1.41 σ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 Cristian Secchi Variabili Aleatorie Vettoriali • Un vettore aleatorio X =(x1,…,xn)T è un vettore le cui componenti sono variabili aleatorie • Il valore atteso di un vettore aleatorio X è dato da: ⎡ E[ x1 ] ⎤ E[ X ] = ⎢⎢ ⎥⎥ ⎢⎣ E[ xn ]⎥⎦ • La matrice di covarianza Σ di un vettore aleatorio X è data da Σ = E[( X − E[ X ])( X − E[ X ])T ] E’ la generalizzazione della varianza! Cristian Secchi Cristian Secchi 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 Cristian Secchi 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 − 1 f ( x) = e σ 2π ( x−µ )2 2σ 2 f(x) µ • • • • Si dimostra che: • • E[X]=µ E[(X-µ)2]=σ2 σ x 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 Cristian Secchi Cristian Secchi -σ 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 (2π ) n det(Σ) e 1 − ( x − µ )T Σ −1 ( x − µ ) 2 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 xTΣx=1 µ • Si dimostra che: • • E[X]=µ E[(X-µ) (X-µ)T]=Σ Cristian Secchi 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(µ, Σ) Cristian Secchi Cristian Secchi 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 Cristian Secchi 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(k ) = Ax(k − 1) + Bu (k − 1) + w(k ) ⎨ ⎩ y (k ) = Cx(k ) + v(k ) • w(k) è un vettore aleatorio che rappresenta le incertezze di processo • v(k) è un vettore aleatorio che rappresenta l’incertezza sulla misura Cristian Secchi Cristian Secchi Raggiungibilità e Controllabilità-- 28 Pag. 14 Filtro di Kalman • w(k) è un processo gaussian con media nulla e matrice di covarianza data, pari a Q(k) • v(k) è un processo gaussiano con media nulla e matrice di covarianza data, pari a R(k) • w(k) e v(k) sono variabili indipendenti à L’incertezza sulla misura non dipende dall’incertezza sul processo e viceversa. • w(k) e v(k) codificano un puro rumore che contiene tutte le incertezze indeterministiche del sistema • Le matrici Q(k) e R(k) si possono determinare facendo un modello statistico del processo o del sensore. Anche con stime iniziali grossolane, il filtro solitamente restituisce buoni risultati. Cristian Secchi Raggiungibilità e Controllabilità-- 29 Filtro di Kalman • Lo stato x(0) è un vettore aleatorio gaussiano di media µ0 e covarianza Σ0. • Lo stato x(k) e l’uscita y(k) 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. Cristian Secchi Cristian Secchi 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(k) 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ˆ (k ): Il valore atteso dello stato stimato • Σ(k): la matrice di covarianza di xˆ (k ) (una misura dell’accuratezza della stima dello stato) Il filtro deve essere inizializzato e poi cicla tra due stadi: • Predizione • Aggiornamento Cristian Secchi 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). Cristian Secchi Cristian Secchi Raggiungibilità e Controllabilità-- 32 Pag. 16 L’algoritmo (Notazione 1) • Predizione • Stato Predetto • Covarianza Predetta • xˆ (k | k − 1) = Axˆ (k − 1 | k − 1) + Bu (k − 1) Σ(k | k − 1) = AΣ(k − 1 | k − 1) AT + Q(k ) Aggiornamento • Innovazione • Covarianza Innovazione e(k ) = y (k ) − Cxˆ (k | k − 1) S (k ) = CΣ(k | k − 1)C T + R(k ) K (k ) = Σ(k | k − 1)C T S (k ) −1 • Guadagno di Kalman • Aggiornamento dello stato • Aggiornamento Covarianza xˆ (k | k ) = xˆ (k | k − 1) + K (k )e(k ) Σ(k | k ) = ( I − K (k )C )Σ(k | k − 1) Raggiungibilità e Controllabilità-- 33 Cristian Secchi L’algoritmo (Notazione 2) • Predizione • Stato Predetto • Covarianza Predetta • Aggiornamento • Innovazione • Covarianza Innovazione • Guadagno di Kalman • Aggiornamento dello stato • Aggiornamento Covarianza Cristian Secchi Cristian Secchi xˆ (k ) − = Axˆ (k − 1) + Bu (k − 1) Σ(k ) − = AΣ(k − 1) AT + Q(k ) e(k ) = y(k ) − Cxˆ (k ) − S (k ) = CΣ(k ) − C T + R(k ) K (k ) = Σ(k ) − C T S (k ) −1 xˆ (k ) = xˆ (k ) − + K (k )e(k ) Σ(k ) = ( I − K (k )C )Σ(k ) − Raggiungibilità e Controllabilità-- 34 Pag. 17 Il filtro di Kalman Predizione (1) Predizione dello stato xˆ (k ) − = Axˆ (k − 1) + Bu (k − 1) (2) Predizione della covarianza Σ(k ) − = AΣ(k − 1) AT + Q(k ) Aggiornamento (1) Calcolo il guadagno di Kalman K (k ) = Σ(k ) − C T (CΣ(k ) − C T + R) −1 (2) Aggiorno la stima con la misura xˆ (k ) = xˆ (k ) − + K (k )e(k ) (3) Aggiorno la covariaza xˆ (k − 1) Σ(k − 1) Σ(k ) = ( I − K (k )C )Σ(k ) − Raggiungibilità e Controllabilità-- 35 Cristian Secchi Esempio ⎧ x(k ) = 3 x(k − 1) + 2 u (k − 1) + w(k ) ⎪ B A ⎨ y ( k ) = 4 x ( k ) + v ( k ) ⎪ C ⎩ Q = 0 .1 R = 0 .8 x(k − 1) = 3 u (k − 1) = 1 Σ(k − 1) = 0.1 y (k ) = 43.78 xˆ (k ) − = Axˆ (k − 1) + Bu (k − 1) = 11 Σ(k ) − = AΣ(k − 1) AT + Q(k ) = 1 e(k ) = y(k ) − Cxˆ (k ) − = −0.22 la covarianza peggiora a causa dell’incertezza di processo S (k ) = CΣ(k ) − C T + R(k ) = 16.8 L’innovazione è molto incerta K (k ) = Σ(k ) − C T S (k ) −1 = 0.23 xˆ (k ) = xˆ (k ) − + K (k )e(k ) = 10.94 Il guadagno di Kalman è basso per non dare troppo peso all’innovazione che introdurrebbe una grande incertezza Σ(k ) = ( I − K (k )C )Σ(k ) = 0.08 − Cristian Secchi Cristian Secchi previsione costruisco l’innovazione usando l’uscita 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. Cristian Secchi 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. Cristian Secchi Cristian Secchi 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 Cristian Secchi 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 Cristian Secchi Cristian Secchi 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 Cristian Secchi Raggiungibilità e Controllabilità-- 41 Esempio E’ possibile modellare il processo come un sistema LTI con un’incertezza x(k ) = Ax(k − 1) + Bu (k − 1) + w(k ) x(k ) = x(k − 1) + w(k ) Lo stato non cambia e non c’è nessun ingresso x( k ) ∈ R A = 1 B = 0 L’uscita è caratterizzata da un’incertezza: y(k ) = Cx(k ) + w(k ) y (k ) = x(k ) + w(k ) Si misura l’intero stato C =1 w(k) è una variabile gaussiana di media nulla e varianza 0.1 Cristian Secchi Cristian Secchi Raggiungibilità e Controllabilità-- 42 Pag. 21 Esempio Raggiungibilità e Controllabilità-- 43 Cristian Secchi Esempio Si consideri un robot olonomo completamente attuato e che si muove su un piano. y ux uy x La posizione sul piano del robot è rilevata con un sensore GPS caratterizzato da una certa incertezza Cristian Secchi Cristian Secchi 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(k ) = x(k − 1) + u (k − 1) + w(k ) ⎨ ⎩ y (k ) = x(k ) + v(k ) ⎛ x ⎞ ⎛ u ⎞ x = ⎜⎜ ⎟⎟ u = ⎜ x ⎟ ⎜ u ⎟ ⎝ y ⎠ ⎝ y ⎠ w(k ) ∈ N (0, Q(k )) v(k ) ∈ N (0, R(k )) 0 ⎞ ⎛ 0.05 Il modello è noto con ⎟⎟ Q(k ) = Q = ⎜⎜ una buona precisone 0.05 ⎠ ⎝ 0 ⎛ 2 0 ⎞ La precisione del un GPS è di ⎟⎟ R(k ) = R = ⎜⎜ un paio di metri ⎝ 0 2 ⎠ Cristian Secchi Raggiungibilità e Controllabilità-- 45 Osservazione • Il robot parte da x=0 e y=1 ed è eccitato con un ingresso ux(k)=sin (0.01k) uy(k)=cos(0.1k)] y (k ) y (k ) 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. Cristian Secchi Cristian Secchi 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ˆ ( k ) Cristian Secchi ⎡4 0⎤ Σ(0) = ⎢ ⎥ ⎣0 4⎦ xˆ ( k ) 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. Cristian Secchi Cristian Secchi Raggiungibilità e Controllabilità-- 48 Pag. 24 Esempio • Il sistema parte dallo stato iniziale [10;10] e si controlla il sistema con un feedback 0 ⎞ ⎛ − 0.5 ⎟⎟ x(k ) u (k ) = ⎜⎜ 0 − 0 . 5 ⎝ ⎠ per rendere il sistema controllato asintoticamente stabile e far convergere a 0 il movimento del sistema Cristian Secchi Raggiungibilità e Controllabilità-- 49 Esempio u (k ) = Ky (k ) u (k ) = Kxˆ (k ) Il rumore sull’uscita peggiora notevolmente le prestazioni del controllore. Utilizzando il filtro di Kalman è possibile migliorare notevolmente le prestazioni del sistema controllato. Cristian Secchi Cristian Secchi Raggiungibilità e Controllabilità-- 50 Pag. 25 CONTROLLO DI SISTEMI ROBOTICI Laurea Specialistica in Ingegneria Meccatronica CONTROLLO DI ROBOT INDUSTRIALI IL FILTRO DI KALMAN Ing. Cristian Secchi Tel. 0522 522235 e-mail: [email protected] http://www.dismi.unimo.it/Members/csecchi Cristian Secchi Pag. 26