Механика и алгоритмы управления роботами

Механика и алгоритмы управления роботами

Курсовая работа по предмету:

Механика и алгоритмы управления роботами

Задание

Кинематическое исследование механизма манипулятора

определить необходимые перемещения звеньев

определить траекторию движения захвата из точки (0,2; 0,2; 0,2) в точку (0,8; 0,5; 0,7), если закон движения №4

определить скорости и ускорения движения звеньев.

реализовать программу расчетов

построить графики:

траектории движения захвата в ZOX

скорости и ускорения захвата

скорости и ускорения 1-ого звена.

Начальные данные

Рис. 1

Кинематика управления манипулятором:

вывести законы движения во времени для детали и захвата;

реализовать программу расчетов.

построить графики:

траектории движения захвата детали в ZOX, если угол движения детали 60°;

скоростей и ускорений ведомых звеньев;

проверить решение графическим способом.

Введение

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

Кинематическое исследование механизма манипулятора. Определение необходимых перемещений звеньев

Рис. 2

=0.2; L2=0.2

В точке А: L3=0.2; j=0; ψ=0

В точке В: L3= 1.08 м; j=0.529 рад; ψ=0.279 рад

Определение траектории, скоростей и ускорений

Перемножив полученные матрицы и вектор L, получим следующие законы для изменения координат захвата по времени:

Перемещение, скорость и ускорение находятся по 4 закону:

f T<=T1 then:=H*Sqr(T) / (2*T1*(Tau - T1));:=H/(T1*(Tau-T1))*T;:=H/(T1*(Tau-T1));elseT<=(Tau-T1) then:=H*(2*T-T1)/(2*(Tau-T1));:=H/(Tau-T1);:=0;T<=Tau then:=H*(1-Sqr(Tau-T)/(2*T1*(Tau-T1)));:=H/(T1*(Tau-T1))*(Tau-T);:=-H/(T1*(Tau-T1)); +;

текст программыUnit1;, Messages, SysUtils, Variants, Classes, Graphics, Controls, Forms,, StdCtrls, ExtCtrls, Grids, ComOBJ;= class(TForm): TButton;: TButton;: TTimer;: TStringGrid;: TImage;: TImage;: TImage;: TImage;: TImage;: TImage;FormCreate(Sender: TObject);Button1Click(Sender: TObject);Timer1Timer(Sender: TObject);public end;: TForm1;

{$R *.dfm}=0.2; ay=0.2; az=0.2;=0.8; by=0.5; bz=0.7;=0.2; L2=0.2;=0.0; aPsi=0.0; aL3=0.2;=0.52955776771375359150; bPsi=0.27980931571248899761; bL3=1.0862780491200215724;,T1,dT,T,tx,ty,tz:Single;,HPsi,Hl3,qFi,qPsi,qL3,q1Fi,q1Psi,q1L3,q2Fi,q2Psi,q2L3:Single;,Kol_Step:Integer;Low_2(H,Tau,T1,T:Single; var q,q1,q2:Single);T<=T1 then:=H*Sqr(T) / (2*T1*(Tau - T1));:=H/(T1*(Tau-T1))*T;:=H/(T1*(Tau-T1));elseT<=(Tau-T1) then:=H*(2*T-T1)/(2*(Tau-T1));:=H/(Tau-T1);:=0;T<=Tau then:=H*(1-Sqr(Tau-T)/(2*T1*(Tau-T1)));:=H/(T1*(Tau-T1))*(Tau-T);:=-H/(T1*(Tau-T1));;;GetX(Fi,Psi,L3:Single):Single;

// Result:=L3*cos(Fi)*cos(Psi)+L2*sin(Fi)+L1*sin(Fi);:= Cos(fi)*Cos(Psi)*L3 - Sin(Fi)*L2;;GetY(Psi,L3:Single):Single;:= Sin(psi)*L3 + L1;;GetZ(Fi,Psi,L3:Single):Single;

Result:= Sin(Fi) * Cos(Psi)*L3 + Cos(Fi)*L2;;TForm1.FormCreate(Sender: TObject);:=2;:=0.45;_Step:=40;;TForm1.Button1Click(Sender: TObject);i:integer;StringGrid1 do begin[0,1]:='Fi'; Cells[0,2]:='Psi'; Cells[0,3]:='L3';[0,4]:='V(Fi)'; Cells[0,5]:='V(Psi)'; Cells[0,6]:='V(L3)';[0,7]:='a(Fi)'; Cells[0,8]:='a(Psi)'; Cells[0,9]:='a(L3)';:=Kol_Step+2;[1,0]:='0';i:=2 to Kol_Step+1 do[i,0]:=FloatToStr(Tau/Kol_Step*(i-1));;:=0;:=ax; ty:=ay; tz:=az;Image4.Canvas do begin.Color:=clGreen;(10+Round(ax*200),300);(10+Round(ax*200+0.7*ay*200),300-Round(0.7*ay*200));(10+Round(0.7*ay*200),300-Round(0.7*ay*200));(10+Round(ax*200+0.7*ay*200),300-Round(0.7*ay*200));(10+Round(ax*200+0.7*ay*200),300-Round(0.7*ay*200+az*200));;:=0;:=-aFi+bFi; HPsi:=ABS(-aPsi+bPsi); HL3:=ABS(-aL3+bL3);:=aFi; qPsi:=aPsi; qL3:=aL3;Fi:=0; q1Psi:=0; q1L3:=0;Fi:=0; q2Psi:=0; q2L3:=0;StringGrid1 do begin[Step+1,1]:=FloatToStr(qFi);[Step+1,2]:=FloatToStr(qPsi);[Step+1,3]:=FloatToStr(qL3);[Step+1,4]:=FloatToStr(q1Fi);[Step+1,5]:=FloatToStr(q1Psi);[Step+1,6]:=FloatToStr(q1L3);[Step+1,7]:=FloatToStr(q2Fi);[Step+1,8]:=FloatToStr(q2Psi);[Step+1,9]:=FloatToStr(q2L3);;.Canvas.MoveTo(20+Step*5,200-Round(qFi*200));.Canvas.MoveTo(20+Step*5,200-Round(q1Fi*300));.Canvas.MoveTo(20+Step*5,100-Round(q2Fi*80));.Canvas.MoveTo(20+Step*5,200-Round(q1L3*300));.Canvas.MoveTo(20+Step*5,100-Round(q2L3*80));:=(Tau/Kol_Step);.Interval:=round(dT*10);.Enabled:=true;;TForm1.Timer1Timer(Sender: TObject);,y,z:Single;(Step);.Enabled:=False;:=T+dT;_2(HFi,Tau,T1,T,qFi,q1Fi,q2Fi);_2(HPsi,Tau,T1,T,qPsi,q1Psi,q2Psi);_2(HL3,Tau,T1,T,qL3,q1L3,q2L3);:=qFi+aFi;:=qPsi+aPsi;:=qL3+aL3;:=GetX(qFi,qPsi,qL3);:=GetY(qPsi,qL3);:=GetZ(qFi,qPsi,qL3);Image4.Canvas do begin.Color:=clBlack;(10+Round(x*200+0.7*y*200),300-Round(0.7*y*200+z*200));.Color:=clGreen;(10+Round(x*200),300);(10+Round(x*200+0.7*y*200),300-Round(0.7*y*200));(10+Round(0.7*y*200),300-Round(0.7*y*200));(10+Round(x*200+0.7*y*200),300-Round(0.7*y*200));(10+Round(x*200+0.7*y*200),300-Round(0.7*y*200+z*200));;Step=Kol_Step:=bFi; qPsi:=bPsi; qL3:=bL3;Fi:=0; q1Psi:=0; q1L3:=0;Fi:=0; q2Psi:=0; q2L3:=0;;StringGrid1 do begin[Step+1,1]:=FloatToStr(qFi);[Step+1,2]:=FloatToStr(qPsi);[Step+1,3]:=FloatToStr(qL3);[Step+1,4]:=FloatToStr(q1Fi);[Step+1,5]:=FloatToStr(q1Psi);[Step+1,6]:=FloatToStr(q1L3);[Step+1,7]:=FloatToStr(q2Fi);[Step+1,8]:=FloatToStr(q2Psi);[Step+1,9]:=FloatToStr(q2L3);;.Canvas.LineTo(20+Step*5,200-Round(qFi*200));.Canvas.LineTo(20+Step*5,200-Round(q1Fi*200));.Canvas.LineTo(20+Step*5,100-Round(q2Fi*50));.Canvas.LineTo(20+Step*5,200-Round(q1L3*200));.Canvas.LineTo(20+Step*5,100-Round(q2L3*50));:=x; ty:=y; tz:=z;Step<Kol_Step then Timer1.Enabled:=True;

end;.

Построение графиков

Траектория движения захвата и график пути первого звена (по заданию).

Рис. 3

Графики скорости и ускорения захвата

Рис. 4

Графики скорости и ускорения первого звена

Рис. 5

Кинематика управления манипулятором

Постановка задачи

Во второй части курсовой работы манипулятор будет рассматриваться в плоскости ZOX. В этой плоскости под углом 60° деталь движется из заданной точки и имеет постоянную скорость. Задача заключается в отыскании законов, позволяющих изменяя размеры звеньев манипулятора за указанное время движения захватить эту деталь.

Вывод формул

Пользователем задаются:

Координаты и скорость движения детали в плоскости ZOX (Xd,Zd,Vd)

Длина руки робота (L3)

Построим проекцию робота в плоскости ZOX

В плоскости ZOX видно только L2 и L3

Законы:=Vd*cos60° Vdz=Vd*sin60°=Xd+Vdx*t=Zd+Vdz*t здесь t- шаг времени

Скорость в точке захвата:=Vdx+(Xc-Xd)/T=Vdz+(Zc-Zd)/T где T=-t/ln(0,001)

Xc:=L3*cos(Fi)-L2*sin(Fi);:=L3*sin(Fi)+L2*cos(Fi);

Берем производную от Xc, Zc. И в итоге, после домножения одного уравнения на sin(Fi) или cos(Fi), а затем преобразования уравнений, получаем следующие формулы:

V:=(Vcx*sin(Fi))-(Vcz*cos(Fi));:=((Vcx*cos(Fi))+(Vcz*sin(Fi)))/L3;

Координаты в точке захвата:=Xc+Vcx*t=Zc+Vcz*t

Построение графиков

Входные параметры: координаты детали (0,700), скорость детали 0,2 м/с, длина руки робота 1 м, угол наклона руки 100 градусов, время движения равно 4.

Рис. 6

Рис. 7

Рис. 8

кинематический манипулятор захват звено

Текст программы

unit Unit1;, Messages, SysUtils, Variants, Classes, Graphics, Controls, Forms,, Grids, StdCtrls, ExtCtrls, ComOBJ;= class(TForm): TStringGrid;: TTimer;: TButton;: TImage;: TImage;: TImage;: TLabel;: TButton;FormCreate(Sender: TObject);Timer1Timer(Sender: TObject);Button1Click(Sender: TObject);Button2Click(Sender: TObject);public end;: TForm1;

{$R *.dfm},VDY,Vd,Alpha,T,Tau,TCatch,d,dT:Single;,Yd,Xd0,Yd0,L,L3,Fi,Fi0:Single;_CX,Cur_CX,Old_CY,Cur_CY,Old_DX,Cur_DX,_DY,Cur_DY,Old_V,Cur_V,Old_W,Cur_W,X_c,Y_c:Single;,LF,Takt:Integer;:Boolean;InitManipul;:=Vd*1000*cos(Alpha*pi/180);:=Vd*1000*sin(Alpha*pi/180);:=(Tau/1.7)/ln(10);:=0; Fail:=False;:=Xd0; Yd:=Yd0;:=L3; Fi:=Fi0;;InitStrGr(T:word;V,W:Single);i:word;Form1.StringGrid1 do[0,1]:='V';[0,2]:='W';:=T+2;i:=0 to T do[i+1,0]:=FloatToStrF(i*dT,ffGeneral,4,2);[T+1,1]:=FloatToStrF(V/1000,ffGeneral,4,2);[T+1,2]:=FloatToStrF(W,ffGeneral,4,2);;;DrawGraf;MT,MV,MW:Single;Cx=15; Cy=100; dH=10;Form1 do begin.Canvas.Pen.Color:=clBlue;.Canvas.MoveTo(Round(Old_CX/10),250-Round(Old_CY/10));.Canvas.LineTo(Round(Cur_CX/10),250-Round(Cur_CY/10));.Canvas.Pen.Color:=clRed;.Canvas.MoveTo(Round(Old_DX/10),250-Round(Old_DY/10));.Canvas.LineTo(Round(Cur_DX/10),250-Round(Cur_DY/10));:=(image2.Width-2*Cx)/(Tau/dt);:=(image2.Height-2*dH)/(2*Vd*3*1000);.Canvas.MoveTo(Round((CX+(Takt-1)*MT)),220-Round((CY-Old_V*MV)));.Canvas.LineTo(Round((CX+Takt*MT)),220-Round((CY-Cur_V*MV)));:=(Image3.Width-2*Cx)/(Tau/dT);:=(Image3.Height-2*dH)/2;.Canvas.MoveTo(Round((CX+(Takt-1)*MT)),250-Round((CY-Old_W*MW)*2));.Canvas.LineTo(Round((CX+Takt*MT)),250-Round((CY-Cur_W*MW)*2));_CX:=Cur_CX; Old_CY:=Cur_CY;_DX:=Cur_DX; Old_DY:=Cur_DY;_V:=Cur_V; Old_W:=Cur_W;;;TForm1.FormCreate(Sender: TObject);:=0; Yd0:=700; Vd:=0.2;:=60; L3:=1000; Fi0:=100;:=4; d:=1; dT:=0.2;:=500; LF:=20000; Takt:=0;(Takt,0,0);;TForm1.Button1Click(Sender: TObject);;.Enabled:=true;.Interval:=round(dt*500);:=0;_CX:=H*cos(Fi0*pi/180)+L3*Sin(Fi0*pi/180);_CY:=H*sin(Fi0*pi/180)-L3*cos(Fi0*pi/180);_c:=Old_CX; Y_c:=Old_CY;_DX:=Xd0; Old_DY:=Yd0;_V:=0; Old_W:=0;;TForm1.Timer1Timer(Sender: TObject);RFi,V,W,VCx,VCy:Single;(Takt);:=Fi*pi/180;_c:=H*cos(RFi)+L*Sin(RFi); Y_c:=H*sin(RFi)-L*cos(RFi);:=VDX+((Xd-X_c)/T); VCy:=VDY+((Yd-Y_c)/T);_c:=X_c+VCx*dT; Y_c:=Y_c+VCy*dT;:=Xd+VDX*dT; Yd:=Yd+VDY*dT;:=TCatch+dT;L<=0 then L:=1;:=(VCx*sin(RFi))-(VCy*cos(RFi))+H/2;:=((VCx*cos(RFi))+(VCy*sin(RFi)))/L;(Takt,V,W);:=Xd+(VDX*dT); Yd:=Yd+(VDY*dT);:=L+(V*dT);:=Fi+(W*dT*180/pi);_CX:=X_c; Cur_CY:=Y_c;_DX:=Xd; Cur_DY:=Yd;_V:=V; Cur_W:=W;;(abs(X_c-Xd)<=d) and (abs(Y_c-Yd)<=d) then begin.Caption:='Goal';.Enabled:=false;;TCatch>Tau then begin.Enabled:=false;.Caption:='Out of time';;L>LF then begin.Enabled:=false;.Caption:='Out of Speed';;;StrGridToExcel(SG:TStringGrid; InverceRows:Boolean);, Workbook: OLEVariant;, EndCol, BeginCol, BeginRow, i, j: integer;:= CreateOleObject('Excel.Application');.Application.EnableEvents:= false;:= ExcelApp.WorkBooks.Add;:=0;:=0;:=SG.ColCount+1;:=SG.RowCount+1;InverceRows theni:=BeginCol to EndCol doj:=BeginRow to EndRow do.WorkSheets[1].Cells[i+1,j+1]:=StrToFloat(SG.Cells[i,j]);.WorkSheets[1].Cells[i+1,j+1]:=SG.Cells[i,j];;i:=BeginCol to EndCol doj:=BeginRow to EndRow do.WorkSheets[1].Cells[i+1,j+1]:=StrToFloat(SG.Cells[j,i]);.WorkSheets[1].Cells[i+1,j+1]:=SG.Cells[j,i];;;.Visible:= true;;TForm1.Button2Click(Sender: TObject);(StringGrid1,True);;

end.