Каскадное ПИД-управление самобалансирующимся роботом

Введение

У вас есть надёжно измеренный угол наклона. Теперь нужно удерживать робота в этом положении, а затем, со временем, добиться, чтобы он оставался на месте, а не перемещался по комнате, формально оставаясь вертикальным.

Это две разные задачи, и почти все сначала пытаются решить их одним контуром. В этом руководстве они реализованы как два вложенных контура ПИД (PID) — именно так управляют настоящими балансирующими роботами.

Почему одного контура недостаточно

Вам нужен робот, который остаётся там, куда его поставили. Управлять можно моментом двигателей. Между ними находится неудобный факт: единственный способ изменить положение робота — сначала изменить его наклон.

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

Выход — управлять двумя величинами с разными скоростями, каскадом.

Структура

   position error → [ OUTER PID ] → tilt target
                                        ↓
   tilt error     → [ INNER PID ] → motor command → robot
                                        ↑
                                   measured tilt

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

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

Это разделение частот — вся суть проектирования. Если внешний контур будет работать с частотой, близкой к внутреннему, они начнут бороться: внешний контур меняет цель быстрее, чем внутренний успевает за ней последовать, и робот раскачает сам себя до падения.

Внутренний контур

float angleError = angle - tiltTarget;      // measured MINUS target
angleIntegral   = constrain(angleIntegral + angleError * dt, -3, 3);

float command = Kp * angleError
              + Ki * angleIntegral
              + Kd * gyroRate;

command = constrain(command, -255, 255);

Три детали здесь важнее коэффициентов.

Ошибка вычисляется как измерение минус цель, в отличие от учебной формулы setpoint − measurement. Чтобы поймать падающего вперёд робота, нужно вести колёса вперёд, под падающую массу, как подставляют руку под метлу. Поэтому положительный наклон должен давать положительную команду двигателям. Если поменять знак, регулятор введёт робота в пол сразу после включения — характерный и резкий сбой, почти всегда вызванный этим знаком.

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

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

Внешний контур

if (++slowTick >= 10) {                     // ten times slower
  slowTick = 0;
  float positionError = targetPosition - wheelPosition;
  tiltTarget = constrain(KpPos * positionError - KdPos * wheelVelocity,
                         -0.12, 0.12);      // radians, about ±7°
}

wheelPosition получают суммированием отсчётов энкодера. Если энкодеров нет, можно интегрировать команду двигателей как грубую замену, но контур станет заметно хуже.

Ограничение обязательно. Внешний контур, которому разрешено требовать 30°, передаёт внутреннему цель, которую тот физически не удержит: робот примет наклон, из которого не сможет восстановиться, и упадёт, буквально выполняя команды. Нескольких градусов достаточно; наклон 7° уже заметно разгоняет робота.

Обратите внимание, что составляющая скорости вычитается. Она гасит приближение, чтобы робот не проскочил мимо цели и не возвращался.

Настройка в рабочем порядке

Сначала настройте только внутренний контур. Установите KpPos и KdPos в ноль, чтобы внешний контур не влиял на робота, а tiltTarget оставался нулевым. Робот будет балансировать на месте и медленно дрейфовать — это ожидаемо и пока нормально.

1. Установите Ki и Kd в ноль. Увеличивайте Kp, пока робот не начнёт резко реагировать на толчок. Слишком мало — он провисает к полу, почти не сопротивляясь; слишком много — гудит.
2. Увеличивайте Kd, пока колебания не погаснут. Это составляющая превращает вибрирующего робота в робота, который стабилизируется. В симуляторе балансировки можно установить D в ноль на работающем роботе и посчитать колебания до падения.
3. Увеличивайте Ki последним и осторожно. Он устраняет медленный наклон из-за смещения крепления IMU. Слишком большое значение вызывает медленную нарастающую раскачку, когда интеграл перерегулируется и разворачивается.

Только после этого включите внешний контур и начните с KpPos примерно одной десятой от интуитивно разумного. Его действие косвенное: вы ждёте, что робот перестанет дрейфовать, а не что отклик будет резким.

Режимы сбоя и их смысл

Отключайте двигатели, когда восстановление невозможно

if (fabs(angle) > FALL_ANGLE) {             // about 35 degrees
  driveMotors(0, 0);
  angleIntegral = 0;
  return;
}

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

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

Куда двигаться дальше

Коэффициенты здесь взаимодействуют сильнее, чем в одноконтурном ПИД, потому что объект неустойчив: нет состояния «примерно правильно», которое лишь плохо работает. Робот либо стоит, либо падает.

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