giải bài toán ngược động học, động lực học và điều khiển trượt rô bốt dư dẫn động dựa trên thuật toán hiệu chỉnh gia lượng véc tơ toạ độ suy rộng

BỘ GIÁO DỤC VÀ ĐÀO TẠO VIỆN KHOA HỌC
VÀ CÔNG NGHỆ VIỆT NAM




VIỆN CƠ HỌC



TRẦN HOÀNG NAM



GIẢI BÀI TOÁN NGƯỢC ĐỘNG HỌC, ĐỘNG LỰC
HỌC VÀ ĐIỀU KHIỂN TRƯỢT RÔBỐT DƯ
DẪN ĐỘNG DỰA TRÊN THUẬT TOÁN HIỆU
CHỈNH GIA LƯỢNG VÉC TƠ TỌA ĐỘ SUY RỘNG








LUẬN ÁN TIẾN SĨ KỸ THUẬT

Hà Nội - 2010

BỘ GIÁO DỤC VÀ ĐÀO TẠO VIỆN KHOA HỌC
VÀ CÔNG NGHỆ VIỆT NAM


VIỆN CƠ HỌC



TRẦN HOÀNG NAM


GIẢI BÀI TOÁN NGƯỢC ĐỘNG HỌC,
ĐỘNG LỰC HỌC VÀ ĐIỀU KHIỂN TRƯỢT RÔBỐT
DƯ DẪN ĐỘNG DỰA TRÊN THUẬT TOÁN
HIỆU CHỈNH GIA LƯỢNG VÉC TƠ
TỌA ĐỘ SUY RỘNG


Chuyên ngành: Cơ học kỹ thuật

Mã số ngành: 62 52 02 01


LUẬN ÁN TIẾN SĨ KỸ THUẬT



Người hướng dẫn khoa học :
1. GS.TSKH Nguyễn Văn Khang
2. PGS.TS Nguyễn Phong Điền








Hà Nội 2010
LỜI CAM ĐOAN


Tôi xin cam đoan đây là công trình nghiên cứu của riêng tôi và chưa được
công bố trong bất cứ công trình nào khác. Các số liệu, kết quả nêu trong luận án
là trung thực.

Tác giả luận án

Trần Hoàng Nam



MỤC LỤC
Trang

Lời cam đoan
Mục lục I
Danh mục các ký hiệu, chữ viết tắt thường sử dụng III
Danh mục các bảng VI
Danh mục các hình vẽ và đồ thị VII
Mở đầu 1
Chương 1. Tính toán động học ngược rôbốt dư dẫn động bằng thuật
toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng 8
1.1 Giải bài toán động học thuận rôb
ốt dư dẫn động bằng phương pháp
ma trận Denavit-Hartenberg 8
1.1.1 Các tham số động học và ma trận Denavit-Hartenberg 10
1.1.2 Phương trình xác định vị trí và hướng của khâu thao tác 12
1.1.3 Bài toán áp dụng 14
1.2 Ma trận tựa nghịch đảo 16
1.2.1 Định nghĩa 16
1.2.2 Ma trận tựa nghịch đảo Moore-Penrose 16
1.2.3 Phương pháp nhân tử Lagrange 18
1.2.4 Nghiệm tổng quát của phương trình đại số tuyến tính 19
1.3 Giải bài toán
động học ngược rôbốt dư dẫn động bằng phương pháp
hiệu chỉnh gia lượng véc tơ tọa độ suy rộng 20
1.3.1 Phương pháp khai triển Taylor 20

1.3.2 Các công thức xác định véc tơ vận tốc và véc tơ gia tốc suy rộng 21
1.3.3 Các công thức xác định véc tơ tọa độ suy rộng 22
1.3.4 Đánh giá sai số 26
1.4 Các bài toán áp dụng 26
1.5 Kết luận chương 1 43
Chương 2 Tính toán động lực h
ọc ngược rôbốt dư dẫn động
trong không gian thao tác dựa trên thuật toán
hiệu chỉnh gia lượng véc tơ tọa độ suy rộng 44
2.1 Dạng thức Lagrange loại 2 của hệ nhiều vật 44
2.2 Giải bài toán ngược động lực học rôbốt dư dẫn động trong không
gian thao tác 47

II
2.3 Các bài toán áp dụng 52
2.4 Kết luận chương 2 69
Chương 3 Điều khiển trượt rôbốt dư dẫn động dựa trên thuật toán số
hiệu chỉnh gia lượng véc tơ tọa độ suy rộng 70
3.1 Cơ sở lý thuyết ổn định Lyapunov 70
3.1.1 Hệ phi tuyến và các điểm cân bằng 70
3.1.2 Khái niệm ổn định 72
3.1.3 Phương pháp trực tiếp Lyapunov 74
3.2
Bài toán điều khiển chuyển động của rôbốt 78
3.3 Điều khiển trượt rôbốt dư dẫn động 80
3.4 Các bài toán áp dụng 85
3.5 Kết luận chương 3 102
Chương 4. Bài toán ngược động học, động lực học
và điều khiển trượt rôbốt BKHN-MCX-04 103
4.1 Tổng quan về phương pháp đo chính xác bề mặt các chi tiết máy 103

4.2 Kết cấu của rô bốt đo BKHN-MCX-04 104
4.3 Tính toán động học ngược 106
4.4 Tính toán động lực học ngược 111
4.5 Điều khiển trượt rôbốt BKHN-MCX-04 122
4.6 Thí nghiệm 126
4.6.1 Cấu tạo của hệ thống thí nghiệm 126
4.6.2 Nguyên lý hoạt động của hệ thống thí nghiệm 127
4.6.3 Hệ thống điều khiển 127
4.6.4 Kết quả thí nghiệm 131
4.7 Kết luận chương 4 133
Kết luận chung 134
Danh mục công trình của tác giả 136
Tài liệu tham khảo 137

III

DANH MỤC KÝ HIỆU,
CHỮ VIẾT TẮT THƯỜNG SỬ DỤNG
a
i
Khoảng dịch chuyển dọc trục x
i
(chiều dài của khâu thứ i)
α
i
Góc quay quanh trục x
i
.
A
(z,θi)

Ma trận biến đổi tọa độ bởi phép quay cơ bản xung quanh trục z.
A
(z,di)
Ma trận biến đổi tọa độ bởi phép tịnh tiến dọc trục z.
A
(x,ai)
Ma trận biến đổi tọa độ bởi phép tịnh tiến dọc trục x.
A
(x,αi)
Ma trận biến đổi tọa độ bởi phép quay xung quanh trục x.
A
i
Ma trận cosin chỉ hướng của khâu thứ i.
A
+
Tựa nghịch đảo của ma trận A.
C
i
Cosin (q
i
).
C
12…n
Cosin(q
1
+q
2
+…+q
n
).

C(q,
)
i
q
&
i
q
&
Véc tơ các lực Coriolis và lực hướng tâm của mô hình thực.
),(
ˆ
qqC
&
i
q
&
Véc tơ các lực Coriolis và lực hướng tâm của bộ điều khiển.
),(
~
qqC
&
Sai số giữa C(q, )
i
q
&
và ),(
ˆ
qqC
&
.

d
i
Khoảng dịch chuyển gốc tọa độ O
i-1
về O’
i
.
d Véc tơ nhiễu.
D
i
Ma trận chuyển đổi tọa độ từ O
0
x
0
y
0
z
0
về hệ tọa độ O
i
x
i
y
i
z
i
.
k
e
Sai số dịch chuyển của bàn kẹp (không gian thao tác).

k
e
&
Sai số vận tốc của bàn kẹp.
k
e
&&
Sai số gia tốc của bàn kẹp.
e(t) Véc tơ sai số bám theo góc quay.
)
t
(e
&
Sai số bám theo vận tốc quay.
ε

Sai số của phép tính.
epx
i
Gia tốc góc của khâu thứ i.
f Số bậc tự do của cơ hệ.
g(q) Véc tơ lực trọng trường suy rộng.
)(
ˆ
qg Véc tơ các lực trọng trường của bộ điều khiển.
)(
~
qg Sai số giữa g(q) và )(
ˆ
qg .

i-1
H
i
Ma trận biến đổi tọa độ từ hệ trục O
i-1
x
i-1
y
i-1
z
i-1
về O
i
x
i
y
i
z
i
.
h=Δt Khoảng thời gian của bước tính toán.
l
i
Vị trí trọng tâm khâu thứ i đối với hệ trục tọa độ khâu i.
λ Nhân tử Lagrange.

IV
λ
i

Hằng số dương thứ i.
m
i
Khối lượng của chất điểm thứ i.
M(q) Ma trận khối lượng suy rộng.
)(
ˆ
qM Momen khối lượng của bộ điều khiển.
)(
~
qM Sai số giữa M(q) và
)(
ˆ
qM
.
I
i
Tenxơ quán tính khối của vật rắn thứ i.
I
zi
Momen quán tính đối với trục z của khâu thứ i.
J
Ti
Ma trận Jacobi tịnh tiến của vật rắn thứ i.
J
Ri
Ma trận Jacobi quay của vật rắn thứ i.
J(q) Ma trận Jacobi.
J
+

(q) Tựa nghịch đảo của ma trận Jacobi.
)(qJ
+
&

Đạo hàm bậc nhất của J
+
(q).
k Hệ số khuyếch đại .
k
Hệ số khuyếch đại của hàm sat.
K
s
Hệ số khuyếch đại dạng trượt.
K
pd
Hệ số khuyếch đại tỷ lệ - đạo hàm.
om
i
Vận tốc góc của khâu thứ i.
P
x
, P
y
, P
z
Các tọa độ x, y, z của điểm thao tác P.
Π Thế năng của hệ.
q∂
Π∂

Thành phần lực suy rộng của các lực có thế.
q Véc tơ tọa độ suy rộng của robot.
q
&
Đạo hàm bậc nhất của q theo thời gian.
q
&&
Đạo hàm bậc 2 của q theo thời gian.
q
0
Véc tơ tọa độ suy rộng tại thời điểm ban đầu.
0
q
&
Đạo hàm bậc nhất theo thời gian của q
0
.
0
q
&&
Đạo hàm bậc 2 theo thời gian của q
0
.
0
~
q
Giá trị gần đúng của q
0
.
Δq

0
Số gia của q
0
.
q
k+1
Giá trị của q tại thời điểm t = t
k+1
.
1k+
q
&
Đạo hàm bậc 1 theo thời gian của q
k+1
.
1k+
q
&&
Đạo hàm bậc 2 của q
k+1
.
1k
~
+
q

Giá trị gần đúng của q
k+1
.
q

i
_dot Đạo hàm bậc nhất của q
i
theo thời gian.
q
i
_2dot Đạo hàm bậc 2 của q
i
theo thời gian .
q
d
Véc tơ tọa độ suy rộng mong muốn.
d
q
&
Đạo hàm của véc tơ suy rộng mong muốn theo thời gian.

V
r
Ci
Véc tơ định vị trọng tâm của vật rắn thứ i.
sgn Hàm dấu.
sat Hàm bão hòa.
S
i
Sin(q
i
).
S
12 n

Sin(q
1
+q
2
+…+q
n
).
θ
i
Góc quay của trục x
i-1
xung quanh trục z
i-1
.
x Véc tơ định vị của khâu thao tác.
x=f(q) Phương trình xác định tọa độ x theo tọa độ suy rộng q.
x
d
Vị trí mong muốn của bàn kẹp.
d
x
&
Vận tốc chyển động mong muốn của bàn kẹp.
d
x
&&
Gia tốc chuyển động mong muốn của bàn kẹp.
x* Trạng thái cân bằng.
d
x

~
Sai số bám của biến x.
d
~
x Véc tơ sai số bám.
v
r

Véc tơ hình học của vận tốc.
v
i
Véc tơ đại số của vận tốc khối tâm của vật rắn thứ i.
V(x) Hàm Lyapunov.
ω
i
Vận tốc góc của vật rắn thứ i.
i
~
ω Toán tử sóng của véc tơ vận tốc góc thứ i.
τ Lực/momen trên khớp động .
T Động năng của toàn hệ.
u Quy luật điều khiển.
DH Denavit-Hartenberg.
MP Moore-Penrose.
PD Tỷ lệ - vi phân.
PID Tỷ lệ - vi phân – tích phân.

VI
DANH MỤC CÁC BẢNG

Trang
Bảng 1.1 Thông số DH rôbốt 5 khâu động 14
Bảng 2.1 Các tham số động học của rôbốt Scara 52
Bảng 2.2 Bảng thông số động lực rôbốt SCARA 4 bậc tự do 56
Bảng 2.3 Các thông số động lực học của rôbốt 5 khâu động 60
Bảng 2.4 Các thông số động học của rôbốt 5 khâu động 60
Bảng 2.5 Các thông số DH rôbốt 6 khâu động 62
Bảng 2.6 Các thông số động lực rôbốt 6 khâu độ
ng 62
Bảng 2.7 Các giá trị thông số động lực học của rôbốt 6 khâu động 67
Bảng 3.1 Các thông số động lực rôbốt 4 khâu động 85
Bảng 3.2 Các thông số động lực rôbốt SCARA 93
Bảng 4.1 Thông số hình học của rôbốt 105
Bảng 4.2 Bảng tham số động học D-H 106
Bảng 4.3 Vị trí khối tâm khâu i của rôbốt trên hệ động 114
Bảng 4.4 Các thông số động lực của rôbốt đo 119
Bảng 4.5 Các k
ết quả đo thực nghiệm 131




VII
DANH MỤC CÁC HÌNH VẼ VÀ ĐỒ THỊ

Trang
Hình 0.1 Rôbốt trên dây chuyền của trung tâm sản xuất linh hoạt 1
Hình 0.2 Rôbốt phục vụ máy phay CNC 2
Hình 0.3 Rôbốt Mitsubishi RV-2AJ 2
Hình 0.4 Hình ảnh một số loại rôbốt 3

Hình 1.1 Sơ đồ thiết lập hệ tọa độ các khâu 9
Hình 1.2 Sơ đồ rôbốt dạng chuỗi 13
Hình 1.3 Rôbốt 5 khâu động 14
Hình 1.4 Sơ đồ khối tính toán A
+
theo phương pháp Moore-Penrose 17
Hình 1.5 Sơ đồ khối giải bài toán động học ngược 25
Hình 1.6 Rôbốt phẳng 5 khâu động 26
Hình 1.7 Các đặc tính chuyển động của khâu 1 27
Hình 1.8 Các đặc tính chuyển động của khâu 2 28
Hình 1.9 Các đặc tính chuyển động của khâu 3 28
Hình 1.10 Các đặc tính chuyển động của khâu 4 28
Hình 1.11 Các đặc tính chuyển động của khâu 5 29
Hình 1.12 Sai số vị trí của điểm thao tác theo trục x 29
Hình 1.13 Sai số vị trí của điểm thao tác theo trục y 29
Hình 1.14 Sai số góc định h
ướng của bàn kẹp 30
Hình 1.15 Dạng chuyển động của rôbốt theo kết quả tính toán 30
Hình 1.16 Các đặc tính chuyển động của khâu 1 30
Hình 1.17 Các đặc tính chuyển động của khâu 2 31
Hình 1.18 Các đặc tính chuyển động của khâu 3 31
Hình 1.19 Các đặc tính chuyển động của khâu 4 31
Hình 1.20 Các đặc tính chuyển động của khâu 5 32
Hình 1.21 Sai số vị trí của điểm thao tác theo trục x 32
Hình 1.22 Sai số vị trí của điểm thao tác theo trục y 32
Hình 1.23 Sai số góc định hướng của bàn k
ẹp 33
Hình 1.24 Dạng chuyển động của rôbốt theo kết quả tính toán 33
Hình 1.25 Sai số vị trí của điểm thao tác theo trục x 34
Hình 1.26 Sai số vị trí của điểm thao tác theo trục y 34

Hình 1.27 Sai số góc định hướng của bàn kẹp 34
Hình 1.28 Dạng chuyển động của rôbốt theo kết quả tính toán 35
Hình 1.29 Sai số vị trí của điểm thao tác theo trục x 35
Hình 1.30 Sai số vị trí của điểm thao tác theo trục y 35
Hình 1.31 Sai số góc định hướ
ng của bàn kẹp 36
Hình 1.32 Dạng chuyển động của rôbốt theo kết quả tính toán 36

VIII
Hình 1.33 Sai số vị trí của điểm thao tác theo trục x 37
Hình 1.34 Sai số vị trí của điểm thao tác theo trục y 37
Hình 1.35 Sai số góc định hướng của bàn kẹp 37
Hình 1.36 Dạng chuyển động của rôbốt theo kết quả tính toán 38
Hình 1.37 Sai số vị trí của điểm thao tác theo trục x 38
Hình 1.38 Sai số vị trí của điểm thao tác theo trục y 38
Hình 1.39 Sai số góc định hướng của bàn kẹp 39
Hình 1.40 Dạng chuyển động của rôbốt theo kế
t quả tính toán 39
Hình 1.41 Rôbốt 6 khâu động 40
Hình 1.42 Đồ thị góc quay của các khớp 41
Hình 1.43 Đồ thị vận tốc góc các khớp 41
Hình 1.44 Đồ thị gia tốc góc các khớp động 41
Hình 1.45 Đồ thị sai số vị trí trong không gian thao tác 42
Hình 1.46 Đồ thị sai số vận tốc trong không gian thao tác 42
Hình 1.47 Đồ thị sai số gia tốc trong không gian thao tác 42
Hình 2.1 44
Hình 2.2 Thuật toán giải bài toán động lực học ngược
trong không gian thao tác 51
Hình 2.3 Sơ đồ kết cấu rôbốt scara 52
Hình 2.4 Đồ thị chuyển

động của các khớp động rôbốt scara 53
Hình 2.5 Đồ thị vận tốc chuyển động của các khớp động rôbốt scara 54
Hình 2.6 Đồ thị gia tốc chuyển động của các khớp động rôbốt scara 54
Hình 2.7 Đồ thị sai số vị trí bàn kẹp rôbốt scara 55
Hình 2.8 Đồ thị sai số vận tốc bàn kẹp rôbốt scara 55
Hình 2.9 Đồ thị sai số gia tốc bàn kẹp rôbốt scara 56
Hình 2.10 Mômen trên khớp động thứ 1 57
Hình 2.11 Mômen trên khớp
động thứ 2 58
Hình 2.12 Lực trên khớp động thứ 3 58
Hình 2.13 Mômen trên khớp động thứ 4 59
Hình 2.14 Rôbốt phẳng 5 khâu động 59
Hình 2.15 Đồ thị các mômen động cơ theo thời gian 61
Hình 2.16 Rôbốt phẳng 6 khâu động 61
Hình 2.17 Mômen trên khớp động thứ 1 67
Hình 2.18 Mômen trên khớp động thứ 2 67
Hình 2.19 Mômen trên khớp động thứ 3 68
Hình 2.20 Mômen trên khớp động thứ 4 68
Hình 2.21 Mômen trên khớp động thứ 5 68

IX
Hình 2.22 Mômen trên khớp động thứ 6 69
Hình 3.1 Các khái niệm về ổn định 72
Hình 3.2 Gốc không ổn định 72
Hình 3.3 Trạng thái hội tụ không ổn định 73
Hình 3.4 Sự phân kỳ trạng thái khi chuyển động
dọc theo các đường năng lượng thấp 76
Hình 3.5 Sự hội tụ đến tập bất biến lớn nhất 77
Hình 3.6 Sơ đồ hoạt động của rôbốt 78
Hình 3.7 Sơ đồ tổng quát của hệ thống

điều khiển không gian khớp 79
Hình 3.8 Sơ đồ tổng quát của hệ thống điều khiển không gian thao tác 79
Hình 3.9 Sơ đồ điều khiển rôbốt 80
Hình 3.10 Hàm sat 83
Hình 3.11 Hàm arctan 83
Hình 3.12 Sơ đồ tính toán và mô phỏng điều khiển rôbốt 84
Hình 3.13 Rôbốt 4 khâu động 85
Hình 3.14 Đồ thị nhiễu tác động trên khớp động 1 86
Hình 3.15 Đồ thị nhiễu tác động trên khớp động 2 86
Hình 3.16 Đồ thị nhiễu tác động trên khớp động 3 87
Hình 3.17 Đồ thị nhiễu tác động trên khớp động 4 87
Hình 3.18 Momen điều khiển tại khớp 1 88
Hình 3.19 Momen điều khiển tại khớp 2 88
Hình 3.20 Momen điều khiển tại khớp 3 89
Hình 3.21 Momen điều khiển tại khớp 4 89
Hình 3.22 Đồ thị sai số suy rộng s1 90
Hình 3.23 Đồ thị sai số suy rộng s2 90
Hình 3.24 Đồ thị sai số suy rộng s3 91
Hình 3.25 Đồ thi sai số suy rộng s4 91
Hình 3.26 Đồ thị tọa độ x(t) trong không gian thao tác theo thờ
i gian 92
Hình 3.27 Đồ thị tọa độ y(t) trong không gian thao tác theo thời gian 92
Hình 3.28 Rôbốt Scara 93
Hình 3.29 Đồ thị nhiễu tác động trên khớp động 1 94
Hình 3.30 Đồ thị nhiễu tác động trên khớp động 2 94
Hình 3.31 Đồ thị nhiễu tác động trên khớp động 3 95
Hình 3.32 Đồ thị nhiễu tác động trên khớp động 4 95
Hình 3.33 Momen điều khiển tại khớp 1 96
Hình 3.34 Momen điều khiển tại khớp 2 96
Hình 3.35 Lực điều khiển tại kh

ớp 3 97
Hình 3.36 Momen điều khiển tại khớp 4 97
Hình 3.37 Đồ thị sai số suy rộng s1 98

X
Hình 3.38 Đồ thị sai số suy rộng s2 98
Hình 3.39 Đồ thị sai số suy rộng s3 99
Hình 3.40 Đồ thi sai số suy rộng s4 99
Hình 3.41 Đồ thị tọa độ x(t) trong không gian thao tác theo thời gian 100
Hình 3.42 Đồ thị tọa độ y(t) trong không gian thao tác theo thời gian 100
Hình 3.43 Đồ thị tọa độ z(t) trong không gian thao tác theo thời gian 101
Hình 3.44 Đồ thị góc quay φ
trong không gian thao tác theo thời gian 101
Hình 4.1 Mô hình rôbốt đo BKHN-MCX-04 104
Hình 4.2 Sơ đồ động học rôbốt đo BKHN-MCX-04 105
Hình 4.3 Quỹ đạo định trước của điể
m E (đường xoắn ốc) 109
Hình 4.4 Đồ thị biến khớp q 109
Hình 4.5 Đồ thị vận tốc góc các khớp
q
&
110
Hình 4.6 Đồ thị gia tốc góc các khớp
q
&&
110
Hình 4.7 Vị trí trọng tâm các khâu 114
Hình 4.8 Quỹ đạo điểm E 119
Hình 4.9 Vị trí các khâu trong quá trình chuyển động 120
Hình 4.10 Trị số của biến khớp 2, 3 và 4 120

Hình 4.11 Vận tốc góc của động cơ dẫn động khâu 2, 3 và 4 121
Hình 4.12 Gia tốc góc của động cơ dẫn động khâu 2, 3 và 4 121
Hình 4.13 Mômen dẫn động cần thiết cho các khâu của rôbốt 122
Hình 4.14 Quỹ đạo định trước của điểm E (đường xoắn ốc) 123
Hình 4.15 Đồ thị tọ
a độ x
E
(t) trong không gian thao tác theo thời gian 124
Hình 4.16 Đồ thị tọa độ y
E
(t) trong không gian thao tác theo thời gian 125
Hình 4.17 Đồ thị tọa độ z
E
(t) trong không gian thao tác theo thời gian 125
Hình 4.18 Mô hình thí nghiệm rôbốt đo BKHN-MCX-04 126
Hình 4.19 Sơ đồ điều khiển toàn bộ rôbốt 128
Hình 4.20 Sơ đồ điều khiển từng khớp của BKHN-MCX-04 129
Hình 4.21 Sơ đồ nguyên lý của mạch điều khiển 1 trục 130
Hình 4.22 Quỹ đạo thực nghiệm của điểm cuối E 132

1
MỞ ĐẦU
1. Lí do lựa chọn đề tài
Công nghiệp hóa và hiện đại hóa nền sản xuất là một chủ trương lớn của
đất nước chúng ta hiện nay. Với xu thế chung của thế giới, để có thể đẩy mạnh
sự nghiệp công nghiệp hóa và hiện đại hóa cần ưu tiên áp dụng các tiến bộ của
khoa học kỹ thuật vào đời sống sản xuất. Trong đó v
ấn đề quan trọng nhất là
phải tăng nhanh lượng tự động hóa vào các quá trình sản xuất công nghiệp. Đây
cũng là một đòi hỏi cấp bách liên quan đến việc giải phóng con người khỏi sự

nặng nhọc, sự nhàm chán của công việc (do sự lặp đi lặp lại các thao tác của
một công việc giản đơn nào đó), sự nguy hiểm của môi trường lao động như sự
nóng bức t
ại các lò hơi, sự lây lan của các bệnh hiểm nghèo tại các cơ sở y tế,
sự ô nhiễm do bụi bặm của các hầm mỏ, sự nguy hiểm ở duới đáy đại dương và
trên không gian vũ trụ… Để có thể khắc phục được những vấn đề vừa nêu, các
công ty ở các nước có nền sản xuất phát triển đã đưa các rôbốt vào các dây
chuyền sản xuất của mình. Dướ
i đây là một số hình ảnh về các rôbốt và nơi ứng
dụng của chúng mà chúng ta thường gặp

Hình 0.1. Rôbốt trên dây chuyền của một trung tâm sản xuất linh hoạt.

2



Hình 0.2 rôbốt trên dây chuyền của máy phay cnc.











hình 0.2. Rôbốt phục vụ máy phay CNC.

Hình 0.3. Rôbốt Mitsubishi RV-2AJ

3


a. b. c.
Hình 0.4. Một số loại rôbốt
• Tại các lò phản ứng hạt nhân (a)
• Thám hiểm đại dương (b)
• Khám phá vũ trụ (c)
Trong các tài liệu về rôbốt [25, 28, 32, 39] người ta đã đưa ra một số khái
niệm về rôbốt công nghiệp. Dưới đây là một số khái niệm (định nghĩa) điển
hình:
• Rôbốt công nghiệp là một máy tự động linh hoạt được sử d
ụng để thay
thế từng phần hoặc toàn bộ các hoạt động cơ bắp và hoạt động trí tuệ của
con người với nhiều khả năng thích nghi khác nhau.
• Rôbốt công nghiệp là một cơ cấu chuyển đổi tự động có thể chương trình

hóa, lặp lại các chương trình, tổng hợp các chương trình đặt ra trên các
trục tọa độ có khả năng định vị, định hướng, di chuy
ển các đối tượng vật
chất, chi tiết, dụng cụ gá lắp, dao cắt … theo những chương trình thay
đổi, đã chương trình hóa nhằm thực hiện các chương trình công nghệ
khác nhau.
• Rôbốt công nghiệp là một tay máy vạn năng hoạt động theo chương trình
và có thể lập trình lại để hoàn thành và nâng cao hiệu quả hoàn thành các
nhiệm vụ khác nhau trong công nghiệp, như vận chuyển nguyên vật liệu,
chi tiết, thiết bị hoặc các dụng cụ chuyên dùng khác.
Những chiếc rôbốt công nghiệp đầu tiên được chế tạo vào năm 1956 bởi
công ty Unimation của George Devol và Joseph F. Engelberger ở Mỹ. Các rôbốt
đầu tiên này chủ yếu được dùng để vận chuyển các vật thể trong một phạm vi
nhỏ. Tính năng làm việc của rôbốt ngày càng được hoàn thiện và nâng cao hơn,
nhất là khả năng nhận biết và và xử lý các thông tin. Các rôbốt ngày nay được
trang bị thêm các loại cảm biến khác nhau để nhậ
n biết môi trường xung quanh
và nhờ các thành tựu to lớn trong các lĩnh vực như điều khiển học, tin học và

4
điện tử học mà các rôbốt có thêm nhiều tính năng đặc biệt. Nhờ vậy mà các
rôbốt công nghiệp đã có một vị trí quan trọng trong các dây chuyền sản xuất
hiện đại, nhất là trong các dây chuyền sản xuất tự động linh hoạt (FMS). Do đó
các nước đang đẩy mạnh việc sản xuất các rôbốt công nghiệp để ứng dụng vào
các ngành công nghiệp, đi đầu trong lĩnh vực chế tạ
o rôbốt công nghiệp hiện
nay đó là Nhật Bản, kế đến là Mỹ, Đức, Ý, Pháp, Anh và Hàn Quốc.
Theo [9, 22, 25, 28, 32, 44, 56, 58], cấu tạo của rôbốt thường có 3 bộ
phận chủ yếu, đó là:
• Tay máy (manipulator)

• Bộ phận dẫn động
• Bộ phận điều khiển
Trong đó tay máy là bộ phận cơ khí quan trọng, đóng vai trò là một bộ
phận chấp hành của rôbốt. Tay máy cấu t
ạo bởi các khâu và các khớp nhằm mô
phỏng theo nguyên tắc hoạt động của bàn tay con người.
Bộ phận dẫn động gồm các động cơ (có thể là động cơ điện, khí nén hoặc
thủy lực) để tạo nên chuyển động cho các khớp của tay máy.
Bộ phận điều khiển giữ vai trò quan trọng như là bộ não của con người.
Bộ điều khiển được dùng để
điều khiển các hoạt động của rôbốt. Bộ phận điều
khiển thường được thực hiện thông qua một hệ thống chương trình điều khiển -
mỗi chương trình đảm nhận một nhiệm vụ cụ thể.
Để có thể khai thác, sử dụng một cách hiệu quả các rôbốt đã được trang
bị, cũng như để có thể tiến hành nghiên cứ
u, thử nghiệm, chế tạo các rôbốt mới
đáp ứng được nhu cầu đòi hỏi ngày càng cao của nền công nghiệp hiện đại thì
việc nghiên cứu rôbốt đang được các cơ sở sản xuất, các nhà khoa học, các
trường học đại học, cao đẳng quan tâm.
Khi nghiên cứu về rôbốt chúng ta thường phải giải quyết các bài toán về
động học, động lực học và điều khiển. Trong các bài toán này thì bài toán ngượ
c
mà đặc biệt là các bài toán ngược của rôbốt dư dẫn động là bài toán khó và hiện
nay đang còn ít được nghiên cứu ở nước ta. Vì vậy tác giả chọn cho mình đề tài
“Giải bài toán ngược động học, động lực học và điều khiển trượt rôbốt dư dẫn
động dựa trên thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng” nhằm cải
tiến phương pháp số giải bài toán ng
ược đã có hiện nay.
2. Đối tượng và nội dung nghiên cứu
Đối tượng nghiên cứu của luận án là các rôbốt dư dẫn động. Rôbốt loại

này có số bậc tự do lớn hơn số tọa độ xác định vị trí và hướng của bàn kẹp, do
đó được sử dụng khá linh hoạt trong các nhiệm vụ thao tác phức tạp. Nội dung
nghiên cứu là khảo sát bài toán động học ngược, bài toán động lực h
ọc ngược
và bài toán điều khiển trượt rôbốt dư dẫn động.

5
3. Tổng quan về vấn đề nghiên cứu
Việc nghiên cứu các bài toán liên quan đến rôbốt dư dẫn động cũng đã
được các tác giả nước ngoài nghiên cứu và trình bày trong các công trình khoa
học của mình. Theo [15, 18, 20, 43, 47, 57, 59, 65] thì một rôbốt được gọi là dư
dẫn động khi số tọa độ suy rộng nhiều hơn số tọa độ tối thiểu xác lập nên vị trí
và hướng của khâu thao tác theo đúng yêu cầu của bài toán công nghệ. Nhờ
tính
dư dẫn động mà rôbốt loại này có khả năng tránh được các vật cản, các điểm kỳ
dị, các giới hạn của biến khớp, …, nghĩa là các rôbốt dư dẫn động có tính linh
hoạt cao hơn hẳn so với các rôbốt chuẩn.
Bài toán động lực học và bài toán điều khiển rôbốt là các bài toán có ý
nghĩa thực tiễn cao, nó làm cơ sở cho việc nghiên cứu chế tạo ra các loại rôbố
t
mới đặc biệt là các rôbốt dư dẫn động, một lĩnh vực mà hiện còn được ít nghiên
cứu ở nước ta.
Trong các tài liệu [9, 22, 25, 28, 32, 44, 55] nói về rôbốt hiện nay, nhìn
chung các tác giả đã trình bày khá đầy đủ về các bài toán động học, động lực
học và điều khiển rôbốt chuẩn.
Bài toán động học được chia làm 2 nhóm là nhóm bài toán động học
thuận và nhóm bài toán động học ngược. Nhóm bài toán động học thuận có
nhi
ệm vụ xác định vị trí và hướng của khâu thao tác khi biết được sơ đồ cấu trúc
của rôbốt và các quy luật chuyển động của các khớp động. Bài toán này được

giải dựa vào phép biến đổi tọa theo phương pháp Denavit-Hartenberg. Kết quả
thu nhận được là một ma trận mô tả hướng và phương trình xác định tọa độ của
bàn kẹp. Nhóm bài toán động học ngược là nhóm bài toán xác định các đặc tính
chuyển động của các kh
ớp động để rôbốt tạo ra được quy luật chuyển động
mong muốn nhằm thực hiện một nhiệm vụ cụ thể nào đó. Việc giải các bài toán
động học ngược thường là phức tạp và khó khăn hơn so với bài toán động học
thuận. Để giải quyết bài toán này thông thường ta phải thiết lập phương trình
mô tả quan hệ giữa tọa độ của khâu thao tác (bàn kẹp) với các góc kh
ớp (đây là
kết quả của bài toán thuận) rồi dựa vào phương trình chuyển động mong muốn
để tìm ra các đặc tính chuyển động cho các khớp. Trong trường hợp số khớp
dẫn động đúng bằng với số tọa độ xác định nên cấu hình của rôbốt thì về
nguyên tắc ta đều giải được và cũng đã được trình bày khá đầy đủ trong các tài
liệu tham khảo đã nêu. Trong trường hợp khi số
khớp dẫn động nhiều hơn số tọa
độ tối thiểu để xác lập vị trí của khâu thao tác (liên quan tới bài toán ngược của
rôbốt dư dẫn động) thì bài toán sẽ có nhiều nghiệm và dạng bài toán này còn ít
được trình bày trong các tài liệu, nhất là các tài liệu bằng tiếng Việt.
Bài toán động lực học cũng có 2 nhóm là nhóm bài toán động lực học
thuận và nhóm bài toán động lực học ngược. Nhiệm vụ của bài toán động lực
học ngược là thiết lập phương trình vi phân chuyển động cho các khâu động của
rôbốt từ đó xác định lực/mômen tác động trên các khớp. Việc thiết lập phương

6
trình vi phân chuyển động trình bày trong các tài liệu [9, 15, 22, 25, 28, 32, 38,
39, 44, 55, 64, 67] đều dựa trên cơ sở các phương trình Newton-Euler hoặc các
phương trình Lagrange loại 2 cho hệ nhiều vật.
Với bài toán động lực học ngược ta đã biết hoặc đã lựa chọn được sơ đồ
cấu trúc của rôbốt, theo yêu cầu của bài toán công nghệ mà ta đã có phương

trình chuyển động của khâu thao tác từ đó ta phải xác định phương trình động
lực học c
ủa rôbốt. Để xây dựng phương trình động lực học ta phải biết được các
đặc tính chuyển động của các khâu, vì vậy ta lại phải giải bài toán động học
ngược. Kết quả giải bài toán động học ngược sẽ được sử dụng để giải bài toán
động lực học ngược.
Nhiệm vụ chủ yếu của bài toán điều khiển là duy trì chuyển động của
khâu thao tác theo 1 qu
ỹ đạo mong muốn nào đó được xác định trước theo yêu
cầu của công nghệ [28, 38]. Để thực hiện được nhiệm vụ này ta có thể thực hiện
bằng 2 cách là điều khiển chuyển động của các khớp động (điều khiển không
gian khớp) và điều khiển chuyển động của bàn kẹp (điều khiển không gian thao
tác). Với phương pháp điều khiển không gian khớp ta tiế
n hành điều khiển
chuyển động của các khớp theo vị trí mong muốn mà ta đã xác định trước còn ở
phương pháp điều khiển không gian thao tác thì ta phải điều khiển khâu thao tác
đúng vị trí mong muốn do đó phải tính toán lượng chuyển động cho các khớp
trong quá trình điều khiển vì vậy mà quá trình điều khiển thường chậm hơn do
khối lượng tính toán nhiều khi điều khiển. Trong luận án của mình, tác gi
ả lựa
chọn phương án điều khiển trong không gian các khớp. Để thực hiện theo
phương án này ta phải xác định trước các đặc tính chuyển động của các khớp
trên cơ sở chuyển động định trước của khâu thao tác, nghĩa là ta lại phải giải lại
bài toán động học ngược của rôbốt. Kết quả của bài toán động học ngược là đầu
vào cho bài toán điều khiển trong không gian khớp. Trong quá trình
điều khiển
ta phải đảm bảo tính ổn định chuyển động của hệ thống. Theo [27, 63] thì điều
khiển dạng trượt là phương pháp điều khiển có tính ổn định cao dù cho trên hệ
thống trong quá trình làm việc có thể có các nhiễu tác động ngẫu nhiên và có
các sai số do cấu trúc đưa lại cho nên tác giả vận dụng phương pháp điều khiển

dạng trượt cho việc điều khiển chuy
ển động trong không gian khớp đối với các
rôbốt dư dẫn động,
Các bài toán nêu trên đều có một điểm chung là đều phải tiến hành giải
bài toán động học ngược và đây là một bài toán khó đối với các rôbốt dư dẫn
động vì vậy mà dưới sự hướng dẫn của GS.TSKH Nguyễn Văn Khang chúng đã
xây dựng “Thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng” làm cơ sở

để giải bài toán động học ngược và xây dựng các thuật toán giải bài toán động
lực học ngược và bài toán điều khiển chuyển động trong không gian khớp. Ở
nước ta việc sử dụng phương pháp số để giải các bài toán ngược của rôbốt, nhất
là đối với rôbốt dư dẫn động hãy còn ít được nghiên cứu.

7
4. Cấu trúc của luận án
Cấu trúc của luận án gồm: Phần mở đầu, 4 chương nội dung và phần kết
luận chung.
Chương 1: “Tính toán động học ngược rôbốt dư dẫn động bằng thuật
toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng”. Trong chương này tác giả
giới thiệu khái niệm về ma trận tựa nghịch đảo và ứng dụng của ma trậ
n tựa
nghịch đảo để giải các bài toán hệ phương trình đại số tuyến tính trong trường
hợp số lượng phương trình của hệ ít hơn số lượng các ẩn số cần tìm. Sau đó đã
trình bày một thuật toán số giải bài toán động học ngược của rôbốt dư dẫn động.
Chương 2: “Tính toán động lực học ngược rôbốt dư dẫn động trong
không gian thao tác dựa trên thu
ật toán hiệu chỉnh gia lượng véc tơ tọa độ suy
rộng”. Nội dung chủ yếu của chương là tập trung xây dựng một thuật toán để
tính toán các lực/mômen tác động lên các khớp động của rôbốt dư dẫn động
trong không gian thao tác.

Chương 3: “Điều khiển trượt rôbốt dư dẫn động dựa trên thuật toán số
hiệu chỉnh gia lượng véc tơ tọa độ suy rộng”. Trong chương này gi
ới thiệu sơ
lược về cơ sở lý thuyết ổn định trong điều khiển và nội dung bài toán điều khiển
chuyển động. Nội dung chính của chương 3 là trình bày một thuật toán giải bài
toán điều khiển chuyển động trong không gian khớp theo phương pháp điều
khiển dạng trượt.
Chương 4: “Động lực học và điều khiển trượt rôbốt đo BKHN-MCX-04”.
Trong ch
ương này áp dụng lý thuyết trình bày trong 3 chương đầu để giải bài
toán động học ngược, động lực học ngược và điều khiển một rôbốt đo mới được
chế tạo tại Trường Đại Học Bách Khoa Hà Nội. Trên mô hình rôbốt tự chế tạo,
dễ dàng xác định các tham số động học, động lực học của rôbốt.
Chương 1
TÍNH TOÁN ĐỘNG HỌC NGƯỢC RÔBỐT DƯ DẪN ĐỘNG
BẰNG THUẬT TOÁN HIỆU CHỈNH GIA LƯỢNG
VÉC TƠ TỌA ĐỘ SUY RỘNG
Trong các tài liệu về rôbốt người ta thường ký hiệu véc tơ tọa độ suy rộng
(véc tơ mô tả cấu hình của rôbốt) bởi
[
]
T
n21
q, ,q,q=q và ký hiệu véc tơ xác
định vị trí và hướng của bàn kẹp trong hệ quy chiếu cố định bởi
[]
T
m21
x, ,x,x=x
. Từ bài toán động học thuận của rôbốt ta xác định được hệ

thức
)(qfx = (1.1)
Trong đó
.R,;R
mn
∈
∈ fxq Khi n
m
=
rôbốt được gọi là rôbốt chuẩn, còn
khi
n
m
<
rôbốt được gọi là rôbốt dư dẫn động.
Nội dung của bài toán động học ngược là: Cho biết
x = x(t), tìm
).
t
(qq
=

Như vậy trong bài toán động học ngược chúng ta phải thiết lập được quan hệ
hình thức:

)(
1
xfq
−
=

(1.2)
Các phương pháp giải bài toán động học ngược rôbốt được phân thành
hai nhóm: nhóm các phương pháp giải tích và nhóm các phương pháp số. Việc
sử dụng các phương pháp giải tích để giải bài toán động học ngược của rôbốt dư
dẫn động được trình bầy trong các tài liệu về rôbốt dư dẫn động [57, 59, 66].
Trong khi đó việc sử dụng các phương pháp số để giải bài toán động học ngược
của rôbốt d
ư dẫn động còn ít được nghiên cứu.
Trong chương này dựa trên công thức khai triển Taylor chúng tôi đề xuất
một thuật toán mới giải bài toán động học ngược rôbốt dư dẫn động. Thuật toán
đề xuất được gọi là thuật toán
hiệu chỉnh gia lượng véc tơ tọa độ suy rộng. Sau
đó áp dụng thuật toán đề xuất để giải bài toán động học ngược của một vài rôbốt
cụ thể, nhằm minh họa cho khả năng tính toán của thuật toán.
1.1 Giải bài toán động học thuận rôbốt dư dẫn động bằng phương pháp ma
trận Denavit – Hartenberg
Để nghiên cứu động học của rôbốt, Denavit và Hartenberg [28, 38, 39,
44] đã đề xuất phương án gắn hệ trục tọa độ lên các khâu của rôbốt, để từ đó
chuyển đổi tọa độ của điểm thao tác về hệ tọa độ gắn liền với hệ quy chiếu cố
định. Hệ tọa độ Denavit – Hartenberg được xây dựng như sau:
Xét 2 khâu kế tiếp nhau của rôbốt là khâu thứ i–1
và khâu thứ i như hình
1.1. Gốc
i
O
của hệ trục tọa độ
iiii
zyxO được gắn liền với khâu thứ i (hệ tọa độ

9
thứ i) và được đặt tại giao điểm của trục khớp động thứ i+1 với đường vuông
góc chung của các trục khớp động thứ i
và thứ i+1. Trường hợp 2 trục khớp
động giao nhau thì gốc tọa độ là điểm giao nhau đó, còn nếu chúng song song
nhau thì gốc tọa độ là điểm bất kỳ trên trục khớp động thứ i+1.
Trục
i
z
của hệ tọa độ thứ i nằm dọc theo trục khớp động thứ i+1.
Trục
i
x
của hệ tọa độ thứ i nằm dọc theo đường vuông góc chung của 2
trục khớp động là khớp thứ i và i+1, có hướng từ khớp động thứ i tới khớp động
thứ i+1. Trong trường hợp 2 trục khớp động giao nhau thì hướng của trục
i
x

trùng với hướng tích véc tơ
1ii −
×
zz .
Trục
i
y
được chọn sao cho hệ tọa độ
iiii
zyxO
là hệ tọa độ thuận.

Hình 1.1 Sơ đồ thiết lập hệ tọa độ các khâu
Với cách thiết lập các trục của hệ tọa độ như đã trình bày thì hệ quy chiếu
đôi khi không được xác định một cách duy nhất, vì vậy ta cần bổ sung thêm một
số điều kiện như sau:
• Đối với hệ tọa độ
0000
zyxO - là hệ tọa độ cố định nằm dọc theo trục của
khớp động thứ 1, có trục
0
z đã được xác lập theo nguyên tắc trên, còn
i
θ

1i−
θ

i
θ

1i+
θ

1i
x
−

i
x

1−i
z
i
a
i
α
i
z
K
hớ
p
thứ i
K
hâu i
Khớp thứ i+1
i
O
1i
O
−

i
d

O
i
’
y
i

Khâu i-1
z
i-1
z
i


10
trục
0
x thì do không có khớp động thứ 0 nên ta có thể chọn tùy ý, miễn là
nó phải vuông góc với trục
0
z.
• Đối với hệ trục tọa độ
nnnn
zyxO
gắn với khâu thứ n, ta thấy do không có
khớp động thứ n+1 nên theo quy ước ở trên thì ta không xác định được
trục
n
z. Trục
n
z không được xác định duy nhất trong khi trục
n
x lại được
chọn theo pháp tuyến của trục
.
1n−

z
Trong trường hợp này nếu khớp n là
khớp quay thì ta có thể chọn trục .z//
1nn −
z Ngoài ra có thể chọn tùy ý
sao cho hợp lý với phép chuyển dịch trục.
• Khi khớp thứ i là khớp tịnh tiến, về nguyên tắc ta có thể chọn trục
1i
z
−
một cách tùy ý. Tuy nhiên trong nhiều trường hợp người ta thường
chọn trục
1i
z
−
dọc theo trục của khớp tịnh tiến này.
1.1.1 Các tham số động học và ma trận Denavit-Hartenberg
Sau khi thiết lập xong các hệ tọa độ, ta thấy vị trí của hệ tọa độ
iiii
zyxO

so với hệ tọa độ
1i1i1i1-i
zyxO
−−−
được xác định bởi 4 tham số sau đây:
1.

i
θ là góc quay trục

1i
x
−
xung quanh trục
1i
z
−
theo chiều ngược chiều
kim đồng hồ để phương của các trục tọa độ
1i
x
−
và
i
x trùng nhau.
2.

'
i1-ii
OOd = là khoảng dịch chuyển tịnh tiến dọc theo trục
1i
z
−
để gốc
tọa độ
1-i
O
chuyển đến
'
i

O - giao điểm của trục
i
x
với
1i
z
−
(hình 1.1).
3.

i
alà khoảng dịch chuyển dọc theo trục
i
x để đưa
'
i
O tới điểm
i
O.
4.
i
α
là góc quay quanh trục
i
x
sao cho trục
1i
z
−
chuyển đến trục

i
z
.
Như vậy với các phép dời trục như đã nêu ở trên, ta có thể đưa hệ quy
chiếu
1i1i1i1-i
zyxO
−−−
về trùng với hệ quy chiếu
iiii
zyxO . Do vậy 4 tham số
i
θ ,
d
i
, a
i
,
i
α nêu trên được gọi là 4 tham số động học Denavit – Hartenberg.
Trong 4 tham số nêu trên thì các tham số
i
avà
i
α luôn là các hằng số, độ
lớn của chúng phụ thuộc vào hình dáng và sự ghép nối của các khâu thứ i và i-1.
Hai tham số còn lại
i
θ và
i

d thì một sẽ là hằng số còn tham số kia sẽ là biến số
phụ thuộc vào loại khớp của khớp động thứ i+1. Nếu khớp thứ i+1 là khớp quay
thì
i
θ là biến,
i
dlà hằng số. Còn khi khớp là khớp tịnh tiến thì ngược lại.
Như vậy ta có thể dùng 4 phép biến đổi cơ bản sau đây để chuyển tọa độ
từ hệ tọa độ
1i1i1i1-i
zyxO
−−−
về hệ tọa độ
iiii
zyxO
:
- Quay xung quanh trục
1i
z
−
một góc
i
θ .
-
Dịch chuyển tịnh tiến dọc trục
1i
z
−
một đoạn d
i

.


11
- Dịch chuyển tịnh tiến dọc trục
i
x một đoạn a
i
.
- Quay xung quanh trục
i
x một góc .α
i

Ta ký hiệu ma trận của phép biến đổi tọa độ từ hệ tọa độ
1i1i1i1-i
zyxO
−−−

sang hệ tọa độ
iiii
zyxO
là
i
1i
H
−
. Sử dụng các tọa độ thuần nhất [25, 28], ta suy
ra ma trận của phép biến đổi này sẽ là tích của 4 ma trận và có dạng như sau:
Theo [28, 38], nếu quay hệ tọa độ xung quanh trục

1i
z
−
một góc
i
θ thì ma
trận biến đổi các tọa độ sẽ là:

⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
θθ
θ−θ
=
⎥
⎥
⎥
⎥
⎦
⎤
⎢

⎢
⎢
⎢
⎣
⎡
=
θ
1000
0100
00cossin
00sincos
1000
zaaa
yaaa
xaaa
ii
ii
)0(
O333231
)0(
O232221
)0(
O131211
),z(
1
1
1
i
A
Còn khi dịch chuyển tịnh tiến theo trục

1i
z
−
một đoạn
i
dthì ma trận biến
đổi sẽ là :

⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
=
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢

⎢
⎣
⎡
=
1000
100
0010
0001
1000
i
)0(
O333231
)0(
O232221
)0(
O131211
)d,z(
1
1
1
i
d
zaaa
yaaa
xaaa
A

ở bước dịch chuyển tịnh tiến theo trục
i
x

một đoạn
i
a
thì ma trận chuyển đổi
tọa độ sẽ là:

⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
=
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣

⎡
=
1000
0100
0010
001
1000
i
)0(
O333231
)0(
O232221
)0(
O131211
)d,x(
1
1
1
i
a
zaaa
yaaa
xaaa
A

và ở bước quay xung quanh trục
i
x một góc
i
α , ma trận chuyển đổi là:

⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
αα
α−α
=
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
=
α

1000
0cossin0
0sincos0
0001
1000
zaaa
yaaa
xaaa
ii
ii
)0(
O333231
)0(
O232221
)0(
O131211
),x(
1
1
1
i
A

Như vậy nếu dùng hệ tọa độ Denavit – Hartenberg thì ma trận chuyển đổi
tọa độ
i
1i
H
−
từ hệ

1i1i1i1-i
zyxO
−−−
sang hệ tọa độ
iiii
zyxO là:


12
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
αα
α−α
⎥
⎥
⎥
⎥
⎦
⎤
⎢

⎢
⎢
⎢
⎣
⎡
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
θθ

θ−θ
=
=
αθ
−
1000
0cossin0
0sincos0
0001
1000
0100
0010
001
1000
100
0010
0001
1000
0100
00cossin
00sincos
.
ii
ii
i
i
ii
ii
),x()a,x()d,z(),z(i
1i

iiii
a
d
AAAAH

Lấy tích của 4 ma trận trên, ta được một ma trận được gọi là ma trận
Denavit–Hartenberg và ta có kết quả như sau:
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
−
−
=
−
1000
dαcosαsin0
θsinaαsinθcosαcosθcosθsin
θcosaαsinθsinαcosθsinθcos
iii
iiiiiii
iiiiiii

i
1i
H (1.3)
Với việc sử dụng công thức (1.3) ta sẽ thực hiện được việc chuyển đổi tọa
độ từ hệ tọa độ này qua hệ tọa độ khác (từ khâu động học này qua khâu động
học khác). Khi đó, công thức để chuyển tọa độ từ hệ tọa độ
{
}
1i1i1i1-i
zyxO
−−−
sang
hệ tọa độ
{}
iiii
zyxO là:
[]
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
=

⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎣
⎡
−
−
−
−
1
z
y
x
1
z
y
x
1i
1i
1i
i
1i
i

i
i
H (1.4)
1.1.2 Phương trình xác định vị trí và hướng của khâu thao tác
Xét một mô hình tổng quát của một rôbốt có n khâu như trên hình vẽ 1.2.
Như đã trình bày ở trên, ma trận
i
H cho ta biết :
•
Vị trí của điểm
i
O trong hệ quy chiếu
{
}
1-i1-i1-i1-i1i
zyxO R
−
.
•
Hướng của vật rắn
i
B trong hệ quy chiếu
{
}
1-i1-i1-i1-i1i
zyxOR
−
.
Như vậy, bằng cách chuyển dần hệ quy chiếu
{

}
nnnnn
zyxOR từ
n
O
về
1-n
O ,
2-n
O , … và cuối cùng là về hệ quy chiếu cố định
{}
00000
zyxOR ta sẽ
xác định được vị trí của điểm gốc
n
O và hướng của khâu động thứ n trong hệ
quy chiếu cố định
{}
00000
zyxOR .