ПИД с ПОС?, убирать или оставить? Опции

[_Ivana]

Здравствуйте. Пытаюсь промоделировать механическую конструкцию, балансирующую за счет маятника-противовеса, управляемого двигателем с редуктором. Есть модель в симулинке, играюсь с ней. Датчика обратной связи 2 - гироскоп на конструкции, дающий угловую скорость, и энкодер на двигателе, дающий угол отклонения противовеса от условного нуля. Мне советовали взять линейную комбинацию невязок с каждого датчика и завести ее на общий ПИД, с выхода которого брать сигнал управления двигателем. Но я для расширения возможностей подаю сигнал с каждого датчика на свой ПИД со своими коэффициентами, а уже сумму выходов ПИД-ов использую в качестве сигнала управления. Подобрал коэффициенты, модель устойчиво балансирует, переваривает достаточно сильные шумы датчиком, люфт редуктора привода, импульсные и шумовые механические воздействия и толчки, скачкообразную смену уставки угла балансира и т.п., сносно выходит на заданную уставку. Но меня немного смущает то, что коэффициенты при пропорциональной и интегральной составляющих ПИДа энкодера получились разных знаков, то есть по одной ветке обратной связи у меня получилась ПОС. Причем, пока все мои попытки от нее избавиться приводят к ухудшению качества управления. Я встречался с электрическими схемами устойчивых усилителей, в которых ПОС была оправдана и компенсировалась более глубокой ООС, но в данном случае я подозреваю, что можно реализовать управление оптимальнее, убрав ПОС.
Что скажете, доктора? Файл модели в симулинке могу предоставить.

[_Ivana]

Вести с полей - почитал Формальского, переделал диффур для моего случая, линеаризовал его в окрестности нужного мне неустойчивого равновесия, вычислил собственные значения, перешел к нормальным координатам, потом от них к жордановой форме, выделил одну неустойчивую моду (собственное значение больше нуля), построил ПД (без интегральных компонент) управление, подавляющее эту неустойчивую моду. Насколько я понимаю (как пишет Формальский) такое управление максимизирует область притяжения при ограниченных ресурсах управления. Еще одно собственное значение меньше нуля, еще два - чисто мнимые, их пока не подавляю - результат виден в анимации в виде колебаний. Наверное, надо отщепить процентов десять от ресурсов управления на подавление двух устойчивых мод, чтобы колебания затухали - чуть пожертвовать областью притяжения ради уменьшения колебательного режима.

ЗЫ насколько я понимаю, сейчас у меня эти колебания должны быть незатухающими, возможно они у меня затухают из-за кустарного метода решения диффура, но я пока не изучил глубоко стандартные матлабовские функции для этого - мне нужен строго равномерный шаг и количество вызовов, не знаю как это реализовать в ode.

CODE

function Main()
clear all; close all;

function r = system_1_point(v)
A = [a11, -a12*cos(v(4)-v(1)); -a12*cos(v(4)-v(1)), a22];
F = [0, a12*sin(v(4)-v(1)); -a12*sin(v(4)-v(1)), 0];
B = [-b1, 0; 0, b2];
C = [-1; 1];
r = A*[v(3); v(6)] + F*[v(2)^2; v(5)^2] + B*[sin(v(1)); sin(v(4))] - C*L;
r = [r;
-v(1) + dt*v(2) + y(ii-1, 1);
-v(2) + dt*v(3) + y(ii-1, 2);
-v(4) + dt*v(5) + y(ii-1, 4);
-v(5) + dt*v(6) + y(ii-1, 5)];
end
function r = system_2_point(v)
A = [a11, -a12*cos(v(4)-v(1)); -a12*cos(v(4)-v(1)), a22];
F = [0, a12*sin(v(4)-v(1)); -a12*sin(v(4)-v(1)), 0];
B = [-b1, 0; 0, b2];
C = [-1; 1];
r = A*[v(3); v(6)] + F*[v(2)^2; v(5)^2] + B*[sin(v(1)); sin(v(4))] - C*L;
r = [r;
-1.5*v(1) + dt*v(2) - 0.5*y(ii-2, 1) + 2*y(ii-1, 1);
-1.5*v(2) + dt*v(3) - 0.5*y(ii-2, 2) + 2*y(ii-1, 2);
-1.5*v(4) + dt*v(5) - 0.5*y(ii-2, 4) + 2*y(ii-1, 4);
-1.5*v(5) + dt*v(6) - 0.5*y(ii-2, 5) + 2*y(ii-1, 5)];
end
function r = system_initial_conditions(v)
A = [a11, -a12*cos(v(4)-v(1)); -a12*cos(v(4)-v(1)), a22];
F = [0, a12*sin(v(4)-v(1)); -a12*sin(v(4)-v(1)), 0];
B = [-b1, 0; 0, b2];
C = [-1; 1];
r = A*[v(3); v(6)] + F*[v(2)^2; v(5)^2] + B*[sin(v(1)); sin(v(4))] - C*L;
r = [r;
v(1) - y(1, 1);
v(4) - y(1, 4);
v(2);
v(5)];
end

m1 = 1; r1 = 0.5; l = 1;
m2 = 2; r2 = 0.9;
t_max = 10;
dt = 5e-3;
% начальные условия - градусы отклонения звеньев от вертикали
fi1_0 = 5; fi2_0 = 3;

L = 0;
I1 = m1*r1^2; I2 = m2*r2^2; g = 9.8;
a11 = I1 + m2*l^2; a22 = I2; a12 = m2*r2*l;
b1 = (m1*r1 + m2*l)*g; b2 = m2*r2*g;
t = 0:dt:t_max;
N = numel(t);
y = zeros(N, 6); % 1,2,3 - угол первого звена с производными
% 4,5,6 - угол второго звена с производными
% начальные условия
y(1, 1) = fi1_0/180*pi; y(1, 2) = 0; y(1, 3) = 0;
y(1, 4) = fi2_0/180*pi; y(1, 5) = 0; y(1, 6) = 0;
solver_options = optimoptions('fsolve', 'Display','off');
v = fsolve(@system_initial_conditions, y(1, :), solver_options);
y(1, :) = v;

hf = figure(1);
clf reset
set(hf,'color', 'black');
axis off, grid off, axis equal
xlim([-(l+r2) (l+r2)]); ylim([-(l+r2) (l+r2)]);
xlim([-l/2 l/2]); ylim([0 l]);
title(['See Double Pendulum Run! ( l=', num2str(l), ', m1=', num2str(m1),....
', r2=', num2str(r2), ', m2=', num2str(m2), ' )'],'Color','white');
hold on
plot(0, 0, 'o', 'LineWidth', 3, 'color', [160/255 160/255 160/255]);
ht = plot(0, 0, ':w');
hl = plot([0, 0, 0], [0, 0, 0], '-', 'LineWidth', 4, 'color', [43/255 84/255 126/255]);
hm1 = plot(0, 0, 'o', 'LineWidth', 1 + round((m1*50)^(1/2)), 'color', [160/255 160/255 160/255]);
hm2 = plot(0, 0, 'o', 'LineWidth', 1 + round((m2*50)^(1/2)), 'color', [160/255 160/255 160/255]);
kfi = -1;

aeq = a12^2 - a11*a22;
beq = a22*b1 - a11*b2;
ceq = b1*b2;
Deq = beq^2 - 4*aeq*ceq;
la1 = (-beq + Deq^0.5)/(2*aeq);
la2 = (-beq - Deq^0.5)/(2*aeq);
% A0 = [a11, -a12; -a12, a22];
K_inv = inv([(b2 + a22*la1)/(a12*la1), (b2 + a22*la2)/(a12*la2); 1, 1]);
% d = K_inv*inv(A0)*[-1; 1];
mu2 = la2^0.5;

kg = 300;
fm = zeros(N, 1);
for ii = 2:N
fi = [y(ii - 1, 1); y(ii - 1, 4)];
dfi = [y(ii - 1, 2); y(ii - 1, 5)];
x = K_inv*fi;
dx = K_inv*dfi;
L = kg*(x(2) + dx(2)/mu2);
fm(ii) = L;

if ii == 2
v = fsolve(@system_1_point, y(ii - 1, :), solver_options);
else
v = fsolve(@system_2_point, y(ii - 1, :), solver_options);
end
y(ii, :) = v;

xa = -l*sin(y(ii, 1));
ya = l*cos(y(ii, 1));
xb = xa - kfi*r2*sin(y(ii, 4));
yb = ya + kfi*r2*cos(y(ii, 4));
set(hl,'XData',[0, xa, xb]); set(hl,'YData',[0, ya, yb]);
set(hm1,'XData', -r1*sin(y(ii, 1))); set(hm1,'YData', r1*cos(y(ii, 1)));
set(hm2,'XData',xb); set(hm2,'YData',yb);
set(ht,'XData', -l.*sin(y(1:ii, 1)) - kfi*r2.*sin(y(1:ii, 4)))
set(ht,'YData', l.*cos(y(1:ii, 1)) + kfi*r2.*cos(y(1:ii, 4)))
drawnow
end

figure(2);
plot(t, (180/pi).*y(:, 1), '-b', t, (180/pi).*y(:, 4), '-g', t, fm(), '-r')
legend('первое звено', 'второе звено', 'управление')
axis on, grid on
end