You are on page 1of 36

Robot scara serpent

MC LC
M U.......................................................................................................................1
......................................................................................................................................2
I. M T I TNG - ROBOT SCARA SERPENT..........................................2
1.1. Mt s loi robot Scara ca cc hng sn xut ...................................... 2
1.2. Cc thng s v vng lm vic ca robot Scara Serpent.......................5
1.2.1. Cu to tay my robot Scara Serpent...............................................6
1.2.1.1. Cu hnh ca robot Scara Serpent.............................................6
1.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..................7
1.3. ng hc robot Scara Serpent.................................................................9
1.3.1. ng hc thun...................................................................................9
1.3.2. ng hc ngc................................................................................13
ng hc ngc: xc nh cc bin khp khi bit v tr tay...................13
1.4. ng lc hc robot Scara Serpent.........................................................14
1.4.1. Hm Euler - Lagrange v cc vn ng lc hc.......................16
1.4.2. ng lc hc robot Scara Serpent..................................................16
1.4.2.1. Tnh ton ng nng v th nng cho tng khp...................17
1.4.2.2. Phng trnh ng lc hc........................................................21
1.5. M t i tng bng h phng trnh trng thi...............................25
2.2. La chn phng php iu khin v b iu khin PID cho tay
my robot. .................................................................................................... 29
III. M PHNG VI M HNH ROBOT SCARA SERPENT..............32
3.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
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
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.
Sinh vin: Phm Vn Khin
3
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.
Sinh vin: Phm Vn Khin
4
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).
Sinh vin: Phm Vn Khin
5
Robot scara serpent
1.2.1. Cu to tay my robot Scara Serpent
1.2.1.1. Cu hnh ca robot Scara Serpent
Bao 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 ng
tnh tin. Gn cho mi thanh ni mt h trc to , ta c:
- Khp 1 quay quanh trc z
0
gc
1
.
- Khp 2 quay quanh trc z
1
gc
2
.
- Khp 3 chuyn ng tnh tin theo trc z
2
on d
3
.
- Khp 4 quay quanh trc z
3
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).
Sinh vin: Phm Vn Khin
6
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.m
2
)
R(
)
L(
H)
m(Kg)
1 J9ZF 12 4,8 4.10
-2
2100 15 0,32.10
4
1,38 100 0,6
2 J9ZF 12 4,8 4.10
-2
2100 15 0,32.10
4
1,38 100 0,6
3 J12ZF 12 4,8 1,210
2
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 M
1
= 4 Kg Khi lng thanh ni 1.
2 M
2
= 1.5 Kg Khi lng thanh ni 2.
3 M
3
= 2 Kg Khi lng thanh ni 3.
4 M
4
= 0.6 Kg Khi lng thanh ni 4.
5 a
1
= 0.25 m Chiu di thanh ni gia 2 khp main v fore .
6 a
2
= 0.15 m Chiu di thanh ni gia 2 khp fore v c tay .
7 D
3
Chiu di thanh ni d
3
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
96
0
( so vi trc Ox ).

2
= -115
0
115
0
(so vi trc thanh 1).
Chuyn ng quay ca khp th nht c hnh chiu bng trong h trc to
OX
0
Y
0
v OX
1
Y
1
(Hnh 1.5) tng ng vi gc quay tng trong thc t l 192
0
.
Chuyn ng quay ca khp th hai c hnh chiu bng trong h trc to OX
1
Y
1
v
OX
2
Y
2
tng ng vi gc quay tng l 230
0
. T ta c th thy c hnh chiu gii
hn khng gian lm vic ca n (Hnh 1.6).
Sinh vin: Phm Vn Khin
7
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.
Sinh vin: Phm Vn Khin
8
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 Hnh 1.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.4 m 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
) a
i
i
(rad) d
i
(m) Bin Chuyn ng
1 0 a
1
1
0
1
Quay
2 -180
0
a
2
2
0
2
Quay
3 0 0 0 d
3
d
3
Tnh tin
4 0 0
4
0
4
Quay

Sinh vin: Phm Vn Khin
9
Robot scara serpent
Kho st vi 3 trc khp quay u tin tng ng vi qu o ca khp quay 4
trong mt phng OX
0
Y
0
. Ma trn T
4
l ma trn biu din tay my robot trong h trc
ta gc: T
4
= A
1
.A
2
.A
3
.A
4
A
n
=
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 sin
A
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 sin
A
0 0 1 0
0 0 0 1

]
]

]

]
]
]
3
3
1 0 0 0
0 1 0 0
A
0 0 1 d
0 0 0 1
]
]
]

]
]
]
4 4
4 4
4
cos sin 0 0
sin cos 0 0
A
0 0 1 0
0 0 0 1

]
]

]

]
]
]
K hiu: S
1
Sin
1
; C
1
Cos
1

S
2
Sin
2
; C
2
Cos
2
S
4
Sin
4
; C
4
Cos
4
Sinh vin: Phm Vn Khin
10
Robot scara serpent
S
12
Sin(
1
+
2
); C
12
Cos(
1
+
2
)
Cc bc tnh ton:
Bc 1:
3
4 4
T A

4 4
4 4 3
4 4
C S 0 0
S C 0 0
T A
0 0 1 0
0 0 0 1

]
]
]

]
]
]
Bc 2:
2 3
4 3 4
T A . T

4 4 4 4
4 4 4 4 2
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 0
T
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 2
4 2 4
T A . T

2 2 2 2 4 4
2 2 2 2 4 4 1
4
3
2 4 2 4 2 4 2 4 2 2
2 4 2 4 2 4 2 4 2 2 1
4
3
C S 0 a .C C S 0 0
S C 0 a .S S C 0 0
T
0 0 1 0 0 0 1 d
0 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 .S
T
0 0 1 d
0 0 0 1

] ]
] ]

] ]

] ]
] ]
] ]
+ +
]
]

]

]
]
]
Bc 4:
4
1
1 4
0
4
T . A T T

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 2 0
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 .S
T
0 0 1 0 0 0 1 d
0 0 0 1 0 0 0 1
+
] ]
] ]
+
] ]

] ]
] ]
] ]
Sinh vin: Phm Vn Khin
11
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 C a C
1 2 4 2 4 1 2 4 2 4 1 2 4 2 4 1 2 4 2 4 2 12 1 1
S (C C S S ) C (S C C S ) S (S C C S ) C (C C S S ) 0 a S a S
1 2 4 2 4 1 2 4 2 4 1 2 4 2 4 1 2 4 2 4 2 12 1 1
0 0 1 d
3
0 0 0 1
+ + + +
+ + + +

]
]
]
]
]
(1.2)
Ma trn
0
T
4
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 p
T T
n o a p
0 0 0 1

(1.3)
Vi:
p a o n

, , ,
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:
n
x
= C
1
(C
2
C
4
+ S
2
S
4
) S
1
(S
2
C
4
C
2
S
4
) (1.4)
n
y
= S
1
(C
2
C
4
+ S
2
S
4
) + C
1
(S
2
C
4
C
2
S
4
) (1.5)
n
z
= 0 (1.6)
o
x
= C
1
(S
2
C
4
- C
2
S
4
) + S
1
(C
2
C
4
+ S
2
S
4
) (1.7)
o
y
= S
1
(S
2
C
4
- C
2
S
4
) - C
1
(C
2
C
4
+ S
2
S
4
) (1.8)
o
z
= 0 (1.9)
a
x
= 0 (1.10)
a
y
= 0 (1.11)
a
z
= -1 (1.12)
V h phng trnh xc nh v tr ca im tc ng cui nh sau:
x = p
x
= a
1
.C
1
+ a
2
.C
12
(1.13)
y = p
y
= a
1
.S
1
+ a
2
.S
12
(1.14)
z = p
z
= - d
3
(1.15)
Sinh vin: Phm Vn Khin
12
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 12
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 a
cos
2.a .a
sin (1 cos )
+

t

(1.17)
T tnh c gc
2
:

2
= atan2(sin
2
,cos
2
) (1.18)
Th C
1
, S
1
vo phng trnh (1.13) v (1.14) thu c:
(a
1
+a
2
C
2
).C
1
a
2
S
2
.S
1
= p
x
a
2
S
2
.C
1
+( a
1
+a
2
C
2
).S
1
= p
y
Gii phng trnh bc nht vi n C
1
, S
1
v s dng (1.16) thu c :

1 2 2 x 2 2 y
x
1
2 2
x y
y 1 2 2 y 2 2 x
1
2 2
x y
(a a C ).p a S .p
C
p p
(a a C ).p a S .p
S
p p
+ +


+
+

+
(1.19)

1
= atan2(S
1
,C
1
)
(1.20)
T phng trnh (1.15) ta c:
d
3
= - p
z
(1.21)
Sinh vin: Phm Vn Khin
13
Robot scara serpent
Mt khc t phng trnh (1.4) c:
n
x
= C
1
(C
2
C
4
+ S
2
S
4
) S
1
(S
2
C
4
C
2
S
4
) (1.22)
Rt gn theo cc cng thc lng gic thu c:
n
X
= cos
1
.cos(
2
-
4
) sin
1
.sin(
2
-
4
) = cos(
1
+
2
-
4
)
(1.23)
sin(
1
+
2
-
4
) =
2
x
1 n (1.24)

2
1 2 4 x x
( ) atan2( 1 n ,n ) +

2
4 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 .p
C
p p
(a a C ).p a S .p
S
p p
atan2 S , C
p p a a
cos
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
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) = [q
1
(t), q
2
(t), ... q
n
(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
T
U
q

(1.30b)
- h(
q
,
q
) - Vect [n x 1] lc ly tm v Coriolit:
h(
q
,
q
) = [h
1
, h
2
, ... , h
n
]
T
. (1.31)
n n
j jkm k m
k 1 m 1
h h q q

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

T
ij i ikm jkm
U I U Trace h
. (1.33)
m k
i
0
2
ikm
q q
T
U

(1.34)
- g(q) - Vect [n x 1] lc trng trng:
g(q) = [g
1
, g
2
, ... , g
n
]
T
. (1.35)
( )
n
i
j j ij
i 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
.
Sinh vin: Phm Vn Khin
15
Robot scara serpent
Vect [4 1] bin khp: q(t) = [q
1
(t), q
2
(t), d
3
(t), q
4
(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 : q
i
=
i
i vi khp quay; q
i
= r
i
i vi khp tnh tin.

i
: mmen lc ng ti khp th i.
ng nng ca khp th i:
2 2
i i i i i
1 1
K m.v J .
2 2
+
(1.39)
Vi J
i
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
d
3
4
Chiu di a
1
a
2
d
3
0
Khi lng m
1
m
2
m
3
m
4
Vn tc v
1
v
2
v
3
v
4
Sinh vin: Phm Vn Khin
16
Robot scara serpent
Chiu di tm khi l
g1
l
g2
l
g3
l
g4
Gi s khi lng nm u mt cc thanh ni, ta c chiu di tm khi chnh
l chiu di ca thanh ni: l
g1
= a
1
l
g2
= a
2
l
g3
= d
3
K hiu chiu di cc khp: a
1
= l
1
a
2
= l
2
d
3
= l
3
H qui chiu gn vi trc to (O
0
x
0
y
0
z
0
) trn khp th nht. Khi mt
phng (O
0
x
0
y
0
) 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 : x
1
, y
1
, z
1
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 2
1
1 g1
v x y z
v ( l .sin . ) (l .cos . )
v l .
+ +
+

& & &


& &
&
Trong : J
1
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 1
K m .v J .
2 2
1 1
K m .l . J .
2 2

+
+
(1.40)
Sinh vin: Phm Vn Khin
17
Robot scara serpent
Khp 2:

'

+ +
+ +
0 z
) . s i n ( l . s i n l y
) . c o s ( l . c o s l x
2
2 1 g 2 1 1 2
2 1 g 2 1 1 2

'

+ + +
+ +
0 z
) ).( .cos( l . .cos l y
) ). ( .si n( l . .sin l x
2
2 1 2 1 g2 1 1 1 2
2 1 2 1 g2 1 1 1 2

[ ] [ ]
2 2 1
2
1 g2 1
2
2 1
2
g2
2
1
2
1
2
2
2
2 1 2 1 g2 1 1 1
2
2 1 2 1 g2 1 1 1
2
2
2
2
2
2
2
2 2
)cos . .( .l 2.l ) ( . l . l v
) ).( .cos( l . .cos l ) ).( .sin( l . .sin l v
z y x v



+ + + +
+ + + + + +
+ +
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 1
K m .v J .
2 2
1 1
K m . l . l .( ) 2.l .l .( . ).cos J .
2 2
P 0
+
]
+ + + + +
]
]

& & & & &



(1.41)
Khp 3:

'

+ +
+ +
3 3
2 1 2 1 1 3
2 1 2 1 1 3
- l z
) . s i n ( l . s i n l y
) . c o s ( l . c o s l x
Sinh vin: Phm Vn Khin
18
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
1 2 1
2
.
2
.
1
2
2
.
2
1
2
1
.
2
3
.
2
3
2
.
2
.
1 2 1 2
.
1 1 1
2
.
2
.
1 2 1 2
.
1 1 1
2
3
.
2
3
.
2
3
.
2
3 3
l ) . .( .l 2.l ) ( . l . l v
l ) ).( .cos( l . .cos l ) ).( .sin( l . .sin l v
z y x v
+ + + + +
+
]
]
]

+ + + +
]
]
]

+ +
+ +
.
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
1
K m .v
2
1
K m . l . l .( ) 2.l .l .( . ) l
2
P m .g.h P m .g.l

]
+ + + + +
]
]

(1.42)
Khp 4:
Sinh vin: Phm Vn Khin
19
Robot scara serpent

'

+ +
+ +
3 4
2 1 2 1 1 4
2 1 2 1 1 4
- l z
) . s i n ( l . s i n l y
) . c o s ( l . c o s l x

'


+ + +
+ +
3
.
4
.
2
.
1 2 1 2
.
1 1 1
.
4
.
2
.
1 2 1 2
.
1 1 1
.
4
l z
) ).( . c os( l . .cos l y
) ) .( .sin( l . .sin l x

. . .
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 1
K m . v J .w
2 2
1 1
K m . l . l . ( ) 2.l .l .( . )cos l J ( )
2 2
P m .g.h
P m .g.l
+
]
+ + + + + + + +
]
]

( ) 2.43

Tng ng nng ca h thng:
K = K
1
+ K
2
+ K
3
+ K
4

(1.44)
t tm khi ti u mt ca thanh ni, ta c: l
g1
= l
1
; l
g2
= l
2
; l
g3
= l
3
.
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
1
K (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
1
l .J . J . . J . .
2
+ + +
& & && & &
(1.45)
Tng th nng ca h thng:
P = P
1
+ P
2
+ P
3
+ P
4
= -(m
3
+ m
4
).g.l
3
(1.46)
Sinh vin: Phm Vn Khin
20
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
1
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 . . ...
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
1
m 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 .

] + + + + + + + + + +
]

] + + + + + + + + +
]
+ + + +
Sinh vin: Phm Vn Khin
21
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 4
m ).l .l .sin . J . +
(1.50)
c.
0

L
1

(1.51)
Khp 2:
2 .
2
2
d L L
dt

(1.52)
a.
2
2 3 4 2 2 4 1 2 3 4 1 2 2 1
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
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. 3 4 3
3
l ). m (m
l
L

Sinh vin: Phm Vn Khin


22
Robot scara serpent
b.
..
3 4 3
.
3
l ). m m (
l
L
dt
d
+

(1.56)
c.
).g m (m
l
L
4 3
3
+

(1.57)
Khp 4:
4 .
4
4
d L L
dt

(1.58)
a.
.
4 4.
.
2 4.
.
1 4.
.
4
J J J

L
+ +

b.
.. .. ..
4. 1 4. 2 4. 4 .
4
d L
J J J
dt

+ +


(1.59)
c.
4
L
0


(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
1 2 2 1 234 4 4 2 24
2
2 234
1 2 2 1 234 24
2
2 234 2
. Sin . l . l . m . J ). J l . m (
]. Cos . l . l . m J l . m [


+ + + +
+ + +
(1.62)
3 34 3 34 3
m .l m .g F
&&
(1.63)

4 4 1 4 2 4 4
J J J + +
&& && &&
(1.64)
Trong :
m
1234
= m
1
+ m
2
+ m
3
+ m
4
.
Sinh vin: Phm Vn Khin
23
Robot scara serpent
m
234
= m
2
+ m
3
+ m
4
.
(1.65)
m
34
= m
3
+ m
4
; Vi m
4
= m
40
+ m
t

m
40
: khi lng ca khp 4
m
t
: khi lng ca ti c ni vi khp 4.
J
124
= J
1
+ J
2
+ J
4
.
J
24
= J
2
+ J
4
; Vi J
4
= J
40
+ J
t
J
40
: m men qun tnh ca khp 4.
J
t
: 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
= F
3
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 h
H 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:
Sinh vin: Phm Vn Khin
24
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) =
[ ]
T
T
4
T
2
T
1
X X X
(1.68)

11 1
1
12 1
x q
X
x q
] ]

] ]
] ]
&
,
21 2
2
22 2
x q
X
x q
] ]

] ]
] ]
&
,
41 4
4
42 4
x q
X
x q
] ]

] ]
] ]
&
(1.69)
Tn hiu vo:

1 1
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)
Sinh vin: Phm Vn Khin
25
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 :
b
ij
(X) l cc thnh phn tng ng ca ma trn H
-1
.
( )
11 22 33 23 32
H
1
b H H H H
D

.
( )
12 13 32 12 33
H
1
b H H H H
D

.

( )
13 12 23 13 22
H
1
b H H H H
D

.
( )
21 23 31 21 33
H
1
b H H H H
D

.

( )
22 11 33 13 31
H
1
b H H H H
D

.

( )
23 13 21 11 23
H
1
b H H H H
D

.
(1.74)
( )
31 21 32 31 22
H
1
b H H H H
D

.
( )
32 12 31 11 32
H
1
b H H H H
D

.
( )
33 11 22 12 21
H
1
b H H H H
D

.
H 11 22 33 21 32 13 31 23 12 11 23 32 21 12 33 31 22 13
D det H H H H H H H H H H H H H H H H H H H + +
a
i
(X) l thnh phn th i ca vect:
( )
( )
( ) ]
]
]
]
]


X h
X h
X h
. H
3
2
1
1
[ ]
1 11 1 12 2 13 3
a b h b h b h + +

[ ]
3 23 2 22 1 21 2
h b h b h b a + +

(1.75)
Sinh vin: Phm Vn Khin
26
[ ]
4 31 1 32 2 33 3
a b h b h b h + +
Robot scara serpent
Sinh vin: Phm Vn Khin
27
Robot scara serpent
II. XY DNG M HNH H THNG IU KHIN
2.1. Cu trc h thng iu khin robot
Robot 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.
Sinh vin: Phm Vn Khin
28
Qu o
t
B iu
chnh
Cm bin
Mi trng
Tay my
n
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 u
k
, b iu khin PID l u
PID.
Hm truyn ca b iu khin:

I
C P D P D
I
K 1
G (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 K
P
, K
I
, K
D
cho ph hp.
Phng php Ziegler Nichols.
Ziegler Nichols l phng php xc nh h s K
P
, hng s thi gian tch phn
T
I
v hng s thi gian vi phn T
D
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) :
Sinh vin: Phm Vn Khin
29
Hnh 2.2: S b iu khin PID
PID
u
k
e
Robot scara serpent
Hnh 2.3: p ng bc thang n v ca h h.
T
1
: thi gian tr
T
2
: hng s thi gian

- Phng php Ziegler Nichols. Trng hp 2.
T
1
v T
2
c xc nh bng cch v ng tip tuyn vi ng cong S ti
im un, ng tip tuyn ny ct trc honh ti T
1
v ng y(t)=K l im c
honh T
2
.
Khi m hnh i tng c dng:
1
Ts
2
K
G(s) e
T s 1

+
Bng 2.1: Thng s b PID.
Thng s
B K
K
P
T
I
T
D
P T
2
/T
1
K 0
PI 0.9T
2
/T
1
K T
1
/0.3 0
PID 1.2T
2
/T
1
K 2T
1
0.5T
1
Xc nh thng s b iu khin PID da vo p ng ca h kn bin gii
n nh.
Sinh vin: Phm Vn Khin
30
Robot scara serpent
Hnh 2.4: p ng ca h kn bin gii n nh.
Bc 1:
t T
I
= , T
D
= 0, thay i K
P
t 0 ti gi tr gii hn K
gh
ng vi u ra h
thng kn c dao ng bin gii n nh. Dao ng ny tng ng vi chu k gii
hn T
gh
.
Bc 2: Thng s b PID c xc nh theo bng :
Bng 2.2: Thng s b PID .
Thng s
B K
K
P
T
I
T
D
P 0.5K
gh
0
PI 0.45K
gh
0.83T
gh
0
PID 0.6K
gh
0.5T
gh
0.125T
gh
Vi:
P
I
I
K
K
T

;
D P D
K K .T
Sinh vin: Phm Vn Khin
31
Robot scara serpent
III. M PHNG VI M HNH ROBOT SCARA SERPENT
3.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 :
Sinh vin: Phm Vn Khin
32
Robot scara serpent
Sinh vin: Phm Vn Khin
33
Robot scara serpent
Sinh vin: Phm Vn Khin
34
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 php
iu khin ng lc hc ngc s dng iu khin PID.Cha xy dng qu o
chuyn 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 thut
ton 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.
.
Sinh vin: Phm Vn Khin
35
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.
Sinh vin: Phm Vn Khin
36

You might also like