Теория‎ > ‎

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

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

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

Гироскоп

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

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

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

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

Расчет угла наклона по значению акселерометраЕсли мы конструируем инклинометр с помощью акселерометра, то для определения угла наклона достаточно применить простые геометрические преобразования к его показаниям. Акселерометр позволяет определять точные углы наклона прибора только в состоянии покоя. Пока на него не действуют внешние силы, на выходе прибора мы получим значение проекции ускорения свободного падения на наблюдаемую ось. На рисунке справа показания акселерометра для одной из его осей отмечены буквой X. Зная G и X, можно вычислить угол отклонения акселерометра от горизонтального положения - a

sin(a) = X/G

a = arcsin(X/G)

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

a = arcsin(X)

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

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

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

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

a = (1-K)*gyr + K*acc

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

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

Ниже представлен лог работы реального мультикоптера с инклинометром на основе комплементарного фильтра.

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

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

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

Вывод 

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