Anais do XX Congresso Brasileiro de Automática Belo Horizonte, MG, 20 a 24 de Setembro de 2014 PROCEDIMENTO PARA AUTOCALIBRAÇÃO DE UNIDADES DE REFERÊNCIA DE ATITUDE COELHO, FERNANDO ANTONIO ALMEIDA Diretoria de Sistemas de Armas da Marinha R. Primeiro de Março, 118. Ed. Barão de Ladário, 21º Andar. Centro. Rio de Janeiro - RJ E-mail:[email protected] HEMERLY, ELDER MOREIRA Divisão de Eng. Eletrônica, Instituto Tecnológico de Aeronáutica Praça Mal. Eduardo, 50, Vila das Acácias, CEP 12228-900, São José dos Campos - SP E-mail: [email protected] Abstract Ellipsis fitting is one of the most used methods to magnetometer and accelerometer calibration, because there is no need for any position references, thereby making it suitable for self calibration and on line applications. Unfortunately, this autocalibration algorithm alone does not compensate some sensing axes misalignment caused by distortion of the magnetic field due to soft-iron interferences. As a solution we propose to use ellipsoid fitting only to calibrate the accelerometer, while the magnetometer is calibrated using attitude information from the accelerometer. With this approach, there is no need for any external reference, what makes the method suitable for low cost applications. Keywords magnetometer, soft-iron, auto-calibration, ellipsoid fitting, misalignment. Resumo Ajustamento de elipses é um dos métodos mais usados para calibração de magnetômetro e acelerômetro, devido a não necessitar de qualquer referência de posição, o que o torna adequado a aplicações que requeiram autocalibração em tempo real. Infelizmente, este método de autocalibração não compensa erros de alinhamento dos eixos de medida causados por distorções do campo magnético devido a interferências ditas tipo soft-iron. Como solução, propõe-se usar ajustamento de elipsoide apenas na calibração do acelerômetro, enquanto que o magnetômetro é calibrado usando informação de atitude fornecida pelo acelerômetro. Neste caso, não há necessidade de uso de qualquer referência externa, o que torna o método adequado para aplicações de baixo custo. Palavras-chave magnetômetro, soft-iron, autocalibração, ajustamento de elipsoide, desalinhamento. Por outro lado, o método de ajustamento de elipsoide usa restrições geométricas do modelo dos erros para estimar os parâmetros de calibração sem a necessidade de referência de atitude (GebreEgziabhe, et al., 2006; Renaudin, et al., 2010; Fang, et al., 2011). Entretanto, esse algoritmo não compensa erros de alinhamento causados por distorções do campo magnético, devidos, principalmente, a interferências ditas do tipo softiron (Gebre-Egziabher, 2007; Li & Li, 2012). Uma bússola magnética calibrada por esse método, sob interferência tipo soft-iron, apresentou erros médios de até 5 graus no rumo, em experimentos práticos (Renaudin, et al., 2010), enquanto que, em simulações de trajetórias típicas de voo de um veículo aero não tripulado, os erros finais no rumo chegam a 10 graus (Gebre-Egziabher, 2007). Li & Li (2012) apresentam o método de calibração de magnetômetro chamado de Dot Product Invariance (DPI), o qual se vale do fato de que o produto interno de dois vetores invariantes é constante. Assim, o vetor aceleração da gravidade medido pelo acelerômetro é usado como auxiliar para calibrar o magnetômetro. Entretanto, em seu artigo Li & Li (2012) têm o acelerômetro previamente calibrado por métodos tradicionais, recaindo novamente na necessidade de referências de atitude externa. 1 Introdução Unidades de Referência de Atitude (URA) de baixo custo são o principal componente do sistema de navegação de pequenos veículos e outros sistemas autônomos. Estas URA normalmente usam tríades de acelerômetros, magnetômetros e às vezes girômetros fundidos em um único dispositivo para produzir medições de atitude e rumo mais precisas e confiáveis que as de cada sensor individualmente. Quando esses sensores são montados na plataforma do veículo, aparecem alguns erros de calibração devido a desalinhamentos mecânicos e, em especial, o magnetômetro sofre de interferência de materiais ferromagnéticos intrinsecamente presentes na plataforma. Assim, mesmo que esses sensores já venham calibrados da fábrica, eles precisam ser calibrados no local onde serão instalados. Métodos tradicionais de calibração precisam de algum tipo de referência de atitude e rumo. Em geral eles comparam as saídas do sensor em diferentes posições com a projeção do vetor sob medida (Syed, et al., 2007; Renk, et al., 2005). Esses algoritmos precisam ser executados em laboratórios com condições controladas e instrumentação especial; logo, não são apropriados para aplicações de baixo custo, externas ou de tempo real. 3437 Anais do XX Congresso Brasileiro de Automática Belo Horizonte, MG, 20 a 24 de Setembro de 2014 Neste trabalho, o acelerômetro é previamente calibrado pelo método de ajustamento de elipsoide, que não usa referência de atitude externa, e depois é utilizado para calibrar o magnetômetro pelo método DPI. Como os acelerômetros não sofrem interferências magnéticas, os efeitos de desalinhamento por soft-iron podem ser compensados com este procedimento. Simulações e dados experimentais são utilizados para demonstrar o bom desempenho do método proposto. soft-iron (Renaudin, et al., 2010). Quando esses efeitos não são significativos, a matriz se aproxima da matriz identidade. O mesmo modelo de erros é aplicável ao acelerômetro, bastando apenas utilizar o vetor gravidade em substituição a , e o equivalente a (2) é (4) 3 Calibração do Acelerômetro 2 Modelagem dos Erros dos Sensores O método de ajustamento de elipsoide utiliza medidas tomadas de um campo magnético (ou gravitacional) estacionário, que pode ser o próprio campo magnético da Terra (ou aceleração da gravidade) em um determinado local. Neste caso, o campo sob medição tem magnitude constante e pode ser encontrado em modelos geológicos como do Britsh Geological Survey (2010). De qualquer modo, como a magnitude do vetor não afeta as medidas de atitude que interessam em aplicações de URA, um valor fictício, por exemplo, próximo da unidade pode ser utilizado no procedimento de calibração. De (4) pode-se escrever Em uma URA, a intensidade do campo magnético da terra, , é a medida utilizada para estimar a direção (rumo) da plataforma onde está instalada. Os três eixos de medida do magnetômetro normalmente são alinhados com os eixos do corpo do veículo, de modo que as medidas a serem apresentadas sejam os componentes do vetor campo magnético da terra projetado nos eixos do corpo, denotado por . Na prática, existem erros de instrumentação intrínsecos à tecnologia do sensor, bem como interferências causadas por materiais ferromagnéticos presentes no veículo, que geram desvios no campo magnético da terra. Assim, o vetor medido, , é corrompido por erros, de acordo com a equação vetorial (1) (5) onde: onde: é uma matriz cheia , é o bias determinístico, e é o ruído dos sensores. A matriz e o vetor em (1) compõem os efeitos originários de diferentes tipos de interferências, como soft-iron, hard-iron e imperfeições construtivas, porém para estimação dos parâmetros, a origem dos erros é indiferente. Uma descrição detalhada dos diferentes efeitos envolvidos em um magnetômetro é feita por Renaudin et al. (2010). Resolvendo-se (1) para obtém-se o valor exato do campo sob medição, (2) , e . Convém notar que embora o ruído seja gaussiano e com média nula, o ruído pode não o ser, pois sua esperança é . No método de ajustamento de elipsoide, considera-se que o nível de ruído do sensor é suficientemente baixo para ser negligenciado. Pylvänäinen (2008) faz uma análise empírica da sensibilidade do algoritmo ao nível do ruído presente. A equação quadrática (5) descreve uma superfície de segunda ordem, por exemplo, um hiperboloide ou elipsoide. Uma condição suficiente para que (5) represente um elipsoide é que a matriz seja positiva definida e que seja positivo (Renaudin, et al., 2010), o que de fato é verdade pela própria definição desses parâmetros. Assim, o método de ajustamento de elipsoide consiste em estimar e em (5), restritos às condições que descrevem um elipsoide, e a literatura apresenta diferentes métodos de estimação desses parâmetros (Halır & Flusser, 1998; Fitzgibbon, et al., 1999; Gebre-Egziabher, 2007; Fang, et al., 2011; Tabatabaci, et al., 2013). Perceba-se que a matriz é simétrica, de modo que requer a estimativa de seis parâmetros apenas. Logo, reconstituir , a partir de , resulta Portanto, calibrar o magnetômetro consiste em estimar e em (2). A matriz pode ser decomposta no produto de uma matriz ortogonal por outra simétrica, chamada de decomposição polar, conforme (3) A matriz simétrica é estimada pelo método de ajustamento de elipsoide, que não é capaz de estimar a matriz ortogonal , como será mostrado na Seção 3. Essa matriz ortogonal pode ser interpretada como o desalinhamento do triedro de medida em relação ao corpo do veículo, podendo ser causada tanto por desalinhamentos mecânicos quanto por distorções do campo magnético devido, principalmente, a efeitos 3438 Anais do XX Congresso Brasileiro de Automática Belo Horizonte, MG, 20 a 24 de Setembro de 2014 inúmeras soluções. Uma solução típica desse problema é tomar a decomposição em valores singulares , e se tem (6) onde a amplitude do campo magnético da Terra, , e seu ângulo de inclinação formado com o vetor gravidade, , podem ser encontrados em tabelas de modelos do campo magnético para o local do experimento (Britsh Geological Survey, 2010). Já para o valor de , o método DPI utiliza a intensidade padrão da gravidade que pode ser normalizada, . As incógnitas em (8), e podem ser estimadas por mínimos quadrados. Convém notar que (6) resulta em uma matriz simétrica, o que significa que o componente ortogonal de (3) não é estimado com o método de ajustamento de elipsoide. O componente ortogonal pode ser interpretado como um erro de alinhamento do triedro do sensor em relação ao triedro do corpo, e é devida a erros de construção/instalação. Porém, no caso do magnetômetro, esse desalinhamento é acrescido por distorções do campo magnético causadas pela presença de material ferromagnético na plataforma do veículo. Como o acelerômetro é imune a essa interferência, esse desalinhamento é minimizado quando o ajustamento de elipsoide é aplicado ao acelerômetro. 5 Estimação da Atitude e Rumo A qualidade da calibração reflete nas estimativas de atitude e rumo geradas pela URA, de modo que é natural avaliar essa qualidade analisando-se os erros nas estimativas desses ângulos. Baseado na projeção, no plano vertical, do vetor gravidade medido pelo acelerômetro, , obtém-se as estimativas dos ângulos de rolagem, , e arfagem, , sendo 4 Calibração do Magnetômetro Li & Li (2012) apresentam o método de calibração de magnetômetro chamado de Dot Product Invariance (DPI), o qual se vale do fato de que o produto interno de dois vetores invariantes é constante. Assim, o método utiliza o vetor aceleração da gravidade, , medido pelos acelerômetros previamente calibrados, presentes na URA, em conjunto com o vetor intensidade do campo magnético, , para estimar os parâmetros de calibração do magnetômetro. Esse algoritmo consegue eliminar os erros de desalinhamento do efeito soft-iron. Porém, esse método utiliza o acelerômetro como referência de atitude, tendo sua qualidade determinada pela qualidade da calibração prévia do acelerômetro. Então, em (Li & Li, 2012), o acelerômetro é previamente calibrado por métodos tradicionais, recaindo novamente na necessidade de laboratórios e instrumentação especiais; logo, não é apropriado a aplicações de baixo custo. Neste artigo, propõe-se calibrar o acelerômetro pelo método de ajustamento de elipsoide, que não requer referência externa de posição, e, como o acelerômetro não sofre interferência magnética, os erros de desalinhamento residuais são minimizados. Em seguida, o método DPI é usado para calibrar o magnetômetro, eliminando assim erros de desalinhamento causados por interferências tipo softiron. Para aplicar o método DPI parte-se da expressão (7) (10) onde é a função inversa da tangente de quatro quadrantes (Mathwork, 2014). A partir desses ângulos, tem-se a matriz de transformação que projeta o vetor intensidade do campo magnético no plano horizontal, conhecido como plano local, gerando o vetor . Desse vetor estima-se o rumo em relação ao “norte magnético”, , dado por (11) Caso julgado necessário, o rumo em (11) pode ser corrigindo para o rumo em relação ao “norte verdadeiro” somado-se o ângulo de declinação do campo magnético da Terra no local (Britsh Geological Survey, 2010). 6 Simulações A fim de avaliar o desempenho do método proposto, o algoritmo foi inicialmente aplicado a dados simulados. Nessas simulações, a intensidade do campo magnético local foi normalizada gerando o vetor unitário . Erros de ortogonalidade dos eixos de medida, desalinhamento, fator de escala e bias foram introduzidos no magnetômetro, gerando os parâmetros da Tabela 1. A decomposição polar da Tabela 1 representa ângulos de desalinhamento de , e , em torno dos eixos Z, Y e X, respectivamente. que, combinada com (2), com o ruído negligenciado, resulta em (8) sendo dada por (9) 3439 Anais do XX Congresso Brasileiro de Automática Belo Horizonte, MG, 20 a 24 de Setembro de 2014 Tabela 1. Parâmetros do magnetômetro linha coluna 2 1 mostram que os dados são representativos do espaço amostral, para aplicação do método de ajustamento de elipsoide. Já os valores medidos são apresentados na Figura 4. 3 1 2 3 Decomposição Polar linha 1 coluna 2 3 1 coluna 2 3 1 2 3 linha 1 2 3 Figura 1. Ângulos de Euler dos movimentos simulados Por outro lado, a aceleração da gravidade foi considerada na direção do eixo Z (vertical) e também normalizada, enquanto os parâmetros utilizados no acelerômetro estão apresentados na Tabela 2. A decomposição polar da Tabela 2 equivale a desalinhamento de , e , em torno dos eixos Z, Y e X, respectivamente. Tabela 2. Parâmetros do acelerômetro linha coluna 2 1 Figura 2. Representação tridimensional das medidas do magnetômetro (valores normalizados) 3 1 2 3 Decomposição Polar linha 1 coluna 2 1 coluna 2 3 1 2 3 linha Figura 3. Representação tridimensional das medidas do acelerômetro (valores normalizados) 3 1 2 3 Além disso, foram introduzidos ruídos gaussianos de medição em ambos os sensores, com desviospadrão da ordem de em cada eixo, ou seja, em uma faixa dinâmica em torno de 60dB, valores típicos do sensor 3DM-GX1 (Renk, et al., 2005), utilizado nos resultados experimentais deste artigo, na Seção 7. Foram então simulados movimentos de guinada, arfagem e rolagem, ou seja, em torno dos eixos do corpo Z, Y, X, respectivamente, conforme a Figura 1, que geraram as representações tridimensionais mostradas na Figura 2 e Figura 3. Essas figuras Figura 4. Medidas normalizadas obtidas na simulação 3440 Anais do XX Congresso Brasileiro de Automática Belo Horizonte, MG, 20 a 24 de Setembro de 2014 O método proposto foi aplicado, e foram estimados os parâmetros apresentados na Tabela 3, onde se percebe estimação exata para o bias do redução dos erros máximos de estimativa de rumo, que são reduzidos de , sem calibração, para após calibração. Ou seja, apenas os erros remanescentes de alinhamento do acelerômetro não são compensados. Esses mesmos erros de desalinhamento são observados nas estimativas de arfagem, que teve seu erro máximo reduzido de para , enquanto o erro máximo de rolagem, reduziu de para . acelerômetro e valores de bem próximos de da Tabela 2. Já a parte ortogonal da decomposição polar não é estimada pelo método de ajustamento de elipsoide aplicado ao acelerômetro, não sendo compensada pela calibração. Tabela 3. Parâmetros estimados pelo método proposto linha 1 Coluna 2 3 1 Coluna 2 3 1 2 3 linha 1 2 3 Decomposição Polar de linha 1 Coluna 2 Figura 5. Erros de estimação dos ângulos de rumo, arfagem e rolagem, em três condições: antes da calibração, com os sensores calibrados por ajustamento de elipsoide e com o método proposto 3 1 2 3 linha 6 Resultados Experimentais 1 Coluna 2 O método de calibração proposto foi aplicado à URA 3DM-GX1, produzida pela MicroStrain. Essa URA, possui também girômetros para estabilização das medidas de atitude, e tem acurácia típica de 0,5o em condições estáticas e em condições dinâmicas (MicroStrain Inc, 2006). Os dados foram coletados no formato raw, ou seja, após conversão analógico-digital de 16 bits com um bit de sinal. Esses dados foram então escalonados em medidas físicas multiplicado-os pelo valor de fundo de escala informado pelo fabricante (1,2 Gauss para o magnetômetro e 3 G para o acelerômetro), e depois normalizados ao dividi-los pela amplitude do campo magnético e gravidade no local de medida (0,23 Gauss e 1 G). Duas configurações foram ensaiadas: (I) sensor sem interferência magnética; e (II) sensor sob interferência magnética de um parafuso de aço, posicionado sobre o sensor conforme apresentado em destaque na Figura 6. 3 1 2 3 Quanto aos parâmetros estimados para o magnetômetro, esses apresentam bias bem próximo do verdadeiro, enquanto que os demais parâmetros apresentam erros entre 1 e 16% dos valores verdadeiros. Porém, a matriz ortogonal resultante da decomposição polar dessa estimativa equivale a ângulos de desalinhamento entre o magnetômetro e o acelerômetro de , , em torno dos eixos Z, Y e X, respectivamente, valores que são próximos aos verdadeiros, , . Estimando-se os erros dos ângulos de atitude calculados a partir de (10) e (11), resultam os gráficos da Figura 5, onde as medidas são realizadas de três modos: sem calibração, com ambos os sensores calibrados por ajustamento de elipsoide, e pelo método proposto. Como os ângulos de arfagem e rolagem são calculados a partir do vetor aceleração, aplicar ajustamento de elipsoide no magnetômetro não altera a estimativa desses ângulos, por isso a Figura 5 não mostra diferença entre os dois métodos de calibração aplicados. A eficácia do método proposto é observada na Figura 6. Parafuso de aço afixado sobre o sensor para introdução de interferência magnética, na configuração II 3441 Anais do XX Congresso Brasileiro de Automática Belo Horizonte, MG, 20 a 24 de Setembro de 2014 Uma mesa de inclinação, não magnética, apresentada na Figura 7, foi utilizada para posicionar o sensor em diversos ângulos de inclinação, e a cada posição eram colhidos dados por cerca de 1s a uma taxa de . As medidas obtidas e suas posições tridimensionais estão traçadas na Figura 8, Figura 9 e Figura 10. Cabe ressaltar que, apesar de a mesa possuir dispositivos de medição dos ângulos de inclinação, ela não é utilizada como referência de atitude para a calibração. apresentados na Tabela 4. A matriz indica a estimação de erros de desalinhamento entre o acelerômetro e o magnetômetro de , e em torno dos eixos Z, Y e X, respectivamente. Tabela 4. Parâmetros estimados para a configuração I linha 1 Coluna 2 3 1 Coluna 2 3 1 2 3 linha 1 2 3 Figura 7. Mesa de inclinação, não magnética, com dois eixos Decomposição Polar de linha 1 coluna 2 3 1 coluna 2 3 1 2 3 linha 1 2 3 Para avaliar a qualidade dessa estimativa, o sensor foi posto sobre a mesa de inclinação nivelada e alinhada ao norte magnético, e foram tomadas medidas de ângulos de rumo de a com passos de . Os erros nas estimativas de atitude e rumo estão mostrados na Figura 11, a qual indica erros máximos após calibração de , e , em rumo, arfagem e rolagem, respectivamente. Quando o magnetômetro é calibrado por ajustamento de elipsoide o erro máximo em rumo apresentado tem a mesma ordem de grandeza, , conforme era de se esperar para a configuração I - sem interferência magnética. Figura 8. Medidas normalizadas da intensidade do campo magnético e da aceleração da gravidade Figura 9. Representação tridimensional das medidas do acelerômetro (valores normalizados) Figura 10. Representação tridimensional das medidas do magnetômetro (valores normalizados) Figura 11. Erros nas estimativas de rumo e atitude para a configuração I Os parâmetros estimados na configuração I estão 3442 Anais do XX Congresso Brasileiro de Automática Belo Horizonte, MG, 20 a 24 de Setembro de 2014 Na configuração II, a estimação dos parâmetros do acelerômetro não se altera, então foram estimados apenas os parâmetros do magnetômetro, que estão apresentados na Tabela 5. Nesta configuração, a interferência magnética adicionada teve grande influência no bias, caracterizando-se como do tipo hard-iron, em vez de soft-iron, mesmo assim, os ângulos de desalinhamento foram alterados para , e em torno dos eixos Z, Y e X, respectivamente. 7 Conclusões Foi proposto um método de autocalibração de unidades de referência de atitude de baixo custo, que combina ajustamento de elipsoide com invariância do produto interno. Simulações e resultados experimentais mostraram que o método é capaz de compensar erros de desalinhamento do magnetômetro, causados por materiais magnéticos que estejam presentes na plataforma, ao contrário de outros métodos tradicionalmente empregados. Como o método não usa referência externa, também pode ser aplicado na calibração adaptativa dos sensores, compensando parâmetros que possam variar com o tempo, por exemplo, em função da temperatura ou da configuração magnética da plataforma onde opera. Tabela 5. Parâmetros estimados na configuração II linha coluna 2 1 3 1 2 3 Referências Bibliográficas Decomposição Polar de linha 1 coluna 2 3 1 coluna 2 3 Britsh Geological Survey, (2010). World Magnetic Model 2010 Calculator. [Online] Disponível em: http://www.geomag.bgs.ac.uk/data_service/m odels_compass/wmm_calc.html [Acesso em 27 Outubro 2013]. Fang, J. et al., (2011). A novel calibration method of magnetic compass based on ellipsoid fitting. IEEE Trans. Instrum. Meas., Jun, Volume 60, pp. 2053-2061. Fitzgibbon, A., Pilu, M. & Fisher, R. B., (1999). Direct least square fitting of ellipses. IEEE Trans. on Pattern Analysis and Machine Intelligence, Volume 21, pp. 476-480. Gebre-Egziabhe, D., Elkaim, G. H. & Powell, J. D., (2006). Calibration of Strapdown Magnetometers in the Magnetic Field Domain. Journal of Aerospace Engineering, Volume 19, pp. 87-102. Gebre-Egziabher, D., (2007). Magnetometer Autocalibration Leveraging Measurement Locus Constraints. Journal of Aircraft, Volume 44, pp. 1361-1368. Halır, R. & Flusser, J., (1998). Numerically stable direct least squares fitting of ellipses. Proc. 6th International Conference in Central Europe on Computer Graphics and Visualization. WSCG, Volume 98, pp. 125132. Li, Z. & Li, X., (2012). A new calibration method for tri-axial field sensors in strap-down navigation systems. Measurement Science and Technology, 3 Setembro, p. 105 105 (6 pp.). Mathwork, 2014. Matlab Documentation Center. [Online] Disponível em: http://www.mathworks.com/help/matlab/ref/at an2.html [Acesso em 30 maio 2014]. MicroStrain Inc, (2006). 3DM-GX1 Datasheet. [Online] Disponível em: http://files.microstrain.com/3DM- 1 2 3 linha 1 2 3 Novamente, para esta configuração, os erros de atitude e rumo foram medidos com a mesa de inclinação nivelada. Os erros estão mostrados na Figura 12, e os valores máximos após calibração foram , e , em rumo, arfagem e rolagem, respectivamente. Já com calibração do magnetômetro por ajustamento de elipsoide apenas, os erros em rumo triplicaram em relação à configuração I, resultando em erro máximo de . Figura 12. Erros nas estimativas de rumo e atitude para a configuração II 3443 Anais do XX Congresso Brasileiro de Automática Belo Horizonte, MG, 20 a 24 de Setembro de 2014 GX1%20Datasheet%20Rev%201.pdf [Acesso em 24 JAN 2014]. Pylvänäinen, T., (2008). Automatic and adaptive calibration of 3D field sensors. Applied Mathematical Modelling, Volume 32, pp. 575-587. Renaudin, V., Afzal, M. H. & Lachapelle, G., (2010). Complete triaxis magnetometer calibration in the magnetic domain. Journal of Sensors, pp. 1-10. Renk, L. R. et al., (2005). Calibrating a triaxial accelerometer-magnetometer - using robotic actuation for sensor reorientation during data collection. IEEE Control Syst., 25(6), pp. 8695. Syed, Z. F. et al., (2007). A new multi-position calibration method for MEMS inertial navigation systems. Meas. Sci. Technol., jul, 18(7), pp. 1897-1907. Tabatabaci, S. A. H., Gluhak, A. & Tafazolli, R., (2013). A Fast Calibration Method for Triaxial Magnetometer. IEEE Transactions on Instrumentation and Measurement, Novembro, Volume 63, pp. 2929-2937. 3444