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

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

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

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

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

1. Гироскоп

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

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

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

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

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

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

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

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

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

Угол наклона с помощью акселерометра

На данной схеме изображена система координат тела Xb/Yb/Zb, повернутая на угол а по часовой стрелке вокруг оси X (смотрит на нас) относительно системы координат мира X/Y/Z. Предполагается, что ось Z направлена вдоль вектора силы гравитации, а оси X и Y вдоль горизонта.

Зная G и Gxb можно вычислить угол b отклонения акселерометра от вектора гравитации Z. Из школы мы помним, что в прямоугольном треугольнике катет равен гипотенузе, умноженной на косинус прилежащего угла. Таким образом:

Gxb = G*cos(b)

Выразим из этого уравнения угол b:

cos(b) = Gxb/G

b = arccos(Gxb/G)

Делая такие вычисления, важно учитывать, что G и Gxb должны измеряться в одинаковых единицах. Например, если мы преобразуем показания акселерометра к единицам гравитации (другими словами G = 1 — земная гравитация), то выражение для угла a примет вид:

a = arccos(Gxb/1) = arccos(Gxb)

И напоследок, вычислим искомый угол a наклона тела относительно горизонта:

a = 90 — b

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

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

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

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

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

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

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

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

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

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

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

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

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

4. Программа

Напишем простую программу для Ардуино и датчика 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-порта либо плоттер. По сути, данная программа уже годится для оценки угла наклона балансирующего робота.

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

5. Эксперимент

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

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

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

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

Вывод

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

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

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


Изменено: