Калибровка магнитометра. Электронный компас
Каждый, кто пробовал ставить на своего робота электронный компас, задавался таким вопросом: а как, собственно, получить из этого прибора некую виртуальную стрелку, которая бы показывала на север? Если мы подключим к Arduino самый популярный датчик HMC5883L, то получим поток чисел, которые ведут себя странным образом при его вращении. Что делать с этими данными? Попробуем разобраться, ведь полноценная навигация робота без компаса невозможна.
Примечание
Во-первых, устройство, которое часто называют компасом, на самом деле является магнитометром. Магнитометр — это прибор, который измеряет напряжённость магнитного поля. Все современные электронные магнитометры изготавливаются по технологии МЭМС и позволяют проводить измерения сразу по трём перпендикулярным осям. Так вот, тот поток чисел, которые выдаёт прибор, — это на самом деле проекции магнитного поля на три оси в системе координат магнитометра.
Такой же формат данных имеют и другие устройства, используемые для позиционирования и навигации: акселерометр и гиротахометр (он же гироскоп).
На рисунке изображён простой случай, когда компас расположен горизонтально поверхности земли на экваторе. Красной стрелкой отмечено направление к северному полюсу. Пунктиром отмечены проекции этой стрелки на соответствующие оси.
Казалось бы, вот оно! Катет равен катету на тангенс противолежащего угла. Для того чтобы получить угол направления, придётся взять арктангенс отношения катетов:
H = atan(X/Y)
Если мы проведём эти несложные вычисления, мы действительно получим какой-то результат. Жаль только, что мы всё ещё не получим верный ответ, ведь мы не учли кучу факторов:
Смещение и искажение вектора магнитного поля Земли вследствие внешних воздействий.
Влияние тангажа и крена на показания компаса.
Разница между географическим и магнитным полюсами — магнитное склонение.
В этой статье мы займёмся изучением этих проблем и узнаем способы их решения. Но для начала посмотрим на показания магнитометра своими глазами. Для этого нам потребуется их как-то визуализировать.
Визуализация показаний магнитометра
Как известно, одна картинка лучше тысячи слов. Поэтому, для большей наглядности, воспользуемся 3D-редактором для визуализации показаний магнитометра. Для этих целей можно использовать SketchUp с плагином «cloud».
Указанный плагин позволяет загружать в SketchUp массивы точек из файла вида:
212 -321 -515
211 -320 -515
209 -318 -514
213 -319 -516
Разделителем может быть символ табуляции, пробел, точка с запятой и т. п. Всё это указывается в настройках плагина. Там же можно попросить склеить все точки треугольниками, что в нашем случае не требуется.
Самый простой способ сохранить показания магнитометра — передавать их через COM-порт на персональный компьютер в монитор последовательного порта, с последующим сохранением их в текстовый файл. Второй способ — подключить к Arduino SD-карту и записывать данные магнитометра в файл на SD-карте.
Разобравшись с записью данных и с импортом их в SketchUp, попробуем теперь провести эксперимент. Будем вращать магнитометр вокруг оси Z, а управляющая программа в это время будет записывать показания датчика каждые 100 мс. Всего будет записано 500 точек. Результат этого эксперимента приведён ниже:
Что можно сказать, глядя на этот рисунок? Во-первых, видно, что ось Z действительно была зафиксирована — все точки расположены, более или менее, в плоскости XY. Во-вторых, плоскость XY немного наклонена, что может быть вызвано либо наклоном моего стола, либо наклоном магнитного поля Земли :)
Теперь взглянем на эту же картину сверху:
Первое, что бросается в глаза — центр координат находится совсем не в центре очерченного круга! Скорее всего, измеряемое магнитное поле чем-то «сдвинуто» в сторону. Причём это «что-то» имеет напряжённость, выше оной у естественного поля Земли.
Второе наблюдение — круг немного вытянут в высоту, что указывает уже на более серьёзные проблемы, о которых мы поговорим ниже.
А что получится, если вращать компас вокруг всех осей одновременно? Правильно, получится не круг, а сфера (точнее сфероид). Вот такая сфера получилась у меня:
Дополнительно к основным 500 точкам сферы добавлены ещё три массива, по 500 точек в каждом. Каждая из добавленных групп точек отвечает за вращение магнитометра вокруг фиксированной оси. Так, нижний круг получен вращением прибора вокруг оси Z. Круг справа — вращением вокруг оси Y. Наконец, плотное кольцо точек слева отвечает за вращение магнитометра вокруг оси X. Почему эти круги не опоясывают шар по экватору, читаем ниже.
Магнитное наклонение
На самом деле, последний рисунок может показаться немного странным. Почему будучи в горизонтальном состоянии, датчик показывает почти максимальное значение по оси Z?? Ситуация повторяется, если мы наклоним прибор, например, осью X вниз — опять получим максимальное значение (левый круг). Получается, что на датчик постоянно действует поле, направленное сквозь датчик вниз к поверхности земли!
Ничего необычного в этом на самом деле нет. Эта особенность магнитного поля земли называется магнитным наклонением. На экваторе поле направлено параллельно земле. В южном полушарии — вверх от земли под некоторым углом. А в северном полушарии, как мы уже наблюдали, — вниз. Смотрим картинку.
Магнитное наклонение никак не помешает нам пользоваться компасом, поэтому не будем о нём особо задумываться, а просто примем к сведению этот интересный факт.
Теперь же перейдём непосредственно к проблемам.
Искажения магнитного поля: Hard & Soft Iron
В зарубежной литературе искажения магнитного поля принято делить на две группы: Hard Iron и Soft Iron. Ниже приведена картинка, иллюстрирующая суть этих искажений.
Hard Iron
Даю справку. Интенсивность магнитного поля земли сильно зависит от земных координат, в которых оно измеряется. Например, в Кейп Тауне (Южная Африка) поле составляет около 0.256 Гс (Гаусс), а в Нью-Йорке в два раза больше — 0.52 Гс. В целом по планете интенсивность магнитного поля варьируется в диапазоне от 0.25 Гс до 0.65 Гс.
Предупреждение
Для сравнения, поле обычного магнитика на холодильник составляет 50 Гс — это в сто раз больше, чем магнитное поле в Нью-Йорке!
Понятно, что чуткий магнитометр может легко запутаться, если рядом с ним возникнет один из таких магнитов. На квадрокоптере, конечно, таких магнитиков нет, но зато есть куда более мощные редкоземельные магниты вентильных двигателей, а ещё электронные цепи контроллера, провода питания и аккумуляторная батарея.
Такие источники паразитного магнитного поля называют Hard Iron. Воздействуя на магнитометр, они придают некоторое смещение измеряемым значениям. Посмотрим, имеются ли Hard Iron искажения у нашей сферы. Проекция точек сферы на плоскость XY выглядит следующим образом:
Видно, что облако точек имеет некоторое заметное смещение по оси 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.
Другая проблема, из-за которой сфера оказалась повёрнутой, устраняется чуть сложнее. Однако, вклад такого искажения в общую ошибку измерения достаточно мал, и мы не будем подробно расписывать способ его «ручного» нивелирования.
Автоматическая калибровка
Надо сказать, получение вручную точных минимальных и максимальных показаний магнитометра — задача не из простых. Для этой процедуры, как минимум, потребуется специальный стенд, в котором можно фиксировать одну из осей прибора.
Гораздо проще воспользоваться автоматическим алгоритмом калибровки. Суть этого метода состоит в аппроксимации облака полученных точек элипсоидом. Другими словами, мы подбираем параметры элипсоида таким образом, чтобы он максимально точно совпадал с нашим облаком точек, построенных на основе показаний магнитометра. Из подобранных таким образом параметров мы сможем добыть величину смещения, коэффициенты масштаба и коэффициенты для ортогонализации осей.
В интернете можно найти несколько программ, пригодных для этого. Например, MagCal, или ещё одна — Magneto. В отличие от MagCal, в Magneto рассчитанные параметры выводятся в готовом к использованию виде, без необходимости дополнительных преобразований. Именно этой программой мы и воспользуемся. Главная и единственная форма программы выглядит следующим образом:
В поле Raw magnetic measurements выбираем файл с исходными данными. В поле Norm of Magnetic or Gravitational field вводим величину магнитного поля Земли в точке нашей дислокации.
Совет
Учитывая, что этот параметр никак не влияет на угол отклонения стрелки нашего виртуального компаса, я поставил значение 1090, что соответствует значению 1 Гаусс.
Затем жмём кнопку Calibrate и получаем:
значения смещения по всем трём осям: Combined bias (b);
и матрицу масштаба и ортогонализации: Correction for combined scale factors, misalignments and soft iron (A-1).
С помощью волшебной матрицы мы ликвидируем сплющенность нашего облака и устраним его лёгкое вращение. Общая формула калибровки выглядит следующим образом:
Vcal = A-1 * (V - Vbias)
где Vcal — вектор калиброванных значений магнитометра для трёх осей; A-1 — матрица масштаба и ортогонализации; Vbias — вектор смещения по трём осям.
Влияние наклона магнитометра на вычисляемое направление
На очереди проблема номер два. В начале статьи мы уже попробовали вычислить угол между севером и стрелкой компаса. Для этого годится простая формула:
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, можно узнать тут: en.wikipedia.org/wiki/Atan2)
Разница между географическим и магнитным полюсом
После того как мы получили более или менее точный угол отклонения стрелки компаса от северного направления, пришло время устранить ещё одну проблему. Дело в том, что магнитный и географический полюсы на нашей планете сильно различаются, в зависимости от того, где мы производим измерение. Другими словами, «север», на который показывает ваш походный компас, совсем не тот север, где льды и белые медведи.
Для нивелирования этих различий, к показаниям датчика необходимо прибавить (или вычесть) определённый угол, называемый магнитным склонением. Например, в Екатеринбурге магнитное склонение имеет величину +14 градусов, а значит, измеренные показания магнитометра следует уменьшить на эти же 14 градусов.
Совет
Для того чтобы выяснить магнитное склонение в ваших координатах, можно воспользоваться специальным ресурсом: magnetic-declination.com
Заключение
В заключение несколько советов по навигации с помощью магнитометра.
Калибровка должна проводиться именно в тех условиях, в которых беспилотник будет совершать реальный полёт.
Магнитометр лучше выносить из корпуса робота на штанге. Так на него будет влиять меньше шумов.
Для вычисления направления лучше использовать связку компас + гироскоп. При этом их показания смешиваются по определённому правилу (data fusion).
Если речь идёт о летательном аппарате с большой курсовой скоростью, рекомендуется использовать связку компас + гироскоп + GPS.