Приложение 2

function regulator

global qInt Td Tk Ud iqErr dqErr qErrOld;

Td=0.0005;

Tk=0;

iqErr = zeros (3,1);

dqErr = zeros (3,1);

qErrOld = zeros (3,1);

[t,x]=ode45(@Model_Dyn,[0:Td*0.1:6],[0;0;0;0;0;0;0;0;0],odeset());

figure1=figure('Color','w','position', [100 100 700 400]);

plot(t,x(:,1),t,x(:,2),t,x(:,3));

set(gcf,'PaperPositionMode','auto');

grid on;

xlabel('t, с','fontsize',10,'FontName','Cambria','FontAngle','italic');

legend('\theta (t), рад','h (t), м','r (t), м');

end

function out = discr(x)

out = round(x*2^16)./2^16;

end

function dx = Model_Dyn(t,x)

g = 9.8;

% Вектор-столбец первых производных :

dx = zeros(9,1);

% Глобальные переменные:

global qErr qErrOld Tk Ud Td iqErr dqErr;

%% ----------------- Переменные состояния: --------------------------------

% Обобщенные координаты:

teta = x(1);

h = x(2);

r = x(3);

% Обобщенные скорости:

dteta = x(4);

dh = x(5);

dr = x(6);

% Токи:

I = zeros(1,3)';

I(1) = x(7);

I(2) = x(8);

I(3) = x(9);

J = 2.2;

m2 = 3;

m3 = 4;

% Матрица инерции:

M = [ J+m3*(r^2) 0 0

0 m2+m3 0

0 0 m3 ];

% Вектор кориолисовых и центробежных сил:

V = [ 2*m3*r*dr*dteta 0 -m3*r*dteta ]';

% Вектор гравитационных сил:

G = [ 0 (m2+m3)*g 0 ]';

% Вектор сил трения:

taud = [ 0.1*sign(teta) 0.2*sign(h) 0.15*sign(r) ]';

% Коэф-ты передачи редукторов:

ired = [ 65 0 0

0 100 0

0 0 50 ];

Km = [ 0.1 0 0

0 0.3 0

0 0 0.1 ];

Ke = [ 0.2 0 0

0 0.2 0

0 0 0.4 ];

L = [ 0.1 0 0

0 0.1 0

0 0 0.1 ];

R = [ 2.5 0 0

0 1 0

0 0 2.5 ];

%% ----------------- Регулятор: -------------------------------------------

% Заданные обобщенные координаты:

qzad = [0.7;0.6;0.8];

q = [teta;h;r];

dq = [dteta;dh;dr];

Коэффициенты ПИД-регулятора:

% Коэффициенты ПИД-регулятора:

Kp = [ 500 0 0; 0 400 0; 0 0 200 ];

Kd = [ 0 0 0; 0 100 0; 0 0 33 ];

Ki = [ 0 0 0; 0 0.04 0; 0 0 0 ];

% Производная ошибки:

qErr = discr(qzad) - discr(q);

dqErr = qErr-qErrOld;

qErrOld = qErr;

%Интеграл ошибки:

iqErr = iqErr + qErr;

% ПИД-регулятор:

U = Kp*qErr + Kd*dqErr + Ki*iqErr;

%% -------- Экстраполятор нулевого порядка: ------------

if(t==0)

Ud=U;

end;

if(t>(Tk+Td))

Tk=Tk+Td;

Ud=U;

end

%% -------- Левая чась уравнений Коши: -----------------

% Производные токов:

dI=inv(L)*(Ud-R*I-Ke*ired*dq);

% Вектор-столбец обобщенных сил:

%tau = [ n1 f2 f3 ];

tau = Km * ired * I;

% Вектор-столбец ускорений обобщенных координат:

ddq = inv(M)*(tau-V-G-taud);

% Первые производные переменных состояния:

dx(1) = dteta;

dx(2) = dh;

dx(3) = dr;

dx(4) = ddq(1);

dx(5) = ddq(2);

dx(6) = ddq(3);

dx(7) = dI(1);

dx(8) = dI(2);

dx(9) = dI(3);

end

6. Литература

1. Фу К., Гонсалес Р., Ли К. Робототехника: Пер. с англ. – М.: Мир, 1989. – 624 с.

2. В.С. Щербаков, М.С. Корытов, А.А. Руппель, В.А. Глушец, С.А. Милюшенко. моделирование и визуализация движения механических систем в Matlab. Учебное пособие. Омск. Сибади, 2007.