Ky Thuat Robot

Khi c qua ti liu ny, nu pht hin sai st hoc ni dung km cht lng
xin hy thng bo chng ti sa cha hoc thay th bng mt ti liu cng

ch ca tc gi khc. Ti li u ny bao g m nhi u ti li u nh c cng ch
bn trong n. Ph n n i dung b n c n c th n m gi a ho c cu i ti li u
ny, hy s d ng ch c nng Search tm chng.
Bn c th tham kho ngun ti liu c dch t ting Anh ti y:
http://mientayvn.com/Tai_lieu_da_dich.html
Thng tin lin h:

Yahoo mail: thanhlam1910_2006@yahoo.com
Gmail: frbwrthes@gmail.com
B CNG THNG
TRNG I HC CNG NGHIP TPHCM
KHOA CNG NGH IN T - T NG
GIO TRNH MN HC
Bin son : Bi Th Cao

Trn Hu Ton
TP.H CH MINH, 03/10/2008
MC LC
Chng 1.
ng qu n v ro ot.
01
ch s ph t tri n Robot.
01
1.2. C c ng d ng ca Robot.
04
1.1.
1.2.1. C c u i m khi s d ng Robot.
04
1.2.2. Mt s lnh vc ng d ng.
05
1.3. C c kh i ni m v robot - robot c ng nghi p.

1.3. .
nh ngha v robot c ng nghi p
07
07
1.3.2. C c thnh phn ca robot c ng nghi p
08
1.3.3. Bc t do ca robot c ng ghi p.
10
.3.4. H to trong robot.
11
1.4. Phn loi robot.
13
1.4.1. Robot c ng nghi p.
13
. Robot ni tip.
13
2. Robot song song.
14
1.4.2. Robot di ng
15
Chng 2. Phn tch h c cn ng tnh v chuyn ng t y my.

2. . C c kh i ni m c bn v ti n tnh hc.
17
17
2. . . Trng th i cn bng.
17
2. .2. c.
17
2. .3. Momen ca lc i vi tm.
17
2. .4. Momen ca lc i vi tr c.
17
2. .5. H lc.
18
2. .6. C c ti n tnh hc.
18
2. .7. Mt s m hnh phn lc lin kt
20
2. .8. Sc b n vt li u.
22
2. .9. c ma s t
23
2.2. Thit k h c cn bng tnh.
24
2.2. . X c nh c c yu t u vo.
24
2.2.2. Thit k c kh.
24
2.2.3. Tnh to n ki m tra cn bng lc cho h .
25
2.3. Phn tch chuy n ng tay m y.
28
2.3. . Gii thi u.
28
2.3.2. H to .
28
2.3.3. Qu o robot.
28
2.3.4. Phn tch chuy n ng tng qu t ca tay m y.
28
2.3.5. Php bin i h to .
29
2.4. Phn tch chuy n ng ca mt s tay m y.
29
2.4. . Phn tch chuy n ng ca tay m y 2 khp quay.
30
2.4.2. Phn tch chuy n ng ca tay m y 3 khp quay.
32
2.4.3. Phn tch chuy n ng ca tay m y nhi u khp ni
34
Chng 3. Cc ph p i n i thu n nh t homogenous tr nsform tion
34
3.1.
ect i m v h to thun nh t.
35
3.2.
h c li c c php tnh v vect v ma trn.
35
3.2. . Php nhn vect.
36
3.2.2. C c php tnh v ma trn.
36
a. Php cng tr ma trn.
36
b. Tch hai ma trn.
37
c. Ma trn ngh ch o ca ma trn thun nh t.
37
d. t ca ma trn.
38
e. o hm v tch phn ca ma trn.
38
3.3. C c php bin i d ng trong ng hc robot.
38
3.3. . Php bin i t nh tin.
39
3.3.2. Php quay quanh c c tr c to .
40
3.3.3. Php quay le Euler .
41
2
3.3.4. Php quay Roll Pitch -Yaw.
42
3.4. Bin i h to v mi quan h gi a c c h to .

2.4. . Bin i h to .
43
2.4.2. Mi quan h gi a c c h to .
44
3.5. M t vt th i t ng lm vi c ca robot.
Chng 4. Phng tr nh ng h c c
ro ot
inem tic equ tions
4. . D n nhp.
47
47
47
4. . . H to v mi quan h gi a c c khu trn robot.
47
4. .2. hu ch p hnh cui v i m t c ng cui.
49
4.2. B th ng s DE
IT HARTENBERG (DH).
4.2.1. D n nhp.
49
49
4.2.2.
di ph p tuyn chung an v gc o n ca khu n n.
50
4.2.3.
hong c ch gi a hai khu dn v gc quay ca khu n n.
50
4.2.4. B thng s DH.
51
4.3. G n h to cho robot.
4.4.
42
51
a. Chn gc ca h to .
51
b. Chn tr c n.
51
c. Chn tr c n.
51
d. G n h to cho robot SC R .
53
c trng ca c c ma trn
53
4.3. . h i ni m ma trn .
53
4.3.2. C c php bin i ma trn .
54
4.5. X c nh c c ma trn T theo ma trn .
55
4.6. Trnh t thit lp h phng trnh ng hc ca robot.
55
4.6. . C c bc thc hi n.
55
a. Chn h to c bn v g n c c h to trung gian.
55
b. p bng th ng s DH.
55
c. X c nh c c ma trn i.
55
d. Tnh c c ma trpn T.
55
3
e. it phng trnh ng hc ca robot.
56
4.6.2. d thit lp phng trnh ng hc mt s robot.

Chng 5. ng lc h c Ro ot v ng dng trong i u hin
64
64
5. . M c ch v phng ph p kho s t ng lc hc Robot.
64
5.2. ng lc hc robot vi phng trnh Euler-Lagrange.
65
5.3. ho s t bi to n ng lc hc ca tay m y nhi u bc t do.
71
5.4. Phng trnh ng lc hc tay m y.
71
5.4. . Tng qu t.
72
5.4.2. Ma trn qu n tnh.
74
5.4.3. ector Coriolis/hng tm.
74
5.4.4. ector trng lc
75
5.5. Xy dng Robot vi c tnh phi tuyn - ng d ng trong i u khi n

Chng 6. i u hin Ro ot
78
6. . Bin i qu o t h ta Descartes sang kh ng gian khp.

6. .
78
i suy ng a thc.
78
80
6. .2. i suy qu o theo thi gian nh nh t.
82
6.2. i u khi n h robot phi tuyn.
82
6.3. i u khi n tr tip h robot.
93
6.4. Tnh to n v i u khi n theo momen - hi tip tuyn tnh h robot.
93
6.4. . o hm ca vng hi tip trong

Feedforward Loop)
Deravition of Inner
93
6.4.2. Thit k PD vng ngoi.
96
6.4.3. d minh ha.
98
6.4.4. Thit k PID vng ngoi.
100
6.4.5. Bng tm t t.
102
6.4.6. p d ng Matlab kho s t c c bi to n c th
103
Chng 1: Tng quan v Robot cng nghip
Chng 1
TNG QUAN V ROBOT

1.1. Lch s pht trin Robot.
Khi nim Robot ra i u tin vo ngy 09/10/1922 ti NewYork, khi
nh son kch ngi Tip Kh Karen Kapek tng tng ra mt c my hot
ng mt cch t ng, n l nim m c ca con ngi lc .
T tng thit k, ch to Robot lun thi thc con ngi. n
nm 1948, ti phng th nghim quc gia Argonne, Goertz ch to thnh
cng tay my i (master-slave manipulator). n nm 1954, Goertz ch to
tay my i s dng ng c servo v c th nhn bit c lc tc ng ln
khu cui.
Nm 1956 hng Generall Mills ch to tay my hot ng trong vic
thm him di dng.
Nm 1968 R.S. Mosher, ca General Electric ch to mt c my bit
i bng 4 chn. H thng vn hnh bi ng c t trong v mi chn vn hnh
bi mt h thng servo thy lc.
Nm 1969, i hc Stanford thit k c Robot t hnh nh nhn
dng hnh nh.
Hnh 1.1 Robot Shakey

Nm 1970 con ngi ch to c Robot t hnh Lunokohod, thm
him b mt ca mt trng.
Trong giai on ny, nhiu nc khc cng tin hnh cng tc nghin
cu tng t, to ra cc Robot iu khin bng my tnh c lp t cc loi cm
bin v thit b giao tip ngi v my.
Hnh 1.2. Robot hn im

(Ngun KUKA, Inc)
Hnh 1.3. Robot phu thut

(Ngun Accury, Inc)
Theo s tin b ca khoa hc k thut, cc Robot ngy cng c ch to

nh gn hn, thc c nhiu chc nng hn, thng minh hn.
Mt lnh vc c nhiu nc quan tm l cc Robot t hnh, cc chuyn
ng ca chng ngy cng a dng, bt chc cc chuyn ng ca chn ngi
hay cc loi ng vt nh : b st, ng vt 4 chn, V cc loi xe Robot
(robocar) nhanh chng c ng dng rng ri trong cc h thng sn xut t
ng linh hot (FMS).
Hnh 1.4. Mobile Robot v ng dng cng ngh x l nh (Ngun SRI,

Stanford, CA)
T tr i con ngi lin tc nghin cu pht trin Robot ng dng
trong qut trnh t ng ho sn xut tng hiu qu kinh doanh. Ngoi ra
Robot cn c s dng thay cho con ngi trong cc cng vic mi trng
c hi, khc nghit,
Chuyn ngnh khoa hc v robot robotics tr thnh mt lnh vc
rng trong khoa hc, bao gm cc vn cu trc c cu ng hc, ng lc
hc, qu o chuyn ng, cht lng iu khin Tu thuc vo mc ch v
phng thc tip cn, chng ta c th tm hiu lnh vc ny nhiu kha cnh
khc nhau.
Hin nay, c th phn bit cc loi Robot hai mng chnh : Cc loi
robot cng nghip (cnh tay my) v cc loi robot di ng (mobile robot). Mi
loi c cc ng dng cng nh c tnh khc nhau. Ngoi ra, trong cc loi
robot cng nghip cn c phn chia da vo cu to ng hc ca n : Robot

ni tip (series robot) v robot song song (parallel robot).
Hnh 1.5. Robot song song 6 bc t do Merlet.( Ngun : Dr. J. - P. Merlet v

Prof. V. Hayward.)
Chnh cng ngh tin tin tt c cc lnh vc : c kh, vi mch, iu
khin, cng ngh thng tin to ra nn tng cng nh nhng thch thc
ln i vi khoa hc nghin cu robot. Chnh v vy, con ngi v ang tip
tc pht trin v nng cao mc hon thin trong lnh vc y hp dn ny.
Hnh 1.6. Nguyn bn ca Robot Hexapod TU Munich ( Ngun : Prof. F.

Pfeiffer, TSI Enterprises, Inc.)
1.2. Cc ng dng ca Robot.
1.2.1. Cc u im khi s dng Robot.
Cc loi Robot tham gia vo qui trnh sn xut cng nh trong i sng
sinh hot ca con ngi, nhm nng cao nng sut lao ng ca dy chuyn
cng ngh, gim gi thnh sn phm, nng cao cht lng cng nh kh nng
cnh tranh ca sn phm to ra.
Robot c th thay th con ngi lm vic n nh bng cc thao tc n
gin v hp l, ng thi c kh nng thay i cng vic thch nghi vi s
thay i ca qui trnh cng ngh.
S thay th hp l ca robot cn gp phn gim gi thnh sn phm, tit
kim nhn cng nhng nc m ngun nhn cng l rt t hoc chi ph cao
nh : Nht Bn, cc nc Ty u, Hoa K
Tt nhin ngun nng lng t robot l rt ln, chnh v vy nu c nhu
cu tng nng sut th cn c s h tr ca chng mi thay th c sc lao
ng ca con ngi. Chng c th lm nhng cng vic n gin nhng d
nhm ln, nhm chn.
Robot c kh nng nghe c siu m, cm nhn c t trng
Bn cnh , mt u im ni bc ca robot l mi trng lm vic.
Chng c th thay con ngi lm vic nhng mi trng c hi, m t, bi
bm hay nguy him. nhng ni nh cc nh my ho cht, cc nh my
phng x, trong lng i dng, hay cc hnh tinh khc th vic ng dng
robot ci thin iu kin lm vic l rt hu dng.
5
1.2.2. M s lnh vc ng dng.

a. ng dng trong cc lnh vc sn xut c kh.
Trong lnh vc c kh, robot c ng dng kh ph bin nh kh nng
hot ng chnh xc v tnh linh hot cao.
Cc loi robot hn l mt ng dng quan trng trong cc nh my sn xut
t, sn xut cc loi v bc c kh
Hnh 1.7. Robot hn trong cng ngh sn xut c kh.

Ngoi ra ngi ta cn s dng robot phc v cho cc cng ngh c, mt
mi trng nng bc, bi bm v cc thao tc lun i hi tin cy.
c bit trong cc h thng sn xut linh hot (FMS), Robot ng vai tr
rt quan trng trong vic vn chuyn v kt ni cc cng on sn xut vi
nhau.
Hnh 1.8. ng dng Robot trong cc h thng sn xut linh hot.

b. ng dng trong lnh vc gia cng lp rp.
Cc thao tc ny thng c t ng ho bi cc robot c gia cng
chnh xc v mc tin cy cao
6
Hnh 1.9. Robot c s dng trong cng on cp liu v lp rp.

c. ng dng trong cc h thng y hc, qun s, kho st a cht.
Ngy nay, vic s dng cc tin ch t Robot n cc lnh vc qun s, y
t, rt c quan tm. Nh kh nng hot ng n nh v chnh xc, Robot
c bit l tay my c dng trong k thut d tm, b phng, v trong cc ca
phu thut y khoa vi tin cy cao.
Hnh 1.10. Cc ng dng Robot trong cc lnh vc thm him, qun s, v tinh
Ngoi ra, tu thuc vo cc ng dng c th khc m Robot c thit k
phc v cho cc mc ch khc nhau, tn dng c cc u im ln ca
chng ng thi th hin kh nng cng ngh trong qu trnh lm vic.
1.3. Cc khi nim v Robot Robot cng nghip.
Lnh vc nghin cu v Robot hin nay rt a dng v phong ph. Trong
7
ti liu ny, chng ti ch trnh by cc kin thc ch yu trn cc loi Robot

cng nghip, tc cc cnh tay my. Cc bi ton cn bng lc, cc phng trnh
ng hc v ng lc hc l nhng nn tng c bn cc bn hc vin c th
tip cn vi chuyn nghnh k thut Robot.
1.3.1. nh ngha v robot cng nghip ( Industrial Robot ).
Tu thuc mi quc gia, t chc v mc ch s dng, chng ta c nhiu
nh ngha v robot cng nghip. V vy trong nhiu ti liu khc nhau, nh
ngha v robot cng nghip cng khc nhau. Theo t in Webster nh ngha
robot l my t ng thc hin mt s chc nng ca con ngi. Theo ISO (
International Standards Organization ) th : Robot cng nghip l tay my a
mc tiu, c mt s bc t do, d dng lp trnh v iu khin tr ng, dng
tho lp phi, dng c hoc cc vt dng khc. Do chng trnh thao tc c
th thay i nn thc hin nhiu nhim v a dng. Tuy nhin Robot cng
nghip c nh ngha nh vy cha hon ton tho ng.
Theo tiu chun ca M RIA ( Robot Institute of America ) nh ngha
robot l loi tay my vn nng c th lp li cc chng trnh c thit k
di chuyn vt liu, chi tit, dng c hay cc thit b chuyn dng, thng qua
cc chng trnh chuyn ng c th thay i hon thnh cc nhim v khc
nhau.
Hnh 1.11. Biu din khng gian ca cnh tay my

1.3.2. Cc thnh phn c bn ca ca Robot cng nghip.
S tng quan cu thnh mt Robot cng nghip chuyn dng :
Cm bin
a. Cnh tay Robot (Robot Arm ):

L b phn c kh gm cc khu lin kt vi nhau bi cc khp ni, cc
b truyn ng nh: B truyn bnh rng, b truyn ai, b truyn trc vtbnh v, vt me- ai c
Hnh 1.12. Cnh tay Robot.

b. Ngun ng lc: Cc thit b to chuyn ng cho Robot, c th l cc thit
b kh nn, thu lc, in.
i vi cc chuyn ng cn chnh xc cao, yu cu gn nh ngi ta
c th dng cc loi ngun truyn ng l cc motor bc, cc motor servo.
Hnh 1.13. Cu to ca motor mt loi motor bc.

c. B iu khin ( Controller ):
L thnh phn quan trng quyt nh kh nng hot ng v chnh xc
ca Robot. B phn ny thng thng c tch hp di dng cc board mch
iu khin, c th c cc loi sau:
IC diu khin trung tm (CPU) kt hp vi cc card iu khin phn theo
modul.
Cc thit b iu khin Robot s dng PLC ( Programable Logic Controller
).
S dng cc b iu khin PMAC ( Programable Multi-Axies Controller ).
Cc b iu khin thit k theo cc dng iu khin hin i nh : B iu
khin m, b iu khin theo mng neuron
d. Cm bin ( Sensor ):
L thit b chuyn cc i lng vt l thnh cc tn hiu in cung cp
cho h thng nhm nng cao kh nng linh hot v chnh xc trong iu
khin. Nh vy Robot chnh l mt h thng iu khin kn vi vng hi tip (
Feedback ) c thc hin t tn hiu thu v t cm bin.Cc loi cm bin
thng gp nh:
Cm bin quang
Cm bin v tr v dch chuyn.
Cm bin o gc.
Cm bin vn tc.
Cm bin gia tc v rung.
Cm bin lc v bin dng.
Cc cm bin trn c th cho tn hiu tng t Analogue hoc tn hiu s (
Digital ), ngoi ra cn s dng cc b m ho v tr, m ho gc dch chuyn
Encoder, Resolver
e. Cc chng trnh:
Cc chng trnh lun tng thch vi cc b iu khin. Chnh v vy cc
loi ngn ng vit chng trnh iu khin cho Robot cng kha a dng, c
th l ngn ng vit cho vi x l (ngn ng my ), ngn ng vit cho PLC
(thuc cc hng khc nhau ), hay cc ngn ng trn my tnh nh: Pascal, C,
C++, Visual Basic, Matlab
10
1.3.3. Bc t do ca Robot cng nghip.

a. Khi nim:
Bc t do l s kh nng chuyn ng ca mt c cu dch chuyn
c mt vt th no trong khng gian. C cu chp hnh ca robot phi t
c mt s bc t do nht nh. Ni chung, c h ca mt robot l mt c cu
h ( l c cu c mt khu ni gi ).
Chuyn ng ca cc khu trong robot thng l mt trong hai khu
chuyn ng c bn l tnh tin hay chuyn ng quay.
b. Xc nh s bc t do ca robot (DOF- Defree Of Freedom).
S bc t do ca robot c xc nh:
W= 6n - i.Pi
W: S bc t do ca robot.
n: S khu ng.
Pi: S khp loi i.
Trong , khp loi i l khp khng ch i bc t do.
Hnh 1.14. Robot PUMA 6 bc t do.

V d: Xc nh s bc t do ca robot sau:
11
Hnh 1.15. Bc t do ca robot

Xc nh c s khp loi 5 l 5 (4 khp quay v mt khp tnh tin ), do
n=5 v P5 =5 nn s bc t do ca robot ny: W= 6.5 5.5 = 5 bc.
Lu :
Hu ht robot s dng khp loi 5 ( khng ch 5 bc t do, chuyn ng
quay hoc tnh tin ). V vy s bc t do ca n cng chnh l s khu ng,
robot c bc t do cng cao th cng linh hot.
Thng thng 3 bc t do u dng nh v, cc bc t do sau nh
hng.
1.3.4. H to trong robot.
Mi robot thng bao gm nhiu khu lin kt vi nhau ( links ) thng
qua cc khp ( joints ) to thnh mt xch ng hc xut pht t mt khu c
bn ng yn. H to gn vi khu c bn gi l h to c bn ( hay h
to chun ).
Cc h to trung gian khc gn vi cc khu ng gi l h to suy
rng.
Ti tng thi im hot ng cc to suy rng
xc nh cu hnh ca robot bng cc chuyn dch di
hoc cc chuyn dch gc ca cc khp tnh tin hoc
khp quay. Cc to suy rng cn li l cc bin
khp.
Tt c cc h to dng trong robot phi tun
theo qui tc bn tay phi : Dng bn tay phi co hai
ngn t v p t, ngn ci tr theo phng din trc z,
ngn tr theo phng din trc x, ngn gia hng trc
y.
d2
z0
0
x0
y0
12
o4
y4
z4
x4
Hnh 1.16. . H to ca robot c n khu.

Cc gc quay 1, 3, 4, 5 v dch chuyn di d2 l cc to suy rng
( cc bin khp ).
kho st ng hc robot ta phi gn trn mi khu ca robot mt h to
. Nguyn tc chung gn h to s c trnh by trong chng III trong
khi xt n phng trnh ng hc ca robot v b thng s DenavitHartenberg.
V d: Xc nh to cho robot SCARA (Robot c 4 bc t do ).
d3
o0
x0
y0
z0
o1
x1
y1
z1
o2
y2
z2
x3
o3
y3
z3
o4
y4
x2
x4
z4
Hnh 1.17. Xc nh to cho cc khu ca Robot Scara.

1.4. Phn loi Robot.
1.4.1. Robot cng nghip.
1. Robot ni tip (series robot).
Thc cht loi Robot ny chnh l cc loi tay my, cc khu v khp ni
ca chng c thit kt lin tip nhau hnh thnh nn cc qu o chuyn
ng nht nh. i vi loi robot ny, chng ta c nhiu cch phn loi khc
nhau :
a. Phn loi theo kiu kt cu.
Robot kiu to cc.
Tay my c 3 chuyn ng tnh
tin theo 3 phng ca h ta
cc trong khng gian.
13
Thng ng dng loi robot ny trong vic vn chuyn phi liu, lp rp,
hn trong mt phng
Hnh 1.18. Robot kiu to cc
Robot kiu to tr.
Vng lm vic ca robot
ny c dng hnh tr rng
Robot Versatran (hng
AFM, Hoa K) l mt robot
thuc loi ny.
Hnh 1.19. Robot kiu to tr

Robot kiu to cu.
Vng lm vic ca robot c
dng hnh cu.
C hai loi cu hnh chnh
thuc kiu robot ny : 3 khp quay
(RRR) 2 khp quay, 1 khp tnh tin
khu cui (RRT)
Hnh 1.20. Robot kiu to cu

Robot kiu Scara.
Robot c cu trc theo kiu
Scada ra i t nm 1979, ti trng
i hc Yamanashi (Nht Bn).
Robot lai ny thng c ng
dng trong cc lnh vc lp rp, vi
cu hnh ca 3 khu u tin l : RRT
Hnh 1.21. Robot kiu Scara.
b. Phn loi theo ngun truyn ng.
H truyn ng in.
H truyn ng thu lc.
H truyn ng kh nn.
c. Phn loi theo cc ng dng.
14
Hnh 1.22. Phn loi cc loi robot chuyn dng. (Ngun : Reis Robotics, ABB
Flexible Automation, CMB Automation)
2. Robot song song (Parallel Robot).
Cc loi Robot thuc nhm ny c cc khu chuyn ng song song tng
i vi nhau. Thng thng chng gm 1 c nh v 1 di ng.
Hnh 1.23. Mt sn phm robot song song (Ngun : PRSCs)

Tu thuc vo s lng cc nhnh ca robot song song m ta c th phn
loi chng vi nhau. Mt loi robot song song c 6 nhnh c s dng rt ph
bin l Hexapod.
1.4.2. Robot di ng (Mobile Robot).
15
y l h Robot c nhiu tnh nng thng minh v linh hot trong qu

trnh ng dng nh kh nng di chuyn c theo lp trnh.
Hnh 1.24. Mobile robot ng dng trong cc lnh vc khc nhau.

H thng mobile robot l lnh vc tht s hp dn i vi cc nh nghin
cu cng nh nhng ngi quan tm, khng ch nh nhng u im ni bc
ca n m cn tnh a dng trong ng dng.
Phn tch ng hc v ng lc hc mobile robot l nhng bi ton c
mc phc tp khc nhau, n tu thuc vo kt cu ca robot cng nh yu
cu v chnh xc, tnh thng minh trong x l tnh hung
Chng ta xem xt mt vi chuyn ng m con ngi mong mun thit k
cc loi mobile robot
+ Chuyn ng theo dng trn :
+ Chuyn ng slide ca cc loi ng vt b st.
+ Chuyn ng chy ca ng vt 4 chn.
+ Chuyn ng i b ca con ngi.
16
Ngy nay con ngi hin thc ho c cc tng ny, mc d mc

chnh xc, tin cy ca mi loi, mi hng sn xut l khc nhau.
Hnh 1.25. Robot chuyn ng bn chn
Hnh 1.26. Mobile Robot tc v
(Ngun : AIBO, SONY, Nht Bn)

Bn)
(Ngun: SDR-4X, SONY, Nht
17
Chng 2: Phn tch h c cn bng tnh v chuyn ng tay my
Chng 2
PHN TCH H C CN BNG TNH V

CHUYN NG CA TAY MY
2.1. Cc khi nim c bn v tin tnh hc
2.1.1. Trng thi cn bng
H vt c xem nh trng thi cn bng khi tng cc ngoi lc tc ng
ln n bng khng. Lc y h vt hoc ng yn hoc chuyn ng thng u
i vi h qui chiu .
Trong thc t lun tn ti lc ma st nn khi h vt t trng thi cn bng
th n ng yn.
2.1.2. Lc
Lc c trng cho tc dng c hc ca vt th ny ln vt th khc
Lc c biu din bng mt vector {phng, chiu, ln, im t}
Trong h trc {x,y,z} th lc F ( Fx , Fy , Fz )

2.1.3. Mmen ca lc i vi tm
Mmen ca lc F t ti A i vi tm O l m0 ( F ) OA F d F
m0 ( F ) c ln bng d.F, im t ti O, phng vung gc vi mt phng
( F , O ) , chiu thun theo chiu xoay ca OA, F

m0 ( F )
2.1.4. Momen ca lc i vi trc ()

Tch F F// F => m0 ( F ) dF

Vy momen cua lc i vi trc bng tch ca thnh phn hnh chiu vung
gc ca lc (ln mt phng vung gc vi trc) vi khong cch t lc hnh
chiu n trc.
Chiu ca momen hng theo chiu xoay ca lc quanh trc.
18
( )
F//
d
O
2.1.5. H lc
H lc tc dng vo mt vt ang kho st ( Fk ) ( F1 , F2 ,..., Fn )
Hai h lc ( Fk ) ( Ph ) khi chng c cng tc dng c hc
Hp lc ca h lc: R c gi l hp lc ca h lc ( Fk ) khi R Fk
H lc cn bng khi R 0
2.1.6. Cc tin tnh hc
Hai lc cn bng khi chng cng phng, ngc hng, cng ln.
Hp lc ca hai lc l vector lc ng cho ca hnh bnh hnh.
F1

R F1 F2
F2
Khi hai vt tng tc vi nhau, chng tc ln nhau mt lc:

Hai lc tng tc cng phng, cng ln, nhng ngc hng.
im t ca 2 lc nm ngay ti v tr tip xc ca 2 vt v hng vung
gc vi tip tuyn ti im tip xc.
Tip tuyn
Vt t do l vt c th dch chuyn ty trong ln cn b t v tr ang xt.
Ngc li gi l vt khng t do
Vt kho st (S) c qui c gi l vt chu lin kt. Cc vt khc tng
tc c hc vi S c gi l vt gy lin kt.
Vt khng t do c th xem l t do nu ta thay th cc vt gy lin kt
bng cc phn lc lin kt.
V d :
19
M r
m
iu kin cn bng ca h tnh
R0
( Fk ) 0
, trong R l vector hp lc v M 0 l m men
M 0 0
chnh vi tm O ca h ( Fk ) .
Ta c
m
(
F
R
ox
x
k)
x
kx
k
k
R( Rx , R y , Rz ) R y Fky , v M 0 ( M ox , M oy , M oz ) M oy m y ( Fk )
k
k
F
z kz
M oz m z ( Fk )
k
Fkx 0
k
Fky 0
k
Fkz 0
k
Vy iu kin h cn bng tnh l ( F ) 0 m
x ( Fk ) 0
m y ( Fk ) 0
k
m z ( Fk ) 0
k
2.1.7. Mt s m hnh phn lc lin kt

a. Phn lc lin kt mt chiu
20
N1
s
s
b. Lin kt bn l tr
s
c. Lin kt bn l cu
Rx
Rz
Ry
K hiu qui c
d. Lin kt gi
K hiu qui c
c
e. Lin kt thanh
Vi d: Xc nh cc phn lc lin kt ca thanh trong h sau
21
N2
450
A1
A2
m(5kg)
2.1.8. Sc bn ca vt liu
Thanh lin kt
dy
dx
dz
a. Cc tc ng lc ln thanh bao gm:

Lc ko: lm cho thanh dn ra theo hai chiu ca lc
Lc nn: lm cho thanh nn li theo hai chiu ca lc
Lc xon: lm cho thanh vn cong
Di tc ng ca cc ngoi lc mi phn t dv(dx,dy,dz) u chu tc
ng ca cc vector lc, c gi l cc tensor ng sut. Cc vector ng sut
ny c c th hin nh hnh v, theo tng cp vector bng nhau v ln
nhng ngc chiu nhau, (dx, dx , , dy, dy , , dz, dz , ) .
b. Trng thi vt l ca thanh khi b xon
b ko gin
b nn li
Na trn ca thanh c xu
hng b ko gin ra
Na di ca thanh c xu
hng b nn li
22
c. Kh nng chu gin v nn ca cc khi loi

Mi loi vt liu c khn nng chi gin v nn khc nhau, chng c gi l
cc gi tr ti hn nn Fn v gi tr ti hn ko FK . Nhng ni chung kh nng

chu nn tt hn so vi chu gin.
Khi b nn qu mc gii hn kim loi s b bin dng, sau ln bin dng ny
chng s c mt gi tr ti hn Fn khc, ln hn gi tr ban u.
Fn
t
Khi b ko qu mc gii hn kim loi s b bin dng, sau ln bin dng ny
chng s c mt gi tr ti hn Fk khc, nh hn gi tr ban u, v c nh th

cho n khi t ri ra.
Fk
t
2.1.9. Lc ma st
a. nh ngha: Ma st l lc sinh ra do s c st gia hai vt. Vt ny c st
sinh ra lc ma st tc ng ln vt kia v ngc li
1
2
Fm 21
1
2
Fm12
Fm12 Fm 21
Fm12 Fm 21
b. Phn loi: C hai loi ma st, l ma st tnh v ma st ng

Ma st tnh l lc ma st xut hin khi hai vt tip xc nhau nhng cha
chuyn ng
23
Ma st ng l lc ma st xut hin khi hai vt tip xc nhau v c s

chuyn ng tng i gia vt ny vi vt kia
c. Tnh cht ca lc ma st:
Lc ma st t l vi din tch tip xc v tc c st gia hai vt
d. Li im ca lc ma st: dng hm, thng ng c, bnh xe
e. Bt li ca lc ma st
Tn cng v ch
Lc ma st sinh ra nhit lm nng h thng, nng cc im tip xc v qua
thi gian gy h hng thit b (bin dng b mt tip xc)
f. Phng php lm gim bt lc ma st
Gim din tch tip xc (S dng cc khe, cc bnh xe, bac n, con trt)
Gim tc c st (tng tc t t)
S dng cc cht bi trn ni tip xc (nht, m b)
2.2. Thit k h c cn bng tnh
2.2.1. Bc 1: Xc nh cc yu t u vo
i tng ph v: khi lng, kch thc hnh dng, cng
Chu trnh ph v: cc thao tc, tin trnh thc hin v cc to , qu o ca
chu trnh
Khng gian phc v
Ngun nng lng cung cp
2.2.2. Bc 2: Thit k khung c kh
V kt cu hnh hc, xc nh cc khp ng
Xc nh cc ngun lc cho cc khp ng: motor(DC, AC, servo), kh nn,
thy lc
Xc nh h truyn ng cho cc khp: trc tip hay gin tip, v tr t
ngun lc, khi lng cc ngun lc
Ti u ho cc bc a, b, c li v lc v n gin v kt cu
Xc nh vt liu cho cc thanh, dng hnh hc v kch thc
2.2.3. Bc 3: Tnh ton cn bng lc cho h
Xc nh cc phn lc lin kt ca cc thanh
Da trn cc phn lc lin kt, xc dnh cc ngun lc: motor(ngu lc), kh
nn(p sut nn),..
24
Tnh ton cn bng lc cho c h: tnh ton cn bng lc cho cc khu v cho
ti trng
V d: Thit k h cn bng tnh cho cnh tay Robot trong dy chuyn
phn loi sn phm di y
Khu 1
Thanh d2
Khu 2
Thanh d1
Tay gp
dng gic ht
ti trng
M
1m
Bng chuyn A
Bng chuyn B
2m
Bc 1:
a. Vt th M c khi lng 0,5kg, kch thc hnh tr cao 10cm, c nhn mc
nn d try xc
b. Nhc vt M ln, di chuyn t bng chuyn A sang bng chuyn B, h vt B
xung
c. Khong cch gia 2 bng chuyn 2m, chiu cao ca bng chuyn 1m, chiu
cao ca vt M l 10cm
d. Ngun nng lng cung cp kh nn
Bc 2:
a. Kt cu hnh hc nh hnh v
Khp 1: xoay quanh trc
Khp 2: khp trt ln xung
Tay gp: dng gic ht
Thanh 1 c chiu cao: 1m + 0,1m +(chiu di cylinder trt)
Thanh 2 c chiu di: 1m
ti trng c hnh dng v kch thc nh hnh v
25
0.5m
Chn
0.5m
0.25m
0.25m
b. Ngun lc
Khp 1: dng v lng kh nn truyn ng xoay trc tip, khi lng 1kg
Khp 2: dng cylinder kh nn truyn ng trt trc tip, khi lng 1kg
Tay gp: dng van kh nn iu khin gic ht, khi lng 200g
c. Vt liu lm cho cc thanh l Inox
Thanh 1: loi thanh trn, 34, khi lng 8kg
Tay gp: phu gic ht, 8
ti trng: St tm si Inox, dy 5mm, khi lng 7kg
Bc 3:
a. Ho rn ton h, xc nh cc phn lc lin kt ca ti trng, nh hnh v
Do h i xng nn:
Pcylinder
Pvolang
N1 N 4 v N 2 N 3
PT cn bng ca h lc:
Pk N i 0

m
(
P
)
m
(
N
( ) i ) 0
( ) k
Pthanh2
PM
( )
N1
Tnh
cn bng
lc:
N4
Pde
Pthanh1
N3
N2
PM Pcylinder Pthanh2 Pvolang Pthanh1 Pde
N1 N 2 N 3 N 4 0
2 N1 2 N 2 225( N ) (1)
26
Pcylinder
PM
Pvolang
1m
Pt1 _ 2
Pt1 _ 1
2N 2
2N1
( )
Pde
Pthanh1
0.25m
Phng trnh cn bng momen
0.75
0.75 ( PM Pcylinder )
Pt1 _ 1 0.5 2 N 2
2
0.25
Pt1 _ 2 0.25 Pvolang 0.25 ( Pde Pthanh1 ) 0

2
11.25 14.0625 N 2 1.5625 2.5 37.5 0
N 2 16.25( N ) (2)
Thay (2) vo (1) ta c
N1 96.25( N )
Nhn xt: ta thy N 2 >0, nn h cn

bng v ta khng cn thm i trng
cho
b. Xc nh ngun lc cho cc khu

Tay ghp: dng van ht chn khng c p sut
P
m. g
m. g
1K (atm ) , ta chn P = 1.5K(atm)

s
.rM 2
Khu 1: Cylinder kh nn c p sut P 1K (atm), ta cng chn P = 1.5K

(atm)
Khu 2: Volang kh nn c p sut P = 1.5K (atm)
c. p sut ngun kh nn cung cp cho ton h: ta chn 2K(atm)
2.3. Phn tch chuyn ng tay my.

2.3.1. Gii thiu v phn tch chuyn ng
Vi mt h tay my c thit k, vn t ra l lm th no xc
nh qu o ca cc khu trong chu trnh hot ng ca Robot
Vic phn tch chuyn ng ca tay my nhm mc ch tm ra cc qu o
ny, nhng vic thc hin c tin hnh theo hai bc: Xc nh to ca
cc khu trung gian, ri t nh ra qu o ca cc khu.
n gin cho vic phn tch chuyn ng, thit k c kh v u khin
Robot, ta thng n gin ho cc khu mt trong hai dng c bn l khp
trt v khp bn l
Khi nim bc chuyn ng t do th hin cho s khu c trn Robot
27
2.3.2. H to
kho st cho chuyn ng cc khu, ta gn vo y mt h ta
(0xyz). H trc ny c t sao cho n gin cho vic kho st
y4
y3
x4
x2
2
y1
x3
z2
y2
z3
x1
z4
z1
2.3.3. Qu o
m t qu o ca tay my ta th hin thng qua cc ta suy rng
ca cc h ta khu. V d m t qu o ca tay my ti v tr M ca tay
gp (khu cui)
x M x M ( q1x , q2 x ,..., qnx )

y M y M ( q1 y , q2 y ,...., qny )
z M z M ( q1z , q2 z ,...., qnz )
Trong , q1, q2, l

cc ta suy rng,
ng vi chuyn ng
ca cc khu.
2.3.4. Phn tch chuyn ng tng qut ca tay my.

a. Bi ton ng hc thun
M hnh ca bi ton l cho trc c cu v quy lut chuyn ng ca h,
th hin qua cc ta suy rng. Ta phi xc nh quy lut chuyn ng ca
mt v tr xc nh no trong h.
Bi ton ny trong thc t, n thng c dng sau khi gii quyt bi ton
ng hc ngc, xc nh ranh gii chuyn ng v kim tra cn bng ng
ca cc phn t trong h.
b. Bi ton ng hc ngc
M hnh ca bi ton l cho trc c cu v quy lut chuyn ng ca khu
cui, ta phi xc nh quy lut chuyn ng ca cc khu thnh vin, tc l xc
nh cc ta suy rng.
28
Bi ton ny cho v s li gii (v s nghim ca cc qi). Trong thc t khi

gii quyt cc bi ton ny, ta thng thm vo n cc iu kin rng buc ca
chuyn ng tay my cho li gii ti u.
2.3.5. Php bin i h ta
Cho hai h trc ta (Oxyz) v (O1x1y1z1) nh hnh v, i0 , j0 , k0 l cc

vector ch phng n v ca h (Oxyz)
z
z1
a
k0
y1
i0 o
o1
j0
x1
Cho a trong h (Oxyz) c th hin a a x i0 a y j0 a z k0

a
a
cos(
a
,
i
)
a
a
cos(
a
,
j
)
a
a
cos(
a
, k0 )
Vi : x
0
y
0
z
nh l v phpchiu hnh hc
Hnh chiu ca a theo hng u bt k l:

au a x cos(u, x ) a y cos(u, y ) a z cos(u, z )
Vy chiu ca: a ln x1 l a x1 a x cos( x1 , x ) a y cos( x1 , y ) a z cos( x1 , z )

a ln y1 l a y1 a x cos( y1 , x ) a y cos( y1 , y ) a z cos( y1 , z )
a ln z1 l

a z1 a x cos( z1 , x ) a y cos( z1 , y ) a z cos( z1 , z )
Vy trong h ta (O1x1y1z1), a ax1i1 a y1 j1 az1k1

Lp bng Cosin ch hng cho h phng trnh trn ta c
x
x1
y1
..
3
3
3
z1
Gi ma trn cosin ch hng t h ta (Oxyz) vo (O1 x1 y1 z1 ) l
29
1 cos( x1 , x ) , 2 cos( y1 , x ) ,
a x1
ax
1 1 1

MC10 2 2 2 => a y1 MC10 a y

a z
3 3 3
a z1
Tng t nh vy nu trong h ta (O1x1y1z1), a ax1i1 a y1 j1 az1k1

Th trong h ta (Oxyz), s c ma trn cosin ch hng l:
1 2 3
a x
a x
T
a MC T a
MC01 1 2 3 => MC01 MC10
=>
y
y
az
1 2 3
a z

1
10
2.4. Phn tch chuyn ng ca mt s tay my.

2.4.1. Phn tch chuyn ng ca tay my 2 khp quay.
x2
x2
y2
y1
y1
z2
y2
z2
x1
z1
x1
z1
Hnh 1a)
Hnh 2a)
Xt chuyn ng ca mt tay my hai bc t do nh hnh 1a, hnh 2a, gi

s ta ho rn khu 2, cho khu 1 chuyn ng xoay
Ta thy im P trong h ta ca khu 2 khng chuyn ng, nhng
trong h ta ca khu 1 th n chuyn ng.
Ta ca P c tnh da vo hnh 1b) v 2b)
30
x2
r2
x2
y1
d1
z2
r2
r1
r1
d1
y1
y2
z2
y2
x1
x1
1
z1
z1
Hnh 1b)
Hnh 2b)
Vy ta ca P trong h khu 1 l
( r1 )1 (d1 )1 ( r2 )1 MC12 (d1 r2 )2
2.4.2. Phn tch chuyn ng ca tay my ba khp quay.

y3
x2
2
y1
x3
z2
y2
z3
x1
z4
z1
Xem xt m hnh ca tay my ba bc t do nh hnh v trn
31
y3
x2
y1
d2
d1
r3
x3
r1
z2
y2
z3
z4
x1
z1

T m hnh vector ta thy: r1 d1 d 2 r3
r1 1 MC12 (d1 d 2 r3 ) 2

MC12 ( d1 ) 2 MC12 ( d 2 r3 ) 2
=> MC ( d ) MC [ MC ( d r ) ]
12
1 2
12
23
2
3 3

MC12 ( d1 ) 2 MC12 MC23 ( d 2 r3 ) 3
Nu xem im P cng l mt khu (khu 4), ta c
y4
y3
x2
y1
d1
P
z4
x3
r1
z2
d2
x4
r3
y2
z3
x1
z1
(
r
)
MC
(
d
)
MC
MC
(
d
d
Vy 1 1
12
1 2
12
23
2
3 )3
MC12 (d1 )2 MC12 MC23 (d 2 )3 MC12 MC23 MC34 (d3 )4 ]
2.4.3. Phn tch chuyn ng ca tay my nhiu khp ni.

32
M rng vi h a bc t do, ta c ta ca khu cui trong h ta

gc l
n
n 1
i
( rT (Ter min al ) )1 d i [ MC j ( j 1) (d i ) i 1 ]
i 1
i 1
j 1
a. Cc bc thc hin cho vic phn tch chuyn dng

Bc 1: Xc nh h trc ta
Xc nh c tnh cc khp: trt hay bn l
t cc h trc ta sao cho trc quay ca khp trng vi trc z, trc thanh
tay my trng vi trc x
Xc nh cc gc quay, chn chiu dng ca gc quay hng t trc
thanh(trc x) ti thanh quay (trong khng gian 1/4 dng)
Bc 2: Xc nh cc ma trn MC
Bc 3: Vit phng trnh xc nh ta ca khu cui.
Bc 4: Tnh ton vn tc v gia tc.
b. V d1: Xc nh ta ca khu cui P trong h tay my nh hnh di.
Cho d1 = 20cm, d2 = 30cm, d3 = 10cm, 1 = 300, 2 = 600, 3 = 450
Gii
z3
x2
2
x1
d1
y2
z2
d2
x3
3
d3
r1
y1
y3
z4
y4
x4
z1
Ta c
(rP )1 MC12 (d1 )2 MC12 MC23 (d 2 )3 MC12 MC23 MC34 (d 3 ) 4
M:
33

1 2 3 cos( x2 , x1 ) cos( y 2 , x1 ) cos( z2 , x1 ) cos 1 sin 1 0

M 12 1 2 3 cos( x2 , y1 ) cos( y 2 , y1 ) cos( z2 , y1 ) sin 1 cos 1 0

0
1
1 2 3 cos( x2 , z1 ) cos( y 2 , z1 ) cos( z2 , z1 ) 0

cos( x3 , x2 ) cos( y3 , x2 ) cos( z3 , x2 ) cos 2 0 sin 2

M 23 cos( x3 , y2 ) cos( y3 , y2 ) cos( z3 , y2 ) sin 2 0 cos 2

cos( x3 , z 2 ) cos( y3 , z2 ) cos( z3 , z2 ) 0
1
0
M 34

cos( x4 , x3 ) cos( y4 , x3 ) cos( z4 , x3 ) cos 3 sin 3 0

cos( x4 , y3 ) cos( y4 , y3 ) cos( z4 , y3 ) sin 3 cos 3 0

0
1
cos( x4 , z3 ) cos( y4 , z3 ) cos( z4 , z3 ) 0
c. V d 2: Xc nh ta ca khu cui P trong h tay my nh hnh di.

Cho d1 = 20cm, d2 = 30cm, d3 = 10cm, 1 = 600, 2 = 300, 3 = 450
z4
z3
x2
2
x1
d2
d1
P
y4
3
x3
r1
z2
x4
d3
y2
y3
y1
z1
34
z4
Chng 3: Cc php bin i thun nht
Chng 3
CC PHP BIN I THUN NHT

chng 2, chng ta tm hiu cc kin thc c bn v cc h cn bng
lc cng nh ng hc ca cnh tay my. i vi cc robot c kt cu n
gin, chng ta c th p dng cc phng thc trc tip v lc, momen v cc
thnh phn ng hc phn tch ng hc cho robot cng nghip. Tuy nhin,
phng php ny gp nhiu kh khn i vi cc bi ton ca robot c cu
hnh phc tp. V vy, trong chng ny chng ta tm hiu cch thc tip cn
khc trong vn gii quyt bi ton ng hc robot, l cc php bin i
trong h to thun nht (gi tt l cc php bin i thun nht). Phng
php ny l bc pht trin t cc nn tng ton hc, c hc tm hiu
chng trc.
3.1. H to thun nht.
biu din 1 im trong khng gian 3 chiu, ngi ta dng vector im (
Point Vector)
Cc vector im thng c k hiu bng cc ch vit thng. V d

a, v , p
Tu thuc h qui chiu c chn m 1 im trong khng gian c th c
biu din bng cc vector im khc nhau
V d :
zC
V
VB
zA
VA
yC
xC
y
xA
v:

i
Nu gi , j , k l cc vector nh v ca h to d no th vector im
v ai bj ck
Vi a,b,c l to v tr ca im v.
o Nu
quan tm ng thi vn v tr v nh hng ta phi biu din vector
im v trong khng gian 4 chiu :
35
x
y
v , vi
z

w
x
a;
w
y
b;
w
z
c
w
Vi w l hng s thc (hng s t l).

+ Khi w=1 th x=a; y=b; z=c : H to thun nht (Lc ny to khng gian
4 chiu trng vi to khng gian 3 chiu)
+ Khi w=0 th x, y, z : Th hin hng ca cc trc to
S dng h to vi w=0 v w=1 th c th th hin c v tr v nh hng
vt th.
+ Ki w0, v w0 th :
v ai bj ck
2
i
3
j k
V d :
o Cc trng hp c bit :
+ [0, ,0, 0, 0]T : Vector khng xc nh.
+ [0, 0, 0, n]T : Vector 0.
+ [x, y, z, 0]T : Vector ch hng.
+ [x, y, z, 1]T : Vector trong h to thun nht.
3.2. Nhc li cc php tnh v vector v ma trn.
3.2.1) Php nhn vector :
Cho 2 vector :
a ax i a y j az k
b bx i by j bz k
a. Tch v hng 2 vector :
a.b ax bx a y by az bz
b. Tch c hng hai vector (Tch hai vector) :
i
j
k

a.b c a x a y a z
bx by bz
3.2.2. Cc php tnh v ma trn :
a. Php cng tr hai ma trn :
iu kin : Cc ma trn phi cng bc (cng kch thc)
36
Cng (tr) hai ma trn A,B cng bc ta c ma trn C cng bc vi cc phn t

Cij Aij Bij
b. Tch hai ma trn :
iu kin : S ct ca ma trn th nht bng s hng ca ma trn th hai.
Tch ca hai ma trn A(m,n) vi ma trn B(n,p) l ma trn C(m,p).
V d :
1
A 4
1 2
6 v B 3 4
5 6
9
3
22 28
A.B C 49 64
76 100
Ch :
+ A.B B.A
+ (k.A).B = k.(A.B) = A.(k.B)
+ A.(B.C) = (A.B).C
+ (A+B).C = A.C+B.C
+ C.(A+B) = C.A+C.B
c. Ma trn nghch o :
A. A1 I
iu kin : Ma trn A l kh o (det(A) 0)

C mt s cch tnh ma trn nghch o. Mt trong s :
+ Tnh nh thc : det(A)
+ Tnh ma trn C l ma trn phn ph i s ca ma trn A :
Cij (1)i j Dij vi Dij det(M ij )
1
+ Tnh ma trn nghch o theo : A
1
CT
det( A)
d. Ma trn nghch o ca ma trn thun nht :

Cho ma trn thun nht A :
37
nx ox
n
oy
A y
nz oz
0
0

A n o a
ax
ay
az
0
px
p y
pz
Tm ma trn nghch o ca ma trn thun nht :

nx n y nz n. p

o
o
o
o
.p
x
y
z
A

a x a y a z a. p
0
0
1
0
V d : Cho
0 0
0 1
A
1 0
0 0
A1
0
0
1
0
0
0
1
2
3
1 3
0 2
0 1
0
1
0
1
0
0
Kim tra :
A. A1
1
0
0
1
0
0
0
0
1
0
0
0
I
0
e. Vt ca ma trn :
Vt ca ma trn vung bc n l tng cc phn t trn ng cho chnh.
n
K hiu : Trace( A) Tr ( A) aii

i 1
f. o hm v tch phn ca ma trn :

Nu cc phn t ca ma trn A l hm nhiu bin th cc phn t ca ma
trn o hm bng o hm ring ca cc phn t ma trn A theo bin tng
ng.
38
a
A d
g
b
e
h
c
f
k
a b c
t
t t
d e f
g h k
t
t t
Tng t cho php tch phn ma trn.
3.3. Cc php bin i ma trn dng trong ng hc robot.
Cho u l vector biu din im cn bin i
h l vector dn c biu din b ma trn H l ma trn chuyn i :
v H .u
L vector biu din im sau khi chuyn i.
3.3.1. Php bin i tnh tin.
Gi s cn tnh tin 1 im hay hay 1 vt th theo vector dn :
h a.i b. j c.k
Ma trn chuyn i tnh tin theo vector dn :
1 0 0 a
0 1 0 b
H
0 0 1 c
0 0 0 1
Gi u l vector biu din im cn tnh tin :

u= [x, y, z, 1]T
1 0 0 a x x a
0 1 0 b y y b

v H .u
0 0 1 c z z c
0 0 0 1 1 1
K hiu :
v= Trans(a,b,c).u
V d : Cho u 2.i 3. j 2.k
h 4.i 3. j 7.k
39
1
0
v H .u
0
0
1
0
0
0
0
1
0
4 2 6
3 3 0
7 2 9

1 1 1
v=Trans(4, -3, 7).u

3.3.2. Php quay quanh cc trc to :
Gi s ta cn quay 1 im hay vt th xung quanh 1 trc no vi gc
quay 0 ta ln lc c cc ma trn chuyn ng quay nh sau :
1
0
Rot ( x, )
0
0
cos
0
sin
sin
0
cos
0
cos
0
Rot ( y, )
sin
cos
sin
Rot ( z , )
0
0
1
sin
0
0
0
cos
0
sin
cos
0
0
0
0
1
0
0
0
0
1
0
0
0
0
0
0
V d : u 7.i 3. j 2.k
Quay mt gc 900 quanh trc z : Rot(z, 90), sau tip tc cho quay y 1
gc 900 : Rot(y, 90)
Thc hin chuyn i :
v Rot ( z,90 ).u

0
1
v R.u
0
1 0
0 0
0
0
1
0
0 7 3
0 3 7
0 2 2

1 1 1
Tip tc cho quay quanh y 1 gc 900 :

40
W= Rot(y, 90).v
0
0
v R.u
1
0
1
1
0
0
0
0
0
0 3 2
0 7 7
0 2 3

1 1 1
Vy c th tnh :
W Rot(y,90 ).Rot ( z,90 ).u
Ch :
+ Php quay cn tun th theo ng th t trc sau .
Trong v d : quay quanh trc z trc, trc y sau, ta k hiu : Rot(y,
90).Rot(z, 90).u
+ V cc php quay cho cc ma trn nn :
Rot(y, 90).Rot(z, 90).u Rot(z,90).Rot(y,90).u
3.3.3. Php quay le( Euler)
Trong thc t vic nh hng khu chp hnh cui thng l kt qu ca
cc php quay quanh trc x, y, z.
Php quay le m t kh nng nh hng ca cc khu chp hnh cui
thng qua cc gc quay , , bi cc php bin i sau :
+ Quay 1 gc quanh trc z.
+ Quay 1 gc quanh trc y mi l y
+ Quay 1 gc quanh trc z mi l z
Euler ( , , ) Rot ( z, ).Rot ( y, ).Rot ( z, ) Rot ( z, ).Rot ( y, ).Rot ( z, )
Ch :
Php quay phi theo th t trc sau , nhng c bit vi php quay le th
s thay i th t khng lm thay i kt qu.
Cng thc tnh :
Euler ( , , ) Rot ( z, ).Rot ( y, ).Rot ( z, )
cos
0
Rot ( z , )
sin
0
1
sin
0
0
0
cos
0
0 cos
0 sin
0 0
1 0
41
sin
cos
0
0
0
0
1
0
0
0
0
cos cos cos sin sin

sin cos cos cos sin
sin cos
cos cos sin sin cos

sin cos sin cos cos
cos sin
sin sin
sin sin
0
cos
0
3.3.4. Php quay roll - pitch yaw.

L php quay dng nh hng khu chp hnh cui thng c dng
trong thc t.
Ta tng tng gn h to xyz ln thn mt con tu
YAW
ROLL
O
PITCH
+ Roll- Chuyn ng lc ca thn tu tng ng vi trc z ca thn tu 1 gc

+ Pitch- Chuyn ng nhp nh ca thn tu tng ng vi vic quay quanh
trc y 1 gc
+ Yaw- Chuyn ng lch hng tng ng vi vic quay quanh trc x 1 gc
z
y
x
Ngi ta s dng php quay ny biu din chuyn ng ca Robot.
Phng php ny c s dng kh ph bin.
42
0
0
0
RPY ( , , ) Rot ( z, ).Rot ( y, ).Rot ( x, )

0 sin 0 1
0
0
0
cos
0
1
0
0
0
cos
sin
Rot ( z , )
sin 0 cos 0 0 sin
cos
0
0
0
1 0
0
0
1
0
cos cos cos sin sin sin cos cos sin cos sin sin
sin cos sin cos sin cos cos sin sin cos cos sin
sin
cos sin
cos cos
0
0
0
0
0
0
Hay c th vit :
C C C S S S C C S C S S
RPY ( , , ) S C S S S C C S S C C S
S
C S
C C
3.4. Bin i h to v mi quan h gia cc h to

3.4.1. Bin i h to .
Gi s cn tnh tin gc to cac O(0,0,0) theo mt vector dn
h 4.i 3. j 7.k th kt qu ta c to im OT :
0
0
OT H .O
0
0
1
0
0
0
0
1
0
4 0 4
3 0 3
7 0 7

1 1 1
Nu ta tip tc thc hin cc php quay i vi h to OT th ta c

h to mi :
+ Nu chn h qui chiu l h to gc th ta thc hin cc php bin i t
phi sang tri :
A Rot(y,90 ).Rot ( z,90 )
43
zT
z'T
Rot(z,-90)
Rot(y,90)
x'T
OT
x'T
OT
OT
yT
xT
z'T
y'T
y'T
+ Nu chn h qui chiu l h to trung gian th ta htc hin cc php bin

i t tri sang phi :
A Rot(y,90 ).Rot ( z,90 )
zT
Rot (yT , 90)
Rot (z'T ,-90)
OT
OT
y'T
yT
xT
x''T
OT
z'T
z''T
x'T
3.4.2. Mi quan h gia cc h to .

Gi s c 3 gc h to A, B, C th h to B c mi quan h vi h to
A c biu din :
B A TBA
C B TCB
44
y''T
zC
P
pC
pA
zB
yC
xC
yB
zA
xB
A
y
xA
Gi s c im P trong h to C c biu din pC . Xc nh mi quan

h ca P trong h to A.
B
Trc ht cn xc nh pB : pB TC . pC
p A TBC . pB TBA .TCB

A
A
B
Vy : TC TB .TC
Tnh cht :
B A TBA
A B TAB
TAB (TBA ) 1
3.5. M t vt th
Vt th l cc i tng lm vic ca Robot . Da vo c im hnh hc
ca chng , ta c th chia chng thnh 3 nhm sau :
+ Nhm cc vt th trn xoay : ngoi gi tr ca v tr v kch thc, ta cn xc
nh to tm v bn knh ca ng cong.
+ Nhm cc vt th c gc cnh : Gi tr c trng l to cc im gii hn.
+ Nhm cc vt th c cu trc hn hp
i vi hot ng cm nm i tng v qu trnh vn ng ca Robot th
vic m t vt th cn phi gn lin vi cc php bin i thun nht.
V d : Cho vt th hnh ln tr t trong h to oxyz nh hnh v :
45
C
z
B
D
y
E
A
x
m t v tr ca vt th ta dng ma trn ca 6 im nh sau, phn t ca

hng cui cng chnh l gi tr w = 1.
1 1 1 1 1 1
0 0
0
0
4
4
A
0 0
2 2 0 0
1 1 1 1
1 1
A A B C D E F
Yu cu : Thc hin cc php bin i : H=Trans(4,0,0) Rot(y,90)
Rot(z,90).
Thc hin nhn cc ma trn thun nht ca cc php bin i theo ng th
t nh trn , ta thu c ma trn H nh sau :
0 0 1 4
1 0 0 0
H
0 1 0 0
0 0 0 1
0
1
A' H . A
0
4
1
A'
0
0
0
1
0
1
0
0
0
4 1
0 0
0 0
1 1
4
6 6
1 1 1
4
1
0
1
4
1
0
1
0
1
1 1 1
0
0 0
1
4
0
1
0
1
2
1
4
1
4
1
46
2
1
1
4
0
Kim tra li bng hnh v : Dng h qui chiu l h to gc.

H=Trans(4,0,0) Rot(y,90) Rot(z,90)
Thc hin ln lc theo th t : Quay quanh trc z , quay quanh trc y, tnh
tin so vi h to gc.
+ Rot(z,90) :
y'
z'
x'
+ Rot(y,90) :
y''
x'
z''
+ Trans(4,0,0) :
y''
O
4
z''
47
x'
Chng 4: Phng trnh ng hc robot
Chng 4
PHNG TRNH NG HC ROBOT

4.1. Dn nhp
Bt k mt Robot no cng bao gm cc khu lin kt vi nhau thng qua
cc khp. Hai chuyn ng c bn ca cc khu thng qua khp quay v khp
tnh tin.
Hnh 4.1. Khp quay v khp tnh tin trong chuyn ng ca robot.
Ta t trn mi khu ca mt Robot mt h trc to . S dng cc php
bin i thun nht c th m t v tr tng i v hng gia cc h to
ny.
Theo Denavit, mi lin h gia hai khu lin k nhau (khu n so vi khu
(n-1)) c m t bi ma trn A l ma trn bin i thun nht gm c cc php
quay v tnh tin gia cc h to vi nhau.
Hnh 4.2. t h trc to cho cc khu ca robot Puma.

Vy, A1 l ma trn m t hng v v tr ca h to gn trn khu th
nht so vi h to gc.
48
Tng t cho A2 , l ma trn m t mi quan h v hng v v tr ca h

to th hai so vi h to gn trn khu th nht.
Tch ca cc ma trn A l ma trn T (Theo Denavit).
V d : T3= A1.A2.A3
Hnh 4.3. Cc vector nh v v nh hng ca tay my.

Lu :
+ Nu mt Robot c 6 khu th :
T6=A1A2A3 A4A5A6.
T6 c gi l ma trn vector cui , m t hng v v tr ca h to gn
ln khu chp hnh cui so vi h to gc.
+ Nu mt Robot c s bc t do w>3 th 3 bc t do u tin dng nh v,
cc bc t do cn li nh hng.

+ H to biu din khu chp hnh cui (im tc ng cui) n o a

:
3 vector ch phng ca h to gn trn khu chp hnh cui, (im tc
ng cui) xc nh bi :
a : Vector c hng m theo bn tay s tip cn n i tng.
o : Vector c hng theo cc ngn tay cm nm hay th i tng.

n : Vector php tuyn ca o v a : n o.a
nx ox a x p x
n o
a y p y
y
y
T6
nz oz a z p z
0
0
1
0
4.2. B thng s Denavit-Hartenberg (DH)
4.2.1. Cc khi nim :
49
Mt Robot gm nhiu khu cu thnh t nhng khu ni tip nhau thng

qua cc khp ng.
Gc chun ca 1 Robot l l khu s 0 v khng tnh vo s cc khu. Khu
1 ni vi khu chun bi khp 1, khng c khp u mt khu cui cng
4.2.2. di php tuyn chung v gc gia hai trc khp :
Bt k mt khu no cng c c trng bi hai yu t :
+ di php tuyn chung an
+ Gc gia cc trc khp o trong mt phng vuuong gc vi an , k hiu l n
Hnh 4.4. Chiu di gc xon ca khu.
n :Gc xon ca khu n( Khp n so vi khp (n+1))

an : Chiu di ca khu n ( Khp n so vi khp (n+1))
Hnh 4.5. Cc thng s ca khu : an, n, dn, n

Cc trng hp c bit :
+ n =0,an =const(2 trc khp song song)
+ / n /=90, an =const (2 trc khp vung gc)
+ n =0(180), an =0 (2 trc khp trng nhau )
50
+ / n /=90, an =0 (2 trc khp ct nhau v vung gc nhau)
Hnh 4.6. Cc trng hp c bit ca phng hai trc khp

4.2.3. Khong cch gia hai khu v gc quay gia hai khu.
Tip tc kho st mi quan h gia cc khu lin k nhau, ph bin l hai
khu lin kt nhau chnh trc ca khp :
Hnh 4.7. Khong cch hai khu v gc quay gia hai khu.
Mi trc khp c hai ng php tuyn chung i vi n, khong cch
gia hai ng php tuyn chung o dc theo trc khp n gi l d n
dn cn gi l khong cch gia hai khu : Khu n so vi khu th (n-1)
Gc gia hai ng php tuyn chung o trong mt phng vung gc vi
trc khp th n l gc n.
n l gc quay ca khu th n so vi khu th (n-1)
4.2.4. B thng s Denavit-Hertenberg :
51
C 4 thng s xc nh trn chnh l b thng s DH : n , an, dn, n

Vi 4 thng s trn , ta c th xc nh v tr v hng ca mi khu so vi
nhau v so vi to gc
Nu khp ni hai khu l khp quay th n l bin khp ( 3 thng s cn li
l hng s)
Nu khp ni l tnh tin th dn l bin khp :( n =0, an =0, n =const)
4.3. Gn h to cho Robot .
kho st ng hc ca Robot ta phi gn trn mi khu ca robot mt
h to . Nguyn tc chung gn h to nh sau :
a. Gc ca h to :
Gc to ca khu th n nm trn ng tm ca trc khp th (n+1) v
nm ti giao im ca ng php tuyn chung an vi trc khp th (n+1)
(Tng qut, cho nhau)
Nu hai trc khp ct nhau th gc to on nm ti chnh im ct .
Nu hai trc khp song song nhau th o n nm trn trc khp th n+1 v ti
mt mt v tr c bit no qu trnh tnh ton l thun li nht.
b. Chn trc Zn :
Trc Zn nm dc theo trc khp th n+1 v c hng v pha cc khu.
c. Chn trc Xn :
Trc Xn nm dc theo ng php tuyn chung hng t trc khp th n
n trc khp th n+1.
Nu hai trc khp ct nhau th xn z n .z n1

d. Chn trc yn theo qui tc bn tay phi.
V d 1: Gn h to v xc nh cc thng s DH cho Robot c hai khu
phng :
52
Hnh 4.8. Xc nh h trc ta v b thng s DH robot hai khp quay phng

B thng s DH ca robot c xc nh :
V d 2: Gn h to v xc nh b thng s DH cho Robot Scara :

a2
a1
d*3
o0
y0
x0
z0
o1
y1
x1
z1
o2
y2
x2
z2
x3
o3
y3
z3
o4
y4
x4
z4
Hnh 4.9. Xc nh h trc ta v b thng s DH cho robot Scara.

B thng s DH :
1*
a1
2*
a2
d3
4*
d4
4.4. c trng ca cc ma trn A.

Ma trn A l ma trn m t mgh hng v v tr ca h to gn trn hai
khu lin k nhau.
Cn c vo thng s ca b DH th ma trn A c c trng bi 4 php
bin i sau :
i. Quay quanh trc zi-1 mt gc i.
ii. Tnh tin dc trc zi-1 mt qung di.
53
iii. Tnh tin dc trc xi-1 ( trng vi xi) mt on ai

iv. Quay quanh trc x1 mt gc i
Bn bc bin i ny c biu hin bng tch ca cc ma trn thun nht
nh sau:
Ai = R (z, i). Tp (0, 0, di). Tp (ai, 0, 0). R (x, i)
cos
sin
Rot ( z, )
0
0
1
0
H1
0
sin
cos
0
0
0 0 a
1 0 0
0 1 0
0 0 1
0
1
0 cos
Rot ( x, )
0 sin
0
0
1
0
H2
0
0 0 0
1 0 0
0 1 d
0 0 1
0
0
sin 0
cos 0
0
1
cos i
sin
i
Ai
0
Hay:
0 0
0 0
1 0
0 1
cos i sin i
cos i cos i
sin i sin i
sin i cos i
sin i
0
cos i
0
ai cos i
ai sin i
di
Ma trn Ai c gi l ma trn chuyn i thun nht, n c dng

R
: Ai i
0
pi
vi Ri l ma trn quay 3 x 3 v pi l vect tnh tin 3 x 1.
1
Lu :
i vi khp tnh tin th i =a=0 nn:
0
1
0 cos
Ai
0 sin
0
0
0
sin
cos
0
0
0
d
1
54
4.5 Xc nh cc ma trn T theo ma trn A.

nht so vi h to gc.
V d : T3= A1.A2.A3
Nu mt Robot c 6 khu th :
T6=A1A2A3 A4A5A6.
H to biu din khu chp hnh cui (im tc ng cui)
n o a : 3 vector ch phng ca h to gn trn khu chp hnh
cui, (im tc ng cui) xc nh bi :
+ a : Vector c hng m theo bn tay s tip cn n i tng.
+ o : Vector c hng theo cc ngn tay cm nm hay th i tng.

+ n : Vector php tuyn ca o v a : n o.a
nx
n
T6 y
nz
ox
oy
ax
ay
oz
0
az
0
px
p y
pz
Ta c th xc nh ma trn T thng qua h to trung gian :

n
Tn Ai
n 1
i 1
Vi : T3 A3
2
T3 A2 A3
4.6. Trnh t thit lp phng trnh ng hc ca robot.

4.6.1. Cc bc thc hin
thit lp phng trnh ng hc ca robot, ta thc hin cc bc sau :
1. Bc1: Chn h to c bn v gn cc h to trung gian khc :
+ Gi nh v tr ban u ca Robot, l v tr cc bin khp thng bng 0
+ Chn gc h to O0, O1
+ Chn trc Z0, Z1 theo nguyn tc chung.
55
Vi cc robot c w<= 3 th khng th nh hng cho trc Zn chn tu .

+ Chn cc trc x0, x1
V ma trn Ai = R (z, i). Tp (0, 0, di). Tp (ai, 0, 0). R (x, i)
nn trc xn-1 chnh l trc quay zn-1 thnh trc Zn :
Lc ny : n= (Zn-1, Zn)
+ Chn trc y theo nguyn tc bn tay phi.
* Lu :
Trong qu trnh gn htd th khi xut hien cc phop bin i : Trans(0.y,0)
v Rot(y,theta) th v tr gi nh ban u l khng ng, cn thay i v tr mi.
2. Bc 2: Lp bng thng s DH.
3. Bc 3: Xc nh cc ma trn Ai
4. Bc 4: Tnh cc ma trn T t ngn ti gc. T4=A1A2A3A4
Tnh ngc t sau ra trc (Thng thng)
5. Bc 5: Vit phng trnh ng hc Robot
4.6.2. Cc v d thit lp phng trnh ng hc :
1. V d 1. Xc nh phng trnh ng hc ca Robot hai bc t do RT
Gn h trc to cho Robot :
z2
y2
O2
x2
z1
y1
x1
O1
l1
y0
O0
x0
z0
Hnh 4.10. Gn h to c bn v cc h to trung gian cho Robot

Khu 1 : Quay quanh trc Z0, chn X0 l php tuyn chung ca (Z0, Z1).
Khu 2 : Tnh tin dc theo trc Z1, chn X1 nm ngang.
Xc nh b thng s DH :
56
Khu
1
2
ai
di
1*
0
90
l1
d 2*
*
*
Cc bin khp : 1 , d 2
Phng trnh ng hc :
+ Cc ma trn c trng A :
c1 0 s1
s1 0 c1
A1
0 1 0
0
0 0
1
0
A2
0
0
0
l1
0
0
0 1 d2
0 0 1
cos i cos i sin i sin i sin i ai cos i

sin
cos
cos
sin
cos
a
sin
i
i
i
i
i
i
i
Ai
0
sin i
cos i
di
0
0
1
0
0
0
0
1
0 cos sin 0
Ai
0 sin cos d
0
0
1
0
0 0
1 0
+ Ma trn vector cui :
c1 0 s1
s1 0
c1
T A1 A2
0 1 0
0
0 0
0
0
l1
1
0
0 c1 0 s1 d 2 s1
0 s1 0 c1 d 2 c1
0 1 d2 0 1 0
l1

0 0 1 0 0
0
1
0 0
1 0
+ Phng trnh ng hc th hin mi quan h v hng v v tr ca ma trn

vector cui theo cc bin khp :

Ba vector ch hng : n, o, a
nx cos 1
ox 0
a x sin 1
n y sin 1 ,
nz 0
oy 0 ,
o z 1
az 0
a y cos 1
Vector nh v : p
p x d 2 sin 1
p y d 2 cos 1
p z l1
1. V d 2. Xc nh phng trnh ng hc Robot c cu hnh RRT
57
Hnh 4.11. Robot hai khu RT
i. Gn h to cho Robot :
Hnh 4.12. Gn h ta ti
chn
v tr ban u cho.
Hnh 4.13. Gn h ta ti v tr la
ii. B thng s DH :
Khu
1
2
3
iii.
cos i
sin
i
Ai
0
ai
di
1*
+90
d1
2*
0
-90
3*
Xc
nh
cc
ma
cos i sin i sin i sin i ai cos i
cos i cos i sin i cos i ai sin i
sin i
cos i
di
0
1
58
trn
Qui uc :
cos 1 = c1
cos 2 = c2
c1c2-s1s2 = cos1 2 = c12

s3c4+c3s4= sin 1 2 = s34
c1c23-s1s23= cos1 2 3 = c123

c1
s1
A1
0
0 s1
0 c1
1
0
0
0
0
0
d1
c 2 0 s 2
s2 0 c2
A2
0 1 0
0
0 0
1
0
A2
0
0 0
1 c2
0
0
1
0
0
0
0
0
0
d 3
c1c 2 s1 c1s 2 c1s 2d 3

s1c 2 c1 s1s 2 s1s 2d 3
T3
s2
0
c2
c 2d 3 d1
0
0
1
0
iv. Vit phng trnh ng hc :

nx
n
T3 y
nz
ox
oy
ax
ay
oz
0
az
0
px
p y
pz
3. V d 3 : Xc nh phng trnh ng hc cho Robot 3 khp quay phng
59
i. B thng s DH :
1
1*
a1
2*
a2
3*
a3
ii. Xc nh cc ma trn A
cos i
sin
i
Ai
0
cos i sin i
cos i cos i
sin i sin i
sin i cos i
sin i
0
cos i
0
ai cos i
ai sin i
di
iii. Tm phng trnh ng hc :

Tng t, thay vo tnh A1 v T3:
c123
s123
T3
0
s123
c123
0
0
0
0
1
0
c123a3 c12a 2 c1a1

s123a3 s12a 2 s1a1
4. V d 4. Xc nh phng trnh ng hc ca robot Puma 6 bc t do.

Robot Puma l sn phm ca cng ty Unimate (USA), l loi robot c 6
bc t do c s dng ti nhiu nc trn th gii.
60
i. Gn h ta cho robot Puma.
Hnh 4. Gn h ta cho robot Puma.

ii. B thng s D-H ca robot Puma :
iii. Phng trnh ng hc ca robot Puma c s khp n = 6

61
c1
s
0
1
1T
0
3T
s1
c1
0
0
c 3
s 3
s 3
c 3
c 5
0
4
5T
s 5
s 5
0
c 5
0
0 0
0 0
1 0
0 1
c 2 s 2 0 0
0
0 1 0
1
2T
s 2 c 2 0 0
0
0 1
0
a2
0 0
,
0 d3
0 1
c 3
0
3
4T
s 4
0
1 0
,
0 0
0 1
c 6 s 6 0 0
0
0
1 0
5
6T
s 6 c 6 0 0
0
0 1
0
s 3
0
c 4
0
a2
1 d 4
0 0
0 1
Ta c :
r11 r12 r12 Px

r
r
r
Py
21
22
23
0
0
1
2
3
4
5
6T 1T 2T 3T 4T 5T 6T
r31 r32 r33 Pz
0
0
0
1
Trong :
r11 c1 [c 23 (c 4 c5 c6 s 4 s5 ) s 23 s5 c5 ] s1 ( s 4 c5 c6 c 4 s 6 )
r21 s1 [c 23 (c 4 c5 c6 s 4 s 6 ) s 23 s5 c6 ] c1 ( s 4 c5 c6 c 4 s 6 )
r31 s 23 (c 4 c5 c6 s 4 s 6 ) c 23 s5 c6
r12 c1 [c 23 (c 4 c5 s 6 s 4 c6 ) s 23 s5 s 6 ] s1 (c 4 c6 s 4 c5 s 6 )
r22 s1 [c 23 (c 4 c5 s 6 s 4 c6 ) s 23 s5 s 6 ] c1 (c 4 c6 s 4 c5 s 6 )
r32 s 23 (c 4 c5 s 6 s 4 c6 ) c 23 s5 s 6
r13 c1 (c 23c 4 c5 s 23c5 ) s1 s 4 s5 ]
r23 s1 (c 23c 4 c5 s 23c5 ) c1 s 4 s5 )
r33 s 23c 4 s5 c 23c5
Px c1 [a 2 c 2 a3 c 23 d 4 s 23 ] d 3 s1
62
Py s1 [a 2 c 2 a3 c 23 d 4 s 23 ] d 3 c1
Pz a3 s 23 a 2 s 2 d 4 c 23
r32 s 23 (c 4 c5 s 6 s 4 c6 ) c 23 s5 s 6
r13 c1 (c 23c 4 c5 s 23c5 ) s1 s 4 s5 ]
r23 s1 (c 23c 4 c5 s 23c5 ) c1 s 4 s5 )
r33 s 23c 4 s5 c 23c5
Px c1 [a 2 c 2 a3 c 23 d 4 s 23 ] d 3 s1
Py s1 [a 2 c 2 a3 c 23 d 4 s 23 ] d 3 c1
Pz a3 s 23 a 2 s 2 d 4 c 23
63
Chng 5: ng lc hc robot v ng dng trong iu khin
Chng 5
NG LC HC ROBOT V NG DNG
TRONG IU KHIN
5.1. Mc ch v phng php kho st ng lc hc robot
Vi nhng mc ch thit k v iu khin, cn thit phi c mt m hnh
ton hc m t ng lc hc ca h thng. V th, chng ny ta s xc lp
phng trnh chuyn ng ca tay my di dng phng trnh vi phn. Phng
php p dng y l xy dng phng trnh chuyn ng ca c h da trn
quan h nng lng, xut pht t nguyn l bo ton v chuyn ha nng lng
trn c s xc lp quan h gia ng nng v th nng ca c h tay my, sau
s dng phng trnh vi phn ca chuyn ng trn c h vi cc i lng
tham gia vo phng trnh gm lc, qun tnh v nng lng.
Vic nghin cu ng lc hc Robot thng gii quyt hai nhim v sau :
1. Xc nh momen v lc ng trong qu trnh chuyn ng. Khi qui lut
bin i ca bin khp qi(t) xem nh bit.
Vic tnh ton lc cng nh momen trong c cu tay my l nhim v tt
yu trong vic la chn cng sut ng c, tnh ton kim tra bn, cng
vng, m bo tin cy cho Robot.
2. Xc nh cc sai s ng, tc l sai s xut hin so vi qui lut chuyn ng
trong chng trnh.
C nhiu phng php nghin cu ng lc hc Robot, nhng nhiu hn
c l phng php c hc Lagrange, c th l phng trnh Lagrange-Euler.
Trong phm vi ni dung ca mn hc ny, chng ta tm hiu nhim v th
nht, t to c s cho vic lp trnh v iu khin robot.
Hm Lagrange ca mt h thng nng lng c nh ngha :
L= K P
Trong : K l tng ng nng ca c h
L l tng th nng ca c h
K v P u l nhng i lng v hng, nn c th chn bt k h ta
no gi bi ton n gin.
Xt mt Robot c n khu th :
64

n
K Ki
P Pi
i 1
i 1
(2.1)
Trong , Ki v Pi l ng nng v th nng ca khu th i xt trong h ta

chn. l cc i lng ph thuc vo nhiu bin s :
Ki K qi , qi v Pi Pqi , qi
(2.2)
Vi qi l ta suy rng ca khp th i.

nh ngha : Lc (hay momen) tng qut tc dng ln khu th i c xc
nh bi phng trnh Lagrange :
F
d L L
dt q q
5.3. Kho st bi ton ng lc hc ca tay my nhiu bc t do

Phng trnh chuyn ng Lagrange thit lp cho mt c h c cho bi:
d L L
dt q q
(2.3)
Trong q l vect biu din cc to suy rng ca cc khu ca Tay

my qi, l vect biu din cc lc suy rng ca cc khu ca tay my v hm
Lagrange l s chnh lch gia ng nng v th nng ca c h :
L K P
(2.4)
a. V d 1.
Ta xt v d xy dng phng trnh chuyn ng ca tay my hai khu phng
lin kt bng khp bn l.
Trong v d ny, ta p dng cc kt qu ca bi ton ng hc c kho
st phn trc. xy dng bi ton ng lc hc, ta kho st c h vi gi
thit rng khi lng ca khu c tp trung cc khp. Ma trn bin khp l:
q 1 2
(2.5)
v ma trn biu din ca lc suy rng c th hin:
(2.6)
vi 1 , 2 l cc m men c cho bi cc c cu tc ng (chng hn l m
men pht ng ca cc ng c in).
65
(x2,y2)
2
a2
m2
g
a1
m1
x
Hnh 5.1: Tay my hai khu bn l

Biu thc ng nng v th nng
Vi khu 1, ta c biu thc ca ng nng v th nng tng ng l:
2 2
K1 1 2 m1a1 1
(2.7)
P1 m1 ga1 sin1
(2.8)
Vi khu 2 ta c:
x2 a1 cos1 a2 cos(1 2 )
(2.9)
y2 a1 sin1 a2 sin(1 2 )
(2.10)
x2 a11 sin1 a2 (1 2 ) sin(1 2 )
(2.11)
y 2 a11 cos1 a2 (1 2 ) cos(1 2 )

(2.12)
Bnh phng vn tc l :
v22 x22 y 22 a1212 a22 (1 2 )2 2a1a2 (12 12 ) cos 2

(2.13)
Do vy ng nng ca khu 2 l:
K2 1 2 m2v22 1 2 m2 a1212 1 2 m2 a 22 (1 2 )2 m2 a1a2 (12 12 ) cos 2
(2.14)
Th nng cho khu 2 l:
P2 m2 gy2 m2 g[a1 sin1 a2 sin(1 2 )]

(2.15)
66
Phng trnh Lagrange

Hm Lagrange cho Tay my ny l:
2 2
L K P K1 K 2 P1 P2 1 2 (m1 m2 )a1 1 1 2 m2 a 22 (1 2 ) 2
(2.16
m2 a1a2 (12 12 ) cos 2 (m1 m2 ) ga1 sin 1 m2 ga2 sin(1 2 )
)
Ta cn xc nh cc biu thc :
L
2
(m1 m2 )a1 1 m2 a 22 (1 2 ) m2 a1 a 2 (21 2 ) cos 2
1
d L
2
(m1 m2 )a1 1 m2 a 22 (1 2 ) m2 a1 a 2 (21 2 ) cos 2 m2 a1 a 2 (212 22 ) cos 2
dt 1
L
(m1 m2 ) ga1 cos 1 m2 ga 2 cos(1 2 )
1
L
m2 a 22 (1 2 ) m2 a1 a 21 cos 2
2
d L
m2 a 22 (1 2 ) m2 a1 a 21 cos 2 m2 a1 a 212 sin 2
dt 2
L
m2 a1 a 2 (12 12 ) sin 2 m2 ga 2 cos(1 2 )
2
Cui cng, phng trnh chuyn ng ca c h tay my c cho bi h
hai phng trnh vi phn:
1 [(m1 m2 )a12 m2 a22 2m1m2cos 2 ] 1

[m2 a22 m2 a1a2cos 2 ] 2 m2 a1a2 (2 1 2 22 ) sin 2
(m1 m2 ) ga1cos 1 m2 ga2cos (1 2 )
(2.18)
2 [m2 a22 m2 a1a2cos 2 ]1 m2 a22 2 m2 a1a2 12 sin 2

m2 ga2cos (1 2 )
Biu din phng trnh chuyn ng ca c h Tay my di dng ma trn
Di dng ma trn, phng trnh chuyn ng hay phng trnh ng lc
hc Tay my di dng ma trn c th vit nh sau:
67
(m1 m2 )a12 m2 a22 2m1m2cos 2 m2 a22 m2 a1a2cos 2 1

(2.19)

2
2
m2 a2 m2 a1a2cos 2
m2 a2
2
m2 a1a2 (2 1 2 22 ) sin 2 (m1 m2 ) ga1cos1 m2 ga2cos (1 2 ) 1

2 sin
m
ga
cos
(
)
m
a
a
2
2
1
2
2
2 1 2 1
2

Ta tm c biu thc ng lc hc tay my di dng chun, c biu
din chung di dng sau :
M (q)q V (q, q ) G(q)

(2.20)
M(q) l ma trn qun tnh, V (q, q ) l vect lc Coriolis hoc/v lc hng
tm v G(q) l vect trng lc.
Vi biu thc trn M(q) l ma trn i xng.
b. V d 2.
Xy dng Phng trnh ng lc hc ca robot hai bc t do cu hnh RT.
d2
O0
Hnh 5.3. Cu hnh ca Robot 2 bc t do RP

Xut pht t phng php ng lc hc cho h c hc tng qut
68
d L L
dt q q
(2.1)

Lagrange l s chnh lch gia ng nng v th nng ca c h, vi:
L K P
(2.2)
Tng t v d 1, ta kho st c h vi gi thit rng khi lng ca khu

c tp trung cc khp. Ma trn bin khp l:
q 1
d2
(2.3)
(2.4)
vi 1 , 2 l cc m men c cho bi cc c cu tc ng (chng hn l

m men pht ng ca cc ng c in).
y
m 2 ( x 2 ,y 2)
l1
m 1(x 1,y 1 )
d2
Hnh 5.4. To ca cc khu trn Robot

+ Vi khu 1 chuyn ng quay, ta c biu thc ca ng nng v th nng
2 2
K1 1 2 m1l1 1
tng ng l:
P1 m1 gl1 sin 1
(2.5)
(2.6)
+ Vi khu 2 chuyn ng tnh tin, ta c:
x2 d 2 cos 1
y2 d 2 sin 1
(2.8)
69
(2.7)
x2 d2 cos 1 d 21 sin 1
(2.9)
y 2 d2 sin 1 d 21 cos 1
(2.10)
Bnh phng vn tc l :
2 2
2
v22 x22 y 22 d 2 1 d2
(2.11)
K2
1
1
1
2 2
2
m2 v22 m2 d 2 1 m2 d2
2
2
2
(2.12)
Th nng cho khu 2 l:
P2 m2 gy2 m2 gd2 sin 1

(2.13)
Phng trnh Lagrange
1 2 2 1
1
2 2
2
L K P K1 K 2 P1 P2 m1l1 1 m2 d 2 1 m2 d2 m1 gl1 sin 1 m2 gd 2 sin 1
2
2
2
1
1 2
2
2 2
Vy : L (m1l1 m2 d 2 )1 m2 d 2 (m1l1 m2 d 2 ) g sin 1
2
2
(2.14)
Nhng hng thc cn tnh c th hin nh di y:
70
L
2
2
(
m
l
m
d
)1
1
1
2
2
1
d L
2
d 2 )
m
l
m
(
2
d
d
1
1
1
2
2
2 1
2
1
dt 1
L
(m1l1 m2 d 2 ) g cos 1
1
L
m2 d2
d 2
(2.15)
)
d L
m2 d2
dt d 2
L
2
m2 d 21 m2 g sin 1
d 2
Cui cng, phng trnh chuyn ng ca c h Tay my c cho bi h
d L L
2
2
m1l1 1 m2 (2d 2 d21 d 2 1 ) (m1l1 m2 d 2 ) g cos 1

dt 1 1
Vy :
d L L
dt q q
1 (m1l12 m2 d 2 2 )1 2m21d 2 d2 (m1l1 m2 d 2 ) g cos 1
d L L
m d 2 m g sin
m
d
2 2
2 2 1
2
1
dt d2 d 2
Vy :
2 m2 d2 m2 d 212 m2 g sin 1

hc tay my c th vit nh sau:
1 (m1l12 m2d 2 2 )1 2m21d 2d2 (m1l1 m2 d2 ) g cos 1

2
d L L
m d 2 m g sin
m
d
2
2
2 2 1
2
1
dt d2 d 2
m1l12 m2 d 2 2 0 1 2m2 d 21d 2 (m1l1 m2 d 2 ) g cos 1 1
m
g
sin
0
m2 d 2 m2 d 21
2
1
2
71
5.4. Phng trnh ng lc hc tay my.

5.4.1. Tng qut.
Chng ta ch ra cc v d ng dng phng trnh Lagrange tnh ton
nhng phng trnh ng lc hc ca cc Tay my. Trong cc v d trn v
ng lc hc ta nhn thy biu thc kt qu c dng:
M (q)q V (q, q ) G(q)

vi q l bin khp, l vect lc hoc m men suy rng.
nhn c phng trnh ng lc hc ca tay my ta bt u t vic xc
nh ng nng v th nng ca c h, xy dng hm Lagrange, sau a cc
hng thc vo phng trnh Lagrange, thu gn ta s nhn c phng trnh
chuyn ng ca c h Tay my.
xy dng m hnh ng lc hc tay my bng cch s dng phng
trnh Lagrange loi II, ta cn phi bit cc thng s sau y:
Khi lng cng nh ta ca khi tm ca cc khu,
Vn tc ca im bt k trn Tay my thit k,
Cc thng s v ma st ng, ma st tnh gia cc khu, khp v tc ng
nhiu nu c.
Do trong thc t, hot ng ca Tay my lun b nh hng bi cc lc ma
st v nhiu, nn ta s khi qut m hnh ng lc hc Tay my va nhn c
nh sau:
M (q)q V (q, q ) F (q ) G(q) d
vi q v c nh ngha trn. M(q) l ma trn qun tnh, V (q, q ) l vect

lc Coriolis/hng tm v G(q) l vect trng lc nh phn tch trn.
phng trnh khi qut trn, ta cng thm lc ma st vo , vi:
F (q ) Fv q Fd
trong Fv l ma trn h s ca ma st tnh v Fd l ma st ng. Ta s a
thm lng nhiu d vo phng trnh, i lng ny gip m t phn b cho
trng hp m hnh ng lc hc c sai st m ta cha lng ht trong qu trnh
xy dng m hnh ton.
Vic xc nh lc ma st rt kh khn, cch m t nh vy c chp nhn.
Hu ht nhng tr lc no chng li chuyn ng u c cc nh nghin cu
m t trong m hnh ng lc hc Tay my theo cch nh trn.
Phng trnh ng lc hc Tay my cng c biu din di dng:
N ( q, q ) d
M ( q)q
72
N (q, q ) V (q, q ) F (q ) G(q)

biu din cho c cc i lng phi tuyn.
5.4.2. Ma trn qun tnh
Ma trn qun tnh M(q) n x n c cc thnh phn c nh ngha bi biu
thc:
Ti
TiT
m jk ( q ) trace
Ii
q
q k
i 1
j
n
- Ti / q j m t s thay i v tr ca im thuc khu th i gy nn bi s

chuyn dch ca khu th j.
- Ii l ma trn qun tnh gi ca khu i v c xc nh di dng khai trin
nh sau:
x 2 dm
xy dm
I i i ri i riT dm
xz dm
x dm
yx dm
y dm
yz dm
y dm
2
zx dm
zy dm
z dm
z dm
2
x dm
y dm
z dm
dm
y cc gi tr c tnh trn khu th i. y l ma trn hng s v xc

nh gi tr mt ln cho mi khu. Ma trn ny ph thuc vo dng hnh hc v
s phn b khi lng ca khu i. Trong cc thnh phn qun tnh c phn
bit nh sau:
M men qun tnh:
I yy ( x 2 z 2 )dm
I zz ( x 2 y 2 )dm
I xx ( y 2 z 2 )dm
M men qun tnh ly tm:
I xz xz dm
I yz yz dm
I xy xy dm
73
my y dm
mz z dm
mx x dm
M men qun tnh bc nht:
vi m l tng khi lng khu i, v:

i
ri x
l bn knh vect biu din trng tm khu th i trong h ta i.

Ta c th vit :
I xx I yy I zz
I xy
Ii
I xz
mx
Vi
Ti / q j
I xy
I xz
I xx I yy I zz
2
I yz
I xx I yy I zz
I yz
2
mz
my
mx
my
mz
= 0, j>i ta c th vit ngn gn hn :
T TiT
m jk (q) trace
Ii
i max( j , k )
q j qk
n
y l mt ma trn i xng dng

5.4.3. Vect coriolis/hng tm
1 T
K
V ( q, q ) M ( q )q
( q M ( q )q ) M q
2 q
q
Cc thnh phn ca vect Coriolis/hng tm c xc nh nh sau:
V (q, q ) vijk qi q j
i, j
vijk
1 mkj mki mij
2 qi
q j
q k
5.4.4.Vect trng lc:

Ta c
74
G( q )
P( q )
q
( g T Ti ( q ))I i e 4
i 1 q
n

e4 = (0, 0, 0, 1)
T , ta suy ra c:
( g T Ti ( q )) I i e 4
i 1 q
n
T
G (q ) ( I n g T ) i I i e 4
q
i 1
n
G ( q )
, j 1,2..., n
y tht s ta c vect G(q) l:
n T Ti
g q I i e 4
1
in1
T
i
g T
I i e4
G( q ) i 2
q 2
T
T
i
g
I i e4
i n
q n
n y ta kho st bi ton ng lc hc Tay my t thu c
cc gi tr lc hay m men suy rng trn mi khp trong qu trnh hot ng ca
robot. Da trn nhng thng s ny ta s a ra nhng gii php thit k kt cu
cng nh iu khin robot tt hn. Bi b iu khin s n gin v c hiu qu
hn nu nhng c tnh ng lc hc bit ca Tay my c kt hp cht ch
ngay t trong giai on thit k.
5.5. ng dng bi ton ng lc hc m t i tng robot trong iu
khin.
Sau khi thc hin tnh ton bi ton ng lc hc robot, chng ta c th s
dng trc tip cc m hnh ton thu c xy dng i tng trong vic m
phng v a ra cc tng trong vn iu khin.
Tt nhin, vic xc nh cc thng s ca robot l rt kh khn, v vy
chng ta ch xy dng i tng robot c tnh cht m phng thc hin cc
gii thut iu khin. V trong thc t, cc thng s ca m hnh ng lc hc
tay my chu nh hng ca rt nhiu cc yu t nh : chnh xc trong gia
cng c kh, nh hng ca cc tc nhn c tnh cht nh nhiu, cc sai s m
hnh khi thc hin tnh ton...
Trong mc ny, bng cc phn mm h tr m phng (Visual C, Visual
Basic, Matlab, ...) chng ta thc hin m hnh ha cc robot t cc phng trnh
75
ng hc v ng lc hc. T c s ny c th thc hin thit k v ch to cc

robot thc thi cc mc tiu ra.
Chng ta s thc hin vic m hnh ha cc i tng robot tm hiu
cc chng trc :
a. Xy dng m hnh m phng iu khin v tr ca robot Puma, da vo cc
phng trnh ng hc tm c chng 4.
Hnh 5.6. M phng robot Puma theo v tr
Hnh 5.7. M phng qu o ca robot Puma.

b. Xy dng m hnh ton cho robot hai bc t do cu hnh RT.
Do tnh cht phc tp trong iu khin, vn ca nhng nh nghin cu l
lm sao c th tm gii thut iu khin cho robot khi m tt c cc khu t thit
76
k n thi cng u gp nhiu kh khn. Mt cng c rt hu hiu c a ra

l m hnh ton ca robot, nn tng ca m hnh ton l bi ton ng lc hc
c xt n. Mc chnh xc , chnh lch sai s m hnh... ph thuc
nhiu vo qu trnh tnh ton ng lc hc, trong khng loi tr cc kh nng
nh hng ca nhiu v cc vn khc lin quan n ng lc hc c h.
Chng ta quay li v d 5.2, t bi ton ng lc hc xy dng cho robot
hai bc t do, cu hnh RT thu c m hnh ton ca i tng robot.
Xt trn lnh vc iu khin, h robot l cc h phi tuyn, chnh v vy vic
iu khin v s dng cc gii thut phi tun theo cc nguyn tc iu khin h
phi tuyn.
Xy dng m hnh robot RT trong matlab :
U1
Theta
(Dien ap dieu khien motor khop 1)
(Goc quay khop 1)
U2
d
(Do dai tinh tien d khop 2)

ROBOT_2DOF
Hnh 5.8. M hnh ton robot 2 bc t do RT

m phng thnh cng, chng ta cn chn cc thng s ca robot thch
hp. Cc thng s ny c th thu thp s liu hay la chn theo cc ti liu
c nghin cu.
1
u1
1
s
f(u)
theta_2dot
theta_dot
1
s
theta_
theta
2
u2
1
s
f(u)
d_2dot
d_dot
1
s
2
d
Hnh 5.9. M hnh ton t phng trnh ng lc hc robot.
77
d2
Chng 6: iu khin Robot
Chng 6
IU KHIN ROBOT
Vn trng tm ca chng ta trong lnh vc nghin cu robot l iu
khin chng theo cc mc tiu c th. Trong chng ny ta cn a ra cc
phng thc iu khin lm cho tay my i theo qu o yu cu c cho
trc. Qu o d kin i hi ngi lp trnh iu khin phi tm kim ng
i c tnh n nhng vn lin quan n mi trng ng dng nh trnh s va
chm, cc yu cu v tc p ng
Trong cc trng hp ng dng, ta khng th iu khin tay my di
chuyn c chnh xc tuyt i theo qu o d kin.V vy cn thc hin cc
thao tc sau tm qu o mong mun trong qu trnh iu khin. Th nht, ta
s ch ra cch thc bin i mt qu o theo mong mun t h ta Descartes
(Cartesian coordinates) qua h ta suy rng (Joint coordinates - hay khng
gian khp). Sau , a ra mt bng nhng im ta, l nhng im thuc qu
o d kin c ri rc ha m ta mong mun im trn khu tc ng cui
s i qua v t ta ch ra cch xy dng li mt qu o lin tc theo yu
cu.
6.1. Bin i qu o t h to Descartes sang khng gian khp
Trong cc ng dng ca robot, mt cng vic c th, v mt l thuyt ta c
th biu din trong khng gian Descartes; v , dch chuyn ca tay my
c m t d dng trong mi quan h v v tr ca n vi cc phn t khc
trong mi trng hot ng bn ngoi. Tuy nhin, vic iu khin chuyn ng
ca cc khu trn tay my sao cho im lm vic trn khu tc ng cui di
chuyn ng theo qu o cho trc li yu cu phi s dng khng gian khp
v vy ta cn s dng gii quyt c bi ton ng lc hc.
y ta cng ch mt kt qu bi ton ng hc ngc m ta bit
phn trc, l c nhiu li gii v chuyn ng ca cc khu thnh vin trong
khng gian khp qd(t) cho im trn khu tc ng cui di chuyn theo qu
o cho (bi ton v nh). V vy vic chn li gii duy nht trong s nhng
li gii c th c l mt vn cn quan tm.
Ngoi ra cch thc hin dch chuyn ca im trn khu tc ng cui
gia cc im ta (ni suy) nh hng n kh nng v phng php iu
khin. y, chng ta c th thc hin gii bi ton ng hc ngc trc tip
hay theo phng php tch nhm ba khu.
6.1.1. Ni suy ng a thc
Gi nh rng mt qu o yu cu c xc nh v c th hin hoc
78
trong khng gian Descartes hoc dng ng hc ngc, trong khng gian khp.
thun tin, ta dng bin khng gian khp q(t) cho k hiu. S khng thun
tin cho vic iu khin khi d liu v qu o vi s lng v hn cc im
c lu trong b nh my tnh, cho nn ta thng lu di dng mt s N hu
hn cc im ta v h qu l s c nhng gi tr qi(tk) tng ng cho mi bin
khp i m t nhng gi tr yu cu v v tr ca cc khu ti nhng im thi
gian ri rc tk. Theo cch q(tk) l mt im trong khng gian R n m bin
khp s i qua ti thi im tk. Ta gi chng l nhng im ta.
Hu ht cc k hoch iu khin robot yu cu mt qu o lin tc.
chuyn thnh mt bng cc im ta qi(tk) cho qu o mong mun qd(t), ta c
th s dng cc cch thc ni suy tu chn. Di y trnh by s lc v ni
suy a thc.
Gi nh rng cc im ta l khng gian ng dng trong thi gian v
c xc nh trn c s ly mu thi gian nh sau:
T t k 1 t k
(6.1)
di chuyn c trn, trong mi khong thi gian [tk+1,tk] ta cn n v
tr mong mun qd(t) v vn tc mong mun qd (t) hp vi bng im ta. Ta c:
qd i ( tk ) qi ( tk )
qd i ( tk ) qi ( tk )
qd i ( tk 1 ) qi ( tk 1 )
qd i ( tk 1 ) qi ( tk 1 )
(6.2)
ph hp vi nhng iu kin gii hn, rt cn thit dng khong [tk,tk+1]
ni suy a thc bc 3:
qdi (t ) ai (t tk )bi (t tk ) 2 ci (t tk )3 di
(6.3)
trong c 4 bin t do. :
q di (t ) bi 2(t t k )ci 3(t t k ) 2 d i

(6.4)
qdi (t ) 2ci 6(t t k )d i

(6.5)
cho nn gia tc l tuyn tnh trong mi mu thi gian.
Ta d dng gii ra c cc h s v bo m hp vi iu kin gii hn.
Thc t ta nhn thy:
79
1
0
1 0
T T2
1 2T
0 ai qi (t k )
0 bi q i (t k )
T 3 ci qi (t k 1 )

3T 2 d i q i (t k 1 )
(6.6)
y, khi gii ra, ta nhn c cc h s ni suy cn tnh trong mi

khong [tk,tk+1]
ai qi (t k )
bi q i (t k )
3[qi (t k 1 ) qi (t k )] T [2q i (t k ) q i (t k 1 )]
(6.7)
T2
2[qi (t k ) qi (t k 1 )] T [q i (t k ) q i (t k 1 )]
di
T3
ci
Ch rng vi k thut ny nhng v tr v vn tc mong mun ti mi

im ly mu c yu cu lu tr di dng bng. Vic s dng ni suy bc
cao nhm bo m s lin tc v v tr, vn tc v gia tc ti mi thi gian tk .
Mc d ta dng k hiu bin khp q(t), iu ny vn lm ni bt s ni suy
qu o c th thc hin c trong khng gian Descartes.
6.1.2. Ni suy qu o theo thi gian nh nht
y l phn quan trng c bit trong qu o LFPD. Gi nh rng gia tc
b gii hn bi gi tr ln nht aM v mong mun Tay my i t im ny n
im khc trong khong thi gian ngn nht. n gin, ta tha nhn rng vn
tc u v vn tc cui c gi tr v 0.
Qu o thi gian nh nht c ch ra trong hnh 6.16. cho bin khp
th i chy t v tr q0 = qi(t0) ti v tr mong mun qf = qi(ti) trong khong thi
gian nh nht tf , gia tc ln nht aM, s c p dng cho n trc thi gian
ngt ts, l thi gian bt u gim tc aM ln nht s c p dng trong khong
thi gian tf. Ch rng c ts v tf u ph thuc vo qo v qf. Ta c th vit:
qi (t s ) q0 1 2 a M (t s t 0 ) 2
q i (t s ) a M (t s t 0 )
qi (t f ) qi (t s ) q i (t s )(t f t s ) 1 2 a M (t f t s ) 2
q i (t f ) q i (t s ) a M (t f t s )
ta c phng trnh vn tc:
q i (t f ) aM (t s t 0 ) aM (t f t s ) 0
hoc
80
t s (t f t 0 ) / 2
(6.8)
iu ny c ngha l s chuyn t gia tc ln nht n gim tc ln nht
xy ra im gia chu k. By gi ta c th thc hin nhng thao tc n gin
trong phng trnh v tr:
qi (t f ) q0 1 2 a M (t s t 0 ) 2 a M (t s t 0 )(t f t s ) 1 2 a M (t f t s ) 2 q f
q f q0
aM
(t s t 0 ) 2 (t s t 0 )(t f t s ) 1 2 (t f t s ) 2
biu thc trn :
t f t 0 (q f q0 ) / a M
(6.9)
Hnh 6.1: Qu o thi gian ngn nht: (a) gia tc; (b) vn tc
Tuy vy, qu o dch chuyn vi thi gian nh nht trn c s s dng gia
tc ln nht khng lin quan trc tip trong robotics l v trong thc t l nhng
tay my lun b gii hn m men bo ha, M. T c im ca phng trnh
chuyn ng ca Tay my xy dng trong phn trc l phi tuyn, do m
m men bo ha thng s khng tng ng vi gii hn hng s trong gia tc.
81
Hnh 6.2 (c): Qu o LFPB v tr

6.2. iu khin h robot phi tuyn .
Nh cp chng trc, h robot l h phi tuyn, v vy trong iu
khin chng ta phi xt n cc phng php iu khin h phi tuyn. Mt s
phng php iu khin phi tuyn c th p dng cho h robot nh : iu khin
tuyn tnh ho vo ra, phng php iu khin trt, phng php iu khin n
nh ho...
Trong gii hn ca mn hc, chng ta tm hiu hai phng thc c bn
iu khin mt robot, sau khi gii quyt cc bi ton ng hc v ng lc
hc robot:
i. iu khin trc tip robot bng cc gii thut iu khin phi tuyn. Cc
phng php iu khin hin i, iu khin thng minh dng cc cng c nh :
tuyn tnh ho, logic m , mng neural
Tuy nhin, mt c th rt ring ca robot l h phi tuyn nhiu u vo v
nhiu u ra. y, n gin chng ta xt iu khin mt motor cho mt
khp ni. Vi h MIMO (Multi Input Multi Output) nh robot, mt phng
thc thng c s dng iu khin trc tip h robot (c cu hnh khng
qu phc tp) l iu khin phn ly. Mi khp ni s c iu khin bi mt
nhnh ca b iu khin c lp nhau. Lu , phng php ny ch tht s hiu
qu khi cu hnh robot khng qu phc tp bi tnh cht phi tuyn ca n.
ii. iu khin theo momen, dng phng php hi tip tuyn tnh h phi
tuyn robot.
Phng php ny thng xut hin trong iu khin th, iu khin thch
nghi, iu khin theo h t hc
6.3. iu khin trc tip h robot.
xy dng gii thut iu khin ph hp vi robot trong cc trng hp
ng dng khc nhau, trc tin chng ta cn xy dng m hnh ton ca i
tng cn iu khin.
Tu thuc vo mc ch iu khin, yu cu v cht lng khc nhau,
chng ta cn la chn cc phng php thit k b iu khin ph hp. i khi,
qu trnh la chn ny l qu trnh th sai tm phng php iu khin ti u.
Trong chng trc, chng ta tm c m hnh ton ca cc i tng
robot t phng trnh ng lc hc ca chng. thun tin cho vic theo di,
y chng ta kho st cc bc vit gii thut iu khin cho mt loi robot
tm hiu trc . Phn mm m phng c s dng y l phn mm
Matlab.
V d : Xy dng b iu khin cho robot 2 bc t do RT bm theo qu o
mong mun.
82
1. Xy dng i tng Robot 2 bc t do thit lp phng trnh ng lc hc

trn.
1
u1
1
s
f(u)
theta_2dot
theta_dot
1
s
theta_
theta
2
u2
1
s
f(u)
d_2dot
d_dot
1
s
2
d
d2
Hnh 6.3. i tng Robot 2 bc t do xy dng trn s Simulink

Chn cc iu kin u theo ng s phn cng ca Robot :
+ iu kin u ca bin khp bng 0.
+ iu kin u ca bin khp d2 bng l1 (Chn =1m)
Chn cc thng s cho Robot 2 bc t do :
+ Khi lung khu 1 : m1 = 0.5 kg.
+ Khi lung khu 2 : m2 = 0.3 kg.
+ Chiu di khu 1 l : l1 = 0.6 m.
+ di tnh tin ti a ca khu 2 so vi gc to l : d 2max = 1m.
+ t trng lng cc khu ti cc u mt ca cc khu hay c th chn
Tensor qun tnh : Izz1=0.015 kgm2; Izz2 = 0.008 kgm2.
2. Thit k b iu khin cho h Robot phi tuyn bm theo qu o mong mun.
Nhn xt :
+ H tay my hai bc t do l h phi tuyn MIMO (da vo phng trnh
ng lc hc) , c hai tn hiu vo l in p (hay momen) t trn mi ng c
iu khin ln lt hai khp quay v tnh tin, hai tn hiu ra l gc quay 1 v
di tnh tin d2.
+ Chuyn ng tnh tin ca khu 2 c th thc hin c nh cc b truyn
c kh bin i chuyn ng quay ca trc ng c thnh chuyn ng tnh tin
ca c cu : b truyn bnh rng-thanh rng, b truyn vtme- ai c bi
83
Hnh6.4. B truyn bnh rng-thanh rng
Hnh 6.5. Kt cu b
truyn vitme-ai c bi
+ C th thit k cc b iu khin SISO iu khin cnh tay my theo

nguyn l tch ri, mi b iu khin s kim sot hot ng ca mt khp lin
kt ca tay my.
+ V y l h c tnh phi tuyn cao nn cc b iu khin thng thng
khng m bo tt kh nng iu khin c h. Ta la chn cc b iu khin
thng minh thc thi kh nng iu khin cho h Robot ny. Mt phng n
la chn y l s dng cc b iu khin m iu khin h bm theo qu o
mong mun.
+ Qua qu trnh la chn v th sai cho cc b iu khin ta nhn thy cc b
iu khin m trc tip, hay PI m, PD m cha cho p ng mong mun.
Chn hai b iu khin m PID iu khin mi khp ng ca Robot.
Trnh t thit k b iu khin nh sau :
Mi b iu khin PID m thit k cho tng khp ca Robot c chn theo
gii php b iu khin PI m ghp song song vi b iu khin PD m. S
m phng thc thi cc b iu khin ny :
84
f(u)
f1
Fcn2
theta_random.mat
Random_theta
theta0_theta
theta_elip.mat
PD_FUZZY_THETA
Elip_theta
f3
U1
PI_FUZZY_THETA
Out1
f(u)
ref1
Fcn
Out1
End_Effector
Trajectory
PI_FUZZY_D
ref2
ROBOT_2DOF
PD_FUZZY_D
d0_d
d_elip.mat
Elip_d
U2
f(u)
d_random.mat
Fcn1
f4
Random_d
f(u)
f2
Fcn3
Hnh 6.6. Xy dng b iu khin cho robot 2 bc t do RT

a. Thit k b iu khin m PI iu khin gc quay khp th nht : Khi
PI_FUZZY_THETA
B iu khin m c tn hiu vo l sai s (E) v vi phn sai s (DE), tn
hiu ra l vi phn in p iu khin (DU).
1
r1
k1
1
s
Gain
Saturation
du/dt
Derivative
Integrator
k3
Gain2
1
u1
Fuzzy_PI
k2
Gain1
Hnh 6.7. S m phng b iu khin PI cho gc quay 1.

Do tay my hot ng trong tm [0 pi] nn E . V vy cc h s
chun ho chn K1=2/pi ; K2=11/pi (K2 chn ph hp vi c tnh ca Robot).
H s K3 c la chn trong qu trnh th sai v tinh chnh cho b iu khin.
Cc tp m biu din cho cc gi tr ngn ng ca bin vo v bin ra c
chn nh sau ( lu vi tn file m1.fis ):
85
Hnh 6.8. Cc tp m chn cho b iu khin PI m iu khin gc quay 1.

Bng kinh nghim v phng php th sai, chng ta c th chn hm lin
thuc ca E, hm lin thuc ca DE, hm lin thuc ca bin ra output_PI.
Cc lut m (h qui tc m) c chn : V chn 5 bin ngn ng cho mi
u vo nn c 52 = 25 lut m c a ra.
b. Thit k b iu khin m PD iu khin gc : Khi PD_FUZZY_THETA
B iu khin m c tn hiu vo l sai s (E) v vi phn sai s (DE), tn
hiu ra l vi phn in p iu khin (DU).
1
r1
k1
k3
Gain
Saturation
du/dt
Derivative
Gain2
1
u1
Fuzzy_PD
k2
Gain1
Hnh 6.9. S m phng b iu khin PD m cho gc quay 1.

V cc qui tc pht biu dng ngn ng ca cc tp m qui nh cho cc
bin vo ra l E, DE trng hp ny hon ton ging vi trung hp thit k
cho b PI m nn ta c th s dng b m thit k cho s iu khin PD
ny .
Cc h s K c chn nh sau : K1 =0.2/pi; K2=2/pi, K3 =20. Cc h s
ny c chn th sai trong qu trnh thit k v tinh chnh b iu khin.
c. Thit k b iu khin m PI iu khin di tnh tin d2 : Khi
PI_FUZZY_D
B iu khin m c tn hiu vo l sai s (E) gia tn hiu t d 2m vi tn
hiu ra thc d2 v vi phn sai s (DE), tn hiu ra l vi phn in p iu khin
(DU).
86
k4
r2
1
s
Gain
Saturation
du/dt
Integrator
1
u2
Gain2
Fuzzy_PI_d
k5
Derivative
k6
Gain1
Hnh 6.10. S m phng b iu khin PI m cho khong tnh tin d2

Do tay my hot ng trong tm [l1 d2max] nn (d 2 max l1 ) E (d 2 max l1 ) . V
vy cc h s chun ho chn K4=6/0.4 ; K5=50/0.4 (K2 chn ph hp vi c
tnh ca Robot). H s K6 c la chn trong qu trnh th sai v tinh chnh
cho b iu khin.
Cc tp m biu din cho cc gi tr ngn ng ca bin vo v bin ra c
chn nh sau ( lu vi tn file m4.fis ):
Hnh 6.11. Cc tp m cho b iu khin PI m iu khin dch chuyn

d2
Hm lin thuc ca E, hm lin thuc ca DE, hm lin thuc ca bin ra
output_PI, cc lut m chn nh trng hp a.
d. Thit k b iu khin m PD iu khin gc : Khi PD_FUZZY_THETA
B iu khin m c tn hiu vo l sai s (E) gia tn hiu t d 2m vi tn
hiu ra thc d2 v vi phn sai s (DE), tn hiu ra l vi phn in p iu khin
(DU).
1
r2
k4
k6
Gain
Saturation
du/dt
Derivative
Gain2
1
u1
Fuzzy_PD_d
k5
Gain1
Hnh 6.12. S m phng b iu khin PD m cho khong tnh tin d 2.

87
V cc qui tc pht biu dng ngn ng ca cc tp m qui nh cho cc

bin vo ra l E, DE trng hp ny hon ton ging vi trung hp thit k
cho b PI m cho d2 nn ta c th s dng b m thit k vi s iu
khin PD ny ( file m4.fis ).
Cc h s K c chn nh sau : K1 =2/0.4; K2=1/0.4, K3 =8. Cc h s
ny c chn th sai trong qu trnh thit k v tinh chnh b iu khin.
3. Qu o t cho Robot.
Nh trnh by mc ni suy qu o cho Robot, y chng ta c th
cho trc mt s qu o t mong mun sao cho qu o ny nm trong vng
lm vic ca Robot (vng vi n) : C th l ng trn, ng elip, qu
o theo mt hm bt k nm trong hnh vanh khn xc nh trc.
Gi s nh ta chn qu o l hnh elip nh sau :
Khng gian lm vic
ca Robot
Qu o l elip mong
mun
0.7m
End Effecttor
l1
l1 + dmax
x
2m
Hnh 6.13. Qu o l elip vi cc di trc ln l 2, di trc be l 1.4

Elip c phng trnh :
x2 y2
1 Chn a=1, b=0.7 nh hnh v

a2 b2
y2
x
1
0.49
2
to tn hiu t l cc hm theo thi gian cho cc bin khp t khng

gian cac, trc tin xut pht t qu o mong mun, chng ta xc nh ln
lt cc im ta, ng vi tng im ta ny chng ta thu thp c s liu
dng bng cc gi tr ca cc bin khp.
y, gi s chng ta chn cc im ta ln lt ng vi hai bin khp
Ai(,d) nh sau :
A1 (0, 1) ; A2(pi/6, 0.8908) ; A3(pi/3, 0.7494) ; A4(pi/2, 0.7); A5(2pi/3,
0.7494); A6(5pi/6, 0.8908); A7(pi, 1).
88
Thi gian ly mu gia cc im ta chn l [tk tk+1]=5s. Vy ta cn t

c qu o mong mun l elip khi khu tc ng cui di chuyn cc gc 1
cch u nhau mt gc 30 , d2 thay i t [0.6 1] trong khong thi gian nh
nhau l 5s.
Chn thi gian ly mu cho c h thng v d liu ni suy l 0.01s.
Dng phng php ni suy ng a thc, chng ta xc nh c ln lt
cc a thc ni gia cc im ta, to qu o mong mun theo cc bin khp.
Mun tng chnh xc ca qu trnh ni suy, chng ta c th tng s
lng cc im ta.
C th vit m file thc hin thao tc ni suy ny, sau lu d liu v
a vo s Simulink. Vit chng trnh gii trc tip hm ni suy hay dng
cc hm ni suy a thc c sn ca Matlab to d liu t cho cc bin khp.
Vi cch thc ny, chng ta hon ton c th xc nh c tn hiu t
cho cc bin khp khi xc nh qu o ca Robot theo mt ng cong bt k.
Kt qu ni suy cho bin khp 1 v d2 theo qu o l elip trn :
+ Ni suy gc 1(t) :
+ Ni suy d2(t) :
4. Kt qu thit k b iu khin bm theo qu o mong mun.

a. Khi cho tn hiu t bt k cho cc bin khp nm trong vng lm vic ca
Robot:
89
+ i vi tn hiu ra l gc :
Hnh 6.14. Kt qu iu khin bm theo qu o t ca gc 1

+ i vi tn hiu ra l di tnh tin d2
Hnh 6.15. Kt qu iu khin bm theo qu o t ca khong tnh tin d 2

Kt qu thu c t qu trnh iu khin, chng ta nhn thy qu o ca
robot bm theo tn hiu t vi cht lng tng i tt, khng xut hin vt l,
tc p ng chp nhn c.
b. Khi cho tn hiu vo l cc d liu ni suy cho gc quay v dch chuyn d2
t qu o elip:
+ i vi tn hiu ra l gc v di tnh tin :
90
Hnh 6.16. Kt qu iu khin bm theo cc qu o ni suy cho tng bin khp

+ Qu o ca khu tc ng cui sau khi iu khin so vi qu o t :
Hnh 6.17. Qu o ca im tc ng cui bm theo qu o hnh elip

c. Vi qu o t l hm bt k c ni suy, ta cng c kt qu bm tt ca
khu tc ng cui :
91
Hnh 6.18. Qu o im tc ng cui robot bm theo qu o t bt k.

5. Xut tn hiu iu khin cho hai vi x l.
Sau khi thit k b iu khin, chng ta cn xut cc tn hiu iu khin U 1
v U2 cho hai motor iu khin hai khp ni ca Robot.
Trc ht chng ta cn xy dng s phn cng cho h thng ny, t c
s ny vit cc chng trnh thu nhn d liu v xut tn hiu trn mi vi x l.
tng thit k mch iu khin robot giao tip vi my tnh, khi da vo m
phng trn matlab :
Rs232
MT-Matlab
VXL A
VXL B
DA
DA
MCH
KCS
NG C A
NG C B
Hnh 6.19. S giao tip t my tnh n cc vi x l iu khin 2 ng c

trn hai khp ni ng c.
6.4. Tnh ton v iu khin theo momen - hi tip tuyn tnh h phi tuyn
robot
Trong mc trn, chng ta tm hiu cc cch thc v nguyn tc xy dng
mt bi ton iu khin trc tip h cnh tay my, s dng cc b iu khin
kinh in cng nh cc b iu khin hin i. Tuy nhin, phng php ny ch
92
kh thi i vi cc loi cnh tay my c kt cu khng qu phc tp. Cn i

vi trng hp tng qut hn, chng ta s dng phng php iu khin theo
momen, mt phng php nhm n gin ho qu trnh iu khin.
Phn ln cc phng php hoch nh iu khin h tay my ngy nay c
th xem nh mt trng hp c bit ca iu khin m men . Tnh ton m
men, nhng khong thi gian nh nhau, l mt ng dng c bit ca hi tip
tuyn tnh ca h thng phi tuyn, l mt hm truyn ph bin trong l thuyt
iu khin h thng hin i.
Trong trng hp tng qut, mc ch ca iu khin theo momen l bin
i mt bi ton thit k iu khin phi tuyn phc tp thnh bi ton thit k
n gin cho h thng tuyn tnh gm c n h thng ph c tch ra, mi h
tun theo nh lut Newton.
Mt cch ti u ha k hoch iu khin m men l chia chng ra thnh
khong iu khin c tnh ton m men (computed torque like) hoc khong
iu khin khng tnh ton m men (noncomputed torque like). Tnh ton iu
khin m men xut hin trong iu khin th, iu khin thch nghi, iu khin
theo h t hc.
6.4.1. o hm ca vng hi tip trong (Deravition of Inner Feedforward
Loop)
Phng trnh ng lc hc Tay my tng qut c dng nh sau:
M (q)q V (q, q ) F vq F d (q ) G(q) d
(6.10)
M (q)q N (q, q ) d
hoc
(6.11)
vi bin khp q(t) thuc khng gian R n, (t) l m men iu khin, d(t) l i
lng nhiu. Nu trong phng trnh ny c k n ng lc hc ca ng c
dn ng th (t) l in p ng vo.
Gi nh rng mt qu o mong mun qd(t) c la chn cho s di
chuyn ca Tay my nh trnh by phn trong 7.3. Vic bo m s hiu chnh
qu o bi cc bin khp, nh ngha mt u ra hay sai s hiu chnh nh sau:
e(t ) qd (t ) q(t )
(6.12)
gii thch nh hng ca u vo qd(t) trn sai s hiu chnh, vi phn hai
ln ta nhn c:
e q d q
e qd q
Tm ra li gii cho q trong (6.11) v thay th vo trong phng trnh trn ta
c:
93
e qd M 1 ( N d )
(6.13)
nh ngha hm vo iu khin:
u qd M 1 ( N )
(6.14)
v hm nhiu
w M 1 d
(6.15)
Ta nh ngha bin x(t) thuc R2n bi:
e
x
e
(6.16)
v ta vit li sai s hiu chnh ng lc hc nh sau:
0
d e 0 I e 0
I w
dt e 0 0 e I

(6.17)
y l h thng sai s tuyn tnh Brunovsky hp vi quy tc chun gm c
n cp i hp nht 1/s2. N c to ra bi u vo iu khin u(t) v hm nhiu
w(t).
Chuyn i hi tip tuyn tnh c th o ngc li nh sau:
M (qd u) N
(6.18)
Ta gi y l nh lut tnh ton iu khin m men. iu quan trng ca
nhng thao tc ny l nhng k tha. Khng c php bin i bin trng thi i
t (6.10) n (6.17). V th, nu ta chn mt iu khin u(t) lm cho (6.17) n
nh vi e(t) tin v 0. Sau u vo iu khin phi tuyn (t) cho bi (6.18)
s gy ra qu o sai lch cho Tay my (6.12). Trn thc t, vic thay th
(6.18) vo (6.11) s cho kt qu:
Mq N d M (qd u ) N
e u M 1 d
(6.19)
cho thy l (6.17) ng n.
Vng lp
phi tuyn
bn trong
N(q,q)94
H thng
tuyn tnh
Hnh 6.20: S k hoch iu khin m men, biu din vng trong v ngoi
S n nh ca (6.17) l khng kh. Trn thc t, php bin i phi tuyn
(6.14) l bin i mt bi ton thit k iu khin phi tuyn phc tp thnh bi
ton thit k n gin cho h thng tuyn tnh gm c n h thng ph c tch
ra, mi h tun theo nh lut Newton.
Kt qu s phi hp iu khin c a ra trong bng tm tc phn
cui. Cn ch rng kt qu ny bao gm mt vng phi tuyn bn trong cng
vi mt tn hiu iu khin bn ngoi u(t). Ta s thy mt vi cch chn la
u(t), trong u(t) s ph thuc vo q(t) v q (t ) , vng bn ngoi s l vng hi
tip. Thng thng, ta c th chn c cu b ng lc hc H(s) cho:
U ( s) H ( s) E ( s)
(6.20)
H(s) c th c chn sao cho hot ng vng kn t hiu qu tt nht.
Theo (2.61) sai s h thng vng kn c chuyn thnh hm s:
T ( s) s 2 I H ( s)
(6.21)
(6.21), ta c nhn xt quan trng l biu thc tnh ton m men ph
thuc vo nghch o ca ng lc hc tay my, v tht vy, i khi ta gi l
ng lc hc iu khin ngc. Thc t, (6.18) m t rng (t) c tnh bi
d u cho q (t) ; kt qu cho php ta tm ra li gii cho bi ton
vic thay th q
ng hc ngc tay my. Nhng d bo cho bit trc vi mt h thng nghch
o, c tnh n nhng p s ca bi ton khi m h thng khng c pha nh
nht bng 0, tt c u c p dng kho st ny.
C mt vi cch tnh (6.18) trnh cho cc ma trn chun nhn vi
nhau ti mi khong thi gian ly mu. Trong mt s trng hp biu thc trn
95
c th tnh ton theo php gii tch. Mt cch tt nht tnh m men (t) l s
dng chuyn i ng lc hc ngc Newton-Euler vi qd u thay vo ch
ca q (t).
Tn hiu vng ngoi u(t) c th c chn dng cho nhng php tnh gn
ng, k c k thut iu khin th v iu khin thch nghi. Trong phn cn li
ca chng ny ta cng s kho st mt vi cch thit k cho u(t) v nhng s
bin thin trong tnh ton v iu khin m men .
6.4.2. Thit k PD vng ngoi
y, chng ta tm hiu vic thit k b iu khin t l vi phn PD vi
tn hiu iu khin u c xc nh :
u K v e K p e
(6.22)
y u vo ca Tay my ni chung s tr thnh biu thc:
M (q)(qd K v e K p e) N (q, q )
(6.23)
Sai s ng lc hc ca vng kn l:
e K v e K p e w
(6.24)
hoc dng khng gian trng thi :
d e 0
dt e K p
I e 0
w
K v e I
(6.25)
Phng trnh c trng ca vng kn l :
c ( s) s 2 I K v s K p
(6.26)
La chn li m bo tnh n nh ca h thng. Cc li thng
dng ly ma trn hm truyn ng cho n x n nh sau:
K v diag k vi ;
K p diag k pi
(6.27)

n
c ( s) ( s 2 k vi s k pi )
i 1
(6.28)
v h thng sai s l n nh tim cn vi iu kin l kvi v kpi u dng. V
96
vy, iu kin l hm nhiu w(t) b gii hn trong kt qu tnh sai s e(t).

Ta ch rng, mc d vic chn ma trn ng cho hm truyn PD a
n vic tch iu khin cp vng ngoi, nhng n khng a n tch ring
iu khin chuyn ng ca khp trong k hoch iu khin. iu ny c c
l bi v php nhn bi M(q) v php cng ca nhng thng s phi tuyn N(q, q )
c cho trc vng trong.
Vic thu nhn khng chnh xc cc tn hiu phn hi v v tr v vn tc
tnh ton cc tn hiu u(t) ca cc khp c th nhm ln. V th, thng tin v tr
q(t) v vn tc q (t) l cn r rng vic tnh ton chnh xc m men iu
khin (t) cho tng khp.
Cn bng biu thc vi dng chun ca phng trnh bc 2 :
p(s) s 2 2 n s n2
(6.29)
vi v n l h s gim chn v tn s ring ca h dao ng. Cho nn, hiu
sut mong mun ca mi sai s e(t) hp thnh c th t c bng cch chn
li nh sau:
k pi1 n2 ;
k vi 2 n
(6.30)
vi v n l h s gim chn mong mun v tn s ring cho sai s ca khp
th i. N c th hu ch khi la chn li phn hi ti trng thi m Tay my
vn ht tm vi ln hn so vi li phn hi trng thi cc khu ca Tay
my co v gn gi c nh, ni m khi lng tp trung ca Tay my gn gc
nht.
iu khng mong mun cho Tay my l mc vt qu gim chn,
iu ny c th l nguyn nhn gy nh hng n chnh xc ca qu o
mong mun khi mun dng chnh xc ti b mt ca i tng cng tc. Cho
nn, li PD thng c chn vi gim chn ti hn =1. Trong trng hp
ny th:
k vi 2 k pi ;
k pi k v2i / 4
(6.31)
Tn s ring n nh hng n tc p ng ca mi sai s hp thnh.
Khi gi tr ca n ln s cho p ng nhanh v s chn la ph thuc vo c
tnh ca i tng. Theo , qu o mong mun s c a vo ch tiu chn
la n . Ta s tho lun v nhng h s a vo trong s chn la ny.
r k r / J
(6.32)
vi J l m men qun tnh khu i v kr l cng ca khu. K , trnh hin
97
tng cng hng, ta s chn n < r / 2. D nhin, m men qun tnh J s thay
i theo hnh dng ca Tay my, cho nn gi tr ln nht c th c dng trong
tnh ton r.
Gii hn trn khc ca n c quy nh bi s xem xt n mc bo ha
m men. Nu li PD qu ln, m men (t) c th t ti gii hn trn.
Hm truyn ca sai s h thng vng kn trong (2.66) l:
e(s) (s 2 I K v s K p ) 1 w(s)
(6.33)
hoc nu Kv v Kp l ng cho, th
ei ( s)
1
w( s) H ( s) w( s)
s k vi s k pi
ei ( s)
s
w( s) sH ( s) w( s)
s 2 k vi s k pi
(6.34)
(6.35)
vi w(s) l nhiu.
6.4.3. V d :
a. Lut iu khin tnh ton m men
Trong v d di y ca c cu hai khu phng lin kt bng khp bn l
(hnh 6.7), ta nhn c kt qu vn tc ca cc khu t bi ton ng hc nh
sau:
x1 a1 sin 1
y1 a1 .cos1
Ta c kt qu sau khi kho st bi ton ng lc hc tay my hai khu vi

hai khp bn l nh sau:
(m1 m2 )a12 m2 a22 2m2 a1a2 cos 2 m2a22 m2 a1a2 cos 2 1 m2 a1a2 (212 22 ) sin 2

m2 a22 m2 a1a2 cos 2
m2a22
m2 a1a212 sin 2
(m m ) ga cos 1 m2 ga2 cos(1 2 ) 1

1 2 1
(1)

m2 ga2 cos(1 2 )
2
98
N c dng chun:
M (q)q V (q, q ) G(q)
(2)
ly khi lng ca mi khu l 1kg v chiu di l 1m.

Ta c lut iu khin PD l:
M (q)(qd Kv e K p e) V (q, q ) G(q)
(3)
vi sai s c nh ngha nh sau:
e qd q
(4)
b. Qu o yu cu:
Ta gi s qu o mong mun ca qd(t) gm cc thnh phn:
1d =g1sin(2t/T)
(5)
2d =g2cos(2t/T)
vi mu T=2s v gi = 0.1 rad. Chn hng s thi gian cho h thng l 0.1s. Lc
ta c:
n = 1/0.1 = 10
kp = n2 =100
kv = 2n = 20
c. Kt qu m phng
Sau khi dng phn mm Matlab m phng ta c kt qu nh sau:
0.15
0.1
0.05
-0.05
-0.1
-0.15
-0.2
5
Time(s )
10
Hnh 6.4: th gc quay ca cc khp theo t

Ta nhn thy thi im u sai s rt ln, l do b iu khin cha p
ng kp. Sau mt khong thi gian p ng, sai s hiu chnh mi gn bng 0,
lc Tay my mi chuyn ng gn vi qu o mong mun. Sai s ln hay
nh ty thuc vo gi tr ta t ban u v c im ca b iu khin cng nh
lut iu khin m ta la chn.
th biu din s bin thin ca cc khu , ta nhn thy, i vi khu 2
gi tr ca m men c nhng thi im xung di 0, l do nh hng ca
trng lng bn thn ca tay my trong qu trnh chuyn ng.
99

40
35
30
25
20
15
10
5
Time(s )
10
Hnh 6.6 : Biu m men ca hai khu

6.4.4. Thit k PID vng ngoi
Tng t nh cch thit k b iu khin PD, chng ta tm b iu khin
vng ngoi cho cht lng tt hn. Thng thng, bin php s dng l thm c
mch tnh tch phn trong vng cho trc iu ny c th t c khi dng
tnh ton iu khin m men vi iu chnh PID :
(6.43)
u K v e K p e K i
(6.44)
y u vo iu khin Tay my, ta c:
M (q)(qd K v e K p e K i ) N (q, q )
(6.45)
vi (t) l phn nguyn ca sai s hiu chnh e(t). Chnh l phn cng thm vo
hiu chnh ng lc hc.
nh lut iu khin ny l c m t thun li bi nh ngha trng thi
T
l x = [T eT e ] T thuc R3n v lm tng thm sai s ng hc (6.17) vi mt
mch tch phn:
0 I 0 0
0
d
e 0 u 0 w
e
0
0
I

dt
e 0 0 0 e I
I
(6.46)
Mt s khi ca b tnh ton iu khin m men c a ra trong
bng tm tc.
y h thng vng kn c dng:
0
d
e 0
dt
e K i
I
0
Kp
(6.47)
100
0 0
I e 0 w
K v e I
a thc c trng ca vng kn l:
c(s) s 3 I K v s 2 K p s K i
(6.48)
Tng t nh trng hp tnh ton b PD, ta cng la chn li m
bo tnh n nh h thng trong iu khin.
Chn t s li
K v diag k vi ; K p diag k pi ; K i diag kii

(6.49)
q
qd
N(q, q )
q
qd
1/s
Ki
M(q)
Robot
arm
K
v
Hnh 2.14: B iu khin monent PID

Cho:(2.92)
Bng cch dng kim tra Routh, n c th c tm thy cho tnh n nh
vng kn m chng ta yu cu:
k i k vi k pi
(6.50)
S bo ha ca c cu tc ng v mch gii hn khuch i tch phn
(Actuator Saturation and Integrator Windup)
Cn lu v hiu qu v nhng vn ko theo trong khi s dng cng c
iu khin PID cho cc Tay my. Vi Tay my tht s c nhng gii hn v in
p v m men bo v mch iu khin cng sut ca ng c kch hot.
101
Nhng gii hn ny c th hoc khng th gy ra nhng vn vi iu khin

PD, nhng n gn nh lun gy ra vn vi b iu khin PID v mt hin
tng c bit n nh l gii hn bo ha trong khuch i tch phn
(Integrator Windup).
Ta tha nhn rng trong trng hp = ki (t) vi (t) l u ra. M men
vo (t) b gii hn bi gi tr ln nht max v nh nht min ca n. Nu ki (t)
t ti max , c th c hoc khng c vn g. Vn pht sinh l khi nu u
vo mch tch phn vn dng, mch tch phn tip tc cng dn (integrate)
hng ln thnh m v ki (t) c th gim xung, n c th tng vt ra ngoi
gii hn max. Khi u vo mch tch phn tr thnh m, n c th mt thi gian
ng k cho ki (t) gim xung di max. Trong lc y c gi ti max, h
qu l u vo iu khin khng ng cho thit b chp hnh.
nh gi s khc bit ca iu khin vng ngoi theo PD v PID ta xem
th biu din sai s hiu chnh cng nh s thay i m men ca ca tng
khu
Ta nhn thy vi d l hng s nhiu (gi s gi tr nhiu l 1N-m) cho mi
khu. Ta thy iu khin theo lut PID th thc t hn lut iu khin PD v n
c cng vo tc ng b nhiu v h qu l h thng n nh hn.
40
35
30
25
20
15
10
5
Time(s)
Hnh 6.9 : th m men ca hai khu

6.4.5. Bng tm tt :
a. Phng trnh ng lc hc tay my :
M (q)q V (q, q ) Fv q Fd (q ) G(q) d
hoc:
M (q)q N (q, q ) d

N (q, q ) V (q, q ) Fv q Fd (q ) G(q)
b. Sai s hiu chnh
e(t ) qd (t ) q(t )
c. Tnh ton m men theo PD
102
10
M (q)(qd Kve K pe) N (q, q )

d. Tnh ton m men theo PID
e
M (q)(qd Kv e K p e Ki ) N (q, q )
g. iu khin PD truyn thng
c Kve K pe
h. iu khin PID truyn thng
e
c K v e K p e Ki
6.2.6. p dng Matlab kho st cc bi ton c th.
Phn tnh ton trn MATLAB c xy dng da trn c s kho st ln
lt bi ton ng hc v ng lc hc ca tay my cho trc, sau la chn
cc quy lut iu khin tuyn tnh a ra kt qu cui cng v s thay i m
men ti cc khp. t a ra k hoch thit k v iu khin tay my mt
cch hp l.
S thit k:
Nhp d liu
Gii bi ton
ng hc
Gii bi ton
ng lc hc
Chn lut iu khin
M phng
103
Robot Cng nghip
Chng I
Gii thiu chung v robot cng nghip
1.1. S lt qu trnh pht trin ca robot cng nghip (IR : Industrial Robot) :
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.
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 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.
C th ni, Robot l s t hp kh nng hot ng linh hot ca cc c cu iu khin t
xa vi mc tri thc ngy cng phong ph ca h thng iu khin theo chng trnh s
cng nh k thut ch to cc b cm bin, cng ngh lp trnh v cc pht trin ca tr khn
nhn to, h chuyn gia ...
Trong nhng nm sau ny, vic nng cao tnh nng hot ng ca robot khng ngng
pht trin. Cc robot c trang b thm cc loi cm bin khc nhau nhn bit mi trng
TS. Phm ng Phc
Robot Cng nghip
chung quanh, cng vi nhng thnh tu to ln trong lnh vc Tin hc - in t to ra cc

th h robot vi nhiu tnh nng c bit, S lng robot ngy cng gia tng, gi thnh ngy
cng gim. Nh vy, robot cng nghip c v tr quan trng trong cc dy chuyn sn xut
hin i.
Mt vi s liu v s lng robot c sn xut mt vi nc cng nghip pht trin
nh sau :
(Bng I.1)
Nc SX
Nm 1990
Nm 1994
Nm 1998
(D tnh)
Nht
60.118
29.756
67.000
M
4.327
7.634
11.100
c
5.845
5.125
8.600
2.500
2.408
4.000
1.488
1.197
2.000
Php
Anh
510
1.086
1.500
Hn quc
1.000
1.200
M l nc u tin pht minh ra robot, nhng nc pht trin cao nht trong lnh vc
nghin cu ch to v s dng robot li l Nht.
1.2. ng dng robot cng nghip trong sn xut :
T khi mi ra i robot cng nghip c p dng trong nhiu lnh vc di gc
thay th sc ngi. Nh vy cc dy chuyn sn xut c t chc li, nng sut v hiu qu
sn xut tng ln r rt.
Mc tiu ng dng robot cng nghip nhm gp phn nng cao nng sut dy chuyn
cng ngh, gim gi thnh, nng cao cht lng v kh nng cnh tranh ca sn phm ng
thi ci thin iu kin lao ng. t c cc mc tiu trn l nh vo nhng kh nng to ln
ca robot nh : lm vic khng bit mt mi, rt d dng chuyn ngh mt cch thnh tho,
chu c phng x v cc mi trng lm vic c hi, nhit cao, cm thy c c t
trng v nghe c c siu m ... Robot c dng thay th con ngi trong cc trng hp
trn hoc thc hin cc cng vic tuy khng nng nhc nhng n iu, d gy mt mi, nhm
ln.
Trong ngnh c kh, robot c s dng nhiu trong cng ngh c, cng ngh hn, ct
kim loi, sn, phun ph kim loi, tho lp vn chuyn phi, lp rp sn phm . . .
Ngy nay xut hin nhiu dy chuyn sn xut t ng gm cc my CNC vi
Robot cng nghip, cc dy chuyn t mc t ng ho cao, mc linh hot cao . . .
y cc my v robot c iu khin bng cng mt h thng chng trnh.
Ngoi cc phn xng, nh my, k thut robot cng c s dng trong vic khai thc
thm lc a v i dng, trong y hc, s dng trong quc phng, trong chinh phc v tr,
trong cng nghip nguyn t, trong cc lnh vc x hi . . .
R rng l kh nng lm vic ca robot trong mt s iu kin vt hn kh nng ca
con ngi; do n l phng tin hu hiu t ng ho, nng cao nng sut lao ng,
gim nh cho con ngi nhng cng vic nng nhc v c hi. Nhc im ln nht ca
robot l cha linh hot nh con ngi, trong dy chuyn t ng, nu c mt robot b hng c
th lm ngng hot ng ca c dy chuyn, cho nn robot vn lun hot ng di s gim
st ca con ngi.
TS. Phm ng Phc
Robot Cng nghip
1.3. Cc khi nim v nh ngha v robot cng nghip :

1.3.1. nh ngha robot cng nghip :
Hin nay c nhiu nh ngha v Robot, c th im qua mt s nh ngha nh sau :
nh ngha theo tiu chun AFNOR (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 to ; c kh nng nh v, nh
hng, di chuyn cc i tng vt cht : chi tit, dao c, g lp . . . theo nhng hnh trnh
thay i chng trnh ho nhm thc hin cc nhim v cng ngh khc nhau.
nh ngha theo RIA (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.
nh ngha theo OCT 25686-85 (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 trnh li hon
thnh cc chc nng vn ng v iu khin trong qu trnh sn xut.
C th ni Robot cng nghip l mt my t ng linh hot thay th tng phn hoc
ton b cc hot ng c bp v hot ng tr tu ca con ngi trong nhiu kh nng thch
nghi khc nhau.
Robot cng nghip c kh nng chng trnh ho linh hot trn nhiu trc chuyn
ng, biu th cho s bc t do ca chng. Robot cng nghip c trang b nhng bn tay
my hoc cc c cu chp hnh, gii quyt nhng nhim v xc nh trong cc qu trnh cng
ngh : hoc trc tip tham gia thc hin cc nguyn cng (sn, hn, phun ph, rt kim loi
vo khun c, lp rp my . . .) hoc phc v cc qu trnh cng ngh (tho lp chi tit gia
cng, dao c, g . . .) vi nhng thao tc cm nm, vn chuyn v trao i cc i tng
vi cc trm cng ngh, trong mt h thng my t ng linh hot, c gi l H thng t
ng linh hot robot ho cho php thch ng nhanh v thao tc n gin khi nhim v sn
xut thay i.
1.3.2. Bc t do ca robot (DOF : Degrees Of Freedom) :
Bc t do l s kh nng chuyn ng ca mt c cu (chuyn ng quay hoc tnh
tin). dch chuyn c mt vt th trong khng gian, c cu chp hnh ca robot phi t
c mt s bc t do. Ni chung c h ca robot l mt c cu h, do bc t do ca n c
th tnh theo cng thc :
5
w = 6n -
ip
i =1
(1.1)
y :
n - S khu ng;
pi - S khp loi i (i = 1,2,. . .,5 : S bc t do b hn ch).
i vi cc c cu c cc khu c ni vi nhau bng khp quay hoc tnh tin (khp
ng loi 5) th s bc t do bng vi s khu ng . i vi c cu h, s bc t do bng tng
s bc t do ca cc khp ng.
nh v v nh hng khu chp hnh cui mt cch tu trong khng gian 3
chiu robot cn c 6 bc t do, trong 3 bc t do nh v v 3 bc t do nh hng.
Mt s cng vic n gin nng h, sp xp... c th yu cu s bc t do t hn. Cc robot
hn, sn... thng yu cu 6 bc t do. Trong mt s trng hp cn s kho lo, linh hot
hoc khi cn phi ti u ho qu o,... ngi ta dng robot vi s bc t do ln hn 6.
1.3.3. H to (Coordinate frames) :
Mi robot thng bao gm nhiu khu (links) lin kt vi nhau qua cc khp (joints),
to thnh mt xch ng hc xut pht t mt khu c bn (base) ng yn. H to gn vi
TS. Phm ng Phc
Robot Cng nghip
khu c bn gi l h to c bn (hay h to chun). Cc h to trung gian khc gn

vi cc khu ng gi l h to suy rng. Trong tng thi im hot ng, cc to suy
rng xc nh cu hnh ca robot bng cc chuyn dch di hoc cc chuyn dch gc cu cc
khp tnh tin hoc khp quay (hnh 1.1). Cc to suy rng cn c gi l bin khp.
z
4
3
d2
n
a
o On
1
y
x
O0
Hnh 1.1 : Cc to suy rng ca robot.

Cc h to gn trn cc khu ca robot phi
tun theo qui tc bn tay phi : Dng tay phi, nm hai
ngn tay t v p t vo lng bn tay, xo 3 ngn : ci,
tr v gia theo 3 phng vung gc nhau, nu chn
ngn ci l phng v chiu ca trc z, th ngn tr ch
phng, chiu ca trc x v ngn gia s biu th
phng, chiu ca trc y (hnh 1.2).
Trong robot ta thng dng ch O v ch s n
ch h to gn trn khu th n. Nh vy h to
c bn (H to gn vi khu c nh) s c k
hiu l O0; h to gn trn cc khu trung gian
tng ng s l O1, O2,..., On-1, H to gn trn khu
chp hnh cui k hiu l On.
O
y
Hnh 1.2 : Qui tc bn tay phi
1.3.4. Trng cng tc ca robot (Workspace or Range of motion):

Trng cng tc (hay vng lm vic, khng gian cng tc) ca robot l ton b th tch
c qut bi khu chp hnh cui khi robot thc hin tt c cc chuyn ng c th. Trng
cng tc b rng buc bi cc thng s hnh hc ca robot cng nh cc rng buc c hc ca
cc khp; v d, mt khp quay c chuyn ng nh hn mt gc 3600. Ngi ta thng dng
hai hnh chiu m t trng cng tc ca mt robot (hnh 1.3).
H
R
Hnh chiu bng

Hnh chiu ng
Hnh 1.3 : Biu din trng cng tc ca robot.
TS. Phm ng Phc
Robot Cng nghip
1.4. Cu trc c bn ca robot cng nghip :

1.4.1. Cc thnh phn chnh ca robot cng nghip :
Mt robot cng nghip thng bao gm cc thnh phn chnh nh : cnh tay robot,
ngun ng lc, dng c gn ln khu chp hnh cui, cc cm bin, b iu khin , thit b
dy hc, my tnh ... cc phn mm lp trnh cng nn c coi l mt thnh phn ca h
thng robot. Mi quan h gia cc thnh phn trong robot nh hnh 1.4.
Cc cm
bin
B iu
khin v
my tnh
Thit b
dy hc
Ngun
ng lc
Cc chng
trnh
Cnh tay
robot
Dng c
thao tc
Hnh 1.4 : Cc thnh phn chnh ca h thng robot.

Cnh tay robot (tay my) l kt cu c kh gm cc khu lin kt vi nhau bng cc
khp ng c th to nn nhng chuyn ng c bn ca robot.
Ngun ng lc l cc ng c in (mt chiu hoc ng c bc), cc h thng xy
lanh kh nn, thu lc to ng lc cho tay my hot ng.
Dng c thao tc c gn trn khu cui ca robot, dng c ca robot c th c nhiu
kiu khc nhau nh : dng bn tay nm bt i tng hoc cc cng c lm vic nh m
hn, mi, u phun sn ...
Thit b dy-hoc (Teach-Pendant) dng dy cho robot cc thao tc cn thit theo
yu cu ca qu trnh lm vic, sau robot t lp li cc ng tc c dy lm vic
(phng php lp trnh kiu dy hc).
Cc phn mm lp trnh v cc chng trnh iu khin robot c ci t trn my
tnh, dng iu khin robot thng qua b iu khin (Controller). B iu khin cn c gi
l Moun iu khin (hay Unit, Driver), n thng c kt ni vi my tnh. Mt moun
iu khin c th cn c cc cng Vo - Ra (I/O port) lm vic vi nhiu thit b khc nhau
nh cc cm bin gip robot nhn bit trng thi ca bn thn, xc nh v tr ca i tng
lm vic hoc cc d tm khc; iu khin cc bng ti hoc c cu cp phi hot ng phi
hp vi robot ...
1.4.2. Kt cu ca tay my :
Nh ni trn, tay my l thnh phn quan trng, n quyt nh kh nng lm vic
ca robot. Cc kt cu ca nhiu tay my c phng theo cu to v chc nng ca tay
ngi; tuy nhin ngy nay, tay my c thit k rt a dng, nhiu cnh tay robot c hnh
dng rt khc xa cnh tay ngi. Trong thit k v s dng tay my, chng ta cn quan tm
n cc thng s hnh - ng hc, l nhng thng s lin quan n kh nng lm vic ca
robot nh : tm vi (hay trng cng tc), s bc t do (th hin s kho lo linh hot ca
robot), cng vng, ti trng vt nng, lc kp . . .
TS. Phm ng Phc
Robot Cng nghip
Cc khu ca robot thng thc hin hai chuyn ng c bn :

Chuyn ng tnh tin theo hng x,y,z trong khng gian Descarde, thng thng
to nn cc hnh khi, cc chuyn ng ny thng k hiu l T (Translation) hoc
P (Prismatic).
Chuyn ng quay quanh cc trc x,y,z k hiu l R (Roatation).
Tu thuc vo s khu v s t hp cc chuyn ng (R v T) m tay my c cc kt
cu khc nhau vi vng lm vic khc nhau. Cc kt cu thng gp ca l Robot l robot
kiu to cc, to tr, to cu, robot kiu SCARA, h to gc (phng sinh) ...
Robot kiu to cc : l tay
my c 3 chuyn ng c bn tnh tin
theo phng ca cc trc h to gc
(cu hnh T.T.T). Trng cng tc c dng
khi ch nht. Do kt cu n gin, loi
tay my ny c cng vng cao,
chnh xc c kh d m bo v vy n
thung dng vn chuyn phi liu, lp
rp, hn trong mt phng ...
T.T.T
Hnh 1.5 : Robot kiu to cc

R.T.T
Robot kiu to tr : Vng lm
vic ca robot c dng hnh tr rng.
Thng khp th nht chuyn ng quay.
V d robot 3 bc t do, cu hnh R.T.T
nh hnh v 1.6. C nhiu robot kiu to
tr nh : robot Versatran ca hng
AMF (Hoa K).
Hnh 1.6 : Robot kiu to tr
Robot kiu to cu : Vng lm vic ca robot c dng hnh cu. thng cng
vng ca loi robot ny thp hn so vi hai loi trn. V d robot 3 bc t do, cu hnh R.R.R
hoc R.R.T lm vic theo kiu to cu (hnh 1.7).
R.R.R
R.R.T
Hnh 1.7 : Robot kiu to cu

Robot kiu to gc (H to phng sinh) : y l kiu robot c dng nhiu
hn c. Ba chuyn ng u tin l cc chuyn ng quay, trc quay th nht vung gc vi
hai trc kia. Cc chuyn ng nh hng khc cng l cc chuyn ng quay. Vng lm vic
ca tay my ny gn ging mt phn khi cu. Tt c cc khu u nm trong mt phng
thng ng nn cc tnh ton c bn l bi ton phng. u im ni bt ca cc loi robot hot
TS. Phm ng Phc
Robot Cng nghip
ng theo h to gc l gn nh, tc l c vng lm vic tng i ln so vi kch c ca

bn thn robot, linh hot cao.
Cc robot hot ng theo h to gc nh : Robot PUMA ca hng Unimation Nokia (Hoa K - Phn Lan), IRb-6, IRb-60 (Thu in), Toshiba, Mitsubishi, Mazak (Nht
Bn) .V.V...
V d mt robot hot ng theo h to gc (H to phng sinh), c cu hnh
RRR.RRR :
Hnh 1.8 : Robot hot ng theo h to gc.

Robot kiu SCARA : Robot SCARA ra
i vo nm 1979 ti trng i hc
Yamanashi (Nht Bn) l mt kiu robot mi
nhm p ng s a dng ca cc qu trnh sn
xut. Tn gi SCARA l vit tt ca "Selective
Compliant Articulated Robot Arm" : Tay my
mm do tu . Loi robot ny thng dng
trong cng vic lp rp nn SCARA i khi
c gii thch l t vit tt ca "Selective
Compliance Assembly Robot Arm". Ba khp
u tin ca kiu Robot ny c cu hnh R.R.T,
cc trc khp u theo phng thng ng. S
ca robot SCARA nh hnh 1.9.
Hnh 1.9 : Robot kiu SCARA
1.5. Phn loi Robot cng nghip :

Robot cng nghip rt phong ph a dng, c th c phn loi theo cc cch sau :
1.4.1. Phn loi theo kt cu :
Theo kt cu ca tay my ngi ta phn thnh robot kiu to cc, Kiu to
tr, kiu to cu, kiu to gc, robot kiu SCARA nh trnh by trn.
1.4.2. Phn loi theo h thng truyn ng :
C cc dng truyn ng ph bin l :
H truyn ng in : Thng dng cc ng c in 1 chiu (DC : Direct Current)
hoc cc ng c bc (step motor). Loi truyn ng ny d iu khin, kt cu gn.
H truyn ng thu lc : c th t c cng sut cao, p ng nhng iu kin lm
vic nng. Tuy nhin h thng thu lc thng c kt cu cng knh, tn ti phi tuyn ln
kh x l khi iu khin.
H truyn ng kh nn : c kt cu gn nh hn do khng cn dn ngc nhng li
phi gn lin vi trung tm ta ra kh nn. H ny lm vic vi cng sut trung bnh v nh,
km chnh xc, thng ch thch hp vi cc robot hot ng theo chng trnh nh sn vi
cc thao tc n gin nhc ln - t xung (Pick and Place or PTP : Point To Point).
TS. Phm ng Phc
Robot Cng nghip
1.4.3. Phn loi theo ng dng :

Da vo ng dng ca robot trong sn xut c Robot sn, robot hn, robot lp rp,
robot chuyn phi .v.v...
1.4.4. Phn loi theo cch thc v c trng ca phng php iu khin :
C robot iu khin h (mch iu khin khng c cc quan h phn hi), Robot iu
khin kn (hay iu khin servo) : s dng cm bin, mch phn hi tng chnh xc v
mc linh hot khi iu khin.
Ngoi ra cn c th c cc cch phn loi khc tu theo quan im v mc ch nghin
cu
-----------------------------------------------------------------------------------------------------------------
TS. Phm ng Phc
Robot cng nghip
Chng II
Cc php bin i thun nht
(Homogeneous Transformation)
Khi xem xt, nghin cu mi quan h gia robot v vt th ta khng nhng cn quan
tm n v tr (Position) tuyt i ca im, ng, mt ca vt th so vi im tc ng cui
(End effector) ca robot m cn cn quan tm n vn nh hng (Orientation) ca khu
chp hnh cui khi vn ng hoc nh v ta mt v tr.
m t quan h v v tr v hng gia robot v vt th ta phi dng n cc php
bin i thun nht.
Chng ny cung cp nhng hiu bit cn thit trc khi i vo gii quyt cc vn
lin quan ti ng hc v ng lc hc robot.
2.1. H ta thun nht :
biu din mt im trong khng gian ba chiu, ngi ta dng Vect im (Point
vector). Vect im thng c k hiu bng cc ch vit thng nh u, v, x1 . . . m t v
tr ca im U, V, X1 ,. . .
Ty thuc vo h qui chiu c chn, trong khng gian 3 chiu, mt im V c th
c biu din bng nhiu vect im khc nhau :
V
vE
vF
E
F
Hnh 2.2 : Biu din 1 im trong khng gian

vE v vF l hai vect khc nhau mc d c hai vect cng m t im V. Nu i, j, k l
cc vec t n v ca mt h to no , chng hn trong E, ta c :
r
r
r
r
v = ai + bj + ck
vi a, b, c l to v tr ca im V trong h .
Nu quan tm ng thi vn nh v v nh hng, ta phi biu din vect v trong
khng gian bn chiu vi sut vect l mt ma trn ct :
v
x
y
z
w
Trong
x/w = a
y/w = b
z/w = c
vi w l mt hng s thc no .
w cn c gi l h s t l, biu th cho chiu th t ngm nh, Nu w = 1 d thy :
x x
y y
z z
= = x=a;
= = y =b;
= =z=a
w 1
w 1
w 1
TS. Phm ng Phc
10
Robot cng nghip
Trong trng hp ny th cc to biu din bng vi to vt l ca im trong

khng gian 3 chiu, h to s dng w=1 c gi l h to thun nht.
Vi w = 0
x y z
= = =
w w w
ta c :
Gii hn th hin hng ca cc trc to .

Nu w l mt hng s no 0 v 1 th vic biu din im trong khng gian tng
ng vi h s t l w :
r
r
r
r
V d :
v = 3i + 4 j + 5k
vi w = 1 (trng hp thun nht) :
v = [3 4 5 1]T
vi w=-10 biu din tng ng s l :
v = [-30 -40 -50 -10]T
T
K hiu [ . . . . ] (Ch T vit cao ln trn ch php chuyn i vect hng thnh vect
ct).
Theo cch biu din trn y, ta qui c :
[0 0 0 0]T l vect khng xc nh
[0 0 0 n]T vi n 0 l vect khng, trng vi gc to
[x y z 0]T l vect ch hng
[x y z 1]T l vect im trong h to thun nht.
2.2. Nhc li cc php tnh v vect v ma trn :
2.2.1. Php nhn vct :
r
r
r
r
a = a xi + a y j + az k
r
r
r
r
b = bx i + by j + bz k
Cho hai vect :
Ta c tch v hng
V tch vect :
r r
i
j
r
r
axb = a a
x
y
bx by
a.b = axbx + ayby + azbz

r
k
az
bz
= (aybz-azby) i + (azbx-axbz) j + (axby-aybx) k

a/ Php cng, tr ma trn :
Cng (tr ) cc ma trn A v B cng bc s c ma trn C cng bc, vi cc phn t cij
bng tng (hiu) ca cc phn t aij v bij (vi mi i, j).
A+B=C
Vi cij = aij + bij.
A-B =C
Vi cij = aij - bij.
Php cng, tr ma trn c cc tnh cht ging php cng s thc.
b/ Tch ca hai ma trn : Tch ca ma trn A (kch thc m x n) vi ma trn B (kch
thc n x p) l ma trn C c kch thc m x p.
V d : cho hai ma trn :
A =
1
4
7
Ta c :
TS. Phm ng Phc
2
5
8
3
6
9
B =
1
3
5
2
4
6
11
Robot cng nghip
1.1+2.3+3.5
4.1+5.3+6.5
7.1+8.3+9.5
C = A.B =
1.2+2.4+3.6
4.2+5.4+6.6
7.2+8.4+9.6
22
49
76
28
64
100
Php nhn hai ma trn khng c tnh giao hon, ngha l : A . B B . A

Ma trn n v I (Indentity Matrix) giao hon c vi bt k ma trn no : I.A = A.I
Php nhn ma trn tun theo cc qui tc sau :
1. (k.A).B = k.(A.B) = A.(k.B)
2. A.(B.C) = (A.B).C
3. (A + B).C = A.C + B.C
4. C.(A + B) = C.A + C.B
c/ Ma trn nghch o ca ma trn thun nht :
Mt ma trn thun nht l ma trn 4 x 4 c dng :
nx
ny
nz
0
T =
Ox
Oy
Oz
0
ax
ay
az
0
px
py
pz
1
nz
Oz
az
0
-p.n
-p.O
-p.a
1
Ma trn nghch o ca T k hiu l T-1 :

nx
Ox
ax
0
T-1 =
ny
Oy
ay
0
(2-1)
Trong p.n l tch v hng ca vect p v n. ngha l :

p.n = pxnx + pyny + pznz
tng t :
p.O = pxOx + pyOy + pzOz
v
p.a = pxax + pyay + pzaz
V d : tm ma trn nghch o ca ma trn bin i thun nht :
0
0
-1
0
H =
0
1
0
0
1
0
0
0
1
2
3
1
Gii : p dng cng thc (2-1), ta c :

-1
H =
0
0
1
0
0
1
0
0
-1 3
0 -2
0 -1
0 1
Chng ta kim chng rng y chnh l ma trn nghch o bng cc nhn ma trn H vi H-1 :
0
0
-1
0
0
1
0
0
1
0
0
0
TS. Phm ng Phc
1
2
3
1
0
0
1
0
0
1
0
0
-1 3
0 -2
0 -1
0 1
1
0
0
0
0
1
0
0
0
0
1
0
0
0
0
1
12
Robot cng nghip
Phng php tnh ma trn nghch o ny nhanh hn nhiu so vi phng php chung;
tuy nhin n khng p dng c cho ma trn 4x4 bt k m kt qu ch ng vi ma trn
thun nht.
d/ Vt ca ma trn :
Vt ca ma trn vung bc n l tng cc phn t trn ng cho :
n
Trace(A) hay Tr(A) =
a
i =1
ii
Mt s tnh cht quan trng ca vt ma trn :

1/ Tr(A) = Tr(AT)
2/ Tr(A+B) = Tr(A) + Tr(B)
3/ Tr(A.B) = Tr(B.A)
4/ Tr(ABCT) = Tr(CBTAT)
e/ o hm v tch phn ma trn :
Nu cc phn t ca ma trn A l hm nhiu bin, th cc phn t ca ma trn o hm
bng o hm ring ca cc phn t ma trn A theo bin tng ng.
V d : cho
a11
a
A = 21
a31
a 41
a12
a13
a 22
a 23
a32
a 42
a33
a 43
a14
a 24
a 34
a 44
a11 a12 a13 a14

t
t
t
t
a
a22 a23 a 24
21
t
t
t dt
dA = t
th :
a31 a32 a33 a34
t
t
t
t
a41 a42 a43 a 44
t
t
t
t
Tng t, php tch phn ca ma trn A l mt ma trn, c :
A(t )dt = { aij (t )dt}

2.3. Cc php bin i
Cho u l vect im biu din im cn bin i, h l vect dn c biu din bng
mt ma trn H gi l ma trn chuyn i . Ta c :
v = H.u
v l vect biu din im sau khi bin i.
2.3.1. Php bin i tnh tin (Translation) :
r
r
r r
Gi s cn tnh tin mt im hoc mt vt th theo vect dn h = ai + bj + ck . Trc
ht ta c nh ngha ca ma trn chuyn i H :
H = Trans(a,b,c) =
TS. Phm ng Phc
1
0
0
0
0
1
0
0
0
0
1
0
a
b
c
1
(2.2)
13
Robot cng nghip
Gi u l vect biu din im cn tnh tin :

u = [x y z w]T
Th v l vect biu din im bin i tnh tin c xc nh bi :
v = H.u =
1
0
0
0
0
1
0
0
0
0
1
0
a
b
c
1
x
y
z
w
x+aw
y+bw
z+cw
w
x/w+a
y/w+b
z/w+c
1
Nh vy bn cht ca php bin i tnh tin l php cng vect gia vect biu din
im cn chuyn i v vect dn.
r
r
r
r
u = 2i + 3j + 2k
V d :
r
r
r
r
h = 4i - 3j + 7k
Th
1 0 0 4
2
2+4
6
v = Hu =
0 1 0 -3 . 3 =
3-3
=
0
0 0 1 7
2
2+7
9
0 0 0 1
1
1
1
v vit l :
v = Trans(a,b,c) u
z
v
2
-3
4
u
3
6
x
Hnh 2..4: Php bin i tnh tin trong khng gian

2.3.2. Php quay (Rotation) quanh cc trc to :
Gi s ta cn quay mt im hoc mt vt th xung quanh trc to no vi gc
quay o, ta ln lt c cc ma trn chuyn i nh sau :
Rot(x, ) =
o
Rot(y, ) =
o
TS. Phm ng Phc
1
0
0
0
0
cos
sin
0
0
-sin
cos
0
0
0
0
1
cos
0
-sin
0
0
1
0
0
sin
0
cos
0
0
0
0
1
(2.3)
(2.4)
14
Robot cng nghip
cos
Rot(z, ) =
sin
0
0
V d : Cho im U biu din bi
(hnh 2.5). Ta c
0 -1
o
v= Rot(z, 90 )u =
1
0
0
0
0
0
o
0
0
-sin
0
0
(2.5)
cos
0
1
0
0 r r0 r 1
r
u = 7i + 3j + 2k quay xung quanh z mt gc = 90o
0
0
1
0
0
0
0
1
7
3
2
1
-3
7
2
1
Nu cho im bin i tip tc quay xung quanh y mt gc 90o ta c :

0
0
-1
0
w = Rot(y, 90 )v =
0
1
0
0
1
0
0
0
0
0
0
1
-3
7
2
1
2
7
3
1
V c th biu din :
2
7
3
1
Ch : Nu i th t quay ta s c w w (hnh 2.6), c th : cho U quay quanh y
trc 1 gc 900, ta c :
w = Rot(y, 90o). Rot(z, 90o) . u
0
0
-1
0
v =
0
1
0
0
1
0
0
0
0
0
0
1
7
3
2
1
2
3
-7
1
= Rot(y, 90o).u
Sau cho im va bin i quay quanh z mt gc 900, ta c :

0
1
0
0
w =
-1
0
0
0
0
0
1
0
0
0
0
1
2
3
-7
1
-3
2
-7
1
= Rot(z, 90o).Rot(y,900)u
R rng : Rot(y, 90o).Rot(z,900)u Rot(z,900).Rot(y, 90o)u

z
z
v
y
y
w
w
u
u
x
Hnh 2.5
w = Rot(y, 90o). Rot(z, 90o)u
TS. Phm ng Phc
Hnh 2.6
w= Rot(z, 90o). Rot(y, 90o)u
15
Robot cng nghip
2.3.3. Php quay tng qut :

Trong mc trn, ta va nghin cu cc php quay c bn xung quanh cc trc to
x,y,z ca h to chun O(x,y,z). Trong phn ny, ta nghin cu php quay quanh mt vect
k bt k mt gc . Rng buc duy nht l vect k phi trng vi gc ca mt h to xc
nh trc.
Ta hy kho st mt h to C, gn ln im tc ng cui (bn tay) ca robot, h C
c biu din bi :
n (Cz)
C=
Cx
nx
ny
nz
0
Cy
Ox
Oy
Oz
0
Cz
az
ay
az
0
Co
0
0
0
1
Co
O(Cy)
a (Cx)
Hnh 2.7 : H to gn trn

khu chp hnh cui (bn tay)
Khi gn h to ny ln bn tay robot (hnh 2.7), cc vect n v c biu th nh
sau :
a : l vect c hng tip cn vi i tng (approach);
O: l vect c hng m theo cc ngn tay nm vo khi cm nm i tng
(Occupation);
n : Vect php tuyn vi (O,a) (Normal).
By gi ta hy coi vect bt k k (m ta cn thc hin php quay quanh n mt gc )
l mt trong cc vect n v ca h C.
r
r
r
r
k = ax i + ay j + azk
Chng hn :
Lc , php quay Rot(k,) s tr thnh php quay Rot(Cz,).

Nu ta c T m t trong h gc trong k l vect bt k, th ta c X m t trong h C
vi k l mt trong cc vect n v. T iu kin bin i thun nht, T v X c lin h :
T = C.X
hay
X = C -1.T
Lc cc php quay di y l ng nht :
hay l
Vy
Rot(k,) = Rot(Cz,)
Rot(k,).T = C.Rot(z,).X = C.Rot(z,).C -1.T
Rot(k,) = C.Rot(z,).C -1
(2.6)
Trong Rot(z,) l php quay c bn quanh trc z mt gc , c th s dng cng

thc (2.5) nh trnh by.
C-1 l ma trn nghch o ca ma trn C. Ta c :
C-1 =
TS. Phm ng Phc
nx
Ox
ax
0
ny
Oy
ay
0
nz
Oz
az
0
0
0
0
1
16
Robot cng nghip
Thay cc ma trn vo v phi ca phng trnh (2.6) :

Rot(k,) =
nx
ny
nz
0
Ox
Oy
Oz
0
ax
ay
az
0
0
0
0
1
cos
sin
0
0
-sin
cos
0
0
0
0
1
0
0
0
0
1
nx
Ox
ax
0
ny
Oy
ay
0
nz
Oz
az
0
0
0
0
1
Nhn 3 ma trn ny vi nhau ta c :

nxnxcos - nxOxsin + nxOxsin + OxOxcos + axax
nxnycos - nyOxsin + nxOysin + OxOycos + ayax
nxnzcos - nzOxsin + nxOzsin + OxOzcos + azax
0
Rot(k,) =
nxnycos - nxOysin + nyOxsin + OxOycos + axay

nynycos - nyOysin + nyOysin + OyOycos + ayay
nznycos - nzOysin + nyOzsin + OzOycos + azay
0
nxnzcos - nxOzsin + nzOxsin + OxOzcos + axaz
nynzcos - nyOzsin + nzOysin + OyOzcos + ayaz
nznzcos - nzOzsin + nzOzsin + OzOzcos + azaz
0
0
0
0
1
(2.7)
n gin cch biu th ma trn, ta xt cc mi quan h sau :

- Tch v hng ca bt k hng hay ct no ca C vi bt k hng hay ct no khc
u bng 0 v cc vect l trc giao.
- Tch v hng ca bt k hng hay ct no ca C vi chnh n u bng 1 v l vect
n v.
r
r r
- Vect n v z bng tch vect ca x v y, hay l : a = n x O
Trong :
ax = nyOz - nzOy
ay = nxOz - nzOx
ax = nxOy - nyOx
Khi cho k trng vi mt trong s cc vect n v ca C ta chn :

kz = ax ; ky = ay ; kz = az
Ta k hiu Vers = 1 - cos (Versin ).
Biu thc (2.6) c rt gn thnh :
Rot(k,) =
kxkxvers+cos
kxkyvers+kzsin
kxkzvers+kysin
0
kykxvers-kzsin
kykyvers+cos
kykzvers+kzsin
0
kzkxvers+kysin
kzkyvers-kxsin
kzkzvers+cos
0
0
0
0
1
(2.8)
y l biu thc ca php quay tng qut quanh mt vect bt k k. T php quay tng
qut c th suy ra cc php quay c bn quanh cc trc to .
TS. Phm ng Phc
17
Robot cng nghip
2.3.4. Bi ton ngc : tm gc quay v trc quay tng ng :

Trn y ta nghin cu cc bi ton thun, ngha l ch nh trc quay v gc quay
trc- xem xt kt qu bin i theo cc php quay ch nh.
Ngc li vi bi ton trn, gi s ta bit kt qu ca mt php bin i no , ta
phi i tm trc quay k v gc quay tng ng. Gi s kt qu ca php bin i thun nht
R=Rot(k, ), xc nh bi :
nx Ox ax 0
R = ny Oy ay 0
nz Oz az 0
0
0
0
1
Ta cn xc nh trc quay k v gc quay . Ta bit Rot(k, ) c nh ngha bi ma
trn (2.6) , nn :
nx
ny
nz
0
Ox
Oy
Oz
0
ax
ay
az
0
0
0
0
1
kxkxvers+cos
= kxkyvers+kzsin
kxkzvers+kysin
0
kykxvers-kzsin
kykyvers+cos
kykzvers+kzsin
0
kzkxvers+kysin
kzkyvers-kxsin
kzkzvers+cos
0
0
0
0
1
Bc 1 : Xc nh gc quay .
* Cng ng cho ca hai ma trn hai v ta c :
nx + Oy + az + 1 = k x2 vers + cos + k y2 vers + cos + k z2 vers + cos + 1
(2.9)
= (1 - coss)( k x2 + k y2 + k z2 ) + 3cos + 1
= 1 - cos + 3cos +1
= 2(1+ cos)
cos
= (nx + Oy + az - 1)/2
* Tnh hiu cc phn t tng ng ca hai ma trn, chng hn :
Oz- ay = 2kxsin
ax - nz = 2kysin
ny - Ox = 2kzsin
(2.10)
Bnh phng hai v ca cc phng trnh trn ri cng li ta c :

(Oz- ay)2 + (ax - nz)2 + (ny - Ox)2 = 4 sin2
sin =
1
(O z - a y ) 2 + (a x - n z ) 2 + (n y - O x ) 2
2
Vi 0 1800 :
tg =
(O z - a y ) 2 + (a x - n z ) 2 + (n y - O x ) 2
(n x + O y + a z - 1)
V trc k c nh ngha bi :
Oz a y
ny Oz
ax nz
kx =
; ky =
; kx =
(2.11)
2sin
2sin
2sin
rng vi cc cng thc (2.8) :
0
- Nu = 00 th kx, ky, kz c dng . Lc ny phi chun ho k sao cho k = 1
0
TS. Phm ng Phc
18
Robot cng nghip
- Nu = 1800 th kx, ky, kz c dng
a0
. Lc ny k khng xc nh c, ta phi
0
dng cch tnh khc cho trng hp ny :

Xt cc phn t tng ng ca hai ma trn (2.9) :
nx = k x2 vers+cos
Oy = k y2 vers+cos
az = k z2 vers+cos
T y ta suy ra :
n x cos
n x cos
kx =
=
1- cos
vers
O y cos
O y cos
ky =
=
vers
1- cos
a z cos
a z cos
kz =
=
1- cos
vers
0
0
Trong khong 90 180 sin lun lun dng
Da vo h phng trnh (2.10) ta thy kx, ky, kz lun c cng du vi v tri. Ta dng
hm Sgn(x) biu din quan h cng du vi x, nh vy :
k x = Sgn(O z a y )
k y = Sgn(a x - n z )
n x cos
1- cos
O y cos
(2.12)
1- cos
a cos
k z = Sgn(n y O x ) z
1- cos
H phng trnh (2.12) ch dng xc nh xem trong cc kx, ky, kz thnh phn no c
gi tr ln nht. Cc thnh phn cn li nn tnh theo thnh phn c gi tr ln nht xc nh
k c thun tin. Lc dng phng php cng cc cp cn li ca cc phn t i xng
qua ng cho ma trn chuyn i (2.9) :
ny + Ox = 2kxkyvers = 2kxky(1 - cos)
Oz + ay = 2kykzvers = 2kykz(1 - cos)
(2.13)
ax + nz = 2kzkxvers = 2kzkx(1 - cos)

Gi s theo h (2.12) ta c kx l ln nht, lc ky, kz s tnh theo kx bng h (2.13); c
ny + Ox
th l :
ky =
2 k x (1 cos )
ax + nz
kz =
2 k x (1 cos )
0
V d : Cho R = Rot[y,90 ]Rot[z,900]. Hy xc nh k v R = Rot[k,]. Ta bit :
0 0 1 0
0
0
R = Rot(y,90 ).Rot(z,90 ) = 1 0 0 0
0 1 0 0
0 0 0 1
Ta c cos = (nx + Oy + az - 1) / 2 = (0 + 0 + 0 - 1) / 2 = -1 / 2
TS. Phm ng Phc
19
Robot cng nghip
1
(O z - a y ) 2 + (a x - n z ) 2 + (n y - O x ) 2
2
1
3
(1 - 0) 2 + (1 - 0) 2 + (1 - 0) 2
=
=
2
2
0
tg = 3 v = 120
sin =
Theo (2.12), ta c :
0 +1/ 2
1
=
1+1/ 2
3
k x = ky = kz = +
Vy : R = Rot(y,900).Rot(z,900) = Rot(k, 1200); vi :

r
1 r 1 r 1 r
k
j+
i+
k=
3
3
3
z
1/ 3
k
1200
1/ 3
y
x
1/ 3
Hnh 2.8 : Tm gc quay v trc quay tng ng
2.3.5. Php quay Euler :

Trn thc t, vic nh hng thng l kt qu ca php quay xung quanh cc trc x,
y, z . Php quay Euler m t kh nng nh hng bng cch :
Quay mt gc xung quanh trc z,
Quay tip mt gc xung quanh trc y mi, l y,
cui cng quay mt gc quanh trc z mi, l z (Hnh 2.9).
zz
z z
yy
y
Hnh 2.9 : Php quay Euler

Ta biu din php quay Euler bng cch nhn ba ma trn quay vi nhau :
Euler (,,) = Rot(z, ) Rot(y, ) Rot(z, )
(2.14)
TS. Phm ng Phc
20
Robot cng nghip
Ni chung, kt qu ca php quay ph thuc cht ch vo th t quay, tuy nhin ,

php quay Euler, nu thc hin theo th t ngc li, ngha l quay gc quanh z ri tip n
quay gc quanh y v cui cng quay gc quanh z cng a n kt qu tng t (Xt
trong cng h qui chiu).
cos -sin 0 0
Cos 0 sin 0
0
1
0
0
Euler (,,) = Rot(z, )
sin
cos 0 0
0
0
1 0
-sin 0 Cos 0
0
0
0
1
0
0
0 1
cos
sin
0
0
-sin
cos
0
0
0
0
1
0
Coscos
sin
-sin cos
0
0
0
0
1
cosCoscos - sinsin
= sinCoscos + cossin
-sin cos
0
-Cos sin
cos
sin sin
0
-cosCossin - sincos
-sinCossin + coscos
sin sin
0
sin
0
Cos
0
0
0
0
1
cossin
sinsin
cos
0
0
0
0
1
(2.15)
2.3.6. Php quay Roll-Pitch-Yaw :
Mt php quay nh hng khc cng thng c s dng l php quay Roll-Pitch v
Yaw.
Ta tng tng, gn h to xyz ln
thn mt con tu. Dc theo thn tu l trc z,
Roll l chuyn ng lc ca thn tu, tng
ng vi vic quay thn tu mt gc quanh
trc z. Pitch l s bng bnh, tng ng vi
quay mt gc xung quanh trc y v Yaw l
s lch hng, tng ng vi php quay mt
gc xung quanh trc x (Hnh 2.10)
x Yaw
Roll
Pitch
Cc php quay p dng cho khu chp

Thn tu
hnh cui ca robot nh hnh 2.11. Ta xc
nh th t quay v biu din php quay nh
Hnh 2.10: Php quay Roll-Pitch-Yaw
sau :
RPY(,,)=Rot(z,)Rot(y,)Rot(x, )
(2.16)
z
Roll,
Pitch,
y
x
Yaw,
Hnh 2.11 : Cc gc quay Roll-Pitch v Yaw ca bn tay Robot.

ngha l, quay mt gc quanh trc x, tip theo l quay mt gc quanh trc y v sau
quay mt gc quanh truc z.
TS. Phm ng Phc
21
Robot cng nghip
Thc hin php nhn cc ma trn quay, cc chuyn v Roll, Pitch v Yaw c biu th
nh sau :
cos
0
-sin
0
RPY(,,)=Rot(z,)
cos
sin
0
0
-sin
cos
0
0
coscos
sincos
-sin
0
0
0
1
0
0
1
0
0
0
0
0
1
sin
0
cos
0
0
0
0
1
cos
0
-sin
0
sinsin
cos
cossin
0
cossinsin - sincos
sinsinsin +coscos
cos sin
0
1
0
0
0
0
cos
sin
0
0
-sin
cos
0
sincos
-sin
cos cos
0
cossincos + sinsin
sinsincos - cossin
cos cos
0
0
0
0
1
0
0
0
1
0
0
0
1
(2.17)
2.4. Bin i h to v mi quan h gia cc h to bin i :

2.4.1 Bin i h to :
Gi s cn
gc to ct O(0, 0, 0) theo mt vect dn
r rtnh tin
r
r
h = 4i - 3j + 7k (hnh 2.12) . Kt qu ca php bin i l :
OT =
1
0
0
0
0
1
0
0
0
0
1
0
4
-3
7
1
0
0
0
1
4
-3
7
1
Ngha l gc ban u c to O(0, 0, 0) chuyn i n gc mi OT c to

(4, -3, 7) so vi h to c.
z
zT
7
OT
yT
xT
-3
Hnh 2.12 : Php bin i tnh tin h to

Tuy nhin trong php bin i ny cc trc to ca OT vn song song v ng hng
vi cc trc to ca O.
TS. Phm ng Phc
22
Robot cng nghip
Nu ta tip tc thc hin cc php bin i quay :

Rot(y,90o)Rot(z,90o).OT
ta s c mt h to hon ton mi, c th ti gc to mi (4,-3,7) khi cho h OT quay
quanh z mt gc 900 (chiu quay dng qui c l ngc chiu kim ng h), ta c :
zT
z'T
y'T
90o
OT
Rot(z,900)
yT
OT
x'T
xT
Ta tip tc quay h OT quanh truc y (trc y ca h to gc ) mt gc 900, Ta c :

z'T
y''T
y'T
90o
OT
Rot(y,900)
x'T
z"T
OT
x''T
V d trn y ta chn H to c s lm h qui chiu v th t thc hin cc

php bin i l t Phi sang Tri. Nu thc hin cc php bin i theo th t ngc li t
Tri sang Phi th h qui chiu c chn l cc h to trung gian. Xt li v d trn :
Rot(y,90o)Rot(z,90o).OT
zT
90o
OT
Rot(y,90o)
yT
y'T
O'T
z'T
xT
x'T
Ta tip tc quay h O'T quanh truc z (By gi l trc z'T ca h to mi) mt gc 900 :
y''T
y'T
90o O'T
z'T
Rot(z',90o)
z"T
O''T
x''T
x'T
Nh vy kt qu ca hai phng php quay l ging nhau, nhng v ngha vt l th

khc nhau.
2.4.2. Quan h gia cc h to bin i :
Gi s ta c 3 h to A, B, C; H B c quan h vi h A qua php bin i A T/ B v
h C c quan h vi h B qua php bin i B T/c . Ta c im P trong h C k hiu PC, ta tm
mi quan h ca im P trong h A, tc l tm PA (Hnh 2.13) :
TS. Phm ng Phc
23
Robot cng nghip
zC
zB
pC
zA
xC
pA
C
B
A
yA
xA
xB
yC
yB
Hnh 2.13 : Quan h gia cc h to bin i.
Chng ta c th bin i pC thnh pB nh sau :

pB = B T/c pC,
Sau bin i pB thnh pA nh sau :
pA = A T/ B pB,
(2.18)
(2.19)
Kt hp (2.18) v (2.19) ta c :
p A = A TB BTC p c
(2.20)
Qua v d trn ta thy c th m t mi quan h gia h to gn trn im tc ng

cui vi h ta c bn, thng qua mi quan h ca cc h to trung gian gn trn cc
khu ca robot, bng ma trn T nh hnh 2.14.
z
O2
O3
Bn tay
O1
T4
O0
O4
y
Hnh 2.14 : H to c bn (base) v cc h to trung gian ca Robot.

2.5. M t mt vt th :
Cc vt th l i tng lm vic ca robot rt a dng v phong ph, tuy nhin c th
da vo nhng c im hnh hc m t chng. Ta c th chia hnh dng vt th thnh 3
nhm chnh sau :
Nhm vt th trn xoay (Rotative)
Nhm vt th c gc cnh (Prismatic)
Nhm vt th c cu trc hn hp (Kombination)
Nhm vt th trn xoay c cc gi tr c trng l to tm v bn knh mt cong.
Nhm vt th c gc cnh c trng bng to ca cc im gii hn.
Nhm cn li c cc gi tr c trng hn hp.
Tuy nhin, i vi hot ng cm nm i tng v qu trnh vn ng ca robot vic
m t vt th cn phi gn lin vi cc php bin i thun nht. Ta xt v d sau y : Cho
mt vt hnh lng tr t trong h to chun O(xyz) nh hnh 2.15.
TS. Phm ng Phc
24
Robot cng nghip
Ta thc hin cc php bin i sau :
z
H = Trans(4,0,0)Rot(y,900)Rot(z,900)
-1,0,2,1
Vi v tr ca vt th, ta c ma trn to ca 6
im c trng m t n l :
1,0,2,1
-1,0,0,1
-1,4,0,1
y
1
0
0
1
-1
0
0
1
-1
0
2
1
1
0
2
1
1
4
0
1
-1
4
0
1
1,0,0,1
1,4,0,1
x
Hnh 2.15 : M t vt th
Sau khi thc hin cc php bin i :

- Quay vt th quanh trc z mt gc 900 (Hnh 2.16),
- Cho vt th quay quanh trc y mt gc 900 (Hnh 2.17),
- Tip tc tnh tin vt th dc theo trc x mt on bng 4 n v (hnh 2.18) ta xc
nh c ma trn to cc im gii hn ca vt th v tr c bin i nh
sau (cc php quay chn h qui chiu l h to gc) :
0
1
0
0
H=
0
0
1
0
1
0
0
0
4
0
0
1
4
1
0
1
1
0
0
1
4
-1
0
1
-1
0
0
1
6
-1
0
1
-1
0
2
1
1
0
2
1
6
1
0
1
4
1
4
1
1
4
0
1
-1
4
0
1
4
1
4
1
x
Hnh 2.16 : Rot (z,900)
TS. Phm ng Phc
Hnh 2.17: Rot (y,900) Rot (z,900)
25
Robot cng nghip
y
O
H = Trans(4,0,0)Rot (y,900)Rot (z,900)
Hnh 2.18: V tr vt th sau khi bin i

2.6. Kt lun :
Cc php bin i thun nht dng miu t v tr v hng ca cc h to trong
khng gian. Nu mt h to c gn lin vi i tng th v tr v hng ca chnh i
tng cng c m t. Khi m t i tng A trong mi quan h vi i tng B bng cc
php bin i thun nht th ta cng c th da vo m t ngc li mi quan h ca B i
vi i tng A.
Mt chuyn v c th l kt qu lin tip ca nhiu php bin i quay v tnh tin. Tuy
nhin ta cn lu n th t ca cc php bin i, nu thay i th t thc hin c th dn
n cc kt qu khc nhau.
Bi tp chng II :
Bi 1 : Cho im A biu din bi vect im v=[ 2 4 1 1 ]T. Tnh tin im A theo vect dn h
= [ 1 2 1 1 ]T, sau tip tc quay im bin i quanh trc x mt gc 900. Xc nh vect
biu din im A sau hai php bin i.
Bi 2 : Vit ma trn bin i thun nht biu din cc php bin i sau :
H = Trans(3,7,9)Rot(x,-900)Rot(z,900)
Bi 3 : Cho ma trn bin i thun nht A, tm ma trn nghch o A-1 v kim chng.
A =
TS. Phm ng Phc
0
0
-1
0
1
0
0
0
0 -1
-1 2
0 0
0 1
26
Robot cng nghip
Bi 4 : Hnh v 2-19 m t h to {B} c

quay i mt gc 300 xung quanh trc zA, tnh tin
dc theo trc xA 4 n v v tnh tin dc theo yA
3 n v.
(a) M t mi qua h ca {B} i vi {A} : ATB ?
(b) Tm mi quan h ngc li BTA ?
{B}
{A}
yB
xB
yA
xA
Hnh 2.19 : Quan h {A} v {B}
1
(1, 1, 1)T, = 900. Tm ma trn R = Rot(k, ).
3
Bi 6 : Xc nh cc gc quay Euler, v cc gc quay RPY khi bit ma trn T6 :
Bi 5 : Cho k =
1
0
0
0
T6 =
0
0
-1
0
0
1
0
0
0
5
3
1
Bi 7 : Mt vt th t trong mt h to tham chiu c xc nh bi php bin i :

U
TP =
0
0
-1
0
1
0
0
0
0 -1
-1 2
0 0
0 1
Mt robot m h to chun c lin h vi h to tham chiu bi php bin i

U
TR =
1
0
0
0
0
1
0
0
0
0
1
0
1
5
9
1
Chng ta mun t bn tay ca robot ln vt th, l lm cho h ta gn trn bn tay

trng vi h to ca vt th. Tm php bin i RTH (biu din mi quan h gia bn tay v
h to gc ca robot) thc hin iu ni trn.
TS. Phm ng Phc
27
Robot cng nghip
Chng III
phng trnh ng hc ca robot
(Kinematic Equations)
3.1. Dn nhp :
Bt k mt robot no cng c th coi l mt tp hp cc khu (links) gn lin vi cc
khp (joints). Ta hy t trn mi khu ca robot mt h to . S dng cc php bin i
thun nht c th m t v tr tng i v hng gia cc h to ny. Denavit. J. gi
bin i thun nht m t quan h gia mt khu v mt khu k tip l mt ma trn A. Ni
n gin hn, mt ma trn A l mt m t bin i thun nht bi php quay v php tnh tin
tng i gia h to ca hai khu lin nhau. A1 m t v tr v hng ca khu u tin; A2
m t v tr v hng ca khu th hai so vi khu th nht. Nh vy v tr v hng ca khu
th hai so vi h to gc c biu din bi ma trn :
T2 = A1.A2
Cng nh vy, A3 m t khu th ba so vi khu th hai v :
T3 = A1.A2.A3 ; v.v...
Cng theo Denavit, tch ca cc ma trn A c gi l ma trn T, thng c hai ch s:
trn v di. Ch s trn ch h to tham chiu ti, b qua ch s trn nu ch s bng 0.
Ch s di thng dng ch khu chp hnh cui. Nu mt robot c 6 khu ta c :
T6 = A1.A2.A3.A4.A5.A6
(3.1)
T6 m t mi quan h v hng v v tr ca khu chp hnh cui i vi h to gc.

Mt robot 6 khu c th c 6 bc t do v c th c nh v tr v nh hng trong trng
vn ng ca n (range of motion). Ba bc t do xc nh v tr thun tu v ba bc t do khc
xc nh hng mong mun. T6 s l ma trn trnh by c hng v v tr ca robot. Hnh 3.1
m t quan h vi bn tay my. Ta t gc to ca h m t ti im gia ca cc ngn
tay. Gc to ny c m t bi vect p (xc nh v tr ca bn tay). Ba vect n v m t
hng ca bn tay c xc nh nh sau :
n
a o
Hnh 3.1 : Cc vect nh v tr v nh hng ca bn tay my
TS. Phm ng Phc
28
Robot cng nghip
Vect c hng m theo bn tay s tip cn n i tng, gi l vect a

(approach).
Vect c hng m theo cc ngn tay ca bn tay nm vo nhau khi cm nm
i tng, gi l vect o (Occupation).
Vect cui cng l vect php tuyn n (normal), do vy ta c :
r r r
n=oxa
Chuyn v T6 nh vy s bao gm cc phn t :
nx
ny
nz
0
T6 =
Ox
Oy
Oz
0
ax
ay
az
0
px
py
pz
1
(3.2)
Tng qut, ma trn T6 c th biu din gn hn nh sau :

Ma trn nh hng R
T6 =
Vect v tr p
(3.3)
Ma trn R c kch thc 3x3, l ma trn trc giao biu din hng ca bn kp (khu
chp hnh cui) i vi h to c bn. Vic xc nh hng ca khu chp hnh cui cn
c th thc hin theo php quay Euler hay php quay Roll, Pitch, Yaw.
Vect im p c kch thc 3x1, biu din mi quan h ta v tr ca ca gc h

ta gn trn khu chp hnh cui i vi h to c bn.
3.2. B thng s Denavit-Hartenberg (DH) :
Mt robot nhiu khu cu thnh t cc khu ni tip nhau thng qua cc khp ng.
Gc chun (Base) ca mt robot l khu s 0 v khng tnh vo s cc khu. Khu 1 ni vi
khu chun bi khp 1 v khng c khp u mt ca khu cui cng. Bt k khu no
cng c c trng bi hai kch thc :
di php tuyn chung : an .
Gc gia cc trc trong mt phng vung gc vi an : n.
Khp n
Khp n+1
Khu n
n
a
Hnh 3.5 : Chiu di v gc xon ca 1 khu.

Thng thng, ngi ta gi an l chiu di v n l gc xon ca khu (Hnh 3.5). Ph
bin l hai khu lin kt vi nhau chnh trc ca khp (Hnh 3.6).
TS. Phm ng Phc
29
Robot cng nghip
Khp n
Khp n-1
Khp n+1
n-1
n+1
Khu n
Khu n+1
Khu n-1
Khu n-2
an
zn-1
dn
zn
xn
On
xn-1
n
Hnh 3.6 : Cc thng s ca khu : , d, a v .
Mi trc s c hai php tuyn vi n, mi php tuyn dng cho mi khu (trc v sau
mt khp). V tr tng i ca hai khu lin kt nh th c xc nh bi dn l khong cch
gia cc php tuyn o dc theo trc khp n v n l gc gia cc php tuyn o trong mt
phng vung gc vi trc.
dn v n thng c gi l khong cch v gc gia cc khu.
m t mi quan h gia cc khu ta gn vo mi khu mt h to . Nguyn
tc chung gn h ta ln cc khu nh sau :
+ Gc ca h to gn ln khu th n t ti giao im ca php tuyn an vi trc
khp th n+1. Trng hp hai trc khp ct nhau, gc to s t ti chnh im ct . Nu
cc trc khp song song vi nhau, gc to c chn trn trc khp ca khu k tip, ti
im thch hp.
+ Trc z ca h to gn ln khu th n t dc theo trc khp th n+1.
+ Trc x thng c t dc theo php tuyn chung v hng t khp n n n+1.
r r
Trong trng hp cc trc khp ct nhau th trc x chn theo tch vect z n x z n-1 .
Trng hp khp quay th n l cc bin khp, trong trng hp khp tnh tin th dn
l bin khp v an bng 0.
Cc thng s an, n, dn v n c gi l b thng s DH.
V d 1 : Xt mt tay my c hai khu phng nh hnh 3.7 :
y2
x2
O2
y1
y0
z0
z2
x1 a
2
a1
1
O0
z1
O1
x0
Hnh 3.7 : Tay my c hai khu phng (v tr bt k).
TS. Phm ng Phc
30
Robot cng nghip
Ta gn cc h to ln cc khu nh hnh v : trc z0, z1 v z2 vung gc vi t giy.

H to c s l O0x0y0z0, chiu ca x0 hng t O0 n O1. Sau khi thit lp h to c s,
H to o1x1y1z1 c hng nh hnh v, O1 t ti tm trc khp 2. H to O2x2y2x2 c gc
O2 t im cui ca khu 2.
Bng thng s Denavit-Hartenbert ca tay my ny nh sau :
Khu
1
2
i
1 *
2 *
i
0
0
ai
a1
a2
di
0
0
Trong i l cc bin khp (dng du * k hiu cc bin khp).

V d 2 : Xem s robot SCARA c 4 khu nh hnh 3.8 :
y l robot c cu hnh kiu RRTR, bn tay c chuyn ng xoay xung quanh trc
ng. H to gn ln cc khu nh hnh v.
a2
z0
z1
1
2
x2
x
0
O
0
O1
O2
x1
z2 x
3
d3
a1
O3
O4
4
d4
x
z3 , z4
Hnh 3.8 : Robot SCARA v cc h to (v tr ban u).
i vi tay my ny cc trc khp u song song nhau, tin li tt c cc gc to
t ti tm cc trc khp. Trc x0 nm trong mt phng t giy. Cc h to khc nh hnh
v. Bng thng s DH ca robot SCARA nh sau :
Khu
1
2
3
4
i
1 *
2 *
0
4 *
i
0
1800
0
0
ai
a1
a2
0
0
di
0
0
d3*
d4
* : Cc bin khp.
3.3. c trng ca cc ma trn A :
Trn c s cc h to n nh cho tt c cc khu lin kt ca robot, ta c th
thit lp mi quan h gia cc h to ni tip nhau (n-1), (n) bi cc php quay v tnh tin
sau y :
Quay quanh zn-1 mt gc n
Tnh tin dc theo zn-1 mt khong dn
Tnh tin dc theo xn-1 = xn mt on an
Quay quanh xn mt gc xon n
TS. Phm ng Phc
31
Robot cng nghip
Bn php bin i thun nht ny th hin quan h ca h to thuc khu th n so

vi h to thuc khu th n-1 v tch ca chng c gi l ma trn A :
An = Rot(z,) Trans(0,0,d) Trans(a,0,0) Rot(x,)
An =
An =
0
0
1
0
cos
sin
0
0
-sin
cos
0
0
cos
sin
0
0
-sin cos
cos cos
sin
0
0
0
0
1
1
0
0
0
0
1
0
0
sin sin
-cos sin
cos
0
0
0
1
0
a
0
d
1
1
0
0
0
(3.4)
0
cos
sin
0
0
-sin
cos
0
a cos
a sin
d
1
0
0
0
1
(3.5)
i vi khp tnh tin (a = 0 v i = 0) th ma trn A c dng :

An =
1
0
0
0
0
cos
sin
0
0
- sin
cos
0
0
0
d
1
(3.6)
i vi mt khu i theo mt khp quay th d, a v l hng s. Nh vy ma trn A

ca khp quay l mt hm s ca bin khp .
i vi mt khu i theo mt khp tnh tin th , l hng s. Ma trn A ca khp
tnh tin l mt hm s ca bin s d.
Nu cc bin s c xc nh th gi tr ca cc ma trn A theo cng c xc
nh.
3.4. Xc nh T6 theo cc ma trn An :
Ta bit :
T6 = A1A2A3A4A5A6
Trong T6 c miu t trong h to gc (h to gn vi khu c bn c nh

ca robot). Nu m t T6 theo cc h to trung gian th n-1 th :
6
n 1
Trong trng hp tng qut, khi

xt quan h ca robot vi cc thit b
khc, nu h to c bn ca robot c
lin h vi mt h to no bi php
bin i Z, Khu chp hnh cui li c
gn mt cng c, c quan h vi vt th
bi php bin i E (hnh 3.9) th v tr v
hng ca im cui ca cng c, kho
st h to tham chiu m t bi X s
c xc nh bi :
X= Z T6E
TS. Phm ng Phc
i=n
Ai
OR
Z
T6
E
A
Hnh 3.9 : Vt th v Robot
32
Robot cng nghip
Quan h ny c th hin trn ton sau :
Z O0 A1
A2
A3
A4
A5
OR
5
4
3
2
E A X
T6
OR
T6
T6
T6
T6
T6
Hnh 3.10 : Ton chuyn v ca robot.
T ton ny ta c th rt ra :
T6 = Z-1 X E-1
-1
-1
(Z v E l cc ma trn nghch o).
3.5. Trnh t thit lp h phng trnh ng hc ca robot :
thit lp h phng trnh ng hc ca robot, ta tin hnh theo cc bc sau :
1. Chn h to c s, gn cc h to m rng ln cc khu.
Vic gn h to ln cc khu ng vai tr rt quan trng khi xc lp h phng
trnh ng hc ca robot, thng thng y cng l bc kh nht. Nguyn tc gn h to
ln cc khu c trnh by mt cch tng qut trong phn 3.5. Trong thc t, cc trc
khp ca robot thng song song hoc vung gc vi nhau, ng thi thng qua cc php bin
i ca ma trn A ta c th xc nh cc h to gn trn cc khu ca robot theo trnh t
sau :
+ Gi nh mt v tr ban u() (Home Position) ca robot.
+ Chn gc to O0, O1, ...
+ Cc trc zn phi chn cng phng vi trc khp th n+1.
+ Chn trc xn l trc quay ca zn thnh zn+1 v gc ca zn vi zn+1 chnh l n+1. Nu zn
v zn+1 song song hoc trng nhau th ta c th cn c nguyn tc chung hay chn xn theo xn+1.
+ Cc h to Oxyz phi tun theo qui tc bn tay phi.
+ Khi gn h to ln cc khu, phi tun theo cc php bin i ca ma trn An.
l bn php bin i : An = Rot(z,) Trans(0,0,d) Trans(a,0,0) Rot(x,). Ngha l ta coi h to
th n+1 l bin i ca h to th n; cc php quay v tnh tin ca bin i ny phi l
mt trong cc php bin i ca An, cc thng s DH cng c xc nh da vo cc php
bin i ny. Trong qu trnh gn h ta ln cc khu, nu xut hin php quay ca trc zn
i vi zn-1 quanh trc yn-1 th v tr ban u ca robot gi nh l khng ng, ta cn chn
li v tr ban u khc cho robot.
2. Lp bng thng s DH (Denavit Hartenberg).
3. Da vo cc thng s DH xc nh cc ma trn An.
4. Tnh cc ma trn T v vit cc phng trnh ng hc ca robot.
()
V tr ban u l v tr m cc bin nhn gi tr ban u, thng bng 0.
TS. Phm ng Phc
33
Robot cng nghip
V d sau y trnh by chi tit ca cc bc khi thit lp h phng trnh ng hc

ca robot :
Cho mt robot c ba khu, cu hnh RRT nh hnh 3.11. Hy thit lp h phng trnh
ng hc ca robot.
2
d3
Hnh 3.11 : Robot RRT

1. Gn h to ln cc khu :
Ta gi nh v tr ban u v chn gc to O0 ca robot nh hnh 3.12. Cc trc z t
cng phng vi cc trc khp.
Ta thy trc z1 quay tng i mt
gc 90 so vi trc z0, y chnh l php quay
quanh trc x0 mt gc 1 (php bin i
Rot(x0,1) trong biu thc tnh An). Ngha l
trc x0 vung gc vi z0 v z1. Ta chn chiu
ca x0 t tri sang phi th gc quay 1=900
(chiu dng ngc chiu kim ng h).
ng thi ta cng thy gc O1 tnh tin
mt on dc theo z0 , so vi O0, chnh l
php bin i Trans(0,0,d1) (tnh tin dc theo
z0 mt on d1) ; cc trc y0,v y1 xc nh
theo qui tc bn tay phi (Hnh 3.12 ) .
y1
x1 z2
z1
1
d1
z0
y0
x0
O0
Hnh 3.12 : Gn cc h to O0 v O1
z3
Tip tc chn gc ta O2 t trng

vi O1 v trc khp th ba v trc khp th
hai ct nhau ti O1 (nh hnh 3.12). Trc z2
cng phng vi trc khp th ba, tc l
quay i mt gc 900 so vi z1 quanh trc y1;
php bin i ny khng c trong biu thc
tnh An nn khng dng c, ta cn chn li
v tr ban u ca robot (thay i v tr ca
khu th 3) nh hnh 3.13.
Theo hnh 3.13, O2 vn c t trng
vi O1, trc z2 c phng thng ng, ngha l
ta quay trc z1 thnh z2 quanh trc x1 mt
gc -900 (tc 2= -900).
u cui ca khu th 3 khng c
khp, ta t O3 ti im gia ca cc ngn
tay, v trc z3, x3 chn nh hnh v, nh vy
ta tnh tin gc to dc theo z2 mt
on d3 (Php bin i Trans(0,0,d3)), v y
l khu tnh tin nn d3 l bin .
TS. Phm ng Phc
d3
O1 , O2
O3
x3
d3
d3
y1 z2
O1 O2
x1 x2
z1
1
d1
z0
O0
y0
x0
Hnh 3.13 : H to
gn ln cc khu
34
Robot cng nghip
Nh vy vic gn cc h to ln cc khu ca robot hon thnh. Thng qua cc

phn tch trn y, ta c th xc nh c cc thng s DH ca robot.
2. Lp bng thng s DH :
i
1*
i*
0
Khu
1
2
3
i
90
-90
0
ai
0
0
0
di
d1
0
d3 *
3. Xc nh cc ma trn A :
Ma trn An c dng :
cos -sin cos sin sin
An = sin
cos cos -cos sin
0
sin
cos
0
0
0
Vi qui c vit tt : C1 = cos1 ; S1 = sin1 ; C2 = cos2 . . .
A1 =
C1
S1
0
0
0
0
1
0
S1
-C1
0
0
0
0
d1
1
A2 =
C2
S2
0
0
0
0
-1
0
-S2
C2
0
0
0
0
0
1
A3 =
1
0
0
0
0
1
0
0
0
0
1
0
0
0
d3
1
0
0
d
1
4. Tnh cc ma trn bin i thun nht T :

+ Ma trn 2T3 = A3
+ Ma trn 1T3 = A2. 2T3
1
T3 =
+
C2 0 -S2 0
S2 0 C2 0
0 -1 0 d2
0
0
0
1
Ma trn T3 = A1 . 1T3
T3 =
TS. Phm ng Phc
C1
S1
0
0
0
0
1
0
1
0
0
0
S1
-C1
0
0
0
1
0
0
0
0
1
0
0
0
d1
1
0
0
d3
1
C2
S2
0
0
C2
S2
0
0
0
0
-1
0
0
0
-1
0
-S2
C2
0
0
-S2
C2
0
0
-S2*d3
C2*d3
0
1
-S2*d3
C2*d3
0
1
35
Robot cng nghip
C1C2
S1d2
S2
0
-S1
C1
0
0
-C1S2
-S1S2
C2
0
-C1S2d3
-S1S2d3
C2d3 + d1
1
Ta c h phng trnh ng hc ca robot nh sau :

nx = C1C2;
Ox = -S1;
ax = -C1S2;
px = -C1S2d3
ny = S1C2;
Oy = C1;
ay = -S1S2;
py = -S1S2d3
nz = S2
Oz = 0;
az = C2;
pz = C2d3 + d1;
(Ta c th s b kim tra kt qu tnh ton bng cch da vo to v tr px,py, pz

tnh so vi cch tnh hnh hc trn hnh v).
3.9. H phng trnh ng hc ca robot STANFORD :
Stanford l mt robot c 6 khu vi cu hnh RRT.RRR (Khu th 3 chuyn ng tnh
tin, nm khu cn li chuyn ng quay). Kt cu ca robot Stanford nh hnh 3.14 :
Hnh 3.14 : Robot Stanford
TS. Phm ng Phc
36
Robot cng nghip
Trn hnh 3.15 trnh by m hnh

ca robot Stanford vi vic gn cc h to
ln tng khu. n gin trong khi
vit cc phng trnh ng hc ca robot,
ta qui c cch vit tt cc hm lng gic
nh sau :
O3,O4,O5,O6
z3,z5,z6
z4
xi
d2
C1 = cos1;
S1 = sin1;
C12 = cos(1+2);
S12 = sin(1+2)
S234 = sin (2+3+4) ... .
d3
z0
z2
O0,O1 z1
x0
H to gn ln cc khu ca robot nh
hnh 3.15. (Khu cui c chiu di v
khong cch bng khng, c th gn cc
loi cng c khc nhau nn chn O6O5).
O2
x1
Hnh 3.15 : H to ca Robot Stanford
Bng thng s DH (Denavit-Hartenberg) ca robot Stanford nh sau :

Khu
i
i
0
-90
1
1 *
900
2
2 *
3
0
0
-900
4
4 *
900
5
5 *
0
6
6 *
(* : Cc bin khp).
ai
0
0
0
0
0
0
di
0
d2
d3 *
0
0
0
Cc ma trm A ca robot Stanford c xc nh nh sau :
A1=
A3=
A5=
C1
S1
0
0
0
0
-1
0
-S1
C1
0
0
1
0
0
0
0
1
0
0
0
0
1
0
0
0
d3
1
C5
S5
0
0
0
0
1
0
S5
-C5
0
0
0
0
0
1
0
0
0
1
A2=
A4=
A6=
C2
S2
0
0
0
0
1
0
S2
-C2
0
0
0
0
d2
1
C4
S4
0
0
0
0
-1
0
-S4
C4
0
0
0
0
0
1
C6
S6
0
0
-S6
C6
0
0
0
0
1
0
0
0
0
1
Tch ca cc ma trn chuyn v A i vi robot Stanford c bt u khu 6 v

chuyn dn v gc; theo th t ny ta c :
TS. Phm ng Phc
37
Robot cng nghip
T6 =
C6
S6
0
0
-S6
C6
0
0
0
0
1
0
C5C6
S5C6
S6
0
T6 = A5A6 =
0
0
0
1
-C5S6
-S5S6
C6
0
S5
-C5
0
0
C4C5C6 - S4S6
S4C5C6 + C4S6
-S5C6
0
T63 = A4A5A6 =
-C4C5S6-S4C6
-S4C5S6 + C4C6
S5S6
0
C4C5C6-S4S6
S4C5C + C4S6
-S5C6
0
T6 = A3A4A5A6 =
T6 =A2 A3A4A5A6 =
0
0
0
1
C4S5
S4S5
C5
0
-C4C5S6 - S4C6
-S4C5S6 + C4C6
S5S6
0
C2(C4C5C6 - S4S6) - S2S5C6

S2(C4C5C6 - S4S6) + C2S5C6
S4C5C6 + C4S6
0
0
0
0
1
C4S5
S4S5
C5
0
0
0
d3
1
-C2(C4C5S6-S4C6)+S2S5S6
-S2(C4C5S6+S4C6)-C2S5S6
-S4C5S6+C4C6
0
C2C4S5 + S2C5
S2C4S5 - C2C5
S4S5
0
S2d3
-C2d3
d2
1
Cui cng :
T6 =
nx
ny
nz
0
Ox
Oy
Oz
0
ax
ay
az
0
px
py
pz
1
A1T61
tnh T6, ta phi nhn A1 vi T61 sau cn bng cc phn t ca ma trn T6 hai v
ta c mt h thng cc phng trnh sau :
nx = C1[C2(C4C5C6 - S4S6) - S2S5C6] - S1(S4C5C6 + C4S6)
ny = S1[C2(C4C5C6 - S4S6) - S2S5C6] + C1(S4C5C6 + C4S6)
nz = -S2(C4C5C6 - S4S6) + C2S5C6
Ox = C1[-C2(C4C5S6 + S4C6) + S2S5S6] - S1(-S4C5S6 + C4C6)
Oy = S1[-C2(C4C5S6 + S4C6) + S2S5S6] + C1(-S4C5C6 + C4C6)
Oz = S2(C4C5S6 + S4C6) + C2S5S6
aX = C1(C2C4S5 + S2C5) - S1S4S5
ay = S1(C2C4S5 + S2C5) + C1S4S5
az = -S2C4S5 + C2C5
px = C1S2d3 - S1d2
py = S1S2d3 + C1d2
pz = C2d3
TS. Phm ng Phc
38
Robot cng nghip
Nu ta bit c cc gi tr ca bin khp, th v tr v hng ca bn tay robot s tm

c bng cch xc nh cc gi tr cc phn t ca T6 theo cc phng trnh trn.
Cc phng trnh trn gi l h phng trnh ng hc thun ca robot Stanford.
3.10. H phng trnh ng hc ca robot ELBOW :
hiu r hn v cch thit lp h phng trnh ng hc ca robot, ta xt thm
trng hp robot Elbow.
Khu 2
Khu 3
Khu 4
Khu 5
Khu 1
Khu 6
Hnh 1.16 : Robot Elbow
z0
z1
a2
O0,O1
z4
a3
O2
a4
O3
z2
O2,O5,O6
xi
z3
z 5, z 6
6
a5 = a6 = 0
Hnh 1.17 : V tr ban u ca robot Elbow v cc h to

B thng s DH ca robot Elbow
Khu
i *
i
0
1
90
1
2
0
2
3
0
3
4
-900
4
5
900
5
6
0
6
(* : cc bin khp )
ai
0
a2
a3
a4
0
0
di
0
0
0
0
0
0
Cc ma trn A ca robot Elbow c xc nh nh sau :

A1=
C1
S1
0
0
TS. Phm ng Phc
0
0
1
0
S1
-C1
0
0
0
0
0
1
A2=
C2
S2
0
0
-S2
C2
0
0
0
0
1
0
C 2 a2
S 2 a2
0
1
39
Robot cng nghip
A3=
C3
S3
0
0
-S3
C3
0
0
A5=
C5
S5
0
0
0
0
1
0
0
0
1
0
S5
-C5
0
0
C 3 a3
S 3 a3
0
1
0
0
0
1
A4=
C4
S4
0
0
A6=
C6
S6
0
0
0
0
-1
0
-S4
C4
0
0
-S6
C6
0
0
C 4 a4
S 4 a4
0
1
0
0
1
0
0
0
0
1
Ta xc nh cc ma trn T theo cc h to ln lt t khu cui tr v gc :

T65 =
C6
S6
0
0
-S6
C6
0
0
T6 = A5A6 =
T63 = A4A5A6 =
T6 = A3A4A5A6 =
0
0
1
0
C5C6
S5C6
S6
0
0
0
0
1
-C5S6
-S5S6
C6
0
S5
-C5
0
0
C4C5C6 - S4S6
S4C5C6+C4S6
-S5C6
0
C34C5C6 - S34S6
S34C5C6+C34S6
-S5C6
0
0
0
0
1
-C4C5S6-S4C6
-S4C5S6+C4C6
S5S6
0
-C34C5C6 - S34C6
-S34C5S6+C34C6
S5S6
0
C4S5
S4S5
C5
0
C34S5
S34S5
C5
0
C4a4
S4a4
0
1
C34a4+C3a3
S34a4+S3a3
0
1
T61 =A2 A3A4A5A6 =

C234C5C6 - S234S6
S234C5C6 + C234S6
-S5C6
0
-C234C5S6 - S234C6
-S234C5S6 + C234C6
S5S6
0
C234S5
S234S5
C5
0
C234a4+C23a3+C2a2
S234a4+S23a3+S2a2
0
1
Cui cng :
T6 =
nx
ny
nz
0
Ox
Oy
Oz
0
ax
ay
az
0
px
py
pz
1
= A1T61
tnh T6, ta phi nhn A1 vi T61 sau cn bng cc phn t ca ma trn T6 ta c

mt h thng cc phng trnh sau :
TS. Phm ng Phc
40
Robot cng nghip
nx = C1(C234C5C6- S234S6) - S1S5C6

ny = S1(C234C5C6- S234S6) + C1S5C6
nz = S234C5C6 + C234S6
Ox = -C1(C234C5S6 + S234C6) + S1S5S6
Oy = -S1(C234C5S6 + S234C6) - C1S5S6
Oz = -S234C5S6 + C234C6
aX = C1C234S5 + S1C5
ay = S1C234S5 - C1C5
az = S234S5
px = C1(C234a4 + C23a3 + C2a2)
py = S1(C234a4 + C23a3 + C2a2)
pz = S234a4 + S23a3 + S2a2
r r
r
Ct u tin ca ma trn T6 c th c xc nh bi tch vect : n = O x a.
3.11. Kt lun :
Trong chng ny chng ta nghin cu vic dng cc php bin i thun nht
m t v tr v hng ca khu chp hnh cui ca robot thng qua vic xc lp cc h to
gn ln cc khu v cc thng s DH. Phng php ny c th dng cho bt c robot no vi
s khu (khp) tu . Trong qu trnh xc lp cc h to m rng ta cng xc nh c v
tr dng ca mi robot. Tu thuc kt cu ca robot cng nh cng c gn ln khu chp hnh
cui m ta c th a cc thng s ca khu chp hnh cui vo phng trnh ng hc hay
khng. Vic tnh ton cc ma trn T thit lp h phng trnh ng hc ca robot thng
tn nhiu thi gian v d nhm ln, ta c th lp trnh trn my tnh tnh ton ( dng k
hiu) nhm nhanh chng xc nh cc ma trn An v thit lp h phng trnh ng hc ca
robot .
Thit lp h phng trnh ng hc ca robot l bc rt quan trng c th da vo
lp trnh iu khin robot. Bi ton ny thng c gi l bi ton ng hc thun
robot. Vic gii h phng trnh ng hc ca robot c gi l bi ton ng hc ngc,
nhm xc nh gi tr ca cc bin khp theo cc thng s bit ca khu chp hnh cui;
vn ny ta s nghin cu trong chng tip theo.
Bi tp chng III :
Bi 1 : Cho ma trn :
T6 =
?
?
?
?
0 -1
0 0
-1 0
0 0
0
1
2
1
l ma trn biu din hng v v tr ca khu chp hnh cui. Tm cc phn t c nh du ?

Bi 2 : Cho mt robot c 3 khu phng nh hnh 3.18, cu hnh RRR. Thit lp h phng
trnh ng hc ca robot.
TS. Phm ng Phc
41
Robot cng nghip
Bi 3 : Cho mt robot c 2 khu tnh tin nh hnh 3.19, cu hnh TT. Thit lp h phng
trnh ng hc ca robot.
Hnh 3.18 : Robot cu hnh RRR
Hnh 3.19 : Robot cu hnh TT
Bi 4 : Cho mt robot c 2 khu phng nh hnh 3.20, cu hnh RT. Thit lp h phng trnh
ng hc ca robot.
Bi 5 : Cho mt robot c 3 khu nh hnh 3.21, cu hnh RTR. Thit lp h phng trnh ng
hc ca robot.
Hnh 3.20 : Robot cu hnh RT
Hnh 3.21 : Robot cu hnh RTR
Bi 6 : Cho mt robot c 3 khu nh hnh 3.22, cu hnh RRR. Thit lp h phng trnh
ng hc ca robot.
Hnh 3.22 : Robot cu hnh RRR
Hnh 3.23 : Robot cu hnh RRRRR
Bi 7 : Cho mt robot c 5 khu nh hnh 3.23, cu hnh RRRRR. Thit lp h phng trnh
ng hc ca robot.
TS. Phm ng Phc
42
Robot cng nghip
Chng IV
Gii phng trnh ng hc robot

hay phng trnh ng hc ngc
(Invers Kinematic Equations)
Trong chng 3, ta nghin cu vic thit lp h phng trnh ng hc ca

robot thng qua ma trn T6 bng phng php gn cc h to ln cc khu v xc
nh cc thng s DH. Ta cng xt ti cc phng php khc nhau m t hng
ca khu chp hnh cui nh cc php quay Euler, php quay Roll-Pitch v Yaw
.v.v...Trong chng ny chng ta s tin hnh gii h phng trnh ng hc thit
lp chng trc nhm xc nh cc bin trong b thng s Denavit - Hartenberg khi
bit ma trn vect cui T6. Kt qu ca vic gii h phng trnh ng hc ng
vai tr ht sc quan trng trong vic iu khin robot. Thng thng, iu ta bit l cc
v tr v hng m ta mun robot phi dch chuyn ti v iu ta cn bit l mi quan
h gia cc h to trung gian phi hp to ra chuyn ng ca robot, hay ni
cch khc chnh l gi tr ca cc bin khp ng vi mi to v hng ca khu
chp hnh cui hoc cng c gn ln khu chp hnh cui, mun vy ta phi gii h
phng trnh ng hc ca robot. Vic nhn c li gii ca bi ton ng hc ngc
l vn kh m ta s nghin cu trong chng ny. Nhim v ca bi ton l xc
nh tp nghim (1, 2, ...,6,di*) khi bit hnh th ca robot thng qua vect cui
T6 (khi nim hnh th ca robot bao gm khi nim v v tr v hng ca khu
chp hnh cui : Configuration = Position + Orientation).
Cng cn lu rng, a s cc robot c b Teach pendant l thit b dy hc,
c nhim v iu khin robot n cc v tr mong mun trong ng trnh u tin (iu
khin im : Point to point ), cc chuyn ng ny s c ghi li vo b nh trung
tm (CPU) ca robot hoc my tnh iu khin robot, sau robot c th thc hin li
ng cc ng tc c hc. Trong qu trnh hot ng ca robot, nu dng qu o
ng i khng quan trng th khng cn li gii ca bi ton ng hc ngc.
4.1. Cc iu kin ca bi ton ng hc ngc :
TS. Phm ng Phc
43
Robot cng nghip
Vic gii bi ton ng hc ngc ca robot cn tho mn cc iu kin sau :

4.1.1. iu kin tn ti nghim :
iu kin ny nhm khng nh : C t nht mt tp nghim (1,2, ...,6,di*)
sao cho robot c hnh th cho trc.
(Hnh th l khi nim m t tng minh ca vect cui T6 c v v tr v
hng).
4.1.2. iu kin duy nht ca tp nghim :
Trong khi xc nh cc tp nghim cn phn bit r hai loi nghim :
+ Nghim ton (Mathematical Solution) : Cc nghim ny tho mn cc
phng trnh cho trc ca T6.
+ Nghim vt l (Physical Solution) : l cc tp con ca nghim ton, ph
thuc vo cc gii hn vt l (gii hn v gc quay, kch thc ...) nhm xc nh tp
nghim duy nht.
Vic gii h phng trnh ng hc c th c tin hnh theo hai phng php
c bn sau :
+ Phng php gii tch (Analytical Method) : tm ra cc cng thc hay cc
phng trnh ton gii tch biu th quan h gia cc gi tr ca khng gian bin trc
v cc thng s khc ca b thng s DH.
+ Phng php s (Numerical Method) : Tm ra cc gi tr ca tp nghim
bng kt qu ca mt qu trnh lp.
4.2. Li gii ca php bin i Euler :
Trong chng 3 ta nghin cu v php bin i Euler m t hng ca
khu chp hnh cui :
Euler (,,) = Rot(z, ) Rot(y, ) Rot(z, )
Tp nghim mun tm l cc gc , , khi bit ma trn bin i ng
nht T6 (cn gi l ma trn vect cui), Nu ta c cc gi tr s ca cc phn t trong
ma trn T6 th c th xc nh c cc gc Euler , , thch hp. Nh vy ta c :
(4-1)
Euler (,,) = T6
V tri ca phng trnh (4-1) c biu din bng cng thc (3-4) , nn ta
c :
cosCoscos - sinsin
sinCoscos + cossin
-sin cos
0
-cosCossin - sincos
-sinCossin + coscos
sin sin
0
nx
ny
nz
0
cossin
sinsin
cos
0
Ox
Oy
Oz
0
ax
ay
az
0
0
0
0
1
px
py
pz
1
(4-2)
Ln lt cho cn bng cc phn t tng ng ca hai ma trn trong phng

trnh (4-2) ta c cc phng trnh sau :
TS. Phm ng Phc
44
Robot cng nghip
nx = cosCoscos - sinsin
ny = sinCoscos + cossin
nz = -sin cos
Ox = -cosCossin - sincos
Oy = -sinCossin + coscos
Oz = sin sin
ax = cossin
ay = sinsin
az = cos
(4.3)
(4-4)
(4-5)
(4-6)
(4-7)
(4-8)
(4-9)
(4-10)
(4-11)
Ta th gii h phng trnh ny tm , , nh sau :

T (4-11) ta c
= cos-1(az)
T (4-9) ta c
= cos-1(ax / sin)
T (4-5) v (4-12) ta c
= cos-1(-nz / sin)
Trong ta dng k hiu cos-1 thay cho hm arccos.
(4-12)
(4-13)
(4-14)
Nhng cc kt qu gii trn cha dng c v cc l do di y :

+ Hm arccos khng ch biu hin cho mt gc cha xc nh m v chnh
xc n li ph thuc vo chnh gc , ngha l :
cos = cos(-) : cha c xc nh duy nht.
dcos
xc nh khng chnh xc.
0,180 = 0 :
d
+ Trong li gii i vi v mt ln na chng ta li dng hm arccos v
chia cho sin, iu ny dn ti s mt chnh xc khi c gi tr ln cn 0.
+ Cc phng trnh (4-13) v (4-14) khng xc nh khi = 0 hoc = 1800.
Do vy chng ta cn phi cn thn hn
khi chn li gii. xc nh cc gc khi gii
bi ton ngc ca robot ta phi dng hm
arctg2 (y,x) (hm arctang hai bin). Hm arctg2
nhm mc ch xc nh c gc thc - duy
nht khi xt du ca hai bin y v x. Hm s tr
v gi tr gc trong khong - < .
X- Y+
TS. Phm ng Phc
X+ Y+
V d :
arctg2(-1/-1)= -1350,
trong khi
arctg2(1/1) = 450
Hm ny xc nh ngay c khi x hoc y
bng 0 v cho kt qu ng.
(Trong mt s ngn ng lp trnh nh
Matlab, turbo C++, Maple hm arctg2(y,x)
c sn trong th vin)
X- Y-
X+ Y-
Hnh 4.1 : Hm arctg2(y,x)
45
Robot cng nghip
c th nhn c nhng kt qu chnh xc ca bi ton Euler, ta thc hin

th thut ton hc sau : Nhn T6 vi ma trn quay nghch o Rot(z, )-1,ta c:
Rot(z, )-1 T6 = Rot(y, ) Rot(z, )
(4-15)
V tri ca phng trnh (4-15) l mt hm s ca ma trn T v gc quay . Ta

thc hin php nhn ma trn v phi ca (4-15), tm ra cc phn t ca ma trn c
gi tr bng 0 hoc bng hng s, cho cc phn t ny cn bng vi nhng phn t
tng ng ca ma trn v tri, c th t (4-15) ta c :
cos
-sin
0
0
sin
cos
0
0
0
0
1
0
0
0
0
1
nx
ny
nz
0
Ox
Oy
Oz
0
ax
ay
az
0
px
py
pz
1
Coscos
sin
-sin cos
0
-Cos sin
cos
sin sin
0
sin
0
Cos
0
0
0
0
1
(4-16)
Tch hai ma trn v tri ca phng trnh (4-16) l mt ma trn m c th
c vit gn li bng cc k hiu sau :
f11(n)
f12(n)
f13(n)
0
f11(O)
f12(O)
f13(O)
0
f11(a)
f12(a)
f13(a)
0
f11(p)
f12(p)
f13(p)
1
f11 = cos x + sin y

f12 = -sin x + cos y
f13 = z
Trong :
(4-17)
(4-18)
(4-19)
v x, y, z l cc phn t ca vect xc nh bi cc d kin f11, f12, f13, v d :

f11(n) = cos nx + sin ny
f12(O) = -sin Ox + cos Oy
f13(a) = az
Nh vy phng trnh (4-16) c th c vit thnh :
f11(n)
f12(n)
f13(n)
0
f11(O)
f12(O)
f13(O)
0
f11(a)
f12(a)
f13(a)
0
Coscos -Cos sin

0
=
sin
cos
0
-sin cos
sin sin
0
0
0
1
sin
0
Cos
0
0
0
0
1
(4-20)
Trong f11, f12, f13 c nh ngha (4-17), (4-18) v (4-19).

Khi tnh ton v tri, ta ch rng px, py, pz bng 0 v php bin i Euler ch
ton php quay khng cha mt php bin i tnh tin no, nn f11(p) = f12(p) = f13(p)
= 0. T phng trnh (4-20), cho cn bng phn t hng 2 ct 3 ta c :
TS. Phm ng Phc
46
Robot cng nghip
f12(a) = -sin ax + cos ay = 0.
(4-21)
Cng hai v vi sin ax v chia cho cos ax ta c :

sin a y
tg =
=
cos a x
Gc c th xc nh bng hm arctg hai bin :
= arctg2(ay, ax).
Ta cng c th gii phng trnh (4-21) bng cch cng hai v vi -cos ay ri
chia hai v cho -cos ax, trit tiu -ax v tri v cos v phi, ta c :
sin -a y
=
cos -a x
Trong trng hp ny gc tm c l :
tg =
= arctg2(-ay, -ax).
Nh vy phng trnh (4-21) c mt cp nghim cch nhau 1800 (y l
nghim ton) v ta c th vit :
= arctg2(ay, ax) v = + 1800.
(Hiu theo cch vit khi lp trnh trn my tnh).
Nu c ax v ay u bng 0 th gc khng xc nh c. iu xy ra khi
bn tay ch thng ln trn hoc xung di v c hai gc v tng ng vi cng
mt php quay. iu ny c coi l mt php suy bin (degeneracy), trong trng
hp ny ta cho = 0.
Vi gi tr ca nhn c, cc phn t ma trn v bn tri ca phng trnh
(4-20) s c xc nh. Tip tc so snh cc phn t ca hai ma trn ta c :
V
Vy
f11(a) = cos ax + sin ay = sin.

f13(a) = az = cos.
= arctg2(cos ax + sin ay, az)
Khi c hai hm sin v cos u c xc nh nh trng hp trn, th gc

thng c xc nh duy nht v khng xy ra trng hp suy bin nh gc trc
y. Cng t phng trnh (4-20) ta c :
f12(n) = -sin nx + cos ny = sin

f12(O) = -sin Ox + cos Oy = cos
TS. Phm ng Phc
47
Robot cng nghip
Vy :
= arctg2(-sin nx + cos ny, -sin Ox + cos Oy)
Tm li, nu cho trc mt php bin i ng nht di dng cc php quay,

ta c th xc nh cc gc Euler tng ng l :
= arctg2(ay, ax) v = + 1800
= arctg2(cos ax + sin ay, az)
= arctg2(-sin nx + cos ny, -sin Ox + cos Oy)
4.3. Li gii ca php bin i Roll, Pitch v Yaw :
Php bin i Roll, Pitch v Yaw c nh ngha :
RPY(,,)= Rot(z,)Rot(y,)Rot(x, )
Vic gii phng trnh : T6 = RPY(,,) s xc nh c cc gc , v .
Cch gii c tin hnh tng t nh khi thc hin li gii cho php quay
Euler. Nhn T6 vi ma trn nghch o Rot(z, )-1, ta c :
Rot(z, )-1T6 = Rot(y,)Rot(x, )

Hay l :
f11(n)
f12(n)
f13(n)
0
f11(O)
f12(O)
f13(O)
0
f11(a)
f12(a)
f13(a)
0
Trong :
cos
0
0
0 =
-sin
0
0
1
sin sin
cos
cos sin
0
sin cos
-sin
coscos
0
0
0
0
1
f11 = cos x + sin y

f12 = -sin x + cos y
f13 = z
Cn bng phn t hng 2 ct 1 : f12(n) = 0, ta c :
-sin x + cos y = 0
Phng trnh ny cho ta hai nghim nh bit :
= arctg2(nx, ny)
= + 1800
Tip tc cn bng cc phn t tng ng ca hai ma trn ta c :

-sin = nz
cos = cos nx + sin ny
TS. Phm ng Phc
(4-22)
48
Robot cng nghip
do vy :
= arctg2(-nz, cos nx + sin ny)
Ngoi ra ta cn c :
-sin = -sin ax + cos ay

cos = -sin Ox + cos Oy
= arctg2(sin ax - cos ay, -sin Ox + cos Oy)
Nn :
Nh vy ta xc nh c cc gc quay Roll, Pitch v Yaw theo cc phn t

ca ma trn T6.
4.4. Gii bi ton ng hc ngc ca robot Stanford :
H phng trnh ng hc ca robot Stanford c thit lp trong chng
III, Ta c :
T6 = A1A2A3A4A5A6
(4-23)
Lin tc nhn (4-23) vi cc ma trn A nghch o, ta c :
A 51
A 11 T6
A 21 A 11 T6
A 31 A 21 A 11 T6
A 41 A 31 A 21 A 11 T6
A 41 A 31 A 21 A 11 T6
= 1T6
= 2T6
= 3T6
= 4T6
= 5T6
(4-24)
(4-25)
(4-26)
(4-27)
(4-28)
Cc phn t v tri ca cc phng trnh ny l hm s ca cc phn t T6 v

cc bin khp ca (n-1) khp u tin. Trong khi cc phn t ca ma trn v bn
phi hoc bng 0, bng hng s hoc l hm s ca cc bin khp th n n khp th
6. T mi phng trnh ma trn, cho cn bng cc phn t tng ng chng ta nhn
c 12 phng trnh. Mi phng trnh c cc phn t ca 4 vect n, O, a, p.
T phng trnh (4-24), ta c :
1
1
A T6 =
TS. Phm ng Phc
C1
0
-S1
0
S1
0
C1
0
f11(n)
f12(n)
f13(n)
0
0
-1
0
0
0
0
0
1
nx
ny
nz
0
f11(O)
f12(O)
f13(O)
0
f11(a)
f12(a)
f13(a)
0
Ox
Oy
Oz
0
ax
ay
az
0
f11(p)
f12(p)
f13(p)
1
px
py
pz
1
49
Robot cng nghip
f11 = C1 x + S1 y
f12 = - z
f13 = -S1 x + C1 y
V bn phi ca (4-24) l :
Trong :
T6 =
C2(C4C5C6 - S4S6) - S2S5C6

S2(C4C5C6 - S4S6) + C2S5C6
S4C5C6 + C4S6
0
-C2(C4C5S6-S4C6)+S2S5S6
-S2(C4C5S6+S4C6)-C2S5S6
-S4C5S6+C4C6
0
C2C4S5 + S2C5
S2C4S5 - C2C5
S4S5
0
S2d3
-C2d3
d2
1
Cc phn t ca ma trn v phi u l hm s ca 2, d3, 4, 5, 6 ngoi tr

phn t hng 3 ct 4, l :
f13(p) = d2
hay :
-S1px + C1py = d2
gii phng trnh dng ny ta c th thay th bi cc hm lng gic sau
y :
px = r cos
py = r sin
Trong :
r = + p 2x + p 2y
= arctg2(py, px)
Th px v py vo phng trnh -S1px + C1py = d2 ta c :
sincos1 - cossin1 = d2 / r
Vi
0 < d2 / r 1
Hay l : sin( - 1) = d2 / r
T ta c :
Vi
0 < - 1 <
cos( - 1) = 1 (d 2 / r ) 2
Trong du tr ph hp vi hnh th vai tri ca robotv du cng ph hp
vi hnh th vai phi ca robot. Cui cng :
1 = arctg2(py, px) - arctg2(d2, 1 (d 2 / r ) 2 )
(4-29)
Nu tnh c 1 th v tri ca phng trnh (4-24) c xc nh.

Cho cn bng cc phn t hng 1 ct 4 v hng 2 ct 4, ta c :
S2d3 = C1px + S1py

-C2d3 = -pz
d3 l dch chuyn di ca khp tnh tin, d3 > 0, nn ta c :
2 = arctg2(C1px + S1py, pz )
TS. Phm ng Phc
(4-30)
50
Robot cng nghip
T phng trnh (4-25) : A 21 A 11 T6 = A 21 1T6 = 2T6, ta c :
f21(n)
f21(O)
f21(a)
f22(n)
f22(O)
f22(a)
f23(n)
0
f23(O)
0
f23(a)
0
f23(p)
1
Trong :
C4C5C6-S4S6
= S4C5C + C4S6
-S5C6
-C4C5S6 - S4C6
-S4C5S6 + C4C6
S5S6
C4S5
S4S5
C5
0
0
d3
0
1
(4-31)
f21 = C2(C1 x + S1 y) - S2 z
f22 = -S1 x + C1 y
f23 = S2(C1 x + S1 y) + C2 z
T cn bng phn t hng 3 ct 4 ta c :
d3 = S2(C1 px + S1 py) + C2 pz
(4-32)
- T phng trnh (4-27) ta c : A 41 A 31 2T6 = 4T6

Thc hin php nhn cc ma trn v tri, v biu din dng rt gn nh sau
:
f41(n)
f42(n)
f43(n)
0
f41(O)
f42(O)
f43(O)
0
Trong :
f41(a)
f42(a)
f43(a)
0
0
0
0
1
C5C6
S5C6
S6
0
-C5S6
-S5S6
C6
0
S5
C5
0
0
0
0
0
1
f41 = C4[C2(C1 x + S1 y) - S2 z] + S4(-S1 x + C1 y)

f42 = -S2(-S1 x + C1 y) - C2 z
f43 = -S4[C2(C1 x + S1 y) + S2 z] + C4(-S1 x + C1 y)
Cn bng phn t hng 3, ct 3 ta c mt hm s ca 4, l : f43(a) = 0.

Hay :
-S4[C2(C1 ax + S1 ay) + S2 az] + C4(-S1 ax + C1 ay) = 0

y l phng trnh lng gic c dng : - sin ax + cos ay = 0. Nh gii
trong cc phn trc y, phng trnh ny c hai nghim :
4 = arctg2(-S1 ax + C1 ay, C2(C1 ax + S1 ay) + S2 az)

4 = 4 + 1800
(4-33)
Nu cc yu t t s v mu s ca (4-33) tin ti 0 th robot ri vo tnh trng

suy bin nh trung hp ni mc 4.2.
Ta cng c th tm gi tr ca gc quay 4 bng cch cn bng cc phn t hng
1 ct 3 v hng 2 ct 3 ca phng trnh ma trn (4-31) , ta c :
C4S5 = C2(C1 ax + S1 ay) - S2 az
TS. Phm ng Phc
51
Robot cng nghip
S4S5
= -S1 ax + C1 ay
Vi 5 > 0 ta c 4 = arctg(-S1 ax + C1 ay, C2(C1 ax + S1 ay) + S2 az)

Vi 5 < 0 ta c 4 = 4 + 1800
ng nh kt qu tm (4-33).
Khi S5 = 0, 5 = 0. Robot c suy bin do c hai trc ca khp 4 v 6 nm thng
hng (z3 z5). v tr ny ch c tng 4+6 l c ngha.
Khi 5 = 0, ta c th t do chn mt gi tr ca 4. Thng gi tr hin hnh
c s dng.
T v phi ca phng trnh A 41 A 31 2T6 = 4T6 = A5A6 ta c th c cc
phng trnh ca S5, C5, S6 v C6 bng cch cn bng cc phn t thch hp. Chng hn
khi cn bng cc phn t ca ma trn hng 1 ct 3 v hng 2 ct 3 ta c :
S5 = C4 [C2(C1 ax + S1 ay) - S2 az] + S4(-S1 ax + C1 ay)

C5 = S2 (C1 ax + S1 ay) + C2 az
T suy ra :
5 = arctg2(C4 [C2(C1 ax + S1 ay) - S2 az] + S4(-S1 ax + C1 ay) ,

(4-34)
S2 (C1 ax + S1 ay) + C2 az )
Cc phng trnh c lin quan n 6 nm ct 1 ca phng trnh ma trn,

l cc thnh phn ca vect n ca T6. Vect ny thng khng c ngha trong tnh
ton, v n lun c
c xc nh bng tch vect ca hai vect O v a nh ni
r th
r
r
trc y ( n = O x a ). Do ta phi tm cch khc xc nh 6.
Thc hin php nhn cc ma trn v tri ca phng trnh (4-28) : A 51 4T6=
5
T6 = A6, biu din dng k hiu ta c :
f51(n)
f52(n)
f53(n)
0
f51(O)
f52(O)
f53(O)
0
0
0
1
0
0
0
0
1
C6
S6
0
0
-S6
C6
0
0
0
0
1
0
0
0
0
1
(4-35)
Trong :
f51 = C5{ C4 [C2(C1 x + S1 y) - S2 z] + S4(-S1 x + C1 y)} + S5[-S2 (C1 x + S1 y) - C2 z]

f52 = -S4 [C2(C1 x + S1 y) - S2 z] + C4[-S1 x + C1 y]
f53 = S5{ C4 [C2(C1 x + S1 y) - S2 z] + S4(-S1 x + C1 y)} + C5[S2 (C1 x + S1 y) - C2 z]
Cho cn bng cc phn t hng 1 ct 2 v hng 2 ct 2 ta nhn c cc gi
tr ca S6 v C6 :
S6 = -C5{C4[C2(C1Ox+S1Oy)-S2Oz] +S4(-S1Ox+C1Oy)} + S5[S2 (C1Ox + S1Oy) + C2Oz]
C6 = -S4 [C2(C1Ox + S1Oy)- S2 Oz] + C4[-S1 Ox + C1 Oy]
T ta xc nh c :
TS. Phm ng Phc
6 = arctg2(S6, C6)
(4-36)
52
Robot cng nghip
Cc biu thc (4-29), (4-30), (4-32), (4-33), (4-34) v (4-36) xc nh tp

nghim khi gii bi ton ngc ca robot Stanford.
4.5. Gii bi ton ng hc ngc ca robot ELBOW :
tip tc lm quen vi vic gii h phng trnh ng hc, chng ta nghin
cu php gii bi ton ng hc ngc ca robot Elbow. H phng trnh ng hc
thun ca robot Elbow dc xc nh trong chng III.
Trc ht ta kho st phng trnh :
A 11 T6 = 1T6 = A2A3A4A5A6
Tng t nh lm, ta xc nh cc phn t ma trn ca hai v nh sau :
f11(n)
f12(n)
f13(n)
0
f11(O)
f12(O)
f13(O)
0
f11(a)
f12(a)
f13(a)
0
f11(p)
f12(p)
f13(p)
1
C234C5C6 - S234S6
-C234C5S6 - S234C6
C234S5
C234a4+C23a3+C2a
S234C5C6 + C234S6
-S5C6
0
-S234C5S6 + C234C6
S5S6
0
S234S5
C5
0
S234a4+S23a3+S2a2
0
1
(4-37)
f11 = C1 x + S1 y
f12 = z
f13 = S1 x + C1 y
C234 = cos(2+3+4)
Ta k hiu :
S234 = sin(2+3+4)
Trong :
Cho cn bng phn t hng 3 ct 4, ta c :

S1 px + C1 py = 0
Suy ra :
1 = arctg2(py , px)
v
1 = 1 + 1800
(4-38)
Trong trng hp robot Elbow, ba khp k tip u song song v khng c kt

qu no nhn c t php nhn vi nhng ma trn nghch o A-1i . Cho n khu th
4 th php nhn vi ma trn nghch o mi c ngha.
A-14A-13A-121T6 = 4T6 = A5A6

Khi xc nh cc phn t ma trn ca hai v ta c :
TS. Phm ng Phc
53
Robot cng nghip
f41(n)
f42(n)
f43(n)
0
f41(O)
f42(O)
f43(O)
0
Trong :
f41(a)
f42(a)
f43(a)
0
f41(p)-C34a2-C4a3-a4
C5C6
0
= S5C6
f43(p)+S34a2+S4a3
S6
1
0
-C5S6
-S5S6
C6
0
S5 0
-C5 0
0
0
0
1
(4-39)
f41 = C234(C1 x + S1 y) + S234 z

f42 = -S1 x + C1 y
f43 = -S234(C1 x + S1 y) + C234 z
Cn bng phn t hng 3 ct 3 ta c mt phng trnh cho 234 :
-S234(C1 ax + S1 ay) + C234 az = 0

Suy ra :
v
234 = arctg2(az , C1 ax + S1 ay)

234 = 234 + 1800
(4-40)
By gi ta tr li phng trnh (4-37). Cn bng cc phn t ma trn hng 1

ct 4 v hng 2 ct 4, ta c :
C1 px + S1 py = C234a4+C23a3+C2a2
(a)
pz
= S234a4+S23a3+S2a2
(b)
Ta gi :
px
= C1 px + S1 py - C234a4
(c)
py
= pz - S234a4
(d)
em (a) + (c) v (b) + (d) ta c ;
px = C23 a3 + C2a2
(e)
(g)
py = S23 a3 + S2a2
Bnh phng hai v v cng hai phng trnh (e) v (g), ta c :
p2x = (C23 a3 + C2a2)2
p2y = (S23 a3 + S2a2)2
p2x + p2y = (S
23
+C
23
)a23 + (S
+C
2
2)a 2
+ 2 a2a3(C23C2 + S23S2 )
Ta c C23C2 + S23S2 = cos(2+3-2) = cos3 = C3. Nn suy ra :
C3 = (p2x + p2y - a23 - a22) / 2a2a3

Trong khi c th tm 3 t hm arccos, ta vn nn tm mt gi tr S3 v dng
hm arctg2 nh thng l :
Ta c :
S3 = (1 C 23 )
Cp nghim ng vi hai du +,- ph hp vi hnh th ca robot lc nng vai ln

v h vai xung :
TS. Phm ng Phc
54
Robot cng nghip
3 = arctg2(S3 , C3)
(4-41)
tm S2 v C2 ta gii h phng trnh (e),(g).

T (e)
T (g)
(C2C3 - S2S3)a3 + C2a2

(S2C3 - C2S3)a3 + S2a2
= px
= py
Khai trin v rt gn :
T (g)
Ta c :
(C3a3 + a2)C2 S3a3.S2 = px

S3a3 .C2 + (C3a3 + a2)S2 = py
=
C 3a 3 + a 2
S 3a 3
p ,x
c = ,
py
- S 3a 3
C 3a 3 + a 2
- S 3a 3
C 3a 3 + a 2
C 3a 3 + a 2
s =
S3a 3
C2 =
C (C 3 a 3 + a 2 )p' y S3 a 3 p' x
=
(C 3 a 3 + a 2 ) 2 + (S3 a 3 ) 2
S2 =
S (C 3 a 3 + a 2 )p' x +S 3 a 3 p' y
=
(C 3 a 3 + a 2 ) 2 + (S 3 a 3 ) 2
p ,x
p ,y
Do mu s dng v bng nhau, nn ta c :
2 = arctg2(S2, C2)
2 = arctg2((C3a3 + a2)py - S3a3px , (C3a3 + a2)px + S3a3py )
(4-42)
n y 4 c xc nh bi :
4 = 234 - 3 - 2
(4-43)
Cc phng trnh dng tnh 5 c thit lp t s cn bng cc phn t ma

trn hng 1 ct 3 v hng 2 ct 3 ca phng trnh 4T6 (4-39) :
S5 = C234(C1ax + S1ay) + S234az

C5 = S1ax - C1ay
Suy ra :
5 = arctg2(C234(C1ax + S1ay) + S234az , S1ax - C1ay)
tm 6 , ta tip tc nhn A-15 vi 4T6 , ta c :
Vit tch ma trn v tri dng k hiu :
TS. Phm ng Phc
A-15 . 4T6 = A6.
(4-44)
55
Robot cng nghip
f51(n)
f52(n)
0
0
Trong :
f51(O)
f52(O)
0
0
0
0
1
0
0
0
0
1
C6
S6
0
0
-S6
C6
0
0
0
0
1
0
0
0
0
1
f51 = C5[C234(C1 x + S1 y) + S234 z] - S5(S1 x + C1 y)

f52 = -S234(C1 x + S1 y) + C234 z
Cho cn bng cc phn t ma trn tng ng, ta c :
S6 = -C5[C234(C1 Ox + S1 Oy) + S234 Oz] - S5(S1 Ox + C1 Oy)

C6 = -S234(C1 Ox + S1 Oy) + C234 Oz
Vy :
6 = arctg2(S6 , C6)
(4-45)
Cc phng trnh (4-38), (4-41), (4-42), (4-43), (4-44) v (4-45) xc nh tp

nghim khi gii bi ton ng hc ngc ca robot Elbow.
4.6. Kt lun :
Phng php gii bi ton ng hc ngc a ra trong chng ny s dng
cc hm lng gic t nhin. Cc gc thng c xc nh thng qua hm arctang hai
bin. Phng php ny c a ra bi Pieper v p dng tt vi nhng robot n gin,
Thng ta nhn c nghim dng cng thc n gin.
Khi gii bi ton ng hc ngc c th xy ra hin tng suy bin . Khi c
nhiu hn mt tp nghim i vi bi ton ngc xc nh v tr v hng ca bn
tay, th cnh tay c gi l suy bin.
Dng phng php Pieper, cc nghim nhn c thng c 4 dng cng thc,
mi dng c mt ngha ng hc ring. Dng th nht :
- sin ax + cos ay = 0
Dng ny cho ta mt cp nghim cch nhau 1800, n m t hai hnh th tng
ng ca robot. Nu cc t s v mu s u bng khng, robot b suy bin, lc
robot mt i mt bc t do.
Dng th hai :
-S1px + C1py = d2
Dng ny cng cho ta cp nghim sai khc nhau 1800, mt ln na li tn ti

kh nng suy bin khi t s v mu s bng 0. Robot trng hp ny thng c mt
khp tnh tin v di tnh tin c coi l > 0.
Dng th ba :
v dng th t :
TS. Phm ng Phc
C1px + S1py = S2d3

- C2d3 = - pz
56
Robot cng nghip
y2
x2
O2
Cc phng thnh ny thng c nghim duy nht.

2
y1 c hai hayznhiu
Ngoi cc dng ph bin, khi robot
khp song song (V d
2
robot Elbow), cc gc ca tng khp phi c xc nh bng cch gii ng thi
a1 cc gc khp. x1 a
y0 h tng
nhiu khp trong mi quan
2
1
1 vi hnh
Tm ra cc nghim ph hp
z1 th ca robot (v tr v hng) l mt trong
nhng vn kh khn nht.
Hux nh cha c thut ton chung no m nh c th
O0
0
tm ra c tp nghim
z0 cho mi robot. Tuy nhin phng php a ra trong chng
ny thit lp c cc nghim s dng tng minh, trc tip.
Hnh 4.3 : H to v cc thng s ca robot 2 khu phng
Khi lp trnh iu khin robot ta phi da vo cc gii hn vt l chn cc
nghim vt l, ngha l loi tr cc nghim ton khng thch hp xc nh mt cu
hnh duy nht ca robot.
Bi tp chng IV :
Bi 1 : Cho mt v tr mong mun ca khu chp hnh cui ca robot c 3 khu phng
nh hnh 4.2; Dng phng php hnh hc xc nh c bao nhiu li gii ca bi
ton ng hc ngc ? Nu hng ca khu chp hnh cui cng c xc nh, th c
bao nhiu li gii ?
Bi 2 : Dng phng php hnh hc gii bi ton ng hc ngc (xc nh cc gc

Hnh
4.2 nh
: Robot
1, 2 ) ca robot c hai khu
phng
hnhcu
4.3hnh
: RRR
TS. Phm ng Phc
57
Robot cng nghip
Bi 3 : Thit lp h phng trnh ng hc ca robot SCARA (hnh 4.4) v gii bi

ton ng hc ngc ca n.
z0
1
O0
a2
z1
x
x
a1
d3
z2
O3
O4
4
z3 , z4
Hnh 4.4 : Robot SCARA
TS. Phm ng Phc
x
d4
x
58
Robot cng nghip
Chng V
ngn ng lp trnh robot
(Robot Programming Languages )
5.1. Gii thiu chung v lp trnh iu khin robot :

Lp trnh iu khin robot th hin mi quan h gia ngi iu khin v
robot cng nghip. Tnh phc tp ca vic lp trnh cng tng khi cc ng dng cng
nghip i hi s dng ng thi nhiu robot vi cc my t ng kh lp trnh khc
to nn h thng sn xut t ng linh hot.
Robot khc vi cc my t ng c nh tnh linh hot, ngha l c th lp
trnh c (Programmable : kh lp trnh). Khng nhng ch c cc chuyn ng ca
robot m ngay c vic s dng cc cm bin cng nh nhng thng tin quan h vi
my t ng khc trong phn xng cng c th lp trnh. Robot c th d dng thch
nghi vi s thay i ca nhim v sn xut bng cch thay i chng trnh iu
khin n.
Khi xem xt vn lp trnh cho robot, chng ta nn nh rng robot l mt
thnh phn ca mt qu trnh c t ng ho. Thut ng, workcell c dng
m t mt tp hp cc thit b m n bao gm mt hoc nhiu robot, h thng bng
chuyn, cc c cu cp phi v g. mc cao hn, Workcell c th c lin kt
trong mng li cc phn xng v th my tnh iu khin trung tm c th iu
khin ton b cc hot ng ca phn xng. V vy, vic lp trnh iu khin robot
trong thc t sn xut cn phi c xem xt trong mi quan h rng hn.
bc u lm quen vi vic lp trnh iu khin robot, chng ny cng
gii thiu tm tt phng php lp trnh iu khin robot TERGAN-45 thng qua
ngn ng ASPECT ca phn mm Procomm Plus for Window
5.2. Cc mc lp trnh iu khin robot :
Ngi s dng c th c nhiu kiu giao din lp trnh iu khin robot.
Trc s pht trin nhanh chng ca cc loi my vi tnh dng trong cng nghip v
cc ngn ng lp trnh ngy cng c nhiu tin ch cao, vic lp trnh iu khin
robot ngy cng d dng v thun tin hn.
TS. Phm ng Phc
Robot cng nghip
59
5.2.1. Lp trnh kiu Dy - Hc :

Cc robot th h u tin c lp trnh bng mt phng php m chng
ta gi l : dy bng ch dn (Teach by showing), robot c iu khin di chuyn
n cc im mong mun v cc v tr c ghi li trong b nh ca my tnh,
sau cc d liu s c c tun t v robot thc hin li cc ng tc c
hc. dy robot, ngi s dng c th hng dn robot bng tay hoc thng qua
mt thit b dy hc gi l Teach pendant. Thit b dy hc gm mt hp nh cm
tay (teaching box) c cc nt bm v card iu khin m n cho php iu khin cc
khp ca robot t c cc gi tr mong mun.
5.2.2. Dng cc ngn ng lp trnh :
Cng vi qu trnh pht trin ngy cng r hn v mnh hn ca my tnh,,
chng trnh iu khin robot c pht trin theo hng vit cc chng trnh bng
cc ngn ng lp trnh ca my tnh. Thng cc ngn ng lp trnh ny c nhng
c im m chng ta c th ng dng vit cc phn mm hay chng trnh iu
khin robot, v chng c gi l ngn ng lp trnh robot. Hu ht cc h thng
iu khin dng ngn ng lp trnh robot vn duy tr kiu giao din Teach pendant
(dy- hc).
Ngn ng lp trnh robot c nhiu dng khc nhau. Chng ta phn chng
thnh ba loi nh sau :
a) Ngn ng robot chuyn dng : nhng ngn ng lp trnh robot ny c
xy dng bng cch to ra mt ngn ng mi hon ton. C php (Syntax) v ng
ngha (Semantics) ca cc ngn ng ny cn phi rt n gin v ngi lp trnh cho
cc ng dng cng nghip khng phi l mt chuyn gia v lp trnh. V d nh ngn
ng VAL (VAL 2) c dng iu khin cc robot cng nghip ca hng
Unimation (Hoa k); hoc mt ngn ng robot chuyn dng khc gi l AL c xy
dng i hc Stanford (hoa k)...
b) To ra cc th vin robot cho mt ngn ng lp trnh cp cao c sn :
Nhng ngn ng lp trnh robot ny c xy dng bng cch da trn cc ngn ng
lp trnh cp cao thng dng (v d nh Pascal) v thm vo mt th vin cc th tc
v hm c bit dng cho robot. Khi vit cc chng trnh Pascal iu khin robot,
ngi s dng s gi cc hm hoc th tc nh ngha trc trong th vin x
l cc ni dung c lin quan n vic tnh ton hoc iu khin robot.
V d PASRO (Pascal for Robot) l mt th vin dng cho lp trnh robot,
cung cp nhiu th tc v hm c bit tnh ton v iu khin robot dng trong
mi trng ngn ng Turbo Pascal, hoc PASRO/C l pht trin ca PASRO, nhng
c vit trn c s ca ngn ng Turbo C.
c) To ra cc th vin robot cho mt ngn ng hoc phn mm a dng
(Robot library for a new general - purpose language) : Nhng ngn ng lp trnh
robot ny c xy dng bng cch s dng cc ngn ng hoc phn mm dng
TS. Phm ng Phc
Robot cng nghip
60
chung cho nhiu mc ch nh l mt chng trnh c bn, sau cung cp thm

mt th vin cha cc th tc c bit dng cho robot. V d nh ngn ng lp trnh
robot AML ca hng IBM v RISE ca hng Silma, ngn ng Aspect ca phn mm
Procomm Plus ...
5.2.3. Ngn ng lp trnh theo nhim v (Task-level programming language)
Mc th ba ca phng php lp trnh robot l to ra cc ngn ng lp trnh
theo nhim v. Nhng ngn ng ny cho php ngi s dng ra cc lnh robot
thc hin mt cng vic mong mun mt cch trc tip m khng cn xc nh mt
cch chi tit cc hot ng ca robot nh cc ngn ng lp trnh thng thng. Mt
h thng lp trnh robot theo nhim v phi c kh nng th hin nhiu cng vic mt
cch t ng. Chng hn, nu mt ch th Grasp the bolt (cm ly bulong) c to
ra, h thng phi vch ra c mt qu o ca tay my m n trnh c s va chm
vi bt k chng ngi vt no chung quanh, chn c v tr tt nht cm ly
bulong mt cch t ng. Ngc li, trong ngn ng lp trnh robot thng thng tt
c nhng s la chn ny phi c thc hin bi ngi lp trnh. Trong thc t,
ngn ng lp trnh theo nhim v cha c dng trong sn xut, n cn l mt lnh
vc ang c nghin cu.
Sau y ta s nghin cu mt phn mm a dng dng truyn d liu v iu
khin thit b c th dng iu khin robot.
5.3. Gii thiu tm tt phn mm Procomm Plus For Windows :
Procomm Plus l phn mm dng truyn d liu v iu khin trc tip cc
thit b qua cng COM ca my tnh c nhn. Vi Procomm Plus ta c th s dng
my tnh nh mt Terminal hoc thc hin cc Scrip files vit bng ngn ng lp
trnh Aspect.
chy phn mm Procomm Plus ch Terminal ta c th dng mt s
cch sau :
a) S dng Desktop Windows : n p chut trn biu tng ca Procomm
Plus terminal Windows.
b) T mc Run... trong Start ca Windows, g lnh : pw3 , chn OK.
c) Vo Start ca Windows, chn Programs, chn Procomm Plus 3, chn
Data Terminal...
Menu chnh ca Procomm Plus c nhiu tin ch, rt thun tin khi iu khin
cc thit b giao din vi my tnh kiu RS 232.
Ca s chnh ca phn mm Procomm plus ch Terminal nh hnh 5.1.
TS. Phm ng Phc
61
Robot cng nghip
Thanh cng c
Menu chnh
Ca s nhp - xut d liu .
Meta keys
Thanh trng thi
Dng chn nhanh kiu giao din
Hnh 5.1 : Ca s chnh ca Procomm Plus for Windows, Version 3.0

Menu chnh : Cung cp cc tin ch cn thit trong qu trnh s dng, menu
chnh c cc menu ko xung (Pulldown) tng t nh nhiu phn mm thng dng
khc. Ni dung ca Menu chnh c th thay i c theo mc ch s dng.
Mt s ni dung ca menu chnh c th dng trong qu trnh iu khin robot
nh sau :
Menu Setup : Dng xc nh cu hnh ca Terminal Windows v ch
giao din gia my tnh vi thit b. Trong menu ny cn c th s dng mc con
Action Bars chn file cha ni dung ca thanh cng c v cho th hin trn mn
hnh.
Menu Data : Trong menu ny ta c th dng cc menu con sau :
+ Clear screen (Alt+C): Xo mn hnh nhp xut d liu;
+ Reset terminal (Alt+U): Xo mn hnh v b m (buffer) ca
Procomm.
Menu Scripts : Trong menu ny ta c th dng cc menu con sau :
+ Start scrips (Alt+.) : Thc hin mt Aspect scrips file, c tn c
th hin trn thanh cng c.
+ Run... (Alt+F5) : M hp hi thoi Run ASPECT file , chng ta
c th chn tn file, thc hin vic dch cc file ngun trc khi chy chng trnh.
+ Compile / Edit... (Alt+F3) : M hp hi thoi son tho v dch cc
file ngun.
TS. Phm ng Phc
Robot cng nghip
62
+ Start recorder... : bt u t ng to ra mt scrips file bng cc

ghi li tt c cc lnh th hin trn mn hnh (nhp t bn phm). Khi chn mc ny
s xut hin mc Stop recorder, dng khi mun kt thc vic ghi t ng scrips file.
C th chn mc ny trn thanh cng c bng cch n chut vo biu tng
.
Menu Tools : Trong menu ny ta c th dng cc menu con sau :
+ Action bar Edition : Dng son tho hay thay i ni dung thanh
cng c cho ph hp vi mc ch s dng.
+ Aspect Editor : M ca s son tho script files bng ngn ng
Aspect, chng ta c th to mi, xem hoc sa i ni dung ca mt file (dng Text).
+ Dialog Editor : M ca s son tho hp hi thoi, cho php ta to
ra cc hp hi thoi bng phng php trc quan (Visual).
Thanh cng c (Tool bar) : c nhiu Icon (biu tng) gip ngi s dng
c th thc hin nhanh mt cng vic bng cch bm chut trn biu tng tng
ng, thay v phi vo Menu chnh. Ni dung ca thanh cng c cng c th thay i
d dng ph hp vi mc ch s dng (mc Action bar Edition).
Ca s nhp - xut d liu : l phn mn hnh ngi s dng nhp vo
cc d liu, cc lnh iu khin v cc th hin thng bo tr v t cc thit b c
iu khin.
Meta Keys : Dng ci t sn cc ng dng thng hay thc hin. Ni
dung ca cc Meta Keys c th thay i c ph hp vi tng mc ch s dng.
Khi mun thc hin mt cng vic gn cho Meta Key ch cn n chut vo Meta
key . Mun son tho hay thay i nhim v ca Meta Keys ta thc hin nh sau :
Cch 1 : n phm ALT+M .
Cch 2 : Chn mc Meta Keys Editor t Tool menu .
Dng chn nhanh kiu giao din : Cho php ngi s dng chn nhanh
kiu thng s giao din gia thit b iu khin v my tnh nh : cng giao din, tc
truyn thng tin, kiu x l d liu ... bng cch n nt chut tri ln cc mc.
5.4. Ngn ng lp trnh ASPECT trong Procomm :
5.4.1. Gii thiu :
Mt ASPECT script file l mt file dng text c to ra cha cc lnh
c thc hin bi Procomm Plus.
Ging nh nhiu ngn ng lp trnh khc, ASPECT yu cu phi dch chng
trnh son tho. Mt script file cha dch, hay cn gi l file ngun, c ui l .was
(Windows Aspect Source); cn mt script file dch c ui l .wax (Windows
Aspect eXecutable). Khi mt script c dch, th cc d liu v cc cu lnh cha
trong file ngun s c chuyn sang m m Procomm c th c v x l mt cch
nhanh chng. Sau khi dch th file dch (.wax) c kch thc nh hn so vi file
ngun.
Tm li : mt script file phi c dch trc khi c th thc hin. Mt file
c dch khng th dch ngc tr li thnh file ngun.
TS. Phm ng Phc
Robot cng nghip
63
Chng ta c th to mi v son tho file ngun (.was) bng trnh ASPECT

Editor hay bt k mt trnh son tho dng text no khc, nhng phi t tn tp c
ui l . was.
to mi mt file ngun hoc thay i b sung ni dung ca mt file c,
t menu chnh ca Procomm, chn Scripts | Compile/Edit... hoc n chut vo biu
tng
trn thanh cng c. Hp hi thoi dng son tho v dch cc script
files nh hnh 5.3.
Mun to mt file mi ta chn nt lnh New; mun sa i ni dung mt file
c (tn file chn trc trong mc File name) ta chn nt lnh Edit; mun thot
khi ca s son tho ta chn nt lnh Exit.
Khi chn nt lnh New hoc Edit, trn mn hnh s xut hin ca s son
tho ta vit hoc s i chng trnh.
Sau khi son tho xong, mun ghi vo a ta chn File | Save hoc File |
Save as ... Ta cng c th chn biu tung Ghi v dch (Save and Compile) trn
thanh cng c ghi vo a ng thi dch thnh file .wax.
Hnh 5.3 : Ca s son tho v dch cc script file

chy mt Aspect script file c th thc hin bng nhiu cch :
+ Chn mc Script trn Menu chnh, tip theo chn mc Run... Lc
ny s xut hin hp hi thoi chn file mun thc hin.
+ n chut trn mc Script file ca thanh cng c, sau chn tn
file mun thc hin.
Nu mt file chy, tn vn cn trong mc Script file, mun chy li
trn thanh cng c.
th n chut vo biu tng
+ C th chy mt script file t ca s Compile/Edit ASPECT file
(Chn mc RUN) (hnh 5.3).
TS. Phm ng Phc
Robot cng nghip
64
5.4.2. Kiu d liu v khai bo bin trong ASPECT :

a) Kiu d liu : ASPECT cung cp cc kiu d liu nh sau :
integer (kiu nguyn) : C gi tr t -32768 n 32767.
float (kiu s thc) : C gi tr t 2.22507385072014e-308 n
1.797693134862315e+308.
long (kiu nguyn di) : C gi tr t -2147483648 n 2147483647.
String (kiu chui) : C th cha t 0 n 256 k t.
Tt c tn ca cc phn t trong ASPECT, nh tn t lnh, tn hm v th tc,
tn nhn (label) v bin ... c chiu di khng qu 30 k t.
b) Cc loi bin : Trong ASPECT c cc loi bin sau :
+ Bin h thng : Bin h thng l cc bin ch c (read-only) m
ASPECT v Procomm Plus c th n nh cc gi tr c bit.
V d : chng ta khng th thay i gi tr ca bin h thng $ROW m n
lun lun bng v tr dng hin ti ca con tr trn mn hnh, ta ch c th c gi tr
ca n bt k ni no trong chng trnh v x l khi cn thit. Bin h thng lun c
du $ u.
+ Bin do ngi dng nh ngha , c hai loi :
- Bin ton cc (Global variables) : Bin ton cc c th c nh ngha
bt k ni no trong chng trnh nhng phi bn ngoi cc khi Th tc v Hm.
Ph bin , cc bin ton cc thng c khai bo u chng trnh. Bin ton cc
c th c tham chiu n t bt c hm hay th tc no ca chng trnh, Nu mt
th tc hoc hm lm thay i gi tr ca mt bin ton cc th gi tr vn c
duy tr cho n khi no c mt lnh khc lm thay i gi tr ca n.
- Bin a phng (Local variables) : Khng ging nh bin ton cc, bin a
phng ch c tham kho n trong phm vi ca th tc v hm m n c nh
ngha. Gi tr ca n s b xo khi ra khi th tc v hm . Ta c th t tn cc
bin a phng ging nhau trong cc th tc v hm khc nhau ca chng trnh,
nhng iu khng c ngha l gi tr ca bin c ghi nh gia cc th tc hoc
hm khc nhau.
+ Tham bin (Parameter variables):
Bt c th tc no, ngoi tr chng trnh chnh (Proc main) u c th khai
bo (nh ngha) n 12 tham bin. Cc tham bin tng t nh cc bin a phng,
ngha l n ch c tham chiu n trong phm vi th tc hoc hm m n c
nh ngha, tuy nhin khc vi bin a phng, cc tham bin nhn cc gi tr ban
u mt cch t ng khi cc th tc hoc hm c gi, cc ga tr s dng c
cung cp bi cu lnh gi. Cc tham bin phi c khai bo u mi th tc hoc
hm, trc bt c lnh no hoc cc bin a phng. Mt tham bin c khai bo
TS. Phm ng Phc
Robot cng nghip
65
ging nh bin a phng. Th t m cc tham bin c nh ngha xc nh th t

m chng s c gi bi cc th tc hoc hm.
c) Khai bo (nh ngha) cc bin : Tt c cc loi bin dng trong chng
trnh phi c khai bo (nh ngha) trc. Nu cc bin c cng kiu d liu, ta c
th khai bo trn mt dng cch nhau bi du phy ( , ).
V d :
Integer sokhop, Tong, i = 1
Float Goc
Integer A[4][4]
Trong v d trn ta khai bo cc bin : sokhop, Tong, i l cc bin nguyn,
trong bin i c gn gi tr ban u l 1. Goc l bin thc. A l bin mng (array)
c kch thc 4x4 , cc phn t ca mng kiu nguyn.
Cch khai bo tham bin trong th tc v hm nh sau :
param (kiu d liu ) (tn) [, tn] . . .
V d : param Integer X, Y, Z
Chng trnh v d :
; Vi du ve khai bao bien.
Proc main
; Chng trnh chnh.
integer A,B,C
; Khai bo 3 bin nguyn.
integer Tong
; Tng ca 3 s (bin nguyn).
A=2, B=4, C=8
; Gn gi tr cho cc bin.
Tong = Sum(A,B,C)
; Gi hm Sum cng cc s.
Usermsg Tong = %d. Tong
; Cho hin tng ca cc s ln mn hnh
Endproc
; Ht chng trnh chnh.
Func Sum : Integer
Param integer X, Y, Z
integer Tong
Tong= X+Y+Z
return Tong
Endfunc
; nh ngha hm Sum tnh tng.

; Khai bo cc tham bin kiu nguyn.
; Khai bo bin Tong (bin a phng).
; Tng ca 3 s.
; Tr v gi tr ca tng ca 3 s.
; ht phn nh ngha hm
(Ghi ch : du ; dng ghi ch trong chng trnh, cc ni dung sau du

; khng c dch).
5.4.3. Cu trc ca chng trnh :
Cu trc chng trnh ca mt ASPECT script file gn ging nh mt file vit
bng ngn ng Pascal, ngha l c mt chng trnh chnh v cc th tc hoc hm
khc. Ch khc nhau c bn l chng trnh chnh c vit trc, chng trnh
chnh c th gi n cc hm hoc th tc c nh ngha sau .
Trong chng trnh chnh khng c khai bo cc tham bin. Khi thc hin
chng trnh, n s ln lt thc hin cc lnh t dng u tin n ht chng trnh.
TS. Phm ng Phc
Robot cng nghip
66
Khi kt thc mt hm hoc th tc c gi, n t ng tr v dng lnh tip theo.

Cu trc chung ca mt chng trnh nh sau :
; Dng u tin dng ghi ch v ni dung chng trnh, dng ny s th hin trong
; hp hi thoi Compile/Edit ngi s dng d nhn bit v ni dung ca chng
; trnh.
Proc main
; bt u chng trnh chnh
(Khai bo bin)
(cc cu lnh th hin ni dung chng trnh)
.....
Endproc
; ht chng trnh chnh.
Proc (tn th tc)
; Bt u mt th tc
(khai bo cc tham bin nu c)
(khai bo cc bin a phng)
(cc cu lnh th hin ni dung th tc)
.....
Endproc
; ht mt th tc
Func (tn hm)
; Bt u mt hm
(khai bo cc tham bin nu c)
(khai bo cc bin a phng)
(cc cu lnh th hin ni dung ca hm)
.....
return (bin)
; tr gi tr ca bin v th tc gi
Endproc
; kt thc hm
5.4.4. Mt s php tnh dng trong ASPECT :
ASPECT s dng nhiu php tnh s hc v logic khc nhau, di y gii
thiu mt s php tnh hay dng :
+, -, *, /
Php ton cng, tr, nhn, chia.
>, <, >=, <= Ln hn, nh hn, ln hn hoc bng, nh hn hoc bng.
!=
Khc vi
!
NOT
&&
AND
||
OR
++, -Tng hoc gim mt n v.
?:
Thc hin mt iu kin
.v.v...
V d 1 : Cho A=2, B=4
A+B-- = 6 : A c cng vi B trc, v ri B gim i 1 (B=3).
A+ --B = 5 : Trc tin B gim i 1, sau cng A vi B.
V d 2 :
TS. Phm ng Phc
Robot cng nghip
67
Proc main
integer A,B,C,D
integer Tong
A=2, B=4
C=A+B
Tong = A+ --B
D=(tong < C) ? tong : C
; nu Tong < C th D=Tong, nu sai D=C
Usermsg " D = %d , C = %d" D,C
Endproc
Kt qu
D = 5 v C = 6.
5.4.5. Mt s t lnh trong ASPECT hay dng khi iu khin robot:

Ngn ng ASPECT c hn 600 t lnh, dng vi nhiu mc ch khc nhau.
Phn ny ch gii thiu s lt mt s lnh hay dng khi lp trnh iu khin
robot.
Ngi c c th s dng mc Help trn menu ca s son tho bit thm
chi tit.
* Cc lnh cn bn :
call :
Gi mt th tc hoc hm t chng trnh chnh hoc t mt th tc
khc.
C php :
Khi gi mt hm :
call <tn> [WITH <danh sch tham bin>] [INTO <bin>]
Khi gi mt th tc :
call <tn> [WITH <danh sch tham bin>]
tn
: tn th tc hoc hm c gi.
Danh sch tham bin : Tn cc tham bin trong th tc hoc hm.
INTO <bin> : Ch dng khi gi mt hm, bin s cha gi tr tr li
ca hm.
case/endcase :
Cu lnh la chn, dng vi t lnh Switch.
C php :
switch <bin> (string | integer | long)
case <gi tr so snh> (string | integer | long)
...
[exitswitch]
; thot khi khi lnh switch khng
...
iu kin.
[endcase]
[default]
; thc hin khi cc trng hp so snh
...
u khng ng.
endcase
TS. Phm ng Phc
68
Robot cng nghip
endswitch
V d :
proc main
integer Alpha = 2
switch Alpha
case 0
usermsg "Alpha = 0"
Endcase
case 1
usermsg "Alpha = 1"
endcase
case 2
usermsg "Alpha = 2"
endcase
endswitch
endproc
; gn gi tr ban u cho bin Alpha=2.

; tm gi tr ca bin s
; Trng hp bin c gi tr bng 0.
; Xut kt qu trn ca s mn hnh.
; Ht trng hp so snh th nht.
; tng t nh trn . . .
; lun i km vi switch kt thc khi lnh switch.
if / endif :
Cu lnh iu kin.
C php :
if <iu kin 1>
...
[elseif <iu kin 2]
...
[else]
...
endif
; kt thc khi lnh if.
(Lnh ny gn ging nh lnh if trong Pascal, khng c t then).
while/endwhile :
Lp li mt s cu lnh cho n khi iu kin kim tra l sai.
V d :
proc main
integer SoLanLap = 0
; Bin nguyn dng m s ln lp
while (SoLanLap++) < 3

; Mi ln lp bin tng gi tr thm mt
endwhile
; Kt thc khi lnh while.
usermsg "Toi da lap %d lan" SoLanLap
endproc
for/endfor : Cu lnh lp theo mt s ln nht nh
C php :
for <bin m>=<gi tr ban u> UPTO | DOWNTO <gi trcui>
[BY <bc>]
.
[exitfor]
; Chuyn iu khin thot khi cu lnh lp for
.
; n dng lnh sau endfor
TS. Phm ng Phc
69
Robot cng nghip
endfor
Return :
Thot khi th tc hoc hm hin ti, tip tc cu lnh tip theo ca
th tc gi.
* Cc lnh khc :
transmit :
Gi mt dng k t (lnh) n cng ang hot ng.
V d :
proc main
transmit "B-250~C-200~F-240~~P+200
endproc
; Chuyn lnh iu khin robot TG-45
Pause :
Tm dng thc hin chng trnh trong mt s giy qui nh.
C php :
pause <s giy | FOREVER>
V d :
Pause 5 : tm dng thc hin chng trnh 5 giy
Pause Forever : Dng vi thi gian khng xc nh. Lnh Pause c th
c hu b khi n Ctl-Break.
K t ~ thay cho lnh pause vi gi tr dng bng 0,5 giy.
V d :
Transmit B+200~~E-100
Sau khi truyn lnh B+200 s tm dng 1 giy (2 k t ~) mi truyn tip
lnh E-100.
chdir :
Thay i ng dn n mt a hoc th mc khc.

C php :
chdir <Tn ng dn>
V d :
Chdir C:\ procom3\Robot
copyfile :
Copy mt file theo ng dn hoc vi mt tn khc.
C php :
copyfile <file ngun> <file ch>
V d :
copy C:\ Procom3\ aspect\ robot.was C:\ tam\ robot1.txt
delfile :
Xo mt file theo ch nh.
C php :
delfile <tn file>
mkdir :
To mt th mc mi.
C php :
mkdir <tn th muc>
rmdir
Xo mt th mc (trng)
TS. Phm ng Phc
70
Robot cng nghip
C php :
rmdir <tn th mc>
rename :
i tn mt file.
C php :
rename <tn file c> <tn file mi>
Fopen :
M mt file c hoc ghi.
C php :
fopen <s hiu file> <tn file> READ | WRITE |
READWRITE | CREATE | APPEND | READAPPEND
Cc tu chn : READ : ch c; READWRITE : c th c v ghi;
CREATE : To mi; APPEND : Ghi tip vo cui file;
READAPPEND : C th c v ghi tip vo cui file.
Fclose :
ng mt file m.
C php :
Fclose <s hiu file>
fputs
Ghi mt chui k t ln file.

C php :
fputs <s hiu file> <chui k t>
V d :
proc main
string Fname = "Vidu.txt"
; Tn file c m.
if fopen 0 Fname CREATE

; To mi v m mt file c tn Vidu.txt
fputs 0 "Day la file moi duoc mo !"
; Ghi mt chui ln file.
fclose 0
; ng file c to mi v m.
else
errormsg "Couldn't open file `"%s`"." Fname
endif
endproc
feof :
Kim tra iu kin cui mt file.

C php :
feof <s hiu file> [bin nguyn]
[bin nguyn] : c gi tr 0 nu cha kt thc file, bng 1
nu kt thc file.
Fgets :
c mt dng k t t mt tp m ghi vo mt bin.
C php :
fgets <s hiu file> <tn bin kiu string>
V d :
proc main
string Fname = "Vidu.txt" ; Tn file cn c
string chuoi
; Bin chuoi nhn gi tr c t file.
if fopen 0 Fname READ
while not feof 0
fgets 0 FInput
TS. Phm ng Phc
; M file ch c (s hiu file id=0).

; Lp li khi cha kt thc file.
; c mt dng t file
71
Robot cng nghip
usermsg FInput
; Th hin dng c
endwhile
fclose 0
; ng file
else
errormsg "Can't open `"%s`" for input." Fname ; bo li nu file khng tn ti.
endif
endproc
usesmsg :
th hin mt dng thng bo hay kt qu trn ca s.
C php :
usermsg <:dng thng bo> [bin1, ...]
Xem cc v d trn.
termwrites : Vit mt dng k t ln ca s nhp xut d liu.
C php :
termwrites <bin hoc dng k t>
Run : Thc hin mt chng trnh bn ngoi (ui COM, EXE hoc BAT).
C php :
run <tn chng trnh>
V d :
proc main
string Prog = "C:\ windows\ pbrush.exe"
; Chng trnh cn thc hin.
run Prog
; Thc hin chng trnh PaintBrush ca Windows.
Endproc
Ngoi cc t lnh gii thiu trn, cn c rt nhiu lnh khc..., ngi s
dng c th tham kho trc tip trong mc HELP ca c s son tho khi cn thit.
Ngn ng ASPECT khng c sn cc hm ton hc nh sin, cos, ... nn khi
mun thc hin cc tnh ton phc tp ta phi dng cc phn mm khc.
5.5. Lp trnh iu khin robot TERGAN - 45 :
Nh ni trn, iu khin robot TERGAN-45 ta c th dng phn mm
Procomm Plus for Windows iu khin trc tip hoc vit cc chng trnh bng
ngn ng ASPECT.
5.5.1. Gii thiu robot TERGAN 45 (TG-45):
TERGAN 45 l mt loi robot dng dy hc do Php sn xut. y l loi
robot ton khp quay, c 4 bc t do. i km vi robot gm c mt b ngun v mt
mun iu khin. Mun iu khin cho php iu khin robot trn cc Terminal
hoc my tnh c giao din kiu RS-232. Cu hnh ca robot nh hnh 5.2 :
Vai o
o
Thn
Cnh tay
o C tay
Bn tay
Hnh 5.3 : S ng Robot TG-45
TS. Phm ng Phc
Robot cng nghip
72
Cc khp quay ca robot c dn ng bng cc ng c in mt chiu c

gn cc potentionmeter, ngoi ra ng m bn tay ca robot ngi ta dng truyn
ng vit-me c gn c hnh trnh, vn tc ng m cc ngn tay c th iu chnh
c.
Cc gc quay gii hn ca cc khu trn robot l :
+ Chuyn ng ca thn
2610.
+ Chuyn ng ca vai
850.
+ Chuyn ng ca cnh tay 2490.
+ Chuyn ng ca c tay
1800.
Tc truyn thng tin qua moun iu khin t 50 n 9600 bauds vi b vi x
l 8 bits, Stop bit l 1 hoc 2. in p ngun cung cp l 110V/220V, 50HZ. in p
iu khin 12V.
Trn mun iu khin c thm cc u vo v ra giao din vi cc thit b
khc (nh cc cm bin, iu khin bng ti nh, ...). Moun iu khin robot TG-45
c thit k giao din vi my tnh bng cc lnh c bn sau :
BXXX : iu khin thn (Base),
EXXX : iu khin vai (paule),
CXXX: iu khin cnh tay (Coude),
FXXX : iu khin c tay (Poignet),
PXXX : ng m bn tay (Pince),
SXXX : iu khin cc tn hiu ra,
IXXX : iu khin cc tn hiu vo.
Chiu di ca cc lnh iu khin l 5 k t m ASCII. K hiu XXX biu
din cc ch s t 000 n 511.
V du :
Lnh B-200 s iu khin thn robot quay sang phi mt gc :
1 = (261o/2) x 200 / 511 51004
Lnh C+200 s iu khin cnh tay robot quay ln pha trn mt gc :
3 = (249o/2) x 200 / 511 48043 (so vi vai).
Lnh P+200 s ng bn tay (dng khi mun nm mt vt) , vn tc ng m
thay i c theo gi tr t 001 n 511. V d P+100 s ng chm hn P+200.
Cc lnh c chuyn n t my tnh s c mun iu khin x l sau
tr li cc thng bo thc hin (message) trn mn hnh.
5.5.2. iu khin trc tip robot TG-45 nh phn mm Procomm :
TS. Phm ng Phc
Robot cng nghip
73
ch TERMINAL ca Procomm Plus ta c th iu khin trc tip robot

Tergan 45 bng cch g trc tip cc lnh lm quay cc khp ca robot, v d :
B+200
C-250
E-100
F-250
P+200
Ta c th ghi li cc lnh va nhp vo mt file thc hin li sau ny, nu
trc khi nhp cc lnh ta chn mc START RECORDER trn menu hoc Icon
tng ng.
5.5.3. Vit chng trnh iu khin robot TERGAN-45 :
Ta c th iu khin robot Tergan-45 bng cch vit cc chng trnh bng
ngn ng ASPECT. Mt chng trnh v d n gin nh sau :
proc main
transmit "E-100~B-250~F-180~C-200~B-300~~~P+150~~~
transmit E+000~C-150~B+300~~C-180~~~~P-200~~~"
transmit "E+200~B-400~~~E-000~~~C-300~~~F-080~~~B-450~~~P+150~~~
transmit C-260~~E+100~~B+300~~~~~~P-200~~"
transmit "F+200~C-130~B-350~F-300~~E-180~~B-400~~~P+100~~"
transmit "E+200~~B+300~~~~E-100~~~F-230~~~~P-200~~~~
transmit C-000~F-000~E-000~B-000~P-200~"
pause 50
clear
termwrites "Da thuc hien xong, xin cho lenh !"
endproc
Khi son tho xong chng trnh ta phi t tn v ghi vo a, v d tn
chng trnh l DEMO.WAS. Sau ta phi dch chng trnh to ra file
DEMO.WAX lc mi c th chy c trong Procomm Plus.
Tuy nhin, nh gii thiu trn, mun iu khin robot TG-45 ch c cc
lnh n gin iu khin cc mt dn ng cc khp quay. Nu ch iu khin
robot bng cc lnh n th khng th m rng kh nng lm vic ca robot c,
hn na vic lp trnh cng mt nhiu cng sc v kh xc nh c cc to m
ta yu cu bn tay robot phi t ti. Do vic lp trnh iu khin robot phi to ra
cc chc nng khc khi iu khin robot nh :
1) Chng trnh c th gip ngi s dng dy robot hc m robot c th lp
li cc chuyn ng c dy-hc mt cch chnh xc.
TS. Phm ng Phc
Robot cng nghip
74
2) Thit k iu khin ng hc thun : ngha l chng trnh cho php ngi

s dng iu khin robot theo gi tr cc gc quay ca khp (tnh bng ) khi xc
nh trc mt cu hnh no ca robot.
3) Thit k iu khin ng hc ngc : ngha l ngi s dng chng trnh
c th iu khin robot theo cc to v tr v hng ca bn tay c xc nh
trc. Khi ta nhp cc gi tr v to v hng ca bn tay th chng trnh t ng
tnh ton cc gc quay ca cc khp robot chuyn ng n v tr yu cu vi
hng c xc nh.
4) Thit k cc tin ch khc nh : iu khin theo ng, to cc meta keys,
to ra cc tr gip cho ngi s dng...
Cc ni dung 2 v 3 cn phi thit lp h phng trnh ng hc ca robot
TERGAN-45 v gii h phng trnh ng hc . Phn tnh ton c th vit bng
ngn ng Pascal hoc C++ m n c gi t chng trnh iu khin (dng lnh
RUN), chng trnh iu khin x l kt qu tnh ton qua cc file trung gian dng
text.
Phn mm Procomm cung cp nhiu tin ch ta c th thit k chng
trnh kiu trc quan (Visual), gip cho vic vit chng trnh v thao tc trong qu
trnh s dng c d dng, thun tin hn.
5.8. Kt lun :
Trong chng ny ch gii thiu mt cch tng qut v cc phng php lp
trnh iu khin robot. Kh c th i su, c th vo mt ngn ng no v n ph
thuc rt nhiu vo loi robot c s dng. Phn ngn ng ASPECT trong phn
mm Procomm c nghin cu trn l mt v d v ng dng cc phn mm dng
cho nhiu mc ch iu khin robot. Trong thc t, tu nhim v c th ca mi
robot, phi hp vi hot ng chung ca cc my mc thit b khc m ta thit k cc
chng trnh c th robot hot ng theo nhng mc ch mong mun.
TS. Phm ng Phc
Robot cng nghip
75
Bi tp chng V :
Bi 1 : Hy vit mt Function ca hm arctg2(y,x) bng ngn ng Turbo Pascal.
Bi 2 : Vit mt chng trnh (ngn ng tu ) nhp cc thng s DH v t ng
xc lp cc ma trn An (Biu hin kt qu trn mn hnh v ghi vo mt file dng
text).
Bi 3 : Vit mt chng trnh bng Turbo Pascal tnh ton ng hc nguc (Xc
nh cc gc quay) ca robot TERGAN-45. D liu nhp t bn phm. Ghi kt qu
vo mt file dng text.
Bi 4 : Vit mt chng trnh iu khin robot TERGAN-45 bng ngn ng ASPECT
robot c cu hnh nh sau :
1 = +300; 2 = -100; 3 = -300; 4 = -250.
Bn tay robot nm li sau khi di chuyn n v tr yu cu.
Bi 5 : Vit mt chng trnh bng ngn ng ASPECT, gi chng trnh tnh ng
hc ngc vit bng Turbo Pascal (nh bi 3), x l kt qu tnh ton iu khin
robot TERGAN-45 theo to v tr v hng ca bn tay.
TS. Phm ng Phc
76
ROBOT Cng nghip
Chng VI
M phng robot trn my tnh

(Robot Simulation)
(Phn thc hnh trn my tnh)
6.1. K thut m phng robot :
M phng l mt k thut hin i, c p dng trong nhiu lnh vc
nghin cu v sn xut.
Khi nghin cu v iu khin robot, ta c th thc hin iu khin trc tip
robot hoc iu khin m phng. iu khin m phng l dng cc m hnh tnh
ton ng hc v ng lc hc ca robot kt hp vi cc phng php ho trn
my vi tnh m t v kt cu v hot ng ca cnh tay robot.
Nghin cu v m phng hot ng ca robot trn my tnh gip cho cc
nh thit k nhanh chng la chn c phng n hnh - ng hc ca robot, c
th kim tra kh nng hot ng ca robot trn mn hnh, kim tra s phi hp ca
robot vi cc thit b khc trong dy chuyn. iu ny rt c ngha trong qu trnh
thit k ch to robot mi hoc b tr dy chuyn sn xut.
Qua m phng ngi thit k c th nh gi tng i y kh nng lm
vic ca phng n thit k m khng cn ch th. N cng c xem l phng
tin i thoi, hiu chnh thit k theo yu cu a dng ca ngi s dng.
Phng php lp trnh m phng cng gip ngi thit k chn c qu
o cng ngh hp l ca robot trong qu trnh lm vic vi mt i tng c th
hay phi hp vi cc thit b khc trong mt cng on sn xut c t ng ho.
Hin nay c nhiu phn mm cng nghip v cc phn mm nghin cu
khc nhau m phng robot, phm vi ng dng v gi thnh ca chng cng khc
nhau. y chng ta nghin cu phng php m phng robot dng phn mm
EASY-ROB.
6.2. Gi thiu phn mm EASY-ROB :
EASY-ROB l cng c m phng robot s dng ho trong khng gian 3
chiu (3D) v cc hnh nh c th hot ng c. Mt h thng 3D-CAD n gin
cho php to ra cc khi hnh hc c bn nh khi tr, khi cu, khi ch nht, khi
tam gic ... v kt cu ca robot. Trong EASY-ROB chng ta c th dng chut
quay hoc tnh tin robot n mt to tu . EASY-ROB cng c cc chc
nng phng to, thu nh i tng v nh nhiu phn mm thit k khc... Chng
trnh cho php thit k cc robot n 12 bc t do. Chuyn ng ca Robot c th
c iu khin theo cc bin khp hoc cc to -ct. Chng ta cng c th
m t ng hc ca robot theo kiu DH hoc trong h to ton cc (Universal
TS. Phm ng Phc
ROBOT Cng nghip
77
Coordinates). Easy-Rob c sn cc trnh iu khin ng hc thun v ngc ca

cc cu hnh robot thng dng, khi thit k ta ch cn khai bo kiu ng hc thch
hp. Trong trng hp robot c kt cu c bit hoc c cc khu b ng gn vi
cc chuyn ng ca cc khp th cn phi gii bi ton ng hc ngc hoc xc
nh hm ton hc m t s ph thuc ca khu b ng i vi khp quay, vit
chng trnh xc nh s ph thuc bng ngn ng C v sau dng tp tin
MAKE.EXE trong C dch thnh tp tin th vin lin kt ng er_kin.dll (EasyRob kinematic Dynamic link library), khi chy chng trnh, EASY-ROB s lin
kt vi tp tin ny v thc hin kiu ng hc c khai bo trong chng trnh
iu khin.
Easy-ROB c mt s cc lnh iu khin ring, Chng trnh c vit theo
kiu x l tun t, tp tin dng Text, c th son tho chng trnh trong bt k
trnh son tho no. Cc cng c gn trn khu chp hnh cui c th thay i c.
Chng ta c th vit mt chng trnh chuyn ng cho mt robot theo mt qu o
mong mun, c th kim tra kh nng vn ti ca cnh tay, xc nh vng lm vic
ca robot . . . Robot m phng c th cm nm hoc th cc i tng lm vic. Cc
chuyn ng ca robot c th ghi vo mt tp tin v c th thc hin li.
Phn mm cho php ta xem c cc h to gn trn cc khu ca
robot, xem c qu o chuyn ng ca im cui cng c gn trn khu chp
hnh cui. Phn mm cn c nhiu tin ch khc nh : cho php ta lp trnh iu
khin robot bng phng php dy hc, thit k cc i tng lm vic ca robot, c
cc ca s v to v gi tr gc quay ca cc khp ti tng thi im khi robot
hot ng...
Vic s dng phn mm EASY-ROB m phng robot gip chng ta hai
kh nng nghin cu :
a/ M phng li mt robot c v cc i tng lm vic ca n. nh gi
kh nng lm vic v mc linh hot ca robot, xc nh cc thng s iu khin,
qu o chuyn ng dng trong iu khin thc.
b/ Nghin cu thit k ng hc, cc kch thc v kt cu ca robot trn
my tnh c th chn c phng n ng hc tt nht, m bo cho robot hon
thnh cc nhim v yu cu.
6.3. Tm hiu mn hnh EASYROB :
a- Menu chnh :
Menu chnh ca phn mm EASY-ROB cung cp cc ni dung hot ng
khc nhau ca phn mm. Bc u lm quen, ta cn quan tm cc Menu sau :
Menu FILE : X l cc tc v trn File. Trong Easy-Rob c nhiu loi file
c qui nh bi phn m rng (ui ca File), v d :
File c dng *.Cel : (Cellfile) m t kt cu Robot, cng c lm vic
v i tng lm vic ca robot. y l mt File tng hp, bao gm c chng trnh
dng iu khin robot.
TS. Phm ng Phc
78
ROBOT Cng nghip
Thanh cng c
Menu chnh
Thanh cng c
Ca s thit k
17
12
18
Hnh 6.1 : Mn hnh EASY-ROB.

File c dng *.Rob : (Robotfile) m t ring kt cu ca mt robot.
File c dng *.Bod : (Bodyfile) m t cc i tng lm vic ca
robot.
. . .dng
.
File .c
*.Tol : (Toolfile) m t cng c gn trn khu chp
hnh cui ca robot.
File c dng *.Vie : (Viewfile) xc nh gc nhn trong khng gian.
File c dng *.igp : (Igrip Partfile) lu tr mt b phn kt cu.
File c dng *.Prg : (Programm) Chng trnh iu khin.
v.v....
Menu Robotics : Dng nhp cc thng s DH, xc nh v tr ca dng
c, xc nh v tr robot v cc thng s khc.
Menu 3D-CAD : Cung cp cc cng c v kt cu robot trong khng
gian 3 chiu (3D) cng nh thit k cc cng c, cc i tng lm vic. v
c kt cu ca robot, da vo cc khi hnh hc n gin ta c th lp ghp chng
li to nn cc hnh dng khc nhau ca robot.
b- Cc thanh cng c :
Cc nt trn thanh cng c dng thc hin cc thao tc nh ca menu
chnh (m khng cn vo menu). S dng cc nt trn thanh cng c cho php ta
thao tc nhanh hn l phi vo menu chnh. Chc nng ca cc nt chnh trn thanh
cng c nh sau :
Thanh cng c nm ngang pha trn, tnh t tri sang phi :
1. Bt tt ch chiu sng cc i tng v.
TS. Phm ng Phc
ROBOT Cng nghip
79
2. Chuyn tt c cc i tng sang dng li.

3. Chuyn i tng dng tr / khi phc tp.
5. Th hin/khng th hin sn.
6. Th hin sn dng li.
7. Reset v tr robot trn mn hnh.
8. Chuyn i ca s khi m Cellfile hoc igip partfile (kt hp vi nt 7).
9. Chy chng trnh.
10. Tm dng chng trnh.
11. Tip tc chy chng trnh.
12. Kt thc chng trnh.
13. Chy chng trnh theo tng bc.
14. Lp li chng trnh sau khi kt thc.
15. 16. Gim v tng tc iu khin.
17. nh gi sai s v xem cc gi tr ng hc.
Thanh cng c nm ngang pha di, tnh t tri sang phi :
1. Thy hoc khng thy kt cu robot.
2. Thy hoc khng thy dng c.
3. Thy hoc khng thy cc i tng lm vic.
4. Th hin/khng th hin h to gn vi dng c .
5. Th hin/khng th hin h to gn trn cc khu ca robot.
6. Th hin v tr iu khin.
7. M phng ng lc hc.
8. Th hin qu o chuyn ng.
9. S dng cc gii hn ca khp.
10. Son tho chng trnh v dy hc.
12. Th hin hoc khng th hin H to gn trn i tng hin thi.
13. Chuyn n i tng tip theo (khi thit k).
14. Xc nh v tr tuyt i ca i tng hin ti.
15. Xc nh v tr tng i ca i tng hin ti.
16. Reset v tr ca i tng hin ti.
17. Ghi li v tr ca i tng sau khi iu chnh.
18. a robot v v tr dng (Home position).
19. iu khin robot theo khp quay.
Thanh cng c thng ng (Thao tc bng chut) , tnh t trn xung :
1. Dng chut view, zoom v Pan.
2.3. iu khin hng ca khu chp hnh cui bng chut.
4. iu khin cc khp 1,2,3 (Dng cc phm chut).
5. Di chuyn thn robot. (h to c s)
6. Di chuyn cc i tng (body) bng chut.
7. Di chuyn tt c cc i tng bng chut.
9. Chuyn i chuyn ng l quay hoc tnh tin (Dng khi hiu chnh i
tng v).
11.12. Tng gim tc iu khin bng chut.
6.4. Thao tc chut :
Easy-Rob cho php dng chut vi nhiu chc nng nh :
TS. Phm ng Phc
ROBOT Cng nghip
80
Khi nt lnh s 1 ca thanh cng c thng ng c chn :

zoom (Phng to, thu nh) : n nt chut phi, r chut ln xung theo
phng thng ng ca mn hnh.
Pan (thay i v tr ca i tng so vi khung mn hnh) : n ng thi
hai nt chut phi v tri, r chut trn mn hnh.
Rotate (quay robot nhn cc gc khc nhau) : n chut tri, r chut.
Khi nt lnh s 4 ca thanh cng c thng ng c chn :
Quay khp 1: n nt chut phi, r chut (nu l khp tnh tin s lm khu
chuyn ng tnh tin).
Quay khp 2: n ng thi 2 nt chut phi v tri, r chut.
Quay khp 3: n nt chut tri, r chut.
6.5. Gn h to :
Mun xc nh h to ca robot trc ht phi thc hin bng tay cc
cng vic sau:
V s ng robot v tr dng, gn h to ca cc khu ln hnh v
trn giy, xc nh cc thng s DH.
Cc bc tip theo :
1- Bt nt lnh s 5 trn menu ngang, di.
2- Vo menu chnh : FILE -> LOAD -> ROBOTFILE chn DHTempl ->
OPEN.
3- Vo menu chnh : ROBOTICS -> ROBOTMOTION + KINEMATICS ->
KINEMATICS DATA.
4- Chn Active Join -> Ok -> Activ Joint (1) RZ (hoc chn TZ nu l khp
tnh tin) -> Ok -> Nhp cc thng s DH ca khu th nht.
5- Chn Quit -> Ok.
Vo li bc 4 -> Number Active Joint(1) -> Ok -> n p chut vo vt
xanh hoc a con tr vo phn nhp d liu (text box) n 2 (By gi s khu ng
l 2), nhp cc thng s DH cho khu s 2 ...
Lm tng t cho n khi s khp yu cu.
Ta c th kim tra cc s liu nhp bng cch kch chut vo menu :
ROBOTICS -> ROBOTMOTION + KINEMATICS -> KINEMATICS DATA->
KINEMATIC INFOMATION xem li s khu, khp v cc thng s DH. Nu
vo d liu sai ta c th hiu chnh li.
th hin h to ca robot trn mn hnh (H to mu vng), nh
kch chut vo nt s 5 ca thanh cng c nm ngang pha di.
TS. Phm ng Phc
ROBOT Cng nghip
81
6.6. V hnh dng robot :

Sau khi hon thnh vic gn h to ca robot, bc tip theo l v hnh
dng ca n. Hnh dng ca robot c th c m phng ging nh robot thc nh
cng c 3D CAD ca EasyRob. Menu 3D-CAD cho php to ra cc khi hnh hc
c bn nh khi tr, khi cu, khi ch nht, khi tam gic ... S phi hp hp l v
kch thc v v tr ca cc khi hnh hc ny cho php th hin c cc kt cu
khc nhau ca robot.
Cc menu ko xung ca Menu 3DCAD nh hnh 6.2, mt s cc chc nng
chnh nh sau :
+ Select group : Chn nhm i tng
thit k : 1/Robot group, 2/Tool group
hay 3/ Body group.
+ Select body from group : Chn cc b
phn ca robot v (theo tn t trc)
ca nhm chn hin hnh.
+ Create/Import new 3D body : To mi
hoc nhp mt b phn c sn. Cn
nhp cc thng s cn thit to ra i
tng mong mun.
+ Modify sel. Body_set Jnt_idx : Hiu
chnh cc thuc tnh ca b phn hin
hnh.
+ Clone : Copy b phn ang v thnh
nhiu hnh.
Hnh 6.2 : Menu 3D-CAD
+ Render : Biu hin i tng dng
li, dng hp, . . .
+ Color : Thay i mu sc.
+ Name : Thay i tn b phn ang v.
+ Clear : Xo i tng (b phn) hin
hnh.
+ Position's : Thay i v tr ca i tng (b phn) hin hnh.
+ 3D CAD Coorsys Visibility : Cho hin hoc n h ta ca i
tng v.
+ Next Body in group : Chn i tng v tip theo.
Dng menu 3D CAD ta ln lt v tt c cc khu ca robot, c th dng
cc mu sc khc nhau th hin hnh dng ca robot. Lu trong qu trnh v,
nu v sai phi dng mc CLEAR xa i hoc dng mc MODIFY CEL hiu
chnh. Mi i tng v phi gn vi mt khu nht nh, c khai bo trong mc
SET JOINT INDEX.
C th dng thanh cng c thng ng pha phi thay i v tr ca cc
i tng v cho thch hp.
TS. Phm ng Phc
82
ROBOT Cng nghip
6.7. Lp trnh iu khin robot m phng :

lp trnh iu khin robot m phng ta dng phng php lp trnh
kiu dy hc. Sau khi thit k hnh dng robot, cng c gn trn khu chp hnh
cui, cc i tng lm vic khc . . . ta c th lp trnh iu khin robot m
phng. Vic lp trnh thc hin theo trnh t sau y :
Nhp chut vo nt lnh s 10 (Show program window) kch hot ca s
lp trnh nh hnh 6.3 :
Hnh 6.3 : Ca s lp trnh.

Chn New t tn cho File chng trnh.
Chn Append nu mun b sung mt chng trnh c trn a.
Xc nh v tr cc im m dng c phi i qua (dng chut iu khin
cc khp, dng menu ng). C sau mi ln xc nh c mt v tr th n nt PTP
(iu khin im) hoc LIN (iu khin ng) hoc VIA (dim trung gian dn
hng khi iu khin ng cong), CIRC (iu khin theo ng cong). Lm lin
tc cho tt c cc im c mt chng trnh hon thin.
Sau khi kt thc vic dy robot hc, n nt Close trn Program Window
kt thc. hiu chnh v b sung cc lnh iu khin khc vo chng trnh, n
chut vo nt EDIT, Dng cc lnh ca EasyRob nh di y hon thin
chng trnh.
ERPL - EASY-ROB-Program Language
Ghi ch :
- n v chiu di l Mt [m], Gc l [deg] hoc [%]
- n v ca tc l [m/s]
- V tr v hng ca h ta gn trn khu chp hnh cui c xc nh gm :
X, Y v Z : ch ta v tr, A, B v C ch gc hng.
Hng ca khu chp hnh cui xc nh theo cc gc ABC l:
Rot (A,B,C) = Rot(X,A) * Rot(Y,B) * Rot(Z,C)
Cu trc chung ca chng trnh, M t c php mt s lnh hay dng :
PROGRAMFILE : Bt u chng trnh
TS. Phm ng Phc
ROBOT Cng nghip
83
ENDPROGRAMFILE or END : Kt thc chng trnh.

CALL fct_name : Gi mt hm c tn fct_name(), c nh ngha trong chng
trnh.
CALL FILE filename : Gi mt File chng trnh c tn filename, File phi c cung
cu trc nh chng trnh chnh.
FCT fct_name() : Bt u nh ngha mt hm c tn fct_name().
ENDFCT : Kt thc nh ngha mt function.
! Cc ghi ch trong chng trnh.
TOOL X Y Z A B C [m,deg] : nh ta im cui ca dng so so vi khu chp
hnh cui.
PTP X Y Z A B C [m,deg] : Di chuyn robot n im mi (ta tuyt i). iu
khin im.
PTP_REL dX dY dZ dA dB dC [m,deg] : Di chuyn robot n im mi (ta
tng i). iu khin im.
LIN X Y Z A B C [m,deg] : Di chuyn robot n im mi (ta tuyt i). iu
khin ng.
LIN_REL dX dY dZ dA dB dC [m,deg] : Di chuyn robot n im mi (ta
tng i). iu khin ng.
CIRC X Y Z A B C [X2 Y2 Z2] [m,deg] : Di chuyn robot n im mi (ta
tuyt i). iu khin ng cong.
[X2 Y2 Z2] - im trung gian (3 im xc nh mt cung trn).
CIRC_REL dX dY dZ dA dB dC [dX2 dY2 dZ2] [m,deg] : Di chuyn robot n
im mi (ta tng i). iu khin ng cong.
WAIT x [sec] : Robot dng hot ng trong x giy.
ERC TRACK ON,OFF : Th hin hoc khng th hin qu o chuyn ng.
ERC LOAD TOOL filename : Gi mt Tool file (*.tol)
ERC LOAD VIEW filename : Gi mt View file (*.vie)
ERC LOAD ROBOT filename Loads a Robot file (*.rob)
ERC LOAD BODY filename Loads a Body file (*.bod)
ERC LOAD TAGS filename Loads a Tag file (*.tag)
ERC GRAB BODY bodyname : Dng c cm ly mt vt th (body) c tn
Bodyname.
ERC GRAB BODY_GRP : Dng c cm ly mt nhm vt th (Body_Grp).
ERC RELEASE BODY bodyname : Dng c th (bung) mt vt th (body) c
tn Bodyname.
ERC RELEASE BODY_GRP Dng c th (bung) mt nhm vt th (Body_Grp).
ERC ROBOT_BASE XYZ ABC [m,deg] : Di chuyn gc ta c bn ca robot
n v tr mi.
v.v...
Cn rt nhiu cc lnh khc ca Easy-Rob, c th tham kho trn Website:
http ://www. easy-rob.com.
=================================================
TS. Phm ng Phc
84
Robot cng nghip
chng VII
ng lc hc Robot
(Dynamic of Robot)
7.1. Nhim v v phng php phn tch ng lc hc robot

Nghin cu ng lc hc robot l cng vic cn thit khi phn tch cng nh tng
hp qu trnh iu khin chuyn ng. Vic nghin cu ng lc hc robot thng gii
quyt hai nhim v sau y :
1/ Xc nh momen v lc ng xut hin trong qu trnh chuyn ng. Khi qui
lut bin i ca bin khp qi(t) coi nh bit.
Vic tnh ton lc trong c cu tay my l rt cn thit chn cng sut ng c,
kim tra bn, cng vng, m bo tin cy ca robot.
2/ Xc nh cc sai s ng tc l sai lch so vi qui lut chuyn ng theo chng
trnh. Lc ny cn kho st Phng trnh chuyn ng ca robot c tnh n c tnh ng
lc ca ng c v cc khu.
C nhiu phng php nghin cu ng lc hc robot, nhng thng gp hn c l
phng php c hc Lagrange, c th l dng phng trnh Lagrange - Euler. i vi cc
khu khp ca robot, vi cc ngun ng lc v knh iu khin ring bit, khng th b
qua cc hiu ng trng trng (gravity effect), qun tnh (initial), tng h (Coriolis), ly
tm (centripetal)... m nhng kha cnh ny cha c xt y trong c hc c in; C
hc Lagrange nghin cu cc vn nu trn nh mt h thng khp kn nn y l nguyn
l c hc thch hp i vi cc bi ton ng lc hc robot.
7.2. C hc Lagrange vi cc vn ng lc ca robot.
L=K-P
(7.1)
Trong :
K l tng ng nng ca h thng
P l tng th nng
K v P u l nhng i lng v hng nn c th chn bt c h to thch hp
no bi ton c n gin. i vi mt robot c n khu, ta c :
n
K = Ki
i =1
P = Pi
i =1
y, Ki v Pi l ng nng v th nng ca khu th i xt trong h to chn.Ta

bit mi i lng Ki v Pi l mt hm s ph thuc nhiu bin s:
Ki = K(qi, q& i ) v Pi = P(qi, q& i )
Vi qi l to suy rng ca khp th i. Nu khp th i l khp quay th qi l gc
quay i, nu l khp tnh tin th qi l di tnh tin di.
Ta nh ngha : Lc tc dng ln khu th i (i=1, 2,..., n) vi quan nim l lc tng
qut (Generalized forces), n c th l mt lc hoc mt momen (ph thuc vo bin khp
qi l tnh tin hoc quay), c xc nh bi:
d L L
Fi =
(7.2)
dt q& i q i
TS. Phm ng Phc
85
Robot cng nghip
Phng trnh ny c gi l phng trnh Lagrange-Euler, hay thng c gi tt

l phng trnh Lagrange.
7.3. V d p dng :
Xt mt robot c hai khu nh hnh v, Cc khu c chiu di l d1 v d2 vi cc

khi lng tng ng m1 v m2 qui i v u mt ca khu. Robot c t thng ng
chu gia tc trng trng g. Cc khp chuyn ng quay vi cc bin khp 1 v 2. Tnh
lc tng qut.
Qua v d ny, ch vi mt mi lin kt hai
y
khu, cc vn t ra u c mt
g = 9,81m/s2
trong qu trnh nghin cu ng lc hc,
v do , v d nu trn c th m rng
p dng trong nhng trng hp phc tp
O0
x
x1
x2
hn. i vi khu 1 :
1
1
K1 = m1 v 12 = m1d 12 & 12
(7.3)
z
2
2
1
P1 = -m1gd1cos1
(7.4)
m1
y1
i vi khu 2 :
V to :
x2 = d1sin1 + d2sin(1 + 2)
2
y2
m2
y2 = -d1cos1 - d2cos(1 + 2)
Chiu cao th nng :
h = d1cos1 + d2cos(1 + 2)
2
2
2
v 2 = x& 2 + y& 2
V mt vn tc :
d
Vi
x& 2 = x 2 = d 1 cos(1 )& 1 + d 2 cos(1 + 2 )(& 1 + & 2 )
dt
d
y& 2 = y 2 = d 1 sin(1 )& 1 + d 2 sin(1 + 2 )(& 1 + & 2 )
dt
2
v 2 = d 12 & 12 + d 22 (& 12 + 2& 1& 2 + & 22 ) + 2d 1d 2 cos( 2 )(& 12 + & 1& 2 )
ng nng v th nng s l :
1
1
K 2 = m 2 v 22 = m 2 d 12 & 12 + d 22 (& 12 + 2& 1& 2 + & 22 ) + 2d 1d 2 cos( 2 )(& 12 + & 1& 2 )
2
2
P2 = m 2 g[d 1 cos(1 ) + d 2 cos( 1 + 2 )]
(7.5)
(7.6)
7.4. Hm Lagrange v lc tng qut :

p dng hm Lagrange cho v d trn, ta c :
L = (K1 + K2) - (P1 + P2)
1
1
L = ( m1 + m 2 )d 12 & 12 + m 2 d 22 (& 12 + 2& 1& 2 + & 22 ) + m 2 d 1d 2 cos 2 (& 12 + & 1& 2 ) +
2
2
+ ( m1 + m 2 )gd 1 cos 1 + m 2 gd 2 cos(1 + 2 )
(7.7)
Khi tnh lc tng qut, cc bin ca h : q1 = 1 v q2 = 2.
i vi khu 1 :
L
L
=
= ( m1 + m 2 )d 12 & 1 + m 2 d 22 (& 1 + & 2 ) + 2 m 2 d 1d 2 cos 2 & 1 + m 2 d 1d 2 cos 2 & 2
&
q& 1 1
TS. Phm ng Phc
86
Robot cng nghip
d L
&& + m d 2 (&&
&&
& &
&&
= ( m1 + m 2 )d 12
1
2 2 1 + 2 ) 2 m 2 d 1d 2 sin 2 2 1 + 2 m 2 d 1d 2 cos 2 1
&
dt 1
&&
m d d sin & 2 + m d d cos
2 1 2
2 2
2 1 2
2 2
L
L
=
= ( m1 + m 2 )gd 1 sin 1 m 2 gd 2 sin(1 + 2 )
q 1 1
Vy :
L
d L
&& +
= [( m1 + m 2 )d 12 + m 2 d 22 + 2 m 2 d 1d 2 cos 2 ]
1
&
dt 1 1
&& 2 m d d sin & & m d d sin & 2 +
(7.8)
+[ m 2 d 22 + m 2 d 1d 2 cos 2 ]
2
2 1 2
2 2 1
2 1 2
2 2
+ ( m1 + m 2 )gd 1 sin 1 + m 2 gd 2 sin( 1 + 2 )
Mun cho khu 1 quay c mt gc 1 th ng c phi to ra mt lc tng qut
F1. Lc tng qut ny c c tnh phi tuyn, l hp tc dng ca nhiu yu t (non linear
and cuppling).
Tng t, tnh lc tng qut ca khu th hai , ta c :
L
= m 2 d 22 & 1 + m 2 d 22 & 2 + m 2 d 1d 2 cos 2 & 1
&
F1 =
d L
&&
& &
&& + m d d cos
= m 2 d 22 &&
1 + m 2 d 22
2
2 1 2
2 1 m 2 d 1d 2 sin 2 1 2
&
dt 2
L
= m2 d1d 2 sin( 2 )&1&2 m2 d1d 2 sin( 2 )&12 m2 gd 2 sin(1 + 2 )
2
v
Vy :
d L L
= [m2 d 22 + m2 d1d 2 cos 2 ]&&1 + m2 d 22&&2

&
dt 2 2
m d d sin( )& 2 + m gd sin( + )
F2 =
(7.9)
phn tch ngha cc thnh phn trong biu thc tnh lc tng qut, ta vit li
cc biu thc F1, F2 nh sau :
&& + D & 2 + D & 2 + D & & + D & & + D
F1 = D11&&
1 + D12
2
111 1
122 2
112 1 2
121 1 2
1
2
2
&&
&&
&
&
&
&
&
&
F2 = D12 1 + D 22 2 + D 211 1 + D 222 2 + D 212 1 2 + D 221 1 2 + D 2
Hiu ng
Hiu ng Hiu ng
Hiu ng
qun tnh
ly tm
tng h
trng trng
Effective inertias Centripetal effect Coriolis effect Gravity
(Trong : D111 = 0; D222 = 0; D112 = D121 = D212 = D221 =-m2d1d2sin2 ...)

Trong cc biu thc trn, cc h s dng Dii hoc D ij th hin hiu ng qun tnh ti
khp i hoc j gy ra bi gia tc ti khp i hoc j. Cc s hng c dng D & 2 l lc ly tm
ijj
tc ng ln khp i gy ra bi vn tc ti khp j. S hng dng D ijk& j&k + D ikj&k& j l lc

Cariolis tc ng ln khp th i gy ra do vn tc ti khp j v k. S hng c dng Di l lc
trng trng tc ng ln khp i.
TS. Phm ng Phc
87
Robot cng nghip
7.5. Phng trnh ng lc hc robot :
Xt khu th i ca mt robot c n khu. Tnh lc tng qut Fi ca khu th i vi

khi lng vi phn ca n l dm. Lc tng qut Fi ng vai tr rt quan trng khi xy dng
s khi thit lp hm iu khin cho robot c n bc t do.
7. 5. 1. Vn tc ca mt im trn robot :
Mt im trn khu th i c m t trong h to c bn l :
(7.10)
r = Ti. ir
i
Trong : r l to ca im xt i vi khu th i, ir khng thay i theo thi
gian. Ti l ma trn chuyn i t khu th i v h to gc : Ti = A1A2...Ai. Nh vy r l
mt hm ca thi gian t.
Tc ca vi khi lng dm c tnh bi cng thc :
i T
dr d i
&r =
= Ti r = i q& j i r
(7.11)
dt dt
j=1q j
Khi tnh bnh phng ca vn tc ny ta c :

&r. &r = r 2 ( x& o , y& o , z& o ) = Tr ( &r &r T )
(7.12)
y
i
Khu i
dm
Ti
r
O0
z
Hnh 7.1. Kho st tc ca vi khi lng dm.
Vi rT l chuyn v vect v Tr l vit tt ca Trace (vt ca ma trn) :

a 11 a 12 ... a 1n
a
a 22 ... a 2 n n
21
= a
Trace
... ... ... ... i =1 ii
a n1 a n 2 a 11 a nn
Hay :
x 2
y . [x y z ] =
y2
z
z
Do vy
d
d
&r 2 = Tr ( &r. &r T ) = Tr ( Ti .i r. Ti T .i r T )
dt
dt
TS. Phm ng Phc
88
Robot cng nghip
i T
i T
= Tr i q& j i r. i q& k i r T
k =1 q k
j=1q j
T
i i Ti i i T Ti
rr .
q& j q& k
= Tr
qk
j =1 k =1 q j
(7.13)
7. 5. 2. Tnh ng nng ca vi khi lng dm.
K hiu Ki l ng nng ca khu th i. dKi l ng nng ca vi khi lng dm t

ti v tr ir trn khu th i.
T
1 i i Ti i i T Ti
dK i = Tr
rr .
q& j q& k dm
2 j=1k =1 q j
q k
T T
1 i i Ti i
= Tr
(7.14)
( r. dm.i r T ). i q& jq& k
2 j=1k =1 q j
q k
V do ng nng ca khu th i s l :
Ti T
1 i i Ti
i i T
K i = dK = Tr
( r. r dm ).
q& j q& k (7.15)
2 j =1 k =1 q j Khau i
q k
Khau i
Ji =
r.i r T dm gi l ma trn gi qun tnh (Pseudo inertia matrix).
Khau i
ngha "gi qun tnh" c s dng v khi thit lp y cc phn t ca ma trn Ji ta

c th lin h vi cc khi nim "mmen qun tnh c cc" v trnh by cc phn t ca Ji
ging nh cc phn t ca mmen qun tnh c cc. Ta xt mi quan h ny nh sau :
Theo nh ngha ta c :
i i
i x 2 dm
x ydm i x i zdm i xdm
i i
i 2
i i
i
x
ydm
y
dm
y
zdm
ydm
J i = i r.i r T dm = Ji = i i
(7.16)
i i
i 2
i
x
zdm
y
zdm
z
dm
zdm
Khau i
i
i
i xdm
ydm zdm dm

By gi ta nhc li mmen qun tnh c cc ca
mt vt th bt k nh hnh v.
y x
Theo nh ngha ta c :
I xx = ( y 2 + z 2 )dm
z
I yy = x 2 + z 2 )dm
Hnh 7.2 : Mmen qun tnh c cc
V v :
Vy :
I zz = ( x 2 + y 2 )dm
1
1
1
x2 = ( y2 + z2 ) + (x2 + z2 ) + (x2 + y2 )
2
2
2
2
x dm =( I xx + I yy + I zz ) / 2 ; .v.v
Ngoi ra ta cn c :
I xy = xydm ; I yz = yzdm ; I xz = xzdm
mx = xdm ; my = ydm ;
TS. Phm ng Phc
mz = zdm
89
Robot cng nghip
i chiu vi ma trn gi qun tnh Ji, ta c th trnh by Ji nh sau :
I xx + I yy + I zz
I yx
I zx
mx
I xx I yy + I zz
I xy
I zy
my
ji =
2
I xx + I yy I zz
I yz
I yz
mz
mx
my
mz
m
Nh vy ngha biu trng ca Ji r.

T
1 i i Ti Ti
Vy ta c :
K i = Tr
Ji
q& j q& k
2 j =1 k =1 q j
q k
Cui cng, ng nng ca mt robot c n khu c tnh :
(7.17)
(7.18)
K = Ki
(7.19)
i =1
7. 5. 3. Tnh th nng ca robot :
Th nng ca khu i c khi lng mi, trng tm c xc nh bi vect ri (vect

biu din trng tm ca khu i trong h to c bn) l :
(7.20)
Pi = -mi. g. ri = -mi. g. Ti iri
Trong , vect gia tc trng trng g c biu din di dng mt ma trn ct :
gx 0
g 0
g = y =
g z 9,8

0 0
Th nng ca ton c cu robot n khu ng s l :
n
P = mi gTi i ri
(7.21)
i =1
7. 5.4. Hm Lagrange :
Sau khi xc nh ng nng v th nng ca ton c cu, ta c hm Lagrange ca
robot c n bc t do :
L=
Ti
Ti T
1 n i i
Trace
J
q i q
2 i =1 j =1 k =1
k
j
q& j q& k + 1 mi gTi i ri
2 i =1
(7.22)
Chng ta ch rng, trong hm Lagrange vn cha cp n nh hng ca

ngun truyn ng (gm cc phn tnh (stator) v phn ng (Rotor) ca ng c in).
7. 5. 5. Phng trnh ng lc hc robot :
Ta bit lc tng qut t ln khu th i ca robot c n khu (Phng trnh

Lagrange - Euler) :
d L L
Fi =
(7.23)
dt q& i q i
Sau khi thit lp hm Lagrange, vi p = 1... n, ta tnh c :
TS. Phm ng Phc
90
Robot cng nghip
(p l ch s ln lt ly theo j v k)
Ti Ti T
Ti
Ti T
1 n i
L 1 n i
q& j
Ji
q& k + Tr
Ji
= Tr
2 i =1 j =1 q j
q p
q& p 2 i =1 k =1 q p
qk
Thay i ch s gi j thnh k trong s hng th hai ,v rng :
(7.24)
Ti
Ti
Ti
Ti T
Ti T
Ti T
(7.25)
Tr
J
J
J
= Tr
= Tr
q i q
q i q
q i q
p
p
j
p
j
j
n
i
Ti
Ti T
L
Ji
q& k
(7.26)
ta c :
= Tr
q& p i =1 k =1 qk
q p
Cng rng : trong Ti(q1, q2, . . . , qi), vi qi l cc bin khp ca i khp u tin. Do
Ti
vy, nu i < p th
= 0.
q p
q& k
n
i
T
T T
L
= Tr i J i i
q& p i = p k =1 qk
q p
Cui cng ta c :
(7.27)
Ly vi phn theo thi gian t ca phng trnh trn :
T
T T
d L
d n i
= Tr i J i i
dt q& p dt i = p k =1 qk
q p
n
i
T
T T
= Tr i J i i
q
q p
i = p k =1
k
q& k
T
n
i
i
2
q&&k + Tr Ti J i Ti q& k q& m +
q p
i = p k =1 m =1
qk qm
n
i
i
2Ti
T T
J i i q& k q& m
+ Tr
qk
i = p k =1 m =1
q p qm
(7.28)
(Bin i theo ch (7.25))

S hng cui ca phng trnh Lagrange Euler l :
2Ti
T T
L 1 n i i
Ji i
= Tr
q p 2 i = p j =1 k =1 q j q p
qk
+
q& j q& k +
2Ti
Ti T
1 n i i
Tr
J
i
2 i =1 j =1 k =1 qk q p
q j
Cui cng ta c lc tng qut ca khu p :
q& j q& k + mi g Ti i ri
q p
i= p
Fp =
(7.29)
L
d L
dt q& p q p
Thay th cc ch s p v i thnh i v j, ta s c :
j
j
n
n
2T j
T jT
T j j
q&&k + Tr
&
&
J
q
q
m
m
g
rj
k
j
j
q
q
q
q
=
=
1
=
1
=
j
i
j
i
k
m
i
i
k
m
(7.30)
Vi mt robot c n bc t do th :
j
n
T j
T jT
Fi = Tr
Jj
qi
j =i k =1
qk
TS. Phm ng Phc
91
Robot cng nghip
q = [q1, q2, . . . ,qn]T

q& = [q& 1 , q& 2 , ... , q& n ] T
F = F[F1, F2, . . . , Fn]T
cho gn, ta biu din :
F = J ( q) q&& + C ( q, q& ) q& + G ( q)
(7.31)
Trong :
J th hin tc dng ca qun tnh, l mt ma trn i xng (n x n);
C th hin tc dng ca lc ly tm v Cariolis, l mt vect (n x 1);
G th hin tc dng ca lc trng trng, cng l mt vect (n x 1).
y l phng trnh ng lc hc ca robot.
Nu thm vo phng trnh trn cc tc dng khc nh : FEX c trng cho cc
ngoi lc tc dng ln trc, V c trng cho hiu ng ma st, ta c :
F = J ( q) q&& + C ( q, q& ) q& + G ( q) + V ( q& ) + FEX
TS. Phm ng Phc
(7.32)
92
Robot cng nghip
Chng VIII
Thit k qu o robot.
(Trajectory Planing)
Trong cc ng dng cng nghip ca robot, ta thng gp hai trng
hp sau :
Trng hp 1 : Khu chp hnh cui ca robot ch cn t c v tr v
hng ti cc im nt (im ta : Knot point). y chnh l phng php
iu khin im (PTP). Ti , bn tay robot thc hin cc thao tc cm nm
i tng hoc bung nh i tng. y l trng hp ca cc robot thc
hin cng vic vn chuyn v trao i phi liu trong mt h thng t ng
linh hot robot ho. Bn tay robot khng trc tip tham gia vo cc nguyn
cng cng ngh nh hn, ct kim loi ... Cc im nt l mc tiu quan trng
nht, cn dng ng i ti cc im nt l vn th yu. Trong trng hp
ny Robot thng c lp trnh bng phng php dy hc (Teach and
playback mode). Trong trng hp ny khng cn tnh ton phng trnh ng
hc hoc ng hc ngc robot, chuyn ng mong mun c ghi li nh
mt tp hp cc gc khp (thc t l tp hp cc gi tr m ho ca bin khp)
robot thc hin li (Playback) khi lm vic.
Trng hp 2 : Khu chp hnh cui ca robot phi xc nh ng i
qua cc im nt theo thi gian thc. l trng hp cc tay my trc tip
thc hin cc nguyn cng cng ngh nh sn, hn, ct kim loi ... Vn
thit k qu o cho cc robot trong trng hp ny l rt quan trng. N
quyt nh trc tip cht lng thc hin cc nguyn cng cng ngh m robot
m nhn. Trong chng ny, chng ta cp n bi ton thit k qu o
vi mt s qu o in hnh. Cc qu o ny khng ch c ngha trong
trng hp ng dng th hai m n bao hm mt ngha chung cho mi
robot, v ngay c trng hp n gin nh cc robot thuc ng dng th nht
cng thc hin nhng chuyn ng qu o c bn m chng ta s nghin cu
di y.
8.1. Cc khi nim v qu o robot :
xc nh c ng i mong mun ca robot theo thi gian, qu
o c th c tnh ton thit k trong mt h to truyn thng Oxyz
(Cartesian Space) hoc thit k trong khng gian bin khp (khng gian
trng vect cc to suy rng ca robot), chng hn vi robot 6 bc t do
th X = [1 , 2 , 3 , 4 . 5 , 6 ] T . Thit k qu o y c hiu l xc nh qui
lut chuyn ng ca cc bin khp iu khin chuyn ng ca tng khp
v tng hp thnh chuyn ng chung ca robot theo mt qu o c
xc nh.
TS. Phm ng Phc
93
Robot cng nghip
Qu o cn thit k nht thit phi i qua mt s im nt cho trc (t

nht l im u v im cui). Ngoi cc im nt chnh, ta cn c th chn
thm cc im nt ph gi l im dn hng (via point) trnh cc chng
ngi vt.
Khi thit k qu o trong khng gian bin khp, ti mi im nt phi
xc nh gi tr ca cc bin khp bng phng php tnh ton ng hc
ngc. Thi gian yu cu ca mi on qu o (gia 2 im nt) l ging
nhau cho tt c cc khp v vy yu cu tt c cc khp phi t n im nt
ng thi. Ngoi vic yu cu thi gian phi ging nhau cho cc khp, vic
xc nh cc hm qu o ca mi bin khp khng ph thuc vo cc hm
ca cc khp khc. V vy vic thit k qu o trong khng gian bin khp
n gin v d tnh ton hn khi m t trong h to cc.
Qu o thit k phi m bo cc iu kin lin tc (continous
conditions) bao gm :
+ Lin tc v v tr (Position)
+ Lin tc v tc (Velocity)
+ Lin tc v gia tc (Acceleration).
x(t)
qi(t2)...
x2
x1
xf-1
xo
Cc im nt
xf
to
t1
t2
tf-1
tf
Hnh 8.1. Tnh lin tc ca qu o robot.

thit k qu o robot, ngi ta thng dng phng php xp x
cc a thc bc n, cc qu o thng gp l :
+ Qu o CS (Cubic Segment) : Tng ng a thc bc 3;
+ Qu o LS (linear Segment) : Tng ng a thc bc 1;
+ Qu o LSPB (Linear Segment with Parabolic Blend) : Phi hp a
thc bc 2 vi a thc bc 1.
q0
on thng
q2
q1
qf
ng cong bc 2
Hnh 8.2 : Qu o LSPB
TS. Phm ng Phc
94
Robot cng nghip
+ Qu o BBPB (Bang Bang Parabolic Blend) : l trng hp c bit

ca qu o LSPB khi on tuyn tnh thu v bng 0 v xut hin im
un.
qf
q0
Hnh 8.2 : Qu o BBPB

Nu cho trc nhiu im nt, ta c th p dng nhiu dng qu o c
bn khc nhau cho mt bin khp.
8.2. Qu o a thc bc 3 :
Khi thit k qu o robot theo a thc bc 3 qua cc im nt, mi
on qu o gia hai im nt s c biu din bng mt phng trnh bc
3 ring bit. Qu o a thc bc 3 m bo s lin tc ca o hm bc nht
v bc hai ti cc im nt.
Ti thi im tk t tk+1, qu o xp x a thc bc 3 ca bin khp
th i l qi(t) c dng :
(8.1)
qi(t) = ai + bi(t - tk) + ci(t - tk)2 + di(t - tk)3
qi(t)
Vi cc rng buc :
qi(tk) = qk v q& i (t k ) = q& k
qi(tk+1) = qk+1 v q& i (t k +1 ) = q& k +1
qk+1
qk
Bc 3
tk+1
tk
(8.2)
T (8.1) ta thy : t = tk ai = qk
Ly o hm ca (8.1) theo t, ta c :
q& i (t) = b i + 2c i (t t k ) + 3d i (t t k ) 2
(8.3)
Ti : t = tk b i = q& k
Ti t = ti+1 ta c hai tham s :
3(q k +1 q k ) (2q& k + q& k +1 ) t k
ci =
(8.4)
t 2k
(q& + q& k ) t k 2(q k +1 q k )
d i = k +1
(8.5)
t 3k
Trong : t k = t k +1 t k
Cc phng trnh (8.4) v (8.5) nhn c khi gii (8.1) ... (8.3).
Tnh lin tc ca vn tc l s m bo cho qu o khng gp khc,
git cc, gy sc trong qu trnh hot ng ca robot. Vn tc v gia tc ti
im cui ca mt on ng cong bc 3 chnh bng vn tc v gia tc ca
on cong bc 3 tip theo.
Cn ch rng khi thit k qu o trong khng gian ct, iu
khin c robot, mi thi im u phi tm c nghim ca bi ton
ng hc ngc. V vy yu cu "no b" ca robot (my tnh) phi thc hin
TS. Phm ng Phc
95
Robot cng nghip
mt khi lng cc php tnh khng l trong mt khong thi gian rt ngn
(vi chc microgiy) m bo thi gian thc khi robot hot ng. Nu ta
khng tm cch ci bin thit k qu o th rt kh m bo yu cu ny.
* V d v thit k qu o CS:
Thit k qu o CS (Path with Cubic segment) ca khp th i i qua
hai im nt c gi tr q0 v qf. Vi cc rng buc q&0 = 0 ; q& f = 0 .
T cc cng thc (8.2) . . . (8.5) ta xc nh cc h s ca a thc bc 3
nh sau :
ai = q0 ; bi = 0;
3(q f q 0 )
- 2(q f q 0 )
ci =
di =
V
2
(t f t 0 )
(t f t 0 ) 3
Do vy qu o qi(t) c dng nh sau :
3(q f q 0 )
2(q f q 0 )
2
q i (t) = q 0 +
t
t
(
)
( t t0 ) 3
0
3
2
(t f t 0 )
(t f t 0 )
6(q f q 0 )
6(q f q 0 )
( t t0 )
( t t0 ) 2
Vn tc l : q& i (t) =
3
2
(t f t 0 )
(t f t 0 )
6(q f q 0 ) 12(q f q 0 )
&q&i (t) =
( t t0 )
V gia tc l :
(t f t 0 ) 2
(t f t 0 ) 3
Trong v d trn, gi s thi gian t0 = 0 v tf = 1 giy, th :
qi(t) = q0 + 3(qf - q0) t2 - 2(qf - q0) t3
qf
q(t)
Qu o
q0
O
t
tf
t0
q& (t)
q& 0 = q& f = 0
tf
t0
6(q f q 0 )
(t f t 0 ) 2
Tc
q&&(t)
Gia tc
t
t0
tf
6(q f q 0 )
(t f t 0 ) 2
Hnh 8.3. Thit k qu o CS

TS. Phm ng Phc
96
Robot cng nghip
T cc phng trnh qu o, phng trnh vn tc v phng trnh gia

tc ta xy dng c cc biu c tnh chuyn ng ca khp th i trn
on qu o thit k.
8.3. Qu o tuyn tnh vi cung hai u l parabol (LSPB) :
Khi yu cu cng c gn trn khu chp hnh cui ca robot chuyn
ng vi vn tc u n, ta dng qu o LSPB.
qi(t)
v = constant
d
(q0+qf)/2
Parabol c
O
t0 tb
Parabol
e
t
tf/2
tf - tb
tf
Hnh 8.3. Qu o LSPB.

Cc iu kin lin tc ca qu o ny th hin :
q(to) = q0 ; q(tf) = qf; v q& (t0 ) = q& (t f ) = 0
v iu kin cng ngh l v = constant.
Qu o c chia lm 3 on :
a/ Trong on 1 : 0 t tb qu o Parabol c dng :
(8.6)
qi(t) = + t + t2
= q(t0) = q0
(8.7)
Khi t = 0 th
(8.8)
Ly o hm (8.6) : q& (t) = + 2 t
Khi t = 0 th
= q&(to ) = 0
Ti thi im tb ta cn c vn tc bng hng s vn tc cho trc v :
= v/2tb
Nn khi t = tb
t v/tb = a = a/2 v qu o c dng :
(0 t tb)
(8.9)
qi(t) = q0 + at2/2
b/ Trong on 2 : [tb, (tf-tb)] qu o tuyn tnh c dng :
qi(t) = 0 + vt
t
(q + q f )
Do tnh i xng : q( f ) = 0
2
2
(q 0 + q f )
t
Suy ra
= 0 + v f
2
2
(q 0 + q f vt f )
0 =
Vy
2
Phng trnh qu o tuyn tnh s l :
TS. Phm ng Phc
97
Robot cng nghip
q f + q 0 vt f
+ vt
2
T iu kin lin tc v v tr, ti thi im tb ta c :
at 2b q f + q 0 vt f
q0 +
=
+ vt b
2
2
Rt ra :
q q f + vt f
tb = 0
v
Vi iu kin tn ti : 0 < tb tf/2, dn n :
q i (t) =
(8.10)
qf q0
2(q f q 0 )
< tf
v
v
iu ny xc nh vn tc phi nm gia cc gii hn trn, nu khng
chuyn ng s khng thc hin c.
V mt vt l :
Nu tf > (qf - q0) / v v tf 2(qf - q0) / v
qf
th : v > (qf - q0) / tf v v 2(qf - q0) / tf.
q0
Ngha l tg < v tg2.
t0
tf
c/ Trong on 3 : (tf - tb) t tf qu o Parabol c dng :

at f2
a
q i (t) = q f
+ at f t t 2
(8.11)
2
2
T cc phng trnh (8.9)...(8.11) ta xy dng c tnh chuyn ng
theo qu o LSPB ca khp qi nh sau :
qf
qi(t); q& (t); q&& (t)

i
i
q0
t0
tb
tf-tb
v = const
t0
tb
tf-tb
tf
q& (t)
i
tf
&q& (t)
i
t0
tb
tf-tb
tf
Hnh 8.4 : c tnh qu o LSPB

TS. Phm ng Phc
98
Robot cng nghip
8.4. Qu o Bang Bang Parabolic blend (BBPB) :

Nh trnh by trn, y l trng hp c bit ca qu o LSPB
khi on tuyn tnh thu v 0.
tf
at 2
qi(t) = q0 +
Vi :
0t
2
2
q q 0 at 2
tf
t tf
qi(t) = 2q0 - qf +2a f
v vi
t2
a
2
th c tnh ca qu o ny nh sau :
qi(t)
qf
q0
t0
q& (t)
i
t
tf/2
tf
Vmax
t
t0
tf/2
tf
&q& (t)
i
t0
tf/2
t
tf
Hnh 8.5. c tnh qu o BBPB
=======================
TS. Phm ng Phc
99
robot cng nghip
Chng 9
Truyn ng v iu khin robot.
9.1. Truyn ng in trong robot:
Truyn ng in c dng kh nhiu trong k thut robot, v c nhiu
u im nh l iu khin n gin khng phi dng cc b bin i ph, khng
gy bn mi trng, cc loi ng c in hin i c th lp trc tip trn cc
khp quay...
Tuy nhin so vi truyn ng thu lc hoc thu kh th truyn ng in
c cng sut thp v thng thng phi cn thm hp gim tc v thng cc
khu ca robot chuyn ng vi tc thp.
Trong k thut robot, v nguyn tc c th dng ng c in cc loi
khc nhau, nhng trong thc t ch c hai loi c dng nhiu hn c. l
ng c in mt chiu v ng c bc.
Ngy nay, do nhng thnh tu mi trong nghin cu iu khin ng c
in xoay chiu, nn cng c xu hng chuyn sang s dng ng c in xoay
chiu trnh phi trang b thm b ngun in mt chiu. Ngoi ra, loi ng
c in mt chiu khng chi gp (DC brushless motor) cng bt u c ng
dng vo k thut robot.
9.1.1. ng c in mt chiu :
ng c in mt chiu gm c hai phn :
+ Stato c nh vi cc cun dy c dng in cm hoc dng nam chm
vnh cu. Phn ny cn c gi l phn cm. Phn cm to nn t thng trong
khe h khng kh.
+ Roto vi cc thanh dn. Khi c dng in mt chiu chy qua v vi
dng t thng xc nh, roto s quay. Phn ny gi l phn ng.
Tu cch u dy gia phn cm so vi phn ng, ta c nhng loi ng
c in mt chiu khc nhau :
+ ng c kch t ni tip (Hnh 9.1.a);
+ ng c kch t song song (Hnh 9.1.b);
+ ng c kch t hn hp (Hnh 9.1.c).
a/
b/
Hnh 9.1. Cc loi ng c in mt chiu.
TS. Phm ng Phc
c/
100
robot cng nghip
Cc thng s ch yu quyt nh tnh nng lm vic ca ng c in mt

chiu l :
U : in p cung cp cho phn ng;
I : Cng dng in ca phn ng;
r : in tr trong ca phn ng;
: T thng;
E : Sc phn in ng phn ng.
Cc quan h c bn ca ng c in mt chiu l :
E = U - rI = kn
k l h s ph thuc vo c tnh ca dy cun v s thanh dn ca phn
ng.
U Ir
S vng quay ca ng c in mt chiu : n =
k
Mmen ng C xc nh t phng trnh cn bng cng sut :
EI = 2nC
k I
C=2
Hay :
Mun iu chnh tc ng c in mt chiu c th thc hin bng

cch :
- Thay i t thng , thng qua vic iu chnh in p dng kch t.
Trong trng hp gi nguyn in p phn ng U, tng tc t 0 n tc
nh mc, th cng sut khng i cn momen gim theo tc .
- iu chnh in p phn ng. Trong trng hp t thng khng i, khi
tng tc t 0 n tc nh mc th mmen s khng i, cn cng sut
tng theo tc .
Mun o chiu quay ca ng c in mt chiu cn thay i hoc chiu
ca t thng (tc chiu ca dng in kch t) hoc thay i chiu dng in
phn ng.
9.1.2. ng c bc :
Nguyn tc hot ng :
Trn hnh 9.2 l s ng c bc loi n gin nht dng nam chm
vnh cu gm stato c 4 cc v roto c 2 cc.
'
N
S
'
'
Hnh 9.2 : S nguyn l hot ng ca ng c bc.

TS. Phm ng Phc
'
robot cng nghip
101
Nu cp in cho cun dy ' th roto s dng v tr m dng t qua

cun dy l ln nht. Nu cp in cho cun dy ' th roto s quay i 900
(Ph thuc chiu dng in cp vo). Khi ng thi cp in cho c 2 cun dy
v th roto s dng v tr gia 00 v 900, v nu dng in vo 2 cun dy
hon ton nh nhau th roto s dng v tr 450.
Nh vy v tr ca roto ph thuc vo s cc c cp in trn stato v
chiu ca dng in cp vo.
Trn y l s nguyn l ca ng c bc loi c t cc v dng nam
chm vnh cu. Trn c s ta c th tm hiu cc loi ng c c nhiu cc v
dng nam chm in c t tnh thay i.
Nh vy tu theo cch cp in cho cc cun dy trn stato ta c th iu
khin cc v tr dng ca roto. Vic cp in cho cc cun dy c th s ho, cho
nn c th ni ng c bc l loi ng c in chuyn cc tn hiu s u vo
thnh chuyn ng c hc tng nc u ra.
u nhc im :
+ Khi dng ng c bc khng cn mch phn hi cho c iu khin v
tr v vn tc.
+ Thch hp vi cc thit b iu khin s. Vi kh nng iu khin s
trc tip, ng c bc tr thnh thng dng trong cc thit b c in t hin
i.
Tuy nhin phm vi ng dng ng c bc l vng cng sut nh v
trung bnh. Vic nghin cu nng cao cng sut ng c bc ang l vn rt
c quan tm hin nay. Ngoi ra, ni chung hiu sut ca ng c bc thp
hn cc loi ng c khc.
Cc thng s ch yu ca ng c bc :
Gc quay :
ng c bc quay mt gc xc nh ng vi mi xung kch thch. Gc
bc cng nh th phn gii v tr cng cao. S bc s l mt thng s quan
trng :
3600
s=
Tc quay v tn s xung :
Tc quay ca ng c bc ph thuc vo s bc trong mt giy. i
vi hu ht cc ng c bc, s xung cp cho ng c bng s bc (tnh theo
pht) nn tc c th tnh theo tn s xung f. Tc quay ca ng c bc
tnh theo cng thc sau :
60 f
n=
(f : bc/pht)/(s : bc /vng)
s
Tong : n - tc quay (vng/pht)
f - tn s xung (Hz)
s - S bc trong mt vng quay.
TS. Phm ng Phc
robot cng nghip
102
Ngoi ra cn cc thng s quan trng khc nh chnh xc v tr,

momen v qun tnh ca ng c...
Cc loi ng c bc :
Tu theo kiu ca roto, ng c bc c chia thnh cc loi sau :
+ ng c bc kiu t tr bin i (VR : Variable Resistance)
+ ng c bc nam chm vnh cu (PM : Permanent Magnet )
+ ng c bc kiu lai (Hybrid)
Tu theo s cun dy c lp trn stato ng c bc c chia thnh cc
loi : 2 pha, 3 pha hoc 4 pha.
Roto ng c bc c nhiu cc (cn gi l rng). S cc ca roto phi
hp vi s cc ca stato xc nh gi tr gc bc . Gc bc ln nht l 900
ng vi ng c c s bc s = 4 bc/vng. Phn ln nhng ng c bc hin
nay c s bc s = 200, nn = 1,80.
S bc cng ln phn gii cng cao v nh v cng chnh xc. Nhng
trong thc t, khng th tng s bc ln qu cao. Tuy nhin c th dng cng
ngh to bc nh chia bc thnh 2 na bc (nh hnh b/ 9.2) hoc t 10
n 125 bc nh. Cng ngh to bc nh cn gi l to vi bc, ch n gin
l m rng phng php ni trn cho nhiu v tr trung gian bng cch cung cp
nhng gi tr dng khc nhau cho mi cun dy. ng c c to bc nh c
phn gii tinh hn nhiu. V d, nu phn 125 bc nh trong mt bc y,
vi 200 bc/vng th phn gii ca ng c l 125 x 200 = 25.000 bc nh/
vng.
9.2. Truyn ng kh nn v thu lc :
Ngoi truyn ng in, trong k thut robot cn thng dng cc loi
truyn ng kh nn hoc thu lc.
9.2.1. Truyn dn ng kh nn :
Dng kh nn trong h truyn ng robot nhiu thun li nh : Do cc
phn xng cng nghip thng c mng li kh nn chung, nn n gin ho
c phn thit b ngun ng lc cho robot. H truyn dn kh nn tng i gn
nh, d s dng, d o chiu, ... Tuy nhin h truyn dn kh nn cng c nhiu
nhc im nh : do tnh nn c ca cht kh nn chuyn ng thng km
theo dao ng, dng khng chnh xc, ngoi ra cn cn trang b thm cc thit b
phun du bi trn, lc bi, gim ting n ...
9.2.2. Truyn dn ng thu lc :
H truyn dn thu lc c nhng u im nh : Ti trng ln, qun tnh
b, d thay i chuyn ng, d iu khin t ng.
Tuy nhin chng cng c nhng nhc im nh : H thu lc lun i
hi b ngun, bao gm thng du, bm thu lc, thit b lc, bnh tch du, cc
TS. Phm ng Phc
103
robot cng nghip
loi van iu chnh, ng ng ... lm h truyn ng cho robot kh cng knh

so vi truyn ng kh nn v truyn ng in.
Nhn chung, h truyn dn thu lc vn c s dng kh ph bin trong
robot, nht l trong trng hp ti nng.
Cc phn t trong h truyn ng bng kh nn v thu lc c tiu
chun ho.
Cc tnh ton thit k h truyn dn kh nn v thu lc c nghin
cu trong cc gio trnh ring.
9.3. Cc phng php iu khin Robot :
Nhim v quan trng u tin ca vic iu khin robot l bo m cho
im tc ng cui E (End-effector) ca tay my dch chuyn bm theo mt qu
o nh trc. Khng nhng th, h to gn trn khu chp hnh cui cn
phi m bo hng trong qu trnh di chuyn. Gii bi ton ngc phng trnh
ng hc ta c th gii quyt v mt ng hc yu cu trn. cng l ni dung
c bn xy dng chng trnh iu khin v tr cho robot.
Tuy nhin vic gii bi ton ny cha xt ti iu kin thc t khi robot
lm vic, nh l cc tc ng ca momen lc, ma st ... Tu theo yu cu nng
cao cht lng iu khin ( chnh xc) m ta cn tnh n nh hng ca cc
yu t trn, v theo , phng php iu khin cng tr nn a dng v phong
ph hn.
9.3.1. iu khin t l sai lch (PE : Propotional Error):
Nguyn tc c bn ca phng php ny rt d hiu; l lm cho h
thng thay i theo chiu hung c sai lch nh nht. Hm sai lch c th l =
d - (t), y d l gc quay mong mun v (t) l gi tr quay thc t ca bin
khp, ta s gi d l "gc t". Khi = 0 th khp t c v tr mong mun.
Nu < 0, th khp di chuyn qu mc v cn chuyn ng ngc li. Nh
vy, kiu iu khin chuyn ng ny l lun c chiu hng lm cho sai lch
xp x zero.
Bn cnh , chng ta cng cn quan tm n phn ln, ngha l,
chng ta khng nhng cn bit "lm cho ng c chuyn ng bng cch no?"
m cn cn bit "cn cung cp cho ng c mt nng lng (mmen ng) l
bao nhiu?". tr li cu hi ny mt ln na, chng ta c th dng tn hiu sai
s = d - . Chng ta hy p dng mt tn hiu iu khin m n t l vi :
F = Kp(d - (t))
(9.1)
Qui lut ny xc nh mt h iu khin phn hi v c gi l h iu

khin t l sai lch.
TS. Phm ng Phc
104
robot cng nghip
9.3.2. iu khin t l - o hm (PD : Propotional Derivative):

Phng php iu khin t l sai lch cn nhiu nhc im nh : H dao
ng ln khi ma st nh (tnh trng vt qu) v trng thi tnh, khi 0 th
momen cng gn bng khng, nn khng gi c v tr di tc dng ca ti.
khc phc iu trn, c th chn phng php iu khin t l - o
hm (PD), vi lc tng qut :
F = K p + K d & (t)
Trong :
(9.2)
- sai s v tr ca khp ng. = d - (t).

& (t) - Thnh phn o hm - vn tc gc.
Ke - H s t l sai lch v tr.
Kd - H s t l vn tc.
9.3.3. iu khin t l - tch phn - o hm

(PID : Propotional Integral Derivative):
H thng vi cu trc lut iu khin PD vn cn mt s nhc im,
khng ph hp vi mt s loi robot. Mt h thng iu khin khc c b sung
thm tn hiu tc t & d v sai lch tc & = & d & (t) tc ng vo khu
khuych i Kd. Phng trnh lc tc ng ln khp ng c dng :
t
F = K e + K d & + K i (t)dt
(9.3)
Vi
& - sai s tc . & = & d & (t) .
Nh vy, tu theo cu trc la chn ca b iu khin, ta em i

chiu cc phng trnh(9.1), (9.2) hoc (9.3) vi phng trnh Lagrange - Euler,
T nhn c cc phng trnh ca h iu khin tng ng. T cc phng
trnh ny ca h iu khin, cn xc nh cc h s t l Ke, Kd, Ki h hot
ng n nh.
9.3.4. Hm truyn chuyn ng ca mi khp ng :
Ni dung phn ny trnh by phng php xy dng hm truyn i vi
trng hp chuyn ng mt bc t do, mi khp thng c iu khin bng
mt h truyn ng ring. Ph bin hn c l ng c in mt chiu.
Xt s truyn ng ca ng c in mt chiu vi tn hiu vo l in
p Ua t vo phn ng, tn hiu ra l gc quay m ca trc ng c; ng c
kiu kch t c lp.
TS. Phm ng Phc
105
robot cng nghip
La
Ra
+
eb(t)
Lf
Ua(t)
_
+
Uf
Rt
ia(t)
m
_
Mm
Jm
Hnh 9.3. S ng c in mt chiu.

Trong thc t, trc ng c c ni vi hp gim tc ri ti trc ph ti
nh hnh 9.4. Gi n l t s truyn, L l gc quay ca trc ph ti, ta c :
L(t) = n m(t)
& L (t) = n & m (t )
(9.4)
&& (t) = n && (t )
L
ML
JL
Mm
fL
Jm
m= L/n
fm
Hnh 9.4. S ng c in cng ph ti.

Mmen trn trc ng c bng tng momen cn ng c quay, cng
vi mmen ph ti quy v trc ng c.
K hiu :
M(t) = M m (t) + M *L (t)

Jm : Mmen qun tnh ca ng c.
JL : Momen qun tnh ph ti.
Ta c :
M m (t) = J m&&m (t) + f m&m (t)

M (t) = J && (t) + f & (t)
L
(9.5)
(9.6)
(9.7)
Trong fm v fL l h s cn ca ng c v ca ph ti.
Theo nh lut bo tn nng lng, cng do ph ti sinh ra, tnh trn trc
ph ti l MLL phi bng cng quy v trc ng c M *L m . T ta c :
TS. Phm ng Phc
106
robot cng nghip
M L (t) L (t)
= nM L (t)
m (t)
Thay (9.1) v (9.4) vo cng thc trn :
M *L (t) =
M *L (t) = n 2 [J L&&m (t) + f L & m (t)]

Thay (9.3) v (9.6) vo (9.2) ta c :
M(t) = ( J m + n 2 J L )&&m (t) + f m n 2 f L )& m (t)
M(t) = J&& (t) + f & (t)
Hay :
m
(9.8)
(9.9)
(9.10)
Vi :
: Mmen qun tnh tng hiu dng.
J = Jm + n2JL
2
f = fm + n fL
: H s ma st tng hiu dng.
Mmen trn trc ng c ph thuc tuyn tnh vi cng dng in
phn ng v khng ph thuc vo gc quay v vn tc gc, ta c :
(9.11)
M(t) = Kaia(t)
ia : Cng dng in phn ng.
Ka : H s t l mmen.
p dng nh lut Kirchhoff cho mch in phn ng :
di (t)
U a (t) = R a i a (t) + L a a + e b (t)
dt
Vi
Ra, La : in tr v in cm phn ng.
eb : sc phn in ng ca ng c.
e b (t) = K b & m (t)
Kb : h s t l ca sc phn in ng.
S dng php bin i Laplace, t (9.12) ta c :
U (s) - sK b m (s)
I a (s) = a
R a + sL a
T (9.10) v (9.11) ta c :
M(s) = s2Jm(s) + sfm(s) = KaIa(s)
K I (s)
m (s) = 2 a a
s J + sf
Thay (9.14) vo (9.15) :
U (s) - sK b m ( s )
m (s) = K a 2 a
(s J + sf)(R a + sLa )
Vi
TS. Phm ng Phc
U a (s) - sK b m ( s ) (s 2 J + sf)(R a + sLa )

=
m (s)
Ka
(9.12)
(9.13)
(9.14)
(9.15)
107
robot cng nghip
U a (s) (s 2 J + sf)(R a + sL a ) + sK a K b
=
m (s)
Ka
m (s)
Ka
=
U a (s) s[(sJ + f)(R a + sL a ) + K a K b ]
Hay :
(9.16)
y l hm truyn cn xc nh, n l t s gia tn hiu ra (gc quay m)

v tn hiu vo ca h thng (in p Ua). V h thng gm c ng c v ph ti
nn tn hiu ra thc t l gc quay ca trc ph ti L, do hm truyn chuyn
ng 1 bc t do ca tay my l :
L (s)
nK a
=
U a (s) s[(R a + sL a )(sJ + f) + K a K b ]
(9.17)
v ta c s khi tng ng vi hm truyn trn l :

Ua(s)
+ _
Ka
sLa+ Ra
1
sJ + f
1
s
L(s)
Kb
Hnh 9.5 : S khi hm truyn chuyn ng mt bc t do.

Trong cng thc (9.17) c th b qua thnh phn in cm phn ng La, v
n thng qu nh so vi cc nhn t nh hng c kh khc. Nn :
L (s)
nK a
=
U a (s) s(sR a J + R a f + K a K b )
(9.18)
9.3.6. iu khin v tr mi khp ng :

Mc ch ca iu khin v tr l lm sao cho ng c chuyn dch khp
ng i mt gc bng gc quay tnh ton m bo qu o chn trc
(chng 8). Vic iu khin c thc hin nh sau : Theo tn hiu sai lch gia
gi tr thc t v gi tr tnh ton ca v tr gc m iu chnh in p Ua(t) t
vo ng c. Ni cch khc, iu khin ng c theo qu o mong mun
phi t vo ng c mt in p t l thun vi sai lch gc quay ca khp
ng.
Trong
~
K p e(t) K p ( L (t) L (t))
U a (t) =
=
n
n
Kp : h s truyn tn hiu phn hi v tr.
TS. Phm ng Phc
(9.19)
108
robot cng nghip
~
e(t) = L (t) L (t) : sai lch gc quay.
~
Gi tr gc quay tc thi : L (t) c o bng cm bin quang hc hoc
chit p. Bin i Laplace phng trnh (9.18) :
~
K p ( L (s) L (s)) K p E(s)
U a (s) =
=
n
n
(9.20)
Thay (9.20) vo phng trnh (9.18) :

Ka Kp
L (s)
=
= G(s)
E(s) s(sR a J + R a f + K a K b )
(9.21)
Sau khi bin i i s ta c hm truyn :

Ka Kp
L (s)
G(s)
=
= 2
=
~
L (s) 1 + G(s) s R a J + s(R a f + K a K b ) + K a K b
Ka Kp / R aJ
(9.22)
(R a f + K a K b )
Ka Kb
2
s +
s+
R aJ
R aJ
Phng trnh (9.22) cho thy rng h iu khin t l ca mt khp ng l
mt h bc hai, n s lun n nh nu cc h s ca ca phng trnh bc hai l
nhng s dng. nng cao c tnh ng lc hc v gim sai s trng thi n
nh ca h ngi ta c th tng h s phn hi v tr Kp v kt hp lm gim dao
ng trong h bng cch thm vo thnh phn o hm ca sai s v tr. Vi vic
thm phn hi ny, in p t ln ng c s t l tuyn tnh vi sai s v tr v
o hm ca n :
~
~
K p ( L (t) L (t)) + K v ( & L (t) & L (t)) K p e(t) + K v e& (t)
(9.23)
U a (t) =
=
n
n
Trong Kv l h s phn hi ca sai s v vn tc.
Vi phn hi nu trn, h thng tr thnh khp kn v c hm truyn nh
th hin trn s khi hnh (9.6). y l phng php iu khin t l - o
hm.
L(s)
Kp+ sKv
_
1
n
Ua(s)
+ _
1
sLa+ Ra
Ka
1
sJ + f
1
s
L(s)
Kb
Hnh 9.6 : S khi iu khin chuyn dch mt khp ng c lin h phn hi.
TS. Phm ng Phc
109
robot cng nghip
Bin i Laplace phng trnh (9.23) v thay Ua(s) vo (9.21) ta c :
K a (K p + sK v )
K a K vs + K a K p
L (s)
=
=
= G(s)
E(s) s(sR a J + R a f + K a K b ) s(sR a J + R a f + K a K b )
(9.24)
T ta c :
K a (K v s + K p )
L (s)
G(s)
=
=
~
L (s) 1 + G(s) s 2 R a J + s(R a f + K a K b + K a K v ) + K a K p
(9.25)
TS. Phm ng Phc
Chng 1
TNG QUAN V ROBOT

1.1. Lch s pht trin Robot.
Khi nim Robot ra i u tin vo ngy 09/10/1922 ti NewYork, khi
nh son kch ngi Tip Kh Karen Kapek tng tng ra mt c my hot
ng mt cch t ng, n l nim m c ca con ngi lc .
T tng thit k, ch to Robot lun thi thc con ngi. n
nm 1948, ti phng th nghim quc gia Argonne, Goertz ch to thnh
cng tay my i (master-slave manipulator). n nm 1954, Goertz ch to
tay my i s dng ng c servo v c th nhn bit c lc tc ng ln
khu cui.
Nm 1956 hng Generall Mills ch to tay my hot ng trong vic
thm him di dng.
Nm 1968 R.S. Mosher, ca General Electric ch to mt c my bit
i bng 4 chn. H thng vn hnh bi ng c t trong v mi chn vn hnh
bi mt h thng servo thy lc.
Nm 1969, i hc Stanford thit k c Robot t hnh nh nhn
dng hnh nh.
Hnh 1.1 Robot Shakey

Nm 1970 con ngi ch to c Robot t hnh Lunokohod, thm
him b mt ca mt trng.
Trong giai on ny, nhiu nc khc cng tin hnh cng tc nghin
cu tng t, to ra cc Robot iu khin bng my tnh c lp t cc loi cm
bin v thit b giao tip ngi v my.
Hnh 1.2. Robot hn im

(Ngun KUKA, Inc)
Hnh 1.3. Robot phu thut

(Ngun Accury, Inc)
Theo s tin b ca khoa hc k thut, cc Robot ngy cng c ch to

nh gn hn, thc c nhiu chc nng hn, thng minh hn.
Mt lnh vc c nhiu nc quan tm l cc Robot t hnh, cc chuyn
ng ca chng ngy cng a dng, bt chc cc chuyn ng ca chn ngi
hay cc loi ng vt nh : b st, ng vt 4 chn, V cc loi xe Robot
(robocar) nhanh chng c ng dng rng ri trong cc h thng sn xut t
ng linh hot (FMS).
Hnh 1.4. Mobile Robot v ng dng cng ngh x l nh (Ngun SRI,

Stanford, CA)
T tr i con ngi lin tc nghin cu pht trin Robot ng dng
trong qut trnh t ng ho sn xut tng hiu qu kinh doanh. Ngoi ra
Robot cn c s dng thay cho con ngi trong cc cng vic mi trng
c hi, khc nghit,
Chuyn ngnh khoa hc v robot robotics tr thnh mt lnh vc
rng trong khoa hc, bao gm cc vn cu trc c cu ng hc, ng lc
hc, qu o chuyn ng, cht lng iu khin Tu thuc vo mc ch v
phng thc tip cn, chng ta c th tm hiu lnh vc ny nhiu kha cnh
khc nhau.
Hin nay, c th phn bit cc loi Robot hai mng chnh : Cc loi
robot cng nghip (cnh tay my) v cc loi robot di ng (mobile robot). Mi
loi c cc ng dng cng nh c tnh khc nhau. Ngoi ra, trong cc loi
robot cng nghip cn c phn chia da vo cu to ng hc ca n : Robot

ni tip (series robot) v robot song song (parallel robot).
Hnh 1.5. Robot song song 6 bc t do Merlet.( Ngun : Dr. J. - P. Merlet v

Prof. V. Hayward.)
Chnh cng ngh tin tin tt c cc lnh vc : c kh, vi mch, iu
khin, cng ngh thng tin to ra nn tng cng nh nhng thch thc
ln i vi khoa hc nghin cu robot. Chnh v vy, con ngi v ang tip
tc pht trin v nng cao mc hon thin trong lnh vc y hp dn ny.
Hnh 1.6. Nguyn bn ca Robot Hexapod TU Munich ( Ngun : Prof. F.

Pfeiffer, TSI Enterprises, Inc.)
1.2. Cc ng dng ca Robot.
1.2.1. Cc u im khi s dng Robot.
Cc loi Robot tham gia vo qui trnh sn xut cng nh trong i sng
sinh hot ca con ngi, nhm nng cao nng sut lao ng ca dy chuyn
cng ngh, gim gi thnh sn phm, nng cao cht lng cng nh kh nng
cnh tranh ca sn phm to ra.
Robot c th thay th con ngi lm vic n nh bng cc thao tc n
gin v hp l, ng thi c kh nng thay i cng vic thch nghi vi s
thay i ca qui trnh cng ngh.
S thay th hp l ca robot cn gp phn gim gi thnh sn phm, tit
kim nhn cng nhng nc m ngun nhn cng l rt t hoc chi ph cao
nh : Nht Bn, cc nc Ty u, Hoa K
Tt nhin ngun nng lng t robot l rt ln, chnh v vy nu c nhu
cu tng nng sut th cn c s h tr ca chng mi thay th c sc lao
ng ca con ngi. Chng c th lm nhng cng vic n gin nhng d
nhm ln, nhm chn.
Robot c kh nng nghe c siu m, cm nhn c t trng
Bn cnh , mt u im ni bc ca robot l mi trng lm vic.
Chng c th thay con ngi lm vic nhng mi trng c hi, m t, bi
bm hay nguy him. nhng ni nh cc nh my ho cht, cc nh my
phng x, trong lng i dng, hay cc hnh tinh khc th vic ng dng
robot ci thin iu kin lm vic l rt hu dng.
5
1.2.2. M s lnh vc ng dng.

a. ng dng trong cc lnh vc sn xut c kh.
Trong lnh vc c kh, robot c ng dng kh ph bin nh kh nng
hot ng chnh xc v tnh linh hot cao.
Cc loi robot hn l mt ng dng quan trng trong cc nh my sn xut
t, sn xut cc loi v bc c kh
Hnh 1.7. Robot hn trong cng ngh sn xut c kh.

Ngoi ra ngi ta cn s dng robot phc v cho cc cng ngh c, mt
mi trng nng bc, bi bm v cc thao tc lun i hi tin cy.
c bit trong cc h thng sn xut linh hot (FMS), Robot ng vai tr
rt quan trng trong vic vn chuyn v kt ni cc cng on sn xut vi
nhau.
Hnh 1.8. ng dng Robot trong cc h thng sn xut linh hot.

b. ng dng trong lnh vc gia cng lp rp.
Cc thao tc ny thng c t ng ho bi cc robot c gia cng
chnh xc v mc tin cy cao
6
Hnh 1.9. Robot c s dng trong cng on cp liu v lp rp.

c. ng dng trong cc h thng y hc, qun s, kho st a cht.
Ngy nay, vic s dng cc tin ch t Robot n cc lnh vc qun s, y
t, rt c quan tm. Nh kh nng hot ng n nh v chnh xc, Robot
c bit l tay my c dng trong k thut d tm, b phng, v trong cc ca
phu thut y khoa vi tin cy cao.
Hnh 1.10. Cc ng dng Robot trong cc lnh vc thm him, qun s, v tinh
Ngoi ra, tu thuc vo cc ng dng c th khc m Robot c thit k
phc v cho cc mc ch khc nhau, tn dng c cc u im ln ca
chng ng thi th hin kh nng cng ngh trong qu trnh lm vic.
1.3. Cc khi nim v Robot Robot cng nghip.
Lnh vc nghin cu v Robot hin nay rt a dng v phong ph. Trong
7
ti liu ny, chng ti ch trnh by cc kin thc ch yu trn cc loi Robot

cng nghip, tc cc cnh tay my. Cc bi ton cn bng lc, cc phng trnh
ng hc v ng lc hc l nhng nn tng c bn cc bn hc vin c th
tip cn vi chuyn nghnh k thut Robot.
1.3.1. nh ngha v robot cng nghip ( Industrial Robot ).
Tu thuc mi quc gia, t chc v mc ch s dng, chng ta c nhiu
nh ngha v robot cng nghip. V vy trong nhiu ti liu khc nhau, nh
ngha v robot cng nghip cng khc nhau. Theo t in Webster nh ngha
robot l my t ng thc hin mt s chc nng ca con ngi. Theo ISO (
International Standards Organization ) th : Robot cng nghip l tay my a
mc tiu, c mt s bc t do, d dng lp trnh v iu khin tr ng, dng
tho lp phi, dng c hoc cc vt dng khc. Do chng trnh thao tc c
th thay i nn thc hin nhiu nhim v a dng. Tuy nhin Robot cng
nghip c nh ngha nh vy cha hon ton tho ng.
Theo tiu chun ca M RIA ( Robot Institute of America ) nh ngha
robot l loi tay my vn nng c th lp li cc chng trnh c thit k
di chuyn vt liu, chi tit, dng c hay cc thit b chuyn dng, thng qua
cc chng trnh chuyn ng c th thay i hon thnh cc nhim v khc
nhau.
Hnh 1.11. Biu din khng gian ca cnh tay my

1.3.2. Cc thnh phn c bn ca ca Robot cng nghip.
S tng quan cu thnh mt Robot cng nghip chuyn dng :
Cm bin
a. Cnh tay Robot (Robot Arm ):

L b phn c kh gm cc khu lin kt vi nhau bi cc khp ni, cc
b truyn ng nh: B truyn bnh rng, b truyn ai, b truyn trc vtbnh v, vt me- ai c
Hnh 1.12. Cnh tay Robot.

b. Ngun ng lc: Cc thit b to chuyn ng cho Robot, c th l cc thit
b kh nn, thu lc, in.
i vi cc chuyn ng cn chnh xc cao, yu cu gn nh ngi ta
c th dng cc loi ngun truyn ng l cc motor bc, cc motor servo.
Hnh 1.13. Cu to ca motor mt loi motor bc.

c. B iu khin ( Controller ):
L thnh phn quan trng quyt nh kh nng hot ng v chnh xc
ca Robot. B phn ny thng thng c tch hp di dng cc board mch
iu khin, c th c cc loi sau:
IC diu khin trung tm (CPU) kt hp vi cc card iu khin phn theo
modul.
Cc thit b iu khin Robot s dng PLC ( Programable Logic Controller
).
S dng cc b iu khin PMAC ( Programable Multi-Axies Controller ).
Cc b iu khin thit k theo cc dng iu khin hin i nh : B iu
khin m, b iu khin theo mng neuron
d. Cm bin ( Sensor ):
L thit b chuyn cc i lng vt l thnh cc tn hiu in cung cp
cho h thng nhm nng cao kh nng linh hot v chnh xc trong iu
khin. Nh vy Robot chnh l mt h thng iu khin kn vi vng hi tip (
Feedback ) c thc hin t tn hiu thu v t cm bin.Cc loi cm bin
thng gp nh:
Cm bin quang
Cm bin v tr v dch chuyn.
Cm bin o gc.
Cm bin vn tc.
Cm bin gia tc v rung.
Cm bin lc v bin dng.
Cc cm bin trn c th cho tn hiu tng t Analogue hoc tn hiu s (
Digital ), ngoi ra cn s dng cc b m ho v tr, m ho gc dch chuyn
Encoder, Resolver
e. Cc chng trnh:
Cc chng trnh lun tng thch vi cc b iu khin. Chnh v vy cc
loi ngn ng vit chng trnh iu khin cho Robot cng kha a dng, c
th l ngn ng vit cho vi x l (ngn ng my ), ngn ng vit cho PLC
(thuc cc hng khc nhau ), hay cc ngn ng trn my tnh nh: Pascal, C,
C++, Visual Basic, Matlab
10
1.3.3. Bc t do ca Robot cng nghip.

a. Khi nim:
Bc t do l s kh nng chuyn ng ca mt c cu dch chuyn
c mt vt th no trong khng gian. C cu chp hnh ca robot phi t
c mt s bc t do nht nh. Ni chung, c h ca mt robot l mt c cu
h ( l c cu c mt khu ni gi ).
Chuyn ng ca cc khu trong robot thng l mt trong hai khu
chuyn ng c bn l tnh tin hay chuyn ng quay.
b. Xc nh s bc t do ca robot (DOF- Defree Of Freedom).
S bc t do ca robot c xc nh:
W= 6n - i.Pi
W: S bc t do ca robot.
n: S khu ng.
Pi: S khp loi i.
Trong , khp loi i l khp khng ch i bc t do.
Hnh 1.14. Robot PUMA 6 bc t do.

V d: Xc nh s bc t do ca robot sau:
11
Hnh 1.15. Bc t do ca robot

Xc nh c s khp loi 5 l 5 (4 khp quay v mt khp tnh tin ), do
n=5 v P5 =5 nn s bc t do ca robot ny: W= 6.5 5.5 = 5 bc.
Lu :
Hu ht robot s dng khp loi 5 ( khng ch 5 bc t do, chuyn ng
quay hoc tnh tin ). V vy s bc t do ca n cng chnh l s khu ng,
robot c bc t do cng cao th cng linh hot.
Thng thng 3 bc t do u dng nh v, cc bc t do sau nh
hng.
1.3.4. H to trong robot.
Mi robot thng bao gm nhiu khu lin kt vi nhau ( links ) thng
qua cc khp ( joints ) to thnh mt xch ng hc xut pht t mt khu c
bn ng yn. H to gn vi khu c bn gi l h to c bn ( hay h
to chun ).
Cc h to trung gian khc gn vi cc khu ng gi l h to suy
rng.
Ti tng thi im hot ng cc to suy rng
xc nh cu hnh ca robot bng cc chuyn dch di
hoc cc chuyn dch gc ca cc khp tnh tin hoc
khp quay. Cc to suy rng cn li l cc bin
khp.
Tt c cc h to dng trong robot phi tun
theo qui tc bn tay phi : Dng bn tay phi co hai
ngn t v p t, ngn ci tr theo phng din trc z,
ngn tr theo phng din trc x, ngn gia hng trc
y.
d2
z0
0
x0
y0
12
o4
y4
z4
x4
Hnh 1.16. . H to ca robot c n khu.

Cc gc quay 1, 3, 4, 5 v dch chuyn di d2 l cc to suy rng
( cc bin khp ).
kho st ng hc robot ta phi gn trn mi khu ca robot mt h to
. Nguyn tc chung gn h to s c trnh by trong chng III trong
khi xt n phng trnh ng hc ca robot v b thng s DenavitHartenberg.
V d: Xc nh to cho robot SCARA (Robot c 4 bc t do ).
d3
o0
x0
y0
z0
o1
x1
y1
z1
o2
y2
z2
x3
o3
y3
z3
o4
y4
x2
x4
z4
Hnh 1.17. Xc nh to cho cc khu ca Robot Scara.

1.4. Phn loi Robot.
1.4.1. Robot cng nghip.
1. Robot ni tip (series robot).
Thc cht loi Robot ny chnh l cc loi tay my, cc khu v khp ni
ca chng c thit kt lin tip nhau hnh thnh nn cc qu o chuyn
ng nht nh. i vi loi robot ny, chng ta c nhiu cch phn loi khc
nhau :
a. Phn loi theo kiu kt cu.
Robot kiu to cc.
Tay my c 3 chuyn ng tnh
tin theo 3 phng ca h ta
cc trong khng gian.
13
Thng ng dng loi robot ny trong vic vn chuyn phi liu, lp rp,
hn trong mt phng
Hnh 1.18. Robot kiu to cc
Robot kiu to tr.
Vng lm vic ca robot
ny c dng hnh tr rng
Robot Versatran (hng
AFM, Hoa K) l mt robot
thuc loi ny.
Hnh 1.19. Robot kiu to tr

Robot kiu to cu.
Vng lm vic ca robot c
dng hnh cu.
C hai loi cu hnh chnh
thuc kiu robot ny : 3 khp quay
(RRR) 2 khp quay, 1 khp tnh tin
khu cui (RRT)
Hnh 1.20. Robot kiu to cu

Robot kiu Scara.
Robot c cu trc theo kiu
Scada ra i t nm 1979, ti trng
i hc Yamanashi (Nht Bn).
Robot lai ny thng c ng
dng trong cc lnh vc lp rp, vi
cu hnh ca 3 khu u tin l : RRT
Hnh 1.21. Robot kiu Scara.
b. Phn loi theo ngun truyn ng.
H truyn ng in.
H truyn ng thu lc.
H truyn ng kh nn.
c. Phn loi theo cc ng dng.
14
Hnh 1.22. Phn loi cc loi robot chuyn dng. (Ngun : Reis Robotics, ABB
Flexible Automation, CMB Automation)
2. Robot song song (Parallel Robot).
Cc loi Robot thuc nhm ny c cc khu chuyn ng song song tng
i vi nhau. Thng thng chng gm 1 c nh v 1 di ng.
Hnh 1.23. Mt sn phm robot song song (Ngun : PRSCs)

Tu thuc vo s lng cc nhnh ca robot song song m ta c th phn
loi chng vi nhau. Mt loi robot song song c 6 nhnh c s dng rt ph
bin l Hexapod.
1.4.2. Robot di ng (Mobile Robot).
15
y l h Robot c nhiu tnh nng thng minh v linh hot trong qu

trnh ng dng nh kh nng di chuyn c theo lp trnh.
Hnh 1.24. Mobile robot ng dng trong cc lnh vc khc nhau.

H thng mobile robot l lnh vc tht s hp dn i vi cc nh nghin
cu cng nh nhng ngi quan tm, khng ch nh nhng u im ni bc
ca n m cn tnh a dng trong ng dng.
Phn tch ng hc v ng lc hc mobile robot l nhng bi ton c
mc phc tp khc nhau, n tu thuc vo kt cu ca robot cng nh yu
cu v chnh xc, tnh thng minh trong x l tnh hung
Chng ta xem xt mt vi chuyn ng m con ngi mong mun thit k
cc loi mobile robot
+ Chuyn ng theo dng trn :
+ Chuyn ng slide ca cc loi ng vt b st.
+ Chuyn ng chy ca ng vt 4 chn.
+ Chuyn ng i b ca con ngi.
16
Ngy nay con ngi hin thc ho c cc tng ny, mc d mc

chnh xc, tin cy ca mi loi, mi hng sn xut l khc nhau.
Hnh 1.25. Robot chuyn ng bn chn
Hnh 1.26. Mobile Robot tc v
(Ngun : AIBO, SONY, Nht Bn)

Bn)
(Ngun: SDR-4X, SONY, Nht
17
Chng 2
PHN TCH H C CN BNG TNH V

CHUYN NG CA TAY MY
2.1. Cc khi nim c bn v tin tnh hc
2.1.1. Trng thi cn bng
H vt c xem nh trng thi cn bng khi tng cc ngoi lc tc ng
ln n bng khng. Lc y h vt hoc ng yn hoc chuyn ng thng u
i vi h qui chiu .
Trong thc t lun tn ti lc ma st nn khi h vt t trng thi cn bng
th n ng yn.
2.1.2. Lc
Lc c trng cho tc dng c hc ca vt th ny ln vt th khc
Lc c biu din bng mt vector {phng, chiu, ln, im t}
Trong h trc {x,y,z} th lc F ( Fx , Fy , Fz )

2.1.3. Mmen ca lc i vi tm
Mmen ca lc F t ti A i vi tm O l m0 ( F ) OA F d F
m0 ( F ) c ln bng d.F, im t ti O, phng vung gc vi mt phng
( F , O ) , chiu thun theo chiu xoay ca OA, F

m0 ( F )
2.1.4. Momen ca lc i vi trc ()

Tch F F// F => m0 ( F ) dF

Vy momen cua lc i vi trc bng tch ca thnh phn hnh chiu vung
gc ca lc (ln mt phng vung gc vi trc) vi khong cch t lc hnh
chiu n trc.
Chiu ca momen hng theo chiu xoay ca lc quanh trc.
18
( )
F//
d
O
2.1.5. H lc
H lc tc dng vo mt vt ang kho st ( Fk ) ( F1 , F2 ,..., Fn )
Hai h lc ( Fk ) ( Ph ) khi chng c cng tc dng c hc
Hp lc ca h lc: R c gi l hp lc ca h lc ( Fk ) khi R Fk
H lc cn bng khi R 0
2.1.6. Cc tin tnh hc
Hai lc cn bng khi chng cng phng, ngc hng, cng ln.
Hp lc ca hai lc l vector lc ng cho ca hnh bnh hnh.
F1

R F1 F2
F2
Khi hai vt tng tc vi nhau, chng tc ln nhau mt lc:

Hai lc tng tc cng phng, cng ln, nhng ngc hng.
im t ca 2 lc nm ngay ti v tr tip xc ca 2 vt v hng vung
gc vi tip tuyn ti im tip xc.
Tip tuyn
Vt t do l vt c th dch chuyn ty trong ln cn b t v tr ang xt.
Ngc li gi l vt khng t do
Vt kho st (S) c qui c gi l vt chu lin kt. Cc vt khc tng
tc c hc vi S c gi l vt gy lin kt.
Vt khng t do c th xem l t do nu ta thay th cc vt gy lin kt
bng cc phn lc lin kt.
V d :
19
M r
m
iu kin cn bng ca h tnh
R0
( Fk ) 0
, trong R l vector hp lc v M 0 l m men
M 0 0
chnh vi tm O ca h ( Fk ) .
Ta c
m
(
F
R
ox
x
k)
x
kx
k
k
R( Rx , R y , Rz ) R y Fky , v M 0 ( M ox , M oy , M oz ) M oy m y ( Fk )
k
k
F
z kz
M oz m z ( Fk )
k
Fkx 0
k
Fky 0
k
Fkz 0
k
Vy iu kin h cn bng tnh l ( F ) 0 m
x ( Fk ) 0
m y ( Fk ) 0
k
m z ( Fk ) 0
k
2.1.7. Mt s m hnh phn lc lin kt

a. Phn lc lin kt mt chiu
20
N1
s
s
b. Lin kt bn l tr
s
c. Lin kt bn l cu
Rx
Rz
Ry
K hiu qui c
d. Lin kt gi
K hiu qui c
c
e. Lin kt thanh
Vi d: Xc nh cc phn lc lin kt ca thanh trong h sau
21
N2
450
A1
A2
m(5kg)
2.1.8. Sc bn ca vt liu
Thanh lin kt
dy
dx
dz
a. Cc tc ng lc ln thanh bao gm:

Lc ko: lm cho thanh dn ra theo hai chiu ca lc
Lc nn: lm cho thanh nn li theo hai chiu ca lc
Lc xon: lm cho thanh vn cong
Di tc ng ca cc ngoi lc mi phn t dv(dx,dy,dz) u chu tc
ng ca cc vector lc, c gi l cc tensor ng sut. Cc vector ng sut
ny c c th hin nh hnh v, theo tng cp vector bng nhau v ln
nhng ngc chiu nhau, (dx, dx , , dy, dy , , dz, dz , ) .
b. Trng thi vt l ca thanh khi b xon
b ko gin
b nn li
Na trn ca thanh c xu
hng b ko gin ra
Na di ca thanh c xu
hng b nn li
22
c. Kh nng chu gin v nn ca cc khi loi

Mi loi vt liu c khn nng chi gin v nn khc nhau, chng c gi l
cc gi tr ti hn nn Fn v gi tr ti hn ko FK . Nhng ni chung kh nng

chu nn tt hn so vi chu gin.
Khi b nn qu mc gii hn kim loi s b bin dng, sau ln bin dng ny
chng s c mt gi tr ti hn Fn khc, ln hn gi tr ban u.
Fn
t
Khi b ko qu mc gii hn kim loi s b bin dng, sau ln bin dng ny
chng s c mt gi tr ti hn Fk khc, nh hn gi tr ban u, v c nh th

cho n khi t ri ra.
Fk
t
2.1.9. Lc ma st
a. nh ngha: Ma st l lc sinh ra do s c st gia hai vt. Vt ny c st
sinh ra lc ma st tc ng ln vt kia v ngc li
1
2
Fm 21
1
2
Fm12
Fm12 Fm 21
Fm12 Fm 21
b. Phn loi: C hai loi ma st, l ma st tnh v ma st ng

Ma st tnh l lc ma st xut hin khi hai vt tip xc nhau nhng cha
chuyn ng
23
Ma st ng l lc ma st xut hin khi hai vt tip xc nhau v c s

chuyn ng tng i gia vt ny vi vt kia
c. Tnh cht ca lc ma st:
Lc ma st t l vi din tch tip xc v tc c st gia hai vt
d. Li im ca lc ma st: dng hm, thng ng c, bnh xe
e. Bt li ca lc ma st
Tn cng v ch
Lc ma st sinh ra nhit lm nng h thng, nng cc im tip xc v qua
thi gian gy h hng thit b (bin dng b mt tip xc)
f. Phng php lm gim bt lc ma st
Gim din tch tip xc (S dng cc khe, cc bnh xe, bac n, con trt)
Gim tc c st (tng tc t t)
S dng cc cht bi trn ni tip xc (nht, m b)
2.2. Thit k h c cn bng tnh
2.2.1. Bc 1: Xc nh cc yu t u vo
i tng ph v: khi lng, kch thc hnh dng, cng
Chu trnh ph v: cc thao tc, tin trnh thc hin v cc to , qu o ca
chu trnh
Khng gian phc v
Ngun nng lng cung cp
2.2.2. Bc 2: Thit k khung c kh
V kt cu hnh hc, xc nh cc khp ng
Xc nh cc ngun lc cho cc khp ng: motor(DC, AC, servo), kh nn,
thy lc
Xc nh h truyn ng cho cc khp: trc tip hay gin tip, v tr t
ngun lc, khi lng cc ngun lc
Ti u ho cc bc a, b, c li v lc v n gin v kt cu
Xc nh vt liu cho cc thanh, dng hnh hc v kch thc
2.2.3. Bc 3: Tnh ton cn bng lc cho h
Xc nh cc phn lc lin kt ca cc thanh
Da trn cc phn lc lin kt, xc dnh cc ngun lc: motor(ngu lc), kh
nn(p sut nn),..
24
Tnh ton cn bng lc cho c h: tnh ton cn bng lc cho cc khu v cho
ti trng
V d: Thit k h cn bng tnh cho cnh tay Robot trong dy chuyn
phn loi sn phm di y
Khu 1
Thanh d2
Khu 2
Thanh d1
Tay gp
dng gic ht
ti trng
M
1m
Bng chuyn A
Bng chuyn B
2m
Bc 1:
a. Vt th M c khi lng 0,5kg, kch thc hnh tr cao 10cm, c nhn mc
nn d try xc
b. Nhc vt M ln, di chuyn t bng chuyn A sang bng chuyn B, h vt B
xung
c. Khong cch gia 2 bng chuyn 2m, chiu cao ca bng chuyn 1m, chiu
cao ca vt M l 10cm
d. Ngun nng lng cung cp kh nn
Bc 2:
a. Kt cu hnh hc nh hnh v
Khp 1: xoay quanh trc
Khp 2: khp trt ln xung
Tay gp: dng gic ht
Thanh 1 c chiu cao: 1m + 0,1m +(chiu di cylinder trt)
Thanh 2 c chiu di: 1m
ti trng c hnh dng v kch thc nh hnh v
25
0.5m
Chn
0.5m
0.25m
0.25m
b. Ngun lc
Khp 1: dng v lng kh nn truyn ng xoay trc tip, khi lng 1kg
Khp 2: dng cylinder kh nn truyn ng trt trc tip, khi lng 1kg
Tay gp: dng van kh nn iu khin gic ht, khi lng 200g
c. Vt liu lm cho cc thanh l Inox
Tay gp: phu gic ht, 8
ti trng: St tm si Inox, dy 5mm, khi lng 7kg
Bc 3:
a. Ho rn ton h, xc nh cc phn lc lin kt ca ti trng, nh hnh v
Do h i xng nn:
Pcylinder
Pvolang
N1 N 4 v N 2 N 3
PT cn bng ca h lc:
Pk N i 0

m
(
P
)
m
(
N
( ) i ) 0
( ) k
Pthanh2
PM
( )
N1
Tnh
cn bng
lc:
N4
Pde
Pthanh1
N3
N2
PM Pcylinder Pthanh2 Pvolang Pthanh1 Pde
N1 N 2 N 3 N 4 0
2 N1 2 N 2 225( N ) (1)
26
Pcylinder
PM
Pvolang
1m
Pt1 _ 2
Pt1 _ 1
2N 2
2N1
( )
Pde
Pthanh1
0.25m
Phng trnh cn bng momen
0.75
0.75 ( PM Pcylinder )
Pt1 _ 1 0.5 2 N 2
2
0.25
Pt1 _ 2 0.25 Pvolang 0.25 ( Pde Pthanh1 ) 0

2
11.25 14.0625 N 2 1.5625 2.5 37.5 0
N 2 16.25( N ) (2)
Thay (2) vo (1) ta c
N1 96.25( N )
Nhn xt: ta thy N 2 >0, nn h cn

bng v ta khng cn thm i trng
cho
b. Xc nh ngun lc cho cc khu

Tay ghp: dng van ht chn khng c p sut
P
m. g
m. g
1K (atm ) , ta chn P = 1.5K(atm)

s
.rM 2
Khu 1: Cylinder kh nn c p sut P 1K (atm), ta cng chn P = 1.5K

(atm)
Khu 2: Volang kh nn c p sut P = 1.5K (atm)
c. p sut ngun kh nn cung cp cho ton h: ta chn 2K(atm)
2.3. Phn tch chuyn ng tay my.

2.3.1. Gii thiu v phn tch chuyn ng
Vi mt h tay my c thit k, vn t ra l lm th no xc
nh qu o ca cc khu trong chu trnh hot ng ca Robot
Vic phn tch chuyn ng ca tay my nhm mc ch tm ra cc qu o
ny, nhng vic thc hin c tin hnh theo hai bc: Xc nh to ca
cc khu trung gian, ri t nh ra qu o ca cc khu.
n gin cho vic phn tch chuyn ng, thit k c kh v u khin
Robot, ta thng n gin ho cc khu mt trong hai dng c bn l khp
trt v khp bn l
Khi nim bc chuyn ng t do th hin cho s khu c trn Robot
27
2.3.2. H to
kho st cho chuyn ng cc khu, ta gn vo y mt h ta
(0xyz). H trc ny c t sao cho n gin cho vic kho st
y4
y3
x4
x2
2
y1
x3
z2
y2
z3
x1
z4
z1
2.3.3. Qu o
m t qu o ca tay my ta th hin thng qua cc ta suy rng
ca cc h ta khu. V d m t qu o ca tay my ti v tr M ca tay
gp (khu cui)
x M x M ( q1x , q2 x ,..., qnx )

y M y M ( q1 y , q2 y ,...., qny )
z M z M ( q1z , q2 z ,...., qnz )
Trong , q1, q2, l

cc ta suy rng,
ng vi chuyn ng
ca cc khu.
2.3.4. Phn tch chuyn ng tng qut ca tay my.

a. Bi ton ng hc thun
M hnh ca bi ton l cho trc c cu v quy lut chuyn ng ca h,
th hin qua cc ta suy rng. Ta phi xc nh quy lut chuyn ng ca
mt v tr xc nh no trong h.
Bi ton ny trong thc t, n thng c dng sau khi gii quyt bi ton
ng hc ngc, xc nh ranh gii chuyn ng v kim tra cn bng ng
ca cc phn t trong h.
b. Bi ton ng hc ngc
M hnh ca bi ton l cho trc c cu v quy lut chuyn ng ca khu
cui, ta phi xc nh quy lut chuyn ng ca cc khu thnh vin, tc l xc
nh cc ta suy rng.
28
Bi ton ny cho v s li gii (v s nghim ca cc qi). Trong thc t khi

gii quyt cc bi ton ny, ta thng thm vo n cc iu kin rng buc ca
chuyn ng tay my cho li gii ti u.
2.3.5. Php bin i h ta
Cho hai h trc ta (Oxyz) v (O1x1y1z1) nh hnh v, i0 , j0 , k0 l cc

vector ch phng n v ca h (Oxyz)
z
z1
a
k0
y1
i0 o
o1
j0
x1
Cho a trong h (Oxyz) c th hin a a x i0 a y j0 a z k0

a
a
cos(
a
,
i
)
a
a
cos(
a
,
j
)
a
a
cos(
a
, k0 )
Vi : x
0
y
0
z
nh l v phpchiu hnh hc
Hnh chiu ca a theo hng u bt k l:

au a x cos(u, x ) a y cos(u, y ) a z cos(u, z )
Vy chiu ca: a ln x1 l a x1 a x cos( x1 , x ) a y cos( x1 , y ) a z cos( x1 , z )

a ln y1 l a y1 a x cos( y1 , x ) a y cos( y1 , y ) a z cos( y1 , z )
a ln z1 l

a z1 a x cos( z1 , x ) a y cos( z1 , y ) a z cos( z1 , z )
Vy trong h ta (O1x1y1z1), a ax1i1 a y1 j1 az1k1

Lp bng Cosin ch hng cho h phng trnh trn ta c
x
x1
y1
..
3
3
3
z1
Gi ma trn cosin ch hng t h ta (Oxyz) vo (O1 x1 y1 z1 ) l
29
1 cos( x1 , x ) , 2 cos( y1 , x ) ,
a x1
ax
1 1 1

MC10 2 2 2 => a y1 MC10 a y

a z
3 3 3
a z1
Tng t nh vy nu trong h ta (O1x1y1z1), a ax1i1 a y1 j1 az1k1

Th trong h ta (Oxyz), s c ma trn cosin ch hng l:
1 2 3
a x
a x
T
a MC T a
MC01 1 2 3 => MC01 MC10
=>
y
y
az
1 2 3
a z

1
10
2.4. Phn tch chuyn ng ca mt s tay my.

2.4.1. Phn tch chuyn ng ca tay my 2 khp quay.
x2
x2
y2
y1
y1
z2
y2
z2
x1
z1
x1
z1
Hnh 1a)
Hnh 2a)
Xt chuyn ng ca mt tay my hai bc t do nh hnh 1a, hnh 2a, gi

s ta ho rn khu 2, cho khu 1 chuyn ng xoay
Ta thy im P trong h ta ca khu 2 khng chuyn ng, nhng
trong h ta ca khu 1 th n chuyn ng.
Ta ca P c tnh da vo hnh 1b) v 2b)
30
x2
r2
x2
y1
d1
z2
r2
r1
r1
d1
y1
y2
z2
y2
x1
x1
1
z1
z1
Hnh 1b)
Hnh 2b)
Vy ta ca P trong h khu 1 l
( r1 )1 (d1 )1 ( r2 )1 MC12 (d1 r2 )2
2.4.2. Phn tch chuyn ng ca tay my ba khp quay.

y3
x2
2
y1
x3
z2
y2
z3
x1
z4
z1
Xem xt m hnh ca tay my ba bc t do nh hnh v trn
31
y3
x2
y1
d2
d1
r3
x3
r1
z2
y2
z3
z4
x1
z1

T m hnh vector ta thy: r1 d1 d 2 r3
r1 1 MC12 (d1 d 2 r3 ) 2

MC12 ( d1 ) 2 MC12 ( d 2 r3 ) 2
=> MC ( d ) MC [ MC ( d r ) ]
12
1 2
12
23
2
3 3

MC12 ( d1 ) 2 MC12 MC23 ( d 2 r3 ) 3
Nu xem im P cng l mt khu (khu 4), ta c
y4
y3
x2
y1
d1
P
z4
x3
r1
z2
d2
x4
r3
y2
z3
x1
z1
(
r
)
MC
(
d
)
MC
MC
(
d
d
Vy 1 1
12
1 2
12
23
2
3 )3
MC12 (d1 )2 MC12 MC23 (d 2 )3 MC12 MC23 MC34 (d3 )4 ]
2.4.3. Phn tch chuyn ng ca tay my nhiu khp ni.

32
M rng vi h a bc t do, ta c ta ca khu cui trong h ta

gc l
n
n 1
i
( rT (Ter min al ) )1 d i [ MC j ( j 1) (d i ) i 1 ]
i 1
i 1
j 1
a. Cc bc thc hin cho vic phn tch chuyn dng

Bc 1: Xc nh h trc ta
Xc nh c tnh cc khp: trt hay bn l
t cc h trc ta sao cho trc quay ca khp trng vi trc z, trc thanh
tay my trng vi trc x
Xc nh cc gc quay, chn chiu dng ca gc quay hng t trc
thanh(trc x) ti thanh quay (trong khng gian 1/4 dng)
Bc 2: Xc nh cc ma trn MC
Bc 3: Vit phng trnh xc nh ta ca khu cui.
Bc 4: Tnh ton vn tc v gia tc.
b. V d1: Xc nh ta ca khu cui P trong h tay my nh hnh di.
Cho d1 = 20cm, d2 = 30cm, d3 = 10cm, 1 = 300, 2 = 600, 3 = 450
Gii
z3
x2
2
x1
d1
y2
z2
d2
x3
3
d3
r1
y1
y3
z4
y4
x4
z1
Ta c
(rP )1 MC12 (d1 )2 MC12 MC23 (d 2 )3 MC12 MC23 MC34 (d 3 ) 4
M:
33

1 2 3 cos( x2 , x1 ) cos( y 2 , x1 ) cos( z2 , x1 ) cos 1 sin 1 0

M 12 1 2 3 cos( x2 , y1 ) cos( y 2 , y1 ) cos( z2 , y1 ) sin 1 cos 1 0

0
1
1 2 3 cos( x2 , z1 ) cos( y 2 , z1 ) cos( z2 , z1 ) 0

cos( x3 , x2 ) cos( y3 , x2 ) cos( z3 , x2 ) cos 2 0 sin 2

M 23 cos( x3 , y2 ) cos( y3 , y2 ) cos( z3 , y2 ) sin 2 0 cos 2

cos( x3 , z 2 ) cos( y3 , z2 ) cos( z3 , z2 ) 0
1
0
M 34

cos( x4 , x3 ) cos( y4 , x3 ) cos( z4 , x3 ) cos 3 sin 3 0

cos( x4 , y3 ) cos( y4 , y3 ) cos( z4 , y3 ) sin 3 cos 3 0

0
1
cos( x4 , z3 ) cos( y4 , z3 ) cos( z4 , z3 ) 0
c. V d 2: Xc nh ta ca khu cui P trong h tay my nh hnh di.

Cho d1 = 20cm, d2 = 30cm, d3 = 10cm, 1 = 600, 2 = 300, 3 = 450
z4
z3
x2
2
x1
d2
d1
P
y4
3
x3
r1
z2
x4
d3
y2
y3
y1
z1
34
z4
Chng 3
CC PHP BIN I THUN NHT

chng 2, chng ta tm hiu cc kin thc c bn v cc h cn bng
lc cng nh ng hc ca cnh tay my. i vi cc robot c kt cu n
gin, chng ta c th p dng cc phng thc trc tip v lc, momen v cc
thnh phn ng hc phn tch ng hc cho robot cng nghip. Tuy nhin,
phng php ny gp nhiu kh khn i vi cc bi ton ca robot c cu
hnh phc tp. V vy, trong chng ny chng ta tm hiu cch thc tip cn
khc trong vn gii quyt bi ton ng hc robot, l cc php bin i
trong h to thun nht (gi tt l cc php bin i thun nht). Phng
php ny l bc pht trin t cc nn tng ton hc, c hc tm hiu
chng trc.
3.1. H to thun nht.
biu din 1 im trong khng gian 3 chiu, ngi ta dng vector im (
Point Vector)
Cc vector im thng c k hiu bng cc ch vit thng. V d

a, v , p
Tu thuc h qui chiu c chn m 1 im trong khng gian c th c
biu din bng cc vector im khc nhau
V d :
zC
V
VB
zA
VA
yC
xC
y
xA
v:

i
Nu gi , j , k l cc vector nh v ca h to d no th vector im
v ai bj ck
Vi a,b,c l to v tr ca im v.
o Nu
quan tm ng thi vn v tr v nh hng ta phi biu din vector
im v trong khng gian 4 chiu :
35
x
y
v , vi
z

w
x
a;
w
y
b;
w
z
c
w
Vi w l hng s thc (hng s t l).

+ Khi w=1 th x=a; y=b; z=c : H to thun nht (Lc ny to khng gian
4 chiu trng vi to khng gian 3 chiu)
+ Khi w=0 th x, y, z : Th hin hng ca cc trc to
S dng h to vi w=0 v w=1 th c th th hin c v tr v nh hng
vt th.
+ Ki w0, v w0 th :
v ai bj ck
2
i
3
j k
V d :
o Cc trng hp c bit :
+ [0, ,0, 0, 0]T : Vector khng xc nh.
+ [0, 0, 0, n]T : Vector 0.
+ [x, y, z, 0]T : Vector ch hng.
+ [x, y, z, 1]T : Vector trong h to thun nht.
3.2. Nhc li cc php tnh v vector v ma trn.
3.2.1) Php nhn vector :
Cho 2 vector :
a ax i a y j az k
b bx i by j bz k
a. Tch v hng 2 vector :
a.b ax bx a y by az bz
b. Tch c hng hai vector (Tch hai vector) :
i
j
k

a.b c a x a y a z
bx by bz
a. Php cng tr hai ma trn :
iu kin : Cc ma trn phi cng bc (cng kch thc)
36
Cng (tr) hai ma trn A,B cng bc ta c ma trn C cng bc vi cc phn t

Cij Aij Bij
b. Tch hai ma trn :
iu kin : S ct ca ma trn th nht bng s hng ca ma trn th hai.
Tch ca hai ma trn A(m,n) vi ma trn B(n,p) l ma trn C(m,p).
V d :
1
A 4
1 2
6 v B 3 4
5 6
9
3
22 28
A.B C 49 64
76 100
Ch :
+ A.B B.A
+ (k.A).B = k.(A.B) = A.(k.B)
+ A.(B.C) = (A.B).C
+ (A+B).C = A.C+B.C
+ C.(A+B) = C.A+C.B
c. Ma trn nghch o :
A. A1 I
iu kin : Ma trn A l kh o (det(A) 0)

C mt s cch tnh ma trn nghch o. Mt trong s :
+ Tnh nh thc : det(A)
+ Tnh ma trn C l ma trn phn ph i s ca ma trn A :
Cij (1)i j Dij vi Dij det(M ij )
1
+ Tnh ma trn nghch o theo : A
1
CT
det( A)
d. Ma trn nghch o ca ma trn thun nht :

Cho ma trn thun nht A :
37
nx ox
n
oy
A y
nz oz
0
0

A n o a
ax
ay
az
0
px
p y
pz
Tm ma trn nghch o ca ma trn thun nht :

nx n y nz n. p

o
o
o
o
.p
x
y
z
A

a x a y a z a. p
0
0
1
0
V d : Cho
0 0
0 1
A
1 0
0 0
A1
0
0
1
0
0
0
1
2
3
1 3
0 2
0 1
0
1
0
1
0
0
Kim tra :
A. A1
1
0
0
1
0
0
0
0
1
0
0
0
I
0
e. Vt ca ma trn :
Vt ca ma trn vung bc n l tng cc phn t trn ng cho chnh.
n
K hiu : Trace( A) Tr ( A) aii

i 1
f. o hm v tch phn ca ma trn :

Nu cc phn t ca ma trn A l hm nhiu bin th cc phn t ca ma
trn o hm bng o hm ring ca cc phn t ma trn A theo bin tng
ng.
38
a
A d
g
b
e
h
c
f
k
a b c
t
t t
d e f
g h k
t
t t
Tng t cho php tch phn ma trn.
3.3. Cc php bin i ma trn dng trong ng hc robot.
Cho u l vector biu din im cn bin i
h l vector dn c biu din b ma trn H l ma trn chuyn i :
v H .u
L vector biu din im sau khi chuyn i.
3.3.1. Php bin i tnh tin.
Gi s cn tnh tin 1 im hay hay 1 vt th theo vector dn :
h a.i b. j c.k
Ma trn chuyn i tnh tin theo vector dn :
1 0 0 a
0 1 0 b
H
0 0 1 c
0 0 0 1
Gi u l vector biu din im cn tnh tin :

u= [x, y, z, 1]T
1 0 0 a x x a
0 1 0 b y y b

v H .u
0 0 1 c z z c
0 0 0 1 1 1
K hiu :
v= Trans(a,b,c).u
V d : Cho u 2.i 3. j 2.k
h 4.i 3. j 7.k
39
1
0
v H .u
0
0
1
0
0
0
0
1
0
4 2 6
3 3 0
7 2 9

1 1 1
v=Trans(4, -3, 7).u

3.3.2. Php quay quanh cc trc to :
Gi s ta cn quay 1 im hay vt th xung quanh 1 trc no vi gc
quay 0 ta ln lc c cc ma trn chuyn ng quay nh sau :
1
0
Rot ( x, )
0
0
cos
0
sin
sin
0
cos
0
cos
0
Rot ( y, )
sin
cos
sin
Rot ( z , )
0
0
1
sin
0
0
0
cos
0
sin
cos
0
0
0
0
1
0
0
0
0
1
0
0
0
0
0
0
V d : u 7.i 3. j 2.k
Quay mt gc 900 quanh trc z : Rot(z, 90), sau tip tc cho quay y 1
gc 900 : Rot(y, 90)
Thc hin chuyn i :
v Rot ( z,90 ).u

0
1
v R.u
0
1 0
0 0
0
0
1
0
0 7 3
0 3 7
0 2 2

1 1 1
Tip tc cho quay quanh y 1 gc 900 :

40
W= Rot(y, 90).v
0
0
v R.u
1
0
1
1
0
0
0
0
0
0 3 2
0 7 7
0 2 3

1 1 1
Vy c th tnh :
W Rot(y,90 ).Rot ( z,90 ).u
Ch :
+ Php quay cn tun th theo ng th t trc sau .
Trong v d : quay quanh trc z trc, trc y sau, ta k hiu : Rot(y,
90).Rot(z, 90).u
+ V cc php quay cho cc ma trn nn :
Rot(y, 90).Rot(z, 90).u Rot(z,90).Rot(y,90).u
3.3.3. Php quay le( Euler)
Trong thc t vic nh hng khu chp hnh cui thng l kt qu ca
cc php quay quanh trc x, y, z.
Php quay le m t kh nng nh hng ca cc khu chp hnh cui
thng qua cc gc quay , , bi cc php bin i sau :
+ Quay 1 gc quanh trc z.
+ Quay 1 gc quanh trc y mi l y
+ Quay 1 gc quanh trc z mi l z
Euler ( , , ) Rot ( z, ).Rot ( y, ).Rot ( z, ) Rot ( z, ).Rot ( y, ).Rot ( z, )
Ch :
Php quay phi theo th t trc sau , nhng c bit vi php quay le th
s thay i th t khng lm thay i kt qu.
Cng thc tnh :
Euler ( , , ) Rot ( z, ).Rot ( y, ).Rot ( z, )
cos
0
Rot ( z , )
sin
0
1
sin
0
0
0
cos
0
0 cos
0 sin
0 0
1 0
41
sin
cos
0
0
0
0
1
0
0
0
0
cos cos cos sin sin

sin cos cos cos sin
sin cos
cos cos sin sin cos

sin cos sin cos cos
cos sin
sin sin
sin sin
0
cos
0
3.3.4. Php quay roll - pitch yaw.

L php quay dng nh hng khu chp hnh cui thng c dng
trong thc t.
Ta tng tng gn h to xyz ln thn mt con tu
YAW
ROLL
O
PITCH
+ Roll- Chuyn ng lc ca thn tu tng ng vi trc z ca thn tu 1 gc

+ Pitch- Chuyn ng nhp nh ca thn tu tng ng vi vic quay quanh
trc y 1 gc
+ Yaw- Chuyn ng lch hng tng ng vi vic quay quanh trc x 1 gc
z
y
x
Ngi ta s dng php quay ny biu din chuyn ng ca Robot.
Phng php ny c s dng kh ph bin.
42
0
0
0
RPY ( , , ) Rot ( z, ).Rot ( y, ).Rot ( x, )

0 sin 0 1
0
0
0
cos
0
1
0
0
0
cos
sin
Rot ( z , )
sin 0 cos 0 0 sin
cos
0
0
0
1 0
0
0
1
0
cos cos cos sin sin sin cos cos sin cos sin sin
sin cos sin cos sin cos cos sin sin cos cos sin
sin
cos sin
cos cos
0
0
0
0
0
0
Hay c th vit :
C C C S S S C C S C S S
RPY ( , , ) S C S S S C C S S C C S
S
C S
C C
3.4. Bin i h to v mi quan h gia cc h to

3.4.1. Bin i h to .
Gi s cn tnh tin gc to cac O(0,0,0) theo mt vector dn
h 4.i 3. j 7.k th kt qu ta c to im OT :
0
0
OT H .O
0
0
1
0
0
0
0
1
0
4 0 4
3 0 3
7 0 7

1 1 1
Nu ta tip tc thc hin cc php quay i vi h to OT th ta c

h to mi :
+ Nu chn h qui chiu l h to gc th ta thc hin cc php bin i t
phi sang tri :
A Rot(y,90 ).Rot ( z,90 )
43
zT
z'T
Rot(z,-90)
Rot(y,90)
x'T
OT
x'T
OT
OT
yT
xT
z'T
y'T
y'T
+ Nu chn h qui chiu l h to trung gian th ta htc hin cc php bin

i t tri sang phi :
A Rot(y,90 ).Rot ( z,90 )
zT
Rot (yT , 90)
Rot (z'T ,-90)
OT
OT
y'T
yT
xT
x''T
OT
z'T
z''T
x'T
3.4.2. Mi quan h gia cc h to .

Gi s c 3 gc h to A, B, C th h to B c mi quan h vi h to
A c biu din :
B A TBA
C B TCB
44
y''T
zC
P
pC
pA
zB
yC
xC
yB
zA
xB
A
y
xA
Gi s c im P trong h to C c biu din pC . Xc nh mi quan

h ca P trong h to A.
B
Trc ht cn xc nh pB : pB TC . pC
p A TBC . pB TBA .TCB

A
A
B
Vy : TC TB .TC
Tnh cht :
B A TBA
A B TAB
TAB (TBA ) 1
3.5. M t vt th
Vt th l cc i tng lm vic ca Robot . Da vo c im hnh hc
ca chng , ta c th chia chng thnh 3 nhm sau :
+ Nhm cc vt th trn xoay : ngoi gi tr ca v tr v kch thc, ta cn xc
nh to tm v bn knh ca ng cong.
+ Nhm cc vt th c gc cnh : Gi tr c trng l to cc im gii hn.
+ Nhm cc vt th c cu trc hn hp
i vi hot ng cm nm i tng v qu trnh vn ng ca Robot th
vic m t vt th cn phi gn lin vi cc php bin i thun nht.
V d : Cho vt th hnh ln tr t trong h to oxyz nh hnh v :
45
C
z
B
D
y
E
A
x
m t v tr ca vt th ta dng ma trn ca 6 im nh sau, phn t ca

hng cui cng chnh l gi tr w = 1.
1 1 1 1 1 1
0 0
0
0
4
4
A
0 0
2 2 0 0
1 1 1 1
1 1
A A B C D E F
Yu cu : Thc hin cc php bin i : H=Trans(4,0,0) Rot(y,90)
Rot(z,90).
Thc hin nhn cc ma trn thun nht ca cc php bin i theo ng th
t nh trn , ta thu c ma trn H nh sau :
0 0 1 4
1 0 0 0
H
0 1 0 0
0 0 0 1
0
1
A' H . A
0
4
1
A'
0
0
0
1
0
1
0
0
0
4 1
0 0
0 0
1 1
4
6 6
1 1 1
4
1
0
1
4
1
0
1
0
1
1 1 1
0
0 0
1
4
0
1
0
1
2
1
4
1
4
1
46
2
1
1
4
0
Kim tra li bng hnh v : Dng h qui chiu l h to gc.

H=Trans(4,0,0) Rot(y,90) Rot(z,90)
Thc hin ln lc theo th t : Quay quanh trc z , quay quanh trc y, tnh
tin so vi h to gc.
+ Rot(z,90) :
y'
z'
x'
+ Rot(y,90) :
y''
x'
z''
+ Trans(4,0,0) :
y''
O
4
z''
47
x'
Chng 4
PHNG TRNH NG HC ROBOT

4.1. Dn nhp
Bt k mt Robot no cng bao gm cc khu lin kt vi nhau thng qua
cc khp. Hai chuyn ng c bn ca cc khu thng qua khp quay v khp
tnh tin.
Hnh 4.1. Khp quay v khp tnh tin trong chuyn ng ca robot.
Ta t trn mi khu ca mt Robot mt h trc to . S dng cc php
bin i thun nht c th m t v tr tng i v hng gia cc h to
ny.
Theo Denavit, mi lin h gia hai khu lin k nhau (khu n so vi khu
(n-1)) c m t bi ma trn A l ma trn bin i thun nht gm c cc php
quay v tnh tin gia cc h to vi nhau.
Hnh 4.2. t h trc to cho cc khu ca robot Puma.

nht so vi h to gc.
48

V d : T3= A1.A2.A3
Hnh 4.3. Cc vector nh v v nh hng ca tay my.

Lu :
+ Nu mt Robot c 6 khu th :
T6=A1A2A3 A4A5A6.
+ Nu mt Robot c s bc t do w>3 th 3 bc t do u tin dng nh v,
cc bc t do cn li nh hng.

+ H to biu din khu chp hnh cui (im tc ng cui) n o a

:
3 vector ch phng ca h to gn trn khu chp hnh cui, (im tc
ng cui) xc nh bi :
a : Vector c hng m theo bn tay s tip cn n i tng.
o : Vector c hng theo cc ngn tay cm nm hay th i tng.

n : Vector php tuyn ca o v a : n o.a
nx ox a x p x
n o
a y p y
y
y
T6
nz oz a z p z
0
0
1
0
4.2. B thng s Denavit-Hartenberg (DH)
4.2.1. Cc khi nim :
49
Mt Robot gm nhiu khu cu thnh t nhng khu ni tip nhau thng

qua cc khp ng.
Gc chun ca 1 Robot l l khu s 0 v khng tnh vo s cc khu. Khu
1 ni vi khu chun bi khp 1, khng c khp u mt khu cui cng
4.2.2. di php tuyn chung v gc gia hai trc khp :
Bt k mt khu no cng c c trng bi hai yu t :
+ di php tuyn chung an
+ Gc gia cc trc khp o trong mt phng vuuong gc vi an , k hiu l n
Hnh 4.4. Chiu di gc xon ca khu.
n :Gc xon ca khu n( Khp n so vi khp (n+1))

an : Chiu di ca khu n ( Khp n so vi khp (n+1))
Hnh 4.5. Cc thng s ca khu : an, n, dn, n

Cc trng hp c bit :
+ n =0,an =const(2 trc khp song song)
+ / n /=90, an =const (2 trc khp vung gc)
+ n =0(180), an =0 (2 trc khp trng nhau )
50
+ / n /=90, an =0 (2 trc khp ct nhau v vung gc nhau)
Hnh 4.6. Cc trng hp c bit ca phng hai trc khp

4.2.3. Khong cch gia hai khu v gc quay gia hai khu.
Tip tc kho st mi quan h gia cc khu lin k nhau, ph bin l hai
khu lin kt nhau chnh trc ca khp :
Hnh 4.7. Khong cch hai khu v gc quay gia hai khu.
Mi trc khp c hai ng php tuyn chung i vi n, khong cch
gia hai ng php tuyn chung o dc theo trc khp n gi l d n
dn cn gi l khong cch gia hai khu : Khu n so vi khu th (n-1)
Gc gia hai ng php tuyn chung o trong mt phng vung gc vi
trc khp th n l gc n.
n l gc quay ca khu th n so vi khu th (n-1)
4.2.4. B thng s Denavit-Hertenberg :
51
C 4 thng s xc nh trn chnh l b thng s DH : n , an, dn, n

Vi 4 thng s trn , ta c th xc nh v tr v hng ca mi khu so vi
nhau v so vi to gc
Nu khp ni hai khu l khp quay th n l bin khp ( 3 thng s cn li
l hng s)
Nu khp ni l tnh tin th dn l bin khp :( n =0, an =0, n =const)
4.3. Gn h to cho Robot .
kho st ng hc ca Robot ta phi gn trn mi khu ca robot mt
h to . Nguyn tc chung gn h to nh sau :
a. Gc ca h to :
Gc to ca khu th n nm trn ng tm ca trc khp th (n+1) v
nm ti giao im ca ng php tuyn chung an vi trc khp th (n+1)
(Tng qut, cho nhau)
Nu hai trc khp ct nhau th gc to on nm ti chnh im ct .
Nu hai trc khp song song nhau th o n nm trn trc khp th n+1 v ti
mt mt v tr c bit no qu trnh tnh ton l thun li nht.
b. Chn trc Zn :
Trc Zn nm dc theo trc khp th n+1 v c hng v pha cc khu.
c. Chn trc Xn :
Trc Xn nm dc theo ng php tuyn chung hng t trc khp th n
n trc khp th n+1.
Nu hai trc khp ct nhau th xn z n .z n1

d. Chn trc yn theo qui tc bn tay phi.
V d 1: Gn h to v xc nh cc thng s DH cho Robot c hai khu
phng :
52
Hnh 4.8. Xc nh h trc ta v b thng s DH robot hai khp quay phng

B thng s DH ca robot c xc nh :
V d 2: Gn h to v xc nh b thng s DH cho Robot Scara :

a2
a1
d*3
o0
y0
x0
z0
o1
y1
x1
z1
o2
y2
x2
z2
x3
o3
y3
z3
o4
y4
x4
z4
Hnh 4.9. Xc nh h trc ta v b thng s DH cho robot Scara.

B thng s DH :
1*
a1
2*
a2
d3
4*
d4
4.4. c trng ca cc ma trn A.

Ma trn A l ma trn m t mgh hng v v tr ca h to gn trn hai
khu lin k nhau.
Cn c vo thng s ca b DH th ma trn A c c trng bi 4 php
bin i sau :
i. Quay quanh trc zi-1 mt gc i.
ii. Tnh tin dc trc zi-1 mt qung di.
53
iii. Tnh tin dc trc xi-1 ( trng vi xi) mt on ai

iv. Quay quanh trc x1 mt gc i
Bn bc bin i ny c biu hin bng tch ca cc ma trn thun nht
nh sau:
Ai = R (z, i). Tp (0, 0, di). Tp (ai, 0, 0). R (x, i)
cos
sin
Rot ( z, )
0
0
1
0
H1
0
sin
cos
0
0
0 0 a
1 0 0
0 1 0
0 0 1
0
1
0 cos
Rot ( x, )
0 sin
0
0
1
0
H2
0
0 0 0
1 0 0
0 1 d
0 0 1
0
0
sin 0
cos 0
0
1
cos i
sin
i
Ai
0
Hay:
0 0
0 0
1 0
0 1
cos i sin i
cos i cos i
sin i sin i
sin i cos i
sin i
0
cos i
0
ai cos i
ai sin i
di
Ma trn Ai c gi l ma trn chuyn i thun nht, n c dng

R
: Ai i
0
pi
vi Ri l ma trn quay 3 x 3 v pi l vect tnh tin 3 x 1.
1
Lu :
i vi khp tnh tin th i =a=0 nn:
0
1
0 cos
Ai
0 sin
0
0
0
sin
cos
0
0
0
d
1
54
4.5 Xc nh cc ma trn T theo ma trn A.

nht so vi h to gc.
V d : T3= A1.A2.A3
Nu mt Robot c 6 khu th :
T6=A1A2A3 A4A5A6.
H to biu din khu chp hnh cui (im tc ng cui)
n o a : 3 vector ch phng ca h to gn trn khu chp hnh
cui, (im tc ng cui) xc nh bi :
+ a : Vector c hng m theo bn tay s tip cn n i tng.
+ o : Vector c hng theo cc ngn tay cm nm hay th i tng.

+ n : Vector php tuyn ca o v a : n o.a
nx
n
T6 y
nz
ox
oy
ax
ay
oz
0
az
0
px
p y
pz
Ta c th xc nh ma trn T thng qua h to trung gian :

n
Tn Ai
n 1
i 1
Vi : T3 A3
2
T3 A2 A3
4.6. Trnh t thit lp phng trnh ng hc ca robot.

4.6.1. Cc bc thc hin
thit lp phng trnh ng hc ca robot, ta thc hin cc bc sau :
1. Bc1: Chn h to c bn v gn cc h to trung gian khc :
+ Gi nh v tr ban u ca Robot, l v tr cc bin khp thng bng 0
+ Chn gc h to O0, O1
+ Chn trc Z0, Z1 theo nguyn tc chung.
55
Vi cc robot c w<= 3 th khng th nh hng cho trc Zn chn tu .

+ Chn cc trc x0, x1
V ma trn Ai = R (z, i). Tp (0, 0, di). Tp (ai, 0, 0). R (x, i)
nn trc xn-1 chnh l trc quay zn-1 thnh trc Zn :
Lc ny : n= (Zn-1, Zn)
+ Chn trc y theo nguyn tc bn tay phi.
* Lu :
Trong qu trnh gn htd th khi xut hien cc phop bin i : Trans(0.y,0)
v Rot(y,theta) th v tr gi nh ban u l khng ng, cn thay i v tr mi.
2. Bc 2: Lp bng thng s DH.
3. Bc 3: Xc nh cc ma trn Ai
4. Bc 4: Tnh cc ma trn T t ngn ti gc. T4=A1A2A3A4
Tnh ngc t sau ra trc (Thng thng)
5. Bc 5: Vit phng trnh ng hc Robot
4.6.2. Cc v d thit lp phng trnh ng hc :
1. V d 1. Xc nh phng trnh ng hc ca Robot hai bc t do RT
Gn h trc to cho Robot :
z2
y2
O2
x2
z1
y1
x1
O1
l1
y0
O0
x0
z0
Hnh 4.10. Gn h to c bn v cc h to trung gian cho Robot

Khu 1 : Quay quanh trc Z0, chn X0 l php tuyn chung ca (Z0, Z1).
Khu 2 : Tnh tin dc theo trc Z1, chn X1 nm ngang.
Xc nh b thng s DH :
56
Khu
1
2
ai
di
1*
0
90
l1
d 2*
*
*
Cc bin khp : 1 , d 2
Phng trnh ng hc :
+ Cc ma trn c trng A :
c1 0 s1
s1 0 c1
A1
0 1 0
0
0 0
1
0
A2
0
0
0
l1
0
0
0 1 d2
0 0 1
cos i cos i sin i sin i sin i ai cos i

sin
cos
cos
sin
cos
a
sin
i
i
i
i
i
i
i
Ai
0
sin i
cos i
di
0
0
1
0
0
0
0
1
0 cos sin 0
Ai
0 sin cos d
0
0
1
0
0 0
1 0
+ Ma trn vector cui :
c1 0 s1
s1 0
c1
T A1 A2
0 1 0
0
0 0
0
0
l1
1
0
0 c1 0 s1 d 2 s1
0 s1 0 c1 d 2 c1
0 1 d2 0 1 0
l1

0 0 1 0 0
0
1
0 0
1 0
+ Phng trnh ng hc th hin mi quan h v hng v v tr ca ma trn

vector cui theo cc bin khp :

Ba vector ch hng : n, o, a
nx cos 1
ox 0
a x sin 1
n y sin 1 ,
nz 0
oy 0 ,
o z 1
az 0
a y cos 1
Vector nh v : p
p x d 2 sin 1
p y d 2 cos 1
p z l1
1. V d 2. Xc nh phng trnh ng hc Robot c cu hnh RRT
57
Hnh 4.11. Robot hai khu RT
i. Gn h to cho Robot :
Hnh 4.12. Gn h ta ti
chn
v tr ban u cho.
Hnh 4.13. Gn h ta ti v tr la
ii. B thng s DH :
Khu
1
2
3
iii.
cos i
sin
i
Ai
0
ai
di
1*
+90
d1
2*
0
-90
3*
Xc
nh
cc
ma
cos i sin i sin i sin i ai cos i
cos i cos i sin i cos i ai sin i
sin i
cos i
di
0
1
58
trn
Qui uc :
cos 1 = c1
cos 2 = c2
c1c2-s1s2 = cos1 2 = c12

s3c4+c3s4= sin 1 2 = s34
c1c23-s1s23= cos1 2 3 = c123

c1
s1
A1
0
0 s1
0 c1
1
0
0
0
0
0
d1
c 2 0 s 2
s2 0 c2
A2
0 1 0
0
0 0
1
0
A2
0
0 0
1 c2
0
0
1
0
0
0
0
0
0
d 3
c1c 2 s1 c1s 2 c1s 2d 3

s1c 2 c1 s1s 2 s1s 2d 3
T3
s2
0
c2
c 2d 3 d1
0
0
1
0
iv. Vit phng trnh ng hc :

nx
n
T3 y
nz
ox
oy
ax
ay
oz
0
az
0
px
p y
pz
3. V d 3 : Xc nh phng trnh ng hc cho Robot 3 khp quay phng
59
i. B thng s DH :
1
1*
a1
2*
a2
3*
a3
ii. Xc nh cc ma trn A
cos i
sin
i
Ai
0
cos i sin i
cos i cos i
sin i sin i
sin i cos i
sin i
0
cos i
0
ai cos i
ai sin i
di
iii. Tm phng trnh ng hc :

Tng t, thay vo tnh A1 v T3:
c123
s123
T3
0
s123
c123
0
0
0
0
1
0
c123a3 c12a 2 c1a1

s123a3 s12a 2 s1a1
4. V d 4. Xc nh phng trnh ng hc ca robot Puma 6 bc t do.

Robot Puma l sn phm ca cng ty Unimate (USA), l loi robot c 6
bc t do c s dng ti nhiu nc trn th gii.
60
i. Gn h ta cho robot Puma.
Hnh 4. Gn h ta cho robot Puma.

ii. B thng s D-H ca robot Puma :
iii. Phng trnh ng hc ca robot Puma c s khp n = 6

61
c1
s
0
1
1T
0
3T
s1
c1
0
0
c 3
s 3
s 3
c 3
c 5
0
4
5T
s 5
s 5
0
c 5
0
0 0
0 0
1 0
0 1
c 2 s 2 0 0
0
0 1 0
1
2T
s 2 c 2 0 0
0
0 1
0
a2
0 0
,
0 d3
0 1
c 3
0
3
4T
s 4
0
1 0
,
0 0
0 1
c 6 s 6 0 0
0
0
1 0
5
6T
s 6 c 6 0 0
0
0 1
0
s 3
0
c 4
0
a2
1 d 4
0 0
0 1
Ta c :
r11 r12 r12 Px

r
r
r
Py
21
22
23
0
0
1
2
3
4
5
6T 1T 2T 3T 4T 5T 6T
r31 r32 r33 Pz
0
0
0
1
Trong :
r11 c1 [c 23 (c 4 c5 c6 s 4 s5 ) s 23 s5 c5 ] s1 ( s 4 c5 c6 c 4 s 6 )
r21 s1 [c 23 (c 4 c5 c6 s 4 s 6 ) s 23 s5 c6 ] c1 ( s 4 c5 c6 c 4 s 6 )
r31 s 23 (c 4 c5 c6 s 4 s 6 ) c 23 s5 c6
r32 s 23 (c 4 c5 s 6 s 4 c6 ) c 23 s5 s 6
r13 c1 (c 23c 4 c5 s 23c5 ) s1 s 4 s5 ]
r23 s1 (c 23c 4 c5 s 23c5 ) c1 s 4 s5 )
r33 s 23c 4 s5 c 23c5
Px c1 [a 2 c 2 a3 c 23 d 4 s 23 ] d 3 s1
62
Py s1 [a 2 c 2 a3 c 23 d 4 s 23 ] d 3 c1
Pz a3 s 23 a 2 s 2 d 4 c 23
r32 s 23 (c 4 c5 s 6 s 4 c6 ) c 23 s5 s 6
r13 c1 (c 23c 4 c5 s 23c5 ) s1 s 4 s5 ]
r23 s1 (c 23c 4 c5 s 23c5 ) c1 s 4 s5 )
r33 s 23c 4 s5 c 23c5
Px c1 [a 2 c 2 a3 c 23 d 4 s 23 ] d 3 s1
Py s1 [a 2 c 2 a3 c 23 d 4 s 23 ] d 3 c1
Pz a3 s 23 a 2 s 2 d 4 c 23
63
Chng 5
NG LC HC ROBOT V NG DNG
TRONG IU KHIN
5.1. Mc ch v phng php kho st ng lc hc robot
Vi nhng mc ch thit k v iu khin, cn thit phi c mt m hnh
ton hc m t ng lc hc ca h thng. V th, chng ny ta s xc lp
phng trnh chuyn ng ca tay my di dng phng trnh vi phn. Phng
php p dng y l xy dng phng trnh chuyn ng ca c h da trn
quan h nng lng, xut pht t nguyn l bo ton v chuyn ha nng lng
trn c s xc lp quan h gia ng nng v th nng ca c h tay my, sau
s dng phng trnh vi phn ca chuyn ng trn c h vi cc i lng
tham gia vo phng trnh gm lc, qun tnh v nng lng.
Vic nghin cu ng lc hc Robot thng gii quyt hai nhim v sau :
1. Xc nh momen v lc ng trong qu trnh chuyn ng. Khi qui lut
bin i ca bin khp qi(t) xem nh bit.
Vic tnh ton lc cng nh momen trong c cu tay my l nhim v tt
yu trong vic la chn cng sut ng c, tnh ton kim tra bn, cng
vng, m bo tin cy cho Robot.
2. Xc nh cc sai s ng, tc l sai s xut hin so vi qui lut chuyn ng
trong chng trnh.
C nhiu phng php nghin cu ng lc hc Robot, nhng nhiu hn
c l phng php c hc Lagrange, c th l phng trnh Lagrange-Euler.
Trong phm vi ni dung ca mn hc ny, chng ta tm hiu nhim v th
nht, t to c s cho vic lp trnh v iu khin robot.
L= K P
Trong : K l tng ng nng ca c h
L l tng th nng ca c h
K v P u l nhng i lng v hng, nn c th chn bt k h ta
no gi bi ton n gin.
Xt mt Robot c n khu th :
64

n
K Ki
P Pi
i 1
i 1
(2.1)
Trong , Ki v Pi l ng nng v th nng ca khu th i xt trong h ta

chn. l cc i lng ph thuc vo nhiu bin s :
Ki K qi , qi v Pi Pqi , qi
(2.2)
Vi qi l ta suy rng ca khp th i.

nh ngha : Lc (hay momen) tng qut tc dng ln khu th i c xc
nh bi phng trnh Lagrange :
F
d L L
dt q q
5.3. Kho st bi ton ng lc hc ca tay my nhiu bc t do

d L L
dt q q
(2.3)

Lagrange l s chnh lch gia ng nng v th nng ca c h :
L K P
(2.4)
a. V d 1.
Ta xt v d xy dng phng trnh chuyn ng ca tay my hai khu phng
lin kt bng khp bn l.
Trong v d ny, ta p dng cc kt qu ca bi ton ng hc c kho
st phn trc. xy dng bi ton ng lc hc, ta kho st c h vi gi
thit rng khi lng ca khu c tp trung cc khp. Ma trn bin khp l:
q 1 2
(2.5)
(2.6)
vi 1 , 2 l cc m men c cho bi cc c cu tc ng (chng hn l m
men pht ng ca cc ng c in).
65
(x2,y2)
2
a2
m2
g
a1
m1
x
Hnh 5.1: Tay my hai khu bn l

Vi khu 1, ta c biu thc ca ng nng v th nng tng ng l:
2 2
K1 1 2 m1a1 1
(2.7)
P1 m1 ga1 sin1
(2.8)
Vi khu 2 ta c:
x2 a1 cos1 a2 cos(1 2 )
(2.9)
y2 a1 sin1 a2 sin(1 2 )
(2.10)
x2 a11 sin1 a2 (1 2 ) sin(1 2 )
(2.11)
y 2 a11 cos1 a2 (1 2 ) cos(1 2 )

(2.12)
Bnh phng vn tc l :
v22 x22 y 22 a1212 a22 (1 2 )2 2a1a2 (12 12 ) cos 2

(2.13)
K2 1 2 m2v22 1 2 m2 a1212 1 2 m2 a 22 (1 2 )2 m2 a1a2 (12 12 ) cos 2
(2.14)
Th nng cho khu 2 l:
P2 m2 gy2 m2 g[a1 sin1 a2 sin(1 2 )]

(2.15)
66
Phng trnh Lagrange

2 2
L K P K1 K 2 P1 P2 1 2 (m1 m2 )a1 1 1 2 m2 a 22 (1 2 ) 2
(2.16
m2 a1a2 (12 12 ) cos 2 (m1 m2 ) ga1 sin 1 m2 ga2 sin(1 2 )
)
Ta cn xc nh cc biu thc :
L
2
(m1 m2 )a1 1 m2 a 22 (1 2 ) m2 a1 a 2 (21 2 ) cos 2
1
d L
2
(m1 m2 )a1 1 m2 a 22 (1 2 ) m2 a1 a 2 (21 2 ) cos 2 m2 a1 a 2 (212 22 ) cos 2
dt 1
L
(m1 m2 ) ga1 cos 1 m2 ga 2 cos(1 2 )
1
L
m2 a 22 (1 2 ) m2 a1 a 21 cos 2
2
d L
m2 a 22 (1 2 ) m2 a1 a 21 cos 2 m2 a1 a 212 sin 2
dt 2
L
m2 a1 a 2 (12 12 ) sin 2 m2 ga 2 cos(1 2 )
2
Cui cng, phng trnh chuyn ng ca c h tay my c cho bi h
1 [(m1 m2 )a12 m2 a22 2m1m2cos 2 ] 1

[m2 a22 m2 a1a2cos 2 ] 2 m2 a1a2 (2 1 2 22 ) sin 2
(m1 m2 ) ga1cos 1 m2 ga2cos (1 2 )
(2.18)
2 [m2 a22 m2 a1a2cos 2 ]1 m2 a22 2 m2 a1a2 12 sin 2

m2 ga2cos (1 2 )
hc Tay my di dng ma trn c th vit nh sau:
67
(m1 m2 )a12 m2 a22 2m1m2cos 2 m2 a22 m2 a1a2cos 2 1

(2.19)

2
2
m2 a2 m2 a1a2cos 2
m2 a2
2
m2 a1a2 (2 1 2 22 ) sin 2 (m1 m2 ) ga1cos1 m2 ga2cos (1 2 ) 1

2 sin
m
ga
cos
(
)
m
a
a
2
2
1
2
2
2 1 2 1
2

Ta tm c biu thc ng lc hc tay my di dng chun, c biu
din chung di dng sau :
M (q)q V (q, q ) G(q)

(2.20)
M(q) l ma trn qun tnh, V (q, q ) l vect lc Coriolis hoc/v lc hng
tm v G(q) l vect trng lc.
Vi biu thc trn M(q) l ma trn i xng.
b. V d 2.
Xy dng Phng trnh ng lc hc ca robot hai bc t do cu hnh RT.
d2
O0
Hnh 5.3. Cu hnh ca Robot 2 bc t do RP

Xut pht t phng php ng lc hc cho h c hc tng qut
68
d L L
dt q q
(2.1)

Lagrange l s chnh lch gia ng nng v th nng ca c h, vi:
L K P
(2.2)
Tng t v d 1, ta kho st c h vi gi thit rng khi lng ca khu

c tp trung cc khp. Ma trn bin khp l:
q 1
d2
(2.3)
(2.4)
vi 1 , 2 l cc m men c cho bi cc c cu tc ng (chng hn l

m men pht ng ca cc ng c in).
y
m 2 ( x 2 ,y 2)
l1
m 1(x 1,y 1 )
d2
Hnh 5.4. To ca cc khu trn Robot

+ Vi khu 1 chuyn ng quay, ta c biu thc ca ng nng v th nng
2 2
K1 1 2 m1l1 1
tng ng l:
P1 m1 gl1 sin 1
(2.5)
(2.6)
+ Vi khu 2 chuyn ng tnh tin, ta c:
x2 d 2 cos 1
y2 d 2 sin 1
(2.8)
69
(2.7)
x2 d2 cos 1 d 21 sin 1
(2.9)
y 2 d2 sin 1 d 21 cos 1
(2.10)
Bnh phng vn tc l :
2 2
2
v22 x22 y 22 d 2 1 d2
(2.11)
K2
1
1
1
2 2
2
m2 v22 m2 d 2 1 m2 d2
2
2
2
(2.12)
Th nng cho khu 2 l:
P2 m2 gy2 m2 gd2 sin 1

(2.13)
Phng trnh Lagrange
1 2 2 1
1
2 2
2
L K P K1 K 2 P1 P2 m1l1 1 m2 d 2 1 m2 d2 m1 gl1 sin 1 m2 gd 2 sin 1
2
2
2
1
1 2
2
2 2
Vy : L (m1l1 m2 d 2 )1 m2 d 2 (m1l1 m2 d 2 ) g sin 1
2
2
(2.14)
Nhng hng thc cn tnh c th hin nh di y:
70
L
2
2
(
m
l
m
d
)1
1
1
2
2
1
d L
2
d 2 )
m
l
m
(
2
d
d
1
1
1
2
2
2 1
2
1
dt 1
L
(m1l1 m2 d 2 ) g cos 1
1
L
m2 d2
d 2
(2.15)
)
d L
m2 d2
dt d 2
L
2
m2 d 21 m2 g sin 1
d 2
Cui cng, phng trnh chuyn ng ca c h Tay my c cho bi h
d L L
2
2
m1l1 1 m2 (2d 2 d21 d 2 1 ) (m1l1 m2 d 2 ) g cos 1

dt 1 1
Vy :
d L L
dt q q
1 (m1l12 m2 d 2 2 )1 2m21d 2 d2 (m1l1 m2 d 2 ) g cos 1
d L L
m d 2 m g sin
m
d
2 2
2 2 1
2
1
dt d2 d 2
Vy :
2 m2 d2 m2 d 212 m2 g sin 1

hc tay my c th vit nh sau:
1 (m1l12 m2d 2 2 )1 2m21d 2d2 (m1l1 m2 d2 ) g cos 1

2
d L L
m d 2 m g sin
m
d
2
2
2 2 1
2
1
dt d2 d 2
m1l12 m2 d 2 2 0 1 2m2 d 21d 2 (m1l1 m2 d 2 ) g cos 1 1
m
g
sin
0
m2 d 2 m2 d 21
2
1
2
71
5.4. Phng trnh ng lc hc tay my.

5.4.1. Tng qut.
Chng ta ch ra cc v d ng dng phng trnh Lagrange tnh ton
nhng phng trnh ng lc hc ca cc Tay my. Trong cc v d trn v
ng lc hc ta nhn thy biu thc kt qu c dng:
M (q)q V (q, q ) G(q)

vi q l bin khp, l vect lc hoc m men suy rng.
nhn c phng trnh ng lc hc ca tay my ta bt u t vic xc
nh ng nng v th nng ca c h, xy dng hm Lagrange, sau a cc
hng thc vo phng trnh Lagrange, thu gn ta s nhn c phng trnh
chuyn ng ca c h Tay my.
xy dng m hnh ng lc hc tay my bng cch s dng phng
trnh Lagrange loi II, ta cn phi bit cc thng s sau y:
Khi lng cng nh ta ca khi tm ca cc khu,
Vn tc ca im bt k trn Tay my thit k,
Cc thng s v ma st ng, ma st tnh gia cc khu, khp v tc ng
nhiu nu c.
Do trong thc t, hot ng ca Tay my lun b nh hng bi cc lc ma
st v nhiu, nn ta s khi qut m hnh ng lc hc Tay my va nhn c
nh sau:
M (q)q V (q, q ) F (q ) G(q) d
vi q v c nh ngha trn. M(q) l ma trn qun tnh, V (q, q ) l vect

lc Coriolis/hng tm v G(q) l vect trng lc nh phn tch trn.
phng trnh khi qut trn, ta cng thm lc ma st vo , vi:
F (q ) Fv q Fd
trong Fv l ma trn h s ca ma st tnh v Fd l ma st ng. Ta s a
thm lng nhiu d vo phng trnh, i lng ny gip m t phn b cho
trng hp m hnh ng lc hc c sai st m ta cha lng ht trong qu trnh
xy dng m hnh ton.
Vic xc nh lc ma st rt kh khn, cch m t nh vy c chp nhn.
Hu ht nhng tr lc no chng li chuyn ng u c cc nh nghin cu
m t trong m hnh ng lc hc Tay my theo cch nh trn.
Phng trnh ng lc hc Tay my cng c biu din di dng:
N ( q, q ) d
M ( q)q
72
N (q, q ) V (q, q ) F (q ) G(q)

biu din cho c cc i lng phi tuyn.
5.4.2. Ma trn qun tnh
Ma trn qun tnh M(q) n x n c cc thnh phn c nh ngha bi biu
thc:
Ti
TiT
m jk ( q ) trace
Ii
q
q k
i 1
j
n
- Ti / q j m t s thay i v tr ca im thuc khu th i gy nn bi s

chuyn dch ca khu th j.
- Ii l ma trn qun tnh gi ca khu i v c xc nh di dng khai trin
nh sau:
x 2 dm
xy dm
I i i ri i riT dm
xz dm
x dm
yx dm
y dm
yz dm
y dm
2
zx dm
zy dm
z dm
z dm
2
x dm
y dm
z dm
dm
y cc gi tr c tnh trn khu th i. y l ma trn hng s v xc

nh gi tr mt ln cho mi khu. Ma trn ny ph thuc vo dng hnh hc v
s phn b khi lng ca khu i. Trong cc thnh phn qun tnh c phn
bit nh sau:
M men qun tnh:
I yy ( x 2 z 2 )dm
I zz ( x 2 y 2 )dm
I xx ( y 2 z 2 )dm
M men qun tnh ly tm:
I xz xz dm
I yz yz dm
I xy xy dm
73
my y dm
mz z dm
mx x dm
M men qun tnh bc nht:
vi m l tng khi lng khu i, v:

i
ri x
l bn knh vect biu din trng tm khu th i trong h ta i.

Ta c th vit :
I xx I yy I zz
I xy
Ii
I xz
mx
Vi
Ti / q j
I xy
I xz
I xx I yy I zz
2
I yz
I xx I yy I zz
I yz
2
mz
my
mx
my
mz
= 0, j>i ta c th vit ngn gn hn :
T TiT
m jk (q) trace
Ii
i max( j , k )
q j qk
n
y l mt ma trn i xng dng

5.4.3. Vect coriolis/hng tm
1 T
K
V ( q, q ) M ( q )q
( q M ( q )q ) M q
2 q
q
Cc thnh phn ca vect Coriolis/hng tm c xc nh nh sau:
V (q, q ) vijk qi q j
i, j
vijk
1 mkj mki mij
2 qi
q j
q k
5.4.4.Vect trng lc:

Ta c
74
G( q )
P( q )
q
( g T Ti ( q ))I i e 4
i 1 q
n

e4 = (0, 0, 0, 1)
T , ta suy ra c:
( g T Ti ( q )) I i e 4
i 1 q
n
T
G (q ) ( I n g T ) i I i e 4
q
i 1
n
G ( q )
, j 1,2..., n
y tht s ta c vect G(q) l:
n T Ti
g q I i e 4
1
in1
T
i
g T
I i e4
G( q ) i 2
q 2
T
T
i
g
I i e4
i n
q n
n y ta kho st bi ton ng lc hc Tay my t thu c
cc gi tr lc hay m men suy rng trn mi khp trong qu trnh hot ng ca
robot. Da trn nhng thng s ny ta s a ra nhng gii php thit k kt cu
cng nh iu khin robot tt hn. Bi b iu khin s n gin v c hiu qu
hn nu nhng c tnh ng lc hc bit ca Tay my c kt hp cht ch
ngay t trong giai on thit k.
5.5. ng dng bi ton ng lc hc m t i tng robot trong iu
khin.
Sau khi thc hin tnh ton bi ton ng lc hc robot, chng ta c th s
dng trc tip cc m hnh ton thu c xy dng i tng trong vic m
phng v a ra cc tng trong vn iu khin.
Tt nhin, vic xc nh cc thng s ca robot l rt kh khn, v vy
chng ta ch xy dng i tng robot c tnh cht m phng thc hin cc
gii thut iu khin. V trong thc t, cc thng s ca m hnh ng lc hc
tay my chu nh hng ca rt nhiu cc yu t nh : chnh xc trong gia
cng c kh, nh hng ca cc tc nhn c tnh cht nh nhiu, cc sai s m
hnh khi thc hin tnh ton...
Trong mc ny, bng cc phn mm h tr m phng (Visual C, Visual
Basic, Matlab, ...) chng ta thc hin m hnh ha cc robot t cc phng trnh
75
ng hc v ng lc hc. T c s ny c th thc hin thit k v ch to cc

robot thc thi cc mc tiu ra.
Chng ta s thc hin vic m hnh ha cc i tng robot tm hiu
cc chng trc :
a. Xy dng m hnh m phng iu khin v tr ca robot Puma, da vo cc
phng trnh ng hc tm c chng 4.
Hnh 5.6. M phng robot Puma theo v tr
Hnh 5.7. M phng qu o ca robot Puma.

b. Xy dng m hnh ton cho robot hai bc t do cu hnh RT.
Do tnh cht phc tp trong iu khin, vn ca nhng nh nghin cu l
lm sao c th tm gii thut iu khin cho robot khi m tt c cc khu t thit
76
k n thi cng u gp nhiu kh khn. Mt cng c rt hu hiu c a ra

l m hnh ton ca robot, nn tng ca m hnh ton l bi ton ng lc hc
c xt n. Mc chnh xc , chnh lch sai s m hnh... ph thuc
nhiu vo qu trnh tnh ton ng lc hc, trong khng loi tr cc kh nng
nh hng ca nhiu v cc vn khc lin quan n ng lc hc c h.
Chng ta quay li v d 5.2, t bi ton ng lc hc xy dng cho robot
hai bc t do, cu hnh RT thu c m hnh ton ca i tng robot.
Xt trn lnh vc iu khin, h robot l cc h phi tuyn, chnh v vy vic
iu khin v s dng cc gii thut phi tun theo cc nguyn tc iu khin h
phi tuyn.
Xy dng m hnh robot RT trong matlab :
U1
Theta
(Goc quay khop 1)
U2
d
(Do dai tinh tien d khop 2)

ROBOT_2DOF
Hnh 5.8. M hnh ton robot 2 bc t do RT

m phng thnh cng, chng ta cn chn cc thng s ca robot thch
hp. Cc thng s ny c th thu thp s liu hay la chn theo cc ti liu
c nghin cu.
1
u1
1
s
f(u)
theta_2dot
theta_dot
1
s
theta_
theta
2
u2
1
s
f(u)
d_2dot
d_dot
1
s
2
d
Hnh 5.9. M hnh ton t phng trnh ng lc hc robot.
77
d2

Ky Thuat Robot

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Ky Thuat Robot

Uploaded by

Copyright:

Available Formats

Khi c qua ti liu ny, nu pht hin sai st hoc ni dung km cht lng

xin hy thng bo chng ti sa cha hoc thay th bng mt ti liu cng

Thng tin lin h:

Bin son : Bi Th Cao

1.2.1. C c u i m khi s d ng Robot.

1.2.2. Mt s lnh vc ng d ng.

1.3. C c kh i ni m v robot - robot c ng nghi p.

nh ngha v robot c ng nghi p

1.3.2. C c thnh phn ca robot c ng nghi p

1.3.3. Bc t do ca robot c ng ghi p.

.3.4. H to trong robot.

1.4. Phn loi robot.

1.4.1. Robot c ng nghi p.

2. Robot song song.

Chng 2. Phn tch h c cn ng tnh v chuyn ng t y my.

2. .3. Momen ca lc i vi tm.

2. .6. C c ti n tnh hc.

2. .7. Mt s m hnh phn lc lin kt

2.2. Thit k h c cn bng tnh.

2.2.2. Thit k c kh.

2.2.3. Tnh to n ki m tra cn bng lc cho h .

2.3. Phn tch chuy n ng tay m y.

2.3. . Gii thi u.

2.3.4. Phn tch chuy n ng tng qu t ca tay m y.

2.3.5. Php bin i h to .

2.4. Phn tch chuy n ng ca mt s tay m y.

2.4. . Phn tch chuy n ng ca tay m y 2 khp quay.

2.4.2. Phn tch chuy n ng ca tay m y 3 khp quay.

2.4.3. Phn tch chuy n ng ca tay m y nhi u khp ni

Chng 3. Cc ph p i n i thu n nh t homogenous tr nsform tion

h c li c c php tnh v vect v ma trn.

3.2. . Php nhn vect.

3.2.2. C c php tnh v ma trn.

a. Php cng tr ma trn.

b. Tch hai ma trn.

c. Ma trn ngh ch o ca ma trn thun nh t.

e. o hm v tch phn ca ma trn.

3.3. C c php bin i d ng trong ng hc robot.

3.3. . Php bin i t nh tin.

3.3.2. Php quay quanh c c tr c to .

3.3.3. Php quay le Euler .

3.3.4. Php quay Roll Pitch -Yaw.

3.4. Bin i h to v mi quan h gi a c c h to .

inem tic equ tions

4. . . H to v mi quan h gi a c c khu trn robot.

4. .2. hu ch p hnh cui v i m t c ng cui.

di ph p tuyn chung an v gc o n ca khu n n.

hong c ch gi a hai khu dn v gc quay ca khu n n.

4.2.4. B thng s DH.

4.3. G n h to cho robot.

4.3.2. C c php bin i ma trn .

4.5. X c nh c c ma trn T theo ma trn .

4.6. Trnh t thit lp h phng trnh ng hc ca robot.

a. Chn h to c bn v g n c c h to trung gian.

e. it phng trnh ng hc ca robot.

4.6.2. d thit lp phng trnh ng hc mt s robot.

5. . M c ch v phng ph p kho s t ng lc hc Robot.

5.2. ng lc hc robot vi phng trnh Euler-Lagrange.

5.3. ho s t bi to n ng lc hc ca tay m y nhi u bc t do.

5.4. Phng trnh ng lc hc tay m y.

5.4.2. Ma trn qu n tnh.

5.4.3. ector Coriolis/hng tm.

5.4.4. ector trng lc

5.5. Xy dng Robot vi c tnh phi tuyn - ng d ng trong i u khi n