1.

XA
O
YA
YB
XB
ZB
OB
ZA

Ta c
]
]
]
]
]
]

0 0 0 0
z z z z
y y y y
x x x x
B
A
P a O n
P a O n
P a O n
T
D dng thy
B
A
T
.
A
B
T
=
A
A
T
=I

B
A
T . [
B
A
T ]
-1
=I

A
B
T = [
B
A
T ]
-1
Do ta s ch xc nh ma trn
A
B
T
t ma trn
B
A
T
Nhn xt
A
B
T
=
B
A
R
0 0 0 1
A
B
O
P ]
]
]
]

Trong
A
B
R
l ma trn ch hng hay l ma trn quay ca A trong to B

OA
B
P
V tr ca tm OA trong to B
Ta c
A
B
R
= [ ]
A
B
A
B
A
B
k j i =
. . .
. . .
. . .
A B A B A B
A B A B A B
A B A B A B
i i j i k i
i j j j k j
i k j k k k
]
]
]
]
]
]
ur ur uur ur uur ur
ur uur uur uur uur uur
ur uur uur uur uur uur

B
A
R
= [ ]
B
A
B
A
B
A
k j i =
. . .
. . .
. . .
B A B A B A
B A B A B A
B A B A B A
i i j i k i
i j j j k j
i k j k k k
]
]
]
]
]
]
ur ur uur ur uur ur
ur uur uur uur uur uur
ur uur uur uur uur uur
=
]
]
]
]
]

z z z
y y y
x x x
a O n
a O n
a O n



B
A
R
=
]
]
]
]
]

z z z
y y y
x x x
a O n
a O n
a O n
ta c
A A A
i j k
]
]
ur uur uur
=
B B B
i j k
]
]
ur uur uur
.
A
B
R
M
1
A B
O O
uuuuur
=
A A A
i j k
]
]
ur uur uur
.
]
]
]
]
]

z
y
x
P
P
P
=
B B B
i j k
]
]
ur uur uur
A
B
R
]
]
]
]
]

z
y
x
P
P
P

B A
O O
uuuuur
= -
B B B
i j k
]
]
ur uur uur
.
]
]
]
]
]

z z z
y y y
x x x
a O n
a O n
a O n
.
]
]
]
]
]

z
y
x
P
P
P

B A
O O
uuuuur
=
B B B
i j k
]
]
ur uur uur
.
.
.
.
p n
p o
p a
]
]

]
]
]

A
O
B
P
=
.
.
.
p n
p O
p a
]
]

]
]
]
trong p.n =

z y x
x x
n P
, ,
Vy ta c
A
B
T =[ ]
1
B
A
T =
]
]
]
]
]
]

1 0 0 0
O
z
pa a a a
pO O O
pn n n n
z y x
y x
z y x
2
X2
z2
X3
z3
X0=X1
z0
z1
l
2
l
1

Lp bng
Khung to i

i
a
i
d
i

1
0
90 0 0

*
1

2
0
90 0
1
l

0
90
3 0 0
2
l
0
2
1
0
A
=
1 1
1 1
0 0
0 0
0 1 0 0
0 0 0 1
C S
S C
]
]

]
]
]
]
;
2
1
A
=
1
0 0 1 0
1 0 0 0
0 1 0
0 0 0 1
l
]
]
]
]
]
]
;
3
2
A
=
2
1 0 0 0
0 1 0 0
0 0 1
0 0 0 1
l
]
]
]
]
]
]
Php bin i biu din

3
2
2
1
1
0
3
0
. . A A A T
=
1 1
1 1
0 0
0 0
0 1 0 0
0 0 0 1
C S
S C
]
]

]
]
]
]
.
1
0 0 1 0
1 0 0 0
0 1 0
0 0 0 1
l
]
]
]
]
]
]
.
2
1 0 0 0
0 1 0 0
0 0 1
0 0 0 1
l
]
]
]
]
]
]
=
1 1 1 2 1 1
1 1 1 2 1 1
0
0
1 0 0 0
0 0 0 1
S C C l S l
C S S l C l
+ ]
]

]
]
]
]
I- Nhm 14 (nhm Sn)
Bi 1:
Gi s, ta c hai h trc ta l
{ A
v
{ B
. Mt im bt k trong khng gian s c
ta trong
{ A
l p
A
= (x
A
, y
A
, z
A
) v trong
{ B
l p
B
= (x
B
, y
B
, z
B
).
Ma trn ng nht l
B
A
T v
A
B
T
- Theo , ta c:
[ ]
[ ]
A
B
1
B
A
A A
B
B
A
1
B
A
B B B
A
A
T T
p . T p
p . T p p . T p

'

]
]
]

]
]
]
]
]
]

1 0
P R
1 0 0 0
p a o n
p a o n
p a o n
T
B
A
B
A
Z Z Z Z
Y Y Y Y
X X X X
B
A
- Gi s, ma trn nghch o c dng
[ ]
]
]
]

1 0
P R
T
* *
1
B
A
3
- Li c:
[ ]
[ ] [ ] [ ] [ ] [ ] [ ]
]
]
]

]
]
]

]
]
]

1 0
0 I
1 0
P R
1 0
P R
I T . T
1 x 3 3 x 3 1 x 3
*
3 x 3
*
1 x 3 B
A
3 x 3 B
A
1
B
A
B
A
Suy ra:
[ ]
[ ]

'

'

B
A
1
B
A *
B
A
1
B
A *
B
A *
B
A
*
B
A
P . R P . R
I . R R
0 P P . R
I R . R
- Ngoi ra, theo tnh cht ca ma trn xoay ta c: [ ] [ ]
T
B
A
1
B
A
R R

T ta tnh c:
[ ]
]
]
]
]
]
]

1 0 0 0
a . p a a a
o . p o o o
n . p n n n
T T
Z Y X
Z Y X
Z Y X
1
B
A
A
B
4
Bi 2:
2.1 Xc nh ng hc thun theo lut D H
5
2.1.1 Xc nh h thng cc trc ta
6
2.1.2. Xc nh tham s ng hc cho cc thanh ni
Thanh ni 1:
- Do hai trc Z giao nhau nn di ng vung gc chung a = 0.
- Do Z
0
xoay quanh X
0
mt gc 90
o
theo chiu kim ng h th trng vi Z
1
nn gc = -90
o
.
- Do gc ta O
0
trng O
1
nn khong cch t tm O
0
n ng vung gc chung d = 0
7
- Khi thanh ni 1 quay mt gc
1
quanh trc Z
0
th X
0
cng phi quay mt gc
1
mi li trng
vi X
1
nn =
1
Thanh ni 2
- Do hai trc Z giao nhau nn di ng
vung gc chung a = 0.
- Do Z
1
xoay quanh X
0
mt gc 90
o
theo
chiu kim ng h th trng vi Z
2
nn gc
= -90
o
.
- Do gc ta O
1
cch O
1
mt khong cch
bng chnh khong di chuyn ca thanh ni
2 nn d = r
- Mc d thanh ni 2 khng xoay nhng trc
X1 phi xoay quanh Z1 mt gc 90
o
theo
chiu kim ng h mi trng vi X2 nn =
-90
o
Thanh ni 3:
- Do hai trc Z trng nhau nn di ng vung gc chung a = 0 v = 0.
- Do gc ta O
2
cch O
3
mt khong khng i l L nn d = L
- Khi thanh ni 3 quay mt gc
3
quanh trc Z
2
th X
2
cng phi quay mt gc
3
mi li trng
vi X
3
nn =
3
8
r
T nhng phn tch trn ta c bng D-H nh sau:
i a d
1 -90
o
0 0
1
2 -90
o
0 r -90
3 0 0 L
3
- Vit cc ma trn m t hng
]
]
]
]
]
]

]
]
]
]
]
]

1 0 0 0
0 0 1 0
0 C 0 S
0 S 0 C
1 0 0 0
0 ) 90 ( Cos ) 90 ( Sin 0
0 Cos ) 90 ( Sin Cos ) 90 ( Cos Sin
0 Sin ) 90 ( Sin Sin ) 90 ( Cos Cos
A
1 1
1 1
o o
1
o
1
o
1
1
o
1
o
1
1
0
]
]
]
]
]
]

]
]
]
]
]
]

1 0 0 0
r 0 1 0
0 0 0 1
0 1 0 0
1 0 0 0
r ) 90 ( Cos ) 90 ( Sin 0
0 ) 90 ( Cos ) 90 ( Sin ) 90 ( Cos ) 90 ( Cos ) 90 ( Sin
0 ) 90 ( Sin ) 90 ( Sin ) 90 ( Sin ) 90 ( Cos ) 90 ( Cos
A
o o
o o o o o
o o o o o
2
1
]
]
]
]
]
]

]
]
]
]
]
]

1 0 0 0
L 1 0 0
0 0 3 C S
0 0 3 S C
1 0 0 0
L ) 0 ( Cos ) 0 ( Sin 0
0 Cos ) 0 ( Sin Cos ) 0 ( Cos Sin
0 Sin ) 0 ( Sin Sin ) 0 ( Cos Cos
A
3
3
o o
3
o
3
o
3
3
o
3
o
3
3
2
9
- Ma trn ng nht ca h thng
]
]
]
]
]
]

]
]
]
]
]
]

]
]
]
]
]
]

]
]
]
]
]
]

]
]
]
]
]
]

]
]
]
]
]
]

1 0 0 0
0 0 S C
rC LS S C C C C
rS LC C C S S S
1 0 0 0
L 1 0 0
0 0 C S
0 0 S C
1 0 0 0
0 0 0 1
rC S C 0
rS C S 0
1 0 0 0
L 1 0 0
0 0 C S
0 0 S C
1 0 0 0
r 0 1 0
0 0 0 1
0 1 0 0
1 0 0 0
0 0 1 0
0 C 0 S
0 S 0 C
T
3 3
1 1 1 3 1 3 1
1 1 1 3 1 3 1
3 3
3 3
1 1 1
1 1 1
3 3
3 3
1 1
1 1
3
0
III- Nhm Chu:
Cu 2:
1. Xc lp h ng hc thun v tr theo lut D-H
10
z0
z1 z2 z3
x3 x2 x0, x1
O2 O0, O1 O3
Bng D-H:
i
i
a
i
d
i

i
1 90 0 0
1
*
2 0 l
2
0
2
*
3 0 l
3
0
3
*
p dng cng thc xc nh cc ma trn bin i:
1
0
0 0 0 1
i i i i i i i
i i i i i i i i
i
i i i
c c s s s a c
s c c c s a s
A
s c d

]
]

]
]
]
Ta xc nh c cc ma trn bin i nh sau:
1 1
1 1 0
1
0 0
0 0
0 1 0 0
0 0 0 1
c s
s c
A
]
]

]
]
]
;
2 2 2 2
2 2 2 2 1
2
0
0
0 0 1 0
0 0 0 1
c s l c
s c l s
A
]
]
]

]
]
]
;
3 3 3 3
3 3 3 3 2
3
0
0
0 0 1 0
0 0 0 1
c s l c
s c l s
A
]
]
]

]
]
]
Ma trn bin i ng nht:
1 23 1 23 1 3 1 23 2 1 2
1 23 1 23 1 3 1 23 2 1 2 0 0 1 2
3 1 2 3
23 23 3 23 2 2
0
0 0 0 1
c c c s s l c c l c c
s c s s c l s c l s c
T A A A
s c l s l s
+ ]
]
+
]

] +
]
]
IV- Nhm Duy Minh:
Bi 2.
11
1. Xc lp h ng hc thun v tr cho Robot theo lut D-H
Robot c ba bc t do
Gm 4 thanh ni t 0 n 4
Gm 3 khp ni, trong : - Khp 1: Khp quay
- Khp 2: Khp tnh tin
- Khp 3: Khp quay
Da theo lut D-H, ta xc nh h trc ta ca thanh ni nh hnh v sau:
3
d2 O3
O2
O1
Y1
Z1
X3
Z3
X2
Z2
X1
Z0
X0
Y0
1
O0
a1
a3
a2
- Bng D-H nh sau:
0 0
90
a3
a2
d2 0
- 3
1 0 0
3
2
1
i i di ai i
a1
*
*
*
- T bng D-H, ta thnh lp cc ma trn:
i
i
i
. .
. .
0
0 0 0 1
i
c i c i s i s i s i ac i
s i c i c i s i c i as i
A
s i c i d

]
]

]
]
]
12
- Thay cc gi tr trong bng D-H vo ma trn trn, ta c cc ma trn nh sau:
1
1
1 1 0 0
1 1 0 0
0 0 1
0 0 0 1
c s
s c
A
a


]
]
]

]
]
]

2
2
2
1 0 0
0 0 1 0
0 1 0
0 0 0 1
a
A
d
]
]

]
]
]
3
3
3
3 3 0 3
3 3 0 3
0 0 1 0
0 0 0 1
c s ac
s c a s
A


]
]

]

]
]
]
- Ta c:
3 3 3 3 2
2 3
3 3 2 3 3
0
0 0 1 0
.
0 d
0 0 0 1
c s a c a
A A
s c a s
+
]
]

]
]
]
Ma trn bin i ng nht m t v tr ca khu tc ng cui trong h to c nh l:
3
1 3 1 3 1 3 1 3 2 1
1 3 1 3 1 3 1 3 2 1 0
3 3 2 1 3 3
0 d
0 0 0 1
cc cs s a cc a c
s c s s c a s c a s
T
s c a a s
+
]
]
+
]

] +
]
]
V- Nhm Phng:
Bi 2: Cho Robot 3DOF .
1. Xc nh h ng hc thun v tr theo lut D-H
2. Tnh ton ng hc o v tr
3. Thit lp h phng trnh ng lc hc
13
X0,X1,X2
Z0,Z1
O0, O1
O2
Z2
O3
Z3
X3
?1
d2
?3
l
C2
lC2
lC3
C3
Xc lp h ng hc thun v tr theo lut D-H
- Chn cc h trc ta nh hnh v
- Lp bng D-H
i
i
a
i
d
i

i
1 0 0 0
1
2 -90
o
0 d
2
0
3 0 0 l
3
T bng D-H ta tnh c:
]
]
]
]
]
]

1 0 0 0
0 1 0 0
0 0
0 0
1 1
1 1
1
0


c s
s c
A
,
]
]
]
]
]
]

1 0 0 0
0 1 0
0 1 0 0
0 0 0 1
2
2
1
d
A
,
]
]
]
]
]
]

1 0 0 0
1 0 0
0 0
0 0
3 3
3 3
3
2
l
c s
s c
A



3
2
2
1
1
0
3
0
. . A A A T
]
]
]
]
]
]

1 0 0 0
0 1 0 0
0 0
0 0
1 1
1 1


c s
s c

]
]
]
]
]
]

3
2
2
0
3
2
2
. .
1 0 0 0
0 1 0
0 1 0 0
0 0 0 1
. A A A
d
]
]
]
]
]
]

]
]
]
]
]
]

]
]
]
]
]
]

1 0 0 0
0
. . .
. . .
1 0 0 0
1 0 0
0 0
0 0
.
1 0 0 0
0 1 0
0 0
0 0
2 3 3
1 1 3 1 3 1
1 1 3 1 3 1
3 3
3 3
2
1 1
1 1
3
0
d c s
c l c s s c s
s l s s c c c
l
c s
s c
d
c s
s c
T







Vy ta c ma trn bin i ng nht ta ca mt im trong h {O
3
} sang h ta {O
0
}
14
]
]
]
]
]
]

1 0 0 0
0
. . .
. . .
2 3 3
1 1 3 1 3 1
1 1 3 1 3 1
3
0
d c s
c l c s s c s
s l s s c c c
T



VI- Nhm Tu n D ng :
Cu 2 :1. Xc lp h ng hc thun v tr cho Robot theo lut D-H
Ta xc nh h trc ta ca h nh sau:
T ta xy dng c bng D-H:
i a
i

i
d
i
1 0 90
o
90
o
d
1
2 0 0 0 d
2
3 0 0
3
l
3
Xy dng cc ma trn bin i ng nht gia cc h ta ca cc thanh ni theo cng
thc:
15
X
0
Z
0
O
0
O
1
, O
2
Z
1
, Z
2
X
1
, X
2
O
3
Z
3
d
1
d
2

3
X
3
d
1
3
d

2
z
1
z
2
z
3 , x
1
2
x x
3
r
3
0
z
0
x
1
r
i-1
A
i
=
0
0 0 0 1
i i i i i i i
i i i i i i i
i i i
c c s s s a c
s c c s c a s
s c d



]
]

]
]
]
]
- ta c:
0
A
1
=
1
0 0 1 0
1 0 0 0
0 1 0
0 0 0 1
d
]
]
]
]
]
]
;
1
A
2
=
2
1 0 0 0
0 1 0 0
0 0 1
0 0 0 1
d
]
]
]
]
]
]
;
2
A
3
=
3 3
3 3
3
0 0
0 0
0 0 1
0 0 0 1
c s
s c
l


]
]
]
]
]
]
- Phng trnh ng hc ca robot:
0
T
E
=
1
0 0 1 0
1 0 0 0
0 1 0
0 0 0 1
d
]
]
]
]
]
]
.
2
1 0 0 0
0 1 0 0
0 0 1
0 0 0 1
d
]
]
]
]
]
]
.
3 3
3 3
3
0 0
0 0
0 0 1
0 0 0 1
c s
s c
l


]
]
]
]
]
]
=
0
T
E
=
2 3
3 3
3 3 1
0 0 1
0 0
0
0 0 0 1
d l
c s
s c d


+ ]
]

]
]
]
]
VII- Ca nhm mnh:
Cu 2: Cho Robot 3DOF c cu hnh nh trn hnh :
1- Xc lp h ng hc thun v tr theo lut D - H
- Bng D H ca Robot:
i
i
(
0
) a
i
d
i
i
(
0
)
16
1 90 0 r
1
0
2 90 0 0 (90 +
*
2

)
3 0 0 r
3
0
T bng D H, ta tnh c cc ma trn bin i:
0
1
1
1 0 0 0
0 0 1 0
A (2.1)
0 1 0 r
0 0 0 1
]
]

]
]
]
2 2
2 2 1
2
s 0 c 0
c 0 s 0
A (2.2)
0 1 0 0
0 0 0 1
]
]

]

]
]
]
2
3
3
1 0 0 0
0 1 0 0
A
0 0 1 r
0 0 0 1
]
]
]

]
]
]
(2.3) ==>
2 2 3 2
0 0 1 2
3 1 2 3
2 2 1 3 2
s 0 c r c
0 1 0 0
T A A A
c 0 s (r r s )
0 0 0 1
]
]

]

] +
]
]
VIII- Nhm Nha:
Bi 2. 1. Xc lp h ng hc thun v tr cho Robot theo lut D-H
Robot c ba bc t do
Gm 4 thanh ni t 0 n 4
Gm 3 khp ni, trong : Khp 1: Khp quay
Khp 2: Khp tnh tin
Khp 3: Khp quay
Da theo lut D-H, ta xc nh h trc ta ca thanh ni nh hnh v sau
17
d2 O3
O1
Y1
1
X3 X2
X1
Z0
X0
Y0
1
O0
a1
a2
3 a3
Z3
Z2
Y3 Y2
O2
- Bng D-H nh sau:
0 0
90
a3
a2
d2 0
1 0 0
3
2
1
i i di ai i
a1
*
*
3
*
- T bng D-H, ta thnh lp cc ma trn:
i
i
i
. .
. .
0
0 0 0 1
i
c i c i s i s i s i ac i
s i c i c i s i c i a s i
A
s i c i d

]
]

]
]
]
- Thay cc gi tr trong bng D-H vo ma trn trn, ta c cc ma trn nh sau:
1
1
1 1 0 0
1 1 0 0
0 0 1
0 0 0 1
c s
s c
A
a

]
]
]

]
]
]
;
2
2
2
1 0 0
0 0 1 0
0 1 0
0 0 0 1
a
A
d
]
]

]
]
]
;
3
3
3
3 3 0 3
3 3 0 3
0 0 1 0
0 0 0 1
c s a c
s c a s
A

]
]
]

]
]
]
- Ta c:
3 3 3 3 2
2 3
3 3 2 3 3
0
0 0 1 0
.
0 d
0 0 0 1
c s a c a
A A
s c a s
+
]
]

] +
]
]
- Ma trn bin i ng nht m t v tr ca khu tc ng cui trong h to c nh l:
18
3
1 3 1 3 1 3 1 3 2 1
1 3 1 3 1 3 1 3 2 1 0
3 3 2 1 3 3
0 d
0 0 0 1
c c c s s a c c a c
s c s s c a s c a s
T
s c a a s
+
]
]
+
]

] + +
]
]
19
20
21
Lap bang D H
i
1 90 0
2 90 180
3 0 0



22
i
i
a
i

i
d
i

1 90
0
0 90+
1
*
l
1

2 90
0
0 90-
2
*
0
3 0 0 0 l
2
+ d
3
*
23
1 1
0
1 1
1
1
sin 0 cos 0
cos 0 sin 0
0 1 0
0 0 0 1
l
A


]
]
]
]
]
]
]

2 2
1
2 2
2
sin 0 cos 0
cos 0 sin 0
0 1 0 0
0 0 0 1
A


]
]
]
]
]
]

2
3
2 3
1 0 0 0
0 1 0 0
0 0 1
0 0 0 1
l d
A
]
]
]
]
]
]

+
1 2 1 1 2 1 2
1 2 1 1 2 1 2
2 2 2
3
2 3
2 3
2 3 1
sin cos sin os sin cos ( )
os sin cos cos cos ( )
cos 0 sin sin ( )
0 0 0 1
sin
sin cos
c l d
c l d
l d l
T



]
]
]
]
]
]
+
+

+ +

[
0
A
1
]
-1
]T
3
=
1
A
2
.
2
A
3


24
1 1 1 1 1 1 1 1
2 2 2 2 3
1 2 2 2 2 3
1 1 1 1 1 1 1 1
0 .( )
0 .( )
0 1 0 0
0 0 0 1
0 0 0 1
x y x y x y x y
z z z z
x y x y x y x y
s n c n s o c o s a c a s p c p
s c c l d
n o a p l c s s l d
c n s n c o s o c a s a c p s p
]
]
]
]
]
]
]
]
]
]
] ]
]
]
+ + + +
+
+
+ + + +

x1
z0
x0
y0
z1
x3
y3
z2
z3
y2
x2
Y1
Lc1
a
r2
r
1 3 1 3 1 1 1
1 3 1 3 1 1 1 0
E
3 3
-s .c s .s c c . .
c .c -c .s s s . .
T =
s c 0 0
0 0 0 1
r s a
r c a

+
1 1 1
1 1 1 0
1
-s 0 c -s .a
c 0 s c .a
A =
0 1 0 0
0 0 0 1

1
2
1 0 0 0
0 1 0 0
A =
0 0 1 r
0 0 0 1

3 3
3 3 2
3
c -s 0 0
s c 0 0
A =
0 0 1 0
0 0 0 1

3 3
3 3 1 2
2 3
c -s 0 0
s c 0 0
A . A =
0 0 1 r
0 0 0 1

25