Каждый, кто пробовал ставить на своего робота электронный компас задавался таким вопросом: а как, собственно, получить из этого прибора некую виртуальную стрелку, которая бы показывала на север? Если мы подключим к Ардуино самый популярный датчик HMC5883L, то получим поток чисел, которые ведут себя странным образом при его вращении. Что делать с этими данным? Попробуем разобраться, ведь полноценная навигация робота без компаса невозможна.
Во первых, устройство, которое часто называют компасом на самом деле является магнитометром. Магнитометр — это прибор, который измеряет напряженность магнитного поля. Все современные электронные магнитометры изготавливаются по технологии МЭМС и позволяют проводить измерения сразу по трем перпендикулярным осям. Так вот тот поток чисел, которые выдает прибор — это на самом деле проекции магнитного поля на три оси в системе координат магнитометра. Такой же формат данных имеют и другие устройства, используемые для позиционирования и навигации: акселерометр и гиротахометр (он же гироскоп).
На рисунке изображен простой случай, когда компас расположен горизонтально поверхности земли на экваторе. Красной стрелкой отмечено направление к северному полюсу. Пунктиром отмечены проекции этой стрелки на соответствующие оси.
Казалось бы, вот оно! Катет равен катету на тангенс противолежащего угла. Для того чтобы получить угол направления придется взять арктангенс отношения катетов:
H = atan(X/Y)
Если мы проведем эти несложные вычисления, мы действительно получим какой-то результат. Жаль только, что мы всё еще не получим верный ответ, ведь мы не учли кучу факторов:
- Смещение и искажение вектора магнитного поля Земли, вследствие внешних воздействий.
- Влияние тангажа и крена на показания компаса.
- Разница между географическим и магнитным полюсами — магнитное склонение.
В этой статье мы займемся изучением этих проблем и узнаем способы их решения. Но для начала посмотрим на показания магнитометра своими глазами. Для этого нам потребуется их как-то визуализировать.
Визуализация показаний магнитометра
Как известно, одна картинка лучше тысячи слов. Поэтому, для большей наглядности, воспользуемся 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 точек. Результат этого эксперимента приведен ниже:
Что можно сказать, глядя на этот рисунок? Во-первых, видно, что ось 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 можно узнать тут: http://en.wikipedia.org/wiki/Atan2)
Разница между географическим и магнитным полюсом
После того как мы получили более или менее точный угол отклонения стрелки компаса от северного направления, пришло время устранить еще одну проблему. Дело в том, что магнитный и географический полюсы на нашей планете, сильно различаются, в зависимости от того, где мы производим измерение. Другими словами, «север» на который показывает ваш походный компас, совсем не тот север где льды и ,белые медведи.
Для нивелирования этих различий, к показаниям датчика необходимо прибавить (или вычесть) определенный угол, называемый магнитным склонением. Например, в Екатеринбурге магнитное склонение имеет величину +14 градусов, а значит измеренные показания магнитометра следует уменьшить на эти же 14 градусов.
Для того чтобы выяснить магнитное склонение в ваших координатах, можно воспользоваться специальным ресурсом: http://magnetic-declination.com/
Заключение
В заключении несколько советов по навигации с помощью магнитометра.
- Калибровка должна проводиться именно в тех условиях, в которых беспилотник будет совершать реальный полет.
- Магнитометр лучше выносить из корпуса робота на штанге. Так на него будет влиять меньше шумов.
- Для вычисления направления лучше использовать связку компас + гироскоп. При этом их показания смешиваются по определенному правилу (data fusion).
- Если речь идет о летательном аппарате с большой курсовой скоростью, рекомендуется использовать связку компас + гироскоп + GPS.
Статья хорошая, порадовало, что упомянули и про влияние наклона и про отклонение физического полюса от магнитного.
Но только вот взял я этот магнитометр и почитал с него сырые значения и пригорюнился чтото — очень уж они (младший байт каждой оси, понятно) скачут при неподвижном даже объекте с отсутствием заметных переменных магнитных полей рядом. Никакой стабильности в показаниях. Думал усреднением чего-то добиться. Но всё одно, считывания 10-100 замеров никакой стабильности не давали. Ошибка уменьшалась в два раза с 100 значений, но всё одно непотребная была. С 1000 замеров уже был разбег всего в пару значений байта, что уже было пригодно для ориентации, но уже применять было бы трудно из-за длительности сбора такой пачки. В итоге кроме как «для справочки» применить турдно — сделать точную навигацию роботу на основе «компаса» у которого значения +-2 градуса скачут просто невозможно.
Александр, у вас что-то получилось с нормальной калибровкой компаса? Сейчас столкнулся с такой же проблемой…
Отлично.
А как же магнитное наклонение?
Насколько я понял — мы ищем углы поворота одной системы координат относительно другой, зная 2 вектора в одной системе (подвижной) и их значения в другой (неподвижной). Так?
И нам надо знать всего лишь 2 угла для поворота одной СК в другую?
И как тогда в дополнению к азимуту (рысканью) найти остальные 2 угла — крена по бортам и крена по носу/корма (тангаж)?
Рысканье = гироскоп + компас, тонгаж = компас + акселерометр, крен = компас +акселерометр.
* тонгаж = акселерометр + гироскоп, крен = акселерометр + гироскоп этот компас тьфу
Возможно в уравнении :
Xcal2 = Xcal*cos(pitch) + Ycal*sin(roll)*sin(pitch) + Zcal*cos(roll)*sin(pitch)
закралась ошибка. У компоненты Z не должны стоять cos(pitch)*sin(roll)?
не там правильно.. можно даташиты к разным компасам найти, тоже самое будет
Самое сложное и интересное, как обычно, упомянуто вскользь.
Откуда Вы брали питч и ролл? В какой размерности они должны быть? Каким образом согласовать оси магнитометра и акселерометра? Допустим, что используется другой датчик магнитометра или акселерометра, да еще и повернутый (перевернутый). Как, выведя в монитор порта снятые данные по осям, определить где какая ось? Почему-то у разных авторов описываются различные варианты питча и ролла, причем у некоторых опускание носа это питч в плюсе, а у других — в минусе. Аналогично с роллом. А еще лучше, сделали бы реальный пример наклонокомпенсированного компаса с выводом в монитор показаний магнитометра, акселерометра, питча, ролла и хеддинга.
1. «Откуда Вы брали питч и ролл?»
Расчёт значений углов тангажа и крена (pitch и roll) можно посмотреть в других статьях автора:
«Акселерометр: что это такое и как им определять наклон тела» (https://robotclass.ru/tutorials/accelerometer-angle-calculation/);
«Комплементарный фильтр» (https://robotclass.ru/articles/complementary-filter/).
2. «В какой размерности они должны быть?»
Тригонометрические функции считают в радианах.
3. «Каким образом согласовать оси магнитометра и акселерометра?»
Вроде бы тут всё просто (кроме случая, когда датчики расположены случайным образом).
1) Лучший вариант — расположить датчики таким образом, чтобы направление их осей совпадало.
2) Если совпадает расположение, но не направление, то значение осей, имеющих противоположное направление, берут со знаком минус с одного из датчиков.
3) Если не совпадает ни направление, ни расположение осей, но оси расположены взаимно параллельно/перпендикулярно. То с одного из датчиков используем подходящую ось. Например: ось X акселерометра совпадает с осью Y магнитометра, но обратна по направлению, тогда у одного из датчиков (например, магнитометра) вместо значений по оси X используем значения по оси Y со знаком минус.
4) Если оси датчиков не параллельны/перпендикулярны, то требуется перерасчёт, который явно выходит за рамки комментария. И вообще лучше просто избегать такого случая.
4. «Как, выведя в монитор порта снятые данные по осям, определить где какая ось?»
Информацию по датчикам лучше всего смотреть в datasheet. Но определить оси по показаниям конечно можно. Попробую объяснить на пальцах:
1) Запустите вывод данных с магнитометра в порт (нам нужен вывод трёх значений – x, y, z).
2) Расположите датчик более-менее горизонтально. Определите (например, с помощью компаса) направление на магнитный север.
Сначала определим расположение осей.
3) Поворачивайте датчик вокруг вертикальной оси до тех пор, пока выдаваемое датчиком значение по одной из осей не станет равно 0. В таком положении соответствующая ось расположена перпендикулярно направлению на магнитный север. Заметьте положение этой оси.
4) Продолжите поворачивать датчик вокруг вертикальной оси до тех пор пока выдаваемое датчиком значение ещё по одной оси не станет равно 0. Расположение обнулившейся оси так же будет перпендикулярно направлению на север. Заметьте положение этой оси.
5) Третья ось будет расположена перпендикулярно первым двум.
Теперь определим направление осей.
6) Расположите датчик более-менее горизонтально и поверните его так, чтобы одна из осей (пусть будет X) была примерно параллельна направлению Север-Юг.
7) Поверните датчик горизонтальной оси, перпендикулярной направлению Север-Юг, так, чтобы сторона датчика, направленная на север, опустилась вниз. Если при этом, значение, выдаваемое датчиком по оси X, увеличится, то ось X направлена в сторону севера. Если уменьшится, то ось X направлена в противоположную сторону – к Югу. (Условия правдивы для северного полушария)
8) Аналогично определите направление остальных осей.
5. «Почему-то у разных авторов описываются различные варианты питча и ролла, причем у некоторых опускание носа это питч в плюсе, а у других — в минусе. Аналогично с роллом.»
Тут в статье неразбериха — в разных местах указана функция с разными аргументами. Чтобы разобраться как правильно, для начала посмотрим референс Arduino IDE, там указан следующий вид: atan2 (double y, double x)
Функция выдаёт угол в радианах отложенный от оси X против часовой стрелки. Так как азимут отсчитывают по часовой стрелке, то значения по оси Y необходимо использовать со знаком «-«.
Автор, а Вы знаете, что магнитная система Земли направлена противоположно географической, т.е. географический север — это магнитный юг, и наоборот? Направление магнитного поля указано правильно, только подписи неверны.
Вы абсолютно правы! Подписи врут. Исправлено.
Цитата: «Другая проблема, из-за которой сфера оказалась повернутой, устраняется чуть сложнее. »
Олег, подскажите пожалуйста, как это исправить.
Отличная статья ! Спасибо автору!