Ещё один PID-контроллер. Теория / forpes.ru

Главная
Ещё один PID-контроллер. Теория

Ещё один PID-контроллер. Теория +2

14.09.2025 14:02

he_projectile 0 2900 Источник

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

Итак, рабочая часть стенда представляет собой луч: неуравновешенную балку с мотором и датчиком. Когда луч находится в исходном состоянии — висит вертикально вниз. Тяга пропеллера создаёт момент силы, вращающий луч против часовой стрелки. Примем это направление за положительное, ограниченное углом 170°. В этом диапазоне сила тяжести всегда вращает луч в отрицательном направлении.

Измерение угла

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

В одной из статей я описал способ определения ориентации инерциального датчика. Ориентация представляется в виде матрицы $3 \times 3$ , в которой столбцы соответствуют осям датчика, а строки — проекциям на север, запад и вертикаль.
Казалось бы, достаточно прикрепить датчик к балке, посмотреть направление оси $\overrightarrow{z'}$ , запомнить направление $\overrightarrow{y'}$ , после поворота посчитать угол между прежним и текущим направлением. Но такой вариант доступен только при полной уверенности в том, что ось датчика $\overrightarrow{z'}$ параллельна оси луча, а я не стану гарантировать это.

Поэтому надо обобщить обе операции для случайного закрепления датчика. Как и раньше, алгоритм прототипируется в матлабе. МК обрабатывает данные датчика, вычисляет матрицу и отправляет её компьютеру по USB-CDC в виде массива из 9 байт. При запуске стенда луч неподвижен. Программа запоминает изначальную ориентацию датчика (). Матрица вращается вокруг вектора $\overrightarrow{a}$ , и получается матрица . Этот вектор и является осью вращения, надо вычислить его. Что мы знаем об $\overrightarrow{a}$ ? Его проекции на и одинаковы, при этом матрицы связаны матрицей перехода:

$M_{tr} = M_2\cdot M_1^{-1}\qquad (1)$

То есть, при применении к $\overrightarrow{a}$ линейного оператора $M_{tr}$ снова выходит $\overrightarrow{a}$ . Получается, что $\overrightarrow{a}$ является собственным вектором для матрицы перехода. Так как размерность матрицы 3, то и собственных векторов тоже 3. Как выбрать нужный? Сперва надо посчитать собственные числа $M_{tr}$ . Собственное число - это скаляр, который растягивает собственный вектор. При повороте не происходит растяжения вектора-оси, или же происходит растяжение с коэффициентом 1. Значит, нужно выбрать вектор, для которого $\lambda=1$ .

У этого решения есть несколько нюансов. Во-первых, погрешности измерений, расчётов и неровность механики приводят к тому, что $M_{tr}$ не в полной мере является ортогональной матрицей, и $\lambda \neq 1$ . Поэтому приходится выбирать число, наиболее близкое к 1. Во-вторых, вычисление собственных чисел матрицы — трудоёмкая задача как для МК, так и для программиста.
Прежде чем кинуться в написание сишных функций, я обратился за советом к ChatGPT и не пожалел.

Совет 1. Транспонировать матрицу вместо вычисления обратной. Для ортогональной матрицы, к которой относится моя матрица ориентации, $M^{-1}=M^T$ . Транспонировать матрицу проще, и это снизит нагрузку на работу МК.

Совет 2. Использовать след матрицы поворота для вычисления угла.

$tr(M_{tr})=M_{tr(11)}+M_{tr(22)}+M_{tr(33)}=1+2cos(\psi)\qquad (2)$ $\psi=acos\bigg(\frac{tr(M_{tr})-1}{2}\bigg),\qquad(3)$

где $\psi$ - угол поворота матрицы поворота.

Из-за погрешностей измерений и вычислений значение $tr(M_{tr})$ может выходить за границы [-1; 1], что вызовет ошибку вычисления арккосинуса, поэтому необходимо проверять это условие и обрезать лишнее. Кто-то скажет, что это заметание проблемы под ковёр, и я соглашусь. Но так как в МК сложно реализовать перехват исключений, то я решил проверять ортогональность и матрицы ориентации отдельно.
Данный метод возвращает $\theta$ при $0 \leq \theta < \pi$ и $2\pi -\theta$ при $\pi \leq \theta < 2\pi$ , однако это не проблема для моего стенда, так как отклонение луча ограничено.

Совет 3. Извлекать ось вращения из антисимметричной части матрицы поворота.

$\overrightarrow{a}=\frac{1}{2sin(\psi)}\cdot\begin{bmatrix}M_{tr(32)} -M_{tr(23)}\\M_{tr(13)} -M_{tr(31)}\\M_{tr(21)} -M_{tr(12)}\end{bmatrix}\qquad (4)$

Таким образом, вычисления угла и оси стали быстрыми и непринуждёнными. При запуске стенда необходимо перевести луч из нижнего положения в положение повыше. Из полученных данных будет вычислен вектор оси. Угол между осью и вертикалью должен иметь значение, близкое к $\pi/2$ . Сильное отклонение приведёт к переходу стенда в аварийное состояние и отключению мотора до перезапуска программы.

Расчёт параметров модели

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

Сила тяжести, действующая на луч и приложенная к центру масс луча, создаёт момент, равный

$\overrightarrow{M_g}=\overrightarrow{l_g} \times m \overrightarrow{g}\\или\\M_g = m \cdot g \cdot l_g \cdot sin(\theta - \varphi),\qquad (5)$

где m — масса луча, — расстояние от оси O до центра масс луча, $\theta$ — угол отклонения луча, $\varphi$ — угол между лучом и вектором до центра масс.

Так как отклонение луча ограничено механически 170°, направление момента не меняется.
Уравнение статики для этой системы выглядит так:

$\overrightarrow{M_g} + \overrightarrow{M_t} = 0\\или\\0=F_T \cdot l_T -m \cdot g \cdot l_m \cdot sin(\theta- \varphi), \qquad (6)$

где — расстояние между осью луча и осью пропеллера, — сила тяги пропеллера.

При дисбалансе сил возникает угловое ускорение:

$\ddot{\theta} = \frac{F_T \cdot l_T -m \cdot g \cdot l_m \cdot sin(\theta- \varphi)}{J}, \qquad (7)$

где - момент инерции луча.

Необходимо добавить сопротивления среды. К ним можно отнести силу трения покоя в подшипнике, силу сухого трения подшипника, силу вязкого сопротивления смазки и силу сопротивления воздуха. Силу сухого трения и трения покоя я исключу, так как хорошо смазал подшипники. Включим оставшиеся силы и получим уравнение динамики системы:

$\ddot{\theta} = \frac{F_T \cdot l_T -m \cdot g \cdot l_m \cdot sin(\theta-\varphi)-k_v\dot{\theta}-k_a\dot{\theta}^2\cdot sign(\dot{\theta})}{J}, \qquad (8)$

где - коэффициент вязкого сопротивления смазки, - коэффициент сопротивления воздуха.

Момент инерции и расстояние до центра масс вычислены в САПРе. Для этого я взвесил каждую деталь и прописал массу в физические свойства модели. Несмотря на то, что равномерность распределения массы по объёму является большим допущением для деталей, напечатанных на FDM-принтере, результат оказался впечатляюще удовлетворительным.
и - эмпирические коэффициенты. Я получил их значения экспериментально. Для этого закрепил мачту горизонтально, отвёл луч на произвольные 39 $^\circ$ , отпустил и позволил ему свободно колебаться под действием силы тяжести и получил функцию затухающих колебаний в табличном виде $\theta_{exp}(t)$ . При этом дифференциальное уравнение обретает вид:

$\begin{cases}\ddot{\theta} = \dfrac{-m \cdot g \cdot l_m \cdot sin(\theta-\varphi)-k_v\dot{\theta}-k_a\dot{\theta}^2\cdot sign(\dot{\theta})}{J}\\\theta(0)=-39 ^\circ\\\dot{\theta}=0\end{cases} \qquad (9)$

Затем написал MATLAB-скрипт, численно решающий приведённое уравнение с такими же начальными условиями, как у $\theta_{exp}(t)$ . Обозначу вычисленные данные как $\theta_{sim}(t)$ . Оба графика представлены на рисунке 3.

Как можно увидеть, частоты значительно различаются, примерно в два раза. Она зависит от параметров и . Подобрав ручками , я понял, что увеличение его вдвое исправляет проблему. После проверки масс всех деталей, я вспомнил, что САПР указывает момент инерции относительно центра масс. Чтобы пересчитать значения для вращения вокруг , нужно применить теорему Гюйгенса-Штейнера:

$J=J_C+m\cdot l_m^2,\qquad (10)$

где - момент инерции, рассчитанный САПРом.
Отклонение частоты уменьшилось до 3%, что видно на рисунке 4.

Рисунок 4. Графики отлично накладываются

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

$RSS(k_v, k_a) = \sum_{i=0}^{N}(\theta_{sim}(t_i,k_v, k_a)-\theta_{exp}(t_i))^2\qquad(11)$

Но вот незадача. Временные метки у $\theta_{exp}(t)$ и $\theta_{sim}(t)$ различаются, выбирать пары значений $\theta$ с одинаковым аргументом не получится. К тому же, частоты незначительно различаются, на шестнадцатом периоде колебания сдвинуты на $\pi/2$ , поэтому RSS будет меняться хаотично при переборе коэффициентов.
Однако известно, что и характеризуют форму огибающей и практически не влияют на период колебаний. Значит, достаточно сравнивать огибающие этих графиков. Для этого из $\theta_{exp}(t)$ и $\theta_{sim}(t)$ выделяются пики, обозначенные $\gamma_{exp}(t)$ и $\gamma_{sim}(t)$ . Затем надо интерполировать $\gamma_{sim}(t)$ сплайом, подставить временные метки из $\theta_{exp}(t)$ и получить соответствующие значения $\gamma_{simInt}(t)$ . Теперь можно сравнить огибающие. Таким образом, вычисление и сводится к минимизации функции RSS:

$RSS(k_v, k_a) = \sum_{i=0}^{N}(\gamma_{simInt}(t_i, k_v, k_a)-\gamma_{exp}(t_i))^2 \to \min_{k_a, k_v \in R^2}\qquad(12)$

Я воспользовался готовой функцией fmincon из пакета Optimization Toolbox. С начальным приближением $k_{v}=0, k_{a} = 0$ решение заняло 16 итераций. Результат можно видеть на рисунке 4, а полученные значения в таблице ниже.

	кг $\cdot$ м	5,52e-3
	кг	0,181
	м	0,139
$\psi$	град	1,71
	попугаи	1,85e-3
	попугаи	1,06е-3

Симуляция PID-регулятора

Наконец, можно приступить к моделированию PID-регулятора. Ограничу задачу достижением и поддержанием угла луча, но в будущем попробую также регулировать скорость вращения луча.

Итак, формула PID-регулятора:

$\begin{cases}u(t)= K_P\cdot e_{rr}(t) +K_I \cdot \int_0^t e_{rr}(\tau)\,d \tau -K_D \cdot \dfrac{de_{rr}(t)}{dt}\\e_{rr}(t)=\theta_{set}-\theta(t)\end{cases},\qquad(13)$

где $\theta_{set}$ - установочное значение, к которому стремится $\theta(t)$ , $e_{rr}(t)$ - ошибка регулирования, - управляющее воздействие, стремящееся уменьшить ошибку. $K_P,\,K_I,\,K_D$ - коэффициенты при пропорциональной, интегральной и дифференциальной составляющих регулятора соответственно.

Значения коэффициентов и определяют работу регулятора, их и предстоит подобрать.
Сперва необходимо врезать в дифур PID-регулятор.

$\begin{cases}\ddot{\theta}(t) = \frac{F_{max} \cdot u(t) \cdot l_T -m \cdot g \cdot l_m \cdot sin(\theta(t)-\varphi)-k_v\dot{\theta}(t)-k_a\dot{\theta}^2(t)\cdot sign(\dot{\theta}(t))} J\\u(t) \in [0,\; 1]\\\dot{\theta}(0) = 0 \\\theta(0) = \theta_0\end{cases}, \qquad(14)$

где $F_{max}$ - максимальная тяга, которую может выдать мотор. $\theta_0$ - начальный угол.

Регулятор управляет тягой, которая может быть только положительной и имеет ограничение сверху. Поэтому удобно ограничить также управляющий параметр . Для решения дифура необходимо принять начальные условия $\dot{\theta}(0)$ и $\theta(0)$ . Регулятор начинает работу при неподвижном опущенном луче, поэтому они равны нулю.

Это дифференциальное уравнение не решается аналитически. Хорошо, что MATLAB может решить его численно. Скрипт выглядит так:

% Параметры
l_m = norm([138.760 -4.145 ])/1000*1; % Расстояние до центра масс, м
m = 0.181;    % Масса, кг
psi = atan2(-4.145, 138.760); % Угол между балкой и вектором до центра масс, радианы
J = 2036.662 /10^6+m*l_m^2; % Момент инерции, кг*м^2
g = 9.81; % м/с^2
kv = 0.0001850; % Сопротивление смазки, попугаи
ka = 0.0001058; % Сопротивление воздуха, попугаи
l_T = 0.254; % Плечо пропеллера, м
F_T=10; % Максимальная тяга пропеллера, Н
alpha = deg2rad(90); % Цель

% Начальные условия
O0 = deg2rad(0);    % Начальный угол
omega0 = 0;   % Начальная угловая скорость
integral0 = 0;
Y0 = [O0; omega0; integral0];

% Время моделирования
tspan = [0 10];

% Решение дифура
[t, Y] = ode45(@(t, Y) pendulumODE(t, Y, l_m, m, g, J, psi, kv, ka, F_T, l_T, alpha, 0.3, 0.5, 0.05, 0.09), tspan, Y0);

% Графики
figure(1); % Создаю график с номером 1 / Обращаюсь к графику 1
clf(figure(1)) % Чищу график 1

plot(t, rad2deg(Y(:,1)), 'r', 'LineWidth', 1.5);
xlabel('Время, с');
ylabel('Наклон луча, град)');
title('Динамика угла');
ylim([0 120])
grid on;

yline(rad2deg(alpha))

function dYdt = pendulumODE(t, Y, l, m, g, J, psi, kv, ka, F_T, l_T, alpha, Kp, Ki, Kd, Ks)
    if Y(1) >= 0.99*pi
        Y(2) = 0;
        Y(1) = 0.99*pi;
    end  
    O = Y(1);         % Угол O
    omega = Y(2);     % Угловая скорость dO/dt
    e = (alpha-O);
    
    d_integral=e;
    
    if Y(3)>10
        Y(3) = 10;
    end
    if Y(3)<-10
        Y(3) = -10;
    end
    integral = Y(3); 
    
    U = Kp*e+Ki*integral-Kd*omega+Ks*sin(alpha);

    if U>1
        U = 1;
    end
    if U < 0.001
        U = 0.001;
    end
    F = F_T*U;
  
    domegadt = (F*l_T-m*g*l*sin(O-psi)-kv*omega*t-ka*omega^2*sign(omega)*t)/J;

    dYdt = [omega; domegadt; d_integral];  % Возвращаем вектор производных
end

Функция ode45 (строка 23) решает дифур итерациями, поэтому на каждом шаге можно выполнять математические операции над переменными, сравнивать и менять их значения. Это позволяет встроить в дифур различные проверки. Например, на строках 59-64 реализовано ограничение управляющего параметра.

Пора перейти к подбору коэффициентов. Для этого надо выбрать критерии качества процесса регулирования. Их много, но меня интересуют следующие:

Время переходного процесса (Т $_\text{ПП}$ ). Это время, за которое регулируемое значение входит в полосу шириной 2 $\Delta$ и больше не выходит из неё. Желаемое значение - 0.
Перерегулирование ( $\frac{\theta_{max} - \theta_\text{ЗЗ}}{\theta_\text{ЗЗ}}$ ). Желаемое значение - 0.
Ошибка регулирования ( $\frac{\theta_{\infty} - \theta_\text{ЗЗ}}{\theta_\text{ЗЗ}}$ ). Желаемое значение - 0.

Рисунок 5. Основные критерии качества процесса регулирования

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

Идеальный переходный процесс представляет собой ступенчатую функцию:

$\theta_{ideal}(t)=\begin{cases}0,\qquad t < 0\\\theta_\text{ЗЗ},\quad t \geq 0\end{cases}$

Регулирование начинается при t=0, значит меня интересует только $\theta_{ideal} = \theta_\text{ЗЗ}$ .Тогда функция меры приобретает следующий вид:

$err(P, I, D) = \int_0^\infty |\theta_{ideal}(t)-\theta(t, P, I, D)|\ dt,$

где $\theta(t)$ - результат симуляции, ,, - коэффициенты PID-регулятора.
Это называется интегральной оценкой качества переходного процесса.

Таким образом, минимизируя , можно получить приемлемый результат, который будет протестирован в следующей части.

Рисунок 6. Приемлемый процесс регулирования

edited by Ms.Кис-Кис-Кис-Кис

Комментарии (0)

T700
14.09.2025 14:58
#28835136
Это статический стенд? И задача вентилятора поддерживать горизонтальное положение стол (луч)?
1. he_projectile Автор
  14.09.2025 14:58
  #28835164
  Хм, да
  
  Я забыл дать ссылку на предыдущую часть, в которой описана конструкция
  
  Держите
  1. T700
    14.09.2025 14:58
    #28835176
    Если да, то я предлогаю полностью отказаться от MPU6050, и подобных микросхем, и вместо них, использовать концевик начала отсчёта (оптический лучше энкодер) и дисковый энкодер для получения точного угла поворота. Это простое решение и надежное (если будет защита энкодера от пыли).
    
    osmanpasha
    14.09.2025 14:58
    #28835290
    Можно ещё магнитный датчик положения поставить. Да можно и обычный потенциометр, в принципе.
    
    TimurZhoraev
    14.09.2025 14:58
    #28839966
    Да, магнитные сейчас довольно хорошие, небольшая проблема - держатель магнита с диаметральной намагниченностью, обычно делаемый 3Д-печатью и заливкой эпоксидкой, хотя некоторую кривизну можно довольно неплохо исправить программно полиномом или таблицей.

TimurZhoraev
14.09.2025 14:58
#28839904
Интересная задача. Попытался посмотреть аппроксимацию нелинейности звеном

Скрипт в Maxima для нахождения параметров колебательного звена
Программа
V1:b[0]/(a[2]*p^2+a[1]*p+1) /*Переход к КЧХ*/; V2:p=%i*2*%pi*f; /*Комплексно-частотная характеристика КЧХ*/ V3:V1,V2; /*Мнимая и действительная части*/ V3R:realpart(V3);V3I:imagpart(V3); /*Точка с нулевой фазой - действительная часть равна нулю (частота свободных колебаний)*/ V4:solve(V3R=0,[a[2]]); /*находим в точке f0*/ V5:V4,f=f0; /*Подставляем в исходное выражение*/ V6:factor(V1),V5; /*Обратное преобразование Лапласа (получение импульсной характеристики) выбрать n - дискриминант отрицательный*/ V6i:ilt(V6,p,t); /*Амплитуда при синусе при t=0*/ V6a1:V6i/sin(2*%pi*f0*sqrt(1-%pi^2*a[1]^2*f0^2)*t); V6a:V6a1,t=0; /*В самом начале амплитуда равна значению в точке A*/ V6b:V6a=A; /*решение для корректировочного коэффициента b0*/ Vb0:solve(V6b,b[0]); assume(τ>0,f0>0); /*Постоянная времени переходного процесса*/ V7:2*%pi^2*a[1]*f0^2=τ; /*Решение для коэффициента a[1]*/ V8:solve(V7,a[1]); Vb01:factor(Vb0),V8; /*Нахождение постоянной времени по уровню*/ V9:A*exp(-τ*t0)=B; V10:solve(V9,τ); /*Нахождение численных параметров по графику f0 - за 16+1/4 периода время 15 секунд точка A в самом начале, точка B - 10 периодов */ V11:[f0=(16+1/4)/15, A=39, B=10];V111:t0=10*f0,V11; /*полный набор численных параметров*/ Num:push(V111,V11); /*Постоянная времени*/ V12:V10,Num;; Num1:append(V12,Num); Vbn:factor(Vb01),Num1; /*коэффициенты a1 , a2, b[0]*/ V13:[V8,V5,Vbn],Num1; float(V13); /*подстановка в импульсную характеристику*/; V14:factor(-V6i),[V13,Num]; wxplot2d(V14,[t,0,25]);
В итоге получилась следующая аппроксимация звеном вида : $b_0/(a_2 \cdot p^2+a_1 \cdot p+1)$ ,

при этом, . В результате имеем следующую импульсную характеристику (с нулевыми начальными условиями, в исходной модели, видимо, они не нулевые, -40 начальный угол)

Импульсная характеристика линейного звена второго порядка
Видно, что +- график одинаковый с точки зрения общей "энергетики" переходного процесса. В конце имеется в оригинальной модели статическое значение порядка -2, и более быстрая диссипация, связанная с сухим трением (вязкий коэффициент). Поэтому для управления целесообразно рассмотреть применение корневых методов для линеаризированного объекта управления.
Что касается замкнутой системы то желаемый результат есть фактически предельно апериодический процесс с кратным апериодическим полюсом. В этом случае все полюса расположены рядом в левой полуплоскости на действительной оси. Как только появляются комплексно-сопряжённые полюса, то имеется перерегулирование. Чем левее они расположены - тем быстрее, но тем больше может быть всплеск, также он характерен и для апериодического процесса с разнесёнными постоянными времени. Круговая частота колебаний определяется мнимой осью, постоянная времени - действительной. В этом случае полюса рационально расположить в секторе порядка 45 град исходящего из нуля, там будет и быстродействие приемлемое и колебательность небольшая. Также ПИД-регулятор с таким объектом сразу в замкнутой системе даст 4й порядок (2+2). Поэтому рационально использовать компенсатор. Очень грубо он может быть синтезирован из указанного выше объекта управления как режекторный фильтр $(a_2 \cdot p^2+a_1 \cdot p+1)/(q_0+p)^2$ , то есть как обратная передаточная функция с вспомогательным дважды апериодическим полюсом . Компенсатор позволяет "выровнять" АЧХ объекта управления, но уменьшает быстродействие. В этом случае он ставится в ошибку управления и к нему суммируется интегратор (и небольшая пропорциональная составляющая), либо на выходе интегратора, для таких объектов производная (дифференциальная) - явный путь к автоколебаниям, интересно взглянуть на результаты оптимизации по времени на соотношения этих постоянных времени, скорее всего дифференциальная будет практически нулевой. Ну и разумеется возможность применения апериодического управления - deadbeat control, но это только для дискретных систем.

Ещё один PID-контроллер. Теория +2

Измерение угла

Расчёт параметров модели

Симуляция PID-регулятора

Комментарии (0)

T700

he_projectile Автор

T700

osmanpasha

TimurZhoraev

TimurZhoraev