9

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 : Sach cua Spong/2.2.1

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

Giai thich ro o
chuong 4.2

(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