Методическое пособие 357

3.2. Моделирование робота с системой динамического управления

Моделирование робота осуществляется в системе MATLAB в следующем порядке.

1. Вводятся кинематические и динамические параметры манипулятора: количество степеней подвижности, типы сочленений, орты локальных систем координат, единичные векторы осей сочленений, радиус-векторы, массы и моменты инерции звеньев (программа 1).

Программа 1

% Ввод параметров манипулятора clc

n=input(' Количество степеней подвижности '); while n<1

warndlg(' n>0 ');

n=input(' Количество степеней подвижности '); end

z=0;

z=input(' Задайте вектор типов звеньев (1-поступ,0-вращ) '); while size(z)~=n

warndlg([' size(z)=',int2str(n),' ']); z=input(' Задайте вектор типов звеньев ');

end

for i=1:n

Q01(1:3,i)=input(['Задайте орт оX=[X Y Z] связанный со звеном ' int2str(i),' '])';

Q02(1:3,i)=input(['Задайте орт оY=[X Y Z] связанный со звеном ' int2str(i),' '])';

Q03(1:3,i)=input(['Задайте орт оZ=[X Y Z] связанный со звеном

' int2str(i),' '])'; end e=zeros(3,n)'; for i=1:n

50

r(3*i-2:3*i,i)=k';

k=input([' Задайте вектор r(' int2str(i),',',int2str(i+1),')=[x y z]']); r(3*i-2:3*i,i+1)=k';

2. Вводятся параметры исполнительных приводов. Для привода постоянного тока вводятся: коэффициенты вязкого трения, моменты инерции якорей двигателей, коэффициенты пропорциональности по моменту и противоЭДС, индуктивности и активные сопротивления якорных цепей (программа 2).

Программа 2 % Ввод параметров приводаclc; k=menu('Tип привода',' ДПТ

Kel=input('Введите коэффициент пропорциональности

51

Rrl=input('Введите сопротивление цепи якоря,Ом '); a12l=1;

a22l=-Kcl/Jrl;a23l=Kml/Jrl; a32l=-Kel/Lrl;a33l=-Rrl/Lrl; f2l=-1/Jrl;b3l=1/Lrl;

else

Kc=input('Введите коэффициент вязкого трения,

Ks=input('Введите коэффициент сжимаемости жидкости, Н/м^2 '); Vc=input('Введите рабочий объем цилин-

дра,м^3 '); Kn=input('Введите наклон характеристики сер-

a32l=-4*Ks*Sp/Vc;a33l=-4*Ks*(Kn+Ku)/Vc; f2l=-1/mp;b3l=4*Ks*Kip/Vc;

end

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

52

Программа 3

% Ввод параметров траекторииclc;n=input('Количество степеней подвижности ');MM=input('Число опорных точек

');td=input('Период дискретности ');for j=1:n for s=1:MM

qq0(j,s)=input('Введите значение координаты в опорной точке '); end end qq0(1:n,1:MM)=[qq0(1:n,1:MM)]for j=1:n w0(j,1)=input('Введите значение скорости координа-

ты');end w0(1:n,1)=[w0(1:n,1)];qzad=zeros(n,1); qzad1=zeros(n,1);

for j=1:n

qzad(j,1)=input(['Введите начальное значение перемещения звена ',int2str(j),' ']);

qzad1(j,1)=input(['Введите начальное значение скорости звена

',int2str(j),' ']); end

for j=1:n

K0(j,1)=input(['Введите значение коэффициента обратной связи по перемещению ']);

end

for j=1:n

K1(j,1)=input(['Введите значение коэффициента обратной связи по скорости ']);

end

r0(1,1)=input('Введите значение r0 ');

4. Определяется продолжительность управления на каждом временном интервале между опорными точками исходя из заданных значений обобщенных координат в начале и конце временного интервала и максимально-допустимых скоростей координат. Формируются задающие сигналы. Для решения задачи интерполяции организованы циклы по s, в которых определяются значения параметров Mjs в соответствии с выражениями (1.45)–(1.47). Далее через периоды дискретности Тд из уравнений (1.41)–(1.43) рассчитываются значения куби-

53

ческого сплайна Pjs3 и его производных между опорными точками, т.е. формируются векторы заданных значений обобщенных координат, скоростей и ускорений (программа 4).

Программа 4

%Расчет траектории

Qq(1:n,1:MM)=qq0(1:n,1:MM);

wcp(1:n,1)=w0(1:n,1);

%Задание t

T=zeros(n,MM); for j=1:n

for s=1:MM if s~=1

TT(j,s)=(abs(Qq(j,s)-Qq(j,s-1)))/wcp(j,1); T(j,s)=TT(j,s)+T(j,s-1);

else T(j,1)=0; end

end end

Tmax=zeros(MM,1); for s=2:MM

for j=1:n

if Tmax(s,1)>=T(j,s) Tmax(s,1)=Tmax(s,1); else

Tmax(s,1)=T(j,s); end

end end

% Вычисление tmax tmax=zeros(MM,1); for s=1:MM

for j=1:n

if tmax(s,1)>=TT(j,s)

54

tmax(s,1)=tmax(s,1); else tmax(s,1)=TT(j,s); end

end end

%Вычисление h h=zeros(MM,1); for s=1:MM

h(s,1)=tmax(s,1); end

end

%Вычисление e1 e1=zeros(n,MM-1); for j=1:n

for s=1:MM-1 if s==1 e1(j,1)=-0.5; else

e1(j,s)=(-(s+1,1)/(h(s,1)+h(s+1,1)))/((h(s,1)/(h(s,1)+h(s+1,1)))* e1(j,s-1)+2);

end end end

%Вычисление d

d=zeros(n,MM); for j=1:n

for s=1:MM if s==1

d(j,1)=6*((Qq(j,2)-Qq(j,1))/(h(2,1)*h(2,1))); elseif s==MM

d(j,MM)=6*((Qq(j,MM-1)-Qq(j,MM))/(h(MM,1)*h(MM,1))); else d(j,s)=6*((Qq(j,s+1)-Qq(j,s))/h(s+1,1)-(Qq(j,s)-Qq(j,s-1))/ h(s,1))/(h(s,1)+h(s+1,1));

end

55

end end

%Вычисление b b=zeros(n,MM); for j=1:n

for s=1:MM if s==1

b(j,1)=d(j,1)/2; elseif s==MM

b(j,MM)=(d(j,MM)-b(j,MM-1))/(e1(j,MM-1)+2); else b(j,s)=(d(j,s)-h(s,1)*b(j,s-1)/(h(s+1,1)+h(s,1)))/(h(s,1)* e1(j,s-1)/(h(s+1,1)+h(s,1))+2);

end end end

%Вычисление M

M=zeros(n,MM); for j=1:n M(j,MM)=b(j,MM); end

s=MM-1; while s>=1 for j=1:n

M(j,s)=e1(j,s)*M(j,s+1)+b(j,s); end

s=s-1; end

% Формирование столбца дискрет t for s=2:MM l(s)=round(tmax(s,1)/td); l(s)=l(s)+l(s-1);

end L=l(MM); t=zeros(L,1); for I=2:L

56

t(I,1)=t(I-1)+td; end

end

% Расчет сплайна

P=zeros(n,L);

P1=zeros(n,L);

P2=zeros(n,L);

I=1;

for s=2:MM while I<=l(s) for j=1:n if I==L

P(j,I)=Qq(j,MM);

P1(j,I)=0;

P2(j,I)=0; else

I=I P(j,I)=(M(j,s-1)*(Tmax(s,1)-t(I,1))^3)/(6*h(s,1))+(M(j,s)* (t(I,1)-Tmax(s-1,1))^3)/(6*h(s,1))+(Qq(j,s-1)-(M(j,s-1)* (h(s,1))^2)/6)*(Tmax(s,1)-t(I,1))/h(s,1)+(Qq(j,s)-(M(j,s)* (h(s,1))^2)/6)*(t(I,1)-Tmax(s-1,1))/h(s,1); P1(j,I)=-(M(j,s-1)*(Tmax(s,1)-t(I,1))^2)/(2*h(s,1))+(M(j,s)* (t(I,1)-Tmax(s-1,1))^2)/(2*h(s,1))-(Qq(j,s-1)-(M(j,s-1)* h(s,1)^2)/6)/h(s,1)+(Qq(j,s)-(M(j,s)*h(s,1)^2)/6)/h(s,1); P2(j,I)=M(j,s-1)*(Tmax(s,1)-t(I,1))/h(s,1)+M(j,s)* (t(I,1)-Tmax(s-1))/h(s,1);

end end I=I+1; end end

5. Рассчитываются параметры матриц уравнения (1.37) исходя из заданных и текущих значений обобщенных координат, скоростей, а также заданных ускорений обобщенных

57

координат. При этом учитываются взаимовлияние звеньев [матрица B(q(t),q(t),d)], гравитационные силы [матрица

C(q(t), d)], изменение моментов инерции при движении манипулятора [в матрице A(q(t), d )] (программа 5).

Программа 5

% Расчет параметров матриц

Pzad=zeros(3,l);

qt=zeros(3,l);

q1t=zeros(3,l);

q2t=zeros(3,l);

A1=zeros(3,3);

B1=zeros(3,1);

qt(1:3,1)=qzad(1:3,1);

q1t(1:3,1)=qzad1(1:3,1);

qt(1,2)=qzad(1,1); for I=1:l-1

if I==1 q1t(j,I)=qzad1(j,1); qt(j,I)=qzad(j,1); end q(1:n,1)=qt(1:n,I); Q1=Q01;

Q2=Q02;

Q3=Q03; for i=1:n

Q=[Q1(:,i) Q2(:,i) Q3(:,i)]; E(:,i)=Q*e(:,i);

end

for i=1:n if z(i)==0 for j=i:n

cosqi=cos(q(i));

sinqi=sin(q(i));

QQ1=cross(E(:,i),cross(Q1(:,i),E(:,i)));

58

QQ2=cross(E(:,i),Q1(:,i));

QQ3=(E(:,i)*(Q1(:,i))')*E(:,i);

Q1(:,i)=QQ1*cosqi+QQ2*sinqi+QQ3;

QQ1=cross(E(:,i),cross(Q2(:,i),E(:,i)));

QQ2=cross(E(:,i),Q2(:,i));

QQ3=(E(:,i)*(Q2(:,i))')*E(:,i);

Q2(:,i)=QQ1*cosqi+QQ2*sinqi+QQ3;

QQ1=cross(E(:,i),cross(Q3(:,i),E(:,i)));

QQ2=cross(E(:,i),Q3(:,i));

QQ3=(E(:,i)*(Q3(:,i))')*E(:,i);

Q3(:,i)=QQ1*cosqi+QQ2*sinqi+QQ3; end

else r(3*i-2:3*i,i)=r(3*i-2:3*i,i)+q(i)*e(:,i); end

end

for i=1:n

Q=[Q1(:,i) Q2(:,i) Q3(:,i)]; E(:,i)=Q*e(:,i); R(3*i-2:3*i,i)=Q*r(3*i-2:3*i,i); R(3*i-2:3*i,i+1)=Q*r(3*i-2:3*i,i+1);

end

for i=1:n for j=1:n if j>i+1

R(3*i-2:3*i,j)=R(3*i-2:3*i,j-1)-R(3*(i+1)-2:3*(i+1),j-1)+ R(3*(i+1)-2:3*(i+1),j);

end if j<i

R(3*i-2:3*i,j)=R(3*(i-1)-2:3*(i-1),j)-R(3*(i-1)-2:3*(i-1),i)+ R(3*i-2:3*i,i);

end end end

% Расчет <E,R>

59