aula 6 aula 6 - mecatronica.eesc.usp.br · aula 6 aula 6 – introdução à robótica móvel...
TRANSCRIPT
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
• Localização Probabilística baseada
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
IntroduçãoIntrodução
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)
EESC-USP © M. Becker 2009
(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
IntroduçãoIntrodução
• 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.
EESC-USP © M. Becker 2009 5/96
IntroduçãoIntrodução
• 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
EESC-USP © M. Becker 2009 6/96
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
EESC-USP © M. Becker 2009
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.
EESC-USP © M. Becker 2009
– 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)
EESC-USP © M. Becker 2009
– 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)
EESC-USP © M. Becker 2009
– 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
EESC-USP © M. Becker 2009
– 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
EESC-USP © M. Becker 2009 12/96
IntroduçãoIntrodução• Robô com Acionamento Diferencial
− Cinemática
EESC-USP © M. Becker 2009 13/96
IntroduçãoIntrodução• Modelo de Erro
EESC-USP © M. Becker 2009 14/96
IntroduçãoIntrodução• Crescimento da
Incerteza para movimentos em linha reta
– Observe que os erros
EESC-USP © M. Becker 2009
– 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-
EESC-USP © M. Becker 2009
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
EESC-USP © M. Becker 2009
Trajetória pré-programadaem quadrado 4x4m
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
EESC-USP © M. Becker 2009
Trajetória pré-programadaem quadrado 4x4m
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
EESC-USP © M. Becker 2009 19/96
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)
EESC-USP © M. Becker 2009
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
ΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣΣ
EESC-USP © M. Becker 2009
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
EESC-USP © M. Becker 2009
SensoresSensores
Localização / Mapeamento
Cognição / Planejamento
Controle de Movimento
AtuadoresAtuadores
22/96
•• IntroduçãoIntrodução
•• Representações da Crença e MapasRepresentações da Crença e Mapas
• Localização Probabilística baseada
Sumário da AulaSumário da Aula
EESC-USP © M. Becker 2009
• Localização Probabilística baseada
em Mapas
• Exemplos de outros Sistemas de
Localização
• Bibliografia Recomendada
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)
EESC-USP © M. Becker 2009
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)
EESC-USP © M. Becker 2009 25/96
Abordagem BayesianaAbordagem Bayesiana• Taxonomia de Modelos Probabilísticos
Mais Geral
EESC-USP © M. Becker 2009
Mais Específico
26/96
Crença em Hipótese ÚnicaCrença em Hipótese Única• Mapa de linhas contínuas (b)
EESC-USP © M. Becker 2009 27/96
Crença em Hipótese ÚnicaCrença em Hipótese Única• Mapa de Grid (c) e Topológico (d)
EESC-USP © M. Becker 2009 28/96
Crença em Hipótese MúltiplaCrença em Hipótese Múltipla• Representação baseada em grid (20cm²)
EESC-USP © M. Becker 2009
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
EESC-USP © M. Becker 2009
• 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)
EESC-USP © M. Becker 2009
• 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)
EESC-USP © M. Becker 2009 32/96
Representação do AmbienteRepresentação do Ambiente
• Discretização
– Decomposição exata por células
EESC-USP © M. Becker 2009 33/96
Representação do AmbienteRepresentação do Ambiente
• Discretização
– Decomposição por células de dimensão fixa• Passagens estreitas desaparecem...
EESC-USP © M. Becker 2009 34/96
Representação do AmbienteRepresentação do Ambiente
• Discretização
– Decomposição por células de dimensão fixa• Exemplo com células muito pequenas...
EESC-USP © M. Becker 2009 35/96
Representação do AmbienteRepresentação do Ambiente
• Discretização
– Decomposição por células adaptativas
EESC-USP © M. Becker 2009 36/96
Representação do AmbienteRepresentação do Ambiente
• Discretização
– Mapa Topológico
EESC-USP © M. Becker 2009 37/96
Representação do AmbienteRepresentação do Ambiente
• Discretização
– Mapa Topológico
EESC-USP © M. Becker 2009
Nó
Arco de Conectividade
38/96
Representação do AmbienteRepresentação do Ambiente
• Discretização
– Mapa Topológico
EESC-USP © M. Becker 2009 39/96
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...
EESC-USP © M. Becker 2009
• Espaços Abertos...
• Como construir a topologia (fronteiras dos nós)
• Fusão Sensorial
• ...
40/96
•• IntroduçãoIntrodução
•• Representações da Crença e MapasRepresentações da Crença e Mapas
•• Localização Probabilística baseada Localização Probabilística baseada
Sumário da AulaSumário da Aula
EESC-USP © M. Becker 2009
•• Localização Probabilística baseada Localização Probabilística baseada em Mapas em Mapas
• Exemplos de outros Sistemas de
Localização
• Bibliografia Recomendada
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á
EESC-USP © M. Becker 2009
• 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
EESC-USP © M. Becker 2009
• 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
EESC-USP © M. Becker 2009 44/96
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
EESC-USP © M. Becker 2009
– 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
EESC-USP © M. Becker 2009 46/96
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
EESC-USP © M. Becker 2009
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
EESC-USP © M. Becker 2008
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
Localização por Localização por MarkovMarkov
• 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
EESC-USP © M. Becker 2009
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
• Regras do Produto
� p(A ∧ B) = p(A|B)p(B)
� p(A ∧ B) = p(B|A)p(A)
EESC-USP © M. Becker 2009
� p(A ∧ B) = p(B|A)p(A)
• Regras de Bayes
� p(A|B) = p(B|A)p(A)
p(B)
50/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)
EESC-USP © M. Becker 2009
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
EESC-USP © M. Becker 2009
– 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
EESC-USP © M. Becker 2009 53/96
Localização por Localização por MarkovMarkov
• Caso # 1 – Mapa Topológico com Sonar
– Dervish Robot – ambiente tipo escritório
EESC-USP © M. Becker 2009 54/96
Localização por Localização por MarkovMarkov• Caso # 1 – Mapa Topológico com Sonar
– Dervish Robot – ambiente tipo escritório
– Atualização da crença para a posição n dada a percepção i
� p(n|i) = p(i|n) p(n)
EESC-USP © M. Becker 2009
• 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
Localização por Localização por MarkovMarkov
• Caso # 1 – Mapa Topológico com Sonar
– Dervish Robot – ambiente tipo escritório
• 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
EESC-USP © M. Becker 2009
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;
EESC-USP © M. Becker 2009
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
Localização por Localização por MarkovMarkov
• 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’ )
EESC-USP © M. Becker 2009
• 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:
EESC-USP © M. Becker 2009
– 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
EESC-USP © M. Becker 2009
– 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
EESC-USP © M. Becker 2009 61/96
Localização por Localização por MarkovMarkov• Caso # 3: Localização em um museu [Thrun]
• Laser Scan # 1
EESC-USP © M. Becker 2009 62/96
Localização por Localização por MarkovMarkov• Laser Scan # 2
EESC-USP © M. Becker 2009 63/96
Localização por Localização por MarkovMarkov• Laser Scan # 3
EESC-USP © M. Becker 2009 64/96
Localização por Localização por MarkovMarkov• Laser Scan # 13
EESC-USP © M. Becker 2009 65/96
Localização por Localização por MarkovMarkov• Laser Scan # 21
EESC-USP © M. Becker 2009 66/96
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)
EESC-USP © M. Becker 2009
– 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
EESC-USP © M. Becker 2009
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:
^
^
EESC-USP © M. Becker 2009
– Encontrando o erro mínimo
– Re-arranjando o termos:
69/96
Localização por Localização por KalmanKalman
• Introdução ao Filtro de Kalman
EESC-USP © M. Becker 2009 70/96
Localização por Localização por KalmanKalman
• Introdução ao Filtro de Kalman
– Predição Dinâmica (robô se movendo)
– Movimento
EESC-USP © M. Becker 2009
– Fusão
71/96
Localização por Localização por KalmanKalman
• 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):
EESC-USP © M. Becker 2009 72/96
Localização por Localização por KalmanKalman
• Exemplo:
Odometria
EESC-USP © M. Becker 2009 73/96
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
EESC-USP © M. Becker 2009
• 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
Localização por Localização por KalmanKalman
• Exemplo
EESC-USP © M. Becker 2009 75/96
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
^
EESC-USP © M. Becker 2009
– 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
EESC-USP © M. Becker 2009
trajetória
77/96
Localização por Localização por KalmanKalman• Exemplo
– 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
EESC-USP © M. Becker 2009
– Baseado na Fig. do Slide anterior, a transformação é dada por:
– E seu Jacobiano:
78/96
Localização por Localização por KalmanKalman• Exemplo
– 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
EESC-USP © M. Becker 2009
• 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-α)
EESC-USP © M. Becker 2009 80/96
Localização por Localização por KalmanKalman• Exemplo
– Matching
• Para encontrar a correspondência entre a predição e a observação, a distância de Mahalanobis é empregada:
EESC-USP © M. Becker 2009
• Com
81/96
Filtros Tipo BayesFiltros Tipo Bayes
• Exemplo
– Aplicando o Filtro de Kalman
• Ganho:
EESC-USP © M. Becker 2009
• Atualizando aestimativa da posição do robô
• A variância associada:
82/96
Localização por Localização por KalmanKalman
• 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:
^
EESC-USP © M. Becker 2009 83/96
Localização por Localização por KalmanKalman
• 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
^
EESC-USP © M. Becker 2009
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
•• IntroduçãoIntrodução
•• Representações da Crença e MapasRepresentações da Crença e Mapas
•• Localização Probabilística baseada Localização Probabilística baseada
Sumário da AulaSumário da Aula
EESC-USP © M. Becker 2009
•• Localização Probabilística baseada Localização Probabilística baseada em Mapas em Mapas
•• Exemplos de outros Sistemas de Exemplos de outros Sistemas de LocalizaçãoLocalização
• Bibliografia Recomendada
85/96
Outros Sist. de LocalizaçãoOutros Sist. de Localização
• Localização baseada em Artificial Landmarks
EESC-USP © M. Becker 2009 86/96
Outros Sist. de LocalizaçãoOutros Sist. de Localização
• Localização baseada em Artificial Landmarks
EESC-USP © M. Becker 2009 87/96
Outros Sist. de LocalizaçãoOutros Sist. de Localização
• Localização baseada em Artificial Landmarks
EESC-USP © M. Becker 2009 88/96
Outros Sist. de LocalizaçãoOutros Sist. de Localização
• Posicionamento através de Beacon Systems(Triangulação)
EESC-USP © M. Becker 2009 89/96
Outros Sist. de LocalizaçãoOutros Sist. de Localização
• Posicionamento através de Beacon Systems(Triangulação)
EESC-USP © M. Becker 2009 90/96
Outros Sist. de LocalizaçãoOutros Sist. de Localização
• Posicionamento através de Beacon Systems(Triangulação)
EESC-USP © M. Becker 2009 91/96
Outros Sist. de LocalizaçãoOutros Sist. de Localização
• Posicionamento através de Beacon Systems(Docking)
EESC-USP © M. Becker 2009 92/96
Outros Sist. de LocalizaçãoOutros Sist. de Localização
• Posicionamento através de Beacon Systems(Código de barras)
EESC-USP © M. Becker 2009 93/96
Outros Sist. de LocalizaçãoOutros Sist. de Localização
• Posicionamento através de Beacon Systems(CONAC)
EESC-USP © M. Becker 2009 94/96
•• IntroduçãoIntrodução
•• Representações da Crença e MapasRepresentações da Crença e Mapas
•• Localização Probabilística baseada Localização Probabilística baseada
Sumário da AulaSumário da Aula
EESC-USP © M. Becker 2009
•• Localização Probabilística baseada Localização Probabilística baseada em Mapas em Mapas
•• 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
EESC-USP © M. Becker 2009 96/96
• Trhun, S., Burgard, W., and Fox, D., 2005, Probabilistic Robotics, The MIT Press, ISBN 0-262-20162-3.