domingo, octubre 21, 2012

4copter: Fusión de sensores (I) -Filtros Complementarios

Uno de los primeros problemas que nos encontramos a la hora de intentar estabilizar un UAV es la necesidad de conocer la posición, la orientación (inclinación) y la velocidad de giro actual.

Existen múltiples sensores que pueden ayudar a conocer estos datos, los sistemas de navegación satelital (GPS, GLONASS, próximamente GALILEO, ...), giroscópios, acelerómetros, barómetros, brújulas magnéticas o magnetrómetros, etc. Las señales de estos sensores, en primer lugar hay que procesarlas y luego combinarlas para eliminar los errores y derivas intrínsecos de los sensores y el ruido o las interferencias del sistema.


Para estabilizar el sistema no es necesario conocer la posición exacta sino que es más importante la orientación y las velocidades de giro; esto es debido a que podemos utilizar un algoritmo Proporcional-Derivativo (PD) para el control de los motores. Por ejemplo para un sólo eje podríamos definir el siguiente algoritmo :

$$ Motor Output = K_p \cdot Angle + K_d \cdot Angular Velocity $$
Para obtener estas variables usaremos una combinación de accelerómetro y giroscópio (el magnetrómetro nos podría servir para terminar de refinar los datos por lo que lo podríamos incluir más adelante).

La forma más obvia es utilizar el acelerómetro para medir el ángulo y el giroscópio para medir la velocidad angular.


La ventaja de esto es que no existe procesado ninguno y se obtienen las muestras sin retardo. El problema principal es que el accelerómetro es muy sensible todas las aceleraciones lineales (vibraciones y desplazamientos lineales) por lo que los parámetros de salida serán muy ruidosos y con errores durante los desplazamientos.

Es conveniente filtrar (con un filtro paso bajo) la señal del accelerómetro.


La ventaja de este sistema es que es muy fácil de programar y el filtro elimina los ruidos y las acceleraciones lineales. Sin embargo esto produce un retardo en la percepción del ángulo, lo cual puede provocar problemas en la estabilización.

Sin embargo, podríamos añadir los datos recogidos por el giroscopio para eliminar el retardo:

No podemos utilizar los datos del giroscopio directamente (integrados en el tiempo para pasar de velocidad angular a ángulo) dado que este tiene una deriva temporal que los inutiliza, sin embargo, a corto plazo sirven para eliminar el retardo producido por el filtro aplicado al accelerómetro.

En código se traduce de forma muy fácil:
$$ Angle = \alpha \cdot (Angle + gyro \cdot dt) + (1 - \alpha) \cdot acc $$
A partir de aquí podemos deducir la constante de tiempo para los filtros paso-bajo y paso-alto:
$$ \tau = \frac{ \alpha \cdot dt }{ 1 - \alpha } $$

Esta última técnica se llama Filtros complementarios, y tiene múltiples ventajas:
  • Reduce el ruido y la deriva producido por los sensores
  • Reduce el retardo en la estimación del ángulo
  • No implica un coste excesivo en tiempo de proceso

S2

Ranganok Schahzaman

EDITO: Se ha utilizado una aproximación para definir el ángulo a partir de los datos del accelerómetro y es que para x pequeños:
$$ sin (x) \approx x $$
Dado que de lo que se trata es de estabilizar el sistema este siempre se moverá en ángulos pequeños y la aproximación utilizada será cierta (x<30º)

Fuente: "The Balance Filter. A Simple Solution for Integrating Accelerometer and Gyroscope Measurements for a Balancing Platform". Shane Colton. Mentor, FRC 97. Rev.1: Submitted as a Chief Delphi white paper - June 25, 2007.

4 comentarios:

Paco Martinez dijo...

Hola, navegando por la red y buscando información sobre magnetómetros me he encontrado este blog.
Yo esto metido en un proyecto de robótica en el que necesito conocer el angulo de inclinación lateral(alabeo) con respecto a la vertical, dispongo de un magnetómetro de 3 ejes, pero no se como tratar los datos para obtener el ángulo de alabeo, independientemente del rumbo y la guiñada.

Si tienes información al respecto ruego me la pases.

Ranganok Schahzaman dijo...

Otia, lo pones difícil... Si sólo tienes el magnetómetro es bastante complicado. Primero porque tendrías que corregir los datos recibidos, ya que los campos magnéticos presentes te estan afectando. Por otro lado además tienes que las líneas de campo (magnético terrestre) no son paralelas a la tierra sino que tienen una inclinación (aproximadamente corresponde con la latitud terrestre).

Deberías poder descartar tanto los campos magnéticos como la latitud a la que te encuentras para poder medir entonces el balanceo de tu robot.

Generalmente el descarte de los campos magnéticos suele hacerse fusionando las medidas con las de un accelerómetro (lo explicaré en un post más adelante), con lo cual si ya tienes el accelerómetro puedes medir directamente el balanceo en ese sensor.

S2

Ranganok Schahzaman

あげる見せて dijo...

Tengo una duda como se calculan esos coeficientes ?

puedes dar un ejemplo de como calcular esos coeficientes ?
no se que poner en

dt
y que representa alfa

Ranganok Schahzaman dijo...

dt es el "diferencial de tiempo", básicamente es el tiempo entre muestras -suponiendo que muestreas las señales a una frecuencia constante, lo cual suele ser habitual-.