Pietro Salvagnini,
Francesco Simmini,
Michele Stoppa,
586929
586206
586204
[email protected];
[email protected];
[email protected]
Padova, 19 Febbraio 2009
Introduzione
ABSTRACT
• navigazione autonoma di robot
in ambiente sconosciuto
• N robot esploratori dotati di
una piccola telecamera
• 1 attore cieco che conosce solo
la sua posizione
OBBIETTIVO:
• 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 per
mantenere il progetto il più generale possibile, limitando
l’inserimento di sensori simulati;
• Funzionalità: creare un sistema effettivamente funzionante;
• Semplicità: cominciare da tecniche semplici, implementabili
praticamente, da raffinare solo dopo averne
verificato il funzionamento per ottenere risultati;
Materiale a disposizione
• Calcolatore centrale
• Pedana di lavoro
Obstacles
2.40m
3.20m
• Robot E-puck
• Telecamera Logitech Quickcam
Telecamera
E-puck !
Panoramica generale 1
IDEA DI BASE
Gli Explorer
Siano [q θ] le coordinate dell’explorer
1. 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 BASE
L’ 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:
• Controllo centralizzato
– Il pc centrale organizza
• Schema di controllo a stati
– Uno stato per ogni robot
– Un’azione per ogni stato
• Ad ogni passo si eseguono
le N+1 azioni definite dagli
stati dei robot
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
1. Mapping
2. Exploration
2. Exploration
3. Path-Planning
4. Turning
3. Path-Planning
4. Turning
Per prepararsi al moto
Se non si ha finito
4. Turning
4. Turning
5. Moving
1. Mapping
5. Re-locating
4. Turning
6. Moving
5. Re-locating
6. Moving
Per pianificare
Se si deve solo girare
Per spostarsi
Se si ha finito il moto
Se si ha finito
Se non 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 !
COMPLICAZIONI
• Cono di visuale troppo stretto  3 foto
• Alcune operazioni possono prolungarsi  si servono prima i
robot in movimento
• Stati Actor leggermente diversi
MAPPING
Costruire 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 = Matrice
Area di 1 cm2 = una cella della mappa
P(x):Probabilità di occupazione = valore della cella
• 0
• 1
• 1
se la cella è libera
se la cella è occupata
se la cella non è stata esplorata
MAPPING- Individuazione degli ostacoli
SENSORE UTILIZZATO: telecamera VGA a colori risoluzione 640 x 480
mProcessore 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 mprocessore
• Tempi di trasmissione lunghi
ridurre informazione trasmessa
• Telecamera non adeguatamente fissata
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
MAPPING- Elaborazione della misura
UTILIZZO DELLA CAMERA
Obiettivo: definire una trasformazione
piano immagine
piano su cui si muove il robot
¡ : [1; X
p]
£ [1; Yp ] 7
!
R2
MAPPING- Elaborazione della misura
GEOMETRIA DELLA CAMERA
d
h
=
d¡ f
cos ¯
c o s( ¯ ¡ ® )
®¼ 0
d =
f h
h ¡ yp
y p co s ®
Da h ed f, parametri della camera, ed y posizione nell’immagine
si ottiene distanza ostacolo da camera
MAPPING- Elaborazione della misura
Riferimento piano immagine
Riferimento telecamera
µ ¡
' =
½=
¢
Riferimento telecamera
Riferimento robot
¶
³
´
¡ la
¢
Infine conoscendo
posizione
e
l’orientazione
¡ x
½si
n
'
= ar ct an ½c o s ' + l
ar ct an
t an µ2
del robot si ottiene una misura» nel
sistema
½cos ' + l
di riferimento assoluto
d
¾=
cos »
X p
2
X p
2
cos '
p
MAPPING- Elaborazione della misura
RISULTATO:
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
MAPPING- Aggiornamento della mappa
Data una misura m(k) si vuole aggiornare la mappa M(k-1)
ottenendo la mappa M(k):
© : (M (k ¡
1) ; m ( k ) ) 7
!
M (k)
• Celle 1 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
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 goal
2. Ridurre il tempo di esplorazione
3. Ridurre lo spazio percorso
Esplorazione
PROBLEMI
• 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
Esplorazione
ALGORITMO DI ESPLORAZIONE
INPUT
p : p osi zi on e at t u al e d el r ob ot
# :d i r ezi on e at t u al e d el r ob ot
M : m ap p a d el t er r i t or i o
d : d est i n azi on e d el ci eco
OUTPUT
p es p : p u nt o d i esp l or azi on e
su ccessi v o
# e s p : an gol o v er so cu i scat t ar e
l a f ot o su ccessi va
Esplorazione
ELABORAZIONE PRELIMINARE DELLA MAPPA PER RISOLVERE
PROBLEMI DI DIMENSIONE DEI ROBOT
SOLUZIONE : ‘ORLARE’ LA MAPPA CIOÈ INGRANDIRE UN PO’ GLI
OSTACOLI CON UN FILTRO UNIFORME.
Esplorazione
NELLA STESSA FASE SI ELABORA LA MAPPA AL FINE DI
ELIMINARE CELLE INESPLORATE ISOLATE
Esplorazione
ALGORITMO DI
ESPLORAZIONE
1 . D et er m in ar e l ' in siem e d el l e cel l e d i f r o n t i er a F 2 M
2 . P er o g n i cel l a calcol ar e l' i n d i ce d i co st o J
3 . D et er m in ar e c = a r gm i n ( J )
f i 2 F
4 . Se k c ¡
pk > d m
in
²
p e s p = c;
²
B = f ( i ; j ) 2 S ( pes p ; R ) : M ( i ; j ) = 1 g
se B 6
= ;
{
( m ; n ) = ar gm i n k ( i ; j ) ¡
( d( 1) ; d( 2) ) k
( i ;j ) 2 B
{
# e s p = a r ct a n
n ¡ pes p ( 2)
m ¡ pes p (1)
al t r i m en t i
{
# e s p = a r ct a n
d( 2) ¡ pes p ( 2)
d( 1) ¡ pes p ( 1)
al t r i m en t i se la zo n a at t or n o n o n µ
e esp l or at a co m p l et am en t e
²
pe s p = p;
²
# e s p = # + ®( k ) ;
al t r i m en t i
²
A = f f
²
pe s p = ar gm i n ( J )
²
B = f ( i ; j ) 2 S ( pes p ; R ) : M ( i ; j ) = 1 g
i
2 F : kf
i
¡
pk > dm
in
g
f i 2 A
se B 6
= ;
{
( m ; n ) = ar gm i n k ( i ; j ) ¡
( d( 1) ; d( 2) ) k
( i ;j ) 2 B
{
# e s p = a r ct a n
n ¡ pes p ( 2)
m ¡ pes p (1)
al t r i m en t i
{
# e s p = a r ct a n
d( 2) ¡ pes p ( 2)
d( 1) ¡ pes p ( 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
J2
J3
Esplorazione
INDICE J
J = J 1 + J 2 + J 3 = c1 ( kf i ¡ dk¡ m in kf j ¡ dk) + c2 ( kf i ¡ pk¡ d ot t ) + c3 ( kf i ¡ po k¡ d o ) 2 ±¡
f
j
2F
²
f
²
d : pun t o di goal
²
p : posi zi on e at t u ale del r obot
²
d o t t : di st an za ot t i m a di m ovi m en t o da p
²
p o : posi zi on e al t r o r obot
²
d o : di st an za m i n i m a desi der at a t r a i du e r obot
²
c1 ; c2 ; c3 : valor e dei pesi del le t r e com pon en t i di J
i
2 i n si em e dei pu n t i di f r on t i er a
1(¡
kf i ¡ po k+ d o )
Esplorazione
Andamento
dei tre indici
Esplorazione
Determinato il punto c che minimizza l’indice J:
²
se k c ¡ pk > d m i n al l or a i l r ob ot si m u ov e v er so c ch e sar µ
a i l p u nt o su ccessi v o d i esp l or azi on e. U n a v ol t a r aggi u nt o i l p u nt o c i l r ob ot f a l a f ot o
su ccessi va v er so u n a zon a i n esp l or at a, cer can d o d i p u nt ar e se p ossi b i l e
v er so i l goal
²
se k c ¡ pk < d m i n al l or a p r ef er i sce r est ar e f er m o e gi r ar e p er f ar e u n a f ot o
i n u n a d i v er sa d i r ezi on e
²
se i l r ob ot com p i e u n gi r o i nt er o sen za m u ov er si ( ci oµ
e n on h a t r ovat o p u nt i
ot t i m i ab b ast an za d i st ant i ) al l or a si m u ov e v er so u n p u nt o su ± ci ent em ent e
d i st ant e d a p m a ch e gl i p er m et t e d i t r ovar e st r ad e al t er n at i v e
Esplorazione
Esplorazione
L’algoritmo di ricerca dei cammini
PROBLEMA:
• individuare un percorso
libero tra due punti della
mappa
• la mappa non è completa, si
aggiorna
SOLUZIONE:
• ricerca di cammini minimi
su un grafo [Dijkstra (1959)]
• aggiornamento dinamico
del percorso
algoritmo D*
Costruzione del grafo
SOLUZIONE SEMPLICE:
10
14
L'algoritmo D* (D-star)
[A. Stentz: An Optimal and Efficient Path-planning for Partially Known Enviroments]
F i n ch µ
e km in < h(S)
TECNICA:
• puntatori per tenere traccia del percorso
• lista aperta dei nodi non ottimi
• funzioni di costo attuale e costo
minimo per mantenere l’ottimalità
IDEA:
• mantenere ottimi i nodi più
vicini al Goal
• propagare i cambiamenti
attraverso i vicini
1. X = ar g m i n Z 2 L , k m i n = m i n X
2L
k(X )
2. Se i l p asso 1. n on p or ge X , F I N E : n on esi st e u n p er cor so
3. E l i m i n ar e X d a L ; t ( X ) = C L O S E D
4. Se k m i n < h ( x ) , ci oµ
eX µ
e u n n od o R A I SE
5. P er o g n i Y v i ci n o d i X :
Se h ( Y ) · k m i n e h ( X ) > h ( Y ) + cX
a l l o r a b( X ) = Y ; h ( X ) = h ( Y ) + c X
Y
,
Y
6. P er o g n i Y v i ci n o d i X :
7. Se t ( Y ) = N E W o
f b( Y ) = X e h ( Y ) 6
= h ( X ) + cY X g
a l l o r a b( Y ) = X ; I n ser i r e( Y; h ( X ) + c Y X )
8. A l t r i m en t i
9. S e b( Y ) 6
= X e h ( Y ) > h ( X ) + cY X
a l l o r a I n ser i r e( X ; h ( X ) )
10. A l t r i m en t i se b( Y ) 6
= X e h(X ) > h(Y ) +
+ cX Y e t ( Y ) = C L O S E D e h ( Y ) > k m i n
a l l o r a I n ser i r e( Y; h ( Y ) )
11. A l t r i m en t i
( k m i n = h ( x ) , ci oµ
eX µ
e u n n od o L O W E R )
P er o g n i Y v i ci n o d i X :
se t ( Y ) = N E W o f b( Y ) = X e h ( Y ) 6
= h ( X ) + cY X g
o f b( y ) 6
= X e h ( Y ) > h ( X ) + cY X g
a l l o r a b( Y ) = X ;I n ser i r e( Y; h ( X ) + c Y X )
D*: esempi
• Risultato prima
chiamata
dell’algoritmo
• Scoperta celle
occupate
• Inserimento nodi nella
lista
• Propagazione ai nodi
che li puntavano
(1)
D*: esempi
RISULTATO:
• È stato trovato un
nuovo percorso
SE POI SI SCOPRE UNA
CELLA LIBERA:
• La si inserisce nella
lista
(2)
D*: esempi
RISULTATO:
• In un solo passo il
percorso si migliora!
(3)
Controllo della traiettoria
PROBLEMA
• Sistema non lineare anolonomo:
il robot non si può spostare lateralmente
TECNICA MOLTO USATA IN LETTERATURA
• Dynamic Feedback Linearization
[De Luca, Oriolo, Venditelli; Wmr control via dynamic
feedback linearization: Design, implementation, and
experimental validation]
PROBLEMA
• Richiede traiettorie smooth per
il calcolo dell’azione di
feedforward
Il punto più avanti
SOLUZIONE SEMPLICE
•
Il punto B fuori
dall’asse delle ruote
x B = x + b cos( µ)
y B = y + b si n ( µ)
può compiere
cammini con
velocità discontinua
Legge di controllo
• Con le nuove coordinate il modello dell’uniciclo diventa
x_ = v cos( µ)
x_B = v cos( µ) ¡
y_ = v si n ( µ)
d
! b si n ( µ) = v d x
d
y_B = v si n ( µ) + ! b cos( µ) = v d y
µ_ = !
µ_ = !
• Si può definire la legge di controllo statica
·
¸
v
!
·
=
¸
cos( µ)
si n ( µ)
¡ b si n ( µ)
b cos( µ)
• Con ingressi
¡ 1
·
¸
vd x
vd y
·
=
¸
v d x cos( µ) + v d y si n ( µ)
( v y cos( µ) ¡ v d x si n ( µ)
1
b
il sistema
diventa lineare
• Si può poi introdurre un termine di retroazione
·
Velocità
desiderata
¸
vd x
vd y
·
!
¸
vd x + k x ( r x ( t ) ¡
vd y + k y ( r y ( t ) ¡
xB )
yB )
Errore di
posizione
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
Riduzione
all’area di
interesse
Costruzione di
“e-puck” medio
FILTRAGGIO
per ottenere punti a massima correlazione
Azzeramento ed
Individuazione
del 1o massimo
Individuazione
del 2o massimo
RILOCALIZZAZIONE – GPS virtuale
CORREZIONE DELL’ANGOLO
tramite 2 localizzazioni successive
Determinazione dell’angolo
Dq 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)