Thit k Robot gpp bnh gas

TRNG I H
C BCH KHOA THNH PH H CH MINH
KHOA C KH
B MN C IN T

BO CO MN HC
H
THIT K H THNG C IN

THIT K
ROBOT GP BNH GAS
CBGD:

PGS.TS. NGUYN
NGUY
TN TIN
KS.NGUYN
N THNH LUN

SVTH:

V HONG LONG

20701353

NGUYN
N PHC HNG
H
20701015
PHM TN PH
NG V0701884

TP H Ch Minh, thng 01 nm 2012

Thit k Robot gp bnh gas

Contents
Chng 1. TNG QUAN.................................................................................................... 1
1. GII THIU ................................................................................................................ 1
2. YU CU THIT K................................................................................................. 1
1.3 CC PHNG N XUT ................................................................................ 3
1.3.1Cc phng n xut cho robot: ....................................................................... 3
1.3.2 Cc phng n xut cho tay gp:................................................................... 6
Chng 2. THIT K C KH........................................................................................... 9
2.1 GII THIU .................................................................................................................. 9
2.1.1Phng n gp gas .............................................................................................. 9
2.1.2 Phng n ca tay my: ..................................................................................... 9
2.2TNH TON NG HC THUN V NGHCH CHO TAY MY ................... 10
2.2.1-ng hc thun ................................................................................................ 10
2.2.2 Bi ton ng hc ngc .................................................................................. 11
2.3 TNH TON DI CC KHU, KHNG GIAN LM VIC, THI
GIAN CH GP CHO TAY MY .............................................................................. 13
2.3.1 di cc khu ................................................................................................ 13
2.3.2 Khng gian lm vic ca tay my .................................................................... 14
2.3.3 Tnh ton thi gian ch gp cho tay my ......................................................... 16
2.4 TO GC CC KHU KHP NG VI TO DESCARTES
CC IM QUAN TRNG ........................................................................................ 18
2.5 PHNG TRNH NG LC HC CHO TAY MY ..................................... 21
2.6 TNH TON B TRUYN NG CHO M HNH ........................................... 24
2.6.1 Tnh ton khp 1.............................................................................................. 24
2.6.2 Tnh ton khp 2 ............................................................................................. 26
2.6.3 Tnh ton khp 3.............................................................................................. 29
Tng t nh khp 2 ................................................................................................. 29
Chng 3. THIT K H MCH IU KHIN ............................................................ 31
3.1 S KHI HOT NG CA H MCH IU KHIN............................. 31
3.2 MCH IU KHIN TRUNG TM .................................................................... 31
3.4-MCH DRIVER ..................................................................................................... 32
3.5 MCH GIAO TIP VI MY TNH.................................................................... 33
Chng 4. GII THUT IU KHIN .......................................................................... 34
ii

Thit k Robot gp bnh gas

4.1. S KHI H THNG ROBOT ..................................................................... 34

4.2 GII THUT IU KHIN .................................................................................. 34
4.2.1 Nguyn tc chung. ............................................................................................ 34
4.2.2 Gii thut c Encoder Timer .......................................................................... 34
4.3 THIT K CHNG TRNH IU KHIN........................................................ 36
4.3.1 Lu gii thut .............................................................................................. 36
4.3.2 Chng trnh iu khin ................................................................................... 37
Chng 5. THC NGHIM V KT QU .................................................................... 40
5.1 M HNH C THC T ....................................................................................... 40
5.2 MCH IN THC T......................................................................................... 40
5.3 KT QU V HNG PHT TRIN TI ................................................... 41
5.3.1 Kt qu............................................................................................................. 41
5.3.2Hng pht trin ............................................................................................... 41
TI LIU THAM KHO ................................................................................................. 42

iii

Thit k Robot gp bnh gas

Chng 1. TNG QUAN

1. GII THIU
Trong bi cnh cc nh cung cp gas cnh tranh mt cch khc lit chim lnh th trng th
nhu cu qung b hnh nh cc cng ty ngy cng c nhng tng c o. Xoay quanh ch
ny ti ca nhm l nghin cu thc hin tng qung b hnh nh cho mt cng ty bng
cch thit k mt robot t ng sp xp, vn chuyn cc bnh gas trong nh kho.

2. YU CU THIT K
-

Robot c t trong nh kho c kch tht 4 x 5 x 3 (m).

Hnh 1.1 Khng gian nh kho

Thit k Robot gp bnh gas

-

Bnh gas cha trong kho l loi 12 Kg vi cc thng s sau :

Thng s

Kch thc

ng knh ngoi thn

300 mm 1

Chiu cao ton thn

585 mm 1

Chiu cao quai xch

160 mm 2

Chiu cao chn

62 mm 2

Chiu cao thn bnh

446 mm 2

ng knh ngoi chn

305 mm 2

ng knh ngoi quai xch

246 mm 2

Chiu dy thn bnh

2.6 mm

Trng lng cha LPG

12 kg LPG

Hnh 1.2-Bnh gas thc t v hnh v trn SolidWorks

Thit k Robot gp bnh gas

Yu cu thit k :
-

C cu c thit k sao cho n gin d thc hin, kinh ph vaa phi,

ph khng chim dng
khng gian nh kho qu ln
l nh kho cha c mt s lng
ng bnh gas nht
nh nh (trn
300 bnh gas). Ngoi ra cn m bo yu cu d dng s dng
ng v bo
b tr sa cha.
Khi nhn vin tt bnh gas mt v tr no ngay caa nh kho th tay my s
s t pht
hin c ngi ii gas hoc
ho kho chun b nhp thm bnh gas:
o Nu nh bnh
nh gas c tt vo l bnh gas khng th tay my ss t pht hin v
hiu c rng:
ng: ang thc hin nhim v i gas v ch ngh, n s n v
gp bnh gas khng n ng ni theo gii thut lpp trnh. Sau , tay my di
chuyn n ni
i bnh
b
c gas v gpp gas ra ngoi cho nhn vin.
o Nu trong tr
ng hp bnh gas c tt vo nh kho l bnh c gas th mc
m nhin
tay my t hiuu rng
r
ang thc hin ch nhpp bnh gas mi
m vo kho v n t
ng gp ln
n lt
l tt c cc bnh c gas vo kho ng ni
i quy nh v gp tt c
cc bnh khng c gas lln ltt trong kho ra ngoi cho nhn vin chuyni.
chuy

1.3 CC PHNG N XUT

XU
1.3.1Cc phng
ng n xut cho robot:
C rt nhiu phng
ng n c th
th thc hin nh dng mobile robot v
i c cu nng h, kiu
bn my vi 3 bc tnh tin,
n, robot scara... Nhng nhn
n chung chng khng mnh
m
v tnh qung
b, PR cho sn phm. V vyy nhm xin xut mt s phng n kh thi cho cc cu tay my vi
u im ln l to dng cc mt
m hnh nh chuyn nghip cho cng ty.
Tay my ba bc t do R-R-R
R
c kt hp c cu
u hnh bnh hnh:

Hnh 1.3-Tay
Tay my 3 bc
b t do R-R-R vi c cu hnh bnh hnh

Thit k Robot gp bnh gas

u im: c cu chc chn, nh hng gp thng l chnh xc , u l khp xoay

nn d ch to .
Nhc im: chim khng gian ln, kh khn trong thit k.

Ty my ba bc t do R-R-R, trong tay gp c treo t do khu cui :

Hnh 1.4-Tay my 3 bc t do R-R-R tay gp treo

u im: C cu n gin, d dng tnh ton cc phng trnh ng hc thun,

nghch, phng trnh ng lc hc cng nh gii thut iu khin.
Nhc im: Vic xc nh hng gp thng t trn xung bng i trng phi
tn thi gian cho tay gp khng cn dao ng.

Tay my bn bc t do R-R-R-R :

Hnh 1.5-Tay my 4 bc t do R-R-R-R

Thit k Robot gp bnh gas

u im: C
cu
c bn bc t do linh ng trong vic gpp v di chuy
chuyn bnh gas.
Nhc im:: Thm
T
mt khu khp th vic tnh ton v thit k s phc tp hn.

Tay my ba bc t do R-T-T:
R

Hnh 1.6
1.6-Tay my ba bc t do R-T-T

u im: nh
nh hng
h
gp thng tt, tnh ton v iu khinn th
thc hin d dng.
Nhc im:: C
C cu ca khp tnh tin kh phc tp, thcc hin
hi m hnh thc t l
kh khn.

Tay my ba bc t do R-T-T:
R

Hnh 1.7-Tay my 3 bc t do R-T-T

Thit k Robot gp bnh gas

u im: nh hng gp thng tt, tnh ton v iu khin thc hin d dng.
Nhc im: C cu ca khp tnh tin kh phc tp, thc hin m hnh thc t l
kh khn.
1.3.2 Cc phng n xut cho tay gp:

C 2 phng n l gp ngang v gp ng:

Gp ngang: Dng c cu tay kp kp gia thn bnh gas.
u im: Bnh gas c gi cht v khng dao ng trong qu trnh di chuyn t
im ny n im khc.
Nhc im: Lc kp ln i hi c cu kp phi chc chn, ngoi ra ma st gia
mt kp v bnh gas phi c m bo thng c trng lc ca chnh bnh gas.
Gp ng: Dng c cu tay mc vnh trong phn trn bnh gas, mc t trong ra ngoi.
u im: C cu mc khng cn lc kp ln.
Nhc im: Khi tay my dch chuyn, ti trng bnh gas s dao ng do khng
c lc kp ngang tm.

Cc xut cho c cu tay gp:

Dng thu lc, kh nn:

Hnh 1.8-C cu tay gp dng thu lc kh nn

Thit k Robot gp bnh gas

Dng c cu bnh rng, thanh rng:

Hnh 1.9-C cu tay gp dng bnh rng, thanh rng

Dng c cu hai bnh rng n khp:

Hnh 1.10-C cu tay gp dng 2 bnh rng n khp

Thit k Robot gp bnh gas

Chng 2. THIT K C KH

2.1 GII THIU

2.1.1Phng n gp gas :
Chn phng n dng c cu hai bnh rng n khp v gp thng t trn xung, tay gp
bt ra ngoi mc vo vnh trn ca bnh gas: thc hin trn m hnh thc t dng ng c
RC (hnh 1.10)

2.1.2 Phng n ca tay my:

Theo thng nht ca nhm, ta chn phng n tay my ba bc t do R-R-R trong tay gp
c treo t do v nhng l do sau:

d dng cho vic thit k m hnh thc t th cc phng n c bc t do tnh tin s b
loi.

Mc d c cu hnh bnh hnh l c cu nh hng cho hng gp l rt chnh xc. Nhng,
vic thit k li gp rt nhiu tr ngi trn m hnh thc t, ngoi ra n cn chim khng
gian ln cho nn cc phng n c c cu hnh bnh hnh cng s b loi b.

Phng php dng tay my bn bc t do R-R-R-R cng d dng tho mn vic nh hng
gp cho tay my, nhng tnh ton phng trnh ng hc thun, nghch, cng nh cc
phng trnh ng lc hc v gii thut iu khin cho tay my l rt kh khn. Ngoi ra,
vic tng thm mt khu khp cng lm tng thm khng gian chim ch ca tay my v
tng thm s phc tp cho c cu.

Nh vy, phng n c la chn l phng n dng tay my R-R-R trong tay gp
c th t do. gii quyt v gc nghing ca ti khi tay my dch chuyn ta ch cn ci
t thi gian t lc tay my n v tr cn gp gas n khi gp gas. Bi ton ny s c
cp sau.

Thit k Robot gp bnh gas

Hnh 2.1-M hnh thit k cho tay my

2.2 TNH TON BI TON NG HC THUN V NGHCH CHO TAY MY:

2.2.1-ng hc thun:

Hnh 2.2-M hnh tnh ton ng hc thun cho tay my

Thit k Robot gp bnh gas

Bng thng s DH :
i

90

l1

l2

l3

Cc ma trn chuyn i DH:

cos 1
sin
1
0
T1 =
0

0 sin 1
0 cos 1
1
0
0
0

0
0
l1

cos 2
sin
2
1
T2 =
0

sin 2
cos 2
0
0

0 l2 cos 2
0 l2 sin 2
1
0

0
1

cos 3
sin
3
2
T3 =
0

sin 3
cos 3
0
0

0 l3 cos 3
0 l3 sin 3
1
0

0
1

cos 1 cos ( 2 + 3 ) cos 1 sin ( 2 + 3 ) sin 1

sin 1 cos ( 2 + 3 ) sin 1 sin ( 2 + 3 ) cos 1

0
0
1
2
T3 = T1 T2 T3 =
cos ( 2 + 3 )
0
sin ( 2 + 3 )

0
0
0

10

cos 1 l3 cos ( 2 + 3 ) + l2 cos 2

sin 1 l3 cos ( 2 + 3 ) + l2 cos 2

l3 sin ( 2 + 3 ) + l2 sin 2 + l1

Thit k Robot gp bnh gas

0

rQ = 0T3 * 3 rQ

0
0
3
rQ =
0

1
Suy ra:
cos 1 l3 cos ( 2 + 3 ) + l2 cos 2

X Q = cos 1 l3 cos ( 2 + 3 ) + l2 cos 2

sin 1 l3 cos ( 2 + 3 ) + l2 cos 2

0

rQ =

Y
=
sin
l
cos
+
+
l
cos
(
)

Q
1
3
2
3
2
2

(1)

+
+
+
l
sin
l
sin
l
(
)

3
2
3
2
2
1

Z Q = l3 sin ( 2 + 3 ) + l2 sin 2 + l1

Mt khc:

0
0
0
0
rP = rQ +
d

0

(2)

T (1) v (2) suy ra:

X P = cos 1 l3 cos ( 2 + 3 ) + l2 cos 2

YP = sin 1 l3 cos ( 2 + 3 ) + l2 cos 2

Z P = l3 sin ( 2 + 3 ) + l2 sin 2 + l1 d

2.2.2 Bi ton ng hc ngc:

Ta xt to ca im P trong h quy chiu 0:

X P = cos 1 l3 cos ( 2 + 3 ) + l2 cos 2 (3)

(3) v ( 4)
Y
YP

= tg1 1 = arctg P
YP = sin 1 l3 cos ( 2 + 3 ) + l2 cos 2 (4)
XP
XP

(5)
Z
=
l
sin
+
+
l
sin
+
l

(
)
2
3
2
2
1
P 3

11

Thit k Robot gp bnh gas

2

X
2
X
(3) P = l3 cos (2 + 3 ) + l2 cos2 P = l3 cos (2 + 3 ) + l2 cos2
cos 1
cos1
2

(5) Z P l1 + d = [l3 sin (2 + 3 ) + l2 sin 2 ] ( Z P l1 + d ) = [l3 sin (2 + 3 ) + l2 sin 2 ]2

Cng v theo v 2 phng trnh trn ta c:
2

X
2
2l2l3 cos 2 cos( 2 + 3 ) + 2l2l3 sin 2 sin( 2 + 3 ) = P + ( Z P l1 + d )
cos 1
2

1
YP
X P2 + YP2
2

1
1
Matkhac :
=
+
tg
=
+
=

X P2
cos 1
XP
X 2 +Y2
2
2l2l3 cos( 2 + 3 2 ) = P 2 P + ( Z P l1 + d )
XP
2l2l3 cos(3 ) =

X P2 + YP2
2
+ ( Z P l1 + d )
2
XP

X P2 + YP2
2
+ ( Z P l1 + d )
2
XP
3 = arccos
2l2l3
Xt phng trnh (3):
XP
= l3 cos ( 2 + 3 ) + l2 cos 2
cos 1
cos 2 ( l3 cos 3 + l2 ) sin 2 ( l3 sin 3 ) =
cos 2 ( l3 cos 3 + l2 )

XP
cos 1

sin 2 ( l3 sin 3 )

( l3 cos 3 + l2 ) + ( l3 sin 3 )
( l3 cos 3 + l2 ) + ( l3 sin 3 )
cos 2 ( l3 cos 3 + l2 )
sin 2 ( l3 sin 3 )

=
2

l3 + l2 + 2l2l3 cos 3

t :

l3 + l2 + 2l2 l3 cos 3

cos =

cos 1

sin =

XP

l3 + l2 2 + 2l2 l3 cos 3

cos 1 l3 + l2 2 + 2l2l3 cos 3

12

(6)

( l3 sin 3 )
2

XP
2

( l3 cos3 + l2 ) + ( l3 sin 3 )

cos 1 l3 + l2 2 + 2l2l3 cos 3

( l3 cos 3 + l2 )
2

XP

l3 + l2 + 2l2l3 cos 3

cos =

Thit k Robot gp bnh gas

(6) cos 2 cos sin 2 sin = cos

cos( 2 + ) = cos
vi:

= arccos

T suy ra:

XP
2

cos 1 l3 + l2 + 2l2l3 cos 3

( l3 cos 3 + l2 )

; = arccos

l3 + l2 2 + 2l2 l3 cos 3

2 = + k *3600

Hay:
2 = arccos

XP
2

cos 1 l3 + l2 + 2l2 l3 cos 3

arccos

( l3 cos 3 + l2 )
2

l3 + l2 + 2l2 l3 cos 3

+ k * 360 0

2.3 TNH TON DI CC KHU, KHNG GIAN LM VIC, THI GIAN CH GP CHO
TAY MY
2.3.1 di cc khu:

Vic tnh ton di cc khu phi tho mn cc iu kin bin sau y:

Tay my n c cc v tr xa nht cao nht, xa nht thp nht, gn nht cao nht v gn
nht thp nht trong nh gas.

Hot ng ca tay my khng lm n chm vi trn nh, bn vch tng, nn nh v cc
bnh gas trong nh kho.

Vic sp xp gas c nhiu bnh gas. (hn 300 bnh).

Hnh 2.3-Tnh ton kch thc cc khu

13

Thit k Robot gp bnh gas

Da vo kch thc chiu cao ca phng (3m), ta tnh ton c chi
chiu cao khi xp chng
bnh gas an ton vo khong <2.5m. Vy, mi chng ch c 4 bnh. Khi chiu cao s l:
h = 0.6 * 4 (4 1) * 0.06 = 2.22( m )

Chiu cao cao nht ca chng ga l 2.22 (m) , chiu cao thp nht bng
b
0.6(m) l khi ch
c mt bnh gas. Nh vy
tin cho vic tnh ton ta chn kch thc
th khu 1 nh sau:

 

0.6  2.22
 1.4
2

Khi tay my vn
n n im xa nht v thp nht (cng nh
xa nht): x= 1.825
y=2.325 z=0.6, th tng chiu
chi di l2 v l3 phi ln hn khong
ng cch hnh hc
h t im A n
im P (hnh v). Ta c:
l2 + l3 x 2 + y 2 + (1.4 0.6 0.3)2 = 3

d dng cho vic tnh ton, ta chn:

ch

l2 = l3 =1.6(m)
2.3.2 Khng gian lm vic
vi ca tay my:

Khng gian lm vic trong mt

phng Oxy:

Hnh 2.4-Khng
Khng gian cha bnh gas trong mt phng
ng Oxy

14

Thit k Robot gp bnh gas

Tng s bnh gas c th xp vo kho: 424 bnh gas. Trong khu bnh c gas v khng
gas mi bn cha c 212 bnh .
Khng gian lm vic trong h quy chiu cc gc quay:
Theta 1: Da vo hnh v trn ta c:

1800 1 180o

Hnh 2.5-Khng gian lm vic trong mt phng Oxy

Theta 2: S dng iu kin khng ng trn kho, ta c:

Hnh 2.6-Tnh ton khu 2

15

Thit k Robot gp bnh gas

sin 2max =

BH 3 1.4 0.2
=
= 0.875 2max = 61.045o
AB
1.6

Vi khong cch 0.2m l khong cch b tr cho c cu khu 2 c b rng khu khong 0.4m.
Nh vy, ta ly:

60o 2 60o
Theta 3: Gc theta3 c khong cho php ph thuc vo theta2. V vng hot ng ca
khp 3 ph thuc vo v tr ca im B. Cho nn bng phng php thc nghim cho m hnh v
tng hp cc trng hp tho mn yu cu trn ta c:

140o 3 140o
2.3.3 Tnh ton thi gian ch gp cho tay my:
Tnh ton vn tc ca tay tay gp n nh : 0.3m/s
2

Gia tc ca tay gp cao nht: amax = 1m / s

Suy ra gc lch cc i ca tay gp so vi phng thng ng:

a
g

max = arc tg = arc tg(0.1) 5.710

Phng trnh dao ng ca tay gp:
m .

d
d

= mg sin k .
2
2

d
k
d
= g sin .
2
m
2

Trng hp khng ti: d=0.3, m=3

+

2
200
sin + 15 ( ) = 0
3

16

Thit k Robot gp bnh gas

Hnh 2.7-p ng dao ng ca tay gp khng ti

Trng hp c ti l bnh khng cha gas: d= 0.6, m=9
+

2
100
sin + 10 ( ) = 0
3

Hnh 2.8-p ng dao ng ca tay gp c ti l bnh khng gas

Trng hp c ti l bnh cha gas: d=0.6, m=15
+

100
50 2
sin +
( ) = 0
3
3

17

Thit k Robot gp bnh gas

Hnh 2.9-p ng dao ng ca tay gp c ti l bnh cha gas

Vy, thi gian ch gp cho tay my l: 2(s).

2.4 TO GC CC KHU KHP NG VI TO DESCARTES CC IM QUAN

TRNG

V tr ngh ca tay my: X=1.6, Y=0, Z=1.1, tng ng vi cc to khu

1 = 0o ,2 = 60o ,3 =120o

V tr xa nht, thp nht: X= 1.825
tg 1 =

Y = 2.325

Z =0.6

y
= 1.274 1 = 51.870 o
x
2

( z l1 + d )
cos 3 =

x
2
2
+
l2 l3
cos

1
= 0.755 3 = 40.963o
2.l2 .l3

cos 2 + 3 =
= 0.986 2 + 3 = 9.602o
2 2.l .cos .cos 3
2

2
1
2
18

Thit k Robot gp bnh gas

Chn :
2 = 30.0835o
3 = 40.963o

V tr xa nht, cao nht: X= 1.825

tg 1 =

hoc

2 = 10.8795 o
3 = 40.963 o

Y =2.325

Z =2.22

y
= 1.274 1 = 51.870 o
x
2

( z l1 + d )
cos 3 =

x
2
2
+
l2 l3
cos 1
= 0.951 3 = 17.955o
2.l2 .l3

= 0.935 2 + 3 = 20.753o
cos 2 + 3 =
2 2.l .cos .cos 3
2

2
1
2
Chn:
2 = 11.7755 o

hoc

3 = 17.955 o

2 = 29.7305 o
3 = 17.955 o

V tr gn nht theo phng y, thp nht: X = 0

Y = 1.275

Z =0.6

x = 0 1 = 90o
2

( z l1 + d )
cos 3 =

y
2
2
+
l2 l3
sin 1
= 0.3168 3 = 108.427o
2.l2 .l3

= 0.682 2 + 3 = 47.021o
cos 2 + 3 =
2 2.l .sin .cos 3
2

2
1
2
Chn:
2 = 7.215o
3 = 108.472o

Hoc

2 = 101.257 o
3 = 108.472o

V tr gn nht theo phng y, cao nht: X= 0

x = 0 1 = 90o
19

(loi)

Y = 1.275

Z =2.22

Thit k Robot gp bnh gas

2

( z l1 + d )

cos 3 =

y
2
2
+
l2 l3
sin 1
= 0.437 3 = 115.944o
2.l2 .l3

cos 2 + 3 =
= 0.751 2 + 3 = 41.297o
2 2.l .sin .cos 3
2

2
1
2
Chn:
2 = 16.221o
3 = 115.944 o

Hoc

2 = 99.264 o
3 = 115.944o

(loi)

V tr gn nht theo phng x, thp nht: X = -1.125

Y =0

Z =0.6

x < 0, y = 0 1 =180o
2

( z l1 + d )

cos 3 =

x
2
2
+
l2 l3
cos 1
= 0.704 3 = 134.747o
2.l2 .l3

cos 2 + 3 =
= 0.914 2 + 3 = 23.962o
2 2.l .cos .cos 3
2

2
1
2
Chn:
2 = 91.3355 o
3 = 134.747 o

(loi) hoc

2 = 43.4115 o
3 = 134.747 o

V tr gn nht theo phng x, cao nht: X = -1.125

Y=0

Z =2.22

x < 0, y = 0 1 =180o
2

( z l1 + d )
cos 3 =

x
2
2
+
l2 l3
cos 1
= 0.508 3 = 120.518o
2.l2 .l3

cos 2 + 3 =
= 0.709 2 + 3 = 44.872o
2 2.l .cos .cos 3
2

2
1
2
Chn:

20

Thit k Robot gp bnh gas

2 = 15.387 o
3 = 120.518 o

Hoc

2 = 105.131o
3 = 120.518o

(loi)

2.5 PHNG TRNH NG LC HC CHO TAY MY

Hnh 2.10-n gin m hnh tay my vi cc kch thc

Vn tc khi tm ca thanh 2:

VG 2 =

2
2
&2 & l2

l2 + 1 cos 2

2 2

Vn tc khi tm ca thanh 3:

21

Thit k Robot gp bnh gas

VG3 =

+ &2l2

&1l2 cos 2

( )

( ) [l cos ] ( )
&

+ &2

2
l
& & l3 &

+ 3 2 + 22l2 &3 &2 3 cos 3

2
2

2
2
2 l3 2

l2l3
2 l
l3
3
&
&
&
l2 + l2l3 cos 3 + 3 + 223 cos3
2
2
2

( )

Vn tc ca ti:

VP =

(& ) [l
1

2
cos 2 ] + &2

( )

2
l2 2 + l32 2l2l3 cos 3 + &3 l32 + 2&2&3 l2l3 cos 3 l32

( )

ng nng ca h:

1
+ m2VG22
2
2
2
1
1
1
= I1 &1 + I 2 &2 + m2 &2
2
2
2

T=

1
I1 &1
2

( )

1
+ I 2 &2
2

( )

( )

1
+ m3 &1
2

( )

( ) (l

1
+ mP &1

( ) [l

cos 2 )

( )

( )

+ &2

2
cos 2 ] + &2

( )

1
+ I 3 &3 &2
2

1
1
+ m3VG23 + mPVP2
2
2
2
2
2l
1 &
2
& 2 l2
+ 1 cos 2 + I 3 3
2
2
2

( )

( ) + (& )

2 l3 2

l2 + l2l3 cos 3 + &3

2

( )

2&3&2

2
2
l2l3
l3
l3
&
&
2 + 2 23 2 cos 3 2

2
l2 2 + l32 2l2l3 cos 3 + &3 l32 + 2&2&3 l2l3 cos 3 l32

( )

22

Thit k Robot gp bnh gas

T =

1 &
1
2

( )

1
+ &2
2

1 &
3
2

( )

( )

2
2
l2

I
m
cos
1
2
2 + m3 ( l2 cos 2 ) + mP ( l2 cos 2 )
2

2 l3 2

l2
I 2 + m2 + I 3 + m3 l2 + l2l3 cos 3 + m p l2 2 + l32 2l2l3 cos 3
2
2

2
2

l2l3

l3
l2
2
2
&
&
+
+
+

+
I
m
m
l
I
m
cos

3
p 3
3
3
3
2 3
3

+ m p ( l2l3 cos 3 l3 )
2
2

Th nng ca h: ( Chn gc th t )
l
l

V = m2 g l1 + 2 cos 2 + m3 g l1 + l2 cos 2 + 3 cos ( 2 + 3 ) + m p g l1 + l2 cos 2 + l3 cos ( 2 + 3 )

2
2

V
=0

1
V

= m2 g sin 2 m3 g sin 2 m3 g sin ( 2 + 3 ) m p g sin ( 2 + 3 )

V
= m2 g sin ( 2 + 3 ) m p g sin ( 2 + 3 )

Q1 = N1

Q2 = N 2
Q = N
3
3

Lc suy rng:
nh lut II Lagrange:

d T T V
+
= Q1.......... (1)
&
dt 1 1 1

d T T V
+
= Q2 .......... ( 2 )
&
dt 2 2 2

d T T + V = Q3 .......... ( 3 )
&
dt 3 3 3

2
2
l2

(1): I1 + m2 cos 2 + m3 ( l2 cos 2 ) + mP ( l2 cos 2 ) &&1 = Q1

2

23

Thit k Robot gp bnh gas

(2):
2

2 l3 2

l2
I 2 + m2 + I 3 + m3 l2 + l2l3 cos 3 + m p l2 2 + l32 2l2l3 cos 3 &&2
2
2

l l
2

1
l2
l
+ I 3 + m3 2 3 cos 3 3 + m p ( l2l3 cos 3 l32 ) &&3 &1 m2 2 + 2m3l2 + 2m p l2 cos 2 sin 2
2
2

2
2

m2 g sin 2 m3 g sin 2 m3 g sin ( 2 + 3 ) mp g sin ( 2 + 3 ) = M 2

( )

(3):
2
2

l2l3

l2
l3
2 &&
2
I 3 + m3 + m p l3 3 + I 3 + m3 cos 3 + m p ( l2l3 cos 3 l3 ) &&2
2
2

1 &
2
2

ll

+ 2m pl2l3 ) cos 3 sin 3 &2&3 m3 2 3 + m p l2l3 cos 3 sin 3

2

m2 g sin ( 2 + 3 ) m p g sin ( 2 + 3 ) = M 3
+

( ) (m l l

3 2 3

2.6 TNH TON B TRUYN NG CHO M HNH

2.6.1 Tnh ton khp 1:
Cng sut ng c 1:

n1' = 5000(vng/pht)
I = 1.5( A)
V = 24(v)
P1 = I .V = 36( w)
Do : P1 = 1.M 1' M 1' =
-

P1

= 0.0216( N .m)

Sau khi qua hp s:

n1 = 30(vng / pht)
k =
-

n1' 500
=
M1 = k.M1' = 3.6( N.m)
n1
3

truyn ng t ng c 1 sang trc bi ta Chn b truyn ai rng c g hnh trn vi t

s truyn 1:1, modun m=1. L do chn b truyn ny v:
 Kch thc b truyn ai rng l nh gn, ph hp vi kch thc ca phn tay
my.

24

Thit k Robot gp bnh gas

 Kt hp vi c cu cng ai ngoi cho nn phng php gia cng khng cn chnh
xc v khong cch cc trc nh khi ta chn b truyn l bnh rng thng, hoc
bnh rng cn
 Khng c hin tng trt gia bnh ai v ai.
 Hiu sut truyn cao vo khong = 0.92 : 0.98 .
 Lc tc ng ln bi v trc ng c l nh.
 Cng sut truyn ng vo khong 36(W) l ph hp vi loi ny v b truyn ai
rng truyn ng c vi cng sut cao nht khong 200Kw.
-

Chn buly c cc thng s hnh hc:

 Buly dn ng:

ng knh vng chia:

ng knh l:

S rng:

d1 =18(mm)

1 = 6(mm)

z1 = 21

 Buly b dn:

ng knh vng chia:

ng knh l:

S rng:
-

d2 =18(mm)

2 = 6(mm)

z2 = 21

Khong cch trc thit k l: a = 70.5( m m )

2

(d d )
d +d
L = 2a + 1 2 + 1 2 = 197(mm)
2
4
Theo bng tiu chun, ta chn ai L=207 (mm), rng ca ai l 5(mm).

Hnh 2.11-C cu truyn ng ca khu 1

25

Thit k Robot gp bnh gas

-

Tng t nh trn, ta chn b truyn ng t n n encoder ca khp 1 bng b truyn

ai rng c g hnh trn vi t s truyn 1:1, modun m=1. Ta c cc thng s:
 Buly dn ng:

ng knh vng chia:

ng knh l:

S rng:

d1 =18(mm)

1 = 6(mm)

z1 = 21

 Buly b dn:

ng knh vng chia:

ng knh l:

S rng:
-

d2 =18(mm)

2 = 6(mm)

z2 = 21

Khong cch trc thit k l: a = 60.5( m m )

2

(d d )
d +d
L = 2a + 1 2 + 1 2 = 177(mm)
2
4
Theo bng tiu chun, ta chn ai L=184 (mm), rng ca ai l 5(mm).

Hnh 2.12-Encoder o s vng quay khu 1

2.6.2 Tnh ton khp 2 :
Tng t khp 1
-

Cng sut ng c 2:

26

Thit k Robot gp bnh gas

n2' = 5000(vng/pht)
I = 1.5( A)
V = 24(v)
P2 = I .V = 36( w)
Do : P2 = 2 .M 2' M 2' =
-

P2

= 0.0216( N .m)

Sau khi qua hp s:

n2 = 30(vng / pht)
k =
-

n2'
= 100 M 2 = k.M 2' = 2.16( N.m)
n2

Khp 2 chu tc ng ca moment ngoi lc ln nht khi khu 2 v khu 3 thng hng vi
nhau v chng nm dc theo phng ngang.

Hnh 2.13-Khu 2 v khu 3 thng hng theo phng ngang

l2
l
+ m3 g (l2 + 3 ) + mp (l2 + l3 )
2
2
0.16
0.16
= 0.2*10*
+ 0.2*10*(0.16 +
) + 1.2*(0.16 + 0.16)
2
2
= 1.024( N .m)

M ngoailuc = m2 g
M ngoailuc
M ngoailuc

Do: M 2 > M ngoailuc . Nn: ta chn b truyn ai rng c g hnh trn, modun m=1,
b dy ai 5(mm) v t s truyn 1:1. L do chn b truyn l:

27

Thit k Robot gp bnh gas

 Kch thc b truyn ai rng l nh gn, ph hp vi kch thc ca phn
khu 1 ca tay my.
 Kt hp vi c cu cng ai ngoi cho nn phng php gia cng khng cn
chnh xc v khong cch cc trc nh khi ta chn b truyn l bnh rng
thng, hoc bnh rng cn
 Khng c hin tng trt gia bnh ai v ai.
 Hiu sut truyn cao vo khong = 0.92 : 0.98 .
 Lc tc ng ln bi v trc ng c l nh.
 Cng sut truyn ng vo khong 36(W) l ph hp vi loi ny v b truyn
ai rng truyn ng c vi cng sut cao nht khong 200kW.
-

Chn bnh ai:

 Buly dn ng:

ng knh vng chia:

ng knh l:

S rng:

d1 =18(mm)

1 = 6(mm)

z1 = 21

 Buly b dn:

ng knh vng chia:

ng knh l:

S rng:
-

d2 =18(mm)

2 = 6(mm)

z2 = 21

Khong cch trc thit k l: a = 93( mm )

2

(d d )
d + d2
L = 2a + 1
+ 1 2 = 242( mm)
2
4
Theo bng tiu chun, ta chn ai L=246(mm), rng ca ai l 5(mm).

Hnh 2.14-C cu truyn ng ca khu 2

28

Thit k Robot gp bnh gas

2.6.3 Tnh ton khp 3:

Tng t nh khp 2
-

Cng sut ng c 3:
n3' = 5000(vng/pht)
I = 1.5( A)
V = 24(v)
P3 = I .V = 36( w)
Do : P3 = 3 .M 3' M 3' =

P3

= 0.0216( N .m)

Sau khi qua hp s:

n3 = 30(vng / pht)
k =
-

n3'
= 100 M 2 = k.M 2' = 2.16( N.m)
n3

Khp 3 chu tc ng ca moment ngoi lc ln nht khi khu 3 nm dc theo phng

ngang (hnh 2.13).

l
M ngoailuc = m3 g ( 3 ) + m p (l3 )
2
0.16
M ngoailuc = 0.2*10*(
) + 1.2*(0.16)
2
M ngoailuc = 0.352( N .m)
Do: M 3 > M ngoailuc . Nn: ta chn b truyn ai rng c g hnh trn, modun
m=1, b dy ai 5(mm) v t s truyn 1:1. L do chn b truyn l:
 Kch thc b truyn ai rng l nh gn, ph hp vi kch thc ca phn
khu 2 ca tay my.
 Kt hp vi c cu cng ai ngoi cho nn phng php gia cng khng cn
chnh xc v khong cch cc trc nh khi ta chn b truyn l bnh rng
thng, hoc bnh rng cn
 Khng c hin tng trt gia bnh ai v ai.
 Hiu sut truyn cao vo khong = 0.92 : 0.98 .
 Lc tc ng ln bi v trc ng c l nh.
 Cng sut truyn ng vo khong 36(W) l ph hp vi loi ny v b truyn
ai rng truyn ng c vi cng sut cao nht khong 200kW.

29

Thit k Robot gp bnh gas

-

Chn bnh ai:

 Buly dn ng:

ng knh vng chia:

ng knh l:

S rng:

d1 =18(mm)

1 = 6(mm)

z1 = 21

 Buly b dn:

ng knh vng chia:

ng knh l:

S rng:
-

d2 =18(mm)

2 = 6(mm)

z2 = 21

Khong cch trc thit k l: a = 93( mm )

2

(d d )
d + d2
L = 2a + 1
+ 1 2 = 242(mm)
2
4
Theo bng tiu chun, ta chn ai L=246(mm), rng ca ai l 5(mm)

Hnh 2.15-C cu truyn ng ca khu 3

30

Thit k Robot gp bnh gas

Chng 3. THIT K H MCH IU KHIN

3.1 S KHI HOT NG CA H MCH IU KHIN

Hnh 3.1-S khi h mch iu khin

3.2 MCH IU KHIN TRUNG TM

Hnh 3.2-Mch iu khin trung tm

31

Thit k Robot gp bnh gas

3.3 MCH ENCODER

Hnh 3.3-Mch Encoder

3.4-MCH DRIVER

Hnh 3.4-Mch Driver ng c

32

Thit k Robot gp bnh gas

3.5 MCH GIAO TIP VI MY TNH

Hnh 3.5-Mch giao tip vi my tnh

33

Thit k Robot gp bnh gas

Chng 4. GII THUT IU KHIN

4.1. S KHI H THNG ROBOT

Hnh 4.1-S khi h thng

4.2 GII THUT IU KHIN

4.2.1 Nguyn tc chung:
- Gi tr cc bin khp c set v v tr 0 sau mi ln khi ng hay tt tay my .
- Gi tr bin khp mong mun c gi t giao din matlab qua giao tip UART ti
mch vi iu khin .
T gi tr tng bin khp ny , vi iu khin trung tm s tnh gi tr u l duty cycle cp
cho tng ng c thng qua b iu khin PID .
4.2.2 Gii thut c Encoder Timer
-

Encoder s dng cho tay my l loi encoder tng i ca hng OMRON , m hiu
E6B2 - CWZ1X : s xung 2000 /1 vng . C cc pha A, B ,Z
Trong PIC 18F4431 c module QEI chuyn dng c encoder . Tuy nhin , do
nhm s dng n 3 encoder nn cn s dng cc module khc ca PIC c tn
hiu encoder.

34

Thit k Robot gp bnh gas

-

Xung pht ra t 2 pha A , B c dng nh hnh di :

Hnh 4.2-Biu xung pht ra t Encoder

-

T nguyn l trn ta c nguyn l c encoder bng interrupt v timer0 nh sau:

 Timer0 c t ch m khi c xung ngoi t thp ln cao , set gi tr cho
timer0 l 65535 . Khi c xung vo timer0 m ln v trn bin m .
 Khi hm ngt c gi ra v pha A c pht hin .
 ng thi khai bo ngt INT0 pht hin xung pha B .
 Nu ti thi im xy ra ngt , xung pha A c trc th chiu quay l 1 ,
ngc li chiu quay l 0
 Khi chiu quay l 1 tng bin m xung , nu chiu quay l 0 (tc l quay
ngc encoder li ) th tr bin m .
 Sau 1 khong thi gian c xung ( 20ms ) . Dng Timer1 to hm ngt m s
xung pht ra trong 1 n v thi gian tnh vn tc gc .
 Gii thut c thc hin tng t vi encoder 3 vi 2 hm ngt INT1 v
INT2 .

35

Thit k Robot gp bnh gas

4.3 THIT K CHNG TRNH IU KHIN
4.3.1 Lu gii thut

Hnh 4.3-Lu gii thut

36

Thit k Robot gp bnh gas

4.3.2 Chng trnh iu khin

Code c Encoder bng module QEI kt hp Power_PWM cho khu 1 :

#include <18F4431.H>
#FUSES HS, NOLVP, NOBROWNOUT, NOPROTECT
#USE DELAY(CLOCK=20M) //khai bao su dung thach anh 20M
#DEFINE postscale 3
#define time_base 1000
#define period 3999
#define compare 0
#define compare_postscale 0
#define dead_time 0
void main()
{ setup_power_pwm (PWM_FREE_RUN,postscale, time_base, period, compare,
compare_postscale, dead_time);
setup_power_pwm_pins(PWM_ODD_ON,PWM_ODD_ON,PWM_ODD_ON,PWM_ODD_ON );
set_tris_a(0b11100);
setup_qei(QEI_MODE_X2);
While(1)
{
set_power_pwm4_duty(4000);
output_high(PIN_D1);
n = qei_get_count(QEI_GET_POSITION_COUNT) ;
delay_ms(1);
}
}

Code c encoder bng Timer0 v INT0 :

#include <18F4431.H>
#FUSES HS, NOLVP, NOBROWNOUT, NOPROTECT
#USE DELAY(CLOCK=20M) //khai bao su dung thach anh 20M
#include <dpLCD420.c>
unsigned int16 countA=0, countB=0 ,flag=1,dem=0,gia_tri_tam=0,tempA=0;
float speed=0,gia_tri=0;
unsigned int1 A_high=0,B_high=0,dir=0;
#INT_TIMER1
// Ngat Timer 0 ;
void count_A_encoder()
{
set_timer1(65534);
A_high++;
countA++;
if (A_high==1)
{
setup_timer_1(RTCC_EXT_H_TO_L|RTCC_DIV_1);

37

Thit k Robot gp bnh gas

}
if (A_high==0)
{
setup_timer_1(RTCC_EXT_L_TO_H|RTCC_DIV_1);
}
}
#int_ext
void count_B_encoder()
{
B_high++;
if (B_high==1)
{
if (A_high==0)
{
countB++;
dir=1;
}
if (A_high==1)
{
countB--;
tempA++;
dir=0;
}
ext_int_edge(H_TO_L);
}
if (B_high==0)
{
if (A_high==0)
{
countB--;
dir=0;
}
if (A_high==1)
{
countB++;
dir=1;
}
ext_int_edge(L_TO_H);
}
}
#INT_TIMER1
// Ngat Timer 1 : 20ms
void rot_speed()
{
dem += 1;
speed = countA ;
gia_tri_tam += speed ;
if(dem==10){
// 200ms
flag = 1;
//
gia_tri = 0.0015708*5*gia_tri_tam ; //
gia_tri_tam = 0;
dem = 0 ;
countA=0;

38

Thit k Robot gp bnh gas

}
set_timer1(15535);
}
void main()
{
set_tris_d(0b00001) ;
lcd_init();
delay_ms(10);
set_tris_c(0b01001);//se
setup_timer_1(RTCC_EXT_L_TO_H|RTCC_DIV_1); // Khoi dong Timer0 lam Bo dem Xung tu Hight
to Low
set_timer1(65534);
//
setup_timer_1(t1_internal|t1_div_by_2); /
set_timer1(15535);
//
enable_interrupts(INT_TIMER0);
//
enable_interrupts(INT_TIMER1);
/
enable_interrupts(INT_EXT);
enable_interrupts(global);
//
ext_int_edge(L_TO_H);
while(true)
{
if (flag == 1)
{
lcd_gotoxy(1,1);
printf(lcd_putc,"%d %d %Ld %Ld",A_high,B_high,countB,tempA);
printf(lcd_putc,"
");
}
else
{
flag=0;
}
}
}

39

Thit k Robot gp bnh gas

Chng 5. THC NGHIM V KT QU

5.1 M HNH C THC T

Hnh 5.1-M hnh c thc t

5.2 MCH IN THC T

Hnh 5.2-Mch in thc t

40

Thit k Robot gp bnh gas

5.3 KT QU V HNG PHT TRIN TI
5.3.1 Kt qu :
-

Ph n c :
 th c hi n xong b n v l p v m hnh th c t
 M hnh hot ng n nh khi cp xung PWM cho 3 ng c .
Phn in :
 Thc hin xong s mch nguyn l vi iu khin , ngun , ngun cng sut
, driver v mch truyn dn UART.
 Thc hin xong mch thc t .
 Cc mch test v chy tt .
Phn lp trnh :
 c c tn hiu 3 encoder.
 PWM cho tng ng c
 Cha thc hin gii thut cho tay my hot ng t nh yu cu ra nh
trn v thi gian c hn .

5.3.2 Hng pht trin :

- Tip tc hon thin gii thut cho tay my .
- Hon thin giao din Matlab iu khin cho tay my
- Chy tay my trong iu kin thc t .

41

Thit k Robot gp bnh gas

TI LIU THAM KHO

[1]

Reza N. Jazar. (Februlary 19,2008), Theory of Applied Robotics: Kinematics, Dynamics,

and Control, RMIT University.

[2]

Nguyn Hu Lc . (2003). Thit k c kh vi AUTOCAD Mechanical .NXB Tp.H Ch

Minh

[3]

Nguyn Don Phc. L thuyt iu khin phi tuyn

[4]

Nguyn Th Phng H. L thuyt iu khin t ng, iu khin ti u

[5]

www.rcgroups.com

[6]

http://www.omron.com.vn/

[7]

http://www.mathwork.com/

42