PLC ĐIỀU KHIỂN ROBOT DELTA PHÂN LOẠI SẢN PHẨM THEO MÀU

BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƯỜNG ĐẠI HỌC
KHOA ĐIỆN – ĐIỆN TỬ

ĐỒ ÁN TỐT NGHIỆP
Chuyên ngành Điện Công Nghiệp

ĐỀ TÀI: ỨNG DỤNG PLC ĐIỀU KHIỂN ROBOT
DELTA PHÂN LOẠI SẢN PHẨM THEO MÀU

Giáo viên hướng dẫn:
Sinh viên thực hiện:

TP. HỒ CHÍ MINH


TRƯỜNG ĐH SPKT TP. HỒ CHÍ MINH

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

KHOA ĐIỆN-ĐIỆN TỬ

ĐỘC LẬP - TỰ DO - HẠNH PHÚC

BỘ MÔN ĐIỆN CÔNG NGHIỆP

----o0o----

NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP
Họ tên sinh viên: MSSV:
MSSV:
Chuyên ngành:

Điện Công nghiệp

Mã ngành:

Hệ đào tạo:

Đại học chính quy

Mã hệ:

Khóa:

Lớp:

I. TÊN ĐỀ TÀI : ỨNG DỤNG PLC ĐIỀU KHIỂN ROBOT DELTA PHÂN LOẠI
SẢN PHẨM THEO MÀU

II. NHIỆM VỤ
1. Các nguyên liệu ban đầu:
CPU PLC S7-1200 1212 DC/DC/DC, Module mở rộng SM 1222 RLY, Web
camera 5MP, Hệ thống băng tải, Bộ sản phẩm mẫu với 3 màu xanh lá, xanh lam, đỏ.
2. Các nội dung cần thực hiện:
Thi công hệ robot delta, Thi công tủ điều khiển, Xây dựng chương trình điều khiển
ở PLC, Xây dựng chương trình xử lý ảnh, Xây dựng ứng dụng điều khiển liên kết PLC
và xử lý ảnh.
III. NGÀY GIAO NHIỆM VỤ: 20/9
IV. NGÀY HOÀN THÀNH NHIỆM VỤ:15/1
V. CÁN BỘ HƯỚNG DẪN:
CÁN BỘ HƯỚNG DẪN

BM. ĐIỆN CÔNG NGHỆP


TRƯỜNG ĐH SPKT TP. HỒ CHÍ MINH

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

KHOA ĐIỆN-ĐIỆN TỬ

ĐỘC LẬP - TỰ DO - HẠNH PHÚC

BỘ MƠN ĐIỆN CƠNG NGHIỆP

----o0o----

LỊCH TRÌNH THỰC HIỆN ĐỒ ÁN TỐT NGHIỆP
Họ tên sinh viên 1:
Lớp: 16………………………………………………………………………….
Họ tên sinh viên 2:
Lớp: 16………………………………………………………………………….
Tên đề tài: Ứng Dụng Plc Điều Khiển Robot Delta Phân Loại Sản Phẩm Theo Màu
Tuần

Nội dung

1-2

Thực hiện chọn đề tài

3

Gặp GVHD để phổ biến quy định

4-5

Nguyên cứu đề tài tìm tài liệu về đề tài.

6

Xây dựng đề cương chi tiết.

7-8

Tìm hiểu và thu thập cơ sở lý thuyết.

9-11

Tiến hành thi cơng phần cứng.

12-14

Xây dựng chương trình điều khiển.

15

Nạp chương trình và hiệu chỉnh mơ hình.

16-17

Xác nhận GVHD

Soạn thảo báo cáo, Báo cáo đề tài tốt
nghiệp.

GIÁO VIÊN HƯỚNG DẪN
(Ký và ghi rõ họ tên)


LỜI CẢM ƠN
Lời đầu tiên, chúng em xin cảm ơn quý thầy cô Trường …đặc biệt là các thầy cô
trong khoa Điện- Điện tử và Bộ môn Điện công nghiệp đã tận tình và hết lịng truyền

đạt những kiến thức cho chúng em trong suốt quãng thời gian 4 năm chúng em học tập
tại trường.
Chúng em xin gửi lời cảm ơn chân thành đến thầy. Người thầy đã hết lòng giúp đỡ
và theo sát chúng em trong quá trình thực hiện và hoàn thành luận văn này.
Chúng em cũng xin gửi lời cảm ơn đến các bạn trong lớp 16. Những người bạn đã
cùng học và giúp đỡ lẫn nhau trong suốt quá trình học tập và luận văn cuối khóa.
Cuối cùng, chúng em xin gửi lời cảm ơn đến thầy cô phản biện và các thầy cô trong
hội động bảo vệ luận văn đã dành thời gian để nhận xét và giúp đỡ chúng em trong quá
trình bảo vệ luận văn.
Lời cuối, chúng em xin gửi lời chúc sức khỏe đến các thầy cô Trường …, đặc biệt
là các thầy cô trong khoa Điện- Điện tử và bộ môn Điện công nghiệp.
TP. HCM, ngày …. tháng …. năm 2021

Sinh viên


MỤC LỤC
CHƯƠNG 1: TỔNG QUAN VỀ ĐỀ TÀI .......................................................................1
1.1. Giới thiệu Robot Delta .........................................................................................1
1.2. Mục tiêu, nhiệm vụ đề tài .....................................................................................3
1.3. Phương án .............................................................................................................3
1.4. Các bước tiến hành ...............................................................................................3
CHƯƠNG 2: CƠ SỞ LÝ THUYẾT ................................................................................5
2.1. Phương trình động lực học Robot Delta ...............................................................5
2.1.1. Cấu tạo robot song song Delta ......................................................................5

2.1.2. Động học thuận Robot ...................................................................................5
2.1.3. Động học nghịch Robot .................................................................................8
2.2. Tổng quan về công nghệ xử lý ảnh ....................................................................11
2.2.1. Các khái niệm của xử lí ảnh ........................................................................11
2.2.2. Các vấn đề cơ bản trong xử lý ảnh ..............................................................13
2.2.3. Những vấn đề lý thuyết được sử dụng trong đề tài .....................................15
2.3. Giới thiệu phần cứng ..........................................................................................19
2.3.1. PLC S7-1200 (1212C DC/DC/DC) .............................................................19
2.3.2. Sơ đồ kết nối của PLC S7-1200 ..................................................................20
2.3.3. Module mở rộng SM 1222, DQ 8xRelay/2A (6ES7222-1HF32-0XB0) ....21
2.3.4. Động cơ bước ..............................................................................................23
2.3.5. Driver Điều Khiển Động Cơ Microstep TB6600 ........................................24

2.3.6. Công tắc hành trình .....................................................................................25
2.3.7. Encoder ........................................................................................................26
2.4. Phần mềm điều khiển Robot...............................................................................26
2.4.1. Thư Viện EmguCV......................................................................................27
2.4.2. Phần mềm TIA Portal V15 ..........................................................................27
2.4.3. OPC và SQL ................................................................................................38
CHƯƠNG 3: THUẬT TOÁN ĐIỀU KHIỂN ...............................................................40
3.1. Thuật tốn điều khiển cho PLC ..........................................................................40
3.1.1. Chương trình điều khiển robot delta............................................................40
3.1.2. Chương trình tính tốn tốc độ động cơ băng tải ..........................................42
3.2. Chương trình điều khiển cho ứng dụng Winform ..............................................43

3.2.1. Giao diện điều khiển ....................................................................................43
3.2.2. Chương trình nhận diện màu và xác định tọa độ.........................................44
3.2.3. Điều khiển robot tự động gắp vật: ...............................................................47
3.2.4. Sắp xếp vật theo thứ tự trên băng tải ...........................................................47
3.2.5. Phương pháp chuyển đổi tọa độ khung hình sang tọa độ của hệ Robot......48
3.3. Kết nối giữa PC-PLC-SQL .................................................................................49
CHƯƠNG 4: THI CƠNG MƠ HÌNH ...........................................................................51
4.1. Thi công phần cứng ............................................................................................51
4.1.1. Chuẩn bị phần cứng .....................................................................................51
4.1.2. Thi công robot delta và tủ điều khiển ..........................................................52
4.2. Thi công phần mềm ............................................................................................57

CHƯƠNG 5: ĐÁNH GIÁ KẾT QUẢ VÀ NHẬN XÉT ..............................................59
5.1. Kết quả ................................................................................................................59
5.1.1. Kết quả xử lý ảnh.........................................................................................59
5.1.2. Kết quả robot gắp vật ..................................................................................59
5.2. Nhận xét ..............................................................................................................60
5.2.1. Những kiến thức đã nguyên cứu..................................................................60
5.2.2. Những hạng mục đạt được ..........................................................................61
5.2.3. Những hạn chế .............................................................................................61
5.2.4. Hướng phát triển ..........................................................................................61


DANH MỤC HÌNH

Hình 1.1 Robot Delta cơng đoạn đóng gói sản phẩm nhà máy Vinamilk ....................... 1
Hình 1.2 Robot Delta trong cơng nghiệp ........................................................................... 2
Hình 1.3 Robot Delta sử dụng để nâng kính giúp bác sĩ phẫu thuật ................................ 2
Hình 1.4 Sáng kiến SV ứng dụng robot Delta diệt cỏ loại sâu ......................................... 3
Hình 2.1 Cấu tạo cơ bản của Robot Delta.......................................................................... 5
Hình 2.2 Mơ hình động học robot delta ............................................................................. 6
Hình 2.3 Mơ hình động học robot delta ............................................................................. 6
Hình 2.4 Tọa độ các điểm J1′, J2′, J3′ trên mặt phẳng Oxy ............................................. 7
Hình 2.5 Mơ hình động học robot delta với các thơng số cho trước ................................ 8
Hình 2.6 Minh họa động học của cánh tay đòn ................................................................. 9
Hình 2.7 Mặt phẳng YZ ...................................................................................................... 9
Hình 2.8 Quy đổi tọa độ .................................................................................................... 10

Hình 2.9 Biểu diễn ảnh dưới dạng ma trận ...................................................................... 11
Hình 2.10 Ảnh pixel 8 bit ................................................................................................. 12
Hình 2.11 Ảnh mức xám ................................................................................................... 12
Hình 2.12 Ảnh theo mã màu RGB ................................................................................... 13
Hình 2.13 Ảnh theo mã màu RGB ................................................................................... 13
Hình 2.14 Ảnh màu và ảnh đa mức xám.......................................................................... 13
Hình 2.15 Chuyển ảnh bằng ma trân điểm ...................................................................... 14
Hình 2.16 Minh họa tìm đường biên bằng bộ lọc Laplacian, Sobel .............................. 15
Hình 2.17 Dải màu HSV ................................................................................................... 15
Hình 2.18 Mơ tả phép giãn ảnh ........................................................................................ 16
Hình 2.19 Mơ tả phép tốn co .......................................................................................... 16
Hình 2.20 Hình minh họa phép co dãn ảnh ..................................................................... 17

Hình 2.21 Kết quả tìm Contours....................................................................................... 17
Hình 2.22 Mơ tả thuật tốn Approximation ..................................................................... 19
Hình 2.23 Hình ảnh của PLC 1212C DC/DC/DC ........................................................... 20
Hình 2.24 Sơ đồ chân của PLC ........................................................................................ 20
Hình 2.25 Hình các loại module mở rộng S7-1200 ........................................................ 22
Hình 2.26 Datasheet của module SM 1222 ..................................................................... 22


Hình 2.27 Động cơ bước ................................................................................................... 23
Hình 2.28 Hình minh hoạt thứ tự cấp điện các cuộn dây và các bước quay ................. 24
Hình 2.29 Driver điều khiển động cơ bước TB6600....................................................... 24
Hình 2.30 Cơng tắc hành trình KW11 ............................................................................. 25

Hình 2.31 Cấu tạo chính Encoder .................................................................................... 26
Hình 2.32 So sánh tốc độ của các thư viện xử lý ảnh ..................................................... 27
Hình 2.33 Cho phép phát xung ......................................................................................... 28
Hình 2.34 Lựa chọn chế độ phát xung PTO .................................................................... 28
Hình 2.35 Xung điều khiển di chuyển và hướng ............................................................. 29
Hình 2.36 Gán biến xuất xung .......................................................................................... 29
Hình 2.37 Cấu trúc điều khiển sử dụng technology object ............................................. 30
Hình 2.38 Điều khiển cơ cấu sử dụng Technology Object ............................................. 30
Hình 2.39 Add new Technology object ........................................................................... 31
Hình 2.40 Thiết lập bộ phát xung cho Technology object .............................................. 31
Hình 2.41 Thiết lập động lực bằng phần mềm ................................................................ 31
Hình 2.42 Chức năng Jerk Limit vơi khả năng làm mịn ................................................. 32

Hình 2.43 Khối MC_Power .............................................................................................. 33
Hình 2.44 Khối MC_Reset ............................................................................................... 33
Hình 2.45 Khối MC_Home .............................................................................................. 34
Hình 2.46 Khối MC_Halt ................................................................................................. 35
Hình 2.47 Khối MC_MoveAbsolute ................................................................................ 36
Hình 2.48 Khối MC_MoveRelative. ................................................................................ 37
Hình 2.49 Khối MC_MoveJog ......................................................................................... 37
Hình 2.50 Ứng dụng Kepserver ........................................................................................ 39
Hình 3.1 Sơ đồ hoạt động của các chương trình con của PLC ....................................... 40
Hình 3.2 Lưu đồ điều khiển PLC với 3 nút nhấn START, STOP, RESET được tạo bởi
WinForm. ........................................................................................................................... 40
Hình 3.3 Khối Power......................................................................................................... 41

Hình 3.4 Khối Homing...................................................................................................... 41
Hình 3.5 Khối điều khiển vị trí động cơ bước ................................................................. 41
Hình 3.6 Khối điều khiển bằng tay................................................................................... 42


Hình 3.7 Lưu đồ tính tốn tốc độ động cơ băng tải......................................................... 42
Hình 3.8 Khối điều khiển HSC ......................................................................................... 43
Hình 3.9 Lưu đồ điều khiển chương trình điều khiển Robot .......................................... 43
Hình 3.10 Lưu đồ xử lý ảnh .............................................................................................. 44
Hình 3.11 Ảnh vật qua chuyển đổi HSV ......................................................................... 45
Hình 3.12 Ảnh sau khi nhị phân hóa theo ngưỡng dùng hàm InRange ......................... 45
Hình 3.13 Ảnh qua 1 lần co dãn ảnh ................................................................................ 46

Hình 3.14 Ảnh qua 2 lần co dãn ảnh ................................................................................ 46
Hình 3.15 Lưu đồ xử lý ảnh .............................................................................................. 47
Hình 3.16 Thuật tốn nhận diện vật theo thứ tự trên băng tải ........................................ 48
Hình 3.17 Minh họa trục tọa độ gốc robot và trục tọa độ vật ......................................... 48
Hình 3.18 Sơ đồ kết nối giữa PC-PLC-SQL thông qua OPC KepServer ...................... 49
Hình 4.1 Hình ảnh thiết kế 3D sử dụng phần mềm Fusion 360 ..................................... 52
Hình 4.2 Các linh kiện của Robot Delta sau khi in 3D ................................................... 52
Hình 4.3 Ráp khung Robot ............................................................................................... 53
Hình 4.4 Sơ đồ đấu nối PLC điều khiển động cơ ............................................................ 53
Hình 4.5 Mơ hình sử dụng Stepmotor và driver TB6600 ............................................... 54
Hình 4.6 Vị trí lắp đặt cơng tắc hành trình ...................................................................... 54
Hình 4.7 Sơ đồ kết nối module SM-1222 với băng tải và bơm ...................................... 54

Hình 4.8 Bơm và động cơ băng tải ................................................................................... 55
Hình 4.9 Sơ đồ kết nối encoder với PLC ......................................................................... 55
Hình 4.10 Hình ảnh Encoder trong mơ hình.................................................................... 55
Hình 4.11 Tủ điều khiển Robot Delta, băng tải và bơm sử dụng nguồn 24V và 12V .. 56
Hình 4.12 Mơ hình thực tế ................................................................................................ 56
Hình 4.13 Trang điều khiển kết nối.................................................................................. 57
Hình 4.14 Trang điều khiển chế độ Manual .................................................................... 57
Hình 4.15 Trang điều khiển Auto ..................................................................................... 58
Hình 4.16 Trang điều khiển Setting ................................................................................. 58
Hình 5.1 Hình ảnh thực tế sau khi qua các bước xử lý và lọc màu ................................ 59
Hình 5.2 Robot gắp vật màu đỏ ........................................................................................ 59

Hình 5.3 Robot gắp vật màu xanh dương ........................................................................ 60
Hình 5.4 Robot gắp vật màu xanh lá ................................................................................ 60


DANH MỤC BẢNG BIỂU
Bảng 2.1 Bảng sơ đồ chân của PLC S7-1200 ................................................................ 21
Bảng 2.2 Các ngõ phát xung của PLC 1212C DC/DC/DC ............................................ 28
Bảng 2.3 Các chân của khối MC_Power ......................................................................... 33
Bảng 2.4 Các chân của khối MC_Reset ........................................................................... 34
Bảng 2.5 Các chân của khối MC_Home. ......................................................................... 35
Bảng 2.6 Các chân của khối MC_Halt ............................................................................. 36

Bảng 2.7 Các chân của khối MC_MoveAbsolute ........................................................... 36
Bảng 2.8 Các chân của khối MC_MoveRelative ............................................................ 37
Bảng 2.9 Các chân của khối MC_MoveJog .................................................................... 38
Bảng 4.1 Danh sách các ngun liệu của mơ hình .......................................................... 51
Bảng 5.1 Bảng đánh giá thực nghiệm .............................................................................. 60


LỜI MỞ ĐẦU
Ngày nay, cùng với sự phát triển vô cùng nhanh chóng của khoa học kĩ thuật, con
người chúng ta ngày càng đạt được nhiều thành tựu ở hầu khắp các lĩnh vực như sản
xuất, y học, công nghệ thông tin, kĩ thuật quân sự…
Robot đã được ra đời và trở thành một công cụ đắc lực và hiệu quả giúp đỡ con

người thực hiện rất nhiều công việc từ đơn giản đến phức tạp nhằm giảm thiểu sức lao
động chân tay cũng như để giải quyết những công việc mà khả năng con người không
thể thực hiện được. Đặc biệt là ngay sau khi trải qua đại dịch COVID-19 thì nhu cầu về
sử dụng Robot trong sản xuất lại càng được chú ý hơn nhằm đảm bảo an tồn cho nhân
cơng cũng nhưng nâng cao hiệu xuất sản xuất, tăng độ chính xác trong các dây chuyền
sản xuất lớn.
Cùng với sự phát triển của các loại Robot công nghiệp phổ biến như Robot Scara,
Robot khớp nối 3 bậc, 4 bậc… thì có một loại cánh tay được ứng dụng rất nhiều trong
lãnh vực phân loại sản phẩm với nhiều đặc tính nổi trội so với các loại robot cổ điển đó
là Robot Delta hay cịn biết đến là Robot song song, chính vì thế nhóm chúng em đã
chọn đề tài: “Thiết kế và điều khiển Robot Delta phân loại vật sử dụng PLC S7-1200”.
Mục tiêu, nhiệm vụ của đề tài là ứng dụng PLC S7-1200 và công nghệ xử lý ảnh

để xây dựng thuật toán điều khiển và thiết kế mơ hình Robot Delta phân loại sản phẩm
theo màu sắc.
Đề tài gồm 5 Chương:
Chương 1: Tổng quan về đề tài:
Trình bày những ứng dụng của Robot Delta và xử lý ảnh trong môi trường thực tế,
nêu nhiệm vụ chính và phương pháp thực hiện trong đề tài.
Chương 2: Cơ sở lý thuyết:
Trình bày lý thuyết về cấu tạo Robot Delta, phương trình động lực học của Robot
Delta, cách tính phương trình động lực học thuận nghịch, các kiến thưc về sử lý ảnh qua
thư viện EmguCV của C#.Net sử dụng trong luận văn.
Giới thiệu các linh kiện phần cứng như PLC S7-1200, cấu tạo và hoạt động của
động cơ bước, cách điều khiển động cơ bước thông qua Driver, cách lập trình ứng dụng

thơng qua nền tảng Winform của C#.Net, cách giao tiếp giữa Application Winform với
phần mềm TIA Portal để truyền dữ liệu từ PC xuống PLC.
Chương 3: Thuật toán điều khiển:


Nội dung của chương trình điều khiển mơ hình Robot Delta mà nhóm đã thực hiện
và các thuật tốn để điều khiển Robot hoạt động.
Chương 4: Thi cơng mơ hình:
Q trình thi cơng phần cứng và phần mềm của mơ hình.
Chương 5: Kết quả và nhận xét
Trình bày kết quả thu được, những hạn chế và đánh giá về hướng phát triển của đề
tài.

CHƯƠNG 1: TỔNG QUAN VỀ ĐỀ TÀI
1.1. Giới thiệu Robot Delta
Khái niệm robot có cấu trúc song song được Gough và Whitehall đưa ra vào năm
1962. Ngày nay, Delta robot (robot song song là một loại robot được sử dụng nhiều
trong công nghiệp, đặc biệt là trong khâu phân loại và đóng gói sản phẩm nhờ vào đặc
tính có tốc độ và độ chính xác cao.
Robot Delta có cơ cấu động học song song ba bậc tự do, cấu trúc vịng kín. Trong
đó có ba nhánh, với mỗi nhánh, một đầu được nối vào giá di động thông qua cơ cấu hình
bình hành. Với cấu trúc hình học như vậy nên giá di động ln định hướng và chỉ có thể
chuyển động tịnh tiến theo 3 trục XYZ.

Với nhiều đặc tính nổi trội nên Robot Delta được ứng dụng rộng rãi ở nhiều lĩnh
vực khác nhau trong cuộc sống. Dưới đây là 1 số ví dụ cụ thể:
Ứng Dụng Trong Cơng Nghiệp:

Hình 1.1 Robot Delta cơng đoạn đóng gói sản phẩm nhà máy Vinamilk

1


Hình 1.2 Robot Delta trong cơng nghiệp
Ứng dụng trong Y học:

Hình 1.3 Robot Delta sử dụng để nâng kính giúp bác sĩ phẫu thuật

2


Ứng dụng trong học tập, nguyên cứu khoa học:

Hình 1.4 Sáng kiến SV ứng dụng robot Delta diệt cỏ loại sâu
1.2. Mục tiêu, nhiệm vụ đề tài
Tìm hiểu cấu tạo thành phần, cấu trúc cơ bản của Robot Delta, tính toán động lực
học và áp dụng thực tế về phân loại sản phẩm.
Xây dựng phần mềm điều khiển Robot Delta với cả 2 chế độ Auto và Manual, mô

phỏng quá trình phân loại sản phẩm trong cơng nghiệp.
1.3. Phương án
Nhận dạng đối tượng bằng Camera sau đó gửi về PC các giá trị tọa độ, màu sắc,
kích thước của đối tượng. PC nhận được dữ liệu sẽ tính tốn qua phương trình động lực
học để gửi thơng tin cho PLC để điều khiển Robot tới gắp và thả vào ô phân loại.
Sử dụng động cơ bước cho 3 cánh tay của Robot để robot vận hành một cách linh
hoạt.
Sử dụng Web Camera để để nhận dạng vì Web Camera có giá thành rẻ, dễ dàng
kết nối với PC nhưng vẫn đáp ứng đủ các tiêu chí theo yêu cầu của đề tài.
1.4. Các bước tiến hành
B1: Xây dựng chương trình tính tốn động lực học thuận/nghịch của Robot Delta.
B2: Cấu hình PLC S7-1200 đề điều khiển nhiều động cơ bước cùng lúc, sử dụng

phần mềm TIA Portal V15.
3


B3: Xây dựng giao diện chương trình điều khiển Robot Delta với ba phần chính:
Kết nối PC với PLC, Chế độ Manual, Chế độ Auto. Sử dụng nền tảng Winform của
C#.NET.
B4: Thiết kế và thi cơng mơ hình in 3D của Robot Delta.
B5: Đưa phương trình động lực vào trong chương trình điều khiển truyền thơng
giữa chương trình điều khiển với PLC bằng OPC Kepserver, tiến hành vận hành Manual.
B6: Nhúng thư viện EmguCV vào phần mềm, lắp thêm Web Camera, điều khiển,
xây dựng chương trình xác định tọa độ và màu sắc của vật mẫu để tiến hành phân loại,

tiến hành vận hành Auto.
B7: Vận hành hiểu chỉnh phần cứng và phần mềm.

4


CHƯƠNG 2: CƠ SỞ LÝ THUYẾT
2.1. Phương trình động lực học Robot Delta
2.1.1. Cấu tạo robot song song Delta

Hình 2.1 Cấu tạo cơ bản của Robot Delta
Cấu tạo của robot bao gồm các phần như sau:

- Bàn máy cố định F, bàn máy động E.
- Ba khâu chủ động F1J1, F2J2, F3J3 liên kết với bàn máy cố định bằng các khớp
quay và được dẫn động bởi 3 động cơ, các động cơ này gắn chặn với bàn máy
cố định F.
- Ba khâu bị động J1E1, J2E2, J3E3, mỗi khâu là một cấu trúc hình bình hành.
Nhờ tính chất của các khâu hình bình hành nên bàn máy động là một vật rắn chuyển
động tịnh tiến trong không gian. Như vậy robot có 3 bậc tự do xác định bởi 3 toạ độ
θ1 , θ2 , θ3 trong không gian khớp.
2.1.2. Động học thuận Robot
Từ các góc θ1 , θ2 , θ3 → Tọa độ (𝐱 𝟎 , 𝐲𝟎 , 𝐳𝟎 ) của điểm E0.
Nếu ta đã biết góc θx , chúng ta có thể dễ dàng tìm thấy tọa độ của các
điểm J1, J2 và J3. Các khớp nối J1E1, J2E2 và J3E3 có thể tự do xoay quanh các

điểm J1, J2 và J3 tương ứng, tạo thành ba hình cầu có bán kính re.

5


Hình 2.2 Mơ hình động học robot delta
Ta di chuyển các tâm của các hình cầu từ các điểm J1 , J2 và J3 đến các điểm J’1 , J2′
và J’3 bằng cách sử dụng các vector chuyển tiếp E1 E0 , E2 E0 và E3 E0 tương ứng. Sau quá
trình chuyển đổi này, tất cả ba hình cầu sẽ giao nhau tại một điểm: E0 , như được thể
hiện trong hình phía dưới:

Hình 2.3 Mơ hình động học robot delta

Vì vậy, để tìm tọa độ (x0 , y0 , z0 ) của điểm E0 , chúng ta cần giải quyết tập hợp ba
2

2

2

phương trình như (x − xj ) + (y − yj ) + (z − zj ) = re2 , nơi tọa độ các tâm hình cầu
(xj , yj , zj ) và bán kính re đã biết.

6

Hình 2.4 Tọa độ các điểm J1′ , J2′ , J3′ trên mặt phẳng Oxy
f
f
tan(300 ) =
2
2√3
e
e
J1 J1′ = J2 J2′ = J3 J3′ = tan(300 ) =
2
2√3

OF1 = OF2 = OF3 =

F1 J1 = rf cos(θ1 ) , F2 J2 = rf cos(θ2 ) , F3 J3 = rf cos(θ3 )
J1′ (0; −
J2′ ((
J3′ ((

f−e
2√3

f−e
2√3

f−e
2√3

− rf cos θ1 ; −rf sin θ1 )

+ rf cos θ2 ) cos 300 ; (
+ rf cos θ3 ) cos 300 ; (

f−e
2√3
f−e
2√3

+ rf cos θ2 ) sin 300 ; −rf sin θ2 )
+ rf cos(θ3 )) sin 300 ; −rf sin θ3 )

Trong các phương trình sau đây ta sẽ chỉ định tọa độ của các điểm J1 , J2 , J3 là
(x1 , y1 , z1 ), (x2 , y2 , z2 ) và (x3 , y3 , z3 ). Lưu ý rằng x1 = 0. Dưới đây là phương trình của
ba hình cầu:
x 2 + (y − y1 )2 + (z − z1 )2 = re2
{(x − x2 )2 + (y − y2 )2 + (z − z2 )2 = re2
(x − x3 )2 + (y − y3 )2 + (z − z3 )2 = re2
x 2 + y 2 + z 2 − 2y1 y − 2z1 z = re2 − y12 − z12
⇒ { x 2 + y 2 + z 2 − 2x2 x − 2y2 y − 2z2 z = re2 − x22 − y22 − z22

x 2 + y 2 + z 2 − 2x3 x − 2y3 y − 2z3 z = re2 − x32 − y32 − z32
Đặt: wi = xi2 + yi2 + zi2 , ta được:

7

(1)
(2)
(3)


(w1 − w2 )
2

(w1 − w3 )
x3 x + (y1 − y3 )y + (z1 − z3 )z =
2
(w 2 − w 3 )
(y2 − y3 )y + (z2 − z3 )z =
(x
−
x
)x
+
2
3

{
2
x2 x + (y1 − y2 )y + (z1 − z2 )z =

(4) = (1) − (2)
(5) = (1 ) − (3)
(6) = (2) − (3)

Từ (4) - (5):
x = a1 z + b1 (7)
y = a2 z + b2 (8)
1

[(z − z1 )(y3 − y1 ) − (z3 − z1 )(y2 − y1 )]
d 2
1
b1 = − [(w2 − w1 )(y3 − y1 ) − (w3 − w1 )(y2 − y1 )]
2d
1
a2 = − [(z2 − z1 )x3 − (z3 − z1 )x2 ]
d
1
[(w2 − w1 )x3 − (w3 − w1 )x2 ]
b2 =
2d

d = (y2 − y1 )x3 − (y3 − y1 )x2
a1 =

Thế (7), (8) vào (1) ta được:
(a21 + a22 + 1)z 2 + 2(a1 + a2 (b2 − y1 ) − z1 )z + (b12 + (b2 − y1 )2 + z12 − re2 ) = 0
Cuối cùng, ta cần giải phương trình này và tìm z0 (chúng ta nên chọn gốc phương
trình âm nhỏ nhất), và sau đó tính x0 và y0 từ (7) và (8).
2.1.3. Động học nghịch Robot
Động học nghịch:

Hình 2.5 Mơ hình động học robot delta với các thơng số cho trước
8

Các thông số: f, rf, re, e cho trước.
Do khớp nối thiết kế của robot F1J1 (xem hình bên dưới) chỉ có thể xoay trong mặt
phẳng YZ, tạo thành hình trịn với tâm tại điểm F1 và bán kính rf.
J1 và E1 được gọi là khớp nối phổ quát, có nghĩa là E1J1 có thể xoay tự do tương đối
với E1, tạo thành hình cầu với tâm tại điểm E1 và bán kính re.

Hình 2.6 Minh họa động học của cánh tay địn
Giao điểm của hình cầu này và mặt phẳng YZ là một đường trịn có tâm tại
điểm E1′ và bán kính E1′ J1 (trong đó E1′ là phép chiếu của điểm E1 trên mặt phẳng
YZ). Điểm J1 có thể được tìm thấy bây giờ như là giao điểm của các đường trịn bán

kính đã biết với tâm là E1′ và F1 (chúng ta chỉ nên chọn một điểm giao nhau với tọa độ
Y nhỏ hơn). Và nếu ta biết J1 , ta có thể tính được góc θ1 .
Xét mặt phẳng YZ:

Hình 2.7 Mặt phẳng YZ
9


Ta có:
E(x0 , y0 , z0 )
e
e

EE1 = tan30o =
2
2√3
e
e
E1 (x0 , y0 −
, z0 ) ⇒ E1′ = (0, y0 −
, z0 )
2√3
2√3
2

EE1′ = x0 ⇒ E1′ J1 = √E1 J12 − E1 E1′ = √re2 − x02
F1 (0, −

f
2√3

, 0)
2

{

2

(yJ1 − yF1 ) + (zJ1 − zF1 ) = rf2
2

2

(yJ1 − yE′1 ) + (zJ1 − zE′1 ) = re2 − x02
(yJ1 +

⇒
{

(yJ1 − y0 +

θ1 = arctan (

f
2√3

e
2√3

2

2

) + zJ21 = rf2
2

⇒ J1 (0, yJ1 , zJ1 )

) + (zJ1 − z0 ) = re2 − x02

zJ1
)
yF1 − yJ1

Do khớp F1 J1 chỉ di chuyển trong mặt phẳng YZ, vì vậy chúng ta hồn tồn bỏ qua
tọa độ X. Để tận dụng tính chất này cho các góc cịn lại θ2 và θ3 , chúng ta nên sử dụng
tính đối xứng của delta robot. Đầu tiên, chúng ta hãy xoay hệ tọa độ trong mặt phẳng
XY xung quanh trục Z một góc 1200 ngược chiều kim đồng hồ, như được hiển thị bên
dưới.

Hình 2.8 Quy đổi tọa độ
10


Ta đã có một khung tham chiếu mới X'Y'Z', và với khung này ta có thể tìm thấy

góc 𝛉𝟐 bằng cách sử dụng cùng một thuật toán mà ta sử dụng để tìm 𝛉𝟏 .
Sự thay đổi duy nhất là chúng ta cần phải xác định tọa độ mới x'0 và y'0 cho
điểm E0, có thể dễ dàng thực hiện bằng cách sử dụng “ma trận xoay” tương ứng.
Để tìm góc 𝛉𝟑 , chúng ta phải xoay khung tham chiếu theo chiều kim đồng hồ.
2.2. Tổng quan về công nghệ xử lý ảnh
2.2.1. Các khái niệm của xử lí ảnh
Xử lý ảnh kỹ thuật số là ngành học nghiên cứu các kỹ thuật xử lý ảnh. Hình ảnh
được đề cập trong nghiên cứu này là cảm biến thị giác dạng ảnh tĩnh (webcam). Về mặt
tốn học, hình ảnh là một hàm liên tục của cường độ ánh sáng trên trường hai chiều. Để
được máy tính xử lý, một hình ảnh phải được trình bày dưới dạng số với các giá trị rời
rạc. Hình ảnh kỹ thuật số có thể được biểu diễn bằng ma trận hai chiều f (x, y) bao gồm
M cột và N hàng.

a. Hình ảnh
Một hình ảnh được định nghĩa là một hàm hai chiều, F(x, y), trong đó x và y là các
tọa độ khơng gian, và biên độ của F tại bất kỳ cặp tọa độ nào (x, y) được gọi là cường
độ của hình ảnh đó tại điểm đó. Khi các giá trị x, y và biên độ của F là hữu hạn, ta gọi nó
là ảnh số.
Nói cách khác, một hình ảnh có thể được xác định bởi một mảng hai chiều được sắp
xếp cụ thể theo hàng và cột.

Hình 2.9 Biểu diễn ảnh dưới dạng ma trận
b. Pixel
Q trình số hóa ảnh biến đổi từ giá trị analog sang digital và lượng tử hóa về giá
trị mà về nguyên tắc mắt thường khơng phân biệt được hai điểm kề nhau. Trong q

trình này người ta sử dụng khái niệm Picture Element mà ta hay gọi là Pixel – Phần tử

11


ảnh, điểm ảnh. Như vậy ảnh là một tập hợp các Pixel. Mỗi Pixel gồm một cặp tọa độ x,
y và màu.
Một Pixel thường được biểu diễn bởi 1, 2, 8 hay 24 bit màu.

Hình 2.10 Ảnh pixel 8 bit
c. Mức xám (Gray level)
Mức xám là kết quả của sự mã hóa tương ứng của một cường độ sáng mỗi điểm

ảnh với một giá trị số, kết quả của quá trình lượng tử hóa. Mã hóa 256 là mức phổ biến
nhất do kĩ thuật, mức xám là 256 thì mỗi pixel sẽ được mã hóa bởi 8 bit.

Hình 2.11 Ảnh mức xám
d. Ảnh màu
Ảnh màu là tổ hợp 3 màu cơ bản: đỏ, lục, xanh dương và thường thu nhận trên các
dãy băng tần khác nhau. Mỗi màu cũng phân thành L cấp khác nhau. Do vậy để lưu trữ
ảnh màu người ta có thể lưu trữ từng mặt màu riêng biệt, mỗi màu được lưu như một
ảnh đa cấp xám. Do đó khơng gian nhớ dành cho ảnh màu gấp 3 lần không gian nhớ
dành cho ảnh xám cùng kích thước.

12