RBCN

Bi tp ln Robot Cng nghip KuKa Robot (6R)
V Ngc Trng CDT2_K52 1

LI NI U

Ngy nay robot c p dng rng ri trong hu ht cc lnh vc nh sn
xut, cng nghip, cng ngh cao nh v tr, hng khng, giao thng vn ti
Cc nc pht trin xut hin xu hng to ra nhng dy chuyn v thit b
t ng c tnh linh hot cao v xu hng ny t c kh nhiu kt qu
kh quan. Chnh v th vic nghin, ng dng v pht trin cc h thng sn
xut t ng linh hot ngy cng tr nn cp thit, nht l cc loi robot. Robot
cng nghip c cu trc ng hc ni tip tuy c nghin cu v pht trin
mnh song nhng tnh ton v ng dng ca n lun lun l mt lnh vc rt
phong ph, a dng v y cng l loi robot thng dng nht hin nay. Trong
c KUKA KR - mt loi tay my 6 bc t do hot ng rt linh hot, c
ng dng nhiu lnh vc sn xut v trong cng nghip. Trong n ny s
nghin cu loi robot ny v cc phn ng hc, ng lc hc, min lm vic
cng nh phn m phng lp trnh s dng cc phn mm h tr.

H Ni, 5/2011

V Ngc Trng
C in t 2 K52


CHNG 1
TM HIU TNG QUAN V ROBOT CHUI H
1.1. S ra i ca robot
Thut ng robot ln u tin xut hin nm 1922 trong tc phm
Rossums Universal Robot ca Karel Capek. Theo ting Sc th robot l
ngi lm tp dch. Trong tc phm ny, nhn vt Rossum v con trai ca ng
to ra nhng chic my gn ging nh con ngi hu h con ngi.
Hn 20 nm sau, ngay sau chin tranh th gii ln th 2, Hoa K xut
hin nhng tay my chp hnh iu khin t xa trong cc phng th nghim v
vt liu phng x.
Chic robot cng nghip c a vo ng dng u tin, nm 1961 mt
nh my t ca General Motors ti Trenton, New Jersey Hoa K.
Nm 1967 Nht Bn mi nhp chic robot cng nghip u tin t cng ty
AMF ca Hoa K (American Machine and Foundry Company). n nm 1990
c hn 40 cng ty Nht Bn, trong c nhng cng ty khng l nh
Hitachi v Mitsubishi a ra th trng nhiu loi robot ni ting. Cho n
nay, cng ngh ch to robot pht trin mnh v c ng dng nhiu trong
i sng v sn xut.
1.2. Phn loi robot
1.2.1. Phn loi theo s bc t do (BTD) trong trng cng tc
Robot c 2 loi chuyn ng c bn lm chun :
- Chuyn ng thng theo cc trc X, Y, Z trong khng gian -cc vi cc
khp lng tr hay khp trt, k hiu l P hoc T.
- Chuyn ng quay quanh cc trc X, Y, Z k hiu l R.
Ty s BTD v v cch t hp T-R, robot s hot ng trong trng cng tc
vi cc hnh khi khc nhau. V d: Robot c trng lm vic l hnh tr nh
RRT, RTT
1.2.2. Phn loi theo cu trc ng hc
Mt phng php phn loi Robot chnh l phn loi theo cu trc hnh hc
ca chng.
Mt robot c gi l robot chui h (robot tun t - serial robot) nu cu
trc ng hc ca chng c dng mt chui ng h; gi l robot song song

(parallel robot) nu cu trc ng hc ca chng c dng mt chui ng; v
gi l robot hn hp nu n bao gm c 2 loi ny.
1) Robot chui h (robot ni tip Serial Manipulators)
y l dng ph bin nht ca robot cng nghip. Thng c dng cu trc
ging cnh tay ngi, l mt chui cc khu rn lin kt vi nhau bng cc
khp (joints) c dng nh vai, khuu tay v c tay. Robot ny c u im chnh
l vng lm vic (workspace) rng v linh ng. Di y ta a ra cc b
phn chnh ca mt robot cng nghip:
- Cu trc c kh gm cc khu
(links) v khp (joints),
- C cu chp hnh (actuators) tc
ng ti cc khp lm tay my
chuyn ng,
- Cm bin (sensors) dng o
lng cc trng thi ca tay my
(cm bin trong) v trng thi mi
trng (cm bin ngoi) nu cn,
- Mt h thng iu khin (c c
my tnh) iu khin v gim
st chuyn ng ca tay my.
Nhc im:
- c im c hu l cng vng thp to mt cu trc ng hc.
- Sai s tch ly c khuch i t khu trc sang khu sau.
- Do phi mang v chuyn ng cng cc b dn ng (thng l cc ng c)
c khi lng ln nn tc nn tc lm vic khng cao.
- Kh nng mang ti khng cao.
Robot loi ny yu cu phi c t nht 6 BTD trong khng gian (ng vi 6
BTD ca i tng lm vic) c th tip cn c i tng v tr bt k
vi kh nng nh hng trong min lm vic ca n. Chnh v vy ch yu l
robot c 6 khp, nh cc loi robot Stanford (RRTRRR), KUKA (6R), Elbow
(6R), Puma (6R) vi ng dng chnh trong cng nghip hin nay l lp rp,
vn chuyn phi v sn phm trong qu trnh sn xut.

Hnh 1.1. IRB660-palletiser ca hng
ABB Robotics.

2) Robot song song (Parallel Manipulator)
Loi robot song song in hnh thng c bn chn di chuyn c ni vi
gi c nh bng mt s nhnh hay chn. Do vy, cc khu ch chu lc ko
hoc lc nn, khng phi chu un. Thng s chn chnh bng s BTD, cc
chn c iu khin bng mt ngun pht ng t trn gi c nh hoc ngay
trn thn ca chn.
u im:
- Kh nng chu ti ln v ti trng c
chia cho cc chn,
- Tc cao hn v chnh xc hn, cu
trc gn nh hn,
- Do tc cao v chnh xc cao nn c
th hot ng nh mt my phay, ngy
nay c p dng vo vic phay cc
b mt gia cng cn chnh xc cao,
nng sut.
- Nhiu ng dng: M phng to song,
gh li my bay (Steward, 1965), bn khung lp rp (Reinholtz v Gokhale,
1987), bn iu khin dao, g ca my phay (Arai, 1991), cc thit b nh im
(Gosselin v Hamel, 1994), cc my chuyn ng c chn bc (Waldron,
1984).
Tuy nhin n cng c mt s nhc im v gp phi mt s tr ngi nh
khng gian lm vic nh, cc chn c th va chm ln nhau cng nh vic thit
k rt kh khn.
3) Robot di chuyn (Mobile Robot)
n gin nh loi t, xe p vi cc bnh li c 2 BTD. Hin nay dng
trong chuyn ch vt liu trong nh may rng, bnh vin, vn phng
4) Robot ging ngi (Humanoid Robot)
Ni mt cch n gin, robot ny c dng hnh dng con ngi, 2 chn, 2
tay, u, v dung cc c cu vi chuyn ng nh cc b phn ca con
ngi.
Hnh 1.2. ABBs IRB340 palletising robot

Cc kiu phn loi khc nh phn loi theo H thng truyn ng, theo
Phng php iu khin, theo c im hot ng ty cc tiu ch t ra m
robot c phn loi nhm nu bt tiu ch .
1.3. Tay my (Manipulator)
Thut ng tay my v robot trong quan nim ca nhiu nh chuyn mn
trong lnh vc ny khng c s khc bit. thun tin trong trnh by, y
ta hiu:
Tay my l mt dng robot c cu to m phng theo nhng c im cu
to c bn ca cnh tay ngi. Cng c th hiu tay my l tp hp cc b phn
v c cu c kh c thit k hnh thnh cc khi c chuyn ng tng i
nhau, c gi l cc khu ng. Trong phn lin kt gia cc khu ng
c gi l cc khp ng hay cn gi l cc phn t thc s thc hin cc
chuyn ng vn hnh tay my nh ng c in, xylanh du p, xylanh kh
nn , Phn quan trng khc trn cc tay my l b phn hay khu tc ng
cui thao tc trn i tng lm vic thng l cc tay gp hoc cc u
cng c chuyn dng.
Tay my hay c th gi l cnh tay c kh ca robot cng nghip thng
thng l mt chui ng h c to thnh t nhiu khu c kt ni vi
nhau nh cc khp ng. Khu cui ca tay my thng c dng mt tay gp
hoc c gn dng c cng tc. Mi khu ng trn tay my c ngun dn
ng ring, nng lng v chuyn ng truyn n cho chung c iu khin
trn c s tn hiu nhn c t b phn phn hi l cc cm bin nhm thng
bo trng thi hot ng ca cc khu chp hnh, trong vn c c bit
quan tm l v tr vn tc dch chuyn ca khu cui khu th hin kt qu
tng hp cc chuyn ng ca cc khu thnh phn.
1.4. Mt s robot chui h thng gp
1.4.1. Tay my ta vung gc (Cartesian robot/ Gantry robot)
Robot hot ng trong h ta ny c minh ha nh hnh v gm ba
chuyn ng nh v X,Y,Z theo cc trc ta vung gc:
Loi robot ny thch hp vi cng vic gp vt th v t chng vo v tr
khc. Cc ng dng c th l ng gi, lp rp my mc, cm nm cc my
cng c hoc hn theo hnh cung. i vi loi robot ny, cnh tay c to
thnh t 3 khp trt m trc ca chng trng vi h trc ta -cc.

u im:
- Khng gian lm vic ln, c th
di n 20m,
- i vi loi gn trn trn s dnh
c din tch sn ln cho cc
cng vic khc,
- H thng iu khin n gin.
Hn ch: Vic thm vo cc loi cn
trc hay cc loi thit b vn chuyn vt
liu khc trong khng gian lm vic
ca robot khng c thch hp
lm.Vic duy tr v tr ca cc c cu dn ng v cc thit b dn ng in i
vi loi trn u gp nhiu tr ngi.
1.4.2. Tay my ta tr (Cylindrical robot)
Trong ba chuyn ng chnh , robot c trang b hai chuyn ng tnh tin
v mt chuyn ng quay.
S dng trong cng ngh lp rp,
cm nm cc my cng c, hn
im. Loi robot ny, trc c dng
h ta tr.
u im :
- C kh nng chuyn ng ngang
v su vo trong cc my sn xut,
- Cu trc theo chiu dc ca my
li nhiu khong trng cho sn,
- Kt cu vng chc, c kh nng
mang ti ln,
- Kh nng lp li tt.
Nhc im duy nht l gii hn tin v pha tri v pha phi do kt cu c
kh v gii hn cc kch c ca c cu tc ng theo chiu ngang.

Hnh 1.3. Tay my ta vung gc
(Cartesian Robot)
Hnh 1.4. Tay my ta tr (Cylinderical
Robot)

1.4.3. Tay my ta cu (Spherical/Polar robot)
Robot loi ny c b tr c t nht hai
chuyn ng quay trong ba chuyn ng
nh v. Dng robot ny l dng s dng
iu khin servo sm nht.
c s dng cm nm cc dng c,
hn im, nh bng, hn hi hoc hn
theo cung. Trc ca robot to thnh h ta
cu.
1.4.4. Tay my ton khp bn l
Loi cu hnh d thc hin nht c
ng dng cho robot l dng khp ni bn
l v k l dng ba trc thng, gi tt l dng SCARA (Selective Compliance
Assembly Robot Arm). Dng ny v dng ta tr l ph cp nht trong ng
dng cng nghip bi v chng cho php cc nh sn xut robot s dng mt
cch trc tip v d dng cc c cu tc ng quay nh cc ng c in, ng
c du p, kh nn.
c s dng gp nh vt, ng gi, lp rp hay cm nm my cng c.
Robot c 2 khp xoay song song vi nhau cho php thao tc trn mt phng.
u im :
- Mc d chim din tch lm
vic t song tm vn kh ln,
t l kch thc - tm vn
c nh gi cao.
- V mt hnh hc, cu hnh dng
khp ni bn l vi ba trc
quay b tr theo phng thng
ng l dng n gin v c
hiu qu nht trong trng hp yu cu gp t v t chi tit theo
phng thng. Trong trng hp ny bi ton ta hoc qu o
chuyn ng i vi robot ch cn gii quyt hai phng x v y cn li
bng cch phi hp ba chuyn ng quay quanh ba trc song song vi
trc z.

Hnh 1.5. Tay my ta cu (Polar
Robot)
Hnh 1.6. Robot SCARA

1.4.5. V KUKA Robotics

Hnh 1.7. Robot KUKA KR 6/2 v 15/2 ca KUKA Robotics

KUKA c thnh lp vo nm 1898 ti
Augsburg, c, tn l Keller und Knappich
Augsburg . C cng ty m l KUKA Roboter
GmbH, t ti Augsburg - c, KUKA
Robotics l mt trong nhng nh sn xut hng
u th gii v robot cng nghip vi khi
lng hng nm t gn 10.000 con. Di robot 5
v 6 trc c ti trng t 3kg n 570 kg v tm
vi t 635mm n 3700mm, u c iu
khin t mt nn tng b iu khin da trn
PC; ring KUKA KR 1000 robot khe nht
th gii ca KUKA, l loi robot c 6 trc c
lm bng titan. Kh nng nng ca n ln n
1000 kg v tm vi l 3200 mm. V n c thit k nhm ti cc ng dng
c bit nng. Do vy, n rt l tng cho cc ngnh cng nghip nh sn xut
vt liu xy dng, t v git l.
Robot ca KUKA c s dng rng ri trong nhiu ngnh cng nghip,
gm sn xut thit b gia dng, sn xut t, khng gian, sn xut hng tiu
dng, ch bin thc phm, dc, y, c khun, nha, v trong nhiu ng dng
phc hp khc nh vn chuyn vt, lp rp, ng gi, xp-d pallet, hn, un,
nh bng b mt. Thng tin chi tit v tp on c ti trang web
www.kukarobotics.com.

Hnh 1.8. KUKA KR 1000 robot
khe nht th gii ca KUKA


Vi hnh nh khc v robot ca hng KUKA :

Hnh 1.9. Zepnick Solutions chn
KUKA Robotics cung cp Robot bng
thp khng g KUKA KR 15 SL cho h
thng x l v ng gi thc phm
mi ca cng ty.

Hnh 1.10. Mt khoang lm vic ca robot
hng KUKA ang thao tc vi cc thng
ung.
Hnh 1.11. Mt khoang lm
vic ca robot ang b bnh
m nht t bng chuyn vo
hp.

Hnh 1.12. H thng robot t ng
phc v cho cc quy bar ca hng
Motoman ang dng mt trong 2 b
kp ly cc.


1.5. ng dng ca robot
ng dng ch yu ca robot cng nghip l hn v lp rp. Gn 25% robot
cng nghip l robot hn. Cc robot lp rp chim 33% dn s robot trn th
gii, c mt nhiu nht trong cc nh my sn xut xe hi v in t.
.

Hnh 1.14. Cc cnh tay robot hn.

Hnh 1.13. Robot hn Motoman EA1400

CHNG 2
TNH TON NG HC V NG LC HC ROBOT
KUKA 6 BC T DO

2.1. Tnh ton ng hc robot KUKA 6 bc t do
y l bc c s cho vic
thit k s b robot, t c
th gii bi ton iu khin
robot theo cc qu o. T y
ta mi c cc thng s
iu khin robot theo mt qu
o cho trc hoc vi lc cho
trc ta thu c mt qu o
chuyn ng nht nh.
Di y l phn tnh ton
ng hc cho Robot 6 bc t do
m c th ng dng gii bi
ton ng hc cho Robot
KUKA KR6/2.

Thc cht ca vic gii bi ton ng hc l i gii 2 bi ton ng hc sau:
+ Bi ton ng hc thun: Bit cc gc quay ca cc khp, tm v tr v hng
ca khu chp hnh cui cng.
+ Bi ton ng hc ngc: Cho bit v tr ca khu chp hnh cui, tm cc
gc quay ca cc khp tng ng, tc l tm cc thng s iu khin cho tng
khu m bo v tr ca khu chp hnh.

Do tnh cht ca cng vic ca tng robot m qu o hot ng ca n to
ra mt s bi ton v iu khin nh sau:
- Nu khng quan tm n qu o chuyn ng m ch quan tm n vic
trong khong thi gian cho trc, robot i ti ng v tr im lm vic.
Do vy m bi ton tr nn n gin hn. N ch cn tha mn cc iu
Hnh 2.1. Cc khp ca Robot KUKA

kin trong phn thit k qu o l c; tc l vn tc im u v
im cui bng 0.
- Do iu kin lm vic m khi di chuyn t im u n im cui,
chng bt buc phi i qua mt s im trung gian nht nh. Dng ny
chng ta c th dung php ni suy to ra mt qu o trn i qua cc
im . iu kin lin tc v qu o v vn tc c tho mn.
- Khu chp hnh phi i theo mt qu o cho trc v c th bit trc
lut chuyn ng dc theo qu o no . y l bi ton iu khin
robot i theo mt qu o vi vn tc bit trc.
y chng ta trnh by phn tnh ton cho trng hp ny: iu khin
robot i theo mt qu o cho trc.
2.1.1. Phng trnh ng hc thun

Trc Gii hn chuyn ng Tc (
0
/sec)
1
185
0
152
2 +115
0
ti -55
0
152
3 +70
0
ti -210
0
152
4
350
0

250
5
130
0

357
6
350
0

660

Bng 2.1. Gii hn chuyn ng ca cc khp ca robot KUKA
1) B thng s DH
Ta xy dng mi quan h ng hc thng qua b thng s DH :
Theo Denavit & Hartenberg, hai ng xut dng ma trn thun nht 4x4
m t quan h gia 2 khu lin tip trong c cu khng gian.
Trc ht, xc nh b thng s c bn gia 2 trc quay ca 2 khp ng i+1
v i:
- a
i
l di ng vung gc chung gia 2 trc khp ng i+1 v i,
- o
i
l gc cho gia 2 trc khp ng i+1 v i,
- d
i
l khong cch o dc trc khp ng i k t ng vung gc chung
gia trc khp ng i+1 v trc khp ng i ti ng vung gc chung
gia trc khp ng i v trc khp ng i-1.
-
i
l gc gia 2 ng vung gc ni trn.

Bin khp :
- Nu khp ng i l khp quay th
i
l bin khp
- Nu khp ng i l tnh tin th d
i
l bin khp
k hiu bin khp, ta dng thm du * v trong trng hp khp tnh tin
th a
i
c xem l bng 0.
Hnh 2.2 biu din cc khp
quay t trn robot KUKA KR 6/2.
Trong c th gi cc khp
A1, A2, A3 ln lt l khp hng
(waist), khp b vai (shoulder),
khp khuu tay (elbow); tp hp c
3 khp ny gi l cnh tay (arm),
cc khp A1, A2, A3 ny quyt
nh v tr ca khu chp hnh cui
(im P trn hnh 2.3 di y).
Cc khp A4, A5, A6 cn li
c gi chung l khp c tay
(wrist), chng quyt nh hng
ca khu chp hnh cui v c tm
quay ti im P.
2) Thit lp cc h ta
Khi gn cc h ta vi cc khu, c mt s lu sau:
Gc ca h ta gn lin vi khu th i (gi l h ta th i) t ti giao
im gia 2 ng vung gc chung (a
i
) v trc khp ng i+1. Trng hp 2
trc giao nhau th gc h ta ly trng vi giao im . Nu 2 trc song
song vi nhau th chn gc h ta l im bt k trn trc khp ng i+1.
Trc z
i
ca h ta th i nm dc theo trc khp ng i+1.
Trc x
i
ca h ta th i nm dc theo ng vung gc chung v hng
t khp ng i n khp ng i+1. Trng hp 2 trc giao nhau th hng ca
trc x
i
trng vi hng ca vect tch z
i
z
i-1
, tc l vung gc vi mt phng
cha z
i
, z
i-1
v phng ca vect tch c hng.

Hnh 2.2. Cc khp ca Robot KUKA KR 6/2.

Hnh v sau y th hin cc h ta t trn robot KUKA KR 6/2 v cc
thng s ng hc theo bng DH:

Hnh 2.3. S b tr cc h ta v cc thng s ng hc cho Robot KUKA.

Sau khi gn cc h trc ta trn cc khu v khp ca robot theo nguyn
tc trnh by trn, ta c cc thng s cho bng DH nh sau:

Khu
i
d
i
a
i
o
i
Khp
1
1
*
0 a
1
= 300 90
0
R
2
2
*
0 a
2
= 650 0
0
R
3
3
*
0 a
3
= 155 90
0
R
4
4
*
d
4
= 600 0 -90
0
R
5
5
*
0 0 90
0
R
6
6
*
d
6
= 125 0 0
0
R

Bng 2.2. Bng thng s DH.

3) M hnh bin i
Trn c s xy dng cc h ta vi 2 khu ng lin tip nh trn
trnh by, ta c th thit lp mi quan h gia 2 h ta lin tip theo 4 bc
sau y:
1 - Quay quanh trc z
i-1
mt gc
i
2 - Tnh tin dc trc z
i-1
mt on d
i
3 - Tnh tin dc trc x
i-1
( trng vi x
i
) mt on a
i
4 - Quay quanh trc x
i
mt gc
i
.
Bn bc ny c biu din bng tch cc ma trn thun nht sau:
A
i
= R(z,
i
).T
p
(0,0,d
i
).T
p
(a
i
,0,0).R(x,
i
) (2.1)
Sau khi thc hin cc php nhn cc ma trn n gin (quay, tnh tin), ta c:

i i i i i i i
i i i i i i i
i
i i i
C S C S S a C
S C C C S a S
A
0 S C d
0 0 0 1
u u o u o u
u u o u o u
o o
(
(
(
=
(
(

(2.2)
Thay cc gi tr trong bng DH vo cng thc A
i
ta tnh c cc ma trn A
i

nh sau:
1 1 1 1
1 1 1 1 0
1
C 0 S a C
S 0 C a S
A
0 1 0 0
0 0 0 1
(
(
(
=
(
(

2 2 2 2
2 2 2 2 1
2
C S 0 a C
S C 0 a S
A
0 0 1 0
0 0 0 1
(
(
(
=
(
(

3 3 3 3
3 3 3 3 2
3
C 0 S a C
S 0 C a S
A
0 1 0 0
0 0 0 1
(
(
(
=
(
(

4 4
4 4 3
4
i 4
C 0 S 0
S 0 C 0
A
0 1 C d
0 0 0 1
o
(
(
(
=
(
(

5 5
5 5 4
5
C 0 S 0
S 0 C 0
A
0 1 0 0
0 0 0 1
(
(
(
=
(
(

6 6
6 6 5
6
6
C S 0 0
S C 0 0
A
0 0 1 d
0 0 0 1
(
(
(
=
(
(


4) Phng trnh ng hc
Ma trn T
i
l tch cc ma trn A
i
v l ma trn m t v tr v hng ca h
ta gn lin vi khu th i, so vi h ta c nh: T
i
= A
1
A
2
A
i
, i = 1..n
Trong trng hp i = n, vi n l s hiu ch h ta gn lin vi im tc
ng cui (End-Effector) th ta c:
T
n
= A
1
A
2
A
n
cng chnh l ma trn trng thi cui T
E
: T
E
= T
n

hay:
x x x x
y y y y
n
z z z z
n s a p
n s a p
T
n s a p
0 0 0 1
(
(
(
=
(
(

y ta gi ta im cui l D(d
x
, d
y
, d
z
) cn im P quyt nh v tr ca
D.
V vy v tr v hng ca khu chp hnh cui c cho bi ma trn :

x x x x
y y y y 0 0 1 2 3 4 5
6 1 2 3 4 5 6
z z z z
n s a d
n s a d
A A . A . A . A . A . A
n s a d
0 0 0 1
(
(
(
= =
(
(

(2.3)
- Nhn 3 ma trn u vi nhau ta c:
0 0 1 2
3 1 2 3
A A . A . A = =
( )
( )
1 23 1 1 23 1 1 2 2 3 23
1 23 1 1 23 1 1 2 2 3 23
23 23 2 2 3 23
C C S CS C a a C a C
S C C S S S a a C a C
S 0 C a S a S
0 0 0 1
+ + (
(
+ +
(
=
(
+
(

(2.4)
- Nhn 3 ma trn cn li:
3 3 4 5
6 4 5 6
A A . A . A = =
4 5 6 4 6 4 5 6 4 6 4 5 6 4 5
4 5 6 4 6 4 5 6 4 6 4 5 6 4 5
5 6 5 6 5 4 6 5
C C C S S C C S S C C S d C S
S C C C S S C S C C S S d S S
S C S S C d d C
0 0 0 1

(
(
+ +
(
=
( +
(

(2.5)
Ta c phng trnh cn bng sau:

0 0 3
6 3 6
A A . A = (2.6)
Trong cc phng trnh trn: C
i
cos
i
S
i
sin
i
C
ij
cos(
i
+
j
) S
ij
sin(
i
+
j
)

Thay cc phng trnh (2.3), (2.4), (2.5) vo PT (2.6) v cn bng cc phn t
ta c h 12 phng trnh:
n
x
= C
1
[C
23
(C
4
C
5
C
6
S
4
S
6
) S
23
S
5
C
6
] + S
1
(S
4
C
5
C
6
+ C
4
S
6
)
n
y
= S
1
[C
23
(C
4
C
5
C
6
S
4
S
6
) S
23
S
5
C
6
] C
1
(S
4
C
5
C
6
+ C
4
S
6
)

n
z
= S
23
(C
4
C
5
C
6
S
4
S
6
) + C
23
S
5
C
6
s
x
= C
1
[-C
23
(C
4
C
5
C
6
+ S
4
S
6
) + S
23
S
5
C
6
] + S
1
(-S
4
C
5
S
6
+ C
4
C
6
)

s
y
= S
1
[-C
23
(C
4
C
5
C
6
+ S
4
S
6
) + S
23
S
5
C
6
] C
1
(-S
4
C
5
S
6
+ C
4
C
6
)

s
z
= - S
23
(C
4
C
5
C
6
+ S
4
S
6
) C
23
S
5
S
6

a
x
= C
1
(C
23
C
4
S
5
+ S
23
C
5
) + S
1
S
4
S
5

a
y
= S
1
(C
23
C
4
S
5
+ S
23
C
5
) C
1
S
4
S
5

a
z
= S
23
C
4
S
5
+ C
23
C
5

d
x
= C
1
[a
1
+ a
2
C
2
+ a
3
C
23
+ d
4
S
23
+ d
6
(C
23
C
4
S
5
+ S
23
C
5
)] + d
6
S
1
S
4
S
5
d
y
= S
1
[a
1
+ a
2
C
2
+ a
3
C
23
+ d
4
S
23
+ d
6
(C
23
C
4
S
5
+ S
23
C
5
)] d
6
S
1
S
4
S
5
d
z
= a
2
S
2
+ a
3
S
23
+ d
4
C
23
+ d
6
(S
23
C
4
S
5
C
23
C
5
).
Nh vy khi bit c cc gi tr ca
i
ta hon ton c th xc nh c
hng v v tr ca khu chp hnh cui bng cch thay cc gi tr ny vo 12
phng trnh trn.
Bi ton ng hc thun c th dng MATLAB lp mt hm hoc chng
trnh chung gii ra 12 nghim c di dng ch hoc tnh ton vi s, c
trnh by phn ph lc.
2.1.2. Phng trnh ng hc ngc
Theo phng php chung khi gii bi ton ng hc ngc, vi n = 6, kh
khn l ch lm sao xc nh c phng trnh no trong cc h phng
trnh tnh ra c cc gc
i
dng cng thc vi cc k hiu tng qut.
C th ni cha c mt thut ton chung no m nh c th tm ra c
tp nghim ca bi ton ng hc ngc cho mi robot. Phng php gii thiu
sau y c th xc nh c
i
d dng v trc gic hn: Phng php cc
nhm 3:
Nh bit, v tr ca mt im hon ton c xc nh trong h ta
khng gian 3 chiu, nn c th phn robot ra nhng nhm 3, ni chung c
vit thnh n = 3+3+3+ Tuy nhin, gc ta ca nhng nhm 3 k t nhm
th 2 tr u l i tng cn c xc nh. Cn i vi nhng robot dng
trong cng nghip thng c khng qu 6 BTD nn vic phn nhm 3+3 li

ph hp vi cc thao tc trong thc t. Thao tc u tin l robot a bn kp
n gn v tr lm vic. C th gi tn l thao tc th. Sau bn kp c x
dch nh v xoay hng sao cho ph hp vi cng vic, gi l thao tc tinh.
Vy cc nhm 3+3 tng ng vi cc thao tc th + tinh.
Vic gii bi ton ng hc ngc tr nn n gin hn nhng phi lu
n cc vn sau:
- i vi nhm 3 thc hin thao tc th th thng ch quan tm n v
tr, m khng quan tm n nh hng nn cc thnh phn ca cc vect
n, s, a u bng 0.
- i vi cc nhm 3 thc hin thao tc tinh th ngc li, cho cc thnh
phn ca vect p bng 0.
Nhc im ca phng php ny, nh ni trn, gc ta ca cc
nhm 3, k t nhm th 2 tr i u l nhng v tr cn xc nh hoc cho
trc. Thng thng ngi ta cho trc chng nh nhng im ta cng ngh.
Mc d c th dng h 12 PT trong phn tnh ton ng hc thun trn
gii bi ton ng hc ngc nhng do chng u l cc phng trnh phi
tuyn nn rt kh gii. Tuy vy, y KUKA l loi robot c c im 6 khp
u l khp quay, trong tm trc quay ca 3 khp cui giao nhau ti 1 im,
thng thng vi cu trc ny th mi im trong khng gian robot s c 8 kh
nng t c n im ny, hay ni cch khc, vi mi im xc nh th
bi ton ng hc ngc s c 8 nghim.
V vy, gii bi ton ng hc ngc ta chia lm 2 bc, bc 1 da vo
phng php hnh hc gii tch tm cc gc lin quan n v tr ca khu
chp hnh cui (
1
,
2
,
3
) Phng trnh ng hc ngc V tr sau
bc 2 c nhim v tnh nt cc gc cn li t cc gc tm c Phng
trnh ng hc ngc Hng.
T cc thnh phn ca phng trnh (2.6) v 12 phng trnh thnh phn ta
c th bit c v tr ca im tm quay c tay (im P trn hnh 1.15):

x x 6 x
y y 6 y
z z 6 z
p d d a
p d d a
p d d a
( (
( (
=
( (
( (

(2.7)


1) Phng trnh ng hc ngc - V tr (Position)
Trn hnh v 2.4 l hnh chiu bng ca cnh tay robot trong 2 trng hp
ca gc
1
.

Hnh 2.4. Hnh chiu bng ca cnh tay
- Gc
1
c tnh t P
y
v P
x
:

1
= atan2(P
y
, P
x
); (2.8)
Theo hnh v,
1
c 2 nghim l
1
v (
1
+ 180
0
), cng u tha mn nghim
ca phng trnh (2.8). Trng hp P
x
= P
y
= 0, phng trnh (2.8) s c v s
nghim, do vy hm atan2 s chn nghim ca
1
= 0.
- Gc
3
c tnh da vo hnh 2.5:

Hnh 2.5. Hnh chiu vung gc vi mt phng cnh tay

S dng nh l hm s cos, ta c:
2 2 2 2 2 2 2
2 3 4 2 3 4 3
q + r = a a d 2a a d .cos(180 ) + + + u (2.9)

2
3 3 3 4 3
atan2( 1 D , D ) atan2(d , a ); u = + + (2.10)
vi
2 2 2 2 2
2 3 4
3
2 2
2 3 4
q r a a d
D
2a a d
+
=
+
; (2.11)
- Gc
2
tnh da vo gc v nh trn hnh v 2.5.
Ta c:
2 2 2 2 2 2 2
3 4 2 2
a d = a q + r 2a q + r .cos + + (2.12)

2
2 2
atan2( 1 D , D ); = (2.13)
vi
2 2 2 2 2
3 4 2
2
2 2
2
a d a q r
D
2a q r
+
=
+
; (2.14)
T :

2
= = atan2(r,q)
2
2 2
atan2( 1 D , D ); (2.15)
Nh vy, cc gc (
1
,
2
,
3
) tnh c v t hp ca chng s cho ra 8
nghim ca b (
1
,
2
,
3
).

2) Phng trnh ng hc ngc - Hng (Orientation)
Hng ca khu chp hnh cui (gn dng c) c gii sau khi bit
c cc gc v tr (
1
,
2
,
3
).
Trong cng thc (2.6), nu ch xt n thnh phn nh hng (ma trn R
3x3
),
ta c:

0 0 3
6 3 6
R R . R =

3 0 T 0
6 3 6
R ( R ) . R = (2.16)

0
R
6
c gii t phn ng hc thun, cn
0
R
3
c khi gii ra cc gc
(
1
,
2
,
3
) theo PT ng hc ngc v tr trn.
Cn
3
R
6
bao gm cc thnh phn ma trn quay ca (
4
,
5
,
6
) theo (2.5) v l
cc gi tr cn c xc nh:

4 5 6 4 6 4 5 6 4 6 4 5
3
6 4 5 6 4 6 4 5 6 4 6 4 5
5 6 5 6 5
C C C S S C C S S C C S
R S C C C S S C S C C S S
S C S S C
(
(
= + +
(
(

(2.17)

Gi s cc thnh phn ca tch ca 2 ma trn v phi (2.16) sau khi tnh c
l ma trn [b
ij
]
3x3
:
(
0
R
3
)
T
.
0
R
6
=
1 23 1 23 23 x x x
1 1 y y y
1 23 1 23 23 z z z
C C S C S n s a
S C 0 . n s a
CS S S C n s a
( (
( (
( (
( (

=
11 12 13
21 22 23
31 32 33
b b b
b b b
b b b
(
(
(
(

(2.18)
T , sau khi nhn c v phi theo (2.18) ra cc gi tr b
ij
ri ng nht
vi cc phn t ca (2.17) ta c c cc nghim (
4
,
5
,
6
):
C 2 b nghim ca (
4
,
5
,
6
):

4
= atan2(b
23
, b
13
);
4
= atan2(b
23
, b
13
) + 180
0
;

5
= atan2
( )
2
33 33
1 b , b ;
5
= atan2
( )
2
33 33
1 b , b ;

6
= atan2(b
32
, -b
31
);
6
= atan2(b
32
, -b
31
) + 180
0
;


CHNG 3
CHNG TRNH M PHNG KT QU TNH TON

3.1. M phng s dng th vin OpenGL vi MFC
Cng c m phng :
+ Ngn ng lp trnh C++ : Xy dng giao din bng MFC.
+ Tm hiu lp trnh ha OpenGL m phng qua cc khi ha.
Phn mm s dng:
+ Visual Studio 2008
+ SolidWorks 2010
+ Matlab R2010b vi Robotics Toolbox for Matlab (release 8)
Copyright Peter Corke 1992-2008 http://www.petercorke.com

3.1.1. Gii thiu v OpenGL
OpenGL (Open Graphics Library) l mt th vin ha tc cao v c
lp vi h thng giao din cc h iu hnh. Tin thn ca OpenGL l IRIS GL
do hng Silicon Graphic Library Inc pht trin cho cc WorkStation ha tc
cao t nm 1982.Sau t nm 1992 th OpenGL tr thnh mt chun
cng nghip v c tnh k thut ca OpenGL do y ban k thut ARB
(Architectural Review Board ) ph chun.
Hnh 3.1. Khi lp phng rng v c v bng OpenGL.

OpenGL l mt tiu chun k thut ha c mc ch nh ra mt giao din
lp trnh ng dng (ting Anh: API) ha 3 chiu. OpenGL cng c th c

dng trong cc ng dng ha 2 chiu. Giao din lp trnh ny cha khong
250 hm v cc cnh phc tp t nhng hm n gin. N c dng rng
ri trong cc tr chi in t. Ngoi ra n cn dng trong cc ng dng CAD,
thc t o, m phng khoa hc, m phng thng tin, pht trin tr chi.
OpenGL cn c mt i th cnh tranh l DirectX ca Microsoft.
3.1.2. Gii thiu ngn ng lp trnh Visual C++
Visual C++ l mt trong cc ngn ng lp trnh thuc dng Visual Studio
ca hng Microsoft, pht trin da trn nn tng l ngn ng lp trnh hng
i tng C++. Visual C++ ra i khong nhng nm 1990 v cng ngy cng
c s dng rng ri nht l cc ng dng trong k thut nh l lp trnh
nhng, lp trnh iu khin v lp trnh m phng. Vic to ra cc giao din
phc tp v trnh by p i vi Visual C++ kh l n gin, v y chnh l
th mnh ca Visual C++ trong vic tr gip c lc cho ngi lp trnh khi
xy dng nhng d n ln hoc trong k thut lp trnh h thng.

Hnh 3.2. Giao din VC++ v to Project MFC.

MFC l lp nn tng ca Microsoft (Microsoft Foundation Classes), l mt
cng c mnh trong lp trnh ng dng cho hu ht cc lnh vc v n c giao
din p ng mi nhu cu ca ngi s dng trong vic thit k cc giao din
ca chng trnh m phng.
3.1.3. Gii thiu phn mm ha SolidWorks
Phn mm SolidWorks l mt phn mm h tr thit k c kh (CAD) cho
php ta v v th hin cc i tng trong khng gian 3 chiu (3D) mt cch
trc quan vi giao din thn thin d s dng cng nh vi h thng Help rt
chi tit v d hiu ; n cn cho php lu v m file di nhiu dng khc nhau
gip lin kt n nhiu phn mm khc nhau. y ta la chn nh dng file
STL(Standard Template Library) chun b cho vic m phng.

Hnh 3.3. To file STL t Solidwork (2010)


3.1.4. Kt qu lp trnh m phng
Bi ton ng hc c m phng s dng giao din MFC v lp trnh vi
phn mm VS9.0 kt hp vi th vin ha OpenGL. Cc khp ca robot
c v trn SolidWorks 2010 v xut sang file STL to models cho vic
m phng bi ton ng hc ngc c trc quan.
1) Cc khu chnh ca robot KUKA KR6/2


2) Cc phn chnh trong chng trnh lp trnh trn MFC
Chng trnh m phng s dng th vin OpenGL gm cc lp v vect, lp
t mu cho cc khu (material), lp iu khin bn phm v chut, lp v
qu o, lp c cc hm tnh tin v quay cc khu (models),
Khi to mt Project mi theo MFC, chng ta c cc lp c to sn bi
Wizard : Lp CFormdata, CKUKAView, CMainFrame,
Ta c th t phn tnh ton lp no cng c, sao cho thun tin khi truy
nhp d liu gia cc lp.
y, ta ly lun lp CKUKAView t cc lnh v v hin th cng hm
Calculate() tnh ton m phng bi ton ng hc ngc ; lp CFormdata
nhn cc s kin xy ra trn lp nhp d liu, chng trnh lm th l
cc nt radio, cc button iu khin robot i theo qu o nh.


3) Kt qu m phng robot KUKA 6 bc t do

Hnh 3.11. M phng Bi ton ng hc ngc theo qu o trn.


Hnh 3.12. M phng BT ng hc ngc theo qu o thng.

3.2. MATLAB
MATLAB l vit tt ca Matrix Laboratory phn mm ca hng
MathWorks, Inc. Ch ring tn gi ca n mch bo i tng ca n l cc
ma trn. Vect v cc i lng v hng c biu din nh l ma trn nx1 v
1x1 tng ng. Cc xu k t cng c s dng trong MATLAB. Matlab tch
hp cc phng php tnh ton, hin th vi ngn ng lp trnh mnh cung
cp cho ngi dng mi trng lm vic tin li gii cc vn tnh ton
khoa hc.
Vi kh nng tnh ton mnh v ma trn, ta c th lp mt chng trnh tnh
ton cc kt qu ca bi ton ng hc thun cng nh ng hc ngc da
trn cc thng s u vo.
Trong phm vi bi tp ln, em thit k hai chng trnh GUI trn
matlab cho php tnh ton ng hc thun v ngc cho robot KuKa.


Thc hin kim tra li kt qu tnh ton nh trn hnh v, ta thy c sai s gia
chng trnh tnh ton ng hc thun v ng hc ngc. Nguyn nhn dn

ti sai s l do vic np gi tr u vo ca cc gc quay theta bi ton ng
hc thun cha ng vi gc theta c tnh ton bi ton ng hc ngc.
Tuy nhin, sai s ny nh hn 10% ( tnh ton vi ta im cui ) v c th
chp nhn c.
M ngun ca cc chng trnh m phng v tnh ton c gi km theo bi
bo co.


TI LIU THAM KHO

[1] Nguyn Thin Phc. Robot Cng nghip. NXB KHKT. H Ni, 2006.
[2] Ng Mnh Hin. Lun vn Thc s: Nghin cu ng hc v ng lc hc
iu khin robot, ng dng trn robot KUKA KR 6/2. HBK H Ni, 2004.
[3] Nguyn c Ngha. Nhp mn MATLAB BM Khoa hc my tnh. H Ni,
2002.
[4] Websites:
1 - KUKA Robotics: www.kukarobotics.com/
2 - The MathWorks, Inc. www.mathworks.com/
3 - A Practical Guide to STL:
http://www.codeproject.com/KB/stl/PracticalGuideStl.aspx
4 - OpenGL page: http://www.opengl.org/documentation/red_book/
5 - Wikipedia.org: http://en.wikipedia.org/wiki/KUKA
6 Wikipedia - OpenGL: http://en.wikipedia.org/wiki/OpenGL
7 C++ tutorial http://cplusplus.com/doc/tutorial/
8 Micosoft Visual Studio 2008 Documentation http://msdn.microsoft.com/en-
us/library/ms177549(VS.90).aspx

MC LC
LI NI U .............................................................................................. 1
CHNG 1 .................................................................................................. 2
TM HIU TNG QUAN V ROBOT CHUI H .................................. 2
1.1. S ra i ca robot ................................................................................. 2
1.2. Phn loi robot ....................................................................................... 2
1.2.1. Phn loi theo s bc t do (BTD) trong trng cng tc ........... 2
1.2.2. Phn loi theo cu trc ng hc ................................................. 2
1.3. Tay my (Manipulator) .......................................................................... 5
1.4. Mt s robot chui h thng gp ......................................................... 5
1.4.1. Tay my ta vung gc (Cartesian robot/ Gantry robot) ........ 5
1.4.2. Tay my ta tr (Cylindrical robot) ........................................ 6
1.4.3. Tay my ta cu (Spherical/Polar robot) ................................ 7
1.4.4. Tay my ton khp bn l ............................................................ 7
1.4.5. V KUKA Robotics ..................................................................... 8
1.5. ng dng ca robot .............................................................................. 11
CHNG 2 ................................................................................................ 12
TNH TON NG HC V NG LC HC ROBOT KUKA 6 BC T
DO ............................................................................................................... 12
2.1. Tnh ton ng hc robot KUKA 6 bc t do ...................................... 12
2.1.1. Phng trnh ng hc thun ..................................................... 13
2.1.2. Phng trnh ng hc ngc .................................................... 18
CHNG 3 ................................................................................................ 23
CHNG TRNH M PHNG KT QU TNH TON ...................... 23
3.1. M phng s dng th vin OpenGL vi MFC ................................... 23
3.1.1. Gii thiu v OpenGL ............................................................... 23
3.1.2. Gii thiu ngn ng lp trnh Visual C++ ................................. 24
3.1.3. Gii thiu phn mm ha SolidWorks ................................. 25
3.1.4. Kt qu lp trnh m phng ....................................................... 26
3.2. MATLAB ............................................................................................. 29
TI LIU THAM KHO .......................................................................... 32

RBCN

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

RBCN

Uploaded by

Copyright:

Available Formats

Bi tp ln Robot Cng nghip KuKa Robot (6R)

V Ngc Trng CDT2_K52 1

You might also like