Робот, удерживающий равновесие, на MPU6050 и ПИД-регуляторе (PID)
Двухколёсный робот, который самостоятельно поднимается, сохраняет вертикальное положение при толчке и удерживает своё место на полу, а не уезжает.
Что вы собираете
Робота, который стоит на двух колёсах, и удерживает его только арифметика.
Механическая сборка действительно проста: два двигателя, рама, датчик, аккумулятор. Проект ценен тем, что у задачи управления нет частичного успеха. Робот, следующий по линии, со средними коэффициентами всё равно следует по линии, хотя и плохо. Робот балансировки со средними коэффициентами ложится на пол. Эта острота делает его лучшим способом действительно понять ПИД-регулятор.
В конце у вас будет робот, который поднимается, противостоит толчку и примерно остаётся там, куда его поставили.
Почему это сложно
Вся масса робота находится выше оси. Поэтому он представляет собой перевёрнутый маятник: вертикальное положение является равновесием так же, как карандаш, установленный на остриё, — реальным в принципе и безнадёжным без постоянной коррекции. Немного наклоните его, и сила тяжести создаст момент, который наклонит ещё сильнее и быстрее.
Единственная защита робота — возможность переместить собственное основание. Вращение колёс вперёд перемещает ось под центр масс и снова поворачивает корпус вертикально. Отсюда следует контринтуитивное правило, управляющее всем проектом: чтобы остановить падение вперёд, двигайтесь вперёд.
Следуют два следствия, которые определяют всё нижеследующее:
- Нельзя непосредственно управлять положением. Управление двигателями по ошибке положения ускоряет выход основания из-под массы, и робот падает в направлении, противоположном запрошенному. Положением необходимо управлять через наклон.
- Существует жёсткий предел восстановления около 35°. За ним требуемое ускорение превышает то, которое могут обеспечить колёса до касания корпусом пола, и никакой коэффициент не помогает.
Перечень компонентов
| Компонент | Кол-во | Примерная стоимость | Примечания |
|---|---|---|---|
| Arduino Uno или Nano | 1 | $5 | Внутреннему контуру нужна частота дискретизации 100–200 Hz, которую Uno легко обеспечивает |
| MPU-6050 | 1 | $2 | Установлен жёстко и горизонтально, рядом с осью колёс |
| Двигатели N20 с энкодерами | 2 | $16 | Металлический редуктор обязателен. См. ниже |
| TB6612FNG | 1 | $3 | Падение напряжения L298N ~2 V приходится ровно на область мелких коррекций |
| Шасси 2WD | 1 | $10 | Высокое и узкое. Аккумулятор устанавливается наверху — это контринтуитивно |
| Аккумулятор | 1 | $10 | 2×18650. Он также обеспечивает полезную массу |
Итого: примерно $45–60.
Три из этих решений принципиально важны, и ошибка в любом делает робота нераспознаваемым для настройки, а не просто хуже.
Двигатели с металлическими редукторами. Робот, удерживающий равновесие, сотни раз в секунду меняет направление небольшими приращениями. Люфт накапливается при каждой смене направления, поэтому мёртвая зона пластикового редуктора приходится ровно туда, где должна действовать коррекция. Результат — робот колеблется вокруг вертикали и не успокаивается, как бы тщательно ни были подобраны коэффициенты, — и это убедительно выглядит как проблема настройки.
Драйвер на MOSFET. L298N теряет примерно 2 V до того, как двигатель вообще что-либо получает, поэтому команды малых коррекций оказываются ниже напряжения движения. Снова возникает мёртвая зона ровно в области мелких коррекций.
Аккумулятор установлен наверху. Это решение удивляет людей. Более высокий робот с массой вверху падает медленнее — период маятника пропорционален квадратному корню длины, — что даёт контуру больше времени на реакцию. Низкий приземистый робот балансировки сложнее, а не проще.
Порядок сборки
Выполняйте шаги по порядку. Каждый можно проверить отдельно, что исключительно важно для проекта с окончательным результатом, имеющим два исхода.
1 — Получите достоверный угол наклона. Подключите MPU6050 и следуйте руководству считывание наклона с MPU6050. Не переходите дальше, пока при вертикальном положении угол не будет около нуля, при наклоне будет меняться в правильную сторону, а при неподвижном роботе — оставаться постоянным. Все последующие решения строятся на этом числе.
2 — Сделайте угол устойчивым к движению. Акселерометр отдельно непригоден на движущемся роботе, а гироскоп отдельно дрейфует за секунды. Дополняющий фильтр объединяет их в одной строке. Проверьте, тряся робот целиком, не вращая его: угол должен почти не меняться.
3 — Настройте внутренний контур в симуляторе. Прежде чем оборудование сможет упасть, используйте симулятор балансировки для понимания влияния каждого коэффициента. Установите дифференциальный коэффициент в ноль на роботе, который уже уравновешивается, и наблюдайте рост колебаний до падения: увидеть этот отказ там намного дешевле, чем на рабочем столе.
4 — Научите робота стоять. Напишите внутренний контур по каскадному ПИД-регулированию балансировки с отключённым внешним контуром. Успех — робот сохраняет вертикальное положение и имеет право бродить. Не пытайтесь пока исправить блуждание.
5 — Добавьте энкодеры и внешний контур. Когда квадратурный подсчёт даст реальное положение колёс, включите внешний контур, чтобы робот удерживал место. Начните с коэффициента намного меньше, чем кажется разумным: его влияние косвенное, и контуры легко начинают бороться друг с другом.
Замечания по оборудованию, которые действительно важны
Установите аккумулятор наверху. Это контринтуитивное и самое полезное механическое решение проекта. Подъём центра масс увеличивает период маятника, поэтому робот падает медленнее, а контур получает больше возможностей поймать его. Низкий робот с тяжёлым низом балансировать гораздо сложнее, чем высокий.
Используйте металлические редукторы. Робот, удерживающий равновесие, сотни раз в секунду меняет направление. В двигателях с пластиковыми редукторами достаточно люфта, чтобы при каждой смене направления первые несколько градусов расходовались впустую, и контур не может это обойти настройкой. Обычное решение — двигатели N20 с энкодерами.
Используйте TB6612FNG, а не L298N. TB6612FNG теряет значительно меньше напряжения на выходном каскаде. На L298N команды малых коррекций попадают ниже порога, при котором двигатель вообще перемещается, создавая мёртвую зону ровно там, где нужны мелкие коррекции.
Обеспечьте двигателям отдельный источник питания. Аккумулятор, проседающий при токе блокировки, вызывает сброс микроконтроллера посреди коррекции. Робот падает по причинам, никак не связанным с контуром управления, что делает отладку мучительной.
Проверка по ходу работы
| Проверка | Что она доказывает |
|---|---|
| При удерживании вручную угол читается ~0° | В измерении не осталось перекоса установки |
| При сотрясении без вращения угол почти не меняется | Фильтр доверяет гироскопу во время ускорения |
| При наклоне вперёд колёса едут вперёд | Знак внутреннего контура правильный |
| Робот противодействует толчку и успокаивается без растущих колебаний | Дифференциальный коэффициент выполняет свою работу |
| После включения внешнего контура робот перестаёт дрейфовать | Энкодеры и внешний контур подключены правильно |
Как выглядит хороший результат
| Измерение | Едва балансирует | Хорошо настроен |
|---|---|---|
| Частота дискретизации внутреннего контура наклона | 100 Hz | 200–500 Hz |
| Частота дискретизации внешнего контура положения | 20 Hz | 20–50 Hz — в несколько раз ниже внутреннего |
| Колебания наклона в покое | ±3–5° | Менее ±1° |
| Дрейф от начальной точки за 60 s | 1–2 m | Менее 30 cm |
| Восстанавливается после толчка | Лёгкого | Сильного рывка |
| Постоянная дополняющего фильтра | 0.98 | 0.98–0.995, выбранная, а не скопированная |
| Время работы до блуждания | Секунды | Неограниченно |
Отношение частот дискретизации двух контуров чаще всего ошибают в первых сборках. Внутренний контур удерживает наклон, а внешний контур формирует требуемый наклон для перемещения. Если частота внешнего контура близка к частоте внутреннего, он следует за целью, до которой внутренний контур ещё не дошёл, и контуры начинают бороться. Рабочее правило: разделение в пять–десять раз.
Строка о «дрейфе» объясняет необходимость энкодеров: робот только с контуром наклона будет прекрасно балансировать, но бродить по комнате, поскольку ничто не сообщает ему, где он находится. Он стоит вертикально и потерян. Внешний контур положения превращает демонстрацию в робота.
Знаки: что портит вечер
Четыре знака должны согласоваться, и вычислить их невозможно: каждый необходимо проверить на рабочем столе, удерживая робота в руке:
| Проверка | Удерживайте робота и… | Ожидаемый результат |
|---|---|---|
| Знак IMU | Наклоните вперёд | Показывается положительный наклон, или стабильно отрицательный — выберите один |
| Знак двигателей | Задайте положительный выход | Колёса едут вперёд |
| Знак внутреннего контура | Наклоните вперёд при работающем контуре | Колёсам задаётся движение вперёд, чтобы поймать падение |
| Знак внешнего контура | Толкните вперёд из состояния покоя | Робот наклоняется назад, чтобы вернуться |
Если перепутать третий знак, робот сильно ускоряет собственное падение в том направлении, в котором уже двигался. Это невозможно не заметить, и лучше проверить при снятых с пола колёсах.
Когда не работает
Почти каждый отказ относится к одному из пяти случаев, и их достаточно различать:
- Мгновенно падает при включении — перевёрнут знак внутреннего контура.
- Нарастающие колебания до падения — слишком мал дифференциальный коэффициент либо D вычисляется по разности углов, а не по скорости гироскопа.
- Гудит в вертикальном положении — слишком велик коэффициент пропорциональной составляющей либо контур работает слишком медленно для этих коэффициентов.
- Оседает на пол без сопротивления — слишком мал коэффициент пропорциональной составляющей либо двигатели насыщаются.
- Балансирует, но уезжает — перекос установки и недостаточная интегральная составляющая для его компенсации.
Таблица режимов отказа подробно рассматривает каждый случай, и все их можно намеренно воспроизвести в симуляторе — это самый быстрый способ научиться их узнавать.
Дорожная карта проекта
Путь сборки
Следуйте по дереву технологий от деталей к работающему роботу. Каждый узел открывается, когда выполнены его предварительные условия, а ваш прогресс сохраняется на этом устройстве.
0 / 16 выполнено
Компоненты
- КонтроллерArduino UnoПрощающая 8-битная плата, с которой большинство знакомится с робототехникой.
- ДатчикMPU-6050 IMUШестиосевой датчик движения — робот ощущает собственный наклон, поворот и ускорение.
- ДрайверДрайвер двигателей TB6612FNGMOSFET H-мост, сохраняющий то напряжение, за которое вы действительно заплатили.
- Исполнительный механизмРедукторный двигатель N20 с энкодеромКомпактный двигатель с металлическим редуктором и энкодером: робот знает, насколько продвинулся.
- ШассиШасси робота 2WDПлатформа, к которой крепятся два двигателя, свободный опорный ролик и вся электроника.
- ПитаниеБатарейный блок питания роботаРазница между работающим роботом и роботом, который постоянно перезагружается.
Руководства на этом пути
- Начальный · 45 минутИзмерение угла наклона по акселерометру и гироскопу MPU6050Преобразование исходных отсчётов MPU6050 в угол наклона в градусах и объяснение, почему каждый датчик не работает в одиночку.
- Средний · 1 hourКомплементарный фильтр: акселерометр и гироскопСведите шумный акселерометр и дрейфующий гироскоп в надёжный угол наклона одной строкой кода.
- Продвинутый · 2 hoursКаскадное ПИД-управление самобалансирующимся роботомДва вложенных контура ПИД: один удерживает угол наклона, другой задаёт наклон, чтобы робот мог двигаться.
- Начальный · 18 мин чтенияКак настроить ПИД-регулятор (PID): практическое руководствоПрактический порядок настройки коэффициентов P, I и D без угадывания — и способ проверить каждый шаг в реальном времени.
- Начальный · 17 мин чтенияЧтение квадратурных энкодеров для измерения расстояния и скоростиПреобразование импульсов квадратурного энкодера в надёжные измерения направления, расстояния и скорости колеса.
Попрактикуйтесь до сборки схемы
Настройте его в интерактивном симуляторе
Путь сборки проходит через лабораторию в браузере. Добейтесь правильного поведения здесь, а затем перенесите настройки на реального робота.
Часто задаваемые вопросы
Насколько сложен робот, удерживающий равновесие, как первый проект?
Это плохой первый и отличный третий проект. Подключение несложно, но объект неустойчив, а значит, частичного успеха нет: робот либо стоит, либо нет, а неверный знак выглядит как плохие коэффициенты, пока вы не поймёте, что наблюдаете. Сначала соберите робота, следующего по линии, чтобы настройка ПИД-регулятора и драйверы двигателей уже были знакомы; тогда проект сводится к одному контуру, а не к пяти неизвестным одновременно.
Почему мой робот балансировки вибрирует вместо того, чтобы стоять?
Обычно коэффициент пропорциональной составляющей слишком велик для частоты контура либо мешает люфт редуктора. Сначала уменьшите Kp. Если вибрация сохраняется при малом коэффициенте, посмотрите на колёса в момент смены направления: если перед началом вращения видна заметная мёртвая зона, проблема в редукторе, и металлогерящий двигатель исправит то, чего не исправит никакая настройка.
Действительно ли нужны энкодеры?
Чтобы подняться, нет. Внутреннему контуру нужен только угол наклона, поэтому робот вполне успешно уравновесится вообще без энкодеров — он будет лишь дрейфовать, потому что ничто не сообщает ему о положении. Энкодеры нужны внешнему контуру для удержания позиции. При желании сначала соберите без них, добейтесь равновесия, затем установите.
С какой частотой должен работать контур управления?
200 Hz — разумный минимум для внутреннего контура, а более высокая частота лучше. Естественное время падения робота задаёт предельный срок: небольшой робот переходит от вертикального положения в необратимое падение намного быстрее секунды, поэтому контур с частотой дискретизации 50 Hz имеет слишком мало возможностей вмешаться. Запускайте его от аппаратного таймера, а не через delay(), поскольку постоянство времени важнее для дифференциальной составляющей, чем точная частота.
Почему он уравновешивается, но медленно пересекает комнату?
Из-за перекоса при установке IMU. Если датчик стоит на градус отклонён от вертикали, контур удерживает неверный угол, а постоянный наклон означает постоянное ускорение. Измерьте смещение, когда робот действительно удерживается вертикально вручную, вычтите его как константу и позвольте интегральной составляющей поглотить остаток. Это можно в точности воспроизвести ползунком смещения IMU в симуляторе.