AUTOLOCALIZZAZIONE DI ROBOT MOBILI

Vincenzo Caglioti

- Autolocalizzazione telemetrica

- Autolocalizzazione visiva

- Richiami sugli stimatori

Stima a massima verosimiglianza

)J()J(Jmin 1T XpXpXpp

Weighted least squares

0J2JJJ 1TT1TT

XpppXp

p

Xp 1T11T JJ)J( -

Covarianza

),( vv

)(J)()( vvvvy h con

vv

v

)(

Jh

)JJ),(( T vy h

Su agiscono errori di misuraX̂

),0(ˆ XX

))(())((2

1 1T

)(pXpX

X/pgg

ef

)g(ˆ pX Sia un valore della misura Xcompatibile con il modello p

))(())((2

1 1T

)(XpXp

p/X

gg

ef

)(

)()()(

X

pX/pp/X

f

fff Bayes

indipendente da p a priori uniforme costante con p

)()( X/pp/X ff

)(J)()( pppp gp

p

p

)(

Jg

con

))(())((2

1 1T

)(XpXp

p/X

gg

ef da

Stima a massima verosimiglianza (ML)

))(()(J())(()(J(2

1 1T

)(pXpppXpp

p/Xgg

kef

))(()(Jmin)(max pXppp/X

ppgf

valore nominale

))((JJ)J(ˆ 1T11T pXpp g-

Minimi quadrati pesati

Covarianza

isoliamo nell’esponente i termini “quadratici” in p

...JJ... 1TT pp

11T J)J()(Var -p

Se i singoli errori di misurazione (es. sui punti) sono indipendenti

1

12

11

1

.00

....

0.0

0.0

n

i

1T11T ))((J)JJ(ˆ pXpp iiii-

iiii g

11T )JJ()(Var -

iiii pe

Autolocalizzazione telemetrica

• scan matching

• stima incrementale

Scan Matching

Uso di telemetro orientabile senza informazioni a priori

-Matching: generazione di un’ipotesi di stima

-Raffinamento della stima generata

Matching

Transformata di Hough Generalizzata (GHT) con “footprints”

a) modellizzazione della mappab) matching tra misure e modello della mappa

E’ richiesta robustezza nei confronti di:-rumore nelle misure-outliers-dati mancanti

ipotesi di stima di posizione e orientamento

Modellizzazione:

Footprint: features “locali” caratterizzate da- parametri intrinseci (es. angoli, distanze, curvature)- parametri estrinseci (es. posizione e orientamento)

eventualmente raggruppate (ad es. se mancano parametri intrinseci)

ordinate in base ai parametri intrinsecidelle singole footprint o dei gruppi di footprint,per reperimento efficiente

Matching tra misure e modello della mappa

-costruzione delle feature dalle misurazioni

- per ciascuna feature (o ciascun gruppo)- selezionare footprint candidate esplorando la struttura dati sulla base dei parametri intrinseci - per ogni footprint candidata

- utilizzare parametri estrinseci per vincolare [x,y,]- per ogni valore di [x,y,] compatibile con i vincoli

- incrementare il contatore di voti di [x,y,]

- selezionare il valore di [x,y,] più votato

Es. footprint

2 par. estrinseci + 1 par. intrinseco

2 par. estrinseci + 0 par. intrinseci

3 par. estrinseci + 1 par. intrinseco

3 par. estrinseci + 1 o 2 par. intrinseci

2 par. estrinseci + 0 par. intrinseci

Raffinamento della stima

i

1T11T ))((J)JJ(ˆ pXpp iiii-

iiii g

ove ii zX è il risultato della misura i-ma

(distanza tra R e ambiente lungo retta di misura i)

z

R

y

x

e ii zg ˆ)( p

mentre ii zg )(p

pT],,[

ˆJ

yxzi

i

T],,[ yxp

)]tan(,)cos(

sin,

)cos(

cos[J

ii z

Varianza

con

11T )JJ()(Var -

iiii p

Stima incrementale

Ciclo movimento misurazione

Partenza: T]ˆ,ˆ,ˆ[ˆ yxp )ˆ(Var pe

Movimento:o misura odometrica

T]ˆ,ˆ,ˆ[ yx

riferito alla posizione corrente

dunque va trasformato per rappresentarlo nel riferimento base

)ˆ(Rotˆ'~ pp

100

0ˆcosˆsin

0ˆsinˆcos

)ˆ(Rot

ove

- errori odometrici : (o di attuazione del movimento)

- errori nella stima di partenza:

loro effetto sull’errore in '~p

d

p̂d

'~pd

dddd )ˆ(Rot)ˆ(Rotˆ'~ pp

dddd )ˆ(Rotˆ)ˆ(Rot'ˆ'~ pp

000

0sincos

0cossin

)(Rot'

ove

dddd )ˆ(Rotˆ)ˆ(Rot'ˆ'~ pp

ddd )ˆ(RotˆK'~ ppcon

000

ˆsinˆˆcosˆ10

)ˆcosˆˆsinˆ(01

yx

yx

K

Stima dopo il movimento (ma prima di ulteriori misure)

)ˆ(Rotˆ'~ pp

Varianza di tale stima

TTT )ˆ(DRot)ˆ(RotKK]'~'~['~ pp ddE

ove )D,0(

ulteriore misura: stima ML equivale a stima di Kalman

'~

J)J'~

J(J'~

'~

'~ 1TT

)'z~-(z)J'~

J(J'~

'~' 1TT pp

ove z è il risultato effettivo della misuramentre 'z~ è il risultato che si otterrebbe:

-in assenza di errori-se il modello vero fosse '~p

Varianza della nuova stima ottenuta

Autolocalizzazione visiva

• “landmark” artificiali

• “landmark” naturali

O

Z

YX

c

yx

f

scene reference- not attached to the camera

image reference- centered on upper left corner- nonsquare pixels (aspect ratio)

Trasformazione scena immagine

principal axisprincipal point

XmXu MP

u

X

Centro ottico

0P O

Spazio nullo destro della matrice di proiezione

OλAX

PAλPAPλPAPXu O

Per ogni A, tutti i punti in AO hanno la stessa immagine di A, pertanto O è centro ottico

1

M

1

1moO

dMd

mMu 0

uMd 1

Direzione

degli infiniti punti aventi immagine u prendiamo quello all’infinito:

questo punto dà la direzione della retta di interpretazione del punto immagine u

Telecamera: sensore di direzione

u d

Landmark artificiali

Hp: landmark planari

Cambio riferimento dal robot al piano contenente il landmark

lanlan Xtaon

mXtR

mu1000

M10

M

lanrob XtR

X10

T

Y

X

T

Y

X

H01000

M taon

mu

sul piano del landmark, i suoi punti hanno Z=0

H: omografia piano landmark piano immagine

mtonton

m 00M100

MH

1) determinare omografia H (da n punti)

2) determinare n, o, t a partire da H

Determinazione dell’omografia H

• determinare un valore nominale da 4 degli n punti risolvendo un’equazione lineare

• raffinare la stima usando gli n punti

3

2

1

T3

T2

T1

h

h

h

h

h

h

H h

ni

i

i

i

..1,

h

h

h

h

h

h

H

w

v

u

ˆ

3T

i

2T

i

1T

i

iT

3

iT

2

iT

1

ii x

x

x

x

x

x

xu

Misure non rumorose

hx

hx

x

xT

iTT

TTTi

3T

i

1T

ii

00

00

h

h

w

uˆ

i

ix

hx

hx

x

xT

iTT

TTi

T

3T

i

2T

ii

00

00

h

h

w

vˆ

i

iy

i

i

ˆ

ˆ)(ˆ

y

xgii hX

con {

Ti2

Ti

Ti3

Ti

T

Ti1

Ti

TTi3

Ti

23

Ti

i)h()h(0

)h(0)h(

)h(

1J

xxxx

xxxx

x

..

ˆ

ˆ..

..

)(..

..

J

..

J i

h

h hh

h

i

i

i y

x

gjacobiano

con

Linearizzazione attorno al valore nominale

i

ii y

xX

Misure affette da rumore

),ˆ(

..

..

i XXX N

..

)(

..

..

ˆ..

ˆii hXX gcon e

n

i

00

00

001

)(min hX

hgStima ML (max likelihood)

))(()(Jmin hXhh

hg

XX

h

ˆmin

)(J))((ˆ hhhXXX

i

1T11T ))((J)JJ(ˆ hXhh iiii-

iiii g

stima

varianza

11T )JJ()(Var -

iiii h

mton 00MH s

)00H(M 1 mton s-

da

• trovare s tale che |n|=1• aggiustare n e o affinché siano consistenti con moto piano

- es. se il piano landmark è verticaleo verticale n orizzontale

Determinazione di n, o, t

Landmark naturali

Es. spigoli verticali

• telecamera prospettica:piani di interpretazione verticali

• telecamera catadiottrica (simmetrica rispetto a un asse verticale)

rette radiali concorrenti nell’immagine di un punto dell’asse

ii

xi

Aspetti geometrici

Luogo dei punti da cui due spigoli x1 e x2 si vedono sotto un angolo

Angolo al centro:doppio dell’angolocirconferenza

x1 x2

Con 3 spigoli: intersezione tra le due circonferenze…1 soluzione valida (si scarta quella costituita da uno spigolo comune)

circonferenza

con n>3 spigoli e misure angolari affette da rumore

Problema delle corrispondenze spigoli misurati spigoli nella mappa

Alternative: • Ransac (random sample consensus)• Trasformata di Hough

Valore di partenza per successivo raffinamento della stima

Ransac:

• scegli a caso un certo numero di terne di spigoli misurati• per ciascuna terna scegli un certo numero di terne di spigoli modello• per ogni corrispondenza (terna-misurata, terna-modello) scelta

• calcola p=[x,y]T intersecando due circonferenze• valuta il consenso della corrispondenza come numero delle altre misure compatibili con la p calcolata

• scegli la coppia di terne corrispondenti con maggior consenso• assegna la corrispondente p al valore di partenza per il raffinamento

Trasformata di Hough:

• discretizza in celle la parte navigabile dell’ambiente• per le coppie di spigoli rilevate misura l’angolo i

• per ciascuna coppie di spigoli rilevata• per ciascuna coppia di spigoli della mappa

• costruisci la circonferenza da cui la coppia è vista sotto l’angolo misurato• per ogni cella attraversata dalla circonferenza

• incrementa il contatore dei voti• determina la cella col massimo numero di voti• assegna il centro della cella al valore di partenza per il raffinamento

Raffinamento della stima

),

..

ˆ

..

(

..

..

i YiN Ymisure

affette da errori indipendenti

n

iY

00

00

001

riduzione delle incognite

YX

110..0

..........

00110

00011

..

..

..

..

1iii

y

xp nuove variabili misurate

H

..........

00

00

000

HH4433

3322

221

TY

..

)(ˆ..

..

)(

..

..

ˆ..

ˆiii ppXX gmisure non rumorose

con)()()()(

)()()(ˆcos

1iT

1iiT

i

1iT

ii

xpxpxpxp

xpxpp

pp

pp..

ˆcosˆsin

1..

..

ˆ..

..

J

..

J i

i

i

i

Linearizzazionetramite jacobiano

XX

p

ˆmin ))(()(Jmin pXpp

pg

))((JJ)J(ˆ 1T11T pXpp g- Stima ML

Varianza11T J)J()(Var -p

Recupero di

dalla posizione p=[x,y]T stimatautilizzare l’infomazione circa un angolo rilevato

: orientamento del segmento tra p e lo spigolo considerato

oppure utilizzare una media tra gli angoli rilevati