Grupo 2, Sistemas Autónomos 2010 1

Implementação do Algoritmo de Monte Carlo para Localização em

Ambientes Interiores
Duarte Meneses, Pedro Agostinho, e Ricardo Faria

Instituto Superior Técnico, Lisboa

Este artigo descreve a implementação de um algoritmo de localização num robot num ambiente interior. O algoritmo utilizado foi a
localização Monte Carlo, utilizando um robot com odometria, o Pioneer P3-DX, equipado com sensor de alcance laser. A predição foi
feita com recurso à odometria e a actualização com o laser e um mapa do espaço. Foram feitas experiências num piso da Torre Norte
do Instituto Superior Técnico, com bons resultados.

Termos de índice — Monte Carlo Localization, Robótica, Localização

incluindo obstáculos e grelhas de distâncias a

I. INTRODUÇÃO obstáculos.
o mundo da Robótica, a auto-localização em “indoor” é  Ressampling – Estudo de algoritmos adequados para
N uns dos problemas mais difíceis e com mais interesse
nesta área. Várias abordagens têm sido implementadas
reamostragem.

com sucesso para lidar com as dificuldades do mundo real

(ruído, incerteza, etc).
A maioria destas abordagens, concilia as informações obtidas
por duas fontes: percepção e odomotria, ambas ruidosas.
A abordagem apresentada neste trabalho realiza a localização
por meio da odomotria, um mapa (fornecido) e informação
visual (distancia adquirida por um sensor).
O sistema de localização adoptado, segue os princípios do
filtro de partículas baseado no método “Monte Carlo
Localization”, aplicado com algumas variações descritas nos
próximos capítulos. Apesar da sua curta existência, o MCL
tornou-se um dos algoritmos mais populares de localização em
robótica. É relativamente fácil de implementar e tem várias
aplicações.
Este sistema foi criado para funcionar num espaço específico,
mais concretamente nos corredores, zona de elevadores e na
sala 5.21 do 5º Piso da Torre Norte do Instituto Superior
Técnico de Lisboa.
II. METODOLOGIA/ABORDAGEM
O objectivo deste trabalho é de implementar um sistema de
localização num robot utilizando especificamente o algoritmo
de Monte Carlo. Actualmente há muita literatura que aborda
este tema, e o primeiro passo foi familiarizar-nos com o
problema da localização em geral e de seguida com o
algoritmo a ser usado.
Começou-se por construir uma interface mais completa com o
robot, tornando-se mais fácil controlá-lo e obter valores da
odometria e laser.
Esta importante etapa permitiu conhecer as características e a
forma como é feito o controlo do robot, e foi um processo algo
longo.
Inicialmente analisou-se separadamente as 3 principais fases
do algoritmo:
 Prediction – Análise da odometria e controlos do
robot; Erros envolvidos, amplitude dos valores.
 Update – Análise do LRF (Laser Range Finder);
Quantidade e qualidade dos dados. Criação de mapa
Por fim, juntou-se todos os blocos, e procedeu-se a exaustivos
testes ao programa de forma a ajustar certos parâmetros dos
modelos usados e confirmar o seu correcto funcionamento de
acordo com os requisitos iniciais.
III. ALGORITMO MONTE CARLO
Com o algoritmo de localização de Monte Carlo, representa-se
a pose em que acreditamos o robot estar através de um
conjunto de partículas. É portanto uma alternativa não
paramétrica do filtro de Bayes. O algoritmo aplica-se a
problemas com espaço de estados discretos já que estas
partículas, de número finito, têm uma pose discreta no mundo
[3].
A pose é constituída pela localização espacial (x,y) e
orientação (teta). O espaço é particionado em quadrículas com
dimensão arbitrária formando uma grelha. A localização
especifica a quadrícula em que o robot se encontra.
Este algoritmo baseia-se no filtro de partículas, integrando o
modelo probabilístico do movimento do robot e os dados dos
seus sensores.
Cada iteração usa a crença da pose actual do robot para gerar
uma nova população de partículas que representam uma nova
estimativa da pose do robot.
O algoritmo é constituído por dois passos fundamentais:
predição e actualização. Na predição, utiliza-se aplica-se o
modelo de movimento a cada uma das partículas, de forma a
actualizar a sua pose. No passo de actualização introduz-se a
informação obtida dos sensores do robot na população de
partículas, para avaliar o seu valor como estimativa da posição
real. São depois seleccionadas probabilisticamente as
melhores partículas, para constituir a nova população.
De seguida apresenta-se o algoritmo de Monte Carlo [1].
Grupo 2, Sistemas Autónomos 2010
Na linha 4 faz-se a previsão do movimento baseada no modelo
da dinâmica. Na linha 5 usa-se o modelo dos sensores para
atribuir pesos às partículas. O último passo, nas linhas 8-10
faz-se a reamostragem. De seguida explica-se com mais
detalhe cada um destes passos.
A. Modelo do Movimento
Com o modelo de movimento, pretende-se obter o modelo
probabilístico da transição p(xt | ut, xt-1), ou seja, como um
comando de controlo ao robot afecta a sua pose. Existem duas
abordagens possíveis: modelo de velocidade e modelo de
odometria. Em ambos os casos, é necessário compreender
como o robot se movimenta e que erros estão associados de
forma a poder modelar probabilisticamente. No primeiro
comanda-se aos motores do robot especificando a velocidade e
estima-se o efeito que teve na sua pose. No segundo o robot
consegue medir o seu movimento, disponibilizando odometria.
A diferença fundamental é que no modelo de odometria a
informação é obtida em retrospectiva, depois do movimento.
Isto não representa um problema para o algoritmo usado, e é
preferível tratar odometria como controlo para evitar aumentar
o espaço de estado.
Para a aplicação em concreto, assume-se que o robot dispõe de
odometria e utiliza-se, portanto, o modelo de odometria. Este
modelo é considerado mais exacto e é o mais usado.
Os robots disponibilizando informação relativa à odometria,
possuem encoders nas rodas que codificam informação
relativa à rotação destas, que é integrada. Estes dados são no
entanto afectados por erros, por exemplo, resultantes de
derrapagens das rodas ou erros de medição internos dos
encoders. Assim, à medida que o robot se vai movimentando,
maior erro vai acumulando na odometria.
Os valores usados obtêm-se subtraindo o valor da odometria
da iteração anterior ao valor actual, representando o
movimento entre as duas iterações. Este valor é decomposto
em três passos, ilustrados na fig. 1.
Figura 1: Decomposição do movimento em 3 componentes
2
Uma primeira rotação em direcção à posição final, uma
translação, e mais uma rotação para a pose final.
Assume-se que cada um destes três componentes é afectado
por ruído independente.
Para modelar a inexactidão da odometria, soma-se a cada um
dos componentes um erro aleatório. Este valor é uma amostra
de uma distribuição gaussiana com parâmetros arbitrários.
Variando os diferentes pesos nas funções de amostragem,
consegue-se diferentes padrões de distribuição das partículas,
com o objectivo de aproximar o erro que está realmente
presente na odometria do robot que está a ser utilizado na
aplicação – ver fig 2.
Após o movimento, é preciso consultar um mapa do local para
verificar se a partícula ficou numa zona interdita. Nestes
casos, gera-se o movimento novamente até isto não acontecer.
Figura 2: Distribuição espacial das partículas de acordo com
diferentes modelos de movimento
B. Atribuição de Pesos
Em cada iteração, algumas partículas, que são consideradas
menos exactas são descartadas enquanto outras, mais
importantes, são replicadas.
Para isso, é necessário ter uma função que atribua a cada
partícula um peso, que será depois determinante para a sua
eliminação ou manutenção no passo da reamostragem.
Os pesos são uma forma de avaliar se a posição da partícula é
confirmada pelos dados do sensor.
Com o sensor obtém-se as distâncias segundo vários ângulos
aos obstáculos. Para comparar estes dados com a posição de
uma partícula, obtém-se para essa posição as distâncias
segundo os mesmos ângulos. A localização vai indexar a
tabela com as distâncias que foi gerada a partir do mapa do
espaço, e adquire-se assim as distâncias para fazer a
comparação e consequente atribuição de peso.
No entanto, os dados do sensor são afectados por erro, pelo
que não se pode atribuir o peso simplesmente por uma
comparação directa entre os dois valores. É importante
construir um modelo do sensor de forma a evitar dar pesos
muito pequenos e aumentar as hipóteses de rejeitar uma
partícula devido aos erros do sensor.
C. Modelo dos Sensores
Os robots podem ser equipados com variados tipos de
sensores. Os sensores que interessam para a aplicação são
sensores de alcance, que medem distâncias para diferentes
ângulos, com o objectivo de ajudar na localização. Os mais
populares são os sensores a laser e sonares. Dependendo do
tipo, podem ter diferentes alcances mínimos e máximos,
resoluções angulares e precisões.
Grupo 2, Sistemas Autónomos 2010
Pode-se considerar, genericamente, quatro tipos de erros que
afectam as medições dos sensores. Como já foi referido, estes
erros afectam a forma como os pesos são atribuídos. Mesmo
em casos em que o valor medido não é muito coerente com a
posição da partícula devido a um erro, o peso é pequeno mas
não nulo, havendo possibilidade de não ser eliminada.
 Erros de precisão do sensor. Resultam de da
resolução limitada dos sensores, do efeito
atmosférico na medida, etc. Este erro pode ser
modelado por uma gaussiana centrada na distância
obtida através do mapa;
 Medidas curtas devidas a objectos que não estão no
mapa e que surgem entre o robot e elementos do
mapa. Pode ser representado por uma distribuição
exponencial entre o valor 0 e a distância da partícula
à parede.
 Falhas de medição: Frequentemente o sensor falha
quando tenta medir uma superfície num ângulo muito
aberto, ou quando a superfície é preta ou absorvente
de luz. A falha também se pode dever por o obstáculo
estar simplesmente fora do alcance do sensor. Estes
erros resultam numa medição com o alcance máximo
do sensor. É portanto necessário considerar um
impulso no valor do alcance máximo.
 Medidas aleatórias: Por outros factores, as medidas
podem retornar valores completamente aleatórios.
Isto é modelado facilmente com uma distribuição
uniforme.
Figura 3: Combinação linear de distribuições de erro
Na figura 3 representa-se uma combinação pesada dos pesos
que os vários tipos de erros contribuem e para quando os
dados coincidem (centro do gaussiana). Note-se que é
importante dar a cada modelação um peso diferente de acordo
com a sua importância, obtidos através de análise
experimental.
D. Reamostragem
Na fase de resampling (reamostragem) pretende-se criar um
novo conjunto de amostras partindo das amostras afectadas de
deslocamento na fase de prediction e os pesos individuais
obtidos na fase de update.
Os pesos das amostras representam uma probabilidade (em
sentido lato) da pose, ser a pose real do robot, ou por outras
palavras representa uma medida de coerência daquilo que é
esperado "observar" na pose da amostra e o que efectivamente
3
é "observado" pelos sensores na pose real do robot. A ideia do
resampling é que haja uma maior preferência de reprodução
das amostras que melhor representam a pose real do robot,
ideia que sugere uma selecção Darwinista.
A forma mais intuitiva de implementar o resampling é o
algoritmo Independent Random Sampling que consiste em
gerar um número aleatório entre 0 e 1 (admitido pesos
normalizados) e ir somando os pesos das partículas de forma
sequencial até o valor da soma atingir esse valor gerado, a
ultima partícula cujo peso foi somado é seleccionada, este
processo que é equivalente a usar uma roleta, tem de ser
repetido M vezes, sendo M o número de partículas.
Figura 4: Representação do algoritmo Low Variance Resampling
Uma alternativa mais recomendada [1] é o algoritmo Low
Variance Resampling, em vez de escolher M números
aleatórios e seleccionar as partículas que correspondem a esses
números, é gerado apenas um numero aleatório entre 0 e M-1,
as partículas são escolhidas somando repetidamente M-1 e
seleccionando a partícula correspondente como se pode
observar na figura 4. Este algoritmo cobre o espaço de
amostras de forma mais sistemática e tem a importante
vantagem de ser computacionalmente mais eficiente, O(M)
contra O(MlogM) do algoritmo Independent Random
Sampling.
IV. IMPLEMENTAÇÃO
O objectivo final é implementar o algoritmo MCL num robot e
fazer testes práticos. Para isso, foi utilizado o robot Pioneer
P3-DX. Este robot possui encoders internos, disponibilizando
odometria. O robot vem equipado com sonar. Devido à
elevada imprecisão deste sensor, optou-se por utilizar um
sensor de alcance laser (LRF), que tem um desempenho bem
melhor. O alcance máximo é de aproximadamente 4 metros,
tendo uma resolução angular de cerca de 0,5º, de -270º até
270º.
O ambiente utilizado para a programação foi o Matlab, porque
já existia uma biblioteca de interface básica com o robot.
Além disso o Matlab permite um desenvolvimento rápido de
alto nível, apesar de ser mais ineficiente em termos
computacionais do que linguagens compiladas como o C. Isto
é uma desvantagem para sistemas em tempo real, como é o
caso.
Grupo 2, Sistemas Autónomos 2010 4

direcções também discretas. Na definição da resolução da

grelha e do número de ângulos teve-se em conta a memória e
o desempenho do algoritmo. Para o desempenho do algoritmo,
quanto maior a resolução, melhor a precisão, mas também
maior é o custo computacional. Na tabela 1 está a memória
ocupada pelos dados para alguns valores do número de
direcções e do tamanho das quadrículas. Optou-se por utilizar
quadrículas de 10 cm e 180 ângulos uniformemente
distribuídos, ocupando 32.6Mb de memória. Estes valores são
perfeitamente aceitáveis tendo em conta a dimensão do
espaço, do robot e da precisão dos seus sensores.

largura
2,5 5 7,5 10 12,5
nº ang.
Figura 5: Mapa do local 360 1044 261 116 65,3 41,8
180 522 130 58 32,6 20,9
Numa primeira abordagem começou-se por uma 90 261 65 29 16,3 10,4
familiarização com os comandos do robot, construindo uma Tabela 1 – Tamanho ocupado em memória dependendo de 2
interface mais elaborada e testando o funcionamento da parâmetros
odometria e do sensor laser. Para ajustar os parâmetros do
modelo do movimento e do modelo de sensor usados, foi Transformou-se o mapa num bitmap, em que as áreas que o
necessário conhecer os factores de erro da odometria e do robot não pode ocupar estão em preto e as outras em branco.
laser. Adaptou-se a resolução do bitmap de acordo com a resolução
Na fig. 6 está o resultado de um dos testes feitos com o da grelha escolhida (10 cm), que resultou em 218x218 pixéis.
objectivo de se obter um erro da odometria após uma série de O bitmap está na Figura 7.
deslocações.

Figura 7: Obstáculos

Figura 6: Exemplo de operação da ferramenta de teste da

odometria
O local onde o robot é utilizado é o 5º piso da Torre Norte do
Instituto Superior Técnico. Todos os testes e ajustes de
parâmetros foram feitos para essa área e os mapas usados
abrangem apenas esse piso.
Para cada ponto onde o robot pode estar, é necessário ter as
distâncias até às paredes, em todas as direcções. Por razões de
desempenho computacional, esta informação deve ser pré-
calculada.
Extraiu-se do mapa da área onde o robot actua uma área
quadrada sem áreas que não são necessárias (Figura 5).
Para que seja utilizado pelo algoritmo é necessário decompô-
lo em pontos discretos através de uma grelha e associar a cada
quadrícula valores relativos às distâncias em diversas
Criou-se um programa em C que tem como input este ficheiro
e cria como output 180 ficheiros de texto, num formato aceite
pelo Matlab. Cada um destes ficheiros tem, para cada
quadrícula do mapa, as distâncias a obstáculos segundo um
determinado ângulo. O programa implementa o algoritmo de
Bresenham [2] para determinar que quadrículas do mapa que
constituem uma linha. Detectando a primeira quadrícula que
representa um espaço ocupado, determina-se a distância entre
a posição e essa quadrícula.
Na Figura 8 mostramos a representação gráfica de um dos
ficheiros de output, utilizando uma escala de cores para as
distâncias, a um ângulo de 110º.
Grupo 2, Sistemas Autónomos 2010 5

Por fim usou-se um método mais complexo mas que deu

melhores resultados. Faz-se uma dupla ponderação, que usa o
peso das partículas calculado durante o algoritmo e um factor
de ponderação proporcional ao número de partículas que se
encontra na mesma partição de espaço.

V. RESULTADOS
Foram feitos vários testes práticos no local para detectar erros
e optimizar o programa para haver uma convergência. O
computador utilizado foi um netbook com poder de
processamento muito limitado, o que tornou essa optimização
ainda mais relevante.
Com 100 partículas o algoritmo converge em cerca de 40
Figura 8: Distâncias em cada ponto até à parede a 110º. Quanto
mais claro, maior a distância. As escalas estão em centímetros.
iterações. Na figura 10 a) b) e c) mostra-se a evolução das
partículas.

Na figura 9 esquematiza-se a estrutura geral do programa feito

em Matlab. Estão ilustrados a sequência dos blocos principais
que compõem o algoritmo.
Foi feita uma implementação directa do algoritmo apresentado
na secção anterior. Houve necessidade de fazer ajustes de
parâmetros e conversões de unidades de acordo com o
hardware utilizado, local de funcionamento e as ferramentas.
No passo do movimento a partir dos dados da odometria, se
uma partícula passar a ocupar uma zona que corresponde a um
obstáculo, em vez de se tentar correr novamente, é gerada uma
a) b)
posição aleatória para essa partícula.

ParticleSET
MCL GetData
Update

Prediction

LaserData OdometryData

MotionModel

MovedParticles
c) d)
GetMapDistances
Update

Figura 10: Evolução do algoritmo. A) Dispersão inicial das

EstimatedPose
LaserModel partículas na primeira iteração. B) Iteração 17. Já existe alguma
aglomeração de partículas em zonas com similaridades. C)
Distinção dos cantos na iteração 27. D) Convergência total na
EstimatePose posição real do robot, na iteração 41
Resampling

ParticleSET
Figura 9: Estrutura geral da implementação do MCL
A interface desenvolvida permite visualizar no mapa a
localização das partículas, comandar o robot e iniciar uma
iteração do algoritmo a qualquer momento.
Com as várias partículas que compõem o conjunto de poses
possíveis do robot, é necessário estimar uma única pose do
robot. Foram testadas várias soluções, algumas em [1], que
foram testadas: média aritmética das poses, média ponderada
pelos pesos de cada partícula e finalmente divisão do espaço
em partições e ponderação baseada no número de partículas
existentes em cada partição.
Weights
Durante o teste aqui apresentado, na vigésima quarta iteração
GenerateNewSet já havia uma localização bem definida do robot e na iteração
seguinte, houve uma falha desconhecida – provavelmente foi
um erro na odometria ou do mapa e as partículas coincidiram
com a parede - e as partículas voltaram a ficar dispersadas
aleatoriamente. O algoritmo conseguiu recuperar desta
situação passadas algumas iterações e convergir novamente.
VI. CONCLUSÕES
O algoritmo MCL foi implementado com sucesso, os testes
realizados produziram resultados bastante satisfatórios. Na
maioria dos testes efectuados, 100 partículas foram suficientes
para obter uma boa estimativa da localização do robot, isto
demonstra que o algoritmo tem potencial para ser utilizado
com hardware de fraca capacidade computacional como
Grupo 2, Sistemas Autónomos 2010 6

processadores embedded. No entanto é importante referir que

com conjuntos de partículas de dimensão fixa reduzida, o
número de iterações inicias do algoritmo até que seja
perceptível uma boa estimativa pode ser elevado, isto porque
as partículas estão muito espalhadas no espaço e pode
acontecer que inicialmente não haja partículas com pose
próxima da real.
A tolerância do algoritmo a uma resolução do mapa que á
primeira vista poderia ser considerada "grosseira" e a
tolerância de erro nos controlos de movimento e nas leituras
de sensores, vem reforçar a ideia de que o algoritmo é
adequado para implementação em sistemas de pequena
dimensão e baixo custo.
Incorrecções em algumas no mapa usado (desenhado com
recurso a uma fita métrica, uma planta imperceptível, e
alguma boa vontade...) provocaram durante os testes um
desvio em certas áreas do mapa. A recuperação de uma boa
estimativa após algumas iterações longe da zona problemática,
veio validar a capacidade do algoritmo recuperar de erros e
dar confiança que é adequado para operação em ambientes
dinâmicos onde possa haver alterações pontuais (pessoas,
portas, etc).
É de notar que a recuperação do desvio acima mencionado
acontece porque há um espalhamento de partículas á volta da
estimativa correcta, ou seja, a consideração de uma odometria
ruidosa impede que as partículas se concentrem todas
demasiado juntas. O que é interessante e notável é que a
recuperação do erro é auxiliada pela incerteza associada aos
comandos e sensores.
De um ponto de vista mais abstracto, a análise dos resultados
permitiu-nos encarar a incerteza como algo que pode ser usado
a nosso favor enquanto numa perspectiva determinista a
incerteza representa uma noção estrita de erro e é
inevitavelmente nociva. Na perspectiva probabilística a
incerteza natural existente em qualquer aparelho de medida e a
natureza dinâmica das condições de operação estão
intimamente incluídas na base conceptual dos algoritmos,
permitindo assim resultados muito mais consistentes,
adaptativos a condições externas e com capacidade de
recuperação de falhas. Este conjunto de características seria,
se não impossível, pelo menos muito difícil e dispendioso de
conseguir em condições determinísticas.

VII. REFERENCIAS
[1] Thrun, Sebastian; Burgard, Wolfram; Fox, Dieter; “Probabilistics
Robotics”. MIT Press, 2005.
[2] Flanagan, Colin; “The Bresenham Line-Drawing Alforithm”. Internet:
www.cs.helsinki.fi/group/goa/mallinnus/lines/bresenh.html. [15 Nov.
2010]
[3] Ribeiro, M. I.; Lima, Pedro; “Localization, Mapping & Simultaneous
Localization and Mapping Part IV – Monte Carlo Localization”, Notas
teóricas de Sistemas Autónomos, Lisboa: Instituto Superior Técnico,
2008.
[4] Thrun, Sebastian; Lima, Pedro; “Probabilist Robotics Basics”, Notas
teóricas de Sistemas Autónomos, Lisboa: Instituto Superior Técnico,
2009.