Как устроено управление движением
Команда конечной позиции не означает мгновенную установку выхода мотора. В Extension хост задаёт цель и ограничения, сервопривод строит траекторию, измеряет движение и рассчитывает воздействие. Эта страница объясняет связи между этими этапами; порядок практической настройки приведён отдельно.
Как работает режим Extension
Хост — компьютер или контроллер, подключённый к сервоприводам. Он отправляет цель: конечное положение и ограничение скорости. Ускорение и параметры компенсации настраиваются до включения мотора. Затем каждый сервопривод:
- Планирует плавное движение к цели.
- Измеряет положение и оценивает скорость.
- Меняет управляющее воздействие, чтобы следовать плану.
- После движения удерживает конечное положение.
Запланированные положение и скорость в каждый момент называются заданием (reference). Регулятор сравнивает их с измеренным движением, поэтому заданное и фактическое положения могут различаться.
| Термин | Значение |
|---|---|
| Отсчёт энкодера | Единица измерения положения; номинально 4096 отсчётов на оборот |
| Effort | Знаковая команда воздействия на мотор: знак определяет направление, величина — силу выходного воздействия |
| Коэффициент (gain) | Настройка, определяющая интенсивность реакции регулятора |
| Feedforward | Дополнительное воздействие, рассчитанное по ожидаемой потребности до появления ошибки положения или скорости |
| Feedback | Измерения и состояние, возвращаемые сервоприводом |
| Регистр | Настройка или показание, обозначенные номером |
| Сессия | Период управления Extension с номером, который задаёт хост |
Effort выражается в единицах ШИМ, управляющих электрическим выходом на мотор. Extension допускает предел воздействия до 1000 в каждом направлении. Это не прямое измерение момента или тока.
S-образное движение
Ускорение и торможение меняются постепенно. Кривая показывает ускорение во времени. Это концептуальная схема, а не измерения оборудования.
S-образный профиль постепенно увеличивает и уменьшает ускорение. Вместо мгновенного полного разгона или торможения сервопривод плавно входит в каждую часть движения и выходит из неё. Это может уменьшать механические толчки и вибрацию.
Полное движение из покоя в покой состоит из этапов:
| Этап | Что происходит |
|---|---|
| 1 | Ускорение постепенно увеличивается |
| 2 | Разгон с постоянным ускорением |
| 3 | Ускорение постепенно уменьшается до нуля |
| 4 | Движение с постоянной скоростью |
| 5 | Торможение постепенно усиливается |
| 6 | Торможение с постоянным замедлением |
| 7 | Торможение постепенно уменьшается до остановки |
При коротком перемещении некоторые этапы пропускаются, если расстояния недостаточно для запрошенной скорости или ускорения. Один предел ускорения применяется и при разгоне, и при торможении.
ramp_time_ms задаёт время роста ускорения от нуля до предела. Большее время делает изменение мягче, но обычно требует больше времени и расстояния для разгона и остановки.
Рывок (jerk) — скорость изменения ускорения. Планировщик ограничивает рывок, поэтому ускорение меняется плавно. Сам рывок может изменяться скачком между этапами. Его приближённая величина:
jerk = 1000 × acceleration / ramp_time_ms [counts/s³]
В формуле ускорение выражено в отсчётах/с², время нарастания — в мс, рывок — в отсчётах/с³.
Изменение цели во время движения
Новая цель продолжает текущий план, сохраняя его положение, скорость и ускорение. Повторная отправка той же цели не перезапускает движение. Во время движения сохраняйте запрошенный предел скорости; изменить его можно для следующего перемещения после остановки планового движения. Ускорение и время нарастания входят в конфигурацию: для их изменения нужны выключенный мотор и неактивная сессия.
Цель за сохранёнными Legacy-пределами позиции сдвигается к ближайшему пределу. Запрошенная скорость также ограничивается настроенным пределом. Каждое перемещение использует настроенное ускорение. Нулевая скорость или нулевое настроенное ускорение недопустимы.
Всё запланированное движение должно оставаться внутри пределов. Поэтому новая цель может быть отклонена, если сервопривод уже приближается к пределу слишком быстро и не успевает затормозить, даже когда сама цель допустима.
После прибытия сервопривод автоматически удерживает цель с включённым мотором. Измеренная позиция может отличаться из-за трения, нагрузки, ограничений воздействия или настройки. Удержание продолжает исправлять ошибку положения, а не фиксирует выходное воздействие на постоянном значении.
Каждый сервопривод строит свой план. Одновременная отправка нескольких целей не гарантирует одновременного прибытия.
Общие пределы позиции
Legacy и Extension используют одни регистры пределов: 9–10 содержат минимум, 11–12 — максимум. Каждый предел — беззнаковое 16-битное значение в скорректированных отсчётах энкодера с учётом направления и коррекции позиции сервопривода. Отдельных пределов Extension нет.
Калибровка связывает отсчёты с физическими положениями. Инструмент калибровки присваивает зафиксированной опорной позе 2048, затем измеряет пределы хода. Это значение не становится автоматически нулевым углом сустава; измеренная середина хода не заменяет опорную позу. Модели гравитации и визуализация требуют соответствующего преобразования отсчётов в углы.
Прошивка читает эти регистры при запуске управления. Если ограничения включены, минимум должен быть меньше максимума, максимум — не больше 32767, а измеренная позиция — находиться внутри диапазона. Иначе запуск завершается ошибкой без движения мотора. Планировщик удерживает всё движение, включая торможение, в прочитанном диапазоне.
Равные минимум и максимум отключают пределы хода, как в Legacy. При этом команды Extension остаются в поддерживаемом диапазоне позиции от −32767 до 32767 отсчётов.
Принятая запись любого предела завершает активную Extension-сессию и выключает мотор, даже если запись применена только в RAM. Для новых значений подготовьте новую сессию. Обычные правила постоянного сохранения Legacy-настроек продолжают действовать.
PID-регулятор
PID определяет, как сервопривод исправляет ошибку положения. Серая кривая показывает колебания, белая — более затухающий отклик; пунктир — цель. Это концептуальная схема, а не измерения оборудования.
Регулятор меняет воздействие на мотор, чтобы следовать запланированному движению. Три составляющие выполняют разные задачи:
| Настройка | Действие | Основной компромисс |
|---|---|---|
| P — коррекция положения | Притягивает мотор к запланированной позиции; большая ошибка вызывает большую коррекцию | Повышение P может уменьшить ошибку, но вызвать колебания |
| I — накопленная коррекция | Накапливает дополнительное воздействие при сохраняющейся ошибке | Может уменьшить постоянную ошибку, но увеличить перерегулирование или время установления |
| D — коррекция скорости | Реагирует на разность запланированной и измеренной скорости; сопротивляется движению при удержании | Может улучшить затухание, но чрезмерная реакция на шум скорости делает воздействие неравномерным |
Расчёт:
position error = planned position − measured position
speed error = planned speed − filtered measured speed
motor output = Kp × position error
+ Kd × speed error
+ accumulated I correction
+ gravity compensation
+ velocity feedforward
Здесь ошибки положения и скорости — разности задания и измерения; выход складывается из P, D, накопленной I-коррекции, компенсации гравитации и velocity feedforward.
Результат ограничивается настроенным максимумом воздействия. Защита мотора может уменьшить его дополнительно. Обратная связь показывает запрошенное и фактически приложенное воздействие, чтобы хост мог обнаружить ограничение.
Измеренная скорость сглаживается по недавним показаниям положения с помощью настраиваемого фильтра. Большее velocity_filter_q8 быстрее реагирует на изменения, но слабее подавляет шум.
Ограничение накопленной коррекции
У I-составляющей есть собственный предел. Регулятор также не позволяет ей продолжать накапливаться в направлении, в котором выход уже ограничен: это называется anti-windup. Составляющая может уменьшаться, помогая выйти из ограничения. Ki, равный нулю, отключает и обнуляет I-коррекцию.
Если сервопривод почти неподвижен не менее 50 мс, а ошибка позиции не превышает одного отсчёта, I перестаёт накапливаться. P, D и оба вида упреждающей компенсации продолжают работать. Отключение выхода мотора обнуляет I-коррекцию.
Упреждающая компенсация по скорости
Velocity feedforward добавляет воздействие на основании плановой скорости; расчёт выполняется внутри сервопривода. Обе кривые идут слева направо по времени. Это концептуальная схема, а не измерения оборудования.
Компенсация заранее обеспечивает часть воздействия, ожидаемого для движения, до появления ошибки слежения. Она рассчитывается внутри сервопривода по плановой скорости:
velocity feedforward = velocity gain × planned speed
Для каждого направления есть свой коэффициент и общий предел результирующего воздействия. Коэффициенты неотрицательные; знак выхода задаётся направлением движения. Предел этой составляющей не должен превышать основной предел воздействия мотора.
Сервопривод знает собственную плановую скорость, поэтому компенсация автоматически следует разгону и торможению и становится нулевой после остановки планового движения. Хост задаёт только настройки, непрерывно отправлять значения velocity feedforward ему не требуется.
Настраивайте эту функцию вместе с остальными параметрами 0xA0, при выключенном моторе и неактивной сессии. Для отключения задайте оба коэффициента и предел воздействия равными нулю.
Компенсация гравитации
Хост рассчитывает составляющую, сервопривод применяет её. Стрелка вниз обозначает вес, вверх — поддерживающее воздействие. Это концептуальная схема, а не измерения оборудования.
Компенсация создаёт воздействие, необходимое для поддержки веса руки и её нагрузки. Она полезна и при неподвижном удержании, когда velocity feedforward равен нулю.
Компенсацию гравитации рассчитывает хост, затем отправляет каждому сервоприводу знаковое значение воздействия. Расчёт может учитывать углы суставов, размеры руки, массы звеньев и полезную нагрузку. Хост должен преобразовать оценку нагрузки в единицы выхода сервопривода. В прошивке нет модели руки.
Направление имеет значение: положительный выход Extension соответствует росту отсчётов энкодера. Принятое направление угла сустава может быть противоположным; преобразование на хосте должно это учитывать.
Каждое обновление гравитации содержит желаемое воздействие. Slew rate — допустимая скорость его изменения в единицах воздействия в секунду — входит в основную конфигурацию и не меняется в течение сессии.
Сервопривод постепенно приближает компенсацию к запрошенному значению и добавляет её к выходу регулятора. Сумма всех составляющих ограничена общим пределом воздействия и защитой мотора.
Если обновления перестают поступать, компенсация продолжает изменяться к последнему запрошенному значению с настроенной скоростью, затем сохраняет его. Таймера истечения нет. Хост должен обновлять компенсацию при изменении позы или нагрузки. Запись нуля в регистр включения удержания 40 очищает компенсацию и выключает выход мотора.
Как использовать это при настройке
Если плановая скорость не успевает достичь заданного предела на коротком ходе, повышение предела не обязательно ускорит движение. Если выход долго ограничен, одних больших коэффициентов недостаточно: нужно проверить нагрузку, траекторию и допустимое воздействие. Если энкодер следует плану, а инструмент не попадает в физическую точку, проверьте калибровку, геометрию и механику.
Сохраняйте различие между плановой и измеренной траекторией, позиционной точностью и повторяемостью. Схемы и четыре ролика выше концептуальные: они не являются измеренными графиками конкретного сервопривода. Общая запись, если она приведена у схемы взаимодействия, содержит демонстрации оборудования, а не измерение внутренних сигналов этой схемы.
Главная раздела · Версия исходников: a995f77. Extension 9.0.