Полезные ссылки
2D-гироподвес на основе Ардуино, MPU6050 и SG90
Гироподвес или гиростабилизированный подвес — это устройство стабилизации положения некоторого переносимого объекта относительно положения носителя. Часто гироподвес применяется для стабилизации видео или фотокамеры. Такие подвесы ещё называют стэдикам (steadicam).
Мы соберем свой 2х-осевой стэдикам при помощи Ардуино, акселерометра и гироскопа MPU6050, а также самых простых сервомоторов SG90.
Как работает гироподвес?
Каждый человек и большинство животных имеет вестибулярный аппарат, который нужен нам для поддержания устойчивого положения тела и головы. Собственно, поскольку этот природный прибор находится у нас в голове, то стабилизируется прежде всего голова, а тело уже стремится принять правильное положение в угоду голове.
Вестибулярный аппарат — это акселерометр, данные с которого непрерывно интерпретируются нашим мозгом для дальнейшей работы с телом. Если мы приделаем видеокамеру к своей голове, то получим вполне сносный гироподвес. Можно даже прикрепить камеру к голове какой-нибудь птицы, как это было реализовано в одном известном ролике. Но птица, как и любое другое животное, вряд ли станет все время смотреть в нужную вам сторону.
Так что же происходит у нас в голове? Предположим, что наше наше находится в строго вертикальном положении и шевелить мы можем только мышцами шеи. Вестибулярный аппарат говорит мозгу, насколько сильно наклонена голова относительно горизонта. Согласно этой оценке, мы напрягаем соответствующие мышцы шеи и наклоняем голову в нужную сторону, пока не достигнем правильного положения.
Точно также работает любой электронный стэдикам (бывают и механические). Датчик положения, который находится в той же части устройства, что и камера, фиксирует текущий угол наклона относительно горизонта. Микроконтроллер управляет моторами гироподвеса, поворачивая камеру (и датчик вместе с ней) в заданное положение. Эти действия происходят непрерывно много раз в секунду. В итоге, оператор может как угодно передвигать и поворачивать стэдикам в пространстве, а камера при этом всегда будет смотреть строго вдоль горизонта.
Примечание №1. А что, глаза разве не участвуют в оценке положения головы? Участвуют. Но если их закрыть, то вы легко справитесь с задачей стабилизации. Исключение составляют сложные задачи, когда одного чувства бывает недостаточно. Например, стойка на одной ноге с закрытыми глазами. Попробуйте. Хотя, и этому можно научиться — мозг очень гибкая штука.
Примечание №2. Почему мы стабилизируемся относительно горизонта? Ведь мы его не видим! Формально, мы стабилизируем голову не вдоль горизонта, а вдоль вектора гравитации. Почитайте как работает акселерометр. Понятие горизонта здесь применяется для удобства восприятия.
Примечание №3. Механический стэдикам?? Для понимания его устройства, можно провести очень простой эксперимент. Попробуйте привязать к обычной гире веревку. Камеру закрепить непосредственно на гире. Второй конец зажмите в руку и поднимите всё этот дело на нужную высоту. Как бы не наклоняли кулак с зажатой веревкой, и как бы не дрожала ваша рука, камера будет снимать достаточно стабильно. Передвигать гирю следует медленно, дабы не превратить всю конструкцию в маятник. Реальный механический стэдикам устроен немного иначе, но физический смысл явления останется тот же.
Лучший способ сделать что-то правильно с первого раза — начертить это. В случае изготовления деталей на 3D-принтере или на ЧПУ станке, без этого вообще не обойтись. Будем чертить узлы гироподвеса в Fusion360.
Всего нам потребуется 3 детали:
- крепление подвеса к корпусу
- узел вращения (roll) — наклоны влево-вправо
- узел тангажа (pitch) — наклоны вниз-вверх
На третьей детали будут крепиться датчик MPU6050 и камера для Raspberry Pi версии 1.3, поэтому для них предусмотрены крепежные отверстия.
Детали достаточно простые, для черчения потребовались только планарные чертежи и инструменты выдавливания. Общий вид устройства представлен на картинке ниже.
Ссылки на все детали в формате STL, вместе с чертежом fusion360, можно найти на последней странице.
Детали были распечатаны на 3D-принтере из PLA пластика. Собранное устройство (пока без камеры) выглядит так:
Подключение гироподвеса к Ардуино
Когда конструкция собрана, можно приступить к подключению гироподвеса к контроллеру. Используем стандартную схему подключения I2C устройств, но с добавлением двух сервомоторов на цифровые GPIO №2 и №3.
| MPU6050 ROC | GND | VCC | SDA | SCL | ||
| Сервомотор вращения | GND | VCC | сигнал | |||
| Сервомотор тангажа | GND | VCC | сигнал | |||
| Ардуино Уно | GND | +5V | A4 | A5 | 2 | 3 |
Как уже было написано выше, алгоритм работы гироподвеса должен выглядеть следующим образом:
- получаем данные с датчиков положения
- вычисляем на какой угол нужно повернуть сервомоторы
- поворачиваем сервомоторы
- переходим к пункту 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
