INTRODUÇÃO À ROBÓTICA MÓVEL

Transcrição

1 INTRODUÇÃO À ROBÓTICA MÓVEL Aua 25 Edson Prestes Departamento de Informática Teórica

2 Locaização Fitragem de Kaman Fitragem de kaman fornece uma abordagem recursiva para a estimação de estados de um sistema dinâmico na presença de ruídos. Ee é caracterizado por manter uma estimativa do estado corrente do sistema x e uma matriz de covariância P do erro de estimativa. Podemos visuaizar o Fitro de Kaman como sendo aquee que fornece como saída uma função de densidade probabiística Gaussiana com média x e covariancia P

3 Locaização Fitragem de Kaman Linear Simpes Observador Sistema: Predição: Correção : onde Atuaização:

4 Locaização Fitragem de Kaman Linear Simpes Observador Na atuaização, o ajuste no estado do robô é sempre perpendicuar ao hiperpano Erros na direção paraea à Ω nunca serão corrigidos. Como resutado, o estado estimado não convergirá para o estado rea do sistema na maioria dos casos[1]. A vantagem deste estimador é que as idéias usadas são as mesma do fitro de Kaman.

5 Locaização Fitragem de Kaman Observando com uma PDF O observador anterior produz uma estimativa que é um vetor. A estimativa produzida por um Fitro de Kaman é uma distribuição gaussiana mutivariada. Ou seja, o fitro de kaman aém de fornecer um vetor de estado estimado fornece também uma matriz de covariância do erro da estimativa P(k k).

6 Locaização Fitragem de Kaman Observando com uma PDF Considere o seguinte sistema com ruído na dinâmica, mas sem ruído nas medidas x(k+1) = F(k)x(k) + G(k) u(k)+v(k) y(k) = H(k)x(k) Ruído Onde v(k) 2 R n é um ruído gaussiano de média zero e matriz de covariância V(k). Usando, temos pois o vaor esperado de v(k) é zero

7 Locaização Fitragem de Kaman Observando com uma PDF A matriz de covariância é dada por Substituindo o vaor de e encontramos Após agumas manipuações temos

8 Locaização Fitragem de Kaman Observando com uma PDF Usando a propriedade da soma do vaor esperado, temos Como v(k) é independente de x(k) e de Como E[v(k)]=0, Torna-se

9 Locaização Fitragem de Kaman Observando com uma PDF Para encontrar a equação de atuaização, escohemos que seja o mais prováve ponto no hiperpano Ω ={ x 2 R n H(k+1)x = y(k+1) } ou seja, o que maximiza a distribuição gaussiana definida por e, i.e., A maximização de P(x), envove a minimização do expoente de e, i.e.,

10 Locaização Fitragem de Kaman Observando com uma PDF A distância e o produto interno de Mahaanobis[1] entre duas variáveis aeatórias x e y e matriz de covariância S são dados por Logo, a minimização de x y 2 M =(x y) T S 1 (x y) Consiste em cacuar x 2 Ω ou seja, de forma a minimizar = = - M

11 Locaização Fitragem de Kaman Observando com uma PDF é ortogona a com reação ao produto interno de mahaanobis, ou seja, infuenciada por P(k+1 k). Logo, x = P(k+1 k)h(k+1) T Considerando a inovação como Temos x = P(k+1 k)h(k+1) T Ki, para K 2 R p. De forma simiar ao apresentado no observador simpes, temos que ter

12 Locaização Fitragem de Kaman Observando com uma PDF Substituindo x, Encontramos

13 Locaização Fitragem de Kaman Observando com uma PDF Sistema: x(k+1) = F(k)x(k) + G(k) u(k)+v(k) y(k) = H(k)x(k) Predição: Correção: onde Atuaização:

14 Locaização Fitragem de Kaman Observando com uma PDF Como o observador simpes, este novo observador possui aguns probemas[1]: Devido ao fato de não termos considerado ruído na eitura dos sensores, as equações de atuaização farão com que a matriz de covariança se torne singuar. Uma matriz singuar possui determinante igua a 0, o que é coerente com o fato de que as medidas sensoriais não possuem incertezas. Isto prejudica a distribuição gaussiana que usa a inversa da matriz de covariância.

15 Locaização Fitragem de Kaman O fitro de Kaman usa os conceitos e mecanismos apresentados anteriormente, com a diferença de que o sistema é descrito por x(k+1) = F(k)x(k) + G(k) u(k) + v(k) y(k) = H(k)x(k) + w(k) Onde u(k) 2 R m é a entrada para o sistema (veocidades, torque, etc); y(k)2 R p é a saída do sistema e contém os vaores dados peos sensores do robô F(k) 2 R n n codifica a dinâmica do sistema G(k) 2 R n m descreve como as entradas infuênciam a dinâmica. v(k) 2 R n é o ruído gaussiano com média 0 e matriz de covariância V(k). H(k)2 R p n descreve como o vetor de estado é mapeado para a saída w(k) 2 R p é o ruído gaussiano com média 0 e matriz de covariância W(k).

16 Locaização Fitragem de Kaman Observando com uma PDF Sistema: x(k+1) = F(k)x(k) + G(k) u(k)+v(k) y(k) = H(k)x(k) + w(k) Predição: Correção: onde Novo termo Atuaização:

17 Locaização Fitragem de Kaman Exempo Dead Reckoning Considere um robô que se move em inha reta com estado e u como sendo a força que se apica ao robô. Sabemos pea segunda ei de newton que ou Logo, o próximo estado do robô é dado por Ruído gaussiano com média zero O robô possui um sensor que mede veocidade.

18 Locaização Fitragem de Kaman Exempo Dead Reckoning Em [1], são assumidos os seguintes parâmetros m=1, T=0.5, W=0.5 (variância na medida) e matriz de covariância V No instante k

19 Locaização Fitragem de Kaman Exempo Dead Reckoning Figuras extraídas de [1]

20 Locaização Fitragem de Kaman Exempo Dead Reckoning A incerteza diminuiu na direção vertica devido à informação obtida peo sensor de veocidade, porém existe perda de informação na direção da posição. Este probema não ocorre devido ao Fitro de Kaman, mas devido ao fato do sistema não ser observáve, ou seja, o estado do sistema não pode ser recuperado a partir das saídas disponíveis. Logo, o erro esperado irá crescer iimitadamente. Se o sistema é observáve, a estimativa fornecida peo Fitro de Kaman converge, ou seja, o erro esperado entre o estado atua e o estado estimado é imitado. Figura extraída de [1]

21 Locaização Fitragem de Kaman Fitro de Kaman Estendido Em gera, os sistemas dinâmicos são não-ineares. Portanto, temos que estender o Fitro de Kaman para tratar a não-inearidade do sistema. Considere o seguinte sistema A fase de predição é dada peas equações onde

22 Locaização Fitragem de Kaman Fitro de Kaman Estendido Correção: onde e

23 Locaização Fitragem de Kaman Exempo - Fitro de Kaman Estendido Considere um robô capaz de detectar andmarks previamente mapeados no ambiente. O estado do robô no instante k é dado por E a entrada de controe é dada por as veocidades inear e anguar. onde u 1 (k) e u 2 (k) representam O sistema dinâmico é dado por O ambiente possui n andmarks com posição porém o robô consegue apenas ver apenas uma quantidade p(k) a cada instante k. A função abaixo associa cada andmark visto com os conhecidos previamente.

24 Locaização Fitragem de Kaman Exempo - Fitro de Kaman Estendido A saída do sistema é dada por com Como a dinâmica do sistema é dada por A Jacobiana de f é Lembrando que

25 Locaização Fitragem de Kaman Exempo - Fitro de Kaman Estendido A matriz H que é usada para produzir a saída do Fitro é definida por com Lembrando que

26 Locaização Fitragem de Kaman Exempo - Fitro de Kaman Estendido A abordagem apresentada assume que a associação entre cada medida com o andmark já conhecido é feita corretamente, sem erros. Na prática, esta associação pode não ser tão fáci!! já que os andmarks podem ser muito parecidos. Este probema é chamado Associação de Dados, ou Data Association. A idéia básica para a associação de dados é considerar para cada medida i ( y i (k+1)) e andmark j um vaor de inovação v ij A medida y i (k+1) é associada ao andmark j que produz a menor inovação de acordo com a norma de Mahaanobis. Esta norma incorpora as incertezas associadas à predição e às medidas.

27 Locaização Fitragem de Kaman Exempo - Fitro de Kaman Estendido Figura extraída de [1]

28 Simpe SLAM - Fitragem de Kaman Estado do sistema Posição do robô Veocidade no eixo x e no eixo y Posições dos andmarks Erros associados ao desocamento A cada instante de tempo o robô consegue ver todos os andmarks, e pode distingui-os. Não temos o probema de associação de dados.

29 Simpe SLAM - Fitragem de Kaman Estimativa da posição para o andmark i Saída produzida peo Fitro Através deste conjunto de equações para o Fitro de Kaman Estendido, estamos estimando tanto a posição do robô quando dos andmarks Matriz de covariância associada ao ruído w(k)

30 Simpe SLAM - Fitragem de Kaman Como transformar este exempo em um SLAM efetivo, capaz de fornecer uma estimativa sobre a posição do robô e dos andmarks desconhecidos presentes no ambiente? A chave é verificar se um andmark é novo ou é agum já armazenado no mapa. Isto é feito através norma de Mahaanobis apicada ao termo de inovação Se o vaor mínimo encontrado é maior que um dado imiar, então o andmark é novo devendo ser iniciaizado e adicionado ao mapa.

31 BIBLIOGRAFIA [1] H. Choset, K. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L.Kavraki, S. Thrun. Principes of Robot Motion : Theory, Agorithms, and Impementations. MIT Press [2] D. Fox, W. Burgard and S. Thrun. Markov Locaization for mobie robots in dynamic environments. Journa of Artificia Inteigence Research (JAIR), 11: , [3] S.Thrun. Learning Metric-Topoogica Maps for Indoor Mobie Robot Navigation. Artificia Inteigence, v.99, n. 1, 21-71,1998. [4]. S. Thrun, W. Burgard, and D. Fox. Probabiistic Robotics. MIT Press, Cambridge, MA, 2005.