DEPARTAMENTO DE CIÊNCIAS EXATAS E ENGENHARIAS CURSO DE ENGENHARIA ELÉTRICA
JOÃO VITOR DAS CHAGAS SILVA
DESENVOLVIMENTO DE UM MÓDULO AHRS PARA APLICAÇÕES DE DETECÇÃO DE MOVIMENTO DO CORPO HUMANO
Ijuí 2019
DESENVOLVIMENTO DE UM MÓDULO AHRS PARA APLICAÇÕES DE DETECÇÃO DE MOVIMENTO DO CORPO HUMANO
Trabalho de Conclusão de Curso de Engenharia Elétrica apresentado como requisito parcial para obtenção do título de Engenheiro Eletricista.
Orientador:Prof. Dr. Maurício de Campos
Ijuí 2019
DESENVOLVIMENTO DE UM MÓDULO AHRS PARA APLICAÇÕES DE DETECÇÃO DE MOVIMENTO DO CORPO HUMANO
Este Trabalho de Conclusão de Curso foi julgado adequado para a obtenção do título de ENGENHEIRO ELETRICISTA e aprovado em sua forma final pelo professor orientador e pelo membro da banca examinadora.
Ijuí, 03 de fevereiro de 2020
Prof. Dr. Maurício de Campos Doutor pela Universidade Federal de Campina Grande - Orientador Prof. Me. Carolina Daiana Radüns Coordenadora do Curso de Engenharia Elétrica/UNIJUÍ BANCA EXAMINADORA Prof. Dr. Maurício de Campos (UNIJUÍ) Doutor pela Universidade Federal de Campina Grande Prof. Dr. Manuel Martín Pérez Reimbold (UNIJUÍ) Doutor pela Universidade Federal do Rio Grande do Sul
AGRADECIMENTOS
Agradeço a minha família por todo apoio e suporte durante toda a minha cami-nhada, sem vocês eu não seria nada, obrigado por tudo.
Ao meu orientador por toda ajuda não apenas na realização desse trabalho mas também durante toda a graduação.
Aos meus colegas do GAIC, por todas as horas de trabalho que passamos juntos e todo apoio durante o desenvolvimento desse trabalho.
Silva, J. V. Desenvolvimento De Um Módulo AHRS Para Aplicações De Detecção De Movimento Do Corpo Humano. 2019. Trabalho de Conclusão de Curso. Curso de Engenharia Elétrica, Universidade Regional do Noroeste do Estado do Rio Grande do Sul – UNIJUÍ, Ijui, 2019
A detecção da orientação dos movimentos realizados pelo corpo humano possui apli-cações em diferentes áreas como medicina, esportes de alta performance, jogos ele-trônicos, e robótica. Dessa forma, o estudo e desenvolvimento desses de dispositivos torna-se atraente, no qual sistemas baseados em sensores inerciais apresentam van-tagem de não depender de estruturas externas. Nesse contexto, o presente trabalho apresenta o desenvolvimento de um protótipo de baixo custo de um sistema AHRS para detecção da orientação das articulações do corpo humano. No qual foi implemen-tado um algoritmo com o estimador gradiente descendente para obter a orientação do dispositivo. Testes foram realizados com o propósito de verificar se o mesmo é capaz de estimar adequadamente sua orientação expressa em ângulos de Euler. Resultados parciais mostram que o dispositivo é capaz de obter adequadamente a orientação em dois eixos de coordenadas perpendiculares e no terceiro eixo a orientação estimada não apresenta coerência com os movimentos realizados. Trabalhos futuros devem ser executados afim de corrigir os problemas apresentados e melhorias no protótipo. Palavras-Chave: Sensores inerciais. Algoritmo gradiente descendente. Estimativa de orientação.
Silva, J. V. Development of an AHRS Module for Human Body Motion Detection Applications. 2019. Final Course Assignment. Electrical Engineering Course, Univer-sidade Regional do Noroeste do Estado do Rio Grande do Sul – UNIJUÍ, Ijui, 2019 Orientation detection of movements made by the human body has many applications in different areas such as medicine, sports of high performance, electronic games and robotics. Therefore, studying and developing these devices is attractive and systems based on inertial sensors present the advantage of not needing external structures. Due to that, this work presents a low-cost prototype development of an AHRS system to detect the human body joints orientation. In which was implemented an algorithm with the descendent gradient estimator to obtain the device orientation. Tests were conducted with the intention of verifying if it were capable of estimating properly its orientation expressed in Euler angles. Partial results show that the device is capable of properly obtaining the two-axis orientation of perpendicular coordinates and in the third axis, the estimated orientation does not present consistency with the movements that were being made. Future work must be made to correct the problems presented and make prototype improvements.
1 Movimentos de extensão e flexão . . . 17
2 Movimentos de adução, abdução, rotação, circundação e flexão e exten-são das mãos . . . 17
3 Movimentos dos pés, mãos e ombros . . . 18
4 Módulos utilizados para detectar a orientação de dois segmentos do braço 19 5 Curva típica da sensibilidade vs Frequência de um acelerômetro . . . . 20
6 Exemplo de Barramento I2C . . . 22
7 Esquema básico da comunicação SPI . . . 22
8 Diagrama de blocos do algoritmo proposto . . . 26
9 Diagrama do stm32L412CBT6 . . . 39
10 Módulo GY-273 . . . 41
11 Módulo GY-521 . . . 42
12 placa . . . 43
13 prototiponovo . . . 43
14 Resultados para a rotação em torno do eixo Z do protótipo . . . 46
15 Resultados para a rotação em torno do eixo x do protótipo . . . 47
16 Resultados para a rotação em torno do eixo x do protótipo . . . 48
17 Resultados para a rotação em torno do eixo x do protótipo . . . 48
1 Comparação entre I2C e SP I . . . . 23
Mocap Motion Capture
AHRS Attitude and Heading Reference System VANTs Veículos aéreos não tripulados
DSC Digital Signal Controller
MEMS Microelectromechanical systems AD Análogico para digital
I2C Inter-integrated Circuit
SPI Serial Peripheral Interface SCLK Serial Clock
MOSI Master Output Slave Input MISO Master Input Slave Output SS Slave Select
KF Filtro de Kalman
EKF Filtro de Kalman Estendido IMU Inertial measurement unit
MARG Magnetic, angular Rate, and gravity ARM Advanced RISC Machine
DMA Direct memory acess FPU Floating point Unit
1 INTRODUÇÃO 12
1.1 OBJETIVOS . . . 14
1.2 METODOLOGIA . . . 14
1.3 ORGANIZAÇÃO DO TRABALHO . . . 15
2 EMBASAMENTO TEÓRICO 16 2.1 MOVIMENTOS DO CORPO HUMANO . . . 16
2.2 MÓDULO AHRS . . . 18
2.3 SENSORES . . . 19
2.4 FILTROS DE FUSÃO DE DADOS . . . 23
2.4.1 Filtro de Kalman . . . 24
2.4.2 Algoritmo Gradiente descendente . . . 25
2.5 TRABALHOS RELACIONADOS . . . 27
3 GRADIENTE DESCENDENTE ALGORITMO 32 3.1 ÁLGEBRA DOS QUATÉRNIOS . . . 32
3.2 EQUACIONAMENTO DO ALGORITMO . . . 33
4 RESULTADOS 39 4.1 DESENVOLVIMENTO DO PROTÓTIPO . . . 39
4.1.1 Microcontrolador . . . 39
4.1.2 Magnetômetro . . . 40
4.1.3 Unidade de medida inercial . . . 41
4.1.4 Protótipo . . . 42
5 CONCLUSÃO 50 5.1 TRABALHOS FUTUROS . . . 50
REFERÊNCIAS 52
APÊNDICE A - DIAGRAMA ESQUEMÁTICO DO PROTÓTIPO 55
APÊNDICE B - BIBLIOTECA DESENVOLVIDA PARA A MPU-6050 58
APÊNDICE C - BIBLIOTECA DESENVOLVIDA PARA O QMC-5883l 73
1
INTRODUÇÃO
Atualmente, a detecção dos movimentos do corpo humano é um campo de interesse na medicina, esportes de alta performance, jogos eletrônicos, robótica entre outras áreas. Diferentes tecnologias têm sido desenvolvidas para essa tarefa, como sistemas mecânicos, óticos, magnéticos, acústicos e inerciais (FOURATI et al., 2014). Esses sistemas podem ser classificados em: baseados em visão com ou sem marcado-res, não baseados em visão ou sistemas guiados roboticamente (ZHOU; HU; HARRIS, 2006). Nesse sentido, o sistema Mocap (motion capture) é baseado em visão com marcadores, o kinect é baseado em visão sem marcadores e sistemas inerciais tais como um AHRS (Attitude and Heading Reference System) é não baseado em visão.
Na medicina, o rastreamento dos movimentos do corpo humano permite que problemas relacionados a restrições de movimentos sejam explorados e reabilitações sejam avaliadas. Por exemplo, em Zhou e Hu (2008) é apresentado uma revisão da literatura de técnicas de rastreamento do movimento humano disponíveis e seu uso para reabilitação. Em Song et al. (2016) foi analisado o uso do kinect e o sistema AHRS para estudar o movimento da cervical. Com isso, evidenciando o interesse e aplicabilidade dessa tecnologia na medicina.
O rastreamento dos movimentos do corpo humano também auxilia no dia a dia de pessoas com deficiências físicas, como controlar cadeiras de rodas motorizadas utilizando movimentos da cabeça, onde joysticks convencionais não podem ser imple-mentados, como em pessoas com lesões severas na coluna (HAN et al., 2003). O trabalho de Mandel, Rofert e Freset (2007) é um exemplo. Nesse trabalho utiliza-se sensores inerciais para detectar o movimento da cabeça para controlar a direção que se move uma cadeira elétrica. Por consequência disso a autonomia e a qualidade de vida dessas pessoas é melhorada.
Nos esportes de alta performance, essas tecnologias podem tornar-se uma ferramenta para o aprimoramento de atletas. Permite que movimentos sejam captados e avaliados. Podendo assim ser usada no ensino e treino de esportes, como em danças e artes marciais. O trabalho de Chan et al. (2011) por exemplo, demonstra que o uso da realidade virtual juntamente com um sistema de detecção de movimentos facilita o aprendizado de danças, em comparação à apenas assistir vídeos em que instrutores exibem os movimentos. Uma extrapolação para o futuro, é a utilização em academias para informar ao usuário qual postura ou movimento está errado, podendo evitar que o mesmo sofra lesões e um tratamento personalizado com menor custo.
de realidade virtual são um bom exemplo, no qual um sistema para captar os movimentos da cabeça do jogador, informa qual direção ele está olhando e assim o cenário se ajustar permitindo visão em 360º. A captação de outros movimentos aumenta a imersão, principalmente em jogos de ação.
Na robótica é um elemento chave para criação de interfaces homem-máquina (IHM) mais amigáveis (QIAN; NIU; YANG, 2013). Podendo ser utilizado para o controle e programação de robôs (POLLARD et al., 2002; NETO; PIRES; MOREIRA, 2009). Com isso simplificando esses processos, reduzindo o tempo e custo para treinamento de operadores e programadores.
Outra aplicação é no controle de cardans para câmeras de VANTs (veículos aéreos não tripulados), como os quadrirrotores. O controle é feito através do movimento da cabeça do piloto, quando ele olha para uma direção a câmera acompanha o movi-mento. Dessa forma, o operador sente-se como se estivesse na aeronave, permitindo uma observação mais natural do ambiente em que o drone está sobrevoando. Um exemplo disso é o DJI Goggles da DJI.
Tendo em vista as possíveis aplicações da detecção de movimentos humanos, o desenvolvimento de um sistema utilizando módulos AHRS torna-se atraente. Esses módulos são comumente utilizado para obter a orientação de aeronaves e veículos aquáticos. Com esse dispositivo a orientação de cada parte do corpo pode ser avaliada independentemente. Possibilitando a reconstrução dos movimentos através da fixação de módulos na cabeça, tronco e em cada segmento dos membros.
O sistema AHRS é constituído basicamente por giroscópios, acelerômetros e magnetômetros triaxiais juntamente com um microcontrolador ou DSC (Digital Signal Controller ). O giroscópio mede a velocidade angular e através da integração no tempo fornece a posição. O acelerômetro é usado para verificar a orientação e sentido da gravidade e o magnetômetro a orientação e sentido do campo magnético da terra criando com isso um referencial baseado no planeta. Dessa forma, proporciona uma melhor estimação da real orientação do objeto. As medidas realizadas pelos sensores são adquiridas e processadas através de um filtro digital no microcontrolador fornecendo a orientação do objeto.
A principal vantagem desse sistema é não necessitar de estruturas externas para ser aplicado, nem um meio com iluminação controlada. Portanto, esses fatores ambientais não são restrições para utilizá-lo em ambientes externos, como no caso de sistemas baseados em visão.
1.1 OBJETIVOS
Considerando os diferentes campos de aplicação do rastreamento dos movimen-tos do corpo humano e as vantagens da utilização de um sistema AHRS. O objetivo desse trabalho é apresentar o desenvolvimento de um protótipo baseado nesse sistema para detecção da orientação do dispositivo que posteriormente pode ser aperfeiçoado para detectar a orientação de segmentos do corpo humano.
• Realização uma revisão bibliográfica sobre os sistema AHRS suas funcionalidades, sensores, filtros digitais e outros trabalhos relacionados.
• Definir os sensores e microcontroladores que serão utilizados no projeto. • Projetar e desenvolver o circuito de aquisição e transmissão de dados. • Implementação do filtro digital escolhido.
1.2 METODOLOGIA
No desenvolvimento desse trabalho foi optado por uma abordagem hipotética-dedutiva. Que apresenta um caráter experimental, através no desenvolvimento de um protótipo para detecção da orientação.
Para alcançar os objetivos propostos, seis etapas devem ser executadas:
• Revisão da literatura em artigos científicos, teses e livros, disponíveis na internet, banco de dados e na biblioteca da UNIJUÍ.
• Seleção dos sensores e do microcontrolador, baseando-se nos resultados obtidos pelos trabalhos verificados na etapa anterior. Considerando o fator econômico. • Desenvolvimento do protótipo em placa de circuito impresso (PCB).
• Implementação dos filtros digitais.
1.3 ORGANIZAÇÃO DO TRABALHO
O presente trabalho está organizado em 5 capítulos. Divididos da seguinte forma: No capitulo 2 está apresentado os principais movimentos do corpo humano, conceitos importantes sobre o sistema AHRS e seus sensores, filtros digitais para estimar a orientação e trabalhos relacionados existentes na literatura.
O capitulo 3 refere-se a álgebra de quatérnios e a derivação do filtro implemen-tado, onde são apresentadas as principais equações do mesmo.
No capitulo 4 estão demonstrados os resultados obtidos, apresentando o pro-tótipo desenvolvido, o programa executado e os gráficos referentes a estimativa de orientação.
Finalmente no capitulo 5 estão apresentadas as conclusões obtidas e as suges-tões de trabalhos futuros.
2
EMBASAMENTO TEÓRICO
Neste capitulo serão apresentados os termos relacionados aos movimentos do corpo humano. O sistema AHRS e os sensores, onde será discutido os conceitos básicos de ambos. Além dos principais trabalhos relacionados disponíveis na literatura, que serviram de base para o desenvolvimento da monografia.
2.1 MOVIMENTOS DO CORPO HUMANO
O corpo humano possui diversas articulações, que executam movimentos no espaço tridimensional. Devido ao interesse de rastreá-los, para as mais diversas aplicações, a seguir é apresentado uma breve descrição dos principais movimentos e suas terminologias.
No livro de Moore, Dalley e Agur (2014) é apresentado o nome de movimentos de várias partes do corpo. No qual alguns são realizados por diferentes segmentos do mesmo, como a flexão, extensão, abdução, adução, rotação medial, rotação lateral, e circundação. O primeiro é a redução do ângulo entre duas partes do corpo e a extensão é o contrário, ambos são realizados pelo braço, perna, tronco, mão, joelho e cotovelo. A abdução e adução são realizadas pelos braços e pernas, consiste no distanciamento e aproximação dos braços e pernas da linha central do corpo. A rotação medial e lateral (rotação lateral externa e rotação medial interna, no caso dos braços) é a rotação em direção ao centro e ao exterior do corpo. O movimento de circundação dos membros inferiores e superiores consiste na combinação de outros movimento mencionados anteriormente. A rotação é realizada pelo tronco, cabeça e punho. Esses movimentos estão ilustrados nas Figuras 1 e 2.
Figura 1: Movimentos de extensão e flexão
Fonte: adaptado de Moore, Dalley e Agur (2014)
Figura 2: Movimentos de adução, abdução, rotação, circundação e flexão e extensão das mãos
Fonte: adaptado de Moore, Dalley e Agur (2014)
Existem movimentos específicos de algumas partes do corpo, como a flexão planar, dorsiflexão, eversão, inversão, elevação e depressão. Os dois primeiros dizem respeito ao movimento do pé em relação ao eixo vertical. Eversão é o movimento do pé na horizontal no sentido ao centro do corpo, a inversão é o movimento do pé na horizontal para a direção contraria à eversão. Elevação e depressão são movimentos realizados pelos ombros para cima e para baixo respectivamente. Como ilustrado na Figura 3.
Figura 3: Movimentos dos pés, mãos e ombros
Fonte: adaptado de Moore, Dalley e Agur (2014)
Existem outros movimentos como os da língua, dedos, olhos e pálpebras. Porém nesse trabalho os movimentos dessas partes do corpo não são de interesse, portanto, não serão descritos.
2.2 MÓDULO AHRS
Um módulo AHRS é um sistema de rastreamento de posição e orientação inercial, consiste basicamente de um acelerômetro, giroscópio e magnetômetro e microcon-trolador. É usada principalmente na mensuração do ângulo de guinagem, arfagem e rolagem de veículos aéreos (PAIXAO; ROSA; NETO, 2011). Atualmente essa tecnologia também tem sido usada para detectar os movimentos do corpo humano. Assim como o desenvolvimento de filtros e arquiteturas especificamente para essa tarefa, como será analisado na seção 2.4. Pois possui vantagens em relação a outras técnicas de rastreamento dos movimentos do corpo humano.
A orientação de cada segmento do corpo pode ser detectado individualmente em relação ao planeta Terra. Para isto, basta distribuir um módulo em cada uma das partes do corpo, como ilustra a Figura 4.
Figura 4: Módulos utilizados para detectar a orientação de dois segmentos do braço
Fonte: adaptado de Atrsaei et al. (2017)
Os sensores são elementos-chave para o correto funcionamento desses siste-mas. Dessa forma na seção 2.3 serão apresentados os sensores e suas principais característicos e parâmetros, necessários para o dimensionamento dos mesmos.
2.3 SENSORES
Os acelerômetros são sensores que medem a aceleração de um corpo e o campo gravitacional da terra (KOK; HOL; SCHÖN, 2017). Segundo Balbinot e Bru-samarello (2015) existem diversas tecnologias de fabricação de acelerômetros, onde destacam-se os piezoelétricos, piezorresistivos e capacitivos. Porém, todos tem o mesmo princípio: uma massa inercial que conforme submetida a uma aceleração pro-duz força. Atualmente existe uma grande disponibilidade do mercado de acelerômetros MEMS (microelectromechanical systems) que resultam em sensores com tamanho e peso reduzido, que é fundamental quando deseja-se captar os movimentos de um ser humano.
Para o dimensionamento de acelerômetros, deve-se analisar a sensibilidade, massa, faixa dinâmica e resposta em frequência (BALBINOT; BRUSAMARELLO, 2015). A sensibilidade é dada em pC/g ou mV/g, no qual 1g = 9.81 m/s2. A massa de um
acelerômetro diz respeito ao seu peso que é importante em determinadas aplicações, pois pode afetar o comportamento do sistema. A faixa dinâmica diz respeito aos limites de operação do acelerômetro sendo compreendida entre uma aceleração mínima e máxima, se esses limites forem ultrapassados podem causar leituras incorretas e danos ao dispositivo. Na Figura 5 está ilustrada a curva da sensibilidade vs a frequência tipica de um acelerômetro. Pode-se observar que em certas faixas a sensibilidade do sensor altera-se, portanto idealmente ele deve operar na região onde não há variação da sensibilidade.
Figura 5: Curva típica da sensibilidade vs Frequência de um acelerômetro
Fonte: Adaptado de GlobalSpec (2015)
Giroscópios são sensores que medem a velocidade angular de um objeto. No qual os tipos mais comuns na literatura de giroscópios MEMs são: do tipo diapasão (tuning-fork), rodas osciladoras, pêndulos de Foucault e ressonador wine glass (BERNS-TEIN, 2003). São constituídos de uma massa interna que é sensível a mudança de velocidade e baseam-se nas leis de Newton e do efeito Coriolis (LI; XIAO; FEI, 2015). No trabalho de Li, Xiao e Fei (2015) são apresentados os principais indicadores de performance dos giroscópios. O viés de estabilidade representa os limites da flutuação em relação ao valor médio da leitura na saída do sensor, dado em graus por hora (º/h). A sensibilidade é o quanto de variação na velocidade é necessária para que seja detectada pelo sensor. O fator de escala por sua vez, é a razão entre a saída do sensor e sua entrada. A largura de banda representa os limites de frequência que o giroscópio pode detectar variações angulares precisamente de forma semelhante ao acelerômetro.
Outro aspecto importante é o alcance que está relacionado aos limites máximos de velocidade detectados pelo sensor.
No mesmo trabalho de Li, Xiao e Fei (2015) é apresentado uma classificação dos giroscópios . Os sensores de baixa precisão possuem um alcance de 300º/s e um viés de estabilidade de 500º/h à 1000º/h. Giroscópios de média precisão possuem alcance de 250º/s e viés de estabilidade de 50º/h à 200º/h. Por último os de da alta precisão possuem alcance de 20º/s a 250º/s e viés de estabilidade de 0.5º/h a 50º/h.
Entre os erros mais comuns principalmente nos giroscópios é desvio (bias) e a presença de ruídos. O desvio varia com o tempo e representa uma diferença entre o valor medido e o valor real. Como a leitura do giroscópio deve ser integrada afim de obter a orientação esse erro tende a se acumular com o tempo, degradando os resultados obtidos (KOK; HOL; SCHÖN, 2017).
Os magnetômetros são sensores capazes de medir um campo magnético e são classificados de duas formas, na capacidade de medir o campo magnético total ou as componentes do mesmo (LENZ; EDELSTEIN, 2006). Para o AHRS o importante é segundo tipo, pois nesses sistemas esses sensores servem para medir a orientação do campo magnético da terra atuando de forma similar a uma bússola.
Possuem critérios de seleção para o projeto semelhantes aos acelerômetros e giroscópios. Como são utilizados para a detecção do campo magnético da terra em um AHRS, eles sofrem com a interferências de outras fontes de campos magnéticos presentes em ambientes com a presença humana (LIGORIO; SABATINI, 2016).
Tanto os acelerômetros como giroscópios e magnetômetros podem ser capazes de medir em um, dois ou três eixos. Para este trabalho os sensores devem ser 3D, ou seja, serem sensíveis em três eixos.
Outro aspecto para o dimensionamento dos sensores é a forma como a informa-ção da medida é transmitida. Se o sensor for analógico, o microcontrolador ou DSC, converte o sinal de analógico para digital através de um conversor AD (analógico para digital), quando maior o número de bits do conversor melhor será a resolução da leitura. Se o sensor for digital ele deve ter um conversor AD com uma grande número de bits para maior resolução. Além de ter um protocolo de comunicação capaz de transmitir a informação.
Um dos protocolos de comunicação mais utilizados pelos sensores para transmitir a sua leitura é o I2C (Inter-Integrated Circuit). Esse barramento é utilizado para a
comunicação dispositivos mestres e escravos utilizando apenas 2 fios que é a principal vantagem desse barramento (VALDEZ; BECKER, 2015). Como ilustrado na Figura 6,
onde o microcontrolador é o mestre e os Expansores de I/O, EEPROM, LCD Driver, sensores, Leds e conversores de dados são os escravos.
Figura 6: Exemplo de Barramento I2C
Fonte: Adaptado de Valdez e Becker (2015)
Outra forma de comunicação bastante utilizada por esses sensores é a SPI (Serial Peripheral Interface). Que consiste em uma comunicação serial síncrona assim como a I2C, que necessita de 3 fios mais o número de escravos, é full duplex, ou seja,
a comunicação ocorre do mestre para o escravo e do escravo para o mestre ao mesmo tempo (SACCO, 2014).
Figura 7: Esquema básico da comunicação SPI
A Figura 7 ilustra o esquema básico da comunicação SPI. No qual existe 6 linhas conectadas no mestre e 4 em cada escravo. O SCLK (Serial Clock) é o clock usados nos dispositivos, MOSI (Master Output Slave Input) saída de dados enviada pelo mestre e recebida pelo escravo. O MISO (Master Input Slave Output) são os dados enviados dos escravos para o mestre. Finalmente SS (Slave Select) é a seleção do escravo que irá comunicar-se com o mestre naquele momento.
Tabela 1: Comparação entre I2Ce SP I
Tecnologia Barramento de comunicação Taxa Máxima Fluxo de dados
SPI 3+ escravos 2 Mbps Full Duplex
I2C 2 (até 127) dispositivos 400 Kbps Half Duplex
Fonte: Adaptado de Sacco (2014)
Na Tabela 1, está representado uma comparação entre o I2C e o SPI, com ela
fica evidente as diferenças entre os protocolos de comunicação. No qual o SPI é a comunicação mais rápida entre as duas, com valor de 2 Mbps e o I2C com 400 Kbps
por segundo. Essas velocidades não são regra e dependem dos sensores que serão utilizados.
2.4 FILTROS DE FUSÃO DE DADOS
A leitura dos acelerômetros , giroscópios e magnetômetros está passível de erro, como em qualquer sensor. Desse modo a implementação de filtros de fusão de dados tem como objetivo atenuar tais erros através da combinação dos dados obtidos pelos mesmos. Com isso obter uma melhor estimação da real orientação de cada segmento do corpo.
Na literatura muitos trabalhos baseiam-se no Filtro de Kalman (KF) mais especi-ficamente de uma variação do mesmo aplicada para sistemas não lineares o Filtro de Kalman Estendido (EKF), onde diversas abordagens para o mesmo são exploradas na literatura. Da mesma forma o filtro proposto no trabalho Madgwick, Harrison e Vaidyanathan (2011) devido sua relativa simplicidade por possuir apenas um parâmetro a ser ajustado e ter resultados similares ao EKF. Portanto nas seções 2.4.1 e 2.4.2 ambos os filtros são introduzidos.
2.4.1
Filtro de Kalman
O Filtro de Kalman consiste em um estimador preditor-corretor, publicado em 1960 por Rudolph E. Kalmann (WELCH; BISHOP, 2006). Que em sistemas bem mode-lados e que possuem uma distribuição gaussiana (distribuição normal) é considerado um ótimo filtro (FARAGHER, 2012).
As etapas de funcionamento do filtro de Kalman podem ser divididas em duas partes, uma que prediz e a outra que corrige. Onde a primeira etapa está descrita pelas equações 1 e 2.
ˆ
x−k = Aˆxk−1+ Buk−1 (1)
Pk− = APk−1AT + Q (2)
Sendo que ˆx−k é a projeção do estado atual, ˆxk−1 é o estado anterior (estado
estimado anteriormente), uk−1 é a entrada do cálculo anterior, A é a matriz de estado
que relaciona o estado atual com o anterior, B é a matriz de entrada que relaciona a entrada anterior com ˆx−k, Pk− é a covariância de erro estimada e Pk−1 é a covariância
do erro anterior e Q representa o ruído do processo (sistema).
Na segunda etapa os valores calculados anteriormente são corrigidos através das leituras dos sensores. Nessa etapa que a fusão dos dados acontece. Nos quais as equações aplicadas estão representadas por 3, 4 e 5.
Kk = Pk−H T (HPk−HT t+ R)−1 (3) ˆ xk= ˆx−k + Kk(Zk− H ˆx−k) (4) Pk = (I − KkH)Pk− (5)
Nas equações 3, 4 e 5 Kké o ganho do Filtro de Kalman, ˆxké estado atual, Pké a
matriz de covariância, R é a covariância do erro de medição, H é a matriz de observação que relaciona o estado do sistema com as medidas realizadas pelos sensores. A matriz Zké a matriz de medida e está relacionado a ˆxk pela equação 6. Onde vk é o ruído das
zk = Hxk+ vk (6)
Para sintonizar o filtro Kalman, Q e R são ajustados para que o filtro tenha uma melhor resposta possível. Além da necessidade de uma covariância de erro inicial P0
para começar o processo.
O filtro Kalman Estendido é uma modificação do filtro de Kalman para sistemas não lineares, como mencionado anteriormente. Funciona com o principio de linearização em torno de um estado atual (WELCH; BISHOP, 2006). Possui as mesmas duas etapas que a versão linear, a diferença é que a relação entre o estado previsto e o estado anterior não é linear, como descrito pela equação 7.
ˆ
x−k = f (ˆxk − 1, uk−1, 0) (7)
A covariância do erro estimado é descrita pela equação 8. Um novo elemento é adicionado nessa equação, o erro de processo Wk, que está relacionado aos erros do
processo em si e não do sistema de medição.
Pk−= AkPk−1AkT + WkQk−1WkT (8)
As etapa de correção é descrita pelas equações 9, 10 e 11.
kk = Pk−HkT HkPk−HkT + VkRkVkT (9) ˆ xk = ˆx−k + Kk(zk− h(ˆx−k, 0)) (10) Pk= (I − KkHk)Pk− (11)
Sendo que h(ˆx−k, 0)representa a relação entre a matriz zk e o estado do sistema.
2.4.2
Algoritmo Gradiente descendente
Esse filtro foi apresentado no trabalho de Madgwick, Harrison e Vaidyanathan (2011), foi projetado para ser computacionalmente eficiente, usado no rastreamento de
movimentos humanos em processos de reabilitação. Que é implementado utilizando um IMU ( Inertial measurement unit ), ou seja, apenas giroscópios e acelerômetros e uma matriz MARG (Magnetic, angular Rate, and gravity) de sensores.
O algoritmo fornece na saída a orientação do sistema (S) em função do tempo relativo ao eixo de coordenadas do planeta Terra (E) em forma de quatérnios descritos comoS
Eqˆest,t.Na Figura 8 é apresentado o diagrama de blocos do algoritmo, onde ⊗ é
uma multiplicação entre quatérnios.
Os quatérnios podem ser utilizados para representar uma rotação de um vetor no espaço tridimensional. Dessa forma, não necessitando de operações trigonométricas como um sistema baseado no teorema de rotação de Euler. Muitos trabalhos como pode ser observado na seção 2.5 utilizam essa notação para representar a orientação de um objeto no espaço tridimensional, em conjunto inclusive com o EKF.
O único parâmetro que precisa ser ajustado é β que é relacionado aos erros de leitura do giroscópio, obtidos através da equação 12. É utilizado o erro máximo de leitura ωmax nos três eixos (x,y,z) do sensor. Isso representa uma vantagem em relação
ao EKF, no qual o ajuste é mais complicado.
β = ||1
2 ⊗ [0 ω˜max ω˜max ω˜max]|| =
s
3
4ω˜max (12)
Figura 8: Diagrama de blocos do algoritmo proposto
Para validar o algoritmo proposto os autores realizaram testes usando como padrão de medida os resultados obtidos com um sistema mocap da Vicon constituído de 8 câmeras MX3+. A implementação ocorreu utilizando um IMU e uma matriz MARG de sensores. Também houve a comparação com um filtro Kalman de propriedade da empresa Xsens, no qual foram implementados com os mesmos dados ambas as técnicas. Para os testes foi usado o xsens Mtx orientation sensor que é constituído de giroscópios, acelerômetros e magnetômetros triaxiais com resolução de 16 bits.
Os resultados foram descritos utilizando os valores RMS dos erros, divididos em erros estáticos e dinâmicos dos ângulos de arfagem, rolagem e guinada representados por φ, θ e ψ respectivamente. Sendo que os valores são considerados estáticos se possuem variação angular de menos de 5◦/s e dinâmicos com valores superiores a isso, estão mostrados na Tabela 2. Demonstrando resultados similares entre as duas técnicas.
Tabela 2: Erros RMS dos testes realizados
Ângulo Algoritmo baseado em EKF MARG IMU RMS[φe] estático 0.789◦ 0.581◦ 0.594◦ RMS[φe] dinâmico 0.769◦ 0.625◦ 0.623◦ RMS[θe] estático 0.819◦ 0.502◦ 0.497◦ RMS[θe] dinâmico 0.847◦ 0.688◦ 0.688◦ RMS[ψe] estático 1.150◦ 1.073◦ N/A RMS[ψe] dinâmico 1.344◦ 1.110◦ N/A
Fonte: Adaptado de Madgwick, Harrison e Vaidyanathan (2011)
Devido ser um algoritmo com menor complexidade se comparado com o EKF, e possui resultados similares, optou-se pela implementação desse filtro afim de obter a estimativa da orientação. O equacionamento do mesmo está descrito no capitulo 3.
2.5 TRABALHOS RELACIONADOS
No trabalho de Yun e Bachmann (2006) é apresentado o filtro de fusão de dados projetado para rastrear a orientação dos membros do corpo humano em tempo real. Os dados do acelerômetros e magnetômetros são pré-processados por um algoritmo QUEST (Quaternion Estimator) e posteriormente fundidos com os dados dos giroscópios por um EKF. Nesse trabalho os segmentos do corpo humano são modelados como um sistema de primeira ordem e sua orientação é representada utilizando quatérnios. É demonstrado duas abordagens para o EKF. Na primeira é sem a utilização do QUEST, e na segunda utilizando-o.
A implementação da segunda abordagem é descrita, onde a obtenção dos prin-cipais parâmetros são detalhados. Um teste usando o software Matlab é efetuado utilizando dados reais de uma matriz MARG, demonstrando que quando não há grande variação de velocidade do objeto o algoritmo QUEST é suficiente para estimar a ori-entação do membro. Porém quando há grandes variações na velocidade o QUEST não é suficiente para estimar com precisão a orientação. Por isso a implementação do filtro Kalman tornou-se necessária. Foram utilizados um giroscópio Tokin CG-L43 com alcance de 90◦/s. Dois acelerômetros ADXL202E de dois eixos, montados de uma forma que fosse possível medir em três eixos. Dois magnetrômetros HMC1051Z e HMC1052 de um 1 e 2 eixos respectivamente e um microcontrolador MSP430F149 da Texas Instruments para validar o algoritmo.
Foram realizados dois testes empíricos. O primeiro usando uma mesa inclinável de alta precisão e o segundo com duas matrizes MARG presas em um braço humano da mesma forma que na Figura 4. No primeiro experimento movimentos foram executados repetidas vezes pela mesa inclinável. Foi observado um erro máximo de 9◦, que segundo os autores foram devidos ao atraso de 100 ms causados pela amostragem que foi de 1ms mais o tempo de execução do algoritmo e da transmissão dos dados. No segundo experimento foram realizados movimentos aleatórios pelo braço em um avatar construído com computação gráfica os repetia, para que os movimentos fossem avaliados quantitativamente. Os resultados mostraram-se satisfatórios pois o avatar era capaz de replicar os movimentos.
Lee e Park (2009) descreveram um novo filtro de Kalman Estendido baseado em quatérnios para detectar a orientação do corpo humano utilizando matriz MARG de sensores. O algoritmo apresentado é similar ao proposto em Yun e Bachmann (2006). Porém esse trabalho apresenta um estimador quatérnio chamado de O2OQ para processar os dados dos acelerômetros e magnetômetros com o objetivo de reduzir a ordem do filtro de Kalman implementado. Possui duas abordagens para representar os vetores contendo os dados dos acelerômetros e magnetômetros que dependem de condições especificas. Essa técnica foi aplicada com a intensão de reduzir o efeito de acelerações lineares sobre a orientação da gravidade e diminuir os efeitos de materiais ferromagnéticos sobre a leitura dos magnetômetros. A representação do vetor que é implementado depende de condições especificas relativas as leituras dos sensores e alguns parâmetros definidos, no qual essa parte do algoritmo é chamada de seletor de vetor.
Para implementar o filtro e valida-lo também foi utilizado o MTx motion tracking da Xsens, utilizando como padrão os dados providos por um sistema Mocap VisualEyes VZ300 optical Tracking System. Ambos com uma taxa de amostragem de 100 Hz. Foram
realizados testes considerando três condições. A primeira com movimentos lentos em um campo magnético homogêneo. Na segunda, os sensores foram mantidos parados e um disco de metal com 18 cm de diâmetro e 3.5 cm de espessura foi balançado próximo ao sensor, para visualizar os efeitos de distúrbios no magnetômetro, causados por elementos ferromagnéticos. O terceiro teste foi realizado com uma grande variação na aceleração com o objetivo de criar pertubações na leitura do acelerômetro. Três modificações do algoritmo foram testadas, afim de verificar o efeito de cada parte do mesmo no resultado final. A primeira considerando o O2OQ sem um seletor de vetor e o filtro Kalman. O segundo modo é com o O2OQ combinado com o KF sem o seletor de vetor. E o terceiro é com o O2OQ com o filtro Kalman e o seletor de vetor. Em todos os casos os resultados das 3 modificações apresentaram resultados diferentes, sendo que a melhor estimativa é apresentada considerando o terceiro modo do estimador.
Brigante et al. (2011) apresenta o desenvolvimento de um sistema para ras-treamento de movimentos corpo humano em tempo real, consistindo de uma matriz MARG de sensores e um microcontrolar de 32 bits chamado de iNEMO. O protocolo de comunicação wireless utilizado foi o ZigBee desenvolvido para ter um menor con-sumo de potência. A matriz de sensores formada por um LSM303DLH que é chip com acelerômetro de valor aceleração máxima selecionável podendo ser de ±2g/±4g/±8g/ e um magnetômetro com uma faixa dinâmica de ±1.3G á ±8.1G, ambos de 3 eixos de leitura. No qual os dados de ambos foram enviados utilizando o protocolo I2C. Os giroscópios implementados foram o LY330ALH de um eixo e o LPR5430AL de dois eixos, com alcance de ±300 deg/s e largura de banda de 140 Hz com sensibilidade de 3.3mv/deg/s. Cada módulo do dispositivo era fixado em pontos-chaves do corpo humano e os dados enviados para uma unidade coletora que poderia armazena-lós em um microSD ou envia-los para um computador.
O filtro de fusão de dados implementado foi um EKF e a orientação no espaço foi descrita em forma de quatérnios.O sistema foi validado comparando-o com um sistema comercial o MTx 3DOF Motion Tracker feito pela Xsens. Os dados foram coletados em experimentos que replicavam o movimentos cotidiano dos seres humanos. Os resultados mostraram que o sistema tem um comportamento bastante similar ao comercial divergindo no inicio por causa EKF. Segundo os autores mostrou-se suficiente preciso para o rastreamento de movimentos do corpo humano em tempo real, ainda possuindo um consumo de potência menor de 330mW em comparação aos 360mW do MTx e os 500mW de um sistema Inertiacube3 da Intersense.
Kong et al. (2013) desenvolveram um sistema miniaturizado para captar os movimentos do corpo humano chamado de WB-4R desenvolvido para tratamentos de reabilitação remoto. O sistema tem como principal característica seu tamanho,
possuindo apenas 17 mm x 20 mm x 8mm com 3.9g de massa. Possui um acelerômetro LIS331DLH com alcance de ±8G resolução de 3.9mg/bit e largura de banda de 500 Hz, giroscópio LYPR540AH com ±1600 deg/s com resolução de 28 bits em uma largura de banda de 160 Hz, magnetômetro HMC5843 com ±4G de alcance, 12 bits de resolução e 50 Hz de largura de banda. Todos com 3 eixos de medidas. O filtro implementado foi EKF com orientação escrita como quatérnios.
No trabalho de Cavallo et al. (2014) uma comparação experimental entre o EKF e o filtro proposto por Madgwick, Harrison e Vaidyanathan (2011) e o e o filtro complementar de Mahony, Hamel e Pflimlin (2008) Com o objetivo de determinar qual abordagem apresenta os resultados mais precisos e menor custo computacional. Para coletar os dados foi utilizado um giroscópio ST-L3GD20 com 16 bits de resolução e alcance de selecionável de ±250◦/s, ±500◦/s, ±2000◦/s e saída nos protocolo I2C
e SPI. O acelerômetro possui alcance de ±2g, ±4g, ±8g e ±16g. O magnetômetro possui faixa de operação entre ±1.3G até ±8.1G. Ambos encapsulados em único CI chamado de ST-LSM303DLHC de 16 bits e protocolo de comunicação I2C. Além de um robo KUKA Youbot, no qual os ângulos das juntas do mesmo serviram como referência para o experimento.
Para avaliar a precisão de todos os algoritmos, os dados dos sensores foram enviados para o computador que implementava os filtros através do Matlab no ambiente computacional Simulink. Onde o EKF teve um melhor resultado entre os filtros seguido do filtro de Madgwick e do filtro de Mahony. Outro experimento tinha como objetivo avaliar o peso computacional de cada um dos filtros, para isso foram implementados em um STM32F303VCT6 ARM-Cortex M4, onde observou-se que o filtro de Mahony tem o menor tempo para executado seguido de madgwick e o EKF.
Pierleoni et al. (2014) propuseram um sistema para detectar quedas de pessoas idosas utilizando uma matriz MARG de sensores e um microcontrolador que implementa o filtro desenvolvido por Madgwick, Harrison e Vaidyanathan (2011) numa frequência de 50 Hz. O módulo foi desenhado para ficar preso no cinto do individuo. Consiste em um acelerômetro ADXL345 de ±16g com 4mg/bit de resolução, um giroscópio ITG-3200 com alcance de ±2000◦/s e resolução de 14.375◦/se um magnetômetro com alcance
de ± 8G e 4mG/bit de resolução além de um microcontrolador ATmega328 .
Dois protocolos foram Utilizados para avaliar as quedas o de Kangas e o de Noury. No qual 10 indivíduos (8 homens e 2 mulheres) realizaram os experimentos. No protocolo de de Kangas foram realizados 540 testes e no de Noury 420. Os autores avaliavam falsas quedas também para ver se o sistema proposto era capaz de distinguir las das reais. O algoritmo teve uma precisão de 100 % no protocolo de Kangas e 90 % no protocolo de Noury. Os detalhes dos experimentos estão descritos no artigo.
No trabalho de Zhang, Jin e Zhang (2017) o filtro proposto por Madgwick, Harrison e Vaidyanathan (2011) e Mahony, Hamel e Pflimlin (2008) foram implementados a fim de verificar a acurácia de ambos para rastrear movimentos do corpo humano. Para os testes foi utilizado a MPU-6050 que possui um acelerômetro e magnetômetro de três eixos. Um giroscópio HMC5883L de três eixos. Comunicando-se com um MCU MKL252128VFT4 através de I2C.
Foram realizados três testes afim de verificar a acurácia das metodologias. O primeiro com o sistema estático, o segundo com ele movendo-se a 11.25◦/s de velocidade afim de simular movimentos de baixa velocidade e o terceiro com 45◦/s para simular movimentos rápidos. Os três testes foram realizados prendendo-se os sensores sobre um motor de passo 42BYGH47, variando as posições do mesmo de -180◦ até 180o◦ . Para verificar em relação aos três eixos (x,y,z) a orientação do motor
de passo foi alterada. Os resultados de ambos os filtros foram considerados bons com erro RMS <0.5◦. Os autores ressaltaram que o filtro de Mahony, Hamel e Pflimlin (2008)
3
GRADIENTE DESCENDENTE ALGORITMO
3.1 ÁLGEBRA DOS QUATÉRNIOS
Nessa seção uma breve introdução a álgebra dos quatérnios é descrita baseada em Kuipers (1999). Um quatérnio consiste em um número complexo de 4 dimensões, um escalar (q1) e 3 complexas (q2, q3, q4), estando representado na equação 13.
q = [q1 q2 q3 q4] (13)
O complexo conjugado denominado q∗ é descrito na equação 14.
q∗ = [q1 − q2 − q3 − q4] (14)
O módulo de um quatérnio é dado pela equação 15. Quando o quatérnio é unitário o mesmo possui módulo igual a 1, como pode ser observado na equação 16 e é denotado por ˆq.
|q| =qq2
1+ q22+ q32+ q42 (15)
|ˆq| =qq2
1+ q22+ q23 + q42 = 1 (16)
Um quatérnion é dito puro quando possui a a componente escalar é igual a zero (equação 17).
q = [0 q2 q3 q4] (17)
A multiplicação de um quaternion a e b é dado pela equação 18.
ˆ a ⊗ ˆb = [a1 a2 a3 a4] ⊗ [b1 b2 b3 b4] = a1b1 − a2b2− a3b3− a4b4 a1b2+ a2b1+ a3b4+ a4b3 a1b3− a2b4+ a3b1+ a4b2 a1b4+ a2b3+ a3b2+ a4b1 T (18)
Um vetor é equivalente a um quatérnio puro, ou seja, com componentes escalar igual a zero. É possível realizar a rotação de um vetor utilizando quatérnios. Por exemplo, um vetor x é rotacionado por um quatérnio unitário ˆq resultando em um vetor v’ através da equação 19. A rotação é realizada em torno de um vetor r (Equação 20), em um ângulo θ, o quatérnio unitário equivalente a essa rotação é expresso pela equação 21 v0 = ˆq ⊗ x ⊗ ˆq∗ (19) r = [rx ry rz] (20) ˆ q = [q1 q2 q3 q4] = " cosθ 2 − rxsin θ 2− rysin θ 2 − rzsin θ 2 # (21)
Portanto, é possível representar rotações na forma de um quatérnio, esse con-ceito é fundamental para a implementação do algoritmo descrito na seção 3.2
3.2 EQUACIONAMENTO DO ALGORITMO
Nessa seção estão apresentadas equações necessárias para a implementação do algoritmo desenvolvido por Madgwick, Harrison e Vaidyanathan (2011), que estão mostradas em detalhes em Madgwick (2011). O filtro estima a orientação do dispositivo baseando-se na velocidade angular, direção do campo magnético e na direção da gravidade.
As leituras realizadas pelo giroscópio triaxial em um eixo de coordenadas do sensor (S) pode ser representado através de um quatérnio puro, descrito na equação 22. A taxa de variação da orientação do eixo de coordenadas da terra(E) em relação ao eixo de coordenadas do sensor (S
Eq) pode ser descrito pela equação 23.ˆ
Sω = [0 ω x ωy ωz] (22) S E˙q = S Eqˆ 2 ⊗ S ω (23)
Dessa forma, a orientação de E relativo a S no tempo t pode ser descrito como na equação 24. Consistindo no valor da orientação anteriormente estimada mais a variação da orientação que ocorreu em um determinado intervalo de tempo ∆t.
S
Eqω,t =SE qˆest,t−1+SE ˙qω,t∆t (24)
As equações anteriores mostram a obtenção da orientação a partir do giroscópio. Para obter a orientação a partir do acelerômetro e do magnetômetro é assumido a premissa de que se um campo é conhecido no eixo de coordenadas da terra, uma medida desse campo no eixo de coordenadas do sensor irá obter a orientação do sensor em relação ao eixo de coordenadas da terra. No qual uma leitura não irá fornecer apenas uma orientação estimada e terá infinitas soluções possíveis. Assim a orientação pode ser alcançada através da formulação de um problema de otimização, onde a orientação do sensorS
Eq é tal que alinha a um referência predefinida do campoˆ
nos eixos de coordenadas da terraEd com a direção do campo mensurada no eixoˆ
de coordenadas do sensor Ss. Assim a função custo é determinada pela equaçãoˆ
25, e S
Eq pode ser determinado através da equação 26 (MADGWICK; HARRISON;ˆ
VAIDYANATHAN, 2011).
f(SEq,ˆ Ed,ˆ Sˆs) =SE q ⊗ˆ E d ⊗ˆ SE qˆ∗−sˆs (25)
min f(SEq,ˆ Ed,ˆ Sˆs) (26)
No qualS
Eq é definido na equação 27,ˆ Ed na equação 28 eˆ Ss na equação 29.ˆ
S Eq = [qˆ 1 q2 q3 q4] (27) Sd = [0ˆ d x dy dz] (28) Sˆs = [0 sx sy sz] (29)
Nesse algoritmo foi utilizado o gradiente descendente apresentado anteriormente. Para um número de n interações descrito na equação 30 para estimar a orientação
S
S Eqk+1 = S E qˆk− µ ∆f(S Eqˆk,Ed,ˆ Ss)ˆ ∆f( S Eqˆk,Ed,ˆ Ss)ˆ , k = 0, 1, 2...n (30) ∆f(SEqˆk,Ed,ˆ Sˆs) = JT (SEqˆk,Ed,ˆ Sˆs)f(S Eqˆk, Ed,ˆ Ss)ˆ (31)
Assumindo que a direção do campo gravitacional terrestre tem direção conhe-cida em z denominado Eg definida na equação 32 e que a leitura do acelerômetroˆ
normalizada (Sa) definida na equação 33 é a direção do campo medida no eixo deˆ
coordenadas do sensorSs. A função custo e a Jacobiana resultantes estão mostradasˆ
nas equações 34 e 35 respectivamente.
Eg = [0ˆ 0 0 1] (32) Sa = [0ˆ a x ay az] (33) fg(SEq,ˆ Sa) =ˆ 2(q2q4− q1q3) − ax 2(q1q2 + q3q4) − ay 2(1 2− q 2 2 − q32) − az (34) Jg(SEq) =ˆ −2q3 2q4 −2q1 2q2 2q2 2q1 2q4 2q3 0 −4q2 −4q3 0 (35)
Para o campo magnético pode-se assumir que o mesmo tem componentes ape-nas na direção x e z. Assim substituindoSˆs pela leitura do magnetômetro normalizada sm eˆ Ed porˆ Eb, descritos pelas equações 36 e 37 respectivamente, resulta na funçãoˆ
custo descrito pela equação 38 e a Jacobiana definida pela equação 39.
sm = [0ˆ
mx my mz] (36)
Eb = [0ˆ b
fb(SEq,ˆ Eb,ˆ Sm) =ˆ 2bx(0.5 − q32− q24) + 2bz(q2q4− q1q3) − mx 2bx(q2q3− q1q4) + 2bz(q1q2+ q3q4) − my 2bx(q1q3 + q2q4) + 2bz(0.5 − q22 − q23) − mz (38) Jb(SEq,ˆ Eb) =ˆ −2bzq3 −2bxq4 + 2bzq2 2bxq3 2bzq4 2bxq3+ 2bzq1 2bxq4− 4bzq2 −4bxq3 − 2bzq1 2bxq2+ 2bzq4 2bxq1− 4bzq3 −4bxq4+ 2bzq2 −2bxq1 + 2bzq3 2bxq2 T (39)
Com a implementação de uma MARG de sensores a função custo é descrita pela equação 40 e a Jacobiana pela equação 41. Como consequência a superficie definida por fg,b(SEq,ˆSˆa,Eb,ˆ Sm). Tem um minimo definido por um ponto (MADGWICK;ˆ
HARRISON; VAIDYANATHAN, 2011) e gradiente 5f definido pela equação 42
fg,b(SEq,ˆ Sˆa,Eb,ˆ Sm) =ˆ fg(SEq,ˆ Sa)ˆ JTg(S Eq,ˆ Eb)ˆ (40) Jg,b(SEq,ˆ Eb) =ˆ JTg(S Eq)ˆ JTb(SEq,ˆ Eb)ˆ (41) 5f = JTg(SEqˆest,t−1)fg(SEqˆest,t−1,Sˆat) JTg,b(S Eqˆest,t−1,Eb)fˆ g,b(SEqˆest,t−1,Sˆa,Eb,ˆ Sm)ˆ (42)
Para essa aplicação segundo o autor aceitável realizar apenas uma interação por tempo de amostra, tendo como consequência uma menor carga computacional para executar o algoritmo. Combinando a estimativa da orientação baseada na leitura do giroscópio com a orientação oriundo do gradiente descendente algoritmo que processa os dados do acelerômetro e magnetômetro e aplicando algumas simplificações definidas pelo autor, resulta na orientação estimada definida pela equação 43, que leva em consideração a taxa de variação da orientação definida em 44 e o erro de orientação da taxa de variação definida na equação 45.
S
Eqˆest,t =SE qˆest,t−1+SE ˙qest,t∆t (43)
S E˙qest,t = S E ˙qω,t − β S E˙ˆq,t (44)
S E˙ˆq,t =
5f
||5f|| (45)
No qual β representa os erros de média zero do giroscópio tais como ruídos, erros de quantização, erros de calibração entre outros erros que podem denegrir a qualidade do sinal do giroscópio. Pode ser calculado pela equação 46, no qual ˜ωβ
representa os erros estimados de média zero do giroscópio.
β =
s
3
4ω˜β (46)
O cálculo do bias do giroscópio é realizado através da equação 47. No qualSω ,t
é calculado através da equação 48, e ζ é um ganho calculado pela equação 49, e ˙ωζ é
o valor estimado do bias em cada eixo.
S ωb,t= ζ S X t ω,t∆t (47) S ω,t = 2ESqˆ ∗ est,t−1⊗ S E ˙ˆq,t (48) ζ = s 3 4ω˜˙ζ (49)
O valor compensado da leitura do giroscópio, portanto, deve ser usado no lugar da leitura do giroscópio, e está definido na equação 50.
S
ωc,t=S ωt−Sωb,t (50)
A direção do campo magnético terrestre é alterado pela presença de estruturas metálicas, causando problemas de inclinação do campo. A correção desse efeito pode ser realizada se a direção de referência do campo magnético possui a mesma inclinação. Para alcançar isso pode-se calcular Ebˆ
t em função de Ehˆt descrito na
equação 51 normalizado para ter componentes na direção do eixo terrestre x e z como mostrado na equação 52.
Ehˆ
Ebˆ t= h 0 qh2 x+ h2y 0 hz i (52) Essa abordagem eliminada a necessidade que a direção de referência necessite ser previamente determinada.
4
RESULTADOS
4.1 DESENVOLVIMENTO DO PROTÓTIPO
O protótipo é constituído de um microcontrolador que recebe os dados de ace-leração, velocidade angular e intensidade do campo magnético em no sistema de coordenadas perpendiculares x,y,z do sensor e processa esses dados para converter na orientação desse sistema de coordenadas em relação ao eixo de coordenadas da terra. Dessa forma, nessa seção é apresentado o protótipo desenvolvido, o microcon-trolador e os sensores utilizados.
4.1.1
Microcontrolador
O microcontrolador utilizado para esse projeto foi o STM32L412CBT6. Esse microcontrolador possui núcleo ARM Cortex-M4 de 32 bits de alta performance e baixo consumo (ultra low power ). O microcontrolador possui memória flash de 128 KB, clock de no máximo 80 MHz e 48 pinos, suficientes para a aplicação. No qual o diagrama simplificado do microcontrolador está ilustrado na Figura 9.
Figura 9: Diagrama do stm32L412CBT6
Na Figura 9 pode-se observar as principais características do microcontrolador. Possui duas interfaces SPI, 3 interfaces I2C e 3 interfaces USART necessários para
comunicação entre dispositivos. Tanto como para sensores como para dispositivos que farão uso dos dados processados pelo microcontrolador. Também apresenta DMA (direct memory acess) e FPU (floating point unit), que são periféricos úteis para a transmissão e processamento de dados relacionados a sistemas de detecção de orientação inerciais.
4.1.2
Magnetômetro
O magnetômetro utilizado foi o QMC5883L que consiste em um magnetômetro Triaxial de baixo custo. É um sensor com uma interface digital que utiliza o protocolo I2C para configuração e transmissão dos dados com uma velocidade máxima de 400
kHz, possui um alcance ajustável de ±2g ou ±8g um conversor analogico digital (AD) de 16 bits. Com esse magnetômetro é possível obter a orientação do campo magnético terrestre servindo como entrada para o filtro digital aplicado posteriormente.
A tensão de alimentação do sensor pode ser de 2.16V até 3.6V, consumo de corrente de 3 µA em standby e com o máximo de 850 µA em modo de condução continuo. Com essas características o sensor apresenta um baixo consumo de energia, importante para dispositivos moveis alimentados por bateria.
A saída do sensor é em 16 bits para cada um dos 3 eixos com valor proporcional a densidade de campo magnético, que deve ser multiplicado por um ganho fornecido pelo datasheet para obter-se o valor em Gauss (G). Os dados referentes as grandezas mensuradas são armazenados em 6 registradores de 8 bits, onde cada par contém a informação referente a um eixo (x,y ou z). A taxa de atualização desses dados no registrador pode ser configurado em 10 Hz, 50 Hz, 100 Hz ou 200 Hz, dependendo da necessidade da aplicação.
O encapsulamento desse sensor possui um tamanho de 3 mm de largura e 3 mm comprimento e 16 pinos a serem soldados. Dessa forma, torna-se um desafio para o projeto da placa de circuito impresso (PCI) e para soldagem do componente por consequência das limitações do método utilizado para a confecção da placa. Por isso optou-se por utilizar um módulo chamada de GY-273 que está ilustrada na Figura 10.
Figura 10: Módulo GY-273
Fonte: Disponível em <https://robotica.store> Acesso em nov. 2019
Esse módulo possui os pinos de alimentação e de comunicação que podem ser soldados na placa através da utilização de barra de pinos. Também possui os resistores pull up necessários para a comunicação I2C e um regulador de tensão de alimentação de 5V para 3.3V que é um valor adequado para a alimentação desse sensor. Dessa forma, simplificando o interfaceamento do mesmo com o microcontrolador.
4.1.3
Unidade de medida inercial
A unidade de medida inercial utilizada (IMU - inertial measurement unit) foi a MPU - 6050, constituída de um acelerômetro e giroscópio triaxial de baixo custo. Possui uma interface digital com protocolo I2C com velocidade máxima de 400 kHz. Possui
alcance ajustáveis para o giroscópio de ±250◦, ±500◦/s, ±1000◦/s e ±2000◦/s e alcance ajustável de ±2g, ±4g, ±8g e ±16g para o acelerômetro. Assim, o acelerômetro pode ser utilizado para medir a direção do campo gravitacional, servindo como entrada do algoritmo para estimar a orientação.
A saída do sensor é em 16 bits para cada um dos 3 eixos do giroscópio e do acelerômetro com valor proporcional a velocidade angular e aceleração respectivamente. Assim como o magnetômetro deve ser multiplicado por um ganho fornecido no datasheet para obter o valor do giroscópio em◦/s e do acelerômetro em g. A taxa de atualização de dados é de 4 Hz até 8 kHz no acelerômetro de 4 Hz até 1 kHz.
O circuito integrado possui 24 pinos e tamanho de 4mm de largura por 4 mm de comprimento. Dessa forma, pelos mesmos motivos citados na seção anterior foi optado por utilizar uma placa de desenvolvimento chamada GY -521, mostrada na Figura 11.
Figura 11: Módulo GY-521
Fonte: Disponível em <https://www.embarcados.com.br> Acesso em nov. 2019
Assim como no GY-273 possui os pinos de alimentação e comunicação I2C além
dos pinos de comunicação I2C auxiliares (XDA e XCL), AD0 que pode ser utilizado para alterar o bit menos significativo do endereço da mpu-6050 e o pino de interrupção, que pode ser configurado para gerar interrupções quando novos dados estiverem disponíveis. Possui os resistores de pull up para a comunicação I2C e um regulador
de tensão de 5V para 3.3V. Dessa forma, não necessitando de elementos adicionais externos para o funcionamento do sensor.
4.1.4
Protótipo
No apêndice A está o esquemático do protótipo desenvolvido, composto pelo STM32L412CBT6, pelo magnetômetro QMC5883l e pela MPU-6050, também pelos circuitos auxilares para o funcionamento do microcontrolador, conectores para alimenta-ção e transmissão de dados. Para auxiliar na depuraalimenta-ção do código foram acrescentados 3 LEDs e um botão bem como um conector para captação do sinal do I2C dos sensores,
afim de verificar se o mesmo está funcionando adequadamente.
A imagem 3D da placa de circuito impresso projetada do protótipo está ilustrado na Figura 12 e possui duas camadas com comprimento de 76 mm e 42 mm de largura.
Figura 12: placa
Fonte: Autor (2019)
A confecção do protótipo foi realizada utilizando-se o método de transferência térmica e o resultado está ilustrado na Figura 13.
Figura 13: prototiponovo
Fonte: Autor (2019)
O protótipo desenvolvido possui todos os elementos essenciais para implementa-ção no algoritmo de estimaimplementa-ção da orientaimplementa-ção, bem como a possibilidade de transmissão dos dados de saída através do protocolo I2C para que possam ser utilizados por outro
dispositivo. Possibilitando dessa forma, que seja aplicado em sistemas de detecção da orientação das articulações do corpo humano.
4.2 SOFTWARE IMPLEMENTADO
Após desenvolvimento e teste elétrico do protótipo, as bibliotecas necessárias para a configuração e comunicação com a MPU-6050 e QMC-5883l foram criadas e estão exibidas no Apêndice B e C respectivamente. Essas bibliotecas possuem as rotinas para configuração, leitura e calibração dos sensores.
Para a calibração do giroscópio da MPU-6050, foi executada uma rotina para determinar o desvio estático do sensor, ou seja, quando o mesmo está imóvel , para posteriormente remove-lo da leitura do sensor. Para executar a rotina é necessário inserir o desvio permitido e o número de amostrar para cada iteração.
Durante a execução da rotina, na primeira iteração é salvo a media das amostras obtidas dentro de um variável que é posteriormente removida das novas leituras. Em seguida, é realizada uma comparação com o valor da nova leitura subtraído do valor armazenado anteriormente verificando se o resultado é menor que o desvio permitido, se não for, novas amostragens são realizadas e o valor médio das leituras são somadas na variável, isso irá se repetir até que o valor das leituras esteja seja menor ou igual que o desvio permitido. Essa rotina de calibração foi implementada para evitar problemas na convergência inicial do algoritmo implementado, o desvio tende a variar e função do tempo, porém o filtro implementado possui uma rotina para remove-lo como discutido no capitulo 3.
O comportamento ideal de um sensor magnético quando plotado um eixo em função de outro é resultar em um circulo com centro na origem, porém devido a in-terferências externas os sensores não apresentam esse comportamento. Podendo ter distorções quanto a forma resultando em leituras elipsoidais e deslocamento do centro em relação a origem. O filtro proposto possui rotinas afim de minimizar os efeitos causados pela distorção elipsoidal do sensor. Portanto, foi necessário uma rotina para diminuir os efeitos de interferencias externas que causam offsets na leitura dos sensores A rotina implementada realiza uma média da diferença entre a leitura minima e máxima do sensor quando o mesmo é rotacionado em todos os sentidos. Nesse processo são feitas 300 amostras a cada 200ms e o resultado é armazenado em uma variável que é posteriormente subtraído da leitura dos sensores.
O código implementado para estimar a orientação está mostrado no Apêndice D. Nesse programa é inicialmente é realizada a configuração da MPU-6050 e do QMC-5883l. A MPU-6050 foi configurada com o oscilador interno de 8Mhz como fonte de clock, taxa de saída de dados em 8 kHz, alcance do acelerômetro de ±4g e alcance do
giroscópio em ±500◦/s. O QMC-5883l foi configurado com taxa de saída de dados em 200 Hz e alcance de ± 2 Gauss.
Após a configuração, a rotina de calibração do giroscópio é realizada com o módulo estático em que foi definido 100 amostras devido ao tempo de execução ser baixo e erro ficar na casa dos centésimos suficientes para a aplicação, e e um desvio permitido de 0,01◦/s que se mostrou efetivo para corrigir o desvio do giroscópio com um tempo de execução relativamente baixo após a calibração estar pronta um LED indicativo é acesso, informando que o mdulo pode ser movimentado. Essa rotina de calibração é implementada toda vez que o código é inicializado devido ao desvio do osciloscópio ser bastante sensível a temperatura. O mesmo não foi realizado no magnetômetro pois o mesmo não possui grande variação no desvio de uma execução pra outra.
Após a calibração o laço de repetição é executado, sendo inicialmente realizado a leitura dos sensores. Os dados do giroscópio então são convertidos de graus por segundo para radianos por segundo pois o equacionamento do algoritmo leva em consideração a velocidade angular em radiano por segundo. Subsequentemente o algoritmo é executado onde os quatérnios unitários que representam a orientação do protótipo em relação a terra é calculado. Por ultimo os valores dos quatérnions são convertidos em ângulos de guinagem (ψ), arfagem(θ) e rolagem(φ) (rotações em Z,Y e X ) através das equações 53, 54 e 55, pois são mais fáceis de serem interpretados graficamente. ψ = Atan 2q2q3− 2q1q4 2q2 1+ 2q22− 1 ! (53) θ = −sin−1(2q2q4+ 2q1q3) (54) φ = Atan 2q3q4− 2q1q2 2q2 1 + 2q24 − 1 ! (55)
O programa foi executado considerando o erro do giroscópio em 3◦/s e desvio
4.3 ESTIMATIVA DA ORIENTAÇÃO E DISCUSSÃO
Afim de verificar o funcionamento do algoritmo implementado foi realizado dois testes. No primeiro cada um dos eixos do dispositivo é alinhado com o eixo da terra afim de que a saída da orientação desse eixo seja zero. Posteriormente o protótipo é movimentado em ângulos de +90◦ e −90◦ afim de verificar se a orientação estimada converge para os valores supracitados e se o tempo de convergência é coerente para a aplicação. No segundo teste o dispositivo é colocado em ângulos de 90◦ por alguns segundos e posteriormente colocado em -90◦ por alguns segundos novamente, esse teste tem o objetivo de verificar a estabilidade da orientação ao longo do tempo.
O resultado obtido para rotação em torno do eixo Z está exibido na Figura 14. O gráfico foi obtido através da ferramenta STMstudio fornecido pela STMicroelectronics, que possibilita a aquisição de dados e plotagem em tempo real através da ferramenta de depuração do microcontrolador.
Figura 14: Resultados para a rotação em torno do eixo Z do protótipo
Fonte: Autor (2019)
Observa-se que o ângulo estimado está convergindo para os valores que fo-ram utilizados como referências nesse teste, mencionados anteriormente. No gráfico também é possível verificar que o tempo de transição entre ±90 ◦ não é constante isso se deve ao fato que o experimento não foi realizado utilizando uma velocidade angular fixa, resultando nas diferenças entre das transições da forma de onda. Durante o experimento observou-se também que a convergência para o valor estimado não
apresentou atrasos que poderiam resultar em valores errados na estimativa do ângulo. Na Figura 15, está o resultado para o segundo experimento em que a posição de ±90◦ é mantida constante por alguns segundos. Observa-se que o valor do ângulo
no gráfico manteve-se constante durante o tempo que foi mantido nessa posição, isso demonstra que o ângulo estimado possui estabilidade.
Figura 15: Resultados para a rotação em torno do eixo x do protótipo
Fonte: Autor (2019)
Na Figura 16 e 17 estão os resultados dos dois experimentos para o eixo x. Para esse eixo o algoritmo também foi capaz de convergir para os ângulos de referência, e possui uma dinâmica que permite a convergência rápida na estimativa da orientação. Mantendo-se a posição constante o algoritmo foi capaz de manter estável a orientação correta.
