Теория управления шаговым двигателем (или как вертеть PTZ камеру)

Настал тот редкий случай, когда в работе программиста микроконтроллеров появилась потребность в математике и даже физике. У меня на работе была интересная задача по теме разработки тяжелой (70кг) PTZ камеры. Там была задачка по физике из раздела кинематики. Правда по уровню сложности её можно отнести к школьной программе 6го класса.

Шаговые двигатели применяются в CNC станках, 2D/3D принтерах, сканерах, плоттерах, PTZ камерах, крутилках радарных антенн, турелей, в линейных актюаторах, в стрелочных приборах (аналоговые часы) и может быть еще где-то.

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

Постановка задачи

дано:

Дан угол, PhiSet  на который следует повернуть вал шагового двигателя. Двигатель работает в трех режимах: ускорение ACCEL, работа на постоянной крейсерской скорости RUN,  замедление DECEL. Получается трапециевидный профиль скорости.

Или ускорение ACCEL и замедление DECEL. Получается треугольный профиль скорости (в случае, если крейсерская скорость OmegaMax очень высока,  а угловой путь set, что надо пройти, очень мал).

В качестве параметров движения даны только ускорение BettaMax [rad/s^2] и крейсерская скорость OmegaMax [rad/s] в режиме RUN. В нулевой момент времени t0 вал трогается с места и начинает набирать угловую скорость Omega с постоянным ускорением BettaMax.  Вычислить момент времени t1, при котором  следует остановить ускорение ACCEL и начать вращение на постоянной скорости OmegaMax (фаза RUN), и вычислить момент время t2, при котором следует остановить движение на постоянной скорости OmegaMax RUN и начать замедление DECEL.

Примечание

Моменты времени t1 и t2 могут совпадать, если BettaMax очень высоко, а OmegaMax мало.  Для симметрии и простоты положим, что ускорение во время набора скорости и ускорение во время замедления по модулю равны BettaMax, но противоположны по знаку. 

Терминология

Профиль - это график зависимости угловой скорости от времени Omega(t).

Фаза RUN - движение с постоянной скоростью.

Хороший инженер должен не только решить задачу, но и быть способным объяснить доступным языком свое решение коллегам.

Решение

Из кинематики известно, что путь это интеграл скорости по времени. В данном случае интеграл считать нет смысла так как фигуры простецкие: 2 треугольника да прямоугольник.

Первым делом процессор должен решить нужна ли вообще фаза RUN?

Площадь равнобедренного треугольника равна. По сути можно сложить квадрат.

Если phi_set < S0, то фаза RUN не нужна и получится треугольный профиль. Если S0 < phi_set, то фаза RUN однозначно нужна и получится трапецивидный профиль. Как выразить S1 зная BettaMax?

Получается что

При треугольном профиле получается, что

Треугольный профиль (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 ) приходится ориентироваться не по времени, как в непрерывной постановке, а по пройденному пути (количеству сделанных шагов шагового двигателя). Про это есть публикация Asynchronous Control System for Stepper Motor With an Incremental Encoder Feedback.

Вывод

Как видите обыкновенной школьной математики вполне достаточно, чтобы решать реальные оценочные задачи из прода станкостроения.

При расчетах всегда на каждом шаге проверяйте размерность в формулах.

Если вы программируете микроконтроллеры и вам приходилось прибегать к математике то пишите это в комментариях.

Links

Online редактор формул http://latex.codecogs.com/eqneditor/editor.php

https://ww1.microchip.com/downloads/en/Appnotes/doc8017.pdf