Luan An Tien Si Nam PDF

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
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

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ố
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
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
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
ai Khoảng dịch chuyển dọc trục xi (chiều dài của khâu thứ i)
αi Góc quay quanh trục xi.
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.
Ai 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.
Ci Cosin (qi).
C12…n Cosin(q1+q2+…+qn).
C(q, q& i ) q& i Véc tơ các lực Coriolis và lực hướng tâm của mô hình thực.
Cˆ (q, q& ) q& Véc tơ các lực Coriolis và lực hướng tâm của bộ điều khiển.
i
~ ˆ (q, q& ) .
C(q, q& ) Sai số giữa C(q, q& ) và C
i
di Khoảng dịch chuyển gốc tọa độ Oi-1 về O’i.

d Véc tơ nhiễu.
Di Ma trận chuyển đổi tọa độ từ O0x0y0z0 về hệ tọa độ Oixiyizi.
ek Sai số dịch chuyển của bàn kẹp (không gian thao tác).
e& k Sai số vận tốc của bàn kẹp.
&e&k Sai số gia tốc của bàn kẹp.
e(t) Véc tơ sai số bám theo góc quay.
e& ( t ) Sai số bám theo vận tốc quay.
ε Sai số của phép tính.
epxi 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.
gˆ (q) Véc tơ các lực trọng trường của bộ điều khiển.
~
g (q) Sai số giữa g(q) và gˆ (q) .
i-1
Hi Ma trận biến đổi tọa độ từ hệ trục Oi-1xi-1yi-1zi-1 về Oixiyizi.
h=Δt Khoảng thời gian của bước tính toán.
li 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.

mi Khối lượng của chất điểm thứ i.
M(q) Ma trận khối lượng suy rộng.
M ˆ (q) Momen khối lượng của bộ điều khiển.
~ ˆ (q) .
M (q) Sai số giữa M(q) và M
Ii Tenxơ quán tính khối của vật rắn thứ i.
Izi Momen quán tính đối với trục z của khâu thứ i.
JTi Ma trận Jacobi tịnh tiến của vật rắn thứ i.
JRi 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.
J& + (q) Đạ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.
Ks Hệ số khuyếch đại dạng trượt.
Kpd Hệ số khuyếch đại tỷ lệ - đạo hàm.
omi Vận tốc góc của khâu thứ i.
Px, Py, Pz Các tọa độ x, y, z của điểm thao tác P.
Π Thế năng của hệ.
∂Π
Thành phần lực suy rộng của các lực có thế.
∂q
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.
q0 Véc tơ tọa độ suy rộng tại thời điểm ban đầu.
q& 0 Đạo hàm bậc nhất theo thời gian của q0.
q
&& 0 Đạo hàm bậc 2 theo thời gian của q0.
~
q Giá trị gần đúng của q0.
0
Δq0 Số gia của q0.

qk+1 Giá trị của q tại thời điểm t = tk+1.
q& k +1 Đạo hàm bậc 1 theo thời gian của qk+1.
q
&& k +1 Đạo hàm bậc 2 của qk+1.
~
q Giá trị gần đúng của qk+1.
k +1
qi_dot Đạo hàm bậc nhất của qi theo thời gian.

qi_2dot Đạo hàm bậc 2 của qi theo thời gian .
qd Véc tơ tọa độ suy rộng mong muốn.
q& d Đạo hàm của véc tơ suy rộng mong muốn theo thời gian.
V
rCi 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.
Si Sin(qi).
S12..n Sin(q1+q2+…+qn).
θi Góc quay của trục xi-1 xung quanh trục zi-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.
xd Vị trí mong muốn của bàn kẹp.
x& d Vận tốc chyển động mong muốn của bàn kẹp.
&x& d Gia tốc chuyển động mong muốn của bàn kẹp.
x* Trạng thái cân bằng.
~
xd Sai số bám của biến x.
~
xd Véc tơ sai số bám.
r
v Véc tơ hình học của vận tốc.
vi 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.
ω ~ Toán tử sóng của véc tơ vận tốc góc thứ i.
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.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
VIII
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.12 Lực trên khớp động thứ 3 58
Hình 2.15 Đồ thị các mômen động cơ theo thời gian 61
IX
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.14 Đồ thị nhiễu tác động trên khớp động 1 86
Hình 3.18 Momen điều khiển tại khớp 1 88
Hình 3.22 Đồ thị sai số suy rộng s1 90
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.35 Lực điều khiển tại khớp 3 97
X

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 độ xE(t) trong không gian thao tác theo thời gian 124
Hình 4.16 Đồ thị tọa độ yE(t) trong không gian thao tác theo thời gian 125
Hình 4.17 Đồ thị tọa độ zE(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 q = [q1 , q 2 ,..., q n ] và ký hiệu véc tơ xác
T
định vị trí và hướng của bàn kẹp trong hệ quy chiếu cố định bởi
x = [x 1 , x 2 ,..., x m ] . Từ bài toán động học thuận của rôbốt ta xác định được hệ
T
thức
x = f (q) (1.1)
Trong đó q ∈ R ; x, f ∈ R . Khi m = n rôbốt được gọi là rôbốt chuẩn, còn
n m
khi m < n 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 q = q( t ).
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:
q = f −1 ( x ) (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 O i của hệ trục tọa độ O i x i y i z i đượ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 z i của hệ tọa độ thứ i nằm dọc theo trục khớp động thứ i+1.
Trục x i 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 x i
trùng với hướng tích véc tơ z i × z i −1 .
Trục y i được chọn sao cho hệ tọa độ O i x i y i z i là hệ tọa độ thuận.
zi-1
θi
Khâu i
θ i −1 zi
θ i+1
Khớp thứ i
Khâu i-1
z i −1
Khớp thứ i+1
O i’ zi
αi
ai yi
di
x i −1 Oi
xi
O i−1
θi
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 độ O 0 x 0 y 0 z 0 - là hệ tọa độ cố định nằm dọc theo trục của
khớp động thứ 1, có trục z 0 đã được xác lập theo nguyên tắc trên, còn
10
trục x 0 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 z 0 .
• Đối với hệ trục tọa độ O n x n y n z n 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 z n . Trục z n không được xác định duy nhất trong khi trục x n lại được
chọn theo pháp tuyến của trục z n−1 . 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 n // z n −1 . 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
z i −1 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 z i −1 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 độ O i x i y i z i
so với hệ tọa độ O i -1 x i −1 y i −1 z i −1 được xác định bởi 4 tham số sau đây:
1. θ i là góc quay trục x i −1 xung quanh trục z i −1 theo chiều ngược chiều
kim đồng hồ để phương của các trục tọa độ x i −1 và x i trùng nhau.
2. d i = O i -1 O i' là khoảng dịch chuyển tịnh tiến dọc theo trục z i −1 để gốc
tọa độ O i-1 chuyển đến O i' - giao điểm của trục x i với z i −1 (hình 1.1).
3. a i là khoảng dịch chuyển dọc theo trục x i để đưa O i' tới điểm O i .
4. α i là góc quay quanh trục x i sao cho trục z i −1 chuyển đến trục z i .
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 O i -1 x i −1 y i −1 z i −1 về trùng với hệ quy chiếu O i x i y i z i . Do vậy 4 tham số θ i ,
di, ai, α 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ố a i và α 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à d i 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, d i là 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 độ O i-1 x i −1 y i −1 z i −1 về hệ tọa độ O i x i y i z i :
- Quay xung quanh trục z i −1 một góc θ i .
- Dịch chuyển tịnh tiến dọc trục z i −1 một đoạn di.
11
- Dịch chuyển tịnh tiến dọc trục x i một đoạn ai.

- Quay xung quanh trục x i 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 độ O i-1 x i −1 y i −1 z i −1
sang hệ tọa độ O i x i y i z i là i −1 H i . 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 z i −1 một góc θ i thì ma
trận biến đổi các tọa độ sẽ là:
⎡ a 11 a 12 a 13 x (O0 ) ⎤ ⎡cos θ i − sin θ i 0 0⎤
⎢ ⎥
y (O0 ) ⎥ ⎢⎢ sin θ i 0 0⎥
1
a a 22 a 23 cos θ i
A ( z ,θ ) = ⎢ 21 1
= ⎥
i
⎢a 31 a 32 a 33 z (O0 ) ⎥ ⎢ 0 0 1 0⎥
⎢ ⎥ ⎢ ⎥
1
⎣0 0 0 1 ⎦ ⎣ 0 0 0 1⎦
Còn khi dịch chuyển tịnh tiến theo trục z i −1 một đoạn d i thì ma trận biến
đổi sẽ là :
⎡ a11 a12 a13 xO( 0 ) ⎤ ⎡1 0 0 0⎤
⎢ ⎥
yO( 0 ) ⎥ ⎢⎢0 0⎥
1
a a22 a23 1 0
A ( z ,d ) = ⎢ 21 1
= ⎥
i
⎢ a31 a32 a33 z O( 0 ) ⎥ ⎢0 0 1 di ⎥
⎢ ⎥ ⎢ ⎥
1
⎣0 0 0 1 ⎦ ⎣0 0 0 1⎦
ở bước dịch chuyển tịnh tiến theo trục x i một đoạn a i thì ma trận chuyển đổi
tọa độ sẽ là:
⎡ a11 a12 a13 xO( 0 ) ⎤ ⎡1 0 0 ai ⎤
⎢ ⎥
yO( 0 ) ⎥ ⎢⎢0 1 0 0⎥
1
a a22 a23
A ( x ,d ) = ⎢ 21 1
= ⎥
i
⎢ a31 a32 a33 z O( 0 ) ⎥ ⎢0 0 1 0⎥
⎢ ⎥ ⎢ ⎥
1
⎣0 0 0 1 ⎦ ⎣0 0 0 1⎦
và ở bước quay xung quanh trục x i một góc α i , ma trận chuyển đổi là:
⎡ a 11 a 12 a 13 x (O0 ) ⎤ ⎡1 0 0 0⎤
⎢ (0) ⎥ ⎢ 0⎥
1
a a 22 a 23 y O ⎥ ⎢0 cos α i − sin α i
A ( x ,α ) = ⎢ 21 1
= ⎥
i
⎢a 31 a 32 a 33 z (O0 ) ⎥ ⎢0 sin α i cos α i 0⎥
⎢ ⎥ ⎢ ⎥
1
⎣0 0 0 1 ⎦ ⎣0 0 0 1⎦
Như vậy nếu dùng hệ tọa độ Denavit – Hartenberg thì ma trận chuyển đổi
tọa độ i −1 H i từ hệ O i -1 x i −1 y i −1 z i −1 sang hệ tọa độ O i x i y i z i là:
12
i −1
H i = A ( z,θi ) .A ( z,di ) A ( x ,ai ) A ( x ,αi )
⎡cos θi − sin θi 0 0⎤ ⎡1 0 0 0 ⎤ ⎡1 0 0 ai ⎤ ⎡1 0 0 0⎤
⎢ sin θ cos θi 0 0⎥ ⎢0 1 0 0 ⎥ ⎢0 1 0 0 ⎥ ⎢0 cos α i − sin α i 0⎥
=⎢ i ⎥⎢ ⎥⎢ ⎥⎢ ⎥
⎢ 0 0 1 0⎥ ⎢0 0 1 d i ⎥ ⎢0 0 1 0 ⎥ ⎢0 sin α i cos α i 0⎥
⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥
⎣ 0 0 0 1⎦ ⎣0 0 0 1 ⎦ ⎣0 0 0 1 ⎦ ⎣0 0 0 1⎦
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:
⎡cos θ i − sin θ i cos α i sin θ i sin α i a i cos θ i ⎤
⎢ sin θ cos θ i cos α i − cos θ i sin α i a i sin θ i ⎥
i −1
Hi = ⎢ i
⎥ (1.3)
⎢ 0 sin α i cos α i di ⎥
⎢ ⎥
⎣ 0 0 0 1 ⎦
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 độ {O i-1 x i −1 y i −1z i −1 } sang
hệ tọa độ {O i x i y i z i } là:
⎡x i ⎤ ⎡ x i −1 ⎤
⎢y ⎥ ⎢y ⎥
⎢ i ⎥ = [ i −1 H i ]⎢ i −1 ⎥ (1.4)
⎢zi ⎥ ⎢ z i −1 ⎥
⎢ ⎥ ⎢ ⎥
⎣1⎦ ⎣ 1 ⎦
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 H i cho ta biết :
• Vị trí của điểm O i trong hệ quy chiếu R i −1 {O i-1 x i-1 y i-1 z i-1 } .
• Hướng của vật rắn Bi trong hệ quy chiếu R i−1 {O i-1 x i-1 y i-1z i-1 }.
Như vậy, bằng cách chuyển dần hệ quy chiếu R n {O n x n y n z n } từ O n
về O n -1 , O n -2 , … và cuối cùng là về hệ quy chiếu cố định R 0 {O 0 x 0 y 0 z 0 } ta sẽ
xác định được vị trí của điểm gốc O n và hướng của khâu động thứ n trong hệ
quy chiếu cố định R 0 {O0 x 0 y 0 z 0 }.
13
On − 2
Oi
●●●
O n −1
●
O2 ●●
O1 On ≡ P
O0
Hình 1.2
Áp dụng liên tiếp các phép biến đổi đối với rôbốt n khâu ta có:
D n = 0 H n = 0 H 1 1 H 2 ...n −1 H n = H 1 H 2 ...H n (1.5)
⎡ a 11 a 12 a 13 x (P0 ) ⎤
⎢ ⎥
a a 22 a 23 y (P0 ) ⎥
D n = ⎢ 21 (1.6)
⎢a 31 a 32 a 33 z (P0 ) ⎥
⎢ ⎥
⎣0 0 0 1 ⎦
Ma trận D n cho ta biết vị trí của điểm P và hướng của khâu thao tác (bàn
kẹp) của rôbốt đối với hệ quy chiếu cố định R 0 .
Như vậy, khi biết được các đặc tính hình học của các khâu và các quy
luật chuyển động của các khớp là ta hoàn toàn có thể xác định được vị trí và
hướng của khâu thao tác (bàn kẹp).
14
1.1.3 Bài toán áp dụng

Ví dụ 1.1. Xác định vị trí của điểm thao tác và hướng của bàn kẹp của
rôbốt 5 khâu động (hình 1.3).
x2 O3 x3 y5
O2 O4
x1 q4 O5
q3
q5
O1 x5
q2
x4
y0
y1
q1
O0
x0
Hình 1.3. Rôbốt 5 khâu động

Ký hiệu O0O1 = a1; O1O2 = a2; O2O3 = a3; O3O4 = a4; O4O5 = a5.
Ta có bảng tham số DH của rôbốt như sau:
Bảng 1.1 Thông số DH rôbốt 5 khâu động
Khâu θ d a α
1 q1 0 a1 0
2 q2 0 a2 0
3 q3 0 a3 0
4 q4 0 a4 0
5 q5 0 a5 0
Qua tính toán ta thu được các kết quả như sau:
⎡C 1 − S1 0 a 1 C1 ⎤ ⎡C 2 − S2 0 a 2C 2 ⎤
⎢S C1 0 a 1S1 ⎥ ⎢S C2 0 a 2S 2 ⎥
H1 = ⎢ 1 ⎥; H2 = ⎢ 2 ⎥;
⎢0 0 1 0 ⎥ ⎢ 0 0 1 0 ⎥
⎢ ⎥ ⎢ ⎥
⎣0 0 0 1 ⎦ ⎣ 0 0 0 1 ⎦
15
⎡C 3 − S3 0 a 3C3 ⎤ ⎡C 4 − S4 0 a 4C4 ⎤
⎢S C3 0 a 3S 3 ⎥ ⎢S C4 0 a 4S4 ⎥
H3 = ⎢ 3 ⎥ ; H4 = ⎢ 4 ⎥;
⎢0 0 1 0 ⎥ ⎢0 0 1 0 ⎥
⎢ ⎥ ⎢ ⎥
⎣0 0 0 1 ⎦ ⎣0 0 0 1 ⎦
⎡C 5 − S5 0 a 5C5 ⎤
⎢S C5 0 a 5S5 ⎥
H5 = ⎢ 5 ⎥.
⎢0 0 1 0 ⎥
⎢ ⎥
⎣0 0 0 1 ⎦
Trong đó ta sử dụng các ký hiệu viết tắt: Si = sin(qi), Ci = cos(qi).
Và ta có ma trận chuyển đổi là:
D 4 = H1H 2 H 3 H 4 H 5 =
⎡C12345 − S12345 0 a 1C1 + a 2 C12 + a 3 C123 + a 4 C1234 + a 5 C12345 ⎤
⎢S C12345 0 a 1S1 + a 2S12 + a 3S123 + a 4S1234 + a 5S12345 ⎥ .
= ⎢ 12345 ⎥
⎢ 0 0 1 0 ⎥
⎢ ⎥
⎣ 0 0 0 1 ⎦
với S12…i = sin(q1+q2+…qi); C12…i = cos(q1+q2+…+qi).
Như vậy tọa độ của điểm thao tác P trong hệ tọa độ cố định là
xP = a1cos(q1)+a2cos(q1+q2)+a3cos(q1+q2+q3)+a4cos(q1+q2+q3+q4)+
+ a5cos(q1+q2+q3+q4+q5).
yP = a1sin(q1)+a2sin(q1+q2)+a3sin(q1+q2+q3)+a4sin(q1+q2+q3+q4)+
+a5sin(q1+q2+q3+q4+q5).
zP= 0.
Ma trận cosin chỉ hướng của bàn kẹp đối với hệ tọa độ cố định là
⎡C12345 − S12345 0⎤
A = ⎢ S12345 C12345 0⎥ .
⎢ ⎥
⎢⎣ 0 0 1⎥⎦
Ta nhận thấy khi m < n thì phương trình (1.1) sẽ là một hệ m phương
trình đại số phi tuyến của n ẩn. Đạo hàm biểu thức (1.1) theo thời gian, ta được
x& = J (q)q&
16
Trong đó ma trận Jacobi J(q) là một ma trận chữ nhật. Vì vậy, khi giải
bài toán động học ngược ta cần phải xử lý các ma trận tựa nghịch đảo. Dưới đây
trình bầy một số khái niệm về ma trận tựa nghịch đảo [41].
1.2 Ma trận tựa nghịch đảo
1.2.1 Định nghĩa
Cho một ma trận chữ nhật A ∈ R . Ma trận X ∈ R nxm sẽ được gọi là ma
mxn
trận tựa nghịch đảo (ma trận nghịch đảo suy rộng) của ma trận A khi và chỉ khi
bốn phương trình điều kiện sau đây được thỏa mãn [41, 45]:
AXA = A (1.7)
XAX = X (1.8)
(AX)T = AX (1.9)
(XA)T = XA (1.10)
1.2.2 Ma trận tựa nghịch đảo Moore - Penrose
Tựa nghịch đảo của ma trận thường dùng là tựa nghịch đảo Moore-
Penrose (MP), được ký hiệu là A+ [62].
Với A là ma trận cỡ mxn, nếu A có hạng đầy đủ, thì tựa nghịch đảo A +
của ma trận A được xác định như sau [41]:
⎧A T ( AA T ) −1 m<n
⎪
A + = ⎨ A −1 m=n (1.11)
⎪( A T A) −1 A T m>n
⎩
Chứng minh. Để chứng minh ma trận nêu trên là tựa nghịch đảo, ta phải
kiểm tra xem nó có thỏa mãn 4 điều kiện từ (1.7) đến (1.10) ở trên hay không.
a. Trường hợp m < n:
Vì A có hạng đầy đủ, nên ma trận (AA)Tmxm là khả đảo.
Khi thay lần lượt A + = A T ( AA T ) −1 vào vế trái của các phương trình điều
kiện, ta sẽ được:
AA + A = A[A T ( AA T ) −1 ]A = AA T ( AA T ) −1 A = A
A + AA + = [A T ( AA T ) −1 ]A[A T ( AA T ) −1 ] = [A T ( AA T ) −1 ][AA T ( AA T ) −1 ]
= A T ( AA T ) −1 = A +
( AA + ) T = [AA T ( AA T ) −1 ] = I T = I = AA T ( AA T ) −1 = AA +
T
17
( A + A) T = [A T ( AA T ) −1 A ] = A T [( AA T ) −1 ] A = A T [( AA T ) T ] A =
T T −1
= A T [AA T ] A = A + A
−1
b. Trường hợp m = n:
Ma trận A có hạng đầy đủ, nên nó là ma trận vuông không suy biến và
A = A −1 , ta có AA + = A + A = I , thay vào 4 phương trình điều kiện trên, ta dễ
+
dàng nhận thấy rằng chúng được thỏa mãn.

c. Trường hợp m > n:
Vì A có hạng đầy đủ, nên rank(A) = n, ma trận (AA)nxn là khả đảo.
Thay A + = ( A T A) −1 A T lần lượt vào vế trái của 4 phương trình điều kiện,
ta được:
AA + A = A[(A T A) −1 A T ]A = A[A −1 (A T ) −1 A T ]A = AA −1 (A T ) A T A = A
−1
A + AA + = A + A[( A T A) A T )] = A + A[A −1 ( A T ) −1 A T ] = A + [AA −1 ( A T ) −1 ] = A +
( AA + ) T = [A(A T A) −1 A T ] = A[A −1 (A T ) −1 ] A T = A[A −1 (A −1 )] A T =

T T T
A[A −1 (A −1 ) T ]A T = A[A −1 (A T )]A T = A[(A T A) −1 ] = A (A T A )

T
[ −1 T
]A T
=
( A + A ) T = [( A T A ) −1 A T A ] = A T [( AA T ) −1 ] A = A T [( AA T ) T ] A =
T T −1
= A T [AA T ] A = A + A
−1
Như vậy ta có sơ đồ khối tính toán tựa nghịch đảo của một ma trận theo
Moore – Penrose :
A m×n
m<n m>n
So sánh m, n
m=n
A + = A T ( AA T ) −1 A + = A −1 A + = ( A T A ) −1 A T
A+
Hình 1.4. Sơ đồ khối tính toán A+ theo phương pháp Moore-Penrose

18
1.2.3 Phương pháp nhân tử Lagrange

Xét phương trình đại số
Ax = y (1.12)
Trong đó A ∈ R m×n ; các véc tơ: x ∈ R n , y ∈ R m với m < n.
Ta sẽ xác định nghiệm của phương trình (1.12), với điều kiện nghiệm này
sẽ làm cực tiểu hóa một hàm mục tiêu nào đó. Ở đây ta sẽ chọn hàm mục tiêu có
dạng toàn phương của biến x:
1
g(x) = x T Wx (1.13)
2
Với Wnxn là ma trận trọng số đối xứng, xác định dương và được lựa chọn
một cách thích hợp. Sử dụng phương pháp nhân tử Lagrange, ta xét hàm mục
tiêu mới có dạng:
1
g * (x, λ ) = x T Wx + λ T (y − Ax) (1.14)
2
trong đó λ ∈ R m được gọi là nhân tử Lagrange. Để g * (x, λ ) đạt cực tiểu, điều
kiện cần phải có là:
T
⎛ ∂g * ⎞
⎜ ⎟ = Wx − A T λ = 0 (1.15)
⎝ ∂x ⎠
T
⎛ ∂g * ⎞
⎜⎜ ⎟⎟ = − Ax + y = 0 (1.16)
⎝ ∂λ ⎠
Từ (1.15) ta suy ra :
x = W −1 A T λ (1.17)
Thế (1.17) vào (1.16), ta được:
− AW −1 A T λ + y = 0 ⇒ λ = ( AW −1 A T ) −1 y
Thế biểu thức của λ vào (1.17), ta tìm được nghiệm của phương trình thỏa
mãn điều kiện cực tiểu hàm mục tiêu (1.13)
x = W −1 A T ( AW −1 A T ) −1 y (1.18)
Có thể thấy, trong trường hợp W = In (ma trận đơn vị cấp n), nghiệm thu
được của phương pháp nhân tử Lagrange cũng chính là nghiệm thu được theo
phương pháp Moore-Penrose.
19
Ta cũng thấy, nghiệm theo phương pháp Moore-Penrose là nghiệm ứng

2
1 T 1 T 1 T x
với hàm mục tiêu g(x) = x Wx = x I n x = x x = cực tiểu, tương ứng
2 2 2 2
với nghiệm có chuẩn nhỏ nhất trong tập nghiệm của (1.12)
1.2.4 Nghiệm tổng quát của phương trình đại số tuyến tính
Xét phương trình đại số
Ax = y (1.19)
Trong đó A ∈ R m×n , x ∈ R n , y ∈ R m với m < n. Ta sẽ chứng minh rằng
nghiệm có dạng:
x = A + y + (I n − A + A)z (1.20)
Với In là ma trận đơn vị cấp n, z là một véc tơ tùy ý thuộc Rn, A + là ma
trận tựa nghịch đảo của A.
Thật vậy, khi thay (1.20) vào vế trái của (1.19), ta được:
Ax = A[A + y + (I n − A + A)z ] = AA + y + A (I n − A + A)z =
= AW −1 A T ( AW −1 A T ) −1 y + ( A − AA + A )z = y + 0.z = y
Ngược lại, ta gọi x là 1 nghiệm trong tập nghiệm của (1.19), nghĩa là
Ax=y, biểu diễn x dưới dạng
x = A + Ax + (I n − A + A)x = A + y + (I n − A + A)x (1.21)
Điều này chứng tỏ (1.20) là nghiệm tổng quát hơn nghiệm (1.21) mà vẫn
thỏa mãn (1.19). Tuy nhiên mọi nghiệm của (1.19) đều có thể biểu diễn dưới
dạng (1.21), do đó (1.20) chính là nghiệm tổng quát của phương trình (1.19) đã
cho.
Một cách tiếp cận khác để giải phương trình (1.19) là áp dụng phương
pháp nhân tử Lagrange. Ta xét hàm mục tiêu dạng toàn phương:
1
g ( x) = ( x − z ) T W ( x − z ) (1.22)
2
Với Wnxn là ma trận trọng số đối xứng xác định dương được lựa chọn một
cách thích hợp. Xét hàm mục tiêu mới:
1
g * (x, λ ) = (x − z ) T W (x − z ) + λ T (y − Ax) (1.23)
2
Để hàm g * (x, λ ) đạt cực tiểu
20
T
⎛ ∂g * ⎞
⎜⎜ ⎟⎟ = W (x − z ) − A T λ = 0 (1.24)
⎝ ∂x ⎠
T
⎛ ∂g * ⎞
⎜ ⎟ = − Ax + y = 0 (1.25)
⎝ ∂λ ⎠
Từ (1.24), ta suy ra
x = W −1 A T λ + z (1.26)
Thay (1.26) vào (1.25), ta được
− AW −1 A T λ − Az + y = 0 ⇒ λ = ( AW -1 A T ) -1 (y - Az)
Thay biểu thức nhận được vào (1.26) ta tìm được nghiệm của phương
trình thỏa mãn điều kiện cực tiểu của hàm mục tiêu (1.22)
x = W−1AT (AW−1AT )−1 y − W−1AT (AW−1AT )−1 Az+ z =
(1.27)
W−1AT (AW−1AT )−1 y + [In − W−1AT (AW−1AT )−1 A]z = A+y + (In − A+ A)z
Có thể thấy, nghiệm (1.27) thu được gồm 2 thành phần: thành phần thứ
nhất là thành phần có chuẩn nhỏ nhất, còn thành phần thứ hai thỏa mãn tính chất
A (I n − A + A )z = ( A − AA + A)z = 0 đó chính là thành phần của véc tơ z chiếu
lên không gian bù (null space) của A.
Thực tế, trong bài toán động học ngược rôbốt dư dẫn động ta thường sử
dụng biểu thức nghiệm theo phương pháp Lagrange với ma trận trọng số W = In
(chính là ma trận tựa nghịch đảo Moore-Penrose )
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
1.3.1 Phương pháp khai triển Taylor
Một số tác giả đã quan tâm đến việc sử dụng phương pháp số giải bài
toán động học ngược của rôbốt [48, 52, 54]. Trong các cuốn sách [48, 52] đã
trình bầy một thuật toán số như sau:
q(t k +1 ) = q(t k ) + q& (t k )Δt (1.28)
Trong đó q& (t k ) được xác định từ công thức
q& (t k ) = J −1 (q& (t k ))x& (t k ) (1.29)
Thế (1.29) vào (1.28) ta được
q(t k +1 ) = q(t k ) + J −1 (q& (t k ))x& (t k )Δt (1.30)
21
Việc tính toán theo công thức (1.30) cho kết quả khá thô (điều này sẽ
được thấy rõ trong ví dụ ở phần 1.4). Trong luận án này chúng tôi tìm cách cải
tiến công thức (1.30) để nhận được một thuật toán mới có độ chính xác cao hơn.
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
Ký hiệu q = [q1 , q 2 ,..., q n ] là véc tơ tọa độ suy rộng (véc tơ mô tả cấu
T
hình của rôbốt), x = [x 1 , x 2 ,..., x m ] là véc tơ xác định vị trí và hướng của bàn
T
kẹp trong hệ quy chiếu cố định. Từ bài toán động học thuận ta có hệ thức
x = f (q) (1.31)
Trong điều khiển rôbốt, chúng ta thường gặp yêu cầu của bài toán công
nghệ là phải điều khiển bàn kẹp chuyển động theo 1 quỹ đạo mong muốn cho
trước nào đó, nghĩa là chúng ta phải xác định các giá trị của q để tạo ra giá trị
mong muốn cho x. Như vậy ở bài toán động học ngược chúng ta phải thiết lập
được quan hệ:
q = f −1 ( x ) (1.32)
Trước hết đạo hàm 2 vế của (1.31) theo thời gian, ta được :
∂f
x& = q& = J (q)q& (1.33)
∂q
Trong đó J(q) là ma trận Jacobi cỡ mxn, với :
⎡ ∂f 1 ∂f 1 ∂f 1 ⎤
⎢ ∂q ...
∂q 2 ∂q n ⎥
∂f ⎢ 1 ⎥
J (q) = = ... ... ... ... ⎥ (1.34)
∂q ⎢ ∂f m ∂f m ∂f m ⎥
⎢ ...
⎢ ∂q 1 ∂q 2 ∂q n ⎥⎦
⎣
Giả sử hạng của J(q) là m, theo [41, 57] ta chọn ma trận tựa nghịch đảo
của ma trận chữ nhật J(q) dưới dạng
J + (q) = J T (q)[J (q)J T (q)] ,
−1
m<n (1.35)
Khi đó từ biểu thức (1.33) ta suy ra công thức tính véc tơ vận tốc suy
rộng:
q& ( t ) = J + (q( t ) )x& ( t ) (1.36)
Như thế nếu biết được x(t), ta tìm được x& (t ) , biết được hàm f(q(t)) ta tìm
được J + (q( t ) ) . Theo (1.36) ta tính được q& ( t ) . Vấn đề ở đây là tìm cách xác
định q(t). Vấn đề này sẽ được trình bày trong mục 1.3.3.
22
Đạo hàm 2 vế của (1.36) theo thời gian, ta được công thức xác định véc
tơ gia tốc suy rộng
&&( t ) = J + (q( t ) )&x&( t ) + J& + (q( t ) )x& ( t )
q (1.37)
Để có thể áp dụng được công thức (1.37) ta cần phải tính được J& + (q( t ) ) .
Theo lý thuyết ta có thể đạo hàm ma trận J + (q( t ) ) theo thời gian. Tuy nhiên rất
khó xác định biểu thức giải tích của ma trận này, do đó ta phải tìm một thuật
toán xác định J& + (q( t ) ) . Từ biểu thức (1.35) ta suy ra :
J + (q( t ) )J (q( t ) )J T (q( t ) ) = J T (q( t ) ) (1.38)
Đạo hàm 2 vế của (1.38) theo thời gian, ta được
J& + (q )J (q )J T (q ) + J + (q )[J& (q )J T (q ) + J (q )J& T (q )] = J& T (q ) (1.39)
Từ (1.39) ta suy ra
J& + (q ) = {J& T (q ) − J + (q )[J& (q )J T (q ) + J (q )J& T (q )]}[J (q )J T (q )]
−1
(1.40)
Ma trận J& (q ) được tính bằng cách đạo hàm trực tiếp các phần tử của ma
trận J (q ) theo thời gian. Thế biểu thức (1.40) vào (1.37) ta tìm được gia tốc
q
&&( t ) .
1.3.3 Các công thức xác định véc tơ tọa độ suy rộng
Sử dụng các công thức (1.36), (1.37) và (1.39) ta sẽ xác định được q& và
&& , nếu như đã xác định được q( t ) và x(t) tại thời điểm khảo sát. Như vậy, để
q
xác định q& và q&& trước hết ta cần xác định q(t). Thuật toán xác định q(t) như
sau:
Chia khoảng thời gian làm việc của rôbốt [0, T] thành N khoảng bằng
nhau
T
Δt = , ta có t k +1 = t k + Δt với k = 0, 1, …, N-1
N
Áp dung khai triển Taylor đối với qk+1 quanh giá trị qk , ta được
1
q k +1 = q( t k + Δt ) = q k + q& k Δt + q&& k (Δt ) 2 + ... (1.41)
2
Thế biểu thức (1.36) vào (1.41) và bỏ qua các vô cùng bé bậc ≥ 2 ta được:
q k +1 = q k + J + (q k )x& k Δt với k = 0, 1, …, N-1
(1.42)
23
Từ đó, ta có các bước tính toán như sau:

1. Tìm q 0 .
2. Tính J (q 0 ), J + (q 0 ), J& (q 0 ) .
&&(t = 0) = q
3. Tính q& ( t = 0) = q& 0 theo (1.36) và tính q && 0 theo (1.37).
4. Tính q k +1 theo (1.40), rồi tính q& k +1 , q
&& k +1 theo (1.36) và (1.37).
Ta thấy việc tính q k+1 theo (1.42) là khá thô. Vì vậy để nâng cao độ chính
xác, ta cần có một thuật toán xác định q k +1 chính xác hơn như dưới đây.
a. Hiệu chỉnh gia lượng véc tơ tọa độ suy rộng tại thời điểm t0
~
Giả sử t 0 = 0 là thời điểm đầu. Ta có thể xác định giá trị gần đúng q 0
của q 0 bằng cách vẽ (hoặc bằng thực nghiệm). Sau đó áp dụng khai triển Taylor
để tìm gần đúng tốt hơn của q 0 như sau:
Giả sử lấy q = q~ + Δq
0 0 0
Theo phương trình (1.31), ta có :
x 0 = f (q 0 ) = f (q ~ ) + ∂f (q
~ + Δq ) = f (q ~ )Δq + ...
∂q
0 0 0 0 0
Từ đó suy ra
~ )Δq ≈ x − f (q
J (q ~ ) (1.43)
0 0 0 0
Giải phương trình đại số tuyến tính (1.43) với ẩn Δq 0 , ta được

~ )[x − f (q
Δq 0 = J + (q ~ )] .
0 0 0
~ bởi q
Nếu Δq 0 ≥ ε thì ta tính lại giá trị mới của q ~ * := q
~ + Δq rồi thế
0 0 0 0
vào (1.43) và lại giải phương trình này. Quá trình lặp được dừng lại khi
Δq 0 < ε .
Và khi đó ta gán giá trị tìm được là q0
q =q ~* (1.44)
0 0
Biết được nghiệm q 0 của phương trình (1.29) tại thời điểm t 0 = 0 . Thay
vào (1.36), (1.37) ta tìm được q& (0) = q& 0 và q
&&(0) = q
&& 0 .
Giá trị sai số cho phép ε được xác định theo yêu cầu về độ chính xác của
bài toán.
24
b. Hiệu chỉnh gia lượng véc tơ tọa độ suy rộng tại thời điểm tk+1
~ theo công thức (1.42)
Trước tiên ta xác định giá trị gần đúng q k +1
~ = q + J + (q )x& Δt
q (1.45)
k +1 k k k
Sau đó ta tìm biểu thức gần đúng tốt hơn của q k+1
~ + Δq
q k +1 = q (1.46)
k +1 k +1
Ta cần xác định Δq k+1
Thế (1.46) vào phương trình (1.31), ta được :
x k +1 = f (q k +1 ) = f (q ~ ) + ∂f (q
~ + Δq ) = f (q ~ )Δq + ...
k +1 k +1 k +1 k +1 k +1
∂q
Từ đó suy ra
J (q
~ )Δq
k +1 k +1
≈ x k +1 − f (q
~ )
k +1
(1.47)
Chú ý rằng J (q
~ ) là ma trận cỡ mxn, Δq là véc tơ có n phần tử. Do đó
k +1 k+1
số ẩn lớn hơn số phương trình (m<n), nghiệm có chuẩn nhỏ nhất của phương
trình (1.47) có dạng :
Δq = J + (q ~ )[x − f (q
~ )] (1.48)
k +1 k +1 k +1 k +1
Sau đó lấy
~ =q
q ~ + Δq k +1 (1.49)
k +1 k +1
Nếu Δq k +1 ≥ ε thì thay (1.47) vào (1.48) và tiếp tục giải phương trình
(1.47). Nếu Δq k +1 < ε thì ta lấy
~
q k +1 = q (1.50)
k +1
Biết được q k +1 , ta tính q& k +1 theo công thức (1.36) và q

&& k+1 theo công thức
(1.37).
Trên hình 1.5 trình bày sơ đồ khối 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.
25
∂f
Cho x = f (q) , x = x( t ) , J (q ) = (q) , t0, q 0 , N, T
∂q
h = ∆t = T/N
k: = 0
~ := q
t k := t 0 , q k 0
~ ) , x = x( t ) , f = f (q
Tính J + (q ~ )
k k k k k
~ )(x − f )
Δq k = J + (q k k k
~ ~
q := q + Δq
k k k
~ := q
q ~ + J + (q )x& Δt Sai
k k −1 k −1 k −1 Δq k < ε
Đúng
Xuất kết qủa qk
Sai
k := k + 1 k≥N
Đúng
KẾT THÚC
Hình 1.5. Sơ đồ khối giải bài toán động học ngược

26
1.3.4 Đánh giá sai số

Để đánh giá sai số của phương pháp ta đưa vào các công thức xác định
sai số của dịch chuyển, vận tốc và gia tốc
e( t k ) = x( t k ) − x k = x( t k ) − f (q k )
e& ( t k ) = x& ( t k ) − x& k = x& ( t k ) − J (q k )q& k (1.51)
&e&( t k ) = &x&( t k ) − &x& k = &x&( t k ) − J& (q k )q& k − J (q k )q
&& k
Trong đó e( t k ) = [e1 ( t k ) e 2 ( t k ) L e m ( t k )] .
T
Độ lớn của các chuẩn của các véc tơ e( t k ), e& ( t k ), &e&( t k ) cho biết độ chính
xác của phương pháp hiệu chỉnh gia lượng véc tơ tọa độ suy rộng.
Trong chương trình tính toán ta sẽ dùng chuẩn Euclid
e( t k ) = e12 ( t k ) + e 22 ( t k ) + L + e 2m ( t k )
Để thấy rõ độ chính xác của phương pháp số đề nghị, ta xét các ví dụ sau.
Trong các ví dụ ấy đặc biệt cần chú ý đến các đồ thị đánh giá sai số. Đó là các
kết quả nghiên cứu mới của chúng tôi.
1.4 Các bài toán áp dụng

Ví dụ 1.2. Xác định quy luật chuyển động của các khâu động của rôbốt
phẳng 5 khâu động có kết cấu như hình vẽ 1.6.
x2
q3 x3
x1 q4
y ϕ
q2
q5 x4
q1 E
x5
O
x
Hình 1.6. Rôbốt phẳng 5 khâu động

Cho biết chiều dài của các khâu là:
a1 = 0.55 (m); a2 = 0.50 (m); a3 = 0.45 (m); a4 = 0.40 (m); a5 = 0.20 (m).
Phương trình chuyển động của điểm E là:
xE = 0.8+0.1cos(2t) (m); yE = - 0.8+0.1sin(2t) (m)
27
Bàn kẹp của rôbốt phải luôn tạo với phương thẳng đứng 1 góc ϕ=1(rad).
Ở trường hợp này, do có 5 tọa độ suy rộng còn số tọa độ để xác định vị trí
và hướng của bàn kẹp là 3 cho nên đây là rôbốt dư dẫn động (dư 2 tọa độ suy
rộng)
Từ sơ đồ kết cấu ta có :
xp = a5*S12345 + a4*S1234 +a3*S123 + a2*S12 +a1*S1
yp = a5*C12345 + a4*C1234 + a3*C123 + a2*C12 + a1*C1
Do đó ma trận Jacobi là:
J11 = a5C12345+a4C1234+a3C123+a2C12+a1C1
J12 = a5C12345+a4C1234+a3C123+a2C1
J13 = a5C12345+a4C1234+a3C123
J14 = a5C12345+a4C1234
J15 = a5C12345
J21 = -a5S12345-a4S1234-a3S123-a2S12-a1S1
J22 = -a5S12345-a4S1234-a3S123-a2S12
J23 = -a5S12345-a4S1234-a3S123
J24 = -a5S12345-a4S1234
J25 = -a5S12345
Sử dụng chương trình giải bài toán động học ngược cho trường hợp có sử
dụng thuật toán hiệu chỉnh gia lượng và không sử dụng thuật toán hiệu chỉnh gia
lượng với các bước thời gian tính toán khác nhau, ta thu được các đồ thị góc
quay, vận tốc góc và gia tốc góc của các khớp động cùng các sai số vị trí do các
phương pháp tính đưa lại như sau:
a. Chọn bước thời gian tính toán h = Δt = 0.001 (s)
• Khi không sử dụng thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy
rộng
4
q1 [rad]
3 q’1 [1/s]
q” 1 [1/s 2]
2
-1
-2
0 1 2 3 4 5 6 7 8 9 10
time [s]
Hình 1.7. Các đặc tính chuyển động của khâu 1

28
1.5
0.5
-0.5
-1 q2 [rad]
q’2 [1/s]
-1.5
q” 2 [1/s 2]
-2
0 1 2 3 4 5 6 7 8 9 10
time [s]
2
q3 [rad]
1 q’3 [1/s]
q”3 [1/s 2]
0
-1
-2
-3
0 1 2 3 4 5 6 7 8 9 10
time [s]
5
q [rad]
4 4
q’ [1/s]
4
3 2]
q”4 [1/s
2
-1
-2
-3
0 1 2 3 4 5 6 7 8 9 10
time [s]

29
4
q [rad]
5
2
q’ [1/s]
5
0 q” [1/s 2]
5
-2
-4
-6
-8
-10
0 1 2 3 4 5 6 7 8 9 10
time [s]

-4
x 10
2
-2
-4
ex [m]
-6
-8
-10
-12
-14
0 1 2 3 4 5 6 7 8 9 10
time [s]
Hình 1.12. Sai số vị trí của điểm thao tác theo trục x
-4
x 10
4
-2
ey [m]
-4
-6
-8
-10
-12
0 1 2 3 4 5 6 7 8 9 10
time [s]
Hình 1.13. Sai số vị trí của điểm thao tác theo trục y
30
-15
x 10
8
eφ [rad]
2
-2
-4
0 1 2 3 4 5 6 7 8 9 10
time [s]
Hình 1.14. Sai số góc định hướng của bàn kẹp
0.8
0.6
y [m]
0.4
0.2
-0.4 -0.2 0 0.2 0.4 0.6 0.8 1

x [m]
Hình 1.15. Dạng chuyển động của rôbốt theo kết quả tính toán
• Khi có sử dụng thuật toán hiệu chỉnh gia lượng với sai số cho phép
ε = 10-6 (rad), kết quả nhận được là :
4
q [rad]
1
3 q’ [1/s]
1
2 q” [1/s 2]
1
-1
-2
0 1 2 3 4 5 6 7 8 9 10
time [s]

31
1.5
0.5
-0.5
q2 [rad]
-1
q’2 [1/s]
-1.5
q”2 [1/s2]
-2
0 1 2 3 4 5 6 7 8 9 10
time [s]

2
-1
q3 [rad]
-2 q’3 [1/s]
q”3 [1/s 2]
-3
0 1 2 3 4 5 6 7 8 9 10
time [s]
6
q4 [rad]
4 q’4 [1/s]
q”4 [1/s 2]
2
-2
-4
0 1 2 3 4 5 6 7 8 9 10
time [s]

32
-2
-4
q5 [rad]
-6
q’5 [1/s]
-8
q”5 [1/s2]
-10
0 1 2 3 4 5 6 7 8 9 10
time [s]

-16
x 10
6
2
ex [m]
-2
-4
-6
0 1 2 3 4 5 6 7 8 9 10
time [s]
Hình 1.21. Sai số theo trục x của điểm thao tác

-15
x 10
3
2
ey [m]
-1
0 1 2 3 4 5 6 7 8 9 10
time [s]
Hình 1.22. Sai số theo trục y của điểm thao tác

33
-15
x 10
2.5
1.5
1
eφ [rad]
0.5
-0.5
-1
0 1 2 3 4 5 6 7 8 9 10
time [s]
0.8
0.6
y [m]
0.4
0.2
-0.4 -0.2 0 0.2 0.4 0.6 0.8 1

x [m]
Qua các hình 1.12, 1.13, 1.14, 1.21, 1.22, 1.23 ta nhận thấy ở bước thời
gian tính toán h = 0.001(s) thì mức độ chính xác của phương pháp tính khi có sử
dụng thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng đạt được là 10-15 -
10-16 (hình 1.21 – 1.23) cao hơn nhiều so với khi không sử dụng thuật toán hiệu
chỉnh gia lượng véc tơ tọa độ suy rộng (chỉ đạt tới 10-4, hình 1.12, 1.13).
34
b. Chọn bước thời gian tính toán h = Δt = 0.01 (s)

• Trường hợp tính toán không sử dụng thuật toán hiệu chỉnh gia lượng
véc tơ tọa độ suy rộng ta được:
0.005
-0.005
ex [m]
-0.01
-0.015
-0.02
-0.025
0 2 4 6 8 10 12 14 16 18 20
time [s]

-3
x 10
5
-5
ey [m]
-10
-15
-20
0 2 4 6 8 10 12 14 16 18 20
time [s]

-15
x 10
4
2
eφ [rad]
-2
-4
0 2 4 6 8 10 12 14 16 18 20
time [s]

35
0.8
0.6
y [m] 0.4
0.2
-0.4 -0.2 0 0.2 0.4 0.6 0.8 1

x [m]
• Khi có sử dụng thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng
-16
x 10
6
2
ex [m]
-2
-4
-6
0 2 4 6 8 10 12 14 16 18 20
time [s]
-16
x 10
6
2
ey [m]
-2
-4
-6
0 2 4 6 8 10 12 14 16 18 20
time [s]
36
-15
x 10
3
eφ [rad]
1
-1
0 2 4 6 8 10 12 14 16 18 20
time [s]
0.8
0.6
y [m]
0.4
0.2
-0.4 -0.2 0 0.2 0.4 0.6 0.8 1

x [m]
Như vậy khi tăng Δt = h lên (giảm số điểm tính toán xuống) ta thấy khi sử
dụng thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng thì độ chính xác
tính toán vẫn đạt khá cao (sai số 10-15 – 10-16, hình 1.29 – 1.31), vì vậy mà đảm
bảo được sự chính xác về quỹ đạo mong muốn cho điểm thao tác E (hình 1.32).
Còn khi không sử dụng thuật toán hiệu chỉnh, độ chính xác chỉ đạt tới 10-3 (hình
1.25, 1.26), vì vậy mà sai số tích lũy lớn và quỹ đạo chuyển động của điểm E sẽ
không được chính xác. Trên hình 1.28 ta thấy khi không sử dụng thuật toán hiệu
chỉnh gia lượng véc tơ tọa độ suy rộng, sau một thời gian tính toán, do sai số
tích lũy mà điểm E của khâu thao tác không vạch nên 1 đường tròn như nhiệm
37
vụ công nghệ đặt ra. Trên hình 1.32 do có sử dụng thuật toán hiệu chỉnh véc tơ
tọa độ suy rộng nên điểm E vẫn luôn chuyển động trên quỹ đạo chương trình.
c. Với bước thời gian tính toán h = Δt = 0.05 (s)
• Khi không sử dụng thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy
rộng
0.05
0
ex [m]
-0.05
-0.1
-0.15
0 2 4 6 8 10 12 14 16 18 20
time [s]
0.05
0
ey [m]
-0.05
-0.1
-0.15
0 2 4 6 8 10 12 14 16 18 20
time [s]
-15
x 10
2
1
eφ [rad]
-1
-2
0 2 4 6 8 10 12 14 16 18 20
time [s]
38
0.8
0.6
y [m]
0.4
0.2
-0.5 0 0.5 1
x [m]
• Khi có sử dụng thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng
ta nhận được các kết quả như sau :
-16
x 10
4
2
ex [m]
-2
-4
0 1 2 3 4 5 6 7 8 9 10
time [s]

-16
x 10
6
2
ey [m]
-2
-4
-6
0 1 2 3 4 5 6 7 8 9 10
time [s]

39
-15
x 10
3
eφ [rad]
1
-1
0 1 2 3 4 5 6 7 8 9 10
time [s]
0.8
0.6
y [m]
0.4
0.2
-0.4 -0.2 0 0.2 0.4 0.6 0.8 1

x [m]
Qua các kết quả tính toán có được ở các trường hợp của ví dụ 1.2 ta nhận
thấy khi dùng thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng để giải bài
toán động học ngược thì mức độ chính xác của các kết quả tính toán sẽ cao hơn
nhiều so với khi không sử dụng thuật toán (10-14, 10-15, 10-16 so với 10-4, 10-3,
10-2) xem các hình 1.12, 1.13, 1.14, 1.21, 1.22, 1.23, 1.25, 1.26, 1.27, 1.29, 1.30,
1.31, 1.33, 1.34, 1.35, 1.37, 1.38, 1.39). Đặc biệt khi sử dụng thuật toán hiệu
chỉnh gia lượng véc tơ tọa độ suy rộng thì chúng ta có thể giảm bớt khối lượng
tính toán xuống (do có thể tăng bước thời gian tính toán lên) còn ở phương pháp
tính toán mà không sử dụng thuật toán thì không thể được (xem các hình 1.28,
1.32, 1.36, 1.40).
40
Ví dụ 1.3. Xác định quy luật chuyển động của các khớp động thuộc rôbốt
6 khâu động có sơ đồ kết cấu như hình vẽ 1.41.
O3 x3
x2
q4 O4
O2
q3
y6
x4
q5 O6
O5
x1 β
y0
O1 q2 q6
x6
x5
y1 O0
q1
x0

Biết :
• Kích thước của các khâu là:
a1 = a2 = 0.30m; a3 = a4 =a5 = 0.40m; a6 = 0.25m
• Phương trình chuyển động của điểm thao tác là:
xp = 0.8+0.2cos(2t); yp = -0.8+0.2sin(2t) ;
• Khâu thao tác luoon tạo với phương nằm ngang 1góc cố định β =900,
Từ sơ đồ kết cấu của rôbốt ta có:
xp = a6*C123456 + a5*C12345 + a4*C1234 +a3*C123 + a2*C12 +a1*C1
yp = a6*S123456 + a5*S12345 + a4*S1234 + a3*S123 + a2*S12 + a1*S1
Do đó ma trận Jacobi là:
J11 = -a6S123456-a5S12345-a4S1234-a3S123-a2S12-a1S1
J12 = -a6S123456-a5S12345-a4S1234-a3S123-a2S12
J13 = -a6S123456-a5S12345-a4S1234-a3S123
J14 = -a6S123456-a5S12345-a4S1234
J15 = -a6S123456-a5S12345
J16 = -a6S123456
J21 = a6C123456+a5C12345+a4C1234+a3C123+a2C12+a1C1
J22 = a6C123456+a5C12345+a4C1234+a3C123+a2C1
J23 = a6C123456+a5C12345+a4C1234+a3C123
J24 = a6C123456+a5C12345+a4C1234
J25 = a6C123456+a5C12345
J26 = a6C123456
41
Chọn bước chia thời gian h = 0.001(s), chọn sai số ε = 10 −6 (rad). Sử dụng
chương trình giải bài toán động học ngược, ta thu được đồ thị vị trí, vận tốc góc
và gia tốc góc của các khớp và sai số tính toán theo thuật toán đề xuất.
2
Goc quay cua cac khop (rad)

q1
1 q2
q3
0
q4
-1 q5
q6
-2
0 2 4 6 8 10
Thoi gian(s)
Hình 1.42. Đồ thị góc quay của các khớp (rad)

Van toc goc cua cac khop (rad/s)
1
q'1
0.5 q'2
q'3
0
q'4
-0.5 q'5
q'6
-1
0 2 4 6 8 10
Thoi gian(s)
Hình 1.43. Đồ thị vận tốc góc các khớp (rad/s)

2
Gia toc goc cac khop (rad/s 2)
1 q" 1
q" 2
0 q" 3
q" 4
-1
q" 5
q" 6
-2
0 2 4 6 8 10
Thoi gian(s)
Hình 1.44. Đồ thị gia tốc góc các khớp động (rad/s2)
42
Sai so vi tri trong khong gian thao tac (m, rad)

-13
x 10
4
3 Sai so theo truc x

Sai so theo truc y
2 Sai so goc dinh huong
-1
0 2 4 6 8 10
Thoi gian(s)
Hình 1.45. Đồ thị sai số vị trí trong không gian thao tác
Sai so van toc trong khong gian thao tac (m/s)
-15
x 10
2
-1
Sai so truc x
Sai so truc y
-2
0 2 4 6 8 10
Thoi gian(s)
Hình 1.46. Đồ thị sai số vận tốc trong không gian thao tác
Sai so gia toc trong khong gian thao tac (m/s 2)
-13
x 10
4
Sai so truc x
3 Sai so truc y
-1
0 2 4 6 8 10
Thoi gian(s)
Hình 1.47. Đồ thị sai số gia tốc trong không gian thao tác
43
Các hình từ 1.42 đến 1.44 cho ta biết sự thay đổi của vị trí, vận tốc và gia
tốc của các khớp. Đặc biệt là các hình từ 1.45 đến 1.47 chỉ ra sai số của các góc
quay, vận tốc góc và gia tốc góc của các khớp khi tính theo thuật toán hiệu
chỉnh véc tơ tọa độ suy rộng, các sai số này là rất bé: sai số đối với các góc quay
nhỏ hơn 4x10-13 rad, đối với các vận tốc góc nhỏ hơn 2x10-15 rad/s, đối với gia
tốc góc nhỏ hơn 4x10-13 rad/s2.
Qua hai ví dụ 1.2 và 1.3 ta thấy sai số của phương pháp rất bé. Điều đó
chứng tỏ tính ưu việt của thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng.
1.5 Kết luận chương 1
Trong chương một đã 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. Thuật toán này được hoàn thiện dưới sự hướng
dẫn của GS. Nguyễn Văn Khang. Chúng tôi đề nghị gọi tên của phương pháp số
này là thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng.
Theo thuật toán trên, một phần mềm tính toán động học rôbốt dư dẫn
động đã được xây dựng dựa trên phần mềm MATLAB. Các ví dụ được trình
bày trong chương này nhằm minh họa cho thuật toán và độ chính xác của thuật
toán cũng như thể hiện rõ tính ưu việt của phương pháp này so với phương pháp
tính toán không sử dụng thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng.
Đây là các ví dụ mới, chưa có trong các sách chuyên khảo.
Qua các ví dụ ta thấy độ chính xác của thuật toán khá cao. Do đó có thể
sử dụng thuật toán này để tính toán động lực học ngược các cơ cấu rôbốt phức
tạp, bởi vì việc tính toán động lực học các rôbốt phức tạp đòi quá nhiều phép
tính do đó mà sai số của phương pháp có một vai trò hết sức quan trọ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
Trong chương này các phương trình Lagrange loại 2 được sử dụng để thiết
lập các phương trình vi phân chuyển động của rôbốt. Cho biết chuyển động của
khâu thao tác, tìm biểu thức của lực/mômen của các khâu dẫn động là nội dung
chính của bài toán động lực học ngược. Như thế bài toán động lực học ngược rôbốt
phải dựa trên bài toán động học ngược và có liên hệ mật thiết với bài toán điều
khiển rôbốt. Trong chương này dựa trên thuật toán hiệu chỉnh gia lượng véc tơ tọa
độ suy rộng, trình bày một thuật toán giải bài toán động lực học ngược rôbốt dư
dẫn động trong không gian thao tác.
2.1 Dạng thức Lagrange loại 2 của hệ nhiều vật
Xét cơ hệ gồm p vật rắn, chịu r liên kết hôlônôm. Số bậc tự do của cơ hệ là
f = 6p − r . Trong đoạn này dựa trên khái niệm ma trận Jacobi, ta thiết lập dạng
thức Lagrange loại 2 cho hệ nhiều vật
hôlônôm [17, 55]. zi
Ký hiệu các tọa độ suy rộng là
q = [ q 1 ,K , q f ] T (2.1) z0 yi
Ci
Vị trí của mỗi vật rắn Bi ở trong hệ
quy chiếu cố định R 0 = Ox 0 y 0 z 0 được xác Bi
xi
định bởi véc tơ xác định vị trí khối tâm và
ma trận côsin chỉ hướng của vật rắn. R0 y0
rC = rC (q, t ), A i = A i (q, t ) (2.2)
i i x0 Hình 2.1
Để đơn giản, dưới đây ta chỉ xét các
hệ hôlônôm giữ và dừng. Khi đó biểu thức (2.2) có dạng
rC = rC (q), A i = A i (q)
i i
(2.3)
Theo [17] trạng thái vận tốc của vật rắn Bi được xác định bởi vận tốc khối
tâm và vận tốc góc của nó
drC ∂rC ~ =A
& AT ⇒ ω
vC = = i i
q& , ω (2.4)
∂q
i i i i
i
dt
45
Nếu ta đưa vào các ký hiệu ma trận Jacobi tịnh tiến J Ti và ma trận Jacobi
quay J R i [17] theo các công thức
∂rCi ∂ω i
J Ti = , J Ri = (2.5)
∂q ∂q&
thì vận tốc khối tâm vật rắn và vận tốc góc của nó có thể viết dưới dạng sau
v Ci = J Ti (q)q& , ω i = J R i (q)q& (2.6)
Từ biểu thức tính động năng của một vật rắn trong [17, 64, 67], ta suy ra
biểu thức động năng của hệ p vật rắn.
p
1 p 1 p
T = ∑ Ti = ∑ m i v TCi v Ci + ∑ ω iT I i ω i (2.7)
i =1 2 i=1 2 i=1
Trong (2.7) thì Ii là ma trận của tenxơ quán tính của vật rắn thứ i đối với
khối tâm Ci của nó ở trong hệ qui chiếu cố định R0.
Chú ý. Trong tính toán thực hành có khi người ta thay việc tính biểu thức
p
∑ω
i =1
T
i
I i ω i bằng biểu thức ∑ω (i )T
i I i( i ) ω i( i ) . Trong đó I i( i ) là ma trận của tenxơ quán
tính của vật rắn đối với khối tâm Ci của nó ở trong hệ qui chiếu động C i x i y i z i gắn
r
liền vào vật Bi, còn ω i( i ) là véc tơ đại số của ωi trong hệ qui chiếu C i x i y i z i .
Thế các biểu thức (2.6) vào biểu thức (2.7) ta được
T = ∑ m i (J Ti q& ) J Ti q& + ∑ (J Ri q& ) I i J R i q&
1 p T 1 p T
2 i=1 2 i=1
(2.8)
= q& T [∑ (m i J TTi J Ti + J TRi I i J Ri )]q&
1 p
2 i =1
Bây giờ ta đưa vào ký hiệu

M (q) = ∑ (m i J TT J T + J TR I i J R )
p
i i i i
(2.9)
i =1
Biểu thức (2.9) được gọi là ma trận khối lượng suy rộng. Khi đó biểu thức
động năng của hệ nhiều vật có dạng rất gọn như sau:
1
T = q& T M (q)q& (2.10)
2
Trong đó
46
⎡ m11 (q) K m1f (q)⎤ ⎡ q1 ⎤ ⎡ q& 1 ⎤ ⎡ &q&1 ⎤

M (q) = ⎢ K K K ⎥ , q = ⎢ M ⎥ , q& = ⎢ M ⎥ , q && = ⎢ M ⎥ (2.11)
⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢⎣m f 1 (q) K m ff (q) ⎥⎦ ⎢⎣q f ⎥⎦ ⎢⎣q& f ⎥⎦ ⎢⎣&q& f ⎥⎦
Biểu thức động năng (2.10) có thể viết lại dưới dạng như sau
1 f f
T = ∑∑ m jk (q 1 ,K, q f )q& k q& j (2.12)
2 j=1 k =1
Đạo hàm biểu thức động năng (2.12) theo các vận tốc suy rộng q& j và các tọa
độ suy rộng qj ta được
∂T f ∂T 1 f f ∂m ik
= ∑ m ij q& j , = ∑∑ q& k q& j
∂q& i j=1 ∂q i 2 j=1 k =1 ∂q i
Từ đó suy ra
d ⎛ ∂T ⎞ f f f ∂m
⎜⎜ ⎟⎟ = ∑ m ij&q& j + ∑∑ ij q& k q& j
dt ⎝ ∂q& i ⎠ j=1 j=1 k =1 ∂q k
Thế các biểu thức trên vào phương trình Lagrange loại 2
d ⎛⎜ ∂T ⎞⎟ ∂T ∂Π
− = − + τj ( j = 1,K, f ) (2.13)
dt ⎜⎝ ∂q& j ⎟⎠ ∂q j ∂q j
ta được
f f f
⎛ 1 ⎞
∑ m &q&
ij
+ ∑∑ ⎜ m jk ,l − m kl , j ⎟q& k q& l + g j (q) = τ j
j
j=1 k =1 ⎝
( j = 1,K, f ) (2.14
j=1 2 ⎠
Trong đó ta sử dụng các ký hiệu
∂m jk ∂m kl ∂Π
m jk ,l = , m kl , j = , g j (q) = (2.15)
∂q l ∂q j ∂q j
Do ma trận khối lượng M (q) là ma trận đối xứng, nên ta có
m ij,k = m jk ,i = m ki , j . Do đó phương trình (2.14) có thể viết lại dưới dạng như sau
1 f f
(m lj,k + m jk ,l − m kl, j )q& k q& j = − ∂Π + τi (i = 1,K, f ) (2.16)
f
j=1
∑ m ij&q& j +∑∑
2 k =1 l=1 ∂q i
Phương trình (2.14) hoặc (2.16) thường được gọi là dạng thức Lagrange loại
2 của hệ nhiều vật.
Nếu ta đưa vào ký hiệu
h ij,k (q) = m ij,k + m ik , j − m kj,i (2.17)
47
Thì phương trình (2.14) có thể viết lại dưới dạng

f f f
∑m
j=1
ij (q )q j + ∑∑ h ij, k q k q j + g i (q ) + τ i
&& & &
j=1 k =1
(i = 1,K, f ) (2.18)
Để thuận tiện cách viết ta đưa vào ký hiệu
f
c ij (q, q& ) = ∑ h ij,k (q)q& k (2.19)
k =1
khi đó
f f f
∑∑ h
j=1 k =1
ij, k (q )q k q j = ∑ c ij (q, q )q j
& &
j=1
& & (2.20)
Phương trình vi phân (2.18) bây giờ có dạng
f f
∑m
j=1
ij (q)&q& j + ∑ c ij (q, q& )q& j + g i (q) = τ i
j=1
(i = 1,K, f ) (2.21)
Phương trình (2.21) có thể viết lại dưới dạng ma trận
M (q)q && + C(q, q& )q& + g (q) = τ (2.22)
trong đó :
q∈Rn là véc tơ tọa độ suy rộng,
M (q) ∈ R n×n là ma trận khối lượng, đối xứng xác định dương,
C(q, q& ) ∈ R n×n là ma trận liên quan đến lực coriolis và lực ly tâm,
g(q) ∈ R n là véc tơ lực do trọng lực,
τ∈Rn là véc tơ lực dẫn động từ các động cơ.
Trong các bài toán điều khiển rôbốt người ta thường hay sử dụng phương
trình vi phân chuyển động dạng (2.22).
Việc thiết lập phương trình vi phân chuyển động của rôbốt ngày nay đã được
tiến hành một cách tự động nhờ các phần mềm. Ở trường Đại học Bách khoa Hà
Nội, nhóm nghiên cứu của GS. Nguyễn Văn Khang đã xây dựng phần mềm
ROBODYN [49] để thiết lập tự động các phương trình vi phân chuyển động của
rôbốt. Trong luận án này sử dụng phần mềm ROBODYN để thiết lập phương trình
vi phân chuyển động của các rôbốt.
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
Một trong các bài toán quan trọng khi tính toán thiết kế rôbốt là xác định các
lực/mômen dẫn động cần thiết cho các khâu dẫn của rôbốt để khâu thao tác của
rôbốt có thể làm việc theo một chương trình đã định trước. Bài toán này được gọi
là bài toán động lực học ngược. Bài toán động lực học ngược một mặt phải dựa vào
48
các thuật toán của bài toán động học ngược, mặt khác có liên quan mật thiết tới bài
toán điều khiển chuyển động của rôbốt.
Trong phần này, dựa trên thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy
rộng giải bài toán động học ngược trình bày một thuật toán số giải các bài toán
động lực học ngược cho rôbốt dư dẫn động trong không gian thao tác.
Mối liên hệ giữa vị trí của bàn kẹp với các biến khớp có thể biểu diễn dưới
dạng
x = f(q) (2.24)
trong đó q ∈ R là véc tơ chứa các biến khớp, x ∈ R là véc tơ chứa vị trí tâm của
n m
bàn kẹp trong một hệ tọa độ cố định và hướng của bàn kẹp.
x = [x 1 x 2 K x m ] q = [q1 q 2 K q n ]
T T

∂f
x& = q& = J (q)q& (2.25)
∂q
trong đó J(q) là ma trận Jacobi cỡ m × n được xác định theo (1.34). Giả sử hạng
của J(q) là m, theo [41, 45] ta chọn
J + (q) = J T (q)[J (q)J T (q)]−1 (2.26)
+
Ma trận J (q) được gọi là ma trận tựa nghịch đảo của ma trận J(q). Từ
(2.25), nếu chỉ quan tâm đến nghiệm bình phương tối thiểu, ta có
q& = J + (q)x& (2.27)
q&& = J + (q)&x& + J& + (q)x& (2.28)
d
với J& + (q) = J + (q).
dt
Các công thức (2.27) và (2.28) cho phép ta xác định được véc tơ vận tốc và
véc tơ gia tốc suy rộng q& và q && , nếu như biết q(t) tại thời điểm khảo sát và quy luật
chuyển động của bàn kẹp x( t ), x& ( t ), &x&( t ) .
Sử dụng phương pháp hiệu chỉnh gia lượng véc tơ tọa độ suy rộng đã trình
bày trong chương 1, chúng ta có các bước xác định q, q& , q && như sau:
Bước 1. Giải hệ phương trình đại số tuyến tính
J (q~ ) Δq ≈ x ( t ) − f ( q ~ ) (2.29)
0 0 0
được
Δq 0 = J + (q ~ )[x( t ) − f (q ~ )] . (2.30)
0 0
49
Bước 2. Điều chỉnh nghiệm

~ := q
q ~ + Δq . (2.31)
0 0 0
Bước 3. Kiểm tra điều kiện sai số

Nếu Δq 0 ≤ ε , ε là sai số cho phép, chuyển sang bước 4, ngược lại thì ta trở
lại bước 1.
Bước 4. Lấy nghiệm q 0 = q ~ .
0
Nếu không chỉ quan tâm đến nghiệm bình phương tối thiểu của phương trình
(2.25) thì biểu thức nghiệm (2.27) có thể được viết lại như sau:
q& = J + (q)x& + [E − J + (q)J (q)]z 0 (2.32)
trong đó z 0 ∈ R n là một vectơ tùy ý và E ∈ R n×n là một ma trận đơn vị.
Việc chọn véc tơ z0 cho phép ta khai thác được ưu điểm (khả năng tránh vật
cản, tránh va chạm với giới hạn khớp, tránh cấu hình kỳ dị) của rôbốt dư dẫn động
so với rôbốt chuẩn. Trong phần này, để tránh va chạm vào giới hạn khớp ta sẽ chọn
z0 để hàm sau đây đạt cực tiểu
2
1 n ⎛ q i − qi ⎞
φ(q) = ∑ c i ⎜⎜ ⎟ (2.33)
2 i =1 ⎝ q iM − q im ⎟⎠
Với qiM (qim) là ký hiệu của giới hạn lớn nhất (nhỏ nhất), qi là giá trị giữa
của khoảng làm việc của khớp, và các trọng số dương ci > 0. Do đó, cực tiểu của
hàm này sẽ làm cho các biến khớp di chuyển về giá trị trung gian, tính dư dẫn động
sẽ được khai thác để giữ cho các biến khớp gần giá trị giữa của khoảng làm việc
của rôbốt, tránh được va chạm với các giới hạn khớp.
Ta lấy
z 0 = − α ∇ φ (q ) (2.34)
trong đó: ∇φ(q) : gradient của φ(q ) , hằng số âm nếu φ(q ) cần cực tiểu hoá.
Từ (2.32) suy ra
&& = J + (q)&x& + J& + (q)x&
q
d + (2.35)
+ [E − J + (q)J (q)]z& 0 −
[J (q)J (q)]z 0
dt
Sau khi tính được các giá trị của q ( t k ), q& ( t k ), q
&&( t k ) tại thời điểm tk, xấp xỉ
ban đầu cho thời điểm tiếp theo t k +1 = t k + Δt được chọn như sau
q k +1 = q k + J + (q)x& k Δt + [E − J + (q k )J (q k )]z 0 Δt .
50
Sau đó thuật toán điều chỉnh gia lượng sai số vectơ tọa độ suy rộng được áp
dụng để tinh chỉnh giá trị xấp xỉ trên.
Phương trình động lực của tay máy rôbốt có thể nhận được nhờ sử dụng
phương trình Lagrăng loại 2 như sau
M (q)q&& + C(q, q& )q& + g (q) = τ , (2.36)
Sử dụng file số liệu các véc tơ q, q& , q&& của bài toán động học ngược, từ
phương trình (2.36) ta có thể xác định mômen/lực cần thiết tương ứng với chuyển
động mong muốn x(t) của bàn kẹp.
Các bước tiến hành tính mômen/lực động cơ khi bàn kẹp chuyển động theo
một quy luật x(t) cho trước như sau:
1. Giải bài toán động học ngược, xác định các tọa độ, vận tốc và gia tốc suy
rộng q, q& , q
&& của các khớp từ chuyển động của bàn kẹp x( t ), x& ( t ), &x&( t ) .
2. Sử dụng phương trình (2.36) tính được các mômen/lực τ của các động cơ
dẫn.
Toàn bộ thuật toán xác định mômen/lực dẫn động các khâu rôbốt dư dẫn
động trong không gian thao tác được minh họa như trên sơ đồ khối (hình 2.2).
51
Xác định f ( q ), M ( q ), C ( q , q& ), g ( q )
Cho x=x(t), t0, q0, N, T
∂f
J (q ) = ( q ); h = Δ t = T / N
∂q
~ =q
k:=0; tk:=t0; q k 0
~ ), x = x ( t ), f = f ( q
Tính J + ( q ~ )
k k k k
~ )( x − f )
Δ q k = J + (q k k k
~ =q
q ~ + Δq
k k k
~ := q
~ + J + (q
~ ) x& Δ t Sai
q k k −1 k −1 k −1 Δq k < ε
Đúng
~ ) x& ; q
q& k = J + ( q ~ ) &x& + J& + ( q
&& k = J + ( q ~ ) x&
k k k k k
Tính M ( q k ), C ( q k , q& k ), g ( q k )
τ = M (q k )q
&& k + C ( q k , q& k ) q& k + g ( q k )
Xuất kết quả τk
Sai
k := k + 1 k≥N
Đúng
KẾT THÚC
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
52

Ví dụ 2.1. Xác định lực/mômen, phương trình chuyển động của rôbốt Scara
như hình 2.3 khi quy luật chuyển động của bàn kẹp có dạng:
xp = 0.30+0.08cos(t) (m), yp = 0.30+0.08sin(t) (m), zp = 0.05cos(t) (m),
Hình 2.3. Sơ đồ kết cấu rôbốt Scara
Rôbốt Scara có 4 khớp dẫn động dạng RRTR trong khi theo yêu cầu của bài
toán cần có 3 tọa độ xác định vị trí của bàn kẹp vì vậy trong trường hợp này nó là
rôbốt dư dẫn động (dư 1 khớp dẫn động).
Từ sơ đồ kết cấu của rôbốt ta có bảng các tham số động học:
Bảng 2.1 Các tham số động học của rôbốt Scara
Khâu θ d a α
1 q1 d1 a1 0
2 q2 0 a2 π
3 0 q3 0 0
4 q4 d4 0 0
53
Giải bài toán động học thuận ta được:

⎡cos(q 1 + q 2 − q 4 ) sin(q1 + q 2 − q 4 ) 0 a 2 cos(q 1 + q 2 ) + a 1 cos(q 1 )⎤
⎢ sin(q + q − q ) − cos(q + q − q ) 0 a sin(q + q ) + a sin(q ) ⎥
D4 = ⎢ 1 2 4 1 2 4 2 1 2 1 1
⎥
⎢ 0 0 −1 −d 4 −q 3 + d 1 ⎥
⎢ ⎥
⎣ 0 0 0 1 ⎦
x P = a 2 cos(q1 + q 2 ) + a 1 cos(q1 )
y P = a 2 sin(q 1 + q 2 ) + a 1 sin(q1 )
z P = −d 4 −q 3 + d 1
suy ra tọa độ của tâm bàn kẹp và ma trận Jacobi là:
⎡− a 1S1 − a 2S12 − a 2S12 0 0⎤
J = ⎢ a 1C1 + a 2 C12 a 2 C12 0 0⎥
⎢ ⎥
⎢⎣ 0 0 − 1 0⎥⎦
Chọn bước thời gian h = 0.001 giây; sai số cho phép ε ≤ 10 −6 từ đó dùng
chương trình giải bài toán động học ngược ta nhận được đồ thị góc quay, vận tốc
góc, gia tốc góc và các sai số chuyển động của bàn kẹp là:
2
Vi tri cac khau (rad, m)
q1(rad)
-1
q2(rad)
-2 q3(m)
q4(rad)
-3
0 2 4 6 8 10
Thoi gian(s)
Hình 2.4. Đồ thị chuyển động của các khớp động rôbốt Scara
54
0.4
Van toc cac khau (rad/s, m/s) 0.3
0.2
0.1
-0.1
q.1(rad/s)
-0.2 q.2(rad/s)
q.3(m/s)
-0.3
q.4(rad/s)
-0.4
0 2 4 6 8 10
Thoi gian(s)
Hình 2.5. Đồ thị vận tốc chuyển động của các khớp động rôbốt Scara
0.6
Gia toc cac khau (rad/s 2, m/s 2)
0.4
0.2
-0.2 q..1(rad/s 2)
q..2(rad/s 2)
-0.4
q..3(m/s 2)
-0.6
q..4(rad/s 2)
-0.8
0 2 4 6 8 10
Thoi gian(s)
Hình 2.6. Đồ thị gia tốc chuyển động của các khớp động rôbốt Scara
55
-15
x 10
Sai so vi tri trong khong gian thao tac (m)

2
-2
-4 Sai so truc x
Sai so truc y
Sai so truc z
-6
0 2 4 6 8 10
Thoi gian(s)
Hình 2.7. Đồ thị sai số vị trí bàn kẹp của rôbốt Scara
-16
x 10
Sai so van toc trong khong gian thao tac (m/s)
1.5
0.5
-0.5
-1
Sai so truc x
-1.5 Sai so truc y
Sai so truc z
-2
0 2 4 6 8 10
Thoi gian(s)
Hình 2.8. Đồ thị sai số vận tốc bàn kẹp của rôbốt Scara
56
-16
Sai so gia toc trong khong gian thao tac (m/s 2)

x 10
3
-1
Sai so truc x
-2 Sai so truc y
Sai so truc z
-3
0 2 4 6 8 10
Thoi gian(s)
Hình 2.9. Đồ thị sai số gia tốc bàn kẹp của rôbốt Scara
Bảng 2.2. Bảng thông số động lực rôbốt Scara 4 bậc tự do
Mômen quán tính khối từng khâu
Vị trí trọng tâm (so với
(tính đối với hệ toạ độ đặt tại
gốc toạ độ gắn trên mỗi Khối
Khâu trọng tâm từng khâu và song song
khớp) lượng
với toạ độ khớp)
xC yC zC Ixx Iyy Izz Ixy Ixz Iyz
1 -(a1-l1x) 0 -l1z m1 I1x I1y I1z 0 0 0
2 -(a2-l2) 0 0 m2 I2x I2y I2z 0 0 0
3 0 0 -l3 m3 I3x I3y I3z 0 0 0
4 0 0 -(d4-l4) m4 I4x I4y I4z 0 0 0
Với bảng tham số động lực học như trong bảng 2.2, thực hiện chương trình
động lực học ngược ta xác định được biểu thức tính mômen/lực tác động trên các
khớp động của rôbốt:
57
τ 1 = (m1.l12+Iz1+m2.a12+2.m2.l2.a1.cos(q2)+m2.l22+Iz2+m3.a22+2.m3.a2.a1.cos(q2)
+m3.a12+Iz3+m4.a22+2.m4.a2.a1.cos(q2)+m4.a12+Iz4). q1_2dot +(m2.l2.a1.cos(q2) +
m2.l22+Iz2+m3.a22+m3.a2.a1.cos(q2)+Iz3+m4.a22+m4.a2.a1.cos(q2)+Iz4). q2_2dot -
Iz4.q4_2dot -a1.sin(q2).(m2.l2+m3.a2+m4.a2). q2_2dot .q1_dot-
a1.sin(q2).(m2.l2+m3.a2+m4.a2).(q2_dot+q1_dot).q2_dot;
τ 2 = (m2.l2.a1.cos(q2)+m2.l22+Iz2+m3.a22+m3.a2.a1.cos(q2)+Iz3+m4.a22+
m4.a2.a1.cos(q2)+Iz4). q1_2dot +(m2.l22+Iz2+m3.a22+Iz3+m4.a22+Iz4). q2_2dot -
Iz4. q4_2dot +a1.sin(q2).(m2.l2+m3.a2+m4.a2).q1_dot2;
τ 3 = (m3+m4). q3_2dot -m3.g-m4.g;
τ 4 = -Iz4. q1_2dot -Iz4. q2_2dot +Iz4. q4_2dot;
Với bộ các giá trị tham số động lực học :
m1 = 10; m2 = 5; m3 = 2; m4 = 1 (kg)
a1 = 0.22; a2 = 0.40; d4 = 0.09 (m)
l1 = 0.15; l2 = 0.16; l3 = 0.19; l4 = 0.04 (m)
Iz1 = 0.21; Iz2 = 0.20; Iz3 = 0.18; Iz4 = 0.15 (kg.m2)
Ta vẽ được đồ thị biến thiên của mômen/lực theo thời gian trên các khớp động
là:
0.4
0.2
Momen khop 1 (Nm)
-0.2
-0.4
Momen1
-0.6
0 2 4 6 8 10
Thoi gian(s)
Hình 2.10. Đồ thị mômen tác động trên khớp 1
58
0.3
Momen2
0.2
Momen khop 2 (Nm)
0.1
-0.1
-0.2
-0.3
-0.4
0 2 4 6 8 10
Thoi gian(s)
-29.2
Luc 3
-29.25
-29.3
(N)
-29.35
Luc khop 3
-29.4
-29.45
-29.5
-29.55
-29.6
0 2 4 6 8 10
Thoi gian(s)
Hình 2.12. Đồ thị lực tác động trên khớp 3

59
0.04
Momen4
0.02
Momen khop 4 (Nm)

0
-0.02
-0.04
-0.06
0 2 4 6 8 10
Thoi gian(s)
Qua các đồ thị trên ta thấy rằng kết quả giải phần động học ngược để xác
định vị trí, vận tốc và gia tốc của các khớp động rôbốt Scara có độ chính xác khá
cao (các hình: 7, 8, 9), sai số vị trí bé hơn 10-14 còn sai số của vận tốc và của gia
tốc bé hơn 10-15, vì vậy mà khi thay vào biểu thức để tính toán giá trị của
lực/mômen cũng sẽ cho ta kết quả với độ chính xác cao.
Ví dụ 2.2. Giải bài toán động lực học ngược của một rôbốt phẳng 5 khâu
động (hình 2.14). Khâu i có chiều dài ai, khối tâm Ci, khoảng cách từ khối tâm đến
khớp nối với khâu trước đó là li, khối lượng mi, và có mômen quán tính đối với
khối tâm Izi, (i = 1,2,...,5). Một số thông số của rôbốt được cho trong bảng 2.3.
B q3 C q4
y ϕ
q2 C3
A C4
C2 D
E q5
q1 g
C1
O
x
Hình 2.14. Sơ đồ rôbốt phẳng 5 khâu động

60
Bàn kẹp (khâu 5) được yêu cầu chuyển động theo một quỹ đạo tròn có
phương trình là
x = 0.8 + 0.1cos(2 t )
y = −0.8 + 0.1sin( 2 t )
và bàn kẹp phải luôn tạo với phương thẳng đứng 1 góc ϕ = 1 (rad) .
Bảng 2.3 Các thông số động lực học của rôbốt 5 khâu động
Khâu 1 2 3 4 5
m [kg] 10 7.5 5 4 1
Iz
0.11 0.10 0.03 0.02 0.02
[kgm2]
a [m] 0.55 0.50 0.45 0.40 0.20
l [m] 0.15 0.16 0.19 0.20 0.10
Ta có bảng các thông số DH của rôbốt như dưới đây:
Bảng 2.4 Các thông số động học của rôbốt 5 khâu động
Khâu θ d a α
1 q1 0 a1 0
2 q2 0 a2 0
3 q3 0 a3 0
4 q4 0 a4 0
5 q5 0 a5 0
Ta sẽ sử dụng kết quả của bài toán động học ngược đã thu nhận được ở ví dụ
1.2 của chương 1.
Với chương trình ROBOTDYN ta sẽ thiết lập được phương trình vi phân
chuyển động của các khâu, song do phương trình quá dài mà không trình bày cụ
thể trong luận án này. Với các thông số động lực học đã cho trong bảng 2.3 ta tính
toán và vẽ được đồ thị mômen tác động trên các khớp động là:
61
160
u1
140
u2
120
u3
100 u4
Moment [Nm]
80 u5
60
40
20
-20
-40
0 5 10 15 20
time [s]
Hình 2.15. Đồ thị các mômen động cơ theo thời gian

Dựa trên đồ thị các mômen động cơ, người kỹ sư dễ dàng lựa chọn các động
cơ phát động phù hợp lắp trên các khâu của rôbốt.
Ví dụ 2.3. Xác định phương trình động lực học của các khâu thuộc rôbốt
phẳng 6 khâu động như hình 2.16.
x2 O3
O4
O2
q4
q3 x4
x1 y6
q5
q2 O5
O1 y0 O6
q6 x6
q1
x0
O x5
Hình 2.16. Rôbốt phẳng 6 khâu động

Phương trình tọa độ chuyển động của điểm thao tác có dạng:
xP = 0.8+0.2cos(2t) (m); yP = -0.8+0.2sin(2t) (m)
62
Bàn kẹp tạo với với phương nằm ngang 1 góc là 900.
Ta ký hiệu O0O1 = a1, O1O2 = a2, O2O3 = a3, O3O4 = a4, O4O5 = a5, O5O6 = a6;
O0C1 = l1, O1C2 = l2, O2C3 = l3, O3C4 = l4, O4C5 = l5, O5C6 = l6.
Ta có các bảng thông số DH và thông số động lực của rôbốt 6 khâu động
như dưới đây
Bảng 2.5 Các thông số DH rôbốt 6 khâu động
Khâu θ d a α
1 q1 0 a1 0
2 q2 0 a2 0
3 q3 0 a3 0
4 q4 0 a4 0
5 q5 0 a5 0
6 q6 0 a6 0
Bảng 2.6 Các thông số động lực rôbốt 6 khâu động
Mômen quán tính khối từng khâu
Vị trí trọng tâm (so với
(tính đối với hệ toạ độ đặt tại trọng
gốc toạ độ gắn trên Khối
Khâu tâm từng khâu và song song với toạ
mỗi khớp) lượng
độ khớp)
xC yC zC Ixx Iyy Izz Ixy Ixz Iyz
1 - (a1 - l1) 0 0 m1 Ix1 Iy1 Iz1 0 0 0
2 - (a2 - l2) 0 0 m2 Ix2 Iy2 Iz2 0 0 0
3 - (a3 - l3) 0 0 m3 Ix3 Iy3 Iz3 0 0 0
4 - (a4 - l4) 0 0 m4 Ix4 Iy4 Iz4 0 0 0
5 - (a5 - l5) 0 0 m5 Ix5 Iy5 Iz5 0 0 0
6 - (a6 - l6) 0 0 m6 Ix6 Iy6 Iz6 0 0 0
Qua thực hiện chương trình tính toán ta thu được các kết quả chính như sau
(do phương trình vi phân chuyển động quá dài nên không trình bày trong bản luận
án, có thể tham khảo trong phụ lục kèm theo):
Khâu 1
Toạ độ khối tâm C1
rC1 = [l1C1 0]
T
l1S1
Ma trận Jacobi tịnh tiến của khâu 1
63
⎡− l1S1 0 0 0 0 0⎤
∂rC1 ⎢
J T1 = = l1 C 1 0 0 0 0 0⎥
∂q ⎢ ⎥
⎢⎣ 1 0 0 0 0 0⎥⎦
Vận tốc góc
ω 1 = [0 0 q& 1 ]
T
Ma trận Jacobi quay

⎡0 0 0 0 0 0⎤
∂ω1 ⎢
J R1 = = 0 0 0 0 0 0⎥
∂q& ⎢ ⎥
⎢⎣1 0 0 0 0 0⎥⎦
Khâu 2
rC 2 = [a 1C1 + l 2 C12 a 1S1 + l 2 S12 0]
T
Ma trận Jacobi tịnh tiến

⎡− a 1S1 − l 2S12 − l 2S12 0 0 0 0⎤
∂rC 2 ⎢
J T2 = = a 1C1 + l 2 C12 l 2 C12 0 0 0 0⎥
∂q ⎢ ⎥
⎢⎣ 1 0 0 0 0 0⎥⎦
Vận tốc góc khâu 2
ω 2 = [0 0 q& 1 + q& 2 ]
T

⎡0 0 0 0 0 0 ⎤
∂ω 2 ⎢
JR2 = = 0 0 0 0 0 0⎥
∂q& ⎢ ⎥
⎢⎣1 1 0 0 0 0⎥⎦
Khâu 3
rC 3 = [a 1C1 + a 2 C12 + l 3 C123 a 1S1 + a 2 S12 + l 3S123 0]
T
⎡− a 1S1 − a 2S12 − l 3S123 − a 2S12 − l 3S123 − l 3S123 0 0 0⎤

∂r
J T3 = C 3 = ⎢ a 1C1 + a 2 C12 + l 3 C123 a 2 C12 + l 3 C123 l 3 C123 0 0 0⎥
∂q ⎢ ⎥
⎢⎣ 0 0 0 0 0 0⎥⎦
64

ω 3 = [0 0 q& 1 + q& 2 + q& 3 ]
T

⎡0 0 0 0 0 0⎤
∂ω 3 ⎢
J R3 = = 0 0 0 0 0 0⎥
∂q& ⎢ ⎥
⎢⎣1 1 1 0 0 0⎥⎦
Khâu 4
rC 4 = [a 1C1 + a 2 C12 + a 3 C123 + l 4 C1234 a 1S1 + a 2 S12 + a 3S123 + l 4 S1234 0]
T

⎡ J 11 J 12 J 13 J 14 0 0⎤
∂rC 4 ⎢
J T4 = = J 21 J 22 J 23 J 24 0 0⎥
∂q ⎢ ⎥
⎣⎢ 0 0 0 0 0 0⎦⎥
Với:
J11 = -a1S1-a2S12-a3S123-l4S1234
J12 = -a2S12-a3S123-l4S1234
J13 = -a3S123-l4S1234
J14 = -l4S1234
J21 = a1C1+a2C12+a3C123+l4C1234
J22 = a2C12+a3C123+l4C1234;
J23 = a3C123+l4C1234
J24 = l4C1234
ω 4 = [0 0 q& 1 + q& 2 + q& 3 + q& 4 ]
T

⎡0 0 0 0 0 0 ⎤
∂ω 4 ⎢
JR4 = = 0 0 0 0 0 0⎥
∂q& ⎢ ⎥
⎢⎣1 1 1 1 0 0⎥⎦
Khâu 5
65
⎡a 1C1 + a 2 C12 + a 3 C123 + a 4 C1234 + l 5 C12345 ⎤

rC 5 = ⎢ a 1S1 + a 2S12 + a 3S123 + a 4S1234 + l 5S12345 ⎥
⎢ ⎥
⎢⎣ 0 ⎥⎦
⎡ J 11 J 12 J 13 J 14 J 15 0⎤
∂rC 5 ⎢
J T5 = = J 21 J 22 J 23 J 24 J 25 0⎥
∂q ⎢ ⎥
⎢⎣ 0 0 0 0 0 0⎥⎦
Với:
J11 = -a1S1-a2S12-a3S123-a4S1234-l5S12345
J12 = -a2S12-a3S123-a4S1234-l5S12345
J13 = -a3S123-a4S1234-l5S12345
J14 = -a4S1234-l5S12345
J15 = -l5S12345
J21 = a1C1+a2C12+a3C123+a4C1234+l5C12345
J22 = a2C12+a3C123+ a4C1234+l5C12345
J23 = a3C123+ a4C1234+l5C12345
J24 = a4C1234+l5C12345
J25 = l5C12345
ω 5 = [0 0 q& 1 + q& 2 + q& 3 + q& 4 + q& 5 ]
T

⎡0 0 0 0 0 0 ⎤
∂ω 5 ⎢
J R5 = = 0 0 0 0 0 0⎥
∂q& ⎢ ⎥
⎢⎣1 1 1 1 1 0⎥⎦
Khâu 6
⎡a 1C1 + a 2 C12 + a 3 C123 + a 4 C1234 + a 5 C12345 + l 6 C123456 ⎤
rC 6 = ⎢ a 1S1 + a 2S12 + a 3S123 + a 4S1234 + a 5S12345 + l 6S123456 ⎥
⎢ ⎥
⎢⎣ 0 ⎥⎦
66
⎡ J 11 J 12 J 13 J 14 J15 J 16 ⎤
∂rC 6 ⎢
J T6 = = J 21 J 22 J 23 J 24 J 25 J 26 ⎥
∂q ⎢ ⎥
⎢⎣ 0 0 0 0 0 0 ⎥⎦
Với:
J11 = -a1S1-a2S12-a3S123-a4S1234-a5S12345-l6S123456
J12 = -a2S12-a3S123-a4S1234-a5S12345-l6S123456
J13 = -a3S123-a4S1234- a5S12345-l6S123456
J14 = -a4S1234-a5S12345-l6S123456
J15 = -a5S12345-l6S123456
J16 = -l6S123456;
J21 = a1C1+a2C12+a3C123+a4C1234+a5C12345+l6C123456
J22 = a2C12+a3C123+ a4C1234+ a5C12345+l6C123456
J23 = a3C123+ a4C1234+ a5C12345+l6C123456
J24 = a4C1234+ a5C12345+l6C123456
J25 = a5C12345+l6C123456
J26 = l6C123456;
ω 6 = [0 0 q& 1 + q& 2 + q& 3 + q& 4 + q& 5 + q& 6 ]
T

⎡0 0 0 0 0 0 ⎤
∂ω 6 ⎢
J R6 = = 0 0 0 0 0 0⎥
∂q& ⎢ ⎥
⎢⎣1 1 1 1 1 1 ⎥⎦
Ma trận tensor quán tính
⎡I x1 0 0⎤ ⎡I x 2 0 0⎤ ⎡I x 3 0 0⎤
I1 = ⎢ 0 I y1 0 ⎥ , I2 = ⎢ 0 I y2 0 ⎥ , I3 = ⎢ 0 I y3 0 ⎥,
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢⎣ 0 0 I z1 ⎥⎦ ⎢⎣ 0 0 I z 2 ⎥⎦ ⎢⎣ 0 0 I z 3 ⎥⎦
⎡I x 4 0 0⎤ ⎡I x 5 0 0⎤ ⎡I x 6 0 0⎤
I4 = ⎢ 0 I y4 0 ⎥ , I5 = ⎢ 0 I y5 0 ⎥ , I6 = ⎢ 0 I y6 0⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢⎣ 0 0 I z 4 ⎥⎦ ⎢⎣ 0 0 I z 5 ⎥⎦ ⎢⎣ 0 0 I z 6 ⎥⎦
67
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
Khâu 1 2 3 4 5 6
m (kg) 10 5 2 1 1 1
Iz (kgm2) 0.11 0.10 0.03 0.02 0.01 0.01
a (m) 0.40 0.35 0.30 0.20 0.10 0.1
l (m) 0.20 0.16 0.15 0.1 0.05 0.05
Khi rôbốt 6 khâu động có các giá trị thông số động lực học như trong bảng
2.7 ta có được các kết quả tính toán mômen các khớp động được trình bày trên các
hình từ 2.17 đến 2.22. Dựa trên các đồ thị này, người kỹ sư dễ dàng lựa chọn các
động cơ phát động lắp trên các khâu của rôbốt.
75
Momen1
70
Momen khop 1 (Nm)
65
60
55
0 2 4 6 8 10
Thoi gian(s)
Hình 2.17. Mômen trên khớp động thứ 1
32
31
Momen khop 2 (Nm)
30
29
28
27
Momen2
26
25
0 2 4 6 8 10
Thoi gian(s)

68
10
Momen khop 3 (Nm)

8
6
Momen3
5
3
0 2 4 6 8 10
Thoi gian(s)
0.5
Momen khop 4 (Nm)
-0.5
-1
-1.5
Momen4
-2
-2.5
0 2 4 6 8 10
Thoi gian(s)
-0.2
Momen5
-0.25
Momen khop 5 (Nm)
-0.3
-0.35
-0.4
-0.45
-0.5
-0.55
0 2 4 6 8 10
Thoi gian(s)

69
0.6
Momen6
0.55
Momen khop 6 (Nm)

0.5
0.45
0.4
0.35
0.3
0.25
0 2 4 6 8 10
Thoi gian(s)
2.4. Kết luận chương 2

Dựa trên thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng trong
chương 1, ở đây trình bày một thuật toán tính toán lực/mômen của các khâu dẫn
động trong không gian thao tác. Từ đó giúp cho người kỹ sư thiết kế chọn các động
cơ lắp vào rôbốt. Sử dụng thuật toán giải bài toán động lực học ngược rôbốt trình
bày trong luận án này cho phép chúng ta rút ngắn thời gian tính toán xuống rất
nhiều mà vẫn đảm bảo được độ chính xác cao (do sai số trong bài toán động học
ngược là rất nhỏ). Các ví dụ trình bày trong chương là các ví dụ mới, chưa có trong
các sách chuyên khảo.
Việc giải bài toán động lực học ngược là một khâu quan trọng trong việc
điều khiển chuyển động của rôbốt. Vấn đề này sẽ được trình bày trong chương 3.
Chương 3
Đ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
Điều khiển chuyển động rôbốt là bài toán quan trọng trong kỹ thuật rôbốt.
Có nhiều phương pháp điều khiển chuyển động rôbốt như điều khiển PD, điều
khiển PID, điều khiển trượt, điều khiển mờ [59, 63, 66, 69, 70, 72]. Trong
chương này giới thiệu sơ lược về khái niệm ổn định chuyển động và điều khiển
chuyển động rôbốt. Trọng tâm của chương là trình bày đ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 [19,
50, 51].
3.1 Cơ sở lý thuyết ổn định Lyapunov

Ổn định là một khái niệm quan trọng trong điều khiển [4, 64, 67]. Một hệ
không ổn định là không thể chấp nhận được về mặt ứng dụng. Về mặt trực
quan, một hệ gọi là ổn định nếu nó xuất phát từ một điểm nào đó lân cận điểm
hoạt động thì nó sẽ luôn ở xung quanh điểm hoạt động đó. Lý thuyết ổn định
Lyapunov có nêu hai phương pháp phân tích ổn định là phương pháp tuyến tính
hoá và phương pháp trực tiếp. Trong phạm vi của luận án này, ta chỉ sử dụng
phương pháp trực tiếp (hay còn gọi là phương pháp hàm Lyapunov).
3.1.1 Hệ phi tuyến và các điểm cân bằng

a. Hệ phi tuyến
Một hệ động lực phi tuyến là hệ có phương trình trạng thái được miêu tả
bằng hệ các phương trình vi phân phi tuyến sau
y& = f (y , t ) (3.1)
Trong đó:
• f ∈ R n là hàm véctơ phi tuyến,
• y ∈ R n là véctơ trạng thái. Số trạng thái n được gọi là cấp (order)
của hệ.
Về mặt trực quan phương trình này giống như phương trình trạng thái của
một hệ không điều khiển. Tuy nhiên, nó chính là phương trình trạng thái tổng
quát dùng cho bài toán phân tích của tất cả các hệ. Xét đối tượng điều khiển có
phương trình trạng thái
y& = f (y, u, t )
Nếu chọn luật điều khiển là u = g (y, t ) , ta thu được hệ y& = f [y , g(y , t ), t ]
do đó ta có thể viết lại dưới dạng (3.1).
71
Hệ tuyến tính là trường hợp riêng của hệ phi tuyến. Lúc đó (3.1) được
viết dưới dạng:
y& = A( t )y , với A(t) là ma trận nxn.
Tùy thuộc vào việc hệ có phụ thuộc hay không phụ thuộc vào biến thời
gian mà hệ phi tuyến được chia thành hệ ôtônôm và hệ phi ôtônôm theo định
nghĩa dưới đây.
Định nghĩa 3.1. Hệ phi tuyến (3.1) được gọi là ôtônôm nếu f không phụ
thuộc tường minh vào thời gian, nghĩa là phương trình trạng thái có dạng.
y& = f (y ) (3.1b)
Ngược lại, ta gọi là hệ phi ôtônôm.
Chú ý rằng, định nghĩa trên cũng như phương trình trạng thái (3.1) áp
dụng cho một hệ có phản hồi. Do đó, tính ôtônôm hay phi ôtônôm là phụ thuộc
vào cả đối tượng điều khiển lẫn bộ điều khiển. Ví dụ, nếu đối tượng điều khiển
có thể coi gần đúng là ôtônôm song bộ điều khiển lại phi ôtônôm thì cả hệ kín
sẽ là phi ôtônôm.
Sự khác nhau cơ bản giữa hệ ôtônôm và hệ phi ôtônôm là quỹ đạo trạng
thái của hệ ôtônôm không phụ thuộc vào giá trị thời điểm đầu. Do đó, quá trình
phân tích tính ổn định của hệ sẽ dễ dàng hơn. Trong tài liệu này, để đơn giản chỉ
trình bày việc nghiên cứu ổn định của các hệ ôtônôm.
b. Điểm cân bằng
Định nghĩa 3.2. Một trạng thái y* được gọi là trạng thái cân bằng của
hệ (hay điểm cân bằng) nếu một khi y(t) = y* nó sẽ nằm nguyên tại trạng thái y*
đó.
Như vậy điểm cân bằng y* sẽ thoả mãn phương trình f(y*) = 0.
Khi y* ≠ 0, ta có thể đổi biến để đưa điểm cân bằng của hệ về trùng với
gốc trạng thái như sau:
Đặt x = y – y* rồi thay vào biểu thức (3.1b), ta được:
x& = y& − y& * = f (x + y*) = g(x)
Ký hiệu lại f(x) = g(x), từ phương trình trên ta suy ra :
x& = f (x) (3.2)
Do đó, ta sẽ nghiên cứu tính ổn định của hệ quanh điểm gốc x* = 0.
Trong nhiều bài toán thực tế, chẳng hạn như bài toán điều khiển rôbốt
bám theo quỹ đạo, ta quan tâm đến tính ổn định của hệ quanh một quỹ đạo phụ
thuộc vào thời gian. Bằng phương pháp đổi biến, ta có thể biến đổi bài toán đã
được đặt ra (là ôtônôm hoặc phi ôtônôm) thành bài toán nghiên cứu sự ổn định
của một hệ phi ôtônôm quanh điểm cân bằng.
72
3.1.2 Khái niệm ổn định

Trong phần này ta phân tích các khái niệm ổn định, không ổn định, ổn
định tiệm cận và ổn định số mũ của nghiệm cân bằng x* = 0 của hệ ôtônôm
(3.2).
Ta ký hiệu Br là miền cầu còn Sr là mặt cầu tâm O bán kính r, BR là miền
cầu còn SR là mặt cầu tâm O bán kính R (R > r).
a. Ổn định và không ổn định
Định nghĩa 3.3. Trạng thái cân bằng x* = 0 được gọi là ổn định nếu
∀R > 0 , ∃r > 0 sao cho, nếu ||x(0)|| < r thì ||x(t)|| < R với ∀t ≥ 0 . Ngược lại,
điểm x* = 0 là điểm không ổn định (hình 3.1).
x1
2
O
x2
Br BR
1 Sr
SR
Hình 3.1 Điểm gốc O là:

• Điểm ổn định (1)
• Điểm không ổn định (2)
Bản chất của khái niệm ổn định Lyapunov ở trên là: quỹ đạo của hệ có
thể tiến đến gần điểm gốc một cách tùy ý, miễn là vị trí ban đầu ở gần gốc một
khoảng đủ nhỏ.
Chú ý rằng, trong hệ phi tuyến, hệ không ổn định không có nghĩa là hệ đó
luôn luôn di chuyển ra xa gốc đến vô cùng. Trên hình 3.3, quỹ đạo hệ di chuyển
từ điểm gần gốc tùy ý cuối cùng tiệm cận theo một chu trình. (Nếu xét một hình
tròn nằm bên trong chu trình đó thì cuối cùng hệ sẽ đi ra khỏi hình tròn đó).
Hình vẽ cũng nói lên sự khác nhau cơ bản giữa ổn định và không ổn định. Có
thể quỹ đạo của hệ vẫn nằm quanh điểm cân bằng ở một khoảng cách hữu hạn
nào đó, nhưng nó không thể gần điểm cân bằng một cách tùy ý.
b. Ổn định tiệm cận và ổn định số mũ
Trong nhiều trường hợp, chỉ dùng khái niệm ổn định Lyapunov là chưa đủ.
Để làm ví dụ, ta xét một vệ tinh bị nhiễu đánh bật ra khỏi vị trí cân bằng của nó.
73
Nếu hệ là ổn định, tùy theo cường độ của nhiễu (vị trí sai lệch ban đầu của vệ
tinh), vệ tinh sẽ ổn định về xung quanh điểm ban đầu. Tuy nhiên, để đảm bảo
chất lượng, vệ tinh phải tiến dần về vị trí ban đầu của nó. Vì thế, cần phải đặt ra
khái niệm về ổn định tiệm cận.
Định nghĩa 3.4. Một điểm cân bằng x* = 0 là ổn định tiệm cận (hình
3.2) nếu nó ổn định, và ngoài ra, ∃r > 0 mà ||x(0)|| < r cho ta x( t ) → 0 khi
t → ∞.
x1
O
x2
Br BR
3
Sr
SR
Hình 3.2 Điểm gốc O là điểm ổn định tiệm cận

Lúc này, trên Hình 3.2, miền cầu Br được gọi là miền hút (domain of
attraction) của điểm cân bằng. Vì tất cả các quỹ đạo xuất phát từ bên trong Br
cuối cùng đều hội tụ về điểm gốc.
Một điểm cân bằng chỉ ổn định Lyapunov nhưng không ổn định tiệm cận
thì được gọi là ổn định biên (marginally stable).
Như vậy, điểm cân bằng của hệ là ổn định tiệm cận nếu nó là ổn định và
là hội tụ. Tuy nhiên, một điểm cân bằng có thể hội tụ nhưng không nhất thiết là
ổn định. Hình 3.3 minh hoạ một trường hợp như vậy. Mọi quỹ đạo xuất phát từ
x2
C
r=1
x1
Hình 3.3. Trạng thái hội tụ không ổn định

74
một điểm khác 0 nằm bên trong hình tròn 1 đơn vị tiến đến đường cong C trước
khi hội tụ về gốc. Do đó, nó không ổn định theo nghĩa Lyapunov.
Trong các ứng dụng thực tế, nếu chỉ dùng khái niệm ổn định tiệm cận là
chưa đủ. Vì ổn định tiệm cận có thể hội tụ về 0 trong một khoảng thời gian vô
cùng. Do đó, ta cần có các khái niệm khác để phản ánh tốc độ hội tụ về 0 của
hệ, đó là khái niệm ổn định số mũ (exponential stability).
Định nghĩa 3.5. Một điểm cân bằng x* = 0 là ổn định số mũ nếu tồn tại
hai giá trị dương α và λ mà:
∀t ≥ 0, x( t ) ≤ α x(0) e − λt trong hình cầu Br nào đó bao quanh gốc.
Giá trị λ được gọi là tốc độ của hội tụ số mũ.
Khái niệm ổn định số mũ là khái niệm chặt hơn khái niệm ổn định tiệm
cận.
c. Ổn định cục bộ và ổn định toàn cục
Các khái niệm đưa ra ở phần trước nhằm mục đích phân tích hành vi cục
bộ của hệ xung quanh điểm cân bằng (các trạng thái ban đầu xuất phát gần điểm
cân bằng). Vì cần phân tích tính ổn định của hệ tại các giá trị trạng thái ban đầu
ở cách xa điểm cân bằng, nên cần đưa ra khái niệm ổn định toàn cục.
Nếu tính ổn định tiệm cận (hoặc ổn định số mũ) là đúng với bất kỳ trạng
thái ban đầu nào, điểm cân bằng được gọi là ổn định tiệm cận (hoặc ổn định số
mũ) toàn cục.
Trong hệ tuyến tính, có ba khái niệm: ổn định tiệm cận, ổn định biên, và
không ổn định. Ổn định tiệm cận trong hệ tuyến tính luôn đồng nghĩa với ổn
định số mũ toàn cục. Không ổn định trong hệ tuyến tính luôn đồng nghĩa với
quỹ đạo của hệ tiến ra vô cùng theo thời gian.
3.1.3 Phương pháp trực tiếp Lyapunov

Ý tưởng của phương pháp trực tiếp Lyapunov là xuất phát từ việc mở
rộng khái niệm cơ năng trong cơ học. Nếu một cơ hệ chịu hao phí năng lượng,
thì sau một khoảng thời gian, hệ sẽ trở về điểm cân bằng. Với một hệ tổng quát,
ta sẽ tìm cách đưa ra một hàm tương tự cơ năng và phân tích sự biến đổi theo
thời gian của hàm đó.
a. Hàm xác định dương và hàm Lyapunov
Định nghĩa 3.6. Một hàm vô hướng và liên tục V(x) được gọi là hàm
xác định dương cục bộ nếu V(0) = 0 và trong một miền cầu BR0,
x ≠ 0 ⇒ V(x) > 0 . Nếu V(0) = 0 và tính chất trên đúng trên toàn không gian
trạng thái thì V(x) được gọi là hàm xác định dương toàn cục.
Hàm V lúc này là hàm có cực tiểu duy nhất tại x = 0. Trên thực tế, nếu ta
cho một hàm bất kỳ có cực tiểu duy nhất trong một miền cầu nào đó. Bằng cách
75
thêm vào một hằng số, ta có thể xây dựng được một hàm xác định dương cục bộ
trong miền cầu đó. Đạo hàm của hàm đó theo thời gian vẫn không thay đổi.
Tương tự, ta đưa ra một số khái niệm liên quan.
Hàm V(x) là hàm xác định âm nếu –V(x) là hàm xác định dương.
Hàm V(x) là hàm bán xác định dương nếu V(0) = 0 và V(x ) ≥ 0; ∀x ≠ 0 .
Hàm V(x) là hàm bán xác định âm nếu –V(x) là hàm bán xác định dương.
Định nghĩa 3.7. Trong một miền cầu BR0, nếu hàm V(x) là xác định
dương có các đạo hàm riêng liên tục và có đạo hàm V & (x) theo thời gian dựa
& (x) ≤ 0 ) thì V(x) sẽ được
trên phương trình vi phân (3.2) là bán xác định âm ( V
gọi là hàm Lyapunov của hệ (3.2).
Ta có thể sử dụng các định lý sau để xét về sự ổn định của hệ thống.
& (x) được hiểu là đạo hàm theo thời gian của hàm
Trong các định lý này thì V
V(x) dựa vào hệ phương trình vi phân (3.2).
b. Định lý Lyapunov cho ổn định cục bộ
Nếu trong một miền cầu đóng BR0, tồn tại hàm vô hướng V(x) có đạo
hàm riêng cấp 1 liên tục thoả mãn:
• V(x) xác định dương (cục bộ trong BR0)
• V & (x) bán xác định âm (cục bộ trong BR0)
thì điểm cân bằng x* = 0 là ổn định.
Nếu V(x) xác định dương và V& (x) là xác định âm (cục bộ trong BR0) thì
điểm cân bằng x* = 0 sẽ ổn định tiệm cận.
c. Định lý Lyapunov cho ổn định toàn cục
Giả sử tồn tại một hàm vô hướng V(x) có đạo hàm bậc nhất liên tục và
thoả mãn
• V(x) xác định dương
• V & (x) xác định âm
• V(x) → ∞ khi x → ∞
thì điểm cân bằng tại gốc là ổn định tiệm cận toàn cục.
Ổn định toàn cục ngoài việc mở rộng miền cầu đóng BR0 ra toàn bộ
không gian trạng thái ta còn cần một tính chất của hàm V(x), đó là tính chất
không bị chặn theo mọi hướng (radially unbounded). Tính chất này nhằm mục
đích bảo đảm đường chu tuyến (contour) V(x) = Vα luôn là một đường cong
đóng. Hình 3.4 minh hoạ trường hợp đường chu tuyến là đường cong mở. Có
thể thấy, mặc dù trạng thái đi theo đường giảm của V(x), x vẫn phân kỳ ra xa
điểm gốc.
76
Chú ý:
– Với một hệ đã cho, có thể tồn tại nhiều hàm Lyapunov khác nhau.
– Với một hệ đã cho, việc lựa chọn hàm Lyapunov này có thể cho ta kết
quả chính xác hơn khi chọn các hàm khác.
– Định lý Lyapunov là điều kiện đủ, nghĩa là nếu ta sử dụng một hàm
không thoả mãn tất cả các điều kiện của định lý thì ta chưa thể kết
luận được bất cứ điều gì về tính ổn định hay không ổn định của hệ.
x2
x(t)
x1
V(x) = V3
V(x) = V2
V(x) = V1
V1 > V2 > V3
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
d. Định lý tập bất biến (invariant set)

Điều kiện về ổn định tiệm cận của hệ phi tuyến là một điều kiện rất quan
trọng. Tuy nhiên, điều kiện này đòi hỏi tính xác định âm của V& (x) , trong khi
trên thực tế, ta thường chỉ nhận được V& (x) là bán xác định âm. Nhờ sự đóng góp
của La Salle trong định lý tập bất biến, ta vẫn có thể kết luận về tính ổn định
tiệm cận của hệ. Khái niệm tập bất biến là sự khái quát hoá của khái niệm điểm
cân bằng.
Tập G gọi là tập bất biến (invariant set) của một hệ động học nếu mọi
quỹ đạo của hệ xuất phát từ một điểm trong G sẽ luôn ở trong G.
Điểm cân bằng, miền hút đều là tập bất biến. Một tập bất biến tầm thường
khác là toàn bộ không gian trạng thái. Với hệ ôtônôm, bất kỳ quỹ đạo nào trong
không gian trạng thái đều là tập bất biến.
77
e. Định lý tập bất biến cục bộ

Xét hệ ôtônôm dạng x& = f (x) .
Với f là một hàm liên tục, V(x) là hàm vô hướng có đạo hàm cấp 1 liên
tục. Giả sử
• Với l > 0 nào đó, miền Ω l được định nghĩa bởi V(x) < l là bị chặn
• V & (x) ≤ 0 ∀x ∈ Ω l
Gọi R là tập các điểm thuộc Ω l với V & (x) = 0 và M là tập bất biến lớn
nhất trong R. Thì mọi nghiệm x(t) xuất phát từ trong Ω l đều hội tụ về M khi
t → ∞.
Chú ý: M là tập bất biến lớn nhất theo nghĩa nó là hợp của tất cả các tập
bất biến trong R. Nếu R là tập bất biến (một khi V& (x) = 0 thì V
& (x) sẽ luôn bằng
0) thì M = R
Ý tưởng của định lý trên là hàm Lyapunov V dần dần sẽ không giảm nữa
&
(tức V(x) hội tụ đến 0).
Hình dưới đây minh hoạ khái niệm sự hội tụ đến tập bất biến M
V=l V
Ωl
R
x1
xo
M
x2
Hình 3.5. Sự hội tụ đến tập bất biến lớn nhất M

Hệ quả
Xét hệ ôtônôm (3.2) với f liên tục, V(x) là hàm vô hướng có đạo hàm
riêng liên tục. Giả sử trong lân cận Ω của điểm gốc
• V(x) là hàm xác định dương cục bộ
• V & (x) bán xác định âm
• Tập R định nghĩa bởi V& (x) = 0 chỉ gồm quỹ đạo x = 0.
Thì điểm cân bằng x* = 0 là ổn định tiệm cận.
Ngoài ra, miền liên thông lớn nhất Ω l (được định nghĩa bởi V(x) < l) nằm
trong Ω là một miền hút của điểm cân bằng.
78
Chú ý:
• Hệ quả trên thay thế điều kiện xác định âm của V & (x) trong định lý ổn
định tiệm cận cục bộ của Lyapunov bằng điều kiện bán xác định âm. Tuy
nhiên kết hợp với điều kiện thứ 3 về quỹ đạo trong R.
• Miền liên thông lớn nhất Ω l là một miền hút của điểm cân bằng, nó
không nhất thiết là miền hút duy nhất, vì hàm V không phải là duy nhất.
• Tập Ω không cần là miền hút. Định lý trên cũng không bảo đảm Ω l là
tập bất biến. Có thể có quỹ đạo xuất phát bên ngoài Ω l và bên trong Ω
nhưng cuối cùng lại ra bên ngoài Ω .
f. Định lý tập bất biến toàn cục
Xét hệ ôtônôm (3.2) với f liên tục, V(x) là hàm vô hướng có đạo hàm cấp
1 liên tục. Giả sử :
• V & (x) ≤ 0 trong toàn không gian trạng thái
• V(x) → ∞ khi x → ∞ .
Nếu R là tập hợp các điểm có V & (x) = 0 và M là tập bất biến lớn nhất
trong R, thì tất cả các nghiệm đều hội tụ tiệm cận toàn cục về M khi t → ∞
3.2 Bài toán điều khiển chuyển động của rôbốt

Nhiệm vụ của bài toán điều khiển chuyển động của rôbốt là đảm bảo
khâu thao tác chuyển động bám theo quỹ đạo cho trước trong không gian thao
tác
q3
q2
P
xP
q1
R0
Hình 3.6 Sơ đồ hoạt động của rôbốt
Cho biết chuyển động của khâu thao tác xd. Từ đó ta xác định được quan hệ:
x d = f (q) ⇒ q d = f −1 (x d ) (3.3)
Để có thể có được chuyển động của điểm thao tác P theo phương trình
định trước, ta có thể dùng các dạng điều khiển dưới đây:
79
- Hệ thống điều khiển trong không gian khớp (hình 3.7),

- Hệ thống điều khiển trong không gian thao tác (hình 3.8).
xd Động học qd Bộ điều Cơ cấu dẫn Động Tay x

ngược khiển động cơ máy
q
Cảm
biến
Hình 3.7. Sơ đồ tổng quát của hệ thống điều khiển không gian khớp
xd Bộ điều Cơ cấu dẫn Bàn kẹp x

khiển động Động cơ rôbốt
Cảm biến
Hình 3.8. Sơ đồ tổng quát của hệ thống điều khiển không gian thao tác
a. Bài toán điều khiển trong không gian khớp

Bài toán này được phân thành 2 bài toán nhỏ
• Bài toán động học ngược: Cho xd , tìm q = f −1 (x d )
• Hệ thống điều khiển trong không gian khớp được thiết kế để đảm bảo vị
trí khớp q luôn bám theo vị trí mong muốn qd, sao cho lượng sai lệch
q − q d → min
Ưu điểm của phương pháp này là bộ điều khiển tác động trực tiếp đến hệ
thống truyền động của các khớp. Nhược điểm của phương pháp là hệ thống điều
khiển này khó đảm bảo độ chính xác của vị trí khâu thao tác x − x d → min, do
sự tồn tại các sai lệch trong cơ cấu dẫn động (khe hở của các khớp, ma sát v.v),
hơn nữa thiếu thông tin về x − x d trong quá trình điều khiển
b. Bài toán điều khiển trong không gian thao tác
Hệ thống điều khiển trong không gian thao tác có chức năng làm cho sai
số giữa x và xd bằng không.
x − x d → min
80
Trong đó x d là véc tơ vị trí khâu thao tác mong muốn, x là véc tơ phản
hồi vị trí thực tế của khâu thao tác.
Ưu điểm của hệ thống điều khiển này là nó tác động trực tiếp tới các biến
của không gian thao tác x. Nhược điểm là trong quá trình điều khiển ta phải
đồng thời giải bài toán động học ngược, do đó khối lượng tính toán lớn, thời
gian điều khiển lâu.
3.3 Điều khiển trượt rôbốt dư dẫn động
Phương trình động lực học của rôbốt dạng chuỗi n bậc tự do có dạng
M (q)q&& + C(q, q& )q& + g(q) + d = τ (3.4)
Trong đó:
M(q) - ma trận khối lượng cỡ n×n,
C(q, q& )q& - véctơ cỡ n chứa các lực côriôlis và lực ly tâm,
g(q) - véctơ cỡ n chứa các lực do trọng trường,
d - véctơ chứa các lực/mômen do kích động nhiễu,
τ - véctơ chứa các lực/mômen của động cơ dẫn động.
Sơ đồ khối điều khiển rôbốt có dạng
d
Động q(t)
q d (t) τ Rôbốt
lực học +
Hình 3.9. Sơ đồ điều khiển rôbốt
Ta đưa vào các véc tơ sai số bám của véc tơ tọa độ suy rộng và của vận
tốc suy rộng
e( t ) = q ( t ) − q d ( t )
(3.5)
e& ( t ) = q& ( t ) − q& d ( t )
theo [63], để sử dụng phương pháp điều khiển trượt ta đưa vào véc tơ sai số suy
rộng như sau
s = e& ( t ) + Λe( t ) (3.6)
trong đó
Λ = diag(λ1 , λ 2 ,..., λ n ) , λ i > 0 (3.7)
Mục tiêu của bài toán điều khiển là e( t ) → 0 được chuyển sang hàm mục
tiêu s( t ) → 0 khi t → ∞
e( t ) → 0 ⇒ s( t ) → 0 khi t → ∞ (3.8)
81
Nếu s = 0 thì hệ phương trình vi phân (3.6) có dạng

e& i ( t ) + λ i e i ( t ) = 0 (i = 1 … n) (3.9)
Từ đó suy ra
de i ( t ) de
= −λ i e i ( t ) ⇒ i = −λ i dt
dt ei
Tích phân phương trình trên ta được
e i ( t ) = e i (0)e − λ t
i
(3.10)
Do đó nếu s i ( t ) → 0 thì e i ( t ) → 0 khi t → ∞
Bây giờ ta tìm luật điều khiển sao cho s i ( t ) → 0 khi t → ∞
Ta đặt
q& r ( t ) = q& d ( t ) − Λe( t )
suy ra
q& ( t ) − q& r ( t ) = q& ( t ) − q& d ( t ) + Λe( t )
(3.11)
= e& ( t ) + Λe( t ) = s( t )
từ đó ta có
s& ( t ) = q
&&( t ) − q && r ( t ) (3.12)
Để tìm luật điều khiển, ta chọn hàm Lyapunov như sau
1
V = s T M (q)s (3.13)
2
Đạo hàm V theo thời gian t ta được
V& = s T M (q)s& + 1 s T M & (q)s (3.14)
2
Từ (3.4) và chú ý đến (3.12) ta có
M (q )(q &&( t ) − q && r ( t )) + M (q)q&& r ( t ) + C(q, q& )q& + g (q) + d = τ
⇒ M (q )s& = τ − C(q, q& )q& − g (q) − M (q )q && r − d (3.15)
Từ (3.11) ta suy ra
q& (t ) = s + q& r (t ) (3.16)
Do đó ta có
C(q, q& )q& = C(q, q& )s + C(q, q& )q& r (3.17)
Thế (3.17 ) vào (3.15) được
M(q)s& = τ − C(q, q& )s − M(q)q && r − C(q, q& )q& r − g(q) − d (3.18)
Thay (3.18) vào (3.14) ta được
&& r − C(q, q& )q& r − g(q) − d] + s T ⎡⎢ M ⎤
& = s T [τ − M (q)q 1 &
V (q) − C(q, q& )⎥s (3.19)
⎣2 ⎦
1 &
Do tính chất s T M (q)s − s T C(q, q& )s = 0
2
82
Ta suy ra
V& = s T [τ − M (q)q && r − C(q, q& )q& r − g (q) − d ] (3.20)
Căn cứ vào (3.20) ta chọn luật điều khiển (mômen cần thiết để đảm bảo
chuyển động theo chương trình) như sau
τ=M ˆ (q)q ˆ (q, q& )q& + gˆ (q) − K s − K sgn(s)
&& r + C (3.21)
r pd s
Trong đó:
sgn(s) = [sgn(s 1 ), sgn(s 2 ),..., sgn(s n )] , và Kpd, Ks là các ma trận thực đối xứng
T
xác định dương, K pd = K Tpd > 0 , K s = K sT > 0

Để đơn giản ta chọn hai ma trận này có dạng là các ma trận đường chéo
K pd = diag{k 11pd , k 22 nn
pd ,..., k pd
} và K s = diag{k11s , k s22 ,..., k snn }
Với cách chọn (3.21), hệ thức (3.20) trở thành
V
~
& = −s T K pd s − s T K s sgn(s) + s T d + s T M [ ~
&& r + C(q, q& )q& r + ~
(q)q g (q) = ]
n n n n n
= −∑ k (pdii )s i2 − ∑ k s(ii ) | s i | + ∑ s i (d i + ρi ) ≤ −∑ k (pdii )s i2 − ∑ (k s(ii ) − | d i + ρi |) | s i |
i =1 i =1 i =1 i =1 i =1
với các sai lệch giữa mô hình thực sử dụng trong (3.4) và các thông số mô hình
sử dụng trong bộ điều khiển (3.21) như sau
~ ˆ (q) ⎫
M (q) = M (q) − M
⎪
~
C(q, q& ) = C(q, q& ) − Cˆ (q, q& )⎪⎬ (3.22)
~
g (q) = g (q) − gˆ (q) ⎪
⎪⎭
~
và ρ i = M (q)q [ ~
&& r + C(q, q& )q& r + ~
g (q) i ]
Như vậy để đảm bảo V & ≤ 0 thì ta phải chọn các phần tử của Ks sao cho
k s(ii ) > d i + ρi . Kpd chỉ là thành phần điều khiển PD được đưa thêm vào để rút
ngắn thời gian chuyển tiếp.
Do đặc điểm của hàm sgn(si) là không liên tục tại giá trị si = 0, do đó ở bộ
điều khiển trên sẽ xảy ra hiện tượng chattering. Một trong những cách khắc
phục thường thấy là làm trơn tính không liên tục của bộ điều khiển bằng cách
thay hàm dấu sgn(s) bằng hàm bão hoà (hình 3.10).
sat (s / ξ) = [sat (s1 / ξ), sat (s 2 / ξ),..., sat (s n / ξ)]T .
Sai số điều khiển trong trường hợp này phải chấp nhận tăng lên. Lúc này,
thành phần k (pdii ) và k s(ii ) trong phạm vi sai số suy rộng si nằm trong khoảng ξ đều
đóng vai trò là bộ điều khiển PD.
83
⎧⎪x x <1
Với sat ( x ) = ⎨
⎪⎩sgn( x ) x ≥1
Hình 3.10. Hàm sat

Ngoài ra, người ta có thể dùng hàm arctan (có căn chỉnh tỷ lệ) để thay
cho hàm sign (hình 3.11)
Hình 3.11. Hàm arctan

Ta có thể sử dụng thuật toán số trong việc giải bài toán điều khiển rôbốt dư
dẫn động trong Matlab/Simulink với sơ đồ thuật giải như trên hình vẽ 3.12.
84
Cho biết
M (q)q && + C(q, q& )q& + g (q) + d = τ
q
&& d ( t ), q& d ( t ), q d ( t ), q( t 0 ), q& ( t 0 )
T
Λ, K pd , K s , T, h =
N
tk : =t0
e( t k ) = q ( t k ) − q d ( t k )
In, vẽ đồ thị q( t k ) , q& ( t k )
e& ( t k ) = q& ( t k ) − q& d ( t k )
e( t k ), s( t k ) ; k=0,1,...N)
s( t k ) = e& ( t k ) + Λe( t k )
q& r ( t k ) = q& d ( t k ) − Λe( t k )

&& r ( t k ) = q
q && d ( t k ) − Λe& ( t k )
ˆ (q( t k ) )q
τ(t k ) = M ˆ (q( t ), q& ( t ) )q& ( t ) +
&& r ( t k ) + C k k r k
+ gˆ (q( t k ) ) − K pd s( t k ) − K s sgn (s( t k ) )
tk+1 := tk + h
Giải PTVP chuyển động từ t = t k → t = t k +1 = t k + h

T
với bước tích phân Δt =
N
&& + C(q, q& )q& + g (q) + d( t ) = τ ( t k )
M (q)q
Thu được q( t k +1 ) , q& ( t k +1 )
k:=k+1 Sai
k≥N
Đúng
KẾT THÚC
Hình 3.12 Sơ đồ tính toán và mô phỏng điều khiển rôbốt

85

Ví dụ 3.1. Điều khiển rôbốt phẳng 4 khâu động theo yêu cầu:
Chuyển động của điểm thao tác P có dạng :
xP = 0.8+0.1cos(2t) (m); yP = -0.8+0.1sin(2t) (m)
Các thông số động lực rôbốt 4 được cho trong bảng 3.1
y4
x2 x4
y0 y2
y3 T4
O T3 O
y1 C C P(x,y,z
T
q3
C q4
x1 O
q2
T1 x3
C
O
O x0
q1
Bảng 3.1 Các thông số động lực rôbốt 4 khâu động
Khâu m[kg] Iz[kgm2] a[m] l[m]

1 10 0.11 0.25 0.15
2 5 0.10 0.30 0.16
3 2 0.03 0.45 0.19
4 1 0.02 0.40 0.20
Chọn bước thời gian cho bài toán động học ngược và chu kỳ lấy mẫu của
bộ điều khiển h = Tc = 0.001 (s), sai số cho phép trong bài toán động học ngược
ε = 10 −6 (rad).
Các thông số của bộ điều khiển Λ = diag(100,100,50,50) ,
K s = diag(30,10,10,5) , K pd = diag(2,2,2,2) , ξ = 0.2. Ở đây, ta đưa thêm khâu
bão hòa vào bộ điều khiển với mục đích giới hạn mômen điều khiển của động
cơ luôn nằm trong giới hạn -100 … 100 (Nm).
86
Đồ thị nhiễu d(t) trên các khớp động được cho như các hình dưới
3
Nhieu1
2
Nhieu khop 1 (Nm)
-1
-2
-3
0 2 4 6 8 10
Thoi gian(s)
Hinh 3.14. Đồ thị các nhiễu tác động trên khớp 1
6
Nhieu2
4
Nhieu khop 2 (Nm)
-2
-4
0 2 4 6 8 10
Thoi gian(s)

87
Nhieu khop 3 (Nm) 2
-1
-2
Nhieu3
-3
0 2 4 6 8 10
Thoi gian(s)
1
Nhieu khop 4 (Nm)
-1
-2
-3 Nhieu4
-4
0 2 4 6 8 10
Thoi gian(s)

88
Áp dụng phần mềm Matlab ta tính được mômen phát động đặt tại các
khớp (các hình 3.18 đến 3.21). Trên các hình từ 3.22 đến 3.25 là đồ thị các sai
số suy rộng si. Trên các hình 3.26 và 3.27 là đồ thị tọa độ chuyển động thực của
khâu thao tác x(t), y(t) và đồ thị chuyển động mong muốn xd, yd theo chương
trình. Ta thấy sự chênh lệch chỉ xảy ra trong khoảng thời gian từ 0 đến 1 giây.
Sau đó quỹ đạo điều khiển bám sát quỹ đạo theo chương trình.
100
momen1
80
Momen khop 1 (Nm)
60
40
20
-20
0 2 4 6 8 10
Thoi gian(s)
Hình 3.18. Mômen điều khiển tại khớp 1 (Nm)
100
momen2
80
Momen khop 2 (Nm)
60
40
20
-20
0 2 4 6 8 10
Thoi gian(s)

89
100
momen3
80
Momen khop 3 (Nm)
60
40
20
-20
0 2 4 6 8 10
Thoi gian(s)
30
momen4
25
Momen khop 4 (Nm)
20
15
10
-5
0 2 4 6 8 10
Thoi gian(s)

90
5
s1
Sai so suy rong 1 (rad/s)
0
-5
-10
-15
0 2 4 6 8 10
Thoi gian(s)
Hình 3.22. Đồ thị sai số suy rộng s1
15
s2
10
-5
-10
0 2 4 6 8 10
Thoi gian(s)

91
0
-5 s3
-10
-15
-20
-25
0 2 4 6 8 10
Thoi gian(s)
5
s4
-5
-10
-15
0 2 4 6 8 10
Thoi gian(s)

92
0.9
Vi tri tren truc x (m)
0.8
0.7
0.6 xd
x
0.5
0 2 4 6 8 10
Thoi gian(s)
Hình 3.26. Đồ thị tọa độ x(t) trong không gian thao tác
-0.65
yd
-0.7 y
Vi tri tren truc y (m)
-0.75
-0.8
-0.85
-0.9
0 2 4 6 8 10
Thoi gian(s)
Hình 3.27. Đồ thị tọa độ y(t) trong không gian thao tác
93
Ví dụ 3.2. Điều khiển Rôbốt Scara theo yêu cầu:

Chuyển động của điểm thao tác P có dạng :
xP = 0.3+0.8cos(t) (m); yP = 0,3+0.08sin(t) (m);
π
ϕ = sin( 2t ) (rad).
2
Các thông số động lực của rôbốt được cho trong bảng 3.2
Hình 3.28. Rôbốt Scara
Bảng 3.2 Các thông số động lực rôbốt Scara

Khâu m[kg] Iz[kgm2] a[m] d[m] l[m]
1 10 0.21 0.22 0.15
2 5 0.20 0.40 0.16
3 2 0.18 0.19
4 1 0.15 0.09 0.04
94
Đồ thị nhiễu d(t) như hình dưới
3
Nhieu1
2
Nhieu khop 1 (Nm)
-1
-2
-3
0 2 4 6 8 10
Thoi gian(s)
Hình 3.29. Đồ thị nhiễu trên khớp động 1
6
Nhieu2
4
Nhieu khop 2 (Nm)
-2
-4
0 2 4 6 8 10
Thoi gian(s)

95
2
Nhieu khop 3 (N) 1
-1
-2 Nhieu3
-3
0 2 4 6 8 10
Thoi gian(s)
2
Nhieu khop 4 (Nm)
-1
-2
-3
Nhieu4
-4
0 2 4 6 8 10
Thoi gian(s)

Chọn bước thời gian cho bài toán động học ngược và chu kỳ lấy mẫu của
bộ điều khiển h = Tc = 0.001 (s), sai số cho phép trong bài toán động học
ngược ε = 10 −6 (rad).
Các thông số của bộ điều khiển Λ = diag(5,5,5,5) , K s = diag(10,10,40,10) ,
K pd = diag(2,2,2,2) , ξ = 0.05.
96
Từ đó ta thu được đồ thị tọa độ của bàn kẹp trong không gian thao tác, sai
số suy rộng s và mômen điều khiển tại các khớp.
10
Momen khop 1 (Nm) momen1
-5
0 2 4 6 8 10
Thoi gian(s)
15
momen2
10
Momen khop 2 (Nm)
-5
0 2 4 6 8 10
Thoi gian(s)

97
10
luc 3
0
Luc khop 3 (N)
-10
-20
-30
-40
0 2 4 6 8 10
Thoi gian(s)
Hình 3.35. Lực điều khiển tại khớp 3 (N)
10
momen4
5
Momen khop 4 (Nm)
-5
-10
-15
-20
0 2 4 6 8 10
Thoi gian(s)

98
0.2
s1
0
-0.2
-0.4
-0.6
-0.8
0 2 4 6 8 10
Thoi gian(s)
Hình 3.37. Sai số suy rộng s1
0.2
s2
0
-0.2
-0.4
-0.6
-0.8
0 2 4 6 8 10
Thoi gian(s)

99
0
s3
Sai so suy rong 3 (m/s)
-0.2
-0.4
-0.6
-0.8
-1
0 2 4 6 8 10
Thoi gian(s)
5
s4
4
-1
0 2 4 6 8 10
Thoi gian(s)

100
0.45
xd
0.4
Vi tri tren truc x (m) x
0.35
0.3
0.25
0.2
0 2 4 6 8 10
Thoi gian(s)
Hình 3.41. Đồ thị tọa độ x(t) trong không gian thao tác
0.4
0.35
Vi tri tren truc y (m)
0.3
0.25
yd
y
0.2
0 2 4 6 8 10
Thoi gian(s)
Hình 3.42. Đồ thị tọa độ y(t) trong không gian thao tác
101
0.3
0.25
Vi tri tren truc z (m)
0.2
zd
0.15
z
0.1
0.05
-0.05
0 2 4 6 8 10
Thoi gian(s)
Hình 3.43. Đồ thị tọa độ z(t) trong không gian thao tác
2
phid
phi
1
Vi tri goc quay (rad)
-1
-2
0 2 4 6 8 10
Thoi gian(s)
Hình 3.44. Đồ thị góc quay φ(t) của bàn kẹp trong không gian thao tác (rad)
Tương tự như ở ví dụ trước, qua các hình vẽ (từ hình 3.29 đến hình 3.44)
ta thấy thời gian cần thiết để bàn kẹp của rôbốt bám được quỹ đạo mong muốn
là vào khoảng 1 giây và mặc dù có các sai số về cấu trúc và có các nhiễu trên
các khớp nhưng rôbốt vẫn bám theo quỹ đạo mong muốn.
102

Dựa trên thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng đã trình
bày ở chương 1, trong chương này trình bày việc vận dụng phương pháp điều
khiển trượt vào bài toán điều khiển chuyển động rôbốt dư dẫn động trong không
gian khớp. Chúng tôi chọn phương án điều khiển rôbốt dư dẫn động trong
không gian khớp vì phương án này có thời gian điều khiển nhanh hơn so với
phương án điều khiển trong không gian thao tác do không phải giải bài toán
động học ngược trong quá trình điều khiển, do đó phù hợp hơn với điều khiển
thời gian thực. Kết quả mới của chương này là trong phần giải bài toán động
học ngược đã sử dụng thuật toán hiệu chỉnh véc tơ tọa độ suy rộng. Kết quả giải
bài toán động học ngược là đầu vào của bài toán điều khiển chuyển động trong
không gian khớp. Ngoài ra các ví dụ trình bày trong chương này là các ví dụ
mới.
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 ĐO BKHN-MCX-04
Để phục vụ cho luận án một rôbốt đo đã được thiết kế chế tạo tại Bộ môn
Cơ khí chính xác và Quang học Trường Đại học Bách khoa Hà nội, chủ trì thiết
kế, chế tạo là TS. Nguyễn Văn Vinh. Rôbốt đo này được quy định gọi là rôbốt
đo BKHN-MCX-04. Chúng tôi có tham gia một phần thiết kế chế tạo và tham
gia thực hiện các thí nghiệm tại Trường Đại học Bách khoa Hà nội. Trên cơ sở
rôbốt chế tạo, trong chương này áp dụng các thuật toán đã trình bày trong các
chương 1, 2 và 3 để tính toán phân tích động học ngược, động lực học ngược và
điều khiển chuyển động rôbốt đo BKHN-MCX-04.
4.1 Tổng quan về phương pháp đo độ chính xác hình học của chi tiết máy
Trong lĩnh vực đo lường, kiểm tra thì đo lường và kiểm tra tự động đang
rất phát triển. Đưa những thành tựu của các ngành điện tử và tin học áp dụng
vào việc ghép nối giữa máy tính và máy công tác đã cho phép chuyển thẳng số
liệu đo vào máy tính để xử lý và đưa ra kết quả của phép đo một cách nhanh
chóng. Các thiết bị đo tọa độ như: Máy đo tọa độ Đề các, tọa độ độc cực…đã
hoàn thiện các khả năng đo lường vốn có của chúng, đặc biệt trong các phép đo
gián tiếp có khối lượng xử lý thông tin lớn. Với khả năng xác định kích thước,
hình dáng, vị trí một cách dễ dàng, với độ chính xác cao trong một khoảng thời
gian ngắn và khả năng tiếp cận đối tượng phong phú, với những phần mềm xử
lý thích hợp, có thể đo những bề mặt phức tạp. Máy đo tọa độ đã góp phần quan
trọng trong việc đảm bảo nâng cao chất lượng sản phẩm và đem lại hiệu quả
cao.
Tuy nhiên, trong các trường hợp sau đây thì máy đo tọa độ Đề các không
thể đáp ứng được:
- Những vật đo có kích thước lớn, nặng, không thể di chuyển đến bàn
đo của máy đo tọa độ Đề các.
- Khi cần đo những chi tiết đã lắp ráp vào máy ở các công trình mà không
thể nào tháo ra được.
- Đo ở các vị trí có sự hạn chế về không gian như đo bên trong khoang
máy, đo trong những chi tiết dạng ống v.v…
Các rôbốt đo đã khắc phục được những hạn chế nêu trên. Với kết cấu gọn
nhẹ, tính cơ động cao, dễ mang vác, tầm với rộng. Thay vào việc phải di chuyển
chi tiết trên bàn đo của máy đo tọa độ Đề các, rôbốt đo có thể tiếp cận đối tượng
đo trong mọi điều kiện của không gian đo.
Rôbốt đo theo tọa độ cực có thể đo được các chi tiết có hình dạng cơ bản
như: hình tròn, hình trụ, mặt phẳng, đường thẳng v.v…trong phạm vi kích thước
104
200x200x200mm của các chi tiết. Khi thực hiện phép đo, trước hết cần đặt đầu
đo của rôbốt vào vị trí ban đầu rồi cho chạy chương trình, các khớp sẽ được
điều khiển và di chuyển đầu đo đến vị trí cần đo và đồng thời có tín hiệu phản
hồi khi đầu đo chạm vào bề mặt chi tiết cần đo.
4.2 Kết cấu của rô bốt đo BKHN-MCX-04
Kết cấu của rôbốt đo BKHN-MCX-04 được minh họa bởi mô hình trên
hình 4.1. Rôbốt được thiết kế với sáu khâu động và các khớp quay, tạo thành
một hệ có cấu trúc động học mạch hở được biểu diễn trên hình 4.2. Rôbốt có
sáu động cơ servo dẫn động độc lập. Động cơ thứ nhất dẫn động khâu 1 quay
xung quanh trục cố định theo phương thẳng đứng. Trục quay của các động cơ
dẫn động các khâu 2, 3 và 4 được bố trí song song với nhau và cùng vuông góc
với trục quay động cơ dẫn động khâu 5. Các thông số thiết kế (độ dài) của các
khâu của rôbốt được cho trong bảng 4.1.
3
4 Quỹđạo
Quỹ đạođịnh
địnhtrước
2
5 đối tượng
đối tượng cần đo
1 6
Hình 4.1. Mô hình rôbốt đo BKHN-MCX-04
Trong khi bốn động cơ đầu tiên có nhiệm vụ đưa điểm O5 (tâm trục của
khớp quay giữa khâu 5 và khâu 6) dịch chuyển theo một quĩ đạo không gian
được định trước theo yêu cầu về phép đo, động cơ thứ 5 làm thay đổi hướng của
105
khâu 5 cho phù hợp với hướng của bề mặt vật cần đo. Động cơ thứ 6 có nhiệm
vụ dẫn động khâu 6 (que thăm dò) để mũi dò (có vị trí tại điểm O6) tiếp xúc với
bề mặt vật cần đo. Với phương án thiết kết như vậy, rôbốt đo BKHN-MCX-04
có khả năng tạo ra các chuyển động của mũi dò một cách linh hoạt, phù hợp với
các phép đo các bề mặt theo các phương khác nhau.
O3 ≡ O4
x2 q4 x3
O2
q3 x4
E ≡ O5
y1 z5
q5
z4
q2 q6
x1
O1
x5 O6
z0 x6
y0
q1
O x0
Hình 4.2. Sơ đồ động học rôbốt đo BKHN-MCX-04
Bảng 4.1. Thông số hình học của rôbốt

Khâu i Khoảng cách Oi-1O i (m)
1 0.14
2 0.15
3 0.20
4 0.0
5 0.163
6 0.080
106
4.3 Tính toán động học ngược

Hình 4.2 mô tả các hệ tọa độ động gắn với các khâu dựa trên quy tắc
chọn hệ trục tọa độ của Denavit-Hartenberg. Các điểm gốc Oi (i = 1,2..., 5) của
các hệ trục tọa độ đều nằm trong cùng một mặt phẳng thẳng đứng.
Nếu xét tổng thể toàn bộ hệ với 6 dẫn động, vị trí và hướng của khâu 6
(khâu thao tác) được xác định trên hệ tọa độ cố định theo 5 tham số: ba tọa độ
Đề-các của điểm gốc O5 và hai góc quay xác định hướng (thí dụ các góc q5, q6).
Như vậy số dư dẫn động của rôbốt là 1.
Do đặc điểm kết cấu của rôbốt được phân tích ở trên, việc điều khiển các
biến khớp qi (i = 1,2..., 5) để điểm O5 chuyển động trên một quĩ đạo định trước
(để thuận tiện ta ký hiệu O5 ≡ E) có vai trò quan trọng đối với các thao tác đo.
Chú ý rằng sự thay đổi trị số của góc quay q5 sẽ không có ảnh hưởng đến vị trí
của điểm E nhưng sẽ làm thay đổi hướng của khâu 5 và tạo ra sự thay đổi hướng
của khâu cuối 6. Bởi vậy, nhiệm vụ phân tích động học ngược của rôbốt được
trình bày trong mục này tập trung vào việc tính toán các trị số của các biến khớp
qi và các đạo hàm q& i , &q& i (i = 1,2,K,5) dựa trên một quĩ đạo định trước của điểm
E.
Bảng 4.2. Bảng tham số động học DH
Khâu i θi di ai αi
1 q1 d1 0 π/2
2 q2 0 a2 0
3 q3 0 a3 0
4 q4 0 0 -π/2
5 q5 d5 0 π/2
Từ hình 4.2 ta có thể thiết lập được bảng các tham số động học DH như
trong bảng 4.2, trong đó các hằng số d1 = OO1 , a 2 = O1O 2 , a 3 = O 2 O 3 , d 5 = O 4 O 5
Các giá trị li (i=1,2,...,5) được cho trong bảng 4.1. Từ bảng tham số DH ta được
các ma trận biến đổi hệ trục
⎡C1 0 S1 0⎤ ⎡C 2 − S2 0 a 2C2 ⎤ ⎡C 3 − S3 0 a 3C3 ⎤
⎢S 0 − C1 0 ⎥ ⎢S C2 0 a 2S2 ⎥ ⎢S C3 0 a 3 S3 ⎥
H1 = ⎢ 1 ⎥, H 2 = ⎢ 2 ⎥, H 3 = ⎢ 3 ⎥
⎢0 1 0 d1 ⎥ ⎢0 0 1 0 ⎥ ⎢0 0 1 0 ⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎣0 0 0 1⎦ ⎣0 0 0 1 ⎦ ⎣0 0 0 1 ⎦
107
⎡C 4 0 − S4 0⎤ ⎡C 5 0 S5 0⎤
⎢S 0 C4 0⎥ ⎢S 0 − C5 0⎥
H4 = ⎢ 4 ⎥, H 5 = ⎢ 5 ⎥
⎢0 −1 0 0⎥ ⎢0 −1 0 d5 ⎥
⎢ ⎥ ⎢ ⎥
⎣0 0 0 1⎦ ⎣0 0 0 1⎦
trong đó ta sử dụng ký hiệu rút gọn Si = sin(q i ) , C i = cos(q i ) . Ma trận chuyển
tọa độ một điểm từ hệ tọa độ cố định {Ox 0 y 0 z 0 } sang hệ tọa độ {O 5 x 5 y 5 z 5 } có
dạng
D5 = H1 H2 H3 H4 H5
⎡C1C 234 C5 − S1S5 − C1S234 S1C5 + C1S5 C 234 − d 5 C1S234 + a 2 C1C 2 + a 3 C1C 23 ⎤
⎢S C C + C S − S S − C1C 5 + S1S5 C 234 − d 5S1S234 + a 2S1C 2 + a 3S1C 23 ⎥
= ⎢ 1 234 5 1 5 1 234
⎥
⎢ S234 C5 C 234 S5S234 d1 + d 5 C 234 + a 2S2 + a 3S23 ⎥
⎢ ⎥
⎣ 0 0 0 1 ⎦
trong đó ta sử dụng các ký hiệu rút gọn: Sijk = sin(q i + q j + q k ) và
C ijk = cos(q i + q j + q k ) . Từ các phần tử của ma trận D5 ta xác định được các tọa
độ của điểm E trên hệ tọa độ cố định
x E = a 2 C1C 2 + a 3 C1C 23 − d 5 C1S234
y E = a 2S1C 2 + a 3S1C 23 − d 5S1S234 (4.1)
z E = a 2S2 + a 3S23 + d 5 C 234 + d1
Phương trình (4.1) biểu diễn quan hệ
x = f (q) (4.2)
trong đó x = [x E , y E , z E ] và q = [q q , q 2 , q 3 , q 4 , q 5 ] . Từ (4.1) ta xác định được
T T
ma trận Jacobi J(q) biểu diễn quan hệ x& = J (q) q& như sau
⎡ ∂x E ∂x E ∂x E ∂x E ∂x E ⎤
⎢ ∂q ∂q 2 ∂q 3 ∂q 4 ∂q 5 ⎥⎥ ⎡ J
⎢ 1 11
J12 J13 J14 J15 ⎤
∂x ⎢ ∂y E ∂y E ∂y E ∂y E ∂y E ⎥ ⎢
J (q) = = = J 21 J 22 J 23 J 24 J 25 ⎥ (4.3)
∂q ⎢ ∂q1 ∂q 2 ∂q 3 ∂q 4 ∂q 5 ⎥ ⎢ ⎥
⎢ ∂z ∂z E ∂z E ∂z E ∂z E ⎥ ⎢⎣J 31 J 32 J 33 J 34 J 35 ⎥⎦
⎢ E ⎥
⎢⎣ ∂q1 ∂q 2 ∂q 3 ∂q 4 ∂q 5 ⎥⎦
Trong đó các phần tử của ma trận J(q) có dạng
J11 = -d5 S1 S234 - a3 S1 C23 - a2 S1 C2
108
J12 = -d5 C1 C234 - a3 C1 S23 - a2 C1 S2

J13 = -d5 C1 C234 - a3 C1 S23
J14 = -d5 C1 C234
J15 = 0
J21 = -d5 C1 S234 + a3 C1C23 + a2 C1C2
J22 = -d5 S1C234 + a3 S1 S23 + a2 S1 S2
J23 = -d5 S1C234 + a3 S1 S23
J24 = -d5 S1C234
J25 = 0
J31 = 0
J32 = -d5 S234 + a3 C23 + a2 C2
J33 = -d5 S234 + a3 C23
J34 = -d5 S234
J35 = 0
Với ma trận J được xác định theo hệ thức (4.3) và một quĩ đạo định trước
của điểm E, ta có thể xây dựng một chương trình tính toán các giá trị biến khớp
qi và các đạo hàm q& i , &q& i (i = 1,2,K,5) dựa trên thuật giải đã được trình bày trong
chương 1.
Để minh họa cho việc tính toán động học ngược bằng tính toán mô phỏng
số, ta xét một thí dụ ứng với điểm E có quĩ đạo là một đường xoắn ốc bám trên
một mặt cầu. Quy luật chuyển động có dạng
⎛t⎞
x E = 3 + 0.1sin(2πt ) sin⎜ ⎟
⎝ 3⎠
⎛t⎞
y E = 0.1cos(2πt ) sin⎜ ⎟ (4.4)
⎝ 3⎠
⎛t⎞
z E = 0.12 + 0.1sin⎜ ⎟
⎝ 3⎠
trong đó các tọa độ được tính theo đơn vị mét.
Quá trình tính toán trị số các biến khớp được thực hiện trên chương trình
π
tính MATLAB trong khoảng thời gian tính 0 ≤ t ≤ (s) với bước thời gian h =
15
0.001 (s) và sai số tính toán ε = 10-6 (rad). Các kết quả tính toán được biểu diễn
trên các hình 4.4, 4.5 và 4.6, trong đó các đồ thị góc quay qi được biểu diễn trên
hình 4.4, hình 4.5 là đồ thị vận tốc góc q& i và hình 4.6 chứa các đồ thị gia tốc góc
109
&q& i . Như đã phân tích ở trên, tham số góc quay q5 là hằng số và không có ảnh
hưởng đến quĩ đạo chuyển động của điểm E. Các kết quả này có thể sử dụng
trong việc điều khiển động học và là dữ liệu cho phân tích động lực học ngược.
Quĩ đạo
Quỹ đạo điểm
điểm E
Hình 4.3. Quỹ đạo định trước của điểm E (đường xoắn ốc)
Hình 4.4. Đồ thị biến khớp q

110
Hình 4.5. Đồ thị vận tốc góc q&
Hình 4.6. Đồ thị gia tốc góc q

&&
111
4.4 Tính toán động lực học ngược

Như đã đề cập tới trong các chương trước, mục tiêu của bài toán phân
tích động lực học ngược là xác định trị số của các mômen động cơ ứng với một
quy luật chuyển động cho trước (về quĩ đạo và vận tốc) của điểm E. Mặc dù
khâu thứ 6 (que đo) dùng để thao tác đo và được điều khiển độc lập với phần
còn lại của hệ, khâu này vẫn có ảnh hưởng khi tính toán động lực học. Bởi vậy
tất cả 6 khâu của rôbốt phải được tính đến trong các phương trình vi phân
chuyển động.
Trị số mômen động cơ τ = [τ1 , τ 2 , τ 3 , τ 4 , τ 5 , τ 6 ] được xác định từ các
T
phương trình động lực mô tả chuyển động của rôbốt đo. Trong trường hợp bỏ
qua ma sát tại các khớp và các lực cản khác, đồng thời coi lực tác động của vật
cần đo lên khâu 6 tại vị trí tiếp xúc là nhỏ không đáng kể, phương trình động
lực được biểu diễn dưới dạng tổng quát
&& + C(q, q& )q& + g (q) = τ
M (q)q (4.5)
Để thiết lập các phương trình này ta sử dụng các ký hiệu:
mi khối lượng của khâu i
ri = [x C , y C , z C ] vị trí khối tâm khâu i trên hệ cố định {Ox 0 y 0 z 0 }
T
i i i
r = [x , y , z (Ci ) ] vị trí khối tâm khâu i trên hệ động {Ox i y i z i }

(i) (i ) (i ) T
i Ci Ci i
v i = r&i vận tốc khối tâm khâu i

ωi véctơ vận tốc góc khâu i biểu diễn trên hệ cố định
ωi( i ) véctơ vận tốc góc khâu i biểu diễn trên hệ động
∂ri
J Ti (q) = ma trận Jacobi tịnh tiến của khâu i
∂q
∂ω i
J Ri (q) = ma trận Jacobi quay của khâu i
∂q&
Ai ma trận côsin chỉ hướng của khâu i
(i )
I i ma trận quán tính khối của khâu i tại khối tâm
của khâu và tính trên hệ tọa độ động
Ii ma trận quán tính khối của khâu i tại khối tâm
của khâu và tính trên hệ tọa độ cố định
Các số hạng trong phương trình (4.5) được xác định theo các bước sau
đây:
112
1. Xác định các ma trận côsin chỉ hướng Ai

Các ma trận biến đổi tọa độ của các khâu đối với hệ tọa độ cố định
⎡C1 0 S1 0⎤
⎢S 0 − C1 0 ⎥
D1 = H1 = ⎢ 1 ⎥,
⎢0 1 0 d1 ⎥
⎢ ⎥
⎣0 0 0 1⎦
⎡C1C 2 − C1S2 S1 a 2 C1C 2 ⎤
⎢S C − S1S2 − C1 a 2S1C 2 ⎥
D 2 = D1H 2 = ⎢ 1 2 ⎥
⎢ S2 C2 0 a 2S2 + d1 ⎥
⎢ ⎥
⎣ 0 0 0 1 ⎦
⎡C1C 23 − C1S23 S1 a 2 C1C 2 + a 3 C1C 23 ⎤
⎢S C − S1S23 − C1 a 2S1C 2 + a 3S1C 23 ⎥
D3 = D 2 H 3 = ⎢ 1 23 ⎥
⎢ S23 C 23 0 a 2S2 + a 3S23 + d1 ⎥
⎢ ⎥
⎣ 0 0 0 1 ⎦
⎡C1C 234 − S1 − C1S234 a 2 C1C 2 + a 3 C1C 23 ⎤
⎢S C C1 − S1S234 a 2S1C 2 + a 3S1C 23 ⎥
D 4 = D3 H 4 = ⎢ 1 234 ⎥
⎢ S234 0 C 234 a 2S2 + a 3S23 + d1 ⎥
⎢ ⎥
⎣ 0 0 0 1 ⎦
⎡C1C 234 − S1S5 − C1S234 S1C 5 + C1S5 C 234 a 2 C1C 2 + a 3C1C 23 − d 5 C1S234 ⎤
⎢S C + C S − S1S234 − C1C5 + S1S5 C 234 a 2S1C 2 + a 3S1C 23 − d 5S1S234 ⎥
D5 = ⎢ 1 234 1 5
⎥
⎢ S234 C5 C 234 C5S234 a 2S2 + a 3S23 + d1 + d 5 C 234 ⎥
⎢ ⎥
⎣ 0 0 0 1 ⎦
Ma trận côsin chỉ hướng Ai được xác định từ 9 thành phần của ma trận
3x3 ở phía trên bên trái của các ma trận biến đổi tọa độ Di. Véctơ tọa độ rOi của
điểm gốc Oi của hệ tọa độ động nằm trong véctơ 3x1 phía trên bên phải của Di.
⎡C1 0 S1 ⎤ ⎡0⎤
A1 = ⎢ S1 0 − C1 ⎥, rO = ⎢ 0 ⎥
⎢ ⎥ ⎢ ⎥ 1
⎢⎣ 0 1 0 ⎥⎦ ⎢⎣d1 ⎥⎦
113
⎡C1C 2 − C1S2 S1 ⎤ ⎡ a 2 C1C 2 ⎤

A 2 = ⎢ S1C 2 − S1S2 − C1 ⎥, rO = ⎢ a 2S1C 2 ⎥
⎢ ⎥ 2⎢ ⎥
⎢⎣ S2 C2 0 ⎥⎦ ⎢⎣a 2S2 + d1 ⎥⎦
⎡C1C 23 − C1S23 S1 ⎤ ⎡a 2 C1C 2 + a 3 C1C 23 ⎤

A 3 = ⎢ S1C 23 − S1S23 − C1 , rO = ⎢ a 2S1C 2 + a 3S1C 23 ⎥
⎥
⎢ ⎥ ⎢ 3 ⎥
⎣⎢ S23 C 23 0 ⎥⎦ ⎢⎣ d1 + a 2S2 + a 3S23 ⎦⎥
⎡C1C 234 − S1 − C1S234 ⎤ ⎡a 2 C1C 2 + a 3 C1C 23 ⎤

A 4 = ⎢ S1C 234 C1 − S1S234 , rO = ⎢ a 2S1C 2 + a 3S1C 23 ⎥
⎥
⎢ ⎥ ⎢ 4 ⎥
⎢⎣ S234 0 C 234 ⎥⎦ ⎢⎣ d1 + a 2S2 + a 3S23 ⎥⎦
⎡C1C 234 C5 − S1S5 − C1S234 S1C 5 + C1S5 C 234 ⎤

A 5 = ⎢S1C 234 C 5 + C1S5 − S1S234 − C1C5 + S1S5 C 234 ⎥
⎢ ⎥
⎢⎣ S234 C5 C 234 S5S234 ⎥⎦
⎡− d 5 C1S234 + a 2 C1C 2 + a 3C1C 23 ⎤
rO = ⎢ − d 5S1S234 + a 2S1C 2 + a 3S1C 23 ⎥
5 ⎢ ⎥
⎢⎣ d1 + d 5 C 234 + a 2S2 + a 3S23 ⎥⎦
⎡− C6S1S5 − S6C1S234 + C6C5C1C234 S6S1S5 − C6C1S234 − S6C5C1C234 S1C5 + C1S5C234 ⎤

A6 = ⎢ C6S5C1 − S6S1S234 + C6C5S1C234 − S6S5C1 − C6S1S234 − S6C5S1C234 − C1C5 + S1S5C234⎥
⎢ ⎥
⎢⎣ C5C6S234 + S6C234 − C5S6S234 + C6C234 S5S234 ⎥⎦
⎡− a6C6S1S5 + a 2C1C2 − (a 6S6C1 − d5C1 )S234 + a 6C6C5C1C234 + a3C1C23 ⎤
rO = ⎢ a6C6C1S5 + a 2S1C2 − (a 6S6S1 − d5S1 )S234 + a 6C6C5S1C234 + a3S1C23 ⎥
6 ⎢ ⎥
⎢⎣ a 6C5C6S234 + (a6S6 + d5 )C234 + a3S23 + a 2S2 + d1 ⎥⎦
2. Xác định tọa độ khối tâm của các khâu theo công thức tổng quát
ri = rOi + A i ri( i ) , (i = 1,2,K,6) (4.6)
trong đó véctơ rOi và ma trận A i được xác định từ bước 1. Véctơ ri( i ) được cho
trước từ các thông số động lực học của rôbốt như trên hình 4.7 và trong bảng
4.3.
114
O3 ≡ O4
x2 x3
l3
C4
O2 l4
C3 C5
C2 x4 l5 O5
y1 z4
l6
l2 x1 C6
O1 O6
C1 x6
l1
O
Hình 4.7. Vị trí trọng tâm các khâu

Bảng 4.3. Vị trí khối tâm khâu i của rôbốt trên hệ động
Vị trí trọng tâm
Khâu i
x (Cii ) y (Cii ) z (Cii )
1 0 -(d1-l1) 0
2 -(a2-l2) 0 0
3 -(a3-l3) 0 0
4 0 0 l4
5 0 -(d5-l5) 0
6 -(a6-l6) 0 0
3. Tính toán ma trận Jacobi tịnh tiến của các khâu dựa trên các đạo hàm
riêng theo biến q, thí dụ như
⎡0 0 0 0 0 0 ⎤
∂r1 ⎢
J T1 = = 0 0 0 0 0 0⎥
∂q ⎢ ⎥
⎢⎣0 0 0 0 0 0⎥⎦
115
⎡− l 2S 2 C 2 − l 2 S 2 C1 0 0 0 0⎤
∂r2 ⎢
J T2 = = l 2 C1 C 2 − l 2S1S2 0 0 0 0⎥
∂q ⎢ ⎥
⎢⎣ 0 l2C2 0 0 0 0⎥⎦
⎡− l 3S1C123 − a 2S1C 2 − l 3 C1S23 − a 2 C1S2 − l 3 C1S23 0 0 0⎤

∂r3 ⎢
J T3 = = l 3 C1C 23 + a 2 C1C 2 − l 3S1S23 − a 2S1S2 − l 3S1S23 0 0 0⎥
∂q ⎢ ⎥
⎣⎢ 0 l 3 C 23 + a 2 C 2 l 3 C 23 0 0 0⎥⎦
4. Tính toán vận tốc góc của các khâu

Trước hết ta thiết lập biểu thức toán tử sóng của các véctơ vận tốc góc
⎡ 0 0 q& 1 ⎤
& =⎢ 0 0 0⎥
~ (1) = A T A
ω 1 1 1
⎢ ⎥
⎢⎣− q& 1 1 0 ⎥⎦
⎡ 0 − q& 2 C 2 q& 1 ⎤
~ ( 2)
ω & = ⎢ q&
= A T2 A 0 − S2 q& 1 ⎥
2 2
⎢ 2 ⎥
⎢⎣− C 2 q& 1 S2 q& 1 0 ⎥⎦
⎡ 0 − q& 2 − q& 3 C 23q& 1 ⎤

~ ( 3)
ω = A 3 A 3 = ⎢q& 2 + q& 3
T &
0 − S23q& 1 ⎥
3
⎢ ⎥
⎢⎣− C 23q& 1 S23q& 1 0 ⎥⎦
⎡ 0 − C 234 q& 1 − q& 2 − q& 3 − q& 4 ⎤

~ T &
ω4 = A4 A4 =
( 4) ⎢ C 234 q& 1 0 − S234 q& 1 ⎥
⎢ ⎥
⎢⎣q& 2 + q& 3 + q& 4 S234 q& 1 0 ⎥⎦
~ (5) = AT A
ω & =
5 5 5
⎡ 0 − S5S234q& 1 − C5 (q& 2 + q& 3 + q& 4 ) q& 5 + C234q& 1 ⎤

⎢
= S5S234q& 1 + C5 (q& 2 + q& 3 + q& 4 ) 0 − C5S234q& 1 + S5 (q& 2 + q& 3 + q& 4 )⎥
⎢ ⎥
⎢⎣ − q& 5 − C234q& 1 C5S234q& 1 − S5 (q& 2 + q& 3 + q& 4 ) 0 ⎥⎦
Từ đó ta suy ra vận tốc góc của các khâu
⎡0⎤ ⎡S2q& 1 ⎤ ⎡ S23q& 1 ⎤ ⎡ S234q& 1 ⎤
ω1(1) = ⎢q& 1 ⎥ , ω(22) = ⎢C2q& 1 ⎥ , ω3(3) = ⎢ C23q& 1 ⎥ , ω(44) = ⎢− (q& 2 − q& 3 − q& 4 )⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢⎣ 0 ⎥⎦ ⎢⎣ q& 2 ⎥⎦ ⎢⎣q& 2 + q& 3 ⎥⎦ ⎢⎣ C234q& 1 ⎥⎦
116
⎡C5S234q& 1 − S5 (q& 2 + q& 3 + q& 4 )⎤

ω5(5) = ⎢ q& 5 + C234q& 1 ⎥
⎢ ⎥
⎢⎣S5S234q& 1 + C5 (q& 2 + q& 3 + q& 4 )⎥⎦
⎡ C5C6S234q& 1 + S6C234q& 1 − S5C6 (q& 2 + q& 3 + q& 4 ) + S6q& 5 ⎤

ω = ⎢− C5S6S234q& 1 + C6C234q& 1 + S5S6 (q& 2 + q& 3 + q& 4 ) + C6q& 5 ⎥
(6)
6
⎢ ⎥
⎣⎢ q6 + S5S234q1 + C5 (q2 + q3 + q4 )
& & & & & ⎦⎥
5. Tính toán các ma trận Jacobi J Ri
Trước hết ta tính toán các ma trận Jacobi quay J (Rii ) tương ứng với ωi( i ) như
sau
⎡0 0 0 0 0 0 ⎤
∂ω1(1) ⎢
J = (1)
= 1 0 0 0 0 0⎥ ,
R1
∂q& ⎢ ⎥
⎢⎣0 0 0 0 0 0⎥⎦
⎡ S 2 0 0 0 0 0⎤
∂ω (22 ) ⎢
JR2 =
( 2)
= C 2 0 0 0 0 0⎥ ,
∂q& ⎢ ⎥
⎢⎣ 0 1 0 0 0 0⎥⎦
⎡ S23 0 0 0 0 0⎤
∂ω 3( 3) ⎢
J ( 3)
= = C 23 0 0 0 0 0⎥ ,
R3
∂q& ⎢ ⎥
⎢⎣ 0 1 1 0 0 0⎥⎦
⎡ S 234 0 0 0 0 0⎤
∂ω (44 ) ⎢
J (R44) = = 0 −1 −1 − 1 0 0⎥ ,
∂q& ⎢ ⎥
⎢⎣C 234 0 0 0 0 0⎥⎦
⎡S 234 C 5 − S5 − S5 − S5 0 0⎤
∂ω 5( 5 ) ⎢
J (R55) = = C 234 0 0 0 1 0⎥ ,
∂q& ⎢ ⎥
⎢⎣ S234S5 C5 C5 C5 0 0⎥⎦
⎡S6 C 234 + S 234 C 5 C 6 − S5 C 6 − S5 C 6 − S5 C 6 S6 0⎤

∂ω (66 ) ⎢
J (R66) = = C 6 C 234 − S 234 C 5S6 S5S 6 S5S 6 S5S 6 C6 0⎥
∂q& ⎢ ⎥
⎢⎣ S234S5 C5 C5 C5 0 1⎥⎦
Ma trận Jacobi quay J Ri tương ứng với ω i được xác định từ J (Rii ) theo hệ thức
117
∂ω i ∂ (A i ω i( i ) ) ∂ω i( i )
J Ri = = = Ai = A i J (Rii ) (4.7)
∂q& ∂q& ∂q&
6. Xác định các phần tử của ma trận khối lượng M (q) của rôbốt từ hệ thức
tổng quát sau
M (q) = ∑ [J TTi m i J Ti + J TRi I i J Ri ]

6
(4.8)
i =1
Để đơn giản hóa các công thức tính nhưng không mất tính tổng quát, ta
coi các trục của hệ tọa độ động là các trục quán tính chính, khi đó ma trận
I i( i ) bao gồm các phần tử là hằng số và được biểu diễn dưới dạng ma trận đường
chéo
⎡I xi 0 0⎤
I =⎢0
(i)
I yi 0⎥ (4.9)
1
⎢ ⎥
⎢⎣ 0 1 I zi ⎥⎦
Chú ý rằng ma trận quán tính khối Ii của khâu i tại điểm khối tâm tính
trên hệ tọa độ cố định được xác định bởi hệ thức
I i = A i I i( i ) A iT (4.10)
Thế các hệ thức (4.7) và (4.9) vào phương trình (4.8) ta nhận được
M (q) = ∑ [J TTi m i J Ti + J (Rii ) T A iT A i I i( i ) A iT A i J (Rii ) ] = ∑ [J TTi m i J Ti + J (Rii ) T I i( i ) J (Rii ) ]

6 6
(4.11)
i =1 i =1
Thế các biểu thức nhận được của J Ti (bước 3), J (Rii ) (bước 5), các giá trị
cho trước của mi và ma trận I i(i ) vào phương trình (4.11) ta nhận được biểu thức
của 6×6 phần tử của ma trận khối lượng M (q) . Do công thức của M (q) được
viết dưới dạng tích và tổng của các ma trận nên việc tính toán có thể thực hiện
khá thuận lợi nhờ một chương trình tính có chức năng xử lý các biểu thức bằng
chữ như chương trình tính toán đa năng MAPLE.
7. Xác định các phần tử của ma trận C(q, q& )
Như đã trình bày trong chương II, các thành phần cij của ma trận C(q, q& )
được xác định từ các phần tử của ma trận M (q) theo hệ thức sau
1 ⎛⎜ ∂m ij ∂m ik ∂m jk ⎞
⎟q& k
C ij = ∑
2 ⎜⎝ ∂q k
+
∂q j
−
∂q i ⎟ (4.12)
⎠
trong đó i = 1,2,...,6 và j = 1,2,...,6.
118
Tuy nhiên, biểu thức của các phần tử của M (q) và C(q, q& ) là những biểu
thức giải tích khá dài và phức tạp nên không được trình bày chi tiết thêm ở đây.
8. Xác định các phần tử của véc tơ g(q) từ biểu thức thế năng của hệ
Thế năng của hệ có thể xác định nhờ công thức
6
Π = ∑ m i g T ri (4.13)
i =1
trong đó véctơ ri được xác định từ bước 2, véc tơ gia tốc trọng trường tính trên
hệ cố định g = [ 0,0, − g ] với g ≈ 9.81 m/s 2 . Sử dụng hệ thức (4.13), thế năng
T
của hệ được viết dưới dạng

Π = m1gl1 + m 2 g (l 2S 2 + d1 ) + m 3 g (l 3S23 + a 2S 2 + d1 )
+ m 4 g(l 4 C 234 + a 3S23 + a 2S2 + d1 )
+ m 5 g(l 5 C 234 + a 3S23 + a 2S2 + d1 )
+ m 6 g (l 6S6 C 234 + l 6 C 6 C 5S 234 + a 3S 23 + a 2S 2 + d 5 C 234 + d1 )
Từ đó, ta suy ra các thành phần của véctơ g(q) theo công thức
T
⎛ ∂Π ⎞
g(q) = ⎜⎜ ⎟⎟ (4.14)
⎝ ∂q ⎠
dưới dạng sau
g1 = 0
g 2 = m 2 gl 2 C 2 + m 3 g(l 3 C 23 + a 2 C 2 )
− m 4 g (l 4S234 − a 3 C 23 − a 2 C 2 ) − m 5 g(l 5S234 − a 3 C 23 − a 2 C 2 )
− m 6 g (l 6S6S234 − l 6 C 6 C 5 C 234 − a 3 C 23 − a 2 C 2 + d 5S 234 )
g 3 = m 3 gl 3 C 23 − m 4 g (l 4S234 + a 3 C 23 ) − m 5 g (l 5S234 − a 3 C 23 )
− m 6 g(l 6S6S 234 − l 6 C 6 C 5 C 234 − a 3 C 23 + d 5 C 234 )
g 4 = − m 4 gl 4S 234 − m 5 gl 5S234 − m 6 g (l 6S6S234 − l 6 C 6 C 5 C 234 + d 5S234 )
g 5 = − m 6 gl 6 C 6S5S234
g 6 = − m 6 gl 6 (C 6 C 234 − C 5S6S 234 )
Cuối cùng, ta tính toán được các trị số mômen của động cơ của rôbốt sau
khi tất cả các số hạng của vế phải của phương trình (4.5) đã được xác định. Để
minh họa cho việc tính toán động lực học ngược bằng tính toán số, ta xét một
thí dụ ứng với quĩ đạo chuyển động của điểm E theo dạng cung tròn phẳng.
Bảng 4.4 cung cấp các trị số của các thông số động lực học của rôbốt phục vụ
cho tính toán số.
119
Bảng 4.4. Các thông số động lực học của rôbốt đo (đối chiếu với hình 4.7)
mi Ixi Iyi Izi li

Khâu i
(kg) (kgm2) (kgm2) (kgm2) (m)
1 2.0 4.0×10-3 3.0×10-3 1.0×10-3 0.10
2 0.9 0.2×10-3 3.0×10-3 3.0×10-3 0.06
3 1.2 0.5×10-3 3.5×10-3 4.0×10-3 0.10
4 1.1 0.6×10-3 2.5×10-3 3.5×10-3 0.04
5 0.5 0.7×10-3 0.2×10-3 0.3×10-3 0.03
6 0.05 0.3×10-4 0.2×10-4 0.1×10-4 0.02
Quy luật chuyển động trên mặt phẳng {Ox 0 z 0 } của điểm E (điểm O5)
được mô tả bởi các phương trình tọa độ (4.15) và minh họa trên hình 4.8.
⎛ π ⎞
x E = 0.2 + 0.12⎜1 − cos t ⎟ (m)
⎝ 4 ⎠
yE = 0 (4.15)
π
z E = 0.14 + 0.12 sin t (m)
4
trong đó thời gian chuyển động hết một nửa đường tròn là 4 (s). Vị trí ban đầu
(tại thời điểm t = 0) của rôbốt được xác định bởi các trị số biến khớp
q 20 = 1.0472, q 30 = 3.5511, q 40 = 2.1206, q 50 = 2.0 (rad). Do chuyển động của điểm
E hoàn toàn trên mặt phẳng, biến khớp của khâu 1 được ấn định q1 = 0.
Hình 4.8. Quĩ đạo điểm E (nửa đường tròn trên mặt phẳng {Ox 0 z 0 })
120
O2
E ≡ O5
O1
O4
Hình 4.9. Vị trí các khâu và các khớp tại một số vị trí chuyển động
Hình 4.10. Trị số của biến khớp 2, 3 và 4

121
Hình 4.11. Vận tốc góc của động cơ dẫn động khâu 2, 3 và 4
Hình 4.12. Gia tốc góc của động cơ dẫn động khâu 2, 3 và 4
122
Hình 4.13. Mômen dẫn động cần thiết các khâu của rôbốt
Kết quả tính toán động học ngược được trình bày trên các hình 4.9 - 4.12.
Như đã phân tích tại mục 4.2, biến khớp q5 giữ giá trị hằng số trong suốt quá
trình chuyển động của điểm E. Với các kết quả tính toán động học ngược đối
với các biến khớp như trên và với khớp 6 chuyển động độc lập theo quy luật
q 6 = π / 2, q& 6 = 0, &q& 6 = 0, ta có thể tính toán được mômen dẫn động cần thiết tại
các khớp 1, 2,...,6 (dẫn động các khâu 1, 2,...,6 tương ứng) từ phương trình vi
phân chuyển động đã thiết lập được. Kết quả tính toán số được biểu diễn trên
hình 4.13.
4.5 Điều khiển trượt rôbốt BKHN-MCX-04
Để tiến hành điều khiển rôbốt chuyển động theo một quy luật định trước
theo yêu cầu của bài toán công nghệ, trước tiên ta phải giải bài toán động học
ngược để xác định giá trị của các véc tơ q, q& , q&& dựa trên thuật toán hiệu chỉnh
gia lượng véc tơ tọa độ suy rộng đã được trình bày trong chương 1, sau đó giải
bài toán động lực học ngược để xác định phương trình chuyển động của rôbốt
và giá trị lực/mômen theo các giá trị của q, q& , q
&& qua đó xác định công suất cần
thiết của động cơ điện.
123
Theo nguyên lý hoạt động của rôbốt BKHN-MCX-04, để đo một chi tiết
nào đó ta phải xác định trước quỹ đạo cần thiết để điều khiển điểm O5 chuyển
động theo quỹ đạo ấy nhằm đưa que đo tiếp xúc với chi tiết cần đo.
Giả sử chi tiết cần đo có dạng hình cầu như hình dưới đây
Quĩ đạo điểm E
Hình 4.14 Quỹ đạo định trước của điểm E (đường xoắn ốc)
Để đo vật thể dạng hình cầu này ta có thể tiến hành điều khiển điểm O5
chuyển động theo 1 đường xoắn ốc trên bề mặt của hình cầu. Phương trình của
đường xoắn ốc có dạng
⎛t⎞
x E = 3 + 0.1sin(2πt ) sin⎜ ⎟
⎝ 3⎠
⎛t⎞
y E = 0.1cos(2πt ) sin⎜ ⎟
⎝ 3⎠
⎛t⎞
z E = 0.12 + 0.1sin⎜ ⎟
⎝ 3⎠
Bài toán động học ngược và động lực học ngược cho trường hợp này đã
được giải quyết trong phần 4.3 và 4.4, ta sẽ sử dụng các kết quả này cho phần
điều khiển.
Như trong phần điều khiển chuyển động của rôbốt đã trình bày ở chương
3 và theo lý thuyết điều khiển dạng trượt được trình bày trong [63], để ổn định
chuyển động của điểm O5 ta chọn hàm Lyapunov có dạng
124
1
V = s T M (q)s
2
Trong đó M(q) là ma trận khối lượng của rôbốt, s là sai số suy rộng trong
điều khiển dạng trượt, với s được xác định bởi
s = e& ( t ) + Λe( t )
trong biểu thức trên thì e(t) và e& ( t ) là độ sai lệch vị trí và sai lệch vận tốc của
các khớp động
e(t) = q(t) – qd(t)
e& ( t ) = q& ( t ) − q& d ( t )
Λ = diag(λ1 , λ 2 ,..., λ 6 )
Sử dụng chương trình điều khiển trượt được xây dựng theo thuật toán tính
toán giải bài toán điều khiển rôbốt ở chương 3 và phần simulink trong Matlab,
ta thu được kết quả điều khiển chuyển động rôbốt BKHN-MCX-04 theo các
thông số của bộ điều khiển đã được chọn là:
Ks= diag([20,20,20,20,1,0.1]); Kpd = diag([0.2,0.2,0.2,0.2,0.02,0.01]);
λ= diag([50,50,50,50,30,20]);
Nhiễu d(t) = random(6,6)*0.01;
Hình 4.15 Đồ thị tọa độ xE(t) trong không gian thao tác
125
Hình 4.16 Đồ thị tọa độ yE(t) trong không gian thao tác.
Hình 4.17 Đồ thị tọa độ zE(t) trong không gian thao tác.
Qua kết quả trên ta thấy rằng điểm cần điều khiển O5 của rôbốt BKHN-
MCX-04 bám khá tốt theo quỹ đạo mong muốn, chỉ sau một khỏang thời gian
0.25 giây điểm O5 đã bám được quỹ đạo mong muốn.
126
4.6 Thí nghiệm

Qua các phần lý thuyết đã trình bày ta thấy các bài toán ngược động lực
học và điều khiển rôbốt dư dẫn động đều liên quan đến việc bài bài toán động
học ngược. Kết quả của bài toán động lực học và điều khiển hoàn toàn phụ
thuộc vào độ chính xác của bài toán động học ngược, vì vậy trong phần thí
nghiệm của mình chúng tôi chọn việc thí nghiệm nhằm kiểm tra độ chính xác
của việc giải bài toán động học ngược. Mô hình thí nghiệm có dạng như hình
ảnh dưới đây:
Hình 4.18 Mô hình thí nghiệm rôbốt đo BKHN-MCX-04
4.6.1 Cấu tạo của hệ thống thí nghiệm

Bộ thí nghiệm gồm có các phần như sau:
Rôbốt BKHN-MCX-04
Camera thu nhận hình ảnh hoạt động của rôbốt
Kính lọc màu
Máy tính
127
4.6.2 Nguyên lý hoạt động của hệ thống thí nghiệm

Trên rôbốt, tại các khớp động ta gắn các đèn led phát nguồn ánh sáng
màu đỏ, phía trước camera ta gắn kính lọc màu có nhiệm vụ chỉ cho ánh sáng
màu đỏ đi qua và tác động vào camera. Trong quá trình làm việc có thể có các
nhiễu về màu sắc từ hệ thống tác động lên camera, để loại các nhiễu sắc này ta
dùng phần mềm lọc nhiễu được viết bằng ngôn ngữ Matlab. Sau khi lọc đi các
nhiễu ta còn lại các đốm sáng phát ra từ các đèn led - vị trí của các khớp mà ta
quan tâm. Phần mềm có nhiệm vụ xác định vị trí tâm của các đốm sáng. Khi xác
định được các tâm của đốm sáng, thì đoạn thẳng nối các tâm chính là các khâu
của rôbốt ở thời điểm khảo sát.
Khi rôbốt chuyển động, các đèn led gắn liền với các khớp động cũng sẽ
thay đổi vị trí của nó. Sự thay đổi vị trí của các khớp động này sẽ được camera
thu nhận, chụp lại ảnh và lưu vào bộ nhớ của máy tính. Phần mềm xử lý ảnh sẽ
xác định tâm của các khớp và lưu vào tệp “toadotam.txt” của các khớp. Mỗi ảnh
sẽ có 2n số tọa độ tâm, trong đó có n giá trị tọa độ theo trục x và n giá trị tọa độ
theo trục y của các tâm ( với n là số các đèn led chiếu sáng, đây chính là số các
khớp động). Khi đã có tọa độ của các tâm sáng, phần mềm sẽ tính toán ra góc
giữa các khâu qua các đoạn thẳng nối liền các đốm sáng và lưu vào tệp
“Goc.txt” là bộ góc giữa các khâu.
Để điều khiển chuyển động của rôbốt ta xây dựng một phần mềm điều
khiển viết bằng ngôn ngữ Visual Basic. Thông qua giao diện của chương trình
ta nhập vào các giá trị góc quay của các khớp động mà ta tính toán được qua bài
toán động học ngược. Chương trình điều khiển sẽ điều khiển các khớp rôbốt
quay với các góc tương ứng. Còn phần mềm xử lý ảnh sẽ ghi và lưu lại các giá
trị góc quay thực và tọa độ của các tâm khớp cũng như vẽ được quỹ đạo chuyển
động của khâu thao tác.
4.6.3 Hệ thống điều khiển
Rôbốt đo gồm 5 khớp điều khiển chuyển động và 1 khớp điều khiển việc
đo lường. Mối một khớp được quản lý bởi 1 bộ vi xử lý. Mối bộ vi xử lý có
chức năng điều khiển khép kín, vừa có nhiệm vụ điều khiển động cơ vừa có
nhiệm vụ đo lường. Các bộ vi xử lý sẽ được nối chung bus và được nối với máy
tính qua cổng RS232. Mỗi một bộ vi xử lí sẽ có một địa chỉ xác định để ta có
thể dễ dàng điều khiển.
Tay máy hoạt động theo nguyên tắc từ các bản vẽ CAD khi thiết kế bề
mặt các chi tiết máy ta xác định quỹ đạo điều khiển theo các điểm cần đo. Trên
quỹ đạo điều khiển đo, ta sẽ xác lập chương trình điều khiển cho các khớp để
dịch chuyển các khâu của rôbốt theo các điểm trong không gian làm việc của
thiết bị. Muốn xác định vị trí của một điểm nào đó trong kh«ng gian, ta ph¸t
lÖnh điều khiển cho tõng khíp tõ m¸y tính. Lệnh điều khiển sẽ được truyền
128
xuống bộ vi xử lí. Khớp nào được điều khiển thì bộ vi xử lí của khớp đó sẽ nhận
được lệnh và bắt đầu điều khiển động cơ. Vị trí của các khớp sẽ được xác định
nhờ hệ thống đo góc bởi các encoder. Nhờ đó ta sẽ xác định được vị trí của đầu
đo. Trong suốt quá trình điều khiển, vị trí của đầu đo luôn được cập nhật trong
máy tính. Khi đầu đo đến đúng vị trí mà ta mong muốn thì vị trí của đầu đo
cũng như vị trí của các khớp sẽ được lưu lại trong máy tính.
Xuất phát từ nguyên tắc hoạt động của rôbốt đo, ta đưa ra các yêu cầu
sau :
- Xây dựng hệ vi xử lí thực hiện 2 chức năng điều khiển động cơ đồng thời
xử lí tín hiệu phản hồi từ hệ đo.
- Xây dựng hệ truyền động điện thực hiện chức năng điều khiển động cơ
một chiều (có đảo chiều).
- Xây dựng hệ xử lí tín hiệu phản hồi từ hệ đo, đồng thời nâng cao độ phân
giải của hệ thống đo.
- Truyền thông đa vi xử lí để kết nối giữa hệ điều khiển của từng khớp với
máy tính.
Từ yêu cầu trên ta có sơ đồ điều khiển cho toàn hệ thống như trên hình 4.
19 và sơ đồ điều khiển của từng khớp như trên hình 4.20
Hình 4.19 Sơ đồ điều khiển toàn bộ rôbốt

129
Hình 4.20 sơ đồ điều khiển từng khớp của BKHN-MCX-04

Trong các hệ thống điều khiển ở trên ta dùng vi điều khiển thuộc họ
MCS-51 của Intel, cụ thể là chip AT89C2051 của hãng Atmel. Sơ đồ nguyên lý
của mạch điều khiển trục có dạng như trên hình 4.21.
12V
D11 D12
U4 DIODE DIODE
5 2
7 1A1 1Y 1 3 1
74HC04 U5E 1A2 1Y 2 2
10 13
12 2A1 2Y 1 14
11 10 F-LOW 2A2 2Y 2 D13 D14 DC
1 DIODE DIODE
15 1E
F-HIGH 2E 12V
6
LED-FH VCC 11 1EN
VCC VCC 2EN
9
VCC1
Hình 4.21
4
VCC U1 C4 VCC2
40
12V L298
D1 39 21 D10 LED-FL RIGHT 105
38 P0.0/AD0 P2.0/A8 22 LEFT
VCC
37 P0.1/AD1 P2.1/A9 23 PWM
D2 36 P0.2/AD2 P2.2/A10 24 F-HIGH
35 P0.3/AD3 P2.3/A11 25
34 P0.4/AD4 P2.4/A12 26
D3 33 P0.5/AD5 P2.5/A13 27
32 P0.6/AD6 P2.6/A14 28 RXD-TXD
10
P0.7/AD7 P2.7/A15 1
D4 1 10 2 7 VCC
2 P1.0/T2 P3.0/RXD 11 R92 6 EN-QUAY
P1.1/T2-EX P3.1/TXD R91
GND
3 12 INT0-THC 3
1
14
D5 4 P1.2 P3.2/INT0 13 11 R02 2 5

5 P1.3 P3.3/INT1 14 8 QD R01 4
6 P1.4 P3.4/T0 15 9 QC 1 3 2 3
D6 7 P1.5 P3.5/T1 16 12 QB CLKB 14 2
VCC
8 P1.6 P3.6/WR 17 QA CLKA 1

130
P1.7 P3.7/RD VCC

74LS90 74HC126 U9A
5
7
D7 19 30
C1 18 XTAL1 ALE/PROG 29 74HC126 U10B
Y1 XTAL2 PSEN VCC
CRY STAL 31 6 5
C2 EA/VPP
D8 9
GND
RST
VCC
4
20
VCC AT89C52
14
U7A U8B
SW1 1 4
RESET C3 3 6
10
C 2 5 74LS90
R1 7
100 74HC00 74HC00 R92 6
7
J4 R91
GND
3
10
10K R2 4 11 R02 2
3 8 QD R01
2 9 QC 1 8 9
VCC U12C U12D QB CLKB
1 12 14
VCC
9 12 QA CLKA
8 EN-THC
11 74HC126
5
EN-THUOC 10 13
Sơ đồ nguyên lý của mạch điều khiển 1 trục

74HC00 74HC00 11 12
VCC
13
F-LOW
131
4.6.4 Kết quả thí nghiệm

Dưới đây là kết quả của thí nghiệm mà chúng tôi đã tiến hành trên rôbốt
BKHN-MCX-04
Giả sử quỹ đạo mong muốn của bàn kẹp của rôbốt BKHN-MCX-04 có
dạng là nửa vòng tròn phẳng với phương trình của nó là
⎛ π ⎞
x E = 0.2 + 0.12⎜1 − cos t ⎟ (m)
⎝ 4 ⎠
yE = 0
π
z E = 0.14 + 0.12 sin t (m)
4
Giá trị tọa độ điểm cuối E và các tọa độ suy rộng của các khớp động đã
được xác định từ thực nghiệm và được lưu lại theo bảng dưới đây.
Bảng 4.5 Các kết quả đo thực nghiệm
TT t (s) xE (m) yE (m) q2 (rad) q3 (rad) q4 (rad)
1 0 0.2000 0.1405 1.0472 3.5511 2.1206
2 0.2 0.2015 0.1578 1.0957 3.5814 2.1564
3 0.4 0.2059 0.1776 1.1396 3.6379 2.1711
4 0.6 0.2131 0.1935 1.1767 3.7157 2.1622
5 0.8 0.2229 0.2110 1.2045 3.8077 2.1294
6 1.0 0.2351 0.2239 1.2212 3.9070 2.0750
7 1.2 0.2495 0.2376 1.2261 4.0085 2.0030
8 1.4 0.2655 0.2459 1.2195 4.1089 1.9177
9 1.6 0.2829 0.2546 1.2025 4.2068 1.8234
10 1.8 0.3012 0.2575 1.1763 4.3013 1.7236
11 2.0 0.3200 0.2605 1.1425 4.3922 1.6213
12 2.2 0.3388 0.2575 1.1023 4.4791 1.5192
13 2.4 0.3571 0.2546 1.0569 4.5616 1.4194
14 2.6 0.3745 0.2459 1.0076 4.6389 1.3242
15 2.8 0.3905 0.2376 0.9557 4.7099 1.2355
16 3.0 0.4049 0.2239 0.9027 4.7733 1.1549
17 3.2 0.4171 0.2110 0.8501 4.8272 1.0843
18 3.4 0.4269 0.1935 0.7997 4.8696 1.0248
19 3.6 0.4341 0.1776 0.7532 4.8987 0.9776
20 3.8 0.4385 0.1578 0.7122 4.9125 0.9432
21 4.0 0.4400 0.1400 0.6783 4.9095 0.9218
132
Hình 4.22 Quỹ đạo thực nghiệm của điểm cuối E

133
Qua kết quả thí nghiệm ta thấy độ sai lệch lớn nhất giữa kết quả tính toán
và kết quả thực nghiệm là 0.1%. Điều đó chứng tỏ thuật toán mà chúng tôi đã sử
dụng để giải bài toán động học ngược là hoàn toàn phù hợp.

Trên cơ sở rôbốt đo tự chế tạo, trong chương này đã tiến hành xác định
các tham số động học, động lực học của rôbốt. Sau đó đã tiến hành tính toán
động học ngược, động lực học ngược và điều khiển rôbốt bám theo quỹ đạo cần
đo. Đây là một loại rôbốt có cấu trúc động học không gian nên việc thiết lập các
phương trình vi phân chuyển động khá phức tạp. Do đó việc tính toán động lực
học ngược và điều khiển chuyển động là không đơn giản nếu ta sử dụng các
công cụ cũ. Nhờ thuật toán “hiệu chỉnh gia lượng véc tơ tọa độ suy rộng”, bài
toán này được giải một cách khá nhanh chóng.
Các kết quả của chương này một mặt chứng minh tính chất đúng đắn của
các thuật toán trong các chương trước, mặt khác nêu ra một khả năng chế tạo và
sử dụng rôbốt trong đo lường chính xác các chi tiết và cụm các chi tiết máy.
134
KẾT LUẬN CHUNG
Để thiết kế chế tạo các rôbốt phục vụ cho việc đẩy mạnh công nghiệp hóa
và hiện đại hóa nền công nghiệp thì việc nghiên cứu động học, động lực học và
điều khiển các rôbốt là một công việc cần thiết và hết sức quan trọng.
Trong luận án của mình, tác giả đã nghiên cứu các bài toán động học,
động lực học và điều khiển chuyển động của rôbốt dư dẫn động. Đây là các
dạng bài toán cơ bản nhất của việc phân tích và tổng hợp các rôbốt. Có thể tóm
tắt lại một số kết quả chính mà chúng tôi đã đạt được như sau:
Những kết quả chính của luận án
1. Dựa trên có sở của công thức khai triển Taylor, đã xây dựng thuật toán
“hiệu chỉnh gia lượng véc tơ tọa độ suy rộng” để tính toán bằng số bài
toán động học ngược rôbốt dư dẫn động. So với các thuật toán số đã có
thuật toán này tính toán có độ chính xác cao hơn.
2. Đã xây dựng một thuật toán giải bài toán động lực học ngược rôbốt dư
dẫn động trong không gian thao tác. Từ đó xây dựng một thuật toán giải
quyết bài toán điều khiển rôbốt dư dẫn động trong không gian trạng thái
theo phương pháp điều khiển dạng trượt.
3. Dựa trên các phần mềm đa năng MATLAB và MAPLE đã xây dựng các
chương trình tính toán động học ngược, động lực học ngượ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. Sử dụng các chương trình tính toán này đã tính
toán nhiều thí dụ minh họa nhằm chứng minh khả năng của thuật toán và
sự chính xác (sai số rất bé) của thuật toán.
4. Với sự giúp đỡ của TS. Nguyễn Văn Vinh, đã tham gia chế tạo rôbốt đo
BKHN-MCX-04. Đã xác định được các tham số động học, động lực học
của rôbốt tự chế tạo này. Sử dụng phần mềm xây dựng trên môi trường
MATLAB và MAPLE đã tiến hành tính toán động học ngược, động lực
học ngược và điều khiển trượt rôbốt đo BKHN-MCX-04. Đã tiến hành thí
nghiệm nhỏ trên mô hình này.
Các bài toán động học ngược, động lực học ngược và điều khiển chuyển
động rôbốt đều có liên quan đến bài toán xác định các tọa độ suy rộng cho nên
khi chúng ta đã xây dựng thuật toán “hiệu chỉnh gia lượng véc tơ tọa độ suy
rộng” là chúng ta có một công cụ hữu ích để giải quyết các bài toán cơ bản về
động học ngược, động lực học ngược và điều khiển rôbốt. Vì vậy có thể nói
thuật toán “hiệu chỉnh gia lượng véc tơ tọa độ suy rộng” là kết quả cơ bản nhất
của luận án này.
135
Một số vấn đề có thể tiếp tục mở rộng nghiên cứu

- Do có bậc tự do còn dư mà khi có một khớp động nào đó bị kẹt thì rôbốt
vẫn có khả năng thực hiện các yêu cầu của bài toán công nghệ vì vậy ta
có thể đầu tư nghiên cứu về hiện tượng kẹt khớp trong quá trình làm việc
của Rôbốt dư dẫn động.
- Nghiên cứu ứng dụng các phương pháp điều khiển khác nhau để lựa chọn
giải pháp điều khiển tối ưu.
- Nghiên cứu triển khai ứng dụng trong thực tế sản xuất các kết quả đã
nghiên cứu được.
136
DANH MỤC CÔNG TRÌNH CỦA TÁC GIẢ
1. Nguyễn Văn Khang, Lê Đức Đạt, Trần Hoàng Nam: “Về một phương
pháp số giải bài toán động học ngược robot dạng chuỗi”. Tuyển tập Hội
nghị Cơ học toàn quốc lần thứ 8, Tập 1, Tr. 250-259. NXB Bách khoa,
Hà Nội 2007.
2. Nguyễn Văn Khang, Nguyễn Quang Hoàng, Lê Đức Đạt, Trần Hoàng
Nam : “Về một thuật toán điều khiển trượt robot dư dẫn động”. Tạp chí
Tin học và Điều khiển học, Tập 24 (2008), No.3, Tr.269-280.
3. Nguyễn Quang Hoàng, Nguyễn Văn Khang, Trần Hoàng Nam: “Bài toán
động học ngược rôbốt dư dẫn động có chú ý đến sự cố kẹt khớp”. Tuyển
tập Hội nghị Cơ học toàn quốc, Tập 2, Tr. 282-290, NXB Khoa học tự
nhiên và Công nghệ, Hà Nội 2009.
4. Nguyễn Văn Khang, Nguyễn Quang Hoàng, Trần Hoàng Nam: “Về bài
toán động lực học ngược rôbốt dư dẫn động”. Tuyển tập Hội nghị Khoa
học Công nghệ Cơ khí chế tạo toàn quốc lần thứ hai, Phân ban “Tự động
hóa và Cơ điện tử”, Tr 41-48, Hà Nội 2009.
5. Nguyen Van Khang, Do Tuan Anh, Nguyen Phong Dien, Tran Hoang
Nam : “In fluence of trajectories on the joint torques of kinematically
redundant manipulators”. Vietnam Journal of Mechanics, vol. 29 (2007),
No.2, pp. 65-72.
6. Nguyen Van Khang, Nguyen Phong Dien, Nguyen Van Vinh, Tran
Hoang Nam : “Inverse kinematic and dynamic analysis of redundant
measuring manipulator BKHN-MCX-04”. Vietnam Journal of
Mechanics, vol. 32 (2010), No.1, pp. 15-26.
137
TÀI LIỆU THAM KHẢO
Tài liệu tiếng Việt
1. Đỗ Tuấn Anh: Bài toán ngược động lực học rôbốt dư dẫn động. Đồ án tốt
nghiệp đại học - Đại học Bách Khoa Hà Nội 2006.
2. R. H. Bishop: Cơ điện tử, Tập 1 ( Phạm Anh Tuấn và các đồng nghiệp
biên dịch). NXB Đại học Quốc gia, Hà Nội 2006.
3. Phạm Thượng Cát: “Điều khiển rôbốt n bậc tự do có nhiều tham số bất
định trong không gian Đề Các”. Tuyển tập Hội nghị Cơ học toàn quốc
năm 2009, Tập 1, Tr. 195-204.
4. Phan Nguyên Di, Nguyễn Văn Khang, Đỗ Sanh: Ổn định chuyển động
trong kỹ thuật. Nhà xuất bàn Khoa học và Kỹ thuật, Hà nội 1986.
5. Nguyễn Văn Đạo : Ổn định của các hệ động lực. Nhà xuất bản Đại học
Quốc gia Hà nội, Hà nội 1998.
6. Lê Đức Đạt: Về điều khiển phi tuyến chuyển động của Robot dư dẫn động
trong không gian thao tác. Đồ án tốt nghiệp đại học, Đại học Bách Khoa
Hà Nội 2008.
7. B. Heimann, W. Gerth, K. Popp : Cơ điện tử. Nhà xuất bản Khoa học và
kỹ thuật, Hà nội 2008 (Người dịch: Nguyễn Văn Khang, Nguyễn Phong
Điền, Nguyễn Quang Hoàng, Hoàng Minh Sơn).
8. Nguyễn Công Hiền, Nguyễn Hoàng Mai: “Điều khiển trượt thích nghi
cho chuyển động tay máy”. Tuyển tập Hội nghị khoa học toàn quốc lần
thứ 6 về Tự động hoá, Hà Nội 2005.
9. Đào Văn Hiệp: Kỹ thuật Rôbốt (in lần thứ 3). NXB Khoa học và Kỹ
thuật, Hà Nội 2006.
10. Nguyễn Trần Hiệp, Phạm Thượng Cát: “Điều khiển rôbốt theo nguyên lý
trượt sử dụng mạng nơron”. Tạp chí Tin học và Điều khiẻn học, Tập 24,
Số 3, Tr.236-246.
11. Nguyễn Thế Hoàn, Phạm Phu: Cơ sở phương trình vi phân và lý thuyết
ổn định, Nhà xuất bản Giáo dục, 2003.
12. Nguyễn Quang Hoàng, Nguyễn Văn Khang: “Về một thuật toán điều
khiển trượt rôbốt công nghiệp”. Tuyển tập Hội nghị Cơ học toàn quốc lần
thứ 8, Tập 1, Tr. 191- 199, Hà Nội 2007.
138
13. Nguyễn Quang Hoàng, Nguyễn Văn Khang, Trần Hoàng Nam: “Bài toán
động học ngược rôbốt dư dẫn động có chú ý đến sự cố kẹt khớp”. Tuyển
tập Hội nghị Cơ học toàn quốc, Tập 2, Tr. 282-290, Hà Nội 2009.
14. F. Holzweissig, H. Dresig : Giáo trình động lực học máy. Nhà xuất bản
Khoa học kỹ thuật, Hà nội 2001, (Người dịch: Nguyễn Văn Khang, Vũ
Liêm Chính, Phan Nguyên Di).
15. Lê Minh Huy: Bài toán ngược và điều khiển rôbốt dư dẫn động. Đồ án
tốt nghiệp đại học, Đại học Bách Khoa Hà nội 2009.
16. Nguyễn Văn Khang: Cơ học kỹ thuật, Nhà xuất bản Giáo dục, Hà Nội.
2009.
17. Nguyễn Văn Khang: Động lực học hệ nhiều vật. Nhà xuất bản Khoa học
và Kỹ thuật, Hà Nội 2007.
18. Nguyễn Văn Khang, Lê Đức Đạt, Trần Hoàng Nam: “Về một phương
pháp số giải bài toán động học ngược robot dạng chuỗi”. Tuyển tập Hội
nghị Cơ học toàn quốc lần thứ 8, Tập 1, Tr. 250-259. NXB Bách khoa,
Hà Nội 2007.
19. Nguyễn Văn Khang, Nguyễn Quang Hoàng, Lê Đức Đạt, Trần Hoàng
Nam : “Về một thuật toán điều khiển trượt robot dư dẫn động”. Tạp chí
Tin học và Điều khiển học, Tập 24 (2008), No.3, Tr.269-280.
20. Nguyễn Văn Khang, Nguyễn Quang Hoàng, Trần Hoàng Nam: “Về bài
toán động lực học ngược rôbốt dư dẫn động”. Tuyển tập Hội nghị khoa
học công nghệ cơ khí chế tạo toàn quốc lần thứ hai, Hà Nội 2009.
21. Lại Khắc Lãi, Nguyễn Việt Anh: Lập trình Matlab và ứng dụng. Nhà
xuất bản Khoa học và Kỹ thuật, Hà nội 2006.
22. Tạ Duy Liêm: Rôbốt và hệ thống công nghệ rôbốt hóa. NXB Khoa học
và Kỹ thuật, Hà Nội 2004.
23. Vũ Đình Nam: Ứng dụng của điều khiển trượt trong Robot công nghiệp.
Đồ án tốt nghiệp đại học - Đại học Bách Khoa Hà Nội 2008.
24. Đinh Văn Phong: Phương pháp số trong Cơ học (in lần thứ 2). NXB
Khoa học và Kỹ thuật, Hà Nội 2006
25. Nguyễn Thiện Phúc: Rôbốt công nghiệp. Nhà xuất bản Khoa học và Kỹ
thuật, 2004.
26. Nguyễn Doãn Phước: Lý thuyết điều khiển nâng cao. Nhà xuất bản Khoa
học và Kỹ thuật, 2005.
139
27. Nguyễn Doãn Phước: Lý thuyết điều khiển phi tuyến. Nhà xuất bản Khoa
học và Kỹ thuật, Hà Nội 2006.
28. Phạm Đăng Phước: Rôbốt công nghiệp. Nhà xuất bản Xây dựng, Hà Nội
2007.
29. Nguyễn Phương, Nguyễn Thị Hương Giang: Cơ sở tự động hóa trong
ngành cơ khí, Nhà xuất bản Khoa học và Kỹ thuật, Hà nội 2005.
30. Nguyễn Ngọc Thuần, Hồ Quỳnh : Ứng dụng ma trận trong kỹ thuật. Nhà
xuất bản Khoa học và kỹ thuật, Hà nội 1978.
31. Nguyễn Phùng Quang: Matlab/Simulink dành cho kỹ sư điều khiển tự
động. Nhà xuất bản Khoa học và Kỹ thuật, 2004.
32. Lê Hoài Quốc, Chung Tấn Lâm: Nhập môn rôbốt công nghiệp. Nhà xuất
bản Khoa học và Kỹ thuật, 2002.
33. Trần Thế San (biên dịch): Cơ sở nghiên cứu và sáng tạo robot. Nhà xuất
bản Thống kê, Hà Nội 2003.
34. Đỗ Sanh : Cơ học giải tích. Nhà xuất bản Bách khoa Hà Nội, Hà nội
2008.
35. Đỗ Sanh: Cơ học kỹ thuật (2 tập). NXB Giáo dục, Hà Nội 2008.
36. Đỗ Sanh, Đinh Văn Phong, Nguyễn Thanh Thủy, Nguyễn Nhật Quang,
Phan Mạnh Dẫn, Đỗ ĐăngKhoa: “Công nghệ tin học và một số bài toán
Cơ kỹ thuật”. Kỷ yếu hội nghị cơ điện tử toàn quốc lần thứ 2, Tr. 182-
189. Hà Nội 2004.
37. Nguyễn Hoài Sơn, Đỗ Thanh Việt, Bùi Xuân Lâm : Ứng dụng Matlab
trong tính toán kỹ thuật. Nhà xuất bản Đại học Quốc gia Tp. Hồ Chí
Minh 2002.
38. Nguyễn Mạnh Tiến: Điều khiển rôbốt công nghiệp. Nhà xuất bản Khoa
học và Kỹ thuật, Hà Nội 2007.
39. Trịnh Quang Vinh, Nguyễn Đăng Bình, Phạm Thành Long: Rôbốt công
nghiệp, Nhà xuất bản Khoa học và kỹ thuật, Hà Nội 2008.
40. J. Wittenburg: Động lực học hệ vật rắn, (Người dịch Nguyễn Đông Anh).
Nhà xuất bản Xây dựng, Hà Nội 2000.
Tài liệu tiếng Anh
41. T.L. Boulion, P.L. Odell : Generalized Inverse matrices . Wiley –

Interscience, New York 1971.
140
42. H. Bruyninckx , O. Khatib : “Gauss’ Principle and the Dynamics of

Redundant and Constrained Manipulators”. Proceedings of the 2000
IEEE International Conference on Robotics & Automation, San
Francisco, CA , April 2000, pp. 2563-2568.
43. G.S. Chirikjian and J.W. Burdick: “The Kinematics of Hyper-Redundant
Robot Locomotion”. IEEE Transactions on Robotics and Automation,
Vol. 11, No. 6, Dec. 1995, pp. 781-793.
44. J. J. Craig: Introduction to Robotics: Mechanics and Control. Pearson
Prentice Hall, New Jersey 2005.
45. G.H. Golub and C.F Van Loan : Matrix computations (Third Edition).
The Johns Hopkins University Press, 1996.
46. Nguyen Quang Hoang, Nguyen Van Khang : “On kinematic inverse and
control of redundant manipulators under consideration of jammed joint”.
Proceeding of the ISRM 2009
47. P. Hsu, John Hauser, and Shankar Sastry: “Dynamic Control of
Redundant Manipulators”. Journal of Robotic Systems, 1989. pp 157-
168
48. R. Kelly, V. Statibanez : Control of Robot Manipulator in Joint Space.
Springer, London 2005.
49. Nguyen Van Khang, Do Thanh Trung : “A contribution to the dynamic
simulation of robot manipulator with the software ROBODYN” Vietnam
Journal of mechanics, vol. 26 (2004), No. 4, pp. 215-225.
50. Nguyen Van Khang, Do Tuan Anh, Nguyen Phong Dien, Tran Hoang
Nam : “In fluence of trajectories on the joint torques of kinematically
redundant manipulators”. Vietnam Journal of Mechanics, vol. 29 (2007),
N2, pp. 65-72.
51. Nguyen Van Khang, Nguyen Phong Dien, Nguyen Van Vinh, Tran
Hoang Nam : “Inverse kinematic and dynamic analysis of redundant
measuring manipulator BKHN-MCX-04”. Submitted to Vietnam Journal
of Mechanics.
52. T.R. Kurfess (Editor): Robotics and Automation Handbook. CRC Press,
2005.
53. D.C. Lay: Linear Algebra and Its Applications. Pearson Addison
Wesley, 2008.
141
54. F.L. Lewis, D.M. Dawson, C.T. Abdallah: Robot Manipulator Control:
Theory and Practice Second Edition. Marcel Dekker, Inc. Second
Edition, 2004.
55. R.H. Murray, Zexiang Li, S.S. Sastry, A. Makenratical: Introduction to
Robotic Manipulation. CRS Press, Boca Rator 1994.
56. R.K. Mittal, I.J. Nagrath: Robotics and Control. Tata Mcgraw-Hill
Publishing Company Limited, New Delhi, 2003.
57. Y. Nakamura : Advanced Robotics/ Redudancy and Optimination.
Addison – Wesley Publishing Company, Reading 1991.
58. S.B. Niku : Introduction to Robotics. Analysis, Systems, Appliacation.
Prentice Hall, Inc. 2000.
59. R.V. Patel, F. Shadpey : Control of redundant robot manipulator,
Springer, Berlin 2005.
60. R. P. Paul: Robot Manipulators/Mathamatics, Programming, and
Control. MIT Press, Cambridge Mass. 1981.
61. W. Perruquetti and J.P.B Barbot: Sliding Mode Control in Engineering.
Marcel D Dekker Inc., New York 2002.
62. C. R. Rao, S. K. Mitra : Generalized Inverse and Applications. John
Wiley & Sons Inc, 1971.
63. J.–J.E. Slotine and W. Li: Applied Nonlinear Control. Prentice Hall,
Englewood Cliffs, New Jersey 1991.
64. R.J. Schilling : Fundamentals of robotics, Analysis and Control. Prentice
hall, Englewood Cliffs, New Jersey, 1990.
65. B. Sicilano: Kinematic Control of Redundant Robot Manipulators: A
Tutorial. Journal of Intelligent and Robotics Systems, 1990.
66. L. Sciavicco and B. Siciliano: “A Solution Algorithm to the Inverse
Kinematic Problem for Redundant Manipulators”. IEEE Journal of
Robotics and Automation, 1988, pp. 1046-1058
67. L. Sciavicco, B. Siciliano: Modelling and Control of Robot Manipulators
(2nd Edition), Springer-Verlag, London 2000.
68. G. Schreiber, C. Ott, G. Hirzinger: “Interactive Redundant Robotics:
Control of the Inverted Pendulum With Null space Motion”. Proceedings
of the 2001 IEEE/RSJ International Conference on Intelligent Robots and
Systems, Maui, Hawaii, USA, Oct. 29 – Nov. 03, 2001, pp. 158-164.
142
69. M.W. Spong, S. Hutchinson, M. Vidyasagar: Robot Modeling

and Control. John Wiley and Sons Inc., New York 2006.
70. A. Visioli: Practical PID Control. Springer-Verlag, London 2006.
71. Q-G. Wang, Zhen Ye, Wen-Jian Cai, Chang-Chieh Hang: PID Control
for Multivariable Processes. Springer-Verlag, Berlin Heidelberg 2008.
72. N. Yagiz: “Robust control of a spatial robot using sliding modes”.
Mathematical & Computational Applications, Vol. 7, 2002, No. 3, pp.
219-228.

Luan An Tien Si Nam PDF

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Luan An Tien Si Nam PDF

Uploaded by

Copyright:

Available Formats

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

VÀ CÔNG NGHỆ VIỆT NAM

TRẦN HOÀNG NAM

GIẢI BÀI TOÁN NGƯỢC ĐỘNG HỌC, ĐỘNG LỰC

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

TRẦN HOÀNG NAM

GIẢI BÀI TOÁN NGƯỢC ĐỘNG HỌC,

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

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

Người hướng dẫn khoa học :

Tác giả luận án

Trần Hoàng Nam

Lời cam đoan

2.3 Các bài toán áp dụng 52

DANH MỤC KÝ HIỆU,

di Khoảng dịch chuyển gốc tọa độ Oi-1 về O’i.

λi Hằng số dương thứ i.

Δq0 Số gia của q0.

qi_dot Đạo hàm bậc nhất của qi theo thời gian.

rCi Véc tơ định vị trọng tâm của vật rắn thứ i.

τ Lực/momen trên khớp động .

DANH MỤC CÁC BẢNG

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

Hình 1.1 Sơ đồ thiết lập hệ tọa độ các khâu 9

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.38 Đồ thị sai số suy rộng s2 98

Hình 4.1 Mô hình rôbốt đo BKHN-MCX-04 104

1. Lí do lựa chọn đề tài

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

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

2. Đối tượng và nội dung nghiên cứu

3. Tổng quan về vấn đề nghiên cứu

4. Cấu trúc của luận án

khi m < n rôbốt được gọi là rôbốt dư dẫn động.

Hình 1.1 Sơ đồ thiết lập hệ tọa độ các khâu

- Dịch chuyển tịnh tiến dọc trục x i một đoạn ai.

1.1.3 Bài toán áp dụng

Hình 1.3. Rôbốt 5 khâu động

dàng nhận thấy rằng chúng được thỏa mãn.

A + AA + = A + A[( A T A) A T )] = A + A[A −1 ( A T ) −1 A T ] = A + [AA −1 ( A T ) −1 ] = A +

( AA + ) T = [A(A T A) −1 A T ] = A[A −1 (A T ) −1 ] A T = A[A −1 (A −1 )] A T =

A[A −1 (A −1 ) T ]A T = A[A −1 (A T )]A T = A[(A T A) −1 ] = A (A T A )

Hình 1.4. Sơ đồ khối tính toán A+ theo phương pháp Moore-Penrose

1.2.3 Phương pháp nhân tử Lagrange

Ta cũng thấy, nghiệm theo phương pháp Moore-Penrose là nghiệm ứng

Từ đó, ta có các bước tính toán như sau:

Theo phương trình (1.31), ta có :

Giải phương trình đại số tuyến tính (1.43) với ẩn Δq 0 , ta được

Biết được q k +1 , ta tính q& k +1 theo công thức (1.36) và q

Hình 1.5. Sơ đồ khối giải bài toán động học ngược

1.3.4 Đánh giá sai số

1.4 Các bài toán áp dụng

Hình 1.6. Rôbốt phẳng 5 khâu động

Hình 1.7. Các đặc tính chuyển động của khâu 1

Hình 1.8. Các đặc tính chuyển động của khâu 2

Hình 1.9. Các đặc tính chuyển động của khâu 3

Hình 1.10. Các đặc tính chuyển động của khâu 4

Hình 1.11. Các đặc tính chuyển động của khâu 5

Hình 1.14. Sai số góc định hướng của bàn kẹp

-0.4 -0.2 0 0.2 0.4 0.6 0.8 1

Hình 1.16. Các đặc tính chuyển động của khâu 1

Hình 1.17. Các đặc tính chuyển động của khâu 2