Комплементарный фильтр

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

Для того чтобы осуществить эту стабилизацию, машина должна:

  • определить углы наклона машины относительно поверхности земли;
  • вычислить отклонение от требуемого положения;
  • подать управляющие сигналы для актуаторы для компенсации отклонения и приведения машины в требуемое положение.

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

Гироскоп

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

a(t) = a(t-1) + gx*dt    [1]

где
a(t) — искомый угол наклона тела;
a(t-1) — угол тела в предыдущий момент времени;
gx — скорость вращения тела вокруг оси X — это то, что измеряет гироскоп;
dt — время, которое прошло с момента предыдущего вычисления угла a.

Однако, у MEMS гироскопа есть один коварный недостаток, который называется дрейфом нуля. Суть этого недостатка сводится к тому, что при остановке вращения гироскопа, он все еще будет показывать значение отличное от нуля.

Другим недостатком такого решения, является применение процедуры дискретного интегрирования, которая по своей природе дает неточный результат.

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

Какой же вывод можно сделать из сказанного выше? А такой, что если строить инклинометр только на основе гироскопа, он будет довольно неточен. Значит и стабилизация машины будет весьма посредственная. Убедиться в этом можно, посмотрев на график в конце 3-го раздела.

Акселерометр

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

a = 90 - arccos(Gyт)    [2]

Здесь Gyт — это проекция ускорения свободного падения на ось акселерометра Xт. Именно это значение возвращает нам датчик. Важно помнить, что величина Gyт должна измеряться в единицах земной гравитации. К примеру, если датчик повернуть осью Y вертикально, то Gyт будет равен единице (G = 1 земная гравитация).

В чем минус акселерометра? К сожалению, любое воздействие внешней силы на датчик будет вносить ошибку в вычисление угла наклона. Такой внешней силой может быть вибрация летательного аппарата, ветер или тяга двигателей. Частично снять воздействие внешних сил можно с помощью фильтра низких частот, но побочным эффектом подобной обработки сигнала будет сильное уменьшение быстродействия инклинометра.

3. Комплементарный фильтр

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

Возникает естественное желание объединить показания этих двух устройств для устранения их недостатков. Сделать такое объединение позволяет комплементарный фильтр, который немного меняет формулу для интегрирования показаний гироскопа:

a(t) = (1-K) * (a(t-1) + gx*dt) + K * acc    [3]

Здесь
a(t) — искомый угол наклона, учитывающий показания акселерометра;
a(t-1) — угол тела в предыдущий момент времени;
gx — скорость вращения тела вокруг оси X;
dt — время, которое прошло с момента предыдущего вычисления угла a;
acc — значение угла наклона, полученное при помощи акселерометра;
K — коэффициент комплементарного фильтра.

Как видно из формулы, итоговая величина угла наклона представляет собой сумму интегрированного значения гироскопа и мгновенного значения акселерометра. По сути, главная задача комплементарного фильтра здесь в том, чтобы с помощью показаний акселерометра нивелировать дрейф нуля гироскопа и ошибки дискретного интегрирования. Указанное выражение именно это и делает. На каждом шаге интегрирования (по сути шаге цикла управления машиной) мы корректируем интеграл угла наклона с помощью показаний акселерометра. Сила этой коррекции определяется коэффициентом фильтра K.

Выбор коэффициента K зависит от величины дрейфа нуля гироскопа, от скорости накопления ошибок вычисления и от условий использования машины. Так, слишком большое значение K приведет к тому, что на результат работы фильтра будет сильно влиять вибрация корпуса беспилотника. Слишком малое значение K может оказаться недостаточным, чтобы ликвидировать дрейф нуля гироскопа. Как правило, коэффициент комплементарного фильтра подбирается вручную для каждого инклинометра исходя из вышеуказанных условий. Например, для любительских мультикоптеров K может принимать значение в диапазоне от 0,05 до 0,01.

Результат работы комплементарного фильтра хорошо видно на следующем графике.

Работа комплементарного фильтра

Синий график — это угол, вычисленный по показаниям гироскопа. Хорошо видно, что этот угол постепенно увеличивается (он может и уменьшаться) — дрейфует.

Красный график — это угол по акселерометру. Графики получены при работе устройства в руке человека, представляете какой будет шум от вибрации двигателей мультикоптера.

Наконец, зеленый график — это угол, вычисленный при помощи комплементарного фильтра. Видно, что угол совпадает с реальным углом наклона и практически не имеет шума.

Программа

Напишем простую программу для Ардуино и датчика MPU6050, которая будет вычислять угол наклона устройства относительно земли.

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

#define TO_DEG 57.29577951308232087679815481410517033f
#define T_OUT 20 // каждый 20 миллисекунд будем проводить вычисления 
#define P_OUT 50 // каждый 50 миллисекунд будем выводить данные
#define FK 0.1 // коэффициент комплементарного фильтра

MPU6050 accelgyro;

float angle_ax, angle_gx, angle_cpl;
int dt = 0;
long int t_next, p_next;

// функция, которая не даёт значению выйти за пределы 
float clamp(float v, float minv, float maxv){
    if( v>maxv )
        return maxv;
    else if( v<minv )
        return minv;
    return v;
}

void setup() {
    Serial.begin(9600);
    // инициализация MPU6050
    accelgyro.initialize();
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
}

void loop() {
    long int t = millis();
    // каждые T_TO миллисекунд выполняем рассчет угла наклона
    if( t_next < t ){
        int16_t ax_raw, ay_raw, az_raw, gx_raw, gy_raw, gz_raw;
        float ay,gx;

        t_next = t + T_OUT;
        // получаем сырые данные от датчиков в MPU6050
        accelgyro.getMotion6(&ax_raw, &ay_raw, &az_raw, &gx_raw, &gy_raw, &gz_raw);

        // преобразуем сырые данные гироскопа в град/сек
        gx = gx_raw / 16.4;
        // преобразуем сырые данные акселерометра в G
        ay = ay_raw / 4096.0;
        ay = clamp(ay, -1.0, 1.0);

        // вычисляем угол наклона по акселерометру
        angle_ax = 90 - TO_DEG*acos(ay);
        // вычисляем угол наклона по гироскопу
        angle_gx = angle_gx + gx * T_OUT/1000.0;
        // корректируем значение угла с помощью акселерометра
        angle_gx = angle_gx*(1-FK) + angle_ax*FK;
    }

    t = millis();
    // каждые P_OUT миллисекунд выводим результат в COM порт
    if( p_next < t ){
        p_next = t + P_OUT;
        Serial.println(angle_gx);
    }
}

Загружаем программу на Ардуино и открываем монитор COM-порта либо плоттер. По сути, данная программа уже годится для оценки угла наклона балансирующего робота.

Работа комплементарного фильтра на Ардуино

Эксперимент

Ниже представлен график показаний датчиков, снятый с реального мультикоптера.

Работа комплементарного фильтра на квадрокоптере

Здесь синяя кривая отражает показания акселерометра. Красная отвечает за работу комплементарного фильтра, а зеленая за тягу двигателей. Видно, что до запуска контроллера аппарат был наклонен вокруг наблюдаемой оси примерно на 24 градуса. На начальном этапе фильтр постепенно корректировал интеграл угла с помощью показаний акселерометра, выведя его на рабочие значения.

Подъем зеленой кривой и затем небольшой отскок вниз означает запуск двигателей и вывод их мощности на некоторое стационарное значение. Точка, где зеленая кривая начинает колебаться, означает начало работы ПИД регулятора, который старается компенсировать отклонение машины от горизонтальной плоскости.

Вывод

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

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

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


Изменено:

Комплементарный фильтр: 14 комментариев

  1. Это чаще называют «альфа-бета фильтр» (b = 1-a). А комплементарный фильтр зачастую связан с Калманом.

  2. А в спецификации на MPU6050 написано, что напряжение 2.3-3.4V. При подключении к пину 5V микросхема не сгорит?

    • Если микросхема не голая, а на модуле, на котором еще стоит стабилизатор питания — то не сгорит. Например, на модуле MPU6050 ROC установлен стабилизатор для понижения напряжения питания до 3.3В.

  3. gx = gx_raw / 16.4;
    ay = ay_raw / 4096.0;
    откуда взялись эти знаменатели? каким образом они преобразуют сырые данные в «человеческие»?

    • Эти знаменатели приводятся в даташите к датчикам. Например, для гироскопа в режиме +-2000 градусов/сек (FS_SEL=3), raw значение нужно поделить на 16,4 чтобы получить градусы в сек. Надо только точно знать, в каком режиме находится датчик. В свежей версии библиотеки для MPU6050 по умолчанию FS_SEL = 0, а значит делить надо не на 16,4, а на 131.

      Фрагмент спецификации MPU6050

      • Олег, не подскажете пожалуйста, как понять, какой режим сейчас включен в библиотеке, или как его настроить? В MPU6050.h

        • В папке с библиотекой есть файл файле MPU6060.cpp, в нём, в свою очередь, есть реализация функции initialize()
          void MPU6050::initialize() {
          setClockSource(MPU6050_CLOCK_PLL_XGYRO);
          setFullScaleGyroRange(MPU6050_GYRO_FS_250);
          setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
          setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
          }

          Вот тут константа MPU6050_GYRO_FS_250 означает как раз режим +-250, что соответствует FS_SEL=0

  4. Олег, подскажите,пожалуйста, возможно ли определить с помощью гироскопа и акселерометра значение угловой скорости в плоскости OXY

    • Да возможно, но для учета дрейфа гороскопа необходим магнитометр, который дополнит акселерометр, но уже по оси вращение по Z

  5. Подскажите, пожалуйста, как изменить программу для отображения угла наклона по оси Y?

  6. А разве не нужно находит длину вектора:
    float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
    float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
    Почему на графике гироскоп не показывает период колебания самого аппарат используется уже другой фильтр или недостача частота дискретизации?
    В приведённой программе показания от вибрации самоинкрементируются современем, без вибрации восстанавливаются, как фильтр работает в полёте при вибрации и ускорениях мультикоптера?

  7. Почему для корректировки оси X гироскопа, Вы используете ось Y акселерометра?

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

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

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