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

W2m=0.15; W3m=0.6; {Максимальные значения скоростей} m=10; {Число опорных точек}

type arr=array[0..m] of real; {раздел объявления типов массивов} var q1,q2,q3:arr; {раздел объявления переменных} b1,b2,b3,d1,d2,d3,M1,M2,M3,t:ARRAY[0..m] of REAL;

e1,e2,e3,R1,R2,R3:ARRAY[0..m-1] of REAL; MU1,MU2,MU3,h:ARRAY[1..m] of REAL; s: BYTE;

t1,t2,t3,cs,cs1,cs11:REAL; {массив угловых координат:}

const q1:arr=(-0.4,-0.2,-0.3,-0.1,0.1,0.3,0.5,0.4,0.5,0.7,0.8); q2:arr=(0.9,1.1,1.0,0.8,0.9,1.2,1.6,2.0,2.2,1.9,2.3); q3:arr=(-1.5,-1.0,-0.8,-0.9,-0.6,-0.1,0.6,1.1,0.9,1.2,1.5);}

{$R *.DFM}

{Процедура определения максимального элемента массива} function MAX(t1,t2,t3:REAL):real;

var h: REAL; begin

h:=t1;

if h<t2 then h:=t2; if h<t3 then h:=t3; MAX:=h;

end;

{Процедура расчета параметров сплайнов} procedure TForm1.Button1Click(Sender: TObject);

for s:=0 to m do {определение продолжительности }

if s<>m then { управления на каждом из участков траектории} begin

t1:=Abs(q1[s+1]-q1[s])/W1m;

171

t2:=Abs(q2[s+1]-q2[s])/W2m; t3:=Abs(q3[s+1]-q3[s])/W3m; h[s+1]:=max(t1,t2,t3);

if h[s+1]=0 then h[s+1]:=0.1; t[s+1]:=t[s]+h[s+1];

end;

{Определение коэффициентов es, bs на текущем участке траектории}

begin

e1[s]:=-0.5; e2[s]:=-0.5; e3[s]:=-0.5; cs:=Sqr(h[s+1]); d1[s]:=6*(q1[s+1]-q1[s])/cs; d2[s]:=6*(q2[s+1]-q2[s])/cs; d3[s]:=6*(q3[s+1]-q3[s])/cs; b1[s]:=d1[s]/2;

b2[s]:=d2[s]/2;

b3[s]:=d3[s]/2;

end;

for s:=1 to m-1 do {вычисление коэффициентов при s=1...m-1} begin

cs1:=h[s+1]+h[s];

MU1[s]:=h[s]/cs1;

MU2[s]:=MU1[s];

MU3[s]:=MU1[s];

R1[s]:=1-MU1[s]; R2[s]:=R1[s]; R3[s]:=R1[s];

e1[s]:=-R1[s]/(MU1[s]*e1[s-1]+2); e2[s]:=-R2[s]/(MU2[s]*e2[s-1]+2); e3[s]:=-R3[s]/(MU3[s]*e3[s-1]+2); d1[s]:=6*((q1[s+1]-q1[s])/h[s+1]-(q1[s]-q1[s-1])/h[s])/cs1; d2[s]:=6*((q2[s+1]-q2[s])/h[s+1]-(q2[s]-q2[s-1])/h[s])/cs1; d3[s]:=6*((q3[s+1]-q3[s])/h[s+1]-(q3[s]-q3[s-1])/h[s])/cs1; b1[s]:=(d1[s]-MU1[s]*b1[s-1])/(MU1[s]*e1[s-1]+2);

172

b2[s]:=(d2[s]-MU2[s]*b2[s-1])/(MU2[s]*e2[s-1]+2); b3[s]:=(d3[s]-MU3[s]*b3[s-1])/(MU3[s]*e3[s-1]+2);

MU1[s]:=1; MU2[s]:=1; MU3[s]:=1; cs11:=Sqr(h[s]); d1[s]:=6*(q1[s-1]-q1[s])/cs11; d2[s]:=6*(q2[s-1]-q2[s])/cs11; d3[s]:=6*(q3[s-1]-q3[s])/cs11;

b1[s]:=(d1[s]-MU1[s]*b1[s-1])/(MU1[s]*e1[s-1]+2); b2[s]:=(d2[s]-MU2[s]*b2[s-1])/(MU2[s]*e2[s-1]+2); b3[s]:=(d3[s]-MU3[s]*b3[s-1])/(MU3[s]*e3[s-1]+2);

M1[s]:=b1[s]; {определение параметров кубического сплайна} M2[s]:=b2[s];

M3[s]:=b3[s];

end;

for s:=m-1 downto 0 do begin

M1[s]:=e1[s]*M1[s+1]+b1[s];

M2[s]:=e2[s]*M2[s+1]+b2[s];

M3[s]:=e3[s]*M3[s+1]+b3[s];

Edit1.text:=FloatToStr(M1[1]);

Edit2.text:=FloatToStr(M2[1]);

Edit3.text:=FloatToStr(M3[1]);

Edit4.text:=FloatToStr(M1[2]);

Edit5.text:=FloatToStr(M2[2]);

Edit6.text:=FloatToStr(M3[2]);

Edit7.text:=FloatToStr(M1[3]);

Edit8.text:=FloatToStr(M2[3]);

Edit9.text:=FloatToStr(M3[3]);

end;

end;

173

6.6. Программная реализация алгоритмов адаптации

Проектирование новых моделей роботов и модернизация существующего парка с точки зрения повышения их динамической точности средствами адаптации связаны с моделированием самонастраивающейся системы управления [27].

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

Таким образом, динамика эталонной модели определяется значениями параметров am1, am2, bm1, bm2, которые, в свою очередь, определяются только настройками подчиненных контуров. Задачами адаптивного управления являются развязывание динамики подсистем отдельных координат реального манипулятора, т. е. компенсация их взаимного влияния, и согласование собственного (локального) движения подсистем с эталонным движением (6.18), возбуждаемым программным управлением.

Решение поставленных задач достигается введением адаптивных управлений Uа1, Uа2, определяемых из выражений

(5.44)–(5.48).

Разработанная в среде DELPHI 5.0 программа 3 решает систему уравнений (5.42)–(5.48) в численном виде.

В начале программы осуществляется ввод инерционных

параметров J1с, J2с, J20, J3 манипулятора, параметров am1, am2, bm1, bm2 эталонной модели, коэффициентов передачи kп1, kп2,

kс1,kс2 приводов; определяются типы массивов и переменных.

174

Далее с клавиатуры вводятся начальные значения само-

настраивающихся коэффициентов ki1(t), ki2(t), ki3(t), ki4(t), k16(t), k17(t), k18(t), k19(t), k110(t), k26(t), k27(t), k28(t), k29(t), k210(t), посто-

янные коэффициенты Gi1, Ai1, Gi2, Ai2, Gi3, Ai3, Gi4, Ai4, pi1, pi2,

G16, A16, G17, A17, G18, A18, G19, A19, G110, A 110, G26, A26, G27, A27,

G28, A28, G29, A29, G210, A210 адаптивного регулятора, заданные перемещения q1З, q2З, начальные значения перемещений q1, q2

и скоростей q1,q2 координат, временные параметры: время

начала перемещения t10, время конца перемещения t2, период дискретности Т.

Исходя из начальных значений изменяющихся парамет-

ров ki1(t), ki2(t), ki3(t), ki4(t), k16(t), k17(t), k18(t), k19(t), k110(t), k26(t), k27(t), k28(t), k29(t), k210(t), q1, q2, q1,q2 , для первого шага кван-

тования по времени определяются адаптивные управляющие воздействия Uа1, Uа2 и текущие значения ускорений координат манипулятора q1,q2 и эталонной модели q1m,q2m .

Для первого шага квантования рассчитываются конечные значения перемещений q1, q2 и скоростей q1,q2 координат

манипулятора, перемещений q1m, q2m и скоростей q1m ,q2m эта-

лонной модели исходя из их начальных значений, ускорений и периода Т квантования по времени.

Определяются производные от самонастраивающихся

коэффициентов ki1(t), ki2(t), ki3(t), ki4(t), k16(t), k17(t), k18(t), k19(t), k110(t), k26(t), k27(t), k28(t), k29(t), k210(t). Рассчитываются их ко-

нечные значения.

Полученные результаты являются начальными значениями параметров для следующего шага квантования, который начинается в момент времени t10 + Т. Циклы расчетов заканчиваются в момент времени t = t2.

Алгоритм программы 3 приведен на рис. 6.10.

175

Рис. 6.10. Алгоритм программы решения уравнений адаптивной системы

176

g=9.8;

Am: array[1..2] of real=(41,41); {Параметры эталонной модели}

Bm: array[1..2] of real=(440,440);

type arr=array[1..2,0..501] of real; {Определение типов массивов}

b,c,e,h,h1,w,v,,z1,z2,m,num:integer;

Ul: array[1..2] of extended; Ur: array[1..2] of extended; Ua: array[1..2] of extended; d: array[1..2] of extended; Tk: array[0..501] of real;

Q: array[1..2,1..5] of real; Qm: array[1..2,1..3] of real; Q4: array[1..2,6..10] of real;

procedure Tform1.Button1Click(Sender: TObject); begin

for b:=1 to 2 do for c:=1 to 2 do begin

Q[b,c]:=Q0[b,c]; Qm[b,c]:=Qm0[b,c];

if c=1 then Q1[b,0]:=Q0[b,c] else Q2[b,0]:=Q0[b,c]; end;

Q[1,4]:=Q5[1]; Q[2,4]:=Q5[2]; t1:=t10; t2:=t20; T:=T0; h:=0; K:=K0; R:=R0;

Q[1,5]:=0; Q[2,5]:=0; while t1<=t2 do begin

Q[1,4]:=Q5[1]; Q[2,4]:=Q5[2];

177

for b:=1 to 2 do Q[b,3]:=Q[b,2]*Q[b,2];

for b:=1 to 2 do {Определение локальных управлений} begin

Ul[b]:=0;

for c:=1 to 4 do Ul[b]:=Ul[b]+K[b,c]*Q[b,c];

if b=1 then e:=2 {Определение развязывающих управлений} else e:=1;

Q4[b,6]:=Q[e,1]; Q4[b,7]:=Q[e,2]; Q4[b,8]:=Q[e,3]; Q4[b,9]:=Q[1,2]*Q[2,2]; Q4[b,10]:=Q[e,4];

Ur[b]:=0;

for c:=6 to 10 do Ur[b]:=Ur[b]+R[b,c]*Q4[b,c]; Ua[b]:=Ul[b]+Ur[b];

end;

{Определение и ограничение ускорений координат и моделей}

B1:=J3*J3*sin(2*(Q[2,1]-Q[1,1])); D1:=J3*cos(Q[2,1]-Q[1,1])*i; E1:=J3*sin(Q[2,1]-Q[1,1]);

Q[1,5]:=(B1*Q[1,3]/2-JS2*i*Kc1*Q[1,2]-JS2*i*Kp1*Q[1,1]-JS2* J3*g*cos(Q[1,1])/l2+JS2*i*Kp1*(Ua[1]+Q[1,4])-J3*cos(Q[2,1]- Q[1,1])*i*Kp2*(Ua[2]+Q[2,4])+JS2*E1*Q[2,3]+D1*Kc2*Q[2,2] + D1*Kp2*Q[2,1]+J3*J2*cos(Q[2,1]-Q[1,1])*g*cos(Q[2, 1])/l2)/ ((JS1*JS2)-J3*J3*cos(Q[2,1]-Q[1,1])*cos(Q[2,1]-Q[1,1]));

if abs(Q[1,5])>40 then begin

if Q[1,5]>0 then Q[1,5]:=40 else Q[1,5]:=-40; end;

Q[2,5]:=(-B1*Q[2,3]/2-JS1*i*Kc2*Q[2,2]-S1*i*Kp2*Q[2,1]- JS1* J2* g*cos(Q[2,1])/l2+JS1*i*Kp2*(Ua[2]+Q[2,4])- J3*cos(Q[2,1] - Q[1,1])*i*Kp1*(Ua[1]+Q[1,4])-JS1*E1*Q[1,3]+ D1*Kc1*Q[1,2]+ D1*Kp1*Q[1,1]+J3*J3*cos(Q[2,1]- Q[1,1])*g*cos(Q[1,1])/l2)/ ((JS1*JS2)-J3*J3*cos(Q[2,1]- Q[1,1])*cos(Q[2,1]-Q[1,1]));

178

if abs(Q[2,5])>25 then begin

if Q[2,5]>0 then Q[2,5]:=25 else Q[2,5]:=-25; end;

for b:=1 to 2 do begin

Qm[b,3]:=-Am[b]*Qm[b,2]+Bm[b]*(Q[b,4]-Qm[b,1]); if b=1 then acc:=40 else acc:=25;

if abs(Qm[b,3])>acc then begin

if Qm[b,3]>0 then Qm[b,3]:=acc else Qm[b,3]:=-acc; end;

end;

h:=h+1;

{Определение конечных значений перемещений, скоростей} {и самонастраивающихся коэффициентов}

for b:=1 to 2 do begin

Q[b,1]:=Q[b,1]+Q[b,2]*T+Q[b,5]*T*T/2;

Qm[b,1]:=Qm[b,1]+Qm[b,2]*T+Qm[b,3]*T*T/2;

Q[b,2]:=Q[b,2]+Q[b,5]*T;

Qm[b,2]:=Qm[b,2]+Qm[b,3]*T; d[b]:=Bm[b]*(P[b,1]*(Q[b,1]-Qm[b,1])+P[b,2]*(Q[b,2]-Qm[b,2]));

for c:=1 to 4 do Kp[b,c]:=-Gk[b,c]*d[b]*Q[b,c]-Ak[b,c]*K[b,c]; if b=1 then e:=2 else e:=1;

for c:=6 to 10 do Rp[b,c]:=-Gr[b,c]*d[b]*Q4[b,c]-Ar[b,c]*R[b,c]; for c:=1 to 4 do

K[b,c]:=K[b,c]+Kp[b,c]*T; for c:=6 to 10 do R[b,c]:=R[b,c]+Rp[b,c]*T;

end;

t1:=t1+T;

end;

end;

179