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

[RHnd]

ПОС у вас в интегральной составляющей.

[RHnd]

Еще информация: ваша система называется Acrobot. Поиск по названию должен дать и матмодель, и методы управления. Вот пример стабилизации. Хотя я уверен, что они используют данные об угле первого звена, который вы не измеряете.

[_Ivana]

Спасибо, посмотрю-погуглю. С абсолютным углом первого звена имхо не должно быть проблем создания устойчивой модели даже без второго датчика (энкодера). Даже гироскоп не нужен - просто ПИД с одного датчика абсолютного угла. Но для этого мне придется покупать готовый узел IMU тысяч за 10 рублей, сам я вряд ли сделаю лучше. А хотелось бы сначала попробовать менее затратные варианты.

[_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

[_Ivana]

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