Robot công nghiệp : Phương trình động học của Robot
Bất kỳ một robot nào cũng có thể coi là một tập hợp các khâu (links) gắn liền với các khớp (joints)
Robot c«ng nghiÖp 27
Ch−¬ng III
ph−¬ng tr×nh ®éng häc cña robot
(Kinematic Equations)
3.1. DÉn nhËp :
BÊt kú mét robot nµo còng cã thÓ coi lµ mét tËp hîp c¸c kh©u (links) g¾n liÒn víi c¸c
khíp (joints). Ta h·y ®Æt trªn mçi kh©u cña robot mét hÖ to¹ ®é. Sö dông c¸c phÐp biÕn ®æi
thuÇn nhÊt cã thÓ m« t¶ vÞ trÝ t−¬ng ®èi vµ h−íng gi÷a c¸c hÖ to¹ ®é nÇy. Denavit. J. ®· gäi
biÕn ®æi thuÇn nhÊt m« t¶ quan hÖ gi÷a mét kh©u vµ mét kh©u kÕ tiÕp lµ mét ma trËn A. Nãi
®¬n gi¶n h¬n, mét ma trËn A lµ mét m« t¶ biÕn ®æi thuÇn nhÊt bëi phÐp quay vµ phÐp tÞnh tiÕn
t−¬ng ®èi gi÷a hÖ to¹ ®é cña hai kh©u liÒn nhau. A1 m« t¶ vÞ trÝ vµ h−íng cña kh©u ®Çu tiªn; A2
m« t¶ vÞ trÝ vµ h−íng cña kh©u thø hai so víi kh©u thø nhÊt. Nh− vËy vÞ trÝ vµ h−íng cña kh©u
thø hai so víi hÖ to¹ ®é gèc ®−îc biÓu diÔn bëi ma trËn :
T2 = A1.A2
Còng nh− vËy, A3 m« t¶ kh©u thø ba so víi kh©u thø hai vµ :
T3 = A1.A2.A3 ; v.v...
Còng theo Denavit, tÝch cña c¸c ma trËn A ®−îc gäi lµ ma trËn T, th−êng cã hai chØ sè:
trªn vµ d−íi. ChØ sè trªn chØ hÖ to¹ ®é tham chiÕu tíi, bá qua chØ sè trªn nÕu chØ sè ®ã b»ng 0.
ChØ sè d−íi th−êng dïng ®Ó chØ kh©u chÊp hµnh cuèi. NÕu mét robot cã 6 kh©u ta cã :
T6 = A1.A2.A3.A4.A5.A6 (3.1)
T6 m« t¶ mèi quan hÖ vÒ h−íng vµ vÞ trÝ cña kh©u chÊp hµnh cuèi ®èi víi hÖ to¹ ®é gèc.
Mét robot 6 kh©u cã thÓ cã 6 bËc tù do vµ cã thÓ ®−îc ®Þnh vÞ trÝ vµ ®Þnh h−íng trong tr−êng
vËn ®éng cña nã (range of motion). Ba bËc tù do x¸c ®Þnh vÞ trÝ thuÇn tuý vµ ba bËc tù do kh¸c
x¸c ®Þnh h−íng mong muèn. T6 sÏ lµ ma trËn tr×nh bµy c¶ h−íng vµ vÞ trÝ cña robot. H×nh 3.1
m« t¶ quan hÖ ®ã víi bµn tay m¸y. Ta ®Æt gèc to¹ ®é cña hÖ m« t¶ t¹i ®iÓm gi÷a cña c¸c ngãn
tay. Gèc to¹ ®é nÇy ®−îc m« t¶ bëi vect¬ p (x¸c ®Þnh vÞ trÝ cña bµn tay). Ba vect¬ ®¬n vÞ m« t¶
h−íng cña bµn tay ®−îc x¸c ®Þnh nh− sau :
n
p
a o
H×nh 3.1 : C¸c vect¬ ®Þnh vÞ trÝ vµ ®Þnh h−íng cña bµn tay m¸y
TS. Ph¹m §¨ng Ph−íc
Robot c«ng nghiÖp 28
∗ Vect¬ cã h−íng mµ theo ®ã bµn tay sÏ tiÕp cËn ®Õn ®èi t−îng, gäi lµ vect¬ a
(approach).
∗ Vect¬ cã h−íng mµ theo ®ã c¸c ngãn tay cña bµn tay n¾m vµo nhau khi cÇm n¾m
®èi t−îng, gäi lµ vect¬ o (Occupation).
∗ Vect¬ cuèi cïng lµ vect¬ ph¸p tuyÕn n (normal), do vËy ta cã :
r r r
n=oxa
ChuyÓn vÞ T6 nh− vËy sÏ bao gåm c¸c phÇn tö :
nx Ox ax px
T6 = ny Oy ay py (3.2)
nz Oz az pz
0 0 0 1
Tæng qu¸t, ma trËn T6 cã thÓ biÓu diÔn gän h¬n nh− sau :
Ma trËn ®Þnh h−íng R Vect¬ vÞ trÝ p (3.3)
T6 =
0 0 0 1
Ma trËn R cã kÝch th−íc 3x3, lµ ma trËn trùc giao biÓu diÔn h−íng cña bµn kÑp (kh©u
chÊp hµnh cuèi) ®èi víi hÖ to¹ ®é c¬ b¶n. ViÖc x¸c ®Þnh h−íng cña kh©u chÊp hµnh cuèi cßn
cã thÓ thùc hiÖn theo phÐp quay Euler hay phÐp quay Roll, Pitch, Yaw.
r
Vect¬ ®iÓm p cã kÝch th−íc 3x1, biÓu diÔn mèi quan hÖ täa ®é vÞ trÝ cña cña gèc hÖ
täa ®é g¾n trªn kh©u chÊp hµnh cuèi ®èi víi hÖ to¹ ®é c¬ b¶n.
3.2. Bé th«ng sè Denavit-Hartenberg (DH) :
Mét robot nhiÒu kh©u cÊu thµnh tõ c¸c kh©u nèi tiÕp nhau th«ng qua c¸c khíp ®éng.
Gèc chuÈn (Base) cña mét robot lµ kh©u sè 0 vµ kh«ng tÝnh vµo sè c¸c kh©u. Kh©u 1 nèi víi
kh©u chuÈn bëi khíp 1 vµ kh«ng cã khíp ë ®Çu mót cña kh©u cuèi cïng. BÊt kú kh©u nµo
còng ®−îc ®Æc tr−ng bëi hai kÝch th−íc :
§é dµi ph¸p tuyÕn chung : an .
Gãc gi÷a c¸c trôc trong mÆt ph¼ng vu«ng gãc víi an : αn.
Khíp n Khíp n+1
Kh©u n
αn
a
H×nh 3.5 : ChiÒu dµi vµ gãc xo¾n cña 1 kh©u.
Th«ng th−êng, ng−êi ta gäi an lµ chiÒu dµi vµ αn lµ gãc xo¾n cña kh©u (H×nh 3.5). Phæ
biÕn lµ hai kh©u liªn kÕt víi nhau ë chÝnh trôc cña khíp (H×nh 3.6).
TS. Ph¹m §¨ng Ph−íc
Robot c«ng nghiÖp 29
Khíp n-1 Khíp n Khíp n+1
θn θn+1
θn-1
Kh©u n
Kh©u n-1 Kh©u n+1
Kh©u n-2 zn
αn
an
dn zn-1 xn
On
xn-1
θn
H×nh 3.6 : C¸c th«ng sè cña kh©u : θ, d, a vµ α.
Mçi trôc sÏ cã hai ph¸p tuyÕn víi nã, mçi ph¸p tuyÕn dïng cho mçi kh©u (tr−íc vµ sau
mét khíp). VÞ trÝ t−¬ng ®èi cña hai kh©u liªn kÕt nh− thÕ ®−îc x¸c ®Þnh bëi dn lµ kho¶ng c¸ch
gi÷a c¸c ph¸p tuyÕn ®o däc theo trôc khíp n vµ θn lµ gãc gi÷a c¸c ph¸p tuyÕn ®o trong mÆt
ph¼ng vu«ng gãc víi trôc.
dn vµ θn th−êng ®−îc gäi lµ kho¶ng c¸ch vµ gãc gi÷a c¸c kh©u.
§Ó m« t¶ mèi quan hÖ gi÷a c¸c kh©u ta g¾n vµo mçi kh©u mét hÖ to¹ ®é. Nguyªn
t¾c chung ®Ó g¾n hÖ täa ®é lªn c¸c kh©u nh− sau :
+ Gèc cña hÖ to¹ ®é g¾n lªn kh©u thø n ®Æt t¹i giao ®iÓm cña ph¸p tuyÕn an víi trôc
khíp thø n+1. Tr−êng hîp hai trôc khíp c¾t nhau, gèc to¹ ®é sÏ ®Æt t¹i chÝnh ®iÓm c¾t ®ã. NÕu
c¸c trôc khíp song song víi nhau, gèc to¹ ®é ®−îc chän trªn trôc khíp cña kh©u kÕ tiÕp, t¹i
®iÓm thÝch hîp.
+ Trôc z cña hÖ to¹ ®é g¾n lªn kh©u thø n ®Æt däc theo trôc khíp thø n+1.
+ Trôc x th−êng ®−îc ®Æt däc theo ph¸p tuyÕn chung vµ h−íng tõ khíp n ®Õn n+1.
r r
Trong tr−êng hîp c¸c trôc khíp c¾t nhau th× trôc x chän theo tÝch vect¬ z n x z n-1 .
Tr−êng hîp khíp quay th× θn lµ c¸c biÕn khíp, trong tr−êng hîp khíp tÞnh tiÕn th× dn
lµ biÕn khíp vµ an b»ng 0.
C¸c th«ng sè an, αn, dn vµ θn ®−îc gäi lµ bé th«ng sè DH.
VÝ dô 1 : XÐt mét tay m¸y cã hai kh©u ph¼ng nh− h×nh 3.7 : y2
x2
O2
y1 z2
θ2
y0 a1 x1 a
2
θ1 z1
O1
O0
x0
z0
H×nh 3.7 : Tay m¸y cã hai kh©u ph¼ng (vÞ trÝ bÊt kú).
TS. Ph¹m §¨ng Ph−íc
Robot c«ng nghiÖp 30
Ta g¾n c¸c hÖ to¹ ®é lªn c¸c kh©u nh− h×nh vÏ : trôc z0, z1 vµ z2 vu«ng gãc víi tê giÊy.
HÖ to¹ ®é c¬ së lµ O0x0y0z0, chiÒu cña x0 h−íng tõ O0 ®Õn O1. Sau khi thiÕt lËp hÖ to¹ ®é c¬ së,
HÖ to¹ ®é o1x1y1z1 cã h−íng nh− h×nh vÏ, O1 ®Æt t¹i t©m trôc khíp 2. HÖ to¹ ®é O2x2y2x2 cã gèc
O2 ®Æt ë ®iÓm cuèi cña kh©u 2.
B¶ng th«ng sè Denavit-Hartenbert cña tay m¸y nÇy nh− sau :
Kh©u θi αi ai di
1 θ1 * 0 a1 0
2 θ2 * 0 a2 0
Trong ®ã θi lµ c¸c biÕn khíp (dïng dÊu * ®Ó ký hiÖu c¸c biÕn khíp).
VÝ dô 2 : Xem s¬ ®å robot SCARA cã 4 kh©u nh− h×nh 3.8 :
§©y lµ robot cã cÊu h×nh kiÓu RRTR, bµn tay cã chuyÓn ®éng xoay xung quanh trôc
®øng. HÖ to¹ ®é g¾n lªn c¸c kh©u nh− h×nh vÏ.
a2
z0 z1
θ1 θ2
O x0 x2
0 O1
x1 O2
d3 z2 x
3
a1
O3
d4
O4 x
θ4
z3 , z4
H×nh 3.8 : Robot SCARA vµ c¸c hÖ to¹ ®é (vÞ trÝ ban ®Çu).
§èi víi tay m¸y nÇy c¸c trôc khíp ®Òu song song nhau, ®Ó tiÖn lîi tÊt c¶ c¸c gèc to¹ ®é
®Æt t¹i t©m c¸c trôc khíp. Trôc x0 n»m trong mÆt ph¼ng tê giÊy. C¸c hÖ to¹ ®é kh¸c nh− h×nh
vÏ. B¶ng th«ng sè DH cña robot SCARA nh− sau :
Kh©u θi αi ai di
1 θ1 * 0 a1 0
2 θ2 * 1800 a2 0
3 0 0 0 d3*
4 θ4 * 0 0 d4
* : C¸c biÕn khíp.
3.3. §Æc tr−ng cña c¸c ma trËn A :
Trªn c¬ së c¸c hÖ to¹ ®é ®· Ên ®Þnh cho tÊt c¶ c¸c kh©u liªn kÕt cña robot, ta cã thÓ
thiÕt lËp mèi quan hÖ gi÷a c¸c hÖ to¹ ®é nèi tiÕp nhau (n-1), (n) bëi c¸c phÐp quay vµ tÞnh tiÕn
sau ®©y :
Quay quanh zn-1 mét gãc θn
TÞnh tiÕn däc theo zn-1 mét kho¶ng dn
TÞnh tiÕn däc theo xn-1 = xn mét ®o¹n an
Quay quanh xn mét gãc xo¾n αn
TS. Ph¹m §¨ng Ph−íc
Robot c«ng nghiÖp 31
Bèn phÐp biÕn ®æi thuÇn nhÊt nÇy thÓ hiÖn quan hÖ cña hÖ to¹ ®é thuéc kh©u thø n so
víi hÖ to¹ ®é thuéc kh©u thø n-1 vµ tÝch cña chóng ®−îc gäi lµ ma trËn A :
An = Rot(z,θ) Trans(0,0,d) Trans(a,0,0) Rot(x,α) (3.4)
cosθ -sinθ 0 0 1 0 0 a 1 0 0 0
An = sinθ cosθ 0 0 0 1 0 0 0 cosα -sinα 0
0 0 1 0 0 0 1 d 0 sinα cosα 0
0 0 0 1 0 0 0 1 0 0 0 1
cosθ -sinθ cosα sinθ sinα a cosθ
An = sinθ cosθ cosα -cosθ sinα a sinθ (3.5)
0 sinα cosα d
0 0 0 1
§èi víi khíp tÞnh tiÕn (a = 0 vµ θi = 0) th× ma trËn A cã d¹ng :
1 0 0 0
An = 0 cosα - sinα 0 (3.6)
0 sinα cosα d
0 0 0 1
§èi víi mét kh©u ®i theo mét khíp quay th× d, a vµ α lµ h»ng sè. Nh− vËy ma trËn A
cña khíp quay lµ mét hµm sè cña biÕn khíp θ.
§èi víi mét kh©u ®i theo mét khíp tÞnh tiÕn th× θ, α lµ h»ng sè. Ma trËn A cña khíp
tÞnh tiÕn lµ mét hµm sè cña biÕn sè d.
NÕu c¸c biÕn sè ®−îc x¸c ®Þnh th× gi¸ trÞ cña c¸c ma trËn A theo ®ã còng ®−îc x¸c
®Þnh.
3.4. X¸c ®Þnh T6 theo c¸c ma trËn An :
Ta ®· biÕt : T6 = A1A2A3A4A5A6
Trong ®ã T6 ®−îc miªu t¶ trong hÖ to¹ ®é gèc (hÖ to¹ ®é g¾n víi kh©u c¬ b¶n cè ®Þnh
cña robot). NÕu m« t¶ T6 theo c¸c hÖ to¹ ®é trung gian thø n-1 th× :
6
n −1
T 6
= ∏ i=n
Ai
Trong tr−êng hîp tæng qu¸t, khi
xÐt quan hÖ cña robot víi c¸c thiÕt bÞ
kh¸c, nÕu hÖ to¹ ®é c¬ b¶n cña robot cã
liªn hÖ víi mét hÖ to¹ ®é nµo ®ã bëi phÐp OR
biÕn ®æi Z, Kh©u chÊp hµnh cuèi l¹i cã Z
g¾n mét c«ng cô, cã quan hÖ víi vËt thÓ E
T6 X
bëi phÐp biÕn ®æi E (h×nh 3.9) th× vÞ trÝ vµ A
h−íng cña ®iÓm cuèi cña c«ng cô, kh¶o
s¸t ë hÖ to¹ ®é tham chiÕu m« t¶ bëi X sÏ H×nh 3.9 : VËt thÓ vµ Robot
®−îc x¸c ®Þnh bëi :
X= Z T6E
TS. Ph¹m §¨ng Ph−íc
Robot c«ng nghiÖp 32
Quan hÖ nÇy ®−îc thÓ hiÖn trªn to¸n ®å sau :
Z O0 A1 A2 A3 A4 A5 E A X
OR 5 OR
T6
4
T6
3
T6
2
T6
1
T6
T6
H×nh 3.10 : To¸n ®å chuyÓn vÞ cña robot.
Tõ to¸n ®å nÇy ta cã thÓ rót ra : T6 = Z-1 X E-1
-1 -1
(Z vµ E lµ c¸c ma trËn nghÞch ®¶o).
3.5. Tr×nh tù thiÕt lËp hÖ ph−¬ng tr×nh ®éng häc cña robot :
§Ó thiÕt lËp hÖ ph−¬ng tr×nh ®éng häc cña robot, ta tiÕn hµnh theo c¸c b−íc sau :
1. Chän hÖ to¹ ®é c¬ së, g¾n c¸c hÖ to¹ ®é më réng lªn c¸c kh©u.
ViÖc g¾n hÖ to¹ ®é lªn c¸c kh©u ®ãng vai trß rÊt quan träng khi x¸c lËp hÖ ph−¬ng
tr×nh ®éng häc cña robot, th«ng th−êng ®©y còng lµ b−íc khã nhÊt. Nguyªn t¾c g¾n hÖ to¹ ®é
lªn c¸c kh©u ®· ®−îc tr×nh bµy mét c¸ch tæng qu¸t trong phÇn 3.5. Trong thùc tÕ, c¸c trôc
khíp cña robot th−êng song song hoÆc vu«ng gãc víi nhau, ®ång thêi th«ng qua c¸c phÐp biÕn
®æi cña ma trËn A ta cã thÓ x¸c ®Þnh c¸c hÖ to¹ ®é g¾n trªn c¸c kh©u cña robot theo tr×nh tù
sau :
+ Gi¶ ®Þnh mét vÞ trÝ ban ®Çu(♦) (Home Position) cña robot.
+ Chän gèc to¹ ®é O0, O1, ...
+ C¸c trôc zn ph¶i chän cïng ph−¬ng víi trôc khíp thø n+1.
+ Chän trôc xn lµ trôc quay cña zn thµnh zn+1 vµ gãc cña zn víi zn+1 chÝnh lµ αn+1. NÕu zn
vµ zn+1 song song hoÆc trïng nhau th× ta cã thÓ c¨n cø nguyªn t¾c chung hay chän xn theo xn+1.
+ C¸c hÖ to¹ ®é Oxyz ph¶i tu©n theo qui t¾c bµn tay ph¶i.
+ Khi g¾n hÖ to¹ ®é lªn c¸c kh©u, ph¶i tu©n theo c¸c phÐp biÕn ®æi cña ma trËn An. ®ã
lµ bèn phÐp biÕn ®æi : An = Rot(z,θ) Trans(0,0,d) Trans(a,0,0) Rot(x,α). NghÜa lµ ta coi hÖ to¹
®é thø n+1 lµ biÕn ®æi cña hÖ to¹ ®é thø n; c¸c phÐp quay vµ tÞnh tiÕn cña biÕn ®æi nÇy ph¶i lµ
mét trong c¸c phÐp biÕn ®æi cña An, c¸c th«ng sè DH còng ®−îc x¸c ®Þnh dùa vµo c¸c phÐp
biÕn ®æi nÇy. Trong qu¸ tr×nh g¾n hÖ täa ®é lªn c¸c kh©u, nÕu xuÊt hiÖn phÐp quay cña trôc zn
®èi víi zn-1 quanh trôc yn-1 th× vÞ trÝ ban ®Çu cña robot ®· gi¶ ®Þnh lµ kh«ng ®óng, ta cÇn chän
l¹i vÞ trÝ ban ®Çu kh¸c cho robot.
2. LËp b¶ng th«ng sè DH (Denavit Hartenberg).
3. Dùa vµo c¸c th«ng sè DH x¸c ®Þnh c¸c ma trËn An.
4. TÝnh c¸c ma trËn T vµ viÕt c¸c ph−¬ng tr×nh ®éng häc cña robot.
(♦)
VÞ trÝ ban ®Çu lµ vÞ trÝ mµ c¸c biÕn nhËn gi¸ trÞ ban ®Çu, th−êng b»ng 0.
TS. Ph¹m §¨ng Ph−íc
Robot c«ng nghiÖp 33
VÝ dô sau ®©y tr×nh bµy chi tiÕt cña c¸c b−íc khi thiÕt lËp hÖ ph−¬ng tr×nh ®éng häc
cña robot :
Cho mét robot cã ba kh©u, cÊu h×nh RRT nh− h×nh 3.11. H·y thiÕt lËp hÖ ph−¬ng tr×nh
®éng häc cña robot.
θ2 d3
θ1
H×nh 3.11 : Robot RRT
1. G¾n hÖ to¹ ®é lªn c¸c kh©u :
Ta gi¶ ®Þnh vÞ trÝ ban ®Çu vµ chän gèc to¹ ®é O0 cña robot nh− h×nh 3.12. C¸c trôc z ®Æt
cïng ph−¬ng víi c¸c trôc khíp.
Ta thÊy trôc z1 ®· quay t−¬ng ®èi mét y1
0
gãc 90 so víi trôc z0, ®©y chÝnh lµ phÐp quay θ2 d3
O1 , O2
quanh trôc x0 mét gãc α1 (phÐp biÕn ®æi
Rot(x0,α1) trong biÓu thøc tÝnh An). NghÜa lµ x1 z2
trôc x0 vu«ng gãc víi z0 vµ z1. Ta chän chiÒu z1
cña x0 tõ tr¸i sang ph¶i th× gãc quay α1=900 θ1
(chiÒu d−¬ng ng−îc chiÒu kim ®ång hå). d1
z0 y0
§ång thêi ta còng thÊy gèc O1 ®· tÞnh tiÕn
mét ®o¹n däc theo z0 , so víi O0, ®ã chÝnh lµ O0 x0
phÐp biÕn ®æi Trans(0,0,d1) (tÞnh tiÕn däc theo
z0 mét ®o¹n d1) ; c¸c trôc y0,vµ y1 x¸c ®Þnh
H×nh 3.12 : G¾n c¸c hÖ to¹ ®é O0 vµ O1
theo qui t¾c bµn tay ph¶i (H×nh 3.12 ) .
z3
TiÕp tôc chän gèc täa ®é O2 ®Æt trïng O3 x3
víi O1 v× trôc khíp thø ba vµ trôc khíp thø
hai c¾t nhau t¹i O1 (nh− h×nh 3.12). Trôc z2 d3
cïng ph−¬ng víi trôc khíp thø ba, tøc lµ ®· d3
quay ®i mét gãc 900 so víi z1 quanh trôc y1; y1 ≡ z2
phÐp biÕn ®æi nÇy kh«ng cã trong biÓu thøc
θ2 O1 ≡ O2
tÝnh An nªn kh«ng dïng ®−îc, ta cÇn chän l¹i
vÞ trÝ ban ®Çu cña robot (thay ®æi vÞ trÝ cña z1 x1 ≡ x2
kh©u thø 3) nh− h×nh 3.13.
Theo h×nh 3.13, O2 vÉn ®−îc ®Æt trïng θ1
d1
víi O1, trôc z2 cã ph−¬ng th¼ng ®øng, nghÜa lµ z0 y0
ta ®· quay trôc z1 thµnh z2 quanh trôc x1 mét
x0
gãc -900 (tøc α2= -900). O0
§Çu cuèi cña kh©u thø 3 kh«ng cã
khíp, ta ®Æt O3 t¹i ®iÓm gi÷a cña c¸c ngãn
H×nh 3.13 : HÖ to¹ ®é
tay, vµ trôc z3, x3 chän nh− h×nh vÏ, nh− vËy
g¾n lªn c¸c kh©u
ta ®· tÞnh tiÕn gèc to¹ ®é däc theo z2 mét
®o¹n d3 (PhÐp biÕn ®æi Trans(0,0,d3)), v× ®©y
lµ kh©u tÞnh tiÕn nªn d3 lµ biÕn .
TS. Ph¹m §¨ng Ph−íc
Robot c«ng nghiÖp 34
Nh− vËy viÖc g¾n c¸c hÖ to¹ ®é lªn c¸c kh©u cña robot ®· hoµn thµnh. Th«ng qua c¸c
ph©n tÝch trªn ®©y, ta cã thÓ x¸c ®Þnh ®−îc c¸c th«ng sè DH cña robot.
2. LËp b¶ng th«ng sè DH :
Kh©u θi αi ai di
1 θ1* 90 0 d1
2 θi* -90 0 0
3 0 0 0 d3 *
3. X¸c ®Þnh c¸c ma trËn A :
Ma trËn An cã d¹ng :
cosθ -sinθ cosα sinθ sinα 0
An = sinθ cosθ cosα -cosθ sinα 0
0 sinα cosα d
0 0 0 1
Víi qui −íc viÕt t¾t : C1 = cosθ1 ; S1 = sinθ1 ; C2 = cosθ2 . . .
C1 0 S1 0
A1 = S1 0 -C1 0
0 1 0 d1
0 0 0 1
C2 0 -S2 0
A2 = S2 0 C2 0
0 -1 0 0
0 0 0 1
1 0 0 0
A3 = 0 1 0 0
0 0 1 d3
0 0 0 1
4. TÝnh c¸c ma trËn biÕn ®æi thuÇn nhÊt T :
+ Ma trËn 2T3 = A3
+ Ma trËn 1T3 = A2. 2T3
C2 0 -S2 0 1 0 0 0 C2 0 -S2 -S2*d3
1
T3 = S2 0 C2 0 0 1 0 0 = S2 0 C2 C2*d3
0 -1 0 d2 0 0 1 d3 0 -1 0 0
0 0 0 1 0 0 0 1 0 0 0 1
+ Ma trËn T3 = A1 . 1T3
C1 0 S1 0 C2 0 -S2 -S2*d3
T3 = S1 0 -C1 0 S2 0 C2 C2*d3
0 1 0 d1 0 -1 0 0
0 0 0 1 0 0 0 1
TS. Ph¹m §¨ng Ph−íc
Robot c«ng nghiÖp 35
C1C2 -S1 -C1S2 -C1S2d3
= S1d2 C1 -S1S2 -S1S2d3
S2 0 C2 C2d3 + d1
0 0 0 1
Ta cã hÖ ph−¬ng tr×nh ®éng häc cña robot nh− sau :
nx = C1C2; ny = S1C2; nz = S2
Ox = -S1; Oy = C1; Oz = 0;
ax = -C1S2; ay = -S1S2; az = C2;
px = -C1S2d3 py = -S1S2d3 pz = C2d3 + d1;
(Ta cã thÓ s¬ bé kiÓm tra kÕt qu¶ tÝnh to¸n b»ng c¸ch dùa vµo to¹ ®é vÞ trÝ px,py, pz ®·
tÝnh so víi c¸ch tÝnh h×nh häc trªn h×nh vÏ).
3.9. HÖ ph−¬ng tr×nh ®éng häc cña robot STANFORD :
Stanford lµ mét robot cã 6 kh©u víi cÊu h×nh RRT.RRR (Kh©u thø 3 chuyÓn ®éng tÞnh
tiÕn, n¨m kh©u cßn l¹i chuyÓn ®éng quay). KÕt cÊu cña robot Stanford nh− h×nh 3.14 :
H×nh 3.14 : Robot Stanford
TS. Ph¹m §¨ng Ph−íc
Robot c«ng nghiÖp 36
Trªn h×nh 3.15 tr×nh bµy m« h×nh
cña robot Stanford víi viÖc g¾n c¸c hÖ to¹ O3,O4,O5,O6
®é lªn tõng kh©u. §Ó ®¬n gi¶n trong khi z3,z5,z6
z4
viÕt c¸c ph−¬ng tr×nh ®éng häc cña robot,
ta qui −íc c¸ch viÕt t¾t c¸c hµm l−îng gi¸c xi
nh− sau :
d2 d3
C1 = cosθ1; z0 z2
S1 = sinθ1;
O0,O1 z1 O2
C12 = cos(θ1+θ2);
S12 = sin(θ1+θ2) x0
S234 = sin (θ2+θ3+θ4) ... . x1
HÖ to¹ ®é g¾n lªn c¸c kh©u cña robot nh−
h×nh 3.15. (Kh©u cuèi cã chiÒu dµi vµ
kho¶ng c¸ch b»ng kh«ng, ®Ó cã thÓ g¾n c¸c
lo¹i c«ng cô kh¸c nhau nªn chän O6≡O5). H×nh 3.15 : HÖ to¹ ®é cña Robot Stanford
B¶ng th«ng sè DH (Denavit-Hartenberg) cña robot Stanford nh− sau :
Kh©u θi αi ai di
1 θ1 * -900 0 0
2 θ2 * 900 0 d2
3 0 0 0 d3 *
4 θ4 * -900 0 0
5 θ5 * 900 0 0
6 θ6 * 0 0 0
(* : C¸c biÕn khíp).
C¸c ma trËm A cña robot Stanford ®−îc x¸c ®Þnh nh− sau :
C1 0 -S1 0 C2 0 S2 0
A1= S1 0 C1 0 A2= S2 0 -C2 0
0 -1 0 0 0 1 0 d2
0 0 0 1 0 0 0 1
1 0 0 0 C4 0 -S4 0
A3= 0 1 0 0 A4= S4 0 C4 0
0 0 1 d3 0 -1 0 0
0 0 0 1 0 0 0 1
C5 0 S5 0 C6 -S6 0 0
A5= S5 0 -C5 0 A6= S6 C6 0 0
0 1 0 0 0 0 1 0
0 0 0 1 0 0 0 1
TÝch cña c¸c ma trËn chuyÓn vÞ A ®èi víi robot Stanford ®−îc b¾t ®Çu ë kh©u 6 vµ
chuyÓn dÇn vÒ gèc; theo thø tù nÇy ta cã :
TS. Ph¹m §¨ng Ph−íc
Robot c«ng nghiÖp 37
C6 -S6 0 0
5
T6 = S6 C6 0 0
0 0 1 0
0 0 0 1
C5C6 -C5S6 S5 0
4
T6 = A5A6 = S5C6 -S5S6 -C5 0
S6 C6 0 0
0 0 0 1
C4C5C6 - S4S6 -C4C5S6-S4C6 C4S5 0
T63 = A4A5A6 = S4C5C6 + C4S6 -S4C5S6 + C4C6 S4S5 0
-S5C6 S5S6 C5 0
0 0 0 1
C4C5C6-S4S6 -C4C5S6 - S4C6 C4S5 0
2
T6 = A3A4A5A6 = S4C5C + C4S6 -S4C5S6 + C4C6 S4S5 0
-S5C6 S5S6 C5 d3
0 0 0 1
C2(C4C5C6 - S4S6) - S2S5C6 -C2(C4C5S6-S4C6)+S2S5S6
1
T6 =A2 A3A4A5A6 = S2(C4C5C6 - S4S6) + C2S5C6 -S2(C4C5S6+S4C6)-C2S5S6
S4C5C6 + C4S6 -S4C5S6+C4C6
0 0
C2C4S5 + S2C5 S2d3
S2C4S5 - C2C5 -C2d3
S4S5 d2
0 1
Cuèi cïng :
nx Ox ax px
T6 = ny Oy ay py = A1T61
nz Oz az pz
0 0 0 1
§Ó tÝnh T6, ta ph¶i nh©n A1 víi T61 sau ®ã c©n b»ng c¸c phÇn tö cña ma trËn T6 ë hai vÕ
ta ®−îc mét hÖ thèng c¸c ph−¬ng tr×nh 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. Ph¹m §¨ng Ph−íc
Robot c«ng nghiÖp 38
NÕu ta biÕt ®−îc c¸c gi¸ trÞ cña biÕn khíp, th× vÞ trÝ vµ h−íng cña bµn tay robot sÏ t×m
®−îc b»ng c¸ch x¸c ®Þnh c¸c gi¸ trÞ c¸c phÇn tö cña T6 theo c¸c ph−¬ng tr×nh trªn.
C¸c ph−¬ng tr×nh trªn gäi lµ hÖ ph−¬ng tr×nh ®éng häc thuËn cña robot Stanford.
3.10. HÖ ph−¬ng tr×nh ®éng häc cña robot ELBOW :
§Ó hiÓu râ h¬n vÒ c¸ch thiÕt lËp hÖ ph−¬ng tr×nh ®éng häc cña robot, ta xÐt thªm
tr−êng hîp robot Elbow.
Kh©u 2
Kh©u 3
Kh©u 4
Kh©u 1 Kh©u 5
Kh©u 6
H×nh 1.16 : Robot Elbow
z0
O0,O1 a2
z4
θ2 a3 θ5
z1 O2 a4
O3 O2,O5,O6
z2 θ3
θ1 z3 θ4 xi
z 5, z 6
θ6 a5 = a6 = 0
H×nh 1.17 : VÞ trÝ ban ®Çu cña robot Elbow vµ c¸c hÖ to¹ ®é
Bé th«ng sè DH cña robot Elbow
Kh©u θi * αi ai di
1 θ1 900 0 0
2 θ2 0 a2 0
3 θ3 0 a3 0
4 θ4 -900 a4 0
5 θ5 900 0 0
6 θ6 0 0 0
(* : c¸c biÕn khíp )
C¸c ma trËn A cña robot Elbow ®−îc x¸c ®Þnh nh− sau :
C1 0 S1 0 C2 -S2 0 C 2 a2
A1= S1 0 -C1 0 A2= S2 C2 0 S 2 a2
0 1 0 0 0 0 1 0
0 0 0 1 0 0 0 1
TS. Ph¹m §¨ng Ph−íc
Robot c«ng nghiÖp 39
C3 -S3 0 C 3 a3 C4 0 -S4 C 4 a4
A3= S3 C3 0 S 3 a3 A4= S4 0 C4 S 4 a4
0 0 1 0 0 -1 0 0
0 0 0 1 0 0 0 1
C5 0 S5 0 C6 -S6 0 0
A5= S5 0 -C5 0 A6= S6 C6 0 0
0 1 0 0 0 0 1 0
0 0 0 1 0 0 0 1
Ta x¸c ®Þnh c¸c ma trËn T theo c¸c hÖ to¹ ®é lÇn l−ît tõ kh©u cuèi trë vÒ gèc :
C6 -S6 0 0
T65 = S6 C6 0 0
0 0 1 0
0 0 0 1
C5C6 -C5S6 S5 0
4
T6 = A5A6 = S5C6 -S5S6 -C5 0
S6 C6 0 0
0 0 0 1
C4C5C6 - S4S6 -C4C5S6-S4C6 C4S5 C4a4
T63 = A4A5A6 = S4C5C6+C4S6 -S4C5S6+C4C6 S4S5 S4a4
-S5C6 S5S6 C5 0
0 0 0 1
C34C5C6 - S34S6 -C34C5C6 - S34C6 C34S5 C34a4+C3a3
2
T6 = A3A4A5A6 = S34C5C6+C34S6 -S34C5S6+C34C6 S34S5 S34a4+S3a3
-S5C6 S5S6 C5 0
0 0 0 1
T61 =A2 A3A4A5A6 =
C234C5C6 - S234S6 -C234C5S6 - S234C6 C234S5 C234a4+C23a3+C2a2
S234C5C6 + C234S6 -S234C5S6 + C234C6 S234S5 S234a4+S23a3+S2a2
-S5C6 S5S6 C5 0
0 0 0 1
Cuèi cïng :
nx Ox ax px
T6 = ny Oy ay py = A1T61
nz Oz az pz
0 0 0 1
§Ó tÝnh T6, ta ph¶i nh©n A1 víi T61 sau ®ã c©n b»ng c¸c phÇn tö cña ma trËn T6 ta ®−îc
mét hÖ thèng c¸c ph−¬ng tr×nh sau :
TS. Ph¹m §¨ng Ph−íc
Robot c«ng nghiÖp 40
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
Cét ®Çu tiªn cña ma trËn T6 cã thÓ ®−îc x¸c ®Þnh bëi tÝch vect¬ : n = O x a.
3.11. KÕt luËn :
Trong ch−¬ng nÇy chóng ta ®· nghiªn cøu viÖc dïng c¸c phÐp biÕn ®æi thuÇn nhÊt ®Ó
m« t¶ vÞ trÝ vµ h−íng cña kh©u chÊp hµnh cuèi cña robot th«ng qua viÖc x¸c lËp c¸c hÖ to¹ ®é
g¾n lªn c¸c kh©u vµ c¸c th«ng sè DH. Ph−¬ng ph¸p nÇy cã thÓ dïng cho bÊt cø robot nµo víi
sè kh©u (khíp) tuú ý. Trong qu¸ tr×nh x¸c lËp c¸c hÖ to¹ ®é më réng ta còng x¸c ®Þnh ®−îc vÞ
trÝ dõng cña mçi robot. Tuú thuéc kÕt cÊu cña robot còng nh− c«ng cô g¾n lªn kh©u chÊp hµnh
cuèi mµ ta cã thÓ ®−a c¸c th«ng sè cña kh©u chÊp hµnh cuèi vµo ph−¬ng tr×nh ®éng häc hay
kh«ng. ViÖc tÝnh to¸n c¸c ma trËn T ®Ó thiÕt lËp hÖ ph−¬ng tr×nh ®éng häc cña robot th−êng
tèn nhiÒu thêi gian vµ dÔ nhÇm lÉn, ta cã thÓ lËp tr×nh trªn m¸y tÝnh ®Ó tÝnh to¸n (ë d¹ng ký
hiÖu) nh»m nhanh chãng x¸c ®Þnh c¸c ma trËn An vµ thiÕt lËp hÖ ph−¬ng tr×nh ®éng häc cña
robot .
ThiÕt lËp hÖ ph−¬ng tr×nh ®éng häc cña robot lµ b−íc rÊt quan träng ®Ó cã thÓ dùa vµo
®ã lËp tr×nh ®iÒu khiÓn robot. Bµi to¸n nÇy th−êng ®−îc gäi lµ bµi to¸n ®éng häc thuËn
robot. ViÖc gi¶i hÖ ph−¬ng tr×nh ®éng häc cña robot ®−îc gäi lµ bµi to¸n ®éng häc ng−îc,
nh»m x¸c ®Þnh gi¸ trÞ cña c¸c biÕn khíp theo c¸c th«ng sè ®· biÕt cña kh©u chÊp hµnh cuèi;
vÊn ®Ò nÇy ta sÏ nghiªn cøu trong ch−¬ng tiÕp theo.
Bµi tËp ch−¬ng III :
Bµi 1 : Cho ma trËn :
? 0 -1 0
T6 = ? 0 0 1
? -1 0 2
? 0 0 1
lµ ma trËn biÓu diÔn h−íng vµ vÞ trÝ cña kh©u chÊp hµnh cuèi. T×m c¸c phÇn tö ®−îc ®¸nh dÊu ?
Bµi 2 : Cho mét robot cã 3 kh©u ph¼ng nh− h×nh 3.18, cÊu h×nh RRR. ThiÕt lËp hÖ ph−¬ng
tr×nh ®éng häc cña robot.
TS. Ph¹m §¨ng Ph−íc
Robot c«ng nghiÖp 41
Bµi 3 : Cho mét robot cã 2 kh©u tÞnh tiÕn nh− h×nh 3.19, cÊu h×nh TT. ThiÕt lËp hÖ ph−¬ng
tr×nh ®éng häc cña robot.
H×nh 3.18 : Robot cÊu h×nh RRR H×nh 3.19 : Robot cÊu h×nh TT
Bµi 4 : Cho mét robot cã 2 kh©u ph¼ng nh− h×nh 3.20, cÊu h×nh RT. ThiÕt lËp hÖ ph−¬ng tr×nh
®éng häc cña robot.
Bµi 5 : Cho mét robot cã 3 kh©u nh− h×nh 3.21, cÊu h×nh RTR. ThiÕt lËp hÖ ph−¬ng tr×nh ®éng
häc cña robot.
H×nh 3.20 : Robot cÊu h×nh RT H×nh 3.21 : Robot cÊu h×nh RTR
Bµi 6 : Cho mét robot cã 3 kh©u nh− h×nh 3.22, cÊu h×nh RRR. ThiÕt lËp hÖ ph−¬ng tr×nh
®éng häc cña robot.
H×nh 3.22 : Robot cÊu h×nh RRR H×nh 3.23 : Robot cÊu h×nh RRRRR
Bµi 7 : Cho mét robot cã 5 kh©u nh− h×nh 3.23, cÊu h×nh RRRRR. ThiÕt lËp hÖ ph−¬ng tr×nh
®éng häc cña robot.
TS. Ph¹m §¨ng Ph−íc
