n thit k h thng c in t

[TH.S:NG BO LM]

LI NI U
Trong s nghip cng nghip ha hin i ha t nc vn t ng ha c
vai tr c bit quan trng.
Nhm nng cao nng sut dy chuyn cng ngh , nng cao cht lng v kh
nng cnh tranh , ci thin iu kin lao ng , nng cao nng sut lao ng,
vn t ra cn c 1 h thng sn xut phi cquan tnh linh hot cao. Robot
cng nghip , dc bit nhng tay my robot l b phn quan trng to ra nhng
h thng .
Tay my Robot c mt trong qu trnh sn xut t nhiu nm trc , ngy nay
tay my robot dng nhiu lnh vc sn xut , sut pht t nhng u im m
tay my robot v c kt li trong qu trnh sn xut lm vic ,tay my c
nhng tnh nng m con ngi khng th c c , kh nng lm vic n nh ,
c th lm vic trong mi trng c hi Do vic u t nghin cu , ch
to ra nhng loi tay my robot phc v cho cng cuc t ng ha sn xut l
rt cncho hin ti v tng lai.
N THIT K H THNG C IN T gip chng em bc u lm quen vn
ct li v c bn nht ca robot , gip cho sinh vin c th h thng ha li
mn hc c hc . ng thi gip sinh vin lm quen cng vic thit k v
lm n tt nghip sau ny.
Trong qu trnh hc tp d c gng hon thnh n vi s tp trung , s n
lc mt cch tt nht vi s hng dn c th thy trong b mn C S THIT
K MY .Nhng do hiu bit hn ch vi cha c nhiu kinh nghim trong thc
tin nn n ny khng trnh khi kh nng thiu xt . Vy em rt mong s
sa cha v gp ca thy c em rt ra kinh nghim v b xung thm kin
thc cho mnh.
Em xin chn thnh cm n!
Sinh vin thc hin:
Nguyn Vn Mnh.

CHNG1:TNG QUAN V ROBOT CNG

NGHIP
Nguyn Vn Mnh Kt.C in t 2 k561

 n thit k h thng c in t
D

[TH.S:NG BO LM]

I.S LC QU TRNH PHT TRIN CA ROBOT CNG

NGHIP.

Hnh 1.1: hnh nh robot cong nghip

Thut ng Robot xut pht t ting Sec (Czech) Robota c ngha l cng
vic tp dch trong v kch Rossums Universal Robots ca Karel Capek, vo nm
1921. Trong v kch ny, Rossum v con trai ca ng ta ch to ra nhng
chic my gn ging vi con ngi phc v con ngi. C l l mt gi
ban u cho cc nh sng ch k thut v nhng c cu, my mc bt chc cc
hot ng c bp ca con ngi. u thp k 60, cng ty M AMF (American
Machine and Foundry Company) qung co mt loi my t ng vn nng v
gi l Ngi my cng nghip (Industrial Robot). Ngy nay ngi ta t tn
ngi my cng nghip (hay robot cng nghip) cho nhng loi thit b c dng
dp v mt vi chc nng nh tay ngi c iu khin t ng thc hin
mt s thao tc sn xut.
Theo vin nghin cu Robot ca M xut nh ngha: Robot cng nghip l
tay my vn nng hot ng theo chng trnh v c th lp trnh li hon
thnh v nng cao hiu qu hon thnh cc nhim v khc nhau trong cng
nghip, nh vn chuyn nguyn vt liu, chi tit, sn phm, dng c hoc cc
thit b chuyn dung khc.
V mt k thut, nhng robot cng nghip ngy nay, c ngun gc t hai
lnh vc k thut ra i sm hn l cc c cu iu khin t xa (Teleoperators)
v cc my cng c iu khin s (NC - Numerically Controlled machine tool).
Cc c cu iu khin t xa (hay cc thit b kiu ch-t) pht trin mnh
trong chin tranh th gii ln th hai nhm nghin cu cc vt liu phng x.
Ngi thao tc c tch bit khi khu vc phng x bi mt bc tng c mt
hoc vi ca quan st c th nhn thy c cng vic bn trong. Cc c cu
iu khin t xa thay th cho cnh tay ca ngi thao tc. N gm c mt b
Nguyn Vn Mnh Kt.C in t 2 k562

 n thit k h thng c in t
D

[TH.S:NG BO LM]

kp bn trong (t) v hai tay cm bn ngoi (ch). C hai, tay cm v b kp,

c ni vi nhau bng mt c cu su bc t do to ra cc v tr v hng
tu ca tay cm v b kp. C cu dng iu khin b kp theo chuyn
ng ca tay cm. Vo khong nm 1949, cc my cng c iu khin s ra i,
nhm p ng yu cu gia cng cc chi tit trong ngnh ch to my bay. Nhng
robot u tin thc cht l s ni kt gia cc khu c kh ca c cu iu khin
t xa vi kh nng lp trnh ca my cng c iu khin s.
Di y chng ta s im qua mt s thi im lch s pht trin ca ngi
my cng nghip. Mt trong nhng robot cng nghip u tin c ch to l
robot Versatran ca cng ty AMF, M. Cng vo khong thi gian ny M xut
hin loi robot Unimate -1900 c dng u tin trong k ngh t. Tip theo
M, cc nc khc bt u sn xut robot cng nghip : Anh -1967, Thu in v
Nht -1968 theo bn quyn ca M; CHLB c -1971; Php - 1972; - 1973. . .
Tnh nng lm vic ca robot ngy cng c nng cao, nht l kh nng nhn
bit v x l. Nm 1967 trng i hc tng hp Stanford (M) ch to ra
mu robot hot ng theo m hnh mt-tay, c kh nng nhn bit v nh
hng bn kp theo v tr vt kp nh cc cm bin. Nm 1974 Cng ty M
Cincinnati a ra loi robot c iu khin bng my vi tnh, gi l robot T3
(The Tomorrow Tool : Cng c ca tng lai). Robot ny c th nng c vt c
khi lng n 40 KG. Ngy nay, khoa hc v Robot Robotics pht trin mnh
m, ngy cng hin i ng dng trong rt nhiu lnh vc khc nhau: Cng
nghip, thm him v tr, gio dc, y t.

II.CC KHI NIM V NH NGHA V ROBOT

CNG NGHIP
Robot cng nghip c th c nh ngha theo mt s tiu chun sau:
Theo tiu chun AFNOR ca Php: Robot cng nghip l mt c cu chuyn
ng t ng c th lp trnh, lp li cc chng trnh, tng hp cc chng trnh
t ra trn cc trc ta , c kh nng nh v, nh hng, di chuyn cc i
tng vt cht nh chi tit, o c, g lp theo nhng hnh trnh thay i
c chng trnh ha nhm thc hin cc nhim v cng ngh khc nhau.

Theo tiu chun RIA ca M (Robot institute of America): Robot l mt tay my

vn nng c th lp li cc chng trnh, c thit k di chuyn vt liu, chi
tit, dng c, hoc cc thit b chuyn dng thng qua cc chng trnh chuyn
ng c th thay i hon thnh cc nhim v khc nhau.
Theo tiu chun TOCT 25686-85 ca Nga: Robot cng nghip l mt my t
ng, c t c nh hoc di ng c, lin kt gia mt tay my v mt h
thng iu khin theo chng trnh, c th lp i lp li hon thnh cc chc
nng vn ng v iu khin trong qu trnh sn xut.

Nguyn Vn Mnh Kt.C in t 2 k563

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Do , robot cng nghip c th c hiu l nhng thit b t ng linh hot,

thc hin cc chc nng lao ng cng nghip ca con ngi di mt h thng
iu khin theo nhng chng trnh c lp trnh sn.
Vi c im c th lp trnh li c, robot cng nghip l thit b t ng ha
v ngy cng tr thnh b phn khng th thiu c ca cc h thng sn xut
linh hot. V vy, robot cng nghip tr thnh phng tin hu hiu t ng
ha, nng cao nng sut lao ng v gim nh cho con ngi nhng cng vic
nng nhc, c hi di s gim st ca con ngi.

III.CU TRC V PHN LOI ROBOT CNG NGHIP

3.1.Cu trc chung ca robot cng nghip:
Mt Robot cng nghip c cu thnh bi cc h thng sau:
+C cu chp hnh(Actuator) to chuyn ng cho cc khu ca tay
my.Ngun ng lc ca cc c cu chp hnh l ng c cc loi: in,thy
lc,kh nn..
+H thng cm bin(Sensor) gm cc sensor v cc thit b chuyn i tn
hiu.Cc Robot cn h thng sensor bn trong nhn bit trng thi ca bn
thn cc c cu ca Robot v cc sensor ngoi nhn bit trng thi ca mi
trng tc ng .
+H thng iu khin(Controller) hin nay thng l my tnh gim st v
iu khin hot ng ca Robot.
+Tay my(Manipulator) l c cu c kh gm cc khu,cc khp.Chng hnh
thnh cnh tay to chuyn ng c bn,c tay to nn s kho lo linh hot
v bn tay (End Effector) trc tip hon thnh cc thao tc trn i tng.

3.2. Bc t do ca robot :
Bc t do ca Robot l i lng c trng cho kh nng linh hot ca chuyn
ng ca Robot. Robot c s bc t do cng ln th kh nng linh hot cng cao
nhng cu trc Robot cng tr nn phc tp. C cu tay ca robot phi c cu
to sao cho khu cui phi c v tr v theo mt hng nht nh no v d
dng di chuyn d dng trong vng lm vic. Mun vy c cu tay ca robot phi
t c mt s bc t do chuyn ng. tnh s bc t do ca robot th c
nhiu cng thc c xut, di y ta a ra cch tnh da vo nh l
Gruebler. nh l ny c s dng ph bin trong nhiu ti liu hin nay.
Theo Gruebler th bc t do f c tnh theo cng thc:

Nguyn Vn Mnh Kt.C in t 2 k564

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Trong :

: L s bc t do ca c cu.
: Bc t do ca mt vt rn khng chi lin kt trong khng gian lm vic ca robot ( = 3 ng vi
khng gian lm vic trong mt phng, = 6 ng vi khng gian lm vic trong khng gian).

: S khu ( k c gi c nh).

: S bc t do ca khp th i.
: Tng s khp ca c cu.

: S bc t do tha
Hnh 1.2. Robot Puma 560 c 6 bc t do

3.3. Phn loi Robot:

3.3.1Phn loi theo phng php iu khin:
C 2 kiu iu khin robot: iu khin h v iu khin kn.
iu khin h: dng truyn ng bc (ng c in hoc ng c thy lc, kh
nn) m qung ng hoc gc dch chuyn t l vi xung iu khin. Kiu ny
n gin nhng cho chnh xc thp.
iu khin kn: (iu khin kiu servo) s dng tn hiu phn hi v tr tng
chnh xc iu khin. C hai kiu iu khin servo: iu khin im-im v
iu khin theo ng (contour).
Kiu iu khin im-im: phn cng tc dch chuyn t im ny n im
kia theo ng thng vi tc khng cao. Kiu iu khin ny thng c
dng trn cc Robot hn im, vn chuyn, tn inh v bn inh.
iu khin contour: m bo cho phn cng tc dch chuyn theo qu o bt
k, vi tc iu khin c. C th gp kiu iu khin ny trn cc Robot hn h
quang v phun sn.

Nguyn Vn Mnh Kt.C in t 2 k565

 n thit k h thng c in t
D

[TH.S:NG BO LM]

3.3.2. Phn loi theo ng dng:

Da vo nhng ng dng ca robot trong sn xut ta c nhng loi robot sau:
robot sn, robot hn, robot lp rp, robot dng trong ngnh dch v, robot
chuyn phi.

Hnh 3 : Robot hn cng nghip

nghnh dch v

Hnh 4: Robot trong

IV. NG DNG CA ROBOT CNG NGHIP TRONG

SN XUT
Trn th gii:
Hin nay trn th gii, do nhu cu s dng robot ngy cng nhiu trong cc qu
trnh sn xut phc tp vi mc ch gp phn nng cao nng sut dy chuyn
cng ngh, gim gi thnh, nng cao cht lng, v nng cao kh nng cnh
tranh ca sn phm ng thi ci thin iu kin lao ng, nn robot cng
nghip cn c nhng kh nng thch ng tt v thng minh hn vi nhng cu
trc n gin v linh hot.

Nguyn Vn Mnh Kt.C in t 2 k566

 n thit k h thng c in t
D

[TH.S:NG BO LM]

C th k n mt s ng dng in hnh ca robot trn th gii nh:

Robot song song dng trong phn loi v ng gi sn phm: IRB 660 Flex
Palletizer, IRB 340 FlexPicker, IRB 260 FlexPicker. Cc robot ny c th gp ln
lt cc hp vc xin bi lit t bng ti v t n vo thng gm 20 hp mt
cch chnh xc.
Robot dng trong cng ngh p phun nha: IRB 6650 ca hng ABB c th thao
tc nhanh, d dng ly sn phm ra khi khun v tr tch khun, gim st, lm
sch, iu khin cht lng da trn camera
Ti Vit Nam:
Nc ta ng dng ca robot cng nghip rt a ng, ty vo nhng nghnh,
cng vic khc nhau m ta c th p dng nhng robot cng nghip ring bit.
Di y l mt s nghnh trong h thng sn xut m p dng robot cng
nghip.
Cng nghip c: robot lm nhim v rt kim loi nng chy vo khun, ct
mp tha, lm sch vt c hoc lm tng bn vt c bng cch phun ct.
Ngnh gia cng p lc: cc qu trnh hn v nhit luyn thng bao gm nhiu
cng vic c hi v nhit cao, iu kin lm vic kh nng n, d gy mt
mi nht l trong cc phn xng rn dp.

CHNG II: THIT K C CU ROBOT CNG

NGHIP
THIT K M HNH 3D BNG PHN MM SOLIDWORKS

Nguyn Vn Mnh Kt.C in t 2 k567

 n thit k h thng c in t
D

[TH.S:NG BO LM]

M hnh tng th robot PUMA

1.Thit k chi tit khu v khu th nht:

M hnh h thng dn ng khu th nht.

Nguyn Vn Mnh Kt.C in t 2 k568

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Hnh chiu v hnh chiu trc o chi tit robot.

Hnh chiu v hnh chiu trc o chi tit np bi , ng c servo v khp ni

Hnh chiu v hnh chiu trc o chi tit a cn , bi ,bulong

Nguyn Vn Mnh Kt.C in t 2 k569

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Hnh chiu v hnh chiu trc o chi tit trc khu 1, trc gim tc cp 1 ,trc ni
BR ch ng

H thng bnh rng gim tc

Nguyn Vn Mnh Kt.C in t 2 k5610

 n thit k h thng c in t
D

[TH.S:NG BO LM]

H thng bnh rng gim tc

Nguyn Vn Mnh Kt.C in t 2 k5611

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Bnh rng, np p cha bi ,lp p

M hnh phn u khu th nht

2.M hnh khu th 2

3.M hnh khu th 3

Nguyn Vn Mnh Kt.C in t 2 k5612

 n thit k h thng c in t
D

[TH.S:NG BO LM]

4.M hnh khu th 4

Nguyn Vn Mnh Kt.C in t 2 k5613

 n thit k h thng c in t
D

[TH.S:NG BO LM]

5.M hnh khu th 5

6.M hnh khu th 6

Nguyn Vn Mnh Kt.C in t 2 k5614

 n thit k h thng c in t
D

[TH.S:NG BO LM]

CHNG III: GII BI TON NG HC

ROBOT PUMA
1.Thit lp h phng trnh ng hc ca Robot:
a.Bc t do ca robot
Bc t do ca robot l kh nng lm vic t do, c lp ca robot.
tnh s bc t do chuyn ng ta s dng cng thc:

Trong :

: L s bc t do ca c cu.

: Bc t do ca khng gian trong tay my thc hin chuyn ng (

= 3 ng vi khng gian lm vic trong mt phng, = 6 ng vi khng
gian lm vic trong khng gian).

n: S khu ng ca Robot.

: S bc t do ca khp th i.

k: S khp ca c cu.

: S rng buc trng.

: S bc t do tha.

Vi robot trong n ny em nghin cu s bc t do l:

= 6(5- 5) + 6 + 0 + 0 = 6.
b.Phng php Denavit Hartenberg
Nguyn Vn Mnh Kt.C in t 2 k5615

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Theo DH, ti mi khp ta gn mt h trc to , quy c v cch t h to

ny nh sau:
-

Trc
.

Trc

zi c lin kt vi trc ca khp th i+1. Chiu ca zi c chn tu

xi c xc nh l ng vung gc chung gia trc khp i v khp

i+1, hng t im trc ca khp i ti khp i+1. Nu hai trc song song
xi c th chn bt k l ng vung gc chung hai trc khp. Trong
x
z z
trng hp hai trc ny ct nhau, i c xc nh theo chiu ca i i 1
th

( hoc quy tc bn tay phi).

-

Trc

yi c xc nh theo xi v zi theo quy tc bn tay phi.

Hnh 3.1 Quy tc bn tay phi

Cc thng s ng hc Denavit Hartenberg c xc nh nh sau:

: khong cch Oi-1 v Oi theo trc zi-1.

: gc gia 2 ng vung gc chung. L gc quay quanh trc z i-1

trc xi-1 chuyn n trc xi theo qui tc bn tay phi.

: gc xoay a trc zi-1v zi quanh zi theo quy tc bn tay phi.

: khong dch chuyn gia 2 trc khp ng k nhau.

Nguyn Vn Mnh Kt.C in t 2 k5616

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Hnh 3.2 Cc thng s ca khu

c.Phng trnh ng hc
Ma trn bin i ta thun nht Denavit - Hartenberg :

Ma trn bin i ta thun nht

biu din trng thi khu thao tc c

th xc nh t t cu trc ng hc robot.
c th nhn c bng cch
nhn lin tip cc ma trn bin i ta thun nht ng vi cc php dch
chuyn h ta t h trc c inh ti h trc ta gn vi EF :

Nguyn Vn Mnh Kt.C in t 2 k5617

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Phuong trnh ng hc robot dng ma trn

Cc phn t ca ma trn l hm ca ta khp v ta thao tc :

Nguyn Vn Mnh Kt.C in t 2 k5618

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Phng trnh (3.12 ) c gi l phng trnhng hc dng ma trn ca robot.

N bao gm 6 phng trnh v hng, trong c 9 phng trnh tm thng.
Cc ma trn

gc trn bn phi cho 3 phng trnh c lp, biu din v tr

im thao tc thuc khu thao tc. Cc ma trn

gc trn bn tri cho 9
phng trnh, m t v hng khu thao tc. Tuy nhin do cc iu kin trc giao
nn ch c 3 phng trnh l c lp.
Phng trnh (3.12 ) c s dng nhiu trong cc bi ton ng hc thun v
ngc.

2. Ma trn Denavit Hartenberg

Cc thng s ng hc Denavit Hartenberg c xc nh nh sau:

: khong cch Oi-1 v Oi theo trc zi-1.

: gc gia 2 ng vung gc chung. L gc quay quanh trc z i-1

trc xi-1 chuyn n trc xi theo qui tc bn tay phi.

: gc xoay a trc zi-1v zi quanh zi theo quy tc bn tay phi.

: khong dch chuyn gia 2 trc khp ng k nhau.

Nguyn Vn Mnh Kt.C in t 2 k5619

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Hnh 2.1 H trc ta ca robot trong m hnh 3D

Bng thng s D-H ca Robot:

Khu

di

ai

Khu 1

Khu 2

d2

Khu 3

a3

90

Khu 4

d4

90

Khu 5

90

Khu 6

d6

a2

90
0

3. Bi ton ng hc thun robot:

Mc tiu ca bi ton ng hc thun l tnh ton v tr v hng ca khu thao tc di dng hm
ca cc bin khp.

Nguyn Vn Mnh Kt.C in t 2 k5620

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Trong :

l v tr bn kp trong h ta c nh. Tp hp cc vector X

Trong khng gian gi l khng gian thao tc ca robot.

q=[ 1, 2, 3, 4, 5, 6 ]

l vector ta suy rng ca cc bin khp. Tp cc

vector q xc nh nn khng gian khp hay cn gi l khng gian cu hnh ca ca robot.

Ta s tm hng v v tr ca bn kp robot t ma trn bin i thun nht
Ma trn bin i ta thun nht Denavit Hartenberg :

i-1Ai=
Thay cc gi tr trong bng thng s DH vo cng thc (1.3) ta c cc ma trn bin i DH gia
cc trc nh sau:
Ma trn Denavit-Hartenbergca khu 1:

(1)

Ma trn Denavit-Hartenbergca khu 2:

Nguyn Vn Mnh Kt.C in t 2 k5621

(2)

 n thit k h thng c in t
D
-

[TH.S:NG BO LM]

Ma trn Denavit-Hartenbergca khu 3:

=
-

(3)

Ma trn Denavit-Hartenbergca khu 4:

=
-

Ma trn Denavit-Hartenbergca khu 5:

=
-

(5)

Ma trn Denavit-Hartenbergca khu 6:

=
-

(4)

Thit lp ma trn khu thao tc :

Nguyn Vn Mnh Kt.C in t 2 k5622

(6)

 n thit k h thng c in t
D

[TH.S:NG BO LM]

=
Vi:

Nguyn Vn Mnh Kt.C in t 2 k5623

(7)

 n thit k h thng c in t
D

[TH.S:NG BO LM]

- Ma trn
cho ta bit hng v v tr ca khu thao tc trong h ta c nh hay ni cch khc
l v tr ca im tc ng cui v hng ca h ta ng gn vo khu ti im tc ng cui
trong h ta c nh. V th n cn c biu din qua thng s cc bin khp ta tm gi l q i.
Trong bi ton c th th n l cc khp xoay i, vi i=16.

- Khi , ma trn (7) c k hiu thnh

S dng cc gc Cardan xc nh hng ca vt rn. Ta gi

l gi tr m t

trc tip v tr v hng ca EXEYEZE so vi h ta O0Z0Y0Z0. Trong :

im tc ng cui E v

[ , , ]

l cc ta

l cc gc quay Cardan ca EX EYEZE so vi h ta

O0Z0Y0Z0. Do cc ta thao tc u l hm ca thi gian. Nn ta c th biu din:

Vi: 0AE(t) l ma trn Cardan m ta hng EXEYEZE so vi h ta O0Z0Y0Z0.

l vect m t v tr ca im tc ng cui trong h ta O0Z0Y0Z0.

Nguyn Vn Mnh Kt.C in t 2 k5624

 n thit k h thng c in t
D

[TH.S:NG BO LM]

T ta c PT ng hc ROBOT c dng:
n = 6 v c cu Robot c 6 khu:

H phng trnh ng hc:

f 1 =x ( q )x e = 0,
f 2= y ( q ) y e = 0,
f 3 =z ( q )z e = 0,
f 4=C 11 ( q ) C11 ( , , ) = 0,
f 5 =C12 ( q ) C12 ( , , ) = 0,
f 6=C13 ( q )C13 ( , , ) = 0
Gi s cc bin khp c quy lut chuyn ng :

Nguyn Vn Mnh Kt.C in t 2 k5625

. Vi

 n thit k h thng c in t
D

[TH.S:NG BO LM]

S dng trng trnh tnh trn phn mm MAPLE:

Ta nhn c ta cc im thao tc cui:

Vn tc im thao tc cui

Nguyn Vn Mnh Kt.C in t 2 k5626

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Gia tc im thao tc cui

th thu c ca im khu thao tc cui s dng phn mm MAPLE:

th Xe,Ye,zE theo t

Nguyn Vn Mnh Kt.C in t 2 k5627

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Qy o chuyn ng ca im tc ng cui E trong khng gian

Hng gc alpha ca khu thao tc gii theo phng php Newton RS

Nguyn Vn Mnh Kt.C in t 2 k5628

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Hng gc beta ca khu thao tc gii theo phng php Newton RS

Hng gc eta ca khu thao tc gii theo phng php Newton RS

Nguyn Vn Mnh Kt.C in t 2 k5629

 n thit k h thng c in t
D

[TH.S:NG BO LM]

th vn tc ca im tc dng cui vE

th gia tc ca im tc dng cui aE

Vn tc gc ca cc khu:
Tnh vn tc gc ca khu th i ca Robot da trn cng thc tnh vn tc gc tuyt i ca vt rn
thng qua cc ma trn Cosin ch hng ca n:

Nguyn Vn Mnh Kt.C in t 2 k5630

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Ma trn omega sng tuyt i ca khu thao tc:

=>
Gia tc gc tuyt i cacc khu :
=dw/dt

4.Bi ton ng hc ngc Robot:

4.1.Gii theo phng php gii tch:
4.2.Gi theo phng php s Newton- Raphson:
4.2.1.Thut gii Newton Raphson:
-

Gi s gii h phi tuyn sau :

M rng t phng php Newton cho hm mt bin, ta bt u t vic o hm tng phn mi

phng trnh

ta xp x nh sau:

(1)
Vi
l gi tr ca nghim ln lp th k. Ta mong mun
(1) c th vit di dng ma trn:

Nguyn Vn Mnh Kt.C in t 2 k5631

, do h n phng trnh

 n thit k h thng c in t
D

Trong

[TH.S:NG BO LM]

l ma trn Jacobi kch thc

(2)
ti bc lp th :

(3)
l vector thng d kch thc

ti bc lp th :

(4)
v cui cng:
(5)
Do nghim ca h ti bc th

l:

(6)
S dng nhng phng trnh ny, thut ton Newton tm li gii cho h phng trnh rt n gin:
1. on m nghim ( y ta chn nghim y bng phng php gii gn ng trong maple hay
ni cch khc dng hm fsolve )
2. Tnh ma trn Jacobi v vector thng d.
3. Gii phng trnh (2) bng cc phng php trong i s tuyn tnh
4. Cp nht gi tr

theo (6).

5. Lp li bc 2 nu li gii cha hi t.

4.2.2.kt qu sau khi p dng phng php Newton -Raphson gii bi ton
ng hc ngc robot puma
H phng trnh ng hc:

f 1 =x ( q )x e = 0,
f 2= y ( q ) y e = 0,
f 3 =z ( q )z e = 0,
f 4=C 11 ( q ) C11 ( , , ) = 0,
Nguyn Vn Mnh Kt.C in t 2 k5632

 n thit k h thng c in t
D

[TH.S:NG BO LM]

f 5 =C12 ( q ) C12 ( , , ) = 0,
f 6=C13 ( q )C13 ( , , ) = 0,
Cc thng s qu o hoa 4 cnh :

Kt qu bi ton :

th gc theta 1 ca khu th nht

Nguyn Vn Mnh Kt.C in t 2 k5633

 n thit k h thng c in t
D

[TH.S:NG BO LM]

th gc theta 2 ca khu th hai

th gc theta 3 ca khu th ba

Nguyn Vn Mnh Kt.C in t 2 k5634

 n thit k h thng c in t
D

[TH.S:NG BO LM]
th gc theta 4 ca khu th t

th gc theta 5 ca khu th nm

th gc theta 6 ca khu th su

Nguyn Vn Mnh Kt.C in t 2 k5635

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Kt qu tnh ton thay ngc vo phng trnh xe,ye

Qu o mong mun

CHNG IV: KHO ST NG LC HC ROBOT

PUMA
1. Biu thc ng nng v th nng ca robot
Trong tnh ton ng hc robot, xc nh v tr ca cc khu ta ch cn s dng h ta c
nh v h ta khp. Trong bi ton ng lc hc robot ta cn thm mt h ta na l h ta
Nguyn Vn Mnh Kt.C in t 2 k5636

 n thit k h thng c in t
D

[TH.S:NG BO LM]

khu. H ta khu l h quy chiu gn vi vt rn, thng c gc trng vi khi tm C i ca vt rn,

cc trc hng theo cc trc qun tnh chnh ca vt rn.

Trong hnh (4.1) h

l h ta khp,

l h ta khu.

Hnh
4.1 H ta khu
Gi s robot l h holonom c p vt rn v r lin kt. Khi s bc t do ca h l n = 6p r. K hiu

cc bin khp ca h l
V tr khu th i c xc nh bi:

Ta khi tm ca khu:

Ma trn cosin ch hng ca khu:

Xt robot c lin kt hlnm gi v dng, khi :

101\* MERGEFORMAT (.)

Theo nh ngha cc ma trn Jacobi tnh tin v ma trn Jacobi quay c xc nh bi cng thc:

202\* MERGEFORMAT (.)

Trong

l vect i s ng vi gc quay

ca vt rn th i, quay quanh trc quay tc thi. Vn

tc khi tm v vn tc gc ca vt rn c xc nh bng cc cng thc sau:

Nguyn Vn Mnh Kt.C in t 2 k5637

 n thit k h thng c in t
D

[TH.S:NG BO LM]

303\* MERGEFORMAT (.)

404\* MERGEFORMAT (.)

a

ng nng robot

Biu thc ng nng ca vt rn c xc nh bi biu thc:

505\* MERGEFORMAT (.)

Trong

l ma trn tenx qun tnh khi ca vt rn i vi h quy chiu i qua khi tm C v

song song vi h quy chiu c nh. Mi lin h gia ma trn tenx qun tnh khi
quan tnh khi

v ma trn

l:

606\* MERGEFORMAT (.)

Vi

l ma trn tenx qun tnh i vi h quy chiu gn lin vi khu. Do

hng s v nu ta chn h ta khu l h qun tnh chnh th

l ma trn

c dng ng cho.

T y suy ra biu thc ng nng ca robot c dng:

707\* MERGEFORMAT (.)

Thay (1.3) v (1.4) vo (1.7) ta c

808\* MERGEFORMAT (.)

Nguyn Vn Mnh Kt.C in t 2 k5638

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Nu ta a vo k hiu:

909\* MERGEFORMAT (.)

Th biu thc ng nng robot (1.8) c dng:

10010\* MERGEFORMAT (.)

Ma trn M(q) l ma trn vung cp n, v c gi l ma trn khi lng suy rng ca robot.
b

Th nng trng lc ca robot

Th nng trng lc ca mi khu ca rbot c xc nh bi biu thc:

11011\* MERGEFORMAT (.)

Trong :

l vecto gia tc trng trng, xt trng hp

l trc thng ng, hng ln, ta

c:

l ta khi tm th i

Th nng ca trng lc ca rbot c dng:

12012\* MERGEFORMAT (.)

2. Thit lp dng thc Lagrange loi 2
Xut pht t phng trnh Lagrange loi 2:

i = 1,2,,n
Ta suy ra phng trnh Lagrange loi 2 dng ma trn:

Nguyn Vn Mnh Kt.C in t 2 k5639

13013\* MERGEFORMAT (.)

 n thit k h thng c in t
D

[TH.S:NG BO LM]

14014\* MERGEFORMAT (.)

Trong :

15015\*

MERGEFORMAT (.)
Trong k thut robot, ngi ta thng k hiu momen ng c bng
T biu thc ng nng (1.10) ta c:

T y ta tnh c cc o hm:

16016\* MERGEFORMAT (.)

17017\* MERGEFORMAT (.)

T biu thc th nng trng lc (1.12), ta c:

18018\* MERGEFORMAT (.)

Th (1.16), (1.17) v (1.18) vo phng trnh (1.14) ta c:

Nguyn Vn Mnh Kt.C in t 2 k5640

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Ta a vo k hiu:

Do M(q) l ma trn i xng nn

, nn c th vit:

19019\* MERGEFORMAT (.)

Bng cch thay i ch s chy trong tng sigma kp nh sau:

Cui cng ta nhn c:

Nguyn Vn Mnh Kt.C in t 2 k5641

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Trong ta nh ngha:

, (s hng ny c tn l Christoffel symbols dng

th nht).
T ta c phng trnh vi phn nh sau:

20020\* MERGEFORMAT (.)

3. Bin i phng trnh vi phn chuyn ng ca robot

nhn c cc phng trnh vi phn chuyn ng ca robot dng quen thuc trong cc
sch v robot cc nc, t phng trnh (1.20) ta thc hin mt s bin i nh sau:
Ta ua vo k hiu:

21021\* MERGEFORMAT (.)

T suy ra:

22022\* MERGEFORMAT (.)

Thay (1.22) vo (1.21) ta c:

23023\*
MERGEFORMAT (.)
Nguyn Vn Mnh Kt.C in t 2 k5642

 n thit k h thng c in t

[TH.S:NG BO LM]

H phng trnh vi phn chuyn ng ca tay my (1.23) c th vit di dng ma trn sau:

24024\* MERGEFORMAT (.)

Trong :

l ma trn khi lng suy rng

l ma trn li tm Coriolis.
Thnh phn
Xt biu thc:

i din cho lc qun tnh ly tm v qun tnh Coriolis tc dng ln robot.

Khi

tng ng vi hiu ng ly tm

Khi

tng ng vi hiu ng Coriolis.

4. p dng vo m hnh robot PUMAta c:

Cc tham s ng lc hc sau xt trn h gn vi khu (coi cc khu l thanh mnh ng cht
cc biu thc I i vi khi tm

trong h

song song vi h ta khu).

Khu
1

Nguyn Vn Mnh Kt.C in t 2 k5643

 n thit k h thng c in t

[TH.S:NG BO LM]

Chn ta suy rng tng i

T cc ma trn denavit-Hartenberg ta suy ra ma trn cosin ch hng ca cc khu. l ma trn
cp 3 gc trn bn tri ca ma trn T:

T cc ma trn cosin ch hng , ta xc nh c ton t sng ca cc vec t vn tc

gc ca cc khu:

Suy ra vn tc gc ca cc khu:

Ta v tr khi tm ng vi tng khu ( ta tng i):

Ma trn v tr ca h ta gn vi tng khu so vi h ta c nh ( l ma trn 3x1 gc trn bn

phi ca ma trn T) l VT
V tr trng tm ca cc khu so vi h ta c nh l:

Ma trn ten x qun tnh ca vt rn i vi khi tm ca n trong h ta

c nh:

Nguyn Vn Mnh Kt.C in t 2 k5644

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Tnh ng nng:

Tnh th nng :

Phng trnh ng lc hc Lagrange loi II:

1, n

i=

Nguyn Vn Mnh Kt.C in t 2 k5645

 n thit k h thng c in t
D

Nghim

[TH.S:NG BO LM]

chnh l lc/ momen iu khin ng vi ta suy rng qi.

Bin i phng trnh vi phn chuyn ng ca robot :

H n phng trnh vi phn chuyn ng ca robot c th vit di dng
sau:

Trong :
l ma trn khi lng suy rng
l ma trn ly tm-Coriolis.
Thnh phn
dng ln robot.

i din cho lc qun tnh li tm v qun tnh Coriolis tc

Kt qu ca bi ton do c kch thc ln nn khng th th hin trn y, kt

qu bi ton ng lc hc c p dng trong bi ton iu khin

Nguyn Vn Mnh Kt.C in t 2 k5646

 n thit k h thng c in t
D

[TH.S:NG BO LM]

CHNG V: THIT K QU O CHUYN

NG CHO ROBOT PUMA
1. Qy o trong khng gian khp
Ta chn 2 im
bt k trong khng gian lm vic,
t phng trnh ng hc ngc ta tnh phn trn ta xc nh c gc khp
ti hai im A, B l
T hai b gc khp tm c ta i thit k qu p q i (i=1:6) bin
thin t qia n qib theo qu o bc 3 (p ng v mt tc gc khp) trong
vng tc(s). Nh vy

Vi cc iu kin nh sau:
(Thng thng ta yu cu vn tc gc khp ti im u v im cui
bng 0 ).
T cc iu kin trn ta thay vo (3.1) gii ta c cc h s.

Nguyn Vn Mnh Kt.C in t 2 k5647

 n thit k h thng c in t
D

[TH.S:NG BO LM]

V d : Robot di chuyn A(300,400,120) n B(500,300,220) trong

5(s) t phng trnh ng hc ngc vi hng ca khu thao tc c coi l
kh i

l :

Ta c:
+

v tr cc bin khp ti thi im u l :

v tr cc bin khp ti thi im cui l :

Phng trnh qu o chuyn ng cc bin khp tnh c l :

+ khu th nht

+ khu th hai

+ khu th ba

+ khu th t

+ khu th nm

+ khu th su

Nguyn Vn Mnh Kt.C in t 2 k5648

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Hnh 3.1 th qy o cc bin khp

CHNG V: THIT K IU KHIN

ROBOT PUMA
H thng iu khin nu di y s thit k theo lut iu khin PD. Khi
thit k h thng iu khin ta b qua ng hc ca c cu chp hnh, qun tnh
ng c. Nh vy chc nng ca b iu khin l to ra mt moomen cn thit
truyn ng khp robot m bo khp robot lun bm theo v tr t.
1. H thng iu khin momen tnh ton
a.Lut iu khin .

Nguyn Vn Mnh Kt.C in t 2 k5649

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Hnh 5.1: S iu khin

Phng php c bn ca lut iu khin l la chn lut iu khin sao cho
kh c cc thnh phn phi tuyn ca phng trnh ng lc hc v lm suy
gim m hnh h tr thnh h tuyn tnh c iu khin kn n gin (tuyn
tnh) vi lut iu khin c pht trin t h khi lng n v. Kt qu s
nhn c mt h thng tuyn tnh m bo chnh xc chuyn ng yu cu.
Da trn phng trnh ng lc hc :

(5.1)
Trong : H(q) l ma trn khi lng suy rng vung cp n
:l vc-t n chiu ca cc thnh phn Coriolis v ly tm
G(q): l vc-t n chiu ca cc thnh phn lc suy rng ca cc
lc c th. Mi phn t ca H, G, V l mt hm ca q v o hm ca q.
Vi cch tip cn iu khin chia tch trong trng hp ny ta c:
vi

l vc-t n cc thnh phn m men dn ng cc khp.

Chng ta chn:

Nguyn Vn Mnh Kt.C in t 2 k5650

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Phng trnh m t Lut iu khin c dng nh sau :

(5.2)
Cn bng hai phng trnh trn v a trn tnh cht H(q) l ma trn thc
dng nn c th ly nghch o, ta nhn c phng trnh vi phn tuyn tnh
cp hai nh sau:

y l phng trnh vi phn tuyn tnh cp 2 c lp

gia cc khp. Do c th thit k cc b iu khin c lp c cu chc PD

cho tng khp nh sau :

(5.3)
T (5.1),(5.2) v (5.3) ta rt ra c phng trnh vi phn sai s v tr ca h

thng kn c dng nh sau :

Phng trnh c tnh dng ton t Laplace l :

Vit cho tng khp ring l ( l khu qun tnh bc hai):

m bo h t trng thi n nh ti han, khi

b.kt qu m phng iu khin bng simulink

Nguyn Vn Mnh Kt.C in t 2 k5651

 n thit k h thng c in t
D

[TH.S:NG BO LM]

qu o qd1 v q1 trn cng mt th

qu o qd2 v q2 trn cng mt th

Nguyn Vn Mnh Kt.C in t 2 k5652

 n thit k h thng c in t
D

[TH.S:NG BO LM]

qu o qd3 v q3 trn cng mt th

qu o qd4 v q4 trn cng mt th

qu o qd5 v q5 trn cng mt th

Nguyn Vn Mnh Kt.C in t 2 k5653

 n thit k h thng c in t
D

[TH.S:NG BO LM]

qu o qd6 v q6 trn cng mt th

Nguyn Vn Mnh Kt.C in t 2 k5654

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Sai lch tnh gc quay cc bin khp:

Nguyn Vn Mnh Kt.C in t 2 k5655

 n thit k h thng c in t
D

[TH.S:NG BO LM]

Nguyn Vn Mnh Kt.C in t 2 k5656

 n thit k h thng c in t
D

[TH.S:NG BO LM]

TI LIU THAM KHO

[1] ng lc hc h nhiu vt - GS. TSKH. Nguyn Vn Khang.
[2] iu khin Robot cng nghip TS. Nguyn Mnh Tin .
[3] MATLAB & SIMULINK dnh cho k s iu khin t ng Nguyn
Phng Quang .
[4] C s Robot cng nghip -GS.TSKH Nguyn Vn Khang .
[5] Robot cng nghip GS. TSKH. Nguyn Thin Phc .
[6] M hnh ha h thng v m phng GS. TS Nguyn Cng Hin v TS.
Nguyn Phm Thc Anh .

Nguyn Vn Mnh Kt.C in t 2 k5657

 n thit k h thng c in t
D

[TH.S:NG BO LM]

NHN XT CA GIO VIN HNG DN

..........
H Ni, ngy thng nm 2014
Gio vin hng dn
Th.S: ng Bo Lm

Nguyn Vn Mnh Kt.C in t 2 k5658