roboforum.ru

Использование комплементарного фильтра для получения угла от MPU-6050
Для начала немного теории.
Акселерометр при помощи арктангенса отношения данных от двух осей позволяет определить угол наклона. Угол этот стабильный, но имеет высокочастотный шум, поскольку акселерометр очень чувствительный. И это в статике. В динамике же дополнительно накладываются ускорения разгона/торможения, вибрация, любые внешние механические воздействия. То есть угол есть, но он утонул в шумах. Усреднять шумы – внести сильное запаздывание, а это неприемлемо, угол надо знать сейчас, а не потом, тем более что он все время меняется.
Сразу внесу ясность – работа этого фильтра не отменяет необходимость калибровки как акселерометра, так и гироскопа. Без калибровки это будет не угол, а погода на Марсе.
При помощи гироскопа можно определить угол, интегрируя скорость вращения. Но гироскоп никогда не имеет состояния «мои внутренние штуки не вращаются». Даже когда он находится в покое – все равно выдает не нулевое значение скорости вращения. Это приводит к дрейфу вычисленного угла. Дрейф небольшой, показания стабильные, но долго так угол не сможешь определять.
Тут на помощь приходит комплементарный фильтр. Он позволяет брать кусок одного данного и прибавлять к нему кусок другого данного. У этого фильтра есть всего один параметр, назовем его просто «коэффициент (К)». Это число меньше единицы. Первый кусок данных умножается на (1 – К), второй кусок данных умножается на К.
Как уже было сказано выше – гироскоп «плавает» со временем, причем с достаточно большим временем. То есть эквивалентно его сигнал низкочастотный, а помеха находится в области сверхнизких частот. Из этого следует, что доля гироскопа в фильтре должна быть «львиной». Акселерометр шумит в области ВЧ, а его настоящие значения более низкочастотные (если снять осциллограмму показаний угла от акселерометра, а потом отобразить в частотную область – это будет явно видно). Так что доля акселерометра должна быть достаточно мала.
Теперь переходим к самому алгоритму.
Сперва получаем все данные от MPU-6050, потом начинаем с ними работать (я не пишу, как их получаем, поскольку это не касается работы алгоритма). Получаем приращение угла от гироскопа, прибавляем его к ранее полученному углу (если эта операция делается впервые, то прибавляем к накопителю угла гироскопа, который уже инициализирован нужным углом после включения. Позже я еще вернусь к этому). Затем угол из накопителя угла гироскопа умножаем на (1 – К) и пока откладываем. Берем данные от акселерометра, вычисляем через арктангенс угол, затем умножаем угол на К. Складываем эти произведения и записываем полученный угол в накопитель угла гироскопа. Умножение производим с учетом знака угла, складываем два произведения в дополнительном коде.
А каким же должен быть коэффициент К? Вопрос, на который нет конкретного ответа. Он определяется конкретно вашим MPU-6050. Кстати, этот фильтр не ограничивается конкретно этим датчиком, для других акселерометров и гироскопов алгоритм тоже работает. Итак, есть два «огня»: дрейф гироскопа и шум акселерометра. Нам нужно найти хорошее место между огнями. Если К взять очень маленьким, то часть от акселерометра не сможет полностью устранить дрейф гироскопа. Робот у вас стоит и не шевелится, а угол медленно, но уплывает. Если К взять очень большим, то останутся шумы от акселерометра, а дрейфа гироскопа не будет. То есть критерием качества можно назвать минимальный К, при котором дрейф не проявляется длительное время (по крайней мере столько, сколько вы хотите для робота автономной работы). А потом для гарантии чуть-чуть увеличить К. Как мне подсказывают товарищи, статьи которых я изучал по данному вопросу, примерный диапазон К от 0.01 до 0.05.
Небольшой программный финт (я же не люблю работать с дробями):
– допустим, я выбрал К = 0.01 и (1 – К) = 0.99. Представляю десятичную дробь в виде обыкновенной, и умножаю числитель и знаменатель на 16777216 (это 0х1000000). Потом мне нужно будет избавиться от трех «лишних» байтов в результате вычисления, зато умножение у меня становится более простым – умножаю на 167772 и 16609444.
Ну и в конце вернусь к вопросу: каким значением надо инициализировать накопитель угла для работы гироскопа.
В самом простом случае, когда вы включаете робота в вертикальном положении, накопитель можно просто обнулить. Очень быстро при помощи фильтра в накопителе появится актуальное значение угла. Второй способ: можно взять угол от акселерометра. Но он же шумный? Правильно, но мы же инициализируем, что нам мешает сделать усреднение? Это займет примерно секунду.
Вот такой вот несложный алгоритм позволяет иметь от двух «врущих» датчиков достаточно точную «правду» об угле наклона.

Ну и немного практических результатов. Я откалибровал гироскоп так, что дрейф составил примерно 3 градуса в минуту. Показания от гироскопа и акселерометра идут 200 раз в секунду. Попробовал К = 0.01, это сразу убрало весь дрейф гироскопа. Скачки угла составили в размахе около 0.05 градуса. Датчик стоит на столе неподвижно, угол скачет в диапазоне от -0.432 до - 0.480 градуса.