ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
MỤC LỤC
LỜI NÓI ĐẦU .................................................................................................................................. 3
PHẦN I: GIỚI THIỆU VỀ ROBOT CÔNG NGHIỆP ..................................................................... 4
1.1. SƠ LƯỢC QUÁ TRÌNH PHÁT TRIỂN CỦA ROBOT CÔNG NGHIỆP (IR: INDUSTRIAL ROBOT) ............................................................................................................... 4
1.2. ỨNG DỤNG ROBOT CÔNG NGHIỆP TRONG SẢN XUẤT ........................................ 5
1.3. CÁC KHÁI NIỆM VÀ ĐỊNH NGHĨA VỀ ROBOT CÔNG NGHIỆP ................................. 6
1.3.1. Định nghĩa robot công nghiệp .......................................................................................... 6
1.3.2. Bậc tự do của robot (DOF: Degrees Of Freedom) ........................................................... 7
1.3.3. Hệ toạ độ (Coordinate frames) ......................................................................................... 7
1.3.4. Trường công tác của robot (Workspace or Range of motion) ......................................... 8
1.3. CẤU TRÚC CƠ BẢN CỦA ROBOT CÔNG NGHIỆP .................................................... 9
1.4.1. Các thành phần chính của robot công nghiệp ................................................................ 10
1.4.2. Kết cấu của tay máy ....................................................................................................... 10
1.5. PHÂN LOẠI ROBOT CÔNG NGHIỆP .............................................................................. 11
1.5.1. Phân loại theo kết cấu .................................................................................................... 11
1.5.2. Phân loại theo hệ thống truyền động .............................................................................. 11
1.5.3. Phân loại theo ứng dụng................................................................................................. 11
1.5.4. Phân loại theo cách thức và đặc trưng của phương pháp điều khiển ............................. 11
PHẦN II: THIẾT KẾ MÔ HÌNH 3D ROBOT RR ......................................................................... 13
2.1 MÔ HÌNH 3D CỦA ROBOT PHẲNG HAI BẬC TỰ DO ................................................. 13
PHẦN III: TÍNH TOÁN ĐỘNG HỌC THUẬN VÀ NGƯỢC ROBOT ...................................... 16
3.1 ĐẶT HỆ TỌA ĐỘ ................................................................................................................ 16
3.2 CÁC XÁC ĐỊNH CÁC THAM SỐ ĐỘNG HỌC CỦA BẢNG DH .................................... 17
3.3 THIẾT LẬP BỘ THÔNG SỐ ĐỘNG HỌC DENAVIT – HARTENBERG ....................... 17
3.4 THIẾT LẬP CÁC PHƯƠNG TRÌNH ĐỘNG HỌC ROBOT ............................................. 19
3.5 BÀI TOÁN ĐỘNG HỌC THUẬN ...................................................................................... 21
3.5 BÀI TOÁN ĐỘNG HỌC NGƯỢC ....................................................................................... 27
PHẦN IV : BÀI TOÁN TĨNH HỌC .............................................................................................. 31
4.1 TÍNH LỰC DẪN ĐỘNG TẠI CÁC KHỚP ĐẢM BẢO CÂN BẰNG TĨNH ..................... 31
4.2 XÉT KHÂU TÁC ĐỘNG CUỐI.......................................................................................... 32
1 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
4.3 XÉT KHÂU THỨ 1: ........................................................................................................... 34
PHẦN V: TÍNH TOÁN ĐỘNG LỰC HỌC ................................................................................ 37
5.1 XÂY DỰNG CẤU TRÚC ĐỘNG HỌC ............................................................................... 37
PHẦN VI: THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG ................................................................... 41
6.1 THIẾT KẾ QUỸ ĐẠO THEO KHÔNG GIAN KHỚP ........................................................ 41
6.2 THIẾT KẾ QUỸ ĐẠO THEO KHÔNG GIAN LÀM VIỆC ................................................ 46
6.2.1 Quỹ đạo của điểm tác động cuối theo đường thẳng từ A đến B trong t (s) ................... 46
6.2.2 Thiết kế quỹ đạo tác động tác động cuối di chuyển theo đường tròn từ A đến B trong tc(s) lấy AB làm đường kính .................................................................................................... 48
PHẦN VII : ĐIỀU KHIỂN VÀ MÔ PHỎNG ROBOT .................................................................. 51
7.1 HỆ THỐNG ĐIỀU KHIỂN TRONG KHÔNG GIAN KHỚP .............................................. 51
7.1.1. Hệ thống điều khiển phản hồi ........................................................................................ 51
7.1.2 Thiết kế cho robot RR .................................................................................................... 53
7.1.3 Sử dụng phương trình động lực học mô phỏng điều khiển trong simulink: .................. 54
7.2 SỬ DỤNG MÔ HÌNH XUẤT RA TỪ SOLIDWORKS ...................................................... 58
PHẦN VIII MÔ PHỎNG CHUYỂN ĐỘNG CỦA ROBOT BẰNG ............................................ 63
PHẦN MỀM VISUAL STUDIO VÀ THƯ VIỆN OPENGL ......................................................... 63
8.1.Thiết kế 3D ............................................................................................................................ 63
8.2 Mô phỏng hoạt động của robot. ............................................................................................. 63
PHẦN IX: KẾT LUẬN ................................................................................................................... 66
PHẦN X :CÁC CHƯƠNG TRÌNH ĐIỀU KHIỂN VÀ THIẾT KẾ CƠ KHÍ CÁC BẢN VẼ ...... 67
Phần XI : TÀI LIỆU THAM KHẢO .............................................................................................. 81
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
2 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
LỜI NÓI ĐẦU Trong sự nghiệp công nghiệp hóa, hiện đại hóa đất nước vấn đề tự động hóa có vai trò đặc biệt quan trọng.
Nhằm nâng cao nâng suất dây chuyền công nghệ, nâng cao chất lượng và khả năng cạnh tranh của sản phẩm, cải thiện điều kiện lao động, nâng cao năng suất lao động..đặt ra là hệ thống sản xuất phải có tính linh hoạt cao.Robot công nghiệp, đặc biệt là những tay máy robot là bô phận quan trọng để tạo ra những hệ thống đó.
Tay máy Robot đã có mặt trong sản xuất từ nhiều năm trước, ngày nay tay máy Robot đã dùng ở nhiều lĩnh vực sản xuất, xuất phát từ những ưu điểm mà tay máy Robot đó và đúc kết lại trong quá trình sản xuất làm việc, tay máy có những tính năng mà con người không thể có được, khả năng làm việc ổn định, có thể làm việc trong môi trường độc hại…..Do đó việc đầu tư nghiêc cứu, chế tạo ra những loại tay máy Robot phục vụ cho công cuộc tự động hóa sản xuất là rất cần thiết cho hiện tại và tương lai.
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ giúp chúng em làm quen và tìm hiểu kĩ hơn với những vấn đề cốt lõi và cơ bản nhất về robot và rất có ích cho chúng em sau này. Qua đó chúng em có thể tìm hiểu sâu hơn và tìm hiểu được cách tiếp cận và giải quyết các vấn đề của môn học. Đồng thời qua làm đồ án cũng hình thành thêm các kĩ năng làm việc,lập kế hoạch,viết báo cáo……rất có ích cho sau này. Trong quá trình làm đồ án có rất nhiều vấn đề đặt ra mà trong phạm vi khả năng của em còn hạn chế có thể chưa giải quyết triệt để được, em rất mong được cô chỉ bảo và hướng dẫn thêm.
Em xin chân thành cảm ơn !
Sinh viên thực hiện:
Đinh Đức Anh
3 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
PHẦN I: GIỚI THIỆU VỀ ROBOT CÔNG NGHIỆP
1.1. SƠ LƯỢC QUÁ TRÌNH PHÁT TRIỂN CỦA ROBOT CÔNG NGHIỆP
(IR: INDUSTRIAL ROBOT)
Thuật ngữ “Robot” xuất phát từ tiếng CH Séc (Czech) “Robota” có nghĩa là công việc tạp dịch trong vở kịch Rossum’s Universal Robots của Karel Capek, vào năm 1921. Trong vở kịch nầy, Rossum và con trai của ông ta đã chế tạo ra những chiếc máy gần giống với con người để phục vụ con người. Có lẽ đó là một gợi ý ban đầu cho các nhà sáng chế kỹ thuật về những cơ cấu, máy móc bắt chước các hoạt động cơ bắp của con người. Đầu thập kỷ 60, công ty Mỹ AMF (American Machine and Foundry Company) quảng cáo một loại máy tự động vạn năng và gọi là “Người máy công nghiệp” (Industrial Robot). Ngày nay người ta đặt tên người máy công nghiệp (hay robot công nghiệp) cho những loại thiết bị có dáng dấp và một vài chức năng như tay người được điều khiển tự động để thực hiện một số thao tác sản xuất. Về mặt kỹ thuật, những robot công nghiệp ngày nay, có nguồn gốc từ hai lĩnh vực kỹ thuật ra đời sớm hơn đó là các cơ cấu điều khiển từ xa (Teleoperators) và các máy công cụ điều khiển số (NC - Numerically Controlled machine tool). Các cơ cấu điều khiển từ xa (hay các thiết bị kiểu chủ-tớ) đã phát triển mạnh trong chiến tranh thế giới lần thứ hai nhằm nghiên cứu các vật liệu phóng xạ. Người thao tác được tách biệt khỏi khu vực phóng xạ bởi một bức tường có một hoặc vài cửa quan sát để có thể nhìn thấy được công việc bên trong. Các cơ cấu điều khiển từ xa thay thế cho cánh tay của người thao tác; nó gồm có một bộ kẹp ở bên trong (tớ) và hai tay cầm ở bên ngoài (chủ). Cả hai, tay cầm và bộ kẹp, được nối với nhau bằng một cơ cấu sáu bậc tự do để tạo ra các vị trí và hướng tuỳ ý của Tay cầm và bộ kẹp. Cơ cấu dùng để điều khiển bộ kẹp theo chuyển động của tay cầm. Vào khoảng năm 1949, các máy công cụ điều khiển số ra đời, nhằm đáp ứng yêu cầu gia công các chi tiết trong ngành chế tạo máy bay. Những robot đầu tiên thực chất là sự nối kết giữa các khâu cơ khí của cơ cấu điều khiển từ xa với khả năng lập trình của máy công cụ điều khiển số. Một trong những Robot Công nghiệp đầu tiên được chế tạo là Robot Versatran của công ty AMF, Mỹ. Cũng vào khoảng thời gian nầy ở Mỹ xuất hiện loại robot Unimate (1900) được dùng đầu tiên trong kỹ nghệ ôtô. Tiếp theo Mỹ, các nước khác bắt đầu sản xuất robot công nghiệp: Anh (1967), Thụy Điển và Nhật (1968) theo bản quyền của Mỹ, CHLB Đức (1971), Pháp (1972), Ý (1973). . . Tính năng làm việc của robot ngày càng được nâng cao, nhất là khả năng
4 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
nhận biết và xử lý. Năm 1967 ở trường Đại học tổng hợp Stanford (Mỹ) đã chế tạo ra mẫu robot hoạt động theo mô hình “mắt-tay”, có khả năng nhận biết và định hướng bàn kẹp theo vị trí vật kẹp nhờ các cảm biến. Năm 1974 Công ty Mỹ Cincinnati đưa ra loại robot được điều khiển bằng máy vi tính, gọi là robot T3 (The Tomorrow Tool: Công cụ của tương lai). Robot nầy có thể nâng được vật có khối lượng đến 40 KG. Có thể nói, Robot là sự tổ hợp khả năng hoạt động linh hoạt của các cơ cấu điều khiển từ xa với mức độ “tri thức” ngày càng phong phú của hệ thống điều khiển theo chương trình số cũng như kỹ thuật chế tạo các bộ cảm biến, công nghệ lập trình và các phát triển của trí khôn nhân tạo, hệ chuyên gia… Trong những năm sau nầy, việc nâng cao tính năng hoạt động của robot không ngừng phát triển. Các robot được trang bị thêm các loại cảm biến khác nhau để nhận biết môi trường chung quanh, cùng với những thành tựu to lớn trong lĩnh vực Tin học - Điện tử đã tạo ra các thế hệ robot với nhiều tính năng đăc biệt, Số lượng robot ngày càng gia tăng, giá thành ngày càng giảm. Nhờ vậy, robot công nghiệp đã có vị trí quan trọng trong các dây chuyền sản xuất hiện đại. Một vài số liệu về số lượng robot được sản xuất ở một vài nước công nghiệp phát triển như sau: Bảng 1: số lượng sản xuất robot ở các nước công nghiệp
Nước SX Năm 1990 Năm 1994 Năm 1998
Nhật 66.118 29.756 67000
Mỹ 4.237 7.634 11000
Đức 5.845 5.125 8.600
Ý 2.500 2.408 4000
Pháp 1.448 1.197 2000
Anh 510 1086 1500
Hàn Quốc 1000 1200
1.2. ỨNG DỤNG ROBOT CÔNG NGHIỆP TRONG SẢN XUẤT
Từ khi mới ra đời robot công nghiệp được áp dụng trong nhiều lĩnh vực dưới góc độ thay thế sức người. Nhờ vậy các dây chuyền sản xuất được tổ chức lại, năng suất và hiệu quả sản xuất tăng lên rõ rệt.
5 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Mục tiêu ứng dụng robot công nghiệp nhằm góp phần nâng cao năng suất dây chuyền công nghệ, giảm giá thành, nâng cao chất lượng và khả năng cạnh tranh của sản phẩm đồng thời cải thiện điều kiện lao động. Đạt được các mục tiêu trên là nhờ vào những khả năng to lớn của robot như : làm việc không biết mệt mỏi, rất dễ dàng chuyển nghề một cách thành thạo, chịu được phóng xạ và các môi trường làm việc độc hại, nhiệt độ cao, “cảm thấy” được cả từ trường và “nghe” được cả siêu âm ... Robot được dùng thay thế con người trong các trường hợp trên hoặc thực hiện các công việc tuy không nặng nhọc nhưng đơn điệu, dễ gây mệt mõi, nhầm lẫn. Trong ngành cơ khí, robot được sử dụng nhiều trong công nghệ đúc, công nghệ hàn, cắt kim loại, sơn, phun phủ kim loại, tháo lắp vận chuyển phôi, lắp ráp sản phẩm . . . Ngày nay đã xuất hiện nhiều dây chuyền sản xuất tự động gồm các máy CNC với Robot công nghiệp, các dây chuyền đó đạt mức tự động hoá cao, mức độ linh hoạt cao . . . ở đây các máy và robot được điều khiển bằng cùng một hệ thống chương trình. Ngoài các phân xưởng, nhà máy, kỹ thuật robot cũng được sử dụng trong việc khai thác thềm lục địa và đại dương, trong y học, sử dụng trong quốc phòng, trong chinh phục vũ trụ, trong công nghiệp nguyên tử, trong các lĩnh vực xã hội . . . Rõ ràng là khả năng làm việc của robot trong một số điều kiện vượt hơn khả năng của con người; do đó nó là phương tiện hữu hiệu để tự động hoá, nâng cao năng suất lao động, giảm nhẹ cho con người những công việc nặng nhọc và độc hại. Nhược điểm lớn nhất của robot là chưa linh hoạt như con người, trong dây chuyền tự động, nếu có một robot bị hỏng có thể làm ngừng hoạt động của cả dây chuyền, cho nên robot vẫn luôn hoạt động dưới sự giám sát của con người.
1.3. CÁC KHÁI NIỆM VÀ ĐỊNH NGHĨA VỀ ROBOT CÔNG NGHIỆP
1.3.1. Định nghĩa robot công nghiệp
Định nghĩa theo tiêu chuẩn AFNOR (Pháp):
Robot công nghiệp là cơ cấu chuyển động tự động có thể lập trình, 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 toạ độ; có khả năng định vị, định hướng, 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 hoá nhằm thực hiện các nhiệm vụ công nghệ khác nhau.
Định nghĩa theo RIA (Robot institute of America):
Robot là một tay máy vạn năng có thể lặp lại các chương trình được thiết kế để di chuyển vật liệu, chi tiết, dụng cụ hoặc các thiết bị chuyên dùng thông qua các chương trình chuyển động có thể thay đổi để hoàn thành các nhiệm vụ khác nhau.
6 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Định nghĩa theo GOCT 25686-85 (Nga):
Robot công nghiệp là một máy tự động, được đặt cố định hoặc di động được, liên kết giữa một tay máy và một hệ thống điều khiển theo chương trình, có thể lập trình lại để hoàn thành các chức năng vận động và điều khiển trong quá trình sản xuất. Có thể nói Robot công nghiệp là một máy tự động linh hoạt thay thế từng phần hoặc toàn bộ các hoạt động cơ bắp và hoạt động trí tuệ của con người trong nhiều khả năng thích nghi khác nhau. Robot công nghiệp có khả năng chương trình hoá 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ệ : hoặc 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 robot hoá” cho phép thích ứng nhanh và thao tác đơn giản khi nhiệm vụ sản xuất thay đổi.
1.3.2. Bậc tự do của robot (DOF: Degrees Of Freedom)
Bậc tự do là số khả năng chuyển động của một cơ cấu (chuyển động quay hoặc tịnh tiến). Để dịch chuyển được một vật thể trong không gian, cơ cấu chấp hành của robot phải đạt được một số bậc tự do. Nói chung cơ hệ của robot là một cơ cấu hở, do đó bậc tự do của nó có thể tính theo công thức:
Ở đây: n - Số khâu động;
pi - Số khớp loại i (i = 1, 2,...,5 : Số bậc tự do bị hạn chế). Đối với các cơ cấu có các khâu được nối với nhau bằng khớp quay hoặc tịnh tiến (khớp động loại 5) thì số bậc tự do bằng với số khâu động... Đối với cơ cấu hở, số bậc tự do bằng tổng số bậc tự do của các khớp động.
Để định vị và định hướng khâu chấp hành cuối một cách tuỳ ý trong không gian 3 chiều robot cần có 6 bậc tự do, trong đó 3 bậc tự do để định vị và 3 bậc tự do để định hướng. Một số công việc đơn giản nâng hạ, sắp xếp... có thể yêu cầu số bậc tự do ít hơn. Các robot hàn, sơn... thường yêu cầu 6 bậc tự do. Trong một số trường hợp cần sự khéo léo, linh hoạt hoặc khi cần phải tối ưu hoá quỹ đạo... người ta dùng robot với số bậc tự do lớn hơn 6.
1.3.3. Hệ toạ độ (Coordinate frames)
Mỗi robot thường bao gồm nhiều khâu (links) liên kết với nhau qua các khớp (joints), tạo thành một xích động học xuất phát từ một khâu cơ bản
7 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
(base) đứng yên. Hệ toạ độ gắn với khâu cơ bản gọi là hệ toạ độ cơ bản (hay hệ toạ độ chuẩn). Các hệ toạ độ trung gian khác gắn với các khâu động gọi là hệ toạ độ suy rộng. Trong từng thời điểm hoạt động, các toạ độ suy rộng xác định cấu hình của robot bằng các chuyển dịch dài hoặc các chuyển dịch góc của các khớp tịnh tiến hoặc khớp quay. Các toạ độ suy rộng còn được gọi là biến khớp (Hình 1.1)
Các hệ toạ độ gắn trên các khâu của robot phải tuân theo qui tắc bàn tay phải: Dùng tay phải, nắm hai ngón tay út và áp út vào lòng bàn tay, xoè 3 ngón : cái, trỏ và giữa theo 3 phương vuông góc nhau, nếu chọn ngón cái là phương và chiều của trục z, thì ngón trỏ chỉ phương, chiều của trục x và ngón giữa sẽ biểu thị phương, chiều của trục y (hình 1.2). Trong robot ta thường dùng chữ O và chỉ số n để chỉ hệ toạ độ gắn trên
khâu thứ n. Như vậy hệ toạ độ cơ bản (Hệ toạ độ gắn với khâu cố định) sẽ được ký hiệu là O0; hệ toạ độ gắn trên các khâu trung gian tương ứng sẽ là O1, O2,..., On-1, Hệ toạ độ gắn trên khâu chấp hành cuối ký hiệu là On.
1.3.4. Trường công tác của robot (Workspace or Range of motion)
Trường công tác (hay vùng làm việc, không gian công tác) của robot là toàn bộ thể tích được quét bởi khâu chấp hành cuối khi robot thực hiện tất cả các chuyển động có thể. Trường công tác bị ràng buộc bởi các thông số hình học của robot cũng như các ràng buộc cơ học của các khớp; ví dụ, một khớp
8 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
quay có chuyển động nhỏ hơn một góc 3600. Người ta thường dùng hai hình chiếu để mô tả trường công tác của một robot (hình 1.3).
1.3. CẤU TRÚC CƠ BẢN CỦA ROBOT CÔNG NGHIỆP
Hình 2. Sơ đồ khối của Robot công nghiệp
9 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
1.4.1. Các thành phần chính của robot công nghiệp
Một robot công nghiệp thường bao gồm các thành phần chính như : cánh tay robot, nguồn động lực, dụng cụ gắn lên khâu chấp hành cuối, các cảm biến, bộ điều khiển , thiết bị dạy học, máy tính ... các phần mềm lập trình cũng nên được coi là một thành phần của hệ thống robot. Cánh tay robot (tay máy) là kết cấu cơ khí gồm các khâu liên kết với nhau bằng các khớp động để có thể tạo nên những chuyển động cơ bản của robot. Nguồn động lực là các động cơ điện (một chiều hoặc động cơ bước), các hệ thống xy lanh khí nén, thuỷ lực để tạo động lực cho tay máy hoạt động. Dụng cụ thao tác được gắn trên khâu cuối của robot, dụng cụ của robot có thể có nhiều kiểu khác nhau như: dạng bàn tay để nắm bắt đối tượng hoặc các công cụ làm việc như mỏ hàn, đá mài, đầu phun sơn... Thiết bị dạy-hoc (Teach-Pendant) dùng để dạy cho robot các thao tác cần thiết theo yêu cầu của quá trình làm việc, sau đó robot tự lặp lại các động tác đã được dạy để làm việc (phương pháp lập trình kiểu dạy học). Các phần mềm để lập trình và các chương trình điều khiển robot được cài đặt trên máy tính, dùng điều khiển robot thông qua bộ điều khiển (Controller). Bộ điều khiển còn được gọi là Mođun điều khiển (hay Unit, Driver), nó thường được kết nối với máy tính. Một mođun điều khiển có thể còn có các cổng Vào - Ra (I/O port) để làm việc với nhiều thiết bị khác nhau như các cảm biến giúp robot nhận biết trạng thái của bản thân, xác định vị trí của đối tượng làm việc hoặc các dò tìm khác; điều khiển các băng tải hoặc cơ cấu cấp phôi hoạt động phối hợp với robot...
1.4.2. Kết cấu của tay máy
Như đã nói trên, tay máy là thành phần quan trọng, nó quyết định khả năng
10 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
làm việc của robot. Các kết cấu của nhiều tay máy được phỏng theo cấu tạo và chức năng của tay người; tuy nhiên ngày nay, tay máy được thiết kế rất đa dạng, nhiều cánh tay robot có hình dáng rất khác xa cánh tay người. Trong thiết kế và sử dụng tay máy, chúng ta cần quan tâm đến các thông số hình - động học, là những thông số liên quan đến khả năng làm việc của robot như: tầm với (hay trường công tác), số bậc tự do (thể hiện sự khéo léo linh hoạt của robot), độ cứng vững, tải trọng vật nâng, lực kẹp . . .
Các khâu của robot thường thực hiện hai chuyển động cơ bản:
• Chuyển động tịnh tiến theo hướng x, y, z trong không gian
Descarde, thông thường tạo nên các hình khối.
• Chuyển động xoay theo các trục x, y, z trong không gian. • Các chuyển động này thường ký hiệu là T (Translation) hoặc P
(Prismatic).
1.5. PHÂN LOẠI ROBOT CÔNG NGHIỆP
1.5.1. Phân loại theo kết cấu
Theo kết cấu của tay máy người ta phân thành robot kiểu toạ độ Đề các, Kiểu toạ độ trụ, kiểu toạ độ cầu, kiểu toạ độ góc, robot kiểu SCARA như đã trình bày ở trên.
1.5.2. Phân loại theo hệ thống truyền động Có các dạng truyền động phổ biến là:
Hệ truyền động điện: Thường dùng các động cơ điện 1 chiều (DC: Direct Current) hoặc các động cơ bước (step motor). Loại truyền động nầy dễ điều khiển, kết cấu gọn. Hệ truyền động thuỷ lực: có thể đạt được công suất cao, đáp ứng những điều kiện làm việc nặng. Tuy nhiên hệ thống thuỷ lực thường có kết cấu cồng kềnh, tồn tại độ phi tuyến lớn khó xử lý khi điều khiển. Hệ truyền động khí nén: có kết cấu gọn nhẹ hơn do không cần dẫn ngược nhưng lại phải gắn liền với trung tâm tạo ra khí nén. Hệ nầy làm việc với công suất trung bình và nhỏ, kém chính xác, thường chỉ thích hợp với các robot hoạt động theo chương trình định sẳn với các thao tác đơn giản “nhấc lên - đặt xuống” (Pick and Place or PTP: Point To Point).
1.5.3. Phân loại theo ứng dụng
Dựa vào ứng dụng của robot trong sản xuất có Robot sơn, robot hàn, robot lắp ráp, robot chuyển phôi .v.v...
1.5.4. Phân loại theo cách thức và đặc trưng của phương pháp điều khiển
Có robot điều khiển hở (mạch điều khiển không có các quan hệ phản hồi),
11 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Robot điều khiển kín (hay điều khiển servo): sử dụng cảm biến, mạch phản hồi để tăng độ chính xác và mức độ linh hoạt khi điều khiển. Ngoài ra còn có thể có các cách phân loại khác tuỳ theo quan điểm và mục đích nghiên cứu.
Sơ đồ điều khiển cơ bản :
Hình 1.5 Sơ đồ điều khiển hở
Hình 1.6 sơ đồ điều khiển kín
12 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
PHẦN II: THIẾT KẾ MÔ HÌNH 3D ROBOT RR Sử dụng phần mềm thiết kế 3D Solidwork
2.1 MÔ HÌNH 3D CỦA ROBOT PHẲNG HAI BẬC TỰ DO
Thiết kế cơ khí của khâu đế và các khâu:
Khâu đế :
Hình 2.1 Khâu đế
Khâu 1: Khâu 2
Hình 2.2:khâu 1 Hình2 .3 Khâu 2
13 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Các chi tiết khác:
Hình 2.4 Bánh răng khâu 1,2 Hình 2.5 Then
Hình 2.6 Nắp hộp bánh răng Hình 2.7 Trục bánh răng
14 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Hình 2.8 Khớp bánh răng 1 Hình 2.9 Khớp bánh răng 2
Cơ cấu phẳng hai bậc tự do RR đc thiết kế như sau:
Hình 2.10 Mô hình 3D của ROBOT phẳng hai bậc tự do
(chi tiết về các thông số xem thêm ở phần phụ lục phía cuối)
15 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
PHẦN III: TÍNH TOÁN ĐỘNG HỌC THUẬN VÀ NGƯỢC ROBOT
3.1 ĐẶT HỆ TỌA ĐỘ
Hình 3.1 hệ trục tọa độ của robot
Theo Denavit – Hartenberg (1955) đã quy ước hệ tọa độ Decard gắn vào mỗi khâu của một tay máy Robot như sau:
* Trục 𝑧𝑖 được chọn dọc theo trục của khớp thứ (i+1).
Hướng của phép quay và phép tịnh tiến được chọn tùy ý.
* Trục 𝑥𝑖 được xác định dọc theo đường vuông góc chung giữa trục khớp động thứ i và (i+ 1), hướng từ khớp động thứ i tới trục ( i+1).
* Trục 𝑦𝑖 được xác định theo quy tắc bàn tay phải.
Từ quy tắc trên ta xây dựng các tọa độ khảo sát (Hình 2.1).
Hệ tọa độ 𝑂0𝑥0𝑦0𝑧0:
16 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Gốc tọa độ đặt tại tâm của khớp động thứ nhất
Trục 𝑂0𝑧0 dọc theo hướng của trục khớp động thứ nhất hướng từ trong ra ngoài.
Trục 𝑂0𝑥0 có phương vuông góc với 𝑧0 và ta chọn ở vị trí nằm ngang.
Trục 𝑂0𝑦0 xác định theo quy tắc bàn tay phải.
Hệ tọa độ 𝑂1𝑥1𝑦1𝑧1: Gốc tọa độ đặt tại tâm của khớp động thứ 2
Trục 𝑂1𝑧1 có phương hướng theo trục khớp động thứ 2 vuông góc với mặt phẳng hình vã và có chiều hướng từ rong ra ngoài (như hình vẽ)
Trục 𝑂1𝑥1có phương nằm trên đường vuông góc chung của trục 𝑂0𝑧0 và
𝑂1𝑧1 do 𝑂0𝑧0 // 𝑂1𝑧1 nên ta chọn 𝑂1𝑥1 có hướng dọc theo khâu 1 như hình vẽ
Trục 𝑂1𝑦1 xác định theo quy tắc bàn tay phải.
Hệ tọa độ 𝑂2𝑥2𝑦2𝑧2:
Gốc tọa độ đặt tại đầu khâu thứ 2.
Trục 𝑂2𝑧2 có phương // với 𝑂1𝑧1 và có chiều hướng từ trong ra ngoài
Trục 𝑂2𝑥2 có phương nằm trên đường vuông góc chung của trục 𝑂1𝑧1 và 𝑂2𝑧2 do đó ta chọn phương dọc theo trục của khâu 2
Trục 𝑂2𝑦2 xác định theo quy tắc bàn tay phải.
3.2 CÁC XÁC ĐỊNH CÁC THAM SỐ ĐỘNG HỌC CỦA BẢNG DH
Vị trí của hệ tọa độ khớp (𝑂𝑥𝑦𝑧)𝑖 đối với hệ tọa độ khớp (𝑂𝑥𝑦𝑧)𝑖−1 được xác định bởi 4 tham số 𝜃𝑖, 𝑑𝑖, 𝛼𝑖, 𝑎𝑖 như sau: 𝜃𝑖 : góc quay quanh trục 𝑧𝑖−1 để trục 𝑥𝑖−1 trùng với trục 𝑥′𝑖 (𝑥𝑖//𝑥′𝑖) 𝑑𝑖 : dịch chuyển tịnh tiế dọc trục 𝑧𝑖−1 để gốc tọa độ 𝑂𝑖−1 chuyể đến 𝑂′𝑖 là giao điểm của trục 𝑥𝑖 và trục 𝑧𝑖−1 𝑎𝑖 : dịch chuyển dọc trục 𝑥𝑖 để điểm 𝑂′𝑖 chuyển đến điểm 𝑂𝑖 𝛼𝑖 : góc quay quanh trục 𝑥𝑖 sao cho trục 𝑧′𝑖−1(𝑧′𝑖−1// 𝑧𝑖−1) trùng với trục 𝑧𝑖
3.3 THIẾT LẬP BỘ THÔNG SỐ ĐỘNG HỌC DENAVIT – HARTENBERG
Với cách thành lập hệ tọa độ như trên ta có thể xác định các tham số động học của robot như sau:
17 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Bảng 3.1 bảng DH các thông số động học của robot RR
khâu
1 𝑑𝑖 0 𝛼𝑖 0 𝑎𝑖 a1
2 0 0 a2 𝜃𝑖 𝜃1 𝜃2
Trong đó là các biến khớp , còn a1 , a2 là các hằng số
Và X=[x1,x2,]T là véc tơ biểu diễn vị trí của bàn kẹp trong hệ cố định.
q=[q1,q2] với q1 = 1, q2 = 2 là các góc xoay các biến khớp
với giới hạn các góc quay thì ta có vùng không gian
làm việc của Robot như sau:
Hình 3.2 Không gian làm việc của Robot
- Dạng tổng quát của ma trận Denavit-Hartenberg cho các khâu
18 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
i-1Ai=
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Ma trận Denavit-Hartenbergcủa khâu 1:
Ma trận Denavit-Hartenbergcủa khâu 2:
3.4 THIẾT LẬP CÁC PHƯƠNG TRÌNH ĐỘNG HỌC ROBOT
Từ các ma trận (2.1) và (2.2) ta xác định được ma trận biến đổi tọa độ thuần nhất của khâu 2 so với trục hệ tọa độ cố định 𝑂0𝑥0𝑦0𝑧0 là :
cho ta biết hướng và vị trí của khâu thao tác trong hệ tọa độ cố định - Ma trận hay nói cách khác là vị trí của điểm tác động cuối và hướng của hệ tọa độ động gắn vào khâu tại điểm tác động cuối trong hệ tọa độ cố định. Vì thế nó còn được biểu diễn qua thông số các biến khớp ta tạm gọi là qi. Trong bài toán cụ thể thì nó là các khớp xoay θi, với i=1÷2.
19 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
- Khi đó, ma trận (2.3) được kí hiệu thành
là giá
là các tọa độ điểm E và [𝛼, 𝛽, 𝜂] là các góc quay Cardan của
Sử dụng các góc Cardan xác định hướng của vật rắn. Ta gọi trị mô tả trực tiếp vị trí và hướng của EX2Y2Z2 so với hệ tọa độ O0Z0Y0Z0. Trong đó: EX2Y2Z2 so với hệ tọa độ O0Z0Y0Z0. Do các tọa độ thao tác đều là hàm của thời gian. Nên ta có thể biểu diễn:
Với: là ma trận Cardan mô ta hướng EX2Y2Z2 so với hệ tọa độ O0Z0Y0Z0.
là vectơ mô tả vị trí của điểm tác động cuối trong hệ tọa độ
O0Z0Y0Z0.
biểu diễn vị trí và hướng của khâu thao tác trong hệ tọa độ - Do ma trận cố định thông qua biến khớp qi (Ma trận trạng thái khâu thao tác theo cấu trúc động học). Còn ma trận cũng mô tả vị trí và hướng của khâu thao tác thông qua hệ tọa độ khâu thao tác. Ở đây ta chọn cách biểu diễn thông qua các góc Cardan.
Từ đó ta có PT động học ROBOT có dạng: . Với
n = 2 vì cơ cấu ROBOT có 2 khâu.
- Từ các hệ thức (3.3), (3.4), (3.5),(3.6), (3.7) Ta xây dựng được hệ 6 phương trình độc lập như sau:
20 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
3.5 BÀI TOÁN ĐỘNG HỌC THUẬN
Bài toán động học thuận: Các thông số đầu vào: q1, q2
, , ), và hướng của khâu tác
Thông số cần xác định: điểm tác động cuối E = ( động cuối so với hệ tọa độ cơ sở.
* Ta có tọa độ của điểm tác động cuối:
Ta chọn quy luật chuyển động cho robot trong thời gian 10s như sau :
Và các thông số hình học:
a1= 65 cm, a2 = 55cm (đề VCK05-2_12)
để thuận tiện cho việc tính toán ta sử dụng phần mềm tính toán maple
và các thông số hình học a1 , a2 vào biểu thức của
Thay các giá trị xE,yE,zE ta có ta có tọa độ của khâu thao là :
21 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Ta có thể vẽ đồ thị của điểm thao tác cuối qua các tọa độ như sau:
Hình 3.3 Đồ thị xE,yE,zE theo t
Hình 3.4 Đồ thị quỹ đạo chuyển động của điểm tác động cuối
22 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Tính vận tốc điểm tác động cuối E, vận tốc góc khâu thao tác
Vận tốc điểm tác động cuối của Robot
Suy ra
Từ phần trên ta đã xây dựng được quy luật chuyển cũng như tìm được tọa độ của khâu thao tác cuối, các biến khớp và đạo hàm các cấp theo t đã biết :
Vận tốc của khâu thao tác chính là đạo hàm vị trí khâu thao tác theo thời gian:
VE= E=
Từ các phương trình về vị trí của điểm tác động cuối của robot ta tính được vận tốc của điểm cuối
Đồ thị vận tốc điểm E:
Hình 3.5 Đồ thị vận tốc điểm E
23 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Đạo hàm các biểu thức vEx,vEy,vEz theo thời gian ta thu được các biểu thức gia tốc dài của điểm E
Khi đó ta có đồ thị gia tốc điểm E:
Hình 3.6 :Đồ thị gia tốc (dài) điểm E
Bài toán xác định vận tốc góc các khâu :
Vận tốc góc của khâu 1 :
Với R1 là ma trận cosin chỉ hướng của khâu 1 và được lấy từ (3.1):
Ta có :
24 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
Cuối cùng nhân 2 ma trận ta được vận tốc góc của khâu 1:
Suy ra được :
Vận tốc góc khâu 2:
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Với R2 là ma trận cosin chỉ hướng của khâu 2 và được lấy từ (3.2):
Ta có :
25 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Suy ra được :
Vận tốc góc khâu thao tác cuối E:
Cuối cùng nhân 2 ma trận ta được vận tốc góc của khâu 2:
Với RE là ma trận cosin chỉ hướng của khâu cuối và được lấy từ (3.3):
Cuối cùng nhân 2 ma trận ta được vận tốc góc của khâu tác động cuối:
Suy ra được :
Ta có :
26 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
Khi đó ta có :
Hình 3.7 : Đồ thị vận tốc góc khâu 2
Dễ thấy rằng đối với cơ cấu robot phẳng thì vector vận tốc góc chỉ có thành phần
theo phương z
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
3.5 BÀI TOÁN ĐỘNG HỌC NGƯỢC
Từ bài toán động học thuận ta có được hệ phương trình :
27 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
và nhiệm vụ của ta là phải tìm lại
Bài toán đặt ra là : cho biết các tọa độ các biến khớp :
ở đây do robot phẳng nên ta luôn luôn có zE = 0
và khi đó ta còn hệ phương trình :
Với các xE , yE, a1 , a2 đã biết.
Bình phương từng vế 2 phương trình trên ta được :
Cộng hai phương trình trên với nhau rồi sau đó rút gọn ta được:
Suy ra :
Và
Do đó ta có
Thay lại q2 vào phương trình của xE , yE và viết lại hệ thì ta thu được một hệ phương trình lượng giác bậc nhất một ẩn :
Giải hệ trên ta được:
28 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Từ đó ta có :
Hay ta có :
Bài toán cụ thể :
1. Quỹ đạo chuyển động là một đường thẳng :
Cho quỹ đạo chuyển động ngược là đường thẳng AB, với A(1039.23,600) và B(50,1039.23) trong thời gian 5s ta thu được đồ thị biến khớp q1 q2 như sau :
Hình 3.8 Đồ thị các biến khớp q1,q2 theo t
29 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
2. Cho quỹ đạo chuyển động là một đường tròn :
Giả sử cần cho điểm tác động cuối E di chuyển theo một đường tròn có phương trình :
Hình 3.9 Đồ thị điểm tác động cuối cần điều khiển
Khi đó bài toán động học ngược cho ta kết quả các biến khớp là :
Hình 3.10 Đồ thị các biến khớp q1,q2 thu được
30 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
PHẦN IV : BÀI TOÁN TĨNH HỌC 4.1 TÍNH LỰC DẪN ĐỘNG TẠI CÁC KHỚP ĐẢM BẢO CÂN BẰNG TĨNH
Cơ cấu gắn vào Robot bao gồm các khâu nối với nhau bởi các khớp. Trọng tâm được đặt vào trung điểm của từng khâu.
Hình 4.1 : mô hình lực tác dụng vào robot
Bài toán đặt ra là: Giả thiết ngoại lực tác động vào khâu thao tác tại điểm E gồm véc tơ lực FE,3 và momen ME,3. Tính lực (và mômen) tác động vào các khớp để đảm bảo robot cân bằng tĩnh.
Lời giải chi tiết như sau:
Ta có hệ phương trình cân bằng lực trong hệ tọa độ cơ sở:
31 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Nếu viết trong dạng ma trận:
Hệ phương trình cân bằng lực trong hệ tọa độ khâu i:
Nếu viết trong dạng ma trận:
4.2 XÉT KHÂU TÁC ĐỘNG CUỐI.
Khâu 2:
Suy ra: PTCB
Với
và
Giả thiết các khâu của Robot đồng nhất hay trọng tâm của các khâu nằm ở chính giữa. Bằng cách đặt các ngoại tực tác dụng vào khâu cuối là Ta có: Ngoại lực tác dụng tại điểm E là: và :
32 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
Vì vậy:
là momen cho ban đầu:
Tính toán
như sau:
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
33 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Thay tất cả vào (3.9) ta thu được :
Tính toán các giá trị như sau: Theo (3.16)
4.3 XÉT KHÂU THỨ 1: PTCB:
34 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
Theo đề bài :
PTCB tương đương với:
được tính từ biểu thức (3.16)
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
35 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
Thay tất cả các biểu thức trên vào (3.19) ta được:
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Từ các công thức trên ta có thể tính được các lực và moment tác dụng lên các khâu
và các khớp của robot để đảm bảo cân bằng tĩnh cho robot.
36 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
PHẦN V: TÍNH TOÁN ĐỘNG LỰC HỌC
5.1 XÂY DỰNG CẤU TRÚC ĐỘNG HỌC
Hình 5.1 : Mô hình cấu trúc động học robot 2 DOF
Gọi lC1 lC2 lần lượt là khoảng cách từ O0, O1 đến trọng tâm các khâu 1 và khâu 2.
Coi các khâu đồng chất tiết diện đều trọng tâm đặt giữa khâu
Chọn tọa độ suy rộng với :
Từ hình vẽ ta dễ dàng tính ra được tọa độ các khối tâm và biểu thức vận tốc góc của từng khâu là :
37 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Biểu thức vận tốc góc các khâu:
Từ đó suy ra các ma trận Jacobi tịnh tiến và ma trận Jacobi quay:
Ma trận ten-xơ quán tính có dạng :
Theo định nghĩa ma trận khối lượng suy rộng của robot có dạng:
38 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Thế các biểu thức (5.4),(5.5),(5.6) vào (5.7) Với
ta được :
Thế (5.8) vào biểu thức động năng của robot:
Ta được :
Biểu thức thế năng cơ hệ có dạng:
Công ảo của các lực suy rộng không thế :
Với E là điểm tác động cuối cùng trên khâu 2, hay E≡O2
Từ hệ thức :
Ta suy ra:
39 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Từ đó ta có :
Thế các biểu thức (5.10),(5.11),(5.12),(5.13) vào biểu thức của phương trình lagrange loại II ta được các phương trình vi phân chuyển động của robot phẳng hai khâu :
40 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
PHẦN VI: THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG
6.1 THIẾT KẾ QUỸ ĐẠO THEO KHÔNG GIAN KHỚP
Là xác định đường biểu diễn của vị trí góc khớp (góc quay của khớp với khớp quay và độ di chuyển của khớp đối với khớp tịnh tiến) theo thời gian khi di chuyển từ vị trí ban đầu 𝑞0(A) tới vị trí cuối 𝑞𝑐(B) trong thời gian 𝑡𝑐với q là biến khớp tổng quát. Quỹ đạo di chuyển của khớp giữa hai vị trí sẽ thỏa mãn 4 điều kiện : vị trí ban đầu và vị trí cuối cùng, tốc độ tại vị trí ban đầu và tốc độ tại vị trí cuối cùng. Do đó đa thức bậc 3 sẽ phù hợp với quỹ đạo của khớp robot:
(6.1)
giả thiết vận tốc tại điểm đầu và điểm cuối bằng 0:
𝑞̇𝑖0 = 0 ; 𝑞̇𝑖𝑐 = 0 Ta có hệ phương trình:
Giải hệ ta được các hệ số của phương trình quỹ đạo.
Bài toán cụ thể:
cho điểm A(1039.2305; 600; 0) và điểm B(50; 1039.2305; 0).
Đơn vị là mm
Thời gian chuyển động
41 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Hình 6.1 Minh họa vị trí 2 điểm A , B trong không gian làm việc
Từ bài toán động học ngược ta tính được các góc tương ứng ban đầu :
và
Với thời gian 5s như trên thay vào (5.3) ta tính được các hệ số của khớp 1:
42 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
hay là
Ta có
Hình 6.2 Đ ường biểu diễn vị trí, vận tốc và gia tốc của khớp 1 thao tác theo thời gian
Phương trình động học của khâu 1:
43 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Tương tự thì ta cũng có các hệ số của khớp 2
hay là :
Ta có
Phương trình động học của khâu 2:
Và ta cũng có đường biểu diễn
Hình 6.3 : đường biểu diễn vị trí, vận tốc, gia tốc của khâu 2 theo thời gian.
44 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Sau khi có các biến khớp ta có thể vẽ lại quỹ đạo của điểm E như hình vẽ :
Hình 6.4 Đồ thị điểm E trong không gian thao tác
Vị trí cơ cấu khi đó :
Hình 6.5 Vị trí khâu của cơ cấu theo t
Ta thấy hai điểm đầu mút của quỹ đạo thực trùng với vị trí ta cần đưa đến nên ta thấy các biến khớp giải được là đúng với yêu cầu đặt ra.
45 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
6.2 THIẾT KẾ QUỸ ĐẠO THEO KHÔNG GIAN LÀM VIỆC
6.2.1 Quỹ đạo của điểm tác động cuối theo đường thẳng từ A đến B trong t (s)
Tay robot di chuyển từ vi trí trong không gian làm việc tới vị trí B
theo một đường thẳng. ta có phương trình đường thẳng AB có dạng sau:
Cho robot chuyển động trong thời gian 5(s) đi từ vị trí A(1039.23; 600; 0) tới vị trí
B(50; 1039.23; 0). Tại vị trí A các khớp
Dễ thấy khi đó A,B nằm trên mặt phẳng z=0, nên ta viết phương trình đường thẳng đi qua hai điểm A và B, ta có dạng:
Từ phương trình (5.8) ta rút ra được quan hệ:
Cũng như cách thiết lập quỹ đạo góc khớp để thỏa mãn điều kiện về vận tốc đầu và vận tốc cuối ta thiết lập quan hệ x=x(t) là đa thức bậc 3.
Cũng các điều kiện
mà quan hệ y=k.x+b => x=x(t) cũng có các điều kiện như
quỹ đạo , thay vào (5.11) ta rút ra được:
khi thiết kế x=x(t) rồi thay vào (6.8)
ta có được y=y(t)
46 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Ví dụ : với điều kiện ban đầu là : tại t=0 thì vị trí điểm E là tại A
và tại t=5s thì vị trí điểm E tại B từ đó ta thay các giá trị ban đầu vào (6.11) và ta có kết quả phương trình đường thẳng AB là :
Và suy ra
Khi đó ta có thể vẽ được đồ thị điểm E trong khoảng thời gian 5s như hình vẽ
Hình 6.6 Đồ thị x(t),y(t) theo đường thẳng và quỹ đạo điểm E
47 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Hình 6.7 Vị trí cơ cấu theo thời gian t
6.2.2 Thiết kế quỹ đạo tác động tác động cuối di chuyển theo đường tròn từ A đến B trong tc(s) lấy AB làm đường kính Ta có phương trình đường tròn trong mặt phẳng giữa hai điểm A (xA,yA) , B(xB,yB) lấy AB làm đường kính là
với
Viết phương trình đường tròn dưới dạng tham số như sau :
cũng để thỏa mãn điều kiện về vận tốc a(t) ta thiết kế cũng phải là bậc 3
Và phải thỏa mãn điều kiện:
Từ đó ta dễ dàng tìm được các hệ số a0 a1 a2 a3 có dạng:
đặt
48 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Thì ta suy ra được :
Ví dụ cần thiết kế di chuyển theo đường tròn từ A (562.92,875) đến B(151.31,837,92) trong 5s với AB là đường kính.
Dựa vào công thức trên ta tính được :
Ta tiến hành vẽ các đồ thị minh họa như hình bên dưới :
Hình 6.8 Đồ thị x(t),y(t) theo cung tròn
49 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Hình 6.9 Quỹ đạo điểm E theo cung tròn
Hình 6.10 Vị trí cơ cấu theo t
50 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
PHẦN VII : ĐIỀU KHIỂN VÀ MÔ PHỎNG ROBOT Các hệ thống điều khiển nêu dưới đây đều theo luật điều khiển PD. Khi thiết kế hệ thống điều khiển ta bỏ qua động học của cơ cấu chấp hành, quán tính động cơ. Như vậy chức năng của bộ điều khiển là tạo ra một momen cần thiết để truyền động khớp robot đảm bảo khớp robot luôn bám theo vị trí đặt.
7.1 HỆ THỐNG ĐIỀU KHIỂN TRONG KHÔNG GIAN KHỚP
Tín hiệu đặt đó là quỹ đạo bậc 3 của các khớp đã được tính toán ở phần thiết kế quỹ đạo.
7.1.1. Hệ thống điều khiển phản hồi a, Luật điều khiển
Hình 7.1.Sơ đồ cấu trúc hệ thống điều khiển robot với bộ điều khiển PD
Tín hiệu đặt vị trí qd được so sánh với vị trí thực của khớp q, sai lệch được đặt vào khâu khuếch đại với hệ số Kp. Tín hiệu ra của khâu tỉ lệ được cộng đại số với tín hiệu tỉ lệ với tốc độ của khớp và đặt cơ cấu chấp hành của robot:
Hay viết với khớp thứ i ta có:
Trong đó : sai số vị trí khớp robot.
sai số tốc độ khớp robot.
là ma trận đường chéo các hệ số khuếch
đại của từng khớp hợp riêng.
51 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
là ma trận đường chéo các hệ số đạo hàm
của từng khớp hợp riêng.
Hệ thống điều kiển với cấu trúc bộ điều khiển (3…) ổn định tuyệt đối toàn
cục. thật vậy, chọn hàm lyapunov có dạng :
tỉ lệ Hàm VL biểu thị tổng năng lượng hệ thống robot: thành phần
năng lượng đầu vào và thành phần là động năng của robot. KP và H là
. các ma trận hệ số dương; nên hàm VL > 0 với
Tính đạo hàm cấp 1 hàm VL ta được:
, nên (4.5) được rút gọn lại như
Do tính đối xứng của các thành phần sau:
Thay phương trình dạng tổng quát vào phương trình trên với giả thiết không có thành phần mômen trọng lực G(p) ta có :
Sử dụng thuộc tính của phương trình động lực học và áp dụng luật điều khiển
(4.1), Phương trình (4.7) được biến đổi nhhuw sau:
Do ma trận là ma trận đối xứng ngược nên = 0
với mọi nên từ (4.7) ta nhận được :
Bất đẳng thức trên cho thấy rằng hệ thông ổn định tuyệt đối. mức độ dương của VL phụ thuộc vào Kp và mức độ âm của VL phụ thuộc vào KD. Do đó tang tốc độ hội tụ bằng cách tang giá trị ma trận KD.
52 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
phụ Ta thấy rằng, mức độ dương của VL phụ thuộc vào Kp; mức độ âm của
thuộc vào Kd . Do đó tăng tốc độ hội tụ bằng tăng giá trị Kd. Nâng cao độ chính xác tinh của hệ thống điều khiển đạt được bằng tăng hệ số Kp của khâu khuếch đại. Tuy nhiên ,Kp và Kd quá lớn sẽ làm giảm độ ổn định và chất lượng quá trình quá độ như độ quá điều chỉnh , thời gian quá độ tăng.
7.1.2 Thiết kế cho robot RR Với các thông số đầu vào:
m1=3 kg, a1=0.650 m
m2=2.5 kg, a2=0.55 m.
Hệ phương trình động lực học có dạng như sau:
Với: ; ;
Phương trình luật điều khiển:
Trong đó : sai số vị trí khớp robot.
sai số tốc độ khớp robot.
là ma trận đường chéo các hệ số khuếch
đại của từng khớp hợp riêng.
là ma trận đường chéo các hệ số đạo hàm
của từng khớp hợp riêng.
Quỹ đạo dặt vào hai khớp có dạng bậc ba được tính toán đảm bảo tay máy di chuyển từ vị trí ban đầu (0.2 0) đến vị trí cuối cùng (0 0.1) trong thời gian 2s.
Ma trận hệ số khuếch đại tỷ lệ và đạo hàm được lựa chọn:
53 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
và
7.1.3 Sử dụng phương trình động lực học mô phỏng điều khiển trong simulink:
Với phương trình động lực học rô bốt RR :
Với luật điều khiển:
Để mô phỏng bằng phương trình động lực học thì ta sử dụng luật điều khiển PD như sau:
Hình 7.2 Sơ đồ khối bộ điều khiển PD
Trong các khối con như sau:
Khối đặt DAT
54 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Hình 7.3 sơ đồ khối đặt DAT
Khối PD :
Hình 7.4 sơ đồ khối PD
Sơ đồ khối Robot
Hình 7.5 sơ đồ khối robot
55 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
% cac thong so mo phong a1=0.65; a2=0.55; g=9.81; m1=3; m2=2.5; tc=5; s0=[1.03923;0.6]; q0=DHN(s0); sc=[0.05;1.03923]; qc=DHN(sc); a01=q0(1); a11=0; a21=3*(qc(1)-q0(1))/tc^2; a31=-2*(qc(1)-q0(1))/tc^3; a02=q0(2); a12=0; a22=3*(qc(2)-q0(2))/tc^2; a32=-2*(qc(2)-q0(2))/tc^3; % ham tinh DHN dong hoc nguoc function [q] = DHN(s) %thong so robot a1=0.65; a2=0.55; %tinhq2 c2=(s(1)^2+s(2)^2-a1^2-a2^2)/2*a1*a2; s2=sqrt(1-c2^2); q2=atan2(s2,c2); %tinh q1 c1=(a1*s(1)+a2*(s(1)*c2+s(2)*s2))/(s(1)^2+s(2)^2); s1=((a1+a2*c2)*s(2)-a2*s2*s(1))/(s(1)^2+s(2)^2); q1=atan2(s1,c1); q=[q1;q2]; end
Với file mô phỏng có thông số như sau
Hàm Fcn: (1/((1/3)*m1*a1^2+(1/3)*m2*a2^2+m2*a1^2+m2*a1*a2*cos(u(3 ))))*(u(1)- ((1/3)*m2*a2^2+0.5*m2*a1*a2*cos(u(3)))*u(6)+m2*a1*a2*sin (u(3))*u(4)*u(5)+0.5*m2*a1*a2*sin(u(3))*u(5)^2- 0.5*(m1+2*m2)*g*a1*cos(u(3))-0.5*m2*g*a2*cos(u(2)+u(3)))
Hàm Fcn1: (1/((1/3)*m2*a2^2))*(u(6)- ((1/3)*m2*a2^2+0.5*m2*a1*a2*cos(u(2)))*u(5)- 0.5*m2*a1*a2*sin(u(2))*u(3)^2- 0.5*m2*g*a2*cos(u(1)+u(2)))
Các hàm Fcn và Fcn1 như sau:
56 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Ta được kết quả mô phỏng như sau :
a,Sai số biến khớp q đặt và q điều khiển là :
Hình 7.6 sai số các biến khớp điều khiển
b giá trị qd và qdk
Hình 7.7 qd và qdk
57 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
c giá trị vqd1,vq1,vqd2,vq2
Hình 7.8 vqd1 vq1 vqd2,vq2
Ta thấy vdq1 và vq1 bám sát nhau, vdq2 và vdq2 cũng bám sát nhau, nên điều khiển theo phương pháp trên là chấp nhận được.
7.2 SỬ DỤNG MÔ HÌNH XUẤT RA TỪ SOLIDWORKS
Để mô phỏng và điều khiển robot thì ta sử dụng phần mềm matlab kết hợp với các toolbox và simulink. Trong đồ án này để cho việc mô phỏng trên matlab được đơn giản thì em sử dụng mô hình robot sau để mô phỏng chuyển động:
Hình 7.9 Mô hình robot để mô phỏng chuyển động
58 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
sơ đồ khối mô hình trên matlab/simechanics:
Sau khi cài đặt xong phần Simmechanics Link,để liên kết giữa Matlab/Simmechanics với Solidworks, ta chạy SolidWorks, trên thanh Task bar của SolidWorks,ta chọn mục Add-ins,và tích vào Simmechanics Link. Lúc này, ta chọn Save as mô hình Assembly mà ta vừa thiết kế, ở mục Save as ta lưu file với tên ‘banvelap’ chọn đuôi ‘.xml’. Muốn tạo mô hình Robot trên Matlab/Simmechanics, tại dòng lệnh của Matlab, gõ: >>Mech_import(’banvelap.xml’); nhấn Enter Ta sẽ thu được mô hình như ở hình vẽ:
Hình 7.10 Sơ đồ mô hình xuất từ Solidwork sang matlab
Trong đó, các khối Revolute thể hiện đó là khớp quay 1 bậc tự do, các khối Part mô tả hình học của các khâu, khối Weld thể hiện ràng buộc cố định giữa khâu đế với hệ tọa độ gốc. Để mô tả hoạt động của Robot, ta cần đặt vào các khớp Momen dẫn động hoặc có thể gán các giá trị về vị trí, vận tốc, gia tốc cho các biến khớp. Ở đây, để mô phỏng thì ta gắn cho Robot các giá trị biến khớp giá trị của vị trí là hàm theo thời gian. Các tín hiệu này được đưa từ Simulink sang.
Các khối cần lấy: Joint Actuator , Joint sensor để đặt và đo tìn hiệu đầu vào
Khối Scope đo tín hiệu đầu ra ,
Khối Clock để đặt các giá trị hàm thời gian
59 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
sơ đồ khối mô phỏng hoạt động của robot khi đưa các khối tín hiệu vào:
hình 7.11 Sơ đồ mô hình simulink
với bộ điều khiển PD như sau :
Hình 7.12 Sơ đồ bộ điều khiển PD
Với các ma trận chéo
60 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Thì ta thu được kết quả như sau :
Momennt điều khiển : M 1,M2
Hình 7.13 sơ đồ moment điều khiển
Các giá trị biến khớp đặt và điều khiển :
Hình 7.14 so sánh các biến khớp đặt và điều khiển
61 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Các giá trị sai số của biến khớp :
Hình 7.15 sai số các giá trị đặt
Giá trị vận tốc điều khiển và vận tốc đặt
Hình 7.16 vận tốc điều khiển và vận tốc đặt
62 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
PHẦN VIII MÔ PHỎNG CHUYỂN ĐỘNG CỦA ROBOT BẰNG
PHẦN MỀM VISUAL STUDIO VÀ THƯ VIỆN OPENGL
8.1.Thiết kế 3D Sử dụng phần mềm Solid Work để thiết kế mô hình 3D của Robot.
Khi thiết kế thì chọn hệ tọa độ để vẽ nên trùng với khi ta chọn hệ tọa độ khi tính
toán để dễ dàng cho quá trình mô phỏng bằng OpenGL.
Hình 8.1 Mô hình mô phỏng trong OpenGL
8.2 Mô phỏng hoạt động của robot. Mô phỏng trên OpenGL :
Vẽ mô hình trên Solid Work rồi xuất ra file .stl .Ở đây ta cần vẽ theo hệ tọa độ
đúng như khi ta đã chọn khi tính toán
Một vài kết quả mô phỏng.
63 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Hình 8.3 Đường thẳng
Hình 8.4 Đường tròn
64 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Hình 8.5 Đường astroid
Hình 8.6 Hoa 4 cánh
65 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
PHẦN IX: KẾT LUẬN Sau một thời gian tìm hiểu nghiên cứu đồng thời được sự giúp đỡ nhiệt tình của thầy hướng dẫn, về cơ bản em đã hoàn thành được đồ án này. Thông qua đồ án này đã giúp em hệ thống lại những kiến thức đã được học, đồng thời giúp em tìm hiểu thêm một số kiến thức mới. đặc biệt giúp em tìm hiểu sâu hơn về robot, một lĩnh vực không còn mới nhưng vẫn đang phát triển khá mạnh ở nước ta.
Trong đồ án này được sự giúp đỡ của bạn bè, thầy cô, và sự tìm hiểu của mình em đã làm được một số phần như sau: thiết kế mô hình robot bằng solidworks theo ý tưởng của mình, tính toán động học thuận ngược, thiết lập phương trình động lực học robot, thiết lập quỹ đạo chuyển động trong không gian khớp và không gian làm việc, thiết kế bộ điều khiển phản hồi trong không gian khớp, mô phỏng Simulink trong không gian khớp. Tuy mới làm được như vậy nhưng qua đó nó cũng giúp em hiểu thêm về robot, nó là nền để em có thể phát triển sau này
Nhưng hạn chế hay chưa làm được trong đồ án: tuy đã cố gắng nhưng em vẫn còn những hạn chế như: chưa điều khiển chính xác được trong không gian làm việc, các thông số hiệu chỉnh chưa được tối ưu để quỹ đạo bám được tốt nhất.
Những hạn chế này , em sẽ cố gắng tiếp tục hoàn thiện hơn vào nhưng đồ án tiếp theo. Em xin chân thành cảm ơn!
66 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
PHẦN X :CÁC CHƯƠNG TRÌNH ĐIỀU KHIỂN VÀ THIẾT KẾ CƠ KHÍ CÁC BẢN VẼ
67 Các chương trình MATLAB clc; clear all; close all % HAM TINH DONG HOC THUAN % khai bao cac bien syms alpha theta a d q1 q2 a1 a2 q3 a3 t q1=sym('q1(t)'); q2=sym('q2(t)'); q=[q1;q2]; % Vector các toa do suy rong q dq1=diff(q1,t); dq2=diff(q2,t); dq =[dq1;dq2]; % Vector các van toc dai disp('TINH TOAN CHUONG 1') disp ('MA TRAN DH TONG QUAT') % ma tran DH DH=[cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta);... sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta);... 0, sin(alpha), cos(alpha), d;... 0, 0, 0, 1] disp('ma tran khau thao tac 1 :A01') A01=subs(DH,{theta,alpha,a,d},{q1,0,a1,0}) % ma tran khau thao tac 1 disp('ma tran khau thao tac 2 :A12') A12=subs(DH,{theta,alpha,a,d},{q2,0,a2,0}) % ma tran khau thao tac 2 disp('ma tran cosin chi huong khau 1 :R01') R01=A01(1:3,1:3) % ma tran cosin chi huong khau 1 disp('ma tran cosin chi huong khau 2 :R02') R12=A12(1:3,1:3) % ma tran cosin chi huong khau 2 % tinh cac ma tran truyen disp('ma tran truyen') disp(' ma tran cosin chi huong khau 1 doi voi khau co dinh R01') R01=A01(1:3,1:3) % ma tran cosin chi huong khau 2 doi voi khau co dinh R02 A02=simplify(A01*A12) disp(' ma tran cosin chi huong khau 2 doi voi khau co dinh R02') R02=A02(1:3,1:3) % Vector vi tri diem thao tac cuoi disp('Vector vi tri khau thao tac cuoi') rE=[A02(1,4);A02(2,4);A02(3,4)] disp('Toa do xE') xE=A02(1,4) disp('Toa do yE') yE=A02(2,4) disp('Toa do zE') zE=A02(3,4) % tinh toan van toc dai diem E va van toc goc cac khau vE = [diff(xE,t);diff(yE,t);diff(zE,t)] % Tính vector van toc khau thao tac cuoi aE=[diff(diff(xE,t),t);diff(diff(yE,t),t);diff(diff(zE,t),t)] R_0E = A02(1:3,1:3) diff_R_0E=[diff(R_0E(1,1),t),diff(R_0E(1,2),t),diff(R_0E(1,3),t);... Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
diff(R_0E(2,1),t),diff(R_0E(2,2),t),diff(R_0E(2,3),t);... diff(R_0E(3,1),t),diff(R_0E(3,2),t),diff(R_0E(3,3),t)] %Tinh dao ham cua R omega_curve = diff_R_0E*R_0E' omega_curve = simplify(omega_curve) disp('Van toc goc') omega = [omega_curve(3,2); omega_curve(1,3) ; omega_curve(2,1)] %----------- % cac lenh subs gia tri cua t vao bieu thuc rEt=subs(rE,{a1,a2,q1,q2},{65,55,sin(2.*t),sin(pi/3.*t)}) vEt=subs(vE,{a1,a2,q1,q2},{65,55,sin(2.*t),sin(pi/3.*t)}) aEt=subs(aE,{a1,a2,q1,q2},{65,55,sin(2.*t),sin(pi/3.*t)}) %============================================= omega_subs=subs(omega,{q1,q2},{sin(2.*t),sin(pi/3.*t)}) %==================================================== % thay t bang so vao cac bieu thuc do t=0:0.1:10; % cho bien thoi gian la 20s %=================================================== xE=subs(rEt(1,1),t); % thay tiep t vao lan nua de ra so yE=subs(rEt(2,1),t); zE=subs(rEt(3,1),t); %================================================= vEx=subs(vEt(1,1),t); % thay tiep t vao lan nua de ra so vEy=subs(vEt(2,1),t); vEz=subs(vEt(3,1),t); %================================================= aEx=subs(aEt(1,1),t); % thay tiep t vao lan nua de ra so aEy=subs(aEt(2,1),t); aEz=subs(aEt(3,1),t); %============================ zE2=zE.*t; % vi cac bieu thuc nay =0 het vEz2=vEz.*t; % nen phai nhan them t vao aEz2=aEz.*t ; % thi cac vector moi cung so chieu %====================================== wx=subs(omega_subs(1,1),t); wy=subs(omega_subs(2,1),t); wz=subs(omega_subs(3,1),t); wx2=wx.*t; wy2=wy.*t; %====================================== disp('thay cac gia tri cua de bai vao ta duoc') %=================================================== figure(1) % ve cac ham toa do plot(t,xE,'r','linewidth',2) hold on plot(t,yE,'b','linewidth',2) plot(t,zE2,'g','linewidth',2) title('Do thi xE,yE,zE') xlabel('t [s]'),ylabel('xE,yE,zE [cm]') legend('xE','yE','zE') hold off grid on %================================================== % ve quy dao chuyen dong trong khong gian figure(2) plot3(xE,yE,zE2,'g','linewidth',2) title('Do thi chuyen dong cua diem cuoi (xE,yE,zE) trong khong gian') xlabel('xE [cm]'), ylabel('yE [cm]'),zlabel('zE [cm]') grid on Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
68
%================================================== % ve do thi vE trong mat phang va trong khong gian figure(3) plot(t,vEx,'r','linewidth',2) hold on plot(t,vEy,'b','linewidth',2) plot(t,vEz2,'g','linewidth',2) title('Do thi vEx,vEy,vEz') xlabel('t [s]'),ylabel('vEx,vEy,vEz [cm/s]') legend('vEx','vEy','vEz') hold off grid on %================================================= figure(4) plot(t,aEx,'r','linewidth',2) hold on plot(t,aEy,'b','linewidth',2) plot(t,aEz2,'g','linewidth',2) title('Do thi aEx,aEy,aEz') xlabel('t [s]'),ylabel('aEx,aEy,aEz [cm/s^2]') legend('aEx','aEy','aEz') hold off grid on %==================================================== figure(5) plot(t,wx2,'r','linewidth',2) hold on plot(t,wy2,'b','linewidth',2) plot(t,wz,'g','linewidth',2) title('Do thi wx,wy,wz') xlabel('t [s]'),ylabel('wx,wy,wz [rad/s]') legend('wx','wy','wz') hold off grid on % CHUONG TRINH TINH DONG HOC NGUOC DHN function [q] = DHN(s) %thong so robot a1=0.65; a2=0.55; %tinhq2 c2=(s(1)^2+s(2)^2-a1^2-a2^2)/2*a1*a2; s2=sqrt(1-c2^2); q2=atan2(s2,c2); %tinh q1 c1=(a1*s(1)+a2*(s(1)*c2+s(2)*s2))/(s(1)^2+s(2)^2); s1=((a1+a2*c2)*s(2)-a2*s2*s(1))/(s(1)^2+s(2)^2); q1=atan2(s1,c1); q=[q1;q2]; end % CHUONG TRINH TINH DONG HOC NGUOC DUONG THANG global a1 a2 xE yE q1 q2 a b xA=1039.23; yA=600; xB=50; yB=1039.23; a=(xB-xA)/5 % t=5s la thoi gian di chuyen tu A->B b=(yB-yA)/5 t=0:0.01:5; xE=a.*t+1039.23;
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
69 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
yE=b.*t+600; a1=650; a2=550; c2=((xE.^2+yE.^2-a1^2-a2^2)/(2*a1*a2)); s2=sqrt(1-c2.^2); q2=atan(s2./c2); % code tinh q1 c1=(a1.*xE+a2*(xE.*cos(q2)+yE.*sin(q2)))./(xE.^2+yE.^2); s1=sqrt(1-c1.^2); %s1=(a1.*yE+a2*(yE.*cos(q2)-xE.sin(q2)))./(xE.^2+yE.^2); q1=atan(s1./c1); q1q2=[q1' q2']; save q1q2thang.txt -ASCII -DOUBLE q1q2 %-------------------- figure(1) plot(xE,yE,'m','linewidth',2) title('Duong thang quy dao diem E') xlabel('x [mm]'),ylabel('y [mm]') legend('xE,yE') grid on %------------------------- figure(2) plot(t,q1,'r','linewidth',2) hold on plot(t,q2,'--b','linewidth',2) title('do thi q1,q2 theo t') xlabel('t [s]'), ylabel('q1,q2 [rad]') legend('q1','q2') grid on hold off % ve duong tron t=0:0.01:2*pi; x=R.*cos(t)+xR; y=R.*sin(t)+yR; a1=650; a2=550; circle=[x ; y]'; save cc.txt -ASCII -DOUBLE circle %-------------------------------- c2=((x.^2+y.^2-a1^2-a2^2)/(2*a1*a2)); s2=sqrt(1-c2.^2); q2=atan(s2./c2); % code tinh q1 c1=(a1.*x+a2*(x.*cos(q2)+y.*sin(q2)))./(x.^2+y.^2); s1=sqrt(1-c1.^2); q1=atan(s1./c1); q1q2tron=[q1' q2']; save q1q2tron.txt -ASCII -DOUBLE q1q2tron %===================== figure(1) plot(x,y,'r','linewidth',2) title('duong tron duong kinh AB') xlabel('x [mm]'),ylabel('y [mm]') axis([0 1000 0 1050]) grid on %============================== figure(2) plot(x,y,'r','linewidth',2) hold on
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
70 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
plot([xA xB],[yA yB],'b','linewidth',2) plot([0 640.125],[0 128.871355],'m','linewidth',3 ) plot([640.125 640.125],[128.871355 662.87],'g','linewidth',3) plot([0 497.92888],[0 417.8119463],'m','linewidth',3 ) plot([497.92888 xB],[417.8119463 yB],'g','linewidth',3) title('vi tri co cau tai A va B') xlabel('x [mm]'),ylabel('y [mm]') axis([0 1000 0 1050]) grid on text(650,670,'A') text(110,850,'B') text(350,50,'khau 1') text(650,400,'khau 2') text(220,300,'khau 1') text(400,600,'khau 2') % ve q1 va q2 figure(3) plot(t,q1,'r','linewidth',2) hold on plot(t,q2,'--b','linewidth',2) title('Do thi q1 va q2 theo t') xlabel('t [s]'),ylabel('q1 q2 [mm]') legend('q1','q2') grid on % chuong trinh ve duong tron mo phong t=0:0.01:10; a1=650; a2=550; x=450+300*cos(2.*t); y=450+300*sin(2.*t); figure(1) plot(x,y,'r','linewidth',2) xlabel('xE') ylabel('yE') title('quy dao can chuyen dong theo la duong tron (x-450)^2+(y- (450)^2=300^2') grid on % chuong trinh ve khong gian lam viec global a1 a2 q1 q2 x y a1=650; a2=550; q1=-pi/6:0.01:7*pi/6; q2=-2*pi/3:0.01:2*pi/3; N=length(q1); x=a1.*cos(q1)+a2.*cos(q1+q2); y=a1.*sin(q1)+a2.*sin(q1+q2); x0=0;y0=0; x0=x0.*(q1./q1); y0=y0.*(q2./q2); x1=a1.*cos(q1); y1=a1.*sin(q1); for i=1:2:N plot([x1(i);x(i)],[y1(i);y(i)],'bo-') hold on grid on end %============ title('khong gian lam viec robot 2 DOF') xlabel('xE'), ylabel('yE') text (20,20,'O') Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
71
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
% chuong trinh tinh ve xE yE khi thiet ke quy dao ra q1 q2 global a1 a2 xE yE q1 q2 t=0:0.01:5; a1=650; a2=550; q1=pi/6+(pi/50).*(t.^2)-(pi/375).*(t.^3); % vi tri khau 1 q2=(pi/25).*(t.^2)-(2*pi/375).*(t.^3); % vi tri khau 2 xE=a1*cos(q1)+a2*cos(q1+q2); yE=a1*sin(q1)+a2*sin(q1+q2); xA=1039.23 yA=600 xB=50 yB=1039.23 plot(xE,yE,'r','linewidth',2) hold on plot([xA,xB],[yA,yB],'b','linewidth',2) xlabel('xE [mm]'),ylabel('yE [mm]') title('Do thi diem E trong khong gian thao tac') text(1050,620,'A') text(30,1050,'B') text(200,850,'Duong thang AB') text(600,1010,'quy dao thuc diem E') grid on hold off % chuong trinh mo phong vi tri co cau theo t % khi da tim ra q1 va q2 theo bai toan thiet ke quy dao global a1 a2 q1 q2 xA xB yA yB x0 y0 %................................................ % nhap tham so a1=650; a2=550; x0=0; y0=0; t=0:0.01:5; N=length(t); q1=pi/6+pi/50.*t.^2-pi/375.*t.^3; q2=pi/25.*t.^2-2*pi/375.*t.^3; xB=a1*cos(q1)+a2*cos(q1+q2); yB=a1*sin(q1)+a2*sin(q1+q2); xA=a1.*cos(q1); yA=a1.*sin(q1); x0=x0.*(t./t); y0=y0.*(t./t); for i=1:10:N plot([x0(i);xA(i)],[y0(i);yA(i)],'ro-',... [xA(i);xB(i)],[yA(i);yB(i)],'bo-') hold on end title('Mo phong vi tri cua co cau theo t') xlabel('xE'), ylabel('yE') text(1060,630,'A') text(45,970,'B') grid on % chuong trinh mo phong vi tri co cau theo t % quy dao la nua duong tron global a1 a2 x y xA= 562.92 yA= 875 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
72
xB= 151.31 yB= 837.92 x0=0; y0=0; a1=650;a2=550; %============== % tinh toa do trung diem va ban kinh xI=0.5*(xA+xB) yI=0.5*(yA+yB) R=0.5*sqrt((xA-xB)^2+(yA-yB)^2) % tinh cac gia tri a0,a1,a2,a3 a0=acos((xA-xI)/R) a11=0 u=acos((xB-xI)/R)-acos((xA-xI)/R); a22=3*u/25 a3=(-2)*u/125 t=0:0.01:5; N=length(t); x=xI+R.*cos(a0+a11.*t+a22.*t.^2+a3.*t.^3); y=yI+R.*sin(a0+a11.*t+a22.*t.^2+a3.*t.^3); % code tinh q1 q2 c2=((x.^2+y.^2-a1^2-a2^2)/(2*a1*a2)); s2=sqrt(1-c2.^2); q2=atan(s2./c2); % code tinh q1 c1=(a1.*x+a2*(x.*cos(q2)+y.*sin(q2)))./(x.^2+y.^2); s1=sqrt(1-c1.^2); q1=atan(s1./c1); xA1=a1.*cos(q1); yA1=a1.*sin(q1); x0=x0.*(t./t); y0=y0.*(t./t); for i=1:15:N plot([x0(i);xA1(i)],[y0(i);yA1(i)],'ro-',... [xA1(i);x(i)],[yA1(i);y(i)],'bo-') hold on axis([0 1000 0 1100]) end title('Mo phong vi tri cua co cau theo t') xlabel('xE'), ylabel('yE') text(580,900,'A') text(140,840,'B') grid on % hinh ve mo phong vi tri co cau theo t % theo quy dao diem thao tac cuoi duong thang global a1 a2 x y a b xA=1039.23; yA=600; xB=50; yB=1039.23; x0=0; y0=0; a1=650; a2=550; a=(yB-yA)/(xB-xA) % t=5s la thoi gian di chuyen tu A->B b=(yA*xB-yB*xA)/(xB-xA) a0=xA a11=0 a22=3*(xB-xA)/25 a3=(-2)*(xB-xA)/125 t=0:0.01:5; N=length(t); x=a0+a11.*t+a22.*t.^2+a3.*t.^3; y=a.*x+b;
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
73 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
% tinh q1 q2 c2=((x.^2+y.^2-a1^2-a2^2)./(2*a1*a2)); s2=sqrt(1-c2.^2); q2=atan(s2./c2); %====================================== % code tinh q1 c1=(a1.*x+a2*(x.*cos(q2)+y.*sin(q2)))./(x.^2+y.^2); s1=sqrt(1-c1.^2); q1=atan(s1./c1); % ve do thi .......... xA1=a1.*cos(q1); yA1=a1.*sin(q1); x0=x0.*(t./t); y0=y0.*(t./t); for i=1:10:N plot([x0(i);xA1(i)],[y0(i);yA1(i)],'r*-',... [xA1(i);x(i)],[yA1(i);y(i)],'bo-') hold on end title('Mo phong vi tri cua co cau theo t') xlabel('xE'), ylabel('yE') text(1060,630,'A') text(45,970,'B') grid on % chuong trinh ve x(t),y(t) trong bai toan thiet ke quy dao thang global a1 a2 x y a b xA=1039.23; yA=600; xB=50; yB=1039.23; a=(yB-yA)/(xB-xA) % t=5s la thoi gian di chuyen tu A->B b=(yA*xB-yB*xA)/(xB-xA) a0=xA; a1=0; a2=3*(xB-xA)/25; a3=(-2)*(xB-xA)/125; t=0:0.01:5; x=a0+a1+a2.*t.^2+a3.*t.^3; y=a.*x+b; % ve do thi %-------------------- figure(1) plot(x,y,'m','linewidth',2) title('Duong thang quy dao diem E') xlabel('x [mm]'),ylabel('y [mm]') text(1050,620,'A') text(45,1020,'B') grid on %------------------------- figure(2) plot(t,x,'r','linewidth',2) hold on plot(t,y,'--b','linewidth',2) title('do thi x,y theo t') xlabel('t [s]'), ylabel(' x,y [mm]') legend('x(t)','y(t)') grid on hold off
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
74 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
% chuong trinh tinh ve do thi x y trong bai toan thiet ke % quy dao theo duong tron global a1 a2 x y xA=562.92; yA=875; xB=151.31; yB=837.92; %============== % tinh toa do trung diem va ban kinh xI=0.5*(xA+xB) yI=0.5*(yA+yB) R=0.5*sqrt((xA-xB)^2+(yA-yB)^2) % tinh cac gia tri a0,a1,a2,a3 a0=acos((xA-xI)/R) a1=0 u=acos((xB-xI)/R)-acos((xA-xI)/R); a2=3*u/25 a3=(-2)*u/125 t=0:0.01:5; x=xI+R.*cos(a0+a1.*t+a2.*t.^2+a3.*t.^3); y=yI+R.*sin(a0+a1.*t+a2.*t.^2+a3.*t.^3); % ve do thi %-------------------- figure(1) plot(x,y,'m','linewidth',2) title('quy dao diem E ') xlabel('x [mm]'),ylabel('y [mm]') %text(1050,620,'A') %text(45,1020,'B') grid on %------------------------- figure(2) plot(t,x,'r','linewidth',2) hold on plot(t,y,'--b','linewidth',2) title('do thi x,y theo t') xlabel('t [s]'), ylabel(' x,y [mm]') legend('x(t)','y(t)') grid on hold off % CHUONG TRINH TINH CHUONG TINH HOC VA DONG LUC HOC % khai bao cac bien syms alpha theta a d q1 q2 a1 a2 q3 a3 t m1 m2 g Fx Fy Mz q1=sym('q1(t)'); q2=sym('q2(t)'); q=[q1;q2]; % Vector các toa do suy rong q dq1=diff(q1,t); dq2=diff(q2,t); dq =[dq1;dq2]; % Vector các van toc dai % xet khau tac dong cuoi FE2=[Fx;Fy;0]; ME2=[0;0;Mz]; F032=FE2 ;M032=ME2; r11=[a1;0;0]; r22=[a2;0;0]; rc11=[-a1/2;0;0]; rc22=[-a2/2;0;0]; P01=[0;-m1*g;0]; P02=[0;-m2*g;0]; r0c2s=[0 0 -a2/2*sin(q1+q2);0 0 a2/2*cos(q1+q2);a2/2*sin(q1+q2) - a2/2*cos(q1+q2) 0]; r02s=[0 0 a2*sin(q1+q2);0 0 -a2*cos(q1+q2);-a2*sin(q1+q2) a2*cos(q1+q2) 0]; % r0c2s va r02s la cac ma tran song cua khau 2 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
75
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
disp('luc tac dung len khau 2 : khau cuoi ') F21=FE2-P02 % luc tac dong khau 2 disp('momoent tac dung len khau 2 : khau cuoi ') M021=M032+r02s*F21-r0c2s*P02 % moment tac dong vao khau 2 % tinh toan cho khau 1 r0c1s=[0 0 -a1/2*sin(q1);0 0 a1/2*cos(q1);a1/2*sin(q1) -a1/2*cos(q1) 0]; r01s=[0 0 a1*sin(q1);0 0 -a1*cos(q1);-a1*sin(q1) a1*cos(q1) 0]; disp('luc tac dung len khau 1 : khau de') F10=F21-P01 disp('moment tac dung len khau 1 : khau de ') M010=M021+r01s*F10-r0c1s*P01 % TINH TOAN CHO CHUONG 5 I1x=0; I1y=1/12*m1*a1^2; I1z=I1y; I2x=0; I2y=1/12*m2*a2^2;I2z=I2y; rc1=[1/2*a1*cos(q1);1/2*a1*sin(q1);0]; % toa do khoi tam C1 rc2=[a1*cos(q1)+1/2*a2*cos(q1+q2);a1*sin(q1)+1/2*a2*sin(q1+q2);0]; disp('bieu thuc van toc goc cac khau') w1=[0;0;dq1] w2=[0;0;dq2] disp('cac ma tran jacobi tinh tien va quay') rc=[rc1;rc2]; JT1=[-1/2*a1*sin(q1) 0;1/2*a1*cos(q1) 0;0 0] JR1=[0 0;0 0;1 0] JT2=[-a1*sin(q1)-1/2*a2*sin(q1+q2) - 1/2*a2*sin(q1+q2);a1*cos(q1)+1/2*a2*sin(q1+q2) 1/2*a2*cos(q1+q2)] JR2=[0 0;0 0;1 1] disp('cac ma tran tenxo quan tinh') I1=[0 0 0;0 I1y 0;0 0 I1z] I2=[0 0 0;0 I2y 0;0 0 I2z] disp('ma tran khoi luong suy rong') M=m1*JT1'*JT1+JR1'*I1*JR1+m2*JT2'*JT2+JR2'*I2*JR2 m11=M(1,1) m12=M(1,2) m21=M(2,1) m22=M(2,2) disp('bieu thuc dong nang robot') T=1/2*dq'*M*dq disp('bieu thuc the nang co dang') TL=m1*g*a1*sin(q1)+m2*g*(a1*sin(q1)+1/2*a2*sin(q1+q2)) % cac thong so mo phong a1=0.65; a2=0.55; g=9.81; m1=3; m2=2.5; tc=5; s0=[1.03923;0.6]; q0=DHN(s0); sc=[0.05;1.03923]; qc=DHN(sc); a01=q0(1); a11=0; a21=3*(qc(1)-q0(1))/tc^2; a31=-2*(qc(1)-q0(1))/tc^3; a02=q0(2); a12=0; a22=3*(qc(2)-q0(2))/tc^2; a32=-2*(qc(2)-q0(2))/tc^3; Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
76
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
77 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
78 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
79 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
80 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD:ThS Mạc Thị Thoa
Phần XI : TÀI LIỆU THAM KHẢO
1.Bài giảng Robotics-PGS.TS.Phan Bùi Khôi.
3. Cơ sở Robot công nghiệp -GS.TSKH Nguyễn Văn Khang
2.Động lực học hệ nhiều vật-GS.TSKH.Nguyễn Văn Khang.
4. Điều khiển Robot công nghiệp – TS. Nguyễn Mạnh Tiến.
5. Robot công nghiệp – GS. TSKH. Nguyễn Thiện Phúc .
6 .Phần help của phần mềm Maple.
7. Phần help của phần mềm Matlab.
81 Sinh viên thực hiện: Đinh Đức Anh Lớp CĐT2- K55
