Nhm 1

Ta c th xc nh cc trc to nh sau:

Ta c th xc nh cc tham s ng hc theo:

i
- gc quay t trc Zi-1 ti trc Zi xung quanh trc Xi
ai - khong cch gia trc Zi-1 v Zi

i
- gc quay t trc Xi-1 ti trc Xi xung quanh trc Zi
di - lch ca khu ( di on Oi-1 n Oi)
Vy ta c bng D H nh sau:

Cng thc tng qut ca ma trn
i-1
A
i


Vy ta c cc ma trn chuyn h trc ta :
1 1
1 1 0
1
cos sin 0 0
sin cos 0 0
A
0 0 1 0
0 0 0 1
u u (
(
u u
(
=
(
(


1
2
2
1 0 0 0
0 0 1 0
A
0 1 0 d
0 0 0 1
(
(
(
=
(
(


2
3
3
1 0 0 0
0 1 0 0
A
0 0 1 d
0 0 0 1
(
(
(
=
(
(


4 4
4 4 3
4
cos sin 0 0
sin cos 0 0
A
0 0 1 0
0 0 0 1
u u (
(
u u
(
=
(
(


1 4 1 4 1 3 1
1 4 1 4 1 3 1
4 4 2
. . .
. . .
0
0 0 0 1
c c c s s d s
s c s s c d c
T
s c d
(
(

(
=
(
(




Nhm 2


1 1
1 1 0
1
0 0
0 0
0 1 0 0
0 0 0 1
c s
s c
A
u u
u u
(
(
(
=
(
(


1
2
0 0 1 0
1 0 0 0
0 1 0 0
0 0 0 1
A
(
(

(
=
(
(



1 4 1 4 1 3 1
1 4 1 4 1 3 1
4 4
. . .
. . .
0 0
0 0 0 1
s s s c c r c
c s c c s r s
T
c s
(
(

(
=
(
(




Nhm 3


1 1
1 1 0
1
0 0
0 0
0 1 0 0
0 0 0 1
c s
s c
A
u u
u u
(
(
(
=
(
(


1
2
0 0 1 0
1 0 0 0
0 1 0 2
0 0 0 1
A
r
(
(

(
=
(
(


2
3
1 0 0 0
0 1 0 0
0 0 1 3
0 0 0 1
A
r
(
(
(
=
(
(


4 4
4 4 3
4
0 0
0 0
0 0 1 0
0 0 0 1
c s
s c
A
u u
u u
(
(
(
=
(
(



1 4 1 4 1 3 1 2 1
1 4 1 4 1 3 1 2 1
4 4
. . . .
. . . .
0 0
0 0 0 1
s s s s c r c r s
c s c c s d s r c
T
c s
(
(
+
(
=
(
(


.








Nhm 4





Nhm 5

Bng D-H :

- T bng D-H trn ta xc nh c cc ma trn chuyn sau :



T=






















Nhm 6


Cc ma trn bin i ng nht:
(
(
(
(

=
1 0 0 0
0 0 1 0
0 0
0 0
1 1
1 1
1
0
u u
u u
c s
s c
A

(
(
(
(

=
1 0 0 0
0 0 1 0
0
0
2 2 2 2
2 2 2 2
2
1
u u u
u u u
s a c s
c a s c
A

2
3
3
1 0 0 0
0 1 0 0
0 0 1
0 0 0 1
A
r
(
(
(
=
(
(


4 4
4 4 3
4
0 0
0 0
0 1 1 0
0 0 0 1
c s
s c
A
u u
u u
(
(
(
=
(
(


T ta tnh c ma trn m t hng v v tr ca robot 4DOF nh
sau:
1 2 4 1 4 1 2 4 1 4 1 2 3 1 2
1 2 4 1 4 1 2 4 1 4 1 2 3 1 2
2 4 2 4 2 3 2
. . . . . . . . .
. . . . . . . . .
. . .
0 0 0 1
s s c c s s s s c c s c r s c
c s c s s c s s s c c c r c c
T
c c c s s r s
+ (
(
+ +
(
=
(
(

1 1
1 1 0
1
0 0
0 0
0 1 0 0
0 0 0 1
c s
s c
A
u u
u u
(
(

(
=
(
(

Nhm 8


T bng D-H trn ta xc nh c cc ma trn chuyn sau :
1 1
1 1
1
0 0
0 0
0
0 1 0 0
0 0 0 1
s c
c s
A
(
(

(
=
(
(


2 2 2 2
2 2 2 2
2
0 .
0 .
1
0 0 1 0
0 0 0 1
c s a c
s c a s
A
(
(

(
=
(
(


3
3
1 0 0 0
0 1 0 0
2
0 0 1
0 0 0 1
r
A
(
(
(
=
(
(


4 4
4 4
4
0 0
0 0
3
0 0 1 0
0 0 0 1
c s
s c
A
(
(
(
=
(
(


1 2 4 1 2 4 1 2 4 1 2 4 1 3 1 1 2 2
1 2 4 1 2 4 1 2 4 1 2 4 1 3 1 1 2 2
. . . . . . . . . . .
. . . . . . . . . . .
0 1 0 0
0 0 0 1
s c c s s s s c s s s c c r c s c a
c c c c s s c c s c s c s r c c c a
T
+ + + (
(

(
=
(
(









Nhm 9








Nhm 10


1 1
1 1 0
1
0 0
0 0
0 1 0 0
0 0 0 1
s c
c s
A
u u
u u
(
(
(
=
(
(



1 2 1 1 2 3 1 2
1 2 1 1 2 3 1 2
2 2 3 2
. . . .
. . . .
0 .
0 0 0 1
s c c s s r s s
c c s c s r c s
T
s c r c

(
(

(
=
(
(









Nhm 11


1 1
1 1 0
1
0 0
0 0
0 1 0 0
0 0 0 1
c s
s c
A
u u
u u
(
(
(
=
(
(





Nhm 12



1 1
1 1 0
1
cos sin 0 0
sin cos 0 0
A
0 0 1 0
0 0 0 1
u u (
(
u u
(
=
(
(


1
2
2
1 0 0 0
0 0 1 0
A
0 1 0 d
0 0 0 1
(
(
(
=
(
(


2
3
3
1 0 0 0
0 1 0 0
A
0 0 1 d
0 0 0 1
(
(
(
=
(
(


4 4
4 4 3
4
cos sin 0 0
sin cos 0 0
A
0 0 1 0
0 0 0 1
u u (
(
u u
(
=
(
(


1 4 1 4 1 3 1
1 4 1 4 1 3 1 0 0 1 2 3
4 1 2 3 4
4 4 2
c c -c s -s -d s
s c -s s c d c
Suy ra T= A = A A A A =
-s -c 0 d
0 0 0 1
(
(
(
(
(







Nhm 14


0
1
1 0 0 0
0 0 1 0
0 1 0 1
0 0 0 1
A
r
(
(

(
=
(
(


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


3 3
3 3 2
3
0 0
0 0
0 1 0 0
0 0 0 1
c s
s c
A
u u
u u
(
(
(
=
(
(


4 4
4 4 3
4
0 0
0 0
0 0 1 0
0 0 0 1
c s
s c
A
u u
u u
(
(
(
=
(
(


3 4 3 4 3
4 4 2
3 4 3 4 3 1
. . 0
0
. .
0 0 0 1
c c c s s
s c r
T
s c s s c r
(
(

(
=
(
(








Nhm 15


0
1
0 0 1 0
1 0 0 0
0 1 0 1
0 0 0 1
A
r
(
(
(
=
(
(


2 2
2 2 1
2
0 0
0 0
0 1 0 0
0 0 0 1
s c
c s
A
u u
u u
(
(
(
=
(
(


2
3
1 0 0 0
0 1 0 0
0 0 1 3
0 0 0 1
A
r
(
(
(
=
(
(


4 4
4 4 3
4
0 0
0 0
0 0 1 0
0 0 0 1
c s
s c
A
u u
u u
(
(
(
=
(
(


4 4
2 4 2 4 2 3 2
2 4 2 4 2 3 2
0 0
. . .
. . .
0 0 0 1
s c
s c s s c r c
T
c c c s s r s
u u
u u u u u u
u u u u u u
(
(

(
=
(
(









Nhm 17


0
1
1
0 0 1 0
1 0 0 0
0 1 0
0 0 0 1
A
r
(
(
(
=
(
(

2 2 2 2
2 2 2 2 1
2
0 .
0 .
0 1 0 0
0 0 0 1
s c a s
c s a c
A
u u u
u u u
(
(
(
=
(
(


2
3
1 0 0 0
0 1 0 0
0 0 1 3
0 0 0 1
A
r
(
(
(
=
(
(


4 4
4 4 3
4
0 0
0 0
0 0 1 0
0 0 0 1
c s
s c
A
u u
u u
(
(
(
=
(
(


4 4
2 4 1 4 2 3 2 2 2
2 4 2 4 2 3 2 2 2 1
0 0
. . . .
. . . .
0 0 0 1
s c
s c s s c r c a s
T
c c c s s r s a c r
(
(

(
=
( + +
(






Nhm 18


0
1
1
1 0 0 0
0 0 1 0
A
0 1 0 r
0 0 0 1
(
(
(
=
(
(

2 2 2 2
2 2 2 2 1
2
cos 0 sin a cos
sin 0 cos a sin
A
0 1 0 0
0 0 0 1
u u u (
(
u u u
(
=
(
(


2
3
3
1 0 0 0
0 1 0 0
A
0 0 1 r
0 0 0 1
(
(
(
=
(
(

4 4
4 4 3
4
cos sin 0 0
sin cos 0 0
A
0 0 1 0
0 0 0 1
u u (
(
u u
(
=
(
(










Nhm 7




1 2 4 1 4 1 2 4 1 4 1 2 1 2 3
1 2 4 1 4 1 2 4 1 4 1 2 1 2 3
2 4 2 4 2 3 2
s .s .c c .s s .s .s c .c s .c s .c .r
c .s .c s .s c .s .s s .c c .c c .c .r
T
c .c c .s s r .s
0 0 0 1
(
(

(
=
(
(


Nhm 13


1 1 1 1
1 1 1 1 0
1
0 .
0 .
0 1 0 0
0 0 0 1
c s a c
s c a s
A
u u u
u u u
(
(
(
=
(
(


1
2
0 0 1 0
1 0 0 0
0 1 0 2
0 0 0 1
A
r
(
(

(
=
(
(


2
3
1 0 0 0
0 1 0 0
0 0 1 3
0 0 0 1
A
r
(
(
(
=
(
(

4 4
4 4 3
4
0 0
0 0
0 0 1 0
0 0 0 1
c s
s c
A
u u
u u
(
(
(
=
(
(





1 4 1 4 1 3 1 2 1 1 1
1 4 1 4 1 3 1 2 1 1 1
4 4
. . . . .
. . . . .
0 0
0 0 0 1
s s s c c r c r s a c
c s c c s r s r c a c
T
c s
+
(
(
+ +
(
=
(
(



















Nhm 16


0
1
1 0 0 0
0 1 0 0
0 0 1 1
0 0 0 1
A
r
(
(
(
=
(
(

2 2
2 2 1
2
0 0
0 0
0 1 0 0
0 0 0 1
c s
s c
A
u u
u u
(
(
(
=
(
(


2
3
1 0 0 0
0 1 0 0
0 0 1 3
0 0 0 1
A
r
(
(
(
=
(
(

4 4
4 4 3
4
0 0
0 0
0 0 1 0
0 0 0 1
c s
s c
A
u u
u u
(
(
(
=
(
(


2 4 2 4 2 3 2
2 4 2 4 2 3 2
4 4 1
. . .
. . .
0
0 0 0 1
c c c s s r s
s c s s c r c
T
s c r
(
(

(
=
(
(