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)