Documenti di Didattica
Documenti di Professioni
Documenti di Cultura
Vetor de localizao
do rob no instante
:
Conjunto de localizaes
Vetor
Vetor
Vetor de posio
do marco no instante
Vetor
Matriz de erros do sensor
:
Conjunto de observaes realizadas desde o instante inicial de navegao
Observao incompleta
nmero do conjunto
,
,
Observao completa
,
,
,
nmero do conjunto
,
,
Distncia entre
e
,
,
,
Vetor
,
,
com a do marco
,
Jacobiano de
,
,
,
,
Vetor de diferenas entre
,
,
e
,
,
Ganho de Kalman
Vetor de controles
:
Vetors de controles fornecidos desde o instante inicial de navegao
no tempo
Varivel
estar ocupada
Varivel
,
,
,
Vetor descritor de
, Valor do pixel de coordenadas , na imagem
, , Mscara gaussiana de centro , e desvio padro
, , Pixel , suavizado gaussianamente por , ,
, , Diferena de gaussianas no espao de escalas
Coordenadas
de um candidato a caracterstica
Coordenadas
do candidato a caracterstica
, Orientao de uma caracterstica
, Magnitude de uma caracterstica
(2.1)
Essa propagao da localizao no tempo no leva em considerao fenmenos imprevisveis,
como deslizamento de rodas e terrenos irregulares, e por isso os seus resultados so apenas
uma estimativa do estado atual do rob naquele instante. Essa incerteza pode ser modelada
com uma distribuio de probabilidades
j lhe so conhecidas,
possvel determinar qual a sua estimativa de posio a partir dos valores
for-
necidos pelas observaes que foram correspondidas com os marcos. A posio desses
marcos no ambiente no depende de valores de localizao do rob, e por isso a estimativa
obtida ser absoluta, limitada apenas pela incerteza relativa posio do marco no ambiente e
do sensor utilizado para realizar a observao. Essa estimativa pode ser incorporada quela
obtida incrementalmente pela odometria de maneira a gerar uma terceira estimativa mais pre-
cisa do que as duas iniciais (Negenborn; 2003). De acordo com a natureza do sensor utilizado
essa informao pode no ser completa, o que requer a utilizao de abordagens diferentes.
Sensores de sonar e rdio, devido grande disperso angular, podem gerar observaes onde
apenas a distncia pode ser utilizada com preciso (Kehagias, Diugash, Singh; 2006), enquan-
9
to sensores visuais permitem, com apenas uma imagem, a obteno de observaes referentes
apenas orientao das estruturas observadas (Bekris, Glick, Kavraki; 2006).
Figura 2.3 - Relaes entre mapeamento e localizao. O rob se desloca entre as posies
de acordo com
os comandos
e detecta os marcos
.
A informao absoluta que um mapa do ambiente capaz de fornecer tambm faz com que
seja possvel resolver problemas clssicos, como o caso da localizao global (Jensfelt,
Kristensen; 1999), onde o rob no conhece sua posio inicial dentro do seu modelo de ma-
pa, e o seqestro de robs (Fox, Burgard, Thrun; 1999), onde o rob subitamente levado
para outra posio e deve se localizar. Contudo, no sempre que um mapa confivel do am-
biente pode ser fornecido anteriormente navegao, seja porque o prprio ambiente ainda
no foi explorado ou porque ele ainda no conhecido, e nessas situaes o rob deve tam-
bm construir incrementalmente o mapa que utilizar para se localizar, sendo essa a base para
o surgimento do problema do SLAM.
2.2. Mapeamento
A utilizao de mapas necessria para que um rob consiga construir um modelo do mundo
ao seu redor e utilizar esse conhecimento para conseguir cumprir os seus objetivos. Para que
um rob possa lidar com o mundo ao seu redor ele deve primeiramente ser capaz de perceber
os objetos ao seu redor, preferencialmente antes de colidir com eles. Isso feito atravs de
sensores que interagem com o ambiente de alguma forma, como, por exemplo, explorando
propriedades da luz (cmeras, laser, infravermelho) ou de ondas mecnicas (sonares). Con-
forme se movimenta pelo ambiente o rob realiza observaes das estruturas que se
encontram ao seu redor, e pode decidir por apenas reagir a elas de acordo com comportamen-
tos programados (Balch, Arkin; 1993) ou ento armazenar a informao contida nessas
observaes para que possam ser recuperadas posteriormente (Thrun; 2002a).
10
Esse armazenamento de informaes feito sob a forma de um mapa, que representa o mode-
lo que o rob possui do mundo e armazena de maneira consistente as observaes que ele
coleta atravs dos seus sensores. Um mapa pode ser visto como um conjunto de variveis
(Pierce, Kuipers; 1994), cada uma capaz de descrever de maneira nica uma determinada par-
cela de interesse do ambiente. Quando o rob realiza uma observao ele inicialmente deve
determinar a qual regio do ambiente essa observao est vinculada, e ento atualiz-la no
mapa de maneira que ela reflita o estado observado. Esse reconhecimento de regies feito a
partir de um conjunto de caractersticas que cada varivel possui e que a torna nica dentro do
mapa, cujo contedo depende do tipo de sensor e de mapa que est sendo utilizado. Caso
uma regio que ainda no possua uma varivel correspondente seja observada essa varivel
adicionada ao mapa, e ser atualizada quando essa mesma regio for observada novamente.
O posicionamento absoluto de uma estrutura
realizada por esses sensores. De maneira similar ao que ocorre com a localizao, os sensores
responsveis pelo mapeamento do ambiente no fornecem informaes exatas, pois esto su-
jeitos a erros que no podem ser previstos ou modelados. Dessa forma, cada uma das
informaes incorporadas ao mapa possuir um determinado valor de incerteza, que far com
que o mapa obtido seja apenas uma estimativa da realidade, e as suas variveis se tornem va-
riveis aleatrias (Brezetz et al.; 1996). Essa impreciso pode refletir de duas maneiras no
mapa obtido: uma observao pode se correspondida com a varivel errada ou ento uma va-
rivel pode ser atualizada com um estado que no corresponde realidade. Erros de
correspondncia podem ser evitados com a utilizao de conjuntos de caractersticas mais ro-
bustos (Olson; 2002) e erros de atualizao so corrigidos atravs do acmulo de observaes
(Nieto et al.; 2003), que tende a fazer com que erros aleatrios sejam eliminados e o estado da
varivel convirja para o estado real.
Existem diversas maneiras de se construir o mapa de um ambiente, cada uma levando em
considerao diferentes aspectos da informao coletada pelos sensores (Gutmann, Konolige;
1999). Mapas mtricos e de caractersticas procuram criar uma representao espacial das
estruturas ao redor do rob, distribuindo-as dentro de um sistema global de coordenadas.
Mapas topolgicos se baseiam na construo de grafos que conectam diferentes reas do am-
biente de uma maneira qualitativa (Choset, Nagatani; 2001). Esse trabalho usa representaes
espaciais para resolver o problema do SLAM, portanto a seguir so descritos em mais deta-
lhes os mapas mtricos e de caractersticas.
11
2.2.1. Mapas Mtricos
Uma maneira de se armazenar as informaes coletadas pelos sensores do rob atravs dos
chamados mapas mtricos, que fazem uso de uma estrutura conhecida como grade de ocupa-
o (Elfes; 1989). Uma grade de ocupao, aqui suposta bidimensional, pode ser modelada
como uma matriz
o estado livre.
,
,
,
,
,
,
,
,
,
,
,
,
,
,
,
,
|
:
,
:
(2.2)
No inicio da navegao cada uma das regies apresenta um estado que indica o conhecimento
que se possui em relao quela regio, e caso no haja nenhum conhecimento atribudo o
valor 0.5, indicando 50% de probabilidade de existir algum obstculo. Conforme o rob se
movimenta ele coleta informaes a partir dos seus sensores e as utiliza para atualizar os esta-
dos das variveis referentes s regies observadas de maneira a faz-los convergir para
aqueles apresentados na realidade. Essa atualizao geralmente feita atravs do Teorema de
Bayes (Murphy; 1999), um resultado da teoria de probabilidades que permite atualizar iterati-
vamente a distribuio de probabilidades de uma varivel aleatria conforme novas
informaes referentes a ela so obtidas. A formulao do Teorema de Bayes apresentada
em (2.3), onde
, a po-
sio do rob naquele instante.
(2.3)
Como cada varivel do mapa possui uma posio fixa no ambiente, a incerteza inerente s
observaes fornecidas pelos sensores faz com que seja atualizado um conjunto de variveis,
12
de acordo com a probabilidade de cada uma delas representar a posio que foi efetivamente
observada. Supondo que os sensores do rob conseguem detectar apenas regies ocupadas e
que os sinais enviados e recebidos viajam no espao antes de interagir com os objetos detec-
tados, pode-se considerar qualquer resultado de observao como ocupado e todo o espao
intermedirio como livre. Na Figura 2.4b apresentado um modelo probabilstico para um
sensor de distncia bidimensional, no caso o sonar. A regio de maior probabilidade indica a
posio da estrutura de acordo com a observao, e o espalhamento dessa probabilidade re-
presenta a incerteza quanto a essa estimativa. As regies de menor probabilidade representam
o caminho percorrido pelo sonar at alcanar o obstculo, que se encontra vazio e por isso su-
as variveis correspondentes podem ser atualizadas de acordo.
(a) (b)
Figura 2.4 - Mapas mtricos.
(a) Grade de Ocupao Probabilstica (b) Distribuio de Probabilidades
A vantagem da utilizao de mapas mtricos est no fato deles fornecerem uma representao
densa do ambiente, o que facilita a utilizao posterior de algoritmos como planejamento de
trajetrias (Stentz; 1994) (Ersson, Hu; 2001) ou de explorao (Zelinsky; 1992). Contudo,
esse tipo de mapa possui um elevado custo computacional, tanto no ponto de vista de proces-
samento quanto de memria, pois necessrio armazenar e lidar com uma grande quantidade
de informaes. Adicionalmente, a caracterizao de regies feita tendo como base apenas
as suas coordenadas absolutas no ambiente, o que torna o reconhecimento bastante sensvel a
erros de mapeamento e localizao, gerando uma grande quantidade de erros de correspon-
dncia. Dessa forma, algoritmos de SLAM que fazem uso de mapas mtricos geralmente se
limitam a utilizar a informao como foi obtida pelos sensores, sem o estabelecimento de
marcos para auxiliar na navegao (Eliazar, Parr; 2003).
2.2.2. Mapas de Caractersticas
Outra maneira de se armazenar informaes coletadas pelos sensores do rob atravs dos
chamados mapas de caractersticas, que filtram essas informaes em busca de padres rel
vantes de alguma forma para a aplicao onde esse mapa ser utilizado e os armazenam sob a
forma de pontos esparsos (
mapa uma estrutura composta por um conjunto de
conjunto de caractersticas
va de posio que indica as suas coordenadas absolutas no
Essa abordagem gera um menor custo computacional de manuteno, pois lida com uma m
nor quantidade de informaes
padres nas informaes que sejam relevantes para a aplicao em questo, assim como a c
racterizao desses padres de maneira a permitir a correspondncia com estruturas
semelhantes. Como as variveis em um mapa de caractersticas no possuem posies fixas
no espao como acontece em um mapa mtrico
incorporada diretamente no parmetro que a define, sob a forma de uma distribuio de p
babilidades similar quela utilizada no tratamento do prob
Supe-se aqui que as estruturas que cada varivel representa no ambiente no se movime
tam, e por isso essa distribuio de probabilidades no se propaga no temp
constante uma vez definida.
(a)
Figura
(a) Mapa resultante
Caractersticas
Outra maneira de se armazenar informaes coletadas pelos sensores do rob atravs dos
ersticas, que filtram essas informaes em busca de padres rel
vantes de alguma forma para a aplicao onde esse mapa ser utilizado e os armazenam sob a
(Buschka; 2006), enquanto o resto descartado. Assim sendo, um
uma estrutura composta por um conjunto de variveis, cada uma contendo
que a define de maneira nica dentro do mapa e uma estimat
e indica as suas coordenadas absolutas no ambiente
Essa abordagem gera um menor custo computacional de manuteno, pois lida com uma m
informaes (Figura 2.5a). Contudo, surge o custo decorrente da busca por
padres nas informaes que sejam relevantes para a aplicao em questo, assim como a c
racterizao desses padres de maneira a permitir a correspondncia com estruturas
s. Como as variveis em um mapa de caractersticas no possuem posies fixas
como acontece em um mapa mtrico, a incerteza quanto a essa estimativa pode ser
incorporada diretamente no parmetro que a define, sob a forma de uma distribuio de p
babilidades similar quela utilizada no tratamento do problema de localizao (Figura 2.5b
se aqui que as estruturas que cada varivel representa no ambiente no se movime
tam, e por isso essa distribuio de probabilidades no se propaga no temp
constante uma vez definida.
(b)
Figura 2.5 - Mapas de caractersticas (Montemerlo; 2003).
(a) Mapa resultante (rvores) (b) Distribuio de Probabilidades
13
Outra maneira de se armazenar informaes coletadas pelos sensores do rob atravs dos
ersticas, que filtram essas informaes em busca de padres rele-
vantes de alguma forma para a aplicao onde esse mapa ser utilizado e os armazenam sob a
, enquanto o resto descartado. Assim sendo, um
variveis, cada uma contendo um
que a define de maneira nica dentro do mapa e uma estimati-
(e a sua incerteza ).
(2.4)
Essa abordagem gera um menor custo computacional de manuteno, pois lida com uma me-
decorrente da busca por
padres nas informaes que sejam relevantes para a aplicao em questo, assim como a ca-
racterizao desses padres de maneira a permitir a correspondncia com estruturas
s. Como as variveis em um mapa de caractersticas no possuem posies fixas
, a incerteza quanto a essa estimativa pode ser
incorporada diretamente no parmetro que a define, sob a forma de uma distribuio de pro-
lema de localizao (Figura 2.5b).
se aqui que as estruturas que cada varivel representa no ambiente no se movimen-
tam, e por isso essa distribuio de probabilidades no se propaga no tempo, permanecendo
(b)
(b) Distribuio de Probabilidades
14
No incio da navegao o mapa se encontra vazio, pois o ambiente ao redor do rob desco-
nhecido, e conforme o rob se movimenta ele coleta informaes com seus sensores em busca
de padres relevantes que possam ser incorporados ao mapa. Quando um padro encontra-
do, o seu conjunto de caractersticas
de cada uma
das variveis do mapa, em busca de padres semelhantes. Se no houver correspondncia
supe-se que o rob est observando uma regio nova do ambiente, e esse padro armaze-
nado sob a forma de uma nova varivel, cuja posio absoluta calculada a partir da
informao fornecida pela observao e a estimativa de posio do rob naquele instante. Se
houver correspondncia essa informao utilizada para refinar as estimativas de posio da-
quela varivel e diminuir a sua incerteza inerente, aumentando com isso a preciso do mapa.
2.3. Algoritmos de SLAM
Como o prprio nome diz, o problema do SLAM surge quando ambos os problemas apresen-
tados anteriormente precisam ser resolvidos simultaneamente, pois o rob no conta com
informaes precisas referentes a nenhum deles. Dessa forma, solues individuais no so
suficientes, pois os erros que cada estimativa possui se propagariam para a outra e rapidamen-
te se acumulariam em um crculo vicioso. Um mapa impreciso no ser capaz de gerar uma
estimativa precisa de localizao, que por sua vez aumentar a incerteza de mapeamento.
necessrio lidar com ambos os problemas simultaneamente, procurando uma soluo que mi-
nimize globalmente as incertezas envolvidas.
A primeira formalizao matemtica de uma soluo para o problema do SLAM foi apresen-
tada durante a dcada de 80 por uma srie de artigos (Smith, Cheeseman; 1986) (Smith, Self,
Cheeseman; 1987) (Durrant-Whyte; 1988) (Smith, Self, Cheeseman; 1990), e em 1989 foram
realizadas as primeiras implementaes dessa soluo por Mourtarlier e Chatila. Contudo,
trabalhos anteriores como os de (Taylor; 1976) e (Brooks; 1982) j haviam tentado quantificar
as incertezas inerentes a um processo atravs de limiares mnimos e mximos, estabelecendo
faixas de tolerncia para uma dada operao e tentando controlar essa faixa para que ela se
mantivesse estvel. Em (Chatila, Laumond; 1985) foi determinado, para o rob HILARE, um
15
valor escalar representativo da incerteza da localizao de um rob que leva em considerao
tanto a sua posio quanto a sua orientao.
Essa soluo para o problema do SLAM utiliza um filtro estocstico conhecido como Filtro
Estendido de Kalman (Kalman; 1960), ou EKF, para acompanhar simultaneamente os estados
de localizao e mapeamento do rob, o que lhe rendeu o nome de EKF-SLAM ou SLAM
Estocstico. A teoria de Filtros de Kalman fornece uma soluo fechada e recursiva para o
problema de se acompanhar uma varivel aleatria de interesse conforme ela evolui no tempo
(Simon, Uhlmann; 1997), e j era tradicionalmente utilizada em solues individuais para o
problema de localizao e mapeamento. O EKF-SLAM procurou unificar esses dois proble-
mas ao utilizar um mesmo EKF para acompanhar simultaneamente o estado de localizao do
rob e dos marcos que ele observa no ambiente. As Equaes 2.5 e 2.6 exemplificam o vetor
de estados
e a matriz de covarincia
, , e a
posio tridimensional dos marcos como
, , .
(2.5)
(2.6)
No incio da navegao o rob conta apenas com o seu estado de localizao e a sua incerteza
inerente, e conforme se movimenta essa incerteza tende a aumentar devido ao acmulo de er-
ros gerado pelo seu modelo de odometria. Ao observar um marco
. Dessa forma,
parte da incerteza referente posio do marco ser devido incerteza de localizao do rob
e parte ser devido aos erros aleatrios gerados pelos sensores que realizaram a observao.
A matriz de covarincia
(3.1)
Um descritor usualmente avaliado de acordo com dois critrios. O primeiro deles a taxa
de correspondncia entre as caractersticas de um mesmo objeto quando visto de maneiras
diferentes (Mikolajczyk, Schmid; 2002), como em diferentes pontos de vista ou luminosidade.
O segundo critrio a taxa de acertos conseguida por um classificador em um conjunto de
imagens, com base em um banco de dados que contm um modelo das caractersticas referen-
tes s possibilidades de classificao (Kadir, Brady, Zisserman; 2004). Diferentes aplicaes
requerem descritores com melhor desempenho em diferentes critrios, e existem situaes
onde um bom desempenho no visto como uma qualidade. O SLAM, por exemplo, se bene-
ficia mais de descritores capazes de reconhecer objetos sob diferentes pontos de vista,
enquanto a classificao de acordo com o tipo de objeto pode gerar correspondncias erradas
em ambientes com objetos similares.
28
3.4. Triangulao
A nica informao espacial que uma imagem consegue fornecer em relao a cada uma de
suas caractersticas so as coordenadas do pixel que elas ocupam. Essa informao, junta-
mente com a geometria da cmera e do sistema de viso, permite calcular o raio de luz que
refletiu na estrutura observada e gerou aquele pixel, restringindo a sua posio dentro do am-
biente a uma linha. No possvel restringir essa posio a um nico ponto, e por isso no h
como obter informaes de distncia a partir de uma nica imagem. Para isso necessrio
que uma mesma estrutura seja observada a partir de dois pontos de vista diferentes, ou seja,
que esteja presente em duas imagens obtidas a partir de posies distintas e conhecidas.
Essa configurao pode ser devido utilizao de mltiplas cmeras em posies diferentes
do rob (Murray, Little; 2000) ou utilizao de uma nica cmera que obtm mltiplas ima-
gens em instantes distintos de navegao (Gaspar; 2002). Com isso torna-se possvel, atravs
da triangulao (Gluckman, Nayar, Thorek; 1998), encontrar o ponto onde ambas as linhas se
cruzam, cada uma tendo como origem a posio do rob no instante em que a imagem foi ob-
tida, e assim determinar tridimensionalmente a estrutura no ambiente. A Figura 3.2 mostra
um exemplo de triangulao realizada com uma nica cmera deslocada no ambiente, saindo
do ponto 1 e seguindo at o ponto 2 com um deslocamento
.
Figura 3.2 - Triangulao com uma nica cmera deslocada no ambiente.
29
3.5. Marcos Visuais
A grande maioria dos algoritmos de SLAM que utilizam sistemas visuais como forma de se
obter informaes do ambiente fazem uso das prprias caractersticas obtidas da imagem co-
mo marcos (Davison; 2003) (Kwok, Dissanayake; 2004) (Se, Lowe, Little; 2002). Diferentes
mtodos de se extrair caractersticas viveis para a utilizao como marcos incluem o SIFT
(Lowe; 2004), Detectores de Harris (Harris, Stephens; 1988) e regies normalizadas de alto
contraste (Shi, Tomasi; 1994). Em (Mikolajczyk, Schmid; 2002) realizada uma extensa
pesquisa referente a mtodos de extrao de caractersticas em imagens e a sua utilizao co-
mo marcos.
A utilizao de caractersticas da imagem diretamente como marcos permite uma elasticidade
muito maior nos algoritmos resultantes, pois no necessrio estabelecer nenhuma restrio
quanto ao tipo de marco que ser utilizado, ele definido dinamicamente sem nenhum vncu-
lo semntico. Por outro lado surge o problema de escalabilidade, pois conforme os ambientes
se tornam maiores a quantidade de marcos cresce rapidamente, tornando o processo computa-
cionalmente invivel. Em (Jung; 2004) apresentada uma maneira de se agrupar diversas
caractersticas em um nico marco, ao invs de utiliz-las individualmente, o que contribui
para a diminuio do custo computacional final. importante perceber que mesmo assim no
h uma informao semntica que os relacione entre si, apenas propriedades inerentes ima-
gem na qual foram obtidos, o que permite a aplicao do algoritmo em diferentes tipos de
ambiente.
Outra abordagem consiste na incorporao de semntica s caractersticas utilizadas, definin-
do aquelas que o algoritmo deve utilizar como marcos durante a navegao. Em (Panzieri,
Pascucci, Ulivi; 2001) apresentado um algoritmo de SLAM que utiliza como marcos fontes
de luz artificiais circulares posicionadas no teto, segmentando-as para a deteco e reconhe-
cimento. Marcos verticais so utilizados em (Kim, Sukkarieh; 2003) para localizao e
mapeamento na navegao de avies no tripulados, cuja posio determinada pela projeo
dessas estruturas nas imagens obtidas. rvores so utilizadas como marcos por (Asmar, Ze-
lek, Abdallah; 2006) e postes em (Fitzgibbons; 2004), se aproveitando da organizao natural
que certos tipos de ambientes (como parques e ruas) tendem a possuir. Embora essa aborda-
gem consiga resultados mais consistentes, os algoritmos obtidos esto limitados s regies
para as quais foram desenvolvidos, o que compromete a sua generalidade.
30
3.6. Viso Omnidirecional
Sensores de viso omnidirecional representam uma famlia de sensores visuais que so capa-
zes de obter simultaneamente informaes referentes a todo o ambiente ao redor da cmera
(Grassi Jr., Okamoto Jr.; 2006). Alm da utilizao de cmeras verdadeiramente omnidirecio-
nais (Nalwa; 1996), existem vrias outras maneiras de se obter essa propriedade atravs de
arranjos especiais. Por exemplo, cmeras rotatrias ou mltiplas cmeras podem obter diver-
sas imagens que so ento combinadas para formar uma nica imagem omnidirecional (Peleg,
Ben-Erza; 1999). Espelhos especiais podem refletir a luz de todo o ambiente na cmera, con-
seguindo com isso imagens omnidirecionais com cmeras comuns (Baker, Nayar; 1998).
Diferentes tipos de geometria de espelho, como esfrica, cnica, parablica ou hiperblica
produzem resultados diferentes nas imagens obtidas.
Dentre essas possibilidades, os sistemas omnidirecionais obtidos com a utilizao de espelhos
so mais compactos e por isso mais indicados para serem embarcados em robs mveis. Den-
tre as geometrias de espelho possveis a hiprbole possui a propriedade de foco nico de
projeo, o que facilita a construo de espelhos e permite a utilizao de cmeras comuns na
construo do sistema de viso omnidirecional (Svoboda, Pajdla; 2002). Espelhos elpticos
devem ser cncavos para apresentarem essa mesma propriedade, o que dificulta a sua cons-
truo. Espelhos parablicos no podem ser utilizados com cmeras convencionais, pois
redirecionam a luz paralelamente, sem um foco nico de projeo.
Figura 3.3 - Imagens omnidirecionais com espelhos hiperblicos.
(a) Esquema de montagem (b) Exemplo de imagem
31
A Figura 3.3b mostra um exemplo de imagem omnidirecional obtida atravs do espelho hi-
perblico esquematizado na Figura 3.3a. A cmera montada na vertical e aponta para o
espelho, que se encontra acima dela, fixo em um suporte a uma distncia que faz com que o
seu foco inferior
. Em (Lindeberg;
1994) mostrado que a invarincia total quanto a mudanas de escala obtida atravs da
normalizao do laplaciano pelo fator
).
(4.3)
Uma aproximao linear da derivada parcial em (4.3) nos leva a:
, , , ,
(4.4)
38
Que pode ser reescrito como:
, , , , 1
(4.5)
Mostra-se com isso que a utilizao de diferenas gaussianas na determinao de caractersti-
cas invariantes a alteraes de escala j incorpora o fator
, com
representando a distncia
entre a posio real do extremo e
1
2
(4.6)
As derivadas parciais referentes a cada coordenada podem ser aproximadas pela diferena en-
tre pixels vizinhos naquela direo, como mostrado abaixo.
1
2
1, , 1, ,
, 1, , 1,
, , 1 , , 1
(4.7)
40
Derivando (4.6) e igualando a zero, encontra-se o valor do offset
(4.8)
Se o offset
de alguma coordenada for maior do que 0.5 o extremo estar mais prximo de
outro ponto da mesma imagem ou das imagens vizinhas no espao de escalas. O valor de
ento alterado para refletir a nova posio, e a interpolao realizada novamente agora em
relao a esse novo ponto. Quando o valor convergir, esse offset pode ser utilizado para de-
terminar o contraste
de
1
2
(4.9)
Onde um limiar predeterminado de contraste que elimina os candidatos que falharem na
comparao como sendo instveis para serem utilizados como caractersticas de acordo com o
SIFT.
4.1.1.3. Eliminao por Curvaturas
A deteco de extremos realizada pelo espao de escalas de diferenas gaussianas tende a
produzir muitos candidatos ao longo de bordas, mesmo quando essas regies so instveis.
Em viso computacional, bordas so definidas como regies na imagem que possuem uma
diferena acentuada entre as suas duas curvaturas principais (Trucco, Verri; 1998). Assim
sendo, o segundo filtro utilizado pelo SIFT procura eliminar candidatos a caractersticas que
possuem uma relao entre curvaturas principais elevada.
As curvaturas principais de um pixel podem ser obtidas atravs do clculo dos autovalores da
sua matriz Hessiana
(4.10)
41
Assim como foi feito anteriormente, as derivadas parciais podem ser calculadas pela diferena
de intensidade dos pixels vizinhos naquela direo.
1, 1,
2
, 1 , 1
2
1, 1 1, 1 1, 1 1, 1
4
(4.11)
Como apenas a proporo entre autovalores relevante, no h a necessidade de calcul-los
explicitamente (Harris, Stephens; 1988). Pode-se obter a soma deles pelo trao da Hessiana
(4.12)
(4.13)
O determinante de
deve ser positivo porque de outra forma o ponto teria sido descartado
por no representar um extremo. Definindo a relao entre curvaturas como um valor de
forma que , temos:
(4.14)
Que j no depende dos valores individuais de curvatura, apenas da proporo entre elas.
Quando 1 as duas curvaturas sero iguais, e quanto maior maior ser a diferena entre
elas. Assim sendo, a eliminao de candidatos a feature com alta relao de curvaturas pode
ser feita pela seguinte comparao:
(4.15)
Onde um limiar mximo de relao entre curvaturas que uma caracterstica pode possuir
para ser considerada estvel de acordo com esse critrio. Implementaes tradicionais do
SIFT utilizam um limiar mximo igual a 10 e nesse trabalho percebeu-se que em imagens
omnidirecionais deve-se utilizar um valor prximo de 2 para que seja mantida a mesma taxa
de eliminao de candidatos, devido maior diferena entre curvaturas que esse tipo de ima-
gem tende a possuir.
42
4.1.2. Clculo do Descritor
O vetor descritor do SIFT baseado em um histograma de orientaes, que define a maneira
como os pixels ao redor da caracterstica se relacionam entre si. A cada orientao ,
atribudo um valor de magnitude , que indica a influncia que ela possui sobre o resul-
tado final, e para obter invarincia rotacional o SIFT institui uma orientao e magnitude
principais para cada caracterstica, e ento monta o seu vetor descritor relativamente a esses
valores. Assim, uma caracterstica rotacionada apresentar a mesma distribuio, e por isso
poder ser correspondida.
,
, 1 , 1
1, 1,
(4.16)
,
1, 1,
, 1 , 1
(4.17)
Para se obter a orientao principal de uma caracterstica uma janela posicionada ao seu re-
dor e os valores de orientao e magnitude de cada um dos pixels nessa regio so calculados.
Os valores de magnitude so ento posicionados em um histograma composto pelas orienta-
es calculadas, como mostrado na Figura 4.3. Para evitar instabilidades causadas por
alteraes bruscas em regies distantes da caracterstica, as magnitudes obtidas so pondera-
das por uma distribuio gaussiana centrada nas coordenadas da caracterstica e com desvio
padro igual quele utilizado na suavizao gaussiana da imagem de onde a caracterstica foi
obtida.
Figura 4.3 - Histograma de orientao.
43
O ponto mais alto do histograma ser a orientao principal da caracterstica, e caso haja ou-
tro pico com valor suficientemente elevado (Lowe prope um limiar de 80% em relao ao
principal) outra caractersticas criada com as mesmas propriedades e apresentando outra ori-
entao principal. Assim, um mesmo pixel pode possuir diversas orientaes vinculadas a
caractersticas distintas, o que contribui para uma melhor representao da imagem e corres-
pondncias mais robustas.
A construo do vetor descritor feita posicionando-se outra janela ao redor da caracterstica,
que dividida em subjanelas de acordo com a complexidade do vetor descritor final desejado.
A orientao e magnitude dos pixels pertencentes a cada janela so calculadas e para cada
subjanela montado um histograma que armazena esses valores, onde as orientaes so dis-
cretizadas tambm de acordo com a complexidade desejada do vetor descritor final. A Figura
4.4 apresenta um exemplo da construo do vetor onde so consideradas 4 subjanelas e 8 va-
lores diferentes de orientao (o crculo azul indica a distribuio gaussiana utilizada).
Novamente utilizada uma suavizao gaussiana (crculo azul) visando diminuir a influncia
de alteraes bruscas em regies distantes da caracterstica
Figura 4.4 - Construo do descritor (Lowe; 2004).
O vetor descritor armazena a magnitudes referentes a cada uma das orientaes, rotacionadas
de acordo com a orientao principal calculada anteriormente. No exemplo da Figura 4.4 o
vetor descritor final possuir 36 posies, enquanto implementaes tradicionais do SIFT cos-
tumam apresentar descritores compostos por 128 valores, obtidos em 16 histogramas com 8
orientaes cada um. Embora essa quantidade seja alta em relao a outros descritores, ela
permite uma alta taxa de correspondncia em objetos que foram submetidos a deformaes,
fazendo com que o SIFT apresente certo grau de invarincia em tambm em relao a essa
transformao.
44
Para se obter invarincia relativa luminosidade as magnitudes contidas no descritor so
normalizadas, pois dessa forma alteraes globais de luminosidade no afetaro os resultados
obtidos. Contudo, variaes no-lineares podem ocorrer devido a efeitos de saturao da c-
mera, o que interfere na distribuio das magnitudes, mas no afeta de forma perceptvel as
orientaes. Assim, uma invarincia parcial quanto a essas alteraes pode ser obtida limi-
tando-se a influncia das magnitudes mais elevadas, saturando o seu valor em um
determinado limiar. Depois disso o descritor novamente normalizado, e estar completo
para ser utilizado na correspondncia entre caractersticas.
4.1.3. Determinao dos Marcos
Uma vez que as caractersticas da imagem j tenham sido extradas e descritas, necessrio
que elas sejam transformadas nos marcos que o rob utilizar para resolver o problema do
SLAM durante a navegao. Uma abordagem possvel consiste na utilizao direta das carac-
tersticas extradas como marcos (Se, Lowe, Little; 2002), utilizando o vetor descritor como o
conjunto de propriedades que define o marco e permite a sua correspondncia posterior. Caso
haja uma correspondncia, a posio da caracterstica no ambiente pode ser estimada por tri-
angulao, completando o conjunto de propriedades que um marco deve possuir para ser
utilizado pelo SLAM da maneira como proposto aqui.
Contudo, essa abordagem tende a gerar uma quantidade muito grande de marcos, pois todas
as caractersticas extradas das imagens coletadas so adicionadas ao mapa de caractersticas
para serem utilizadas pelo SLAM. Uma grande parte dessas caractersticas pode ser conside-
rada espria, por no representar nenhum objeto consistente no ambiente, e por isso no sero
estveis para serem correspondidas em imagens posteriores. Adicionalmente, a correspon-
dncia entre caractersticas individuais pode gerar uma grande quantidade de erros, devido a
fatores como similaridade entre objetos no ambiente. Essas correspondncias erradas podem
propagar informaes incorretas para as estimativas de localizao e mapeamento e aumentar
a impreciso dos resultados.
Uma alternativa j discutida a atribuio de relaes semnticas aos objetos correspondidos,
de maneira a utilizar apenas caractersticas que possuam um determinado conjunto de propri-
edades pr-estabelecidas. Mas essa abordagem vai contra a proposta desse trabalho, que a
navegao em qualquer tipo de ambiente. Por isso, optou-se aqui pela determinao de con-
45
juntos no semnticos de caractersticas (Jung; 2004), de acordo com as relaes que elas a-
presentam dentro da imagem de onde foram extradas. Foram escolhidos dois critrios
diferentes para a determinao desses conjuntos, a distncia entre caractersticas na imagem e
a diferena de contraste entre elas. Supe-se aqui que caractersticas que representem uma
mesma estrutura estaro prximas entre si e possuiro um contraste similar na imagem.
Inicialmente a posio de cada caracterstica comparada com a posio das outras extradas
da imagem, e aquelas que estiverem suficientemente prximas de acordo com algum limiar
pr-estabelecido de distncia seguem para a segunda comparao, que a diferena de con-
traste que elas apresentem. Caso tambm estejam dentro do limiar de contraste elas so
reunidas com a caracterstica inicial e o processo repetido com elas, at que no reste ne-
nhuma caracterstica que satisfaa ambos os critrios. Se a quantidade de caractersticas
reunidas dessa forma for maior do que um limiar de tamanho estabelecido ele considerado
um objeto no ambiente e essas caractersticas sero utilizadas como marcos nessa iterao
do algoritmo, caso contrrio so eliminadas. Dessa forma a quantidade de marcos gerada a
cada instante diminui consideravelmente, mas aqueles gerados sero mais estveis nas etapas
posteriores.
Figura 4.5 - Conjuntos de caractersticas. Os pontos pretos indicam as caractersticas que foram locali-
zadas na imagem (limiar: 3 pixels).
As caractersticas referentes a cada objeto observado so armazenadas independentemente no
mapa de caracterstica do rob, cada uma possuindo o seu prprio valor de incerteza e de po-
sicionamento. O ndice do objeto ao qual pertencem mantido de forma a permitir o
estabelecimento de vnculos durante a etapa de correspondncia, como mostrado a seguir.
46
4.2. Correspondncia
A correspondncia entre caractersticas realizada pela comparao entre os valores contidos
em seus vetores descritores em busca daqueles que se encontrarem mais prximos no espao
k-dimensional. Uma caracterstica ser correspondida com outra quando a distncia euclidia-
na (3.1) entre ambas for a menor possvel dado o conjunto de caractersticas possveis da
imagem a qual cada uma delas pertence. Para aumentar a confiabilidade dos resultados so
descartadas as correspondncias onde a proporo entre a menor distncia euclidiana obtida e
a segunda for maior do que um determinado limiar. Dessa forma so eliminadas correspon-
dncias ambguas, como o caso daquelas extradas do plano de fundo da imagem, e que por
isso no so estveis como marcos.
Contudo, o custo computacional gerado por uma busca exaustiva pela melhor correspondn-
cia entre dois conjuntos de caractersticas elevado devido alta dimensionalidade que os
vetores devem possuir para conseguir descrever de maneira satisfatria cada caracterstica.
Outro fator limitante a grande quantidade de caractersticas que uma nica imagem tende a
possuir, o que aumenta o espao de possibilidades de correspondncia. Algoritmos eficientes
de busca, como as kd-tree (Friedman, Bentley, Finkel; 1977) no apresentam uma melhora
significativa para casos com dimenses elevadas, portanto no solucionam esse problema.
Em (Beis, Lowe; 1997) proposta uma soluo probabilstica para o problema da correspon-
dncia, que consegue encontrar as caractersticas com menor distncia euclidiana em
aproximadamente 95% das tentativas com um ganho computacional de duas ordens de gran-
deza (Figura 4.6), chamado BBF (Best Bin Fit).
Figura 4.6 - Comparao entre kd-tree e Best Bin Fit (Arya, Mount; 1993).
47
O BBF mantm uma verso modificada da kd-tree, que organiza os componentes do vetor
descritor de cada caracterstica de forma que valores semelhantes fiquem prximos entre si.
Quando uma busca realizada so verificadas primeiramente as distncias referentes a deter-
minadas dimenses do vetor (Arya, Mount; 1993), e a busca interrompida quando aquela
regio de caractersticas se mostra distante daquela que est sendo correspondida. Essa busca
probabilstica gera um ganho computacional de duas ordens de magnitude e gera resultados
no timos em aproximadamente 5% das correspondncias. O BBF se torna especialmente
eficiente em casos onde caractersticas ambguas so descartadas, como o caso daquelas ge-
radas pelo plano de fundo da imagem (Lowe; 1999).
Ainda assim, a comparao individual entre caractersticas pode ocasionar erros de corres-
pondncia, especialmente em ambientes estruturados que tendem a possuir repetio de
padres, como portas em um corredor e rvores em um parque. Dessa forma, esse trabalho
apresenta e utiliza uma segunda etapa na correspondncia entre caractersticas, que leva em
considerao os conjuntos estabelecidos durante a determinao de marcos. Inicialmente as
caractersticas de cada um dos objetos armazenados no mapa so correspondidas com as ca-
ractersticas extradas da imagem, em busca de semelhanas que indiquem o reconhecimento
de estruturas. Em seguida calculada a porcentagem de correspondncias que foram feitas em
relao quantidade de caractersticas que o objeto possui, e caso esse valor esteja abaixo de
um limiar essas correspondncias so ignoradas. A probabilidade de que um conjunto de ca-
ractersticas seja corespondido de maneira errada menor do que a probabilidade de apenas
uma o seja, o que aumenta a confiabilidade dos resultados finais e diminui consideravelmente
o custo computacional do processo.
Adicionalmente, dessa forma possvel instituir uma maneira de se eliminar marcos conside-
rados esprios, de acordo com a influncia que eles apresentam durante a correspondncia de
seus objetos. Caso um objeto seja consistentemente reconhecido, mas uma de suas caracters-
ticas no o seja, ela pode ser removida do mapa de maneira a diminuir a quantidade de marcos
com os quais o rob deve lidar. Se isso fizer com que a quantidade de caractersticas do obje-
to caia abaixo de um limiar estabelecido, outras caractersticas que cumpram os requisitos de
distncia e de contraste na imagem onde ele foi correspondido podem ser incorporadas a ele, e
se isso no for possvel o objeto como um todo pode ser eliminado como no sendo mais re-
levante para a localizao e mapeamento.
48
4.3. Triangulao
A triangulao uma etapa necessria quando sensores de viso so utilizados para que seja
possvel conseguir informaes de distncias de marcos em relao ao rob. Cada observao
de um marco realizada gera o conjunto
, de informaes, onde
, , representa
a posio e orientao do rob no instante de obteno da imagem (supondo navegao bidi-
mensional) e , indica as coordenadas do marco dentro dela. Alm disso, a geometria
do espelho e do sistema de viso conhecida, permitindo que seja determinado o raio de luz
que deu origem quele pixel e com isso o segmento de reta no qual o marco extraido deve es-
tar localizado. Caso esse marco seja correspondido em uma imagem posterior possvel obter
um segundo segmento de reta, e a interseco entre ambos determinar a posio tridimensio-
nal do marco no ambiente.
A Figura 4.7 apresenta um diagrama das principais entidades geomtricas na triangulao e
como elas se relacionam entre si. Os pontos
.
Figura 4.7 - Entidades e parmetros relevantes na triangulao.
49
O vetor que modela o raio de luz incidente na cmera pode ser obtido a partir dos pontos
e como mostrado abaixo.
(4.18)
Contudo, ainda no o raio desejado, pois o marco extrado na imagem representa uma es-
trutura que foi refletida no espelho hiperblico utilizado, e por isso necessrio definir qual o
vetor que representa esse mesmo raio de luz antes de ser refletido. Para isso devemos calcular
inicialmente o ponto na hiprbole que intersecta .
4
2
1 2
(4.19)
Esse ponto e o foco superior da hiprbole
cos
sin
sin
cos
0 0 1
(4.20)
Essa informao ento armazenada na varivel aleatria correspondente a esse marco no
mapa de caractersticas. Posteriormente, caso haja uma correspondncia entre o marco e uma
nova caracterstica, a posio da estrutura representada por ambos pode ser calculada como a
interseco entre
(4.21)
4.4. FastSLAM
O algoritmo escolhido nesse trabalho para incorporar as informaes obtidas pelo sistema de
viso omnidirecional o FastSLAM, desenvolvido por Montemerlo em parceria com diversos
outros pesquisadores (Montemerlo et al.; 2002) (Montemerlo; 2003). Desde ento ele vem se
tornando cada vez mais popular em tarefas de localizao e mapeamento simultneos, geran-
do diversos trabalhos que procuram testar e ampliar os seus limites (Haehnel et al.; 2003)
(Nieto et al.; 2006) (Thrun et al.; 2004) (Bailey, Nieto, Nebot; 2006) ou aumentar a sua efici-
ncia (Montemerlo et al.; 2003).
A grande contribuio do FastSLAM para o problema da localizao e mapeamento simult-
neos est na sua capacidade de lidar de uma forma eficiente com grandes quantidades de
marcos, algo importante quando so utilizados sensores visuais. Para conseguir isso explo-
rada a propriedade mostrada em (Murphy; 1999) que diz que o problema do SLAM pode ser
dividido em diversos problemas menores de estimao, um para cada varivel que relevante
na localizao e no mapeamento. Em particular, utiliza-se a noo de independncia entre
observaes que diz que, se a localizao do rob de alguma forma conhecida, o posiciona-
mento dos marcos observados pode ser fatorado em problemas independentes entre si. Essa
fatorao mostrada a seguir, onde
a posiao do rob,
o conjunto de observaes,
marcos.
(4.22)
51
Obviamente a localizao do rob no conhecida, mas pode ser aproximada com um Filtro
de Partculas, onde cada partcula representa uma possvel localizao do rob e tratada in-
ternamente como se fosse correta. Para cada uma das partculas o problema de mapeamento
do ambiente pode ser subdividido em problemas de estimao independentes, cada um re-
ferente a um nico marco, e tratado por um EKF. Assim sendo, o FastSLAM lida
simultaneamente com EKFs, um para cada marco para cada partcula, e um Filtro de Par-
tculas para estimar a localizao do rob. Esse valor pode parecer alto, mas todas as matrizes
possuem uma dimensionalidade constante e baixa (igual a 3 no caso de marcos tridimensio-
nais contra 3 3 no EKF-SLAM), e por serem independentes entre si apenas as estimativas
referentes aos marcos observados so atualizadas a cada instante. Implementaes eficientes
conseguem no FastSLAM um custo computacional proporcional a (contra
no
EKF-SLAM), permitindo a utilizao de milhares ou at mesmo (Montemerlo et al.; 2002)
milhes de marcos simultaneamente com um custo computacional aceitvel.
Diferentemente do que acontece na abordagem tradicional do EKF-SLAM, o FastSLAM lida
com mltiplas hipteses de mapa, armazenando-as juntamente com a hiptese de localizao
que cada partcula representa. Assim sendo, possvel lidar com ambigidades de forma na-
tural, pois diferentes mapas podem possuir diferentes hipteses de associao de dados, e
aquelas que se mostrarem erradas so eliminadas implicitamente durante a etapa de Reamos-
tragem do Filtro de Partculas. Essa propriedade, juntamente com o fato de que o FastSLAM
atualiza seus marcos localmente, faz com que ele seja menos sensvel a erros de correspon-
dncia. Adicionalmente, a utilizao de mltiplos mapas gera um efeito de atualizao global
similar quele apresentado pelo EKF-SLAM, pois quando uma partcula eleita a mais pro-
vvel no apenas a distribuio de probabilidades da localizao do rob que se altera, mas
tambm a de todos os marcos armazenados, refletindo a sua hiptese de mapeamento que
tambm se torna a mais provvel.
O alto custo computacional gerado pela necessidade de se manter mltiplos mapas amorte-
cido por algoritmos eficientes de armazenamento e acesso a informaes, que explora as
redundncias existentes entre marcos cuja mesma estimativa compartilhada por mais do que
uma partcula. Consegue-se dessa forma um custo computacional que no depende do tama-
nho do mapa armazenado, mas sim da quantidade de marcos observados pelo rob a cada
instante, que um valor aproximadamente constante durante todo o perodo de navegao.
52
4.4.1. Mapeamento
A etapa de mapeamento no FastSLAM tem como entrada um conjunto de marcos obtidos a
partir da imagem omnidirecional coletada pelo sensor de viso, processada de acordo com o
SIFT. Esses marcos so correspondidos com aqueles que j esto armazenados do mapa de
caractersticas do rob e podem ser processados de maneira diferente de acordo com o nme-
ro de correspondncias que j foram realizadas. Quando um marco observado pela primeira
vez ele adicionado ao mapa sem nenhuma estimativa, de maneira a permitir a sua posterior
correspondncia e triangulao. Quando a primeira correspondncia realizada possvel
por triangulao obter uma primeira estimativa de posio para esse marco, assim como da
incerteza referente a essa estimativa. A partir desse momento, novas correspondncias gera-
ro novas estimativas que podem ser utilizadas para refinar aquela j existente, diminuindo a
sua incerteza. Essas atualizaes so feitas de acordo com as equaes do Filtro Estendido de
Kalman, supondo distribuies de probabilidade gaussianas e linearizao de funes.
Inicialmente necessrio definir um modelo de erros do sistema de viso que quantifique a
distribuio de probabilidades que cada uma das estimativas obtidas a partir dele possuir.
Supondo distribuies gaussianas pode-se definir a mdia como a prpria estimativa obtida na
triangulao, definindo que ela possui a maior probabilidade de representar o estado correto
do rob (os erros envolvidos so aleatrios com mdia nula). J a matriz de covarincia ob-
tida empiricamente a partir de testes que quantifiquem a sensibilidade do sistema ao
surgimento de erros. A matriz de erros utilizada nesse trabalho mostrada em (4.23), onde
indica a sensibilidade que o sistema possui para o surgimento de erros do tipo devido a
uma estimativa do tipo . Por exemplo, a sensibilidade do rob ao surgimento de erros na es-
timativa de distncia devido aos valores referentes sua estimativa de orientao angular em
relao ao eixo xy dada pelo coeficiente
(4.23)
Devido deformao que a geometria omnidirecional gera sobre diferentes regies da ima-
gem podem ser utilizados valores diferentes de sensibilidade de acordo com as coordenadas
do pixel que representa a caracterstica. Em especial, regies mais distantes do centro sero
mais sensveis do que as centrais, gerando uma matriz com valores mais elevados.
53
4.4.1.1. Adio de Marcos
Um marco observado pela primeira vez em uma imagem no possui uma estimativa de posi-
o no ambiente, e por isso tambm no possui uma incerteza em relao a essa posio
(pode-se, ao invs disso, dizer que essa incerteza infinita). A nica informao disponvel
nesse instante a posio
, (4.24)
4.4.1.2. Atualizao de Marcos
Quando uma caracterstica extrada de uma imagem correspondida com um marco armaze-
nado no seu mapa possvel calcular o vetor de observaes
,
,
referente ao marco que foi
correspondido, a partir da sua estimativa de posio
naquele instante. Esse vetor composto por trs valores, o primeiro correspondente distn-
cia entre o sensor e o marco e os outros dois correspondentes aos ngulos e que o
posicionam unicamente no espao. Esse vetor de observaes definido em (4.25) e ser di-
ferente para cada partcula, pois cada uma possui uma hiptese de localizao diferente.
,
,
(4.25)
54
A atualizao de marcos pode ocorrer de duas maneiras diferentes. Caso essa seja a primeira
correspondncia, o marco ainda no contar com nenhuma estimativa de posio, e por isso
essa primeira estimativa ser incorporada completamente sua varivel, assim como a sua
incerteza. Caso seja uma correspondncia posterior, essa estimativa, assim como a sua incer-
teza, ser utilizada para atualizar aquela j existente no marco, de maneira a diminuir a sua
incerteza inerente.
Primeira Correspondncia
Quando um marco
cos cos sin cos 0
sin cos cos cos 0
sin 0 sin
(4.26)
A matriz de covarincia
,
(4.27)
Esses dois valores so adicionados varivel representativa do marco no ambiente apresenta-
da em (4.24), completando-a com uma estimativa de posio e sua respectiva incerteza como
mostrado em (4.28). Para efeitos de triangulao, as informaes referentes s coordenadas
do pixel na imagem so atualizadas com os valores obtidos na ltima observao, e com isso
a posio do rob
, , , (4.28)
55
Correspondncias Posteriores
A atualizao do estado dos marcos armazenados no mapa de caractersticas realizada de
acordo com as equaes do EKF, individualmente para cada marco em cada uma das partcu-
las. O sistema de equaes no-lineares genricas que o EKF se prope a resolver mostrado
a seguir:
,
,
(4.29)
Onde
,
,
(4.30)
A funo que relaciona a localizao do rob
,
,
(4.31)
Como a formulao matemtica do Filtro de Kalman se baseia em sistemas lineares, neces-
srio linearizar a funo
,
,
. O Jacobiano
,
,
,
,
,
,
,
(4.32)
Onde
.
A diferena
,
,
entre o vetor de observaes
,
,
obtido a partir dos valores de triangulao e
o vetor
,
,
obtido a partir dos valores contidos no mapa do rob indica o erro entre o
que foi observado e o que deveria ter sido.
,
,
,
,
,
,
(4.33)
A covarincia da inovao
,
(4.34)
O Ganho de Kalman
,
definido como:
(4.35)
Com isso possvel calcular a nova posio do marco (4.36) e a sua matriz de covarincia
correspondente (4.37).
,
,
(4.36)
(4.37)
Essas estimativas substituem aquelas armazenadas na varivel correspondente ao marco no
mapa. A incorporao de novas informaes referentes a esse marco faz com que a sua incer-
teza diminua, aumentando a preciso da estimativa final de posicionamento e melhorando os
resultados finais de mapeamento.
57
4.4.1.3. rvore de Marcos
Uma implementao direta da etapa de mapeamento no FastSLAM possui um custo computa-
cional proporcional quantidade de partculas e de marcos utilizados. Isso porque cada
partcula carrega consigo um mapa composto por estimativas de posio de cada um de seus
marcos, e deve propagar essas estimativas no tempo de acordo com a evoluo do Filtro de
Partculas. O custo computacional relativo quantidade de partculas no pode ser evitado
porque cada partcula representa uma hiptese independente. Contudo, devido relao de
independncia que foi estabelecida entre marcos, grande parte da informao contida nos ma-
pas construdos dessa forma ser redundante, pois diversas partculas eventualmente
compartilharo de uma mesma estimativa referente a um marco, principalmente aps uma e-
tapa de Reamostragem.
Tirando proveito disso, o FastSLAM consegue diminuir o seu custo computacional para uma
funo logartmica em relao quantidade de marcos, permitindo a utilizao simultnea de
quantidades muito maiores do que seria possvel de outra forma. Para isso utilizada uma
rvore binria (Figura 4.8a), onde a estimativa de cada marco armazenada em uma das fo-
lhas (ns inferiores da rvore). Partculas que compartilham de uma mesma estimativa
relativa a um marco compartilharo dessa mesma folha, o que elimina a cpia de informaes
redundantes em diversas partculas. Conforme novas estimativas referentes a um marco so
incorporadas so geradas novas folhas que as armazenam e novos caminhos que apontam para
elas, mantendo os outros intactos (Figura 4.8b)
(a) (b)
Figura 4.8 - rvore binria de marcos tradicional (Montemerlo; 2003).
(a) Formato esttico (b) Adio de novas estimativas
58
Nesse trabalho a estrutura apresentada acima foi modificada de maneira a se obter um resulta-
do em mdia mais eficiente, que no pior caso alcana o custo logartmico da implementao
padro utilizada pelo FastSLAM. Ao invs de armazenar as estimativas apenas nas folhas da
rvore binria elas so armazenadas em todos os ns (Figura 4.10a), gerando uma economia
de memria (so necessrios menos ns) e de tempo (no necessrio percorrer toda a rvore
para se chegar em alguns dos ns). Cada marco recebe um ndice nico que indica qual a sua
posio dentro da rvore, e ela manuseada de maneira a sempre permanecer balanceada,
com a alterao de sua raiz, quando necessrio. Esse balanceamento realizado pela rotao
da raiz da rvore, realizada no sentido anti-horrio (Figura 4.9) devido ao acrscimo de mar-
cos em ordem ascendente de ndices que faz a rvore ficar mais populada do lado direito.
Figura 4.9 - Rotao de ns para a esquerda.
Quando um marco observado pela partcula e a sua estimativa de posio alterada, um no-
vo n gerado para armazenar essa estimativa, e um novo caminho que leva at ele
construdo na rvore binria da partcula. Caso esse caminho passe por outras estimativas,
elas tambm sero reconstrudas, realizando uma cpia aparentemente desnecessria de in-
formaes. Contudo, caso esses marcos sejam observados em um instante posterior, suas
estimativas podero ser atualizadas sem a necessidade de construir outro caminho, pois ele j
ser nico quela partcula. De qualquer forma, a implementao apresentada em trabalhos
tradicionais de FastSLAM requer a criao de ns responsveis pela busca binria, gerando
um custo semelhante.
(a) (b)
Figura 4.10 - rvore binria proposta. (a) Formato esttico (b) Adio de novas estimativas
59
A Figura 4.10b mostra um exemplo da estrutura descrita aqui, para o caso em que dois marcos
(2 e 7) foram observados. Os ns em cinza so referentes rvore binria compartilhada, e os
em branco foram reconstrudos devido s estimativas incorporadas pela partcula em seu ma-
pa. Uma implementao tradicional como aquela mostrada em (Montemerlo et al.; 2003)
necessitaria de mais trs ns para conseguir o mesmo resultado, dois para armazenar as esti-
mativas propriamente ditas e um para completar o caminho que leva at a folha que
armazenaria a estimativa do marco de identidade 2. Dessa forma necessrio um menor tem-
po de atualizao para conseguir o mesmo resultado, o que gera um aumento de eficincia no
algoritmo. Adicionalmente, a rvore binria tradicional necessita de 14 ns para armazenar
essa informao, contra apenas 7 necessrios na implementao proposta, gerando uma eco-
nomia significativa de memria.
4.4.2. Localizao
O problema da localizao tratado pelo FastSLAM por um Filtro de Partculas, que mantm
um conjunto finito de amostras retiradas da distribuio de probabilidades que modela a posi-
o do rob a cada instante (Liu, Chen; 1998). Cada partcula representa uma hiptese
diferente de localizao e mapeamento, e utiliza esses valores para determinar o seu peso, um
indicador da sua probabilidade de representar os estados corretos do rob em um dado instan-
te. Conforme novas informaes relativas aos marcos observados so incorporadas a essas
estimativas esse peso alterado de maneira a refletir a distribuio de probabilidades das par-
tculas naquele instante.
Um Filtro de Partculas composto por trs etapas que so realizadas em seqncia. Na Figu-
ra 4.11 essa seqncia indicada, assim como a influncia que cada uma das etapas possui
sobre a distribuio das partculas no ambiente e sobre o peso de cada uma delas. A seguir
so apresentados os conceitos e a formulao envolvida em cada uma dessas etapas.
Figura 4.11 - Etapas do Filtro de Partculas (regies escuras possuem maior probabilidade de serem corretas).
60
4.4.2.1. Predio
A etapa de Predio no Filtro de Partculas no depende de informaes externas, apenas do
modelo de movimentao que o rob utiliza para estimar a sua localizao conforme navega
pelo ambiente. Inicialmente o estado de cada partcula propagado de maneira determinsti-
ca, de acordo com o vetor de controles
(4.38)
Essa transio determinstica, ou seja, no leva em considerao os erros acumulados pelo
rob durante o deslocamento. Esses erros podem ser causados devido a imprecises na rota-
o e na translao do rob, e para maximizar o conjunto de erros possveis (Thrun, Fox,
Burgard; 2005) includo um terceiro grau de liberdade durante a movimentao, referente a
61
uma rotao ao redor da posio final, como mostrado na Figura 4.13. Assim, o rob inicial-
mente rotaciona na sua posio inicial, em seguida translada para a posio final e em seguida
rotaciona nela de maneira a assumir a sua orientao final, e todos esses movimentos esto
sujeitos a erros.
Figura 4.13 - Deslocamentos realizados durante a movimentao.
Esses trs deslocamentos podem ser facilmente calculados a partir da geometria do problema.
A primeira rotao calculada subtraindo-se a orientao intermediria do rob (obtida aps a
primeira rotao) da sua orientao inicial. A translao calculada pelo Teorema de Pitgo-
ras levando-se em considerao o deslocamento do rob nas coordenadas e . A ltima
rotao calculada pela subtrao da orientao final do rob da sua orientao intermediria.
(4.39)
Esses deslocamentos mdios so utilizados como base para a incorporao do modelo de er-
ros desenvolvido para o rob. Intuitivamente, quanto maior o deslocamento maior dever
ser o erro incorporado estimatva, de acordo com a sensibilidade que o rob possui para o
surgimento de erros de cada natureza. Essa sensibilidade estimada empiricamente atravs
da atribuio de coeficientes
(4.40)
62
Multiplicando os deslocamentos obtidos pelos seus coeficientes correspondentes so determi-
nados os desvios-padro que representam a incerteza em relao a cada um dos
deslocamentos realizados, modelando-os como uma distribuio gaussiana.
(4.41)
A posio de cada uma das partculas obtida por amostragem dentro dessa distribuio de
probabilidades, onde a mdia igual ao valor determinstico obtido em (4.38) e o desvio pa-
dro dado por (4.41). Uma maneira de se realizar essa amostragem atravs da equao de
Box-Muller, mostrada em (4.42) onde e so variveis aleatrias uniformemente distribu-
das entre 0,1.
, 2ln cos2 (4.42)
O resultado , obtido uma varivel aleatria gaussiana com mdia e desvio padro
. O deslocamento
0,
,
0,
,
0,
,
(4.43)
Para se obter
necessrio projetar
cos
sin
(4.44)
FastSLAM 2.0
O FastSLAM 2.0, apresentado em (Montemerlo et al.; 2003) uma evoluo do algoritmo
tradicional de FastSLAM que procura aumentar a eficincia do Filtro de Partculas utilizado
para estimar a localizao do rob a cada instante. Mais especificamente, o FastSLAM 2.0
procura aumentar a eficincia da etapa de Predio do Filtro de Partculas de forma a evitar o
surgimento de partculas em regies de baixa probabilidade. Para conseguir isso, as informa-
63
es coletadas pelos sensores do rob so utilizadas diretamente na etapa de amostragem de
partculas, ao invs de apenas na determinao do peso de cada uma delas. No h nenhum
aumento na qualidade dos resultados finais obtidos pelo FastSLAM 2.0 em relao imple-
mentao original, apenas consegue-se obter os mesmos resultados com significativamente
menos partculas. Em particular, para sistemas lineares o FastSLAM 2.0 consegue conver-
gncia para 1, constituindo dessa forma o nico algoritmo de SLAM com tempo de
atualizao constante.
Com mostrado anteriormente, a etapa de Predio envolve a amostragem do estado de locali-
zao de cada uma das partculas dentro da distribuio de probabilidades gerada pela
propagao desse estado no tempo, cuja mdia dada por (4.38) e o desvio padro por (4.41).
No FastSLAM 2.0 essa distribuio de probabilidades modificada pelas observaes coleta-
das pelo que foram correspondidas com marcos que j possuem uma estimativa de posio,
de maneira a diminuir a sua impreciso e aumentar a probabilidade de que sejam amostradas
partculas prximas do estado real de localizao do rob. A covarincia em relao a cada
observao calculada a partir da matriz
,
(4.45)
Em (4.45) calculada a nova covarincia da distribuio de probabilidades que ser utilizada
na amostragem durante a Predio. J a nova mdia calculada em (4.46), novamente utili-
zado a matriz de covarincia da inovao
,
e o jacobiano
,
(4.46)
No caso de haver mais de uma observao correspondida, todo o processo deve ser repetido
de maneira a incorporar a sua estimativa tambm. importante lembrar que a alterao da
localizao estimada do rob far com que o jacobiano
,
e a matriz de covarincia
,
se
alterem, e devam ser calculadas novamente. Adicionalmente, as adies em (4.45) e (4.46)
devem ser feitas utilizando-se os resultados obtidos na incorporao da estimativa anterior, e
no os originais obtidos na ltima iterao. Uma vez que todas as observaes j tenham sido
incorporadas, pode-se realizar a amostragem com a mdia e covarincia resultantes.
64
O FastSLAM 2.0 tende a ser mais eficiente do que o FastSLAM especialmente em situaes
onde a preciso dos sensores de coleta de informaes do ambiente muito maior em relao
preciso do sistema de odometria utilizado, algo que acontece na grande maioria das aplica-
es (Montemerlo et al.; 2003). Isso faz com que a regio de probabilidades onde uma
partcula pode se localizar durante a predio se torne grande em comparao quela que a
Reamostragem gera uma vez que as observaes so incorporadas, gerando um desperdcio de
partculas. Ao manter uma maior quantidade de partculas em reas de alta probabilidade o
FastSLAM 2.0 consegue tambm uma maior diversidade de hipteses, o que resulta na capa-
cidade de fechamento de loops mais extensos.
4.4.2.2. Atualizao
Durante a etapa de Predio o estado de localizao das partculas foi alterado pela movimen-
tao do rob, de acordo com seus estados anteriores, o vetor de controles e a funo de
transio que os relaciona. O estado de mapeamento das partculas tambm foi alterado pelas
etapas do EKF que modelam a posio de cada um dos marcos que elas possuem em seus res-
pectivos modelo de mapa. A etapa de Atualizao do Filtro de Partculas utiliza essas
alteraes para verificar quais partculas possuem o conjunto de estados de localizao e ma-
peamento com maior probabilidade de representar o estado correto que o rob apresenta
naquele instante. Essa probabilidade modelada pelo peso que cada partcula possui, sendo
que quanto maior o peso maior a probabilidade, e supe-se que a soma de todos os pesos
igual a 1, ou seja, o estado correto do rob representado de maneira exata por uma de suas
partculas.
As mesmas informaes que foram utilizadas para atualizar a estimativa de posio de um
marco tambm podem ser utilizadas para atualizar a estimativa de posio do rob, pois esses
valores esto relacionados entre si. Uma observao determina a posio de um marco no
ambiente quando a localizao do rob conhecida, e uma observao tambm determina a
localizao do rob no ambiente caso a posio do marco seja conhecida. Dessa forma, quan-
do no h nenhuma correspondncia entre caractersticas e marcos a etapa de Atualizao no
acontece, e os pesos das partculas se mantm os mesmos durante essa iterao. Similarmen-
te, caso ocorram vrias correspondncias os pesos so alterados seqencialmente, devido
hiptese de independncia estabelecida entre os marcos que compem o mapa.
65
Quanto mais prximas a estimativas de localizao e mapeamento da partcula estiverem da
estimativa obtida pela observao correspondida, maior a probabilidade de que ela represente
o estado correto do rob, e por isso maior ser o seu peso. Essa semelhana quantificada
pelo vetor de diferenas
,
,
definido em (4.33), sendo que quanto maior ele for menor ser a
semelhana. A matriz de covarincia da inovao
,
1
2
exp
1
2
,
,
,
,
(4.47)
Depois que todas as atualizaes tenham sido feitas os pesos do conjunto de partculas so
normalizados (4.48) para que a sua soma volte a ser igual a 1. O resultado da etapa de Atuali-
zao uma nova distribuio de probabilidades que leva em considerao as informaes
obtidas pelo rob naquele instante. A partcula com o maior peso representa o estado mais
provvel de localizao e mapeamento para o rob, indicando qual a sua posio e a de todos
os marcos j observados no ambiente.
(4.48)
4.4.2.3. Reamostragem
A etapa de Reamostragem procura eliminar partculas que no representem uma hiptese v-
lida para a localizao do rob, ao mesmo tempo em que multiplica aquelas que possuem
pesos elevados, aumentando a densidade de partculas na regio que se supe prxima da lo-
calizao real do rob. Como a quantidade de partculas com as quais se lida constante (ou
ao menos finita) a densidade delas tende a diminuir conforme ocorre o espalhamento causado
pela etapa de Predio, fazendo com que eventualmente no reste nenhuma partcula prxima
do local onde o rob realmente se encontra e at a partcula de maior peso no representar
uma estimativa precisa o suficiente.
66
Como essa proximidade determinada pelo peso que cada partcula possui, a Reamostragem
se baseia em procurar eliminar aquelas que possuem um peso baixo e criar novas cpias da-
quelas que possuem um peso elevado. Uma maneira simples de se realizar isso e que gera
resultados bastante satisfatrios consiste em realizar uma amostragem no conjunto de partcu-
las, utilizando os pesos como distribuio de probabilidades. Partculas com maior peso tero
maior probabilidade de ser escolhidas, o que desejvel dado que isso tambm significa que
elas tm uma maior chance de representarem o estado correto de localizao do rob. Outros
mtodos de se realizar a Reamostragem so apresentados em (Rekleitis; 2003), e um mtodo
particularmente eficiente, que possui custo computacional linear em relao quantidade de
partculas, encontrado (Thrun, Fox, Burgard; 2005).
, ,
(4.49)
Inicialmente, amostrado um nmero aleatrio de uma distribuio uniforme normalizada
entre [0,1]. O conjunto de partculas
cujo peso fez com que a somatria ultrapassasse ser escolhida como amostra e
adicionada no conjunto final de partculas
tenha a mesma quantidade de elementos que
(4.50), que quantifica a varincia do conjunto
(4.50)
(4.51)
Um aumento no valor de
, o que significa que elas esto se afastando cada vez mais, gerando uma densidade local
menor. Da mesma forma, uma diminuio no valor de