Предыстория
Некоторое время назад я сделал обратный маятник. После нескольких итераций с шаговыми моторами равновесия достичь не удалось. Тогда у меня было мало опыта и понимания проблемы, поэтому я переделал его с мотором постоянного тока, как большинство учебных проектов. Однако, встречаются статьи о том, как сделать балансирующего робота на шаговых моторах. По сути это одна и та же задача, поэтому я к ней решил вернуться и разобраться с прошлых неудачах. Ниже я опишу все, что нужно знать, чтобы сделать своего робота, и трудности, с которыми столкнулся.
Постановка задачи
Сделать балансирующего робота из доступных компонентов с возможностью управления и дальнейшего расширения как платформы.
Используемое железо:
Arduino Nano (Keywish BLE-Nano) последовательный порт аппаратно проброшен через Bluetooth, что устраняет расходы процессора на коммуникацию. Можно использовать классический контроллер Arduino Nano, код совместим, но не будет управления
MPU6050 - популярный инерциальный модуль для определения угла наклона робота
A4988 x2 - драйверы шаговых моторов
Nema17 motor x2 - шаговые моторы
Цена компонентов: ~75$
В чем привлекательность использования шаговых моторов? В задачах удержания равновесия нужно создавать силу, действующую на тело робота или стержень, в случае обратного маятника. Значит, нужно управлять угловым ускорением вала. В случае с шаговыми моторами это можно сделать программно, без обратной связи, если предположить, что двигатель не пропускает шаги. Однако, тут важно своевременно формировать импульсы на шаг. Об этом речь пойдет ниже. В случае с коллекторным мотором нужен энкодер для обратной связи и контур управления моментом.
Мат. модель
Движение робота описывается следующими уравнениями:
где R - радиус колеса; I = 1/2*M*R^2 - момент инерции колеса; b1 - трение в оси колеса; b2 - трение качения.
Моделирование с разными способами управления приводится здесь. Так, например, выглядит свободная модель без трения и управления, и со стабилизацией.
Алгоритм стабилизации
Для удержания равновесия нужна обратная связь по углу, а чтобы робот не уехал со стола в процессе отладки, нужно занулить скорость, поэтому схема выглядит так:
Контур скорости задает желаемый угол, а внутренний контур его достигает. В реальности координата X не измеряется, в ходе вычислений уже есть скорость V, которую, как предполагается, шаговый мотор точно выдает.
Управление скоростью
Управление скоростью автоматически получилось по схеме выше: чтобы заставить робот ехать вперед или назад, нужно задать входную скорость.
Но как быть с поворотом? Если добавить константу скорости прямо на двигатель, это не повлияет на равновесие, т.к. ускорение не изменится. Предполагается, что скорость, которую задает пользователь, изменяется гораздо медленее, чем реагирует робот на изменение угла. Тогда для поворота можно добавить скорость к одному двигателю и вычесть у другого. Стоит обратить внимание на насыщение мотора, если поворачивать очень быстро, то при попытке балансировать скорости на одном колесе сложатся, момент упадет, и двигатель начнет пропускать шаги; либо контроллер не будет успевать генерировать импульсы с достаточной частотой.
Сложности реализации
Управлять шаговым мотором просто, когда скорость не критична. В данном случае, нужно управлять ускорением, поэтому следует учесть два параллельных процесса: подача импульсов для шага и пересчет задержки между шагами.
Оценим требуемую частоту импульсов на шаг. Пусть максимальная скорость - 1 оборот в секунду (при колесе от роликов диаметром 72мм, ~22 см/с), двигатель настроен в режиме 1/8 шага, 1600 импульсов на оборот, значит, как минимум процесс, отправляющий импульсы на шаг должен работать с частотой 1.6kHz, причем время должно выдерживаться точно, значит, нужно использовать прерывания по таймеру. Но достаточно ли этого?
Здесь возникает понятие - разрешение (resolution) по скорости, т.е. насколько малые приращения по скорости контроллер способен выдать. Задержка между шагами - это целое количество прерываний таймера (1.6kHz, 800Hz, 533Hz, 400Hz, ...) Программой ниже можно оценить насколько точно можно аппроксимировать желаемые изменения скорости в зависимости от частоты прерываний.
Код
import numpy as np
import matplotlib.pyplot as plt
from math import pi
PULSES_PER_REVOLUTION = 1600
def get_physical_velocity(velocity, frequency):
ticks_per_pulse = round(2.0 * pi * frequency / (velocity * PULSES_PER_REVOLUTION))
return 2.0 * pi * frequency / (ticks_per_pulse * PULSES_PER_REVOLUTION)
times = np.linspace(0, pi, 1000)
velocities = np.sin(times)
physical_velocities = list(map(lambda x: get_physical_velocity(x, 1600), velocities))
physical_velocities_2 = list(map(lambda x: get_physical_velocity(x, 50000), velocities))
plt.plot(times, velocities)
slow_label, = plt.plot(times, physical_velocities, label="1.6kHz")
fast_label, = plt.plot(times, physical_velocities_2, label="50kHz")
plt.legend([slow_label, fast_label], ['1.6kHz', '50kHz'])
plt.grid(True)
plt.show()
тоесть, чем ниже частота, тем хуже можно аппроксимировать скорость в области высоких скоростей, что для робота будет равносильно удару. Я использовал 50kHz. Стоит помнить, что чем выше частота прерываний таймера, тем медленнее работает основной цикл.
Второй процесс - рассчет задержки между шагами, в идеальном случае, если робот движется с ускорением, то после каждого шага нужно пересчитать задержку до следующего шага, т.е. это было бы правильно делать прямо в обработчике прерывания таймера, но формула содержит деление ticks_per_pulse = round(2.0 * pi * frequency / (velocity * PULSES_PER_REVOLUTION)), и обработчик может не успеть закончить работу до наступления след. прерывания. Поэтому задержка пересчитывается в основном цикле как можно чаще.
Несколько советов, как ускорить программу:
Wire.setClock(1000000UL);- ускоряем коммуникацию с MPU6050Использовать DMP (Digital Motion Processor) встроенный в MPU6050, а не реализовывать комплиментарный фильтр или фильтр Калмана, это дало ускорение обработки основного цикла в 5 раз (2.5kHz)
Использовать пин прерывания на IMU и читать данные только тогда, когда закончена обработка, чтобы не опрашивать его постоянно, это ускорило основной цикл еще в 8 раз до 19kHz
не забыть откалибровать IMU, иначе измерения угла все время будут плыть.
Код проекта доступен здесь.
Схема
Ниже схема, по которой я заказал печатную плату в JLPCB. Хотелось бы получить обратную связь от опытных схемотехников, особенно по части разведения питания.
Результат
Т.к. в BLE-Nano встроен Bluetooth, и не надо писать дополнительный код, легко добавить управление. Я переделал SerialTerminal под Android и сделал из него пульт управления.
План дальнейшего развития:
Сделать расширяемую платформу (другой контроллер или Raspberry будет делать всю высокоуровневую работу, а Nano будет предоставлять API по настройке и управлению)
Добавить сенсоры (препятствий, линии под роботом, микрофон, камеру)
Добавить индикаторы (заряда батареи, угла, скорости)
Присоединить телефон и установить конференц-связь
Буду рад, если кто-то захочет присоединиться.
Ссылки
Комментарии (22)
lab412
02.09.2021 10:48Хорошая игрушка получилась. А насколько быстро оно ездить может? разонгать бы до десятков километров в час - было бы куда интереснее. Я тут не специалист, но чаще всего когда такие вот роботы в мультиках или кино (нарисованные короче) ездят - они наклоняются вперед а не стоят вертикально. Это "киношный эффект" всего лишь или реально при больших скоростях стабильное положение будет с наклоном вперед?
LynXzp
02.09.2021 12:33Не киношный эффект www.youtube.com/watch?v=WOVhm-JA-wg (не лучший ролик для демонстрации, но что-то найти можно)
Ну и за одно десятки км/ч. Правда не чисто от робота, но балансирует вперед-назад исключительно робот.
zjor Автор
02.09.2021 18:18Тут робот сильно отклоняется: https://www.instructables.com/Brushless-Gimbal-Balancing-Robot/
Скорее нужно бесколлекторные моторы использовать и какой-нибудь отдельный контроллер типа ODrive (тоже в планах)
Но можно рассмотреть требования к железу, если разгонять до 10км/ч: это 2.7m/c, при диаметре колеса 72мм, угловая скорость ~12 оборотов в секунду. При микрошаговом режиме, 1600 пульсов на оборот, почти 20kHz только для того, чтобы вращать колеса, но надо еще надо учесть разрешение по скорости для плавной аппроксимации ускорения, т.е. частоту прерываний бы раз в 10 увеличить, итого 200kHz только на таймер. Но если взять контроллер побыстрее, например, ESP WROOM32, 240MHz, то вполне осуществимо, но надо еще учесть, как уменьшится момент мотора при такой скорости.
Eddy71
02.09.2021 10:55Вопрос, обратный предыдущему: каковы энергозатраты в случае если надо чтобы платформа не ехала, а стояла на месте (вертикально)?
kvazimoda24
02.09.2021 17:41Думаю, основные затраты там - электрлника и ток удержания моторов. Это при минимальных дестабилизирующих факторах.
А так, если поставить на ветру да ещё и на наклонной плоскости, то, конечно, затраты будут в разы больше. Правда, и вертикально он стоять не будет :)
heone
08.09.2021 13:37Постоянный воздушный поток, это из области аэротрубы. В природе нет идеального ветра. Поэтому и дестабилизирующие факторы скорее всего не будут минимальными.
zjor Автор
02.09.2021 18:03контроллер и акселерометр потребляют пренебрежимо мало по сравнению с моторами, но их можно отключать, когда робот близко к положению равновесия (даже если он наклонен, противодействуя ветру). Он может оставаться к покое из-за трения, а как только будет выходить из равновесия, моторы сразу включатся. В след. версии схемы добавлю линии enabled для драйверов, и можно будет собрать статистику, сколько энергии на этом получится сэкономить.
Mc_Key
03.09.2021 01:59+1Классный проэкт. На GitHub есть отличный проэкт робота балансира на bldc моторах с применением векторного управления (FOC) на базе arduino.
https://github.com/simplefoc/Arduino-FOC-balancer
Andy931
06.09.2021 14:13+1Класс! Вдохновляет! Обязательно попробую свою реализацию!
Отличное, ëмкое и понятное изложение без воды! Спасибо!
barabanus
07.09.2021 22:08+1Небольшая ремарка насчёт matplotlib: вы уже указали label при вызове
plt.plot(), поэтому достаточно вызватьplt.legend(), чтобы сгенерировать автоматическую легенду. Такое использование гораздо лаконичнее.Ещё одна ремарка относительно PID: для контроллеров обычно используют целочисленную арифметику. Если уже считаете с плавающей точкой, то для накопительных переменных надо брать число с большей точностью для избежания переполнения, иначе малые приросты просто перестанут менять перменную.
Smileedition
08.09.2021 05:25генерировать импульсы программно не лучшее решение, тем более на таких частотах, нужно использовать выходы самого таймера, а в коде только менять параметры его работы
zjor Автор
08.09.2021 12:04я только за, если таймер может генерировать импульс в широком диапазоне частот, а у нас есть только делитель частоты и прерывание по переполнению счетчика. Плюс импульс на шаг выглядит как LOW->HIGH, подождать как минимум микросекунду, HIGH->LOW - уже какая-то логика, к тому же 2 двигателя - 2 таймера, а в Nano их три и не все можно программировать без последствий для измерения времени.
Smileedition
08.09.2021 12:41вам нужен таймер с выходом на ногу контроллера
большой диапазон достигается за счет того что можно менять и регистры таймера и делитель частоты
16-bit Timer/Counter1 with PWM вот этот раздел в reference manual
AaRoN_2021
08.09.2021 11:59Интересно, а вот если увеличить масштам, обьем этого робота, то будет вопще супер. Кстати а именно только через Arduino Nano можно собрать или можно собрать из разных Arduino микрокотролеров? Нагрузка какая типо нужна ли подзарядка? потомучто этим роботам нужна подзардка, если конечно я неошибаюсь.
Sterpa
22.09.2021 13:28Прекрасная работа, @zjor!
Обратил внимание, что практически все подобные работы с балансировкой движения начинаются с мат. модели, особенно для коптеров, где получается достаточно много сложных диф. уравнений. Но ведь в самих PID регуляторах эти уравнения не используются, или я не прав?
Я сам сталкивался только с PID регуляторами температуры, когда по датчику температуры или фотоэлементу определяется цвет расплава, а управляющий сигнал от PID повышает или понижает расход газа в печи. Но там нигде сама физика процессов не используется, т.к. PID-регулятору совершенно все равно чем управлять и в каких единицах.
Мне казалось, что подобным образом PID регуляторы контролируют углы эйлера в полете. Но для этого вроде тоже никакие уравнения не нужны, углы мы получаем уже с гироскопа...
Буду очень признателен, если кто в двух словах пояснит, где используются уравнения движения в связке с PID, или отправит в направлении источника знаний, с примерами))
zjor Автор
22.09.2021 16:22спасибо,
это верно, PID-регулятору все равно чем управлять, но подгонять коэффициенты сразу на устройстве может быть очень дорого и долго, более того, не при всех коэффициентах система стабилизируется, а реальное устройство может сломаться. Поэтому все отлаживается на модели, а для этого нужны уравнения. Для подбора коэффициентов достаточно линеаризованной модели, тогда уравнения существенно упрощаются (sinx ~ x, cosx ~ 1, и т.д.), но я люблю посмотреть на картину в целом, поэтому моделирую полную систему. Могу посоветовать вот эти 2 канала: https://www.youtube.com/watch?v=Pi7l8mMjYVE&list=PLMrJAkhIeNNR20Mz-VpzgfQs5zrYi085m, https://www.youtube.com/user/ControlLectures/playlists
vlakir
Отлично! Добротная инженерная разработка, стиль изложения и оформление тоже на высоте.
А нет мысли реализовать осциллирующий подвес? Была бы мегаинтересная платформа для экспериментов. Или энергозатраты не позволяют сделать такое на автономном источнике? Не пробовали прикинуть?
zjor Автор
Спасибо, послушал Максима Ильяхова и выкинул половину слов :)
Имеете ввиду маятник Капицы? Можно, в принципе, как раз план сделать платформу, чтобы можно было и механические модули прикручивать, а питание раздавать на всего робота.
vlakir
Технарь, читающий Ильяхова - редкое явление) Результат налицо.
Ага, я про маятник Капицы. Его же при желании можно загнать в хаотический режим, там вообще поле непаханное для экспериментов.