Đồ á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

ĐẠ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


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:
Trường:

Cơ điện tử
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.4

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

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

2.2

2.3

Ngun lý hoạt động..........................................................................................5
2.1.1

Phân tích ngun lý hoạt động.............................................................5

2.1.2

Các thông số kỹ thuật quan trọng.........................................................5

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

Kết luận..............................................................................................................7

CHƯƠNG 3. THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN..............................................8
3.1

3.2

3.3

Độ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

Độ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

Thiết kế quỹ đạo chuyển động.......................................................................20
3.3.1

Mơ hình hố động cơ điện một chiều.................................................24

3.3.1

Mơ hình hố 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-25 Đồ thị đáp ứng của khớp 2...........................................................................35
Hình 3-26 Đồ thị đáp ứng của khớp 3...........................................................................35
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-31 Đồ thị đáp ứng khâu 2 với bộ điều khiển PID.............................................38
Hình 3-32 Đồ thị đáp ứng khâu 3 với bộ điều khiển PID.............................................39
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 tồ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-41 Giá trị biến khớp khâu 2..............................................................................43
Hình 3-42 Sai số biến khớp khâu 2...............................................................................44
Hình 3-43 Vận tốc khâu 2.............................................................................................44
Hình 3-44 Giá trị biến khớp khâu 3..............................................................................45
Hình 3-45 Sai số biến khớp khâu 3...............................................................................45
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-6 Bảng Routh hàm truyền 2..............................................................................32
Bảng 3-7 Bảng Routh hàm truyền 3..............................................................................33
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
hồ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 tồ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 độ hồ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 hồn hảo để thực hiện
là một vấn đề. Với Scara, nó có thể kiểm số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 hồ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 hồn tồ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 hồ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. Ngồ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 tồ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 ngun 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 NGUN 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 Ngun lý hoạt động
2.1.1 Phân tích ngun 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

q3

0

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
sin q 1 cos q 1
T 01= A1=
0
0
0
0

0 a1 cos q 1
0 a1 sin q 1
1
d1

0
1

cos q 2 −sin q 2
sin q 2 cos q 2
T 12= A2=
0
0
0
0

0 a2 cos q 2
0 a2 sin q 2
1
d2
0
1

]
]

(3-1)

(3-2)

0

T 2= A 1 A 2

[

cos q12 −sin q12
sin q12 cos q12
¿
0
0
0
0

[ ]

1
0
2
T 3= A 3=
0
0
0

0 a2 cos q 12+ a1 cos q1
0 a 2 sin q 12+ a1 sin q 1
1
d1
0
1

0

1

0
1
0
0

0 0
0 0
1 q3
0 1

]

(3-4)

2

T 3=T 1 T 2 T 3

[

(3-3)

cos q12 −sin q12
sin q12 cos q12
¿
0
0
0
0

0 a2 cos q 12+ a1 cos q1
0 a 2 sin q 12+ a1 sin q 1
1
d 1+ d 2+ q3
0
1

Với: q 12=q1 +q 2
 Vị trí và hướng điểm thao tác:
10

]

(3-5)


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

{

x=a2 cos q 12+ a1 cos q1
y=a2 sin q 12+a 1 sin q 1
z=d1 + d2 +q 3

(3-6)

{

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
R = sin q 12 cos q 12 0
0
0
1
0
3

]

]

(3-7)

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

{

(3-8)

α =0 °
β=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
δ r 0E (q)
v=
= 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
0
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
q˙ 1

 Vận tốc góc khâu 2:
w 2= R˙ 2 R2 =
0

0

0T

 Vận tốc góc khâu 3:
w 3= R˙ 3 R3 =
0

0

0T

(3-10)

[ ]

(3-11)

[ ]

(3-12)

0
0
q˙ 1 + q˙ 2

0
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
đó.
u cầu của bài tốn động học ngược là từ tọa độ, góc quay trong khơng gian thao
tác tính tốn được tọa độ khớp trong khơng gian khớp. Có nhiều cách để giải bài tố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 tốn động học ngược.
Với robot Scara 3 bậc tự do thì bài tố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)
−a2 sinq2 . x E + ( a 2 cosq 2 +a 1(3-15)
). yE

sinq1
=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
→ q1=arctan

Kết luận: Kết quả của bài toán động học ngược:

13