v.caglioti - autolocalizzazione autolocalizzazione di robot mobili vincenzo caglioti
TRANSCRIPT
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