Dynamic

NGUYN NGC BNH CT 43
BI TON NG HC THUN
I.1. Xc lp cc h to
Ta chn h ta nh sau:
- Trc zi trng phng vi trc khp ng th i+1.
- V cc trc zi u theo phng thng ng nn trc xi c chn ty sao
cho xi vung gc vi z1 .
- Trc yi c chn theo quy tc bn tay phi.
- Trc z4 chn trng vi z3.
Cc h ta c th hin nh trn hnh 1.1
H = 1270 (mm)
L1 = 600 (mm)
L2 = 700 (mm)
d4 = 600 (mm)
Xc nh cc tham s ng hc D-H
T hnh 1.1 ta c bng cc tham s ng hc D-H nh sau:
Khu
d
a
1
1*
H
L1
2
2 *
0
L2
3
0
d3 *
0
4
4*
d4
0
0
0
NGUYN NGC BNH CT 43

I.2. Xc nh cc ma trn D H
Quy c vit tt cc hm lng gic nh sau :
C1= cos 1 ; S1= sin 1 ; C12= cos( 1 +2 ) ; S12= sin( 1 +2 ) ; . . . . .
T bng tham s D H, ta c cc ma trn:

C1
S
H1 = 1
0
0
1
0
H3 =
0
S1 0 L1C1
C1 0 L1 S1
(1.1)
0 1 H
0 0
1
0
1
0
0
0 0
0 0
(1.3)
1 d3
0 1
C12
S
D2 = H1 .H 2 = 12
0
S12
C12
0
0
C12
S
D3 = H1 .H 2 .H3 = 12
0
0 L1C1 + L2 C12
0 L1 S1 + L2 S12
1
H
0
1
S12
C12
0
0
S2
C2
0
0
0 L2 C2
0 L2 S2
(1.2)
1
0
0
1
C4
S
H4 = 4
0
S4
C4
0
0
0 0
0 0
1 d4
0 1
(1.5)
0 L1C1 + L2 C12
0 L1 S1 + L2 S12
1
d3 + H
0
1
C12C4 + S12 S4
S C C S
12 4
D4 = H1 .H 2 .H3 .H 4 = 12 4
C2
S
H2 = 2
0
C12 S4 + S12 C4
C12 C4 S12 S4
0
0
(1.6)
0 L1 C1 + L2 C12
0 L1 S1 + L2 S12 (1.7)
1 H d 3 d 4
0
1
Vy, to im E c dng:
xE(0) = L1C1 + L2C12
yE(0) = L1S1 + L2S 12

z E(0) = H d 3 d 4
(1.4)
NGUYN NGC BNH CT 43

I.3. H phng trnh ng hc
Khu cng tc ca Robot c phng trnh ng hc c bn dng:
nx
n
DE = y
nz
sx
sy
sz
0
ax
ay
az
0
px
p y
pz
(1.8)
Ta c: DE = D4, suy ra:

nx
n
y
nz
sx
sy
sz
0
ax
ay
az
0
px C12 C4 + S12 S4
py S12 C4 C12 S4
=
pz
0

1
0
C12 S4 + S12 C4
C12 C4 S12 S4
0
0
0 L1 C1 + L2 C12
0 L1 S1 + L2 S12
1 H d3 d4
0
1
So snh cc phn t tng ng ca cc ma trn 2 v, ta c h phng

trnh ng hc:
nx = C12 C4 + S12 S4 ;
n = S C C S ;
12 4
12 4
y
nz = 0;
sx = C12 S4 + S12 C4 ;
s = C C S S ;
12 4
12 4
y
sz = 0;
ax = 0;
a y = 0;
az = 1;
p = LC + L C ;
1 1
2 12
x
p y = L1 S1 + L2 S12 ;
pz = H ( d3 + d4 ).
(1.9)
Nu cho trc cc gi tr bin khp thay i theo thi gian, th v tr v hng ca

khu cng tc (bn kp) ca robot SCARA trong mi thi im s hon ton c xc
nh t h phng trnh (1.9).
NGUYN NGC BNH CT 43
BI TON NG HC NGC
H phng trnh (1.9) gip ta xc nh c v tr v hng ca bn kp khi bit
trc quy lut thay i ca cc gi tr bin khp theo thi gian. Tuy nhin, trong thc t
lm vic ca robot, thng thng chng ta li c trc quy lut chuyn ng ca bn kp,
cn xc nh quy lut thay i cc bin khp tng ng. y l ni dung ca bi ton
ng hc ngc.
Bi ton ng hc ngc c c bit quan tm v li gii ca n l c s ch
yu xy dng chng trnh iu khin chuyn ng ca robot bm theo mt qu o
cho trc.
ng hc ngc robot SCARA nhn c thng qua vic tm nghim ca
phng trnh :
C12C4 + S12 S4
S C C S
12 4
12 4
C12 S4 + S12 C4
C12 C4 S12 S4
0
0
0 L1 C1 + L2 C12
0 L1 S1 + L2 S12 A p
=
1 H d3 d4 0 1
0
1
r11
Trong : A l ma trn cosin ch hng ca vt : A = r21

r31
(2.1)
r12
r22
r32
r13
r23
r33
O = [xv yv zv]T l ta ca khu cng tc

Thc t, ta d dng nhn thy rng phng trnh 2.1 ch c th gii c khi A c
dng
C
A = S
0
S
C
0
0
0
1
(2.2)
(2.3)
Khi ta c: 1 + 2 - 4 = = atan2( r12 , r11)
Chiu v tr ca tay my ln mt phng Ox0y0 ( nh hnh 2.1) ta c :

+) C2 =
xv2 + yv2 L12 L22

=> S2 = 1 C22 => 2= atan2( 1 C22 , C2 )
2.L1.L2
+) 1 = = atan2( yv , xv ) atan2( L2.S2 , L1 + L2.C2 )
(2.5)
(2.6)
(2.4)
NGUYN NGC BNH CT 43

T (2.3) ta suy ra: 4 = 1 + 2 - = 1 + 2 atan2( r12 , r11)
Cui cng d3 c xc nh bng cng thc : d3 = H (zv + d4)
(2.7)
T cc phng trnh (2.4) ,(2.5),(2.6),(2.7) ta c cc b nghim :
2 = atan2( 1 C , C2 )
2
2
1 = = atan2 ( yv , xv
xv2 + yv2 L12 L22

C2 =
2.L1.L2
atan2 ( L2 .S2, L1 + L2 .C2
(2.8)
4 = 1 + 2 atan2( r12 , r11 )

d3 = H
( zv
+ d4 )
zo
Hnh 2.1
L1
H
L2
O0
px
xo
py
pz
yo
NGUYN NGC BNH CT 43
XY DNG QUY AO CHUYN NG CUA TAY KEP
gp c vt, 2 ma cua tay kep phai kep vao canh nho cua vt. T h toa
t ln tay kep O4x4y4z4 ta co h toa oa trn vt nh hinh ve sau:
zv
xv
yv
Quy ao di chyn cua vt trong mt phng O0x0y0 nh sau:
Hnh 2.2
Trong qua trinh vt di chuyn, hng cua vt thay i theo hinh 2.3
NGUYN NGC BNH CT 43
Hnh 2.3
xv
Ov
z0
yv
y0
zv
O0
x0
Hnh 2.4
H ta ca vt trong khng gian th hin trn hnh 2.4

Ta chia qu o AB thnh 100 Ai (i=0100; A0=A; A100 = B ) sao cho
A O A = 1,80. Khi ta c : = -900 + 1,80.i
i v i +1
* Ta ca vt trong mt phng O0x0y0 l: xv = xA + R.cos

yv = yA + R.sin
* Ma trn cosin chi hng ca vt so vi h ta c nh O0x0y0z0.
Ta nhn thy, quay h quy chiu O0 sang h quy chiu Ov ta cn thc
hin 2 chuyn ng : 1/ Quay quanh trc z0 theo chiu dng mt gc
NGUYN NGC BNH CT 43

2/ Quay quanh trc x0 theo chiu dng mt gc 1800
Nh vy, ma trn cosin ch hng ca vt l
cos
A = sin
0
sin 0 1 0 0 cos sin

cos 0 0 1 0 = sin cos
0
1 0 0 1 0
0
0
0
1
Nh vy, ta c ma trn bin i thun nht ca vt l :

cos
sin
sin
cos
0
0
0 x A + R cos
0 y A + R sin
1
zv
0
1
= -900 + 1,80.i
Thay
r11=cos ;
r12 = sin ;
xv = xA + R.cos;
xv = xA + R.cos; vo (2.8) ta c:
= 900
+ 1,80 .i ( i=0 100)
( x A + R .c os 2) +( y A +R .sin 2)
C2 =
2.L1 .L2
L2
L2
2 = atan2( 1 C 2 2 C
, 2)
1 = = atan2( y A
R+ .sin ,x A R c+. os
)
4 = 1 + 2 atan2( sin c, os )
d3 = H
z v +d4 )
(2.9)
atan2
( L S2. 2,L
L )C2. + 2
NGUYN NGC BNH CT 43

CHNG TRNH VIT TRN MATLAB
syms q1 q2 q3 q4;
syms a1 a2 a3 a4;
syms d1 d2 d3 d4;
syms alpha teta a d;
% thong so cua cac khau dong hoc:
Ho=1270; L1=600; L2=700; L4=600;
% cac thong so D-H:
q1
;
q2
;
0
d1=Ho
;
d2=0
;
d3
a1=L1
;
a2=L2
;
a3=0
alpha1=0 ; alpha2=pi ; alpha3=0
;
;
;
;
q4
d4=L4
a4=0
alpha4=0
;
;
;
;
%Khai bao ma tran D-H dang tong quat:

H=[cos(teta) -sin(teta)*cos(alpha) sin(teta)*sin(alpha) a*cos(teta)
sin(teta) cos(teta)*cos(alpha)
-cos(teta)*sin(alpha) a*sin(teta)
0
sin(alpha)
cos(alpha)
d
0
0
%--xac dinh cac ma tran D-H cua robot----
teta=q1; d=d1; a=a1; alpha=alpha1;

H1=subs(H);
H2=subs(H);
teta=0; d=d3; a=a3; alpha=alpha3;
H3=subs(H);
H4=subs(H);
D21=H1*H2;
D31=H1*H2*H3;
D41=H1*H2*H3*H4
%nhap cac thong so:
t=100;% so diem chia
kq=fopen('kq.txt','w');
kq1=fopen('q1.tab','w');
kd3=fopen('d3.tab','w');
fprintf(kq,'
q1(rad)
q2(rad)
d3(mm)
xE(mm)
yE(mm)
zE(mm)\n');
R=input('nhap gia tri ban kinh
: R=');
a=input('nhap gia tri hoanh do tam
: a=');
b=input('nhap gia tri tung do tam
: b=');
z0=input('nhap gia tri do Ho ban dau: z0=');
R=R;
a=a;
b=b;
z0=z0;
%---vec to Ox,Oy,Oz cua he truc toa do co dinh-Ox=[1;0;0];
Oy=[0;1;0];
Oz=[0;0;1];
%--- HA TAY MAY
q4(rad)
];
NGUYN NGC BNH CT 43

for i=0:1:10;
%---toa do diem E--phi=-pi/2;
xE=a+R*cos(phi);
yE=b+R*sin(phi);
zE=z0-i*(z0/10);
%---------ma tran co sin chi huong tai diem E--------A=[cos(phi) sin(phi) 0; sin(phi) -cos(phi) 0; 0 0 -1];
C2=(xE*xE+yE*yE-L1*L1-L2*L2)/(2*L1*L2);
q2=atan2(sqrt(1-C2*C2),C2);
q1=atan2(b+R*sin(phi), a+R*cos(phi))-atan2(L2*sin(q2),L1+L2*cos(q2));
q4=q1+q2-atan2(sin(phi),cos(phi));
d3=Ho-zE-d4;
q1=q1*180/pi;
q2=q2*180/pi;
q4=q4*180/pi;
d3=d3;
t=i
q_1=[t q1];
q_2=[t q2];
d_3=[t d3];
q_4=[t q4];
fprintf(kq1,'%12.4f %12.4f
\n',q_1);
\n',q_2);
fprintf(kd3,'%12.4f %12.4f
\n',d_3);
\n',q_4);
q=[q1 q2 d3 q4 xE yE zE];
fprintf(kq,'%12.4f %12.4f %12.4f
%12.4f
\n',q);
end;
%12.4f
%12.4f
%12.4f
%---NANG VAT LEN

for i=0:1:10;
%---toa do diem E--phi=-pi/2;
xE=a+R*cos(phi);
yE=b+R*sin(phi);
zE=i*(z0/10);
d3=Ho-zE-d4;
q1=q1*180/pi;
q2=q2*180/pi;
q4=q4*180/pi;
d3=d3;
t=i+11
q_1=[t q1];
q_2=[t q2];
NGUYN NGC BNH CT 43

d_3=[t d3];
q_4=[t q4];
\n',q_1);
\n',q_2);
\n',d_3);
\n',q_4);
fprintf(kq,'%12.4f %12.4f %12.4f
%12.4f
\n',q);
end;
%12.4f
%12.4f
%12.4f
%--- DI CHUYEN THEO QUY DAO

for i=0:1:100;
%---toa do diem E--phi=-pi/2+ i*0.01*pi;
xE=a+R*cos(phi);
yE=b+R*sin(phi);
zE=z0;
d3=Ho-z0-d4;
q1=q1*180/pi;
q2=q2*180/pi;
q4=q4*180/pi;
d3=d3;
t=i+22
q_1=[t q1];
q_2=[t q2];
d_3=[t d3];
q_4=[t q4];
\n',q_1);
\n',q_2);
\n',d_3);
\n',q_4);
fprintf(kq,'%12.4f %12.4f %12.4f
%12.4f
%12.4f
%12.4f
\n',q);
end;
%--- HA VAT XUONG
for i=0:1:10;
%---toa do diem E--phi=pi/2;
xE=a+R*cos(phi);
yE=b+R*sin(phi);
zE=z0-i*(z0/10);
%12.4f
NGUYN NGC BNH CT 43

d3=Ho-zE-d4;
q1=q1*180/pi;
q2=q2*180/pi;
q4=q4*180/pi;
d3=d3;
t=i+123
q_1=[t q1];
q_2=[t q2];
d_3=[t d3];
q_4=[t q4];
\n',q_1);
\n',q_2);
\n',d_3);
\n',q_4);
fprintf(kq,'%12.4f %12.4f %12.4f
%12.4f
%12.4f
%12.4f
\n',q);
end;
%---NANG TAY MAY LEN
for i=0:1:10;
%---toa do diem E--phi=pi/2;
xE=a+R*cos(phi);
yE=b+R*sin(phi);
zE=i*(z0/10);
%12.4f
d3=Ho-zE-d4;
q1=q1*180/pi;
q2=q2*180/pi;
q4=q4*180/pi;
d3=d3;
t=i+134
q_1=[t q1];
q_2=[t q2];
d_3=[t d3];
q_4=[t q4];
\n',q_1);
\n',q_2);
\n',d_3);
\n',q_4);
fprintf(kq,'%12.4f %12.4f %12.4f
%12.4f
\n',q);
end;
%12.4f
%12.4f
%12.4f
NGUYN NGC BNH CT 43

fclose(kq);

Dynamic

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Dynamic

Uploaded by

Copyright:

Available Formats

NGUYN NGC BNH CT 43

NGUYN NGC BNH CT 43

T bng tham s D H, ta c cc ma trn:

yE(0) = L1S1 + L2S 12

NGUYN NGC BNH CT 43

Ta c: DE = D4, suy ra:

So snh cc phn t tng ng ca cc ma trn 2 v, ta c h phng

Nu cho trc cc gi tr bin khp thay i theo thi gian, th v tr v hng ca

NGUYN NGC BNH CT 43

Trong : A l ma trn cosin ch hng ca vt : A = r21

O = [xv yv zv]T l ta ca khu cng tc

Khi ta c: 1 + 2 - 4 = = atan2( r12 , r11)

Chiu v tr ca tay my ln mt phng Ox0y0 ( nh hnh 2.1) ta c :

xv2 + yv2 L12 L22

+) 1 = = atan2( yv , xv ) atan2( L2.S2 , L1 + L2.C2 )

NGUYN NGC BNH CT 43

T cc phng trnh (2.4) ,(2.5),(2.6),(2.7) ta c cc b nghim :

xv2 + yv2 L12 L22

atan2 ( L2 .S2, L1 + L2 .C2

4 = 1 + 2 atan2( r12 , r11 )

NGUYN NGC BNH CT 43

XY DNG QUY AO CHUYN NG CUA TAY KEP

Quy ao di chyn cua vt trong mt phng O0x0y0 nh sau:

NGUYN NGC BNH CT 43

H ta ca vt trong khng gian th hin trn hnh 2.4

* Ta ca vt trong mt phng O0x0y0 l: xv = xA + R.cos

NGUYN NGC BNH CT 43

sin 0 1 0 0 cos sin

Nh vy, ta c ma trn bin i thun nht ca vt l :

+ 1,80 .i ( i=0 100)

NGUYN NGC BNH CT 43

%Khai bao ma tran D-H dang tong quat:

teta=q1; d=d1; a=a1; alpha=alpha1;

NGUYN NGC BNH CT 43

%---NANG VAT LEN

NGUYN NGC BNH CT 43

%--- DI CHUYEN THEO QUY DAO

NGUYN NGC BNH CT 43

NGUYN NGC BNH CT 43

You might also like