Магнитометры (Компас) — Абсолютный курс в магнитном поле Земли

Магнитометр — это цифровой компас. Он измеряет напряженность магнитного поля Земли по трем осям \((B_x, B_y, B_z)\). В робототехнике он используется для определения курса (Heading/Yaw) — угла поворота робота относительно магнитного севера. Это единственный доступный на борту сенсор, который дает абсолютную ориентацию по одной оси.

Показания сырого магнитометра — это вектор в системе координат датчика, на который влияют:

1. Наклон робота (магнитное поле Земли имеет вертикальную составляющую).
2. Постоянные помехи от магнитов и железа на самом роботе (Hard Iron).
3. Деформации поля от проводящих материалов (Soft Iron).
4. Магнитное склонение (разница между магнитным и истинным севером).

Магнитное поле Земли можно представить как вектор, направленный вниз под углом к поверхности (в Северном полушарии). Его можно разложить: \[ \vec{B}_{earth} = \vec{B}_{horiz} + \vec{B}_{vert} \]

• Горизонтальная составляющая \(B_{horiz}\) указывает на магнитный север.
• Вертикальная составляющая \(B_{vert}\) зависит от широты.

В идеальном случае, если датчик расположен идеально горизонтально, курс вычисляется через арктангенс: \[ \psi_{raw} = \arctan2(-B_y, B_x) \] где \(B_x\) и \(B_y\) — показания магнитометра в горизонтальной плоскости.

Проблема: Робот редко стоит идеально ровно. Наклон вносит ошибку.

Если известны углы наклона \(\theta\) (pitch) и \(\phi\) (roll) из акселерометра или IMU, можно пересчитать магнитные показания в горизонтальную плоскость.

Пусть \(R_x(\phi)\) и \(R_y(\theta)\) — матрицы поворота для компенсации крена и тангажа. Тогда скорректированные горизонтальные компоненты: \[ \begin{bmatrix} B_{x,h} \\ B_{y,h} \\ B_{z,h} \end{bmatrix} = R_y(-\theta) \cdot R_x(-\phi) \cdot \begin{bmatrix} B_x \\ B_y \\ B_z \end{bmatrix} \]

После этого истинный курс вычисляется как: \[ \psi_{comp} = \arctan2(-B_{y,h}, B_{x,h}) \]

Магнитный север не совпадает с географическим (истинным) севером. Угол между ними называется магнитным склонением (δ). Оно зависит от местоположения и медленно меняется со временем.

Чтобы получить курс относительно истинного севера: \[ \psi_{true} = \psi_{magnetic} + \delta \]

Пример: Для Москвы склонение \( \delta \approx +12^\circ\). Это значит, что стрелка компаса показывает на \(12^\circ\) восточнее истинного севера. Значение нужно прибавить.

Калибровка магнитометра — самый важный этап. Она решает две основные проблемы:

Причина: Постоянные магниты (моторы, динамики) и намагниченные стальные детали создают постоянное смещающее поле \(\vec{O}\).

Эффект: Центр “магнитной сферы” сдвигается от начала координат \((0,0,0)\).

Математика: Каждое измерение искажено: \[ \vec{B}_{measured} = \vec{B}_{earth} + \vec{O} \]

Калибровка: Нужно повращать датчик во всех возможных ориентациях и найти минимальные и максимальные значения по каждой оси. Смещение вычисляется как: \[ O_x = \frac{\max(B_x) + \min(B_x)}{2}, \quad O_y = \frac{\max(B_y) + \min(B_y)}{2}, \quad O_z = \frac{\max(B_z) + \min(B_z)}{2} \]

Причина: Металлические конструкции (алюминий, сталь) искажают форму магнитного поля. Идеальная сфера превращается в эллипсоид.

Эффект: Разная чувствительность по осям и перекрестные помехи.

Математика: Преобразование описывается матрицей 3x3: \[ \vec{B}_{corrected} = \mathbf{S} \cdot (\vec{B}_{measured} - \vec{O}) \] где \(\mathbf{S}\) — матрица масштабирования и перекрестной компенсации (Soft Iron Matrix).

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

Arduino → QMC5883L
  5V    → VCC
  GND   → GND
  A4    → SDA
  A5    → SCL

Не забудьте подтягивающие резисторы ~4.7 кОм на SDA/SCL к 3.3V.

#include <Wire.h>
#include <QMC5883LCompass.h> // Используйте библиотеку QMC5883L

QMC5883LCompass compass;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  compass.init();
  // Здесь должен быть вызов процедуры калибровки!
}

void loop() {
  // Прочитать значения
  compass.read();

  // Получить сырые значения
  int x = compass.getX();
  int y = compass.getY();
  int z = compass.getZ();

  // Вычислить азимут (курс) от 0 до 359 градусов
  int azimuth = compass.getAzimuth();

  // Вывести
  Serial.print("X: "); Serial.print(x);
  Serial.print(" Y: "); Serial.print(y);
  Serial.print(" Z: "); Serial.print(z);
  Serial.print(" Azimuth: "); Serial.println(azimuth);

  delay(100);
}

1. Размещение — это всё. Устанавливайте магнитометр:

   • На мачте/стойке как можно дальше от источников помех.
   • На расстоянии. Магнитное поле от провода с током убывает как \( \frac{1}{r} \), от постоянного магнита как \( \frac{1}{r^3} \). Каждые лишние 5 см — на вес золота.
   • Вдали от силовых трасс и DC-DC преобразователей.

2. Калибруйте на месте. Калибровка, проведенная на столе, не будет работать, когда вы прикрутите датчик к роботу с моторами. Калибруйте готового собранного робота.

3. Используйте компенсацию наклона. Серьезный проект без акселерометра для компенсации наклона не имеет смысла. Используйте 9-DOF IMU (акселерометр+гироскоп+магнитометр).

4. Обновляйте магнитное склонение. Используйте актуальные данные для ваших координат (можно получить из онлайн-калькуляторов или библиотек).

5. Проверяйте “здоровье” поля. Перед началом движения робот может проверять, что величина полного вектора магнитного поля \( \sqrt{B_x^2 + B_y^2 + B_z^2} \) близка к ожидаемой для данной местности (~25-65 µT). Сильное отклонение говорит о близкой помехе.

Цель: Увидеть, как металл искажает показания.

Что нужно: Любой магнитометр (например, QMC5883L), Arduino, компьютер.

Шаги:

1. Загрузите простой скетч, который выводит сырые \(B_x, B_y, B_z\).
2. Откройте Serial Plotter. Медленно вращайте датчик в воздухе, описывая сферу. На графике вы увидите синусоиды.
3. Фаза 1 (без помех): Оцените центры этих синусоид (приблизительно должны быть около 0).
4. Фаза 2 (с помехой): Поднесите к датчику небольшой магнит или отвёртку. Снова повращайте. Вы увидите, как вся синусоида сместилась по вертикали. Это и есть Hard Iron смещение \(\vec{O}\).

Вывод: Без вычитания этого смещения вычисленный курс будет всегда сдвинут на фиксированный угол. Калибровка — это процесс измерения и последующего вычитания этого смещения.

Магнитометр — ключ к абсолютной ориентации, но он хрупок и капризен. Освоив его, вы готовы к построению полной системы ориентации:

1. Sensor Fusion (Фильтры) — как объединить данные магнитометра, гироскопа и акселерометра в стабильный и точный курс с помощью фильтра Маджвика или Калмана.
2. Полная навигационная система — как интегрировать компас с GPS и одометрией для надежного позиционирования.
3. Калибровка сложных систем — углубленные методы калибровки и анализ погрешностей.

Финальная рекомендация: Для первого серьезного проекта используйте готовый 9-DOF модуль (например, на базе BNO055 или ICM-20948), который берет на себя всю сложную математику слияния данных и компенсации. Это сэкономит массу времени. Создавайте же свой алгоритм fusion, только когда поймете его необходимость и будете готовы к глубокому погружению.