Динамическая калибровка магнитометра

19

Я работаю над магнитометром AK8975, являющимся частью IMU. Что кажется мне очень сложным. Эта микросхема выдает трехмерный вектор в качестве выходного сигнала, описывающего магнитное поле Земли в любом месте на земле или вблизи нее.

Я попробовал два типа алгоритмов вычисления заголовка: один простой, arctan(-y/x)а другой математика отмены наклона (тангажа) и банка (крена), как указано ниже. И по наклонности, и по банкам дают неправильный вывод.

Я могу получить правильный курс относительно земли (используя простые доступные открытые учебные ресурсы), когда он вращается, сохраняя горизонтальный относительно плана земли, используя любой из двух алгоритмов.

Я пробовал калибровку на ошибки мягкого и твердого железа. Я мог бы построить его в 3D и показать идеальную 3D-сферу. Все еще не работает на склонность или склонение.

Любой указатель будет полезен.

Код и его реализации, как показано ниже:

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) ;

}

Там , где magnetom_x, #_yи #_zявляются компонентами 3D вектора , который на самом деле является значением RAW от магнитометра. крен и шаг от загадочного выхода фильтра Калмана от бортового акселерометра и гироскопа. Эти три датчика находятся в ATAVRSBIN1 . Крен и подача в порядке до этой стадии.

Теперь простой расчет заголовка в соответствии с journal_of_sensors_renaudin et al_2010c.pdf должен был быть MAG_Heading = atan2(-magnetom_y, magnetom_x) ;с компенсацией, как указано выше.

Общий код просто из открытых AHRS .


Данные в формате Roll, Pitch и Yaw. Я повернул устройство только рукой. Первые три были сконцентрированы только на Roll, Pitch и Yaw соответственно. Остальные два сначала поворачивают устройство примерно на 45 градусов вдоль X (прокручено), затем поворачивают вдоль локального Z магнитометра. Затем то же самое повторяют с вращением на 45 градусов вдоль Y (наклонно), затем поворачивают вдоль локального Z магнитометра.

Графики построены в диапазоне от -180 до 180 градусов.

Рулон Углы в градусах в файле Характеристики YAW на Roll.

Подача Углы в градусах в файле Характеристики YAW на Pitch.

рыскание Углы в градусах в файле Характеристики YAW на самом Yaw.

Угол рыскания под углом 45 градусов Углы в градусах в файле Характеристики YAW на Yaw с наклоном 45 градусов.

Тяга относительно 45 градусов наклона (наклон) Углы в градусах в файле Характеристики YAW на Yaw с углом 45 градусов.

Примечание: для последних 2 изображений: сначала хранится в исходном положении, то же самое для всех (см. Текстовые файлы). Затем повернули на 45 градусов, затем с помощью плоского устройства (с магнитометром) повернули вдоль оси Z магнитометра.

Аналогично для последнего изображения устройство было наклонено на 45 градусов, а затем вдоль оси Z магнитометра.

Я надеюсь, что это поможет решить мою проблему.


Новые разработки заключаются в следующем:

Я работал над заголовком. Я получил следующий вывод. Рулон CSV

Подача CSV

рыскание CSV


Rick2047
источник
пока нет ответа !!
4
Я думаю, что вы получите больше отклика, если вы покажете математику, которую вы пытаетесь реализовать, и код, который вы использовали для ее реализации. Нам мало что нужно делать иначе, чем «это не работает, помогите» - так гласит ваш вопрос. Сожалею!
Мартин Томпсон
Использование магнитометра является очень специализированной областью, с которой сравнительно немного людей будут иметь опыт. Прочитав ваш вопрос несколько раз, я все еще не уверен, что именно не так. Вы говорите, что это дает "неправильный вывод", но это довольно расплывчато. Может быть, некоторые числовые примеры?
Джейсон Р
1
Это вопрос о том, как интерпретировать выходные данные датчика или как рассчитать полезные для навигации меры из вектора x, y, z, который дает датчик? Ваши измерения повторяются с другим экземпляром того же датчика?
Викацу
1
@Rahul - я удивлен, что это не привлекает больше внимания!
Кевин Вермеер

Ответы:

8

Мне нравятся твои графики. Они ясно показывают, что крен, тангаж и рыскание работают. Поздравляем! Это уже больший прогресс, чем большинство людей.

Я предполагаю, что код, который вы представили, вычисляет «неправильное» значение MAG_Heading, отличное от ожидаемого значения MAG_Heading.

Нам было бы намного легче помочь вам, если бы вы дали нам: (Это раздел «Опишите симптомы» «Как правильно задавать вопросы» )

  • AK8975 магнитометр выходные значения M_X, M_Y и m_z в какой - то один момент времени.
  • Значения высоты тона и крена в одно и то же время
  • предположительно неправильное выходное значение MAG_Heading, рассчитанное по этим значениям
  • то, что вы ожидали, будет правильным MAG_Heading

Поэтому я могу предположить, что, возможно, вы сталкиваетесь с теми же проблемами, которые я создаю для себя :-).

  • Какой формат угла ожидают ваши функции sin () и cos () и atan2 ()? Вам нужно сделать какое-то преобразование между форматом, тональностью и креном, хранящимися в этом формате? Вам нужно конвертировать из этого формата в то, что вам нужно MAG_heading? (скобки, градусы или радианы? с плавающей или с фиксированной точкой?)
  • Есть ли смещение в необработанных значениях m_x, m_y, m_z, которые необходимо вычесть?
  • Выстроены ли все части так, как это предусмотрено кодом? В частности, совпадает ли ось тангажа и крена с осью магнитометра? (Должен ли m_x указывать вперед, вдоль оси крена? Должен ли m_y указывать вправо, вдоль оси тангажа?)
  • Может быть, какое-то значение датчика или другое - возможно, m_z - необходимо отменить, прежде чем вводить в этот код?
  • Может быть, этот код прерывается тем или иным прерыванием, которое портит его внутренние значения? Кажется, я вспоминаю другой проект, в котором после того, как кто-то вставил «деление» в подпрограмму прерывания, каждый расчет функции триггера в других местах программы часто давал неверный результат.
  • Может быть, прерывания запускаются так часто, что этот код на самом деле никогда не заканчивается?

Кажется, есть другие люди, обсуждающие очень похожий код в другом месте: 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 ; и т.п.

davidcary
источник
Я видел это сейчас .. Я вернусь.
Rick2047
\ 1 / Я создаю CSV-файл для [Roll, Pitch, Yaw и (Mx, My, Mz)]. \ 2 / Я ожидаю, что заголовок MAG_Heading не должен меняться с изменением крена и тангажа по крайней мере до двух последовательных квадрантов. Направление означает, что если оно направляется на NE, то оно должно продолжать указывать NE до тех пор, пока оно не пересекает 90 градусов от горизонта в направлениях вращения вверх или вниз для наклона и должно быть одинаковым в случае крена или комбинации.
Rick2047
\ 3 / Все внутренние расчеты были выполнены в радианах, и все функции-консины рассчитывают только радианы. Для отображения только значения копируются и преобразуются в градусы. \ 4 / С плавающей точкой. \ 5 / Все датчики выровнены, а также все возможные соответствующие комбинации были опробованы для выравнивания с использованием SENSOR_SIGN [9]. \ 6 / Я делаю это в опросе, поэтому проблем, связанных с прерываниями, здесь не будет.
Rick2047
Для моей точки \ 1 / здесь для всех графиков я также приложил соответствующие CSV-файлы. Или вы просили что-то еще. Спасибо за этот хороший ответ. :) Я буду реализовывать «Как задавать вопросы умным путем» настолько, насколько позволят мои усилия и время. :)
Rick2047
\ 1 / Я скорее буду работать на [Roll, pitch, yaw и (Mx, My, Mz) (курс, склонение)].
Rick2047
1

Приложение к сведению для LSM303 имеет полезное руководство для калибровки с компенсацией наклона компаса , который применим к вашей проблеме. Это довольно подробно, иначе я бы переписал здесь расчеты. Обратите внимание, что значения акселерометра необходимы для расчета полного шага, крена и рыскания, поскольку вращение вокруг оси линий магнитного поля не приводит к изменению значений магнитометра. Аналогично для гравитации с акселерометром.

geometrikal
источник
Ссылка показывает разные знаки в одном и том же уравнении. Я попробую все другие комбинации.
Rick2047