UNIVERSIDADE FEDERAL DO ESPÍRITO SANTO CENTRO TECNOLÓGICO DEPARTAMENTO DE ENGENHARIA ELÉTRICA DISSERTAÇÃO DE MESTRADO SÉRGIO SILVA MUCCIACCIA ALGORITMO DE CALIBRAÇÃO DE MAGNETÔMETROS TRIAXIAIS UTILIZANDO AJUSTE DE QUÁDRICA POR DISTÂNCIA ALGÉGRICA VITÓRIA – ES AGOSTO/2017 SÉRGIO SILVA MUCCIACCIA ALGORITMO DE CALIBRAÇÃO DE MAGNETÔMETROS TRIAXIAIS UTILIZANDO AJUSTE DE QUÁDRICA POR DISTÂNCIA ALGÉBRICA Parte manuscrita da dissertação de mestrado do aluno Sérgio Silva Mucciaccia, apresentada ao Programa de Pós-graduação em Engenharia Elétrica da Universidade Federal do Espírito Santo, como requisito para a obtenção do título de mestre em Engenharia Elétrica. Orientador: Prof. Dr. Anselmo Frizera Neto Co-orientador: Prof. Dr. Evandro Ottoni Teatini Salles VITÓRIA – ES AGOSTO/2017 Dados Internacionais de Catalogação-na-publicação (CIP) (Biblioteca Setorial Tecnológica, Universidade Federal do Espírito Santo, ES, Brasil) Sandra Mara Borges Campos – CRB-6 ES-000593/O Mucciaccia, Sérgio Silva, 1991- M942a Algoritmo de calibração de magnetômetros triaxiais utilizando ajuste de quádrica por distância algébrica / Sérgio Silva Mucciaccia. – 2017. 77 f. : il. Orientador: Anselmo Frizera Neto. Coorientador: Evandro Ottoni Teatini Salles. Dissertação (Mestrado em Engenharia Elétrica) – Universidade Federal do Espírito Santo, Centro Tecnológico. 1. Magnetômetro – Calibração. 2. Elipsóide. 3. Mínimos quadrados. 4. Algoritmos. 5. Sensores inerciais. 6. Distância algébrica. I. Frizera Neto, Anselmo. II. Salles, Evandro Ottoni Teatini. III. Universidade Federal do Espírito Santo. Centro Tecnológico. IV. Título. CDU: 621.3 SÉRGIO SILVA MUCCIACCIA ALGORITMO DE CALIBRAÇÃO DE MAGNETÔMETROS TRIAXIAIS UTILIZANDO AJUSTE DE QUÁDRICA POR DISTÂNCIA ALGÉBRICA Parte manuscrita da dissertação de mestrado do aluno Sérgio Silva Mucciaccia, apresentada ao Programa de Pós-graduação em Engenharia Elétrica da Universidade Federal do Espírito Santo, como requisito para a obtenção do título de mestre em Engenharia Elétrica. Aprovada em 22 de agosto de 2017. COMISSÃO EXAMINADORA: ________________________________________________ Prof. Dr. Anselmo Frizera Neto Universidade Federal do Espírito Santo Orientador ________________________________________________ Prof. Dr. Evandro Ottoni Teatini Salles Universidade Federal do Espírito Santo Co-orientador ________________________________________________ Prof. Dr. Patrick Marques Ciarelli Universidade Federal do Espírito Santo Examinador ________________________________________________ Prof. Dr. Samuel Lourenço Nogueira Universidade Federal de São Carlos Examinador AGRADECIMENTOS À Universidade Federal do Espírito Santo e ao Programa de Pós-Graduação em Engenharia Elétrica por viabilizarem esta pesquisa e ao CNPq e à FAPES pelo apoio financeiro. Ao Prof. Dr. Anselmo Frizera Neto, pela orientação neste trabalho, por me acompanhar e auxiliar com dedicação e competência desde a graduação e pelos valiosos ensinamentos. Ao Prof. Dr. Evandro Ottoni Teatini Salles por todo o apoio, pela co-orientação nesta dissertação e por me ensinar os principais conceitos utilizados neste trabalho em suas aulas. A todos os professores do Programa de Pós-Graduação em Engenharia Elétrica da UFES que tanto contribuíram para a minha vida acadêmica e no desenvolvimento deste trabalho. À minha mãe Margarete Amorim da Silva e ao meu irmão Bruno Silva Mucciaccia pelo amor e suporte dados ao longo desta dissertação e durante toda a minha vida. E finalmente a Deus, por todas as bênçãos e oportunidades. RESUMO As medidas obtidas de sensores de campo magnético são sensíveis a perturbações e erros, sendo necessário um método de calibração que pode melhorar consideravelmente sua precisão. O ajuste de elipsoides é um dos métodos mais utilizados para calibração de magnetômetros, porém a maioria dos algoritmos utiliza métodos iterativos, causando problemas de tempo de execução e convergência. Como alternativa, propõe-se um algoritmo direto de cálculo dos parâmetros de calibração utilizando o método dos mínimos quadrados com a métrica de distância algébrica. O presente trabalho apresenta um algoritmo de calibração de magnetômetros e a sua utilização em um sistema de calibração e fusão de dados de magnetômetros, acelerômetros e giroscópios baseado em um filtro de Kalman formando um sensor inercial capaz de obter sua orientação no espaço. As simulações computacionais e testes com dados reais mostram que o algoritmo de calibração elimina quase a totalidade dos erros lineares, enquanto executa muito mais rápido que os algoritmos tradicionais encontrados na literatura. As medições de um magnetômetro calibrado com o algoritmo proposto são utilizadas em conjunto com medições provenientes de acelerômetros e giroscópios para formar uma unidade de medição inercial (IMU) usando um filtro de Kalman simples. O sistema completo funcionou como esperado e os resultados dos testes indicam que o algoritmo de calibração de magnetômetros é adequado para utilização em uma IMU, sendo mais de dez vezes mais rápido que os algoritmos tradicionais e apresentando precisão similar. Palavras-chave: Calibração de magnetômetros, ajuste de elipsoide, mínimos quadrados, distância algébrica, sensores inerciais. ABSTRACT The measurements obtained from magnetometers are sensitive to disturbances and errors, requiring a calibration method that can considerably improve accuracy. The ellipsoid fitting is one of the most widely used methods for magnetometer calibration, but most algorithms use iterative methods, causing runtime and convergence problems. As an alternative, a direct algorithm based on the method of least squares using the algebraic distance metric is proposed. This present work presents an algorithm of calibration of magnetometers and its use in a system of calibration and fusion of data of magnetometers, accelerometers and gyroscopes based on a Kalman filter forming an inertial sensor able to obtain its orientation in the space. Computational simulations and tests with real data show that the calibration algorithm eliminates almost all the linear errors while performing much faster than traditional algorithms. Measurements of a magnetometer calibrated with the proposed algorithm are used in conjunction with measurements from accelerometers and gyroscopes to form an inertial measurement unit (IMU) using a simple Kalman filter. The complete system worked as expected and the test results indicate that the magnetometer calibration algorithm is suitable for use in an IMU being more than ten times faster than traditional algorithms and presenting similar accuracy. Keywords: Magnetometer calibration, ellipsoid fitting, least squares, algebraic distance, inertial sensors. LISTA DE FIGURAS Figura 1 – Magnetômetro construído no século XIX utilizando o modelo criado por Gauss. 9 Figura 2 – Acelerômetro B&K modelo 4303, provavelmente o primeiro acelerômetro piezoelétrico disponível comercialmente. 10 Figura 3 - Giroscópio projetado por Léon Foucault e construído por Pierre Dumoulin-Froment em 1852. Se encontra no museu de artes e ofícios de Paris. 11 Figura 4 - Conjunto de engrenagens feito com tecnologia MEMS visto pelo microscópio. 12 Figura 5 - IMUs de três diferentes fabricantes. Em (a) o tech IMU CV4 da Technaid, em (b) o MTi-100 IMU da Xsens e em (c) o MyoMotion clinical da Noraxon. 12 Figura 6 - Várias tecnologias que utilizam sensores inerciais. Em (a) é apresentada uma ilustração de um VANT utilizando sensores inerciais, em (b) sensores inerciais sendo utilizados em um controlador de jogos, em (c) é mostrada a utilização destes sensores para reabilitação e em (d) sua utilização na análise de movimentos. 15 Figura 7 - Nuvem de pontos sem erros à esquerda e nuvem de pontos com erro de translação à direita. 20 Figura 8 - Nuvem de pontos sem erros à esquerda e nuvem de pontos com erro de escala à direita. 20 Figura 9 - Nuvem de pontos sem erros à esquerda e nuvem de pontos com erros de escala e rotação à direita. 22 Figura 10 - Curvas de mesma distância algébrica. 25 Figura 11 – Combinação de duas gaussianas. 31 Figura 12-Algumas formas básicas representadas pela equação das quádricas. 42 Figura 13 - Função do erro em relação a t. 46 Figura 14 – Corte no plano xy de um elipsoide obtido pelo ajuste utilizando descida de gradiente. 56 Figura 15 - Mesmo corte da Figura 14, mas com um ajuste com erro quadrático menor. 57 Figura 16 - Caminho dos ajustes até o ajuste ótimo. 59 Figura 17 - Caminho da descida de gradiente para um paraboloide. 59 Figura 18 - Ajuste aos pontos com ruído gaussiano de desvio-padrão 2,5 µT. 62 Figura 19 - Ajuste aos pontos com ruído gaussiano de desvio-padrão 5,0 µT. 63 Figura 20 - Ajuste aos pontos com ruído gaussiano de desvio-padrão 25,0 µT. 63 Figura 21 - Ajuste aos pontos com ruído gaussiano de desvio-padrão 0,1 µT em 3D. 64 Figura 22 - Comparação dos algoritmos quanto ao tempo de execução. 67 Figura 23 - Teste do algoritmo proposto em um sistema embarcado. 68 LISTA DE SÍMBOLOS �⃗⃗� Vetor 3x1 representando uma medição de um magnetômetro triaxial. 𝑚𝑟⃗⃗ ⃗⃗ ⃗ Vetor 3x1 representado o valor real do campo magnético no magnetômetro. 𝐸 Matriz 9x9 que modela o erro de sensibilidade, a distorção soft-iron e a não ortogonalidade. e⃗ Vetor 3x1 que representa os erros de offset e a distorção soft-iron combinados. n⃗ Vetor 3x1 que representa ruído aleatório gaussiano de média zero. mc⃗⃗⃗⃗ ⃗ Vetor 3x1 representando a medição de um magnetômetro após a calibração. C Matriz de calibração 9x9 necessária para compensar os erros da matriz 𝐸. o⃗ Vetor de calibração 3x3 necessário para compensar os erros no vetor e⃗ . D Distância algébrica de um ponto 3x1 até uma quádrica qualquer. 𝑟𝑥, 𝑟𝑦, 𝑟𝑧 Os 3 raios de um elipsoide alinhado com os eixos. 𝑔𝑥, 𝑔𝑦, 𝑔𝑧 Medidas de velocidade angular em torno de cada eixo obtidas por um giroscópio. ω Variável escalar representando a velocidade angular de um objeto girante. 𝑎 Vetor 3x1 que representa o único eixo de um objeto girante que se mantém na mesma direção durante o giro. 𝑎𝑥, 𝑎𝑦, 𝑎𝑧 Os três valores escalares do vetor 𝑎 . θ Ângulo da rotação sofrida por um objeto entre duas medições de um giroscópio. 𝑓 Frequência de amostragem do giroscópio. 𝑞𝑥, 𝑞𝑦, 𝑞𝑧 , 𝑞𝑤 Valores de um quatérnio de rotação obtidos da medição de um giroscópio. µ Média de uma variável aleatória. σ Variância de uma variável aleatória. 𝑃 Matriz de covariâncias relativa às variáveis escalares do vetor 𝑣 . 𝑃𝑘 Valor da matriz 𝑃 em um instante k. 𝐹𝑘 Matriz de predição do filtro de Kalman encontrada no passo k. 𝑄𝑘 Matriz de ruído do filtro de Kalman. 𝐵𝑘 Matriz de controle do filtro de Kalman. 𝑢𝑘⃗⃗⃗⃗ Vetor de controle do filtro de Kalman. 𝑁 Número de medições efetuadas por um magnetômetro na etapa de treinamento dos parâmetros de calibração. Є Somatório do erro quadrático de distância algébrica de cada um dos 𝑁 pontos da etapa de treinamento até a quádrica. 𝑀 Matriz 9x9 formada pelos termos dos coeficientes do sistema linear obtidos com as derivadas parciais da equação da quádrica em relação a cada coeficiente. 𝑤1, 𝑤2, … , 𝑤9 Termos livres do sistema linear das derivadas parciais da equação da quádrica. 𝑎, 𝑏, … , 𝑗 Coeficientes da equação da quádrica que possui erro quadrática de distância algébrica mínimo em relação ao conjunto de pontos de treinamento. 𝑥, 𝑦, 𝑧 Coordenadas de um ponto arbitrário no espaço tridimensional, obtido pela medição de um magnetômetro triaxial. 𝐴 Matriz principal da equação das quádricas na forma matricial. É sempre uma matriz simétrica, independente da quádrica. 𝑉 Matriz de autovetores ortogonal da matriz 𝐴. 𝐷 Forma diagonal da matriz 𝐴. 𝑥𝑐, 𝑦𝑐, 𝑧𝑐 Coordenadas de um ponto arbitrário no espaço tridimensional, obtido pela medição de um magnetômetro triaxial, após a calibração. 𝑗𝑐 Termo livre da equação da quádrica depois das transformações de calibração. É utilizado para normalizar a calibração obtendo-se uma esfera de raio unitário. 𝑟1, 𝑟2, … , 𝑟9 Coeficientes lineares de uma equação paramétrica da reta em 9 dimensões. 𝑠1, 𝑠2, … , 𝑠9 Termos livres da equação paramétrica da reta em 9 dimensões. 𝑡 Variável independente da equação paramétrica da reta. Cada valor de 𝑡 representa um ponto distinto sobre a reta. 𝜏 Valor da variável 𝑡 que torna o determinante da matriz 𝐴 nulo. 𝑈, 𝑉,𝑊 Matrizes representando transformações lineares em um espaço tridimensional. SUMÁRIO 1 Introdução ................................................................................................................ 9 1.1 Apresentação .................................................................................................... 9 1.2 Objetivos ......................................................................................................... 14 1.2.1 Objetivo Geral ........................................................................................... 14 1.2.2 Objetivos Específicos ................................................................................ 14 1.3 Justificativa ...................................................................................................... 14 1.4 Organização da dissertação ............................................................................ 18 2 Fundamentação Teórica e Proposta de Pesquisa .................................................. 19 2.1 Algoritmos de calibração de magnetômetros ................................................ 19 2.1.1 Erros nas medições dos sensores ............................................................. 19 2.1.2 Compensação dos erros ............................................................................ 22 2.1.3 A métrica de distância algébrica no ajuste de elipsoides ......................... 23 2.2 Algoritmos de calibração de acelerômetros e giroscópios ............................. 26 2.3 Medições dos giroscópios ............................................................................... 27 2.4 O filtro de Kalman para fusão de dados em sensores inerciais ...................... 28 2.4.1 Combinação de gaussianas ....................................................................... 28 2.4.2 O equacionamento do filtro de Kalman ................................................... 31 2.5 Considerações finais ....................................................................................... 34 3 Materiais e métodos .............................................................................................. 35 3.1 Materiais ......................................................................................................... 35 3.2 Calibração do magnetômetro ......................................................................... 36 3.2.1 O método de ajuste de quádrica proposto ............................................... 36 3.2.2 Calculando os parâmetros de calibração .................................................. 39 3.3 Argumentos teóricos ...................................................................................... 42 3.3.1 Comparação entre os resultados de algoritmos com e sem restrições ... 42 3.3.2 O efeito das restrições no ajuste .............................................................. 46 3.3.3 Notas sobre os resultados dos algoritmos com restrições ....................... 50 3.4 Fusão de sensores inerciais ............................................................................ 51 4 Validação experimental .......................................................................................... 52 4.1 Descrição dos experimentos e métricas de validação .................................... 52 4.2 Simulações computacionais ............................................................................ 52 4.2.1 Algoritmos com restrição de elipsoide ..................................................... 52 4.2.2 Simulações com o algoritmo proposto ..................................................... 60 4.3 Testes em sistema embarcado ....................................................................... 66 4.4 Discussão dos resultados ................................................................................ 68 5 Conclusões e trabalhos futuros .............................................................................. 69 5.1 Conclusões e contribuição da pesquisa .......................................................... 69 5.2 Publicações ..................................................................................................... 70 5.3 Trabalhos Futuros ........................................................................................... 70 Referências bibliográficas ............................................................................................. 72 9 1 Introdução 1.1 Apresentação Os sensores de orientação têm presença ubíqua na sociedade atual, sendo utilizados em celulares (KOS; TOMAZIC; UMEK, 2016), robôs (POMARES et al., 2011), métodos de navegação (YUAN et al., 2015) e outras aplicações. Dentre eles, três se destacam por não necessitarem de um referencial fixo: o magnetômetro, o acelerômetro e o giroscópio. O campo magnético e a gravidade funcionam como referencial para os magnetômetros e acelerômetros, respectivamente. Já a medição da orientação com giroscópios necessita de um referencial inicial de direção, mas não necessita de um referencial fixo externo (WILSON, 2005). O primeiro magnetômetro encontrado na literatura foi inventado por Carl Friedrich Gauss em 1833 e é descrito em um dos trabalhos fundamentais do autor titulado “Intensitas vis magneticae terrestris ad mesuram absolutam revocata” (ASSIS, 2003). Foi devido a este trabalho que Gauss teve a unidade de densidade de fluxo magnético associada ao seu nome. Basicamente, o sistema consistia em um dispositivo mecânico que media a intensidade e direção do campo magnético local (BAYLOR; HAZARD, 1900). A Figura 1 ilustra o magnetômetro de Gauss. Figura 1 – Magnetômetro construído no século XIX utilizando o modelo criado por Gauss. Fonte: (BAYLOR; HAZARD, 1900). 10 Os primeiros acelerômetros eram sensores unidirecionais baseados em uma massa, molas e um sensor de pressão. A massa aplicava uma força no sensor de pressão que variava com a aceleração do dispositivo no eixo sensível, deste modo, era possível medir a aceleração indiretamente utilizando a pressão (WALTER, 2007). A Figura 2 mostra o primeiro acelerômetro piezoelétrico disponível comercialmente que data de 1945. Figura 2 – Acelerômetro B&K modelo 4303, provavelmente o primeiro acelerômetro piezoelétrico disponível comercialmente. Fonte: (WALTER, 2007). A invenção do giroscópio foi creditada a Jean Bernard Léon Foucault, apesar de existirem máquinas mais antigas utilizando o mesmo efeito de conservação do momento angular. Em 1852, Foucault construiu um giroscópio e o utilizou como uma forma alternativa de comprovar experimentalmente a rotação da Terra (GARIEL; BERTRAND, 1878). O giroscópio mantém um eixo apontando para uma mesma direção enquanto a Terra gira. Portanto, ao longo do dia, a direção do eixo do giroscópio se modifica de acordo com a rotação da Terra. A Figura 3 mostra um giroscópio construído utilizando o mesmo projeto de Foucault como base. 11 Figura 3 - Giroscópio projetado por Léon Foucault e construído por Pierre Dumoulin-Froment em 1852. Se encontra no museu de artes e ofícios de Paris. Fonte: (Museu de Artes e Ofícios de Paris, 2016). Esses sensores eram máquinas mecânicas que se utilizavam de fenômenos físicos, nas quais se poderia tirar uma medida de orientação. Pela natureza destas máquinas, elas eram de difícil miniaturização, pois a construção de dispositivos mecânicos, como discos, molas ou engrenagens em dimensões muito pequenas é extremamente difícil. Em 1950, surgiu a tecnologia de dispositivos micro-eletro-mecânicos (MEMS) que consiste na criação de máquinas mecânicas em um cristal de silício através de reações químicas de corrosão e deposição. Essa tecnologia permitiu a criação de discos, molas e engrenagens em escalas microscópicas e como consequência, a construção de diversos sensores mecânicos extremamente pequenos (BOGUE, 2007). A Figura 4 mostra um sistema de engrenagens criado com a tecnologia MEMS vista pelo microscópio, onde o sistema inteiro possui 8mm de diâmetro. É interessante notar que um sistema de engrenagens está sujeito a vários mecanismos de desgaste, o que torna impressionante a tecnologia de construção de um sistema robusto nestas dimensões. Muitos sensores são mais simples que o sistema mostrado na figura, possibilitando uma miniaturização ainda maior (GOHDSSI; PINYEN, 2011). 12 Figura 4 - Conjunto de engrenagens feito com tecnologia MEMS visto pelo microscópio. Fonte: (GOHDSSI; PINYEN, 2011). Além disso, com o crescente progresso na estatística e no desenvolvimento de hardware começaram a surgir métodos para aumentar consideravelmente a precisão destes sensores através da compensação de erros e do ruído aleatório. Trabalhos de nomes como Wiener e Kalman (WIENER, 1949; KALMAN, 1960) formaram uma base para o projeto de sistemas cada vez mais robustos, possibilitando a calibração dos sensores, filtragem dos ruídos e até mesmo a fusão de vários sensores para formar um sensor com maior precisão. Com essas novas teorias e a miniaturização dos magnetômetros, acelerômetros e giroscópios foi possível a construção de uma unidade de medição inercial (IMU, do inglês, Inertial Measurement Unit). Uma IMU é um sistema que combina esses três tipos de sensores e utiliza algoritmos de fusão de sensores para obter um sensor capaz de medir sua orientação completa no espaço. A Figura 5 mostra algumas dessas unidades de diferentes fabricantes. Figura 5 - IMUs de três diferentes fabricantes. Em (a) o tech IMU CV4 da Technaid, em (b) o MTi-100 IMU da Xsens e em (c) o MyoMotion clinical da Noraxon. Fontes: (a) (Technaid, 2016), (b) (Xsens, 2016), (c) (Noraxon, 2016). 13 A miniaturização, o aumento na velocidade e o aumento na precisão das IMUs tornou possível sua utilização em cada vez mais aplicações, como na reabilitação e na análise de movimento (FONG; CHAN, 2010), gerando uma demanda crescente por sensores melhores. Para satisfazer a essa demanda, a evolução dos sensores deve ocorrer em duas frentes, no processo de fabricação destes sensores como também nos métodos para tratar os dados gerados. Assim, uma vez que dispositivos mais complexos são construídos a partir das IMUs, a pressão para métodos e algoritmos mais precisos e eficientes tem sido cada vez maior. Um sistema completo de sensoriamento eletrônico normalmente se constitui de uma placa com sensores sob a forma de circuitos integrados e uma unidade de processamento onde podem ocorrer processos de calibração, filtragem de ruídos e fusão de dados (WILSON, 2005). A calibração consiste em retirar os erros lineares presentes nas medições dos sensores que podem ocorrer devido ao processo de fabricação ou às condições de utilização do sensor (HOL, 2011). Qualquer sensor eletrônico está sujeito a ruídos aleatórios, que podem ser causados por vibrações, variações de temperatura, interferências eletromagnéticas ou outras condições na qual o sensor está submetido. Os processos de filtragem de ruídos conseguem retirar uma parte destes ruídos aleatórios que estão presentes no sinal gerado pelo sensor. Finalmente, a fusão de sensores consiste em um método para combinar informações de vários sensores gerando uma informação mais precisa que os sensores individuais ou até uma informação nova, que não pode ser obtida por nenhum dos sensores separadamente (KHALEGHI et al., 2013). O tema deste trabalho trata dos algoritmos de calibração, filtragem de ruído e fusão de dados aplicados especificamente a magnetômetros, acelerômetros, giroscópios e unidades de medição inercial. Um novo algoritmo de calibração de magnetômetros baseado em ajuste de elipsoide1 utilizando mínimos quadrados com a métrica de distância algébrica é apresentado em detalhes, sendo explicadas todas as teorias e equações envolvidas. O algoritmo é utilizado em conjunto com um algoritmo de fusão de dados baseado em um filtro de Kalman formando um sistema completo de unidade de medição inercial (IMU). Enfim, são 1 O ajuste de elipsoide é um método de minimização onde dado um conjunto de pontos no espaço tridimensional e uma métrica de erro é encontrado um elipsoide que possui erro mínimo em relação ao conjunto de pontos. O elipsoide obtido se relaciona aos erros de medição do magnetômetro. 14 mostrados os resultados dos algoritmos apresentados por meio de testes de simulação computacional e testes experimentais. 1.2 Objetivos 1.2.1 Objetivo Geral O principal objetivo deste trabalho é desenvolver um algoritmo de calibração de magnetômetros com baixo custo computacional. A finalidade do algoritmo é ser utilizado em conjunto com outros algoritmos de calibração e fusão de dados, de modo que magnetômetros, acelerômetros e giroscópios sejam combinados em uma unidade de medição inercial. 1.2.2 Objetivos Específicos Os objetivos específicos que devem ser atingidos para alcançar o objetivo geral são: • Pesquisar na literatura os métodos e algoritmos existentes e compará-los quanto a precisão e eficiência para auxiliar nas decisões de projeto do sistema. • Desenvolver um algoritmo de calibração de magnetômetro. • Projetar e implementar um algoritmo de calibração de acelerômetros e giroscópios. • Desenvolver um algoritmo de fusão formando uma unidade de medição inercial. • Realizar simulações computacionais e experimentais com os algoritmos desenvolvidos. 1.3 Justificativa Várias tecnologias que estão em desenvolvimento atualmente se beneficiam grandemente do crescente aumento de qualidade dos sensores de orientação. Alguns exemplos são a internet das coisas, a computação vestível, a navegação de veículos aéreos não tripulados (VANTs), as tecnologias de reabilitação assistida, a análise de movimento e a realidade virtual (AHMAD et al., 2013). Devido a essas aplicações, o desenvolvimento nas áreas de pesquisa relativas às IMUs tem o potencial de gerar externalidade positiva, contribuindo para o progresso de várias tecnologias promissoras. A Figura 6 ilustra a utilização dos sensores inerciais em algumas tecnologias. 15 A área da medição de orientação por sensores inerciais tem sido objeto de diversos estudos e ainda possui muito espaço para pesquisas (GAHLOT; GAHLOT, 2012). Novos sensores com características de velocidade, precisão e consumo de energia cada vez melhores foram lançados no mercado nas últimas décadas e futuramente a miniaturização e o aumento de precisão nestes sensores possivelmente permitirão sua utilização em novas aplicações. Figura 6 - Várias tecnologias que utilizam sensores inerciais. Em (a) é apresentada uma ilustração de um VANT utilizando sensores inerciais, em (b) sensores inerciais sendo utilizados em um controlador de jogos, em (c) é mostrada a utilização destes sensores para reabilitação e em (d) sua utilização na análise de movimentos. Fontes: (a) (Memsic, 2016), (b) (Ponto do videogame, 2016), (c) (Xsens, 2016), (d) (Noraxon, 2016). Apesar das constantes melhorias no desenvolvimento dos magnetômetros, por melhor que o sensor seja, se o seu objetivo é medir o campo magnético de uma fonte específica, como o da Terra, ele sempre estará suscetível às distorções de outros campos magnéticos gerados pela presença de materiais ferromagnéticos, pois essa alteração se dá no próprio campo magnético a ser medido. Assim sendo, mesmo com os avanços recentes na tecnologia de fabricação destes sensores, a necessidade do progresso nos métodos de compensação das distorções e ruídos magnéticos causadas pelo ambiente condiciona a melhoria da medição da orientação final obtida dos sensores. Atualmente, a maioria das aplicações que utilizam magnetômetros possuem um processo de calibração baseado no ajuste de elipsoide, realizado por meio de algoritmos iterativos de otimização de uso geral com o método de mínimos quadrados em distância 16 euclidiana (OLIVARES et al., 2013). Enquanto o ajuste de elipsoide tem mostrado bons resultados (RENAUDIN; AFZAL; LACHAPELLE, 2010) e tem sido usado por grande parte dos trabalhos atuais, a utilização de métodos iterativos e até mesmo a métrica de distância euclidiana tem sido questionadas. Os métodos iterativos utilizados atualmente, muitas vezes, apresentam problemas no tempo de convergência, exigindo uma grande quantidade de computações e condições iniciais precisas (FANG et al., 2011). Estes problemas não existem em métodos por resolução analítica das equações. Já a utilização da distância euclidiana tem sido questionada pela precisão dos resultados, podendo não ser a melhor métrica para a calibração de magnetômetros. Para este trabalho, foram pesquisados alguns artigos que apresentam processos para calibração de magnetômetros. Todos os artigos pesquisados utilizaram o método de ajuste de elipsoide, o que mostra uma tendência na escolha do melhor método entre os pesquisadores. Nesse sentido é possível citar Camps, Harasse e Monin (2009), Fang et al. (2011), Hemerly e Coelho (2014), Renaudin, Afzal e Lachapelle (2010) e Vanconcelos et al. (2011). O ajuste de uma superfície como um elipsoide pode ser visto como a minimização de alguma métrica de erro. As métricas mais utilizadas são a distância euclidiana (VASCONCELOS et al., 2011) e a distância algébrica (HEMERLY; COELHO, 2014), mas alguns autores como Renaudin, Afzal e Lachapelle (2010) argumentam que estas duas métricas possuem viés estatístico e utilizam métricas alternativas. Assim sendo, não existe um consenso sobre a métrica a ser utilizada para o ajuste. Em relação aos algoritmos utilizados para realizar o ajuste de elipsoide, existe uma variabilidade ainda maior na literatura sendo encontrados algoritmos baseados em métodos como Levenberg-Marquadt (CAMPS; HARASSE; MONIN, 2009), resolução por auto- vetores (HEMERLY; COELHO, 2014) e estimadores de máxima verossimilhança (VASCONCELOS et al., 2011). Portanto, as técnicas para a realização do método de ajuste de elipsoide para calibração de magnetômetros ainda oferecem campo para novas pesquisas. Assim sendo, o algoritmo de calibração de magnetômetros apresentado neste trabalho foi desenvolvido visando a contribuição com o avanço das técnicas de calibração de magnetômetros. A calibração de acelerômetros e giroscópios também é importante, porém já existem métodos eficientes e precisos na literatura para a calibração destes sensores (STANCIN; TOMAZIC, 2014) considerando os requisitos da utilização em sensores inerciais. Já as medições dos magnetômetros ficam muitas vezes a desejar em eficiência e precisão quando se trata da utilização em sensores de orientação. 17 A fusão dos acelerômetros, magnetômetros e giroscópios em uma unidade de medição inercial permite que a orientação seja calculada de forma mais precisa. Como o giroscópio tri- axial mede apenas a velocidade angular, o cálculo da orientação com este sensor envolve a integração de suas medições causando um aumento do erro com o tempo. O acelerômetro pode funcionar como um sensor de inclinação, mas não pode oferecer a orientação no plano horizontal, assim como o magnetômetro não consegue obter a orientação no plano perpendicular ao campo magnético. Portanto, este tipo de fusão promove a complementação e redundância das informações obtidas pelos sensores e o faz de modo indireto, já que nenhum destes sensores calcula diretamente a orientação. Existem várias formas de representar uma orientação no espaço, dentre elas estão os ângulos de Euler, o vetor de orientação, a matriz de orientação e os quatérnios (RAO, 2006). Apesar de qualquer uma dessas formas poder ser convertida em qualquer outra, a escolha da representação tende a alterar significativamente os algoritmos utilizados na fusão (BERGAMINI et al., 2014). As formas de representação já foram bem caracterizadas na literatura e as duas formas mais utilizadas são os ângulos de Euler e os quatérnios, cada uma apresentando características específicas. O trabalho apresentado em (GAHLOT; GAHLOT, 2012) compara estas formas de representação concluindo que a utilização dos quatérnios é a que possui mais vantagens para utilização em um filtro de Kalman. Isso se deve à maior eficiência da rotação em quatérnios e o fato desta representação ser adequada para integração em relação ao tempo. Alguns dos principais algoritmos de fusão utilizados são o filtro complementar (KHALKHALI-SHARIFI; SHAHDLOO; VOSSOUGHI, 2014) e o filtro de Kalman (ABYARJOO et al., 2012). Observa-se, na literatura, uma forte tendência ao uso do filtro de Kalman como ferramenta para a fusão, filtragem e predição para sensores inerciais, sendo possível utilizá-lo em uma única plataforma com rapidez e precisão (SABATINI, 2011). A área de pesquisa em sensores inerciais possui uma literatura rica, na aplicação da análise de movimento, por exemplo, existem dezenas de artigos, vários deles obtendo bons resultados (FONG; CHAN, 2010). Apesar de ser uma área bem estudada, ainda existem pequenos problemas e desafios a serem resolvidos capazes de contribuir significativamente para a melhora nas aplicações destes sensores (GAHLOT; GAHLOT, 2012). Os sensores inerciais têm sido alvo de muita pesquisa desde o fim da década de 1990 até os últimos anos, com produtos comerciais baseados nesta tecnologia já sendo utilizados na atualidade, porém 18 alguns destes produtos apresentam problemas ainda não solucionados na literatura (SABATINI, 2011). O tema de pesquisa relativo aos algoritmos de fusão para sensores inerciais já é bem estudado, mas ainda tem necessidade de algumas contribuições científicas. Portanto, a proposta deste trabalho de um novo algoritmo de calibração de magnetômetros e sua utilização em um algoritmo de fusão de dados para formar uma IMU tem uma importância significativa para o desenvolvimento das técnicas de calibração e fusão da área. 1.4 Organização da dissertação Esta dissertação está dividida em 5 capítulos. Após a breve introdução realizada no Capítulo 1, o Capítulo 2 apresenta a fundamentação teórica para explicar os conceitos e teorias utilizados neste trabalho iniciando com o tema de calibração de magnetômetros, passando pelo tema da fusão de sensores inerciais e ao final apresentando a proposta de pesquisa deste trabalho. O Capítulo 3 está dividido em quatro partes. A primeira apresenta os materiais utilizados neste trabalho. A segunda descreve o novo algoritmo proposto para calibração de magnetômetros e o equacionamento envolvido na sua implementação. A terceira expõe alguns argumentos teóricos a favor da utilização do algoritmo apresentado. Finalmente a quarta parte descreve o algoritmo implementado para a fusão dos acelerômetros, magnetômetros e giroscópios em uma unidade de medição inercial capaz de obter a orientação da plataforma onde estão os sensores. O Capítulo 4 começa descrevendo os experimentos e métricas de validação. Após isto, primeiramente são apresentados os testes envolvendo simulações computacionais e em seguida os testes experimentas feitos com sensores de uma plataforma de prototipação. O Capítulo termina com uma seção de discussão dos resultados obtidos nos testes realizados. O Capítulo 5 retorna a alguns conceitos dos capítulos passados para apresentar a conclusão do trabalho, a contribuição desta pesquisa para a área em questão e discute algumas possibilidades de trabalhos futuros nos temas deste trabalho. 19 2 Fundamentação Teórica e Proposta de Pesquisa 2.1 Algoritmos de calibração de magnetômetros 2.1.1 Erros nas medições dos sensores Um magnetômetro triaxial mede a intensidade e direção do campo magnético no ponto onde ele está localizado. A medição efetuada por esse sensor pode não corresponder exatamente ao campo magnético real e é esta diferença que caracteriza os erros de medição. Estes são divididos em dois tipos, os erros lineares que por sua natureza podem ser calculados e compensados e os erros não lineares que nem sempre podem ser compensados e quando podem, exigem métodos sofisticados de tratamento de sinais. Esta seção caracteriza os erros lineares, sendo que neste trabalho os erros não lineares são considerados como ruído aleatório e não são tratados. Os principais erros lineares que afetam as medições dos magnetômetros são o offset, o erro de sensibilidade, as distorções hard-iron e soft-iron e a não ortogonalidade (VASCONCELOS et al., 2011). Estes erros serão explicados a seguir. 2.1.1.1 Offset O offset é definido como o valor que é medido pelo magnetômetro quando o campo magnético real no sensor é zero. O sensor é fabricado para determinadas condições de uso como a tensão de alimentação e a temperatura de funcionamento. Sua utilização em condições diferentes para os quais foi projetado pode provocar um desvio constante nos valores de todas as medições, causando um offset. Além disso, pequenos erros no material podem estar presentes depois da fabricação ou podem ser acumulados durante a utilização dos sensores. Estes erros aparecem na forma de impurezas ou danos na estrutura do material e podem alterar as variáveis que influenciam na medição como a resistência de alguns componentes do sensor (FRADEN, 2010). Neste trabalho o magnetômetro é utilizado para medir campos magnéticos constantes, como o campo magnético da Terra. Ao girar o sensor em um campo magnético constante, a magnitude do vetor de medição deveria ser constante e apenas sua direção deveria variar. Assim, em condições ideais, as medições obtidas girando o magnetômetro nas três dimensões deveriam formar uma esfera centrada na origem. No caso do erro de offset, os pontos das medições que deveriam formar uma esfera centrada na origem são deslocados modificando a posição de seu centro. Este efeito é ilustrado na Figura 7. 20 Figura 7 - Nuvem de pontos sem erros à esquerda e nuvem de pontos com erro de translação à direita. Fonte: Produção do próprio autor. 2.1.1.2 Erro de sensibilidade Um magnetômetro tri-axial é formado por três sensores anisotrópicos que podem ser ligeiramente diferentes. Devido a isso, as medições de cada um dos sensores podem apresentar um valor escalado em relação aos outros dois. O erro de sensibilidade ocorre devido a essas diferenças de escala em cada eixo do magnetômetro (RENAUDIN; AFZAL; LACHAPELLE, 2010). Esse erro tende a escalar as coordenadas das medidas e transformar a esfera de medições em um elipsoide. Figura 8 - Nuvem de pontos sem erros à esquerda e nuvem de pontos com erro de escala à direita. Fonte: Produção do próprio autor. 21 A Figura 8 apresenta dois conjuntos de medições obtidos com a rotação de um magnetômetro. À esquerda são mostradas as medições de um magnetômetro ideal. À direita as mesmas medições para um magnetômetros duas vezes mais sensível no eixo 𝑥 que nos outros eixos. Como ilustrado nessa figura, a sensibilidade de cada um dos eixos de um magnetômetro afeta a forma da nuvem de pontos obtida, sendo que uma maior sensibilidade em um eixo acaba tornando o diâmetro do elipsoide maior naquele eixo. 2.1.1.3 Distorção hard-iron A distorção hard-iron ocorre quando algum objeto na mesma plataforma que o sensor, ou seja, que sempre gira junto com ele, gera um campo magnético constante que se soma às suas leituras (FANG et al., 2011). Um exemplo deste efeito ocorre em telefones celulares que possuem magnetômetros. O alto-falante desses aparelhos possui um ímã permanente que vai afetar as medições do magnetômetro causando uma distorção hard-iron. Esta distorção tem o mesmo efeito nas medições quando comparada com o erro de offset, porém se deve a materiais externos ao sensor. Com esse erro a esfera de medições aparece transladada, com o centro fora da origem como ilustrado na Figura 7. 2.1.1.4 Distorção soft-iron Alguns materiais influenciam ou distorcem o campo magnético a sua volta, mas não geram um campo magnético por si próprios (FANG et al., 2011). Esses materiais podem ser anisotrópicos, apresentando uma variação de suas propriedades magnéticas de acordo com a direção. Neste caso existem direções de mais fácil magnetização e direções de mais difícil magnetização do material, fazendo com que o campo magnético nas redondezas do material seja amplificado ou reduzido de maneira diferente para cada direção do campo magnético aplicado sobre ele. Com a presença destes materiais nas proximidades de um magnetômetro, as medidas efetuadas são distorcidas apresentando intensidade maior em certas direções para o mesmo valor de campo magnético. Portanto, a distorção soft-iron se deve à interação de materiais ferromagnéticos na plataforma do sensor com campos externos e adiciona valores que variam com a orientação do sensor escalando e girando a esfera de medições em um elipsoide inclinado (ZHANG; YANG, 2015). Este efeito está ilustrado na Figura 9. 22 2.1.1.5 Não ortogonalidade A não ortogonalidade se dá devido ao desalinhamento dos eixos, que não formam um ângulo reto entre si. Alternativamente, erros na estrutura do material transdutor o fazem ter uma sensibilidade a campos de outros eixos, gerando o mesmo efeito do desalinhamento dos eixos nas medições (CAMPS; HARASSE; MONIN, 2009). Assim como a distorção soft-iron, este efeito transforma a esfera de medições em um elipsoide inclinado como ilustrado na Figura 9. Figura 9 - Nuvem de pontos sem erros à esquerda e nuvem de pontos com erros de escala e rotação à direita. Fonte: Produção do próprio autor. 2.1.2 Compensação dos erros Todos estes erros podem ser modelados conforme apresentado na equação (1), que é utilizada por vários autores da literatura (HEMERLY; COELHO, 2014; RENAUDIN; AFZAL; LACHAPELLE, 2010; VASCONCELOS et al., 2011) como modelo das medições de um magnetômetro triaxial. O vetor �⃗⃗� representa a medição de um magnetômetro e o vetor 𝑚𝑟⃗⃗ ⃗⃗ ⃗ representa o valor real do campo magnético que seria medido por um magnetômetro ideal. A matriz 𝐸 representa o erro de sensibilidade, a distorção soft-iron e a não ortogonalidade em conjunto e o vetor 𝑒 representa o offset e a distorção hard-iron. Portanto, esta equação modela todos os erros lineares que afetam as medidas do magnetômetro. O vetor �⃗� é o ruído aleatório presente nas medições, que por ser considerado aleatório não é compensado pelo método do ajuste de elipsoide. �⃗⃗� = 𝐸 · 𝑚𝑟⃗⃗ ⃗⃗ ⃗ + 𝑒 + �⃗� (1) 23 A calibração consiste em minimizar os efeitos da matriz 𝐸 e do vetor 𝑒 sobre as medições. Para isso, são necessários uma matriz 𝐶 que, idealmente, é a inversa da matriz de erro, e um vetor 𝑜 , que deve compensar os efeitos do vetor de erro formando a equação (2). 𝑚𝑐⃗⃗ ⃗⃗ ⃗ = 𝐶 · �⃗⃗� + 𝑜 (2) Sendo 𝑚𝑐⃗⃗ ⃗⃗ ⃗ a medição calibrada. Substituindo-se (1) em (2) a equação (3) é obtida. 𝑚𝑐⃗⃗ ⃗⃗ ⃗ = 𝐶𝐸 · 𝑚𝑟⃗⃗ ⃗⃗ ⃗ + 𝐶𝑒 + 𝑜 + 𝐶�⃗� (3) Para a calibração ideal 𝐶𝐸 = 𝐼, sendo 𝐼 a matriz identidade, e 𝑜 = −𝐶𝑒 . Portanto, a calibração ideal é realizada para se obter a equação (4). 𝑚𝑐⃗⃗ ⃗⃗ ⃗ = 𝑚𝑟⃗⃗ ⃗⃗ ⃗ + 𝐶�⃗� (4) Assim, o valor da medição depois da calibração 𝑚𝑐⃗⃗ ⃗⃗ ⃗ está mais próximo ao valor real do campo magnético 𝑚𝑟⃗⃗ ⃗⃗ ⃗ que o valor da medição sem calibração �⃗⃗� . O ruído em cada eixo pode aumentar devido à multiplicação com a matriz 𝐶, porém, como ela multiplica tanto o campo magnético �⃗⃗� , quanto o ruído �⃗� , a relação sinal/ruído é a mesma antes e depois da calibração. Essa mesma formulação é utilizada em vários métodos de calibração de magnetômetros propostos na literatura (HEMERLY; COELHO, 2014; RENAUDIN; AFZAL; LACHAPELLE, 2010; VASCONCELOS et al., 2011). A aplicação da equação (2), para obter as medições calibradas de um magnetômetro, exige antes que os parâmetros de calibração 𝐶 e 𝑜 sejam obtidos. Isso pode ser feito estimando a matriz 𝐸 e o vetor 𝑒 de um determinado magnetômetro em um determinado local, e com isso, calculando os valores dos parâmetros de calibração. É aí que entra o método do ajuste de elipsoide, que com uma nuvem de pontos obtida consegue calcular o elipsoide que melhor se encaixa nos pontos. Com o resultado do ajuste é possível calcular os parâmetros de calibração a partir do elipsoide obtido. 2.1.3 A métrica de distância algébrica no ajuste de elipsoides Para aplicar um método de minimização de erro no ajuste de elipsoides é necessária a utilização de alguma métrica para ser minimizada. As métricas mais utilizadas para este tipo de ajuste são a distância euclidiana e a distância algébrica. A equação da distância algébrica de uma quádrica com coeficientes de 𝑎 até 𝑗 ao ponto de coordenadas (𝑥1, 𝑦1, 𝑧1) é dada por (5). 24 𝐷 = 𝑎𝑥1 2 + 𝑏𝑦1 2 + 𝑐𝑧1 2 + 𝑑𝑥1𝑦1 + 𝑒𝑥1𝑧1 + 𝑓𝑦1𝑧1 + 𝑔𝑥1 + ℎ𝑦1 + 𝑖𝑧1 + 𝑗 (5) Se a distância algébrica for zero a equação se torna a equação da quádrica. Isso garante que todos os pontos com 𝐷 = 0 da quádrica se encontram em sua superfície. Assim como a distância euclidiana, a distância algébrica é invariante a rotação e translação. Isso significa que se a quádrica e o ponto sofrerem uma rotação, translação ou uma combinação destes a distância entre os dois permanecerá constante. Devido a esta propriedade, apenas o caso em que um elipsoide está centrado na origem e alinhado com os eixos precisa ser analisado, já que todos os outros casos serão uma rotação e translação deste. Para este caso mais simples, a equação da distância algébrica é dada a seguir em (6). 𝐷 = 𝑥2 𝑟𝑥2 + 𝑦2 𝑟𝑦2 + 𝑧2 𝑟𝑧2 − 1 (6) Manipulando esta equação percebe-se que o conjunto de pontos com distância algébrica 𝑑 de um elipsoide forma outro elipsoide, conforme apresentado em (7). 𝑥2 (𝑑 + 1)𝑟𝑥2 + 𝑦2 (𝑑 + 1)𝑟𝑦2 + 𝑧2 (𝑑 + 1)𝑟𝑧2 − 1 = 0 (7) Este elipsoide possui exatamente as mesmas proporções do elipsoide original, porém todos os seus eixos estão escalados pelo mesmo valor de √𝑑 + 1. A Figura 10 apresentada a seguir ilustra bem a distância algébrica para o caso de uma elipse. Os pontos 𝑝1 e 𝑝2 possuem a mesma distância algébrica da elipse em vermelho (a mais interna), porém o ponto 𝑝2 tem o dobro da distância euclidiana até a elipse (𝑑𝑒) do que o ponto 𝑝1. A proporção entre as distâncias dos pontos é exatamente a mesma proporção dos eixos da elipse de dois para um. Para elipsoides este efeito é o mesmo, exceto pelo fato de que eles terão uma dimensão a mais com outra proporcionalidade nesta direção. Os pontos sobre a elipse têm tanto distância euclidiana, quanto distância algébrica iguais a zero. 25 Figura 10 - Curvas de mesma distância algébrica. Fonte: Produção do próprio autor. Para a calibração de magnetômetros, o elipsoide apresenta um eixo mais alongado devido a fenômenos como o erro de sensibilidade e a distorção soft-iron e ambos também afetam os erros de medição. Portanto, um ponto na direção de um eixo maior tende a possuir um erro maior reduzindo sua confiabilidade. Uma parte do erro não é aumentada de acordo com a direção, como por exemplo o erro de quantificação do campo magnético, mas em geral os pontos tendem a apresentar maior variância na direção onde o elipsoide é maior. A componente da distância de um ponto a um elipsoide que está alinhada ao maior eixo contribui menos com a distância algébrica que a componente alinhada com o menor eixo justamente na proporção aproximada do erro nestes eixos. Este fato representa uma vantagem da distância algébrica em relação a distância euclidiana, onde as curvas obtidas, minimizando o erro quadrático de distância algébrica, estarão menos suscetíveis a esta diferença de variância nos eixos. Quando a distância entre o ponto e o elipsoide é muito menor que as dimensões do elipsoide, a distância algébrica se torna aproximadamente proporcional à distância euclidiana. Já quando ela é muito maior que as dimensões do elipsoide, a distância algébrica tende a ser proporcional ao quadrado da distância euclidiana. Estas características são percebidas quando um ponto se afasta do elipsoide em uma única direção, já que a proporcionalidade muda com 26 a direção como mencionado no parágrafo anterior. A Tabela 1 apresenta valores de distância euclidiana e distância algébrica ao longo de um eixo de um elipsoide com raio 𝑟 = 1 neste eixo. Tabela 1 - Comparação entre a distância euclidiana e a algébrica entre um ponto e a superfície de um elipsoide. Ponto Distância euclidiana Distância algébrica (1; 0; 0) 0 0 (1,01; 0; 0) 0,01 0,0201 (1,02; 0; 0) 0,02 0,0404 (1,1; 0; 0) 0,1 0,21 (1,2; 0; 0) 0,2 0,44 (100; 0; 0) 99 9999 (200; 0; 0) 199 39999 Fonte: Produção do próprio autor. A equação da distância euclidiana entre um elipsoide e um ponto é complexa e de difícil manipulação, fazendo com que a maioria dos algoritmos atuais que utilizam esta métrica sejam baseados em métodos iterativos. A formulação mais simples da distância algébrica tende a tornar mais fácil a resolução direta das equações para se ajustar o elipsoide aos pontos. 2.2 Algoritmos de calibração de acelerômetros e giroscópios Existem várias técnicas de calibração para acelerômetros e giroscópios. Diferente dos magnetômetros, as condições de uso dos acelerômetros MEMs não variam tanto, por isso eles podem ficar sem calibração até mesmo por alguns anos. O método do elipsoide, utilizado para calibração magnetômetros, também pode ser utilizado para acelerômetros. No entanto, em geral a calibração destes sensores é feita na fábrica, só sendo necessária uma calibração nova depois de meses de uso do sensor. Os giroscópios MEMs são mais estáveis que os magnetômetros no que se refere à calibração, mas possuem um erro de offset considerável. A utilização de um método de calibração simples para a compensação do offset no giroscópio já é suficiente para obter bons resultados. A calibração dos magnetômetros é a mais crítica e a mais complexa, requerendo mais atenção que os outros sensores (FRADEN, 2010). 27 2.3 Medições dos giroscópios Quando um giroscópio micro-eletro-mecânico realiza uma medição, sua saída consiste em um vetor de três dimensões onde cada dimensão representa a velocidade angular do objeto em torno daquele eixo (XIA; YU; KONG, 2014). Por serem três valores, esta forma pode ser confundida com os ângulos de Euler, mas trata-se de uma representação diferente. Normalmente, a unidade de saída é em radianos por segundo (rad/s) e, neste caso, uma saída 𝒈 = (2𝜋, 0, 0) do giroscópio representaria que o objeto está girando em torno do eixo x a uma velocidade de uma volta por segundo. Como nesse trabalho será usada a representação de rotação por quatérnios, será necessário realizar a conversão das medições do giroscópio para os quatérnios. Uma forma de se converter é pelo cálculo da velocidade angular total e o vetor de rotação que podem ser calculados pelas medidas do giroscópio 𝑔𝑥, 𝑔𝑦 e 𝑔𝑧 na forma das equações (8) e (9), respectivamente. 𝜔 = √𝑔𝑥 2 + 𝑔𝑦 2 + 𝑔𝑧 2 (8) 𝑎 = ( 𝑔𝑥 𝜔 , 𝑔𝑦 𝜔 , 𝑔𝑧 𝜔 ) (9) O ângulo de rotação θ ao redor do vetor 𝑎 pode ser calculado dividindo-se a velocidade angular pela frequência de amostragem do sensor como em (10). 𝜃 = 𝜔 𝑓 (10) Com esses valores já é possível obter o quatérnio que representa a rotação realizada pelo objeto, medida pelo giroscópio como uma velocidade angular, utilizando a equação (11). 𝑞𝑥 = 𝑎𝑥𝑠𝑒𝑛(𝜃/2) 𝑞𝑦 = 𝑎𝑦𝑠𝑒𝑛(𝜃/2) 𝑞𝑧 = 𝑎𝑧𝑠𝑒𝑛(𝜃/2) 𝑞𝑤 = 𝑐𝑜𝑠(𝜃/2) (11) Se várias medições forem realizadas pelo giroscópio e todas elas forem convertidas para a forma de quatérnio, basta multiplicar todos esses quatérnios que o resultado dará o quatérnio de orientação do objeto em relação a orientação inicial. Observe que o custo computacional de uma multiplicação de dois quatérnios deve-se apenas à 8 multiplicações de 28 ponto flutuante (RAO, 2006), sendo menor que custo da multiplicação nas matrizes de orientação. 2.4 O filtro de Kalman para fusão de dados em sensores inerciais Depois de efetuada a calibração do magnetômetro, do acelerômetro e do giroscópio e convertidos os dados de saída para sua forma adequada, as informações destes sensores podem ser fundidas com um filtro de Kalman para formar uma IMU. A seção seguinte apresenta o equacionamento da fusão de sensores unidimensionais com ruído gaussiano que servirá de base para a apresentação do filtro de Kalman na próxima seção. 2.4.1 Combinação de gaussianas A função de densidade de probabilidade de uma variável aleatória gaussiana é dada pela equação (12). 𝑓(𝑥, µ, 𝜎) = 1 √2𝜋𝜎2 · 𝑒 − (𝑥−µ)2 2𝜎2 (12) Sendo 𝑥 uma variável aleatória representando a medição de um sensor unidimensional, µ a média e σ o desvio-padrão dessas medições. A combinação da informação redundante contida em duas variáveis aleatórias gaussianas é então dada pela equação (13), com a multiplicação das duas funções seguida de normalização. 𝑐𝑜𝑚𝑏(𝑥) = 𝑓(𝑥, µ1, 𝜎1) · 𝑓(𝑥, µ2, 𝜎2) ∫ 𝑓(𝑥, µ1, 𝜎1) · 𝑓(𝑥, µ2, 𝜎2)𝑑𝑥 ∞ −∞ (13) Separando o termo do numerador, e realizando a multiplicação, a equação (14) é obtida. 𝑛𝑢𝑚(𝑥, µ, 𝜎) = 𝑓(𝑥, µ1, 𝜎1) · 𝑓(𝑥, µ2, 𝜎2) 𝑛𝑢𝑚(𝑥, µ, 𝜎) = 1 √2𝜋𝜎1 2 · 𝑒 − (𝑥−µ1) 2 2𝜎1 2 · 1 √2𝜋𝜎2 2 · 𝑒 − (𝑥−µ2) 2 2𝜎2 2 𝑛𝑢𝑚(𝑥, µ, 𝜎) = 1 √2𝜋𝜎1 2 · 𝑒 − 𝑥2−2𝑥µ1+µ1 2 2𝜎1 2 · 1 √2𝜋𝜎2 2 · 𝑒 − 𝑥2−2𝑥µ2+µ2 2 2𝜎2 2 𝑛𝑢𝑚(𝑥, µ, 𝜎) = 1 2𝜋√𝜎1 2𝜎2 2 · 𝑒 −( 𝑥2−2𝑥µ1+µ1 2 2𝜎1 2 + 𝑥2−2𝑥µ2+µ2 2 2𝜎2 2 ) (14) 29 Isolando o termo do expoente tem-se: 𝑏 = 𝑥2 − 2𝑥µ1 + µ1 2 2𝜎1 2 + 𝑥2 − 2𝑥µ2 + µ2 2 2𝜎2 2 𝑏 = (𝜎1 2 + 𝜎2 2)𝑥2 − 2(µ1𝜎2 2 + µ2𝜎1 2)𝑥 + µ1 2𝜎2 2 + µ2 2𝜎1 2 2𝜎1 2𝜎2 2 Dividindo numerador e denominador por (𝜎1 2 + 𝜎2 2) a equação (15) é obtida. 𝑏 = 𝑥2 − 2 µ1𝜎2 2 + µ2𝜎1 2 𝜎1 2 + 𝜎2 2 𝑥 + µ1 2𝜎2 2 + µ2 2𝜎1 2 𝜎1 2 + 𝜎2 2 2 𝜎1 2𝜎2 2 𝜎1 2 + 𝜎2 2 (15) Como 𝑏 é um polinômio em 𝑥 tem-se que o numerador n𝑢𝑚(𝑥, µ, 𝜎) é proporcional a uma gaussiana. Utilizando a técnica de completar quadrados podemos transformar o numerador de 𝑏 em um quadrado perfeito somado com um excedente como na equação (16). 𝑏 = (𝑥 − µ1𝜎2 2 + µ2𝜎1 2 𝜎1 2 + 𝜎2 2 ) 2 2 𝜎1 2𝜎2 2 𝜎1 2 + 𝜎2 2 + (µ1 − µ2) 2 2(𝜎1 2 + 𝜎2 2) (16) Finalmente, o numerador da equação (13) com as manipulações no expoente, resulta na equação (17). 𝑛𝑢𝑚(𝑥, µ, 𝜎) = 1 2𝜋√𝜎1 2𝜎2 2 · 𝑒 − (𝑥− µ1𝜎2 2+µ2𝜎1 2 𝜎1 2+𝜎2 2 ) 2 2 𝜎1 2𝜎2 2 𝜎1 2+𝜎2 2 · 𝑒 − (µ1−µ2) 2 2(𝜎1 2+𝜎2 2) (17) Agora, isolando-se o denominador da equação (13) e realizando as operações necessárias, a equação (18) é obtida com os seguintes passos: 𝑑𝑒𝑛(𝑥, µ, 𝜎) = ∫ 1 2𝜋√𝜎1 2𝜎2 2 · 𝑒 −( 𝑥2−2𝑥µ1+µ1 2 2𝜎1 2 + 𝑥2−2𝑥µ2+µ2 2 2𝜎2 2 ) 𝑑𝑥 ∞ −∞ 𝑑𝑒𝑛(𝑥, µ, 𝜎) = 1 2𝜋√𝜎1 2𝜎2 2 · ∫ 𝑒 −( 𝑥2−2𝑥µ1+µ1 2 2𝜎1 2 + 𝑥2−2𝑥µ2+µ2 2 2𝜎2 2 ) 𝑑𝑥 ∞ −∞ 𝑑𝑒𝑛(𝑥, µ, 𝜎) = 1 2𝜋√𝜎1 2𝜎2 2 · √𝜋√2 𝜎1 2𝜎2 2 𝜎1 2 + 𝜎2 2 · 𝑒 − (µ1−µ2) 2 2(𝜎1 2+𝜎2 2) 30 𝑑𝑒𝑛(𝑥, µ, 𝜎) = 1 √2𝜋√𝜎1 2 + 𝜎2 2 · 𝑒 − (µ1−µ2) 2 2(𝜎1 2+𝜎2 2) (18) Dividindo o numerador 𝑛𝑢𝑚(𝑥, µ, 𝜎) pelo denominador 𝑑𝑒𝑛(𝑥, µ, 𝜎), a função 𝑐𝑜𝑚𝑏(𝑥) pode ser reescrita como uma gaussiana dada pela equação (19). 𝑐𝑜𝑚𝑏(𝑥) = √𝜎1 2 + 𝜎2 2 √2𝜋√𝜎1 2𝜎2 2 · 𝑒 − (𝑥− µ1𝜎2 2+µ2𝜎1 2 𝜎1 2+𝜎2 2 ) 2 2 𝜎1 2𝜎2 2 𝜎1 2+𝜎2 2 (19) A média e a variância desta nova função são dadas pelas equações (20) e (21) a seguir. µ𝑚 = µ1𝜎2 2 + µ2𝜎1 2 𝜎1 2 + 𝜎2 2 (20) 𝜎𝑚 2 = 𝜎1 2𝜎2 2 𝜎1 2 + 𝜎2 2 (21) E finalmente, a função obtida pela fusão de duas gaussianas pode ser reescrita como uma gaussiana como na equação (22). 𝑐𝑜𝑚𝑏(𝑥) = 1 √2𝜋𝜎𝑚2 · 𝑒 − (𝑥−µ𝑚) 2 2𝜎𝑚 2 (22) Em geral, se uma função for multiplicada por outra função analítica o resultado tende a ser uma função mais complexa, como acontece na multiplicação de polinômios ou de funções senoidais. Porém, como mostrado na equação (19), a combinação de duas gaussianas (multiplicação seguida de normalização) também resulta em uma gaussiana. Isso é uma grande vantagem para os algoritmos de fusão baseados em gaussianas, já que várias delas podem ser multiplicadas sucessivamente sem aumentar a complexidade da função final. Também é interessante notar que a equação (20) representa uma média ponderada e a equação (21) uma média harmônica dividida por dois. Assim como uma combinação de resistores em paralelo, a aplicação da equação (21) em dois valores positivos gera sempre um valor menor que qualquer um dos dois valores iniciais. Como uma variância menor representa uma menor incerteza, a combinação de duas gaussianas gera uma função com menor variância e, portanto, com menos incerteza que qualquer uma das duas gaussianas iniciais. A Figura 11 ilustra as características mencionadas. 31 Figura 11 – Combinação de duas gaussianas. Fonte: produção do próprio autor. 2.4.2 O equacionamento do filtro de Kalman O filtro de Kalman é um método matemático utilizado quando um sistema dinâmico possui uma informação incerta em que é possível fazer uma previsão sobre como ela se comportará no futuro (CASTANEDO, 2013), podendo o filtro ser de tempo contínuo ou de tempo discreto. Neste trabalho, como os sensores são digitais e possuem uma taxa de amostragem, é utilizado o filtro de Kalman de tempo discreto que será explicado nesta seção. Para entender o filtro de Kalman, é interessante começar por uma situação simples. Supondo um sistema onde o ângulo da articulação de um robô deve ser medido e existem dois sensores, um capaz de medir o ângulo da articulação e o outro a velocidade angular que este ângulo se encontra no momento. Admite-se também que estes sensores possuem a mesma taxa de amostragem. Assim, uma medição do conjunto de sensores resultará em um vetor 𝑣 representado na equação (23). 𝑣 = ( 𝜃 𝜔 ) (23) 32 Como os sensores estão sujeitos a erros que são representados por uma gaussiana, o vetor 𝑣 de medição estará associado a uma matriz de variâncias-covariâncias como na equação (24). 𝑃 = ( 𝛴𝜃𝜃 𝛴𝜃𝜔 𝛴𝜔𝜃 𝛴𝜔𝜔 ) (24) Já que a velocidade angular é conhecida, é possível fazer um palpite sobre como estarão essas medidas depois de um intervalo de tempo ∆𝑡 correspondente ao período de amostragem dos sensores. Devido ao momento de inércia do sistema, é provável que a velocidade angular continue a mesma, ou pelo menos em um valor próximo já que para uma mudança grande neste valor exigiria uma aceleração angular grande e a potência disponível para executar essa aceleração é, normalmente, limitada. O ângulo, por outro lado, provavelmente será próximo ao ângulo anterior mais a variação dada pela velocidade angular do sistema. Sendo assim, um palpite razoável do estado atual pode ser feito pelo estado anterior como apresentado na equação (25). A matriz 𝐹𝑘 dada na equação (26) poder ser chamada de matriz de predição. 𝑣𝑘 = 𝐹𝑘 · 𝑣𝑘−1 (25) 𝐹𝑘 = ( 1 ∆𝑡 0 1 ) (26) Sendo 𝑘 a unidade de tempo discreto. O valor da matriz de covariâncias também pode ser previsto como na equação (27) (KALMAN, 1960). 𝑃𝑘 = 𝐹𝑘𝑃𝑘−1𝐹𝑘 𝑇 (27) Porém, um motor na articulação do robô pode gerar uma aceleração angular mudando o ângulo e a velocidade angular do próximo estado. Esta mudança no sistema pode ser considerada adicionando-se uma componente 𝐵𝑘𝑢𝑘⃗⃗⃗⃗ , onde 𝐵𝑘 é a matriz de controle e 𝑢𝑘⃗⃗⃗⃗ o vetor de controle. Ao mesmo tempo, essa previsão não é perfeita e essa imperfeição pode ser modelada como uma matriz de ruído 𝑄𝑘 adicionada na matriz de covariâncias. Com essas considerações, a expressão completa da etapa de predição é dada pela equação (28). 𝑣𝑘 = 𝐹𝑘𝑣𝑘−1 + 𝐵𝑘𝑢𝑘⃗⃗⃗⃗ 𝑃𝑘 = 𝐹𝑘𝑃𝑘−1𝐹𝑘 𝑇 + 𝑄𝑘 (28) Com essas equações, agora existem duas fontes de informação sobre o estado atual do sistema: a predição realizada usando o estado passado e a medição atual dos sensores. As duas fontes de informação são representadas por um vetor de variáveis aleatórias gaussianas 33 e, como visto na seção anterior, a fusão da informação redundante de duas variáveis aleatórias gaussianas pode ser feita com as equações (20) e (21) obtendo-se uma única variável com menor erro. Como o filtro de Kalman é iterativo, é mais interessante manipular as equações (20) e (21) para ficarem na forma incremental como a seguir nas equações (29) e (30). 𝜇′ = µ1 + 𝜎1 2(µ2 − µ1) 𝜎1 2 + 𝜎2 2 (29) 𝜎′2 = 𝜎1 2 − 𝜎1 4 𝜎1 2 + 𝜎2 2 (30) Considerando µ1 e 𝜎1 2 como a média e variância da variável gaussiana predita, µ2 e 𝜎2 2 como a média e variância da variável gaussiana medida pelos sensores e usando a variável 𝑞 da equação (31) para simplificar as expressões, a equação (32) representa a fusão do valor predito e do medido. 𝑞 = 𝜎1 2 𝜎1 2 + 𝜎2 2 (31) 𝜇′ = µ1 + 𝑞 · (µ2 − µ1) 𝜎′2 = 𝜎1 2 − 𝑞𝜎1 2 (32) Como no caso considerado os valores são vetores de variáveis aleatórias, e não variáveis aleatórias escalares, as expressões na equação (32) podem ser estendidas para a forma matricial como nas equações (33) e (34) a seguir, sendo 𝛴1 e 𝛴2 as matrizes de covariância da etapa de predição e da medição dos sensores, respectivamente. Q = 𝛴1(𝛴1 + 𝛴2) −1 (33) 𝜇′⃗⃗ ⃗ = µ1⃗⃗⃗⃗ + Q · (µ2⃗⃗⃗⃗ − µ1⃗⃗⃗⃗ ) 𝛴′ = 𝛴1 − Q𝛴1 (34) Portanto, o filtro de Kalman se trata de uma fusão de informação entre o valor predito e o valor realmente medido em um instante 𝑘. Como para realizar essa predição só são necessárias as matrizes apresentadas, apenas essas matrizes precisam ser armazenadas, e não todos os valores passados já obtidos. Em (KALMAN, 1960) é mostrado que aplicando-se sucessivamente os passo descritos o valor final da estimativa converge e que a estimativa obtida pelo filtro é ótima. É importante observar que a estimativa obtida pelo filtro de Kalman só é ótima caso todas as variáveis aleatórias consideradas tenham distribuição normal. 34 2.5 Considerações finais Neste capítulo foram apresentados os principais conceitos utilizados no desenvolvimento deste trabalho. Foram explicados a natureza dos erros dos magnetômetros e a modelagem utilizada para equacionar estes erros, bem como o ajuste de elipsoide e a métrica de distância algébrica. Em seguida, foi mostrado como converter as medições de um giroscópio MEMS para a forma de quatérnios de orientação. Também foi exposto o embasamento teórico para a realização da fusão de dados necessária em uma IMU utilizando o filtro de Kalman. O capítulo a seguir desenvolve o equacionamento necessário para a implementação do algoritmo de calibração de magnetômetros proposto neste trabalho. Além disso, apresenta argumentos teóricos a favor da utilização deste algoritmo. 35 3 Materiais e métodos 3.1 Materiais O sistema desenvolvido neste trabalho se trata de um conjunto de algoritmos de calibração, filtragem de dados e fusão de sensores escritos em linguagem de programação. Todo o código foi feito em duas versões: uma para a realização de simulações computacionais e uma para a utilização em sistemas embarcados. Na primeira versão do sistema todos algoritmos foram escritos na linguagem de programação MATLAB utilizando o ambiente de programação da MathWorks MATLAB R2016a. Tanto essa linguagem quanto o ambiente são ideais para simulações computacionais com os algoritmos separadamente ou com todo o sistema, devido a sua facilidade de uso e a sua imensa biblioteca de funções disponíveis. Na segunda versão do sistema, todo o código fonte em MATLAB foi reescrito na linguagem de programação C. Para isso, foi utilizada a plataforma mbed, que consiste em um conjunto de bibliotecas que contêm os métodos para utilização dos sensores e outros periféricos, assim como várias funções auxiliares. Esta plataforma é específica para os processadores ARM de 32-bits da família Cortex-M, portanto o código foi desenvolvido apenas para utilização com esta família de processadores. Para os testes da segunda versão do sistema, foi utilizada a plataforma da Freescale FRDM-K64F em conjunto com a expansão FRDM-FXS-MULTI-B que possui acelerômetros triaxiais, magnetômetros triaxiais e um giroscópio triaxial. Esta plataforma possui um processador ARM Cortex-M4 de 32 bits, com instruções específicas para processamento digital de sinais e uma unidade de ponto flutuante. Os testes experimentais foram comparados com os resultados da biblioteca própria da Freescale para fusão de sensores (FSFLK) que é compatível com a mesma plataforma FRDM- K64F. O datasheet da biblioteca (“Freescale Sensor Fusion Library for Kinetis MCUs”, 2015) descreve um método experimental para a verificação quantitativa dos erros presentes na medição calibrada, assim como apresenta os valores de erros obtidos pela FSFLK. Portanto, este método foi utilizado para a comparação dos resultados dos dois sistemas. No restante deste Capítulo serão apresentados e explicados os métodos necessários para a implementação do sistema proposto neste trabalho. 36 3.2 Calibração do magnetômetro 3.2.1 O método de ajuste de quádrica proposto A equação (35) a seguir descreve a equação geral das quádricas. 𝑎𝑥2 + 𝑏𝑦2 + 𝑐𝑧2 + 𝑑𝑥𝑦 + 𝑒𝑥𝑧 + 𝑓𝑦𝑧 + 𝑔𝑥 + ℎ𝑦 + 𝑖𝑧 + 𝑗 = 0 (35) Se todos os coeficientes desta equação forem multiplicados por uma constante ela continua representando o mesmo conjunto de pontos e, portanto, a mesma quádrica. Para retirar este parâmetro livre e evitar a solução trivial que consiste em todos os parâmetros iguais a zero, a equação (35) deve ser normalizada. A normalização apresentada na equação (36) tem as vantagens de ser invariante a rotação e translação e evita que ocorram singularidades no espaço dos parâmetros. 𝑎 + 𝑏 + 𝑐 = 1 (36) A equação (36) multiplicada por 𝑧2 pode ser reunida com a equação (35) formando uma equação das quádricas normalizada representada em (37). 𝑎(𝑥2 − 𝑧2) + 𝑏(𝑦2 − 𝑧2) + 𝑧2 + 𝑑𝑥𝑦 + 𝑒𝑥𝑧 + 𝑓𝑦𝑧 + 𝑔𝑥 + ℎ𝑦 + 𝑖𝑧 + 𝑗 = 0 (37) A distância algébrica de um ponto (𝑥, 𝑦, 𝑧) até esta quádrica é dada pela equação (38). 𝐷 = 𝑎(𝑥2 − 𝑧2) + 𝑏(𝑦2 − 𝑧2) + 𝑧2 + 𝑑𝑥𝑦 + 𝑒𝑥𝑧 + 𝑓𝑦𝑧 + 𝑔𝑥 + ℎ𝑦 + 𝑖𝑧 + 𝑗 (38) Sendo assim, dado um conjunto de 𝑁 pontos, o erro quadrático de distância algébrica entre esses pontos e uma quádrica qualquer normalizada é dado pela equação (39). Є = ∑(𝑎(𝑥𝑖 2 − 𝑧𝑖 2) + 𝑏(𝑦𝑖 2 − 𝑧𝑖 2) + 𝑧𝑖 2 + 𝑑𝑥𝑖𝑦𝑖 + 𝑒𝑥𝑖𝑧𝑖 + 𝑓𝑦𝑖𝑧𝑖 + 𝑔𝑥𝑖 + ℎ𝑦𝑖 + 𝑖𝑧𝑖 + 𝑗) 2 𝑁 𝑖=1 (39) O objetivo do algoritmo de calibração do magnetômetro é encontrar a quádrica que tem o menor erro quadrático de distância algébrica entre um dado conjunto de pontos. Portanto, os coeficientes 𝑎, 𝑏, 𝑑, 𝑒, 𝑓, 𝑔, ℎ, 𝑖 e 𝑗 que resultam no erro quadrático mínimo devem ser encontrados. Para calcular a derivada parcial do erro quadrático em relação ao coeficiente 𝑎, podemos isolar todos os termos que não dependem de 𝑎 como na equação (40). 𝐺 = 𝑏(𝑦𝑖 2 − 𝑧𝑖 2) + 𝑧𝑖 2 + 𝑑𝑥𝑖𝑦𝑖 + 𝑒𝑥𝑖𝑧𝑖 + 𝑓𝑦𝑖𝑧𝑖 + 𝑔𝑥𝑖 + ℎ𝑦𝑖 + 𝑖𝑧𝑖 + 𝑗 (40) Substituindo a equação (40) na equação (39) o resultado é a equação (41). 37 Є = ∑(𝑎(𝑥𝑖 2 − 𝑧𝑖 2) + 𝐺)2 𝑁 𝑖=1 (41) Resolvendo o binômio de Newton a equação (42) é obtida. Є = ∑𝑎2(𝑥𝑖 2 − 𝑧𝑖 2)2 + 2𝑎(𝑥𝑖 2 − 𝑧𝑖 2) · 𝐺 + 𝐺2 𝑁 𝑖=1 (42) A derivada parcial do erro quadrático em relação ao coeficiente 𝑎 pode ser calculada facilmente a partir da equação (42) resultando na equação (43). 𝑑Є 𝑑𝑎 = ∑2𝑎(𝑥𝑖 2 − 𝑧𝑖 2)2 𝑁 𝑖=1 + 2(𝑥𝑖 2 − 𝑧𝑖 2) · 𝐺 (43) A expressão completa pode ser obtida substituindo o valor de 𝐺 na equação (43). A mesma lógica pode ser utilizada para calcular as derivadas parciais em relação a cada um dos 9 coeficientes obtendo-se o conjunto de equações (44). 38 Como no conjunto de equações (44) os coeficientes são invariantes aos somatórios, a propriedade distributiva pode ser aplicada em cada equação para se encontrar os termos relativos a cada coeficiente isoladamente sendo no total 81 termos. O conjunto de equações (45) mostra alguns termos. O termo 𝑀11, por exemplo, é o termo da primeira equação relativo ao coeficiente 𝑎. 𝑀11 = ∑2(𝑥𝑖 2 − 𝑧𝑖 2)2 𝑁 𝑖=1 ⋯ 𝑀19 = ∑2(𝑥𝑖 2 − 𝑧𝑖 2) 𝑁 𝑖=1 𝑀21 = ∑2(𝑥𝑖 2 − 𝑧𝑖 2)(𝑦𝑖 2 − 𝑧𝑖 2) 𝑁 𝑖=1 ⋯ 𝑀29 = ∑2(𝑦𝑖 2 − 𝑧𝑖 2) 𝑁 𝑖=1 (45) ⋮ ⋱ ⋮ 𝑀91 = ∑2(𝑥𝑖 2 − 𝑧𝑖 2) 𝑁 𝑖=1 ⋯ 𝑀99 = ∑2(𝑥𝑖 2 − 𝑧𝑖 2) 𝑁 𝑖=1 Isso mostra que todas as equações são lineares em relação aos coeficientes da quádrica normalizada. Logo, se as derivadas parciais forem igualadas a zero e o sistema linear for resolvido, os coeficientes que resultam no erro quadrático mínimo serão encontrados. É importante observar que além dos termos dos coeficientes, as derivadas parciais também possuem termos livres que devem ser isolados no outro lado da igualdade, para obter o sistema linear. Alguns termos livres são mostrados na equação (46). 𝑤1 = −2 · (𝑥𝑖 2 − 𝑧𝑖 2) · 𝑧𝑖 2 𝑤2 = −2 · (𝑦𝑖 2 − 𝑧𝑖 2) · 𝑧𝑖 2 𝑤3 = −2 · 𝑥𝑖𝑦𝑖 · 𝑧𝑖 2 ⋮ 𝑤9 = −2 · 𝑧𝑖 2 (46) Portanto, os coeficientes da quádrica com menor erro quadrático de distância algébrica podem ser obtidos por meio da equação (47). [𝑎, 𝑏, 𝑑, 𝑒, 𝑓, 𝑔, ℎ, 𝑖, 𝑗]𝑇 = 𝑀−1 · �⃗⃗� (47) Sendo que a matriz 𝑀 é montada de acordo com o conjunto de equações (45). Assim, o valor do coeficiente 𝑐 pode ser calculado como na equação (48). 39 𝑐 = 1 − 𝑎 − 𝑏 (48) Com os coeficientes da quádrica em mãos os valores da matriz 𝐶 e do vetor 𝑜 podem ser calculados para realizar a calibração, o que será mostrado na próxima seção. 3.2.2 Calculando os parâmetros de calibração A equação (49) representa a forma matricial da equação das quádricas. (𝑥 𝑦 𝑧) ( 𝑎 𝑑 2 𝑒 2 𝑑 2 𝑏 𝑓 2 𝑒 2 𝑓 2 𝑐 ) ( 𝑥 𝑦 𝑧 ) + (𝑥 𝑦 𝑧) ( 𝑔 ℎ 𝑖 ) + 𝑗 = 0 (49) A matriz 3x3 desta equação é uma matriz real simétrica, e matrizes deste tipo possuem a propriedade de serem sempre diagonalizáveis. Além disso, é sempre possível obter uma matriz de autovetores ortogonal a partir de uma matriz real simétrica (BOLDRINI et al., 1986). Considerando esta matriz como 𝐴 conforme a equação (50), temos 𝐴 = ( 𝑎 𝑑 2 𝑒 2 𝑑 2 𝑏 𝑓 2 𝑒 2 𝑓 2 𝑐 ) (50) Com a matriz de autovetores ortogonal 𝑉 da matriz 𝐴 é possível calcular a forma diagonal 𝐷 da matriz 𝐴 como na equação (51). 𝐷 = 𝑉𝑇𝐴𝑉 = ( 𝜆𝑥 0 0 0 𝜆𝑦 0 0 0 𝜆𝑧 ) (51) Os autovalores da matriz A são inversamente proporcionais ao quadrado dos raios do elipsoide. Para que os três raios do elipsoide sejam iguais e ele se torne uma esfera, basta que os três autovalores na matriz diagonal sejam iguais, preferencialmente que a matriz seja a matriz identidade. A transformação da equação (52) transforma a matriz 𝐴 em uma matriz 𝐴′ cuja forma diagonal é a identidade. 𝐴′ = 𝑉 · √𝐷−1 · 𝑉𝑇 · 𝐴 · 𝑉 · √𝐷−1 · 𝑉𝑇 (52) 40 Para que a matriz 𝐴 sofra a transformação 𝐶−1 = 𝑞 · 𝑉 · √𝐷−1 · 𝑉𝑇, onde 𝑞 é um valor escalar arbitrário, basta substituir os pontos de entrada como na equação (53). Nesta equação 𝑥′, 𝑦′ e 𝑧′ são as coordenadas de um ponto depois da transformação e 𝑥, 𝑦 e 𝑧 são as coordenadas do ponto antes da transformação. ( 𝑥 𝑦 𝑧 ) = 𝑞 · 𝑉 · √𝐷−1 · 𝑉𝑇 · ( 𝑥′ 𝑦′ 𝑧′ ) (53) Aplicando-se essa transformação nos pontos de entrada, a forma obtida deve ser a de uma esfera alinhada com os eixos, porém o centro da esfera estará deslocado da origem. A equação resultante depois da transformação é mostrada na equação (54). (𝑥′ 𝑦′ 𝑧′)𝑞 𝑉√𝐷−1𝑉𝑇𝐴𝑉√𝐷−1𝑉𝑇𝑞 ( 𝑥′ 𝑦′ 𝑧′ ) + (𝑥′ 𝑦′ 𝑧′)𝑞𝑉√𝐷−1𝑉𝑇 ( 𝑔 ℎ 𝑖 ) + 𝑗 = 0(54) A equação da esfera alinhada com os eixos de raio unitário e deslocada da origem pode ser representada pela equação (55). (𝑥 − 𝑐𝑥) 2 + (𝑦 − 𝑐𝑦) 2 + (𝑧 − 𝑐𝑧) 2 = 1 (55) Expandindo os termos quadráticos a equação (56) é obtida. 𝑥2 + 𝑦2 + 𝑧2 − 2𝑐𝑥𝑥 − 2𝑐𝑦𝑦 − 2𝑐𝑧𝑧 + 𝑐𝑥 2 + 𝑐𝑦 2 + 𝑐𝑧2 − 1 = 0 (56) Como a equação (54) representa uma esfera alinhada com os eixos, de raio unitário e deslocada da origem sua forma expandida deve ser exatamente a da equação (56). Igualando as equações (54) e (56), como a matriz 𝑞𝑉√𝐷−1𝑉𝑇 · 𝐴 · 𝑉√𝐷−1𝑉𝑇𝑞 é a matriz identidade, os termos 𝑥2, 𝑦2 𝑒 𝑧2 podem ser cancelados dos dois lados e a equação (57) é obtida. (𝑥 𝑦 𝑧)𝑞𝑉√𝐷−1𝑉𝑇 ( 𝑔 ℎ 𝑖 ) + 𝑗 = (𝑥 𝑦 𝑧)( −2𝑐𝑥 −2𝑐𝑦 −2𝑐𝑧 ) + 𝑐𝑥 2 + 𝑐𝑦 2 + 𝑐𝑧2 − 1 (57) Da equação (57) pode-se concluir que a igualdade da equação (58) deve ser satisfeita. ( −2𝑐𝑥 −2𝑐𝑦 −2𝑐𝑧 ) = 𝑞𝑉√𝐷−1𝑉𝑇 ( 𝑔 ℎ 𝑖 ) (58) O centro da esfera de raio unitário obtida depois da transformação, pode, portanto, ser calculado pela equação (59). 41 ( 𝑐𝑥 𝑐𝑦 𝑐𝑧 ) = − 1 2 · 𝑞𝑉√𝐷−1𝑉𝑇 ( 𝑔 ℎ 𝑖 ) (59) Logo, para levar o centro desta esfera para a origem basta somar os pontos de entrada depois de aplicada a transformação 𝐶−1 = 𝑞 · 𝑉 · √𝐷−1 · 𝑉𝑇 com o vetor 𝑜 mostrado em (60). 𝑜 = −( 𝑐𝑥 𝑐𝑦 𝑐𝑧 ) = 1 2 𝐶−1 ( 𝑔 ℎ 𝑖 ) (60) A matriz 𝐶 e o vetor 𝑜 já conseguem transformar o elipsoide em uma esfera centrada na origem. Porém a esfera obtida não terá raio unitário, pois o termo livre 𝑗𝑐 da equação após a calibração não terá valor 1. Sendo assim, o coeficiente 𝑞 pode ser aproveitado para realizar a normalização da equação e obter um raio unitário. Para isso, basta obter os valores de 𝐶 e 𝑜 considerando 𝑞 = 1 e calcular o valor do termo livre que seria obtido como na equação (61). 𝑗𝑐 = 𝑜 𝑇 · 𝑜 + (𝑔 ℎ 𝑖) · 𝐶−1 · 𝑜 (61) Com o valor do termo livre na condição de 𝑞 = 1, o valor de 𝑞 pode ser escolhido de modo a tornar 𝑗𝑐 = 1. Para isso a equação (62) pode ser utilizada. 𝑞 = 1 √𝑗𝑐 (62) Portanto, as equações (63) e (64) podem ser utilizadas para calcular os parâmetros de calibração, para transformar o elipsoide em uma esfera de raio unitário e centrada na origem. 𝐶 = (𝑞 · 𝑉 · √𝐷−1 · 𝑉𝑇) −1 (63) 𝑜 = 1 2 · 𝑞 · 𝑉 · √𝐷−1 · 𝑉𝑇 · ( 𝑔 ℎ 𝑖 ) (64) Finalmente, a equação (65) é a equação necessária para calibrar os pontos de medição obtendo-se uma medição com uma quantidade de erros muito menor que a medição original. Essa equação utiliza a matriz da equação (63), que faz a nuvem de pontos ficar em forma esférica, e o vetor da equação (64), que leva o centro da nuvem de pontos para a origem. As variáveis 𝑥𝑐, 𝑦𝑐 e 𝑧𝑐 representam as coordenadas de um ponto de medição após a calibração. ( 𝑥𝑐 𝑦𝑐 𝑧𝑐 ) = 𝐶 ( 𝑥 𝑦 𝑧 ) + 𝑜 (65) 42 3.3 Argumentos teóricos 3.3.1 Comparação entre os resultados de algoritmos com e sem restrições O algoritmo proposto na seção anterior é bem simples e eficiente, mas na pesquisa bibliográfica feita para este trabalho não foi encontrado nenhum artigo ao menos citando a possibilidade de realização de um algoritmo deste tipo. Isso se deve ao fato de que o algoritmo obtém os coeficientes da equação geral das quádricas e a superfície obtida pode ser qualquer uma das quádricas e não apenas um elipsoide. As quádricas englobam 17 diferentes formas básicas e o método de ajuste de elipsoide requer que a forma obtida seja um elipsoide. A Figura 12 ilustra todas as formas reais e não degeneradas que podem ser representadas pela equação geral das quádricas (35). Figura 12-Algumas formas básicas representadas pela equação das quádricas. Fonte: Wolfram Alpha. 43 Devido a esse problema, os algoritmos atuais baseados em distância algébrica (e também os baseados em distância euclidiana) adicionam equações de restrição ao sistema para garantir que os coeficientes obtidos da quádrica sejam os de um elipsoide (CAMPS; HARASSE; MONIN, 2009; HEMERLY; COELHO, 2014; RENAUDIN; AFZAL; LACHAPELLE, 2010). Como essas restrições são equações não lineares, sua adição faz com que um sistema linear se transforme em um sistema não-linear cuja solução analítica é muito mais difícil e acaba sendo necessária a utilização de métodos iterativos ou soluções extremamente complexas das equações, ambas muito mais custosas computacionalmente que a resolução do sistema linear. O objetivo desta seção é mostrar que essas restrições não protegem o sistema quando a forma ótima obtida não é um elipsoide. Dada uma nuvem de pontos de medição, o sistema linear da equação (44) apresentado na seção 3.2.1 pode ser montado. Como qualquer outro sistema linear, sua solução pode ser possível e determinada, possível e indeterminada ou impossível. Primeiro, podemos supor que a solução do sistema é possível e determinada para depois analisar as outras possibilidades. A solução deste sistema tem todas as derivadas parciais do erro de distância algébrica iguais a zero, portanto, ela pode representar tanto um mínimo quanto um máximo da função de erro. No caso específico deste sistema, a solução será sempre um ponto de mínimo global, dada a natureza quadrática do erro, o que será provado posteriormente. Se o sistema for possível e determinado, a sua solução pode ser um elipsoide ou alguma outra quádrica. Se a solução do sistema for um elipsoide, então o resultado será o mesmo obtido pelos algoritmos que utilizam distância algébrica com as restrições de elipsoide. Neste caso, as restrições não entram em ação, pois a quádrica com menor erro já é um elipsoide. As diferenças encontradas entre os dois métodos (sem restrições ou com restrições) se dá apenas quando a quádrica encontrada pela resolução do sistema linear não é um elipsoide, sendo que os métodos com restrição irão obter sempre um elipsoide. O conjunto de todas superfícies representadas pela equação das quádricas normalizada (equação 37) pode ser visto como um espaço topológico de 9 dimensões, onde cada ponto deste espaço representa uma superfície e as 9 dimensões são os 9 coeficientes da equação (37). Com essas considerações, a solução do sistema linear da equação (44) será chamada aqui de ponto ótimo e a solução de qualquer algoritmo com restrições será chamada de ponto restrito (já que o ponto está restrito à região dos elipsoides no espaço topológico). 44 Se o ponto restrito e o ponto ótimo forem pontos diferentes, caso em que o primeiro será um elipsoide e o segundo outra quádrica qualquer, uma reta de 9 dimensões pode ser calculada de modo que ela passe pelos dois pontos. Para isso, pode ser utilizada a equação paramétrica das retas em 9 dimensões mostrada na equação (66). Os coeficientes 𝑟1, 𝑟2, … , 𝑟9 e 𝑠1, 𝑠2, … , 𝑠9 representam os coeficientes da equação paramétrica e cada valor de 𝑡 está associado a um ponto da reta. 𝑎 = 𝑟1𝑡 + 𝑠1 𝑏 = 𝑟2𝑡 + 𝑠2 𝑑 = 𝑟3𝑡 + 𝑠3 𝑒 = 𝑟4𝑡 + 𝑠4 𝑓 = 𝑟5𝑡 + 𝑠5 𝑔 = 𝑟6𝑡 + 𝑠6 ℎ = 𝑟7𝑡 + 𝑠7 𝑖 = 𝑟8𝑡 + 𝑠8 𝑗 = 𝑟9𝑡 + 𝑠9 (66) O parâmetro 𝑡 pode ser escolhido de modo que para 𝑡 = 0 o ponto ótimo seja obtido (𝑎1, 𝑏1, … , 𝑗1) e para 𝑡 = 1 o ponto restrito seja obtido (𝑎2, 𝑏2, … , 𝑗2). Nesse caso, os coeficientes da equação da reta podem ser calculados como na equação (67). 𝑠1 = 𝑎1 , 𝑟1 = 𝑎2 − 𝑎1 𝑠2 = 𝑏1 , 𝑟2 = 𝑏2 − 𝑏1 𝑠3 = 𝑑1 , 𝑟3 = 𝑑2 − 𝑑1 𝑠4 = 𝑒1 , 𝑟4 = 𝑒2 − 𝑒1 𝑠5 = 𝑓1 , 𝑟5 = 𝑓2 − 𝑓1 𝑠6 = 𝑔1 , 𝑟6 = 𝑔2 − 𝑔1 𝑠7 = ℎ1 , 𝑟7 = ℎ2 − ℎ1 𝑠8 = 𝑖1 , 𝑟8 = 𝑖2 − 𝑖1 𝑠9 = 𝑗1 , 𝑟9 = 𝑗2 − 𝑗1 (67) O erro quadrático para cada ponto desta reta pode ser obtido em função de 𝑡, substituindo os coeficientes da equação (39) pela sua representação na equação (66) resultando na equação (68). Є = ∑( (𝑟1𝑡 + 𝑠1)(𝑥𝑖 2 − 𝑧𝑖 2) + (𝑟2𝑡 + 𝑠2)(𝑦𝑖 2 − 𝑧𝑖 2) + 𝑧𝑖 2 + (𝑟3𝑡 + 𝑠3)𝑥𝑖𝑦𝑖 + (𝑟4𝑡 + 𝑠4)𝑥𝑖𝑧𝑖 + (𝑟5𝑡 + 𝑠5)𝑦𝑖𝑧𝑖 + (𝑟6𝑡 + 𝑠6)𝑥𝑖 + (𝑟7𝑡 + 𝑠7)𝑦𝑖 + (𝑟8𝑡 + 𝑠8)𝑧𝑖 + 𝑟9𝑡 + 𝑠9 ) 2 𝑁 𝑖=1 (68) Depois de expandir os termos dentro dos parênteses, todos os termos dependentes de 𝑡 podem ser agrupados na função 𝐺𝑖 e todos os termos independentes podem ser agrupados na função 𝐻𝑖 obtendo-se a equação (69). 45 Є = ∑( 𝐺𝑖𝑡 + 𝐻𝑖 ) 2 𝑁 𝑖=1 (69) Como 𝑡 é invariante ao somatório, com algumas manipulações a equação (70) é obtida. Є = ∑𝐺𝑖 2𝑡2 + 2𝐺𝑖𝐻𝑖𝑡 + 𝐻𝑖 2 𝑁 𝑖=1 Є = (∑𝐺𝑖 2 𝑁 𝑖=1 ) 𝑡2 + (∑2𝐺𝑖𝐻𝑖 𝑁 𝑖=1 ) 𝑡 + (∑𝐻𝑖 2 𝑁 𝑖=1 ) (70) A equação (70) mostra que o erro quadrático dos pontos da reta em função de 𝑡 forma uma parábola. O coeficiente quadrático da equação desta parábola é o somatório de vários valores reais elevados ao quadrado e, portanto, é um valor não negativo. Isso mostra que a parábola tem concavidade voltada para cima e logo tem um único mínimo global. Como no ponto ótimo as derivadas parciais em todas as dimensões são zero e a reta é na verdade uma combinação linear das dimensões, a derivada parcial do erro em relação a 𝑡 também deve ser zero no ponto ótimo. Portanto, o ponto ótimo se encontra no vértice desta parábola, sendo o menor valor possível, e o ponto restrito em algum outro local. Isso também pode ser provado de uma maneira alternativa manipulando as derivadas parciais da equação (44) igualadas a zero e a equação (69) para se obter a equação (71). (∑2𝐺𝑖𝐻𝑖 𝑁 𝑖=1 ) = 0 (71) A obtenção da equação (71) é muito extensa e não será mostrada aqui. Na verdade, se o ponto 1 (𝑎1, 𝑏1, … , 𝑗1) for o ponto ótimo, esta igualdade é obtida para qualquer ponto colocado no lugar do ponto 2 (𝑎2, 𝑏2, … , 𝑗2). O somatório também pode ser calculado facilmente por computador e, de fato, para qualquer instância do problema de ajuste de elipsoide o valor obtido será bem próximo a zero (devido aos erros causados pelas operações de ponto flutuante). Se o coeficiente linear da parábola é igual a zero, então o seu vértice está em 𝑡 = 0, justamente onde está o ponto ótimo, já que no vértice 𝑡 = −𝑏/2𝑎, sendo 𝑏 o coeficiente linear e 𝑎 o quadrático. A Figura 13 ilustra os pontos na parábola. 46 Figura 13 - Função do erro em relação a t. Fonte: Produzida pelo próprio autor. Esta etapa prova que sempre existe um caminho partindo de qualquer ponto, inclusive do ponto restrito, ao ponto ótimo (sem restrições) em que o erro sempre decresce. Como qualquer seção do erro em relação a uma das dimensões forma uma parábola, isso significa que nesse espaço, de erro quadrático de distância algébrica, existe apenas um mínimo global e não existem mínimos locais. 3.3.2 O efeito das restrições no ajuste Para entender o que acontece com o elipsoide do ponto restrito enquanto percorre o caminho até o ponto ótimo é mais interessante pensar em termos de escalas, rotações e translações do que nas mudanças dos coeficientes da quádrica. Porém, nem toda quádrica pode ser obtida a partir dessas transformações aplicadas a uma quádrica qualquer, então é necessário provar que no caso de um elipsoide, qualquer mudança em seus coeficientes pode ser representada como uma combinação de escalas, rotações e translações. A equação (49) é repetida a seguir em (72), já que a forma matricial da equação das quádricas torna mais fácil a manipulação dos coeficientes. (𝑥 𝑦 𝑧) ( 𝑎 𝑑 2 𝑒 2 𝑑 2 𝑏 𝑓 2 𝑒 2 𝑓 2 𝑐 ) ( 𝑥 𝑦 𝑧 ) + (𝑥 𝑦 𝑧) ( 𝑔 ℎ 𝑖 ) + 𝑗 = 0 (72) 47 “Toda matriz simétrica é diagonalizável através de uma matriz ortogonal” (BOLDRINI et al., 1986). Logo, sendo 𝐴 a matriz 3x3 da equação (72) e 𝑈 a matriz da transformação que a torna diagonal, a forma diagonal 𝐷 da matriz 𝐴 pode ser escrita como: 𝐷 = 𝑈𝑇𝐴𝑈 (73) Como na equação (73), para qualquer matriz diagonal 𝐷 existe uma matriz 𝑉 capaz de levar esta matriz para qualquer outra matriz diagonal 𝐷’ desde que a matriz 𝐷 seja não- singular. 𝐷′ = 𝑉𝑇𝐷𝑉 (74) Já que qualquer matriz simétrica é diagonalizável por uma matriz ortogonal, sempre se escolhe a matriz diagonal da equação (74) de modo que uma matriz ortogonal a leve a uma matriz simétrica qualquer desejada, como na equação (75). 𝐴′ = 𝑊𝑇𝐷′𝑊 (75) Portanto, desde que a matriz A seja não-singular (e assim tenha uma forma diagonal não-singular), existe uma matriz 𝑈𝑉𝑊 capaz de transformá-la em qualquer matriz simétrica existente, como na equação (76). 𝐴′ = 𝑊𝑇𝑉𝑇𝑈𝑇 · 𝐴 · 𝑈𝑉𝑊 (76) Para que a equação (72) represente um elipsoide, a matriz 𝐴 deve ter todos os autovalores com o mesmo sinal e, portanto, será não-singular. Além disso, como toda matriz ortogonal é também uma matriz de rotação e toda matriz diagonal representa uma escala, a transformação com a matriz 𝑈𝑉𝑊 representa uma sequência rotação-escala-rotação, o que mostra que apenas com rotações e escalas os coeficientes 𝑎, 𝑏, 𝑐, 𝑑, 𝑒, 𝑓 de um elipsoide podem ser alterados individualmente para qualquer valor desejado. Para ver o efeito de uma translação arbitrária nos coeficientes da equação da quádrica, basta adicionar as translações em cada coordenada como na equação (77). 𝑎(𝑥 + ∆𝑥)2 + 𝑏(𝑦 + ∆𝑦)2 + 𝑐(𝑧 + ∆𝑧)2 + 𝑑(𝑥 + ∆𝑥)(𝑦 + ∆𝑦) + 𝑒(𝑥 + ∆𝑥)(𝑧 + ∆𝑧) +𝑓(𝑦 + ∆𝑦)(𝑧 + ∆𝑧) + 𝑔(𝑥 + ∆𝑥) + ℎ(𝑦 + ∆𝑦) + 𝑖(𝑧 + ∆𝑧) + 𝑗 = 0 (77) Essa equação pode ser manipulada para que os novos coeficientes dos termos fiquem mais evidentes como na equação (78). 48 𝑎𝑥2 + 𝑏𝑦2 + 𝑐𝑧2 + 𝑑𝑥𝑦 + 𝑒𝑥𝑧 + 𝑓𝑦𝑧 + (2𝑎∆𝑥 + 𝑑∆𝑦 + 𝑒∆𝑧 + 𝑔)𝑥 +(𝑑∆𝑥 + 2𝑏∆𝑦 + 𝑓∆𝑧 + ℎ)𝑦 + (𝑒∆𝑥 + 𝑓∆𝑦 + 2𝑐∆𝑧 + 𝑖)𝑧 + 𝑎∆𝑥2 + 𝑏∆𝑦2 +𝑐∆𝑧2 + 𝑑∆𝑥∆𝑦 + 𝑒∆𝑥∆𝑧 + 𝑓∆𝑦∆𝑧 + 𝑔∆𝑥 + ℎ∆𝑦 + 𝑖∆𝑧 + 𝑗 = 0 (78) Assim, o cálculo dos novos coeficientes 𝑔′, ℎ′ e 𝑖′ depois da translação pode ser colocado em sua forma matricial dada na equação (79). ( 𝑔′ ℎ′ 𝑖′ ) = ( 2𝑎 𝑑 𝑒 𝑑 2𝑏 𝑓 𝑒 𝑓 2𝑐 )( ∆𝑥 ∆𝑦 ∆𝑧 ) + ( 𝑔 ℎ 𝑖 ) (79) A matriz 3x3 desta equação é a mesma matriz 𝐴 da equação das quádricas em sua forma matricial, porém com todos os elementos dobrados (os elementos fora da diagonal estavam divididos por dois). Portanto, do mesmo modo que a matriz 𝐴, essa matriz também é não-singular e pode ser invertida como mostrado na equação (80). (2𝐴)−1 ( 𝑔′ − 𝑔 ℎ′ − ℎ 𝑖′ − 𝑖 ) = ( ∆𝑥 ∆𝑦 ∆𝑧 ) (80) A equação (80) mostra como escolher a translação de modo a alterar individualmente os coeficientes 𝑔, ℎ, 𝑖 para qualquer valor desejado. Como mostrado, com escala e rotação é possível alterar os coeficientes 𝑎, 𝑏, 𝑐, 𝑑, 𝑒, 𝑓 e a translação consegue alterar os coeficientes 𝑔, ℎ, 𝑖. O único coeficiente que não pode ser alterado como desejado é o coeficiente 𝑗, mas já que a equação possui um parâmetro livre a manipulação de 9 parâmetros já pode ser suficiente para se obter qualquer superfície. Para isso, é possível normalizar a equação depois de cada transformação. Como a normalização se trata de apenas multiplicar os dois lados da equação por um valor, ela não muda a superfície representada pela equação. Portanto, utilizando apenas composições das transformações de rotação, escala e translação é possível levar um elipsoide inicial a qualquer outro elipsoide possível. Com isso, para auxiliar nas análises posteriores, seria interessante desenvolver um método onde, dados dois elipsoides, uma sequência de transformações capaz de transformar um elipsoide no outro pudesse ser obtida. Considerando o elipsoide inicial como uma primeira quádrica e o elipsoide a qual se quer chegar como uma segunda quádrica, a sequência de transformações pode ser obtida com o procedimento explicado a seguir: 49 i. Na primeira quádrica, calcular e aplicar a translação 𝑇1 necessária para deixar os coeficientes 𝑔, ℎ e 𝑖 com valor zero utilizando a equação (80). ii. Na segunda quádrica calcular o seu ∆𝑗 como na equação (81). ∆𝑗 = 𝑎∆𝑥2 + 𝑏∆𝑦2 + 𝑐∆𝑧2 + 𝑑∆𝑥∆𝑦 + 𝑒∆𝑥∆𝑧 + 𝑓∆𝑦∆𝑧 + 𝑔∆𝑥 + ℎ∆𝑦 + 𝑖∆𝑧 (81) iii. Calcular a matriz de autovetores 𝑈 da matriz 𝐴 da primeira quádrica. iv. Multiplicar os coeficientes da segunda quádrica por 𝑘 = −𝑗/(∆𝑗 − 𝑗) calculando a matriz de autovetores 𝑊 desta quádrica v. Calcular a matriz diagonal 𝐷 dividindo os autovalores da primeira quádrica pelos da segunda. vi. Utilizar a equação (76) para obter os coeficientes 𝑎, 𝑏, 𝑐, 𝑑, 𝑒, 𝑓 da segunda quádrica multiplicados por 𝑘. vii. Calcular a translação 𝑇2 que coloca os coeficientes 𝑔, ℎ e 𝑖 da quádrica obtida (que estão com valor zero) nos coeficientes da segunda quádrica multiplicados por 𝑘. O resultado obtido terá todos os coeficientes da segunda quádrica, porém multiplicados por 𝑘 = −𝑗/(∆𝑗 − 𝑗). viii. Por fim, a normalização deve ser aplicada o que consistirá em dividir todos os coeficientes por 𝑘, deste modo a segunda quádrica será obtida e já estará normalizada. Com esses passos, para qualquer mudança nos coeficientes de um elipsoide, é possível encontrar uma sequência de transformações lineares e normalização que gera exatamente a mesma mudança. A equação (82) mostra exatamente isso, a translação 𝑇1 faz com que a rotação e escala 𝑈𝑉𝑊 seja centrada na origem, portanto, a equação representa uma escala alinhada com os eixos e em relação ao centro do elipsoide, uma rotação do elipsoide em torno de seu centro e uma translação final. 𝑣′⃗⃗ ⃗ = 𝑈𝑉𝑊 · (𝑣 + 𝑇1) + 𝑇2 (82) Portanto, no caminho da reta indo do ponto restrito ao ponto ótimo, descrita na seção anterior, o elipsoide está passando por transformações de escala, rotação e translação. Um passo infinitesimal na reta, modifica os parâmetros da equação (82) também de forma infinitesimal. Além disso, qualquer rotação e translação de um elipsoide gera também um elipsoide e qualquer escala infinitesimal de um elipsoide também forma um elipsoide. Logo, no caminho do ponto restrito ao ponto ótimo existem infinitos elipsoides com erro quadrático 50 de distância algébrica menor que o elipsoide do ponto restrito, até que em algum ponto começa o espaço das quádricas que não são elipsoides. Isto mostra que qualquer algoritmo restrito, que use minimização do erro quadrático de distância algébrica, gera um resultado não ótimo sempre que o ajuste ótimo do algoritmo sem restrições não é um elipsoide. Se existe um elipsoide com erro menor que o elipsoide retornado por um algoritmo restrito, então o elipsoide com menor erro deveria ser utilizado, e não o resultado do algoritmo restrito. 3.3.3 Notas sobre os resultados dos algoritmos com restrições Na situação de algoritmos com restrições de elipsoide, podem acontecer dois casos: 1) No caso em que, para uma dada nuvem de pontos, o ajuste ótimo para esta nuvem é um elipsoide, igualar as derivadas parciais da equação (44) a zero tem como resultado a obtenção do elipsoide ótimo. 2) E, para o caso em que esta nuvem de pontos o ajuste ótimo for outra quádrica, que não um elipsoide (ponto ótimo), igualar as derivadas parciais a zero vai calcular esta quádrica enquanto os métodos com restrição de elipsoide vão calcular um elipsoide (ponto restrito). Na seção 3.1.3 foi provado que para o segundo caso se uma reta multidimensional for traçada entre o ponto restrito e o ponto ótimo, seguir essa reta em direção ao erro mínimo resulta em um erro sempre decrescente. Também foi provado que para cada passo dado seguindo esta reta a mudança nos coeficientes pode ser representada por um conjunto de translações, escalas e rotações. Porém, operações de translação e rotação não transformam um elipsoide em outra quádrica, pois o conjunto dos elipsoide é fechado sob rotação e translação. A única transformação capaz de transformar o elipsoide em outra quádrica é a escala. Portanto, seguir a reta no sentido do ponto ótimo significa que um dos eixos do elipsoide vai ser escalado até o infinito, quando ele vai colapsar em outra quádrica. Então, para qualquer elipsoide calculado com restrições, é possível gerar elipsoides cada vez mais esticados que possuam um erro cada vez menor. Observe que isso vale para todo tipo de algoritmo que use distância algébrica, mesmo se for iterativo ou analítico e mesmo se utilizar outra forma de representação do elipsoide que não seja a equação das quádricas. Portanto, as restrições utilizadas em praticamente todos os 51 algoritmos da literatura atual não são eficazes para a calibração dos magnetômetros, já que justamente quando a restrição é necessária (a quádrica ótima não é um elipsoide) o resultado do ajuste não é ótimo. Para calibração de magnetômetros, uma diferença de 10% ao longo de algum diâmetro do elipsoide já pode fazer com que a calibração piore o resultado das medições ao invés de melhorar. Com as equações da seção 3.3.1 foi possível construir um algoritmo que encontra um resultado com menor erro quadrático de distância algébrica para qualquer elipsoide obtido por um algoritmo com restrições. 3.4 Fusão de sensores inerciais A contribuição principal deste trabalho é o novo algoritmo proposto de calibração de magnetômetros. Porém, a motivação principal para o desenvolvimento deste algoritmo é para melhorar a utilização dos magnetômetros nos sensores inerciais compostos por giroscópios, acelerômetros e magnetômetros. Um sensor inercial foi feito através da fusão das medições calibradas do giroscópio, acelerômetro e magnetômetro. Para a realização da fusão foi utilizado um filtro de Kalman simples cujo equacionamento foi apresentado na seção 2.5. Para a calibração do giroscópio e do acelerômetro foram utilizados algoritmos simples já existentes na literatura e o algoritmo proposto foi utilizado para a calibração do magnetômetro. A implementação do sistema completo é importante, pois as medidas obtidas do acelerômetro e do giroscópio calibrados utilizando métodos conhecidos, assim como os ângulos finais obtidos depois da fusão de todos os sensores são informações que ajudam a verificar a qualidade dos dados obtidos e calibrados do magnetômetro. Além disso, como o filtro de Kalman possui uma estimativa dos erros das medições obtidas de cada sensor, essa informação é útil na escolha dos melhores pontos de entrada para serem utilizados na calibração do magnetômetro em uma configuração de realimentação. 52 4 Validação experimental 4.1 Descrição dos experimentos e métricas de validação A validação experimental do algoritmo proposto neste trabalho foi feita de duas maneiras: por meio de simulações computacionais e por meio de testes experimentais em sistema embarcado. Para as simulações computacionais os testes consistiram em gerar simulações das medições de um magnetômetro de acordo com a modelagem apresentada no capítulo 2. Os valores de saída do algoritmo, já calibrados, foram comparados com as medições simuladas sem a calibração encontrando-se a porcentagem de compensação dos erros de offset e hard- iron e dos erros de sensibilidade e soft-iron. Como o algoritmo apresentado neste trabalho é um algoritmo de minimização, ele vai encontrar os mesmos resultados que qualquer outro algoritmo que utilize o método dos mínimos quadrados e a mesma métrica de distância. Por isso, a precisão do algoritmo não foi comparada com a de algoritmos da literatura. Isso só não ocorre nos casos especiais em que o ajuste ótimo obtido pelo algoritmo não é um elipsoide. Para estes casos foram feitas várias simulações para esclarecer os resultados obtidos. Na seção de testes experimentais em um sistema embarcado, é descrita a comparação do algoritmo com um algoritmo comercial de código aberto existente na biblioteca de fusão de sensores da Freescale (FSFLK) (“Freescale Sensor Fusion Library for Kinetis MCUs”, 2015). A comparação é feita no custo computacional e na precisão de determinar o norte magnético da Terra, sendo o erro medido em graus. 4.2 Simulações computacionais 4.2.1 Algoritmos com restrição de elipsoide Na seção 3.3 foi apresentada uma demonstração matemática simples provando que os elipsoides obtidos com os algoritmos que utilizam restrição de elipsoide não são ótimos e que quando as restrições alteram o resultado do ajuste, o elipsoide obtido não serve para a calibração de magnetômetros. O objetivo desta seção é confirmar estes resultados teóricos por meio de simulações computacionais utilizando os algoritmos com restrição de elipsoide. 53 Com este objetivo, foi criado um algoritmo avaliador. O objetivo deste algoritmo é provar que um algoritmo de ajuste de elipsoide a ser avaliado retorna resultados não ótimos. Para isso ele tenta encontrar uma solução melhor que a solução do algoritmo avaliado, provando que o algoritmo não é ótimo para a instância do problema considerada. Na seção 3.3 foi mostrado que é possível obter uma solução melhor, em alguma instância, para qualquer método de ajuste que utilize restrições de elipsoide. Portanto, o algoritmo avaliador deve obter essas soluções, mostrando caso a caso que estes algoritmos não são ótimos. O algoritmo é simples. Primeiro é escolhida uma superfície quádrica que represente qualquer forma básica exceto a de um elipsoide. Depois disso, vários pontos pertencentes à superfície são escolhidos e é adicionado ruído gaussiano nestes pontos. O ruído pode fazer com que o ajuste ótimo se t