Comparando medición de Acelerómetro y gyróscopoAntes de comenzar con la comparación de las mediciones y la posterior fusión de las mismas, hay que recordar un par de cosas.
Lo primero es entender que el gyróscopo mide la velocidad angular del UAV y al integrarla obtenemos el ángulo de gyro. Hay que tener en cuenta que con el gyróscopo obtenemos el ángulo de giro con el Marco de Referencia del UAV.
El acelerómetro, en cambio, mide la inclinacion del UAV respecto de la Gravedad. La cual es fija en el Marco de Referencia de la Tierra. Por lo tanto las mediciones no son compatibles y deberemos realizar alguna pequeña transformación para poder obtener las mediciones en el mismo Marco de Referencia, generalmente, el de la Tierra.
Los giróscopos, al integrar la velocidad angular para obtener el ángulo, va presentando deriva. Es decir, que aunque el sensor esté quieto, se va integrando (acumulando) el ruido. Estos sensores son poco sensibles a las altas frecuencias, por lo que su salida suele ser bien suave aunque hagamos movimientos bruscos.
Los acelerómetros miden el ángulo de giro respecto al vector de la gravedad. El resultado es funcion de funciones senos y cosenos, las cuales tienen zonas más lineales/sensible que otras. Por ejemplo, el seno de ángulos bajos es más sensible al seno de angulos cercanos a 90°. Esto es, la diferencia entre el seno de 10° y el seno de 20° es 0,1579. En cambio la diferencia entre el seno de 70° y el de 80° es 0,045. Por otra parte los acelerómetros son muy sensibles a las altas frecuancias y cualquier vibracion se ve reflejado en la salida de los mismo.
Todo esto esta mejor explicado en este video de youtube:
Bien, si quisiéramos hacer un dispositivo que se oriente unicamente con acelerómetros podríamos poner un buen filtro pasa bajos para eliminar el ruido del acelerómetro. Si bien esto funcionaría, el filtro introducirá un retardo considerable entre el movimiento real y el apreciado a la salida del filtro.
Si quisieramos hacer un sistema que se oriente solo con gyróscopos tendriamos como problema la deriva. Aunque hay sistemas en donde no es tan importante la deriva, como por ejemplo los mouse con tecnología MEMs. Ya que lo que se mide es la orientacion relativa a la posicion anterior... o algo por el estilo...
Bien, podemos ver que cada sensor tiene un fuerte y una devilidad y aprovechando los fuertes de cada uno podemos construir un mejor sistema de orientacion. A esto se le llama fusion de los datos de los sensores. Algoritmos de fuión hay muchos, entre ellos un simple filtro complementario, el filtro de kalman, el de kalman extendido para sistemas no lineales, Madwgik, etc.
Bien, pero antes de fuisionar los datos debemos poder ver las características de cada sensor. Para ello he realizado un nuevo programa en el cual se loguean ambos sensores para luego mostrarlo en Excel.
Mi main.c es el siguiente:
#ifdef __USE_CMSIS
#include "LPC17xx.h"
#endif
#define GYRO_X_SCALE 0.07 //Sensibilidad en cada eje
#define GYRO_Y_SCALE 0.07 //extraído del datasheet
#define GYRO_Z_SCALE 0.07
#define ACEL_X_SCALE 0.0039 //Sensibilidad en cada eje
#define ACEL_Y_SCALE 0.0039 //extraído del datasheet
#define ACEL_Z_SCALE 0.0039
#define SAMPLESS_BIASS 1000 //Número de muestras para calular biass
#define PI 3.1415926
volatile uint32_t usTicks;
float rollGyro=0.0, pitchGyro=0.0;
float pitchAccel=0.0, rollAccel=0.0;
int timeStep = 10000; //Tiempo que tiene que durar el main en uSegfloat x1,y1,z1;
#define sampTime timeStep/1000000 //SampleTime en segundos
float xg, yg, zg, xa, ya, za;
signed short accel_X, accel_Y, accel_Z;
float biasAccelX, biasAccelY, biasAccelZ;
signed short gyro_X, gyro_Y, gyro_Z;
float biasGyroX, biasGyroY, biasGyroZ;
__INLINE static void delay_us (uint32_t delayTicks) {
uint32_t currentTicks;
currentTicks = usTicks; // read current tick counter
while ((usTicks - currentTicks) < delayTicks);
}
#include <cr_section_macros.h>
#include <NXP/crp.h>
#include <math.h>
#include "uart2.h"
#include <stdio.h>
#include "i2c.h"
#include "L3G4200D.h"
#include "ADXL345.h"
void SysTick_Handler(void) {
usTicks++; /* increment counter necessary in Delay() */
}
/*******************************************************************************
** Main Function main()
*******************************************************************************/
int main (void){
uint32_t timer=0;
//Configuro el SysTick para que interrumpa cada 1useg
if (SysTick_Config(SystemCoreClock / 1000000)) {
while (1);
}
I2C1Init(); /* initialize I2c1 */
ADXL345_init(); // Inicializamos el Acelerometro
L3G4200D_init(); // Inicializamos el Gyróscopo
UART2_Init(115200); // Inicializo el UART a 115200
L3G4200D_GetBiass(); // Obtengo el offset del giróscopo
ADXL345_GetBiass(); // Obtengo el offset del acelerometro
UART2_PrintString ("\r\nAcelerometro ADXL345 y Giroscopo L3G4200D\r\n");
UART2_PrintString ("\r\nPitch A\tRoll A\tPitch G\tRoll G\r\n");
timer = usTicks; //get a start value to determine the time the loop takes
while ( 1 ){
L3G4200D_read_data();
xg=((float)gyro_X - biasGyroX) * GYRO_X_SCALE;
yg=((float)gyro_Y - biasGyroY) * GYRO_Y_SCALE;
zg=((float)gyro_Z - biasGyroZ) * GYRO_Z_SCALE;
pitchGyro = pitchGyro + xg * sampTime;
rollGyro = rollGyro - yg * sampTime;
ADXL345_read_data();
xa=((float)accel_X - biasAccelX) * ACEL_X_SCALE;
ya=((float)accel_Y - biasAccelY) * ACEL_Y_SCALE;
za=((float)accel_Z - biasAccelZ) * ACEL_Z_SCALE;
//Calculate the pitch in degrees as measured by the accelerometers.
pitchAccel = atan2(ya, za) * 360.0 / (2*PI);
rollAccel = -asin(xa) * 360.0 / (2*PI);
uart2_printDouble(rollAccel, 3);
UART2_Sendchar('\t');
uart2_printDouble(rollGyro, 3);
UART2_Sendchar('\t');
uart2_printDouble(pitchAccel, 3);
UART2_Sendchar('\t');
uart2_printDouble(pitchGyro, 3);
UART2_Sendchar('\r');
UART2_Sendchar('\n');
timer=usTicks-timer; // Obtengo cuánto tiempo pasó en el bucle
timer=timeStep-timer; // Obtengo lo que falta para llegar al timeStep
delay_us(timer); // Espero hasta llegar al timeStep
timer=usTicks; //make one loop last time step msec
}
}
Como se puede observar, primero defino la sensibilidad de cada sensor de acuerdo al datasheet. Defino la cantidad de muestras para la obtención del offset de cada sensor y algunas variables necesarias.
Mi funcion de delay ahora está en uSeg para poder tener mejor control del tiempo del bucle. En este caso estoy trabajando a 10mSeg.
Luego, como siempre, inicializo el I2C, la UART, el Gyro y el Accel. Obtengo el offset de cada uno y lo imprimo en el terminal.
En el bucle principal calculo el ángulo con el Gyro primero y luego con el acelerómetro. Luego logueo los dos. Finalmente hago control de tiempo de loop.
El resultado obtenido es el siguiente:
Bien, aquí podemos ver el "ruido" del acelerómetro en verde y celeste y la deriva final del giróscopo en rojo y violeta.
El paso siguiente será fusionar los datos para obtener una respuesta mas suabe y sin deriva...
Saludos!
