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

Введение

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

Это одна строка арифметики. Для самобалансирующегося робота она также почти всегда является правильным ответом: фильтр Кацмана даёт лишь небольшое улучшение точности, но требует огромной сложности и времени ЦП (CPU) на 8-разрядном микроконтроллере.

Идея

Каждый датчик надёжен в своей полосе частот.

• Акселерометр надёжен на низких частотах. В среднем за секунду он правильно сообщает, куда направлена сила тяжести; при опросе за 10 ms он в основном сообщает собственное движение робота.
• Гироскоп надёжен на высоких частотах. За 10 ms его интеграл скорости практически точен; за минуту накопленное смещение всё портит.

Значит, нужно пропустить акселерометр через фильтр нижних частот, гироскоп — через фильтр верхних частот, а затем сложить результаты. Фильтры построены так, что их отклики в сумме дают ровно единицу на каждой частоте: они дополняют друг друга, откуда и название. Ни одна полоса не учитывается дважды и не пропускается.

Реализация

const float ALPHA = 0.98;
float angle = 0;

void updateAngle(float accelAngle, float gyroRate, float dt) {
  angle = ALPHA * (angle + gyroRate * dt) + (1.0 - ALPHA) * accelAngle;
}

Это весь фильтр. Прочитайте его так: возьмите угол, до которого, по оценке гироскопа, робот повернулся с момента последнего обновления, и сдвиньте его на 2% пути к тому, что сейчас утверждает акселерометр.

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

Выбор постоянной времени

ALPHA — не произвольный параметр. Он соответствует постоянной времени:

τ = (ALPHA · dt) / (1 − ALPHA)

τ — граница между полосами. Изменения быстрее τ приходят от гироскопа; медленнее — побеждает акселерометр. При цикле 5 ms и ALPHA = 0.98 τ = 0.245 s — примерно четверть секунды полномочий гироскопа, прежде чем акселерометр снова начнёт проявлять себя.

Обратите внимание, что следует из формулы: ALPHA зависит от частоты цикла. Скопировав значение из проекта с частотой 100 Hz в свой цикл 500 Hz, вы молча сделали фильтр в пять раз медленнее. Если период цикла изменился, пересчитайте ALPHA из желаемого τ, а не сохраняйте прежнее число.

Время цикла должно быть настоящим

dt должно быть реально прошедшим временем, а не тем, которое вы собирались выдержать:

unsigned long now = micros();
float dt = (now - lastUpdate) / 1000000.0;
lastUpdate = now;

Жёсткое задание dt = 0.005 из-за вызова delay(5) — распространённое сокращение, которое тихо отравляет результат: только чтение по I²C занимает почти миллисекунду, поэтому реальный цикл составляет 6 ms, и каждое интегрирование на 17% короткое. Затем угол дрейфует так, что выглядит точно как смещение гироскопа, и это нельзя устранить калибровкой.

Ещё лучше запускать цикл от аппаратного таймера с фиксированной частотой, а не через delay(). Контуру балансировки нужна согласованная частота гораздо больше, чем конкретное её значение.

Правильные знаки

Самая частая ошибка — несовпадение знаков двух источников. Если акселерометр сообщает +3° при наклоне вперёд, а гироскоп — отрицательную скорость для того же движения, слагаемые борются друг с другом: выход фильтра будет отставать, дрожать и установится на неверном значении, и никакая настройка ALPHA это не исправит.

Проверьте напрямую. Выведите рядом accelAngle и текущий интеграл gyroRate, затем медленно наклоните робота вперёд рукой:

• Оба должны изменяться в одном направлении.
• Оба должны достигать примерно одинаковой величины.

Если они изменяются противоположно, измените знак скорости гироскопа. Если отличаются на 90°, для плоскости падения робота выбраны неправильные оси акселерометра.

Почему не фильтр Кацмана?

Фильтр Кацмана — статистически оптимальный ответ, если известны характеристики шума датчиков, и он действительно работает немного лучше, особенно при длительном ускорении.

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

Сначала добейтесь балансировки на комплементарном фильтре. Если робот балансирует хорошо, ответ у вас есть. Более широкое рассмотрение компромиссов приведено в шуме датчиков, фильтрации и слиянии.

Проверьте перед замыканием контура

Две проверки ловят почти всё и занимают по минуте:

Держите неподвижно две минуты. Отфильтрованный угол должен оставаться на месте. Если он ползёт, калибровка смещения гироскопа неверна — слагаемое акселерометра должно предотвращать именно это, поэтому ползание означает, что ALPHA слишком велико для вашего смещения.

Трясите, не вращая. Перемещайте корпус робота из стороны в сторону, сохраняя вертикальное положение. Один акселерометр будет сильно колебаться; отфильтрованный угол должен почти не двигаться. Если он тоже колеблется, ALPHA слишком мало и вы доверяете акселерометру во время ускорения — именно в такой ситуации он работает хуже всего.

Когда угол сохраняет неподвижность в покое и остаётся достоверным при встряхивании, его достаточно для балансировки. Замкните контур в каскадном ПИД-управлении балансировкой (PID) или посмотрите на настроенный контур в симуляторе балансировки. Та же форма фильтра — гладкий, но дрейфующий сигнал, который корректирует другой, шумный, но ограниченный, — используется, когда уличный робот превращает данные GPS с частотой 1 Hz в пригодную оценку координат.