Управление скоростью колёс с обратной связью по энкодерам

ШИМ (PWM) — это напряжение, а не скорость

analogWrite(motorPin, 180) не запрашивает скорость. Он запрашивает около 70% напряжения аккумулятора, а получаемая скорость зависит от нагрузки на колесо, трения в редукторе, состояния заряда и температуры щёток.

Поэтому робот с двумя одинаковыми двигателями и одинаковым ШИМ едет по дуге. Они никогда не были одинаковыми: один редуктор туже, другой комплект щёток новее, и при одинаковом напряжении их скорости отличаются на 3–8%. На трёх метрах это заметная кривая.

На роботе с дифференциальным приводом это можно замаскировать константой подстройки. На базе mecanum — нет, потому что обратная кинематика задаёт четырём колёсам совершенно разные скорости: в приведённом там примере одному колесу достаётся 0.020 m/s, а другому 0.580 m/s из одной команды, и именно отношения определяют направление робота. Колесо, которое медленнее на 5%, не делает робота немного медленнее: оно изменяет направление движения робота.

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

Измеряйте скорость, которая действительно есть

Начните с отсчётов квадратурного энкодера. Преобразование задаётся фиксированной геометрией:

counts per wheel revolution = CPR_motor × gear ratio × 4     (×4 for quadrature edges)
metres per count            = 2πr / counts per revolution

Для энкодера двигателя 12 CPR за редуктором 30:1 и колёсами 97 mm: 12 × 30 × 4 = 1440 отсчётов/оборот, а 2π × 0.0485 / 1440 = 212 µm на отсчёт.

Теперь часть, которую все делают неправильно:

// DON'T: counts since last loop, divided by loop time.
float speed = (count - lastCount) * METRES_PER_COUNT / dt;

При 100 Hz и 0.05 m/s на одну выборку приходится один отсчёт. Сигнал скорости равен 0, 212, 0, 212 µm — прямоугольный сигнал с пульсациями ±100%, а D усилит их в гудящий двигатель. Есть три выхода, и обычно нужен первый:

1. Выбирайте медленнее, чем управляете. Вычисляйте скорость за 20 ms (50 Hz) и запускайте контур с частотой 50 Hz. В окне больше отсчётов, квантование меньше, запаздывание фильтра отсутствует.
2. Измеряйте время между фронтами, а не число фронтов за время. Это отлично работает на низкой скорости, неудобно на высокой и требует временных меток прерываний.
3. Пропустите скорость через фильтр нижних частот. Это дешевле всего, но расходует запас устойчивости — того самого, которого у вас меньше всего.

// A 50 Hz velocity estimate, with the counter read atomically because an
// encoder ISR is writing it.
noInterrupts();
long count = encoderCount;
interrupts();

float dt = (now - lastTime) * 1e-6f;             // micros(), so no ms rounding
float v = (count - lastCount) * METRES_PER_COUNT / dt;
lastCount = count; lastTime = now;

volatile long encoderCount и блок noInterrupts() не опциональны: чтение 32 бит на AVR занимает несколько инструкций, и если обработчик прерывания попадёт посередине, получится число, которого никогда не было.

Сначала упреждение, затем PID

Первое, что хочется сделать, — взять ПИД-регулятор (PID). Сначала выполните простое 90%.

Установившаяся скорость двигателя постоянного тока (DC) близка к линейной зависимости от приложенного напряжения, поэтому, зная нужную скорость, можно вычислить большую часть ШИМ:

pwm ≈ kS·sign(v) + kV·v

• kV — наклон. Сколько ШИМ требуется на один m/s.
• kS — свободный член. ШИМ, необходимый только для преодоления статического трения, до того как колесо вообще начнёт двигаться.

Измерьте оба значения за десять минут:

1. Поставьте робота на опоры так, чтобы колёса свободно вращались.
2. Медленно увеличивайте ШИМ, пока колесо только-только не начнёт вращаться. Это kS, обычно 25–45 из 255.
3. Задайте ШИМ 100, 150, 200, 250 и запишите установившуюся скорость для каждого значения.
4. Аппроксимируйте прямой. Её наклон — kV, и при продолжении назад он должен приблизительно дать kS.

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

float feedforward = kS * sign(target) + kV * target;
float output = feedforward + kP * error + kI * integral;

Без упреждения интегратору при каждом изменении цели приходится строить всю команду с нуля. Это медленно, приводит к перерегулированию и является причиной того, что многие настроенные вручную контуры двигателей получают огромный kI и робот с рывками.

Регулятор

P и I. Не D.

struct WheelLoop {
  float kS, kV, kP, kI;
  float integral = 0, target = 0;

  int update(float measured, float dt) {
    float error = target - measured;
    float ff = kS * (target > 0 ? 1 : target < 0 ? -1 : 0) + kV * target;
    float unclamped = ff + kP * error + kI * (integral + error * dt);
    int   output    = (int)constrain(unclamped, -255, 255);

    // Conditional integration: only accumulate if the output is NOT saturated,
    // or if the error would wind the integrator back toward the middle.
    // Otherwise a stalled wheel builds a command it cannot use and then keeps
    // driving for a second after you let go of the stick.
    if (fabsf(unclamped) < 255.0f || (error * unclamped) < 0) integral += error * dt;

    return output;
  }
};

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

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

Настройка в правильном порядке

1. Только упреждение. kP = kI = 0. Задайте 0.3 m/s и измерьте результат. Отклонение в пределах примерно 10% означает, что kV выбран верно. Исправьте его до любых других изменений.
2. Увеличивайте kP до появления гудения, затем отступите на треть. Вы слушаете вой, который появляется вместе с коэффициентом и исчезает без него.
3. Увеличивайте kI, пока не исчезнет установившаяся ошибка. Колесу с правильным упреждением нужно совсем немного. Начните с kP/10 в секунду и увеличивайте постепенно.
4. Проверяйте под нагрузкой, а не на опорах. Ведь важно проверить колесо, которому задана наибольшая скорость.
5. Проверяйте все четыре колеса вместе, двигаясь по диагонали. В этой команде самый большой разброс целей колёс, и именно там проявится ненастроенное колесо.

Симулятор ПИД-регулятора быстрее робота развивает интуицию для шагов 2 и 3, а руководство по настройке описывает общую процедуру.

Частота дискретизации

Частота контура должна быть значительно выше частоты управляемого процесса. У небольшого редукторного двигателя постоянного тока скорость устанавливается с постоянной времени примерно 50–150 ms; обычное правило — десять выборок за это время, то есть 50–200 Hz.

Запускайте по фиксированному расписанию, а не «как быстро работает loop()», потому что dt входит и в оценку скорости, и в интегратор:

const unsigned long PERIOD_US = 10000;   // 100 Hz
static unsigned long next = 0;

void loop() {
  unsigned long now = micros();
  if ((long)(now - next) < 0) return;    // signed compare survives rollover
  next += PERIOD_US;
  // ... read encoders, run four loops, write four PWMs
}

Знаковое сравнение — та же ловушка переполнения millis() в форме micros(), где она возникает через 71 минуту, а не через 49 дней.

Что это даёт в измерениях

Один и тот же робот, одна и та же команда, один и тот же пол:

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

Диагностика

Далее

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