TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP

KHOA CƠ KHÍ
BỘ MÔN CƠ ĐIỆN TỬ

THUYẾT MINH ĐỒ ÁN
THIẾT KẾ RÔ BỐT CÔNG NGHIỆP
Đề tài: Thiết kế robot Fanuc Irmate
200ib 6 bậc tự do

GVHD: Th.S Dương Quốc Khánh

Sinh viên: Vũ Đỗ Bình
Mssv: K205520114216
Lớp: K56-CĐT.03

Thái nguyên 2023

 TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP
KHOA CƠ KHÍ
BỘ MÔN CƠ ĐIỆN TỬ

ĐỒ ÁN MÔN HỌC

THIẾT KẾ ROBOT CÔNG NGHIỆP

Sinh viên: Vũ Đỗ Bình Mã số sinh viên : K205520114216

Lớp: K56-CĐT03

Ngành: Cơ điện tử Chuyên ngành: Kỹ thuật Cơ điện tử

Ngày giao đề: 6/4/2023 Ngày hoàn thành: 16/6/2023

1. Tên đề tài: Thiết kế robot Fanuc Irmate 200ib 6 bậc tự do

2. Nội dung thuyết minh

1. Tổng quan.

2. Động học tay máy.

3. Chuyên đề

4. Các bản vẽ, chương trình và đồ thị

Đồ họa kết cấu: AutoCad; Đồ thị biến khớp: Matlab; Phương pháp số:
Solver trên Excel; Mô phỏng kết cấu: SolidWorks, Inventor…

TRƯỞNG BỘ MÔN GIÁO VIÊN HƯỚNG DẪN

(Ký và ghi rõ họ tên) (Ký và ghi rõ họ tên)

Dương Quốc Khánh

 ĐÁNH GIÁ CỦA GIÁO VIÊN HƯỚNG DẪN
Tiến trình thông qua đồ án:

Tuần Nội dung thông qua GVHD

1
2
3
4
5
6
7
8
9
10
 NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................

Thái Nguyên, ngày....tháng.....năm 20....

GIÁO VIÊN HƯỚNG DẪN

(Ký ghi rõ họ tên)

 ĐÁNH GIÁ CỦA GIÁO VIÊN CHẤM
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................

...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................

Thái Nguyên, ngày....tháng.....năm 20....

GIÁO VIÊN CHẤM 1 GIÁO VIÊN CHẤM 2

(Ký ghi rõ họ tên) (Ký ghi rõ họ tên)
 MỤC LỤC
CHƯƠNG 01 : TỔNG QUAN………………….…………………1

1.1Tổng quan về robot Fanuc LR mate 200ib……………………..……...…….1

1.1.1. Thông tin về Fanuc LR mate 200ib……………………………..………….…1
1.1.2.Thông số kỹ thuật………………………………………………….…...…….1
1.2. Xác định robot chế tạo……………………………………...........…………3
1.2.1.Mô tả………………………………………………………………………3
1.2.2. Robot Fanuc LR mate 200iB…………………………………….….……3

CHƯƠNG 02: ĐỘNG HỌC ROBOT…………………………………..…….4

2.1. Tổng hợp cấu trúc động học…………………………………………..……4
2.2. Phân tích động học…………………………..……………………………..4
2.2.1. Động học Robot Fanuc LR mate 200ib…………………………….…….4
2.2.2. Xây dựng phương trình động học cho robot FANUC LR MATE 200I…..6
2.2.2.1. Tiến hành đặt hệ trục tọa độ, lập bảng D-H………………………...…..6
2.2.2.2. Viết hệ phương trình động hoc (giải bằng matlab)……………….…….7
2.2.3. Bài toán động học ngược…………………………………………….…..11
2.2.3.1. Thiết lập bài toán, thông số đầu vào…………………………………...13

2.2.3.2. Phương pháp giải bài toán giải bài toán động học ngược robot…...…...14
2.2.3.3. Giải bài toán động học ngược trên excel…………………………….…15

CHƯƠNG 3: QUỸ ĐẠO CHUYỂN ĐỘNG ROBOT…………………..…..19

3.1. Thiết kế quỹ đạo chuyển động trong không gian khớp……………….…..19
3.2 Cá ch xâ y dự ng đườ ng cong nộ i suy……………………………………………………...…….19
3.2.1 Cơ sở lý thuyết……………………………………………..…..…………………………………..….…19
3.2.2 Lập trình trên matlap…………………………………………………….……21
3.4. Bả n vẽ mộ t số khâ u chuyển độ ng………………………………………………………………30
Kết luận đồ án……………………………………………………….31

Lời nói đầu

 Trong sự nghiệp công nghiệp hoá, hiện đại hoá đất nước vấn đề tự động hoá sản xuất có
vai trò đặc biệt quan trọng. Nhu cầu nâng cao năng suất và chất lượng sản phẩm ngày
càng
đòi hỏi ứng dụng rộng rãi các phương tiện tự động hoá sản xuất. Xu hướng tạo ra những
dây chuyền về thiết bị tự động có tính linh hoạt cao đang hình thành. Vì thế ngày càng
tăng
nhanh nhu cầu ứng dụng Robot để tạo ra các hệ thống sản xuất tự động linh hoạt.
Robot thực hiện gắp vật tự động được nghiên cứu và ứng dụng nhiều trong các hệ
thống
sản xuất tự động nhằm tăng chính xác về vị trí, liên tục theo chu trình hoạt động, đảm bảo
quá trình sản xuất ổn định, nâng cao năng xuất và chất lượng sản phẩm.

1.Với đề tài : Thiết kế robot Fanuc LR mate 200ib 6 bậc tự do với các thông số như
sau :

đồ án đã giải quyết được những công việc sau :

 Tìm hiểu tổng quát quá trình phát triển và ứng dụng của robot trong công nghiệp.
 Thiết kế động học Robot.
 Xây dựng quỹ đạo chuyển động Robot.
 Nghiên cứu lý thuyết điều khiển tự động robot công nghệp.
 Xây dựng mô hình mô phỏng trên phần mềm.

Đây là một đề tài rộng với vốn kiến thức lớn, thời gian có hạn, cho nên những kết
quả, những nhận định mà chúng em đã tìm hiểu và đưa ra chắc chắn sẽ có nhiều thiếu
sót. Chúng em thành thật mong được các thầy cô giáo trong bộ môn chỉ bảo thêm để
đề tài hoàn thiện hơn, ý nghĩa hơn. Em bày tỏ lời cảm ơn chân thành đến thầy giáo
“Dương Quốc Khánh” và các thầy cô giáo trong bộ môn đã chỉ bảo tận tình và tạo điều
kiện cho chúng em hoàn thành đồ án.
 CHƯƠNG 01. TỔNG QUAN

1.1. Tổng quan về robot Fanuc LR mate 200ib

1.1.1. Thông tin về Fanuc LR mate 200ib
Sê-ri LR Mate là dòng robot mini điều khiển bằng điện, thế hệ mới nhất của FANUC
Robotics được thiết kế để tải / dỡ máy chính xác, xử lý vật liệu, loại bỏ vật liệu, lắp ráp và
thử nghiệm trong phòng thí nghiệm. Kích thước nhỏ, khéo léo và tốc độ cao của robot làm
cho nó trở thành giải pháp hoàn hảo cho nhiều ứng dụng công nghiệp và thương mại.
Hệ thống điều khiển R-30iB
Công nghệ tiên tiến mới nhất của FANUC hiệu quả, nhỏ gọn và có thể xếp chồng lên
nhau, năng suất được cải thiện, với phần cứng tích hợp hiện đại và hơn 250 chức năng
phần mềm. Được thiết kế để trực quan hơn, nó tối ưu hóa năng lượng để giảm mức tiêu thụ
năng lượng, hiệu suất cao trong thời gian chu kỳ với tốc độ và độ chính xác cao hơn.
Nó có một hệ thống điều khiển PMC lập trình tích hợp hiệu suất cao, có thể truy cập hệ
thống I / O robot hoàn chỉnh, cho phép dễ dàng tách hoặc điều khiển không đồng bộ các
thiết bị ngoại vi mà không ảnh hưởng tiêu cực đến hiệu suất của robot. Các vị trí lắp khác
nhau cho phép bạn tiết kiệm không gian, dễ dàng tích hợp vào các ô sản xuất, lắp đặt các
robot khác nhau. Các hệ thống điều khiển R-30iB sử dụng phần mềm bảo mật được cấp
bằng sáng chế của FANUC, điều đó có nghĩa là bạn không cần phải lo lắng về vi-rút, tin
tặc hoặc các vấn đề ổn định. Với iPguard Touch trực quan, thông minh hơn và dễ sử dụng.
1.1.2.Thông số kỹ thuật

Thông tin về robot:

 Tải trọng tối đa của Robot: 5Kg.

 Tầm với tối đa: 700 mm.
 Độ lặp lại: 0,04 mm.
 Bộ điều khiển: R-30iB

Tốc độ chuyển động của robot:

 Trục 1: 140º / seg

 Trục 2: 150º / seg
 Trục 3: 160º / seg
 Trục 4: 400º / seg
 Trục 5: 330º / seg
 Trục 6: 480º / seg

1
Ứng dụng Robot:
 Tải / dỡ máy
 Xử lý vật liệu
 Kiểm tra và lấy mẫu
 Điện tử / phòng sạch
 Bộ phận làm sạch / giặt
 hội,, tổ hợp
 Loại bỏ vật liệu
 Đóng gói
 Giáo dục

Hình 1: Robot Fanuc LR mate 200ib

2
1.2. Xác định robot chế tạo
1.2.1.Mô tả : Thiết kế robot Fanuc Irmate 200ib 6 bậc tự do với các
thông số như sau.

1.2.2. Robot Fanuc LR mate 200iB

Hình 1.2: Robot Fanuc LR mate 200iB

3
 CHƯƠNG 2: ĐỘNG HỌC ROBOT
2.1. Tổ ng hợ p cấ u trú c độ ng họ c.

 Động học robot nghiên cứu các đặc trưng của chuyển động mà không quan
tâm đến nguyên nhân gây ra chúng như lực và mô men. Khoa học động học
nghiên cứu về vị trí, vận tốc, gia tốc. Do đó, động học chỉ liên quan đến
hình học và thời gian thay đổi của chuyển dộng. Sự thay đổi của các khâu
của robot liên quan đến hướng và vị trí của khâu chấp hành cuối cùng bởi
sự ràng buộc của các khớp. Những quan hệ động học đó là trọng tâm của
việc nghiên cứu động học robot. Việc nghiên cứu động học có hai vấn đề:
Phân tích động học và tổng hợp động học. Tuy nhiên vấn đề phân tích động
học và tổng hợp động học luôn liên quan đến nhau.
 Động học robot nghiên cứu cách giải của 2 lớp bài toán :
 Bài toán động học thuận: Đây là bài toán cho trước chương trình
chuyển động dưới dạng quan hệ hàm qi(t) của các biến khớp, ta cần
phải xác định quy luật biến đổi của các tham số động học đặc trưng cho
chuyển động của các khâu. Việc giải bài toán thuận của động học robot
chủ yếu nhằm thiết lập phương trình động học robot và xác định vị trí
của tay kẹp.
 Bài toán động học ngược: Đây là bài toán ta cần xác định biến khớp
qi(t) đảm bảo cho bộ phận công tác của robot thực hiện các chuyển
động cho trước. Việc giải bài toán ngược là cơ sở để xây dựng phương
trình điều khiển chuyển động của robot.

2.2. Phâ n tích độ ng họ c.

2.2.1. Động học Robot Fanuc LR mate 200ib

4
5
  Từ thông số như trên, sẽ tiến hành lược đồ hóa robot.

Hình 2.1: Lược đồ hóa robot fanuc lr mate 200ib

2.2.2. Xây dựng phương trình động học cho robot FANUC LR MATE 200IB

2.2.2.1. Tiến hành đặt hệ trục tọa độ, lập bảng D-H

 Đặt hệ trục tọa độ  Lập bảng D-H

Bảng 1: Thông số bảng D-H

R (z, θi) T (z,di) T (x,ai) R (x,αi)

1 (θ1) d1= 350 a1= 150 90°

2 (θ2) 0 a2= 250 0°

3 (θ3) 0 0 -90°

4 (θ4) d4= 450 0 90°

5 (θ5) 0 0 -90°

6 (θ6) d6= 80 0 0°

6
2.2.2.2. Viết hệ phương trình động hoc (giải bằng

matlab)  Từ ma trận tổng quát:

 Tiến hành xây dựng các ma trận:

[ ]
Cα 1 0 Sα 1 150∗Cα 1
Sα 1 0 −Cα 1 150∗Sα 1
A01 0 1 0 350
0 0 0 1

[ ]
Cα 2 −Sα 2 0 250∗Cα 2
Sα 2 Cα 2 0 250∗Sα 2
A12
0 0 1 0
0 0 0 1

[ ]
Cα 3 0 Sα 3 0
Sα 3 0 −Cα 3 0
A23
0 1 0 0
0 0 0 1

[ ]
Cα 4 0 −Sα 4 0
Sα 4 0 Cα 4 0
A34
0 1 0 450
0 0 0 1

7
 [ ]
Cα 5 0 Sα 5 0
Sα 5 0 −Cα 5 0
A45 0 1 0 0
0 0 0 1

[ ]
Cα 6 −Sα 6 0 0
Sα 6 Cα 6 0 0
A56 0 0 0 80
0 0 0 0

 Tiến hành dung matlab để nhân ma trận:

𝐴 = G = 𝐴10. 𝐴12. 𝐴23. 𝐴43. 𝐴45. 𝐴65

G= T06 =

[- sin(q6)*(cos(q4)*sin(q1) + sin(q4)*(cos(q1)*cos(q2)*cos(q3)
- cos(q1)*sin(q2)*sin(q3))) - cos(q6)*(cos(q5)*(sin(q1)*sin(q4)
- cos(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3)))
+ sin(q5)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))),
sin(q6)*(cos(q5)*(sin(q1)*sin(q4) -
cos(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))) +
sin(q5)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))) -
cos(q6)*(cos(q4)*sin(q1) + sin(q4)*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))), sin(q5)*(sin(q1)*sin(q4) -
cos(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))) -
cos(q5)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2)),
150*cos(q1) + 250*cos(q1)*cos(q2) + 80*sin(q5)*(sin(q1)*sin(q4)
- cos(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3)))
- 80*cos(q5)*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2)) - 450*cos(q1)*cos(q2)*sin(q3) -
450*cos(q1)*cos(q3)*sin(q2)]
[ sin(q6)*(cos(q1)*cos(q4) + sin(q4)*(sin(q1)*sin(q2)*sin(q3)
- cos(q2)*cos(q3)*sin(q1))) + cos(q6)*(cos(q5)*(cos(q1)*sin(q4)
- cos(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)))
- sin(q5)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))),
cos(q6)*(cos(q1)*cos(q4) + sin(q4)*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1))) - sin(q6)*(cos(q5)*(cos(q1)*sin(q4) -
cos(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))) -
sin(q5)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))), -
sin(q5)*(cos(q1)*sin(q4) - cos(q4)*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1))) - cos(q5)*(cos(q2)*sin(q1)*sin(q3) +

8
cos(q3)*sin(q1)*sin(q2)), 150*sin(q1) + 250*cos(q2)*sin(q1) -
80*sin(q5)*(cos(q1)*sin(q4) - cos(q4)*(sin(q1)*sin(q2)*sin(q3)
- cos(q2)*cos(q3)*sin(q1))) -
80*cos(q5)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))
- 450*cos(q2)*sin(q1)*sin(q3) - 450*cos(q3)*sin(q1)*sin(q2)]
[
cos(q6)*(sin(q5)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) +
cos(q4)*cos(q5)*(cos(q2)*sin(q3) + cos(q3)*sin(q2))) -
sin(q4)*sin(q6)*(cos(q2)*sin(q3) + cos(q3)*sin(q2)),
- sin(q6)*(sin(q5)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) +
cos(q4)*cos(q5)*(cos(q2)*sin(q3) + cos(q3)*sin(q2))) -
cos(q6)*sin(q4)*(cos(q2)*sin(q3) + cos(q3)*sin(q2)),
cos(q5)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) -
cos(q4)*sin(q5)*(cos(q2)*sin(q3) + cos(q3)*sin(q2)),
250*sin(q2) + 450*cos(q2)*cos(q3) - 450*sin(q2)*sin(q3) +
80*cos(q5)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) -
80*cos(q4)*sin(q5)*(cos(q2)*sin(q3) + cos(q3)*sin(q2)) + 350]
[
0,
0,
0,
1]

 Ma trận 4x4 mô tả vị trí và hướng chuyển đổi từ hệ tọa độ gốc O0 sang hệ

tọa độ O6.

 Đồng nhất hệ số với ma trận trên ta được hệ phương trình động học thuận
của Robot sau:
nx =

- sin(q6)*(cos(q4)*sin(q1) + sin(q4)*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))) - cos(q6)*(cos(q5)*(sin(q1)*sin(q4) -
cos(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))) +
sin(q5)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2)))

ny =

sin(q6)*(cos(q1)*cos(q4) + sin(q4)*(sin(q1)*sin(q2)*sin(q3) -

9
cos(q2)*cos(q3)*sin(q1))) + cos(q6)*(cos(q5)*(cos(q1)*sin(q4) -
cos(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))) -
sin(q5)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))

nz =

cos(q6)*(cos(q2 + q3)*sin(q5) + sin(q2 + q3)*cos(q4)*cos(q5)) -

sin(q2 + q3)*sin(q4)*sin(q6)

sx =

sin(q6)*(cos(q5)*(sin(q1)*sin(q4) -
cos(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))) +
sin(q5)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))) -
cos(q6)*(cos(q4)*sin(q1) + sin(q4)*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3)))

sy =

cos(q6)*(cos(q1)*cos(q4) + sin(q4)*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1))) - sin(q6)*(cos(q5)*(cos(q1)*sin(q4) -
cos(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))) -
sin(q5)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))

sz =

- sin(q6)*(cos(q2 + q3)*sin(q5) + sin(q2 + q3)*cos(q4)*cos(q5))

- sin(q2 + q3)*cos(q6)*sin(q4)

ax =

sin(q5)*(sin(q1)*sin(q4) - cos(q4)*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))) - cos(q5)*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))

ay =

- sin(q5)*(cos(q1)*sin(q4) - cos(q4)*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1))) - cos(q5)*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))

10
 az =

cos(q2 + q3)*cos(q5) - sin(q2 + q3)*cos(q4)*sin(q5)

Px =

a1*cos(q1) + d6*(sin(q5)*(sin(q1)*sin(q4) -
cos(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))) -
cos(q5)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))) +
a2*cos(q1)*cos(q2) - 4*cos(q1)*cos(q2)*sin(q3) -
4*cos(q1)*cos(q3)*sin(q2)

Py =

a1*sin(q1) - d6*(sin(q5)*(cos(q1)*sin(q4) -
cos(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))) +
cos(q5)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))) +
a2*cos(q2)*sin(q1) - 4*cos(q2)*sin(q1)*sin(q3) -
4*cos(q3)*sin(q1)*sin(q2)

Pz =

d1 + 4*cos(q2 + q3) + a2*sin(q2) - (d6*sin(q2 + q3)*sin(q4 +

q5))/2 + d6*cos(q2 + q3)*cos(q5) + (d6*sin(q4 - q5)*sin(q2 +
q3))/2

2.2.3. Bài toán động học ngược.

 Bài toán thuận động học nhằm xác định định vị và định hướng của phần
công tác khi cho trước các biến khớp. Bài toán ngược cho trước vị trí và
định hướng của khâu tác động sau cùng đòi hỏi phải xác định bộ thông số
tọa độ suy rộng để đảm bảo chuyển động cho trước của phần công tác. Đối
với các tay máy dạng chuỗi động hở, ứng với một bộ thông số mô tả định vị
và định hướng của phần công tác khi giải bài toán ngược có thể xảy ra các
trường hợp:
 Có thể có nhiều lời giải khác nhau.
 Các phương trình đồng nhất thức thường có dạng phi tuyến.
 Có thể gặp nghiệm vô định vì có các liên kết thừa.

11
  Có thể có lời giải toán học, song lời giải này không chấp nhận được
về mặt vật lí do kết cấu của cấu trúc không đáp ứng được.
 Nhìn chung khi số bậc tự do càng lớn thì bài toán ngược càng khó giải, số
nghiệm toán học lại càng nhiều, để chọn được nghiệm điều khiển đòi hỏi
phải loại bỏ các nghiệm không phù hợp dựa trên cơ sở các ràng buộc về
giới hạn hoạt động của các khớp. Việc lựa chọn phương pháp để giải bài
toán ngược cũng là một vấn đề vì không có phương pháp tổng quát nào có
thể áp dụng cho tất cả các robot.

 Giả sử quỹ đạo chuyển động của robot khi làm việc:

Hình 2.2: Quỹ đạo làm việc của robot

12
 Hình 2.3.Mô phỏng trên autocad , lấy tọa độ

Điểm Px Py Pz Sx Ax Ay
1 -200 100 100 0 -1 0
2 -187.94 140 100 0 -1 0
3 -153.21 209.22 100 0 -1 0
4 -100 249 100 0 -1 0
5 -34.73 267 100 0 -1 0
6 34.73 288 100 0 -1 0
6 100 268 100 0 -1 0
6 153.21 228.56 100 0 -1 0
6 187.94 168 100 0 -1 0
6 200 100 100 0 -1 0

Hình 2.4: Thông số các điểm

2.2.3.1. Thiết lập bài toán, thông số đầu vào.

a.1 Thiết lập bài toán:

13
 Cho vị trí và hướng của bàn kẹp tức là biết ma trận AP . Cần phải xác định
các biến khớp qi (i=1 ... 6) theo vị trí và hướng bàn kẹp.

 Input: Ma trận T là tích các ma trận thành phần đã tính ở bài toán thuận. Ma
trận
A là tọa độ thực đã biết.
 Output: Kết quả của biến khớp qi ( i = 1,2,3,4,5,6 ).

a.2 Thông số đầu vào:

 d1=350
 a1=150
 a2=250
 d4=450
 d6=80

2.2.3.2. Phương pháp giải bài toán giải bài toán động học ngược robot

 Sử dụng pháp số GRG Nonlinear để giải bài toán: Phương pháp này là
tìm giá trị gần đúng nhất của biến khớp mà sai số của nó nằm trong phạm vi
cho phép. Cân bằng các phần tử của hai ma trận tọa độ lý thuyết và tọa độ
thực ta có hệ phương trình.
 Bài toán cần giải động học ngược của cơ cấu là: cho biết vị trí tay kẹp so
với các khớp q1 q2 … để xử lý bài toàn ta cần xử dụng tới excel. Với các giá
trị ta có như sau:
 Từ ma trận ta được hệ phương trình động học nghịch:

 Chúng ta đi tìm Min(L) =0 với L =L1+L2+L3+L4+L5+L6

Với a14, a24, a34, a22, a23, a33 là các tọa độ thực đã biết.

14
2.2.3.3. Giải bài toán động học ngược trên excel.

 Bước 1: Nhập các dữ liệu cần thiết cho việc tính toán.
 Dữ liệu về độ dài các khâu: d1=350, a1=150a2=250, d4=450, d6=80
 Dữ liệu về Px,Py,Pz,Sx,Ax,Ay. Vì robot có 6 bậc tự do nên ta chỉ
cần lấy 6 phương trình.
 Khởi tạo các biến khớp q1, q2, q3,q4,q5,q6
 Khởi tạo các giá trị trong ma trận A: a11, a24, a34, a12, a13, a23.
 Các giá trị đã biết của biến khớp.
 Bước 2: Tính các giá trị L và tổng của chúng.
 L1=  L2=
 L3=
 L4=
 L5=(𝐴𝑦 − 𝑎23)2
 L6=(𝐴𝑧 − 𝑎33)2
 L=L1+L2+L3+L4+L5_L6
 Bước 3: Dùng công cụ Solver tìm biến khớp q1,q2,q3,q4,q5,q6.
 Chọn gói công cụ Solver bằng đường dẫn


Hình 2.5: Chọn gói công cụ Solver

+ Khai báo nhãn cho các thông số đầu vào và gán giá trị ban đầu bằng 0, Tính các
phần tử L và tính tổng bằng hàm SUM:

15
+ Thiết lập trong Solver ( các biến q i, hàm F, add, option)

16
+ vào thanh công cụ Data chọn solver để bắt đầu thực hiện tìm ngiệm

Bảng giới hạn giá trị biến khớp trong slover

+ Nhập giá trị tọa độ tay kẹp tại vị trí P1 và dùng công cụ Slover của excel ta
giải được các biến khớp:

Bảng 2: Thông số đầu vào.

17
Bảng 3: Thông số các biến khớp

Hình 2.6: Ảnh mô phỏng trên excel

18
CHƯƠNG 3: QUỸ ĐẠ O CHUYỂ N ĐỘ NG ROBOT
3.1. Thiết kế quỹ đạ o chuyển độ ng trong khô ng gian khớ p.

Hình 3.1 : Không gian quỹ đạo làm việc

3.2 Cách xây dựng đường cong nội suy
3.2.1 Cơ sở lý thuyết
Quỹ đạo không gian khớp có dạng hàm bậc 3:
u ( t )=ai t + bi t +c i t +d i(*)
3 2

Phương trình tiếp tuyến với quỹ đạo U(t) hay phương trình vận tốc tại thời
điểm t là :
u' ( t )=3.a i t 2+ 2. bi t+ c i

19
 Hình 3.2: Sơ đồ tính toán hệ số góc tiếp tuyến chung tại Pi .
- Xét ui (t ) :
Điều kiện về vận tốc công nghệ tại điểm đầu và cuối quỹ đạo.
u̇=0
- Điều kiện đi qua 2 điểm của một phương trình:

{ui (t i−1 )=q i−1

u i (t i )=q i
- Điều kiện chuyển tiếp trơn tại điểm chuyển tiếp:
i i−1 i+ 1 i
q −q q −q
k i= ;k i +1=
t i−t i−1 t i+ 1−t i
- Chúng ta xét dấu của k i với k i+1 :
+ Nếu sig ( k i ) ≠ sig ( k i+1 ) → v i=0
k i +k i +1
+ Nếu sig ( k i )=sig ( k i+1 ) → v i=Kc=
2

{
3 2 i−1
ui (t i −1 )=a i t i−1 + bi t i−1 + ci t i−1+ di =q
ui ( t i ) =ai t i3 + bi t i2+ c i t i +d i=q i
Ta có hệ phương trình: 2
u̇i ( t i−1 ) =3. ai t i−1 + 2. bi t i−1 + ci=v i−1
2
u̇i ( t i )=3. ai t i +2 . bi t i +c i ¿ v i

Trong đó :

- Hai phương trình điều kiện đi qua : và .

- Hai phương trình điều kiện vận tốc : và .

* Thông thường chúng ta cho trước vận tốc tại điểm làm việc ban đầu bằng 0 :

20
Giải hệ phương trình trên ta được các giá trị ai, bi, ci, di để thay vào phương trình ui (t )
.
Bảng 3.1 : Hệ số góc k

Bảng 3.2 : Hệ số góc chuyển tiếp kc

3.2.2 Lập trình trên matlap

Dùng plot trong matlab để vẽ đồ thị.

t1=0;
t2=1;
t3=2;
t4=3;
t5=4;
t6=5;
t7=6;
t8=7;
t9=8;
t10=9;
t11=10;

A1=[t1.^3 t1.^2 t1 1 ; t2.^3 t2.^2 t2 1 ; 3.*t1.^2 2.*t1 1 0; 3.*t2.^2 2.*t2 1 0]

A2=[t2.^3 t2.^2 t2 1 ; t3.^3 t3.^2 t3 1 ; 3.*t2.^2 2.*t2 1 0; 3.*t3.^2 2.*t3 1 0]

21
A3=[t3.^3 t3.^2 t3 1 ; t4.^3 t4.^2 t4 1 ; 3.*t3.^2 2.*t3 1 0; 3.*t4.^2 2.*t4 1 0]
A4=[t4.^3 t4.^2 t4 1 ; t5.^3 t5.^2 t5 1 ; 3.*t4.^2 2.*t4 1 0; 3.*t5.^2 2.*t5 1 0]
A5=[t5.^3 t5.^2 t5 1 ; t6.^3 t6.^2 t6 1 ; 3.*t5.^2 2.*t5 1 0; 3.*t6.^2 2.*t6 1 0]
A6=[t6.^3 t6.^2 t6 1 ; t7.^3 t7.^2 t7 1 ; 3.*t6.^2 2.*t6 1 0; 3.*t7.^2 2.*t7 1 0];

A7=[t7.^3 t7.^2 t7 1 ; t8.^3 t8.^2 t8 1 ; 3.*t7.^2 2.*t7 1 0; 3.*t8.^2 2.*t8 1 0];

A8=[t8.^3 t8.^2 t8 1 ; t9.^3 t9.^2 t9 1 ; 3.*t8.^2 2.*t8 1 0 ; 3.*t9.^2 2.*t9 1 0];

A9=[t9.^3 t9.^2 t9 1 ; t10.^3 t10.^2 t10 1 ; 3.*t9.^2 2.*t9 1 0 ; 3.*t10.^2 2.*t10 1 0];
A10=[t10.^3 t10.^2 t10 1; t11.^3 t11.^2 t11 1 ; 3*t10.^2 2.*t10 1 0 ; 3.*t11.^2 2.*t11 1
0];

%Q1
B1=[2.446903; 2.227772;0; -0.26808];

B2=[2.227772; 1.910746; ;--0.26808; - 0.28258];

B3=[1.910746; 1.662615; - 0.28258; -0.24317];

B4=[1.662615; 1.424401; -0.24317; -0.2126];

B5=[1.424401; 1.237424; -0.2126; -0.20006];

B6=[1.237424; 1.024277; -0.20006; -0.21288];

B7=[1.024277; 0.811666; -0.22246; -0.22246];

B8=[0.811666; 0.579361; -0.2293; -0.2293];

B9=[0.579361; 0.353069; -0.2293; 0];

%Q2
B1=[-1.53064; -1.44743; 0; 0.134011];

B2=[-1.44743; -1.26261; 0.134011; 0.151851];

B3=[-1.26261; -1.14373; 0.151851; 0.101717];

B4=[-1.14373; -1.05918; 0.101717; 0.111926];

B5=[-1.05918; -0.91988; 0.111926; 0.086347];

B6=[ -0.91988; -0.88649; 0.086347; 0.020261];

22
B7=[-0.88649; -0.87935; 0.020261 ; 0];

B8=[-0.87935; -0.91425; 0; -0.04875];

B9=[ -0.91425; -0.97686; -0.04875; 0];

%Q3
B1=[2.803155; 2.878232; 0; 0.098423];

B2=[2.878232; 3; 0.098423; 0.060884];

B3=[3;3; 0.060884;0]
B4=[3;3;0;0];
B5=[3;3;0;0];
B6=[3;3;0;0];
B7=[3;3;0;0];
B8=[3;3;0;0];
B9=[3;3;0;0];

%Q4
B1=[-1.88682; -1.63986; 0; 0.259347];

B2=[-1.63986; -1.36812; 0.259347; 0.177423];

B3=[-1.36812; -1.28502; 0.177423; 0.063165];

B4=[-1.28502; -1.24179; 0.063165; 0.121356];

B5=[-1.24179; -1.04231; 0.121356; 0.118406];

B6=[-1.04231; -1.00498; 0.118406; 0];

B7=[-1.00498; -1.00975; 0; -0.04824];

B8=[-1.00975; -1.10147; -0.04824; -0.10687];

B9=[-1.10147; -1.22349; -0.10687; 0];

%Q5
B1=[2.402796; 2.225284; 0; -0.2621];

23
B2=[2.225284; 1.878589; -0.2621; -0.32036];
B3=[1.878589; 1.584561; -0.32036; -0.2802];

B4=[1.584561; 1.318193; -0.2802; -0.24458];

B5=[1.318193; 1.095403; -0.24458; -0.19761];

B6=[1.095403; 0.922973; -0.19761; -0.1727];

B7=[0.922973; 0.750007; -0.1727; -0.1901];

B8=[0.750007; 0.542766; -0.1901; -0.20789];

B9=[0.542766; 0.334221; -0.20789; 0];

%Q6
B1=[1.140024; 1.41779; 0; 0.325039];

B2=[1.41779; 1.790103; 0.325039; 0.218525];

B3=[1.790103; 1.85484; 0.218525; 0];

B4=[1.85484; 1.830124; 0; -0.07515];

B5=[1.830124; 1.704539; -0.07515; -0.13441];

B6=[1.704539; 1.561296; -0.13441; -0.13211];

B7=[1.561296; 1.440309; -0.13211; -0.08974];

B8=[1.440309; 1.381814; -0.08974; -0.03431];

B9=[1.381814; 1.371684; -0.03431; 0];

X1=inv(A1)*B1
X2=inv(A2)*B2
X3=inv(A3)*B3
X4=inv(A4)*B4
X5=inv(A5)*B5
X6=inv(A6)*B6
X7=inv(A7)*B7

24
X8=inv(A8)*B8
X9=inv(A9)*B9

T1=0:0.01:1
T2=1:0.01:2
T3=2:0.01:3
T4=3:0.01:4
T5=4:0.01:5
T6=5:0.01:6
T7=6:0.01:7
T8=7:0.01:8
T9=9:0.01:9
T10=10:0.01:10
T11=11:0.01:11

u1=X1(1).*T1.^3 +X1(2).*T1.^2+ X1(3).*T1 +X1(4)

u2=X2(1).*T2.^3 +X2(2).*T2.^2+ X2(3).*T2 +X2(4)
u3=X3(1).*T3.^3 +X3(2).*T3.^2+ X3(3).*T3 +X3(4)
u4=X4(1).*T4.^3 +X4(2).*T4.^2 +X4(3).*T4 +X4(4)
u5=X5(1).*T5.^3 +X5(2).*T5.^2 +X5(3).*T5 +X5(4)
u6=X6(1).*T6.^3 +X6(2).*T6.^2 +X6(3).*T6 +X6(4)
u7=X7(1).*T7.^3+X7(2).*T7.^2 +X7(3).*T7 +X7(4)
u8=X8(1).*T8.^3+X8(2).*T8.^2+X8(3).*T8+X8(4)
u9=X9(1).*T9.^3+X9(2).*T9.^2+X9(3).*T9+X9(4)

plot ( T1,u1,'linewidth',3)
hold on
grid on
plot ( T2,u2,'linewidth',3)
hold on
grid on
plot ( T3,u3,'linewidth',3)
hold on
grid on
plot ( T4,u4,'linewidth',3)
hold on
grid on
plot ( T5,u5,'linewidth',3)
hold on
grid on
plot ( T6,u6,'linewidth',3)
hold on
grid on
plot ( T7,u7,'linewidth',3)
hold on
grid on

25
plot(T8,u8,'linewidth',3)
hold on
grid on
plot(T9,u9,'linewidth',3)
hold on
grid on

grid on

3.3 Đồ thị sự thay đổi của các biến khớp tại cái vị trí theo thời gian

Hình 3.3: Sự thay đổi của biến khớp q1 theo thời gian

26
Hình 3.4: Sự thay đổi của biến khớp q2 theo thời gian

Hình 3.5: Sự thay đổi của biến khớp q3 theo thời gian

27
Hình 3.6: Sự thay đổi của biến khớp q4 theo thời gian

Hình 3.7: Sự thay đổi của biến khớp q5 theo thời gian

28
Hình 3.8: Sự thay đổi của biến khớp q6 theo thời gian

29
  3.4. Bản vẽ một số khâu chuyển động
Khâu 6

Khâu 1

30
 KẾ T LUẬ N ĐỒ Á N
1. Khá i quá t lạ i nộ i dung đồ á n
 Tiến hành lựa chọn robot Fanuc Irmate 200ib của hãng Fanuc đáp ứng được
yêu cầu công nghệ.
 Tiến hành mô hình hoá, xây dựng phương trình động học cho robot, giải bài
toán động học thuận và động học ngược.
 Tiến hành mô phỏng quỹ đạo chuyển động cho robot.
 Tiến hành thiết kế kết cấu khớp thứ ba của robot đã chọn.
2. Kiến thứ c đã tích luỹ
Đây là đồ án đầu tiên mà em được thực hiện, thông qua đó, em được rèn luyện
tính tự giác, độc lập và đặc biệt là tính kiên trì chủ động.
Em đã học được cách sử dụng cơ bản của các phần mềm thông dụng như
Matlab, Autocad, …Đồng thời tìm hiểu được thêm những thông tin tổng quan về
Robot công nghiệp và thấy được tầm quan trọng của Robot trong cuộc sống hiện đại.
Qua đó hiểu được quy trình về thiết kế một sản phẩm robot và biết được cách kết hợp
được những kiến thức lý thuyết được học trên lớp với việc tra cứu tài liệu để tìm ra lời
giải cho được bài toán yêu cầu.
3. Ưu - nhượ c điểm.
Đồ án của em về cơ bản đã hoàn thành được nội dung lý thuyết, các yêu cầu
thiết kế và đã đạt được yêu cầu về kỹ thuật, ứng dụng. Robot có thể di chuyển được
cánh tay, mang theo tay kẹp để kẹp gắp vật từ vị trí này đến vị trí kia.
Tuy nhiên, do hạn chế về thời gian thực hiện đồ án cũng như kiến thức chuyên
môn còn chưa nắm vững và cũng là lần đầu làm đồ án nên chưa có kinh nghiệm, đồ án
của em chắc chắn vẫn còn nhiều hạn chế như tốc độ xử lý, điều khiển…
Em rất mong nhận được lời nhận xét của các thầy cô để đề tài đồ án của em có
thể hoàn thiện hơn. Em xin chân thành cảm ơn!
4. Hướng phát triển đề tài
Robot được ứng dụng tương đối rộng rãi trong các nghành công nghiệp
hiện đại. Như trong các nghành công nghiệp lớn như Đóng tàu, hầm mỏ, cơ khí
… thay thế con người và chất lượng làm việc tốt hơn con người.

Chúng ta có thể lập trình cho robot thông minh hơn, cách chi tiết cơ khí
và động cơ, dùng các tính toán cụ thể để tăng cường độ chính xác cho robot và
có khả năng làm việc hiệu quả hơn. Cải tiến kỹ thuật xây dựng dây truyền tự
động phục vụ như cầu phát triển của con người trong tương lai.

31
Một lần nữa em xin chân thành cảm ơn thầy giáo Dương Quốc Khánh và các
thầy cô đã tạo điều kiện và giúp đỡ em rất nhiều để em hoàn thành bản đồ án
Robot công nghiệp này. Trong quá trình thực hiện khó tránh khỏi có sai sót
mong quý thầy cô đóng góp và góp ý để đề tài được hoàn thiện hơn.

Em xin chân thành cảm ơn!

TÀ I LIỆ U THAM KHẢ O

1. Phạm Thành Long: Robot Công nghiệp, 2010. NXB Khoa học và Kỹ thuật,
Hà Nội – 2010.

2. Nguyễn Văn Khang: Động lực học hệ nhiều vật. NXB Khoa học và Kỹ thuật,
Hà Nội, 2007.

3. Bộ môn Cơ điện tử; Bài giảng Rô bốt Công nghiệp; Trường Đại học Kỹ thuật
Công nghiệp Thái Nguyên.

4. Nguyễn Thiện Phúc; Robot Công nghiệp, 2002. NXB Khoa học và Kỹ thuật,
Hà Nội – 2002.

5. Nguyễn Văn Khang: Robot Công nghiệp, 2011. NXB Giáo dục, Hà Nội,
2011.

6. Trần Thế San, TS Nguyễn Tiến Dũng: Cơ sở nghiên cứu & sáng tạo ROBOT-
Nhà xuất bản thống kê – 2003.

32
33
 KẾ T LUẬ N ĐỒ Á N
1. Khá i quá t lạ i nộ i dung đồ á n
 Tiến hành lựa chọn robot Fanuc Irmate 200ib của hãng Fanuc đáp ứng được
yêu cầu công nghệ.
 Tiến hành mô hình hoá, xây dựng phương trình động học cho robot, giải bài
toán động học thuận và động học ngược.
 Tiến hành mô phỏng quỹ đạo chuyển động cho robot.
 Tiến hành thiết kế kết cấu khớp thứ ba của robot đã chọn.
2. Kiến thứ c đã tích luỹ
Đây là đồ án đầu tiên mà em được thực hiện, thông qua đó, em được rèn luyện
tính tự giác, độc lập và đặc biệt là tính kiên trì chủ động.
Em đã học được cách sử dụng cơ bản của các phần mềm thông dụng như
Matlab, Autocad, …Đồng thời tìm hiểu được thêm những thông tin tổng quan về
Robot công nghiệp và thấy được tầm quan trọng của Robot trong cuộc sống hiện đại.
Qua đó hiểu được quy trình về thiết kế một sản phẩm robot và biết được cách kết hợp
được những kiến thức lý thuyết được học trên lớp với việc tra cứu tài liệu để tìm ra lời
giải cho được bài toán yêu cầu.
3. Ưu - nhượ c điểm.
Đồ án của em về cơ bản đã hoàn thành được nội dung lý thuyết, các yêu cầu
thiết kế và đã đạt được yêu cầu về kỹ thuật, ứng dụng. Robot có thể di chuyển được
cánh tay, mang theo tay kẹp để kẹp gắp vật từ vị trí này đến vị trí kia.
Tuy nhiên, do hạn chế về thời gian thực hiện đồ án cũng như kiến thức chuyên
môn còn chưa nắm vững và cũng là lần đầu làm đồ án nên chưa có kinh nghiệm, đồ án
của em chắc chắn vẫn còn nhiều hạn chế như tốc độ xử lý, điều khiển…
Em rất mong nhận được lời nhận xét của các thầy cô để đề tài đồ án của em có
thể hoàn thiện hơn. Em xin chân thành cảm ơn!
4. Hướng phát triển đề tài
Robot được ứng dụng tương đối rộng rãi trong các nghành công nghiệp
hiện đại. Như trong các nghành công nghiệp lớn như Đóng tàu, hầm mỏ, cơ khí
… thay thế con người và chất lượng làm việc tốt hơn con người.

Chúng ta có thể lập trình cho robot thông minh hơn, cách chi tiết cơ khí
và động cơ, dùng các tính toán cụ thể để tăng cường độ chính xác cho robot và
có khả năng làm việc hiệu quả hơn. Cải tiến kỹ thuật xây dựng dây truyền tự
động phục vụ như cầu phát triển của con người trong tương lai.

34
Một lần nữa em xin chân thành cảm ơn thầy giáo Dương Quốc Khánh và các
thầy cô đã tạo điều kiện và giúp đỡ em rất nhiều để em hoàn thành bản đồ án
Robot công nghiệp này. Trong quá trình thực hiện khó tránh khỏi có sai sót
mong quý thầy cô đóng góp và góp ý để đề tài được hoàn thiện hơn.

Em xin chân thành cảm ơn!

TÀ I LIỆ U THAM KHẢ O

1. Phạm Thành Long: Robot Công nghiệp, 2010. NXB Khoa học và Kỹ thuật,
Hà Nội – 2010.

2. Nguyễn Văn Khang: Động lực học hệ nhiều vật. NXB Khoa học và Kỹ thuật,
Hà Nội, 2007.

3. Bộ môn Cơ điện tử; Bài giảng Rô bốt Công nghiệp; Trường Đại học Kỹ thuật
Công nghiệp Thái Nguyên.

4. Nguyễn Thiện Phúc; Robot Công nghiệp, 2002. NXB Khoa học và Kỹ thuật,
Hà Nội – 2002.

5. Nguyễn Văn Khang: Robot Công nghiệp, 2011. NXB Giáo dục, Hà Nội,
2011.

6. Trần Thế San, TS Nguyễn Tiến Dũng: Cơ sở nghiên cứu & sáng tạo ROBOT-
Nhà xuất bản thống kê – 2003.

35