Post on 19-Jan-2020
transcript
Cristian Secchi Pag. 1
CONTROLLO DI ROBOT INDUSTRIALILaurea Magistrale in Ingegneria Meccatronica
ROBOTICA MOBILEROBOTICA MOBILE
Ing. Cristian SecchiTel. 0522 522235
e-mail: secchi.cristian@unimore.ithttp://www.dismi.unimo.it/Members/csecchi
Dalla Robotica Fissa a quella mobileDalla Robotica Fissa a quella mobile• A partire dagli anni ’60, compaiono i primi robot industriali• Utilizzati per sostituire l’uomo in compiti pericolosi o semplici e
ripetitivi
Controllo di Robot -- 2
• Si trattava di compiti da svolgere in loco• Gli ambienti di lavoro iniziarono ad essere modellati in base alle
esigenze di controllo dei robotCristian Secchi
Cristian Secchi Pag. 2
Dalla Robotica Fissa a quella mobileDalla Robotica Fissa a quella mobile
Robotica Mobile
Imita la locomozione degli animali e le relative funzionalità
Robotica Mobile
Imita la funzionalità degli arti umani superiori
Controllo di Robot -- 3Cristian Secchi
Definizione:Un robot mobile è una macchina automatica in grado di muoversi nell’ambiente che la circonda.
Possibili ApplicazioniPossibili Applicazioni
Applicazioni Indoor• Pulizia di ambienti estesi• Robotica di servizio per musei, negozi,
ecc...ecc...• Sorveglianza interna di edifici• Stoccaggio merci in magazzini
automatizzati• ...
Applicazioni Outdoor• Applicazioni militari
Controllo di Robot -- 4Cristian Secchi
pp• Sminamento• Esplorazione spaziale e subacquea• Protezione Civile e sorveglianza boschiva• Agricoltura automatizzata• …
Cristian Secchi Pag. 3
Cenni storiciCenni storici
• Primi robot mobili a partire dal 1940• Razzi V1 - V2 (rozzo autopilota
inerziale)
Teletank
Goliath
Controllo di Robot -- 5Cristian Secchi
• Filoguidato• Motori Elettrici
• Carro telecontrollato• 500 m di distanza
Uso industriale: Uso industriale: AutomaticAutomatic GuidedGuided VehiclesVehicles (AGV)(AGV)
Usato per la gestione di fine linea di produzione. Porta i pallet di prodotto stoccato dal magazzino alle baie di carico camion.In un impianto possono circolare più di 30 AGV contemporaneamente.
Controllo di Robot -- 6Cristian Secchi
Elettric80 AGV
Cristian Secchi Pag. 4
EsempiEsempi
Uso Ospedaliero: HELPMATE
Usato negli ospedali per trasporto dimedicinali e di logistica
Si muove autonomamente
Naviga tramite una telecamera e usa come riferimento le lampade sul soffitto
Domestico: Roomba
A i l li i i
Controllo di Robot -- 7Cristian Secchi
Aspirapolvere per applicazionidomestiche
Grazie ad algoritmi di path planningpulisce la stanza senza bisogno diimpostare mappe
EsempiEsempi
Robot Sottomarini• Utilizzati per archeologia sottomarina• Recupero relitti (Titanic)• Ispezione struttureIspezione strutture• Monitoraggio acque
Applicazione Spaziali• R li ti ti d l 1965
Controllo di Robot -- 8Cristian Secchi
• Realizzati a partire dal 1965• Altamente autonomi• In grado di lavorare senza
supervisione• Dotati di alta ridondanza dei sensori
Cristian Secchi Pag. 5
EsempiEsempi
Robot Agricoli (attesi circa per il 2015)
Robot mobili concepiti per i i i l
Controllo di Robot -- 9
operazioni agricole:
• Ambienti strutturati (frutteti ad hoc)
• vasti campi di cereali
Cristian Secchi
EsempiEsempi
Robot Pioneer
• Sviluppato dalla Stanford University
Controllo di Robot -- 10Cristian Secchi
• Sviluppato dalla Stanford University• Scelto per esplorare il sarcofago di Chernobyl• Robot Modulare. Consente di montare:
• braccio robotico• telecamera• scanner laser
Cristian Secchi Pag. 6
Principi di LocomozionePrincipi di Locomozione
• I concetti che si trovano in natura sono difficili da riprodurre ingegneristicamente
• La maggior parte dei sistemi ingegneristici usano le ruote o i cingoli
• Il rotolamento è il principio di locomozione più efficiente, ma non si trova in natura (la natura non ha mai inventato la ruota!)
• Tuttavia, la camminata di un bipede è vicina al rotolamento
Controllo di Robot -- 11Cristian Secchi
Principi di LocomozionePrincipi di Locomozione
Camminata Bipede• Approssima il rotolamento• Rotolamento di un poligono con lato
pari al passo della personap p p• Più il passo è corto, meglio il
rotolamento viene approssimato
I robot che camminano• si possono adattare meglio a un
ambiente variabile• iù l ti fi i i tt
Controllo di Robot -- 12Cristian Secchi
• sono più lenti su superficie piatte • sono fortemente sovra-attuatiI robot con ruote• sono più veloci su superfici piatte• sono poco adattabili
Cristian Secchi Pag. 7
Principi di LocomozionePrincipi di Locomozione
Controllo di Robot -- 13Cristian Secchi
LocomotionLocomotion!!
Controllo di Robot -- 14Cristian Secchi
Cristian Secchi Pag. 8
Robot con Ruote (Robot con Ruote (WheeledWheeled Mobile Mobile RobotsRobots -- WMR)WMR)
• Le ruote sono la soluzione più appropriata per molte applicazioni
• Tre ruote sono sufficienti per garantire la stabilità del robot
• Quante/Quali ruote usare e come disporle dipende dall’applicazione
Controllo di Robot -- 15Cristian Secchi
Tipi di ruoteTipi di ruote
Ruota Fissa Ruota Centrata Orientabile
Ruota Scentrata e Orientabile(Ruota Castor) Ruota Svedese:proprietà
omnidirezionali
Controllo di Robot -- 16Cristian Secchi
Cristian Secchi Pag. 9
SwedishSwedish WheelsWheels
• Si basano sull’idea di poter sfruttare l’attrito anche trasversalmente alla direzione di moto della ruota
Controllo di Robot -- 17Cristian Secchi
Possibili ConfigurazioniPossibili Configurazioni
Due Ruote
Tre Ruote
Controllo di Robot -- 18Cristian Secchi
Cristian Secchi Pag. 10
(Lego(Lego--))SegwaySegway
Controllo di Robot -- 19Cristian Secchi
Possibili ConfigurazioniPossibili Configurazioni
• Quattro Ruote
• Sei Ruote
Controllo di Robot -- 20Cristian Secchi
Cristian Secchi Pag. 11
Esempio 6 ruote: NASA Esempio 6 ruote: NASA AthleteAthlete
Controllo di Robot -- 21Cristian Secchi
SpazioSpazio delledelle ConfigurazioniConfigurazioni
• E’ caratterizzato dal numero di parametri che servono a localizzare la configurazione del robot
• E’ l’analogo dello spazio di giunto per un robot mobile
• Dipende dalla struttura del robot mobile
Esempio: Uniciclo Esempio: Biciclo
Controllo di Robot -- 22Cristian Secchi
Cristian Secchi Pag. 12
VincoliVincoli
• Si dice vincolo una qualunque condizione imposta ad un sistema materiale che impedisce di assumere una generica posizione e/o atto di moto
• Un sistema materiale si dice soggetto a vincoli olonomi se tra le coordinate del sistema esistono dei legami espressi da relazioni finite (vincoli di posizione) oppure se tra le coordinate del sistema esistono dei legami espressi da relazioni differenziabili integrabili finite.
• Un vincolo si dice anolonomo se la relazione differenziale tra le
Controllo di Robot -- 23
• Un vincolo si dice anolonomo se la relazione differenziale tra le coordinate non è riducibile a forma finita.
Cristian Secchi
VincoliVincoli AnolonomiAnolonomi
• Ipotesi semplificativa: Ogni ruota rotola senza strisciamento (cioè senza slittare nè longitudinalmente nè lateralmente)
• Ogni ruota introduce un vincolo anolonomo nel sistema in quanto non consente una traslazione normale alla direzione di rotolamento
• Le ruote limitano la mobilità istantanea del robot senza però, in genere, precludere la possibilità di raggiungere configurazioni arbitrari (es.: Parcheggio parallelo di un automobile).
Controllo di Robot -- 24Cristian Secchi
Cristian Secchi Pag. 13
Vincoli Vincoli AnolonomiAnolonomi
⎪⎨
⎧ −+= )2
cos(cos θπθ nt vvx&θ
⎪⎩
⎨−+= )
2sin(sin θπθ nt vvy&
θ
Il disco rotola senza strisciare sul piano, la velocità normale al verso di rotolamento è nulla: vn=0
⎧
Controllo di Robot -- 25Cristian Secchi
⎩⎨⎧
==
θθ
sincos
t
t
vyvx
&
&
dxdy
=θtan θθ cossin dydx =
0cossin =− θθ yx && Vincolo sulla mobilità
VincoliVincoli AnolonomiAnolonomi
• Per ogni ruota è possibile scrivere il vincolo anolonomo in termini di coordinate generalizzate q come
0)( =qqa &
• Per N ruote i vincoli possono essere riscritti come
• I vincoli anolonomi non comportano una riduzione dello spazio delle
0)( =qqA & Vincolo Pfaffiano
Controllo di Robot -- 26
I vincoli anolonomi non comportano una riduzione dello spazio delle configurazioni ma comportano un riduzione della mobilitàistantanea del robot
Cristian Secchi
Cristian Secchi Pag. 14
VincoliVincoli AnolonomiAnolonomi
0)( =qqA & Limitazione della velocità
)(ker qAq∈&
Le velocità ammissibili possono essere generate da una matrice G(q) tale che:
Cq Ker(A(q))Im(G(q)) ∈∀=
Controllo di Robot -- 27Cristian Secchi
vqGq )(=&
Modello cinematico tipico di un robot mobile con vincoli anolonomi
Ingresso cinematico
ModelloModello cinematicocinematico didi un WMRun WMR
vqGq )(=&
• Rappresenta le direzioni di moto ammissibili nello spazio delle configurazioni
• Lega gli ingressi in velocità nello spazio operativo con le derivate delle configurazioni
• E’ necessario per affrontare le problematiche principali della robotica mobile• Pianificazioni di traiettoria
Controllo di Robot -- 28
• Pianificazioni di traiettoria• Controllo• Localizzazione• ….
Cristian Secchi
Cristian Secchi Pag. 15
Modello cinematico dell’Modello cinematico dell’UnicicloUniciclo
Un uniciclo è un veicolo avente una sola ruota orientabile
⎟⎟⎟⎞
⎜⎜⎜⎛
= yx
q⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
0sincos
θθ
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
100 • Configurazione:
x
y θ
⎟⎠
⎜⎝θ
0]0,cos,[sincossin =−=− qyx &&& θθθθ
]0,cos,[sin)( θθ −=qA
⎦⎣⎥⎦⎢⎣1
• Vincolo:
• In forma Pfaffiana: 0)( =qqA &
Controllo di Robot -- 29Cristian Secchi
],,[)(q
))(Im()100
,0
sincos
())(( qGspanqAKer =⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡= θ
θ
In forma Pfaffiana:
• modello cinematico:
0)( =qqA
⎟⎟⎟
⎠
⎞
⎜⎜⎜
⎝
⎛=
100sin0cos
)( θθ
qG
Modello Cinematico dell’Modello Cinematico dell’UnicicloUniciclo
⎥⎦
⎤⎢⎣
⎡
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡+
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
ωθθ
ωθθ
vvq
100
0sincos
100
0sincos
&
Modello cinematico dell’uniciclo
⎪
⎪⎨
⎧
==
θθ
&
&
sincos
vyvx
⎥⎦⎢⎣⎥⎦⎢⎣⎥⎦⎢⎣
Gli ingressi v e ω hanno un chiaro significato fisico:
1. v è la velocità lineare del punto di contatto tra la ruota e il suolo ed è pari al prodotto tra velocità di rotazione della ruota attorno al suo asse orizzontale e il raggio della stessa
⎪⎩ =ωθ&
Controllo di Robot -- 30Cristian Secchi
orizzontale e il raggio della stessa2. ω è la velocità angolare del robot, pari alla velocità di rotazione della
ruota intorno all’asse verticale
Agendo su v e ω è possibile variare la configurazione del robot
Cristian Secchi Pag. 16
Modello cinematico dell’Modello cinematico dell’UnicicloUniciclo
• L’uniciclo propriamente detto ha gravi problemi di bilanciamento in condizioni statiche
• In pratica si ricorre a strutture cinematicamente equivalenti ma più stabili da un punto di vista meccanico. Le strutture più utilizzate p psono l’uniciclo a trazione differenziale (differential drive) e quello a trazione sincronizzata
Controllo di Robot -- 31Cristian Secchi
UnicicloUniciclo differentialdifferential drivedrive
• Ha due ruote fisse coassiali e attuate indipendentemente e una ruota castor (solitamente di dimensioni ridotte) il cui scopo è di mantenere il robot in equilibrio statico
• q=[x,y,θ]. (x,y) sono le coordinate del punto medio dell’asse delle ruote e θ è l’orientamento del robot
• La ruota castor è passiva e le due ruote sono attuate indipendentemente. Il robot trasla se ωR=ωL e ruota sul posto se ωR=-ωL
Controllo di Robot -- 32
su pos o se ωR ωL
• E’ la realizzazione più diffusa dell’unicicloKephera III
Pioneer
Sentinel
Cristian Secchi Pag. 17
UnicicloUniciclo differentialdifferential drivedrive
La velocità del punto medio dell’asse delle ruote v e la velocità angolare del robot ω sono legate in maniera biunivoca alle velocità angolare delle ruote dalla seguente relazione:
⎪⎪⎧ +
=rv LR )( ωω ωR = velocità della ruota destra
⎪⎪⎩
⎪⎪⎨ −
=d
r
v
LR )(2ωωω
ωR e oc tà de a uota dest aωL = velocità della ruota destrar= raggio delle ruoted= distanza tra i centri delle ruote
Controllo di Robot -- 33
Dato un ingresso cinematico [v, ω]T da imporre all’uniciclo è sempre possibile trovare una coppia [ωR, ωL]T che lo riproducono
UnicicloUniciclo differentialdifferential drivedrive
⎥⎤
⎢⎡⎥⎤
⎢⎡
=⎥⎤
⎢⎡ Rrrv ω2/2/
E’ possibile trovare il modello cinematico dell’uniciclo differential drive con le velocità angolari delle ruote come ingresso:
⎥⎦
⎤⎢⎣
⎡
⎥⎥⎥⎥⎤
⎢⎢⎢⎢⎡
=⎥⎦
⎤⎢⎣
⎡⎥⎥⎤
⎢⎢⎡
⎥⎥⎤
⎢⎢⎡
= RR r
r
r
rrr
qωθ
θ
θ
θ
ωθθ
sin2
cos
sin2
cos
2200
sincos
&
⎥⎦
⎢⎣⎥⎦
⎢⎣ −
=⎥⎦
⎢⎣ Ldrdr ωω //
Controllo di Robot -- 34Cristian Secchi
⎥⎦
⎢⎣
⎥⎥⎥
⎦⎢⎢⎢
⎣−
⎥⎦
⎢⎣⎥⎥
⎦⎢⎢
⎣−⎥
⎥⎦⎢
⎢⎣
LL
dr
drd
rdrq
ωω 2210
Cristian Secchi Pag. 18
UnicicloUniciclo a trazione differenzialea trazione differenziale
Ha tre ruote orientabili e allineate che sono comandate in modo solidale da due soli motori attraverso un accoppiamento meccanico (es.:attraverso un accoppiamento meccanico (es.: catena, cinghia di trasmissione).
Un motore impone la rotazione delle ruote attorno al loro asse orizzontale e, quindi, determina la trazione al robot. L’altro motore impone la rotazione delle ruote attorno al loro asse verticale e, quindi, determina l’orientamento del robot
Controllo di Robot -- 35Cristian Secchi
Questo tipo di robot è cinematicamente equivalente all’uniciclo. Il punto di coordinate (x,y) può rappresentare un punto qualsiasi del robot mentre θrappresenta l’orientamento del veicolo. Gli ingressi cinematici dell’uniciclo ideale sono gli stessi dell’uniciclo a trazione differenziale.
Modello cinematico del BicicloModello cinematico del Biciclo
Un biciclo è un veicolo avente una ruota orientabile e una ruota fissa disposte come in figura.
⎥⎥⎤
⎢⎢⎡yx
C fi i
⎥⎥⎥
⎦⎢⎢⎢
⎣
=
ϕθy
qConfigurazione:
),(),( rr yxyx =
Controllo di Robot -- 36Cristian Secchi
Consideriamo il caso in cui il biciclo è a trazione anteriore.
C= Centro di istantanea rotazione
Cristian Secchi Pag. 19
Modello cinematico del BicicloModello cinematico del Biciclo
Il sistema è soggetto a due vincoli di puro rotolamento, uno per ogni ruota.
0)cos()sin( =+−+ ϕθϕθ yx ff && (xf,yf) è la posizione cartesiana del punto di contatto della ruota
0)cos()sin( =− θθ yx &&del punto di contatto della ruota anteriore
θ
θ
sin
cos
lyy
lxx
f
f
+=
+=
xf e yf non fanno parte della configurazione del biciclo perché dipendono da x,y e θ
Controllo di Robot -- 37Cristian Secchi
0)cos()sin( =+−+ ϕθϕθ ff yx &&
0cos)cos()sin( =−+−+ ϕθϕθϕθ &&& lyx
Il vincolo introdotto dalla ruota anteriore è:
Modello cinematico del BicicloModello cinematico del Biciclo
I vincoli cinematici del biciclo sono
0)cos()sin(0cos)cos()sin(
=−=−+−+
θθϕθϕθϕθ
yxlyx
&&
&&&
0)cos()sin( θθ yx
⎥⎦
⎤⎢⎣
⎡−+−+
−=
0cos)cos()sin(00cossin
)(ϕϕθϕθ
θθl
qA
⎤⎡⎤⎡ θ
Controllo di Robot -- 38Cristian Secchi
))(Im()
1000
,
0
sin1cossincoscos
())(( qGl
spanqAKer =
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢
⎣
⎡
=ϕ
ϕθϕθ
Cristian Secchi Pag. 20
Modello cinematico del BicicloModello cinematico del Biciclo
⎤⎡⎤⎡⎤⎡ θθ
v = velocità lineare di trazioneω = la velocità angolare
Controllo di Robot -- 39Cristian Secchi
⎥⎦
⎤⎢⎣
⎡
⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢
⎣
⎡
=
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
+
⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢
⎣
⎡
=ωϕ
ϕθϕθ
ωϕ
ϕθϕθ
v
lv
lq
1000
0
sin1cossincoscos
1000
0
sin1cossincoscos
&
Modello cinematico del BicicloModello cinematico del Biciclo
Il biciclo propriamente detto ha gravi problemi di bilanciamento in condizioni statiche
Controllo di Robot -- 40Cristian Secchi
In pratica si ricorre a strutture cinematicamente equivalenti ma più stabili da un punto di vista meccanico. Le strutture più utilizzate sono la struttura a triciclo e l’automobile.
Cristian Secchi Pag. 21
Il triciclo e l’automobileIl triciclo e l’automobile
Triciclo• Due ruote fisse sull’asse posteriore e una
ruota orientabile in posizione anteriore
• Le ruote fisse sono controllate da un unico motore che determina la trazione. Un altro motore controlla l’orientamento della ruota anteriore che determina lo sterzo
Automobile (Car-like)• Due ruote fisse sull’asse posteriore e due
Controllo di Robot -- 41Cristian Secchi
Due ruote fisse sull asse posteriore e due ruote orientabili sull’asse anteriore.
• Il sistema è controllato da due motori. Uno determina la trazione (anteriore o posteriore) e l’altro lo sterzo.
Il triciclo e l’automobileIl triciclo e l’automobile
• Sia il triciclo che l’automobile hanno lo stesso modello cinematico del biciclo.
• Il t di di t ( ) è il t di d ll’ t i• Il punto di coordinate (x,y) è il punto medio dell’asse posteriore,
• θ è l’orientamento del veicolo
• ϕ è l’angolo di sterzo
• v e ω sono rispettivamente la velocità di trazione e la velocità di
Controllo di Robot -- 42Cristian Secchi
• v e ω sono rispettivamente la velocità di trazione e la velocità di sterzo
Cristian Secchi Pag. 22
UnicicloUniciclo e Bicicloe Biciclo
• La struttura cinematica a uniciclo e a biciclo sono le più utilizzate e diffuse nelle applicazioni (in particolare quelle industriali)
• Altre strutture cinematiche sono utilizzate per applicazioni particolari
• I modelli cinematici di strutture più complesse si ottengono tenendo in considerazione i vincoli introdotti da ciascuna ruota.
Controllo di Robot -- 43Cristian Secchi
CompitiCompiti didi Moto Moto ElementareElementare
• Parcheggio
• Inseguimento di Cammini (Path Following)
Controllo di Robot -- 44Cristian Secchi
• Inseguimento di Traiettorie
Cristian Secchi Pag. 23
Schema Schema didi principio principio didi un robot mobileun robot mobile
• Attuatori (Att) : Motori DC con riduttori• End-Effector : attrezzo, pinza, mano• Sensori:
• Propriocettivi (encoder, giroscopi, …)
Controllo di Robot -- 45Cristian Secchi
p ( , g p , )• Eterocettivi (bumbers, rangefinders (infrarossi, ultrasuoni),
laser, visione (mono, stereo, …)• Controllo:
• Alto livello• Basso livello
Schema Schema didi ControlloControllo a basso a basso livellolivello
• Controllori PI ad alto guadagno controllano i motori del robot affinchè il robot si muova secondo il profilo di velocità desiderato
• Il controllo a basso livello si occupa solamente di controllare gli
Controllo di Robot -- 46
attuatori del robot secondo le istruzioni del controllo ad alto livello
• Se i guadagni sono abbastanza alti, il controllo di basso livello rende il robot un sistema puramente cinematico
Cristian Secchi
Cristian Secchi Pag. 24
Schema Schema didi ControlloControllo ad alto ad alto livellolivello
• A seconda il della configurazione di setpoint, il controllo ad alto livello deve decidere che velocità imporre al robot
• Dal punto di vista del controllo ad alto livello il robot è un sistema p amente cinematico con ing essi in elocità
Controllo di Robot -- 47Cristian Secchi
puramente cinematico, con ingressi in velocità
• Il controllo di alto livello si occupa di decidere il movimento mentre quello a basso livello si occupa di controllare i motori affinché tale movimento sia attuato
Controllo di WMRControllo di WMR
• Il progetto del controllo di basso livello • è semplice in quanto si riduce al progetto di un PI per un motore
elettrico (sistema lineare)• non risente dei vincoli introdotti dalle ruotenon risente dei vincoli introdotti dalle ruote
• La pianificazione della traiettoria da seguire• Fornisce il setpoint al controllo ad alto livello• Deve tenere in considerazione i vincoli sulla mobilità del robot
• Il progetto del controllo ad alto livello
Controllo di Robot -- 48
p g• richiede di considerare considerare il modello cinematico di un
robot• deve tenere in considerazione i vincoli introdotti dalle ruote• E’ complesso perché deve controllare un sistema non lineare
Cristian Secchi
Cristian Secchi Pag. 25
PianificazionePianificazione
• E’ il bl di d t i t i tt i ll i d ll• E’ il problema di determinare una traiettoria nello spazio delle configurazioni per portare il robot da una certa configurazione iniziale ad una certa configurazione finale ammissibili (cioè compatibili con i vincoli cinematici del robot)
• Ogni punto della traiettoria DEVE essere compatibile con i vincoli cinematici del robot
Esempio: Uniciclo
Controllo di Robot -- 49Cristian Secchi
TRAIETTORIA INAMMISSIBILEIl robot non potrà mai inseguire il riferimento perché esso non è compatibile con i vincoli cinematici
Esempio: Uniciclo
PianificazionePianificazione
Pianificazione: Trovare una traiettoria ammissibile q(t) per t in [ti,tf] che porti il robot da una configurazione iniziale q(ti)=qi a una configurazione finale q(tf)=qf in assenza di ostacoli
Una traiettoria può essere
s=0
s=L
Una traiettoria può essere decomposta in un cammino q(s) e una legge oraria s=s(t)
Legge oraria: come vado dal punto di distanza zero dall’inizio della curva al punto di distanza L dall’inizio della curva.
C i Q l è l fi i
Controllo di Robot -- 50Cristian Secchi
s 0 Cammino: Qual è la configurazione corrispondente al parametro s nella curva?
Il cammino rappresenta la forma geometrica della curva parametrizzata da s mentre la legge oraria mi rappresenta il modo in cui la curva è percorsa.
Cristian Secchi Pag. 26
PianificazionePianificazione
sqsdsdq
dtdqq &&& '===Separazione spazio temporale della traiettoria:
Affinché la traiettoria sia ammissibile per un WMR, essa deve soddisfare i vincoli anolonomi del robot per cui:vincoli anolonomi del robot per cui:
0')()( == sqqAqqA &&
0>s&0')( =qqA
Condizione di ammissibilità geometrica del cammino
Un cammino ammissibile sarà dato da uqGq ~)('=
Controllo di Robot -- 51Cristian Secchi
Il cammino geometrico si trova determinando gli ingressi geometrici ũ. Una volta trovato il cammino geometrico ammissibile, occorrerà scegliere una legge oraria s(t), ovvero la velocità a cui il cammino è percorso, per determinare completamente la traiettoria.
PianificazionePianificazione
Una volta trovato l’ingresso geometrico ũ e determinata la legge oraria s(t), qual è l’ingresso cinematico da applicare al robot per farlo muovere lungo la traiettoria desiderata?
uqGq )(=&
)(~)()(' suqGsq =
ssuqGsdsdq
&& )(~)(=
Controllo di Robot -- 52Cristian Secchi
?=ussuqGq && )(~)(=
ssutu &)(~)( =
Cristian Secchi Pag. 27
EsempioEsempio
Per un uniciclo i vincoli anolonomi implicano la seguente condizione di ammissibilità per il cammino geometrico
0cos'sin'']0,cos,[sin =−=− θθθθ yxq
La condizione esprime il fatto che la velocità cartesiana deve essere orientata lungo la direzione del moto. I cammini ammissibili per l’uniciclo sono dati da:
ωθθθ
~'
~sin'
~cos'
===
vyvx
Controllo di Robot -- 53Cristian Secchi
ωθ
stsvtv&
&
ωω ~)(
~)(==
Una volta determinati gli ingressi geometrici, gli ingressi cinematici sono dati da
Pianificazione tramite Pianificazione tramite differentialdifferential flatnessflatness
uxgxfx )()( +=&Un sistema dinamico non lineare
possiede la proprietà di differential flatness (o piattezza differenziale) se esiste un insieme di variabili misurabili y, dette uscite flat ( o piatte), tali che lo stato x e l’ingresso u possono essere espressi algebricamente in funzione di y e di un certo numero delle sue derivate
),...,,,(),...,,,(
)(
)(
r
r
yyyyuuyyyyxx
&&&
&&&
=
=
Controllo di Robot -- 54Cristian Secchi
Una volta determinata una traiettoria y nel tempo, risultano determinate anche la traiettoria dello stato x e dell’ingresso u
Cristian Secchi Pag. 28
Pianificazione tramite Pianificazione tramite differentialdifferential flatnessflatness
Per l’uniciclo e il biciclo le coordinate cartesiane x e y sono uscite flat
θ ~'
Modello geometrico dell’uniciclo l’orientamento dipende dalla derivata prima delle uscite flat
ωθθθ
~'
~sin'
~cos'
===
vyvx
1,0)'/'arctan()','( =+== kkxyyx πθθ
p
Le due scelte possibili per θ dipendono dal fatto che lo stesso cammino può essere percorso a marcia avanti (k=0) o a marcia indietro (k=π). Se l’orientamento iniziale del robot è assegnato, solo una di queste scelte è co etta
Controllo di Robot -- 55Cristian Secchi
corretta.
k=0 k=π
Pianificazione tramite Pianificazione tramite differentialdifferential flatnessflatness
Se un robot mobile ammette uscite piatte y, queste possono essere utilizzate per risolvere i problemi di pianificazione. Infatti, è possibile usare un qualsiasi schema di interpolazione per pianificare il cammino di tali uscite in modo da soddisfare le condizioni al contorno. L’evoluzione delle altre variabili associate e gli ingressi cinematici si potranno calcolarevariabili associate e gli ingressi cinematici si potranno calcolare algebricamente a partire da y(s). Il cammino nello spazio delle configurazioni generato in tal modo soddisferà automaticamente i vincoli anolonomi del robot.
Controllo di Robot -- 56Cristian Secchi
Cristian Secchi Pag. 29
Pianificazione tramite Pianificazione tramite differentialdifferential flatnessflatness
Si consideri il problema di pianificare un cammino per l’uniciclo che porti il robot da una configurazione iniziale qi=(xi,yi,θi) a una configurazione finale qf=(xf,yf,θf). Suppongo di parametrizzare la curva con un parametro s=[0,1].
x e y sono uscite flat! Pianifico la traiettoria sulle uscite flat!
2233
2233
)1()1()1()(
)1()1()1()( −+−+−−= ssssxsxssx xxif
β
βα
Una possibile pianificazione su x e y si può ottenere tramite un polinomio cubico
Controllo di Robot -- 57Cristian Secchi
2233 )1()1()1()( −+−+−−= ssssysyssy yyif βα
fi
fi
yyyyxxxx
====
)1()0()1()0( Le condizioni al contorno
su x e y sono soddisfatte
Pianificazione tramite Pianificazione tramite differentialdifferential flatnessflatness
Occorre che siano soddisfatte le condizioni al contorno anche sull’orientamento che è legato a x’ e y’. Devono allora essere soddisfatte le seguenti condizioni:
ki e kf sono parametri liberi (non nulli) ma dello stessokk θθ )1(')0(' (non nulli) ma dello stesso segno. Rappresentano le velocità geometriche all’istante iniziale e a quello finale e influenzano il tipo di cammino che si ottiene.
ffii
ffii
kykykxkx
θθθθ
cos)1('sin)0('cos)1('cos)0('
====
Le condizioni al contorno su θ consentono di determinare i parametri αx, αy, βx e βy Per esempio, se ki=kf=k si ottiene:
Controllo di Robot -- 58Cristian Secchi
y, βx βy. p , i f
⎥⎦
⎤⎢⎣
⎡++
=⎥⎦
⎤⎢⎣
⎡⎥⎦
⎤⎢⎣
⎡−−
=⎥⎦
⎤⎢⎣
⎡
ii
ii
y
x
ff
ff
y
x
ykxk
ykxk
3sin3cos
3sin3cos
θθ
ββ
θθ
αα
Cristian Secchi Pag. 30
Pianificazione tramite Pianificazione tramite differentialdifferential flatnessflatness
Una volta trovata la traiettoria nello spazio delle configurazioni, quali sono gli ingressi geometrici e gli ingressi cinematici corrispondenti?
Dal modello cinematico del robot si ottengonoDal modello cinematico del robot si ottengono
22
22
))('())('()(')('')(')('')(~
))('())('()(~
sysxsysxsxsys
sysxsv
+−
=
+±=
ω
il segno della velocità lineare dipende dal senso di marcia
Controllo di Robot -- 59Cristian Secchi
Pianificazione dell’Pianificazione dell’UnicicloUniciclo -- AlgoritmoAlgoritmo
• Require: qi=(xi,yi,θi), qf=(xf,yf,θf)
1. Pianificare il cammino sulle uscita flat (x,y)
2233
2233
)1()1()1()(
)1()1()1()(
−+−+−−=
−+−+−−=
ssssysyssy
ssssxsxssx
f
xxif
βα
βα
2. Imporre le condizioni al contorno per determinare i coefficienti
3. Determinare gli ingressi geometric
)1()1()1()( ++ ssssysyssy yyif βα
⎥⎦
⎤⎢⎣
⎡++
=⎥⎦
⎤⎢⎣
⎡⎥⎦
⎤⎢⎣
⎡−−
=⎥⎦
⎤⎢⎣
⎡
ii
ii
y
x
ff
ff
y
x
ykxk
ykxk
3sin3cos
3sin3cos
θθ
ββ
θθ
αα
22 ))('())('()(~
Controllo di Robot -- 60
4. Determinare la legge oraria e gli ingressi da applicare al robot
Cristian Secchi
22
22
))('())('()(')('')(')('')(~
))('())('()(~
sysxsysxsxsys
sysxsv
+−
=
+±=
ω
sstssvtv&
&
)(~)()(~)(
ωω ==
Cristian Secchi Pag. 31
Pianificazione tramite Pianificazione tramite differentialdifferential flatnessflatness
[ ]0100=iq [ ]000=fqParcheggio parallelo
k=10 k=100
Controllo di Robot -- 61Cristian Secchi
Pianificazione tramite Pianificazione tramite differentialdifferential flatnessflatness
[ ]3/710 π−=iq [ ]000=fqk=10
k=100k=50
Controllo di Robot -- 62Cristian Secchi
Cristian Secchi Pag. 32
Controllo del motoControllo del moto
Data una traiettoria o una configurazione desiderata, costruire una legge di controllo che porti il robot a seguire la traiettoria o nella configurazione desiderata
• Il bl d l t ll d l t di b t bil i• Il problema del controllo del moto di un robot mobile viene affrontato utilizzando il modello cinematico
• Si suppone che gli ingressi cinematici agiscano direttamente sulle variabili di configurazione
• Nel caso dell’uniciclo e del biciclo, gli ingressi di controllo sono v e ω• Questo è lecito perché nella maggior parte dei robot mobili non è
possibile imporre direttamente la coppia motrice sulle ruote a causa di lli di ll b li ll i i ll’ hi
Controllo di Robot -- 63Cristian Secchi
di anelli di controllo a basso livello integrati nell’architettura hardware o software.
• Considereremo principalmente il problema del controllo del robot a uniciclo.
Controllo del motoControllo del moto
Regolazione di configurazione: Il robot deve raggiungere una configurazione desiderata qd=(xd, yd, θd)T a partire da una configurazione iniziale q0=(x0, y0, θ0)T .
E’ il problema più difficile
Inseguimento di traiettoria: Il robot deve riprodurre asintoticamente una traiettoria cartesiana desiderata (x (t) y (t)) a partire da
Problema più li di
Controllo di Robot -- 64Cristian Secchi
cartesiana desiderata (xd(t),yd(t)) a partire da una configurazione iniziale q0=(x0, y0, θ0)T che può essere o meno agganciata alla traiettoria
semplice e di maggior interesse pratico
Cristian Secchi Pag. 33
Inseguimento di traiettorie Inseguimento di traiettorie –– II--O SFLO SFL
• Dipende dal punto preso come riferimento del robot (uscita del sistema), ossia al quale vogliamo assegnare il moto desiderato• Il punto (x,y) del centro asse non può mai avere una velocità
I-O SFL: Input-Output state feedback linearization
laterale rispetto all’orientamento del veicolo
θθ
==
&
&
&
sincos
vyvx
Il sistema deve fermarsi e ruotare. Non è possibile percorrere una traiettoria discontinua
l i àP1
P2 P3
P4
Controllo di Robot -- 65Cristian Secchi
ωθ =
Movimento istantaneo proibito
Prendendo il centro asse (x,y) come uscita da controllare abbiamo dei vincoli nella mobilità del robot.
a velocità costanteP1 P4
Inseguimento di traiettorie Inseguimento di traiettorie –– II--O SFLO SFL
Si definisce come uscita da controllare un punto fuori dall’asse delle ruote di coordinate
0≠bθcosbxxB +=
),( BB yx
b
θsinbyyB +=
Il punto (xB,yB) non ha più vincoli cinematici e può muoversi, istantaneamente, anche lateralmente rispetto alla direzione di moto
Controllo di Robot -- 66Cristian Secchi
),( yx
Posso definire due ingressi cinematici che determinano la velocità lungo l’asse x e lungo l’asse y del punto B
Cristian Secchi Pag. 34
Inseguimento di traiettorie Inseguimento di traiettorie –– II--O SFLO SFL
ωθ
θωθθωθωθθω
=
+=+=−=−=
&
&&
&&
cossincossincossin
bvbyybvbxx
B
B
dyB
dxB
vyvx
==
&
&
⎥⎦
⎤⎢⎣
⎡=⎥
⎦
⎤⎢⎣
⎡⎥⎦
⎤⎢⎣
⎡ −=⎥
⎦
⎤⎢⎣
⎡ω
θωθθ
θθ vT
vbb
yx
B
B )(cossinsincos
&
&0)(det ≠= bT θ
La matrice T(θ) è sempre invertibile
⎤⎡⎤⎡⎤⎡⎤⎡ θθ i
Controllo di Robot -- 67Cristian Secchi
⎥⎦
⎤⎢⎣
⎡
⎥⎥⎦
⎤
⎢⎢⎣
⎡−=⎥
⎦
⎤⎢⎣
⎡=⎥
⎦
⎤⎢⎣
⎡ −
dy
dx
dy
dx
vv
bbvv
Tv
θθθθ
θω cos1sin1
sincos)(1
Inseguimento di traiettorie Inseguimento di traiettorie –– II--O SFLO SFL
Dalle relazioni precedenti si ricava il seguente sistema lineare e disaccoppiato sui canali ingressi-uscite
dxB vx =& La direzione x e la direzione y del punto B possono essere controllate
)sincos(1 θθθ dxdy
dyB
vvb
vy
−=
=
&
&possono essere controllate indipendentemente l’una dall’altra tramite gli ingressi vdx e vdy
Data una traiettoria (xdes,ydes) da inseguire, è possibile costruire trovare vdxe vdy che garantiscono l’inseguimento utilizzando queste semplici leggi
Controllo di Robot -- 68Cristian Secchi
lineari:
)()(
2
1
Bdesdesdy
Bdesdesdx
yykyvxxkxv−+=−+=
&
& yyexxe
desy
desx
−=−=
00
2
1
=+=+
yy
xx
ekeeke
&
&
00
→→
y
x
ee
Cristian Secchi Pag. 35
Inseguimento di traiettorie Inseguimento di traiettorie –– II--O SFLO SFL
Una volta trovati vdx e vdy, si trovano gli ingressi cinematici da dare in ingresso all’uniciclo per ottenere il movimento desiderato del punto B tramite
⎥⎦
⎤⎢⎣
⎡
⎥⎥⎦
⎤
⎢⎢⎣
⎡−=⎥
⎦
⎤⎢⎣
⎡=⎥
⎦
⎤⎢⎣
⎡ −
dy
dx
dy
dx
vv
bbvv
Tv
θθθθ
θω cos1sin1
sincos)(1
Controllo di Robot -- 69Cristian Secchi
Inseguimento di traiettorie Inseguimento di traiettorie –– II--O SFLO SFL
Il punto (x,y) arrotonda l’angolo vivo. L’arrotondamento può essere reso piccolo a
P2 P3021 ==→ dydx vvvPP
piacere scegliendo b abbastanza piccolo
Controllo di Robot -- 70Cristian Secchi
P1 P4014
043
032
=−=→
−==→
==→
dydx
dydx
dydx
vvvPP
vvvPP
vvvPP
Cristian Secchi Pag. 36
IOIO--SFL per l’SFL per l’UnicicloUniciclo -- AlgoritmoAlgoritmo
Require: qdes(t)=(xdes(t),ydes(t),θdes(t))
1. scegliere il punto fuori dall’asse (xB,yB)=(x+bcosθ,y+bsinθ) definendo b
2. while True3. Determinare vdx(t)e vdy(t) tramite
4. Determinare v(t) e ω(t) tramite
))()(()()())()(()()(
2
1
tytyktytvtxtxktxtv
Bdesdesdy
Bdesdesdx
−+=−+=
&
&
Controllo di Robot -- 71
5. Applicare gli ingressi 6. end while
Cristian Secchi
⎥⎦
⎤⎢⎣
⎡
⎥⎥
⎦
⎤
⎢⎢
⎣
⎡
−=⎥
⎦
⎤⎢⎣
⎡=⎥
⎦
⎤⎢⎣
⎡ −
)()(
)(cos1)(sin1)(sin)(cos
)()(
))(()()( 1
tvtv
tb
tb
tt
tvtv
tTttv
dy
dx
dy
dx
θθ
θθθ
ω
EsempioEsempio
Controllo di Robot -- 72Cristian Secchi
Cristian Secchi Pag. 37
NavigazioneNavigazione
Spesso un robot si trova a doversi muovere in un ambiente in cui sono presenti ostacoli (fissi o mobili). Il robot, tramite la conoscenza dell’ambiente e/o tramite l’utilizzo dei sensori a bordo deve riconoscere dove sono gli ostacoli ed evitarli al fine di navigare in maniera sicura (cioè
lli i i) ll’i t d ll i di lsenza collisioni) all’interno dello spazio di lavoro.
Navigazione: Data una configurazione di partenza e una di arrivo note, è il problema di trovare una percorso libero da collisioni che porti il robot dalla configurazione di partenza a quella di arrivo
goalq
Controllo di Robot -- 73Cristian Secchi
startq
Bug Bug AlgorithmsAlgorithms
Gli algoritmi bug si basano sulla possibilità del robot di percepire, tramite opportuni sensori eterocettivi (bumpers, range finders), la presenza di un ostacolo quando vi sono molto vicini o addirittura quando lo stanno toccando.tocca do
Idea di base: Muovere il robot in linea retta verso l’obiettivo. Quando si incontra un ostacolo, lo si circumnaviga fino a quando non è possibile muoversi in linea retta verso l’obiettivo.
Controllo di Robot -- 74Cristian Secchi
Si assume che il robot sia un punto (es.: no controllo sull’orientamento per uniciclo) dotato di un sensore di contatto. Si assume inoltre che il robot sia in grado di conoscere la sua posizione, oltre a quella della configurazione di partenza e di quella obiettivo.
Cristian Secchi Pag. 38
Bug1Bug1
Il robot ha due tipi di comportamento tra i quali commuta:
Si definisca come m-line il segmento che collega un punto a qgoal
motion to goal: A partire da un leave point, il robot si muove lungo la m-line che connette il leave point al goal finchè non raggiunge il goal o incontra un ostacolo. Se il robot incontra un ostacolo, il punto di contatto viene detto hit point e il comportamento del robot commuta al boundary following.
boundary following: a partire da un hit point, il robot circumnaviga l’ostacolo finchè non ritorna all’hit point. Poi determina il punto più vicino al
Controllo di Robot -- 75Cristian Secchi
p p pgoal lungo il perimetro dell’ostacolo e si sposta lungo il perimetro fino a raggiungere quel punto, detto leave point e il comportamento del robot commuta al motion to goal.
Bug1Bug1
LiqHiq
leave point i-esimo
hit point i-esimo
startL qq =0 0=i Inizializzazione
All’inizio il sistema si trova in un leave point
Controllo di Robot -- 76Cristian Secchi
Cristian Secchi Pag. 39
Bug1Bug1
Se la linea che collega il leave point i-esimo con il goal intereseca l’ostacolo i-esimo, allora non esiste nessun percorso che consenta di raggiungere il goal. In tal caso l’algoritmo lo segnala
Controllo di Robot -- 77Cristian Secchi
Bug1 Bug1 AlgorithmAlgorithm
Input: Un robot con un sensore di contattoOutput: Un percorso fino a qgoal o una conclusione che tale percorso non esiste
0: i=1; qL0=qstart;
1: while Forever do2: repeat2: repeat3: Da q Li-1, muoviti verso qgoal. 4: until qgoal è raggiunto or si tocca un ostacolo a qH
i5: if Il goal è raggiunto then6: Exit. 7: end if8: repeat9: Segui il bordo dell’ostacolo. 10: until qgoal è raggiunto or ci si ritrova a qH
i11: Determina il punto qL
i sul perimetro che è alla minima distanza dal goal.
Controllo di Robot -- 78Cristian Secchi
12: vai a qLi.
13: if la m-line che passa da qLi interseca l’ostacolo su cui si trova qL
i then14: Concludi che qgoal non è raggiungibile e exit. 15: end if16: i=i+116: end while
Cristian Secchi Pag. 40
Bug2 Bug2
• Come per il Bug1 anche il Bug2 commuta tra due comportamenti: il motion to goal e il boundary following
• Per il Bug2, la m-line è fissa ed è il segmento di linea che connette qstart a qgoal
motion to goal: A partire da un leave point, il robot si muove lungo la m-line che connette qstart a qgoal finchè non raggiunge il goal o incontra un ostacolo. Se il robot incontra un ostacolo, il punto di contatto viene detto hit point e il comportamento del robot commuta al boundary following.
Controllo di Robot -- 79Cristian Secchi
boundary following: a partire da un hit point, il robot circumnaviga l’ostacolo finchè non raggiunge un punto sulla m-line, detto leave point e il comportamento del robot commuta al motion to goal.
Bug2Bug2
LiqHiq
leave point i-esimo
hit point i-esimo
goalL qq =0 0=i Inizializzazione
All’inizio il sistema si trova in un leave point
Controllo di Robot -- 80Cristian Secchi
Cristian Secchi Pag. 41
Bug2Bug2
Se durante il boundary following il robot incontra di nuovo il punto da cui è partito sulla m-line allora non esiste nessun percorso che consenta di raggiungere il goal. In tal caso l’algoritmo lo segnala
Controllo di Robot -- 81Cristian Secchi
Bug2 Bug2 AlgorithmAlgorithm
Input: Un robot con sensore di contatto Output: Un percorso fino a qgoal o una conclusione che tale percorso non esiste
0: i=1; qL0=qstart;
1: while True do2 t2: repeat3: Da qL
i-1, muoviti verso qgoal lungo la m-line. 4: until qgoal è stato raggunto or si tocca un ostacolo all’hit point qH
i. 5: Gira a sinistra (o a destra). 6: repeat7: Segui il bordo dell’ostacolo 8: until9: qgoal è raggiunto or10: qH
i è raggiunto di nuovo or11: la m-line è raggiunta di nuovo a un punto m tale che
Controllo di Robot -- 82Cristian Secchi
11: la m line è raggiunta di nuovo a un punto m tale che 12: m ≠ qH
i (il robot non ha raggiunto l’ hit point), 13: d(m, qgoal) < d(m, qH
i (il robot è più vicino), e14: Se il robot si muove verso il goal, non entra in contatto con l’ostacolo. 15: Si pongta qL
i+1 = m16: i=i+117: end while
Cristian Secchi Pag. 42
Bug1 o Bug2?Bug1 o Bug2?
A prima vista sembra che Bug2 sia meglio di Bug1 in quanto esso non comporta una circumnavigazione completa degli ostacoli. Tuttavia, questo non è sempre vero e dipende dal tipo di ostacoli
B 1 è li di B 2Bug2 è meglio di Bug1 Bug1 è meglio di Bug2
Controllo di Robot -- 83Cristian Secchi
Bug1 o Bug2?Bug1 o Bug2?
• Bug1 fa una ricerca esaustiva e trova il leave point migliore. Questo richiede che il robot percorra tutto il perimetro dell’ostacolo
• Bug2 usa un approccio opportunistico. Quando trova un leave point che è migliore di quelli che ha visto prima , lo sceglie (approccio greedy)
• Quando gli ostacoli sono “semplici”, tipicamente Bug2 è migliore di Bug1
Controllo di Robot -- 84
• Nel caso in cui gli ostacoli siano “complessi”, Bug1 è migliore di Bug2
Cristian Secchi
Cristian Secchi Pag. 43
TangentTangent BugBug
• E’ un miglioramento del Bug2.• Proposto in:
Kamon I., Rivlin E., Rimon, E., A new range-sensor based globally convergent navigation algorithm for mobile robots IEEE Internationalconvergent navigation algorithm for mobile robots, IEEE International Conference on Robotics and Automation, 1996
disponibile all’indirizzohttp://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=503814&isnum
ber=10815accessibile solo dagli indirizzi IP UNIMORE.
Controllo di Robot -- 85Cristian Secchi
Navigazione attraverso funzioni PotenzialiNavigazione attraverso funzioni Potenziali
• Gli algoritmi bug sono indicati solamente per robot mobili che si muovono sul piano e non si considera l’intera variabile delle configurazioni
• Attraverso la pianificazione basata su campi potenziali è possibile pianificare il moto anche per sistemi con variabili di configurazione di dimensione elevata.
• Come gli algoritmi bug, consentono di generare il percorso
Controllo di Robot -- 86
Come gli algoritmi bug, consentono di generare il percorso incrementalmente, cambiandolo man mano che un ostacolo è percepito
Cristian Secchi
Cristian Secchi Pag. 44
Navigazione attraverso funzioni PotenzialiNavigazione attraverso funzioni Potenziali
RRU m →: Funzione potenziale
TUU ⎤⎡ ∂∂
n
qqUq
qUqU ⎥
⎦
⎤⎢⎣
⎡∂∂
∂∂
=∇ )()()(1
L Gradiente di U
La funzione potenziale può essere interpretata come una funzione energia sullo spazio delle configurazioni e il gradiente di U rappresenta la
Controllo di Robot -- 87Cristian Secchi
energia sullo spazio delle configurazioni e il gradiente di U rappresenta la forza indotta dalla presenza del potenziale. Il gradiente in una punto q punta verso la direzione in cui U cresce.
Navigazione attraverso funzioni PotenzialiNavigazione attraverso funzioni Potenziali
22 111 T 22
21 2
121
21)( qqqqqU T +==
[ ]21)( qqqU =∇
Controllo di Robot -- 88Cristian Secchi
Cristian Secchi Pag. 45
Navigazione attraverso funzioni PotenzialiNavigazione attraverso funzioni Potenziali
L’approccio delle funzioni potenziali guida il robot come se fosse una particella immersa in un campo potenziale dato dai gradienti di svariate funzioni potenziali. Alla configurazione obiettivo è associata una funzione potenziale che genera un gradiente che attira la particella mentre ad ogni ostacolo è associato una funzione potenziale che genera unogni ostacolo è associato una funzione potenziale che genera un gradiente che respinge le particelle. La combinazione dei due gradienti produce un percorso libero da collisioni verso la configurazione obiettivo.
Controllo di Robot -- 89Cristian Secchi
Navigazione attraverso funzioni PotenzialiNavigazione attraverso funzioni Potenziali
Le funzioni potenziali possono essere viste come dei paesaggi in cui il robot è immerso e lungo i quali si muove spostandosi da un punto “in alto” a un punto più “in basso”. Il robot segue un percorso in discesa, seguendo il gradiente negato della funzione potenziale. Questa tecnica di movimento si chiama discesa del gradiente (gradient descent).movimento si chiama discesa del gradiente (gradient descent).
U(q) )()( qUtq −∇=&
Controllo di Robot -- 90Cristian Secchi
q
Il robot termina quando raggiunge una configurazione q* nella quale il gradiente si annulla. Questo punto può essere un minimo, un massimo o un punto di sella. Per come verranno disegnati i potenziali, q* sarà un minimo (locale)
Cristian Secchi Pag. 46
Navigazione attraverso funzioni PotenzialiNavigazione attraverso funzioni Potenziali
Come si costruisce la funzione potenziale che genera il cammino da qstart a qgoal evitando gli ostacoli?
)(UPotenziale attrattivo. Ha il minimo in
i d di Il l è di)(qUatt
∑=
=obs
i
n
ireprep qUqU
1
)()(
corrispondenza di qgoal.Il suo ruolo è di attirare il robot verso l’obiettivo.
Potenziale repulsivo. E’ dato dalla somma dei potenziali repulsivi per ogni ostacolo. Ha il ruolo di allontanare il
Controllo di Robot -- 91Cristian Secchi
robot dagli ostacoli presenti nell’ambiente
)()()( qUqUqU repatt += )()( qUtq −∇=&
Il potenziale attrattivoIl potenziale attrattivo
Il potenziale Uatt deve essere monotonicamente crescente al crescere della distanza da qgoal. In tal modo il robot, tramite gradien descent, viene attirato alla configurazione obiettivo qgoal. Ciò equivale a dire che qgoal è una configurazione di minimo isolata e che il sistema non ha punti critici. I di d d( ) l di t t ibil lt èIndicando con d(q,qgoal) la distanza tra q e qgoal, una possibile scelta è:
),(21 2
goalaatt qqdkU = )(),(21 2
goalagoalaatt qqkqqdkU −=∇=∇
Controllo di Robot -- 92Cristian Secchi
Quando il robot è lontano dall’obiettivo, vi si avvicina velocemente (la forza che lo attrae è grande) mentre quando il robot è vicino all’ obiettivo, vi si avvicina dolcemente (la forza che lo attrae è piccola)
Cristian Secchi Pag. 47
Il potenziale attrattivoIl potenziale attrattivo
Controllo di Robot -- 93Cristian Secchi
Il potenziale repulsivoIl potenziale repulsivo
E’ composto da una somma di potenziali repulsivi, ciascuno associato a un ostacolo. Il termine i-esimo deve essere tale da generare una forza repulsiva tanto più grande quanto più il robot è vicino all’ostacolo i-esimo. Inoltre, esso non deve generare nessuna forza se il robot è sufficientemente lontano dall’ostacolo ostacolo i-esimo. Una possibile scelta è:
⎪⎩
⎪⎨
⎧
>
≤⎟⎟⎠
⎞⎜⎜⎝
⎛−
=
*)(0
*)(*
1)(
121
)(
2
Qqd
QqdQqd
kqU
i
ii
rrep
ii ∑
=
=obs
i
n
ireprep qUqU
1)()(
Controllo di Robot -- 94Cristian Secchi
Q*=distanza oltre la quale il robot ignora l’ostacolodi(q)=distanza tra il robot e l’ostacolo i esimo.nobs=numero di ostacoli
Cristian Secchi Pag. 48
Il potenziale repulsivoIl potenziale repulsivo
⎪
⎪⎨
⎧≤∇⎟⎟
⎠
⎞⎜⎜⎝
⎛−
=∇*)()(
)(1
*1
)(
2
QqdqdqdQ
kqU ii
ir
repi
i
⎪⎩ > *)(0 Qqdi
Se di(q) va a zero allora il gradiente va a infinito!
∑∇=∇obsn
qUqU )()(
Controllo di Robot -- 95Cristian Secchi
∑=
∇=∇i
ireprep qUqU
1)()(
L’algoritmo di discesa del gradienteL’algoritmo di discesa del gradiente
Input: Un modo di calcolare il gradiente U (q) in un punto qOutput: Una sequenza di punti {q(0), q(1), ..., q(i)}
repatt UUUtq ∇−−∇=−∇=)(&
p q p {q( ), q( ), , q( )}
1: q(0) = qstart
2: i = 0 3: while U (q(i)) ≠ 0 do4: q(i + 1) = q(i) - α(i) U (q(i)) 5: i = i + 1 6: end while
Il l (i) d t i l l h d l i il b t d
Controllo di Robot -- 96Cristian Secchi
Il valore a(i) determina la lunghezza del passo con cui il robot scende lungo il gradiente di U. Se a(i) è piccolo, l’algoritmo è accurato ma computazionalmente pesante mentre se a(i) è grande l’algoritmo è più veloce ma meno accurato. La scelta di a(i) dipende dal tipo di applicazione.
Cristian Secchi Pag. 49
Minimi LocaliMinimi Locali
E’ possibile che l’algoritmo di discesa del gradiente si fermi in un minimo locale
Controllo di Robot -- 97Cristian Secchi
Il potenziale attrattivo e quello repulsivo si controbilanciano!!
Minimi LocaliMinimi Locali
Esistono approcci per risolvere il problema dei minimi locali:
1) Wave front planners:J. Barraquand, B. Langlois, J.C. Latombe, Numerical potential field techniques for robot path planning, IEEE u e ca pote t a e d tec ques o obot pat p a g,Transactions on Man and Cybernetics 22(2),1992disponibile a
http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=148426&isnumber=3929
da un indirizzo IP UNIMORE
Controllo di Robot -- 98Cristian Secchi
2) Navigation Functions. Si veda il libro Principles of Robot Motiondispobilile in biblioteca
Cristian Secchi Pag. 50
Altri algoritmi di navigazioneAltri algoritmi di navigazione
• Esistono altre tecniche di navigazione per certi versi più complete ed efficienti di quelle illustrate sinora:• Roadmaps• Cell Decomposition• Sampling based algorithms• …
• Si consulti il seguente testo disponibile in biblioteca per una trattazione esaustiva:• Choset Lynch Hutchinson Kantor Burgard Kavraki Thrun
Controllo di Robot -- 99
• Choset, Lynch, Hutchinson, Kantor, Burgard, Kavraki, Thrun, Principle of Robot Motion- Theory, algorithms and Implementations, MIT Press 2005
Cristian Secchi
ControlloControllo del Motodel Moto
• Altre problematiche interessanti• Pianificazione in presenza di vincoli cinematici (velocità limitata)• Cammini a tempo minimo• Sistemi multi robot• …
Controllo di Robot -- 100Cristian Secchi
Cristian Secchi Pag. 51
SLAM SLAM –– SimultaneousSimultaneous LocalizationLocalization and and MappingMapping
• Per poter seguire una traiettoria desiderata un robot mobile deve conoscere la propria configurazione
• Per poter eseguire una strategia di navigazione non banale, un robot deve conoscere una mappa dell’ambiente circostante
• Ogni controllore in retroazione richiede la conoscenza dello stato del robot (posizione assoluta)
Controllo di Robot -- 101
• In generale, per rendere veramente autonomo un robot mobile, è necessario che esso sia in grado di localizzarsi e di costruire una mappa dell’ambiente circostante
Cristian Secchi
SLAM SLAM –– SimultaneousSimultaneous LocalizationLocalization and and MappingMapping
Lo SLAM cerca una risposta alla domanda:
E’ possibili usare un veicolo che parte da
1. una posizione iniziale ignota, in un 2. ambiente ignoto, per 3. costruire incrementalmente una
mappa dell’ambiente, e 4. allo stesso tempo
usare la mappa per determinare la
Controllo di Robot -- 102Cristian Secchi
usare la mappa per determinare la posizione del robot?
Cristian Secchi Pag. 52
Origini: la localizzazione Origini: la localizzazione odometricaodometrica
OdometriaL’odometria è la tecnica per stimare la posizione di un veicolo su ruote basandosi su informazioni provenienti da sensori che misurano lo spazio percorso da alcune delle ruote e l’angolo di sterzo (se presente).
• Gli encoder incrementali sulle ruote ne misurano la rotazione, ma non l’assetto del robot in un sistema di riferimento fisso assoluto
• Se una ruota slitta l’encoder rileva uno spostamento che non è
Controllo di Robot -- 103
• Se una ruota slitta, l encoder rileva uno spostamento che non è coerente con il cambio di assetto del robot
Cristian Secchi
Localizzazione Localizzazione odometricaodometrica: l’: l’uniciclouniciclo
⎪⎩
⎪⎨
⎧
==
θ
θθ
&
&
&
sincos
vyvx Supponiamo comandi costanti durante ogni
intervallo di campionamento (generalmenteverificato se controllo digitale)
• Se all’istant k(=kT) siano noti • la configurazione q(k) del robot• i comandi v(k) e ω(k) che saranno applicati nell’intervallo [k,k+1)
⎪⎩ =ωθ verificato se controllo digitale)
Controllo di Robot -- 104
• il periodo di campionamento T
• è possibile utilizzare il modello cinematico del robot per determinare q(k+1)
Cristian Secchi
Cristian Secchi Pag. 53
Localizzazione Localizzazione odometricaodometrica: l’: l’uniciclouniciclo• Discretizzando il modello con il metodo di Eulero si ottiene
⎪⎩
⎪⎨
⎧
+=+=+
+=+
)()()1()(sin)()1(
)(cos)()()1(
kTkkkTkvky
kTkvkxkx
ωθθθ
θ
• E’ molto semplice ma vi sono alcuni errori nella localizzazione• si considera θ(k) come costante in [k,k+1)• non è detto che i valori comandati v(k) e ω(k) siano quelli realmente
applicati (non idealità del sistema di attuazione)• errori dovuti allo slittamento delle ruote• errori di calibrazione (d e r)
⎩ )()()(
Controllo di Robot -- 105
( )• errori introdotti dal sistema di trazione• …
• La localizzazione odometrica produce un dato inaffidabile e caratterizzato da un errore non deterministica significativo e che sia accumula durante il moto del robot
Cristian Secchi
Uso della sola odometriaUso della sola odometria
GPS (for reference)
Controllo di Robot -- 106Cristian Secchi
Odometry
Cristian Secchi Pag. 54
Sensori Sensori EterocettiviEterocettivi
• Per compensare gli errori introdotti dall’odometria e per costruire una mappa dell’ambiente circostante si utilizzano sensori eterocettivi, solidali al robot e capaci di misurare la posizione del robot rispetto all’ambiente esternop
• I sensori eterocettivi più utilizzati sono gli scanner laser, le telecamere (monoscopiche o stereoscopiche) e il GPS (outdoor o indoor) e, per semplici applicazioni, il sonar
Controllo di Robot -- 107Cristian Secchi
Lo Scanner LaserLo Scanner Laser
• Un fascio di raggi è emesso con un certo passo angolare e ciascun f i i i l di d ll’ l h l i
Controllo di Robot -- 108
fascio restituisce la distanza dell’ostacolo che colpisce. • Per ogni fascio viene restituita l’informazione di range (distanza) e
bearing (angolo di emissione del fascio laser)• Sono misurate le coordinate polari di ciascun elemento dell’ambiente
in un sistema di riferimento solidale al robot
Cristian Secchi
Cristian Secchi Pag. 55
Il problema della localizzazioneIl problema della localizzazione
• Data una mappa m di punti di riferimento la cui posizione è nota apriori
• Misurare la posizione relativa dei punti di riferimento y(k) (es.:range e bearing)
• Determinare la posizione del veicolo x(k) basandosi sulla misura y(k)• Determinare la posizione del veicolo x(k) basandosi sulla misura y(k)• un filtro è necessario perché le misure sono tipicamente
rumorose• La posizione dei landmark viene utilizzata per ridurre l’incertezza
nella posizione del robot
• xk: posizione del veicolo all’stante k
• u : l’ingresso di controllo applicato all’stante k-1 y
yk+1
Controllo di Robot -- 109Cristian Secchi
• uk: l ingresso di controllo applicato all stante k-1e tenuto costanto nell’intervallo [k-1,k)
• yk: misura del landmark pressa allistante k
• Xk: storia delle posizioni {x1,..,xk}
• Uk: storia degli ingressi di controllo {u1,…,uk}
• m: insieme dei landmark
yk-1
yk
Il problema del Il problema del mappingmapping
• Date le posizioni del veicolo Xk
• Misurare la posizione relativa dei punti di riferimento y(k) (es.:range e bearing)
• Determinare la mappa basandosi sulla misura y(k)• un filtro è necessario perché le misure sono tipicamente
rumorose• La posizione del veicolo viene
usata per ridurre l’incertezza nelrilevamento dei landmark
yk,j
Controllo di Robot -- 110Cristian Secchi
• Xk: storia delle posizioni {x1,..,xk}
• mi: posizine del landmark i
• m: insieme dei landmark
yk-1,i
yk,i
Cristian Secchi Pag. 56
Il problema dello SLAMIl problema dello SLAM
• Dalla conoscenza delle misure Yk
• Determinare la posizione del veicolo Xk
• Costruire la mappa m delle posizioni dei landmark
• xk: posizione del veicolo all’stante k
• uk: l’ingresso di controllo applicato all’stante k-1e tenuto costanto nell’intervallo [k-1,k)
• yk: misura del landmark pressa allistante k
• Xk: storia delle posizioni {x1,..,xk}
Controllo di Robot -- 111Cristian Secchi
p { 1, , k}
• Uk: storia degli ingressi di controllo {u1,…,uk}
• Yk: storia delle misure {y1,…,yk}
• m: insieme dei landmark
SLAMSLAM
• La localizzazione e il mapping sono problemi correlati• due quantità incerte devono essere inferite tramite una sola
misura• Una soluzione può essere ottenuta solo se i problemi di
localizzazione e mapping sono considerati insiemelocalizzazione e mapping sono considerati insieme
Controllo di Robot -- 112Cristian Secchi
L’incertezza nella posizione si correla con l’incertezza della mappa!
Cristian Secchi Pag. 57
SLAM SLAM -- SettingSetting
• Un veicolo con un modello cinematico noto si muove in un ambiente popolato di landmark fissi (modello del processo)
• Il veicolo è dotato di un sensore che può misurare la posizione relativa tra ciascun landmark e il veicolo stesso (modello dell’osservazione)
Controllo di Robot -- 113Cristian Secchi
Modello del processoModello del processo
• Supponiamo, per ora, che il modello cinematico del robot sia un sistema lineare descritto da:
)()()1()( kwkukAxkx vvvv ++−=dove• A è la matrice di transizione dello stato• uv è il vettore degli ingressi• wv è un vettore aleatorio gaussiano con media nulla e matrice di
covarianza Qv(k) che modella l’incertezza del modello
• L’ i di t i i d ll t t il l d k i i è d t
Controllo di Robot -- 114
• L’equazione di transizione dello stato per il landmark i-esimo è data da:
Cristian Secchi
)1()( −= kpkp ii
Cristian Secchi Pag. 58
Modello del processoModello del processo
• E’ possibile raggruppare la dinamica del robot e la dinamica dei landmark in un unico modello del processo
• Lo stato del processo è dato da
• Il modello del processo p descritto da:
( )TTN
TT ppkxkxv
L1)()( =
)()()1(0
)(0
)()1()1(
0000
)()(
)( 111 11 kwkukAx
kwkukpkx
IA
kpkx
kx p
v
p
vv
p
v
++−=⎟⎟⎟⎞
⎜⎜⎜⎛
+⎟⎟⎟⎞
⎜⎜⎜⎛
+⎟⎟⎟⎞
⎜⎜⎜⎛
−−
⎟⎟⎟⎞
⎜⎜⎜⎛
=⎟⎟⎟⎞
⎜⎜⎜⎛
=L
L
Controllo di Robot -- 115
dove Ipi e 0pi sono rispettivamente la matrice identità e la matrice nulla di ordine pi
Cristian Secchi
)()()1(
00)1(00
)( kwkukAx
kpIp
kx
NNN ppNpN
++=
⎟⎟⎟
⎠⎜⎜⎜
⎝
+
⎟⎟⎟
⎠⎜⎜⎜
⎝
+
⎟⎟⎟
⎠⎜⎜⎜
⎝ −⎟⎟⎟
⎠⎜⎜⎜
⎝
=
⎟⎟⎟
⎠⎜⎜⎜
⎝
=MMM
L
MOMMM
Modello dell’OsservazioneModello dell’Osservazione
• Supponiamo, per ora, che i sensori che osservano i landmark siano lineari. Il modello dell’osservazione del i-esimo landmark è dato da:
)()()( kvkxCky iii +=• dove:
• vi(k) è un vettore aleatorio gaussiano con media nulla e matrice di covarianza Rv(k) che modella l’incertezza della misura
• Ci è la matrice di osservazione (cioè la matrice di uscita) che mette in relazione lo stato del processo con la misura relativa del landmark yi e può essere scritta come:
)()()( kvkxCky iii +
Controllo di Robot -- 116
• Il modello dell’osservazione può essere pertanto riscritto come
Cristian Secchi
( )0000 LLipi CAC −=
)()( kAxpCky vipi i−=
misura del landmarkrelativa alla posizione del veicolo
Cristian Secchi Pag. 59
SLAMSLAM
• Determinare la posizione del robot e quella dei landmark equivale a stimare lo stato del processo in base alle misure
• Si l di i d l h ll d ll i è tt i t• Sia la dinamica del processo che quella delle misure è caratterizzata da un’incertezza gaussiana
• Il Filtro di Kalman risolve il problema! E consente di risolverlo online!
Controllo di Robot -- 117Cristian Secchi
Il Filtro di Il Filtro di KalmanKalman
• Predizione• Stato Predetto
• Covarianza Predetta
)1()1|1(ˆ)1|(ˆ −+−−=− kukkxAkkx
)()1|1()1|( kQAkkAkk T +−−Σ=−Σ
• Aggiornamento• Innovazione
• Covarianza Innovazione
• G d di K l
)1|(ˆ)()( −−= kkxCkyke
)()1|()( kRCkkCkS T +−Σ=
1)()1|()( −Σ kSCkkkK T
Controllo di Robot -- 118Cristian Secchi
• Guadagno di Kalman
• Aggiornamento dello stato
• Aggiornamento Covarianza
)()1|()( −Σ= kSCkkkK
)()()1|(ˆ)|(ˆ kekKkkxkkx +−=
)1|())(()|( −Σ−=Σ kkCkKIkk
Cristian Secchi Pag. 60
Il filtro di Il filtro di KalmanKalman
(1) Predizione dello stato
(2) Predizione della covarianza)1()1(ˆ)(ˆ −+−=− kukxAkx
Predizione
( ))()1()( kQAkAk T +−Σ=Σ −
(1) Calcolo il guadagno di Kalman1))(()()( −−− +ΣΣ RCkCCkkK TT
Aggiornamento
Controllo di Robot -- 119Cristian Secchi
(2) Aggiorno la stima con la misura
(3) Aggiorno la covariaza
1))(()()( +ΣΣ= RCkCCkkK TT
)()()(ˆ)(ˆ kekKkxkx += −
−Σ−=Σ )())(()( kCkKIk)1()1(ˆ −Σ− kkx
Filtro di Filtro di KalmanKalman Esteso (Esteso (ExtendedExtended KalmanKalman FilterFilter –– EKF)EKF)
• Il filtro di Kalman può essere utilizzato solo se il modello del processo e il modello dell’osservazione sono sistemi lineari
• Il modello cinematico di un robot mobile (es : uniciclo automobile)• Il modello cinematico di un robot mobile (es.: uniciclo, automobile) sono nonlineari
• Il modello dell’osservazione spesso è nonlineare (es.: utilizzando scanner laser)
• Il Filtro di Kalman Esteso (EKF) permettere di superare l’ipotesi à
Controllo di Robot -- 120
di linearità sul processo e sulla misura
Cristian Secchi
Cristian Secchi Pag. 61
EKFEKF
• Il processo e le misure sono in realtà descrivibili da funzioni nonlineari:
⎨⎧ +−−= )())1(),1(()( kwkukxfkx
• dove w(k) e v(k) sono vettori aleatori di media nulla che descrivono l’incertezza di processo e di misura e le cui matrici di covarianza sono rispettivamente Q(k) e R(k)
⎩⎨ += )())(()(
)())()(()(kvkxhky
f
Controllo di Robot -- 121Cristian Secchi
EKFEKF
• L’idea che sta alla base dell’EKF è quella di approssimare il sistema il cui stato è da stimare linearizzandolo intorno allo stato stimato. Data la stima dello stato all’istante k-1, si linearizza il sistema intorno a quello stato e poi si stima lo stato all’istante k.
• Ad ogni istante si lavora su un sistema lineare e, quindi, è possibile utilizzare il filtro di Kalman per la stima dello stato
• Ad ogni istante, il sistema lineare che si considera è diverso perché si linearizza intorno a diversi punti di equilibrio
∂f
Controllo di Robot -- 122Cristian Secchi
))1(ˆ)1()(())1(,)1(ˆ())1(ˆ)1((|))1(,)1(ˆ())1(),1(( ))1(),1(ˆ( −−−+−−=−−−∂∂
+−−≈−− −− kxkxkFkukxfkxkxxfkukxfkukxf kukx
))(ˆ)()(())(ˆ())(ˆ)((|))(ˆ())(( ))(ˆ(−−−− −+=−
∂∂
+≈ − kxkxkHkxhkxkxxhkxhkxh kx
Cristian Secchi Pag. 62
L’algoritmo EKFL’algoritmo EKF
• Predizione• Stato Predetto
• Covarianza Predetta
))1(),1(ˆ()(ˆ −−=− kukxfkx
)()()1()()( kQkFkkFk T +−Σ=Σ −
• Aggiornamento• Innovazione
• Covarianza Innovazione
• G d di K l
))(ˆ()()( −−= kxhkyke
)()()()()( kRkHkkHkS T +Σ= −
1)()()()( −−Σ kSkHkkK T
Controllo di Robot -- 123Cristian Secchi
• Guadagno di Kalman
• Aggiornamento dello stato
• Aggiornamento Covarianza
1)()()()( Σ= kSkHkkK T
)()()(ˆ)(ˆ kekKkxkx += −
−Σ−=Σ )())()(()( kkHkKIk
EKF EKF –– LocalizzazioneLocalizzazione
⎪
⎪⎨
⎧
==
θθ
&
&
sincos
vyvx
Modello non lineare!
Un robot di tipo differential drive deve seguire una traiettoria circolare
⎪⎩ =ωθ&
Controllo di Robot -- 124Cristian Secchi
Un robot di tipo differential drive deve seguire una traiettoria circolare predefinita. L’inseguimento di traiettoria è implementato tramite IO-SFL ed è quindi necessario una misura della posizione del robot per costruire l’azione di controllo. Il modello cinematico del robot è affetto da una incertezza di processo. La posizione del robot è misurata da una telecamera e la misura è affetta da un’incertezza.
Cristian Secchi Pag. 63
Modello del processoModello del processo
⎪⎩
⎪⎨
⎧
==
ωθ
θθ
&
&
&
coscos
vyvx
⎪⎩
⎪⎨
⎧
++
+−−+−=+−−+−=
)()1()()(
)()1(sin)1()1()()()1(cos)1()1()(
kwkTkk
kwkkTvkykykwkkTvkxkx
y
x
ωθθ
θθ
⎩ =ωθ ⎩ +−+= )()1()()( kwkTkk θωθθ
01.0001.0
==
bT
θθ
sincos
byybxx
B
B
+=+=
)()(
2
1
Bdesdesdy
Bdesdesdx
yykyvxxkxv−+=−+=
&
&
Controllo di Robot -- 125Cristian Secchi
⎥⎦
⎤⎢⎣
⎡
⎥⎥⎦
⎤
⎢⎢⎣
⎡−=⎥
⎦
⎤⎢⎣
⎡
dy
dx
vv
bb
vθθθθ
ω cos1sin1sincos
30)cov( ==⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡Q
www
y
x
θ
Modello dell’OsservazioneModello dell’Osservazione
⎟⎟⎟
⎠
⎞
⎜⎜⎜
⎝
⎛+⎟⎟⎟
⎠
⎞
⎜⎜⎜
⎝
⎛=
)()()(
)()()(
)(kvkvkv
kkykx
ky y
x
θθ ⎟⎟⎟
⎠
⎞
⎜⎜⎜
⎝
⎛==
⎟⎟⎟
⎠
⎞
⎜⎜⎜
⎝
⎛
8.00008.00008.0
)())()()(
cov( kRkvkvkv
y
x
θ
• Tramite l’uscita ho un accesso diretto allo stato. • L’uscita è però caratterizzata da un’incertezza
Controllo di Robot -- 126Cristian Secchi
Cristian Secchi Pag. 64
RisultatiRisultati
Il robot si perde e la traiettoria l (bl ) è di d llreale (blu) è diversa da quella
desiderata (nero). I cerchi rossi indicano le letture del sensore.
Controllo di Robot -- 127Cristian Secchi
EKFEKF
• Utilizzando un EKF è possibile costruire una stima dello stato robusta rispetto alle incertezze
• Il modello del robot è non lineare e quindi devo costruire i Jacobianidel modello del processo e del modello dell’osservazione ad ognidel modello del processo e del modello dell osservazione ad ogni step
Controllo di Robot -- 128Cristian Secchi
Cristian Secchi Pag. 65
EKF PredizioneEKF Predizione
• Considero il modello del processo senza rumore
⎟⎟⎞
⎜⎜⎛
−−+−−−+−
=−−= )1(sin)1()1()1(cos)1()1(
))1(),1(()( kkTvkykkTvkx
kukxfkx θθ
• costruisco il Jacobiano di f calcolato in
⎟⎟
⎠⎜⎜
⎝ −+ )1()()()()())(),(()(
kTkyf
ωθ
)1(),1(ˆ −− kukx
111
sin01⎟⎞
⎜⎛ −
⎟⎟⎟⎞
⎜⎜⎜⎛
∂∂∂∂∂
∂∂
∂∂
Tvfff
fyf
xf
θθ
Controllo di Robot -- 129Cristian Secchi
))1(),1(ˆ|(333
222
100cos10)(
−−⎟⎟⎟
⎠⎜⎜⎜
⎝
=
⎟⎟⎟⎟⎟
⎠⎜⎜⎜⎜⎜
⎝ ∂∂
∂∂
∂∂
∂∂
∂∂
∂∂
=
kukx
Tv
fyf
xf
fyf
xfkF θ
θ
θ
EKF PredizioneEKF Predizione
• Predico lo stato utilizzando il modello e aggiorno la covarianza
))1(),1(ˆ()(ˆ −−=− kukxfkx
)()()1()()( kQkFkkFk T +−Σ=Σ −
Controllo di Robot -- 130Cristian Secchi
Cristian Secchi Pag. 66
EKF AggiornamentoEKF Aggiornamento
• Costruisco il Jacobiano della funzione di uscita
⎟⎟⎟⎞
⎜⎜⎜⎛
+⎟⎟⎟⎞
⎜⎜⎜⎛
== )()(
)()(
))(()( kvkv
kykx
kxhky y
x
⎟⎠
⎜⎝
⎟⎠
⎜⎝ )()( kvk θθ
))(ˆ|(333
222
111
100010001
)(−
⎟⎟⎟
⎠
⎞
⎜⎜⎜
⎝
⎛=
⎟⎟⎟⎟⎟⎟⎟⎞
⎜⎜⎜⎜⎜⎜⎜⎛
∂∂∂∂∂
∂∂
∂∂
∂∂
∂∂
∂∂
=
kxhhh
hyh
xh
hyh
xh
kHθ
θ
Controllo di Robot -- 131
• Costruisco l’innovazione e la sua covarianza
Cristian Secchi
⎟⎠
⎜⎝ ∂∂∂ yx θ
))(ˆ()()( −−= kxhkyke)()()()()( kRkHkkHkS T +Σ= −
EKF AggiornamentoEKF Aggiornamento
• Costruisco il guadagno di Kalman e aggiorno la stima e la sua covarianza
1)()()()( −−Σ= kSkHkkK T
)()()(ˆ)(ˆ kekKkxkx += −
−Σ−=Σ )())()(()( kkHkKIk
Controllo di Robot -- 132Cristian Secchi
Cristian Secchi Pag. 67
RisultatiRisultati
Il robot (blu) segue la traiettoria desiderata (nero)
La stima iniziale è molto incerta ma il filtro di Kalman
Controllo di Robot -- 133Cristian Secchi
e la stima della posizione del robot (verde) è accurata
la rende sempre più accurata.
CONTROLLO DI ROBOT INDUSTRIALILaurea Magistrale in Ingegneria Meccatronica
ROBOTICA MOBILEROBOTICA MOBILE
Ing. Cristian SecchiTel. 0522 522235
e-mail: secchi.cristian@unimore.ithttp://www.dismi.unimo.it/Members/csecchi