Calibración dinámica de magnetómetro

Estoy trabajando en un magnetómetro AK8975 como parte de una IMU. Lo que parece ser muy complicado para mí. Este chip proporciona un vector 3D como salida que describe el campo magnético de la Tierra en cualquier lugar de la Tierra o cerca de él.

Probé dos tipos de algoritmos de cálculo de rumbo: uno es simple arctan(-y/x)y otro es matemática cancelada por inclinación (inclinación) y banco (balanceo) como se menciona a continuación. Tanto en inclinación como en bancos dan resultados incorrectos.

Puedo obtener el rumbo correcto de la tierra (usando recursos de estudio abiertos disponibles simples) cuando se gira manteniendo horizontal el plano del suelo usando cualquiera de los dos algos.

Intenté la calibración para errores de hierro blando y duro. Podría trazarlo en 3D y muestra una esfera 3D perfecta. Todavía no funciona en inclinación o declinación.

Cualquier puntero será útil.

El código y sus implementaciones son los siguientes:

void Compass_Heading()
{
  double MAG_X;
  double MAG_Y;
  double cos_roll;
  double sin_roll;
  double cos_pitch;
  double sin_pitch;

  cos_roll = cos(roll);
  sin_roll = sin(roll);
  cos_pitch = cos(pitch);
  sin_pitch = sin(pitch); 

  //// Tilt compensated Magnetic filed X:
  MAG_X = magnetom_x*cos_pitch + magnetom_y*sin_roll*sin_pitch + magnetom_z*cos_roll*sin_pitch;
  //// Tilt compensated Magnetic filed Y:
  MAG_Y = magnetom_y*cos_roll-magnetom_z*sin_roll;
  //// Magnetic Heading


  MAG_Heading = atan2(-MAG_Y, MAG_X) ;

}

Donde magnetom_x, #_yy #_zson componentes de un vector 3D que en realidad son los valores RAW del magnetómetro. el balanceo y la inclinación provienen de una salida misteriosa del filtro Kalman del acelerómetro y giroscopio a bordo. Estos tres sensores están en ATAVRSBIN1 . El balanceo y el lanzamiento están bien hasta esta etapa.

Ahora un simple cálculo de rumbo de acuerdo con journal_of_sensors_renaudin et al_2010c.pdf debería haber sido MAG_Heading = atan2(-magnetom_y, magnetom_x) ;y con una compensación como la anterior.

El código general es simplemente de OPEN AHRS .

Datos en formato Roll, Pitch y Yaw. Gire el dispositivo solo con mi mano. Los primeros tres se han concentrado solo en Roll, Pitch y Yaw respectivamente. El resto dos primero gira el dispositivo alrededor de 45 grados a lo largo de X (enrollado) y luego gira a lo largo de la Z local del magnetómetro. Luego se repite lo mismo con una rotación de alrededor de 45 grados a lo largo de Y (inclinado) y luego gira a lo largo de la Z local del magnetómetro.

Los gráficos trazados dentro del rango de -180 a 180 grados.

Ángulos en grados en un archivo Las características YAW en Roll.

Ángulos en grados en un archivo Las características YAW en Pitch.

Ángulos en grados en un archivo Las características YAW en el propio Yaw.

Ángulos en grados en un archivo Las características YAW en Yaw con 45 grados enrollados.

Ángulos en grados en un archivo Las características YAW en Yaw con 45 grados de inclinación.

Nota: Para las últimas 2 imágenes: primero se mantiene en la posición inicial, eso es lo mismo para todos (consulte los archivos txt). Luego se rodó 45 grados y luego se utilizó el dispositivo plano (con magnetómetro) a lo largo del eje Z del magnetómetro.

De manera similar para la última imagen, el dispositivo se ha inclinado 45 grados y luego a lo largo del eje Z del magnetómetro.

Espero que esto ayude a resolver mi problema.

Los nuevos desarrollos son los siguientes:

Trabajé un poco en el título. Obtuve el siguiente resultado. csv

csv

csv

Me gustan tus gráficos. Muestran claramente que el balanceo, el cabeceo y la guiñada parecen estar funcionando. ¡Felicidades! Eso ya es más progreso que la mayoría de la gente hace.

Supongo que el código que presentó está calculando el valor MAG_Heading "incorrecto", diferente del valor MAG_Heading que esperaba.

Sería mucho más fácil para nosotros ayudarlo si nos brinda: (Esta es la sección "describir los síntomas" de "Cómo hacer preguntas de manera inteligente" )

• los valores de salida del magnetómetro AK8975 m_x, m_y y m_z en algún punto único en el tiempo.
• Los valores de cabeceo y balanceo en el mismo instante.
• el valor de salida MAG_Heading supuestamente incorrecto calculado a partir de esos valores
• lo que esperaba que fuera el MAG_Heading correcto

Así que tengo que especular que tal vez te encuentres con el mismo tipo de problemas que creo para mí :-).

• ¿Qué formato de ángulo esperan sus funciones sin () y cos () y atan2 ()? ¿Necesita hacer algún tipo de conversión entre el formato pitch y roll que se almacenan en ese formato? ¿Necesita convertir de ese formato a lo que necesita MAG_heading? (brads, grados o radianes? punto flotante o punto fijo?)
• ¿Hay un desplazamiento en los valores m_x, m_y, m_z sin procesar que se debe restar?
• ¿Están todas las partes alineadas en la forma asumida por el código? En particular, ¿el eje de cabeceo y balanceo está alineado con el eje del magnetómetro? (¿Se supone que m_x apunta hacia adelante, a lo largo del eje de balanceo? ¿Se supone que m_y apunta hacia la derecha, a lo largo del eje de cabeceo?)
• ¿Quizás algún valor del sensor u otro, tal vez m_z, necesite ser negado antes de alimentar este código?
• ¿Quizás este código está siendo interrumpido por una interrupción u otra que corrompe sus valores internos? Me parece recordar un proyecto diferente que, después de que alguien puso una "división" en una rutina de interrupción, cada cálculo de la función trigonométrica en cualquier otra parte del programa a menudo daría un resultado incorrecto.
• ¿Tal vez las interrupciones se disparan con tanta frecuencia que este código nunca termina de ejecutarse?

Parece que hay otras personas discutiendo códigos muy similares en otros lugares: http://diydrones.com/forum/topics/heading-from-3d-magnetometer ; http://diydrones.ning.com/profiles/blogs/dcm-imu-theory-first-draft ; http://aeroquad.com/showthread.php?1138-REVOLUTION!!!-New-IMU !!! ; http://www.rcgroups.com/forums/showthread.php?t=1436742&page=6 ; http://aeroquad.com/showthread.php?691-Hold-your-heading-with-HMC5843-Magnetometer ; etc.

La nota de aplicación para el LSM303 tiene una guía útil para calibrar una brújula con compensación de inclinación que es aplicable a su problema. Es bastante detallado, de lo contrario habría reescrito los cálculos aquí. Tenga en cuenta que los valores del acelerómetro son necesarios para los cálculos de inclinación, balanceo y guiñada completos, ya que una rotación alrededor del eje de las líneas del campo magnético no produce cambios en los valores del magnetómetro. Del mismo modo para la gravedad con el acelerómetro.