Программа

Фильтр Мажвика для вычисления углов наклона

Олег Евсегнеев

Для работы нам понадобятся две библиотеки:

  • библиотека MPU6050 для получения сырых данных с датчика
  • и библиотека Madgwick, реализующая алгоритм фильтра

Как получить данные с MPU6050 мы уже рассказывали в отдельных уроках про гироскоп и акселерометр. Разберемся теперь с фильтром.

Библиотека Madgwick имеет три полезные функции:

  • MadgwickAHRSupdateIMU — работает только с акселерометром и гироскопом
  • MadgwickAHRSupdate — работает с акселерометром, гироскопом и магнитометром
  • quat2Euler — преобразует кватернион положения в углы Эйлера

Кватернион — это такое сложное число (гиперкомплексное), которое в нашем случае служит для описания положения тела в пространстве. Фильтр Мажвика работает именно с кватернионом положения, а не с углами Эйлера, как мы привыкли со школы. В нашем уроке для наглядности результата, мы преобразуем кватернион положения в углы Эйлера и выведем их в последовательный порт. Однако следует заметить, что в серьёзных подсистемах автопилота углы Эйлера не используются, по ряду причин.

Итак, напишем программу, которая будет получать от модуля сырые данные датчиков, обрабатывать их фильтром и выводить график углов наклона.

 
#include "I2Cdev.h"
#include "MPU6050.h"
#include "MadgwickAHRS.h"

#define TO_RAD 0.01745329252f // этот коэффициент нужен нам для перевода градусов в радианы

MPU6050 accelgyro;

unsigned long tm, imu_t, prn_t;
const unsigned int imu_to = 20; // период обработки показаний датчиков
const unsigned int prn_to = 100; // период вывода информации в COM порт

float tdelta;
int16_t ax, ay, az;
int16_t gx_raw, gy_raw, gz_raw;

float imu[3];
float quat[4];
float e[3];

void setup() {
    Serial.begin(9600);
    Serial.println("Initializing I2C devices...");
    accelgyro.initialize(); // инициализация датчиков
    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
}

void loop() {
    // каждые 20мс вычисляем углы наклона
    tm = millis();
    if (imu_t + imu_to < tm) {
        tdelta = tm - imu_t; // вычисляем дельту времени в миллисекундах
        imu_t = tm;

        // запрашиваем данные у датчика MPU6050
        accelgyro.getMotion6(&ax, &ay, &az, &gx_raw, &gy_raw, &gz_raw);

        // преобразуем сырые данные гироскопа в рад/с
        float gx = gx_raw * TO_RAD / 131.0;
        float gy = gy_raw * TO_RAD / 131.0;
        float gz = gz_raw * TO_RAD / 131.0;
 
        // вызываем алгоритм фильтра и передаем туда:
        // - дельту времени в секундах
        // - данные гироскопа в рад/с
        // - сырые данные акселерометра
        MadgwickAHRSupdateIMU(tdelta/1000.0, gx, gy, gz, (float)ax, (float)ay, (float)az);
        quat[0] = q0; quat[1] = q1; quat[2] = q2; quat[3] = q3;
        // преобразуем кватернион в углы Эйлера 
        quat2Euler(&quat[0], &imu[0]);
    }

    // каждые 100мс добавляем новую точку графика
    tm = millis();
    if (prn_t + prn_to < tm) {
        prn_t = tm;
        Serial.print(imu[0]/TO_RAD); Serial.print("\t");
        Serial.print(imu[1]/TO_RAD); Serial.print("\t");
        Serial.println(imu[2]/TO_RAD);
    }
}

Небольшое пояснение по константе TO_RAD. Из курса школьной математики мы знаем, что для перевода градусов в радианы, необходимо число градусов умножить на ПИ(3.1415…) и разделить на 180. В качестве оптимизации, мы можем один раз разделить ПИ на 180 и затем всегда использовать полученный коэффициент — 0.01745329252 для перевода градусов в радианы. Таким образом, везде где мы умножаем на TO_RAD — мы переводим градусы в радианы, а там где мы делим на TO_RAD — наоборот, переводим радианы в градусы.

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

Работа фильтра Мажвика
Углы наклона MPU6050, вычисленные с помощью Фильтра Мажвика

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

Именно gimbal мы попробуем сделать в следующий раз!


Изменено: