Representações da Crença e Mapas Localização Probabilística baseada em Mapas Exemplos de outros Sistemas de Localização Bibliografia Recomendada

Transcrição

1 Aula 6 Introdução à Robótica Móvel Localização Prof. Dr. Marcelo Becker EESC - USP

2 Sumário da Aula Introdução Representações da Crença e Mapas Localização Probabilística baseada em Mapas Exemplos de outros Sistemas de Localização Bibliografia Recomendada 2/96

3 Introdução Odometria Localização baseada em sensores externos, beacons, ou landmarks Localização Probabilística baseada em MAPAS 3/96

4 Introdução Posição Estimada Atualização da Posição (Estimativa?) Encoder Predição da Posição (p.e: odometria) SIM Observações e Predições que casaram (matched) MAPA (data base) Posição Predita Percepção Observações Dados dos Sensores ou Características do ambiente extraídas 4/96

5 Introdução Desafios da Localização: Conhecimento da Posição absoluta (p.e.: GPS) não é suficiente 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. 5/96

6 Introduçã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 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 6/96

7 Introduçã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 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

8 Introduçã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 encoders e/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

9 Introdução Fonte dos erros em Odometria 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

10 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) Variação no contato das rodas Contato desigual (escorregamento, não planar, etc.) 10/96

11 Introduçã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 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, turn e drift errors são muito maiores que range errors rs!! 11/96

12 Introdução Robô com Acionamento Diferencial 12/96

13 Introdução Robô com Acionamento Diferencial Cinemática 13/96

14 Introdução Modelo de Erro 14/96

15 Introdução Crescimento da Incerteza para movimentos em linha reta Observe que os erros na direção perpendicular ao movimento crescem muito mais rápido... 15/96

16 Introdução Crescimento da Incerteza para movimentos circulares Observe que as elipses de erro não permanecem perpendiculares ao movimento... 16/96

17 Introdução Calibração de Erros [Borenstein] Experimento com trajetória em quadrado unidirecional Trajetória pré-programada em quadrado 4x4m Trajetória pré-programada em 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

18 Introduçã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) Trajetória pré-programada em quadrado 4x4m 18/96

19 Introdução Erros determinísticos e não determinísticos 19/96

20 Introduçã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) Como detectar que a posição desejada foi atingida? B 20/96

21 Introdução Navegação Baseada em Comportamento Comunique os Dados Descubra novas Áreas Sensores Detecte a Posição do Goal Evite Obstáculos Σ Atuadores Siga a Parede à Direita / à Esquerda Fusão 21/96

22 Introdução Navegação Baseada em Modelos Percepção Localização / Mapeamento Sensores Cognição / Planejamento Atuadores Controle de Movimento 22/96

24 Representação da Crença Mapas Contínuos com hipóteses simples (a) Mapas Contínuos com hipóteses múltiplas (b) Mapas discretos com distribuição de probabilidade (c) Mapas Discretos Topológicos com probabilidade (d) 24/96

25 Representação da Crença Contínua Precisão limitada pelos dados dos sensores Tipicamente uma estimativa de pose baseada em hipótese única Perdido enquanto divergindo Representação compacta e razoável em termos computacionais Discreta Precisão limitada pela resolução de discretização Tipicamente uma estimativa de pose baseada em múltiplas hipóteses Nunca está perdido (q do diverge, converge para outra célula) Necessita de considerável poder computacional e de memória (não para mapas topológicos) 25/96

26 Abordagem Bayesiana Taxonomia de Modelos Probabilísticos Mais Geral Mais Específico 26/96

27 Crença em Hipótese Única Mapa de linhas contínuas (b) 27/96

28 Crença em Hipótese Única Mapa de Grid (c) e Topológico (d) 28/96

29 Crenç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

30 Representações de Mapas Precisão vs. Aplicação Precisão das características (features) vs. Precisão do mapa Precisão vs. Complexidade computacional Representação contínua Decomposição (discretização) 30/96

31 Representaçã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

32 Representação do Ambiente Linhas contínuas Mapa arquitetural (a) Representação com conjunto de linhas infinitas (b) 32/96

33 Representação do Ambiente Discretização Decomposição exata por células 33/96

34 Representação do Ambiente Discretização Decomposição por células de dimensão fixa Passagens estreitas desaparecem... 34/96

35 Representação do Ambiente Discretização Decomposição por células de dimensão fixa Exemplo com células muito pequenas... 35/96

36 Representação do Ambiente Discretização Decomposição por células adaptativas 36/96

37 Representação do Ambiente Discretização Mapa Topológico 37/96

38 Representação do Ambiente Discretização Mapa Topológico Nó Arco de Conectividade 38/96

39 Representação do Ambiente Discretização Mapa Topológico 39/96

40 Desafios 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... Como construir a topologia (fronteiras dos nós) Fusão Sensorial... 40/96

42 Localização Probabilística baseada 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á 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

43 Localização Probabilística baseada em Mapas Atualização da Ação Modelo Act : s t = Act(o t, s t-1 ) Com o t : medida do encoder e s t-1 : crença a priori da posição Aumenta a incerteza Atualização da Percepção Modelo See : s t = See(i t, s t ) Com i t : dados dos sensores exteroceptivos e s t : posição atualizada Diminui a incerteza 43/96

44 Localização Probabilística baseada em Mapas Aumentando a crença através do movimento 44/96

45 Localização Probabilística baseada 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 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

46 Localização Probabilística baseada em Mapas 5 passos para localização baseada em mapas 46/96

47 Localização Probabilística baseada 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; 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. 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 colisão com um objeto) o filtro irá falhar e a posição estará definitivamente perdida. 47/96

48 Localização por Markov 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 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. EESC-USP M. Becker /96

49 Localização por Markov Aplicando a teoria da probabilidade... P(A): probabilidade de que A é verdade P.e.: p(r t =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 robô e as medidas dos seus sensores P(A B): Probabilidade condicional de A sabendo B P.e.: p(r t =l i t ): probabilidade de que o robô r está na posição l, dadas as leituras do sensor i t, no instante t 49/96

50 Localização por Markov Regras do Produto p(a B) = p(a B)p(B) p(a B) = p(b A)p(A) Regras de Bayes p(a B) = p(b A)p(A) p(b) 50/96

51 Localização por Markov 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) 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 L seja igual a 1. 51/96

52 Localização por Markov Mapa a partir de uma crença de um estado e dos dados dos sensores, empregando o Modelo Act: p(l t o t ) = p(l t l t-1,o t ) p(l t-1 )dl t-1 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

53 Localização por Markov Caso # 1 Mapa Topológico com Sonar Dervish Robot ambiente tipo escritório 53/96

54 Localização por Markov Caso # 1 Mapa Topológico com Sonar Dervish Robot ambiente tipo escritório 54/96

55 Localização por Markov 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) 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(n t i t ) = p(n t n t-i,i t ) 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

56 Localização por Markov Caso # 1 Mapa Topológico com Sonar Dervish Robot ambiente tipo escritório O cálculo de p(n t n t-1,i t ) é feito pela multiplicação da probabilidade da geração do evento i 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(n t n t-1,i t ) = p(i t,n t ). p(,n t-1 ). p(,n t-2 )..... p(,n t-i+1 ). 56/96

57 Localização por Markov 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; 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

58 Localização por Markov 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 t o t ) = p(l t l t-1,o t ) 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

59 Localização por Markov 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: Os erros de medida podem ser descritos por uma distribuição com média 59/96

60 Localização por Markov 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 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 tornase dominante 60/96

61 Localização por Markov Caso # 2 Mapa de Grid 61/96

62 Localização por Markov Caso # 3: Localização em um museu [Thrun] Laser Scan # 1 62/96

63 Localização por Markov Laser Scan # 2 63/96

67 Localização por Markov 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

68 Localização por Kalman Ideia Fonte de Erros do Sistema Controle Sistema Sensores Estado do Sistema (desejado, mas desconhecido) Medidas Observadas Estimativa Ótima do Estado do Sistema Fonte de Erros dos Sensores 68/96

69 Localização por Kalman Introdução ao Filtro de Kalman Duas medidas S: ^ q 1 = q 1 com variância σ 1 2 ^ q 2 = q 2 com variância σ 2 2 Encontrando o erro mínimo Re-arranjando o termos: 69/96

70 Localização por Kalman Introdução ao Filtro de Kalman 70/96

71 Localização por Kalman Introdução ao Filtro de Kalman Predição Dinâmica (robô se movendo) Movimento Fusão 71/96

72 Localização por Kalman 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): 72/96

73 Localização por Kalman Exemplo: Odometria 73/96

74 Localização por Kalman 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 n 0 de observações simples z j (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 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 h i (mais a frente...) 74/96

75 Localização por Kalman Exemplo 75/96

76 Localização por Kalman 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 z t. 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 n i observações preditas A função h i é predominantemente uma transformação de coordenadas entre os sistemas do sensor e o inercial. 76/96

77 Localização por Kalman 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 77/96

78 Localização por Kalman 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 por: E seu Jacobiano: 78/96

79 Localização por Kalman Exemplo Matching Comparação entre as observações z j (k+1) dos sensores, com os componentes z t 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

80 Localização por Kalman Exemplo de Matching (r-α) 80/96

81 Localização por Kalman Exemplo Matching Para encontrar a correspondência entre a predição e a observação, a distância de Mahalanobis é empregada: Com 81/96

82 Filtros Tipo Bayes Exemplo Aplicando o Filtro de Kalman Ganho: Atualizando aestimativa da posição do robô A variância associada: 82/96

83 Localização por Kalman Estimativa: caso 1D Para o caso unidimensional, com h t (z t,p(k+1 k))= ^ z t pode-se mostrar que a estimativa corresponde ao filtro para 1 dimensão mostrado antes: 83/96

84 Localização por Kalman 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 a inovação devido às medidas (verde) obtém-se a atualização da estimativa da posição do robô (vermelho). 84/96

86 Outros Sist. de Localização Localização baseada em Artificial Landmarks 86/96

89 Outros Sist. de Localização Posicionamento através de Beacon Systems (Triangulação) 89/96

92 Outros Sist. de Localização Posicionamento através de Beacon Systems (Docking) 92/96

93 Outros Sist. de Localização Posicionamento através de Beacon Systems (Código de barras) 93/96

94 Outros Sist. de Localização Posicionamento através de Beacon Systems (CONAC) 94/96

96 Bibliografia Recomendada Siegwart, R. and Nourbakhsh, I.R., 2004, Introduction to Autonomous Mobile Robots, 1st Edition, MIT Press, ISBN X Trhun, S., Burgard, W., and Fox, D., 2005, Probabilistic Robotics, The MIT Press, ISBN /96