IMPLEMENTAÇÃO DE SISTEMA DE ATITUDE USANDO SENSORES MARG

Transcrição

1 Anais do XVI Encontro de Iniciação Científica e Pós-Graduação do ITA XVII ENCITA / 11 Instituto Tecnológico de Aeronáutica São José dos Campos SP Brasil 19 de outubro de 11 IMPLEMENTAÇÃO DE SISTEMA DE ATITUDE USANDO SENSORES MARG Gustavo Souza de Lacerda Instituto Tecnológico de Aeronáutica CTA Rua H-B São José dos Campos - SP Bolsista PIBIC-CNPq gustavocamus@gmail.com Elder Moreira Hemerly Instituto Tecnológico de Aeronáutica ITA Praça Mal. Eduardo Gomes 5 Vila das Acácias São José dos Campos - SP hemerly@ita.br Resumo. Este projeto iniciado em agosto de 1 teve como objetivo central estabelecer as equações básicas do Filtro de Kalman usando sensores do tipo MARG Magnetometers Accelerometers and Rate Gyros para estimação de atitude simular a solução para estimativa de atitude em MATLAB e analisar desempenho com dados reais. Para tanto os seguintes passos foram seguidos: familiarização do aluno com o problema de estimação de atitude; compreensão dos princípios básicos do algoritmo a ser implementado; codificação do algoritmo em MATLAB; realização de testes utilizando dados simulados em MATLAB; realização de teste empregando leituras reais de acelerômetros girômetros e magnetômetros e; análise crítica dos resultados. Palavras chave: controle de atitude filtro de Kalman AHRS navegação. 1. Introdução Neste projeto inicialmente o aluno familiarizou-se com o problema de estimação de atitude usando como base sensores do tipo MARG. Em seguida estudou-se o uso de um Filtro de Kalman para a fusão ótima entre os sensores. A partir daí analisou-se o código MATLAB desenvolvido pelo aluno Jorge Melegati em 8 para estimação de atitude usando sensores MARG e Filtro de Kalman. Avaliou-se os resultados obtidos naquele período e estudou-se algumas alternativas para melhorar o código. Por fim estudou-se a interferência da aceleração na determinação da atitude de um sistema.. Descrição do Problema Atitude é um termo que descreve a orientação de um veículo ou objeto no espaço. Assim sendo determinar a atitude de um corpo a partir de leituras de um certo tipo de instrumento é um problema clássico e antigo da engenharia. Este problema pode ser visto em vários tipos de indústrias da de entretenimento à aeroespacial. Figura 1. Exemplos de sistemas de determinação de atitude

2 Quando estamos trabalhando com aplicações que não exigem grande acurácia geralmente determinamos a atitude do corpo usando um AHRS Attitude Heading Reference System. Em especial se a aplicação demanda uma acurácia mediana e um custo baixo os sensores utilizados são tipicamente do tipo MARG 3 magnetômetros 3 acelerômetros e 3 girômetros. Nesses sistemas podemos utilizar diversas formas de parametrização para a atitude do corpo o que afeta diretamente a forma das equações diferenciais cinemáticas do problema. Em particular as parametrizações por ângulos de Euler e por quartenions são as mais comuns de serem empregadas. Porque os resultados são mais intuitivos e mais fáceis de se interpretar trabalharemos aqui com a parametrização de ângulos de Euler muito embora eles sofram com uma singularidade quando θ arfagem = +9. Os ângulos são três: guinada yaw arfagem pitch e rolamento roll. Figura. Representação dos ângulos de Euler para um corpo Nesse contexto os principais objetivos deste projeto são: a familiarizar o aluno com o problema de estimação de atitude usando sensores MARG e com o uso do Filtro de Kalman para realização de fusão ótima de sensores; b implementar um algoritmo para AHRS em linguagem MATLAB; c realizar testes com o algoritmo implementado com dados simulados em MATLAB e dados reais provenientes de 9 sensores 3 acelerômetros 3 girômetros e 3 magnetômetros; d investigar preliminarmente alternativas para se tratar o caso de movimentos acelerados e; e efetuar análise crítica dos resultados obtidos. A fim de atingir todos os objetivos acima listados a metodologia empregada no desenvolvimento do projeto pode ser sintetizada em três passos: a análise do problema de atitude com ênfase nas soluções baseadas em sensores MARG e Filtro de Kalman; b simulação de todos os aspectos envolvidos na estimação de atitude em MATLAB; objetivando fácil depuração do código e análise preliminar de desempenho em situações simuladas e; c realização de experimentos com dados reais e análise crítica dos resultados. 3. Resultados Obtidos Usando os ângulos de Euler o sistema de equações diferenciais que descrevem a atitude de um veículo é: #!. ".!. + = 1 sin cos. # p - cos cos" sin cos" c - q " - c " sin sin" cos sin" / r 1 onde Ψ θ e Φ representam respectivamente os ângulos de guinada arfagem e rolamento; p q e r representam as medidas obtidas dos três girômetros ortogonais.

3 Uma análise detalhada da equação acima nos mostra que a medida do erro em qualquer grandeza angular nos levará a erros de atitude que crescem com o tempo uma vez que as equações diferenciais do sistema estão acopladas entre si. Para realizar uma análise de erro mais realista precisa-se de um modelo de alta fidelidade para os girômetros. Este modelo é dado pela seguinte equação: p m = 1+ s f p t + b p t onde p m é a saída medida pelo sensor p t é o real valor da medida angular que o girômetro está tentando medir s f é um fator de escala que corrompe a medida e b p t é o bias. A expressão é igualmente válida para q e r. De modo geral o bias bt é dado pela forma: bt = b + b 1 t + b w t 3 onde b é uma constante b 1 t representa a componente variante com o tempo do bias e b w t representa o ruído da amostra. Os valores b 1 t e b w t são estocásticos. De fato b 1 t pode ser modelado como um processo aleatório gaussiano exponencialmente correlacionado de constante de tempo τ variância σ b1 e densidade espectral de potência de saída pode ser modelado por um ruído branco em banda limitada. =! b1 ". Já b w t às vezes chamado de ruído Desde que os erros dos girômetros são estocásticos em natureza temos que fazer uma análise de covariância para analisarmos o desempenho de um AHRS usando apenas a leitura dos girômetros. Para tal é necessário linearizar os modelos de erro resultando no seguinte sistema dinâmico: Q wb1.! " x = F! x " + # w " 4 onde o vetor de erro de estado é dado por:.! " x =!#!!!b p!b q!b r T 5 sendo as três primeiras entradas os erros dos ângulos de Euler e as três últimas os erros no bias em cada uma das medidas dos girômetros. A matriz de erro dinâmico F é dada por: F =! sen" q cos#! rsen# sen# cos# qsen# + r cos# cos " cos" cos" cos"!qsen# + r cos# cos#!sen# qsen# + r cos# cos " sen" cos" q cos#! rsen# 1 sen#sen" cos"! 1! 1 cos#sen" cos"! 1 6 e a matriz de mapeamento de ruído do processo é:

4 ! = sen" cos" cos# cos# cos" sen" sen"sen# cos"sen# 1 cos# cos# Por fim o vetor de ruído do processo w! é dado por: w! = " n p n q n r w p w q w # r 8 onde as três primeiras entradas representam os erros de medida em banda larga de p q e r e as três últimas entradas são os termos de ruído para as bias de cada girômetro. Então a densidade espectral de potencia para w! é dada por: # Q w =! p! q! r! wp " wp! wq " wq! wr " wr 9 Finalmente a matriz de covariância de erro de estado no passa k P k é definida como propagada no tempo através da seguinte relação:! " # x k " # T x k e é P k +1 =! K P K! K T + C d 1 onde Φ k é o equivalente discreto da matriz F no passo k; C d é o equivalente discreto de ΓQ w Γ Todos os resultados apresentados até agora correspondem a um sistema composto apenas por girômetros de baixo desempenho que apesar de terem boa resposta em banda alta levam a erros de atitudes ilimitados. Por isso deve-se acrescentar ao sistema de atitude sensores ou um sistema com erros limitados. O sistema adicional pode ser usado para reinicializar periodicamente a solução de atitude derivada dos girômetros. Nesse caso usou-se como sistema adicional uma tríade de acelerômetros e magnetômetros presentes no MARG. A fusão entre os sensores foi feita através da aplicação de um filtro de Kalman. Para o estimador de ângulos de Euler o vetor de estado que nos interessa é:

5 ! x = " # bp b q b r T 11 O modelo dinâmico para este estimador é semelhante ao apresentado anteriormente porém com uma pequena diferença. Nesse caso a matriz de mapeamento de ruído de processo é dada por:! = " sen# " sen# cos cos " cos# "sen# + " " sen#sen " cos#sen cos cos T 1 onde α e β são parâmetros. A equação que atualiza os dados obtidos é:! "+ x = L " k y # H k x" ^ # = L " ^ k y # H k y" k = L k! " y k onde o índice - indica a grandeza antes da atualização e o índice + indica depois. O vetor medida! y k é dado por: 13! y = " k AS # AS AS 14 onde AS indica que a estimativa dos ângulos de Euler provém do sistema adicional acelerômetros e magnetômetros. Essas estimativas são encontradas integrando a equação 1 ao longo do tempo entre duas atualizações consecutivas. Para cada atualização o ganho de Kalman L k e a matriz de covariância de estado P k + são calculadas pelas relações: L k = P k! H T H T P! k H + R v!1 15 e P +! K = I 6 x6! L K H P K 16 onde a matriz de medida H é definida como: H =! I 3x 3 " 3x 3 # 17 e matriz de covariância do ruído de medida R v é dada por: R v =! "! #! 18 sendo as entradas da diagonal acima os ruídos em banda larga na medida de Ψ θ e Φ do sistema adicional.

6 Finalmente a cada iteração os ângulos de Euler e os bias são atualizados conforme:! k + =! k " + # x k + 1 k + = k " + # x k + k + = k " + # x k + 3 b + pk = b " pk + # x + k 4 b + qk = b " qk + # x + k 5 b + rk = b " rk + # x + k 6 19 Esquematicamente podemos representar o nosso problema de determinação de atitude de um corpo através de sensores MARG por meio de um fluxograma que segue representado abaixo: Fig. 3 Fluxograma geral da determinação de atitude de um corpo Todo o algoritmo descrito neste artigo havia sido implementado em ambiente MATLAB pelo aluno Jorge Melegati em 8 através de uma Bolsa PIBIC referência [3]. Assim sendo para evitar retrabalho o trabalho de programação deste projeto consistiu numa revisão detalhada do código por ele implementado àquela época. Primeiramente verificou-se que tal implementação estava de fato funcionando corretamente e que os resultados por ela gerados eram satisfatórios. Contudo na busca do melhor resultado possível estudou-se a sintonia entre os parâmetros do filtro. Após uma série de testes realizados observou-se que as alterações realizadas nestes parâmetros nada contribuíram ou até mesmo atrapalharam o desempenho do sistema. Por este motivo optou-se por utilizar os parâmetros que já estavam definidos anteriormente. Neste caso os resultados obtidos podem ser visualizados nos gráficos abaixo:

7 Fig. 4 Ângulo de Rolamento Fig. 5 Ângulo de arfagem

8 Fig. 6 Ângulo de Guinada Como podemos observar a partir destas figuras os piores resultados ocorrem para o ângulo de guinada porque na sua obtenção não são utilizados os dados oriundos dos acelerômetros. Isto ocorre pois o campo gravitacional que é a referência para este caso é sempre perpendicular ao plano em que está o referido ângulo impedindo que os dados do acelerômetro possam ser aproveitados. Neste momento é importante também analisar o efeito de outras acelerações no movimento dos sensores. Fig. 7 Corpo em movimento De fato quando o sistema está sob influência de uma aceleração externa o vetor referência do acelerômetro passa a ser não só o vetor gravidade mas sim a resultante de toda a aceleração aplicada sobre o corpo. Neste caso a primeira solução que pode ser empregada é a de atribuir pequenos pesos para todos os três valores dos acelerômetros quando uma aceleração externa é detectada. Assim a estimativa da atitude passa a depender basicamente dos giroscópios e magnetômetros. Entretanto uma forma mais eficaz de amenizar o efeito de uma aceleração externa consiste em estimar a direção da mesma e então atribuir pesos pequenos apenas para os valores dos acelerômetros afetados por esta aceleração externa. Os valores dos acelerômetros não afetados continuam contribuindo igualmente para a determinação da atitude.

9 4. Conclusões Neste projeto estudou-se a implementação de um sistema de atitude usando sensores do tipo MARG. O trabalho desenvolvido ao longo de um ano de pesquisa pode ser sintetizado nas seguintes etapas: Familiarização com o problema de estimação de atitude; Estudo sobre Filtro de Kalman; Análise do algoritmo de estimação de atitude; Realização de testes e ajuste dos parâmetros do filtro; Análise teórica do caso onde há movimento acelerado. Todos os resultados obtidos foram satisfatórios. 5. Agradecimentos Agradeço ao professor Elder Hemerly pelo rico conhecimento transmitido a mim durante o projeto; ao ITA pelo suporte dado para o projeto pudesse ser desenvolvido; e ao CNPQ pela oportunidade e por tornar possível a realização deste gratificante trabalho. 6. Referências [1] Gebre-Egziabher D. Hayward R. C. Powell J. D. Design of Multi-Sensor Attitude Determination Systems In IEEE Transactions on Aerospace and Electronic Systems pg [] Gebre-Egziabher D. Elkaim G.H. "MAV Attitude Determination From Observations of Earth s Magnetic and Gravity Field Vectors" In EEE Journal of Aerospace Electronic Systems Volume 44 Issue 3 8. [3] J. A. Melegati "Implementação de Sistema de Atitude em DSP" Artigo Final CNPQ 8. [4] Soo Suh Young "Orientation estimation using a quaternion-based indirect Kalman filter with adaptive estimation of external acceleration" In IEEE Transactions on Instrumentation and Measurement Volume 59 Issue 1 1.