Cristian Secchi Pag. 1
Ing. Cristian Secchi Tel. 0522 522235
e-mail: [email protected] http://www.dismi.unimo.it/Members/csecchi
CONTROLLO DI SISTEMI ROBOTICI Laurea Specialistica in Ingegneria Meccatronica
CONTROLLO DI ROBOT INDUSTRIALI IL FILTRO DI KALMAN
Raggiungibilità e Controllabilità-- 2
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
Cristian Secchi
Tutti questi rumori danno luogo sia a un’incertezza di misura che a un’incertezza di processo.
Cristian Secchi Pag. 2
Raggiungibilità e Controllabilità-- 3
Tecnologia e Incertezza
Cristian Secchi
k m
x1 x2 u
y
b
Incertezza di processo: attriti statici, dissipazione della molla, quantizzazione controllo
Incertezza di misura: quantizzazione della misura (es.: encoder), rumore
Raggiungibilità e Controllabilità-- 4
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.: ) viene integrato dal modello del sistema e, di conseguenza, i rumori vengono integrati e l’incertezza sulla stima si accumula
Cristian Secchi
xCy ˆ−
Cristian Secchi Pag. 3
Raggiungibilità e Controllabilità-- 5
Esempio
Cristian Secchi
Raggiungibilità e Controllabilità-- 6
Esempio
Cristian Secchi
Se all’uscita e sul processo è sovrapposto un rumore di media 0.1 e varianza 0.5
y(t)
La stima degrada!
Cristian Secchi Pag. 4
Raggiungibilità e Controllabilità-- 7
Esempio: Localizzazione
Cristian Secchi
Modellando l’incertezza e sfruttando informazioni note è possibile gestire l’incertezza e ottenere informazioni molto sicure
Raggiungibilità e Controllabilità-- 8
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
Cristian Secchi
Rudolph Emil Kalman ha ricevuto la National Medal of Science da Barack Obama nel 2010
Cristian Secchi Pag. 5
Raggiungibilità e Controllabilità-- 9
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
Cristian Secchi
Raggiungibilità e Controllabilità-- 10
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:
Cristian Secchi
possibilirisultatideiNumeroAinrisultatideinumeroA =)Pr(
Cristian Secchi Pag. 6
Raggiungibilità e Controllabilità-- 11
Cenni di Probabilità
• Lanciando un dado non truccato con 6 facce, qual è la probabilità che esca 1 o 2?
Cristian Secchi
}2,1{=A
}6,5,4,3,2,1{=S
31
62)Pr( ==A
Raggiungibilità e Controllabilità-- 12
Assiomi della probabilità
Cristian Secchi
Siano A e B due eventi:
•
•
•
1)Pr(0 ≤≤ A
1)Pr( =S
)Pr()Pr()Pr()Pr( BABABA ∧−+=∨
0)Pr( =∅
BA∧A BTrue
)Pr(1))(Pr( AAnot −=
Se A e B sono eventi disgiunti, allora
)Pr()Pr()Pr( BABA +=∨
Cristian Secchi Pag. 7
Raggiungibilità e Controllabilità-- 13
Variabili Aleatorie Discrete
Cristian Secchi
• 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à-- 14
Variabili Aleatorie Continue
Cristian Secchi
• 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
a
dxxpbax )()),(Pr(
x
p(x) E’ più probabile essere vicini al centro della campana
Cristian Secchi Pag. 8
Raggiungibilità e Controllabilità-- 15
La somma delle probabilità è 1
Cristian Secchi
∑ =x
xP 1)(
Variabili Aleatorie Discrete
∫ =1)( dxxp
Variabili Aleatorie Continue
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à-- 16
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 Pag. 9
Raggiungibilità e Controllabilità-- 17
Esempio
• La probabilità un giocatore estragga un asso dopo che l’altro ha estratto un asso è data da:
Cristian Secchi
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.
20412
524513
524
)1|1( ==== YXP
Raggiungibilità e Controllabilità-- 18
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] 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
∑=
=n
iii xPxXE
1
)(][
Cristian Secchi Pag. 10
Raggiungibilità e Controllabilità-- 19
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
Cristian Secchi
∫∞
∞−
= dxxxpXE )(][
Raggiungibilità e Controllabilità-- 20
Varianza
• La varianza σ2 di un segnale attorno al suo valore atteso è data da
• 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
! 2 = E[(X !E[X])2 ]= (x !E[X])2 p(x)dx!"
"
#
∑=
−=−=n
iii xPXExXEXE
1
222 )(])[(]))([(σ Tempo discreto
Tempo continuo
E[X]=1
σ2 =1.41
E[X]=1
σ2 =0.32
Cristian Secchi Pag. 11
Raggiungibilità e Controllabilità-- 21
Variabili Incorrelate
• Due variabili aleatorie X e Y si dicono incorrelate, se
• 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
Cristian Secchi
][][][ YEXEXYE =
Raggiungibilità e Controllabilità-- 22
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:
• La matrice di covarianza Σ di un vettore aleatorio X è data da
Cristian Secchi
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
=
][
][][
1
nxE
xEXE
]])[])([[( TXEXXEXE −−=Σ
E’ la generalizzazione della varianza!
Cristian Secchi Pag. 12
Raggiungibilità e Controllabilità-- 23
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
Cristian Secchi
Raggiungibilità e Controllabilità-- 24
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
• 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
Cristian Secchi
2
2
2)(
21)( σ
µ
πσ
−−
=x
exf
-σ σ
µ
f(x)
x
Cristian Secchi Pag. 13
Raggiungibilità e Controllabilità-- 25
Variabili Aleatorie Gaussiane
• Un vettore aleatorio si dice gaussiano (o normale) se la sua densità di probabilità è del tipo
• 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
)()(21 1
)det()2(1)(
µµ
π
−Σ−− −
Σ=
xx
n
T
exf
µ
Raggiungibilità e Controllabilità-- 26
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 Pag. 14
Raggiungibilità e Controllabilità-- 27
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à-- 28
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
• w(k) è un vettore aleatorio che rappresenta le incertezze di processo
• v(k) è un vettore aleatorio che rappresenta l’incertezza sulla misura
Cristian Secchi
⎩⎨⎧
+=
+−+−=
)()()()()1()1()(
kvkCxkykwkBukAxkx
Cristian Secchi Pag. 15
Raggiungibilità e Controllabilità-- 29
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à-- 30
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 Pag. 16
Raggiungibilità e Controllabilità-- 31
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 • : Il valore atteso dello stato stimato • Σ(k): la matrice di covarianza di (una misura
dell’accuratezza della stima dello stato) • Il filtro deve essere inizializzato e poi cicla tra due stadi:
• Predizione • Aggiornamento
Cristian Secchi
)(ˆ kx)(ˆ kx
Raggiungibilità e Controllabilità-- 32
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 Pag. 17
Raggiungibilità e Controllabilità-- 33
L’algoritmo (Notazione 1)
Cristian Secchi
• Predizione • Stato Predetto
• Covarianza Predetta
• Aggiornamento • Innovazione
• Covarianza Innovazione
• Guadagno di Kalman
• Aggiornamento dello stato
• Aggiornamento Covarianza
)1()1|1(ˆ)1|(ˆ −+−−=− kBukkxAkkx
)()1|1()1|( kQAkkAkk T +−−Σ=−Σ
)1|(ˆ)()( −−= kkxCkyke
)()1|()( kRCkkCkS T +−Σ=
1)()1|()( −−Σ= kSCkkkK T
)()()1|(ˆ)|(ˆ kekKkkxkkx +−=
)1|())(()|( −Σ−=Σ kkCkKIkk
Raggiungibilità e Controllabilità-- 34
L’algoritmo (Notazione 2)
Cristian Secchi
• Predizione • Stato Predetto
• Covarianza Predetta
• Aggiornamento • Innovazione
• Covarianza Innovazione
• Guadagno di Kalman
• Aggiornamento dello stato
• Aggiornamento Covarianza
)1()1(ˆ)(ˆ −+−=− kBukxAkx
)()1()( kQAkAk T +−Σ=Σ −
−−= )(ˆ)()( kxCkyke
)()()( kRCkCkS T +Σ= −
1)()()( −−Σ= kSCkkK T
)()()(ˆ)(ˆ kekKkxkx += −
−Σ−=Σ )())(()( kCkKIk
Cristian Secchi Pag. 18
Raggiungibilità e Controllabilità-- 35
Il filtro di Kalman
Cristian Secchi
(1) Predizione dello stato
(2) Predizione della covarianza )1()1(ˆ)(ˆ −+−=− kBukxAkx
)()1()( kQAkAk T +−Σ=Σ −
Predizione
(1) Calcolo il guadagno di Kalman
(2) Aggiorno la stima con la misura
(3) Aggiorno la covariaza
1))(()()( −−− +ΣΣ= RCkCCkkK TT
)()()(ˆ)(ˆ kekKkxkx += −
−Σ−=Σ )())(()( kCkKIk
Aggiornamento
)1()1(ˆ −Σ− kkx
Raggiungibilità e Controllabilità-- 36
Esempio
Cristian Secchi
⎪⎩
⎪⎨
⎧
+=
+−+−=
)()(4)(
)()1(2)1(3)(
kvkxky
kwkukxkx
C
BA8.01.0
=
=
RQ
78.43)(1.0)1(1)1(3)1( ==−Σ=−=− kykkukx
11)1()1(ˆ)(ˆ =−+−=− kBukxAkx1)()1()( =+−Σ=Σ − kQAkAk T
22.0)(ˆ)()( −=−= −kxCkyke8.16)()()( =+Σ= − kRCkCkS T
23.0)()()( 1 =Σ= −− kSCkkK T
94.10)()()(ˆ)(ˆ =+= − kekKkxkx08.0)())(()( =Σ−=Σ −kCkKIk
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
Cristian Secchi Pag. 19
Raggiungibilità e Controllabilità-- 37
Illustrazione del funzionamento
Cristian Secchi
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à-- 38
Illustrazione del funzionamento
Cristian Secchi
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 Pag. 20
Raggiungibilità e Controllabilità-- 39
Illustrazione del funzionamento
Cristian Secchi
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à-- 40
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 Pag. 21
Raggiungibilità e Controllabilità-- 41
Esempio
Cristian Secchi
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à-- 42
Esempio
Cristian Secchi
E’ possibile modellare il processo come un sistema LTI con un’incertezza
)()1()1()( kwkBukAxkx +−+−=
)()1()( kwkxkx +−=01)( ==∈ BARkx
Lo stato non cambia e non c’è nessun ingresso
L’uscita è caratterizzata da un’incertezza:
)()()( kwkCxky +=
1=C)()()( kwkxky +=Si misura l’intero stato
w(k) è una variabile gaussiana di media nulla e varianza 0.1
Cristian Secchi Pag. 22
Raggiungibilità e Controllabilità-- 43
Esempio
Cristian Secchi
Raggiungibilità e Controllabilità-- 44
Esempio
Cristian Secchi
Si consideri un robot olonomo completamente attuato e che si muove su un piano.
x
y
ux
uy
La posizione sul piano del robot è rilevata con un sensore GPS caratterizzato da una certa incertezza
Cristian Secchi Pag. 23
Raggiungibilità e Controllabilità-- 45
Esempio
Cristian Secchi
Il robot può essere modellato come un sistema LTI discreto, supponendo che la sua velocità non cambi significativamente tra due istanti di campionamento
⎩⎨⎧
+=
+−+−=
)()()()()1()1()(
kvkxkykwkukxkx
⎟⎟⎠
⎞⎜⎜⎝
⎛=yx
x ⎟⎟⎠
⎞⎜⎜⎝
⎛=
y
x
uu
u
))(,0()( kQNkw ∈ ))(,0()( kRNkv ∈
⎟⎟⎠
⎞⎜⎜⎝
⎛==
05.00005.0
)( QkQ
⎟⎟⎠
⎞⎜⎜⎝
⎛==
2002
)( RkR
Il modello è noto con una buona precisone
La precisione del un GPS è di un paio di metri
Raggiungibilità e Controllabilità-- 46
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)]
Cristian Secchi
L’incertezza corrompe la misura e dall’uscita non è possibile risalire accuratamente al movimento del robot. La precisione è limitata dall’incertezza della misura.
)(ky )(ky e traiettoria reale
Cristian Secchi Pag. 24
Raggiungibilità e Controllabilità-- 47
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
Cristian Secchi
[ ]10)0(ˆ =x ⎥⎦
⎤⎢⎣
⎡=Σ
4004
)0(
)(ˆ kx e traiettoria reale )(ˆ kx
Raggiungibilità e Controllabilità-- 48
Esempio
Cristian Secchi
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 Pag. 25
Raggiungibilità e Controllabilità-- 49
Esempio
• Il sistema parte dallo stato iniziale [10;10] e si controlla il sistema con un feedback
per rendere il sistema controllato asintoticamente stabile e far convergere a 0 il movimento del sistema
Cristian Secchi
)(5.0005.0
)( kxku ⎟⎟⎠
⎞⎜⎜⎝
⎛
−
−=
Raggiungibilità e Controllabilità-- 50
Esempio
Cristian Secchi
)()( kKyku = )(ˆ)( kxKku =
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 Pag. 26
Ing. Cristian Secchi Tel. 0522 522235
e-mail: [email protected] http://www.dismi.unimo.it/Members/csecchi
CONTROLLO DI SISTEMI ROBOTICI Laurea Specialistica in Ingegneria Meccatronica
CONTROLLO DI ROBOT INDUSTRIALI IL FILTRO DI KALMAN
Top Related