Filtro complementar: fusão de dados do acelerômetro e giroscópio

Introdução

Você tem um acelerômetro que acerta em média, mas erra no momento atual, e um giroscópio que acerta agora, mas erra com o passar do tempo. O filtro complementar parte da observação de que esses dois perfis de erro são opostos e, portanto, podemos aproveitar a melhor parte de cada um.

É uma única operação aritmética. Também é, para um robô autobalanceador, quase sempre a resposta certa — um filtro de Kalman oferece um pequeno ganho de precisão ao custo de muita complexidade e tempo de CPU em um microcontrolador de 8 bits.

A ideia

Cada sensor é confiável em uma faixa de frequência diferente.

• O acelerômetro é confiável em baixa frequência. Sua média ao longo de um segundo indica corretamente a direção para baixo; quando se integra o sinal por apenas 10 ms, ele informa principalmente o movimento do próprio robô.
• O giroscópio é confiável em alta frequência. Ao longo de 10 ms, a integração de sua taxa angular é praticamente exata; ao longo de um minuto, o viés acumulado a inviabiliza.

Portanto: aplique um filtro passa-baixa ao acelerômetro, um passa-alta ao giroscópio e some os resultados. Os filtros são projetados para que suas respostas somem exatamente um em todas as frequências — eles se complementam, e desse vem o nome. Nenhuma faixa é contada duas vezes nem fica sem cobertura.

A implementação

const float ALPHA = 0.98;
float angle = 0;

void updateAngle(float accelAngle, float gyroRate, float dt) {
  angle = ALPHA * (angle + gyroRate * dt) + (1.0 - ALPHA) * accelAngle;
}

Esse é o filtro inteiro. Leia assim: aplique o deslocamento obtido pela integração da taxa do giroscópio desde a última atualização e dê um passo de 2% na direção do ângulo que o acelerômetro indica no momento.

O termo do giroscópio assegura uma resposta rápida. O pequeno termo do acelerômetro fornece uma correção lenta e contínua que impede o acúmulo da deriva do giroscópio — ele puxa a estimativa de volta à realidade física com suavidade suficiente para que picos de aceleração transitória mal a alterem.

Escolhendo a constante de tempo

ALPHA não é um ajuste arbitrário. Ele corresponde a uma constante de tempo:

τ = (ALPHA · dt) / (1 − ALPHA)

τ marca a fronteira entre as faixas. Mudanças mais rápidas que τ vêm do giroscópio; mudanças mais lentas são dominadas pelo acelerômetro. Com um ciclo de 5 ms e ALPHA = 0.98, τ = 0,245 s — aproximadamente um quarto de segundo de autoridade do giroscópio antes de o acelerômetro voltar a se impor.

Observe o que a fórmula implica: ALPHA depende da frequência do ciclo. Copie um valor de um projeto que roda a 100 Hz para um ciclo de 500 Hz e seu filtro ficará silenciosamente cinco vezes mais lento. Se o tempo do ciclo mudar, recalcule ALPHA a partir do τ realmente desejado, em vez de manter o número.

O tempo precisa ser real

dt deve ser o tempo realmente decorrido, não o tempo que você planejou que decorresse:

unsigned long now = micros();
float dt = (now - lastUpdate) / 1000000.0;
lastUpdate = now;

Definir fixamente dt = 0.005 porque você chamou delay(5) é um atalho comum que corrompe silenciosamente o resultado — somente a leitura I²C leva quase um milissegundo, então seu ciclo real dura 6 ms e cada integração fica 17% curta. O ângulo então sofre deriva de um jeito que parece exatamente um viés do giroscópio e que não pode ser eliminado por calibração.

Ainda melhor, execute o ciclo com um temporizador de hardware em uma frequência fixa, em vez de usar delay(). Um ciclo de balanceamento precisa muito mais de regularidade temporal do que de uma frequência específica.

Ajustando os sinais corretamente

A falha mais comum é uma incompatibilidade de sinal entre as duas fontes. Se o acelerômetro informa +3° para uma inclinação para a frente e o giroscópio informa uma taxa negativa para o mesmo movimento, os dois termos passam a atuar em sentidos opostos: a saída do filtro ficará atrasada, oscilará e estabilizará no valor errado, e nenhum ajuste de ALPHA resolverá isso.

Verifique diretamente. Imprima accelAngle e a integração acumulada de gyroRate lado a lado e incline o robô lentamente para a frente com a mão:

• Os dois devem mudar na mesma direção.
• Os dois devem alcançar magnitudes aproximadamente iguais.

Se mudarem em direções opostas, inverta o sinal da taxa do giroscópio. Se diferirem em 90°, você está lendo os eixos errados do acelerômetro para o plano em que o robô cai.

Por que não usar um filtro de Kalman?

Um filtro de Kalman é a resposta estatisticamente ótima se você conhece as características de ruído dos seus sensores — e ele tem desempenho um pouco melhor, especialmente durante aceleração contínua.

Para um robô balanceador, a diferença raramente justifica seu uso. O filtro complementar custa uma multiplicação e uma soma, tem um único parâmetro ajustável com significado físico claro e falha de maneiras que podem ser diagnosticadas imprimindo dois números. Um filtro de Kalman exige aritmética matricial a cada ciclo, tem uma matriz de covariância para ajustar e, quando se comporta mal, a causa é realmente difícil de ver.

Primeiro, faça o robô se balancear com um filtro complementar. Se ele se balancear bem, você tem a solução. Há uma análise mais completa das compensações em ruído, filtragem e fusão de sensores.

Verifique antes de fechar a malha

Duas verificações detectam quase tudo, e ambas levam um minuto:

Mantenha-o parado por dois minutos. O ângulo filtrado deve permanecer estável. Se variar lentamente, a calibração do viés do giroscópio está incorreta — o termo do acelerômetro deveria impedir exatamente isso, portanto essa variação significa que ALPHA está alto demais para o seu viés.

Sacuda-o sem girá-lo. Mova o robô lateralmente, mantendo-o na vertical. Somente o acelerômetro oscilará violentamente; o ângulo filtrado mal deverá se mover. Se o ângulo filtrado também oscilar, ALPHA está baixo demais e você está confiando no acelerômetro durante a aceleração — justamente a situação em que ele é menos confiável.

Quando o ângulo permanecer estável parado e continuar confiável durante os sacudos, ele será bom o suficiente para o balanceamento. Feche a malha no controle de balanceamento com PID em cascata ou veja um ciclo ajustado funcionando no simulador de balanceamento. O mesmo formato de filtro — um sinal suave, mas sujeito à deriva, corrigido por um sinal ruidoso, mas com valores limitados — é como um robô ao ar livre transforma uma correção GPS de 1 Hz em uma posição utilizável.