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

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

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

  • определить углы наклона машины относительно поверхности земли;

  • вычислить отклонение от требуемого положения;

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

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

Гироскоп

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

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

где

  • a(t) — искомый угол наклона тела;

  • a(t-1) — угол тела в предыдущий момент времени;

  • gx — скорость вращения тела вокруг оси X — это то, что измеряет гироскоп;

  • dt — время, которое прошло с момента предыдущего вычисления угла a.

Предупреждение

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

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

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

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

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

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

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

Здесь Gyт — это проекция ускорения свободного падения на ось акселерометра Xт. Именно это значение возвращает нам датчик.

Важно

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

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

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

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

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

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.

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

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

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

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

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

Программа

Напишем простую программу для Arduino и датчика 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);
    }
}

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

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

Эксперимент

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

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

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

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

Вывод

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

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

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