5. Регуляторы приводов манипулятора

5.1 Непрерывный пид-регулятор

Непрерывный ПИД-регулятор в модели системы реализуется с помощью следующего программного кода:

Листинг 3:

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

Kp = [ 100 0 0; 0 100 0; 0 0 100 ];

Kd = [ 0 0 0; 0 50 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;

Рисунок 8

Настройка параметров ПИД-регулятора произведена частично методом оптимизации по критерию минимума квадрата ошибки. На рисунке 8 представлены графики отработки относительных координат θ = 0,7 рад, h = 0,6 м, r = 0,8 м.

5.2. Дискретный пид-регулятор

Для моделирования работы дискретного ПИД-регулятора используются блоки программного кода, содержащие экстраполятор нулевого порядка и квантователь по уровню (листинг 2), программный код ПИД-регулятора при этом не отличается от непрерывного (листинг 3).

Когда период квантования мал (Td = 0,005 c), переходные процессы практически не отличаются от переходных процессов в непрерывной системе (рисунок 9).

Рисунок 9

При увеличении Td до значения 0,1 c реакция системы меняется. Появляется статическая ошибка по вертикальной координате и наблюдается перерегулирование по остальным координатам:

5.3 Регулятор с прямым расчётом момента

Этот принцип управления заключается в прямом расчёте требуемого момента, который должны развивать привода:

где – ошибка и производная от ошибки соответственно,– коэффициенты дифференциальной и пропорциональной составляющей ПД- регулятора,– ускорение задающего сигнала. Реализуем регулятор с помощью следующего программного кода:

Kp = [ 100 0 0; 0 200 0; 0 0 100 ];

Kd = [ 0 0 0; 0 50 0; 0 0 33 ];

% Регулятор с расчетом момента:

U=(inv(Km)*inv(ired)*(M*(Kp*qErr+Kd*dqErr)+V+G+taud));

Рисунок 10

5.4. Гравитационный ПД-регулятор

Этот регулятор реализует следующий закон управления:

Рисунок 11.

Т.е. такой регулятор состоит из ПД-регулятора ошибки по положению и компенсатора гравитационных сил и сил трения. В программном коде регулятор реализуется следующим образом:

Kp = [ 400 0 0; 0 2000 0; 0 0 400 ];

Kd = [ 0 0 0; 0 20 0; 0 0 500 ];

% Гравитационный регулятор:

U=inv(Km)*inv(ired)*(Kp*qErr+Kd*dqErr+G+taud);

Приложения Приложение 1

function PZDinamiki

clc

%========================= Вектор гравитации: ============================

global g

g = [0 0 -9.84 0];

%========================= Массы звеньев: ================================

global m

m = [ 25 15 10 7 5 8 ];

%== Матрица из радиус-векторов центра масс звеньев в их собственной СК: ==

global r

r = [ 0 0 0.5

0 0.12 0.12

0 -0.25 0

0 0 0.1

0 -0.15 0

-0.15 0 0 ];

% ======== Компоненты тензора инерции звеньев в системе координат, ========

% ======== связанной со звеном и перенесенной в центр масс звена: =========

Jxx = [ 4.6875 0.4 0.8333 0.0233 0.0375 0 ]';

Jyy = [ 4.6875 0.38 0 0.0233 0 0.06 ]';

Jzz = [ 0 0.21 0.8333 0 0.0375 0.06 ]';

Jxy = zeros(6);

Jxz = zeros(6);

Jzy = zeros(6);

global J;

J = zeros(4,4,6);

for I = 1:1:6

J(:,:,i) = [(-Jxx(i)+Jyy(i)+Jzz(i))/2 Jxy(i) Jxz(i) m(i)*r(i,1)

Jxy(i) (Jxx(i)-Jyy(i)+Jzz(i))/2 Jzy(i) m(i)*r(i,2)

Jxz(i) Jzy(i) (Jxx(i)+Jyy(i)-Jzz(i))/2 m(i)*r(i,3)

m(i)*r(i,1) m(i)*r(i,2) m(i)*r(i,3) m(i) ];

end

%======= Обобщенные координаты, соответствующие исходной траектории: ========

global qps;

%======================= Находим обобщенные скорости: =======================

qv = zeros (131,6);

for i = 1:1:6

qv(1:130,i) = diff(qps(:,i))./0.5;

end

%====================== Находим обобщенные ускорения: =======================

qa = zeros (131,6);

for i = 1:1:6

qa(1:130,i) = diff(qv(:,i))./0.5;

end

global Q

Q = zeros(4,4,6);

Qv = [ 0 -1 0 0

1 0 0 0

0 0 0 0

0 0 0 0 ];

Qp = [ 0 0 0 0

0 0 0 0

0 0 0 1

0 0 0 0 ];

Q(:,:,1) = Qv; Q(:,:,2) = Qp; Q(:,:,3) = Qp;

Q(:,:,4) = Qv; Q(:,:,5) = Qv; Q(:,:,6) = Qv;

tau = zeros (6,131);

for i = 1:1:131

tau(:,i) = D(qps(i,:))*qa(i,:)'+h(qps(i,:),qv(i,:))+c(qps(i,:));

end

end

function vect = c(q)

vect = zeros(6,1);

for i = 1:1:6

vect(i) = ci(q,i);

end

end

function ch = ci(q,i)

global r;

global m;

global g

ch=0;

for j = i:1:6

ch = ch+(-m(j)*g*(Uij(q,j,i)*[ r(j,:) 1]'));

end

end

function sum = hikm(q,i,k,m)

global J;

sum = 0;

for j = max([ i,k,m ]):1:6

matr = Uijk(q,j,k,m)*J(:,:,j)*(Uij(q,j,i)');

sum = sum +trace(matr);

end

end

function ch = hi(q,qv,i)

ch = 0;

for kkk = 1:1:6

for mmm = 1:1:6

ch = ch+hikm(q,i,kkk,mmm)*qv(kkk)*qv(mmm);

end

end

end

function vect = h(q,qv)

vect = zeros(6,1);

for I = 1:1:6

vect(i) = hi(q,qv,i);

end

end

function sum = Dik(q,i,k)

global J;

sum = 0;

for j = max( [i,k] ):1:6

matr = Uij(q,j,k)*J(:,:,j)*(Uij(q,j,i)');

sum = sum +trace(matr);

end

end

function matr = D(q)

matr = zeros(6,6);

for ii = 1:1:6

for kk = 1:1:6

matr(ii,kk) = Dik(q,ii,kk);

end

end

end

function matr = Uij(q,i,j)

global Q;

if j<=i

matr = Ai_j(q,0,j-1) * Q(:,:,j) * Ai_j(q,j-1,i);

else

matr = zeros(4,4);

end

end

function matr = Uijk(q,i,j,k)

global Q;

if (i>=k)&&(k>=j)

matr = Ai_j(q,0,j-1)*Q(:,:,j)*Ai_j(q,j-1,k-1)*Q(:,:,k)*Ai_j(q,k-1,i);

elseif (i>=j)&&(j>=k)

matr = Ai_j(q,0,k-1)*Q(:,:,k)*Ai_j(q,k-1,j-1)*Q(:,:,j)*Ai_j(q,j-1,i);

else

matr = zeros(4,4);

end

end

function mA = Ai_1__i(q,i)

q = [ 0 1 0.3 pi/3 -pi/2 pi/2 ];

L = [ 1.8 0.3 0.5 0.3 0.6 0.2 ];

d = [ 0 q(2) q(3) 0 L(4)+L(5) 0 ];

a = [ 0 L(2) L(3) 0 0 L(6) ];

theta =[ q(1) pi/2 0 (pi/2)+q(4) pi-q(5) (pi/2)+q(6)];

alpha =[ 0 0 pi/2 pi/2 pi/2 0 ];

mA = [ cos(theta(i)) -cos(alpha(i))*sin(theta(i)) sin(alpha(i))*sin(theta(i)) a(i)*cos(theta(i))

sin(theta(i)) cos(alpha(i))*cos(theta(i)) -sin(alpha(i))*cos(theta(i)) a(i)*sin(theta(i))

0 sin(alpha(i)) cos(alpha(i)) d(i)

0 0 0 1 ];

end

function mA = Ai_j(q,i,j)

mA = eye(4);

for k =(i+1):1:(j)

mA = mA * Ai_1__i(q,k);

end

end