Калибровка компаса — Хардкор

Этот пост немного выбивается из общей тематики блога и будет интересен скорее интересующимся DIY и роботостроением, чем программистам, однако нам пригодится все: и навык программирования в области анализа данных, и математическая смекалка, и 3D-воображение 🙂
 

Небольшое интро

Очень часто перед создателями квадрокоптеров и прочих передвигающихся штук встает проблема- нужно определять ориентацию в пространстве. Крен, тангаж, рыскание — термины как раз из авиации. Так вот, для этого применяют следующие устройства: акселлерометр (измеряет линейное ускорение + гравитацию), гироскоп (измеряет скорость вращения вокруг 3-х осей) и, наконец, магнитометр, или по-простому компас, который измеряет напряженность и ориентацию магнитного поля. Писать на тему ориентации в пространстве можно (и нужно) много, однако сегодняшний топик- калибровка компаса на примере HMC5883L, самой популярной в среде DIY модели, который умеет по шине I2C (стандартной для большинства сенсоров) отдавать 3-х мерный вектор магнитного поля.

Проблемы

 

magnetometer point cloud animated

Облако точек с компаса (кликабельно, GIF)

Нельзя просто так взять новый компас и начать его использовать. В идеале, если мы будем вращать компас, то вектор должен сохранять свою длину (так-как напряженность поля не меняется) и описывать сферу вокруг нуля. Разумеется, на практике это не так: оси сенсора имеют разную чувствительность и сдвиг относительно нуля, но, что хуже, они часто не совсем перпендикулярны, не говоря уже про всякие источники искажения самого магнитного поля (Hard iron & Soft iron). Поэтому нужно получить калибровочную матрицу трансформации для конкретного компаса. Самый простой и популярный подход- покрутить компас, собрать облако точек, предположить, что это эллипсоид и вычислить матрицу, которая «сожмет» его до сферы. Есть довольно много готовых реализаций такой калибровки и их объединяет один минус: они не способны исправить искривление и поворот эллипсоида, вызванные неточным взаимным размещением осей внутри сенсора, потому что это невозможно сделать имея лишь облако точек.

Для кого-то это приемлемо, но не для нас! Только хардкор!

Дано

  • сам компас (HMC5883L или любой другой) + любой контроллер, который умеет общаться с I2C девайсами, например Arduino (я рассчитываю, что мы умеем его программировать)
  • свободная поверхность стола, желательно, выставленного по уровню
  • коробка или брусок с взаимно-перпендикулярными и ровными сторонами (проверяем угольником или транспартиром). Или даже две.

Собираем все вместе

magnetometers

На одну из граней устанавливаем компас, как можно ровнее относительно граней бруска и подключаем провода, желательно достаточно длинные. Я клею сразу все свои компасы-акселерометры на брусок клеем-карандашом, потом легко снять- отщелкивается от малейшего усилия. На контроллер заливаем программу, которая будет каждые X миллисекунд забирать данные с компаса и слать на комп. На компе пишем другую программу, которая это все логирует во что нибудь вроде CSV. Не помешает добавить в программу на компе или контроллеру еще и кнопку/переключатель/перемычку и логировать её состояние вместе с данными с сенсора. Теперь нумеруем стороны бруска от 1 до 6 так, чтобы смежные номера были на смежных сторонах.

Основная идея

1ring

Если мы поставим брусок на стол и повернем его в плоскости стола на 360 градусов, то вектор магнитной индукции очертит круг на мнимой сфере. Что важно, так это то, что мы знаем, что это круг в плоскости стола, вокруг вектора гравитации.

Теперь, если мы перевернем брусок на противоположную сторону и снова повернем- получим еще один круг на сфере вокруг вектора гравитации. Если мысленно провести прямую через центры этих кругов, то она будет параллельна оси (какой- зависит от того, как вы установили компас). Если же внутренние оси сенсора не перпендикулярны или повернуты относительно корпуса микросхемы, то по отклонению этой прямой от оси мы можем определить поправку, которую нужно внести, чтобы круги совместились вдоль оси. Это исправит кривизну и без этого не обойтись, если мы захотим совместить оси акселерометра и компаса. А мы захотим.

Сбор данных

Собираем датасет из векторов каждого 6 кругов, по одному на сторону бруска. Можете собирать их отдельно, можете использовать кнопку, чтобы увеличивать счетчик кругов- главное, чтобы точки данные кругов были отделяемы друг от друга. Лично я использовал кнопку, которую нажимал, пока переворачиваю брусок. Это позволило и отсеять данные «не кругов», и пронумеровать сами круги. Нумерация сторон бруска- это одновременно и план последовательности сбора данных, чтобы не думать во время процесса, и памятка, по которой потом можно выяснить какой круг какому противоположен.

Определяем центры кругов

Есть много вариантов. Например, взять среднее от минимума и максимума по каждой оси для каждого круга. Или взять среднее от всего круга,  однако тут есть нюанс: как бы мы не старались, точки все равно будут неравномерно на круге, просто взять среднее- неверно, оно «уползет» к скоплениям. Лично я объединял точки, располагающиеся близко друг к другу, тем самым прореживая скопления часто стоящих точек, после этого уже брал среднее арифметическое. Так или иначе, нужно получить 6 точек- центров кругов.

Что дальше

Заметно довольно сильное отклонение оси

Заметно довольно сильное отклонение оси

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

  1. т.к. точки — это векторы, исходящие из нуля, мы можем обращаться с ними как с векторами: разность между двух противоположных центров- это вектор, идущий от вычитаемого к уменьшаемому. Определяем три таких вектора- по одному на пару противоположных кругов. Эта тройка показывает реальное взаимное расположение осей внутри микросхемы.
  2. на ось, соответствующую каждому такому вектору, укажет его самый значимый член.
  3. наша цель- матрица трансформации, которая исправит их взаимную неперпендикулярность и поворот относительно референсной системы координат.
  4. разделив векторы на значение их самых значимых членов мы получим колонки матрицы, которая искривляет элипсоид. В отличии от единичной матрицы она добавляет влияние осей друг на друга. Матрица, обратная данной, это и есть матрица трансформации, которая обратит действие искажающей матрицы и мы получим эллипсоид с полюсами, совпадающими с осями компаса.

Масштабирование и сдвиг

Применяем полученную матрицу к кольцам, а дальше все как обычно. Определяем габариты эллипсоида и его центр (по центрам колец). Теперь мы должны к нашей матрице 3×3 добавить справа колонку из нулей, после чего добавить снизу строку-вектор из четырех элементов: первые три- это сдвиг центра эллипса, взятый со знаком минус, а последний — единица. Теперь наша матрица не только исправляет кривизну, но и возвращает центр эллипса в ноль. Один нюанс: теперь наш датасет нужно дополнить четвертой колонкой из единиц, чтобы можно было умножать на эту матрицу 4×4, а после умножения- удалять четвертую колонку, чтобы получить только вектора. Определяем масштабирующие множители для каждой из трех осей, чтобы получить сферу нужного вам диаметра: k = D/(max_val — min_val),  после чего умножаем первые три столбца матрицы на соответствующие им множители. Четвертый столбец можно оставить как есть.

Тада!

rings_calibratedВот и все, мы получили матрицу которая:

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

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

 

calibrated magnetometer sphere

Облако точек после калибровки
(кликабельно, GIF)

А как же акселлерометр?

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

Гироскоп

С гироскопом сложнее, я пока не придумал ничего лучше установки бруска на серву постоянного вращения. Зная скорость, можно сбалансировать поочередно все 3 оси. Но на практике я это не применил пока что.

Ох, вы это осилили! 🙂 Надеюсь, пригодилось.

Ну и, как всегда, в комментариях оставляйте ваши вопросы, идеи и суровую критику 🙂
Спасибо, что читаете!

UPD: Пользователь Юрий написал отличный пост в продолжение этой темы. Рекомендую.

Запись опубликована в рубрике DIY, Алгоритмы, Программирование с метками , , , , , , , . Добавьте в закладки постоянную ссылку.

26 комментариев: Калибровка компаса — Хардкор

  1. Юрий говорит:

    Я тоже столкнулся с проблемой калибровки магнитометра. Хорошая статья! Ничуть не хуже подобных англоязычных.
    Но только теория. Можно примеры кода, скриншотов, цифр….?
    Можно увидеть хотя бы матрицу трансформации которая у Вас получилась? В какой программе обрабатывали данные с магнитометра, MatLab?
    Можно еще пример кода как в arduino умножать вектор на матрицу трансформации?

    • muzhig говорит:

      Юрий, я очень рад, что статья пригодилась кому-то. Постараюсь вам помочь:
      1. Данные я обрабатываю в python-программе (т.к. это мой основной язык). Использую библиотеку для математики- numpy, которая чем-то похожа на матлаб.
      2. Матрицу трансформации показать, конечно, показать-то могу, только вам она не поможет 🙂 компасы все разные, да и от местоположения сильно зависит.
      3. Написал небольшую библиотеку для линейной алгебры на ардуино. Там есть матрицы, векторы и кватернионы. Отдаю на растерзание «как есть».

  2. Юрий говорит:

    И все таки не очень понятно (таким тупым как я) как из координат центров окружностей получить матрицу трансформации. Пожалуйста, приведите пример, как получить эту матрицу…

    • muzhig говорит:

      многое из того, что связано с математикой, либо кажется оооочень сложным, пока не окунешься в теорию, либо очень простым, когда в теорию вникнешь. Это именно тот случай. Попробую объяснить на пальцах, трансформацию можно разбить на три части:
      1. масштабирование- расстояние между центров противоположных кругов должно быть одинаковым и равно диаметру сферы.
      2. афинное преобразование, по-просту «искривление» — это углы между линиями, проходящими через центры окружностей, они должны быть перпендикулярны.
      3. поворот — эти линии должны быть параллельны осям координат.

      Вся прелесть матриц в том, что не нужно каждый этап рассчитывать отдельно. Есть исходная матрица, есть трансформированная — получаем матрицу, которая привела к этой трансформации, инвертируем ее- получаем матрицу, которая отменяет действие «искривляющей» матрицы. 🙂

  3. Юрий говорит:

    А что такое в данном случае «самый значимый член вектора» ?
    «на ось, соответствующую каждому такому вектору, укажет его самый значимый член.»
    «разделив векторы на значение их самых значимых членов мы получим колонки матрицы, которая искривляет элипсоид.»
    Например есть у меня координаты трех векторов полученнх от центров окружностей:
    x1, y1, z1
    x2, y2, z2
    x3, y3, z3

    как мне разделить эти векторы, чтобы получить матрицу которая искривляет элипсоид?)

    • muzhig говорит:

      Самый значимый член- это член с максимальным абсолютным значением.
      пример: вектор [12.15, -13.24, 144.234], самый значимый член 144.234. т.к. этот член по счету третий- значит вектор ближе всего к оси Z. Делим вектор, получаем [0.084, -0.092, 1.0].
      Аналогично поступаем с другими двумя векторами. Потом выстраиваем их по порядку, так, чтобы сначала был вектор ближний к оси X, потом Y, потом Z.
      используем их как колонки матрицы:

      [x1 x2 x3
      y1 y2 y3
      z1 z2 z3]
      главная диагоняль (x1, y2, z3), равна 1.0

      • Юрий говорит:

        Ага, понятно) Правда не дождавшись Вашего ответа я еще понаписал комментариев, с искривлением кажется разобрался =)

  4. Юрий говорит:

    как я понял нужно сделать так:
    1, y1/x1, z1/x1
    x2/y2, 1, z2/y2
    x3/z3, y3/z3, 1
    или я туплю?

    • Юрий говорит:

      Извиняюсь что загрязняю комментарии. Вроде нашел матрицу трансформации (пока только искривления осей эллипсоида).
      Вот например, есть у меня три вектора полученных от шести колец:
      x1=11 y1=4 z1=3
      x2=2 y2=10 z2=1
      x3=3 y3=4 z3=12

      Делим на самый значимый член, получаем:
      1 0.36 0.27
      0.2 1 0.1
      0.25 0.33 1

      Находим обратную данной матрицу, получаем:
      1.13 -0.32 -0.27
      -0.20 1.09 -0.05
      -0.22 -0.28 1.08

      Теперь, например умножаем первый вектор на полученную матрицу:
      (1.13 -0.32 -0.27)
      (11 4 3) х (-0.20 1.09 -0.05) = (10.97 0 0.06)
      (-0.22 -0.28 1.08)

      второй вектор:
      (1.13 -0.32 -0.27)
      (2 10 1) х (-0.20 1.09 -0.05) = (0.04 9.97 0.04)
      (-0.22 -0.28 1.08)

      третий вектор:
      (1.13 -0.32 -0.27)
      (3 4 12) х (-0.20 1.09 -0.05) = (-0.04 0.04 11.95)
      (-0.22 -0.28 1.08)

      Т.е. после умножения на полученную матрицу векторы становятся параллельны осям координат.
      Только у меня сомнение: векторы нужно записывать как векторы-строки или векторы-столбцы?

      • muzhig говорит:

        Юрий, насчет комментариев извиняться на нужно! Наоборот, спасибо вам, что интересуетесь 🙂
        Я не удивлюсь, если вы- первый, кто опробовал этот способ на практике (кроме меня, конечно-же)

        Насчет векторов, вкраце- не важно, как записывать, зависит лишь от способа умножения матрицы и вектора (что слева и что справа). У вас вектор-строка умножается на матрицу- такую матрицу нужно строить из векторов-строк (вы сделали все верно).
        Более подробно читайте в комментарии ниже

    • muzhig говорит:

      Вообще, во моей версии, вашу матрицу нужно транспонировать. Хотя, зависит от того, как вы рассматриваете умножение: если вектор-строку слева на матрицу справа — тогда все верно, матрица составляется из векторов-строк. Если же умножаем матрицу на вектор-столбец (как это делаю я), то матрица строится из столбцов.

      По умножению матриц вот шпаргалка

      Теперь попробую объяснить, почему получается искривляющая матрица из этих столбцов: когда мы умножаем матрицу и вектор, каждому элементу вектора соответствует три числа в соответствующей строке матрицы. Эти три числа- это вклад этого элемента вектора в каждый из трех элементов результирующего вектора. Наши кривые вектора как раз состоят из: 1. единица — вклад оси вектора в главный его разряд. 2. еще два числа- ее вклад в элементы, соответствующие перпендикулярным осям, то есть, искривление.
      При умножении матрицы, составленной из таких векторов-столбцов, на «чистый» вектор-столбец мы получим «искривленный» вектор, каждый элемент которого будет включать как значение соответствующей оси, так и побочные помехи от перпендикулярных.

  5. Юрий говорит:

    А что если не считывать целые кольца, а просто по 4 точки с кольца считать (поворачивая магнитометр на 90 градусов), и от них найти середину? или даже по три точки =) может погрешность и удовлетворительная будет.

    • Юрий говорит:

      а лучше по 2 точки

      • muzhig говорит:

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

        • Юрий говорит:

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

  6. Ренат говорит:

    Уважаемый muzhig, позвольте задать вопрос.
    Я не очень хорошо представляю как магнетометр устроен внутри, поэтому вопрос может прозвучать глупо, но все же.

    А что если калибровать компас не программно, а «железно», фиксируя угол наклона оси Oz в сторону остальных осей? Мы же тем самым проекцию на плоскость xOy приводим к правильной окружности? Или этого мало?

    И еще один вопрос, если магнетометр между, скажем, севером и востоком показывает не 90 градусов, а 60 — это глюк компаса или это исправляется калибровкой(склоняюсь к последнему, но так как знания для меня новые, хотелось бы услышать мнение «бывалого»)

    Спасибо!

    • Ренат говорит:

      Спасибо, вопрос снимается. Зафиксировал Oz, сместил показания в начало координат и сжал, в моем случае, Oy на 1.02. Обошелся без матриц)

    • muzhig говорит:

      Прошу прощения, что поздно отвечаю 🙂
      Дело в том, что я столкнулся с неперпендикулярностью осей сенсора. Это приводит как раз к тому, что поворот на 90 градусов сенсор воспринимает как поворот на меньший или больший угол. Матрица может устранить этот косяк, а вот простым сдвигом и масштабированием не обойтись, исправив ошибку в одной полусфере вы добавите ее в другую. Тут нужно именно т.н. афинное преобразование.

  7. Иван говорит:

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

    • muzhig говорит:

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

      В статье я упоминаю, что круг очерчивает магнитный вектор вокруг вектора гравиации (вектор гравитации смотрит в центр круга), после чего я переворачиваю модель на 180 градусов, от чего оба вектора меняют знак (например, гравитация смотрела вверх отн. сенсора, а теперь вниз, так же и магнитный вектор- между собой они как были так и остались, перевернулся только сенсор), и я снова вращаю сенсор в плоскости, перпендикулярной вектору гравитации, что дает еще один круг, очерчиваемый магнитной индукцией вокруг гравитации. Так как я точно знаю, что повернул ровно на 180 градусов, я могу быть уверен, что вектор гравитации для первого круга будет параллелен вектору гравитации второго круга, именно этот факт и дает мне возможность определить как наклон внутренних осей осносительно корпуса серсора, так и смещение нуля и масштаб каждой оси, на что я и ввожу поправку.

  8. Сергей говорит:

    Предлагаю способ калибровки гироскопа.
    По магнитометру. Помещаем плату с датчиком(3 в 1), аккумулятором и беспроводным интерфейсом, например serial over bluetooth, в корпус фиксируем свободное перемещение компонент в корпусе. Желательно чтобы с корпусом весь был ~1кг.
    Подвешиваем это на нить, раскручиваем. Магнитометр показывает синусоидальные колебания, период этих колебаний по каждой оси магнитометра сравниваем с показаниями гироскопа.
    ПС считая что магнитометр не врёт.

  9. наум говорит:

    магнитометр как раз врет больше всех остальных датчиков))

  10. наум говорит:

    *магнетометр

  11. Антон говорит:

    Здравствуйте, я школьник, не очень шарю в вузовской математике. Поясните, пожалуйста: «Определяем масштабирующие множители для каждой из трех осей, чтобы получить сферу нужного вам диаметра: k = D/(max_val — min_val), после чего умножаем первые три столбца матрицы на соответствующие им множители.»
    Что такое max_val , min_val, D?
    Заранее спасибо)

    • muzhig говорит:

      Добрый день, Антон, спасибо за вопрос. Вообще, статью я давненько писал, но, насколько помню, это я так делал поправку на различия в чувствительности каждой из осей сенсора. Думаю, D — это желаемый диаметр сферы, например он может быть равен 1. (max_val — min_val) это величина разброса в датасете. Ну, например по оси X показатели колеблются от -10 до 10, значит разброс будет 20. А по оси Y они могут колебаться от -30 до 30, и тогда для нее разброс будет 60.
      Соответственно, поделив колонки матрицы на разброс и умножив на желаемый диаметр я получал матрицу, которая приводит кособокие данные сенсора к точкам на сфере (приближенно)

      • Антон говорит:

        Угу, а если я уже привёл показания сенсора к точке( Брал среднее арифметическое от 100 показаний в данной точке. Конечно, не лучший способ, но для первого опыта,думаю, достаточно), то этот шаг я пропускаю?

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

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