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