MPU-6050: 6-осевой IMU для автовыравнивания мультикоптеров (Arduino)
Примечание
Автор статьи и видеоуроков: J.M. Brokking (Joop Brokking), brokking.net. Ниже подробный пересказ по его странице imu.html (копия в Wayback Machine). Два видео серии указаны на странице Видео и ссылки на оригинальные страницы.
Чтобы мультикоптер сам выравнивался, полётный контроллер должен точно знать, где «низ», как строительный уровень на аппарате по осям тангажа и крена. Автор объясняет, как написать простой 6-осевой IMU (гироскоп + акселерометр) на MPU-6050. В своём контроллере он этот датчик не использует, но показывает именно его: дёшев, доступен и содержит оба датчика в одном корпусе. Железо для уроков: Funduino Pro Mini, MPU-6050, LCD.
Файлы
Часть 1. IMU только на гироскопе
Гироскоп измеряет угловую скорость (градусы в секунду). Угловое движение одинаково в любой точке аппарата, но датчик лучше ставить в центре, где вибрации минимальны.
По даташиту MPU-6050 при диапазоне 500°/с выход равен 65,5 на 1°/с. Выход — знаковое 16-битное число (как int в Arduino); максимум примерно 500 × 65,5 = 32750. Один оборот вокруг оси рыскания за минуту (6°/с) даёт 6 × 65,5 = 393.
Гироскоп нужно калибровать: среднее по 2000 отсчётов вычитается из сырого значения (тот же код, что в YMFC-3D). Неподвижный датчик иначе «показывает» движение.
Вибрации моторов видны на графике (последовательный плоттер Arduino IDE): при вращающемся моторе выход гироскопа заметно колеблется, даже если датчик не движется.
Интегрирование. Если складывать значения гироскопа во времени, получается пройденный угол. При частоте цикла 250 Гц каждое значение делится на 250 и на 65,5, то есть умножается на 0,0000611 и прибавляется к углу (переменная типа float). Проверка: 60 секунд × 250 = 15000 отсчётов × 393 × 0,0000611 = 360,18°.
Связь осей при рыскании. Если повернуть аппарат по рысканию на 90°, оси тангажа и крена меняются местами, а накопленные углы остаются прежними. Нужно перенести часть крена в тангаж и наоборот. Зависимость оказалась синусной (автор построил график для шага рыскания 5° и сравнил с синусом): при рыскании синус тангажа вычитается из крена, а синус крена прибавляется к тангажу (угол в радианах: значение рыскания × 0,0000611 × π/180).
Проблема: со временем углы «уплывают» (дрейф гироскопа, вибрации ускоряют его), и нет опоры на горизонт при старте с наклонной поверхности. Решается акселерометром.
Часть 2. Добавляем акселерометр
Модель акселерометра: шарик в кубе, подвешенный на струнах к датчикам; сила, удерживающая шарик от падения, измеряется по каждой оси. При диапазоне ±8 g выход 4096 на 1 g. Перевёрнутый датчик даёт отрицательное значение. При наклоне на 45° выходы по двум осям равны (в примере 2920 и 2920), а длина вектора тяжести считается по теореме Пифагора: √(2920² + 2920²) ≈ 4129, то есть так же, как в горизонтальном положении. Угол даёт арксинус отношения компоненты к длине вектора (в коде результат
asinумножается на 57,296 для перевода в градусы); для 2920/4129 ≈ 0,707 получается 45°.Углы тангажа и крена по акселерометру не дрейфуют, но при любых ускорениях (вибрации в полёте) «скачут», то есть для выравнивания ненадёжны. Скользящее среднее убирает шум, но запаздывает при реальном изменении угла.
Итог автора: IMU на гироскопе меньше страдает от вибраций, но дрейфует; IMU на акселерометре бесполезен из-за вибраций, пригодно только среднее значение.
Комплементарный фильтр. Берётся большая часть угла гироскопа и очень малая часть угла акселерометра (в примере акселерометр делится на 25 и добавляется), чего достаточно, чтобы скомпенсировать дрейф, без запаздывания. Автор прямо рекомендует не использовать фильтр Калмана, если вы его не понимаете («никогда не используйте то, чего полностью не понимаете»): акселерометр из-за вибраций всегда ненадёжен, поэтому его используют только для коррекции дрейфа гироскопа.
Старт. После калибровки гироскопа начальный угол гироскопа приравнивается к углу акселерометра (аппарат в покое); затем включается коррекция.
Калибровка акселерометра. Положить датчик по уровню, записать значения углов и вычитать их из последующих измерений.
Результат автора — «очень простой, но надёжный IMU» для автовыравнивания квадрокоптера. Он используется в YMFC-AL: квадрокоптер с автостабилизацией на Arduino Uno, YMFC-32: квадрокоптер на STM32 (Blue Pill) и других проектах.
Видео автора (YouTube)
Видео проекта на канале автора MacPuffdog: плеер ведёт на оригинал, ролики лежат у автора.
Фото на сайте автора
Фотографии и рисунки к проекту лежат на сервере автора (у сайта нет HTTPS, поэтому здесь только ссылки, они открываются в новой вкладке). Копия всех файлов хранится у нас в архиве.
