ROBOT công nghiệp - Chương 4: Giải phương trình động học robor hay phương trình động học ngược
Trong chương này chúng ta sẽ tiến hành giải hệ phương trình động học đã thiết lập ở chuơng trước nhằm xác định các biến trong bộ thông số Denavit - hartenberg khi đã biết ma trận vectơ cuối
42
Robot c«ng nghiÖp
Ch−¬ng IV
Gi¶i ph−¬ng tr×nh ®éng häc robot
hay ph−¬ng tr×nh ®éng häc ng−îc
(Invers Kinematic Equations)
Trong ch−¬ng 3, ta ®· nghiªn cøu viÖc thiÕt lËp hÖ ph−¬ng tr×nh ®éng häc cña
robot th«ng qua ma trËn T6 b»ng ph−¬ng ph¸p g¾n c¸c hÖ to¹ ®é lªn c¸c kh©u vµ x¸c
®Þnh c¸c th«ng sè DH. Ta còng ®· xÐt tíi c¸c ph−¬ng ph¸p kh¸c nhau ®Ó m« t¶ h−íng
cña kh©u chÊp hµnh cuèi nh− c¸c phÐp quay Euler, phÐp quay Roll-Pitch vµ Yaw
.v.v...Trong ch−¬ng nÇy chóng ta sÏ tiÕn hµnh gi¶i hÖ ph−¬ng tr×nh ®éng häc ®· thiÕt
lËp ë ch−¬ng tr−íc nh»m x¸c ®Þnh c¸c biÕn trong bé th«ng sè Denavit - Hartenberg khi
®· biÕt ma trËn vect¬ cuèi T6. KÕt qu¶ cña viÖc gi¶i hÖ ph−¬ng tr×nh ®éng häc ®ãng
vai trß hÕt søc quan träng trong viÖc ®iÒu khiÓn robot. Th«ng th−êng, ®iÒu ta biÕt lµ c¸c
vÞ trÝ vµ h−íng mµ ta muèn robot ph¶i dÞch chuyÓn tíi vµ ®iÒu ta cÇn biÕt lµ mèi quan
hÖ gi÷a c¸c hÖ to¹ ®é trung gian ®Ó phèi hîp t¹o ra chuyÓn ®éng cña robot, hay nãi
c¸ch kh¸c ®ã chÝnh lµ gi¸ trÞ cña c¸c biÕn khíp øng víi mçi to¹ ®é vµ h−íng cña kh©u
chÊp hµnh cuèi hoÆc c«ng cô g¾n lªn kh©u chÊp hµnh cuèi, muèn vËy ta ph¶i gi¶i hÖ
ph−¬ng tr×nh ®éng häc cña robot. ViÖc nhËn ®−îc lêi gi¶i cña bµi to¸n ®éng häc ng−îc
lµ vÊn ®Ò khã mµ ta sÏ nghiªn cøu trong ch−¬ng nÇy. NhiÖm vô cña bµi to¸n lµ x¸c
®Þnh tÖp nghiÖm (θ1, θ2, ...,θ6,di*) khi ®· biÕt h×nh thÓ cña robot th«ng qua vect¬ cuèi
T6 (kh¸i niÖm “h×nh thÓ” cña robot bao gåm kh¸i niÖm vÒ vÞ trÝ vµ h−íng cña kh©u
chÊp hµnh cuèi : Configuration = Position + Orientation).
Còng cÇn l−u ý r»ng, ®a sè c¸c robot cã bé Teach pendant lµ thiÕt bÞ d¹y häc,
cã nhiÖm vô ®iÒu khiÓn robot ®Õn c¸c vÞ trÝ mong muèn trong ®éng tr×nh ®Çu tiªn (®iÒu
khiÓn ®iÓm : Point to point ), c¸c chuyÓn ®éng nÇy sÏ ®−îc ghi l¹i vµo bé nhí trung
t©m (CPU) cña robot hoÆc m¸y tÝnh ®iÒu khiÓn robot, sau ®ã robot cã thÓ thùc hiÖn l¹i
®óng c¸c ®éng t¸c ®· ®−îc häc. Trong qu¸ tr×nh ho¹t ®éng cña robot, nÕu d¹ng quÜ ®¹o
®−êng ®i kh«ng quan träng th× kh«ng cÇn lêi gi¶i cña bµi to¸n ®éng häc ng−îc.
4.1. C¸c ®iÒu kiÖn cña bµi to¸n ®éng häc ng−îc :
TS. Ph¹m §¨ng Ph−íc
43
Robot c«ng nghiÖp
ViÖc gi¶i bµi to¸n ®éng häc ng−îc cña robot cÇn tho¶ m·n c¸c ®iÒu kiÖn sau :
4.1.1. §iÒu kiÖn tån t¹i nghiªm :
§iÒu kiÖn nÇy nh»m kh¼ng ®Þnh : Cã Ýt nhÊt mét tÖp nghiÖm (θ1,θ2, ...,θ6,di*)
sao cho robot cã h×nh thÓ cho tr−íc.
(“H×nh thÓ” lµ kh¸i niÖm m« t¶ t−êng minh cña vect¬ cuèi T6 c¶ vÒ vÞ trÝ vµ
h−íng).
4.1.2. §iÒu kiÖn duy nhÊt cña tÖp nghiÖm :
Trong khi x¸c ®Þnh c¸c tÖp nghiÖm cÇn ph©n biÖt râ hai lo¹i nghiÖm :
+ NghiÖm to¸n (Mathematical Solution) : C¸c nghiÖm nÇy tho¶ m·n c¸c
ph−¬ng tr×nh cho tr−íc cña T6.
+ NghiÖm vËt lý (Physical Solution) : lµ c¸c tÖp con cña nghiÖm to¸n, phô
thuéc vµo c¸c giíi h¹n vËt lý (giíi h¹n vÒ gãc quay, kÝch th−íc ...) nh»m x¸c ®Þnh tÖp
nghiÖm duy nhÊt.
ViÖc gi¶i hÖ ph−¬ng tr×nh ®éng häc cã thÓ ®−îc tiÕn hµnh theo hai ph−¬ng ph¸p
c¬ b¶n sau :
+ Ph−¬ng ph¸p gi¶i tÝch (Analytical Method) : t×m ra c¸c c«ng thøc hay c¸c
ph−¬ng tr×nh to¸n gi¶i tÝch biÓu thÞ quan hÖ gi÷a c¸c gi¸ trÞ cña kh«ng gian biÕn trôc
vµ c¸c th«ng sè kh¸c cña bé th«ng sè DH.
+ Ph−¬ng ph¸p sè (Numerical Method) : T×m ra c¸c gi¸ trÞ cña tÖp nghiÖm
b»ng kÕt qu¶ cña mét qu¸ tr×nh lÆp.
4.2. Lêi gi¶i cña phÐp biÕn ®æi Euler :
Trong ch−¬ng 3 ta ®· nghiªn cøu vÒ phÐp biÕn ®æi Euler ®Ó m« t¶ h−íng cña
kh©u chÊp hµnh cuèi :
Euler (Φ,θ,ψ) = Rot(z, Φ) Rot(y, θ) Rot(z, ψ)
TÖp nghiÖm muèn t×m lµ c¸c gãc Φ, θ, ψ khi ®· biÕt ma trËn biÕn ®æi ®ång
nhÊt T6 (cßn gäi lµ ma trËn vect¬ cuèi), NÕu ta cã c¸c gi¸ trÞ sè cña c¸c phÇn tö trong
ma trËn T6 th× cã thÓ x¸c ®Þnh ®−îc c¸c gãc Euler Φ, θ, ψ thÝch hîp. Nh− vËy ta cã :
Euler (Φ,θ,ψ) = T6 (4-1)
VÕ tr¸i cña ph−¬ng tr×nh (4-1) ®· ®−îc biÓu diÔn b»ng c«ng thøc (3-4) , nªn ta
cã :
cosΦCosθcosψ - sinΦsinψ -cosΦCosθsinψ - sinΦcosψ cosΦsinθ 0
sinΦCosθcosψ + cosΦsinψ -sinΦCosθsinψ + cosΦcosψ sinΦsinθ 0 =
-sinθ cosψ sinθ sinψ cosθ 0
0 0 0 1
nx Ox ax px
ny Oy ay py (4-2)
nz Oz az pz
0 0 0 1
LÇn l−ît cho c©n b»ng c¸c phÇn tö t−¬ng øng cña hai ma trËn trong ph−¬ng
tr×nh (4-2) ta cã c¸c ph−¬ng tr×nh sau :
TS. Ph¹m §¨ng Ph−íc
44
Robot c«ng nghiÖp
nx = cosΦCosθcosψ - sinΦsinψ (4.3)
ny = sinΦCosθcosψ + cosΦsinψ (4-4)
nz = -sinθ cosψ (4-5)
Ox = -cosΦCosθsinψ - sinΦcosψ (4-6)
Oy = -sinΦCosθsinψ + cosΦcosψ (4-7)
Oz = sinθ sinψ (4-8)
ax = cosΦsinθ (4-9)
ay = sinΦsinθ (4-10)
az = cosθ (4-11)
Ta thö gi¶i hÖ ph−¬ng tr×nh nÇy ®Ó t×m Φ, θ, ψ nh− sau :
θ = cos-1(az)
Tõ (4-11) ta cã (4-12)
Φ = cos-1(ax / sinθ)
Tõ (4-9) ta cã (4-13)
ψ = cos-1(-nz / sinθ)
Tõ (4-5) vµ (4-12) ta cã (4-14)
Trong ®ã ta ®· dïng ký hiÖu cos-1 thay cho hµm arccos.
Nh−ng c¸c kÕt qu¶ ®· gi¶i ë trªn ch−a dïng ®−îc v× c¸c lý do d−íi ®©y :
+ Hµm arccos kh«ng chØ biÓu hiÖn cho mét gãc ch−a x¸c ®Þnh mµ vÒ ®é chÝnh
x¸c nã l¹i phô thuéc v¸o chÝnh gãc ®ã, nghÜa lµ :
cosθ = cos(-θ) : θ ch−a ®−îc x¸c ®Þnh duy nhÊt.
dcosθ
θ x¸c ®Þnh kh«ng chÝnh x¸c.
0,180 = 0 :
dθ
+ Trong lêi gi¶i ®èi víi Φ vµ ψ mét lÇn n÷a chóng ta l¹i dïng hµm arccos vµ
chia cho sinθ, ®iÒu nÇy dÉn tíi sù mÊt chÝnh x¸c khi θ cã gi¸ trÞ l©n cËn 0.
+ C¸c ph−¬ng tr×nh (4-13) vµ (4-14) kh«ng x¸c ®Þnh khi θ = 0 hoÆc θ = ±1800.
Do vËy chóng ta cÇn ph¶i cÈn thËn h¬n
khi chän lêi gi¶i. §Ó x¸c ®Þnh c¸c gãc khi gi¶i
bµi to¸n ng−îc cña robot ta ph¶i dïng hµm
arctg2 (y,x) (hµm arctang hai biÕn). Hµm arctg2
y
nh»m môc ®Ých x¸c ®Þnh ®−îc gãc thùc - duy
X+ Y+
X- Y+
nhÊt khi xÐt dÊu cña hai biÕn y vµ x. Hµm sè tr¶
vÒ gi¸ trÞ gãc trong kho¶ng -π ≤ θ < π.
θ
VÝ dô :
x
arctg2(-1/-1)= -1350,
arctg2(1/1) = 450
trong khi
Hµm nÇy x¸c ®Þnh ngay c¶ khi x hoÆc y
b»ng 0 vµ cho kÕt qu¶ ®óng. X+ Y-
X- Y-
(Trong mét sè ng«n ng÷ lËp tr×nh nh−
Matlab, turbo C++, Maple hµm arctg2(y,x) ®·
H×nh 4.1 : Hµm arctg2(y,x)
cã s¼n trong th− viÖn)
TS. Ph¹m §¨ng Ph−íc
45
Robot c«ng nghiÖp
§Ó cã thÓ nhËn ®−îc nh÷ng kÕt qu¶ chÝnh x¸c cña bµi to¸n Euler, ta thùc hiÖn
thñ thuËt to¸n häc sau : Nh©n T6 víi ma trËn quay nghÞch ®¶o Rot(z, Φ)-1,ta cã:
Rot(z, Φ)-1 T6 = Rot(y, θ) Rot(z, ψ) (4-15)
VÕ tr¸i cña ph−¬ng tr×nh (4-15) lµ mét hµm sè cña ma trËn T vµ gãc quay Φ. Ta
thùc hiÖn phÐp nh©n ma trËn ë vÕ ph¶i cña (4-15), t×m ra c¸c phÇn tö cña ma trËn cã
gi¸ trÞ b»ng 0 hoÆc b»ng h»ng sè, cho c¸c phÇn tö nÇy c©n b»ng víi nh÷ng phÇn tö
t−¬ng øng cña ma trËn ë vÕ tr¸i, cô thÓ tõ (4-15) ta cã :
cosΦ sinΦ Cosθcosψ -Cosθ sinψ sinθ
0 0 nx Ox ax px 0
-sinΦ cosΦ sinψ cosψ
0 0 ny Oy ay py = 0 0
-sinθ cosψ sinθ sinψ Cosθ
0 0 1 0 nz Oz az pz 0
0 0 0 1 0 0 0 1 0 0 0 1
(4-16)
TÝch hai ma trËn ë vÕ tr¸i cña ph−¬ng tr×nh (4-16) lµ mét ma trËn mµ cã thÓ
®−îc viÕt gän l¹i b»ng c¸c ký hiÖu sau :
f11(n) f11(O) f11(a) f11(p)
f12(n) f12(O) f12(a) f12(p)
f13(n) f13(O) f13(a) f13(p)
0 0 0 1
f11 = cosΦ x + sinΦ y (4-17)
Trong ®ã :
f12 = -sinΦ x + cosΦ y (4-18)
f13 = z (4-19)
vµ x, y, z lµ c¸c phÇn tö cña vect¬ x¸c ®Þnh bëi c¸c d÷ kiÖn f11, f12, f13, vÝ dô :
f11(n) = cosΦ nx + sinΦ ny
f12(O) = -sinΦ Ox + cosΦ Oy
f13(a) = az
Nh− vËy ph−¬ng tr×nh (4-16) cã thÓ ®−îc viÕt thµnh :
Cosθcosψ -Cosθ sinψ sinθ 0
f11(n) f11(O) f11(a) 0
sinψ cosψ
= 0 0
f12(n) f12(O) f12(a) 0 (4-20)
-sinθ cosψ sinθ sinψ Cosθ 0
f13(n) f13(O) f13(a) 0
0 0 0 1
0 0 0 1
Trong ®ã f11, f12, f13 ®· ®−îc ®Þnh nghÜa ë (4-17), (4-18) vµ (4-19).
Khi tÝnh to¸n vÕ tr¸i, ta chó ý r»ng px, py, pz b»ng 0 v× phÐp biÕn ®æi Euler chØ
toµn phÐp quay kh«ng chøa mét phÐp biÕn ®æi tÞnh tiÕn nµo, nªn f11(p) = f12(p) = f13(p)
= 0. Tõ ph−¬ng tr×nh (4-20), cho c©n b»ng phÇn tö ë hµng 2 cét 3 ta cã :
TS. Ph¹m §¨ng Ph−íc
46
Robot c«ng nghiÖp
f12(a) = -sinΦ ax + cosΦ ay = 0. (4-21)
Céng hai vÕ víi sinΦ ax vµ chia cho cosΦ ax ta cã :
sin Φ a y
tgΦ = =
cos Φ a x
Gãc Φ cã thÓ x¸c ®Þnh b»ng hµm arctg hai biÕn :
Φ = arctg2(ay, ax).
Ta còng cã thÓ gi¶i ph−¬ng tr×nh (4-21) b»ng c¸ch céng hai vÕ víi -cosΦ ay råi
chia hai vÕ cho -cosΦ ax, triÖt tiªu -ax ë vÕ tr¸i vµ cosΦ ë vÕ ph¶i, ta cã :
sin Φ -a y
tgΦ = =
cos Φ -a x
Trong tr−êng hîp nÇy gãc Φ t×m ®−îc lµ :
Φ = arctg2(-ay, -ax).
Nh− vËy ph−¬ng tr×nh (4-21) cã mét cÆp nghiÖm c¸ch nhau 1800 (®©y lµ
nghiÖm to¸n) vµ ta cã thÓ viÕt :
Φ = arctg2(ay, ax) vµ Φ = Φ + 1800.
(HiÓu theo c¸ch viÕt khi lËp tr×nh trªn m¸y tÝnh).
NÕu c¶ ax vµ ay ®Òu b»ng 0 th× gãc Φ kh«ng x¸c ®Þnh ®−îc. §iÒu ®ã x¶y ra khi
bµn tay chØ th¼ng lªn trªn hoÆc xuèng d−íi vµ c¶ hai gãc Φ vµ ψ t−¬ng øng víi cïng
mét phÐp quay. §iÒu nÇy ®−îc coi lµ mét phÐp suy biÕn (degeneracy), trong tr−êng
hîp nÇy ta cho Φ = 0.
Víi gi¸ trÞ cña Φ nhËn ®−îc, c¸c phÇn tö ma trËn ë vÕ bªn tr¸i cña ph−¬ng tr×nh
(4-20) sÏ ®−îc x¸c ®Þnh. TiÕp tôc so s¸nh c¸c phÇn tö cña hai ma trËn ta cã :
f11(a) = cosΦ ax + sinΦ ay = sinθ.
f13(a) = az = cosθ.
Vµ
θ = arctg2(cosΦ ax + sinΦ ay, az)
VËy
Khi c¶ hai hµm sin vµ cos ®Òu ®−îc x¸c ®Þnh nh− tr−êng hîp trªn, th× gãc
th−êng ®−îc x¸c ®Þnh duy nhÊt vµ kh«ng x¶y ra tr−êng hîp suy biÕn nh− gãc Φ tr−íc
®©y. Còng tõ ph−¬ng tr×nh (4-20) ta cã :
f12(n) = -sinΦ nx + cosΦ ny = sinψ
f12(O) = -sinΦ Ox + cosΦ Oy = cosψ
TS. Ph¹m §¨ng Ph−íc
47
Robot c«ng nghiÖp
ψ = arctg2(-sinΦ nx + cosΦ ny, -sinΦ Ox + cosΦ Oy)
VËy :
Tãm l¹i, nÕu cho tr−íc mét phÐp biÕn ®æi ®ång nhÊt d−íi d¹ng c¸c phÐp quay,
ta cã thÓ x¸c ®Þnh c¸c gãc Euler t−¬ng øng lµ :
Φ = arctg2(ay, ax) vµ Φ = Φ + 1800
θ = arctg2(cosΦ ax + sinΦ ay, az)
ψ = arctg2(-sinΦ nx + cosΦ ny, -sinΦ Ox + cosΦ Oy)
4.3. Lêi gi¶i cña phÐp biÕn ®æi Roll, Pitch vµ Yaw :
PhÐp biÕn ®æi Roll, Pitch vµ Yaw ®· ®−îc ®Þnh nghÜa :
RPY(Φ,θ,ψ)= Rot(z,Φ)Rot(y,θ)Rot(x, ψ)
ViÖc gi¶i ph−¬ng tr×nh : T6 = RPY(Φ,θ,ψ) sÏ x¸c ®Þnh ®−îc c¸c gãc Φ,θ vµ ψ.
C¸ch gi¶i ®−îc tiÕn hµnh t−¬ng tù nh− khi thùc hiÖn lêi gi¶i cho phÐp quay
Euler. Nh©n T6 víi ma trËn nghÞch ®¶o Rot(z, Φ)-1, ta cã :
Rot(z, Φ)-1T6 = Rot(y,θ)Rot(x, ψ)
Hay lµ :
cosθ sinθ sinψ sinθ cosψ 0
f11(n) f11(O) f11(a) 0
cosψ -sinψ
0= 0 0
f12(n) f12(O) f12(a) (4-22)
-sinθ cosθ sinψ cosθcosψ 0
f13(n) f13(O) f13(a) 0
0 0 0 1
0 0 0 1
f11 = cosΦ x + sinΦ y
Trong ®ã :
f12 = -sinΦ x + cosΦ y
f13 = z
C©n b»ng phÇn tö ë hµng 2 cét 1 : f12(n) = 0, ta cã :
-sinΦ x + cosΦ y = 0
Ph−¬ng tr×nh nÇy cho ta hai nghiÖm nh− ®· biÕt :
Φ = arctg2(nx, ny)
Φ = Φ + 1800
vµ
TiÕp tôc c©n b»ng c¸c phÇn tö t−¬ng øng cña hai ma trËn ta cã :
-sinθ = nz
cosθ = cosΦ nx + sinΦ ny
TS. Ph¹m §¨ng Ph−íc
48
Robot c«ng nghiÖp
do vËy :
θ = arctg2(-nz, cosΦ nx + sinΦ ny)
Ngoµi ra ta cßn cã :
-sinψ = -sinΦ ax + cosΦ ay
cosψ = -sinΦ Ox + cosΦ Oy
ψ = arctg2(sinΦ ax - cosΦ ay, -sinΦ Ox + cosΦ Oy)
Nªn :
Nh− vËy ta ®· x¸c ®Þnh ®−îc c¸c gãc quay Roll, Pitch vµ Yaw theo c¸c phÇn tö
cña ma trËn T6.
4.4. Gi¶i bµi to¸n ®éng häc ng−îc cña robot Stanford :
HÖ ph−¬ng tr×nh ®éng häc cña robot Stanford ®· ®−îc thiÕt lËp trong ch−¬ng
III, Ta cã :
T6 = A1A2A3A4A5A6 (4-23)
Liªn tôc nh©n (4-23) víi c¸c ma trËn A nghÞch ®¶o, ta ®−îc :
= 1T6
−
A 1 1 T6 (4-24)
= 2T6
− −
A 2 1 A 1 1 T6 (4-25)
= 3T6
− − −
A 3 1 A 2 1 A 1 1 T6 (4-26)
= 4T6
− − − −
A 4 1 A 3 1 A 2 1 A 1 1 T6 (4-27)
= 5T6
− − − − −
A 51 A 4 1 A 3 1 A 2 1 A 1 1 T6 (4-28)
C¸c phÇn tö ë vÕ tr¸i cña c¸c ph−¬ng tr×nh nÇy lµ hµm sè cña c¸c phÇn tö T6 vµ
c¸c biÕn khíp cña (n-1) khíp ®Çu tiªn. Trong khi ®ã c¸c phÇn tö cña ma trËn vÕ bªn
ph¶i hoÆc b»ng 0, b»ng h»ng sè hoÆc lµ hµm sè cña c¸c biÕn khíp thø n ®Õn khíp thø
6. Tõ mçi ph−¬ng tr×nh ma trËn, cho c©n b»ng c¸c phÇn tö t−¬ng øng chóng ta nhËn
®−îc 12 ph−¬ng tr×nh. Mçi ph−¬ng tr×nh cã c¸c phÇn tö cña 4 vect¬ n, O, a, p.
Tõ ph−¬ng tr×nh (4-24), ta cã :
C1 S1 0 0 nx Ox ax px
−1
0 0 -1 0 ny Oy ay py
A T6 =
1
-S1 C1 0 0 nz Oz az pz
0 0 0 1 0 0 0 1
f11(n) f11(O) f11(a) f11(p)
= f12(n) f12(O) f12(a) f12(p)
f13(n) f13(O) f13(a) f13(p)
0 0 0 1
TS. Ph¹m §¨ng Ph−íc
49
Robot c«ng nghiÖp
f11 = C1 x + S1 y
Trong ®ã :
f12 = - z
f13 = -S1 x + C1 y
VÕ bªn ph¶i cña (4-24) lµ :
C2(C4C5C6 - S4S6) - S2S5C6 -C2(C4C5S6-S4C6)+S2S5S6 C2C4S5 + S2C5 S2d3
1
T6 = S2(C4C5C6 - S4S6) + C2S5C6 -S2(C4C5S6+S4C6)-C2S5S6 S2C4S5 - C2C5 -C2d3
S4C5C6 + C4S6 -S4C5S6+C4C6 S4S5 d2
0 0 0 1
C¸c phÇn tö cña ma trËn vÕ ph¶i ®Òu lµ hµm sè cña θ2, d3, θ4, θ5, θ6 ngo¹i trõ
phÇn tö ë hµng 3 cét 4, ®ã lµ :
f13(p) = d2
hay :
-S1px + C1py = d2
§Ó gi¶i ph−¬ng tr×nh ë d¹ng nÇy ta cã thÓ thay thÕ bëi c¸c hµm l−îng gi¸c sau
®©y :
px = r cosΦ
py = r sinΦ
r = + p2 + p2
Trong ®ã : x y
Φ = arctg2(py, px)
ThÕ px vµ py vµo ph−¬ng tr×nh -S1px + C1py = d2 ta cã :
sinΦcosθ1 - cosΦsinθ1 = d2 / r 0 < d2 / r ≤ 1
Víi
Hay lµ : sin(Φ - θ1) = d2 / r 0 < Φ - θ1 < π
Víi
Tõ ®ã ta cã :
cos(Φ - θ1) = ± 1 − (d 2 / r ) 2
Trong ®ã dÊu trõ phï hîp víi h×nh thÓ vai tr¸i cña robotvµ dÊu cäng phï hîp
víi h×nh thÓ vai ph¶i cña robot. Cuèi cïng :
θ1 = arctg2(py, px) - arctg2(d2, ± 1 − (d 2 / r ) 2 ) (4-29)
NÕu tÝnh ®−îc θ1 th× vÕ tr¸i cña ph−¬ng tr×nh (4-24) ®−îc x¸c ®Þnh.
Cho c©n b»ng c¸c phÇn tö ë hµng 1 cét 4 vµ hµng 2 cét 4, ta cã :
S2d3 = C1px + S1py
-C2d3 = -pz
d3 lµ dÞch chuyÓn dµi cña khíp tÞnh tiÕn, d3 > 0, nªn ta cã :
θ2 = arctg2(C1px + S1py, pz ) (4-30)
TS. Ph¹m §¨ng Ph−íc
50
Robot c«ng nghiÖp
− − −
Tõ ph−¬ng tr×nh (4-25) : A 2 1 A 1 1 T6 = A 2 1 1T6 = 2T6, ta cã :
C4C5C6-S4S6 -C4C5S6 - S4C6 C4S5
f21(n) f21(O) f21(a) 0 0
f22(n) f22(O) f22(a) 0 = S4C5C + C4S6 -S4C5S6 + C4C6 S4S5 0
-S5C6 S5S6 C5
f23(n) f23(O) f23(a) f23(p) d3
0 0 0 1 0 0 0 1
(4-31)
f21 = C2(C1 x + S1 y) - S2 z
Trong ®ã :
f22 = -S1 x + C1 y
f23 = S2(C1 x + S1 y) + C2 z
Tõ c©n b»ng phÇn tö ë hµng 3 cét 4 ta cã :
d3 = S2(C1 px + S1 py) + C2 pz (4-32)
− −
- Tõ ph−¬ng tr×nh (4-27) ta cã : A 4 1 A 3 1 2T6 = 4T6
Thùc hiÖn phÐp nh©n c¸c ma trËn ë vÕ tr¸i, vµ biÓu diÔn ë d¹ng rót gän nh− sau
:
f41(n) f41(O) f41(a) 0 C5C6 -C5S6 S5 0
f42(n) f42(O) f42(a) 0 = S5C6 -S5S6 C5 0
f43(n) f43(O) f43(a) 0 S6 C6 0 0
0 0 0 1 0 0 0 1
f41 = C4[C2(C1 x + S1 y) - S2 z] + S4(-S1 x + C1 y)
Trong ®ã :
f42 = -S2(-S1 x + C1 y) - C2 z
f43 = -S4[C2(C1 x + S1 y) + S2 z] + C4(-S1 x + C1 y)
C©n b»ng phÇn tö hµng 3, cét 3 ta ®−îc mét hµm sè cña θ4, ®ã lµ : f43(a) = 0.
Hay :
-S4[C2(C1 ax + S1 ay) + S2 az] + C4(-S1 ax + C1 ay) = 0
§©y lµ ph−¬ng tr×nh l−îng gi¸c cã d¹ng : - sinΦ ax + cosΦ ay = 0. Nh− ®· gi¶i
trong c¸c phÇn tr−íc ®©y, ph−¬ng tr×nh nÇy cã hai nghiÖm :
θ4 = arctg2(-S1 ax + C1 ay, C2(C1 ax + S1 ay) + S2 az) (4-33)
θ4 = θ4 + 1800
vµ
NÕu c¸c yÕu tè tö sè vµ mÉu sè cña (4-33) tiÕn tíi 0 th× robot r¬i vµo t×nh tr¹ng
suy biÕn nh− tru−êng hîp ®· nãi ë môc 4.2.
Ta còng cã thÓ t×m gi¸ trÞ cña gãc quay θ4 b»ng c¸ch c©n b»ng c¸c phÇn tö hµng
1 cét 3 vµ hµng 2 cét 3 cña ph−¬ng tr×nh ma trËn (4-31) , ta cã :
C4S5 = C2(C1 ax + S1 ay) - S2 az
TS. Ph¹m §¨ng Ph−íc
51
Robot c«ng nghiÖp
S4S5 = -S1 ax + C1 ay
Víi θ5 > 0 ta ®−îc θ4 = arctg(-S1 ax + C1 ay, C2(C1 ax + S1 ay) + S2 az)
Víi θ5 < 0 ta ®−îc θ4 = θ4 + 1800
®óng nh− kÕt qu¶ ®· t×m (4-33).
Khi S5 = 0, θ5 = 0. Robot cã suy biÕn do c¶ hai trôc cña khíp 4 vµ 6 n»m th¼ng
hµng (z3 ≡ z5). ë vÞ trÝ nÇy chØ cã tæng θ4+θ6 lµ cã ý nghÜa.
Khi θ5 = 0, ta cã thÓ tù do chän mét gi¸ trÞ cña θ4. Th−êng gi¸ trÞ hiÖn hµnh
®−îc sö dông.
− −
Tõ vÕ ph¶i cña ph−¬ng tr×nh A 4 1 A 3 1 2T6 = 4T6 = A5A6 ta cã thÓ cã c¸c
ph−¬ng tr×nh cña S5, C5, S6 vµ C6 b»ng c¸ch c©n b»ng c¸c phÇn tö thÝch hîp. Ch¼ng h¹n
khi c©n b»ng c¸c phÇn tö cña ma trËn hµng 1 cét 3 vµ hµng 2 cét 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) ,
S2 (C1 ax + S1 ay) + C2 az ) (4-34)
C¸c ph−¬ng tr×nh cã liªn quan ®Õn θ6 n»m ë cét 1 cña ph−¬ng tr×nh ma trËn, ®ã
lµ c¸c thµnh phÇn cña vect¬ n cña T6. Vect¬ nÇy th−êng kh«ng cã ý nghÜa trong tÝnh
to¸n, vÝ nã lu«n cã thÓ ®−îc x¸c ®Þnh b»ng tÝch vect¬ cña hai vect¬ O vµ a nh− ®· nãi
rr
r
tr−íc ®©y ( n = O x a ). Do ®ã ta ph¶i t×m c¸ch kh¸c ®Ó x¸c ®Þnh θ6.
−
Thùc hiÖn phÐp nh©n c¸c ma trËn ë vÕ tr¸i cña ph−¬ng tr×nh (4-28) : A 5 1 4T6=
5
T6 = A6, biÓu diÔn ë d¹ng ký hiÖu ta cã :
f51(n) f51(O) 0 0 C6 -S6 0 0
f52(n) f52(O) 0 0 = S6 C6 0 0 (4-35)
f53(n) f53(O) 1 0 0 0 1 0
0 0 0 1 0 0 0 1
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 c©n b»ng c¸c phÇn tö ë hµng 1 cét 2 vµ hµng 2 cét 2 ta nhËn ®−îc c¸c gi¸
trÞ cña 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]
θ6 = arctg2(S6, C6)
Tõ ®ã ta x¸c ®Þnh ®−îc : (4-36)
TS. Ph¹m §¨ng Ph−íc
52
Robot c«ng nghiÖp
C¸c biÓu thøc (4-29), (4-30), (4-32), (4-33), (4-34) vµ (4-36) x¸c ®Þnh tÖp
nghiÖm khi gi¶i bµi to¸n ng−îc cña robot Stanford.
4.5. Gi¶i bµi to¸n ®éng häc ng−îc cña robot ELBOW :
§Ó tiÕp tôc lµm quen víi viÖc gi¶i hÖ ph−¬ng tr×nh ®éng häc, chóng ta nghiªn
cøu phÐp gi¶i bµi to¸n ®éng häc ng−îc cña robot Elbow. HÖ ph−¬ng tr×nh ®éng häc
thuËn cña robot Elbow ®· d−îc x¸c ®Þnh trong ch−¬ng III.
Tr−íc hÕt ta kh¶o s¸t ph−¬ng tr×nh :
−
A 1 1 T6 = 1T6 = A2A3A4A5A6
T−¬ng tù nh− ®· lµm, ta x¸c ®Þnh c¸c phÇn tö ma trËn cña hai vÕ nh− sau :
f11(n) f11(O) f11(a) f11(p)
f12(n) f12(O) f12(a) f12(p) =
f13(n) f13(O) f13(a) f13(p)
0 0 0 1
C234C5C6 - S234S6 -C234C5S6 - S234C6 C234S5 C234a4+C23a3+C2a
2
S234C5C6 + C234S6 -S234C5S6 + C234C6 S234S5 S234a4+S23a3+S2a2
-S5C6 S5S6 C5 0
0 0 0 1
(4-37)
f11 = C1 x + S1 y
Trong ®ã :
f12 = z
f13 = S1 x + C1 y
C234 = cos(θ2+θ3+θ4)
Ta ®· ký hiÖu :
S234 = sin(θ2+θ3+θ4)
Cho c©n b»ng phÇn tö ë hµng 3 cét 4, ta cã :
S1 px + C1 py = 0
Suy ra :
θ1 = arctg2(py , px)
θ1 = θ1 + 1800
vµ (4-38)
Trong tr−êng hîp robot Elbow, ba khíp kÕ tiÕp ®Òu song song vµ kh«ng cã kÕt
qu¶ nµo nhËn ®−îc tõ phÐp nh©n víi nh÷ng ma trËn nghÞch ®¶o A-1i . Cho ®Õn kh©u thø
4 th× phÐp nh©n víi ma trËn nghÞch ®¶o míi cã ý nghÜa.
A-14A-13A-121T6 = 4T6 = A5A6
Khi x¸c ®Þnh c¸c phÇn tö ma trËn cña hai vÕ ta ®−îc :
TS. Ph¹m §¨ng Ph−íc
53
Robot c«ng nghiÖp
f41(n) f41(O) f41(a) f41(p)-C34a2-C4a3-a4 C5C6 -C5S6 S5 0
f42(n) f42(O) f42(a) 0 = S5C6 -S5S6 -C5 0
f43(n) f43(O) f43(a) f43(p)+S34a2+S4a3 S6 C6 0 0
0 0 0 1 0 0 0 1
(4-39)
f41 = C234(C1 x + S1 y) + S234 z
Trong ®ã :
f42 = -S1 x + C1 y
f43 = -S234(C1 x + S1 y) + C234 z
C©n b»ng phÇn tö hµng 3 cét 3 ta ®−îc mét ph−¬ng tr×nh cho θ234 :
-S234(C1 ax + S1 ay) + C234 az = 0
Suy ra :
θ234 = arctg2(az , C1 ax + S1 ay)
θ234 = θ234 + 1800
vµ (4-40)
B©y giê ta trë l¹i ph−¬ng tr×nh (4-37). C©n b»ng c¸c phÇn tö ma trËn ë hµng 1
cét 4 vµ hµng 2 cét 4, ta cã :
C1 px + S1 py = C234a4+C23a3+C2a2 (a)
pz = S234a4+S23a3+S2a2 (b)
p’x = C1 px + S1 py - C234a4
Ta gäi : (c)
p’y = pz - S234a4 (d)
§em (a) + (c) vµ (b) + (d) ta ®−îc ;
p’x = C23 a3 + C2a2 (e)
p’y = S23 a3 + S2a2 (g)
B×nh ph−¬ng hai vÕ vµ céng hai ph−¬ng tr×nh (e) vµ (g), ta cã :
p’2x = (C23 a3 + C2a2)2
p’2y = (S23 a3 + S2a2)2
p’2x + p’2y = (S 2 2
) a 2 3 + (S 2 2 2
+C +C 2)a 2 + 2 a2a3(C23C2 + S23S2 )
23 23 2
Ta cã C23C2 + S23S2 = cos(θ2+θ3-θ2) = cosθ3 = C3. Nªn suy ra :
C3 = (p’2x + p’2y - a23 - a22) / 2a2a3
Trong khi cã thÓ t×m θ3 tõ hµm arccos, ta vÉn nªn t×m mét gi¸ trÞ S3 vµ dïng
hµm arctg2 nh− th−êng lÖ :
S3 = ± (1 − C 2 )
Ta cã : 3
CÆp nghiÖm øng víi hai dÊu +,- phï hîp víi h×nh thÓ cña robot lóc n©ng vai lªn
vµ h¹ vai xuèng :
TS. Ph¹m §¨ng Ph−íc
54
Robot c«ng nghiÖp
θ3 = arctg2(S3 , C3) (4-41)
§Ó t×m S2 vµ C2 ta gi¶i hÖ ph−¬ng tr×nh (e),(g).
Tõ (e) ⇒ (C2C3 - S2S3)a3 + C2a2 = p’x
Tõ (g) ⇒ (S2C3 - C2S3)a3 + S2a2 = p’y
Khai triÓn vµ rót gän :
(C3a3 + a2)C2 - S3a3.S2 = p’x
Tõ (g) ⇒ S3a3 .C2 + (C3a3 + a2)S2 = p’y
Ta cã :
C 3a 3 + a 2 - S 3a 3
∆=
S 3a 3 C 3a 3 + a 2
p ,x p ,x
- S 3a 3 C 3a 3 + a 2
∆c = , ∆s =
p ,y
py C 3a 3 + a 2 S3a 3
∆ C (C 3 a 3 + a 2 )p' y −S3 a 3 p' x
=
C2 =
∆ (C 3 a 3 + a 2 ) 2 + (S3 a 3 ) 2
∆ S (C 3 a 3 + a 2 )p' x +S 3 a 3 p' y
=
S2 =
∆ (C 3 a 3 + a 2 ) 2 + (S 3 a 3 ) 2
Do mÉu sè d−¬ng vµ b»ng nhau, nªn ta cã :
θ2 = arctg2(S2, C2)
θ2 = arctg2((C3a3 + a2)p’y - S3a3p’x , (C3a3 + a2)p’x + S3a3p’y ) (4-42)
§Õn ®©y θ4 ®−îc x¸c ®Þnh bëi :
θ4 = θ234 - θ3 - θ2 (4-43)
C¸c ph−¬ng tr×nh dïng ®Ó tÝnh θ5 ®−îc thiÕt lËp tõ sù c©n b»ng c¸c phÇn tö ma
trËn hµng 1 cét 3 vµ hµng 2 cét 3 cña ph−¬ng tr×nh 4T6 (4-39) :
S5 = C234(C1ax + S1ay) + S234az
C5 = S1ax - C1ay
Suy ra :
θ5 = arctg2(C234(C1ax + S1ay) + S234az , S1ax - C1ay) (4-44)
§Ó t×m θ6 , ta tiÕp tôc nh©n A-15 víi 4T6 , ta ®−îc : A-15 . 4T6 = A6.
ViÕt tÝch ma trËn vÕ tr¸i ë d¹ng ký hiÖu :
TS. Ph¹m §¨ng Ph−íc
55
Robot c«ng nghiÖp
f51(n) f51(O) 0 0 C6 -S6 0 0
f52(n) f52(O) 0 0 = S6 C6 0 0
0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1
f51 = C5[C234(C1 x + S1 y) + S234 z] - S5(S1 x + C1 y)
Trong ®ã :
f52 = -S234(C1 x + S1 y) + C234 z
Cho c©n b»ng c¸c phÇn tö ma trËn t−¬ng ø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
VËy :
θ6 = arctg2(S6 , C6) (4-45)
C¸c ph−¬ng tr×nh (4-38), (4-41), (4-42), (4-43), (4-44) vµ (4-45) x¸c ®Þnh tÖp
nghiÖm khi gi¶i bµi to¸n ®éng häc ng−îc cña robot Elbow.
4.6. KÕt luËn :
Ph−¬ng ph¸p gi¶i bµi to¸n ®éng häc ng−îc ®−a ra trong ch−¬ng nÇy sö dông
c¸c hµm l−îng gi¸c tù nhiªn. C¸c gãc th−êng ®−îc x¸c ®Þnh th«ng qua hµm arctang hai
biÕn. Ph−¬ng ph¸p nÇy ®−îc ®−a ra bëi Pieper vµ ¸p dông tèt víi nh÷ng robot ®¬n gi¶n,
Th−êng ta nhËn ®−îc nghiÖm ë d¹ng c«ng thøc ®¬n gi¶n.
Khi gi¶i bµi to¸n ®éng häc ng−îc cã thÓ x¶y ra hiÖn t−îng “suy biÕn” . Khi cã
nhiÒu h¬n mét tÖp nghiÖm ®èi víi bµi to¸n ng−îc ®Ó x¸c ®Þnh vÞ trÝ vµ h−íng cña bµn
tay, th× c¸nh tay ®−îc gäi lµ suy biÕn.
Dïng ph−¬ng ph¸p Pieper, c¸c nghiÖm nhËn ®−îc th−êng cã 4 d¹ng c«ng thøc,
mçi d¹ng cã mét ý nghÜa ®éng häc riªng. D¹ng thø nhÊt :
- sinΦ ax + cosΦ ay = 0
D¹ng nÇy cho ta mét cÆp nghiÖm c¸ch nhau 1800, nã m« t¶ hai h×nh thÓ t−¬ng
øng cña robot. NÕu c¸c tö sè vµ mÉu sè ®Òu b»ng kh«ng, robot bÞ suy biÕn, lóc ®ã
robot mÊt ®i mét bËc tù do.
-S1px + C1py = d2
D¹ng thø hai :
D¹ng nÇy còng cho ta cÆp nghiÖm sai kh¸c nhau 1800, mét lÇn n÷a l¹i tån t¹i
kh¶ n¨ng suy biÕn khi tö sè vµ mÉu sè b»ng 0. Robot ë tr−êng hîp nÇy th−êng cã mét
khíp tÞnh tiÕn vµ ®é dµi tÞnh tiÕn ®−îc coi lµ > 0.
C1px + S1py = S2d3
D¹ng thø ba :
- C2d3 = - pz
vµ d¹ng thø t− :
TS. Ph¹m §¨ng Ph−íc
56
Robot c«ng nghiÖp
y2
x2
O2
C¸c ph−¬ng th×nh nÇy th−êng cã nghiÖm duy nhÊt.
z2
y1
Ngoµi c¸c d¹ng phæ biÕn, khi robot cã θ2 hay nhiÒu khíp song song (VÝ dô
hai
robot Elbow), c¸c gãc cña tõng khíp ph¶i ®−îc x¸c ®Þnh b»ng c¸ch gi¶i ®ång thêi
nhiÒu khíp trong mèi quan hÖ tæng c¸c gãc khíp. x1 a2
a1
y0
θ1 O
T×m ra c¸c nghiÖm phï hîp víi h×nh 1thÓ cña robot (vÞ trÝ vµ h−íng) lµ mét trong
z1
nh÷ng vÊn ®Ò khã kh¨n nhÊt. HÇu nh− ch−a cã thuËt to¸n chung nµo mµ nhê ®ã cã thÓ
x0
O0
t×m ra ®−îc tÖp nghiÖm cho mäi robot. Tuy nhiªn ph−¬ng ph¸p ®−a ra trong ch−¬ng
z0
nÇy ®· thiÕt lËp ®−îc c¸c nghiÖm sè ë d¹ng t−êng minh, trùc tiÕp.
H×nh 4.3 : HÖ to¹ ®é vµ c¸c th«ng sè cña robot 2 kh©u ph¼ng
Khi lËp tr×nh ®iÒu khiÓn robot ta ph¶i dùa vµo c¸c giíi h¹n vËt lý ®Ó chän c¸c
nghiÖm vËt lý, nghÜa lµ lo¹i trõ c¸c nghiÖm to¸n kh«ng thÝch hîp ®Ó x¸c ®Þnh mét cÊu
h×nh duy nhÊt cña robot.
Bµi tËp ch−¬ng IV :
Bµi 1 : Cho mét vÞ trÝ mong muèn cña kh©u chÊp hµnh cuèi cña robot cã 3 kh©u ph¼ng
nh− h×nh 4.2; Dïng ph−¬ng ph¸p h×nh häc ®Ó x¸c ®Þnh cã bao nhiªu lêi gi¶i cña bµi
to¸n ®éng häc ng−îc ? NÕu h−íng cña kh©u chÊp hµnh cuèi còng ®−îc x¸c ®Þnh, th× cã
bao nhiªu lêi gi¶i ?
Bµi 2 : Dïng ph−¬ng ph¸p h×nh häc ®Ó gi¶i bµi to¸n ®éng häc ng−îc (x¸c ®Þnh c¸c gãc
θ1, θ2 ) cña robot cã hai kh©u×nh 4.2 nh− h×nhcÊu h×nh RRR
H ph¼ng : Robot 4.3 :
TS. Ph¹m §¨ng Ph−íc
57
Robot c«ng nghiÖp
Bµi 3 : ThiÕt lËp hÖ ph−¬ng tr×nh ®éng häc cña robot SCARA (h×nh 4.4) vµ gi¶i bµi
to¸n ®éng häc ng−îc cña nã.
a2
z0 z1
θ1 θ2
x
x
O0
x
z2
d3 x
a1
O3
d4
O4 x
θ4
z3 , z4
H×nh 4.4 : Robot SCARA
TS. Ph¹m §¨ng Ph−íc