X
Собираем балансирующего робота на шаговых моторах
Основная идея: собрать робота, который сможет держать равновесие на двух колесах. Под сборкой подразумевается конструирование и программа балансировки.
Программные листинги
Проблемой балансировки я занялся сравнительно давно, а если точно около двух лет назад. Стумулом к этому послужило написание книги "Мобильные роботы на базе 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 сек.) поднять его в положение балансировки.
Собственно пока все, программу прилагаю, схема робота в предыдущем ролике.
Забыл рассказать, за счет чего робот двигается:
,где 
- заданная скорость робота.
А повороты осуществляются смещением скоростей колес в разную сторону на константу.