Doan2 Huy

ĐẠI HỌC BÁCH KHOA HÀ NỘI
ĐỒ ÁN THIẾT KẾ HỆ THỐNG ĐIỀU

KHIỂN
Thiết kế hệ thống điều khiển robot SCARA

ba bậc tự do
VŨ QUANG HUY
huy.vq195051@sis.hust.edu.vn
Ngành Kỹ thuật Cơ điện tử

Chuyên ngành Hệ thống cơ điện tử thông minh
Giảng viên hướng dẫn: TS.GVC. Nguyễn Trọng Du

Chữ ký của GVHD
Bộ môn: Cơ điện tử
Trường: Cơ khí
HÀ NỘI, 1/2024
LỜI NÓI ĐẦU
Hơn nửa thế kỷ qua, nền khoa học, kỹ thuật và công nghệ nước ta có những bước
chuyển biến tích cực trên mọi phương diện, có nhiều đóng góp quan trọng vào sự
nghiệp phát triển kinh tế - xã hội của đất nước. Trong bối cảnh toàn cầu hóa và xu
hướng phát triển nền kinh tế, hơn bao giờ hết, khoa học, kỹ thuật và công nghệ biểu
hiện như là “chìa khóa” của sự phát triển xã hội. Những sản phẩm của nó xuất hiện
thường xuyên trong cuộc sống của con người. Từ công việc nhẹ nhàng cho đến những
công việc khó khăn và nguy hiểm đều đã có máy móc làm thay con người.
Với sự phát triển mạnh mẽ nêu trên, đòi hỏi con người phải hiểu biết nhiều hơn để
làm chủ những công nghệ hiện đại. Vì vậy, với những kiến thức em được thầy cô
hướng dẫn và truyền đạt là cơ sở để em nghiên cứu, phát triển đồ án “Thiết kế hệ
thống điều khiển cho Robot Scara”.
Trong thời gian làm đồ án, em đã nhận được nhiều sự giúp đỡ, đóng góp ý kiến và
chỉ bảo nhiệt tình của thầy cô và bạn bè.
Em xin gửi lời cảm ơn chân thành đến các thầy cô trong khoa Cơ Điện Tử - Trường
Cơ Khí, Đại học Bách Khoa Hà Nội, những người đã giúp đỡ em trong quá trình thực
hiện, đặc biệt là thầy Nguyễn Trọng Du đã tận tình hướng dẫn trực tiếp chúng em
trong quá trình làm đồ án, bổ sung cho chúng em những kiến thức về học thuật cũng
như sự hỗ trợ rất lớn về mặt tinh thần.
Do thời gian làm đồ án có hạn và kiến thức còn hạn chế nên em không thể tránh
khỏi những thiếu sót. Em rất mong nhận được sự đóng góp ý kiến của thầy/cô để đồ án
này được hoàn thiện hơn nữa.
Hà Nội, ngày … tháng … năm 2023
Sinh viên thực hiện
MỤC LỤC
CHƯƠNG 1. TỔNG QUAN VỀ ROBOT SCARA....................................................1

1.1 Giới thiệu chung về Robot SCARA.................................................................1
1.1.1 Giới thiệu chung...................................................................................1
1.1.2 Ứng dụng của Robot SCARA..............................................................1
1.1.3 Ưu điểm của Robot SCARA................................................................2
1.1.4 Nhược điểm của Robot SCARA..........................................................3
1.2 Một số loại Robot SCARA................................................................................3
1.3 Yêu cầu thiết kế.................................................................................................3
1.3.1 Hệ thống cơ khí....................................................................................3
1.3.2 Hệ thống điện tử...................................................................................4
1.3.3 Phần mềm và điều khiển......................................................................4
1.4 Kết luận..............................................................................................................4
CHƯƠNG 2. PHÂN TÍCH NGUYÊN LÝ VÀ THÔNG SỐ KỸ THUẬT...............5
2.1 Nguyên lý hoạt động..........................................................................................5
2.1.1 Phân tích nguyên lý hoạt động.............................................................5
2.1.2 Các thông số kỹ thuật quan trọng.........................................................5
2.2 Xác định các thành phần của hệ thống điều khiển.........................................5
2.2.1 Động cơ................................................................................................5
2.2.2 Bộ nguồn..............................................................................................6
2.2.3 Bộ điều khiển động cơ..........................................................................7
2.3 Kết luận..............................................................................................................7
CHƯƠNG 3. THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN..............................................8
3.1 Động học Robot SCARA...................................................................................8
3.1.1 Động học thuận Robot SCARA...........................................................8
3.1.2 Động học ngược Robot SCARA........................................................12
3.2 Động lực học Robot SCARA..........................................................................13
3.2.1 Động lực học thuận............................................................................14
3.2.2 Động lực học ngược...........................................................................20
3.3 Thiết kế quỹ đạo chuyển động.......................................................................20
3.3.1 Mô hình hoá động cơ điện một chiều.................................................24
3.3.1 Mô hình hoá và xác định hàm truyền các khâu..................................29
3.3.2 Đánh giá tính ổn định, xác định sai lệch tĩnh.....................................31
3.4 Thiết kế bộ điều khiển PID.............................................................................36
3.5 Mô phỏng, phân tích và đánh giá các chỉ tiêu kỹ thuật của hệ thống........39
3.6 Lựa chọn các phần tử cho hệ thống điều khiển............................................42
3.7 Các mạch điện và ghép nối phần tử...............................................................53
3.8 Kết luận............................................................................................................54
CHƯƠNG 4. LẬP TRÌNH ĐIỀU KHIỂN................................................................55
4.1 Thiết kế giao diện điều khiển.........................................................................55
4.2 Mô hình Simulink điều khiển Robot.............................................................56
4.3 Kết quả mô phỏng...........................................................................................56
4.4 Kết luận............................................................................................................59
KẾT LUẬN..................................................................................................................60
TÀI LIỆU THAM KHẢO..........................................................................................61
PHỤ LỤC.....................................................................................................................62
DANH MỤC HÌNH VẼ
Hình 1-1 Robot SCARA.................................................................................................1
Hình 1-2 Ứng dụng của Robot SCARA..........................................................................2
Hình 1-3 Robot SCARA Denso HM-G...........................................................................3
Hình 1-4 Robot SCARA Fanuc SR 6iA và Robot SCARA Epson.................................3
Hình 2-1 Động cơ bước...................................................................................................6
Hình 2-2 Nguồn Adaptor tổ ong.....................................................................................6
Hình 2-3 Driver điều khiển động cơ...............................................................................7
Hình 2-4 Dây cáp RS 232, Arduino Mega......................................................................7
Hình 3-1 Sơ đồ động học Robot SCARA 3 bậc tự do.....................................................8
Hình 3-2 Không gian làm việc của Robot.....................................................................10
Hình 3-3 Không gian làm việc của Robot 2D...............................................................10
Hình 3-4 Hệ tọa độ khối tâm của Robot........................................................................13
Hình 3-5 Quỹ đạo chuyển động của Robot...................................................................20
Hình 3-6 Mô hình Simulink thiết kế quỹ đạo chuyển động..........................................22
Hình 3-7 Quỹ đạo các biến khớp khâu 1.......................................................................23
Hình 3-8 Quỹ đạo biến khớp khâu 2.............................................................................23
Hình 3-9 Quỹ đạo biến khớp khâu 3.............................................................................24
Hình 3-10 Sơ đồ khối Robot bao gồm hộp số...............................................................24
Hình 3-11 Sơ đồ nguyên lý động cơ.............................................................................26
Hình 3-12 Sơ đồ khối mạch điện phần ứng...................................................................26
Hình 3-13 Sơ đồ khối phần mô – men điện từ..............................................................27
Hình 3-14 Sơ đồ khối phần mô – men cân bằng trên động cơ......................................27
Hình 3-15 Sơ đồ khối động cơ......................................................................................27
Hình 3-16 Sơ đồ khối động cơ......................................................................................28
Hình 3-17 Sơ đồ trục vít me – đai ốc bi........................................................................29
Hình 3-18 Sơ đồ khối trục vít me – đai ốc bi................................................................30
Hình 3-19 Sơ đồ khối khâu 3........................................................................................30
Hình 3-20 Đồ thị Nyquist của hàm truyền Gkhau 1 (s )......................................................31
Hình 3-21 Đồ thị Nyquist của hàm truyền Gk h au 2 (s ).....................................................32
Hình 3-22 Đồ thị Nyquist của hàm truyền Gkhau 3 (s)......................................................33
Hình 3-23 Sơ đồ khối tính sai lệch tĩnh với đầu vào u(t)=1(t)......................................34
Hình 3-24 Đồ thị đáp ứng của khớp 1...........................................................................34
Hình 3-27 Mô hình bộ điều khiển PID..........................................................................36
Hình 3-28 Sơ đồ khối bộ điều khiển PID......................................................................37
Hình 3-29 Sơ đồ khối PID điều khiển động cơ.............................................................37
Hình 3-30 Đồ thị đáp ứng khâu 1 với bộ điều khiển PID.............................................38
Hình 3-33 Mô hình CAD của robot trong Matlab.........................................................39
Hình 3-34 Khối 3D Scara robot....................................................................................40
Hình 3-35 Mô hình hệ thống điều khiển toàn bộ robot.................................................40
Hình 3-36 Khối Position Reference (Thiết kế quỹ đạo)................................................41
Hình 3-37 Khối động cơ DC.........................................................................................41
Hình 3-38 Giá trị biến khớp khâu 1..............................................................................42
Hình 3-39 Sai số biến khớp khâu 1...............................................................................42
Hình 3-40 Vận tốc khâu 1.............................................................................................43
Hình 3-43 Vận tốc khâu 2.............................................................................................44
Hình 3-46 Vận tốc khâu 3.............................................................................................46
Hình 3-47 Động cơ và bộ điều khiển hãng Yaskawa....................................................47
Hình 3-48 Thông số kỹ thuật của dòng động cơ SGMAV............................................48
Hình 3-49 Giao diện của bộ điều khiển SGDV – 5R5A...............................................48
Hình 3-50 Sơ đồ các chân I/O cổng CN1 của servo driver SGDV – 5R5A.................50
Hình 3-51 Phương pháp xuất xung điều khiển servo ở chế độ điều khiển vị trí...........51
Hình 3-52 Vi điều khiển STM32F407 DISCOVERY..................................................52
Hình 4-1 Giao diện điều khiển Robot Scara.................................................................55
Hình 4-2 Sơ đồ Simulink điều khiển Robot..................................................................56
Hình 4-3 Kết quả mô phỏng động học thuận................................................................57
Hình 4-4 Kết quả mô phỏng động học nghịch..............................................................57
Hình 4-5 Điều khiển quỹ đạo tròn.................................................................................58
Hình 4-6 Điều khiển quỹ đạo xoắn ốc...........................................................................58
Hình 4-7 Robot về vị trí gốc..........................................................................................58
DANH MỤC BẢNG BIỂU
Bảng 3-1 Bảng D-H Robot SCARA 3 bậc tự do.............................................................8
Bảng 3-2 Vị trí trọng tâm của các khâu.........................................................................14
Bảng 3-3 Thông số quỹ đạo chuyển động.....................................................................21
Bảng 3-4 Bảng thông số động cơ servo sử dụng trong Robot SCARA........................29
Bảng 3-5 Bảng Routh hàm truyền 1..............................................................................31
Bảng 3-8 Thông số của PID Controller.........................................................................37
Bảng 3-9 So sánh biến tần và bộ điều khiển Servo.......................................................46
CHƯƠNG 1. TỔNG QUAN VỀ ROBOT SCARA
Chương này, em sẽ trình bày sơ lược về lịch sử hình thành của Robot SCRA 3 bậc
tự do. Đưa ra một số đánh giá về ưu điểm, nhược điểm và giới thiệu một số loại Robot
này. Em cũng đã trình bày tóm tắt về yêu cầu của đồ án bao gồm yêu cầu về hệ thống
cơ khí, hệ thống điện, phần mềm và cách điều khiển.
1.1 Giới thiệu chung về Robot SCARA
1.1.1 Giới thiệu chung
SCARA là viết tắt của “Selective Compliance Assembly Robot Arm” hay “Selective
Compliance Articulated Robot Arm”.
Cánh tay của nó cứng theo trục Z và dễ uốn trong các trục XY, Nhờ bố trí khớp trục
song song của SCARA, cánh tay có thể di chuyển trong mặt phẳng XY nhưng cứng
nhắc theo hướng 'Z', (Selective Compliance). Điều này là thuận lợi cho nhiều loại hoạt
động lắp ráp, tức là, chèn một pin tròn vào một lỗ tròn mà không ràng buộc.
Thuộc tính thứ hai của SCARA là bố trí cánh tay hai liên kết tương tự như cánh tay
người của chúng ta (Articulated). Tính năng này cho phép cánh tay mở rộng vào các
khu vực hạn chế và sau đó rút lại hoặc "gập lại" ra khỏi đường đi. Điều này thuận lợi
cho việc chuyển các bộ phận từ ô này sang ô khác hoặc để tải/dỡ tải các trạm xử lý
được đính kèm.
SCARA thường nhanh hơn các hệ thống robot Descartes tương đương. Giá treo bệ
đơn của họ yêu cầu một dấu chân nhỏ và cung cấp một hình thức lắp dễ dàng, không
bị cản trở. Mặt khác, SCARA có thể đắt hơn các hệ thống Descartes tương đương và
phần mềm điều khiển yêu cầu chuyển động học nghịch đảo cho các chuyển động nội
suy tuyến tính, Tuy nhiên, Phần mềm này thường đi kèm với SCARA và thường minh
bạch đối với người dùng cuối.
Hình 1-1 Robot SCARA
1.1.2 Ứng dụng của Robot SCARA

Scara robot là một trong những loại robot công nghiệp mang tính ứng dụng cao
nhất hiện nay. Chuyển động tay máy của Scara giúp nó có khả năng gắp đặt vật liệu
hoàn hảo. Vì vậy, nó được ứng dụng nhiều nhất trong những dây chuyền sản xuất có
1
công đoạn công việc này. Hơn nữa, nó cũng được sử dụng cho các công việc liên quan
đến hàn xì ở các xưởng cơ khí chế tạo.
Scara robot applications – ứng dụng của Scara – luôn là đề tài được nhiều doanh
nghiệp quan tâm. Liệu robot này có mang đến được những hiệu quả công việc tối ưu,
thay thế cho sức người ở những công việc nặng nhọc? Thực tế đã chứng minh loại
robot cánh tay máy này hoàn toàn đáp ứng vượt trội những kỳ vọng của con người.
Trong sự phát triển công nghiệp hóa, hiện đại hóa, cánh tay máy đóng vai trò quan
trọng trong việc thay thế con người thực hiện được những công việc phức tạp với thời
gian làm việc 24/24, công suất tốt và không có sai sót. Trong những môi trường làm
việc độc hại, nhiễm phóng xạ hoặc chất độc, sự xuất hiện của Scara chính là biện pháp
tốt nhất để đảm bảo an toàn cho người lao động.
Hình 1-2 Ứng dụng của Robot SCARA
1.1.3 Ưu điểm của Robot SCARA

Loại robot này có rất nhiều ưu điểm nổi bật:
 Tốc độ: Được thiết kế tự động hóa hoạt động và có lập trình sẵn, tốc độ của
Scara tốt hơn nhiều so với tốc độ của con người. Nó không cần e ngại bất cứ
điều kiện làm việc nào, vẫn có thể đảm bảo được tốc độ hoàn hảo như cài đặt
dù trong môi trường nhiệt độ cao hay hóa chất.
 Độ chính xác tuyệt đối: Khi yêu cầu của doanh nghiệp là kiểm soát các lực
lượng càng chính xác càng tốt, thì Scara có lợi thế nổi bật hơn hẳn. Ví dụ trong
cơ khí chế tạo máy hay lắp ráp điện tử, quy trình lắp ráp yêu cầu phải chèn 1
chốt, đảm bảo nó được gài đúng cách thì việc căn chỉnh hoàn hảo để thực hiện
là một vấn đề. Với Scara, nó có thể kiểm soát được lực định hướng, vì vậy có
thể tác động thao tác chính xác với lực phù hợp, vừa đảm bảo hoàn thành công
việc vừa không ảnh hưởng đến các linh kiện hay thiết bị cần hoàn thiện.
 Độ lặp lại: Một công việc mang tính lặp đi lặp lại sẽ khiến con người cảm thấy
rất mệt mỏi và chán nản. Thậm thái thái độ làm việc cũng sẽ bị tác động không
2
nhỏ. Trong khi đó, Scara hoàn toàn có thể khắc chế được các vấn đề này. Đây là
ưu điểm chung của robot, và vì sao robot ngày càng được nghiên cứu mở rộng
để thay thế con người trong những công việc chân tay mang tính tuần hoàn liên
tục.
1.1.4 Nhược điểm của Robot SCARA
 Tải trọng: Tải trọng của Robot Scara thường khá là nhỏ so với các Robot tay
máy công nghiệp khác với tải trọng tối đa chỉ khoảng 30kg. Đa phần các Robot
Scara thường chỉ có tải trọng trong khoảng 5-15kg.
 Độ linh hoạt: Trục z của Robot scara dễ bị hạn chế về chiều dài hoạt động hơn
so với các cấu trúc robot Descartes, hay robot toạ độ cầu.
1.2 Một số loại Robot SCARA
Hình 1-3 Robot SCARA Denso HM-G
Hình 1-4 Robot SCARA Fanuc SR 6iA và Robot SCARA Epson
1.3 Yêu cầu thiết kế
3
1.3.1 Hệ thống cơ khí
SCARA là một cấu hình tiêu chuẩn giữa các robot. Giai đoạn thiết kế sơ bộ, chúng
ta có những điểm sau đây:
 Robot SCARA 3 bậc tự do
 Tải trọng: 5kg
 Tầm với: 970mm
 Hình trình trục vít me (z): 400mm
 Độ chính xác lặp (x, y) = ± 0.02 mm , (z) = ± 0.01 mm
 Sử dụng bộ truyền vít me – đai ốc cho khâu tịnh tiến.
1.3.2 Hệ thống điện tử
Các thiết bị điện tử liên quan phụ thuộc hoàn toàn vào loại động cơ được sử dụng.
Để đạt độ chính xác cao, chúng ta có thể sử dụng động cơ bước hoặc động cơ servo
trong bộ truyền động chung. Do đó, phần chính của hệ thống điện tử liên quan đến
việc thiết kế các mạch điều khiển động cơ bước (servo) thích hợp. Ngoài phần mềm
điều khiển được sử dụng, đây là yếu tố quan trọng nhất, có thể ảnh hưởng lớn đến hiệu
suất của robot. Ở đây, chúng ta chỉ tập trung vào các trình điều khiển động cơ bước
(servo).
Một số lựa chọn ban đầu cần phải chú ý:
 Loại động cơ: Đơn cực hoặc lưỡng cực.
 Nguyên lý ổ đĩa: L/R hoặc PWM.
 Chế độ hoạt động: Nửa bước hoặc toàn bước.
 Đặc trưng mô-men xoắn – tốc độ.
1.3.3 Phần mềm và điều khiển
Phần mềm điều khiển cần thực hiện các nghĩa vụ sau:
 Đường dẫn liên tục trong không gian.
 Chuyển động xen kẽ (Join – interpolated motion).
 Nội suy cung tròn.
 Nội suy tuyến tính.
 Phối hợp chuyển động tuyến tính: tất cả các trục khởi động và dừng các bước
chuyển động cùng một thời gian.
1.4 Kết luận
Qua yêu cầu của đồ án đưa ra, và việc tìm hiểu về robot SCARA trong công nghiệp,
em đã có đánh giá sơ lược về Robot và các nội dung cần tiến hành. Ở chương sau em
sẽ tiếm hành tìm hiểu và phân tích nguyên lý và các thông số kỹ thuật quan trọng của
Robot SCARA.
4
CHƯƠNG 2. PHÂN TÍCH NGUYÊN LÝ VÀ THÔNG SỐ KỸ THUẬT
Trong chương này, em sẽ tiến hành phân tích nguyên lý hoạt động và các thông số
kỹ thuật quan trọng liên quan đến robot SCARA… Các thông số này có thể bao gồm
khả năng tải, bán kính làm việc, độ chính xác vị trí, tốc độ di chuyển và độ ổn định....
Bằng việc phân tích nguyên lý hoạt động và các thông số kỹ thuật, em sẽ có cái
nhìn tổng quan về tính năng và khả năng của robot SCARA. Điều này giúp hiểu rõ hơn
về ưu điểm và giới hạn của loại robot này và cung cấp cơ sở để đánh giá xem robot
SCARA có phù hợp với nhu cầu và yêu cầu của ứng dụng cụ thể hay không.
2.1 Nguyên lý hoạt động
2.1.1 Phân tích nguyên lý hoạt động
 Robot hoạt động dựa trên sự chuyển động quay của các động cơ điện (động cơ
bước, động cơ servo), các động cơ này được đặt tại các khớp. Tại khớp một và
khớp hai, động cơ liên kết với khớp xoay qua dây đai hoặc bánh răng để truyền
chuyển động. Khớp ba là khớp tịnh tiến sử dụng bộ trục vít me đai ốc bi để biến
chuyển động quay từ động cơ sang chuyển động tịnh tiến. Kết hợp với các cảm
biến để giới hạn chuyển động cũng như đạt được vị trí, tốc độ chính xác.
 Động cơ và cảm biến được điều khiển bằng hệ thống máy tính – controller
thông qua các chương trình phần mềm Matlab/Simulink, G-code, . . .
2.1.2 Các thông số kỹ thuật quan trọng
 Số khâu điều khiển: 3
 Tầm với (chiều dài cánh tay): 970 mm
 Vùng làm việc:
- Trục quay khớp 1: ± 165 °
- Trục quay khớp 2: ± 147 °
- Hành trình trục vít me (z) khớp 3: 0÷ 400 mm
 Tải trọng tối đa: 10kg
 Độ chính xác lặp:
- X-Y: ± 0.02 mm
- Z: ± 0.01 mm
2.2 Xác định các thành phần của hệ thống điều khiển
2.2.1 Động cơ
Robot SCARA dùng động cơ Servo hoặc Stepper để điều khiển chuyển động của
các khớp.
a. Động cơ bước (Stepper Motor)
Động cơ Stepper là động cơ điều khiển vòng hở, tức là không có phản hồi về vị trí,
tốc độ. Hoạt động dựa trên sự cấp điện tuần hoàn cho các cuộn dây để tạo từ thông
biến thiên khiến cho lõi động cơ (thường là nam châm) quay. Tốc độ của động cơ phụ
5
thuộc vào tần số mà cuộn dây được cấp điện và ngắt điện – “step sate”. Stepper motor
có hai cách điều khiển là nửa bước (half-step) và một bước (full - step).
Hình 2-5 Động cơ bước
b. Cảm biến:
Cảm biến là thiết bị nhận “stimulus” và trả về tín hiệu điện. “Stimulus” là tính chất,
thuộc tính, . . . được cảm nhận và chuyển thành tín hiệu điện.
2.2.2 Bộ nguồn
Thực tế không thể điều khiển trực tiếp động cơ servo công suất lớn trực tiếp từu bộ
vi điều khiển vì nguồn ra của vi điều khiển nhỏ (+5V), vì vậy cần một bộ điều khiển
(driver) cho servo motor, với nguồn cấp cho driver này phải đủ lớn để đảm bảo cường
độ dòng ra điều khiển động cơ lớn (cỡ A). Vì vậy ở đây để điều khiển động cơ thì phải
cấp nguồn cho hai thiết bị là vi điều khiển (có thể dùng pin với tổng nguồn từ 5V đến
12V) và driver (dùng bộ nguồn riêng dành cho driver).
Hình 2-6 Nguồn Adaptor tổ ong
6
2.2.3 Bộ điều khiển động cơ
Tùy mỗi hãng sản xuất servo motor sẽ có bộ driver riêng để điều khiển. Trong bộ
driver sẽ có đầu kết nối với servo motor, rắc cắm cho thành phần phản hồi (feedback)
encoder, và các đầu cấp nguồn, thêm vào đó là các điểm giao tiếp với máy tính hay vi
điều khiển (RX, TX).
Hình 2-7 Driver điều khiển động cơ
Giao tiếp với bộ điều khiển:

 Giao tiếp với driver thông qua giao tiếp UART (2 chân RX, TX). Có hai cách
kết nối là qua máy tính và qua vi điều khiển.
 Đối với kết nối qua máy tính: dùng cáp kết nối như RS232 và sử dụng phần
mềm để giao tiếp.
 Đối với kết nối qua vi điều khiển: dùng dây kết nối giữa các chân RX, TX của
vi điều khiển với driver, xuất code sang vi điều khiển để driver có thể hoạt
động.
Hình 2-8 Dây cáp RS 232, Arduino Mega
2.3 Kết luận

Sau khi hoàn thành chương 2, em đã tìm hiểu được nguyên lý hoạt động của Robot
SCARA, các thông số kỹ thuật quan trọng, hiểu rõ hơn về tổng quan của hệ thống điều
7
khiển như động cơ, cảm biến, bộ nguồn… Từ đó hiểu được cấu tạo và nguyên lý, giúp
cho phần thiết kế hệ thống điều khiển trong chương 3.
8
CHƯƠNG 3. THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN
Robot Scara là một robot có tính ứng dụng cao nhất hiện nay. Trong công nghiệp
robot được sử dụng một cách tự động với một quỹ đạo được người thiết kế đặt ra sao
cho phù hợp với mục đích ứng dụng ví dụ như gắp thả linh kiện, lắp ráp linh kiện nhỏ,
khắc lazer, in 3D, hàn,… Với các quỹ đạo đó thì cần có một hệ thống điều khiển đảm
bảo tính đáp ứng liên tục cho robot. Nên trong chương này em sẽ thiết kế một quỹ đạo
và xây dựng hệ thống điều khiển robot với quỹ đạo đó.
3.1 Động học Robot SCARA
3.1.1 Động học thuận Robot SCARA
Hình 3-9 Sơ đồ động học Robot SCARA 3 bậc tự do
Ta có bảng D-H như sau:

Bảng 3-1 Bảng D-H Robot SCARA 3 bậc tự do
Khâu θ d a α
1 q1 d1 a1 0
2 q2 d2 a2 0
9
3 0 q3 0 0
Với biến tại các khớp (Tọa độ suy rộng): q=[q1 q2 q3 ]T
{
q 1 ϵ [−165 ° ; 165 °]
Miền giá trị của biến khớp: q2 ϵ [−147 ° ; 147 °]
q3 ϵ [0 ; 400 mm]
{
d 1=500(mm)
d 2=100(mm)
Tham số động học:
a1=500 ( mm )
a2=450(mm)
 Ma trận biến đổi thuần nhất giữa hai khâu liên tiếp là:
[ ]
cos q 1 −sin q 1 0 a1 cos q 1
sin q 1 cos q 1 0 a1 sin q 1 (3-1)
T 01= A1=
0 0 1 d1
0 0 0 1
[ ]
cos q 2 −sin q 2 0 a2 cos q 2
sin q 2 cos q 2 0 a2 sin q 2 (3-2)
T 12= A2=
0 0 1 d2
0 0 0 1
0
T 2= A 1 A 2
[ ]
cos q12 −sin q12 0 a2 cos q 12+ a1 cos q1
sin q12 cos q12 0 a 2 sin q 12+ a1 sin q 1 (3-3)
¿
0 0 1 d1
0 0 0 1
[ ]
1 0 0 0
2 0 1 0 0 (3-4)
T 3= A 3=
0 0 1 q3
0 0 0 1
0 0 1 2
T 3=T 1 T 2 T 3
[ ]
cos q12 −sin q12 0 a2 cos q 12+ a1 cos q1
sin q12 cos q12 0 a 2 sin q 12+ a1 sin q 1 (3-5)
¿
0 0 1 d 1+ d 2+ q3
0 0 0 1
Với: q 12=q1 +q 2
 Vị trí và hướng điểm thao tác:
10
Tọa độ điểm thao tác:
{
x=a2 cos q 12+ a1 cos q1 (3-6)
y=a2 sin q 12+a 1 sin q 1
z=d1 + d2 +q 3
{
x=450 c θ12+ 500 c θ1
Phương trình động học thuận của Robot: y=450 s θ 12+500 s θ1
z=600+q 3
Sử dụng phần mềm Matlab , ta tìm được không gian làm việc của Robot:
Hình 3-10 Không gian làm việc của Robot
Hình 3-11 Không gian làm việc của Robot 2D
11
Không gian chuyển động của Robot chuyển động theo giá trị biến khớp
q 1=(−16 5∘ ÷16 5∘ ) ; q 2=(−14 7 ∘ ÷ 14 7∘ ) ; q3 =( 0 ÷ 400 mm ) . Chuyển động theo trục
z=( 200 ÷ 600 mm ) , tầm với trong mặt phẳng Oxy là 970mm.
a. Xác định hướng của khâu thao tác

- Sử dụng ma trận định hướng của khâu thao tác là ma trận Cardan:
[ ]
cosβcos η −cosβsin η sinβ
sinαsinβcos η+ cosαsinη −sinαsinβsin η+cosαcosη −sinαcosβ
−cosαsinβcos η+ sinαsinη cosαsinβsin η+ sinαcosη cosαcosβ
- Ma trận cosin chỉ hướng của khâu thao tác là:
[ ]
cos q12 −sin q 12 0 (3-7)
0
R = sin q 12 cos q 12 0
3
0 0 1
- Đồng nhất hệ số hai phương trình và giải ta được:
{
α =0 ° (3-8)
β=0 °
η=q1 +q 2
Kết quả trên chính là hướng của khâu thao tác cuối biểu diễn theo 3 góc Cardan.
b. Vận tốc của điểm thao tác cuối và vận tốc góc các khâu
 Vận tốc khâu cuối:
[ ][ ]
−a2 sin q12−a1 sin q 1 −a2 sin q 12 0 q̇ 1
0 δ r 0E (q)
v=
3 = a2 cos q12 +a 1 cos q1 a 2 cos q12 0 . q̇ 2 =J TE q̇(3-9)
δq
0 0 1 q̇ 3
 Vận tốc các khâu:
Ta có: w oi = Ṙ i . RTi nên:
 Vận tốc góc khâu 1:
[]
0
w 01= Ṙ01 R01 T = 0 (3-10)
q̇ 1
[ ]
0 (3-11)
0 0 0T
w 2= Ṙ2 R2 = 0
q̇ 1 + q̇2
[ ]
0 (3-12)
0 0 0T
w 3= Ṙ3 R3 = 0
q̇ 1 + q̇2
12
3.1.2 Động học ngược Robot SCARA
Bài toán động học ngược có vai trò rất quan trọng trong lập trình và điều khiển
chuyển động của robot. Bởi vì trong thực tế thường cần điều khiển robot sao cho tay
kẹp di chuyển tới các vị trí nhất định trong không gian thao tác theo một quy luật nào
đó.
Yêu cầu của bài toán động học ngược là từ tọa độ, góc quay trong không gian thao
tác tính toán được tọa độ khớp trong không gian khớp. Có nhiều cách để giải bài toán
động học ngược từ hình học đến giải tích. Tuy nhiên trong báo cáo này, em xin trình
bày phương pháp giải tích để giải bài toán động học ngược.
Với robot Scara 3 bậc tự do thì bài toán động học ngược là tọa độ của điểm tác động
khâu cuối trong không gian thao tác kết hợp với góc quay theo trục z của khâu tác
T
động cuối: [ x E y E z E θ E ]
Từ phương trình động học, ta có:
x E =a 2 cos q12+ a1 cos q 1( 1)
y E =a 2 sin q 12+a 1 sin q 1 (2)
z E=d1 +d 2 +q 3 (3)
Bình phương hai vế của (1) và (2) ta được:
¿ (3-13)
Cộng hai vế của phương trình trên ta được:

2 2 2 2
x E + y E =a1 + a2 +2 a1 a2 cosq 2
2 2 2 2
x E + y E −(a1 + a2 )
→ cosq 2=
2 a1 a2
( )
2 2 2 2
x + y E −(a 1 + a2 )
→ q2=θ 2=arccos E
2 a1 a2
Biến đổi phương trình (1) và (2) về hệ phương trình có 2 ẩn là cosq 1 và sinq1:
{ x E =( a2 cosq 2+ a1 ) cosq 1+(−a2 sinq2 )sinq1

y E=a2 sinq2 cosq 1+(a 2 cosq 2 +a1 ) sinq1
Áp dụng Kramer’s rule:
¿
(3-14)
( )
sinq1 −a2 sinq2 . x E + ( a 2 cosq 2 +a 1(3-15)
). yE
→ q1=arctan =arctan ⁡( )
cosq 1 ( a2 cosq 2+ a1 ) . x E +a 2 sinq2 . y E
Từ phương trình (3) có: q 3=z E −d 1−d2
Kết luận: Kết quả của bài toán động học ngược:
13
{ ( )
sinq 1 −a 2 sinq2 . x E + ( a2 cosq 2+ a1 ) . y E
q1 =θ1=arctan =arctan ⁡( )
cosq 1 ( a 2 cosq 2 +a 1) . x E + a2 sinq2 . y E
q 2=θ2 =arccos (x E2 + y E2−(a12 +a 22 )
2 a1 a2 )
q 3=z E −d 1−d 2
3.2 Động lực học Robot SCARA
Hình 3-12 Hệ tọa độ khối tâm của Robot
Hình trên là bản vẽ kỹ thuật của một robot SCARA 3 bậc tự do.
Cánh tay robot được cấu tạo từ ba trục, mỗi trục có một khớp nối. Trục Z là trục chính,
chịu trách nhiệm cho chuyển động lên xuống của cánh tay robot. Trục X chịu trách
nhiệm cho chuyển động sang trái và phải của cánh tay robot. Trục Y chịu trách nhiệm
cho chuyển động xoay của cánh tay robot.
Đầu robot được gắn ở cuối cánh tay robot. Đầu robot có thể được sử dụng để cầm nắm
các vật thể hoặc thực hiện các thao tác khác.
Trong đó:
- ( x 0 , y o , z 0): hệ tọa độ cơ sở
- ( x ci , y ci , z ci ): hệ tọa độ khối tâm của từng khâu
- Oci: gốc tọa độ của khối tâm từng khâu
- l i là khoảng cách từ tâm Ci đến gốc tọa độ Oi
- d i, a i: là các tham số được xây dựng theo quy tắc D-H
14
3.2.1 Động lực học thuận
Để tính toán động lực học Robot, ta sẽ đi thiết lập phương trình vi phân chuyển
động của robot. Phương trình vi phân chuyển động của robot được xây dựng theo
phương trình Lagrange loại II có dạng tổng quát như sau:
( )
d ∂T
−
∂ T −∂ Π
dt ∂ q̇ i ∂ q̇i
=
∂ qi
kt
+Qi , i=1 ,n
(3-16)
Với:
 T – Động năng của hệ Robot
 Π - Thế năng của hệ Robot
¿
 Q - Vector lực suy rộng không thế
 i=1 ÷ n với n là số bậc tự do (n=3)
Trong tính toán thiết kế robot người ta thường sử dụng dạng ma trận của phương
trình Lagrange loại II để thuận lợi trong sử dụng các công cụ toán học và tiến hành mô
phỏng trên máy tính. Phương trình vi phân chuyển động của Robot có dạng:
M ( q ) q̈+C ( q , q̇ ) q̇ +G ( q ) +Q=U (3-17)
Trong đó:
 M(q) là ma trận khối lượng
 C ( q , q̇ ) là ma trận Coriolis
 G ( q ) là vector trọng lực
 Q là vector ngoại lực suy rộng
t
[τ1 τ 2 τ 3 ] là vector lực dẫn động tại các khớp. Với khớp tịnh tiến τ i là lực F mi, với
khớp quay thì τ i là ngẫu lực có momen M mi.
Bảng 3-2 Vị trí trọng tâm của các khâu
Khâ Vị trí trọng tâm Khối Ma trận quán tính Θ

u xc yc zc lượng I xx I yy I zz ❑xy ❑ yz ❑zx
1 −l 1 0 0 m1 I 1 xx I 1 yy I 1 zz 0 0 0
2 −l 2 0 0 m2 I 2 xx I 2 yy I 2 zz 0 0 0
3 0 0 −l 3 m3 I 3 xx I 3 yy I 3 zz 0 0 0
 Xét khâu 1:
- Giả thiết khâu 1 là một thanh phẳng đồng chất, tiết diện ngang không đáng kể
và có trọng tâm đặt tại trung điểm của thanh.
T
- Tọa độ khối tâm C1 trong hệ tọa độ R1: r C = [− l1 0 0 ]
1
1
Với: l 1 là khoảng cách từ tâm C1 đến gốc tọa độ O1

- Tọa độ khối tâm C1 trong hệ tọa độ R0:
15
r C =r O + R1 r C =[(a ¿ ¿ 1−l 1 )cos q1 ¿(a ¿ ¿ 1−l 1)sin q 1 ¿ d 1 ]
0 0 0 1
1 1 1
(3-18)
- Ma trận Jacobi tịnh tiến:
[ ]
−(a ¿ ¿ 1−l 1 )sin q1 ¿ 0 0
δ r 0C
JT = = (a ¿ ¿ 1−l 1 )cos q1 ¿ 0 0 0 ¿
1
δq 1 (3-19)
0 ¿
- Ma trận Jacobi quay:
[ ]
(3-20)
δ w1 0 0 0
0
JR = =0 0 0
1
δq
1 0 0
+ Ma trận Momen quán tính khối:
[ ]
0 0 0
[ ]
2
I 1 xx 0 0 m1 a1 (3-21)
1 0 0
I = 0 I 1 yy 0 =
C1 12
2
0 0 I 1 zz m1 a 1
0 0 0
12
 Xét khâu 2:
- Giả thiết khâu 2 là một thanh phẳng đồng chất, có tiết diện ngang không đáng
kể và có trọng tâm đặt tại trung điểm của thanh.
T
- Tọa độ khối tâm C2 trong hệ tọa độ R2: r C = [−l 2 0 0 ]
2
2

r C =r O + R2 r C =[ (a ¿ ¿ 2−l 2)cos q 12+a 1 cos q1 ¿(a ¿ ¿ 2−l 2)sin q 12+a 1 sin q 1 ¿ d 1 +d 2 ]
0 0 0 2
2 2 2
(3-22)
[ ]
−(a ¿ ¿ 2−l 2)sin q12−a 1 sin q 1 ¿−(a ¿ ¿ 2−l 2) sin q12 ¿ 0
δ rC
0
JT = = (a ¿ ¿ 2−l 2 )cos q12 +a1 cos q 1 ¿ (a ¿ ¿ 2−l 2)cos q 12 ¿ 0 0¿0¿

2
2
δq
¿
(3-23)
[ ]
(3-24)
δ w2 0 0 0
0
JR = =0 0 0
2
δq
1 1 0
16
[ ]
0 0 0
[ ]
2
I 2 xx 0 0 m2 a2 (3-25)
2 0 0
I = 0 I 2 yy 0 =
C2 12
2
0 0 I 2 zz m 2 a2
0 0 0
12
 Xét khâu 3
- Giả thiết khâu 3 là một thanh phẳng đồng chất, tiết diện ngang không đáng kể
và có trọng tâm đặt tại trung điểm của thanh.
T
- Tọa độ khối tâm C3 trong hệ tọa độ R3:r C = [−l 3 0 0 ]
3
3

r C =r O + R3 r C =[ (a ¿ ¿ 2−l 2)cos q 12+ a1 cos q1 ¿(a ¿ ¿ 2−l 2)sin q 12+ a1 sin q 1 ¿ d1−l3 + d 2+ q3 ]
0 0 0 3
3 3 3
(3-26)
[ ]
δ r 0C −(a ¿ ¿ 2−l 2)sin q12−a1 sin q 1 ¿ 0 0
JT = = (a ¿ ¿ 2−l 2 )cos q12 +a 1 cos q1 ¿ 0 0 1 ¿
3
3
δq (3-27)
0 ¿
[ ]
(3-28)
δ w3 0 0 0
0
JR = =0 0 0
3
δq
1 1 0
[ ]
m3 l3
2 (3-29)
[ ]
0 0
I 3 xx 0 0 12
3
I = 0 I 3 yy
C3 0 = m3 l 23
0 0
0 0 I 3 zz 12
0 0 0
 Tính toán ma trận khối lượng:
3
(3-30)
M ( q ) =∑ J TTk mk J Tk + J TRk R 0k I kCk R0k T J Rk
k=1
[ ]
m11 (q) m12(q ) m13 (q) (3-31)
¿ m21 (q) m22(q ) m23 (q)
m31 (q) m32(q ) m33 (q)
 Khâu 1
17
[ ]
m 1 a1
2 (3-32)
0 0
J TT 1 m1 J T 1= 4
0 0 0
0 0 0
[ ]
2
m1 a 1
0 0
J TR 1 R01 I 1C 1 R01 T J R 1= 12 (3-33)
0 0 0
0 0 0
 Khâu 2:
[ ]
2 2 2
a2 a a2 a1 a2 cos q 2
+a 1 a 2 cosq 1 + 1 + 0
4 4 4 2
T
J T 2 m2 J T 2=m2 a2 a1 a2 cos q 2
2
a22
+ 0 (3-34)
4 2 4
0 0 0
[ ]
2 2
m 2 a2 m2 a2
0
12 12 (3-35)
T 0 2 0T
J R 2 R2 I C 2 R2 J R 2= m2 a22 m2 a22
0
12 12
0 0 0
 Khâu 3:
[ ]
a21 +a 22+ 2 a1 a2 cosq 2 a22 +a1 a2 cosq 2 0
T
J T 3 m3 J T 3=m3 a22 + a1 a2 cosq 2 a22 0 (3-36)
0 0 1
[ ]
0 0 0 (3-37)
T 0 3 0T
J R I
R3 3 C3 R J R 3= 0 0 0
3
0 0 0
[ ]
3 m11 (q) m12(q) m13 (q)
→ M ( q )=∑ J TTk mk J Tk + J TRk R0k I Ck
k
R 0k T J Rk = m21 (q ) m22(q) m23 (q)
k=1
m31 (q ) m32(q) m33 (q)
(3-38)
( )
2 2
m 1 a1 a2 2 2 2
Với: + m11= +m2 + a1 +a1 a2 c θ 2 +m3 (a 2 + a1 + 2 a1 a 2 c θ2 )
3 3
18
( )
2
a 2 a1 a2 c θ 2 2
+ m12=m21=m2 + + m 3 (a2 +a1 a2 c θ 2)
3 2
2
m a
+ m22= 2 2 + m3 a 22
3
+ m13=m31=m23=m32=0
+ m33=m3
 Tính toán thế năng, lực thế:

∂π
- Vecto G(p) là vecto thế năng với các thành phần Gi= ∂ q . Đặt gốc thế năng tại chân
i
khâu cố định, dễ thấy thế năng của Robot cũng chính là tổng thế năng của các khâu.
Thế năng Robot: Π=Π 1+ Π 2 + Π 3
T
Với g= [ 0 0 −g ] (3-39)
Thế năng khâu 1:

Π 1=m1 g z c 1=m1 g d 1 (3-40)

Π 2=m2 g z c 2=m2 g (d ¿ ¿ 1+d 2 )¿ (3-41)

Π 3=m3 g z c3=m3 g(d ¿ ¿ 1+d 2−d 3+ l3 )¿ (3-42)
Lực thế:
[ ][
G1
]
T
∂Π ∂Π ∂Π T
G ( q )= G 2 = =[ 0 0 −m3 g ]
∂ q1 ∂ q2 ∂ q3 (3-43)
G3
 Ma trận Coriolis C
- Ma trận đặc trưng cho lực quán tính Coriolis và lực quán tính li tâm C ( q , q̇ ) xác
định bởi:
c 11 (q , q̇) c12 (q , q̇ ) c 13 (q , q̇) (3-44)
⌈ c 21(q , q̇) c22 (q , q̇ ) c 23 (q , q̇) ⌉
c 31(q , q̇) c32 (q , q̇ ) c 33 (q , q̇)
- Các phần tử của ma trận này được tính theo công thức Christoffel:
1
3
∂ m + ∂mik ∂ m jk (3-45)
c ij ( q , q̇ )= ∑ ( ij ¿
2 k=1 ∂ qk ∂ q j
−
∂ qi
). q̇k ¿
- Sử dụng phần mềm Matlab, kết quả tính toán như sau:
m2 (3-46)
c 11 ( q , q̇ )=−( +m 3)a1 a2 q̇2 sinq 2
2
m2 (3-47)
c 12 ( q , q̇ )=−( +m3 )( q̇1 + q̇ 2)a1 a2 sinq2
2
19
m2 (3-48)
c 21 ( q , q̇ )=( +m3)a 1 a 2 q̇ 1 sinq2
2
c 13 ( q , q̇ ) =b c 22 ( q , q̇ )=c23 ( q , q̇ )=c 31 ( q , q̇ )=c32 ( q , q̇ )=c 33 ( q (3-49)
, q̇ )
 Lực suy rộng của các lực không thế Q
Trong nội dung đồ án, Robot được coi là mô hình lý tưởng, do đó bỏ qua lực ma
sát, lực cản nhớt. Lực không thế ở đây chỉ bao gồm lực do vật tác động lên khâu thao
tác cuối.
Xét trường hợp tổng quát với lực và momen có giá trị:
[] []
Fx Mx
F 3= F y M 3= M y
Fz Mz
Lực suy rộng được xác định theo công thức

T 0
Q=U + J E F E
Trong đó:
[ ]
0
0
- Lực tác động lên khâu cuối là F = 0 E
−F z
[]
τ1
- U = τ 2 là lực dẫn động τ i của động cơ đặt tại các khớp.
τ3
[ ][ ]
0 τ1
Vậy Q= 0 + τ 2 là vector lực suy rộng cần xác định.
−F z τ 3
Kết luận
Kết quả của bài toán động lực học:
[ ][ ][ ] [ ] [ ] [ ]
m11 ( q ) m12 ( q ) 0 c 11 ( q , q̇ ) c 12 ( q , q̇ ) 0 q̇ 1 0 0 τ1
m21 ( q ) m22 ( q ) 0 + c 21 ( q , q̇ ) 0 0 q̇ 2 + 0 = 0 + τ 2
0 0 m33 ( q ) 0 0 0 q̇ 3 m3 g −F z τ 3
Hay lực điều khiển tại các khớp:
[]
τ1 m11 ( q ) q̈ 1+ m12 ( q ) q̈2 + c11 ( q , q̇ ) q̇1 +c 12 ( q , q̇ ) q̇ 2
τ = τ 2 =⌈ m21 ( q ) q̈ 1+ m22 ( q ) q̈ 2+ c21 ( q , q̇ ) q̇ 1 ⌉
τ3 m33 ( q ) q̈ 3 +m3 g+ F z
20
3.2.2 Động lực học ngược
Nhiệm vụ của bài toán động lực học ngược là tìm ra lực dẫn động τ khi đã biết
chuyển động của Robot bao gồm các vecto q ( t ) , q̇ ( t ) , q̈ ( t ). Chỉ cần thay vào phương
trình với các thành phần đã được thiết lập ở trên để tìm các lực và ngẫu lực dẫn động
tại các khâu 1,2,3 của Robot.
M (q) q̈+ C(q , q̇) q̇+G(q)=τ
Sau khi nghiên cứu động lực học, ta đã tính toán các lực khớp và các mô – men tại
các góc khớp phát sinh trong quá trình chuyển động của Robot. Từ đó hỗ trợ trong
việc tính công suất của các khâu 1,2,3 và tiến hành chọn động cơ phù hợp cho cơ cấu.
3.3 Thiết kế quỹ đạo chuyển động
Để khảo sát quỹ đạo làm việc của bài toán đặt ra , ta chọn quỹ đạo làm việc gắp thả
vật để khảo sát :
Hình 3-13 Quỹ đạo chuyển động của Robot
Quỹ đạo của Robot bắt đầu từ điểm S là điểm khởi động của Robot, di chuyển đến
điểm A’ tiếp theo di chuyển theo trục z đến điểm A sau đó Robot dừng gắp vật. Sau
khi gắp vật xong, Robot di chuyển đến điểm B’ tiếp theo di chuyển đến điểm B theo
trục z sau đó Robot dừng thả vật.
Toạ độ các điểm:
S(0.97, 0, 0.6) là điểm khởi động của Robot.
A(0, 0.4, 0.3); A’(0, 0.4, 0.5); B(0.51, 0, 0.3); B’(0.51, 0, 0.5)
Bảng 3-3 Thông số quỹ đạo chuyển động
Quỹ SA’ A’A Dừng gắp AA’ A’B’ B’B Dừng thả BB’
đạo vật vật
Thời 2 2 1 2 3 2 1 2
gian(s)
21
Để thiết kế được quỹ đạo cho Robot, người ta thường dùng phương pháp xấp xỉ đa
thức bậc n hoặc dạng phối hợp. Trong nội dung báo cáo này nhóm sử dụng quỹ đạo
theo đa thức bậc 3. Vì dạng này vừa đơn giản trong thiết kế đồng thời thỏa mãn được
các yêu cầu như: Đảm bảo tính liên tục của vận tốc → quỹ đạo không giật cục, gấp
khúc, gây sốc trong quá trình hoạt động của Robot.
Ta có phương trình đường thẳng đi qua 2 điểm M (x 0 , y 0 , z 0) và N (x e , y e , z e )
x−x 0 y− y 0 z−z 0 (3.50)

= =
x e−x 0 y− y 0 z−z 0
Độ dời dịch chuyển 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:
{ x=a 0+ a1 t +a 2 t + a3 t
2 3 (3.51)
ẋ=a1 +2 a2 t+3 a3 t 2
ye− y0 y0 xe− y e x0 (3.52)

y= x+
x e −x 0 x e −x 0
z e −z 0 z 0 x e −z e x 0 (3.53)
z= x+
x e −x 0 x e −x 0
Với thời gian đặt cho quỹ đạo là t e (s) để đi từ M đến N theo một đường thẳng. Với
điều kiện đầu:
{
x E ( 0 )=x 0
ẋ E ( 0 ) = ẋ0 =0 (3.54)
x E ( t e ) =x e
ẋ E ( t e ) = ẋ e =0
Ta xác định được các hệ số:
{
a 0=x 0
a1 =0 (3.55)
3( x e −x 0 )
a2=
t 2e
−2(x e −x 0 )
a3=
t 2e
Sơ đồ Simulink thiết kế quỹ đạo
22
Hình 3-14 Mô hình Simulink thiết kế quỹ đạo chuyển động
Quỹ đạo đặt ban đầu của Robot là quỹ đạo bậc 3 theo thời gian có được giá trị điểm
kết thúc E, giải động học ngược có được kết quả biến khớp. Đây sẽ là quỹ đạo làm
việc làm tín hiệu đầu vào để thiết kế bộ điều khiển.
Quỹ đạo các biến khớp:
Hình 3-15 Quỹ đạo các biến khớp khâu 1
23
Hình 3-16 Quỹ đạo biến khớp khâu 2
Hình 3-17 Quỹ đạo biến khớp khâu 3
Vậy ta đã có các biến khớp ứng với quỹ đạo đã thiết kế. Các biến khớp này sẽ được
sử dụng làm đầu vào khi thiết kế bộ điều khiển.
24
3.3.1 Mô hình hoá động cơ điện một chiều
Phương trình về cơ
Hình 3-18 Sơ đồ khối Robot bao gồm hộp số
Trong đó:
 motor driver: bộ điều khiển động cơ
 motor: động cơ
 encoder: cảm biến phản hồi tốc độ, vị trí của động cơ
 motor inertia: quán tính động cơ
 motor friction: ma sát động cơ
 gearbox: hộp giảm tốc
 load inertia: quán tính tải trọng
 J m là mô – men quán tính của động cơ
 J l là mô – men quán tính của khâu gắn trên khớp đó
 τ m là mô – men ở trục động cơ
 τ l là mô – men tải quán tính
 θm là góc quay ở trục động cơ
 θ s là góc quay ở tải
 Bm là hệ số giảm chấn của động cơ
 Bl là hệ số giảm chấn của tải
θs
Ta có hệ số giảm tốc: n=
θm
Sử dụng nguyên lý D’Alambert, ta có:
τ 1 −B 1 θ̇ s=J 1 θ̈ s (3.56)
Với:
∂θ s
θ̇ s= =ω s là tốc độ góc của tải (rad/ s)
dt
∂θ s ∂ ω s
θ̈ s= 2 = là gia tốc góc của tải (rad/ s2)
dt dt
Phương trình trên tương đương với:
25
τ 1 −B 1 w s=J 1 ẇ s (3.57)
Sử dụng nguyên lý trên cho trục động cơ chính ta được:

τ m−n τ 1−B m θ̇ m=J m θ̈m (3.58)
Kết hợp các phương trình trên ta thu được:

τ m=( J m+ n2 J l ) θ̈ m +(Bm +n 2 Bl ) θ̇m (3.59)
2
J td =J m+ n J l là mô – men quán tính tác động ở trục động cơ
2
Btd =Bm +n Bl là hệ số giảm chấn tác dụng ở trên trục động cơ
Ta thu được:
τ m=J td θ̈m + Btd θ̇m (3.60)
τ m=J td Ẇ m + Btd W m (3.61)
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 cứ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ừ i k được giữ không đổi.
Hình 3-19 Sơ đồ nguyên lý động cơ
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 cứng
di (3.62)
U =L + Ri+ K e w
dt
+ 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].
+ K e – hằng số sức điện động [Vs/rad].
+ K e ω – sức điện động phần ứng [V].
Biến đổi Laplace 2 vế phương trình, ta được:
U ( s )=LsI ( s ) + RI ( s ) + K e w( s) (3.63)
Sơ đồ khối tương ứng:
26
Hình 3-20 Sơ đồ khối mạch điện phần ứng
 Phương trình mô – men điện từ của động cơ:

Với dòng kích từ i k không đổi thì từ thông khe khí Φ=k 2 i k là không đổi và mô – men
điện từ M của động cơ tỷ lệ với dòng điện phần cứng:
M =K m i k (3.64)
Trong đó:
K m =k 1 Φ=k 1 k 2 i k là hằng số mô – men của động cơ, [N.m/A]
Với: + k 1 – hằng số phụ thuộc vào kết cấu động cơ.
+ k 2 – hằng số đặc trưng đoạn tuyến tính của từ thông thay đổi theo i k .
Biến đổi Laplace 2 vế ta được:
M ( s )=K m I (s) (3.65)
Sơ đồ khối tương đương:
Hình 3-21 Sơ đồ khối phần mô – men điện từ
 Phương trình cân bằng mô – men trên trục động cơ

dw (3.66)
M =J +Bw + M t
dt
Trong đó:
+ J – Mô – men 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 ]
+ M t – Mô – men phụ tải (nhiễu), [ N .m ]
Biến đổi Laplace 2 vế phương trình:
M ( s )=Jsw ( s ) + Bw ( s )+ M t ( s) (3.67)
Sơ đồ khối tương ứng:
27
Hình 3-22 Sơ đồ khối phần mô – men cân bằng 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 3-23 Sơ đồ khối động 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:
Km
w(s) (Ls + R)(Js + B) Km
Gdc = = ¿=
U (s) K m K e 2
LJ s + ( LB+ RJ ) s+(K m K e + RB)
(Js+ B)
Ls+ R ¿ (3.68)
Trong đồ án này, ta đ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 θm – góc quay của động cơ. Đạo hàm của góc quay là vận tốc góc:
θm ( t )=∫ w (t ) dt (3.69)
Chuyển sang miền tần số, ta được:

w (s ) (3.70)
θ s ( t )=
s
Hàm truyền của động cơ trở thành:
θ m ( s) Km
Gdc (s )= =
U (s) L J td s + ( L Btd + R J td ) s 2 + ( K m K e + R Btd ) s
3
(3.71)
L J td
Đặt τ t= là hằng số thời gian điện từ, τ c = là hằng số thời gian cơ.
R Btd
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:
28
θm( s ) K td (3.72)
Gdc ( s )= =
U ( s ) T 1 s +T 2 s2 + s
3
Trong đó:
τ t τ c R B td (τ + τ )R Btd Km
T 1= ; T 2= t c ; K td =
K m K e + R B td K m K e + R B td K m K e + R Btd
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 3-24 Sơ đồ khối động cơ
Sau khi tìm được hàm truyền của động cơ, tiến hành chọn động cơ servo để đáp ứng
được các tham số trong hàm truyền của động cơ.
Bảng 3-4 Bảng thông số động cơ servo sử dụng trong Robot SCARA
Cường độ dòng điện 𝐴(𝐴𝑚𝑝𝑒) 4,4

Hằng số sức điện động K e (𝑉𝑠/𝑟𝑎𝑑) 56/60
Hệ số mô – men xoắn K m (𝑁𝑚/𝐴) 0,487
Mô – men quán tính của rotor J m (kg .m2) 0,769. 1 0− 4
Điện trở 𝑅(Ω) 1,83
Độ tự cảm 𝐿(𝐻) 4,72. 1 0−3
Tốc độ quay 𝑛(𝑟𝑝𝑚) 3000
Hệ số giảm chấn 𝐵𝑚(𝑁𝑚/𝑠) 2,58. 1 0− 4
3.3.1 Mô hình hoá và xác định hàm truyền các khâu

Mô hình hoá khâu 3
Trục vít me – đai ốc bi khâu 3( Khâu 3 là khâu tịnh tiến)
Hình 3-25 Sơ đồ trục vít me – đai ốc bi
Xét bộ truyền vít me – đai ốc bi như hình.
29
Tín hiệu vào: vận tốc góc ω (t) của vít me, [rad/s]
Tín hiệu ra: lượng dịch chuyển y (t ) 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ệ:
t
p (3.73)
y (t)= ∫ w (t ) dt
2π 0
Biến đổi Laplace 2 vế với điều kiện đầu bằng 0:
p w(s) (3.74)
Y ( s )=
2π s
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:
Y ( s) p K (3.75)
= =
w( s) 2 πs s
p
Trong đó : K= – hệ số tích phân.
2π
Sơ đồ khối tương đương:
Hình 3-26 Sơ đồ khối trục vít me – đai ốc bi
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 3-27 Sơ đồ khối khâu 3
Hàm truyền khâu 3:

n3 K td K (3.76)
Gkhâu 3= 3 2
T 1 s +T 2 s + s

Do khâu 2 sử dụng độ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.
θm (s) n 2 K td (3.77)
Gkhâu 2= =
U (s) T 1 s + T 2 s 2 +s
3
Trong đó:
τ t τ c R B td (τ t + τ c )R Btd Km
T 1= ; T 2= ; K td =
30
J td L
τ c= là hằng số thời gian cơ, τ t= là hằng số thời gian điện từ
Btd R
Do khâu 1 sử dụng độ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.
θm (s) n 1 K td (3.78)
Gkhâu 1= =
U (s) T 1 s + T 2 s 2+ s
3
Trong đó:
τ t τ c R B td (τ t + τ c )R Btd Km
T 1= ; T 2= ; K td =
J td L
τ c= là hằng số thời gian cơ, τ t= là hằng số thời gian điện từ
Btd R
Xác định hàm truyền các khâu
Sử dụng Matlab tính các hàm truyền:
Kết quả thu được:
0,01188
Gk h au 1= −5 3 2 (Rad/Volt)
5,259∗1 0 s +0,02039 s +0,4551 s
0,01188
9,123∗10 s + 0,003539 s + 0,4551 s
0,003875
7,904∗1 0 s + 0,03066 s + 0,4623 s
3.3.2 Đánh giá tính ổn định, xác định sai lệch tĩnh
Kiểm tra tính ổn định của Gkhau 1 (s )
- Kiểm tra tính ổn định của hệ hở:
+ Điều kiện cần:
Ta thấy các hệ số của đa thức mẫu số của Gk h au 1 (s ) đều lớn hơn và khác 0. Vậy thoả
mãn điều kiện cần của tiêu chuẩn Routh.
+ Điều kiện đủ:
Bảng 3-5 Bảng Routh hàm truyền 1
−5
5,259∗10 0,4551
0,02039 0
0,4551
Các số hạng thuộc cột đầu tiên của bảng Routh đều dương nên thoải mãn điều kiện
ổn định. Vậy hệ thống được mô tả bằng phương trình đã cho là ổn định.
- Kiểm tra tính ổn định của hệ kín
Theo tiêu chuẩn Nyquist
31
Hình 3-28 Đồ thị Nyquist của hàm truyền Gkhau 1 (s )
Theo tiêu chuẩn Nyquist[1]: Nếu hàm truyền Gh (s) của hệ hở có m điểm cực không
nằm bên trái trục ảo (nằm trên trục ảo hoặc nằm bên phải trục ảo), thì cần và đủ để hệ
kín ổn định là đường đồ thị Nyquist của hệ hở bao điểm -1+0j của mặt phẳng phức
đúng m lần theo chiều ngược kim đồng hồ. Ta thấy đồ thị Nyquist không bao điểm (-
1+0j) nên sẽ không có điểm cực nào trên trục ảo hoặc bên phải trục ảo, cho nên hệ kín
ổn định.
Kiểm tra tính ổn định của Gk h au 2 (s )
Ta thấy các hệ số của đa thức mẫu số của Gk h au 2 (s ) đều lớn hơn và khác 0. Vậy thoả
+ Điều kiện đủ:
−6
9,123∗1 0 0,4551
0,003539 0
0,4551
32
Hình 3-29 Đồ thị Nyquist của hàm truyền Gk h au 2 (s )
ổn định.
Kiểm tra tính ổn định của Gkhau 3 (s)

Ta thấy các hệ số của đa thức mẫu số của Gk h au 3 (s) đều lớn hơn và khác 0. Vậy thoả
+ Điều kiện đủ
−5
7,904∗1 0 0,4623
0,03066 0
0,4623
33
Hình 3-30 Đồ thị Nyquist của hàm truyền Gkhau 3 ( s)
ổn định
Xác định sai lệch tĩnh
Cả 3 khâu đều có hàm truyền dạng:
K (3.79)
G= 3 2
T 1 s +T 2 s + s
Xét tín hiệu đầu vào có dạng 𝑢(𝑡) = 1(𝑡)
Hình 3-31 Sơ đồ khối tính sai lệch tĩnh với đầu vào u(t)=1(t)
Sai lệch tĩnh được tính theo công thức:

1 1 (3.80)
e ∞=lim sE ( s )=lim = =0
s→ 0 s→0 1+G (s) 1+lim G (s)
s→0
Theo tiêu chuẩn đánh giá sai lệch tĩnh, hệ thống ổn định.
Đánh giá tính ổn định
 Khớp 1
34
Hình 3-32 Đồ thị đáp ứng của khớp 1
- Độ vọt lố: 0%
- Thời gian tăng: 80s (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: 210s
- 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
 Khớp 2
+ Độ vọt lố: 0%
+ Thời gian tăng: 80s (là khoảng thời gian kể từ khi hệ thống đạt 10% cho đến
+ Thời gian xác lập: 210s
+ 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
 Khớp 3
35
+ Độ vọt lố: 0%
+ Thời gian tăng: 250s (là khoảng thời gian kể từ khi hệ thống đạt 10% cho đến
+ Thời gian xác lập: 700s
+ 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.
3.4 Thiết kế bộ điều khiển PID
PID Controller (Propotional – Integral – Derivative Controller) là bộ điều khiển
vi tích phân tỷ lệ, là một cơ chế phản hồi vòng điều khiển được sử dụng rộng rãi trong
công nghiệp. Bộ điều khiển PID được sử dụng nhiều nhất trong các hệ thống điều
khiển vòng kín. 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.
36
Hình 3-35 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 q d 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.
t
d (3.81)
u(t )=K p e (t)+ K d e (t)+ K i∫ e(τ )dτ
dt
Với
e ( t )=qd ( t )−q(t) (3.82)
+ e (t) – sai lệch giữa giá trị đặt mong muốn (setpoint) và giá trị thực tế đo được
+ q d (t ) – giá trị đặt mong muốn (setpoint) tại thời điểm đo
+ q (t) – giá trị thực tế của biến khớp tại thời điểm đo
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. Việc lựa chọn các thông số cho PID Controller sử
dụng công cụ PID Tuner tích hợp trên Matlab.
Hình 3-36 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
37
Hình 3-37 Sơ đồ khối PID điều khiển động cơ
Xác định được các thông số của PID Controller:

Bảng 3-8 Thông số của PID Controller
Khâu Kp Ki Kd
1 3181,468 17229,578 117,609
2 5248,967 71094,881 37,143
3 13279,113 68743,44 594,64
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 u(t )=1(t)
 Khâu 1
38
Hình 3-38 Đồ thị đáp ứng khâu 1 với bộ điều khiển PID

- Thời gian tăng: 0,06s (là khoảng thời gian kể từ khi hệ thống đạt 10% cho đến
- Thời gian xác lập: 0,5s
- 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
 Khâu 2

 Khâu 3
39

Nhận xét: Sau khi thiết kế bộ điều khiển PID, 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.
3.5 Mô phỏng, phân tích và đánh giá các chỉ tiêu kỹ thuật của hệ thống
Sau khi thiết kế bộ điều khiển PID, ta đi mô phỏng đánh giá bộ điều khiển đã
thiết kế.
Mô hình CAD sau khi đưa vào MATLAB
Hình 3-41 Mô hình CAD của robot trong Matlab
Mô hình Simscape xuất từ SolidWorks
40
Hình 3-42 Khối 3D Scara robot
Mô hình điều khiển cho toàn bộ robot
Hình 3-43 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ác biến khớp q 1 , q 2 , q 3phụ thuộc vào vị trí
các điểm thao tác trong không gian, ở đây các biến khớp thoả mãn quỹ đạo
hình học cho trước và được giải bằng bài toán động học ngược.
 Tín hiệu sẽ đi qua 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ò đầu vào của bộ điều khiển PID
41
Hình 3-44 Khối Position Reference (Thiết kế quỹ đạo)
Hình 3-45 Khối động cơ DC
42
Ta mô phỏng và thu được đồ thị về quỹ đạo của từng biến khớp, sai số và vận tốc
Hình 3-46 Giá trị biến khớp khâu 1
Chuyển động của khâu 1 đảm bảo quỹ đạo hình học, diễn ra trong không gian theo
thời gian với quy luật vận tốc:
- Chuyển động đều, vận tốc không đổi.
- Quy luật vận tốc hình thang.
- Độ dịch chuyển là đa thức bậc 3 của thời gian.
Hình 3-47 Sai số biến khớp khâu 1
Giá trị biến khớp các khâu đặt vào và đầu ra là trùng khớp.
Sai số biến khớp khâu 1 là rất nhỏ : 3. 10−3 rad
43
Hình 3-48 Vận tốc khâu 1
44
- Giá trị biến khớp các khâu đặt vào và đầu ra là trùng khớp.
- Sai số biến khớp khâu 2 là rất nhỏ : 20. 10−3 rad
45
- Giá trị biến khớp các khâu đặt vào và đầu ra là trùng khớp.
46
- Sai số biến khớp khâu 3 là rất nhỏ: 1. 10−4 mm .
Như vậy bộ điều khiển ta đã thiết kế là phù hợp.

3.6 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ẽ 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 mô – men
xoắn lớn và điều khiển vị trí chính xác cao, ứng dụng trong Robot, máy CNC…
Bảng 3-9 So sánh biến tần và bộ điều khiển Servo
Tiêu chí Biến tần Bộ điều khiển Servo

Ứng dụng điều Sử dụng cho các ứng dụng Sử dụng cho các ứng dụng
47
điều khiển ở tốc độ và độ
không yêu cầu tốc độ và độ
khiển chính xác cao, thay đổi
chính xác cao, ổn định
trạng thái nhanh và liên tục
Cơ bản là điều khiển tốc độ Điều khiển vị trí, tốc độ và
Chế độ điều khiển
động cơ Mô – men
Một bộ điều khiển servo cơ
Khả năng điều khiển Một biến tần có thể điều
bản chỉ điều khiển một
nhiều động cơ khiển nhiều động cơ
động cơ servo duy nhất
Chậm Nhanh
Độ đáp ứng
100 rad/s hoặc thấp hơn Khoảng 200 - 15000 rad/s
Điều khiển vị trí và
Đến khoảng 100 µm Lên đến khoảng 1µm
dừng chính xác cao
Chế độ khóa Không Có
Tần số khởi
20 rpm hoặc thấp hơn 20 – 600 rpm
động/dừng
Mô – men xoắn cực
Khoảng 150% Khoảng 300%
đại
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 – 06A cho khâu 3 và SMGAV-08A cho khâu 1, 2
( hãng Yaskawa)
Hình 3-55 Động cơ và bộ điều khiển hãng Yaskawa
48
Hình 3-56 Thông số kỹ thuật của dòng động cơ SGMAV
 Rate Output: Đầu ra định mức ( công suất )

 Rate Torque: Mô – men xoắn định mức
 Instantaneous Peak Torque : Mô – men xoắn cực đại tức thời
 Rated Current : Dòng điện đánh giá
 Instantaneous Max Current : Dòng điện tối đa tức thời
 Rate Speed: Tốc độ định mức
 Max Speed: Tốc độ tối đa
 Torque Constant : Hằng số mô – men xoắn
 Rotor Moment of Inertia : Mô – men quán tính của Rotor
 Rated Power Rate : Công suất định mức
 Rated Angular Acceleration : Gia tốc góc định mức
 Applicable SERVOPACK: Mã bộ điều khiển tương ứng với mã động cơ
Hình 3-57 Giao diện của bộ điều khiển SGDV – 5R5A
49
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.
- 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 analog.

- 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.
50
Hình 3-58 Sơ đồ các chân I/O cổng CN1 của servo driver SGDV – 5R5A
 Cổng CN1: Cổng kết nối ở chế độ điều khiển vị trí

 Các chân đầu vào, đầu ra em sẽ giải thích ở phần 2.7 mạch ghép nối phần tử
điều khiển
 Position reference : Vị trí đặt
 Backup battery : Pin dự phòng
 Servo ON (Servo ON when ON) : Servo bật khi bật
 P Control (P Control when ON) : Điều khiển vị trí khi bật
 Forward run prohibited (Prohibited when OFF): Không chạy thuận khi tắt
 Reverse run prohibited (Prohibited when OFF): Không chạy nghịch khi tắt
 Alarm reset (Reset when ON) : Đặt lại báo thức khi bật
 Forward external torque limit (Limit when ON) : Chuyển tiếp giới hạn mô –
men xoắn bên ngoài (giới hạn khi bật)
 Reverse external torque limit (Limit when ON) : Đảo ngược giới hạn mô –
men xoắn bên ngoài (giới hạn khi bật)
51
 Safety function device : Thiết bị an toàn
 Alarm code output (OFF for alarm) : Đầu ra mã báo động
 Encoder output pulse : Xung đầu ra bộ mã hoá
 Power supply for open – collector output reference : Nguồn cung cấp để mở
- tham chiếu đầu ra bộ thu
 Positioning completed (ON when positioning completes ) : Hoàn thành định
vị (bật khi định vị hoàn tất)
 Rotation Detection output (ON when the motor speed exceeds the settings) :
Đầu ra phát hiện quay (bật khi tốc độ động cơ vượt quá cài đặt)
 Servo ready output (ON when ready) : Đầu ra servo sẵn sàng
 Servo alarm output : Đầu ra cảnh báo servo
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ơ,
SIGN chân cấp xung điều khiển chiều quay (H level tương ứng với chiều quay thuận,
L level tương ứng với chiều quay nghịch). Các chế độ cài đặt chế độ cho Driver, ở đây
chọn chế độ Factory Settings:
Hình 3-59 Phương pháp xuất xung điều khiển servo ở chế độ điều khiển vị trí
 Paramater : Tham số
 Reference Pulse : Xung tham chiếu
 Input Pulse Multi – Plier : Đầu vào xung
 Forward Run Reference : Tham số chạy chuyển tiếp
 Reverse Run Reference : Tham số chạy ngược
 Sign + pulse train (Positive logic) : Tín hiệu và mạch xung (Logic dương)
52
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à:
n=1048576. k (3.83)
Với k là hệ số giảm tốc.

Khi đó muốn điều khiển động cơ quay được 𝑎 vòng ta cần cấp 𝑛 × 𝑎 xung vào chân
PULS. Để đảo chiều thì thay đổi mức logic ở chân SIGN.
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...
Mạch điều khiển Robot
Trong công nghiệp, vi điều khiển được sử dụng phổ biến là STM32 và Arduino
Mega 2560. Sau khi phân tích thông số kỹ thuật của chúng em nhận thấy STM32 vượt
trội hơn Arduino Mega 2560 về tốc độ, bộ nhớ,… nên sẽ phù hợp với Robot cần độ
chính xác cao như SCARA. 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 3-60 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
- 192KB of SRAM
- 512 bytes of OTP memory
 Điện áp hoạt động: 1.8 – 3.6V
53
 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
3.7 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 200V. Nguồn điện cho driver là nguồn điện 1
pha AC 200V. 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 5VDC.
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
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
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ừ 2 dây pha của nguồn điện 3 pha để cấp nguồn cho driver hoạt động.
Mạch ghép nối phần tử điều khiển
Để đ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.
Các chân OUTPUT:
 ALO1, ALO2, ALO3: Các bit báo lỗi của driver
54
 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à 16 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 48 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.
3.8 Kết luận
Sau chương 3, em đã hiểu hơn về cách tính động học và động lực học của Robot
SCARA nói riêng cũng như Robot công nghiệp nói chung. Từ đó thiết kế được quỹ
đạo chuyển động, thiết kế bộ điều khiển PID, và đưa ra được các mô phỏng, phân tích
và đánh giá về robot SCARA 3 bậc tự do.
55
CHƯƠNG 4. LẬP TRÌNH ĐIỀU KHIỂN
Chương 3 đã tập trung vào thiết kế hệ thống điều khiển cho Robot SCARA, giúp
chúng ta hiểu về kiến trúc, phương pháp và quá trình thiết kế hệ thống. Trên cơ sở này,
chương 4 sẽ tiếp tục lập trình điều khiển cho Robot SCARA 3 DOF.
Trong chương này, em sẽ đi vào chi tiết về việc lập trình điều khiển cho Robot
SCARA. Điều khiển robot yêu cầu việc lập trình các thuật toán và mã điều khiển để
điều phối các chuyển động của các cần cơ khí và đáp ứng các tác vụ cụ thể.
Đầu tiên, em sẽ tìm hiểu về ngôn ngữ lập trình phổ biến được sử dụng trong điều
khiển robot SCARA,G-code hoặc M-code. Tiếp theo, em sẽ nghiên cứu về các thuật
toán và phương pháp lập trình điều khiển cho Robot SCARA 3 DOF. Các thuật toán
này có thể bao gồm điều khiển tới điểm, điều khiển theo quỹ đạo liên tục, … và cuối
cùng em sẽ thiết kế một giao diện điều khiển để người dùng dễ dàng hơn trong việc
nhập dữ liệu.
4.1 Thiết kế giao diện điều khiển
Hình 4-61 Giao diện điều khiển Robot Scara
Giao diện điều khiển bao gồm:
56
- Nút nhấn Open, Close, Exit: Bật, tắt mô hình Simcape Robot Scara và thoát
khỏi giao diện điều khiển.
- Forward Kinematics: Cho phép người dùng nhập vào các biến khớp θ1 , θ2 ,d 3
vào phần set. Tọa độ điểm thao tác cuối E sẽ hiển thị trên phần Inverse
Kinematics. Tọa độ các biến khớp theo thời gian khi Robot di chuyển sẽ được
hiển thị ở phần feedback.
- Inverse Kinematics : Cho phép người dùng nhập vào tọa độ x, y, z của điểm
thao tác cuối E. Tọa độ các biến khớp sẽ hiển thị ở phần set trong Forward
Kinematics. Đồng thời tọa độ các biến khớp theo thời gian khi Robot di chuyển
sẽ hiển thị ở phần feed back.
- Trajectory: Cho phép người dùng chọn quỹ đạo mong muốn như ( về gốc,
đường tròn, xoắn ốc) sau đó bấm run để chạy, bấm stop khi muốn dừng. Quỹ
đạo sẽ được hiển thị trên đồ thị phía dưới.
4.2 Mô hình Simulink điều khiển Robot
Để thực hiện việc mô phỏng điều khiển Robot trên giao diện vừa thiết kế, ta cần xây
dựng mô hình Simulink. Trong sơ đồ, em có sử dụng khối sensor để nhận vào tín hiệu
feedback tương ứng với tín hiệu của encoder trong thực tế.
Hình 4-62 Sơ đồ Simulink điều khiển Robot
4.3 Kết quả mô phỏng

- Động học thuận:
57
Hình 4-63 Kết quả mô phỏng động học thuận
- Động học nghịch: x = 500(mm), y = 800(mm), z = 200(mm)
Hình 4-64 Kết quả mô phỏng động học nghịch

- Quỹ đạo
 Đường tròn tâm O(0;0;0.2) (m) bán kính 800(mm)
58
Hình 4-65 Điều khiển quỹ đạo tròn
 Đường xoắn ốc bán kính 800(mm), bước xoắn 150(mm)
Hình 4-66 Điều khiển quỹ đạo xoắn ốc

 Gốc: x = 1090(mm), y = 0, z = 450(mm)
Hình 4-67 Robot về vị trí gốc

59
4.4 Kết luận
Kết thúc chương 4, em đã xây dựng được giao diện điều khiển Robot với các tính
năng tính toán và điều khiển động học thuận, động học nghịch và theo quỹ đạo cố
định. Thêm vào đó em cũng đã xây dựng thêm phần feedback để có thể giao tiếp hai
chiều với Robot (gửi tín hiệu điều khiển Robot cũng như nhận tín hiệu phản hồi xem
Robot đã đến vị trí hay chưa). Từ đó giúp dễ dàng điều khiển và vận hành Robot hơn
trong công nghiệp.
60
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
- Chọn phần tử cho hệ thống điều khiển
- Thiết kế sơ đồ mạch cấp nguồn, mạch điều khiển
- Xây dựng giao diện mô phỏng điều khiển Robot
Trải qua quá trình tìm hiểu, 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 Du, 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 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 (Cô) để báo cáo của em được hoàn
thiện hơn nữa. Qua đây, em đã rút ra được những kiến thức, kinh nghiệm 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.
61
TÀI LIỆU THAM KHẢO
[1] Lý thuyết điều khiển tuyến tính - Nguyễn Doãn Phước

[2] Bài giảng điện tử Robotics – PGS. TS. Nguyễn Quang Hoàng, nhóm chuyên môn
Cơ học ứng dụng, Khoa Cơ điện tử, Trường Cơ khí, Đại học Bách Khoa Hà Nội
[3] Bài giảng điện tử Thiết kế Robot – PGS. TS. Phan Bùi Khôi, nhóm chuyên môn
Cơ học ứng dụng, Khoa Cơ điện tử, Trường Cơ khí, Đại học Bách Khoa Hà Nội
[4] 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
[5] YASKAWA - AC Servo Drives ∑ -𝑉 𝑆𝑒𝑟𝑖𝑒𝑠 USER’S MANUAL Design and
Maintenance Rotational Motor Analog Voltage AC Servo Drives ∑ -𝑉 𝑆𝑒𝑟𝑖𝑒𝑠and
Pulse Train Reference
[6] YASKAWA - AC Servo Drives ∑ -𝑉 𝑆𝑒𝑟𝑖𝑒𝑠 Product Catalog
62
PHỤ LỤC
Các chương trình code trên Matlabfunction [q1,q2,q3] = Dong_hoc_nguoc(p)
- Giao diện điều khiển Robot

function varargout = untitled(varargin)
% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name', mfilename, ...
'gui_Singleton', gui_Singleton, ...
'gui_OpeningFcn', @untitled_OpeningFcn, ...
'gui_OutputFcn', @untitled_OutputFcn, ...
'gui_LayoutFcn', [] , ...
'gui_Callback', []);
if nargin && ischar(varargin{1})
gui_State.gui_Callback = str2func(varargin{1});
end
if nargout
[varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT
% --- Executes just before untitled is made visible.

function untitled_OpeningFcn(hObject, eventdata, handles, varargin)
handles.output = hObject;
% Update handles structure
guidata(hObject, handles);
ModelName='Myrobot';
global var;
%KHOI TAO TEN DO THI
title(handles.axes1,'TRAJECTORY','Color',[0 0 0],'FontSize',10);
xlabel(handles.axes1,'xE(mm)','Color',[1 0 0]);
ylabel(handles.axes1,'yE(mm)','Color',[0 1 0]);
zlabel(handles.axes1,'zE(m)','Color',[0 0 1]);
% --- Outputs from this function are returned to the command line.
function varargout = untitled_OutputFcn(hObject, eventdata, handles)
varargout{1} = handles.output;
%_________________________________________________________________________
%HIEN THI THETA1
function ed_theta1_Callback(hObject, eventdata, handles)
theta1=get(handles.ed_theta1,'string')
function ed_theta1_CreateFcn(hObject, eventdata, handles)
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
%HIEN THI THETA2

function ed_theta2_Callback(hObject, eventdata, handles)
theta2=get(handles.ed_theta2,'string')
function ed_theta2_CreateFcn(hObject, eventdata, handles)
end
63
%HIEN THI D3
function ed_d3_Callback(hObject, eventdata, handles)

d3=get(handles.ed_d3,'string')
function ed_d3_CreateFcn(hObject, eventdata, handles)
end
%HIEN THI TOA DO X

function ed_x_Callback(hObject, eventdata, handles)
x=get(handles.ed_x,'string')
function ed_x_CreateFcn(hObject, eventdata, handles)

end
%HIEN THI TOA DO Y

function ed_y_Callback(hObject, eventdata, handles)
y=get(handles.ed_y,'string')
function ed_y_CreateFcn(hObject, eventdata, handles)
end
%HIEN THI TOA DO Z

function ed_z_Callback(hObject, eventdata, handles)
z=get(handles.ed_z,'string')
function ed_z_CreateFcn(hObject, eventdata, handles)
end
%POPUPMENU
function pm_Program_Callback(hObject, eventdata, handles)
function pm_Program_CreateFcn(hObject, eventdata, handles)
end
%BAI TOAN NGHICH

function btt_Go_Callback(hObject, eventdata, handles)
%khai bao tham so
a1=500;a2=450;a3=0;
d1=0.500;d2=0.100;
%tu x,y,z tinh ra cac bien goc va khop
x = str2num(get(handles.ed_x,'string'));
y = str2num(get(handles.ed_y,'string'));
z = ((str2num(get(handles.ed_z,'string'))));
if((z<0.200) || (z>0.600))
msgbox('Gia tri z [0.200 0.600] (m)');
else
d3=(d1+d2-z)
theta2=acosd(((x^2+y^2)-(a1^2+a2^2))/(2*a1*a2))
c1=(a1*x+a2*(x*cosd(theta2)+y*sind(theta2)))/(x^2+y^2);
s1=(a1*y+a2*(y*cosd(theta2)-x*sind(theta2)))/(x^2+y^2);
64
theta1=(-1)*(atan2d(s1,c1))
t1=int16(theta1);
t2=int16(theta2);
set(handles.ed_theta1,'string',num2str(-t1));
set(handles.ed_theta2,'string',num2str(t2));
set(handles.ed_d3,'string',num2str((d3)));
set_param([handles.modelname '/Actuator1'],'after',num2str(theta1));
set_param([handles.modelname '/Actuator3'],'after',num2str(d3));
guidata(hObject,handles);
end
%BAI TOAN THUAN

function btt_Start_Callback(hObject, eventdata, handles)
a1=500;a2=450;a3=0;
d1=0.500;d2=0.100;
theta1=(-1)*(str2num(get(handles.ed_theta1,'string')));
theta2=str2num(get(handles.ed_theta2,'string'));
d3=(str2num(get(handles.ed_d3,'string')));
%kiem tra gia tri nhap
if((d3<0) || (d3>0.400))
msgbox('Gia tri d3 [0 0.400] (m)');
else
x=a1*cosd(theta1)+a2*cosd(theta1+theta2);
y=(a1*sind((-1)*theta1)+a2*sind((-1)*theta1+theta2));
z=(d1+d2-d3);
set(handles.ed_x,'string',num2str(x));
set(handles.ed_y,'string',num2str(y));
set(handles.ed_z,'string',num2str(z));
end
%START TIMER
function btt_Run_Callback(hObject, eventdata, handles)
global t tmr theta3;
theta3=0;
t=0;
tmr=timer('TimerFcn',
{@TimerCallback,hObject,handles},'Period',1,'ExecutionMode','fixedRate');
start(tmr);
%CHUONG TRINH QUY DAO

function TimerCallback(obj, event,hObject, handles)
modelName = 'Myrobot';
global t tmr tmr0 t0;
luachon=get(handles.pm_Program,'value');
switch luachon
case 1 %goc
if t <=1
t=t+1;
x=950*t;
65
y=0;
z=0.600;%(m)
set(handles.ed_t,'string',num2str(t));
plot3(handles.axes1,x,y,z,'red.','LineWidth',0.5);
%comet3(x,y,z);
a1=500;a2=450;a3=0;
d1=0.500;d2=0.100;
d3=d1+d2-z;
c2=(((x^2+y^2)-(a1^2+a2^2))/(2*a1*a2));
theta2=(acosd(((x^2+y^2)-(a1^2+a2^2))/(2*a1*a2)));
theta1=(-1)*(atan2d(s1,c1));
t1=int16(theta1);
t2=int16(theta2);
set(handles.ed_d3,'string',num2str(d3));
end
if(t==1)
stop(tmr)
tmr.Running
delete(tmr)
tmr0=timer('TimerFcn',
{@Kiemtra,hObject,handles},'Period',1,'ExecutionMode','fixedRate');
start(tmr0);
end
case 2 %duong tron

if t<360
t=t+1;
x = 0+800*cosd(t);
y = 0+800*sind(t);
z = 0.300;
plot3(handles.axes1,x,y,z,'green.','LineWidth',1);
a1=500;a2=450;a3=0;
d1=0.500;d2=0.100;
d3=d1+d2-z;
c2=(((x^2+y^2)-(a1^2+a2^2))/(2*a1*a2));
66
s2=sqrt(1-(c2^2));
t1=int16(theta1);
t2=int16(theta2);
end
if(t==360)
stop(tmr)
tmr.Running
delete(tmr)
start(tmr0);
end
case 3
if t<360
t=t+1;
x = 0+800*cosd(t);
y = 0+800*sind(t);
z = 0.3+(t/2400);
plot3(handles.axes1,x,y,z,'blue.','LineWidth',1);
a1=500;a2=450;a3=0;
d1=0.500;d2=0.100;
d3=d1+d2-z;
c2=(((x^2+y^2)-(a1^2+a2^2))/(2*a1*a2));
s2=sqrt(1-(c2^2));
t1=int16(theta1);
t2=int16(theta2);
67
end
if(t==360)
stop(tmr)
tmr.Running
delete(tmr)
start(tmr0);
end
end
%STOP TIMER KHI DANG RUN

function btt_Stop_Callback(hObject, eventdata, handles)
global tmr
stop (tmr);
tmr.Running
delete (tmr);
%OPEN MODEL
function btt_Open_Callback(hObject, eventdata, handles)
handles = guidata(hObject);
modelName = 'Myrobot';
handles.modelname = modelName;
handles.Goc1 = 0; % Bien goc khau 1
handles.Fbgoc1 = 0; % Feedback goc khau 1
handles.Goc2 = 0; % Bien goc khau 2
handles.Fbgoc2 = 0; % Feedback goc khau 2
handles.d3 = 0;%bien khau3
handles.Fbd3 = 0;%feedback khau3
% Load the simulink model
if ~modelIsLoaded(modelName)
load_system(modelName);
end
set_param(handles.modelname,'BlockReduction','off');
set_param(handles.modelname,'StopTime','inf');
set_param(handles.modelname,'SimulationMode','normal');
%khi model start thi goi ham localAddEventListener
set_param(handles.modelname,'StartFcn','localAddEventListener');
% Start the model
set_param(handles.modelname, 'SimulationCommand', 'start');
set(hObject,'Enable','off');
set(handles.btt_Close, 'Enable','on');
%CLOSE MODEL
function btt_Close_Callback(hObject, eventdata, handles)
set_param(handles.modelname,'SimulationCommand','stop');
set_param(handles.modelname,'StopTime','10');
set_param(handles.modelname,'StartFcn','');
set(hObject,'Enable','off');
set(handles.btt_Open,'Enable','on');
set_param([handles.modelname '/Actuator1'],'after','0');
68
try
catch ME
% Get rid of the figure if it was created
if exist('hf','var') && ~isempty(hf) && ishandle(hf)
delete(hf);
end
% Get rid of the model if it was loaded
bdclose(all);
end
%................TAO CAC SU KIEN...........................................

function modelLoaded=modelIsLoaded(modelName)
try
modelLoaded=~isempty(find_system('Type','block_diagram','Name',modelName));
catch
modelLoaded=False;
end
%SU KIEN :localEventListener tach do data

% Callback Function for executing the event listener on the gain block
function localEventListener(block, eventdata)
simData = block.OutputPort(1).Data;
apphandles = guidata(untitled) ;
apphandles.fbGoc1 = (-1)*(int16(simData(3)));
apphandles.fbGoc2 = int16(simData(2));
apphandles.fbd3 = double(simData(1));
format bank
set(apphandles.ed_fbt1, 'String',num2str(apphandles.fbGoc1));
set(apphandles.ed_fbt2, 'String',num2str(apphandles.fbGoc2));
set(apphandles.ed_fbd3, 'String',num2str(apphandles.fbd3));
drawnow;
%KIEM TRA
function Kiemtra(obj, event,hObject, handles)
global tmr0 t0
thet1=get(handles.ed_theta1,'string');
thet2=get(handles.ed_theta2,'string');
t1=get(handles.ed_fbt1,'string');
t2=get(handles.ed_fbt2,'string');
if((thet2==t2) & (thet1==t1))
msgbox('$--DONE--$');
stop(tmr0)
tmr0.Running
delete(tmr0)
end
%SU KIEN
% Callback Function for adding an event listener to the gain block
%h = add_exec_event_listener(blk,event,listener);
function eventhandle = localAddEventListener
eventhandle = add_exec_event_listener('Myrobot/Sensor', ...

'PostOutputs', @localEventListener);
%HIEN THI PHAN HOI THETA1

function ed_fbt1_Callback(hObject, eventdata, handles)
function ed_fbt1_CreateFcn(hObject, eventdata, handles)
69
end
%HIEN THI PHAN HOI THETA2

function ed_fbt2_Callback(hObject, eventdata, handles)
function ed_fbt2_CreateFcn(hObject, eventdata, handles)
end
%HIEN THI PHAN HOI D3

function ed_fbd3_Callback(hObject, eventdata, handles)
function ed_fbd3_CreateFcn(hObject, eventdata, handles)
end
%HIEN THI BO DEM

function ed_t_Callback(hObject, eventdata, handles)
function ed_t_CreateFcn(hObject, eventdata, handles)
end
%CREATE AND DELETE FCN

function figure1_DeleteFcn(hObject, eventdata, handles)
function figure1_CreateFcn(hObject, eventdata, handles)
% --- Executes on button press in btt_Exit.

function btt_Exit_Callback(hObject, eventdata, handles)
close
% hObject handle to btt_Exit (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
2. Quỹ đạo đặt

function E = Quy_dao_dat(t)
a1 = 0.5;
a2 = 0.45;
d1 = 0.5;
d2 = 0.1;
StartPoint=[a1+a2 0 d1+d2];
% 4 Diem lap rap
A1=[0 0.4 0.3]; % Diem A
A2=[0 0.4 0.5]; % Diem A'
B1=[0.51 0 0.3]; % Diem B
B2=[0.51 0 0.5]; % Diem B'
% Huong quy dao khau thao tac-----------------------------
eta0=0;
eta1=pi/2;
% Thoi gian cac doan quy dao SA'-A'A-AA'-A'B'-B'B---------
T=[2 2 2 3 2 2]; % Don vi:s
% Thoi gian dung de gap vat va thả vat -------------------
Td =1; % Don vi:s
%================Nhap cac diem quy dao====================
70
if (t<=T(1))&&(t>=0)
A=StartPoint; % Quy dao S-A'
B=A2;
Te=T(1);t_in=0;
xA=A(1);yA=A(2);zA=A(3);
xB=B(1);yB=B(2);zB=B(3);
xE=xA+3*(xB-xA)/(Te^2)*(t-t_in)^2-2*(xB-xA)/(Te^3)*(t-t_in)^3;
yE=(yB-yA)/(xB-xA)*xE+(yA*xB-yB*xA)/(xB-xA);
zE=(zB-zA)/(xB-xA)*xE+(zA*xB-zB*xA)/(xB-xA);
E=[xE;yE;zE];
elseif (t<=T(1)+T(2))&&(t>=T(1))
A=A2; % Quy dao A'-A
B=A1;
Te=T(2);t_in=T(1);
xA=A(1);yA=A(2);zA=A(3);
xB=B(1);yB=B(2);zB=B(3);
zE=zA+3*(zB-zA)/(Te^2)*(t-t_in)^2-2*(zB-zA)/(Te^3)*(t-t_in)^3;
xE=(xB-xA)/(zB-zA)*zE+(xA*zB-xB*zA)/(zB-zA);
yE=(yB-yA)/(zB-zA)*zE+(yA*zB-yB*zA)/(zB-zA);
E=[xE;yE;zE];
% Dung de gap vat

elseif (t<=T(1)+T(2)+Td)&&(t>=T(1)+T(2))
A=A1; % Quy dao A-A: Dung tai diem A
xA=A(1);yA=A(2);zA=A(3);
zE=zA;
xE=xA;
yE=yA;
E=[xE;yE;zE];
%
elseif (t<=T(1)+T(2)+T(3)+Td)&&(t>=T(1)+T(2)+Td)
A=A1; %Quy dao A-A'
B=A2;
Te=T(3);t_in=T(1)+T(2)+Td;
xA=A(1);yA=A(2);zA=A(3);
xB=B(1);yB=B(2);zB=B(3);
E=[xE;yE;zE];
elseif (t<=T(1)+T(2)+T(3)+T(4)+Td)&&(t>=T(1)+T(2)+T(3)+Td)
A=A2; % Quy dao A'-B'
B=B2;
Te=T(4);t_in=T(1)+T(2)+T(3)+Td;
xA=A(1);yA=A(2);zA=A(3);
xB=B(1);yB=B(2);zB=B(3);
xE=xA+3*(xB-xA)/(Te^2)*(t-t_in)^2-2*(xB-xA)/(Te^3)*(t-t_in)^3;
yE=(yB-yA)/(xB-xA)*xE+(yA*xB-yB*xA)/(xB-xA);
zE=(zB-zA)/(xB-xA)*xE+(zA*xB-zB*xA)/(xB-xA);
E=[xE;yE;zE];
elseif (t<=T(1)+T(2)+T(3)+T(4)+T(5)+Td) && (t>=T(1)+T(2)+T(3)+T(4)+Td)

A=B2; % Quy dao B'-B
B=B1;
Te=T(5);t_in=T(1)+T(2)+T(3)+T(4)+Td;
xA=A(1);yA=A(2);zA=A(3);
xB=B(1);yB=B(2);zB=B(3);
71
E=[xE;yE;zE];
% Dung de tha vat

elseif (t<=T(1)+T(2)+T(3)+T(4)+T(5)+2*Td) && (t>=T(1)+T(2)+T(3)+T(4)+T(5)+Td)
A=B1; % Quy dao B-B: Dung tai diem B
xA=A(1);yA=A(2);zA=A(3);
zE=zA;
xE=xA;
yE=yA;
E=[xE;yE;zE];
elseif (t<=T(1)+T(2)+T(3)+T(4)+T(5)+T(6)+2*Td) &&

(t>=T(1)+T(2)+T(3)+T(4)+T(5)+2*Td)
A=B1; % Quy dao B-B'
B=B2;
Te=T(6);t_in=T(1)+T(2)+T(3)+T(4)+T(5)+2*Td;
xA=A(1);yA=A(2);zA=A(3);
xB=B(1);yB=B(2);zB=B(3);
E=[xE;yE;zE];
else
E=[0;0;0];
vE=[0;0;0];
aE=[0;0;0];
Eta_angle=[0;0;0];
end
end
3. Động học ngược

function [q1,q2,q3] = Dong_hoc_nguoc(p)
a1=0.5; a2=0.45;
d1=0.5;d2=0.1;
g=9.81; %m/s^2
% Dau vao vi tri
xk=p(1);yk=p(2);zk=p(3);
%Tinh dong hoc nguoc vi tri
c2=(xk^2+yk^2-a1^2-a2^2)/(2*a1*a2);
s2=sqrt(1-c2^2);
q2=atan2(s2,c2);
c1=((a2*c2+a1)*xk+a2*s2*yk)/(xk^2+yk^2);
s1=((a2*c2+a1)*yk-a2*s2*xk)/(xk^2+yk^2);
q1=atan2(s1,c1);
q3=zk-d1-d2;
end
4. Không gian làm việc của Robot

%clear all
clc
%variable
syms t1 t2 d3
d1 = 500;
d2 = 100;
a1 = 500;
a2 = 450;
T01 = [cos(t1) -sin(t1) 0 a1*cos(t1); sin(t1) cos(t1) 0 a1*sin(t1); 0 0 1 d1; 0 0
0 1];
T12 = [cos(t2) -sin(t2) 0 a2*cos(t2); sin(t2) cos(t2) 0 a2*sin(t2); 0 0 1 0; 0 0 0
1];
72
T23 = [1 0 0 0; 0 -1 0 0; 0 0 -1 -d3; 0 0 0 1];
T03 = T01 * T12 * T23;
x = T03(1,4);
y = T03(2,4);
z = T03(3,4);
t1 = linspace(-167*pi/180, 167*pi/180,50);
t2 = linspace(-145*pi/180, 145*pi/180,50);
d3 = linspace(0, 400,50);
[q1, q2, q3] = ndgrid(t1, t2, d3);
xP = a2*cos(q1 + q2) + a1*cos(q1);

yP = a2*sin(q1 + q2) + a1*sin(q1);
zP = d1 +d2 - q3;
plot3(xP(:),yP(:),zP(:),'.');
hold on
xlabel('x(mm)');
ylabel('y(mm)');
zlabel('z(mm)');
grid on
Mô – men quán tính của khâu gắn trên khớp
 Khớp 1:
Mô – men quán tính J l 1 của khớp 1 được xác định là mô – men quán tính theo trục z và
do khớp 1 phải chịu tác động bao gồm mô – men quán tính của các khâu phía sau tác
dụng lên.
73
Từ bảng trên ta có J l 1=18 , 6( kg . m2)
 Khớp 2
Từ bảng trên ta có J l 2=3 , 12(kg .m2 )

 Khớp 3
74
Từ bảng trên ta có J l 3=4,167.1 0−3 (kg .m2 )
75

Doan2 Huy

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Doan2 Huy

Uploaded by

Copyright:

Available Formats

ĐẠI HỌC BÁCH KHOA HÀ NỘI

ĐỒ ÁN THIẾT KẾ HỆ THỐNG ĐIỀU

Thiết kế hệ thống điều khiển robot SCARA

Ngành Kỹ thuật Cơ điện tử

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

CHƯƠNG 1. TỔNG QUAN VỀ ROBOT SCARA....................................................1

Hình 1-1 Robot SCARA

1.1.2 Ứng dụng của Robot SCARA

Hình 1-2 Ứng dụng của Robot SCARA

1.1.3 Ưu điểm của Robot SCARA

Hình 1-3 Robot SCARA Denso HM-G

Hình 1-4 Robot SCARA Fanuc SR 6iA và Robot SCARA Epson

1.3 Yêu cầu thiết kế

Hình 2-5 Động cơ bước

Hình 2-6 Nguồn Adaptor tổ ong

Hình 2-7 Driver điều khiển động cơ

Giao tiếp với bộ điều khiển:

Hình 2-8 Dây cáp RS 232, Arduino Mega

2.3 Kết luận

Hình 3-9 Sơ đồ động học Robot SCARA 3 bậc tự do

Ta có bảng D-H như sau:

Với biến tại các khớp (Tọa độ suy rộng): q=[q1 q2 q3 ]T

Hình 3-10 Không gian làm việc của Robot

Hình 3-11 Không gian làm việc của Robot 2D

a. Xác định hướng của khâu thao tác

- Đồng nhất hệ số hai phương trình và giải ta được:

Cộng hai vế của phương trình trên ta được:

{ x E =( a2 cosq 2+ a1 ) cosq 1+(−a2 sinq2 )sinq1

3.2 Động lực học Robot SCARA

Hình 3-12 Hệ tọa độ khối tâm của Robot

Khâ Vị trí trọng tâm Khối Ma trận quán tính Θ

Với: l 1 là khoảng cách từ tâm C1 đến gốc tọa độ O1

- Ma trận Jacobi tịnh tiến:

- Ma trận Jacobi quay:

Với: l 2 là khoảng cách từ tâm C2 đến gốc tọa độ O2

- Ma trận Jacobi tịnh tiến:

JT = = (a ¿ ¿ 2−l 2 )cos q12 +a1 cos q 1 ¿ (a ¿ ¿ 2−l 2)cos q 12 ¿ 0 0¿0¿

- Ma trận Jacobi quay:

Với: l 3 là khoảng cách từ tâm C3 đến gốc tọa độ O3

- Ma trận Jacobi tịnh tiến:

- Ma trận Jacobi quay:

+ Ma trận Momen quán tính khối:

 Tính toán thế năng, lực thế:

Thế năng khâu 1:

Thế năng khâu 2:

Thế năng khâu 3:

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

Lực suy rộng được xác định theo công thức

Hay lực điều khiển tại các khớp:

Hình 3-13 Quỹ đạo chuyển động của Robot

Bảng 3-3 Thông số quỹ đạo chuyển động

x−x 0 y− y 0 z−z 0 (3.50)

ye− y0 y0 xe− y e x0 (3.52)

Ta xác định được các hệ số:

Sơ đồ Simulink thiết kế quỹ đạo

Hình 3-15 Quỹ đạo các biến khớp khâu 1

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

Hình 3-18 Sơ đồ khối Robot bao gồm hộp số

Sử dụng nguyên lý trên cho trục động cơ chính ta được:

Kết hợp các phương trình trên ta thu được:

τ m=J td Ẇ m + Btd W m (3.61)

Hình 3-19 Sơ đồ nguyên lý động cơ

Sơ đồ khối tương ứng: