Đồ án tốt nghiệp cơ điện tử

MỤC LỤC
LỜI NÓI ĐẦU...............................................................................................................6
CHƯƠNG 1: TỔNG QUAN VỀ HỆ THỐNG............................................................7
1.1. Tổng quan về hệ thống phân loại sản phẩm.............................................
1.2. Yêu cầu đối với đề tài.........................................................................................
1.3. Thông số cần đạt được.......................................................................................
1.4. Phác thảo và phương pháp thực hiện..........................................................
CHƯƠNG 2: XỬ LÝ HÌNH ẢNH.............................................................................9
2.1. Khái niệm xử lý ảnh............................................................................................
2.1.1. Khái niệm...............................................................................................9
2.1.2. Các vấn đề cơ bản trong hệ thống xử lý ảnh.........................10
2.1.3. Xác định và phân loại màu sắc...................................................................11
2.2. Xử lý ảnh với PYTHON......................................................................................
2.2.1. Giới thiệu PYTHON............................................................................12
2.2.2. Thiết lập giao diện xử lý hình ảnh..............................................12
CHƯƠNG 3: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ..........................................14
3.1

Tính chọn và thiết kế hệ thống băng tải..................................................................

3.1.1

Phân tích yêu cầu đối với hệ thống.........................................................14

3.1.2

Tính chọn thiết bị....................................................................................14

3.2

Tính chọn và thiết kế cánh tay Robot.....................................................................

3.2.1

Phân tích yêu cầu đối với Robot.............................................................16

3.2.2

Tính chọn động cơ vận hành Robot........................................................18

3.2.3

Bài tốn động học thuận Robot..............................................................19

3.2.4

Bài toán động học ngược Robot.............................................................22

3.3

Quỹ đạo của Robot...............................................................................................

CHƯƠNG 4:LỰA CHỌN LINH KIỆN VÀ LẮP ĐẶT..............................................39
1


4.1

Lựa chọn các linh kiện tiêu chuẩn..........................................................................

4.1.1 Bộ điều khiển..............................................................................................41

4.1.2 Thông số linh kiện hệ thống băng tải..........................................................42
4.1.3 Thông số linh kiện cánh tay Robot...........................................................47
4.3 Mơ hình sản phẩm.........................................................................................51
4.2.1 Sơ đồ nguyên lý..........................................................................................51
4.2.2 Sơ đồ giải thuật...........................................................................................52
4.2.3 Sơ đồ đấu nối .............................................................................................53
4.3

Mơ hình sản phẩm..................................................................................................

4.3.1

Tổng quan sản phẩm...............................................................................54

4.3.2

Mơ hình tổng quan cánh tay Robot........................................................55

4.3.3

Mơ hình tổng quan hệ thơng băng tải.....................................................59

4.4

Hình ảnh sản phẩm thực tế.....................................................................................

CHƯƠNG 5: ĐÁNH GIÁ HỆ THỐNG......................................................................64
5.1

Thông số đã đạt được..............................................................................................

5.2

Ưu điểm và nhược điểm.........................................................................................

5.2.1

Ưu điểm..................................................................................................64

5.2.2

Nhược điểm............................................................................................64

5.3

Đánh giá sai số........................................................................................................

KẾT LUẬN..................................................................................................................65
PHỤ LỤC.....................................................................................................................67
TÀI LIỆU THAM KHẢO83

2


CHƯƠNG 1: TỔNG QUAN VỀ HỆ THỐNG
1.1. Tổng quan về hệ thống phân loại sản phẩm
Hệ thống phân loại sản phẩm được sử dụng như một công cụ để kiểm tra chất lượng
sản phẩm hoặc phân loại các sản phẩm đầu ra sau các q trình gia cơng chế tạo sản
phẩm .Mục tiêu của hệ thống cơ điện tử ngoài nâng cao chất lượng cịn phải góp phần
nâng cao năng suất dây chuyền công nghệ, giảm bớt nhân lực và đồng thời cải thiện môi

trường lao động.
Để đáp ứng được các yêu cầu trên chúng em đã xây dựng một hệ thống với chức năng
phân loại sản phẩm theo yêu cầu đặt ra, có khả năng linh hoạt cao và hoạt động ổn định
trong mơi trường thí nghiệm cũng như thực tế. Hệ thống được thiết kế cho ứng dụng vào
quy trình phân loại các sản phẩm có màu sắc khác nhau và sử dụng Camera để đảm bảo
độ chính xác trong quá trình vận hành.
1.2. Yêu cầu đối với đề tài
-

Sử dụng Camera để phát hiện sản phẩm.

-

Sử dụng Python thiết lập giao diện người dùng để hiển thị và xử lý
hình ảnh thu được từ Camera.

-

Thiết kế và điều khiển cánh tay Robot phân loại sản phẩm đã qua
xử lý.

-

Sử dụng Arduino xuất tín hiệu điều khiển cơ cấu chấp hành.

1.3. Thông số cần đạt được
-

Năng suất : 200 sản phẩm/giờ.

-

Diện tích hệ thống: 1200mm x 1000mm.

-

Loại sản phẩm: Khối lập phương 50mm x 50mm x50mm.

-

Quy trình thực hiện: Khi nhận tín hiệu cảm biến, xy lanh sẽ đẩy
phôi vào hệ thống băng tải. Camera nhận diện được sản phẩm đi
qua sẽ gửi tín hiệu điều khiển cánh tay Robot gắp vật phân loại
vào các vị trí cố định đã thiết lập.

-

Hiệu ứng hỗ trợ: LCD hiển thị số sản phẩm đã phân loại và led
thông báo màu đã nhận diện.

1.4. Phác thảo và phương pháp thực hiện
Với mục tiêu đã đề ra, dựa trên các kiến thức đã học và thông tin
tham khảo từ các nguồn tài liệu, nhóm đề tài đã đưa ra một số phương
pháp giải quyết như sau:
-

Nghiên cứu lý thuyết
3

+ Tham khảo các hệ thống đã có trong thực tế và các thiết bị cần
sử dụng.
+ Tìm hiểu các phần mềm hỗ trợ: Python, Arduino, Solidwork,
AutoCad,…
+ Nghiên cứu các đề tài, tài liệu liên quan đến phân loại sản
phẩm, điều khiển Robot,…
+ Tìm hiểu cách xây dựng hệ thống điều khiển và chương trình
điều khiển.
-

Nghiên cứu thực nghiệm
+ Mơ hình hóa, tính tốn tọa độ vật thể qua xử lý Camera, tính
tốn hệ thống cơ khí đảm bảo độ chính xác và độ bền cần thiết
cho hệ thống.
+ Sử dụng phần mềm Solidwork phục vụ thiết kế các bộ phận và
mô phỏng.
+ Sử dụng Arduino và Python để lập trình điều khiển hệ thống.
+ Tính tốn vị trí vật thể trong thực tiễn lập trình tọa độ điều
khiển Robot.

-

Phác thảo hệ thống

Hình 1.1:Sơ đồ phác thảo hệ thống

4


CHƯƠNG 2: XỬ LÝ HÌNH ẢNH

2.1. Khái niệm xử lý ảnh
Trong những năm gần đây, xử lý ảnh (image processing) đã đạt được nhiều thành tựu
và tiến bộ vượt bậc. Trong đó, nhận dạng và phân loại hình ảnh là một trong những lĩnh
vực được theo đuổi một cách tích cực nhất. Ý tưởng cốt lõi của việc nhận dạng và phân
loại hình ảnh là phân tích ảnh từ các dữ liệu thu được bởi các cảm biến hình ảnh như
Camera, Webcam,…Nhờ hệ thống xử lý ảnh, con người đã giảm được khối lượng cơng
việc cũng như tăng sự chính xác trong việc đưa ra các quyết định liên quan đến xử lý ảnh
trên nhiều lĩnh vực: dây chuyền sản xuất tự động, robotic, các hệ thống an ninh,…
Mục đính của đồ án này là trình bày và thực hiện phương pháp nhận dạng và phân loại
sản phẩm theo màu sắc trên dây chuyển sản suất tự động. Chính vì thế, bài báo cáo này sẽ
trình bày lần lượt từ cơ sở lý thuyết xử lý ảnh, nhận dạng vật thể, chương trình Python
với mục đích xử lý và nhận dạng hình ảnh.
2.1.1. Khái niệm
- Xử lý ảnh là quá trình thao tác ảnh đầu vào nhằm cho ra kết quả

mong muốn. Kết quả đầu ra có thể là một ảnh “tốt hơn” hoặc
“một kết luận”.
- Ảnh có thể xem là tập hợp các điểm ảnh (pixel) và mỗi điểm ảnh

được xem là đặc trưng cường độ sáng hay một dấu hiệu nào đó
tại một vị trí nào đó của đối tượng trong không gian.
- Thị giác máy định nghĩa là q trình trích xuất đặc trưng và lấy

thơng tin trong ảnh của đối tượng trong không gian thực.

 Quá trình thị giác máy bao gồm
+ Thu nhận hình ảnh: có thể nhận được qua Camera màu hoặc
đen trắng, đây là loại ảnh tương tự.
+ Tiền xử lý: sau bộ phận thu nhận ảnh, ảnh có thể bị nhiễu hoặc
độ tương phản thấp nên cần đưa vào bộ tiền xử lý để nâng cao

chất lượng ảnh. Chức năng: lọc nhiễu, nhận dạng ảnh.
+ Phân đoạn ảnh: là tách ảnh đầu vào thành các vùng để biểu
diễn, phân tích và nhận dạng ảnh.
+ Biểu diễn ảnh: các vật thể sau khi được phân đoạn có thể
được mơ tả dưới dạng chuỗi các điểm và biểu diễn ảnh thường
được sử dụng khi ta quan tâm đến đặc tính bên trong của vùng
ảnh.
5


+ Nhận dạng và nội suy: là quá trình phân loại vật thể dựa trên
cơ sở các chi tiết mô tả vật thể và nhận dạng ảnh là quá trình
xác định ảnh và quá trình này thu được bằng cách so sánh với
mẫu đã lưu trữ từ trước.
+ Cơ sở tri thức: các quá trình xử lý liệt kê trong hình thức xử lý
ảnh được thực hiện dưới sự giám sát và thực hiện dựa trên cơ
sở các kiến thức về lĩnh vực xử lý ảnh.
2.1.2. Các vấn đề cơ bản trong hệ thống xử lý ảnh
a) Điểm ảnh (pixel)
Điểm ảnh được xem là dấu hiệu hay cường độ sáng tại một độ trong không gian đối
tượng và ảnh được xem là 1 tập hợp các điểm ảnh.
Để biểu diễn ảnh, chúng ta thưởng dùng các phần tử đặc trưng của ảnh là pixel. Do đó ta
có thể biểu diễn một ảnh bởi một hàm 2 biến chứa thông tin. Các mơ hình biểu diễn ảnh
cho ta một mơ tả logic hay định lượng các tính chất của hàm này. Chất lượng ảnh hoặc
tính hiệu quả của các kỹ thuật xử lý phụ thuộc vào nhiều yếu tố như: độ phân giải, nhiễu,
…
b) Ngưỡng (Thresholding)
Một trong những vấn đề quan trọng trong hệ thống thị giác máy là
xác định ảnh nền để diễn tả đối tượng. Cơng việc này có thể dễ đối với
con người nhưng lại khó cho máy tính.

Nếu giá trị cường độ sáng của một đối tượng nằm trong một khoảng và giá trị cường
độ sáng của các điểm ảnh nền nằm bên ngồi khoảng đó, thì ảnh nhị phân có thể được
hình thành bằng cách sử dụng thao tác ngưỡng.
Thao tác ngưỡng là phương pháp chuyển đổi ảnh xám thành ảnh nhị phân nhằm tách
đối tượng đích ra khỏi ảnh nền theo ý muốn.
Thực tế, nhiều Camera được thiết kế để thực hiện việc phân ngưỡng bằng phần cứng
và tạo ra ảnh nhị phân. Tuy nhiên, trong hầu hết các ứng dụng thì Camera tạo ra ảnh xám
và ảnh nhị phân bằng cách thực hiện thao tác ngưỡng.
c) Phân loại ảnh
- Ảnh màu (Color image)
- Ảnh xám (Gray image)
- Ảnh nhị phân (Binary image)

d) Mức xám (Gray level)
Mức xám là số các giá trị có thể có của các điểm ảnh trong ảnh. Nếu
từng điểm ảnh có 8 bit xám thì sẽ có 256 giá trị (0 – 255) trong đó có 0
là màu đen, 255 là màu trắng.
6


e) Các mơ hình màu
Mơ hình màu là một chỉ số kỹ thuật của một hệ tọa độ màu 3 chiều
và tập các màu nhỉ thành phần có thể trơng thấy được trong các hệ
thống tọa độ màu thuộc một gam màu đặc trưng. Các mơ hình màu cơ
bản :RGB, HSV, HLS,…

 Mơ hình màu RGB: Đỏ - Lục – Lam
Là một đơn vị tập các màu thành phần sắp xếp theo hình lập
phương của hệ trục tọa độ Đề các.

Hình 2.1:Mơ hình màu RGB
Đường chéo chính của khối lập phương với sự cân bằng về số lượng
từng màu gốc tương ứng với các mức độ xám : đen là (0,0,0) và trằng
(1,1,1).

 Mơ hình màu HSV: Hue - Saturation – Value

Hình 2.2:Mơ hình màu HSV
- Hệ thống tọa độ có dạng hình trụ và tập màu thành phần của
khơng gian bên trong mơ hình màu được xác định là hình nón
hoặc là hình chóp sáu cạnh.

7


- Thông số sắc màu H (Hue) được đo bời góc quanh trục thẳng đứng
với màu đỏ là Red 00 , màu xanh lục là Green 120 0 , màu xanh lam
là Blue 2400.
- Giá trị của thông số S (Saturation) là một tập các giá trị đi từ 0 trên
đường trục tâm (trục V) đến 1 trong các mặt bên tại đỉnh của
hình chóp sáu cạnh.
- Mơ hình màu có dạng hình chóp sáu cạnh này có đường cao V với
đỉnh là điểm gốc tọa độ (0,0).
- Điểm ở đỉnh là Black và có giá trị tọa độ màu V=0.
- Khi điểm có V=1 và S=0 là điểm White.
2.1.3. Xác định và phân loại màu sắc
Để xác định đối tượng mong muốn sử dụng phương pháp ngưỡng để tách đối tượng
dựa vào giá trị cường độ sáng ra khỏi ảnh nền theo mong muốn.
Sử dụng thuật toán phân loại bằng mơ hình màu HSV.
2.2. Xử lý ảnh với PYTHON

2.2.1. Giới thiệu PYTHON

Hình 2.3:Python
PYTHON là ngơn ngữ lập trình bậc cao cho các mục đích lập trình đa năng. Python
được thiết kế với ưu điểm mạnh là dễ đọc, dễ học và dễ nhớ. Đồng thời nó là mơi trường
tương tác có thể thực hiện nhiều nhiệm vụ: phát triển giải thuật, phân tích dữ liệu, tính
tốn các phép tính số học và hình ảnh hóa dữ liệu. So với các ngơn ngữ lập trình truyền
thống như C, C++ hay PLC thì Python có nhiều ưu điểm hơn nhờ tốc độ lập trình và thư
viện lớn, đa dạng hỗ trợ.
PYTHON được sử dụng trong nhiều lĩnh vực ứng dụng bao gồm: phân
tích dữ liệu, khoa hoc dữ liệu /AI, phát triển web, phát triển phần mềm,
kỹ thuật dữ liệu, giao diện và điều khiển phần cứng,… Ngồi ra,
PYTHON cịn được tích hợp các hộp cơng cụ (mỗi hộp gồm hệ thống các
8


hàm có những chức năng đặc biệt của PYTHON) nhằm giải quyết các
lớp vấn đề cụ thể trong các lĩnh vực ứng dụng khác nhau.
Tất cả các đặc tính về hình ảnh cần thiết cho hình ảnh hóa các dữ liệu
khoa học kỹ thuật đều được tính hợp trong PYTHON. Những đặc tính
này bao gồm: các hàm vẽ ảnh 2 và 3 chiều, các hàm vẽ thể tích 3
chiều, các công cụ tương tác tạo ảnh, tạo video, khả năng xuất kết quả
dưới các dạng file ảnh thông thường.
2.2.2. Thiết lập giao diện xử lý hình ảnh
GUI (Graphical User Interface) là giao diện người dùng đồ họa. Nghĩa là khi hệ thống
hoạt động chúng ta sẽ sử dụng chuột để thao tác lên các thanh công cụ đã được lập trình
sẵn. Nó cho phép chúng ta tương tác giao diện chương trình từ đó có được góc nhìn
khách quan nhất và sử dụng code để tương tác và cài đặt giao diện.
Để xác định đối tượng mong muốn sử dụng phương pháp ngưỡng để tách đối tượng
dựa vào giá trị cường độ sáng ra khỏi ảnh nền theo mong muốn. Sau khi thu thập hình

ảnh sẽ sử dụng thuật tốn chuyển đổi ảnh từ mơ hình màu RGB sang mơ
hình màu HSV, từ các thông số cường độ sáng đã cài đặt từ trước máy
tính sẽ so sánh và phân loại các khối vật có màu tương ứng.

9


Hình 2.4:Xử lý ảnh

10


CHƯƠNG 3:THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
3.1 Tính chọn và thiết kế hệ thống băng tải
3.1.1 Phân tích yêu cầu đối với hệ thống
Yêu cầu đặt ra khi thiết kế hệ thống bao gồm cơ cấu cấp phôi tự động và phát hiện
vật thể sử dụng Camera.
Mơ hình theo sát với yêu cầu thực tế sử dụng trong các nhà máy và các xưởng
phân loại đó là điều khiển bằng bảng điều khiển hệ thống sẽ tự động cấp sản phẩm vào vị
trí phân loại, hệ thống cấp phơi sẽ bao gồm cảm biến phát hiện phôi và các xy lanh cấp
phôi vào hệ thống.
Phần phân loại phôi theo màu sắc, sử dụng một cảm biến quang để phát hiện vật
thể đồng thời sử dụng Camera và cánh tay robot để đưa phơi vào đúng vị trí chứa phơi
theo đúng màu sắc.
3.1.2 Tính chọn thiết bị
 Tính chọn Xy lanh
- Thơng số của xy lanh M10x60 mm, đường kính trục piston ∅4mm.
- Phơi có khối lượng khoảng: 40g đến 50g.
- Cơng thức tính lực đẩy của xy lanh khi piston đi ra :
F A = A1∗p e

1

1

Với:
F A : Lực tác động khi cần piston đi ra.
1

A1: Diện tích làm việc của piston phía khoang piston.

A 1=

2

2

π D π1
=
=0.79 cm 2
4
4

D: Đường kính mặt đáy piston (D=10 mm).
pe : Áp suất khí nén trong xy lanh.
1

pe = 0.4 MPa≈ 4.08 kg/cm2
1

- Lực tác dụng khi cần piston đi ra là: F A = 0.79*4.08≈ 3.22(N)

1

(thỏa mãn điều kiện)
- Khi piston đi vào có cơng thức tính lực:
F A = A 2∗pe
2

Với
F A : Lực tác động khi cần piston đi vào.
2

11

2


A2 : Diện tích làm việc của piston phía khoang piston.
2

2

A2= ( π∗( D −d ))
4

=

( π∗( 12−0.4 2) )
=
0.66 cm2
4

D: Đường kính mặt đáy piston (D=1.6 cm).
d: Đường kính cần piston (d=1.6 cm).
- Lực tác dụng khi cần piston đi vào là:
F A = 0.66*4.08 = 2.70 (N)
2

 Tính chọn băng tải
-

Chiều dài băng tải: 780mm

-

Chiều rộng băng tải: 60mm

-

Chiều cao băng tải so với mặt đất: 100mm

-

Đường kính rulo: 30mm

-

Dẫn động: Động cơ giảm tốc ZS-RE81

-

Vận tốc băng tải: v = 0,024 m/s

-

Trong 1 thời điểm có 4 vật trên băng tải, khối lượng một vật ước tính và
khoảng 50g=0.05kg, kích thước sản phẩm 50*50(mm).
- Băng tải nhựa PVC có tổng chiều dài 1.6m, rộng 0.06m, dày 1.5mm.

S(băng tải) = 0.06*1.6 = 0.096 m2
Với khối lượng riêng của băng tải nhựa PVC là 3.2 kg/m2.

Tổng tải = m(vật) + m(băng tải) = 0.30 + 0.05*4 = 0.5kg
Hệ số ma sát giữa băng tải nhựa PVC và con lăn nhựa PE là 0.25.
Hệ số truyền dẫn bộ truyền đai là : 0.95.
Năng suất của hệ số trên ta tính chọn động cơ như sau:
V=

( 40∗4+250∗3)
=15.2 (mm/s)
(27∗π )

N2

=

(15.2∗60)
≈ 11
π∗27

1200

i= 11 =110
- Chọn tỉ số truyền theo catalog của động cơ ZYTD loại ZS-RE81 là i=100, động cơ
ZS-RE81 có tốc độ 6000v/ph qua hộp giảm tốc còn 60v/ph.
 Momen cần thiết để kéo băng tải:
F = mg(μcos(0))=0.5*9.81*025=1.23 (N)
12


TL

=

FD 1.23∗0.025
2 ηi = 2∗0.095∗1 =0.017 Nm

Chọn hệ số an tồn SF=2 ta có:
SF

T L =0.017*2 = 0.034 Nm

 Momen sinh ra ở động cơ
TM

=

SF

TL
iηg

=

0.034∗1000
120∗0.98 = 0.29 Nm

 Momen quán tính tổng hợp

J = 2 J CL + J m
- Khối lượng con lăn khoảng 80g ≈ 0.08kg.
J m = m(
J CL

=

2
A2
D
¿
= 0.5* = 9.11*10−5 kg.m2
2
4
4π

1
1
2
m D = *0.08*0.027 2 = 7.29*10−6 kg.m 2
8
8

J = 9.11*10−5 + 7.29*10−6 = 9.84*10−5 kg.m2
 Chọn động cơ ZS-RE81 phù hợp với yêu cầu của hệ thống.
3.2 Tính chọn và thiết kế cánh tay Robot
3.2.1 Phân tích yêu cầu đối với Robot
- Yêu cầu đặt ra khi lựa chọn và thiết kế Robot cần phải có vùng hoạt động linh hoạt, phù
hợp với dây chuyền sản xuất hàng loạt.
- Với sự tham khảo các Robot thực tế chúng em đã thiết kế một robot chuỗi 4 bậc tự do
R-R-R-R. Thiết kế với kết cấu đơn giản với các khớp quay linh hoạt, hạn chế số khâu để
vẫn đảm bảo yêu cầu đã đặt ra khi làm việc.
Đặc điểm Robot:
+ Nhiệm vụ: Gắp vật trên băng tải về các vị trí cố định sau khi phân loại.
+ Chuyển động: Qua tín hiệu điều khiển Robot sẽ điều chỉnh các khớp quay di
chuyển tay kẹp gắp vật từ băng tải sang các vị trí hộp chứa sản phẩm đã được phân loại.
+Vị trí làm việc: Bên trái băng tải.
+ Tầm với: 440mm.
+ Cấu tạo:
 Số bậc tự do: 4.
 Số khâu: 4
 Khâu 1: d1=227.5mm; a1=90mm.
13


 Khâu 2: a2=130mm.
 Khâu 3: a3=130mm.
 Khâu 4: a4=90mm.
 Số khớp: 4
 Khớp 1 xoay 340 độ.
 Khớp 2 xoay 210 độ.
 Khớp 3 xoay 220 độ.

 Khớp 4 xoay 220 độ

Hình 3.1:Phác thảo cánh tay Robot
 Phác thảo hệ thống

14


Hình 3.2:Phác thảo hệ thống
3.2.2 Tính chọn động cơ vận hành Robot
 Khớp 2:
 Lực tác dụng : F = m . g (giá trị max, bỏ qua ma sát):
= 1,2 . 9,81
= 11,772 (N)
 Tỉ số truyền động cơ: u = 1/9
 Momen xoắn: T = F . d
= 11,772. 260
= 3060,72(Nm) =3,06(Nmm)
 Momen xoắn tại động cơ: Tđc = T.u
= 3060,72. 1/9
= 340.08(Nmm) =0,34 (Nm)
 Chọn động cơ StepMotor Nema17 có momen xoắn 588 Nmm >340 Nmm.
Thơng số kỹ thuật:
• Điện áp sử dụng: 3,9V/5V
• Dịng hoạt động: 1,2A
• Momen xoắn: 580Nmm
• Kích thước: 42x42x38 mm
• Kích thước trục: 5x20mm
15

•
•

Bước: 1,80
Tốc độ quay: 80 vòng/s

 Khớp 3 (khớp 4 chọn cùng loại):
 Lực tác dụng : F = m . g (giá trị max, bỏ qua ma sát):
= 0,6 . 9,81
= 5,886 (N)
 Tỉ số truyền động cơ: u = 1/6
 Momen xoắn: T = F . L
= 5,886. 130
= 765,18(Nmm) =0,765 (Nm)
 Momen xoắn tại động cơ: Tđc = T.u
= 765,18. 1/6
= 127,53(Nmm) =0,127 (Nm)
 Chọn động cơ StepMotor Nema17 có momen xoắn 588 Nmm >127 Nmm.
Thơng số kỹ thuật:
• Điện áp sử dụng: 3,9V/5V
• Dịng hoạt động: 1,2A
• Momen xoắn: 580Nmm
• Kích thước: 42x42x38 mm
• Kích thước trục: 5x20mm
• Bước: 1,80
• Tốc độ quay: 80 vịng/s
3.2.3 Bài toán động học thuận Robot
Các hệ tọa độ:

16


Hình 3.3:Hệ tọa độ Robot
Để chuyển từ hệ Oi-1xi-1yi-1zi-1 sang hệ Oixiyizi là thực hiện liên tiếp 4 phép biến đổi toạ độ.
Bảng D-H:
Khớp

R

O0 - O 1

(1)

d1

a1

900

O1 - O 2

(2)

0

a2

0

O2 - O 3

(3)

0

a3

0

O3 - O 4

(4)

0

a4

0

T

z,

Pi-1 = R

z,i

.T

Aii-1(qi)= R

z, di

z,i

T

z,d

.T

.T

x, ai

z, di

.R

.T

x, ai

x,i

x,a

. Pi

.R

x,i

Ma trận biến hình tổng hợp của các phép quay trên: Aii-1:
Ai = Rotz,i. Transz, di. Transx, ai. Rotx,i= A

[

Ai =

C❑ −S α .C θ Sα . Sθ ai .C ❑
S❑ C❑ . Cθ −C α . S θ a i . S ❑
0 Sθ C θ di
0001
i

i

i

i

i

i

Sα = sin α ; Sθ = sin θ
i

i

i

i

i

i

i

17

i

i

i

i

i

Trong đó: C❑ = cos❑ ; C θ = cos θ
i

i

i

i

]

R

x,β


Thay các giá trị trong bảng D-H vào công thức trên ta được các ma
trận Ai như sau:

0

[

=

A1

1

A2

2

A3

A

3
4

C α 1 0 Sα 1 a 1C α 1
S α 1 0−C α 1 a 1 S α 1
010d1
0 00 1

]

=

[

Cα 2−S α 2 0 a 2C α 2
Sα 2C α 20 a 2 Sα 2
0 0 10
0001

=

[

Cα 3−S α 3 0 a 3 C α 3
S α 3 C α 3 0 a 3 Sα 3
0010
0001

=

[

C❑ −S α 0 a 4 C α 4
S ❑ Cα 0 a 4 S α 4
0010
0001
4

4

4

4

]
]

]

Hệ phương trình động học xác định bởi:

0

0

1

2

3

A 4 = A1 . A2 . A3 . A 4 =

[ ]
n x s x ax P x
ny s y ay P y
nz s z az P z
0001

Ma trận cosin chỉ phương biểu diễn vị trí và hướng giữa O 0 và O4:

[

cos ⁡( x 0 , x 4 ) cos ⁡(x0 , y 4 ) cos ⁡( x 0 , z 4 ) P x
cos ⁡( y 0 , x 4 ) cos ⁡( y 0 , y 4 ) cos ⁡( y 0 , z 4 ) P y
AE =
cos ⁡( z 0 , x 4 )cos ⁡(z 0 , y 4 )cos ⁡( z 0 , z 4) P z
0001

18

]


nx = cos(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3)) sin(q4)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))
sx = - cos(q4)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2)) sin(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))
ax = sin(q1)
ny = - cos(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)) sin(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))
sy = sin(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)) cos(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))

ay = -cos(q1)
nz = cos(q4)*(cos(q2)*sin(q3) + cos(q3)*sin(q2)) +
sin(q4)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))
sz = cos(q4)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) sin(q4)*(cos(q2)*sin(q3) + cos(q3)*sin(q2))
az = 0
Px = a1*cos(q1) + a2*cos(q1)*cos(q2) +
a4*cos(q4)*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3)) a4*sin(q4)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2)) +
a3*cos(q1)*cos(q2)*cos(q3) - a3*cos(q1)*sin(q2)*sin(q3)
Py = a1*sin(q1) + a2*cos(q2)*sin(q1) a4*cos(q4)*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)) a4*sin(q4)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)) +
a3*cos(q2)*cos(q3)*sin(q1) - a3*sin(q1)*sin(q2)*sin(q3)
Pz = d1 + a2*sin(q2) + a4*cos(q4)*(cos(q2)*sin(q3) +
cos(q3)*sin(q2)) + a4*sin(q4)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) +
a3*cos(q2)*sin(q3) + a3*cos(q3)*sin(q2)
3.2.4 Bài toán động học ngược Robot
Từ ma trận A04 và AE có hệ phương trình động học ngược:

{

( Px−a 14 )2=0
2
( Py−a 24 ) =0
( Pz−a 34 )2=0
( Sx−a 31 )2=0

Với các a14, a24, a34, a11 là các tọa độ thực đã biết.
Giải bài toán trên EXCEL:
- B1: Nhập các dữ liệu cần thiết để tính tốn:
19

+ Dữ liệu về Px, Py, Pz, Sx.
+ Khởi tạo các biến khớp q1, q2, q3, q4, đều bằng 0.
+ Khởi tạo các giá trị trong ma trận: a14, a24, a34, a11 đều bằng 0.
+ Chọn giá trị các thông số: d1 = 227.5mm, a1 = 90mm, a2 = 130mm, a3 =
130mm, a4 = 90mm.
- B2: Tính các giá trị L và tổng:
L1 = (Px - a14)^ 2
L2 = (Py - a24)^ 2
L3 = (Pz - a34)^ 2
L4 = (Sx – a31)^ 2
L = L1+ L2 + L3+ L4

Hình 3.4: Khởi tạo giá trị ban đầu.
- B3: Dùng Solver tìm q1, q2, q3, q4:
Chọn gốc tọa độ Oxyz tại khớp 1 của Robot:
Điểm P0: vị trí và hướng làm việc của tay kẹp tại thời điểm ban đầu

[ ]

… 0 … 265
………0
P0 =
… … … 200
0001

Điểm P1:

[ ]

… 0 … 265

………0
P1 = … … … 150
0001

20