Robot c«ng nghiÖp 42
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
Robot c«ng nghiÖp 43
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
nxOxaxpx
nyOyaypy(4-2)
nzOzazpz
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
Robot c«ng nghiÖp 44
nx = cosΦCosθcosψ - sinΦsinψ (4.3)
n
y = sinΦCosθcosψ + cosΦsinψ (4-4)
n
z = -sinθ cosψ (4-5)
O
x = -cosΦCosθsinψ - sinΦcosψ (4-6)
O
y = -sinΦCosθsinψ + cosΦcosψ (4-7)
O
z = sinθ sinψ (4-8)
a
x = cosΦsinθ (4-9)
a
y = sinΦsinθ (4-10)
a
z = cosθ (4-11)
Ta thö gi¶i hÖ ph−¬ng tr×nh nÇy ®Ó t×m Φ, θ, ψ nh− sau :
Tõ (4-11) ta cã θ = cos-1(az) (4-12)
Tõ (4-9) ta cã Φ = cos-1(ax / sinθ) (4-13)
Tõ (4-5) vµ (4-12) ta cã ψ = cos-1(-nz / sinθ) (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
d = 0
0,180
θ
θ
: θ x¸c ®Þnh kh«ng chÝnh x¸c.
+ 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
nh»m môc ®Ých x¸c ®Þnh ®−îc gãc thùc - duy
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 -π ≤ θ < π.
θ
x
y
X- Y- X+ Y-
H
×nh 4.1 : Hµm arctg2(y,x)
X- Y+ X+ Y+
VÝ dô :
arctg2(-1/-1)= -1350,
trong khi arctg2(1/1) = 450
Hµm nÇy x¸c ®Þnh ngay c¶ khi x hoÆc y
b»ng 0 vµ cho kÕt qu¶ ®óng.
(Trong mét sè ng«n ng÷ lËp tr×nh nh−
Matlab, turbo C++, Maple hµm arctg2(y,x) ®·
cã s¼n trong th− viÖn)
TS. Ph¹m §¨ng Ph−íc
Robot c«ng nghiÖp 45
§Ó 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Φ 0 0 nxOxaxpx Cosθcosψ -Cosθ sinψ sinθ 0
-sinΦ cosΦ 0 0 nyOyaypy=sinψ cosψ 0 0
0 0 1 0 nzOzazpz -sinθ cosψ sinθ sinψ Cosθ 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
Trong ®ã : f11 = cosΦ x + sinΦ y (4-17)
f
12 = -sinΦ x + cosΦ y (4-18)
f
13 = 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
f
12(O) = -sinΦ Ox + cosΦ Oy
f
13(a) = az
Nh− vËy ph−¬ng tr×nh (4-16) cã thÓ ®−îc viÕt thµnh :
f11(n) f11(O) f11(a) 0
Cosθcosψ -Cosθ sinψ sinθ 0
f12(n) f12(O) f12(a) 0
=sinψ cosψ 0 0
(4-20)
f13(n) f13(O) f13(a) 0
-sinθ cosψ sinθ sinψ Cosθ 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
Robot c«ng nghiÖp 46
f12(a) = -sinΦ ax + cosΦ ay = 0. (4-21)
Céng hai vÕ víi sinΦ ax vµ chia cho cosΦ ax ta cã :
tg a
ax
ΦΦ
Φ
==
sin
cos
y
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ã :
tg -a
-a x
ΦΦ
Φ
==
sin
cos
y
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θ.
Vµ f13(a) = az = cosθ.
VËy θ = arctg2(cosΦ ax + sinΦ ay, az)
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ψ
f
12(O) = -sinΦ Ox + cosΦ Oy = cosψ
TS. Ph¹m §¨ng Ph−íc