PHẠM VĂN KHIỀN

8/6/2019 PHM VN KHIN

1/36

Robot scara serpent

MC LC

M U.......................................................................................................................1......................................................................................................................................2

I. M T I TNG - ROBOT SCARA SERPENT..........................................21.1. Mt s loi robot Scara ca cc hng sn xut ...................................... 21.2. Cc thng s v vng lm vic ca robot Scara Serpent.......................5

1.2.1. Cu to tay my robot Scara Serpent...............................................61.2.1.1. Cu hnh ca robot Scara Serpent.............................................61.2.1.2. Cc thng s k thut ca robot Scara Serpent........................6

1.2.2. Gii hn khng gian lm vic ca robot Scara Serpent..................71.3. ng hc robot Scara Serpent.................................................................9

1.3.1. ng hc thun...................................................................................91.3.2. ng hc ngc................................................................................13ng hc ngc: xc nh cc bin khp khi bit v tr tay...................13

1.4. ng lc hc robot Scara Serpent.........................................................141.4.1. Hm Euler - Lagrange v cc vn ng lc hc.......................161.4.2. ng lc hc robot Scara Serpent..................................................16

1.4.2.1. Tnh ton ng nng v th nng cho tng khp...................171.4.2.2. Phng trnh ng lc hc........................................................21

1.5. M t i tng bng h phng trnh trng thi...............................252.2. La chn phng php iu khin v b iu khin PID cho taymy robot.....................................................................................................29

III. M PHNG VI M HNH ROBOT SCARA SERPENT..............323.1. t vn ................................................................................................32

..............................................................................................................36.....................................................................................................................36

TI LIU THAM KHO.......................................................36

M U

Theo qu trnh pht trin ca x hi, nhu cu nng cao sn xut v cht lng

sn phm ngy cng i hi ng dng rng ri cc phng tin t ng ha sn xut.

Xu hng to ra nhng dy chuyn v thit b t ng c tnh linh hot cao hnh

thnh v pht trin mnh mV th ngy cng tng nhanh nhu cu ng dng ngi

my to ra cc h sn xut t ng linh hot.Sinh vin: Phm Vn Khin

1


2/36

Robot scara serpent

Robot ng dng rng ri v ng vai tr quan trng trong sn xut cng nh

trong i sng. Robot l c cu a chc nng c kh nng lp trnh c dng di

chuyn nguyn vt liu, cc chi tit, cc dng c thng qua cc truyn ng c lp

trnh trc. Khoa hc robot ch yu da vo cc php ton v i s ma trn.

- Robot c th thao tc nh con ngi v c th hp tc vi nhau mt cch thng

minh.

- Robot c cnh tay vi nhiu bc t do v c th thc hin c cc chuyn

ng nh tay ngi v iu khin c bng my tnh hoc c th iu khin

bng chng trnh c np sn trong chip trn bo mch iu khin robot.

h iu khin robot c tin cy, chnh xc cao, gi thnh h v tit

kim nng lng th nhim v c bn l h iu khin robot phi m bo gi tr yu

cu ca cc i lng iu chnh v iu khin. Ngoi ra, h iu khin robot phi

m bo n nh ng v tnh, chng c nhiu trong v ngoi, ng thi khng gy

tc hi cho mi trng nh: ting n qu mc quy nh, sng hi ca in p v dng

in qu ln cho li in v.v...

I. M T I TNG - ROBOT SCARA SERPENT

1.1. Mt s loi robot Scara ca cc hng sn xut

Robot Scara l mt trong nhng robot ph bin nht trong cng nghip. Chuyn

ng ca robot ny rt n gin nhng li ph hp vi cc dy chuyn v ng dng

Sinh vin: Phm Vn Khin

2


3/36

Robot scara serpent

hu hiu trong nhim v nht v t sn phm. Robot Scara (Selectively Compliant

Articulated Robot Arm) c ngha l tay my lp rp chn lc.

Cu trc ng hc loi tay my ny thuc h phng sinh, c cc trc quay, cc

khp u l thng ng. N c cu to hai khp cnh tay, mt khp c tay v mt

khp tnh tin. Cc khp quay hot ng nh ng c in c phn hi v tr. Khp

tnh tin hot ng nh xi-lanh kh nn, trc vt hoc thanh rng.

Mt s loi robot Scara ca cc hng sn xut:

Hnh 1.1a: Turbo Scara SR60 ca hng Bosch. Hnh 1.1b: Assembly Scara Robot ca Hirata.

Hnh 1.1c: Scara Robot ca EPSON. Hnh 1.1d: Scara Robot ca DENSO.


3
http://www.directindustry.com/prod/denso-robotics/http://www.directindustry.com/prod/epson-factory-automation/http://www.directindustry.com/prod/hirata/


4/36

Robot scara serpent

Hnh 1.1e: Scara Robot of ADEPT. Hnh 1.1f: Scara Robot of RANOME.

Hnh 1.1g: Scara Robot of KUKA. Hnh 1.1h: Scara Robot of STAUBLI.

Hnh 1.2: Robot Scara Serpent ca FEEDBACK.


4
http://www.directindustry.com/prod/staubli-robots/http://www.directindustry.com/prod/kuka-roboter/http://www.directindustry.com/prod/janome-sewing-machine/http://www.directindustry.com/prod/adept-technology/


5/36

Robot scara serpent

Trong phn ny ti s xy dng m t ton hc i tng robot Scara Serpent

ca FEEDBACK.

1.2. Cc thng s v vng lm vic ca robot Scara Serpent

Do chuyn ng ca robot Scara n gin, d dng nn n c s dng kh

ph bin trong cng nghip. y nghin cu robot Scara Serpent (Hnh 1.3) mt

loi c bn trong nhm robot cng nghip ny.

Hnh 1.3: Robot Scara Serpent.

Chiu cao ca Robot c th thay i d dng bng cch thay i v tr g thn

robot trn trc c bn, gip tay my thun li trong vic thay i cng vic.

Vi thit k ng c truyn ng cho c tay c t trn trc c bn v lin h

vi c tay bng ai truyn, nn n m bo c gc quay ca c tay khng thay i

trong qu trnh tay my chuyn ng.

Truyn ng cho 2 khp ca tay my v c tay bng ng c servo mt chiu

c phn hi v tr to thnh mt vng iu khin kn. Chuyn ng thng ng c

thc hin bng piton kh nn.

Robot Scara Serpent c th c lp trnh t my tnh bng cch t d liu cho

mi trc. Hoc iu khin bng tay s dng thit b li in (steering) cho tay my

dng cc cun dy in t trong gi treo (pendant).


5


6/36

Robot scara serpent

1.2.1. Cu to tay my robot Scara Serpent

1.2.1.1.Cu hnh ca robot Scara SerpentBao gm mt chui cc thanh cng c lin kt vi nhau bi cc khp:

Hnh 1.4:Cu hnh v cc h trc ta gn trn Robot Scara Serpent.

Robot Scara Serpent gm 3 khp chuyn ng quay v mt khp chuyn ngtnh tin. Gn cho mi thanh ni mt h trc to , ta c:

- Khp 1 quay quanh trc z0 gc 1.

- Khp 2 quay quanh trc z1 gc 2.

- Khp 3 chuyn ng tnh tin theo trc z2 on d3.

- Khp 4 quay quanh trc z3 gc 4.1.2.1.2.Cc thng s k thut ca robot Scara Serpent

Thng s ca ng c 1, 2, 3 tng ng vi cc khp 1, 2, 4 ca tay my robot

Scara Serpent (xem bng 1.1).

- ng c 1 truyn ng cho khp 1 (main).

- ng c 2 truyn ng cho khp 2 (fore).


6


7/36

Robot scara serpent

- ng c 3 truyn ng cho khp 4 - khp c tay (wrist).

Bng 1.1:Thng s cc ng c ca robot.

TT Loi U(V) I(A) M(Nm) N(v/p) P(W) J(Kg.m2) R()

L(H)

m(Kg)

1 J9ZF 12 4,8 4.10-2 2100 15 0,32.104 1,38 100 0,6

2 J9ZF 12 4,8 4.10-2 2100 15 0,32.104 1,38 100 0,6

3 J12ZF 12 4,8 1,2102 2100 26 1,5.10-4 0,95 100 1

Bng 1.2: Cc thng s ng hc ca robot Scara Serpent.

TT Thng s Kch thc ng hc

1 M1 = 4 Kg Khi lng thanh ni 1.

2 M2 = 1.5 Kg Khi lng thanh ni 2.

3 M3 = 2 Kg Khi lng thanh ni 3.

4 M4 = 0.6 Kg Khi lng thanh ni 4.

5 a1 = 0.25 m Chiu di thanh ni gia 2 khp main v fore .

6 a2 = 0.15 m Chiu di thanh ni gia 2 khp fore v c tay .

7 D3 Chiu di thanh ni d3 ph thuc vo ch lm vic ca tay my.

1.2.2. Gii hn khng gian lm vic ca robot Scara Serpent

Cc bin khp c cc gii hn gc quay nh sau :

1 = -96 0 960 ( so vi trc Ox ).

2 = -115 0 1150 (so vi trc thanh 1).

Chuyn ng quay ca khp th nht c hnh chiu bng trong h trc to

OX0Y0 v OX1Y1 (Hnh 1.5) tng ng vi gc quay tng trong thc t l 1920.

Chuyn ng quay ca khp th hai c hnh chiu bng trong h trc to OX1Y1 v

OX2Y2 tng ng vi gc quay tng l 2300. T ta c th thy c hnh chiu gii

hn khng gian lm vic ca n (Hnh 1.6).


7


8/36

Robot scara serpent

Hnh 1.5:Gii hn gc quay ca 2 khp.

Hnh 1.6:Gii hn khng gian lm vic ca robot Scara Serpent.


8


9/36

Robot scara serpent

Nh vy khong khng gian m tay my c th vi ti l ton b hnh tr vi

y c ng gii hn bn trong l mt cung trn c bn knh r = 0.231 (m) v

ng gii hn bn ngoi l ng trn bn knh R = 0.4 (m).

1.3. ng hc robot Scara Serpent

Robot Scara Serpent c cu trc ng hc c biu din nh trn Hnh1.4.

Robot c 3 trc quay v 1 bn kp, tuy nhin ba khp ng u tin c gi l b

phn c bn v trc ht, nh chng tay my c th thc hin bc ch yu trong thao

tc nh v, tc l a bn kp n ln cn im lm vic, sau nh khp ng cn

li bn kp c nh hng v vi chnh n v tr gia cng chnh xc.

1.3.1. ng hc thun

Vic xy dng cc phng trnh ng hc thun ca robot c tin hnh tun

t theo cc bc sau:

Bc 1: Xc nh cc h to

Ta s dng quy c Denavit-Hartenberg m t y v tr ca ca ton

thn robot cng nghip.Hnh 1.4m t cc h trc to gn vi cc khc tay ca

robot Scara Serpent.

Bc 2: Xy dng bng thng s DH

Bng 1.3:Tham s Denavit Hartenberg ca robot Scara Serpent.

Thanh ni i (0) ai i(rad) di (m) Bin Chuyn ng

1 0 a1 1 0 1 Quay2 -1800 a2 2 0 2 Quay

3 0 0 0 d3 d3 Tnh tin

4 0 0 4 0 4 Quay


9


10/36

Robot scara serpent

Kho st vi 3 trc khp quay u tin tng ng vi qu o ca khp quay 4

trong mt phng OX0Y0. Ma trn T4 l ma trn biu din tay my robot trong h trc

ta gc: T4= A1.A2.A3.A4

An=

n n n n n n n

n n n n n n n

n n n

cos sin cos sin sin a cos

sin cos cos cos sin a sin

0 sin cos d

0 0 0 1

(1.1)

Thay s liu trong bng tham s c:

1 1 1 1

1 1 1 1

1

cos sin 0 a .cos

sin cos 0 a sinA

0 0 1 0

0 0 0 1

=

2 2 2 2

2 2 2 2

2

cos sin 0 a .cos

sin cos 0 a sinA

0 0 1 0

0 0 0 1

=

3

3

1 0 0 0

0 1 0 0A

0 0 1 d

0 0 0 1

=

4 4

4 4

4

cos sin 0 0sin cos 0 0

A0 0 1 0

0 0 0 1

=

K hiu: S1Sin 1 ; C1Cos 1

S2Sin 2 ; C2Cos 2

S4Sin 4 ; C4Cos 4


10


11/36

Robot scara serpent

S12Sin( 1+ 2); C12Cos( 1+ 2)

Cc bc tnh ton:

Bc 1: 3 4 4T A=

4 4

4 43

4 4

C S 0 0

S C 0 0T A

0 0 1 0

0 0 0 1

= =

Bc 2: 2 34 3 4T A . T=

4 4 4 4

4 4 4 42

4

3 3

1 0 0 0 C S 0 0 C S 0 0

0 1 0 0 S C 0 0 S C 0 0T

0 0 1 d 0 0 1 0 0 0 1 d

0 0 0 1 0 0 0 1 0 0 0 1

= =

Bc 3: 1 24 2T A . T=

2 2 2 2 4 4

2 2 2 2 4 41

4

3

2 4 2 4 2 4 2 4 2 2

2 4 2 4 2 4 2 4 2 21

4

3

C S 0 a .C C S 0 0

S C 0 a .S S C 0 0T

0 0 1 0 0 0 1 d0 0 0 1 0 0 0 1

C C S S C S S C 0 a .C

S C C S S S C C 0 a .ST

0 0 1 d

0 0 0 1

=

+ + =

Bc 4: 41

14

0

4 T.ATT ==

1 1 1 1 2 4 2 4 2 4 2 4 2 2

1 1 1 1 2 4 2 4 2 4 2 4 2 20

4

3

C -S 0 a .C C C S S S C C S 0 a .C

S C 0 a .S S C C S (C C S S ) 0 a .ST

0 0 1 0 0 0 1 d

0 0 0 1 0 0 0 1

+ + =


11


12/36

Robot scara serpent

C (C C S S ) S (S C C S ) C (S C C S ) S (C C S S ) 0 a C1 2 4 2 4 1 2 4 2 4 1 2 4 2 4 1 2 4 2 4 2 12

S (C C S S ) C (S C C S ) S (S C C S ) C (C C S S ) 0 a S1 2 4 2 4 1 2 4 2 4 1 2 4 2 4 1 2 4 2 4 2 12

0 0 1 d3

0 0 0 1

+ + +

+ + +

(1.2)

Ma trn 0T4 biu din tay my robot trong h to gc.

Mt khc theo k hiu tng qut:

x x x x

y y y y

E 4

z z z z

n o a p

n o a pT T

n o a p0 0 0 1

= = (1.3)

Vi: paon ,,, ln lt l cc vect nh v, vect nh hng, vect ti v

vect v tr biu din hng v v tr ca tay my trong khng gian lm vic.

T ma trn trn ta c h phng trnh ng hc thun tay my robot:

nx = C1(C2 C4 + S2S4) S1(S2C4 C2S4) (1.4)

ny = S1(C2 C4 + S2S4) + C1(S2C4 C2S4) (1.5)

nz = 0 (1.6)

ox = C1(S2C4- C2 S4) + S1(C2C4 + S2S4) (1.7)

oy = S1(S2C4 - C2 S4) - C1(C2C4 + S2S4) (1.8)

oz = 0 (1.9)

ax = 0 (1.10)

ay = 0 (1.11)

az = -1 (1.12)

V h phng trnh xc nh v tr ca im tc ng cui nh sau:

x = px = a1.C1 + a2.C12 (1.13)

y = py = a1.S1 + a2.S12 (1.14)

z = pz = - d3 (1.15)


12


13/36

Robot scara serpent

1.3.2. ng hc ngc

ng hc ngc: xc nh cc bin khp khi bit v tr tay.

T phng trnh ng hc thun c:

2 2 2 2 2

x 1 1 2 12 1 2 1 1

2 2 2 2 2

y 1 1 2 12 1 2 1 12

p a .C a .C 2.a .a .C .C

p a .S a .S 2.a .a .S .S

= + +

= + +

2 2 2 2 2 2 2 2

X Y 1 1 1 2 12 12 1 2 1 12 1 12 p p a .(S C ) a .(S C ) 2.a .a .(S .S C .C )+ = + + + + +

2 2 2 2

x y 1 2 1 2 2 p p a a 2.a .a .C + = + +

(1.16)

Do :

2 2 2 2

X Y 1 2

2

1 2

2

2 2

p p a acos

2.a .a

sin (1 cos )

+ =

=

(1.17)

T tnh c gc 2:

2= atan2(sin 2,cos 2) (1.18)

Th C1, S1 vo phng trnh (1.13) v (1.14) thu c:

(a1+a2C2).C1 a2S2.S1 = px

a2S2.C1 +( a1+a2C2).S1 = py

Gii phng trnh bc nht vi n C1, S1 v s dng (1.16) thu c :

1 2 2 x 2 2 yx

1 2 2

x y

y 1 2 2 y 2 2 x

1 2 2

x y

(a a C ).p a S .pC

p p

(a a C ).p a S .pS

p p

+ += =

+

+ = =

+

(1.19)

1= atan2(S1,C1)(1.20)

T phng trnh (1.15) ta c:

d3 = - pz (1.21)


13


14/36

Robot scara serpent

Mt khc t phng trnh (1.4) c:

nx = C1(C2 C4 + S2S4) S1(S2C4 C2S4) (1.22)

Rt gn theo cc cng thc lng gic thu c:

nX = cos 1.cos( 2- 4) sin 1.sin( 2- 4) = cos( 1+ 2- 4)(1.23)

sin( 1+ 2- 4) =2

x1 n (1.24)

21 2 4 x x

( ) atan2( 1 n ,n+ =

24 1 2 x x

atan2( 1 n ,n )= + (1.25)

Vy h phng trnh ng hc ngc ca robot Scara Serpent l:

( )

( )

1 2 2 x 2 2 y

1 2 2

x y

1 2 2 y 2 2 x

1 2 2

x y

1 1 1

2 2 2 2

X Y 1 2

2

1 2

2

2 2

2 2 2

3 z

2

4 1 2 x x

(a a C ).p a S .pC

p p

(a a C ).p a S .pS

p patan2 S ,C

p p a acos

2.a .a

sin (1 cos )

atan2 S ,C

d p

atan2( 1 n ,n )

+ += +

+ =

+ = + = =

= = = +

(1.26)

1.4. ng lc hc robot Scara Serpent

m t mi quan h gia lc, mmen vi v tr, vn tc v gia tc ca i

tng robot, cn phi xy dng c phng trnh ng lc hc, t phc v cho

cng vic thit k v iu khin robot. V vy cn phi tnh ton y cc thng s

ca i tng trc khi a vo m phng.Sinh vin: Phm Vn Khin

14


15/36

Robot scara serpent

Phng trnh ng lc hc ca robot c biu din nh sau:

H(q)q h(q,q) g(q) (t)+ + = && & (1.27)

Trong :

- (t) - Vect [n x 1] lc ng to nn n khp ng:

(t) = [ 1(t), 2(t), ..., n(t)]T (1.28)

- q(t) - Vect [n x 1] bin khp:

q(t) = [q1(t), q2(t), ... qn(t)]T. (1.29)

- H(q) - Ma trn [n x n], c cc phn t H ik sau y:

( )n

T

jk ij i ik i max(j,k)

H Trace U I U=

= . (j, k =1, 2, ...n). (1.30a)

0

i

ij

j

TU

q

=

(1.30b)

- h(q , q ) - Vect [n x 1] lc ly tm v Coriolit:

h(q , q ) = [h1, h2, ... , hn]T. (1.31)n n

j jkm k mk 1 m 1

h h q q= =

= & & . (j =1, 2, ...n). (1.32)

( )= Tijiikmjkm UIUTraceh . (1.33)

mk

i

02

ikmqq

TU

= (1.34)

- g(q) - Vect [n x 1] lc trng trng:

g(q) = [g1, g2, ... , gn]T. (1.35)

( )n

i

j j iji j

g m gU r =

= . (1.36)

Vi robot Scara Serpent gm 4 chuyn ng (3 chuyn ng quay v 1 chuyn

ng tnh tin) v m phng vi 3 chuyn ng quay c cc phng trnh c th sau:

Vect [4 1] lc ng: (t) = [ 1(t), 2(t), 3(t), 4(t)]T.


15


16/36

Robot scara serpent

Vect [4 1] bin khp: q(t) = [q1(t), q2(t), d3(t), q4(t)]T.

1.4.1. Hm Euler - Lagrange v cc vn ng lc hc

Lagrange nh ngha s khc bit gia ng nng v th nng ca h thng:

L = K P (1.37)

Trong : K : l ng nng ca h thng.

P : l tng th nng ca h thng.

Do phng trnh ng lc hc c xc nh bng biu thc:

i

i i

d L L

dt q q

=

&

(1.38)

Trong : qi = i i vi khp quay; qi = ri i vi khp tnh tin.

i : mmen lc ng ti khp th i.

ng nng ca khp th i:

2 2i i i i i1 1K m .v J .

2 2= +

(1.39)

Vi Ji l mmen qun tnh ca khp th i.

1.4.2. ng lc hc robot Scara Serpent

Theo cu hnh, robot Scara Serpent c cc thng s Bng 1.4:

Bng 1.4: Thng s ca robot Scara Serpent.

Khp 1 Khp 2 Khp 3 Khp 4

Bin 1 2 d3 4

Chiu di a1 a2 d3 0

Khi lng m1 m2 m3 m4

Vn tc v1 v2 v3 v4


16


17/36

Robot scara serpent

Chiu di tm khi lg1 lg2 lg3 lg4

Gi s khi lng nm u mt cc thanh ni, ta c chiu di tm khi chnh

l chiu di ca thanh ni: lg1 = a1 lg2 = a2 lg3 = d3

K hiu chiu di cc khp: a1 = l1 a2 = l2 d3 = l3

H qui chiu gn vi trc to (O0x0y0z0) trn khp th nht. Khi mt

phng (O0x0y0) l mt phng ng th.

1.4.2.1. Tnh ton ng nng v th nng cho tng khp

Khp 1:

1 g1 1

1 g1 1

1

x l .cos

y l .sin

z 0

=

= =

.

1 g1 1 1

.

1 g1 1 1

.

1

x l .sin .

y l .cos .

z 0

=

=

=

&

&

Trong : x1, y1, z1 l hnh chiu ca thanh ni s 1 ln cc trc x, y, z.

2 2 2

1 1 1 1

2 2

1 g1 1 1 g1 1 1

2 2 211 g1

v x y z

v ( l .sin . ) (l .cos . )

v l .

= + +

= +

=

& & &

& &

&

Trong : J1 momen qun tnh khp 1.

1

2 2

1 1 1 1 1

. .2 2 2

1 1 g1 1 1 1

P 0

1 1K m .v J .

2 2

1 1K m .l . J .

2 2

=

= +

= +

(1.40)


17


18/36

Robot scara serpent

Khp 2:

=++=

++=

0z). s i n (l.s inly

). c o s (l. c o slx

2

21g 2112

21g 2112

=

+++=

++=

0z

)).(.cos(l..cosly

)).(.s in(l..sinlx

2

2121g 21112

2121g 21112

[ ] [ ]

221

2

1g21

2

21

2

g2

2

1

2

1

2

2

2

2121g2111

2

2121g2111

2

2

2

2

2

2

2

22

)cos..(.l2.l)(.l.lv

)).(.cos(l..cosl)).(.sin(l..sinlv

zyxv

++++=

++++++=

++=

2 2

2 2 2 2 2

. .2 2 2 2 2 2

2 2 1 1 g2 1 2 1 g2 1 1 2 2 2 2

2

1 1K m .v J .

2 2

1 1K m . l . l .( ) 2.l .l .( . ).cos J .

2 2

P 0

= +

= + + + + +

=

& & & & &

(1.41)

Khp 3:

=

++=

++=

33

212113

212113

- lz

).s in (l. s inly



18


19/36

Robot scara serpent

. . . .

3 1 1 1 2 1 2 1 2

. . . .

3 1 1 1 2 1 2 1 2

. .

3 3

x l .sin . l .sin( ).( )

y l .cos . l .cos( ).( )

z l

= + +

= + + +

=

.2

3

.

2

.

1

.2

121

2.

2

.

1

2

2

.2

1

2

1

.2

3

.2

3

2.

2

.

1212

.

111

2.

2

.

1212

.

111

2

3

.2

3

.2

3

.2

33

l)..(.l2.l)(.l.lv

l)).(.cos(l..cosl)).(.sin(l..sinlv

zyxv

+++++=

+

++++

++=

++=

.2

3 3 3

. . .. . . .2 2 2 2 2 2

3 3 1 1 2 1 2 1 2 1 1 2 3

3 3 3 3 3 3

1K m .v

21

K m . l . l .( ) 2.l .l .( . ) l2

P m .g.h P m .g.l

=

= + + + + + = = =

(1.42)

Khp 4:


19


20/36

Robot scara serpent

=

++=

++=

34

212114

212114

- lz

). s i n (l. s i nly


=

+++=

++=

3

.

4

.

2

.

1212

.

111

.

4

.

2

.

1212

.

111

.

4

lz

)).(.cos(l..cosly

)).(.s in(l..sinlx

. . .2 2 2

4 4 4 4

2 2 .. . . . . .2 2

4 1 1 1 2 1 2 1 2 1 1 1 2 1 2 1 2 3

. . . .. . . .2 2 2 2 2 2 2

4 1 1 2 1 2 1 2 1 1 2 3

v x y z

v l .sin . l .sin( ).( ) l .cos . l .cos( ).( ) l

v l . l .( ) 2.l .l .( . ) l

= + +

= + + + + + + +

= + + + + +

.2 2

4 4 4 4 4

. . .. . . . . . .2 2 2 2 2 2 2

4 4 1 1 g2 1 2 1 g2 1 1 2 2 3 4 1 2 4

4 4 4

4 4 3

1 1K m . v J .w2 2

1 1K m . l . l . ( ) 2 .l .l .( . )cos l J ( )

2 2

P m .g.h

P m .g.l

= +

= + + + + + + + +

=

= ( )2.43Tng ng nng ca h thng:

K = K1 + K2 + K3 + K4

(1.44)

t tm khi ti u mt ca thanh ni, ta c: lg1= l1; lg2= l2; lg3= l3

.2 2 2

1 2 3 4 1 2 3 4 2 1 2 4 1

2 2

2 3 4 1 2 2 1 2 3 4 2 2 4 1 2

3 2

2 3 4 1 2 2 1 2 2 3 4 2 2 4 2

3 4

1K (m m m m ).l (m m m ).l J J J . ...

2

(m m m ).l .l .cos . (m m m )l J J . . ...

1

(m m m ).l .l .cos . . (m m m ).l J J . ...2

1(m m ).

2

= + + + + + + + + + +

+ + + + + + + + +

+ + + + + + + + +

+ +

& & &

& & &

2 2

3 4 4 4 1 4 4 2 4

1l .J . J . . J . .

2+ + +& & & & & &

(1.45)

Tng th nng ca h thng:

P = P1+ P2+ P3+ P4 = -(m3+ m4).g.l3 (1.46)


20


21/36

Robot scara serpent

1.4.2.2. Phng trnh ng lc hc

Lagrange ca h thng:

2 2 2

1 2 3 4 1 2 3 4 2 1 2 4 1

2

2 3 4 1 2 2 1

2

2 3 4 2 2 4 1 2

2 3 4 1 2 2 1 2

2 2

2 3 4 2 2 4 2

L K P

1L [(m m m m ).l (m m m ).l J J J ]. ...

2

(m m m ).l .l .Cos . ...

[(m m m ).l J J ]. . ...

(m m m )l .l .Cos . . ...

1[(m m m ).l J J ]. ...

2

1(

2

=

= + + + + + + + + +

+ + + +

+ + + + + +

+ + + +

+ + + + + +

+

&

&

& &

& &

&

2 2

3 4 3 4 4 4 2 4 4 1 4 3 4 3

1m m )l J . J . . J . . (m m ).g.l

2+ + + + + +& & & & & &

(1.47)

Cc phn t ca phng trnh ng lc hc:

i

i i

d L L

dt q q

=

&

(1.48)

Khp 1:

1

1 1

d L L

dt

=

&

(1.49)

a..

2 2

1 2 3 4 1 2 3 4 2 1 2 4 1.

1

..

2

2 3 4 1 2 2 1 2 3 4 2 2 4 2

. .

2 3 4 1 2 2 2 4 4

L(m m m m ).l (m m m ).l J J J . ...

2.(m m m ).l .l .cos . (m m m )l J J . ...

(m m m ).l .l .cos .. J .

= + + + + + + + + + +

+ + + + + + + + +

+ + + +


21


22/36

Robot scara serpent

b...

2 2

1 2 3 4 1 2 3 4 2 1 2 4 1.

1

.. . .

2 3 4 1 2 2 1 2 3 4 1 2 2 1 2

.. ..2

2 3 4 2 2 4 2 2 3 4 1 2 2 2

2 3

d L(m m m m ).l (m m m ).l J J J . ...

dt

2.(m m m ).l .l .cos . 2.(m m m ).l .l .sin . . .. .

(m m m )l J J . (m m m ).l .l .cos . ...

(m m

= + + + + + + + + + +

+ + + + + +

+ + + + + + + + +

+ +. ..2

4 1 2 2 2 4 4m ).l .l .sin . J .+

(1.50)

c.

0

L

1

=

(1.51)

Khp 2:

2 .

22

d L L

dt

=

(1.52)

a.2

2 3 4 2 2 4 1 2 3 4 1 2 2

2

2

2 3 4 2 2 4 2 4 4

L[(m m m )l J J ]. (m m m )l .l .Cos .

[(m m m )l J J ]. J .

= + + + + + + +

+ + + + + +

& &&

& &

b.

2

2 3 4 2 2 4 1 2 3 4 1 2 2 1

2

2 3 4 1 2 2 1 2

2

2 3 4 2 2 4 2 4 4

d L[(m m m )l J J ]. (m m m )l .l .Cos .

dt

[(m m m )l .l .Sin . .

[(m m m )l J J ]. J .

= + + + + + + +

+ + +

+ + + + + +

&& &&&

& &

&& &&

(1.53)

c.2

2 3 4 1 2 2 1 2 3 4 1 2 2 1

2

L[(m m m )l .l ( Sin ). (m m m )l .l .Sin . .

= + + + +

& & (1.54)

Khp 3:

3

3 3

d L L

dt l l

=

&

(1.55)

a. 3433

l).m(ml

L

+=


22


23/36

Robot scara serpent

b...

343.

3

l).mm(

l

L

dt

d+=

(1.56)

c. ).gm(ml

L43

3

+=

(1.57)

Khp 4:

4 .

44

d L L

dt

=

(1.58)

a..

44.

.

24.

.

14..

4

JJJ

L++=

b.

.. .. ..

4. 1 4. 2 4. 4.

4

d LJ J J dt

= + +

(1.59)

c.4

L0

=

(1.60)

Nh vy phng trnh ng lc hc ca h thng c biu din bng cc

phng trnh sau:

2 2

1 1234 1 234 2 124 234 1 2 2 1

2

234 2 24 234 1 2 2 2 4 4

2

234 1 2 2 2 234 1 2 2 1 2

[m .l m .l J 2.m .l .l .Cos ]. ...

(m .l J m .l .l .Cos ). J . ...

.m .l .l .Sin . 2m .l .l .Sin . .

= + + + +

+ + + + +

&&

&& &&

& &&

(1.61)

2

122123444224

2

2234

122123424

2

22342

.Sin.l.l.m.J).Jl.m(

].Cos.l.l.mJl.m[

++++

+++=(1.62)

3 34 3 34 3m .l m .g F = =&& (1.63)

4 4 1 4 2 4 4J J J = + +&& && && (1.64)

Trong :

m1234 = m1 + m2 + m3 + m4.


23


24/36

Robot scara serpent

m234 = m2 + m3 + m4 .

(1.65)

m34 = m3 + m4 ;Vi m4 = m40 + mt

m40 : khi lng ca khp 4

mt : khi lng ca ti c ni vi khp 4.

J124 = J1 + J2 + J4 .

J24 = J2 + J4 ; Vi J4 = J40 + Jt

J40 : m men qun tnh ca khp 4.

Jt : m men qun tnh ca ti c ni vi khp 4.

Vi 1 , 2 , 4 ln lt l mmen ng ti cc khp quay 1, 2 v 4.

3 = F3 l lc ng t ln khp tnh tin 3.

Nu ch xt mmen ng vi 3 khp quay th c th vit gn li ba phng

trnh ng lc hc (1.61), (1.62), (1.64) tin cho qu trnh tnh ton nh sau:

1 11 12 13 1 1

2 21 22 23 2 2

4 31 32 33 4

H H H hH H H . h

H H H 0

= +

&&&&

&&

hay:

2

1 11 12 13 1 2 1 2

2

2 21 22 23 2 1

4 31 32 33 4

H H H T 2T

H H H . -T

H H H 0

+ = +

&& & &&

&& &

&&

(1.66)

Cc thnh phn trong phng trnh ng lc hc c xc nh:


24


25/36

Robot scara serpent

2 2

11 1234 1 234 2 124 234 1 2 2

2

12 234 2 24 234 1 2 2

13 4

21 12

2

22 234 2 24

23 4

31 32 33 4

234 1 2 2

2

1 2 1 2

2

2 1

H m .l m .l J 2.m .l .l .Cos

H m .l J m .l .l .Cos

H J

H H

H m .l J

H J

H H H J

T m .l .l .Sin

h T 2T

h T

= + + +

= + +

=

== +

=

= = =

=

= +

=

& &&

&

(1.67)

1.5. M t i tng bng h phng trnh trng thi

t bin trng thi cho tng khp nh sau:

X(t) = [ ]TT4T2T1 XXX (1.68)

11 1

1

12 1

x qX

x q

= =

&

,21 2

2

22 2

x qX

x q

= =

&

,41 4

4

42 4

x qX

x q

= =

&

(1.69)

Tn hiu vo:

11

2 2

4 4

u

u u

u

= =

(1.70)

H phng trnh vi phn trng thi ca cc khp c vit nh sau:

Khp 1:11 12

12 1 11 1 12 2 13 4

x x

x a b u b u b u

=

= + + +

&

&(1.71)

Khp 2:21 22

22 2 21 1 22 2 23 4

x x

x a b u b u b u

=

= + + +

&

&(1.72)


25


26/36

Robot scara serpent

Khp 4:41 42

42 4 31 1 32 2 33 4

x x

x a b u b u b u

=

= + + +

&

&(1.73)

Trong :

bij(X) l cc thnh phn tng ng ca ma trn H-1.

( )11 22 33 23 32H

1 b H H H H

D= .

( )12 13 32 12 33H

1b H H H H

D= .

( )13 12 23 13 22H

1b H H H H

D

= .

( )21 23 31 21 33H

1 b H H H H

D= .

( )22 11 33 13 31H

1 b H H H H

D= .

( )23 13 21 11 23H

1 b H H H H

D= .

(1.74)

( )31 21 32 31 22H

1 b H H H HD

= .

( )32 12 31 11 32H

1 b H H H H

D= .

( )33 11 22 12 21H

1 b H H H H

D= .

H 11 22 33 21 32 13 31 23 12 11 23 32 21 12 33 31 22D det H H H H H H H H H H H H H H H H H H= = + +

ai(X) l thnh phn th i ca vect:( )( )( )

Xh

XhXh

.H

3

2

1

1

[ ]1 11 1 12 2 13 3a b h b h b h= + + [ ]3232221212 hbhbhba ++=

(1.75)


26

[ ]4 31 1 32 2 33 3a b h b h b h= + +


27/36

Robot scara serpent


27


28/36

Robot scara serpent

II. XY DNG M HNH H THNG IU KHIN

2.1. Cu trc h thng iu khin robotRobot thng t ng thc hin cc nhim v dch chuyn hoc cc thao tc.

Cc hot ng ca robot thng c lp trnh, vic thc hin cc nhim v thng

lm thay i mt phn hoc ton b v tr ca robot trong khng gian. C th coi robot

hin i nh nhng ngi my, c th thc hin cc thao tc phc tp, c chnh xc

cao.

Hnh 2.1: S cu trc chung ca h thng iu khin tay my robot.

iu khin chuyn ng ca tay my trong khng gian l xc nh n thnh phn

mmen lc tng qut tc ng ln cc khp, mmen lc tng qut c cung cp bi

c cu chp hnh. Qu trnh iu khin phi m bo b iu khin s thc hin iu

khin chuyn ng ca cc khp theo qu o q(t) sao cho q(t) lun bm q(t), vi

q(t) l vect qu o chuyn ng mong mun.

S cu trc chung ca h thng iu khin tay my cho Hnh 2.1, tay my

s bm theo qu o t trc (c lp trnh sn hoc c th a vo t chng trnh

phm mm kt ni bn ngoi) s dng b iu chnh iu khin chuyn ng v

lc, s dng cc cm bin thu nhn cc thng s v mi trng lm vic m bo

cho hot ng bn vng, kh c nhiu ngoi n, nhn bit v p ng thch nghi

c vi nhng s thay i ca mi trng cng nh s thay i cc tham s ng

hc ca ca h thng.


28

Qu ot

B iuchnh

Cm bin

Mi trngTay my

n


29/36

Robot scara serpent

2.2. La chn phng php iu khin v b iu khin PID cho tay my robot.

Phng php iu khin c la chn l phng php iu khin ng lc hc

ngc vi u vo b iu khin l sai lch gia tn hiu t v tn hiu u ra. u ra

l tn hiu iu khin uk, b iu khin PID l uPID.

Hm truyn ca b iu khin:

IC P D P DI

K 1G (s) K K s K 1 T s

s Ts

= + + = + +

B iu khin PID c s dng kh rng ri v tnh n gin ca n c v cu

trc ln nguyn l lm vic. Mun h thng c c cht lng nh mong mun th

phi phn tch i tng ri trn c s chn cc tham s KP, KI, KD cho ph hp.

Phng php Ziegler Nichols.

Ziegler Nichols l phng php xc nh h s KP, hng s thi gian tch phn

TI v hng s thi gian vi phn TD da trn c tnh qu ca h thng iu khin.

C hai phng php hiu chnh Ziegler Nichols u hng ti mc tiu t qu iu chnh khong 25%.

- Phng php Ziegler Nichols. Trng hp 1.

Xc nh thng s b iu khin PID da vo p ng bc thang n v ca h

h (nu i tng khng cha cc khu tch phn hay nghim phc lin hp th ng

qu ca i tng c dng ch S) :


29

Hnh 2.2: S b iu khin PID

PID

uke


30/36

Robot scara serpent

Hnh 2.3: p ng bc thang n v ca h h.

T1: thi gian trT2: hng s thi gian

- Phng php Ziegler Nichols. Trng hp 2.

T1 v T2 c xc nh bng cch v ng tip tuyn vi ng cong S ti

im un, ng tip tuyn ny ct trc honh ti T1 v ng y(t)=K l im c

honh T2.

Khi m hnh i tng c dng: 1T s

2

KG(s) eT s 1

=+

Bng 2.1: Thng s b PID.

Thng sB K

KP TI TD

P T2/T1K 0

PI 0.9T2/T1K T1/0.3 0

PID 1.2T2/T1K 2T1 0.5T1

Xc nh thng s b iu khin PID da vo p ng ca h kn bin gii

n nh.


30


31/36

Robot scara serpent

Hnh 2.4: p ng ca h kn bin gii n nh.

Bc 1:

t TI = , TD= 0, thay i KP t 0 ti gi tr gii hn Kgh ng vi u ra h

thng kn c dao ng bin gii n nh. Dao ng ny tng ng vi chu k gii

hn Tgh.

Bc 2: Thng s b PID c xc nh theo bng :

Bng 2.2: Thng s b PID .

Thng sB K

KP TI TD

P 0.5K gh 0

PI 0.45K gh 0.83Tgh 0

PID 0.6K gh 0.5Tgh 0.125Tgh

Vi: PII

KKT

= ; D P DK K .T=


31


32/36

Robot scara serpent

III. M PHNG VI M HNH ROBOT SCARA SERPENT3.1. t vn

Trong chng II tin hnh xy dng m hnh ca cc khu tnh ton, m

hnh robot v b iu khin PID. Sau xy dng cc chng trnh phc v m

phng v kho st cc ch lm vic ca h thng

Vic m hnh ha v nghin cu m phng h thng iu khin chuyn ng

c thc hin trn phn mm EASY-ROB

3.2. M phng robot scara trn EASY-ROB.

V robot scara v m phng robot da vo bi ton quy.

Bi ton: c 3 ci a v 3 ci cc.Cho 3 a cc bn ngoi sau dng thut ton l

th tc quy m phng.Robot scara s chuyn 3 a ca cc cc bn ngoi theo

thut ton quy cui cng tt c cc a c t vo cc gia.

Chng trnh ny c thc hin trn phn mm EASY-ROB.

Mt vi hnh v robot scara :


32


33/36

Robot scara serpent


33


34/36

Robot scara serpent


34


35/36

Robot scara serpent

KT LUN

Bo co ny nghin cu v l thuyt iu khin, t xy dng thut ton

iu khin robot theo phng php ng lc hc ngc s dng b iu khin PID

cho robot Scara Serpent vi ba khp ng. Ti kim nghim tnh ng n ca

thut ton iu khin thng qua vic xy dng cc phng trnh ng lc hc cho

robot Scara Serpent da vo thng s cho ca nh sn xut.

a ra cng thc tnh ng hc thun v ng hc ngc cho robot Scara

Serpent, xy dng m hnh ton hc cho c cu truyn ng robot, tng hp h truyn

ng. Thit k b iu khin PID theo phng php Ziegler Nichols.

Tn ti: V thi gian c hn nn ch dng kho st h thng theo phng phpiu khin ng lc hc ngc s dng iu khin PID.Cha xy dng qu ochuyn ng chun ca robot.Phn mm EASY-ROB cn thiu nhiu chc nng m phng chnh xc.

Hng pht trin: Nghin cu v l thuyt iu khin,t xy dng thutton iu khin robot theo phng php ng lc hc ngc s dng b iu khin

PID t chnh.M phng chnh xc hn, lp trnh trn phn mm Matlab-Simulink..


35


36/36

Robot scara serpent

TI LIU THAM KHO

1.GS.TSKH.Nguyn Vn Khang: ng lc hc h nhiu vt.Nh xut bn khoa hc k

thut H ni 2007

2. Phm Cng Ng: L thuyt iu khin t ng. Nh xut bn Khoa hc K thut.

H ni 1998.

3. Richard C.Dorf, Robert H.Bishop: Modern Control System, 10Editor. Person

Prentice Hall, Inc., 2005.

4. Ogata, K. (2002). Modern Control Engineering, Fourth Edition. Prentice Hall, Inc.

Dr. Young, G. Textbook & Lecture notes: MAE4053 Automatic Control Systems

5. Phn mm EASY-ROB.

Si h i Ph V Khi

Documents

PHẠM VĂN KHIỀN