Mpu 6050 как совместить гироскоп и акселерометр

14

Урок 12. Управление сервоприводами с помощью гироскопа MPU6050 Gy-521

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

В этом примере мы научимся управлять двумя серво приводами с помощью акселерометра, когда мы будем отклонять акселерометр Gy-521 (MPU6050) по координате X и Y сервоприводы будут поворачиваться на отклоненный угол.

В данном уроке нам понадобится:

Для реализации проекта нам необходимо установить библиотеки:

Сборка:

1) Подключаем Акселерометр Gy-521

Для питания модуля необходимо использовать строго 3.3V! Для этого можно использовать преобразователь напряжения на 3.3V.

2) Сервоприводы подключаем следующим образом:

Сервопривод рекомендуется питать от внешнего источника питания, если запитать сервопривод от ардуины, то могут возникнуть помехи и перебои в работе arduino. Организовать это можно с помощью источника питания 9V и комбинированного стабилизатора 5V ,3.3V.

Учебное пособие по акселерометру и гироскопу MPU6050. Подключение к Ардуино

MPU6050 имеет 3-осевой акселерометр и 3-осевой гироскоп, интегрированные в один чип. Гироскоп измеряет скорость вращения или скорость изменения углового положения во времени по осям X, Y и Z. Для измерения используется технология MEMS и эффект Кориолиса.

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

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

MPU6050 также называют устройством слежения за движением по шести осям или устройством с 6 степенями свободы (шесть степеней свободы) из-за его 6 выходов, или 3 выхода акселерометра и 3 выхода гироскопа.

Ардуино и MPU6050

Давайте посмотрим, как мы можем подключить и прочитать данные с датчика MPU6050 с помощью Ардуино. Для связи с Ардуино мы используем протокол I2C, поэтому нам нужны всего два провода для подключения по линии данных и два провода для питания.

Скетч MPU6050 Ардуино

Ниже приведем полный код, а после разъясним его поподробнее:

Описание кода: Итак, сначала нам нужно подключить библиотеку Wire.h, которая используется для I2C связи, и определить некоторые переменные, необходимые для хранения данных.

В разделе setup() нам нужно инициализировать библиотеку Wire.h и сбросить датчик через регистр управления . Для этого нам нужно взглянуть в datasheet, где мы можем увидеть адрес регистра:

Также, если мы хотим, мы можем выбрать полный диапазон для акселерометра и гироскопа, используя их регистры конфигурации. В этом примере мы будем использовать диапазон по умолчанию + — 2g для акселерометра и диапазон 250 градусов/с для гироскопа, поэтому оставим эту часть кода закомментированной:

В разделе loop() мы начинаем с чтения данных акселерометра. Данные для каждой оси хранятся в двух байтах или регистрах, и мы можем видеть адреса этих регистров в datasheet датчика:

Чтобы прочитать их все, мы начинаем с первого регистра и с помощью функции RequiestFrom() запрашиваем чтение всех 6 регистров для осей X, Y и Z. Затем мы читаем данные из каждого регистра, и, поскольку выходные данные состроят из старшего и младшего байта, мы соответствующим образом объединяем их, чтобы получить правильные значения:

Чтобы получить выходные значения от -1g до + 1g, подходящие для расчета углов, мы делим выходной сигнал с предварительно выбранной чувствительностью.

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

Далее тем же методом получаем данные гироскопа:

Мы считываем шесть регистров гироскопа, соответствующим образом объединяем их данные и делим их на предварительно выбранную чувствительность, чтобы получить результат в градусах в секунду:

Здесь вы можете заметить, что выходные значения корректируются на величину ошибок (объясним далее). Поскольку выходные данные выражаются в градусах в секунду, то нам нужно умножить их на время, чтобы получить только градусы. Значение времени фиксируется перед каждой итерацией чтения с помощью функции millis().

Наконец, мы объединяем данные акселерометра и гироскопа с помощью дополнительного фильтра. Здесь мы берем 96% данных гироскопа, потому что они очень точны и не подвержены внешним воздействиям.

Обратной стороной гироскопа является то, что он дрейфует или вносит ошибку в выходной сигнал с течением времени. Поэтому в долгосрочной перспективе мы используем данные акселерометра, в данном случае 4%, что достаточно для устранения ошибки дрейфа гироскопа.

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

Прежде чем мы взглянем на результаты, объясним, как получить значения коррекции ошибок. Для вычисления этих ошибок мы можем вызвать пользовательскую функцию calculate_IMU_error(), когда датчик находится в неподвижном положении.

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

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

Работа с Arduino и MPU6050

MPU6050 представляет собой 3-х осевой гироскоп и 3-х же осевой акселерометр в одном корпусе. Сразу поясню, что это и для чего:

• Гироскоп измеряет угловую скорость вращения вокруг оси, условно в градусах/секунду. Если датчик лежит на столе – по всем трём осям будет значение около нуля. Для нахождения текущего угла по скорости нужно интегрировать эту скорость. Гироскоп может чуть привирать, поэтому ориентироваться на него для расчёта текущего угла не получится даже при идеальной калибровке.
• Акселерометр измеряет ускорение вдоль оси, условно в метрах/секунду/секунду. Если датчик лежит на столе или движется с постоянной скоростью – на оси будет спроецирован вектор силы тяжести. Если датчик движется с ускорением – вдобавок к ускорению свободного падения получим составляющие вектора ускорения. Если датчик находится в свободном падении (в том числе на орбите планеты) – величины ускорений по всем осям будут равны 0. Зная проекции вектора силы тяжести можно с высокой точностью определить угол наклона датчика относительно него (привет, школьная математика). Если датчик движется – однозначно определить направление вектора силы тяжести не получится, соответственно угол тоже.

Модуль стоит в районе 150 рублей на нашем любимом AliExpress (ссылка, ссылка), а также входит в Arduino набор GyverKIT.

Итак, по отдельности акселерометр и гироскоп не могут обеспечить точное измерение угла, но вместе – ещё как могут! Об этом мы поговорим ниже. А начнём с подключения и базового общения с датчиком.

Подключение

Датчик подключается на шину i2c (SDA -> A4, SCL -> A5, GND -> GND). На плате стоит стабилизатор, позволяющий питаться от пина 5V (VCC -> 5V).

На модуле выведен пин AD0. Если он никуда не подключен (или подключен к GND) – адрес датчика на шине i2c будет 0x68, если подключен к питанию (VCC) – адрес будет 0x69. Таким образом без дополнительных микросхем можно подключить два датчика на шину с разными адресами.

Получение сырых данных

Библиотека MPU6050 от i2cdev

Расшифровка сырых данных

По каждой оси и параметру датчик выдаёт 16-битное знаковое значение ( -32768.. 32767 ). При стандартных настройках (как в примерах выше) это значение отражает:

• Ускорение в диапазоне -2.. 2 g (9.82 м/с/с).
• Угловую скорость в диапазоне -250.. 250 градусов/секунду.

Таким образом, для перевода сырых данных в величины СИ (если это нужно) можно сделать по каждой оси:

• Ускорение в м/с/с при чувствительности 2: float accX_f = accX / 32768 * 2
• Угловая скорость в град/с при чувствительности 250: float gyrX_f = gyrX / 32768 * 250

Думаю закономерность понятна.

Настройка чувствительности

Датчик позволяет настроить чувствительность по каждому параметру, в библиотеке i2cdev это делается так:

• setFullScaleAccelRange(MPU6050_ACCEL_FS_2); – диапазон -2.. 2 g
• setFullScaleAccelRange(MPU6050_ACCEL_FS_4); – диапазон -4.. 4 g
• setFullScaleAccelRange(MPU6050_ACCEL_FS_8); – диапазон -8.. 8 g
• setFullScaleAccelRange(MPU6050_ACCEL_FS_16); – диапазон -16.. 16 g

• setFullScaleGyroRange(MPU6050_GYRO_FS_250); – диапазон -250.. 250 град/с
• setFullScaleGyroRange(MPU6050_GYRO_FS_500); – диапазон -500.. 500 град/с
• setFullScaleGyroRange(MPU6050_GYRO_FS_1000); – диапазон -1000.. 1000 град/с
• setFullScaleGyroRange(MPU6050_GYRO_FS_2000); – диапазон -2000.. 2000 град/с

Перевести сырые данные можно таким же способом, как в предыдущем пункте, но с учётом нового диапазона.

Калибровка оффсетов

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

16384 по оси Z и нули по всем остальным осям ускорения и угловой скорости. Но это фантастика =)

Максимально правильно использовать калибровку в проекте нужно так: калибровка по запросу (кнопка, меню, и т.д.), затем запись калибровочных значений в EEPROM. При запуске – чтение и настройка оффсетов из. Рассмотрим несколько примеров калибровки, первый – из библиотеки. Калибрует долго, но максимально точно. При малых вибрациях и движениях датчика в процессе калибровки (даже от громкого звука) калибровка может не закончиться. Второй вариант – мой упрощённый алгоритм калибровки, калибрует быстро, без возможности зависнуть при тряске, но даёт менее точный результат. Я делюсь примерами, в свой проект их нужно будет переносить вручную и аккуратно =)

Алгоритмы калибровки

16384 по оси Z и нули по всем остальным осям ускорения и угловой скорости. Но это фантастика =) Максимально правильно использовать калибровку в проекте нужно так: калибровка по запросу (кнопка, меню, и т.д.), затем запись калибровочных значений в EEPROM. При запуске – чтение и настройка оффсетов. Да, можно замерить значения по всем 6 осям в покое, сохранить их в переменные, а затем вычитать из свежих прочитанных в процессе работы. Такой способ работает для каких-то базовых операций с датчиком (определение перегрузок, тряски, наличия вращения, и т.д.). Для использования MPU6050 с целью максимально точного определения углов поворота платы такой вариант к сожалению не подходит: калибровать нужно рекурсивно. Рассмотрим несколько примеров калибровки, первый – из библиотеки. Калибрует долго, но максимально точно. При малых вибрациях и движениях датчика в процессе калибровки (даже от громкого звука) калибровка может не закончиться. Второй вариант – мой упрощённый алгоритм калибровки, калибрует быстро, без возможности зависнуть при тряске, но даёт менее точный результат. Я делюсь примерами, в свой проект их нужно будет переносить вручную и аккуратно =)

101 применение шлюза UART-to-I2C/SPI/1W (RH-0004). Часть 6. Подключение 3-осевого гироскопа/акселерометра MPU6050 к компьютеру

Насмотрелся я в ютубе видосов про всякие гироскопы, ну и захотелось мне тоже с ними поиграться. Одной из самых популярных оказалась платка GY-521 с чипом MPU6050 (фотка справа). Дёшево и сердито. Что из этого в итоге получилось, я сейчас попробую рассказать.

Итак, чип MPU6050 включает в себя:

• 3-осевой гироскоп c 16-битным АЦП
  настраиваемый диапазон: ±250, ±500, ±1000, ±2000 0 /sec (dps)
• 3-осевой акселерометр с 16-битным АЦП
  настраиваемый диапазон: ±2g, ±4g, ±8g, ±16g
• температурный сенсор
• digital motion processor (DMP)
• буфер FIFO (1024 байта)

Приятные и полезные «плюшки»:

• ток потребления в нормальном режиме всего 4 мА (а есть ещё режим пониженного потребления)
• система прерываний (детектор движения с программируемым порогом срабатывания, переполнение буфера FIFO…)
• выбор источника тактирования из нескольких вариантов, включая внешний источник и внутренний генератор 8 MHz
• программируемый низкочастотный фильтр (DLPF)
• программируемая частота сэмплирования (Sample Rate = Gyroscope Output Rate / (1+ SMPLRT_DIV), где Gyroscope Output Rate = 1 kHz при включенном DLPF и 8 kHz при выключенном)
• возможность подключить внешний трехосевой магнитометр

Работа с чипом происходит через регистры, посредством интерфейсов SPI или I2C, в зависимости от модификации чипа. Мне достался чип с интерфейсом I2C (что, собственно, без разницы). Документ с картой регистров можно скачать в конце статьи, а схема показана на рисунке справа.

Как правило, модуль GY-521 используют совместно с Arduino, либо с чем-то подобным, имеющим на борту достаточно «толстый» контроллер. «Толстый» контроллер нужен ввиду того, что данные гироскопа и акселерометра (мгновенные скорости вращения и ускорения по трём осям) обычно нафиг никому не нужны. Всем интересно положение тела в пространстве, а для того, чтобы его получить — нужно данные гироскопа и акселерометра интегрировать.

Есть второй путь, — использовать встроенный Digital Motion Processor. Он может сам проводить интегрирование и отдавать нам уже положение тела в пространстве в виде кватерниона (это такая математическая сущность, которой очень удобно описывать всякие вращения, подробнее как-нибудь отдельно про неё напишу).

Я решил использовать DMP. Это позволяло обойтись без сложных вычислений и подключить модуль напрямую к компьютеру с помощью шлюза UART-to-I2C/SPI/1W (шлюз, как всегда, был дополнен преобразователем USB-to-UART, чтобы подключаться к компьютеру по USB).

Запитать плату также можно прямо от шлюза, — для этого надо установить перемычку выбора питания на шлюзе (JP1) в положение +5В. Контакты соединяем следующим образом:

• SCL Clock (тактирование I2C)
• SDA Data (данные I2C)
• VCC Supply (питание)
• GND COM (общий)

С DMP есть только одна проблема — на него совершенно нет документации. Её в принципе и на сам чип не много, но на DMP вообще нет. Перерыв кучу разных источников, я обнаружил, что все работающие варианты сходятся к одному источнику — библиотеке под AVR Дэвида Джирони (Davide Gironi) от 2012 года, который, в свою очередь, ссылается на библиотеку для ардуино от Джефа Роуберга (Jeff Rowberg).

Как этот DMP работает, какие фильтры использует, — никто толком не знает (кто-то конечно знает, но кто знает — не рассказывает). В DMP просто заливается в виде бинарников эн-цать блоков кода, в процессе чего необходимо сделать несколько включений, выключений, переключений (этот алгоритм я опишу ниже), и всё, — остаётся только читать данные из буфера FIFO.

Размер буфера, как уже отмечалось, 1024 байта. Есть двухбайтный регистр-счётчик, содержащий количество байт в буфере (FIFO_COUNTH, FIFO_COUNTL). Чтобы эти регистры корректно обновлялись нужно обязательно вычитывать оба регистра, причём первым обязательно FIFO_COUNTH. Кроме того, можно настроить прерывания по переполнению буфера и по поступлению в буфер очередной порции данных.

Данные от DMP имеют вид блоков по 42 байта следующей структуры:

Как видите, размер буфера FIFO не кратен размеру пакета от DMP, то есть надо считывать данные быстрее, чем буфер успеет переполниться, ну или в случае переполнения придётся что-то придумывать для определения начала пакета от DMP. Я решил, что успею сделать это даже по USB, если выбрать частоту сэмплирования поменьше, скажем 200 Гц и ниже.

Итак, мне оставалось просто портировать нужные функции из C для AVR в С++ Builder, что и было в итоге сделано. Задача сильно облегчалась тем, что я, в своё время, написал специальную динамически подключаемую библиотеку (rh0004.dll) для работы со шлюзом и теперь для общения по I2C пользовался стандартными функциями из этой библиотеки. Функции я портировал следующие:

• mpu6050_readBytes — прочитать указанное число байт, начиная с определённого адреса
• mpu6050_readByte — прочитать один байт по указанному адресу
• mpu6050_writeBytes — записать указанное число байт, начиная с определённого адреса
• mpu6050_writeByte — записать один байт по указанному адресу
• mpu6050_readBits — прочитать указанные биты из байта по определённому адресу
• mpu6050_readBit — прочитать один бит из байта по определённому адресу
• mpu6050_writeBits — записать указанные биты в байт по определённому адресу
• mpu6050_writeBit — записать один бит в байт по определённому адресу
• mpu6050_setMemoryBank — выбор банка памяти
• mpu6050_setMemoryStartAddress — установка указателя адреса в выбранном банке памяти
• mpu6050_readMemoryBlock — чтение блока памяти
• mpu6050_writeMemoryBlock — запись блока памяти
• mpu6050_writeDMPConfigurationSet — запись блока конфигурации DMP
• mpu6050_getFIFOCount — чтение количества байт, находящихся в буфере FIFO
• mpu6050_getFIFOBytes — чтение заданного количества байт из буфера FIFO
• mpu6050_getIntStatus — чтение регистра INT_STATUS (статус прерываний)
• mpu6050_resetFIFO — очистить буфер FIFO
• mpu6050_getXGyroOffset
• mpu6050_setXGyroOffset
• mpu6050_getYGyroOffset
• mpu6050_setYGyroOffset
• mpu6050_getZGyroOffset
• mpu6050_setZGyroOffset
• mpu6050_setSleepDisabled — выйти из спящего режима
• mpu6050_setSleepEnabled — перейти в спящий режим
• mpu6050_testConnection — прочитать регистр MPU6050_RA_WHO_AM_I
• mpu6050_init — инициализация акселерометра и гироскопа
• mpu6050_dmpInitialize — инициализация DMP
• mpu6050_dmpEnable — включение DMP
• mpu6050_dmpDisable — отключение DMP

Как вы наверное заметили, часть функций (такие как get(set)X(Y,Z)GyroOffset) не описана вообще, а часть описана непонятно (скажем, про блоки и банки памяти). Это спасибо документации, в ней вообще отсутствуют описания регистров с нулевого по 12-й, а так же со 109-го по 113-й (судя по препарируемой библиотеке, они имеют отношение к начальной калибровке и DMP). Эту часть пришлось тупо скопировать, без какого-либо понимания почему, зачем и как.

Переходим к самому интересному. Алгоритм инициализации акселерометра и гироскопа (функция mpu6050_init) выглядит следующим образом:

1. выходим из спящего режима, — функция mpu6050_setSleepDisabled() сбрасывает бит MPU6050_PWR1_SLEEP_BIT в регистре MPU6050_RA_PWR_MGMT_1
2. выбираем источник тактирования, — я выбрал XGYRO (прописываем соответствующие биты в регистр MPU6050_RA_PWR_MGT_1)
3. настраиваем низкочастотный фильтр (прописываем соответствующие биты в регистр MPU6050_RA_CONFIG), я настроил фильтр на 42 Hz
4. настраиваем частоту сэмплирования (прописываем соответствующие биты в регистр MPU6050_RA_SMPLRT_DIV), я использовал частоту 200 Hz
5. настраиваем диапазон шкалы гироскопа (прописываем соответствующие биты в регистр MPU6050_RA_GYRO_CONFIG)
6. настраиваем диапазон шкалы акселерометра (прописываем соответствующие биты в регистр MPU6050_RA_ACCEL_CONFIG)

Собственно, после описанных выше манипуляций чип заработает и можно будет читать с него «живые» данные гироскопа и акселерометра по всем трём осям. Жаль, что эти данные без обработки, как мы уже говорили, никому не нужны. Для обработки придётся включить и настроить ещё и DMP. Алгоритм инициализации и настройки DMP несколько длиннее, поэтому здесь я его приводить не буду. Этот алгоритм можно посмотреть в логах демонстрационной программы.

Демонстрационная программа для компьютера, позволяющая вычитывать из чипа данные от DMP и визуализировать угловое положение чипа в пространстве, была написана на C++ Builder-е. Для визуализации использовалась библиотека OpenGL. Программу с исходниками, документацию на чип MPU6050 и ссылку на видео в youtube, демонстрирующее работу программы, можно найти ниже.