Errores de ángulo en IMU (usando Accl y Gyro)

0

Estoy haciendo Autonomous Quad. Estoy programando mi IMU con ADXL335 -3 Axis analog Accelerometer & L3G4200D Gyro digital de 3 ejes. Estoy usando el launchpad LM4F120XL de TI basado en ARM Cortex M4F

Tengo un problema al implementar IMU. Aquí está mi algoritmo de IMU ...

Por ADC estoy leyendo el acelerómetro en Dx, Dy & Dz. Ahora que tengo 12bit ADC estoy usando la siguiente fórmula para obtener las lecturas en "g"

Rx = [(Dx*(3.3/4095) - Tx] / 0.3
Ry = [(Dy*(3.3/4095) - Ty] / 0.3
Rz = [(Dz*(3.3/4095) - Tz] / 0.3

En condición estable, he comprobado el Tx, Ty, Tz:

Tx = 1.630820
Ty = 1.658477
Tz = 1.997728

Para Gyro (a 250 dps con 8.75 mdps / dígito)

Para obtener la lectura en dps, estoy usando la siguiente fórmula

X = Gx*8.75/1000
Y = Gy*8.75/1000
Z = Gz*8.75/1000

(Aquí Gx, Gy & Gz son datos de giroscopios ... los he comprobado, son valores firmados y también son precisos (ruido mínimo))

Quiero medir los ángulos con respecto a todos los ejes (inclinación, balanceo y giro). Para eso estoy haciendo según lo siguiente

Th = 5;
T = 0.05; // approx. 5ms I've checked it using counter
c = 180/3.14;

while (1) {
  // initial values of Rxe, Rye, Rze are same as Rx, Ry, Rz
  Xd = atan2(Rxe, sqrt( (Rze*Rze) + (Rye*Rye) ));
  Yd = atan2(Rye, sqrt( (Rze*Rze) + (Rxe*Rxe) ));
  Zd = atan2(Rze, sqrt( (Rye*Rye) + (Rxe*Rxe) ));

  // To calculate Xavg, Yavg, Zavg from two consecutive X, Y, Z values respectively
  gyro();

  if ((Xavg >= Th) || (Xavg <= Th)) {
    Xd = Xd + (Xavg*T);
  }
  if ((Yavg >= Th) || (Yavg <= Th)) {
    Yd = Yd + (Yavg*T);
  }
  if ((Zavg >= Th) || (Zavg <= Th)) {
    Zd = Zd + (Zavg*T);
  }

  Rxg = sin(Yd) / (sqrt(1 + ((cos(Yd) * cos(Yd)) * (tan(Xd) * tan(Xd)))));
  Ryg = sin(Xd) / (sqrt(1 + ((cos(Xd) * cos(Xd)) * (tan(Yd) * tan(Yd)))));
  Rzg = ((Rzg > 0) ? 1 : ((Rzg < 0) ? -1 : 1))*(sqrt(1 - (Rxg*Rxg)-(Ryg*Ryg)));

  get_accl_data();            // To get ADC values
  accelerometer();            // To convert ADC valuse in "g" - Rx, Ry, Rz

  Rxe = (Rx*0.015)+(Rxg*0.985);      // weighted average
  Rye = (Ry*0.015)+(Ryg*0.985);
  Rze = (Rz*0.015)+(Rzg*0.985);

  Xd *= c;     // converting in degree
  Yd *= c;
  Zd *= c;

  Print(Xd);
  Print(Xd);
  Print(Xd);

  Xd /*=c;    // converting in radians again
  Yd /*=c;
  Zd /*=c;
}

Estoy imprimiendo Xd, Yd, Zd ... Está bien si se puede jugar o dentro de +/- 10 grados ... Pero por más de 10 grados estoy obteniendo valores de falla. Quiero decir, es informar si el dispositivo es satable o no O si se está moviendo en el lado positivo (en sentido contrario a las agujas del reloj) o en el lado negativo (en el sentido de las agujas del reloj) PERO no da la salida exacta de +/- 40 grados mientras está en un ángulo de 40 con respeto al suelo!

¿Qué me estoy perdiendo? Por favor, ayúdame ... Lo he hecho recomendando algunos documentos y blogs.

// todas las variables se toman correctamente (según mi conocimiento).

Una pregunta más: ¿Realmente necesito conocer estos ángulos? ¿O puedo usar los valores actuales también para hacer que mi cuadrángulo sea estable? ¿Qué otra información necesitaré para que mi cuadrángulo sea estable?

    

1 respuesta

1

(Esta pregunta se ha publicado hace más de un año)

Necesitas estos ángulos para aplicar un control, un PID, por ejemplo, para estabilizar el quadcopter en modo flotante. Se supone que tu control está activo cuando no estás controlando a distancia tu drone y quieres que sea perfectamente horizontal. Esto significa que los ángulos theta (eje x) y phi (eje y) estarán cerca de cero y alcanzarán el valor de referencia deseado (= 0) gracias al control.

Tengo una pregunta para usted: ¿ha registrado estos valores mientras los motores giraban? He notado un aumento importante en la amplitud de ruido causada por las vibraciones en el chasis.

    
respondido por el UserK

Lea otras preguntas en las etiquetas