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

Фильтр Мажвика

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

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

Есть два распространённых алгоритма, которые работают лучше альфа-бета — это фильтр Калмана и фильтр Мажвика. Оба являются достаточно сложными, чтобы разбираться с ними в рамках данного урока. Однако, мы можем использовать готовую программную реализацию фильтра в своих целях.

Фильтр Себастиана Мажвика

Данный алгоритм смешивания данных акселерометра, гироскопа и магнитометра был предложен Себастианом Мажвиком (Sebastian Madgwick) в 2009 году, в рамках научного исследования на соискание Ph.D. Статью с описанием математической модели фильтра, как и реализацию алгоритма на разных языках программирования можно найти на сайте проекта (ссылка в конце урока).

Примечание

Точность фильтра Мажвика достигает точности фильтра Калмана, но в отличие от последнего, требует меньше вычислительных ресурсов. Это очень полезное свойство, которое делает алгоритм пригодным для работы на слабых микроконтроллерах.

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

Подключение

Соединим контакты датчика с Ардуино Уно согласно стандартной схеме для интерфейса I2C:

	GND	VCC	SDA	SCL
MPU6050 ROC	GND	VCC	SDA	SCL
Ардуино Уно	GND	+5V	A4	A5

Внешний вид макета

Программа

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

• библиотека 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 мы попробуем сделать в следующий раз!