Báo Cáo Đ Án Robot

111Equation Chapter 1 Section 1
TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP. HCM

KHOA ĐIỆN – ĐIỆN TỬ

BÁO CÁO ĐỒ ÁN
ĐỀ TÀI: THIẾT KẾ VÀ THI CÔNG
MÔ HÌNH CÁNH TAY ROBOT 4 BẬC
TỰ DO
GVHD: TS. Trần Đức Thiện

NHÓM:
SVTH: Lê Văn Cảm 20151441
Bạch Công Minh 20151122
Thành phố Hồ Chí Minh, Tháng 1 năm 2024

ĐA_ROBOT GVHD:TS.TRẦN ĐỨC THIỆN
MỤC LỤC
CHƯƠNG 1: TỔNG QUAN ĐỀ TÀI..........................................................................1
1. Đặt vấn đề............................................................................................................1
2. Mục tiêu đề tài.....................................................................................................1
3. Phương pháp nghiên cứu....................................................................................2
4. Giới hạn đề tài.....................................................................................................3
5. Nội dung đề tài.....................................................................................................3
CHƯƠNG 2: THIẾT KẾ ROBOT BẰNG PHẦN MỀM SOLIDWORK................5
1. Vẽ mô hình trên Solidwork................................................................................5
2. Đặt trục cho mô hình........................................................................................15
CHƯƠNG 3: TÍNH TOÁN ĐỘNG HỌC THUẬN, ĐỘNG HỌC NGHỊCH........17
1. Động học thuận..................................................................................................17
2. Động học nghịch................................................................................................19
2.1.Tìm theta1.......................................................................................................19
2.2.Tìm theta2.......................................................................................................19
2.3 Tìm theta3.......................................................................................................20
2.4 Tìm theta4.......................................................................................................20
2.5.Kết luận...........................................................................................................20
3. Không gian làm việc.............................................................................................21
CHƯƠNG 4: LƯU ĐỒ GIẢI THUẬT CỦA HỆ THỐNG......................................23
1. Lưu đồ Set Home...............................................................................................23
2. Lưu đồ làm việc.................................................................................................24
CHƯƠNG 5: SƠ ĐỒ KẾT NỐI DÂY CỦA ROBOT..............................................25
CHƯƠNG 6: THI CÔNG MÔ HÌNH.......................................................................26
1. Thiết kế phần cứng............................................................................................26
1.1 Lựa chọn thiết bị.........................................................................................26
1.2 Chi phí các thiết bị......................................................................................29
2. Giao diện hệ thống............................................................................................30
CHƯƠNG 7: ĐÁNH GIÁ...........................................................................................31
1. Kết quả đạt được...............................................................................................31
2. Hạn chế...............................................................................................................31
3. Hướng phát triển...............................................................................................31
PHỤ LỤC CHƯƠNG TRÌNH MATLAB.................................................................32

TÀI LIỆU THAM KHẢO..........................................................................................51
DANH MỤC HÌNH ẢNH

Hình 1. Chuyển đổi đơn vị trong Solidwork........................................................5
Hình 2. Tổng quan mô hình Robot thiết kế trong Solidworks.............................5
Hình 3. Hình ảnh mặt cắt và các hình chiếu của đế 1..........................................6
Hình 6. Hình ảnh mặt cắt và các hình chiếu của đế tròn xoay.............................7
Hình 7. Hình ảnh mặt cắt và các hình chiếu của miếng đệm lót ở đế..................8
Hình 8. Hình ảnh mặt cắt và các hình chiếu của miếng đệm nối các link với
nhau......................................................................................................................8
Hình 9. Hình ảnh mặt cắt và các hình chiếu của link 1.1.....................................9
Hình 10. Hình ảnh mặt cắt và các hình chiếu của link 1.3...................................9
Hình 11. Hình ảnh mặt cắt và các hình chiếu của link 1.2.................................10
Hình 12. Hình ảnh mặt cắt và các hình chiếu của miếng thánh giá...................10
Hình 13. Hình ảnh mặt cắt và các hình chiếu của miếng đệm nối các link.......11
Hình 14. Hình ảnh mặt cắt và các hình chiếu của link 2_1................................11
Hình 21. Hình ảnh mặt cắt và các hình chiếu của miếng gắn nam châm...........15
Hình 22. Hình ảnh đặt trục theo phương phép biến đổi của J.Craig..................16
Hình 23. Sơ đồ nối dây kết nối Arduino Shield V3 và Module Driver A4988..25
Hình 24. Hình sơ đồ nối dây kết nối Arduino Shield V3 và với động cơ và các
linh kiện khác.....................................................................................................25
Hình 25. Giao diện của hệ thống........................................................................30
DANH MỤC BẢNG

Bảng 1. Thông số và ký hiệu robot 4 bậc tự do.............................................................17
Bảng 2. Bảng D-H.........................................................................................................17
Bảng 3. Bảng khối lượng các khâu tương ứng..............................................................26
Bảng 4. Bảng danh sách các thiết bị..............................................................................26
Bảng 5. Chi phí các thiết bị...........................................................................................29
ĐA_ROBOT GVHD:TS. TRẦN ĐỨC THIỆN
CHƯƠNG 1: TỔNG QUAN ĐỀ TÀI

1. Đặt vấn đề
Trong sự nghiệp công nghiệp hóa, hiện đại hóa đất nước vấn đề tự động hóa có
vai trò đặt biệt quan trọng. Nhằm nâng cao năng suất dây chuyền công nghệ, nâng cao
chất lượng và khả năng cạnh tranh sản phẩm, cải thiện điều kiện lao động, nâng cao
năng suất lao động … vấn đề đặt ra là hệ thống sản xuất phải có tính linh hoạt cao.
Robot công nghiệp, đặc biệt là những tay máy robot là bộ phận quan trọng để tạo ra
những hệ thống đó. Tay máy Robot đã có mặt trong sản xuất từ nhiều năm trước, ngày
nay tay máy Robot đã dùng ở nhiều lĩnh vực sản xuất, từ những ưu điểm mà tay máy
Robot hoạt động trong quá trình sản xuất, làm việc, chúng ta có thể rút ra những tính
năng mà con người không thể có được: khả năng làm việc ổn định, có thể làm việc
trong môi trường độc hại … Do đó việc đầu tư nghiên cứu, chế tạo ra những tay máy
Robot phục vụ cho công cuộc tự động hóa sản xuất là rất cần thiết cho hiện tại và
tương lai.
2. Mục tiêu đề tài
Để hiểu rõ về bản chất của kết cấu cơ khí, cách thức tạo ra một mô hình robot,
cách lập trình và điều khiển một cánh tay robot do mình tự chế tạo và xa hơn nữa là
làm chủ kỹ thuật chế tạo robot trong tương lai, bài báo cáo này sẽ phân tích quá trình
thiết kế cánh tay robot bằng phần mềm Solidworks sau đó điều khiển mô hình thực của
robot di chuyển trong mặt phẳng đến những điểm được chỉ định.
Mục tiêu cơ bản nhất là thiết kế và xây dựng robot 4 bậc tự do: Mục tiêu này
tập trung vào việc nghiên cứu, thiết kế và xây dựng một robot 4 bậc tự do với các khả
năng di chuyển linh hoạt và thực hiện các nhiệm vụ cụ thể.
Mục tiêu tiếp theo tập trung vào việc điều khiển và lập trình robot 4 bậc tự do:
Mục tiêu này tập trung vào việc phát triển phần mềm điều khiển và lập trình cho robot
4 bậc tự do, cho phép nó thực hiện các nhiệm vụ theo các thuật toán và quyết định
được lập trình trước.
Điều khiển và định vị robot 4 bậc tự do: Mục tiêu này tập trung vào việc nghiên
cứu và phát triển các phương pháp điều khiển và định vị cho robot 4 bậc tự do, để nó
có thể di chuyển và hoạt động chính xác trong không gian 3D.
1
Xa hơn nữa, đề tài nghiên cứu này có thể giúp nghiên cứu và phát triển các
thuật toán và công nghệ cho robot 4 bậc tự do: Mục tiêu này tập trung vào việc nghiên
cứu và phát triển các thuật toán và công nghệ mới để cải thiện hiệu suất, độ chính xác
và khả năng tự động hóa của robot 4 bậc tự do.
3. Phương pháp nghiên cứu
Nghiên cứu các tài liệu liên quan nhằm tổng hợp lựa chọn các phương án từ đó
đưa ra mô hình robot phù hợp, trên cơ sở đó tính toán thiết kế bộ điều khiển cho robot
Chế tạo mô hình để kiểm chứng kết quả.
a) Phương pháp nghiên cứu lý thuyết
- Tìm hiểu qua sách vở, tài liệu trên các diễn đàn.
- Tìm hiểu về các bài toán, mô hình hóa giúp cho việc tính toán và chọn
các trang thiết bị điện và cơ khí.
- Tìm hiểu cơ sở lý thuyết về các loại động cơ, hệ thống cảm biến, bộ điều
khiển điển hình trong robot 4 bậc tự do.
- Tìm hiểu về cơ sở lý thuyết cho Arduino, giao diện C#, ứng dụng, viết
chương trình điều khiển và giao diện người dùng cho robot.
b) Phương pháp nghiên cứu thực nghiệm
- Nghiên cứu robot 4 bậc tự do trên thực tế hoặc các mô hình của đề tài
trước.
- Sử dụng phần mềm Solidworks để thiết kế và mô phỏng hệ thống cơ khí.
- Nghiên cứu từ các video trên mạng về phần kết nối, hoạt động vủa các
thiệt bị cũng như cách điều khiển chúng.
c) Phương pháp thực hiện sử dụng phương pháp quản lí 5M+1E
Khi áp dụng mô hình 5M+1E vào nghiên cứu thiết kế mô hình hệ thống lưu
kho tự động, các yếu tố 5M+1E có thể được hiểu như sau:
Manpower (Nhân lực): Đảm bảo có đủ nhân lực và nguồn lực cần thiết để
thực hiện dự án. Điều này bao gồm việc xác định vai trò và trách nhiệm của từng
thành viên trong nhóm nghiên cứu và đảm bảo sự phối hợp và làm việc hiệu quả giữa
các thành viên.
2
Material (Vật liệu): Quản lý và kiểm soát việc sử dụng vật liệu trong quá
trình xây dựng robot. Điều này bao gồm việc tìm kiếm và lựa chọn các vật liệu phù
hợp với yêu cầu kỹ thuật, kiểm soát chất lượng vật liệu và quản lý kho vật liệu.
Machine (Máy móc): Đảm bảo có đủ và phù hợp các thiết bị và máy móc
cần thiết để thực hiện quá trình xây dựng robot. Bao gồm việc lựa chọn, mua sắm,
bảo trì và sử dụng các thiết bị và máy móc một cách hiệu quả.
Method (Phương pháp): Xác định và áp dụng các phương pháp xây dựng
robot và quản lý dự án. Bao gồm việc lựa chọn phương pháp thiết kế, phương pháp
lắp ráp, phương pháp kiểm tra và kiểm soát chất lượng, cũng như phương pháp quản
lý thời gian và nguồn lực.
Measurement (Đo lường): Đo lường và đánh giá quá trình và kết quả của dự
án. Điều này bao gồm việc định nghĩa các chỉ số, tiêu chí đo lường và phương pháp
đánh giá để theo dõi tiến độ, chất lượng và hiệu suất của quá trình xây dựng robot.
Environment (Môi trường): Đánh giá và quản lý các yếu tố môi trường có
thể ảnh hưởng đến quá trình xây dựng robot. Bao gồm việc đảm bảo an toàn lao động,
tuân thủ các quy định về môi trường và quản lý rủi ro trong quá trình làm việc. quả và
hiệu suất cao.
Phương pháp quản lý 5M+1E giúp đảm bảo sự cân đối và hiệu quả trong
quản lý dự án xây dựng robot 4 bậc tự do. Bằng cách quản lý chặt chẽ các yếu tố quan
trọng này, bạn có thể tăng khả năng hoàn thành dự án đúng hạn, đạt được chất lượng
và hiệu suất mong muốn.
4. Giới hạn đề tài
Đề tài thiết kế mô hình robot 4 bậc tự do có một số giới hạn như sau:
- Hình dạng thiết kế chưa được tối ưu
- Mô hình bị giới hạn góc quay nên không gian làm việc Robot còn hạn
chế
- Mô hình chỉ sử dụng chất liệu mica nên độ bền còn thấp, khả năng chịu
tải thấp
- Hoạt động hạn chế, thiếu linh hoạt.
5. Nội dung đề tài
Cấu trúc của bài báo cáo được tổ chức như sau:
3
Chương 1: Tổng quan đề tài

Chương 2: Thiết kế mô hình Robot bằng phần mềm Solidworks
Chương 3: Tính toán động học Robot
Chương 4: Lưu đồ giải thuật cho hệ thống
Chương 5: Sơ đồ kết nối dây của Robot
Chương 6: Thi công mô hình
Chương 7: Đánh giá
4
CHƯƠNG 2: THIẾT KẾ ROBOT BẰNG PHẦN MỀM SOLIDWORK

1. Vẽ mô hình trên Solidwork
Sau khi tải phần mềm về, ta cần chọn đơn vị sao cho chính xác với đơn vị ta
đang sử dụng trên phần mềm Solidwork. Về việc vẽ robot ta đang cần đơn vị là
milimet nên ta sẽ chuyển đổi thành milimet. Ta có thể thay đổi đơn vị ở góc phải dưới
màn hình Solidwork như hình 1.
Hình 1. Chuyển đổi đơn vị trong Solidwork

Mô hình 3D Robot trong môi trường Assembly trong Solidworks như hình 2.
Hình 2. Tổng quan mô hình Robot thiết kế trong Solidworks
5
Ta sẽ đi vào chi tiết từng khâu như sau:

 Khâu đế:
Hình 3. Hình ảnh mặt cắt và các hình chiếu của đế 1
6
Hình 6. Hình ảnh mặt cắt và các hình chiếu của đế tròn xoay
7
Hình 7. Hình ảnh mặt cắt và các hình chiếu của miếng đệm lót ở đế
Hình 8. Hình ảnh mặt cắt và các hình chiếu của miếng đệm nối các link với
nhau
8
 Link 1:
Hình 9. Hình ảnh mặt cắt và các hình chiếu của link 1.1
9
Hình 12. Hình ảnh mặt cắt và các hình chiếu của miếng thánh giá
10
Hình 13. Hình ảnh mặt cắt và các hình chiếu của miếng đệm nối các link
 Link 2:
Hình 14. Hình ảnh mặt cắt và các hình chiếu của link 2_1
11
12
 Link 3:
13
 Khâu 4:
14
Hình 21. Hình ảnh mặt cắt và các hình chiếu của miếng gắn nam châm
2. Đặt trục cho mô hình
Để thực hiện đặt trục robot ta thực hiện các bước sau:
 Bước 1: Vẽ cấu hình robot và xác định các đường vuông góc chung của
robot
 Bước 2: Vẽ trục tọa độ thứ {0} trùng với góc tọa độ
 Bước 3: Xác định trục Zi trùng với trục động cơ (tại các khớp quay thứ i)
 Bước 4: Xác định trục Xi dọc theo các đường vuông góc chung giữa 2
trục Z kề nhau
 Bước 5: Xác định trục Yi theo quy tắc bàn tay phải
 Bước 6: Xác định sao cho trục của hệ tọa độ {0} trùng với hệ tọa độ {1}
và hệ trục tọa độ {N} sẽ chọn XN là ngẫu nhiên (thường chọn dọc theo vị trí
điểm đầu cuối)
15
Hình 22. Hình ảnh đặt trục theo phương phép biến đổi của J.Craig
16
CHƯƠNG 3: TÍNH TOÁN ĐỘNG HỌC THUẬN, ĐỘNG HỌC NGHỊCH

1. Động học thuận
Đựa vào hình 22 ta đặt trục xác định thông số của robot:
Bảng 1. Thông số và ký hiệu robot 4 bậc tự do
Ký hiệu Mô tả Thông số Đơn vị
l0 Chiều dài link1 65 mm
Chiều dài link 5 122.5 mm
d1 Khoảng cách giữa trục tọa độ đế và khớp 1 0 mm
d2 Khoảng cách giữa trục tọa độ khớp 2 và khớp 3 0 mm
d3 Khoảng cách giữa trục tọa độ khớp 3 và khớp 4 0 mm
Khoảng cách giữa trục tọa độ khớp 4 và khớp 5 0 mm
Dựa vào bảng 1 ta lập được bảng D-H như sau:
Bảng 2. Bảng D-H
1 0 0
2 0 0 +
3 0 0
4 0 0
5 0 0 0
Trong đó: i là vị trí của khớp; là góc giữa trục Zi và Zi+1 được tính quanh
trục Xi; là khoảng cách giữa trục Zi và Zi+1 được đo theo trục Xi; là khoảng cách
giữa trục Xi-1 và Xi dọc theo trục Zi; là góc giữa trục Xi-1 và Xi quanh trục Zi.
Công thức quay tịnh tiến của J.craig
212\*
MERGEFORMAT (.)
17
Ma trận chuyển đổi theo phương pháp J.craig
313\*
MERGEFORMAT (.)
Ta lần lượt tính từng ma trận chuyển đổi các hệ 1,2,3,4,5. Sau đó, ta nhân các
ma trận lại với nhau ra được ma trận tổng quát. Lấy phần tử hàng 1 cột 4, hàng 2 cột 4,
hàng 3 cột 4 của ma trận tổng quát này ta ra được công thức vị trí điểm cuối
( Px,Py,Pz).
Ta có :
p x =l 5 c 1 s 2 s3 s 4 −l 3 c 1 s 2−l 4 c 1 c2 c 3−l 4 c 1 c3 s2−l 5 c 1 c2 c 3 s 4−l 5 c 1 c 2 c 4 s 3−l 5 c1 c 4 c 3 s 2−l 1
414\* MERGEFORMAT (.)
515\*
MERGEFORMAT (.)
616\* MERGEFORMAT (.)
18
2. Động học nghịch
Ta có : 717\*
MERGEFORMAT (.)
 818\*
MERGEFORMAT (.)
2.1.Tìm theta1
Ta có : 919\*
MERGEFORMAT (.)
Suy ra ta được: 10110\*

MERGEFORMAT (.)
Biến đổi phương trình: 11111\*

MERGEFORMAT (.)
S 12112\*
MERGEFORMAT (.)
Ta được: 13113\*
MERGEFORMAT (.)
2.2.Tìm theta2
Ta có phương trình: 14114\*

MERGEFORMAT (.)
Biến đổi : 15115\*

MERGEFORMAT (.)
19
16116\*
MERGEFORMAT (.)
17117\*
MERGEFORMAT (.)
Thay vào ta được: 18118\*

MERGEFORMAT (.)
Thay vào ta được: 19119\*

MERGEFORMAT (.)
Kết quả: 20120\* MERGEFORMAT (.)

2.3 Tìm theta3
Ta có phương trình: 21121\*

MERGEFORMAT (.)
Biến đổi: 22122\*

MERGEFORMAT (.)
Đặt: 23123\*
MERGEFORMAT (.)
Ta có được phương trình: 24124\*

MERGEFORMAT (.)
20
Suy ra: 25125\*

MERGEFORMAT (.)
26126\*
MERGEFORMAT (.)
Kết quả: 27127\*

MERGEFORMAT (.)
2.4 Tìm theta4
Từ phường trình trên ta có được: 28128\*

MERGEFORMAT (.)
2.5.Kết luận
2.5.1. Kết luận động học thuận
p x =l 5 c 1 s 2 s3 s 4 −l 3 c 1 s 2−l 4 c 1 c2 c 3−l 4 c 1 c3 s2−l 5 c 1 c2 c 3 s 4−l 5 c 1 c 2 c 4 s 3−l 5 c1 c 4 c 3 s 2−l 1
Với L1 = 65;
L2 = 238;
L3 = 105;
L4 = 130;
L5 = 149;
2.5.2. Kết luận động học nghịch
a)
b)
Với và .
Trong đó, .
21
c)
Với và .
Trong đó, .
d)
3. Không gian làm việc
Dựa vào tọa độ điểm của robot ta tìm được từ động học thuận ở phần 2.1 và giới
hạn góc quay của robot, kết hợp với phần mềm Matlab, ta tìm được không gian làm
việc của robot khi theta234 = 0.
clc; clear all; close all;
L1 = 65;
L2 = 233;
L3 = 105;
L4 = 130;
L5 = 122.5;
tt=0; ii = 0; jj = 0;
for t1 = -90:5:90
for t3 = -90:5:90
for t2 = -90:5:90
for t4 = -90:5:90
if (t2 + t3 + t4) == 0 %% Điều kiện của khâu cuối
tt=tt+1;
Px = L5*cosd(t1)*sind(t2)*sind(t3)*sind(t4) -
L3*cosd(t1)*sind(t2) - L4*cosd(t1)*cosd(t2)*sind(t3) -
L4*cosd(t1)*cosd(t3)*sind(t2) - L5*cosd(t1)*cosd(t2)*cosd(t3)*sind(t4) -
L5*cosd(t1)*cosd(t2)*cosd(t4)*sind(t3) - L5*cosd(t1)*cosd(t3)*cosd(t4)*sind(t2) -
L1;
Py = -sind(t1)*(L4*sind(t2 + t3) + L3*sind(t2) + L5*sind(t2 +
t3 + t4)) ;
Pz = L2 + L4*cosd(t2 + t3) + L3*cosd(t2) + L5*cosd(t2 + t3 +
t4);
emtry(:,tt) = Px;
emtry1(:,tt) = Py;
emtry2(:,tt) = Pz;
end
end
end
end
end
figure(1);
plot3(emtry,emtry1,emtry2);
22
23
CHƯƠNG 4: LƯU ĐỒ GIẢI THUẬT CỦA HỆ THỐNG

1. Lưu đồ Set Home
24
2. Lưu đồ làm việc
25
CHƯƠNG 5: SƠ ĐỒ KẾT NỐI DÂY CỦA ROBOT

5.1. Sơ đồ kết nối Arduino Shield V3 với Module Driver A4988
Hình 23. Sơ đồ nối dây kết nối Arduino Shield V3 và Module Driver A4988
5.2. Sơ đồ kết nối Arduino Shield V3 với động cơ và các thiết bị khác
Hình 24. Hình sơ đồ nối dây kết nối Arduino Shield V3 và với động cơ và các
linh kiện khác
26
CHƯƠNG 6: THI CÔNG MÔ HÌNH

1. Thiết kế phần cứng
1.1 Lựa chọn thiết bị
Đầu tiên ta cần xác định khối lượng các khâu, chiều dài các khâu để chọn Servo
phù hợp với ứng dụng. Chiều dài các khâu được thể hiện tại Bảng 1. Khối lượng thông
số ta lấy tự các file in 3D và được các thống số trong Bảng 3.
Bảng 3. Bảng khối lượng các khâu tương ứng
STT Khí hiệu Khâu tương ứng Khối lượng Đơn vị
1 Đế robot 1207 g
2 Khâu 1 2701 g
3 Khâu 2 1200 g
4 Khâu 3 1416 g
5 Khâu 4 704 g
6 Khâu 5 305 g
Tiếp theo, các thiết bị nhóm sử dụng sẽ được liệt kê trong Bảng
Bảng 4. Bảng danh sách các thiết bị
DANH SÁCH CÁC THIẾT BỊ

STT TÊN HÌNH ẢNH ĐẶC ĐIỂM
1 Arduino Nguồn nuôi mạch: 5V
Uno R3 Số chân Digital: 14(6 chân PWM)
Số chân Analog: 6
Tốc độ: 16MHz
Dòng ra chân 3.3V: 50mA
2 Dây cắm Kết nối các phần tử trong hệ
test board thống
(Đực cái)
27
3 Dây cắm Kết nối các phần tử trong hệ

test board thống
(Đực đực)
4 Ốc M2, Cố định động cơ và các khớp

M3,M4,M
5
5 Tán Cố định động cơ và các khớp

M2,M3,M
4,M5
6 Bu Lông Cố định động cơ và các khớp
7 Step motor -Đường kính trục 5mm.

Nema 17
- Cường độ định mức 1.5A, mô
men giữ 0.55 Nm, góc quay mỗi
bước 1.8 °
- Dây nối dài 1m, đầu dây chuẩn
XH2.54.
28
8 Step motor - Kích thước: 57x57x54MM

Nema 23 - Đường kính trục: 6.35mm
( 57mm và - Dòng: 2A - Điện áp: 3-49V
76mm) - Bước: 1.8° - Phase: 2 Phase
9 Thanh trục - Dùng trong truyền động lên

xuống của cơ cấu trục
10 Khớp nối được sử dụng để nối trục động

mặt bích cơ tạo thành mặt bích
11 Được dùng để tăng lực căng

Ròng Rọc của dây
Trơn
2GT20
12 Dùng để hổ trợ chuyển động

Bạc Đạn quay của động cơ
Vòng Bi
13 Vòng đai -Dùng để truyền chuyển động

quay của động cơ
29
14 Pulley -Dùng để truyền chuyển động

GT2 quay của động cơ
15 -Dùng để giữ cố định trục quay

Gối Đỡ
Vòng Bi
16 -Dòng điện max: 1-9V

Công tắc -Điện áp AC max: 250V
hành trình - Lực giải phóng: 0.49N
- dòng điện định mức 5V, 250V
1.2 Chi phí các thiết bị

Bảng 5. Chi phí các thiết bị
STT Tên thiết bị Đơn giá Số Thành Link
lượng giá
1 Cắt Mica 500.000 1 500.000
2 Arduino 140.000 1 140.000
Uno
3 Nguồn tổ 75.000 1 75.000 https://icdayroi.com/nguon-
ong 12V xung-12v5a-nguon-to-ong
4 Tán 150.000 1 150.000
M2,3,4,5
5 Bu Lông 15.000 3 45.000
6 Step Motor 60.000 2 120.000
Nema 17
7 Step Motor 160.000 2 320.000
Nema 23
8 Khớp nối 27.000 4 108.000 https://www.thegioiic.com/
mặt bích kfl08-goi-do-vong-bi-8mm
9 Ròng Rọc 14.000 4 56.000 https://www.thegioiic.com/rong-
30
Trơn roc-tron-2gt20-ranh-6-5mm-
2GT20 truc-5mm
10 Bạc Đạn 2.800 4 11.200 https://www.thegioiic.com/bac-
Vòng Bi dan-vong-bi-trong-5mm-ngoai-
16mm-day-5mm
11 Vòng đai 49.000 4 196.000
12 Pulley GT2 42.000 4 168.000 https://imaker.vn/pulley-gt2-40-

rang-ban-6mm
13 17.000 4 108.000 https://www.thegioiic.com/
Gối đỡ kfl08-goi-do-vong-bi-8mm
vòng bi
14 Công tắc 5.000 4 20.000
hành trình
2. Giao diện hệ thống
Hình 25. Giao diện của hệ thống
31
CHƯƠNG 7: ĐÁNH GIÁ

1. Kết quả đạt được
Trong quá trình nghiên cứu và thực hiện báo cáo này, bọn em đã xem xét và
nghiên cứu các chủ đề quan trọng liên quan đến động lực học của robot và quy hoạch
quỹ đạo. Em tính động học thuận và động học nghịch của robot, thực hiện quy hoạch
quỹ đạo cho robot thông qua việc sử dụng thuật toán.
Tính động học thuận và động học nghịch đã được thảo luận để hiểu rõ hơn về
cách robot di chuyển một cách linh hoạt và hiệu quả trong môi trường đã định hướng,
cũng như khả năng của nó trong việc thích nghi và vượt qua các trở ngại.
Quy hoạch quỹ đạo robot đã được trình bày thông qua việc tạo ra quỹ đạo bằng
cách kết hợp mô hình động học của robot và thuật toán. Điều này giúp robot di chuyển
theo một đường dẫn thuận lợi và an toàn để đạt được mục tiêu cụ thể trong không gian.
2. Hạn chế
- Cơ cấu robot chưa được tối ưu, kích thước robot còn nhỏ cùng với việc góc
quay của động cơ bị hạn chế nên không gian làm việc còn hạn chế.
- Động cơ sử dụng chưa tối ưu cho việc điều khiển do trong quá trình chạy trục
động cơ vẫn còn bị rơ, lệch trục dẫn đến việc quy hoạch quỹ đạo còn rung lắc
và không trơn tru
- Robot còn gặp nhiều nỗi trong quá trình vận hành do các hạn chế trong thiết
kế cơ khí và phần cứng.
3. Hướng phát triển
Mô hình cánh tay robot 4 bậc tự do có thể phát triển theo nhiều hướng sau:
- Tăng mức giới hạn góc quay cho động cơ tạo ra vùng không gian làm việc lớn
hơn
- Lập trình quy hoạch nhiều quỹ đạo khác nhau: đường tròn, chữ cái…
32
PHỤ LỤC CHƯƠNG TRÌNH MATLAB

 Chương trình tính động học thuận:
clc;
clear all;
% L1 = 65;
% L2 = 233;
% L3 = 105;
% L4 = 130;
% L5 = 122.5;
syms d1 L1 L2 L3 L4 L5;
syms t1 t2 t3 t4;
syms r11 r12 r13 r21 r22 r23 r31 r32 r33 px py pz;
%
% t1 = 0;
% t2 = sym(pi/2);
% t3 = sym(-pi/4);
% t4 = sym(-pi/4);
FK = [r11 r12 r13 px; r21 r22 r23 py; r31 r31 r33 pz; 0 0 0 1]
T01 = JCraig_rad(-L1,0,L2,t1);
T12 = JCraig_rad(0,sym(pi/2),0,t2+sym(pi/2));
T23 = JCraig_rad(L3,0,0,t3);
T34 = JCraig_rad(L4,0,0,t4);
T45 = JCraig_rad(L5,0,0,0);
T05 = T01*T12*T23*T34*T45;
Px = simplify(T05(1,4))
Py = simplify(T05(2,4))
Pz = simplify(T05(3,4))
%T10*T05 = T10*(T01*T12*T23*T34*T45)
T10 = inv(T01);
T10FK = T10*FK;
T12345 = T12*T23*T34*T45;
% n1 = simplify(T10FK(1,4))
%
% m1 = simplify(T12345(1,4))
% m2 = simplify(T12345(2,4))
% m3 = simplify(T12345(3,4))
33
 Chương trình tính động học nghịch:

clear all
clc
syms x y z L1 L2 L3 L4 L5
L1 = 65;
L2 = 233;
L3 = 105;
L4 = 130;
L5 = 122.5;
P = [- 65*2^(1/2) - 170 0 65*2^(1/2) + 711/2];
x = P(1,1);
y = P(1,2);
z = P(1,3);
theta234 = 0;
%% Tính theta1
theta1 = atan2d(y,x + L1);
if(theta1 < -90 || theta1 > 90)
theta1 = atan2d(-y,-x - L1);
end
%% Tính nx ny
nx = cosd(theta1)*L1+ cosd(theta1)*x + sind(theta1)*y + L5*sind(theta234);
ny = z - L2 - L5*cosd(theta234);
%% Tính theta3
c3 = (nx^2 + ny^2 - L4^2 - L3^2)/(2 * L3 * L4);
if(abs(c3 - 1) < 0.0001)
c3 = 1;
end
s3_1 = sqrt(1 - c3^2);
s3_2 = -sqrt(1 - c3^2);
theta3_1 = atan2d(s3_1, c3);
theta3_2 = atan2d(s3_2, c3);
%% Tính theta2
c2_1 = (ny * (L4*cosd(theta3_1) + L3) - nx*L4*sind(theta3_1)) /
((L4*cosd(theta3_1) + L3)^2 + (L4*sind(theta3_1))^2);
s2_1 = (-nx*(L4*cosd(theta3_1)+L3) - L4*ny*sind(theta3_1)) / ((L4*cosd(theta3_1)
+ L3)^2 + (L4*sind(theta3_1))^2);
theta2_1 = atan2d(s2_1, c2_1);
c2_2 = (ny * (L4*cosd(theta3_2) + L3) - nx*L4*sind(theta3_2)) /

((L4*cosd(theta3_2) + L3)^2 + (L4*sind(theta3_2))^2);
s2_2 = (-nx*(L4*cosd(theta3_2)+L3) - L4*ny*sind(theta3_2)) / ((L4*cosd(theta3_2)
+ L3)^2 + (L4*sind(theta3_2))^2);
theta2_2 = atan2d(s2_2, c2_2);
%% Tính theta4
theta4_1 = theta234 - theta2_1 - theta3_1;
%% Góc theta
theta = [theta1 theta2_1 theta3_1 theta4_1;
theta1 theta2_2 theta3_2 theta4_2]
34
 Chương trình tính động học thuận tỏng C#:

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace GuiControlRobot
{
internal class InverseKinematics
{
public static double[,] CalculateTheta(double x, double y, double z, double theta234)
{
double L1, L2, L3, L4, L5;
L1 = 65;
L2 = 238;
L3 = 105;
L4 = 130;
L5 = 149;
theta234 = theta234 * (Math.PI / 180);
double theta1;
double nx, ny;
double c3, s3_1, s3_2, theta3_1, theta3_2;
double c2_1, s2_1, theta2_1, c2_2, s2_2, theta2_2;
double theta4_1, theta4_2;
double[,] theta = new double[2, 4];
//Tính theta1
theta1 = Math.Atan2(y, x + L1);
if ( x+L1 < 0)
{
theta1 = Math.Atan2(-y, -x - L1);
}
// Tính nx ny
nx = (Math.Cos(theta1) * L1) + (Math.Cos(theta1) * x) + (Math.Sin(theta1) * y) + (L5 * Math.Sin(theta234));
ny = z - L2 - (L5 * Math.Cos(theta234));
// Tính theta3
c3 = (nx*nx + ny*ny - L4*L4 - L3*L3) / (2 * L3 * L4);
if(Math.Abs(c3 - 1) < 0.0001)
c3 = 1;
s3_1 = Math.Sqrt(1 - (c3*c3));
s3_2 = -Math.Sqrt(1 - (c3*c3));
theta3_1 = Math.Atan2(s3_1, c3);
theta3_2 = Math.Atan2(s3_2, c3);
// Tính theta2
c2_1 = (ny * (L4 * Math.Cos(theta3_1) + L3) - nx * L4 * Math.Sin(theta3_1)) / ((Math.Pow((L4 *
Math.Cos(theta3_1)+ L3), 2) + Math.Pow((L4 * Math.Sin(theta3_1)), 2)));
s2_1 = (-nx * (L4 * Math.Cos(theta3_1) + L3) - L4 * ny * Math.Sin(theta3_1)) / ((Math.Pow((L4 *
Math.Cos(theta3_1) + L3), 2) + Math.Pow((L4 * Math.Sin(theta3_1)), 2)));
theta2_1 = Math.Atan2(s2_1, c2_1);
c2_2 = (ny * (L4 * Math.Cos(theta3_2) + L3) - nx * L4 * Math.Sin(theta3_2)) / ((Math.Pow((L4 *

s2_2 = (-nx * (L4 * Math.Cos(theta3_2) + L3) - L4 * ny * Math.Sin(theta3_2)) / ((Math.Pow((L4 *
theta2_2 = Math.Atan2(s2_2, c2_2);
35
// Tính theta4
theta1 = theta1 * (180 / Math.PI);

theta2_1 = theta2_1 * (180 / Math.PI);
theta[0, 0] = theta1;
theta[0, 1] = theta2_1;
theta[1, 0] = theta1;
//theta[2, 0] = nx;
//theta[2, 1] = ny;
//theta[2, 2] = c3;
return theta;
}
}
}
 Chương trình tính động học thuận C#:
using System;
using System.Linq;
using System.Text;
{
internal class ForwardKinematics
{
public static void CalculatePxPyPz(int degree1, int degree2, int degree3, int degree4, out double Px, out double
Py, out double Pz )
{
double L1, L2, L3, L4, L5;
L1 = 65;
L2 = 238;
L3 = 105;
L4 = 130;
L5 = 149;
double t1 = degree1 * Math.PI / 180.0;

Px = L5 * Math.Cos(t1) * Math.Sin(t2) * Math.Sin(t3) * Math.Sin(t4) - L3 * Math.Cos(t1) * Math.Sin(t2) -

L4 * Math.Cos(t1) * Math.Cos(t2) * Math.Sin(t3) - L4 * Math.Cos(t1) * Math.Cos(t3) * Math.Sin(t2) -
L5 * Math.Cos(t1) * Math.Cos(t2) * Math.Cos(t3) * Math.Sin(t4) - L5 * Math.Cos(t1) * Math.Cos(t2) *
Math.Cos(t4) * Math.Sin(t3) - L5 * Math.Cos(t1) * Math.Cos(t3) * Math.Cos(t4) * Math.Sin(t2) - L1;
//;L5* cos(t1)*sin(t2) * sin(t3) * sin(t4) - L3 * cos(t1) * sin(t2) - L4 * cos(t1) * cos(t2)
* sin(t3) - L4 * cos(t1) * cos(t3) * sin(t2) - L5 * cos(t1) * cos(t2) *
36
cos(t3) * sin(t4) - L5 * cos(t1) * cos(t2) * cos(t4) * sin(t3) - L5 * cos(t1) *

cos(t3) * cos(t4) * sin(t2) - L1
Py = (-1)*Math.Sin(t1) * (L4 * Math.Sin(t2 + t3) + L3 * Math.Sin(t2) + L5 * Math.Sin(t2 + t3 + t4));
Pz = L2 + L4 * Math.Cos(t2 + t3) + L3 * Math.Cos(t2) + L5 * Math.Cos(t2 + t3 + t4);
}
}
}
 Chương trình C# giao diện người dùng:
using System;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.IO.Ports;
{
public partial class MainControl : Form
{
private int degree1;

private int x, y, z;
public MainControl()
{
InitializeComponent();
string[] Baudrate = { "1200", "2400", "4800", "9600", "11520" };
cboBaudrate.Items.AddRange(Baudrate);
Control.CheckForIllegalCrossThreadCalls = false;
}
private void Form2_Load(object sender, EventArgs e)
{
cboCOMPort.DataSource = SerialPort.GetPortNames();
cboBaudrate.Text = "9600";
}
private void Form2_FormClosing(object sender, FormClosingEventArgs e)
37
{
try
{
if (serCom.IsOpen)
{
serCom.Close();
}
}
catch (Exception error)
{
MessageBox.Show(error.Message);
}
}
private void button1_Click(object sender, EventArgs e)
{
Application.Exit();
}
private void butConnect_Click(object sender, EventArgs e)

{
try
{
serCom.PortName = cboCOMPort.Text;
serCom.BaudRate = Convert.ToInt32(cboBaudrate.Text);
serCom.Open();
MessageBox.Show("Successfully connected to robot!");
txtConnect.Text = "Ready";
txtConnect.BackColor = Color.Green;
btnDisconnect.Enabled = true;
butConnect.Enabled = false;
}
{
}
}
private void but_SetHome_Click(object sender, EventArgs e)

{
try
{
38
if (serCom.IsOpen)
if (checkBox_Simultaneous.Checked)
{
{
serCom.Write(1+"H"+"#");
}
}
}
{
}
}
private void CalculateFK_Click(object sender, EventArgs e)

{
int theta1 = Convert.ToInt16(txtBox_FK_t1.Text);
double newX, newY, newZ;
ForwardKinematics.CalculatePxPyPz(theta1,theta2,theta3,theta4,out newX, out newY, out newZ);
newX = Math.Round(newX,2);
newY = Math.Round(newY,2);
newZ = Math.Round(newZ,2);
txt_DisplayPx.Text = newX.ToString();
txt_DisplayPy.Text = newY.ToString();
txt_DisplayPz.Text = newZ.ToString();
}
private void Send_FK_Click(object sender, EventArgs e)

{
degree1 = Convert.ToInt16(txtBox_FK_t1.Text);
if (serCom.IsOpen)
{
if(checkBox_Simultaneous.Checked)
39
{
serCom.Write(degree1 + "A" + "#");
serCom.Write(degree2 + "B" + "#");
serCom.Write(degree3 + "C" + "#");
serCom.Write(degree4 + "D" + "#");
}
}
}
private void Send_IK_Click(object sender, EventArgs e)

{
double d1_1 = Convert.ToDouble(txt_DisplayT1_1.Text);
degree1 = Convert.ToInt16(d1_1);
if (serCom.IsOpen)
{
{
}
}
}
private void but_Check_IK_Click(object sender, EventArgs e)

{
double t1_1_need_check = Convert.ToDouble(txt_DisplayT1_1.Text);
40

char check = checkForward.CheckForward(t1_1_need_check, t2_1_need_check, t3_1_need_check,

t4_1_need_check, t2_1_need_check, t2_2_need_check, t3_2_need_check, t4_2_need_check);
if (check == '1')
{
txt_Result_IK.Text = "Pack 1 is correct";
}
if(check == '2')
{
txt_Result_IK.Text = "Pack 2 is correct";
}
if (check == '3')
{
txt_Result_IK.Text = "Both pack are correct";
}
private void Send_IK_2_Click(object sender, EventArgs e)

{
if (serCom.IsOpen)
{
{
41
}
}
}
private void but_Hut_Click(object sender, EventArgs e)

{
if (serCom.IsOpen)
{
{
serCom.Write(1 + "M" + "#");
}
}
}
private void but_Nha_Click(object sender, EventArgs e)

{
if (serCom.IsOpen)
{
{
serCom.Write(0 + "M" + "#");
}
}
}
private void CalculateIK_Click(object sender, EventArgs e)

{
double Px = Convert.ToDouble(txt_IK_Px.Text);
double Py = Convert.ToDouble(txt_IK_Py.Text);
double Pz = Convert.ToDouble(txt_IK_Pz.Text);
double t234 = Convert.ToDouble(txt_IK_t234.Text);

double newT1_1, newT2_1, newT3_1, newT4_1;
double newT1_2, newT2_2, newT3_2, newT4_2;
double[,] thetaArray = InverseKinematics.CalculateTheta(Px, Py, Pz, t234);
42
newT1_1 = thetaArray[0, 0];


newT1_1 = Math.Round(newT1_1,2);
newT4_1 = Math.Round(newT4_1, 2);

txt_DisplayT1_1.Text = newT1_1.ToString();
private void btnDisconnect_Click(object sender, EventArgs e)

{
try
{
if (serCom.IsOpen)
{
serCom.Close();
MessageBox.Show("Disconnected to Robot");
43
txtConnect.Text = "Unavailable";
txtConnect.BackColor = Color.Red;
btnDisconnect.Enabled = false;
butConnect.Enabled = true;
}
}
{
}
}
}
}
 Chương trình Arduino:

// De LOW: quay nguoc chieu kim ; HIGH: thuan
// khai bao chan dieu khien cho step driver, shield v3
int en=8;
int dirX = 5; //dirX
int stepX = 2; //stepX
int dirY = 6; //dirY

int stepY = 3; //stepY
int dirZ = 7; //dirZ

int stepZ = 4; //stepZ
int dirA = 13; //dirA

int stepA = 12; //stepA
#define limitX 9 //limitX

#define limitY 10 //limitY
#define limitZ A2 //limitZ
#define limitA A0 //limitA
#define magnet A1 //Magnet
int docX, docY, docZ, docA; //Biến trạng thái của Limit Switch
char c; //Biến nhận từ giao diện
String InData; // Chuỗi để xử lí
int8_t indexA, indexB, indexC, indexD, indexH, indexM;
//Biến góc và bước truyền vào để điều khiển động cơ
int curDegree1 = 0, curDegree2 = 0, curDegree3 = 0, curDegree4 = 0;
int tarDegree1 = 0, tarDegree2 = 0, tarDegree3 = 0, tarDegree4 = 0;
int diffDegree1 = 0, diffDegree2 = 0, diffDegree3 = 0, diffDegree4 = 0;
44
int stepsMotor1, stepsMotor2, stepsMotor3, stepsMotor4;
int TT_SetHome = 0;
int TT_Magnet = 0;
void setup()
{
Serial.begin(9600);
pinMode(en, OUTPUT);
pinMode(dirX, OUTPUT);
pinMode(stepX, OUTPUT);
pinMode(limitX, INPUT_PULLUP);
pinMode(dirY, OUTPUT);
pinMode(stepY, OUTPUT);
pinMode(limitY, INPUT_PULLUP);
pinMode(dirZ, OUTPUT);
pinMode(stepZ, OUTPUT);
pinMode(limitZ, INPUT_PULLUP);
pinMode(dirA, OUTPUT);
pinMode(stepA, OUTPUT);
pinMode(limitA, INPUT_PULLUP);
pinMode(magnet, OUTPUT);
digitalWrite(en,LOW);
void Receive_Winform_Data()
{
while (Serial.available() > 0)
{
c = Serial.read();
if(c == '\n')
{
break;
}
else
{
InData += c;
}
}
}
45
void Parse_Data()
{
String str_SetHome, str_Magnet, str_DegreeMotor1, str_DegreeMotor2,
str_DegreeMotor3, str_DegreeMotor4 ;
indexH = InData.indexOf('H');
indexA = InData.indexOf('A');
indexB = InData.indexOf('B');
indexC = InData.indexOf('C');
indexD = InData.indexOf('D');
indexM = InData.indexOf('M');
if(indexH >= 0)
{
str_SetHome = InData.substring(0, indexH);
TT_SetHome = str_SetHome.toInt();
}
if(indexM >= 0)
{
str_Magnet = InData.substring(0, indexM);
TT_Magnet = str_Magnet.toInt();
}
if(indexA >= 0)
{
str_DegreeMotor1 = InData.substring(0, indexA);
tarDegree1 = str_DegreeMotor1.toInt();
}
if(indexB >= 0)
{
str_DegreeMotor2 = InData.substring(0, indexB);
}
if(indexC >= 0)
{
str_DegreeMotor3 = InData.substring(0, indexC);
}
if(indexD >= 0)
{
str_DegreeMotor4 = InData.substring(0, indexD);
}
}
void SetHome()
{
//------------------SET HOME LINK 4 ----------------------------
46
//Đưa link4 về vị trí LSA

//Serial.println("Dang SetHome");
digitalWrite(dirA,0);
for(int x = 10; x < 12000; x++ )
{
docA = digitalRead(limitA);
if(docA==0)
x = 12000;
digitalWrite(stepA, HIGH);
delayMicroseconds(700);
digitalWrite(stepA, LOW);
}
//Đưa link 4 về vị trí 0
digitalWrite(dirA,1);
for(int x = 10; x < 4400; x++ ) //tính lại
{
}
//------------------SET HOME LINK 3 ----------------------------

//Đưa link3 về vị trí LS Z
digitalWrite(dirZ,0);
for(long x = 10; x < 40000; x++ )
{
docZ = digitalRead(limitZ);
if(docZ==0)
x = 40000;
digitalWrite(stepZ, HIGH);
digitalWrite(stepZ, LOW);
}
digitalWrite(dirZ,1);
{
}
//------------------SET HOME LINK 2 ----------------------------

//Đưa link2 về vị trí LS Y
47
digitalWrite(dirY,0);
for(long x = 10; x < 30000; x++ )
{
docY = digitalRead(limitY);
if(docY==0)
x=30000;
digitalWrite(stepY, HIGH);
digitalWrite(stepY, LOW);
}
digitalWrite(dirY,1);
{
}
//------------------SET HOME LINK 1 ----------------------------
//Đưa link1 về vị trí LSA
digitalWrite(dirX,0);
for(long x = 10; x < 30000; x++ )
{
docX = digitalRead(limitX);
if(docX==0)
x=30000;
digitalWrite(stepX, HIGH);
digitalWrite(stepX, LOW);
}
digitalWrite(dirX,1);
{
}
}
void MoveMotor1()
{
if(tarDegree1 != curDegree1)
{
48
diffDegree1 = tarDegree1 - curDegree1; // Góc cần quay
stepsMotor1 = diffDegree1*100/3; //(degree/1.8)*(Tỉ số truyền)*Microstep

// StepMotor1: Số bước cần cấp vào
digitalWrite(dirX,stepsMotor1>0 ? LOW: HIGH); //Nếu số bước(góc quay) > 0
thì set cho dirX LOW, ngược lại thì HIGH
for (int i = 0; i < abs(stepsMotor1); i++)

{
}
curDegree1 = tarDegree1;
}
}
void MoveMotor2()
{
{
diffDegree2 = tarDegree2 - curDegree2;
digitalWrite(dirY,stepsMotor2>0 ? LOW: HIGH); //Nếu số bước(góc quay) > 0


{
}
}
}
void MoveMotor3()
{
{
digitalWrite(dirZ,stepsMotor3>0 ? LOW: HIGH); //Nếu số bước(góc quay) > 0

49

{
}
}
}
void MoveMotor4()
{
{
digitalWrite(dirA,stepsMotor4>0 ? LOW: HIGH); //Nếu số bước(góc quay) > 0


{
}
}
}
void loop()
{
Receive_Winform_Data();
if(c == '#')
{
Parse_Data();
c = 0;
InData = "";
}
MoveMotor1();
MoveMotor2();
MoveMotor3();
MoveMotor4();
50
if(TT_SetHome == 1)
{
Serial.println("Dang SetHome");
SetHome();
TT_SetHome++;
}
if(TT_Magnet == 1)
{
//Serial.println("Dang Hut");
digitalWrite(magnet, 1);
}
if(TT_Magnet == 0)
{
//Serial.println("Dang Nha");
digitalWrite(magnet, 0);
}
51
TÀI LIỆU THAM KHẢO

[1] N. T. Thịnh, Giáo Trình Kỹ Thuật Robot, Trường Đại Học Sư Phạm Kỹ Thuật
TP.HCM.
[2] J. J. Craig, Introduction to Robotics: Mechanics and Control, 2005.
[3] T. T. H. D. G. H. P. T. H. Trang., "Tính toán động học thuận, nghịch và quy
hoạch quỹ đạo cho robot 3 bậc tự do (lý thuyết)," 20 10 2023. [Online]. Available: Tạ
Trung Hậu. Dương Gia Hân. Phan Thị Huyền Trang. Tính toán động học thuận,
nghịch và quy hoạch quỹ đạo cho robot 3 bậc tự do (lý thuyết)
https://www.youtube.com/watch?v=88hcb-GgmfM&t=520s.
[4] N. P. Quang, Matlab và Simulink dành cho kỹ sư điều khiển tự động.
52

Báo Cáo Đ Án Robot

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Báo Cáo Đ Án Robot

Uploaded by

Copyright:

Available Formats

111Equation Chapter 1 Section 1

TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP. HCM

GVHD: TS. Trần Đức Thiện

Thành phố Hồ Chí Minh, Tháng 1 năm 2024

PHỤ LỤC CHƯƠNG TRÌNH MATLAB.................................................................32

DANH MỤC HÌNH ẢNH

DANH MỤC BẢNG

CHƯƠNG 1: TỔNG QUAN ĐỀ TÀI

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

CHƯƠNG 2: THIẾT KẾ ROBOT BẰNG PHẦN MỀM SOLIDWORK

Hình 1. Chuyển đổi đơn vị trong Solidwork

Hình 2. Tổng quan mô hình Robot thiết kế trong Solidworks

Ta sẽ đi vào chi tiết từng khâu như sau:

Hình 3. Hình ảnh mặt cắt và các hình chiếu của đế 1

Hình 4. Hình ảnh mặt cắt và các hình chiếu của đế 2

Hình 5. Hình ảnh mặt cắt và các hình chiếu của đế 3

CHƯƠNG 3: TÍNH TOÁN ĐỘNG HỌC THUẬN, ĐỘNG HỌC NGHỊCH

Ma trận chuyển đổi theo phương pháp J.craig

414\* MERGEFORMAT (.)

2. Động học nghịch

Suy ra ta được: 10110\*

Biến đổi phương trình: 11111\*

Ta có phương trình: 14114\*

Biến đổi : 15115\*

Thay vào ta được: 18118\*

Thay vào ta được: 19119\*

Kết quả: 20120\* MERGEFORMAT (.)

Ta có phương trình: 21121\*

Biến đổi: 22122\*

Ta có được phương trình: 24124\*

Suy ra: 25125\*

Kết quả: 27127\*

Từ phường trình trên ta có được: 28128\*

clc; clear all; close all;

CHƯƠNG 4: LƯU ĐỒ GIẢI THUẬT CỦA HỆ THỐNG

2. Lưu đồ làm việc

CHƯƠNG 5: SƠ ĐỒ KẾT NỐI DÂY CỦA ROBOT

CHƯƠNG 6: THI CÔNG MÔ HÌNH

DANH SÁCH CÁC THIẾT BỊ

3 Dây cắm Kết nối các phần tử trong hệ

4 Ốc M2, Cố định động cơ và các khớp

5 Tán Cố định động cơ và các khớp

6 Bu Lông Cố định động cơ và các khớp

7 Step motor -Đường kính trục 5mm.

8 Step motor - Kích thước: 57x57x54MM

9 Thanh trục - Dùng trong truyền động lên

10 Khớp nối được sử dụng để nối trục động

11 Được dùng để tăng lực căng

12 Dùng để hổ trợ chuyển động

13 Vòng đai -Dùng để truyền chuyển động

14 Pulley -Dùng để truyền chuyển động

15 -Dùng để giữ cố định trục quay

16 -Dòng điện max: 1-9V

1.2 Chi phí các thiết bị

12 Pulley GT2 42.000 4 168.000 https://imaker.vn/pulley-gt2-40-

2. Giao diện hệ thống

Hình 25. Giao diện của hệ thống

CHƯƠNG 7: ĐÁNH GIÁ

PHỤ LỤC CHƯƠNG TRÌNH MATLAB

 Chương trình tính động học nghịch:

P = [- 65*2^(1/2) - 170 0 65*2^(1/2) + 711/2];

c2_2 = (ny * (L4*cosd(theta3_2) + L3) - nx*L4*sind(theta3_2)) /

 Chương trình tính động học thuận tỏng C#:

P = [- 652^(1/2) - 170 0 652^(1/2) + 711/2];

c2_2 = (ny * (L4cosd(theta3_2) + L3) - nxL4*sind(theta3_2)) /

stepsMotor1 = diffDegree1100/3; //(degree/1.8)(Tỉ số truyền)*Microstep

stepsMotor2 = diffDegree2320/9; //(degree/1.8)(Tỉ số truyền)*Microstep

stepsMotor3 = diffDegree3320/3; //(degree/1.8)(Tỉ số truyền)*Microstep

stepsMotor4 = diffDegree4100/3; //(degree/1.8)(Tỉ số truyền)*Microstep