TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI

VIỆN CƠ KHÍ

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

***************

ĐỒ ÁN
THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
ĐỀ TÀI : THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN ROBOT SCARA

Giảng viên hướng dẫn : TS. Nguyễn Trọng Doanh

Sinh viên thực hiện : Trần Lam Trường
MSSV : 20170955
Lớp : CK.CĐT.04 - K62

Hà Nội, tháng 7 năm 2021

ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

LỜI NÓI ĐẦU

Hiện nay, khoa học kỹ thuật về robot nói chung và về robot công nghiệp nói riêng
đang phát triển rất mạnh mẽ và đặc biệt ở các nước có nền kinh tế phát triển, đã góp phần
mang lại hiệu quả kinh tế cao. Việc nghiên cứu chế tạo robot công nghiệp để ứng dụng
trong sản xuất là rất cần thiết.

Việc sử dụng robot công nghiệp trong các nhà máy sản xuất đã làm tăng năng suất,
nâng cao chất lượng của sản phẩm. Nhận thấy vai trò và tầm quan trọng của robot như
vây, ở nước ta hiện này, robot đang được đầu tư nghiên cứu rất nhiều, đặc biệt ở các
trường đại học trong đó có Trường Đại học Bách khoa Hà Nội.

“Đồ án Thiết kế hệ thống cơ điện tử” là một học phần bắt buộc trong chương trình
Kỹ sư Cơ điện tử. Học phần giúp cho sinh viên bước đầu làm quen với việc thiết kế hệ
thống điều khiển của một hệ thống robot cơ bản, ứng dụng các phần mềm như MATLAB
- Simulink, SimMechanics...để tính toán, xây dựng hệ thống điều khiển, mô phỏng
nguyên lý hoạt động robot, giúp cho sinh viên nắm vững được các kiến thức cơ bản của
các học phần và làm quen với nhiệm vụ của người kỹ sư. Có thể nói đây là học phần
không thể thiếu được đối với kỹ sư Cơ điện tử.

Được sự hướng dẫn và giúp đỡ nhiệt tình của TS. Nguyễn Trọng Doanh, em đã
hoàn thành đề tài được giao “Thiết kế hệ thống điều khiển robot SCARA”. Bởi thời gian
và kiến thức có giới hạn, sẽ không tránh khỏi những sai sót. Do vậy em rất mong được
sự chỉ bảo và sự đóng góp ý kiến của thầy để đề tài được hoàn thiện hơn.

Em xin chân thành cảm ơn sự hướng dẫn của TS. Nguyễn Trọng Doanh đã giúp
em hoàn thành đề tài này.

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

Trường
Trần Lam Trường

2
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI SME.EDU - Mẫu 6.a rev1

VIỆN CƠ KHÍ Học kỳ: 2

Bộ môn: Cơ điện tử Năm học: 2020-2021

ĐỒ ÁN MÔN HỌC: THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN Mã HP: ME5512

Thời gian thực hiện: 15 tuần; Mã đề: CĐT02-…

Ngày giao nhiệm vụ: …/…/20…; Ngày hoàn thành: …/…/20…;

Họ và tên sv: Trần Lam Trường MSSV: 20170955 Mã lớp: ………… Chữ ký sv: ……….

Ngày …/…/20… Ngày …/…/20… Ngày …/…/20…

BỘ MÔN CĐT NGƯỜI RA ĐỀ CB Hướng dẫn
(ký, ghi rõ họ tên) (ký, ghi rõ họ tên) (ký, ghi rõ họ tên)

I. Nhiệm vụ thiết kế: Thiết kế hệ thống điều khiển cho Robot SCARA với 2 trục quay và 1
trục tịnh tiến
II. Số liệu cho trước:
Cho một robot có 3 khâu
III. Yêu cầu thiết kế:
_ Khoảng cách trục: L1=350; L2 =350; Miền tịnh tiến: 0 – 400mm; Góc quay của các
khớp: 330o x 294o
- Tốc độ max: 20cm/s
- Độc chính xác lặp: +/- 0.05mm và : +/- 0.1o
- Tải trong max: 0.5 kg
1. Mô hình hóa hệ thống điều khiển
- Mô hình hóa cho một trục chuyển động, xác định hàm truyền
- Xây dựng mô hình hệ thống điều khiển cho toàn bộ robot
2. Mô phỏng và đánh giá tính ổn định, các chỉ tiêu chất lượng

3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

- Sử dụng MATLAB-Simulink để mô phỏng động lực học cho một trục chuyển động và cho toàn
bộ robot
- Đánh giá tính ổn định
- Xác định sai lệch tĩnh
3. Thiết kế hệ thống điều khiển
- Chọc các phần tử của hệ thống điều khiển
- Xác định các hàm điều khiển
- Xây dựng bản vẽ mạch
4. Mô phỏng nguyên lý hoạt động (Tự chọn, có thể sử dụng Solidworks hoặc SimMechanic)

4
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

MỤC LỤC
LỜI NÓI ĐẦU ......................................................................................................... 2

MỤC LỤC ............................................................................................................... 5

DANH MỤC HÌNH ẢNH ....................................................................................... 7

CHƯƠNG 1: TỔNG QUAN HỆ THỐNG ĐIỀU KHIỂN .....................................10

1.1. TỔNG QUAN VỀ HỆ THỐNG................................................................................ 10

1.2. NGUYÊN LÝ HOẠT ĐỘNG VÀ THÔNG SỐ KỸ THUẬT ........................................... 16

CHƯƠNG 2: ĐỘNG HỌC, ĐỘNG LỰC HỌC VÀ THIẾT KẾ QUỸ ĐẠO

CHUYỂN ĐỘNG ...................................................................................................18

2.1. KHẢO SÁT ĐỘNG HỌC ....................................................................................... 18

2.2. THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG TRONG KHÔNG GIAN THAO TÁC................. 22
2.3. TÍNH TOÁN ĐỘNG LỰC HỌC .............................................................................. 27

CHƯƠNG 3: MÔ HÌNH HÓA HỆ THỐNG ĐIỀU KHIỂN .................................30

3.1. MÔ HÌNH HÓA MỘT TRỤC CHUYỂN ĐỘNG, XÁC ĐỊNH HÀM TRUYỀN .................. 30
3.1.1. Mô hình hóa động cơ điện một chiều ....................................................... 30
3.1.2. Mô hình hóa các khâu .............................................................................. 37
3.2. XÂY DỰNG MÔ HÌNH HỆ THỐNG ĐIỀU KHIỂN CHO TOÀN BỘ ROBOT .................. 47

CHƯƠNG 4: MÔ PHỎNG VÀ ĐÁNH GIÁ TÍNH ỔN ĐỊNH, CÁC CHỈ TIÊU

CHẤT LƯỢNG ......................................................................................................51

4.1. SỬ DỤNG MATLAB-SIMULINK ĐỂ MÔ PHỎNG ĐỘNG LỰC HỌC CHO MỘT TRỤC
CHUYỂN ĐỘNG VÀ CHO TOÀN BỘ ROBOT ................................................................. 51
4.2. ĐÁNH GIÁ TÍNH ỔN ĐỊNH, XÁC ĐỊNH SAI LỆCH TĨNH ......................................... 52

CHƯƠNG 5: THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN ..........................................56

5
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

5.1. THIẾT KẾ BỘ ĐIỀU KHIỂN PID .......................................................................... 56

5.2. LỰA CHỌN CÁC PHẦN TỬ CHO HỆ THỐNG ĐIỀU KHIỂN...................................... 66
5.3. CÁC MẠCH ĐIỆN VÀ GHÉP NỐI PHẦN TỬ ........................................................... 74

KẾT LUẬN ............................................................................................................79

DANH MỤC TÀI LIỆU THAM KHẢO ................................................................80

6
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

DANH MỤC HÌNH ẢNH

Hình 1. Robot SCARA ................................................................................................... 10

Hình 2. Khâu đế .............................................................................................................. 11
Hình 3. Khâu 1 ................................................................................................................ 11
Hình 4. Khâu 2 ................................................................................................................ 12
Hình 5. Khâu 3 - Cơ cấu vít me bi.................................................................................. 12
Hình 6. Động cơ Servo BPTA007 .................................................................................. 13
Hình 7. Công tắc hành trình............................................................................................ 13
Hình 8. PLC Siemens S7-1200 ....................................................................................... 14
Hình 9. Vi điều khiển STM32F407 ................................................................................ 14
Hình 10. Mạch điện tử trong robot ................................................................................. 15
Hình 11. HMI Siemens TOUCH Panel TP 177A .......................................................... 16
Hình 12. Sơ đồ khối mô tả nguyên lý điều khiển robot ................................................. 16
Hình 13. Hệ tọa độ Denavit-Hartenberg......................................................................... 18
Hình 14. Quỹ đạo thực hiện gắp và thả vật .................................................................... 23
Hình 15. Quỹ đạo biến khớp khâu 1 ............................................................................... 25
Hình 16. Quỹ đạo biến khớp khâu 2 ............................................................................... 25
Hình 17. Quỹ đạo biến khớp khâu 3 ............................................................................... 26
Hình 18. Hệ tọa độ khối tâm........................................................................................... 27
Hình 19. Sơ đồ khớp robot hoàn chỉnh bao gồm hộp số ................................................ 30
Hình 20. Sơ đồ nguyên lý động cơ điện DC................................................................... 32
Hình 21. Sơ đồ khối mạch điện phần ứng ...................................................................... 33
Hình 22. Sơ đồ khối phần momen điện từ ...................................................................... 33
Hình 23. Sơ đồ khối phần cần bằng momen trên động cơ ............................................. 34
Hình 24. Sơ đồ khối động cơ với tín hiệu vào điện áp và tín hiệu ra vận tốc ................ 34
Hình 25. Sơ đồ khối động cơ với tín hiệu vào điện áp và tín hiệu ra góc quay ............. 36
7
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Hình 26. Sơ đồ trục vít me - đai ốc bi ............................................................................ 37

Hình 27. Sơ đồ khối trục vít me - đai ốc bi .................................................................... 37
Hình 28. Sơ đồ khối khâu 3 ............................................................................................ 38
Hình 29. Tính toán momen quán tính khâu 3 ................................................................. 39
Hình 30. Đồ thị Nyquist của hàm truyền khâu 3 ............................................................ 41
Hình 31. Tính toán momen quán tính khâu 2 ................................................................. 42
Hình 32. Đồ thị Nyquist hàm truyền khâu 2 .................................................................. 44
Hình 33. Tính toán momen quán tính khâu 1 ................................................................. 45
Hình 34. Đồ thị Nyquist của hàm truyền khâu 1 ............................................................ 47
Hình 35. Trình tự xây dựng mô hình hệ thống điều khiển ............................................. 47
Hình 36. Mô hình CAD của robot trong MATLAB....................................................... 48
Hình 37. Khối 3D Scara robot ........................................................................................ 49
Hình 38. Mô hình hệ thống điều khiển toàn bộ robot .................................................... 49
Hình 39. Khối Position Reference .................................................................................. 50
Hình 40. Khối động cơ DC ............................................................................................. 50
Hình 41. Mô hình simulink cho từng khâu ..................................................................... 51
Hình 42. Sơ đồ khối tính sai lệch tĩnh với tín hiệu vào u(t) = 1(t) ................................. 52
Hình 43. Đồ thị đáp ứng khâu 1 ..................................................................................... 53
Hình 44. Đồ thị đáp ứng khâu 2 ..................................................................................... 54
Hình 45. Đồ thị đáp ứng khâu 3 ..................................................................................... 55
Hình 46. Mô hình bộ điều khiển PID ............................................................................. 56
Hình 47. Sơ đồ khối bộ điều khiển PID ......................................................................... 57
Hình 48. Sơ đồ khối PID điều khiển động cơ................................................................. 57
Hình 49. Giao diện công cụ PID Tuner trên MATLAB ................................................. 58
Hình 50. Đồ thị đáp ứng khâu 1 với bộ điều khiển PID ................................................. 59
Hình 51. Đồ thị đáp ứng khâu 2 với bộ điều khiển PID ................................................. 60
Hình 52. Đồ thị đáp ứng khâu 3 với bộ điều khiển PID ................................................. 61

8
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Hình 53. Giá trị biến khớp khâu 1 .................................................................................. 62

Hình 54. Sai số biến khớp khâu 1 ................................................................................... 62
Hình 55. Vận tốc khâu 1 ................................................................................................. 63
Hình 56. Giá trị biến khớp khâu 2 .................................................................................. 63
Hình 57. Sai số biến khớp khâu 2 ................................................................................... 64
Hình 58. Vận tốc khâu 2 ................................................................................................. 64
Hình 59. Giá trị biến khớp khâu 3 .................................................................................. 65
Hình 60. Sai số biến khớp khâu 3 ................................................................................... 65
Hình 61. Vận tốc khâu 3 ................................................................................................. 66
Hình 62. Servo Motor + Driver Yaskawa ...................................................................... 68
Hình 63. Thông số kỹ thuật của dòng servo motor SGMAV ......................................... 68
Hình 64. Giao diện của servo driver SGDV-5R5A ........................................................ 69
Hình 65. Cổng kết nối CN1 ............................................................................................ 70
Hình 66. Sơ đồ các chân I/O của servo driver SGDV-5R5A ......................................... 71
Hình 67. Phương pháp xuất xung điều khiển servo ở chế độ điều khiển vị trí .............. 72
Hình 68. Board vi điều khiển STM32F407 DISCOVERY ............................................ 73
Hình 69. Sơ đồ ghép nối nguồn với 3 servo driver ........................................................ 75
Hình 70. Kết nối động cơ với driver ............................................................................... 76
Hình 71. Sơ đồ ghép nối servo driver ở chế độ điều khiển vị trí ................................... 77

9
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

CHƯƠNG 1: TỔNG QUAN HỆ THỐNG ĐIỀU KHIỂN

1.1. Tổng quan về hệ thống
Mục tiêu thiết kế
Dựa trên cơ sở đã hoàn thành Đồ án Thiết kế hệ thống cơ khí – Robot SCARA
3 bậc tự do, ta tiếp tục chỉnh sửa và hoàn thành những công việc sau:
• Điều chỉnh hệ thống cơ khí theo yêu cầu của bài toán điều khiển.
• Mô hình hóa hệ thống điều khiển cho robot, mô phỏng động lực học cho robot.
• Thiết kế bộ điều khiển, đánh giá các chỉ tiêu ổn định.
• Lựa chọn các phần tử của hệ thống điều khiển. Xây dựng mạch điện cho robot.
Thiết kế cơ khí

Hình 1. Robot SCARA

10
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Hình 2. Khâu đế

Hình 3. Khâu 1

11
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Hình 4. Khâu 2

Hình 5. Khâu 3 - Cơ cấu vít me bi

12
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Một số linh kiện cơ bản của hệ thống điều khiển

• Động cơ Servo
Động cơ Servo đóng vai trò là thành phần động lực chính của robot. Chúng
biến điện năng thành cơ năng để robot thực hiện các chuyển động.

Hình 6. Động cơ Servo BPTA007

• Cảm biến vị trí (Công tắc hành trình)

Công tắc hành trình để giới hạn hành trình của bộ phận chuyển động, nhận tín
hiệu khi có tác động vật lý vào bộ phận cơ khí của cảm biến, chuyển thành tín hiệu
điện điều khiển cơ cấu chấp hành.

Hình 7. Công tắc hành trình

13
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

• Bộ điều khiển logic khả trình (PLC)

PLC là bộ điều khiển logic khả trình. Người dùng hoàn toàn có thể thay đổi
thuật toán điều khiển bằng việc lập trình PLC. Trong hệ thống robot, PLC đóng vai
trò như là “bộ não” giúp tính toán và điều khiển mọi hoạt động của robot.

Hình 8. PLC Siemens S7-1200

• Vi điều khiển (Micro Controller Unit)

Ngoài PLC, các hệ thống robot còn có thể sử dụng các vi điều khiển trong vai
trò “bộ não”. So với PLC thì vi điều khiển đòi hỏi kết nối đơn giản hơn, nhỏ họn hơn
tuy nhiên độ bền và chống nhiễu kém hơn khi làm việc trong môi trường công nghiệp.

Hình 9. Vi điều khiển STM32F407

14
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

• Mạch điện tử
Mạch điện tử trong hệ thống robot đóng vai trò như là mạch màu trong cơ thể
người. Nó đóng vai trò kết nối các bộ phận của robot thành một thể thống nhất. Mạch
điện tử có thể bao gồm:
- Mạch vi điều khiển: Đóng vai trò như một bộ não của robot, điều khiển
toàn bộ hoạt động của robot theo chương trình đã lập trình sẵn.
- Mạch cảm biến: Đóng vai trò như các giác quan của robot, giúp robot nhận
diện môi trường xung quanh để gửi tín hiệu về vi điều khiển.
- Mạch công suất: Có chức năng điều khiển công suất, tốc độ của động cơ,
dùng để điều khiển hoạt động các cơ cấu của robot.

Hình 10. Mạch điện tử trong robot

• Thiết bị giao diện người – máy (HMI)

Thông thường trên các robot sẽ trang bị một thiết bị giúp giao tiếp giữa
người và robot (HMI). Chủng loại HMI đa dạng từ phương thức thao tác: nút bấm,
cảm ứng hoặc phối hợp cả hai và nhiều kích cỡ tùy vào nhu cầu và đặc điểm thực
tế. Nhờ có HMI mà quá trình điều khiển & giám sát robot trở nên trực quan và tiện
lợi hơn.
15
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Hình 11. HMI Siemens TOUCH Panel TP 177A

1.2. Nguyên lý hoạt động và thông số kỹ thuật

Nguyên lý hoạt động

Hình 12. Sơ đồ khối mô tả nguyên lý điều khiển robot

Nguyên lý hoạt động của robot:

• Tùy vào yêu cầu nhiệm vụ và chương trình đã được lập trình sẵn, bộ điều khiển
sẽ xuất tín hiệu điều khiển tới các cơ cấu chấp hành thực hiện chuyển động,
thao tác (động cơ quay làm cơ cấu quay, tay kẹp mở hay mỏ hàn bắt đầu hàn…)

16
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

• Sau đó hệ thống cảm biến sẽ phản hồi tín hiệu lại cho bộ điều khiển để điều
chỉnh và xử lý những sai lệch (nếu có). Mục tiêu của việc sử dụng hệ thống
điều khiển vòng kín (closed-loop) là để triệu tiêu tín hiệu nhiễu và kiểm soát
được hoạt động của robot, đảm bảo robot hoạt động chính xác theo yêu cầu.
Thông số kỹ thuật
• Số bậc tự do: 3 (2 khớp quay, 1 khớp tịnh tiến)
• Khoảng cách các trục quay: a1 = 350 mm, a2 = 350 mm
• Chiều cao khâu 1 so với mặt đất: d1 = 340 mm
• Khoảng cách trục khâu 2 so với khâu 1: d2 = 110 mm
• Góc quay các khớp: 330°x294°
• Miền tịnh tiến: 0 – 400mm
• Tốc độ tối đa: 20cm/s
• Độ chính xác lặp: ±0.05mm và ±0.1°
• Tải trọng đối đa: 0,5kg

17
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

CHƯƠNG 2: ĐỘNG HỌC, ĐỘNG LỰC HỌC VÀ THIẾT

KẾ QUỸ ĐẠO CHUYỂN ĐỘNG
2.1. Khảo sát động học
Động học thuận
Ta tiến hành tính toán động học thuận cho robot bằng phương pháp Denavit-
Hartenberg. Từ các biến khớp đã cho, ta tính được trạng thái khâu cuối của robot (vị
trí, hướng, vận tốc và gia tốc).
Do bài toán động học robot đã được khảo sát ở Đồ án Thiết kế hệ thống cơ khí –
Robot nên sau đây em xin được rút gọn quy trình tính toán và chỉ đưa ra kết quả bài
toán.
Hệ tọa độ và các ma trận Denavit-Hartenberg

Hình 13. Hệ tọa độ Denavit-Hartenberg

Bảng D-H
18
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Khâu θ d a α
1 𝑞1 𝑑1 𝑎1 0
2 𝑞2 𝑑2 𝑎2 0
3 0 q3 0 0

Các ma trận biến đổi thuần nhất

𝑐𝑜𝑠( 𝑞1 ) − 𝑠𝑖𝑛( 𝑞1 ) 0 𝑎1 𝑐𝑜𝑠( 𝑞1 )
𝑠𝑖𝑛( 𝑞1 ) 𝑐𝑜𝑠( 𝑞1 ) 0 𝑎1 𝑠𝑖𝑛( 𝑞1 )
𝑇10 = [ ]
0 0 1 𝑑1
0 0 0 1
𝑐𝑜𝑠( 𝑞2 ) − 𝑠𝑖𝑛( 𝑞2 ) 0 𝑎2 𝑐𝑜𝑠( 𝑞2 )
𝑠𝑖𝑛( 𝑞2 ) 𝑐𝑜𝑠( 𝑞2 ) 0 𝑎2 𝑠𝑖𝑛( 𝑞2 )
𝑇21 = [ ]
0 0 1 𝑑2
0 0 0 1
1 0 0 0
0 1 0 0
𝑇32 = [ ]
0 0 1 𝑞3
0 0 0 1
𝑐𝑜𝑠( 𝑞1 + 𝑞2 ) − 𝑠𝑖𝑛( 𝑞1 + 𝑞2 ) 0 𝑎2 𝑐𝑜𝑠( 𝑞1 + 𝑞2 ) + 𝑎1 𝑐𝑜𝑠( 𝑞1 )
𝑠𝑖𝑛( 𝑞1 + 𝑞2 ) 𝑐𝑜𝑠( 𝑞1 + 𝑞2 ) 0 𝑎2 𝑠𝑖𝑛( 𝑞1 + 𝑞2 ) + 𝑎1 𝑠𝑖𝑛( 𝑞1 )
𝑇20 = [ ]
0 0 1 𝑑2 + 𝑑1
0 0 0 1
𝑐𝑜𝑠( 𝑞1 + 𝑞2 ) − 𝑠𝑖𝑛( 𝑞1 + 𝑞2 ) 0 𝑎2 𝑐𝑜𝑠( 𝑞1 + 𝑞2 ) + 𝑎1 𝑐𝑜𝑠( 𝑞1 )
𝑠𝑖𝑛( 𝑞1 + 𝑞2 ) 𝑐𝑜𝑠( 𝑞1 + 𝑞2 ) 0 𝑎2 𝑠𝑖𝑛( 𝑞1 + 𝑞2 ) + 𝑎1 𝑠𝑖𝑛( 𝑞1 )
𝑇30 = [ ]
0 0 1 𝑞3 + 𝑑2 + 𝑑1
0 0 0 1

19
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Phương trình động học robot

𝑎11 (𝑞) 𝑎12 (𝑞) 𝑎13 (𝑞) 𝑥(𝑞) 𝑐11 (𝛼, 𝛽, 𝜂) 𝑐12 (𝛼, 𝛽, 𝜂) 𝑐13 (𝛼, 𝛽, 𝜂) 𝑥𝐸
𝑎 (𝑞) 𝑎22 (𝑞) 𝑎23 (𝑞) 𝑦(𝑞) 𝑐 (𝛼, 𝛽, 𝜂) 𝑐22 (𝛼, 𝛽, 𝜂) 𝑐23 (𝛼, 𝛽, 𝜂) 𝑦𝐸
[ 21 ] = [ 21 ]
𝑎31 (𝑞) 𝑎32 (𝑞) 𝑎33 (𝑞) 𝑧(𝑞) 𝑐31 (𝛼, 𝛽, 𝜂) 𝑐32 (𝛼, 𝛽, 𝜂) 𝑐33 (𝛼, 𝛽, 𝜂) 𝑧𝐸
0 0 0 1 0 0 0 1
Kết quả bài toán động học thuận
Tọa độ điểm tác động cuối:
𝑥𝐸 = 𝑥(𝑞) 𝑥𝐸 = 𝑎2 𝑐𝑜𝑠( 𝑞1 + 𝑞2 ) + 𝑎1 𝑐𝑜𝑠( 𝑞1 )
{𝑦𝐸 = 𝑦(𝑞) => {𝑦𝐸 = 𝑎2 𝑠𝑖𝑛( 𝑞1 + 𝑞2 ) + 𝑎1 𝑠𝑖𝑛( 𝑞1 )
𝑧𝐸 = 𝑧(𝑞) 𝑧𝐸 = 𝑞3 + 𝑑2 + 𝑑1
Xác định hướng của khâu thao tác:
𝑐11 (𝛼, 𝛽, 𝜂) = 𝑎11 (𝑞) 𝑐𝑜𝑠 𝛽 𝑠𝑖𝑛 𝜂 = 𝑐𝑜𝑠( 𝑞1 + 𝑞2 )
{𝑐13 (𝛼, 𝛽, 𝜂) = 𝑎13 (𝑞) => { 𝑠𝑖𝑛 𝛽 = 0
𝑐23 (𝛼, 𝛽, 𝜂) = 𝑎23 (𝑞) − 𝑠𝑖𝑛 𝛼 𝑐𝑜𝑠 𝛽 = 0

𝑠𝑖𝑛 𝛽 = 0 𝛼=0
=> => {𝛽 = 0
𝜋
𝑠𝑖𝑛 𝛼 = 0 𝜂 = 𝑞1 + 𝑞2 −
𝑐𝑜𝑠(𝑞1 +𝑞2 ) 𝜋 2
{𝑠𝑖𝑛 𝜂 = = 𝑠𝑖𝑛( 𝑞1 + 𝑞2 − )
𝑐𝑜𝑠 𝛽 2

Ma trận Jacobi tịnh tiến của khâu cuối:

−𝑎2 𝑠12 − 𝑎1 𝑠1 −𝑎2 𝑠12 0
𝐽𝑇𝐸 = [ 𝑎2 𝑐12 + 𝑎1 𝑐1 𝑎2 𝑐12 0]
0 0 1
Vận tốc dài điểm tác động cuối:
[−𝑎2 𝑠12 − 𝑎1 𝑠1 ] 𝑞̇ 1 − 𝑎2 𝑠12̇ 𝑞2
(0)
𝑣3 = 𝐽𝑇𝐸 (𝑞). 𝑞̇ = [ [𝑎2 𝑐12 + 𝑎1 𝑐1 ] 𝑞̇ 1 + 𝑎2 𝑐12̇ 𝑞2 ]
𝑞̇ 3
Gia tốc dài điểm cuối:

20
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

(0) [−𝑎2 𝑠12 − 𝑎1 𝑠1 ] 𝑞̈ 1 − 𝑎2 𝑠12̈ 𝑞2

(0) 𝑑(𝑣3 )
𝑎3 = = [ [𝑎2 𝑐12 + 𝑎1 𝑐1 ] 𝑞̈ 1 + 𝑎2 𝑐12̈ 𝑞2 ]
𝑑𝑡
𝑞̈ 3
Vector vận tốc góc khâu 3:
0 −𝑞̇ 1 − 𝑞̇ 2 0
(0) 𝑇
𝜔
̃3 = 𝐴̇03 . 𝐴03 = [𝑞̇ 1 + 𝑞̇ 2 0 0]
0 0 0
Vận tốc góc khâu 3 là :
(0)
𝜔3 = [0 0 𝑞̇ 1 + 𝑞̇ 2 ]𝑇
Vector vận tốc khâu 1:
0 −𝑞̇ 1 0
(0) ̇ 0 0𝑇
𝜔
̃1 = 𝐴1 . 𝐴1 = [𝑞̇ 1 0 0]
0 0 0
Vận tốc góc khâu 1 là:
(0)
𝜔1 = [0 0 𝑞̇ 1 ]𝑇
Vector vận tốc góc khâu 2:
0 −𝑞̇ 1 − 𝑞̇ 2 0
(0) ̇ 0 0𝑇
𝜔
̃2 = 𝐴2 . 𝐴2 = [𝑞̇ 1 + 𝑞̇ 2 0 0]
0 0 0
Vận tốc góc khâu 2:
(0)
𝜔2 = [0 0 𝑞̇ 1 + 𝑞̇ 2 ]𝑇
Động học ngược
Kết quả bài toán động học ngược vị trí:
𝑎2 [𝑦𝐸 𝑐𝑜𝑠( 𝑞2 ) − 𝑥𝐸 𝑠𝑖𝑛( 𝑞2 )] + 𝑎1 𝑦𝐸
𝑞1 = 𝑎𝑟𝑐𝑡𝑎𝑛 ( ) (𝑟𝑎𝑑)
𝑎2 [𝑥𝐸 𝑐𝑜𝑠( 𝑞2 ) + 𝑦𝐸 𝑠𝑖𝑛( 𝑞2 )] + 𝑎1 𝑥𝐸
𝑥𝐸 2 + 𝑦𝐸 2 − 𝑎2 2 − 𝑎1 2
𝑞2 = 𝑎𝑟𝑐𝑐𝑜𝑠 ( ) (𝑟𝑎𝑑)
2𝑎1 𝑎2
{𝑞3 = 𝑧𝐸 − 𝑑2 − 𝑑1 (𝑚)

21
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

2.2. Thiết kế quỹ đạo chuyển động trong không gian thao tác
Không gian làm việc của robot
Các tham số động học:
𝑎1 = 350(𝑚𝑚) = 0,35(𝑚)
𝑎 = 350(𝑚𝑚) = 0,35(𝑚)
{ 2
𝑑1 = 340(𝑚𝑚) = 0,34(𝑚)
𝑑2 = 110(𝑚𝑚) = 0,11(𝑚)
Miền giá trị các biến khớp:
𝑞1 ∈ [−165∘ ; 165∘ ]
{𝑞2 ∈ [−147∘ ; 147∘ ]
𝑞3 ∈ [0; 400𝑚𝑚]
Kết quả bài toán động học thuận:
𝑥𝐸 = 𝑎2 𝑐𝑜𝑠( 𝑞1 + 𝑞2 ) + 𝑎1 𝑐𝑜𝑠( 𝑞1 )
{𝑦𝐸 = 𝑎2 𝑠𝑖𝑛( 𝑞1 + 𝑞2 ) + 𝑎1 𝑠𝑖𝑛( 𝑞1 )
𝑧𝐸 = 𝑞3 + 𝑑2 + 𝑑1
Không gian làm việc của robot được thể hiện trên phần mềm MATLAB như sau
Cơ sở thiết kế quỹ đạo
Đối với robot SCARA thực hiện nhiệm vụ gắp và thả vật, ta sẽ lựa chọn quỹ
đạo di chuyển là đường thẳng. Việc chọn quỹ đạo đường thẳng có 2 lợi điểm, nó vừa
đơn giản hóa việc tính toán, thiết kế, vừa tiết kiệm năng lượng khi di chuyển so với
các dạng quỹ đạo hình học khác.
Phương trình đường thẳng đi qua 2 điểm 𝑀(𝑥0 ; 𝑦0 ; 𝑧𝑜 ) và 𝑁(𝑥𝑒 ; 𝑦𝑒 ; 𝑧𝑒 ) là:
𝑥 − 𝑥0 𝑦 − 𝑦0 𝑧 − 𝑧0
= =
𝑥𝑒 − 𝑥0 𝑦𝑒 − 𝑦0 𝑧𝑒 − 𝑧0
Độ dời của điểm thao tác E theo phương x, y, z là đa thức bậc 3 theo thời gian:
𝑥 = 𝑎0 + 𝑎1 𝑡 + 𝑎2 𝑡 2 + 𝑎3 𝑡 3
{
𝑥̇ = 𝑎1 + 2𝑎2 𝑡 + 3𝑎3 𝑡 2

22
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

𝑦𝑒 − 𝑦0 𝑦0 𝑥𝑒 − 𝑦𝑒 𝑥0
𝑦= 𝑥+
𝑥𝑒 − 𝑥0 𝑥𝑒 − 𝑥0
𝑧𝑒 − 𝑧0 𝑧0 𝑥𝑒 − 𝑧𝑒 𝑥0
𝑧= 𝑥+
𝑥𝑒 − 𝑥0 𝑥𝑒 − 𝑥0
Với thời gian đặt cho quỹ đạo là 𝑡𝑒 (𝑠) để đi từ M đến N theo một đường thẳng.
Các điều kiện biên:
𝑥𝐸 (0) = 𝑥𝑜
𝑥̇ (0) = 𝑥̇ 𝑜 = 0
{ 𝐸
𝑥𝐸 (𝑡𝑒 ) = 𝑥𝑒
𝑥̇ 𝐸 (𝑡𝑒 ) = 𝑥̇ 𝑒 = 0
Ta xác định được các hệ số như sau:
𝑎0 = 𝑥0
𝑎1 = 0
3(𝑥𝑒 − 𝑥0 )
𝑎2 =
𝑡𝑒2
−2(𝑥𝑒 − 𝑥0 )
𝑎3 =
{ 𝑡𝑒3
Bài toán thiết kế quỹ đạo
Robot SCARA thường được áp dụng trong các hệ thống phân loại sản phẩm,
nên quỹ đạo thực hiện được đề xuất như sau:

Hình 14. Quỹ đạo thực hiện gắp và thả vật

23
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Đây là quỹ đạo để thực hiện gắp 1 vật từ vị trí A sang vị trí B :
• Di chuyển đầu hút từ S ra 𝐴′ trong 2 giây (với S là vị trí ban đầu của Robot)
• Di chuyển từ 𝐴′ xuống 𝐴 trong 2 giây và dừng lại 1 giây để thực hiện gắp
mạch
• Di chuyển từ A lên A’ trong 2 giây
• Di chuyển từ A’ sang B’ trong 2 giây
• Di chuyển từ B’ xuống B (vị trí thả mạch) trong 2 giây và dừng 1 giây để
thả mạch
• Di chuyển từ B về B’ trong 2 giây
• Di chuyển từ B’ về A mới để thực hiện chu trình tiếp theo .

Tọa độ các điểm:

𝑆(700; 0; 450) là điểm khởi động của robot.
𝐴′(0; 400; 350)
𝐴(0; 400; 100)
𝐵(400; 0; 100)
𝐵′(400; 0; 350)
Quỹ SA’ A’A Dừng gắp AA’ A’B’ B’B Dừng thả BB’
đạo mạch mạch
Thời 2 2 1 2 3 2 1 2
gian(s)

24
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Hình 15. Quỹ đạo biến khớp khâu 1

Hình 16. Quỹ đạo biến khớp khâu 2

25
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Hình 17. Quỹ đạo biến khớp khâu 3

26
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

2.3. Tính toán động lực học

Hình 18. Hệ tọa độ khối tâm

Bài toán động lực học robot SCARA đã được khảo sát ở Đồ án Thiết kế hệ
thống cơ khí – Robot. Sau đây em sẽ rút gọn quy trình tính toán và chỉ đưa ra kết quả
bài toán.
Phương trình vi phân chuyển động của robot có dạng:
𝑀(𝑞)𝑞̈ + 𝐶(𝑞, 𝑞̇ )𝑞̇ + 𝐺(𝑞) = 𝑄

27
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Ma trận khối lượng M

3 𝑚11 (𝑞) 𝑚12 (𝑞) 𝑚13 (𝑞)
𝑇 𝑇 (𝑘) 𝑇
𝑀(𝑞) = ∑(𝐽𝑇𝑘 𝑚𝑘 𝐽𝑇𝑘 + 𝐽𝑅𝑘 𝐴0𝑘 𝐼𝐶𝑘 𝐴0𝑘 𝐽𝑅𝑘 ) = [𝑚21 (𝑞) 𝑚22 (𝑞) 𝑚23 (𝑞)]
𝑘=1 𝑚31 (𝑞) 𝑚32 (𝑞) 𝑚33 (𝑞)
𝑎1 2 𝑎2 2
𝑚11 (𝑞) = 𝐼𝑧𝑧1 + 𝐼𝑧𝑧2 + 𝐼𝑧𝑧3 + 𝑚1 + [𝑎1 2 + + 𝑎1 𝑎2 𝑐𝑜𝑠( 𝑞2 )] 𝑚2
4 4
+ [𝑎1 2 + 𝑎2 2 + 2𝑎1 𝑎2 𝑐𝑜𝑠( 𝑞2 )]𝑚3
𝑚12 (𝑞) = 𝑚21 (𝑞)
1 𝑎2 2
= 𝐼𝑧𝑧2 + 𝐼𝑧𝑧3 + [ 𝑎1 𝑎2 𝑐𝑜𝑠( 𝑞2 ) + ] 𝑚2
2 4
+ [𝑎2 2 + 𝑎1 𝑎2 𝑐𝑜𝑠( 𝑞2 )]𝑚3
𝑎2 2
𝑚22 (𝑞) = 𝐼𝑧𝑧2 + 𝐼𝑧𝑧3 + 𝑚 + 𝑎2 2 𝑚3
4 2
𝑚33 (𝑞) = 𝑚3
𝑚13 (𝑞) = 𝑚31 (𝑞) = 𝑚23 (𝑞) = 𝑚32 (𝑞) = 0
Ma trận Coriolis C
𝑐11 (𝑞, 𝑞̇ ) 𝑐12 (𝑞, 𝑞̇ ) 𝑐13 (𝑞, 𝑞̇ )
𝐶(𝑞, 𝑞̇ ) = [𝑐21 (𝑞, 𝑞̇ ) 𝑐22 (𝑞, 𝑞̇ ) 𝑐23 (𝑞, 𝑞̇ )]
𝑐31 (𝑞, 𝑞̇ ) 𝑐32 (𝑞, 𝑞̇ ) 𝑐33 (𝑞, 𝑞̇ )
𝑚2
𝑐11 (𝑞, 𝑞̇ ) = − ( + 𝑚3 ) 𝑎1 𝑎2 𝑠𝑖𝑛( 𝑞2 )𝑞̇ 2
2
𝑚2 𝑚2
𝑐12 (𝑞, 𝑞̇ ) = − ( + 𝑚3 ) 𝑎1 𝑎2 𝑠𝑖𝑛( 𝑞2 )𝑞̇ 1 − ( + 𝑚3 ) 𝑎1 𝑎2 𝑠𝑖𝑛( 𝑞2 )𝑞̇ 2
2 2
𝑚2
𝑐21 (𝑞, 𝑞̇ ) = ( + 𝑚3 ) 𝑎1 𝑎2 𝑠𝑖𝑛( 𝑞2 )𝑞̇ 1
2
𝑐13 (𝑞, 𝑞̇ ) = 𝑐22 (𝑞, 𝑞̇ ) = 𝑐23 (𝑞, 𝑞̇ ) = 𝑐31 (𝑞, 𝑞̇ ) = 𝑐32 (𝑞, 𝑞̇ ) = 𝑐33 (𝑞, 𝑞̇ ) = 0

28
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Ma trận trọng lượng G

3
𝑇
𝐺(𝑞) = − ∑ 𝑚𝑖 𝐽𝑇𝑖 (𝑞)𝑔(0) = [0 0 −𝑚3 𝑔]𝑇
𝑖=1

Lực suy rộng của các lực không thế Q

(0)
𝑄 = 𝑈 + 𝐽𝐸𝑇 𝐹𝐸
(0)
Trong đó: + Lực tác động lên khâu cuối là 𝐹𝐸 = [0 0 −𝐹𝑧 ]𝑇
+ 𝑈 = [𝜏1 𝜏2 𝜏3 ]𝑇 là lực dẫn động  i của động cơ đặt tại các khớp.
0 𝜏1
Vậy: 𝑄 = [ 0 ] + [𝜏2 ] là vector lực suy rộng cần xác định.
−𝐹𝑧 𝜏3
Kết luận
Kết quả của bài toán động lực học:
𝑚11 (𝑞) 𝑚12 (𝑞) 0 𝑞̈ 1 𝑐11 (𝑞, 𝑞̇ ) 𝑐12 (𝑞, 𝑞̇ ) 0 𝑞̇ 1 0 0
[𝑚21 (𝑞) 𝑚22 (𝑞) 0 ] [𝑞̈ 2 ] + [𝑐21 (𝑞, 𝑞̇ ) 0 𝑞̇
0] [ 2 ] + [ 0 ] + [0]
0 0 𝑚33 (𝑞) 𝑞̈ 3 0 0 0 𝑞̇ 3 −𝑚3 𝑔 𝐹𝑧
𝜏1
𝜏
= [ 2]
𝜏3
Hay lực điều khiển tại các khớp:
𝜏1 𝑚11 (𝑞)𝑞̈ 1 + 𝑚12 (𝑞)𝑞̈ 2 + 𝑐11 (𝑞, 𝑞̇ )𝑞̇ 1 + 𝑐12 (𝑞, 𝑞̇ )𝑞̇ 2
𝜏 = [𝜏2 ] = [ 𝑚21 (𝑞)𝑞̈ 1 + 𝑚22 (𝑞)𝑞̈ 2 + 𝑐21 (𝑞, 𝑞̇ )𝑞̇ 1 ]
𝜏3 𝑚33 (𝑞)𝑞̈ 3 − 𝑚3 𝑔 + 𝐹𝑧

29
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

CHƯƠNG 3: MÔ HÌNH HÓA HỆ THỐNG ĐIỀU KHIỂN

3.1. Mô hình hóa một trục chuyển động, xác định hàm truyền
3.1.1. Mô hình hóa động cơ điện một chiều
Phương trình về cơ

Hình 19. Sơ đồ khớp robot hoàn chỉnh bao gồm hộp số

𝐽𝑚 là momen quán tính của động cơ

𝐽𝑙 là momen quán tính của khâu gắn trên khớp đó
𝜏𝑚 là momen ở trục động cơ
𝜏𝑙 là momen tải quán tính
𝜃𝑚 là góc quay ở trục động cơ
𝜃𝑠 là góc quay ở tải
𝐵𝑚 là hệ số giảm chấn của động cơ
𝐵𝑙 là hệ số giảm chấn của tải

Hệ số giảm tốc:
𝜃𝑠
𝑛=
𝜃𝑚
30
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Theo nguyên lý D’Alembert ta có:

𝜏𝑙 − 𝐵𝑙 . 𝜃̇𝑠 = 𝐽𝑙 . 𝜃̈𝑠
Với :
𝜕𝜃𝑠
𝜃𝑠̇ = = 𝜔𝑠 là tốc độ góc của tải (𝑟𝑎𝑑/𝑠)
𝑑𝑡
𝜕𝜃𝑠 𝜕𝜔𝑠
𝜃𝑠̈ = = là gia tốc góc của tải (𝑟𝑎𝑑/𝑠 2 )
𝑑𝑡 2 𝑑𝑡
Phương trình trên tương đương với:
𝜏𝑙 − 𝐵𝑙 𝜔𝑠 = 𝐽𝑙 𝜔̇𝑠
Sử dụng nguyên lý trên cho trục động cơ ta có:
𝜏𝑚 − 𝑛𝜏𝑙 − 𝐵𝑚 . 𝜃̇𝑚 = 𝐽𝑚 . 𝜃̈𝑚
Kết hợp 3 phương trình trên, ta suy ra được:
𝜏𝑚 = (𝐽𝑚 + 𝑛2 𝐽𝑙 ). 𝜃̈𝑚 + (𝐵𝑚 + 𝑛2 𝐵𝑙 ). 𝜃̇𝑚
Thu gọn ta được :
𝜏𝑚 = 𝐽𝑡𝑑 . 𝜃̈𝑚 + 𝐵𝑡𝑑 𝜃̇𝑚
Hay có thể viết lại:
𝜏𝑚 = 𝐽𝑡𝑑 . 𝜔̇ 𝑚 + 𝐵𝑡𝑑 𝜔𝑚
Với: + 𝐽𝑡𝑑 = 𝐽𝑚 + 𝑛2 𝐽𝑙
+ 𝐵𝑡𝑑 = 𝐵𝑚 + 𝑛2 𝐵𝑙

31
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Phương trình về điện

Động cơ điện một chiều (Động cơ điện DC) kích từ độc lập, được điều khiển
bằng điện áp phần ứng. Sơ đồ nguyên lý của loại động cơ này được thể hiện trên
hình, trong đó dòng kích từ 𝑖𝑘 được giữ không đổi.

Hình 20. Sơ đồ nguyên lý động cơ điện DC

Với: + Tín hiệu vào là điện áp u đặt vào phần ứng [Volt; V].
+ Tín hiệu ra là vận tốc góc ω của động cơ [rad/s].
Sử dụng 3 phương trình cơ bản như sau:
• Phương trình mạch điện phần ứng
𝑑𝑖
𝑈=𝐿 + 𝑅𝑖 + 𝐾𝑒 𝜔
𝑑𝑡

Trong đó: + R – điện trở phần ứng, [Ω].

+ L – điện cảm phần ứng, [H].
+ i – dòng điện phần ứng, [A].
+ 𝐾𝑒 – hằng số sức điện động [Vs/rad].
+ 𝐾𝑒 𝜔 – sức điện động phần ứng [V].
Biến đổi Laplace 2 vế phương trình, ta được:
𝑈(𝑠) = 𝐿𝑠𝐼(𝑠) + 𝑅𝐼(𝑠) + 𝐾𝑒 𝜔(𝑠)
Sơ đồ khối tương ứng:

32
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Hình 21. Sơ đồ khối mạch điện phần ứng

• Phương trình momen điện từ của động cơ

Với dòng kích từ 𝑖𝑘 không đổi thì từ thông khe khí Φ = 𝑘2 𝑖𝑘 là không đổi và
momen điện từ M của động cơ tỉ lệ với dòng điện phần ứng:
𝑀 = 𝐾𝑚 𝑖
Trong đó 𝐾𝑚 = 𝑘1 Φ = 𝑘1 𝑘2 𝑖𝑘 là hằng số momen của động cơ, [N.m/A]
Với: + 𝑘1 - hằng số phụ thuộc vào kết cấu động cơ.
+ 𝑘2 - hằng số đặc trưng đoạn tuyến tính của từ thông thay đổi theo 𝑖𝑘 .
Biến đổi Laplace hai vế ta được:
𝑀(𝑠) = 𝐾𝑚 𝐼(𝑠)
Sơ đồ khối tương đương :

Hình 22. Sơ đồ khối phần momen điện từ

• Phương trình cân bằng momen trên trục động cơ

𝑑𝜔
𝑀=𝐽 + 𝐵𝜔 + 𝑀𝑡
𝑑𝑡
Trong đó:
+ J – momen quán tính của động cơ và tải quy về trục động cơ, [kg.m2]
+ B – hệ số ma sát nhớt của động cơ và tải quy về trục động cơ, [kg.m2]
33
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

+ 𝑀𝑡 – momen phụ tải (nhiễu), [Nm]

Biến đổi Laplace 2 vế phương trình:
𝑀(𝑠) = 𝐽𝑠𝜔(𝑠) + 𝐵𝜔(𝑠) + 𝑀𝑡 (𝑠)
Sơ đồ khối tương ứng:

Hình 23. Sơ đồ khối phần cần bằng momen trên động cơ

Kết hợp 3 sơ đồ khối thành phần, ta thu được sơ đồ khối của động cơ:

Hình 24. Sơ đồ khối động cơ với tín hiệu vào điện áp và tín hiệu ra vận tốc

Hàm truyền của động cơ DC với tín hiệu vào là điện áp và tín hiệu ra là vận tốc:
𝐾𝑚
𝜔(𝑠) (𝐿𝑠 + 𝑅)(𝐽𝑠 + 𝐵) 𝐾𝑚
𝐺𝑑𝑐 (𝑠) = = =
𝑈(𝑠) 1 + 𝐾𝑚 𝐾𝑒 𝐿𝐽𝑠 2 + (𝐿𝐵 + 𝑅𝐽)𝑠 + (𝐾𝑚 𝐾𝑒 + 𝑅𝐵)
(𝐿𝑠 + 𝑅)(𝐽𝑠 + 𝐵)

34
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Trong đồ án này, ta quan tâm đến điều khiển vị trí của robot, cụ thể là góc quay tại
các khớp, do đó khi mô hình hóa động cơ, tín hiệu ra phải là đại lượng dịch chuyển
của góc.

Gọi θ𝑚 – góc quay của động cơ. Do đó đạo hàm của góc quay chính là vận tốc góc:

Θ𝑚 (𝑡) = ∫ 𝜔(𝑡)𝑑𝑡

Chuyển sang miền tần số, ta được:

𝜔(𝑠)
Θ𝑚 (𝑠) =
𝑠
Lúc này, hàm truyền của động cơ trở thành:
Θ𝑚 (𝑠) 𝐾𝑚
𝐺𝑑𝑐 (𝑠) = =
𝑈(𝑠) 𝐿𝐽𝑡𝑑 𝑠 3 + (𝐿𝐵𝑡𝑑 + 𝑅𝐽𝑡𝑑 )𝑠 2 + (𝐾𝑚 𝐾𝑒 + 𝑅𝐵𝑡𝑑 )𝑠
𝐿 𝐽
Đặt: 𝜏𝑡 = – hằng số thời gian điện từ, 𝜏𝑐 = 𝑡𝑑 – hằng số thời gian cơ
𝑅 𝐵𝑡𝑑
Hàm truyền của động cơ DC với tín hiệu vào là điện áp và tín hiệu ra là góc quay:
Θ𝑚 (𝑠) 𝐾𝑡đ
𝐺𝑑𝑐 (𝑠) = =
𝑈(𝑠) 𝑇1 𝑠 3 + 𝑇2 𝑠 2 + 𝑠
Trong đó:
𝜏𝑡 𝜏𝑐 𝑅𝐵𝑡𝑑 (𝜏𝑡 + 𝜏𝑐 )𝑅𝐵𝑡𝑑 𝐾𝑚
𝑇1 = ; 𝑇2 = ; 𝐾𝑡𝑑 =
𝐾𝑚 𝐾𝑒 + 𝑅𝐵𝑡𝑑 𝐾𝑚 𝐾𝑒 + 𝑅𝐵𝑡𝑑 𝐾𝑚 𝐾𝑒 + 𝑅𝐵𝑡𝑑

35
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Sơ đồ khối của động cơ DC với tín hiệu vào là điện áp và tín hiệu ra là góc quay:

Hình 25. Sơ đồ khối động cơ với tín hiệu vào điện áp và tín hiệu ra góc quay

Bảng thông số động cơ servo DC sử dụng trong robot SCARA:

Momen xoắn 𝑇(𝑁𝑚) 4
Cường độ dòng điện 𝐴(𝐴𝑚𝑝𝑒) 4,4
Hằng số sức điện động 𝐾𝑒 (𝑉𝑠/𝑟𝑎𝑑) 56/60
Hệ số momen xoắn 𝐾𝑚 (𝑁𝑚/𝐴) 0,9
Quán tính của rotor 𝐽𝑚 (𝑘𝑔. 𝑚2 ) 2,97.10-4
Điện trở 𝑅(Ω) 1,83
Độ tự cảm 𝐿(𝐻) 4,72.10-3
Tốc độ quay 𝑛(𝑟𝑝𝑚) 2500
Hệ số giảm chấn 𝐵𝑚 (𝑁𝑚/𝑠) 2,58.10-4

36
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

3.1.2. Mô hình hóa các khâu

Trục vít me – đai ốc bi khâu 3

Hình 26. Sơ đồ trục vít me - đai ốc bi

Xét bộ truyền vít me – đai ốc bi như hình.

Tín hiệu vào: vận tốc góc 𝑤(𝑡) của vít me, [rad/s]
Tín hiệu ra: lượng dịch chuyển 𝑦(𝑡) của khâu 3, [m]
Gọi p là bước của vít me, [m]. Ta có phương trình quan hệ:
𝑡
𝑝
𝑦(𝑡) = × ∫ 𝑤(𝑡)𝑑𝑡
2𝜋 0

Biến đổi Laplace 2 vế với điều kiện đầu bằng 0:

𝑝 𝑤(𝑠)
𝑌(𝑠) = ×
2𝜋 𝑠
Lập tỷ số tín hiệu ra trên tín hiệu vào, ta thu được hàm truyền tích phân:
𝑌(𝑠) 𝑝 𝐾
= =
𝑤(𝑠) 2𝜋𝑠 𝑠
𝑝
Trong đó: 𝐾= – hệ số tích phân
2𝜋
Sơ đồ khối tương đương:

Hình 27. Sơ đồ khối trục vít me - đai ốc bi

37
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Kết hợp sơ đồ khối các phần tử động cơ và trục vít me bi, ta thu được sơ đồ khối mô
tả khâu 3 như sau:

Hình 28. Sơ đồ khối khâu 3

Hàm truyền của khâu 3:

𝐾𝑡đ 𝐾
𝐺𝑘ℎ𝑎𝑢3 (𝑠) =
𝑇1 𝑠 3 + 𝑇2 𝑠 2 + 𝑠
Sử dụng phần mềm Solidworks để tính toán momen quán tính của khâu 𝐽𝑙

38
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Hình 29. Tính toán momen quán tính khâu 3

Theo kết quả tính toán 𝐽𝑙3 = 1,6261.10−4 (𝑘𝑔. 𝑚2 )

𝐽𝑡𝑑3 = 𝐽𝑚3 + 𝑛3 2 𝐽𝑙3 = 2,97.10−4 + 52 ⋅ 1,6261.10−4 = 4,36.10−3 (𝑘𝑔. 𝑚2 )
{
𝐵𝑡𝑑3 = 𝐵𝑚3 + 𝑛3 2 . 𝐵𝑙3 = 2,58.10−4 + 52 ⋅ 0,001 = 0,025(𝑁𝑚𝑠/𝑟𝑎𝑑)
Ta xác định được:
𝐾𝑚 = 0.9(𝑁𝑚/𝐴)
𝐾𝑒 = 0,93(𝑉𝑠/𝑟𝑎𝑑)
𝑅 = 1,83(𝑂ℎ𝑚)

39
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

𝐿 = 4,72.10−3 (𝐻)
𝑛=5
𝐾𝑚
𝐾𝑡𝑑 = = 1,02
𝐾𝑚 𝐾𝑒 + 𝑅𝐵𝑡𝑑
𝑝 0,025
𝐾= = = 3,98. 10−3
2𝜋 2𝜋
𝜏𝑡 𝜏𝑐 𝑅𝐵𝑡𝑑
𝑇1 = = 2,33. 10−5
𝐾𝑚 𝐾𝑒 + 𝑅𝐵𝑡𝑑
(𝜏𝑡 + 𝜏𝑐 )𝑅𝐵𝑡𝑑
𝑇2 = = 9,17. 10−3
𝐾𝑚 𝐾𝑒 + 𝑅𝐵𝑡𝑑

4,06.10−3
=> 𝐺𝑘ℎ𝑎𝑢3 (𝑠) =
2,33. 10−5 𝑠 3 + 9,17. 10−3 𝑠 2 + 𝑠

Kiểm tra tính ổn định của khâu 3

Kiểm tra tính ổn định của hệ kín theo tiêu chuẩn Nyquist:

40
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Hình 30. Đồ thị Nyquist của hàm truyền khâu 3

Ta thấy rằng đồ thị Nyquist không bao điểm (−1 + 0𝑗) nên hệ kín sẽ ổn định.
Mô hình hóa khâu 2
Do khâu 2 chỉ sử dụng tổ hợp động cơ DC + hộp giảm tốc, không sử dụng
thêm cơ cấu truyền động nên hàm truyền của khâu 2 chính là hàm truyền của động
cơ đã xây dựng ở phần 3.1.1.
Θ𝑚2 (𝑠) 𝐾𝑡đ
𝐺𝑘ℎ𝑎𝑢2 (𝑠) = =
𝑈2 (𝑠) 𝑇1 𝑠 3 + 𝑇2 𝑠 2 + 𝑠
Trong đó:
𝜏𝑡 𝜏𝑐 𝑅𝐵𝑡𝑑 (𝜏𝑡 + 𝜏𝑐 )𝑅𝐵𝑡𝑑 𝐾𝑚
𝑇1 = ; 𝑇2 = ; 𝐾𝑡đ =
𝐾𝑚 𝐾𝑒 + 𝑅𝐵𝑡𝑑 𝐾𝑚 𝐾𝑒 + 𝑅𝐵𝑡𝑑 𝐾𝑚 𝐾𝑒 + 𝑅𝐵𝑡𝑑
𝐿 𝐽
𝜏𝑡 = – hằng số thời gian điện từ, 𝜏𝑐 = 𝑡𝑑 – hằng số thời gian cơ
𝑅 𝐵𝑡𝑑
41
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Sử dụng phần mềm Solidworks để tính toán momen quán tính của khâu 𝐽𝑙

Hình 31. Tính toán momen quán tính khâu 2

Theo kết quả tính toán 𝐽𝑙2 = 0,91(𝑘𝑔. 𝑚2 )

1
𝐽𝑡𝑑2 = 𝐽𝑚2 + 𝑛2 2 𝐽𝑙2 = 2,97.10−4 + 2
⋅ 0,91 = 8,38.10−4 (𝑘𝑔. 𝑚2 )
{ 41
1
𝐵𝑡𝑑2 = 𝐵𝑚2 + 𝑛2 2 . 𝐵𝑙2 = 2,58.10−4 + 2 ⋅ 0,1 = 3,17.10−4 (𝑁𝑚𝑠/𝑟𝑎𝑑)
41
Ta xác định được:
42
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

𝐾𝑚 = 0.9(𝑁𝑚/𝐴)
𝐾𝑒 = 0,93(𝑉𝑠/𝑟𝑎𝑑)
𝑅 = 1,83(𝑂ℎ𝑚)
𝐿 = 4,72.10−3 (𝐻)
1
𝑛=
41
𝐾𝑚
𝐾𝑡𝑑 = = 1,075
𝐾𝑚 𝐾𝑒 + 𝑅𝐵𝑡𝑑
𝜏𝑡 𝜏𝑐 𝑅𝐵𝑡𝑑
𝑇1 = = 4,72. 10−6
𝐾𝑚 𝐾𝑒 + 𝑅𝐵𝑡𝑑
(𝜏𝑡 + 𝜏𝑐 )𝑅𝐵𝑡𝑑
𝑇2 = = 3,62. 10−3
𝐾𝑚 𝐾𝑒 + 𝑅𝐵𝑡𝑑

1,075
=> 𝐺𝑘ℎ𝑎𝑢2 (𝑠) =
4,72. 10−6 𝑠 3 + 3,62. 10−3 𝑠 2 + 𝑠

Kiểm tra tính ổn định của khâu 2

Kiểm tra tính ổn định của hệ kín theo tiêu chuẩn Nyquist:

43
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Hình 32. Đồ thị Nyquist hàm truyền khâu 2

Ta thấy rằng đồ thị Nyquist không bao điểm (−1 + 0𝑗) nên hệ kín sẽ ổn định.
Mô hình hóa khâu 1
Do khâu 1 chỉ sử dụng tổ hợp động cơ DC + hộp giảm tốc, không sử dụng
thêm cơ cấu truyền động nên hàm truyền của khâu 1 chính là hàm truyền của động
cơ đã xây dựng ở phần 3.1.1.
Θ𝑚1 (𝑠) 𝐾𝑡đ
𝐺𝑘ℎ𝑎𝑢1 (𝑠) = =
𝑈1 (𝑠) 𝑇1 𝑠 3 + 𝑇2 𝑠 2 + 𝑠
Trong đó:
𝜏𝑡 𝜏𝑐 𝑅𝐵𝑡𝑑 (𝜏𝑡 + 𝜏𝑐 )𝑅𝐵𝑡𝑑 𝐾𝑚
𝑇1 = ; 𝑇2 = ; 𝐾𝑡đ =
𝐾𝑚 𝐾𝑒 + 𝑅𝐵𝑡𝑑 𝐾𝑚 𝐾𝑒 + 𝑅𝐵𝑡𝑑 𝐾𝑚 𝐾𝑒 + 𝑅𝐵𝑡𝑑
𝐿 𝐽
𝜏𝑡 = – hằng số thời gian điện từ, 𝜏𝑐 = 𝑡𝑑 – hằng số thời gian cơ
𝑅 𝐵𝑡𝑑
44
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Sử dụng phần mềm Solidworks để tính toán momen quán tính của khâu 𝐽𝑙

Hình 33. Tính toán momen quán tính khâu 1

Theo kết quả tính toán 𝐽𝑙1 = 5,27(𝑘𝑔. 𝑚2 )

1
𝐽𝑡𝑑1 = 𝐽𝑚1 + 𝑛1 2 𝐽𝑙1 = 2,97.10−4 + 2 ⋅ 5,27 = 3,43.10−3 (𝑘𝑔. 𝑚2 )
{ 41
1
𝐵𝑡𝑑1 = 𝐵𝑚1 + 𝑛1 2 . 𝐵𝑙1 = 2,58.10−4 + 2 ⋅ 0,1 = 3,17.10−4 (𝑁𝑚𝑠/𝑟𝑎𝑑)
41
Ta xác định được:
𝐾𝑚 = 0.9(𝑁𝑚/𝐴)
𝐾𝑒 = 0,93(𝑉𝑠/𝑟𝑎𝑑)
45
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

𝑅 = 1,83(𝑂ℎ𝑚)
𝐿 = 4,72.10−3 (𝐻)
1
𝑛=
41
𝐾𝑚
𝐾𝑡𝑑 = = 1,075
𝐾𝑚 𝐾𝑒 + 𝑅𝐵𝑡𝑑
𝜏𝑡 𝜏𝑐 𝑅𝐵𝑡𝑑
𝑇1 = = 1,93. 10−5
𝐾𝑚 𝐾𝑒 + 𝑅𝐵𝑡𝑑
(𝜏𝑡 + 𝜏𝑐 )𝑅𝐵𝑡𝑑
𝑇2 = = 7,5. 10−3
𝐾𝑚 𝐾𝑒 + 𝑅𝐵𝑡𝑑

1,075
𝐺𝑘ℎ𝑎𝑢1 (𝑠) = (𝑟𝑎𝑑/𝑉)
1,93. 10−5 𝑠 3 + 7,5. 10−3 𝑠 2 + 𝑠

Kiểm tra tính ổn định của khâu 1

Kiểm tra tính ổn định của hệ kín theo tiêu chuẩn Nyquist:

46
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Hình 34. Đồ thị Nyquist của hàm truyền khâu 1

Ta thấy rằng đồ thị Nyquist không bao điểm (−1 + 0𝑗) nên hệ kín sẽ ổn định.
3.2. Xây dựng mô hình hệ thống điều khiển cho toàn bộ robot

Hình 35. Trình tự xây dựng mô hình hệ thống điều khiển

47
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Trình tự các bước thực hiện bài toán

• Xác định các thông số vật lý của robot
• Mô hình hóa bằng phần mềm SolidWorks
• Xuất bản CAD tương ứng sang MATLAB/SimMechanics
• Xây dựng bộ điều khiển PID và mô phỏng
Mô hình CAD sau khi đưa vào MATLAB

Hình 36. Mô hình CAD của robot trong MATLAB

48
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Mô hình Simscape xuất từ SolidWorks

Hình 37. Khối 3D Scara robot

Mô hình điều khiển cho toàn bộ robot

Hình 38. Mô hình hệ thống điều khiển toàn bộ robot

• Đầu vào của hệ thống là giá trị của các biến khớp (𝑞1 , 𝑞2 , 𝑞3 ) phụ thuộc vào
vị trí các điểm thao tác trong không gian, ở đây các biến khớp thỏa mãn quỹ

49
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

đạo hình học cho trước và được giải bằng bài toán động học ngược như đã
trình bày ở phần 2.1 và 2.2.
• Tín hiệu vào sẽ đi qua các khối Điều khiển PID, khối động cơ DC và khối mô
hình 3D của robot.
• Đầu ra của hệ thống là quỹ đạo của các biến khớp, vận tốc của các biến khớp
và tín hiệu sai lệch đóng vai trò là đầu vào của bộ điều khiển PID.

Hình 39. Khối Position Reference

Hình 40. Khối động cơ DC

50
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

CHƯƠNG 4: MÔ PHỎNG VÀ ĐÁNH GIÁ TÍNH ỔN

ĐỊNH, CÁC CHỈ TIÊU CHẤT LƯỢNG
4.1. Sử dụng MATLAB-Simulink để mô phỏng động lực học cho một trục
chuyển động và cho toàn bộ robot
Sơ đồ Simulink cho từng khâu:

Hình 41. Mô hình simulink cho từng khâu

51
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

4.2. Đánh giá tính ổn định, xác định sai lệch tĩnh
Xác định sai lệch tĩnh
Cả 3 khâu đều có hàm truyền dạng:
𝐾
𝐺(𝑠) =
𝑇1 𝑠 3 + 𝑇2 𝑠 2 + 𝑠
Xét tín hiệu đầu vào có dạng 𝑢(𝑡) = 1(𝑡)

Hình 42. Sơ đồ khối tính sai lệch tĩnh với tín hiệu vào u(t) = 1(t)

Sai lệch tĩnh được tính theo công thức:

1 1
𝑒∞ = lim 𝑒(𝑡) = lim 𝑠𝐸(𝑠) = lim = =0
𝑡→0 𝑠→0 𝑠→0 1 + 𝐺(𝑠) 1 + lim 𝐺(𝑠)
𝑠→0

Theo tiêu chuẩn đánh giá sai lệch tĩnh, hệ thống ổn định.

52
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Đánh giá tính ổn định

• Khâu 1

Hình 43. Đồ thị đáp ứng khâu 1

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

53
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

• Khớp 2

Hình 44. Đồ thị đáp ứng khâu 2

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

54
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

• Khớp 3

Hình 45. Đồ thị đáp ứng khâu 3

- Độ vọt lố: 0%
- Thời gian tăng: 108s (là khoảng thời gian kể từ khi hệ thống đạt 10% cho đến
khi đạt 90% giá trị xác lập)
- Thời gian xác lập: 193 s
- Trạng thái ổn định: Giá trị xác lập là 1, vậy sai lệch tĩnh bằng 0 và hệ ổn định
Đánh giá: Dù hệ thống đã đạt đến trạng thái ổn định, tuy nhiên thời gian xác lập là quá
lớn, tốc độ đáp ứng không đủ nhanh. Do đó đòi hỏi phải thiết kế bộ điều khiển để tăng
tốc độ đáp ứng, giảm thời gian xác lập của hệ thống.

55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

CHƯƠNG 5: THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN

5.1. Thiết kế bộ điều khiển PID
PID Controller (Propotional – Integral – Derivative Controller) – bộ điều khiển tỷ
lệ vi tích phân là một cơ chế vòng phản hồi điều khiển được sử dụng rộng rãi trong công
nghiệp. Bộ điều khiển PID sẽ tính toán giá trị sai số (sai lệch giữa tín hiệu ra thực tế đo
được và tín hiệu mong muốn). Sau đó, PID sẽ làm giảm sai số bằng cách điều chỉnh tín
hiệu đầu vào. Sai lệch càng nhỏ thì hệ thống càng đạt đến trạng thái làm việc mong muốn.

Hình 46. Mô hình bộ điều khiển PID

Trong đồ án này, với mục tiêu điều khiển robot đạt đến vị trí mong muốn bất kỳ trong
không gian thao tác (các tọa độ khớp 𝑞𝑑 phải đạt đến giá trị theo quỹ đạo yêu cầu), ta
sẽ sử dụng luật điều khiển PID cho robot. Tín hiệu điều khiển của PID Controller là
sai lệch về vị trí biến khớp theo thời gian hay nói cách khác tín hiệu điều khiển chính
là sai số giữa vị trí hiện tại và vị trí mong muốn.
𝑡
𝑑𝑒(𝑡)
𝑢(𝑡) = 𝐾𝑃 . 𝑒(𝑡) + 𝐾𝐷 . + 𝐾𝐼 . ∫ 𝑒(𝑡)𝑑𝑡
𝑑𝑡 0

Với 𝑒(𝑡) = 𝑞𝑑 (𝑡) − 𝑞(𝑡)

56
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

+ 𝑒(𝑡) – sai lệch giữa giá trị đặt mong muốn (setpoint) và giá trị thực tế đo được
+ 𝑞𝑑 (𝑡) – giá trị đặt mong muốn (setpoint) tại thời điểm đo
+ 𝑞(𝑡) – giá trị thực tế của biến khớp tại thời điểm đo
Tiến hành xây dựng bộ điều khiển PID trong MATLAB như sau:
• Thiết lập sơ đồ Simulink cho bộ điều khiển

Hình 47. Sơ đồ khối bộ điều khiển PID

• Trong MATLAB-Simulink đã có sẵn khối PID Controller, ta sẽ sử dụng trực tiếp

khối này mà không cần xây dựng lại

Hình 48. Sơ đồ khối PID điều khiển động cơ

57
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

• Việc lựa chọn các thông số cho PID Controller có thể được thực hiện bằng nhiều
phương pháp. Đơn giản nhất là chỉnh bằng tay, thử các bộ thông số khác nhau cho
đến khi đạt được tín hiệu mong muốn. Cách này tuy đơn giản, không phải tính
toán nhưng lại tốn thời gian, không khoa học và rất “hên xui”. Cách thứ hai là
chúng ta có thể sử dụng các phương pháp toán học như Ziegler-Nichols, Chien-
Hrones-Reswick, tối ưu độ lớn…Và cách thứ 3, cũng là cách sử dụng trong đồ án
này, đó là sử dụng công tụ PID Tuner tích hợp trên MATLAB.

Hình 49. Giao diện công cụ PID Tuner trên MATLAB

Xác định được các thông số của PID Controller:

Khâu 𝑲𝑷 𝑲𝑰 𝑲𝑫
1 4522,21 61581,04 38,22
2 3196,13 52494,7 −3,46
3 4548,37 65186,78 31,61

58
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Ta sẽ đánh giá chất lượng của bộ điều khiển đã thiết kế với đầu vào mẫu 𝑢(𝑡) = 1(𝑡)
• Khâu 1:

Hình 50. Đồ thị đáp ứng khâu 1 với bộ điều khiển PID

- Độ vọt lố: 7,62%

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

59
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

• Khâu 2:

Hình 51. Đồ thị đáp ứng khâu 2 với bộ điều khiển PID

- Độ vọt lố: 13%

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

60
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

• Khâu 3:

Hình 52. Đồ thị đáp ứng khâu 3 với bộ điều khiển PID

- Độ vọt lố: 11,6%

- Thời gian tăng: 0,0144s (là khoảng thời gian kể từ khi hệ thống đạt 10% cho đến
khi đạt 90% giá trị xác lập)
- Thời gian xác lập: 0,151 s
- Trạng thái ổn định: Giá trị xác lập là 1, vậy sai lệch tĩnh bằng 0 và hệ ổn định
Đánh giá: Sau khi thiết kế bộ điều khiển PID, nhận thấy hệ thống đã xuất hiện overshoot
nhỏ, do đây là điều gần như không thể tránh khỏi nên hoàn toàn chấp nhận được, hơn hết
tốc độ đáp ứng rất nhanh và thời gian xác lập đã giảm cực kỳ đáng kể sẽ làm cho hệ
thống hoạt động hiệu quả hơn.
Sau khi thiết kế PID Controller cho robot, ta thu được đồ thị về quỹ đạo của từng biến
khớp, sai số và vận tốc của chúng như sau:

61
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Hình 53. Giá trị biến khớp khâu 1

Hình 54. Sai số biến khớp khâu 1

62
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Hình 55. Vận tốc khâu 1

Hình 56. Giá trị biến khớp khâu 2

63
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Hình 57. Sai số biến khớp khâu 2

Hình 58. Vận tốc khâu 2

64
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Hình 59. Giá trị biến khớp khâu 3

Hình 60. Sai số biến khớp khâu 3

65
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Hình 61. Vận tốc khâu 3

5.2. Lựa chọn các phần tử cho hệ thống điều khiển

Hệ thống điều khiển robot bao gồm 2 phần tử cơ bản và quan trọng: Hệ thống điều
khiển động cơ servo và bộ điều khiển robot.
Hệ thống điều khiển động cơ servo
Cụm thiết bị điều khiển servo gồm 2 thành phần: Bộ điều khiển và động cơ servo.
Với bộ điều khiển động cơ servo, hiện nay trong công nghiệp thường có 2 lựa chọn. Thứ
nhất, ta sẽ sử dụng biến tần để điều khiển động cơ. Trước kia, biến tần thường được sử
dụng để điều khiển động cơ AC cho các ứng dụng công suất lớn như máy nén khí hay
băng tải nặng, tuy nhiên công nghệ biến tần ngày càng phát triển đã cho phép mở rộng
phạm vi ứng dụng, vì thế có thể sử dụng biến tần để điều khiển servo. Thứ hai, ta sẽ sử
dụng Servo Driver. Các Servo Driver được thiết kế module hóa, nghĩa là mỗi driver sẽ

66
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

tương thích với một số loại động cơ servo nhất định, quy định theo từng hãng sản xuất.
Loại này phù hợp với những ứng dụng công suất nhỏ, đòi hỏi momen xoắn lớn và điều
khiển vị trí chính xác cao, ứng dụng trong robot, máy CNC…
Biến tần Bộ điều khiển Servo
Ứng dụng điều khiển Sử dụng cho các ứng dụng Sử dụng cho các ứng dụng
không yêu cầu tốc độ và độ điều khiển ở tốc độ và độ
chính xác cao, ổn định chính xác cao, thay đổi
trạng thái nhanh và liên tục
Chế độ điều khiển Cơ bản là điều khiển tốc độ Điều khiển vị trí, tốc độ và
động cơ momen
Khả năng điều khiển nhiều Một biến tần có thể điều Một bộ điều khiển servo cơ
động cơ khiển nhiều động cơ bản chỉ điều khiển một
động cơ servo duy nhất
Độ đáp ứng Chậm Nhanh
100 rad/s hoặc thấp hơn Khoảng 200 - 15000 rad/s
Điều khiển vị trí và dừng Đến khoảng 100 µm Lên đến khoảng 1µm
chính xác cao
Chế độ khóa Không Có
Tần số khởi động/dừng 20 rpm hoặc thấp hơn 20 – 600 rpm
Momen xoắn cực đại Khoảng 150% Khoảng 300%
Công suất 100W – 300kW 10W – 60kW
Kích thước Lớn, nặng Nhỏ gọn và nhẹ hơn
Chi phí đầu tư Tương đối thấp Cao

Từ những đặc điểm trên, ta sẽ lựa chọn Servo Driver để điều khiển servo cho hệ thống
robot SCARA. Trong đồ án này, em sẽ lựa chọn cụm thiết bị driver SGDV-5R5A và
động cơ servo SGMAV – 08A của hãng Yaskawa.

67
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Hình 62. Servo Motor + Driver Yaskawa

Hình 63. Thông số kỹ thuật của dòng servo motor SGMAV

68
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Hình 64. Giao diện của servo driver SGDV-5R5A

Driver chia làm 3 vùng chính:

• Phần Panel Operator: Bao gồm 1 màn hình LED hiển thị và nút bấm, sử dụng để
cấu hình các chức năng, cài đặt chế độ cho driver hoạt động.
• Phần cấp nguồn (nằm ở sườn bên trái), gồm có:
+ Cổng L1, L2, L3: Cấp nguồn điện cho động cơ.
+ Cổng L1C, L2C: Cấp nguồn điện cho driver hoạt động.

69
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

+ Cổng U, V, W: Cổng nối với động cơ .

• Phần cổng input/output (nằm ở sườn bên phải), gồm có:
+ Cổng CN5: Cổng kết nối ra màn hình annalog.
+ Cổng CN3: Cổng kết nối với các thiết bị điều khiển.
+ Cổng CN7: Cổng kết nối USB dành cho máy tính.
+ Cổng CN1: Cổng I/O vào ra, nhận các tín hiệu điều khiển từ bộ điều khiển
trung tâm, xuất các tín hiệu cảnh báo, …
+ Cổng CN8: Cổng chức năng cho các thiết bị an toàn.
+ Cổng CN2: Kết nối với encoder của động cơ.
Driver là một bộ điều khiển phản hồi vòng kín dưới dạng module hoá, như khối điều
khiển PID ở phần thiết kế hệ thống điều khiển. Các tín hiệu điều khiển sẽ được nhận từ
các cổng điều khiển như CN3, CN7 hoặc CN1. Tuỳ thuộc yêu cầu thiết kế ứng dụng mà
người dùng có thể lựa chọn các bộ điều khiển trung tâm khác nhau để cấp tín hiệu điều
khiển cho driver.
Nếu điều khiển qua cổng CN1 ta có thể có thêm một bộ cáp để ra chân cho tuỳ mục đích
yêu cầu cụ thể. Chẳng hạn như kết nối vào các dòng vi điều khiển, các thiết bị cấp xung
mà cần sử dụng nhiều chân I/O.

Hình 65. Cổng kết nối CN1

70
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Hình 66. Sơ đồ các chân I/O của servo driver SGDV-5R5A

71
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Vị trí của động cơ được kiểm soát chính bởi 2 chân đầu vào là PULS (chân số 7 và 8) và
SIGN (chân số 11 và 12), trong đó PULS là chân cấp xung cho driver, mỗi xung cấp vào
động cơ sẽ di chuyển tương ứng với xung của encoder được gắn trên động cơ. Các chế
độ cài đặt chế độ cho Driver, ở đây chọn chế độ Factory Settings:

Hình 67. Phương pháp xuất xung điều khiển servo ở chế độ điều khiển vị trí

Từ encoder 20bit → Số xung/vòng quay động cơ, qua tỷ lệ hộp giảm tốc ta có thể
tính ra được số xung để động cơ quay 1 vòng sau hộp giảm tốc là:
𝑛 = 1048576 × 𝑘
Với k là hệ số giảm tốc.
Khi đó muốn điều khiển động cơ quay được 𝑎 𝑣ò𝑛𝑔 ta cần cấp 𝑛 × 𝑎 xung vào chân
PULS. Để đảo chiều thì thay đổi mức logic ở chân SIGN.

72
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Trong đồ án này, mục tiêu của việc thiết kế hệ thống điều khiển là robot cần đáp
ứng được yêu cầu về vị trí, do đó sử dụng chế độ điều khiển về vị trí của robot. Sẽ cần
tối thiểu là 3 chân PULS và 3 chân SIGNS để điều khiển 3 bộ driver. Ngoài ra sẽ cần
thêm một số các cổng INPUT khác nữa để cho phép servo hoạt động, cảnh báo lỗi...
Bộ điều khiển robot
Khi lựa chọn bộ điều khiển cho một cơ cấu tự động, như đã trình bày tóm tắt ở
Chương 1, ta thường nghĩ đến 2 sự lựa chọn: vi điều khiển hoặc PLC.
Dựa vào những hiểu biết của mình về vi điều khiển, cùng với đó là khả năng dễ tiếp cận,
có nhiều tài nguyên mở, cộng đồng hỗ trợ lớn, tính linh hoạt trong lập trình, em xin đề
xuất lựa chọn vi điều khiển ARM-Cortex M4 STM32F407VGT6.

Hình 68. Board vi điều khiển STM32F407 DISCOVERY

Thông số kỹ thuật cơ bản của vi điều khiển STM32F407VGT6:

• Core: ARM 32-bit Cortex M4
• Tần số xung nhịp: Tối đa 168MHz
• Bộ nhớ:
- 1MB of Flash memory

73
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

- 192KB of SRAM
- 512 bytes of OTP memory
• Điện áp hoạt động: 1.8 – 3.6V
• Có 100 chân I/O
• Timer: gồm 14 timers, trong đó 6 timer có 4 kênh xuất PWM điều khiển động
cơ, 2 timer có 2 kênh PWM
• Có 3 bộ chuyển đổi Analog to Digital 12 bit, mỗi bộ 16 kênh. Có 1 bộ chuyển
đổi Digital to Analog
• Giao thức truyền thông: 3 x I2C, 6 x USART, 3 x SPI, 2 x CAN, USB_OTG
5.3. Các mạch điện và ghép nối phần tử
Mạch nguồn
Hệ thống sử dụng 3 driver điều khiển 3 động cơ servo, điện áp cấp cho động cơ
hoạt động sử dụng nguồn điện 3 pha AC 220V.
Nguồn điện cho driver là nguồn điện 1 pha AC 220V. Nguồn điện này có thể lấy từ 2
dây của hệ thống điện 3 pha.
Nguồn điện cho bộ điều khiển là nguồn 24VDC.
Hệ thống cấp nguồn bao gồm các thiết bị chính như:
• 1QF: Cầu dao tổng
• 1FT: Bộ lọc nhiễu
• 1KM: Bộ khởi động từ cho nguồn điều khiển
• 2KM: Bộ khởi động từ cho nguồn chính
• 1Ry: Relay
• 1PL: Đèn cảnh bảo lỗi động cơ
• 1SA, 2SA, 3SA: Bộ chống sét và bảo vệ quá tải
• 1D: Diode chống dội xung
• Các công tắc để đóng cắt các nguồn điện

74
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Hình 69. Sơ đồ ghép nối nguồn với 3 servo driver

Nguồn điện 3 pha được lấy từ hệ thống và cấp vào cổng nguồn động cơ bao gồm
các chân L1, L2, L3 tương ứng với 3 dây pha của nguồn điện. Trên đường dây có các

75
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Relay Terminal để kết nối mở rộng đường dây ra các driver. Động cơ được kết nối vào
port U,V,W trên driver và dây tín hiệu encoder. Các cổng L1C, L2C lấy nguồn 1 pha từ
1 dây pha và 1 dây trung tính để cấp nguồn cho driver hoạt động.

Hình 70. Kết nối động cơ với driver

Mạch ghép nối phần tử

Để điều khiển vị trí, ta sử dụng chân PULS và SIGN để điều khiển vị trí và chiều quay
của động cơ. Ngoài ra còn các chân INPUT có chức năng khác như:
• S-ON: Cho phép driver hoạt động.
• P-CON: Positon Control, chế độ hoạt động là điều khiển vị trí.
• P-OT: Giới hạn, hãm chiều quay của động cơ theo chiều thuận.
• N-OT: Giới hạn, hãm chiều quay của động cơ theo chiều nghịch.

76
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Hình 71. Sơ đồ ghép nối servo driver ở chế độ điều khiển vị trí

77
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

Các chân OUTPUT:

• ALO1, ALO2, ALO3: Các bit báo lỗi của driver
• COIN: Chân báo động cơ đã tới đúng vị trí mong muốn.
• TGON: Chân báo động cơ vượt quá vận tốc đặt
• SRDY: Chân báo tín hiệu động cơ đã sẵn sàng hoạt động hay chưa
Như vậy đối với 1 động cơ cần tối thiểu là 10 cổng I/O để điều khiển và giám sát hoạt
động của driver. 3 động cơ sẽ cần 30 cổng. Và các chân I/O trên mạch điều khiển đã
chọn là đủ để điều khiển hệ thống.

78
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

KẾT LUẬN
Trên đây là toàn bộ báo cáo đồ án môn học của em về đề tài “Thiết kế hệ thống
điều khiển robot SCARA 3 bậc tự do”. Em đã nghiêm túc thực hiện theo đúng nhiệm
vụ được giao. Với những kiến thức đã được giảng dạy về robot, em đã hoàn thành những
công việc sau:
• Mô hình hóa động cơ điện và tính hàm truyền của các khâu
• Xây dựng mô hình điều khiển cho toàn bộ robot
• Sử dụng MATLAB-Simulink để mô phỏng robot chuyển động
• Đánh giá tính ổn định, xác định sai lệch tĩnh từ hàm truyền các khâu
• Xây dựng bộ điều khiển PID cho robot
• Lựa chọn các phần tử điều khiển và bản vẽ ghép nối các phần tử
• Xây dựng GUI mô phỏng động học thuận, động học ngược robot

Trải qua quá trình tìm tòi, tính toán và thiết kế, em đã hoàn thành nội dung báo cáo
theo mục tiêu của đồ án. Tuy vậy, trong quá trình thực hiện, dù được sự hướng dẫn tận
tình của TS. Nguyễn Trọng Doanh, nhưng do hạn chế về mặt kiến thức, kinh nghiệm
cũng như thời gian nên bản thân em sẽ không thể tránh khỏi những thiếu sót. Em rất
mong nhận được sự góp ý và đánh giá của thầy để báo cáo được hoàn thiện hơn. Qua
đây, em sẽ rút ra được những kiến thức, kinh nghiệm quý báu cho các học phần khác, đồ
án tốt nghiệp và cả những vấn đề trong công việc sau này.
Một lần nữa, em xin chân thành cảm ơn sự tận tâm hướng dẫn, chỉ bảo và những góp
ý của TS. Nguyễn Trọng Doanh.

79
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
______________________________________________________________________________

DANH MỤC TÀI LIỆU THAM KHẢO

[1] Bài giảng điện tử Robotics – PGS. TS. Nguyễn Quang Hoàng, Bộ môn Cơ học ứng
dụng, Viện Cơ khí, Đại học Bách Khoa Hà Nội
[2] Giáo trình Kỹ thuật Robot – PGS. TS. Nguyễn Trường Thịnh, Đại học Sư phạm kỹ
thuật TP Hồ Chí Minh
[3] M. A. Sen, V. Bakircioglu, M. Kalyoncu, “Modelling and PID Control of Scara
Robot”, International Conference on Engineering Technologies (ICENTE’ 17), Dec 07-
09, 2017
[4] YASKAWA - AC Servo Drives ∑ −𝑉 𝑆𝑒𝑟𝑖𝑒𝑠 USER’S MANUAL Design and
Maintenance Rotational Motor Analog Voltage AC Servo Drives ∑ −𝑉 𝑆𝑒𝑟𝑖𝑒𝑠and Pulse
Train Reference
[5] YASKAWA - AC Servo Drives ∑ −𝑉 𝑆𝑒𝑟𝑖𝑒𝑠 Product Catalog

80