В этой статье мы узнаем, как использовать акселерометр и датчик гироскопа MPU6050 совместно с Ардуино. Объясним, как работает MPU6050 и как считывать с него данные.
MPU6050 — как это работает
MPU6050 имеет 3-осевой акселерометр и 3-осевой гироскоп, интегрированные в один чип. Гироскоп измеряет скорость вращения или скорость изменения углового положения во времени по осям X, Y и Z. Для измерения используется технология MEMS и эффект Кориолиса.
Выходные данные гироскопа измеряются в градусах в секунду, поэтому для получения углового положения нам просто нужно интегрировать угловую скорость.
С другой стороны, акселерометр MPU6050 измеряет ускорение. Вкратце, он может измерять гравитационное ускорение по трем осям, и используя некоторую тригонометрию, мы можем вычислить угол, под которым расположен датчик. Итак, если мы объединим данные акселерометра и гироскопа, мы сможем получить очень точную информацию об ориентации датчика в пространстве.
MPU6050 также называют устройством слежения за движением по шести осям или устройством с 6 степенями свободы (шесть степеней свободы) из-за его 6 выходов, или 3 выхода акселерометра и 3 выхода гироскопа.
Ардуино и MPU6050
Давайте посмотрим, как мы можем подключить и прочитать данные с датчика MPU6050 с помощью Ардуино. Для связи с Ардуино мы используем протокол I2C, поэтому нам нужны всего два провода для подключения по линии данных и два провода для питания.
Скетч MPU6050 Ардуино
Ниже приведем полный код, а после разъясним его поподробнее:
#include <Wire.h>
const int MPU = 0x68; // MPU6050 I2C адрес
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float roll, pitch, yaw;
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
float elapsedTime, currentTime, previousTime;
int c = 0;
void setup() {
Serial.begin(19200);
Wire.begin(); // Инициализировать связь
Wire.beginTransmission(MPU); // Начать связь с MPU6050 // MPU = 0x68
Wire.write(0x6B); // обращаемся к регистру 6B
Wire.write(0x00); // сброс - поместите 0 в регистр 6B
Wire.endTransmission(true); //закончить передачу
/*
// Настройка чувствительности акселерометра - полный диапазон шкалы (по умолчанию +/- 2g)
Wire.beginTransmission(MPU);
Wire.write(0x1C); //обращаемся к регистру ACCEL_CONFIG (1C hex)
Wire.write(0x10); //Установить биты регистра как 00010000 (диапазон полной шкалы +/- 8g)
Wire.endTransmission(true);
// Настроить чувствительность гироскопа - полный диапазон шкалы (по умолчанию +/- 250 град / с)
Wire.beginTransmission(MPU);
Wire.write(0x1B); // Обращаемся к регистру GYRO_CONFIG (1B hex)
Wire.write(0x10); // Установить биты регистра как 00010000 (полная шкала 1000 град / с)
Wire.endTransmission(true);
delay(20);
*/
// Вызов этой функции, покажет значения ошибок IMU для вашего модуля
calculate_IMU_error();
delay(20);
}
void loop() {
// === Считать данные акселерометра === //
Wire.beginTransmission(MPU);
Wire.write(0x3B); // Начинаем с регистра 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); // Чтение всех 6 регистров, значение каждой оси сохраняется в 2 регистрах
// Для диапазона + -2g нам нужно разделить необработанные значения на 16384 в соответствии с datasheet
AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // Значение по оси X
AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Значение по оси Y
AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Значение по оси Z
// Расчет крена и тангажа по данным акселерометра
accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 0.58;
accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) + 1.58;
// === Считываем данные гироскопа === //
previousTime = currentTime; // Предыдущее время сохраняется до фактического чтения
currentTime = millis(); // Текущее время Фактическое время чтения
elapsedTime = (currentTime - previousTime) / 1000; // Разделим на 1000, чтобы получить секунды
Wire.beginTransmission(MPU);
Wire.write(0x43); // Адрес первого регистра данных гироскопа 0x43
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); // Чтение всех 4 регистров, значение каждой оси сохраняется в 2 регистрах
GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // Для диапазона 250 градусов / с мы должны сначала разделить необработанное значение на 131.0, согласно datasheet
GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
// Корректируем выходы с вычисленными значениями ошибок
GyroX = GyroX + 0.56; // GyroErrorX ~(-0.56)
GyroY = GyroY - 2; // GyroErrorY ~(2)
GyroZ = GyroZ + 0.79; // GyroErrorZ ~ (-0.8)
// В настоящее время необработанные значения выражаются в градусах в секунду, град / с, поэтому нам нужно умножить на sendonds (s), чтобы получить угол в градусах
gyroAngleX = gyroAngleX + GyroX * elapsedTime; // deg/s * s = deg
gyroAngleY = gyroAngleY + GyroY * elapsedTime;
yaw = yaw + GyroZ * elapsedTime;
// Дополнительный фильтр - объединить значения угла акселерометра и гироскопа
roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;
// Выводим значения на серийный монитор
Serial.print(roll);
Serial.print("/");
Serial.print(pitch);
Serial.print("/");
Serial.println(yaw);
}
void calculate_IMU_error() {
// Мы можем вызвать эту функцию в разделе setup(), чтобы вычислить ошибку данных акселерометра и гироскопа. Отсюда мы получим значения ошибок, используемые в приведенных выше уравнениях, напечатанные на последовательном мониторе.
// Обратите внимание, что мы должны разместить датчик плоско, чтобы получить правильные значения, чтобы затем мы могли считать правильные значения
// Считываем значения акселерометра 200 раз
while (c < 200) {
Wire.beginTransmission(MPU);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);
AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
// Sum all readings
AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));
AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI));
c++;
}
// Разделим сумму на 200, чтобы получить значение ошибки
AccErrorX = AccErrorX / 200;
AccErrorY = AccErrorY / 200;
c = 0;
// Считываем значения гироскопа 200 раз
while (c < 200) {
Wire.beginTransmission(MPU);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);
GyroX = Wire.read() << 8 | Wire.read();
GyroY = Wire.read() << 8 | Wire.read();
GyroZ = Wire.read() << 8 | Wire.read();
// Sum all readings
GyroErrorX = GyroErrorX + (GyroX / 131.0);
GyroErrorY = GyroErrorY + (GyroY / 131.0);
GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
c++;
}
// Разделим сумму на 200, чтобы получить значение ошибки
GyroErrorX = GyroErrorX / 200;
GyroErrorY = GyroErrorY / 200;
GyroErrorZ = GyroErrorZ / 200;
// Выводим значения ошибок на Serial Monitor
Serial.print("AccErrorX: ");
Serial.println(AccErrorX);
Serial.print("AccErrorY: ");
Serial.println(AccErrorY);
Serial.print("GyroErrorX: ");
Serial.println(GyroErrorX);
Serial.print("GyroErrorY: ");
Serial.println(GyroErrorY);
Serial.print("GyroErrorZ: ");
Serial.println(GyroErrorZ);
}
Описание кода: Итак, сначала нам нужно подключить библиотеку Wire.h, которая используется для I2C связи, и определить некоторые переменные, необходимые для хранения данных.
В разделе setup() нам нужно инициализировать библиотеку Wire.h и сбросить датчик через регистр управления . Для этого нам нужно взглянуть в datasheet, где мы можем увидеть адрес регистра:
Также, если мы хотим, мы можем выбрать полный диапазон для акселерометра и гироскопа, используя их регистры конфигурации. В этом примере мы будем использовать диапазон по умолчанию + — 2g для акселерометра и диапазон 250 градусов/с для гироскопа, поэтому оставим эту часть кода закомментированной:
// Настройка чувствительности акселерометра - полный диапазон шкалы (по умолчанию +/- 2g) Wire.beginTransmission(MPU); Wire.write(0x1C); //обращаемся к регистру ACCEL_CONFIG (1C hex) Wire.write(0x10); //Установить биты регистра как 00010000 (диапазон полной шкалы +/- 8g) Wire.endTransmission(true); // Настроить чувствительность гироскопа - полный диапазон шкалы (по умолчанию +/- 250 град / с) Wire.beginTransmission(MPU); Wire.write(0x1B); // Обращаемся к регистру GYRO_CONFIG (1B hex) Wire.write(0x10); // Установить биты регистра как 00010000 (полная шкала 1000 град / с) Wire.endTransmission(true); delay(20);
В разделе loop() мы начинаем с чтения данных акселерометра. Данные для каждой оси хранятся в двух байтах или регистрах, и мы можем видеть адреса этих регистров в datasheet датчика:
Чтобы прочитать их все, мы начинаем с первого регистра и с помощью функции RequiestFrom() запрашиваем чтение всех 6 регистров для осей X, Y и Z. Затем мы читаем данные из каждого регистра, и, поскольку выходные данные состроят из старшего и младшего байта, мы соответствующим образом объединяем их, чтобы получить правильные значения:
// === Считать данные акселерометра === // Wire.beginTransmission(MPU); Wire.write(0x3B); // Начинаем с регистра 0x3B (ACCEL_XOUT_H) Wire.endTransmission(false); Wire.requestFrom(MPU, 6, true); // Чтение всех 6 регистров, значение каждой оси сохраняется в 2 регистрах // Для диапазона + -2g нам нужно разделить необработанные значения на 16384 в соответствии с datasheet AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // Значение по оси X AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Значение по оси Y AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Значение по оси Z
Чтобы получить выходные значения от -1g до + 1g, подходящие для расчета углов, мы делим выходной сигнал с предварительно выбранной чувствительностью.
Наконец, используя две формулы, мы вычисляем углы крена и тангажа на основе данных акселерометра:
// Расчет крена и тангажа по данным акселерометра accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 0.58; accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) + 1.58;
Далее тем же методом получаем данные гироскопа:
Мы считываем шесть регистров гироскопа, соответствующим образом объединяем их данные и делим их на предварительно выбранную чувствительность, чтобы получить результат в градусах в секунду:
// === Считываем данные гироскопа === // previousTime = currentTime; // Предыдущее время сохраняется до фактического чтения currentTime = millis(); // Текущее время Фактическое время чтения elapsedTime = (currentTime - previousTime) / 1000; // Разделим на 1000, чтобы получить секунды Wire.beginTransmission(MPU); Wire.write(0x43); // Адрес первого регистра данных гироскопа 0x43 Wire.endTransmission(false); Wire.requestFrom(MPU, 6, true); // Чтение всех 4 регистров, значение каждой оси сохраняется в 2 регистрах GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // Для диапазона 250 градусов / с мы должны сначала разделить необработанное значение на 131.0, согласно datasheet GyroY = (Wire.read() << 8 | Wire.read()) / 131.0; GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
Здесь вы можете заметить, что выходные значения корректируются на величину ошибок (объясним далее). Поскольку выходные данные выражаются в градусах в секунду, то нам нужно умножить их на время, чтобы получить только градусы. Значение времени фиксируется перед каждой итерацией чтения с помощью функции millis().
// Корректируем выходы с вычисленными значениями ошибок GyroX = GyroX + 0.56; // GyroErrorX ~(-0.56) GyroY = GyroY - 2; // GyroErrorY ~(2) GyroZ = GyroZ + 0.79; // GyroErrorZ ~ (-0.8) // В настоящее время необработанные значения выражаются в градусах в секунду, град / с, поэтому нам нужно умножить на sendonds (s), чтобы получить угол в градусах gyroAngleX = gyroAngleX + GyroX * elapsedTime; // deg/s * s = deg gyroAngleY = gyroAngleY + GyroY * elapsedTime; yaw = yaw + GyroZ * elapsedTime;
Наконец, мы объединяем данные акселерометра и гироскопа с помощью дополнительного фильтра. Здесь мы берем 96% данных гироскопа, потому что они очень точны и не подвержены внешним воздействиям.
Обратной стороной гироскопа является то, что он дрейфует или вносит ошибку в выходной сигнал с течением времени. Поэтому в долгосрочной перспективе мы используем данные акселерометра, в данном случае 4%, что достаточно для устранения ошибки дрейфа гироскопа.
// Дополнительный фильтр - объединить значения угла акселерометра и гироскопа roll = 0.96 * gyroAngleX + 0.04 * accAngleX; pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;
Однако, поскольку мы не можем рассчитать рыскание по данным акселерометра, мы не можем реализовать для него дополнительный фильтр.
Прежде чем мы взглянем на результаты, объясним, как получить значения коррекции ошибок. Для вычисления этих ошибок мы можем вызвать пользовательскую функцию calculate_IMU_error(), когда датчик находится в неподвижном положении.
Здесь мы делаем 200 измерений для всех выходов, суммируем их и делим на 200. Поскольку мы удерживаем датчик в горизонтальном неподвижном положении, ожидаемые выходные значения должны быть 0. Таким образом, с помощью этого расчета мы можем получить среднюю ошибку датчика.
void calculate_IMU_error() {
// Мы можем вызвать эту функцию в разделе setup(), чтобы вычислить ошибку данных акселерометра и гироскопа. Отсюда мы получим значения ошибок, используемые в приведенных выше уравнениях, напечатанные на последовательном мониторе.
// Обратите внимание, что мы должны разместить датчик плоско, чтобы получить правильные значения, чтобы затем мы могли считать правильные значения
// Считываем значения акселерометра 200 раз
while (c < 200) {
Wire.beginTransmission(MPU);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);
AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
// Sum all readings
AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));
AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI));
c++;
}
// Разделим сумму на 200, чтобы получить значение ошибки
AccErrorX = AccErrorX / 200;
AccErrorY = AccErrorY / 200;
c = 0;
// Считываем значения гироскопа 200 раз
while (c < 200) {
Wire.beginTransmission(MPU);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);
GyroX = Wire.read() << 8 | Wire.read();
GyroY = Wire.read() << 8 | Wire.read();
GyroZ = Wire.read() << 8 | Wire.read();
// Sum all readings
GyroErrorX = GyroErrorX + (GyroX / 131.0);
GyroErrorY = GyroErrorY + (GyroY / 131.0);
GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
c++;
}
// Разделим сумму на 200, чтобы получить значение ошибки
GyroErrorX = GyroErrorX / 200;
GyroErrorY = GyroErrorY / 200;
GyroErrorZ = GyroErrorZ / 200;
// Выводим значения ошибок на Serial Monitor
Serial.print("AccErrorX: ");
Serial.println(AccErrorX);
Serial.print("AccErrorY: ");
Serial.println(AccErrorY);
Serial.print("GyroErrorX: ");
Serial.println(GyroErrorX);
Serial.print("GyroErrorY: ");
Serial.println(GyroErrorY);
Serial.print("GyroErrorZ: ");
Serial.println(GyroErrorZ);
}
Мы просто печатаем значения на последовательном мониторе, и, узнав их, мы можем реализовать их в коде, как показано ранее, как для расчета крена, так и для тангажа, а также для трех выходов гироскопа.
Наконец, с помощью функции Serial.print мы можем распечатать значения Roll, Pitch и Yaw на последовательном мониторе и посмотреть, правильно ли работает датчик.
Постоянно растут значения, за минуту более ста и продолжают расти, как так?
Великолепный вопрос. Лежит плата на столе. Покурил и посмотрел
результаты улетели заоблачно.
Кстати, авторы, когда размещают скетч его хоть проверяют?
У вас акселерометр конфигурируется диапазоном +-8g, а данные обрабатываются как при шкале +-2g, значение делится на 16384.
P.S.: Спасибо за статью! У меня нули приходили, а оказывается в 0x6B надо было 0x00 записать!