Accelerometer Pitch Roll Angles – Esplora

En un post anterior comentamos que para determinar la orientación completa de una unidad de medición inercial, debíamos utilizar la combinación de acelerómetro y giroscopio.

Aunque pueda resultar tedioso, pero al fin y al cabo cumple su función. Pero en caso de disponer solamente de un acelerómetro; no obtendremos toda la información completa de la orientación de nuestros robots, pero sí que podremos despejar los ángulos de roll y pitch. Mientras que el de yaw no nos será posible de obtener.

En este caso las matemáticas son muy fáciles.

En este tutorial vamos a realizar una serie de ejercicios para obtener los ángulos de inclinación (Roll) y alabeo (Pitch) con solo un acelerómetro.

En mi caso utilizaré la placa de Arduino Esplora, que proporciona unas funciones muy sencillas para disponer de esta información.

 

 

#include <Esplora.h>

void setup() {
  Serial.begin(9600);
}

void loop() {
  Serial.print("   X : ");
  Serial.print((Esplora.readAccelerometer(X_AXIS)));
  Serial.print("  Y : ");
  Serial.print((Esplora.readAccelerometer(Y_AXIS)));
  Serial.print("   Z : ");
  Serial.println((Esplora.readAccelerometer(Z_AXIS)));
  delay(100);
}

A través de la página de Ardublockly y utilizando los bloques adaptados para la placa Esplora, podemos desarrollar el programa anterior.

La respuesta de estos valores sin necesidad de girarlo son los que aparecen en la primera imagen, y al inclinarlo para disponerlo con el eje X en dirección vertical es la siguiente.

---

Como podremos observar, el primer problema es que tendremos un offset de lectura en cada eje. Al menos para el acelerómetro de la placa Esplora. En estado de reposo, al no haber fuerzas de aceleración (a excepción de la gravedad), deberíamos obtener un valor nulo en esos ejes.

El segundo problema, es que las unidades tienen un máximo que desconocemos, por lo que no es tan evidente despejar el ángulo, siendo el cociente de g un valor definido y el de lectura del acelerómetro no.

Es por ello que utilizaremos las ecuaciones equivalentes del vector para no depender de el parámetro g que se asoia a la gravedad.

Por lo que vamos a empezar hallando el valor de Pitch que depende de un cociente en los otros dos ejes. Al ser un cociente con las mismas unidades, adimensionalizamos el resultado, por lo que sabremos que está bien.

El problema que tendremos es el del valor de offset de estos dos ejes que seguiran molestando y nuestra ecucación aparecerá con unos términos adicionales:

   

En caso de que nuestro acelerómetro no tuviera este problema en la adquisición de datos no sería necesario calibrarlo.

 

---

Mientras tanto tendremos que restar estos valores para calibrar estos sensores.

Dejo el siguiente programa para realizar la calibración inicial desde el puerto serie y una vez montado, podremos obtener los valores de estos dos ángulos con una gran precisión.

Nos pedirá que iniciemos el acelerómetro en una posición sobre el plano, para hallar los offset de dos ejes a los que no les afecta la gravedad y una vez determinados los valores de offset, disponemos la placa en posición vertical para hallar el offset que nos queda.

#include <Esplora.h>

const float alpha = 0.5;
double fXg = 0;
double fYg = 0;
double fZg = 0;
double xOffset, yOffset, zOffset;

void setup() {
  Serial.begin(9600);
  delay(4000);
  xOffset = 0;
  yOffset = 0;
  zOffset = 0;
  double samples = 100;
  
  Serial.println("Esplora Pitch Roll - Accelerometer Calibration");
  while (!Serial.available()){
    
  }
  if(Serial.available()){
    Serial.println("Calibration X, Y Axis. Please put Esplora on the plane on the table");  
    for (int i = 0 ; i < samples; i++){
      xOffset += Esplora.readAccelerometer(X_AXIS) ;    // read the X axis
      yOffset += Esplora.readAccelerometer(Y_AXIS) ;    // read the Y axis
    }
    Serial.println("X, Y Calibration done");
    Serial.readString();
    Serial.flush();
  }

  Serial.println("Calibration of Z Axis. Please put Esplora on the vertical plane");
  while (!Serial.available()){
    
  }
  if(Serial.available()){
    for (int i = 0 ; i < samples; i++){
      zOffset += Esplora.readAccelerometer(Z_AXIS) ;    // read the X axis
    }
    Serial.println("Calibration finish");
  }
  
  xOffset = xOffset/samples ;
  yOffset = yOffset/samples ;
  zOffset = zOffset/samples ;
  
  Serial.print("x: ");      // print the label for X
  Serial.print(xOffset);      // print the value for the X axis
  Serial.print("\ty: ");    // print a tab character, then the label for Y
  Serial.print(yOffset);      // print the value for the Y axis
  Serial.print("\tz: ");    // print a tab character, then the label for Z
  Serial.println(zOffset);
}
 
void loop() {
  
  double pitch, roll;
  float xAxis = Esplora.readAccelerometer(X_AXIS) -xOffset;    // read the X axis
  float yAxis = Esplora.readAccelerometer(Y_AXIS) -yOffset;    // read the Y axis
  float zAxis = Esplora.readAccelerometer(Z_AXIS) -zOffset; 

  fXg = xAxis * alpha + (fXg * (1.0 - alpha));
  fYg = yAxis * alpha + (fYg * (1.0 - alpha));
  fZg = zAxis * alpha + (fZg * (1.0 - alpha));

  //Roll &amp; Pitch Equations
  pitch  = (atan2(-fYg, fZg)*180.0)/M_PI;
  roll = (atan2(fXg, sqrt(fYg*fYg + fZg*fZg))*180.0)/M_PI;

  Serial.print(" Pitch: ");
  Serial.print(pitch);
  Serial.print(" Roll: ");
  Serial.println(roll);

  delay(100);
  
}

Para más explicaciones se puede consultar el siguiente enlace que proporciona más información de ejemplo.