TODOPIC
Mecatrónica => UAV => Mensaje iniciado por: elgarbe en 05 de Enero de 2014, 11:48:53
-
Los acelerómetros son sensores que miden la aceleracion a la que el sensor es sometido. Si bien existen sensores que miden en uno o dos ejes, hoy en día son muy comunes los acelerómetros que miden en los tres ejes.
Debido a que todo cuerpo en la superficie terrestre está sometido a la "Fuerza de la gravedad", el sensor en estado de reposo medirá la "aceleracion de la gravedad". Las aceleraciones pueden compararce contra dicha aceleración y por lo tanto 9,8m/seg2 se toma como 1g. Entonces nuestro acelerómetro expresará su salida en tantos g's.
En mi caso estoy trabajando y mostraré resultados aquí del ADXL345 de Analog Devices. Lo tengo montado una plaquita que compre en dx.com. Si bien posee interfaz SPI, en la placa esta cableado el bus I2C, para facilitar la lectura de los 4 sensores de la IMU.
Esta es la página del producto. (http://www.analog.com/en/mems-sensors/mems-inertial-sensors/adxl345/products/product.html) En ella podrán encontrar la hoja de datos así como algunas notas de aplicacion.
La gran mayoría de los sensores de tecnologia MEMS (en realidad la mayoria de todos los sensores) poseen 3 parámetros fundamentales:
El Offset (Zero rate level): Cuando el acelerómetro esta "quieto" debería entregar en su salida 0 g (1g en el eje Z). Sin embargo no es así y con el acelerómetro en reposo siempre tendremos un valor distinto de 0. En la hoja de datos del ADXL, en la Tabla 1, podemos ver que el Zero g OFFSET es del orden de los +-150mg en los ejes X e Y y +-250mg en el eje Z.
Es muy importante determinar el offset de nuestro sensor para obtener una medicion correcta.
Sensivilidad (Sensitivity): Nos indica cuantos mg equivale cada bit del dato binario de salida. Por ejemplo, para un rango de +-16g tenemos 31.2 mg/LSB. Esto significa que cuando la salida del sensor sea 1 representará 0.0312 g, cuando la salida sea 10 significará que el sensor está midiendo 0.312 g. La sensibilidad es bastante fija y está muy cerca de lo que dice la hoja de datos. Se puede determinar prácticamente para mejorar la presicion de la medicion.
Rango(range): El rango es entre que valores el sensor puede medir. Por ejemplo, este sensor podee 4 rangos configurables: +-16g, +-8g y +-4g y +-2g.
El offset y la sensivilidad son funcion de la temperatura y en aplicaciones de alta presicion se podría corregir con las variaciones de temperatura.
Otro dato importante es el ODR (Output Data Rate). Este sensor podemos elegir entre 0.1Hz hasta 3200Hz. Si no he leido mál a mayor ODR mayor nivel de ruido tendremos en la medicion.
Bien, con esto estamos mas o menos en condiciones de empezar a probar estos sensores. Para ello hay que tener un poco de idea de como funciona el bus I2C para entender mejor. Ver acá. (http://www.todopic.com.ar/foros/index.php?topic=41979.msg348895#msg348895)
Edito: Un lugar con una excelente explicacion sobre la teoría de los Acelerómetros y gyróscopos. (http://www.starlino.com/imu_guide.html)
Saludos
-
Leyendo el registro Device Id
Bueno, en este punto necesitaremos la hoja de datos para ver un poco las características de nuestro Acelerómetro y su configuracion.
En mi caso uso el ADXL345 de Analog.
Lo primero que hay que analizar es la tabla 1
(http://imagizer.imageshack.us/v2/800x600q90/802/fzbp.jpg) (https://imageshack.com/i/mafzbpj)
Como primer parámetro tenemos el Rango del Accel, podemos elegir entre +-2, +-4, +-8 y +-16. A mayor rango, vamos a poder medir aceleraciones mayores y además será menos sensible a aceleraciones más pequeñas. Este sensor posee una foma de trabajar que permite matener la mejor sensibilidad (3.9mg) usando cualquier rango. Esto se activa eligiendo Full Resolution. En mi caso elegiré +-16g con full resolution activado.
Una vez elegido el rango, la sensivilidad queda determinada. En mi caso 3.9mg por cada bit en el dato de salida.
Luego tenemos el ODR, podemos elegir entre un amplio rango, desde 0.1 hasta 3200Hz. Tengo entendido que a mayor ODR mas "ruidosa" será la señal de salida. En principio elegiría 100 o 200Hz.
El resto de los parámetros los podemos ver un poco más adelante.
Bien, en la Tabla19 tenemos un listado de todos los registros del accel, los cuales debemos/podemos leer/escribir para hacerlo funcionar.
El primer registro que podemos utilizar para testeo es el Device Id (como el Who_am_I del gyro) en la direccion 0x00 cuyo valor por defecto es 229 decimal.
Tenemos varios registros de control, los cuales debemos configurar para obtener la respuesta deseada del acelerómetro. Los más importantes son BW_RATE, POWER_CTL y DATA_FORMAT
Tenemos tambien los 6 registros con los datos crudos de cada eje a partir de la direccion 0x32
Bueno, veamos un pequeño ejemplo para leer el registro Dev_Id y verificar el funcionamiento de la comunicacion:
Lamentablemente para muchos, mi placa de desarrollo es la LPCXpresso, por lo que el código que propongo es para ella. Es C, por lo que se podrá modificar facilmente a cualquier otro uC.
Bueno, este es mi archivo ADXL345.h
/*
* ADXL345.h
*
* Created on: 04/12/2013
* Author: elgarbe
*/
#ifndef ADXL345_H_
#define ADXL345_H_
#define ADXL345_READ_ADDR 0xA7
#define ADXL345_WRITE_ADDR 0xA6
#define ADXL345_DEV_ID 0x00
#define ADXL345_POWER_CTL 0x2D
#define ADXL345_BW_RATE 0x2C
#define ADXL345_DATA_FORMAT 0x31
#define ADXL345_DATA_ADDR 0x32
#define PORT_USED 1
extern volatile uint8_t I2CMasterBuffer[I2C_PORT_NUM][BUFSIZE];
extern volatile uint8_t I2CSlaveBuffer[I2C_PORT_NUM][BUFSIZE];
extern volatile uint32_t I2CReadLength[I2C_PORT_NUM];
extern volatile uint32_t I2CWriteLength[I2C_PORT_NUM];
short make_word(unsigned char HB, unsigned char LB);
void ADXL345_init();
unsigned char ADXL345_read(unsigned char reg);
unsigned char ADXL345_write(unsigned char reg_address, unsigned char value);
unsigned char ADXL345_read_data();
short make_word(unsigned char HB, unsigned char LB){
return ((HB << 8) | LB);
}
void ADXL345_init(){
ADXL345_write(ADXL345_POWER_CTL, 0b00000000);
ADXL345_write(ADXL345_DATA_FORMAT, 0b00001011); // 1XXX Full res, XX11 +-2g
ADXL345_write(ADXL345_BW_RATE, 0b00001010); // 100Hz Data Output Rate
ADXL345_write(ADXL345_POWER_CTL, 0b00001000);
}
unsigned char ADXL345_read(unsigned char reg){
I2CWriteLength[PORT_USED] = 2;
I2CReadLength[PORT_USED] = 1;
I2CMasterBuffer[PORT_USED][0] = ADXL345_WRITE_ADDR;
I2CMasterBuffer[PORT_USED][1] = reg;
I2CMasterBuffer[PORT_USED][2] = ADXL345_READ_ADDR;
I2CEngine( PORT_USED );
return(I2CSlaveBuffer[PORT_USED][0]);
}
unsigned char ADXL345_write(unsigned char reg_address, unsigned char value){
I2CWriteLength[PORT_USED] = 3;
I2CReadLength[PORT_USED] = 0;
I2CMasterBuffer[PORT_USED][0] = ADXL345_WRITE_ADDR;
I2CMasterBuffer[PORT_USED][1] = reg_address;
I2CMasterBuffer[PORT_USED][2] = value;
return(I2CEngine( PORT_USED ));
}
#endif /* ADXL345_H_ */
Bueno, como ven al principio estan todos los define con las direcciones de cada registro. Luego tengo las funciones típicas necesarias para usar el accel en esta aplicacion.
Mi main.c es este:
/*
===============================================================================
Name : main.c
Author : $(author)
Version :
Copyright : $(copyright)
Description : main definition
===============================================================================
*/
#ifdef __USE_CMSIS
#include "LPC17xx.h"
#endif
#include <cr_section_macros.h>
#include <NXP/crp.h>
#include "i2c.h"
#include "ADXL345.h"
#include "uart2.h"
/*******************************************************************************
** Main Function main()
*******************************************************************************/
int main (void){
unsigned char dev_id;
I2C1Init(); /* initialize I2c1 */
ADXL345_init();
UART2_Init(115200); // Inicializo el UART a 115200
UART2_PrintString ("\rAcelerómetro ADXL345\r\n");
UART2_PrintString ("======== elgarbe ==========\r\n");
dev_id=ADXL345_read(ADXL345_DEV_ID);
UART2_PrintString("\r\nEl registro DEVICE ID contiene: ");
uart2_printUint32(dev_id, 10);
while ( 1 ){
}
}
Simplemente se inicializa el I2C, luego se inicializa el accel. Para ello solo hace falta escribir 3 registros. El POWER_CTL con 0x00 para asegurar el modo SLEEP previo a la configuracion del sensor, DATA_FORMAT configuramos Full Resolution y +-16g, BW_RATE en 100Hz
Finalmente inicializamos la UART, escribimos un mensaje y luego leo el registro Dev_Id y mostrar su valor.
El terminal obtengo la siguiente salida
(http://imagizer.imageshack.us/v2/800x600q90/11/uy05.jpg) (https://imageshack.com/i/0buy05j)
Bueno, la próxima lectura de los datos crudos.
Saludos!
-
Leyendo los datos RAW
Bueno, ahora que ya estamos "hablando" con el accel, debemos poder leer la informacion de los G que está midiendo el acelerómetro.
Para ello simplemente voy a agregar una funcion que me permita leer los 6 registros de datos y unir X_L xon X_H para obtener el dato completo del eje X (al igual que con los otros ejes).
short make_word(unsigned char HB, unsigned char LB){
return ((HB << 8) | LB);
}
unsigned char ADXL345_read_data(){
char result;
I2CWriteLength[PORT_USED] = 2;
I2CReadLength[PORT_USED] = 6;
I2CMasterBuffer[PORT_USED][0] = ADXL345_WRITE_ADDR;
I2CMasterBuffer[PORT_USED][1] = ADXL345_DATA_ADDR;
I2CMasterBuffer[PORT_USED][2] = ADXL345_READ_ADDR;
result=I2CEngine( PORT_USED );
accel_X = make_word(I2CSlaveBuffer[PORT_USED][1], I2CSlaveBuffer[PORT_USED][0]);
accel_Y = make_word(I2CSlaveBuffer[PORT_USED][3], I2CSlaveBuffer[PORT_USED][2]);
accel_Z = make_word(I2CSlaveBuffer[PORT_USED][5], I2CSlaveBuffer[PORT_USED][4]);
return(result);
}
Como se puede ver se leen 6 registros, a partir de la direccion ADXL345_DATA_ADDR. Luego se arma el dato combinando de a dos los registros leídos para armar un short con signo (16 bits).
En el main tenemos:
/*
===============================================================================
Name : main.c
Author : $(author)
Version :
Copyright : $(copyright)
Description : main definition
===============================================================================
*/
#ifdef __USE_CMSIS
#include "LPC17xx.h"
#endif
signed short accel_X, accel_Y, accel_Z;
volatile uint32_t msTicks; /* counts 1ms timeTicks */
__INLINE static void delay_ms (uint32_t delayTicks) {
uint32_t currentTicks;
currentTicks = msTicks; // read current tick counter
while ((msTicks - currentTicks) < delayTicks);
}
#include <cr_section_macros.h>
#include <NXP/crp.h>
#include <math.h>
#include "i2c.h"
#include "ADXL345.h"
#include "uart2.h"
void SysTick_Handler(void) {
msTicks++; /* increment counter necessary in Delay() */
}
/*******************************************************************************
** Main Function main()
*******************************************************************************/
int main (void){
//Configuro el SysTick para que interrumpa cada 1mseg
if (SysTick_Config(SystemCoreClock / 1000)) {
while (1);
}
I2C1Init(); // Inicializamos el bus I2C
ADXL345_init(); // Inicializamos el Gyróscopo
UART2_Init(115200); // Inicializamos el UART a 115200
UART2_PrintString ("\rAcelerómetro ADXL345\r\n");
UART2_PrintString ("======== elgarbe ==========\r\n");
UART2_PrintString ("\r\nRAW X\tRAW Y\tRAW Z\r\n");
while ( 1 ){
ADXL345_read_data();
uart2_printInt32(accel_X, 10);
UART2_Sendchar('\t');
uart2_printInt32(accel_Y, 10);
UART2_Sendchar('\t');
uart2_printInt32(accel_Z, 10);
UART2_Sendchar('\r');
UART2_Sendchar('\n');
delay_ms(500);
}
}
Como se puede ver, tenemos 3 variables globales signed short para almacenar los datos del accel.
Aquí estamos haciendo uso del SysTick timer de los Cortex M3. Está configurado para que interrumpa cada 1 mseg, lo usamos para crear la funcion delay_ms.
Como antes, inicializamos el bus I2C, el acelerómetro y la UART. Luego llamamos a la funcion read_data del ADXL345 e imprimimos en la UART el resultado.
Esta es una imagen de nuestra terminal:
(http://imagizer.imageshack.us/v2/800x600q90/24/05gd.jpg) (https://imageshack.com/i/0o05gdj)
Bien, esos son los datos crudos del gyro, no son G (aceleracion respecto a la gravedad). La primera observacion que podemos hacer es que no son 0 como uno esperaría obtener con el acelerómetro sin mover, fijo en la mesa (para el eje X e Y, el eje Z si debe medir algo, debe medir la aceleracion de la gravedad)... En cambio obtenemos ciertos valores. Bueno, esos valores son el Zero g Level. Otra cosa que observamos es que dicho valor cambia constantemente. Por ejemplo en el eje X tenemos valores que van de 0 a -1 "cuentas del ADC". Esto puede ser mucho o poco, aún no lo sabemos ya que no tenemos los datos en G para compararlos con los 16g de alcance con el que tenemos configurado al accel. Aunque si pensamos un poco, el accel tiene un ADC de 13bits y me parece que obtener una variacion de 1 cuentas en 65535 no parece ser mucho, me parece que es simplemente "ruido" en el ADC... pero de esto no estoy seguro.
Bien, entonces el siguiente paso lógico sería convertir los datos RAW en G.
Eso quedará para la próxima entrega.
Saludos!
-
Eliminando Offset - obteniendo G's
Bueno, llegó el momento de que este bichito empiece a dar informacion que sirva para algo!
Como vimos en el post anterior, el acelerómetro no entrega 0 cuentas cuando está en reposo, sino que posee un Zero G Level. Es por ello que debemos realizar una funcion que obtenga el offset para poder restarlo a cada medicion posterior y obtener una lectura correcta.
Mi funcion de obtener offset es la siguiente:
void ADXL345_GetBiass(void){
int i;
for (i = 0; i < SAMPLESS_BIASS; i += 1) {
ADXL345_read_data();
biass_X += accel_X;
biass_Y += accel_Y;
biass_Z += accel_Z;
delay_ms(1);
}
biass_X /= SAMPLESS_BIASS;
biass_Y /= SAMPLESS_BIASS;
biass_Z /= SAMPLESS_BIASS;
biass_Z -= 256;
}
Como pueden observar, simplemente tomo SAMPLESS_BIASS cantidad de muestras y hago un promedio de ellas. En este caso estoy tomando unas 600 muestras. Cuantas más muestras mejor. Notar que en el eje X no debemos restar lo que corresponde a 1G (1/0.0039 = 256).
Una vez que obtengo el Offset, lo siguiente es la conversion de LSB's o cuentas o datos RAW en G's. Para ello utilizamos la sensibilidad. En la tabla 1 antes vista, encontramos que en full resolution tenemos una sensibilidad de 3.9 mg/LSB
En definitiva los datos "correctos" o reales son (RAW - Offset) * Sensibilidad
Bien, mi main queda ahora de la siguiente forma:
/*
===============================================================================
Name : main.c
Author : $(author)
Version :
Copyright : $(copyright)
Description : main definition
===============================================================================
*/
#ifdef __USE_CMSIS
#include "LPC17xx.h"
#endif
#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
signed short accel_X, accel_Y, accel_Z;
float x1,y1,z1;
double biass_X, biass_Y, biass_Z;
#define SAMPLESS_BIASS 600 //Número de muestras para calular biass
volatile uint32_t msTicks; /* counts 1ms timeTicks */
__INLINE static void delay_ms (uint32_t delayTicks) {
uint32_t currentTicks;
currentTicks = msTicks; // read current tick counter
while ((msTicks - currentTicks) < delayTicks);
}
#include <cr_section_macros.h>
#include <NXP/crp.h>
#include <math.h>
#include "i2c.h"
#include "ADXL345.h"
#include "uart2.h"
void SysTick_Handler(void) {
msTicks++; /* increment counter necessary in Delay() */
}
/*******************************************************************************
** Main Function main()
*******************************************************************************/
int main (void){
double gx, gy, gz;
//Configuro el SysTick para que interrumpa cada 1mseg
if (SysTick_Config(SystemCoreClock / 1000)) {
while (1);
}
I2C1Init(); // Inicializamos el bus I2C
ADXL345_init(); // Inicializamos el Gyróscopo
UART2_Init(115200); // Inicializamos el UART a 115200
UART2_PrintString ("\rAcelerómetro ADXL345\r\n");
UART2_PrintString ("======== elgarbe ==========\r\n");
UART2_PrintString ("Obteniendo Offset...\r\n");
ADXL345_GetBiass();
UART2_PrintString ("Offset X: ");
uart2_printDouble(biass_X, 4);
UART2_PrintString("\r\n");
UART2_PrintString ("Offset Y: ");
uart2_printDouble(biass_Y, 4);
UART2_PrintString("\r\n");
UART2_PrintString ("Offset Z: ");
uart2_printDouble(biass_Z, 4);
UART2_PrintString("\r\n");
UART2_PrintString ("\r\nRAW X\tRAW Y\tRAW Z\tGs X\tGs Y\tGs Z\r\n");
while ( 1 ){
ADXL345_read_data();
gx=((float)accel_X - biass_X);
gy=((float)accel_Y - biass_Y);
gz=((float)accel_Z - biass_Z);
uart2_printDouble(gx , 2);
UART2_Sendchar('\t');
uart2_printDouble(gy , 2);
UART2_Sendchar('\t');
uart2_printDouble(gz , 2);
UART2_Sendchar('\t');
gx *= ACEL_X_SCALE;
gy *= ACEL_Y_SCALE;
gz *= ACEL_Z_SCALE;
uart2_printDouble(gx, 3);
UART2_Sendchar('\t');
uart2_printDouble(gy, 3);
UART2_Sendchar('\t');
uart2_printDouble(gz, 3);
UART2_PrintString("\r\n");
delay_ms(500);
}
}
Bueno, básicamente, primero obtenemos el offset con la nueva funcion incorporada. Muestro esos datos.
Luego calculo los datos sin offset y los muestro. Finalmente multiplico por la sensibilidad y muestro los datos en G's
Esta es una captura de lo que obtengo:
(http://imagizer.imageshack.us/v2/800x600q90/822/g42o.jpg) (https://imageshack.com/i/mug42oj)
De aquí podemos ver aunque hemos eliminado el offset, todavía tenemos datos distintos de cero, pero en este caso están mas cerca de 0. Tamien notamos que en el peor de los casos tenemos -0.014 G's en el eje Y cuando deveríamos tener 0. Será mucho? Pues veamos, el rango del Accel está en 16G por lo que 0.014 representa el 0.087%!!!! A mí me parece que eso es más que suficiente! Tambien notemos que es un poco mas "ruidos" que el gyro en el que teníamos 0.028%
Bien, veamos ahora que pasa si giramos el sensor en un sentido y lo dejamos quieto, pero inclinado:
(http://imagizer.imageshack.us/v2/800x600q90/89/0hur.jpg) (https://imageshack.com/i/2h0hurj)
Como se puede observar he girado el sensor "alrededor" del eje X y lo he hecho hasta un ángulo de 45°. Cuando dejé de girar el sensor, el resultado es que tengo medicion en el eje Y y en el eje Z. Esto es porque al girar el sensor empieza a medir la aceleracion de la gravedad en alguno de los ejes.
Bueno, ya estamos llegando al final, solo nos queda calcular el ángulo a partir de la aceleración...
Saludos!
-
Calculando el ángulo de giro
Bien, ya estamos listos para poder obtener el tan preciado ángulo que ha girado la IMU con el acelerómetro. Para ello debemos simplemente usar funciones trigonométrias.
La técnica es bien simple, .
Entonces mi nuevo main es:
/*
===============================================================================
Name : main.c
Author : $(author)
Version :
Copyright : $(copyright)
Description : main definition
===============================================================================
*/
#ifdef __USE_CMSIS
#include "LPC17xx.h"
#endif
#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 PI 3.1415926
signed short accel_X, accel_Y, accel_Z;
float x1,y1,z1;
double biass_X, biass_Y, biass_Z;
#define SAMPLESS_BIASS 600 //Número de muestras para calular biass
volatile uint32_t msTicks; /* counts 1ms timeTicks */
__INLINE static void delay_ms (uint32_t delayTicks) {
uint32_t currentTicks;
currentTicks = msTicks; // read current tick counter
while ((msTicks - currentTicks) < delayTicks);
}
#include <cr_section_macros.h>
#include <NXP/crp.h>
#include <math.h>
#include "i2c.h"
#include "ADXL345.h"
#include "uart2.h"
void SysTick_Handler(void) {
msTicks++; /* increment counter necessary in Delay() */
}
/*******************************************************************************
** Main Function main()
*******************************************************************************/
int main (void){
//Configuro el SysTick para que interrumpa cada 1mseg
if (SysTick_Config(SystemCoreClock / 1000)) {
while (1);
}
I2C1Init(); // Inicializamos el bus I2C
ADXL345_init(); // Inicializamos el Gyróscopo
UART2_Init(115200); // Inicializamos el UART a 115200
UART2_PrintString ("\rAcelerómetro ADXL345\r\n");
UART2_PrintString ("======== elgarbe ==========\r\n");
UART2_PrintString ("Obteniendo Offset...\r\n");
ADXL345_GetBiass();
UART2_PrintString ("Offset X: ");
uart2_printDouble(biass_X, 4);
UART2_PrintString("\r\n");
UART2_PrintString ("Offset Y: ");
uart2_printDouble(biass_Y, 4);
UART2_PrintString("\r\n");
UART2_PrintString ("Offset Z: ");
uart2_printDouble(biass_Z, 4);
UART2_PrintString("\r\n");
UART2_PrintString ("\r\ng's X\tg's Y\tg's Z\tpitch\troll\r\n");
while ( 1 ){
double pitchAccel, rollAccel;
ADXL345_read_data();
x1=((float)accel_X - biass_X) * ACEL_X_SCALE;
y1=((float)accel_Y - biass_Y) * ACEL_Y_SCALE;
z1=((float)accel_Z - biass_Z) * ACEL_Z_SCALE;
pitchAccel = atan2(y1, z1) * 360.0 / (2*PI);
rollAccel = -asin(x1) * 360.0 / (2*PI);
uart2_printDouble(x1, 3);
UART2_Sendchar('\t');
uart2_printDouble(y1, 3);
UART2_Sendchar('\t');
uart2_printDouble(z1, 3);
UART2_Sendchar('\t');
uart2_printDouble(pitchAccel, 3);
UART2_Sendchar('\t');
uart2_printDouble(rollAccel, 3);
UART2_Sendchar('\r');
UART2_Sendchar('\n');
delay_ms(20);
}
}
En este caso no hace falta el control del tiempo en el loop ya que no hay integracion, es simple cálculo trigonométrico
Bueno, en el hyperterminal he logueado en un archivo los datos leidos del sensor para luego abrirlo con Excel y realizar el siguiente gráfico:
(http://imagizer.imageshack.us/v2/800x600q90/24/k2qv.jpg) (https://imageshack.com/i/0ok2qvj)
Como se puede observar, en la prueba he girado el sensor alrededor del eje X, primero 90° en un sentido, luego lo vuelvo a la posicion original luego lo giro en otro sentido y finalmente lo dejo quieto unos segundos.
El eje horizontal no tiene unidad, es simplemente el número de muestra
Como podemos ver este sensor no sufre de drift o deriva. Si lo dejo quieto mide siempre lo mismo, aunque pase el tiempo. Pero presenta un problema, es "ruidoso" ver en el segundo giro que aparecen unos escalones en la rampa de subida.
Como podemos concluir cada sensor tiene un pro y una contra. El acelerómetro no tiene deriva, pero detecta cualquier vibracion. El gyro es más estable en su medicion, menos inmune a las altas frecuencias, pero tiene deriva...
El paso siguiente será buscar una técnica (filtro) que nos permita mezclar los dos sensores y obtener una sola medicion del ángulo de giro...
Saludos!
-
Comparando medición de Acelerómetro y gyróscopo
Antes 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:
(http://imagizer.imageshack.us/v2/800x600q90/560/i7nd.jpg) (https://imageshack.com/i/fki7ndj)
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!
-
Muy buen trabajo Elgarbe, he leido tus post y estan muy bien explicados, creo que faltaria que hicieras referencia a tus otros post.
Yo he hecho uso de acelerómetro y de giroscopios pero para un robot equilibrista, yo utlicé el filtro kalman para unir ambos datos, pero me gustaría saber mas sobre los otros filtros que piensas usar
Saludos
-
Muy buen trabajo Elgarbe, he leido tus post y estan muy bien explicados, creo que faltaria que hicieras referencia a tus otros post.
Yo he hecho uso de acelerómetro y de giroscopios pero para un robot equilibrista, yo utlicé el filtro kalman para unir ambos datos, pero me gustaría saber mas sobre los otros filtros que piensas usar
Saludos
Hola! me alegro que sea util!
Ok, voy a armar una especie de índice así queda todo más ordenado.
Muy interesante lo del robot equilibrista! Lo tenes funcionando? Yo andaba con ganas de intentar armar uno de esos vehículos de 2 ruedas que aceleran cuando lo inclinas hacia adelante y se mantienen vericales. Pero por ahora tengo que ir cerrando el tema de los sensores y la controladora de vuelo.
El tema filtros estoy empezando a investigar. Por ahora voy a arrancar mostrando el complementario, que como dijo Pablo es una sola línea y es simple de implementar. El kalman lo implementaste estudiando el sistema dinámico en donde está aplicado para determinar las matrices? Ahora estoy por abrir un post sobre ese tema la fusion de sensores con filtros y si queres me contás lo que hiciste así me ahorras investigacion!!!!
Saludos y gracias!
-
Muy interesante lo del robot equilibrista! Lo tenes funcionando? Yo andaba con ganas de intentar armar uno de esos vehículos de 2 ruedas que aceleran cuando lo inclinas hacia adelante y se mantienen vericales. Pero por ahora tengo que ir cerrando el tema de los sensores y la controladora de vuelo.
El tema filtros estoy empezando a investigar. Por ahora voy a arrancar mostrando el complementario, que como dijo Pablo es una sola línea y es simple de implementar. El kalman lo implementaste estudiando el sistema dinámico en donde está aplicado para determinar las matrices? Ahora estoy por abrir un post sobre ese tema la fusion de sensores con filtros y si queres me contás lo que hiciste así me ahorras investigacion!!!!
mi robotito, o vehículo, porque esta grandote, esta funcionando, en cuanto a lo del filtro, aunque lei sobre la teoria del filtro y como armar la matriz, utilice variables que ya estaba probadas para los sensores que utilicé, solo hice algunos cambios a las constantes
en este (http://www.todopic.com.ar/foros/index.php?topic=12748.400) post tratamos los robots equilibristas, ahi hay muchos links útiles sobre el filtro kalman y el complementario, creo que no hay sobre algún otro.
-
Una cosa que queda pendiente tanto con el giróscopo como con el acelerómetro es obtener la sensivilidad. Como ven en el gráfico anterior, al comparar el acelerómetro con el giróscopo no obtengo el mismo angulo, hay una pequeña diferencia, atribuible a pequeñas diferencias en la sensibilidad.
El que más me preocupa es el acelrómetro. He visto algoritmos de calibracion en el que se pide colocar el acelerómetro en distintas posiciones, se obtienen lecturas y luego se hacen cáluclos para obtener la sensivilidad. Tengo mis dudas de esos algoritmos ya que, creo, necesitan que se coloque el sensor con cada eje perfectamente vertical/horizontal y por lo tanto depende mucho de que el usuario coloque el sensor en la posicion esperada.
Yo pensaba hacer algo más o menos así:
Pedir al usuario que coloque el sensor con el eje Z mirando hacia abajo (mas o menos)
Empezar a tomar muestras y pedir al usuario que gire el acelerómetro lentamente alrededor del punto vertical mientras tomamos muestras.
Determinamos el máximo valor leido.
Pedimos al usuario que gire 180° el acelerómetro y ahora quede el eje Z mirando hacia arriba.
Hacemos lo mismo.
Lo que hemos hecho es obtener el valor RAW de 1g en un sentido y luego en el otro. Restando ambos valores y dividiendo por dos tenemos el valor raw de 1g. Tambien obtenemos de aquí el offset, ya que el valor maximo en un sentido puede ser mayor que en el otro sentido. Luego dividiendo por la cantidad de bits del ADC obtenemos la sensibilidad....
Que les parece? funcionaría? conocen otro algoritmo?
Saludos!
-
Claro parece buena técnica, y sería para los 3 ejes, ya que como dices la calibración en la mesa del usuario no es perfectamente horizontal.
Yo uso una técnica que tiene una idea similar.
Me guié de este pdf, en la página 3("Correction of Sensor Bias Error and Sensitivity Error"):
http://www.kionix.com/sites/default/files/AN012%20Accelerometer%20Errors.pdf
Lo debes posicionar en 6 posiciones distintas y en cada una de ellas sensar el acelerómetro, como veras es bien parecida, solo que acá no hallan el valor máximo durante el giro sino sacan un promedio de las diversas posiciones colocadas.
La sensibilidad esta dada por Sxx,Syy,Szz y las bias por Bx, By, Bz( en gravedad 0)
La lectura calibrada del acel seria.
Acel_calibrado= (Acel_leido - B)/S
-
mira esto
http://www.ehowenespanol.com/encontrar-sensibilidad-del-acelerometro-como_292587/ (http://www.ehowenespanol.com/encontrar-sensibilidad-del-acelerometro-como_292587/)
es prácticamente lo mismoq ue planteas, creo que esto necesitarias hacerlo solo una vez para caracterizar tu sensor y después ya no seria necesario
-
mira esto
http://www.ehowenespanol.com/encontrar-sensibilidad-del-acelerometro-como_292587/ (http://www.ehowenespanol.com/encontrar-sensibilidad-del-acelerometro-como_292587/)
es prácticamente lo mismoq ue planteas, creo que esto necesitarias hacerlo solo una vez para caracterizar tu sensor y después ya no seria necesario
el problema que creo tiene ese metodo es que supones que la aceleración 1g en el primer paso, pero para eso el sensor tiene que tener el eje Z perfectamente vertical, cosa que es imposible de determinar. por eso yo sugería registrar y mover el acelerometro cerca de la vertical, buscando la mayor lectura. la mayor lectura sí puedo asegurar que es la medición correspondiente a 1g. me explico? o estoy cometiendo un error?..
Saludos!
-
ya entendi, buscas que el usuario lo lleve al extremo de la medición, en ambos sentidos, asi tienes +1g y -1g.
-
Claro parece buena técnica, y sería para los 3 ejes, ya que como dices la calibración en la mesa del usuario no es perfectamente horizontal.
Yo uso una técnica que tiene una idea similar.
Me guié de este pdf, en la página 3("Correction of Sensor Bias Error and Sensitivity Error"):
http://www.kionix.com/sites/default/files/AN012%20Accelerometer%20Errors.pdf
Lo debes posicionar en 6 posiciones distintas y en cada una de ellas sensar el acelerómetro, como veras es bien parecida, solo que acá no hallan el valor máximo durante el giro sino sacan un promedio de las diversas posiciones colocadas.
La sensibilidad esta dada por Sxx,Syy,Szz y las bias por Bx, By, Bz( en gravedad 0)
La lectura calibrada del acel seria.
Acel_calibrado= (Acel_leido - B)/S
mi duda es cuanto error se puede cometer al suponer que en la posicion 1 el eje z esta bien vertical. a lo mejor exagero un poco porque mi imu esta enchufada en un protoboard y se mueve para todos lados... quizás si el sensor esta en una placa junto con el uC y uno puede ver que esta bastante horizontal alcanza. al fin y al cabo todas las implementaciones que he visto son como la que muestras, poner el sensor en 6 posiciones distintas y medir.
gano algo aumentando el numero de posiciones? digo, si mido en 12 posiciones diferentes, obtendría mejores resultados?
en este link, starlino usa 12 posiciones y habla del metodo de los menores cuadrados? no he tenido tiempo de mirarlo bien....
Saludos!!
-
Bueno, finalmente hice el programa para calibrar el acelerómetro con "mi técnica".
La idea era registrar los datos RAW de cada eje del acelerómetro buscando la mínima excursión y la máxima excursión, puntos en los cuales tomamos como medidas de 1g. Luego, haciendo 2g/(Xmax - Xmin) obtengo la sensibilidad en g/LSB.
Mi main.c es el siguiente:
#ifdef __USE_CMSIS
#include "LPC17xx.h"
#endif
signed short accel_X, accel_Y, accel_Z;
volatile uint32_t msTicks; /* counts 1ms timeTicks */
__INLINE static void delay_ms (uint32_t delayTicks) {
uint32_t currentTicks;
currentTicks = msTicks; // read current tick counter
while ((msTicks - currentTicks) < delayTicks);
}
#include <cr_section_macros.h>
#include <NXP/crp.h>
#include <math.h>
#include "i2c.h"
#include "ADXL345.h"
#include "uart2.h"
void SysTick_Handler(void) {
msTicks++; /* increment counter necessary in Delay() */
}
/*******************************************************************************
** Main Function main()
*******************************************************************************/
int main (void){
signed short MinZ=0, MaxZ=0;
//Configuro el SysTick para que interrumpa cada 1mseg
if (SysTick_Config(SystemCoreClock / 1000)) {
while (1);
}
I2C1Init(); // Inicializamos el bus I2C
ADXL345_init(); // Inicializamos el Gyróscopo
UART2_Init(115200); // Inicializamos el UART a 115200
UART2_PrintString ("\rAcelerómetro ADXL345\r\n");
UART2_PrintString ("======== elgarbe ==========\r\n");
ADXL345_read_data();
MinZ = accel_Z;
MaxZ = accel_Z;
while ( 1 ){
ADXL345_read_data();
if(accel_Z < MinZ){
MinZ = accel_Z;
UART2_PrintString ("\r\nNuevo Mínimo: ");
uart2_printInt32(MinZ, 10);
UART2_PrintString ("\r\n");
}
if(accel_Z > MaxZ){
MaxZ = accel_Z;
UART2_PrintString ("\r\nNuevo Máximo: ");
uart2_printInt32(MaxZ, 10);
UART2_PrintString ("\r\n");
}
delay_ms(20);
}
}
Entonces en el hiperterminal a medida que voy moviendo lentamente el sensor tratando de colocarlo con el eje Z lo más paralelo posible a la vertical voy viendo como van cambiando las lecturas. Cuando pasa un tiempo en que muevo y roto el sensor buscando un nuevo mínimo/máximo y veo que no lo consigo, giro 180° el sensor y busco el otro punto.
Para el eje Z esto es lo que he obtenido:
Acelerómetro ADXL345
======== elgarbe ==========
Nuevo Máximo: 1636
Nuevo Máximo: 1637
Nuevo Máximo: 1638
Nuevo Máximo: 1639
Nuevo Máximo: 1641
Nuevo Máximo: 1646
Nuevo Máximo: 1647
Nuevo Máximo: 1649
Nuevo Mínimo: 1550
Nuevo Mínimo: 1545
Nuevo Mínimo: 1543
Nuevo Mínimo: 1542
Nuevo Mínimo: 1534
...
Nuevo Mínimo: 1101
Nuevo Mínimo: 1099
Nuevo Mínimo: 1098
Nuevo Mínimo: 1091
Nuevo Mínimo: 1090
Nuevo Mínimo: 1089
Nuevo Mínimo: 1086
Nuevo Mínimo: 1085
Nuevo Mínimo: 1084
Nuevo Mínimo: 1083
Por lo que la sensibilidad sería: 2g / (1649-1083) = 0,003533- Por hoja de datos la sensibilidad debería ser 0,0039
Así puedo repetir con cada eje para calcular cada sensivilidad. Incluso lo ideal sería calibrar 10 veces y hacer un promedio de las sensivilidades obtenidas.
También estoy notando que debería trabajar en alguna GUI para ver los datos que manda el uC. El hyperterminal queda chico para esto. Para la calibracion lo ideal sería ver los datos RAW y el mínimo encontrado así podemos ver si estamos moviendo el sensor más cerca o más lejos del mínimo...
Bien, el tema es que cuando cargué los datos de calibracion del acelerómetro en el programa que mide el ángulo con el acelerómetro y con el gyróscopo tengo unos 5 grados de diferencia. Por lo que supongo el gyro debe tener una pequeña variacion en su sensivilidad también.
El tema que calibrar el giro es un poco más complicado, ya que hay que ponerlo a girar a velocidad constante y encima hay que alinear bien el eje del motor con el eje del gyro que quiero medir, tienen que ser perfectamente paralelos (los ejes).... en fin, lo que puedo hacer es suponer que el acelerómetro está bien calibrado y corregir el giro para que las mediciones sean iguales.....
Haceindo esto tengo que las sensivilidades en los ejes del Gyro pasan de 0.07 teóricos a 0.077.
Con esos valores obtengo el siguiente resultado:
(http://imagizer.imageshack.us/v2/800x600q90/41/jvhr.jpg) (https://imageshack.com/i/15jvhrj)
Como se puede observar las gráficas tienen casi el mismo punto máximo y coinciden bastante bien...
Bueno, con estoy creo que doy por cerrado el tema de los acelerómetro, a no ser que alguien quiera ver algo más.
Quedaría por ver el tema de la fusión de los datos con el gyro que lo pondré en otro post.
Saludos!
-
Una consulta que tengo, como haces para visualizar los datos en EXCEL...tenés por ahí el macro que estás usando ?
-
Una consulta que tengo, como haces para visualizar los datos en EXCEL...tenés por ahí el macro que estás usando ?
Es otro tema pendiente... reniego un monton a la hora de hacer un gráfico. Lo que hago es loguear con el hyperterminal en un archivo de texto plano separado por tabulacion. Abro ese archivo en excel y hago el gráfico a mano....
Te puedo pasar un excel con el gráfico y los datos, pero no sé si te servirá.
Estoy trabajando con c# y un control zedgraphic para graficar en tiempo real, pero el tema de los delegados en C# me está matando...
Saludos!
-
Estoy trabajando con c# y un control zedgraphic para graficar en tiempo real, pero el tema de los delegados en C# me está matando...
dentro de c# podrías usar los objetos del tipo "chart", yo asi realicé las graficas en tiempo real
-
Es otro tema pendiente... reniego un monton a la hora de hacer un gráfico. Lo que hago es loguear con el hyperterminal en un archivo de texto plano separado por tabulacion. Abro ese archivo en excel y hago el gráfico a mano....
Gracias por la info !
-
Estoy trabajando con c# y un control zedgraphic para graficar en tiempo real, pero el tema de los delegados en C# me está matando...
dentro de c# podrías usar los objetos del tipo "chart", yo asi realicé las graficas en tiempo real
Si, es cierto. Pero use el control que use está el tema de los delegados, no? digo al leer datos con el puerto y querer modificar controles...
Saludos!