AUTOLOCALIZZAZIONE
DI ROBOT MOBILI
Vincenzo Caglioti
V.Caglioti - Autolocalizzazione
- Richiami sugli stimatori
- Autolocalizzazione telemetrica
- Autolocalizzazione visiva
V.Caglioti - Autolocalizzazione
Stima a massima verosimiglianza
V.Caglioti - Autolocalizzazione
Weighted least squares
1
min Jp  X   (Jp  X)  (Jp  X)
T
p

T T 1
T T 1
Jp  X   p J  Jp  2p J  X  0
p
1
1
p  (J  J) J  X
T
-1 T
V.Caglioti - Autolocalizzazione
Covarianza
v   ( v, )
y  h( v )  
( v)  J( v  v)
con
y   (h( v), JJ )
T
V.Caglioti - Autolocalizzazione
h( v)
J
v v
Su X̂ agiscono errori di misura
ˆ   (0, )
XX
Sia
ˆ  g( p)
X
f (X/p )  e
un valore della misura X
compatibile con il modello p
1
 ( X g ( p )) T  1 ( X g ( p ))
2
V.Caglioti - Autolocalizzazione
f ( X/p ) f (p)
f (p/X ) 
f ( X)
a priori uniforme  costante con p
indipendente da p

Bayes
f (p/X )  f ( X/p )
f (p/X )  e
1
 ( g ( p )  X ) T  1 ( g ( p )  X )
2
V.Caglioti - Autolocalizzazione
g (p)  
( p )  J (p  p )
con
valore nominale
 da
f (p/X )  e
g (p)
J
p p
1
 ( g ( p )  X ) T  1 ( g ( p )  X )
2
1
 ( J ( p  p ) ( X g ( p )) T  1 ( J ( p  p ) ( X g ( p ))
2
f (p/X )  ke
Stima a massima verosimiglianza (ML)
max f (p/X )  min J(p  p )  ( X  g (p ))
p
p
V.Caglioti - Autolocalizzazione

Minimi quadrati pesati
ˆp  p  (J T  1J) -1 J T  1 ( X  g (p ))
Covarianza
isoliamo nell’esponente i termini “quadratici” in p
1
...p J  Jp...
T T
1
Var (p)  (J  J)
T
V.Caglioti - Autolocalizzazione
-1
Se i singoli errori di misurazione (es. sui punti)
sono indipendenti
1
1
0
 1 
pˆ  p  ( J i  i J i )
T
1
.
0
-1
i
.
0
J
T
i
0
0
.
.
1
. n
1
 i ( Xi  g i ( p ))
i
Var (p)  ( J i  i J i )
T
e
0 .
1
2
.
1
i
V.Caglioti - Autolocalizzazione
-1
Autolocalizzazione telemetrica
• scan matching
• stima incrementale
V.Caglioti - Autolocalizzazione
Scan Matching
Uso di telemetro orientabile
senza informazioni a priori
-Matching: generazione di un’ipotesi di stima
-Raffinamento della stima generata
V.Caglioti - Autolocalizzazione
Matching
E’ richiesta robustezza nei confronti di:
-rumore nelle misure
-outliers
-dati mancanti
Transformata di Hough Generalizzata (GHT)
con “footprints”
a) modellizzazione della mappa
b) matching tra misure e modello della mappa
ipotesi di stima di posizione e orientamento
V.Caglioti - Autolocalizzazione
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 intrinseci
delle singole footprint o dei gruppi di footprint,
per reperimento efficiente
V.Caglioti - Autolocalizzazione
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,q]
- per ogni valore di [x,y,q] compatibile con i vincoli
- incrementare il contatore di voti di [x,y,q]
- selezionare il valore di [x,y,q] più votato
V.Caglioti - Autolocalizzazione
Es. footprint
2 par. estrinseci + 0 par. intrinseci
2 par. estrinseci + 0 par. intrinseci
2 par. estrinseci + 1 par. intrinseco
3 par. estrinseci + 1 par. intrinseco
3 par. estrinseci + 1 o 2 par. intrinseci
V.Caglioti - Autolocalizzazione
Raffinamento della stima
pˆ  p  ( J i  i J i )
T
1
i
-1
J
T
i
1
 i ( Xi  g i ( p ))
i
ove
X i  zi
è il risultato della misura i-ma
(distanza tra R e ambiente lungo retta di misura fi)
gi (p)  zˆi
mentre g i (p )  zi
e
z
x
R
y
f
q
a
V.Caglioti - Autolocalizzazione
con
zˆi
Ji 
T
[ x, y,q ]
p  [ x , y ,q ]
T
p
cos a
sin a
Ji  [
,
, zi tan(q  f  a )]
cos(q  f  a ) cos(q  f  a )
Varianza
Var (p)  ( J i  i J i )
T
i
V.Caglioti - Autolocalizzazione
1
-1
Stima incrementale
Ciclo
Partenza:
movimento
T
ˆ
pˆ  [ xˆ , yˆ ,q ]
Movimento:
o misura odometrica
misurazione
e
  Var (pˆ )
T
ˆ
ˆ
ˆ
  [x, y, q ]
riferito alla posizione corrente
V.Caglioti - Autolocalizzazione
dunque va trasformato per rappresentarlo nel riferimento base
~
p '  pˆ  Rot (qˆ)
ove
cos qˆ  sin qˆ 0
Rot (qˆ)  sin qˆ cos qˆ 0
0
0
1
V.Caglioti - Autolocalizzazione
- errori odometrici :
(o di attuazione del movimento)
dp̂
- errori nella stima di partenza:
 loro effetto sull’errore in
d
~'
~ ' dp
p
~
dp '  dpˆ  dRot (qˆ)    Rot (qˆ)d
~ '  dpˆ  Rot' (qˆ)dqˆ  Rot (qˆ)d
dp
V.Caglioti - Autolocalizzazione
ove
 sin q
Rot' (q )  cosq
0
 cosq
 sin q
0
0
0
0
~
dp '  dpˆ  Rot' (qˆ)dqˆ  Rot (qˆ)d
~ '  Kdpˆ  Rot (qˆ)d
dp
con
1 0  (xˆ sin qˆ  yˆ cos qˆ)
K 0 1
xˆ cos qˆ  yˆ sin qˆ
0 0
0
V.Caglioti - Autolocalizzazione
Stima dopo il movimento (ma prima di ulteriori misure)
~
p '  pˆ  Rot (qˆ)
Varianza di tale stima
~
T
T
T
~
~
ˆ
ˆ
'  E[dp ' dp ' ]  KK  Rot (q )DRot (q )
ove
   (0, D)
V.Caglioti - Autolocalizzazione
ulteriore misura: stima ML equivale a stima di Kalman
~ T ~ T
1
~
p'  p ' ' J (J' J  ) (z - ~z ' )
ove z è il risultato effettivo della misura
mentre ~
z ' è il risultato che si otterrebbe:
-in assenza di errori
~
-se il modello vero fosse p '
Varianza della nuova stima ottenuta
~ ~ ~ T ~ T
1 ~
'  '' J (J' J  ) J'
V.Caglioti - Autolocalizzazione
Autolocalizzazione visiva
• “landmark” artificiali
• “landmark” naturali
V.Caglioti - Autolocalizzazione
V.Caglioti - Autolocalizzazione
Trasformazione scena immagine
X
x
y
c
f
O
principal axis
principal point
Y
u
X
image reference
- centered on upper left corner
- nonsquare pixels (aspect ratio)
Z
scene reference
- not attached to the camera
u  PX  M m X
V.Caglioti - Autolocalizzazione
Centro ottico
Spazio nullo destro della matrice di proiezione
PO  0
X  λA  O
u  PX  λPA  PO  λPA  PA
Per ogni A, tutti i punti in AO hanno la stessa
immagine di A, pertanto O è centro ottico
 o    M 1m 

O     

 1  1 
V.Caglioti - Autolocalizzazione
Direzione
degli infiniti punti aventi immagine u prendiamo
quello all’infinito:
questo punto dà la direzione della retta di
interpretazione del punto immagine u
d
u M m 
 M d
0
d  M 1  u
Telecamera: sensore di direzione
u d
V.Caglioti - Autolocalizzazione
Landmark artificiali
Hp: landmark planari
Cambio riferimento dal robot al piano contenente il landmark
X rob
u M m
R t

X lan
0 1
R t
X lan  M m
0 1
V.Caglioti - Autolocalizzazione
n o a t
X lan
0 0 0 1
sul piano del landmark, i suoi punti hanno Z=0
u M m
H M m
n o a t
0 0 0 1
X
X
Y
H Y
0
T
T
n o t
M n o t  0 0 m
0 0 1
H: omografia piano landmark  piano immagine
V.Caglioti - Autolocalizzazione
1) determinare omografia H (da n punti)
2) determinare n, o, t a partire da H
V.Caglioti - Autolocalizzazione
Determinazione dell’omografia H
• determinare un valore nominale da 4 degli n punti
risolvendo un’equazione lineare
• raffinare la stima usando gli n punti
h
H h
h
T
1
T
2
T
3
h1
 h  h2
h3
V.Caglioti - Autolocalizzazione
T
1
i
T
2
i
T
3
i
ui
h x
uˆ i  vi  Hxi  h x
wi
h x
Misure non rumorose
{
x i h1
 xi h 2 , i  1..n
T
xi h 3
T
ˆi
x
ˆ  g (h) 
X
i
i
yˆ i
T
con
T
T
u i x i h1
xi
xˆi 
 T  T
w i xi h 3
0
T
vi xi h 2
0T
yˆ i 
 T  T
w i xi h 3
0
V.Caglioti - Autolocalizzazione
0T 0T h
T
T
0 xi h
T
xi
0T
0T h
T
xi h
Linearizzazione attorno al valore nominale
..
xˆi
jacobiano
..
..

g i (h)
yˆ i
J  Ji 

h h
h
..
..
h
..
con
T
(xi h 3 )xi
1
Ji  T 2
(xi h 3 )
0T
T
0
 (xi h1 )xi
T
T
T
(xi h 3 )xi
V.Caglioti - Autolocalizzazione
T
T
 (xi h 2 )xi
T
T
Misure affette da rumore
..
ˆ , )
X  Xi  N ( X
..
Xi 
..
..
ˆ  X
ˆ  g (h)
con X
i
i
..
..
Stima ML (max likelihood)
e

xi
yi
1 0
  0 i
0 0
min X  g (h)
V.Caglioti - Autolocalizzazione
h
0
0
n

ˆ  (X  
XX
( h ))  J (h  h )
ˆ
min X  X
h


 min J(h  h )  (X  g (h )) 
h
stima
T
1
T
1
hˆ  h  ( J i  i J i ) -1  J i  i ( Xi  g i ( h ))
i
i
varianza
Var (h)  ( J i  i J i ) -1
T
1
i
V.Caglioti - Autolocalizzazione
Determinazione di n, o, t
da
sH  M n o t  0 0 m
n o t  M -1 ( sH  0 0 m )
• trovare s tale che |n|=1
• aggiustare n e o affinché siano consistenti con moto piano
- es. se il piano landmark è verticale
o verticale
n orizzontale
V.Caglioti - Autolocalizzazione
Landmark naturali
Es. spigoli verticali
xi
• telecamera prospettica:
piani di interpretazione verticali
ai
• telecamera catadiottrica (simmetrica
rispetto a un asse verticale)
rette radiali concorrenti
nell’immagine di un punto dell’asse
V.Caglioti - Autolocalizzazione
Aspetti geometrici
Luogo dei punti da cui due spigoli x1 e x2 si vedono sotto un angolo d:
x1
circonferenza
x2
d
d
Angolo al centro:
doppio dell’angolo
circonferenza
Con 3 spigoli: intersezione tra le due circonferenze
…1 soluzione valida
(si scarta quella costituita da uno spigolo comune)
V.Caglioti - Autolocalizzazione
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
V.Caglioti - Autolocalizzazione
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
V.Caglioti - Autolocalizzazione
Trasformata di Hough:
• discretizza in celle la parte navigabile dell’ambiente
• per le coppie di spigoli rilevate misura l’angolo di
• 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
V.Caglioti - Autolocalizzazione
Raffinamento della stima
V.Caglioti - Autolocalizzazione
..
..
Y  a i  N ( aˆ i , Y )
..
..
misure
1 0
Y  0  i
0 0
affette da errori indipendenti
x
riduzione delle incognite p 
y
..
..
X  d i  a i  a i 1
..
..
0
0
n
 nuove variabili misurate
1 1 0
0 1 1

.. .. ..
0 .. 0
0 0
0 0
Y
.. ..
1 1
H
V.Caglioti - Autolocalizzazione
1   2
 2
T
  H Y H 
0
..
misure non rumorose
con
cos dˆi (p) 
 2
 2  3
 3
..
0
 3
3   4
..
0
0
 4
..
0
0
0
..
..
..
..
ˆ  X
ˆ  g (p)  dˆ (p)
X
i
i
i
..
..
..
(p  xi ) T (p  xi 1 )
(p  xi ) T (p  xi ) (p  xi 1 ) T (p  xi 1 )
V.Caglioti - Autolocalizzazione
..
dˆi
J  Ji 
p
..
..
..
Linearizzazione
tramite jacobiano
ˆ
min X  X
p
Stima ML
Varianza

p
..
1  cos dˆi
 
sin dˆi p
..
 min J(p  p )  ( X  g (p ))
p

pˆ  p  (J T  1J) -1 J T  1 ( X  g (p ))
Var (p)  (J T  1J) -1
V.Caglioti - Autolocalizzazione
p
Recupero di q
dalla posizione p=[x,y]T stimata
utilizzare l’infomazione circa un angolo a rilevato
y: orientamento del segmento tra p e lo spigolo considerato
q y a
oppure utilizzare una media tra gli angoli rilevati
V.Caglioti - Autolocalizzazione