View
221
Download
0
Category
Tags:
Preview:
Citation preview
Pietro Salvagnini, 586929 pietro.salvagnini@gmail.com; Francesco Simmini, 586206 francesco.simmini@gmail.com; Michele Stoppa, 586204 stoppa.michele@gmail.com
Padova, 19 Febbraio 2009
Introduzione
ABSTRACTABSTRACT• navigazione autonoma di robot
in ambiente sconosciuto• N robot esploratori dotati di
una piccola telecamera• 1 attore cieco che conosce solo
la sua posizioneOBBIETTIVO:
• individuare un percorso dalla posizione iniziale dell'attore ad un obbiettivo noto, attraverso la costruzione di una mappa dell'ambiente.
E
E
A
Filosofia Progettuale
• Realismo: utilizzare tutti gli strumenti a disposizione permantenere il progetto il più generale possibile, limitandol’inserimento di sensori simulati;
• Funzionalità: creare un sistema effettivamente funzionante;
• Semplicità: cominciare da tecniche semplici, implementabilipraticamente, da raffinare solo dopo averneverificato il funzionamento per ottenere risultati;
Materiale a disposizione
• Calcolatore centrale
• Robot E-puck • Telecamera Logitech Quickcam
• Pedana di lavoro
3.20m
2.40m
Telecamera
E-puck !
Obstacles
Panoramica generale 1
IDEA DI BASEIDEA DI BASEGli ExplorerExplorerSiano [q θ] le coordinate dell’explorer1. Fare una foto e aggiornare la mappa {Mapping}2. Algoritmo di esplorazione: [poss θoss] {Exploration}3. Pianificare un percorso da q a poss {Path-
Planning}4. Muoversi con controllo in retroazione dagli encoder {Moto}5. Correggere la posizione tramite GPS virtuale {Re-locating}6. Girarsi in direzione θoss e ricominciare da 1. {Turning}Proseguire finché non avvista l’obbiettivo
Panoramica generale 2
IDEA DI BASEIDEA DI BASEL’ Actor Actor intanto deve:1. aspettare aggiornamenti della mappa {Wait}2. calcolare la strada più breve con l'informazione nota a
questa a punto, supponendo lo spazio ancora ignoto privo di ostacoli, e decidere se partire {Path-Planning}
3. se si parte, muoversi fino a dove la mappa è conosciuta {Moto}
Proseguire finché non giunge all’obbiettivo
Lo schema di controllo
ORGANIZZAZIONE:ORGANIZZAZIONE:
• Controllo centralizzatoControllo centralizzato– Il pc centrale organizzaIl pc centrale organizza
• Schema di controllo a statiSchema di controllo a stati– Uno stato per ogni robotUno stato per ogni robot– Un’azione per ogni statoUn’azione per ogni stato
• Ad ogni passo si eseguono Ad ogni passo si eseguono le le N+1N+1 azioni definite dagli azioni definite dagli stati dei robotstati dei robot
Stati:Stati:1. {Mapping}
• Foto• Costruzione mappa
2. {Exploration}• Scelta punto successivo
3. {Planning}• Trovare il percorso e pianificare
4. {Turning}5. {Re-locating}6. {Moving}
Lo schema di controllo
2. Exploration
3. Path-Planning4. Turning
4. Turning
4. Turning5. Moving1. Mapping
1. Mapping
2. Exploration
3. Path-Planning
4. Turning
5. Re-locating 4. Turning
Per pianificareSe si deve solo girare
Per prepararsi al moto
Se non si ha finito
Per spostarsiSe si ha finito il moto
6. Moving6. Moving5. Re-locating
Se non si ha finitoSe si ha finito
Lo schema di controllo
• La suddivisione è necessaria per poter tenere un tempo di campionamento basso
• Si dividono le operazioni onerose in più fasi• Nonostante tutto: T=2 sec !COMPLICAZIONICOMPLICAZIONI• Cono di visuale troppo stretto Cono di visuale troppo stretto 3 foto3 foto• Alcune operazioni possono prolungarsi Alcune operazioni possono prolungarsi si servono si servono prima prima i i
robot in robot in movimentomovimento
• Stati Stati Actor Actor leggermente diversileggermente diversi
MAPPINGCostruire mappa dell’ambiente attraverso le misure ottenute
dal sensore, la telecamera, posta sui robot esploratori:
• Rappresentazione della mappa
• Individuazione degli ostacoli
• Elaborazione delle misure
• Aggiornamento della mappa
MAPPING- Rappresentazione della mappa
Occupancy grid cells
Mappa = MatriceArea di 1 cm2 = una cella della mappa
P(x):Probabilità di occupazione = valore della cella
• 0 se la cella è libera• 1 se la cella è occupata• se la cella non è stata esplorata1
SENSORE UTILIZZATO: telecamera VGA a colori risoluzione 640 x 480
Processore capacità limitate: possibilità di utilizzo 320 x 320 bianco e nero, campionata 8:1, quindi con risoluzione effettiva 40 x 40.
Problematiche principali:• Adattare alcuni parametri lavorando direttamente sul processore• Tempi di trasmissione lunghi ridurre informazione trasmessa• Telecamera non adeguatamente fissata
MAPPING- Individuazione degli ostacoli
MAPPING- Individuazione degli ostacoli
MAPPA 2D Sufficiente individuare i bordi orizzontali.Pavimento chiaro ed ostacoli scuri.Per ogni colonna si parte dal basso e quando si
trovano due celle scure consecutive si segnala il bordo.
VANTAGGI:veloce e funzionale per il nostro problema:
si trasmette solo un vettore, non tutta l’immagine
SVANTAGGI:Taratura della soglia
UTILIZZO DELLA CAMERA
MAPPING- Elaborazione della misura
Obiettivo: definire una trasformazionepiano immagine piano su cui si muove il robot
¡ : [1;X p]£ [1;Yp] 7! R2
GEOMETRIA DELLA CAMERA
MAPPING- Elaborazione della misura
dh =
d¡ f cos ¯cos( ¯ ¡ ®)
yp cos®
®¼0d= f h
h¡ yp
Da h ed f, parametri della camera, ed y posizione nell’immagine si ottiene distanza ostacolo da camera
Infine conoscendo la posizione e l’orientazione del robot si ottiene una misura nel sistema
di riferimento assoluto
Riferimento piano immagine
Riferimento telecamera
MAPPING- Elaborazione della misura
' = arctanµ ¡
X p2 ¡ xp
¢
X p2
tan¡
µ2
¢¶
½= dcos'
»= arctan³
½sin '½cos ' +l
´
¾= ½cos ' +lcos»
Riferimento telecamera
Riferimento robot
RISULTATO:
MAPPING- Elaborazione della misura
MAPPING- Elaborazione della misura
CONSIDERAZIONI E PROBLEMATICHE:
• Scarsa precisione della misura sopra i 30 pixel, ottima per pixel vicini. Da considerarsi nella tecnica di aggiornamento
• Si è scelto di posizionare sempre l’ostacolo più vicino possibile al robot, all’interno della regione corrispondente a ciascun pixel
MAPPING- Esempio
Data una misura m(k) si vuole aggiornare la mappa M(k-1) ottenendo la mappa M(k):
MAPPING- Aggiornamento della mappa
©: (M (k ¡ 1);m(k)) 7! M (k)
• Celle di M(k-1) ed esplorate in m(k) prendono il valore che hanno in m(k)
•Celle occupate in M(k-1) e libere in m(k) diventano libere in M(k)
•Celle libere in M(k-1) ed occupate in m(k) restano libere in M(k)
Si fa così perché le misure sono molto conservative.
Posso solo vedere ostacoli dove non ci sono
1
Esplorazione
ESPLORAZIONE Assegnare ad ogni robot la successiva posizione di osservazione e la direzione verso cui scattare la foto successiva
1. Mirata ad individuare un percorso fino al goal2. Ridurre il tempo di esplorazione3. Ridurre lo spazio percorso
Esplorazione
• Gestire una visibilità limitata dei robot ( angolo di visibilità di 28° e lunghezza di visibilità di 40 cm)
• Garantire che i robot esplorino l’ambiente con percorsi possibilmente diversi per ottimizzare i tempi
• Gestire situazioni complicate, come strade chiuse, o degeneri come goal irraggiungibile,per esempio a causa di passaggi troppo stretti
PROBLEMI
Esplorazione
INPUT OUTPUT
ALGORITMO DI ESPLORAZIONE
pesp : punto di esplorazionesuccessivo#esp: angolo verso cui scattarela foto successiva
p : posizioneattualedel robot#:direzioneattualedel robotM : mappa del territoriod : destinazionedel cieco
EsplorazioneELABORAZIONE PRELIMINARE DELLA MAPPA PER RISOLVERE PROBLEMI DI DIMENSIONE DEI ROBOT
SOLUZIONE : ‘ORLARE’ LA MAPPA CIOÈ INGRANDIRE UN PO’ GLI OSTACOLI CON UN FILTRO UNIFORME.
EsplorazioneNELLA STESSA FASE SI ELABORA LA MAPPA AL FINE DI ELIMINARE CELLE INESPLORATE ISOLATE
EsplorazioneALGORITMO DI ESPLORAZIONE
1. Determinare l'insieme delle celle di frontiera F 2 M
2. Per ogni cella calcolare l'indice di costo J
3. Determinare c = argmin(J )f i 2 F
4. Se kc ¡ pk > dmin
² pesp = c;
² B = f (i; j ) 2 S(pesp;R) : M (i; j ) = 1 g
se B 6= ;
{ (m;n) = argmin(i ;j )2 B
k(i; j ) ¡ (d(1);d(2))k
{ #esp = arctan n¡ pesp (2)m¡ pesp (1)
altrimenti
{ #esp = arctan d(2)¡ pesp (2)d(1)¡ pesp (1)
altrimenti se la zona attorno non µe esplorata completamente
² pesp = p;
² #esp = #+ ®(k);
altrimenti
² A =f f i 2 F : kf i ¡ pk > dming
² pesp = argmin(J )f i 2A
² B = f (i; j ) 2 S(pesp;R) : M (i; j ) = 1 g
se B 6= ;
{ (m;n) = argmin(i ;j )2 B
k(i; j ) ¡ (d(1);d(2))k
{ #esp = arctan n¡ pesp (2)m¡ pesp (1)
altrimenti
{ #esp = arctan d(2)¡ pesp (2)d(1)¡ pesp (1)
Esplorazione
• Individuare le celle di frontiera ( frontier cells) tra la zona esplorata e quella sconosciuta, e dividerla in N parti dove N è il numero dei robot esploratori.
Esplorazione
• Si minimizza un indice di costo J:
– Distanza dal goal: si vuole che il robot scelga un punto di frontiera vicino al goal.– Distanza dalla posizione attuale: si desidera che il robot compi percorsi di lunghezza standard.– Distanza dalla posizione dell’altro robot: si vuole che i due robot esplorino zone diverse per ottimizzare i tempi.
J1
J3
J2
Esplorazione
² f i 2 insieme dei punti di frontiera
² d : punto di goal
² p : posizione attuale del robot
² dott : distanza ottima di movimento da p
² po : posizione altro robot
² do : distanza minima desiderata tra i due robot
² c1;c2;c3 : valore dei pesi delle tre componenti di J
J = J 1 + J 2 + J 3 = c1(kf i ¡ dk¡ minf j 2F
kf j ¡ dk) + c2(kf i ¡ pk¡ dott) + c3(kf i ¡ pok¡ do)2 ±¡ 1(¡ kf i ¡ pok+do)
INDICE J
EsplorazioneAndamento
dei tre indici
Esplorazione
² se kc¡ pk > dmin allora il robot si muove verso c che sarµa il punto suc-cessivo di esplorazione. Una volta raggiunto il punto c il robot fa la fotosuccessiva verso una zona inesplorata, cercando di puntare se possibileverso il goal
² sekc¡ pk < dmin allora preferiscerestarefermo egirareper fareuna fotoin una diversa direzione
² seil robot compieun giro intero senzamuoversi (cioµenon ha trovatopuntiottimi abbastanzadistanti) allorasi muoveversounpuntosu±cientementedistanteda p ma chegli permettedi trovarestradealternative
Determinato il punto c che minimizza l’indice J:
Esplorazione
Esplorazione
L’algoritmo di ricerca dei cammini
PROBLEMA:PROBLEMA:• individuare un percorso
libero tra due punti della mappa
• la mappa non è completa, si aggiorna
SOLUZIONE:SOLUZIONE:• ricerca di cammini minimi
su un grafografo [Dijkstra (1959)]• aggiornamento dinamico
del percorso algoritmo D*algoritmo D*
Serve un grafo!
Costruzione del grafo
SOLUZIONE SEMPLICE:SOLUZIONE SEMPLICE:10
14
L'algoritmo D* (D-star)[A. Stentz: An Optimal and Efficient Path-planning for Partially Known Enviroments]
IDEA:IDEA:• mantenere ottimi i nodi più mantenere ottimi i nodi più vicini al vicini al GoalGoal
• propagare i cambiamenti propagare i cambiamenti attraverso i attraverso i vicinivicini
F inchµe kmin < h(S)
1. X = argminZ2L , kmin = minX 2L k(X )
2. Se il passo 1. non porgeX , F INE: non esisteun percorso
3. EliminareX da L ; t(X ) = CLOSED
4. Se kmin < h(x), cioµeX µeun nodo RAISE
5. Per ogni Y vicino di X :Se h(Y ) · kmin eh(X ) > h(Y ) +cX Y ,allora b(X ) = Y ; h(X ) = h(Y ) +cX Y
6. Per ogni Y vicino di X :
7. Se t(Y ) = NEW ofb(Y ) = X eh(Y ) 6= h(X ) +cY X gallora b(Y ) = X ; Inserire(Y;h(X ) +cY X )
8. A ltrimenti9. Se b(Y ) 6= X e h(Y ) > h(X ) +cY X
allora Inserire(X ;h(X ))10. A ltrimenti se b(Y ) 6= X e h(X ) > h(Y ) +
+cX Y e t(Y ) = CLOSED e h(Y ) > kmin
allora Inserire(Y;h(Y ))
11. A ltrimenti (kmin = h(x), cioµeX µeun nodo LOWER)Per ogni Y vicino di X :se t(Y ) = NEW o fb(Y ) = X eh(Y ) 6= h(X ) +cY X go fb(y) 6= X e h(Y ) > h(X ) +cY X gallora b(Y ) = X ;Inserire(Y;h(X ) +cY X )
TECNICA:TECNICA:• puntatori puntatori per tenere traccia del per-corso
• lista apertalista aperta dei nodi non ottimi
• funzioni di costo attualecosto attuale e e costo costo minimominimo per mantenere l’ottimalità
D*: esempi (1)
• Risultato prima chiamata dell’algoritmo
• Scoperta celle occupate
• Inserimento nodi nella lista
• Propagazione ai nodi che li puntavano
D*: esempi (2)
RISULTATO:RISULTATO:• È stato trovato un
nuovo percorso
SE POI SI SCOPRE UNA SE POI SI SCOPRE UNA CELLA LIBERA:CELLA LIBERA:
• La si inserisce nella lista
D*: esempi (3)
RISULTATO:RISULTATO:• In un solo passo il
percorso si migliora!
Controllo della traiettoria
TECNICA MOLTO USATA IN LETTERATURATECNICA MOLTO USATA IN LETTERATURA• Dynamic Feedback Linearization
[De Luca, Oriolo, Venditelli; Wmr control via dynamic feedback linearization: Design, implementation, and experimental validation]
PROBLEMAPROBLEMA• Richiede traiettorie smooth per
il calcolo dell’azione di feedforward
PROBLEMAPROBLEMA• Sistema non lineare anolonomo:
il robot non si può spostare lateralmente
Il punto più avanti
SOLUZIONE SEMPLICESOLUZIONE SEMPLICE• Il punto B fuori
dall’asse delle ruote
può compiere cammini con velocità discontinua
xB = x +bcos(µ)
yB = y+bsin(µ)
Legge di controllo• Con le nuove coordinate il modello dell’uniciclo diventa
_xB = vcos(µ) ¡ ! bsin(µ) d= vdx
_yB = vsin(µ) + ! bcos(µ) d= vdy
_µ= !
_x = vcos(µ)
_y = vsin(µ)_µ= !
• Si può definire la legge di controllo statica·v!
¸=
·cos(µ) ¡ bsin(µ)sin(µ) bcos(µ)
¸¡ 1 ·vdx
vdy
¸=
·vdx cos(µ) +vdy sin(µ)1b(vy cos(µ) ¡ vdx sin(µ)
¸
• Con ingressi il sistema diventa lineare
• Si può poi introdurre un termine di retroazione ·vdx
vdy
¸!
·vdx +kx(rx(t) ¡ xB )vdy +ky(ry(t) ¡ yB )
¸
Velocità Velocità desideratadesiderata
Errore di Errore di posizioneposizione
LOCALIZZAZIONE
TECNICHE PER LA LOCALIZZAZIONE TECNICHE PER LA LOCALIZZAZIONE • Odometria
Errore di misura piccolo ma non scorrelato Già implementata, richiede solo comunicazione con robot Utilizzata per il controllo del moto
• GPS virtuale costituito da telecamera Errore di misura maggiore ma scorrelato Computazionalmente più oneroso. Effettuata in uno stato apposito al termine del moto
RILOCALIZZAZIONE – GPS virtuale
INDIVIDUAZIONE DEL ROBOT CERCATO:
Si prende lo sfondo f0
Si prende l’immagine attuale f
Si fa la differenza f0 -f
RILOCALIZZAZIONE
Costruzione di “e-puck” medio
Riduzione all’area diinteresse
FILTRAGGIOper ottenere punti a massima correlazione
Individuazione del 1o massimoAzzeramento ed Individuazione del 2o massimo
RILOCALIZZAZIONE – GPS virtuale
CORREZIONE DELL’ANGOLOCORREZIONE DELL’ANGOLOtramite 2 localizzazioni successive
Determinazione dell’angolo tra la posizione del robot prima della localizzazione e quella corretta: coincide con l’errore sull’orientazione iniziale
RISULTATI
Ringraziamenti
• Claudio Lora per la disponibilità in NAVLAB• Gio Cosi per l’utile consulenza• Antonio Simmini per la colorazione degli
Obstacles• Tommaso Stoppa per il montaggio Video• La famiglia Salvagnini per aver ospitato un
piccolo NAVLAB (il SALVLAB)
Recommended