IMU-сенсор на 10 степеней свободы (Troyka-модуль)

IMU-сенсор (Inertial measurement unit — Инерционное измерительное устройство) — позволяет определить положение вашего девайса в пространстве. IMU-сенсор включает в себя:

  • Гироскоп, определяющий угловую скорость вокруг собственных осей X, Y, Z.
  • Акселерометр, определяющий величину ускорения свободного падения по осям X, Y, Z.
  • Компас, определяющий углы между собственными осями сенсора X, Y, Z и силовыми линиями магнитного поля Земли
  • Барометр, определяющий атмосферное давление, высоту над уровнем моря и температуру

Видеообзор

Подключение и настройка

IMU-сенсор общается с управляющей электроникой по протоколу I²C / TWI. Для подключения используются два трёхпроводных шлейфа. При подключении модуля к Arduino используйте Troyka Shield. Для обработки данных с IMU-сенсора мы написали библиотеку Troyka-IMU. Она скрывает в себе все тонкости протокола, через который передаются данные c акселерометра, гироскопа, компаса и барометра, и предоставляет простые и понятные функции для вывода значений.

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

Пример работы

В качестве примера выведем в Serial-порт следующие данные:

  • гироскопа: угловую скорость вокруг собственных осей X, Y, Z;
  • акселерометра: направление и величину ускорения свободного падения по осям X, Y, Z;
  • компаса: значения азимута;
  • барометра: абсолютное давление и температуру окружающей среды.
imu.ino
// библиотека для работы I²C
#include <Wire.h>
// библиотека для работы с модулями IMU
#include <TroykaIMU.h>
 
// создаём объект для работы с гироскопом
Gyroscope gyro;
// создаём объект для работы с акселерометром
Accelerometer accel;
// создаём объект для работы с компасом
Compass compass;
// создаём объект для работы с барометром
Barometer barometer;
 
// калибровочные значения компаса
// полученные в калибровочной матрице из примера «compassCalibrateMatrix»
const double compassCalibrationBias[3] = {
  524.21,
  3352.214,
  -1402.236
};
 
const double compassCalibrationMatrix[3][3] = {
  {1.757, 0.04, -0.028},
  {0.008, 1.767, -0.016},
  {-0.018, 0.077, 1.782}
};
 
void setup()
{
  // открываем последовательный порт
  Serial.begin(115200);
  // выводим сообщение о начале инициализации
  Serial.println("Begin init...");
  // инициализация гироскопа
  gyro.begin();
  // инициализация акселерометра
  accel.begin();
  // инициализация компаса
  compass.begin();
  // инициализация барометра
  barometer.begin();
  // калибровка компаса
  compass.calibrateMatrix(compassCalibrationMatrix, compassCalibrationBias);
  // выводим сообщение об удачной инициализации
  Serial.println("Initialization completed");
  Serial.println("Gyroscope\t\t\tAccelerometer\t\t\tCompass\t\tBarometer");
}
 
void loop()
{
  // вывод угловой скорости в градусах в секунду относительно оси X
  Serial.print(gyro.readDegPerSecX());
  Serial.print("\t");
  // вывод угловой скорости в градусах в секунду относительно оси Y
  Serial.print(gyro.readDegPerSecY());
  Serial.print("\t");
  // вывод угловой скорости в градусах в секунду относительно оси Z
  Serial.print(gyro.readDegPerSecZ());
  Serial.print("\t\t");
  // вывод направления и величины ускорения в м/с² по оси X
  Serial.print(accel.readAX());
  Serial.print("\t");
  // вывод направления и величины ускорения в м/с² по оси Y
  Serial.print(accel.readAY());
  Serial.print("\t");
  // вывод направления и величины ускорения в м/с² по оси Z
  Serial.print(accel.readAZ());
  Serial.print("\t\t");
  // выводим азимут относительно оси Z
  Serial.print(compass.readAzimut());
  Serial.print(" Degrees\t");
  // вывод значения абсолютного давления
  Serial.print(barometer.readPressureMillibars());
  Serial.print("\t");
  // вывод значения температуры окружающей среды
  Serial.print(barometer.readTemperatureC());
  Serial.print("\t");
  Serial.println("");
  delay(100);
}

Ориентация объекта в пространстве

Представьте, что вы находитесь в самолёте. Ось X проходит от хвоста к носу самолёта. Ось Y идёт от левого крыла к правому. Ось Z направлена снизу вверх. Определим ориентацию самолёта в пространстве с помощью углов Эйлера.

  • Угол Yaw (рысканье) — вращение вокруг оси Z. Поворот носа влево, а хвоста вправо (и наоборот).
  • Угол Pitch (тангаж) — вращение вокруг оси Y. Опускает нос самолёта и задирает хвост (и наоборот).
  • Угол Roll (крен) — вращение вокруг оси X. Поднимает одно крыло вверх, опуская другое вниз.

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

В фильтре Маджвика реализовано два варианта определение ориентации объекта в пространстве.

  • Первый вариант применим к инерционным навигационным системам (ИНС), включающим акселерометр и гироскоп. При этом направлением на север пренебрегаем.
  • Второй вариант применим к ИНС, включающих дополнительно трёхосевой магнитометр.

Пример фильтра без магнитометра

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

Madgwick6DOF.ino
// библиотека для работы I²C
#include <Wire.h>
// библиотека для работы с модулями IMU
#include <TroykaIMU.h>
 
// множитель фильтра
#define BETA 0.22f
 
// создаём объект для фильтра Madgwick
Madgwick filter;
 
// создаём объект для работы с акселерометром
Accelerometer accel;
// создаём объект для работы с гироскопом
Gyroscope gyro;
 
// переменные для данных с гироскопов, акселерометров
float gx, gy, gz, ax, ay, az;
 
// получаемые углы ориентации
float yaw, pitch, roll;
 
// переменная для хранения частоты выборок фильтра
float fps = 100;
 
void setup()
{
  // открываем последовательный порт
  Serial.begin(115200);
  Serial.println("Begin init...");
  // инициализация акселерометра
  accel.begin();
  // инициализация гироскопа
  gyro.begin();
  // выводим сообщение об удачной инициализации
  Serial.println("Initialization completed");
}
 
void loop()
{
  // запоминаем текущее время
  unsigned long startMillis = millis();
 
  // считываем данные с акселерометра в единицах G
  accel.readGXYZ(&ax, &ay, &az);
  // считываем данные с акселерометра в радианах в секунду
  gyro.readRadPerSecXYZ(&gx, &gy, &gz);
  // устанавливаем коэффициенты фильтра
  filter.setKoeff(fps, BETA);
  // обновляем входные данные в фильтр
  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;
  // вычисляем частоту обработки фильтра
  fps = 1000 / deltaMillis;
}

Пример фильтра c магнитометром

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

Madgwick9DOF.ino
// библиотека для работы I²C
#include <Wire.h>
// библиотека для работы с модулями IMU
#include <TroykaIMU.h>
 
// множитель фильтра
#define BETA 0.22
 
// создаём объект для фильтра Madgwick
Madgwick filter;
 
// создаём объект для работы с акселерометром
Accelerometer accel;
// создаём объект для работы с гироскопом
Gyroscope gyro;
// создаём объект для работы с компасом
Compass compass;
 
// переменные для данных с гироскопа, акселерометра и компаса
float gx, gy, gz, ax, ay, az, mx, my, mz;
 
// получаемые углы ориентации (Эйлера)
float yaw, pitch, roll;
 
// переменная для хранения частоты выборок фильтра
float fps = 100;
 
// калибровочные значения компаса
// полученные в калибровочной матрице из примера «compassCalibrateMatrixx»
const double compassCalibrationBias[3] = {
  524.21,
  3352.214,
  -1402.236
};
 
const double compassCalibrationMatrix[3][3] = {
  {1.757, 0.04, -0.028},
  {0.008, 1.767, -0.016},
  {-0.018, 0.077, 1.782}
};
 
void setup()
{
  // открываем последовательный порт
  Serial.begin(115200);
  Serial.println("Begin init...");
  // инициализация акселерометра
  accel.begin();
  // инициализация гироскопа
  gyro.begin();
  // инициализация компаса
  compass.begin();
 
  // калибровка компаса
  compass.calibrateMatrix(compassCalibrationMatrix, compassCalibrationBias);
  // выводим сообщение об удачной инициализации
  Serial.println("Initialization completed");
}
 
void loop()
{
  // запоминаем текущее время
  unsigned long startMillis = millis();
 
  // считываем данные с акселерометра в единицах G
  accel.readGXYZ(&ax, &ay, &az);
  // считываем данные с гироскопа в радианах в секунду
  gyro.readRadPerSecXYZ(&gx, &gy, &gz);
  // считываем данные с компаса в Гауссах
  compass.readCalibrateGaussXYZ(&mx, &my, &mz);
 
  // устанавливаем коэффициенты фильтра
  filter.setKoeff(fps, BETA);
  // обновляем входные данные в фильтр
  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;
  // вычисляем частоту обработки фильтра
  fps = 1000 / deltaMillis;
}

Отображение объекта в пространстве

Для наглядности приведём пример отображение IMU-сенсора в виде самолёта в графической среде Processing.

Код для Arduino

MadgwickProcessing9DOF.ino
// скетч для вывода кватернионов в serial-порт
// для дальнейшего графического просмотра ориентации объекта
// в среде processing «MadgwickProcessingDraw9DOF.pde»
 
// библиотека для работы I²C
#include <Wire.h>
// библиотека для работы с модулями IMU
#include <TroykaIMU.h>
 
// множитель фильтра
#define BETA 0.22
 
// создаём объект для фильтра Madgwick
Madgwick filter;
 
// создаём объект для работы с акселерометром
Accelerometer accel;
// создаём объект для работы с гироскопом
Gyroscope gyro;
// создаём объект для работы с компасом
Compass compass;
 
// переменные для данных с гироскопа, акселерометра и компаса
float gx, gy, gz, ax, ay, az, mx, my, mz;
 
// получаемые углы ориентации (Эйлера)
float yaw, pitch, roll;
 
// переменная для хранения частоты выборок фильтра
float fps = 100;
 
// калибровочные значения компаса
// полученные в калибровочной матрице из примера «compassCalibrateMatrixx»
const double compassCalibrationBias[3] = {
  524.21,
  3352.214,
  -1402.236
};
 
const double compassCalibrationMatrix[3][3] = {
  {1.757, 0.04, -0.028},
  {0.008, 1.767, -0.016},
  {-0.018, 0.077, 1.782}
};
 
void setup()
{
  // открываем последовательный порт
  Serial.begin(115200);
  // инициализация акселерометра
  accel.begin();
  // инициализация гироскопа
  gyro.begin();
  // инициализация компаса
  compass.begin();
  // калибровка компаса
  compass.calibrateMatrix(compassCalibrationMatrix, compassCalibrationBias);
}
 
void loop()
{
  // запоминаем текущее время
  unsigned long startMillis = millis();
  // считываем данные с акселерометра в единицах G
  accel.readGXYZ(&ax, &ay, &az);
  // считываем данные с гироскопа в радианах в секунду
  gyro.readRadPerSecXYZ(&gx, &gy, &gz);
  // считываем данные с компаса в Гауссах
  compass.readCalibrateGaussXYZ(&mx, &my, &mz);
  // устанавливаем коэффициенты фильтра
  filter.setKoeff(fps, BETA);
  // обновляем входные данные в фильтр
  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.readQuaternions(&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;
  // вычисляем частоту обработки фильтра
  fps = 1000 / deltaMillis;
}

Код для Processing

Для работы ниже приведённого кода скачайте библиотеки для Processing и распакуйте в папку хранения библиотек для Processing [userdir]/Processing/libraries

MadgwickProcessingDraw9DOF.pde
import processing.serial.*;
import toxi.geom.*;
import toxi.processing.*;
 
// 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
int newLine = 13; 
String [] massQ = new String [4];
float[] ypr = new float[3];
 
void setup() 
{
  // size form 300x300
  size(400, 400, P3D); 
  // open serial port
  // replace "COM29" with the COM port on which your arduino is connected
  port = new Serial(this, "COM29", 115200);
  // wait init Serial for Arduino
  delay(1000);
}
 
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 Quards
  drawQuards();
  // end of object
  popMatrix();
  // send 's' to Arduino
  port.write('s');
}
 
void serialEvent() { // Function for parsing serial message
  // 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 drawQuards() {
  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() {
  textMode(SHAPE); // set text mode to 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.0f * (q[0] * q[2] - q[1] * q[3]), 1.0f - 2.0f * (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-сенсора в пространстве.

Элементы платы

Гироскоп на L3G4200D

Гироскоп основан на чипе L3G4200D и представляет собой миниатюрный датчик перемещений в трёхмерном пространстве, выполненный по технологии MEMS компании STMicroelectronics в корпусе LGA-16 (4x4x1.1 mm). Общение гироскопа с управляющей электроникой осуществляется по протоколу I²C / TWI. Адрес устройства равен 0b1101000.

Акселерометр на LIS331DLH

Акселерометр основан на чипе LIS331DLH и представляет собой миниатюрный датчик ускорения выполненный по технологии MEMS компании STMicroelectronicsd в корпусе LGA 16 (3x3x1 мм). Общение акселерометра с управляющей электроникой осуществляется по протоколу I²C / TWI. Адрес устройства равен 0b0011000.

Компас на LIS3MDL

Компас основан на чипе LIS3MDL и выполнен по технологии MEMS компании STMicroelectronics в корпусе LGA-12 (2.0x2.0x1.0 мм). Общение компаса с управляющей электроникой осуществляется по протоколу I²C / TWI. Адрес устройства равен 0b0011100.

Барометр на LPS331

Барометр основан на чипе LPS331 и выполнен по технологии MEMS компании STMicroelectronics в корпусе HCLGA-16L (3 x 3 x 1 mm). Общение барометра с управляющей электроникой осуществляется по протоколу I²C / TWI. Адрес устройства равен 0b1011100.

Обвязка для согласования уровней напряжения

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

Контакты подключения 3-проводных шлейфов

Контакты питания:

  • Земля (G) — чёрный провод. Соедините с пином GND Arduino.
  • Питание (V) — красный провод. Соедините с пином 5V Arduino.
  • Не используется.

Контакты шины I²C:

  • Сигнальный (D) — Подключите к SDA пину Arduino.
  • Сигнальный (С) — Подключите к SCL пину Arduino.
  • Не используется.

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

Характеристики

  • Напряжение питания: 3.3–5 В
  • Выходной интерфейс: I²C
  • Диапазон измерения:
    • Гироскопа: ±250/±500±2000 dps
    • Акселерометра: ±2g/±4g/±8g
    • Компаса: ±4/±8/±12/±16 Гаусс
    • Барометра: 260…1260 мбар
  • Частота обновления сигнала:
    • Гироскопа: 100…800 Гц
    • Акселерометра: 0.5…1000 Гц
    • Компаса: 0.625…80 Гц
    • Барометра: 1…25 Гц
  • Температурный диапазон: −40…+85 °C
  • Габариты: 25×25 мм