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

ĐẠ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:
Sinh viên thực hiện:

TS. LÊ HỒI NAM
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

R
L
.
T

- Nghiên cứu hệ thống, thiết kế robot 6 bậc

- Điều khiển, thiết kế phần mềm và phần cứng cho robot 6 bậc

U
D

- Xây dựng mơ hình điều khiển robot
4. Nội dung đề tài đã thực hiện
- Mơ hình: 1
- 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
TRƯỜNG ĐẠI HỌC BÁCH KHOA
KHOA CƠ KHÍ

CỘNG HỊA XÃ HƠI CHỦ NGHĨA
VIỆT NAM

Độ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
TRẦN VĂN ĐỔ
Lớp: 15CDT1

MSSV: 101150159
MSSV: 101150160

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 vật liệu: nhôm nguyên khối, nhựa, nhôm định hình.

C
C

R
L
.
T

4. Nội dung các phần thuyết minh và tính tốn:
Chương 1: Tổng quan về đề tài.

U
D

Chương 2: Thiết kế cơ khí
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” .

R
L
.
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

U
D

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ó
liên quan, mà em có thể hồn thành xong đề tài.

Đề tài đã hồn thành xong, nhưng khơng thể tránh nhiều thiếu sót mong q 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 q 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ừ q 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.

R
L
.
T

U
D

Sinh viên thực hiện

Nguyễn Đình Danh

ii

Trần Văn Đổ

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
C

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

R

L
.
T

1.1.1. Lịch sử hình thành và phát triển ...........................................................................2
1.1.2. Xu hướng phát triển ..............................................................................................2
1.1.3. Cấu trúc cơ bản của robot công nghiệp ................................................................3
1.2.

U
D

Ứ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. 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 tố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

R

L
.
T

2.3.5. Khâu 5 và khâu 6 ................................................................................................23
2.3.6. Mơ hình thiết kế tổng thể ....................................................................................23
2.4.

U
D

Chọn tay kẹp .......................................................................................................24

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 tốn thuật tố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

R
L
.
T

U
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
C

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

R
L
.
T

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

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 hồ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: Khn dưới ngón tay mềm ..........................................................................26
Hình 2.22: Khn 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

R
L
.
T

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

U
D

Bảng 2.1: Bảng thông số khâu 1,2 .................................................................................12
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 tố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
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
hiểu để nghiên cứu.

C
C

R
L

.
T

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

 Trình bày giới thiệu chung về cánh tay robot

U
D

 Phương pháp thiết kế cơ khí, điện tử
 Tính tố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
Trần Văn Đổ

Hướng dẫn: TS. Lê Hoài Nam

1


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

R
L
.
T

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),…
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

U
D

(American Machine and Foundry Company) của Mỹ. Đến năm 1990 có hơn 40 cơng ty
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
Trần Văn Đổ

Hướng dẫn: TS. Lê Hồi Nam

2


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.

C
C

1.2. Ứng dụng của cánh tay robot
1.2.1.

Ứng dụng trong nông nghiệp

R
L
.
T

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
đồ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

U
D

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
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
Trần Văn Đổ

Hướng dẫn: TS. Lê Hoài Nam

3


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 tồ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

R
L
.
T

U
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
Trần Văn Đổ

Hướng dẫn: TS. Lê Hoài Nam

4


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

C
C

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

R
L
.
T

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ế hồn hảo cho con người. Rõ ràng, nhờ chúng,
chúng ta giảm thiểu được số vụ tai nạn lao động đáng kể.

U
D

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

Đặt vấn đề

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 hồ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
khiển

Các khâu của
robot

Khâu chấp
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
Trần Văn Đổ

Hướng dẫn: TS. Lê Hoài Nam

5


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

R
L

.
T

U
D

Sinh viên thực hiện: Nguyễn Đình Danh
Trần Văn Đổ

Hướng dẫn: TS. Lê Hoài Nam

6


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
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ó
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ơ
đ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
nghiệp nhưng chủ yếu với các robot có sức nâng dưới 40kg.

C
C

R
L
.
T

U
D

Với những yêu cầu đó, ta chọn sức nâng tay máy là 1kg.
2.1.3.

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 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
Trần Văn Đổ

Hướng dẫn: TS. Lê Hoài Nam

7


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 vng 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

2.1.4.

C
C

Độ chính xác vị trí

R
L
.
T

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

U
D

và khả năng bám quỹ đạo của nó.

Đố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 qn 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
Trần Văn Đổ

Hướng dẫn: TS. Lê Hoài Nam

8


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 tốn này có thể ứng dụng vào q trình thiết
kế robot nói chung.

C
C

R
L
.
T

U
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
Trần Văn Đổ

Hướng dẫn: TS. Lê Hoài Nam

9


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

R
L
.
T

U
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
Trần Văn Đổ

Hướng dẫn: TS. Lê Hoài Nam

10


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.

Ngun tắc chung tính chọn hệ dẫn động

C
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

R
L
.
T

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:
-

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
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
nằm ngang.

-

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.

U
D

Tính tố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 vng 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
Trần Văn Đổ

Hướng dẫn: TS. Lê Hoài Nam

11

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

C
C

Ta có bảng thông số DH:

R
L
.
T

Bảng 2.1: Bảng thông số khâu 1,2
Khâu

θi

1

θ1*+90

2

θ2*+90

U
D

αi

ai

di

0

l1

0

0

l2

0

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

Ta có các mà trận Ai của robot như sau:
𝑐𝑜𝑠(𝜃1)
𝑠𝑖𝑛(𝜃1)
𝐀𝟏 = [
0
0

−𝑠𝑖𝑛(𝜃1)
𝑐𝑜𝑠(𝜃1)

0
0

0
0
1
0

𝑙1 ∗ 𝑐𝑜𝑠(𝜃1)
𝑙1 ∗ 𝑠𝑖𝑛(𝜃1) ]
0
1

( 2.1 )

𝑐𝑜𝑠(𝜃2)
𝑠𝑖𝑛(𝜃2)
𝐀𝟐 = [
0
0

−𝑠𝑖𝑛(𝜃2)
𝑐𝑜𝑠(𝜃2)
0
0

0
0
1
0

𝑙2 ∗ 𝑐𝑜𝑠(𝜃2)
𝑙2 ∗ 𝑠𝑖𝑛(𝜃2) ]
0
1

( 2.2 )

𝐶12
𝑆12
Ma trận T1= A1 và T2 = A1.A2 = [
0
0

−𝑆12
𝐶12
0
0

0
0
1
0

𝑙1𝐶1 + 𝑙2𝐶12
𝑙1𝑆1 + 𝑙2𝑆12
]
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
Trần Văn Đổ

Hướng dẫn: TS. Lê Hồi Nam

12


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
Qi = [
0
0

−1
0
0
0

0
0
0
0

0
0
]
0
0

( 2.4 )

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

Đặt:
𝑗−1

Uij= {

𝑇𝑗−1 𝑄𝑗

𝑇𝐼

C
C

𝑁ế𝑢 𝑗 ≤ 𝑖

0

R
L
.
T

𝑁ế𝑢 𝑗 > 𝑖

Áp dụng ta có:
0
𝜕𝑇
1
U11 = 1 =Q1A1= [
𝜕𝜃1
0
0

−1
0
0
0

0
0
0
0

( 2.6 )

U
D

0 𝐶1
0 𝑆1
][
0 0
0 0

−𝑆1
𝐶1
0
0

0
0
1
0

𝑙1𝐶1
−𝑆1
𝑙1𝑆1
𝐶1
]=[
0
0
1
0

( 2.7 )

−𝐶1
−𝑆1
0
0

0
0

0
0

−𝑙1𝑆1
𝑙1𝐶1
]
0
0

Tương tự U21 và U22 được tính như sau:
0
𝜕𝑇
1
U21= 2 = Q1T2 = [
𝜕𝜃1
0
0

U22=

𝜕𝑇2

𝜕𝜃2

−1 0 0 𝐶12 −𝑆12 0 𝑙1𝐶1 + 𝑙2𝐶12
0 0 0 𝑆12 𝐶12 0 𝑙1𝑆1 + 𝑙2𝑆12
][
]=
0 0 0
0

0
1
0
0 0 0
0
0
0
1
−𝑆12 −𝐶12 0 −(𝑙1𝑆1 + 𝑙2𝑆12)
𝑙1𝐶1 + 𝑙2𝐶12 ]
[ 𝐶12 −𝑆12 0
0
0
0
0
0
0
0
0

=A1Q1𝐴2 =

𝑐𝑜𝑠(𝜃1) −𝑠𝑖𝑛(𝜃1)
𝑠𝑖𝑛(𝜃1) 𝑐𝑜𝑠(𝜃1)
[
0
0
0
0

0 𝑙1 ∗ 𝑐𝑜𝑠(𝜃1) 0 −1 0
0 𝑙1 ∗ 𝑠𝑖𝑛(𝜃1) 1 0 0
][
0 0 0
1
0
0 0 0
0
1

Sinh viên thực hiện: Nguyễn Đình Danh
Trần Văn Đổ

0 𝑐𝑜𝑠(𝜃2)
0 𝑠𝑖𝑛(𝜃2)
][
0
0
0
0

Hướng dẫn: TS. Lê Hoài Nam

−𝑠𝑖𝑛(𝜃2)
𝑐𝑜𝑠(𝜃2)
0
0

0
0

1
0

𝑙2 ∗ 𝑐𝑜𝑠(𝜃2)
𝑙2 ∗ 𝑠𝑖𝑛(𝜃2)
]
0
1

13


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

−𝑆12
𝐶12
=[
0
0

−𝐶12
−𝑆12
0
0

0
0
0
0

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

Theo (6-19)/tr 117 sách Robot CN, giả thiết các tích qn tính bằng khơng, ta có các ma
trận giả qn tính JI như sau:
1/3𝑚1𝑙12
0
J1 = [
0
−1/2𝑚1𝑙1

0
0
0
0

0
0
0
0

1/3𝑚2𝑙22
0
J2 = [
0
−1/2𝑚2𝑙2

−1/2𝑚1𝑙1
0
]
0
𝑚1

0
0
0
0

0
0
0
0

−1/2𝑚2𝑙2
0
]
0
𝑚2

Theo (6-35)/tr 120 sách Robot CN, Ta có hiệu ứng qn tính trên các khâu như sau:
𝑇
𝑇
D11 = Tr(U11J1𝑈11
) + Tr(U21J2𝑈21
)

( 2.8 )

=Tr
−𝑙1𝑆1 1/3𝑚1𝑙12
0
𝑙1𝐶1
][
0
0
−1/2𝑚1𝑙1
0

−𝑆1 −𝐶1 0
𝐶1 −𝑆1 0
{[
0
0
0
0
0
0

0
0
0
0

1

4

3

3

0
0
0
0

U
D

= m1l12 + m2l22 + m2l22C2

C
C

R
L
.
T

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

0
0
−1/2𝑚2𝑙2
0
0
0
0

0 −1/2𝑚1𝑙1
−𝑆1
𝐶1
0
0
−𝐶1 −𝑆1
][
0
0
0
0
0
𝑚1
−𝑙1𝑆1 𝑙1𝐶1

0
0
0
0

0
0

]}
0
0

−𝑆12
𝐶12
0 −1/2𝑚2𝑙2
−𝐶12
−𝑆12
0
0
][
0
0
0
0
−(𝑙1𝑆1 + 𝑙2𝑆12) 𝑙1𝐶1 + 𝑙2𝐶12
0
𝑚2

0
0
0
0

0
0
]}
0
0

𝑇
D12 = D21 = Tr(U22J2𝑈21
) = Tr

−𝑆12
𝐶12
{[
0
0

−𝐶12
−𝑆12
0
0

1

1

3

2

−𝑙2𝑆12 1/3𝑚2𝑙22
0
𝑙2𝐶12
][
0
0

−1/2𝑚2𝑙2
0

0
0
0
0

0
0
0
0

0
0
0
0

−𝑆12
−1/2𝑚2𝑙2
−𝐶12
0
][
0
0
−(𝑙1𝑆1 + 𝑙2𝑆12)
𝑚2

𝐶12
−𝑆12

0
𝑙1𝐶1 + 𝑙2𝐶12

0
0
0
0

0
0
] }
0
0

= m2l22 + m2l22C2
𝑇
D22 = Tr(U22J2𝑈22
)

=
−𝑆12 −𝐶12 0 −𝑙2𝑆12 1/3𝑚2𝑙22
0
Tr{[ 𝐶12 −𝑆12 0 𝑙2𝐶12 ] [
0
0
0
0
0
−1/2𝑚2𝑙2
0

0
0
0
1

1

1

3

3

3

0
0
0
0

0
0
0
0

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

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

0
0
0
0

0
0
] }
0
0

= m2 l22S122 + m2l22C122 = m2 l22
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
Trần Văn Đổ

Hướng dẫn: TS. Lê Hoài Nam

14


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

′

C(𝜃, 𝜃 ) = [

Vì vậy:

1
2

− m2S2l12 𝜃′22 – m2S2l12 𝜃′1 𝜃′2
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 )

0
−𝑆1 −𝐶1 0 −𝑙1𝑆1
𝑙1/2
=-m1(0, -g, 0, 0)[ 𝐶1 −𝑆1 0 𝑙1𝐶1 ] [
]
0
0
0
0
0
0
0
0
0
1
−𝑆12 −𝐶12
-m2(-g, 0, 0, 0)[ 𝐶12 −𝑆12
0
0
0
0
𝑆1

𝑙2

2

2

0 −(𝑙1𝑆1 + 𝑙2𝑆12)
0
0
𝑙1𝐶1 + 𝑙2𝐶12 ] [−𝑙2/2]
0
0
0
1
0
0

C
C

R
L
.
T

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

U
D

−𝑆12 −𝐶12 0
G2 = m2gTU22r2 = -m2(-g, 0, 0, 0) [ 𝐶12 −𝑆12 0
0

0
0
0
0
0

= m2gl2(

𝐶12
2

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

- S12)

m1gl1(c1 −
Ta có ma trận G(𝜃)=[

𝑆1
2

𝑙2

) + m2g( C12 − l1S1 − l2S12)
2

m2gl2(

𝐶12
2

− S12)

]

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))
1

F=[

3

m1l12 +
1
3

4

m2l22 + m2l22 C2

3
2

m2l2 +

1
2

m2l22 C2

1

−2m2S2l12 𝜃̇22 – m2S2l12 𝜃̇1 𝜃̇2

[

1
m2S2l22 𝜃̇12
2

Sinh viên thực hiện: Nguyễn Đình Danh
Trần Văn Đổ

( 2.10 )
1
3

m2l22 +
1
3

m1gl1(c1 −
]+[

1
2

m2l22 C2

m2l22
𝑆1
2

𝜃1̈
] ( ̈ )+
𝜃2

𝑙2

) + m2g( C12 − l1S1 − l2S12)
2

m2gl2(

𝐶12
2

Hướng dẫn: TS. Lê Hoài Nam

− S12)

]

15