Используйте IMU-сенсор (от англ. Inertial Measurement Unit — Инерционное измерительное устройство) для определения положение вашего девайса в пространстве.
IMU-модуль на 10 степеней свободы включает в себя четыре отдельных сенсора:
Показания акселерометра, гироскопа и магнитометра можно использовать как входные данные для фильтра Маджвика, Махони или Калмана, для определения положение вашего девайса в пространстве через кватернионы.
Кватернионы «сильный» инструмент для выполнения вычислений по ориентации объекта в пространстве, но им не хватает наглядности. Поэтому полученные кватернионы из фильтра удобнее представить в самолётных углах Эйлера: Pitch, Roll и Yaw
В фильтре Маджвика реализовано два варианта определение ориентации объекта в пространстве.
В связи с искажениями от внешних магнитных полей, откалибруйте магнитометр для работы в режиме электронного компаса.
В качестве мозга для считывания показаний с датчика рассмотрим платформу из серии Arduino, например, Uno.
На аппаратном уровне инерционный модуль общается с управляющей электроникой по шине I²C. Но не переживайте о битах и байтах: используйте библиотеку TroykaIMU и на выходе получите готовы данные.
Подключите IMU-сенсор к пинам питания и шины I²C — SDA и SCL на платформе Uno. Для коммуникации используйте соединительные провода «мама-папа»
Для быстрой сборки и отладки устройства возьмите плату расширения Troyka Shield, которая одевается сверху на Arduino Uno методом бутерброда. Для коммуникации используйте трёхпроводной шлейф «мама-мама», который идёт в комплекте с датчиком.
С Troyka Slot Shield провода не понадобятся вовсе.
В качестве примера выведем в Serial-порт следующие данные:
// Библиотека для работы с модулями IMU #include <TroykaIMU.h> // Создаём объект для работы с гироскопом Gyroscope gyroscope; // Создаём объект для работы с акселерометром Accelerometer accelerometer; // Создаём объект для работы с магнитометром/компасом Compass compass; // Создаём объект для работы с барометром Barometer barometer; void setup() { // Открываем последовательный порт Serial.begin(9600); // Выводим сообщение о начале инициализации Serial.println("IMU Begin"); // Инициализируем гироскоп gyroscope.begin(); // Инициализируем акселерометр accelerometer.begin(); // Инициализируем компас compass.begin(); // Инициализируем барометр barometer.begin(); // Выводим сообщение об удачной инициализации Serial.println("Initialization completed"); Serial.println("Gyroscope\t\t\tAccelerometer\t\t\tCompass\t\tBarometer"); } void loop() { // Выводим угловую скорость в градусах в секунду относительно оси X, Y и Z Serial.print(gyroscope.readRotationDegX()); Serial.print("\t"); Serial.print(gyroscope.readRotationDegY()); Serial.print("\t"); Serial.print(gyroscope.readRotationDegZ()); Serial.print("\t\t"); // Выводим направления и величины ускорения в м/с² относительно оси X, Y и Z Serial.print(accelerometer.readAccelerationAX()); Serial.print("\t"); Serial.print(accelerometer.readAccelerationAY()); Serial.print("\t"); Serial.print(accelerometer.readAccelerationAZ()); Serial.print("\t\t"); // Выводим напряженность магнитного поля в Гауссах относительно оси X, Y и Z Serial.print(compass.readMagneticGaussX()); Serial.print("\t"); Serial.print(compass.readMagneticGaussY()); Serial.print("\t"); Serial.print(compass.readMagneticGaussZ()); Serial.print("\t\t"); // Выводим значения атмосферного давления в мм рт.ст. Serial.print(barometer.readPressureMillimetersHg()); Serial.print("\t"); // Выводим значения высоты над уровнем море Serial.print(barometer.readAltitude()); Serial.print("\t"); // Выводим значения температуры окружающей среды Serial.print(barometer.readTemperatureC()); Serial.println(); delay(100); }
Определим ориентацию объекта в пространстве без учёта направления на север. Для этого показания акселерометра и гироскопа используем как входные данные для фильтра Маджвика. На выходе получим кватернион, который для наглядности пересчитаем в самолётные углы Эйлера.
// Библиотека для работы с модулями IMU #include <TroykaIMU.h> // Создаём объект для фильтра Madgwick Madgwick filter; // Создаём объект для работы с гироскопом Gyroscope gyroscope; // Создаём объект для работы с акселерометром Accelerometer accelerometer; // Переменные для данных с гироскопа и акселерометра float gx, gy, gz, ax, ay, az; // Переменные для хранения самолётных углов ориентации float yaw, pitch, roll; // Переменная для хранения частоты выборок фильтра float sampleRate = 100; void setup() { // Открываем последовательный порт Serial.begin(9600); // Выводим сообщение о начале инициализации Serial.println("IMU Begin"); // Инициализируем гироскоп gyroscope.begin(); // Инициализируем акселерометр accelerometer.begin(); // Инициализируем фильтр filter.begin(); // Выводим сообщение об удачной инициализации Serial.println("Initialization completed"); } void loop() { // Запоминаем текущее время unsigned long startMillis = millis(); // Считываем данные с акселерометра в единицах G accelerometer.readAccelerationGXYZ(ax, ay, az); // Считываем данные с гироскопа в радианах в секунду gyroscope.readRotationRadXYZ(gx, gy, gz); // Устанавливаем частоту фильтра filter.setFrequency(sampleRate); // Обновляем входные данные в фильтр filter.update(gx, gy, gz, ax, ay, az); // Получаем из фильтра углы: yaw, pitch и roll yaw = filter.getYawDeg(); pitch = filter.getPitchDeg(); roll = filter.getRollDeg(); // Выводим полученные углы Эйлера в Serial-порт Serial.print("yaw: "); Serial.print(yaw); Serial.print("\t\t"); Serial.print("pitch: "); Serial.print(pitch); Serial.print("\t\t"); Serial.print("roll: "); Serial.println(roll); // Вычисляем затраченное время на обработку данных unsigned long deltaMillis = millis() - startMillis; // Вычисляем частоту обработки фильтра sampleRate = 1000 / deltaMillis; }
Определим ориентацию объекта в пространстве с учётом направления на север. Для этого показания акселерометра, гироскопа и компаса используем как входные данные для фильтра Маджвика. На выходе получим кватернионы, которые для наглядности пересчитаем в самолётные углы Эйлера.
// Библиотека для работы с модулями IMU #include <TroykaIMU.h> // Создаём объект для фильтра Madgwick Madgwick filter; // Создаём объект для работы с гироскопом Gyroscope gyroscope; // Создаём объект для работы с акселерометром Accelerometer accelerometer; // Создаём объект для работы с магнитометром/компасом Compass compass; // Переменные для данных с гироскопа, акселерометра и компаса float gx, gy, gz, ax, ay, az, mx, my, mz; // Переменные для хранения самолётных углов ориентации float yaw, pitch, roll; // Переменная для хранения частоты выборок фильтра float sampleRate = 100; // Калибровочные данные для работы магнитометра в режиме компаса // Подробности читайте в документации про калибровку модуля // http://wiki.amperka.ru/articles:troyka-magnetometer-compass-calibrate const float compassCalibrationBias[3] = { 567.893, -825.35, 1061.436 }; const float compassCalibrationMatrix[3][3] = { { 1.909, 0.082, 0.004 }, { 0.049, 1.942, -0.235 }, { -0.003, 0.008, 1.944} }; void setup() { // Открываем последовательный порт Serial.begin(9600); // Выводим сообщение о начале инициализации Serial.println("IMU Begin"); // Инициализируем гироскоп gyroscope.begin(); // Инициализируем акселерометр accelerometer.begin(); // Инициализируем компас compass.begin(); // Инициализируем фильтр filter.begin(); // Устанавливаем калибровочные данные compass.setCalibrateMatrix(compassCalibrationMatrix, compassCalibrationBias); // Выводим сообщение об удачной инициализации Serial.println("Initialization completed"); } void loop() { // Запоминаем текущее время unsigned long startMillis = millis(); // Считываем данные с акселерометра в единицах G accelerometer.readAccelerationGXYZ(ax, ay, az); // Считываем данные с гироскопа в радианах в секунду gyroscope.readRotationRadXYZ(gx, gy, gz); // Считываем данные с компаса в Гауссах compass.readCalibrateMagneticGaussXYZ(mx, my, mz); // Устанавливаем частоту фильтра filter.setFrequency(sampleRate); // Обновляем входные данные в фильтр filter.update(gx, gy, gz, ax, ay, az, mx, my, mz); // Получаем из фильтра углы: yaw, pitch и roll yaw = filter.getYawDeg(); pitch = filter.getPitchDeg(); roll = filter.getRollDeg(); // Выводим полученные углы Эйлера в serial-порт Serial.print("yaw: "); Serial.print(yaw); Serial.print("\t\t"); Serial.print("pitch: "); Serial.print(pitch); Serial.print("\t\t"); Serial.print("roll: "); Serial.println(roll); // Вычисляем затраченное время на обработку данных unsigned long deltaMillis = millis() - startMillis; // Вычисляем частоту обработки фильтра sampleRate = 1000 / deltaMillis; }
Для наглядности приведём пример отображение IMU-сенсора в пространстве в виде самолёта. Для запуска примера необходимо прошить платформу Arduino кодом ниже и настроить графическую среду Processing.
// Библиотека для работы с модулями IMU #include <TroykaIMU.h> // Создаём объект для фильтра Madgwick Madgwick filter; // Создаём объект для работы с гироскопом Gyroscope gyroscope; // Создаём объект для работы с акселерометром Accelerometer accelerometer; // Переменные для данных с гироскопа и акселерометра и компаса float gx, gy, gz, ax, ay, az; // Переменные для хранения самолётных углов ориентации float yaw, pitch, roll; // Переменная для хранения частоты выборок фильтра float sampleRate = 100; void setup() { // Открываем последовательный порт Serial.begin(9600); // Инициализируем гироскоп gyroscope.begin(); // Инициализируем акселерометр accelerometer.begin(); // Инициализируем фильтр filter.begin(); } void loop() { // Запоминаем текущее время unsigned long startMillis = millis(); // Считываем данные с акселерометра в единицах G accelerometer.readAccelerationGXYZ(ax, ay, az); // Считываем данные с гироскопа в радианах в секунду gyroscope.readRotationRadXYZ(gx, gy, gz); // Устанавливаем частоту фильтра filter.setFrequency(sampleRate); // Обновляем входные данные в фильтр filter.update(gx, gy, gz, ax, ay, az); if (Serial.available() > 0) { int val = Serial.read(); // Если пришёл символ 's' if (val == 's') { float q0, q1, q2, q3; filter.readQuaternion(q0, q1, q2, q3); // Выводим кватернион в serial-порт Serial.print(q0); Serial.print(","); Serial.print(q1); Serial.print(","); Serial.print(q2); Serial.print(","); Serial.println(q3); } } // Вычисляем затраченное время на обработку данных unsigned long deltaMillis = millis() - startMillis; // Вычисляем частоту обработки фильтра sampleRate = 1000 / deltaMillis; }
// Библиотека для работы с модулями IMU #include <TroykaIMU.h> // Создаём объект для фильтра Madgwick Madgwick filter; // Создаём объект для работы с гироскопом Gyroscope gyroscope; // Создаём объект для работы с акселерометром Accelerometer accelerometer; // Создаём объект для работы с магнитометром/компасом Compass compass; // Переменные для данных с гироскопа, акселерометра и компаса float gx, gy, gz, ax, ay, az, mx, my, mz; // Переменные для хранения самолётных углов ориентации float yaw, pitch, roll; // Переменная для хранения частоты фильтра float sampleRate = 100; // Калибровочные данные для работы магнитометра в режиме компаса // Подробности читайте в документации про калибровку модуля // http://wiki.amperka.ru/articles:troyka-magnetometer-compass-calibrate const float compassCalibrationBias[3] = { 567.893, -825.35, 1061.436 }; const float compassCalibrationMatrix[3][3] = { { 1.909, 0.082, 0.004 }, { 0.049, 1.942, -0.235 }, { -0.003, 0.008, 1.944 } }; void setup() { // Открываем последовательный порт Serial.begin(9600); // Инициализируем гироскоп gyroscope.begin(); // Инициализируем акселерометр accelerometer.begin(); // Инициализируем компас compass.begin(); // Инициализируем фильтр filter.begin(); // Устанавливаем калибровочные данные compass.setCalibrateMatrix(compassCalibrationMatrix, compassCalibrationBias); } void loop() { // Запоминаем текущее время unsigned long startMillis = millis(); // Считываем данные с акселерометра в единицах G accelerometer.readAccelerationGXYZ(ax, ay, az); // Считываем данные с гироскопа в радианах в секунду gyroscope.readRotationRadXYZ(gx, gy, gz); // Считываем данные с компаса в Гауссах compass.readCalibrateMagneticGaussXYZ(mx, my, mz); // Устанавливаем частоту фильтра filter.setFrequency(sampleRate); // Обновляем входные данные в фильтр filter.update(gx, gy, gz, ax, ay, az, mx, my, mz); if (Serial.available() > 0) { int val = Serial.read(); // Если пришёл символ 's' if (val == 's') { float q0, q1, q2, q3; filter.readQuaternion(q0, q1, q2, q3); // Выводим кватернион в serial-порт Serial.print(q0); Serial.print(","); Serial.print(q1); Serial.print(","); Serial.print(q2); Serial.print(","); Serial.println(q3); } } // Вычисляем затраченное время на обработку данных unsigned long deltaMillis = millis() - startMillis; // Вычисляем частоту обработки фильтра sampleRate = 1000 / deltaMillis; }
import processing.serial.*; import toxi.geom.*; import toxi.processing.*; // NOTE: requires ToxicLibs to be installed in order to run properly. // 1. Download from http://toxiclibs.org/downloads // 2. Extract into [userdir]/Processing/libraries // (location may be different on Mac/Linux) // 3. Run and bask in awesomeness // The serial port Serial port; String message; float[] q = new float[4]; Quaternion quat = new Quaternion(1, 0, 0, 0); // New line character in ASCII final char newLine = '\n'; String [] massQ = new String [4]; float[] ypr = new float[3]; void setup() { // Size form 400x400 size(400, 400, P3D); // Open serial port // Replace "COM7" with the COM port on which your arduino is connected port = new Serial(this, "COM7", 9600); } void draw() { // Read and parse incoming serial message serialEvent(); // Set background to black background(0); printQuaternions(); printYawPitchRoll(); // Set position to centre translate(width / 2, height / 2); // Begin object pushMatrix(); float[] axis = quat.toAxisAngle(); rotate(axis[0], axis[2], axis[3], axis[1]); // Draw main body in red drawBody(); // Draw front-facing tip in blue drawCylinder(); // Draw Triangles drawTriangles(); // Draw Quads drawQuads(); // End of object popMatrix(); // Send character 's' to Arduino port.write('s'); } void serialEvent() { // Read from port until new line (ASCII code 13) message = port.readStringUntil(newLine); if (message != null) { // Split message by commas and store in String array massQ = split(message, ","); q[0] = float(massQ[0]); q[1] = float(massQ[1]); q[2] = float(massQ[2]); q[3] = float(massQ[3]); } // Print values to console print(q[0]); print("\t"); print(q[1]); print("\t"); print(q[2]); print("\t"); print(q[3]); println("\t"); // Set our toxilibs quaternion to new data quat.set(q[0], q[1], q[2], q[3]); } void drawCylinder() { float topRadius = 0; float bottomRadius = 20; float tall = 20; int sides = 8; // Begin object pushMatrix(); translate(0, 0, -120); rotateX(PI/2); fill(0, 0, 255, 200); float angle = 0; float angleIncrement = TWO_PI / sides; beginShape(QUAD_STRIP); for (int i = 0; i < sides + 1; ++i) { vertex(topRadius * cos(angle), 0, topRadius * sin(angle)); vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle)); angle += angleIncrement; } endShape(); // if it is not a cone, draw the circular top cap if (topRadius != 0) { angle = 0; beginShape(TRIANGLE_FAN); // Center point vertex(0, 0, 0); for (int i = 0; i < sides + 1; i++) { vertex(topRadius * cos(angle), 0, topRadius * sin(angle)); angle += angleIncrement; } endShape(); } // If it is not a cone, draw the circular bottom cap if (bottomRadius != 0) { angle = 0; beginShape(TRIANGLE_FAN); // Center point vertex(0, tall, 0); for (int i = 0; i < sides + 1; i++) { vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle)); angle += angleIncrement; } endShape(); } popMatrix(); } void drawBody() { fill(255, 0, 0, 200); box(10, 10, 200); } void drawTriangles() { // Draw wings and tail fin in green fill(0, 255, 0, 200); beginShape(TRIANGLES); // Wing top layer vertex(-100, 2, 30); vertex(0, 2, -80); vertex(100, 2, 30); // Wing bottom layer vertex(-100, -2, 30); vertex(0, -2, -80); vertex(100, -2, 30); // Tail left layer vertex(-2, 0, 98); vertex(-2, -30, 98); vertex(-2, 0, 70); // Tail right layer vertex( 2, 0, 98); vertex( 2, -30, 98); vertex( 2, 0, 70); endShape(); } void drawQuads() { beginShape(QUADS); vertex(-100, 2, 30); vertex(-100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80); vertex( 100, 2, 30); vertex( 100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80); vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(100, -2, 30); vertex(100, 2, 30); vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, -30, 98); vertex(-2, -30, 98); vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, 0, 70); vertex(-2, 0, 70); vertex(-2, -30, 98); vertex(2, -30, 98); vertex(2, 0, 70); vertex(-2, 0, 70); endShape(); } void printQuaternions() { // Set text mode to shape textMode(SHAPE); textSize(13); fill(255, 255, 255); text("Quaternions:", 20, 20, 10); text(q[0], 20, 40, 10); text(q[1], 20, 60, 10); text(q[2], 20, 80, 10); text(q[3], 20, 100, 10); } void printYawPitchRoll() { // Calculate yaw/pitch/roll angles ypr[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1) * 57.2; ypr[1] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1) * 57.2; ypr[2] = -atan2(2 * (q[0] * q[2] - q[1] * q[3]), 1 - 2 * (q[2] * q[2] + q[1] *q[1])) * 57.2; text("Yaw:", 150, 20, 10); text(ypr[0], 150, 40, 10); text("Pitch:", 220, 20, 10); text(ypr[1], 220, 40, 10); text("Roll:", 290, 20, 10); text(ypr[2], 290, 40, 10); }
При запуске визуализации на «processinge» откроется окно с графическим отображением IMU-сенсора в виде самолёта. Самолёт на экране будет повторять перемещения IMU-сенсора в пространстве.
В качестве мозга для считывания показаний с датчика рассмотрим платформы из серии Espruino, например, Iskra JS.
Подключите IMU-сенсор к пинам питания и шины I²C — SDA и SCL платформы Iskra JS. Для коммуникации используйте соединительные провода «мама-папа».
Для быстрой сборки и отладки устройства возьмите плату расширения Troyka Shield, которая одевается сверху на Iskra JS методом бутерброда. Для коммуникации используйте трёхпроводной шлейф «мама-мама», который идёт в комплекте с датчиком.
С Troyka Slot Shield провода не понадобятся вовсе.
В качестве примера выведем в консоль следующие данные:
В качестве мозга для считывания показаний с датчика рассмотрим одноплатные компьютеры Raspberry Pi, например, Raspberry Pi 4.
Подключите датчик IMU к пинам SDA и SCL шины I²C компьютера Raspberry Pi.
Для быстрой сборки и отладки устройства возьмите плату расширения Troyka Cap, которая надевается сверху на малину методом бутерброда.
А написать пример кода для Raspberry Pi оставим вам домашним заданием.
Акселерометр выполнен на чипе LIS331DLH и представляет собой миниатюрный датчик ускорения, разработанный по технологии MEMS от компании STMicroelectronics. Адрес устройства по умолчанию равен 0x18, но может быть изменен на 0x19. Подробности читайте в разделе смена адреса модуля.
Гироскоп выполнен на чипе I3G4250D и представляет собой миниатюрный датчик перемещений в трёхмерном пространстве, разработанный по технологии MEMS от компании STMicroelectronics. Адрес устройства по умолчанию равен 0x68, но может быть изменен на 0x69. Подробности читайте в разделе смена адреса модуля.
Магнитометр выполнен на чипе LIS3MDL и представляет собой миниатюрный датчик магнитного поля в трёхмерном пространстве, разработанный по технологии MEMS от компании STMicroelectronics. Адрес устройства по умолчанию равен 0x1С, но может быть изменен на 0x1E. Подробности читайте в разделе смена адреса модуля.
Барометр выполнен на чипе LPS25HB по технологии MEMS от компании STMicroelectronics. Адрес устройства по умолчанию равен 0x5С, но может быть изменен на 0x5D. Подробности читайте в разделе смена адреса модуля.
Линейный понижающий регулятор напряжения NCP698SQ33T1G обеспечивает питание MEMS-чипов и других компонентов сенсора. Диапазон входного напряжения от 3,3 до 5 вольт. Выходное напряжение 3,3 В с максимальным выходным током 150 мА.
Преобразователь логических уровней PCA9306DCT необходим для сопряжения датчика с разными напряжениями логических уровней от 3,3 до 5 вольт. Другими словами сенсор совместим как с 3,3 вольтовыми платами, например, Raspberry Pi, так и с 5 вольтовыми — Arduino Uno.
Датчик подключается к управляющей электронике через две группы Troyka-контактов:
Иногда в проекте необходимо использовать несколько IMU-сенсоров. Для этого на модуле предусмотрены четыре пары контактных площадки, по одной на каждый чип. Для смена адреса капните каплей припоя на отведённую контактную площадку.
Модуль | Адрес без перемычки | Адрес с перемычкой |
---|---|---|
Акселерометр | 0x18 | 0x19 |
Гироскоп | 0x68 | 0x69 |
Магнитометр | 0x1С | 0x1E |
Барометр | 0x5С | 0x5D |
Модуль | Чип | I²C-адрес (без перемычки) | I²C-адрес (с перемычкой) | Максимальная чувствительность | Диапазон измерений |
---|---|---|---|---|---|
Акселерометр | LIS331DLH | 0x18 | 0x19 | 9,8×10-3 м/с² | ±2 / ±4 / ±8 g |
Гироскоп | I3G4250D | 0x68 | 0x69 | 8,7×10-3 °/с | ±250 / ±500 / ±2000 °/с² |
Магнитометр | LIS3MDL | 0x1C | 0x1E | 1,46×10-4 Гс | ±4/ ±8/ ±12/ ±16 Гс |
Барометр | LPS25HB | 0x5C | 0x5D | 2,4×10-4 мбар | 260–1260 мбар |