ScaraRobot 20170852 5 9

Đồ án thiết kế hệ thống điều khiển
Nội dung
LỜI NÓI ĐẦU ........................................................................................................................................ 4
BẢNG PHỤ LỤC ................................................................................................................................... 5
DANH SÁCH HÌNH VẼ ................................................................................................................... 5
DANH SÁCH BẢNG ........................................................................................................................ 9
Chương 1. Phân tích nguyên lý và thông số kỹ thuật........................................................................11
1.1. Tổng quan: ................................................................................................................................ 11
1.2. Nguyên lý hoạt động: ............................................................................................................... 13
1.3. Các thành phần của hệ thống điều khiển: .............................................................................. 14
1.4. Thông số kỹ thuật cần thiết kế của robot:.............................................................................. 15
1.5. Bài toán điều khiển Robot: ...................................................................................................... 18
Chương 2. Thiết kế hệ thống cơ khí....................................................................................................21
Mô hình nghiên cứu ........................................................................................................................ 21
2.1. Động học robot SCARA ........................................................................................................... 22
2.1.1. Động học thuận robot........................................................................................................ 22
2.1.2. Động học ngược robot:...................................................................................................... 25
2.1.3. Ý nghĩa của bài toán động học robot:.............................................................................. 29
2.2. Động lực học robot SCARA: ................................................................................................... 30
2.2.1. Động lực học thuận: .......................................................................................................... 31
2.2.2. Động lực học ngược:.......................................................................................................... 40
2.3. Vật liệu, khối lượng, momen quán tính các khâu: ................................................................ 41
2.3.1. Khâu 0 (Khâu cố định): .................................................................................................... 41
2.3.2. Khâu 1 và khâu 2: ............................................................................................................. 43
GVHD: TS. Mạc Thị Thoa SVTH: Nguyễn Tiến Phát

1
2.3.3. Khâu 3: ............................................................................................................................... 47
Chương 3. Không gian làm việc và thiết kế quỹ đạo Robot .............................................................49
3.1. Không gian làm việc:................................................................................................................ 49
3.2. Thiết lập quy luật chuyển động của 3 khâu: .......................................................................... 51
Chương 4. Mô hình hóa các khâu của Robot.....................................................................................56
Tổng quan: ....................................................................................................................................... 56
4.1. Động cơ điện một chiều: .......................................................................................................... 57
4.2. Trục vít me đai ốc bi: ............................................................................................................... 59
4.3. Mô hình hóa khâu vít me đai ốc bi (Khâu 3): ........................................................................ 61
4.4. Mô hình hóa khâu 1 và 2: ........................................................................................................ 63
4.5. Xác định các thông số của động cơ: ........................................................................................ 64
4.6. Tính toán và kiểm tra tính ổn định của các hàm truyền G(s): ............................................. 65
4.7. Mô phỏng và đánh giá tính ổn định của các chỉ tiêu chất lượng: ........................................ 80
4.7.1. Xây dựng mô hình Simulink mô tả cho một khớp của Robot: ...................................... 80
4.7.2. Sử dụng Matlab – Simulink để mô phỏng cho một trục chuyển động và cho toàn bộ
Robot: ........................................................................................................................................... 81
4.7.3. Xác định sai lệch tĩnh và đánh giá chất lượng đáp ứng: ................................................ 85
Chương 5. Thiết kế hệ thống điều khiển ............................................................................................89
5.1. Các phương pháp điều khiển: ................................................................................................. 89
5.1.1. Phương pháp điều khiển momen: .................................................................................... 89
5.1.2. Phương pháp điều khiển PID: ........................................................................................ 100
5.2. Thiết kế bộ điều khiển bám quỹ đạo: ................................................................................... 108
5.2.1. Bộ điều khiển bám quỹ đạo: ........................................................................................... 108

2
5.2.2. Chọn hệ số cho bộ điều khiển bám quỹ đạo:................................................................. 110
Chương 6. Mô phỏng nguyên lý hoạt động (SIMMECHANICS) ..................................................111
Chương 7. Lựa chọn thiết bị và thiết kế mạch điều khiển ..............................................................112
7.1.Chọn các phần tử của hệ thống điều khiển: .......................................................................... 112
7.2. Xác định hàm điều khiển:...................................................................................................... 115
7.2.1. Điều khiển vị trí (Position): ............................................................................................ 116
7.2.2. Điều khiển tốc độ (Speed): .............................................................................................. 122
7.2.3. Điều khiển lực, momen (Torque): .................................................................................. 125
7.3. Xây dựng bản vẽ:.................................................................................................................... 130
Tài liệu tham khảo: ............................................................................................................................139

3
LỜI NÓI ĐẦU

Thế kỷ 21 là thế kỷ của khoa học kỹ thuật tiên tiến và hiện đại. Những sản
phẩm của nó xuất hiện thường xuyên trong cuộc sống của con người trên toàn thế
giới. Từ những công việc nhẹ nhàng cho đến những công việc khó khăn và nguy
hiểm đều đã có máy móc làm thay con người. Và các ngành khoa học kỹ thuật vì
thế mà càng ngày càng phát triển, trong đó có ngành Cơ Điện Tử.
Với sự phát triển mạnh mẽ như vậy của nền khoa học kỹ thuật đòi hỏi con
người phải hiểu biết nhiều hơn để làm chủ những công cụ hiện đại. Vì vậy, với
những kiến thức em được thầy cô truyền đạt là cơ sở ban đầu để em nghiên cứu
và chiếm lĩnh những kiến thức cao hơn nữa.
Đồ án mà em được giao là “Thiết kế hệ thống điều khiển cho robot”.
Trong quá trình thực hiện đồ án với sự hướng dẫn tận tình của thầy cô và
sự cố gắng của bản thân và đến bây giờ thì em đã hoàn thành đồ án. Dù đã rất cẩn
thận và cố gắng nhưng đây là lần đầu tiên em làm đồ án nên không thể tránh khỏi
những sai sót, em mong được các thầy cô góp ý để đồ án của em được hoàn thiện
hơn nữa.
Em xin chân thành cảm ơn sự hướng dẫn tận tình của cô Mạc Thị Thoa
cùng các thầy cô trong viện cơ khí trường Đại học Bách Khoa Hà Nội.
Hà nội, ngày 13 tháng 9 năm 2021
Sv: Nguyễn Tiến Phát

4
BẢNG PHỤ LỤC

DANH SÁCH HÌNH VẼ
STT Hình Tên hình Trang
1 1.1 Denso HM-G Series 12
2 1.2 Các thành phần chính của robot 13
3 1.3 Sơ đồ động học của robot 14
4 1.4 Các thông số kĩ thuật chính của Robot 15
5 1.5 Giới hạn miền làm việc 16
6 1.6 Phân loại các phương pháp điều khiển robot 19
7 1.7 Sơ đồ khối hệ thống điều khiển quỹ đạo robot 19
8 2.1 Mô hình robot scara 21
9 2.2 Sơ đồ động học Robot SCARA 3 bậc tự do 22
10 2.3 Mô phỏng matlab Robot Scara 30
11 2.4 Khâu 0 42
12 2.5 Khối lượng, momen quán tính của khâu 0 42
13 2.6 Khâu 1 44
15 2.8 Khâu 2 45
17 2.10 Khâu 3 48
19 3.1 Không gian làm việc của robot trong bài toán thuận (góc 50
nhìn từ trên xuống)

5
20 3.2 Không gian làm việc của robot trong bài toán thuận (góc 50
nhìn chéo)
21 3.3 Biểu đồ vị trí tương ứng theo thời gian của khâu 1 54
24 4.1 Các thành phần hệ thống điều khiển 56
25 4.2 Tổng quan hệ thống điều khiển 57
26 4.3 Mô hình hóa động cơ điện 1 chiều 57
27 4.4 Bộ truyền vít me đai ốc bi 59
28 4.5 Mô hình hóa chuyển động của khâu 3 61
29 4.6 Mô hình hóa chuyển động của khâu 1, khâu 2 63
30 4.7 Mô hình tính momen quán tính cho khớp 1 66
31 4.8 Kết quả tính cho khớp 1 66
32 4.9 Tính toán hàm truyền khâu 1 68
33 4.10 Đồ thị Nyquist của hàm truyền G1 (s) 69
34 4.11 Đồ thị Bode của hàm truyền G1 (s) 70

6

45 4.22 Sơ đồ simulink cho động cơ servo DC 81
46 4.23 Sơ đồ simulink khớp 1 với hồi tiếp âm 82
51 4.26 Sơ đồ simulink mô phỏng cho toàn bộ robot 84
52 4.27 Khối Robot trong Simulink 85
53 4.28 Đồ thị đáp ứng của khớp 1 86
56 5.1 Sơ đồ hệ thống điều khiển robot Scara (Phương pháp 92
điều khiển momen)
57 5.2 Sơ đồ hệ thống theo phương pháp điều khiển momen 92
trên Matlab/simulink
58 5.3 Thông số quỹ đạo 93
59 5.4 Các khối chức năng chính 93
60 5.5 Khối PD 94
61 5.6 Khối tính lực dẫn động 94
62 5.7 Khối động lực học ngược 95
63 5.8 Sơ đồ mô phỏng 3D Robot 96
64 5.9 Đồ thị biến khớp mong muốn và đáp ứng 1 97

7
65 5.10 Sai số biến khớp 1 97

70 5.15 Mô phỏng Robot Scara 100
71 5.16 Sơ đồ khối bộ điều khiển PID 100
72 5.17 Sơ đồ simulink một trục khớp 102
73 5.18 Sơ đồ Simulink khối Controller 102
74 5.19 Block PID controller trên Matlab 102
75 5.20 Cửa sổ PID Tuning 103
76 5.21 Mục Tuning Tools và Results 103
77 5.22 Đồ thị đáp ứng biến khớp của trục khớp sau khi đã 104
chỉnh các hệ số PID
84 5.29 Sơ đồ simulink khối controller 109
85 5.30 Sơ đồ simulink điều khiển bám quỹ đạo 110
86 6.1 Robot Scara trong Matlab Simulink 111

8
87 7.1 Sơ đồ khối của hệ thống điều khiển 112

88 7.2 Họ driver điều khiển lớp B 114
89 7.3 PLC MITSUBISHI FX3G-40MR/DS 115
90 7.4 Kết nối điều khiển vị trí 116
91 7.5 Kết nối điều khiển lực, momen 126
92 7.6 Sơ đồ điện của hệ thống 130
93 7.7 Mô hình quỹ đạo chuyển động 131
DANH SÁCH BẢNG

STT Bảng Tên bảng Trang
1 1.1 Thông số kĩ thuât của HM-4A604G 17
2 2.1 Bảng tham số động học Denavit – Hartenberg 23
3 2.2 Các tham số động học 31
4 4.1 Thông số động cơ servo DC được sử dụng trên Khâu 1 64
- Scara Robot.
5 4.2 Thông số động cơ servo DC được sử dụng trên Khâu 65
2,3 - Scara Robot.
6 7.1 Bảng lựa chọn động cơ và driver Panasonic 113
7 7.2 Các thông số liên quan đầu vào xung (Điều khiển vị trí) 117
8 7.3 Các thông số liên quan đến điều khiển xung (Điều 117
khiển vị trí)
9 7.4 Các thông số liên quan đến lọc lệnh vị trí 118
10 7.5 Các thông số liên quan đến cải tạo xung 118

9
11 7.6 Các thông số liên quan đến xóa bộ đếm độ lệch 120
12 7.7 Các thông số liên quan đến vị trí đầu ra 120
13 7.8 Các thông số liên quan đến ức chế xung lệnh 121
14 7.9 Các thông số liên quan đến điều khiển tốc độ 122
15 7.10 Các thông số liên quan đến kiểm soát vận tốc bằng lệnh 123
tốc độ nội suy
16 7.11 Các thông số liên quan đến chức năng kẹp không tốc độ 124
17 7.12 Các thông số liên quan đến đầu ra tốc độ 124
18 7.13 Thông số liên quan đến phạm vi trùng hợp tốc độ 125
19 7.14 Các tham số liên quan đến việc tăng, giảm tốc độ 125
20 7.15 Bảng lựa chọn lệnh momen 126
21 7.16 Các thông số liên quan đến đầu vào (Lựa chọn lệnh mô- 127
men xoắn 1, 3)
22 7.17 Các thông số liên quan đến đầu vào (Lựa chọn lệnh mô- 128
men xoắn 2)
23 7.18 Các thông số liên quan đến giới hạn tốc độ (Lựa chọn 129
lệnh mô-men xoắn 1, 3)
24 7.19 Các thông số liên quan đến giới hạn tốc độ (Lựa chọn 129
lệnh mô-men xoắn 2)
25 7.20 Vị trí các điểm trong quỹ đạo chuyển động 131
26 7.21 Bảng quy đổi điều khiển vị trí 131
27 7.22 Ký hiệu trong bản vẽ 138

10
Chương 1. Phân tích nguyên lý và thông số kỹ thuật

1.1. Tổng quan:
Thuật ngữ “Robot” xuất phát từ tiếng Séc (Czech) “Robota” có nghĩa là
công việc tạp dịch và được sử dụng lần đầu tiên bởi Karel Capek trong vở kịch
Rosum’s Universal Robots được xuất bản vào năm 1921 của ông. Trong vở kịch
này, Rossum và con trai của ông ta đã chế tạo ra những chiếc máy gần giống với
con người để phục vụ cho con người. Có lẽ đó là một gợi ý ban đầu cho các nhà
sáng chế kỹ thuật về việc sáng chế nhưng cơ cấu, máy móc bắt chước các hoạt
động cơ bắp của con người.
Đầu thập kỷ 60, công ty Mỹ AMF (American Machine and Foundry

Company) quảng cáo một loại máy tự động vạn năng gọi là “Người máy công
nghiệp” (Industrial Robot). Ngày nay người ta đặt tên người máy công nghiệp
(hay robot công nghiệp) cho những loại thiết bị có dáng dấp và một vài chức năng
như tay người được điều khiển tự động để thực hiện một số thao tác sản xuất.
Trong chiến tranh thế giới thứ hai, với nhu cầu nguyên cứu các vật liệu
phóng xạ, các cơ cấu điều khiển từ xa (hay các thiết bị kiểu chủ-tớ) đã ra đời nhằm
hỗ trợ cho con người có thể tách biệt khỏi khu vực phóng xạ. Người thao tác được
tách biệt khỏi khu vực phóng xạ bởi một bức tường có một hoặc vài cửa quan sát
để có thể nhìn thấy được công việc bên trong. Các cơ cấu điều khiển từ xa thay
thế cho cánh tay của người thao tác, nó là những cơ cấu phỏng sinh học bao gồm
những khâu khớp và các dây chằng gắn liền với hệ điều hành chính là cánh tay
của người, thao tác thông qua các cơ cấu khuếch đại cơ khí. Một bộ kẹp ở bên
trong và hai tay cầm ở bên ngoài được nối với nhau bằng một cơ cấu sáu bậc tự

11
do để tạo ra các vị trí và hướng tùy ý cho tay cầm và bộ kẹp. Cơ cấu này dùng để
điều khiển bộ kẹp theo chuyển động của tay cầm.
Tính năng làm việc của Robot ngày căng được nâng cao, nhất là khả năng
nhận biết và xử lý. Năm 1967 ở trường Đại học tổng hợp Standford (Mỹ) đã chế
tạo ra mẫu robot hoạt động theo mô hình “mắt-tay”, có khả năng nhận biết và định
hướng bàn kẹp theo vị trí vật kẹp nhờ các cảm biến. Năm 1974 công ty Mỹ
Cincinnati đưa ra loại robot điều khiển bằng máy vi tính, gọi là robot TTT (The
Tomorrow Tool: Công cụ của tương lai). Robot này có thể nâng được vật có khối
lượng lên đến 40kg.
Robot SCARA là một loại Robot công nghiệp ra đời năm 1979 tại trường
đại học Yamanaski của Nhật Bản. SCARA là viết tắt của Selective Compliance
Assembly Robot Arm hoặc Selective Compliance Articulated Robot Arm. Hầu
hết robot SCARA đều dựa trên cấu trúc nối tiếp gồm hai khớp quay và một khớp
trượt, được chuyên dùng cho công việc lắp ráp với tại trọng nhỏ theo phương
thẳng đứng.
Hình 1.1. Denso HM-G Series

12
1.2. Nguyên lý hoạt động:

Các phần mềm lập trình và chương trình điều khiển robot được cài đặt trên
máy tính dùng để điều khiển robot thông qua Bộ điều khiển, bộ điều khiển có thể
kết nối với các thiết bị khác như cảm biến, băng tải, cơ cấu cấp phôi giúp robot
xác định vị trí, đặc điểm đối tượng làm việc từ đó tính toán và điều khiển các trục
thực hiện theo yêu cầu mong muốn.
Hình 1.2 Các thành phần chính của robot
Các thành phần chính của robot:
- Cánh tay robot: là kết cấu cơ khí bao gồm các khâu liên kết với nhau bằng các
khớp động tạo nên chuyển động cơ bản của robot.
- Nguồn động lực: là động cơ các loại như động cơ điện, khí nén…
- Hệ thống cảm biến: giúp robot nhận biết trạng thái các cơ cấu của robot và trạng
thái của môi trường làm việc.
- Dụng cụ thao tác được gắn trên khâu cuối của robot, làm bộ phận trực tiếp thực
hiện các thao tác trên đối tượng làm việc.

13
- Hệ thống điều khiển gồm máy tính và bộ điều khiển để giám sát và điều khiển
hoạt động của robot.
1.3. Các thành phần của hệ thống điều khiển:

a) Đối tượng điều khiển
Hình 1.3. Sơ đồ động học của robot
- Các khâu của robot:

o Khâu 0 (Khâu cố định): Giữ cố định Robot và nối với khâu động 1 qua khớp quay
1.
o Khâu 1: Khâu động theo phương ngang, có khả năng quay quanh trục z0 thông
qua khớp quay 1.
o Khâu 2: Khâu động theo phương ngang, có khả năng quay quanh trục z1 thông
qua khớp quay 2.
o Khâu 3: Chuyển động theo thẳng đứng nhờ sử dụng bộ truyền vít me – đai ốc.
- Các bộ truyền động được sử dụng:
o Khớp quay 1,2: Sử dụng truyền động qua hộp giảm tốc.
o Khớp tịnh tiến 3: Sử dụng bộ truyền động vít me – đai ốc.
14
- Động cơ: Servo Motor.

- Bộ truyền động đai răng.
- Hộp giảm tốc.
- Ổ lăn.
b) Các thành phần của hệ thống điều khiển:
- Động cơ: Gồm 3 động cơ tương ứng với 3 khớp của robot:
+ Khớp 1 : Động cơ MDMF102L3 – 1kW (Panasonic Servo Minas A6).
+ Khớp 2 : Động cơ MHMF042L2 - 400W (Panasonic Servo Minas A6).
+ Khớp 3 : Động cơ MHMF042L2 - 400W (Panasonic Servo Minas A6).
- Bộ điều khiển động cơ: MITSUBISHI FX3G
1.4. Thông số kỹ thuật cần thiết kế của robot:

L0 = 340 mm, L1 = 250 mm, L2 = 350 mm, L3 = 400 mm.
Hình 1.4. Các thông số kĩ thuật chính của Robot

15
➔ Khả năng duỗi xa nhất tính từ khớp số 1: 600 mm.
Tốc độ tối đa:
- Khớp 1 (Quay): 450 độ/s = 75 vòng/phút.

- Khớp 2 (Quay): 667 độ/s = 111 vòng/phút.
- Khớp 3 (Tịnh tiến): v = 2,78 m/s.
Khả năng lặp lại vị trí cũ (độ chính xác):
- Khớp 1 + khớp 2: ±0.02 mm.

- Khớp 3 (Tịnh tiến): ±0.01 mm.
Tải trọng tối đa: 20kg.
Miền làm việc của các khớp:
- Khớp xoay 1: ±165 độ.

- Khớp xoay 2: ±147 độ.
- Khâu tịnh tiến 3: 400mm.
Hình 1.5. Giới hạn miền làm việc

16
Bảng 1.1: Thông số kĩ thuât của HM-4A604G
Số trục 4
Tầm với (mm) 600
Chiều dài khâu 1 (mm) 250
Chiều dài khâu 2 (mm) 350
J1 (°) ±165
Phạm vi của chuyển động J2 (°) ±147
Z (mm) 200, 300, 400
T (°) ±360
Phát hiện vị trí Encoder tuyệt đối
Tải trọng tối đa (kg) 20
Trọng lượng (kg) 56
Thời gian chu kỳ (giây) 0,29 (với tải 2-kg)
Tốc độ tổng hợp tối đa (mm /giây) 8780
Vận tốc lớn nhất J1 (°/s) 450
J2 (°/s) 667
Z (mm/s) 2780

17
T (°/s) 1540
Độ chính xác lặp J1+J2 (mm) ±0,02
Z (mm) ±0,01
T (°) ±0,005
Lực phù hợp tối đa (1 giây hoặc ít hơn, N) 98
Mômen quán tính lớn nhất cho phép (kgm2) 0,25 (với tải 10-kg)
Hệ thống dây điện (tích hợp trong tay) 24 dây
Đường ống dẫn khí (tích hợp trong cánh tay) 4 hệ thống
1.5. Bài toán điều khiển Robot:
Liên quan đến đặc điểm làm việc của robot có thể chia bài toán điều khiển
robot thành hai loại: điều khiển thô và điều khiển tinh. Ở bài toán điều khiển thô,
sẽ xác định luật điều khiển thích hợp để điều khiển tốc độ, vị trí do đó chuyển
động của các khớp bám sát quỹ đạo thiết kế trong thời gian quá trình quá độ nhỏ
nhất. Bài toán thứu hai liên quan đến quá trình khi robot di chuyển tiếp xúc với
môi trường làm việc như trường hợp của robot tiếp xúc với một thiết bị máy.
Như vậy quá trình làm việc này sẽ yêu cầu điều khiển cả lực và vị trí.

18
Hình 1.6. Phân loại các phương pháp điều khiển robot
Điều khiển thô hay điều khiển quỹ đạo có thể thực hiện ở tọa độ khớp hay tọa độ
Đề-các tùy thuộc vào quỹ đạo thiết kế cho tọa độ khớp hay tọa độ Đề-các. Điều
khiển chuyển động tinh là điều khiển lực thực chất là điều khiển lực và quỹ đạo.
Điều khiển lực bao gồm điều khiển trở kháng và điều khiển hỗn hợp.
Hình 1.7. Sơ đồ khối hệ thống điều khiển chuyển động (quỹ đạo) robot

19
Dựa vào kiểu phản hồi của tín hiệu có thể chia nhỏ thành 2 kiểu điều khiển Robot:
điều khiển hở và điều khiển kín.
- Điều khiển hở: Dùng truyền động bước (động cơ điện hoặc động cơ thủy lực, khí
nén…) mà quãng đường hoặc góc dịch chuyển tỷ lệ với số xung điều khiển. Điều
khiển kiểu này đơn giản nhưng đạt độ chính xác thấp.
- Điều khiển kín: (điều khiển servo), sử dụng tín hiệu phản hồi vị trí để tăng độ
chính xác điều khiển, có 2 kiểu: điều khiển điểm-điểm và điều khiển theo đường
(contour).
▪ Với điều khiển điểm-điểm, phần công tác dịch chuyển từ điểm này đến điểm kia
theo đường thẳng với tốc độ cao (trạng thái không làm việc). Nó chỉ làm việc tại
các điểm dừng. Kiểu điều khiển này dùng trên các Robot hàn điểm, vận chuyển,
tán đinh, bắn đinh…
▪ Điều khiển contour đảm bảo cho phần công tác dịch chuyển theo quỹ đạo bất kì
với tốc độ có thể điều khiển được. Có thể gặp kiểu điều khiển này trên các Robot
hàn hồ quang, phun sơn…

20
Chương 2. Thiết kế hệ thống cơ khí

Mô hình nghiên cứu
Bậc tự do của robot là khả năng làm việc tự do, độc lập của robot.
Để tính bậc tự do chuyển động ta sử dụng công thức:
f = λ(n − k) + ∑ki=1 fi + fc − fp (2.1)
Trong đó:
- f : số bậc tự do của cơ cấu. Hình 2.1. Mô hình robot scara
- λ: bậc tự do của không gian trong đó tay máy thực hiện chuyển động.
- n: số khâu động của robot.
- k: số khớp động của cơ cấu.
- fi : số bậc tự do của khớp thứ i.
- fc : số ràng buộc trùng.
- fp : số bậc tự do thừa.
Với robot trong bài tiểu luận này em nghiên cứu có số bậc tự do là:
f = 6.(3-3)+3+0-0 = 3.
Robot có: - 3 khâu động.
- 3 khớp động: 2 khớp quay, 1 khớp tịnh tiến.

21
2.1. Động học robot SCARA

2.1.1. Động học thuận robot
Mục đích của bài toán động học thuận là xác định vị trí của khâu tác động
cuối của tay máy khi biết các biến khớp của tay máy.
Thiết lập các hệ tọa độ trên robot SCARA:
Hình 2.2. Sơ đồ động học Robot SCARA 3 bậc tự do.
Thiết lập bảng Denavit – Hartenberg (D-H):
Theo D-H, tại mỗi khớp ta gắn một hệ tọa độ, quy ước về cách đặt hệ tọa độ như
sau:
- Trục zi được gắn với trục của khớp thứ i+1. Chiều của zi được chọn tùy ý.
- Trục xi được xác định là đường vuông góc chung giữa khớp thứ i và i+1,
hướng từ điểm trục của khớp thứ i tới khớp thứ i+1. Nếu hai trục song song
thì xi có thể chọn theo chiều bất kỳ là đường vuông góc chung hai trục

22
khớp. Trong trường hợp hai trục này cắt nhau, xi được xác định theo chiều
của zi xzi+1 (hoặc quy tắc bàn tay phải).
- Trục yi được xác định theo xi và zi theo quy tắc bàn tay phải.
Các thông số động học Denavit-Hartenberg (D-H) được xác định như sau:
- di : Khoảng cách Oi−1 và Oi theo trục zi−1 .

- θi : Góc giữa hai đường vuông góc chung. Là góc quay quanh trục zi−1 để
trục xi−1 chuyển động đến trục xi theo quy tắc bàn tay phải.
- αi : Khoảng dịch chuyển giữa 2 trục khớp động kề nhau.
Bảng 2.1. Bảng tham số động học Denavit – Hartenberg
Khâu θi di ai αi
1 θ1 d1 a1 0
2 θ2 d2 a2 0
3 0 d3 0 0
Tầm với của robot là 0,6m nên có thể chọn a1 = 0,25m và a2 = 0,35m.
Ma trận truyền biến đổi tọa độ thuần nhất Denavit-Hartenberg (D-H):
cθi −sθi cαi sθi sαi ai cθi

sθ cθi cαi −cθi sαi ai sθi
i-1
Ai = [ i ] (2.2)
0 sαi cαi di
0 0 0 1
Các ma trận biến đổi tọa độ thuần nhất:

23
cθ1 −sθ1 cα1 sθ1 sα1 a1 cθ1 cθ1 −sθ1 0 a1 cθ1

sθ cθ1 cα1 −cθ1 sα1 a1 sθ1 sθ cθ1 0 a1 sθ1
0
A1= [ 1 ] =[ 1 ] (2.3)
0 sα1 cα1 d1 0 0 1 d1
0 0 0 1 0 0 0 1
cθ2 −sθ2 cα2 sθ2 sα2 a2 cθ2 cθ2 −sθ2 0 a2 cθ2
sθ cθ2 cα2 −cθ2 sα2 a2 sθ2 sθ cθ2 0 a2 sθ2
1
A2= [ 2 ] =[ 2 ] (2.4)
0 sα2 cα2 d2 0 0 1 d2
0 0 0 1 0 0 0 1
cθ3 −sθ3 cα3 sθ3 sα3 a3 cθ3 1 0 0 0
sθ cθ3 cα3 −cθ3 sα3 a3 sθ3 0 1 0 0
2
A3= [ 3 ]=[ ] (2.5)
0 sα3 cα3 d3 0 0 1 d3
0 0 0 1 0 0 0 1
cθ12 −sθ12 0 a2 cθ12 + a1 cθ1

sθ cθ12 0 a2 sθ12 + a1 sθ1
0
A2 = 0A11A2 =[ 12 ] (2.6)
0 0 1 d1 + d2
0 0 0 1
Vậy theo Denavit – Hartenberg, ma trận biến đổi tọa độ thuần nhất từ hệ tọa độ
gắn với khâu thao tác được tính như sau:
cθ12 −sθ12 0 a2 cθ12 + a1 cθ1

sθ cθ12 0 a2 sθ12 + a1 sθ1
0
A3 = 0A11A22A3 = [ 12 ] (2.7)
0 0 1 d1 + d2 + d3
0 0 0 1
Vị trí và huớng của điểm thao tác:
- Tọa độ của điểm thao tác:

a2 cθ12 + a1 cθ1
rE (0) =[xE yE zE ]T= [a2 sθ12 + a1 sθ1 ] (2.8)
d1 + d2 + d3

24
- Ma trận cosin chỉ huớng của khâu thao tác:
cθ12 −sθ12 0
0
R3 = [sθ12 cθ12 0] (2.9)
0 0 1
Vận tốc và gia tốc của điểm thao tác:
a2 cθ12 + a1 cθ1 −a2 sθ12 − a1 sθ1 −a2 sθ12 0 θ̇1

(0) (0) d
vE = rĖ = [a2 sθ12 + a1 sθ1 ] = [ a2 cθ12 + a1 cθ1
dt
a2 cθ12 0 ].[θ̇2 ]
d1 + d2 + d3 0 0 1 ḋ3
(2.10)
−a2 sθ12 − a1 sθ1 −a2 sθ12 0 θ̇1

(0) d
(0)
aE = vĖ = ([ a2 cθ12 + a1 cθ1
dt
a2 cθ12 0 ].[θ̇2 ]) =
0 0 1 ḋ3
θ̇1 θ̇12 . (−a2 cθ12 − a1 cθ1 ) + θ̇2 θ̇12 . ( −a2 cθ12 ) + θ̈1 . (−a2 sθ12 − a1 sθ1 ) + θ̈2 . ( −a2 sθ
[ θ̇1 θ̇12 . (−a2 sθ12 − a1 sθ1 ) + θ̇2 θ̇12 . ( a2 sθ12 ) + θ̈1 . (a2 cθ12 + a1 cθ1 ) + θ̈2 . ( −a2 cθ1
d̈3
(2.11)
2.1.2. Động học ngược robot:

Mục đích của bài toán động học ngược là xác định giá trị các biến khớp khi
biết được vị trí và hướng của các khâu (so với hệ tọa độ gốc).
• Động học ngược về vị trí:
Ký hiệu vecto tọa độ suy rộng: q = [q1 , q2 , q3 ]T = [θ1 , θ2 , d3 ]T . Để giải bài

toán động học ngược, cần biết trước 3 tọa độ bất kỳ về vị trí hoặc hướng của điểm

25
thao tác của robot. Cụ thể, khi biết ba tọa độ của robot trong không gian là
x3 , y3 , z3 , thu được hệ phương trình sau:
xE = a2 cθ12 + a1 cθ1
{ yE = a2 sθ12 + a1 sθ1 (2.12)
zE = d1 + d2 + d3
Trong đó:
- q1 = θ1 , q1 = θ2 , q3 = d3 là các biến khớp, các nghiệm cần giải.

- a1 , a2 , xE , yE , zE là các số liệu cho trước.
• Động học ngược tìm các biến khớp:
Bình phương 2 phương trình đầu của hệ phương trình (2.12) sau đó cộng lại ta
được phương trình sau:
a1 2 + a2 2 + 2a1 a2 cosθ2 = xE 2 + yE 2 (2.13)
xE 2 +yE 2 −a1 2 −a2 2

=> cosθ2 = (2.14)
2a1 a2
Ta có sinθ2 = ±√1 − cosθ2 2
xE 2 +yE 2 −a1 2 −a2 2 2 xE 2 +yE 2 −a1 2 −a2 2

=> q2 = arctan(±√1 − ( ) ⁄ ) (2.15)
2a1 a2 2a1 a2
Biến đổi 2 phương trình đầu của hệ phương trình (2.12) về dạng sau:
x = a2 cθ12 + a1 cθ1 x = cθ1 (a1 + a2 cθ2 ) − a2 sθ1 sθ2

{ E => { E
yE = a2 sθ12 + a1 sθ1 yE = sθ1 (a1 + a2 cθ2 ) + a2 cθ1 sθ2

26
xE (a1 +a2 cθ2 ) + yE a2 sθ2

cosθ1 =
1 a 2 +a 2 +2a a cosθ
2 1 2 2
=> { yE (a1 +a2 cθ2 ) − xE a2 sθ2
(2.16)
sinθ1 = − 2 2
a1 +a2 +2a1 a2 cosθ2
yE (a1 +a2 cθ2 ) − xE a2 sθ2

=> q1 = arctan (- ) (2.17)
xE (a1 +a2 cθ2 ) + yE a2 sθ2
Từ (2.12) => q3 = zE – (d1 + d2 ) (2.18)
Vậy ta xác định được các tọa độ khớp như sau:
yE (a1 + a2 cθ2 ) − xE a2 sθ2

q1 = arctan (− )
xE (a1 + a2 cθ2 ) + yE a2 sθ2
xE 2 + yE 2 − a1 2 − a2 2 2 xE 2 + yE 2 − a1 2 − a2 2
q2 = arctan ( ±√1 − ( ) ⁄ )
2a1 a2 2a1 a2
{ q3 = zE − (d1 + d2 )
(2.19)
• Động học ngược về vận tốc:
Vận tốc dài của các khâu tương ứng với các khớp tịnh tiến và vận tốc góc ứng với
các khớp quay.
Ta có các phương trình động học sau:
f1 = a2 cθ12 + a1 cθ1 − xE = 0
{ f2 = a2 sθ12 + a1 sθ1 − yE = 0 (2.20)
f3 = d1 + d2 + d3 − zE = 0
Ma trận Jacobi tịnh tiến của điểm tác động cuối:

27
∂f1 ∂f1 ∂f1

∂q1 ∂q2 ∂q3
∂f2 ∂f2 ∂f2
−a2 sθ12 − a1 sθ1 −a2 sθ12 0
Jq = - ∂q1 ∂q2 ∂q3
= - [ a2 cθ12 + a1 cθ1 a2 cθ12 0] (2.21)
∂f3 ∂f3 ∂f3 0 0 1
[∂q1 ∂q2 ∂q3 ]
Như vậy, có thể tính vận tốc điểm tác động cuối cùng: Jq q̇ = 0v3 . Nếu cho
trước vận tốc điểm khâu thao tác thì vận tốc các tọa độ suy rộng q̇ được tính như
sau:
q̇ = Jq −1 . 0v3 (2.22)
∂f1 ∂f1 ∂f1

∂p1 ∂p2 ∂p3
∂f2 ∂f2 ∂f2
−1 0 0
Jp = ∂p1 ∂p2 ∂p3
= [ 0 −1 0 ] (2.23)
∂f3 ∂f3 ∂f3 0 0 −1
[∂p1 ∂p2 ∂p3 ]
Áp dụng công thức: q̇ = Jq −1 Jp ṗ để tính vận tốc các biến khớp:
cθ12 ẋ E + sθ12 ẏ E
q̇ 1 a1 sθ2
−1
q̇ = [q̇ 2 ] = Jq Jp ṗ = −( 12 + 1 )ẋ − ( sθ12 + sθ1 )ẏ
cθ cθ (2.24)
q̇ 3 a1 sθ2 a2 sθ2 E a1 sθ2 a2 sθ2 E
[ ż E ]
Vậy, ta có:
- Khâu 3 ứng với khớp tịnh tiến nên có vận tốc dài q̇ 3 = ż E .
- Khâu 1 và 2 ứng với khớp quay nên có vận gốc góc tương ứng lần lượt là:
cθ12 ẋ E +sθ12 ẏ E cθ12 cθ1 sθ12 sθ1

q̇ 1 = ; q̇ 2 = −( + )ẋ E − ( + )ẏ E .
a1 sθ2 a1 sθ2 a2 sθ2 a1 sθ2 a2 sθ2

28
• Động học ngược về gia tốc:
Gia tốc dài của các khâu tương ứng với các khớp tịnh tiến và gia tốc góc tương
ứng với các khớp quay.
Ta có phương trình động học sau:
Jq q̇ = Jp ṗ => J̇q q̇ + Jq q̈ = J̇p ṗ + Jp p̈
=> q̈ = Jq −1 [−J̇q q̇ + J̇p ṗ + Jp p̈ ] (2.25)
0 0 0
̇
Mà Jp = [0 0 0 ] => q̈ = Jq −1 [−J̇q q̇ + Jp p̈ ] (2.26)
0 0 0
Trong đó, ta tính được J̇q , Jq −1 theo (2.21)
−a2 (θ̇1 + θ̇2 )cθ12 − a1 θ̇1 cθ1 −a2 (θ̇1 + θ̇2 )cθ12 0
J̇q = [−a2 (θ̇1 + θ̇2 )sθ12 − a1 θ̇1 sθ1 −a2 (θ̇1 + θ̇2 )sθ12 0] (2.27)
0 0 0
−cθ12 −sθ12
0
a1 sθ2 a1 sθ2
Jq −1 = cθ12
+
cθ1 sθ12
+
sθ1
0 (2.28)
a1 sθ2 a2 sθ2 a1 sθ2 a2 sθ2
[ 0 0 −1 ]
2.1.3. Ý nghĩa của bài toán động học robot:

Sử dụng kết quả của bài toán động học thuận và động học ngược, mô phỏng
chuyển động của Robot Scara 3 bậc tự do phục vụ cho bài toán điều khiển.

29
Hình 2.3 Mô phỏng matlab Robot Scara
2.2. Động lực học robot SCARA:

Mục đích của phương pháp nghiên cứu động lực học:
- Cho mục đích thiết kế, điều khiển robot, cần thiết phải có một mô hình toán
học thể hiện mối quan hệ động lực học tác dụng lên robot. Cần xây dựng
các phương trình vi phân cho chuyển động của robot dựa trên cơ sở các
định luật bảo toàn năng lượng cho robot.
Nghiên cứu động lực học nhằm giải quyết bài toán sau cho robot:
- Tính toán các lực khớp và các momen tại các góc khớp phát sinh trong quá
trình chuyển động của robot.
- Xác định các sai số so với chuyển động đã hoạch định làm cơ sơ cho quá
trình điều khiển robot (trong bài toán điều khiển).
30
2.2.1. Động lực học thuận:

Các tham số động lực học (coi các thanh đồng chất, tiết diện ngang không đáng
kể, trọng tâm của các khâu đặt tại trung điểm của nó)
Bảng 2.2. Các tham số động học
Khâu Vị trí trọng tâm Khối Ma trận momen quán tính

xc yc zc lượng Ixx Iyy Izz Ixy Iyz Izx
1 −a1 0 0 m1 0 m1 a1 2 m1 a1 2 0 0 0
2 12 12
2 −a2 0 0 m2 0 m2 a2 2 m2 a2 2 0 0 0
2 12 12
3 0 0 −L3 m3 m3 L3 2 m3 L3 2 0 0 0 0
2 12 12
Để tính toán động lực học Robot, ta sẽ đi thiết lập phương trình vi phân chuyển
động của robot.
Phương trình vi phân chuyển động của Robot được xây dựng theo phương trình
Lagrange loại II có dạng tổng quát như sau:
d ∂T T ∂T T ∂Π T
( ) -( ) = -( ) + Qi * (2.29)
dt ∂q̇ i ∂q̇ i ∂qi
Với
- T – động năng của Robot.

- Π – thế năng của Robot.
- Q* – Vecto lực suy rộng không thế.

31
Phương trình vi phân chuyển động của Robot có dạng:
M(q)q̈ + C(q,q̇ ) q̇ + G(q) + Q = τ (2.30)
Với M là ma trận khối lượng, C là ma trận Coriolis, G là trọng lực, Q là lực suy
rộng của các lực không thế và τ = [τ1 , τ2 , τ3 ]T là lực điều khiển tại các khớp.
Ma trận Jacobi của các khâu:
• Khâu 1: Giả thiết khâu 1 là một thanh thẳng đồng chất có chiều dài 250mm,
tiết diện ngang không đáng kể và có trọng tâm đặt tại trung điểm của thanh.
Ma trận biến đổi hệ tọa độ thuần nhất của trọng tâm khâu 1 0Ac1 được tính:
−a1
cθ1 −sθ1 0 a1 cθ1 1 0 0
2
sθ cθ1 0 a1 sθ1
Ta có : 0A1 = [ 1 ] ; 1Ac1 = 0 1 0 0 (2.31)
0 0 1 d1 + d2 0 0 1 0
0 0 0 1 [0 0 0 1 ]
a1
cθ1 −sθ1 0 cθ1
2
a1
=>0Ac1 = 0A11Ac1 = sθ1 cθ1 0
2
sθ1 (2.32)
0 0 1 d1 + d2
[ 0 0 0 1 ]
a1
cθ1
cθ1 −sθ1 0
2
0 a1 0
=> rc1 = [ sθ1 ] ; Rc1 = [sθ1 cθ1 0] (2.33)
2
d1 + d2 0 0 1
a1
cθ1
2
a1
sθ1 ]=> Ma trận Jacobi tịnh tiến của hệ tọa độ trọng tâm khâu 1:
0
rc1 = [
2
d1 + d2

32
−a1
sθ1 0 0
d 2
0
(0rc1) = [ a1
JT1 =
dθ cθ1 0 0] (2.34)
2
0 0 0
cθ1 −sθ1 0
0
Rc1 = [sθ1 cθ1 0]
0 0 1
cθ1 sθ1 0 −sθ1 −cθ1 0
=> 0 T
R c1 =[−sθ1 cθ1 0 ̇
0 ]; R c1 =[ cθ1 −sθ1 0 ] θ̇1
0 0 1 0 0 0
(2.35)
0 −θ̇1 0 0
0 T 0 ̇
̃ 1r
w = R c1 R c1 = [θ̇1 0
r
0 ] =>w1 = [ 0 ] (2.36)
0 0 0 θ̇1
Ma trận Jacobi quay của hệ tọa độ trọng tâm khâu 1:
0 0 0
d
JR1 = (w1r ) = [0 0 0 ] (2.37)
dθ̇
1 0 0
• Khâu 2: Giả thiết khâu 2 là một thanh thẳng đồng chất có chiều dài 350mm,
tiết diện ngang không đáng kể và có trọng tâm đặt tại trung điểm của thanh.
Ma trận biến đổi hệ tọa độ thuần nhất của trọng tâm khâu 2 0Ac2 được tính:
−a2
cθ12 −sθ12 0 a2 cθ12 + a1 cθ1 1 0 0
2
sθ cθ12 0 a2 sθ12 + a1 sθ1 2
Ta có : 0A2 = [ 12 ]; Ac2 = 0 1 0 0
0 0 1 d1 + d2 0 0 1 0
0 0 0 1 [0 0 0 1 ]
(2.38)
33
a2
cθ12 −sθ12 0 cθ12 + a1 cθ1
2
a2
=>0Ac2 =0A22Ac2 = sθ12 cθ12 0
2
sθ12 + a1 sθ1 (2.39)
0 0 1 d1 + d2
[ 0 0 0 1 ]
a2
cθ12 + a1 cθ1 cθ12 −sθ12 0
2
0 a2 0
=> rc2 = [ sθ12 + a1 sθ1 ]; Rc2 = [sθ12 cθ12 0] (2.40)
2
d1 + d2 0 0 1
a2
cθ12 + a1 cθ1
2
0 a2
rc2 = [ sθ12 + a1 sθ1 ]
2
d1 + d2
=>Ma trận Jacobi tịnh tiến của hệ tọa độ trọng tâm khâu 2:
−a2 −a2
sθ12 − a1 sθ1 sθ12 0
d 2 2
0 0
( rc2) = [ a2 a2
JT2 =
dθ cθ12 + a1 cθ1 cθ12 0] (2.41)
2 2
0 0 0
cθ12 −sθ12 0
0
Rc2 = [sθ12 cθ12 0]
0 0 1
cθ12 sθ12 0 −sθ12 −cθ12 0
0 T
=> R c2 =[−sθ12 cθ12 0 ̇
0 ]; R c2 =[ cθ12 −sθ12 0 ] (θ̇1 + θ̇2 )
0 0 1 0 0 0
(2.42)
0 −(θ̇1 + θ̇2 ) 0 0
r 0 T 0 ̇ r
w
̃ 2 = R c2 R c2 = [(θ̇1 + θ̇2 ) 0 0 ] =>w2 = [ 0 ] (2.43)
̇θ1 + θ̇2
0 0 0
34
0 0 0
d
JR2 = ̇ (w2r ) = [0 0 0 ] (2.44)
dθ
1 1 0
• Khâu 3: Giả thiết khâu 3 là một thanh thẳng đồng chất có chiều dài L3 , tiết
diện ngang không đáng kể và có trọng tâm đặt tại trung điểm của thanh. Ma
trận biến đổi hệ tọa độ thuần nhất của trọng tâm khâu 3 0Ac3 được tính:
cθ12 −sθ12 0 a2 cθ12 + a1 cθ1 1 0 0 0

sθ cθ12 0 a2 sθ12 + a1 sθ1 3 0 1 0 0
Ta có : 0A3 = [ 12 ] ; Ac3 = −L3
0 0 1 d1 + d2 + d3 0 0 1
2
0 0 0 1 [0 0 0 1 ]
(2.45)
cθ12 −sθ12 0 a2 cθ12 + a1 cθ1

sθ12 cθ12 0 a2 sθ12 + a1 sθ1
=>0Ac3 = L3 (2.46)
0 0 1 d1 + d2 + d3 −
2
[ 0 0 0 1 ]
a2 cθ12 + a1 cθ1 cθ12 −sθ12 0

0 a sθ
=> rc3 = [ 2 12 + a sθ 0
1 1 ]; Rc3 = [sθ cθ12 0] (2.47)
L 12
d1 + d2 + d3 − 3 0 0 1
2
a2 cθ12 + a1 cθ1
0
rc3 = [ a2 sθ12 + a1 sθ1 ]
L
d1 + d2 + d3 − 3
2
=> Ma trận Jacobi tịnh tiến của hệ tọa độ trọng tâm khâu 3:

35
−a2 sθ12 − a1 sθ1 −a2 sθ12 0

0 d 0
JT3 = ( rc3) = [ a2 cθ12 + a1 cθ1 a2 cθ12 0] (2.48)
dθ
0 0 1
cθ12 −sθ12 0
0
Rc3 = [sθ12 cθ12 0]
0 0 1
cθ12 sθ12 0 −sθ12 −cθ12 0
=> 0 T
R c3 =[−sθ12 cθ12 0 ̇
0 ]; R c3 =[ cθ12 −sθ12 0 ] (θ̇1 +θ̇2 )
0 0 1 0 0 0
(2.49)
0 −(θ̇1 + θ̇2 ) 0 0
r 0 T 0 ̇ r
w
̃ 3 = R c3 R c3 = [θ̇1 + θ̇2 0 0 ] =>w3 = [ 0 ] (2.50)
̇θ1 + θ̇2
0 0 0
0 0 0
d r
JR3 = (w 3 ) = [0 0 0] (2.51)
dθ̇
1 1 0
Tính tensor quán tính của các khâu:
Coi các khâu của robot là các thanh đồng chất tiết diện đều và bề ngang
không đáng kể, trong đó Θci là tensor quán tính của khâu i đối với khối tâm của
nó, mi là khối lượng của khâu i. Ta có:
0 0 0 0 0 0
m1 a1 2 m2 a 2 2
Θc1 = 0 12
0 ; Θc2 = 0 12
0 ;
m1 a1 2 m2 a2 2
[0 0
12 ] [0 0
12 ]

36
m3 L3 2
0 0
12
Θc3 = m3 L3 2 (2.52)
0 0
12
[ 0 0 0]
Tính toán ma trận khối lượng:
M(q) = [∑ni=1( JTi

T T
mi JTi + JRi Θci JRi )]nxn (2.53)
• Khâu 1:
m1 a1 2
0 0
T 4
JT1 m1 JT1 =[ 0 0 0] (2.54)
0 0 0
m1 a1 2
0 0
T 12
JR1 Θc1 JR1 =[ 0 0 0] (2.55)
0 0 0
• Khâu 2:
a2 2 a2 2 a1 a2 cθ2
+ a1 2 + a1 a2 cθ2 + 0
4 4 2
T
JT2 m2 JT2 == m2 a2 2 a1 a2 cθ2 a2 2 (2.56)
+ 0
4 2 4
[ 0 0 0]
m2 a2 2 m2 a2 2
0
12 12
T
JR2 Θc2 JR2 = m2 a2 2 m2 a2 2 (2.57)
0
12 12
[ 0 0 0]
• Khâu 3:

37
a1 2 + a2 2 + 2a1 a2 cθ2 a2 2 + a1 a2 cθ2 0

T
JT3 m3 JT3 = m3 [ a2 2 + a1 a2 cθ2 a2 2 0] (2.58)
0 0 1
0 0 0
T
JR3 Θc3 JR3 = [0 0 0 ] (2.59)
0 0 0
Không mất tính tổng quát, khi robot SCARA gắp tải ta có thể coi tải như một
khâu, tạm gọi là Khâu 4 với m4 = mt . Đối với tải trọng, momen quán tính là không
đáng kể Θc4 = 0.
• Khâu 4:
Ta có hệ tọa độ khâu cuối chính là vị trí giữ tải trọng như vậy JT4 = JT3 ; JR4 =
JR3
a1 2 + a2 2 + 2a1 a2 cθ2 a2 2 + a1 a2 cθ2 0

T
Vậy: JT4 m4 JT4 = m4 [ a2 2 + a1 a2 cθ2 a2 2 0] (2.60)
0 0 1
0 0 0
T
JR4 Θc4 JR4 = [0 0 0 ] (2.61)
0 0 0
m11 m12 m13
=> M(q) = [∑4i=1( JTi
T
mi JTi + T
JRi Θci JRi )]3x3 = [m21 m22 m23 ] (2.62)
m31 m32 m33
Với
m1 a1 2 a2 2
- m11 = + m2 ( + a1 2 + a1 a2 cθ2 ) + (m3 + mt )(a2 2 + a1 2 + 2a1 a2 cθ2 )
3 3
a2 2 a1 a2 cθ2
- m12 = m21 = m2 (
3
+
2
) + (m3 + mt )(a2 2 + a1 a2 cθ2 )

38
m2 a2 2
- m22 = + (m3 + mt )a2 2
3
- m13 = m31 = m23 = m32 = 0

- m33 = m3 + mt .
Tính toán thế năng, lực thế:
∂Π
Vecto G(q) là vecto thế năng với các thành phần Gi = . Đặt gốc thế năng
∂qi
tại chân khâu cố định, dễ thấy thế năng của robot cũng chính là tổng thế năng của
các khâu và tải:
Thế năng Robot: Π = Π1 + Π2 + Π3 + Π4 (2.63)
0
Với g = [ 0 ]:
−g
- Thế năng khâu 1:Π1 = m1 gzc1 = m1 gd1

- Thế năng khâu 2 :Π2 = m2 gzc2 = m2 g(d1 + d2 )
L3
- Thế năng khâu 3 :Π3 = m3 gzc3 = m3 g(d1 + d2 + d3 − )
2
L3
- Thế năng tải: Π4 = m4 gzc4 = mt g(d1 + d2 + d3 − ).
2
L3
Π = m1 gd1 + m2 g(d1 + d2 ) + (m3 + mt )g(d1 + d2 + d3 − ) (2.64)
2
Lực thế:

39
∂Π
∂q1
G1 0
∂Π
G(q) = [G2 ] = ∂q2
=[ 0 ] (2.65)
G3 ∂Π −(m3 + mt )g
[∂q3 ]
Tính toán ma trận Coriolis:
C1
1 ∂mkj ∂mlj ∂m
C(q,q̇ ) q̇ = [C2 ] với Cj = ∑nk,l=1(k, l; j) q̇ k q̇ l = ∑nk,l=1 ( + − kl)q̇ k q̇ l
2 ∂ql ∂qk ∂qj
C3
1
C1 −[m2 + 2(m3 + mt )]a1 a2 sθ2 (q̇ 1 q̇ 2 + q̇ 2 q̇ 2 )
2
C(q,q̇ ) q̇ = [C2 ] = [ 1
[m2 + 2(m3 + mt )]a1 a2 sθ2 q̇ 1 q̇ 1 ](2.66)
C3 2
0
Như vậy, ta đã tính được các thành phần trong phương trình vi phân chuyển
động của Robot. Việc giải phương trình vi phân khi đã biết vector lực dẫn động τ
chính là giải bài toán động lực học thuận.
τ = [τ1 , τ2 , τ3 ]T là lực điều khiển tại các khớp.
2.2.2. Động lực học ngược:

Nhiệm vụ của bài toán động lực học ngược là tìm ra lực dẫn động τ khi đã
biết chuyển động của Robot bao gồm các vecto q(t), q̇ (t), q̈ (t) . Chỉ cần thay vào
phương trình với các thành phần đã được thiết lập ở trên.
M(q)q̈ + C(q,q̇ ) q̇ + G(q) = τ (2.67)
để tìm các lực và ngẫu lực dẫn động tại các khâu 1,2 và 3 của Robot.

40
Sau khi nghiên cứu động lực học, ta đã tính toán các lực khớp và các momen
tại các góc khớp phát sinh trong quá trình chuyển động của robot. Từ đó hỗ trợ
trong việc tính công suất của các khâu 1,2,3 và tiến hành chọn động cơ phù hợp
cho cơ cấu.
2.3. Vật liệu, khối lượng, momen quán tính các khâu:
2.3.1. Khâu 0 (Khâu cố định):
a. Vật liệu:
Thép C45
Thành phần hóa học của thép C45:
Đặc tính cơ học của thép C45:

41
b.Khối lượng, momen quán tính khâu 0:
Hình 2.4. Khâu 0
Hình 2.5. Khối lượng, momen quán tính của khâu 0
Từ các thông số xuất ra từ Solidworks ta thu được:
Khối lượng:

42
m1 = 37,97(kg)
Vị trị khối tâm:
Lc1 = 0,18(m)
Momen quán tính khối (với các trục tọa độ là trục quán tính chính trung tâm):
I1zz = 0,40(kg. m2 )
I1yy = 0,67(kg. m2 )
I1xx = 0,68(kg. m2 )
2.3.2. Khâu 1 và khâu 2:
a. Vật liệu:
Thép C45

43
b. Khối lượng, momen quán tính khâu 1:
Hình 2.6. Khâu 1

44
Khối lượng:
m1 = 23,72(kg)
Lc1 = 0,13(m)
I1zz = 0,41(kg. m2 )
I1yy = 0,37(kg. m2 )
I1xx = 0,08(kg. m2 )
c. Khối lượng, momen quán tính khâu 2:
Hình 2.8. Khâu 2

45
Khối lượng:
m2 = 27,16(kg)
Lc2 = 0,13(m)
I2zz = 0,68(kg. m2 )
I2yy = 0,63(kg. m2 )
I2xx = 0,11(kg. m2 )

46
2.3.3. Khâu 3:
a. Vật liệu:
Thép C45
b. Khối lượng, momen quán tính khâu 3:

47
Hình 2.10. Khâu 3
Hình 2.11. Khối lượng, momen quán tính khâu 3

48
Khối lượng:
m3 = 1,79(kg)
I3zz = 0,000117(kg. m2 )
I3yy = 0,047(kg. m2 )
I3xx = 0,047(kg. m2 )
Chương 3. Không gian làm việc và thiết kế quỹ đạo Robot

3.1. Không gian làm việc:
d1 = 300(mm)
d = 100(mm)
Tham số động học: { 2
a1 = 250(mm)
a2 = 350(mm)
Biến khớp: q = [q1 , q2 , q3 ]T
q1 = (−165∘ ÷ 165∘ ); q2 = (−147∘ ÷ 147∘ ); q3 = (0 ÷ 400mm)
Phương trình động học thuận của robot:
x = 250cos(q1 + q2 ) + 350cos(q1 )
{ y = 250sin(q1 + q2 ) + 350sin(q1 ) (mm)
z = 400 − q3
Không gian làm việc được xây dựng trên phần mềm Matlab:

49
Hình 3.1. Không gian làm việc của robot trong bài toán thuận (góc nhìn từ trên xuống)
Hình 3.2. Không gian làm việc của robot trong bài toán thuận (góc nhìn chéo)

50
3.2. Thiết lập quy luật chuyển động của 3 khâu:

Vấn đề thiết kế quỹ đạo chuyển động liên quan mật thiết đến bài toán điều
khiển robot di chuyển từ vị trí này đến vị trí khác trong không gian làm việc.
Đường đi và quỹ đạo được thiết kế là các lượng đặt cho hệ thống điều khiển vị
trí của robot. Do đó độ chính xác của quỹ đạo sẽ ảnh hưởng đến chất lượng di
chuyển của robot.
Thông thường, quỹ đạo ở dạng đa thức bậc cao sẽ đáp ứng được các yêu cầu
về vị trí, tốc độ, gia tốc ở mỗi điểm giữa 2 đoạn di chuyển. Yêu cầu của thiết kế
quỹ đạo là:
• Khâu chấp hành phải đảm bảo đi qua lần lượt các điểm trong không gian làm
việc hoặc di chuyển theo một quỹ đạo xác định. Quỹ đạo của robot phải là
đường cong đảm bảo tính liên tục về vị trí trong một khoảng nhất định.
• Không có bước nhảy về vận tốc, gia tốc.
• Quỹ đạo thường là đường cong thông thường.
• Quản lý được gia tốc tại từng thời điểm.
• Trên thực tế hiên nay có nhiều quỹ đạo là dạng đường cong dạng:
Đa thức bậc 2: x(t)=a0 + a1 t + a2 t 2
Đa thức bậc 3: x(t)= a0 + a1 t + a2 t 2 + a3 t 3
Đa thức bậc cao: x(t)= a0 + a1 t + a2 t 2 +. . . +ak t k
Trong đồ án này em sử dụng dạng quỹ đạo là đa thức bậc 3 có dạng:
X(t)= a0 + a1 t + a2 t 2 + a3 t 3
Cho 2 điểm A, B bất kỳ trong không gian làm việc, biết tọa độ điểm thao
tác và hướng của khâu thao tác. Thiết kế quỹ đạo chuyển động bất kỳ từ A đến B,
51
chỉ quan tâm đến các điều kiện về vị trí, vận tốc, gia tốc tại điểm đầu và điểm
cuối.
Chọn quỹ đạo thiết kế là đa thức bậc 3 theo thời gian có dạng:
qi (t) =ai + bi t + ci t 2 + di t 3 với i = 1..3 ứng với 3 biến khớp tương ứng với
θ2 , θ2 , d 3 .
Ta có hệ phương trình quỹ đạo chuyển động:
q1 (t) = a1 + b1 t + c1 t 2 + d1 t 3
{q2 (t) = a2 + b2 t + c2 t 2 + d2 t 3 (2.68)
q3 (t) = a3 + b3 t + c3 t 2 + d3 t 3
Đơn vị của các góc quay là rad, và khoảng tịnh tiến của khâu 3 là mm.
Với giá trị chiều dài các khâu: a1 = 250(mm); a2 = 350(mm) đã cho ở đề
bài và các quy luật chuyển động ta đặt cho các góc theta, giá trị d ở trên thay vào
tọa độ thao tác điểm cuối, vận tốc điểm thao tác cuối, gia tốc điểm thao tác cuối
ta được các hàm biến đổi theo t.
Yêu cầu thời gian robot đi từ A đến B trong T(s) và vận tốc tại điểm A,B bằng 0.
Từ đó ta có:
qi (0) = qi (A) = ai
qi (Ti ) = qi (B) = ai + bi Ti + ci Ti 2 + di Ti 3
(2.69)
q̇ i (0) = q̇ i (A) = bi = 0
2
{ q̇ i (Ti ) = q̇ i (B) = bi + 2ci Ti + 3di Ti

52
ai = qi (A)
bi = 0
3[qi (B)−qi (A)]
=> ci =
Ti 2
2[qi (B)−qi (A)]
{di = − Ti 3
3[qi (B)−qi (A)] 2[qi (B)−qi (A)] 3

qi (t) = qi (A) + 2 t2 − t
Ti Ti 3
6[qi (B)−qi (A)] t2
=> q̇ i (t) = . (t − )
Ti 2 Ti
6[qi (B)−qi (A)] 2t
{ q̈ i (t) = . (1 − )
Ti 2 Ti
t2 t3
qi (t) = qi (A) + 3[qi (B) − qi (A)] − 2[qi (B) − qi (A)]
Ti Ti
6[qi (B)−qi (A)] t t2
=> q̇ i (t) = .( − ) (2.70)
Ti Ti Ti
6[qi (B)−qi (A)] 2t
{ q̈ i (t) = . (1 − )
Ti 2 Ti
Tính toán thời gian đi hết một hành trình của các khâu:
Nhận xét: Với biểu đồ vị trí theo thời gian hoạt động, ta nhận thấy càng
ngày đồ thị càng có xu hướng nằm ngang cho thấy vận tốc (hoặc vận tốc góc)
càng ngày càng nhanh mà không trở về 0 khi kết thúc hành trình. Với đồ án này,
ta cần thiết kế để vận tốc (hoặc vận tốc góc) các khâu để thời điểm kết thúc trở về
0 để cơ cấu tiến hành gắp vật.
Như vậy, ta lấy thời gian khâu di chuyển cả hành trình bằng 2 lần thời gian
khâu di chuyển một nửa hành trình đầu từ đồ thị. (do thời gian khâu tăng vận tốc
(hoặc vận tốc góc) từ 0 lên lớn nhất và thời gian khâu giảm vận tốc (hoặc vận tốc
góc) từ lớn nhất về 0 là như nhau)

53
Hình 3.3. Biểu đồ vị trí tương ứng theo thời gian của khâu 1
Từ biểu đồ, ta có thời gian nhanh nhất đi hết hành trình của khâu 1:
T1 = 2.(1,1-0,1)= 2s

54
T2 = 2.(0,875-0,1) =1,55s
T3 = 2.(0,42-0,1)=0,64s
Nhận thấy T1 >T2 >T3 , chọn chu kỳ hoạt động cho cả 3 khâu của robot bằng
T = T1 = T2 = T3 = 2s để đảm bảo cả 3 khâu có đủ thời gian để đi từ điểm A tới B
(A,B là 2 điểm tùy ý nằm trong vùng hoạt động của robot).

55
Chương 4. Mô hình hóa các khâu của Robot

Tổng quan:
Hệ thống điều khiển truyền động cho robot là một hệ thống điều khiển tự động
điển hình, trước tiên đi vào quá trình tính tính toán thiết kế, ta cần biết một hệ
thống điều khiển tự động gồm các thành phần nào, nó bao gồm 3 thành phần cơ
bản sau:
- Đối tượng điều khiển (Object)
- Bộ điều khiển (Controller)
- Thiết bị cảm biến đo lường (Measuring Device)
Hình 4.1. Các thành phần hệ thống điều khiển
Vậy công việc cần làm để thiết kế một hệ thống điều khiển tự động là phải xác
định được đối tượng điều khiển, sau đó tìm ra bộ điều khiển thích hợp để điều
khiển đối tượng đấy. Cụ thể trong phần xác định đối tượng điều khiển này ta phải
xây dựng được sơ đồ khối mô tả hoạt động của cả hệ robot thông qua các phần tử
điển hình. Và từ việc xác định các phần tử điển hình đó ta có được hàm truyền
của hệ thống từ các phép biến đổi toán học thông thường.

56
Hình 4.2. Tổng quan hệ thống điều khiển
4.1. Động cơ điện một chiều:
Hình 4.3. Mô hình hóa động cơ điện 1 chiều
Tín hiệu vào là điện áp u đặt vào phần ứng [Volt;V]
Tín hiệu ra là vận tốc góc ω của động cơ [rad/s]
- Quan hệ giữa momen trên trục động cơ Mt và dòng điện phần ứng im được xác
định qua hằng số tỉ lệ K m (N.m/A) như sau:
Mt = im . K m (4.1)

57
- Sức phản điện động e(t) của phần ứng động cơ đang quay trong từ trường tỉ lệ
với vận tốc góc ω (rad/s) động cơ:
e(t) = K e . ω(t) (4.2)
Trong đó K e là hằng số sức điện động(V.s/rad)
- Momen cản Mc trên trục động cơ:

Mc (t) = b. ω(t) (4.3)
Trong đó: b (kg.m2 ) là hệ số cản trục roto
Áp dụng định luật Newton cho chuyển động quay của trục động cơ, ta có:
dω
Mt - Mc = J.
dt
dω
→ J. + b. ω = Mt = im . K m (4.4)
dt
Biến đổi Laplace, ta được (Js+b).ω(s) =K m .I(s) (4.5)
Áp dụng định luật Kirchoff cho mạch điện phần ứng:
di(t)
u(t) = i(t).R + L. + e(t) (4.6)
dt
Trong đó
- e(t) = K e . ω(t): Sức phản điện động của phần ứng động cơ (V).
- L: Độ tự cảm (H).
- R: Điện trở (Ω).
Biến đổi Laplace:

58
U(s) = I(s).R + L.s.I(s) + K e . ω(s)
= (Ls+R).I(s)+ K e . ω(s) (4.7)
Từ (4.5), (4.7)
ω(s) Km
 =
U(s) LJs2 +(RJ+Lb )s+(Rb+Km Ke )
θm (s) Km 1
 = 2
. (4.8)
U(s) LJs +(RJ+Lb )s+(Rb+Km Ke ) s
4.2. Trục vít me đai ốc bi:
Xét bộ truyền vit me đai ốc bi như hình vẽ:
Hình 4.4. Bộ truyền vít me đai ốc bi
Tín hiệu vào: Vận tốc góc ω(t) của vít me (rad/s)
Tín hiệu ra: Lượng di động y(t) của bàn máy gắn liền với trục vít me đai ốc bi
hay độ dịch chuyển của bàn máy
Gọi P(m) là bước của vít me ta có quan hệ
P t
y(t) = . ∫0 ω(t)dt (4.9)
2π
Biến đổi Laplace hai về với điều kiện đầu bằng 0 ta có:

59
P ω(s)
Y(s) = (4.10)
2π s
Ta được hàm truyền:
Y(s) P Q
= = (4.11)
ω(s) 2πs s
P
Với Q =
2π
Sơ đồ tương đương:
W3 (s) P 1
=> Đối với khâu 3: G3 (s) =
U3 (s) 2π s

60
4.3. Mô hình hóa khâu vít me đai ốc bi (Khâu 3):
Hình 4.5. Mô hình hóa chuyển động của khâu 3
Với các thông số:
- Jm : Mô men quán tính của động cơ.

- J1 : Mô men quán tính của bánh đai 1.
- ωm : Vận tốc góc trục động cơ gắn với bánh đai 1.
- τm : Mô men của trục động cơ.
- d1 : Đường kính bánh đai 1.
- θm : Góc quay của bánh 1.
- B1 : Hệ số giảm chấn trên trục 1.
- n: Tỉ số truyền của 2 bánh đai.
- J2 : Mô men quán tính của bánh đai 2.

61
- ωt : Vận tốc góc trục động cơ gắn với bánh đai 2.

- τt : Mô men của trục mang tải.
- θt : Góc quay của bánh 2.
d1 θm
Tỷ số truyền: n = = (4.12)
d2 θt
Theo nguyên lý D’Alembert ta có:
τt − B2 . θ̇t = J2 . θ̈t (4.13)
Sử dụng nguyên lý trên cho trục động cơ ta có:
τm − nτt − B1 . θ̇m = (Jm + J1 ). θ̈m (4.14)
Kết hợp (4.12), (4.13), (4.14) ta có:
τm = (Jm + J1 + n2 J2 ). θ̈m + (B1 + n2 B2 ). θ̇m (4.15)
Thu gọn ta được τm = Jtd . θ̈m + Btd θ̇m (4.16)
hay τm = Jtd . ω̇m + Btd ωm (4.17)
Với:
Jtd = Jm + J1 + n2 J2
Btd = B1 + n2 B2
Từ hàm truyền của động cơ (4.8) ta thay các thông số J td , Btd vào ta thu được
hàm truyền của khâu:

62
ω(s) Km
= (4.18)
U(s) LJtd s2 +(RJ td +LBtd )s+(RBtd +Km Ke )
Y(s) P
Theo (4.11) => =
ω(s) 2πs
Y(s) Km P
 = . (4.19)
U(s) LJtd s2 +(RJ td +LBtd )s+(RBtd +Km Ke ) 2πs
Với P = 20mm là bước vít của trục vít me.
4.4. Mô hình hóa khâu 1 và 2:
Hình 4.6. Mô hình hóa chuyển động của khâu 1, khâu 2
Với các thông số:
- Jm : Mô men quán tính của động cơ.

- J1 : Mô men quán tính của hộp giảm tốc.
- ωm : Vận tốc góc trục động cơ gắn với bánh đai 1.
- τm : Mô men của trục động cơ.

63

- θm : Góc quay của bánh 1.
- n: Tỉ số truyền của 2 bánh đai.
- J2 : Mô men quán tính của khâu gắn với đầu ra hộp giảm tốc.
- ωt : Vận tốc góc trục động cơ gắn với bánh đai 2.
- τt : Mô men của trục mang tải.
- θt : Góc quay của bánh 2.
4.5. Xác định các thông số của động cơ:
Các thông số :
Bảng 4.1. Thông số động cơ servo DC được sử dụng trên Khâu 1 - Scara Robot.
Rated Torque (N.m) 4,77

Peak current (A) 5,2
Voltage constant (V/rad/s) _ K e 56/60
Torque coeffient (Nm/A)_ K m 0,9
Rotor inertia (kg.m2 )_ Jm 2,97E-04
R (Ohm) 1,83
L (H) 4,72E-03
Rated speed (rpm) 2500
Bm (Nm/s) 2,58E-04

64
Bảng 4.2. Thông số động cơ servo DC được sử dụng trên Khâu 2,3 - Scara Robot.
Rated Torque (N.m) 1,27
Peak current (A) 2,1
Voltage constant (V/rad/s) _ K e 48/60
Torque coeffient (Nm/A)_ K m 0,8
Rotor inertia (kg.m2 )_ Jm 0,58E-04
R (Ohm) 2,88
L (H) 6,4E-03
Rated speed (rpm) 3000
Bm (Nm/s) 2.22E-04
4.6. Tính toán và kiểm tra tính ổn định của các hàm truyền G(s):
a. Khớp 1 :
Tính momen quán tính Jl1
Momen quán tính Jl1 của khớp 1 được xác định là momen quán tính theo trục z
và do khớp 1 phải chịu tác động bao gồm cả momen quán tính của các khâu phía
sau tác dụng lên , sử dụng phần mềm SolidWork ta thu được kết quả như sau :
Phần màu xanh là các khâu được chọn để tính :

65
Hình 4.7. Mô hình tính momen quán tính cho khớp 1
Hình 4.8. Kết quả tính cho khớp 1

66
Từ bảng kết quả trên ta thu được Jl1 có giá trị bằng 9,83 (Kg.m2 )
1
Jtd = Jm1 + Ja1 + n1 2 Jl1 = 6,18.10−4 + 2 ⋅ 9,83 = 2,116.10−3 (Kg. m2 )
{ 81
1
Btd = Bm1 + n1 2 . Bl1 = 2,58.10−4 + 2 ⋅ 0,01 = 2,595.10−4 (Nms/Rad)
81
(4.20)
Với:
- Jm1 : Mô men quán tính của động cơ

- Ja1 : Mô men quán tính của hộp giảm tốc
Jm1 + Ja1 = 6,18.10−4 (Kg.m2 )
- Jl1 = 9,83 (Kg.m2 ): Mô men quán tính của khâu gắn với hộp giảm tốc
- Bm1 = 2,58.10−4 (Nms/Rad): Hệ số giảm chấn của động cơ
- Bl1 = 0,01 (Nms/Rad): Hệ số giảm chấn của khâu gắn với hộp giảm tốc
1
- n1 = : Tỉ số truyền của hộp giảm tốc
81
Kết hợp với các thông số đã xác định của động cơ:
- K m = 0,9: Hằng số tỉ lệ.

- K e = 0,93: Hằng số sức điện động.
- R = 1,83(Ω): Điện trở.
- L = 4,72.10−3 (H): Điện cảm.

67
Hình 4.9. Tính toán hàm truyền khâu 1
B1 (s) 0,9
Gkhau1 (s) = = (Rad/Volt)
A1 (s) 9,989.10−6 s3 +0,003874s2 +0,8105s
(4.21)
Kiểm tra tính ổn định của G1 ( s ) :
➢ Kiểm tra tính ổn định của hệ hở :

+ Điều kiện cần:
Ta thấy các hệ số của đa thức A1 (s) đều lớn hơn và khác 0. Vậy thỏa mãn điều
kiện cần của tiêu chuẩn Routh.
+ Điều kiện đủ :
- Lập bảng Routh

9,989.10−6 0,08105
0,003847 0
0,003847

68
Nhận thấy các số hạng thuộc cột đầu tiên của bảng Routh đều dương nên thỏa
mãn điều kiện ổn định. Vậy hệ thống được mô tả bằng phương trình đã cho là ổn
định.
➢ Kiểm tra tính ổn định của hệ kín :

Theo tiêu chuẩn Nyquist :
- Dùng lệnh Nyquist(G1,G2) trong matlab ta có được đồ thị Nyquist như sau:
Hình 4.10. Đồ thị Nyquist của hàm truyền 𝐺1 (𝑠)

Qua đồ thị ta nhận xét rẳng: Theo định lý 2.22 (Nyquist) (Sách ‘Cơ sở lí
thuyết điều khiển tuyến tính’) thì ‘Nếu hàm truyền 𝐆𝐡 (𝐬) của hệ hở có m
điểm cực không nằm bên trái trục ảo thì điều kiện cần và đủ để hệ kín ổn
định là đồ thị Nyquist của hệ hở bao điểm (-1+0j) của mặt phẳng phức m lần
theo chiều ngược chiều kim đồng hồ’. Ta thấy rằng đồ thị Nyquist không bao
điểm (-1+0j) nên sẽ không có điểm cực nào nằm trên trục ảo hoặc bên phải
trục ảo, cho nên hệ kín sẽ ổn định.

69
Đồ thị Bode :
- Dùng lệnh Bode(G1,G2) trong matlab ta có được đồ thị Bode như sau:
Hình 4.11. Đồ thị Bode của hàm truyền 𝐺1 (𝑠)
Nhận xét: Theo chuẩn Bode
Để hệ kín 𝐺𝑘 (𝑠) ổn định thì đồ thị bode của hệ hở G ( s ) phải thõa mãn đồng thời
cả hai yếu tố là có độ dữ trữ pha và độ dự trữ biên độ lớn hơn 0. Từ đồ thị của
𝐺1 (𝑠) cho thấy có độ dự trữ biên độ 𝐺𝑚 và độ dự trữ pha 𝑃𝑚 đồng thời lớn hơn 0,
vậy hệ kín này ổn định
b. Khớp 2 :
Tính momen quán tính J l 2

70

71
Từ bảng kết quả trên ta thu được Jl2 có giá trị bằng 2,62 (Kg.m2 )
1
Jtd = Jm2 + Ja2 + n2 2 Jl2 = 0,58.10−4 + 2 ⋅ 2,62 = 4,573.10−4 (Kg. m2 )
{ 81
1
Btd = Bm2 + n2 2 . Bl2 = 2,22.10−4 + 2 ⋅ 0,01 = 2,235.10−4 (Nms/Rad)
81
(4.22)
Với:

Jm2 + Ja2 = 0,58.10−4 (Kg.m2 )
- Jl2 = 2,62 (Kg.m2 ): Mô men quán tính của khâu gắn với hộp giảm tốc
- Bl2 = 0,01 (Nms/Rad): Hệ số giảm chấn của khâu gắn với hộp giảm tốc
1
- n2 = : Tỉ số truyền của hộp giảm tốc
81
- K m = 0,8(N.m/A): Hằng số tỉ lệ.

- K e = 0,8(V.s/Rad): Hằng số sức điện động.
- R = 2,88(Ω): Điện trở.
- L = 6,4.10−3 (H): Điện cảm.

72
B2 (s) 0,8
Gkhau2 (s) = = (Rad/Volt)
A2 (s) 2,927.10−6 s3 +0,001319s2 +0,6406s
(4.23)
Kiểm tra tính ổn định của G2 (s):

+ Điều kiện đủ :

2,927.10−6 0,6406
0,001319 0

73
0,001319
định.



74
Đồ thị Bode :
Để hệ kín 𝐺𝑘 (𝑠) ổn định thì đồ thị bode của hệ hở G ( s ) phải thõa mãn đồng thời
vậy hệ kín này ổn định
c.Khớp 3
Tính momen quán tính Jl3

75
Từ bảng kết quả trên ta thu được Jl2 có giá trị bằng 3,4096.10−4 (Kg.m2 )

76
Jtd = Jm3 + Ja3 + n3 2 Jl3 = 0,58.10−4 + 12 ⋅ 3,4096.10−4 = 3,9096.10−4 (Kg. m2 )

{
Btd = Bm3 + n3 2 . Bl3 = 2,22.10−4 + 12 ⋅ 0,01 = 1,0222.10−2 (Nms/Rad)
(4.24)
Với:

Jm3 + Ja3 = 0,58.10−4 (Kg.m2 )
- Jl3 = 3,4096.10−4 (Kg.m2 ): Mômen quán tính của khâu gắn với hộp giảm
tốc
- Bl3 = 0.01 (Nms/Rad): Hệ số giảm chấn của khâu gắn với hộp giảm tốc
- n3 = 1 : Tỉ số truyền của hộp giảm tốc
- K m = 0,8(N.m/A): Hằng số tỉ lệ.

- K e = 0,8(V.s/Rad): Hằng số sức điện động.
- R = 2,88(Ω): Điện trở.
- L = 6,4.10−3 (H): Điện cảm.

77

B3 (s) 0,016
Gkhau3 (s) = = (m/Volt)
A3 (s) 1,183.10−5 s3 +0,004889s2 +4,137s
(4.25)
Kiểm tra tính ổn định của G3 (s):

+ Điều kiện đủ:

1,183.10−5 4,137
0,004889 0
0,004889
78
định.



79
Đồ thị Bode :
Để hệ kín 𝐺𝑘 (𝑠) ổn định thì đồ thị bode của hệ hở 𝐺(𝑠) phải thõa mãn đồng thời
vậy hệ kín này ổn định.
4.7. Mô phỏng và đánh giá tính ổn định của các chỉ tiêu chất lượng:
4.7.1. Xây dựng mô hình Simulink mô tả cho một khớp của Robot:
Cơ sở lý thuyết
Theo (4.6) và (4.16), ta có:

80
d 2 θm dθm d 2 θm 1 dθm
Jtd = τ m − B td => = (K i − B )
dt 2 dt dt 2 Jtd m td
dt
di di 1 dθm
L = V − Ri − e(t) => = (−Ri + V − K e )
dt dt L dt
(4.27)
Sơ đồ Simulink
Hình 4.22. Sơ đồ simulink cho động cơ servo DC
4.7.2. Sử dụng Matlab – Simulink để mô phỏng cho một trục chuyển động
và cho toàn bộ Robot:
➢ Sơ đồ simulink cho khớp 1 :

81
Hình 4.23. Sơ đồ simulink khớp 1 với hồi tiếp âm
➢ Hàm truyền khâu 1 :
➢ Hàm truyền hồi tiếp âm khâu 1 :

82

83
➢ Sơ đồ Simulink mô phỏng động lực học và điều khiển cho toàn bộ robot
Hình 4.26. Sơ đồ simulink mô phỏng cho toàn bộ robot

84
Hình 4.27. Khối Robot trong Simulink
4.7.3. Xác định sai lệch tĩnh và đánh giá chất lượng đáp ứng:
a. Xác định sai lệch tĩnh:
Hàm truyền của cả 3 khâu đều có dạng :
Km
G(s) = (4.28)
JLs3 +(JR+LB)s2 +(BR+Km Kb )s
Xét sai lệch tĩnh với tín hiệu đầu vào là 1(t)
Sai lệch tĩnh
e = lim e(t) = lim sE(s)

t−>0 s−>0
1 1
e = lim = =0 (4.29)
s−>0 1+G(s) 1+ lim G(s)
s−>0

85
b. Xác định đặc tính của đồ thị đáp ứng cho từng khớp:
➢ Khớp 1
Hình 4.28. Đồ thị đáp ứng của khớp 1
- Độ vọt lố: 0%.

- Thời gian tăng: 2,03s (là khoảng thời gian kể từ khi hệ thống đạt 10% cho
đến khi đạt 90% giá trị xác lập).
- Thời gian xác lập: 3,63 s.
- Trạng thái ổn định: Giá trị cuối là 1, vậy sai lệch tĩnh bằng 0 và hệ ổn
định.

86
➢ Khớp 2
- Độ vọt lố: 0%.

- Thời gian tăng: 1,75s (là khoảng thời gian kể từ khi hệ thống đạt 10% cho
đến khi đạt 90% giá trị xác lập).
- Thời gian xác lập: 3,13 s.
- Trạng thái ổn định: Giá trị cuối là 1, vậy sai lệch tĩnh bằng 0 và hệ ổn
định.

87
➢ Khớp 3
- Độ vọt lố : 0%.
- Thời gian tăng : 1,81s (là khoảng thời gian kể từ khi hệ thống đạt 10%
cho đến khi đạt 90% giá trị xác lập).
- Thời gian xác lập : 3,22 s.
- Trạng thái ổn định : Giá trị cuối là 1, vậy sai lệch tĩnh bằng 0 và hệ ổn
định.

88
Chương 5. Thiết kế hệ thống điều khiển
5.1. Các phương pháp điều khiển:
5.1.1. Phương pháp điều khiển momen:
a. Phương pháp điều khiển momen:

Phương pháp này xem xét sự tinh chỉnh của luật điều khiển vòng hở: Với vị trí
và vận tốc hiện tại của khâu thao tác, hủy bỏ tất cả các phi tuyến và áp dụng
chính xác momen xoắn cần thiết để vượt qua quán tính của bộ truyền động.
τ = M(q)q̈ d + C(q,q̇ )q̇ + G(q,q̇ ) (5.1)
Thay thế luật điều khiển (5.1) vào các phương trình chuyển động của robot
(2.67) ta được:
M(q)q̈ = M(q) q̈ d (5.2)
Do M(q) luôn nhận giá trị dương, ta có:
q̈ = q̈ d (5.3)
Do đó, nếu vị trí và vận tốc ban đầu của tay máy khớp với vị trí và vận tốc mong
muốn, tay máy sẽ đi theo quỹ đạo mong muốn. Như trước đây, luật kiểm soát này
sẽ không hiệu chuẩn cho bất kỳ lỗi điều kiện ban đầu nào.
Các thuộc tính bám của luật điều khiển có thể được cải thiện bằng cách thêm phản
hồi trạng thái. Độ tuyến tính của phương trình (5.3) gợi ý luật điều khiển sau:
τ = M(q)(q̈ d - K v ė - K p e) + C(q,q̇ ) q̇ + G(q,q̇ ) (5.4)

89
Trong đó e = q - qd và K v , K p là các ma trận khuếch đại hằng số. Thay thế vào
phương trình (2.67), động lực sai số (error dynamics) có thể được viết lại như sau:
M(q)(ë + K v ė + K p e) = 0 (5.5)
Do M(q) luôn nhận giá trị dương, ta có:
ë + K v ė + K p e = 0 (5.6)
Đây là một phương trình vi phân tuyến tính xác định lỗi giữa các quỹ đạo thực tế
và mong muốn. Phương trình (5.4) được gọi là luật điều khiển momen.
Luật điều khiển momen bao gồm hai thành phần. Chúng ta có thể viết phương
trình (5.4) dưới dạng:
τ = M(q)q̈ d + C(q,q̇ ) q̇ + G(q,q̇ ) + M(q)(-K v ė - K p e)
= τff + τfb (5.7)
Với
- τff = M(q)q̈ d + C(q,q̇ ) q̇ + G(q,q̇ ) là thành phần tiếp thuận (feedforward). Nó

cung cấp số lượng momen xoắn cần thiết để dẫn động hệ thống dọc theo quỹ đạo
danh nghĩa của nó.
- τfb = M(q)(-K v ė - K p e) là thành phần phản hồi. Nó cung cấp điểm chỉnh sửa để
giảm bất kỳ lỗi nào trong quỹ đạo của trình thao tác.
Để hệ thống ổn định, cần chọn K v , K p là các ma trận chéo (xác định dương và đối
xứng).

90
Ưu điểm của luật điều khiển momen là nó chuyển đổi một hệ động lực phi tuyến
thành một hệ tuyến tính, cho phép sử dụng bất kỳ công cụ tổng hợp điều khiển
tuyến tính nào. Kết quả thử nghiệm cho thấy bộ điều khiển momen có các đặc tính
hiệu suất rất tốt và nó đang ngày càng trở nên phổ biến.
b. Luật điều khiển PD:

Luật điều khiển PD có dạng như sau:
τ = -K v ė - K p e (5.8)
Trong đó, K v , K p là các ma trận xác định dương và e = q - qd .
Vì luật điều khiển này không có thành phần tiếp thuận (feedforward), nên nó
không bao giờ có thể đạt được theo dõi chính xác cho các quỹ đạo không tầm
thường. Một sửa đổi phổ biến là thêm thành phần tích phân để loại bỏ các lỗi trạng
thái ổn định (steady-state errors).
Vì chúng ta chủ yếu quan tâm đến việc bám quỹ đạo, chúng ta xem xét một phiên
bản sửa đổi của luật điều khiển PD:
τ = M(q)q̈ d + C(q,q̇ ) q̇ + G(q,q̇ ) - K v ė - K p e (5.9)
Bộ điều khiển này được gọi là bộ điều khiển PD tăng cường (augmented PD
control law). Hệ thống ổn định khi K v và K p là các ma trận xác định dương.
c. Mô phỏng và phân tích, đánh giá các chỉ tiêu kỹ thuật của hệ thống:
- Xây dựng sơ đồ khối hệ thống điều khiển Scara:

91
Hình 5.1. Sơ đồ hệ thống điều khiển robot Scara (Phương pháp điều khiển momen)
- Xây dựng chương trình mô phỏng điều khiển hệ thống (Matlab/Simulink):
Hình 5.2. Sơ đồ hệ thống theo phương pháp điều khiển momen trên Matlab/simulink

92
Khai báo quỹ đạo chuyển động của các khâu vào Matlab:
Hình 5.3. Thông số quỹ đạo
Hình 5.4. Các khối chức năng chính

93
Hình 5.5. Khối PD
Hình 5.6. Khối tính lực dẫn động

94
Hình 5.7. Khối động lực học ngược

95
Hình 5.8. Sơ đồ mô phỏng 3D Robot

96
Hình 5.9. Đồ thị biến khớp mong muốn và đáp ứng 1
Hình 5.10. Sai số biến khớp 1

97

98

99
Hình 5.15. Mô phỏng Robot Scara
5.1.2. Phương pháp điều khiển PID:
a. Bộ điều khiển PID:
Hình 5.16. Sơ đồ khối bộ điều khiển PID

100
Với mục tiêu đưa robot về một vị trí mong muốn ứng với các tọa độ khớp qd
theo yêu cầu quỹ đạo cần thực hiện, ta xét luật điều khiển đơn giản và phổ biến
đáp ứng yêu cầu này. Đó là luật điều khiển PID. Tín hiệu điện điều khiển được
tính tỷ lệ với sai lệch về vị trí và vận tốc của thời điểm hiện tại so với giá trị mong
muốn.
Theo luật điều khiển này, tín hiệu điện điều khiển:
de(t) t
u(t) = K p . e(t) + K d . + K I .∫0 e(t)dt (5.10)
dt
Với e(t) = qd (t) - q(t).
Trong đó:
- e(t) là sai lệch giá trị biến khớp mong muốn với giá trị biến khớp đạt
được tại thời điểm t.
- qd (t) là giá trị biến khớp mong muốn tại thời điểm t được lấy ra từ yêu
cầu của quỹ đạo mong muốn.
- q(t) là giá trị biến khớp thực đạt được tại thời điểm t thu được từ hệ thống
phản hồi đầu ra của hệ thống , cụ thể ở đây là cảm biến đo góc khớp trên
robot.
Triển khai trên Matlab
Xây dựng khối controller

101
Hình 5.17. Sơ đồ simulink một trục khớp
Hình 5.18. Sơ đồ Simulink khối Controller
b. Chọn các hệ số cho hàm điều khiển:

Sử dụng công cụ Tune PID của Matlab để chọn được bộ số PID thích hợp cho
từng trục.
Hình 5.19. Block PID controller trên Matlab

102
Tại mục Select tuning method chọn Tune... để bắt đầu quá trình chọn hệ số PID
Hình 5.20. Cửa sổ PID Tuning
Tại mục Tuning Tools của của sổ tùy chỉnh hai thanh trượt để thu được đồ thị
đáp ứng mong muốn. Kết thúc nhấn vào Update block để nhập hệ số PID tương
ứng với đồ thị mong muốn.
Hình 5.21. Mục Tuning Tools và Results

103
Kết quả đáp ứng sáu khi đã chỉnh được hệ số PID
Hình 5.22. Đồ thị đáp ứng biến khớp của trục khớp sau khi đã chỉnh các hệ số PID
Với cách làm như trên ta thu được hệ số PID cho các trục khớp của Robot như
sau:
Khớp Kp Ki Kd
1 3,235 163,136 0,0157
2 3,122 210,739 0,0091
3 34731,850 840949,615 109,2343
Kết quả của hệ cả hệ điều khiển sau khi đã có bộ điều khiển hoàn chỉnh áp dụng
cho quỹ đạo của một chu trình gắp nhả vật được thiết kế ở phần 2
Với đường màu xanh là giá trị mong muốn và đường màu đỏ là giá trị đáp ứng

104

105

106
Nhận xét :
Hệ điều khiển đã cho ra đồ thị biến khớp đáp ứng gần như bám sát so với đồ thị
mong muốn và có sai số nhỏ.
107
5.2. Thiết kế bộ điều khiển bám quỹ đạo:
5.2.1. Bộ điều khiển bám quỹ đạo:
Xét hàm truyền cả ba khâu có dạng:
θm (s) Km 1
= 2 . (5.11)
V(s) Jtd Ls +(Btd L+Jtd R)s+Btd R+Km Ke s
Suy ra:
K m . V(s) = Jtd Ls 3 . θm (s) + (Btd L + Jtd R)s 2 . θm (s) + (Btd R + K m K e )s. θm (s)
Phương trình vi phân chuyển động cho một khớp:
K m . V(t) = Jtd L. θm (3) (t) + (Btd L + Jtd R). θ̈m (t) + (Btd R + K m K e ). θ̇m (t)
Hay:
Jtd L Btd L+Jtd R Btd R+Km Ke

V(t) = . q(3) (t) + . q̈ (t) + .q̇ (t) (5.12)
Km Km Km
Mục đích của phương pháp này là hàm điều khiển được chọn sao cho phương
trình động học của sai số có dạng:
e(3) - K 3 ë - K 2 ė - K1 e = 0 (5.13)
Trong đó :
- e(t) = q(t) - qd (t) là sai lệch giá trị biến khớp mong muốn với giá trị biến
khớp đạt được tại thời điểm t.
- qd (t) là giá trị biến khớp mong muốn tại thời điểm t được lấy ra từ yêu
cầu của quỹ đạo mong muốn.

108
- q(t) là giá trị biến khớp thực đạt được tại thời điểm t thu được từ hệ thống
phản hồi đầu ra của hệ thống , cụ thể ở đây là cảm biến đo góc khớp trên
robot.
Để có được điều này , ta chọn hàm điều khiển :
Jtd L Btd L+Jtd R Btd R+Km Ke

u(t) = .( qd (3) +K 3 ë +K 2 ė +K1 e) + .q̈ + .q̇ (5.14)
Km Km Km
Như vậy, hệ trên (5.13) sẽ ổn định nếu như đa thức sau là Hurwitz (các điểm cực
nằm bên trái trục ảo):
H(s) = s 3 + K 3 s 2 + K 2 s + K1 (5.15)
Vậy các hệ số K1 , K 2 , K 3 được chọn sao cho đa thức trên là Hurwitz (các điểm
cực nằm bên trái trục ảo).
Thiết kế khối controller trên matlab:
Hình 5.29. Sơ đồ simulink khối controller

109
5.2.2. Chọn hệ số cho bộ điều khiển bám quỹ đạo:
Theo (5.15) các các hệ số K1 , K 2 , K 3 được chọn sao cho đa thức H(s) trên là
Hurwitz (các điểm cực nằm bên trái trục ảo).
Dựa vào đó ta chọn được các bộ số sau :
Khớp K1 K2 K3
1 12.109 74.105 15.103
2 288.108 18.107 2.104
3 18.1011 18.107 15.103
Sơ đồ simulink cho toàn bộ hệ thống :
Hình 5.30. Sơ đồ simulink điều khiển bám quỹ đạo

110
Chương 6. Mô phỏng nguyên lý hoạt động (SIMMECHANICS)
Đã hoàn thành mô phỏng động lực học trên Simscape trên Matlab cho một quá
trình gắp nhả vật trong 12,15s :
Hình 6.1. Robot Scara trong Matlab Simulink

111
Chương 7. Lựa chọn thiết bị và thiết kế mạch điều khiển
7.1.Chọn các phần tử của hệ thống điều khiển:
Hình 7.1. Sơ đồ khối của hệ thống điều khiển
Các thành phần chính của robot bao gồm:

- Động cơ AC Servo.
- Driver điều khiển động cơ Servo.
- Bộ điều khiển trung tâm: PLC hoặc Vi điều khiển.
Động cơ AC Servo và Driver lựa chọn động cơ và driver của hãng Panasonic

112
Bảng 7.1. Bảng lựa chọn động cơ và driver Panasonic

113
Hình 7.2. Họ driver điều khiển lớp B
Mạch điều khiển: PLC MITSUBISHI FX3G-40MR/DS

Thông số kỹ thuật:
• Điện áp nguồn cung cấp: 24 VDC.
• Số ngõ vào/ ngõ ra: 24 DI/ 16 DO.
• Loại ngõ ra: Relay.
• Kết nối truyền thông: RS422, hỗ trợ mở rộng board RS232, RS485,
Ethernet, CC-link.
• Bộ đếm tốc độ cao: 6 ngõ vào max 60 kHz.
• Phát xung tốc độ cao: 3 chân 100 kHz, có thể mở rộng thông qua module.
• Tổng I/O: 40.
• Có thể mở rộng lên tới 128 I/O thông qua module hoặc 256 I/O thông qua
mạng CC-link.
• DC power supply type.

114
Hình 7.3. PLC MITSUBISHI FX3G-40MR/DS
7.2. Xác định hàm điều khiển:
Các chế độ điều khiển của động cơ Servo bao gồm:

- Điều khiển vị trí (Position).
- Điều khiển tốc độ (Speed).
- Điều khiển lực, momen (Torque).
Ba chế độ này được định nghĩa và điều khiển theo thiết kế của Driver.

115
7.2.1. Điều khiển vị trí (Position):
Hình 7.4. Kết nối điều khiển vị trí
a. Quy trình đầu vào xung lệnh:

Các lệnh vị trí có 3 loại:
• 2 pha xung.
• Xung hướng dương/ xung hướng âm.
• Xung + hướng.
Thiết lập cấu hình xung và phương pháp đếm xung dựa trên các đựac điểm kỹ
thuật và cấu hình cài đặt bộ điều khiển.
Các thiết bị đầu cuối đầu vào có thể đáp ứng được 2 hệ thống sau:
• Đầu vào 1 đầu vào bộ thu dòng “PULSH1, PULSH2, SIGNH1, SIGNH2”
(4 Mpps).
• Đầu vào 2 đầu vào bộ ghép quang “PULS1, PULS2, SIGN1, SIGN2”
(500 kpps).

116
Bảng 7.2. Các thông số liên quan đầu vào xung (Điều khiển vị trí)
Tham số Ý nghĩa Phạm vi Chức năng

Pr0.05 Lựa chọn lệnh 0 đến 1 Chọn đầu vào ghép hoặc đầu độc
đầu vào xung. quyền cho trình điều khiển dòng
dưới dạng xung lệnh đầu vào.
Pr0.06 Xoay xung lệnh 0 đến 1 Đặt hướng đếm khi xung lệnh là đầu
thiết lập hướng. vào.
Pr0.07 Đầu vào xung 0 đến 3 Đặt phương pháp đếm khi xung
lệnh thiết lập chế lệnh là đầu vào.
độ.
b. Chức năng bộ truyền:

Chức năng này nhân lệnh xung đầu vào từ bộ điều khiển máy với lệnh
trước xác định hệ số chia hoặc nhân và áp dụng kết quả cho điều khiển vị trí
phần dưới dạng lệnh vị trí. Bằng cách sử dụng chức năng này, các chuyển động
quay mong muốn hoặc khoảng cách di chuyển trên mỗi đơn vị xung lệnh đầu
vào có thể được thiết lập; hoặc xung lệnh có thể tăng tần số nếu không thể đạt
được tốc độ động cơ mong muốn do giới hạn công suất đầu ra xung của bộ điều
khiển.
Bảng 7.3. Các thông số liên quan đến điều khiển xung (Điều khiển vị trí)

Pr0.08 Số lượng xung 0 đến Thiết lập xung lệnh tạo chuyển
lệnh trên một 1048576 động quay cho động cơ.
vòng quay
Pr0.09 Tử số 0 đến Đặt tử số của phép chia/ phép
1048576 nhân hoạt động được thực hiện
theo xung lệnh đầu vào.

117
Pr0.10 Mẫu số 0 đến Đặt mẫu số của phép chia/ phép

1073741824 nhân hoạt động được thực hiện
theo xung lệnh đầu vào.
c. Chức năng lọc lệnh vị trí:
Để làm cho lệnh vị trí được chia hoặc nhân với hệ số trơn tru, ta thiết lập bộ lọc
lệnh.
Bảng 7.4. Các thông số liên quan đến lọc lệnh vị trí
Tham số Ý nghĩa Phạm vi Đơn vị Chức năng

Pr2.22 Lệnh vị trí 0 đến 0,1ms Thiết lập hằng số thời gian trì
làm mịn 10000 hoãn để đáp ứng với vị trí.
Pr2.23 Lệnh vị trí 0 đến 0,1ms Thiết lập hằng số thời gian trì
bộ lọc FIR 10000 hoãn để đáp ứng với vị trí.
d. Chức năng tái tạo xung:

Thông tin về lượng chuyển động có thể được gửi đến bộ điều khiển trong
dạng xung pha A và B từ trình điều khiển servo. Khi nguồn đầu ra là bộ mã hóa,
tín hiệu pha Z được xuất ra một lần cho mỗi vòng quay động cơ; hoặc nếu quy
mô phản hồi, tín hiệu được xuất ra ở vị trí không tuyệt đối. Độ phân giải đầu ra,
logic pha B vànguồn đầu ra (bộ mã hóa hoặc thang đo bên ngoài) có thể được
thiết lập với các tham số.
Bảng 7.5. Các thông số liên quan đến cải tạo xung

Pr0.11 Đếm xung 0 đến P/r Bạn có thể thiết lập số lượng
đầu ra độ 262144 xung đầu ra trên một vòng
phân giải của quay động cơ cho mỗi OA và
động cơ. OB với thiết lập Pr0.11.

118
Pr0.12 Đảo ngược 0 đến 3 - Bạn có thể thiết lập logic pha B
xung logic và nguồn đầu ra của đầu ra
đầu ra. xung. Với tham số này, bạn có
thể đảo ngược quan hệ pha
giữa xung pha A và xung pha
B bằng cách đảo ngược logic
pha B.
Pr5.03 Mẫu số của 0 đến - Đối với ứng dụng trong đó số
xung phân 262144 xung trên mỗi vòng quay
chia đầu ra. không phải là số nguyên, tham
số này có thể được đặt thành
giá trị khác 0 và tỷ lệ chia có
thể được đặt bằng cách đặt tử
số của phép chia thành Pr0.11
và mẫu số của phép chia thành
Pr5.03.
Pr5.33 Tái tạo xung 0 đến 1 - Bật / tắt phát hiện bảo vệ giới
thiết lập giới hạn tái tạo xung Err28.0.
hạn đầu ra.
Pr6.20 Thiết lập pha 0 đến μs Thiết lập độ rộng tái tạo pha Z
Z của thang 400 của thang phản hồi theo đơn vị
đo bên ngoài. thời gian.
Pr6.21 Thiết lập pha 0 đến pulse Điều khiển hoàn toàn kín sử
Z bên ngoài 268435 dụng thang phản hồi tuyệt đối
áp suất nối 456 nối tiếp. Khi xuất xung bằng
tiếp. cách sử dụng thang phản hồi
làm nguồn của đầu ra, hãy đặt
khoảng thời gian đầu ra pha Z
theo đơn vị xung đầu ra pha A
của thang phản hồi (trước khi
nhân với 4).

119
Pr6.22 Lựa chọn 0 đến 1 - Chọn phương pháp tái tạo xung
phương pháp A, B và Z thang phản hồi song
đầu ra xung song.
thang đo bên
ngoài pha A,
B.
e. Chức năng xóa bộ đếm độ lệch:

Đầu vào xóa bộ đếm độ lệch (CL) xóa số đếm của bộ đếm độ lệch vị trí ở
điều khiển vị trí về 0.
Bảng 7.6. Các thông số liên quan đến xóa bộ đếm độ lệch

Pr5.17 Chế độ đầu vào 0 đến 4 Bạn có thể thiết lập các điều kiện bù trừ
xóa bộ đếm của tín hiệu đầu vào rõ ràng của bộ đếm.
f. Chức năng vị trí đầu ra (INP):

Việc hoàn thành định vị có thể được xác minh bằng đầu ra hoàn thành
định vị (INP). Khi giá trị tuyệt đối của bộ đếm độ lệch vị trí tại điều khiển vị trí
bằng hoặc thấp hơn phạm vi hoàn chỉnh của vị trí theo tham số, đầu ra là BẬT.
Sự hiện diện và không có của lệnh vị trí có thể được chỉ định là một trong những
điều kiện phán đoán.
Bảng 7.7. Các thông số liên quan đến vị trí đầu ra

Pr4.31 Định vị hoàn 0 đến Đơn vị Thiết lập thời gian của độ lệch
thành phạm 262144 điều vị trí tại đó tín hiệu định vị hoàn
vi (Tại vị khiển chỉnh (INP1) được xuất ra.
trí).
Pr4.32 Định vị hoàn 0 đến 3 - Chọn điều kiện để xuất ra tín
thành thiết hiệu định vị hoàn chỉnh (INP1).

120
lập đầu ra
(Tại vị trí).
Pr4.33 Thời gian 0 đến 1ms Thiết lập thời gian giữ khi
giữ INP. 30000 Pr4.32 Định vị hoàn tất thiết lập
đầu ra = 3.
Pr4.42 Phạm vi định 0 đến Đơn vị Thiết lập thời gian của độ lệch
vị hoàn 262144 điều vị trí tại đó tín hiệu định vị hoàn
thành lần thứ khiển chỉnh (INP2) được xuất ra.
2 (Tại vị trí).
g. Chức năng ức chế xung lệnh (INH):

Quá trình đếm đầu vào xung lệnh có thể được kết thúc cưỡng bức bằng
cách sử dụng tín hiệu đầu vào ức chế xung lệnh (INH). Khi đầu vào INH BẬT,
trình điều khiển servo sẽ kích hoạt xung lệnh, vô hiệu hóa chức năng đếm xung.
Cài đặt mặc định của chức năng ức chế này là vô hiệu hóa. Để sử dụng
chức năng INH, hãy thay đổi cài đặt của Pr5.18 “Ngõ vào cấm xung lệnh không
hợp lệ”.
Bảng 7.8. Các thông số liên quan đến ức chế xung lệnh

Pr5.18 Sự vô hiệu của 0 đến 1 Chọn bật / tắt đầu vào ức chế xung
đầu vào ức chế lệnh.
xung lệnh.
Pr5.19 Thiết lập đọc đầu 0 đến 4 Chọn chu kỳ đọc tín hiệu đầu vào
vào ức chế xung ức chế xung lệnh cho phép / vô hiệu
lệnh. hóa. Khi trạng thái của một số tín
hiệu được đọc trong khoảng thời
gian đọc định trước giống nhau, hãy
cập nhật trạng thái tín hiệu.

121
7.2.2. Điều khiển tốc độ (Speed):
a. Điều khiển vận tốc bằng lệnh tốc độ tương tự:
Điện áp đầu vào lệnh tốc độ tương tự được chuyển đổi thành lệnh tốc độ
số tương đương. Có thể đặt bộ lọc để loại bỏ nhiễu hoặc điều chỉnh độ lệch.
Bảng 7.9. Các thông số liên quan đến điều khiển tốc độ

Pr3.00 Thiết lập tốc 0 đến 3 Trình điều khiển này được
-
độ, chuyển trang bị chức năng cài đặt tốc
đổi bên trong độ bên trong để bạn có thể
/ bên ngoài. kiểm soát tốc độ chỉ với đầu
vào tiếp xúc.
Pr3.01 Lệnh tốc độ 0 đến 1 - Chọn phương pháp chỉ định
lựa chọn hướng thuận chiều/ ngược
hướng quay. chiều.
Pr3.02 Độ khuếch 10 đến (r/min) Dựa trên điện áp được áp dụng
đại đầu vào 2000 /N cho lệnh tốc độ tương tự
của lệnh tốc (SPR), hãy thiết lập độ khuếch
độ. đại chuyển đổi thành tốc độ
lệnh của động cơ.
Pr3.03 Đảo ngược 0 đến 1 - Chỉ định cực tính của điện áp
đầu vào lệnh được áp dụng cho lệnh tốc độ
tốc độ. tương tự (SPR).
Pr4.22 Thiết lập bù -5578 0.359mV Thiết lập giá trị hiệu chỉnh độ
đắp đầu vào đến lệch được áp dụng cho điện áp
tương tự 1 5578 cấp cho đầu vào tương tự 1.
(AI1).
Pr4.23 Bộ lọc đầu 0 đến 0.01ms Thiết lập hằng số thời gian của
vào tương tự 6400 bộ lọc trễ thứ nhất để xác định
1 (AI1). thời gian trễ sau điện áp đặt
vào đầu vào tương tự 1.

122
b. Kiểm soát vận tốc bằng lệnh tốc độ nội suy:
Bạn có thể kiểm soát tốc độ bằng cách sử dụng lệnh tốc độ bên trong được
đặt thành tham số. Bằng cách sử dụng các lựa chọn lệnh tốc độ nội bộ 1, 2, 3
(INTSPD1, 2, 3), bạn có thể chọn một lệnh thích hợp nhất trong số tối đa 8 cài
đặt lệnh tốc độ nội bộ. Cài đặt mặc định sử dụng lệnh tốc độ tương tự. Để sử
dụng lệnh tốc độ bên trong, hãy chọn lệnh đó thông qua Pr3.00 “Cài đặt tốc độ
bên trong / bên ngoài”.
Bảng 7.10. Các thông số liên quan đến kiểm soát vận tốc bằng lệnh tốc độ nội suy

Pr3.00 Thiết lập tốc 0 đến 3 - Trình điều khiển này được
độ, chuyển đổi trang bị chức năng cài đặt tốc
bên trong / bên độ bên trong để bạn có thể
ngoài kiểm soát tốc độ chỉ với đầu
vào tiếp xúc.
Pr3.01 Lệnh tốc độ lựa 0 đến 1 - Chọn phương pháp chỉ định
chọn hướng hướng thuận chiều/ ngược
quay chiều.
Pr3.04 Tốc độ thiết Thiết lập tốc độ lệnh nội suy,
lập tốc độ đầu từ 1 đến 1.
tiên
lập tốc độ thứ 2 từ 1 đến 2.
Pr3.09 Tốc độ thiết -2000 r/min Thiết lập tốc độ lệnh nội suy,
lập tốc độ thứ 6 đến từ 1 đến 6.
Pr3.10 Tốc độ thiết 2000 Thiết lập tốc độ lệnh nội suy,
123

c. Chức năng kẹp không tốc độ (ZEROSPD)

Bạn có thể buộc đặt lệnh tốc độ thành 0 bằng cách sử dụng đầu vào kẹp
tốc độ 0.
Bảng 7.11. Các thông số liên quan đến chức năng kẹp không tốc độ

Pr3.15 Lựa chọn 0 đến 3 - Bạn có thể thiết lập chức năng
chức năng của đầu vào kẹp tốc độ 0.
kẹp 0 tốc độ
Pr3.16 Mức kẹp 0 đến 1 r/min chọn thời gian kích hoạt điều
không tốc khiển vị trí khi lựa chọn chức
độ năng kẹp 0 tốc độ Pr3.15 được
đặt thành 2 hoặc 3.
d. Đầu ra tốc độ dạt được (AT-SPEED):

Tín hiệu AT-SPEED được xuất ra khi động cơ đạt đến tốc độ được đặt
thành Pr4.36 “Tốc độ đạt được”.
Bảng 7.12. Các thông số liên quan đến đầu ra tốc độ

Pr4.36 At-speed 10 đến r/min Đặt thời gian phát hiện của đầu
(Tốc độ đến) 20000 ra tốc độ đến (AT-SPEED).
e. Đầu ra trùng hợp tốc độ (V-COIN):

Đây là tín hiệu đầu ra khi tốc độ động cơ bằng với tốc độ quy định bởi các
lệnh tốc độ. Tốc độ động cơ được đánh giá là trùng với tốc độ quy định khi sự
khác biệt so với lệnh tốc độ trước / sau khi tăng / giảm tốc nằm trong phạm vi
được chỉ định bởi Pr4.35 “Phạm vi trùng khớp với tốc độ”.
124
Bảng 7.13. Thông số liên quan đến phạm vi trùng hợp tốc độ

Pr4.35 Phạm vi 10 đến r/min Đặt thời gian phát hiện đầu ra
trùng hợp 20000 trùng hợp tốc độ (V-COIN).
tốc độ
f. Chức năng cài đặt lệnh tăng/ giảm tốc độ:

Chức năng này kiểm soát tốc độ bằng cách thêm lệnh tăng hoặc giảm tốc
trong trình điều khiển vào lệnh tốc độ đầu vào.
Sử dụng chức năng này, bạn có thể sử dụng khởi động mềm khi nhập lệnh
tốc độ từng bước hoặc khi sử dụng thiết lập tốc độ nội bộ. Bạn cũng có thể sử
dụng chức năng tăng / giảm tốc hình chữ S để giảm thiểu sốc do thay đổi tốc độ.
Bảng 7.14. Các tham số liên quan đến việc tăng, giảm tốc độ

Pr3.12 Thiết lập 0 đến ms/ Thiết lập thời gian xử lý gia
thời gian 10000 (1000r/min) tốc để đáp ứng với đầu vào
tăng tốc lệnh tốc độ.
Pr3.13 Thiết lập 0 đến ms/ Thiết lập thời gian xử lý
thời gian 10000 (1000r/min) giảm tốc theo đầu vào lệnh
giảm tốc tốc độ.
Pr3.14 Thiết lập 0 đến ms Đặt thời gian đường cong S
thời gian 10000 cho quá trình tăng / giảm tốc
tăng / giảm khi áp dụng lệnh tốc độ.
tốc Sigmoid
7.2.3. Điều khiển lực, momen (Torque):
Các mô-men xoắn kiểm soát được thực hiện theo để các mô-men xoắn
lệnh quy định trong các hình thức của điện áp analog. Để điều khiển mô-men

125
xoắn, đầu vào giới hạn tốc độ được yêu cầu ngoài lệnh mô-men xoắn để duy trì
tốc độ động cơ trong giới hạn tốc độ .
Với dòng A5, 3 chế độ điều khiển mô-men xoắn có sẵn, mỗi chế độ yêu
cầu lệnh mô-men xoắn và giới hạn tốc độ khác nhau như thể hiện trong bảng
dưới đây:
Bảng 7.15. Bảng lựa chọn lệnh momen
Giá trị cài đặt Lệnh mô-men xoắn Đầu vào giới hạn
đầu vào vận tốc
0 Lựa chọn lệnh Đầu vào tương tự 1 Giá trị tham số
mô-men xoắn 1 (AI1, độ phân giải (Pr3.21)
16-bit)
1 Lựa chọn lệnh Đầu vào tương tự 2 Đầu vào tương tự 1
mô-men xoắn 2 (AI2, độ phân giải (AI1, độ phân giải
12-bit) 16-bit)
2 Lựa chọn lệnh Đầu vào tương tự 1 Giá trị tham số
mô-men xoắn 3 (AI1, độ phân giải (Pr3.21, Pr3.22)
16-bit)
Hình 7.5. Kết nối điều khiển lực, momen

126
a. Quy trình lệnh đầu vào mô-men xoắn tương tự:

Quá trình này chuyển đổi đầu vào lệnh mô-men xoắn tương tự (điện áp)
thành lệnh mô-men xoắn kỹ thuật số tương đương có cùng tác dụng. Bạn có thể
đặt bộ lọc hoặc điều chỉnh cài đặt tắt để loại bỏ tiếng ồn.
Bảng 7.16. Các thông số liên quan đến đầu vào (Lựa chọn lệnh mô-men xoắn 1, 3)

Pr3.18 Chọn hướng 0 đến 1 - Chọn hướng dương/ hướng
lệnh mô-men âm của lệnh mômen.
xoắn
Pr3.19 Độ khuếch đại 10 đến 0.1V/ Dựa trên điện áp (V) được áp
đầu vào của 100 100% dụng cho lệnh mô-men xoắn
lệnh mô-men tương tự (TRQ R), thiết lập độ
xoắn lợi chuyển đổi thành lệnh mô-
men xoắn (%).
Pr3.20 Đảo ngược 0 đến 1 - Thiết lập cực tính của điện áp
đầu vào của đặt vào lệnh mô-men xoắn
lệnh mô-men tương tự (TRQR).
xoắn
Pr4.22 Thiết lập bù -5578 0.359m Thiết lập giá trị hiệu chỉnh độ
đầu vào tương đến V lệch được áp dụng cho điện áp
tự 1 (AI1) 5578 cấp cho đầu vào tương tự 1.
Pr4.23 Bộ lọc đầu vào 0 đến 0.01ms Thiết lập hằng số thời gian của
tương tự 1 6400 bộ lọc trễ thứ nhất để xác định
(AI1) thời gian trễ sau điện áp đặt

127
Bảng 7.17. Các thông số liên quan đến đầu vào (Lựa chọn lệnh mô-men xoắn 2)

Pr3.18 Chọn hướng 0 đến 1 - Chọn hướng dương / hướng âm
lệnh mô- của lệnh mômen.
men xoắn
Pr3.19 Độ khuếch 10 đến 0.1V/ Dựa trên điện áp (V) được áp
đại đầu vào 100 100% dụng cho lệnh mô-men xoắn
của lệnh tương tự (TRQ R), thiết lập độ
mô-men lợi chuyển đổi thành lệnh mô-
xoắn men xoắn (%).
Pr3.20 Đảo ngược 0 đến 1 - Thiết lập cực tính của điện áp
đầu vào của đặt vào lệnh mô-men xoắn
lệnh mô- tương tự (TRQR).
men xoắn
Pr4.25 Thiết lập bù -342 đến 5.86mV Thiết lập giá trị hiệu chỉnh độ
đầu vào 342 lệch được áp dụng cho điện áp
tương tự 2 cấp cho đầu vào tương tự 2.
(AI2)
vào tương 6400 bộ lọc trễ thứ nhất xác định
tự 2 (AI2) thời gian trễ sau điện áp đặt vào
đầu vào tương tự 2.
b. Chế năng giới hạn tốc độ:

Giới hạn tốc độ là một trong những chức năng bảo vệ được sử dụng trong
quá trình điều khiển mô-men xoắn.
Đây chức năng điều chỉnh các động cơ tốc độ quá mà nó không không
vượt quá các tốc độ giới hạn trong khi mô-men xoắn được kiểm soát.
Trong khi giới hạn tốc độ được sử dụng để điều khiển động cơ, lệnh mô-
men xoắn áp dụng cho động cơ không tỷ lệ thuận với lệnh mô-men xoắn tương

128
tự. Lệnh mô-men xoắn sẽ có kết quả như sau: tốc độ động cơ bằng tốc độ giới
hạn.
Bảng 7.18. Các thông số liên quan đến giới hạn tốc độ (Lựa chọn lệnh mô-men xoắn 1, 3)

Pr3.21 Giá trị giới 0 đến r/min Thiết lập giới hạn tốc độ được
hạn tốc độ 1 20000 sử dụng để kiểm soát mô-men
Pr3.22 Giá trị giới 0 đến r/min xoắn.
hạn tốc độ 2 2000
kẹp 0 tốc độ
Bảng 7.19. Các thông số liên quan đến giới hạn tốc độ (Lựa chọn lệnh mô-men xoắn 2)
Tham số Ý nghĩa Phạm vi Đơn vịChức năng

Pr3.02 Độ khuếch 10 đến Dựa trên điện áp được áp dụng
(r/min)
đại đầu vào 2000 cho lệnh tốc độ tương tự
/N
của lệnh tốc (SPR), hãy thiết lập độ lợi
độ chuyển đổi thành tốc độ lệnh
của động cơ.
Pr4.22 Thiết lập bù -5578 0.359mV Thiết lập giá trị hiệu chỉnh độ
đầu vào đến 5578 lệch được áp dụng cho điện áp
tương tự 1 cấp cho đầu vào tương tự 1.
(AI1)
vào tương tự 6400 bộ lọc trễ thứ nhất để xác định
1 (AI1) thời gian trễ sau điện áp đặt
kẹp 0 tốc độ

129
7.3. Xây dựng bản vẽ:
a. Sơ đồ điện:
Hình 7.6. Sơ đồ điện của hệ thống

130
b. Sơ đồ điều khiển:
Hình 7.7. Mô hình quỹ đạo chuyển động

Bảng 7.20. Vị trí các điểm trong quỹ đạo chuyển động
STT Vị trí Ox Oy Oz Q1 Q2 Q3
1 Home 0 0 0 0 0 0
2 A’ 0 500 400 49.4642 68.1983 100
3 A 0 500 200 49.4642 68.1983 300
4 A’ 0 500 400 49.4642 68.1983 100
5 B’ 500 0 400 -40.5358 68.1963 100
6 B 500 0 200 -40.5358 68.1963 300
7 B’ 500 0 400 -40.5358 68.1963 100
Bảng 7.21. Bảng quy đổi điều khiển vị trí (với Encoder 10 bit)
STT Q1 Q2 Q3 Pulse1 Pulse2 Pulse3

1 0 0 0 0 0 0
2 49.4642 68.1983 100 11397 15713 5120
3 49.4642 68.1983 300 11397 15713 15360
4 49.4642 68.1983 100 11397 15713 5120
5 -40.5358 68.1963 100 -9939 15712 5120
6 -40.5358 68.1963 300 -9939 15712 15360
7 -40.5358 68.1963 100 -9939 15712 5120

131
Bước 1: Khởi tạo việc bật/ tắt hệ thống và đặt các servo ở chế độ làm việc

132
Bước 2: Đưa toàn bộ các trục về vị trí khởi tạo (Home)

133
Bước 3: Đưa khâu thao tác từ vị trí ban đầu đến vị trí A’

134
Bước 4: Đưa khâu thao tác từ vị trí A’ đến vị trí A
Bước 5: Đưa khâu thao tác từ vị trí A đến vị trí A’

135
Bước 6: Đưa khâu thao tác từ vị trí A’ đến vị trí B’
Bước 7: Đưa khâu thao tác từ vị trí B’ đến vị trí B

136
Bước 8: Đưa khâu thao tác đến vị trí B’
Chú giải:
D
Toán hạng Ý nghĩa
S1 Số lượng xung đầu ra (địa chỉ tuyệt đối)
Phạm vi: -999 999 đến 999 999
S2 Tần số xung đầu ra
Phạm vi: 10 đến 100 000 (Hz)
D1 Số hiệu cổng phát xung
D2 Số hiệu cổng đầu ra tín hiệu hướng quay
Khi sử dụng bộ tạo xung tốc độ cao đặc biệt thì tín hiệu hướng quay phải
sử dụng đầu ra tương ứng theo bảng dưới đây:
Bộ chuyển đầu ra tốc độ cao Đầu ra xung Đầu ra hướng quay
Đơn vị thứ 1 D1 = Y000 D2 = Y004
D1 = Y001 D2 = Y005
Đơn vị thứ 2 D1 = Y002 D2 = Y006
D1 = Y003 D2 = Y007

137
Bảng 7.22. Ký hiệu trong bản vẽ
STT Ký hiệu Ý nghĩa

1 CD1 Cầu dao mạch động lực.
2 CC1 Cầu chì.
3 ON Cấp điện cho động cơ.
4 OFF Ngắt điện động cơ.
5 START Bắt đầu hệ thống.
6 HOME Đưa các khâu về vị trí gốc.
7 ENDSTOP1 Công tắc hành trình khâu 3.
10 X1~7 Chân tín hiệu đầu vào của PLC.
11 Y0, Y1, Y2 Chân phát xung tương ứng với động cơ 3,2,1.
12 Y4, Y5, Y6 Chân tín hiệu hướng quay tương ứng với động cơ
3,2,1.
13 Y10, Y11, Y12 Chân khởi động Servo tương ứng với động cơ 3,2,1.
14 COM0, COM1, Chân đối chiếu giá trị của PLC (24V).
COM2, COM3,
COM4
15 L1, L2, L3, Chân cấp nguồn cho động cơ Servo.
L1C, L2C
16 PULS1 Chân nhận xung điều khiển và chân đối chiếu giá trị.
PULS2
17 SIGN1 Chân nhận tín hiệu hướng quay và chân đối chiếu giá
SIGN2 trị.
18 SRV-ON Chân nhận tín hiệu bật động cơ.
19 COM+ 24V.
20 COM- 0V.
21 DC24V Nguồn cấp điện 24V.
22 U, V, W, E Nguồn điện từ driver cấp cho động cơ.
23 X6 (driver) Chân nhận tín hiệu Encoder của driver.

138
Tài liệu tham khảo:

1. Trịnh Chất: “Cơ sở thiết kế máy và chi tiết máy”, nhà xuất bản khoa học và
kĩ thuật,2001.
2. Trịnh Chất – Lê Văn Uyển: “Tính toán thiết kế hệ dẫn động cơ khí tập I và
tập II”, nhà xuất bản giáo dục, 1998.
3. PGS.TS. Ninh Đức Tốn: “Dung sai lắp ghép và kỹ thuật đo lường”, nhà
xuất bản giáo dục, 2006.
4. PGS.TS. Phan Bùi Khôi: Bài giảng Robotics.

139

ScaraRobot 20170852 5 9

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

ScaraRobot 20170852 5 9

Uploaded by

Copyright:

Available Formats

Đồ án thiết kế hệ thống điều khiển

LỜI NÓI ĐẦU ........................................................................................................................................ 4

BẢNG PHỤ LỤC ................................................................................................................................... 5

DANH SÁCH HÌNH VẼ ................................................................................................................... 5

DANH SÁCH BẢNG ........................................................................................................................ 9

Chương 1. Phân tích nguyên lý và thông số kỹ thuật........................................................................11

1.1. Tổng quan: ................................................................................................................................ 11

1.2. Nguyên lý hoạt động: ............................................................................................................... 13

1.3. Các thành phần của hệ thống điều khiển: .............................................................................. 14

1.4. Thông số kỹ thuật cần thiết kế của robot:.............................................................................. 15

1.5. Bài toán điều khiển Robot: ...................................................................................................... 18

Chương 2. Thiết kế hệ thống cơ khí....................................................................................................21

Mô hình nghiên cứu ........................................................................................................................ 21

2.1. Động học robot SCARA ........................................................................................................... 22

2.1.1. Động học thuận robot........................................................................................................ 22

2.1.2. Động học ngược robot:...................................................................................................... 25

2.1.3. Ý nghĩa của bài toán động học robot:.............................................................................. 29

2.2. Động lực học robot SCARA: ................................................................................................... 30

2.2.1. Động lực học thuận: .......................................................................................................... 31

2.2.2. Động lực học ngược:.......................................................................................................... 40

2.3.1. Khâu 0 (Khâu cố định): .................................................................................................... 41

2.3.2. Khâu 1 và khâu 2: ............................................................................................................. 43

GVHD: TS. Mạc Thị Thoa SVTH: Nguyễn Tiến Phát

2.3.3. Khâu 3: ............................................................................................................................... 47

3.1. Không gian làm việc:................................................................................................................ 49

Chương 4. Mô hình hóa các khâu của Robot.....................................................................................56

Tổng quan: ....................................................................................................................................... 56

4.1. Động cơ điện một chiều: .......................................................................................................... 57

4.2. Trục vít me đai ốc bi: ............................................................................................................... 59

4.3. Mô hình hóa khâu vít me đai ốc bi (Khâu 3): ........................................................................ 61

4.4. Mô hình hóa khâu 1 và 2: ........................................................................................................ 63

4.5. Xác định các thông số của động cơ: ........................................................................................ 64

Chương 5. Thiết kế hệ thống điều khiển ............................................................................................89

5.1. Các phương pháp điều khiển: ................................................................................................. 89

5.1.1. Phương pháp điều khiển momen: .................................................................................... 89

5.1.2. Phương pháp điều khiển PID: ........................................................................................ 100

5.2.1. Bộ điều khiển bám quỹ đạo: ........................................................................................... 108

GVHD: TS. Mạc Thị Thoa SVTH: Nguyễn Tiến Phát

Chương 6. Mô phỏng nguyên lý hoạt động (SIMMECHANICS) ..................................................111

Chương 7. Lựa chọn thiết bị và thiết kế mạch điều khiển ..............................................................112

7.1.Chọn các phần tử của hệ thống điều khiển: .......................................................................... 112

7.2. Xác định hàm điều khiển:...................................................................................................... 115

7.2.1. Điều khiển vị trí (Position): ............................................................................................ 116

7.2.2. Điều khiển tốc độ (Speed): .............................................................................................. 122

7.2.3. Điều khiển lực, momen (Torque): .................................................................................. 125

7.3. Xây dựng bản vẽ:.................................................................................................................... 130

Tài liệu tham khảo: ............................................................................................................................139

GVHD: TS. Mạc Thị Thoa SVTH: Nguyễn Tiến Phát

LỜI NÓI ĐẦU

Đồ án mà em được giao là “Thiết kế hệ thống điều khiển cho robot”.

Hà nội, ngày 13 tháng 9 năm 2021

Sv: Nguyễn Tiến Phát

GVHD: TS. Mạc Thị Thoa SVTH: Nguyễn Tiến Phát

BẢNG PHỤ LỤC

GVHD: TS. Mạc Thị Thoa SVTH: Nguyễn Tiến Phát

GVHD: TS. Mạc Thị Thoa SVTH: Nguyễn Tiến Phát

42 4.19 Tính toán hàm truyền khâu 3 78

GVHD: TS. Mạc Thị Thoa SVTH: Nguyễn Tiến Phát

65 5.10 Sai số biến khớp 1 97

GVHD: TS. Mạc Thị Thoa SVTH: Nguyễn Tiến Phát

87 7.1 Sơ đồ khối của hệ thống điều khiển 112

DANH SÁCH BẢNG

GVHD: TS. Mạc Thị Thoa SVTH: Nguyễn Tiến Phát