Настал тот редкий случай, когда в работе программиста микроконтроллеров появилась потребность в математике и даже физике. У меня на работе была интересная задача по теме разработки тяжелой (70кг) PTZ камеры.
Там была задачка по физике из раздела кинематики. Правда по уровню сложности её можно отнести к школьной программе 6го класса.
Шаговые двигатели применяются в CNC станках, 2D/3D принтерах, жестких дисках, сканерах, плоттерах, PTZ камерах, крутилках радарных антенн, турелей, в линейных актюаторах, в стрелочных приборах (аналоговые часы) и может быть еще где-то.
У шаговых двигателей есть одна проблема. Из-за высоких моментов инерции нагрузки шаговые двигатели могут потерять шаги и не отработать команды. Чтобы минимизировать потерю шагов вал шагового двигателя надо раскручивать плавно. Самый простой способ - это использовать трапециевидные профили движения.
Постановка задачи
дано:
Параметр |
Пояснение |
Units |
PhiSet |
Угол на который надо повернуть вал шагового двигателя |
Radians |
OmegaMax |
Максимально возможная угловая частота |
Radians/s |
BettaMax |
Максимальное угловое ускорение |
Radians/(s^2) |
Дан угол, PhiSet на который следует повернуть вал шагового двигателя. Двигатель работает в трех режимах: ускорение ACCEL, работа на постоянной крейсерской скорости RUN, замедление DECEL. Получается трапециевидный профиль скорости.
Или ускорение ACCEL и замедление DECEL. Получается треугольный профиль скорости (в случае, если крейсерская скорость OmegaMax очень высока, а угловой путь PhiSet, что надо пройти, очень мал).
В качестве параметров движения даны только ускорение BettaMax [rad/s^2] и крейсерская скорость OmegaMax [rad/s] в режиме RUN. В нулевой момент времени t0 вал трогается с места и начинает набирать угловую скорость Omega с постоянным ускорением BettaMax. Вычислить момент времени t1, при котором следует остановить ускорение ACCEL и начать вращение на постоянной скорости OmegaMax (фаза RUN), и вычислить момент время t2, при котором следует остановить движение на постоянной скорости OmegaMax RUN и начать замедление DECEL.
Примечание
Моменты времени t1 и t2 могут совпадать, если BettaMax очень высоко, а PhiSet мало. Для симметрии и простоты положим, что ускорение во время набора скорости и ускорение во время замедления по модулю равны BettaMax, но противоположны по знаку.
Терминология
Профиль - это график зависимости угловой скорости от времени Omega(t).
Фаза RUN - движение с постоянной скоростью.
Хороший инженер должен не только решить задачу, но и быть способным внятно объяснить доступным языком свое решение коллегам.
Решение
Из кинематики известно, что путь это интеграл скорости по времени. В данном случае интеграл считать нет смысла так как фигуры простецкие: 2 треугольника да прямоугольник.
Первым делом процессор должен решить нужна ли вообще фаза RUN?
Площадь равнобедренного треугольника равна S0. По сути можно сложить квадрат.
Если phi_set < S0, то фаза RUN не нужна и получится треугольный профиль. Если S0 < phi_set, то фаза RUN однозначно нужна и получится трапецевидный профиль. Как выразить S0 зная BettaMax?
Получается что S0
При треугольном профиле получается, что
Треугольный профиль (phi_set <= S0)
Ок. Как же определить t1(время переключения) в случае треугольного профиля? Надо просто решить данную систему уравнений. Выразить t1. t0 можно обнулить.
Выразив OmegaTop из каждой формулы и приравняв их можно выразить t1. Получается
А теперь случай трапецевидного профиля (S0 < phi_set)
С t1 просто. Из OmegaMax=BettaMax*t1 получаем
В трапецевидном профиле площадь состоит из 2х треугольников и одного прямоугольника
ввиду симметрии получим
Выражаем время включения фазы замедления
Как видите все расчеты был составлен чисто из геометрических соображений (площадь графика ω(t) всегда должна быть равна phi_set ).
Ниже представлен простой алгоритм на языке Дракон, который помогает наглядно показать алгоритм вычисления моментов времени t1 и t2 переключения режимов ACCEL/RUN/DECEL при повороте на угол set в условиях заданной скорости OmegaMax в режиме RUN и допустимого ускорения BettaMax.
Решение на языке MATLAB
function [ t1 t2 tf wlim] = calcTimesF( phi_set, w_max, B_max )
S1=((w_max)^2)/B_max;
if(phi_set < S1)
%without RUN t1= t2
t1=sqrt(phi_set/B_max);
t2=t1;
tf=2*t1;
wlim = B_max*t1;
else
%with RUN t1 < t2
t1=w_max/B_max;
t2=(phi_set/w_max);
tf=t2+t1;
wlim = w_max;
end
end
Эту задачу в реальности надо решать для дискретного движения, так как в реальности имеем дело с шаговым двигателем. Там заметно усложняется решение. В изменении стадии движения (ACCEL, RUN, DECEL ) приходится ориентироваться не по времени, как в непрерывной постановке, а по пройденному пути (количеству сделанных шагов шагового двигателя). Так как MCU(шка) отлично считает количество положительных фронтов на проводе STEP. Плюс нужна реализация функции квадратного корня на уровне прошивки. Про это есть публикация Asynchronous Control System for Stepper Motor With an Incremental Encoder Feedback.
Вывод
Как видите обыкновенной школьной математики вполне достаточно, чтобы решать реальные оценочные задачи из prod(a) станкостроения.
При расчетах всегда на каждом шаге проверяйте размерность в формулах.
Если вы программируете микроконтроллеры и вам приходилось прибегать к математике, то пишите это в комментариях.
Акроним |
расшифровка |
PTZ |
Pan-tilt-zoom |
CNC |
computer numerical control |
Links
Online редактор формул http://latex.codecogs.com/eqneditor/editor.php
Комментарии (20)
iliasam
13.01.2023 07:20+8Я-то уж по заголовку подумал, что в статье будет что-то про микрошаг, особенности шаговых двигателей, травильный подбор тока.
DvoiNic
13.01.2023 10:31+2Подобную задачу сын в 9 классе решал, когда манипулятор делал… (для серв, но принцип тот же)
devlev
13.01.2023 11:46+2Как тут выше отметили - это все школьная задачка, а вот самое интересное начинается впереди, когда надо определять сами значения: максимальную угловую частоту и максмальное угловое ускорение. Мне кажется на больших шаговых двигателях без обратной связи будет сложно просто по одним шагам получить корректный результат.
DvoiNic
13.01.2023 12:22+1для приблизительного расчета можно взять паспортный момент двигателя (или сервы), и посчитать (хотя бы прикинуть) момент реальной полезной нагрузки (с учетом «сложных условий» — дополнительной нагрузки или погодных условий ).
mmMike
13.01.2023 13:25+1на практике я руками подбирал максимальное уcкскорение для каждой оси CNC.
Увеличиваешь пока пропуски не появятся. А потом ставишь в на 30% меньше в настройках.DvoiNic
13.01.2023 15:40+2Можно и так. Но так как у сына была задача учебная — я ему показывал как действовать не «методом тыка», а методом расчетов. Практика, в общем, подтвердила расчеты, хотя в частностях показала недостатки: при резком замедлении — «прекращении линейного ускорения», на точке перегиба — у сервы происходило «перерегулирование». Ну, заодно изучили возникающие явления — «колебания» и «резонанс».
Вылечили переводом ускорения-замедления в S-кривую, но уже без «расчетов», эмпирически.
VT100
13.01.2023 21:30+1Вот тоже бросился в глаза момент резкой смены ускорения, особенно на треугольном профиле.
devlev
13.01.2023 23:14+1Я пытался сделать программу для перемещения коллекторного двигателя с энкодером на заданное число оборотов (расстояние), чтобы оказаться в нужном месте. На рисунке ниже показано усредненный график (5 трем замерам) в шагах энкодера и 5 фаз двигателя по 1 секунде: стоп, вперед, стоп, назад, стоп. При этом для торможении я использовал реверс двигателя.
А вот рисунок как раз с выводом ускорений (разность предыдущего значение со следующим).
Собственно момент резкой смены ускорения это обсалютно нормально, а вот не линейность ускорений не очевидна.
Кстати о трапеции, был даже и такой график как раз где я пытался оперделить фазу двигателя когда он вышел на рабочии обороты - т.е. фаза ускорения пройдена. Ниже на графике синия линия это число шагов, а красная за счет специального коэффициента определеяет логическое значение для ускорения. На практике это правда не понадобилось - проще было по шагам вычислять когда нужно начинать тормозить.
MaxEkb77
13.01.2023 13:25-1Чем банальный пид не угодил ?
aabzel Автор
14.01.2023 03:41Пробовал и пид.
ПИД при старте видит, что ошибка большая и начинает скачком выставлять высокую скорость.
В результате теряются шаги микро шага. Еще и ременная передача пробуксовывает и образуется скрежет всей конструкции.
Чтобы этого избежать приходится после ПИД последовательно ставить цифровой фильтр нижних частот.
А потом еще с ПИД не понятно когда он, наконец, установится в допустимую ошибку. Это время непредсказуемое.MaxEkb77
14.01.2023 08:12+1ПИД при старте видит, что ошибка большая и начинает скачком выставлять высокую скорость
Значит настройки пида такие.
В результате теряются шаги микро шага. Еще и ременная передача пробуксовывает и образуется скрежет всей конструкции.
МБ проблемы с механикой еще ?
А потом еще с ПИД не понятно когда он, наконец, установится в допустимую ошибку. Это время непредсказуемое.
Вполне себе успешно справляется в пределах пары микрон при наличии обратной связи.
An_private
13.01.2023 13:28+4Что-то мне кажется, что в современных условиях это вообще не задача.
Эххх, разбередили :) Больше двадцати лет назад писал прошивку для управления синхронным приводом киношного 35 мм магнитофона ЛОМО. Трёхфазный сигнал + дополнительный пилот-тон, из которого внутренний привод решал - какое напряжение подавать на двигатель, чтобы он не вышел из синхрона. MCS-51 (ВЕ48), ассемблер, отладка методом прошивки ПЗУ и засовывания в кроватку. Плавный разгон с заданным ускорением, такое же плавное торможение и стабилизация скорости с отклонением не более 0.1% Эхххх, времена... :)
DvoiNic
13.01.2023 15:46+1времена меняются… сейчас это напишут на питоне, засунут в малинку… зато сделают это быстрее. Но будет жрать в разы больше ресурсов. ресурсы нынче дешевы.
aabzel Автор
13.01.2023 17:33Мы в 2015м писали прошивки-спинеры шаговых двигателей на STM32.
У ARM Cortex-M4 как раз есть поддержка вычисления корня одной ASM командой.
beeptec
14.01.2023 14:57+1Как видите обыкновенной школьной математики вполне достаточно, чтобы решать реальные оценочные задачи из prod(a) станкостроения.
Ой ли?
Всё уже давно решается до неприличия проще, посредством обратной связи с применением энкодеров.
Сегодня угловые датчики применяют абсолютно везде во всех отраслях от медицины до космоса. Причем датчики могут быть как аналоговыми, так и цифровыми.
Kabron287
14.01.2023 16:16-1DECEL вообще не нужен
aabzel Автор
14.01.2023 18:07На валу шагового двигателя может быть огромная нагрузка (у меня было 70кг) с высоким моментов инерции.
Если просто перестать подавать импульсы на драйвер шагового двигателя, то надо тогда отключать обмотки двигателя от чипа драйвера так как в этом режиме шаговый двигатель будет работать по инерции как генератор переменного тока.beeptec
14.01.2023 18:34+1Да, в режиме удержания вала, ампераж там в пределах номинальных параметров мотора.
Если речь идет о PTZ, там мотор самый что ни на есть слаботочный, не более 1А и в случае с конкретной задачей это вполне нормально. Камера всегда будет оставаться в четко задаваемом положении.
Генератором шаговик может работать только в том случае, как впрочем и любой мотор другого типа, если Вы его отсоедините от всего и будете непрерывно вращать вал.
Автор практически нигде не упоминает функцию сброса текущего состояния с возвращением камеры в условный "0", что важно для всего дальнейшего сценария видеоконтроля с момента старта или в случае аварийного останова и т.п., одними угловыми фидбэками задачу не решить. В обратной связи кроме энкодеров, я бы применил и функции машинного зрения (распознавания объектов или их перемещение в зоне контроля).
mmMike
Не усложняется, а упрощается. В целочисленной арифметике без всяких умножений и делений.
Просто приближением пошаговым (шаг двигателя) с двух концов траектории для заданного максимального ускорения.
И вообще, задача "от нуля в ноль скорости" для 2D это самый тривиальный случай. Сложнее, когда траекторию рассчитать нужно по отрезкам и нужно учитывать граничные ускорения на переходах между отрезками в 3D (CNC).
Хотя в конечном итоге все сводится к целочисленным вычислениям с int64
На микроконтроллерах чаще всего нет аппаратной реализации операций с плавающей точкой. Да и округления могут принести неожиданные сюрпризы с поломкой фрезы.