Filtro complementario: fusión de acelerómetro y giróscopo

Introducción

Tienes un acelerómetro que acierta en promedio pero se equivoca ahora mismo, y un giróscopo que acierta ahora mismo pero se equivoca con el tiempo. El filtro complementario es la observación de que estos dos perfiles de error son opuestos, y que por tanto puedes tomar la mitad buena de cada uno.

Es una línea de aritmética. También es, para un robot autoequilibrado, casi siempre la respuesta correcta —un filtro de Kalman te compra una pequeña mejora de precisión a un gran coste en complejidad y tiempo de CPU en un microcontrolador de 8 bits.

La idea

Cada sensor es fiable en una banda de frecuencias distinta.

• El acelerómetro es fiable en baja frecuencia. Promediado durante un segundo informa correctamente dónde está abajo; muestreado durante 10 ms informa sobre todo del propio movimiento del robot.
• El giróscopo es fiable en alta frecuencia. En 10 ms su tasa integrada es esencialmente exacta; en un minuto el sesgo acumulado la ha arruinado.

Así que: paso bajo al acelerómetro, paso alto al giróscopo, y súmalos. Los filtros se diseñan para que sus respuestas sumen exactamente uno en cada frecuencia —se complementan, de ahí el nombre. Ninguna banda se cuenta dos veces y ninguna se pierde.

La implementación

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;
}

Esa es todo el filtro. Léelo como: toma adónde dice el giróscopo que nos hemos movido desde la última vez, y acércalo un 2 % hacia lo que el acelerómetro afirma ahora.

El término del giróscopo aporta reactividad. El pequeño término del acelerómetro aporta una corrección lenta y permanente que evita que se acumule la deriva del giróscopo —tira constantemente de la estimación de vuelta hacia la verdad física, con suavidad suficiente para que los picos transitorios de aceleración apenas la muevan.

Elegir la constante de tiempo

ALPHA no es un mando arbitrario. Corresponde a una constante de tiempo:

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

τ es la frontera entre las bandas. Los cambios más rápidos que τ vienen del giróscopo; más lentos que τ, gana el acelerómetro. Con un bucle de 5 ms y ALPHA = 0.98, τ = 0,245 s —aproximadamente un cuarto de segundo de autoridad del giróscopo antes de que el acelerómetro empiece a reafirmarse.

Fíjate en lo que implica la fórmula: ALPHA depende de tu tasa de bucle. Copia un valor de un proyecto a 100 Hz a tu bucle de 500 Hz y habrás hecho tu filtro cinco veces más lento en silencio. Si cambia tu temporización de bucle, recalcula ALPHA desde el τ que realmente quieres en lugar de conservar el número.

La temporización tiene que ser real

dt debe ser el tiempo que realmente transcurrió, no el que pretendías que transcurriera:

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

Fijar dt = 0.005 porque llamaste delay(5) es un atajo común que envenena en silencio el resultado —la sola lectura I²C tarda cerca de un milisegundo, así que tu bucle real es de 6 ms y cada integración queda un 17 % corta. El ángulo entonces deriva de una forma que se parece exactamente al sesgo del giróscopo y no puede calibrarse.

Mejor aún, ejecuta el bucle desde un temporizador hardware a tasa fija en lugar de delay(). Un bucle de equilibrio necesita temporización consistente mucho más que una tasa concreta.

Acertar con los signos

El fallo más común con diferencia es una discordancia de signos entre ambas fuentes. Si el acelerómetro informa +3° para una inclinación adelante y el giróscopo informa una tasa negativa para el mismo movimiento, ambos términos luchan: la salida del filtro irá con retardo, vibrará y se asentará en un valor erróneo, y ninguna sintonía de ALPHA lo arreglará.

Compruébalo directamente. Imprime accelAngle y la integral en marcha de gyroRate lado a lado, luego inclina el robot lentamente adelante a mano:

• Ambos deberían moverse en la misma dirección.
• Ambos deberían alcanzar aproximadamente la misma magnitud.

Si se mueven en opuesto, niega la tasa del giróscopo. Si difieren en 90°, estás leyendo los ejes equivocados del acelerómetro para el plano en que cae tu robot.

¿Por qué no un filtro de Kalman?

Un filtro de Kalman es la respuesta estadísticamente óptima si conoces las características de ruido de tus sensores —y rinde algo mejor, sobre todo durante aceleración sostenida.

Para un robot equilibrista la diferencia rara vez lo justifica. El filtro complementario cuesta un multiplicar-sumar, tiene un parámetro sintonizable con un significado físico claro, y falla de formas que puedes diagnosticar imprimiendo dos números. Un filtro de Kalman cuesta aritmética de matrices cada ciclo, tiene una matriz de covarianza que sintonizar, y cuando se porta mal la razón es genuinamente difícil de ver.

Haz que el robot se equilibre primero con un filtro complementario. Si se equilibra bien, tienes tu respuesta. Hay un tratamiento más amplio de los compromisos en ruido de sensores, filtrado y fusión.

Verifícalo antes de cerrar el lazo

Dos comprobaciones atrapan casi todo, y ambas llevan un minuto:

Mantenlo quieto dos minutos. El ángulo filtrado debería quedarse quieto. Si se arrastra, tu calibración de sesgo del giróscopo está mal —el término del acelerómetro debería estar evitando justo esto, así que el arrastre significa que ALPHA es demasiado alta para tu sesgo.

Agítalo sin girarlo. Mueve el robot de un lado a otro manteniéndolo vertical. Solo el acelerómetro oscilará salvajemente; el ángulo filtrado apenas debería moverse. Si el ángulo filtrado oscila también, ALPHA es demasiado baja y estás confiando en el acelerómetro durante la aceleración —justo la situación en que es peor.

Una vez que el ángulo se mantiene quieto cuando está quieto y se mantiene honesto cuando lo agitas, es lo bastante bueno para equilibrarse sobre él. Cierra el lazo en control PID en cascada, u observa un lazo sintonizado funcionar en el simulador de equilibrio. La misma forma de filtro —una señal que deriva pero es suave, corregida por una ruidosa pero acotada— es como un robot de exterior convierte una corrección GPS de 1 Hz en una posición utilizable.