Как правильно установить датчик гироскоп на робота

Работа с 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 можно общаться напрямую при помощи встроенной библиотеки Wire.h и даташита, а можно взять очень мощную библиотеку от i2cdev: официальный сайт (на момент написания не работает) или GitHub. Для работы понадобится сама библиотека MPU6050 и библиотека I2Cdev (удобного способа скачать там нет, поэтому выложил архив с обеими на Яндекс.Диск). Мы будем работать с библиотекой, но я оставлю пример опроса всех ускорений и угловых скоростей напрямую, т.к. библиотека тяжёлая, а пример очень лёгкий.

Опрос вручную по i2c

#include "Wire.h" const int MPU_addr = 0x68; // адрес датчика // массив данных // [accX, accY, accZ, temp, gyrX, gyrY, gyrZ] // acc - ускорение, gyr - угловая скорость, temp - температура (raw) int16_t data[7]; void setup() < // инициализация Wire.begin(); Wire.beginTransmission(MPU_addr); Wire.write(0x6B); // PWR_MGMT_1 register Wire.write(0); // set to zero (wakes up the MPU-6050) Wire.endTransmission(true); Serial.begin(9600); >void loop() < getData(); // получаем // выводим for (byte i = 0; i < 7; i++) < Serial.print(data[i]); Serial.print('\t'); >Serial.println(); delay(200); > void getData() < Wire.beginTransmission(MPU_addr); Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) Wire.endTransmission(false); Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers for (byte i = 0; i < 7; i++) < data[i] = Wire.read() >

Опрос через библиотеку i2cdev
#include «MPU6050.h» MPU6050 mpu; int16_t ax, ay, az; int16_t gx, gy, gz; void setup() < Wire.begin(); Serial.begin(9600); mpu.initialize(); // состояние соединения Serial.println(mpu.testConnection() ? "MPU6050 OK" : "MPU6050 FAIL"); delay(1000); >void loop()

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

Эта библиотека является одной из самых сложных в Ардуино-среде, да и сам датчик чего стоит – 50 страниц даташита + 50 страниц с описанием регистров и настроек… А в библиотеке – несколько сотен (!) функций для работы с модулем и его многочисленными настройками. Рассмотрим самые “нужные”, которые точно пригодятся в работе:

Методы для MPU6050

// ============== ГИРОСКОП ============== // получить угловые скорости по осям int16_t getRotationX(); int16_t getRotationY(); int16_t getRotationZ(); void getRotation(int16_t* x, int16_t* y, int16_t* z); // получить диапазон гироскопа // 0 = +/- 250 degrees/sec // 1 = +/- 500 degrees/sec // 2 = +/- 1000 degrees/sec // 3 = +/- 2000 degrees/sec uint8_t getFullScaleGyroRange(); // установить диапазон гироскопа в формате // MPU6050_GYRO_FS_250 // MPU6050_GYRO_FS_500 // MPU6050_GYRO_FS_1000 // MPU6050_GYRO_FS_2000 void setFullScaleGyroRange(uint8_t range); // ============== АКСЕЛЕРОМЕТР ============== // получить ускорения по осям int16_t getAccelerationX(); int16_t getAccelerationY(); int16_t getAccelerationZ(); void getAcceleration(int16_t* x, int16_t* y, int16_t* z); // получить диапазон акселя // 0 = +/- 2g // 1 = +/- 4g // 2 = +/- 8g // 3 = +/- 16g uint8_t getFullScaleAccelRange(); // установить диапазон акселя в формате // MPU6050_ACCEL_FS_2 // MPU6050_ACCEL_FS_4 // MPU6050_ACCEL_FS_8 // MPU6050_ACCEL_FS_16 void setFullScaleAccelRange(uint8_t range); // ============== ВСЯКОЕ ============== // получить ускорение, угловую скорость void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz); // получить температуру int16_t getTemperature(); // перезагрузить void reset(); // управление режимом сна bool getSleepEnabled(); void setSleepEnabled(bool enabled); // ============== ПРЕРЫВАНИЕ СВОБОДНОГО ПАДЕНИЯ ============== // порог датчика свободного падения 0..255. Одна единица - 2mg uint8_t getFreefallDetectionThreshold(); // получить порог void setFreefallDetectionThreshold(uint8_t threshold); // установить порог // время определения свободного падения 0..255. Одна единица - 1мс uint8_t getFreefallDetectionDuration(); // получить время void setFreefallDetectionDuration(uint8_t duration); // установить время // прерывание bool getIntFreefallEnabled(); void setIntFreefallEnabled(bool enabled); // ============== ПРЕРЫВАНИЕ ДВИЖЕНИЯ ============== // порог датчика движения 0..255. Одна единица - 2mg uint8_t getMotionDetectionThreshold(); // получить порог void setMotionDetectionThreshold(uint8_t threshold); // установить порог // время определения движения 0..255. Одна единица - 1мс uint8_t getMotionDetectionDuration(); // получить время void setMotionDetectionDuration(uint8_t duration); // установить время // прерывание bool getIntMotionEnabled(); void setIntMotionEnabled(bool enabled); // ============== ПРЕРЫВАНИЕ ОТСУТСТВИЯ ДВИЖЕНИЯ ============== // порог определения отсутствия движения 0..255. Одна единица - 2mg uint8_t getZeroMotionDetectionThreshold(); // получить порог void setZeroMotionDetectionThreshold(uint8_t threshold);// установить порог // время определения отсутствия движения 0..255. Одна единица - 1мс uint8_t getZeroMotionDetectionDuration(); // получить время void setZeroMotionDetectionDuration(uint8_t duration); // установить время // прерывание bool getIntZeroMotionEnabled(); void setIntZeroMotionEnabled(bool enabled); // ============== ПРЕРЫВАНИЯ ============== // включить/выключить прерывания bool getInterruptMode(); void setInterruptMode(bool mode); // байт со всеми прерываниями. Настройка прерываний uint8_t getIntEnabled(); void setIntEnabled(uint8_t enabled); // статусы прерываний uint8_t getIntStatus(); bool getIntFreefallStatus(); bool getIntMotionStatus(); bool getIntZeroMotionStatus(); // ============== ОФФСЕТЫ ============== // установить. XYZ аксель, XYZ гиро void setXAccelOffset(int16_t offset); void setYAccelOffset(int16_t offset); void setZAccelOffset(int16_t offset); void setXGyroOffset(int16_t offset); void setYGyroOffset(int16_t offset); void setZGyroOffset(int16_t offset); // получить. XYZ аксель, XYZ гиро int16_t getXAccelOffset(); int16_t getYAccelOffset(); int16_t getZAccelOffset(); int16_t getXGyroOffset(); int16_t getYGyroOffset(); int16_t getZGyroOffset(); // вывести оффсеты в порт void PrintActiveOffsets(); void CalibrateGyro(uint8_t Loops = 15); // калибровать гиро(кол-во итераций) void CalibrateAccel(uint8_t Loops = 15); // калибровать аксель(кол-во итераций)

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

По каждой оси и параметру датчик выдаёт 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 град/с

Ну и пример
void setup()

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

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

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

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

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

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

Калибровка из примера библиотеки

// положи датчик горизонтально, надписями вверх // ДАЖЕ НЕ ДЫШИ НА НЕГО // отправь любой символ в сериал, чтобы начать калибровку // жди результатов // --------------------- НАСТРОЙКИ ---------------------- const int buffersize = 70; // количество итераций калибровки const int acel_deadzone = 10; // точность калибровки акселерометра (по умолчанию 8) const int gyro_deadzone = 6; // точность калибровки гироскопа (по умолчанию 2) // --------------------- НАСТРОЙКИ ---------------------- #include "I2Cdev.h" #include "MPU6050.h" #include "Wire.h" MPU6050 mpu(0x68); int16_t ax, ay, az, gx, gy, gz; int mean_ax, mean_ay, mean_az, mean_gx, mean_gy, mean_gz, state = 0; int ax_offset, ay_offset, az_offset, gx_offset, gy_offset, gz_offset; /////////////////////////////////// SETUP //////////////////////////////////// void setup() < Wire.begin(); Serial.begin(9600); mpu.initialize(); // ждём очистки сериал while (Serial.available() && Serial.read()); // чистим while (!Serial.available()) < Serial.println(F("Send any character to start sketch.\n")); delay(1500); >while (Serial.available() && Serial.read()); // чистим ещё на всякий Serial.println("\nMPU6050 Calibration Sketch"); delay(2000); Serial.println("\nYour MPU6050 should be placed in horizontal position, with package letters facing up"); delay(3000); // проверка соединения Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); delay(1000); // сбросить оффсеты mpu.setXAccelOffset(0); mpu.setYAccelOffset(0); mpu.setZAccelOffset(0); mpu.setXGyroOffset(0); mpu.setYGyroOffset(0); mpu.setZGyroOffset(0); > /////////////////////////////////// LOOP //////////////////////////////////// void loop() < if (state == 0) < Serial.println("\nReading sensors for first time. "); meansensors(); state++; delay(1000); >if (state == 1) < Serial.println("\nCalculating offsets. "); calibration(); state++; delay(1000); >if (state == 2) < meansensors(); Serial.println("\nFINISHED!"); Serial.print("\nSensor readings with offsets:\t"); Serial.print(mean_ax); Serial.print("\t"); Serial.print(mean_ay); Serial.print("\t"); Serial.print(mean_az); Serial.print("\t"); Serial.print(mean_gx); Serial.print("\t"); Serial.print(mean_gy); Serial.print("\t"); Serial.println(mean_gz); Serial.print("Your offsets:\t"); Serial.print(ax_offset); Serial.print(", "); Serial.print(ay_offset); Serial.print(", "); Serial.print(az_offset); Serial.print(", "); Serial.print(gx_offset); Serial.print(", "); Serial.print(gy_offset); Serial.print(", "); Serial.println(gz_offset); Serial.println("\nData is printed as: acelX acelY acelZ giroX giroY giroZ"); Serial.println("Check that your sensor readings are close to 0 0 16384 0 0 0"); Serial.println("If calibration was succesful write down your offsets so you can set them in your projects using something similar to mpu.setXAccelOffset(youroffset)"); while (1); >> /////////////////////////////////// FUNCTIONS //////////////////////////////////// void meansensors() < long i = 0, buff_ax = 0, buff_ay = 0, buff_az = 0, buff_gx = 0, buff_gy = 0, buff_gz = 0; while (i < (buffersize + 101)) < // read raw accel/gyro measurements from device mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); if (i >100 && i if (i == (buffersize + 100)) < mean_ax = buff_ax / buffersize; mean_ay = buff_ay / buffersize; mean_az = buff_az / buffersize; mean_gx = buff_gx / buffersize; mean_gy = buff_gy / buffersize; mean_gz = buff_gz / buffersize; >i++; delay(2); > > void calibration() < ax_offset = -mean_ax / 8; ay_offset = -mean_ay / 8; az_offset = (16384 - mean_az) / 8; gx_offset = -mean_gx / 4; gy_offset = -mean_gy / 4; gz_offset = -mean_gz / 4; while (1) < int ready = 0; mpu.setXAccelOffset(ax_offset); mpu.setYAccelOffset(ay_offset); mpu.setZAccelOffset(az_offset); mpu.setXGyroOffset(gx_offset); mpu.setYGyroOffset(gy_offset); mpu.setZGyroOffset(gz_offset); meansensors(); Serial.println(". "); if (abs(mean_ax) >

Мой вариант калибровки

#include "Wire.h" #include "I2Cdev.h" #include "MPU6050.h" MPU6050 mpu; #define BUFFER_SIZE 100 int16_t ax, ay, az; int16_t gx, gy, gz; void setup() < Wire.begin(); Serial.begin(9600); mpu.initialize(); mpu.setXAccelOffset(0); mpu.setYAccelOffset(0); mpu.setZAccelOffset(0); mpu.setXGyroOffset(0); mpu.setYGyroOffset(0); mpu.setZGyroOffset(0); Serial.println(F("Send any character to start sketch")); delay(100); while (1) < //входим в бесконечный цикл if (Serial.available() >0) < //если нажата любая кнопка Serial.read(); //прочитать (чтобы не висел в буфере) break; //выйти из цикла >> delay(1000); > void loop() < // выводим начальные значения mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); Serial.print(ax); Serial.print(" "); Serial.print(ay); Serial.print(" "); Serial.print(az); Serial.print(" "); Serial.print(gx); Serial.print(" "); Serial.print(gy); Serial.print(" "); Serial.println(gz); calibration(); // выводим значения после калибровки mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); Serial.print(ax); Serial.print(" "); Serial.print(ay); Serial.print(" "); Serial.print(az); Serial.print(" "); Serial.print(gx); Serial.print(" "); Serial.print(gy); Serial.print(" "); Serial.println(gz); delay(20); while (1); >// ======= ФУНКЦИЯ КАЛИБРОВКИ ======= void calibration() < long offsets[6]; long offsetsOld[6]; int16_t mpuGet[6]; // используем стандартную точность mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2); mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250); // обнуляем оффсеты mpu.setXAccelOffset(0); mpu.setYAccelOffset(0); mpu.setZAccelOffset(0); mpu.setXGyroOffset(0); mpu.setYGyroOffset(0); mpu.setZGyroOffset(0); delay(10); Serial.println("Calibration start. It will take about 5 seconds"); for (byte n = 0; n < 10; n++) < // 10 итераций калибровки for (byte j = 0; j < 6; j++) < // обнуляем калибровочный массив offsets[j] = 0; >for (byte i = 0; i < 100 + BUFFER_SIZE; i++) < // делаем BUFFER_SIZE измерений для усреднения mpu.getMotion6(&mpuGet[0], &mpuGet[1], &mpuGet[2], &mpuGet[3], &mpuGet[4], &mpuGet[5]); if (i >= 99) < // пропускаем первые 99 измерений for (byte j = 0; j < 6; j++) < offsets[j] += (long)mpuGet[j]; // записываем в калибровочный массив >> > for (byte i = 0; i < 6; i++) < offsets[i] = offsetsOld[i] - ((long)offsets[i] / BUFFER_SIZE); // учитываем предыдущую калибровку if (i == 2) offsets[i] += 16384; // если ось Z, калибруем в 16384 offsetsOld[i] = offsets[i]; >// ставим новые оффсеты mpu.setXAccelOffset(offsets[0] / 8); mpu.setYAccelOffset(offsets[1] / 8); mpu.setZAccelOffset(offsets[2] / 8); mpu.setXGyroOffset(offsets[3] / 4); mpu.setYGyroOffset(offsets[4] / 4); mpu.setZGyroOffset(offsets[5] / 4); delay(2); > /* // выводим в порт Serial.println("Calibration end. Your offsets:"); Serial.println("accX accY accZ gyrX gyrY gyrZ"); Serial.print(mpu.getXAccelOffset()); Serial.print(", "); Serial.print(mpu.getYAccelOffset()); Serial.print(", "); Serial.print(mpu.getZAccelOffset()); Serial.print(", "); Serial.print(mpu.getXGyroOffset()); Serial.print(", "); Serial.print(mpu.getYGyroOffset()); Serial.print(", "); Serial.print(mpu.getZGyroOffset()); Serial.println(" "); Serial.println(" "); */ >

Мой вариант калибровки + запись в EEPROM

// калибровка и запись в EEPROM // при запуске настраиваем оффсеты // послать символ в сериал - начать повторную калибровку #include "Wire.h" #include "I2Cdev.h" #include "MPU6050.h" MPU6050 mpu; #define BUFFER_SIZE 100 #define START_BYTE 1010 int16_t ax, ay, az; int16_t gx, gy, gz; void setup() < Wire.begin(); Serial.begin(9600); mpu.initialize(); //----------------- ВСПОМИНАЕМ ОФФСЕТЫ ------------------------ int offsets[6]; EEPROM.get(START_BYTE, offsets); // ставим оффсеты из памяти mpu.setXAccelOffset(offsets[0]); mpu.setYAccelOffset(offsets[1]); mpu.setZAccelOffset(offsets[2]); mpu.setXGyroOffset(offsets[3]); mpu.setYGyroOffset(offsets[4]); mpu.setZGyroOffset(offsets[5]); //----------------- ВСПОМИНАЕМ ОФФСЕТЫ ------------------------ Serial.println(F("Send any character to start sketch")); delay(100); while (1) < //входим в бесконечный цикл if (Serial.available() >0) < // если нажата любая кнопка Serial.read(); // прочитать (чтобы не висел в буфере) break; // выйти из цикла >> delay(1000); > void loop() < calibration(); Serial.println("Current offsets:"); Serial.println("accX accY accZ gyrX gyrY gyrZ"); Serial.print(mpu.getXAccelOffset()); Serial.print(", "); Serial.print(mpu.getYAccelOffset()); Serial.print(", "); Serial.print(mpu.getZAccelOffset()); Serial.print(", "); Serial.print(mpu.getXGyroOffset()); Serial.print(", "); Serial.print(mpu.getYGyroOffset()); Serial.print(", "); Serial.print(mpu.getZGyroOffset()); Serial.println(" "); Serial.println(" "); // выводим значения после калибровки Serial.println("Readings:"); mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); Serial.print(ax); Serial.print(" "); Serial.print(ay); Serial.print(" "); Serial.print(az); Serial.print(" "); Serial.print(gx); Serial.print(" "); Serial.print(gy); Serial.print(" "); Serial.println(gz); delay(20); while (1); >// ======= ФУНКЦИЯ КАЛИБРОВКИ И ЗАПИСИ В ЕЕПРОМ ======= void calibration() < long offsets[6]; long offsetsOld[6]; int16_t mpuGet[6]; // используем стандартную точность mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2); mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250); // обнуляем оффсеты mpu.setXAccelOffset(0); mpu.setYAccelOffset(0); mpu.setZAccelOffset(0); mpu.setXGyroOffset(0); mpu.setYGyroOffset(0); mpu.setZGyroOffset(0); delay(5); for (byte n = 0; n < 10; n++) < // 10 итераций калибровки for (byte j = 0; j < 6; j++) < // обнуляем калибровочный массив offsets[j] = 0; >for (byte i = 0; i < 100 + BUFFER_SIZE; i++) < // делаем BUFFER_SIZE измерений для усреднения mpu.getMotion6(&mpuGet[0], &mpuGet[1], &mpuGet[2], &mpuGet[3], &mpuGet[4], &mpuGet[5]); if (i >= 99) < // пропускаем первые 99 измерений for (byte j = 0; j < 6; j++) < offsets[j] += (long)mpuGet[j]; // записываем в калибровочный массив >> > for (byte i = 0; i < 6; i++) < offsets[i] = offsetsOld[i] - ((long)offsets[i] / BUFFER_SIZE); // учитываем предыдущую калибровку if (i == 2) offsets[i] += 16384; // если ось Z, калибруем в 16384 offsetsOld[i] = offsets[i]; >// ставим новые оффсеты mpu.setXAccelOffset(offsets[0] / 8); mpu.setYAccelOffset(offsets[1] / 8); mpu.setZAccelOffset(offsets[2] / 8); mpu.setXGyroOffset(offsets[3] / 4); mpu.setYGyroOffset(offsets[4] / 4); mpu.setZGyroOffset(offsets[5] / 4); delay(2); > // пересчитываем для хранения for (byte i = 0; i < 6; i++) < if (i < 3) offsets[i] /= 8; else offsets[i] /= 4; >// запись в память EEPROM.put(START_BYTE, offsets); >

Мой алгоритм калибровки

// ======= ФУНКЦИЯ КАЛИБРОВКИ ======= void calibration() < long offsets[6]; long offsetsOld[6]; int16_t mpuGet[6]; // используем стандартную точность mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2); mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250); // обнуляем оффсеты mpu.setXAccelOffset(0); mpu.setYAccelOffset(0); mpu.setZAccelOffset(0); mpu.setXGyroOffset(0); mpu.setYGyroOffset(0); mpu.setZGyroOffset(0); delay(10); for (byte n = 0; n < 10; n++) < // 10 итераций калибровки for (byte j = 0; j < 6; j++) < // обнуляем калибровочный массив offsets[j] = 0; >for (byte i = 0; i < 100 + BUFFER_SIZE; i++) < // делаем BUFFER_SIZE измерений для усреднения mpu.getMotion6(&mpuGet[0], &mpuGet[1], &mpuGet[2], &mpuGet[3], &mpuGet[4], &mpuGet[5]); // пропускаем первые 99 измерений if (i >= 99) < for (byte j = 0; j < 6; j++) < offsets[j] += (long)mpuGet[j]; // записываем в калибровочный массив >> > for (byte i = 0; i < 6; i++) < offsets[i] = offsetsOld[i] - ((long)offsets[i] / BUFFER_SIZE); // учитываем предыдущую калибровку if (i == 2) offsets[i] += 16384; // если ось Z, калибруем в 16384 offsetsOld[i] = offsets[i]; >// ставим новые оффсеты mpu.setXAccelOffset(offsets[0] / 8); mpu.setYAccelOffset(offsets[1] / 8); mpu.setZAccelOffset(offsets[2] / 8); mpu.setXGyroOffset(offsets[3] / 4); mpu.setYGyroOffset(offsets[4] / 4); mpu.setZGyroOffset(offsets[5] / 4); delay(2); > >

Также в библиотеке есть готовые функции для калибровки акселя и гиро, они принимают количество итераций калибровки (до 15):

mpu.CalibrateAccel(6); mpu.CalibrateGyro(6);

Функции блокирующие, выполняются и автоматически выставляют оффсеты, т.е. датчик сразу калибруется. Для чтения оффсетов (например для записи в EEPROM) можно воспользоваться тем же способом, что и раньше:

mpu.getXAccelOffset(); mpu.getYAccelOffset(); mpu.getZAccelOffset(); mpu.getXGyroOffset(); mpu.getYGyroOffset(); mpu.getZGyroOffset();

Получение углов

Самый простой способ – получение углов чисто из ускорения через арксинус/арккосинус. Этот способ плох тем, что даёт угол в диапазоне +-90 градусов, так как имеет место быть gimbal lock.

Углы через тригонометрию
#include «MPU6050.h» MPU6050 mpu; void setup() < Wire.begin(); Serial.begin(9600); mpu.initialize(); // запускаем датчик >void loop() < int16_t ax = mpu.getAccelerationX(); // ускорение по оси Х // стандартный диапазон: +-2g ax = constrain(ax, -16384, 16384); // ограничиваем +-1g float angle = ax / 16384.0; // переводим в +-1.0 // и в угол через арксинус if (angle

Также акселерометр шумит и показания плавают, не говоря о минимальном разрешении +-2g и резком возникновении ошибки при ускорении модуля вдоль измеряемой оси, то есть угол покоящегося модуля он покажет, но стоит начать его двигать – угол изменится. Для точного определения угла использую показания ускорения в связке с угловой скоростью.

Для получения углов из показаний акселерометра и гироскопа есть три основных способа:

Фильтрация при помощи фильтра Калмана.
Фильтрация при помощи комплементарного фильтра.
Определение углов при помощи встроенного в датчик DMP (Digital Motion Processor).

Из всех трёх самый хороший результат даёт встроенный процессор, на его фоне Калман и комплементари можно даже не рассматривать, они практически не работают. Пример рассмотрим в следующей главе ниже.

Ниже привожу совмещённый пример с упрощённым комплементари, обычным и Калманом, всё в одном скетче. Но повторюсь, по сравнению с DMP оно вообще не то показывает =)

Скетч с фильтрами

#define COMPL_K 0.07 #include "Wire.h" #include "Kalman.h" Kalman kalmanX; Kalman kalmanY; uint8_t IMUAddress = 0x68; /* IMU Data */ int16_t accX; int16_t accY; int16_t accZ; int16_t tempRaw; int16_t gyroX; int16_t gyroY; int16_t gyroZ; float accXangle; // Angle calculate using the accelerometer float accYangle; float temp; float gyroXangle = 180; // Angle calculate using the gyro float gyroYangle = 180; float compAngleX = 180; // Calculate the angle using a Kalman filter float compAngleY = 180; float kalAngleX; // Calculate the angle using a Kalman filter float kalAngleY; uint32_t timer; void setup() < Serial.begin(9600); Wire.begin(); Wire.beginTransmission(IMUAddress); Wire.write(0x6B); // PWR_MGMT_1 register Wire.write(0); // set to zero (wakes up the MPU-6050) Wire.endTransmission(true); kalmanX.setAngle(180); // Set starting angle kalmanY.setAngle(180); timer = micros(); >void loop() < //uint32_t looptime = micros(); getValues(); calculateAngles(); //Serial.println(micros() - looptime); Serial.print(kalAngleX); Serial.print(" "); Serial.print(kalAngleY); Serial.println(); /* Serial.print(compAngleX); Serial.print(" "); Serial.print(compAngleY); Serial.println(); */ delay(20); // The accelerometer's maximum samples rate is 1kHz >void getValues() < /* Update all the values */ Wire.beginTransmission(IMUAddress); Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) Wire.endTransmission(false); Wire.requestFrom(IMUAddress, 14, true); // request a total of 14 registers accX = Wire.read() void calculateAngles() < /* Calculate the angls based on the different sensors and algorithm */ accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG; accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG; float gyroXrate = (float)gyroX / 131.0; float gyroYrate = -((float)gyroY / 131.0); // Calculate gyro angle without any filter gyroXangle += gyroXrate * ((float)(micros() - timer) / 1000000); gyroYangle += gyroYrate * ((float)(micros() - timer) / 1000000); /* // Calculate gyro angle using the unbiased rate gyroXangle += kalmanX.getRate() * ((float)(micros() - timer) / 1000000); gyroYangle += kalmanY.getRate() * ((float)(micros() - timer) / 1000000); */ /* // Calculate the angle using a Complimentary filter compAngleX = ((float)(1 - COMPL_K) * (compAngleX + (gyroXrate * (float)(micros() - timer) / 1000000))) + (COMPL_K * accXangle); compAngleY = ((float)(1 - COMPL_K) * (compAngleY + (gyroYrate * (float)(micros() - timer) / 1000000))) + (COMPL_K * accYangle); */ // Calculate the angle using a Kalman filter kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (float)(micros() - timer) / 1000000); kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (float)(micros() - timer) / 1000000); timer = micros(); >

Библиотека Kalman.h, положи рядом со скетчем

/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 ("Copyleft"). Contact information ------------------- Kristian Lauszus, TKJ Electronics Web : http://www.tkjelectronics.com e-mail : [email protected] */ #ifndef _Kalman_h #define _Kalman_h class Kalman < public: Kalman() < /* We will set the varibles like so, these can also be tuned by the user */ Q_angle = 0.001; Q_bias = 0.003; R_measure = 0.03; bias = 0; // Reset bias P[0][0] = 0; // Since we assume tha the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical P[0][1] = 0; P[1][0] = 0; P[1][1] = 0; >; // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds double getAngle(double newAngle, double newRate, double dt) < // KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145 // Modified by Kristian Lauszus // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it // Discrete Kalman filter time update equations - Time Update ("Predict") // Update xhat - Project the state ahead /* Step 1 */ rate = newRate - bias; angle += dt * rate; // Update estimation error covariance - Project the error covariance ahead /* Step 2 */ P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle); P[0][1] -= dt * P[1][1]; P[1][0] -= dt * P[1][1]; P[1][1] += Q_bias * dt; // Discrete Kalman filter measurement update equations - Measurement Update ("Correct") // Calculate Kalman gain - Compute the Kalman gain /* Step 4 */ S = P[0][0] + R_measure; /* Step 5 */ K[0] = P[0][0] / S; K[1] = P[1][0] / S; // Calculate angle and bias - Update estimate with measurement zk (newAngle) /* Step 3 */ y = newAngle - angle; /* Step 6 */ angle += K[0] * y; bias += K[1] * y; // Calculate estimation error covariance - Update the error covariance /* Step 7 */ P[0][0] -= K[0] * P[0][0]; P[0][1] -= K[0] * P[0][1]; P[1][0] -= K[1] * P[0][0]; P[1][1] -= K[1] * P[0][1]; return angle; >; void setAngle(double newAngle) < angle = newAngle; >; // Used to set angle, this should be set as the starting angle double getRate() < return rate; >; // Return the unbiased rate /* These are used to tune the Kalman filter */ void setQangle(double newQ_angle) < Q_angle = newQ_angle; >; void setQbias(double newQ_bias) < Q_bias = newQ_bias; >; void setRmeasure(double newR_measure) < R_measure = newR_measure; >; private: /* Kalman filter variables */ double Q_angle; // Process noise variance for the accelerometer double Q_bias; // Process noise variance for the gyro bias double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise double angle; // The angle calculated by the Kalman filter - part of the 2x1 state matrix double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state matrix double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate double P[2][2]; // Error covariance matrix - This is a 2x2 matrix double K[2]; // Kalman gain - This is a 2x1 matrix double y; // Angle difference - 1x1 matrix double S; // Estimate error - 1x1 matrix >; #endif

Большую кучу теории про Эйлера и кватернионы можно почитать вот здесь.

Оптимальное получение углов

Пример получения углов при помощи DMP и встроенной библиотеки MPU6050_6Axis_MotionApps20.h. Единственное, скетч занимает прилично памяти (11кб Flash и 500б SRAM), так как фактически содержит прошивку для DMP и загружает её в модуль при каждом старте программы. С модуля можно подключить пин INT на один из пинов прерывания МК, например на D2 в случае с Arduino Nano (номер прерывания 0). DMP будет дёргать прерывание, сообщая о готовности, чтобы мы не тратили лишнее время на его постоянный опрос. Но опрашивать то можно и по таймеру (второй пример).

Получаем углы с DMP по прерыванию (обновил скетч!)

#include "I2Cdev.h" #include "MPU6050_6Axis_MotionApps20.h" MPU6050 mpu; volatile bool mpuFlag = false; // флаг прерывания готовности uint8_t fifoBuffer[45]; // буфер void setup() < Serial.begin(115200); Wire.begin(); //Wire.setClock(1000000UL); // разгоняем шину на максимум // инициализация DMP и прерывания mpu.initialize(); mpu.dmpInitialize(); mpu.setDMPEnabled(true); attachInterrupt(0, dmpReady, RISING); >// прерывание готовности. Поднимаем флаг void dmpReady() < mpuFlag = true; >void loop() < // по флагу прерывания и готовности DMP if (mpuFlag && mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) < // переменные для расчёта (ypr можно вынести в глобал) Quaternion q; VectorFloat gravity; float ypr[3]; // расчёты mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); mpuFlag = false; // выводим результат в радианах (-3.14, 3.14) Serial.print(ypr[0]); // вокруг оси Z Serial.print(','); Serial.print(ypr[1]); // вокруг оси Y Serial.print(','); Serial.print(ypr[2]); // вокруг оси X Serial.println(); // для градусов можно использовать degrees() >>

Без прерываний, но с таймером

#include "I2Cdev.h" #include "MPU6050_6Axis_MotionApps20.h" MPU6050 mpu; uint8_t fifoBuffer[45]; // буфер void setup() < Serial.begin(115200); Wire.begin(); //Wire.setClock(1000000UL); // разгоняем шину на максимум // инициализация DMP mpu.initialize(); mpu.dmpInitialize(); mpu.setDMPEnabled(true); >void loop() < static uint32_t tmr; if (millis() - tmr >= 11) < // таймер на 11 мс (на всякий случай) if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) < // переменные для расчёта (ypr можно вынести в глобал) Quaternion q; VectorFloat gravity; float ypr[3]; // расчёты mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); // выводим результат в радианах (-3.14, 3.14) Serial.print(ypr[0]); // вокруг оси Z Serial.print(','); Serial.print(ypr[1]); // вокруг оси Y Serial.print(','); Serial.print(ypr[2]); // вокруг оси X Serial.println(); // для градусов можно использовать degrees() tmr = millis(); // сброс таймера >> >

По производительности дела обстоят так: без прерываний нам приходится проверять готовность DMP, что занимает 146 мкс, а сам DMP готов отдавать данные через каждые 10 мс (9.1 + 0.8). Обработка данных занимает 2 мс, из которых половина – обработка буфера (функция dmpGetCurrentFIFOPacket). Тестовый код для отладки через анализатор выглядел так:

void loop() < fastWrite(13, 1); if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) < fastWrite(12, 1); mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); fastWrite(12, 1); >fastWrite(13, 0); >

И получились вот такие тайминги: В реальном примере с таймером мы не тратим 146 мкс в цикле, а просто асинхронно ждём 10 (а лучше 11) миллисекунд. Работа с прерыванием или с таймером будет выглядеть более красиво: Если вы вдруг ничего не поняли: оба примера можно смело использовать, прерывание – не панацея, если в программе есть возможность считать время и поднять программный таймер, как во втором примере. Но с прерыванием можно безо всяких таймеров точно поймать момент, когда DMP обработал данные. Если это вдруг нужно.

Некоторые примеры

Определение периода тряски вдоль оси X
Скетч временно не работает, надеюсь починю

// скетч определения периода тряски акселерометра #define DEADZONE 4000 #define MAX_LOOP 900 #include "Wire.h" #include "I2Cdev.h" #include "MPU6050.h" MPU6050 mpu; float k = 0.1; int16_t ax, ay, az, ax_f; int16_t gx, gy, gz; int16_t loopTime; boolean toggle_flag; unsigned long maxTimer, analyzeTimer; void setup() < Wire.begin(); Serial.begin(9600); mpu.initialize(); Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); // Acceleration range: ± 2 ± 4 ± 8 ± 16 g // 1G value: 16384 8192 4096 2048 // MAX G value: 32768 mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_8); // Gyroscope range: 250 500 1000 2000 °/s // MAX value: 32768 mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250); pinMode(13, OUTPUT); >void loop() < if (millis() - analyzeTimer >= 2) < // шаг анализа периода mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // запросить данные ax_f += (ax - ax_f) * 0.1; // отфильтровать if (ax_f >DEADZONE && toggle_flag) < toggle_flag = false; digitalWrite(13, 1); loopTime = millis() - maxTimer; // расчёт периода движения maxTimer = millis(); >else if (ax_f < -DEADZONE && !toggle_flag) < toggle_flag = true; digitalWrite(13, 0); loopTime = millis() - maxTimer; // расчёт периода движения maxTimer = millis(); >if (millis() - maxTimer > MAX_LOOP) loopTime = 0; > > Serial.println(loopTime); analyzeTimer = millis(); >

Определение периода вращения (ось Y по радиусу)

// скетч определения периода вращения акселерометра #define DEADZONE 700 #define MIN_LOOP 100 #define MAX_LOOP 900 #include "Wire.h" #include "I2Cdev.h" #include "MPU6050.h" MPU6050 mpu; float k = 0.2; int16_t ax, ay, az, ay_f; int16_t gx, gy, gz; int last_acc, level; boolean rising_flag = true, falling_flag = false, toggle_flag; unsigned long maxTimer, analyzeTimer; int loopTime; void setup() < Wire.begin(); Serial.begin(9600); mpu.initialize(); Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); // Acceleration range: ± 2 ± 4 ± 8 ± 16 g // 1G value: 16384 8192 4096 2048 // MAX G value: 32768 mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_16); // Gyroscope range: 250 500 1000 2000 °/s // MAX value: 32768 mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250); >void loop() < if (millis() - analyzeTimer >3) < // шаг анализа периода mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // запросить данные ay_f += (ay - ay_f) * k; // отфильтровать if (rising_flag) // поднимаемся // ждём падения сигнала на величину DEADZONE if (last_acc < ay_f + DEADZONE) < // если сигнал не упал, принять величину как точку для следующего сравнения last_acc = ay_f; toggle_flag = true; >else < // если есть падение, и прошёл минимальный период // (защита от всяких случайных ударов и дёрганий) if (toggle_flag && (millis() - maxTimer >MIN_LOOP)) < toggle_flag = false; rising_flag = false; falling_flag = true; loopTime = millis() - maxTimer; // расчёт периода движения maxTimer = millis(); //level = last_acc; >> if (falling_flag) < // опускается // ждём прироста сигнала на величину DEADZONE if (last_acc >ay_f - DEADZONE) < // падаем // если сигнал не вырос, принять величину как точку для следующего сравнения last_acc = ay_f; toggle_flag = true; >else < // если есть прирост, и прошёл минимальный период // (защита от всяких случайных ударов и дёрганий) if (toggle_flag && (millis() - maxTimer >MIN_LOOP)) < toggle_flag = false; rising_flag = true; falling_flag = false; //level = last_acc; >> > /* Serial.print("$"); Serial.print(ay_f); Serial.print(" "); Serial.print(level); Serial.print(" "); Serial.print(loopTime*10); Serial.println(";"); */ if (millis() - maxTimer > MAX_LOOP) loopTime = 0; Serial.println(loopTime); analyzeTimer = millis(); > >

Сигнализация, высокоточное определение вибрации

// простая сигнализация // датчик реагирует даже на постукивание // пальцем на противоположном конце стола #include "Wire.h" #include "I2Cdev.h" #include "MPU6050.h" MPU6050 mpu; int16_t ax, ay, az; int16_t gx, gy, gz; long ACC, GYR; long maxACC, maxGYR, trACC, trGYR; void setup() < Wire.begin(); Serial.begin(9600); mpu.initialize(); mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2); delay(2000); // "калибровка" максимальных значений for (int i = 0; i < 30; i++) < mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); ACC = abs(ax) + abs(ay) + abs(az); GYR = abs(gx) + abs(gy) + abs(gz); if (ACC >maxACC) maxACC = ACC; if (GYR > maxGYR) maxGYR = GYR; delay(5); > maxACC = maxACC + trACC; maxGYR = maxGYR + trGYR; > void loop() < mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // сумма модулей ACC = abs(ax) + abs(ay) + abs(az); GYR = abs(gx) + abs(gy) + abs(gz); if (ACC >maxACC || GYR > maxGYR) < Serial.println("KEK!"); >delay(10); >

Прерывание по движению (NEW!)

Настраиваем датчик на выдачу прерываний по движению (например для сигнализации). Таким же образом можно настроить на прерывания по свободному падению, см. доку выше.

#include "MPU6050.h" MPU6050 mpu; void setup() < // запускаем всё Wire.begin(); Serial.begin(9600); mpu.initialize(); mpu.setInterruptMode(true); // включаем режим прерываний mpu.setIntMotionEnabled(true); // включаем прерывания движения mpu.setMotionDetectionThreshold(2); // порог 0..255 mpu.setMotionDetectionDuration(2); // таймаут 0..255 attachInterrupt(0, isr, RISING); // прерывание >volatile bool motion; void isr() < motion = true; // флаг прерывания >void loop() < // по флагу прерывания пишем в порт // прерыванием можно будить МК и так далее if (motion) < Serial.println("Motion!"); motion = false; >>

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

Ардуино: акселерометр MPU6050

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

На этот раз мы перейдем от теории к практике: подключим датчик к Ардуино, и напишем пару программ для работы с ним. Подключать будем модуль MPU6050 от RobotClass.

Акселерометр и гироскоп MPU6050 от RobotClass - ROC

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

Характеристики модуля MPU6050 ROC:

напряжение питания: от 3,5 до 6 В;
потребляемый ток: 500 мкА;
ток в режиме пониженного потребления: 10 мкА при 1,25 Гц, 20 мкА при 5 Гц, 60 мкА при 20 Гц, 110 мкА при 40 Гц;
диапазон: ± 2, 4, 8, 16g;
разрядность АЦП: 16;
интерфейс: I2C (до 400 кГц).

Список необходимых компонентов

Для выполнения всех экспериментов в данном уроке, кроме самого датчика MPU6050, потребуются: Ардуино-совместимый контроллер и немного проводов вилка-розетка. Необходимые компоненты можно добавить в корзину прямо здесь, и затем оформить заказ в нашем интернет-магазине.

Подключение MPU6050 к Ардуино

На плате имеется восемь контактов:

VCC — положительный контакт питания;
GND — земля;
SDA — линия данных I2C;
SCL — линия синхроимпульсов I2C;
INT — настраиваемое прерывание;
AD0 — I2C адрес; по-умолчанию AD0 подтянут к земле, поэтому адрес устройства — 0x68; если соединить AD0 к контактом питания, то адрес изменится на 0x69;
XCL, XDA — дополнительный I2C интерфейс для подключения внешнего магнитометра.

Соединим контакты датчика с Arduino Uno согласно стандартной схеме для интерфейса I2C:

MPU6050 ROC	GND	VCC	SDA	SCL
Ардуино Уно	GND	+5V	A4	A5

Принципиальная схема

Схема подключения MPU6050 к Ардуино

Внешний вид макета

Схема подключения MPU6050 к Ардуино

Программа для получения сырых данных с акселерометра MPU6050

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

#include "I2Cdev.h" #include "MPU6050.h" #define T_OUT 20 MPU6050 accel; unsigned long int t_next; void setup() < Serial.begin(9600); accel.initialize(); Serial.println(accel.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); >void loop() < long int t = millis(); if( t_next < t )< int16_t ax_raw, ay_raw, az_raw, gx_raw, gy_raw, gz_raw; t_next = t + T_OUT; accel.getMotion6(&ax_raw, &ay_raw, &az_raw, &gx_raw, &gy_raw, &gz_raw); Serial.println(ay_raw); // вывод в порт проекции ускорения на ось Y >>

Для работы программы потребуются библиотеки: MPU6050 и I2Cdev, ссылки на которые можно найти в конце урока.

Загружаем программу на Ардуино и открываем окно графика. Поворачиваем датчик вокруг оси X на 90 градусов в одну сторону, потом на 90 в другую. Получится примерно такая картина.

Сырые данные с акселерометра MPU6050

На графике хорошо видно, что при наклоне оси Y вертикально, акселерометр выдает значения близкие к четырём тысячам. Откуда берется это число?

Точность измерения ускорения в MPU6050

Дело в том, что датчик MPU6050 позволяет настраивать точность измерений. Можно выбрать один из четырех классов точности: ±2G, 4G, 8G и 16G, где 1G — это одна земная гравитация. Используемая нами библиотека по-умолчанию настраивает датчик на диапазон ±8G (прим. по ссылке внизу статьи библиотека по-умолчанию устанавливает ±2G).

С другой стороны, MPU6050 имеет 16 разрядный АЦП. 2 в степени 16 даст нам число 65 536. Поскольку датчик может измерять и отрицательное и положительное ускорение, то он будет выдавать нам числа от -32768 до +32768.

Сложив эти два факта вместе получаем, что при таких настройках 1G будет равен числу 4096 (ну а -1G равен числу -4096). Это вполне совпадает с наблюдаемыми на графике значениями!

Следующий шаг — преобразование этих странных чисел в привычные нам углы, измеряемые в градусах.

Программа для вычисления угла наклона акселерометра MPU6050

Добавим в предыдущую программу вычисление угла поворота датчика вокруг оси X:

#include "I2Cdev.h" #include "MPU6050.h" #define TO_DEG 57.29577951308232087679815481410517033f #define T_OUT 20 MPU6050 accel; float angle_ax; long int t_next; float clamp(float v, float minv, float maxv)< if( v>maxv ) return maxv; else if( v void setup() < Serial.begin(9600); accel.initialize(); // первичная настройка датчика Serial.println(accel.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); >void loop() < long int t = millis(); if( t_next < t )< int16_t ax_raw, ay_raw, az_raw, gx_raw, gy_raw, gz_raw; float ay,gx; t_next = t + T_OUT; accel.getMotion6(&ax_raw, &ay_raw, &az_raw, &gx_raw, &gy_raw, &gz_raw); // сырые данные акселерометра нужно преобразовать в единицы гравитации // при базовых настройках 1G = 4096 ay = ay_raw / 4096.0; // на случай, если на акселерометр действуют посторонние силы, которые могут // увеличить ускорение датчика свыше 1G, установим границы от -1G до +1G ay = clamp(ay, -1.0, 1.0); // функция acos возвращает угол в радианах, так что преобразуем // его в градусы при помощи коэффициента TO_DEG if( ay >= 0) < angle_ax = 90 - TO_DEG*acos(ay); >else < angle_ax = TO_DEG*acos(-ay) - 90; >Serial.println(angle_ax); // вывод в порт угла поворота вокруг оси X > >

Загружаем программу в Ардуино и снова пробуем вращать датчик. Теперь на графике отображается угол наклона в градусах!

График угла наклона, полученного с помощью акселерометра MPU6050

Ну вот, мы получили уже что-то пригодное для дальнейшего использования. Видно, что датчик поворачивался сначала на 30 с лишним градусов в одну сторону, потом примерно на 60 в другую. Работает!

Заключение

На этом уроке мы получили с датчика MPU6050 сначала сырые данные, а потом и угол его наклона в градусах. Это большое достижение. Но впереди еще немного математики и еще более крутые результаты! Будем делать комплементарный фильтр, который позволит работать с датчиком даже в условиях вибрации и тряски.

Несколько идей/заданий:

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

Принципиальная схема датчика

Полезные ссылки

Спецификация производителя MPU6050
Сборник библиотек для устройств I2C
Библиотека MPU6050
Библиотека I2Cdev

Изменено: 5 Янв, 2018 23:06

Ардуино: акселерометр MPU6050 : 33 комментария

Евгений говорит 11.03.2018 в 02:24 :

Подробная статья, чувствуется что автор хорошо владеет материалом.
А не доводилось ли автору использовать MPU-6050 для вычисления траектории? Если такой опыт имеется, то прошу поделиться. Спасибо!

Никита говорит 24.07.2022 в 22:47 :

вычислять траекторию по измеренниям (3 ускорения и 3 угловой скорости) — дохлый номер, поскольку дрейф гироскопа постоянно придется занулять, а сделать это можно только тогда, когда тело будет покоится.
что-то похожее, что вы хотли делали в этом видео https://www.youtube.com/watch?v=6ijArKE8vKU&t=9s&ab_channel=SebMadgwickResearch

Владимир говорит 14.03.2018 в 22:30 :

Всё хорошо, но автору нужно быть корректнее. Если что-то объясняешь, то не должно быть ошибок. Иначе начинающий, попробовав приведённые примеры и не получив положительного результата, может вообще забить на тему.
Загрузив первый пример, тут же получаешь ошибку компиляции. А проблема в ошибке: Объявляется MPU6050 accel;, а дальше применяется accelgyro (должно быть одинаково). Мелочь, конечно, но начинающего может ввести в ступор. Далее объясняется, откуда взялось «4000 тысячам», а фактически значения около 16000. Фразу «открываем окно графика» тоже не сразу понять. Оказывается, в версии 1.8.5 IDE Arduino появилось «Плоттер по последовательному соединению». Дальше пока не разбирался, но хотелось бы начать въежать в тему, а не ребусы разгадывать.

Олег Евсегнеев говорит 16.03.2018 в 00:37 :

Владимир, спасибо за замечания! Код исправили. Насчёт 4000 вместо 16000, то это особенность той версии библиотеки, с которой запускалась программа. В текущей версии действительно стоит другой диапазон — ±2G, поэтому и числа у вас ±16000.
А плоттер хорошая штука, рекомендую всегда иметь под рукой свежую версию IDE, благо она свободно распространяемая!

РАЗРАБОТКА СИСТЕМЫ СТАБИЛИЗАЦИИ УГЛА ОТКЛОНЕНИЯ БАЛАНСИРУЮЩЕГО РОБОТА Текст научной статьи по специальности «Механика и машиностроение»

Аннотация научной статьи по механике и машиностроению, автор научной работы — Жмудь Вадим Аркадьевич, Федоров Дмитрий Сергеевич, Ивойлов Андрей Юрьевич, Трубин Виталий Геннадьевич

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

i Надоели баннеры? Вы всегда можете отключить рекламу.

Текст научной работы на тему «РАЗРАБОТКА СИСТЕМЫ СТАБИЛИЗАЦИИ УГЛА ОТКЛОНЕНИЯ БАЛАНСИРУЮЩЕГО РОБОТА»

Разработка системы стабилизации угла отклонения балансирующего робота

Д.С. Федоров, А.Ю. Ивойлов, В.А. Жмудь, В.Г. Трубин ФГБОУВПО НГТУ, Новосибирск, Россия

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

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

В данной статье описан процесс разработки системы стабилизации двухколесного балансирующего робота. Внешний вид робота представлен на Рис. 1. Данное устройство представляет собой платформу, на которой размещена управляющая электроника робота и датчики. К платформе жестко крепятся статоры двигателей постоянного тока. На роторах же двигателей закреплены колеса. Идея работы устройства состоит в том, чтобы поддерживать вертикальное положение робота путем вращения колес, поддерживая равновесие балансировкой.

Балансирующий робот с математической точки зрения представляет собой перевернутый маятник с точкой подвеса на колесе (Рис. 2) [1]. Это нелинейная неустойчивая система, которая может служить для отработки и сравнения различных алгоритмов управления. Многие неустойчивые системы содержат звенья, представляющие собой перевернутый маятник: транспортное средство Segway, двуногие шагающие механизмы, экзоскелеты. Поэтому результаты, полученные в данной статье, могут иметь широкое практическое применение.

Первичной задачей является задача определения математической модели объекта, а также проверка разрешимости задачи синтеза.

Рис 1. Балансирующий двухколесный робот

Рис 2. Маятник с точкой подвеса на колесе: О — точка подвеса, Мк — момент колеса, у — угол поворота колеса, г — радиус колеса, тк — масса колеса, (хк, ук) — центр масс колеса, ф — угол наклона, (хр, ур) — центр масс робота, I — расстояние от центра масс робота до точки подвеса (центра масс колеса), g — ускорение свободного падения

Неотъемлемой частью работы любой системы автоматического управления является измерение выходной переменной. От точности этого измерения зависит качество регулирования и запас устойчивости. В этой статье рассмотрены особенности работы акселерометра и гироскопа, входящих в измерительную систему МРП6050, а также предложено объединять показания этих двух датчиков с помощью комплементарного

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

Результатом работы является система стабилизации балансирующего робота, рассчитанная модальным методом. Эта система поддерживает состояние робота в вертикальном положении и может отрабатывать внешние возмущения.

1. ПОЛУЧЕНИЕ МАТЕМАТИЧЕСКОЙ МОДЕЛИ ОБЪЕКТА

Перед началом разработки системы стабилизации необходимо выяснить, что представляет собой объект управления с точки зрения математической модели преобразования сигналов. Для построения системы стабилизации необходимо уточнить, какие величины требуется измерять или оценивать, в каком режиме работает система. На основании этих сведений требуется выбрать способ управления, метод расчета регулятора и способ оценки неизвестных величин.

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

где T — кинетическая энергия системы, Qt -обобщенная сила, qi — обобщенные координаты.

В данном случае обобщенными координатами являются угол наклона ф и угол поворота колеса у. Кинетическая энергия системы имеет следующий вид:

где Tk — кинетическая энергия кинетическая энергия робота.

Полная кинетическая энергия каждой из составляющих включает в себя вращательную и поступательную энергии:

2 2 2 2 Р mW + ткг y

Здесь Jk и ^ — моменты инерции колеса и робота соответственно, р — радиус инерции колеса.

Поступательная скорость робота

определяется скоростью его центра масс:

Поскольку ук = 0, координаты центра масс стержня маятника и центра колеса связаны

xp = xk +1 sin j, следовательно

Хр = xk +1 jcosj = ry& +1 j cosj ,

,.2,;,2 . o. -, í •______ . /2 -2___2

xp = r y + 2ry& ■ l j cos j +1 • j cos2 j yp = l cos j, следовательно yp =-l j sin j,

Тогда скорость центра масс робота:

2 _ ,.2.„2 , ______ . /2___2 ,„

vp = r j + 2rl\j/(pcosj+1 cos2 ф-ф + +1p sinp ф-ф2 = r2 j2 + Prljp cosp+12фp.

В итоге кинетическая энергия робота будет иметь следующий вид:

+ (r j + Prljpcosp +1 ф ). (3)

где X — радиус инерции робота относительно оси колес. С учетом (P) и (3) кинетическая энергия системы будет равна:

T = тку2(р2 + rp) + т

+ (Л2р2 + Prljp cos ф +12фр). (4)

Получим первое дифференциальное уравнение системы в соответствии с (1), дифференцируя (4) по ф, ф и t:

— = — (21ф + Prlj cos ф +12ф), йф 2

= (2l p + 2rl jj cos p — j sin p ■ p) +12 p),

= mv (l2 +12)p + mvrljcosp =

= mvgl sin p — 2MK. (5)

Здесь Мк — момент, действующий на платформу со стороны двигателя. Получим втрое дифференциальное уравнение системы в соответствии с (1), дифференцируя (4) по p, p и t:

= 2my(p2 + r2) + — (2r y + 2rj sin j),

= 2mKy(p + r ) + mvr y +

+ mvrl (jcos j- j2 sin j), 17

i Не можете найти то, что вам нужно? Попробуйте сервис подбора литературы.

= y(2mK (p2 + r2) + mpr2) +

+ трг1 (&со$р-ф28тр) = Мк. (6) Соотношения для двигателя постоянного тока имеют вид:

ke ‘ M ДВ = KmI Я JДВе = МДВ -МНАГ ■

Здесь Мдв — момент, генерируемый двигателем, МНАГ — нагрузочный момент, JдВ — момент инерции двигателя, ш — угловая скорость вращения ротора относительно статора, £ -угловое ускорение ротора, и, 1Я, ЯЯ — напряжение, ток и сопротивление якорной цепи, кт, ке -конструктивные коэффициенты, причем кт включает в себя передаточный коэффициент редуктора, связывающего колесо с валом двигателя.

Из уравнения (8) выразим ток в якорной цепи и подставим в (7):

Выражение для момента двигателя будет иметь следующий вид:

Подставим (11) в (9) и разрешим уравнение относительно нагрузочного момента:

МНАГ = ^m—m-e- w- J ДВе. (12)

Поскольку платформа и колеса являются нагрузкой для двигателей, примем МК = МндГ. Подставим (12) в (5) и (6), с учетом того, что ( = у-ф и e = ф-у:

(mp (Л2 +12) + 2 Jдв )/+(mprl — 2 Jm )у cos р = = mpgl sin р + 2 ^^ (у/ — ф) — 2 kmU

У(2(тк (Р2 + r2) + 2 mpr2) + J дв ) +

+ (mprl — JДВ )( фcos р — фsin р) = kU kk

Проведем линеаризацию в точке р = 0, принимая sin р = р, cos р = 1, Пренебрегая массой колеса, получим:

(mp (Л2 +12) + 2 J дв )р + (mprl — 2J т )y =

y(mpr 2 + JДВ )(mprl — JДВ )ф =

К = mp (Л2 +12) + 2Jдв ;

h12 = mprl — 2 JДВ ;

h13 = mprl — J ДВ ;

h14 = mpr 2 + J ДВ ; kk

h21 = 2; h22 = mvgl;

Ь = ктке . К = 2 кти .

«23 0 ‘ «24 ^ 0 ‘ КЯ КЯ

Добавим два уравнения: у = у ; ф = ф. Введем переменные состояния: х1 =ф; х2 = р; х3 = у; х4 = у .

Тогда в матричном виде система (13) примет следующий вид:

Н1 х = Н2х + Н3и , (14)

Чтобы перейти к стандартному описанию умножим систему (14) слева на Н1-1. В итоге получим:

х = Ах + Ви , Здесь

h11h23 + Л13 h21

i Не можете найти то, что вам нужно? Попробуйте сервис подбора литературы.

1Л14 — Л12Л13 1Л3 2 + 3 Л31

h11h14 — Л12Л13 h11h14 — Л12Л13

Для определения устойчивости системы определим ее характеристическое уравнение:

= Р4 + А3 р3 + А2 р2 + А р + А0,

_(Л11 + Л12)Л23 + (Л13 + Л14)Л21

Поскольку часть коэффициентов этого уравнения отрицательны, система является неустойчивой (что было очевидно).

За выход системы возьмем сумму ф и так как эти величины будут напрямую измеряться с помощью датчиков, тогда матрица связи выхода объекта у и вектора переменной состояния х будет иметь вид: С = [0 1 0 1].

Выполним проверку системы на управляемость.

Матрица управляемости будет иметь вид:

Так как объект управления является одноканальным, то критерием его управляемости является неравенство определителя этой матрицы нулю: аег(и) Ф 0.

Это несложно проверить с помощью среды МАТЬАВ. В данном случае определитель матрицы управляемости равен:

(а2 а4Ь2 + 2а3а4Ь2)2

он всегда положителен, управляем.

Выполним проверку системы на наблюдаемость. Матрица наблюдаемости имеет вид:

Данный объект одноканальный, поэтому он будет наблюдаемым, если det(U) Ф 0 .

2 2 _ 2 л 2 2 2 1 2 л 2

а2 а4 — 2а2а3а4 — 4а2а4а3 + а3 а4 + ¿а2а4а3 — 4а2а4а3 (а1а3 — а^)2 Следовательно, объект наблюдаем.

Для расчета регулятора был выбран модальный метод синтеза [2]. Данный метод применен для решения задачи стабилизации в силу следующих причин. Во-первых, основным режимом работы системы стабилизации робота является режим отработки начальных условий, а модальный метод дает лучший результат именно в таких системах. Во-вторых, для решения задачи стабилизации необходимо оценивать четыре переменных состояния системы, две из которых могут быть измерены напрямую (угол наклона платформы и угол поворота колеса соответственно). В соответствии с модальным методом синтеза закон управления имеет следующий вид:

и = Кх , и = [к1 к2 к3 к4 ],

где К — матрица коэффициентов регулятора. Здесь параметры матрицы К соответствуют коэффициентам в обратной связи при следующих величинах:

к1 — производная угла наклона платформы ф, к2 — угол наклона платформы ф, к3 — производная угла поворота колеса у, к4 — угол поворота колеса у, Далее необходимо определить характеристическое уравнение системы исходя из следующего соотношения:

А( р) = det( р1 — (А + ВК)) =

= р4 + А3 (К) р3 + А2 (К) р2 + А (К) р + А .

После этого составляется желаемое уравнение системы:

С (р) = р4 + С3 р3 + С2 р2 + С р + С0.

Для нахождения коэффициентов регулятора необходимо приравнять коэффициенты при степенях оператора р характеристического уравнения к соответствующим коэффициентам желаемого уравнения.

2. СТРУКТУРА РОБОТА

В соответствие с полученной математической моделью системы и выбранным методом синтеза регулятора была выбрана структура робота, представленная на Рис. 3.

Двигатель А Двигатель Б

Рис.3. Структурная схема робота

Ядром системы является микроконтроллер STM32F100RBT6, установленный на отладочной плате STM32VLDISCOVERY. Данный

микроконтроллер полностью реализует управление системой.

В качестве датчика угла наклона применен измерительный модуль MPU6050. Этот модуль включает в себя трехосевые гироскоп и акселерометр и может быть применен для определения не только угла наклона, но и его первой производной (угловой скорости) [3]. Это позволяет напрямую измерить еще одну переменную состояния, что существенно упрощает процедуру разработки системы стабилизации.

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

Так же в систему включен модуль связи по интерфейсу UART. К данному модулю может быть подключен преобразователь TTL-USB для связи с ПК или модуль Bluetooth, например, HC05. Данный модуль введен в систему для отладки ее работы, получению параметров системы в текущий момент времени, а также отправки команд устройству.

Таким образом, можно сформулировать следующие задачи, которые необходимо решить в процессе разработки системы.

1. Реализация управления сервомоторами и получение данных по углу поворота колеса

2. Определение угла наклона робота с помощью измерительного модуля MPU6050

3. Определение параметров математической модели системы

4. Синтез регулятора модальным методом

В следующих разделах статьи описано последовательное решение этих задач.

3. УПРАВЛЕНИЕ СЕРВОМОТОРАМИ

Одним из самых распространенных способов управления напряжением двигателей является широтно-импульсная модуляция. На вход двигателя подается прямоугольные импульсы постоянной частоты, но различной скважности. При достаточно высокой частоте импульсов напряжение сглаживается обмотками двигателя и эквивалентно постоянному напряжению определенной величины.

Для управления двигателем был задействован встроенный в микроконтроллер таймер, который имеет четыре канала управления с возможностью настройки их в режиме генерации ШИМ-сигнала. Модулированное напряжение подается на двигатель через специальные микросхемы -драйверы напряжения.

4. ОЦЕНКА УГЛА ПОВОРОТА И СКОРОСТИ ВРАЩЕНИЯ КОЛЕСА

Для оценки угла поворота колеса используются встроенные в двигатель квадратурные инкрементальные энкодеры. Каждый энкодер имеет два вывода: канал А и канал В. За один оборот колеса будет сгенерировано 180 импульсов на обоих выводах, причем время импульса Ти равно времени паузы Тп, что позволяет измерять угол поворота колеса с точностью до одного градуса, путем регистрирования не только положительных, но и отрицательных фронтов. Сигнал на одном канале сдвинут относительно другого. Это позволяет определять также направление поворота колеса. На Рис. 4 представлена диаграмма сигналов с каналов энкодера при повороте колеса по часовой стрелке, а на Рис. 5 — при работе против часовой стрелки.

Рис. 4. Сигналы энкодера при повороте колеса по часовой стрелке

Рис. 5. Сигналы энкодера при повороте колеса против часовой стрелки

Для регистрации фронтов был задействован таймер микроконтроллера. Каждый вывод энкодера подключен к отдельному его каналу. Если при регистрации положительного фронта на канале А значение на канале В равно нулю или

при отрицательном фронте — единице, то угол поворота инкрементируется, в других случаях -декрементируется.

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

Дифференцирующий фильтр — это устройство, позволяющее получить оценку фильтруемой величины, а также её производные. Рассмотрим его работу на примере дифференцирующего фильтра второго порядка. Структурная схема фильтра приведена на Рис. 6.

Рис. 6. Структурная схема дифференцирующего фильтра второго порядка: у — фильтруемый сигнал, у0 -его оценка, у0 = Су0 / А , у0 = сУ0 / А

Данной структурной схеме соответствует дифференциальное уравнение:

у = Т2 у0 + 2ХТу0 + у0.

На вход фильтра подается выходной сигнал объекта, на выходе можно получить оценку выходного сигнала объекта и ее первую производную. Теоретически, данный фильтр так же позволяет получить оценку второй производной (для этого необходимо взять сигнал со входа первого интегратора), но в силу зашумленности выходного сигнала объекта, качество получаемой оценки второй производной, как правило, неудовлетворительно. Для корректного определения п-й производной требуется дифференцирующий фильтр п + 1 порядка [4].

При построении фильтра необходимо правильно выбрать его параметры. Основными параметрами данного фильтра являются постоянная времени фильтра Т и коэффициент демпфирования с. Первый параметр определяет длительность переходных процессов в фильтре, то есть, его быстродействие. Второй параметр определяет колебательность процессов в фильтре. Рекомендуется выбирать данный параметр в пределах 0,5-1. Величина постоянной времени фильтра определяется постоянной времени процессов в объекте. Для корректного оценивания выходного сигнала объекта необходимо, чтобы постоянная времени фильтра была в 3-5 раз меньше, чем наименьшая постоянная времени объекта. Однако, следует помнить, что чем меньше значение постоянной времени фильтра, тем хуже эффект от его применения. Таким образом, значение

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

Наиболее простой способ реализации дифференцирующего фильтра второго порядка со структурой, описанной выше, состоит в замене операции интегрирования операцией

= | уЖ ^ у[к] = у[к — 1] + Ау х Аt.

где у[к-1] — значение фильтруемого параметра на предыдущем шаге, у[к] — значение на текущем, Ау[к] — входной параметр интегратора (производная выходного сигнала) на текущем шаге, Аt — шаг интегрирования. Фильтр второго порядка может быть реализован

последовательным выполнением следующих операций:

у[к ] 2ХАу[к — 1] Ау[к — 1]

i Не можете найти то, что вам нужно? Попробуйте сервис подбора литературы.

Ау[к ] = Ау[к — 1] + А2 у[к ]Аt, У[к ] = у[к — 1] + АуУ[к ]Аt,

где, А2у[к] — значение оценки второй производной, Ау — значение оценки первой производной, У — значение оценки фильтруемой величины, у -значение фильтруемой величины, Дt — шаг интегрирования.

В отличие от теоретической модели фильтра, в фильтре, реализованном на микроконтроллере, появляется еще один важный параметр, который необходимо правильно выбрать. Это шаг интегрирования. Основное правило при его выборе — шаг интегрирования должен быть меньше наименьшего параметра фильтра, то есть, постоянной времени. Для корректной работы фильтра необходимо, чтобы шаг интегрирования был меньше постоянной времени хотя бы в 3 раза.

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

Для работы в реальном времени, операции, приведенные выше, могут быть реализованы в виде функции, которая вызывается через интервалы времени, равные выбранному шагу интегрирования. Ниже представлены результаты работы дифференцирующего фильтра, примененного для оценки скорости вращения колеса. На Рис. 7 приведены результаты измерения угла поворота колеса, на Рис. 8 —

результат оценки скорости вращения колеса по данным угла поворота.

Рис. 7. Угол поворота колеса

Рис. 8. Оценка скорости вращения колеса

5. ПОЛУЧЕНИЕ МАТЕМАТИЧЕСКОЙ МОДЕЛИ СЕРВОМОТОРА

Для работы двигателя в соответствии с алгоритмом управления системы необходимо получить его математическую модель. Приближенно поведение двигателя постоянного тока описывается следующими соотношениями:

Je= М — М1; М = кт1; и — Е = ¡Я ; Е = к Г ;

где J — момент инерции вала; М — момент, создаваемый двигателем; М1 — нагрузочный момент; кт, ке — конструктивные коэффициенты двигателя; Я сопротивление обмотки якоря; Е -противо-ЭДС.

В режиме холостого хода при напряжении и0 на входе двигателя решением дифференциального уравнения (15) является функция:

где К = 1/ ке, Т = JЯ 1(ктке). Если проинтегрировать это уравнение на интервале от 0 до /, получим выражение для угла поворота ротора двигателя:

р(Г) = = и0 К (I — Т) + и0 КТе

Как видно из уравнения (17), с течением времени угол поворота приближается к прямой, описываемой уравнением:

Для получения математической модели двигателя были проведены измерения угла поворота ротора при напряжении и=5 В. Результат измерения угла поворота двигателя представлены на Рис. 9. Точки на линейном участке графика были аппроксимированы прямой, по коэффициентам которой были определены параметры модели:

К = 1,56 V ■ ^/гай, Т = 0,12 5.

Для проверки было проведено моделирование системы с полученными значениями параметров. На Рис. 10 представлены результаты моделирования системы и данные, полученные по результатам измерения для скорости вращения ротора двигателя.

Рис. 9. Данные для угла поворота ротора двигателя: синяя кривая — данные эксперимента, красная — данные по результатам моделирования.

Далее необходимо определить конструктивные коэффициенты двигателя кт и ке. Значение ке полностью определяется параметром К, а именно: К = 1/ке.

Однако данный параметр обладает существенной нелинейностью. Причинами наличия нелинейности являются конструктивные особенности двигателя, а также микросхем драйверов, используемых для управления двигателем. На Рис. 11 представлены переходные характеристики двигателя при значениях входного напряжения 1-10 В с шагом 1 В. На Рис. 12 представлена зависимость параметра ке от входного напряжения.

Для определения параметра кт был проведен эксперимент, в ходе которого измерялось значение пускового тока при известном нагрузочном моменте на валу двигателя. Схема опыта приведена на Рис. 13.

500 450 400 350 -Мо е1тд ептегЛ

Рис. 10. Данные для скорости вращения ротора двигателя: синяя кривая — данные эксперимента, красная — данные по результатам моделирования.

Рис. 11. Переходные характеристики двигателя при различных значениях входного напряжения

Рис. 12. Зависимость величины параметра ке от входного напряжения

Рис. 13. Схема эксперимента по определению конструктивного коэффициента кт. Мк — момент, создаваемый колесом; О — ось колеса; г — радиус колеса; РН — сила натяжения нити (вектор); Рг — сила давления груза на опору (вектор)

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

МК = г • РН = г (РГ1 — Рг 2 ) ,

где РГ1 — сила давления груза на опору при отключенном электродвигателе, РГ2 — при включенном. На Рис. 14 изображена полученная зависимость момента колеса от тока в якорной цепи.

Рис. 14. Зависимость момента колеса от тока в якорной цепи

Значение кт было определено из выражения: кт = М /1.

На основании эксперимента было определено следующее значение коэффициента: кт = 0,25.

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

6. ПРИНЦИП РАБОТЫ АКСЕЛЕРОМЕТРА

Акселерометр — это устройство, измеряющее проекцию на его оси суммы всех сил, приложенных к его корпусу, кроме силы тяжести [3]. То есть он измеряет проекцию кажущегося ускорения (разность между истинным ускорением объекта и гравитационным ускорением). Отсюда следует, что применение акселерометра для определения угла наклона имеет смысл, когда сила, приложенная к корпусу (реакция опоры), равна по модулю силе тяжести и направлена в противоположную сторону, то есть датчик должен находиться в покое или, по крайней мере, двигаться без ускорения, что в реальных системах встречается не часто.

Существует несколько видов акселерометров, отличающихся по чувствительным элементам и принципу действия. На Рис. 15 представлена структурная схема одноосевого акселерометра, который преобразует изменение положения чувствительного элемента 2 в напряжение с помощью потенциометра 3.

Рис. 15. Внутреннее устройство одноосного акселерометра. 1 — пружина, 2 — чувствительный элемент, 3 — потенциометр, 4 — демпфер, 5 — корпус

Чувствительный элемент представляет собой массу, закрепленную на пружинах, которые прикреплены к корпусу. Демпфер используется для уменьшения влияния собственных колебаний чувствительного элемента. На Рис. 15 к корпусу не приложены никакие силы вдоль оси X. Если же корпусу сообщить ускорение, то картина изменится, станет такой, как показано на Рис. 16.

В данном случае к корпусу была приложена сила Е1, чувствительный элемент смещается влево, в сторону, противоположную Е1. Это смещение регистрирует потенциометр, и на выходе датчика появляется напряжение, пропорциональное приложенной силе.

Рис. 17. Внутреннее устройство одноосного акселерометра при воздействии силы вдоль его оси

Если же расположить одноосный акселерометр перпендикулярно земной поверхности, то есть вдоль вектора силы тяжести, то датчик произведет измерение силы реакции опоры, как показано на Рис. 18.

Рис. 18. Внутреннее устройство одноосного акселерометра при измерении силы реакции опоры

При повороте датчика из положения на Рис. 18 в положение на Рис. 19, значение проекции силы Е на ось Х будет уменьшаться. Если к данному одноосевому акселерометру добавить второй такой же, но расположить его ось перпендикулярно оси первого, получится двухосевой акселерометр.

Рис. 19. Расчетная схема угла наклона по проекциям вектора силы реакции опоры на оси акселерометра

Как известно, по проекциям вектора на две оси (ОХ, ОУ) декартовой системы координат можно восстановить угол данного вектора в этой системе, что и требуется.

7. ПРИНЦИП РАБОТЫ ГИРОСКОПА

Существует несколько разновидностей микроэлектромеханических гироскопов, различающихся внутренним устройством, но всех их объединяет то, что их работа основана на использовании силы Кориолиса. В каждом из них есть рабочее тело, совершающее возвратно-поступательные движения. Если вращать подложку, на которой находится это тело, то на него начнет действовать сила Кориолиса, направленная перпендикулярно оси вращения и направлению движения тела. На Рис. 20 представлен механизм работы этой силы.

Рис. 20. Механизм работы силы Кориолиса: О -вектор угловой скорости, V — вектор линейной скорости, Ес — сила Кориолиса

Зная линейную скорость и силу Кориолиса можно определить угловую скорость.

Одна из возможных реализаций гироскопа имеет следующую структуру: закрепленная на гибких подвесках рамка, внутри которой совершает поступательные колебательные движения некая масса [4]. Структура такого сенсора представлена на Рис. 21.

Рис. 21. Внутренняя структура гироскопа: 1 -крепление массы, 2 — рабочая масса, 3 — крепление внутренней рамки, 4 — сенсоры перемещения внутренней рамки, 5 — внутренняя рамка, 6 — подложка

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

подложной расположены обкладки плоских конденсаторов (сенсоры перемещения), таким образом, измеряя их емкость, можно фиксировать движение рамки относительно подложки.

На Рис. 22 представлена структура гироскопа при его вращении в плоскости ХУ по часовой стрелке.

Рис. 22. Структура гироскопа при вращении: О -вектор угловой скорости, V — вектор линейной скорости, Ес — сила Кориолиса

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

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

8. ПОДКЛЮЧЕНИЕ МРиб050

В качестве датчика угла наклона робота применяется измерительный модуль МРП6050. Данный модуль включает в себя два МЭМС-датчика — акселерометр и гироскоп. Модуль имеет цифровой выход /2С. Встроенный АЦП преобразует аналоговые сигналы датчиков в цифровой код для дальнейшей передачи по интерфейсу /2с. Ниже приведены технические характеристики модуля.

• Напряжение питания 2,375 К-3,46 V;

• ИГО-буфер объемом 1024 байт;

цифровые фильтры для гироскопа, акселерометра и термодатчика;

• Интерфейс I2C для записи и чтения; регистров устройства, работающий на частоте до 400 кГц;

i Не можете найти то, что вам нужно? Попробуйте сервис подбора литературы.

• Программируемый пользователем диапазон измерений: ±250, ±500, ±1000, и ±2000 °/с;

• Встроенный 16-разрядный АЦП;

• Цифровой программируемый ФНЧ;

• Ток в режиме работы — 3,6 мА;

• Ток в режиме ожидания 5 мкА.

МРП6050 был подключен к 12С модулю

микроконтроллера 5ГМ32Р100ЛВГ6£ для отправки команд и считывания из регистров необходимых данных. После отправки модулю команды о начале измерений происходит постоянная оцифровка показаний со всех осей гироскопа, акселерометра и термодатчика. Остается только считывать информацию из необходимых регистров. Частота записи новых данных в эти регистры аналогово-цифровым преобразователем зависит от выбранной пользователем чувствительности сенсора и, следовательно, диапазона измерений.

Связь датчика и микроконтроллера осуществляется по интерфейсу 12С. Данный интерфейс имеет архитектуру ведущий -ведомый, то есть одно устройство (ведущее) производит запрос на чтение или запись ведомых устройств. В нашем случае ведомым является МРП6050, а ведущим 12С модуль, находящийся на микроконтроллере. Для связи используется две двунаправленные линии: тактирования и данных. Ведомое устройство имеет свой адрес, который на данной линии должен быть уникальным (в нашем случае это 0хБ0). Линии должны быть подтянуты к уровню логической единицы. Как правило, устройства подключаются к линиям через выводы с открытым коллектором (стоком). Преимуществом является возможность подключения большого количества ведомых устройств, а также сравнительно высокая дальность передачи данных. Но при увеличении дальности фактическая скорость передачи данных снижается. Это связано с тем, что фронты сглаживаются из-за увеличения емкости проводов.

Стоит обратить внимание на некоторые особенности, связанные с программной реализацией алгоритма получения данных на микроконтроллере серии БТМ32.

Во-первых, необходимо сбрасывать флаги состояния модуля 12С-периферии микроконтроллера. Осуществляется это чтением регистра флагов. Если этого не сделать, 12С -модуль МРП6050 не сможет перейти к следующей стадии работы и произойдет зависание программы. Момент времени, когда необходимо сбрасывать флаги зависит от конкретной микросхемы контроллера. Поэтому необходимо тщательно изучить документацию на данный периферийный

модуль микроконтроллера, а также документ, содержащий сведения об ошибках в документации (ERRATA SHEET).

Вторая особенность связана с завершением сеанса связи при чтении данных из регистров MPU6050. Сигналом завершения обмена данных является установка стоп-условия на линии данных. В случае с микроконтроллером STM32F100 команду на генерацию стоп-условия для него необходимо отдать после прочтения предпоследнего байта. Если принимается один байт, это необходимо сделать после получения бита подтверждения адреса от ведомого устройства. Если это сделать после прочтения последнего байта, микроконтроллер будет ожидать прием еще одного байта, которого не будет, и программа зависнет.

9. ОПРЕДЕЛЕНИЕ УГЛА

ОТКЛОНЕНИЯ ОТ ВЕРИКАЛИ

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

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

Напомним, что акселерометр измеряет проекцию на его оси суммы всех сил, приложенных к нему, за исключением силы тяжести. То есть в статике производится измерение силы реакции опоры на данное устройство. А так как эта сила противоположна по направлению гравитационной, то, зная величины её проекций на оси, можно определить угол отклонения от вертикали. Это можно сделать по формуле:

Факс [i +1] — угол отклонения в текущий

времени, вычисленный с помощью

акселерометра, Fon [i +1] — проекция силы

реакции опоры на ось у, а Еоп [И +1] — на ось х.

Гироскоп измеряет угловую скорость в определенной плоскости, используя эффект Кориолиса. В данном случае нас интересует угловая скорость в плоскости ХУ. Получить величину угла можно проинтегрировав сигнал гироскопа:

При реализации на микроконтроллере данная операция заменяется численным интегрированием:

Рн V +1] = Рн И + Р [ +1] =

где рн [И +1], рн [И] — угол, вычисленный с помощью гироскопа на текущем и предыдущем шаге, О[И] — угловая скорость на предыдущем

шаге, Арн [И +1] — приращение угла за промежуток времени, равный шагу интегрирования, Аt -шаг интегрирования.

На Рис. 23 и Рис. 24 представлены графики углов, полученных с использованием данных акселерометра и гироскопа, входящих измерительную систему МРП6050. Для удобства начальный угол гироскопа выбран максимально близким к углу акселерометра. На Рис. 23 видно, что разброс значений угла, вычисленного из показаний акселерометра, превышает один градус, а на Рис. 24 имеется четырехкратная ошибка его определения в динамике. Показания угла, вычисленного из угловой скорости гироскопа, более корректны на втором графике, но на первом имеется значительный дрейф этого угла.

160 200 Секучда’ЮО

Рис. 23. Угол, вычисленный с помощью показаний акселерометра и гироскопа в статике

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

компенсировал недостатки. Один из возможных вариантов такого фильтра — комплементарный фильтр, или альфа-бета фильтра.

150 200 250 СекундэМОО

Рис. 24. Угол, вычисленный с помощью показаний акселерометра и гироскопа в динамике

10. КОМПЛЕМЕНТАРНЫЙ фильтр

Комплементарный фильтр или альфа-бета фильтр в данном случае используется для определения угла наклона и оперирует в своей работе показаниями акселерометра и гироскопа. Ниже представлена формула для вычисления этого угла:

р[И +1] = (1 — К)(Р] + АРн [И +1]) +

+ КрА [И +1], (19) где К — коэффициент фильтра (имеет значение в пределах от 0 до 1), р[И], р[И +1]- угол, вычисленный с помощью фильтра, на текущем и предыдущем шагах.

Подставим (17) и (18) в (19) и получим окончательное выражение для вычисления угла отклонения:

Р[1 + 1] = (1 — К)(р[И] + а[1 ]Аt) +

Данный фильтр является упрощенным вариантом фильтра Калмана для одномерного случая, где первое слагаемое представляет собой звено предсказания. Только приращение угла определяется исходя не из математической модели системы и известной величины управления, а угловой скорости на предыдущем шаге, которая измеряется напрямую гироскопом. Также коэффициент К является постоянной величиной. Всё это значительно упрощает вычислительный процесс, при этом полученное значение угла можно считать достаточно точным приближением к реальному. Стоит отметить, что применение гироскопа хорошо тем, что на его выходе мы получаем величину угловой скорости, что избавляет нас от необходимости дифференцировать величину угла, которая имеет высокочастотную составляющую. Особенно

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

Изменяя значение коэффициента К, мы определяем, какое из слагаемых будет иметь больший вес. При уменьшении К показания фильтра будут стремиться к показаниям гироскопа, а при увеличении — к показаниям акселерометра. При К = 0 уравнение (19) вырождается в (18), и мы получим величину угла без высокочастотных помех, но постоянно «уплывающую». При К = 1 уравнение (19) вырождается в (17), и вычисленный угол будет сильно зашумлен и подвержен влиянию линейных ускорений. Задача состоит в правильном подборе этого коэффициента, чтобы оценка угла, полученная с выхода этого фильтра, удовлетворяла заданным требованиям. На Рис. 25 и Рис. 26 представлены графики при различных значениях параметра К. Шаг дискретизации равен 1 мс. Начальные значения угла гироскопа и фильтра заданы отличными от начального значения угла акселерометра на 2 градуса для более удобной оценки медленных процессов в фильтре. В работающей системе эти значения должны совпадать, как это показано на Рис. 27.

Рис. 25. Значения углов гироскопа, акселерометра и альфа-бета фильтра при К = 0,01

Как видно из графиков, при уменьшении значения параметра К увеличивается время протекания медленных процессов в фильтре. Это обусловлено ослаблением влияния на его показания данных акселерометра (линия стала более гладкой) и увеличением влияния данных гироскопа. Изменение начальных условий не влияет на время протекания медленных процессов (см. Рис. 26 и Рис. 27). Также можно наблюдать появление статической ошибки, которая будет расти при уменьшении коэффициента К и увеличении скорости смещения угла гироскопа, но эту скорость можно считать постоянной, и она

зависит от точности датчика и вычислительной погрешности: чем меньше шаг интегрирования, тем меньше ошибка и ниже скорость дрейфа. При этом статическая ошибка не зависит от начальных условий. При К = 0,001 она составила 1,2-1,3°. Для наиболее точной работы системы рекомендуется начальное значение фильтра приравнять начальному значению угла акселерометра. При этом смещение можно вычислить опытным путем, и оно будет одинаковым во всем диапазоне измерения угла.

1 ‘ Угол фипьтр 2-Угол акселерометр 3-Угол гироскоп

Рис. 26. Значения углов гироскопа, акселерометра и альфа-бета фильтра при К = 0,001

150 200 Секувда’100

Рис. 27. Значения углов гироскопа, акселерометра и альфа-бета фильтра при К = 0,001 и одинаковых начальных условиях

На Рис. 28 и Рис. 29 представлены графики углов гироскопа, акселерометра и альфа-бета фильтра в динамике при различных значениях параметра К. На Рис. 28 угол фильтра в большей степени соответствует углу гироскопа. Это означает, что для коэффициента К лучшим значением будет 0,001, несмотря на большее время протекания медленных процессов. Измерив смещение, мы сможем его скомпенсировать, но на протяжении времени протекания медленных процессов (первых трех секунд) угол будет определяться неточно: наибольшее значение ошибки будет в нулевой момент времени и будет

равно величине смещения. Но ошибку в 1-2° регулятор более успешно отработает, чем ошибки, вносимые акселерометром при большем значении параметра К. В динамике задержка фильтра минимальна (при К = 0,001 не превышает 20 мс).

Рис. 28. Значения углов гироскопа, акселерометра и альфа-бета фильтра в динамике при К = 0,01

Рис. 29. Значения углов гироскопа, акселерометра и альфа-бета-фильтра в динамике при K = 0,001

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

На Рис. 31 показаны графики изменения углов при падении балансирующего робота, который изначально находился в вертикальном положении. Из графиков видно, что падение робота в одну сторону акселерометр изначально определил, как падение в другую, вследствие своей внутренней структуры. В системе с обратной связью это может привести к появлению положительной обратной связи, что может сделать систему неустойчивой. Но использование комплементарного фильтра решило эту проблему.

i Не можете найти то, что вам нужно? Попробуйте сервис подбора литературы.

зооо 20 00 1000 о 1000 -2 ООО

1-Угол фильт р son

2-1\ Угол аксеп 3-Угол гирос

Как правильно установить датчик гироскоп на робота

Звезда активна Звезда не активна

Введение:

Завершающий урок цикла «Первые шаги» посвятим изучению гироскопического датчика. Данный датчик, как и ультразвуковой, присутствует только в образовательной версии набора Lego mindstorms EV3. Тем не менее, пользователям домашней версии конструктора советуем тоже обратить внимание на данный урок. Возможно, что прочитав о назначении и использовании этого датчика, вы пожелаете его приобрести в дополнение к своему набору.

10.1. Изучаем гироскопический датчик

Гироскопический датчик (Рис. 1) предназначен для измерения угла вращения робота или скорости вращения. Сверху на корпусе датчика нанесены две стрелки, обозначающие плоскость, в которой работает датчик. Поэтому важно правильно установить датчик на робота. Также для более точного измерения крепление гироскопического датчика должно исключать его подвижность относительно корпуса робота. Даже во время прямолинейного движения робота гироскопический датчик может накапливать погрешность измерения угла и скорости вращения, поэтому непосредственно перед измерением следует осуществить сброс в 0 текущего показания датчика. Вращение робота против часовой стрелки формирует отрицательные значения измерений, а вращение по часовой стрелке — положительные.

Гироскопический датчик

Рис. 1

Рассмотрим программный блок «Гироскопический датчик» (Рис. 2) Желтой палитры. Этот программный блок имеет три режима работы: «Измерение», «Сравнение» и «Сброс». В режиме «Измерение» можно измерить «Угол», «Скорость» или одновременно «Угол и скорость».

Программный блок

Рис. 2

Давайте закрепим гироскопический датчик на нашем роботе (Рис. 3), подсоединим его кабелем к порту 4 модуля EV3 и рассмотрим примеры использования.

Крепление гироскопического датчика на роботе

Рис. 3

Задача №22: написать программу движения робота по квадрату с длиной стороны квадрата, равной длине окружности колеса робота.

Решение:

Перед началом движения сбросим датчик в 0, используя программный блок «Гироскопический датчик» Желтой палитры;
Мы уже знаем: чтобы проехать прямолинейно требуемое расстояние — необходимо, воспользовавшись программным блоком «Независимое управление моторами», включить оба мотора на 1 оборот.
Для поворота робота на 90 градусов в этот раз воспользуемся гироскопическим датчиком:
1. используя программный блок «Независимое управление моторами», заставим робота вращаться вправо вокруг своей оси;
2. используя программный блок «Ожидание» в режиме «Гироскопический датчик», будем ждать, пока значение угла поворота не достигнет 90 градусов;
3. Выключим моторы;
Попробуйте решить Задачу №22 самостоятельно, не подглядывая в решение.

Решение Задачи №22

Рис. 4

«Первые шаги» — послесловие

Десятый урок завершает курс «Первые шаги». На протяжении всех уроков вы познакомились с конструктором Lego mindstorms EV3, со средой программирования, научились использовать моторы и датчики. Если вы успешно одолели курс «Первые шаги», то впереди вас ждет знакомство с решением популярных задач робототехники в рамках курса «Практика». Удачи!

Комментарии
1 2 3 4
-3 #31 САВА 07.08.2019 10:07
Цитирую михаил ж:

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

Скачайте блок на оф сайте майндстормс,
Или ев 3 едукейшон
+2 #32 Ансар 17.07.2020 09:06

Мы не могли разобраться с гироскопическим датчиком но в этом сайте всё легко и понятно!Огромное спасибо вам

-2 #33 Seymour 20.01.2021 22:41
Thanks for finally writing about >Использование гироскопического
датчика Lego mindstorms EV3
-3 #34 Роман 26.03.2022 11:26

Если в блоке «Ожидание» использовать режим «гироскопический датчик — изменить — угол», то сбрасывать на 0 будет не нужно и не будет путаницы с отрицательными значениями.

1 2 3 4
Добавить комментарий

Внимание!
Возрастная категория посетителей сайта — (10+).
Все комментарии перед публикацией проходят модерацию.
- Вы здесь:
- Главная
- Первые шаги
- Урок №10 — Изучаем гироскопический датчик
Вход на сайт

Популярные статьи
- Урок №2 — Программирование движения робота
- Урок №12 — Сумо
- Как добавить датчики в среду программирования домашней версии
- Урок №1 — Знакомство с конструктором
- Урок №11 — Кегельринг
Похожие публикации:

Как правильно установить датчик гироскоп на робота

Работа с Arduino и MPU6050

Подключение

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

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

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

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

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

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

Получение углов

Оптимальное получение углов

Некоторые примеры

Ардуино: акселерометр MPU6050

Характеристики модуля MPU6050 ROC:

Список необходимых компонентов

Подключение MPU6050 к Ардуино

Принципиальная схема

Внешний вид макета

Программа для получения сырых данных с акселерометра MPU6050

Точность измерения ускорения в MPU6050

Программа для вычисления угла наклона акселерометра MPU6050

Заключение

Несколько идей/заданий:

Принципиальная схема датчика

Полезные ссылки

Ардуино: акселерометр MPU6050 : 33 комментария

РАЗРАБОТКА СИСТЕМЫ СТАБИЛИЗАЦИИ УГЛА ОТКЛОНЕНИЯ БАЛАНСИРУЮЩЕГО РОБОТА Текст научной статьи по специальности «Механика и машиностроение»

Текст научной работы на тему «РАЗРАБОТКА СИСТЕМЫ СТАБИЛИЗАЦИИ УГЛА ОТКЛОНЕНИЯ БАЛАНСИРУЮЩЕГО РОБОТА»

Как правильно установить датчик гироскоп на робота

Введение:

10.1. Изучаем гироскопический датчик

«Первые шаги» — послесловие

Вход на сайт

Популярные статьи

Добавить комментарий Отменить ответ