3127

Ввод постоянных па-

1раметров манипулятора, эталонной модели и приводов

2Определение типов

массивов и переменных

Ввод коэффициентов,

3задающих воздействий, начальных условий и временных параметров t10, t2, T

4 t1 = t10

5 Определение локаль-

ных управляющих воздействий Uaлl, Uал2

6Определение развязы-

вающих управляющих воздействий Uaрl, Uар2

7Определение управляю-

щих воздействий Ual,Uа2

9	Определение переме-
	щений координат и
	эталонных моделей
	q1, q2, q1m, q2m

10Определение скоростей

координат и эталонных моделей q1 , q2 , q1m , q2m

11Определение произ-

водных от самонастраивающихся коэф-

фициентов ki j , i =1, 2, j=l 4, 6 10

12Определение новых

значений самонастраивающихся коэффициентов kij, i =1, 2, j =1 4, 6 10

13 t1 = t1+T

241

Да

14 t1 < t2

{Параметры манипулятора}

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

Программа 3

implementation const

JS1=2.5; JS2=4.13; J2=3.51; J3=-1.93; l2=0.5; g=9.8;

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

242

{Контурные настройки}

Kc1=1; Kc2=1.68; Kp1=11; Kp2=17.56;

i=102.5; {Передаточное число}

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

t1,t2,T,B1,D1,E1,acc: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;

243

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]; 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;

244

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

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]));

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

245

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

246

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;

6.7. Программная реализация алгоритмов планирования движения мобильного робота

Планирование движений тележки и манипулятора мобильного робота с учетом препятствий основано на уравнениях (3.24), (3.27 3.35). Программа 4 для расчета параметров при планировании движений тележки разработана в среде DELPHI 3.0 /18/. Ее алгоритм приведен на рис. 6.11.

В начале программы задаются длина и ширина тележки. Затем вводятся начальные значения координат х(t0), y(t0) центра масс транспортной тележки и их конечные значения x(tT), y(tT), величина -области, начальные значения параметров b1, b2, b3, входящие в уравнение для определения курсового угла.

247

Далее в соответствии с параметрами препятствий рассчитываются координаты вершин прямоугольников, в которые заключены препятствия. На основе величины -области рассчитываются координаты запрещенной зоны, ближе которой центр масс тележки не должен подходить к препятствию.

Организуется цикл по времени. Рассчитывается начальный курсовой угол (t, b2, b3), обеспечивающий движение к конечной точке по прямой линии, при этом b2= b3 = 0. По курсовому углу определяются координаты x(t, ), y(t, ).

Для текущего	значения x(t) определяются параметры ai, bi, такие, что ai<x(t) bi, и рассчи-
тываются значения	i, i. Для координаты y(t) должно выполняться неравенство |y(t)- i|< i. Это

означает, что тележка находится в свободной зоне.

Если данное неравенство верно, то текущее время увеличивается на период дискретности и начинается следующий шаг цикла со старыми коэффициентами b2, b3. Если условие нарушается, то производится коррекция коэффициентов b2, b3 с помощью конечно-сходящегося алгоритма «Полоска - 1». Измененные параметры b2, b3 задаются на новом этапе расчета как начальные и цикл начинается снова (t=0). Так продолжается до тех пор, пока для всех участков при перемещении тележки не будет выполняться условие |y(t)- i|< i.

Расчеты завершаются после того, как координаты х(t) и y(t) попадут в окрестность заданной опорной точки: x(t) = x(tT) , y(t) = y(tT) , где - точность позиционирования тележки.

Последние значения коэффициентов b2, b3			принимаются как результат вычислений и коррекции
и выводятся на экран.
1 Определение типов			248
массивов, перемен-	9		Определение
ных и функции f1		координат Х,Y

249

{Определение типов переменных и массивов}

Рис. 6.11. Алгоритм расчета параметров траектории

Программа 4

implementation label 1,2,3,4,5; var i,j,k,x0,y0:integer;

z,t1,f0,b20,b21,b22,b23,a,e,h,t,q,P,l,f,d,KX0,KY0,KXt, KYt:real;

xp: array[1..2, 1..8] of real;	yp: array[1..2, 1..8] of real;
xn: array[1..2, 1..8] of integer;	yn: array[1..2, 1..8] of integer;
xz: array[1..2, 1..8] of integer;	yz: array[1..2, 1..8] of integer;

{$R *.DFM}

{ функция для нахождения курсового угла f} function f1(t,b21,b22,b23:real):real;

begin f1:=(b21*t+b22*t*t+b23*t*sqr(t));

250