BỘ GIÁO DỤC VÀ ĐÀO TẠO

TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT

THÀNH PHỐ HỒ CHÍ MINH

ĐỒ ÁN TỐT NGHIỆP
NGÀNH CÔNG NGHỆ KỸ THUẬT CƠ ĐIỆN TỬ

NHẬN DẠNG VÀ ĐIỀU KHIỂN QUỸ ĐẠO CHUYỂN ĐỌNG

SCARA ROBOT

GVHD: Th.S. VÕ LÂM CHƯƠNG

SVTH: QUẢNG ĐẠI MINH VƯỢNG
NGUYỄN ĐÌNH THÔNG
NGUYỄN QUOC ĐẠT

SKL011 005

Tp. Hồ Chí Minh, tháng 7/2023

 BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH
__________________________
KHOA CƠ KHÍ CHẾ TẠO MÁY

ĐỒ ÁN TỐT NGHIỆP
Đề tài: “NHẬN DẠNG VÀ ĐIỀU KHIỂN QUỸ ĐẠO CHUYỂN ĐỘNG
SCARA ROBOT”

Sinh viên thực hiện:

1. QUẢNG ĐẠI MINH VƯỢNG MSSV: 19146068 Lớp: 191462C
2. NGUYỄN ĐÌNH THÔNG MSSV: 19146398 Lớp: 191462C
3. NGUYỄN QUỐC ĐẠT MSSV: 19146316 Lớp: 191462C
Giảng viên hướng dẫn: Th.S VÕ LÂM CHƯƠNG
Khóa: 2019 - 2023

Tp. Hồ Chí Minh, tháng 07/2023

TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP. HCM CỘNG HOÀ XÃ HỘI CHỦ NGHĨA VIỆT NAM
KHOA CƠ KHÍ CHẾ TẠO MÁY Độc lập - Tự do – Hạnh phúc

Bộ môn Cơ điện tử

NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP

Học kỳ II/ năm học 2022-2023
Giảng viên hướng dẫn: ThS. Võ Lâm Chương
Sinh viên thực hiện:
1. Quảng Đại Minh Vượng MSSV: 19146068 Điện thoại: 0979087906
2. Nguyễn Đình Thông MSSV: 19146398 Điện thoại: 0372372273
3. Nguyễn Quốc Đạt MSSV: 19146316 Điện thoại: 0333060185
1. Mã số đề tài: 22223DT182
Tên đề tài: Nhận dạng và điều khiển quỹ đạo chuyển động Scara robot
2. Các số liệu, tài liệu ban đầu:
- Mô hình cơ khí từ đồ án Cơ điện tử
3. Nội dung chính của đồ án:
- Nhận dạng thông số cơ cấu robot SCARA
- Xây dựng phương pháp điều khiển chuyển động cho robot
- Mô phỏng Matlab và thực thi trên mô hình robot thực tế sử dụng PLC và module điều
khiển chuyển động.
4. Các sản phẩm dự kiến:
- Mô hình Robot SCARA thực tế
- Bộ điều khiển robot: Tủ điện PLC
- Tập bản vẽ thiết kế
- Báo cáo đồ án tốt nghiệp
5. Ngày giao đồ án: 15/03/2023
6. Ngày nộp đồ án: 15/07/2023
7. Ngôn ngữ trình bày: Bản báo cáo: Tiếng Anh  Tiếng Việt 
Trình bày bảo vệ: Tiếng Anh  Tiếng Việt 

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

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

 Được phép bảo vệ ……………………………………….

(GVHD ký, ghi rõ họ tên)

i
 LỜI CAM KẾT
- Tên đề tài: Nhận dạng và điều khiển quỹ đạo chuyển động Scara Robot
- GVHD: ThS. Võ Lâm Chương
- Họ tên sinh viên: Quảng Đại Minh Vượng
- MSSV: 19146068 Lớp: 191462C
- Địa chỉ sinh viên: TP. Thủ Đức, TP. HCM
- Số điện thoại liên lạc: 0979087906 Email: 19146068@student.hcmute.edu.vn
- Ngày nộp khóa luận tốt nghiệp (ĐATN): /07/2023
- Lời cam kết: “Tôi xin cam đoan khóa luận tốt nghiệp (ĐATN) này là công trình do
chính tôi nghiên cứu và thực hiện. Tôi không sao chép từ bất kỳ một bài viết nào đã
được công bố mà không được trích dẫn nguồn gốc. Nếu có bất kỳ một sự vi phạm nào,
tôi xin chịu hoàn toàn trách nhiệm.”

Tp. Hồ Chí Minh, ngày tháng 07 năm 2023

Ký tên

ii
 LỜI CẢM ƠN
Nhóm thực hiện xin chân thành cảm ơn đến quý giảng viên Trường Đại học Sư phạm
Kỹ Thuật và Khoa Cơ khí Chế tạo máy đã tạo điều kiện để nhóm có cơ hội học tập và nghiên
cứu đề tài này. Đặc biệt xin cảm ơn thầy Võ Lâm Chương cùng các thầy cô trong Bộ môn Cơ
điện tử và các anh chị sinh viên trong khoa Cơ khí Chế tạo máy đã tận tình hướng dẫn, giải
đáp thắc mắc và giúp đỡ nhóm trong quá trình hoàn thành đồ án. Mặc dù đã cố gắng rất nhiều
nhưng với kiến thức và khả năng vận dụng vào tính toán còn hạn chế nên không thể tránh
khỏi thiếu sót trong báo cáo. Nhóm thực hiện rất mong nhận được những sự đánh giá, nhận
xét cũng như lời góp ý quý báu từ quý thầy cô để có thể hoàn thiện đồ án này một cách tốt
nhất. Chúng em xin chân thành cảm ơn!

Nhóm sinh viên thực hiện.

iii
 TÓM TẮT ĐỒ ÁN
SCARA là robot phổ biến trong công nghiệp, được ứng dụng chủ đạo trong các hệ
thống dây chuyền tự động thực hiện các tác vụ như lắp ráp linh kiện điện tử, đóng gói sản
phẩm… Đặc tính của robot là có độ chính xác và hoạt động mang tính ổn định cao. Quỹ đạo
mà SCARA thực hiện thường là quỹ đạo điểm - điểm vì tính đặc thù chuyên thực hiện các
công việc liên quan đến việc di chuyển đến các vị trí cố định đã thiết lập sẵn. Tuy nhiên trong
đề tài này, quỹ đạo SCARA robot sẽ được phát triển để có thể di chuyển theo các chuyển
động đa dạng hơn. Đề tài thực hiện việc xây dựng hệ thống điều khiển quỹ đạo chuyển động
robot SCARA theo các quỹ đạo cho trước đồng thời đề xuất phương pháp nhận dạng thông
số cho robot.
Robot SCARA gồm 4 bậc tự do được nghiên cứu, xây dựng các phương trình động
học, động lực học theo phương pháp Lagrange-Euler, thiết kế quỹ đạo đường thẳng và đường
cung tròn ứng dụng điều khiển robot chạy theo quỹ đạo được tạo thành từ hai quỹ đạo cơ bản
này. Đồng thời tiến hành tính toán quỹ đạo khớp tối ưu sử dụng chuỗi Fourier và Optimization
Toolbox trong Matlab và sử dụng phương pháp bình phương cực tiểu để nhận dạng thông số
robot. Hệ thống điều khiển được thiết kế và thi công sử dụng bộ điều khiển PLC dòng Q với
các module hỗ trợ tương ứng để điều khiển động cơ AC servo kết hợp với driver Mitsubishi
MR-J3 -10B. Thiết kế giao diện người dùng trên PC, giao tiếp với PLC thông qua phần
mềm MX Component.
Đề tài hoàn thiện phần cơ khí cho robot thiết kế chế tạo đồ gá động cơ, đầu công
tác và bàn vẽ robot. Hoàn thiện hệ thống điều khiển vị trí đảm bảo được độ chính xác,
điều khiển robot chạy bám đúng theo quỹ đạo đường thẳng, đường cung tròn và các
đường tạo từ hai thành phần này. Đồng thời chứng minh được phương pháp bình
phương cực tiểu phù hợp để nhận dạng thông số.
Nhóm sinh viên thực hiện.

iv
 ABSTRACT
PARAMETER IDENTIFICATION AND CONTROL OF SCARA ROBOT'S
MOTION TRAJECTORY
SCARA, which stands for Selective Compliance Assembly Robot Arm, is a popular
type of robot widely used in the industry. It is primarily applied in automated assembly lines
to perform tasks such as assembling electronic components and packaging products. The
characteristic features of SCARA robots are their high precision and stable operation. They
typically follow point-to-point trajectories because they are specialized in tasks that involve
moving to predefined fixed positions. However, in this project, the trajectory of the SCARA
robot will be developed to allow for more diverse movements. The project aims to build a
control system for the SCARA robot's motion trajectory along predefined paths and propose
a method to identify parameters for the robot.
The SCARA robot being studied has four degrees of freedom. The project involves
deriving kinematic and dynamic equations using the Lagrange-Euler method, designing
straight-line and circular trajectories, and controlling the robot to follow trajectories formed
from these basic paths. Additionally, trajectory optimization is performed using the Fourier
series and Optimization Toolbox in Matlab, and a least squares method is used to identify the
robot's parameters. The control system is designed and implemented using a Q-series PLC
(Programmable Logic Controller) with corresponding support modules to control AC servo
motors in conjunction with the Mitsubishi MR-J3-10B driver. The user interface is designed
on a PC, communicating with the PLC through MX Component software.
The mechanical part of the project involves completing the design of the robot,
including motor brackets, end effectors, and the drawing table. The position control system is
perfected to ensure accuracy, and the robot is controlled to precisely follow straight-line,
circular, and other paths created from these two basic trajectories. Additionally, the project
demonstrates the suitability of the least squares method for parameter identification.

v
 MỤC LỤC
NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP......................................................................................... i
LỜI CAM KẾT ........................................................................................................................ ii
LỜI CẢM ƠN .........................................................................................................................iii
TÓM TẮT ĐỒ ÁN ................................................................................................................. iv
MỤC LỤC .............................................................................................................................. vi
DANH MỤC BẢNG BIỂU .................................................................................................... ix
DANH MỤC SƠ ĐỒ, HÌNH VẼ............................................................................................. x
DANH MỤC TỪ VIẾT TẮT ............................................................................................... xiii
CHƯƠNG 1. GIỚI THIỆU ...................................................................................................... 1
1.1 Tính cấp thiết của đề tài ............................................................................................... 1
1.2 Ý nghĩa khoa học và thực tiễn của đề tài ..................................................................... 1
1.3 Mục tiêu nghiên cứu của đề tài .................................................................................... 2
1.4 Đối tượng và phạm vi nghiên cứu ............................................................................... 2
1.4.1 Đối tượng nghiên cứu .................................................................................................. 2
1.4.2 Phạm vi nghiên cứu ..................................................................................................... 2
1.5 Phương pháp nghiên cứu ............................................................................................. 2
1.5.1 Cơ sở phương pháp luận .............................................................................................. 2
1.5.2 Các phương pháp nghiên cứu cụ thể ............................................................................ 2
1.6 Kết cấu của đồ án tốt nghiệp ........................................................................................ 3
CHƯƠNG 2. TỔNG QUAN NGHIÊN CỨU ĐỀ TÀI ........................................................... 4
2.1 Giới thiệu ..................................................................................................................... 4
2.1.1 Robot công nghiệp ....................................................................................................... 4
2.1.2 Robot SCARA và ứng dụng ........................................................................................ 4
2.2 Đặc tính của hệ thống .................................................................................................. 5
2.3 Kết cấu hệ thống .......................................................................................................... 6
2.3.1 Cơ cấu truyền động ...................................................................................................... 7
2.3.2 Thông số hệ dẫn động .................................................................................................. 9
2.3.3 Các thiết bị khác......................................................................................................... 10
2.4 Các nghiên cứu liên quan đến đề tài .......................................................................... 10
2.4.1 Các nghiên cứu ngoài nước ....................................................................................... 10
2.4.2 Các nghiên cứu trong nước ........................................................................................ 14
2.4.3 Các tồn tại của hệ thống ............................................................................................. 14
CHƯƠNG 3. CƠ SỞ LÝ THUYẾT ...................................................................................... 15
3.1 Các kiến thức liên quan về robot ............................................................................... 15
3.1.1 Bậc tự do của robot .................................................................................................... 15
3.1.2 Hệ tọa độ .................................................................................................................... 15
vi
3.1.3 Không gian công tác .................................................................................................. 15
3.2 Động học .................................................................................................................... 16
3.3 Động lực học .............................................................................................................. 17
3.3.1 Giới thiệu ................................................................................................................... 17
3.3.2 Phương trình Lagrange -Euler ................................................................................... 17
3.4 Chuỗi Fourier ............................................................................................................. 20
3.5 Phương pháp Least Squares ....................................................................................... 21
CHƯƠNG 4. BÀI TOÁN ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC .......................................... 22
4.1 Động học robot .......................................................................................................... 22
4.1.1 Động học thuận .......................................................................................................... 22
4.1.2 Động học nghịch ........................................................................................................ 24
4.2 Động lực học robot .................................................................................................... 26
4.2.1 Xác định thông số động học ...................................................................................... 26
4.2.2 Jacobian và vận tốc đầu công tác ............................................................................... 28
4.2.3 Phương trình Lagrange - Euler .................................................................................. 30
CHƯƠNG 5. NHẬN DẠNG VÀ QUY HOẠCH QUỸ ĐẠO CHUYỂN ĐỘNG ROBOT . 33
5.1 Yêu cầu của đề tài ...................................................................................................... 33
5.2 Phương hướng và giải pháp thực hiện ....................................................................... 33
5.2.1 Phương án 1 ............................................................................................................... 33
5.2.2 Phương án 2 ............................................................................................................... 33
5.3 Lựa chọn phương án .................................................................................................. 34
5.4 Trình tự công việc tiến hành ...................................................................................... 34
5.5 Nhận dạng thông số robot .......................................................................................... 35
5.5.1 Giới thiệu ................................................................................................................... 35
5.5.2 Quỹ đạo khớp tối ưu .................................................................................................. 36
5.5.3 Mô hình nhận dạng thông số...................................................................................... 38
5.5.4 Bộ dự báo hồi quy tuyến tính..................................................................................... 39
5.5.5 Phương pháp Least Squares ....................................................................................... 40
5.6 Quy hoạch quỹ đạo chuyển động robot ..................................................................... 41
5.6.1 Quy hoạch quỹ đạo theo đường thẳng ....................................................................... 41
5.6.2 Thiết kế quỹ đạo theo đường cung tròn ..................................................................... 42
CHƯƠNG 6. TÍNH TOÁN THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN ROBOT .................... 46
6.1 Sơ đồ hệ thống điều khiển ......................................................................................... 46
6.2 Các thiết bị sử dụng trong hệ thống ........................................................................... 48
6.2.1 Động cơ AC Servo ..................................................................................................... 48
6.2.2 Driver Servo và hệ thống mạng SSCNET III ............................................................ 49
6.2.3 Bộ điều khiển lập trình PLC Mitsubishi Q series ...................................................... 50

vii
6.2.4 Các module hỗ trợ ...................................................................................................... 50
6.3 Hệ thống trạm PLC .................................................................................................... 53
6.3.1 Các thiết bị liên quan ................................................................................................. 53
6.4 Cài đặt thông số cho module QD75MH4 .................................................................. 54
6.4.1 Cài đặt tham số cho bộ điều khiển Servo................................................................... 54
6.4.2 Cài đặt tham số của bộ truyền động điện tử (Electronic gear) .................................. 54
6.4.3 Giới hạn tốc độ (Speed limit value) ........................................................................... 57
6.4.4 Các tham số khác ....................................................................................................... 58
6.5 Lưu đồ điều khiển ...................................................................................................... 59
6.6 Thiết kế giao diện người dùng ................................................................................... 59
6.6.1 Cấu hình giao thức kết nối PLC và Visual Studio ..................................................... 59
6.6.2 Kết nối SQL Server với Visual Studio ...................................................................... 61
6.6.3 Giao diện ứng dụng điều khiển robot SCARA .......................................................... 62
6.6.4 Màn hình chính .......................................................................................................... 62
6.6.5 Màn hình chế độ JOG ................................................................................................ 63
6.6.6 Màn hình chế độ tự động ........................................................................................... 64
6.6.7 Màn hình quản lý người dùng .................................................................................... 66
CHƯƠNG 7. THỰC NGHIỆM – ĐÁNH GIÁ ..................................................................... 68
7.1 Chạy thực nghiệm quỹ đạo ........................................................................................ 68
7.1.1 Quỹ đạo đường thẳng ................................................................................................. 68
7.1.2 Quỹ đạo cung tròn ...................................................................................................... 71
7.1.3 Quỹ đạo kết hợp ......................................................................................................... 75
7.2 Nhận xét – đánh giá kết quả....................................................................................... 78
7.2.1 Kết quả đạt được ........................................................................................................ 78
7.2.2 Nhận xét ..................................................................................................................... 78
KẾT LUẬN – ĐỀ NGHỊ ....................................................................................................... 79
TÀI LIỆU THAM KHẢO ..................................................................................................... 80
PHỤ LỤC..................................................................................................................................I

viii
 DANH MỤC BẢNG BIỂU
Bảng 2. 1 Thông số kỹ thuật robot SCARA ............................................................................ 6
Bảng 4. 1 Bảng thông số Denavit-Hartenberg ...................................................................... 22
Bảng 5. 1 Hệ số Fourier a ij ................................................................................................... 37
Bảng 5. 2 Hệ số Fourier b ij ................................................................................................... 38
Bảng 5. 3 Hệ số q j 0 ............................................................................................................... 38
Bảng 5. 4 Thông số nhận dạng .............................................................................................. 40
Bảng 6. 1 Thông số kỹ thuật động cơ AC Servo ................................................................... 48
Bảng 6. 2 Thông số driver MR-J3-10B ................................................................................. 49
Bảng 6. 3 Thông số kỹ thuật Module Q00UCPU.................................................................. 50
Bảng 6. 4 Thông số kỹ thuật Module Q00UCPU.................................................................. 51
Bảng 6. 5 Thông số kỹ thuật module Digital Input QX42 .................................................... 51
Bảng 6. 6 Thông số kỹ thuật Module Digital Output QY42P ............................................... 52
Bảng 6. 7 Thông số kỹ thuật Module QJ71E71 – 100 .......................................................... 52
Bảng 6. 8 Thông số kỹ thuật bộ lọc nguồn 1 pha .................................................................. 53
Bảng 6. 9 Giới hạn hành trình của trục 1 và 2 ....................................................................... 58
Bảng 6. 10 Tham số điểm gốc (OPR basic parameter) ......................................................... 58
Bảng 7. 1 Sai số vị trí quỹ đạo đường thẳng ......................................................................... 70
Bảng 7. 2 Sai số vị trí quỹ đạo cung tròn .............................................................................. 74

ix
 DANH MỤC SƠ ĐỒ, HÌNH VẼ
Hình 2. 1 Hệ thống robot lắp ráp trong nhà máy của Vinfast [20] ......................................... 4
Hình 2. 2 Sắp xếp Pin và kiểm tra mối hàn sử dụng robot SCARA [19] [10] ........................ 5
Hình 2. 3 Đóng gói bánh kẹo và sản xuất khăn ướt sử dụng robot SCARA .......................... 5
Hình 2. 4 Mô hình robot SCARA ........................................................................................... 6
Hình 2. 5 Các thành phần của hộp số Harmonic ..................................................................... 7
Hình 2. 6 Cấu tạo của Vít me bi .............................................................................................. 8
Hình 2. 7 Bộ truyền đai răng ................................................................................................... 8
Hình 2. 8 Bộ truyền đai trục 4 ................................................................................................. 9
Hình 2. 9 Bàn vẽ robot .......................................................................................................... 10
Hình 2. 10 Đầu kẹp bút ......................................................................................................... 10
Hình 3. 1 Quy tắc bàn tay phải .............................................................................................. 15
Hình 3. 2 Mô hình hóa các khâu và khớp theo quy tắc D-H [5] ........................................... 16
Hình 3. 3 Mô tả hệ trục tọa độ khớp xoay robot [5] ............................................................. 18
Hình 4. 1 Hệ trục tọa độ gắn thực tế lên Robot SCARA ...................................................... 22
Hình 4. 2 Phạm vi hoạt động cánh tay robot ........................................................................ 24
Hình 4. 3 Khu vực làm việc của robot SCARA trong không gian ....................................... 24
Hình 5. 1 Sơ đồ nhận dạng thông số robot ............................................................................ 35
Hình 5. 2 Đồ thị biểu diễn giá trị góc quay và vận tốc của khớp 1 và khớp 2 ...................... 37
Hình 5. 3 Mô hình nhận dạng thông số robot ....................................................................... 38
Hình 5. 4 Đồ thị mô-men thực tế và mô-men nhận dạng khớp 1 và khớp 2......................... 40
Hình 5. 5 Sai số giữa mô-men thực tế và mô-men nhận dạng khớp 1 và khớp 2 ................. 41
Hình 5. 6 Quy hoạch quỹ đạo theo đường thẳng .................................................................. 41
Hình 5. 7 Đường tròn biểu diễn các giá trị góc quay ............................................................ 44
Hình 6. 1 Sơ đồ kết nối QD75MH4 – Driver MR-J3B – AC Servo ..................................... 46
Hình 6. 2 Sơ đồ kết nối driver MR-J3-10B với động cơ AC Servo và PC [11] ................... 47
Hình 6. 3 Tủ điện PLC thiết kế và thực tế ............................................................................ 47
Hình 6. 4 Động cơ AC Servo [13] ........................................................................................ 48
Hình 6. 5 Sơ đồ nguyên lý hệ thống AC Servo ..................................................................... 49
Hình 6. 6 Driver Servo MR-J3-10B [12] .............................................................................. 49
Hình 6. 7 Module Q00UCPU [14] ........................................................................................ 50
Hình 6. 8 Module điều khiển vị trí QD75MH4 [12] ............................................................. 50
Hình 6. 9 Sơ đồ chân module Digital Input QX42 [15][17] ................................................. 51
Hình 6. 10 Sơ đồ chân module Digital Output QY42P [16][17] .......................................... 51
Hình 6. 11 Module giao tiếp Ethernet QJ71E71 – 100 [17] ................................................. 52
Hình 6. 12 Hệ thống trạm PLC sử dụng trong đề tài ............................................................ 53
Hình 6. 13 Cấu tạo và sơ đồ nguyên lý bộ lọc nguồn 1 pha [22] .......................................... 53
x
Hình 6. 14 Màn hình thiết lập tham số module QD75MH4 ................................................. 54
Hình 6. 15 Sơ đồ nguyên lý bộ truyền động điện tử [11] ..................................................... 54
Hình 6. 16 Cơ cấu truyền động trục 1 [11] ........................................................................... 55
Hình 6. 17 Cơ cấu truyền động trục 2 [11] ........................................................................... 55
Hình 6. 18 Cơ cấu truyền động trục 3 ................................................................................... 56
Hình 6. 19 Cơ cấu truyền động trục 4 ................................................................................... 57
Hình 6. 20 Giới hạn hành trình của trục 1 và 2 ..................................................................... 58
Hình 6. 21 Lưu đồ điều khiển chương trình PLC ................................................................. 59
Hình 6. 22 Giao diện phần mềm MX Component ................................................................ 60
Hình 6. 23 Chọn thông số trạm muốn cấu hình .................................................................... 60
Hình 6. 24 Chọn giao thức kết nối với máy tính (PC side I/F) ............................................. 60
Hình 6. 25 Chọn dòng PLC được kết nối (PLC side I/F)...................................................... 60
Hình 6. 26 Chọn chức năng của PLC trong mạng ................................................................ 60
Hình 6. 27 Hoàn thành thiết lập truyền thông ....................................................................... 60
Hình 6. 28 Sơ đồ các bước thêm thư viện ActUtlType vào Visual C#.NET ........................ 61
Hình 6. 29 Tạo mới cơ sở dữ liệu.......................................................................................... 61
Hình 6. 30 Sơ đồ các bước thiết lập truyền thông giữa Visual C#. NETvà cơ sở dữ liệu SQL
Server ..................................................................................................................................... 61
Hình 6. 31 Giao diện đăng nhập............................................................................................ 62
Hình 6. 32 Giao diện ứng dụng điều khiển robot SCARA ................................................... 62
Hình 6. 33 Giao diện chế độ chạy theo trục (JOG Mode) .................................................... 63
Hình 6. 34 : Chế độ chạy tự động ......................................................................................... 64
Hình 6. 35 Cửa sổ nhập giá trị đầu vào quỹ đạo đường thẳng .............................................. 64
Hình 6. 36 Cửa sổ nhập giá trị đầu vào quỹ đạo đường cung tròn ....................................... 65
Hình 6. 37 Cửa sổ nhập giá trị tọa độ đầu vào ...................................................................... 65
Hình 6. 38 Cửa sổ hiển thị mã lỗi các trục ............................................................................ 66
Hình 6. 39 Cửa sổ hiển thị chi tiết lỗi ................................................................................... 66
Hình 6. 40 Quản lý người dùng ............................................................................................. 66
Hình 6. 41 Giao diện cửa sổ thêm, chỉnh sửa tài khoản........................................................ 67
Hình 7. 1 Quỹ đạo đường thẳng thực tế và mô phỏng .......................................................... 68
Hình 7. 2 Đồ thị góc quay của khớp 1 và khớp 2 thực tế và mô phỏng................................ 69
Hình 7. 3 Đồ thị vận tốc của khớp 1 và khớp 2 thực tế và mô phỏng .................................. 69
Hình 7. 4 Đồ thị vận tốc dài khâu công tác robot quỹ đạo đường thẳng .............................. 70
Hình 7. 5 Quỹ đạo cung tròn thực tế và mô phỏng ............................................................... 72
Hình 7. 6 Đồ thị góc quay khớp 1 và khớp 2 thực tế và mô phỏng ...................................... 73
Hình 7. 7 Đồ thị vận tốc khớp 1 và khớp 2 thực tế và mô phỏng ......................................... 73
Hình 7. 8 Đồ thị vận tốc dài khâu công tác robot quỹ đạo cung tròn ................................... 74

xi
Hình 7. 9 Tọa độ các điểm biểu diễn quỹ đạo UTE .............................................................. 75
Hình 7. 10 Quỹ đạo UTE ...................................................................................................... 75
Hình 7. 11 Đồ thị góc quay và vận tốc của khớp 1 và khớp 2 quỹ đạo UTE ....................... 76
Hình 7. 12 Tọa độ các điểm biểu diễn quỹ đạo bông hoa ..................................................... 76
Hình 7. 13 Quỹ đạo bông hoa ............................................................................................... 77
Hình 7. 14 Đồ thị góc quay và vận tốc khớp 1 và khớp 2 quỹ đạo bông hoa ....................... 77
Hình 7. 15 Đồ thị vận tốc dài khâu công tác robot quỹ đạo bông hoa .................................. 77

xii
 DANH MỤC TỪ VIẾT TẮT
SCARA Selective Compliance Assembly Robot Arm.
PLC Programmable Logic Controller.
SSCNET III Servo System Controller NETWORK III.
SQL Structured Query Language.
OPR Home Position Return.
OP Home Position.
PC Personal Computer.
ISO International Organization for Standardization.
PSO Particle Swarm Optimization.
PID Proportional–Integral–Derivative controller.

xiii
 CHƯƠNG 1. GIỚI THIỆU

1.1 Tính cấp thiết của đề tài

Robot là một phần không thể thiếu trong dây chuyền công nghiệp hiện đại, sự ra đời
của robot là một bước tiến lớn thúc đẩy nền kinh tế và góp phần thay đổi lực lượng lao động
truyền thống sang hệ thống tự động hóa. Ưu điểm của robot nằm ở sự chính xác, tính bền bỉ
và tạo ra được hiệu suất cao trong công việc.
Hiện nay các tác vụ mà robot thực hiện rất đa dạng và hoạt động trong vùng không
gian khác nhau, vì thế tinh linh động và hệ thống điều khiển chính xác được coi là yếu tố quan
trọng để nâng cao tính ứng dụng của robot. Một trong những phương pháp rất được ưa chuộng
là phương pháp điều khiển dựa trên mô hình. Robot thực tế được tiến hành xác định các thông
số liên quan và chuyển hóa thành mô hình toán học. Mô hình này sau đó được ứng dụng để
xây dựng thuật toán và phục vụ cho việc phân tích, đánh giá robot trước khi thực hiện thử
nghiệm trên robot thực.
Tuy nhiên trong thực tế, việc xác định thông số robot gặp nhiều khó khăn do thông tin
bị sai lệch hay không được cung cấp đầy đủ từ nhà sản xuất, từ đó không đảm bảo được tính
chính xác trong quá trình thực hiện. Việc phát triển các phương pháp nhận dạng thông số là
một giải pháp tối ưu để giải quyết vấn đề này. Phương pháp nhận dạng tiến hành chạy thử
nghiệm robot để tính toán, tìm ra các thông số quan trọng ảnh hưởng đến quá trình hoạt động.
Việc này giúp loại bỏ được các yếu tố không cần thiết hay không ảnh hướng quá nhiều đến
robot giảm bớt độ phức tạp và giúp dễ dàng hơn trong tính toán. Các thông số sau đó sẽ được
tích hợp để hình thành nên mô hình toán học, từ đó có thể ứng dụng vào việc xây dựng hệ
thống điều khiển nhằm đánh giá sâu hơn và kiểm soát được tốt hơn hiệu suất hoạt động của
robot. Đồng thời hỗ trợ trong quá trình xây dựng thuật toán điều khiển quỹ đạo để robot có
thể di chuyển thực hiện theo đúng các tác vụ mong muốn, linh hoạt trong chuyển động, có
thể di chuyển đến nhiều vị trí khác nhau trong không gian hoạt động với tốc độ và và độ chính
xác cao.
Trên cơ sở đó, đề tài “Nhận dạng và điều khiển quỹ đạo chuyển động Scara Robot”
được thực hiện nhằm áp dụng những kiến thức, kỹ thuật đã được học để xây dựng một mô
hình điều khiển cho robot có thể ứng dụng làm mô hình để thực hành trong các môn học liên
quan đến robot, điều khiển tự động trong tương lai.
1.2 Ý nghĩa khoa học và thực tiễn của đề tài
Đề tài “Nhận dạng và điều khiển quỹ đạo chuyển động Scara robot” được thực hiện
mang ý nghĩa khoa học và tính thực tiễn, cụ thể:
Ý nghĩa khoa học:
- Ứng dụng được các phương pháp nhận dạng thông số robot: việc áp dụng phương pháp
nhận dạng thông số robot giúp hiểu rõ hơn về robot, cũng như tạo tiền đề để xây dựng

1
 nên hệ thống đánh giá, phân tích sâu hơn các kết quả và trạng thái của robot khi được
vận hành.
- Mô phỏng thử nghiệm robot trên Matlab: Đề tài sử dụng phần mềm Matlab và các
công cụ hỗ trợ để phục vụ cho việc mô phỏng, xây dựng thuật toán cho robot. Việc mô
phỏng giúp tiết kiệm thời gian hơn trong quá trình tính toán, hoàn thiện mô hình.
- Xây dựng được hệ thống điều khiển robot: hệ thống điều khiển robot mang tính linh
họa và có độ chính xác cao được thực hiện giúp tăng được độ chính xác, an toàn cũng
như hiệu suất khi hoạt động.
Ý nghĩa thực tiễn:
- Tăng tính linh hoạt trong điều khiển robot: Robot được xây dựng một giao diện người
dùng, giúp người dùng dễ dàng hơn trong việc sử dụng, điều khiển robot.
- Tăng độ chính xác trong điều khiển robot: hệ thống giúp thử nghiệm được robot trong
các chế độ khác nhau từ đó dễ dàng điều chỉnh phục vụ cho các tác vụ và hoạt động
được yêu cầu.
1.3 Mục tiêu nghiên cứu của đề tài
- Xây dựng phương trình động học và động lực học cho robot.
- Đề xuất phương pháp nhận dạng thông số động học.
- Thiết kế và thi công tủ điện điều khiển sử dụng PLC và module điều khiển vị trí.
- Quy hoạch quỹ đạo chuyển động của robot.
- Mô phỏng trong Matlab để kiểm chứng các phương pháp đã nghiên cứu
1.4 Đối tượng và phạm vi nghiên cứu
1.4.1 Đối tượng nghiên cứu
- Robot SCARA 4 bậc tự do
- Các tài liệu liên quan về robot.
1.4.2 Phạm vi nghiên cứu
- Nghiên cứu thiết kế hệ thống điều khiển robot SCARA bằng bộ điều khiển PLC dòng
Q và xây dựng giao diện người dùng trên PC, giao tiếp với PLC thông qua phần mềm
MX Component.
1.5 Phương pháp nghiên cứu
1.5.1 Cơ sở phương pháp luận
- Phương pháp điều khiển robot dựa trên mô hình toán học (động học và động lực học):
động học và động lực học phục vụ cho việc điều khiển và phân tích hoạt động của tay
máy robot.
- Phương pháp kiểm chứng và đánh giá: kiểm chứng kết quả robot chạy thực tế so với
tính toán.
1.5.2 Các phương pháp nghiên cứu cụ thể
- Nghiên cứu mô hình robot thực tế, đánh giá sơ bộ về khả năng vận hành của robot.
2
 - Nghiên cứu lý thuyết về điều khiển robot công nghiệp
- Nghiên cứu phương pháp xây dựng mô hình robot giữa Matlab và SolidWorks.
- Tìm hiểu về phương pháp nhận dạng thông số của robot.
- Nghiên cứu phương pháp đánh giá hiệu suất, độ chính xác của robot.
1.6 Kết cấu của đồ án tốt nghiệp
Đồ án tốt nghiệp gồm 7 chương, trong đó:
- Chương 2 trình bày tổng quan các vấn các khái niệm, định nghĩa, kiến thức liên quan
đến đề tài nghiên cứu. Nêu ra các nghiên cứu trong và ngoài nước có liên quan đến đề
tài và xác định nhiệm vụ thực hiện trong đề tài.
- Chương 3 trình bày cơ sở lý thuyết, các phương pháp, kiến thức cần thiết để thực hiện
đề tài.
- Chương 4 xây dựng phương trình động học và động lực học robot để làm cơ sở tính
toán cho phần nhận dạng và quy hoạch quỹ đạo cho robot.
- Chương 5 phân tích yêu cầu từ đó đưa ra các phương án, giải pháp để lựa chọn và xác
định các công việc cần triển khai để hoàn thành đề tài, đồng thời tiến hành nhận dạng
thông số động học. Trong chương này cũng trình bày cách quy hoạch quỹ đạo cho
robot.
- Chương 6 nghiên cứu, thiết kế hệ thống điều khiển robot, trình bày sơ đồ điện, hệ thống
PLC, đồng thời xây dựng giao diện cho người dùng.
- Chương 7 tiến hành chạy thực nghiệm quỹ đạo cho robot, thu thập dữ liệu và đưa ra
nhận xét, đánh giá kết quả đã đạt được.

3
 CHƯƠNG 2. TỔNG QUAN NGHIÊN CỨU ĐỀ TÀI
2.1 Giới thiệu
2.1.1 Robot công nghiệp
Robot là một thành phần không thể thiếu trong ngành công nghiệp hiện nay. Nhờ vào
khả năng linh hoạt, đa dạng trong việc thực hiện các nhiệm vụ khác nhau. robot đem lại nguồn
lợi nhuận không hề nhỏ, đồng thời cũng giúp ích cho quá trình phát triển sản xuất, giảm thiểu
tai nạn lao động.
Theo tiêu chuẩn ISO [21] “Robot công nghiệp là một loại máy móc được điều khiển
tự động và lập trình sẵn, sử dụng vào nhiều mục đích khác nhau, có khả năng vận động nhiều
hơn ba trục và có thể cố định hoặc di động theo những ứng dụng của nó trong công nghiệp tự
động”. Robot công nghiệp có khả năng linh hoạt cao nhờ vào số bậc tự do của chúng. Để hoàn
thành nhiều tác vụ khác nhau robot thường được gắn các cơ cấu chấp hành cho phép thích
ứng nhanh và thao tác đơn giản khi nhiệm vụ sản xuất thay đổi.
Hiện nay robot đang dần thay thế con người trong nhiều công việc từ sản xuất đến an
ninh, thám hiểm hay nghiên cứu không gian. Có thể thấy việc ứng dụng robot là một bước
tiến cần thiết để giúp con người tiếp cận gần hơn với những bước tiến mới trong tương lai.

Hình 2. 1 Hệ thống robot lắp ráp trong nhà máy của Vinfast [20]
2.1.2 Robot SCARA và ứng dụng
SCARA là một loại robot công nghiệp thường được sử dụng trong công việc lắp ráp,
lựa chọn, sắp xếp, kiểm tra và đóng gói. Robot SCARA thường có 4 bậc tự do và có cấu trúc
khớp song song (parallel axis), cho phép robot hoạt động dễ dàng trong không gian hai chiều
(X, Y). Robot SCARA thường có tốc độ nhanh hơn các hệ thống robot Cartesian, và có kích
thước nhỏ gọn, điều này giúp việc lắp đặt dễ dàng hơn. SCARA được phát triển vào năm 1981
dưới sự hướng dẫn của giáo sư Hiroshi Makino, trường đại học Yamanashi, Nhật Bản.
Nhờ vào tốc độ và kích thước nhỏ gọn, SCARA robot được ứng dụng rộng rãi trong
nhiều ngành công nghiệp:

4
 - Ứng dụng trong ngành công nghiệp điện tử: Robot SCARA được sử dụng để lắp ráp
các linh kiện điện tử nhỏ và nhạy cảm, như mạch in, chip, LED... các công việc đòi
hỏi tốc độ và độ chính xác cao.

Hình 2. 2 Sắp xếp Pin và kiểm tra mối hàn sử dụng robot SCARA [19] [10]
- Ứng dụng trong việc đóng gói thực phẩm và sản xuất hàng tiêu dùng.

Hình 2. 3 Đóng gói bánh kẹo và sản xuất khăn ướt sử dụng robot SCARA
Bên cạnh đó, SCARA cũng có các điểm hạn chế:
- Không gian làm việc hạn chế: Robot SCARA chỉ có thể hoạt động hiệu quả trong
không gian hai chiều (XY), không phù hợp với các ứng dụng yêu cầu di chuyển theo
trục Z.
- Khả năng chịu tải hạn chế: Robot SCARA chỉ có thể gắp và di chuyển các vật liệu nhẹ
và nhỏ, không phù hợp với các ứng dụng yêu cầu chịu tải nặng hoặc kích thước lớn.
- Độ phức tạp cao: Robot SCARA yêu cầu một hệ thống điều khiển và lập trình phức
tạp, đòi hỏi kỹ thuật viên có kỹ năng cao để vận hành và bảo trì.
2.2 Đặc tính của hệ thống
SCARA là loại robot khá phổ biến trong công nghiệp vì tính linh hoạt và khả năng
thích ứng được với nhiều tác vụ khác nhau của chúng. Tùy vào từng hệ thống robot SCARA
khác nhau mà các đặc tính có thể thay đổi, các đặc tính cơ bản của robot SCARA được thực
hiện trong đề tài:
- Cấu trúc robot: Gồm 4 bậc tự do trong đó có ba khớp xoay và 1 khớp tịnh tiến. Robot
có khả năng di chuyển theo trục x, y và cả trục z.

5
 - Ứng dụng của robot: Robot SCARA thường được ứng dụng trong các công việc lắp
ráp đòi hỏi tính chính xác cao như gắp đặt linh kiện, đóng gói sản phẩm. Trong đề tài
robot được sử dụng để thực hiện chuyển động theo quỹ đạo cho trước.
- Phạm vi hoạt động nhỏ: Không gian hoạt động của robot khá hạn chế, đề tài áp dụng
robot vẽ trong một khoảng không gian cho trước 360 x 510 mm.
2.3 Kết cấu hệ thống

Hình 2. 4 Mô hình robot SCARA

Robot SCARA có 4 bậc tự do, gồm 3 khớp quay và một khớp tịnh tiến. Các khâu liên kết
nối tiếp với nhau thông qua các khớp.
Cơ cấu truyền động chính được sử dụng ở các khớp như sau:
- Khớp 1 và 2 sử dụng bộ truyền Harmonic gear có tỉ số truyền lần lượt là 80:1 và 50:1
- Khớp 3 sử dụng bộ truyền vít me và puly-đai răng: bước vít p = 20 mm, tỉ số truyền
54:20
- Khớp 4 sử dụng bộ truyền puly-đai răng có tỉ số truyền là 35:20
Bảng 2. 1 Thông số kỹ thuật robot SCARA
Phạm vi tối đa 455 [mm]
Tầm với của cánh tay Khâu 1 225 [mm]
Khâu 2 230 [mm]
Khớp 1 −19 /15 19 /15
Giới hạn các khớp
Khớp 2 −47 / 30  47 / 30
Khớp 3 (tịnh tiến theo trục z) 0-170 [mm]
Khớp 4 (quay quanh trục z) 2
Khớp 1 3.927 [rad/s]
Khớp 2 6.283 [rad/s]
Tốc độ tối đa
Khớp 3 600 [mm/s]
Khớp 4 6 [rad/s]

6
2.3.1 Cơ cấu truyền động
❖ Hộp số Harmonic

Hình 2. 5 Các thành phần của hộp số Harmonic

Hộp số Harmonic gồm 3 phần chính:
- Circular Spline (CS) là một bánh răng vành trong.
- Flexspline (FS) là một vành răng mềm, có tính đàn hồi.
- Wave Generator (WG): bộ tạo sóng có hình elip.
Nguyên lý hoạt động: Bộ tạo sóng được gắn chặt vào FS, khi bộ tạo sóng quay, các răng trên
bánh răng mềm ăn khớp trong với vành răng CS, tỉ số truyền giữa FS và CS là tỉ số truyền
của hộp số Harmonic.
Công thức tính tỷ số truyền:
nFS − nCS
u= (2.1)
nFS
Trong đó:
• nFS là số răng của FS.
• nCS là số răng của CS.
- Ưu điểm:
• Tỉ số truyền lớn.
• Không có độ rơ giữa các răng.
• Kích thước nhỏ gọn.
• Hiệu suất cao.
• Độ đồng trục cao.
• Khả năng truyền mô-men xoắn lớn.
- Nhược điểm:
• Khó chế tạo.
• Giá thành cao.

7
 ❖ Truyền động vít me bi
Vít me bi (ball screw) là một cơ cấu chuyển động tuyến tính, biến đổi chuyển động
quay thành chuyển động tịnh tiến. Vít me bi có thể chịu được lực dọc trục cao, với một ma
sát nội bộ nhỏ và được chế tạo với sai số nhỏ, do đó thích hợp cho các ứng dụng trong các
máy móc cần độ chính xác cao.

Hình 2. 6 Cấu tạo của Vít me bi

Cấu tạo và nguyên lý hoạt động: Cụm vít me bi bao gồm vít me (A), đai ốc (B) và bi
(C), mỗi vít đều có các rãnh xoắn phù hợp và các viên bi lăn giữa các rãnh này tạo sự tiếp xúc
giữa đai ốc và vít. Khi trục vít hoặc đai ốc quay, các viên bi bị lệch bởi bộ phận làm lệch
hướng vào hệ thống hồi lưu bi của đai ốc và chúng di chuyển qua hệ thống thu hồi bi (D) quay
trở lại đầu đối diện của đai ốc theo một đường liên tục. Sau đó các viên bi thoát ra khỏi hệ
thống hồi thu hồi bi vào rãnh ren của vít me và đai ốc liên tục, tuần hoàn.
Ưu điểm:
- Ít tổn thất do ma sát, hiệu suất cao.
- Không có độ rơ, đảm bảo sự chính xác cao.
- Ứng dụng được trong việc truyền động có tốc độ cao.
Nhược điểm:
- Khó gia công, chế tạo.
- Giá thành cao.
❖ Truyền động đai răng
Bộ truyền đai răng làm việc theo nguyên tắc ăn khớp là chính, ma sát là phụ. Do đó
khác với những bộ truyền đai khác, lực căng trên đai nhỏ. Cơ năng được truyền từ bánh đai
chủ động (1) sang bánh đai bị động (2) nhờ đai răng (3).

Hình 2. 7 Bộ truyền đai răng

8
Ưu điểm:
- Không gây ồn trong quá trình làm việc.
- Có thể truyền động với vận tốc lớn.
- Tỉ số truyền ổn định.
- Ít tạo nhiệt.
- Không cần căng đai. Giảm tải, tăng tuổi thọ.
Nhược điểm:
- Phải gắn thêm ít nhất một mặt bích cho bánh răng.
- Đai răng được tiêu chuẩn hóa, nên việc lựa chọn bánh đai và đai răng phải đồng bộ.
2.3.2 Thông số hệ dẫn động
Trong đề tài này, cơ cấu truyền động trong robot SCARA đã có sẵn. Bao gồm:
- Khâu 1 và khâu 2 chuyển động quay, sử dụng hộp giảm tốc Harmonic.
- Khâu 3 và khâu 4 sử dụng bộ truyền động vít me và puly-đai răng.

Hình 2. 8 Bộ truyền đai trục 4

9
2.3.3 Các thiết bị khác
Bàn vẽ và đầu kẹp bút được thiết kế và chế tạo để phù hợp với ứng dụng vẽ quỹ đạo
chuyển động của robot.

Hình 2. 9 Bàn vẽ robot

Hình 2. 10 Đầu kẹp bút

2.4 Các nghiên cứu liên quan đến đề tài
2.4.1 Các nghiên cứu ngoài nước
Trên thế giới, vấn đề về nhận dạng thông số robot đã được thực hiện từ lâu và có nhiều
bài báo được thực hiện.
- Bài báo “Modeling and identification for high-performance robot control: an RRR-
robotic arm case study” [3] được thực hiện bởi D. Kostic; Bram de Jager; M.
Steinbuch; R. Hensen vào năm 2004 giải thích về quy trình xây dựng nên phương trình
động học và động lực học cho robot phù hợp với thiết kế điều khiển robot. Quy trình
này gồm các bước sau:

10
 • Bước 1: Xác định các phương trình động học và động lực học cho robot và kiểm
tra tính chính xác của chúng. Các phương trình động học dựa trên các ma trận
biến đổi đồng nhất (homogeneous transformation matrices) và các phương trình
động lực học dựa trên nguyên lý Lagrange. Các tác giả cũng nghiên cứu các yếu
tố ảnh hưởng đến động lực học của robot như ma sát, mô-men quán tính, lực
Coriolis, trọng lực, lực giật (backlash) và lực giãn nở (compliance).
• Bước 2: Tiến hành thí nghiệm nhận dạng các thông số của mô hình. Các tác giả
thiết kế các quỹ đạo thí nghiệm để thu thập dữ liệu về vị trí, vận tốc, gia tốc và
mô-men lực của các khớp robot. Các thông số được ước lượng bằng phương
pháp hồi quy tuyến tính (linear regression) và phương pháp bình phương cực
tiểu (least squares). Đặc biệt, các tác giả tập trung vào việc mô hình hoá ma sát
và ước lượng các thông số ma sát.
• Bước 3: Kiểm tra tính chính xác của mô hình và các thông số đã ước lượng. Các
tác giả so sánh kết quả mô phỏng và kết quả thực nghiệm để đánh giá sai số của
mô hình. Các tác giả cũng kiểm tra tính nhất quán của các thông số khi thay đổi
điều kiện thí nghiệm.
• Bước 4: Nhận dạng các mô hình robot khác với mô hình đã xây dựng. Các tác
giả sử dụng bộ lọc Kalman (Kalman filter) để ước lượng các sai số không thể
quan sát được trong mô hình. Các sai số này có thể do các yếu tố như nhiễu, sai
số đo, ...
Quy trình này được áp dụng cho một cánh tay robot có ba khớp xoay (RRR-robotic
arm) với động cơ truyền động trực tiếp (direct-drive). Các tác giả cho rằng các mô hình
động học và động lực học có thể được sử dụng trong điều khiển phi tuyến tính dựa trên
mô hình (model-based nonlinear control).
- Bài báo “Dynamic Parameter Identification for a Manipulator with Joint Torque
Sensors Based on an Improved Experimental Design” [4] được thực hiện bởi Jia,
Jidong; Zhang, Minglu; Zang, Xizhe; Zhang, He; Zhao, Jie vào năm 2019. Bài báo này
nghiên cứu về cách nhận dạng các thông số động lực học của một robot có cảm biến
mô-men lực ở các khớp dựa trên phương pháp thiết kế cải thiện kết quả thực nghiệm.
Phương pháp này gồm các bước sau:
• Bước 1: Tối ưu hoá quỹ đạo thí nghiệm để tạo ra các tín hiệu kích thích cho robot.
Các tác giả dùng chuỗi Fourier để biểu diễn các quỹ đạo và tối ưu hoá các hệ số
của chuỗi Fourier theo một tiêu chí kết hợp nhiều tiêu chí (multi-criteria
embedded criterion). Tiêu chí này gồm có ba tiêu chí nhỏ là: (1) tối đa hoá độ
phân biệt của ma trận thông tin Fisher (Fisher information matrix); (2) tối thiểu
hoá độ lệch chuẩn của các thông số đã ước lượng; và (3) tối thiểu hoá độ lớn
của mô-men lực.
11
 • Bước 2: Tiến hành thí nghiệm trên một robot hai bậc tự do có cảm biến mô-men
lực ở các khớp và thu thập dữ liệu về vị trí, vận tốc, gia tốc và mô-men lực của
các khớp. Các tác giả cũng áp dụng các phương pháp loại bỏ nhiễu như phương
pháp loại bỏ nhiễu trung bình (mean filter) và phương pháp loại bỏ nhiễu
Savitzky–Golay (Savitzky–Golay filter) để xử lý dữ liệu.
• Bước 3: Dùng phương pháp bình phương trọng số (weighted least squares) để
ước lượng các thông số động lực học của robot. Các tác giả xây dựng một hàm
chi phí dựa trên sai số giữa mô-men lực thực nghiệm và mô-men lực mô hình
và tối thiểu hoá hàm chi phí để tìm ra các thông số.
• Bước 4: Kiểm tra tính chính xác của các thông số đã ước lượng bằng cách so sánh
kết quả mô phỏng và kết quả thực nghiệm. Các tác giả cũng so sánh kết quả của
phương pháp đề xuất với kết quả của các phương pháp khác để đánh giá hiệu
quả của phương pháp.
Kết quả cho thấy phương pháp đề xuất có thể giải quyết được vấn đề mâu thuẫn giữa
các tiêu chí khác nhau và cải thiện độ bền vững của quá trình xác định so với các
phương pháp khác. Độ lệch chuẩn trung bình của các thông số đã ước lượng là 0,04 và
0,3 thấp hơn so với tiêu chí F1 và F3, cho thấy nhiễu đã được giảm thiểu hiệu quả.
Ngoài ra, các đường cong thực nghiệm khớp với mô hình ước lượng, và giá trị hiệu
dụng (root mean square) là 0,038, chứng tỏ độ chính xác của phương pháp.
- Bài báo “Robust servosystem design with two degrees of freedom and its application
to novel motion control of robot manipulators” [8], được thực hiện bởi T. Umeno, T.
Kaneko và Y. Hori. Bài báo này đề xuất một phương pháp thiết kế hệ thống servo dựa
trên bộ điều khiển hai bậc tự do (TDOF) và ứng dụng nó vào điều khiển chuyển động
tiên cho cánh tay robot. Phương pháp này gồm các bước sau:
• Bước 1 của phương pháp này là xây dựng một bộ điều khiển TDOF đơn giản cho
hệ thống servo. Bộ điều khiển TDOF có hai hàm truyền chính: hàm truyền từ
đầu vào đến lỗi (input-error transfer function) và hàm truyền từ đầu vào đến đầu
ra (input-output transfer function). Hai hàm truyền này được tham số hoá bằng
cách sử dụng hai tham số thuộc vòng ổn định và hợp lý (ring of stable and
proper rational functions). Hai tham số này giúp điều chỉnh được các tính chất
của bộ điều khiển như phản hồi với tín hiệu kích thích, sai số theo dõi, sai số
giữ vị trí, khả năng khử nhiễu và khả năng chịu được sự thay đổi của tham số
hệ thống.
• Bước 2 của phương pháp này là tối ưu hoá hai tham số của bộ điều khiển TDOF
để đạt được các tiêu chí thiết kế mong muốn cho hệ thống servo. Các tiêu chí
thiết kế có thể là: (1) giảm thiểu sai số theo dõi; (2) giảm thiểu sai số giữ vị trí;
(3) giảm thiểu sai số do nhiễu; (4) giảm thiểu sai số do thay đổi tham số; và (5)
12
 giảm thiểu sai số do hiệu ứng phi tuyến. Các tiêu chí này được biểu diễn bằng
các hàm nhạy cảm (sensitivity functions) và các hàm nhạy cảm bổ sung
(complementary sensitivity functions). Các hàm nhạy cảm cho biết mức độ ảnh
hưởng của nhiễu và thay đổi tham số lên sai số của hệ thống. Các hàm nhạy
cảm bổ sung cho biết mức độ ảnh hưởng của nhiễu và thay đổi tham số lên đầu
ra của hệ thống.
• Bước cuối cùng của phương pháp này là áp dụng bộ điều khiển TDOF vào các
loại điều khiển chuyển động mới lạ cho cánh tay robot. Các tác giả đã minh hoạ
phương pháp này bằng cách thực hiện các loại điều khiển chuyển động khác
nhau cho robot, như điều khiển tuân theo (compliance control), điều khiển lực
(force control) và điều khiển lai (hybrid control). Các loại điều khiển này đều
dựa trên cơ sở của điều khiển vị trí chắc chắn.
Kết quả cho thấy phương pháp này có thể thiết kế một hệ thống servo chắc chắn và
linh hoạt, có khả năng loại bỏ nhiễu từ bên ngoài và giảm sai số theo dõi. Phương pháp
này cũng cho phép thực hiện các loại điều khiển chuyển động tiên tiến cho robot mà
không cần biết trước các thông số động lực học của robot.
- Bài báo “Dynamic parameter identification of robot manipulators based on the optimal
excitation trajectory” [9] được viết bởi nhóm tác giả Xijie Guo, Lei Zhang and Kai
Han. Bài báo này nghiên cứu về cách nhận dạng các thông số động lực học của robot
dựa trên quỹ đạo kích thích tối ưu. Phương pháp này gồm các bước sau:
• Bước 1: Xây dựng mô hình động lực học bao gồm ma sát cho robot và biến đổi
các tham số để được một dạng tuyến tính của các tham số cần nhận dạng. Mô
hình động lực học được xây dựng dựa trên nguyên lý Lagrange và các tham số
được biến đổi bằng phương pháp biến đổi Kronecker (Kronecker
transformation).
• Bước 2: Thiết kế quỹ đạo kích thích tối ưu để tạo ra các tín hiệu đầu vào cho
robot. Các tác giả sử dụng chuỗi Fourier để biểu diễn các quỹ đạo và tối ưu hoá
các hệ số của chuỗi Fourier theo một tiêu chí nhúng nhiều tiêu chí. Tiêu chí này
bao gồm ba tiêu chí con là: (1) tối thiểu hoá sai số của ma trận thông tin Fisher
(Fisher information matrix); (2) tối thiểu hoá sai số của các tham số đã ước
lượng; và (3) tối thiểu hoá mô-men lực cần thiết. Các tác giả sử dụng thuật toán
tối ưu hoá đàn hạt (particle swarm optimization) để giải bài toán tối ưu.
• Bước 3: Thực hiện thí nghiệm trên robot theo quỹ đạo kích thích đã thiết kế và
thu thập dữ liệu về vị trí, vận tốc, gia tốc và mô-men lực của các khớp. Các tác
giả cũng xử lý nhiễu trong dữ liệu bằng phương pháp loại bỏ nhiễu trung bình
(mean filter).

13
 • Bước 4: Sử dụng phương pháp ước lượng hợp lý cực đại (maximum likelihood
estimation) để nhận dạng các tham số động lực học của robot. Các tác giả xây
dựng một hàm chi phí dựa trên sai số giữa mô-men lực thực nghiệm và mô-men
lực mô hình và tối thiểu hoá hàm chi phí để tìm ra các tham số.
Kết quả cho thấy phương pháp này có thể giải quyết được vấn đề mâu thuẫn giữa các
tiêu chí khác nhau và cải thiện độ chính xác của các tham số đã ước lượng. Phương
pháp này cũng có khả năng chống nhiễu cao và duy trì độ chính xác của mô hình động
lực học
2.4.2 Các nghiên cứu trong nước
Ở Việt Nam, các bài báo được thực hiện liên quan đến phần nhận dạng còn khá ít, và
chưa được quan tâm nhiều:
- Bài báo “SỬ DỤNG THUẬT TOÁN TỐI ƯU HÓA BẦY ĐÀN PSO ĐỂ TỐI ƯU
HÓA CÁC THÔNG SỐ CỦA BỘ ĐIỀU KHIỂN PID SỬ DỤNG CHO ROBOT DÂY
SONG SONG” [1] được viết bởi Phạm Đình Bá, Nguyễn Đình Khiêm và Mai Hùng
Tuấn. Bài báo này nghiên cứu về cách sử dụng thuật toán tối ưu hóa bầy đàn PSO để
tối ưu hóa các thông số của bộ điều khiển PID được sử dụng cho robot dây song song.
Các tác giả đã đề xuất một phương pháp tối ưu hóa bầy đàn PSO để tìm ra các thông
số tối ưu cho bộ điều khiển PID. Kết quả cho thấy phương pháp này có thể cải thiện
hiệu suất của robot dây song song.
2.4.3 Các tồn tại của hệ thống
Ở Việt Nam chưa có nhiều đề tài thực hiện việc nhận dạng thông số cho robot ứng
dụng ma trận hồi quy tuyến tính của phương trình động lực học ngược và ước lượng thông số
bằng phương pháp bình phương cực tiểu.
Hướng giải quyết trong đề tài: xây dựng hệ thống điều khiển robot bằng PLC giúp đảm bảo
được tính chính xác trong quá trình chạy thực nghiệm quỹ đạo, thu thập thông tin đầu vào
phục vụ cho quá trình nhận dạng robot.
Ứng dụng phần mềm Matlab để tính toán mô phỏng mô hình SCARA robot trước khi
thử nghiệm chạy thực tế. Việc này giúp tiết kiệm được thời gian sai sót trong quá trình tính
toán.
Áp dụng chuỗi Fourier và Optimization Toolbox trong Matlab để tạo quỹ đạo khớp tối ưu và
ứng dụng phương pháp bình phương cực tiểu để nhận dạng thông số.

14
 CHƯƠNG 3. CƠ SỞ LÝ THUYẾT
3.1 Các kiến thức liên quan về robot
3.1.1 Bậc tự do của robot
Bậc tự do là thuật ngữ dùng để mô tả số khả năng chuyển động độc lập của robot trong
không gian. Các chuyển động độc lập của robot bao gồm chuyển động tịnh tiến (dọc, ngang)
và chuyển động quay (xoay). Bởi vì, cơ hệ của robot là một cơ cấu hở, do đó bậc tự do của
nó có thể tính theo công thức:
5
W = 6n −  ipi (3.1)
i =1

Trong đó:
• n là số khâu động
• pi là số khớp loại i
3.1.2 Hệ tọa độ
Kết cấu robot bao gồm nhiều khâu và chúng được liên kết với nhau thông qua các
khớp, tạo thành một xích động học xuất phát từ một khâu cơ bản cố định (nơi đặt hệ tọa độ
gốc). Các khâu động được gắn kết với các hệ tọa độ trung gian riêng (hệ tọa độ cục bộ). Trong
quá trình hoạt động các tọa độ cục bộ xác định cấu hình của robot bằng cách dịch chuyển dài
(khớp tịnh tiến) hoặc dịch chuyển góc (khớp quay).
Tất cả các hệ tọa độ được gắn trên các khâu của robot đều tuân theo quy tắc bàn tay
phải, được mô tả như sau: dùng tay phải, nắm hai ngón tay út và áp út vào lòng bàn tay, xòe
ngón cái, ngón trỏ và ngón giữa theo 3 phương vuông góc với nhau, phương và chiều của trục
x, y, sẽ lần lượt là phương và chiều của ngón trỏ và ngón giữa nếu chọn ngón cái là phương,
chiều của trục z.

Hình 3. 1 Quy tắc bàn tay phải

3.1.3 Không gian công tác
Không gian công tác của robot bao gồm toàn bộ không gian mà khâu chấp hành cuối
của robot có thể di chuyển trong quá trình thực hiện các chuyển động. Tuy nhiên, không gian
làm việc của robot bị giới hạn bởi các thông số hình học của robot và các ràng buộc cơ học
của các khớp. Ví dụ, một khớp quay chỉ có thể di chuyển trong khoảng góc nhỏ hơn 360 độ.
15
Những ràng buộc này đảm bảo rằng robot thực hiện các chuyển động trong phạm vi an toàn
và đáng tin cậy.
3.2 Động học
Động học thực hiện việc nghiên cứu chuyển động của robot mà không quan tâm đến
lực và mô-men tác dụng lên nó [7]. Hai bài toán chính trong phần động học là bài toán động
học thuận và động học nghịch. Động học thuận [2] nghiên cứu tìm ra phương trình mô tả vị
trí và định hướng trong không gian của khâu cuối với đầu vào là giá trị các biến khớp. Động
học nghịch ngược lại đi tìm phương trình mô tả cách tính các biến khớp với đầu vào là vị trí
của khâu cuối.
Để thực hiện được việc này ta cần xác định các ma trận chuyển vị của các khớp trong
robot sử dụng quy tắc Denavit-Hartenberg.

Hình 3. 2 Mô hình hóa các khâu và khớp theo quy tắc D-H [5]
Gắn hệ trục tọa độ lên các khâu:
• Trục Zˆi −1 đặt dọc theo trục của khớp i
• Trục Xˆ i −1 là tích hữu hướng giữa Zˆi −1 và Zˆ i

• Trục Yˆ xác định theo quy tắc bàn tay phải

Các thông số D-H
• Khoảng cách giữa hai trục Zˆ i và Zˆi +1 theo phương của Xˆ i : ai
• Góc quay quanh Xˆ i giữa hai trục Zˆ i và Zˆi +1 : ai
• Khoảng cách giữa hai trục Xˆ i −1 và Xˆ i theo phương của Zˆi −1 : d i
• Góc quay quanh Zˆi −1 giữa hai trục Xˆ i −1 và Xˆ i : θ i
Ma trận biến đổi từ hệ i-1 sang hệ i:

16
  cosθi − sin θi cos  i sin θi sin  i ai cosθi 
 sin θ cosθi cos  i − cosθi sin  i ai sin θi 
i −1
Ti =  i
(3.2)
 0 sin  i cos  i di 
 
 0 0 0 1 
Các bước xây dựng phương trình động học:
Bước 1: Đặt hệ trục tọa độ cho robot
Bước 2: Thiết lập bảng DH dựa trên quy tắc Denavit-Hartenberg.
Bước 3: Tính toán các ma trận chuyển vị
Bước 4: Dựa vào các ma trận chuyển vị tính toán biến đổi, hình thành nên phương trình
động học.
3.3 Động lực học
3.3.1 Giới thiệu
Động lực học nghiên cứu tìm ra phương trình chuyển động của robot, giúp xác định
các giá trị lực và mô-men của từng khớp từ đó kiểm soát được quá trình điều khiển chuyển
động. Hai phương pháp phổ biến được dùng để tính toán phương trình động lực học là
Newton-Euler và phương pháp Lagrange. Newton-Euler được thực hiện dựa trên định luật II
Newton, mô tả động lực học robot thông qua lực tác động lên từng khâu của robot. Phương
pháp Lagrange mô tả động lực học robot thông qua động năng và thế năng của hệ.
Phương pháp Lagrange cụ thể là dùng phương trình Lagrange-Euler có nhiều ưu điểm
hơn vì dễ tính toán, có thể biểu diễn phương trình động lực học dưới dạng phương trình vi
phân bậc hai với các thành phần được mô tả như một hệ thống khép kín.
3.3.2 Phương trình Lagrange -Euler
❖ Phương trình Lagrange
( ) ( )
L θ,θ = K θ,θ − U ( θ ) (3.3)
Trong đó:
( )
K θ,θ : tổng động năng của hệ
U ( θ ) : tổng thế năng của hệ

- Phương trình động năng

1 
( )
n
1
K θ,θ =   mi vT ci vci + . i wiT . Ci I i . i wi  (3.4)
i =1  2 2 
- Phương trình thế năng

( )
n
U =  −mi . o g T . o PCi + U refi (3.5)
i =1
Trong đó:
mi : Tổng khối lượng của khâu thứ i

17
vci và i wiT : Vận tốc tuyến tính tại trọng tâm của khâu thứ i và vận tốc quay tại khâu thứ i.
Ci
I i : Mô-men quán tính tại trọng tâm của khâu thứ i
o
g T : gia tốc trọng trường
o
PCi : vị trí trọng tâm ở khâu thứ i
U refi : năng lượng thế năng gốc
- Phương trình Lagrange -Euler viết dưới dạng tổng quát:
d L L
− = (3.6)
dt θ θ
❖ Các phương trình xác định thông số động học robot:

Hình 3. 3 Mô tả hệ trục tọa độ khớp xoay robot [5]

- Khớp xoay
Vận tốc góc của khâu thứ i+1 ở hệ trục (i+1):
i +1
wi +1 = i +1 ˆ
R i +1wi +1 + θi +1 i +1 Z (3.7)
i i +1

Vận tốc dài của điểm gốc ở hệ trục (i+1):

i +1
vi +1 = i +1
i R ( i vi + i wi  i Pi +1 ) (3.8)
- Khớp tịnh tiến
Vận tốc góc của khâu thứ i+1 ở hệ trục (i+1):
i +1
wi +1 = i +i1R i wi (3.9)
Vận tốc dài của điểm gốc ở hệ trục (i+1):

vi +1 = i +i1R ( i vi + i wi  i Pi +1 ) + d i +1 i +1 Ẑi +1
.
i +1
(3.10)
Trong đó:
i +1
i R : ma trận xoay của hệ trục (i) lên (i+1).
i +1
wi +1 : vận tốc góc của khâu thứ (i+1)
i
wi : vận tốc góc của khâu thứ (i)
.
θi+1 : vận tốc góc tại khớp thứ (i+1)
.
d i +1 : vận tốc dài tại khớp thứ (i+1)
18
 i +1
Ẑi +1 : vector của trục Z
i +1
vi +1 : vận tốc tuyến tính ở điểm gốc của (i+1)
i
vi : vận tốc tuyến tính ở điểm gốc của (i)
i
Pi +1 : vị trí của điểm gốc (i+1) chiếu lên (i)
 0 −z y   x 
 
Với: S =  z 0 −x  ,  = y  (3.11)
 −y x 0   z 

Do đó:  P = SP
 cosθi − sin θi 0 ai −1 
 sin θ cos  cosθi cos  i −1 − sin  i −1 − sin  i −1di 
i −1
Ai =  i i −1
(3.12)
 sin θi sin  i −1 cosθi sin  i −1 c os  i −1 c os  i −1di 
 
 0 0 0 1 
Dạng tổng quát phương trình vi phân robot
M (θ)θ + C (θ,θ)θ + G(θ) =  (3.13)
Trong đó:
M (θ) : ma trận quán tính
C (θ,θ) : ma trận Coriolis
G( ) : vector trọng trường
θ = θ1 d3  ;θ = θ1
T T
d3  ;θ = θ1 θ 2 d3 
T
θ2 θ2

Công thức tính các ma trận thành phần


M= (3.14)
θ
 T M 
θ θ 
dM 1  
1

C= −  ...  (3.15)
dt 2  
θT M 
 θ n 
U
G= (3.16)

Phương trình động lực học robot
M (θ)θ + C (θ,θ)θ + G (θ) +  f + J T Fe =  (3.17)
Trong đó:
 f = c si gn(θ) + bθ : lực ma sát

J T : ma trận Jacobian
Fe : ngoại lực tác động lên robot tại khâu cuối
19
3.4 Chuỗi Fourier
Chuỗi Fourier cho phép biểu diễn một hàm không chu kỳ thành một tổng các thành
phần có chu kỳ giúp đơn giản hóa việc biểu diễn quỹ đạo cho robot. Các ưu điểm chính của
chuỗi Fourier [6]:
- Cải thiện được tỷ lệ tín hiệu trên tạp âm bằng cách lấy dữ liệu trung bình trong miền
thời gian, dữ liệu sẽ được xử lý trước khi được dùng cho việc nhận dạng. Trong thực
tế việc đo đạc tín hiệu dòng thường không đảm bảo được độ chính xác và rất nhạy cảm
với nhiễu.
- Điều chỉnh được quỹ đạo của robot, bằng cách điều chỉnh giá trị giới hạn của các thông
số giới hạn khớp, vận tốc, gia tốc.
- Tham số hóa quỹ đạo, quỹ đạo được tạo ra nhờ việc xác định các tham số trong chuỗi
Fourier giúp việc tối ưu hóa trở nên dễ dàng hơn.
Quỹ đạo của từng khớp được biểu diễn dưới dạng tổng hữu hạn của hàm sine và cosine.
Nf

a
j
bj 

q j =   i sin( f it ) − i cos( f it )  + q j 0
f i
i =1  f i 

Nf

q j =  aij cos( f it ) − bi j sin( f it ) (3.18)

i =1
Nf

q j =  −aij  f sin( f it ) + bi j  f cos( f it )

i =1

Trong đó:
• aij , bi j : hệ số Fourier
•  f : tần số góc,  f = 2f o . Với f o là tần số cơ bản.
• N f : số lượng thành phần chu kỳ.
• q j , q j , q j : góc quay, vận tốc, gia tốc khớp của khâu j

Chuỗi Fourier có tất cả 2 N f + 1 tham số, 2 N f là tổng số lượng hệ số Fourier, còn lại là
tham số q j 0 .
Chuỗi Fourier cho phép điều chỉnh độ phân giải tần số và băng thông trong quá trình
tạo ra quỹ đạo khớp tối ưu. Đồng thời, các ràng buộc về giới hạn khớp, vận tốc, gia tốc cũng
được áp dụng để đảm bảo robot hoạt động được trong phạm vi cho phép.

(
ˆ = arg min  cond A ( ,  f )) (3.19)

20
Trong đó:
qmin  q ( pTs ,  )  qmax

qmin  q ( pTs ,  )  qmax
q  q ( pT ,  )  q
 min s max

 
 s ( q ( pTs ,  ) )  S

(3.20)

T
0 p f
Ts
T f , Ts : chu kỳ của chuỗi Fourier và chu kỳ lấy mẫu, T f = 2 /  f
S : không gian hoạt động của robot

s ( q ( pTs ,  ) ) : tọa độ đầu công tác của robot có được nhờ phương trình động học.
3.5 Phương pháp Least Squares
Phương pháp Least Squares cho phép ước lượng các tham số động học của robot dựa
trên phương trình tuyến tính nghịch đảo theo các thông số. Phương pháp được thực hiện theo
các bước sau đây:
- Xây dựng mô hình toán học robot dưới dạng phương trình tuyến tính theo các tham số
động lực học.
- Biến đổi các tham số động lực học thành một tập hợp các tham số cơ bản.
- Xây dựng quỹ đạo khớp tối ưu cho robot để thu thập dữ liệu phục vụ nhận dạng.
- Ước tính thông số bằng phương pháp Least Squares.
Tiêu chuẩn ước lượng tham số:
2 2
1 N 1 N (3.21)
VZ (θ, Z N ) = 
N i =1
( y (t ) − ˆ
y (t | θ)) = 
N i =1
( y(t ) −  T (t | θ))

Vector ước lượng tham số:

(3.22)
θ̂ N = arg min VN (θ, Z N )
Đạo hàm bậc 1 theo tham số để tìm giá trị cực tiểu của VN
d
(VN (θ, Z N )) = 0
dθ
Nghiệm của phương trình có dạng
−1
N  N 
θ̂ N =    (t ) T (t )     (t ) y (t )  (3.23)
 i =1   i =1 

21
 CHƯƠNG 4. BÀI TOÁN ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC

4.1 Động học robot

4.1.1 Động học thuận

Hình 4. 1 Hệ trục tọa độ gắn thực tế lên Robot SCARA

Bảng 4. 1 Bảng thông số Denavit-Hartenberg
i ai i di i
1 L2 0 0 1
2 L3 180 0 2
3 0 0 d3 0
4 0 0 0 4
Tọa độ điểm P so với hệ trục tọa độ gốc (base-frame):
R
P = R T0 .0 T1.1T2 .2 T3 .3 T4 .4 P (4.1)

Trong đó: Các ma trận biến đổi từ hệ (i-1) sang hệ i:

1 0 0 0
0 1 0 0 
R
T0 =  (4.2)
0 0 1 L1 
 
0 0 0 1

22
 cosθ1 − sin θ1 0 L2 cosθ1 
 sin θ cosθ1 0 L2 sin θ1 
0
T1 =  1
(4.3)
 0 0 1 0 
 
 0 0 0 1 

cosθ 2 sin θ 2 0 L3 cosθ 2 

 sin θ − cosθ 2 0 L3 sin θ 2 
1
T2 =  2
(4.4)
 0 0 −1 0 
 
 0 0 0 1 

1 0 0 0
0 1 0 0 
2
T3 =  (4.5)
0 0 1 d3 
 
0 0 0 1

cosθ 4 − sin θ 4 0 0
 sin θ cosθ 4 0 0 
3
T4 =  4
(4.6)
 0 0 1 0
 
 0 0 0 1

0 
0 
4
P=  (4.7)
0 
 
1 
Ma trận chuyển vị từ hệ trục R sang hệ trục i = 4 là:
cos(θ1 + θ 2 − θ 4 ) sin(θ1 + θ 2 − θ 4 ) 0 L2 cosθ1 + L3 cos(θ1 + θ 2 ) 
 sin(θ + θ − θ ) − cos(θ + θ − θ ) 0 L sin θ + L sin(θ + θ ) 
R
T4 =  1 2 4 1 2 4 2 1 3 1 2 
(4.8)
 0 0 −1 L1 − d3 
 
 0 0 0 1 
Tọa độ điểm P trên hệ trục R:
 Px   L2 cosθ1 + L3 cos(θ1 + θ 2 ) 
 P   L sin θ + L sin(θ + θ ) 
R
P =  y =  2 1 3 1 2 
(4.9)
 Pz   L1 − d3 
   
1  1 

23
 ❖ Không gian làm việc

Hình 4. 2 Phạm vi hoạt động cánh tay robot

Hình 4. 3 Khu vực làm việc của robot SCARA trong không gian
4.1.2 Động học nghịch
Từ ma trận (4.9) động học thuận ta có:
Px = L2 cosθ1 + L3 cos(θ1 + θ 2 )
Py = L2 sin θ1 + L3 sin(θ1 + θ 2 )
(4.10)
Pz = L1 − d3
 Px 2 + Py 2 = L2 2 + L32 + 2 L2 L3 cos θ 2
Px 2 + Py 2 − L2 2 − L32
 cos θ 2 =
2 L2 L3
(4.11)
Px 2 + Py 2 − L2 2 − L32
 sin θ 2 =  1 − ( ) 2

2 L2 L3

24
 θ2 = a tan 2(sin θ2 ,cos θ2 ) (4.12)
Px = L2 cosθ1 + L3 cos(θ1 + θ 2 )
 Px = L2 cosθ1 + L3 cosθ1 cosθ 2 − L3 sin θ1 sin θ 2 (4.13)
 Px = ( L2 + L3 cosθ 2 ) cosθ1 − L3 sin θ 2 sin θ1
Py = L2 sin θ1 + L3 sin(θ1 + θ 2 )
 Py = L2 sin θ1 + L3 sin θ1 cosθ 2 + L3 sin θ 2 cosθ1 (4.14)
 Py = L3 sin θ 2 cosθ1 + ( L2 + L3 cosθ 2 )sin θ1
Áp dụng định lý Cramer:
 Px   L2 + L3 cosθ 2 − L3 sin θ 2  cosθ1 
P  =   . sin θ 
 y  L3 sin θ 2 L2 + L3 cosθ 2  1

 L + L3 cosθ 2 − L3 sin θ 2 
D = Det  2
L2 + L3 cosθ 2 
(4.15)
 L3 sin θ 2
= ( L2 + L3 cosθ 2 ) + ( L3 sin θ 2 )
2 2

= L2 2 + 2 L2 L3 cosθ 2 + L32
Thay cos2 từ (4.11) ta được:

Px 2 + Py 2 − L2 2 − L32
D = L2 + 2 L2 L3
2
+ L32
2 L2 L3 (4.16)
= Px + Py
2 2

 Px − L3 sin θ 2 
P L2 + L3 cosθ 2  Px ( L2 + L3 cosθ 2 ) + Py L3 sin θ 2
cosθ1 =  =
y

Px 2 + Py 2 Px 2 + Py 2
 L2 + L3 cosθ 2 Px  (4.17)
 L sin θ Py  Py ( L2 + L3 cosθ 2 ) − Px L3 sin θ 2
sin θ1 =  =
3 2

Px + Py 2
2
Px 2 + Py 2
θ1 = a tan 2(sin θ1 ,cosθ1 )
❖ Khâu 3
d3 = L1 − Pz (4.18)

❖ Khâu 4
θ4 =  − (θ1 + θ2 ) (4.19)

  là góc quay khâu công tác so với hệ tọa độ gốc – base frame.

25
4.2 Động lực học robot
4.2.1 Xác định thông số động học
❖ Khâu 1

cosθ1 − sin θ1 0 0
 sin θ cosθ1 0 0 
R
A1 =  1
;
 0 0 1 L1 
 
 0 0 0 1
cosθ1 − sin θ1 0  cosθ1 sin θ1 0 
R1 =  sin θ1 cosθ1 
0  ; R0 =  − sin θ1 cosθ1 0  ;
0 1 (4.20)

 0 0 1   0 0 1 
0  Lc 2 
0  
; P1 =  0  ; Pc1 =  0 
1

 L1   0 
❖ Khâu 2

cosθ 2 − sin θ 2 0 L2 
 sin θ cosθ 2 0 0 
1
A2 =  2
;
 0 0 1 0
 
 0 0 0 1
cosθ 2 − sin θ 2 0  cosθ 2 sin θ 2 0
R2 =  sin θ 2 cosθ 2 
0  ; R1 =  − sin θ 2 0  ;
(4.21)
1 2
cosθ 2
 0 0 1   0 0 1 
 L2   Lc 3 
; 1P2 =  0  ; 2 Pc 2 =  0 
 0   0 
❖ Khâu 3

1 − sin θ 2 0 L3 
0  1 0 0   L3  0 
2
A3 = 
1 0 0  ; 2 R3 = 0 1 0  ; 2 P3 =  0  ; Pc 3 = 0  (4.22)
0 0 1 −d3       
  
 0 0 1 
  −
 3d  0 
0 0 0 1 
❖ Tại khâu 1:
- Vị trí trọng tâm
 Lc 2 cos(θ1 ) 
0
Pc1 = A1. Pc1 =  Lc 2 sin(θ1 ) 
R 1
(4.23)
 L2 

26
- Vận tốc góc
0
1 0 0 ˆ = 0
w1 = R1 w0 + θ1 Z 1
(4.24)
1  
θ1 
- Vận tốc dài tại gốc tọa độ
0
v1 = R1 ( v0 + w0  P1 ) = 0 
1 0 0 0 0
(4.25)
0 
- Vận tốc tại trọng tâm
 .

− θ
 1 c2L sin(θ1)
 
vc1 = 0 R1 ( 1v1 + 1w1  1Pc1 ) =  θ1Lc 2 cos(θ1 ) 
.
1
(4.26)
 0 
 
 
❖ Tại khâu 2:
- Vị trí trọng tâm
 Lc 3 cos(θ1 + θ 2 ) + L2 cos(θ1 ) 
0
Pc 2 = R A2 . 2 Pc 2 =  Lc 3 sin(θ1 + θ 2 ) + L2 sin(θ1 )  (4.27)
 L1 
- Vận tốc góc
 0 
2 1 1 ˆ =
w2 = R2 w1 + θ 2 Z  0 

2
(4.28)
2
θ + θ 
 1 2
- Vận tốc dài tại gốc tọa độ
 θ1 L2 sin(θ 2 ) 
 
2
v2 = R1 (
2
v1 + w1  P2 ) = θ1 L2 cos(θ 2 ) 
1 1 1
(4.29)
 0 
 
- Vận tốc tại trọng tâm
0
vc 2 = 0 R1 1R2 ( 2 v2 + 2 w2  2 Pc 2 )
 −θ1 L2 sin(θ1 ) − θ1 Lc 3 sin(θ1 + θ 2 ) − θ 2 Lc 3 sin(θ1 + θ 2 ) 
  (4.30)
= θ1 Lc 3 cos(θ1 + θ 2 ) + θ 2 Lc 3 cos(θ1 + θ 2 ) + θ1 L2 cos(θ1 ) 
 0 
 
❖ Tại khâu 3:
- Vị trí trọng tâm
 L3 cos(θ1 + θ 2 ) + L2 cos(θ1 ) 
0
Pc 3 = R A3 . 3 Pc 3 =  L3 sin(θ1 + θ 2 ) + L2 sin(θ1 )  (4.31)
 L1 − d3 

27
 - Vận tốc góc
 0 
3
w3 = 3 R2 2 w2 =  0  (4.32)
θ1 + θ 2 
- Vận tốc dài tại gốc tọa độ
 θ1 L2 sin(θ 2 ) 
 
3
v3 = 3 R2 ( 2 v2 + 2 w2  2 P3 ) =  L3 (θ1 + θ 2 ) + θ1 L2 cos(θ 2 )  (4.33)
 0 
 
- Vận tốc tại trọng tâm
0
vc 3 = 0 R1 1R2 2 R3 ( 3v3 + 2 w2  3 Pc 3 ) + d3 3 Ẑ3
 −θ1 L2 sin(θ1 ) − θ1 Lc 3 sin(θ1 + θ 2 ) − θ 2 Lc 3 sin(θ1 + θ 2 ) 
  (4.34)
= θ1 Lc 3 cos(θ1 + θ 2 ) + θ 2 Lc 3 cos(θ1 + θ 2 ) + θ1 L2 cos(θ1 ) 
 d3 
 
4.2.2 Jacobian và vận tốc đầu công tác
Vector vận tốc tổng quát của robot bao gồm v vận tốc dài và w vận tốc góc của đầu
công tác.
v
V =  (4.35)
 w
 dPx / dt   wx 

Với v =  dPy / dt  và w =  wy 

(4.36)
 dPz / dt   wz 
Dựa vào mối liên hệ giữa vận tốc của đầu công tác và chuyển động của các khớp
trong quá trình hoạt động ta có thể tính toán được ma trận Jacobi cho robot.
v J 
V =   =  v θ
 w  J w 
Phương trình tổng quát thể hiện mối liên hệ giữa chuyển động vi phân của khớp và
chuyển động vi phân của cánh tay [2].
 dx   dθ1 
 dy   dθ 
   2
 dz   Robot   dθ3 
 =   hay  D  =  J  Dθ  (4.37)
δx   Jacobi   dθ 4 
 δy   dθ 5 
   
 δz   dθ 6 

28
 Trong đó  dx dy dz  trong  D  thể hiện chuyển động vi phân của cánh tay theo trục
x, y, z;  δx δy δz  trong  D  thể hiện chuyển động vi phân của các góc quay quanh trục x,
y, z.  Dθ  thể hiện chuyển động vi phân của các khớp.

Từ công thức (4.9) ta tính được vận tốc dài theo công thức sau:
 PX   − L2 sin θ1 − L3 sin(θ1 + θ 2 ) − L3 sin(θ1 + θ 2 ) 0   1 
   
v =  PY  =  L2cosθ1 + L3 cos(θ1 + θ 2 ) L3 cos(θ1 + θ 2 ) 0  2  (4.38)
 PZ  
  0 0 −1  d3 
Vận tốc góc của đầu công tác:
0
4 = 0 1 + 0 R1 1 2 + 0 R2 2 3 + 0 R3 3 4 (4.39)
Vì khớp cuối là khớp tịnh tiến nên vận tốc góc 2 3 = 0 , suy ra:
0
4 = 0 1 + 0 R1 1 2 + 0 R3 3 4 = 0 1 + 0 R1 1 2 + 0 R1 1R2 2 R3 3 4
 0  cosθ1 − sin θ1 0   0  cos(θ1 + θ 2 ) − sin(θ1 + θ 2 ) 0   0 
=  0  +  sin θ1 cosθ1 0   0  +  sin(θ1 + θ 2 ) cos(θ1 + θ 2 ) 0   0 
1   0 0 1  2   0 0 1   4  (4.40)
0 0   0   0 
   
= 0+0 + 0  =     0 

1  2   4   1 + 2 + 4 
Vecto vận tốc cơ cấu cuối của robot
 − L2 sin θ1 − L3 sin(θ1 + θ 2 ) − L3 sin(θ1 + θ 2 ) 0 0 
 L cosθ + L cos(θ + θ ) L cos(θ + θ ) 0 0   1 
 2 1 3 1 2 3 1 2  
v  0 0 −1 0  2 
V = =  .
0 0  d 
(4.41)
   0 0
 3
 0 0 0 0   
  4
 1 1 0 1 
Từ ma trận (3.27) trên, ta thu được ma trận Jacobi của robot Scara là:
 − L2 sin θ1 − L3 sin(θ1 + θ 2 ) − L3 sin(θ1 + θ 2 ) 0 0
 L cosθ + L cos(θ + θ ) L cos(θ + θ ) 0 0 
 2 1 3 1 2 3 1 2

 0 0 −1 0
J =  (4.42)
 0 0 0 0
 0 0 0 0
 
 1 1 0 1

29
4.2.3 Phương trình Lagrange - Euler
❖ Động năng:
K = K1 + K 2 + K 3
1
2
( m1 ( 0 v1c '. 0 v1c ) + 1w1 '.I1. 2 w2 ) + ( m3 ( 0 v2 c '. 0v2 c ) + 2 w2 '.I 2 . 2 w2 )
1
2
+ ( m3 ( 0 v3c '. 0 v3c ) + 3 w3 '.I 3 . 3 w3 )
1
2
1 (4.43)
= [m1 Lc 2 2 + m2 ( L2 2 + Lc 32 + 2 L2 Lc 3 cos θ 2 )
2
+ m3 ( L2 2 + L32 + 2 L2 L3 cos θ 2 ) + I1 + I 2 + I 3 ]θ12
+[m2 Lc 3 ( Lc 3 + L2 cos θ 2 ) + m3 ( L2 L3 cos θ 2 + L32 ) + I 2 + I 3 ]θ1θ 2
1 1
+ [m2 Lc 32 + m3 L32 + I 3 ]θ 22 + m3 d32
2 2
❖ Thế năng:

U = m1 0 0 g  0 Pc1 + m2  0 0 g  0 Pc 2 + m3 0 0 g  0 Pc 3
(4.44)
= gm3[l0 (m1 + m2 ) + m3 (l0 − d3 )
❖ Phương trình Lagrange

1
L = K − U = [m1lc12 + m2 (l12 + lc 2 2 + 2l1lc 2 cos θ 2 )
2
+ m3 (l1 + l2 + 2l1l2 cos θ 2 ) + I1 + I 2 + I 3 ]θ12
2 2

(4.45)
+[m2lc 2 (lc 2 + l1 cos θ 2 ) + m3 (l1l2 cos θ 2 + l2 2 ) + I 2 + I 3 ]θ1θ 2
1 1
+ [m2lc 2 2 + m3l2 2 + I 3 ]θ 22 + m3 d32 − g[l0 (m1 + m2 ) + m3 (l0 − d3 )
2 2
L
= θ1 ( I1 + I 2 + I 3 + l12 m2 + l12 m3 + l2 2 m2 + lc12 m1 + lc 2 2 m2
θ1
+2l1l2 m3 cos(θ 2 ) + 2l1lc 2 m2 cos(θ 2 ))
+θ 2 ( I 2 + I 3 + l2 2 m3 + lc 2 2 m2 + l1l2 m3 cos(θ 2 ) + l1lc 2 m2 cos(θ 2 ))
L
=0
θ 1
L
= θ1 ( I 2 + I 3 + l2 2 m3 + lc 2 2 m2 + l1l2 m3 cos(θ 2 ) + l1lc 2 m2 cos(θ 2 ))
θ 2 (4.46)
.
+ θ 2 ( I 2 + I 3 + l2 2 m3 + lc 2 2 m2 )
L
= −(θ1 + θ 2 )θ1l1 sin(θ 2 )(l2 m3 + lc 2 m2 )
θ 2
L
= d3 m3
d3
L
= gm3
d 3

30
 ❖ Phương trình mô-men-lực các khớp
d L d L L
1 = θ1 + θ2 −
dt θ1 dt θ1 θ 1
d L L
2 = − (4.47)
dt θ 2 θ 2
d L L
F3 = −
dt d3 d 3
Suy ra,
 m1 Lc 2 2 + I1 + m2 L2 2 + m2 Lc 32 + I 2 + 
1 = 
 m L 2 + m L 2 + I + 2(m L L + m L L ) cos(θ )  1
θ
 3 2 3 3 3 2 2 c3 3 2 3 2 

 m2 Lc 32 + I 2 + m3 L32 + I 3  (4.48)
+  θ2
 +(m2 L2 Lc 3 + m3 L2 L3 ) cos( 2 ) 
−2 ( m2 L2 Lc 3 + m3 L2 L3 ) sin(θ 2 )θ1θ 2
− ( m2 L2 Lc 3 + m3 L2 L3 ) sin(θ 2 )θ 22
 m2 Lc 32 + I 2 + m3 L32 + I 3 
2 =   θ1
 +(m2 L2 Lc 3 + m3 L2 L3 ) cos(θ 2 ) 
+ ( m2 Lc 32 + I 2 + m3 L32 + I 3 ) θ 2
(4.49)

+ ( m2 L2 Lc 3 + m3 L2 L3 ) sin( 2 )θ12
..
F3 = m3 d3 − m3 g (4.50)
❖ 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 ) (4.51)
Từ công thức (3.14), (3.15), (3.16), ta tính được.
- Ma trận quán tính
 M 11 M 12 M 13 
M =  M 21 M 22 M 23  (4.52)
 M 31 M 32 M 33 
Với,
M 11 = I1 + I 2 + I 3 + L2 2 m2 + L2 2 m3 + L32 m3 + Lc 2 2 m1 + Lc 32 m2
+2l1l2 m3 cos(θ 2 ) + 2l1lc 2 m2 cos(θ 2 )
M 12 = L32 m3 + L2 L3m3 cos(θ 2 ) + m2 Lc 32 + L2 Lc 3m2 cos(θ 2 ) + I 2 + I 3
(4.53)
M 21 = L32 m3 + L2 L3m3 cos(θ 2 ) + m2 Lc 32 + L2 Lc 3m2 cos(θ 2 ) + I 2 + I 3
M 22 = L32 m3 + m2lc 2 2 + I 2 + I 3
M 33 = m3
M 13 = M 23 = M 31 = M 32 = 0

31
 - Ma trận Coriolis
 C11 C12 C13 
C = C21 C22 C23  (4.54)
C31 C32 C33 
Với:
C11 = −2θ 2 L2 sin(θ 2 )( L3 m3 + Lc 3 m2 )
C12 = −θ 2 L2 sin(θ 2 )( L3 m3 + Lc 3 m2 )
( L2 sin(θ 2 )( L3m3 + Lc 3m2 )(2θ1 − θ 2 ))
C21 =
2 (4.55)
θ L sin(θ 2 )( L3m3 + Lc 3m2 )
C22 = 1 2
2
C13 = C23 = C31 = C32 = C33 = 0
- Vector trọng trường
 0 
G =  0  (4.56)
 -gm3

32
CHƯƠNG 5. NHẬN DẠNG VÀ QUY HOẠCH QUỸ ĐẠO CHUYỂN ĐỘNG ROBOT

5.1 Yêu cầu của đề tài

Các nội dung yêu cầu trong đề tài:
- Nhận dạng các thông số động học của robot: Nhận dạng thông số giúp đánh giá được
tốt hơn quá trình hoạt động của robot. Công việc nhận dạng thường tập trung vào các
vấn đề chính: tìm thông số cần nhận dạng, quá trình thu thập dữ liệu, phương pháp tối
ưu và các điều kiện ràng buộc trong hệ thống robot nhằm đảm bảo tính khả thi và chất
lượng khi thực hiện.
- Quy hoạch quỹ đạo chuyển động cho robot: Từ yêu cầu thực hiện thao tác chuyển động
theo quỹ đạo cho trước, có thể thấy robot sẽ thực hiện 3 chuyển động chính theo trục
x và trục y và di chuyển theo trục z để nâng hạ bút. Để thực hiện nhiều quỹ đạo khác
nhau, quy hoạch quỹ đạo cần mang tính tổng quát đảm bảo tính linh hoạt, không bị
phụ thuộc vào một quỹ đạo nhất định.
5.2 Phương hướng và giải pháp thực hiện
5.2.1 Phương án 1
Đối với phần nhận dạng thông số có một phương pháp khá phổ biến là phương pháp
PSO (Particle Swarm Optimization) nhận dạng thông số robot dựa trên tìm kiếm theo quần
thể của các phần tử trong không gian tìm kiếm. Mô phỏng hành vi bầy đàn của các phần từ
để tìm kiếm giải pháp trong không gian thông số.
Về phần quỹ đạo chuyển động thực hiện tìm phương trình mô tả chuyển động theo
từng quỹ đạo riêng biệt sau đó biến đổi để tìm ra các phương trình giá trị góc quay, vận tốc,
gia tốc tương ứng của khớp để điều khiển. Phương pháp này đảm bảo robot di chuyển mượt
mà, không bị ngắt quãng trong quá trình hoạt động, tuy nhiên gặp hạn chế ở khâu tính toán
phức tạp và tốn thời gian khi phải thực hiện nhiều dạng quỹ đạo khác nhau.
5.2.2 Phương án 2
Một phương pháp nhận dạng khác là phương pháp bình phương cực tiểu dựa trên việc
tìm kiếm giá trị nhỏ nhất của tổng bình phương sai số giữa dữ liệu thực tế và mô hình dự
đoán. Kết quả thu được là giá trị gần nhất cho các thông số mô hình dựa trên sự trùng khớp
với giữ liệu thực tế.
Quy hoạch quỹ đạo cho robot có thể được tiếp cận bằng phương pháp chia nhỏ các
thành phần trong quỹ đạo thành hai hình cơ bản là đường thẳng và cung tròn. Khi được yêu
cầu thực hiện một biên dạng bất kỳ ta có thể chia nhỏ hình vẽ thành nhiều đường thẳng và
cung tròn nối tiếp nhau từ đó đưa thông số đầu vào để điều khiển robot.

33
5.3 Lựa chọn phương án
Phương pháp Least Squares được sử dụng cho mô hình robot SCARA hiện tại vì có
nhiều ưu điểm:
- Phương pháp sử dụng đại số tuyến tính để tìm giá trị nhỏ nhất của tổng bình phương
sai số giúp giải quyết được bài toán tối ưu một cách chính xác trong nhiều trường hợp.
- Độ phức tạp tính toán thấp hơn so với phương pháp PSO phải tính toán vị trí vận tốc
của từng phần tử trong quần thể.
- Least Squares phù hợp với các mô hình có mối quan hệ tuyến tính.
- Các bước để thực hiện nhận dạng thông số bằng phương pháp bình phương cực tiểu:
• Thu thập dữ liệu: dữ liệu mô-men, lực của từng khớp được thu thập từ robot thực tế
sau khi tiến hành chạy theo quỹ đạo khớp tối ưu.
• Xây dựng phương trình động lực học: xác định phương trình chuyển động của robot
để tìm ra các thông số cần nhận dạng. Phương trình biểu diễn mối liên hệ giữa các
thông số và dữ liệu vừa thu thập được.
• Thiết lập hàm mục tiêu: Xác định hàm mục tiêu theo phương pháp bình phương cực
tiểu để tìm giá trị nhỏ nhất của hàm.
• Đánh giá – xác nhận kết quả: Dựa trên kết quả tính toán chạy thực nghiệm với mô hình
thực tế và mô hình toán học xây dựng được để so sánh và điều chỉnh thông số cho phù
hợp.
Đối với quy hoạch quỹ đạo chuyển động, lựa chọn phương án 2 quy hoạch quỹ đạo
cho hai đường cơ bản đường thẳng và đường cung tròn
Sử dụng hàm bậc 3 để thiết kế quỹ đạo chuyển động: bao gồm hai chuyển động chính
là đường thẳng và đường cung tròn. Phương án này giúp việc thiết kế quỹ đạo trở nên đơn
giản hơn về mặt tính toán cũng như linh hoạt trong việc thực hiện nhiều dạng quỹ đạo khác
nhau.
5.4 Trình tự công việc tiến hành
Sau khi đã chọn được phương án thực thiện đề nhóm tiến hành lên kế hoạch thực hiện
với các mốc thời gian và công việc như sau:
a. Xem xét, đánh giá sơ bộ mô hình robot SCARA hiện có
- Phần điều khiển: các module PLC.
- Các linh kiện liên quan.
- Xem xét mô hình robot về tình trạng sử dụng.
b. Thiết kế hệ thống điều khiển cho robot
- Thiết kế bản vẽ điện cho robot.
- Tiến hành mua linh kiện để chuẩn bị thi công.
- Thực hiện thi công tủ điện điều khiển.
- Sửa chữa phần hỏng hóc của robot.
34
 - Thiết kế bàn vẽ cho robot.
c. Tính toán lý thuyết liên quan đến robot
- Động học robot.
- Động lực học robot.
- Thiết kế quỹ đạo chuyển động cho robot.
d. Thực hiện nhận dạng thông số robot
- Các phương pháp sử dụng để tiến hành nhận dạng.
- Tiến hành chạy thử nghiệm robot để thu thập dữ liệu cho phần nhận dạng.
- Đánh giá các thông số tính toán được.
e. Tổng kết – nhận xét – đánh giá quá trình thực hiện đề tài
- Tổng kết lại các kết quả đã đạt được trong đề tài vào báo cáo.
- Đánh giá và đưa ra nhận xét các vấn đề làm được và chưa làm được.
- Đưa ra hướng phát triển trong tương lai
5.5 Nhận dạng thông số robot
5.5.1 Giới thiệu
Nhận dạng thông số động lực học là một phần quan trọng trong việc xây dựng hệ thống
điều khiển cho robot đảm bảo được độ an toàn, chính xác và thực hiện được nhiều tác vụ khác
nhau. Các thông số động lực học có được sẽ giúp hoàn thiện được mô hình robot phục vụ cho
việc tính toán, chạy mô phỏng mà không cần phải chạy mô hình thực tế quá nhiều lần giúp
làm giảm thời gian, chi phí cũng như năng suất hoạt động.

Hình 5. 1 Sơ đồ nhận dạng thông số robot

Các bước để tiến hành nhận dạng thông số động lực học robot:
- Xác định phương trình động lực học: Rút gọn phương trình động lực học về dưới dạng
phương trình tuyến tính.
- Xác định quỹ đạo khớp tối ưu: Quỹ đạo khớp tối ưu là quỹ đạo giúp tối ưu được thời
gian, năng lượng mà robot sử dụng cũng như đảm bảo các yếu tố về giới hạn mô-men,
giới hạn khớp, vận tốc và gia tốc mà robot cần phải đáp ứng.

35
 - Nhận dạng thông số: Tìm ra các tham số ảnh hưởng đến phương trình động lực học
của robot.
- Kiểm nghiệm: Chạy quỹ đạo thử nghiệm trên mô hình và robot thực tế để đánh giá
kết quả thu được, từ đó điều chỉnh lại thông số cho phù hợp.
5.5.2 Quỹ đạo khớp tối ưu
Quỹ đạo khớp tối ưu được hình thành nhờ việc đi tìm quỹ đạo sao cho thông số điều
kiện (condition number) về giá trị nhỏ nhất. Thông số điều kiện N c là tỉ số giữa giá trị lớn
nhất và giá trị nhỏ nhất của  , N c càng nhỏ thì quỹ đạo càng được tối ưu.
Sử dụng hàm FMINCON trong Matlab để tối ưu hóa quỹ đạo. Các bước thực hiện:
j j
- Sử dụng hàm random ngẫu nhiên các hệ số của chuỗi Fourier ai , bi và q j 0
- Sử dụng hàm fmincon xuất ra thông số điều kiện và các tham số tương ứng của chuỗi.
Các số liệu đầu ra sẽ được sử dụng làm đầu vào cho lần chạy tiếp theo.
- So sánh thông số điều kiện giữa hai lần chạy liên tiếp nếu giá trị chênh lệch lớn hơn
thì tiếp tục chạy ngược lại nếu giá trị chênh lệch nhỏ hơn thì dừng chương trình.
Thông số của chuỗi Fourier được sử dụng.
- Số lượng thành phần: N f = 5 , suy ra tổng tham số cần tìm là 11.
- Tần số cơ bản: fo = 0.1Hz
- Tốc độ lấy mẫu: 100Hz
❖ Giá trị giới hạn của khớp 1 và khớp 2
Tốc độ tối đa của động cơ HF-KP13 là 3000 vòng/ phút, tương đương 13107200 pps .
Cáp kết nối SSCNET III của bộ khuếch đại có tốc độ truyền là 50Mbps, đáp ứng được nhu
cầu của hệ thống.
Giới hạn tốc độ = Quãng đường di chuyển trên mỗi vòng quay x Tốc độ tối đa của
động cơ. Do đó giới hạn tốc độ của mỗi trục là:
- Trục 1: 4.5x3000 = 13500[deg/ min] (5.1)
- Trục 2: 7.2x3000 = 21600[deg/ min] (5.2)
- Trục 3: 7407.41x3000 = 22221[mm / min] (5.3)
- Trục 4: 205.71428x3000 = 617142[deg/ min] (5.4)
- Trục 1:
• Giới hạn khớp:
−113.905o  q1  113.905o  −2[rad]  q1  2[rad] (5.5)

• Vận tốc lớn nhất:

1 
13500[deg/ min] = 13500x x = 3.927[rad / s] (5.6)
60 180
• Gia tốc lớn nhất: Thời gian nhanh nhất để vận tốc
(5.7)
tăng từ 0 đến giá trị cực đại là 500[ms]
36
 3.927
−3
= 7.8540[rad/s 2 ]
500x10
- Trục 2:
• Giới hạn khớp:
(5.8)
− 140.8136o  q2  − 140.813o  − 2.46[rad]  q1  2.46[rad]
• Giới hạn vận tốc:
1  (5.9)
21600[deg/ min] = 21600x x = 6.283[rad / s]
60 180
• Gia tốc lớn nhất: Thời gian nhanh nhất để vận tốc tăng từ 0 đến
giá trị cực đại là 500[ms]
(5.10)
6.283
−3
= 12.5660[rad/s 2 ]
500x10
•
 −2  q1  2
 
 −2.46  q2  2.46
 −3.927  q  −3.927
 1
Suy ra,   (5.11)
 −6.283  q2  6.283
−7.8540  q  7.8540
 1

−12.5660  q2  12.5660
Quỹ đạo khớp tối ưu được viết bằng chương trình Matlab được mô tả ở Hình 5.3.

Hình 5. 2 Đồ thị biểu diễn giá trị góc quay và vận tốc của khớp 1 và khớp 2
Các thông số của chuỗi Fourier thu thập được sau khi tối ưu hóa quỹ đạo chuyển động.
Bảng 5. 1 Hệ số Fourier a ij
a ij j =1 j=2
i =1 0.0958 0.1269
i=2 0.0534 0.1446
i =3 0.0308 0.0783
i=4 0.0169 0.0371
i =5 0.0018 -0.0175
37
 Bảng 5. 2 Hệ số Fourier b ij
b ij j =1 j=2
i =1 -0.3984 0.1889
i=2 -0.1196 -0.0140
i =3 -0.0139 -0.0655
i=4 -0.0104 -0.0424
i =5 -0.0113 -0.0017
Bảng 5. 3 Hệ số q j 0
q j0 j =1 j=2
0.3979 0.0674
5.5.3 Mô hình nhận dạng thông số
Mô hình nhận dạng thông số robot được xây dựng trên Matlab, được mô tả ở hình (5.3).

Hình 5. 3 Mô hình nhận dạng thông số robot

Các khối trong mô hình:
(1) Quỹ đạo khớp tối ưu và mô-men thu thập từ robot thực tế
(2) Mô hình nhận dạng thông số
(3) Xuất dữ liệu
(4) Kiểm nghiệm kết quả
Quỹ đạo khớp tối ưu sau khi được tính toán sẽ được chạy trên robot thực tế và thu thập
dữ liệu đầu ra là các thông số về góc quay, vận tốc, gia tốc từng khớp. đồng thời thu thập dữ
liệu mo-men mà robot xuất ra trong quá trình hoạt động. Khối (1) đảm nhận nhiệm vụ đọc dữ
liệu sau đó đưa vào khối nhận dạng (2) để tìm ra các thông số động học của robot. Thông số
này sẽ được cập nhật liên tục trong quá trình chạy mô phỏng. Kết quả cuối cùng được đưa và
phần kiểm nghiệm để so sánh mô-men thực tế và mô-men tính toán dựa trên các thông số
động học vừa mới tính toán.

38
5.5.4 Bộ dự báo hồi quy tuyến tính
Từ phương trình động lực học robot (4.41) (4.42) và (4.43), ta tiến hành biến đổi để
đưa về dạng phương trình tuyến tính:
y () =  ( q, q, q ) p (5.12)
Ta có,
 m1 Lc 2 2 + I1 + m2 L2 2 + m2 Lc 32 + I 2 + 
1 = 
 m L + m L + I + 2(m L L + m L L ) cos(θ )  1
2 2
θ
 3 2 3 3 3 2 2 c3 3 2 3 2 

 m L + I 2 + m3 L3 + I 3
2 2

+  2 c3  θ2 (5.13)
 +(m2 L2 Lc 3 + m3 L2 L3 ) cos( 2 ) 
−2 ( m2 L2 Lc 3 + m3 L2 L3 ) sin(θ 2 )θ1θ 2
− ( m2 L2 Lc 3 + m3 L2 L3 ) sin(θ 2 )θ 2 2
 m L 2 + I 2 + m3 L32 + I 3 
2 =  2 c 3  θ1
 +(m2 L2 Lc 3 + m3 L2 L3 ) cos(θ 2 ) 
+ ( m2 Lc 32 + I 2 + m3 L32 + I 3 ) θ 2
(5.14)

+ ( m2 L2 Lc 3 + m3 L2 L3 ) sin( 2 )θ12
..
F3 = m3 d3 − m3 g (5.15)
Đặt,
I zz1 = m1lc12 + I1 + m2l12 + m2lc 2 2 + I 2 + m3l12 + m3l2 2 + I 3
(5.16)
I zz 2 = m2lc 2 2 + I 2 + m3l2 2 + I 3
I zz 3 = m2l1lc 2 + m3l1l2
1 = ( I zz1 + 2mr cos(θ 2 ) ) θ1 + I zz 2 + I zz 3 cos(θ 2 )θ 2
(5.17)
−2 I zz 3 sin(θ 2 )θ1θ 2 − I zz 3 sin(θ 2 )θ12
2 = I zz 2 + I zz 3 cos(θ 2 )θ1 + I zz 2θ 2 + I zz 3 (θ 2 )θ12 (5.18)
F3 = m3d3 − m3 g (5.19)
( ) ( )
1 = I zz1θ1 + I zz 2θ 2 + I zz 3  2θ1 + θ 2 cos(θ 2 ) − θ + 2θ1θ 2 sin(θ 2 ) 
2
2 (5.20)
( ) (
2 = I zz 2 θ1 + θ 2 + I zz 3 θ1 cos(θ 2 ) + θ12 sin(θ 2 ) ) (5.21)
F3 = m3 (d3 − g ) (5.22)
Bộ dự báo hồi quy có dạng:
y () =  ( q, q, q ) p (5.23)

 I zz1 
 1  I 
 
y () =  2  ; p =  zz 2  (5.24)
 I zz 3 
 F3   
 m3 

39
 θ1

θ2 ( 2θ1 ) ( )
+ θ 2 cos(θ 2 ) − θ 2 2 + 2θ1θ 2 sin(θ 2 ) 0 

 = 0

(θ + θ )
1 2 (θ cos(θ ) + θ
1 2 1
2
sin(θ ) )
2 0 

(5.25)
0 0 0 (d3 − g ) 
 
Trong đó,
y() : vector mô-ment robot
p : vector tham số động học robot
 ( q, q, q ) : vector hồi quy, các phần tử trong vector được gọi là phần tử hồi quy
5.5.5 Phương pháp Least Squares
Từ bộ dự báo hồi quy công thức (5.23), áp dụng công thức (3.23) thông số động học
của robot được xác định theo công thức sau:
p = (T  ) −1 T y =  + y
(5.26)
+
với,  là ma trận giả nghịch đảo của 
Bảng 5. 4 Thông số nhận dạng
Thông số Giá trị nhận dạng
I zz1 (kgm 2 ) 0.0703

I zz 2 (kgm 2 ) 1.2874

I zz 3 ( kgm 2 ) 0.1953
m3 (kg ) 0.0020

Hình 5. 4 Đồ thị mô-men thực tế và mô-men nhận dạng khớp 1 và khớp 2

40
 Hình 5. 5 Sai số giữa mô-men thực tế và mô-men nhận dạng khớp 1 và khớp 2
Từ đồ thị (5.4) và (5.5) cho thấy còn có sai số giữa mô-men thực tế và mô-men nhận
dạng vì nhiều yếu tố khó kiểm soát trong quá trình thu thập, lọc dữ liệu đầu vào. Tuy nhiên,
phương pháp cũng đã nhận dạng được các thông số của robot, giúp chứng minh được phương
trình động lực học và mô hình hóa robot thực tế ứng dụng kiểm soát quá trình hoạt động robot.
5.6 Quy hoạch quỹ đạo chuyển động robot
5.6.1 Quy hoạch quỹ đạo theo đường thẳng
Bài toán: Thiết kế quỹ đạo của điểm tác động cuối theo đoạn thẳng đi từ A tới B trong
thời gian t0 đến t f :

Hình 5. 6 Quy hoạch quỹ đạo theo đường thẳng

Ta có phương trình đường thẳng từ A( x0 ; y0 ) tới B ( x f ; y f ) là:
x − x0 y − y0
=
x f − x0 y f − y0
(5.27)
y f − y0 y0 x f − y f x0
y= x+
x f − x0 x f − x0
Giả sử quy luật chuyển động của robots là hàm bậc 3 với x = x(t ) , ta có:
x(t ) = a0 + a1t + a2t 2 + a3t 3 (5.28)
v(t ) = x '(t ) = a1 + 2a2t + 3a3t 2 (5.29)

41
Xét điều kiện biên:
 x(t0 ) = x0
 v (t ) = v = 0
 x 0 x0
 (5.30)
 x(t f ) = x f

vx (t f ) = vxf = 0
Từ hệ phương trình (5.30),ta tính được:
 a0 = x0
 a1 = 0


 a = 3( x f − x0 )
 2 t 2f (5.31)

 −2( x f − x0 )
a3 =

 t 3f
Thay(5.31) vào (5.12) và (5.13) ta được:
3( x f − x0 ) 2( x f − x0 )
x(t ) = x0 + 2
t2 − 3
t3 (5.32)
tf tf
y f − y0 y0 x f − y f x0 y f − y0 y0 x f − y f x0
y= x+  y (t ) = x(t ) + (5.33)
x f − x0 x f − x0 x f − x0 x f − x0
Tương tự, ta rút phương phương trình quỹ đạo theo hướng y:
3( y f − y0 ) 2( y f − y0 )
y (t ) = y0 + 2
t2 − 3
t3 (5.34)
tf tf
x f − x0 x0 y f − x f y0 x f − x0 x0 y f − x f y0
x= x+  x(t ) = y (t ) + (5.35)
y f − y0 y f − y0 y f − y0 y f − y0
Từ phương trình vị trí điểm tác động cuối, ta áp dụng phương trình động học ngược để xác
định góc quay các khớp 1 và khớp 2, ta có:
Công thức tính toán góc quay khớp 1 và khớp 2 từ phương trình (4.18) và (4.12):
1 = a tan 2(sin 1,cos1 ) (5.36)
2 = a tan 2(sin 2 ,cos2 ) (5.37)
Sử dụng công thức (4.38) để tính vận tốc dài đầu công tác
 PX   − L2 sin θ1 − L3 sin(θ1 + θ 2 ) − L3 sin(θ1 + θ 2 ) 0   1 
   
v =  PY  =  L2cosθ1 + L3 cos(θ1 + θ 2 ) L3 cos(θ1 + θ 2 ) 0  2  (5.38)
 PZ  
  0 0 −1  d3 
5.6.2 Thiết kế quỹ đạo theo đường cung tròn
Bài toán: Thiết kế quỹ đạo của điểm tác động cuối theo đường cung tròn từ 3
điểm A( xA ; y A ), B( xB ; yB ), C ( xC ; yC ) trong thời gian từ t0 đến t f
Ta có phương trình quỹ đạo đường tròn dạng tham số:

42
  x = xI + R sin( (t ))
 (5.39)
 y = yI + R cos( (t ))
Trong đó:
R : bán kính đường tròn qua 3 điểm A, B, C (đường tròn ngoại tiếp ABC )
I ( xI ; yI ) : Tâm đường tròn ngoại tiếp ABC
 (t ) : hàm góc quay theo thời gian t
Tính giá trị tọa độ tâm I và bán kính R từ hệ phương trình:
 IA = IB 
 IA = IB
2 2
 ( x A − xI ) 2 + ( y A − y I ) 2 = ( x B − x I ) 2 + ( y B − y I ) 2

  2  (5.40)
 IA = IC  = ( xA − xI ) + ( y A − yI ) = ( xC − xI ) + ( yC − yI )

2 2 2 2 2
 IA IC
Với các điểm A( xA ; y A ), B( xB ; yB ), C ( xC ; yC ) cụ thể ta giải hệ phương trình tìm
được giá trị xI , yI , từ đó tính bán kính R:

R = IA = ( x A − xI ) 2 + ( y A − yI ) 2 (5.41)
Giả sử quy luật chuyển động của robots là hàm bậc 3 với s = s(t ) , ta có:
s (t ) = as 0 + as1t + as 2t 2 + as 3t 3 = R (t )
a a a a (5.42)
  (t ) = s 0 + s1 t + s 2 t 2 + s 3 t 3 = a0 + a1t + a2t 2 + a3t 3
R R R R
 (t ) =  '(t ) = a1 + 2a2t + 3a3t 2
(5.43)
Với các điều kiện biên:
  (t0 ) =  0
  (t ) =  = 0
 0 0
 (5.44)
  (t f ) =  f

 (t f ) =  f = 0
Trong đó:
 0 [rad] : Góc quay tại điểm bắt đầu, t = t0 [s]
0 [rad/s] : Vận tốc góc tại điểm bắt đầu, t = t0 [s]
 f [rad] : Góc quay tại điểm kết thúc, t = t f [s]
 f [rad/s] : Vận tốc góc điểm kết thúc, t = t f [s]
Giải hệ phương trình (5.44) tính được các hệ số a0 , a1 , a2 , a3 :
 a0 =  0
 a1 = 0

 3( f −  0 )
 2a = (5.45)
t 2f

 −2( f −  0 )
a3 =
 t 3f

43
 3( f − 0 ) 2( f −  0 )
  (t ) = 0 + 2
t2 − 3
t3 (5.46)
tf tf
Thuật toán xác định góc và chiều quay:
Bài toán quy hoạch quỹ đạo cung tròn từ 3 điểm là A, B, C phân biệt
- Quy ước chiều quay (+) theo chiều ngược chiều kim đồng hồ
y
- Sử dụng hàm tính góc arctan [rad] với −     và tan  =
x
- Trong đó (x, y) là tọa độ điểm trên mặt phẳng Descartes
Ta có các trường hợp sau:

- Với (x, y) thuộc góc phần tư thứ I, 0   
2

- Với (x, y) thuộc góc phần tư thứ II,  
2

- Với (x, y) thuộc góc phần tư thứ III, −    −
2

- Với (x, y) thuộc góc phần tư thứ IV, −   0
2

Hình 5. 7 Đường tròn biểu diễn các giá trị góc quay
Các trường hợp đặc biệt:
- Với y = 0 và x  0 ,  = 0
- Với y = 0 và x  0 ,  = 

- Với y  0 và x = 0 ,  =
2

44
 
- Với y  0 và x = 0 ,  = −
2
Để dễ dàng cho việc tính toán, ta chuyển đổi giá trị góc quay  về khoảng giá trị [ ;2 ]
3
- Với (x, y) thuộc góc phần tư thứ III,    
2
3
- Với (x, y) thuộc góc phần tư thứ IV,    2
2
Áp dụng để tính toán, ta có:
Góc quay tại điểm A
xIA = x A − xI (5.47)
y IA = y A − y I (5.48)
yIA y 
tan  0 =   0 = arctan  IA 
xIA x  (5.49)
 IA 
Tương tự với góc quay tại điểm C
xIC = xC − xI (5.50)
yIC = yC − yI (5.51)
yIC y 
tan  f =   f = arctan  IC 
xIC x  (5.52)
 IC 
Từ phương trình vị trí điểm tác động cuối, ta áp dụng phương trình động học ngược (4.17)
và (4.12) để xác định góc quay các khớp 1 và khớp 2:
1 = a tan 2(sin 1,cos1 ) (5.53)
2 = a tan 2(sin 2 ,cos2 ) (5.54)

45
 CHƯƠNG 6. TÍNH TOÁN THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN ROBOT

6.1 Sơ đồ hệ thống điều khiển

Hệ thống điện bao gồm bộ điều khiển PLC dòng Q với module điều khiển vị trí
QD75MH4 để điều khiển các AC Servo, module điều khiển động cơ kết nối qua PLC bằng
cáp quang sử dụng mạng truyền thông SSCNET III với tốc độ truyền 50 Mbps. Dưới đây là
sơ đồ kết nối QD75MH4 – Driver MR-J3B – AC Servo (Hình 6.1).

Hình 6. 1 Sơ đồ kết nối QD75MH4 – Driver MR-J3B – AC Servo

46
Hình 6. 2 Sơ đồ kết nối driver MR-J3-10B với động cơ AC Servo và PC [11]

Hình 6. 3 Tủ điện PLC thiết kế và thực tế

47
6.2 Các thiết bị sử dụng trong hệ thống
6.2.1 Động cơ AC Servo
❖ Giới thiệu

Hình 6. 4 Động cơ AC Servo [13]

Động cơ AC Servo được cấu tạo từ 3 bộ phận chính: Stato, rotor và bộ mã hóa
- Stator: đóng vai trò như một bệ, có chức năng quấn sợi dây xung quanh lõi để cung
cấp lực cần thiết xoay rotor.
- Rotor: là trục quay sử dụng nam châm vĩnh cửu và nó được liên kết với bộ mã hóa.
- Bộ mã hóa: được dùng để đo vị trí tuyệt đối của động cơ và các giá trị khác. Nó là
bộ cảm biến dò và chuyển đổi các góc xoay thành tín hiệu điện, sau đó xuất tín hiệu
này.
Bảng 6. 1 Thông số kỹ thuật động cơ AC Servo
Thông số kỹ thuật Giá trị
Điện áp cung cấp 200 V 
Công suất 0.1[kW ]
Tốc độ tối đa 3000  vòng / phút 
Cấp độ bảo vệ IP65
Encoder 18-bit, độ phân giải 262144  xung 
❖ Nguyên lý hoạt động
Động cơ AC Servo đóng vai trò là thiết bị dẫn động, hoạt động theo yêu cầu gửi xuống
từ bộ khuếch đại servo đồng thời trả về giá trị vị trí về (được thực hiện bởi encoder) theo cơ
chế vòng điều khiển hồi tiếp. Bộ khuếch đại (Driver servo) thực hiện nhận lệnh từ bộ điều
khiển trung tâm sau đó gửi lệnh đến động cơ AC Servo.

48
 Hình 6. 5 Sơ đồ nguyên lý hệ thống AC Servo
6.2.2 Driver Servo và hệ thống mạng SSCNET III
❖ Driver Servo MRJ3-10B

Hình 6. 6 Driver Servo MR-J3-10B [12]

Bảng 6. 2 Thông số driver MR-J3-10B
Thông số kỹ thuật Giá trị
3-phase 200  230[VAC] hoặc
Nguồn cấp
1-phase 200 230[VAC ] 50 − 60[ Hz]
Dòng điện vào định mức 0.9[ A]
Tần số biến đổi Tối đa 5%
Dòng điện đầu ra 3-phase 170[VAC ]
Công suất 100[W ]
Khối lượng 0.8[kg ]
❖ Mạng truyền thông SSCNET III
SSCNET III (System Servo Control Network III) là hệ thống điều khiển mạng servo
của riêng hãng Mitsubishi sử dụng cáp kết nối là cáp quang với tốc độ truyền dữ liệu tối đa là
50 Mbps. Do vậy, mạng SSCNET III có khả năng cung cấp độ chính xác và độ tin cậy cao
trong quá trình truyền dữ liệu. Bộ điều khiển sử dụng mạng SSCNET III là các thiết bị chuyên
dụng như QD75MH4, Q172H, Q173H…
49
6.2.3 Bộ điều khiển lập trình PLC Mitsubishi Q series
PLC Mitsubishi dòng Q hỗ trợ người dùng kết hợp và chọn lựa sự phối hợp tối ưu giữa
CPU, mô-đun giao tiếp truyền thông, các mô-đun thông minh và các module I/O để áp dụng
vào từng ứng dụng cụ thể. Khả năng kết hợp: có thể kết hợp PLC CPU, Motion CPU, process
controllers và ngay cả PC vào cùng một hệ thống điều khiển (tối đa 4 CPU). Tùy vào nhu cầu
ứng dụng, mỗi dòng CPU sẽ có những lợi thế khác nhau theo yêu cầu.
Trong đề tài này, nhóm thực hiện điều khiển chương trình với module Q00UCPU.

Hình 6. 7 Module Q00UCPU [14]

Bảng 6. 3 Thông số kỹ thuật Module Q00UCPU
Thông số kỹ thuật Giá trị
Tốc độ xử lý lệnh 0.08[ s]
Dung lượng bộ nhớ 10k[step]
Số I/O có sẵn 1024
Số I/O có thể mở rộng 8192
Cổng truyền thông RS232 hoặc USB
Số Timer 2048
Số Counter 1024
6.2.4 Các module hỗ trợ
❖ Module điều khiển vị trí QD75MH4

Hình 6. 8 Module điều khiển vị trí QD75MH4 [12]

50
Bảng 6. 4 Thông số kỹ thuật Module Q00UCPU
Thông số kỹ thuật Giá trị
Số trục tối đa có thể kiểm soát vận hành 4
Dữ liệu cho định vị vị trí 600  data / truc 
Đơn vị điều khiển vị trí cho các trục Pulse, mm, inch, degree
Nội suy tuyến tính 4 trục, 2 nội suy tròn
Hỗ trợ mạng truyền thông SSCNET III
❖ Module Digital Input QX42

Hình 6. 9 Sơ đồ chân module Digital Input QX42 [15][17]

Bảng 6. 5 Thông số kỹ thuật module Digital Input QX42
Thông số kỹ thuật Giá trị
Điện áp ngõ vào hoạt động 24[VDC]
Số ngõ vào 64
Thời gian đáp ứng Lên tới 1[ms]

❖ Module Digital Output QY42P

Hình 6. 10 Sơ đồ chân module Digital Output QY42P [16][17]

51
Bảng 6. 6 Thông số kỹ thuật Module Digital Output QY42P
Thông số kỹ thuật Giá trị
Điện áp hoạt động 24[VDC]
Số chân I/O 64
Loại ngõ ra Transistor
Kiểu kết nối Sink
Dòng điện tiêu hao 0.15[ A]
❖ Module giao tiếp Ethernet Qj71E71 – 100

Hình 6. 11 Module giao tiếp Ethernet QJ71E71 – 100 [17]

Bảng 6. 7 Thông số kỹ thuật Module QJ71E71 – 100
Thông số kỹ thuật Giá trị
Khoảng cách tối đa trên từng phân đoạn 100[m]
Số node/connection tối đa 2 [tầng]
Số kết nối tương thích cho phép 16
Bộ nhớ ngẫu nhiên 6[kW ] 1
Số I/O 32 points / slot
Dòng điện tiêu hao (với điện áp 5[V ] ) 0.6[ A]

52
6.3 Hệ thống trạm PLC

Hình 6. 12 Hệ thống trạm PLC sử dụng trong đề tài

Bảng 6. 1 Hệ thống trạm PLC
STT Vị trí Loại Tên
0 PLC PLC CPU Q00UCPU
1 0 Intelligent QJ71E71-100
2 1 Input Qx42
3 2 Output Qy42P
4 3 Intelligent QD75MH4
6.3.1 Các thiết bị liên quan

Hình 6. 13 Cấu tạo và sơ đồ nguyên lý bộ lọc nguồn 1 pha [22]

Bảng 6. 8 Thông số kỹ thuật bộ lọc nguồn 1 pha
Thông số kỹ thuật Giá trị
Dòng điện định mức 20[ A]
Điện áp định mức 250 VAC 
Kích thước 97  56  44  mm 
Tần số hoạt động 50 / 60  Hz 

53
6.4 Cài đặt thông số cho module QD75MH4
6.4.1 Cài đặt tham số cho bộ điều khiển Servo

Hình 6. 14 Màn hình thiết lập tham số module QD75MH4

6.4.2 Cài đặt tham số của bộ truyền động điện tử (Electronic gear)
Bộ truyền động điện tử (EG) là bộ phận xác định số vòng quay (số xung tương ứng
với mỗi vòng quay) mà động cơ cần thực hiện để di chuyển máy theo quãng đường di chuyển
được lập trình sẵn.

Hình 6. 15 Sơ đồ nguyên lý bộ truyền động điện tử [11]

Khái niệm về bộ truyền động điện tử được thể hiện bằng biểu thức sau:
- Số xung trên một vòng quay (No. of pulses per rotation) = AP
- Quãng đường di chuyển trên một vòng quay (Movement amount per rotation) =
AL.
- Hệ số nhân (Magnification Unit) = AM.
- Quãng đường di chuyển trên một xung (Movement amount per pulse) = ∆𝑆
AP AP
EG = = [10] (6.1)
ΔS ALxAM
54
❖ Trục 1

Hình 6. 16 Cơ cấu truyền động trục 1 [11]

- Do trục 1 quay quanh trục Z nên đơn vị được cài đặt (Unit setting) là độ (2: degree)
- Tỉ số truyền của hộp số Harmonic tại trục 1: n = 1:80
- Động cơ sử dụng: HP-KP13 với encoder có 262144 xung → AP = 262144[ PLS ]
- Góc quay của trục mỗi vòng quay của động cơ:
1
ΔS = 360[deg]xn = 360x = 4.5[deg/ round ] (6.2)
80
Thay các hệ số trên vào biểu thức (6.1), ta được:
AP 262144
=
ΔS 4.5x1
 AP = 262144 (6.3)

 AL = 4.5
 AM = 1

❖ Trục 2

Hình 6. 17 Cơ cấu truyền động trục 2 [11]

- Do trục 2 quay quanh trục Z nên đơn vị được cài đặt (Unit setting) là độ (2: degree)
- Tỉ số truyền của hộp số Harmonic tại trục 2: n = 1: 50
- Động cơ sử dụng: HP-KP13 với encoder có 262144 xung → AP = 262144[ PLS ]
- Góc quay của trục tên mỗi vòng quay của động cơ:
55
 1
ΔS = 360[deg]xn = 360x = 7.2[deg/ round ] (6.4)
50
Thay các hệ số trên vào biểu thức (6.1), ta được:
AP 262144
=
ΔS 7.2x1
.  AP = 262144 . (6.5)

 AL = 7.2
 AM = 1

❖ Trục 3

Hình 6. 18 Cơ cấu truyền động trục 3

- Do trục 3 tịnh tiến theo trục Y nên đơn vị được cài đặt (Unit setting) là mm (3: mm)
- Tỉ số truyền puly đai răng: n = 20 : 54 , bước vít: p = 20[mm]
- Động cơ sử dụng: HP-KP13 với encoder có 262144 xung → AP = 262144[ PLS ]
- Quãng đường trục vít trên mỗi vòng quay của động cơ:
20
ΔS = pxn = 20x (6.6)
54
Thay các hệ số trên vào biểu thức (6.1), ta được:
AP 262144 262144
= 
ΔS 20 x 20 7407.41x10−3 x1
54
(6.7)
 AP = 262144
 −3
 AL = 7407.41x10
 AM = 1


56
 ❖ Trục 4

Hình 6. 19 Cơ cấu truyền động trục 4

- Do trục 4 quay quanh trục Z nên đơn vị được cài đặt (Unit setting) là độ (2: degree)
- Tỉ số truyền puly đai răng tại trục 4: n = 20 : 35
- Động cơ sử dụng: HP-KP13 với encoder có 262144 xung → AP = 262144[ PLS ]
- Góc quay của khâu công tác trên mỗi vòng quay của động cơ:
20
ΔS = 360[deg]xn = 360x (6.8)
35
Thay các hệ số trên vào biểu thức (6.1), ta được:
AP 262144 262144
= 
ΔS 360 x 20 205.7142x1
35
(6.9)
 AP = 262144

 AM = 205.7142
 AM = 1

6.4.3 Giới hạn tốc độ (Speed limit value)
Tốc độ tối đa của động cơ HF-KP13 là 3000 vòng/ phút, tương đương 13107200 pps .
Cáp kết nối SSCNET III của bộ khuếch đại có tốc độ truyền là 50Mbps, đáp ứng được nhu
cầu của hệ thống.
Giới hạn tốc độ = Quãng đường di chuyển trên mỗi vòng quay x Tốc độ tối đa của
động cơ. Do đó giới hạn tốc độ của mỗi trục là:
Trục 1: 4.5x3000 = 13500[deg/ min] (6.10)
Trục 2: 7.2x3000 = 21600[deg/ min] (6.11)
Trục 3: 7407.41x3000 = 22221[mm / min] (6.12)
Trục 4: 205.71428x3000 = 617142[deg/ min] (6.13)

57
6.4.4 Các tham số khác
❖ Giới hạn hành trình (Software stroke upper/lower limit value)
Thiết đặt tham số này để giới hạn phạm vi hoạt động của trục. Để tránh gây hư hỏng
phần cứng, giá trị của tham số giới hạn hành trình phải nằm trong hành trình của phần cứng
(Hardware stroke limit).

Hình 6. 20 Giới hạn hành trình của trục 1 và 2

Bảng 6. 9 Giới hạn hành trình của trục 1 và 2
Giới hạn dưới (lower limit) Giới hạn trên (upper limit)
Phần cứng Phần mềm Phần cứng Phần mềm
Trục 1 0 10 226 220
Trục 2 280 290 0 350
Trục 3 0 5000μm 170000μm 120000μm
Sử dụng tham số giới hạn hành trình (Software stroke limit valid/invalid selection):
Chọn valid để sử dụng tham số giới hạn hành trình, invalid để bỏ qua.
Tham số điểm gốc (OPR basic parameter)
Bảng 6. 10 Tham số điểm gốc (OPR basic parameter)
Trục Địa chỉ điểm gốc (OP address) Tốc độ quay về điểm gốc (OPR speed)

1 113 1000 degree/min

2 140 1000 degree/min
3 30000 2000 mm/min.
4 0 4000 degree/min

58
6.5 Lưu đồ điều khiển

Hình 6. 21 Lưu đồ điều khiển chương trình PLC

6.6 Thiết kế giao diện người dùng
6.6.1 Cấu hình giao thức kết nối PLC và Visual Studio
Để thiết lập giao tiếp giữa máy tính và PLC, nhóm sử dụng phần mềm MX-Component
của hãng Mitsubishi. Phần mềm này cho phép truyền và nhận dữ liệu giữa máy tính và PLC
thông qua cổng kết nối RS-232C hoặc USB. Các bước cài đặt và cấu hình phần mềm MX-
Component được mô tả chi tiết trong các bước sau:

59
Hình 6. 22 Giao diện phần mềm MX Hình 6. 23 Chọn thông số trạm muốn cấu
Component hình

Hình 6. 24 Chọn giao thức kết nối với Hình 6. 25 Chọn dòng PLC được kết nối
máy tính (PC side I/F) (PLC side I/F)

Hình 6. 26 Chọn chức năng của PLC Hình 6. 27 Hoàn thành thiết lập truyền
trong mạng thông

60
 Hình 6. 28 Sơ đồ các bước thêm thư viện ActUtlType vào Visual C#.NET
6.6.2 Kết nối SQL Server với Visual Studio

Hình 6. 30 Sơ đồ các bước thiết lập

Hình 6. 29 Tạo mới cơ sở dữ liệu truyền thông giữa Visual C#. NETvà cơ
sở dữ liệu SQL Server

61
6.6.3 Giao diện ứng dụng điều khiển robot SCARA

Hình 6. 31 Giao diện đăng nhập

(1) Account: Nhập tên tài khoản
(2) Password: Nhập mật khẩu để đăng nhập
*: Tài khoản mặc định: admin, mật khẩu: admin
(3) Exit: Đóng ứng dụng

6.6.4 Màn hình chính

Hình 6. 32 Giao diện ứng dụng điều khiển robot SCARA

(1) Function: Chức năng cơ bản của ứng dụng
(2) Account Management: Quản lý tài khoản đăng nhập
(3) Log Out: Đăng xuất tài khoản
(4) Connect to PLC: Kết nối truyền thông giữa PC và PLC
(5) Servo On: Gửi tín hiệu cấp nguồn, bật động cơ Servo
(6) PLC On: Gửi tín hiệu mở module QD75MH4
(7) Auto: Chế độ chạy tự động
(8) JOG: Chế độ jogging, chạy theo độc lập từng trục
(9) Return home: Các khớp trở lại vị trí home (OPR)

62
6.6.5 Màn hình chế độ JOG

Hình 6. 33 Giao diện chế độ chạy theo trục (JOG Mode)

Chức năng chính:

- Điều khiển vị trí từng khâu theo tốc độ, hướng di chuyển lựa chọn trên màn hình.
- Hiển thị các thông số góc quay, vận tốc từng khớp và vị trí đầu công tác.
Chức năng các nút nhấn:
- Set Speed: cài đặt tốc độ cho từng động cơ.
- Forward: điều khiển trục động cơ quay cùng chiều kim đồng hồ.
- Reverse: điều khiển trục động cơ quay ngược chiều kim đồng hồ.
- Read Data: Đọc các thông số vị trí, vận tốc, dòng điện từ PLC.
- Error: Hiển thị bảng kiểm tra lỗi động cơ servo.
- Reset: Xóa tất cả các lỗi hiện tại của servo.
- Stop: Gửi tín hiệu tắt động cơ servo, dừng đọc giá trị vị trí, vận tốc, dòng điện.

63
6.6.6 Màn hình chế độ tự động

Hình 6. 34 : Chế độ chạy tự động

Chức năng chính
- Điều khiển robot theo quỹ đạo đường thẳng, đường cung tròn, quỹ đạo kết hợp từ dữ
liệu tọa độ điểm nhập vào.
- Hiển thị vị trí, vận tốc, dòng điện thực tế của các khớp và quỹ đạo mẫu.
Chức năng các nút nhấn
- Line: Hiển thị cửa sổ nhập tọa độ điểm bắt đầu A, điểm kết thúc B, thời gian chạy và
trục nội suy (theo hướng x hoặc y).

Hình 6. 35 Cửa sổ nhập giá trị đầu vào quỹ đạo đường thẳng
- Arc: Hiển thị cửa sổ nhập tọa độ điểm bắt đầu A, điểm giữa B, điểm kết thúc C và thời
gian chạy.

64
 Hình 6. 36 Cửa sổ nhập giá trị đầu vào quỹ đạo đường cung tròn
- Multi Line: Hiển thị cửa sổ nhập tọa độ kết hợp.

• Add Value: Hiển thị cửa sổ nhập giá trị đầu vào theo quỹ đạo đường thẳng (Line)
hoặc đường cung tròn (Arc)

Hình 6. 37 Cửa sổ nhập giá trị tọa độ đầu vào

• End Point: Tích chọn để gửi tín hiệu điều khiển khâu 3, nhấc bút đến điểm bắt đầu
đường kế tiếp và hạ bút tiếp tục quỹ đạo.
• Complete: Hoàn thành quỹ đạo tại điểm kết thúc của đường được chọn.
- Start: Bắt đầu chạy quỹ đạo, gửi tín hiệu đọc giá trị vị trí, vận tốc, dòng điện ở từng
động cơ hiển thị lên màn hình.
- Pause: Tạm dừng robot.
- Stop: Tắt động cơ servo, dừng đọc giá trị vị trí, vận tốc, dòng điện từng động cơ.
- Write: Ghi dữ liệu điều khiển vị trí xuống vùng nhớ mô-đun QD75MH4.
65
 - Error: Hiển thị cửa sổ kiểm tra lỗi động cơ servo.

Hình 6. 38 Cửa sổ hiển thị mã lỗi các trục

• Check Error: Đọc giá trị lỗi ở PLC, hiển thị lên màn hình.
• View: Hiển thị cửa sổ chi tiết lỗi và giải pháp khắc phục.
• Reset: Xóa lỗi hiện tại.

Hình 6. 39 Cửa sổ hiển thị chi tiết lỗi

6.6.7 Màn hình quản lý người dùng

Hình 6. 40 Quản lý người dùng

66
 Chức năng: Hiển thị danh sách tài khoản hiện tại, phân quyền sử dụng và có thể thêm,
chỉnh sửa hoặc xóa tài khoản (nếu quyền sử dụng là “Teacher”).
Chức năng nút nhấn:
- ADD: hiển thị cửa sổ thêm tài khoản.
- DELETE: xóa tài khoản ở vị trí chỉ định.
- EDIT: hiển thị cửa sổ chỉnh sửa tài khoản ở vị trí chỉ định.

Hình 6. 41 Giao diện cửa sổ thêm, chỉnh sửa tài khoản

67
 CHƯƠNG 7. THỰC NGHIỆM – ĐÁNH GIÁ
7.1 Chạy thực nghiệm quỹ đạo
7.1.1 Quỹ đạo đường thẳng
Tọa độ điểm đầu – điểm cuối đường thẳng: A (0.4, -0.2), B (0.2, -0.2) với thời gian 3s.
x(t ) = a0 + a1t + a2t 2 + a3t 3 (7.1)
Các điều kiện biên:
 x(0) = 0.4
v (0) = v = 0
 x x0
 (7.2)
 x(te ) = 0.2

 vxe = vxe = 0
Giải hệ phương trình (7.2), ta được:
 a0 = 0.4
 a1 = 0

 3(0.2 − 0.4) 1
 a2 = =− (7.3)

2
3 15
 −2(0.2 − 0.4) 2
a3 = =
 3 3
135
1 2 3
Vậy x(t ) = 0.4 − t 2 + t (7.4)
15 135

Hình 7. 1 Quỹ đạo đường thẳng thực tế và mô phỏng

68
 Hình 7. 2 Đồ thị góc quay của khớp 1 và khớp 2 thực tế và mô phỏng

Hình 7. 3 Đồ thị vận tốc của khớp 1 và khớp 2 thực tế và mô phỏng

Từ hình 7.2 ta có thể thấy đồ thị góc quay của khớp 1 vào khớp 2 trong tính toán và
chạy thực nghiệm có sai lệch ít và đảm bảo được robot chạy chính xác với quỹ đạo mong
muốn (hình 7.1). Tương tự đồ thị ở hình 7.3 cho thấy vận tốc đáp ứng của robot trong quá
trình chạy thực tế và tính toán là khá tương đồng, tuy còn có sai số tuy nhiên robot vẫn đảm
bảo hoạt động được trơn tru (vận tốc đáp ứng giảm dần về 0 không có hiện tượng dừng đột
ngột).

69
 Bảng 7. 1 Sai số vị trí quỹ đạo đường thẳng

Mô phỏng Thực nghiệm Sai lệch

Thời gian
PX (m) PY (m) PX (m) PY (m) PX (%) PY (%)
0 0.4 -0.2 0.399499 -0.1997542 0.13% 0.12%
0.1 0.397754 -0.2 0.396865 -0.1997465 0.22% 0.13%
0.2 0.391406 -0.2 0.389905 -0.1998513 0.38% 0.07%
0.3 0.381543 -0.2 0.380382 -0.1998472 0.30% 0.08%
0.4 0.36875 -0.2 0.366659 -0.1997719 0.57% 0.11%
0.5 0.353613 -0.2 0.350648 -0.1998612 0.84% 0.07%
0.6 0.336719 -0.2 0.331344 -0.200162 1.60% 0.08%
0.7 0.318652 -0.2 0.310512 -0.2002447 2.55% 0.12%
0.8 0.3 -0.2 0.300722 -0.199883 0.24% 0.06%
0.9 0.281348 -0.2 0.287361 -0.2000979 2.14% 0.05%
1 0.263281 -0.2 0.263851 -0.2000713 0.22% 0.04%
1.1 0.246387 -0.2 0.243698 -0.2003281 1.09% 0.16%
1.2 0.23125 -0.2 0.233641 -0.2000103 1.03% 0.01%
1.3 0.218457 -0.2 0.215649 -0.2001447 1.29% 0.07%
1.4 0.208594 -0.2 0.208654 -0.2000711 0.03% 0.04%
1.5 0.202246 -0.2 0.202791 -0.2001267 0.27% 0.06%
1.6 0.2 -0.2 0.199992 -0.2000105 0.00% 0.01%
Độ sai lệch trung bình 0.76% 0.08%

Hình 7. 4 Đồ thị vận tốc dài khâu công tác robot quỹ đạo đường thẳng
Hình 7.4 thể hiện đáp ứng vận tốc dài trong tính toán và mô phỏng là khá tương đồng,
có thể thấy giá trị đầu và giá trị cuối của vận tốc theo trục x trong thực tế đều gần bằng 0 đảm
bảo robot hoạt động trơn tru.

70
7.1.2 Quỹ đạo cung tròn
Quy hoạch quỹ đạo cung tròn từ 3 điểm: A (0.2; -0.1), B (0.3, -0.1414), C (0.4; -0.1)
trong khoảng thời gian từ 0 đến 2(s).
❖ Xác định tâm I và bán kính R
Ta có hệ phương trình:
 IA = IB  IA2 = IB 2 ( xA − xI ) + ( y A − yI ) = ( xB − xI ) + ( yB − yI )
2 2 2 2

  2 
 IA = IC  IA = IC ( xA − xI ) + ( y A − yI ) = ( xC − xI ) + ( yC − yI )
2 2 2 2 2

(0.2 − xI ) 2 + (−0.1 − yI ) 2 = (0.3 − xI ) 2 + (−0.1414 − y1 ) 2


(0.2 − xI ) + (−0.1 − yI ) = (0.4 − xI ) + (−0.1 − y1 )
2 2 2 2

(7.5)
0.04 − 0.4 xI + xI 2 + 0.01 + 0.2 yI + y12 = 0.09 − 0.6 xI + x 2 + 0.02 + 0.4 yI + y12

0.04 − 0.4 xI + xI + 0.01 + 0.2 yI + y1 = 0.16 − 0.8 x1 + xI + 0.01 + 0.2 y1 + y1
2 2 2 2

0.2 xI − 0.2 yI = 0.06  x = 0.3

  I
0.4 xI = 0.12  yI = 0
Với tâm I (0.3;0) ta tính được bán kính R
2
R = IA = ( x A − xI ) 2 + ( y A − yI ) 2 = (0.2 − 0.3) 2 + (−0.1 − 0) 2 = (7.6)
10
Tính toán giá trị góc quay tại điểm A (  0 )và điểm C (  f ):
Góc quay tại điểm A
xIA = x A − xI = 0.2 − 0.3 = −0.1 (7.7)
yIA = y A − y I = −0.1 − 0 = −0.1

yIA −0.1 y  
tan 0 = = = 1  0 = arctan  IA  = + k ,(k  Z ) (7.8)
xIA −0.1 x  4
 IA 
Với xIA  0, yIA  0  Điểm A thuộc góc phần tư thứ III
3  3 5 (7.9)
   0     + k   k = 1 , 0 =
2 4 2 4
Góc quay tại điểm C
xIC = xC − xI = 0.4 − 0.3 = 0.1 (7.10)
yIC = yC − yI = −0.1 − 0 = −0.1
yIC −0.1  y  −
tan  f = = = −1   f = arctan  IA  = + k ,(k  Z ) (7.11)
xIC 0.1 x  4
 IA 
Với xIC  0, yIC  0  Điểm C thuộc góc phần tư thứ IV

3 3 − 7 (7.12)
   f  2   + k  2  k = 2, f =
2 2 4 4
Giả sử quy luật chuyển động của robots là hàm bậc 3 với s = s(t ) , ta
(7.13)
có:
71
 s (t ) = as 0 + as1t + as 2t 2 + as 3t 3 = R (t )
as 0 as1 a a
  (t ) = + t + s 2 t 2 + s 3 t 3 = a0 + a1t + a2t 2 + a3t 3
R R R R
Với các điều kiện biên:
 5
  (t0 ) =  0 = 4

  (t0 ) = 0 = 0
 (7.14)
 (t f ) =  f = 7
 4
  (t ) =  = 0
 f f

Giải hệ phương trình tính được các hệ số a0 , a1 , a2 , a3 :

 5
 a0 =
 a =  4

0 0
 a1 = 0 a1 = 0
 
 3( f −  0 )  
= 
 2a  3.
(7.15)
 a2 = 22 = 0.375
tf2 2

 −2( f −  0 ) 
a3 =  
3 −2.
 t 
a3 = 23 = −0.125
f 2

5
  (t ) = + 0.375 t 2 − 0.125 t 3
4
 2  5 
 x = xI + R sin( (t )) = 0.3 + sin  + 0.375 t 2 − 0.125 t 3  (7.16)
 10  4 

 y = y + R cos( (t )) = 2 cos  5 + 0.375 t 2 − 0.125 t 3 
 I  
 10  4 

Hình 7. 5 Quỹ đạo cung tròn thực tế và mô phỏng

72
 Hình 7. 6 Đồ thị góc quay khớp 1 và khớp 2 thực tế và mô phỏng

Hình 7. 7 Đồ thị vận tốc khớp 1 và khớp 2 thực tế và mô phỏng

Từ hình 7.6 và hình 7.7 ta nhận thấy rằng đáp ứng góc quay và vận tốc của khớp 1 và
khớp 2 là khá chính xác. Thực nghiệm cho thấy robot không có hiện tượng bị ngắt quãng, hay
dừng đột ngột và cho ra được quỹ đạo chính xác (hình 7.5).

73
 Bảng 7. 2 Sai số vị trí quỹ đạo cung tròn
Thời gian Mô phỏng Thực nghiệm Sai lệch
PX (m) PY (m) PX (m) PY (m) PX (%) PY (%)
0 0.2 -0.1 0.2 -0.1 0.00% 0.00%
0.1 0.2018 -0.1017 0.2 -0.1 0.89% 1.67%
0.2 0.207 -0.1065 0.2 -0.1 3.38% 6.10%
0.3 0.2155 -0.1134 0.2 -0.0999 7.19% 11.90%
0.4 0.2273 -0.1213 0.2009 -0.1007 11.61% 16.98%
0.5 0.2422 -0.1291 0.2042 -0.1039 15.69% 19.52%
0.6 0.2598 -0.1356 0.2094 -0.1082 19.40% 20.21%
0.7 0.2794 -0.1399 0.2182 -0.1145 21.90% 18.16%
0.8 0.3 -0.1414 0.234 -0.1233 22.00% 12.80%
0.9 0.3206 -0.1399 0.2478 -0.1288 22.71% 7.93%
1 0.3402 -0.1356 0.2752 -0.1358 19.11% 0.15%
1.1 0.3578 -0.1291 0.3073 -0.1371 14.11% 6.20%
1.2 0.3727 -0.1213 0.3411 -0.1314 8.48% 8.33%
1.3 0.3845 -0.1134 0.3697 -0.1192 3.85% 5.11%
1.4 0.393 -0.1065 0.3883 -0.1084 1.20% 1.78%
1.5 0.3982 -0.1017 0.398 -0.1008 0.05% 0.88%
1.6 0.4 -0.1 0.4 -0.1 0.00% 0.00%
Độ sai lệch trung bình 10.09% 8.10%

Hình 7. 8 Đồ thị vận tốc dài khâu công tác robot quỹ đạo cung tròn
Đồ thị 7.8 cho thấy vận tốc dài tính toán và mô phỏng có độ sai lệch nhỏ, sai lệch xảy
ra do sai lệch trong giá trị góc quay và gia tốc đáp ứng trong quá trình chạy robot còn sai số.

74
7.1.3 Quỹ đạo kết hợp
❖ Vẽ quỹ đạo UTE
Các bước chương trình thực hiện
- Chia quỹ đạo thành các đoạn thẳng và đường cung tròn nhỏ và nhập vào giao diện
điều khiển.
- Chương trình thực hiện quy hoạch quỹ đạo
- Xuất dữ liệu khớp và vận tốc và nạp vào PLC để điều khiển robot chạy.

Hình 7. 9 Tọa độ các điểm biểu diễn quỹ đạo UTE

Hình 7. 10 Quỹ đạo UTE

75
 Hình 7. 11 Đồ thị góc quay và vận tốc của khớp 1 và khớp 2 quỹ đạo UTE
Quỹ đạo “UTE” được xây dựng từ 8 quỹ đạo đường thẳng kết hợp 1 quỹ đạo cung
tròn. Để thực hiện nhấc và đặt bút tại các quỹ đạo rời rạc, nhóm thiết kế checkbox Enpoint
trong bảng nhập giá trị tọa độ điểm để người dùng tích chọn, tín hiệu này sẽ được gửi xuống
PLC để thực thi điều khiển trục 3 di chuyển lên xuống, tương tự để gửi tín hiệu kết thúc nội
suy người dùng tích chọn vào checkbox Complete.
Từ đồ thị hình 7.11 vận tốc góc khớp 1,2 có thể thấy vận tốc tăng dần và giảm về 0
trong từng quỹ đạo cơ bản giúp robot hoạt động mượt mà tại giao điểm của 2 quỹ đạo cơ bản
nối tiếp.
❖ Vẽ quỹ đạo Bông Hoa

Hình 7. 12 Tọa độ các điểm biểu diễn quỹ đạo bông hoa

76
 Hình 7. 13 Quỹ đạo bông hoa

Hình 7. 14 Đồ thị góc quay và vận tốc khớp 1 và khớp 2 quỹ đạo bông hoa

Hình 7. 15 Đồ thị vận tốc dài khâu công tác robot quỹ đạo bông hoa

77
 Từ đồ thị 7.14 và 7.15 cho thấy giá trị góc khớp chạy đúng được quỹ đạo bông hoa đề
ra (hình 7. 13), tuy nhiên đáp ứng vận tốc của từng khớp và vận tốc dài của khâu công tác
chưa được tối ưu còn có hiện tượng gây ra hiện tượng ngắt quãng khi chạy thực nghiệm do
quỹ đạo được cấu thành từ nhiều quỹ đạo cung tròn thành phần.
7.2 Nhận xét – đánh giá kết quả
7.2.1 Kết quả đạt được
Sau quá trình chạy thực nghiệm robot theo các quỹ đạo khác nhau, có thể thấy:
- Robot hoạt động đúng với quỹ đạo được quy hoạch.
- Thuật toán quy hoạch quỹ đạo robot có tính linh hoạt cao.
- Tại các điểm khớp đổi chiều quay và vị trí hoàn thành quỹ đạo vận tốc được giảm
gần về 0 giúp robot hoạt động trơn tru.
7.2.2 Nhận xét
Hầu hết các quỹ đạo đều được cấu thành từ hai đường cơ bản là đường thẳng và đường
cung tròn điều này giúp robot có thể linh hoạt trong việc di chuyển bám theo nhiều hình có
biên dạng khác nhau. Dựa vào đồ thị ta thấy, robot có thể hoạt động ở vận tốc cao mà vẫn
đảm bảo được độ chính xác.

78
 KẾT LUẬN – ĐỀ NGHỊ
1. Kết luận
Sau quá trình thực hiện đồ án, nhóm đã hoàn thành được những nhiệm vụ sau:
- Thiết kế và thi công tủ điện điều khiển robot SCARA.
- Thiết kế, chế tạo đồ gá động cơ, đầu công tác và bàn vẽ.
- Hoàn thành bản vẽ cơ khí, sơ đồ đấu nối thiết bị điện.
- Tính toán động học và động lực học robot.
- Thực hiện mô phỏng kiểm chứng động học robot trên phần mềm Matlab.
- Quy hoạch quỹ đạo chuyển động và kiểm tra chạy trên robot thực tế.
- Thiết kế giao diện điều khiển cho robot trên phần mềm trên PC với các chế độ Return
Home, Jogging (chạy từng trục), chạy bám quỹ đạo Single Line và MultiLine.
2. Đề nghị
Hướng phát triển của đề tài trong tương lai:
- Lắp đặt camera để thực hiện xác định vị trí phục vụ cho việc thiết lập thông số ban đầu
và hiệu chỉnh robot chính xác hơn.
- Nghiên cứu, áp dụng thuật toán điều khiển thông minh, thuật toán chỉnh định thông số
động lực học robot.

79
 TÀI LIỆU THAM KHẢO
Tiếng Việt
[1] Phạm Đình Bá, Nguyễn Đình Khiêm, Mai Hùng Tuấn, SỬ DỤNG THUẬT TOÁN
TỐI ƯU HÓA BẦY ĐÀN PSO ĐỂ TỐI ƯU HÓA CÁC THÔNG SỐ CỦA BỘ ĐIỀU
KHIỂN PID SỬ DỤNG CHO ROBOT DÂY SONG SONG, pp. 36-40, Tạp chí Khoa
học Công nghệ Hàng Hải, số 66, 2021.
[2] Nguyễn Trường Thịnh, KỸ THUẬT ROBOT, NXB Đại học Quốc gia TP.HCM,
TP.HCM 2014
Tiếng Anh
[3] D. Kostic, Bram de Jager, M. Steinbuch, R. Hensen, Modeling and identification for
high-performance robot control: an RRR-robotic arm case study, pp. 904 – 919,
Volume: 12, Issue: 6, IEEE Transactions on Control Systems Technology, 2004
[4] Jidong Jia, Minglu Zhang, et al, Dynamic Parameter Identification for a Manipulator
with Joint Torque Sensors Based on an Improved Experimental Design, pp. 2248
Volume 19, Issue: 10, Sensors and Robot Control, 2019
[5] John J. Craig, Introduction to Robotics Mechanics and Control, Pearson Education,
,United States of America, 2005
[6] J. Swevers, C. Ganseman, J. De Schutter, H. Van Brussel, EXPERIMENTAL ROBOT
IDENTIFICATION USING OPTIMISED PERIODIC TRAJECTORIES, pp. 561-
577, Volume 10, Issue 5, Mechanical Systems and Signal Processing, September 1996
[7] Saeed Benjamin Niku, Introduction to Robotics: Analysis, Control, Applications, 2nd
Edition, Pearson Education, United States of America, August 2010
[8] T. Umeno; T. Kaneko; Y. Hori, Robust servosystem design with two degrees of
freedom and its application to novel motion control of robot manipulators, pp. 473-
485, Volume 40, Issue: 5, IEEE Transactions on Industrial Electronics, 1993
[9] Xijie Guo, Lei Zhang and Kai Han, Dynamic parameter identification of robot
manipulators based on the optimal excitation trajectory, pp. pp. 2145-2150, 2018 IEEE
International Conference on Mechatronics and Automation (ICMA), 2018
Nguồn khác
[10] Delta's Automated Inspection Solutions Ensure Notebook Cooling Fan Quality and
Increase Reliability, link https://www.adeltaww.com/en-US/news/12947, 7/2023
[11] Mitsubishi Electric, MR-J3_B Servo Amplifier Instruction manual, link MR-J3-_B
SERVO AMPLIFIER INSTRUCTION MANUAL (mitsubishielectric.com), 2014
[12] Mitsubishi Electric, MELSEC-Q QD75MH Positioning Module User's Manual
(Details) - Mitsubishi Electric Factory Automation, link
https://dl.mitsubishielectric.com/dl/fa/document/manual/ssc/ib0300117/ib0300117b.
pdf, 7/2023
80
[13] Mitsubishi Electric, Catalogue HF-KP13 AC servo, link MELSERVO-J3
(mitsubishielectric.com), 7/2023
[14] Mitsubishi Electric, Q00UCPU - Mitsubishi Electric Factory Automation – EMEA,
link Q00UCPU - Mitsubishi Electric Factory Automation - EMEA, 7/2023
[15] Mitsubishi Electric, QX42 - Mitsubishi Electric Factory Automation - EMEA, link
QX42 - Mitsubishi Electric Factory Automation - EMEA, 7/2023
[16] Mitsubishi Electric, QY42P - Mitsubishi Electric Factory Automation – EMEA, link
QY42P - Mitsubishi Electric Factory Automation - EMEA, 7/2023.
[17] Mitsubishi Electric, QJ71E71-100 - Mitsubishi Electric Factory Automation, link
QJ71E71-100 - Mitsubishi Electric Factory Automation - EMEA, 7/2023
[18] Mitsubishi Electric, I/O Module Type Building Block User's Manual, link I/O
Module Type Building Block User's Manual (mitsubishielectric.com), 7/2023
[19] How to Know When A SCARA Robot is the Right Choice for Your Application, link
How to Know When A SCARA Robot is the Right Choice for Your Application How
to Know When A SCARA Robot is the Right Choice for Your Application
(fanucamerica.com), 7/2023
[20] Khám phá Nhà máy ô tô VinFast hiện đại hàng đầu Đông Nam Á, link Khám phá
Nhà máy ô tô VinFast hiện đại hàng đầu Đông Nam Á - Cộng đồng VinFast Toàn cầu,
7/2023
[21] Robot definitions at ISO - International Federation of Robotics, link International
Federation of Robotics (ifr.org), 7/2023
[22] Thegioiic, CW4L2-20A-R Bộ Lọc Nguồn 1 Pha 20A 115/250V, link CW4L2-20A-
R Bộ Lọc Nguồn 1 Pha 20A 115/250V, Nguồn lọc 115/250 VAC 50/60Hz, 20A
(thegioiic.com), 7/2023

81
PHỤ LỤC

I
S K L 0 0 2 1 5 4