События‎ > ‎

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

Отправлено 4 сент. 2012 г., 14:07 пользователем Олег Евсегнеев   [ обновлено 5 сент. 2012 г., 6:44 ]
Пожалуй одним из основных этапов создания автономного мультикоптера является разработка подсистемы стабилизации. Суть работы данного компонента сводится к определению углов наклона аппарата относительно поверхности земли и передача управляющих импульсов на двигатели машины для компенсации этих углов. 

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

Итак, для экспериментов мной был использован гироскоп Pololu LPR550AL, который позволяет определять скорость вращения вокруг всего двух осей X и Y (крен и тангаж). Прибор имеет два режима работы, различающихся диапазоном измеряемых угловых скоростей: ±500°/сек и ±2000°/сек. В качестве гироскопа применялся трех-осный Pololu MMA7361L с возможными диапазонами измерений ±1.5/6g.
Акселерометр Pololu MMA7361L   
Гироскоп LPR550AL

Оба прибора сообщают свои показания в виде аналогового сигнала и должны быть подключены к соответствующим входам микроконтроллера. В нашем случае, как и прежде, эксперименты проводились на базе платформы NXP LPC1768.

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


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

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

acc_x = (acc_x_raw * vdd - acc_zero) / acc_sens

где acc_x_raw - значение на аналоговом входе в диапазоне от 0.0 до 1.0;
vdd - рабочее напряжение сенсора ( 3.3В );
acc_zero - напряжение на входе в состоянии покоя - когда ось повернута параллельно земле (как правило равно vdd/2 - 1.65В);
acc_sens - чувствительность прибора - сколько вольт соответствует силе в один g (0.8В для диапазона ±1.5g).

Затем, полученное значение силы тяжести следовало преобразовать в угол наклона датчика. Что и было сделано с помощью примитивной тригонометрии по формуле:
  
sin(A) = acc_x/1

Как видно, выражение получилось достаточно бесполезным. По сути, вычисленное ранее значение acc_x является проекцией силы тяжести на наблюдаемую ось. А следовательно, зная величину силы тяжести (1g) и проекцию этой силы на ось, можно узнать косинус угла между этими векторами, а также синус угла наклона оси к земле. Вытаскивать же сам угол из синуса мы не будем, ибо во-первых вычисление арксинуса угла операция достаточно тяжелая, а во-вторых, мы знаем что при малых углах sin(A) = A (в радианах), чего нам вполне достаточно.

Таким образом, переменная acc_x теперь хранит величину угла наклона акселерометра в радианах.

Гироскоп

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

Но для начала, как и в случае акселерометра, необходимо осуществить преобразование сигнала гироскопа к рабочему виду. Здесь, кстати сказать, обнаружилась небольшая проблемка. Дело в том, что в гироскопе LPR550AL напряжение в точке покоя вовсе не VDD/2. Величину этого напряжения можно измерить на специальной ноге прибора (около 1.23В согласно даташиту). Поскольку занимать лишний аналоговый вход на MCU для этих измерений вовсе не хотелось, было решено в алгоритм инициализации квадрокоптера включить процедуру определения точки покоя опытным путем, с помощью усреднения показаний датчика в покое за небольшой промежуток времени. Ясно, что с таким смещением точки покоя, без определенных схемотехнических решений диапазон гироскопа будет урезан, о чем собственно и говорится в даташите. В итоге имеем формулу приведения:

gyr_x = (gyr_x_raw * vdd - gyr_zero) / gyr_sens

где gyr_x_raw - значение на аналоговом входе в диапазоне от 0.0 до 1.0;
vdd - рабочее напряжение сенсора ( 3.3В );
gyr_zero - напряжение на входе в состоянии покоя - когда прибор не движется (1.23В в нашем случае);
gyr_sens - чувствительность прибора - сколько вольт соответствует вращению со скоростью 1градус в секунду (0.0005В для диапазона ±2000°/сек).

Поскольку мы не стали раскрывать синус при вычислении угла с акселерометра, он (угол) получился в радианах. Я не стал преобразовывать его в градусы, и вместо этого преобразовал показания гироскопа из гр/сек в рад/сек:

gyr_x = gyr_x * PI / 180

В итоге, мы имеем в переменной gyr_x значение скорости вращения гироскопа вокруг оси X, измеряемое в радианах в секунду.

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

Работа фильтра сводится к интегрированию показаний гироскопа, но с небольшой примесью сигнала акселерометра. Фильтр работает в общем цикле процедуры интеграции угла наклона и выглядит следующим образом:

while(1){
  acc_x = (acc_x_raw * vdd - acc_zero) / acc_sens;
  
  gyr_x = (gyr_x_raw * vdd - gyr_zero) / gyr_sens;
  gyr_x = gyr_x * PI / 180;
  
  imu_x = imu_x + gyr_x * tdelta;
  imu_x = 0.9 * imu_x + 0.1 * acc_x;

  wait(tdelta);
}

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

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

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

И небольшое видео работы фильтра в динамике:



Comments