Профили движения: трапецеидальное ускорение и S-кривая

Введение

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

Профиль движения — это недостающая информация. Он превращает «перейти на 100 mm» в расписание — где ось должна находиться в каждый момент между текущим временем и прибытием — и передаёт регулятору цель, которая перемещается со скоростью, которую машина действительно может обеспечить.

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

Что на самом деле требует команда только положения

Рассмотрим скачок: в момент t = 0 цель меняется с 0 mm на 100 mm.

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

• Шаговому двигателю задают частоту шагов, которую его момент не может обеспечить на такой скорости, он пропускает шаги и, работая без обратной связи, даже не знает об этом. Положение теперь постоянно неверно. Почему это происходит и почему всегда четырьмя шагами.
• Двигатель постоянного тока (DC) с контуром ПИД-регулятора (PID) видит огромную мгновенную ошибку, насыщает выход, накапливает интегральную составляющую и перерегулирует.
• Сервопривод уходит на максимальную скорость и прибывает, когда весь механизм ещё движется.

Ни одна из этих проблем не связана с настройкой. Это одна и та же проблема: команда была физически невыполнима.

Трапецеидальный профиль

Исправление — задавать скорость и изменять её с темпом, который двигатель способен поддерживать. Разгоняйтесь с постоянным ускорением до крейсерской скорости, удерживайте её, затем тормозите с тем же ускорением. Если построить график скорости от времени, получится трапеция — отсюда название.

Три числа определяют профиль: расстояние d, максимальную скорость v_max и ускорение a.

t_accel = v_max / a                     time spent ramping
d_accel = v_max² / (2a)                 distance used by one ramp
t_total = v_max / a  +  d / v_max       whole move, if it reaches v_max

Для перемещения на 100 mm со скоростью 100 mm/s и a = 500 mm/s²: каждый разгон занимает 0.2 s и проходит 10 mm, оставляя 80 mm крейсерского движения, то есть всего 1.2 s.

Вот вся функция от времени — положение, за которым должен следовать ваш контур управления в момент 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;
}

Обратите внимание, чем эта функция не является: это не регулятор. Она формирует уставку. Всё, что было у вас раньше — планировщик шагов шагового двигателя, контур ПИД-регулятора, команда сервоприводу — продолжает выполнять следующее. Профиль меняет только то, чему следуют.

Запрошенное перемещение не всегда получается

Ветвь 2 * dAcc > d — первая, с которой сталкиваются люди, и её часто диагностируют неправильно.

Если перемещение короче расстояния, необходимого для разгона и повторного торможения, ось никогда не достигает заданной скорости. Она разгоняется, достигает середины пути и сразу тормозит — получается треугольник, а не трапеция.

Порог равен d < v_max² / a. Для приведённых выше чисел это 20 mm, поэтому любое перемещение короче 20 mm достигает скорости ниже 100 mm/s независимо от настроек.

Именно поэтому повышение v_max на машине с множеством коротких перемещений часто не даёт измеримого результата. Эти перемещения никогда не ограничивались скоростью; они ограничивались ускорением. Повседневный пример — 3D-принтер, выполняющий плотное заполнение.

Повороты по-прежнему разрывны

Трапецеидальный профиль исправил скорость. Посмотрите на ускорение — проблема просто переместилась.

В момент t = 0 ускорение мгновенно меняется с 0 на 500 mm/s². В верхней точке разгона оно мгновенно возвращается к 0. Каждое изменение — это скачок ускорения, а скорость изменения ускорения называется рывком, поэтому скачок ускорения означает бесконечный импульс рывка.

Бесконечность — тоже не число, которое машина может создать. Вместо неё получается:

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

S-кривая

Профиль S-кривой ограничивает не только ускорение, но и рывок. Вместо скачка к полному ускорению он плавно входит в него за время рывка t_j = a_max / j_max, удерживает его, а затем плавно выходит. Кривая скорости приобретает скруглённые углы — отсюда название, — а ускорение становится собственной трапецией.

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

Во что обходится: ровно одно t_j

Эту часть стоит запомнить, потому что она делает компромисс конкретным.

Когда оба профиля достигают одинаковых v_max и a_max, перемещение по S-кривой занимает больше времени ровно на t_j — на одно время рывка, а не на два, независимо от длины перемещения:

trapezoidal   T = v_max/a + d/v_max
S-curve       T = v_max/a + d/v_max + t_j          where t_j = a_max / j_max

Для приведённого перемещения 1.2 s превращается в 1.3 s. Это штраф 8% для этого перемещения и меньший процент для более длинных, но взамен получается ограниченный рывок.

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

В этой формуле скрыто ограничение. Ускорение достигает a_max только при v_max ≥ a_max² / j_max. Ниже этого порога перемещение не достигает и полного ускорения, и получается треугольный профиль ускорения — та же история «не хватило расстояния», но на один порядок производной выше.

«Рывок» в прошивке принтера — не рывок

Об этом стоит знать до настройки чего-либо, потому что словарь активно вводит в заблуждение.

Классический параметр Marlin DEFAULT_XJERK не является производной ускорения. Это максимальное мгновенное изменение скорости, разрешённое в стыке двух перемещений, в mm/s — запас для прохождения угла, позволяющий планировщику пройти поворот без торможения до нуля. Это полезно, но другая величина с неправильным названием и неправильными единицами.

Более новые версии Marlin по умолчанию используют Junction Deviation (отклонение в стыках), моделируя угол как дугу окружности с заданным отклонением в mm. Klipper использует square_corner_velocity — скорость прохождения угла 90°. Все три отвечают на вопрос «как не останавливаться на каждом углу?», и ни одна из них не ограничивает рывок в физическом смысле.

Поэтому если сообщение на форуме советует уменьшить jerk (рывок) для прекращения звона, сначала выясните, какая величина имеется в виду. Уменьшение скорости в стыке снижает удар при прохождении угла, но не ограничивает da/dt внутри одного перемещения.

Ваш ПИД, скорее всего, в порядке

Если замкнутый контур оси перерегулирует и колеблется при каждом перемещении, первым порывом будет расстроить его — уменьшить Kp, добавить производную и бороться с перерегулированием.

Сначала попробуйте задать профиль уставки.

Ступенчатый вход требует от ПИД-регулятора (PID) бесконечного выхода. Его член ошибки начинается с полного расстояния перемещения, производная видит мгновенный скачок, а интегральная составляющая накапливается, пока выход насыщен, а ось не может реагировать. Расстройка делает регулятор вялым везде, чтобы пережить один невозможный момент.

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

Где это встречается на реальном роботе

• Шаговые двигатели. setMaxSpeed() и setAcceleration() в AccelStepper дают трапецеидальный профиль и больше ничего. Опции S-кривой нет; для неё нужно самостоятельно формировать уставку.
• Сервоприводы любительского класса. У SG90 есть внутренний регулятор, который нельзя увидеть или настроить, поэтому профилирование означает постепенное изменение заданного угла вместо одной записи цели — та же идея, применённая снаружи.
• Двигатели постоянного тока (DC) с энкодером. Профилируйте уставку положения и позвольте ПИД-регулятору с энкодером следовать за ней. Именно в этой схеме профилирование окупается лучше всего.
• Мобильные роботы. То же ограничение действует для linear.x в мосте cmd_vel: скачок команды скорости заставляет колёса проскальзывать, что портит одометрию. Ограничьте скорость изменения вектора twist.
• Принтеры и станки с ЧПУ (CNC). Marlin, Klipper и GRBL планируют трапецеидальные перемещения с правилом прохождения углов между ними.

Когда что-то идёт не так

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