Pull to refresh

BalanceBall. Самобалансирующая платформа от А до Я

C++
Sandbox
Я студент Физтеха, факультета радиотехники и кибернетики, и как положено всем второкурсникам, в весеннем семестре надо сделать проект по информатике и микроконтроллерам. Я решил убить двух зайцев одним выстрелом и объединил два проекта в один.

Долго выбирал тему проекта. Из идей была игра, похожая на Guitar Hero, но контроллером были твои руки, настукивающие барабанные биты по столу. Также задумывался над созданием светодиодной матрицы, управляемой по Bluetooth с компьютера, со своим простеньким скриптовым языком для написания анимаций. Задумки были интересными, но я все равно казались чем-то не тем. Потом я случайно наткнулся на это видео и сразу загорелся идеей реализовать что-то подобное. Вот что у меня вышло в итоге:



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

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


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

1. Веб камера передает изображение на ноутбук
2. По изображению определяются координаты шарика
3. Координаты подаются на вход PID регуляторов (2 независимых регулятора по X и по Y), и регуляторы считают углы, на которые должны повернуться сервоприводы
4. Углы передаются на Arduino и выставляются на сервоприводах

Изобразить это можно так:

image

Проектирование механики платформы


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

Сервоприводы решил брать посильнее, чтобы быть уверенным, что они смогут наклонить платформу. Остановился на TOWER PRO MG996R, с моментом 9.4 кг/см.

image

Платформу, тяги, некоторые крепления решено было вырезать из орг стекла на лазерном резаке (благо он есть у нас в институте). Проектировал все в SolidWorks. Снизу на платформу было решено прикрепить крестовину, чтобы уже на нее крепить и шаровую опору, и угловые крепления для тяг. Вот несколько фотографий процесса создания:

Paltform
Fixer

Начал искать шаровые опоры. Найти подходящую было довольно сложно, и притом стоили они дорого. Мне подсказали, что вместо шаровой можно использовать карданчик, как на ключах. Купил карданчик, большой болт (на нем все держится) и гайки. К карданчику приварил гайку. Вот что получилось:

Cardan

Сборка платформы


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

image

Из фанеры вырезал квадратную площадку, на которую все крепил. Вот так выглядела платформа в собранном виде:

image

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

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

Первый опыт


После полной сборки написал простенький скетч для Arduino, который наклонял платформу на некоторые углы.



Все вроде бы работало, и было сложно в это поверить.

Немного поигравшись с устройством, определил углы, когда платформа параллельна полу, определил максимальный и минимальный углы (±13 градусов). Настало время писать код.

Написание кода


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

1. Компьютерное зрение
2. Реализация PID регулятора
3. Общение с Arduino по serial порту
4. Скетч для Arduino

Все писал на C++. Весь код можно посмотреть в этом репозитории.

Компьютерное зрение


Использовал OpenCV. Детектирование объекта решил делать по цвету. Во многом мне помогла эта статья. Алгоритм детектирования объекта можно разложить на несколько шагов:

1. Получение кадра с камеры
2. Конвертация из RGB в HSV
3. Пропускание через маску некоторого цвета (в моем случае белого)
4. Расчет моментов изображения
5. Расчет координат шарика по моментам

Реализация PID-регулятора


Прочитав на Википедии про PID регуляторы и посмотрев несколько уже готовых реализаций на github, мне не составило труда реализовать его на C++. На Хабре я видел немало статей про PID-регуляторы, поэтому на нем останавливаться не буду.

Общение с Arduino по serial-порту


Весь код я писал и запускал на OSX, поэтому проблем с общением по serial порту не было. Углы на Arduino я посылал в виде строчки "[ANGLE_X]:[ANGLE_Y]$", где ANGLE_X и ANGLE_Y отвечали соответственно за наклон по своей оси и менялись в пределах от -100 до 100 (углы в процентах). Чтобы отослать эту строчку, я просто писал в файл вида "/dev/tty.usbmodem1421".

Скетч для Arduino


Схема подключения выглядела так:

image

Arduino получала по serial порту строчку, парсила ее и после выставляла на сервоприводах нужные углы.

Заключение


На создание BalanceBall я потратил примерно 2 месяца. Самым сложным оказалась реализация механики. PID настраивал экспериментально. В планах создание математической модели платформы и теоретический расчет коэффициентов PID-регуляторов.

Всем спасибо за внимание!

Ссылки


Репозиторий проекта: github.com/karfly/balanceball
Папка с материалами: goo.gl/bmkexm

ПИД-регулятор
Дискретный ПИД регулятор
Color Detection & Object Tracking
Tags:ArduinoC++OpenCVPID controller
Hubs: C++
Total votes 65: ↑62 and ↓3+59
Views30K

Popular right now