Полезные ссылки

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

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

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

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

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

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

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

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

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

MPU6050 ROCGNDVCCSDASCL
Ардуино УноGND+5VA4A5

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

Подключение MPU6050 к Ардуино

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

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

Сайт с материалами по фильтру Мажвика:

http://x-io.co.uk/open-source-imu-and-ahrs-algorithms

Библиотека для Ардуино:

https://github.com/robotclass/RobotClass-MadgwickAHRS

Изображение на титуле страницы — торпедный гироскоп конца 19 века. Источник: http://www.muzej-rijeka.hr

0

Изменено:

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

  1. Добрый день, спасибо Вам за статью.
    Но я вот не понял о сырых данных акселерометра. Разве не нужно учитывать настройки акселерометра (2G, 4G…) и делить на соответствующий коэффициент (4096 и т.п.).
    И ещё вопрос, у меня почему-то ось Х и Y гироскопа и акселерометра не совпадают, я был уверен что так и есть, но Вы негде их не меняете местами для функции Маджвика…
    Спасибо.

    0
    • Для данных акселерометра нет нужды делать нормировку, она уже есть внутри библиотеки. Насчет несовпадения осей, тут главное не запутаться. К примеру если мы вращаем датчик вокруг оси X, то акселерометр естественно не будет ничего показывать по этой оси. Ведь он измеряет наклон этой оси, а она неподвижна. Это еще не значит, что оси не совпадают.

      1+
      • Спасибо большое, а для каких настроек (±2g, ±4g, ±8g, ±16g) нормировка в библиотеке, и как ее изменить?

        0

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

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

Этот сайт использует Akismet для борьбы со спамом. Узнайте как обрабатываются ваши данные комментариев.