Emparelhamento-Filtragem na Localização de Robots Móveis

Transcrição

1 O ciclo Predição-Emparelhamento Emparelhamento-Filtragem na Localização de Robots Móveis Maria Isabel Ribeiro Instituto Superior Técnico Instituto de Sistemas e Robótica mir@isr.ist.utl.pt Outubro de 1999 All rights reserved

2 Sumário Localização Absoluta com base no Ciclo prediçãoemparelhamento filtragem Predição Emparelhamento Filtragem Resultados Experimentais com sonar com laser Conclusões

3 Referências! Artur Arsénio, M. Isabel Ribeiro. Active Range Sensing for Mobile Robot Localization. In Proceedings da IEEE/RSJ International Conference on Intelligent Robotic Systems, (IROS 98), pp , Victoria, Canadá, Outubro de 1998.! Artur Arsénio, M. Isabel Ribeiro. Absolute Localization of Mobile Robots using Natural Landmarks. In Proceedings da IEEE International Conference on Electronics, Circuits and Systems, (ICECS 98), pp , Lisboa, Portugal, Setembro de 1998.! Frédéric Chenavier, James L. Crowley. Position Estimation for a Mobile Robot Using Vision and Odometry. In Proceedings of the 1992 IEEE International Conference on Robotics and Automation, pp , Nice, França, Maio de 1992.! James L. Crowley. World Modeling and Position Estimation for a Mobile Robot using Ultrasonic Ranging. In Proceedings of the IEEE International Conference on Robotics and Automation, pp , Arizona, EUA, Maio de 1989.! John J. Leonard, Hugh F. Durrant-Whyte. Mobile Robot Localization by Tracking Geometric Beacons. In IEEE Transactions on Robotics and Automation, pp , Vol.7, No.3, Junho de 1991.! John J. Leonard, Hugh F. Durrant-Whyte. Directed Sonar Sensing for Mobile Robot Navigation. Kluwer Academic Publishers, Boston, 1992.! Martin David Adams, Sensor Modeling, Design and Data Processing for Autonomous Navigation, World Scientific, 1999 (Chapter 8)

4 Localização relativa + localização absoluta Observações Processo cíclico composto por três fases EMPARELHAMENTO ACTUALIZAÇÃO PREDIÇÃO Mapa do mundo

5 Filtro de Kalman Estendido (EKF) Dinâmica do Sistema X(k + 1) = Z(k) = f(x(k),u(k)) + v(k) h(x(k)) + w(k) v(k) ~ N(0,Q(k)) w(k) ~ N(0,R(k)) EKF - Predição Xˆ (k + 1/ k) = f(xˆ (k / k),u(k)) df df Σ (k + 1/ k) = ΣX(k / k) dx dx EKF - Filtragem T X + Q(k) Xˆ (k + 1/ k + 1) = Xˆ (k + 1/ k) + K(k + 1)(Z(k + 1) h(xˆ (k + 1/ k)) T dh 1 K(k + 1) = ΣX(k + 1/ k) S (k + 1) dx T dh dh S(k + 1) = ΣX(k + 1/ k) + R(k + 1) dx dx Σ (k + 1/ k + 1) = Σ (k + 1/ k) K(k + 1)S(k + 1)K(k + 1) X X T

6 Predição Estimativa prevista da localização das observações Ẑ(k S(k + + 1/k) 1) Xˆ (k / k) PREDIÇÃO FILTR A G E M Σ X (k / k) Odometria Xˆ (k + 1/ k) Σ X (k + 1/ k) Modelo das observações ẑ i (k + 1/k) Mapa do mundo

7 Predição da localização Dinâmica do robot Dados X (k + 1) = f(x(k),u(k)) + Xˆ (k / k), PREDIÇÃO do estado Xˆ (k + 1/k) = Σ (k + 1/k) Σ X df dx f(xˆ v(k) (k / k),û(k),q(k) (k /k),û(k)) T x = Σx + Fase de PREDIÇÃO do EKF. df (k /k) dx Q(k) Dinâmica linearizada em torno da última estimativa filtrada

8 Predição das observações Ẑ(k + 1/k) S(k + 1) Xˆ (k + 1/ k) Σ X (k + 1/k) Modelo das observações Mapa do mundo " Mapa do mundo " Modelo das observações " Modelo dos sensores

9 Modelo das observações z (k) h (X (k),p ) w (k) j = i s i + j j = 1,..., m caracterização do alvo localização do sensor observação associada ao sensor j ruído de observação w (k) ~ N(0,R (k)) j j Z (k) = [ z T 1 (k) z2(k)! zm(k) ] tipo e caracterização do sensor processamento dos dados características geométricas consideradas no mapa localização do robot localização do sensor no robot

10 Modelo do sensor Caracterização da percepção espacial Caracterização dos erros 24 sensores de ultrasom (Polaroid) Laser Range Scanner=LRF+PTU

11 Modelo do sensor-sonar percepção espacial = r distância = r r β(r) d r ~ N(d, 2 σ d ) β t (d) σ d = r (m) 6 Distância (metros) largura de feixe 2 β t Largura de feixe efectiva como função da distância Orientação (graus)

12 Modelo do sensor-laser scanner percepção espacial = (distância,ângulo) =( r, ϕ ) LRF - incerteza radial PTU - incerteza angular r ~ N(d, σ ϕ ~ N( θ, 2 d ) 2 σ θ ) θ d σd = 1,5cm, σθ = 0, 05 o

13 Mapa do mundo " Pontos Y W p " Segmentos de recta p r p p = p p = r p θ x y p y {W} Y W {W} p r p x p θ X X W W Y " Landmarks naturais W p y {W} um ponto dois segmentos de recta p r2 p θ 2 p r1 p θ1 p " p x p pθ p = pθ p p X r1 r2 x y W 1 2

14 Predição da Localização do sensor y W s W Y (k) y(k) {W} R Y y R s x(k) x W s x R s (k) θ R s θ R s θ(k) W X R X X s Xˆ (k + x (k) = y θ W s W s W s 1/k) X (k) X s (k) (k) cosθ(k) (k) = + X(k) sinθ(k) (k) 0 sinθ(k) cosθ(k) 0 0 x 0 y 1 θ R s R s R s Xˆ s (k + + 1/k) Mapa do mundo + Modelo do sensor predição das observações

15 Predição das Observações " Pontos Y W p p = p p x y ŷ s (k + p y 1/ k ) rˆ ϕˆ θˆ s (k + 1/ k ) {W} xˆ s (k + 1/ k ) p x X W ẑi (k + 1/ k) = h i(xˆ s(k + 1/ k),p p ) h i = rˆ ϕ ˆ = (p x xˆ s ) p arctg p 2 y x + (p ŷ xˆ s s y θˆ ŷ s s ) 2 Sonar 2 d σ R i(k) = 0 0 σθ 2

16 Predição das Observações " Segmentos de recta p r p = r p θ ŷ s (k + Y W 1/ k ) p r p θ rˆ ϕˆ θˆ s (k + 1/ k ) {W} xˆ s (k + 1/ k ) X W ẑ (k 1/k) h (Xˆ (k i + = i s + 1/k),p p ) h i = rˆ ϕ ˆ = p r xˆ s cos(pθ) ŷ p θˆ θ s s sin(p θ ) Sonar 2 σ r 0 Ri(k) = σr 0 σϕ, σϕ são função de 2 2 σd, σθ Ri (k) = σ 2 d

17 Emparelhamento dados dos sensores ou características extraídas Observações z j (k + 1) EMPARELHAMENTO Variáveis aleatórias definidas no mesmo espaço de medida ẑ i (k + 1/ k) (zm(k + 1), ẑ (k + 1/k) m Sim FILTR A G E M Xˆ (k / k) Σ X (k / k) Odometria PREDIÇÃO Xˆ (k + 1/ k) Σ X (k + 1/ k) Modelo das observações Mapa do mundo

18 Emparelhamento " O que é o emparelhamento " Quais são os pares z (k + 1), ẑ (k 1/ k) ) válidos? ( j i + " Porque é necessário? " Pares inválidos podem levar à divergência do EKF " Para ignorar observações ruidosas (outliers) " sonar -reflexões múltiplas " laser - reflexões especulares " Para ignorar predições erradas ou inexistentes

19 Laser - Reflexões Especulares

20 Sonar - Predições Inexistentes W Y rˆ p θ β t θˆ s (k + 1/ k ) ŷ s (k + 1 / k ) Teste de visibilidade {W} xˆ s (k + 1 / k ) p θ θˆ s (k + 1/ k) βt 2 ϕˆ

21 Sonar - Predições Erradas " Observações previstas erradas OBSERVAÇÃO z MEDIDA PREVISTA ẑ objecto não representado no mapa localização real localização prevista z ẑ z ẑ

22 " Como se concretiza? vij ( k + 1) = z j(k + 1) ẑ i(k + 1/ k) Emparelhamento inovações T dh dh S (k 1) E[ (k 1) (k 1) ] i (k 1/ k) i ij + = vij + vij + = ΣX + + R i(k + 1) dx dx T v ij (k T ) S (k + 1) v (k + 1) ij ij γ região de validade centrada em ẑ i (k + 1/ k) laser Teste de inclusão de z j (k + 1) na região de validade centrada em ẑ i (k + 1/ k) z = r ϕ ẑ z z espaço das observações z z ẑ sonar z = [] r

23 Emparelhamento e Filtragem " Resultado do emparelhamento V(k + 1) = Z(k + 1) Ẑ(k + 1/ k) z z = z S(k + 1) = E[ V(k + 1) V(k + 1) 1 2 n (k + 1) ẑ (k + 1) ẑ # (k + 1) ẑ T ] = 1 dh dx 2 n (k + 1/ k) (k + 1/ k) (k + 1/ k) Σ X dh (k + 1/ k) dx T pares válidos + R(k + 1) " Filtragem dh dx T dh = 1 $ dx T dh n dx T dhi dx h = i X X= Xˆ (k+ 1/ k) R(k + 1) = diag(r 1 (k + 1), R2(k + 1), $,Rn(k + 1)) Xˆ (k + 1/k + 1) = Xˆ (k + 1/k) + K(k + 1)[Z(k + 1) Ẑ(k + 1/k)] K(k + 1), ΣX (k + 1/ k + 1)

24 E se não houver emparelhamentos? Observações z j (k + 1) Não Pode ser um erro sistemático EMPARELHAMENTO Sim ẑ i (k + 1/ k) FILTR A G E M PREDIÇÃO Mapa do mundo

25 Translação Detector de Bias Localização prevista Localização real Previsão das observações a partir da localização prevista direcções segundo as quais não é possível fazer predição de acordo com o modelo do sensor

28 Translação Detector de Bias Localização prevista Previsão das observações a partir da localização prevista Localização real Observações reais como se vistas da localização prevista direcções segundo as quais não é possível fazer predição de acordo com o modelo do sensor

29 Detector de Bias Translação Orientação Orientação e Translação

30 Detecção e correcção de Bias Xˆ (k + 1/k) Ẑ (k + 1/k) Z (k + 1) NÃO Há nº suficiente de emparelhamentos? detecção e correcção de bias novo vector Xˆ (k + 1/k) novo vector Ẑ (k + 1/k) Z (k + 1)

31 Experiência 1 - sonar depois do 1º passo de filtragem Localização real x = 2100 mm y = 2050 mm θ = 20 o Localização inicial dada por um operador x = 2400 mm σx = 100mm y = 1800 mm σy = 100mm θ = 17 o σ = 15o θ Estimativa da localização xˆ = 2190mm σx = 27mm ŷ = 2030mm σy = 26mm ˆ o σ = 3o θ = 19.9 θ Xˆ (0 / 0) = Xˆ (1/ 0) Xˆ (0 / 0) Xˆ (1/1) Y X Ẑ (1/ 0) Z(1 ) Observações dos sonares transladadas para a localização prevista Emparelhamento válido Direcções segundo as quais não é possível fazer predição

32 Localização real x = 2100 mm y = 2050 mm θ = 20 o Experiência 1 - sonar depois do 3º passo de filtragem Localização inicial dada por um operador x = 2400 mm y = 1800 mm θ = 17 o Estimativa da localização xˆ = 2134mm σx = 14mm ŷ = 2063mm σy = 14mm θ ˆ = 17.9 o σ = 2.4o Xˆ (3 / 2) θ Ẑ (3 / 2) Z(3) Xˆ (3 / 3) praticamente não se vê Observações dos sonares transladadas para a localização prevista Y X Emparelhamento válido Direcções segundo as quais não é possível fazer predição

33 Experiência 1 - sonar depois do 7º passo de filtragem Localização real x = 2100 mm y = 2050 mm θ = 20 o Localização inicial dada por um operador x = 2400 mm y = 1800 mm θ = 17 o Estimativa da localização xˆ = 2123mm σx = 14mm ŷ = 2067mm σy = 10mm ˆ o σ = 2.2o θ = 18.3 Xˆ (7 / 6) θ Xˆ (7 / 7) praticamente não se vê Y X Ẑ (7 / 6) Z(7) Observações dos sonares transladadas para a localização prevista Emparelhamento válido Direcções segundo as quais não é possível fazer predição

34 Experiência 2 - sonar Bias de translacção Bias de orientação Bias de orientação e translacção

35 Escolha da landmark óptima - laser " A partir de Xˆ (k + 1/k) há m landmarks (segmento + ponto+segmento) visíveis " O processo de aquisição de dados é lento. " Qual delas escolher para prosseguir para o passo de filtragem?

36 Escolha da landmark óptima - laser Previsão de m landmarks visíveis Matriz covariância inovação Ganho do filtro Matriz covariância a posteriori ẑ (k + 1/k) S1(k + 1/k) K1(k + 1) Σ1 X (k + 1/k 1 + ẑ (k + 1/k) S2(k + 1/k) K2(k + 1) Σ2X (k + 1/k 2 + 1) 1) Xˆ (k + 1/k) ẑ (k + 1/k) ẑ # S (k + 1/k) K (k + 1) X (k + 1/k i i i Σi + (k + 1/k) S # (k + 1/k) K (k + 1) X (k + 1/k m m m Σm + 1) 1) Sem que sejam feitas observações Qual a landmark óptima?

37 Critério de escolha da landmark óptima min ( det Σi X (k + 1/k + 1) ) Produto dos valores próprios Volume Paralelipipedo que engloba o elipsóide de erro Computacionalmente pesado min ( tr Σi X (k + 1/k + 1) ) Soma dos valores próprios É o critério implementado pelo filtro de Kalman Não é computacionalmente pesado min ( λ max ( Σi X (k + 1/k + 1) ) Maior valor próprio Representa a maior incerteza Computacionalmente pesado

38 Critério de escolha da landmark óptima " Dimensões diferentes (m e rad) 1 m 1 m 0.84 m 1 rad=57.3º 1 rad=57.3º 1 m 1 m

39 Critério de escolha da landmark óptima " Resultado da trilateração E 0 + E1 d 0 + d 1 E 0 + E 0 E 1 E d d d 0 d 1 1

40 Critério de escolha da landmark óptima " Aproximação poligonal da área de erro + E 0 E 1 90º robot 90º + E 0 E 1 robot robot 2ε Situação de erro mínimo robot α 2δ variação da área 2δ 4 δε sinα 2ε

41 Experiência 1 - laser Localização real x = m y = m θ = 61º X Y 19.9m Estimativa inicial 7.7m xˆ = 2.866m, ŷ = 2.288m, θˆ = 61.5º, σ σ σ x y θ = 7.94cm = 7.03cm = 0.93º 16.9m 9.2m 7.8m 12.1m elipse aumentada 20 vezes

42 Experiência 1 - laser Localização real x = m y = m θ = 61º Estimativa inicial X Y xˆ = 2.866m, ŷ = 2.288m, θˆ = 61.5º, σ σ σ x y θ = 7.94cm = 7.03cm = 0.93º Estimativa final xˆ = 2.775m, ŷ = 2.290m, θˆ = 61.2º, σ σ σ x y θ = 1.4cm = 1.7cm = 0.14º elipse aumentada 20 vezes

43 Experiência 2 - laser elipse aumentada 20 vezes

44 Dois sensores - O mesmo ciclo A priori map Odometry k-1 --> k Estimated Robot Location k-1 Laser sensor Prediction Range Points Intensity Image Prediction Predicted Robot Location Canny Predicted Walls Prediction Matching and Integration Estimated Robot Location Vertical Edges Predicted Edges Matching and Integration Estimated Robot Location k

45 Ciclo de Localização From John J. Leonard, Hugh F. Durrant-Whyte. Directed Sonar Sensing for Mobile Robot Navigation. Kluwer Academic Publishers, Boston, 1992.

46 Ciclo de Localização e Contrução de Mapa From John J. Leonard, Hugh F. Durrant-Whyte. Directed Sonar Sensing for Mobile Robot Navigation. Kluwer Academic Publishers, Boston, 1992.

47 Outras técnicas de localização " localização só com medidas relativas " modelação e correcção dos erros sistemáticos na odometria " melhoramentos individuais na navegação inercial (filtragem) " uso conjunto de odometria e de navegação inercial (filtragem e escolha alternativa entre sensores - Girodometria) " histograma de ângulo " localização só com medidas absolutas " faróis activos " reconhecimento de marcas " marcas naturais ou artificiais " emparelhamento com o modelo " localização com base em representações particulares do mapa " componentes principais

48 Perspectivas para localização " Localização por objectivos " função do problema a resolver " como suporte directo à navegação " função dos sensores " Localização suportada em diversas representações do mundo " mapas geométricos " mapas topológicos " outro tipo de mapas " Construção/actualização de mapas e localização simultaneas

49 Conclusões O ciclo PREDIÇÃO-EMPARELHAMENTO-FILTRAGEM é uma metodologia geral aplicável à localização de robots móveis usando, isoladamente, diversos tipos de sensores integrando dados de sensores distintos à construção de mapas de ambientes à localização e à construção simultanea de mapas de ambientes