Robot công nghiệp
27

Chơng III

phơng trình động học của robot
(Kinematic Equations) 3.1. Dẫn nhập :

Bất kỳ một robot nào cũng có thể coi là một tập hợp các khâu (links) gắn liền với các
khớp (joints). Ta hãy đặt trên mỗi khâu của robot một hệ toạ độ. Sử dụng các phép biến đổi
thuần nhất có thể mô tả vị trí tơng đối và hớng giữa các hệ toạ độ nầy. Denavit. J. đã gọi
biến đổi thuần nhất mô tả quan hệ giữa một khâu và một khâu kế tiếp là một ma trận A. Nói
đơn giản hơn, một ma trận A là một mô tả biến đổi thuần nhất bởi phép quay và phép tịnh tiến
tơng đối giữa hệ toạ độ của hai khâu liền nhau. A
1
mô tả vị trí và hớng của khâu đầu tiên; A
2

mô tả vị trí và hớng của khâu thứ hai so với khâu thứ nhất. Nh vậy vị trí và hớng của khâu
thứ hai so với hệ toạ độ gốc đợc biểu diễn bởi ma trận :

T
2
= A
1
.A
2


5
.A
6
(3.1)

T
6
mô tả mối quan hệ về hớng và vị trí của khâu chấp hành cuối đối với hệ toạ độ gốc.
Một robot 6 khâu có thể có 6 bậc tự do và có thể đợc định vị trí và định hớng trong trờng
vận động của nó (range of motion). Ba bậc tự do xác định vị trí thuần tuý và ba bậc tự do khác
xác định hớng mong muốn. T
6
sẽ là ma trận trình bày cả hớng và vị trí của robot. Hình 3.1
mô tả quan hệ đó với bàn tay máy. Ta đặt gốc toạ độ của hệ mô tả tại điểm giữa của các ngón
tay. Gốc toạ độ nầy đợc mô tả bởi vectơ p (xác định vị trí của bàn tay). Ba vectơ đơn vị mô tả
hớng của bàn tay đợc xác định nh sau :

n
p
a
o

p
x

T
6
= n
y
O
y
a
y
p
y
(3.2)
n
z
O
z
a
z
p
z

0 0 0 1

Tổng quát, ma trận T
6
có thể biểu diễn gọn hơn nh sau :

Ma trận định hớng R Vectơ vị trí p (3.3)
a
Khớp n
Khớp n+1

n
Khâu n
Hình 3.5 : Chiều dài và góc xoắn của 1 khâu.
Thông thờng, ngời ta gọi a
n
là chiều dài và
n
là góc xoắn của khâu (Hình 3.5). Phổ
biến là hai khâu liên kết với nhau ở chính trục của khớp (Hình 3.6).
TS. Phạm Đăng Phớc
Robot công nghiệp
29


n+1
Khâu n-2
Hình 3.6 : Các thông số của khâu :

, d, a và

.

Mỗi trục sẽ có hai pháp tuyến với nó, mỗi pháp tuyến dùng cho mỗi khâu (trớc và sau
một khớp). Vị trí tơng đối của hai khâu liên kết nh thế đợc xác định bởi d
n
là khoảng cách
giữa các pháp tuyến đo dọc theo trục khớp n và
n
là góc giữa các pháp tuyến đo trong mặt
phẳng vuông góc với trục.
d

n
, d
n
và
n
đợc gọi là bộ thông số DH.

Ví dụ 1 : Xét một tay máy có hai khâu phẳng nh hình 3.7 :

1

2
a
1
a
2
O
0
z
1
z
2
x
1
y
1
y
2
O
1

2
vuông góc với tờ giấy.
Hệ toạ độ cơ sở là O
0
x
0
y
0
z
0
, chiều của x
0
hớng từ O
0
đến O
1
. Sau khi thiết lập hệ toạ độ cơ sở,
Hệ toạ độ o
1
x
1
y
1
z
1
có hớng nh hình vẽ, O
1
đặt tại tâm trục khớp 2. Hệ toạ độ O
2
x

2
*
0 a
2
0

Trong đó
i
là các biến khớp (dùng dấu * để ký hiệu các biến khớp).

Ví dụ 2 : Xem sơ đồ robot SCARA có 4 khâu nh hình 3.8 :
Đây là robot có cấu hình kiểu RRTR, bàn tay có chuyển động xoay xung quanh trục
đứng. Hệ toạ độ gắn lên các khâu nh hình vẽ.
Hình 3.8 : Robot SCARA và các hệ toạ độ (vị trí ban đầu).
O
0

1

a
2
O
1
O
2
d
4
Đối với tay máy nầy các trục khớp đều song song nhau, để tiện lợi tất cả các gốc toạ độ
đặt tại tâm các trục khớp. Trục x
0
nằm trong mặt phẳng tờ giấy. Các hệ toạ độ khác nh hình
vẽ. Bảng thông số DH của robot SCARA nh sau :

Khâu

i

i
a
i
d
i
1

1
*
0 a
1
0


Tịnh tiến dọc theo z
n-1
một khoảng d
n

Tịnh tiến dọc theo x
n-1
= x
n
một đoạn a
n

Quay quanh x
n
một góc xoắn
n

TS. Phạm Đăng Phớc
Robot công nghiệp
31

Bốn phép biến đổi thuần nhất nầy thể hiện quan hệ của hệ toạ độ thuộc khâu thứ n so
với hệ toạ độ thuộc khâu thứ n-1 và tích của chúng đợc gọi là ma trận A :

A
n
= Rot(z,) Trans(0,0,d) Trans(a,0,0) Rot(x,) (3.4)

1 0 0 0
A
n
= 0
cos - sin
0 (3.6)
0
sin cos
d
0 0 0 1

Đối với một khâu đi theo một khớp quay thì d, a và là hằng số. Nh vậy ma trận A
của khớp quay là một hàm số của biến khớp .
Đối với một khâu đi theo một khớp tịnh tiến thì , là hằng số. Ma trận A của khớp
tịnh tiến là một hàm số của biến số d.
Nếu các biến số đợc xác định thì giá trị của các ma trận A theo đó cũng đợc xác
định.

3.4. Xác định T
6
theo các ma trận A
n
:

Ta đã biết : T
6
= A
1
A

Z
T
6
E
A
O
R
Trong trờng hợp tổng quát, khi
xét quan hệ của robot với các thiết bị
khác, nếu hệ toạ độ cơ bản của robot có
liên hệ với một hệ toạ độ nào đó bởi phép
biến đổi Z, Khâu chấp hành cuối lại có
gắn một công cụ, có quan hệ với vật thể
bởi phép biến đổi E (hình 3.9) thì vị trí và
hớng của điểm cuối của công cụ, khảo
sát ở hệ toạ độ tham chiếu mô tả bởi X sẽ
đợc xác định bởi :

Hình 3.9 : Vật thể và Robot

X= Z T
6
E
TS. Phạm Đăng Phớc
Robot công nghiệp
32

Quan hệ nầy đợc thể hiện trên toán đồ sau :
O
R
Z
EX
A
O
0

Hình 3.10 : Toán đồ chuyển vị của robot.

Từ toán đồ nầy ta có thể rút ra : T
6
= Z
-1
X E
-1
(Z
-1

là trục quay của z
n
thành z
n+1
và góc của z
n
với z
n+1
chính là
n+1
. Nếu z
n

và z
n+1
song song hoặc trùng nhau thì ta có thể căn cứ nguyên tắc chung hay chọn x
n
theo x
n+1
.
+ Các hệ toạ độ Oxyz phải tuân theo qui tắc bàn tay phải.
+ Khi gắn hệ toạ độ lên các khâu, phải tuân theo các phép biến đổi của ma trận A
n
. đó
là bốn phép biến đổi : A
n
= Rot(z,) Trans(0,0,d) Trans(a,0,0) Rot(x,). Nghĩa là ta coi hệ toạ
độ thứ n+1 là biến đổi của hệ toạ độ thứ n; các phép quay và tịnh tiến của biến đổi nầy phải là
một trong các phép biến đổi của A
n


1. Gắn hệ toạ độ lên các khâu :
Ta giả định vị trí ban đầu và chọn gốc toạ độ O
0
của robot nh hình 3.12. Các trục z đặt
cùng phơng với các trục khớp.

Ta thấy trục z
1
đã quay tơng đối một
góc 90
0
so với trục z
0
, đây chính là phép quay
quanh trục x
0
một góc
1
(phép biến đổi
Rot(x

1
) (tịnh tiến dọc theo
z
0
một đoạn d
1
) ; các trục y
0
,và y
1
xác định
theo qui tắc bàn tay phải (Hình 3.12 ) .

Tiếp tục chọn gốc tọa độ O
2
đặt trùng
với O
1
vì trục khớp thứ ba và trục khớp thứ
hai cắt nhau tại O
1
(nh hình 3.12). Trục z
2

cùng phơng với trục khớp thứ ba, tức là đã
quay đi một góc 90
0
so với z
1
quanh trục y

Đầu cuối của khâu thứ 3 không có
khớp, ta đặt O
3
tại điểm giữa của các ngón
tay, và trục z
3
, x
3
chọn nh hình vẽ, nh vậy
ta đã tịnh tiến gốc toạ độ dọc theo z
2
một
đoạn d
3
(Phép biến đổi Trans(0,0,d
3
)), vì đây
là khâu tịnh tiến nên d
3
là biến . H
ình 3.12 : Gắn các h
ệ
to
ạ
đ
ộ
O

1

1

2
d
3
H
ình 3.11 : Robot RR
T

x
2
O
3
O
2
z
2
z
3
z
0
O
0
x
0
O
1
y

2. Lập bảng thông số DH :

Khâu

i

i
a
i
d
i
1

1
*
90 0 d
1
2

i
*
-90 0 0
3 0 0 0 d
3
*

3. Xác định các ma trận A :
Ma trận A
n
có dạng :

0
A
1
= S
1
0 -C
1
0
0 1 0 d
1
0 0 0 1

C
2
0 -S
2
0
A
2
= S
2
0 C
2
0
0 -1 0 0
0 0 0 1

1 0 0 0
A
3

2
-S
2
*d
3
1
T
3
= S
2
0 C
2
0 0 1 0 0 = S
2
0 C
2
C
2
*d
3
0 -1 0 d
2
0 0 1 d
3
0 -1 0 0
0 0 0 1 0 0 0 1 0 0 0 1
+ Ma trận T
3
= A
1

3
0 1 0 d
1
0 -1 0 0
0 0 0 1 0 0 0 1 TS. Phạm Đăng Phớc
Robot công nghiệp
35
C
1
C
2
-S
1
-C
1
S
2
-C
1
S
2
d
3
= S
1
d
2

1
C
2
;
O
x
= -S
1
;
a
x
= -C
1
S
2
;
p
x
= -C
1
S
2
d
3
n
y
= S
1
C
2

= C
2
;
p
z
= C
2
d
3
+ d
1
; (Ta có thể sơ bộ kiểm tra kết quả tính toán bằng cách dựa vào toạ độ vị trí p
x
,p
y
, p
z
đã
tính so với cách tính hình học trên hình vẽ).

3.9. Hệ phơng trình động học của robot STANFORD :

Stanford là một robot có 6 khâu với cấu hình RRT.RRR (Khâu thứ 3 chuyển động tịnh
tiến, năm khâu còn lại chuyển động quay). Kết cấu của robot Stanford nh hình 3.14 :

);
S
12
= sin(
1
+
2
)
S
234
= sin (
2
+
3
+
4
) .

Hệ toạ độ gắn lên các khâu của robot nh
hình 3.15. (Khâu cuối có chiều dài và
khoảng cách bằng không, để có thể gắn các
loại công cụ khác nhau nên chọn O
6
O
5
).

Bảng thông số DH (Denavit-Hartenberg) của robot Stanford nh sau :

Khâu

-90
0
0 0
5

5
*
90
0
0 0
6

6
*
0 0 0
(* : Các biến khớp).

Các ma trậm A của robot Stanford đợc xác định nh sau :

C
1
0 -S
1
0 C
2
0 S
2
0
A
1

3
0 -1 0 0
0 0 0 1 0 0 0 1

C
5
0 S
5
0 C
6
-S
6
0 0
A
5
= S
5
0 -C
5
0 A
6
=S
6
C
6
0 0
0 1 0 0 0 0 1 0
0 0 0 1 0 0 0 1

d

,O
4,
O
5
,O
6
x
1
O
2 Tích của các ma trận chuyển vị A đối với robot Stanford đợc bắt đầu ở khâu 6 và
chuyển dần về gốc; theo thứ tự nầy ta có :

TS. Phạm Đăng Phớc
Robot công nghiệp
37
C
6
-S
6
0 0
T
6
5
= S
6
C
6

-C
5
0
S
6
C
6
0 0
0 0 0 1

C
4
C
5
C
6
- S
4
S
6
-C
4
C
5
S
6
-S
4
C
6

+ C
4
C
6
S
4
S
5
0
-S
5
C
6
S
5
S
6
C
5
0
0 0 0 1

C
4
C
5
C
6
-S
4

C
5
C

+ C
4
S
6
-S
4
C
5
S
6
+ C
4
C
6
S
4
S
5
0
-S
5
C
6
S
5
S

5
S
6
-S
4
C
6
)+S
2
S
5
S
6
T
6
1
=A
2
A
3
A
4
A
5
A
6
= S
2
(C
4

S
6
S
4
C
5
C
6
+ C
4
S
6
-S
4
C
5
S
6
+C
4
C
6
0 0

C
2
C
4
S
5

n
x
O
x
a
x
p
x
T
6
= n
y
O
y
a
y
p
y
= A
1
T
6
1
n
z
O
z
a
z
p

) - S
2
S
5
C
6
] - S
1
(S
4
C
5
C
6
+ C
4
S
6
)
n
y
= S
1
[C
2
(C
4
C
5
C

C
5
C
6
- S
4
S
6
) + C
2
S
5
C
6
O
x
= C
1
[-C
2
(C
4
C
5
S
6
+ S
4
C
6

6
+ S
4
C
6
) + S
2
S
5
S
6
] + C
1
(-S
4
C
5
C
6
+ C
4
C
6
)
O
z
= S
2
(C
4

S
4
S
5
a
y
= S
1
(C
2
C
4
S
5
+ S
2
C
5
) + C
1
S
4
S
5
a
z
= -S
2
C
4

p
z
= C
2
d
3
TS. Phạm Đăng Phớc
Robot công nghiệp
38
Nếu ta biết đợc các giá trị của biến khớp, thì vị trí và hớng của bàn tay robot sẽ tìm
đợc bằng cách xác định các giá trị các phần tử của T
6
theo các phơng trình trên.
Các phơng trình trên gọi là hệ phơng trình động học thuận của robot Stanford.

3.10. Hệ phơng trình động học của robot ELBOW :

Để hiểu rõ hơn về cách thiết lập hệ phơng trình động học của robot, ta xét thêm
trờng hợp robot Elbow.
Khâu 1
Khâu 2
Khâu 3
Khâu 4
Khâu 5
Khâu 6
z
3
z
5
,z
6
x
i
O
0
,O
1
a
2
a
3
a
4

5
O
2
,O
5
,O
6
O
3
O
2

1

1
90
0
0 0
2

2
0 a
2
0
3

3
0 a
3
0
4

4
-90
0
a
4
0
5

5
90

=S
2
C
2
0S
2
a
2
0 1 0 0 0 0 1 0
0 0 0 1 0 0 0 1
TS. Phạm Đăng Phớc
Robot công nghiệp
39
C
3
-S
3
0 C
3
a
3
C
4
0 -S
4
C
4

6
-S
6
0 0
A
5
= S
5
0 -C
5
0 A
6
=S
6
C
6
0 0
0 1 0 0 0 0 1 0
0 0 0 1 0 0 0 1

Ta xác định các ma trận T theo các hệ toạ độ lần lợt từ khâu cuối trở về gốc :

C
6
-S
6
0 0
T
6
5

-S
5
S
6
-C
5
0
S
6
C6 0 0
0 0 0 1

C
4
C
5
C
6
- S
4
S
6
-C
4
C
5
S
6
-S
4

4
C
5
S
6
+C
4
C
6
S
4
S
5
S
4
a
4
-S
5
C
6
S
5
S
6
C
5
0
0 0 0 1


a
3
T
6
2
= A
3
A
4
A
5
A
6
= S
34
C
5
C
6
+C
34
S
6
-S
34
C
5
S
6
+C

1
=A
2
A
3
A
4
A
5
A
6
=

C
234
C
5
C
6
- S
234
S
6
-C
234
C
5
S
6
- S

234
C
5
S
6
+ C
234
C
6
S
234
S
5
S
234
a
4
+S
23
a
3
+S
2
a
2
-S
5
C
6
S

6
1
n
z
O
z
a
z
p
z
0 0 0 1

Để tính T
6
, ta phải nhân A
1
với T
6
1
sau đó cân bằng các phần tử của ma trận T
6
ta đợc
một hệ thống các phơng trình sau :

TS. Phạm Đăng Phớc
Robot công nghiệp
40
n
x
= C

6
) + C
1
S
5
C
6
n
z
= S
234
C
5
C
6
+ C
234
S
6
O
x
= -C
1
(C
234
C
5
S
6
+ S

z
= -S
234
C
5
S
6
+ C
234
C
6
a
X
= C
1
C
234
S
5
+ S
1
C
5
a
y
= S
1
C
234
S

= S
1
(C
234
a
4
+ C
23
a
3
+ C
2
a
2
)
p
z
= S
234
a
4
+ S
23
a
3
+ S
2
a
2Bài tập chơng III :
Bài 1 : Cho ma trận :
? 0-10
T
6
= ? 0 0 1
? -1 0 2
? 0 0 1

là ma trận biểu diễn hớng và vị trí của khâu chấp hành cuối. Tìm các phần tử đợc đánh dấu ? Bài 2 : Cho một robot có 3 khâu phẳng nh hình 3.18, cấu hình RRR. Thiết lập hệ phơng
trình động học của robot.

TS. Phạm Đăng Phớc
Robot công nghiệp
41

Bài 3 : Cho một robot có 2 khâu tịnh tiến nh hình 3.19, cấu hình TT. Thiết lập hệ phơng
trình động học của robot.

H
ình 3.18 : Robot cấu hình RRR
H
ình 3.19 : Robot cấu hình T
T

H
ình 3.21 : Robot cấu hình RTR Bài 6 : Cho một robot có 3 khâu nh hình 3.22, cấu hình RRR. Thiết lập hệ phơng trình
động học của robot.
H
ình 3.23 : Robot cấu hình RRRRR

H
ình 3.22 : Robot cấu hình RRR
Bài 7 : Cho một robot có 5 khâu nh hình 3.23, cấu hình RRRRR. Thiết lập hệ phơng trình
động học của robot.
TS. Phạm Đăng Phớc