Калибровка магнитометра. Электронный компас

Каждый, кто пробовал ставить на своего робота электронный компас задавался таким вопросом: а как, собственно, получить из этого прибора некую виртуальную стрелку, которая бы показывала на север? Если мы подключим к Ардуино самый популярный датчик HMC5883L, то получим поток чисел, которые ведут себя странным образом при его вращении. Что делать с этими данным? Попробуем разобраться, ведь полноценная навигация робота без компаса невозможна.

Калибровка магнитометра/компаса hmc5883l

Во первых, устройство, которое часто называют компасом на самом деле является магнитометром. Магнитометр — это прибор, который измеряет напряженность магнитного поля. Все современные электронные магнитометры изготавливаются по технологии МЭМС и позволяют проводить измерения сразу по трем перпендикулярным осям. Так вот тот поток чисел, которые выдает прибор — это на самом деле проекции магнитного поля на три оси в системе координат магнитометра. Такой же формат данных имеют и другие устройства, используемые для позиционирования и навигации: акселерометр и гиротахометр (он же гироскоп).

На рисунке изображен простой случай, когда компас расположен горизонтально поверхности земли на экваторе. Красной стрелкой отмечено направление к северному полюсу. Пунктиром отмечены проекции этой стрелки на соответствующие оси.

round2

Казалось бы, вот оно! Катет равен катету на тангенс противолежащего угла. Для того чтобы получить угол направления придется взять арктангенс отношения катетов:

H = atan(X/Y)

Если мы проведем эти несложные вычисления, мы действительно получим какой-то результат. Жаль только, что мы всё еще не получим верный ответ, ведь мы не учли кучу факторов:

  1. Смещение и искажение вектора магнитного поля Земли, вследствие внешних воздействий.
  2. Влияние тангажа и крена на показания компаса.
  3. Разница между географическим и магнитным полюсами — магнитное склонение.

В этой статье мы займемся изучением этих проблем и узнаем способы их решения. Но для начала посмотрим на показания магнитометра своими глазами. Для этого нам потребуется их как-то визуализировать.

1. Визуализация показаний магнитометра

Как известно, одна картинка лучше тысячи слов. Поэтому, для большей наглядности, воспользуемся 3D-редактором для визуализации показаний магнитометра. Для этих целей, можно использовать SketchUp с плагином «cloud» (http://rhin.crai.archi.fr/rld/plugin_details.php?id=678)

Указанный плагин позволяет загружать в SketchUp массивы точек из файла вида:
212 -321 -515
211 -320 -515
209 -318 -514
213 -319 -516

Разделителем может быть символ табуляции, пробел, точка с запятой и т.п. Всё это указывается в настройках плагина. Там же можно попросить склеить все точки треугольниками, что в нашем случае не требуется.

Самый простой способ сохранить показания магнитометра — передавать их через COM-порт на персональный компьютер в монитор последовательного порта, с последующим сохранением их в текстовый файл. Второй способ — подключить к Ардуино SD карту и записывать данные магнитометра в файл на SD карте.

Разобравшись с записью данных и с импортом их в SketchUp, попробуем теперь провести эксперимент. Будем вращать магнитометр вокруг оси Z, а управляющая программа в это время будет записывать показания датчика каждые 100 мс. Всего будет записано 500 точек. Результат этого эксперимента приведен ниже:

ringz

Что можно сказать, глядя на этот рисунок? Во-первых, видно, что ось Z действительно была зафиксирована — все точки расположены, более или менее, в плоскости XY. Во-вторых, плоскость XY немного наклонена, что может быть вызвано либо наклоном моего стола, либо наклоном магнитного поля Земли 🙂

Теперь взглянем на эту же картину сверху:

ringz_top

Первое, что бросается в глаза — центр координат находится совсем не в центре очерченного круга! Скорее всего, измеряемое магнитное поле чем-то «сдвинуто» в сторону. Причем это «что-то» имеет напряженность, выше оной у естественного поля Земли.

Второе наблюдение — круг немного вытянут в высоту, что указывает уже на более серьезные проблемы, о которых мы поговорим ниже.

А что получится, если вращать компас вокруг всех осей одновременно? Правильно, получится не круг, а сфера (точнее сфероид). Вот такая сфера получилась у меня:

fullxyz

Дополнительно к основным 500 точкам сферы, добавлены еще три массива, по 500 точек в каждом. Каждая из добавленных групп точек отвечает за вращение магнитометра вокруг фиксированной оси. Так, нижний круг получен вращением прибора вокруг оси Z. Круг справа — вращением вокруг оси Y. Наконец, плотное кольцо точек слева отвечает за вращение магнитометра вокруг оси X. Почему эти круги не опоясывают шар по экватору, читаем ниже.

2. Магнитное наклонение

На самом деле, последний рисунок может показаться немного странным. Почему будучи в горизонтальном состоянии, датчик показывает почти максимальное значение по оси Z?? Ситуация повторяется если мы наклоним прибор, например, осью X вниз — опять получим максимальное значение (левый круг). Получается, что на датчик постоянно действует поле направленное сквозь датчик вниз к поверхности земли!

Ничего необычного в этом на самом деле нет. Эта особенность магнитного поля земли называется магнитным наклонением. На экваторе поле направлено параллельно земле. В южном полушарии — вверх от земли под некоторым углом. А в северном полушарии, как мы уже наблюдали — вниз. Смотрим картинку.

inclination

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

Теперь же перейдем, непосредственно к проблемам.

2.1. Искажения магнитного поля: Hard & Soft Iron

В зарубежной литературе, искажения магнитного поля принято делить на две группы: Hard Iron и Soft Iron. Ниже приведена картинка, иллюстрирующая суть этих искажений.

magnetic-field-distortion-web

Hard Iron

Даю справку. Интенсивность магнитного поля земли сильно зависит от земных координат, в которых оно измеряется. Например, в Кейп Тауне (Южная Африка) поле составляет около 0.256 Гс (Гаусс), а в Нью-Йорке в два раза больше — 0.52 Гс. В целом по планете, интенсивность магнитного поля варьируется в диапазоне от 0.25 Гс до 0.65 Гс.

Для сравнения, поле обычного магнитика на холодильник составляет 50 Гс, — это в сто раз больше чем магнитное поле в Нью Йорке!! Понятно, что чуткий магнитометр может легко запутаться, если рядом с ним возникнет один из таких магнитов. На квадрокоптере, конечно, таких магнитиков нет, но зато есть куда более мощные редкоземельные магниты вентильных двигателей, а еще электронные цепи контроллера, провода питания и аккумуляторная батарея.

Такие источники паразитного магнитного поля называют Hard Iron. Воздействуя на магнитометр, они придают некоторое смещение измеряемым значениям. Посмотрим, имеются ли Hard Iron искажения у нашей сферы. Проекция точек сферы на плоскость XY, выглядит следующим образом:

full_side

Видно, что облако точек имеет некоторое заметное смещение по оси Y влево. По оси Z смещение практически отсутствует. Ликвидировать такое искажение очень просто: достаточно увеличить или уменьшить получаемые от прибора значения на величину смещения. Например, калибровка Hard Iron для оси Y будет иметь вид:

Ycal_hard = Y — Ybias

где Ycal_hard — калиброванное значение;
Y — исходное значение;
Ybias — величина смещения.

Чтобы вычислить Ybias нам потребуется зафиксировать максимальное и минимальное значение Y, а затем воспользоваться простым выражением:

Ybias = (Ymin-Ymax)/2 — Ymin

где Ybias — искомая величина смещения;
Ymin — минимальное значение оси Y;
Ymax — максимальное значение оси Y.

Soft Iron

В отличие от Hard Iron, искажение типа Soft носит куда более коварный характер. Опять же, проследим этот вид воздействия на собранных ранее данных. Для этого, обратим внимание на то, что шар на картинке сверху, и не шар вовсе. Его проекция на ось YZ немного сплющена сверху, и слегка повернута против часовой стрелки. Вызваны эти искажения, наличием ферромагнитных материалов рядом с датчиком. Таким материалом является металлическая рама квадрокоптера, корпус двигателя, проводка, или даже металлические болты крепления.

Исправить ситуацию со сплющенностью поможет умножение показаний датчика на некоторый множитель:

Ycal_soft = Y * Yscale

где Ycal_hard — калиброванное значение;
Y — исходное значение;
Yscale — коэффициент масштабирования.

Для того чтобы найти все коэффициенты (для X,Y и Z) необходимо выявить ось с наибольшей разностью между максимальным и минимальным значением, и затем воспользоваться формулой:

Yscale = (Amax-Amin)/(Ymax-Ymin)

где Yscale — искомый коэффициент искажения по оси Y;
Amax — максимальное значение на некоторой оси;
Amin — минимальное значение на некоторой оси;
Ymax — максимальное значение на оси Y;
Ymin — минимальное значение на оси Y.

Другая проблема, из-за которой сфера оказалась повернутой, устраняется чуть сложнее. Однако, вклад такого искажения в общую ошибку измерения достаточно мал, и мы не будем подробно расписывать способ его «ручного» нивелирования.

2.2. Автоматическая калибровка

Надо сказать, получение вручную точных минимальных и максимальных показаний магнитометра задача не из простых. Для этой процедуры, как минимум, потребуется специальный стенд, в котором можно фиксировать одну из осей прибора.

Гораздо проще воспользоваться автоматическим алгоритмом калибровки. Суть этого метода состоит в аппроксимации облака полученных точек элипсоидом. Другими словами, мы подбираем параметры элипсоида таким образом, чтобы он максимально точно совпадал с нашим облаком точек, построенных на основе показаний магнитометра. Из подобранных таким образом параметров, мы сможем добыть величину смещения, коэффициенты масштаба и коэффициенты для ортогонализации осей.

В интернете можно найти несколько программ, пригодных для этого. Например, MagCal, или еще одна — Magneto. В отличие от MagCal, в Magneto рассчитанные параметры выводятся в готовом к использованию виде, без необходимости дополнительных преобразований. Именно этой программой мы и воспользуемся. Главная и единственная форма программы выглядит следующим образом:

magneto

В поле «Raw magnetic measurements» выбираем файл с исходными данными. В поле «Norm of Magnetic or Gravitational field» вводим величину магнитного поля Земли в точке нашей дислокации. Учитывая, что этот параметр никак не влияет на угол отклонения стрелки нашего виртуального компаса, я поставил значение 1090, что соответствует значению 1 Гаусс.

Затем жмем кнопку Calibrate и получаем:

  1. значения смещения по всем трем осям: Combined bias (b);
  2. и матрицу масштаба и ортогонализации: Correction for combined scale factors, misalignments and soft iron (A-1).

С помощью волшебной матрицы мы ликвидируем сплющенность нашего облака и устраним его легкое вращение. Общая формула калибровки выглядит следующим образом:

Vcal = A-1 * (V — Vbias)

где Vcal — вектор калиброванных значение магнитометра для трех осей;
A-1 — матрица масштаба и ортогонализации;
Vbias — вектор смещения по трем осям.

3. Влияние наклона магнитометра на вычисляемое направление

На очереди проблема номер два. В начале статьи мы уже попробовали вычислить угол между севером и стрелкой компаса. Для этого годится простая формула:

H = atan(Y/X)

где H — угол отклонения стрелки компаса от северного направления;
X,Y — калиброванные значения магнитометра.

Представим теперь, что мы фиксируем ось X строго по направлению к северу, и начинаем вращать датчик вокруг этой оси (придаем крен). Получается, что проекция поля на ось X остается неизменной, а вот проекция на Y меняется. Согласно формуле, стрелка компаса будет показывать либо на северо-запад, либо на северо-восток, в зависимости от того, в какую сторону делаем крен. Это и есть, заявленная в начале статьи, вторая проблема электронного компаса.

Решить проблему поможет геометрия. Нам нужно всего лишь повернуть магнитный вектор в систему координат, заданную инклинометром. Для этого, поочередно перемножим две матрицы косинусов на вектор:

Vcal2 = Ry*Rx*Vcal

где Vcal — магнитный вектор, очищенный от Hard и Soft искажений;
Rx и Ry — матрицы поворота вокруг осей X и Y;
Vcal2 — магнитный вектор, очищенный от влияния крена и тангажа.

Пригодная для программы контроллера формула будет иметь вид:

Xcal2 = Xcal*cos(pitch) + Ycal*sin(roll)*sin(pitch) + Zcal*cos(roll)*sin(pitch)
Ycal2 = Ycal*cos(roll) — Zcal*sin(roll)
H = atan2( -Ycal2, Xcal2 )

где roll и pitch — наклоны вокруг осей X и Y;
Xcal,Ycal,Zcal — вектор магнитометра (Vcal);
Ycal2, Ycal2 — калиброванные значения магнитометра (Zcal2 не считаем — он нам не пригодится);
H — угол между севером и стрелкой компаса.

(О том, кто такой atan2 можно узнать тут: http://en.wikipedia.org/wiki/Atan2)

3. Разница между географическим и магнитным полюсом

После того как мы получили более или менее точный угол отклонения стрелки компаса от северного направления, пришло время устранить еще одну проблему. Дело в том, что магнитный и географический полюсы на нашей планете, сильно различаются, в зависимости от того, где мы производим измерение. Другими словами, «север» на который показывает ваш походный компас, совсем не тот север где льды и ,белые медведи.

Для нивелирования этих различий, к показаниям датчика необходимо прибавить (или вычесть) определенный угол, называемый магнитным склонением. Например, в Екатеринбурге магнитное склонение имеет величину +14 градусов, а значит измеренные показания магнитометра следует уменьшить на эти же 14 градусов.

Для того чтобы выяснить магнитное склонение в ваших координатах, можно воспользоваться специальным ресурсом: http://magnetic-declination.com/

Заключение

В заключении несколько советов по навигации с помощью магнитометра.

  1. Калибровка должна проводиться именно в тех условиях, в которых беспилотник будет совершать реальный полет.
  2. Магнитометр лучше выносить из корпуса робота на штанге. Так на него будет влиять меньше шумов.
  3. Для вычисления направления лучше использовать связку компас + гироскоп. При этом их показания смешиваются по определенному правилу (data fusion).
  4. Если речь идет о летательном аппарате с большой курсовой скоростью, рекомендуется использовать связку компас + гироскоп + GPS.

Изменено:

Калибровка магнитометра. Электронный компас: 2 комментария

  1. Статья хорошая, порадовало, что упомянули и про влияние наклона и про отклонение физического полюса от магнитного.
    Но только вот взял я этот магнитометр и почитал с него сырые значения и пригорюнился чтото — очень уж они (младший байт каждой оси, понятно) скачут при неподвижном даже объекте с отсутствием заметных переменных магнитных полей рядом. Никакой стабильности в показаниях. Думал усреднением чего-то добиться. Но всё одно, считывания 10-100 замеров никакой стабильности не давали. Ошибка уменьшалась в два раза с 100 значений, но всё одно непотребная была. С 1000 замеров уже был разбег всего в пару значений байта, что уже было пригодно для ориентации, но уже применять было бы трудно из-за длительности сбора такой пачки. В итоге кроме как «для справочки» применить турдно — сделать точную навигацию роботу на основе «компаса» у которого значения +-2 градуса скачут просто невозможно.

  2. Отлично.
    А как же магнитное наклонение?
    Насколько я понял — мы ищем углы поворота одной системы координат относительно другой, зная 2 вектора в одной системе (подвижной) и их значения в другой (неподвижной). Так?
    И нам надо знать всего лишь 2 угла для поворота одной СК в другую?
    И как тогда в дополнению к азимуту (рысканью) найти остальные 2 угла — крена по бортам и крена по носу/корма (тангаж)?

Добавить комментарий

Ваш e-mail не будет опубликован. Обязательные поля помечены *