1

Robot Cng nghip

Chng I
Gii thiu chung v robot cng nghip
1.1. S lt qu trnh pht trin ca robot cng nghip (IR : Industrial Robot) :
Thut ng Robot xut pht t ting Sec (Czech) Robota c ngha l cng vic tp
dch trong v kch Rossums Universal Robots ca Karel Capek, vo nm 1921. Trong v kch
ny, Rossum v con trai ca ng ta ch to ra nhng chic my gn ging vi con ngi
phc v con ngi. C l l mt gi ban u cho cc nh sng ch k thut v nhng c
cu, my mc bt chc cc hot ng c bp ca con ngi.
u thp k 60, cng ty M AMF (American Machine and Foundry Company) qung
co mt loi my t ng vn nng v gi l Ngi my cng nghip (Industrial Robot).
Ngy nay ngi ta t tn ngi my cng nghip (hay robot cng nghip) cho nhng loi thit
b c dng dp v mt vi chc nng nh tay ngi c iu khin t ng thc hin mt s
thao tc sn xut.
V mt k thut, nhng robot cng nghip ngy nay, c ngun gc t hai lnh vc k
thut ra i sm hn l cc c cu iu khin t xa (Teleoperators) v cc my cng c iu
khin s (NC - Numerically Controlled machine tool).
Cc c cu iu khin t xa (hay cc thit b kiu ch-t) pht trin mnh trong chin
tranh th gii ln th hai nhm nghin cu cc vt liu phng x. Ngi thao tc c tch bit
khi khu vc phng x bi mt bc tng c mt hoc vi ca quan st c th nhn thy
c cng vic bn trong. Cc c cu iu khin t xa thay th cho cnh tay ca ngi thao tc;
n gm c mt b kp bn trong (t) v hai tay cm bn ngoi (ch). C hai, tay cm v b
kp, c ni vi nhau bng mt c cu su bc t do to ra cc v tr v hng tu ca tay
cm v b kp. C cu dng iu khin b kp theo chuyn ng ca tay cm.
Vo khong nm 1949, cc my cng c iu khin s ra i, nhm p ng yu cu
gia cng cc chi tit trong ngnh ch to my bay. Nhng robot u tin thc cht l s ni kt
gia cc khu c kh ca c cu iu khin t xa vi kh nng lp trnh ca my cng c iu
khin s.
Di y chng ta s im qua mt s thi im lch s pht trin ca ngi my cng
nghip. Mt trong nhng robot cng nghip u tin c ch to l robot Versatran ca cng
ty AMF, M. Cng vo khong thi gian ny M xut hin loi robot Unimate -1900 c
dng u tin trong k ngh t.
Tip theo M, cc nc khc bt u sn xut robot cng nghip : Anh -1967, Thu
in v Nht -1968 theo bn quyn ca M; CHLB c -1971; Php - 1972; - 1973. . .
Tnh nng lm vic ca robot ngy cng c nng cao, nht l kh nng nhn bit v
x l. Nm 1967 trng i hc tng hp Stanford (M) ch to ra mu robot hot ng
theo m hnh mt-tay, c kh nng nhn bit v nh hng bn kp theo v tr vt kp nh
cc cm bin. Nm 1974 Cng ty M Cincinnati a ra loi robot c iu khin bng my vi
tnh, gi l robot T3 (The Tomorrow Tool : Cng c ca tng lai). Robot ny c th nng c
vt c khi lng n 40 KG.
C th ni, Robot l s t hp kh nng hot ng linh hot ca cc c cu iu khin t
xa vi mc tri thc ngy cng phong ph ca h thng iu khin theo chng trnh s
cng nh k thut ch to cc b cm bin, cng ngh lp trnh v cc pht trin ca tr khn
nhn to, h chuyn gia ...
Trong nhng nm sau ny, vic nng cao tnh nng hot ng ca robot khng ngng
pht trin. Cc robot c trang b thm cc loi cm bin khc nhau nhn bit mi trng

TS. Phm ng Phc

Robot Cng nghip

chung quanh, cng vi nhng thnh tu to ln trong lnh vc Tin hc - in t to ra cc

th h robot vi nhiu tnh nng c bit, S lng robot ngy cng gia tng, gi thnh ngy
cng gim. Nh vy, robot cng nghip c v tr quan trng trong cc dy chuyn sn xut
hin i.
Mt vi s liu v s lng robot c sn xut mt vi nc cng nghip pht trin
nh sau :
(Bng I.1)
Nc SX
Nm 1990
Nm 1994
Nm 1998
(D tnh)
Nht
60.118
29.756
67.000
M
4.327
7.634
11.100
c
5.845
5.125
8.600
2.500
2.408
4.000

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

TS. Phm ng Phc

Robot Cng nghip

1.3. Cc khi nim v nh ngha v robot cng nghip :

1.3.1. nh ngha robot cng nghip :
Hin nay c nhiu nh ngha v Robot, c th im qua mt s nh ngha nh sau :
nh ngha theo tiu chun AFNOR (Php) :
Robot cng nghip l mt c cu chuyn ng t ng c th lp trnh, lp li cc
chng trnh, tng hp cc chng trnh t ra trn cc trc to ; c kh nng nh v, nh
hng, di chuyn cc i tng vt cht : chi tit, dao c, g lp . . . theo nhng hnh trnh
thay i chng trnh ho nhm thc hin cc nhim v cng ngh khc nhau.
nh ngha theo RIA (Robot institute of America) :
Robot l mt tay my vn nng c th lp li cc chng trnh c thit k di
chuyn vt liu, chi tit, dng c hoc cc thit b chuyn dng thng qua cc chng trnh
chuyn ng c th thay i hon thnh cc nhim v khc nhau.
nh ngha theo OCT 25686-85 (Nga) :
Robot cng nghip l mt my t ng, c t c nh hoc di ng c, lin kt
gia mt tay my v mt h thng iu khin theo chng trnh, c th lp trnh li hon
thnh cc chc nng vn ng v iu khin trong qu trnh sn xut.
C th ni Robot cng nghip l mt my t ng linh hot thay th tng phn hoc
ton b cc hot ng c bp v hot ng tr tu ca con ngi trong nhiu kh nng thch
nghi khc nhau.
Robot cng nghip c kh nng chng trnh ho linh hot trn nhiu trc chuyn
ng, biu th cho s bc t do ca chng. Robot cng nghip c trang b nhng bn tay
my hoc cc c cu chp hnh, gii quyt nhng nhim v xc nh trong cc qu trnh cng
ngh : hoc trc tip tham gia thc hin cc nguyn cng (sn, hn, phun ph, rt kim loi
vo khun c, lp rp my . . .) hoc phc v cc qu trnh cng ngh (tho lp chi tit gia
cng, dao c, g . . .) vi nhng thao tc cm nm, vn chuyn v trao i cc i tng
vi cc trm cng ngh, trong mt h thng my t ng linh hot, c gi l H thng t
ng linh hot robot ho cho php thch ng nhanh v thao tc n gin khi nhim v sn
xut thay i.
1.3.2. Bc t do ca robot (DOF : Degrees Of Freedom) :
Bc t do l s kh nng chuyn ng ca mt c cu (chuyn ng quay hoc tnh
tin). dch chuyn c mt vt th trong khng gian, c cu chp hnh ca robot phi t
c mt s bc t do. Ni chung c h ca robot l mt c cu h, do bc t do ca n c
th tnh theo cng thc :
5

w = 6n -

ip
i =1

(1.1)

y :

n - S khu ng;
pi - S khp loi i (i = 1,2,. . .,5 : S bc t do b hn ch).
i vi cc c cu c cc khu c ni vi nhau bng khp quay hoc tnh tin (khp
ng loi 5) th s bc t do bng vi s khu ng . i vi c cu h, s bc t do bng tng
s bc t do ca cc khp ng.
nh v v nh hng khu chp hnh cui mt cch tu trong khng gian 3
chiu robot cn c 6 bc t do, trong 3 bc t do nh v v 3 bc t do nh hng.
Mt s cng vic n gin nng h, sp xp... c th yu cu s bc t do t hn. Cc robot
hn, sn... thng yu cu 6 bc t do. Trong mt s trng hp cn s kho lo, linh hot
hoc khi cn phi ti u ho qu o,... ngi ta dng robot vi s bc t do ln hn 6.
1.3.3. H to (Coordinate frames) :
Mi robot thng bao gm nhiu khu (links) lin kt vi nhau qua cc khp (joints),
to thnh mt xch ng hc xut pht t mt khu c bn (base) ng yn. H to gn vi

TS. Phm ng Phc

Robot Cng nghip

khu c bn gi l h to c bn (hay h to chun). Cc h to trung gian khc gn

vi cc khu ng gi l h to suy rng. Trong tng thi im hot ng, cc to suy
rng xc nh cu hnh ca robot bng cc chuyn dch di hoc cc chuyn dch gc cu cc
khp tnh tin hoc khp quay (hnh 1.1). Cc to suy rng cn c gi l bin khp.
z
4
3

d2

n
a

o On

1
y
x

O0

Hnh 1.1 : Cc to suy rng ca robot.

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

O
y

Hnh 1.2 : Qui tc bn tay phi

1.3.4. Trng cng tc ca robot (Workspace or Range of motion):

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

H
R

Hnh chiu bng

Hnh chiu ng
Hnh 1.3 : Biu din trng cng tc ca robot.

TS. Phm ng Phc

Robot Cng nghip

1.4. Cu trc c bn ca robot cng nghip :

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

B iu
khin v
my tnh

Thit b
dy hc

Ngun
ng lc

Cc chng
trnh

Cnh tay
robot

Dng c
thao tc

Hnh 1.4 : Cc thnh phn chnh ca h thng robot.

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

TS. Phm ng Phc

Robot Cng nghip

Cc khu ca robot thng thc hin hai chuyn ng c bn :

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

T.T.T

Hnh 1.5 : Robot kiu to cc

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

R.R.R

R.R.T

Hnh 1.7 : Robot kiu to cu

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

Robot Cng nghip

ng theo h to gc l gn nh, tc l c vng lm vic tng i ln so vi kch c ca

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

Hnh 1.8 : Robot hot ng theo h to gc.

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

Hnh 1.9 : Robot kiu SCARA

1.5. Phn loi Robot cng nghip :

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

TS. Phm ng Phc

Robot Cng nghip

1.4.3. Phn loi theo ng dng :

Da vo ng dng ca robot trong sn xut c Robot sn, robot hn, robot lp rp,
robot chuyn phi .v.v...
1.4.4. Phn loi theo cch thc v c trng ca phng php iu khin :
C robot iu khin h (mch iu khin khng c cc quan h phn hi), Robot iu
khin kn (hay iu khin servo) : s dng cm bin, mch phn hi tng chnh xc v
mc linh hot khi iu khin.
Ngoi ra cn c th c cc cch phn loi khc tu theo quan im v mc ch nghin
cu
-----------------------------------------------------------------------------------------------------------------

TS. Phm ng Phc

Robot cng nghip

Chng II
Cc php bin i thun nht
(Homogeneous Transformation)
Khi xem xt, nghin cu mi quan h gia robot v vt th ta khng nhng cn quan
tm n v tr (Position) tuyt i ca im, ng, mt ca vt th so vi im tc ng cui
(End effector) ca robot m cn cn quan tm n vn nh hng (Orientation) ca khu
chp hnh cui khi vn ng hoc nh v ta mt v tr.
m t quan h v v tr v hng gia robot v vt th ta phi dng n cc php
bin i thun nht.
Chng ny cung cp nhng hiu bit cn thit trc khi i vo gii quyt cc vn
lin quan ti ng hc v ng lc hc robot.
2.1. H ta thun nht :
biu din mt im trong khng gian ba chiu, ngi ta dng Vect im (Point
vector). Vect im thng c k hiu bng cc ch vit thng nh u, v, x1 . . . m t v
tr ca im U, V, X1 ,. . .
Ty thuc vo h qui chiu c chn, trong khng gian 3 chiu, mt im V c th
c biu din bng nhiu vect im khc nhau :
V
vE
vF
E
F

Hnh 2.2 : Biu din 1 im trong khng gian

vE v vF l hai vect khc nhau mc d c hai vect cng m t im V. Nu i, j, k l
cc vec t n v ca mt h to no , chng hn trong E, ta c :
r
r
r
r
v = ai + bj + ck
vi a, b, c l to v tr ca im V trong h .
Nu quan tm ng thi vn nh v v nh hng, ta phi biu din vect v trong
khng gian bn chiu vi sut vect l mt ma trn ct :
v

x
y
z
w

Trong

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

vi w l mt hng s thc no .
w cn c gi l h s t l, biu th cho chiu th t ngm nh, Nu w = 1 d thy :

x x
y y
z z
= = x=a;
= = y =b;
= =z=a
w 1
w 1
w 1

TS. Phm ng Phc

10

Robot cng nghip

Trong trng hp ny th cc to biu din bng vi to vt l ca im trong

khng gian 3 chiu, h to s dng w=1 c gi l h to thun nht.
Vi w = 0

x y z
= = =
w w w

ta c :

Gii hn th hin hng ca cc trc to .

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

r
r
r
r
a = a xi + a y j + az k
r
r
r
r
b = bx i + by j + bz k

Cho hai vect :

Ta c tch v hng
V tch vect :
r r
i
j
r
r
axb = a a
x
y
bx by

a.b = axbx + ayby + azbz

r
k
az
bz

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

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 cij
bng tng (hiu) ca cc phn t aij v bij (vi mi i, j).
A+B=C
Vi cij = aij + bij.
A-B =C
Vi cij = aij - bij.
Php cng, tr ma trn c cc tnh cht ging php cng s thc.
b/ Tch ca hai ma trn : Tch ca ma trn A (kch thc m x n) vi ma trn B (kch
thc n x p) l ma trn C c kch thc m x p.
V d : cho hai ma trn :
A =

1
4
7

Ta c :

TS. Phm ng Phc

2
5
8

3
6
9

B =

1
3
5

2
4
6

11

Robot cng nghip

1.1+2.3+3.5
4.1+5.3+6.5
7.1+8.3+9.5

C = A.B =

1.2+2.4+3.6
4.2+5.4+6.6
7.2+8.4+9.6

22
49
76

28
64
100

Php nhn hai ma trn khng c tnh giao hon, ngha l : A . B B . A

Ma trn n v I (Indentity Matrix) giao hon c vi bt k ma trn no : I.A = A.I
Php nhn ma trn tun theo cc qui tc sau :
1. (k.A).B = k.(A.B) = A.(k.B)
2. A.(B.C) = (A.B).C
3. (A + B).C = A.C + B.C
4. C.(A + B) = C.A + C.B
c/ Ma trn nghch o ca ma trn thun nht :
Mt ma trn thun nht l ma trn 4 x 4 c dng :
nx
ny
nz
0

T =

Ox
Oy
Oz
0

ax
ay
az
0

px
py
pz
1

nz
Oz
az
0

-p.n
-p.O
-p.a
1

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

nx
Ox
ax
0

T-1 =

ny
Oy
ay
0

(2-1)

Trong p.n l tch v hng ca vect p v n. ngha l :

p.n = pxnx + pyny + pznz
tng t :
p.O = pxOx + pyOy + pzOz
v
p.a = pxax + pyay + pzaz
V d : tm ma trn nghch o ca ma trn bin i thun nht :
0
0
-1
0

H =

0
1
0
0

1
0
0
0

1
2
3
1

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

-1

H =

0
0
1
0

0
1
0
0

-1 3
0 -2
0 -1
0 1

Chng ta kim chng rng y chnh l ma trn nghch o bng cc nhn ma trn H vi H-1 :
0
0
-1
0

0
1
0
0

1
0
0
0

TS. Phm ng Phc

1
2
3
1

0
0
1
0

0
1
0
0

-1 3
0 -2
0 -1
0 1

1
0
0
0

0
1
0
0

0
0
1
0

0
0
0
1

12

Robot cng nghip

Phng php tnh ma trn nghch o ny nhanh hn nhiu so vi phng php chung;
tuy nhin n khng p dng c cho ma trn 4x4 bt k m kt qu ch ng vi ma trn
thun nht.
d/ Vt ca ma trn :
Vt ca ma trn vung bc n l tng cc phn t trn ng cho :
n

Trace(A) hay Tr(A) =

a
i =1

ii

Mt s tnh cht quan trng ca vt ma trn :

1/ Tr(A) = Tr(AT)
2/ Tr(A+B) = Tr(A) + Tr(B)
3/ Tr(A.B) = Tr(B.A)
4/ Tr(ABCT) = Tr(CBTAT)
e/ o hm v tch phn ma trn :
Nu cc phn t ca ma trn A l hm nhiu bin, th cc phn t ca ma trn o hm
bng o hm ring ca cc phn t ma trn A theo bin tng ng.

V d : cho

a11
a
A = 21
a31

a 41

a12

a13

a 22

a 23

a32
a 42

a33
a 43

a14
a 24

a 34

a 44

a11 a12 a13 a14

t
t
t
t
a
a22 a23 a 24
21

t
t
t dt
dA = t
th :
a31 a32 a33 a34
t
t
t
t
a41 a42 a43 a 44

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

A(t )dt = { aij (t )dt}

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

TS. Phm ng Phc

1
0
0
0

0
1
0
0

0
0
1
0

a
b
c
1

(2.2)

13

Robot cng nghip

Gi u l vect biu din im cn tnh tin :

u = [x y z w]T
Th v l vect biu din im bin i tnh tin c xc nh bi :
v = H.u =

1
0
0
0

0
1
0
0

0
0
1
0

a
b
c
1

x
y
z
w

x+aw
y+bw
z+cw
w

x/w+a
y/w+b
z/w+c
1

Nh vy bn cht ca php bin i tnh tin l php cng vect gia vect biu din
im cn chuyn i v vect dn.
r
r
r
r
u = 2i + 3j + 2k
V d :
r
r
r
r
h = 4i - 3j + 7k
Th
1 0 0 4
2
2+4
6
v = Hu =
0 1 0 -3 . 3 =
3-3
=
0
0 0 1 7
2
2+7
9
0 0 0 1
1
1
1
v vit l :

v = Trans(a,b,c) u
z

v
2

-3
4

u
3

6
x

Hnh 2..4: Php bin i tnh tin trong khng gian

2.3.2. Php quay (Rotation) quanh cc trc to :
Gi s ta cn quay mt im hoc mt vt th xung quanh trc to no vi gc
quay o, ta ln lt c cc ma trn chuyn i nh sau :
Rot(x, ) =
o

Rot(y, ) =
o

TS. Phm ng Phc

1
0
0
0

0
cos
sin
0

0
-sin
cos
0

0
0
0
1

cos
0
-sin
0

0
1
0
0

sin
0
cos
0

0
0
0
1

(2.3)

(2.4)

14

Robot cng nghip

cos
Rot(z, ) =
sin
0
0
V d : Cho im U biu din bi
(hnh 2.5). Ta c
0 -1
o
v= Rot(z, 90 )u =
1
0
0
0
0
0
o

0
0
-sin
0
0
(2.5)
cos
0
1
0
0 r r0 r 1
r
u = 7i + 3j + 2k quay xung quanh z mt gc = 90o
0
0
1
0

0
0
0
1

7
3
2
1

-3
7
2
1

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

0
0
-1
0

w = Rot(y, 90 )v =

0
1
0
0

1
0
0
0

0
0
0
1

-3
7
2
1

2
7
3
1

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

0
0
-1
0

v =

0
1
0
0

1
0
0
0

0
0
0
1

7
3
2
1

2
3
-7
1

= Rot(y, 90o).u

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

0
1
0
0

w =

-1
0
0
0

0
0
1
0

0
0
0
1

2
3
-7
1

-3
2
-7
1

= Rot(z, 90o).Rot(y,900)u

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

z

z
v
y

y
w

w
u

u
x

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

TS. Phm ng Phc

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

15

Robot cng nghip

2.3.3. Php quay tng qut :

Trong mc trn, ta va nghin cu cc php quay c bn xung quanh cc trc to
x,y,z ca h to chun O(x,y,z). Trong phn ny, ta nghin cu php quay quanh mt vect
k bt k mt gc . Rng buc duy nht l vect k phi trng vi gc ca mt h to xc
nh trc.
Ta hy kho st mt h to C, gn ln im tc ng cui (bn tay) ca robot, h C
c biu din bi :
n (Cz)

C=

Cx
nx
ny
nz
0

Cy
Ox
Oy
Oz
0

Cz
az
ay
az
0

Co
0
0
0
1

Co
O(Cy)

a (Cx)

Hnh 2.7 : H to gn trn

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

Chng hn :

Lc , php quay Rot(k,) s tr thnh php quay Rot(Cz,).

Nu ta c T m t trong h gc trong k l vect bt k, th ta c X m t trong h C
vi k l mt trong cc vect n v. T iu kin bin i thun nht, T v X c lin h :
T = C.X
hay
X = C -1.T
Lc cc php quay di y l ng nht :

hay l
Vy

Rot(k,) = Rot(Cz,)
Rot(k,).T = C.Rot(z,).X = C.Rot(z,).C -1.T
Rot(k,) = C.Rot(z,).C -1

(2.6)

Trong Rot(z,) l php quay c bn quanh trc z mt gc , c th s dng cng

thc (2.5) nh trnh by.
C-1 l ma trn nghch o ca ma trn C. Ta c :
C-1 =

TS. Phm ng Phc

nx
Ox
ax
0

ny
Oy
ay
0

nz
Oz
az
0

0
0
0
1

16

Robot cng nghip

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

Rot(k,) =

nx
ny
nz
0

Ox
Oy
Oz
0

ax
ay
az
0

0
0
0
1

cos
sin
0
0

-sin
cos
0
0

0
0
1
0

0
0
0
1

nx
Ox
ax
0

ny
Oy
ay
0

nz
Oz
az
0

0
0
0
1

Nhn 3 ma trn ny vi nhau ta c :

nxnxcos - nxOxsin + nxOxsin + OxOxcos + axax
nxnycos - nyOxsin + nxOysin + OxOycos + ayax
nxnzcos - nzOxsin + nxOzsin + OxOzcos + azax
0

Rot(k,) =

nxnycos - nxOysin + nyOxsin + OxOycos + axay

nynycos - nyOysin + nyOysin + OyOycos + ayay
nznycos - nzOysin + nyOzsin + OzOycos + azay
0
nxnzcos - nxOzsin + nzOxsin + OxOzcos + axaz
nynzcos - nyOzsin + nzOysin + OyOzcos + ayaz
nznzcos - nzOzsin + nzOzsin + OzOzcos + azaz
0

0
0
0
1
(2.7)

n gin cch biu th ma trn, ta xt cc mi quan h sau :

- Tch v hng ca bt k hng hay ct no ca C vi bt k hng hay ct no khc
u bng 0 v cc vect l trc giao.
- Tch v hng ca bt k hng hay ct no ca C vi chnh n u bng 1 v l vect
n v.
r
r r
- Vect n v z bng tch vect ca x v y, hay l : a = n x O
Trong :

ax = nyOz - nzOy
ay = nxOz - nzOx
ax = nxOy - nyOx

Khi cho k trng vi mt trong s cc vect n v ca C ta chn :

kz = ax ; ky = ay ; kz = az
Ta k hiu Vers = 1 - cos (Versin ).
Biu thc (2.6) c rt gn thnh :
Rot(k,) =

kxkxvers+cos
kxkyvers+kzsin
kxkzvers+kysin
0

kykxvers-kzsin
kykyvers+cos
kykzvers+kzsin
0

kzkxvers+kysin
kzkyvers-kxsin
kzkzvers+cos
0

0
0
0
1

(2.8)

y l biu thc ca php quay tng qut quanh mt vect bt k k. T php quay tng
qut c th suy ra cc php quay c bn quanh cc trc to .

TS. Phm ng Phc

17

Robot cng nghip

2.3.4. Bi ton ngc : tm gc quay v trc quay tng ng :

Trn y ta nghin cu cc bi ton thun, ngha l ch nh trc quay v gc quay
trc- xem xt kt qu bin i theo cc php quay ch nh.
Ngc li vi bi ton trn, gi s ta bit kt qu ca mt php bin i no , ta
phi i tm trc quay k v gc quay tng ng. Gi s kt qu ca php bin i thun nht
R=Rot(k, ), xc nh bi :
nx Ox ax 0
R = ny Oy ay 0
nz Oz az 0
0
0
0
1
Ta cn xc nh trc quay k v gc quay . Ta bit Rot(k, ) c nh ngha bi ma
trn (2.6) , nn :

nx
ny
nz
0

Ox
Oy
Oz
0

ax
ay
az
0

0
0
0
1

kxkxvers+cos

= kxkyvers+kzsin
kxkzvers+kysin
0

kykxvers-kzsin
kykyvers+cos
kykzvers+kzsin
0

kzkxvers+kysin
kzkyvers-kxsin
kzkzvers+cos
0

0
0
0
1

Bc 1 : Xc nh gc quay .
* Cng ng cho ca hai ma trn hai v ta c :
nx + Oy + az + 1 = k x2 vers + cos + k y2 vers + cos + k z2 vers + cos + 1

(2.9)

= (1 - coss)( k x2 + k y2 + k z2 ) + 3cos + 1
= 1 - cos + 3cos +1
= 2(1+ cos)
cos
= (nx + Oy + az - 1)/2
* Tnh hiu cc phn t tng ng ca hai ma trn, chng hn :
Oz- ay = 2kxsin
ax - nz = 2kysin
ny - Ox = 2kzsin

(2.10)

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

(Oz- ay)2 + (ax - nz)2 + (ny - Ox)2 = 4 sin2

sin =

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

Vi 0 1800 :
tg =

(O z - a y ) 2 + (a x - n z ) 2 + (n y - O x ) 2

(n x + O y + a z - 1)
V trc k c nh ngha bi :
Oz a y
ny Oz
ax nz
kx =
; ky =
; kx =
(2.11)
2sin
2sin
2sin
rng vi cc cng thc (2.8) :
0
- Nu = 00 th kx, ky, kz c dng . Lc ny phi chun ho k sao cho k = 1
0

TS. Phm ng Phc

18

Robot cng nghip

- Nu = 1800 th kx, ky, kz c dng

a0
. Lc ny k khng xc nh c, ta phi
0

dng cch tnh khc cho trng hp ny :

Xt cc phn t tng ng ca hai ma trn (2.9) :
nx = k x2 vers+cos
Oy = k y2 vers+cos
az = k z2 vers+cos
T y ta suy ra :
n x cos
n x cos
kx =
=
1- cos
vers
O y cos
O y cos
ky =
=
vers
1- cos
a z cos
a z cos
kz =
=
1- cos
vers
0
0
Trong khong 90 180 sin lun lun dng
Da vo h phng trnh (2.10) ta thy kx, ky, kz lun c cng du vi v tri. Ta dng
hm Sgn(x) biu din quan h cng du vi x, nh vy :
k x = Sgn(O z a y )

k y = Sgn(a x - n z )

n x cos
1- cos
O y cos

(2.12)
1- cos
a cos
k z = Sgn(n y O x ) z
1- cos
H phng trnh (2.12) ch dng xc nh xem trong cc kx, ky, kz thnh phn no c
gi tr ln nht. Cc thnh phn cn li nn tnh theo thnh phn c gi tr ln nht xc nh
k c thun tin. Lc dng phng php cng cc cp cn li ca cc phn t i xng
qua ng cho ma trn chuyn i (2.9) :
ny + Ox = 2kxkyvers = 2kxky(1 - cos)
Oz + ay = 2kykzvers = 2kykz(1 - cos)

(2.13)

ax + nz = 2kzkxvers = 2kzkx(1 - cos)

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

19

Robot cng nghip

1
(O z - a y ) 2 + (a x - n z ) 2 + (n y - O x ) 2
2
1
3
(1 - 0) 2 + (1 - 0) 2 + (1 - 0) 2
=
=
2
2
0
tg = 3 v = 120
sin =

Theo (2.12), ta c :

0 +1/ 2
1
=
1+1/ 2
3

k x = ky = kz = +

Vy : R = Rot(y,900).Rot(z,900) = Rot(k, 1200); vi :

r
1 r 1 r 1 r
k
j+
i+
k=
3
3
3
z
1/ 3
k
1200
1/ 3

y
x
1/ 3
Hnh 2.8 : Tm gc quay v trc quay tng ng

2.3.5. Php quay Euler :

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

z z

yy
y

Hnh 2.9 : Php quay Euler

Ta biu din php quay Euler bng cch nhn ba ma trn quay vi nhau :
Euler (,,) = Rot(z, ) Rot(y, ) Rot(z, )
(2.14)
TS. Phm ng Phc

20

Robot cng nghip

Ni chung, kt qu ca php quay ph thuc cht ch vo th t quay, tuy nhin ,

php quay Euler, nu thc hin theo th t ngc li, ngha l quay gc quanh z ri tip n
quay gc quanh y v cui cng quay gc quanh z cng a n kt qu tng t (Xt
trong cng h qui chiu).
cos -sin 0 0
Cos 0 sin 0
0
1
0
0
Euler (,,) = Rot(z, )
sin
cos 0 0
0
0
1 0
-sin 0 Cos 0
0
0
0
1
0
0
0 1

cos
sin
0
0

-sin
cos
0
0

0
0
1
0

Coscos
sin
-sin cos
0

0
0
0
1

cosCoscos - sinsin
= sinCoscos + cossin
-sin cos
0

-Cos sin
cos
sin sin
0

-cosCossin - sincos
-sinCossin + coscos
sin sin
0

sin
0
Cos
0

0
0
0
1

cossin
sinsin
cos
0

0
0
0
1

(2.15)
2.3.6. Php quay Roll-Pitch-Yaw :
Mt php quay nh hng khc cng thng c s dng l php quay Roll-Pitch v
Yaw.
Ta tng tng, gn h to xyz ln
thn mt con tu. Dc theo thn tu l trc z,
Roll l chuyn ng lc ca thn tu, tng
ng vi vic quay thn tu mt gc quanh
trc z. Pitch l s bng bnh, tng ng vi
quay mt gc xung quanh trc y v Yaw l
s lch hng, tng ng vi php quay mt
gc xung quanh trc x (Hnh 2.10)

x Yaw

Roll

Pitch

Cc php quay p dng cho khu chp

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

Roll,

Pitch,
y
x
Yaw,

Hnh 2.11 : Cc gc quay Roll-Pitch v Yaw ca bn tay Robot.

ngha l, quay mt gc quanh trc x, tip theo l quay mt gc quanh trc y v sau
quay mt gc quanh truc z.
TS. Phm ng Phc

21

Robot cng nghip

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

RPY(,,)=Rot(z,)

cos
sin
0
0

-sin
cos
0
0

coscos
sincos
-sin
0

0
0
1
0

0
1
0
0

0
0
0
1

sin
0
cos
0

0
0
0
1

cos
0
-sin
0

sinsin
cos
cossin
0

cossinsin - sincos
sinsinsin +coscos
cos sin
0

1
0
0
0

0
cos
sin
0

0
-sin
cos
0

sincos
-sin
cos cos
0

cossincos + sinsin
sinsincos - cossin
cos cos
0

0
0
0
1
0
0
0
1
0
0
0
1
(2.17)

2.4. Bin i h to v mi quan h gia cc h to bin i :

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

OT =

1
0
0
0

0
1
0
0

0
0
1
0

4
-3
7
1

0
0
0
1

4
-3
7
1

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

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

zT

7
OT
yT
xT

-3

Hnh 2.12 : Php bin i tnh tin h to

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

TS. Phm ng Phc

22

Robot cng nghip

Nu ta tip tc thc hin cc php bin i quay :

Rot(y,90o)Rot(z,90o).OT
ta s c mt h to hon ton mi, c th ti gc to mi (4,-3,7) khi cho h OT quay
quanh z mt gc 900 (chiu quay dng qui c l ngc chiu kim ng h), ta c :
zT

z'T
y'T

90o
OT

Rot(z,900)

yT

OT

x'T

xT

Ta tip tc quay h OT quanh truc y (trc y ca h to gc ) mt gc 900, Ta c :

z'T

y''T
y'T
90o

OT

Rot(y,900)

x'T

z"T

OT
x''T

V d trn y ta chn H to c s lm h qui chiu v th t thc hin cc

php bin i l t Phi sang Tri. Nu thc hin cc php bin i theo th t ngc li t
Tri sang Phi th h qui chiu c chn l cc h to trung gian. Xt li v d trn :
Rot(y,90o)Rot(z,90o).OT
zT
90o

OT

Rot(y,90o)

yT

y'T

O'T
z'T

xT

x'T

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

90o O'T
z'T

Rot(z',90o)

z"T

O''T
x''T

x'T

Nh vy kt qu ca hai phng php quay l ging nhau, nhng v ngha vt l th

khc nhau.
2.4.2. Quan h gia cc h to bin i :
Gi s ta c 3 h to A, B, C; H B c quan h vi h A qua php bin i A T/ B v
h C c quan h vi h B qua php bin i B T/c . Ta c im P trong h C k hiu PC, ta tm
mi quan h ca im P trong h A, tc l tm PA (Hnh 2.13) :

TS. Phm ng Phc

23

Robot cng nghip

zC

zB

pC

zA

xC
pA

C
B

A
yA

xA

xB

yC

yB

Hnh 2.13 : Quan h gia cc h to bin i.

Chng ta c th bin i pC thnh pB nh sau :

pB = B T/c pC,
Sau bin i pB thnh pA nh sau :
pA = A T/ B pB,

(2.18)
(2.19)

Kt hp (2.18) v (2.19) ta c :
p A = A TB BTC p c

(2.20)

Qua v d trn ta thy c th m t mi quan h gia h to gn trn im tc ng

cui vi h ta c bn, thng qua mi quan h ca cc h to trung gian gn trn cc
khu ca robot, bng ma trn T nh hnh 2.14.
z
O2

O3

Bn tay

O1
T4
O0

O4
y

Hnh 2.14 : H to c bn (base) v cc h to trung gian ca Robot.

2.5. M t mt vt th :
Cc vt th l i tng lm vic ca robot rt a dng v phong ph, tuy nhin c th
da vo nhng c im hnh hc m t chng. Ta c th chia hnh dng vt th thnh 3
nhm chnh sau :
Nhm vt th trn xoay (Rotative)
Nhm vt th c gc cnh (Prismatic)
Nhm vt th c cu trc hn hp (Kombination)
Nhm vt th trn xoay c cc gi tr c trng l to tm v bn knh mt cong.
Nhm vt th c gc cnh c trng bng to ca cc im gii hn.
Nhm cn li c cc gi tr c trng hn hp.
Tuy nhin, i vi hot ng cm nm i tng v qu trnh vn ng ca robot vic
m t vt th cn phi gn lin vi cc php bin i thun nht. Ta xt v d sau y : Cho
mt vt hnh lng tr t trong h to chun O(xyz) nh hnh 2.15.
TS. Phm ng Phc

24

Robot cng nghip

Ta thc hin cc php bin i sau :

z
H = Trans(4,0,0)Rot(y,900)Rot(z,900)
-1,0,2,1

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

1,0,2,1

-1,0,0,1

-1,4,0,1
y

1
0
0
1

-1
0
0
1

-1
0
2
1

1
0
2
1

1
4
0
1

-1
4
0
1

1,0,0,1

1,4,0,1

x
Hnh 2.15 : M t vt th

Sau khi thc hin cc php bin i :

- Quay vt th quanh trc z mt gc 900 (Hnh 2.16),
- Cho vt th quay quanh trc y mt gc 900 (Hnh 2.17),
- Tip tc tnh tin vt th dc theo trc x mt on bng 4 n v (hnh 2.18) ta xc
nh c ma trn to cc im gii hn ca vt th v tr c bin i nh
sau (cc php quay chn h qui chiu l h to gc) :

0
1
0
0

H=

0
0
1
0

1
0
0
0

4
0
0
1

4
1
0
1

1
0
0
1

4
-1
0
1

-1
0
0
1

6
-1
0
1

-1
0
2
1

1
0
2
1

6
1
0
1

4
1
4
1

1
4
0
1

-1
4
0
1

4
1
4
1

x
Hnh 2.16 : Rot (z,900)

TS. Phm ng Phc

Hnh 2.17: Rot (y,900) Rot (z,900)

25

Robot cng nghip

y
O

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

Hnh 2.18: V tr vt th sau khi bin i

2.6. Kt lun :
Cc php bin i thun nht dng miu t v tr v hng ca cc h to trong
khng gian. Nu mt h to c gn lin vi i tng th v tr v hng ca chnh i
tng cng c m t. Khi m t i tng A trong mi quan h vi i tng B bng cc
php bin i thun nht th ta cng c th da vo m t ngc li mi quan h ca B i
vi i tng A.
Mt chuyn v c th l kt qu lin tip ca nhiu php bin i quay v tnh tin. Tuy
nhin ta cn lu n th t ca cc php bin i, nu thay i th t thc hin c th dn
n cc kt qu khc nhau.

Bi tp chng II :
Bi 1 : Cho im A biu din bi vect im v=[ 2 4 1 1 ]T. Tnh tin im A theo vect dn h
= [ 1 2 1 1 ]T, sau tip tc quay im bin i quanh trc x mt gc 900. Xc nh vect
biu din im A sau hai php bin i.
Bi 2 : Vit ma trn bin i thun nht biu din cc php bin i sau :
H = Trans(3,7,9)Rot(x,-900)Rot(z,900)
Bi 3 : Cho ma trn bin i thun nht A, tm ma trn nghch o A-1 v kim chng.
A =

TS. Phm ng Phc

0
0
-1
0

1
0
0
0

0 -1
-1 2
0 0
0 1

26

Robot cng nghip

Bi 4 : Hnh v 2-19 m t h to {B} c

quay i mt gc 300 xung quanh trc zA, tnh tin
dc theo trc xA 4 n v v tnh tin dc theo yA
3 n v.
(a) M t mi qua h ca {B} i vi {A} : ATB ?
(b) Tm mi quan h ngc li BTA ?

{B}
{A}

yB

xB

yA

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

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

1
0
0
0

T6 =

0
0
-1
0

0
1
0
0

0
5
3
1

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

U

TP =

0
0
-1
0

1
0
0
0

0 -1
-1 2
0 0
0 1

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

U

TR =

1
0
0
0

0
1
0
0

0
0
1
0

1
5
9
1

Chng ta mun t bn tay ca robot ln vt th, l lm cho h ta gn trn bn tay

trng vi h to ca vt th. Tm php bin i RTH (biu din mi quan h gia bn tay v
h to gc ca robot) thc hin iu ni trn.

TS. Phm ng Phc

27

Robot cng nghip

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

(3.1)

T6 m t mi quan h v hng v v tr ca khu chp hnh cui i vi h to gc.

Mt robot 6 khu c th c 6 bc t do v c th c nh v tr v nh hng trong trng
vn ng ca n (range of motion). Ba bc t do xc nh v tr thun tu v ba bc t do khc
xc nh hng mong mun. T6 s l ma trn trnh by c hng v v tr ca robot. Hnh 3.1
m t quan h vi bn tay my. Ta t gc to ca h m t ti im gia ca cc ngn
tay. Gc to ny c m t bi vect p (xc nh v tr ca bn tay). Ba vect n v m t
hng ca bn tay c xc nh nh sau :

n
a o

Hnh 3.1 : Cc vect nh v tr v nh hng ca bn tay my

TS. Phm ng Phc

28

Robot cng nghip

Vect c hng m theo bn tay s tip cn n i tng, gi l vect a

(approach).
Vect c hng m theo cc ngn tay ca bn tay nm vo nhau khi cm nm
i tng, gi l vect o (Occupation).
Vect cui cng l vect php tuyn n (normal), do vy ta c :

r r r
n=oxa
Chuyn v T6 nh vy s bao gm cc phn t :
nx
ny
nz
0

T6 =

Ox
Oy
Oz
0

ax
ay
az
0

px
py
pz
1

(3.2)

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

Ma trn nh hng R

T6 =

Vect v tr p

(3.3)

Ma trn R c kch thc 3x3, l ma trn trc giao biu din hng ca bn kp (khu
chp hnh cui) i vi h to c bn. Vic xc nh hng ca khu chp hnh cui cn
c th thc hin theo php quay Euler hay php quay Roll, Pitch, Yaw.

Vect im p c kch thc 3x1, biu din mi quan h ta v tr ca ca gc h

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

Khp n+1
Khu n
n
a

Hnh 3.5 : Chiu di v gc xon ca 1 khu.

Thng thng, ngi ta gi an l chiu di v n l gc xon ca khu (Hnh 3.5). Ph
bin l hai khu lin kt vi nhau chnh trc ca khp (Hnh 3.6).

TS. Phm ng Phc

29

Robot cng nghip

Khp n

Khp n-1

Khp n+1

n-1

n+1
Khu n
Khu n+1

Khu n-1

Khu n-2

an
zn-1

dn

zn
xn

On

xn-1

n
Hnh 3.6 : Cc thng s ca khu : , d, a v .
Mi trc s c hai php tuyn vi n, mi php tuyn dng cho mi khu (trc v sau
mt khp). V tr tng i ca hai khu lin kt nh th c xc nh bi dn l khong cch
gia cc php tuyn o dc theo trc khp n v n l gc gia cc php tuyn o trong mt
phng vung gc vi trc.
dn v n thng c gi l khong cch v gc gia cc khu.
m t mi quan h gia cc khu ta gn vo mi khu mt h to . Nguyn
tc chung gn h ta ln cc khu nh sau :
+ Gc ca h to gn ln khu th n t ti giao im ca php tuyn an vi trc
khp th n+1. Trng hp hai trc khp ct nhau, gc to s t ti chnh im ct . Nu
cc trc khp song song vi nhau, gc to c chn trn trc khp ca khu k tip, ti
im thch hp.
+ Trc z ca h to gn ln khu th n t dc theo trc khp th n+1.
+ Trc x thng c t dc theo php tuyn chung v hng t khp n n n+1.
r r
Trong trng hp cc trc khp ct nhau th trc x chn theo tch vect z n x z n-1 .
Trng hp khp quay th n l cc bin khp, trong trng hp khp tnh tin th dn
l bin khp v an bng 0.
Cc thng s an, n, dn v n c gi l b thng s DH.
V d 1 : Xt mt tay my c hai khu phng nh hnh 3.7 :

y2
x2
O2

y1
y0

z0

z2
x1 a
2

a1
1

O0

z1

O1

x0

Hnh 3.7 : Tay my c hai khu phng (v tr bt k).

TS. Phm ng Phc

30

Robot cng nghip

Ta gn cc h to ln cc khu nh hnh v : trc z0, z1 v z2 vung gc vi t giy.

H to c s l O0x0y0z0, chiu ca x0 hng t O0 n O1. Sau khi thit lp h to c s,
H to o1x1y1z1 c hng nh hnh v, O1 t ti tm trc khp 2. H to O2x2y2x2 c gc
O2 t im cui ca khu 2.
Bng thng s Denavit-Hartenbert ca tay my ny nh sau :
Khu
1
2

i
1 *
2 *

i
0
0

ai
a1
a2

di
0
0

Trong i l cc bin khp (dng du * k hiu cc bin khp).

V d 2 : Xem s robot SCARA c 4 khu nh hnh 3.8 :
y l robot c cu hnh kiu RRTR, bn tay c chuyn ng xoay xung quanh trc
ng. H to gn ln cc khu nh hnh v.
a2
z0
z1
1
2
x2
x
0
O
0

O1

O2

x1

z2 x
3

d3

a1

O3
O4
4

d4
x

z3 , z4
Hnh 3.8 : Robot SCARA v cc h to (v tr ban u).
i vi tay my ny cc trc khp u song song nhau, tin li tt c cc gc to
t ti tm cc trc khp. Trc x0 nm trong mt phng t giy. Cc h to khc nh hnh
v. Bng thng s DH ca robot SCARA nh sau :
Khu
1
2
3
4

i
1 *
2 *
0
4 *

i
0
1800
0
0

ai
a1
a2
0
0

di
0
0
d3*
d4

* : Cc bin khp.
3.3. c trng ca cc ma trn A :
Trn c s cc h to n nh cho tt c cc khu lin kt ca robot, ta c th
thit lp mi quan h gia cc h to ni tip nhau (n-1), (n) bi cc php quay v tnh tin
sau y :
Quay quanh zn-1 mt gc n
Tnh tin dc theo zn-1 mt khong dn
Tnh tin dc theo xn-1 = xn mt on an
Quay quanh xn mt gc xon n

TS. Phm ng Phc

31

Robot cng nghip

Bn php bin i thun nht ny th hin quan h ca h to thuc khu th n so

vi h to thuc khu th n-1 v tch ca chng c gi l ma trn A :
An = Rot(z,) Trans(0,0,d) Trans(a,0,0) Rot(x,)
An =

An =

0
0
1
0

cos
sin
0
0

-sin
cos
0
0

cos
sin
0
0

-sin cos
cos cos
sin
0

0
0
0
1

1
0
0
0

0
1
0
0

sin sin
-cos sin
cos
0

0
0
1
0

a
0
d
1

1
0
0
0

(3.4)

0
cos
sin
0

0
-sin
cos
0

a cos
a sin
d
1

0
0
0
1

(3.5)

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

An =

1
0
0
0

0
cos
sin
0

0
- sin
cos
0

0
0
d
1

(3.6)

i vi mt khu i theo mt khp quay th d, a v l hng s. Nh vy ma trn A

ca khp quay l mt hm s ca bin khp .
i vi mt khu i theo mt khp tnh tin th , l hng s. Ma trn A ca khp
tnh tin l mt hm s ca bin s d.
Nu cc bin s c xc nh th gi tr ca cc ma trn A theo cng c xc
nh.
3.4. Xc nh T6 theo cc ma trn An :
Ta bit :

T6 = A1A2A3A4A5A6

Trong T6 c miu t trong h to gc (h to gn vi khu c bn c nh

ca robot). Nu m t T6 theo cc h to trung gian th n-1 th :
6

n 1

Trong trng hp tng qut, khi

xt quan h ca robot vi cc thit b
khc, nu h to c bn ca robot c
lin h vi mt h to no bi php
bin i Z, Khu chp hnh cui li c
gn mt cng c, c quan h vi vt th
bi php bin i E (hnh 3.9) th v tr v
hng ca im cui ca cng c, kho
st h to tham chiu m t bi X s
c xc nh bi :
X= Z T6E

TS. Phm ng Phc

i=n

Ai

OR
Z
T6

E
A

Hnh 3.9 : Vt th v Robot

32

Robot cng nghip

Quan h ny c th hin trn ton sau :

Z O0 A1

A2

A3

A4

A5

OR

5
4
3
2

E A X

T6

OR

T6

T6

T6

T6

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

V tr ban u l v tr m cc bin nhn gi tr ban u, thng bng 0.

TS. Phm ng Phc

33

Robot cng nghip

V d sau y trnh by chi tit ca cc bc khi thit lp h phng trnh ng hc

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

Hnh 3.11 : Robot RRT

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

y1

x1 z2

z1

1
d1

z0

y0

x0

O0

Hnh 3.12 : Gn cc h to O0 v O1
z3

Tip tc chn gc ta O2 t trng

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

d3

O1 , O2

O3

x3

d3
d3

y1 z2
O1 O2

x1 x2

z1

1
d1

z0
O0

y0

x0

Hnh 3.13 : H to
gn ln cc khu

34

Robot cng nghip

Nh vy vic gn cc h to ln cc khu ca robot hon thnh. Thng qua cc

phn tch trn y, ta c th xc nh c cc thng s DH ca robot.
2. Lp bng thng s DH :
i
1*
i*
0

Khu
1
2
3

i
90
-90
0

ai
0
0
0

di
d1
0
d3 *

3. Xc nh cc ma trn A :
Ma trn An c dng :
cos -sin cos sin sin
An = sin
cos cos -cos sin
0
sin
cos
0
0
0
Vi qui c vit tt : C1 = cos1 ; S1 = sin1 ; C2 = cos2 . . .
A1 =

C1
S1
0
0

0
0
1
0

S1
-C1
0
0

0
0
d1
1

A2 =

C2
S2
0
0

0
0
-1
0

-S2
C2
0
0

0
0
0
1

A3 =

1
0
0
0

0
1
0
0

0
0
1
0

0
0
d3
1

0
0
d
1

4. Tnh cc ma trn bin i thun nht T :

+ Ma trn 2T3 = A3
+ Ma trn 1T3 = A2. 2T3
1

T3 =
+

C2 0 -S2 0
S2 0 C2 0
0 -1 0 d2
0
0
0
1
Ma trn T3 = A1 . 1T3
T3 =

TS. Phm ng Phc

C1
S1
0
0

0
0
1
0

1
0
0
0
S1
-C1
0
0

0
1
0
0

0
0
1
0
0
0
d1
1

0
0
d3
1
C2
S2
0
0

C2
S2
0
0
0
0
-1
0

0
0
-1
0
-S2
C2
0
0

-S2
C2
0
0
-S2*d3
C2*d3
0
1

-S2*d3
C2*d3
0
1

35

Robot cng nghip

C1C2
S1d2
S2
0

-S1
C1
0
0

-C1S2
-S1S2
C2
0

-C1S2d3
-S1S2d3
C2d3 + d1
1

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

nx = C1C2;
Ox = -S1;
ax = -C1S2;
px = -C1S2d3

ny = S1C2;
Oy = C1;
ay = -S1S2;
py = -S1S2d3

nz = S2
Oz = 0;
az = C2;
pz = C2d3 + d1;

(Ta c th s b kim tra kt qu tnh ton bng cch da vo to v tr px,py, pz

tnh so vi cch tnh hnh hc trn hnh v).
3.9. H phng trnh ng hc ca robot STANFORD :
Stanford l mt robot c 6 khu vi cu hnh RRT.RRR (Khu th 3 chuyn ng tnh
tin, nm khu cn li chuyn ng quay). Kt cu ca robot Stanford nh hnh 3.14 :

Hnh 3.14 : Robot Stanford

TS. Phm ng Phc

36

Robot cng nghip

Trn hnh 3.15 trnh by m hnh

ca robot Stanford vi vic gn cc h to
ln tng khu. n gin trong khi
vit cc phng trnh ng hc ca robot,
ta qui c cch vit tt cc hm lng gic
nh sau :

O3,O4,O5,O6

z3,z5,z6

z4

xi
d2

C1 = cos1;
S1 = sin1;
C12 = cos(1+2);
S12 = sin(1+2)
S234 = sin (2+3+4) ... .

d3

z0

z2

O0,O1 z1

x0

H to gn ln cc khu ca robot nh
hnh 3.15. (Khu cui c chiu di v
khong cch bng khng, c th gn cc
loi cng c khc nhau nn chn O6O5).

O2

x1

Hnh 3.15 : H to ca Robot Stanford

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

Khu
i
i
0
-90
1
1 *
900
2
2 *
3
0
0
-900
4
4 *
900
5
5 *
0
6
6 *
(* : Cc bin khp).

ai
0
0
0
0
0
0

di
0
d2
d3 *
0
0
0

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

A1=

A3=

A5=

C1
S1
0
0

0
0
-1
0

-S1
C1
0
0

1
0
0
0

0
1
0
0

0
0
1
0

0
0
d3
1

C5
S5
0
0

0
0
1
0

S5
-C5
0
0

0
0
0
1

0
0
0
1

A2=

A4=

A6=

C2
S2
0
0

0
0
1
0

S2
-C2
0
0

0
0
d2
1

C4
S4
0
0

0
0
-1
0

-S4
C4
0
0

0
0
0
1

C6
S6
0
0

-S6
C6
0
0

0
0
1
0

0
0
0
1

Tch ca cc ma trn chuyn v A i vi robot Stanford c bt u khu 6 v

chuyn dn v gc; theo th t ny ta c :

TS. Phm ng Phc

37

Robot cng nghip

T6 =

C6
S6
0
0

-S6
C6
0
0

0
0
1
0

C5C6
S5C6
S6
0

T6 = A5A6 =

0
0
0
1
-C5S6
-S5S6
C6
0

S5
-C5
0
0

C4C5C6 - S4S6
S4C5C6 + C4S6
-S5C6
0

T63 = A4A5A6 =

-C4C5S6-S4C6
-S4C5S6 + C4C6
S5S6
0

C4C5C6-S4S6
S4C5C + C4S6
-S5C6
0

T6 = A3A4A5A6 =

T6 =A2 A3A4A5A6 =

0
0
0
1
C4S5
S4S5
C5
0

-C4C5S6 - S4C6
-S4C5S6 + C4C6
S5S6
0

C2(C4C5C6 - S4S6) - S2S5C6

S2(C4C5C6 - S4S6) + C2S5C6
S4C5C6 + C4S6
0

0
0
0
1

C4S5
S4S5
C5
0

0
0
d3
1

-C2(C4C5S6-S4C6)+S2S5S6
-S2(C4C5S6+S4C6)-C2S5S6
-S4C5S6+C4C6
0
C2C4S5 + S2C5
S2C4S5 - C2C5
S4S5
0

S2d3
-C2d3
d2
1

Cui cng :
T6 =

nx
ny
nz
0

Ox
Oy
Oz
0

ax
ay
az
0

px
py
pz
1

A1T61

tnh T6, ta phi nhn A1 vi T61 sau cn bng cc phn t ca ma trn T6 hai v
ta c mt h thng cc phng trnh sau :
nx = C1[C2(C4C5C6 - S4S6) - S2S5C6] - S1(S4C5C6 + C4S6)
ny = S1[C2(C4C5C6 - S4S6) - S2S5C6] + C1(S4C5C6 + C4S6)
nz = -S2(C4C5C6 - S4S6) + C2S5C6
Ox = C1[-C2(C4C5S6 + S4C6) + S2S5S6] - S1(-S4C5S6 + C4C6)
Oy = S1[-C2(C4C5S6 + S4C6) + S2S5S6] + C1(-S4C5C6 + C4C6)
Oz = S2(C4C5S6 + S4C6) + C2S5S6
aX = C1(C2C4S5 + S2C5) - S1S4S5
ay = S1(C2C4S5 + S2C5) + C1S4S5
az = -S2C4S5 + C2C5
px = C1S2d3 - S1d2
py = S1S2d3 + C1d2
pz = C2d3

TS. Phm ng Phc

38

Robot cng nghip

Nu ta bit c cc gi tr ca bin khp, th v tr v hng ca bn tay robot s tm

c bng cch xc nh cc gi tr cc phn t ca T6 theo cc phng trnh trn.
Cc phng trnh trn gi l h phng trnh ng hc thun ca robot Stanford.
3.10. H phng trnh ng hc ca robot ELBOW :
hiu r hn v cch thit lp h phng trnh ng hc ca robot, ta xt thm
trng hp robot Elbow.
Khu 2
Khu 3
Khu 4

Khu 5

Khu 1
Khu 6
Hnh 1.16 : Robot Elbow

z0

z1

a2

O0,O1

z4

a3

O2

a4
O3

z2

O2,O5,O6

xi

z3

z 5, z 6
6

a5 = a6 = 0

Hnh 1.17 : V tr ban u ca robot Elbow v cc h to

B thng s DH ca robot Elbow
Khu
i *
i
0
1
90
1
2
0
2
3
0
3
4
-900
4
5
900
5
6
0
6
(* : cc bin khp )

ai
0
a2
a3
a4
0
0

di
0
0
0
0
0
0

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

A1=

C1
S1
0
0

TS. Phm ng Phc

0
0
1
0

S1
-C1
0
0

0
0
0
1

A2=

C2
S2
0
0

-S2
C2
0
0

0
0
1
0

C 2 a2
S 2 a2
0
1

39

Robot cng nghip

A3=

C3
S3
0
0

-S3
C3
0
0

A5=

C5
S5
0
0

0
0
1
0

0
0
1
0
S5
-C5
0
0

C 3 a3
S 3 a3
0
1
0
0
0
1

A4=

C4
S4
0
0

A6=

C6
S6
0
0

0
0
-1
0

-S4
C4
0
0

-S6
C6
0
0

C 4 a4
S 4 a4
0
1
0
0
1
0

0
0
0
1

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

T65 =

C6
S6
0
0

-S6
C6
0
0

T6 = A5A6 =

T63 = A4A5A6 =

T6 = A3A4A5A6 =

0
0
1
0

C5C6
S5C6
S6
0

0
0
0
1
-C5S6
-S5S6
C6
0

S5
-C5
0
0

C4C5C6 - S4S6
S4C5C6+C4S6
-S5C6
0
C34C5C6 - S34S6
S34C5C6+C34S6
-S5C6
0

0
0
0
1
-C4C5S6-S4C6
-S4C5S6+C4C6
S5S6
0

-C34C5C6 - S34C6
-S34C5S6+C34C6
S5S6
0

C4S5
S4S5
C5
0
C34S5
S34S5
C5
0

C4a4
S4a4
0
1
C34a4+C3a3
S34a4+S3a3
0
1

T61 =A2 A3A4A5A6 =

C234C5C6 - S234S6
S234C5C6 + C234S6
-S5C6
0

-C234C5S6 - S234C6
-S234C5S6 + C234C6
S5S6
0

C234S5
S234S5
C5
0

C234a4+C23a3+C2a2
S234a4+S23a3+S2a2
0
1

Cui cng :
T6 =

nx
ny
nz
0

Ox
Oy
Oz
0

ax
ay
az
0

px
py
pz
1

= A1T61

tnh T6, ta phi nhn A1 vi T61 sau cn bng cc phn t ca ma trn T6 ta c

mt h thng cc phng trnh sau :

TS. Phm ng Phc

40

Robot cng nghip

nx = C1(C234C5C6- S234S6) - S1S5C6

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

Bi tp chng III :
Bi 1 : Cho ma trn :
T6 =

?
?
?
?

0 -1
0 0
-1 0
0 0

0
1
2
1

l ma trn biu din hng v v tr ca khu chp hnh cui. Tm cc phn t c nh du ?

Bi 2 : Cho mt robot c 3 khu phng nh hnh 3.18, cu hnh RRR. Thit lp h phng
trnh ng hc ca robot.

TS. Phm ng Phc

41

Robot cng nghip

Bi 3 : Cho mt robot c 2 khu tnh tin nh hnh 3.19, cu hnh TT. Thit lp h phng
trnh ng hc ca robot.

Hnh 3.18 : Robot cu hnh RRR

Hnh 3.19 : Robot cu hnh TT

Bi 4 : Cho mt robot c 2 khu phng nh hnh 3.20, cu hnh RT. Thit lp h phng trnh
ng hc ca robot.
Bi 5 : Cho mt robot c 3 khu nh hnh 3.21, cu hnh RTR. Thit lp h phng trnh ng
hc ca robot.

Hnh 3.20 : Robot cu hnh RT

Hnh 3.21 : Robot cu hnh RTR

Bi 6 : Cho mt robot c 3 khu nh hnh 3.22, cu hnh RRR. Thit lp h phng trnh
ng hc ca robot.

Hnh 3.22 : Robot cu hnh RRR

Hnh 3.23 : Robot cu hnh RRRRR

Bi 7 : Cho mt robot c 5 khu nh hnh 3.23, cu hnh RRRRR. Thit lp h phng trnh
ng hc ca robot.

TS. Phm ng Phc

42

Robot cng nghip

Chng IV

Gii phng trnh ng hc robot

hay phng trnh ng hc ngc
(Invers Kinematic Equations)

Trong chng 3, ta nghin cu vic thit lp h phng trnh ng hc ca

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

TS. Phm ng Phc

43

Robot cng nghip

Vic gii bi ton ng hc ngc ca robot cn tho mn cc iu kin sau :

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

-cosCossin - sincos
-sinCossin + coscos
sin sin
0
nx
ny
nz
0

cossin
sinsin
cos
0
Ox
Oy
Oz
0

ax
ay
az
0

0
0
0
1
px
py
pz
1

(4-2)

Ln lt cho cn bng cc phn t tng ng ca hai ma trn trong phng

trnh (4-2) ta c cc phng trnh sau :

TS. Phm ng Phc

44

Robot cng nghip

nx = cosCoscos - sinsin
ny = sinCoscos + cossin
nz = -sin cos
Ox = -cosCossin - sincos
Oy = -sinCossin + coscos
Oz = sin sin
ax = cossin
ay = sinsin
az = cos

(4.3)
(4-4)
(4-5)
(4-6)
(4-7)
(4-8)
(4-9)
(4-10)
(4-11)

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

T (4-11) ta c
= cos-1(az)
T (4-9) ta c
= cos-1(ax / sin)
T (4-5) v (4-12) ta c
= cos-1(-nz / sin)
Trong ta dng k hiu cos-1 thay cho hm arccos.

(4-12)
(4-13)
(4-14)

Nhng cc kt qu gii trn cha dng c v cc l do di y :

+ Hm arccos khng ch biu hin cho mt gc cha xc nh m v chnh
xc n li ph thuc vo chnh gc , ngha l :
cos = cos(-) : cha c xc nh duy nht.
dcos
xc nh khng chnh xc.
0,180 = 0 :
d
+ Trong li gii i vi v mt ln na chng ta li dng hm arccos v
chia cho sin, iu ny dn ti s mt chnh xc khi c gi tr ln cn 0.
+ Cc phng trnh (4-13) v (4-14) khng xc nh khi = 0 hoc = 1800.
Do vy chng ta cn phi cn thn hn
khi chn li gii. xc nh cc gc khi gii
bi ton ngc ca robot ta phi dng hm
arctg2 (y,x) (hm arctang hai bin). Hm arctg2
nhm mc ch xc nh c gc thc - duy
nht khi xt du ca hai bin y v x. Hm s tr
v gi tr gc trong khong - < .

X- Y+

TS. Phm ng Phc

X+ Y+

V d :
arctg2(-1/-1)= -1350,
trong khi
arctg2(1/1) = 450
Hm ny xc nh ngay c khi x hoc y
bng 0 v cho kt qu ng.
(Trong mt s ngn ng lp trnh nh
Matlab, turbo C++, Maple hm arctg2(y,x)
c sn trong th vin)

X- Y-

X+ Y-

Hnh 4.1 : Hm arctg2(y,x)

45

Robot cng nghip

c th nhn c nhng kt qu chnh xc ca bi ton Euler, ta thc hin

th thut ton hc sau : Nhn T6 vi ma trn quay nghch o Rot(z, )-1,ta c:
Rot(z, )-1 T6 = Rot(y, ) Rot(z, )

(4-15)

V tri ca phng trnh (4-15) l mt hm s ca ma trn T v gc quay . Ta

thc hin php nhn ma trn v phi ca (4-15), tm ra cc phn t ca ma trn c
gi tr bng 0 hoc bng hng s, cho cc phn t ny cn bng vi nhng phn t
tng ng ca ma trn v tri, c th t (4-15) ta c :
cos
-sin
0
0

sin
cos
0
0

0
0
1
0

0
0
0
1

nx
ny
nz
0

Ox
Oy
Oz
0

ax
ay
az
0

px
py
pz
1

Coscos
sin
-sin cos
0

-Cos sin
cos
sin sin
0

sin
0
Cos
0

0
0
0
1

(4-16)
Tch hai ma trn v tri ca phng trnh (4-16) l mt ma trn m c th
c vit gn li bng cc k hiu sau :

f11(n)
f12(n)
f13(n)
0

f11(O)
f12(O)
f13(O)
0

f11(a)
f12(a)
f13(a)
0

f11(p)
f12(p)
f13(p)
1

f11 = cos x + sin y

f12 = -sin x + cos y
f13 = z

Trong :

(4-17)
(4-18)
(4-19)

v x, y, z l cc phn t ca vect xc nh bi cc d kin f11, f12, f13, v d :

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

f11(n)
f12(n)
f13(n)
0

f11(O)
f12(O)
f13(O)
0

f11(a)
f12(a)
f13(a)
0

Coscos -Cos sin

0
=
sin
cos
0
-sin cos
sin sin
0
0
0
1

sin
0
Cos
0

0
0
0
1

(4-20)

Trong f11, f12, f13 c nh ngha (4-17), (4-18) v (4-19).

Khi tnh ton v tri, ta ch rng px, py, pz bng 0 v php bin i Euler ch
ton php quay khng cha mt php bin i tnh tin no, nn f11(p) = f12(p) = f13(p)
= 0. T phng trnh (4-20), cho cn bng phn t hng 2 ct 3 ta c :

TS. Phm ng Phc

46

Robot cng nghip

f12(a) = -sin ax + cos ay = 0.

(4-21)

Cng hai v vi sin ax v chia cho cos ax ta c :

sin a y
tg =
=
cos a x
Gc c th xc nh bng hm arctg hai bin :
= arctg2(ay, ax).
Ta cng c th gii phng trnh (4-21) bng cch cng hai v vi -cos ay ri
chia hai v cho -cos ax, trit tiu -ax v tri v cos v phi, ta c :

sin -a y
=
cos -a x
Trong trng hp ny gc tm c l :
tg =

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

V
Vy

f11(a) = cos ax + sin ay = sin.

f13(a) = az = cos.
= arctg2(cos ax + sin ay, az)

Khi c hai hm sin v cos u c xc nh nh trng hp trn, th gc

thng c xc nh duy nht v khng xy ra trng hp suy bin nh gc trc
y. Cng t phng trnh (4-20) ta c :

f12(n) = -sin nx + cos ny = sin

f12(O) = -sin Ox + cos Oy = cos

TS. Phm ng Phc

47

Robot cng nghip

Vy :

= arctg2(-sin nx + cos ny, -sin Ox + cos Oy)

Tm li, nu cho trc mt php bin i ng nht di dng cc php quay,

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

Rot(z, )-1T6 = Rot(y,)Rot(x, )

Hay l :

f11(n)
f12(n)
f13(n)
0

f11(O)
f12(O)
f13(O)
0

f11(a)
f12(a)
f13(a)
0

Trong :

cos
0
0
0 =
-sin
0
0
1

sin sin
cos
cos sin
0

sin cos
-sin
coscos
0

0
0
0
1

f11 = cos x + sin y

f12 = -sin x + cos y
f13 = z

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

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

= arctg2(nx, ny)
= + 1800

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

-sin = nz
cos = cos nx + sin ny

TS. Phm ng Phc

(4-22)

48

Robot cng nghip

do vy :

= arctg2(-nz, cos nx + sin ny)

Ngoi ra ta cn c :

-sin = -sin ax + cos ay

cos = -sin Ox + cos Oy

= arctg2(sin ax - cos ay, -sin Ox + cos Oy)

Nn :

Nh vy ta xc nh c cc gc quay Roll, Pitch v Yaw theo cc phn t

ca ma trn T6.
4.4. Gii bi ton ng hc ngc ca robot Stanford :
H phng trnh ng hc ca robot Stanford c thit lp trong chng
III, Ta c :
T6 = A1A2A3A4A5A6
(4-23)
Lin tc nhn (4-23) vi cc ma trn A nghch o, ta c :

A 51

A 11 T6
A 21 A 11 T6
A 31 A 21 A 11 T6
A 41 A 31 A 21 A 11 T6
A 41 A 31 A 21 A 11 T6

= 1T6
= 2T6
= 3T6
= 4T6
= 5T6

(4-24)
(4-25)
(4-26)
(4-27)
(4-28)

Cc phn t v tri ca cc phng trnh ny l hm s ca cc phn t T6 v

cc bin khp ca (n-1) khp u tin. Trong khi cc phn t ca ma trn v bn
phi hoc bng 0, bng hng s hoc l hm s ca cc bin khp th n n khp th
6. T mi phng trnh ma trn, cho cn bng cc phn t tng ng chng ta nhn
c 12 phng trnh. Mi phng trnh c cc phn t ca 4 vect n, O, a, p.
T phng trnh (4-24), ta c :
1
1

A T6 =

TS. Phm ng Phc

C1
0
-S1
0

S1
0
C1
0

f11(n)
f12(n)
f13(n)
0

0
-1
0
0

0
0
0
1

nx
ny
nz
0

f11(O)
f12(O)
f13(O)
0

f11(a)
f12(a)
f13(a)
0

Ox
Oy
Oz
0

ax
ay
az
0

f11(p)
f12(p)
f13(p)
1

px
py
pz
1

49

Robot cng nghip

f11 = C1 x + S1 y
f12 = - z
f13 = -S1 x + C1 y
V bn phi ca (4-24) l :
Trong :

T6 =

C2(C4C5C6 - S4S6) - S2S5C6

S2(C4C5C6 - S4S6) + C2S5C6
S4C5C6 + C4S6
0

-C2(C4C5S6-S4C6)+S2S5S6
-S2(C4C5S6+S4C6)-C2S5S6
-S4C5S6+C4C6
0

C2C4S5 + S2C5
S2C4S5 - C2C5
S4S5
0

S2d3
-C2d3
d2
1

Cc phn t ca ma trn v phi u l hm s ca 2, d3, 4, 5, 6 ngoi tr

phn t hng 3 ct 4, l :

f13(p) = d2
hay :

-S1px + C1py = d2
gii phng trnh dng ny ta c th thay th bi cc hm lng gic sau
y :

px = r cos
py = r sin
Trong :

r = + p 2x + p 2y
= arctg2(py, px)

Th px v py vo phng trnh -S1px + C1py = d2 ta c :

sincos1 - cossin1 = d2 / r

Vi

0 < d2 / r 1

Hay l : sin( - 1) = d2 / r
T ta c :

Vi

0 < - 1 <

cos( - 1) = 1 (d 2 / r ) 2
Trong du tr ph hp vi hnh th vai tri ca robotv du cng ph hp
vi hnh th vai phi ca robot. Cui cng :

1 = arctg2(py, px) - arctg2(d2, 1 (d 2 / r ) 2 )

(4-29)

Nu tnh c 1 th v tri ca phng trnh (4-24) c xc nh.

Cho cn bng cc phn t hng 1 ct 4 v hng 2 ct 4, ta c :

S2d3 = C1px + S1py

-C2d3 = -pz
d3 l dch chuyn di ca khp tnh tin, d3 > 0, nn ta c :
2 = arctg2(C1px + S1py, pz )

TS. Phm ng Phc

(4-30)

50

Robot cng nghip

T phng trnh (4-25) : A 21 A 11 T6 = A 21 1T6 = 2T6, ta c :

f21(n)

f21(O)

f21(a)

f22(n)

f22(O)

f22(a)

f23(n)
0

f23(O)
0

f23(a)
0

f23(p)
1

Trong :

C4C5C6-S4S6
= S4C5C + C4S6
-S5C6

-C4C5S6 - S4C6
-S4C5S6 + C4C6
S5S6

C4S5
S4S5
C5

0
0
d3
0
1
(4-31)

f21 = C2(C1 x + S1 y) - S2 z
f22 = -S1 x + C1 y
f23 = S2(C1 x + S1 y) + C2 z

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

d3 = S2(C1 px + S1 py) + C2 pz

(4-32)

- T phng trnh (4-27) ta c : A 41 A 31 2T6 = 4T6

Thc hin php nhn cc ma trn v tri, v biu din dng rt gn nh sau
:

f41(n)
f42(n)
f43(n)
0

f41(O)
f42(O)
f43(O)
0

Trong :

f41(a)
f42(a)
f43(a)
0

0
0
0
1

C5C6
S5C6
S6
0

-C5S6
-S5S6
C6
0

S5
C5
0
0

0
0
0
1

f41 = C4[C2(C1 x + S1 y) - S2 z] + S4(-S1 x + C1 y)

f42 = -S2(-S1 x + C1 y) - C2 z
f43 = -S4[C2(C1 x + S1 y) + S2 z] + C4(-S1 x + C1 y)

Cn bng phn t hng 3, ct 3 ta c mt hm s ca 4, l : f43(a) = 0.

Hay :

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

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

4 = arctg2(-S1 ax + C1 ay, C2(C1 ax + S1 ay) + S2 az)

4 = 4 + 1800

(4-33)

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

suy bin nh trung hp ni mc 4.2.
Ta cng c th tm gi tr ca gc quay 4 bng cch cn bng cc phn t hng
1 ct 3 v hng 2 ct 3 ca phng trnh ma trn (4-31) , ta c :

C4S5 = C2(C1 ax + S1 ay) - S2 az

TS. Phm ng Phc

51

Robot cng nghip

S4S5

= -S1 ax + C1 ay

Vi 5 > 0 ta c 4 = arctg(-S1 ax + C1 ay, C2(C1 ax + S1 ay) + S2 az)

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

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

C5 = S2 (C1 ax + S1 ay) + C2 az
T suy ra :

5 = arctg2(C4 [C2(C1 ax + S1 ay) - S2 az] + S4(-S1 ax + C1 ay) ,

(4-34)
S2 (C1 ax + S1 ay) + C2 az )

Cc phng trnh c lin quan n 6 nm ct 1 ca phng trnh ma trn,

l cc thnh phn ca vect n ca T6. Vect ny thng khng c ngha trong tnh
ton, v n lun c
c xc nh bng tch vect ca hai vect O v a nh ni
r th
r
r
trc y ( n = O x a ). Do ta phi tm cch khc xc nh 6.
Thc hin php nhn cc ma trn v tri ca phng trnh (4-28) : A 51 4T6=
5
T6 = A6, biu din dng k hiu ta c :

f51(n)
f52(n)
f53(n)
0

f51(O)
f52(O)
f53(O)
0

0
0
1
0

0
0
0
1

C6
S6
0
0

-S6
C6
0
0

0
0
1
0

0
0
0
1

(4-35)

Trong :

f51 = C5{ C4 [C2(C1 x + S1 y) - S2 z] + S4(-S1 x + C1 y)} + S5[-S2 (C1 x + S1 y) - C2 z]

f52 = -S4 [C2(C1 x + S1 y) - S2 z] + C4[-S1 x + C1 y]
f53 = S5{ C4 [C2(C1 x + S1 y) - S2 z] + S4(-S1 x + C1 y)} + C5[S2 (C1 x + S1 y) - C2 z]
Cho cn bng cc phn t hng 1 ct 2 v hng 2 ct 2 ta nhn c cc gi
tr ca S6 v C6 :
S6 = -C5{C4[C2(C1Ox+S1Oy)-S2Oz] +S4(-S1Ox+C1Oy)} + S5[S2 (C1Ox + S1Oy) + C2Oz]
C6 = -S4 [C2(C1Ox + S1Oy)- S2 Oz] + C4[-S1 Ox + C1 Oy]
T ta xc nh c :

TS. Phm ng Phc

6 = arctg2(S6, C6)

(4-36)

52

Robot cng nghip

Cc biu thc (4-29), (4-30), (4-32), (4-33), (4-34) v (4-36) xc nh tp

nghim khi gii bi ton ngc ca robot Stanford.
4.5. Gii bi ton ng hc ngc ca robot ELBOW :
tip tc lm quen vi vic gii h phng trnh ng hc, chng ta nghin
cu php gii bi ton ng hc ngc ca robot Elbow. H phng trnh ng hc
thun ca robot Elbow dc xc nh trong chng III.
Trc ht ta kho st phng trnh :

A 11 T6 = 1T6 = A2A3A4A5A6
Tng t nh lm, ta xc nh cc phn t ma trn ca hai v nh sau :
f11(n)
f12(n)
f13(n)
0

f11(O)
f12(O)
f13(O)
0

f11(a)
f12(a)
f13(a)
0

f11(p)
f12(p)
f13(p)
1

C234C5C6 - S234S6

-C234C5S6 - S234C6

C234S5

C234a4+C23a3+C2a

S234C5C6 + C234S6
-S5C6
0

-S234C5S6 + C234C6
S5S6
0

S234S5
C5
0

S234a4+S23a3+S2a2
0
1
(4-37)

f11 = C1 x + S1 y
f12 = z
f13 = S1 x + C1 y
C234 = cos(2+3+4)
Ta k hiu :
S234 = sin(2+3+4)
Trong :

Cho cn bng phn t hng 3 ct 4, ta c :

S1 px + C1 py = 0
Suy ra :
1 = arctg2(py , px)
v
1 = 1 + 1800

(4-38)

Trong trng hp robot Elbow, ba khp k tip u song song v khng c kt

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

A-14A-13A-121T6 = 4T6 = A5A6

Khi xc nh cc phn t ma trn ca hai v ta c :

TS. Phm ng Phc

53

Robot cng nghip

f41(n)
f42(n)
f43(n)
0

f41(O)
f42(O)
f43(O)
0

Trong :

f41(a)
f42(a)
f43(a)
0

f41(p)-C34a2-C4a3-a4
C5C6
0
= S5C6
f43(p)+S34a2+S4a3
S6
1
0

-C5S6
-S5S6
C6
0

S5 0
-C5 0
0
0
0
1
(4-39)

f41 = C234(C1 x + S1 y) + S234 z

f42 = -S1 x + C1 y
f43 = -S234(C1 x + S1 y) + C234 z

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

-S234(C1 ax + S1 ay) + C234 az = 0

Suy ra :
v

234 = arctg2(az , C1 ax + S1 ay)

234 = 234 + 1800

(4-40)

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

ct 4 v hng 2 ct 4, ta c :
C1 px + S1 py = C234a4+C23a3+C2a2
(a)
pz
= S234a4+S23a3+S2a2
(b)
Ta gi :
px
= C1 px + S1 py - C234a4
(c)
py
= pz - S234a4
(d)
em (a) + (c) v (b) + (d) ta c ;
px = C23 a3 + C2a2
(e)
(g)
py = S23 a3 + S2a2
Bnh phng hai v v cng hai phng trnh (e) v (g), ta c :
p2x = (C23 a3 + C2a2)2
p2y = (S23 a3 + S2a2)2

p2x + p2y = (S

23

+C

23

)a23 + (S

+C

2
2)a 2

+ 2 a2a3(C23C2 + S23S2 )

Ta c C23C2 + S23S2 = cos(2+3-2) = cos3 = C3. Nn suy ra :

C3 = (p2x + p2y - a23 - a22) / 2a2a3

Trong khi c th tm 3 t hm arccos, ta vn nn tm mt gi tr S3 v dng
hm arctg2 nh thng l :
Ta c :

S3 = (1 C 23 )

Cp nghim ng vi hai du +,- ph hp vi hnh th ca robot lc nng vai ln

v h vai xung :

TS. Phm ng Phc

54

Robot cng nghip

3 = arctg2(S3 , C3)

(4-41)

tm S2 v C2 ta gii h phng trnh (e),(g).

T (e)
T (g)

(C2C3 - S2S3)a3 + C2a2

(S2C3 - C2S3)a3 + S2a2

= px
= py

Khai trin v rt gn :
T (g)
Ta c :

(C3a3 + a2)C2 S3a3.S2 = px

S3a3 .C2 + (C3a3 + a2)S2 = py
=

C 3a 3 + a 2
S 3a 3

p ,x
c = ,
py

- S 3a 3
C 3a 3 + a 2

- S 3a 3
C 3a 3 + a 2

C 3a 3 + a 2
s =
S3a 3

C2 =

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

(C 3 a 3 + a 2 ) 2 + (S3 a 3 ) 2

S2 =

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

(C 3 a 3 + a 2 ) 2 + (S 3 a 3 ) 2

p ,x
p ,y

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

2 = arctg2(S2, C2)
2 = arctg2((C3a3 + a2)py - S3a3px , (C3a3 + a2)px + S3a3py )

(4-42)

n y 4 c xc nh bi :
4 = 234 - 3 - 2

(4-43)

Cc phng trnh dng tnh 5 c thit lp t s cn bng cc phn t ma

trn hng 1 ct 3 v hng 2 ct 3 ca phng trnh 4T6 (4-39) :

S5 = C234(C1ax + S1ay) + S234az

C5 = S1ax - C1ay
Suy ra :
5 = arctg2(C234(C1ax + S1ay) + S234az , S1ax - C1ay)
tm 6 , ta tip tc nhn A-15 vi 4T6 , ta c :
Vit tch ma trn v tri dng k hiu :

TS. Phm ng Phc

A-15 . 4T6 = A6.

(4-44)

55

Robot cng nghip

f51(n)
f52(n)
0
0

Trong :

f51(O)
f52(O)
0
0

0
0
1
0

0
0
0
1

C6
S6
0
0

-S6
C6
0
0

0
0
1
0

0
0
0
1

f51 = C5[C234(C1 x + S1 y) + S234 z] - S5(S1 x + C1 y)

f52 = -S234(C1 x + S1 y) + C234 z

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

S6 = -C5[C234(C1 Ox + S1 Oy) + S234 Oz] - S5(S1 Ox + C1 Oy)

C6 = -S234(C1 Ox + S1 Oy) + C234 Oz
Vy :

6 = arctg2(S6 , C6)

(4-45)

Cc phng trnh (4-38), (4-41), (4-42), (4-43), (4-44) v (4-45) xc nh tp

nghim khi gii bi ton ng hc ngc ca robot Elbow.
4.6. Kt lun :
Phng php gii bi ton ng hc ngc a ra trong chng ny s dng
cc hm lng gic t nhin. Cc gc thng c xc nh thng qua hm arctang hai
bin. Phng php ny c a ra bi Pieper v p dng tt vi nhng robot n gin,
Thng ta nhn c nghim dng cng thc n gin.
Khi gii bi ton ng hc ngc c th xy ra hin tng suy bin . Khi c
nhiu hn mt tp nghim i vi bi ton ngc xc nh v tr v hng ca bn
tay, th cnh tay c gi l suy bin.
Dng phng php Pieper, cc nghim nhn c thng c 4 dng cng thc,
mi dng c mt ngha ng hc ring. Dng th nht :

- sin ax + cos ay = 0
Dng ny cho ta mt cp nghim cch nhau 1800, n m t hai hnh th tng
ng ca robot. Nu cc t s v mu s u bng khng, robot b suy bin, lc
robot mt i mt bc t do.
Dng th hai :

-S1px + C1py = d2

Dng ny cng cho ta cp nghim sai khc nhau 1800, mt ln na li tn ti

kh nng suy bin khi t s v mu s bng 0. Robot trng hp ny thng c mt
khp tnh tin v di tnh tin c coi l > 0.
Dng th ba :
v dng th t :

TS. Phm ng Phc

C1px + S1py = S2d3

- C2d3 = - pz

56

Robot cng nghip

y2
x2
O2

Cc phng thnh ny thng c nghim duy nht.

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

Bi tp chng IV :
Bi 1 : Cho mt v tr mong mun ca khu chp hnh cui ca robot c 3 khu phng
nh hnh 4.2; Dng phng php hnh hc xc nh c bao nhiu li gii ca bi
ton ng hc ngc ? Nu hng ca khu chp hnh cui cng c xc nh, th c
bao nhiu li gii ?

Bi 2 : Dng phng php hnh hc gii bi ton ng hc ngc (xc nh cc gc

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

TS. Phm ng Phc

57

Robot cng nghip

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

ton ng hc ngc ca n.

z0

1
O0

a2

z1

x
x

a1

d3

z2
O3
O4
4

z3 , z4

Hnh 4.4 : Robot SCARA

TS. Phm ng Phc

x
d4
x

58

Robot cng nghip

Chng V
ngn ng lp trnh robot
(Robot Programming Languages )

5.1. Gii thiu chung v lp trnh iu khin robot :

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

TS. Phm ng Phc

Robot cng nghip

59

5.2.1. Lp trnh kiu Dy - Hc :

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

TS. Phm ng Phc

Robot cng nghip

60

chung cho nhiu mc ch nh l mt chng trnh c bn, sau cung cp thm

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

TS. Phm ng Phc

61

Robot cng nghip

Thanh cng c

Menu chnh

Ca s nhp - xut d liu .

Meta keys

Thanh trng thi

Dng chn nhanh kiu giao din

Hnh 5.1 : Ca s chnh ca Procomm Plus for Windows, Version 3.0

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

TS. Phm ng Phc

Robot cng nghip

62

+ Start recorder... : bt u t ng to ra mt scrips file bng cc

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

TS. Phm ng Phc

Robot cng nghip

63

Chng ta c th to mi v son tho file ngun (.was) bng trnh ASPECT

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

Hnh 5.3 : Ca s son tho v dch cc script file

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

TS. Phm ng Phc

Robot cng nghip

64

5.4.2. Kiu d liu v khai bo bin trong ASPECT :

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

TS. Phm ng Phc

Robot cng nghip

65

ging nh bin a phng. Th t m cc tham bin c nh ngha xc nh th t

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

; nh ngha hm Sum tnh tng.

; Khai bo cc tham bin kiu nguyn.
; Khai bo bin Tong (bin a phng).
; Tng ca 3 s.
; Tr v gi tr ca tng ca 3 s.
; ht phn nh ngha hm

(Ghi ch : du ; dng ghi ch trong chng trnh, cc ni dung sau du

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

TS. Phm ng Phc

Robot cng nghip

66

Khi kt thc mt hm hoc th tc c gi, n t ng tr v dng lnh tip theo.

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

TS. Phm ng Phc

Robot cng nghip

67

Proc main
integer A,B,C,D
integer Tong
A=2, B=4
C=A+B
Tong = A+ --B
D=(tong < C) ? tong : C
; nu Tong < C th D=Tong, nu sai D=C
Usermsg " D = %d , C = %d" D,C
Endproc
Kt qu

D = 5 v C = 6.

5.4.5. Mt s t lnh trong ASPECT hay dng khi iu khin robot:

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

TS. Phm ng Phc

68

Robot cng nghip

endswitch
V d :
proc main
integer Alpha = 2
switch Alpha
case 0
usermsg "Alpha = 0"
Endcase
case 1
usermsg "Alpha = 1"
endcase
case 2
usermsg "Alpha = 2"
endcase
endswitch
endproc

; gn gi tr ban u cho bin Alpha=2.

; tm gi tr ca bin s
; Trng hp bin c gi tr bng 0.
; Xut kt qu trn ca s mn hnh.
; Ht trng hp so snh th nht.
; tng t nh trn . . .

; lun i km vi switch kt thc khi lnh switch.

if / endif :
Cu lnh iu kin.
C php :
if <iu kin 1>
...
[elseif <iu kin 2]
...
[else]
...
endif
; kt thc khi lnh if.
(Lnh ny gn ging nh lnh if trong Pascal, khng c t then).
while/endwhile :
Lp li mt s cu lnh cho n khi iu kin kim tra l sai.
V d :
proc main
integer SoLanLap = 0

; Bin nguyn dng m s ln lp

while (SoLanLap++) < 3

; Mi ln lp bin tng gi tr thm mt
endwhile
; Kt thc khi lnh while.
usermsg "Toi da lap %d lan" SoLanLap
endproc
for/endfor : Cu lnh lp theo mt s ln nht nh
C php :
for <bin m>=<gi tr ban u> UPTO | DOWNTO <gi trcui>
[BY <bc>]
.
[exitfor]
; Chuyn iu khin thot khi cu lnh lp for
.
; n dng lnh sau endfor

TS. Phm ng Phc

69

Robot cng nghip

endfor
Return :
Thot khi th tc hoc hm hin ti, tip tc cu lnh tip theo ca
th tc gi.
* Cc lnh khc :
transmit :

Gi mt dng k t (lnh) n cng ang hot ng.

V d :
proc main
transmit "B-250~C-200~F-240~~P+200
endproc

; Chuyn lnh iu khin robot TG-45

Pause :
Tm dng thc hin chng trnh trong mt s giy qui nh.
C php :
pause <s giy | FOREVER>
V d :
Pause 5 : tm dng thc hin chng trnh 5 giy
Pause Forever : Dng vi thi gian khng xc nh. Lnh Pause c th
c hu b khi n Ctl-Break.
K t ~ thay cho lnh pause vi gi tr dng bng 0,5 giy.
V d :

Transmit B+200~~E-100
Sau khi truyn lnh B+200 s tm dng 1 giy (2 k t ~) mi truyn tip
lnh E-100.
chdir :

Thay i ng dn n mt a hoc th mc khc.

C php :
chdir <Tn ng dn>

V d :
Chdir C:\ procom3\Robot
copyfile :
Copy mt file theo ng dn hoc vi mt tn khc.
C php :
copyfile <file ngun> <file ch>
V d :
copy C:\ Procom3\ aspect\ robot.was C:\ tam\ robot1.txt
delfile :
Xo mt file theo ch nh.
C php :
delfile <tn file>
mkdir :
To mt th mc mi.
C php :
mkdir <tn th muc>
rmdir

Xo mt th mc (trng)

TS. Phm ng Phc

70

Robot cng nghip

C php :

rmdir <tn th mc>

rename :
i tn mt file.
C php :
rename <tn file c> <tn file mi>
Fopen :
M mt file c hoc ghi.
C php :
fopen <s hiu file> <tn file> READ | WRITE |
READWRITE | CREATE | APPEND | READAPPEND
Cc tu chn : READ : ch c; READWRITE : c th c v ghi;
CREATE : To mi; APPEND : Ghi tip vo cui file;
READAPPEND : C th c v ghi tip vo cui file.
Fclose :
ng mt file m.
C php :
Fclose <s hiu file>
fputs

Ghi mt chui k t ln file.

C php :
fputs <s hiu file> <chui k t>

V d :
proc main
string Fname = "Vidu.txt"

; Tn file c m.

if fopen 0 Fname CREATE

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

Kim tra iu kin cui mt file.

C php :
feof <s hiu file> [bin nguyn]
[bin nguyn] : c gi tr 0 nu cha kt thc file, bng 1
nu kt thc file.

Fgets :
c mt dng k t t mt tp m ghi vo mt bin.
C php :
fgets <s hiu file> <tn bin kiu string>
V d :
proc main
string Fname = "Vidu.txt" ; Tn file cn c
string chuoi
; Bin chuoi nhn gi tr c t file.
if fopen 0 Fname READ
while not feof 0
fgets 0 FInput

TS. Phm ng Phc

; M file ch c (s hiu file id=0).

; Lp li khi cha kt thc file.
; c mt dng t file

71

Robot cng nghip

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

o
Thn

Cnh tay

o C tay
Bn tay

Hnh 5.3 : S ng Robot TG-45

TS. Phm ng Phc

Robot cng nghip

72

Cc khp quay ca robot c dn ng bng cc ng c in mt chiu c

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

TS. Phm ng Phc

Robot cng nghip

73

ch TERMINAL ca Procomm Plus ta c th iu khin trc tip robot

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

TS. Phm ng Phc

Robot cng nghip

74

2) Thit k iu khin ng hc thun : ngha l chng trnh cho php ngi

s dng iu khin robot theo gi tr cc gc quay ca khp (tnh bng ) khi xc
nh trc mt cu hnh no ca robot.
3) Thit k iu khin ng hc ngc : ngha l ngi s dng chng trnh
c th iu khin robot theo cc to v tr v hng ca bn tay c xc nh
trc. Khi ta nhp cc gi tr v to v hng ca bn tay th chng trnh t ng
tnh ton cc gc quay ca cc khp robot chuyn ng n v tr yu cu vi
hng c xc nh.
4) Thit k cc tin ch khc nh : iu khin theo ng, to cc meta keys,
to ra cc tr gip cho ngi s dng...
Cc ni dung 2 v 3 cn phi thit lp h phng trnh ng hc ca robot
TERGAN-45 v gii h phng trnh ng hc . Phn tnh ton c th vit bng
ngn ng Pascal hoc C++ m n c gi t chng trnh iu khin (dng lnh
RUN), chng trnh iu khin x l kt qu tnh ton qua cc file trung gian dng
text.
Phn mm Procomm cung cp nhiu tin ch ta c th thit k chng
trnh kiu trc quan (Visual), gip cho vic vit chng trnh v thao tc trong qu
trnh s dng c d dng, thun tin hn.
5.8. Kt lun :
Trong chng ny ch gii thiu mt cch tng qut v cc phng php lp
trnh iu khin robot. Kh c th i su, c th vo mt ngn ng no v n ph
thuc rt nhiu vo loi robot c s dng. Phn ngn ng ASPECT trong phn
mm Procomm c nghin cu trn l mt v d v ng dng cc phn mm dng
cho nhiu mc ch iu khin robot. Trong thc t, tu nhim v c th ca mi
robot, phi hp vi hot ng chung ca cc my mc thit b khc m ta thit k cc
chng trnh c th robot hot ng theo nhng mc ch mong mun.

TS. Phm ng Phc

Robot cng nghip

75

Bi tp chng V :
Bi 1 : Hy vit mt Function ca hm arctg2(y,x) bng ngn ng Turbo Pascal.
Bi 2 : Vit mt chng trnh (ngn ng tu ) nhp cc thng s DH v t ng
xc lp cc ma trn An (Biu hin kt qu trn mn hnh v ghi vo mt file dng
text).
Bi 3 : Vit mt chng trnh bng Turbo Pascal tnh ton ng hc nguc (Xc
nh cc gc quay) ca robot TERGAN-45. D liu nhp t bn phm. Ghi kt qu
vo mt file dng text.
Bi 4 : Vit mt chng trnh iu khin robot TERGAN-45 bng ngn ng ASPECT
robot c cu hnh nh sau :
1 = +300; 2 = -100; 3 = -300; 4 = -250.
Bn tay robot nm li sau khi di chuyn n v tr yu cu.
Bi 5 : Vit mt chng trnh bng ngn ng ASPECT, gi chng trnh tnh ng
hc ngc vit bng Turbo Pascal (nh bi 3), x l kt qu tnh ton iu khin
robot TERGAN-45 theo to v tr v hng ca bn tay.

TS. Phm ng Phc

76

ROBOT Cng nghip

Chng VI

M phng robot trn my tnh

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

TS. Phm ng Phc

ROBOT Cng nghip

77

Coordinates). Easy-Rob c sn cc trnh iu khin ng hc thun v ngc ca

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

TS. Phm ng Phc

78

ROBOT Cng nghip

Thanh cng c

Menu chnh

Thanh cng c

Ca s thit k

17

12

18

Hnh 6.1 : Mn hnh EASY-ROB.

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

TS. Phm ng Phc

ROBOT Cng nghip

79

2. Chuyn tt c cc i tng sang dng li.

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

TS. Phm ng Phc

ROBOT Cng nghip

80

Khi nt lnh s 1 ca thanh cng c thng ng c chn :

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

TS. Phm ng Phc

ROBOT Cng nghip

81

6.6. V hnh dng robot :

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

TS. Phm ng Phc

82

ROBOT Cng nghip

6.7. Lp trnh iu khin robot m phng :

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

Hnh 6.3 : Ca s lp trnh.

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

TS. Phm ng Phc

ROBOT Cng nghip

83

ENDPROGRAMFILE or END : Kt thc chng trnh.

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

TS. Phm ng Phc

84

Robot cng nghip

chng VII
ng lc hc Robot
(Dynamic of Robot)

7.1. Nhim v v phng php phn tch ng lc hc robot

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

K = Ki
i =1

P = Pi
i =1

y, Ki v Pi l ng nng v th nng ca khu th i xt trong h to chn.Ta

bit mi i lng Ki v Pi l mt hm s ph thuc nhiu bin s:
Ki = K(qi, q& i ) v Pi = P(qi, q& i )
Vi qi l to suy rng ca khp th i. Nu khp th i l khp quay th qi l gc
quay i, nu l khp tnh tin th qi l di tnh tin di.
Ta nh ngha : Lc tc dng ln khu th i (i=1, 2,..., n) vi quan nim l lc tng
qut (Generalized forces), n c th l mt lc hoc mt momen (ph thuc vo bin khp
qi l tnh tin hoc quay), c xc nh bi:
d L L
Fi =

(7.2)
dt q& i q i

TS. Phm ng Phc

85

Robot cng nghip

Phng trnh ny c gi l phng trnh Lagrange-Euler, hay thng c gi tt

l phng trnh Lagrange.
7.3. V d p dng :

Xt mt robot c hai khu nh hnh v, Cc khu c chiu di l d1 v d2 vi cc

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

ng nng v th nng s l :
1
1
K 2 = m 2 v 22 = m 2 d 12 & 12 + d 22 (& 12 + 2& 1& 2 + & 22 ) + 2d 1d 2 cos( 2 )(& 12 + & 1& 2 )
2
2
P2 = m 2 g[d 1 cos(1 ) + d 2 cos( 1 + 2 )]

(7.5)
(7.6)

7.4. Hm Lagrange v lc tng qut :

p dng hm Lagrange cho v d trn, ta c :
L = (K1 + K2) - (P1 + P2)
1
1
L = ( m1 + m 2 )d 12 & 12 + m 2 d 22 (& 12 + 2& 1& 2 + & 22 ) + m 2 d 1d 2 cos 2 (& 12 + & 1& 2 ) +
2
2
+ ( m1 + m 2 )gd 1 cos 1 + m 2 gd 2 cos(1 + 2 )
(7.7)
Khi tnh lc tng qut, cc bin ca h : q1 = 1 v q2 = 2.
i vi khu 1 :
L
L
=
= ( m1 + m 2 )d 12 & 1 + m 2 d 22 (& 1 + & 2 ) + 2 m 2 d 1d 2 cos 2 & 1 + m 2 d 1d 2 cos 2 & 2
&
q& 1 1

TS. Phm ng Phc

86

Robot cng nghip

d L
&& + m d 2 (&&
&&
& &
&&
= ( m1 + m 2 )d 12
1
2 2 1 + 2 ) 2 m 2 d 1d 2 sin 2 2 1 + 2 m 2 d 1d 2 cos 2 1
&
dt 1
&&
m d d sin & 2 + m d d cos
2 1 2

2 2

2 1 2

2 2

L
L
=
= ( m1 + m 2 )gd 1 sin 1 m 2 gd 2 sin(1 + 2 )
q 1 1
Vy :

L
d L
&& +

= [( m1 + m 2 )d 12 + m 2 d 22 + 2 m 2 d 1d 2 cos 2 ]
1
&
dt 1 1
&& 2 m d d sin & & m d d sin & 2 +
(7.8)
+[ m 2 d 22 + m 2 d 1d 2 cos 2 ]
2
2 1 2
2 2 1
2 1 2
2 2
+ ( m1 + m 2 )gd 1 sin 1 + m 2 gd 2 sin( 1 + 2 )
Mun cho khu 1 quay c mt gc 1 th ng c phi to ra mt lc tng qut
F1. Lc tng qut ny c c tnh phi tuyn, l hp tc dng ca nhiu yu t (non linear
and cuppling).
Tng t, tnh lc tng qut ca khu th hai , ta c :
L
= m 2 d 22 & 1 + m 2 d 22 & 2 + m 2 d 1d 2 cos 2 & 1
&
F1 =

d L
&&
& &
&& + m d d cos
= m 2 d 22 &&
1 + m 2 d 22
2
2 1 2
2 1 m 2 d 1d 2 sin 2 1 2
&
dt 2

L
= m2 d1d 2 sin( 2 )&1&2 m2 d1d 2 sin( 2 )&12 m2 gd 2 sin(1 + 2 )
2

v
Vy :

d L L

= [m2 d 22 + m2 d1d 2 cos 2 ]&&1 + m2 d 22&&2

&
dt 2 2
m d d sin( )& 2 + m gd sin( + )
F2 =

(7.9)

phn tch ngha cc thnh phn trong biu thc tnh lc tng qut, ta vit li
cc biu thc F1, F2 nh sau :
&& + D & 2 + D & 2 + D & & + D & & + D
F1 = D11&&
1 + D12
2
111 1
122 2
112 1 2
121 1 2
1
2
2
&&
&&
&
&
&
&
&
&
F2 = D12 1 + D 22 2 + D 211 1 + D 222 2 + D 212 1 2 + D 221 1 2 + D 2
Hiu ng
Hiu ng Hiu ng
Hiu ng
qun tnh
ly tm
tng h
trng trng
Effective inertias Centripetal effect Coriolis effect Gravity

(Trong : D111 = 0; D222 = 0; D112 = D121 = D212 = D221 =-m2d1d2sin2 ...)

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

tc ng ln khp i gy ra bi vn tc ti khp j. S hng dng D ijk& j&k + D ikj&k& j l lc

Cariolis tc ng ln khp th i gy ra do vn tc ti khp j v k. S hng c dng Di l lc
trng trng tc ng ln khp i.

TS. Phm ng Phc

87

Robot cng nghip

7.5. Phng trnh ng lc hc robot :

Xt khu th i ca mt robot c n khu. Tnh lc tng qut Fi ca khu th i vi

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

Khi tnh bnh phng ca vn tc ny ta c :

&r. &r = r 2 ( x& o , y& o , z& o ) = Tr ( &r &r T )

(7.12)

y
i

Khu i

dm

Ti
r
O0

z
Hnh 7.1. Kho st tc ca vi khi lng dm.

Vi rT l chuyn v vect v Tr l vit tt ca Trace (vt ca ma trn) :

a 11 a 12 ... a 1n
a
a 22 ... a 2 n n
21
= a
Trace
... ... ... ... i =1 ii

a n1 a n 2 a 11 a nn
Hay :
x 2

y . [x y z ] =
y2

z
z

Do vy
d
d
&r 2 = Tr ( &r. &r T ) = Tr ( Ti .i r. Ti T .i r T )
dt
dt

TS. Phm ng Phc

88

Robot cng nghip

i T
i T
= Tr i q& j i r. i q& k i r T
k =1 q k

j=1q j
T
i i Ti i i T Ti

rr .
q& j q& k
= Tr
qk
j =1 k =1 q j

(7.13)

7. 5. 2. Tnh ng nng ca vi khi lng dm.

K hiu Ki l ng nng ca khu th i. dKi l ng nng ca vi khi lng dm t

ti v tr ir trn khu th i.
T

1 i i Ti i i T Ti
dK i = Tr
rr .
q& j q& k dm
2 j=1k =1 q j
q k

T T
1 i i Ti i
= Tr
(7.14)
( r. dm.i r T ). i q& jq& k
2 j=1k =1 q j
q k

V do ng nng ca khu th i s l :

Ti T
1 i i Ti
i i T
K i = dK = Tr
( r. r dm ).
q& j q& k (7.15)
2 j =1 k =1 q j Khau i
q k
Khau i

Ji =

r.i r T dm gi l ma trn gi qun tnh (Pseudo inertia matrix).

Khau i

ngha "gi qun tnh" c s dng v khi thit lp y cc phn t ca ma trn Ji ta

c th lin h vi cc khi nim "mmen qun tnh c cc" v trnh by cc phn t ca Ji
ging nh cc phn t ca mmen qun tnh c cc. Ta xt mi quan h ny nh sau :
Theo nh ngha ta c :
i i
i x 2 dm
x ydm i x i zdm i xdm

i i
i 2
i i
i
x
ydm
y
dm
y
zdm
ydm

J i = i r.i r T dm = Ji = i i
(7.16)

i i
i 2
i
x
zdm
y
zdm
z
dm
zdm
Khau i

i
i
i xdm
ydm zdm dm

By gi ta nhc li mmen qun tnh c cc ca
mt vt th bt k nh hnh v.
y x
Theo nh ngha ta c :

I xx = ( y 2 + z 2 )dm
z
I yy = x 2 + z 2 )dm
Hnh 7.2 : Mmen qun tnh c cc

V v :
Vy :

I zz = ( x 2 + y 2 )dm

1
1
1
x2 = ( y2 + z2 ) + (x2 + z2 ) + (x2 + y2 )
2
2
2
2
x dm =( I xx + I yy + I zz ) / 2 ; .v.v

Ngoi ra ta cn c :
I xy = xydm ; I yz = yzdm ; I xz = xzdm

mx = xdm ; my = ydm ;

TS. Phm ng Phc

mz = zdm

89

Robot cng nghip

i chiu vi ma trn gi qun tnh Ji, ta c th trnh by Ji nh sau :

I xx + I yy + I zz
I yx
I zx
mx

I xx I yy + I zz

I xy
I zy
my
ji =

2
I xx + I yy I zz

I yz
I yz
mz

mx
my
mz
m

Nh vy ngha biu trng ca Ji r.

T

1 i i Ti Ti
Vy ta c :
K i = Tr
Ji
q& j q& k
2 j =1 k =1 q j
q k

Cui cng, ng nng ca mt robot c n khu c tnh :

(7.17)

(7.18)

K = Ki

(7.19)

i =1

7. 5. 3. Tnh th nng ca robot :

Th nng ca khu i c khi lng mi, trng tm c xc nh bi vect ri (vect

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

g = y =
g z 9,8

0 0
Th nng ca ton c cu robot n khu ng s l :
n

P = mi gTi i ri

(7.21)

i =1

7. 5.4. Hm Lagrange :
Sau khi xc nh ng nng v th nng ca ton c cu, ta c hm Lagrange ca
robot c n bc t do :

L=

Ti
Ti T
1 n i i

Trace
J

q i q
2 i =1 j =1 k =1
k
j

q& j q& k + 1 mi gTi i ri

2 i =1

(7.22)

Chng ta ch rng, trong hm Lagrange vn cha cp n nh hng ca

ngun truyn ng (gm cc phn tnh (stator) v phn ng (Rotor) ca ng c in).
7. 5. 5. Phng trnh ng lc hc robot :

Ta bit lc tng qut t ln khu th i ca robot c n khu (Phng trnh

Lagrange - Euler) :
d L L
Fi =

(7.23)
dt q& i q i
Sau khi thit lp hm Lagrange, vi p = 1... n, ta tnh c :
TS. Phm ng Phc

90

Robot cng nghip

(p l ch s ln lt ly theo j v k)
Ti Ti T
Ti
Ti T
1 n i
L 1 n i
q& j

Ji
q& k + Tr
Ji
= Tr
2 i =1 j =1 q j
q p
q& p 2 i =1 k =1 q p
qk
Thay i ch s gi j thnh k trong s hng th hai ,v rng :

(7.24)

Ti
Ti
Ti
Ti T
Ti T
Ti T

(7.25)
Tr
J
J
J
= Tr
= Tr
q i q
q i q
q i q
p
p
j
p
j
j
n
i
Ti
Ti T
L

Ji
q& k
(7.26)
ta c :
= Tr
q& p i =1 k =1 qk
q p
Cng rng : trong Ti(q1, q2, . . . , qi), vi qi l cc bin khp ca i khp u tin. Do
Ti
vy, nu i < p th
= 0.
q p

q& k

n
i
T
T T
L
= Tr i J i i
q& p i = p k =1 qk
q p

Cui cng ta c :

(7.27)

Ly vi phn theo thi gian t ca phng trnh trn :

T
T T
d L
d n i
= Tr i J i i
dt q& p dt i = p k =1 qk
q p
n
i
T
T T
= Tr i J i i
q
q p
i = p k =1
k

q& k

T
n
i
i
2

q&&k + Tr Ti J i Ti q& k q& m +

q p
i = p k =1 m =1
qk qm

n
i
i
2Ti
T T
J i i q& k q& m
+ Tr
qk
i = p k =1 m =1
q p qm

(7.28)

(Bin i theo ch (7.25))

S hng cui ca phng trnh Lagrange Euler l :

2Ti
T T
L 1 n i i
Ji i
= Tr
q p 2 i = p j =1 k =1 q j q p
qk
+

q& j q& k +

2Ti
Ti T
1 n i i

Tr
J

i
2 i =1 j =1 k =1 qk q p
q j

Cui cng ta c lc tng qut ca khu p :

q& j q& k + mi g Ti i ri

q p
i= p

Fp =

(7.29)

L
d L

dt q& p q p

Thay th cc ch s p v i thnh i v j, ta s c :
j
j
n
n

2T j
T jT
T j j
q&&k + Tr
&
&

J
q
q
m
m
g
rj

k
j
j

q
q
q
q

=
=
1
=
1
=
j
i
j
i
k
m
i
i
k
m

(7.30)
Vi mt robot c n bc t do th :
j
n
T j
T jT
Fi = Tr
Jj
qi
j =i k =1
qk

TS. Phm ng Phc

91

Robot cng nghip

q = [q1, q2, . . . ,qn]T

q& = [q& 1 , q& 2 , ... , q& n ] T
F = F[F1, F2, . . . , Fn]T

cho gn, ta biu din :

F = J ( q) q&& + C ( q, q& ) q& + G ( q)

(7.31)

Trong :
J th hin tc dng ca qun tnh, l mt ma trn i xng (n x n);
C th hin tc dng ca lc ly tm v Cariolis, l mt vect (n x 1);
G th hin tc dng ca lc trng trng, cng l mt vect (n x 1).
y l phng trnh ng lc hc ca robot.
Nu thm vo phng trnh trn cc tc dng khc nh : FEX c trng cho cc
ngoi lc tc dng ln trc, V c trng cho hiu ng ma st, ta c :

F = J ( q) q&& + C ( q, q& ) q& + G ( q) + V ( q& ) + FEX

TS. Phm ng Phc

(7.32)

92

Robot cng nghip

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

93

Robot cng nghip

Qu o cn thit k nht thit phi i qua mt s im nt cho trc (t

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

x2
x1
xf-1
xo

Cc im nt

xf
to

t1

t2

tf-1

tf

Hnh 8.1. Tnh lin tc ca qu o robot.

thit k qu o robot, ngi ta thng dng phng php xp x
cc a thc bc n, cc qu o thng gp l :
+ Qu o CS (Cubic Segment) : Tng ng a thc bc 3;
+ Qu o LS (linear Segment) : Tng ng a thc bc 1;
+ Qu o LSPB (Linear Segment with Parabolic Blend) : Phi hp a
thc bc 2 vi a thc bc 1.
q0

on thng

q2

q1

qf

ng cong bc 2

Hnh 8.2 : Qu o LSPB

TS. Phm ng Phc

94

Robot cng nghip

+ Qu o BBPB (Bang Bang Parabolic Blend) : l trng hp c bit

ca qu o LSPB khi on tuyn tnh thu v bng 0 v xut hin im
un.
qf

q0

Hnh 8.2 : Qu o BBPB

Nu cho trc nhiu im nt, ta c th p dng nhiu dng qu o c
bn khc nhau cho mt bin khp.
8.2. Qu o a thc bc 3 :
Khi thit k qu o robot theo a thc bc 3 qua cc im nt, mi
on qu o gia hai im nt s c biu din bng mt phng trnh bc
3 ring bit. Qu o a thc bc 3 m bo s lin tc ca o hm bc nht
v bc hai ti cc im nt.
Ti thi im tk t tk+1, qu o xp x a thc bc 3 ca bin khp
th i l qi(t) c dng :
(8.1)
qi(t) = ai + bi(t - tk) + ci(t - tk)2 + di(t - tk)3
qi(t)

Vi cc rng buc :
qi(tk) = qk v q& i (t k ) = q& k
qi(tk+1) = qk+1 v q& i (t k +1 ) = q& k +1

qk+1
qk

Bc 3

tk+1
tk
(8.2)
T (8.1) ta thy : t = tk ai = qk
Ly o hm ca (8.1) theo t, ta c :
q& i (t) = b i + 2c i (t t k ) + 3d i (t t k ) 2
(8.3)
Ti : t = tk b i = q& k
Ti t = ti+1 ta c hai tham s :
3(q k +1 q k ) (2q& k + q& k +1 ) t k
ci =
(8.4)
t 2k
(q& + q& k ) t k 2(q k +1 q k )
d i = k +1
(8.5)
t 3k
Trong : t k = t k +1 t k
Cc phng trnh (8.4) v (8.5) nhn c khi gii (8.1) ... (8.3).
Tnh lin tc ca vn tc l s m bo cho qu o khng gp khc,
git cc, gy sc trong qu trnh hot ng ca robot. Vn tc v gia tc ti
im cui ca mt on ng cong bc 3 chnh bng vn tc v gia tc ca
on cong bc 3 tip theo.
Cn ch rng khi thit k qu o trong khng gian ct, iu
khin c robot, mi thi im u phi tm c nghim ca bi ton
ng hc ngc. V vy yu cu "no b" ca robot (my tnh) phi thc hin
TS. Phm ng Phc

95

Robot cng nghip

mt khi lng cc php tnh khng l trong mt khong thi gian rt ngn
(vi chc microgiy) m bo thi gian thc khi robot hot ng. Nu ta
khng tm cch ci bin thit k qu o th rt kh m bo yu cu ny.
* V d v thit k qu o CS:
Thit k qu o CS (Path with Cubic segment) ca khp th i i qua
hai im nt c gi tr q0 v qf. Vi cc rng buc q&0 = 0 ; q& f = 0 .
T cc cng thc (8.2) . . . (8.5) ta xc nh cc h s ca a thc bc 3
nh sau :
ai = q0 ; bi = 0;
3(q f q 0 )
- 2(q f q 0 )
ci =
di =
V
2
(t f t 0 )
(t f t 0 ) 3
Do vy qu o qi(t) c dng nh sau :
3(q f q 0 )
2(q f q 0 )
2
q i (t) = q 0 +
t
t
(
)

( t t0 ) 3
0
3
2
(t f t 0 )
(t f t 0 )
6(q f q 0 )
6(q f q 0 )
( t t0 )
( t t0 ) 2
Vn tc l : q& i (t) =
3
2
(t f t 0 )
(t f t 0 )
6(q f q 0 ) 12(q f q 0 )
&q&i (t) =

( t t0 )
V gia tc l :
(t f t 0 ) 2
(t f t 0 ) 3
Trong v d trn, gi s thi gian t0 = 0 v tf = 1 giy, th :
qi(t) = q0 + 3(qf - q0) t2 - 2(qf - q0) t3
qf
q(t)

Qu o

q0
O

t
tf

t0
q& (t)

q& 0 = q& f = 0

tf

t0

6(q f q 0 )
(t f t 0 ) 2

Tc

q&&(t)

Gia tc
t
t0

tf

6(q f q 0 )
(t f t 0 ) 2

Hnh 8.3. Thit k qu o CS

TS. Phm ng Phc

96

Robot cng nghip

T cc phng trnh qu o, phng trnh vn tc v phng trnh gia

tc ta xy dng c cc biu c tnh chuyn ng ca khp th i trn
on qu o thit k.
8.3. Qu o tuyn tnh vi cung hai u l parabol (LSPB) :
Khi yu cu cng c gn trn khu chp hnh cui ca robot chuyn
ng vi vn tc u n, ta dng qu o LSPB.
qi(t)

v = constant
d

(q0+qf)/2
Parabol c
O
t0 tb

Parabol
e

t
tf/2

tf - tb

tf

Hnh 8.3. Qu o LSPB.

Cc iu kin lin tc ca qu o ny th hin :
q(to) = q0 ; q(tf) = qf; v q& (t0 ) = q& (t f ) = 0
v iu kin cng ngh l v = constant.
Qu o c chia lm 3 on :
a/ Trong on 1 : 0 t tb qu o Parabol c dng :
(8.6)
qi(t) = + t + t2
= q(t0) = q0
(8.7)
Khi t = 0 th
(8.8)
Ly o hm (8.6) : q& (t) = + 2 t
Khi t = 0 th
= q&(to ) = 0
Ti thi im tb ta cn c vn tc bng hng s vn tc cho trc v :
= v/2tb
Nn khi t = tb
t v/tb = a = a/2 v qu o c dng :
(0 t tb)
(8.9)
qi(t) = q0 + at2/2
b/ Trong on 2 : [tb, (tf-tb)] qu o tuyn tnh c dng :
qi(t) = 0 + vt
t
(q + q f )
Do tnh i xng : q( f ) = 0
2
2
(q 0 + q f )
t
Suy ra
= 0 + v f
2
2
(q 0 + q f vt f )
0 =
Vy
2
Phng trnh qu o tuyn tnh s l :
TS. Phm ng Phc

97

Robot cng nghip

q f + q 0 vt f
+ vt
2
T iu kin lin tc v v tr, ti thi im tb ta c :
at 2b q f + q 0 vt f
q0 +
=
+ vt b
2
2
Rt ra :
q q f + vt f
tb = 0
v
Vi iu kin tn ti : 0 < tb tf/2, dn n :
q i (t) =

(8.10)

qf q0
2(q f q 0 )
< tf
v
v
iu ny xc nh vn tc phi nm gia cc gii hn trn, nu khng
chuyn ng s khng thc hin c.
V mt vt l :
Nu tf > (qf - q0) / v v tf 2(qf - q0) / v
qf
th : v > (qf - q0) / tf v v 2(qf - q0) / tf.
q0

Ngha l tg < v tg2.

t0

tf

c/ Trong on 3 : (tf - tb) t tf qu o Parabol c dng :

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

qi(t); q& (t); q&& (t)

i
i

q0
t0

tb

tf-tb
v = const

t0

tb

tf-tb

tf

q& (t)
i
tf
&q& (t)
i

t0

tb

tf-tb

tf

Hnh 8.4 : c tnh qu o LSPB

TS. Phm ng Phc

98

Robot cng nghip

8.4. Qu o Bang Bang Parabolic blend (BBPB) :

Nh trnh by trn, y l trng hp c bit ca qu o LSPB
khi on tuyn tnh thu v 0.
tf
at 2
qi(t) = q0 +
Vi :
0t
2
2
q q 0 at 2
tf
t tf
qi(t) = 2q0 - qf +2a f
v vi
t2
a
2
th c tnh ca qu o ny nh sau :
qi(t)
qf
q0
t0
q& (t)
i

t
tf/2

tf

Vmax
t

t0

tf/2

tf

&q& (t)
i

t0

tf/2

t
tf

Hnh 8.5. c tnh qu o BBPB

=======================

TS. Phm ng Phc

99

robot cng nghip

Chng 9
Truyn ng v iu khin robot.
9.1. Truyn ng in trong robot:
Truyn ng in c dng kh nhiu trong k thut robot, v c nhiu
u im nh l iu khin n gin khng phi dng cc b bin i ph, khng
gy bn mi trng, cc loi ng c in hin i c th lp trc tip trn cc
khp quay...
Tuy nhin so vi truyn ng thu lc hoc thu kh th truyn ng in
c cng sut thp v thng thng phi cn thm hp gim tc v thng cc
khu ca robot chuyn ng vi tc thp.
Trong k thut robot, v nguyn tc c th dng ng c in cc loi
khc nhau, nhng trong thc t ch c hai loi c dng nhiu hn c. l
ng c in mt chiu v ng c bc.
Ngy nay, do nhng thnh tu mi trong nghin cu iu khin ng c
in xoay chiu, nn cng c xu hng chuyn sang s dng ng c in xoay
chiu trnh phi trang b thm b ngun in mt chiu. Ngoi ra, loi ng
c in mt chiu khng chi gp (DC brushless motor) cng bt u c ng
dng vo k thut robot.
9.1.1. ng c in mt chiu :
ng c in mt chiu gm c hai phn :
+ Stato c nh vi cc cun dy c dng in cm hoc dng nam chm
vnh cu. Phn ny cn c gi l phn cm. Phn cm to nn t thng trong
khe h khng kh.
+ Roto vi cc thanh dn. Khi c dng in mt chiu chy qua v vi
dng t thng xc nh, roto s quay. Phn ny gi l phn ng.
Tu cch u dy gia phn cm so vi phn ng, ta c nhng loi ng
c in mt chiu khc nhau :
+ ng c kch t ni tip (Hnh 9.1.a);
+ ng c kch t song song (Hnh 9.1.b);
+ ng c kch t hn hp (Hnh 9.1.c).

a/

b/
Hnh 9.1. Cc loi ng c in mt chiu.

TS. Phm ng Phc

c/

100

robot cng nghip

Cc thng s ch yu quyt nh tnh nng lm vic ca ng c in mt

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

Mun iu chnh tc ng c in mt chiu c th thc hin bng

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

'

N
S

'

'

Hnh 9.2 : S nguyn l hot ng ca ng c bc.

TS. Phm ng Phc

'

robot cng nghip

101

Nu cp in cho cun dy ' th roto s dng v tr m dng t qua

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

Tc quay v tn s xung :
Tc quay ca ng c bc ph thuc vo s bc trong mt giy. i
vi hu ht cc ng c bc, s xung cp cho ng c bng s bc (tnh theo
pht) nn tc c th tnh theo tn s xung f. Tc quay ca ng c bc
tnh theo cng thc sau :
60 f
n=
(f : bc/pht)/(s : bc /vng)
s
Tong : n - tc quay (vng/pht)
f - tn s xung (Hz)
s - S bc trong mt vng quay.
TS. Phm ng Phc

robot cng nghip

102

Ngoi ra cn cc thng s quan trng khc nh chnh xc v tr,

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

103

robot cng nghip

loi van iu chnh, ng ng ... lm h truyn ng cho robot kh cng knh

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

(9.1)

Qui lut ny xc nh mt h iu khin phn hi v c gi l h iu

khin t l sai lch.
TS. Phm ng Phc

104

robot cng nghip

9.3.2. iu khin t l - o hm (PD : Propotional Derivative):

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

Trong :

(9.2)

- sai s v tr ca khp ng. = d - (t).

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

9.3.3. iu khin t l - tch phn - o hm

(PID : Propotional Integral Derivative):
H thng vi cu trc lut iu khin PD vn cn mt s nhc im,
khng ph hp vi mt s loi robot. Mt h thng iu khin khc c b sung
thm tn hiu tc t & d v sai lch tc & = & d & (t) tc ng vo khu
khuych i Kd. Phng trnh lc tc ng ln khp ng c dng :
t

F = K e + K d & + K i (t)dt

(9.3)

Vi

& - sai s tc . & = & d & (t) .

Nh vy, tu theo cu trc la chn ca b iu khin, ta em i

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

105

robot cng nghip

La

Ra

+
eb(t)

Lf

Ua(t)
_

+
Uf

Rt

ia(t)

m
_
Mm

Jm

Hnh 9.3. S ng c in mt chiu.

Trong thc t, trc ng c c ni vi hp gim tc ri ti trc ph ti
nh hnh 9.4. Gi n l t s truyn, L l gc quay ca trc ph ti, ta c :
L(t) = n m(t)
& L (t) = n & m (t )
(9.4)
&& (t) = n && (t )
L

ML
JL

Mm
fL

Jm
m= L/n
fm

Hnh 9.4. S ng c in cng ph ti.

Mmen trn trc ng c bng tng momen cn ng c quay, cng
vi mmen ph ti quy v trc ng c.

K hiu :

M(t) = M m (t) + M *L (t)

Jm : Mmen qun tnh ca ng c.
JL : Momen qun tnh ph ti.

Ta c :

M m (t) = J m&&m (t) + f m&m (t)

M (t) = J && (t) + f & (t)
L

(9.5)

(9.6)
(9.7)

Trong fm v fL l h s cn ca ng c v ca ph ti.
Theo nh lut bo tn nng lng, cng do ph ti sinh ra, tnh trn trc
ph ti l MLL phi bng cng quy v trc ng c M *L m . T ta c :

TS. Phm ng Phc

106

robot cng nghip

M L (t) L (t)
= nM L (t)
m (t)
Thay (9.1) v (9.4) vo cng thc trn :
M *L (t) =

M *L (t) = n 2 [J L&&m (t) + f L & m (t)]

Thay (9.3) v (9.6) vo (9.2) ta c :
M(t) = ( J m + n 2 J L )&&m (t) + f m n 2 f L )& m (t)
M(t) = J&& (t) + f & (t)
Hay :
m

(9.8)

(9.9)

(9.10)

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

s J + sf
Thay (9.14) vo (9.15) :
U (s) - sK b m ( s )
m (s) = K a 2 a

(s J + sf)(R a + sLa )
Vi

TS. Phm ng Phc

U a (s) - sK b m ( s ) (s 2 J + sf)(R a + sLa )

=
m (s)
Ka

(9.12)

(9.13)

(9.14)

(9.15)

107

robot cng nghip

U a (s) (s 2 J + sf)(R a + sL a ) + sK a K b
=
m (s)
Ka
m (s)
Ka
=
U a (s) s[(sJ + f)(R a + sL a ) + K a K b ]

Hay :

(9.16)

y l hm truyn cn xc nh, n l t s gia tn hiu ra (gc quay m)

v tn hiu vo ca h thng (in p Ua). V h thng gm c ng c v ph ti
nn tn hiu ra thc t l gc quay ca trc ph ti L, do hm truyn chuyn
ng 1 bc t do ca tay my l :

L (s)
nK a
=
U a (s) s[(R a + sL a )(sJ + f) + K a K b ]

(9.17)

v ta c s khi tng ng vi hm truyn trn l :

Ua(s)
+ _

Ka

sLa+ Ra

1
sJ + f

1
s

L(s)

Kb

Hnh 9.5 : S khi hm truyn chuyn ng mt bc t do.

Trong cng thc (9.17) c th b qua thnh phn in cm phn ng La, v
n thng qu nh so vi cc nhn t nh hng c kh khc. Nn :

L (s)
nK a
=
U a (s) s(sR a J + R a f + K a K b )

(9.18)

9.3.6. iu khin v tr mi khp ng :

Mc ch ca iu khin v tr l lm sao cho ng c chuyn dch khp
ng i mt gc bng gc quay tnh ton m bo qu o chn trc
(chng 8). Vic iu khin c thc hin nh sau : Theo tn hiu sai lch gia
gi tr thc t v gi tr tnh ton ca v tr gc m iu chnh in p Ua(t) t
vo ng c. Ni cch khc, iu khin ng c theo qu o mong mun
phi t vo ng c mt in p t l thun vi sai lch gc quay ca khp
ng.

Trong

~
K p e(t) K p ( L (t) L (t))
U a (t) =
=
n
n
Kp : h s truyn tn hiu phn hi v tr.

TS. Phm ng Phc

(9.19)

108

robot cng nghip

~
e(t) = L (t) L (t) : sai lch gc quay.
~
Gi tr gc quay tc thi : L (t) c o bng cm bin quang hc hoc
chit p. Bin i Laplace phng trnh (9.18) :
~
K p ( L (s) L (s)) K p E(s)
U a (s) =
=
n
n

(9.20)

Thay (9.20) vo phng trnh (9.18) :

Ka Kp
L (s)
=
= G(s)
E(s) s(sR a J + R a f + K a K b )

(9.21)

Sau khi bin i i s ta c hm truyn :

Ka Kp
L (s)
G(s)
=
= 2
=
~
L (s) 1 + G(s) s R a J + s(R a f + K a K b ) + K a K b

Ka Kp / R aJ
(9.22)
(R a f + K a K b )
Ka Kb
2
s +
s+
R aJ
R aJ
Phng trnh (9.22) cho thy rng h iu khin t l ca mt khp ng l
mt h bc hai, n s lun n nh nu cc h s ca ca phng trnh bc hai l
nhng s dng. nng cao c tnh ng lc hc v gim sai s trng thi n
nh ca h ngi ta c th tng h s phn hi v tr Kp v kt hp lm gim dao
ng trong h bng cch thm vo thnh phn o hm ca sai s v tr. Vi vic
thm phn hi ny, in p t ln ng c s t l tuyn tnh vi sai s v tr v
o hm ca n :
~
~
K p ( L (t) L (t)) + K v ( & L (t) & L (t)) K p e(t) + K v e& (t)
(9.23)
U a (t) =
=
n
n
Trong Kv l h s phn hi ca sai s v vn tc.
Vi phn hi nu trn, h thng tr thnh khp kn v c hm truyn nh
th hin trn s khi hnh (9.6). y l phng php iu khin t l - o
hm.
L(s)

Kp+ sKv
_

1
n

Ua(s)
+ _

1
sLa+ Ra

Ka

1
sJ + f

1
s

L(s)

Kb

Hnh 9.6 : S khi iu khin chuyn dch mt khp ng c lin h phn hi.
TS. Phm ng Phc

109

robot cng nghip

Bin i Laplace phng trnh (9.23) v thay Ua(s) vo (9.21) ta c :

K a (K p + sK v )
K a K vs + K a K p
L (s)
=
=
= G(s)
E(s) s(sR a J + R a f + K a K b ) s(sR a J + R a f + K a K b )

(9.24)

T ta c :
K a (K v s + K p )
L (s)
G(s)
=
=
~
L (s) 1 + G(s) s 2 R a J + s(R a f + K a K b + K a K v ) + K a K p

(9.25)

TS. Phm ng Phc