IA368-W Métodos Estocásticos em Robótica Móvel

Transcrição

1 IA368-W Métodos Estocásticos em Robótica Móvel Prof. Eleri Cardozo Prof. Eric Rohmer Colaboradores: Leonardo R. Olivi Paulo G. Pinheiro Ricardo S. Souza Fernando C.A. Pinho PROPAGAÇÃO DE ERROS EM ODOMETRIA

2 Propagação de Erros Sistema Entrada Saída Se o valor de entrada é uma variável aleatória modelada por uma PDF o que podemos dizer da variável de saída? s cos( ) s sin( ) Exemplo: x' x y ' y ' Como incertezas em x, y e se propagam para x', y' e '? 3 Propagação de Erros Propagação de erros em sistemas lineares n yk aki xi, k,..., m y = Ax i cov(x) = x (matriz de covariancia) cov(y) = cov(ax) = y = A x AT OBS: Suponha que as variáveis aleatórias x são independentes: x = diag( i ), i =.,,,.n. Mesmo assim y não é uma matriz diagonal, ou seja, erros nas variáveis yi e yj, i j são correlacionados. 4

3 Propagação de Erros Saída (y) Linearização em torno do estado atual df dx y y = f( x) f f : função de transferência y df ( x ). x dx Entrada (x) x x+ x 5 Propagação de Erros Generalização para n dimensões: x [x x... xn]t y [y y... ym]t f F (vetor de funções) = f ( x,..., xn ) f ( x,..., x ) n x matriz de covariancia x (n,n) T... f m ( x,..., xn ) y matriz de covariancia y (m,m) df (Jacobiano) = f dx f f f x x xn y f x f T 6

4 Propagação de Erros Se o vetor x puder ser particionado em [xp xq]t onde os elementos de xp e Xq são não correlacionados (independentes) então: 7 Propagação de Erros: Odometria Seja o robô em [x y ]T. Ao se deslocar para [x' y' ']T qual a incerteza associada à esta nova pose? Nosso sistema é o modelo cinemático aproximado do robô diferencial: x' x y ' y ' d e d e cos( ) 4b d e d e sin( ) f ( x, y,, d, e ) 4b d e b 8

5 Propagação de Erros: Odometria Supondo que as incertezas na determinação da pose (odômetro) e no deslocamento (atuadores) sejam não diretamente correlacionadas, temos: f p x f y f s sin( ) p f s cos( ) x s cos( ) f y s sin( ) 9 Propagação de Erros: Odometria p f T s sin( ) s cos( ) Se o robô inicia em uma pose conhecida com precisão temos: p p ' Ou seja, a incerteza na pose ocorrerá somente após o movimento se iniciar.

6 Propagação de Erros: Odometria f d s f e d e e cos( d ) 4b d e d e y sen( ) 4b d e b x f Propagação de Erros: Odometria Vamos considerar que os erros introduzidos pelos deslocamentos das rodas direita e esquerda sejam independentes e a matriz de covariança dada por: Ou seja, o variância é proporcional ao deslocamento realizado pelas rodas direita e esquerda, independentemente.

7 Propagação de Erros: Odometria Considerando que: x s cos( ) f y s sin( ) 3 Propagação de Erros: Odometria 4

8 Propagação de Erros: Odometria Algoritmo:. Robô em pose conhecida, por exemplo [ ]T. p = p = Δ = 3. Robô se desloca Δd e Δe (estime com /motion/vel) 4. Calcule 5. Faça p = p e vá para 3. 5 Propagação de Erros: Odometria Y X Por que a incerteza em Y é maior que em X? 6

9 Propagação de Erros: Odometria Suponha o robô se deslocando em linha reta (Δθ = ) 7 Propagação de Erros: Odometria Mesmo se deslocando em linha reta (x), erros são propagados também na direção perpendicular (y) e na orientação (θ). 8

10 Evolução dos Erros de Odometria Seja: s =.m = b =.5m kd = ke =.5 (5 mm/m) 9 Evolução dos Erros de Odometria 5 mm X 3 mm Y,6o

11 Evolução dos Erros de Odometria Mesmo exemplo com = 9o*i, i =,... 8 mm X 8 mm Y,6o Propagação de Erros: Aspetos Práticos Y pk+ Pk+ s Pk pk X GET /motion/pose para acessar Pk e Pk+

12 LOCALIZAÇÃO EMPREGANDO FILTRO DE KALMAN ESTENDIDO 3 Filtro de Kalman O Filtro de Kalman foi desenvolvido por Rudolf Kalman com base em trabalhos anteriores de Thorvald Thiele e Peter Swerling e contribuições posteriores de Richard Bucy. Alguns autores denominam Filtro de Kalman-Bucy. O Filtro de Kalman resolve o problema da estimação de estado em sistemas lineares ou linearizados (Filtro de Kalman Estendido). É a técnica de estimação de estado mais empregada, apesar de algumas de suas deficiências serem bem conhecidas. 4

13 Filtro de Kalman Entrada Sistema Dinâmico Saída Estado Sensores Como estimar o estado do sistema com base:. No seu modelo dinâmico (espaço de estados).. Na leitura dos sensores utilizados para inferir o estado. Supondo que a transição de estados (planta) e os sensores sofram a influência de erros não sistemáticos. 5 Filtro de Kalman O Filtro de Kalman provê um estimador ótimo* para o caso onde:. O sistema tem dinâmica linear.. O sistema é completamente observável (todos os estados podem ser estimados a partir do sensoriamento das saídas do sistema). 3. As leituras dos sensores são funções lineares do estado. 4. Os erros que afetam a transição de estado e a leitura dos sensores são independentes e possuem distribuições normais com média zero. (*): ótimo no sentido de minimizar a variância do estado estimado. 6

14 Filtro de Kalman O que precisamos conhecer para aplicar o Filtro de Kalman:. O modelo dinâmico do sistema:. A conexão dos sensores com o estado: 3. O estado inicial (x) e sua covariância ( ) O que obtemos do Filtro de Kalman: - O estado (xt) e sua covariância ( t) estimados no instante t a partir de informações obtidas no instant t- (filtro recursivo). 7 Filtro de Kalman A mecânica de utilização do Filtro de Kalman é a seguinte: Predição. Calculamos a média das variáveis de estado no instante atual t ( t) com base no estado obtido no instante anterior ( xt-) e do controle imposto ao sistema no instante atual ( ut).. Calculamos a covariância no estado atual t a partir da covariânçia no estado anterior t- (propagação de erros). Atualização 3. Calculamos o ganho do filtro Kt. 4. Efetuamos uma medida dos sensores ( zt). 5. Calculamos a inovação (zt - Ct t) 6. Corrigimos a média calculada ( t) e a covariância ( t) a partir do ganho do filtro e da inovação. 8

15 Filtro de Kalman D É possível estabelecer as relações do Filtro de Kalman para sistemas com vetor de estado unidimensional diretamente a partir das propriedades das distribuições Gaussianas. Modelo do sistema: xt axt bu t N (, x ) z t xt N (, z ) Predição: t axt bu t t a t x Estimativa de xt com base no modelo do sistema. Atualização: Como incorporar a leitura zt para diminuir a incerteza em xt? 9 Filtro de Kalman D p(x) N(xt, φt) N(μt, σt ) N(zt, σz) x Estimativa do estado com o modelo do sistema Estimativa do estado com o sensor Estimativa do estado combinada (modelo + sensor) 3

16 Filtro de Kalman D Caso particular: a =, b =, c = Regra de Bayes: P(xt+ zt) = (P(zt xt).p(xt)) = N(zt, z).n(xt t) Na multiplicação de duas Gaussianas: t t z t z t z t Multiplicando por xt e substituindo xt por μt e zt no lado direito: t zt t z xt ( ) t t z t t z t z t z t Kt t z xt t K t ( z t t ), t t Fusão de t com zt. 3 Filtro de Kalman D Observa-se pela expressão anterior que se o sensor é ruim ( z grande) a estimação do estado é influenciada mais pelo modelo do sistema. Se o sensor é bom ( z pequeno) a estimação é influenciada mais pelo sensor. Note que o desvio padrão da nova estimativa ( t) é menor que o da estimativa antes da fusão ( t). O Filtro de Kalman D pode ser considerado um Filtro de Bayes onde foi acrescida a dinâmica do sistema que fornece uma predição do estado. O que Kalman fez foi generalizar o Filtro para dimensões maiores e adaptá-lo para sistemas não lineares (como o nosso robô!). 3

17 Filtro de Kalman xt At xt Bt ut t zt Ct xt t t ~ N, Rt t ~ N, Qt x é um vetor de dimensão n u é um vetor de dimensão m A é uma matriz de dimensão n x n B é uma matriz de dimensão n x m C é uma matriz de dimensão k x n R é uma matrix de dimensão n x n, simétrica e positiva semidefinida Q é uma matrix de dimensão k x k, simétrica e positiva semidefinida OBS: At, Bt, Ct, Rt e Qt usualmente não variam com o tempo. 33 Filtro de Kalman Predição: t At t Bu t t At t AtT Rt Atualização: K t t C tt (C t t C tt Qt ) t t K t ( zt Ct t ) t ( I K t Ct ) t 34

18 Filtro de Kalman: Exemplo Faixa de erros do sensor u=a-g v O sensor consegue estimar qual é a altura do avião num tempo t. 35 Filtro de Kalman: Exemplo ut st st vt t ( t ) vt vt u t t A variável de controle é a aceleração vertical (u) imprimida pela asa. Queremos estimar st. Adicionando os erros causados por vibrações, turbulências, etc: ut ( t ) N, s vt vt u t t N, v st st vt t 36

19 Filtro de Kalman: Exemplo Modelo no espaço de estados ( t é constante): ( t ) st N (, s ) t st u v v t N (, ) t t v t R Modelo do sensor (exemplo: altímetro): z t C t xt N (, z ) s z t t N (, z ) vt Q Na prática, tudo o que se tem é a leitura do sensor z. 37 Filtro de Kalman: Exemplo t A ( t ) B t s R s v v s v Q z C 38

20 Filtro de Kalman: Exemplo Fase de Predição t st t st t ut vt t vt t t t t s v R 39 Filtro de Kalman: Exemplo Fase de Atualização K t t t N (, ) z xt t K t z t t t K t t 4

21 Filtro de Kalman: Exemplo Construções em Octave/Matlab úteis para implementar o Filtro de Kalman: randn(k) - matriz k x k de números aleatórios gerados a partir da distribuição N(, ). eye(k) - matriz identidade de dimensão k. inv(m) - matriz inversa de M. operadores matriciais: * (multiplicação), + (soma), ' (transposição), \ (solução de equação linear). 4 Filtro de Kalman: Exemplo t.5 s v. z 8. u.5 (m / s ) 4

22 Filtro de Kalman: Vantagens Converge mesmo com estado inicial muito impreciso. Uma única hipótese é mantida (outro extremo de Processos de Markov). Fornece uma estimativa da incerteza do estado em cada estágio. Permite a incorporação de toda a informação (sensores) disponível. Implementação simples com álgebra de matrizes. 43 Filtro de Kalman: Desvantagens Otimalidade garantida apenas para sistemas lineares com ruído gaussiano. O mapeamento das leituras dos sensores e do estado (matriz C) raramente é linear na prática. Pode sofrer divergência por instabilidade numérica (inversão de matrizes) ou por modelo incorreto (matrizes A, B, C). Requer calibração (determinação das matrizes R e Q). Não trata o problema do sequestro do robô (provê localização local). 44

23 Filtro de Kalman em Localização Robótica O estado que desejamos estimar é a pose atual do robô. O controle que exercemos sobre o robô é a velocidade linear e rotacional (ou velocidade de cada roda de um robô difererencial). Temos um modelo cinemático do robô diferencial (não linear!). Temos sensores para estimar o estado (laser, GPS, câmeras, etc.). Problema: como relacionar o estado (pose) atual com as leituras dos sensores (matriz C)? Dado que o modelo do sistema é não linear, devemos utilizar o Filtro de Kalman Estendido. 45 Filtro de Kalman Estendido Em sistemas com dinâmica não linear, as matrizes A, B e C são substituídas por vetores de funções g e h: xt g (ut, xt ) t zt h( xt ) t t ~ N, Rt t ~ N, Qt Se xt- possui distribuição gaussiana, xt e zt não preservarão esta distribuição como no caso de sistema linear. No filtro de Kalman Estendido as funções g e h são linearizadas em torno do último ponto de operação do sistema conhecido (xt-). Entretanto, a otimalidade do filtro não é mais garantida com a linearização. 46

24 Filtro de Kalman Estendido g (ut, xt ) g (ut, t ) Gt ( xt t ) g (ut, xt ) Gt g N (ut, xt ) x x N Gt é uma matrix N x N h( xt ) h( t ) H t ( xt t ) h ( t ) H t hm ( t ) x N x Ht é uma matrix M x N 47 Filtro de Kalman Estendido xt = g(ut, xt-) g(ut, μt-) Gt(xt- - μt-) Gt g μt- xt- 48

25 Filtro de Kalman Estendido Predição: t g (ut, t ) t Gt t GtT Rt Atualização: K t t H tt ( H t t H tt Qt ) t t K t ( zt h( t )) t ( I K t H t ) t 49 FK em Localização Robótica Variáveis de estado: xt yt t T st vt t t t t Variáveis de controle: Δs (ou v.δt) e Δθ (ou ω.δt): ut t xt st cos( t ) t g (ut, xt ) yt st sin( t ) t t Função g: Modelo cinemático do robô. 5

26 FK em Localização Robótica Jacobiano de g com relação a x, y e θ: t st sin( t ) Gt st cos( t t ) Para um robô diferencial Jacobiano de g com relação a Δsr e Δsl t st t cos( t ) 4b sen( t ) s Vt sen( t t ) t cos( t t ) 4b b s cos( t t ) t sen( t t ) 4b t st t sen( t ) cos( t ) 4b b 5 FK em Localização Robótica k s s r t k s sl Podemos usar o Jacobiano de g com relação a Δs e Δ t st t cos( t ) sen( t ) st Vt sen( t t ) cos( t t ) k s s t k 5

27 FK em Localização Robótica Lx xt Ly yt h( xt ) Supondo um landmark L Lx e Ly obtidos do mapa arctan Ly yt, Lx xt t Função h: L y yt Lx xt q q Ht L x L y yt x t q q q ( Lx xt ) ( Ly yt ) Jacobiano de h: 53 FK em Localização Robótica Fase de Predição: t x s cos( ) t t t t yt st sin( t ) t t t, x t t, y t, t Gt t GtT Vt t Vt T Rt Predição da pose do robô com base no seu modelo cinemático x N(,) Rt y N(,) N(,) Caso a pose inicial seja conhecida com precisão absoluta. 54

28 FK em Localização Robótica Fase de Atualização K t t H tt ( H t t H tt Qt ) t t K t ( z t h( t )) t ( I K t H t ) t d Qt th σd e σth são os desvios padrão do sensor para range e bearing (distância e angulação) 55 FK em Localização Robótica Z zt xt xt Z yt yt arctan Z yt yt, Z xt xt t Zx e Zy são features detectadas pelo Split-And-Merge (escolhidos por máxima verossimilança com os landmarks Lx e Ly). L xt L y y t h( t ) arctan Ly yt, Lx xt t x 56

29 FK em Localização Robótica Landmark real Ly rh Zy z- h rz-rh Landmark estimado rz y x rz rh z h Kt Lx Zx Atualize x, y e tal que rz-rh e z- h 57 FK em Localização Robótica Landmark real Ly Zy Landmark estimado rh y rz As distâncias e orientações estimadas pelo modelo e fornecidas pelo laser devem ficar próximas. x Zx Lx Após correção da pose pelo Filtro de Kalman. 58

30 FK em Localização Robótica É possível utilizar mais de uma feature no algoritmo? Por exemplo, aquelas com veressimilhança acima de? Sim, podemos adicionar os vetores z e h múltiplas features. Neste caso, para M features: a dimensão de z e h é M x a dimensão de Ht é M x 3 a dimensão de Qt é M x M a dimensão de Kt é 3 x M Podemos não apenas acrescentar múltiplas features, mas múltiplas features de naturezas diferentes detectadas por diferentes sensores (ex: laser e câmera). Entretanto, devemos evitar combinar sensores de mesma natureza muito precisos (ex: laser) com sensores muito imprecisos (ex: sonar). 59 FK em Localização Robótica O modelo cinemático fornece uma estimativa da odometria. Se o robô dispõe de odometria (encoders, giros, etc.) podemos utilizá-la na fase de predição do Filtro de Kalman para obter diretamente t? Sim, caso a odometria forneça estimativas com erro gaussianos e média zero. Entretanto, na prática a odometria apresenta erros cumulativos (média diferente de zero) e neste caso não deve ser utilizada como sensor adicional. Lx xt q L y yt Ht q L y yt q L x x t q ld Qt l ox oy o q ( Lx xt ) ( L y yt ) 6

31 FK em Localização Robótica Quando dispomos de odometria, outra alternativa é utilizá-la na predição da pose atual do robô, ao invés de estimar a pose a partir do modelo cinemático. Neste caso o modelo cinemático ainda é utilizado para computar a propagação da incerteza na pose (Σ). 6