FILTRAGEM ROBUSTA PARA NAVEGAÇÃO INERCIAL

Transcrição

1 FILTRAGEM ROBUSTA PARA NAVEGAÇÃO INERCIAL Roberto Santos Inoue, Marco Henrique Terra, Adriano A.G. Siqueira, Jean Mimar Santa Cruz Yabarrena Universidade de São Paulo - Departamento de Engenharia Elétrica C.P.39 - CEP São Carlos, SP, Brasil Universidade de São Paulo - Departamento de Engenharia Mecânica C.P.39 - CEP São Carlos, SP, Brasil s: rsinoue@sel.eesc.usp.br, terra@sc.usp.br, siqueira@sc.usp.br, jeanmimar@gmail.com Abstract This paper deals with the application of robust state-space estimation of an inertial navigation system (which comprehends attitude, velocity, position and biases). This system is developed through the fusion of magnetometers, rate gyros, acelerometers and a receptor (global positioning system). In order to eleminate the attitude estimation singularities, the rigidy body orientation is modeled through the quaternion. Results based on actual measures are presented to show the effectiveness of the approach we are proposing. Keywords Kalman Filter, Robust Filtering, Inertial Navigation. Resumo Este artigo estuda a aplicação de filtragem robusta em um sistema de navegação inercial completo (atitude, velocidade, posição e biases). O sistema de navegação inercial será desenvolvido através da fusão de magnetômetros, giroscópios, acelerômetros e de um receptor (sistema global de posicionamento). Para eliminar os problemas de singularidades na estimativa da atitude, a orientação do corpo rígido é modelada através de um quatérnio. Resultados baseados em medidas reais são apresentados para mostrar a eficiência do método proposto. Palavras-chave, Filtragem Robusta, Navegação Inercial. 1 Introdução O desenvolvimento de sistemas de referência de atitude, proa, e posição são essenciais para o controle e navegação de robôs móveis e veículos aéreos não-tripulados. O desenvolvimento de sensores inerciais MEMS (sensores microeletromecânicos, do inglês micro-electromechanical sensors) de baixo custo, tem possibilitado a utilização de magnetômetros, giroscópios, acelerômetros baseados em tecnologia MEMS e receptores em sistemas de navegação inerciais completos, com medidas de atitude, velocidade, posição e biases, veja para maiores detalhes (Jung and Tsiotras, 7). O filtro de Kalman estendido tem sido utilizado para a fusão de dados de sensores para sistemas de navegação inercial, veja por exemplo Farrel and Barth (1998), Roumeliotis et al. (1998), Rogers (3), Castellanos et al. () e Bijker and Steyn (8). Porém, há várias fontes de incertezas que influem no sistema de navegação inercial, elas incluem por exemplo imprecisão dos sensores, alterações do ambiente externo tais como temperatura e rajadas de vento. Portanto, é interessante investigar o uso da estimativa robusta em navegação inercial. Em Sayed (1), Ishihara et al. (6) e Ishihara and Terra (8), foram desenvolvidos métodos para estimativa no espaço de estado para quando os parâmetros do modelo linear estão sujeitos a incertezas. Essas técnicas têm sido aplicadas em problemas práticos e os resultados obtidos têm sido promissores. Por exemplo, os filtros robustos propostos em Sayed (1) foram utilizados em Dominguez et al. (6) para estimar o acompanhamento de trajetória da ponta de um dedo em interfaces homem máquina. Levando em conta os resultados mencionados, este artigo propõe a utilização da abordagem proposta em (Sayed, 1) para resolver o problema de estimativa robusta de um sistema de referência de atitude, proa e posição. Resultados baseados em dados experimentais e um estudo comparativo entre o filtro robusto considerado e o filtro de Kalman estendido (EKF, do inglês Extended Kalman Filter) serão apresentados para a validação da estratégia proposta. Este artigo está organizado da seguinte forma: Na Seção é apresentado o sistema de referência de atitude e proa utilizando quatérnio para representar a atitude de um corpo rígido. Na Seção 3 é apresentado o sistema de referência de posição. Na Seção 4 é apresentado o filtro robusto considerado. Na Seção são apresentados os resultados do estudo comparativo proposto. E na Seção 6 é apresentada a conclusão do artigo. Sistema de referência de atitude e proa Nessa seção, quatérnios (Kuipers, 1998) são utilizados para obter uma representação de atitude de

2 um corpo rígido livre de singularidades. O quatérnio por convenção é representado por q = cos α + senα r, tal que q T, (1) sendo r um vetor unitário, conhecido como eixo de Euler, α uma rotação sobre este eixo e T = {q q T q = 1, q = q q T T, q R, q R 3 1 }. A multiplicação de quatérnios, representada por, é dada por a b = a b a b a b + b a + a b. () Para o problema de atitude, o quatérnio representa uma rotação do sistema de coordenadas inercial, I, para o sistema de coordenadas do corpo, B. Se s é um vetor expresso no sistema de coordenadas inercial I, então suas coordenadas em B são b = q s q 1, (3) sendo b = b T T, s = s T T e q 1 = q, q T T. Assim, pode-se escrever (3) como b = R(q)s, (4) sendo R(q) a matriz de rotação dos ângulos de Euler (ψ,θ,φ), com convenção (zyx), definida como R(q) = (q q T q)i (qq T q q ). () A derivada do quatérnio pode ser escrita matricialmente como q = 1 q = 1 ω Ω(ω)q = 1 Ξ(q)ω, (6) sendo ω T Ω(ω) = ω ω, Ξ(q) = q T I 3 3 q + q ω o vetor velocidade angular no sistema de coordenadas fixo do corpo, ω e q as matrizes representativas do produto vetorial E = E 3 E E 3 E 1 E E 1 (7) e I 3 3 a matriz identidade no R 3 3. As velocidades angular real ω e estimada ˆω são modeladas da seguinte forma (Castellanos et al., ; Roumeliotis et al., 1998) ω = ω g b g η g, (8) ˆω = ω g ˆb g, (9) sendo ω g a velocidade angular medida do giroscópio, b g o bias do giroscópio e η g o ruído branco gaussiano do bias do giroscópio., Utilizando o erro do processo (Brown and Hwang, 1997) para escrever o bias b g, tem-se ḃ g = 1 τ b g + η bg, (1) ˆb g =, (11) sendo η bg o ruído branco gaussiano do bias do giroscópio e τ a constante de tempo obtida através do gráfico da variância de Allan (IEEE, 1997). Substituindo (8) em (6), obtém-se q = 1 Ξ(q)(ω g b g ) 1 Ξ(q)η g (1) = 1 Ω(ˆω)q 1 Ξ(q)η g. (13) Combinando as equações (13) e (1), e definindo o estado como x 1 = q T b T g T R 7 1, obtém-se ẋ 1 = F 1 x 1 + G 1 η 1, (14) sendo 1 F 1 = Ω(ˆω) τ I, G 1 = Ξ(q) 4 3 e η 3 3 I 1 = 3 3 A equação de medida é dada por ηg η bg. (1) z 1 = h 1 (x 1 ) + v 1, (16) sendo h 1 (x 1 ) = a T ψ T R 4 1, v 1 processo gaussiano de média zero, a R 3 1 a medida da aceleração no eixo do corpo do veículo e ψ o ângulo de proa magnético, gerado pelos magnetômetros. A medida da aceleração é dada por a = R(q)g = g(q 1 q 3 q q ) g(q q 3 + q q 1 ) g(q q 1 q + q 3), (17) sendo g = g T e g =-9.81 m/s. O ângulo de proa por ( ) ψ = tan 1 (q1 q + q q 3 ) q + q 1 q. (18) q 3 Linearizando (16) em torno de x 1, tem-se z 1 = H 1 x 1 + v 1, com H 1 = z 1 x 1. (19) 3 Sistema de referência de posição Conforme mostrado em (Farrel and Barth, 1998), se as velocidades no sistema de coordenadas NED (North, East e Down), υ = υ N υ E υ D T são conhecidas e as posições geodéticas no sistema de coordenadas LLA (latitude, longitude e altitude), p = λ φ h T são as desejadas, então o vetor

3 de variação geodética LLA é relacionado com a velocidade NED da seguinte forma 1 λ υ N φ = 1 ()cosλ υ E, ḣ 1 υ D () sendo R λ o raio de curvatura no meridiano em uma latitude dada, ˆR λ = a(1 e ) (1 e sen, (1) λ) 1. ˆR φ o raio transversal da curvatura, a R φ = (1 e sen, () λ). a = m o raio equatorial, e e =.818 a excentricidade. Como os acelerômetros MEMS medem a dinâmica da aceleração combinada com a aceleração estática (gravidade da terra), a gravidade deve ser removida antes da aceleração ser integrada para obter a velocidade. Assim a equação da aceleração NED é a NED = a N a E a D = R T (q)a (3) sendo a as medidas dos acelerômetros no sistema de coordenada do corpo rígido, g = 9.81 m/s a gravidade e R(q) a matriz de cossenos diretores em termo do quatérnio q entre o sistema de coordenada NED e do corpo rígido B. A aceleração do corpo rígido a e a estimada â são modeladas da seguinte forma a = a a b a η a, (4) â = a a ˆb a, () sendo a a a aceleração medida do acelerômetro, b a o bias do acelerômetro e η a o ruído branco gaussiano. Utilizando o erro do processo (Brown and Hwang, 1997) para escrever o bias b a, tem-se ḃ a = 1 τ b a + η ba, (6) ˆb a =, (7) sendo τ a constante de tempo obtida através do gráfico da variância de Allan (IEEE, 1997) e η ba o ruído branco gaussiano do bias do acelerômetro. A equação da variação da velocidade NED como mostrado em (Farrel and Barth, 1998) é υ N υ E υ D = a N a E a D + g υ E senλ ()cosλ + υnυd υ Eυ Nsenλ ()cosλ + υeυd (R φ+h ) υ E υ N. (8) Substitiuindo (6 ) em (3) e em seguida em (8), tem-se υ N υ E = R T (q)a a b a η a υ D υ E senλ ()cosλ + υnυd + υ Eυ Nsenλ ()cosλ + υeυd (R φ+h ). g υ E υ N (9) Define-se o estado do sistema dinâmico como x = p T υ T NED b T a T R 9 1, η = η T a η T ba T R 6 1 como o ruído branco gaussiano e a equação de medida observável do como z = h (x ) + v R 6 1, sendo h (x) = p T υ T NED T, v o processo gaussiano de média zero. Linearizando (), (9) e (6) em torno de x, tem-se a dinâmica linearizada sendo F = G = ẋ = F x + G η, z = H x + v, F 11 F F 1 F R T (q) τ I R T (q) I 3 3, (3), H = I e F 11, F 1, F 1 e F como mostrado em (31). 4 Filtro robusto Há várias fonte de incertezas que influem na localização e determinação da atitude, elas incluem imprecisão dos sensores, alterações do ambiente externo tais como temperatura e rajadas de vento. Estas incertezas violam a premissa central da formulação do filtro de Kalman, na qual os parâmetros fundamentais do modelo {Φ,G,H,R,Q} são precisos, sendo Φ = e F kt I + F k T a matriz de transição de estado. Quando esta suposição é violada, o desempenho do filtro pode deteriorar apreciavelmente. Um maneira de modelar essas incertezas é considerar os parâmetros {Φ, G} como valores nominais e assumir que os valores reais variam em um certo conjunto cujos valores centrais são formados pelos parâmetros nominais. Assim, conforme mostrado em (Sayed, 1) e utilizado em (Inoue et al., 8), considere o sistema de incertezas da forma x k+1 = (Φ k + δφ k )x k + (G k + δg k )η k, z k = H k x k + v k, e suas pertubações modeladas como (3) δφ k δg k = M k E f E g, (33)

4 υn υ F 11 = E sin λ υ E ()(cos λ) F 1 = F = () () cos λ υe υe (sin λ) ()(cos λ) υ E υ N υe υn (sin λ) + υ D υ E sin λ () cos λ ()(cos λ) υ E sin λ () cos λ υ N sin λ () cos λ + υd υn +h υe, F 1 = υ E sin λ υn υd () cos λ () υe υn sin λ υe υd () cos λ () υ E + υn () () υ N υ E (R λ + h) 1 ((R φ + h)cos λ) 1 1 e,, (31) os ruídos dados por η k = N (,Q k ) e v k = N (,R k ). O modelo (33) permite o projetista restringir as fontes de distorção selecionando as entradas {E f E g } apropriadamente (Sayed, 1). Este filtro robusto procura minimizar o erro de estimativa para o pior caso possível criado pela faixa de incertezas δφ k e δg k. As estimativas {ˆx k+1 k, ˆx k+1 k+1 } no algoritmo que segue, podem ser obtidas por iterações recursivas entre os passos e 3. Para o caso quando ˆλ i =, os passos e 3 são reduzidos para o tempo padrão e para as equações de atualização de medida do filtro de Kalman. O algoritmo do filtro robusto é dado abaixo: Modelo de Incertezas: Sistema (3 ). Π >, R >, Q > são matrizes de ponderações. ˆx k+1 k = ˆΦ kˆx k k P k+1 k = Φ k ˆPk k Φ T k + Ĝk ˆQ k Ĝ k, K k+1 = P k+1 k H T k+1h k+1 P k+1 k H T k+1 + ˆR k+1 1 ˆx k+1 k+1 = ˆx k+1 k + K k+1 z k+1 h(ˆx k+1 k ) P k+1 k+1 = I K k+1 H k+1 P k+1 k Resultados Para a obtenção dos resultados foi utilizando o sensor MNAV1CA, mostrado na Fig. 1. O pacote de sensores embarcados do MNAV1CA inclui acelerômetros em 3 eixos, sensor de taxa angular em 3 eixos e magnetômetros em 3 eixos e um receptor. Condições Iniciais: ˆx i = P i Hi T R 1 i z i P i = (Π 1 i + Hi T R 1 i H i ) 1. Passo 1: Se H k+1 M k =, então configure ˆλ k = (filtro não robusto). Caso contrário, selecione α (tipicamente α f 1) e configure ˆλ k = (1 + α f ) M T k H T k+1 R1 k+1 H k+1m k. Passo : Substitua {Q k,r k,p k k,g k,φ k } por: ˆQ 1 k = Q 1 k ˆR k+1 = R k + ˆλ k E T g k I + ˆλ k E fk P k k E T f k 1 E gk ˆλ 1 k H k+1m k M T k H T k+1 ˆP k k = (P 1 k k + ˆλ k E T f,ke f,k ) 1 = P k k P k k E T f k (ˆλ 1 k I + E f k P k k E T f k ) 1 E fk P k k Ĝ k = G k ˆλ k Φ k ˆPk k E T f k E gk ˆΦ k = (Φ k ˆλ k Ĝ k ˆQik E T g k E fk )(Iˆλ k ˆPk k E T f k E fk ) Passo 3: Atualize {ˆx k k, P k k } como segue: Figura 1: MNAV - O ângulo de proa é obtido através dos magnetômetros em 3 eixos, m = m x m y m z T, utilizando o seguinte algoritmo π tan1 ( mx m y ) se m y > π + π ψ = tan1 ( mx m y ) se m y < π se m y = e m x < se m y = e m x > Os dados de saída do sensor MNAV1CA são obtidos no formato digital (RS-3) a uma taxa de Hz. E os algoritmos dos filtros foram rodados em MATLAB a uma taxa de amostragem T =.4 s e τ = s para o filtro de Kalman estendido (Brown and Hwang, 1997) e para o filtro robusto (Sayed, 1). As matrizes de ponderações, Q e R são coincidentes para o EKF e o filtro robusto e foram

5 configuradas com os mesmos valores. As matrizes de ponderações específicas do filtro robusto, M,E f,e g, foram selecionadas conforme variância das matrizes Φ e G, sendo o parâmetro α f configurado com o valor.. As Figuras a apresentam os resultados obtidos..1 Estudo comparativo Um estudo comparativo entre o EKF e o filtro Robusto é mostrado na Tabela 1. A norma L do vetor do erro de observação foi utilizada para comparar os algoritmos: L z(t) = 1 t r z(t) (t r t ) dt, t sendo a norma Euclideana, z(t) o vetor de erro da observação total, z 1 (t) o erro de observação da atitude e z (t) o erro de observação de posição e o tempo do experimento é t r = 8 s. Algoritmos L z(t) L z 1 (t) L z (t) FKE FR Melhoria (%) Tabela 1: Estudo Comparativo Observe que o filtro robusto obteve uma melhora de 13. % no erro de observação total,.6 % no erro de observação da atitude e 1.8 % no erro de observação da posição sobre o filtro de Kalman estendido. 6 Conclusão Nesse artigo, um filtro robusto para o sistema de referência de atitude, proa e posição comumente utilizados no controle e navegação de robôs móveis e veículos aéreos não-tripulados, foi desenvolvido. Esse filtro minimiza os efeitos das incerteza do modelo na estimativa dos valores de atitude, proa e posição. O filtro proposto foi implementado utilizando dados experimentais do sensor MNAV1CA. Um estudo comparativo mostrou que o filtro robusto apresentou uma melhora média de 13. % sobre o filtro de Kalman estendido no erro de observação total, atitude e posição, para o caso apresentado neste artigo. Para trabalhos futuros pretende-se implementar os algoritmos propostos em um microprocessador para ser embarcado em um veículo aéreo não-tripulado ou em um robô móvel. Referências Bijker, J. and Steyn, W. (8). Kalman filter configurations for a low-cost loosely integrated inertial navigation system on an airship, Control Engineering Practice 16: Brown, R. G. and Hwang, P. Y. C. (1997). Introduction to random signal and applied Kalman filtering: with Matlab exercises and solutions, 3rd edn, John Wiley & Sons, New York, USA. Castellanos, J. F. G., Lesecq, S. and Marchand, N. (). A low-cost air data attitude heading reference system for the tourism airplane applications, IEEE Sensors, Irvine, CA, USA, p. 4. Dominguez, S. M., Keaton, T. and Sayed, A. H. (6). A robust tracking method for multimodal wearable computer interfacing, IEEE Transactions on Multimedia 8(): Farrel, J. and Barth, M. (1998). The global positioning system and inertial navigation, McGraw-Hill. IEEE (1997). Guide and test procedure for single axis interferometric fiber optic gyros, IEEE Std Inoue, R. S., Terra, M. H. and Jr, V. G. (8). Robust state-space estimation for mobile robot localization, LARS 8. IEEE Latin American Robotic Symposium, Salvador, Brazil, pp Ishihara, J. Y. and Terra, M. H. (8). Robust state prediction for descriptor systems, Automatica 44(8): Ishihara, J. Y., Terra, M. H. and Campos, J. C. T. (6). Robust Kalman filter for descriptor systems, IEEE Transaction on Automatic Control 1(8): Jung, D. and Tsiotras, P. (7). Inertial attitude and position reference system development for a small UAV, American Institute of Aeronautics and Astronautics, Rohnert Park, California. Kuipers, J. B. (1998). Quaternions and rotation sequences, Pricenton University Press, Princenton, New Jersey. Rogers, R. M. (3). Applied mathematics in integrated navigation systems, nd edn, AIAA education series, Reston, VA. Roumeliotis, S. I., Sukhatme, G. S. and Bekey, G. A. (1998). Smoother based 3d attitude estimation for mobile robot localization, Technical report, USC, Sayed, A. H. (1). A framework for state-space estimation with uncertain models, IEEE Transaction on Automatic Control 46:

6 ψ (graus) θ (graus) φ (graus) ψ (graus) Figura : Estimação da atitude com o EKF: Guinada ψ, Arfagem θ e Rolagem φ 1 1 θ (graus) 1 1 φ (graus) Latitude (graus) Figura 3: Estimação da atitude com o filtro Robusto: Guinada ψ, Arfagem θ e Rolagem φ Longitude (graus) Altitude (m) Figura 4: Estimação da posição geodéticas no sistema de coordenadas LLA: Latitude λ, Longitude φ e Altitude h v N (m/s) v E (m/s) v D (m/s) Figura : Estimação das velocidades no sistemas de coordenadas NED: υ N, υ E e υ D