Robot Cng nghip

1


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
2
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
Php
1.488 1.197 2.000
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
3
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 :
w = 6n - (1.1) ip
i
i =

1
5
y : n - S khu ng;
p
i
- 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
4
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.

d
2

5
n
a
o
z
y
O
0
O
n
x










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).
x
y
O
z
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 O
0
; h to gn trn cc khu trung gian
tng ng s l O
1
, O
2
,..., O
n-1
, H to gn trn khu
chp hnh cui k hiu l O
n
.
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 360
0
. Ngi ta thng dng
hai hnh chiu m t trng cng tc ca mt robot (hnh 1.3).

R

H












Hnh chiu ng Hnh chiu bng
Hnh 1.3 : Biu din trng cng tc ca robot.
TS. Phm ng Phc
Robot Cng nghip
5
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
Cnh tay
robot
Dng c
thao tc
B iu
khin v
my tnh
Ngun
ng lc
Thit b
dy hc
Cc chng
trnh















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
6
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
Hnh 1.6 : Robot kiu to tr

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).



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
7
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
8
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
9

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, x
1
. . . m t v
tr ca im U, V, X
1
,. . .
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
E
V
F
v
F
E







Hnh 2.2 : Biu din 1 im trong khng gian

v
E
v v
F
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 :

x x/w = a
v = y Trong y/w = b
z z/w = c
w

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
w
x
x a = = =
1
;
y
w
y
y b = = =
1
;
z
w
z
z a = = =
1


TS. Phm ng Phc
Robot cng nghip
10
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 ta c :
x
w
y
w
z
w
= = =
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 :
V d :
r
r r
r
v i j k = + + 3 4 5
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

K hiu [ . . . . ]
T
(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 :
Cho hai vect :
r
r r
r
a a i a j a k
x y z
= + +

r
r r
r
b b i b j b k
x y z
= + +
Ta c tch v hng a.b = a
x
b
x
+ a
y
b
y
+ a
z
b
z
V tch vect :
a
r
x =
r
b
z y x
z y x
b b b
a a a
k j i
r r r
= (a
y
b
z
-a
z
b
y
)
r
i + (a
z
b
x
-a
x
b
z
)
r
j + (a
x
b
y
-a
y
b
x
)
r
k

2.2.2. Cc php tnh v ma trn :
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 c
ij

bng tng (hiu) ca cc phn t a
ij
v b
ij
(vi mi i, j).
A + B = C Vi c
ij
= a
ij
+ b
ij
.
A - B = C Vi c
ij
= a
ij
- b
ij
.
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 :

1 2 3 1 2
A = 4 5 6 v B = 3 4
7 8 9 5 6
Ta c :
TS. Phm ng Phc
Robot cng nghip
11
1.1+2.3+3.5 1.2+2.4+3.6 22 28
C = A.B = 4.1+5.3+6.5 4.2+5.4+6.6 = 49 64
7.1+8.3+9.5 7.2+8.4+9.6 76 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 :

n
x
O
x
a
x
p
x
T = n
y
O
y
a
y
p
y
n
z
O
z
a
z
p
z
0 0 0 1

Ma trn nghch o ca T k hiu l T
-1
:

n
x
n
y
n
z
-p.n
T
-1
= O
x
O
y
O
z
-p.O (2-1)
a
x
a
y
a
z
-p.a
0 0 0 1

Trong p.n l tch v hng ca vect p v n. ngha l :
p.n = p
x
n
x
+ p
y
n
y
+ p
z
n
z
tng t : p.O = p
x
O
x
+ p
y
O
y
+ p
z
O
z
v p.a = p
x
a
x
+ p
y
a
y
+ p
z
a
z
V d : tm ma trn nghch o ca ma trn bin i thun nht :

0 0 1 1
H = 0 1 0 2
-1 0 0 3
0 0 0 1

Gii : p dng cng thc (2-1), ta c :

0 0 -1 3
H
-1
= 0 1 0 -2
1 0 0 -1
0 0 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 1 0 0 -1 3 1 0 0 0
0 1 0 2 0 1 0 -2 = 0 1 0 0
-1 0 0 3 1 0 0 -1 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1
TS. Phm ng Phc
Robot cng nghip
12
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 :
Trace(A) hay Tr(A) =

=
n
i
ii
a
1
Mt s tnh cht quan trng ca vt ma trn :
1/ Tr(A) = Tr(A
T
)
2/ Tr(A+B) = Tr(A) + Tr(B)
3/ Tr(A.B) = Tr(B.A)
4/ Tr(ABC
T
) = Tr(CB
T
A
T
)

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

=
44 43 42 41
34 33 32 31
24 23 22 21
14 13 12 11
a a a a
a a a a
a a a a
a a a a
A

th : dt
t
a
t
a
t
a
t
a
t
a
t
a
t
a
t
a
t
a
t
a
t
a
t
a
t
a
t
a
t
a
t
a
dA
44 43 42 41
34 33 32 31
24 23 22 21
14 13 12 11

=
Tng t, php tch phn ca ma trn A l mt ma trn, c :

} ) ( { ) ( dt t a dt t A
ij
=
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) :
Gi s cn tnh tin mt im hoc mt vt th theo vect dn
r
r r
r
h ai bj ck = + + . Trc
ht ta c nh ngha ca ma trn chuyn i H :

1 0 0 a
H = Trans(a,b,c) = 0 1 0 b (2.2)
0 0 1 c
0 0 0 1

TS. Phm ng Phc
Robot cng nghip
13
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 :

1 0 0 a x x+aw x/w+a
v = H.u = 0 1 0 b . y = y+bw = y/w+b
0 0 1 c z z+cw z/w+c
0 0 0 1 w w 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.
V d :
r
r r r
r r r
u = 2i + 3j + 2k
h = 4i - 3j + 7k
r
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
















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 :

1 0 0 0
Rot(x,
o
) =
0
cos -sin
0 (2.3)
0
sin cos
0
0 0 0 1


cos
0
sin
0
Rot(y,
o
) =
0 1 0 0 (2.4)

-sin
0
cos
0
0 0 0 1

z
y
x
h
u
v
4
6
2
3
-3
2
0
7
9
TS. Phm ng Phc
Robot cng nghip
14

cos -sin
0 0
Rot(z,
o
) = sin cos
0 0 (2.5)
0 0 1 0
0 0 0 1
V d : Cho im U biu din bi
r
r r r
u = 7i + 3j + 2k quay xung quanh z mt gc = 90
o

(hnh 2.5). Ta c
0 -1 0 0 7 -3
v= Rot(z, 90
o
)u = 1 0 0 0 3 = 7
0 0 1 0 2 2
0 0 0 1 1 1

Nu cho im bin i tip tc quay xung quanh y mt gc 90
o
ta c :

0 0 1 0 -3 2
w = Rot(y, 90
o
)v = 0 1 0 0 7 = 7
-1 0 0 0 2 3
0 0 0 1 1 1
V c th biu din :
2
w = Rot(y, 90
o
). Rot(z, 90
o
) . u = 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 90
0
, ta c :

0 0 1 0 7 2
v = 0 1 0 0 3 = 3 = Rot(y, 90
o
).u
-1 0 0 0 2 -7
0 0 0 1 1 1

Sau cho im va bin i quay quanh z mt gc 90
0
, ta c :

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

R rng : Rot(y, 90
o
).Rot(z,90
0
)u Rot(z,90
0
).Rot(y, 90
o
)u

y
w
z
u
x
v
x
y
u
v
w
z









Hnh 2.5 Hnh 2.6
w = Rot(y, 90
o
). Rot(z, 90
o
)u w= Rot(z, 90
o
). Rot(y, 90
o
)u

TS. Phm ng Phc
Robot cng nghip
15
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 :


C
x
C
y
C
z
C
o
n
x
O
x
a
z
0
C = n
y
O
y
a
y
0
n
z
O
z
a
z
0
0 0 0 1

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.

Chng hn :
r r r r
k = a i + a j + a k
x y z


Lc , php quay Rot(k,) s tr thnh php quay Rot(C
z
,).
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 :

Rot(k,) = Rot(C
z
,)
hay l Rot(k,).T = C.Rot(z,).X = C.Rot(z,).C
-1
.T
Vy 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 :

n
x
n
y
n
z
0
C
-1
= O
x
O
y
O
z
0
a
x
a
y
a
z
0
0 0 0 1

a (C
x
)
O(C
y
)
C
o
n (C
z
)
Hnh 2.7 : H to gn trn
khu chp hnh cui (bn tay)
TS. Phm ng Phc
Robot cng nghip
16

Thay cc ma trn vo v phi ca phng trnh (2.6) :

n
x
O
x
a
x
0
cos -sin
0 0 n
x
n
y
n
z
0
Rot(k,) =
n
y
O
y
a
y
0
sin cos
0 0 O
x
O
y
O
z
0
n
z
O
z
a
z
0 0 0 1 0 a
x
a
y
a
z
0
0 0 0 1 0 0 0 1 0 0 0 1

Nhn 3 ma trn ny vi nhau ta c :


n
x
n
x
cos - n
x
O
x
sin + n
x
O
x
sin + O
x
O
x
cos + a
x
a
x
Rot(k,) = n
x
n
y
cos - n
y
O
x
sin + n
x
O
y
sin + O
x
O
y
cos + a
y
a
x

n
x
n
z
cos - n
z
O
x
sin + n
x
O
z
sin + O
x
O
z
cos + a
z
a
x
0

n
x
n
y
cos - n
x
O
y
sin + n
y
O
x
sin + O
x
O
y
cos + a
x
a
y
n
y
n
y
cos - n
y
O
y
sin + n
y
O
y
sin + O
y
O
y
cos + a
y
a
y
n
z
n
y
cos - n
z
O
y
sin + n
y
O
z
sin + O
z
O
y
cos + a
z
a
y
0

n
x
n
z
cos - n
x
O
z
sin + n
z
O
x
sin + O
x
O
z
cos + a
x
a
z
0
n
y
n
z
cos - n
y
O
z
sin + n
z
O
y
sin + O
y
O
z
cos + a
y
a
z
0
n
z
n
z
cos - n
z
O
z
sin + n
z
O
z
sin + O
z
O
z
cos + a
z
a
z
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.
- Vect n v z bng tch vect ca x v y, hay l :
r r
r
a = n x O

Trong : a
x
= n
y
O
z
- n
z
O
y
a
y
= n
x
O
z
- n
z
O
x
a
x
= n
x
O
y
- n
y
O
x

Khi cho k trng vi mt trong s cc vect n v ca C ta chn :
k
z
= a
x
; k
y
= a
y
; k
z
= a
z

Ta k hiu Vers = 1 - cos (Versin ).
Biu thc (2.6) c rt gn thnh :

k
x
k
x
vers+cos k
y
k
x
vers-k
z
sin k
z
k
x
vers+k
y
sin 0
Rot(k,) =
k
x
k
y
vers+k
z
sin k
y
k
y
vers+cos k
z
k
y
vers-k
x
sin 0 (2.8)
k
x
k
z
vers+k
y
sin k
y
k
z
vers+k
z
sin k
z
k
z
vers+cos 0
0 0 0 1

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
Robot cng nghip
17
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 :
n
x
O
x
a
x
0
R = n
y
O
y
a
y
0
n
z
O
z
a
z
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 :

n
x
O
x
a
x
0 k
x
k
x
vers+cos k
y
k
x
vers-k
z
sin k
z
k
x
vers+k
y
sin 0
n
y
O
y
a
y
0 = k
x
k
y
vers+k
z
sin k
y
k
y
vers+cos k
z
k
y
vers-k
x
sin 0
n
z
O
z
a
z
0 k
x
k
z
vers+k
y
sin k
y
k
z
vers+k
z
sin k
z
k
z
vers+cos 0
0 0 0 1 0 0 0 1
(2.9)
Bc 1 : Xc nh gc quay .
* Cng ng cho ca hai ma trn hai v ta c :
n
x
+ O
y
+ a
z
+ 1 = vers + cos + vers + cos + vers + cos + 1 k
x
2
k
y
2
k
z
2
= (1 - coss)( + + ) + 3cos + 1 k
x
2
k
y
2
k
z
2
= 1 - cos + 3cos +1
= 2(1+ cos)
cos = (n
x
+ O
y
+ a
z
- 1)/2
* Tnh hiu cc phn t tng ng ca hai ma trn, chng hn :
O
z
- a
y
= 2k
x
sin
a
x
- n
z
= 2k
y
sin (2.10)
n
y
- O
x
= 2k
z
sin

Bnh phng hai v ca cc phng trnh trn ri cng li ta c :

(O
z
- a
y
)
2
+ (a
x
- n
z
)
2
+ (n
y
- O
x
)
2
= 4 sin
2


sin =
1
2
(O - a ) + (a - n ) + (n - O )
z y
2
x z
2
y x
2

Vi 0 180
0
:
tg =
(O - a ) + (a - n ) + (n - O )
(n + O + a - 1)
z y
2
x z
2
y x
2
x y z

V trc k c nh ngha bi :
k =
O a
2sin
z y
x

; k =
a n
2sin
x z
y

; k =
n O
2sin
y z
x

(2.11)
rng vi cc cng thc (2.8) :
- Nu = 0
0
th k
x
, k
y
, k
z
c dng
0
0
. Lc ny phi chun ho k sao cho k = 1
TS. Phm ng Phc
Robot cng nghip
18
- Nu = 180
0
th k
x
, k
y
, k
z
c dng
a 0
0
. Lc ny k khng xc nh c, ta phi
dng cch tnh khc cho trng hp ny :

Xt cc phn t tng ng ca hai ma trn (2.9) :
n
x
= k vers+cos
x
2
O
y
= k
y
2
vers+cos
a
z
= k
z
2
vers+cos
T y ta suy ra :
k
n
vers

n
1- cos
x
x x
=

=
cos cos

k
O
vers

O
1- cos
y
y y
=

=
cos cos

k
a
vers

a
1- cos
z
z z
=

=
cos cos

Trong khong 90
0
180
0
sin lun lun dng

Da vo h phng trnh (2.10) ta thy k
x
, k
y
, k
z
lun c cng du vi v tri. Ta dng
hm Sgn(x) biu din quan h cng du vi x, nh vy :

k Sgn(O
n
1- cos
x z
x
=

a
y
)
cos

k Sgn(a - n )
O
1- cos
y x z
y
=
cos

(2.12)
k Sgn(n O
a
1- cos
z y x
z
=

)
cos

H phng trnh (2.12) ch dng xc nh xem trong cc k
x
, k
y
, k
z
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) :
n
y
+ O
x
= 2k
x
k
y
vers = 2k
x
k
y
(1 - cos)
O
z
+ a
y
= 2k
y
k
z
vers = 2k
y
k
z
(1 - cos) (2.13)
a
x
+ n
z
= 2k
z
k
x
vers = 2k
z
k
x
(1 - cos)
Gi s theo h (2.12) ta c k
x
l ln nht, lc k
y
, k
z
s tnh theo k
x
bng h (2.13); c
th l : k
n O
k
y
y
x
=
+

x
2 1 ( cos )

k
a n
k
z
x
x
=
+

z
2 1 ( cos )

V d : Cho R = Rot[y,90
0
]Rot[z,90
0
]. Hy xc nh k v R = Rot[k,]. Ta bit :
0 0 1 0
R = Rot(y,90
0
).Rot(z,90
0
) = 1 0 0 0
0 1 0 0
0 0 0 1
Ta c cos = (n
x
+ O
y
+ a
z
- 1) / 2 = (0 + 0 + 0 - 1) / 2 = -1 / 2
TS. Phm ng Phc
Robot cng nghip
19

sin =
1
2
(O - a ) + (a - n ) + (n - O )
z y
2
x z
2
y x
2

=
1
2
(1 - 0) + (1 - 0) + (1 - 0) =
3
2
2 2 2

tg = 3 v = 120
0

Theo (2.12), ta c :
k
x
= k
y
= k
z
= +
+
+
=
0 1 2
1 1 2
1
3
/
/


Vy : R = Rot(y,90
0
).Rot(z,90
0
) = Rot(k, 120
0
); vi :
r r r r
k
1
3
i
1
3
j
1
3
k = + +












Hnh 2.8 : Tm gc quay v trc quay tng ng
1/ 3
1/ 3
1/ 3
k
O
120
0
y
z
x

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).













Hnh 2.9 : Php quay Euler
x
y
z z
zz
yy
y
x x x

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
Robot cng nghip
20
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
Coscos -Cos sin sin
0
=
sin cos
0 0
sin cos
0 0
0 0 1 0
-sin cos sin sin Cos
0
0 0 0 1 0 0 0 1


cosCoscos - sinsin -cosCossin - sincos cossin
0
=
sinCoscos + cossin -sinCossin + coscos sinsin
0

-sin cos sin sin cos
0
0 0 0 1
(2.15)

Cos
0
sin
0
cos -sin
0 0
Euler (,,) = Rot(z, )
0 1 0 0
sin cos
0 0

-sin
0
Cos
0 0 0 1 0
0 0 0 1 0 0 0 1

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)

z
y
x
Thn tu
Yaw

Roll

Pitch

Cc php quay p dng cho khu chp
hnh cui ca robot nh hnh 2.11. Ta xc
nh th t quay v biu din php quay nh
sau :
Hnh 2.10: Php quay Roll-Pitch-Yaw
RPY(,,)=Rot(z,)Rot(y,)Rot(x, ) (2.16)











Yaw,
y
z
Pitch,
Roll,
x
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
Robot cng nghip
21

Thc hin php nhn cc ma trn quay, cc chuyn v Roll, Pitch v Yaw c biu th
nh sau :


cos
0
sin
0 1 0 0 0
0 1 0 0 0
cos -sin
0
RPY(,,)=Rot(z,)
-sin
0
cos
0 0
sin cos
0
0 0 0 1 0 0 0 1


cos -sin
0 0
cos sinsin sincos
0
=
sin cos
0 0 0
cos -sin
0
0 0 1 0
-sin cossin cos cos
0
0 0 0 1 0 0 0 1


coscos cossinsin - sincos cossincos + sinsin
0
=
sincos sinsinsin +coscos sinsincos - cossin
0

-sin cos sin cos cos
0
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 tnh tin gc to ct O(0, 0, 0) theo mt vect dn
r r r r
h = 4i - 3j + 7k (hnh 2.12) . Kt qu ca php bin i l :

1 0 0 4 0 4
O
T
= 0 1 0 -3 0 = -3
0 0 1 7 0 7
0 0 0 1 1 1

Ngha l gc ban u c to O(0, 0, 0) chuyn i n gc mi O
T

c to
(4, -3, 7) so vi h to c.

y
T
x
T
O
T
z
T
z
y
x
O
7
-3
4












Hnh 2.12 : Php bin i tnh tin h to

Tuy nhin trong php bin i ny cc trc to ca O
T
vn song song v ng hng
vi cc trc to ca O.

TS. Phm ng Phc
Robot cng nghip
22
Nu ta tip tc thc hin cc php bin i quay :

Rot(y,90
o
)Rot(z,90
o
).O
T

ta s c mt h to hon ton mi, c th ti gc to mi (4,-3,7) khi cho h O
T
quay
quanh z mt gc 90
0
(chiu quay dng qui c l ngc chiu kim ng h), ta c :




Rot(z,90
0
)


Ta tip tc quay h O
T
quanh truc y (trc y ca h to gc ) mt gc 90
0
, Ta c :




Rot(y,90
0
)
y'
T
O
T
x'
T
z'
T
z"
T
O
T
y''
T
x''
T
y
T
x
T
O
T
90
o
z
T
y'
T
O
T
x'
T
z'
T
90
o
y


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,90
o
)Rot(z,90
o
).O
T

y
T
x
T
O
T
90
o
z
T


y'
T
O'
T
z'
T
Rot(y,90
o
)


x'
T


Ta tip tc quay h O'
T
quanh truc z (By gi l trc z'
T
ca h to mi) mt gc 90
0
:

z"
T
O''
T
y''
T
x''
T


y'
T
x'
T
z'
T
O'
T
90
o
Rot(z',90
o
)



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 v
h C c quan h vi h B qua php bin i . Ta c im P trong h C k hiu P
A
B
T/
B
c
T/
C
, ta tm
mi quan h ca im P trong h A, tc l tm P
A
(Hnh 2.13) :


TS. Phm ng Phc
Robot cng nghip
23










Hnh 2.13 : Quan h gia cc h to bin i.
p
A
p
C
z
C
x
C
y
C
x
B
z
B
y
B
x
A
z
A
y
A
C
B
A

Chng ta c th bin i p
C
thnh p
B
nh sau :
p
B
= p
B
c
T/
C
, (2.18)
Sau bin i p
B
thnh p
A
nh sau :
p
A
= p
A
B
T/
B
, (2.19)

Kt hp (2.18) v (2.19) ta c :
(2.20)
c C
B
B
A
A
p T T p =

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.











O
0
O
1
O
2
O
3
T
4
O
4
Bn tay
y
z
x
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
Robot cng nghip
24

Ta thc hin cc php bin i sau :

H = Trans(4,0,0)Rot(y,90
0
)Rot(z,90
0
)

Vi v tr ca vt th, ta c ma trn to ca 6
im c trng m t n l :

C C C C C
1 -1 -1 1 1 -1
0 0 0 0 4 4
0 0 2 2 0 0
1 1 1 1 1 1


Sau khi thc hin cc php bin i :

- Quay vt th quanh trc z mt gc 90
0
(Hnh 2.16),
- Cho vt th quay quanh trc y mt gc 90
0
(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) :


C C C C C
0 0 1 4 1 -1 -1 1 1 -1
H = 1 0 0 0 0 0 0 0 4 4
0 1 0 0 0 0 2 2 0 0
0 0 0 1 1 1 1 1 1 1


C C C C C
4 4 6 6 4 4
= 1 -1 -1 1 1 1
0 0 0 0 4 4
1 1 1 1 1 1

-1,4,0,1
-1,0,2,1
-1,0,0,1
1,4,0,1
1,0,2,1
1,0,0,1
y
x
Hnh 2.15 : M t vt th
x
y
z
C
O

C
C
C

C
O
x
y
z
C C
C C
C
z













Hnh 2.17: Rot (y,90
0
) Rot (z,90
0
)
Hnh 2.16 : Rot (z,90
0
)



TS. Phm ng Phc
Robot cng nghip
25

H = Trans(4,0,0)Rot (y,90
0
)Rot (z,90
0
)
O
C
C C
C

y
z
x
C













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 90
0
. 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,-90
0
)Rot(z,90
0
)

Bi 3 : Cho ma trn bin i thun nht A, tm ma trn nghch o A
-1
v kim chng.

0 1 0 -1
A = 0 0 -1 2
-1 0 0 0
0 0 0 1
TS. Phm ng Phc
Robot cng nghip
26


Bi 4 : Hnh v 2-19 m t h to {B} c
quay i mt gc 30
0
xung quanh trc z
A
, tnh tin
dc theo trc x
A
4 n v v tnh tin dc theo y
A

3 n v.
x
B
y
B
{B}
x
A
y
A
{A}
(a) M t mi qua h ca {B} i vi {A} :
A
T
B
?
(b) Tm mi quan h ngc li
B
T
A
?


Hnh 2.19 : Quan h {A} v {B}

Bi 5 : Cho k =
1
3
(1, 1, 1)
T
, = 90
0
. Tm ma trn R = Rot(k, ).
Bi 6 : Xc nh cc gc quay Euler, v cc gc quay RPY khi bit ma trn T
6
:

1 0 0 0
T
6
= 0 0 1 5
0 -1 0 3
0 0 0 1

Bi 7 : Mt vt th t trong mt h to tham chiu c xc nh bi php bin i :

0 1 0 -1
U
T
P
= 0 0 -1 2
-1 0 0 0
0 0 0 1

Mt robot m h to chun c lin h vi h to tham chiu bi php bin i

1 0 0 1
U
T
R
= 0 1 0 5
0 0 1 9
0 0 0 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
R
T
H
(biu din mi quan h gia bn tay v
h to gc ca robot) thc hin iu ni trn.

TS. Phm ng Phc
Robot cng nghip
27

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. A
1
m t v tr v hng ca khu u tin; A
2

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 :

T
2
= A
1
.A
2


Cng nh vy, A
3
m t khu th ba so vi khu th hai v :

T
3
= A
1
.A
2
.A
3
; 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 :

T
6
= A
1
.A
2
.A
3
.A
4
.A
5
.A
6
(3.1)

T
6
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. T
6
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
p
a
o











Hnh 3.1 : Cc vect nh v tr v nh hng ca bn tay my
TS. Phm ng Phc
Robot cng nghip
28
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 :

a x o = n
r r r


Chuyn v T
6
nh vy s bao gm cc phn t :

n
x
O
x
a
x
p
x

T
6
= n
y
O
y
a
y
p
y
(3.2)
n
z
O
z
a
z
p
z

0 0 0 1

Tng qut, ma trn T
6
c th biu din gn hn nh sau :

Ma trn nh hng R Vect v tr p (3.3)
T
6
=
0 0 0 1

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
r
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 : a
n
.
Gc gia cc trc trong mt phng vung gc vi a
n
:
n
.












a
Khp n Khp n+1

n
Khu n
Hnh 3.5 : Chiu di v gc xon ca 1 khu.
Thng thng, ngi ta gi a
n
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
Robot cng nghip
29

n+1
Khu n+1
Khp n-1
Khp n+1 Khp n
x
n
a
n
z
n
O
n
Khu n
Khu n-1
d
n
z
n-1
x
n-1

n-1








Khu n-2








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 d
n
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.
d
n
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 a
n
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.
Trong trng hp cc trc khp ct nhau th trc x chn theo tch vect .
1 - n n
z x z
r r
Trng hp khp quay th
n
l cc bin khp, trong trng hp khp tnh tin th d
n

l bin khp v a
n
bng 0.
Cc thng s a
n
,
n
, d
n
v
n
c gi l b thng s DH.

V d 1 : Xt mt tay my c hai khu phng nh hnh 3.7 :

2
a
1
a
2
O
0
z
1
z
2
x
1
y
1
y
2
O
1
O
2
z
0
x
0
y
0
x
2










Hnh 3.7 : Tay my c hai khu phng (v tr bt k).
TS. Phm ng Phc
Robot cng nghip
30
Ta gn cc h to ln cc khu nh hnh v : trc z
0
, z
1
v z
2
vung gc vi t giy.
H to c s l O
0
x
0
y
0
z
0
, chiu ca x
0
hng t O
0
n O
1
. Sau khi thit lp h to c s,
H to o
1
x
1
y
1
z
1
c hng nh hnh v, O
1
t ti tm trc khp 2. H to O
2
x
2
y
2
x
2
c gc
O
2
t im cui ca khu 2.

Bng thng s Denavit-Hartenbert ca tay my ny nh sau :

Khu

i

i
a
i
d
i
1

1
*
0 a
1
0
2

2
*
0 a
2
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.












Hnh 3.8 : Robot SCARA v cc h to (v tr ban u).
O
0

1
x
0
x
1
d
3
x
2
x
3
x
z
3
, z
4

4
O
3
O
4
z
0 z
1
z
2
a
1
a
2
O
1
O
2
d
4
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 x
0
nm trong mt phng t giy. Cc h to khc nh hnh
v. Bng thng s DH ca robot SCARA nh sau :

Khu

i

i
a
i
d
i
1

1
*
0 a
1
0
2

2
*
180
0
a
2
0
3 0 0 0 d
3
*
4

4
*
0 0 d
4

* : 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 z
n-1
mt gc
n

Tnh tin dc theo z
n-1
mt khong d
n

Tnh tin dc theo x
n-1
= x
n
mt on a
n

Quay quanh x
n
mt gc xon
n

TS. Phm ng Phc
Robot cng nghip
31

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 :

A
n
= Rot(z,) Trans(0,0,d) Trans(a,0,0) Rot(x,) (3.4)


cos -sin
0 0 1 0 0 a 1 0 0 0
A
n
=
sin cos
0 0 0 1 0 0 0
cos -sin
0
0 0 1 0 0 0 1 d 0
sin cos
0
0 0 0 1 0 0 0 1 0 0 0 1


cos -sin cos sin sin a cos

A
n
=
sin cos cos -cos sin a sin
(3.5)
0
sin cos
d
0 0 0 1

i vi khp tnh tin (a = 0 v
i
= 0) th ma trn A c dng :

1 0 0 0
A
n
= 0
cos - sin
0 (3.6)
0
sin cos
d
0 0 0 1

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 T
6
theo cc ma trn A
n
:

Ta bit : T
6
= A
1
A
2
A
3
A
4
A
5
A
6

Trong T
6
c miu t trong h to gc (h to gn vi khu c bn c nh
ca robot). Nu m t T
6
theo cc h to trung gian th n-1 th :
=
6
1 n
T

A
i
i n =

6
X
Z
T
6
E
A
O
R
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 :

Hnh 3.9 : Vt th v Robot

X= Z T
6
E
TS. Phm ng Phc
Robot cng nghip
32

Quan h ny c th hin trn ton sau :


A
1
A
2
A
3
A
4
A
5

6
5
T
6
4
T
6
3
T
6
2
T
6
1
T
6 T
O
R
O
R
Z E X A
O
0













Hnh 3.10 : Ton chuyn v ca robot.

T ton ny ta c th rt ra : T
6
= Z
-1
X E
-1
(Z
-1
v E
-1
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 O
0
, O
1
, ...
+ Cc trc z
n
phi chn cng phng vi trc khp th n+1.
+ Chn trc x
n
l trc quay ca z
n
thnh z
n+1
v gc ca z
n
vi z
n+1
chnh l
n+1
. Nu z
n

v z
n+1
song song hoc trng nhau th ta c th cn c nguyn tc chung hay chn x
n
theo x
n+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 A
n
.
l bn php bin i : A
n
= 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 A
n
, 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 z
n

i vi z
n-1
quanh trc y
n-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 A
n
.
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
Robot cng nghip
33
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.











1. Gn h to ln cc khu :
Ta gi nh v tr ban u v chn gc to O
0
ca robot nh hnh 3.12. Cc trc z t
cng phng vi cc trc khp.

Ta thy trc z
1
quay tng i mt
gc 90
0
so vi trc z
0
, y chnh l php quay
quanh trc x
0
mt gc
1
(php bin i
Rot(x
0
,
1
) trong biu thc tnh A
n
). Ngha l
trc x
0
vung gc vi z
0
v z
1
. Ta chn chiu
ca x
0
t tri sang phi th gc quay
1
=90
0

(chiu dng ngc chiu kim ng h).
ng thi ta cng thy gc O
1
tnh tin
mt on dc theo z
0
, so vi O
0
, chnh l
php bin i Trans(0,0,d
1
) (tnh tin dc theo
z
0
mt on d
1
) ; cc trc y
0
,v y
1
xc nh
theo qui tc bn tay phi (Hnh 3.12 ) .

Tip tc chn gc ta O
2
t trng
vi O
1
v trc khp th ba v trc khp th
hai ct nhau ti O
1
(nh hnh 3.12). Trc z
2

cng phng vi trc khp th ba, tc l
quay i mt gc 90
0
so vi z
1
quanh trc y
1
;
php bin i ny khng c trong biu thc
tnh A
n
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, O
2
vn c t trng
vi O
1
, trc z
2
c phng thng ng, ngha l
ta quay trc z
1
thnh z
2
quanh trc x
1
mt
gc -90
0
(tc
2
= -90
0
).
u cui ca khu th 3 khng c
khp, ta t O
3
ti im gia ca cc ngn
tay, v trc z
3
, x
3
chn nh hnh v, nh vy
ta tnh tin gc to dc theo z
2
mt
on d
3
(Php bin i Trans(0,0,d
3
)), v y
l khu tnh tin nn d
3
l bin .


Hnh 3.12 : Gn cc h to O
0
v O
1
y
1
x
1
y
0
z
1
z
2
O
1
, O
2
O
0
z
0

2 d
3
x
0
d
1

2 d
3
Hnh 3.11 : Robot RRT
x
2
O
3
O
2
z
2
z
3
z
0
O
0
x
0
O
1
y
1
d
1
x
1
y
0
z
1

2
d
3
x
3
d
3
Hnh 3.13 : H to
gn ln cc khu
TS. Phm ng Phc
Robot cng nghip
34

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 :

Khu

i

i
a
i
d
i
1

1
*
90 0 d
1
2

i
*
-90 0 0
3 0 0 0 d
3
*

3. Xc nh cc ma trn A :
Ma trn A
n
c dng :

cos -sin cos sin sin
0
A
n
=
sin cos cos -cos sin
0
0
sin cos
d
0 0 0 1
Vi qui c vit tt : C
1
= cos
1
; S
1
= sin
1
; C
2
= cos
2
. . .

C
1
0 S
1
0
A
1
= S
1
0 -C
1
0
0 1 0 d
1
0 0 0 1

C
2
0 -S
2
0
A
2
= S
2
0 C
2
0
0 -1 0 0
0 0 0 1

1 0 0 0
A
3
= 0 1 0 0
0 0 1 d
3
0 0 0 1

4. Tnh cc ma trn bin i thun nht T :
+ Ma trn
2
T
3
= A
3
+ Ma trn
1
T
3
= A
2
.
2
T
3

C
2
0 -S
2
0 1 0 0 0 C
2
0 -S
2
-S
2
*d
3
1
T
3
= S
2
0 C
2
0 0 1 0 0 = S
2
0 C
2
C
2
*d
3
0 -1 0 d
2
0 0 1 d
3
0 -1 0 0
0 0 0 1 0 0 0 1 0 0 0 1
+ Ma trn T
3
= A
1
.
1
T
3

C
1
0 S
1
0 C
2
0 -S
2
-S
2
*d
3
T
3
= S
1
0 -C
1
0 S
2
0 C
2
C
2
*d
3
0 1 0 d
1
0 -1 0 0
0 0 0 1 0 0 0 1


TS. Phm ng Phc
Robot cng nghip
35
C
1
C
2
-S
1
-C
1
S
2
-C
1
S
2
d
3
= S
1
d
2
C
1
-S
1
S
2
-S
1
S
2
d
3

S
2
0 C
2
C
2
d
3
+ d
1
0 0 0 1

Ta c h phng trnh ng hc ca robot nh sau :

n
x
= C
1
C
2
;
O
x
= -S
1
;
a
x
= -C
1
S
2
;
p
x
= -C
1
S
2
d
3
n
y
= S
1
C
2
;
O
y
= C
1
;
a
y
= -S
1
S
2
;
p
y
= -S
1
S
2
d
3
n
z
= S
2
O
z
= 0;
a
z
= C
2
;
p
z
= C
2
d
3
+ d
1
;


(Ta c th s b kim tra kt qu tnh ton bng cch da vo to v tr p
x
,p
y
, p
z

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
Robot cng nghip
36
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 :

C
1
= cos
1
;
S
1
= sin
1
;
C
12
= cos(
1
+
2
);
S
12
= sin(
1
+
2
)
S
234
= sin (
2
+
3
+
4
) ... .

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 O
6
O
5
).

Bng thng s DH (Denavit-Hartenberg) ca robot Stanford nh sau :

Khu

i

i
a
i
d
i
1

1
*
-90
0
0 0
2

2
*
90
0
0 d
2
3 0 0 0 d
3
*
4

4
*
-90
0
0 0
5

5
*
90
0
0 0
6

6
*
0 0 0
(* : Cc bin khp).

Cc ma trm A ca robot Stanford c xc nh nh sau :

C
1
0 -S
1
0 C
2
0 S
2
0
A
1
= S
1
0 C
1
0 A
2
= S
2
0 -C
2
0
0 -1 0 0 0 1 0 d
2
0 0 0 1 0 0 0 1

1 0 0 0 C
4
0 -S
4
0
A
3
= 0 1 0 0 A
4
= S
4
0 C
4
0
0 0 1 d
3
0 -1 0 0
0 0 0 1 0 0 0 1

C
5
0 S
5
0 C
6
-S
6
0 0
A
5
= S
5
0 -C
5
0 A
6
= S
6
C
6
0 0
0 1 0 0 0 0 1 0
0 0 0 1 0 0 0 1

d
2 d
3
z
4
z
3
,z
5
,z
6
z
2
O
0
,O
1
x
i
x
0
z
0
z
1
Hnh 3.15 : H to ca Robot Stanford
O
3
,O
4,
O
5
,O
6
x
1
O
2


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
Robot cng nghip
37
C
6
-S
6
0 0
T
6
5
= S
6
C
6
0 0
0 0 1 0
0 0 0 1

C
5
C
6
-C
5
S
6
S
5
0
T
6
4
= A
5
A
6
= S
5
C
6
-S
5
S
6
-C
5
0
S
6
C
6
0 0
0 0 0 1

C
4
C
5
C
6
- S
4
S
6
-C
4
C
5
S
6
-S
4
C
6
C
4
S
5
0
T
6
3
= A
4
A
5
A
6
= S
4
C
5
C
6
+ C
4
S
6
-S
4
C
5
S
6
+ C
4
C
6
S
4
S
5
0
-S
5
C
6
S
5
S
6
C
5
0
0 0 0 1

C
4
C
5
C
6
-S
4
S
6
-C
4
C
5
S
6
- S
4
C
6
C
4
S
5
0
T
6
2
= A
3
A
4
A
5
A
6
= S
4
C
5
C

+ C
4
S
6
-S
4
C
5
S
6
+ C
4
C
6
S
4
S
5
0
-S
5
C
6
S
5
S
6
C
5
d
3
0 0 0 1

C
2
(C
4
C
5
C
6
- S
4
S
6
) - S
2
S
5
C
6
-C
2
(C
4
C
5
S
6
-S
4
C
6
)+S
2
S
5
S
6
T
6
1
=A
2
A
3
A
4
A
5
A
6
= S
2
(C
4
C
5
C
6
- S
4
S
6
) + C
2
S
5
C
6
-S
2
(C
4
C
5
S
6
+S
4
C
6
)-C
2
S
5
S
6
S
4
C
5
C
6
+ C
4
S
6
-S
4
C
5
S
6
+C
4
C
6
0 0

C
2
C
4
S
5
+ S
2
C
5
S
2
d
3
S
2
C
4
S
5
- C
2
C
5
-C
2
d
3
S
4
S
5
d
2
0 1
Cui cng :
n
x
O
x
a
x
p
x
T
6
= n
y
O
y
a
y
p
y
= A
1
T
6
1
n
z
O
z
a
z
p
z
0 0 0 1

tnh T
6
, ta phi nhn A
1
vi T
6
1
sau cn bng cc phn t ca ma trn T
6
hai v
ta c mt h thng cc phng trnh sau :
n
x
= C
1
[C
2
(C
4
C
5
C
6
- S
4
S
6
) - S
2
S
5
C
6
] - S
1
(S
4
C
5
C
6
+ C
4
S
6
)
n
y
= S
1
[C
2
(C
4
C
5
C
6
- S
4
S
6
) - S
2
S
5
C
6
] + C
1
(S
4
C
5
C
6
+ C
4
S
6
)
n
z
= -S
2
(C
4
C
5
C
6
- S
4
S
6
) + C
2
S
5
C
6
O
x
= C
1
[-C
2
(C
4
C
5
S
6
+ S
4
C
6
) + S
2
S
5
S
6
] - S
1
(-S
4
C
5
S
6
+ C
4
C
6
)
O
y
= S
1
[-C
2
(C
4
C
5
S
6
+ S
4
C
6
) + S
2
S
5
S
6
] + C
1
(-S
4
C
5
C
6
+ C
4
C
6
)
O
z
= S
2
(C
4
C
5
S
6
+ S
4
C
6
) + C
2
S
5
S
6
a
X
= C
1
(C
2
C
4
S
5
+ S
2
C
5
) - S
1
S
4
S
5
a
y
= S
1
(C
2
C
4
S
5
+ S
2
C
5
) + C
1
S
4
S
5
a
z
= -S
2
C
4
S
5
+ C
2
C
5
p
x
= C
1
S
2
d
3
- S
1
d
2
p
y
= S
1
S
2
d
3
+ C
1
d
2
p
z
= C
2
d
3
TS. Phm ng Phc
Robot cng nghip
38
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 T
6
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 1
Khu 2
Khu 3
Khu 4
Khu 5
Khu 6












Hnh 1.16 : Robot Elbow

6
z
4
z
0
a
5
= a
6
= 0
z
2
z
3
z
5
,z
6
x
i
O
0
,O
1
a
2
a
3
a
4

5
O
2
,O
5
,O
6
O
3
O
2 z
1
Hnh 1.17 : V tr ban u ca robot Elbow v cc h to














B thng s DH ca robot Elbow

Khu

i
*

i
a
i
d
i
1

1
90
0
0 0
2

2
0 a
2
0
3

3
0 a
3
0
4

4
-90
0
a
4
0
5

5
90
0
0 0
6

6
0 0 0
(* : cc bin khp )

Cc ma trn A ca robot Elbow c xc nh nh sau :

C
1
0 S
1
0 C
2
-S
2
0 C
2
a
2
A
1
= S
1
0 -C
1
0 A
2
= S
2
C
2
0 S
2
a
2
0 1 0 0 0 0 1 0
0 0 0 1 0 0 0 1
TS. Phm ng Phc
Robot cng nghip
39



C
3
-S
3
0 C
3
a
3
C
4
0 -S
4
C
4
a
4
A
3
= S
3
C
3
0 S
3
a
3
A
4
= S
4
0 C
4
S
4
a
4
0 0 1 0 0 -1 0 0
0 0 0 1 0 0 0 1

C
5
0 S
5
0 C
6
-S
6
0 0
A
5
= S
5
0 -C
5
0 A
6
= S
6
C
6
0 0
0 1 0 0 0 0 1 0
0 0 0 1 0 0 0 1

Ta xc nh cc ma trn T theo cc h to ln lt t khu cui tr v gc :

C
6
-S
6
0 0
T
6
5
= S
6
C
6
0 0
0 0 1 0
0 0 0 1

C
5
C
6
-C
5
S
6
S
5
0
T
6
4
= A
5
A
6
= S
5
C
6
-S
5
S
6
-C
5
0
S
6
C6 0 0
0 0 0 1

C
4
C
5
C
6
- S
4
S
6
-C
4
C
5
S
6
-S
4
C
6
C
4
S
5
C
4
a
4
T
6
3
= A
4
A
5
A
6
= S
4
C
5
C
6
+C
4
S
6
-S
4
C
5
S
6
+C
4
C
6
S
4
S
5
S
4
a
4
-S
5
C
6
S
5
S
6
C
5
0
0 0 0 1

C
34
C
5
C
6
- S
34
S
6
-C
34
C
5
C
6
- S
34
C
6
C
34
S
5
C
34
a
4
+C
3
a
3
T
6
2
= A
3
A
4
A
5
A
6
= S
34
C
5
C
6
+C
34
S
6
-S
34
C
5
S
6
+C
34
C
6
S
34
S
5
S
34
a
4
+S
3
a
3
-S
5
C
6
S
5
S
6
C
5
0
0 0 0 1

T
6
1
=A
2
A
3
A
4
A
5
A
6
=

C
234
C
5
C
6
- S
234
S
6
-C
234
C
5
S
6
- S
234
C
6
C
234
S
5
C
234
a
4
+C
23
a
3
+C
2
a
2
S
234
C
5
C
6
+ C
234
S
6
-S
234
C
5
S
6
+ C
234
C
6
S
234
S
5
S
234
a
4
+S
23
a
3
+S
2
a
2
-S
5
C
6
S
5
S
6
C
5
0
0 0 0 1

Cui cng :
n
x
O
x
a
x
p
x
T
6
= n
y
O
y
a
y
p
y
= A
1
T
6
1
n
z
O
z
a
z
p
z
0 0 0 1

tnh T
6
, ta phi nhn A
1
vi T
6
1
sau cn bng cc phn t ca ma trn T
6
ta c
mt h thng cc phng trnh sau :

TS. Phm ng Phc
Robot cng nghip
40
n
x
= C
1
(C
234
C
5
C
6
- S
234
S
6
) - S
1
S
5
C
6
n
y
= S
1
(C
234
C
5
C
6
- S
234
S
6
) + C
1
S
5
C
6
n
z
= S
234
C
5
C
6
+ C
234
S
6
O
x
= -C
1
(C
234
C
5
S
6
+ S
234
C
6
) + S
1
S
5
S
6
O
y
= -S
1
(C
234
C
5
S
6
+ S
234
C
6
) - C
1
S
5
S
6
O
z
= -S
234
C
5
S
6
+ C
234
C
6
a
X
= C
1
C
234
S
5
+ S
1
C
5
a
y
= S
1
C
234
S
5
- C
1
C
5
a
z
= S
234
S
5
p
x
= C
1
(C
234
a
4
+ C
23
a
3
+ C
2
a
2
)
p
y
= S
1
(C
234
a
4
+ C
23
a
3
+ C
2
a
2
)
p
z
= S
234
a
4
+ S
23
a
3
+ S
2
a
2

Ct u tin ca ma trn T
6
c th c xc nh bi tch vect :
r
r
r
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 A
n
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 :
? 0 -1 0
T
6
= ? 0 0 1
? -1 0 2
? 0 0 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
Robot cng nghip
41

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.23 : Robot cu hnh RRRRR

Hnh 3.22 : Robot cu hnh RRR









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
Robot cng nghip
42



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 T
6
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 T
6
. 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
,d
i
*) khi bit hnh th ca robot thng qua vect cui
T
6
(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
Robot cng nghip
43
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
,d
i
*)
sao cho robot c hnh th cho trc.
(Hnh th l khi nim m t tng minh ca vect cui T
6
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 T
6
.
+ 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 T
6
(cn gi l ma trn vect cui), Nu ta c cc gi tr s ca cc phn t trong
ma trn T
6
th c th xc nh c cc gc Euler , , thch hp. Nh vy ta c :
Euler (,,) = T
6
(4-1)

V tri ca phng trnh (4-1) c biu din bng cng thc (3-4) , nn ta
c :

cosCoscos - sinsin -cosCossin - sincos cossin 0
sinCoscos + cossin -sinCossin + coscos sinsin 0 =
-sin cos sin sin cos 0
0 0 0 1

n
x
O
x
a
x
p
x

n
y
O
y
a
y
p
y
(4-2)
n
z
O
z
a
z
p
z

0 0 0 1
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
Robot cng nghip
44
n
x
= cosCoscos - sinsin (4.3)
n
y
= sinCoscos + cossin (4-4)
n
z
= -sin cos (4-5)
O
x
= -cosCossin - sincos (4-6)
O
y
= -sinCossin + coscos (4-7)
O
z
= sin sin (4-8)
a
x
= cossin (4-9)
a
y
= sinsin (4-10)
a
z
= cos (4-11)

Ta th gii h phng trnh ny tm , , nh sau :

T (4-11) ta c = cos
-1
(a
z
) (4-12)
T (4-9) ta c = cos
-1
(a
x
/ sin) (4-13)
T (4-5) v (4-12) ta c = cos
-1
(-n
z
/ sin) (4-14)
Trong ta dng k hiu cos
-1
thay cho hm arccos.

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
d
= 0
0,180

: xc nh khng chnh xc.

+ 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 = 180
0
.


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
X- Y-
X+ Y-
Hnh 4.1 : Hm arctg2(y,x)
X- Y+
X+ Y+

V d :
arctg2(-1/-1)= -135
0
,
trong khi arctg2(1/1) = 45
0
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)
TS. Phm ng Phc
Robot cng nghip
45

c th nhn c nhng kt qu chnh xc ca bi ton Euler, ta thc hin
th thut ton hc sau : Nhn T
6
vi ma trn quay nghch o Rot(z, )
-1
,ta c:

Rot(z, )
-1
T
6
= 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 n
x
O
x
a
x
p
x
Coscos -Cos sin sin 0
-sin cos 0 0 n
y
O
y
a
y
p
y
= sin cos 0 0
0 0 1 0 n
z
O
z
a
z
p
z
-sin cos sin sin Cos 0
0 0 0 1 0 0 0 1 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 :

f
11
(n) f
11
(O) f
11
(a) f
11
(p)
f
12
(n) f
12
(O) f
12
(a) f
12
(p)
f
13
(n) f
13
(O) f
13
(a) f
13
(p)
0 0 0 1

Trong : f
11
= cos x + sin y (4-17)
f
12
= -sin x + cos y (4-18)
f
13
= z (4-19)

v x, y, z l cc phn t ca vect xc nh bi cc d kin f
11
, f
12
, f
13
, v d :
f
11
(n) = cos n
x
+ sin n
y
f
12
(O) = -sin O
x
+ cos O
y
f
13
(a) = a
z
Nh vy phng trnh (4-16) c th c vit thnh :

f
11
(n) f
11
(O) f
11
(a) 0
Coscos -Cos sin sin 0
f
12
(n) f
12
(O) f
12
(a) 0
= sin cos 0 0
(4-20)
f
13
(n) f
13
(O) f
13
(a) 0
-sin cos sin sin Cos 0
0 0 0 1
0 0 0 1

Trong f
11
,

f
12
, f
13
c nh ngha (4-17), (4-18) v (4-19).

Khi tnh ton v tri, ta ch rng p
x
, p
y
, p
z
bng 0 v php bin i Euler ch
ton php quay khng cha mt php bin i tnh tin no, nn f
11
(p) = f
12
(p) = f
13
(p)
= 0. T phng trnh (4-20), cho cn bng phn t hng 2 ct 3 ta c :
TS. Phm ng Phc
Robot cng nghip
46

f
12
(a) = -sin a
x
+ cos a
y
= 0. (4-21)

Cng hai v vi sin a
x
v chia cho cos a
x
ta c :
tg
a
a
x

= =
sin
cos
y

Gc c th xc nh bng hm arctg hai bin :

= arctg2(a
y
, a
x
).

Ta cng c th gii phng trnh (4-21) bng cch cng hai v vi -cos a
y
ri
chia hai v cho -cos a
x
, trit tiu -a
x
v tri v cos v phi, ta c :

tg
-a
-a
x

= =
sin
cos
y

Trong trng hp ny gc tm c l :

= arctg2(-a
y
, -a
x
).

Nh vy phng trnh (4-21) c mt cp nghim cch nhau 180
0
(y l
nghim ton) v ta c th vit :
= arctg2(a
y
, a
x
) v = + 180
0
.
(Hiu theo cch vit khi lp trnh trn my tnh).

Nu c a
x
v a
y
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 :

f
11
(a) = cos a
x
+ sin a
y
= sin.
V f
13
(a) = a
z
= cos.

Vy = arctg2(cos a
x
+ sin a
y
, a
z
)

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 :

f
12
(n) = -sin n
x
+ cos n
y
= sin
f
12
(O) = -sin O
x
+ cos O
y
= cos

TS. Phm ng Phc
Robot cng nghip
47
Vy : = arctg2(-sin n
x
+ cos n
y
, -sin O
x
+ cos O
y
)

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(a
y
, a
x
) v = + 180
0
= arctg2(cos a
x
+ sin a
y
, a
z
)
= arctg2(-sin n
x
+ cos n
y
, -sin O
x
+ cos O
y
)

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 : T
6
= 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 T
6
vi ma trn nghch o Rot(z, )
-1
, ta c :

Rot(z, )
-1
T
6
= Rot(y,)Rot(x, )
Hay l :

f
11
(n) f
11
(O) f
11
(a) 0
cos sin sin sin cos 0
f
12
(n) f
12
(O) f
12
(a) 0
= 0 cos -sin 0
(4-22)
f
13
(n) f
13
(O) f
13
(a) 0
-sin cos sin coscos 0
0 0 0 1
0 0 0 1

Trong : f
11
= cos x + sin y
f
12
= -sin x + cos y
f
13
= z

Cn bng phn t hng 2 ct 1 : f
12
(n) = 0, ta c :

-sin x + cos y = 0
Phng trnh ny cho ta hai nghim nh bit :

= arctg2(n
x
, n
y
)
v = + 180
0


Tip tc cn bng cc phn t tng ng ca hai ma trn ta c :

-sin = n
z
cos = cos n
x
+ sin n
y

TS. Phm ng Phc
Robot cng nghip
48
do vy :
= arctg2(-n
z
, cos n
x
+ sin n
y
)
Ngoi ra ta cn c :
-sin = -sin a
x
+ cos a
y
cos = -sin O
x
+ cos O
y

Nn : = arctg2(sin a
x
- cos a
y
, -sin O
x
+ cos O
y
)

Nh vy ta xc nh c cc gc quay Roll, Pitch v Yaw theo cc phn t
ca ma trn T
6
.

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 :
T
6
= A
1
A
2
A
3
A
4
A
5
A
6
(4-23)

Lin tc nhn (4-23) vi cc ma trn A nghch o, ta c :

A
1
T
1
6
=
1
T
6
(4-24)
A A
1
T
2
1 1
6
=
2
T
6
(4-25)
A
3
A A
1
T
1
2
1 1
6
=
3
T
6
(4-26)
A A
3
A A
1
T
4
1 1
2
1 1
6
=
4
T
6
(4-27)
A A A
3
A A
1
T
5
1
4
1 1
2
1 1
6
=
5
T
6
(4-28)

Cc phn t v tri ca cc phng trnh ny l hm s ca cc phn t T
6
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 :

C
1
S
1
0 0 n
x
O
x
a
x
p
x
A T
1
1
6
=
0 0 -1 0 n
y
O
y
a
y
p
y
-S
1
C
1
0 0 n
z
O
z
a
z
p
z
0 0 0 1 0 0 0 1


f
11
(n) f
11
(O) f
11
(a) f
11
(p)
= f
12
(n) f
12
(O) f
12
(a) f
12
(p)
f
13
(n) f
13
(O) f
13
(a) f
13
(p)
0 0 0 1
TS. Phm ng Phc
Robot cng nghip
49

Trong : f
11
= C
1
x + S
1
y
f
12
= - z
f
13
= -S
1
x + C
1
y
V bn phi ca (4-24) l :

C
2
(C
4
C
5
C
6
- S
4
S
6
) - S
2
S
5
C
6
-C
2
(C
4
C
5
S
6
-S
4
C
6
)+S
2
S
5
S
6
C
2
C
4
S
5
+ S
2
C
5
S
2
d
3
1
T
6
= S
2
(C
4
C
5
C
6
- S
4
S
6
) + C
2
S
5
C
6
-S
2
(C
4
C
5
S
6
+S
4
C
6
)-C
2
S
5
S
6
S
2
C
4
S
5
- C
2
C
5
-C
2
d
3
S
4
C
5
C
6
+ C
4
S
6
-S
4
C
5
S
6
+C
4
C
6
S
4
S
5
d
2
0 0 0 1

Cc phn t ca ma trn v phi u l hm s ca
2
, d
3
,
4
,
5
,
6
ngoi tr
phn t hng 3 ct 4, l :

f
13
(p) = d
2
hay :
-S
1
p
x
+ C
1
p
y
= d
2
gii phng trnh dng ny ta c th thay th bi cc hm lng gic sau
y :
p
x
= r cos
p
y
= r sin

Trong : r = + p p
x
2
+
y
2

= arctg2(p
y
, p
x
)

Th p
x
v p
y
vo phng trnh -S
1
p
x
+ C
1
p
y
= d
2
ta c :

sincos
1
- cossin
1
= d
2
/ r Vi 0 < d
2
/ r 1

Hay l : sin( -
1
) = d
2
/ r Vi 0 < -
1
<
T ta c :
cos( -
1
) = 1
2
2
( / ) d r

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(p
y
, p
x
) - arctg2(d
2
, 1
2
2
( / ) d r ) (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 :

S
2
d
3
= C
1
p
x
+ S
1
p
y
-C
2
d
3
= -p
z

d
3
l dch chuyn di ca khp tnh tin, d
3
> 0, nn ta c :

2
= arctg2(C
1
p
x
+ S
1
p
y
, p
z
) (4-30)
TS. Phm ng Phc
Robot cng nghip
50
T phng trnh (4-25) : A A
1
T
2
1 1
6
= A
2
1 1
T
6
=
2
T
6
, ta c :

f
21
(n) f
21
(O) f
21
(a) 0
C
4
C
5
C
6
-S
4
S
6
-C
4
C
5
S
6
- S
4
C
6
C
4
S
5 0
f
22
(n) f
22
(O) f
22
(a) 0 = S
4
C
5
C

+ C
4
S
6
-S
4
C
5
S
6
+ C
4
C
6
S
4
S
5 0
f
23
(n) f
23
(O) f
23
(a) f
23
(p)
-S
5
C
6
S
5
S
6
C
5 d
3
0 0 0 1 0 0 0 1
(4-31)
Trong : f
21
= C
2
(C
1
x + S
1
y) - S
2
z
f
22
= -S
1
x + C
1
y
f
23
= S
2
(C
1
x + S
1
y) + C
2
z

T cn bng phn t hng 3 ct 4 ta c :

d
3
= S
2
(C
1
p
x
+ S
1
p
y
) + C
2
p
z
(4-32)

- T phng trnh (4-27) ta c : A A
4
1
3
1 2
T
6
=
4
T
6
Thc hin php nhn cc ma trn v tri, v biu din dng rt gn nh sau
:

f
41
(n) f
41
(O) f
41
(a) 0 C
5
C
6
-C
5
S
6
S
5
0
f
42
(n) f
42
(O) f
42
(a) 0 = S
5
C
6
-S
5
S
6
C
5
0
f
43
(n) f
43
(O) f
43
(a) 0 S
6
C
6
0 0
0 0 0 1 0 0 0 1

Trong : f
41
= C
4
[C
2
(C
1
x + S
1
y) - S
2
z] + S
4
(-S
1
x + C
1
y)
f
42
= -S
2
(-S
1
x + C
1
y) - C
2
z
f
43
= -S
4
[C
2
(C
1
x + S
1
y) + S
2
z] + C
4
(-S
1
x + C
1
y)

Cn bng phn t hng 3, ct 3 ta c mt hm s ca
4
, l : f
43
(a) = 0.
Hay :
-S
4
[C
2
(C
1
a
x
+ S
1
a
y
) + S
2
a
z
] + C
4
(-S
1
a
x
+ C
1
a
y
) = 0

y l phng trnh lng gic c dng : - sin a
x
+ cos a
y
= 0. Nh gii
trong cc phn trc y, phng trnh ny c hai nghim :


4
= arctg2(-S
1
a
x
+ C
1
a
y
, C
2
(C
1
a
x
+ S
1
a
y
) + S
2
a
z
) (4-33)
v
4
=
4
+ 180
0


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 :

C
4
S
5
= C
2
(C
1
a
x
+ S
1
a
y
) - S
2
a
z
TS. Phm ng Phc
Robot cng nghip
51
S
4
S
5
= -S
1
a
x
+ C
1
a
y

Vi
5
> 0 ta c
4
= arctg(-S
1
a
x
+ C
1
a
y
, C
2
(C
1
a
x
+ S
1
a
y
) + S
2
a
z
)
Vi
5
< 0 ta c
4
=
4
+ 180
0
ng nh kt qu tm (4-33).
Khi S
5
= 0,
5
= 0. Robot c suy bin do c hai trc ca khp 4 v 6 nm thng
hng (z
3
z
5
). 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
4
A
1
3
1 2
T
6
=
4
T
6
= A
5
A
6
ta c th c cc
phng trnh ca S
5
, C
5
, S
6
v C
6
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 :

S
5
= C
4
[C
2
(C
1
a
x
+ S
1
a
y
) - S
2
a
z
] + S
4
(-S
1
a
x
+ C
1
a
y
)
C
5
= S
2
(C
1
a
x
+ S
1
a
y
) + C
2
a
z
T suy ra :

5
= arctg2(C
4
[C
2
(C
1
a
x
+ S
1
a
y
) - S
2
a
z
] + S
4
(-S
1
a
x
+ C
1
a
y
) ,
S
2
(C
1
a
x
+ S
1
a
y
) + C
2
a
z
) (4-34)

Cc phng trnh c lin quan n
6
nm ct 1 ca phng trnh ma trn,
l cc thnh phn ca vect n ca T
6
. Vect ny thng khng c ngha trong tnh
ton, v n lun c th c xc nh bng tch vect ca hai vect O v a nh ni
trc y ( ). Do ta phi tm cch khc xc nh
r
r
r
n = O x a
6
.

Thc hin php nhn cc ma trn v tri ca phng trnh (4-28) : A
4
5
1
T =
6
5
T = A , biu din dng k hiu ta c :
6 6

f
51
(n) f
51
(O) 0 0 C
6
-S
6
0 0
f
52
(n) f
52
(O) 0 0 = S
6
C
6
0 0 (4-35)
f
53
(n) f
53
(O) 1 0 0 0 1 0
0 0 0 1 0 0 0 1
Trong :

f
51
= C
5
{ C
4
[C
2
(C
1
x + S
1
y) - S
2
z] + S
4
(-S
1
x + C
1
y)} + S
5
[-S
2
(C
1
x + S
1
y) - C
2
z]
f
52
= -S
4
[C
2
(C
1
x + S
1
y) - S
2
z] + C
4
[-S
1
x + C
1
y]
f
53
= S
5
{ C
4
[C
2
(C
1
x + S
1
y) - S
2
z] + S
4
(-S
1
x + C
1
y)} + C
5
[S
2
(C
1
x + S
1
y) - C
2
z]
Cho cn bng cc phn t hng 1 ct 2 v hng 2 ct 2 ta nhn c cc gi
tr ca S
6
v C
6
:

S
6
= -C
5
{C
4
[C
2
(C
1
O
x
+S
1
O
y
)-S
2
O
z
] +S
4
(-S
1
O
x
+C
1
O
y
)} + S
5
[S
2
(C
1
O
x
+ S
1
O
y
) + C
2
O
z
]
C
6
= -S
4
[C
2
(C
1
O
x
+ S
1
O
y
)- S
2
O
z
] + C
4
[-S
1
O
x
+ C
1
O
y
]

T ta xc nh c :
6
= arctg2(S
6
, C
6
) (4-36)
TS. Phm ng Phc
Robot cng nghip
52

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
1
T
1
6
=
1
T
6
= A
2
A
3
A
4
A
5
A
6

Tng t nh lm, ta xc nh cc phn t ma trn ca hai v nh sau :

f
11
(n) f
11
(O) f
11
(a) f
11
(p)

f
12
(n) f
12
(O) f
12
(a) f
12
(p)
=
f
13
(n) f
13
(O) f
13
(a) f
13
(p)

0 0 0 1


C
234
C
5
C
6
- S
234
S
6
-C
234
C
5
S
6
- S
234
C
6
C
234
S
5
C
234
a
4
+C
23
a
3
+C
2
a
2
S
234
C
5
C
6
+ C
234
S
6
-S
234
C
5
S
6
+ C
234
C
6
S
234
S
5
S
234
a
4
+S
23
a
3
+S
2
a
2
-S
5
C
6
S
5
S
6
C
5
0
0 0 0 1
(4-37)
Trong : f
11
= C
1
x + S
1
y
f
12
= z
f
13
= S
1
x + C
1
y
Ta k hiu : C
234
= cos(
2
+
3
+
4
)
S
234
= sin(
2
+
3
+
4
)

Cho cn bng phn t hng 3 ct 4, ta c :
S
1
p
x
+ C
1
p
y
= 0
Suy ra :

1
= arctg2(p
y
, p
x
)
v
1
=
1
+ 180
0
(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
-1
i
. Cho n khu th
4 th php nhn vi ma trn nghch o mi c ngha.

A
-1
4
A
-1
3
A
-1
2
1
T
6
=
4
T
6
= A
5
A
6

Khi xc nh cc phn t ma trn ca hai v ta c :
TS. Phm ng Phc
Robot cng nghip
53

f
41
(n) f
41
(O) f
41
(a) f
41
(p)-C
34
a
2
-C
4
a
3
-a
4
C
5
C
6
-C
5
S
6
S
5
0
f
42
(n) f
42
(O) f
42
(a) 0 = S
5
C
6
-S
5
S
6
-C
5
0
f
43
(n) f
43
(O) f
43
(a) f
43
(p)+S
34
a
2
+S
4
a
3
S
6
C
6
0 0
0 0 0 1 0 0 0 1
(4-39)
Trong : f
41
= C
234
(C
1
x + S
1
y) + S
234
z
f
42
= -S
1
x + C
1
y
f
43
= -S
234
(C
1
x + S
1
y) + C
234
z

Cn bng phn t hng 3 ct 3 ta c mt phng trnh cho
234
:

-S
234
(C
1
a
x
+ S
1
a
y
) + C
234
a
z
= 0
Suy ra :

234
= arctg2(a
z
, C
1
a
x
+ S
1
a
y
)
v
234
=
234
+ 180
0
(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 :
C
1
p
x
+ S
1
p
y
= C
234
a
4
+C
23
a
3
+C
2
a
2

(a)
p
z
= S
234
a
4
+S
23
a
3
+S
2
a
2
(b)
Ta gi : p
x
= C
1
p
x
+ S
1
p
y
- C
234
a
4


(c)
p
y
= p
z
- S
234
a
4


(d)
em (a) + (c) v (b) + (d) ta c ;
p
x
= C
23
a
3
+ C
2
a
2


(e)
p
y
= S
23
a
3
+ S
2
a
2


(g)

Bnh phng hai v v cng hai phng trnh (e) v (g), ta c :
p
2
x
= (C
23
a
3
+ C
2
a
2
)
2
p
2
y
= (S
23
a
3
+ S
2
a
2
)
2

p
2
x
+ p
2
y
= (S
2
23
+ C
2
23
)a
2
3
+ (S
2
2
+ C
2
2
)a
2
2
+ 2 a
2
a
3
(C
23
C
2
+ S
23
S
2
)


Ta c C
23
C
2
+ S
23
S
2
= cos(
2
+
3
-
2
) = cos
3
= C
3
. Nn suy ra :

C
3
= (p
2
x
+ p
2
y
- a
2
3
- a
2
2
) / 2a
2
a
3

Trong khi c th tm
3
t hm arccos, ta vn nn tm mt gi tr S
3
v dng
hm arctg2 nh thng l :
Ta c : S
3
= ( ) 1
3
2
C

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
Robot cng nghip
54

3
= arctg2(S
3
, C
3
) (4-41)

tm S
2
v C
2
ta gii h phng trnh (e),(g).

T (e) (C
2
C
3
- S
2
S
3
)a
3
+ C
2
a
2
= p
x

T (g) (S
2
C
3
- C
2
S
3
)a
3
+ S
2
a
2
= p
y


Khai trin v rt gn :

(C
3
a
3
+ a
2
)C
2
- S
3
a
3
.S
2
= p
x

T (g) S
3
a
3
.C
2
+ (C
3
a
3
+ a
2
)S
2
= p
y

Ta c :
=
C a + a - S a
S a C a + a
3 3 2
3 3 3 3 2
3 3

c
=
p - S a
p C a + a
x
,
y
,
3 3 2
3 3

s
=
C a + a p
S a p
3 3 2 x
,
3 3 y
,


C
2
=
2
3 3
2
2 3 3
x 3 3 y 2 3 3
C
) a (S ) a a (C
p' a S )p' a a (C
+ +
+
=

S
2
=

S
3 3 2 x 3 3 y
3 3 2
2
3 3
2
(C a a )p' S a p'
(C a a ) (S a )
=
+ +
+ +


Do mu s dng v bng nhau, nn ta c :


2
= arctg2(S
2
, C
2
)

2
= arctg2((C
3
a
3
+ a
2
)p
y
- S
3
a
3
p
x
, (C
3
a
3
+ a
2
)p
x
+ S
3
a
3
p
y
) (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
4
T
6
(4-39) :

S
5
= C
234
(C
1
a
x
+ S
1
a
y
) + S
234
a
z
C
5
= S
1
a
x
- C
1
a
y
Suy ra :

5
= arctg2(C
234
(C
1
a
x
+ S
1
a
y
) + S
234
a
z
, S
1
a
x
- C
1
a
y
) (4-44)

tm
6
, ta tip tc nhn A
-1
5
vi

4
T
6
, ta c : A
-1
5
.

4
T
6
= A
6
.
Vit tch ma trn v tri dng k hiu :
TS. Phm ng Phc
Robot cng nghip
55

f
51
(n) f
51
(O) 0 0
C
6
-S
6
0 0
f
52
(n) f
52
(O) 0 0
= S
6
C
6
0 0
0 0 1 0
0 0 1 0
0 0 0 1
0 0 0 1

Trong : f
51
= C
5
[C
234
(C
1
x + S
1
y) + S
234
z] - S
5
(S
1
x + C
1
y)
f
52
= -S
234
(C
1
x + S
1
y) + C
234
z

Cho cn bng cc phn t ma trn tng ng, ta c :

S
6
= -C
5
[C
234
(C
1
O
x
+ S
1
O
y
) + S
234
O
z
] - S
5
(S
1
O
x
+ C
1
O
y
)
C
6
= -S
234
(C
1
O
x
+ S
1
O
y
) + C
234
O
z
Vy :

6
= arctg2(S
6
, C
6
) (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 a
x
+ cos a
y
= 0

Dng ny cho ta mt cp nghim cch nhau 180
0
, 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 : -S
1
p
x
+ C
1
p
y
= d
2

Dng ny cng cho ta cp nghim sai khc nhau 180
0
, 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 : C
1
p
x
+ S
1
p
y
= S
2
d
3
v dng th t : - C
2
d
3
= - p
z
TS. Phm ng Phc
Robot cng nghip
56

Cc phng thnh ny thng c nghim duy nht.
Ngoi cc dng ph bin, khi robot c hai hay nhiu khp song song (V d
robot Elbow), cc gc ca tng khp phi c xc nh bng cch gii ng thi
nhiu khp trong mi quan h tng cc gc khp.

Tm ra cc nghim ph hp vi hnh th ca robot (v tr v hng) l mt trong
nhng vn kh khn nht. Hu nh cha c thut ton chung no m nh c th
tm ra c tp nghim cho mi robot. Tuy nhin phng php a ra trong chng
ny thit lp c cc nghim s dng tng minh, trc tip.

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.

2
a
1
a
2
O
0
z
1
z
2
x
1
y
1
y
2
O
1
O
2
z
0
x
0
y
0
x
2
Hnh 4.3 : H to v cc thng s ca robot 2 khu phng




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

1
,
2
) ca robot c hai khu phng nh hnh 4.3 :
Hnh 4.2 : Robot cu hnh RRR







TS. Phm ng Phc
Robot cng nghip
57








Bi 3 : Thit lp h phng trnh ng hc ca robot SCARA (hnh 4.4) v gii bi
ton ng hc ngc ca n.


















O
0

1
x
x
d
3
x
x
x
z
3
, z
4

4
O
3
O
4
z
0 z
1
z
2
a
1
a
2
d
4
Hnh 4.4 : Robot SCARA
TS. Phm ng Phc
Robot cng nghip
58



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
Robot cng nghip
61

Thanh cng c Menu chnh
Ca s nhp - xut d liu .
Dng chn nhanh kiu giao din
Meta keys Thanh trng thi

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
th n chut vo biu tng trn thanh cng c.
+ 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 ; nh ngha hm Sum tnh tng.
Param integer X, Y, Z ; Khai bo cc tham bin kiu nguyn.
integer Tong ; Khai bo bin Tong (bin a phng).
Tong= X+Y+Z ; Tng ca 3 s.
return Tong ; Tr v gi tr ca tng ca 3 s.
Endfunc ; 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
Robot cng nghip
68
endswitch
V d :
proc main
integer Alpha = 2 ; gn gi tr ban u cho bin Alpha=2.

switch Alpha ; tm gi tr ca bin s
case 0 ; Trng hp bin c gi tr bng 0.
usermsg "Alpha = 0" ; Xut kt qu trn ca s mn hnh.
Endcase ; Ht trng hp so snh th nht.
case 1 ; tng t nh trn . . .
usermsg "Alpha = 1"
endcase
case 2
usermsg "Alpha = 2"
endcase
endswitch ; lun i km vi switch kt thc khi lnh switch.
endproc

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
Robot cng nghip
69
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 ; Chuyn lnh iu khin robot TG-45
endproc

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
Robot cng nghip
70
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 ; M file ch c (s hiu file id=0).
while not feof 0 ; Lp li khi cha kt thc file.
fgets 0 FInput ; c mt dng t file
TS. Phm ng Phc
Robot cng nghip
71
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 :







o
o
o
Thn
Vai Cnh tay
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 261
0
.
+ Chuyn ng ca vai 85
0
.
+ Chuyn ng ca cnh tay 249
0
.
+ Chuyn ng ca c tay 180
0
.

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
= (261
o
/2) x 200 / 511 51
0
04

Lnh C+200 s iu khin cnh tay robot quay ln pha trn mt gc :

3
= (249
o
/2) x 200 / 511 48
0
43 (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 A
n
(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
= +30
0
;
2
= -10
0
;
3
= -30
0
;
4
= -25
0
.
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