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

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

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

Задание.

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

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

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

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

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

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

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

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

Рис. 1.

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

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

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

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

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

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

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

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

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

Рис. 2.

L1=0.2; L2=0.2.

В точке А: L3=0.2; =0; ш=0.

В точке В: L3= 1.08 м; =0.529 рад; ш=0.279 рад Определение траектории, скоростей и ускорений.

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

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

f T<=T1 then.

begin.

q:=H*Sqr (T) / (2*T1*(Tau — T1));

q1:=H/(T1*(Tau-T1))*T;

q2:=H/(T1*(Tau-T1));

end else.

if T<=(Tau-T1) then.

begin.

q:=H*(2*T-T1)/(2*(Tau-T1));

q1:=H/(Tau-T1);

q2:=0;

end.

else.

if T<=Tau then.

begin.

q:=H*(1-Sqr (Tau-T)/(2*T1*(Tau-T1)));

q1:=H/(T1*(Tau-T1))*(Tau-T);

q2:=-H/(T1*(Tau-T1)); +.

end;

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

unit Unit1;

interface.

uses.

Windows, Messages, SysUtils, Variants, Classes, Graphics, Controls, Forms,.

Dialogs, StdCtrls, ExtCtrls, Grids, ComOBJ;

type.

TForm1 = class (TForm).

Button1: TButton;

Button2: TButton;

Timer1: TTimer;

StringGrid1: TStringGrid;

Image1: TImage;

Image2: TImage;

Image3: TImage;

Image4: TImage;

Image5: TImage;

Image6: TImage;

procedure FormCreate (Sender: TObject);

procedure Button1Click (Sender: TObject);

procedure Timer1Timer (Sender: TObject);

private public end;

var.

Form1: TForm1;

implementation.

{$R *.dfm}.

const.

ax=0.2; ay=0.2; az=0.2;

bx=0.8; by=0.5; bz=0.7;

L1=0.2; L2=0.2;

aFi=0.0; aPsi=0.0; aL3=0.2;

bFi=0.52 955 776 771 375 357 952; bPsi=0.27 980 931 571 248 898 048; bL3=1.862 780 491 200 215 680;

var.

Tau, T1, dT, T, tx, ty, tz: Single;

HFi, HPsi, Hl3, qFi, qPsi, qL3, q1Fi, q1Psi, q1L3, q2Fi, q2Psi, q2L3: Single;

Step, Kol_Step:Integer;

procedure Low₂(H, Tau, T1, T:Single; var q, q1, q2:Single);

begin.

if T<=T1 then.

begin.

q:=H*Sqr (T) / (2*T1*(Tau — T1));

q1:=H/(T1*(Tau-T1))*T;

q2:=H/(T1*(Tau-T1));

end else.

if T<=(Tau-T1) then.

begin.

q:=H*(2*T-T1)/(2*(Tau-T1));

q1:=H/(Tau-T1);

q2:=0;

end.

else.

f T<=Tau then.

begin.

q:=H*(1-Sqr (Tau-T)/(2*T1*(Tau-T1)));

q1:=H/(T1*(Tau-T1))*(Tau-T);

q2:=-H/(T1*(Tau-T1));

end;

end;

function GetX (Fi, Psi, L3: Single):Single;

begin.

// Result:=L3*cos (Fi)*cos (Psi)+L2*sin (Fi)+L1*sin (Fi);

Result:= Cos (fi)*Cos (Psi)*L3 — Sin (Fi)*L2;

end;

function GetY (Psi, L3: Single):Single;

begin.

Result:= Sin (psi)*L3 + L1;

end;

function GetZ (Fi, Psi, L3: Single):Single;

begin.

Result:= Sin (Fi) * Cos (Psi)*L3 + Cos (Fi)*L2;

end;

procedure TForm1. FormCreate (Sender: TObject);

begin.

Tau:=2;

T1:=0.45;

Kol_Step:=40;

end;

procedure TForm1. Button1Click (Sender: TObject);

var i: integer;

begin.

with StringGrid1 do begin.

Cells[0,1]: ='Fi'; Cells[0,2]: ='Psi'; Cells[0,3]: ='L3';

Cells[0,4]:='V (Fi)'; Cells[0,5]: ='V (Psi)'; Cells[0,6]: ='V (L3)';

Cells[0,7]:='a (Fi)'; Cells[0,8]: ='a (Psi)'; Cells[0,9]: ='a (L3)';

ColCount:=Kol_Step+2;

Cells[1,0]:='0';

For i:=2 to Kol_Step+1 do.

Cells[i, 0]: =FloatToStr (Tau/Kol_Step*(i-1));

end;

Step:=0;

tx:=ax; ty:=ay; tz:=az;

with Image4. Canvas do begin.

Pen.Color:=clGreen;

MoveTo (10+Round (ax*200), 300);

LineTo (10+Round (ax*200+0.7*ay*200), 300-Round (0.7*ay*200));

MoveTo (10+Round (0.7*ay*200), 300-Round (0.7*ay*200));

LineTo (10+Round (ax*200+0.7*ay*200), 300-Round (0.7*ay*200));

ineTo (10+Round (ax*200+0.7*ay*200), 300-Round (0.7*ay*200+az*200));

end;

T:=0;

HFi:=-aFi+bFi; HPsi:=ABS (-aPsi+bPsi); HL3:=ABS (-aL3+bL3);

qFi:=aFi; qPsi:=aPsi; qL3:=aL3;

q1Fi:=0; q1Psi:=0; q1L3:=0;

q2Fi:=0; q2Psi:=0; q2L3:=0;

with StringGrid1 do begin.

Cells[Step+1,1]: =FloatToStr (qFi);

Cells[Step+1,2]:=FloatToStr (qPsi);

Cells[Step+1,3]:=FloatToStr (qL3);

Cells[Step+1,4]:=FloatToStr (q1Fi);

Cells[Step+1,5]:=FloatToStr (q1Psi);

Cells[Step+1,6]:=FloatToStr (q1L3);

Cells[Step+1,7]:=FloatToStr (q2Fi);

Cells[Step+1,8]:=FloatToStr (q2Psi);

Cells[Step+1,9]:=FloatToStr (q2L3);

end;

Image1.Canvas.MoveTo (20+Step*5,200-Round (qFi*200));

Image2.Canvas.MoveTo (20+Step*5,200-Round (q1Fi*300));

Image3.Canvas.MoveTo (20+Step*5,100-Round (q2Fi*80));

Image5.Canvas.MoveTo (20+Step*5,200-Round (q1L3*300));

Image6.Canvas.MoveTo (20+Step*5,100-Round (q2L3*80));

dT:=(Tau/Kol_Step);

Timer1.Interval:=round (dT*10);

Timer1.Enabled:=true;

end;

procedure TForm1. Timer1Timer (Sender: TObject);

var.

x, y, z:Single;

begin.

Inc (Step);

Timer1.Enabled:=False;

T:=T+dT;

Low₂(HFi, Tau, T1, T, qFi, q1Fi, q2Fi);

Low₂(HPsi, Tau, T1, T, qPsi, q1Psi, q2Psi);

Low₂(HL3,Tau, T1, T, qL3, q1L3,q2L3);

qFi:=qFi+aFi;

qPsi:=qPsi+aPsi;

qL3:=qL3+aL3;

x:=GetX (qFi, qPsi, qL3);

y:=GetY (qPsi, qL3);

z:=GetZ (qFi, qPsi, qL3);

with Image4. Canvas do begin.

Pen.Color:=clBlack;

LineTo (10+Round (x*200+0.7*y*200), 300-Round (0.7*y*200+z*200));

Pen.Color:=clGreen;

MoveTo (10+Round (x*200), 300);

LineTo (10+Round (x*200+0.7*y*200), 300-Round (0.7*y*200));

MoveTo (10+Round (0.7*y*200), 300-Round (0.7*y*200));

LineTo (10+Round (x*200+0.7*y*200), 300-Round (0.7*y*200));

LineTo (10+Round (x*200+0.7*y*200), 300-Round (0.7*y*200+z*200));

end;

if Step=Kol_Step.

then.

begin.

Fi:=bFi; qPsi:=bPsi; qL3:=bL3;

q1Fi:=0; q1Psi:=0; q1L3:=0;

q2Fi:=0; q2Psi:=0; q2L3:=0;

end;

with StringGrid1 do begin.

Cells[Step+1,1]: =FloatToStr (qFi);

Cells[Step+1,2]:=FloatToStr (qPsi);

Cells[Step+1,3]:=FloatToStr (qL3);

Cells[Step+1,4]:=FloatToStr (q1Fi);

Cells[Step+1,5]:=FloatToStr (q1Psi);

Cells[Step+1,6]:=FloatToStr (q1L3);

Cells[Step+1,7]:=FloatToStr (q2Fi);

Cells[Step+1,8]:=FloatToStr (q2Psi);

Cells[Step+1,9]:=FloatToStr (q2L3);

end;

Image1.Canvas.LineTo (20+Step*5,200-Round (qFi*200));

Image2.Canvas.LineTo (20+Step*5,200-Round (q1Fi*200));

Image3.Canvas.LineTo (20+Step*5,100-Round (q2Fi*50));

Image5.Canvas.LineTo (20+Step*5,200-Round (q1L3*200));

Image6.Canvas.LineTo (20+Step*5,100-Round (q2L3*50));

tx:=x; ty:=y; tz:=z;

if Step.

end;

end.

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

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

Рис. 3.

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

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

Рис. 5.

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

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

Вывод формул Пользователем задаются:

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

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

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

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

Законы:

Vdx=Vd*cos60 Vdz=Vd*sin60.

Xd=Xd+Vdx*t.

Zd=Zd+Vdz*t здесь tшаг времени Скорость в точке захвата:

Vcx=Vdx+(Xc-Xd)/T.

Vcz=Vdz+(Zc-Zd)/T где T=-t/ln (0,001).

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

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

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

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

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

Координаты в точке захвата:

Xc=Xc+Vcx*t.

Zc=Zc+Vcz*t.

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

Рис. 6.

Рис. 7.

Рис. 8.

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

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

unit Unit1;

interface.

uses.

Windows, Messages, SysUtils, Variants, Classes, Graphics, Controls, Forms,.

Dialogs, Grids, StdCtrls, ExtCtrls, ComOBJ;

type.

TForm1 = class (TForm).

StringGrid1: TStringGrid;

Timer1: TTimer;

Button1: TButton;

Image1: TImage;

Image2: TImage;

Image3: TImage;

Label1: TLabel;

Button2: TButton;

procedure FormCreate (Sender: TObject);

procedure Timer1Timer (Sender: TObject);

procedure Button1Click (Sender: TObject);

procedure Button2Click (Sender: TObject);

private public end;

var.

Form1: TForm1;

implementation.

{$R *.dfm}.

var.

VDX, VDY, Vd, Alpha, T, Tau, TCatch, d, dT:Single;

Xd, Yd, Xd0, Yd0,L, L3, Fi, Fi0: Single;

Old_CX, Cur_CX, Old_CY, Cur_CY, Old_DX, Cur_DX,.

Old_DY, Cur_DY, Old_V, Cur_V, Old_W, Cur_W, X_c, Y_c:Single;

H, LF, Takt: Integer;

Fail:Boolean;

procedure InitManipul;

begin.

VDX:=Vd*1000*cos (Alpha*pi/180);

VDY:=Vd*1000*sin (Alpha*pi/180);

T:=(Tau/1.7)/ln (10);

TCatch:=0; Fail:=False;

Xd:=Xd0; Yd:=Yd0;

L:=L3; Fi:=Fi0;

end;

procedure InitStrGr (T:word;V, W: Single);

var i: word;

begin.

with Form1. StringGrid1 do.

begin.

Cells[0,1]: ='V';

Cells[0,2]:='W';

ColCount:=T+2;

for i:=0 to T do.

Cells[i+1,0]: =FloatToStrF (i*dT, ffGeneral, 4,2);

Cells[T+1,1]:=FloatToStrF (V/1000,ffGeneral, 4,2);

Cells[T+1,2]:=FloatToStrF (W, ffGeneral, 4,2);

end;

end;

procedure DrawGraf;

var MT, MV, MW: Single;

const Cx=15; Cy=100; dH=10;

begin.

with Form1 do begin.

image1.Canvas.Pen.Color:=clBlue;

image1.Canvas.MoveTo (Round (Old_CX/10), 250-Round (Old_CY/10));

image1.Canvas.LineTo (Round (Cur_CX/10), 250-Round (Cur_CY/10));

image1.Canvas.Pen.Color:=clRed;

image1.Canvas.MoveTo (Round (Old_DX/10), 250-Round (Old_DY/10));

image1.Canvas.LineTo (Round (Cur_DX/10), 250-Round (Cur_DY/10));

MT:=(image2.Width-2*Cx)/(Tau/dt);

MV:=(image2.Height-2*dH)/(2*Vd*3*1000);

image2.Canvas.MoveTo (Round ((CX+(Takt-1)*MT)), 220-Round ((CY-Old_V*MV)));

image2.Canvas.LineTo (Round ((CX+Takt*MT)), 220-Round ((CY-Cur_V*MV)));

MT:=(Image3.Width-2*Cx)/(Tau/dT);

MW:=(Image3.Height-2*dH)/2;

image3.Canvas.MoveTo (Round ((CX+(Takt-1)*MT)), 250-Round ((CY-Old_W*MW)*2));

image3.Canvas.LineTo (Round ((CX+Takt*MT)), 250-Round ((CY-Cur_W*MW)*2));

Old_CX:=Cur_CX; Old_CY:=Cur_CY;

Old_DX:=Cur_DX; Old_DY:=Cur_DY;

Old_V:=Cur_V; Old_W:=Cur_W;

end;

end;

procedure TForm1. FormCreate (Sender: TObject);

begin.

Xd0:=0; Yd0:=700; Vd:=0.2;

Alpha:=60; L3:=1000; Fi0:=100;

Tau:=4; d:=1; dT:=0.2;

H:=500; LF:=20 000; Takt:=0;

InitStrGr (Takt, 0,0);

end;

procedure TForm1. Button1Click (Sender: TObject);

begin.

InitManipul;

Timer1.Enabled:=true;

Timer1.Interval:=round (dt*500);

Takt:=0;

Old_CX:=H*cos (Fi0*pi/180)+L3*Sin (Fi0*pi/180);

Old_CY:=H*sin (Fi0*pi/180)-L3*cos (Fi0*pi/180);

X_c:=Old_CX; Y_c:=Old_CY;

Old_DX:=Xd0; Old_DY:=Yd0;

Old_V:=0; Old_W:=0;

end;

procedure TForm1. Timer1Timer (Sender: TObject);

var RFi, V, W, VCx, VCy: Single;

begin.

Inc (Takt);

RFi:=Fi*pi/180;

X_c:=H*cos (RFi)+L*Sin (RFi); Y_c:=H*sin (RFi)-L*cos (RFi);

VCx:=VDX+((Xd-X_c)/T); VCy:=VDY+((Yd-Y_c)/T);

X_c:=X_c+VCx*dT; Y_c:=Y_c+VCy*dT;

Xd:=Xd+VDX*dT; Yd:=Yd+VDY*dT;

TCatch:=TCatch+dT;

if L<=0 then L:=1;

V:=(VCx*sin (RFi))-(VCy*cos (RFi))+H/2;

W:=((VCx*cos (RFi))+(VCy*sin (RFi)))/L;

InitStrGr (Takt, V, W);

Xd:=Xd+(VDX*dT); Yd:=Yd+(VDY*dT);

L:=L+(V*dT);

Fi:=Fi+(W*dT*180/pi);

Cur_CX:=X_c; Cur_CY:=Y_c;

Cur_DX:=Xd; Cur_DY:=Yd;

Cur_V:=V; Cur_W:=W;

DrawGraf;

if (abs (X_c-Xd)<=d) and (abs (Y_c-Yd)<=d) then begin.

Label1.Caption:='Goal';

Timer1.Enabled:=false;

end;

if TCatch>Tau then begin.

Timer1.Enabled:=false;

Label1.Caption:='Out of time';

end;

if L>LF then begin.

Timer1.Enabled:=false;

Label1.Caption:='Out of Speed';

end;

end;

Procedure StrGridToExcel (SG:TStringGrid; InverceRows: Boolean);

Var.

ExcelApp, Workbook: OLEVariant;

EndRow, EndCol, BeginCol, BeginRow, i, j: integer;

Begin.

ExcelApp:= CreateOleObject ('Excel.Application');

ExcelApp.Application.EnableEvents:= false;

Workbook:= ExcelApp.WorkBooks.Add;

BeginCol:=0;

BeginRow:=0;

EndCol:=SG.ColCount+1;

EndRow:=SG.RowCount+1;

If InverceRows then.

begin.

For i:=BeginCol to EndCol do.

For j:=BeginRow to EndRow do.

Try.

ExcelApp.WorkSheets[1]. Cells[i+1,j+1]:=StrToFloat (SG.Cells[i, j]);

Except.

ExcelApp.WorkSheets[1].Cells[i+1,j+1]:=SG.Cells[i, j];

End;

end.

Else.

begin.

For i:=BeginCol to EndCol do.

For j:=BeginRow to EndRow do.

Try.

ExcelApp.WorkSheets[1]. Cells[i+1,j+1]:=StrToFloat (SG.Cells[j, i]);

Except.

ExcelApp.WorkSheets[1].Cells[i+1,j+1]:=SG.Cells[j, i];

End;

end;

ExcelApp.Visible:= true;

End;

procedure TForm1. Button2Click (Sender: TObject);

begin.

StrGridToExcel (StringGrid1,True);

end;

end.