X
Самобалансирующий ( балансирующий )
робот
на шаговых моторах
(на основе микроконтроллера ESP32)
В данной статье будет рассмотрен самобалансирующий робот как с теоретической, так и практической точки зрения. Ранее я уже реализовывал балансирующих роботов сначала на коллекторных моторах с редуктором без обратной связи, затем на шаговых моторах с контроллером Arduino NANO.
Сссылка на статью по самобалансирующемуроботу на шаговых мотора на arduino nano (есть ошибки в алгоритме):
./articles/electronics/balancing-robot-on-step-motors/
Сылка на статью по самобалансирующемуроботу на редукторных DC моторчиках на arduino nano:
./articles/mechanics/how-to-make-the-balancing-robot/
Но предыдущие проекты не были закончены, в них мне не удалось достичь качественной работы роботов в основных режимах: робот на коллекторных моторах был плохо управляемым (движения), робот на шаговых моторах из второго урока недостаточно эффективно балансировал. Но по ним конечно можно было проследить определенную эволюцию принципов, которые закладывались в балансировку.
На сегодняшний день можно сделать следующие выводы: робот для балансировки должен обязательно иметь возможность измерять свое перемещение, чего не было, да и не могло быть в первом роботе; кроме перемещения нужно учитывать скорость движения робота, что не производилось во второй попытке. Но об этом несколько позже, не будем забегать вперед и вначале рассмотрим конструкцию робота.
Шаговые моторы
Для того чтобы не измерять перемещение робота используются шаговые моторы, которые в идеальных условиях не пропускают шаги, а значит эти шаги можно использовать для измерения пройденного расстояния. Отсюда мы получим и скорость робота.
Шаговые моторы должны иметь достаточную мощность для работы без перегрузок. Для этого подходят моторы Nema17 рассчитанные на ток 1.2-1.5А. Более мощные моторы будут избыточными, менее мощные не смогут без пропуска шагов перемещать достаточно тяжелую конструкцию робота.
DRV8825
В качестве драйверов используются недорогие хорошо зарекомендовавшие себя драйверы DRV8825
На рисунке название ряда контактов сверху имеют линию, это знак «Инверсии» сигнала, он означает, что активный сигнал на данной линии – отрицательный:
- ENABLE – включение управления мотором (если 1 – выключен, 0 – включен);
- М0,М1,М2 – задают режим деления шага (дробления шага) см.таблицу 3.1;
- RESET – перезагрузка (в рабочем состоянии мотора должен быть подтянут к питанию логики 3.3в-5в);
- SLEEP – сон (в рабочем состоянии мотора должен быть подтянут к питанию логики 3.3в-5в);
- STEP – шаг, импульс на данном входе, это команда мотору сделать шаг;
- DIR – задает направление вращения вала мотора (само направление условно и зависит от порядка подключения обмоток);
- VMOT – (+) контакт для подачи напряжения питания положительный;
- GND MOT – (–/GND) контакт для подачи напряжения питания - (ЗЕМЛЯ);
- B1 и B2 – разъем для подключение первой обмотки (номера обмоток и номера контактов в обмотках условны);
- A1 и A2 – разъем для подключение второй обмотки (номера обмоток и номера контактов в обмотках условны);
- FAULT – выход сигнала аварии (действие не изучено, возможно устанавливается в низкий уровень при превышении нагрузки, замыкании или обрыве обмотки мотора);
- GND LOGIC – GND (земля) логической части (контроллера управления), если источники питания логики и силовой части различны и не имеют общей «земли», если источник питания один, данный контакт можно не подключать.
Некоторые современные драйверы шаговых моторов могут дробить шаги на более мелкие, за счет плавного изменения тока обмоток. DRV8825 может делить шаг на 32 микрошага, таким микрошагом мы пользоваться не будем, но возможно воспользуемся данной возможностью для дробления шага до 1/16 (в роботе дробление шага 1/4), это увеличит плавность хода, что важно в нашем случае важно.
На драйвере есть подстроечный резистор (он находится под небольшим напряжением), он нужен для настройки тока фазы.
Для подстройки тока фазы требуется мультиметр и тоненькая плоская отвертка. На мультиметре устанавливаем режим измерения напряжения (до 2-5 вольт). Подключаем драйвер к силовому питанию. Измеряем напряжение между GND и металлической вращающейся головкой подстроечного резистора. Измерив на подключенном к питанию драйвере напряжение между резистором и GND, и умножив полученное значение на 2, получим ток фазы. В нашем случае следует добиться значения в пределах 0.7-1.0В, что соответствует току через обмотки мотора 1.4-2А. Большие значения поставить можно, но это приведет к нагреву драйвера и мотора, а также ускорит разряд батареи робота. Меньшие значения могут стать причиной того, что робот под нагрузкой (в режиме управления роботом) не будет поворачивать колеса или будет «пропускать» шаги. Вращаем головку (без усилий — можно повредить резистор), добиваемся нужного значения.
Для защиты драйвера от перегрева следует использовать радиатор (он идет в комплекте с драйвером). Радиатор снабжен клейкой основой. Снимите бумагу с клейкой части радиатора. Установите радиатор на микросхему драйвера, прижмите, произведите вращательные движения на 5-10 градусов, чтобы удалить пузыри воздуха между радиатором и микросхемой.
Значение микрошага шагового мотора в зависимости от подключения контактов M0,M1,M2 драйвера DRV8825.
Контроллер (управление)
В качестве контроллера использовался ESP32. Довольно интересный контроллер, который может быть запрограммирован из различных IDE, но мы будем использовать простую IDE Arduino, поддержку которой обеспечил производитель. В последнее время я довольно активно использую данные контроллеры по причине высокой производительности и низкой стоимости, также стоит отметить широкий перечень возможностей, которые представляет ESP32. Это скорее микрокомпьютер, чем контроллер 512КБ ОЗУ, 4МБ Flash, два ядра по 240МГц, 64-разрядные таймеры и пр. и пр...
Электропитание робота
В качестве источника питания для робота используется 4 литиевых аккумулятора формата 18650 подключенные последовательно, что дает напряжение 14-16 вольт и ток до 5А. Использование 4х аккумуляторов и столь высокого напряжения основано на требованиях двигателей к току (до 1.5А) и драйверов drv8825 к напряжению (не менее 9В). Если у вас есть другие источники удовлетворяющие данному требованию, можете смело их использовать. Подобный ток будет потребляться не всегда, а в периоды максимальной нагрузки моторов.
Также потребуется один стабилизатор для уменьшения напряжения до 5В. Данное напряжение способен «переварить» внутренний стабилизатор платы ESP32 (сам контроллер использует напряжение 3.3вольта). Можно использовать линейный стабилизатор типа L7805CV или импульсный понижающий стабилизатор настроенный на выходное напряжение 5 вольт.
Ниже, в схеме робота будет приведено подключение линейного стабилизатора. Отмечу, что ножка 1 - (+входа), 2 — (GND), 3 - (+выходного напряжения +5вольт). Между обоими положительными контактами и GND устанавливаются гасящие помехи конденсаторы.
Боксы аккумуляторов располагаются сверху, как показано на следующем рисунке, высокий центр тяжести делает робота устойчивым при балансировке, особенно это заметно при движении робота. Роботу с высоким центром тяжести требуется меньший наклон (в сторону движения) для достижения высокой скорости.
Сборка робота
Схемы робота
Схемы робота приведены ниже. Также сборку робота можно проследить по роликам:
1 часть.
2 часть
Схемы приведены для двух роботов, на гироприборе BNO055 и MPU6050, их подключение не отличается, а вот программное обеспечение отличается значительно.
Учимся работать с гироприбором
Если вы уже собрали робота, то отключите его моторы, вынув положительный контакт (провода питания драйверов DRV8825), или другим способом, если робот не собран. Возьмите модуль ESP32 и гироприбор, далее будут показаны принципы работы с MPU6050 и BNO055. В любом случае гироприбор должен быть подключен согласно схемы, указанной выше и соответствующей вашему гироприбору. У приборов MPU6050 и BNO055, которые были у меня под рукой, при установке их на макетную плату контакты SCL и SDA распаяны по разному (перекрещены). Для удобства установки я использовал по разному GPIO ESP32: для BNO055 – GPIO22/SCL, GPIO23/SDA; MPU6050 – GPIO23/SCL, GPIO22/SDA.
Но все зависит от распайки чипа на плате, вы можете самостоятельно менять GPIO для ESP32 I2C в строке включения I2C Wire.begin(SDA, SCL), здесь вместо SDA и SCL установите номера GPIO вашего подключения. Строка Wire.begin() для BNO055 находится в файле Adafruit_BNO055.cpp , для MPU6050 - в файле gyro_acsel.h .
Ориентацию осей также рекомендую определять экспериментально, получая показания прибора.
Если у вас BNO055, который имеет несколько режимов работы интерфейса, переключите его в режим I2C интерфейса закоротив перемычки S0 (PS0) и S1 (PS1) на GND, на рисунке с GY-BNO055 перемычки уже впаяны (рядом со стрелками осей). Прибор CJMCU-055 подразумевает возможность установки внешнего кварцевого генератора самостоятельно, для этого есть отверстия под пайку (выше надписи CJMCU-055).
Считаем, что гироприбор стоит на роботе, так как и будет стоять в дальнейшем. Сторону робота ближе к которой расположен гироприбор будем считать «FRONT», а соответственно ту, где расположены драйверы моторов и выключатель — «BACK», напишите маркером или сделайте метки маркером на роботе.
Снимаем показания с гироприборов
(далее идет выборочное цитирование из моей книги «Мобильные роботы на базе Arduino»)
Гироскоп, это прибор способный реагировать на изменение ориентации объекта. Простейшим примером гироскопа является волчок. Если волчок раскрутить на перемещаемой поверхности, например, на доске, а затем наклонять эту доску, то волчок будет пытаться сохранить свою ориентацию.
Гироскоп был изобретен в начале XIX века, и в конце того же века был впервые применен на практике для стабилизации курса торпед.
На рисунке показано, как можно измерить отклонение машинки от первоначального направления с помощью гироскопа. Черная стрелка – вектор движения, штрихованная – показания гироскопа. В позиции (б) машинка завернула, но стрелка гироскопа осталась на месте, она указывает на заданное при включении гироскопа направление. Зная это отклонение, без труда можно скорректировать направление, но только направление, гироскоп ничего не знает о пройденном расстоянии и о том, по какому маршруту это расстояние было пройдено.
Акселерометр
Акселерометр служит для измерения ускорения. При помощи акселерометра можно измерять ускорение при движении и, используя простые формулы, определять положения тела в пространстве. Например, если знать начальную скорость и считывать ускорение в равные малые промежутки времени, несложно вычислить пройденное расстояние Рассмотрим это на примере прямолинейного движения:
,
где 
- расстояние, пройденное ранее, 
- итоговое расстояние, 
- приращение времени (время, которое движется машинка от А до Б), 
- начальная скорость (в точке А), 
- полученное от акселерометра ускорение (желательно, чтобы от А к Б ускорение не изменялось). Точность вычислений зависит от частоты сбора данных и от вычислительных способностей процессора.
Второе предназначение акселерометра, это нахождение углов отклонения объекта от вертикали, для точного измерения объект должен быть неподвижен или двигаться прямолинейно без ускорения. В этом случае прибор регистрирует только одно ускорение – ускорение свободного падения, которое направленно к центру земли. На рисунке изображен неподвижный объект (робот) на наклонной плоскости. Координатная ось Y акселерометра робота направлена вперед, ось Z вниз, ось X к наблюдателю.
,
где 
- угол отклонения робота от вертикали в плоскости формируемой осями Y и Z, 
- проекция ускорения свободного падения на ось Y акселерометра, 
- проекция на ось Z, можно также использовать формулы:
или 
, где 
- ускорение свободного падения, но для электронного прибора, который будет рассмотрен ниже, рекомендуется использовать формулу оперирующую только относительными величинами.
Рисунок Нахождение угла отклонения от вертикали (робот неподвижен)
Если у вас BNO055 используйте скетч BNO055test для тестирования гироприбора.
Прошивка
Я предполагаю, что вы знаете как устанавливать поддержку ESP32 в среду Arduino IDE. Я предполагаю, что вы знаете, как прошить ESP32. Замечу, что некоторые платы, например DEVKIT, требуют нажать кнопку BOOT для прошивки (во время прошивки), а затем кнопку RESET. Эти кнопки расположены на плате контроллера.
Исследуем работу BNO055
Если у вас BNO055 используйте скетч BNO055test для тестирования гироприбора.
1. Прошейте робота данной программой.
2. В Arduino IDE откройте «Монитор порта», установите скорость обмена 115200, как показано на следующем рисунке.
Теперь в мониторе порта вы увидите несколько строк данных, которые будут обновляться каждые 0.3 сек (delay(300)).
Первая строка содержит углы склонения/поворота робота. Вторая строка содержит угловую скорость по осям. Оси ориентации и угловой скорости могут не совпадать по названиям, какие оси использовать и с каким знаком следует установить.
Углы склонения/поворота робота
Покрутите робота, наклоните робота вперед/назад, вправо/влево.
event.orientation.x= ?? покажет угол поворота/налона робота в градусах по (вокруг) оси X прибора, event.orientation.y= ?? по оси Y, event.orientation.z= ?? по оси Z.
Установите робота параллельно горизонту и вращайте без наклонов. Заметьте — значения по какой оси изменяются — исключите эту ось — она нам пока не потребуется. Наклоните робота вправо/влево, запомните значения по какой оси будут в градусах соответствовать наклону, запомните эту ось, она нам не потребуется.
Нам нужна ось значения которой будут изменяться при наклонах робота вперед/назад, запомните эту ось, а также как будут изменяться значения угла — какой знак будет иметь угол поворота при наклоне вперед. Если этот знак будет отрицательным в программе балансировки у полученного значение угла потребуется менять знак (инвертировать).
Угловая скорость
Теперь обратив внимание на вторую строку данных, которая содержит значения угловых скоростей вокруг осей.
Gyro.x()= ?? Gyro.y()= ?? Gyro.z()= ??
Вращайте робота в горизонтальной плоскости.
ВНИМАНИЕ, значение угловой скорости нужно снимать именно во время движения!
Запомните ось, значения по которой будут значительно изменяться, они должны иметь разные знаки при вращение по часовой стрелки и против. Эту ось мы не должны использовать.
Наклоняйте робота вправо влево. Запомните ось, значения по которой будут значительно изменяться, они должны иметь разные знаки при повороте робота вправо и влево. Эту ось мы не должны использовать.
Наклоняйте робота вперед - назад. Запомните ось, значения по которой будут значительно изменяться, они должны иметь разные знаки при действии наклонения вперед - назад.
Еще раз отмечу, берите во внимание только показания, которые получаются, когда действие склонения робота производится!!! Когда робот остановлен, значение угловой скорости будут близки нулю и колебаться по знаку, он нам не интересны.
Эту ось мы будем использовать.
Если при действии «наклон вперед» угловая скорость по оси имеет отрицательный знак, в дальнейшем требуется менять знак (инвертировать).
Исследуем работу MPU6050
BNO055 сложный прибор, который сам производит промежуточные расчеты и подает на выход результат готовый к использованию, MPU6050 — простой прибор, который показывает только угловую скорость и довольно зашумленные показания акселерометра. Соответственно ряд расчетов потребуется проводить в программе. Рассмотрим, какие расчеты требуются.
MPU6050 не показывает угол отклонения от заданного направления. Прибор, регистрирует текущую угловую скорость (скорость вращения), и значения влияния гравитации по осям (акселерометр). Зная мгновенную угловую скорость, можно вычислить угол, на который повернется объект за некоторый промежуток времени:
,
где 
- начальный угол, рассчитанный ранее, - малый промежуток времени, на котором можно считать, что угловая скорость не менялась, 
- полученное от электронного гироскопа в течении значение угловой скорости, 
- рассчитанный угол, на который повернулся объект вокруг исследуемой оси с начала поворота. На рисунке вращение происходит вокруг оси Z.
Показания угловой скорости резко возрастают при вращении прибора, а при покое минимизируются но, несмотря на полную неподвижность прибора, они не нулевые и колеблются вокруг одного значения. Эту ошибку можно (рекомендуется) компенсировать, если до начала работы с прибором некоторое время держать его в неподвижном состоянии, собирая показания его угловой скорости, рассчитать среднее, а затем эту величину постоянно вычитать из полученных от прибора данных. Возможен также температурный дрейф, тогда погрешность будет зависеть от температуры, что учесть сложнее, но возможно.
Шкала значений MPU-6050
Получаемые от прибора значения не похожи по величине на ускорение свободного падения (~9,8м/с2), что это за значения, чем оперирует данный прибор, как их перевести в привычные величины? Для ответа на этот вопрос следует обратиться к документации. На сайте производителя InvenSense (www.invensense.com), есть таблица основных характеристик MPU-6050. В частности, чтобы перевести показания гироскопа в градус/сек, их требуется поделить на 131.
Для перевода ускорения в привычную для нас систему мер требуется поделить полученное значение от прибора на 16384, а затем умножить на 1g= 9,80665 м/с², но в программе балансировки нам это не потребуется, так как будет измеряться угол склонения, а для этого достаточно относительных величин по осям акселерометра.
Комплементарный фильтр
При помощи угловой скорости невозможно измерить угол склонения робота, можно измерить только изменение этого угла, да и то постоянно будет накапливаться погрешность. В теории нам было бы достаточно показаний акселерометра, чтобы получить склонение робота вперед/назад или вправо/влево но акселерометр MPU6050 сильно шумит, его показания довольно сильно изменяются, хотя и вокруг одного значения.
Ниже приведен пример измерения угла склонения робота по показаниям акселерометра. Я наклонял робота плавно, но показания довольно сильно пляшут.
Но на помощь нам приходит комплементарный фильтр! В нем смешиваются в определенных пропорциях показания по гироскопу и акселерометру, что позволяет получать довольно точную картину наклона робота. К сожалению такой фильтр невозможно применить при повороте робота в горизонтальной плоскости — не будет задействованы показания гравитации в акселерометре.
При объединение показаний акселерометра и гироскопа на основе комплементарного фильтра можно рассчитать значение угла наклона по формуле:
,
где 
- расчетное значение угла наклона, 
- значение угла наклона из предыдущего расчета, 
- коэффициент комплементарного фильтра (подбирается экспериментально от 0.001 до 0.05), 
- угловая скорость, - время итерации, 
- угол склонения, рассчитанный по текущим показаниям акселерометра.
Итоговая величина является долевой суммой интегрированного значения показаний гироскопа и мгновенного значения показаний акселерометра
Доля показаний акселерометра мала, поскольку они умножаются на малую величину (0.001 - 0.05), Основной вклад вносят показания гироскопа, они умножаются на величину 
(0,999-0,95),
Но благодаря акселерометру, компенсируются ошибки измерений, которые обязательно возникают и накапливаются, если пользоваться только гироскопом..
Коэффициент был взят 0.001, этого оказалось достаточно для компенсации дрейфа нуля гироскопа.
Благодаря использованию комплементарного фильтра, который математически намного проще, чем фильтр Калмана, удалось добиться стабилизации робота, и он успешно справляется с задачей балансировки.
Цитирование «Мобильные роботы на базе Arduino» закончено.
Используйте скетч MPU6050test для тестирования гироприбора.
-
Прошейте робота данной программой.
-
В Arduino IDE откройте «Монитор порта», установите скорость обмена 115200, как показано на следующем рисунке.
-
Перезагрузите ESP32 кнопокой «Reset», не трогайте робота в течении 4х секунд, в это время производится расчет смещения показаний акселерометра.
Теперь в мониторе порта вы увидите несколько строк данных, которые будут обновляться каждые 0.1 сек .
Gyro = показания угловой скорости в радианах.
Acsel= показания угла наклона по акселерометру в радианах.
AcYsum =показания угла наклона Комплементарный фильтр.
Если значения в порту не соответствуют ожиданию, значит ваш прибор повернут и следует использовать другие оси для расчета показаний угловой скорости и угла акселерометра (они комментированы).
Если при действии «наклон вперед» угловая скорость по оси имеет отрицательный знак, в дальнейшем требуется изменить знак (инвертировать). То же касается показаний угла Acsel. При проведении наклона вперед угловая скорость — положительна. Зафиксируйте робота склоненным вперед, значение Acsel должно быть также положительным (иначе меняем знак у того значения, которое имеет неправильный знак).
Запомним те изменения, которые внесены в типовой пример, они потребуются в полной программе балансировки.
Теория и практика балансировки
По теоретической части управления обратным маятником существует довольно много литературы, с довольно красивыми математическими выкладками, но зачастую они не приведут вас к созданию эффективно работающего алгоритма балансировки.
Фактически при балансировке мы получаем колебательный контур, а колебания в контуре могут быть затухающими и незатухающими. Если наш робот колеблясь совершает каждый раз колебание с увеличивающейся амплитудой, то …. нужно что то подкрутить — изменить звенья обратной связи.
Хотя, пойдем от простого к сложному.
Все описанные далее формулы должны применяться к роботу периодически, через 5-10 миллисекунд.
Самое простое что можно сделать, это попытаться измерить склонение робота, для этого потребуется гироприбор, а затем в зависимости от величины склонения задать движение робота, которое направлено в сторону склонения и пропорционально самому склонению.
На практике вы таким образом сможете, если сильно постараетесь, получить простенькую балансировку успешность которой будет зависеть от множества параметров в том числе сопротивления грунта, склонения стола, ветра и т.д.
Для точности нужно брать за основу пропорциональности синус угла склонения, но так как синус угла до 30 градусов отличается незначительно от своего значения в радианах, то для простоты и увеличения скорости обработки будем брать сам угол в радианах.
,
скорость ответа колес робота Speed должна быть пропорциональна углу склонения робота Ang. Это первая фаза построения регулятора.
Не нужно добиваться балансировки робота на месте, достаточно, чтобы робот ехал вперед или назад некоторое время балансируя.
При балансировке таким простым механизмом вы заметите, что подобрать рабочий режим довольно трудно и точность робота сразу собьется если немного изменить его вес. Здесь на помощь приходить вторая величина, которую мы можем получить от гироприбора, это угловая скорость, гироприбор показывает ее по всем трем осям, но нам нужна только одна, та вокруг которой совершает падение робот (это может быть любая ось — как установлен прибор).
Предположим, что мы узнали скорость падения робота, она может быть отрицательной и положительной. Также скорость падения может не совпадать с наклоном робота — робот наклонен в одну сторону, а благодаря компенсации колесами не падает, а поднимается.
Допустим ситуацию, когда робот склонён но уже не падает, а поднимается, логично предположить, что в этом случае робот должен хотя-бы остановиться, но он, «видя» свой наклон, продолжит ехать вперед, что приведет в обратному склонению (перерегулированию) и в дальнейшем к падению. Отсюда можно сделать вывод, что знание одного склонения робота не достаточно. Получаем вторую составляющую формулы балансировки — скорость падения.
,
As – угловая скорость (скорость падения) , Kd — коэффициент пропорциональности.
Если применить данную формулу, то можно невооруженным глазом заметить, что балансировка стала плавнее. Для подбора Kd, примите Kp = 0, и добейтесь существенного противодействия падению одним вторым звеном. Робот будет падать но должен существенно этому сопротивляться.
Но еще один вопрос как быть, если робот уже движется с определенной скоростью и падает, где и как мы учитываем текущую линейную скорость робота? - нигде. А это желательно сделать!
Если заданная скорость совпадает со скоростью движения, а это практически так для шаговых моторов, то формула превращается в следующую:
,
где Speed_old – текущая скорость.
Если теперь запустить робота, он сразу уйдет в критическое отклонение, это связано с тем, что Kp и Kd для данного режима будут велики и должны быть существенно снижены. Настройка похожа на предыдущую: сначала принимаем Kd=0, добиваемся балансировки, затем Kp=0, также добиваемся существенного сопротивления робота падению, затем оба коэффициента снижаем вдвое от полученных значений и подключаем оба.
Не нужно добиваться балансировки робота на месте, достаточно, чтобы робот ехал вперед или назад некоторое время балансируя.
Вы уже задали вопрос — «почему робот не стоит на месте?» - пора задать!
Дело в том, что точно отцентрировать робота относительно гироприбора сложно и робот будет всегда немного наклонен вперед или назад, наклон задается смещением центра тяжести. Для компенсации этого наклона применяется обратная связь по пройденному расстоянию, а это не что иное как скорость умноженная на dt – время действие данной скорости. Так как скорость робота изменяется периодически, то следует использовать накопительную (интегральную) форму:
,
Move – пройденный роботом путь в относительных единицах (для шаговых моторов это могут быть шаги), dt- время действия скорости (период между опросами в секундах),
,
Ki — коэффициент пропорциональности.
Кажется все нормально, но на деле робот начинает колебаться с огромной амплитудой — изменяя Ki добейтесь этого. Дело в том, что работе интегральной составляющей Move нужно помогать останавливая робота за счет составляющей, которая уменьшается с уменьшением скорости робота, это как раз сама скорость робота — Speed, добавим ее небольшую часть для компенсации раскачивания робота интегральным звеном...
,
где Kki – коэффициент затухания для интегрального звена.
Теперь наш робот будет устойчиво стоять и даже если его толкнуть — вернет себе устойчивое равновесие. Но, он еще не может двигаться!
Для того чтобы робот двигался нужно компенсировать последствия его движения, а его движения будут оказывать влияние на звено с накоплением — интегральное звено и его компенсатор затухания.
Если принять заданную скорость движения за SpZ, то формула примет вид:
,
Вот теперь робот будет двигаться с указанной скоростью и не только по равнине, но и в гору и с горы!
Можно и далее сглаживать движение робота, введением звеньев высшего порядка, но их влияние, как показывает практика, не велико.
Программы
Программный код я приводить не буду, так как не умею сделать это красиво, как в редакторах кода. Тексты программ снабжены комментариями. Если есть вопросы пишите на почту momotmvu@yandex.ru, оставляйте коментарии под статьей или под роликом на Youtube.
Программы запакованы архиватором zip.
Тестирование гироприборов:
/articles/programming/selfbalance-robot-esp32-bno055-mpu6050/doc/BNO055test.zip
/articles/programming/selfbalance-robot-esp32-bno055-mpu6050/doc/mpu6050test.zip
Программы балансировки
к BNO055 /articles/programming/selfbalance-robot-esp32-bno055-mpu6050/doc/selfbalBNO055.zip
к MPU6050 /articles/programming/selfbalance-robot-esp32-bno055-mpu6050/doc/selfbalmpu6050.zip
Изменяем направление вращения колес
Если колеса по факту вращаются не в ту сторону, следует поменять в файле step_motor.h значения указанных ниже констант на противоположные:
#define DIR_L_FRONT false //Пин направления левого шага
#define DIR_R_FRONT true //Пин направления правого шага
#define DIR_L_BACK true //Пин направления левого шага
#define DIR_R_BACK false //Пин направления правого шага
Изменяем значения GPIO
Если вы для своего робота используете не те GPIO, что прописаны в схемах и программе, их можно заменить в файле step_motor.h
#define STEP_L 15 //Пин левого шага
#define STEP_R 4 //Пин правого шага
#define STEPER_EN 2 //Пин врключения моторов
#define DIR_L 13 //Пин направления левого шага
#define DIR_R 16 //Пин направления правого шага
ОСТАВИТЬ КОММЕНТАРИЙ
Форма авторизации
ВОЙТИ С ПОМОЩЬЮ:
ИЛИ Авторизация на сайте: