Симулятор балансирующего робота: ПИД (PID) маятника

Управление

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

Самый прямой способ управления — сам робот: перетащите корпус на любой угол и отпустите. Поза, в которой вы его оставили, — это поза, которую подхватит регулятор; отдельного шага повторного взведения нет, а ползунки коэффициентов применяются к работающей симуляции сразу. Установите коэффициент производной в ноль во время балансировки, и он начнёт колебаться в ту же секунду.

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

Установите фокус на холсте и используйте Space, S, L, R и F для тех же элементов управления с клавиатуры.

Теория

Удержание робота на двух колёсах означает удержание перевёрнутого маятника в вертикальном положении. Центр масс находится выше оси, поэтому вертикальное положение является равновесием в том же смысле, что и карандаш, стоящий на острие: математически реальное, физически невозможное без постоянной коррекции. Наклоните его на любой угол, и гравитация создаст момент, который наклонит ещё сильнее. Чем дальше он уходит, тем быстрее движется.

У робота есть ровно один способ противостоять этому моменту — переместить ось. Ускорение колёс вперёд возвращает основание под центр масс, поворачивая корпус обратно к вертикали. Поэтому коррекция поначалу кажется перевёрнутой: чтобы остановить падение вперёд, нужно ехать вперёд, точно как вы подводите руку под удерживаемую метлу, а не убираете её.

Это задаёт роботу жёсткий предел. При угле примерно больше 35° необходимое горизонтальное ускорение превышает то, что колёса могут обеспечить до удара корпуса о пол, и никакое увеличение коэффициентов не помогает. Пунктирные линии на графике отмечают эту границу.

Алгоритм

Регулятор каскадный — два контура ПИД-регулятора, один задаёт уставку другому.

Внутренний контур работает быстро и удерживает угол наклона. Он сравнивает измеренный угол с целью и формирует команду приводу:

error   = measured_angle − target_angle
motor   = Kp·error + Ki·∫error + Kd·d(measured)/dt

Обратите внимание: ошибка измеряется как измеренная минус целевая, а не наоборот, как в учебнике. Чтобы поймать падение вперёд, нужно двигать колёса вперёд, поэтому положительный наклон должен давать положительную команду приводу. Этот знак — самая частая причина того, что первый балансирующий робот с включением падает на пол.

Внешний контур работает медленнее и удерживает положение. Он не может управлять двигателями — они уже заняты внутренним контуром, — поэтому задаёт целевой наклон для внутреннего контура:

target_angle = Kp_pos·(desired_position − position) − Kd_pos·velocity

Когда робот отстаёт от желаемого положения, внешний контур запрашивает небольшой наклон вперёд; внутренний затем движет колёса, чтобы удержать этот наклон, и робот перемещается вперёд. Наклон намеренно ограничен несколькими градусами. Внешний контур, которому разрешено требовать большой угол, передаст внутреннему цель, которую тот не может удержать, и оба контура сразу потеряют устойчивость.

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

Исходный код

// Inner loop: tilt → motors. Runs every 5 ms off a timer, not in loop().
float angleError = angle - tiltTarget;          // measured minus target
angleIntegral   = constrain(angleIntegral + angleError * dt, -3, 3);
float command   = Kp * angleError
                + Ki * angleIntegral
                + Kd * gyroRate;                // rate straight from the gyro

command = constrain(command, -255, 255);
if (fabs(angle) > FALL_ANGLE) command = 0;      // past saving; cut the motors
driveMotors(command, command);

// Outer loop: position → tilt target. Runs ten times slower, and may only
// ever ask for a small lean.
if (++slowTick >= 10) {
  slowTick = 0;
  float positionError = 0 - wheelPosition;
  tiltTarget = constrain(KpPos * positionError - KdPos * wheelVelocity,
                         -0.12, 0.12);          // radians, about ±7°
}

Две детали напрямую переносятся из этого симулятора. Берите производную от угловой скорости по данным гироскопа, а не вычисляйте разность углов: гироскоп измеряет эту скорость напрямую и без шума, который усиливает дифференцирование. И отключайте двигатели за пределом угла восстановления, а не заставляйте регулятор продолжать прилагать усилие, — так вы сохраните редукторы и сделаете отказ очевидным, а не насильственным.

Оборудование

Датчик — MPU6050, содержащий одновременно акселерометр и гироскоп, потому что по отдельности ни один из них непригоден. Акселерометр знает, где находится низ, но его сигнал подавляется собственным движением робота; гироскоп чисто отслеживает вращение, но дрейфует за секунды. Комплементарный фильтр формирует на их основе стабильный и быстрый сигнал угла.

Для двигателей нужно достаточно низкое передаточное число, чтобы получить момент, и достаточно быстрые, чтобы поймать падение, — редукторные двигатели N20 около 200 RPM часто выбирают именно поэтому; ими управляет TB6612FNG, низкое падение напряжения которого важно, когда контур тысячи раз в секунду запрашивает небольшие коррекции.

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

Допущения и отличия от реального оборудования

Модель представляет собой 2D-модель перевёрнутого маятника с одной осью наклона, жёсткими звеньями, без проскальзывания колёс и IMU, который сообщает истинный угол и скорость только с дрейфом и шумом, заданными параметрами модели. Настоящий балансирующий робот ощущает момент двигателя, меняющийся вместе с напряжением аккумулятора, квантование энкодера, смещение монтажа IMU и температурный дрейф, а также может упасть набок — степень свободы, которую эта лаборатория полностью убирает. Каскадная структура ПИД-регулятора и производная по угловой скорости гироскопа переносятся напрямую, но смещение при монтаже, настройку фильтра и отсечку по углу падения необходимо заново определить на физическом шасси.

Схема соединений

Два правила соединений определяют, заработает ли этот робот.

Аккумулятор двигателей подключается только к выводу VM драйвера и больше никуда. Ток двигателя никогда не должен проходить через Arduino, а все общие провода — плата, драйвер, IMU и отрицательный вывод аккумулятора — должны быть соединены, иначе управляющие сигналы не имеют общей точки отсчёта, а отказ будет незаметным.

Энкодер питается от логического питания, а не от шины двигателей. Питание энкодера от источника двигателя либо выводит его из строя, либо заполняет отсчёты коммутационными помехами. Канал A энкодера также подключите к выводу с поддержкой прерываний; опрос из loop() теряет отсчёты, а потерянные отсчёты означают, что внешний контур постепенно теряет положение робота.

Установите IMU как можно ближе к оси колёсной базы. Высокое расположение умножает линейное ускорение, которое он видит при каждой коррекции, и делает задачу объединения данных сложнее без какой-либо пользы.