Программа и тест

2D-гироподвес на основе Ардуино, MPU6050 и SG90

Гироподвес или гиростабилизированный подвес — это устройство стабилизации положения некоторого переносимого объекта относительно положения носителя. Часто гироподвес применяется для стабилизации видео или фотокамеры. Такие подвесы ещё называют стэдикам (steadicam).

Мы соберем свой 2х-осевой стэдикам при помощи Ардуино, акселерометра и гироскопа MPU6050, а также самых простых сервомоторов SG90.

Как работает гироподвес?

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

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

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

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

Примечание №1. А что, глаза разве не участвуют в оценке положения головы? Участвуют. Но если их закрыть, то вы легко справитесь с задачей стабилизации. Исключение составляют сложные задачи, когда одного чувства бывает недостаточно. Например, стойка на одной ноге с закрытыми глазами. Попробуйте. Хотя, и этому можно научиться — мозг очень гибкая штука.

Примечание №2. Почему мы стабилизируемся относительно горизонта? Ведь мы его не видим! Формально, мы стабилизируем голову не вдоль горизонта, а вдоль вектора гравитации. Почитайте как работает акселерометр. Понятие горизонта здесь применяется для удобства восприятия.

Примечание №3. Механический стэдикам?? Для понимания его устройства, можно провести очень простой эксперимент. Попробуйте привязать к обычной гире веревку. Камеру закрепить непосредственно на гире. Второй конец зажмите в руку и поднимите всё этот дело на нужную высоту. Как бы не наклоняли кулак с зажатой веревкой, и как бы не дрожала ваша рука, камера будет снимать достаточно стабильно. Передвигать гирю следует медленно, дабы не превратить всю конструкцию в маятник. Реальный механический стэдикам устроен немного иначе, но физический смысл явления останется тот же.

Лучший способ сделать что-то правильно с первого раза — начертить это. В случае изготовления деталей на 3D-принтере или на ЧПУ станке, без этого вообще не обойтись. Будем чертить узлы гироподвеса в Fusion360.

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

  • крепление подвеса к корпусу
  • узел вращения (roll) — наклоны влево-вправо
  • узел тангажа (pitch) — наклоны вниз-вверх

На третьей детали будут крепиться датчик MPU6050 и камера для Raspberry Pi версии 1.3, поэтому для них предусмотрены крепежные отверстия.

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

2D-гироподвес

Ссылки на все детали в формате STL, вместе с чертежом fusion360, можно найти на последней странице.

Детали были распечатаны на 3D-принтере из PLA пластика. Собранное устройство (пока без камеры) выглядит так:

Гироподвес на Ардуино и MPU6050
Гироподвес на Ардуино и MPU6050

Подключение гироподвеса к Ардуино

Когда конструкция собрана, можно приступить к подключению гироподвеса к контроллеру. Используем стандартную схему подключения I2C устройств, но с добавлением двух сервомоторов на цифровые GPIO №2 и №3.

MPU6050 ROCGNDVCCSDASCL
Сервомотор вращенияGND VCC сигнал
Сервомотор тангажаGND VCC сигнал
Ардуино УноGND+5VA4A523

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

  1. получаем данные с датчиков положения
  2. вычисляем на какой угол нужно повернуть сервомоторы
  3. поворачиваем сервомоторы
  4. переходим к пункту 1

Исходный код программы:

include "I2Cdev.h"
include "MPU6050.h"
include "MadgwickAHRS.h"
include <Servo.h>

define TO_RAD 0.01745329252f
define PITCH_DIR -1 // 1 - направление сервомотора и датчика совпадают, -1 - нет
define PITCH_DEFAULT -90 // начальный угол датчика
define ROLL_DIR -1
define ROLL_DEFAULT 0
define PITCH 0
define ROLL 1
define LPF 0.7

MPU6050 accelgyro;

Servo servos[2]; // тангаж (pitch), вращение (roll)
int angle[2] = {0,0}; // pitch, roll

unsigned long tm, imu_t, stab_t;
const unsigned int imu_to = 20; // каждые 20 мс опрашиваем датчик
const unsigned int stab_to = 50; // каждые 50 мс управляем сервомоторами
const unsigned int calib_to = 1000; // 1 секунду после старта калибруемся

float tdelta;
int16_t ax, ay, az;
int16_t gx_raw, gy_raw, gz_raw;
float imu[3];
float imu_f[3];
float quat[4];
bool calibrated = 0;

void setup() {
  Serial.begin(9600);
  Serial.println("Initializing I2C devices…");
  accelgyro.initialize(); // инициализация датчиков
  Serial.println("Testing device connections…");
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful!" : "MPU6050 connection failed");
  servos[0].attach(2);
  servos[1].attach(3);
}

int clamp (int a, int mn, int mx){
  if( a>mx ) return mx;
  if( a<mn ) return mn;
  return a;
}

void loop() {
  // каждые 20мс вычисляем углы наклона
  tm = millis();
  if (imu_t + imu_to < tm) {
    tdelta = tm - imu_t; // вычисляем дельту времени в миллисекундах
    imu_t = tm;
    accelgyro.getMotion6(&ax, &ay, &az, &gx_raw, &gy_raw, &gz_raw);

    // преобразуем сырые данные гироскопа в рад/с
    float gx = gx_raw * TO_RAD / 131.0;
    float gy = gy_raw * TO_RAD / 131.0;
    float gz = gz_raw * TO_RAD / 131.0;

    // вызываем алгоритм фильтра Мажвика
    MadgwickAHRSupdateIMU(tdelta/1000.0, gx, gy, gz, (float)ax, (float)ay, (float)az);
    quat[0] = q0; quat[1] = q1; quat[2] = q2; quat[3] = q3;
    // преобразуем кватернион в углы Эйлера
    quat2Euler(&quat[0], &imu[0]);
    // применяем фильтр низких частот
    imu_f[0] = imu_f[0]LPF + imu[0](1-LPF); imu_f[1] = imu_f[1]LPF + imu[1](1-LPF); imu_f[2] = imu_f[2]LPF + imu[2](1-LPF);
  }

  // первые 1000мс тратим на калибровку
  tm = millis();
  if (!calibrated && (calib_to < tm)) {
    calibrated = 1;
    imu_f[0] = imu[0];
    imu_f[1] = imu[1];
    imu_f[2] = imu[2];
  }

  // каждые 50мс управляем сервомоторами
  tm = millis();
  if (calibrated && (stab_t + stab_to < tm)) {
    stab_t = tm;
    float a = 0;
    // PITCH
    a = imu_f[0]/TO_RAD + PITCH_DEFAULT;
    if( abs(a)>2 ){
      angle[PITCH] = clamp(angle[PITCH] + a*PITCH_DIR, 0, 180);
      servos[PITCH].write(angle[PITCH]);
    }
    // ROLL
    a = imu_f[1]/TO_RAD + ROLL_DEFAULT;
    if( abs(a)>2 ){
      angle[ROLL] = clamp(angle[ROLL] + a*ROLL_DIR, 0, 180);
      servos[ROLL].write(angle[ROLL]);
    } 
  }
}

Загружаем программу на Ардуино и пробуем шевелить гироподвес за основное крепление.

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

Библиотека для Ардуино:

https://github.com/robotclass/RobotClass-MadgwickAHRS

Чертежи гироподвеса:

https://www.thingiverse.com/thing:3772375

0

Изменено: