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

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

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

определить траекторию движения захвата из точки (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.52955776771375359150; bPsi=0.27980931571248899761; bL3=1.0862780491200215724;

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_2(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_2(HFi,Tau,T1,T,qFi,q1Fi,q2Fi);

Low_2(HPsi,Tau,T1,T,qPsi,q1Psi,q2Psi);

Low_2(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<Kol_Step then Timer1.Enabled:=True;

end;

end.

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