aula 6 aula 6 - mecatronica.eesc.usp.br · aula 6 aula 6 – introdução à robótica móvel...

Aula 6 Aula 6 –– Introdução à Robótica MóvelIntrodução à Robótica Móvel

LocalizaçãoLocalizaçãoLocalizaçãoLocalização

Prof. Dr.Prof. Dr. Marcelo BeckerMarcelo Becker

EESC - USP

•• IntroduçãoIntrodução

• Representações da Crença e Mapas

• Localização Probabilística baseada

Sumário da AulaSumário da Aula

EESC-USP © M. Becker 2009


em Mapas

• Exemplos de outros Sistemas de

Localização

• Bibliografia Recomendada

2/96

IntroduçãoIntrodução

• Odometria

• Localização baseada em sensores externos, beacons, ou landmarks

• Localização Probabilística baseada em • Localização Probabilística baseada em MAPAS

EESC-USP © M. Becker 2009 3/96


EncoderEncoderPredição da

Posição(p.e: odometria)

Predição da Posição

(p.e: odometria)

Atualização da Posição

(Estimativa?)

Atualização da Posição

(Estimativa?)

Posição

Estimada

Observações e Predições que “casaram” (matched)


(p.e: odometria)(p.e: odometria)

MAPA(data base)

MAPA(data base)

ObservaçõesObservaçõesPer

cepç

ãoDados dos Sensores ou Características do ambiente extraídas

Posição Predita

SIMSIM

“casaram” (matched)

4/96


• Desafios da Localização:

– Conhecimento da Posição absoluta (p.e.: GPS) não é suficiente

– Localização “humana” em relação ao – Localização “humana” em relação ao ambiente

– Percepção e movimento são importantes:

• Ruído dos sensores,

• Efeito Aliasing nos sensores,

• Estimativa da posição por odometria,

• Etc.



• Ruído dos Sensores

– Reduzem drasticamente as informações úteis dos sensores.

– Muito influenciado pelas características do ambiente (p.e.: iluminação, rugosidade da ambiente (p.e.: iluminação, rugosidade da superfície, etc.)

– Influenciado também pelo princípio de medida do sensor (p.e.: interferência entre sensores de US)

– Pode-se lidar com eles:• Utilizando múltiplas leituras

• Aplicando fusão temporal ou de multissensores


IntroduçãoIntrodução• Efeito Aliasing

– Em robótica, o padrão é empregar múltiplas leituras de sensores

– Com múltiplos sensores, há uma multiplicidade de mapeamentos e estimativas de localização


mapeamentos e estimativas de localização baseados nos dados de entrada do sistema de percepção do robô

• Logo, a quantidade de informação obtida pelos sensores é geralmente insuficiente para estimar a posição do robô através de uma única leitura

– Localização é baseada em uma série de leituras

– Informação suficiente é obtida ao longo do tempo

7/96

IntroduçãoIntrodução• Odometria e Dead Reckoning

– A atualização da posição é baseada em sensores proprioceptivos

• Odometria: sensores das rodas

• Dead reckoning: sensores de orientação tb.


– O movimento do robô, sentido pelos encoderse/ou sensores de orientação são integrados para se obter a posição:☺ Fácil e rápido � Erros tb. são integrados

• O uso de sensores adicionais para a orientação (p.e.: giroscópios) pode ajudar a reduzir os erros acumulados, mas o problema persiste...

8/96

Introdução Introdução • Fonte dos erros em Odometria

– Erros determinísticos podem ser eliminados

Determinísticos(sistemáticos)

Não-Determinísticos(não sistemáticos)


– Erros determinísticos podem ser eliminados através de calibração

– Erros não determinísticos tem que ser descritos através de modelos matemáticos e sempre serão fontes de incerteza na estimativa da posição do robô

9/96

Introdução Introdução • Maiores Fontes dos erros em Odometria

– Resolução limitada durante a integração (incrementos de tempo, resolução de medida, ...)

– Desalinhamento das rodas (determinístico)

– Rodas de diâmetros diferentes (determinístico)


– Rodas de diâmetros diferentes (determinístico)

– Variação no contato das rodas

– Contato desigual (escorregamento, não planar, etc.)

10/96

IntroduçãoIntrodução• Erros de Integração

– Range Error: integral da distância da trajetória do movimento do robô

• Soma dos movimentos das rodas

– Turn Error: idem para curvas


– Turn Error: idem para curvas• Diferença dos movimentos das rodas

– Drift Error: A diferença no erro das rodas induz ao erro na orientação angular do robô...

Após longos de tempo, Após longos de tempo, turnturn e e driftdrift errorserrors são são muito maiores que muito maiores que range range errerroorsrs!!!!

11/96

IntroduçãoIntrodução• Robô com Acionamento Diferencial


IntroduçãoIntrodução• Robô com Acionamento Diferencial

− Cinemática


IntroduçãoIntrodução• Modelo de Erro


IntroduçãoIntrodução• Crescimento da

Incerteza para movimentos em linha reta

– Observe que os erros


– Observe que os erros na direção perpendi-cular ao movimento crescem muito mais rápido...

15/96

IntroduçãoIntrodução• Crescimento da

Incerteza para movimentos circulares

– Observe que as elipses de erro não permane-


de erro não permane-cem perpendiculares ao movimento...

16/96

IntroduçãoIntrodução• Calibração de Erros [Borenstein]

– Experimento com trajetória em quadrado unidirecional


Trajetória pré-programadaem quadrado 4x4m


Curva de 87°ao invés de 90°(devido à incerteza na distância entre as rodas)Trajetória Curva, e não reta, devido à diferença nos ∅s

das rodas (erro de orientação de 3°)

17/96

IntroduçãoIntrodução• Calibração de Erros [Borenstein]

– Experimento com trajetória em quadrado bidirecional



Trajetória Curva devido à diferença nos ∅s das rodas (erro de orientação de 3°)

Curva de 93°ao invés de 90°(devido à incerteza na distância entre as rodas)

18/96

IntroduçãoIntrodução• Erros determinísticos e não

determinísticos


IntroduçãoIntrodução• Localizar ou não?

– Como navegar entre A e B?• Navegar sem atingir obstáculos e detectar o posição do

goal

– Possível através da estratégia de sempre seguir as paredes (pela esquerda)


paredes (pela esquerda)• Como detectar que a posição desejada foi atingida?

B

20/96

IntroduçãoIntrodução• Navegação Baseada em Comportamento

Comunique os Dados

Descubra novas Áreas

ΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣ


SensoresSensoresDetecte a Posição do Goal

Evite Obstáculos

Siga a Parede à Direita/ à Esquerda

ΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣ AtuadoresAtuadores

Fusão

21/96

IntroduçãoIntrodução• Navegação Baseada em Modelos

Percepção

Localização / Mapeamento


SensoresSensores

Localização / Mapeamento

Cognição / Planejamento

Controle de Movimento

AtuadoresAtuadores

22/96


•• Representações da Crença e MapasRepresentações da Crença e Mapas





em Mapas


Localização


23/96

Representação da CrençaRepresentação da Crença

• Mapas Contínuos com hipóteses simples (a)

• Mapas Contínuos com hipóteses múltiplas (b)


hipóteses múltiplas (b)

• Mapas discretos com distribuição de probabilidade (c)

• Mapas Discretos Topológicos com probabilidade (d)

24/96

Representação da CrençaRepresentação da Crença• Contínua

– Precisão limitada pelos dados dos sensores

– Tipicamente uma estimativa de pose baseada em hipótese

• Discreta– Precisão limitada pela

resolução de discretização

– Tipicamente uma estimativa de pose baseada em múltiplas hipótesesbaseada em hipótese

única

– “Perdido” enquanto divergindo

– Representação compacta e razoável em termos computacionais

múltiplas hipóteses

– Nunca está “perdido” (qdo

diverge, converge para outra célula)

– Necessita de considerável poder computacional e de memória (não para mapas topológicos)


Abordagem BayesianaAbordagem Bayesiana• Taxonomia de Modelos Probabilísticos

Mais Geral


Mais Específico

26/96

Crença em Hipótese ÚnicaCrença em Hipótese Única• Mapa de linhas contínuas (b)


Crença em Hipótese ÚnicaCrença em Hipótese Única• Mapa de Grid (c) e Topológico (d)


Crença em Hipótese MúltiplaCrença em Hipótese Múltipla• Representação baseada em grid (20cm²)


Trajetória do Robô Crenças das Posições do Robô para 2, 3 e 4

29/96

Representações de Mapas Representações de Mapas

• Precisão vs. Aplicação

• Precisão das características (features) vs. Precisão do mapa

• Precisão vs. Complexidade


• Precisão vs. Complexidade computacional

– Representação contínua

– Decomposição (discretização)

30/96

Representação do AmbienteRepresentação do Ambiente• Formas:

– Contínua Métrica → x,y,θ

– Discreta Métrica → grid métrico

– Discreta Topológica → grid topológico

• Modelagem:– Raw data (p.e.: dados de sensores laser)


• Grande volume de dados

• Utiliza todas as informações adquiridas

– Características de Baixo Nível (geometria)

• Médio volume de dados

• Filtra informações úteis (ainda tem ambiguidades)

– Características de Alto Nível (carro, portas, árvores, etc.)

• Baixo volume de dados

• Filtra informações úteis, sem ambiguidades, informações insuficientes

31/96

Representação do AmbienteRepresentação do Ambiente

• Linhas contínuas

– Mapa arquitetural (a)

– Representação com conjunto de linhas infinitas (b)



• Discretização

– Decomposição exata por células



• Discretização

– Decomposição por células de dimensão fixa• Passagens estreitas desaparecem...



• Discretização

– Decomposição por células de dimensão fixa• Exemplo com células muito pequenas...



• Discretização

– Decomposição por células adaptativas



• Discretização

– Mapa Topológico



• Discretização



Nó

Arco de Conectividade

38/96


• Discretização



Desafios na Rep. por MapasDesafios na Rep. por Mapas

• O mundo real é dinâmico

• A percepção ainda é o maior desafio

– Propensa a erros

– A extração de informações úteis é difícil

• Espaços Abertos...


• Espaços Abertos...

• Como construir a topologia (fronteiras dos nós)

• Fusão Sensorial

• ...

40/96



•• Localização Probabilística baseada Localização Probabilística baseada



•• Localização Probabilística baseada Localização Probabilística baseada em Mapas em Mapas


Localização


41/96

Localização Probabilística Localização Probabilística baseada em Mapasbaseada em Mapas• Considere um robô movimentando-se em um

ambiente conhecido

• Quando inicia o movimento, sabe-se exatamente a sua posição. Assim, pode-se rastrear sua posição através da odometria

• Entretanto, durante o seu movimento, o robô ficará


• Entretanto, durante o seu movimento, o robô ficará incerto com relação a sua posição (propagação dos erros...)

• Atualiza-se a sua posição baseado na sua percepção do ambiente– Pode-se usar fusão sensorial com a odometria para estimar

a real posição do robô

42/96

Localização Probabilística Localização Probabilística baseada em Mapasbaseada em Mapas• Atualização da Ação

– Modelo Act : s’t = Act(ot, st-1)

Com ot: medida do encoder e st-1: crença a priori da posição

• Aumenta a incerteza

• Atualização da Percepção


• Atualização da Percepção

– Modelo See : st = See(it, s’t)

Com it: dados dos sensores exteroceptivos e s’t: posição

atualizada

• Diminui a incerteza

43/96

Localização Probabilística Localização Probabilística baseada em Mapasbaseada em Mapas

• Aumentando a crença através do movimento


Localização Probabilística Localização Probabilística baseada em Mapasbaseada em Mapas• Dados:

– A posição estimada p(k|k)

– Sua covariância Σ p(k|k) para o instante k

– A entrada do controle atual u(k)

– O conjunto de observações atual Z(k+1) e


– O conjunto de observações atual Z(k+1) e

– O mapa M(k)

• Calcule

– Nova estimativa (a posteriori) para a posição p(k+1|k+1)

– Sua covariância Σ p(k+1|k+1)

45/96

Localização Probabilística Localização Probabilística baseada em Mapasbaseada em Mapas• 5 passos para localização baseada em

mapas


Localização Probabilística Localização Probabilística baseada em Mapasbaseada em Mapas• Localização: Markov ↔ Filtro de Kalman

� Markov� Localização inicia a partir de qualquer posição desconhecida;� Recupera-se de situações ambíguas;

� Filtro de Kalman� Rastreia o robô e é inerentemente muito preciso e eficiente;� Entretanto, se a incerteza do robô se torna muito grande (p.e.: após a


ambíguas;� Entretanto, atualizar a probabilidade de todas as posições em todo o espaço de estados a qualquer instante de tempo, exige uma representação discreta do espaço (grid). OS requisitos de memória e processamento podem ser grandes se o grid for de pequenas dimensões.

se torna muito grande (p.e.: após a colisão com um objeto) o filtro irá falhar e a posição estará definitivamente perdida.

47/96

Localização por Localização por MarkovMarkov

• Emprega uma representação discreta para a probabilidade em todas as posições no espaço de estados;

• Usualmente emprega um grid ou mapa topológico (grafo) para representar o


topológico (grafo) para representar o ambiente através de um número finito de estados (posições);

• Durante cada atualização, a probabilidade de cada estado (elemento do mapa)de toda o espaço é atualizado.

48/96


• Aplicando a teoria da probabilidade ...

– P(A): probabilidade de que A é verdade• P.e.: p(rt=1): probabilidade de que o robô r está na posição

l no instante de tempo t

– Deseja-se calcular a probabilidade de cada posição individual do robô. Dadas as ações do


posição individual do robô. Dadas as ações do robô e as medidas dos seus sensores

– P(A|B): Probabilidade condicional de A sabendo B• P.e.: p(rt=l|it): probabilidade de que o robô r está na

posição l, dadas as leituras do sensor it, no instante t

49/96

Localização por Localização por MarkovMarkov• Mapa a partir de uma crença de um estado e dos

dados dos sensores, empregando o Modelo See:

� p(l|i) = p(i|l) p(l)

p(i)


p(i)

• Onde: p(l) é a crença antes do processo de atualização; p(i|l) é a probabilidade de obter uma medida i estando na posição l; e p(i) é o fator de normalização para que a soma de todos l para Lseja igual a 1.

51/96

Localização por Localização por MarkovMarkov• Mapa a partir de uma crença de um estado e dos

dados dos sensores, empregando o Modelo Act:

� p(lt|ot) = ∫ p(lt|l’t-1 ,ot) p(l’t-1)dl’t-1

– Somando todos os modos possíveis pelos quais o robô pode


– Somando todos os modos possíveis pelos quais o robô pode chegar a l.

• Hipótese de Markov: a atualização depende apenas do estado anterior,das ações mais recentes do robô e das observações mais recentes da percepção.

52/96

Localização por Localização por MarkovMarkov• Caso # 1 – Mapa Topológico com Sonar

– Dervish Robot – ambiente tipo escritório



• Caso # 1 – Mapa Topológico com Sonar



Localização por Localização por MarkovMarkov• Caso # 1 – Mapa Topológico com Sonar


– Atualização da crença para a posição n dada a percepção i

� p(n|i) = p(i|n) p(n)


• Onde: p(n|i) é a nova probabilidade de estar na posição n; p(n) é a crença atual; e p(i|n) é a probabilidade de ver i em n (tabela...)

– Não faz-se a atualização!! Mas, o robô está se movendo e a ação do robô e sua percepção serão empregadas para a atualização:

� p(nt|it) = ∫ p(nt|n’t-i ,it) p(n’t-i)dn’t-i

• t-i é usado ao invés de t-1 porque a distância em um mapa topológico entre n’ e n pode ser extremamente dependente do mapa

55/96


• Caso # 1 – Mapa Topológico com Sonar


• O cálculo de p(nt|n’t-1,it) é feito pela multiplicação da probabilidade da geração do evento i na posição n pela probabilidade de ter uma falha na


posição n pela probabilidade de ter uma falha na geração do evento s em todos os nós entre n’ e n.

� p(nt|n’t-1,it) = p(it,nt). p(∅,nt-1). p(∅,nt-2). ... . p(∅,nt-i+1).

56/96

Localização por Localização por MarkovMarkov• Exemplo de Cálculo

– Assuma que o robô tenha 2 crenças de estado não nulas :• p(1-2) = 1.0 e p(2-3) = 0,2 (as probabilidades não precisam somar 1!!)

– Tem-se certeza que ele está se movendo em direção LESTE• O estado 2-3 irá progredir para 3 e o estado 3-4, para 4;

• O estado 3 e o 3-4 podem ser eliminados pois, a probabilidade de detectar uma porta aberta é nula;


detectar uma porta aberta é nula;

• A probabilidade de atingir o estado 4 é o produto da probabilidade inicial p(2-3) = 0,2 por:

– (a) a probabilidade de encontrar algo no nó 3 e a probabilidade de encontrar o corredor à esquerda e a porta à direita no nó 4 e

– (b) a probabilidade de encontrar o corredor à esquerda e a porta à direita no nó 4.

• Por simplicidade, assume-se que a probabilidade de detectar nada no nó 3-4 é 1,0.

• Tem-se então:– 0,2 . [0,6 . 0,4 . 0,05] . 0,7 . [0,9 . 0,1] → p (4) = 0,003

– De modo similar para o nó 1-2: → p (2) = 0,3

57/96


• Uma decomposição fina do grid(x,y,θ), 15cm x 15cm x 1° :– Atualização da ação e da percepção:

– Ação:• Soma das posições anteriores possíveis e do modelo de movimento:

• p(l |o ) = ∑ p(l |l’ ,o ) p(l’ )


• p(lt|ot) = ∑ p(lt|l’t-1 ,ot) p(l’t-1)

– Percepção:• Dada a percepção i, qual a probabilidade

de estar na posição l:

� p(l|i) = p(i|l) p(l)p(i)

58/96

Localização por Localização por MarkovMarkov• O desafio é o cálculo de p(i|l)

– O número de possíveis leituras dos sensores e de dados geométricos é muito alto

– p(i|l) é calculado usando um modelo do comportamento dos sensores do robô, sua posição e o mapa métrico local da região em torno de l

– Hipótese:


– Hipótese:

• Os erros de medida podem ser descritos por uma distribuição com média

59/96

Localização por Localização por MarkovMarkov• Caso # 1 – Mapa de Grid

• O caso unidimensional– Início

• Nenhum conhecimento, logo, tem-se uma distribuição uniforme

– O robô detecta o 1° pilar


– O robô detecta o 1° pilar• Observando um pilar, a probabilidade

de estar em qualquer pilar é a mesma

– O robô se move• O modelo de Action possibilita estimar a

nova distribuição de probabilidade baseada na distribuição anterior e no movimento

– O robô detecta o 2° pilar• Baseado no conhecimento a priori, a

probabilidade de estar no pilar 2 torna-se dominante

60/96

Localização por Localização por MarkovMarkov• Caso # 2 – Mapa de Grid


Localização por Localização por MarkovMarkov• Caso # 3: Localização em um museu [Thrun]

• Laser Scan # 1


Localização por Localização por MarkovMarkov• Laser Scan # 2


Localização por Localização por MarkovMarkov• Comentários:

– O uso de grids menores resulta em grande necessidade de memória e processamento

– Várias técnicas foram desenvolvidas para reduzir a complexidade (reduzir a quantidade de estados que são atualizados a cada passo)


– Amostragem Aleatória / Filtro de Partículas

• Crença aproximada dos estados (p.e.: apenas 10% das possíveis posições são atualizadas)

• O processo de amostragem é baseado em “pesos”, ou seja, locais próximos a picos de probabilidade tem mais “peso”

• Tem-se que garantir que locais mais distantes ainda são considerados para evitar que o robô fique “perdido”

67/96

Localização por Localização por KalmanKalman• Ideia

Fonte de Erros do Sistema

Controle Sistema


Controle Sistema

Estimativa Ótima do Estado do

Sistema

Estado do Sistema (desejado, mas desconhecido)

Medidas Observadas

Sensores

Fonte de Erros dos Sensores

68/96

Localização por Localização por KalmanKalman

• Introdução ao Filtro de Kalman

– Duas medidas• q1 = q1 com variância σ1

2

• q2 = q2 com variância σ22

– S:

^

^


– Encontrando o erro mínimo

– Re-arranjando o termos:

69/96



– Predição Dinâmica (robô se movendo)

– Movimento


– Fusão

71/96


• Exemplo

– No primeiro passo, a posição do robô no instante k+1 é predita com base em sua posição anterior (instante k) e seu movimento devido à ação de controle u(k):



• Exemplo:

Odometria


Localização por Localização por KalmanKalman• O 2° passo é obter a observação Z(k+1) dos sensores a uma

nova posição no instante k+1

• A observação geralmente consiste em um conjunto n0 de observações simples zj(k+1) extraídas de diferentes sensores. Ela pode ser representada por raw data ou features (linhas, portas, ou qualquer landmark)

• As observações são realizadas no sistema de referência do


• As observações são realizadas no sistema de referência do sensor {S}

– As observações têm que ser transformadas para o sistema inercial

{W}

– A predição das medidas têm que ser transformadas para o

sistema do sensor {S}• Essas transformações utilizam a função hi (mais a frente...)

74/96


• Exemplo


Localização por Localização por KalmanKalman• Predição da medida

– No passo seguinte, utiliza-se a predição da posição do robô: p = (k+1|k) e o mapa M(k) para gerar as observações preditas zt.

– Elas têm de ser transformadas para o sistema do sensor

^


– Assim, pode-se definir a predição das medidas como o conjunto contendo todas as ni observações preditas

– A função hi é predominantemente uma transformação de coordenadas entre os sistemas do sensor e o inercial.

76/96

Localização por Localização por KalmanKalman• Exemplo

– Para a predição, apenas as paredes que estão no campo de visão são selecionadas

– Isto é feito através da correspondência das linhas (paredes) com o nó da trajetória


trajetória

77/96


– As predições generalizadas das medidas têm de ser transformadas para o sistema do robô {R}

– Baseado na Fig. do Slide anterior, a transformação é dada


– Baseado na Fig. do Slide anterior, a transformação é dada por:

– E seu Jacobiano:

78/96


– Matching

• Comparação entre as observações zj(k+1) dos sensores, com os componentes zt do mapa (armazenados)

• Para cada predição de medida em que foi encontrada uma observação correspondente, calcular a inovação


• A covariância da inovação é obtida pela lei de propagação do erro

• A validade da correspondência entre as medidas e as predições podem ser avaliadas, p.e., pela distância de Mahalanobis:

79/96

Localização por Localização por KalmanKalman• Exemplo de Matching (r-α)



– Matching

• Para encontrar a correspondência entre a predição e a observação, a distância de Mahalanobis é empregada:


• Com

81/96

Filtros Tipo BayesFiltros Tipo Bayes

• Exemplo

– Aplicando o Filtro de Kalman

• Ganho:


• Atualizando aestimativa da posição do robô

• A variância associada:

82/96


• Estimativa: caso 1D

– Para o caso unidimensional, com ht(zt,p(k+1|k))= zt

pode-se mostrar que a estimativa corresponde ao filtro para 1 dimensão mostrado antes:

^



• Exemplo de estimativa

– Estimativa do Filtro de Kalman para a nova posição do robô p(k|k):

• Unificando (fusing) a predição da posição do robô (magenta) com

^


posição do robô (magenta) com a inovação devido às medidas (verde) obtém-se a atualização da estimativa da posição do robô (vermelho).

84/96







•• Exemplos de outros Sistemas de Exemplos de outros Sistemas de LocalizaçãoLocalização


85/96

Outros Sist. de LocalizaçãoOutros Sist. de Localização

• Localização baseada em Artificial Landmarks



• Posicionamento através de Beacon Systems(Triangulação)



• Posicionamento através de Beacon Systems(Docking)



• Posicionamento através de Beacon Systems(Código de barras)



• Posicionamento através de Beacon Systems(CONAC)








•• Exemplos de outros Sistemas de Exemplos de outros Sistemas de LocalizaçãoLocalização

•• Bibliografia RecomendadaBibliografia Recomendada

95/96

Bibliografia RecomendadaBibliografia Recomendada

• Siegwart, R. and Nourbakhsh, I.R., 2004, Introduction to Autonomous Mobile Robots, 1st Edition, MIT Press, ISBN 0-262-19502-X

• http://www.mobilerobots.org

• Trhun, S., Burgard, W., and Fox, D., 2005, Probabilistic


• Trhun, S., Burgard, W., and Fox, D., 2005, Probabilistic Robotics, The MIT Press, ISBN 0-262-20162-3.

aula 6 aula 6 - mecatronica.eesc.usp.br · aula 6 aula 6 – introdução à robótica móvel...

Documents