5

ng Hc Vn Tc
5.1 Ma trn Skew Symmetric
Ma trn skew symmetric gip cho vic n gin nhiu vic tnh ton.
nh ngha 1: Mt ma trn S c gi l skew symmetric nu v ch nu
0 + S S
T
(5.1)
T (5.1), t a thy rng cc phn t trn ng cho
0
ii
s
, v cc phn t ngoi ng
cho
j i s s
ji ij
,
. V vy, S ch cha 3 thnh phn c lp c dng nh sau:
1
1
1
]
1

0
0
0
1 2
1 3
2 3
s s
s s
s s
S
(5.2)
Ta biu din tp hp mi ma trn 3x3 skew symmetric bng k hiu SS(3).
Nu
T
z y x
a a a ) , , ( a
, ta nh ngha ma trn skew symmetric
) (a S
nh sau:
1
1
1
]
1

0
0
0
) (
3
x y
x z
y
a a
a a
a s
a S
(5.3)
V d 5.1: Ta biu din 3 vect ta n v c s
j i,
v k .
T T T
) 1 , 0 , 0 ( ; ) 0 , 1 , 0 ( ; ) 0 , 0 , 1 ( k j i
Theo nh ngha, ma trn skew symmetric tng ng l
1
1
1
]
1

1
1
1
]
1

1
1
1
]
1

0 0 0
0 0 1
0 1 0
) ( ;
0 0 1
0 0 0
1 0 0
) ( ;
0 1 0
1 0 0
0 0 0
) ( k S j S i S
(5.4)
5.2 Tnh cht ma trn skew symmetric:
1
(a) Tnh cht 1: tuyn tnh (Linearity)
Vi mi vect
3
, R b a v 2 s v hng

ta c
) ( ) ( ) ( b a b a S S S + +
(5.5)
(b) Tnh cht 2:
Vi mi vect
T
z y x
p p p ) , , ( p
p a p a x ) ( S
(5.6)
NL:
k y x y x j y x y x i y x y x
y y y
x x x
k j i
) ( ) ( ) (
1 2 2 1 3 1 1 3 2 3 3 2
3 2 1
3 2 1
+ +
1
1
1
]
1

y x c x
sin y x c
Nu
) 3 ( SO R
v
3
, R b a
b a b a R R R x x ) (
(5.7)
Biu thc (5.7) ch ng khi R l trc giao
T (5.6)-(5.7) ta c:
b a
b a
b a
b a b a
) (
) (
) ( ) (
) ( ) (
R S
R
RR R
R R R RS
T
T T

x
x
x
(5.8)
Ta c mt kt qu rt quan trng sau:
) ( ) ( a a R S R RS
T
(5.9)
V tri ca (5.9) biu din mt php bin i tng ng ca ma trn
) (a S
; c ngha l,
s biu din ma trn
) (a S
trong h ta c quay bi R th ging vi ma trn skew
symmetric
) ( a R S
tng ng vi vect
a
quay bi R .
Gi s
) 3 ( ) ( SO R R
. V R trc giao vi mi ta c:
I R R
T
) ( ) ( (5.10)
Vi phn (5.10) ta c
2
0 ) ( ) ( +

d
dR
R R
d
dR
T
T
(5.11)
nh ngha ma trn
T
R
d
dR
S ) ( :

(5.12)

d
dR
R R
d
dR
S
T
T
T T
) ( ) (

,
_

(5.13)
Biu thc (5.11) c vit li nh sau:
0 +
T
S S (5.14)
Vy ma trn S nh ngha bi (5.12) l skew symmetric
Nhn 2 v ca (5.12) cho R cho
) (

SR
d
dR

(5.15)
Biu thc (5.15) rt quan trng. N c ngha rng vic tnh ton vi phn ca ma trn
quay R tng ng vi vic nhn ma trn quay vi mt ma trn skew symmetric S .
Hn na, hu ht cc trng hp ma trn R l ma trn quay c bn hay tch s ca cc ma
trn quay c s.
V d 5.2: Xt ma trn quay c s
, x
R R
) (
0 1 0
1 0 0
0 0 0
cos sin 0
sin cos 0
0 0 1
sin cos
cos sin 0
0 0 0
i S R
d
dR
S
T

1
1
1
]
1

1
1
1
]
1

1
1
1
]
1

(5.16)

,
,
) (
x
x
R i S
d
dR
(5.17)
Tng t,

,
,
) (
y
y
R j S
d
dR
v

,
,
) (
z
z
R k S
d
dR
(5.18)
V d 5.3: Xt ma trn quay
, k
R
quanh trc k bt k, ta c

,
,
) (
k
k
R k S
d
dR
(5.19)
3
5.3 Vn tc gc: trng hp tng qut
Xt vn tc gc quay quanh trc ty . Gi s ma trn quay R thay i theo thi gian,
ngha l,
) 3 ( ) ( SO t R R
, ta c:
) ( ) ( ) ( t R t S t R

(5.20)
V
) (t S
l skew symmetric, n c th biu din nh
)) ( ( t S
i vi vect duy nht
) (t
.
Vect
) (t
l vn tc gc ca h ta quay i vi h ta c nh ti thi im
t
.
) ( )) ( ( ) ( t R t S t R

(5.21)
V d 5.4: Gi s
) ( ,
) (
t x
R t R

) ( )) ( ( ) ( ) ( t R t S t R i S
dt
d
d
dR
R

(5.22)
trong

i
5.4 Cng vn tc gc
Kho st vn tc gc ca 2 h ta
1 1 1 1
z y x o v
2 2 2 2
z y x o chuyn ng tng i vi h
ta c nh
0 0 0 0
z y x o
. Ta c
) ( ) ( ) (
1
2
0
1
0
2
t R t R t R (5.23)
Theo kt qu (5.22), vi phn (5.23) ta c
0
2
0
2
1
2
0
1
1
2
0
1
0
2
) ( R S R R R R R +

(5.24)
Tnh s hng th 1:
0
2
0 1
2
0
1
0 1
2
0
1
) ( ) ( R S R R S R R
a a

(5.25)
Tnh s hng th 2:
0
2
1 0
1
1
2
0
1
1 0
1
1
2
0
1
0
1
1 0
1
1
2
1 0
1
1
2
0
1
) (
) ( ) (
) (
R R S
R R R S R R R S R
R S R R R
b
b
T
b
b

(5.26)
T (5.24) ta thu c { }
0
2
1 0
1
0 0
2
0
2
) ( ) ( ) ( R R S S R S
b a
+ (5.27)
V
) ( ) ( ) ( b a S b S a S + +
ta c

1 0
1
0 0
2 b a
R + (5.28)
Kt qu trn c th c m rng cho bt k h trc ta
1 1
2
0
1
0
...

n
n n
R R R R (5.29)
4
0 0 0
) (
n n n
R S R

(5.30)
Trong
1 0
1
3
4
0
3
2
3
0
2
1
2
0
1
0
1
0
...

+ + + + +
n
n n n
R R R R (5.31)
5.5 Vn tc thng ca mt im gn vo mt h ta chuyn ng
Gi s im
p
c gn cng vo h ta
1 1 1 1
z y x o v
1 1 1 1
z y x o quay tng i vi h
ta
0 0 0 0
z y x o
. Ta c
1 0
1
0
) ( p t R p (5.32)
Vn tc im c tnh nh sau:
0 0 0 0
1 0
1
0
1 0
1
1 0
1
0
) (
) ( ) (
) ( ) (
p p S
p t R S
p t R p t R p
x

(5.33)
Lu : trong biu thc (5.33), 0
1
p
v ta ca im khng i i vi
1 1 1 1
z y x o .
By gi, xt ma trn thun nht biu din chuyn ng tng i ca
1 1 1 1
z y x o i vi
0 0 0 0
z y x o
ph thuc thi gian:
1
]
1

1 0
) ( ) (
) (
0
1
0
1 0
1
t O t R
t H
(5.34)
n gin vic k hiu ta b tham s t v cc ch s
O Rp p +
1 0
(5.35)
Vi phn (5.35) ta c
v r
O Rp S
O p R p
+
+
+
x

1
1 0
) (
(5.36)
trong
1
Rp r l vect t
1
O n
p
c biu din theo hng ca h ta
0 0 0 0
z y x o
,
v
v
l tc ti gc
1
O s dch chuyn.
5.6 Xy dng ma trn Jacobian
Xt tay my
n
khu vi cc bin khp
n
q q ,...,
1
. t
5
1
]
1

1 0
) ( ) (
) (
0 0
0
q O q R
q T
n n
n
(5.37)
biu din php bin i t h ta c cu tc ng cui i vi h ta nn, v
T
n
q q q ) ,..., (
1
l vect bin khp. Khi robot chuyn ng,
i
q
, ) (
0
q O
n
v ) (
0
q R
n
thay i
theo thi gian. Mc tiu phn ny l tm mi lin h gia vn tc thng v vn tc gc ca
c cu tc ng cui vi vn tc khp
) (t q
. t
T
n n n
R R S ) ( ) (
0 0 0

(5.38)
biu th vn tc gc
0
n
ca c cu tc ng cui, v t
0 0
n n
O v

(5.39)
biu th vn tc thng ca c cu tc ng cui. Ta tm biu thc c dng
q J
q J v
n
v n

0
0
(5.40)
hay
q J
v
n
n
n

0
0
0

1
]
1

(5.41)
trong
v
J
v

J
l cc ma trn 3xn, v Jacobian
n
J
J
J
v
n
x 6
0

1
]
1

,
n
l s khu
Ta s tm mt biu thc Jacobian n gin cho bt k tay my no.
(1) Vn tc gc
t
1 i
i
l vn tc gc khu i tng i vi h ta
1 1 1 1 i i i i
z y x o
gy bi chuyn ng
quay ca khp i .
k
i
i
i i
i
i
q z q

1
1
1
(5.42)
Nu khp i l khp trt, chuyn ng ca h ta i i vi h ta
) 1 ( i
l chuyn
ng tnh tin, ngha l 0
1

i
i

V vy, vn tc gc ton b ca c cu tc ng cui,

0
n
, trong h ta nn c xc
nh nh sau:

+ + +
n
i
i i i n n n n
z q R q R q q
1
0
1
0
1
0
1 2 2 1 1
0
... k k k
(5.43)
6
trong
1
i

i vi khp quay, v
0
i

i vi khp trt, v
k
0
1
0
1

i i
R z
D nhin
T
z ) 1 , 0 , 0 (
0
0
, v phn t
[ ]
1 0 1
...

n n
z z J

5.6.2 Vn tc thng
Vn tc thng ca c cu tc ng cui ch l
0
n
O

, ta c
i
n
i i
n
n
q
q
O
O

1
0
0
(5.44)
Ct th i ca
v
J
c cho nh sau:
i
n
vi
q
O
J

0
(5.45)
Ct th i ca Jacobian c th tm c bng cch gi tt c khp c nh tr khp th i
v kch hot khp i vi vn tc n v.
By gi ta xt 2 trng hp: khp quay v khp trt
(1) Trng hp 1 : khp trt
1
]
1

+ +

1
]
1

1
]
1

1
]
1

1
]
1

1 0
1 0 1 0 1 0
1 0
0
1
1 0
1
0
1 1 0
1 1
1 0
1
0
0 0
i
i
i i
i
n i
o
n
i
n
i
n
i
i
i
i i
o
i
i
n
i
i i
n
n n
O O R O R R
O R O R O R
T T T
T
O R
(5.46)
Nh vy
0
1
1 0
1
0 0

+ +
i
i
i i
i
n i n
O O R O R O
Nu ch c khp i chuyn ng th
i
n
O v
0
1 i
O l hng s. Hn na, nu khp i l khp
trt th ma trn quay
0
1 i
R cng l hng s. Cui cng, theo qui c D-H th
T
i i i i i i
i
i
d s s a c a O ) , , , (
1

. Cc ma trn quay
0
i
R v
0
1 i
R cng l hng s i vi
i
d
. Ta c
7
0
1
0
1 1
1
1
0
1
0
0

1
1
1
]
1

1
1
1
]
1

i i i i
i
i i
i i
i
i
i
i
i
i
i
i i
n
z d R d
d
s a
c a
d
R O R
d q
O

(5.47)
hay
1

i vi
z J
(2) Trng hp 2 : khp quay
Ta c
i i
q
. V
0
i
R khng l hng s i vi
i

, ta thu c
[ ]
) (
) )( (
] )[ (
) ( ) (
0
1
0 0
1
0
1
0 0
1
1 0
1
0 0
1
1 0
1
0
1
0 0
1
1 0
1
0
1 0
1
0 0

+
+

i n i i
i n i i
i
i i
i
n i i i
i
i i i i
i
n i i i
i
i
i
i
i
n i
i
i
i i
i
n i
i
n
i
O O z
O O z S
O R O R z S
O R z S O R z S
O R O R
O R O R O
x

(5.48)
Tnh

1 0
1
0
1
1 0
1
0
1
1 0
1
0
1
0
1
1 0
1
0
1
0
1
) (
) (
) )( (
) (
0

1
1
1
]
1

1
1
1
]
1

i
i i i i
i
i i i i
i
i i
T
i i i
i
i i i
i i i
i i
i
i
i i
i i
i
i
O R z S
O R k R S
O R R k S R
O k S R
c a
s a
R
d
s a
c a
R

(5.49)
V vy,
) (
1 1

i n i vi
O O z J x
(5.50)
Cng thc (5.50) c minh ha trn hnh 5.1
8
Hnh 5.1: Chuyn ng ca c cu tc ng cui do khu i
Kt hp Jacobian gc v Jacobian thng
- Na trn ca Jacobian
v
J
c cho nh sau:
[ ]
vn v v
J J J ...
1

(5.52)
Nu khp i l khp quay, phn t ct th i c tnh:
) (
1 1

i n i vi
O O z J x
(5.53)
Nu khp i l khp trt ,
1

i vi
z J
(5.54)
- Na di ca Jacobian

J
c cho nh sau:
[ ]
n
J J J

...
1

(5.55)
Nu khp i l khp quay, phn t ct th i c tnh:
1

i i
z J

(5.56)
Nu khp i l khp trt ,
0
i
J

(5.57)
Nh vy, Jacobian cho tay my
n
khu c dng nh sau:
[ ]
n
J J J J ...
2 1

(5.58)
9
trong phn t ct th i :
1
]
1

1
1 1
) (
i
i n i
i
z
O O z
J
x
, nu khp i l khp quay (5.59)
1
]
1

0
1 i
i
z
J
, nu khp i l khp trt (5.60)
Cng thc trn lm cho vic xc nh Jacobian ca bt k tay my no tr nn n gin v
c sn tt c cc i lng cn tnh mt khi bi ton ng hc thun tnh c. Cc i
lng tnh Jacobian ch l vect n v
i
z
v ta ca cc gc
n
O O ,...,
1
. Kt qu trn
rt quan trng khi ta cn tnh vn tc ca khi tm trn cc khu tnh phng trnh
ng lc hc ca chuyn ng.
V d 5.5: Xt tay my 3 khu phng (hnh 5.2). Tnh vn tc thng
v
v vn tc gc

.
Hnh 5.2: Tm vn tc khu 2 ca robot 3 khu phng .
Ta c
[ ]q J J J
v

3 2 1

1
]
1

(5.61)
trong cc ct ca Jacobian c xc nh theo cng thc (5.59) v (5.60) vi
n
O
c
thay bng
c
O
:
0
) (
) (
3
1 1 2
0 0 1

J
O O z J
O O z J
c
c
x
x
(5.62)
V vn tc ca khu 2 khng b nh hng bi s chuyn ng ca khu 3 (ch xt v mt
10
ng hc). Tht vy, phn lc ca khu 3 s nh hng n s chuyn ng ca khu 2;
vn ny s c xt trong phn kho st ng lc hc .
V d 5.6: Xt tay my 2 khu phng nh hnh v.
Ma trn Jacobian c dng nh sau:
1
]
1

1 0
1 2 1 0 2 0
) ( ) (
) (
z z
O O z O O z
q J
x x
(5.63)
Hnh 5.3: Tay my 2 khu phng
Cc i lng trn c tnh nh sau:
1
1
1
]
1

0
0
0
0
O
,
1
1
1
]
1

0
1 1
1 1
1
s a
c a
O
,
1
1
1
]
1

+
+

0
12 2 1 1
12 2 1 1
2
s a s a
c a c a
O
,
1
1
1
]
1

1
0
0
1 0
z z
(5.64)
1
1
1
1
1
1
1
1
]
1

1 1
0 0
0 0
0 0
12 2 12 2 1 1
12 2 12 2 1 1
c a c a c a
s a s a s a
J
(5.65)
Ta thy rng 2 hng u tin ca (5.65) l Jacobian vn tc thng ca
2
O tng i vi
nn. Hng th 3 l Jacobian vn tc thng theo hng
0
z
, bng 0 trong trng hp ny.
11
Ba hng cui biu din vn tc gc ca h ta cui cng, ch n gin l php quay
quanh trc thng ng vi tc
2 1


+ .
V d 5.7: Tay my SCARA
Jacobian l mt ma trn 6x4 v SCARA ch c 4 bc t do. Ta cn tnh ma trn
j j
A A T ...
1
0

Jacobian c dng sau

1
]
1

3 1 0
2 1 4 1 0 4 0
0
0 ) ( ) (
z z z
z O O z O O z
J
x x
(5.65)
1
1
1
]
1

0
1 1
1 1
1
s a
c a
O
,
1
1
1
]
1

+
+

0
12 2 1 1
12 2 1 1
2
s a s a
c a c a
O
,
1
1
1
]
1

+
+

4 3
12 2 1 1
12 2 1 1
4
d d
s a s a
c a c a
O
(5.66)
Tng t
k
1 0
z z
, v
k
3 2
z z
. V vy, Jacobian tay my SCARA l
1
1
1
1
1
1
1
1
]
1

1 0 1 1
0 0 0 0
0 0 0 0
0 1 0 0
0 0
0 0
12 2 12 2 1 1
12 2 12 2 1 1
c a c a c a
s a s a s a
J
(5.67)
5.7 K d (Singularities)
Jacobian
) 6 ( ) ( n J x q
xc nh mt nh x gia
q
(vn tc khp) v
T
) , ( v X

(vn
tc c cu tc ng cui):
q q X

) ( J (5.68)
(5.68) xc nh mt php bin i tuyn tnh vi phn gia
q d
v X d
q q X d J d ) (
(5.69)
Cc vi phn ny c th c th ngh nh l vic xc nh hng trong
6
R
v
n
R
, tng
ng.
Cc v tr m ti hng ca Jacobian gim c tm quan trng c bit. V tr gi l v
tr k d (singularities hay singilar configuration). Vic xc nh im k d cho tay my l
quan trng v nhiu l do:
12
(1). Cc im k d biu din cc v tr ti hng ca chuyn ng khng tnh c.
(2). Ti cc im k d, vn tc bao ca c cu tc ng cui c th tng ng vi vn
tc khp khng b bao.
(3). Ti cc im k d, lc bao ca c cu tc ng cui c th tng ng vi mmen
quay ca khp khng b bao.
(4) Cc im k d thng (khng lun lun) tng ng vi cc im trn bin khng
gian lm vic ca tay my, c ngha l, tm vi ti a ca tay my.
(5). Cc im k d tng ng vi cc im trong khng gian lm vic tay my khng th
t ti khi c s thay i nh ca tham s khu, nh chiu di khu, di, v.v
(6) Gn cc im k d s khng tn ti nghim duy nht cho bi ton ng hc nghch.
Trong trng hp nh th c th v nghim hay v s nghim.
V d 5.8: Xt phng trnh h thng hai chiu
q q X d Jd d
1
]
1

0 0
1 1
(5.70)
tng ng vi 2 phng trnh
0
2 1

+
dy
dq dq dx
(5.71)

1 ) ( J rank
v ta c th thy rng vi bt k gi tr ca
1
dq v
2
dq khng lm thay i
dy
. Nn bt k vect X d c thnh phn th 2 khc khng biu din hng khng th t
c ca chuyn ng tc thi.
5.8.1 Tch cc im k d
Ta thy rng c th tnh c mt tp cc phng trnh ng hc thun cho tay my bt
k bng cch gn cng mt h ta vo tng khu theo cch bt k, tnh cc php bin
i homogenous v nhn chng li vi nhau. Qui tc D-H ch l mt cch thc hin
iu ny. D cc phng trnh kt qu ph thuc vo cc h ta chn, th v tr ca
tay my, v bn cht l cc i lng hnh hc, c lp vi vic gn cc h ta .
iu ny cho php ta tch vic xc nh cc v tr k d, cho cc tay my khp cu, thnh
2 bi ton n gin hn. Bi ton 1 l xc nh v tr k d cho cnh tay, bao gm 3 khu
hay nhiu hn, bi ton th 2 l xc nh k d cho c tay t chuyn ng ca c tay khp
cu.
Xt tay my c 3 bc t do cho cnh tay v 3 bc t do cho c tay. Trong trng hp ny,
Jacobian l ma trn 6x6 v mt v tr
q
l k d nu v ch nu
0 ) ( det q J
(5.72)
13
Jacobian c th vit nh sau:
[ ]
1
]
1

22 21
12 11
J J
J J
J J J
O p
(5.73)
Sau , v 3 khp cui cng lun l khp quay
1
]
1

5 4 3
5 6 5 4 6 4 3 6 3
) ( ) ( ) (
z z z
O O z O O z O O z
J
O
x x x
(5.74)
V cc trc c tay ng quy ti mt im chung O, nu ta chn cc h ta sao cho
0
6 5 4 3
O O O O
, th
O
J
c tnh nh sau:
1
]
1

5 4 3
0 0 0
z z z
J
O
(5.75)
Ct th i ca
p
J
l
1
]
1

1
1 1
) (
i
i i
i
z
O O z
J
x
, nu khp i l khp quay (5.76)
1
]
1

0
1

i
i
z
J
, nu khp i l khp trt (5.77)
Ma trn Jacobian c dng khi tam gic
1
]
1

22 21
11
0
J J
J
J
(5.78)
vi nh thc
22 11
det det det J J J
11
J c ct th i ) (
1 1

i i
O O z x
nu khp i l khp quay, v
1 i
z
nu khp i l khp trt.
[ ]
5 4 3 22
z z z J
(5.79)
V vy tp cc v tr k d ca tay my l hp ca tp cc v tr cnh tay tha 0 det
11
J .
5.7.2 K d c tay
14
Hnh 5.4: K d c tay khp cu
5.7.3 K d cnh tay
Hnh 5.5: Cnh tay khp bn l
15