View and download: https://uploading.vn/5j3cxht450rp

1. BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƢỜNG ĐẠI HỌC CÔNG NGHỆ TP. HCM
---------------------------
NGUYỄN NGỌC LONG
NGHIÊN CỨU THIẾT KẾ VÀ ĐIỀU KHIỂN
TAY MÁY GẮP CÁC THÙNG CARTON SẮP XẾP
LÊN PALLET.
LUẬN VĂN THẠC SĨ
Chuyên ngành : KỸ THUẬT CƠ ĐIỆN TỬ
Mã số ngành: 60520114
TP. HỒ CHÍ MINH, tháng 8 năm 2015

2. BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƢỜNG ĐẠI HỌC CÔNG NGHỆ TP. HCM
---------------------------
NGUYỄN NGỌC LONG
NGHIÊN CỨU THIẾT KẾ VÀ ĐIỀU KHIỂN
TAY MÁY GẮP CÁC THÙNG CARTON SẮP XẾP
LÊN PALLET.
LUẬN VĂN THẠC SĨ
Chuyên ngành : KỸ THUẬT CƠ ĐIỆN TỬ
Mã số ngành: 60520114
CÁN BỘ HƢỚNG DẪN KHOA HỌC: TS. NGUYỄN DUY ANH
TP. HỒ CHÍ MINH, tháng 8 năm 2015

3. CÔNG TRÌNH ĐƯỢC HOÀN THÀNH TẠI
TRƢỜNG ĐẠI HỌC CÔNG NGHỆ TP. HCM
Cán bộ hướng dẫn khoa học : TS. NGUYỄN DUY ANH
Luận văn Thạc sĩ được bảo vệ tại Trường Đại học Công nghệ TP. HCM
Ngày 7 tháng 8 năm 2015
Thành phần Hội đồng đánh giá Luận văn Thạc sĩ gồm:
T
TT
Họ và tên Chức danh Hội đồng
1
1
TS. Nguyễn Thanh Phương Chủ tịch
2
2
PGS.TS.Lê Hữu Sơn Phản biện 1
3
3
TS.Võ Hoàng Duy Phản biện 2
4
4
PGS.TS.Ngô Cao Cường Ủy viên
5
5
TS. Nguyễn Hùng Ủy viên, Thư ký
Xác nhận của Chủ tịch Hội đồng đánh giá Luận sau khi Luận văn đã được
sửa chữa.
Chủ tịch Hội đồng đánh giá luận văn

4. NHIỆM VỤ LUẬN VĂN THẠC SĨ
Họ tên học viên : NGUYỄN NGỌC LONG Giới tính: nam
Ngày, tháng, năm sinh: 01/07/1986 Nơi sinh: BÌNH PHƯỚC
Chuyên ngành: Kỹ Thuật Cơ Điện Tử MSHV: 1341840016
I- Tên đề tài:
NGHIÊN CỨU THIẾT KẾ VÀ ĐIỀU KHIỂN TAY MÁY GẮP CÁC THÙNG
CARTON SẮP XẾP LÊN PALLET.
II- Nhiệm vụ và nội dung:
Thiết kế mô hình tay máy gắp sản phẩm.
Tìm hiểu và mô phỏng thuật toán tối ưu việc sắp xếp các thùng carton.
Mô phỏng động học và giải thuật sắp xếp các thùng carton lên pallet.
III- Ngày giao nhiệm vụ: 25 tháng 8 năm 2014
IV- Ngày hoàn thành nhiệm vụ: 10 tháng 7 năm 2015
V- Cán bộ hƣớng dẫn: TS. NGUYỄN DUY ANH
CÁN BỘ HƢỚNG DẪN KHOA QUẢN LÝ CHUYÊN NGÀNH
TRƯỜNG ĐH CÔNG NGHỆ TP. HCM
PHÒNG QLKH – ĐTSĐH
CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM
Độc lập – Tự do – Hạnh phúc

5. i
LỜI CAM ĐOAN
Tôi xin cam đoan đây là công trình nghiên cứu của riêng tôi. Các số liệu, kết quả
nêu trong Luận văn là trung thực và chưa từng được ai công bố trong bất kỳ công trình
nào khác.
Tôi xin cam đoan rằng mọi sự giúp đỡ cho việc thực hiện Luận văn này
đã được cảm ơn và các thông tin trích dẫn trong Luận văn đã được chỉ rõ nguồn gốc.
Học viên thực hiện Luận văn
(Ký và ghi rõ họ tên)

6. ii
LỜI CÁM ƠN
Tôi xin chân thành cảm ơn các thầy cô phụ trách giảng dạy hệ cao học Cơ Điện
Tử trường đại học Công Nghệ TP.HCM đặc biệt là TS. Nguyễn Duy Anh, thầy là
giảng viên chính phụ trách việc hướng dẫn giúp đỡ trong suốt quá trình thực hiện luận
văn của tôi.
Thời gian thực hiện luận văn tuy chỉ trong vài tháng những kiến thức học được là
rất nhiều, rất bổ ích.Tôi sẽ luôn ghi nhớ những kiến thức và kinh nghiệm thực tiễn mà
thầy đã truyền thụ và lưu giữ những kỷ niệm trong suốt thời gian thực hiện luận văn
này, lấy đó làm nền tảng để tiếp tục phát triển kiến thức và nghề nghiệp của mình
trong tương lai.
Thành phố Hồ Chí Minh, ngày tháng năm 2015
Học viên
NGUYỂN NGỌC LONG

7. iii
TÓM TẮT LUẬN VĂN
Xuất phát từ nhu cầu thực tế tại nơi làm việc của tôi là tại xưởng sản xuất của
Công Ty TNHH Giải Pháp Tự Động Hóa TECHKING, trong quá trình sản xuất ở tại
khâu đóng thùng thành phẩm, hiện tại chúng tôi đang cần 1 cánh tay robot công nghiệp
để làm nhiệm vụ gắp các thùng carton thành phẩm sắp xếp lên pallet để đưa đi nhập
kho. Trong thực tế sản xuất thì các thùng thành phẩm có trọng lượng 25 kg, sử dụng
sức người để nâng lên và sắp xếp lên các pallet gỗ. Việc này mất nhiều sức lực, đặc
biệt thao tác lao động này dễ làm tổn thương cột sống lưng và năng suất lao động cũng
không cao.
Luận văn trình bày việc nghiên cứu và xây dựng một mô hình cánh tay robot 5
bậc tự do phối hợp hoạt động trong một dây chuyền sản xuất, các thùng carton sau khi
được đóng gói thành phẩm thì đi qua máy dán thùng để dán băng keo, đầu ra của máy
dán thùng là các thùng hàng thành phẩm. Robot có nhiệm vụ gắp các thùng hàng thành
phẩm này sắp xếp lên pallet theo thứ tự các lớp thùng được lập trình sẵn. Vì vậy, mục
tiêu chính của luận văn là nghiên cứu thiết kế bộ điều khiển quĩ đạo cánh tay robot có
thể thực hiện việc gắp các thùng carton lên pallet theo đúng thứ tự các lớp thùng được
quy định và đưa ra kiểm chứng bằng mô hình thực nghiêm. Ứng dụng của robot có thể
được dùng cho các nghiên cứu lớn hơn sau này khi đó có thể tích hợp để toàn bộ dây
chuyền sản xuất là robot và các máy móc thông minh, vai trò của con người chỉ là thực
hiện việc giám sát trên máy vi tính.

8. iv
ABSTRACT
I am working for the limited liability automation solutions company Tecking.
Now, at the stage of crating in this company, our need a industrial robot arm to lift up
and arrange the finished cartons to pallets after that this robot will takes its to storage.
In fact, our company are using human power to lift up and arrange the finished
cartons which are 25 kilograms that is takes a lot of strength, expecially it can make
hurt the lumbar spine of labors while the productivity is not high.
My thesis presents the studying and building a model of the five degrees of
freedom arm robot which have coordinate of activities in a production line. The
cartons are packed after that through the machine sealing and become finished goods
at the output. The mission of robot are lifting up the packages and arranging them into
pallet in sequence which is preprogrammed. Therefore, the main objective of this
thesis are studying and designing trajectory controller of robot arm. This arm robot
may lift up the cartons into pallet and arrange them in the order which is
preprogrammed. Be verifiable by empirical model. Application of this robot can be
used for the larger studying. In that time, in the production line may be use all of
robots and intelligent machines while the people just monitor it in computers.

9. v
MỤC LỤC
LỜI CAM ĐOAN ............................................................................................................. i
LỜI CÁM ƠN.................................................................................................................. ii
TÓM TẮT LUẬN VĂN.................................................................................................. iii
ABSTRACT.................................................................................................................... iv
DANH MỤC HÌNH ....................................................................................................... vii
DANH MỤC BẢNG ....................................................................................................... ix
TỔNG QUAN ...........................................................................................1
CHƢƠNG 1:
1.1 Tổng quan về robot.......................................................................................................1
1.2 Cấu trúc cơ bản của tay máy công nghiệp.....................................................................5
1.3 Mục tiêu, nhiệm vụ và tổ chức luận văn........................................................................6
MÔ PHỎNG ĐỘNG HỌC VỊ TRÍ CỦA ROBOT IRB6600...................8
CHƢƠNG 2:
2.1 Giới thiệu chương trình RobotStudio:...........................................................................8
2.2 Các bước lập trình bằng phần mềm RobotStudio ..........................................................9
2.3 Rapid programing.........................................................................................................9
MÔ HÌNH ROBOT 5 BẬC TỰ DO .......................................................12
CHƢƠNG 3:
3.1 Tổng quan về robot 5 bậc tự do sử dụng trong luận văn..............................................12
3.2 Thiết kế tay gắp ..........................................................................................................15
3.3 Sơ lược động học vị trí của robot................................................................................26
3.4 Thiết kế nạch điện.......................................................................................................34
3.5 Mạch điều khiển .........................................................................................................38
3.6 Giao diện điều khiển...................................................................................................45
GIẢI THUẬT ĐIỀU KHIỂN ROBOT GẮP VẬT.................................47
CHƢƠNG 4:
4.1 Bài toán điều khiển.....................................................................................................47

10. vi
4.2 Quỹ đạo chuyển động.................................................................................................47
4.3 Hoạch định quỹ đạo....................................................................................................50
4.4 Phương pháp điều khiển .............................................................................................53
4.5 Giải thuật điều khiển...................................................................................................55
THỰC NGHIỆM, ĐÁNH GIÁ KẾT QUẢ.............................................59
CHƢƠNG 5:
5.1 Giới thiệu ...................................................................................................................59
5.2 Quá trình thực nghiệm................................................................................................60
5.3 Đánh giá sai số ...........................................................................................................63
5.4 Kết luận......................................................................................................................63
TỔNG KẾT VÀ HƢỚNG PHÁT TRIỂN ĐỀ TÀI................................69
CHƢƠNG 6:
6.1 Đánh giá kết quả đạt được ..........................................................................................69
6.2 Hướng phát triển của đề tài.........................................................................................69
TÀI LIỆU THAM KHẢO..............................................................................................65

11. vii
DANH MỤC HÌNH
Hình 1.1: Robot hàn trong công nghiệp ............................................................1
Hình 1.2: Robot gắp thùng carton lên pallet của hãng...... ................................ 3
Hình 1.3 : Robot gắp thùng carton lên băng tải của hãng Yaskawa Nhật Bản…4
Hình 1.4: Hệ thống AKB robot do nhóm nghiên cứu của kỹ sư Lê Anh ...........4
Hình 1.5: Cấu trúc cơ bản robot công nghiệp. ................. ................................ 4
Hình 2.1: Mô tả các bước lập trình bằng phần mềm RobotStudio ....................9
Hình 2.2: Cấu trúc chương trình trong RAPID ................ .................................9
Hình 2.3: Mô phỏng quỹ đạo di chuyển của ....................................................11
Hình 2.4: Mô phỏng quỹ đạo di chuyển của robot............................................11
Hình 3.1: Mô hình thực tế robot sử dụng trong luận văn .................................12
Hình 3.2: Kích thước các chi tiết trên mô hình robot. ......................................13
Hình 3.3: Sơ đồ động học robot .....................................................................13
Hình 3.4: Cánh tay robot trên SolidWork .......................................................14
Hình 3.5: Cơ cấu 3 chấu kẹp và 4 chấu kẹp ....................................................19
Hình 3.6: Cơ cấu tay quay con trượt ..............................................................19
Hình 3.7: Mô hình cơ cấu gắp phương án 2 và sơ đồ động .............................20
Hình 3.8: Minh họa cơ cấu .............................................................................20
Hình 3.9: Sơ đồ nguyên lý ..............................................................................20
Hình 3.10: Tọa độ điểm cần điều khiển ..........................................................21
Hình 3.11: Kích thước tay gắp ........................................................................21
Hình 3.12: Lực kẹp giữ vật không rơi .............................................................22
Hình 3.13: Sơ đồ tác động lực của tay gắp ở vị trí kẹp ....................................23
Hình 3.14: Sơ đồ tác động lực một bên tay gắp ở vị trí kẹp ............................24
Hình 3.15: Mô hình cánh tay robot 3D và không gian hoạt động ....................26
Hình 3.16: Định nghĩa các thông số DH cho khớp và khâu thứ I ....................28
Hình 3.17: Kết cấu robot khảo sát ..................................................................28
Hình 3.18: Đặt hệ tọa độ cho các khâu và khớp ..............................................29
Hình 3.19: Hình ảnh ứng với trạng thái . ........................................................34

12. viii
Hình 3.20: Hình ảnh ứng với trạng thái ..........................................................35
Hình 3.21: Sơ đồ khối kết nối hệ thống ..........................................................38
Hình 3.22: Sơ đồ mạch điện vi điều khiển Transmit ........................................39
Hình 3.23: Sơ đồ mạch điện vi điều khiển Receive .........................................40
Hình 3.24: Sơ đồ giao tiếp RS232 ..................................................................41
Hình 3.25: Sơ đồ CAN bus .............................................................................41
Hình 3.26: Driver DCS3T-25 .........................................................................42
Hình 3.27: Sơ đồ mạch kết nối nguồn 24V, động cơ và driver ........................43
Hình 3.28: Sơ đồ mạch nguồn cho vi điều khiển và cảm biến tiệm cận ...........43
Hình 3.29: Mạch 3D được thiết kế băng phần mềm Altium ............................44
Hình 3.30: Mạch điều khiển sau khi được chế tạo ..........................................44
Hình 3.31: Hệ thống mạch điện điều khiển......................................................45
Hình 3.32: Giao diện điều khiển trên máy tính ...............................................46
Hình 3.33: Lấy giá trị tọa độ các điểm tựa theo quỹ đạo .................................47
Hình 3.34: Lấy giá trị tọa độ ra file excel nhờ chương trình xyzPathMac ........47
Hình 4.1: Đường biểu diễn vị trí, tốc độ, gia tốc của khớp 1 theo thời gian .....53
Hình 4.2: Vòng điều khiển hướng đối tượng ...................................................55
Hình 4.3: Lưu đồ tổng quát điều khiển cánh tay lắp vật ..................................57
Hình 4.4: Lưu đồ giải thuật điều khiển trên Transmit khi tiến hành lắp vật..... 58
Hình 4.5: Lưu đồ giải thuật điều khiển cho Receive khớp tay máy .................59
Hình 4.6: Lưu đồ giải thuật điều khiển cho Receive tay gắp ...........................60
Hình 5.1: Mô hình khối lập phương ...............................................................61
Hình 5.2: Mô hình robot thực nghiệm ............................................................61
Hình 5.3: Ảnh chụp trong quá trình robot gắp khối lập phương. ....................62
Hình 5.4: Kết quả gắp khối lập phương. ........................................................63
Hình 5.5: Kết quả gắp khối lập phương lặp lại 5 lần liên tục. .........................64
Hình 5.6: Kết quả sai lệch lắp vật giữa 5 lần là 2,6 mm. ..................................64

13. ix
DANH MỤC BẢNG
Bảng 3.1 Thông số các động cơ tại khớp của robot ......................................14
Bảng 3.2: Bảng so sánh động cơ bước và động cơ servo ...............................17
Bảng 3.3: Bảng so sánh các loại động cơ servo ............................................18
Bảng 3.4: Bảng thông số D-H .......................................................................29
Bảng 3.5: Bảng so sánh các loại giao tiếp .....................................................36
Bảng 5.1: Bảng khảo sát ..............................................................................63

14. 1
TỔNG QUAN VỀ ROBOT CÔNG NGHIỆP, ỨNG DỤNG,
CHƢƠNG 1:
MỤC TIÊU, TỔ CHỨC LUẬN VĂN.
1.1 Tổng quan về robot
1.1.1 Giới thiệu sơ lƣợc về robot
Khoa học công nghệ đã và đang từng bước thay đổi cuộc sống của chúng ta,
với sự phát triển không ngừng của khoa học, con người đã và đang biến những điều
không thể thành có thể. Một trong số những phát minh lớn nhất mà chúng ta đã tạo
ra chính là những con Robot. Với những Robot đủ lĩnh vực và kích cỡ, chúng ta có
thể thực hiện được nhiều việc nặng nhọc hơn, tỉ mỉ hơn và nhất là tăng hiệu quả
công việc một cách nhanh chóng.
Hình 1.1: Robot hàn trong công nghiệp.
Ngày nay, ngành công nghiệp chiếm một vai trò rất quan trọng trong nền kinh
tế của mỗi quốc gia. Từ những ngành sản xuất, chế biến lương thực thực phẩm,
nước uống cho đến các ngành công nghệ chế tạo máy, công nghệ chế tạo ôtô, các
ngành công nghệ cao . Tất cả những ngành đó ngày càng phát triển đòi hỏi nhu cầu
về nâng cao năng xuất và chất lượng sản phẩm.

15. 2
Robot công nghiệp có khả năng chương trình hóa linh hoạt trên nhiều trục
chuyển động, biểu thị cho số bậc tự do của chúng . Robot công nghiệp được trang bị
những bàn tay máy hoặc các cơ cấu chấp hành, giải quyết những nhiệm vụ xác định
trong các quá trình công nghệ , trực tiếp tham gia thực hiện các nguyên công
( sơn, hàn, phun phủ, rót kim loại vào khuôn đúc, lắp ráp máy…) hoặc phục vụ các
quá trình công nghệ ( tháo lắp chi tiết gia công, dao cụ, đồ gá..) với những thao tác
cầm nắm, vận chuyển và trao đổi các đối tượng với các trạm công nghệ, trong một
hệ thông máy tự động linh hoạt, được gọi là “ Hệ thống tự động linh hoạt hóa” cho
phép thích ứng nhanh và thao tác đơn giản khi có thay đổi trong nhiệm vụ sản xuất.
Tuy nhiên các định nghĩa về robot cũng rất đa dạng , Robot là một cơ cấu
chuyển đổi tự động có thể chương trình hóa, lặp lại các chương trình, tổng hợp các
chương trình đặt ra trên các trục tọa độ có khả năng định vị, di chuyển các đối
tượng vật chất, chi tiết, dao cụ, gá lắp. Theo những hành trình thay đổi đã chương
trình hóa nhằm thực hiện các nhiệm vụ công nghệ khác nhau (Theo tiêu chuẩn
AFNOR của Pháp). Hoặc Robot là một thiết bị có nhiều trục, thực hiện các chuyển
động có thể chương trình hóa và nối ghép các chuyển động của chúng trong những
khoảng cách tuyến tính hay phi tuyến của động trình. Chúng được điều khiển bởi
các bộ phận hợp nhất ghép kết nối với nhau, có khả năng học và nhớ các chương
trình, chúng được trang bị dụng cụ hoặc các phương tiện công nghệ khác để thực
hiện các nhiệm vụ sản xuất trực tiếp hay gián tiếp (Theo tiêu chuẩn VDI 2860/BRD)
. Hoặc Robot là máy tự động liên kết giữa một tay máy và một cụm điều khiển
chương trình hóa, thực hiện một chu trình công nghệ một cách chủ động với sự điều
khiển có thể thay thế những chức năng tương tự của con người (Theo tiêu chuẩn
GHOST 1980).
Tổng hợp lại thì robot cần có những đặc điểm, có khả năng nhận biết môi
trường xung quanh, tương tác với những vật thể trong môi trường, tự động thực
hiện tác vụ đã lập trình sẵn, có khả năng điều khiển được bằng các lệnh có thể thay
đổi tùy theo yêu cầu người sử dụng đống thời có sự uyển chuyển và khéo léo trong
vận động.

16. 3
1.1.2 Tình hình phát triển Robot công nghiệp trên thế giới và tại Việt Nam
Có thể nói robot làm nhiệm vụ gắp các thùng carton sắp xếp lên pallet được
ứng dụng khá phổ biến trong các dây chuyền sản xuất đóng gói của các nhà máy xí
nghiệp. Chúng đã thay thế toàn bộ các hoạt động cơ bắp của con người trong khâu
sắp xếp thùng carton lên pallet, giúp nâng cao năng suất và hiệu quả trong quá trình
sản xuất công nghiệp.
Hình 1.2 : Robot gắp thùng carton lên pallet của hãng Kawasaki Nhật Bản.
Từ những năm 1960 người Mỹ đã có những nghiên cứu và ứng dụng robot
trong công nghiệp trong đó có ứng dụng robot gắp các thùng carton sắp xếp lên
pallet. Sau đó thì tới các nước châu âu khác như Anh , Đức, Pháp ,Ý, Thụy Điển.
Tại châu á thì từ những năm 1968 người Nhật cũng đã có những nghiên cứu và
ứng dụng robot đầu tiên.
Với sự áp dụng rộng rãi các tiến bộ kỹ thuật về vi xử lý và công nghệ thông
tin, số lượng robot công nghiệp đã gia tăng với nhiều tính năng vượt bậc. Đến
nay, trên thế giới có khoảng trên 200 công ty chuyên sản xuất robot điển hình

17. 4
như: Robots.Pro, Vecna Robotics, Robot Dynamics của Mỹ (với những sản phẩm
nổi tiếng như: robot lấy sách tự động, robot HOAP-3, robot BEAR, robot tự
hành Spirit and Opportunity…). Các công ty Yaskawa ( Motoman), Fanuc, Toyota,
Honda, Hitachi, Kawasaki, shikawajima-Harima, Yasukawa của Nhật (với những
sản phẩm: robot Asimo, robot EMIEW 2, robot Simroid, robot chơi vĩ cầm, robot
phẫu thuật...) và các công ty nỗi tiếng khác của Tây Âu và của Nga . Do đó, ta có
thể thấy rằng việc nghiên cứu và ứng dụng robot công nghiệp nói chung và ứng
dụng robot công nghiệp sắp xếp các thùng carton lên pallet trong các dây chuyền
sản xuất công nghiệp riêng nói là một lĩnh vực rất quan trọng và được các nước
phát triển đặc biệt quan tâm.
Hình 1.3: Robot gắp thùng carton lên băng tải của hãng Yaskawa Nhật Bản.
Còn tại Việt Nam, nghiên cứu phát triển robot đã có những bước tiến đáng kể
trong 25 năm vừa qua. Nhiều đơn vị trên toàn quốc thực hiện các nghiên cứu cơ bản
và nghiên cứu ứng dụng về robot như trung tâm tự động hoá, Đại học Bách Khoa
Hà Nội, Viện Điện tử, Tin học, Tự động hoá thuộc Bộ Công Nghiệp, Đại học Bách
khoa TP.HCM, Viện Khoa học và Công nghệ quân sự, Học viện Kỹ thuật Quân sự,
Viện Cơ học, Viện công nghệ thông tin thuộc Viện KHCNVN.

18. 5
Tuy nhiên thực tế thì các công trình nghiên cứu của chúng ta vẫn chưa thể đưa
ra thương mại hóa trên thị trường, vì về cơ bản các yếu tố trình độ khoa học và công
nghệ chúng ta vẫn đi sau thế giới nhiều thập kỷ. Hiện nay Việt Nam vẫn chưa có
công ty nào nghiên cứu chế tạo và thương mại hóa thành công các robot ứng dụng
trong công nghiệp. Những năm vừa qua thì cũng chỉ ghi nhận những bước đột phá
nhất định trong nghiên cứu và chế tạo mô hình robot tay máy 5 bậc tự do của
nhóm nghiên cứu của kỹ sư Lê Anh Kiệt, giám đốc Công ty cơ khí chế tạo máy
AKB. Anh là người đầu tiên tại Việt Nam nghiên cứu và chế tạo thành công hệ
thống robot loại 5 bậc tự do ứng dụng cho đào tạo tại các trường cao đẳng, đại học.
Hình 1.4: Hệ thống AKB robot do nhóm nghiên cứu của kỹ sư Lê Anh Kiệt chế tạo.
1.2 Cấu trúc cơ bản của tay máy công nghiệp
Tay máy là cơ cấu cơ khí gồm các khâu khớp , chúng hình thành cánh tay
(arm) để tạo các chuyển động cơ bản, cổ tay (Wrist) tạo nên sự khéo léo, linh hoạt
và bàn tay (Hand) hoặc phần công tác (End Effector) để trực tiếp hoàn thành các
các thao tác trên đối tượng. Dụng cụ thao tác là phần cuối cùng của robot thực hiện

19. 6
tác vụ yêu cầu. Hệ thống dẫn động có thể là cơ khí, thủy khí hoặc điện khí, là bộ
phận chủ yếu tạo nên sự chuyển dịch ở các khớp động. Hệ thống cảm biến tín hiệu
thực hiện việc nhận biết và biến đổi thông tin về hoạt động của bản thân robot (cảm
biến nội tín hiệu) và của môi trường, đối tượng mà robot phục vụ (cảm biến ngoại
tín hiệu). Các thông tin đặt trước hoặc cảm biến được sẽ đưa vào hệ thống điều
khiển sau khi xử lí bằng máy tính, rồi tác động vào hệ thống truyền dẫn động của
tay máy. Hệ thống điều khiển là bộ phận quan trọng nhất giúp lập trình toàn bộ
chuyển động của robot có 3 nhiệm vụ chính là nhận dữ liệu từ cảm biến và phân
tích, quyết định chuyển động của robot, giao tiếp với người sử dụng.
Một robot công nghiệp bao gồm các phần cơ bản sau:
Cảm biến
ngoại tín
hiệu
Hệ thống
điều
khiển
Cảm biến
nội tín
hiệu
Hệ thống
dẫn động
Dụng cụ
thao tác
Máy tính
Hình 1.5: Cấu trúc cơ bản robot công nghiệp
1.3 Mục tiêu, nhiệm vụ và tổ chức luận văn
1.3.1 Mục tiêu của đề tài
Vấn đề cần quan tâm khi nghiên cứu về robot tay máy chính là tối ưu hóa
đường đi của cánh tay robot khi thực hiện các thao tác sản xuất và lựa chọn kết cấu

20. 7
thiết bị phù hợp cùng giải thuật điều khiền để nâng cao tối đa sự chính xác trên từng
thao tác của robot đáp ứng được yêu cầu thực tế đặt ra.
Đề tài chỉ nghiên cứu mô hình robot tay máy cho ứng dụng gắp các thùng
carton sắp xếp lên pallet, xây dựng giải thuật tối ưu hóa đường đi của cánh tay
robot. Xây dựng mô hình toán, mô phỏng đường đi của cánh tay robot, thiết kế mô
hình thực nghiệm.
1.3.2 Nhiệm vụ của đề tài
Thiết kế mô hình cơ khí tay gắp vật cho robot, thiết kế mạch điện điều khiển
mô hình robot, xây dựng giải thuật gắp các thùng carton sắp xếp lên pallet và thực
nghiệm kiểm chứng giải thuật gắp vật.
1.3.3 Tổ chức luận văn
Chương 1: Tổng quan về robot công nghiệp, ứng dụng, mục tiêu, nhiệm vụ,
tổ chức luận văn.
Chương 2: Mô phỏng động học vị trí của robot IRB6600 sử dụng phần mềm
Robot studio của hãng ABB .
Chương 3: Mô hình robot 5DOF sử dụng trong luận văn
Chương 4: Giải thuật điều khiển robot gắp vật
Chương 5: Thực nghiệm và đánh giá kết quả
Chương 6: Tổng kết và hướng phát triển đề tài

21. 8
MÔ PHỎNG ĐỘNG HỌC VỊ TRÍ CỦA ROBOT IRB6600
CHƢƠNG 2:
SỬ DỤNG PHẦN MỀM ROBOTSTUDIO CỦA HÃNG ABB.
2.1 Giới thiệu chƣơng trình RobotStudio:
RobotStudio là phần mềm lập trình và mô phỏng của hãng ABB, tạo nên môi
trường gần giống với thực tế sản xuất nhất nhờ các tính năng tiên tiến nhằm nâng
cao và tối ưu hiệu quả hoạt động của robot trước khi đưa hệ thống vào sản xuất thực
tế.
Các tính năng nổi bật của phần mềm RobotStudio gồm , CAD Import các dữ
liệu CAD được thiết kế với các phần mềm chuyên dụng như SOLIDWORKS,
CATIA, INVENTOR có thể được IMPORT dễ dàng vào môi trường làm việc trong
RobotStudio. Điều này giúp cho việc lập trình có thể diễn ra chính xác và gần gũi
với thực tế. Auto Path từ dữ liệu CAD của chi tiết cần gia công, tính năng Auto Path
giúp tạo ra đường đi của của Robot một cách chính xác, tính năng Auto Reach sẽ
giúp kiểm tra khả năng với tới mục tiêu hay không để từ đó có thể bố trí không gian
làm việc một cách chính xác và hợp lý nhất. Collision detection phát hiện xung đột
là một đặc tính giúp ngăn chặn các thiệt hại về chi phí đối với thiết bị bằng cách cho
phép người sử dụng lựa chọn các chi tiết nhất định để RobotStudio có thể tự động
giám sát và để ý xem có xung đột hay không khi chạy một chương trình. Robot
Virtual FlexPendant là bản sao đồ hoạ chính xác của FlexPendant “thực tế” và được
hỗ trợ bởi “Virtual Robot”. Về cơ bản, tất cả những gì được thực hiện được bằng
FlexPendant thực tế thì có thể được thực hiện trên “Virtual FlexPendant”, khiến cho
chương trình này trở thành một công cụ giảng dạy và đào tạo tuyệt vời. True
Upload và Download, toàn bộ chương trình robot có thể tải về từ RobotStudio
xuống hệ thống thực tế mà không cần thêm bất cứ sự chuyển đổi nào.
Lợi ích cho quá trình sản xuất thực tế RobotStudio sẽ giúp cho các nhà sản
xuất tiết kiệm thời gian và tiền bạc, giảm rủi ro bằng cách mô phỏng, xác định các
giải pháp và các cách bố trí hợp lý. Lập trình cho các chi tiết mới mà không làm ảnh

22. 9
hưởng tới dây chuyển sản xuất. Tối ưu hóa các chương trình của Robot để nâng cao
năng suất và chất lượng sản phẩm thông qua tạo ra các đường đi chính xác hơn.
2.2 Các bƣớc lập trình bằng phần mềm RobotStudio
Hình 2.1: Mô tả các bước lập trình bằng phần mềm RobotStudio
2.3 Rapid programing
RobotStudio sử dụng ngôn ngữ Rapid programing trong lập trình điều khiển
Robot và các thiết bị ngoại vi. Một chương trình thường được xây dựng dựa trên 4
phần , main routine ( chương trình chính), Subroutines/Procedures (chương trình
con/ thủ tục, để chia chương trình ra nhiều phần nhỏ, có thể được gọi từ chương
trình chính hoặc chương trình con khác) Program data (dùng để khai báo vị trí, các
biến số, các hệ trục tọa độ) và System module ( các module hệ thống có sẵn trong
bộ nhớ khi được cài đặt).
Hình 2.2: Cấu trúc chương trình trong RAPID
Thiết kế
layout sơ
bộ hệ
thống
Xác định
quỹ đạo
chuyển động
của Robot
Lập trình Mô phỏng
Tối ưu hóa
hệ thống

23. 10
Các Data thông dụng:
Speed data: dùng để thiết lập tốc độ chuyển động
Ví dụ: v5, v10 có TCP (tool center point) lần lượt là: 5 mm/s, 10 mm/s
Robtarget: dùng để xác định 1 vị trí cho Robot
Ví dụ:
CONST robtarget
T1_P1:=[[145,15,110],[0,0,1,0],[0,0,1,1],[9E9,9E9,9E9,9E9,9E9,9E9]];
Tên của target: T1_P1
Tọa độ so với robot base: (x,y,z) = (145,15,110)
Hướng (quaternioner): (q1,q2,q3,q4) = (0,0,1,0)
Cấu hình robot: khớp (1,4,6,x) = (0,0,-1,1)
Tool data: dùng để khai báo đặc tính cho công cụ sẽ sử dụng (tay gắp, băng tải…)
Ví dụ:
PERS tooldata tGun:=[TRUE,[[74,0,107],[0.7,0,0.7,0]],[5,[0,0,1],[1,0,0,0],0,0,0]];
Name of tool: tGun
Robot hold tool: true or false
Position of TCP: (x,y,z)
Orientation of TCP (quaternioner): (q1,q2,q3,q4)
Các Instruction thông dụng : MoveL, MoveJ, MoveC ( là các lệnh yêu cầu
robot chuyển động linear, joint hoặc circular tương ứng). Set ( thiết lập giá trị cho
I/O) WaitTime (delay một khoảng thời gian tính bằng giây).

24. 11
Hình 2.3: Mô phỏng quỹ đạo di chuyển của robot bằng phần mềm RobotStudio.
Hình 2.4: Mô phỏng quỹ đạo di chuyển của robot xếp thùng carton lên pallet bằng
phần mềm RobotStudio.

25. 12
MÔ HÌNH ROBOT 5 BẬC TỰ DO ĐƢỢC SỬ DỤNG
CHƢƠNG 3:
TRONG LUẬN VĂN.
3.1 Tổng quan về robot 5 bậc tự do sử dụng trong luận văn.
Robot được sử dụng trong luận văn là cánh tay robot 5 bậc tự do gồm 5 khớp
xoay được thiết kế sẵn. Robot bao gồm phần tay máy được thiết kế với 3 bậc tự do
và phần cổ tay được thiết kế với 2 bậc tự do. Do được thiết kế như vậy, vị trí x, y, z
của end-effector sẽ được quyết định bởi 3 khớp xoay đầu tiên, 2 khớp xoay còn lại
sẽ điều khiển hướng của end-effector.
Hình 3.1: Mô hình thực tế robot sử dụng trong luận văn.

26. 13
Hình 3.2: Kích thước các chi tiết trên mô hình robot.
θ1
θ3 θ4
θ5
θ2
Hình 3.3: Sơ đồ động học robot

27. 14
Hệ dẫn động của robot là các động cơ điện DC servo của hãng Harmonic
Driver, nguồn cấp động cơ là nguồn 24V, encoder có độ phân giải là 200
xung/vòng. Động cơ sử dụng hộp giảm tốc Harmonic. Dựa vào datasheet động cơ
của hãng ta có được các thông số động cơ tại các khớp như sau:
Bảng 3.1: Thông số các động cơ tại khớp của robot
Khớp
Tỉ số truyền
hộp giảm tốc
Tỉ số truyền
ngoài bánh
răng
Độ phân giải
encoder
(xung/vòng)
Maximum
output Torque
(N.m)
1 110 1 22000 20
2 110 2.8 61600 50
3 100 2.5 50000 25
4 100 1 20000 5
5 50 1 10000 3.5
Để có thể điều khiển được cánh tay robot cần phải tính toán động học, động
lực học và điều khiển cánh tay di chuyển theo quỹ đạo trên phần mềm mô phỏng.
Hình 3.4: Cánh tay robot trên SolidWork

28. 15
3.2 Thiết kế tay gắp
Tùy theo công dụng của robot được sử dụng trong quá trình công nghệ , phần
công tác cuối cùng của tay máy được trang bị những bộ phận công tác khác nhau,
có thể là những thiết bị gắp, kẹp để thực hiện việc gắp và giữ đối tượng, cũng có thể
là những dụng cụ khác như đầu hàn hồ quang tự động, đầu phun sơn, phun men.
Theo mục tiêu của luận văn cần thiết kế một tay kẹp để lắp sản phẩm trên băng
tải. Cơ cấu đầu gắp đảm nhiệm vai trò rất quan trọng trong việc điều khiển chính
xác vị trí của robot cũng như điều khiển bám quỹ đạo. Ngoài ra đầu gắp còn rất
quan trọng trong nhiệm vụ chính là gắp chính xác để giữ vật. Do đó thiết kế cơ cấu
đầu gắp cho robot 5 bậc tự do cần phải có sự đòi hỏi về độ chính xác, khả năng linh
hoạt theo các hướng, kết cấu đơn giản, đảm bảo lực kẹp giữ vật chắc chắn, nhanh và
chính xác, tải nhẹ và dễ dàng di chuyển trong không gian. Từ những yêu cầu trên,
dưới đây là một số phương án phân tích lựa chọn.
3.2.1 Phƣơng án hệ dẫn động cho tay gắp.
Tay gắp sử dụng động cơ điện
Ưu điểm : nhỏ gọn là một trong những ưu điểm nổi bật, cơ cấu tác động
nhanh và chính xác. Có thể thay đổi áp suất kẹp ,có khả năng áp dụng kỹ thuật điều
khiển phức tạp cho các chuyển động, giá thành không cao và nhiều động cơ có
momen lớn.
Nhược điểm : khe hở bộ truyền bánh răng làm giảm độ chính xác, có thể gây
quá nhiệt khi quá tải.
Tay gắp sử dụng xi lanh khí nén
Ưu điểm : nhả và kẹp nhanh, giá thành không cao, nguồn khí nén phổ biến
trong công nghiệp, cơ cấu tác động có thể dừng mà không hư hỏng.
Nhược điểm : lực kẹp không lớn, áp suất khí nén giới hạn sự điều khiển và độ
chính xác. Khí xả gây ồn, khí bị rò rỉ gây trở ngại, khó điều khiển tốc độ.

29. 16
Tay gắp sử dụng xi lanh thủy lực
Ưu điểm : lực kẹp lớn, có thể chịu tải trọng nặng, tốc độ chạy êm.
Nhược điểm : Giá thành chi phí cao, không thích hợp với cơ cấu tác động
nhanh. Cần có hệ thống xả dầu, chiếm diện tích so với các nguồn dẫn động khác. Sự
rò rỉ dầu dây ô nhiễm và nguy hiểm.
Kết luận : Từ những yêu ưu và nhược điểm của từng hệ, chọn phương án tay
kẹp sử dụng nguồn dẫn động là động cơ điện. Do tay kẹp sử dụng động cơ điện có
khả năng kẹp giữ vật nhanh, chính xác, có thể điều chỉnh được lực kẹp dễ dàng.
3.2.2 Phân tích lựa chọn động cơ điện cho tay gắp
Động cơ bƣớc
Ưu điểm : điều khiển chính xác góc quay, giá thành rẻ.
Nhược điểm : dòng từ driver tới cuộn dây động cơ không thể tăng hoặc giảm
trong lúc hoạt động. Do đó, nếu bị quá tải động cơ sẽ bị trượt bước gây sai lệnh
trong điều khiển. Động cơ bước gây nhiễu ,rung động và không thích hợp cho các
ứng dụng cần tốc độ cao .
Động cơ servo
Ưu điểm : vòng điều khiển là vòng kín ,nếu tải đặt vào động cơ tăng bộ điều
khiển sẽ tăng dòng tới cuộn dây động cơ giúp tiếp tục quay, tránh hiện tượng trượt
bước động cơ và có thể hoạt động ở tốc độ cao.
Nhược điểm : động cơ servo hoạt động không trùng khớp với lệnh điều
khiển bằng động cơ bước, giá thành cao. Khi dừng lại, động cơ servo thường dao
động tại vị trí dừng dây rung lắc.

30. 17
Bảng 3.2: Bảng so sánh động cơ bước và động cơ servo
Các tính năng Động cơ bƣớc Động cơ servo
Mạch driver
Đơn giản. Người dùng
có thể chế tạo chúng
Mạch phức tạp. Thông
thường người sử dụng
phải mua mạch driver từ
các nhà sản xuất.
Nhiễu và rung động Đáng kể Rất ít
Tốc độ
Chậm (tối đa 1000-2000
rpm)
Nhanh hơn (tối đa 3000-
5000 rpm)
Hiện tượng trượt bước
Có thể xảy ra (Nếu tải
quá lớn) Khó xảy ra
Khó xảy ra (Động cơ vẫn
chạy trơn tru nếu tải đặt
vào tăng)
Phương pháp điều
khiển
Vòng hở (không
encoder)
Vòng kín (có encoder)
Giá thành
(Động cơ + driver)
Rẻ Đắt
Độ phân giải
2 pha PM: 7.5° (48 ppr)
2 pha HB: 1.8° (200 ppr)
hoặc 0.9° (400 ppr)
5 pha HB: 0.72° (500
ppr) hoặc 0.36° (1000
ppr)
Phụ thuộc độ phân giải
của encoder.
Thông thường vào
khoảng 0.36° (1000ppr) –
0.036° (10000ppr)
Kết luận : dựa vào yêu cầu của tay gắp cần đảm bảo độ cứng vững, ít
rung động có thể hoạt động ở tốc độ cao. Do đó, phương án sử dụng động cơ servo
được chọn để đáp ứng yêu cầu trên.

31. 18
Bảng 3.3: Bảng so sánh các loại động cơ servo
R/C servo
DC servo có
chổi than
DC servo
không chổi
than
AC servo
Ƣu
điểm
Vòng điều
khiển là vòng
kín, có thể
điều khiển
chính xác vị
trí, tích hợp
mạch điều
khiển bên
trong.
Vận hành ở tốc
độ cao, vận hành
êm, tỷ lệ moment
trên khối lượng
cao. Động cơ cho
moment ổn định.
Có thể tăng tốc
trong thời gian
ngắn, hiệu suất
cao.
Có những ưu
điểm và khắc
phục được
những nhược
điểm của động
cơ DC có chổi
than.
Điều khiển
chính xác
moment lớn,
chi phí bảo trì
thấp, khả năng
dùng ngõ ra
đồng bộ AC
cao.
Nhƣợc
điểm
Góc quay bị
giới hạn. Giá
thành rẻ hơn
so với các
loại khác.
Hạn chế chủ yếu
của động cơ DC
có chổi than là
chổi than bị mòn,
phát sinh tia lửa
điện, động cơ
thoát nhiệt khó
khăn.
Do động cơ
kích từ bằng
nam châm vĩnh
cửa nên giá
thành cao.
Cuộn dây là
stato có thể
nặng, khó gia
tốc.
Hệ điều chỉnh
tốc độ phức tạp
và đắt tiền.
Kết luận : Tay gắp cần một động cơ có monent đủ để kẹp giữ vật khi di
chuyển, kết cấu gọn nhẹ để đảm bảo khối lượng tải nhẹ dễ dàng di chuyển, có thể
điều khiển dễ dàng chính xác. Do đó động cơ R/C servo được chọn.

32. 19
3.2.3 Phƣơng án thiết kế cơ khí cho tay gắp.
Phƣơng án 1 : Sử dụng cơ cấu có nhiều hơn 2 chấu kẹp
Hình 3.5: Cơ cấu 3 chấu kẹp và 4 chấu kẹp
Ưu điểm: gắp được vật có hình dạng phức tạp.
Nhược điểm: Kết cấu, tính toán và điều khiển phức tạp.
Phƣơng án 2: Thiết kế sử dụng cơ cấu tay quay con trượt để đảm bảo sự di
chuyển song song của 2 má kẹp.
.
Hình 3.6: Cơ cấu tay quay con trượt
(1 Má kẹp , 2 Khớp trượt , 3 Thanh nối , 4 Tay quay, 5 Đế , 6 Động cơ , 7 Khớp
gắn trục động cơ, 8 Bạc nối động cơ , 9 Động cơ R/C servo , 10/ Thanh dẫn)

33. 20
Hình 3.7: Mô hình cơ cấu gắp phương án 2 và sơ đồ động.
Ưu điểm : kết cấu có độ cứng vững cao, có thể kẹp giữ vật nhanh và không
rung động , dễ dàng tính toán động học vị trí cho cơ cấu để xác định vị trí
end ‒effector. Do đó, thuận lợi cho quá trình tính toán vị trí điều khiển của robot, cơ
cấu gọn, nhẹ và linh hoạt trong việc gắp vật, có thể thay đổi khoảng cách hai má
kẹp để tăng giảm phạm vi kẹp vật.
Nhược điểm : việc gia công, chế tạo và lắp ráp khó khăn.
Phƣơng án 3:
Hình 3.8: Minh họa cơ cấu. Hình 3.9: Sơ đồ nguyên lý.
Ưu điểm : cơ cấu đơn giản.
Nhược điểm : xuất hiện lực dọc trong quá trình kẹp , khó khăn trong việc tính
toán, thiết kế và chế tạo bánh răng.

34. 21
Kết luận : Từ những yêu cầu đặt, chọn phương án 2 là phương án sử dụng cơ
cấu tay quay con trượt do cơ cấu có kích thước gọn, kết cấu cứng vững, đảm bảo
kẹp vật chính xác và nhanh chóng, có thể gia công và thỏa mản yêu cầu thích ứng
linh hoạt cho việc điều khiển đã nêu ra ở đầu bài toán.
3.2.4 Động học vị trí cho cơ cấu tay kẹp.
Để xác định động học vị trí cho cơ cấu tay kẹp, ta phân tích chuyển động của
tay kẹp trong hệ tọa độ XYZ gắn liền với khớp xoay thứ 5 của trục động cơ thứ 5.
Sau khi gắn tay kẹp tọa độ (x,y,z) cần điều khiển sẽ phụ thuộc vào kích thước của
tay kẹp. Do đó ta tiến hành khảo sát kích thước tay kẹp để các định giá trị a của
khâu 5 (trình bày trong chương 2 tính toán động học cho robot). Từ đó, ta xác định
được giá trị tọa độ (x,y,z) điều khiển.
θ5
Z5
X5
a
θ5
(X, Y, Z)
control
a
Hình 3.10: Tọa độ điểm cần điều khiển
Khảo sát kích thước end-effector với vật kẹp có đường kính 30mm. Độ
rộng mở tối đa lựa chọn là 40mm , các bộ phận khác của tay gắp được thiết kế
theo yêu cầu về độ rộng mở.
Hình 3.11: Kích thước tay gắp

35. 22
Sau khi gắn tay gắp giá trị a trong phương trình tọa độ (x, y, z) của
end-effector ở chương 2 sẽ thay đổi có tọa độ điều khiển :
x = T
5
0
[1,4] = ac1s234 L1c1c2 L2c1c23 L3c1c234 (3.1)
y = T
5
0
[2,4] = as1s234 L1c2s1 L2s1c23 L3s1c234 (3.2)
z = T
5
0
[3,4] = -ac234 L1s2 L2s23 L3
s234 (3.3)
Với : a = 51+100 = 151 (mm)
3.2.5 Tính toán lực kẹp cho tay gắp
Lực tác động do động cơ R/C dẫn động sinh ra, đặt lên đầu vào của tay kẹp.
Yêu cầu lực kẹp đủ lớn giữ vật trong tư thế làm việc, thông qua quan hệ cơ bản của
cơ cấu lực kẹp của tay gắp được tính toán như sau :
Vật kẹp được giữ bằng 2 tấm phẳng dựa vào ma sát.
Hình 3.12: Lực kẹp giữ vật không rơi
Vật nhựa có khối lượng m = 20g ,vuông = 30 mm, má kẹp (nhôm). Hệ số ma
sát giữa má kẹp và vật µ= 0,61. trọng lực tác dụng lên vật. Xét một bên
của má kẹp:
Ta có:
R1 R2
Rn
2
m.9,81
2
20.10 3
.9,81
2
0,0981(N) (3.4)

36. 23
Để vật được giữ, lực ma sát giữa vật với má kẹp phải bằng với trọng lực tác
dụng lên vật. Ta được phương trình cân bằng lực theo phương đứng.
R1 Fms1 N1. 0,0981(N)
(3.5)
R2 Fms2 N2. 0,0981(N)
(3.6)
Suy ra:
N1 N2
0,0981
0,61
0,16(N)
Vậy lực kẹp tác dụng cần thiết để vật được giữ là :
Fkep1 Fkep2 N1 N2 0,16(N)
(3.7)
Fkep1 Fkep2
Fmasat1
N1 N2
Fdc1
Fdc2
Fmasat2
Hình 3.13: Sơ đồ tác động lực của tay gắp ở vị trí kẹp

37. 24
Moment động cơ R/C cần thiết để cung cấp lực kẹp.
Do hai bên lực của ta gắp đối xứng nhau, ta chỉ cần xét một bên của tay gắp.
Ta có:
Khối lượng má kẹp: mA 14g
Khối lượng con trượt: mB 9g
Khối lượng thanh dẫn: mC 3g
Khối lượng tay quay: mD 1g
Hệ số ma sát giữa nhôm và nhôm 1,05 ; Góc β 52,730
Thanh E và khoảng D dài 13,5 mm và 16,5 mm.
Fkep1
N1
Fdc1
A
B
C
D
Vật gắp
E
α
β
P1
25
15
48
73
Fmasat1
Hình 3.14: Sơ đồ tác động lực một bên tay gắp ở vị trí kẹp.
Trọng lực tác dụng lên con trượt B :
N1 P1 (mA mB mC mD).10-3
.9,81 0,265(N)
(3.8)
Fmasat1 1,05.0,265 0,278(N)

38. 25
Áp dụng phương trình cân bằng lực theo phương ngang ta có :
Fdc1. cos(β) Fkep1 Fmasat1 (3.9)
Suy ra lực và moment cần thiết để giữ vật :
Fdc1 0,193(N)
Mdc1 0,193.13,5 2,61(N.mm)
Kết luận:
Động cơ được sử dụng để dẫn động tay kẹp là động cơ R/C servo MG996R có
moment là 981(N.mm). Với giá trị moment cần thiết là 2,61(N.mm), động cơ R/C
hoàn toàn có thể đảm bảo việc kẹp giữ được vật chắc chắn trong quá trình hoạt
động.
Mô hình tay kẹp được chế tạo gia công từ vật liệu nhôm đảm bảo yêu cầu đặt
ra. Mô hình 3D cánh tay robot sau khi gắn tay kẹp và không gian hoạt động.
44 339
590
120
72
223
386
152 51
R
3
3
9
R
1
2
7
140º
140º
Hình 3.15: Mô hình cánh tay robot 3D và không gian hoạt động.

39. 26
3.3 Sơ lƣợc động học vị trí của robot
Một tay máy có thể biểu diễn bằng một chuỗi động học kín hoặc hở, bao gồm
các khâu liên kết với nhau thông qua các khớp quay hoặc tịnh tiến với mục đích là
thay đổi tư thế, tầm với, điểm tác động của robot. Để điều khiển được chuyển động
đó, ta cần xác định được vị trí của từng khâu, khớp robot trong không gian. Có hai
bài toán cơ bản về động học vị trí của robot.
Động học thuận: Bài toán cho trước góc quay, khoảng cách tịnh tiến của các
khâu trong cơ cấu ứng với gốc tọa độ mỗi khâu đã được gắn và yêu cầu xác định vị
trí, hướng của cơ cấu tác động cuối cùng robot so với hệ tọa độ gốc.
Động học ngƣợc: Bài toán cho trước vị trí, hướng của khâu tác động cuối
cùng robot và yêu cầu xác định các góc quay, khoảng tịnh tiến của từng khâu cần
thiết để robot có vị trí cho trước đó.
3.3.1 Hệ tọa độ - thông số DH
Để giải bài toán động học thuật của robot, Denavit và Hartenberg đã đề xuất
phương án gắn hệ trục tọa độ lên các khâu của robot, để từ đó chuyển đổi tọa độ của
điểm thao tác về hệ tọa độ gắn liền với hệ quy chiếu cố định. Hệ tọa độ Denavit –
Hartenberg được xây dựng như sau :
Trục Zi của hệ tọa độ gắn lên khâu thứ i đặt dọc theo trục khớp thứ i+1.
Trục Xi thường được đặt dọc theo pháp tuyến chung và hướng từ trục Zi-1 đến Zi
Trục Yi xác định theo quy tắc bàn tay phải Yi = Zi x Xi.
Sau khi đặt hệ tọa độ, ta xác định được các thông số theo nguyên tắc Denavit-
Hartenberg.
ai: khoảng cách giữa 2 trục Zi-1 và Zi theo chiều Xi .
i: góc quay cần thiết của trục Zi-1 theo chiều trục Xi đến khi song song với trục Zi .
di: khoảng cách giữa trục Xi-1 và Xi theo chiều Zi-1 .
: góc quay cần thiết của trục Xi-1 theo chiều chục Zi-1 đến khi song song với trục.

40. 27
Hình 3.16: Định nghĩa các thông số DH cho khớp và khâu thứ i
Động học thuận cho robot 5 bậc tự do
Kết cấu robot khảo sát :
θ1
θ3 θ4
θ5
L1 L2 L3
a
θ2
Hình 3.17: Kết cấu robot khảo sát
Kích thước các khâu :
L1 = 152(mm), L2 = 120 (mm), L3 = 43 (mm), a = 51(mm)
Góc giới hạn của các khớp:
θ1: 1400
θ2: 450
θ3: 900
600
θ4< 1800
θ5: 3600

41. 28
 Đặt hệ tọa độ, gọi tên các khâu, khớp :
Z0 = Y1
Y0
X1=X0
Y2
X2
Y3
X3
X4
Z4
Z5
X5
Hình 3.18: Đặt hệ tọa độ cho các khâu và khớp
Bảng 3.4: Bảng thông số D-H:
Khâu thứ i ai (mm) αi (độ) θi (độ) di (mm)
1 0 90 θ1 0
2 L1 0 θ2 0
3 L2 0 θ3 0
4 L3 90 θ4 0
5 0 0 θ5 a

42. 29
Các ma trận chuyển đổi:
T
1
0
[
c1 0 s1 0
s1 0 c1 0
0 1 0 0
0 0 0 1
] T
2
1
[
c2 s2 0 L1.c2
s2 c2 0 L1.s2
0 0 1 0
0 0 0 1
]
T
3
2
[
c3 s3 0 L2.c3
s3 c3 0 L2.s3
0 0 1 0
0 0 0 1
] T
4
3
[
c4 0 s4 L3.c4
s4 0 c4 L3.s4
0 1 0 0
0 0 0 1
]
T
5
4
[
c5 s5 0 0
s5 c5 0 0
0 0 1 a
0 0 0 1
]
Ma trận chuyển đổi từ hệ tọa độ tool về hệ toàn cục:
T
5
0
[
s1s5 c5c1c234 c5s1 s5c1c234 c1s234 px
c1s5 c5s1c234 c1c5 s5s1c234 s1s234 py
c5s234 s5s234 c234 pz
0 0 0 1
]
Trong đó:
px
= ac1s234 L1c1c2 L2c1c23 L3c1c234
py
= as1s234 L1c2s1 L2s1c23 L3s1c234
pz
= -ac234 L1s2 L2s23 L3
s234
Kết quả của bài toán động học thuận là vị trí tọa độ x, y, z của tool trong không gian
XYZ phụ thuộc vào các giá trị của các biến khớp:
x = T
5
0
[1,4] = ac1s234 L1c1c2 L2c1c23 L3c1c234 (3.10)
y = T
5
0
[2,4] = as1s234 L1c2s1 L2s1c23 L3s1c234 (3.11)
z = T
5
0
[3,4] = -ac234 L1s2 L2s23 L3
s234 (3.12)
Trong đó: s234 sin ( 2 3 4) và c234 cos ( 2 3 4)

43. 30
3.3.2 Động học nghịch cho robot 5 bậc tự do
Ta phân tích phương trình:
T
1
0 1
. T
5
0
T
5
1
T
2
1
. T
3
2
. T
4
3
. T
5
4
Dựa vào bài toán động học thuận ta có:
VT : T
1
0 -1
[
c1 s1 0 0
0 0 1 0
s1 -c1 0 0
0 0 0 1
] ; T
5
0
[
T11
5
0
T12
5
0
T13
5
0
x
T21
5
0
T22
5
0
T23
5
0
y
T31
5
0
T32
5
0
T33
5
0
z
0 0 0 1]
VP =
[
c5c234 -s5c234 s234 as234 L1c2 L2c23 L3c234
c5s234 -s5s234 - c234 -ac234 L1s2 L3s234 L2s23
s5 c5 0 0
0 0 0 1 ]
Cân bằng 2 vế ở hàng 3 cột 4 ta có:
T
5
1
3,4 s1.x c1.y 0
Kết quả giá trị góc 1:
1 arctan2( y,x) (3.13)
và 1 arctan2 ( y,x ) 1800
Ta có 2 khớp tiếp theo là 2 khớp song song nên sẽ không có kết quả nào
nhận được từ phép nhân với những ma trận nghịch đảo T-1
i 1
i
. Ta khảo xác
phương trình ở khâu thứ 3.
T 1
. T 1
3
2
. T 1
2
1
. T 1
1
0
. T
5
0
T
5
4
4
3
VT: T-1
. T-1
3
2
. T-1
2
1
4
3
[
c234 s234 0 -L1c34-L2c4-L3
0 0 1 0
s234 -c234 0 -L1s34-L2s4
0 0 0 1
]

44. 31
[
c1 s1 0 0
0 0 1 0
s1 -c1 0 0
0 0 0 1
] ;
[
T11
5
0
T12
5
0
T13
5
0
x
T21
5
0
T22
5
0
T23
5
0
y
T31
5
0
T32
5
0
T33
5
0
z
0 0 0 1]
VP : T
5
4
[
c5 -s5
0 0
s5 c5 0 0
0 0 1 a
0 0 0 1]
Cân bằng 2 vế ở hàng 1 cột 3 ta có:
c234.(c1. T13
5
0
s1. T23
5
0
) + s234. T33
5
0
= 0
s234
c234
(c1. T13
5
0
s1. T23
5
0
)
T33
5
0
Suy ra:
( 2 3 4) arctan2(-( ) T33
5
0
) (3.14)
và ( 2 3 4) arctan2(-( ) T33
5
0
) 18 0
Cân bằng hàng 1 cột 4 và hàng 3 cột 4 ta được :
{
c234(c1.x s1.y) s234.z L1c34 L2c4 L3
s234(c1.x s1.y) c234.z L1s34 L2s4 a
Bình phương 2 vế 2 phương trình và cộng lại ta có:
(c234
(c1.x s1.y) s234.z L3)
2
(s234(c1.x s1.y) c234.z a)
2
c3
(c234
(c1.x s1.y) s234.z L3)
2
(s234(c1.x s1.y) c234.z a)2
L1
2
L2
2
2L1L2
s3 √1-c3
2
Khi cánh tay robot lúc nâng lên và hạ xuống tương ứng với 2 cặp nghiệm.
3 arctan2( s3,c3)
Quay lại với phương trình:
T
1
0 1
. T
5
0
T
5
1
T
2
1
. T
3
2
. T
4
3
. T
5
4

45. 32
Cân bằng 2 vế ở hàng 1 cột 4 và hàng 2 cột 4 ta có:
{
c1.x s1.y as234 L1c2 L2c23 L3c234
z ac234 L1s2 L3s234 L2s23
{
c1.x s1.y as234 L3c234 (L1 L2c3)c2 L2s3s2
z ac234 L3s234 L2s3c2 (L1
L2c3)s2
|
(L1 L2c3) L2s3
L2s3 (L1
L2c3)
|
c |
c1.x s1.y as234 L3c234 L2s3
z ac234 L3s234 (L1
L2c3)|
s |
(L1 L2c3) c1.x s1.y as234 L3c234
L2s3 z ac234 L3s234
|
c2
c
(c1.x s1.y as234 L3c234).(L1
L2c3) L2s3(z ac234 L3s234)
(L1 L2c3)(L2
c3 L1) (L3s3)2
( )
s2
s (z ac234 L3s234).(L1 L2c3) L2s3(c1.x s1.y as234 L3c234)
(L1 L2c3)(L2
c3 L1) (L3s3)2
( )
Kết quả thu được:
2 arctan2(s2,c2)
(3.17)
Đến đây 4 được xác định bởi: 4 234- 3- 2
(3.18)
Kết quả của bài toán ngược:
1 arctan2(y,x)
1 arctan2(y,x) 1800
234 = arctan(s23,c23) arctan(-(c1. T13
5
0
s1. T23
5
0
), T33
5
0
)
234 234 1800
3 arctan(s3,c3)

46. 33
với s3 √1-c3
2
c3
(c234
(c1.x s1.y) s234.z L3)
2
(s234(c1.x s1.y) c234.z a)
2
L1
2
L2
2
2L1L2
2 = arctan( s2,c2)
c2
(c1.x s1.y as234 L3c234).(L1
L2c3) L2s3(z ac234 L3s234)
(L1 L2c3)(L2
c3 L1) (L3s3)2
(z ac234 L3s234).(L1 L2c3) L2s3(c1.x s1.y as234 L3c234)
(L1 L2c3)(L2
c3 L1) (L3s3)2
Hai nghiệm của 2 ứng với hai góc 234 thể hiện 2 hình ảnh (RIGHT,
ABOVE – RIGHT, BELOW) và (LEFT, ABOVE – LEFT, BELOW).
θ2
θ3
L2
L1
L3
RIGHT
ABOVE
RIGHT
BELOW
Hình 3.19: Hình ảnh ứng với trạng thái RIGHT-ABOVE và RIGHT-BELOW

47. 34
θ2
θ3
L2
L1
L3
LEFT
ABOVE
LEFT
BELOW
Hình 3.20: Hình ảnh ứng với trạng thái LEFT-ABOVE và LEFT-BELOW
3.4 Thiết kế mạch điện.
3.4.1 Tổng quan về mạch điện.
Yêu cầu đặt ra cho hệ thống mạch điện là một vi điều khiển sẽ nhận tín hiệu
điều khiển từ máy tính, sau đó truyền tín hiệu này đến 5 vi điều khiển khác để điều
khiển 5 động cơ của 5 khớp cánh tay robot. Đồng thời vi điều khiển này phải đọc tín
hiệu của cảm biến và xuất tín hiệu điều khiển động cơ tay kẹp. Các vi điều khiển
giao tiếp với nhau bằng mạng giao tiếp ổn định, ít sai số, chống nhiễu, mạch giao
tiếp đơn giản và đặc biệt hỗ trợ cho việc điều khiển thời gian thực ( do cánh tay
robot phải đảm bảo về thời gian di chuyển chính xác để có thể đồng bộ với tốc độ
băng tải và lắp vật đúng vị trí ). Vi điều khiển nhận tín hiệu của máy tính sẽ giao
tiếp với máy tính bằng chuẩn RS232. Tiến hành so sánh các loại giao tiếp giữa các
vi điều khiển để đưa ra lựa chọn, ta tiến hành so sánh ưu, nhược điểm của 3 loại
giao tiếp phổ biến nhất là I2C, SPI và giao tiếp CAN.

48. 35
Bảng 3.5: Bảng so sánh các loại giao tiếp
I2C SPI CAN
Khái niệm
hệ thống
Multiple
masters,
multiple slaves
Single master,
multipe slaves
Multiple master, multiple
slaves
Số đường
dây tín hiệu
2, (SCL, SDA) 4, ( active-low
CS, SI, SO, SCK)
2, ( CAN_H, CAN_L)
Tốc độ
truyền
Tối đa 3,4
Mbps
Tối đa 10 Mbps Tối đa 1 Mbps
Phân biệt
giữa các
nodes
truyền
Dùng địa chỉ
phân biệt. Phân
biệt chủ tớ giữa
các thiết bị.
Dùng một đường
truyền phân biệt
một thiết bị. Phân
biệt chủ và tớ
giữa các thiết bị.
Không phân biệt giữa các
nodes, chỉ phân biệt địa chỉ
tin nhắn. Các node bình
đẳng không phân biệt chủ
tớ và có thể giao tiếp độc
lập với nhau.
Khả năng
chống nhiễu
Không truyền
xa, dễ bị nhiễu
tín hiệu.
Không truyền xa,
dễ bị nhiễu tín
hiệu.
Có thể truyền xa, có khả
năng chống nhiễu do đường
truyền CAN có tính chất vi
sai.
Hoạt động
tốt khi
Giao tiếp nhiều
chủ nhiều tớ
Giao tiếp một chủ
một tớ
Giao tiếp tốt trong mọi
trường hợp vì khả năng độc
lập cao
Mức độ ưu
tiên giữa
các dữ liều
truyền
Không phân
biệt
Không phân biệt Có phân biệt mức độ ưu
tiên truyền của từng tin
nhắn. Ý nghĩa quan trong
trong việc thiết kế hệ thống
nhúng thời gian thực

49. 36
Kết luận:
Hệ thống dữ liệu truyền không cần tốc độ quá cao, chỉ cần độ chính xác, khả
năng chống nhiễu và hệ thống mạch điện đơn giản. Do đó mạng giao tiếp CAN
được chọn để truyền nhận tín hiệu giữa các vi điều khiển.
Ta chọn vi điều khiển PIC18F4680 vì loại PIC dễ sử dụng, giá thành không
cao, tích hợp đầy đủ các module cần thiết như giao tiếp Rs232, module giao tiếp
CAN, module ngắt ngoài đọc tín hiệu cảm biến, module timer bắn xung điều khiển
driver. Do việc tính toán động học được thực hiện toàn bộ bằng phần mềm Visual
Studio 2010 trên máy tính nên không đòi hỏi PIC có tốc độ xử lý cao cũng như tài
nguyên lớn.

50. 37
PIC18F4680
TRANSMIT
PIC18F4680
RECEIVE
Driver 1 Motor 1
Encoder 1
PIC18F4680
RECEIVE
Driver 2 Motor 2
Encoder 2
PIC18F4680
RECEIVE
Driver 3 Motor 3
Encoder 3
PIC18F4680
RECEIVE
Driver 4 Motor 4
Encoder 4
PIC18F4680
RECEIVE
Driver 5 Motor 5
Encoder 5
Encoder 1
Limit Switch 1
Limit Switch 2
Limit Switch 3
Limit Switch 4
Limit Switch 5
Cảm biến tiệm
cận
Motor 7
Tay kẹp
Driver 6
Motor 6
Băng tải
Encoder 6
RS232
RS232
CAN
PIC18F4680
RECEIVE
I/O
Hình 3.21: Sơ đồ khối kết nối hệ thống

51. 38
3.5 Mạch điều khiển
Từ những yêu cầu của hệ thống, ta thiết kế mạch điều khiển gồm một mạch
transmit truyền tín hiệu và 6 receive nhận tín hiệu để điều khiển 5 động cơ của 5
khớp cánh tay robot và 1 mạch để điều khiển động cơ tay kẹp. Mạch Transfer đọc
tín hiệu cảm biến tiệm cận các mạch giao tiếp với nhau bằng giao tiếp can giao tiếp
với máy tính bằng RS232.
Hình 3.22: Sơ đồ mạch điện vi điều khiển Transmit

52. 39
Hình 3.23: Sơ đồ mạch điện vi điều khiển Receive
Trong thiết kế tất cả các mạch đều có thể giao tiếp với nhau bằng giao tiếp
CAN, do PIC18F4680 đã hỗ trợ 2 chân giao tiếp CAN nên đã chỉ cần sử dụng thêm
MCP2551 để chuyển đổi mức điện áp tín hiệu cho phù hợp với điện áp trong CAN
bus.
Bên cạnh giao tiếp CAN, các mạch vi điều khiển Transmit và Receive đều có
khả năng giao tiếp với máy tính bằng giao tiếp RS232 giúp thuận tiện trong việc
điều khiển và kiểm tra dữ liệu truyền trong mạng giao tiếp. Để thực hiện được giao
tiếp RS232 với máy tính ta thiết kế cho các mạch điều khiển có thêm Max232 và
kết nối với máy tính qua cổng COM. Chức năng của Max232 là để đảo ngược điện

53. 40
áp và động bộ điện áp của 2 chuẩn TTL (transistor-transistor logic) của vi điều
khiển và chuẩn RS-232 của máy tính.
Dưới đây là sơ đồ nguyền lý kết nối của Max232 trong mạch điều khiển :
Hình 3.24: Sơ đồ giao tiếp RS232
Việc truyền dữ liệu nhờ vào cập dây truyền tín hiệu vi sai, nó có ý nghĩa là tín
hiệu được lấy dựa vào sự sai khác giữa 2 đường dây CAN H và CAN L. Đường dây
bus được kết thúc bằng điện trở 120Ω.
Hình 3.25: Sơ đồ CAN bus
Driver được sử dụng trong luận văn là driver DCS3T-25 với nguồn cung cấp
cho driver 24V , công suất 300W đến 600W, giao tiếp với máy tính qua RS232 ,
ngõ vào xung encoder, ngõ vào cách ly 5VDC (PULSE, DIR, CLK).

54. 41
Hình 3.26: Driver DCS3T-25
Mô tả các ngõ vào và ra trên mạch Driver :
Nguồn cung cấp cho Driver từ 15 đến 24VDC (+24VDC, GND)
Các tín hiệu ngõ vào cách ly +5VDC (DIR+, DIR-, CLK+, CLK-)
Nguồn cho động cơ 24 tới 100VDC (V+_MOTOR, GND)
Ngõ ra nối với động cơ (MOTOR+, MOTOR-)
Tín hiệu từ encoder (GND, +5VDC, E_INDEX, E_QEA, E_QEB)
Cổng RS232 để kết nối với phần mềm.

55. 42
Hình 3.27: Sơ đồ mạch kết nối nguồn 24V, động cơ và driver
Mạch kết nối nguồn với động cơ và kết nối động cơ với driver. Mạch này
được thiết kết các công tắc đóng mở cấp nguồn cho từng động cơ, giúp cho việc
kiểm soát nguồn cấp và có thể ngắt an toàn khi hoạt động.
Hình 3.28: Sơ đồ mạch nguồn cho vi điều khiển và cảm biến tiệm cận

56. 43
Mạch nguồn cung cấp cho các vi điều khiển sử dụng IC LM2596-ADJ có khả
năng chịu dòng max 3A, điện ấp cấp DC tối đa là 40V hoàn toàn có thể dùng cho
nhiều vi điều khiển (PIC18F4680 dòng tối đa khoảng 100mA, bình thường là
25mA). Các mạch nguyên lí và PCB được vẽ bằng phần mềm Altium.
Hình 3.29: Mạch 3D được thiết kế băng phần mềm Altium
Mạch thiết kế được chuyển sang giao diện 3D để có thể sắp xếp vị trí các chi
tiết và kiểm tra việc chồng lấn các chi tiết trước khi tiến hành chế tạo mạch.
Hình 3.30: Mạch điều khiển sau khi được chế tạo

57. 44
Hình 3.31: Hệ thống mạch điện điều khiển
Hệ thống mạch điện gồm: mạch Transmit, 6 mạch Receive, 6 Driver điều
khiển động cơ sử dụng DCS3T-25,1 mạch nguồn sử dụng IC LM2596-ADJ, mạch
cấp nguồn động cơ và Driver, mạng CAN.
Hệ thống gồm có PC truyền nhận dữ liệu với mạch Transmit, mạch điều
khiển, điều khiển hoạt động robot, bộ nguồn chuyển đổi 220V AC  24V DC cấp
nguồn cho robot hoạt động và cho driver điều khiển, mô hình Robot 5 bậc tự do và
đầu công tác.

58. 45
3.6 Giao diện điều khiển
Hình 3.32: Giao diện điều khiển trên máy tính
Giao diện được thiết kết để điều khiển tất cả hoạt động cánh tay robot. Giao
diện được viết bằng ngôn ngữ C# trên Visual Studio 2010. Đây là nơi tính toán
động học thuận, động học ngược và quĩ đạo của cánh tay sau đó mới gửi về cho
mạch Transmit, từ mạch Transmit sẽ chuyển dữ liệu đến các mạch Receive.
Giao diện bao gồm các Module sau:
RS232 kết nối 232 giữa máy tính và mạch điều khiển.
MANUAL điều khiển vị trí theo bài toán động học thuận và nghịch.
AUTO di chuyển giữa 2 điểm với quỹ đạo đường thẳng kiểm soát được số
điểm tựa và thời gian di chuyển quỹ đạo.
Thực hiện việc gắp hộp lập phương từ vị trí mặc định tới vị trí mới để kiểm tra
quỹ đạo thiết lập và thời gian di chuyển của cánh tay, có 2 quĩ đạo thiết lập ứng với
2 vị trí cấp thùng hàng và vị trí sắp xếp thùng hàng.

59. 46
Hình 3.33: Lấy giá trị tọa độ các điểm tựa theo quỹ đạo thiết lập bằng phần
mềm Solidworks 2014
Hình 3.34: Lấy giá trị tọa độ ra file excel nhờ
chương trình xyzPathMacro

60. 47
GIẢI THUẬT ĐIỀU KHIỂN ROBOT GẮP VẬT
CHƢƠNG 4:
4.1 Bài toán điều khiển
Để cánh tay robot di chuyển từ vị trí bắt đầu đến vị trí lắp vật, cánh tay robot
phải di chuyển qua các điểm trung gian, gọi chúng là các điểm tựa. Các điểm tựa
này các vị trí bắt buộc phải đi qua để thực hiện nhiệm vụ lắp ráp và cả những vị trí
phải đi qua để tránh chướng ngại vật. Tùy theo vị trí lắp ráp mà ta quyết định số
điểm tựa. Khi biết vị trí và định hướng của khâu công tác ta áp dụng bài toán động
học ngược để xác định được các giá trị biến khớp (toạ độ suy rộng) để điều khiển
chuyển động của từng khớp và tổng hợp lại thành chuyển động chung của robot
theo quỹ đạo mong muốn , hay nói chính xác hơn là theo các điểm tựa của quỹ đạo.
Bài toán điều khiển lúc này đặt ra phải thiết kế một quỹ đạo lắp vật không chỉ
dừng lại ở vị di chuyển từ điểm này đến điểm khác mà quỹ đạo bây giờ đòi hỏi phải
phụ thuộc vào thời gian di chuyển. Do nhiệm vụ đặt ra là cánh tay robot phải lắp vật
chính xác lên bảng làm việc khi bảng làm việc này di chuyển trên băng tải. Vì vậy,
ta tiến hành khảo sát sự chuyển động giữa các điểm tựa có ràng buộc về yếu thời
gian, nó đồng nghĩa với ràng buộc về yếu tố tốc độ và gia tốc của cơ cấu robot.
4.2 Quỹ đạo chuyển động
Có 2 cách tiếp cận và lựa chọn quy luật chuyển động hợp lý giữa các điểm
tựa, cách thứ nhất được tiến hành trong hệ tọa độ suy rộng, tức là trong không gian
các biến khớp i(t). Khi đó, nếu cho trước các điều kiện như đảm bảo độ liên tục và
điều hòa ở những điểm chuyển tiếp. Ta có thể xác định các hệ số của hàm đa thức
biểu thị gần đúng đường cong quỹ đạo. Lập trình quỹ đạo trong hệ tọa độ các biến
khớp sẽ cho biết thông tin về biến khớp là những thông số điều khiển trực tiếp các
khớp động . Vì vậy có nhiều khả năng đảm bảo thời gian thực trong điều khiển, việc
lập trình dễ dàng. Nhược điểm chính là thông qua các biến khớp chưa thể hình dung
ra ngay vị trí của cơ cấu tay máy trong không gian, mà vì vậy phải tính toán nhiều
lần.

61. 48
Cách thứ hai thường dùng trong hệ tọa độ Đề các cho trước dạng hàm giải
tích, ví dụ hàm bậc nhất biểu thị quỹ đạo gồm nhiều đoạn thẳng nối tiếp nhau. Sau
đó, thay thế gần đúng quỹ đạo này bằng quan hệ hàm tương ứng của các biến khớp.
Ưu điểm của phương pháp dùng hệ tọa độ Đề các là dễ dàng quan sát được và đảm
bảo cho tay robot di chuyển theo một đường đi xác định. Nhược điểm là khối lượng
tính toán lớn và yêu cầu thời gian xử lý nhanh.
Kết luận: Với ưu điểm đảm bảo thời gian thực và việc lập trình dễ dàng ta
chọn phương pháp hoạch định quỹ đạo trong không gian biến khớp.
Cách thức xây dựng quỹ đạo, từ các tọa độ điểm tựa giải bài toán động học
ngược để tìm giá trị các biến khớp, rồi dùng phép nội suy xấp xỉ bằng hàm đa thức
bậc thấp để xây dựng quy luật thay đổi thích hợp các giá trị biến khớp, tương ứng
với quỹ đạo qua các điểm tựa. Có nhiều phương pháp nội suy xấp xỉ như đường đa
thức bậc 5, đường đa thức bậc 3.
Phương pháp đường đa thức bậc 3 , trong phương pháp đường đa thức bậc 3,
quãng đường, vận tốc và gia tốc được cho bởi biểu thức sau:
si(t) ai3t3
ai2t2
ai1t ai0 (4.1)
ṡi(t) 3ai3t2
2ai2t ai1
(4.2)
sï (t) 6ai3t 2ai2 (4.3)
Việc xác định các tham số của các đa thức dựa trên các điều kiện ràng buộc
sao cho đảm bảo điều kiện liên tục về vị trí, vận tốc và gia tốc.
Ràng buộc vị trí: vị trí ban đầu và vị trí kết thúc được biết trước, vị trí kết thúc
đoạn di thứ i là vị trí bắt đầu của đoạn thứ i+1.
si(0) sk 1
si(tk) si 1(0) sk
si 1(tk 1) sk 1

62. 49
Ràng buộc vận tốc:
ṡi(0) ṡk 1
ṡi(tk) ṡi 1(0) ṡk
ṡi 1(tk 1) ṡk 1
Ràng buộc gia tốc:
s̈i(tk) s̈i 1(0) s̈k
Phương pháp đương đa thức bậc 5
Ở phương pháp này quãng đường, vận tốc và gia tốc được cho bởi biểu thức
sau :
s(t) A t5
Bt4
Ct3
Dt2
Et F (4.4)
ṡ(t) 5At4
4Bt3
3Ct2
2Dt E (4.5)
s̈(t) 20At3
12Bt2
6Ct 2D (4.6)
Các điều kiện biên tại thời điểm t 0 (điểm A) và thời điêm t T (điểm B) :
t = 0: s = s0 ; ṡ s0
̇ ; s̈ s0
̈
t = T: s = sT ; ṡ sT
̇ ; s̈ sT
̈
Bằng cách giải biểu thức (6.7), ta có được các hệ số (A,B,C,D,E,F)
(
s0
sT
ṡ0
sT
s̈0
s̈T) (
0 0 0 0 0 1
T5
T4
T3
T2
T 1
0 0 0 0 1 0
5T4
4T3
3T2
2T 1 0
0 0 0 2 0 0
20T3
12T2
6T 2 0 0) (
A
B
C
D
E
F)
Kết luận:
Trong phương pháp đường đa thức bậc 3, quĩ đạo bao gồm một đoạn thẳng
tuyến tính và các đoạn parabol. Tuy nhiên thành phần gia tốc không liên tục như
đường đa thức bậc 5. Trong phương pháp đường đa thức bậc 5, với vận tốc ban đầu
khác 0, hoạch định quỹ đạo theo đường đa thức bậc 5 có sự vọt lố quãng đường s.

63. 50
Do nhiệm vụ đề tài chỉ dừng lại ở mức điều khiển vị trí và vận tốc nên ta chọn
đường đa thức bậc 3 để hoạch định quỹ đạo lắp vật cho cánh tay.
4.3 Hoạch định quỹ đạo
Áp dụng phương pháp hoạch định quỹ đạo dùng phép nội suy đường đa thức
bậc 3 trong không gian tọa độ khớp để hoạch định định quỹ đạo gắp vật. Ta xét
trong trường hợp chung. Giả sử khớp thứ n quay góc 𝜃𝑖 và 𝜃𝑖+ trong các khoảng
thời gian 𝑡𝑘 và 𝑡𝑘+ tương ứng, quỹ đạo đa thức bậc ba của biến khớp n có dạng:
i(t) ai3t3
ai2t2
ai1t ai0 (4.7)
Và:
i 1(t) a(i 1)3t3
a(i 1)2t2
a(i 1)1t a(i 1)0 (4.8)
Vận tốc:
̇i(t) 3ai3t2
2ai2t ai1 (4.9)
Việc xác định các tham số của các đa thức dựa trên các điều kiện ràng buộc
sao cho đảm bảo điều kiện liên tục về vị trí, vận tốc và gia tốc.
Ràng buộc vị trí:
i(0) k 1
i(tk) i 1(0) k
i 1(tk 1) k 1
Ràng buộc vận tốc:
̇i(0) ̇k 1
̇i(tk) ̇i 1(0) ̇k
̇i 1(tk 1) ̇k 1

64. 51
Ràng buộc gia tốc:
̈i(tk) ̈i 1(0) ̈k
Các khoảng thời gian được chia đều nên tk tk 1 ∆t. Giải các phương trình trên
ta được:
ai3
2( k k 1) ( ̇k
̇k 1)∆t
∆t3
(4.10)
ai2
6( k k 1) 2( ̇k 2 ̇k 1)∆t
2∆t2
(4.11)
ai1
̇k 1 (4.12)
ai0 k 1 (4.13)
Quỹ đạo đa thức bậc ba:
i(t)
2( k k 1) ( ̇k
̇k 1)∆t
∆t3 t3
6( k k 1) 2( ̇k 2 ̇k 1)∆t
2∆t2 t2 ̇k 1t k 1
(4.14)
Ta xét một quỹ đạo:
Giả sử cánh tay robot di chuyển từ điểm tựa A đến điểm tựa B. Tọa độ 2 điểm
tựa này được hoạch định sẵn và cánh tay robot phải di chuyển qua. Để đưa đầu công
tác di chuyển từ điểm tựa A đến điểm tựa B, khớp 1 sẽ di chuyển một góc từ 30o
đến 60o
thời gian hoạch định là 5s. Tốc độ tại A và B là 0.
(0) a0 30
(5) a0 5a1 52
a2 53
a3 60
̇(0) a1 0
̇(5) a1 2.5.a2 3.52
a3 0

65. 52
Ta thu được hệ số của quỹ đạo
a0 30; a1 0/s; a2 3,6/s2
; a3 0,48/s3
Phương trình biểu diễn vị trí, tốc độ và gia tốc của khớp có dạng:
(t) 30 3,6.t2
0,48.t3 (4.15)
̇(t) 7,2.t 1,44.t2 (4.16)
̈(t) 7,2 2,88.t (4.17)
Hình 4.1: Đường biểu diễn vị trí, tốc độ, gia tốc của khớp 1 theo thời gian
Tương tự đối với các góc khớp còn lại ta có thể sử dụng phương pháp đường
bậc 3 để tính toán quỹ đạo của robot trong không gian khớp.

66. 53
4.4 Phƣơng pháp điều khiển
Để điều khiển đầu công tác của robot di chuyển từ A đến B, ta điều khiển từng
khớp bám theo quỹ đạo mong muốn. Có 2 phương pháp tiếp cận vấn đề điều khiển
các khớp: điều khiển khớp độc lập và điều khiển dựa trên mô hình đối tượng.
Điều khiển khớp độc lập, mỗi khớp trong cánh tay robot được xem là một hệ
độc lập, điều khiển khớp theo quỹ đạo được hoạch định bằng các phương pháp đã
nêu trên. Phương pháp này được áp dụng rộng rãi trong công nghiệp và thường có
hai vòng điều khiển , vòng điều khiển vận tốc và vòng điều khiển vị trí. Luật điều
khiển được sử dụng là PID có nhược điểm là phải sử dụng khâu tích phân để bù sai
số điều khiển, điều này dẫn đến hệ thống cần có thời gian xác lập và độ vọt lố.
Điều khiển dựa trên mô hình hướng đối tượng: từ bài toán động lực học ta có
thể tính được lực hoặc moment cần thiết để điều khiển khớp theo quĩ đạo,đây là
phương pháp điều khiển feedforward. Tuy nhiên, điều khiển feedforward phụ thuộc
vào độ chính xác của mô hình đối tượng được tính toán, chính vì thế nhiễu sẽ ảnh
hưởng lớn đến sai số hành trình. Ví dụ, khi tải thay đổi, các thông số lực trong mô
hình sẽ thay đổi theo ,vòng điều khiển feedback được thêm vào để bù sai số trong
các trường hợp này.
Hình 4.2: Vòng điều khiển hướng đối tượng.

67. 54
Ta có thể thấy phương pháp điều khiển theo hướng đối tượng là tốt hơn vì
đảm bảo được độ chính xác với cả 2 vòng điều khiển feedback và feedforward. Tuy
nhiên, độ phức tạp sẽ cao hơn và số lượng phép tính lớn. Vì vậy, sẽ không phù hợp
với vi điều khiển có tốc độ tính toán và tài nguyên giới hạn. Do việc áp dụng hệ
thống điều khiển mới vào điều khiển cánh tay robot này và bước đầu phát triển đề
tài nên cần một hệ thống điều khiển đơn giản hơn và dễ áp dụng vào thực tiễn. Đề
tài chọn phương pháp điều khiển khớp độc lập để áp dụng vào điều khiển cánh tay
robot lắp vật.
Trong phương pháp này, mỗi khớp được xem như một hệ thống độc lập nhằm
điều khiển khớp theo quỹ đạo cho trước. Thường có hai vòng điều khiển , vòng điều
khiển vận tốc và vòng điều khiển vị trí. Do thời gian luận văn có giới hạn nên ở đây
ta chỉ sử dụng vòng điều khiển vị trí và luật điều khiển được sử dụng là PID
(proportional- integral – derivative controller).
Kết luận: Phương án điều khiển khớp độc lập bám theo quỹ đạo thường có
hai vòng điều khiển. Vòng điều khiển vận tốc và vòng điều khiển vị trí , do đề tài
chỉ dừng ở mức ban đầu và thời gian giới hạn nên đề tài đã sử dụng Driver DCS3T-
25 để thực hiện giải thuật PID. Đề tài sẽ xây dựng quỹ đạo các điểm tựa cho cánh
tay robot và sau đó đưa các tọa độ vị trí điểm tựa vào phương trình động học ngược
được lập trình bằng phần mềm C# tính ra các góc quay tương ứng của các khớp. Từ
góc quay các khớp, phần mềm sẽ tính ra số xung cần quay cho 5 động cơ tương ứng
với số góc quay cần di chuyển từ điểm tựa này đến điểm tựa khác và gửi dữ liệu này
xuống cho vi điều khiển. Theo lý thuyết, ta dựa vào phương trình nội suy đường bậc
3, có vị trí điểm mong muốn và thời gian quy định thì ta sẽ tính ra được vận tốc
mong muốn của mỗi khớp. Ứng với thời gian bắt đầu di chuyển trong 1 đoạn là thời
gian gửi dữ liệu vị trí và vận tốc từ máy tính xuống vi điều khiển. Dữ liệu vị trí bây
giờ là số xung và dữ liệu vận tốc bây giờ là tốc độ bắn xung của vi điều khiển điều
khiển driver. Thời gian di chuyển giữa các quỹ đạo là như nhau, số điểm tựa trong
quỹ đạo do ta hoạch định, do đó thời gian gi chuyển toàn bộ quỹ đạo ta sẽ tính
được. Lúc này ta chỉ cần đồng bộ thời gian di chuyển từ vị trí cảm biến của bảng

68. 55
làm việc đến vị trí lắp vật bằng với thời gian di chuyển của cánh tay robot thì có thể
lắp vật chính xác vào vị trí mong muốn.
4.5 Giải thuật điều khiển
Để có thể lắp vật chính xác vào vị trí cần lắp ráp khi bảng làm việc di chuyển
trên băng tải đòi hỏi hệ thống phải có cảm biến xác định vị trí của bảng làm việc so
với vị trí cần gắp. Từ yêu cầu đó, ta sử dụng cảm biến tiệm cận để thực hiện công
việc này. Khoảng cách đặt cảm biến là cố định, vận tốc băng tải cố định được điều
khiển bởi Driver DCS3T-25 do đó thời gian di chuyển của bảng làm việc đến vị trí
lắp ráp là xác định được.
Hình 4.3: Lưu đồ tổng quát điều khiển cánh tay lắp vật

69. 56
S
Đ
S
Đ
S S
Đ
S Đ
Hình 4.4: Lưu đồ giải thuật điều khiển trên Transmit khi tiến hành lắp vật
Start Nhận dữ liệu
từ máy tính
End
Return
Khai báo cấu hình hoạt động
cho các chức năng : Can, Uart
Khai các biến sự kiện ngắt
Kích hoạt ngắt
Truyền tín hiệu lên máy tính
Nhận dữ liệu từ máy tính
Gởi dữ liệu nhận được cho 6
mạch Receive , qua mạng Can
và tín hiệu output
Đọc các sai số từ mạch Receive
điều khiển khớp tay máy.
Lưu dữ liệu nhận được
Bật cớ ngắt
Truyền tín hiệu lên máy
tính
Tín hiệu cảm
biến tiệm cận ?
Ngắt Rs232 ?
Dữ liệu nhận =
32 byte ?
Err1 = 0
Err2 = 0
Err3 = 0
Err4 = 0
Err5 = 0

70. 57
S S
Đ
S
Đ
S
Đ
S
Đ
Hình 4.5: Lưu đồ giải thuật điều khiển cho Receive khớp tay máy
Start
Khai báo cấu hình hoạt động
cho các chức năng : Can, Timer
Khai báo các biến, sự kiện ngắt.
Kích hoạt ngắt
Ngắt nhận tin nhắn Can.
Bắn xung và kích chân chiều
vào driver để điều khiển góc
quay động cơ.
Gởi tin nhắn cho Transmit
Error = 0 ?
End
Ngắt nhận tin
nhắn Can.
Kiểm tra địa
chỉ tin nhắn ?
Dữ liệu nhận –
8 byte ?
Lưu dữ liệu data trong tin
nhắn.
Bật cờ ngắt
Gởi tin nhắn
Error cho
Transmit
Gởi tin nhắn
hoàn thành
cho Transmit
Error = 0 ?
Return

71. 58
Start
Khai báo cấu hình hoạt
động cho các chức
năng input và Timer1
Khai báo các biến, sự
kiện ngắt
Kích hoạt ngắt
Bắn xung điều khiển
động cơ tay kẹp nhả
End
Nhận tín hiệu output
của mạch Transmit
Hình 4.6: Lưu đồ giải thuật điều khiển cho Receive tay gắp

72. 59
THỰC NGHIỆM, ĐÁNH GIÁ KẾT QUẢ
CHƢƠNG 5:
5.1 Giới thiệu
5.1.1 Mục tiêu, yêu cầu thực nghiệm
Kiểm tra hoạt động, độ ổn định của cơ cấu tay kẹp
Kiểm tra hoạt động của các mạch điện.
Kiểm tra hoạt động của giải thuật gắp vật và chương trình điều khiển.
Hiệu chỉnh.
5.1.2 Các thành phần thí nghiệm
Mô hình khối lập phương mô phỏng thùng hàng cần gắp.
Hình 5.1: Mô hình khối lập phương
Đề tài được thực nghiệm trên mô hình cánh tay robot 5 bậc tự do.
Hình 5.2: Mô hình robot thực nghiệm

73. 60
5.2 Quá trình thực nghiệm
5.2.1 Kiểm tra hoạt động của mạch điện, bài toán động học và di chuyển của
cánh tay theo quỹ đạo.
Bằng cách sử dụng giao diện chương trình trên máy tính, đề tài tiến hành thực
hiện việc điều khiển vị trí cho từng khớp, sau đó điều khiển cánh tay robot di
chuyển theo quỹ đạo lắp ráp. Lúc này, ta đưa bảng làm việc đến vị trí lắp ráp rồi cho
dừng lại để kiểm tra cánh tay robot có di chuyển đúng quỹ đạo, lắp ráp có chính xác
và đúng thời gian mong muốn không.
Hình 5.3: Ảnh chụp trong quá trình robot gắp khối lập phương.
Theo quỹ đạo hoạch định cánh tay robot sẽ di chuyển từ tọa độ (250,0,-85) và
kết thúc quỹ đạo lắp vật tại tọa độ (0,-250,-105) thời gian di chuyển là 1 phút 5
giây. Quỹ đạo có 56 điểm và thời gian di chuyển giữa các điểm là 1,2 giây tương
ứng với thời gian mỗi lần gửi dữ liệu của máy tính xuống Transmit.

74. 61
Hình 5.4: Kết quả gắp khối lập phương
Vùng gạch màu đen là vùng đặt mong muốn (kích thước 30mm X 30mm).
Đường màu vàng là đường kẽ vị trí khối lập phương chiếm chỗ khi được robot gắp.
Sai lệnh cho phép để đảm bảo việc sắp xếp đạt yêu cầu là sai lệch so với biên chuẩn
nhỏ hơn 4mm .
Thực nghiệm được tiến hành 5 lần và cho kết quả:
Bảng 5.1: Bảng khảo sát
Số lần
Sai số so với vị trí
mong muốn (mm)
Thời gian thực hiện (s)
1 2,4 65
2 3,1 65
3 2,7 64
4 2,4 64
5 2,5 65

75. 62
Thực nghiệm kiểm tra sai số lặp lại 5 lần liên tiếp.
Hình 5.5: Kết quả gắp khối lập phương lặp lại 5 lần liên tục.
Hình 5.6: Kết quả sai lệch lắp vật giữa 5 lần là 3,5 mm.

76. 63
5.2.2 Kết quả
Mạch điện robot hoạt động tốt, mạch điều khiển đáp ứng được yêu cầu đặt ra.
Robot di chuyển theo quỹ đạo mong muốn nhưng còn sai số. Việc còn lại là đồng
bộ tốc độ di chuyển của bảng làm việc từ vị trí cảm biến tiệm cận đến vị trí lắp vật.
5.3 Đánh giá sai số
Nhìn chung kết quả lắp ráp là khá chính xác có thể gắp vật đúng vị trí mong
muốn nhưng còn có sai số (sai số cao nhất là 3,1 mm tuy nhiên vẫn đảm bảo gắp
khối lập phương vào đúng vị trí mong muốn, sai số nhỏ nhất là 2,4 mm). Sai số vị
trí so với tọa độ cần tới tương đối lớn nhưng so với quỹ đạo di chuyển là nhỏ và do
cánh tay di chuyển qua nhiều điểm tựa thuộc quỹ đạo nên sai số này chấp nhận
được, vẫn đảm bảo gắp vật vào vị trí mong muốn thành công.
5.4 Kết luận
Những kết quả trên là những kết quả bước đầu nên còn thiếu sót và còn sai số
trong quá trình điều khiển. Việc điều khiển robot vẫn chưa toàn diện cả vận tốc và
vị trí nên cánh tay lúc di chuyển theo quỹ đạo vẫn còn khựng chưa đều và êm, ngoài
ra chưa khử được sai số do độ rơ của các kết cấu cơ khí.

77. 64
TỔNG KẾT VÀ HƢỚNG PHÁT TRIỂN ĐỀ TÀI
CHƢƠNG 6:
6.1 Đánh giá kết quả đạt đƣợc
6.1.1 Nội dung hoàn thành
Đề tài đã thực hiện được các nội dung đề ra trong phần nhiệm vụ, cánh tay
robot có thể hoạt động và gắp khối lập phương di chuyển đúng với yêu cầu đề ra.
Trong nội dung đề tài đã thực hiện được việc tổng quan về tình hình nghiên cứu và
ứng dụng tay máy robot trong việc sắp xếp các thùng carton lên pallet, thiết kế mô
hình cơ khí của băng tải, tay gắp và tiến hành gia công chết tạo, thiết kế mạch điện
điều khiển, giao diện giao tiếp giữa máy tính và robot, xây dựng được mạng giao
tiếp CAN ổn định và ít sai số, giải thuật điều khiển cánh tay robot lắp vật khối hộp
lập phương, lắp mô hình, chạy thực nghiệm hệ thống và đánh giá kêt quả thu được.
6.1.2 Hạn chế của đề tài
Trong quá trình thực hiện đề tài, nhận thấy có những vấn đề phát sinh và chưa
được khắc phục triệt đó là sai số do độ rơ cơ khí, sai số chế tạo tay kẹp dẫn đến kết
quả lắp ráp có sai số. Trong thực nghiệm chỉ dừng lại ở việc điều khiển vị trí chưa
thực hiện được điều khiển vận tốc do đó cánh tay robot di chuyển không thể êm
mượt mà còn bị khựng lại giữa 2 đoạn điểm tựa trong quĩ đạo.
6.2 Hƣớng phát triển của đề tài
Dựa trên những hạn chế của đề tài, ta có được những định hướng phát triển
trong tương lai như sử dụng vi điều khiển có tốc độ cao để xử lý cao hơn để đọc
chính xác số liệu encoder. Từ đó có thể kiểm tra driver có điều khiển vị trí chính
xác như nhận xung điều khiển từ vi điều khiển không . Sử dụng phương pháp khử
rơ về cả cơ khí lẫn mạch điện nhằm tăng độ chính xác cho việc điều khiển hệ thống.
Áp dụng vòng điều khiển vận tốc để cánh tay robot có thể hoạt động êm và ổn định
hơn.

78. 65
TÀI LIỆU THAM KHẢO
[1] John J.Craig. (2005). Introduction to Robotics, Mechanical and Control,
third edition, Pearson Education International.
[2] Nguyễn Mạnh Tiến. (2007). Điều khiển robot công nghiệp. Nhà xuất bản
Khoa học và kỹ thuật.
[3] Nguyễn Thiện Phúc. (2006). Robot công nghiệp. Nhà xuất bản Khoa học và
kỹ thuật.
[4] Nguyễn Hữu Lộc. (2012). Cơ sở thiết kế máy. Nhà xuất bản Đại Học Quốc
Gia Thành Phố Hồ Chí Minh.
[5] Peter Corke,Robotics. (2008). Vision and Control,Fundamental Algorithms
in Matlab.
[6] Trịnh Chất, Tính toán thiết kế Hệ dẫn động Cơ khí Tập 1. Nhà xuất bản
Giáo Dục Việt Nam,
[7] Trịnh Chất. (2010) Tính toán thiết kế Hệ dẫn động Cơ khí Tập 2. Nhà xuất
bản Giáo Dục Việt Nam.
[8] Phạm Đăng Phước. (2007). Giáo trình Robot Công Nghiệp. NXB Xây Dựng
Hà Nội.
[9] Reza N. Jazar, Theory of Applied Robotics,Kinematics,Dynamics,and
Control Second Edition.
[10] Advisors, Dr. Hatem Elaydi and Dr.Iyad Abuhadrous. (2011). Simulation
and Interfacing of 5 DOF Educational Robot Arm, A master thesis.