Симулятор трапецеидального профиля и S-кривой

Управление

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

Отобразить переключает между обоими профилями и каждым по отдельности. Воспроизвести перемещает временной маркер по графикам в реальном времени — это самый быстрый способ почувствовать, что S-кривая заканчивается позже.

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

Теория

Четыре графика расположены друг над другом, и каждый является производной предыдущего. Такая компоновка и есть доказательство: излом скорости — это скачок ускорения, а скачок ускорения — импульс рывка. При дифференцировании разрыв не исчезает — он перемещается вниз, пока не становится неограниченным.

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

Профиль типа S-кривая ограничивает также рывок. Ускорение нарастает в течение t_j = a_max / j_max, удерживается и снова убывает, поэтому скорость приобретает скруглённые углы. Он обеспечивает ограниченный рывок.

Ни один профиль не может всегда выполнить каждое ограничение. Перемещение короче v²/a никогда не достигает максимальной скорости. S-кривая с максимальной скоростью ниже a²/j никогда не достигает предела ускорения. И то и другое обычно, и оба случая видны здесь.

Алгоритм

Лаборатория решает профиль, а не интегрирует его. Внутри одного сегмента рывок постоянен, поэтому:

a(τ) = a₀ + j·τ
v(τ) = v₀ + a₀·τ + j·τ²/2
x(τ) = x₀ + v₀·τ + a₀·τ²/2 + j·τ³/6

Эти выражения точны, поэтому перемещение попадает точно в цель, а не рядом с ней, и перетаскивание ползунка сразу перепланирует движение.

Выбор пиковой скорости использует одно тождество: разгон скорости нечётно-симметричен относительно середины, поэтому при любой форме он всегда проходит ровно v_peak/2 × duration. Это превращает вопрос «есть ли этап постоянной скорости?» в одно сравнение и позволяет получить пиковую скорость с ограничением расстояния в замкнутом виде — квадратное уравнение, пока ускорение насыщается, и кубический корень после этого.

Исходный код

Небольшой генератор трапецеидального профиля, который можно вставить в скетч. Он возвращает положение, за которым должен следовать ваш контур управления в момент t:

// Target position t seconds into a trapezoidal move of `d` mm.
float profilePosition(float t, float d, float vMax, float a) {
  float tAcc = vMax / a;
  float dAcc = vMax * vMax / (2 * a);

  if (2 * dAcc > d) {                      // too short to reach vMax
    float tPeak = sqrtf(d / a);            // triangular profile
    if (t >= 2 * tPeak) return d;
    if (t < tPeak) return 0.5f * a * t * t;
    float left = 2 * tPeak - t;
    return d - 0.5f * a * left * left;
  }

  float tCruise = (d - 2 * dAcc) / vMax;
  float tTotal  = 2 * tAcc + tCruise;
  if (t >= tTotal) return d;
  if (t < tAcc)           return 0.5f * a * t * t;
  if (t < tAcc + tCruise) return dAcc + vMax * (t - tAcc);

  float left = tTotal - t;
  return d - 0.5f * a * left * left;
}

Вызывайте его один раз за период управления и передавайте результат любому замкнутому контуру. AccelStepper делает эквивалентное внутренне для шаговых двигателей; опции S-кривой там нет, поэтому профиль с ограничением рывка нужно создавать самостоятельно.

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

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

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

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

target position  →  PROFILE  →  setpoint  →  controller  →  driver  →  motor
                                                 ↑                        │
                                                 └────── encoder ─────────┘

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

Полный вывод, путаница вокруг «рывка» в Marlin и причина того, почему профилирование часто исправляет ПИД-регулятор, который выглядел плохо настроенным, приведены в профилях движения: трапецеидальное ускорение и S-кривая.