Последний раз редактировалось _Ivana 09.03.2014, 22:35, всего редактировалось 1 раз.
Вести с полей - почитал Формальского, переделал диффур для моего случая, линеаризовал его в окрестности нужного мне неустойчивого равновесия, вычислил собственные значения, перешел к нормальным координатам, потом от них к жордановой форме, выделил одну неустойчивую моду (собственное значение больше нуля), построил управление, подавляющее эту неустойчивую моду. Насколько я понимаю (как пишет Формальский) такое управление максимизирует область притяжения при ограниченных ресурсах управления. Еще одно собственное значение меньше нуля, еще два - чисто мнимые, их пока не подавляю - результат виден в анимации в виде колебаний. Наверное, надо отщепить процентов десять от ресурсов управления на подавление двух устойчивых мод, чтобы колебания затухали - чуть пожертвовать областью притяжения ради уменьшения колебательного режима. ЗЫ насколько я понимаю, сейчас у меня эти колебания должны быть незатухающими, возможно они у меня затухают из-за кустарного метода решения диффура, но я пока не изучил глубоко стандартные матлабовские функции для этого - мне нужен строго равномерный шаг и количество вызовов, не знаю как это реализовать в ode. (Оффтоп)
Код: 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
|
Конструкция - прямоугольник из листового материала ставится вертикально на свою сторону, на нем закреплен датчик угла отклонения от вертикали (пока не будем об особенностях реализации, считаем что он есть), ПИД контроллер и двигатель с редуктором, ось которого горизонтальна и находится в плоскости нашего прямоугольника. На оси двигателя жестко закреплен маятник качения, который, работая как противовес, балансирует всю конструкцию и не дает ей упасть. Я создал такую тему на форумах робототехники, определяюсь с моделями двигателей и уже хочу собрать рабочий макет для экспериментов. Да, сразу макет, вместо нормального подхода - теоретических расчетов, моделирования системы в матлабе/симулинке, выбора параметров и т.п. Понимаю, что такой подход дилетантский и не гарантирует никакого результата, но я не обладаю достаточными знаниями теории и симулинка для подобных расчетов. Хотя и практические такое конструкции я тоже еще не строил, с другой стороны 
в точке опоры и 
в точке соединения звеньев, в т.