ĐẠI HỌC ĐÀ NẴNG

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

KHOA CƠ KHÍ

ĐỒ ÁN TỐT NGHIỆP

ĐỀ TÀI:
THIẾT KẾ VÀ CHẾ TẠO CÁNH TAY ROBOT
6 BẬC TỰ DO

Người hướng dẫn: TS. LÊ HOÀI NAM

Sinh viên thực hiện: TRẦN VĂN ĐỔ
NGUYỄN ĐÌNH DANH

Đà Nẵng, 12/2019
 TÓM TẮT ĐỒ ÁN TỐT NGHIỆP

1. Tên đề tài: Thiết kế chế tạo robot 6 bậc tự do

Sinh viên thực hiện: Nguyễn Đình Danh Lớp: 15CDT1 MSV: 101150159
Trần Văn Đổ Lớp: 15CDT1 MSV: 101150160
Giáo viên hướng dẫn: TS. Lê Hoài Nam
2. Nhu cầu thực tế của đề tài
Hiện nay, nhu cầu sử dụng robot để thay thế con người làm những công việc nặng
nhọc, cũng như trong những môi trường khắc nghiệt ngày càng cấp thiết. Ngành công
nghiệp robot đang ngày càng phát triển theo nhiều hình thức và cách sử dụng khác nhau.
Để xây dựng lên một hệ thống, dây chuyền sản xuất tự động, không thể thiếu robot tự
động. Chính vì thế, nhóm tác giả đã đi sâu nghiên cứu và giải quyết vấn đề này.
3. Phạm vi nghiên cứu
C C
- Nghiên cứu hệ thống, thiết kế robot 6 bậc

. L R
- Xây dựng mô hình điều khiển robot
U T
- Điều khiển, thiết kế phần mềm và phần cứng cho robot 6 bậc

4. Nội dung đề tài đã thực hiện

- Mô hình: 1
D
- Bản vẽ: 5 (A0)
- Thuyết minh gồm: 56 trang
5. Kết quả đạt được
- Tìm hiểu chung về các loại robot.
- Giải động học thuận và nghịch của robot 6 bậc.
- Nắm được phương pháp, thuật toán điều khiển.
- Thiết lập điều khiển hoạt động robot
Sinh viên thực hiện

Nguyễn Đình Danh Trần Văn Đổ

 ĐẠI HỌC ĐÀ NẴNG CỘNG HÒA XÃ HÔI CHỦ NGHĨA
TRƯỜNG ĐẠI HỌC BÁCH KHOA VIỆT NAM
KHOA CƠ KHÍ Độc lập - Tự do - Hạnh phúc

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

Họ và tên sinh viên: NGUYỄN ĐÌNH DANH MSSV: 101150159

TRẦN VĂN ĐỔ MSSV: 101150160
Lớp: 15CDT1 Khoa: Cơ Khí Ngành: Kỹ Thuật Cơ Điện Tử
1. Tên đề tài đồ án: Thiết kế và chế tạo robot 6 bậc tự do
2. Đề tài thuộc diện: ☐ Có ký kết thỏa thuận sở hữu trí tuệ đối với kết quả thực hiện
3. Các số liệu và dữ liệu ban đầu:
Vùng làm việc của robot 6 bậc
Các kích thước thông số của robot
C C
. L
4. Nội dung các phần thuyết minh và tính toán: R
Các vật liệu: nhôm nguyên khối, nhựa, nhôm định hình.

Chương 1: Tổng quan về đề tài.

Chương 2: Thiết kế cơ khí
U T
D
Chương 3: Thiết kế hệ thống điều khiển robot
Chương 4: Kết luận
5. Các bản vẽ, đồ thị ( ghi rõ các loại và kích thước bản vẽ ):
Bản vẽ tổng thể robot (1A0)
Bản vẽ sơ đồ động (1A0)
Bản vẽ sơ đồ mạch điện (1A0)
Bản vẽ lưu đồ thuật toán (1A0)
Bản vẽ các cụm chi tiết (1A0)
6. Họ tên người hướng dẫn: TS. Lê Hoài Nam
7. Ngày giao nhiệm vụ đồ án: …/…/2019
8. Ngày hoàn thành đồ án: …/…/2019
Đà nẵng, ngày 16 tháng 12 năm 2019
Trưởng Bộ môn: Người hưỡng dẫn

TS. Lê Hoài Nam

 LỜI NÓI ĐẦU

Ngày nay, với những ứng dụng của khoa học kỹ thuật tiên tiến, thế giới của chúng ta
đã và đang một ngày thay đổi, văn minh và hiện đại hơn. Sự phát triển của kỹ thuật điện
tử đã tạo ra hàng loạt những thiết bị với các đặc điểm nổi bật như sự chính xác cao, tốc
độ nhanh, tính tự động hóa cao, là những yếu tố rất cần thiết góp phần cho hoạt động
của con người đạt hiệu quả.
Có thể nói rằng robot mang tới cho cuộc sống con người một cuộc sống mới, một
cách trải nghiệm cuộc sống và đôi khi còn là người bạn. Những hãng robot từ các nước
nổi tiếng trên thế giới từ Đức, Nhật Bản, Nga, Hoa Kỳ ngày một khẳng định sự hiện
diện của robot là phần không thiếu trong cuộc sống hiện nay và tương lai của phía trước.
Nó xuất hiện ở tất cả các lĩnh vực từ khoa học vĩ mô cho tới vi mô và ngày một đa dạng.

C C
Với xu thế phát triển của thời đại cũng những kiến thức quý báu được thầy cô truyền
đạt qua 5 năm học tại trường. Nhóm tác giả quyết định lựa chọn đề tài “Thiết kế và chế
tạo robot 6 bậc tự do” .
. L R
U T
Sau một thời gian học tập và rèn luyện, với sự hướng dẫn chỉ bảo tận tình của thầy
giáo TS. Lê Hoài Nam, cùng sự trợ giúp của các quý thầy cô bộ môn và các tài liệu có

D
liên quan, mà em có thể hoàn thành xong đề tài.
Đề tài đã hoàn thành xong, nhưng không thể tránh nhiều thiếu sót mong quý thầy cô
giáo thông cảm và chỉ bảo thêm để đề tài có thể phát triển và ứng dụng rộng rãi trong
thực tế.
Em xin chân thành gửi lời cảm ơn đến các quý thầy cô!

i
 CAM ĐOAN

Chúng em xin cam đoan đồ án tuân thủ tốt các quy định về liêm chính học thuật:
Không bịa đặt, đưa ra thông tin sai lệch so với nguồn trích dẫn.
Không ngụy tạo số liệu trong quá trình khảo sát, thí nghiệm, thực hành, thực tập hoặc
hoạt động học thuật khác.
Không sử dụng các hình thức gian dối trong việc trình bày, thể hiện các hoạt động
học thuật hoặc kết quả từ quá trình học thuật của mình.
Không đạo văn, sử dụng từ ngữ, cách diễn đạt của người khác như thể là của mình,
trình bày, sao chép, dịch đoạn, hoặc nêu ý tưởng của người khác mà không trích dẫn.

C C
Không tự đạo văn, sử dụng lại thông tin nghiên cứu của mình mà không có trình dẫn
hoặc phân mảnh thông tin về kết quả nghiên cứu của mình để công bố trên nhiều ấn
phẩm.

. L R
U T Sinh viên thực hiện

D Nguyễn Đình Danh Trần Văn Đổ

ii
 MỤC LỤC

TÓM TẮT ĐỒ ÁN TỐT NGHIỆP

NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP
LỜI NÓI ĐẦU i
CAM ĐOAN ii
MỤC LỤC iii
DANH SÁCH CÁC BẢNG, HÌNH VẼ vi
MỞ ĐẦU ........................................................................................................................1
Chương 1: TỔNG QUAN VỀ ĐỀ TÀI ............................................................................2
1.1.
C
Giới thiệu về cánh tay robot .................................................................................2

C
R
1.1.1. Lịch sử hình thành và phát triển ...........................................................................2

. L
1.1.2. Xu hướng phát triển ..............................................................................................2

T
1.1.3. Cấu trúc cơ bản của robot công nghiệp ................................................................3
1.2.
D U
Ứng dụng của cánh tay robot ................................................................................3
1.2.1. Ứng dụng trong nông nghiệp ................................................................................3
1.2.2. Ứng dụng robot trong y tế ....................................................................................4
1.2.3. Ứng dụng robot trong công nghiêp.......................................................................4
1.3. Mục tiêu của đề tài................................................................................................5
1.3.1. Đặt vấn đề .............................................................................................................5
1.3.2. Cấu trúc của dự án: ...............................................................................................5
Chương 2: THIẾT KÊ CƠ KHÍ .......................................................................................7
2.1. Lựa chọn phương án .............................................................................................7
2.1.1. Yêu cầu công nghệ. ..............................................................................................7
2.1.2. Sức nâng tay máy..................................................................................................7
2.1.3. Số bậc tự do của phần công tác ( DOF: Degrees Of Freedom) ............................7
2.1.4. Độ chính xác vị trí ................................................................................................8
2.1.5. Tốc độ dịch chuyển...............................................................................................8
iii
2.1.6. Xác định vùng làm việc của robot và phương án thiết kế về mặt động học ........9
2.2. Hệ thống dẫn động và sức bền kết cấu .................................................................9
2.2.1. Mô hình dẫn động và tỷ số truyền ........................................................................9
2.2.2. Nguyên tắc chung tính chọn hệ dẫn động ..........................................................11
2.2.3. Tính toán phương trình động lực học .................................................................11
2.2.4. Phân tích lực do trọng trường tác dụng lên trục .................................................16
2.2.5. Chọn động cơ ......................................................................................................18
2.3. Chế tạo mô hình ..................................................................................................20
2.3.1. Tấm đế ................................................................................................................20
2.3.2. Khâu 1 .................................................................................................................21
2.3.3. Khâu 2 .................................................................................................................21

C C
2.3.4. Khâu 3 và khâu 4 ................................................................................................22

. L R
2.3.5. Khâu 5 và khâu 6 ................................................................................................23

T
2.3.6. Mô hình thiết kế tổng thể ....................................................................................23
2.4.
U
Chọn tay kẹp .......................................................................................................24

D
2.4.1. Giới thiệu về tay gắp mềm(soft grippers) ...........................................................24
2.4.2. Thiết kế ...............................................................................................................25
2.4.3. Các bước chế tạo tay gắp mềm ...........................................................................26
Chương 3: THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN ROBOT ........................................28
3.1. Các thành phần trong hệ thống ...........................................................................28
3.1.1. Bộ điều khiển ......................................................................................................28
3.1.2. Driver điều khiển động cơ ..................................................................................30
3.1.3. Động cơ bước .....................................................................................................38
3.2. Tính toán thuật toán điều khiển ..........................................................................39
3.2.1. Động học thuận robot .........................................................................................39
3.2.2. Động học ngược robot ........................................................................................42
Chương 4: KẾT LUẬN ..................................................................................................46
4.1. Kết quả ................................................................................................................46

iv
4.2. Đánh giá ..............................................................................................................47
4.2.1. Phần cơ khí .........................................................................................................47
4.2.2. Phần điện tử ........................................................................................................47
4.2.3. Phần điều khiển ..................................................................................................47
4.3. Kết luận ...............................................................................................................47
TÀI LIỆU THAM KHẢO .............................................................................................48
PHỤ LỤC 1: CHƯƠNG TRÌNH ARDUINO ...............................................................49
PHỤ LỤC 2: CHƯƠNG TRÌNH PYTHON .................................................................55

C C
. L R
U T
D

v
 DANH SÁCH CÁC BẢNG, HÌNH VẼ

Hình 1.1: Robot thu hoạch tự động .................................................................................3

Hình 1.2: Cánh tay robot hỗ trợ trong y tế ......................................................................4
Hình 1.3: Các cánh tay robot trong lắp ráp sản xuất ô tô ................................................5
Hình 1.4: Các thành phần của hệ thống ...........................................................................5
Hình 2.1: Vùng làm việc của robot .................................................................................9
Hình 2.2: Bộ truyền đai khâu 1 .....................................................................................10
Hình 2.3: Bộ truyện xích khâu 3 ...................................................................................10
Hình 2.4: Bộ truyền đai khâu 4 .....................................................................................11
Hình 2.5 : Hình biểu diễn tính động học .......................................................................12

C
Hình 2.6: Sơ đồ lực đặt vào đầu ra trục khớp 2 ở trạng thái giữ ...................................17

C
. L R
Hình 2.7: Động cơ bước khâu 2 ....................................................................................18
Hình 2.8: Động cơ bước khâu 1 ....................................................................................19

T
Hình 2.9: Động cơ bước khâu 3 ....................................................................................19

U
D
Hình 2.10: Động cơ bước khâu 4 ..................................................................................19
Hình 2.11: Động cơ bước khâu 5 ..................................................................................20
Hình 2.12: Động cơ bước khâu 6 ..................................................................................20
Hình 2.13: Tấm đế .........................................................................................................21
Hình 2.14: Khâu 1 .........................................................................................................21
Hình 2.15: Khâu 2 .........................................................................................................22
Hình 2.16: Khâu 3 & 4 ..................................................................................................22
Hình 2.17: Khâu 5 & 6 ..................................................................................................23
Hình 2.18: Mô hình hoàn chỉnh .....................................................................................23
Hình 2.19: Tay gắp mềm gắp đồ ăn của hãng Soft Robotics ........................................24
Hình 2.20: Tay gắp mềm ...............................................................................................25
Hình 2.21: Khuôn dưới ngón tay mềm ..........................................................................26
Hình 2.22: Khuôn trên tay gắp ......................................................................................26
Hình 2.23: Tay gắp mềm sau khí chế tạo ......................................................................27
Hình 3.1: Sơ đồ nguyên lý điều khiển ...........................................................................28
vi
Hình 3.2: Sơ đồ chân Arduino Mega2560 .....................................................................29
Hình 3.3: Driver điều khiển động cơ .............................................................................30
Hình 3.4: Các cổng kết nối của driver ...........................................................................31
Hình 3.5: Các kiểu kết nối trên Driver ..........................................................................33
Hình 3.6: Các công tắc cài đặt thông số cho Driver ......................................................33
Hình 3.7: Biểu đồ trình tự xung điều khiển ...................................................................35
Hình 3.8: Các công tắc cài đặt thông số cho DM320T .................................................36
Hình 3.9: Biểu đồ điều khiển Driver DM320T .............................................................38
Hình 3.10: Động cơ bước ..............................................................................................38
Hình 3.11: Hệ tọa độ trên mô hình ................................................................................39
Hình 3.12: Hình chiếu tính góc khớp 1 .........................................................................42
Hình 3.13: Hình chiếu tính góc thứ 3 ............................................................................43

C C
Hình 3.14: Hình chiếu tính góc 2 ..................................................................................44

. L R
Hình 4.1: Robot 6 bậc tự do ..........................................................................................46

U T
Bảng 2.1: Bảng thông số khâu 1,2 .................................................................................12

D
Bảng 2.2: Khối lượng sơ bộ các khâu ...........................................................................16
Bảng 3.1: Thống số kĩ thuật cơ bản của board Arduino Mega2560 .............................29
Bảng 3.2: Bảng thông số kĩ thuật của Driver DM542T ................................................31
Bảng 3.3: Cổng kêt nối tín hiệu .....................................................................................32
Bảng 3.4: Cổng kết nối nguồn và động cơ ....................................................................32
Bảng 3.5: Bảng thiết lập thông số các công tắc 5,6,7,8.................................................34
Bảng 3.6: Bảng thiết lập thông số ở các công tắc 1,2,3 ................................................35
Bảng 3.7: Bảng thông số kĩ thuật của Driver DM320T ................................................36
Bảng 3.8: Bảng thiết lập thông số các công tắc 5,6,7....................................................37
Bảng 3.9: Bảng thiết lập thông số ở các công tắc 1,2,3 ................................................37
Bảng 3.10: Bảng thông số D-H .....................................................................................40
Bảng 4.1:Bảng thông số cơ bản mô hình.......................................................................46

vii
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

MỞ ĐẦU

Kỹ thuật tự động là một trong những ngành kỹ thuật cao đang phát triển mạnh mẽ.
robotics, là một trong những chuyên ngành kỹ thuật điều khiển tự động, đang được
ứng dụng rộng rãi trên thế giới, đã thu hút được nhiều sự chú ý ở nước ta. Để xây
dựng nền sản xuất hiện đại, chúng ta cần nhanh chóng ứng dụng và phát triển tự động
hóa, nổi bật là các sản phẩm Cơ điện tử như robot. Vì thế đề tài nghiên cứu này xoay
quanh vấn đề tìm hiểu tính toán và thiết kế cánh tay robot công nghiệp 6 bậc tự do
nhằm mục đính nghiên cứu tiếp cận nghành công nghiệp mới này.
Mục tiêu được đặt ra là thiết kế và xây dựng thành công mô hình cánh tay máy 6
bậc tự do hoạt động hiệu quả.
Đối tượng được nghiên cứu chính ở đây là cánh tay máy AR2 với 6 khớp quay linh

hiểu để nghiên cứu.

C C
hoạt, bên cạnh đó mô hình robot 6 bậc KUKA KR60 của hãng KUKA cũng được tìm

Đồ án nghiên cứu gồm:

. L R
U
 Phương pháp thiết kế cơ khí, điện tử T
 Trình bày giới thiệu chung về cánh tay robot

D
 Tính toán động học, động lực học
 Xây dựng mô hình cánh tay robot

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 1
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

Chương 1: TỔNG QUAN VỀ ĐỀ TÀI

1.1. Giới thiệu về cánh tay robot

1.1.1. Lịch sử hình thành và phát triển
Thuật ngữ “robot” lần đầu tiên xuất hiện năm 1922 trong tác phẩm “Rosum’s
Universal Robot” của Karel Capek. Theo tiếng Czech thì robot là người làm tạp dịch.
Trong tác phẩm này nhân vật Rosum và con trai ông đã tạo ra những chiếc máy gần
giống như con người để hầu hạ con người.
Hơn 20 năm sau, ước mơ viễn tưởng của Karel Capek đã bắt đầu hiện thực. Ngay sau
chiến tranh thế giới lần thứ 2, ở Mỹ đã xuất hiện những tay máy chép hình điều khiển
từ xa, trong các phòng thí nghiệm phóng xạ.
Năm 1959, Devol và Engelber đã chế tạo robot công nghiệp đầu tiên tại công ty
Unimation.

C C
. L R
Tiếp theo Mỹ, các nước khác cũng bắt đầu sản xuất robot Công Nghiệp: Anh – (1967),
Thụy Điển – (1968), CHLB Đức – (1971), Pháp – (1972), Ý – (1973),…

U T
Năm 1967, Nhật Bản mới nhập chiếc robot công nghiệp đầu tiên từ công ty AMF
(American Machine and Foundry Company) của Mỹ. Đến năm 1990 có hơn 40 công ty

D
của Nhật, trong đó có những công ty khổng lồ như Hitachi, Mitsubishi và Honda đã đưa
ra thị trường nhiều loại robot nổi tiếng.
1.1.2. Xu hướng phát triển
Robot đã có những tiến bộ đáng kể trong hơn nửa thế kỷ qua. Robot đầu tiên được
ứng dụng trong công nghiệp vào những năm 60 để thay thế con người làm các công việc
nặng nhọc, nguy hiểm trong môi trường độc hại. Do nhu cầu cần sử dụng ngày càng
nhiều trong các quá trình sản xuất phức tạp nên robot công nghiệp cần có những khả
năng thích ứng linh họat và thông minh hơn.
Có thể kể đến một số loại robot được quan tâm nhiều thời gian qua là: Tay máy robot
(Robot Manipulators), robot di động (Mobile Robots), robot phỏng sinh học (Bio
Inspired Robots) và robot cá nhân (Personal Robots). Robot di động được nghiên cứu
nhiều như xe tự hành trên mặt đất AGV (Autonomous Guided Vehicles), robot tự hành
dưới nước AUV (Autonomous Underwater Vehicles), Máy bay không người lái UAV
(Unmanned Arial Vehicles)

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 2
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

1.1.3. Cấu trúc cơ bản của robot công nghiệp

Một robot công nghiệp được cấu thành bởi các hệ thống sau:
- Tay máy (Manipulator) là cơ cấu cơ khí gồm các khâu, khớp. Chúng hình thành
cánh tay để tạo các chuyển động cơ bản, cổ tay tạo lên sự khéo léo, linh hoạt vá
bàn tay (End Effecr) để trực tiếp hoàn thành các thao tác trên đối tượng.
- Cơ cấu chấp hành tạo chuyển động cho các khâu của tay máy. Nguồn động lực
của các cơ cấu chấp hành là động cơ các loại: điện, thủy lực, khí nén hoặc kết
hợp giữa chúng.
- Hệ thống cảm biến gồm các sensor và thiết bị chuyển đổi tín hiệu cần thiết khác.
Các robot cần hệ thống sensor trong để nhận biết trạng thái của bản thân các cơ
cấu của robot và các sensor ngoài để nhận biết trạng thái của môi trường.
- Hệ thống điều khiển (controller) hiện nay thường là máy tính để giám sát vá điều
khiển hoạt động của robot.
1.2. Ứng dụng của cánh tay robot
1.2.1. Ứng dụng trong nông nghiệp
C C
L R
Tay máy công nghiệp được chế tạo, sử dụng từ những năm 1960. Đưa robot vào cánh

.
U T
đồng hoặc nhà kính có thể giúp giải quyết nhiều vấn đề liên quan đến nông nghiệp. Máy
có thể hoạt động liên tục, kể cả vào ban đêm với đủ ánh sáng. Chúng có thể được điều

D
chỉnh để chịu được nhiệt độ và độ ẩm mà không bị quá tải nhiệt. Nhờ vậy, người nông
dân có thể thu hoạch ớt, cà chua, … với chất lượng cao nhất.

Hình 1.1: Robot thu hoạch tự động

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 3
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

Từ năm 1990, ứng dụng của robot công nghiệp đã lan sang các lĩnh vực sản xuất
ngoài ngành chế tạo máy như ứng dụng trong sản xuất thực phẩm và dược phẩm. Lúc
này, độ linh hoạt của robot được nâng cao để đáp ứng sự thay đổi của môi trường sản
xuất có nhiều bất định.
1.2.2. Ứng dụng Robot trong y tế
Robot y tế có thể phân loại theo cấu trúc cơ khí, mức độ tự động, chức năng xử lý và
môi trường hoạt động. Các robot mổ hiện nay đã đạt được độ chính xác cỡ milimet. Các
nghiên cứu phát triển về robot y tế hiện nay nhằm bảo đảm độ an toàn cho bệnh nhân,
có độ chính xác cao với giá thành cạnh tranh. Các vấn đề về tương tác người-máy, xử
lý ảnh động 3D y tế với độ phân giải cao, điều chỉnh lực và giao tiếp ngôn ngữ tự nhiên
là một số vấn đề robot y tế rất cần các nghiên cứu sâu.

C C
. L R
U T
D
Hình 1.2: Cánh tay robot hỗ trợ trong y tế
Robot y tế được sử dụng ngày càng nhiều ở các bệnh viện. Tuy nhiên còn nhiều rào
cản xã hội và giải pháp kỹ thụât chưa hoàn hảo cản trở việc ứng dụng đại trà robot trong
lĩnh vực y tế.
1.2.3. Ứng dụng robot trong công nghiêp
Các cánh tay robot ngày càng được chú trọng đầu tư nghiên cứu và phát triển với tính
ứng dụng ngày một cao hơn đặc biệt là trong công nghiệp tự động hóa ngày nay. Bạn có
thể dễ dàng bắt gặp các thiết bị này trong những môi trường sản xuất hiện đại, chúng
đảm nhận nhiều nhiệm vụ khác nhau như: phân loại kích cỡ sản phẩm, gắp vật trên các
băng tải bỏ vào khay đựng hay các công việc đòi hỏi sự chuẩn xác cao như hàn, sơn,
kiểm tra và thử nghiệm sản phẩm,…

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 4
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

C
Hình 1.3: Các cánh tay robot trong lắp ráp sản xuất ô tô

C
. L R
Với các nhiệm vụ nguy hiểm, và môi trường làm việc độc hại, rủi ro như khói bụi,
nhiệt, phóng xạ…. Robot là sự thay thế hoàn hảo cho con người. Rõ ràng, nhờ chúng,

1.3. Mục tiêu của đề tài

U T
chúng ta giảm thiểu được số vụ tai nạn lao động đáng kể.

1.3.1. Đặt vấn đề

D
Với những nhu cầu trong ngành công nghiệp hiện nay, nhóm đã thực hiện lựa chọn
đề tài thiêt kế chế tạo robot 6 bậc tự do cho đồ án tốt nghiệp năm nay, với mục đích xây
dựng thành công mô hình hoàn chỉnh, phục vụ cho nhu cầu nghiện cứu về lĩnh vực đang
rât nổi trội trong thời gian gần đây.
Ở quy mô đồ án tốt nghiệp này, nhóm đã thiết kế một robot 6 bậc tự do kết hợp với
việc sử dụng tay gắp mềm để gắp các vật mà không gây hư tổn, hay gắp đồ ăn để phục
vụ đa dạng hơn trong việc sử dụng nâng gắp vật.
1.3.2. Cấu trúc của dự án:
Các thành phần của đồ án gồm các phần chính:

Máy tính Khối điều Các khâu của Khâu chấp

khiển robot hành cuối

Hình 1.4: Các thành phần của hệ thống

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 5
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

Máy tính là công cụ điều khiển cũng như giao tiếp với các khối điều khiển để nhận
và gửi dữ liệu.
Khối điều khiển thực hiện nhận các tín hiệu từ máy tính để điều khiển chuyển động
cho mô hình robot, đồng thời cũng nhận nhiệm vụ giao tiếp với máy tính.
Các khâu của robot là nơi gắn các động cơ bước nhận lệnh từ khối điều khiển làm
xoay đồng thời nhiều khớp tạo các thao tác linh hoạt cho mô hình.
Dụng cụ thao tác được gắn trên khâu cuối của robot, dụng cụ thao tác của robot được
nhóm sử dụng ở đây là tay gắp mền gắp nhả vật nhờ vào sự bơm xả khí.

C C
. L R
U T
D

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 6
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

Chương 2: THIẾT KÊ CƠ KHÍ

2.1. Lựa chọn phương án

2.1.1. Yêu cầu công nghệ.
Robot cần thiết kế là robot dùng trong lắp ráp các chi tiết cơ khí tương đối nhẹ, có thể
đặt trong những môi trường khắc nghiệt khác nhau với các công việc khác nhau, các yêu
cầu kỹ thuật cụ thể như sau:
2.1.2. Sức nâng tay máy
Đó là khối lượng lớn nhất của vật thể mà robot có thể nâng được (không kể khối lượng
của các cơ cấu trong tay máy) trong điều kiện nhất định, ví dụ khi tốc độ dịch chuyển
cao nhất hoặc khi tay với dài nhất. Nếu robot có nhiều tay thì đó là tổng sức nâng của
các cánh tay. Thông số này quan trọng với các robot vận chuyển, xếp dỡ, lắp ráp,… Dải

C
sức nâng của tay máy thay đổi rất rộng từ 0.1 đến hàng nghìn kilogram. Các robot có

C
R
sức nâng lớn thường dùng hệ truyền động thủy lực và điện, trong đó tỉ lệ dùng động cơ

. L
điện ngày càng tăng. Truyền động khí nén cho đến nay vẫn dùng nhiều trong robot công

T
nghiệp nhưng chủ yếu với các robot có sức nâng dưới 40kg.

2.1.3.
D U
Với những yêu cầu đó, ta chọn sức nâng tay máy là 1kg.
Số bậc tự do của phần công tác ( DOF: Degrees Of Freedom)
Bậc tự do là số khả năng chuyển động của một cơ cấu (chuyển động quay hoặc tịnh
tiến). Để dịch chuyển được một vật thể trong không gian, cơ cấu chấp hành của robot
phải đạt được một số bậc tự do.
Đối với các cơ cấu có các khâu được nối với nhau bằng khớp quay hoặc tịnh tiến
(khớp động loại 5) thì số bậc tự do bằng với số khâu động. Đối với cơ cấu hở, số bậctự
do bằng tổng số bậc tự do của các khớp động.
Để định vị và định hướng khâu chấp hành cuối một cách tuỳ ý trong không gian 3
chiều, robot cần có 6 bậc tự do, trong đó 3 bậc tự do để định vị và 3 bậc tự do để định
hướng. Một số công việc đơn giản nâng hạ, sắp xếp... có thể yêu cầu số bậc tự do ít hơn.
Các robot hàn, sơn... thường yêu cầu 6 bậc tự do. Trong một số trường hợp cần sự khéo
léo, linh hoạt hoặc khi cần phải tối ưu hoá quỹ đạo người ta dùng robot với số bậc tự do
lớn hơn 6.
Robot phải có xoay tay để xoay các đối tượng theo yêu cầu đặt ra. Vì vật khớp phải
có là khớp xoay tay. Khớp này được thiết kế là khớp quay.

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 7
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

Robot cần tiếp cận đối tượng nên cần 2 khớp loại 5 là khớp vai và khớp khuỷu tay.
Đồng thời để tránh chướng ngại, hoạt động linh hoạt, robot cần thêm khớp cổ tay bố trí
sao cho khớp cổ tay song song với hai khớp vai và khuỷu tay.
Vùng làm việc chỉ là một mặt phẳng, nên muốn tăng vùng làm việc robot, ta thiết kế
một bậc tự do xoay quanh trục với trục vuông góc với mặt phẳng nằm ngang để biến
vùng làm việc từ một mặt phẳng thành một phần của hình cầu. Theo phân tích, ta dùng
khớp loại 5.
Vậy tổng số bậc tự do thiết kế là 6. Các khớp quay đó bao gồm:
- Khớp cơ sở (vai)
- Khớp vai
- Khớp khuỷu tay
- Khớp cổ tay
- Khớp xoay cổ tay
- Khớp xoay tay
Độ chính xác vị trí
C C
R
2.1.4.

. L
Độ chính xác định vị thể hiện khả năng đối tượng đạt được độ chính xác tới điểm

T
đích. Đó là thông số rất quan trọng, ảnh hưởng đến thao tác chính xác của phần công tác
và khả năng bám quỹ đạo của nó.

D U
Đối với thiết bị điều khiển số, độ chính xác định vị liên quan đến 2 thông số là độ
phân giải điều khiển (control resolution) và độ lặp lại (respeatability).
Tùy theo yêu cầu công nghệ, người ta dùng các robot có độ chính xác định vị trong
khoảng (0.05-5) mm. Với khả năng của máy tính và các thiết bị điều khiển hiện nay thì
việc giảm sai số định vị xuống 0.05 mm không gây vấn đề về kinh tế và kỹ thuật.
Ở đây độ chính xác định vị cần thiết là 0.5 mm.
2.1.5. Tốc độ dịch chuyển
Về năng suất, người ta mong muốn tốc độ dịch chuyển của phần công tác hoặc từng
khâu càng cao càng tốt. Tuy nhiên, xét về mặt cơ học, tốc độ cao sẽ gây dẫn đến các vấn
đề như giảm tính ổn định, lực quán tính lớn, sự hao mòn nhanh của các cơ cấu,…
Về điều khiển, với độ phân giải nhất định của bộ điều khiển, muốn tăng tốc độ dịch
chuyển có thể làm giảm độ chính xác định vị. Vì vậy, vấn đề chọn tốc độ dịch chuyển
hợp lý luôn phải được đặt ra khi thiết kế cũng như lựa chọn robot.
Vì vậy chọn vận tốc các khớp của robot tối đa 10 vòng/phút.

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 8
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

2.1.6. Xác định vùng làm việc của robot và phương án thiết kế về mặt động học
Hình dáng và thể tích vùng làm việc của robot là thông tin quan trọng khi lựa chọn
ứng dụng vào các mục đích cụ thể. Vì vậy để đáp ứng được yêu cầu làm việc trong một
không gian làm việc mong muốn và nhu cầu về độ linh hoạt trong vùng làm việc đó ta
đưa ra mô hình thiết kế robot 6 như hình vẽ và nó có vùng hoạt động như hình vẽ ( vùng
giới hạn bao bởi nét đứt). Kết quả của bài toán này có thể ứng dụng vào quá trình thiết
kế robot nói chung.

C C
. L R
U T
D
Hình 2.1: Vùng làm việc của robot
Thông qua các yêu cầu về vùng làm việc ta đưa ra phương án thiết kế động học, ta sẽ
nói ở phần sau.
Thiết kế các trục của các khâu khác nhau, tùy thuộc vào chức năng làm việc, đảm bảo
đủ độ cứng và khả năng làm việc lâu dài
Các bộ truyền không nhất thiết phải sử dụng các bộ truyền để giảm tốc vì đã có hộp
giảm tốc riêng cho mỗi động cơ. Các bộ truyền được chọn phải phù hợp với từng cơ cấu
và chức năng làm việc.
2.2. Hệ thống dẫn động và sức bền kết cấu
2.2.1. Mô hình dẫn động và tỷ số truyền
Vì kết cấu của robot là nhỏ nên sử dụng bộ truyền đai răng và bộ truyền xích để dẫn
động. Bộ truyền đai răng có nhiệm vụ chính là kéo dài khoảng cách trục từ động cơ tới
trục vít, ta có thể bố trí động cơ có khối lượng lớn về gần gối truc, mục đích là để giảm
tải trọng cho trục động cơ.
- Khớp quay thứ nhất được dẫn động qua bộ truyền đai răng với tỉ số truyền 1:4.

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 9
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

Hình 2.2: Bộ truyền đai khâu 1

- Khớp quay thứ hai được dẫn động với tỉ số truyền 1:1
- Khớp quay thứ ba được dẫn động qua bộ truyền xích với tỉ số truyền 1:1

C C
. L R
U T
D

Hình 2.3: Bộ truyện xích khâu 3

- Khớp quay thứ tư được dẫn động qua bộ truyền đai răng với tỉ số truyền 1:3 và
hộp giảm tốc với tỉ số truyền 1:14

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 10
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

Hình 2.4: Bộ truyền đai khâu 4

2.2.2. Nguyên tắc chung tính chọn hệ dẫn động

C
Thiết kế chi tiết từng khâu, khớp từ khớp trên cùng xuống khớp thấp nhất

C
-
. R
Thiết kế chi tiết hệ dẫn động và kết cấu cho từng khâu, khớp theo các bước sau:

L
Chọn vị trí nguy hiểm nhất, lực ma sát trên ổ trục sinh ra lớn nhất và momen sinh

nằm ngang.
U T
ra do trọng lượng của khâu tiếp theo là lớn nhất. Thông thường là vị trí cánh tay

-
-
D
Vẽ sơ đồ phân bố lực và tính được momen sinh ra trên trục.
Chọn động cơ bước theo momen làm việc, tốc độ, độ phân giải, kích thước cánh
tay…
- Phân phối lại tỉ số truyền nếu cần.
- Thiết kế chi tiết trên phần mềm SolidWork, gán vật liệu tính khối lượng của khâu
chứa trục khớp.
2.2.3. Tính toán phương trình động lực học
Xét các khâu của robot ta thấy chỉ có khâu 2 và khâu 3 robot quay vuông góc với trục
động cơ nên có cánh tay đòn lớn nhất và động lực học của 2 khâu nay là quan trọng
nhất, chúng chịu lực nhiều nhất. Khi đó, ta quy robot thành 2 khâu với khâu 1 chính là
khâu thứ 2 của robot với độ dài khâu l1, khâu thứ 2 là sự tổng hợp của khâu 3, 4, 5, 6
với độ dài khâu là l2. Ta gắn hệ tọa độ như hình vẽ:

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 11
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

Hình 2.5 : Hình biểu diễn tính động học

Ta có bảng thông số DH:
C C
L R
Bảng 2.1: Bảng thông số khâu 1,2

.
Khâu

1
θi
θ1*+90
U T αi
0
ai
l1
di
0

2 θ2*+90

Đặt θ1 = θ1*+90 và θ2 = θ2*+90

D 0 l2 0

Ta có các mà trận Ai của robot như sau:

𝑐𝑜𝑠(𝜃1) −𝑠𝑖𝑛(𝜃1) 0 𝑙1 ∗ 𝑐𝑜𝑠(𝜃1)
𝑠𝑖𝑛(𝜃1) 𝑐𝑜𝑠(𝜃1) 0 𝑙1 ∗ 𝑠𝑖𝑛(𝜃1) ]
𝐀𝟏 = [ ( 2.1 )
0 0 1 0
0 0 0 1
𝑐𝑜𝑠(𝜃2) −𝑠𝑖𝑛(𝜃2) 0 𝑙2 ∗ 𝑐𝑜𝑠(𝜃2)
𝑠𝑖𝑛(𝜃2) 𝑐𝑜𝑠(𝜃2) 0 𝑙2 ∗ 𝑠𝑖𝑛(𝜃2) ]
𝐀𝟐 = [ ( 2.2 )
0 0 1 0
0 0 0 1
𝐶12 −𝑆12 0 𝑙1𝐶1 + 𝑙2𝐶12
𝑆12 𝐶12 0 𝑙1𝑆1 + 𝑙2𝑆12
Ma trận T1= A1 và T2 = A1.A2 = [ ]
0 0 1 0
0 0 0 1
Ta có phương pháp tính đạo hàm của ma trận Ai đối với biến khớp qi theo cách đơn gian
như sau:

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 12
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

𝑑𝐴𝑖
= Qi.Ai ( 2.3 )
𝑑𝑞𝑖

Đối với khớp quay:

0 −1 0 0
1 0 0 0
Qi = [ ] ( 2.4 )
0 0 0 0
0 0 0 0
Trong trường hợp i = 1, 2, ..., n ta có:
𝜕𝑇𝑖 𝜕
= (A1A2... Aj-1Aj... Ai-1Ai) ( 2.5 )
𝜕𝑗 𝜕𝑞𝑗

Theo công thức (6-40), (6-43) sách Robot CN chỉ có Aj phụ thuộc vào qij ta có:
𝜕𝑇𝑖 𝑑𝐴𝑗
= A1A2... Aj-1 ... Ai-1Ai = A1A2... Aj-1QjAj... Ai-1Ai ( 2.6 )
𝜕𝑞𝑗 𝑑𝑞𝑗

Đặt:

Uij= {
𝑇𝑗−1 𝑄𝑗
𝑗−1
𝑇𝐼 𝑁ế𝑢 𝑗 ≤ 𝑖
C C ( 2.7 )
0
Áp dụng ta có:
𝑁ế𝑢 𝑗 > 𝑖

. L R
0
1
−1
0
0
0
U
0 𝐶1
0 𝑆1
T
−𝑆1
𝐶1
0
0
𝑙1𝐶1
𝑙1𝑆1
−𝑆1
𝐶1
−𝐶1
−𝑆1
0
0
−𝑙1𝑆1
𝑙1𝐶1

D
𝜕𝑇
U11 = 1 =Q1A1= [ ][ ]=[ ]
𝜕𝜃1 0 0 0 0 0 0 1 0 0 0 0 0
0 0 0 0 0 0 0 1 0 0 0 0
Tương tự U21 và U22 được tính như sau:
0 −1 0 0 𝐶12 −𝑆12 0 𝑙1𝐶1 + 𝑙2𝐶12
𝜕𝑇 1 0 0 0 𝑆12 𝐶12 0 𝑙1𝑆1 + 𝑙2𝑆12
U21= 2 = Q1T2 = [ ][ ]=
𝜕𝜃1 0 0 0 0 0 0 1 0
0 0 0 0 0 0 0 1
−𝑆12 −𝐶12 0 −(𝑙1𝑆1 + 𝑙2𝑆12)
[ 𝐶12 −𝑆12 0 𝑙1𝐶1 + 𝑙2𝐶12 ]
0 0 0 0
0 0 0 0
𝜕𝑇2
U22= =A1Q1𝐴2 =
𝜕𝜃2
𝑐𝑜𝑠(𝜃1) −𝑠𝑖𝑛(𝜃1) 0 𝑙1 ∗ 𝑐𝑜𝑠(𝜃1) 0 −1 0 0 𝑐𝑜𝑠(𝜃2) −𝑠𝑖𝑛(𝜃2) 0 𝑙2 ∗ 𝑐𝑜𝑠(𝜃2)
𝑠𝑖𝑛(𝜃1) 𝑐𝑜𝑠(𝜃1) 0 𝑙1 ∗ 𝑠𝑖𝑛(𝜃1) 1 0 0 0 𝑠𝑖𝑛(𝜃2) 𝑐𝑜𝑠(𝜃2) 0 𝑙2 ∗ 𝑠𝑖𝑛(𝜃2)
[ ][ ][ ]
0 0 1 0 0 0 0 0 0 0 1 0
0 0 0 1 0 0 0 0 0 0 0 1

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 13
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

−𝑆12 −𝐶12 0 −𝑙2𝑆12

𝐶12 −𝑆12 0 𝑙2𝐶12
=[ ]
0 0 0 0
0 0 0 0
Theo (6-19)/tr 117 sách Robot CN, giả thiết các tích quán tính bằng không, ta có các ma
trận giả quán tính JI như sau:
1/3𝑚1𝑙12 0 0 −1/2𝑚1𝑙1 1/3𝑚2𝑙22 0 0 −1/2𝑚2𝑙2
0 0 0 0 0 0 0 0
J1 = [ ] J2 = [ ]
0 0 0 0 0 0 0 0
−1/2𝑚1𝑙1 0 0 𝑚1 −1/2𝑚2𝑙2 0 0 𝑚2
Theo (6-35)/tr 120 sách Robot CN, Ta có hiệu ứng quán tính trên các khâu như sau:
𝑇 𝑇
D11 = Tr(U11J1𝑈11 ) + Tr(U21J2𝑈21 ) ( 2.8 )
=Tr
−𝑆1 −𝐶1 0 −𝑙1𝑆1 1/3𝑚1𝑙12 0 0 −1/2𝑚1𝑙1 −𝑆1 𝐶1 0 0
{[
𝐶1 −𝑆1 0
0 0 0
𝑙1𝐶1
0
][
0
0
0
0
0
0
0
0
][

C C
−𝐶1 −𝑆1
0 0
0
0
0
0
]}

R
0 0 0 0 −1/2𝑚1𝑙1 0 0 𝑚1 −𝑙1𝑆1 𝑙1𝐶1 0 0
+Tr
−𝑆12 −𝐶12 0 −(𝑙1𝑆1 + 𝑙2𝑆12) 1/3𝑚2𝑙22 0

T . L
0 −1/2𝑚2𝑙2 −𝑆12 𝐶12 0 0

U
0 0 0 0 −𝐶12 −𝑆12 0 0
{[ 𝐶12 −𝑆12 0 𝑙1𝐶1 + 𝑙2𝐶12 ] [ ][ ]}
0 0 0 0 0 0 0 0 0 0 0 0

D
0 0 0 0 −1/2𝑚2𝑙2 0 0 𝑚2 −(𝑙1𝑆1 + 𝑙2𝑆12) 𝑙1𝐶1 + 𝑙2𝐶12 0 0
1 4
= m1l12 + m2l22 + m2l22C2
3 3
𝑇
D12 = D21 = Tr(U22J2𝑈21 ) = Tr
−𝑆12 −𝐶12 0 −𝑙2𝑆12 1/3𝑚2𝑙22 0 0 −1/2𝑚2𝑙2 −𝑆12 𝐶12 0 0
𝐶12 −𝑆12 0 𝑙2𝐶12 0 0 0 0 −𝐶12 −𝑆12 0 0
{[ ][ ][ ] }
0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 −1/2𝑚2𝑙2 0 0 𝑚2 −(𝑙1𝑆1 + 𝑙2𝑆12) 𝑙1𝐶1 + 𝑙2𝐶12 0 0
1 1
= m2l22 + m2l22C2
3 2
𝑇
D22 = Tr(U22J2𝑈22 )
=
−𝑆12 −𝐶12 0 −𝑙2𝑆12 1/3𝑚2𝑙22 0 0 −1/2𝑚2𝑙2 −𝑆12 𝐶12 0 0
0 0 0 0 −𝐶12 −𝑆12 0 0
Tr{[ 𝐶12 −𝑆12 0 𝑙2𝐶12 ] [ ][ ] }
0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 −1/2𝑚2𝑙2 0 0 𝑚2 −𝑙2𝑆12 𝑙2𝐶12 0 0
1 1 1
= m2 l22S122 + m2l22C122 = m2 l22
3 3 3

Theo (6-36)/ Tr 124 sách Robot CN ta có thành phần của lực ly tâm và Cariolis của các
khâu như sau:

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 14
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

1
Khâu 1: C1 = D111𝜃′12 + D112𝜃′1 𝜃′2 + D121𝜃′1 𝜃′2 + D122𝜃′22 = − m2S2l12𝜃′22 –
2
2
m2S2l1 𝜃′1 𝜃′2
1
Khâu 2: C2 = D211𝜃′12 + D212𝜃′1 𝜃′2 + D221𝜃′1 𝜃′2 + D222𝜃′22 = m2S2l22𝜃′12
2
1
− m2S2l12 𝜃′22 – m2S2l12 𝜃′1 𝜃′2
′ 2
Vì vậy: C(𝜃, 𝜃 ) = [ 1 ]
m2S2l22 𝜃′21
2

Tiếp theo ta xác định thành phần ảnh hưởng của lực trọng trường Gi. Theo (6-37)/tr124
sách Robot CN ta có:
G1 = - (m1𝑔𝑇 U11r1+ m2𝑔𝑇 U21r2) ( 2.9 )
−𝑆1 −𝐶1 0 −𝑙1𝑆1 0
𝑙1/2
=-m1(0, -g, 0, 0)[ 𝐶1 −𝑆1 0 𝑙1𝐶1 ] [ ]
0 0 0 0 0
0 0 0 0 1
−𝑆12 −𝐶12 0 −(𝑙1𝑆1 + 𝑙2𝑆12) 0
-m2(-g, 0, 0, 0)[ 𝐶12 −𝑆12
0 0
0
0
𝑙1𝐶1 + 𝑙2𝐶12 ] [−𝑙2/2]
0 0
C C
0
𝑆1
0
𝑙2
0 0

. L R
1

T
= m1gl1(c1- ) + m2g( C12-l1S1-l2S12)
2 2

−𝑆12 −𝐶12 0
G2 = m2gTU22r2 = -m2(-g, 0, 0, 0) [ 𝐶12 −𝑆12 0
0
0
0
0
0
0 D U −𝑙2𝑆12

0
0
0
𝑙2𝐶12 −𝑙2/2
][
0
1
]

𝐶12
= m2gl2( - S12)
2
𝑆1 𝑙2
m1gl1(c1 − ) + m2g( C12 − l1S1 − l2S12)
2 2
Ta có ma trận G(𝜃)=[ 𝐶12 ]
m2gl2( − S12)
2

Cuối cùng ta có phương trình động lực học của robot theo phương trình Lagrange-Euler
ta có:
F= J(q(t)𝑞̈ (t)) + C(q(t),𝑞̇ (t)) + G(q(t)) ( 2.10 )
1 4 1 1
m1l12 + m2l22 + m2l22 C2 m2l22 + m2l22 C2 𝜃1̈
3 3 3 2
F=[ 1 1 1 ] ( ̈ )+
m2l2 + 2
m2l22 C2 m2l22 𝜃2
3 2 3
1 𝑆1 𝑙2
−2m2S2l12 𝜃̇22 – m2S2l12 𝜃̇1 𝜃̇2 m1gl1(c1 −
2
) + m2g( C12 − l1S1 − l2S12)
2
[ 1 ]+[ 𝐶12 ]
2
m2S2l22 𝜃̇12 m2gl2( − S12)
2

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 15
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

2.2.4. Phân tích lực do trọng trường tác dụng lên trục
Sau khi ta tính toán chọn cơ cấu phù hợp cho các trục khớp 3, 4, 5 và 6 ta thiết kế chi
tiết các khâu này trên phần mềm SolidWork. Gán vật liệu cho từng chi tiết sau đó tính
toán được khối lượng các chi tiết theo bảng sau:
Bảng 2.2: Khối lượng sơ bộ các khâu

Khối
STT Tên Chi Tiết Vật Liệu Chú Thích
Lượng(g)
1 Đế khâu 2 Nhôm 773

2 Cánh tay khâu 2 Nhôm 621

Trục 2, mặt bích ghép, đai Nhôm,

3 130
ốc, vít Gang

C
Thép hợp
4 Ổ bi trục 2 215
kim

R C
5

6
Bộ truyền xích khâu 2

Động cơ khâu 2
T . L
Thép 205

1800 Động cơ bước

7
Trục 3, mặt bích ghép, đai
ốc, vít
D U Nhôm,
Gang
165

8 Động cơ khâu 3 880 Động cơ bước

Thép hợp
9 Ổ bị trục 3 205
kim
10 Khâu 4 Nhôm 621
11 Trục 4, đai, giá đỡ động cơ 4 Nhôm, Thép 283

12 Động cơ 4 310 Động cơ bước

13 Ổ bi trục 5 Thép 185

14 Động cơ khâu 5, vít me Thép 400 Động cơ bước

15 Khâu 5 Nhôm 37
16 Động cơ trục 6 320 Động cơ bước

Tay gắp bằng

17 Tay gắp mềm silicon 80
silicon

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 16
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

- Khối lượng khâu 2: m2 = 4829 g

- Khối lượng khâu 4: m3 = 1614 g
- Khối lượng khâu 5: m4 = 542 g
- Khối lượng khâu 6, tay kẹp và vật kẹp: m5 = 280 g
Chọn vị trí nguy hiểm nhất chính là khi các khâu 2, 3, 4, 5, 6 duỗi thẳng ở vị trí nằm
ngang, lúc này momen tĩnh do trọng trường gây ra là lớn nhất:

C C
. L R
U T
D
Hình 2.6: Sơ đồ lực đặt vào đầu ra trục khớp 2 ở trạng thái giữ
Trong đó:
 P2, P4, P5, P6 là trọng lượng của các khâu 2, 4, 5, 6. Pv là trọng lương vật
nâng
 P2 = m2.g = 4,83 . 9,8 = 47,33 N
 P4 = m4.g = 1,61 . 9,8 = 15,78 N
 P5 = m5.g = 0,54 .9,8 = 5,29 N
 P6 = m6.g = 0,18 .9,8 = 1,76 N
 Pv = mv.g = 0,1 .9,8 = 0,98 N

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 17
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

2.2.5. Chọn động cơ

Tính mô men tĩnh tác dụng lên trục 2:
- Tổng mô men tĩnh do trọng lực gây ra trên trục O2:
 Mt2 = P2.0,152 + P4.0,416 + P5.0,546 + P6.0,596 + Pv.0,635 = 18,32 Nm
Ngoài ra khi làm việc từ trạng thái ban đầu là đứng yên, lực quán tính sinh ra kết hợp
với tải trọng tĩnh sẽ gây ra lực cản lớn nhất đặt lên trục khớp 2. Ở đây ta bỏ qua ma sát
ổ trục do sử dụng ổ bi có hiệu suất truyền động cao (hệ số ma sát thấp 0,0015 – 0.006).
Tính mô men cản do quán tính của các bộ phận chuyển động:
- Để đơn giản trong việc tính toán ta xem các khâu là các thanh thẳng và vật nâng
là chất điểm trong quá trình tính mô men quán tính.
- Mô men quán tính khâu 2 đối với trục O2: I2 = 4,83 .0,3052/3 = 0,15 Nm2.
 I4 = 1,61. 0,2082/12 + 1,61.0,4162= 0,2844 Nm2.( định lí dời trục Steiner-
Huygens)
 I5 = 0,164 Nm2;
C C
-
 I6 = 0,115 Nm2.

. L R
Cho động cơ đạt yêu cầu có thể tăng tốc từ 0-600v/ph trong 1s.Ta có gia tốc góc
cần đạt

U
 ɛdc = 600.π/(60.1.180) = 0,17 rad/s2. T
- Mô men sinh ra do quán tính:
D
 Mqt2 = (I2 + I4 + I5+ I6 ). ɛdc = (0,15 + 0,2844 + 0,164 + 0,115). 0,17
= 0,12Nm.
- Mô men cản trên trục 2: Mc2= Mt2 + Mqt2 = 18,32 + 0,12 ≈ 18440 Nmm.
- Suy ra mô men yêu cầu trên trục động cơ:
 Mdcyc = 18440 Nmm.
Ta chọn động cơ Step nema 23HS22-2804S-HG50 có các thông số kỹ thuật như sau:
- Động cơ bước khâu 2 : Nema 23 Stepper
Motor L=56mm Gear Ratio 50:1
- Mã : 23HS22-2804S-HG50
- Thông số kĩ thuật:
 Tỷ lệ bánh răng: 50: 1
 Dòng điện định mức / pha: 2.8A
 Mô-men xoắn cho phép: 40Nm

Hình 2.7: Động cơ bước khâu 2

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 18
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

Với tính toán tương tự ta cũng tính chọn được các động cơ khâu tiếp theo và chọn
được động cơ bước phù hợp với yêu cấu như sau:

- Động cơ bước khâu 1 : Động cơ Nema 17

Stepper Motor L=39mm Gear Ratio 20:1
- Mã : 17HS15-1684S-HG20
- Thông số kĩ thuật:
 Tỷ lệ bánh răng: 20: 1
 Dòng điện định mức / pha: 1.68A
 Mô-men xoắn cho phép: 15Nm
Hình 2.8: Động cơ bước khâu 1

- Động cơ bước khâu 3 : Nema 17 Stepper

C C
Motor L=39mm Gear Ratio 50:1
- Mã : 17HS15-1684S-HG50
. L R
- Thông số kĩ thuật:
 Tỷ lệ bánh răng: 50: 1
U T
D
 Dòng điện định mức / pha: 1.68A
 Mô-men xoắn cho phép: 15Nm

Hình 2.9: Động cơ bước khâu 3

- Động cơ bước khâu 4 : Nema 11

Stepper Motor Bipolar L=51mm w/
Gear Ratio 14:1
- Mã : 11HS20-0674S-PG14
- Thông số kĩ thuật:
 Tỷ lệ bánh răng: 14: 1
 Dòng điện định mức / pha: 0.67A
 Mô-men xoắn cho phép: 3Nm

Hình 2.10: Động cơ bước khâu 4

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 19
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

- Động cơ bước khâu 5 : Nema 17

External 48mm Stack 1.68A Lead
8mm/0.31496" Length 200mm
- Mã : 17LS19-1684E-200G
- Thông số kĩ thuật:
 Điện áp: 2.8V
 Dòng điện định mức / pha: 1.68A
 Mô-men xoắn cho phép: 44 Ncm
Hình 2.11: Động cơ bước khâu 5

- Động cơ bước khâu 6 : Nema 14

Stepper Motor Bipolar L=33mm w/
Gear Ratio 19:1
- Mã : 14HS13-0804S-PG19
C C
- Thông số kĩ thuật:
 Tỷ lệ bánh răng: 19: 1
. L R
 Dòng điện định mức / pha: 1A
 Mô-men xoắn cho phép: 4Nm
U T
D Hình 2.12: Động cơ bước khâu 6

2.3. Chế tạo mô hình

2.3.1. Tấm đế
Đế gồm 2 phần, phần trên tiếp xúc với động cơ và bắt với khâu 1 được làm bằng vật
liệu nhôm đặc gia công. Phần dưới là phần chống trượt và dùng để tiếp xúc với mặt bàn
được làm bằng nhựa PLA in 3D.
Ngoài ra phần đế được mang bộ truyền đai thang và trụ có ổ bi đũa để truyền momen
cho khâu 1 chuyển động. Được thế kế chắc chắn, làm trọng tâm cho robot chuyển động
đồng thời đảm bảo dễ dàng tháo lắm di chuyển.

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 20
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

Hình 2.13: Tấm đế

2.3.2. Khâu 1
C C
trong mặt phẳng song song với mặt phẳng đế.
. L R
Khâu 1 được gắn lên đế bằng liên kết bu lông, đồng thời nhận chuyển động và quay

U T
D

Hình 2.14: Khâu 1

2.3.3. Khâu 2
Khâu 2 là khâu giúp mô hình đạt các thông số chiều cao yêu cầu, là khâu có chuyển
động quay vuông góc với mặt phẳng đế. Có hình dạng như hình dưới:

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 21
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

C C
. L R
2.3.4. Khâu 3 và khâu 4
U T
Hình 2.15: Khâu 2

D
Khâu 3 gắn với khâu 4 giúp tạo tầm với cho robot tạo 2 chuyển động quay theo 2 mặt
phẳng vông góc với nhau góp phần tạo chuyển động đã dạng cho mô hình.

Hình 2.16: Khâu 3 & 4

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 22
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

2.3.5. Khâu 5 và khâu 6

Là 2 khâu tạo các chuyển động phức tạp trong các thao tác chấp hành cuối cùng của
tay gắp theo yêu cầu đặt ra, mang các hình dáng như hình dưới:

C C
. L R
Hình 2.17: Khâu 5 & 6
2.3.6. Mô hình thiết kế tổng thể

U T
D

Hình 2.18: Mô hình hoàn chỉnh

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 23
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

2.4. Chọn tay kẹp

Với bài toán yêu cầu đặt ra cần chế tạo một tay gắp có thể thực hiện các nhiệm vụ các
thao tác gắp vật mềm, linh hoạt, bám vào biên dạng bề mặt nhưng không làm hư hỏng
vật thể chúng em đã đưa ra ý tưởng thiết kế và chế tạo một tay gắp mềm bằng silicone.
2.4.1. Giới thiệu về tay gắp mềm(soft grippers)
Robot mềm là một lĩnh vực nghiên cứu đang phát triển trên thế giới hiện nay, sử
dụng sự tuân thủ và khả năng thích ứng linh hoạt của các cấu trúc mềm để phát triển
robot có khả năng thích ứng cao cho các tương tác mềm. Một lĩnh vực trong đó robot
mềm có khả năng tạo ra tác động đáng kể là trong việc phát triển các dụng cụ kẹp và
thao tác mềm.

C C
. L R
U T
D
Hình 2.19: Tay gắp mềm gắp đồ ăn của hãng Soft Robotics
Với yêu cầu gia tăng về tự động hóa, các hệ thống robot được yêu cầu thực hiện
nhiệm vụ trong các môi trường không có cấu trúc vô định hình và cần sự linh hoạt trong
biên dạng vật gắp, điều kiện mà robot cứng nhắc thông thường không phù hợp nhất.
Điều này đòi hỏi một sự thay đổi mô hình trong các phương pháp và vật liệu được sử
dụng để phát triển robot sao cho chúng có thể thích nghi và hoạt động an toàn trong môi
trường mà nó hoạt động.
Một giải pháp cho vấn đề này là robot mềm, ở đây chúng ta đang nói đến tay gắp
mềm, nó phép tương tác mềm với môi trường xung quanh trong khi vẫn duy trì khả năng
tác dụng lực đáng kể.
Ở trong khuôn khổ đồ án này chúng ta đề cập đến tay gắp mềm mà chúng ta đang áp
dụng được chế tạo bằng vật liệu silicone., với khả năng chịu áp suất lớn, bền với nhiệt,
độ linh hoạt cao nó được đánh giá là một loại tay gắp ưu việt hiện nay.
Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 24
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

Với những lợi ích tiềm tàng của tay gắp mềm đối với cuộc sống và xã hội chúng ta
cần khai thác và phát triển chúng để ứng dụng vào thực tế.
2.4.2. Thiết kế
Với kích thước yêu cầu gắn được lên cánh tay robot và chịu được lực khi gắp vật
cũng như chịu được áp suất cho phép, đối với vật liệu silicone áp suất chịu được tối đa
là 1130kg/m2.
Cánh tay được thiết kế với kích thước phù hợp với yêu cầu đặt ra khi thiết kế cánh
tay robot, đảm bảo gắp nhả vật tốt, đồng thời đảm bảo yêu cầu gọn nhẹ dễ dàng tháo lắp
thay thế khi cần thiết. Dó đó tay gắp được nhóm thiết kế có kích thước và hình dáng như
hình vẽ:

C C
. L R
U T
D

Hình 2.20: Tay gắp mềm

Từ biên dạng và kích thước của ngón tay gắp mềm ta thực hiện thiết kế khuôn để đúc
ngón tay:
Bên dưới là phần khuôn đúc tay gắp được thiết kế gồm 2 phần chính:
- Khuôn trên
 Khuôn đúc định hình biên dạng cho ngón tay gắp
 Xương định hình lỗ cấp khí
 Nắp đậy
- Khuôn dưới

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 25
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

Hình 2.21: Khuôn dưới ngón tay mềm

C C
. L R
U T
D

Hình 2.22: Khuôn trên tay gắp

2.4.3. Các bước chế tạo tay gắp mềm
 Giới thiệu về Silicones Dragon Skin
Silicones Dragon Skin là hợp chất silicon lỏng được sử dụng cho nhiều ứng dụng
khác nhau, từ tạo hiệu ứng da và các hiệu ứng đặc biệt khác để làm khuôn sản xuất để
đúc nhiều loại vật liệu. Do tính chất vật lý vượt trội và tính linh hoạt của cao su, chúng
cũng được sử dụng cho các bộ phận giả y tế và các ứng dụng đệm.
Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 26
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

Cao su Dragon Skin cũng được sử dụng cho nhiều ứng dụng công nghiệp và có dải
nhiệt độ hoạt động trong khoảng -65° F đến + 450°F (-53°C đến + 232°C).
 Tính năng, đặc điểm
Dragon Skin là vật liệu có hiệu ứng đặc biệt linh hoạt, mềm mại, siêu mạnh mẽ và co
giãn cực kỳ tốt, Chúng có thể đạt được vô số hiệu ứng màu bằng cách thêm các sắc tố
silicon Silc Pig hoặc bột hiệu ứng Cast Magic. Để sử dụng Silicones Dragon Skin được
trộn 1A: 1B theo trọng lượng hoặc thể tích. Khi pha trộn chúng với nhau cao su lưu hóa
ở nhiệt độ phòng (73 ° F / 23 ° C) với độ co rút không đáng kể, trước khi để chúng lưu
hóa( đông cứng lại) chúng ta cần hút chân không để hạn chế bọt khí.
 Các bước chế tạo
- Lắp khuôn đúc phần buồn khí tay gắp và phần đế ngón tay.
- Trộn các thành phần silicone để tạo hỗn hợp silicone đồng chất: Chúng ta khuấy
đều 2 lọ Silicones Dragon Skin 30A và Silicones Dragon Skin 30B trước khi pha

cân nhắc lượng silicone pha trộn cho phù hợp.

C C
trộn với tỉ lệ 1:1, tùy thuộc vào tay gắp có kích thước là lớn hay nhỏ mà chúng ta

-
L R
Hút chân không hỗn hợp silicone để loại bỏ bọt khí.
.
-
-
U T
Đúc phần buồng khí: Rót hỗn hợp silicone vào khuôn và đợi silicone lưu hóa.
Đúc phần đế: bao gồm phần đế và phần xương cố định để tạo sự chắc chắn và

-
D
hướng co của ngón tay. Rót phần silicone vào đế tạo xương phần đế này bằng
một mảnh giấy mềm, đợi siicone lưu hóa
Dán phần buồn khí vào phần đế để tạo ra một tay gắp mềm.

Hình 2.23: Tay gắp mềm sau khí chế tạo

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 27
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

Chương 3: THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN ROBOT

3.1. Các thành phần trong hệ thống

C C
. L R
U T
D
Hình 3.1: Sơ đồ nguyên lý điều khiển
Máy tính là nơi nhận dữ liệu từ người điều khiển sau đo chuyển đến bộ điều khiển
trung tâm để tiến hành xử lý điều khiển vị trí làm việc mong muốn cho robot được viết
theo mã mà hệ thống quy định.
Bộ điều khiển thực hiện việc truyền nhận dữ liệu điều khiển từ máy tính và giải mã
tín hiệu đồng thời thực hiện tính toán truyền xung điều khiển đến các driver điều khiển
động cơ để thực hiện điều khiển góc quay cho động cơ. Bộ điều khiển còn thực hiện
nhận các tín hiện phản hồi của các công tắc hành trình, nút nhấn để thực hiện các chức
năng được đề ra cho bộ điều khiển.
3.1.1. Bộ điều khiển
Trong nghiên cứu này, việc lựa chọn các thành phần điều khiển được dựa trên mức
độ gần gũi, dễ sử dụng và đạt hiệu quả kinh tế nên bộ phận điều khiển được nhóm tác
giả đã lựa chọn là boar điều khiển Arduino Mega 2560, kết hợp giao tiếp máy tính qua
giao tiếp nối tiếp UART để tiến hành truyền nhận thông tin.

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 28
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

Hình 3.2: Sơ đồ chân Arduino Mega2560

Khối vi điều khiển sử dụng vi điều khiển Arduino Mega, nó đóng vai trò hết sức quan

C
trọng trong hệ thống để điều khiển hệ thống. Các chân của vi điều khiển sẽ được kết nối
C
. L R
với các khối khác như rơ le, bàn phím, khối hiển thị,… Toàn bộ dữ liệu mà ta thiết kế
để điều khiển hệ thống khóa số đều được chứa trong bộ nhớ của vi điều khiển

U
thông số kĩ thuật cơ bản của board bao gồm: T
Arduino Mega 2560 là một vi điều khiển hoạt động dựa trên chip ATmega2560. Các

Vi điều khiển
D
Bảng 3.1: Thống số kĩ thuật cơ bản của board Arduino Mega2560
ATmega2560
Điện áp hoạt động 5VDC
Nguồn ngoài (jackdc) 7-12VDC
Xung clock 16Mhz
Số chân digital I/O 54
Số chân PWM 15
Số chan Analog 16
Giao tiếp UART 4 Bộ
Giao tiếp SPI 1 Bộ
Giao tiếp I2C 1 Bộ
Ngắt ngoài 6
256Kb
Flash
(8Kb dùng cho Bootloader)
SRAM 8Kb
EEPROM 4Kb

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 29
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

 Một vài lưu ý khi sử dụng Arduino Mega:

- Khi bắt đầu sử dụng Arduino Mega 2560, bạn nên chú ý lựa chọn lại board. Bằng
cách vào Tool → Board → Arduino Mega 2560. Nhằm tránh trước đó bạn đã sử
dụng loại Arduino khác cổng vẫn còn nhận là board cũ nên khi build bạn sẽ gặp
lỗi.
- Khi sử dụng chân RX, TX cuả Arduino, các bạn nên nhớ rút dây cắm tại 2 chân
này ra rồi hãy bắt đầu upload. Sau đó hãy cắm lại bình thường và sử dụng để
tránh gặp phải lỗi.
- Không được phép cắm trực tiếp chân GND vào chân nguồn 5V, có thể dẫn tới
hỏng mạch.
3.1.2. Driver điều khiển động cơ
Driver được lựa chọn điều khiển ở đây là loại DM524T và DM320T của hãng
Leadshine. Là loại driver step với thiết kế đơn giản, dễ dàng thiết lập. Bằng cách dùng

C C
công nghệ điều khiển bước tiên tiến của Leadshine, driver này có thể cung cấp năng
lượng cho động cơ bước 2 pha và 4 pha với mô men xoắn tối ưu và tránh được tiếng ồn

. L R
động cơ xuống mức tối đa. Điện áp hoạt động của nó là 20-50 VDC và có thể tạo ra
dòng điện lên tới 4,2 A. Tất cả các bước vi mô và dòng điện đầu ra được thực hiện thông

U T
qua các công tắc DIP. Dòng Step lý tưởng sử dụng driver DM524T là dòng động cơ
Nema 17 và 23 còn đối với driver DM320T là dòng động cơ Nema dưới 17.
D

Hình 3.3: Driver điều khiển động cơ

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 30
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

Trong robot driver DM542T được sử dụng cho các khâu 1,2,3,5,6. Driver DM320T
được sử dụng cho khâu 4.
 Các thông số kĩ thuật của Driver DM542T như sau:
Bảng 3.2: Bảng thông số kĩ thuật của Driver DM542T

DM542T
Parameters Min Typical Max
Unit

Output Current 1.0 - 4.2 (3.0 A

RMS)

Supply Voltage 20 24-48 50 VDC

Logic signal 7 10 16 mA
current

C C
Pulse input
frequency 0 -
. L R 200 kHz

Minimal pulse 2.5

U T - -
width

Minimal direction
D μS

setup 5.0 - - μS

Isolation resistance 500 MΩ

- Chức năng các cổng kết nối của driver DM520T:

Hình 3.4: Các cổng kết nối của driver

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 31
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

- DM542T có hai khối kết nối dành cho kết nối tín hiệu điều khiển và dành
cho kết nối nguồn và động cơ. Các bảng sau đây là mô tả ngắn gọn về hai kết
nối.
Bảng 3.3: Cổng kêt nối tín hiệu

Chân kết
nối Chi tiết
PUL+ Tín hiệu xung: Xung hoạt động ở sườn tăng; 4-5V khi PUL-ở mức
cao, 0-0,5V khi PUL-ở mức thấp. Độ rộng xung tối thiểu 2,5μs.
Thêm một điện trở để giới hạn dòng điện ở mức logic đầu vào +
12V hoặc + 24V điện áp (1K cho + 12V, 2k cho + 24V). Giống như
PUL- tín hiệu DIR và ENA
Tín hiệu TRỰC TIẾP: Tín hiệu này có các mức điện áp thấp / cao để
thể hiện hai hướng quay của động cơ .. Thời gian thiết lập hướng tối
DIR+
C
thiểu là 5μs. Đồng thời hoán đổi kết nối của hai dây của một cuộn

C
dây (ví dụ A + và A-) với driver sẽ đảo ngược hướng động cơ

DIR-
. L R
U T
Bật tín hiệu: Tín hiệu này được sử dụng để bật / tắt driver. Mức cao
+ 5V (tín hiệu điều khiển NPN) để cho phép driver và mức thấp để
ENA+
D
vô hiệu hóa driver. PNP và tín hiệu điều khiển vi sai thì ngược lại,
cụ thể là mức Thấp để cho phép. Theo mặc định nó được để lại
ENA- UNCONNECTED (ENABLED).

Bảng 3.4: Cổng kết nối nguồn và động cơ

Chân kết
nối Chi tiết
GND Kết nối chân ground của nguồn

+V Cung cấp điện kết nối tích cực. Đề nghị điện áp nguồn 24-48VDC
Kết nối động cơ pha A. Kết nối động cơ dây A + với A + Pin;
A+, A-
động cơ A- dây đến A- pin
Kết nối động cơ pha B. Kết nối động cơ dây B + với B + Pin;
B+, B-
động cơ B- dây đến B- pin

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 32
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

- Đê thực hiện kết nối Driver với vi điều khiển ta có 2 kiểu nối driver Driver
DM524 chấp nhận 2 kiểu kết nối là kết nối mở và kết nối PNP như hình dưới

Hình 3.5: Các kiểu kết nối trên Driver

- Cấu hình công tắc DIP
 Driver sử dụng công tắc DIP 8 bit để đặt độ phân giải microstep và dòng điện
hoạt động của động cơ, như hình dưới đây :

C C
. L R
U T
D

Hình 3.6: Các công tắc cài đặt thông số cho Driver

 Để thực hiện thiết lập thông số ban đầu cho mỗi driver ta chỉ việc thực hiện bật
tắt các công tắc tương ứng như bảng dưới đây để thiết lập các giá trị phù hợp với
nhu cầu sử dụng.

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 33
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

 Đối với các công tắc 5,6,7,8 ta có thể bật tắt để thiết lập chế độ chia bước phù
hợp cho mỗi động cơ, phụ thuộc vào độ chính xác muốn đạt được mà tiến hành
chia cho hợp lý.
Bảng 3.5: Bảng thiết lập thông số các công tắc 5,6,7,8

Bước/vòng quay
Microstep SW5 SW6 SW7 SW8
(cho đông cơ bước 1.8o )

2 400 OFF ON ON ON

4 800 ON OFF ON ON

8 1600 OFF OFF ON ON

16 3200 ON ON OFF ON

32 6400 OFF ON OFF ON

64 12800 ON
C C OFF OFF ON

128 25600

. L R OFF OFF OFF ON

10
1000

2000
U T ON

OFF
ON

ON
ON

ON
OFF

OFF

20

25
4000

5000
D ON

OFF
OFF

OFF
ON

ON
OFF

OFF

40 8000 ON ON OFF OFF

50 10000 OFF ON OFF OFF

100 20000 ON OFF OFF OFF

125 25000 OFF OFF OFF OFF

 Đối với các công tăc 1,2,3 ta bật tắt để hiệu chỉnh dòng cấp cho động cơ bước
sao cho phù hợp với momen xoắn muốn đạt được ở các khâu nhưng vẫn phải phụ
thuộc vào bảng được thống kê bên dưới đây:
 Công tắc 4 được dùng cho mục đích cài đặt dòng sử dụng bằng một nữa so với
bảng chọn nếu để OFF, và bằng dòng trong bảng nếu để ON.

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 34
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

Bảng 3.6: Bảng thiết lập thông số ở các công tắc 1,2,3

Dòng đỉnh Dòng RMS SW1 SW2 SW3

1.00A 0.71A ON ON ON
1.46A 1.04A OFF ON ON
1.91A 1.36A ON OFF ON
2.37A 1.69A OFF OFF ON
2.84A 2.03A ON ON OFF

3.31A 2.36A OFF ON OFF

3.76A 2.69A ON OFF OFF
4.20A 3.00A OFF OFF OFF

Biểu đồ trình tự các tính hiệu điều khiển

C C
R
-

quy tắc như sơ đồ sau

T . L
 Để tránh một số thao tác lỗi và sai lệch, PUL, DIR và ENA cần tuân thủ một số

D U

Hình 3.7: Biểu đồ trình tự xung điều khiển

- Lưu ý
 ENA phải đi trước ít nhất 5μs.
 DIR phải đi trước cạnh hiệu quả PUL 5μs để đảm bảo hướng chính xác.
 Độ rộng xung không nhỏ hơn 2,5μs;
 Chiều rộng mức thấp không nhỏ hơn 2,5μs.

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 35
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

 Tương tự ta có các thông số kĩ thuật của Driver DM320T như sau:

Bảng 3.7: Bảng thông số kĩ thuật của Driver DM320T

DM320T
Parameters
Min Typical Max Unit
2.2
Output Current 0.3 - A
(1.6 RMS)
Supply Voltage 10 24 30 VDC

Logic signal current 7 10 16 mA

Pulse input frequency 0 - 60 kHz

Minimal pulse width 7.5 - - μS

Minimal direction setup

Isolation resistance
7.5

100
-

C C - μS

MΩ

-
. L R
Cổng kết nối của driver DM320T cũng tương tự với các cổng kết nối của

-
DM542T đã nêu ở trên.
Cấu hình công tắc DIP
U T
D
 Driver sử dụng công tắc DIP 6 bit để đặt độ phân giải microstep và dòng điện
hoạt động của động cơ, như hình dưới đây :

Hình 3.8: Các công tắc cài đặt thông số cho DM320T

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 36
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

 Cấu hình độ phân giải Microstep

Độ phân giải microstep được đặt bởi các công tắc DIP SW5,6,7,8 như trong
bảng sau:
Bảng 3.8: Bảng thiết lập thông số các công tắc 5,6,7

Bước/vòng quay
Microstep SW5 SW6 SW7
(đối với động cơ 1,8 độ)

2 400 ON ON ON
4 800 OFF ON ON
8 1600 ON OFF ON
16 3200 OFF OFF ON
32 6400 ON ON OFF
64 12800 OFF ON OFF
20
40
4000
8000
C CON
OFF
OFF
OFF
OFF
OFF

 Cấu hình bộ chia dòng điện

. L R
U T
Ba bit đầu tiên (SW1, 2, 3) của công tắc DIP được sử dụng để đặt dòng cho động cơ.
Chọn một cài đặt gần nhất với động cơ hiện tại được sử dụng. Bộ chia được cho trong
bảng sau:
D
Bảng 3.9: Bảng thiết lập thông số ở các công tắc 1,2,3

Peak Current RMS Current SW1 SW2 SW3

0.3A 0.21A ON ON ON

0.5A 0.35A OFF ON ON

0.7A 0.49A ON OFF ON

1.0A 0.71A OFF OFF ON

1.3A 0.92A ON ON OFF

1.6A 1.13A OFF ON OFF

1.9A 1.34A ON OFF OFF
2.2A 1.56A OFF OFF OFF

 Để điều khiển được thực hiện chính xác ta cần điều khiển theo đúng các quy tắc
được nêu dưới hình sau:

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 37
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

 Lưu ý
Hình 3.9: Biểu đồ điều khiển Driver DM320T

C C
- 1: ENA phải đi trước ít nhất 5μs.
. L R
- Độ rộng xung không nhỏ hơn 7,5μs.
U T
- DIR phải đi trước cạnh hiệu quả PUL 5μs để đảm bảo hướng chính xác;

3.1.3. Động cơ bước

D
Động cơ bước được lựa chọn là loại động cơ bước sử dụng hộp giảm tốc giúp tăng
momen xoắn của trục ra động

Hình 3.10: Động cơ bước

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 38
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

Động cơ bước được điều khiển dưới dạng các xung điện rời rạc kế tiếp nhau tạo thành
các chuyển động góc quay hoặc các chuyển động của rotor. Động cơ bước không quay
theo cơ chế thông thường, chúng quay theo từng bước bên có chế độ chính xác rất cao
về mặt điều khiển học.
3.2. Tính toán thuật toán điều khiển
Phương trình tính toán động học của robot là một bài toán tuyệt đối cần thiết trong
việc tìm mối quan hệ giữa góc quay và vị trí cuối của cánh tay robot, với việc tìm ra mỗi
quan hệ này sẽ giúp việc điều khiển trở nên linh hoạt và dê dàng hơn hết.
3.2.1. Động học thuận robot
Phương trình động học thuận là phương trình chỉ mỗi quan hệ giữa góc quay từng
khâu và vị trí chấp hành cuối, hay nói cách khác từ giá trị góc quay ta có thể dễ dàng
xác định hướng và vị trí cách tay duỗi tới.
Đê tiến hành giải động học thuận ta thực hiện gắn hệ tọa độ phụ hợp cho từng khâu,

phương trình động học của robot.

C C
lập bảng thông số DH (Denavit Hartenberg), sau đó xác định các ma trận An để thiết lập

. L R
Giải phương trình động học thuận cho mô hình được thực hiện bên dưới:

U T
D

Hình 3.11: Hệ tọa độ trên mô hình

Sau khi gắn hệ trục tọa độ ta lập được bảng thông số DH như sau:

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 39
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

Bảng 3.10: Bảng thông số D-H

Khâu 𝐚𝐢 𝜶𝐢 𝐝𝐢 𝛉𝐢

1 𝑎1 -90 𝑑1 𝜃1 ∗

2 𝑎2 0 0 𝜃2 ∗

3 0 90 0 𝜃3 ∗

4 0 -90 𝑑4 𝜃4 ∗

5 0 90 0 𝜃5 ∗

6 0 0 𝑑6 𝜃6 ∗

C C
Từ công thức tổng quát cho các ma trận Ai là :
. L R
Ai = [
cos 𝜃
𝑠𝑖𝑛𝜃
−𝑠𝑖𝑛𝜃. 𝑐𝑜𝑠𝛼
𝑐𝑜𝑠𝜃. 𝑐𝑜𝑠𝛼
𝑠𝑖𝑛𝜃. 𝑠𝑖𝑛𝛼

U
−𝑐𝑜𝑠𝜃. 𝑠𝑖𝑛𝛼 T 𝑎. 𝑐𝑜𝑠𝜃
𝑎. 𝑠𝑖𝑛𝜃
] ( 3.1 )
0
0
𝑠𝑖𝑛𝛼
0
D𝑐𝑜𝑠𝛼
0
Với quy ước viêt tắt: C1 = Cos(θ1); S1 = Sin(θ1);…
𝑑
1

Ta có các ma trận 𝑨𝒊 ( 𝒊 = 𝟏 ÷ 𝟔)
C1 0 − S1 a1 C1
S1 0 C1 a1 S1
A1 = [ ]
0 −1 0 d1
0 0 0 1
C2 − S2 0 a2 . C2
S2 C2 0 a2 . S2
A2 = [ ]
0 0 1 0
0 0 0 1
C3 0 S3 0
S3 0 − C3 0
A3 = [ ]
0 1 0 0
0 0 0 1
𝐶4 0 −𝑆4 0
𝑆4 0 𝐶4 0
A4 = [ ]
0 −1 0 d4
0 0 0 1
Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 40
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

C5 0 S5 0
S5 0 − C5 0
𝐴5 = [ ]
0 1 0 0
0 0 0 1
C6 − S6 0 0
S6 C6 0 0
A6 = [ ]
0 0 1 d6
0 0 0 1

Ma trận T0-6 mô tả hướng và vị trí của khâu chấp hành cuối đối với gốc tọa độ được tính
như sau
𝑻0−6 = 𝐴1 . 𝐴2 . 𝐴3 . 𝐴4 . 𝐴5 . 𝐴6 có dạng:
𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑝𝑥
𝑛 𝑜𝑦 𝑎𝑦 𝑝𝑦
𝑇0−6 =[ 𝑦 ] ( 3.2 )
𝑛𝑧 𝑜𝑧 𝑎𝑧 𝑝𝑧
0 0 0 1
Ta có phương trình động học của robot:
C C
L R
𝒏𝒙 = -S6.(C4.S1– S4.(C1.S2.S3 – C1.C2.C3)) –C6.(C5.(S1.S4 + C4.(C1.S2.S3 –

.
C1.C2.C3)) + S5.(C1.C2.S3 + C1.C3.S2))

U T
𝒏𝒚 = S6.(C1.C4 + S4(S1.S2.S3 – C2.C3.S1)) + C6(C5.(C1.S4 – C4(S1.S2.S3 –
C2.C3.S1)) – S5(C2.S1.S3 + C3.S1.S2))
𝒏𝒛
D
= S23.S4.S6 – C6(C23.S5 + S23.C4.C5)
𝒐𝒙 = S6.(C5.(S1.S4 + C4.(C1.S2.S3 – C1.C2.C3)) + S5(C1.C2.S3 + C1.C3.S2)) –
C6(C4.S1 – S4(C1.S2.S3 – C1.C2.C3))
𝒐𝒚 = C6.(C1.C4 + S4.(S1.S2.S3 – C2..C3.S1)) – S6.(C5.(C1.S4 – C4.(S1.S2.S3 –
C2.C3.S1)) – S5.(C2.S1.S3 + C3.S1.S2))
𝒐𝒛 = S6.(C23.S5 + S23.C4.C5) + S23.C6.S4
𝒂𝒙 = C5(C1.C2.S3 + C1.C3.S2) – S5.(S1.S4 + C4.(C1.S2.S3 – C1.C2.C3))
𝒂𝒚 = S5.(C1.S4 – C4.(S1.S2.S3 – C2.C3.S1)) + C5.(C2.S1.S3 + C3.S1.S2)
𝒂𝒛 = C23.C5 – S23.C4.S5
𝒑𝒙 = 𝑎1 C1 +𝑑4 S23.C1 + 𝑎2 .C1.C2 + 𝑑6 .S23.C1.C5 - 𝑑6 .S1.S4.S5 + 𝑑6 .C1.C2.C3.C4.S5
- 𝑑6 .C1.C4.S2.S3.S5
𝒑𝒚 = 𝑑4 S23.S1 -𝑎1 + 𝑎2 C2.S1 + 𝑑6 .S23.C5.S1 + 𝑑6 .C1.S4.S5 + 𝑑6 .C2.C3.C4.S1.S5 -
𝑑6 .C4S1.S2.S3.S5
𝒑𝒛 = 𝑑1 + 𝑑4 .C23 - 𝑎2 S2 - (𝑑6 .S23.S45)/2 + 𝑑6 .C23.C5 + (𝑑6 .S.(t4 - t5).S23)/2
Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 41
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

3.2.2. Động học ngược robot

Phương trình động học ngược là bài toán tìm mỗi quan hệ giữa vị trí cuối và gọc quay
của các khớp của robot. Bài toán cho phép người điều khiển chỉ cần nhập hướng và vị
trí mong muốn, phương trình sẽ tính toán để đưa ra các góc điều khiển robot đến vị trí
yêu cầu.
- Tính góc khớp 1

C C
. L R
U T
D
Hình 3.12: Hình chiếu tính góc khớp 1
Ta có ma trận mô ta hướng và vị trí khâu chấp hành cuối có dạng là:
𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑝𝑥
T60 𝑛 𝑜𝑦 𝑎𝑦 𝑝𝑦
=[ 𝑦 ] ( 3.3 )
𝑛𝑧 𝑜𝑧 𝑎𝑧 𝑝𝑧
0 0 0 1
Với 3 vector mô tả hướng của khâu chấp hành cuối là các vector n, o, a.

Gọi vector 06 N k 0 là vector hướng của khâu chấp hành cuối theo trục z
𝑎𝑥
 Nk 0
0
6 = a = [ 𝑎𝑦 ]
𝑎𝑧

Gọi 64 Pk 0 là vector từ khâu 4 đến khâu 6 vậy ta có:

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 42
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

d6 ∗ 𝑎𝑥
4
6
0
Pk 0 = N k 0
6 x d6 = [ d6 ∗ 𝑎𝑦 ]
d6 ∗ 𝑎𝑧

Tương tự ta gọi 06 Pk 0 và 04 Pk 0 là vector từ khâu gốc đến khâu cuối và từ khâu gốc đến
khâu thứ 4.
P𝑥
Với Pk 0 = [ 𝑃𝑦 ]
0
6
𝑃𝑧
Như hình vẽ ta thấy :
P𝑥 − d6 ∗ 𝑎𝑥
0
4 Pk 0 = Pk 0 - Pk 0 = [ 𝑃𝑦 − d6 ∗ 𝑎𝑦 ]
0
6
4
6
𝑃𝑧 − d6 ∗ 𝑎𝑧
Vậy từ hình 1 ta dễ dàng tính được:
 Góc θ1 = atan2(𝑃𝑦 − d6 ∗ 𝑎𝑦 , P𝑥 − d6 ∗ 𝑎𝑥 ).

C C
- Tính góc θ3

. L R
U T d4

a2 D

Hình 3.13: Hình chiếu tính góc thứ 3

Tương tự gọi các vector 10 Pk 0 và 14 Pk 0 là các vector từ gốc đến khâu 1 và từ khâu 1 đến
khâu 4.

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 43
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

P𝑥 − d6 ∗ 𝑎𝑥
Với 0
4 Pk 0 = [ 𝑃𝑦 − d6 ∗ 𝑎𝑦 ] đã biết.
𝑃𝑧 − d6 ∗ 𝑎𝑧
𝑎1 ∗ cos(𝜃1)
Ta có 0
1 Pk 0 = [ 𝑎1 ∗ sin(𝜃1)]
𝑑1
P𝑥 − d6 ∗ 𝑎𝑥 − 𝑎1 ∗ cos(𝜃1)
Từ đó ta tính được vector Pk 0 = Pk 0 - Pk 0 = [ 𝑃𝑦 − d6 ∗ 𝑎𝑦 − 𝑎1 ∗ sin(𝜃1)]
1
4
0
4
0
1
𝑃𝑧 − d6 ∗ 𝑎𝑧 − 𝑑1

Gọi độ dài vector 14 Pk 0 là P14k0 ta có:

| 14 Pk 0 | = P14k0 =
2
√( P𝑥 − d6 ∗ 𝑎𝑥 − 𝑎1 ∗ cos(𝜃1))2 + (𝑃𝑦 − d6 ∗ 𝑎𝑦 − 𝑎1 ∗ sin(𝜃1)) + (𝑃 − d6 ∗ 𝑎𝑧 − 𝑑1 )2
𝑧

Qua đó ta xác định được góc Ø:

C C
Cos(Ø) =
a  d  P14
2 2 2

. L
 Ø = arccos(Ra  d 2  P142
2

Vậy góc θ3 =
2ad
𝜋
–Ø
U T 2ad

- Tính góc θ2
2
D

Hình 3.14: Hình chiếu tính góc 2

Ta có vector 14 Pk 0 đã tính được ở trên. Vậy ta cần tính 14 Pk1

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 44
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

𝑐𝑜𝑠(𝜃1) 0 𝑠𝑖𝑛(𝜃1) 𝑎1 ∗ 𝑐𝑜𝑠(𝜃1)

𝑠𝑖𝑛(𝜃1) 0 −𝑐𝑜𝑠(𝜃1) 𝑎1 ∗ 𝑠𝑖𝑛(𝜃1)
Tacó: 04 T = A1=[ ]
0 1 0 𝑑1
0 0 0 1
𝑐𝑜𝑠(𝜃1) 0 𝑠𝑖𝑛(𝜃1)
Vậy 10 R = [ 𝑠𝑖𝑛(𝜃1) 0 −𝑐𝑜𝑠(𝜃1)]
0 1 0
Suy ra: 14 Pk1 = 10 R  14 Pk 0
Vậy ta tính được góc:

β 1 = atan2( 14 Pk 0 y , 14 Pk 0 x ).
2
a2 2  d 4 2  14 Pk1
β 2 = arccos( ).
2a2 d 4
𝜋
Vậy góc θ2 = β 1 + β 2 -

- Tính góc khớp thứ θ4,θ5 và θ6

2

C C
. L R
Để tìm 3 góc quay tiếp trên ta sử dụng ma trận quay 64 R . Ta có: 64 R = 40 R 1 x 60 R

Ngoài ra : 64 R = Rotx(θ4) x Roty(θ5) x Rotz(θ6)

U T
−𝐶4𝐶5𝐶6 − 𝑆4𝑆6
= [−𝑆4𝐶5𝐶6 + 𝐶4𝑆6
𝑆5𝐶6
𝐶4𝐶5𝐶6 − 𝑆4𝐶6
𝑆4𝐶5𝑆6 + 𝐶4𝐶6
−𝑆5𝑆6
D −𝐶4𝑆5
−𝑆4𝑆5 ]
−𝐶5
Tử đó dễ dàng ta tính được:
θ4 = atan2(-S4S5, -C4S5);
θ5 = atan2(-C5, √1 − (−𝐶5)2 );
θ6 = Atan2(-(-S5S6), S5S6);

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 45
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

Chương 4: KẾT LUẬN

4.1. Kết quả

Hệ điều khiển đáp ứng được yêu cầu bài toán về điều khiển vị trí, chạy ổn định được
theo hành trình được thiết lập, thực hiện được tất cả các chức năng đã thiết lập.
Sau nhiều tháng làm việc và thiết kế, mặc dù còn nhiều thiếu sót, nhưng bước đầu
nhóm cũng đã hoàn thành về mặt nội dung toàn bộ đề tài theo như yêu cầu ban đầu đặt
ra.

C C
. L R
U T
D

Hình 4.1: Robot 6 bậc tự do

Các thông số kĩ thuật chính:
Bảng 4.1:Bảng thông số cơ bản mô hình

Số thứ tự Đặc tính Kí hiệu Thông số

1 Khối lượng robot mr 12 kg

2 Tầm với tối đa H 62.9 cm
3 Số trục quay Ax 6

4 Khối lượng tủ điện mt 25 kg

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 46
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

4.2. Đánh giá

4.2.1. Phần cơ khí
Robot chuyển động tốt, điều khiển mượt mà, ít gặp sự cố.
Kiểu dáng chắc chắn song bên cạnh đó vẫn còn nhiểu điểm cần khắc phục như sự liên
kết giữa các khâu khớp, tính bền vững để hoạt động lâu dài, độ sai lệch trong hoạt động.
4.2.2. Phần điện tử
Các thiết bị mạch điện được thiết kế chắc chắn, có hộp chống va đập và cách điện để
tránh chập cháy tốt.
Các thành phần bảo vệ nguồn và driver cho động cơ đã được lắp đặt.
4.2.3. Phần điều khiển
Đã điều khiển được cánh tay robot hoạt động ổn định, xử lý tốt phần dạy học. Bên
cạnh đó còn thiếu sót ở mặt xử lý ảnh, nhóm chưa xây dựng và thể hiện được phần xử
lý ảnh của robot.
4.3. Kết luận
C C
. L R
Sau nhiều tháng liên tục tìm tòi và phát triển nhóm tác giả đã hoàn thành đúng tiến
độ đặt ra, đáp ứng được các yêu cầu trong nhiệm vụ thiết kế đồ án. Cùng với việc áp

U T
dụng các nghiên cứu về mặt lí thuyết kết hợp với thiết kế thực tiễn nhóm đã có thêm
nhiều kiến thức hơn về điều khiển tự động, robot, cũng như về các kết cấu cơ khí mới.

D
Về mặt lí thuyết tìm hiểu được quy trình thiết kế một Robot bắt đầu từ việc

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 47
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

TÀI LIỆU THAM KHẢO

[1] Robot công nghiệp, Phạm Đăng Phước, Nhà xuất bản Xây Dựng, 2007
[2] Nhập Môn Robot Công Nghiệp Tập 1 - Ts. Lê Hoài Quốc
[3] Cơ Sở Nghiên Cứu _ Sáng Tạo Robot - Trần Thế San
[4] Modelling and Simulation of KUKA KR6 Robot Manipulator, NASR ABDO
ALI MOHAMMED, 2017.
[5] https://github.com/Chris-Annin/AR2 (truy cập lần cuối 28/11/2019)
[6] https://www.anninrobotics.com/(truy cập lần cuối 28/11/2019)

C C
. L R
U T
D

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 48
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

PHỤ LỤC 1: CHƯƠNG TRÌNH ARDUINO

#include <Servo.h>
Servo servo0;
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;
Servo servo7;

C
String inData;
String function;
char WayPt[101][50];
R C
int WayPtDel;

T . L
const int J1stepPin = 2;
const int J1dirPin = 3; D U
const int J2stepPin = 4;
const int J2dirPin = 5;
const int J3stepPin = 6;
const int J3dirPin = 7;
const int J4stepPin = 8;
const int J4dirPin = 9;
const int J5stepPin = 10;
const int J5dirPin = 11;
const int J6stepPin = 12;
const int J6dirPin = 13;
const int TRstepPin = 20;
const int TRdirPin = 21;
const int J1calPin = 14;
const int J2calPin = 15;

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 49
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

const int J3calPin = 16;

const int J4calPin = 17;
const int J5calPin = 18;
const int J6calPin = 19;
///////khâu J1 /////////
if (J1dir == 1 && J1rotdir == 1)
{
digitalWrite(J1dirPin, LOW);
}
else if (J1dir == 1 && J1rotdir == 0)
{
digitalWrite(J1dirPin, HIGH);
}
else if (J1dir == 0 && J1rotdir == 1)

C
{

C
digitalWrite(J1dirPin, HIGH);

R
}
else if (J1dir == 0 && J1rotdir == 0)
{

T . L
U
digitalWrite(J1dirPin, LOW);

D
}
/////// khâu J2 /////////
if (J2dir == 1 && J2rotdir == 1)
{
digitalWrite(J2dirPin, LOW);
}
else if (J2dir == 1 && J2rotdir == 0)
{
digitalWrite(J2dirPin, HIGH);
}
else if (J2dir == 0 && J2rotdir == 1)
{
digitalWrite(J2dirPin, HIGH);
}
else if (J2dir == 0 && J2rotdir == 0)
{
digitalWrite(J2dirPin, LOW);
}
/////// khâu J3 /////////
if (J3dir == 1 && J3rotdir == 1)

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 50
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

{
digitalWrite(J3dirPin, LOW);
}
else if (J3dir == 1 && J3rotdir == 0)
{
digitalWrite(J3dirPin, HIGH);
}
else if (J3dir == 0 && J3rotdir == 1)
{
digitalWrite(J3dirPin, HIGH);
}
else if (J3dir == 0 && J3rotdir == 0)
{
digitalWrite(J3dirPin, LOW);
}

C
///////khâu J4 /////////

C
if (J4dir == 1 && J4rotdir == 1)

R
{

}
digitalWrite(J4dirPin, LOW);

T . L
U
else if (J4dir == 1 && J4rotdir == 0)

D
{
digitalWrite(J4dirPin, HIGH);
}
else if (J4dir == 0 && J4rotdir == 1)
{
digitalWrite(J4dirPin, HIGH);
}
else if (J4dir == 0 && J4rotdir == 0)
{
digitalWrite(J4dirPin, LOW);
}
///////khâu J5 /////////
if (J5dir == 1 && J5rotdir == 1)
{
digitalWrite(J5dirPin, LOW);
}
else if (J5dir == 1 && J5rotdir == 0)
{
digitalWrite(J5dirPin, HIGH);
}

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 51
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

else if (J5dir == 0 && J5rotdir == 1)

{
digitalWrite(J5dirPin, HIGH);
}
else if (J5dir == 0 && J5rotdir == 0)
{
digitalWrite(J5dirPin, LOW);
}
///////khâu J6 /////////
if (J6dir == 1 && J6rotdir == 1)
{
digitalWrite(J6dirPin, LOW);
}
else if (J6dir == 1 && J6rotdir == 0)
{

C
digitalWrite(J6dirPin, HIGH);

C
}

R
else if (J6dir == 0 && J6rotdir == 1)
{
digitalWrite(J6dirPin, HIGH);

T . L
U
}

D
else if (J6dir == 0 && J6rotdir == 0)
{
digitalWrite(J6dirPin, LOW);
}
void loop() {
//test led
if (digitalRead(J1calPin) == HIGH || digitalRead(J2calPin) == HIGH || digitalRead(J3calPin)
== HIGH || digitalRead(J4calPin) == HIGH || digitalRead(J5calPin) == HIGH ||
digitalRead(J6calPin) == HIGH)
{
digitalWrite(J6dirPin, HIGH);
}
else
{
digitalWrite(J6dirPin, LOW);
}

//start loop
WayPtDel = 0;
while (Serial.available() > 0 or WayPtDel == 1)
{
Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 52
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

char recieved = Serial.read();

inData += recieved;
// Process message when new line character is recieved
if (recieved == '\n')
{
String function = inData.substring(0, 2);
while (digitalRead(J1calPin) == LOW && J1done < J1step || digitalRead(J2calPin) == LOW
&& J2done < J2step || digitalRead(J3calPin) == LOW && J3done < J3step ||
digitalRead(J4calPin) == LOW && J4done < J4step || digitalRead(J5calPin) == LOW &&
J5done < J5step || digitalRead(J6calPin) == LOW && J6done < J6step)
{
if (J1done < J1step && (digitalRead(J1calPin) == LOW))
{
digitalWrite(J1stepPin, LOW);
}
delayMicroseconds(5);
if (J1done < J1step && (digitalRead(J1calPin) == LOW))
{
C C
digitalWrite(J1stepPin, HIGH);
J1done = ++J1done;
. L R
}
delayMicroseconds(5);
U T
{
digitalWrite(J2stepPin, LOW);
D
if (J2done < J2step && (digitalRead(J2calPin) == LOW))

}
delayMicroseconds(5);
if (J2done < J2step && (digitalRead(J2calPin) == LOW))
{
digitalWrite(J2stepPin, HIGH);
J2done = ++J2done;
}
delayMicroseconds(5);
if (J3done < J3step && (digitalRead(J3calPin) == LOW))
{
digitalWrite(J3stepPin, LOW);
}
delayMicroseconds(5);
if (J3done < J3step && (digitalRead(J3calPin) == LOW))
{
digitalWrite(J3stepPin, HIGH);
J3done = ++J3done;
Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 53
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

}
delayMicroseconds(5);
if (J4done < J4step && (digitalRead(J4calPin) == LOW))
{
digitalWrite(J4stepPin, LOW);
}
delayMicroseconds(5);
if (J4done < J4step && (digitalRead(J4calPin) == LOW))
{
digitalWrite(J4stepPin, HIGH);
J4done = ++J4done;
}
delayMicroseconds(5);
if (J5done < J5step && (digitalRead(J5calPin) == LOW))
{

C
digitalWrite(J5stepPin, LOW);

C
}

R
delayMicroseconds(5);

. L
if (J5done < J5step && (digitalRead(J5calPin) == LOW))
{

T
U
digitalWrite(J5stepPin, HIGH);

D
J5done = ++J5done;;
}
delayMicroseconds(5);
if (J6done < J6step && (digitalRead(J6calPin) == LOW))
{
digitalWrite(J6stepPin, LOW);
}
delayMicroseconds(5);
if (J6done < J6step && (digitalRead(J6calPin) == LOW))
{
digitalWrite(J6stepPin, HIGH);
J6done = ++J6done;
}

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 54
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

PHỤ LỤC 2: CHƯƠNG TRÌNH PYTHON

savePosData()
CalcFwdKin()
else:
almStatusLab.config(text="J1 AXIS LIMIT", bg = "red")
almStatusLab2.config(text="J1 AXIS LIMIT", bg = "red")
DisplaySteps()

def J2jogNeg():
global JogStepsStat
global J2StepCur
global J2AngCur
C C
global xboxUse
if xboxUse != 1:
. L R
U T
almStatusLab.config(text="SYSTEM READY", bg = "cornflowerblue")

D
almStatusLab2.config(text="SYSTEM READY", bg = "cornflowerblue")
Speed = speedEntryField.get()
ACCdur = ACCdurField.get()
ACCspd = ACCspeedField.get()
DECdur = DECdurField.get()
DECspd = DECspeedField.get()
J2Degs = float(J2jogDegsEntryField.get())
if JogStepsStat.get() == 0:
J2jogSteps = int(J2Degs/J2DegPerStep)
else:
#switch from degs to steps
J2jogSteps = J2Degs
J2Degs = J2Degs*J2DegPerStep
if (J2Degs <= -(J2NegAngLim - J2AngCur)):
command =
"MJB"+J2motdir+str(J2jogSteps)+"S"+Speed+"G"+ACCdur+"H"+ACCspd+"I"+DECdur+"
K"+DECspd+"\n"
Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 55
Trần Văn Đổ
 Thiết kế chế tạo cánh tay robot 6 bậc tự do

ser.write(command.encode())
ser.flushInput()
time.sleep(.2)
ser.read()
J2StepCur = J2StepCur - int(J2jogSteps)
J2AngCur = round(J2NegAngLim + (J2StepCur * J2DegPerStep),2)
J2curAngEntryField.delete(0, 'end')
J2curAngEntryField.insert(0,str(J2AngCur))
/////////////*/////////////*/////////////
def Servo1on():
savePosData()
servoPos = servo1onEntryField.get()

C
command = "SV1P"+servoPos+"\n"
ser.write(command.encode())
ser.flushInput()
R C
time.sleep(.2)

T . L
U
ser.read()

def Servo1off():
D
savePosData()
servoPos = servo1offEntryField.get()
command = "SV1P"+servoPos+"\n"
ser.write(command.encode())
ser.flushInput()
time.sleep(.2)
ser.read()

Sinh viên thực hiện: Nguyễn Đình Danh Hướng dẫn: TS. Lê Hoài Nam 56
Trần Văn Đổ