Интернет-лаборатория роботов ZiZiBOT.RU

Проектирование и разработки в области робототехники и автоматизации технологических процессов. Производство готовых роботов и конструкторов для творчества. Консультации и обучение по электронике и программированию.

г. Юрга,
ул.Ленинградская 38/83

+7 923-503-6074

Балансирующий робот на шаговых моторах

Собираем балансирующего робота на шаговых моторах

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

Программные листинги

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

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

Начинаю использовать шаговики

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

Я посоветовался с теми, кто уже пробовал делать подобных роботов, но брать чужие механики за основу не стал. Меня заводила именно идея конструирования и моделирования ПО для робота самотсоятельно.

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

Первоначальные схемы робота я привожу:

Начальная схема питания
Схема управления
Схема управления

В дальнейшем моторы были заменены на более мощные NEMA 17 (17hs4401) с номинальным током 1.7А и резистивным сопротивлением обмоток 1.8Ом . У старых двигателей номинальный ток был неизвестен, а вот сопротивление обмоток было около 26 Ом, что непозволяло мотору развивать достаточную мощность.

Собственно чего стесняться, приведу ролик про сборку данного робота:

Комплектующие для балансирующего робота

№ пункта Название Количество
1 Корпус (я смоделировал его и выфрезеровал из 3мм фанеры), но можно сделать из пластиковых линеек 1
2 бокс аккумуляторный (стандартный подпружиненый, по 2шт аккумулятора в бокс) 2
3 аккумумляторы 18650 литиевый 3.7В (соединены последовательно), желательно высокого качества 4
4 драйвер шаговых моторов DRV8825, потребуется установить радиатор, я установил его с обратной стороны на плату (на рассеивающую метализацию) 2
5 выключатель (должен держать импульсные токи до 4 ампер) 1
6 гироcкоп-акселерометр MPU-6050 (берегитесь подделки) 1
7 стабилизатор напряжения 5В  (7805),  (питание BT-приемопередатчика), данный стабилизатор позволил оставлять HC-05 без напряжения, при подключении робота к компьютеру по USB. 1
8 конденсаторы  электролиты 16в 200мкф (требуются на вход и выход стабилизатора 7805 5в) 2
9 конденсаторы электролиты 25в 2000мкф (на входы питания шаговых драйверов - по инструкции подключения) 2
10 конденсаторы керамика 104 100нф (везде где только можно) на питание гироскопа, arduino, HC-05 (убираем высокачастотные помехи) 2
11 провода силовые и сигнальные (толстые и тонкие) по потребности
12 Bluetooth-приемопередатчик HC-05/HC-06 (через него рулим роботом) 1
13 шаговые моторы NEMA 17 (17hs4401) 2
14  Колеса капролоновые 77мм диаметр (самофрезерованные) 2
15 Arduino NANO (не лучший вариант - лучше Mega или ESP8266), но что было под рукой 1
16 Диод на цепь питания Arduino NANO. Подцепил их последовательно для снижения питания от 4 последовательно соединенных 18650 (напряжение может достигать 18в), а каждый диод гасит немного на себе. Кроме того при программирование напряжение не наводится на силовые цепи робота 4

Математические выкладки

Длина шага моего робота при настройке делителя шага на 4:

, где r – радиус колеса

Расстояние, которое должен преодолеть робот, чтобы компенсировать свой наклон:

,

где – собственно расстояние,
– расстояние от пола до центра масс робота, - угол наклона робота,
или приближенно:

,

где – угол наклона робота в радианах (при углах до ).

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

,

- требуемое количество шагов робота.

Данное количество шагов мы будем втискивать в рамки некоторого времени, а это уже не что иное, как скорость! И дополнительно приложим к нему коэффициент – получили пропорциональное звено.

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

.

В старых роботах я это не учитывал!!!

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

,

где - скорость изменения угла падения робота, - коэффициент влияния скорости падения робота на результат (скорость шагового привода).


Теперь зададим себе вопрос, а что если ось равновесия робота немного отклонена? И расчетное значение нуля по показаниям гироскопа с акселерометром (комплементарного фильтра) не будут совпадать с реальным значением сбалансированного положения робота.

У меня это несовпадение было около 3-х градусов.

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


Этим займется Интегральное звено.

, где – значение интеграла, – коэффициент влияния интегрального звена (как правило, он меньше 1.0).

И итоговая формула выглядит так:

,

А если на чистоту, то у меня получилось так:

,где – немного больше 1.0

Теперь перейду к программе, я попытался максимально сэкономить на расчетах,

Запрос состояния MPU-5060 занимает не менее 1.4 миллисекунды, мои расчеты около 0.6 миллисекунд в старой программе.

Но теперь придется управлять шаговыми моторами, а значит работать с прерываниями.

Я программировал на Arduino NANO (ATmega328), у данного контроллера только один 16 разрядный таймер, а управлять нужно отдельно шагами двух моторов!

Вот что получилось, я использовал библиотеку Timer1, хотя и не очень обоснованно:


код

Запрос к гироскопу акселерометру проходит каждые 5 милисекунд (200 раз в секунду), если реже, то накапливается погрешность, теряется точность

А вот корректировка скорости робота осуществляется всего 10 раз в секунду.

Много ресурсов забирает прерывание, которое срабатывает каждые 12 микросекунд и запускает перерасчет шагов обоих моторов. Если использовать контроллер, который имеет 2а 16ти-разрядных таймера, этой проблемы можно избежать (например, ArduinoMega). Более того, становиться возможным использование микрошага 8 и 16, что повышает точность отработки робота.

Также много ресурсов потребляла эмуляция последовательного порта библиотекой SoftwareSerial, от нее пришлось отказаться, а управление роботом осуществляется по D0-D1 аппаратному порту, который не расходует ресурсы основного логического ядра контроллера - реализован аппаратно.

Чтобы BT-адаптер не мешал при прошивке робота, он запитывается отдельно.


Перед включением, робота следует положить и не трогать, он подсчитает смещение нуля гироскопа, а когда замигает 13 светодиод (D13) (через 3 сек.) поднять его в положение балансировки.

Собственно пока все, программу прилагаю, схема робота в предыдущем ролике.

Забыл рассказать, за счет чего робот двигается:

,где - заданная скорость робота.

А повороты осуществляются смещением скоростей колес в разную сторону на константу.

Программные листинги


X

Написать сообщение:

Укажите свой номер телефона или e-mail для обратной связи
- e-mail
ИЛИ
- номер телефона

Текст сообщения: