Фильтр может быть использован при решении различных задач управления приводами, кранами, станками с ЧПУ, 3d принтерами, в железнодорожном и другом транспорте, в радиолокации, в авиации, при управлении БЛА, в ракето- и торпедо-строении, при проектировании лифтов, при построении различными претензионных систем плавного пуска и торможения.
При "медленно меняющихся" входных значениях выходная последовательность повторяет входную (должна!).
Выходные значения "стараются максимально быстро повторять" входные, но на первую, вторую и третью производную выходного значения накладываются ограничения.
Для инициализации и сброса фильтра служит функция rfilt_init().
Для обновления параметров фильтра "на лету" служит функция rfilt_tune().
Основная рабочая функция фильтра - rfilt_step().
Считается, что выходное значение на каждом интервале меняется по закону:
dt = t[i+1] - t[i]
x[i+1] = x[i] + V[i] * dt + A[i] * dt^2 / 2 + R[i] * dt^3 / 6 (1)
где:
dt - шаг разбиения по времени;
x[i] - выходное значение на текущем шаге
x[i+1] - выходное значение на следующем шаге
V[i] - текущая (начальная на интервале) скорость
A[i] - текущее (начальника на интервале) ускорение
R[i] - выбранный для данного интервала рывок
Скорость и ускорение расчитывается следующим образом:
V[i+1] = V[i] + A[i] * dt + R[i] * dt^2 / 2 (2)
aAi+1] = A[i] + R[i] * dt (3)
Для того, чтобы при вычислениях не использовать dt, dt^2 и dt^3 используются приведенные величины скорости, ускорения и рывка (фактически это конечные разности первого, второго и третьего порядка):
v[i] = dx[i] = V[i] * dt
a[i] = d2x[i] = A[i] * dt^2
r[i] = d3x[i] = R[i] * dt^3
При такой подмене формулы (1), (2) и (3) принимают упрощенный вид:
x[i+1] = x[i] + v[i] + a[i] / 2 + r[i] / 6 (1')
v[i+1] = v[i] + a[i] + r[i] / 2 (2')
a[i+1] = a[i] + r[i] (3')
Не простой задачей является вычисление оптимального значения величины r[i] для каждого интервала разбиения. Оптимизация проводится по минимальному времени достижения заданного значения на выходе фильтра при движении с заданными ограничениями величин скорости, ускорения и рывка.
Пользователь задает следующие ограничения для работы фильтра:
v_max - максимальная скорость изменения выходного значения
a_up_max - максимальное ускорение разгона
a_down_max - максимальное ускорение торможения
r_max - максимальный рывок
beta - коэффициент запаса фильтра по рывку от 0.001 до 0.1
x_min, x_mаx - минимальное и максимальное выходное значение
При работе фильтра далее используются следующие обозначения приведенных значений порогов скорости, ускорения и рывка:
V = v_max * dt, V > 0
Au = a_up_max * dt^2, Au > 0
Ad = a_down_max * dt^2, Ad > 0
R = r_max * dt^3, R > 0
Для текущего зачтения скорости и ускорения вычисляется "минимальный тормозной путь" - величина перемещения при оптимальном торможении до нулевой скорости и нулевого ускорения с ограничением скорости, ускорения и рывка.
Если "пора/нужно тормозить", то выбирается рывок для данной фазы оптимального торможения.
Рывок рассчитывается пропорционально после оценок двух тормозных путей:
- тормозной путь после такта с максимальным положительным рывком;
- тормозной путь после такта с максимальным отрицательным рывком. Если заданная точка находится "в трубке", то рывок вычисляется пропорционально промахам. Если трубка "не дотягивает" или "перепрыгивает" заданное значение, то берется максимальный/минимальный рывок.
Если модуль скорости растет то это разгон (v[i] * a[i] > 0), если модуль скорости снижается, то это торможение (v[i] * a[i] < 0).
Так как при разгоне и торможении используются разные параметры ограничения ускорения, то может использоваться следующий код:
trend = v[i] * a[i];
if (trend < 0.)
{ // торможение (скорость и ускорение не равны нулю и имеют разный знак
A = Ad; // максимальное приведенное ускорение торможения
}
else if (trend > 0. || a[i] != 0.)
{ // разгон (знаки скорости и ускорения совпадают
// или есть ускорение при нулевой скорости)
A = Au; // максимальное приведенное ускорение разгона
}
-
Полный покой Предыдущее выходное значение равно заданному, текущая скорость и ускорение равны нули. Нулевой рывок сохраняет состояние системы. Полный покой - это частный случай малого хода и отдельно в программе не обрабатывается.
-
Малый ход Текущие малые значения скорости и ускорения, близость заданного значения к предыдущему позволяют без превышения установленных ограничения по скорости, ускорения и рывку произвести перемещение за время не более одного такта dt в заданную точку.
Малый ход - это работа фильтра в режиме "трубы", когда выходные значения полностью без задержек и искажений повторяют входные.
-
Торможение В случае, если кротчайший тормозной путь приводит к выбегу выходного значения за заданное значение, то активируется программа торможения.
-
Реверс Направление движения (знак скорости) не соответствует разнице заданного и текущего положения. Требуется изменить направление движения.
На реверсе - требуется достичь максимального ускорения торможения Ad до тех пор пока "неверная" скорость не достигнет нуля.
- Разгон Производится набор скорости (с максимально допустимым рывком и ускорением).
Режим разгона возможен, если величина тормозного пути не превышает разницу заданного и текущего положения.
Критическая скорость разгона:
Vcrit = V - a[i]^2 / (2. * R),
где
V - максимально допустимая приведенная скорость (V > 0),
R - максимально допустимый приведенный рывок (R > 0).
Определение возможности свободного разгона без превышения предельной скорости можно следующим образом:
if ((a[i] > 0. && v[i] >= Vcrit) ||
(a[i] < 0. && v[i] <= -Vcrit))
{ // текущая скорость достигла критической
// с максимальным по модулю рывком снизить ускорение до нуля
}
- Удержание Vmax При достижение максимальной скорости v[i] >= V или v[i] <= -V ускорение должно быть обнулено.
| Nреж | ds = y[i]-x[i] | v | a | Режим |
|---|---|---|---|---|
| 1 | 0 | 0 | 0 | Покой |
| 2 | ~0 | ~0 | ~0 | Малый ход |
| 3 | + | + | - | Торможение |
| 3 | - | - | + | Торможение |
| 4 | + | - | - | Провал в реверс |
| 4 | - | + | + | Провал в реверс |
| 4 | + | - | + | Выход из реверса |
| 4 | - | + | - | Выход из реверса |
| 5 | + | + | + | Разгон |
| 5 | - | - | - | Разгон |
| 5 | + | + | 0 | Удержание +Vmax |
| 5 | - | - | 0 | Удержание -Vmax |
Примечание: На начальной фазе программного торможения из-за ограничения рывка знаки могут соответствовать режму "Разгон", пока ускорение не помянет знак на соответствующий снижению скорости.
При расчете тормозного пути УДОБНО запомнить знак скорости (фактически направление движения) и если скорость отрицательна поменять знак скорости и ускорения на противоположный, вычислить тормозной путь (время торможения) для положительного значения начальной скорости (что удобнее, меньше шансов совершить ошибку). Знак тормозного пути поменять на выходе, если знак скорости и ускорения менялся.
Скорость набираемая/сбрасываемая за время снижения ускорения разгона/торможения до нуля:
dV = a^2 / (2 * R), (4)
где
a - ускорение на начале интервала (начальное ускорение)
R - рывок с которым ведется линейное снижение до нуля ускорения
Еще раз в рамочку, соотношение скорости и ускорения, которые могут обеспечить торможение с постоянным рывком R до полной остановки (v = a = 0):
2 * V * R = A^2
T = A / R
Если осуществляется движение с постоянной скоростью (ускорение равно нулю), то торможение может состоять из 2-х или 3-х фаз.
Если начальная скорость (по модулю) |v| <= Ad^2 / R, где Ad - максимальное ускорение торможения, R - максимальный рывок, то торможение состоит из 2-х "симметричных" фаз:
- фаза линейного набора ускорения торможения до amax = sqrt(|v| * R), где v - скорость на начале интервала, R - максимальный рывок (скорость нарастания ускорения торможения)
- фаза линейного снижения ускорения торможения с amax до нуля. Время обоих фаз одинаковое. Из-за сравнительно небольшой начальной скорости за время тормозного пути ускорение торможения не успевает достигнуть максимально допустимого значения Ad. Время каждой фазы t1 = t2 = |amax| / R = sqrt(|v| / R)
Время полного торможения T2 = t1 + t2 = 2 * sqrt(|v| / R). (5)
Тормозной путь второй фазы можно легко посчитать, если представить движение в обратном времени с постоянным рывком R от нулевой скорости и ускорения до скорости |v|/2 и ускорения |amax| в течении t2:
s2 = 1/6 * R * t2^3 = 1/6 * |v| * sqrt(|v| / R) = 1/6 * amax^3 / R^2
Тормозной путь первой фазы можно представить как площадь под графиком скорости, а это площадь прямоугольника |v| * t1 за вычетом s2, т.е s1 = |v| * sqrt(|v| / R) - s2,
Общий тормозной путь:
S2 = s1 + s2 = |v| * sqrt(|v| / R) (6)
Если начальная скорость (по модулю) |v| > Ad^2 / R, то торможение состоит из 3-х фаз:
- фаза линейного набора ускорения торможения до максимального Ad
- фаза равноускоренного торможение с максимальным ускорением Ad
- фаза линейного снижения ускорения торможения с Ad до нуля и сброс скорости
Время первой и третей фазы t1 = t3 = Ad / R. Время второй фазы можно посчитать как время сброса скорости с постоянным ускорением ad на величину |v| - 2 * V12 с ускорением Ad, где V12 = Ad^2 / (2 * R) - величины сброса скорости на 1-й и 3-й фазе, то есть: t2 = |v| / Ad - Ad / R.
Время полного торможения T3 = t1 + t2 + t3 = |v| / Ad + Ad / R (7)
Тормозной путь первой фазы - как площадь под графиком функции скорости: s1 = |v| * t1 - s3 = |v| * Ad / R - s3
Тормозной путь второй фазы - площадь трапеции с основаниями v1 = |v| - Ad^2 / (2 * R), v2 = Ad^2 / (2 * R) и высотой t2 s2 = t2 * (v1 + v2) / 2 = (|v| / Ad - Ad / R) * |v| / 2
Суммарный тормозной путь всех 3-х фаз
S3 = s1 + s2 + s3 = |v| * (Ad / R + |v| / A) / 2 (8)
При разгоне, когда знаки скорости и ускорения совпадают при использовании максимального рывка для обнуления ускорения разгона затрачивается время:
Ts = |a| / R, (9)
где |a| - модуль текущего ускорения разгона, а часть тормозного пути составляет
Ss = |a| / R * (|v| + a^2 / (3 * R)) (10)
После данного интервала (сброса ускорения разгона) расчёт тормозного пути и времени может быть осуществлен в соответствии с алгоритмом торможения после равномерного движения с постоянной скорости, приведенном выше.
После начала "настоящего" торможения, когда знаки скорости и ускорения не совпадают, тормозной путь и время вычисляется особенно хлопотно.
Возможно торможение в 2 фазы:
- увеличение ускорения торможения вплоть до максимального Ad,
- сброс ускорения торможения до нуля и полный останов. Данные фазы аналогичны 2-м фазам торможения после равномерного движения с постоянной скоростью с особенностью в том, что первая фаза "усечена" по времени т.к. мы уже тормозим и набрали какое-то отрицательное по отношение к скорости ускорение.
Возможно торможение в 3 фазы:
- увеличение ускорения торможения до максимального Ad,
- торможение с максимальным ускорением Ad,
- сброс ускорения торможения до нуля и полный останов. Данные фазы аналогичны 3-м фазам торможения после равномерного движения с постоянной скоростью с особенностью в том, что первая фаза "усечена" по времени т.к. мы уже тормозим и набрали какое-то отрицательное по отношение к скорости ускорение.
Возможна щекотливая ситуация (почему?) с "выбегом" по скорости, когда за время сбрасывания ускорения с максимальным рывком скорость успевает поменять знак, что приводит к торможению в 3-5 фаз с реверсом (изменением знака скорости):
- сброс скорости до нуля, при этом ускорение до нуля не доходит
- сброс ускорения до нуля
- 2 или 3 фазы торможения после движения с постоянной скоростью (см. выше). За счет "выбега" (изменения знака скорости) торможение происходит "долго".
Ответ на вопрос "почему?": Во время движения с большой скоростью произошло крутое изменение заданного значения. Во время действия большого ускорения (ускорения разворота) скорость упала. Если скорость |V| < A^2/(2 * R), то не возможно с ограниченным рывком обнулить одновременно скорость и ускорение.
Если модуль скорости меньше, чем v1 = a^2 / (2 * R), то торможение будет с "выбегом" с большим числом фаз.
Если модуль скорости больше, чем v2 = (Ad^2 - a[i]^2 / 2) / R, то торможение в три фазы, в противном случае в две. Примечание:
v2 = dV1 + dV3 - пороговая скорость для 3-х фазного торможения
dV1 = (Ad^2 - a[i]^2) / (2 * R) - падение скорости по модулю за первую фазу
dV3 = Ad^2 / (2 * R) - скорость на входе в третью фазу
Первая фаза из трех (увеличиваем тормозное ускорение с а[i] до Ad):
t1 = (Ad - |a[i]|) / R - время ф.1 (12)
s1 = v * t1 + a * t1^2 / 2 - R * t1^3 / 6 - путь ф.1 (13)
Вторая фаза из трех (тормозим с постоянным ускорением Ad):
t2 = (|v| - v2) / Ad; - время ф.2 (14)
s2 = (|v| + a^2 / (2 * R)) * t2 / 2 - путь ф.2 (15)
Примечаение: s2 - это площадь трапеции с основаниями: Va = |v| - dV1, Vb = dV3 и высотой h=t2. (16) s2 = (Va + Vb) * h / 2 (17)
Третья фаза из трех (обнуляем скорость и ускорение с максимальным рывком):
t3 = Ad / R - время фазы 3 (18)
s3 = R * t3^3 / 6 - путь фазы 3 (19)
Если торможение в две фазы (v <= v2), то вычисляем:
Amax = sqrt(R * |v| + a^2 / 2) - максимальное по модулю ускорения (20)
Первая фаза из двух (увеличиваем тормозное ускорения с а[i] до Amax):
t1 = (Amax - |a|) / R - время первой фазы (21)
s1 = v * t1 + a * t1^2 / 2 - R * t1^3 / 6 - путь первой фазы (22)
Вторая фаза из двух (обнуляем скорость и ускорение с максимальным рывком):
t2 = Amax / R - время второй фазы (23)
s2 = R * t2^3 / 6. - путь второй фазы (24)
Если торможение с реверсом (v <= v1 = a^2 / (2 * R)), то вычисляем:
// фаза 1 (сбрасываем ускорение до нуля)
t1 = |a| / R
s1 = v * t1 + a * t1^2 / 2. + R * t1^3 / 6.;
// теперь задача сводится к торможению после равномерного движения
// в две или три фазы
v = v1 - |v|; // считаем, что остаточная скорость положительна для удобства
if (v <= Ad * Ad / R)
{ // торможение состоит из 3-х фаз
t2 = 2 * sqrt(v / R); // время фазы 2 и 3
s2 = -dt * v / 2.; // тормозной путь ф2 и ф3
}
else
{ // торможение состоит из 4-х фаз
t2 = v / Ad + Ad / R; // время фаз 2, 3 и 4
s23 = -v * t2 / 2.; // тормозной путь ф2..ф4
}
s = s1 + s2;
t = t1 + t2;
}
rfilt_init()- функция первичной инициализации компонентаrfilt_trfilt_tune()- обновление параметров фильтра «на лету»rfilt_step()- основная рабочая функция
rfilt_limit_r()- функция ограничения рывка с учётом текущей скорости, ускорения и заданных при инициализации параметровrfilt_s()- основная идейная функций вычисления тормозного пути и времениrfilt_test()- функция оценки возможности «малого хода»rfilt_r()- функция итеративного вычисления рывка для следующего такта