LOCALIZAÇÃO E MAPEAMENTO EM TEMPO REAL UTILIZANDO ROBÔ SIMULADO NO MICROSOFT® ROBOTICS DEVELOPER STUDIO

1/66

CENTRO TÉCNICO AEROESPACIALINSTITUTO TECNOLÓGICO DE AERONÁUTICA

DIVISÃO DE ENGENHARIA ELETRÔNICADEPARTAMENTO DE SISTEMAS DE CONTROLE

LOCALIZAÇÃO E MAPEAMENTO EM TEMPOREAL UTILIZANDO ROBÔ SIMULADO NO

MICROSOFT® ROBOTICS DEVELOPER STUDIO

Wilian França CostaOrientador: Prof. Dr. Jackson Paul Matsuura

Co-orientador: Prof. Dr. Alexandre da Silva Simões





São José dos Campos, 23 de dezembro de 2009São José dos Campos, 23 de dezembro de 2009

2/66

Introdução

Introdução (1/3)Objetivo Metodologia (3)MRDS (4)SLAM (11)Scan Matching (13)Solução Proposta (7)Resultados (15)Conclusão (2)Trabalhos Futuros (3).

● Para que um robô móvel seja considerado autônomo, ele deve possuir a capacidade de navegar com segurança em um ambiente desconhecido evitando obstáculos (BAILEY, 2002, p. 2).

● Usualmente o robô monta sua base de conhecimento através da construção progressiva de um mapa utilizando os dados de seus sensores (GUTMANN; KONOLIGE, 1999; HOWARD, 2004).

3/66

Motivação● Localização e Mapeamento Simultâneo

(SLAM).● Desafio constante em robótica móvel.● Diversas abordagens, sendo que as

mais comuns utilizam métodos estocásticos (HAHNEL et al., 2003), iterativos (LU; MILIOS, 1994) e combinações entre eles (BIBER; STRASSER, 2006).


4/66

Motivação● Exemplo de sucesso: robô Stanley

desenvolvido pela equipe do Dr. Sebastian Thrun e que ganhou o desafio DARPA em 2005 (THRUN et al., 2006).

(a) Ilustração do sensor laser: o sensor é inclinado para escanear o terreno enquanto o robô se movimenta. O robô possui 5 sensores montados em ângulos diferentes. (b) Cada scanner monta uma representação 3D em nuvem-de-pontos para identificar obstáculos.Fonte:(THRUN et al., 2006).


5/66

Objetivo

Introdução (3)ObjetivoMetodologia (3)MRDS (n)SLAM (1/n)Scan Matching (n)Solução Proposta (n)Resultados (n)ConclusãoTrabalhos Futuros.

Desenvolvimento e implementação de um algoritmo para a estimação e correção da posição do robô permitindo a construção em tempo real do mapa do ambiente explorado em condições na qual a leitura odométrica apresenta imperfeições (ruídos, deslizamentos, etc.).

6/66

Metodologia● Para verificação da solução proposta

foram utilizados 5 (cinco) ambientes estruturados (ambientes que podem ser descritos por primitivas geométricas) e estáticos (sem pessoas ou objetos móveis).

● Foram efetuados um total de dez execuções, uma para cada algoritmo em teste no ambiente.

Introdução (3)Objetivo Metodologia (1/3)MRDS (4)SLAM (11)Scan Matching (13)Solução Proposta (7)Resultados (15)Conclusão (2)Trabalhos Futuros (3).

7/66

Metodologia● Em cada experimento, os mapas

internos do robô são construídos utilizando os dados fornecidos pelos sensores aplicados aos algoritmos em teste.

● Ambientes de teste, sensores e robô simulados no MRDS.


8/66

Metodologia● Os algoritmos são avaliados com

relação ao mapa e posição real (ground truth).

● São apurados os índices de desempenho propostos:● Erro Absoluto de Posição (EAA);● Erro Absoluto Angular (EAA).● Teste Kolmogorov-Smirnov (KS).


9/66

MICROSOFT® ROBOTICS DEVELOPER STUDIO (MRDS)● Ambiente de desenvolvimento para

aplicações robóticas baseado em Windows®.

● Suporte a uma ampla variedade de robôs comerciais.

● Possui um runtime “leve” assíncrono orientado a serviços.

● Conjunto de ferramentas para autoria e simulação.

Introdução (3)Objetivo Metodologia (3)MRDS (1/4)SLAM (11)Scan Matching (13)Solução Proposta (7)Resultados (15)Conclusão (2)Trabalhos Futuros (3).

10/66

MICROSOFT® ROBOTICS DEVELOPER STUDIO (MRDS)● Concurrency and Coordination Runtime

(CCR)● Biblioteca de código gerenciado

(DLL) .Net (CLR).● Facilita a manipulação de I/O

assincrona eliminando a complexidade envolvida com tratamento de threads, locks e semáforos.


11/66

MICROSOFT® ROBOTICS DEVELOPER STUDIO (MRDS)● Distributed Software Services (DSS)

Bloco básico representando um serviço DSS e exemplo de funcionamento em conjunto com CCR (Forwarder). Fonte: (MICROSOFT, 2009).


12/66

MICROSOFT® ROBOTICS DEVELOPER STUDIO (MRDS)


13/66

Localização e Mapeamento Simultâneo (SLAM)

Fonte: http://www.cs.washington.edu/homes/letchner/classwork/cs327/MobileRoboticsMiniPresentation.ppt

Introdução (3)Objetivo Metodologia (3)MRDS (4)SLAM (1/11)Scan Matching (13)Solução Proposta (7)Resultados (15)Conclusão (2)Trabalhos Futuros (3).

14/66

Localização e Mapeamento Simultâneo (SLAM)● Posição do Robô

x

y

(a) 2D (plano) – (x, y, θ)(b) 3D (espaço) – (x, y, z, φ, θ, ψ)

(a) (b)Introdução (3)Objetivo Metodologia (3)MRDS (4)SLAM (2/11)Scan Matching (13)Solução Proposta (7)Resultados (15)Conclusão (2)Trabalhos Futuros (3).

15/66

Localização e Mapeamento Simultâneo (SLAM)● Localização

Dados: ● Mapa Global.● Posição inicial.● Leituras dos sensores.

Obter:● Posição do robô enquanto se

movimenta.Introdução (3)Objetivo Metodologia (3)MRDS (4)SLAM (3/11)Scan Matching (13)Solução Proposta (7)Resultados (15)Conclusão (2)Trabalhos Futuros (3).

16/66


● Dead-recknoning (navegação estimada) – odometria, sensores inerciais.

● Problema: integração do erro.● GPS

● Problema: restrito a ambientes externos.● Marcos com localização conhecida.

● Problema: restrito a ambientes conhecidos.


17/66


● Baseada em mapa: mapa global construído incrementalmente a partir de sucessivas leituras dos sensores (mapa local) enquanto robô explora o ambiente.

● Problema: necessidade de sensores precisos e ambientes com características físicas de fácil distinção.


18/66

Localização e Mapeamento Simultâneo (SLAM)● Mapeamento

● Mapas Métricos: (a) Grades de Ocupação e (b) Nuvem-de-Pontos .

(a) (b)


19/66


● Mapas Topológicos: Grafos (a) e Diagramas de Voronoi (b).

(a) (b)Fonte: (SLAMET et al., 2006)


20/66


● Mapas Baseados em Características: Mapas de Marcos (Landmark Maps).

Exemplo estimação do mapa e posição do veiculo via filtro de Kalman.Fonte: Thrun (2002).


21/66

Localização e Mapeamento Simultâneo (SLAM)● Segundo Thrun (2002) ao lidar com

situações reais, uma solução para o problema SLAM deve lidar com os seguintes aspectos:● Limitações e ruídos dos sensores.● Acúmulo de erros.● Dimensionalidade dos dados.● Correta associação entre dados do

ambiente, tratamento de obstáculos e ambientes dinâmicos.


22/66

Localização e Mapeamento Simultâneo (SLAM)● Abordagens

● EKF um dos mais utilizados na solução SLAM (THRUN; BURGARD; FOX, 2005).

● Mapas de marcos são empregados para calcular estimativas de posicionamento e modelar as incertezas relativas a solução gerada.

● Utiliza modelo estatístico de sensores e movimentação do robô.


23/66

Localização e Mapeamento Simultâneo (SLAM)

● Abordagens● Correspondência (matching) entre marcos

(BORENSTEIN et al., 1997) ou entre pontos identificados pelo sensor no ambiente (LU; MILIOS, 1994).

● Abordagem essencialmente numérica e computacional (DISSANAYAKE et al., 2001).

● Boa parte destes métodos empregam variações do algoritmo ICP (Iterative Closest Point) desenvolvido por Besl e McKay (1992).


24/66

● Alinhamento entre conjunto de pontos (2D ou 3D).

● Conjunto observado é alinhado a um conjunto referência com o objetivo de obter a máxima correspondência entre seus elementos.

● Obter a translação e rotação necessária para a sobreposição máxima entre os conjuntos.

Scan Matching

Introdução (3)Objetivo Metodologia (3)MRDS (4)SLAM (11)Scan Matching (1/13)Solução Proposta (7)Resultados (15)Conclusão (2)Trabalhos Futuros (3).

25/66

● Segundo Lu e Milios (1994), pode-se definir também como um método de busca em que a menor distância entre os conjuntos é o estado final desejado.

● Usualmente a estimativa de deslocamento por dead-recknoning é utilizada como aproximação inicial para esta busca (LU; MILIOS,1994).

Scan Matching


26/66

Pode-se classificar estes algoritmos como:● Correspondências baseadas

apenas em características (feature to feature).

● Correspondências baseadas em pontos e características (point to feature).

● Correspondências baseadas apenas em pontos (point to point).

Scan Matching


27/66

Correspondência entre pontos gerados pelo sensor laser● Caso (point to point).● Neste trabalho, o sensor está

montado em um plano paralelo ao chão do robô, perfazendo um recorte 2D do ambiente.

Scan Matching


28/66

Scan Matching

(a) Direção de transmissão e ângulo máximo de escaneamento do Sick LMS200. Fonte: https://www.mysick.com(b) Configuração e leitura do sensor Sick no P3DX. Fonte: http://www.mbeckler.org/coursework/2007-2008/robotics_slam.pdf

(a) (b)


http://www.mbeckler.org/coursework/2007-2008/robotics_slam.pdf

29/66

Scan Matching

(a) (b)

(a) Coordenadas polares e cartesianas para o ponto e respectivas equações (b).

(3.1)

(3.2)

pik

=180º

=0,5ºi=0,... , N.N=360Introdução (3)

Objetivo Metodologia (3)MRDS (4)SLAM (11)Scan Matching (6/13)Solução Proposta (7)Resultados (15)Conclusão (2)Trabalhos Futuros (3).

30/66

Scan Matching

Os pontos de uma leitura devem ser associadas com os de outra.

(a) Antes do alinhamento e (b) Depois do alinhamento.

poskcorr

posk1corr

correspondência

(a) (b)

poskcorr

posk1odo

correspondência


31/66

Scan Matching● Leituras consecutivas (k e k+1)

apresentam um deslocamento relativo e um incremento rotacional entre os planos XY

K e XY

k+1 dado

por Tk.


32/66

Scan Matching

Para que seja estabelecida uma correta correspondência entre as leituras as novas devem ser projetadas em XY

K utilizando T

k.


33/66

Scan Matching● Correspondência

● Descarte de outliers e contagem de correspondências válidas


34/66

Scan Matching● Grau de sobreposição da transformação

● Índice de qualidade da transformação


35/66

Scan Matching● O Algoritmo ICP (Iterative Closest Point)


36/66

Scan Matching● O Algoritmo ICP – Tranformação rigida (MARTÍNEZ et al., 2006).


37/66

Solução Proposta● (a) Aproximação inicial (tempo discreto

k=0).● (b) Aproximação inicial (tempo discreto

k>0)

(a) (b)

poskcorr

posk1odo

correspondência

T kcand. 0

T k−1ótimo.

Introdução (3)Objetivo Metodologia (3)MRDS (4)SLAM (11)Scan Matching (13)Solução Proposta (1/7)Resultados (15)Conclusão (2)Trabalhos Futuros (3).

poskcorr

posk1odo

correspondência

T kcand. 0

38/66

Solução Proposta● (c) Testes adicionais baseados em índice

de qualidade.● (d) Será escolhida a transformação que

apresentar o menor valor para o índice para a equação (3.9).

correspondência

poskcorr T k

aprox. 1

T kaprox. 2

T kaprox. 3

T kcand.4

T kótimo


(c) (d)

correspondência

poskcorr

T kcand.1 T k

cand. 2

T kcand. 3

T kcand. 4

39/66

Solução Proposta● Mapas: local e global.● Passos:

1. Projeta-se mapa local utilizando a estimativa odométrica.

2.3. Calcula-se a qualidade desta

transformação dado pela equação (3.9)4. Se índice calculado para a transformação

for menor que limiar, a aproximação será considerada boa e esta será usada para inserir as informações do mapa local no mapa global.


40/66

Solução Proposta

Passos:5. Caso não (valor maior que limiar), então

são efetuados os seguintes testes adicionais.


41/66

Solução Proposta● Passos:

5. (Continuação)


42/66

Solução Proposta

Passos:6. A transformação que apresentar o

menor índice será empregada.


43/66

Solução Proposta●


44/66

Resultados● Ambiente B – Mapa 3D e Trajetória.

Ambiente com MAIOR “rugosidade”.

Introdução (3)Objetivo Metodologia (3)MRDS (4)SLAM (11)Scan Matching (13)Solução Proposta (77)Resultados (1/15)Conclusão (2)Trabalhos Futuros (3).

45/66

Resultados● Ambiente B - Trajetórias


46/66

Resultados● Ambiente B

Ambiente B: EAP, EAA


47/66


Ambiente B: Mapas Finais


48/66

Resultados● Ambiente B - Diferença relativa de

89, 1% para ed final

(m) e 74, 64% para e

θ final(º).


49/66

Resultados● Ambiente B -Teste KS


50/66

Resultados● Ambiente E – Mapa 3D e Trajetória.

Ambiente com MENOR “rugosidade”.


51/66

Resultados● Ambiente E - Trajetórias


52/66


Ambiente E: EAP, EAA


53/66


Ambiente E: Mapas Finais


54/66

Resultados● Ambiente E - Diferença relativa maior que

90% para os valores finais dos erros em favor do ICP.


55/66

Resultados● Ambiente E – Teste KS

● Observa-se que tanto as correções efetuadas pelo ICP, quanto as efetuadas pela solução não foram muito significativas para a correção de deslocamento. Entretanto foram efetivas para a correção angular.


56/66

ResultadosDemais Ambientes● Ambiente A - Diferença relativa de

95,59% para ed final

(m) e 95,24% para e

θ final(º) para solução.


57/66

ResultadosDemais Ambientes● Ambiente C - Diferença relativa de

80,16% para ed final

(m) e 24,45% para e

θ final(º) para a solução.


58/66

ResultadosDemais Ambientes● Ambiente D - Diferenças relativas

maiores que 90% em favor do ICP.


59/66

Conclusão● Através dos resultados obtidos pode-se

afirmar que este método obtém êxito em alguns casos, principalmente aqueles em que o ambiente possui uma maior ”rugosidade”.

● Para estes ambientes, os resultados foram sensivelmente superiores, obtendo uma diferença relativa maior que 50% para os valores finais dos erros de deslocamento e maior que 20% para os valores finais dos erros angulares (ambientes A, B e C).

Introdução (3)Objetivo Metodologia (3)MRDS (4)SLAM (11)Scan Matching (13)Solução Proposta (7)Resultados (15)Conclusão (1/2)Trabalhos Futuros (3).

60/66

Conclusão● Para os ambientes com menor

“rugosidade” (D e E) observou-se que tanto o ICP quanto a solução conseguem melhorar a estimativa, mas não muito devido a ambiguidade destes ambientes.

Introdução (3)Objetivo Metodologia (3)MRDS (4)SLAM (11)Scan Matching (13)Solução Proposta (7)Resultados (15)Conclusão (2/2)Trabalhos Futuros (3).

61/66

Trabalhos Futuros

Para melhorar a solução proposta, pode-se considerar a utilização em conjunto com:● Métodos probabilísticos como ferramenta

para se minimizar os efeitos dos problemas causados pela ambiguidade presente em certos ambientes. Durante as simulações este fenômeno demostrou-se mais significativo do que era esperado.

● Métodos de aprendizado por reforço como em Monteiro e Ribeiro (2004), também poderiam diminuir os efeitos causados pela ambiguidade.

Introdução (3)Objetivo Metodologia (3)MRDS (4)SLAM (11)Scan Matching (13)Solução Proposta (7)Resultados (15)Conclusão (2)Trabalhos Futuros (1/3)

62/66

Trabalhos Futuros● Fusão de vários mapas de robôs diferentes,

utilizando um método como o descrito em Biber e Strasser (2006).

● Inserção de métodos para aceleração da seleção e descarte prévios de outliers. O tempo de processamento pode ser reduzido significativamente se implementado um método que permita diminuir o número de comparações em cada iteração, ou ao menos mantê-lo constante.


63/66

Trabalhos Futuros● Adição de métodos para detecção de ciclos

no ambiente e semelhança entre regiões previamente exploradas. O uso em conjunto com um método como o proposto em Gutmann e Konolige (1999), no qual é feito registo local e correlação global, pode apresentar bons resultados.

● Utilização em conjunto com algoritmos para planejamento de rotas: pode ser empregado juntamente com mapas topológicos, para construção de algoritmos de navegação, ou algum outro método como, por exemplo, VFH (BORENSTEIN; KOREN, 1991).


64/66

Aplicações● Robotic mapping/floor plan

technologyhttp://www.youtube.com/watch?v=jxMVshpYYyk&NR=1

http://www.youtube.com/watch?v=jxMVshpYYyk&NR=1

65/66

Links Interesantes● SLAM for dummies

http://ocw.mit.edu/NR/rdonlyres/Aeronautics-and-Astronautics/16-412JSpring-2005/9D8DB59F-24EC-4B75-BA7A-F0916BAB2440/0/1aslam_blas_repo.pdf

● OpenSlamhttp://www.openslam.org/

● The Mobile Robot Programming Toolkit (MRPT)http://babel.isa.uma.es/mrpt/index.php/Main_Page

● The Rawseeds Projecthttp://www.rawseeds.org

● Professional Microsoft Robotics Developer Studio Companion Web Site.http://www.promrds.com

66/66

CENTRO TÉCNICO AEROESPACIALINSTITUTO TECNOLÓGICO DE AERONÁUTICA

DIVISÃO DE ENGENHARIA ELETRÔNICADEPARTAMENTO DE SISTEMAS DE CONTROLE









São José dos Campos, 23 de dezembro de 2009São José dos Campos, 23 de dezembro de 2009

Documents

LOCALIZAÇÃO E MAPEAMENTO EM TEMPO REAL UTILIZANDO ROBÔ SIMULADO NO MICROSOFT® ROBOTICS DEVELOPER STUDIO