Измерение угла наклона по акселерометру и гироскопу MPU6050

Введение

MPU6050 — самое доступное практичное решение задачи «в какую сторону вверх». На одном кристалле находятся два независимых датчика — трёхосный акселерометр и трёхосный гироскоп, — и роботу с самобалансировкой нужны оба: каждый из них по отдельности бесполезен именно в тех случаях, когда второй полностью компенсирует его ограничения.

Это руководство поможет пройти путь от неподключённого модуля до надёжного угла наклона в градусах, пока робот стоит неподвижно. Чтобы сохранить корректность угла при движении робота, нужен комплементарный фильтр, которому посвящено следующее руководство и который опирается на все полученные здесь знания.

Что фактически измеряет каждый датчик

Акселерометр измеряет не наклон. Он измеряет ускорение, включая ускорение свободного падения, поэтому он вообще может работать как датчик наклона: в неподвижном состоянии единственное ускорение составляет 1 g и направлено вниз, а распределение этого вектора по осям позволяет определить ориентацию.

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

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

Их слабость в том, что интегрирование накапливает ошибку. Любое небольшое постоянное смещение показания скорости интегрируется в угол, который растёт без ограничений. Гироскоп, показывающий 0.5 °/s при полной неподвижности, через минуту заявит, что робот повернулся на 30°. Это дрейф, и его нельзя устранить более качественной пайкой.

Итак: акселерометр прав в долгосрочной перспективе, но неверен в любой отдельный момент; гироскоп прав в любой отдельный момент, но неверен в долгосрочной перспективе. Эта симметрия и служит причиной существования слияния данных датчиков.

Подключение

MPU6050 использует I²C, поэтому ему нужны четыре провода; если сканер не находит его по адресу 0x68, на указанной странице объясняется, почему в техническом описании указан адрес 0xD0.

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

Чтение исходных значений

Нужные регистры расположены последовательно, поэтому все данные можно получить одним пакетным чтением:

#include <Wire.h>

const uint8_t MPU = 0x68;        // 0x69 if AD0 is tied high

void setup() {
  Serial.begin(115200);
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);              // PWR_MGMT_1
  Wire.write(0);                 // clear sleep bit — it boots asleep
  Wire.endTransmission(true);
}

void readRaw(int16_t& ax, int16_t& ay, int16_t& az,
             int16_t& gx, int16_t& gy, int16_t& gz) {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);              // ACCEL_XOUT_H
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, (uint8_t)14, (uint8_t)true);

  ax = Wire.read() << 8 | Wire.read();
  ay = Wire.read() << 8 | Wire.read();
  az = Wire.read() << 8 | Wire.read();
  Wire.read(); Wire.read();      // temperature, discarded
  gx = Wire.read() << 8 | Wire.read();
  gy = Wire.read() << 8 | Wire.read();
  gz = Wire.read() << 8 | Wire.read();
}

После включения модуль находится в спящем режиме. Если не сбросить бит sleep в PWR_MGMT_1, он будет выдавать постоянный поток нулей, что выглядит точно как неисправность подключения и заставляет людей без причины возвращаться к паяльнику.

Преобразование отсчётов в единицы измерения

Исходные значения — знаковые 16-битные отсчёты в диапазоне, заданном конфигурацией датчика. При настройках по умолчанию:

• Акселерометр: ±2 g для ±32768 отсчётов → 16384 отсчёта на g
• Гироскоп: ±250 °/s для ±32768 отсчётов → 131 отсчёт на °/s

float accelX = ax / 16384.0;     // g
float rateY  = gy / 131.0;       // degrees per second

Определение наклона по акселерометру

Когда робот стоит вертикально, а плата установлена вертикально, угол наклона в плоскости падения робота определяется по двум из трёх осей:

float pitch = atan2(accelX, accelZ) * 180.0 / PI;

Используйте atan2, а не asin одной оси. atan2 учитывает обе составляющие, поэтому остаётся корректным во всех четырёх квадрантах и не обращается в бесконечность при приближении одной оси к нулю. Вариант с asin для одной оси выглядит исправным на стенде при малых углах, но начинает вести себя странно, как только робот наклоняется больше 45°.

Калибровка смещений

У каждого MPU6050 есть производственное смещение. Гироскоп в неподвижном состоянии не показывает ровно ноль, а акселерометр — ровно 1 g по одной оси и ноль по остальным. Однократно измерьте ошибку при запуске, удерживая робота неподвижно:

float gyroBiasY = 0;

void calibrate() {
  const int samples = 1000;
  long total = 0;
  for (int i = 0; i < samples; i++) {
    int16_t ax, ay, az, gx, gy, gz;
    readRaw(ax, ay, az, gx, gy, gz);
    total += gy;
    delay(2);
  }
  gyroBiasY = (total / (float)samples) / 131.0;
}

Вычитайте gyroBiasY из каждого последующего показания скорости. Этот единственный шаг отличает гироскоп, который дрейфует на градус в минуту, от гироскопа, который дрейфует на градус в секунду.

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

Смещение из-за установки не равно смещению датчика

Есть второе смещение, которое калибровка не может устранить: IMU установлен не идеально. Если плата отклонена от истинной вертикали на 1.5°, идеально откалиброванный датчик всё равно покажет 1.5°, когда робот действительно стоит вертикально.

Для балансирующего робота это имеет огромное значение, а на стенде незаметно. Контур регулирования будет добросовестно удерживать угол, который вы задали как вертикальный, поэтому ошибка 1.5° означает постоянный наклон робота на 1.5°, который означает постоянное ускорение и движение через всю комнату до столкновения.

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

Проверьте результат перед дальнейшей разработкой

Выведите угол и проверьте три условия вручную:

1. В вертикальном положении значение близко к нулю. Иначе необходимо вычесть смещение из-за установки.
2. При наклоне робота вперёд угол увеличивается, а назад — уменьшается. Если знак инвертирован, поменяйте его здесь, а не компенсируйте в контуре регулирования, где это впоследствии запутает диагностику.
3. Показание стабильно в неподвижном состоянии и мгновенно меняется при движении. Шум в неподвижном состоянии означает, что нужно проверить подключение и питание; запаздывание означает, что где-то выполняется слишком агрессивное усреднение.

Когда угол можно считать достоверным в состоянии покоя, можно обеспечить его достоверность при движении с помощью комплементарного фильтра, а затем замкнуть контур в каскадной системе управления равновесием с ПИД-регулятором (PID).